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];
}
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 Matrix_read(const std::string file_name, yarp::sig::Matrix & mat)
{
FILE * fp;
uint32_t rows,cols;
double elem;
fp = fopen(file_name.c_str(),"rb");
if( fp == NULL ) return false;
//reading dimensions information
fread(&rows,sizeof(uint32_t),1,fp);
fread(&cols,sizeof(uint32_t),1,fp);
mat.resize(rows,cols);
for(size_t i=0;i<rows;i++) {
for(size_t j=0;j<cols;j++ ) {
fread(&elem,sizeof(double),1,fp);
mat(i,j) = elem;
}
}
/*
if( gsl_matrix_fread(fp,(gsl_matrix*)mat.getGslMatrix()) == GSL_EFAILED ) {
fclose(fp);
return false;
}
*/
fclose(fp);
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;
}
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 buildRotMat2(const double &theta,yarp::sig::Matrix &rot)
{
rot.resize(2,2);
rot(0,0)=cos(theta);
rot(0,1)=sin(theta);
rot(1,0)=-sin(theta);
rot(1,1)=cos(theta);
}
void idyntree2yarp(const iDynTreeMatrixType & idyntreeMatrix,
yarp::sig::Matrix & yarpMatrix)
{
yarpMatrix.resize(idyntreeMatrix.rows(),idyntreeMatrix.cols());
for(size_t row=0; row < idyntreeMatrix.rows(); row++)
{
for(size_t col=0; col < idyntreeMatrix.cols(); col++)
{
yarpMatrix(row,col) = idyntreeMatrix(row,col);
}
}
}
void HandIKModule::fillMatrixFromBottle(const Bottle* b, yarp::sig::Matrix &m, int rows, int cols)
{
int k=0;
m.resize(rows,cols);
for (int i=0; i<rows; i++)
{
for (int j=0; j<cols; j++)
{
m(i,j)=b->get(k).asDouble();
k++;
}
}
}
void buildRotMat3(const double &alpha,const double &beta,const double &gamma,yarp::sig::Matrix &rot)
{
rot.resize(3,3);
rot(0,0)=cos(beta)*cos(gamma);
rot(0,1)=-cos(beta)*sin(gamma);
rot(0,2)=sin(beta);
rot(1,0)=sin(alpha)*sin(beta)*cos(gamma)+cos(alpha)*sin(gamma);
rot(1,1)=-sin(alpha)*sin(beta)*sin(gamma)+cos(alpha)*cos(gamma);
rot(1,2)=-sin(alpha)*cos(beta);
rot(2,0)=-cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma);
rot(2,1)=cos(alpha)*sin(beta)*sin(gamma)+sin(alpha)*cos(gamma);
rot(2,2)=cos(alpha)*cos(beta);
}
bool insituFTSensorCalibrationThread::getCalibration(yarp::sig::Matrix & mat)
{
mat.resize(3,6);
bool ret = true;
std::string err;
ret = estimator.computeForceCalibrationMatrixEstimation(err);
if( !ret )
{
yError("insituFTSensorCalibrationThread CalibrationMatrixEstimation failed %s",err.c_str());
return false;
}
return estimator.getEstimatedForceCalibrationMatrix(InSituFTCalibration::wrapMat(mat));
}
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 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 Matrix_read(const std::string file_name, yarp::sig::Matrix & mat)
{
FILE * fp;
uint32_t rows,cols;
double elem;
std::cerr << "Opening matrix " << file_name << std::endl;
fp = fopen(file_name.c_str(),"rb");
if( fp == NULL ) {
std::cerr << "Error opening matrix " << file_name << std::endl;
return false;
}
//reading dimensions information
fread(&rows,sizeof(uint32_t),1,fp);
fread(&cols,sizeof(uint32_t),1,fp);
mat.resize(rows,cols);
for(size_t i=0; i<rows; i++) {
for(size_t j=0; j<cols; j++ ) {
fread(&elem,sizeof(double),1,fp);
std::cout << "Read element " << i << " " << j << " : " << elem << std::endl;
mat(i,j) = elem;
}
}
/*
if( gsl_matrix_fread(fp,(gsl_matrix*)mat.getGslMatrix()) == GSL_EFAILED ) {
fclose(fp);
return false;
}
*/
fclose(fp);
return true;
}
