Exemplo n.º 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];
}
Exemplo n.º 2
0
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;
}
Exemplo n.º 4
0
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;
}
Exemplo n.º 5
0
//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;
}
Exemplo n.º 6
0
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 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;
}
Exemplo n.º 8
0
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;
}
Exemplo n.º 9
0
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);
	}
}
Exemplo n.º 10
0
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);
	}
}
Exemplo n.º 11
0
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;
}
Exemplo n.º 12
0
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 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;
}
Exemplo n.º 14
0
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;
    }
}
Exemplo n.º 15
0
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;
}
Exemplo n.º 16
0
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;
}
Exemplo n.º 17
0
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;
}
Exemplo n.º 18
0
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;
}
Exemplo n.º 19
0
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;
}
Exemplo n.º 20
0
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;
}
Exemplo n.º 21
0
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;
}
Exemplo n.º 22
0
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;
}
Exemplo n.º 23
0
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);
        }
    }
}
Exemplo n.º 24
0
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;
}
Exemplo n.º 25
0
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);
}
Exemplo n.º 26
0
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);
}
Exemplo n.º 27
0
yarp::sig::Vector LinearMean::calculateMean(const yarp::sig::Matrix &x)
{
  //  std::cout << "calc mean, x size:" << x.rows() << " by " << x.cols() << ", meanHyparam size: " << meanLinearHyperparam.size() << std::endl;// fflush(stdout);
    Vector fmean(x.rows(), meanOffset);
    fmean+=x*meanLinearHyperparam;
    return fmean;
}
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;
}
Exemplo n.º 29
0
Vector BoundingBox::findIndexes(const yarp::sig::Matrix &corner_i)
{
    Vector indexes(3);
    yarp::sig::Vector point1(3); point1[0]=corner_i(0,0); point1[1]=corner_i(0,1); point1[2]=corner_i(0,2);

    int m=0;
    for (int i=0; i<3; i++)
    {
        for (int j=0; j<3; j++)
        {
            if (j<=i)
                continue;
            int index=-1;
            double minimum=10000;
            for (int t=1; t<corner_i.rows(); t++)
            {
                yarp::sig::Vector tmp(3); tmp[0]=corner_i(t,0); tmp[1]=corner_i(t,1); tmp[2]=corner_i(t,2);
                double value=norm(Helpers::extractSubVector(point1,i,j)-Helpers::extractSubVector(tmp,i,j));
                if (value<minimum)
                {
                    index=t;
                    minimum=value;
                }
            }
            indexes[m]=index;
            m+=1;
        }
    }
    return indexes;
}
Exemplo n.º 30
0
    //---------------------------------------------------------
    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;
    };