Beispiel #1
1
void iCubShoulderConstr::appendMatrixRow(yarp::sig::Matrix &dest,
                                         const yarp::sig::Vector &row)
{
    yarp::sig::Matrix tmp;

    // if dest is already filled with something
    if (dest.rows())
    {   
        // exit if lengths do not match     
        if (row.length()!=dest.cols())
            return;

        tmp.resize(dest.rows()+1,dest.cols());

        // copy the content of dest in temp
        for (int i=0; i<dest.rows(); i++)
            for (int j=0; j<dest.cols(); j++)
                tmp(i,j)=dest(i,j);

        // reassign dest
        dest=tmp;
    }
    else
        dest.resize(1,row.length());

    // append the last row
    for (int i=0; i<dest.cols(); i++)
        dest(dest.rows()-1,i)=row[i];
}
Beispiel #2
0
void YarpJointDev::PosAxisAngle_To_PosEulerSingle ( const yarp::sig::Vector& xd,
        const yarp::sig::Vector& od,
        std::vector< float >& res ) {


    assert ( xd.length() == 3 );
    assert ( od.length() == 4 );

    float wx, wy, wz;

    AxisAngle_To_Euler ( od[0], od[1], od[2], od[3], wx, wy, wz );

    res.clear();

    res.push_back ( xd[0] );
    res.push_back ( xd[1] );
    res.push_back ( xd[2] );

    res.push_back ( wx );
    res.push_back ( wy );
    res.push_back ( wz );


}
Beispiel #3
-1
void iCubShoulderConstr::appendVectorValue(yarp::sig::Vector &dest, double val)
{
    yarp::sig::Vector tmp(dest.length()+1);
    for (size_t i=0; i<dest.length(); i++)
        tmp[i]=dest[i];

    dest=tmp;
    dest[dest.length()-1]=val;
}