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 Quaternion::fromRotationMatrix(const yarp::sig::Matrix &R)
{
if ((R.rows()<3) || (R.cols()<3))
{
yError("fromRotationMatrix() failed, matrix should be >= 3x3");
yAssert(R.rows() >= 3 && R.cols() >= 3);
}
double tr = R(0, 0) + R(1, 1) + R(2, 2);
if (tr>0.0)
{
double sqtrp1 = sqrt(tr + 1.0);
double sqtrp12 = 2.0*sqtrp1;
internal_data[0] = 0.5*sqtrp1;
internal_data[1] = (R(2, 1) - R(1, 2)) / sqtrp12;
internal_data[2] = (R(0, 2) - R(2, 0)) / sqtrp12;
internal_data[3] = (R(1, 0) - R(0, 1)) / sqtrp12;
}
else if ((R(1, 1)>R(0, 0)) && (R(1, 1)>R(2, 2)))
{
double sqdip1 = sqrt(R(1, 1) - R(0, 0) - R(2, 2) + 1.0);
internal_data[2] = 0.5*sqdip1;
if (sqdip1>0.0)
sqdip1 = 0.5 / sqdip1;
internal_data[0] = (R(0, 2) - R(2, 0))*sqdip1;
internal_data[1] = (R(1, 0) + R(0, 1))*sqdip1;
internal_data[3] = (R(2, 1) + R(1, 2))*sqdip1;
}
else if (R(2, 2)>R(0, 0))
{
double sqdip1 = sqrt(R(2, 2) - R(0, 0) - R(1, 1) + 1.0);
internal_data[3] = 0.5*sqdip1;
if (sqdip1>0.0)
sqdip1 = 0.5 / sqdip1;
internal_data[0] = (R(1, 0) - R(0, 1))*sqdip1;
internal_data[1] = (R(0, 2) + R(2, 0))*sqdip1;
internal_data[2] = (R(2, 1) + R(1, 2))*sqdip1;
}
else
{
double sqdip1 = sqrt(R(0, 0) - R(1, 1) - R(2, 2) + 1.0);
internal_data[1] = 0.5*sqdip1;
if (sqdip1>0.0)
sqdip1 = 0.5 / sqdip1;
internal_data[0] = (R(2, 1) - R(1, 2))*sqdip1;
internal_data[2] = (R(1, 0) + R(0, 1))*sqdip1;
internal_data[3] = (R(0, 2) + R(2, 0))*sqdip1;
}
}
bool Matrix::setSubmatrix(const yarp::sig::Matrix &m, int r, int c)
{
if((c<0) || (c+m.cols()>ncols) || (r<0) || (r+m.rows()>nrows))
return false;
for(int i=0;i<m.rows();i++)
for(int j=0;j<m.cols();j++)
(*this)[r+i][c+j] = m(i,j);
return true;
}
yarp::sig::Vector iCub::skinDynLib::toVector(yarp::sig::Matrix m)
{
Vector res(m.rows()*m.cols(),0.0);
for (int r = 0; r < m.rows(); r++)
{
res.setSubvector(r*m.cols(),m.getRow(r));
}
return res;
}
bool KDLtoYarp(const KDL::Rotation & kdlRotation, yarp::sig::Matrix & yarpMatrix3_3)
{
if( yarpMatrix3_3.rows() != 3 || yarpMatrix3_3.cols() != 3 ) { yarpMatrix3_3.resize(3,3); }
//Both kdl and yarp store the rotation matrix in row major order
memcpy(yarpMatrix3_3.data(),kdlRotation.data,3*3*sizeof(double));
return true;
}
bool yarpWholeBodyModelV1::convertBasePose(const Frame &xBase, yarp::sig::Matrix & H_world_base)
{
if( H_world_base.cols() != 4 || H_world_base.rows() != 4 )
H_world_base.resize(4,4);
xBase.get4x4Matrix(H_world_base.data());
return true;
}
bool isEqual(const yarp::sig::Matrix& m1, const yarp::sig::Matrix& m2, double precision)
{
if (m1.cols() != m2.cols() || m1.rows() != m2.rows())
{
return false;
}
for (size_t i = 0; i < m1.rows(); i++)
{
if (!isEqual(m1.getRow(i), m2.getRow(i), precision))
{
return false;
}
}
return true;
}
yarp::sig::Matrix localSE3inv(const yarp::sig::Matrix &H, unsigned int verbose)
{
if ((H.rows()<4) || (H.cols()<4))
{
if (verbose)
fprintf(stderr,"localSE3inv() failed\n");
return yarp::sig::Matrix(0,0);
}
yarp::sig::Matrix invH(4,4);
yarp::sig::Vector p(3);
yarp::sig::Matrix Rt=H.submatrix(0,2,0,2).transposed();
p[0]=H(0,3);
p[1]=H(1,3);
p[2]=H(2,3);
p=Rt*p;
for (unsigned int i=0; i<3; i++)
for (unsigned int j=0; j<3; j++)
invH(i,j)=Rt(i,j);
invH(0,3)=-p[0];
invH(1,3)=-p[1];
invH(2,3)=-p[2];
invH(3,3)=1.0;
return invH;
}
bool idynInertia2kdlRotationalInertia(const yarp::sig::Matrix & idynInertia,KDL::RotationalInertia & kdlRotationalInertia)
{
if(idynInertia.cols() != 3 || idynInertia.rows() != 3 ) return false;
//if(idynInertia(0,1) != idynInertia(1,0) || idynInertia(0,2) != idynInertia(2,0) || idynInertia(1,2) != idynInertia(2,1)) return false;
kdlRotationalInertia = KDL::RotationalInertia(idynInertia(0,0),idynInertia(1,1),idynInertia(2,2),idynInertia(0,1),idynInertia(0,2),idynInertia(1,2));
return true;
}
bool Matrix_write(const std::string file_name, const yarp::sig::Matrix & mat)
{
FILE * fp;
uint32_t rows,cols;
double elem;
rows = mat.rows();
cols = mat.cols();
fp = fopen(file_name.c_str(),"wb");
if( fp == NULL ) return false;
//writing dimensions informations
fwrite(&rows,sizeof(uint32_t),1,fp);
fwrite(&cols,sizeof(uint32_t),1,fp);
for(size_t i=0;i<rows;i++) {
for(size_t j=0;j<cols;j++ ) {
elem = mat(i,j);
fwrite(&elem,sizeof(double),1,fp);
}
}
/*
if( gsl_matrix_fwrite(fp,(gsl_matrix*)mat.getGslMatrix()) == GSL_EFAILED ) {
fclose(fp);
return false;
}*/
fclose(fp);
return true;
}
Vector yarp::math::dcm2ypr(const yarp::sig::Matrix &R)
{
yAssert((R.rows() >= 3) && (R.cols() >= 3));
Vector v(3); // yaw pitch roll
if (R(0, 2)<1.0)
{
if (R(0, 2)>-1.0)
{
v[0] = atan2(-R(0, 1), R(0, 0));
v[1] = asin(R(0, 2));
v[2] = atan2(-R(1, 2), R(2, 2));
}
else // == -1
{
// Not a unique solution: psi-phi=atan2(-R12,R11)
v[0] = 0.0;
v[1] = -M_PI / 2.0;
v[2] = -atan2(R(1, 0), R(1, 1));
}
}
else // == +1
{
// Not a unique solution: psi+phi=atan2(-R12,R11)
v[0] = 0.0;
v[1] = M_PI / 2.0;
v[2] = atan2(R(1, 0), R(1, 1));
}
return v;
}
//---------------------------------------------------------
bool eigenToYarpMatrix(const Eigen::MatrixXd& eigenMatrix, yarp::sig::Matrix& yarpMatrix)
{
if((yarpMatrix.cols() == 0)||(yarpMatrix.rows() == 0))
{
cout<<"ERROR: input matrix is empty (eigenToYarpMatrix)"<<endl;
return false;
}
//resize and fill eigen vector with yarp vector elements
yarpMatrix.resize(eigenMatrix.rows(),eigenMatrix.cols());
for(unsigned int i = 0; i < yarpMatrix.rows(); i++)
for(unsigned int j = 0; j < yarpMatrix.cols(); j++)
yarpMatrix(i,j) = eigenMatrix(i,j);
return true;
};
bool idynMatrix2kdlRotation(const yarp::sig::Matrix & idynMatrix, KDL::Rotation & kdlRotation)
{
if(idynMatrix.cols() != 3 || idynMatrix.rows() != 3) return false;
kdlRotation = KDL::Rotation(idynMatrix(0,0),idynMatrix(0,1),idynMatrix(0,2),
idynMatrix(1,0),idynMatrix(1,1),idynMatrix(1,2),
idynMatrix(2,0),idynMatrix(2,1),idynMatrix(2,2));
return true;
}
void RobotInterface::getVelocityState(yarp::sig::Matrix& outputVelocity)
{
outputVelocity.resize(velocityState.rows(), velocityState.cols());
for(int i = 0; i < outputVelocity.rows(); i++)
{
for(int d = 0; d < outputVelocity.cols(); d++) outputVelocity(i, d) = velocityState(i, d);
}
}
void RobotInterface::getPositionState(yarp::sig::Matrix& outputPosition)
{
outputPosition.resize(positionState.rows(), positionState.cols());
for(int i = 0; i < outputPosition.rows(); i++)
{
for(int d = 0; d < outputPosition.cols(); d++) outputPosition(i, d) = positionState(i, d);
}
}
void RobotInterface::getPressureState(yarp::sig::Matrix& outputPressure)
{
outputPressure.resize(pressureState.rows(), pressureState.cols());
for(int i = 0; i < outputPressure.rows(); i++)
{
for(int d = 0; d < outputPressure.cols(); d++) outputPressure(i, d) = pressureState(i, d);
}
}
void DictionaryLearning::printMatrixYarp(const yarp::sig::Matrix &A)
{
cout << endl;
for (int i=0; i<A.rows(); i++)
for (int j=0; j<A.cols(); j++)
cout<<A(i,j)<<" ";
cout<<endl;
cout << endl;
}
bool Kalman::set_R(const yarp::sig::Matrix &_R)
{
if ((_R.cols()==R.cols()) && (_R.rows()==R.rows()))
{
R=_R;
return true;
}
else
return false;
}
bool idynMatrix2kdlFrame(const yarp::sig::Matrix & idynMatrix, KDL::Frame & kdlFrame)
{
if( idynMatrix.cols() != 4 || idynMatrix.rows() != 4 ) return false;
KDL::Rotation kdlRotation;
KDL::Vector kdlVector;
idynMatrix2kdlRotation(idynMatrix.submatrix(0,2,0,2),kdlRotation);
idynVector2kdlVector(idynMatrix.subcol(0,3,3),kdlVector);
kdlFrame = KDL::Frame(kdlRotation,kdlVector);
return true;
}
bool Kalman::set_B(const yarp::sig::Matrix &_B)
{
if ((_B.cols()==B.cols()) && (_B.rows()==B.rows()))
{
B=_B;
return true;
}
else
return false;
}
bool Kalman::set_Q(const yarp::sig::Matrix &_Q)
{
if ((_Q.cols()==Q.cols()) && (_Q.rows()==Q.rows()))
{
Q=_Q;
return true;
}
else
return false;
}
bool Kalman::set_A(const yarp::sig::Matrix &_A)
{
if ((_A.cols()==A.cols()) && (_A.rows()==A.rows()))
{
A=_A;
At=A.transposed();
return true;
}
else
return false;
}
bool Kalman::set_H(const yarp::sig::Matrix &_H)
{
if ((_H.cols()==H.cols()) && (_H.rows()==H.rows()))
{
H=_H;
Ht=H.transposed();
return true;
}
else
return false;
}
void yarp2idyntree(const yarp::sig::Matrix & yarpMatrix,
iDynTree::MatrixDynSize & idyntreeMatrix)
{
idyntreeMatrix.resize(yarpMatrix.rows(),yarpMatrix.cols());
for(size_t row=0; row < idyntreeMatrix.rows(); row++)
{
for(size_t col=0; col < idyntreeMatrix.cols(); col++)
{
idyntreeMatrix(row,col) = yarpMatrix(row,col);
}
}
}
void printMatrixYarp(yarp::sig::Matrix &A)
{
cout << endl;
for (int i=0; i<A.rows(); i++)
{
for (int j=0; j<A.cols(); j++)
{
cout<<A(i,j)<<" ";
}
cout<<endl;
}
cout << endl;
}
iDynTree::Transform yarpTransform2idyntree(yarp::sig::Matrix transformYarp)
{
ASSERT_EQUAL_DOUBLE(transformYarp.rows(),4);
ASSERT_EQUAL_DOUBLE(transformYarp.cols(),4);
Rotation R(transformYarp(0,0),transformYarp(0,1),transformYarp(0,2),
transformYarp(1,0),transformYarp(1,1),transformYarp(1,2),
transformYarp(2,0),transformYarp(2,1),transformYarp(2,2));
Position p(transformYarp(0,3),transformYarp(1,3),transformYarp(2,3));
return iDynTree::Transform(R,p);
}
void OpenSoT::SubTask::setWeight(const yarp::sig::Matrix &W)
{
assert(W.rows() == this->getTaskSize());
assert(W.cols() == W.rows());
this->_W = W;
yarp::sig::Matrix fullW = _taskPtr->getWeight();
for(unsigned int r = 0; r < this->getTaskSize(); ++r)
for(unsigned int c = 0; c < this->getTaskSize(); ++c)
fullW(this->_subTaskMap.getRowsVector()[r],
this->_subTaskMap.getRowsVector()[c]) = this->_W(r,c);
_taskPtr->setWeight(fullW);
}
bool idynDynamicalParameters2kdlRigidBodyInertia(const double idynmass,const yarp::sig::Vector & idynrC,const yarp::sig::Matrix & idynI,KDL::RigidBodyInertia & kdlRigidBodyInertia)
{
if(idynrC.size() != 3 || idynI.cols() != 3 || idynI.rows() != 3 ) return false;
KDL::Vector kdlrC;
KDL::RotationalInertia kdlRotationalInertia;
int ret = idynVector2kdlVector(idynrC,kdlrC);
assert(ret);
//printMatrix("idynI",idynI);
if( !idynInertia2kdlRotationalInertia(idynI,kdlRotationalInertia) ) return false;
kdlRigidBodyInertia = KDL::RigidBodyInertia(idynmass,kdlrC,kdlRotationalInertia);
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;
}
bool Matrix::operator==(const yarp::sig::Matrix &r) const
{
//check dimensions first
if ( (rows()!=r.rows()) || (cols()!=r.cols()))
return false;
const double *tmp1=data();
const double *tmp2=r.data();
int c=rows()*cols();
while(c--)
{
if (*tmp1++!=*tmp2++)
return false;
}
return true;
}