//creates a full transform as given by a DCM matrix at the pos and norm w.r.t. the original frame, from the pos and norm (one axis set arbitrarily)
bool AvoidanceHandlerAbstract::computeFoR(const yarp::sig::Vector &pos, const yarp::sig::Vector &norm, yarp::sig::Matrix &FoR)
{
if (norm == zeros(3))
{
FoR=eye(4);
return false;
}
yarp::sig::Vector x(3,0.0), z(3,0.0), y(3,0.0);
z = norm;
if (z[0] == 0.0)
{
z[0] = 0.00000001; // Avoid the division by 0
}
y[0] = -z[2]/z[0]; //y is in normal plane
y[2] = 1; //this setting is arbitrary
x = -1*(cross(z,y));
// Let's make them unitary vectors:
x = x / yarp::math::norm(x);
y = y / yarp::math::norm(y);
z = z / yarp::math::norm(z);
FoR=eye(4);
FoR.setSubcol(x,0,0);
FoR.setSubcol(y,0,1);
FoR.setSubcol(z,0,2);
FoR.setSubcol(pos,0,3);
return true;
}
bool KDLtoYarp_position(const KDL::Frame & kdlFrame, yarp::sig::Matrix & yarpMatrix4_4 )
{
yarp::sig::Matrix R(3,3);
yarp::sig::Vector p(3);
KDLtoYarp(kdlFrame.M,R);
KDLtoYarp(kdlFrame.p,p);
if( yarpMatrix4_4.rows() != 4 || yarpMatrix4_4.cols() != 4 ) { yarpMatrix4_4.resize(4,4); }
yarpMatrix4_4.zero();
yarpMatrix4_4.setSubmatrix(R,0,0);
yarpMatrix4_4.setSubcol(p,0,3);
yarpMatrix4_4(3,3) = 1;
return true;
}
