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];
}
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 );
}
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;
}