//ANALOG SENSOR
int GazeboYarpJointSensorsDriver::read(yarp::sig::Vector &out)
{
    ///< \todo TODO in my opinion the reader should care of passing a vector of the proper dimension to the driver, but apparently this is not the case
    /*
    if( (int)jointsensors_data.size() != jointsensors_nr_of_channels ||
        (int)out.size() != jointsensors_nr_of_channels ) {
        return AS_ERROR;
    }
    */
    
    if( (int)jointsensors_data.size() != jointsensors_nr_of_channels ) {
        return AS_ERROR;
    }
   
    if( (int)out.size() != jointsensors_nr_of_channels ) {
        std::cout << " GazeboYarpJointSensorsDriver:read() warning : resizing input vector, this can probably be avoided" << std::endl;
        out.resize(jointsensors_nr_of_channels);
    }
      
    data_mutex.wait();
    out = jointsensors_data;
    data_mutex.post();
    
    return AS_OK;
}
예제 #2
2
bool iDynTreetoYarp(const iDynTree::Wrench & iDynTreeWrench,yarp::sig::Vector & yarpVector)
{
    if( yarpVector.size() != 6 ) { yarpVector.resize(6); }
    memcpy(yarpVector.data(),iDynTreeWrench.getLinearVec3().data(),3*sizeof(double));
    memcpy(yarpVector.data()+3,iDynTreeWrench.getAngularVec3().data(),3*sizeof(double));
    return true;
}
예제 #3
1
void eulerAngles(const std::vector<yarp::sig::Vector> &edges1,const std::vector<yarp::sig::Vector> &edges2,const std::vector<yarp::sig::Vector> &crossProduct,yarp::sig::Vector &alpha,yarp::sig::Vector &beta,yarp::sig::Vector &gamma)
{
    double singamma;
    alpha.resize(crossProduct.size(),0.0);
    beta.resize(crossProduct.size(),0.0);
    gamma.resize(crossProduct.size(),0.0);
    for (int i=0; i<crossProduct.size(); i++)
    {
        double value=crossProduct.at(i)[0];
        beta[i]=asin(std::min(abs(value),1.0)*Helpers::sign(value));
        if (value==1.0)
            alpha[i]=asin(Helpers::sign(edges2.at(i)[2])*std::min(1.0,abs(edges2.at(i)[2])));
        else
        {
            double tmp=crossProduct.at(i)[2]/cos(beta[i]);
            alpha[i]=acos(Helpers::sign(tmp)*std::min(1.0,abs(tmp)));
            if (Helpers::sign(crossProduct.at(i)[1])!=Helpers::sign(-sin(alpha[i])*cos(beta[i])))
                alpha[i]=-alpha[i];
            singamma=-edges2.at(i)[0]/cos(beta[i]);
            if (edges1.at(i)[0]>=0)
                gamma[i]=asin(Helpers::sign(singamma)*std::min(1.0,abs(singamma)));
            else
                gamma[i]=-M_PI-asin(Helpers::sign(singamma)*std::min(1.0,abs(singamma)));
        }
    }
}
예제 #4
1
void yarp_IMU_interface::sense(yarp::sig::Vector &orientation,
                               yarp::sig::Vector &linearAcceleration,
                               yarp::sig::Vector &angularVelocity)
{
    this->sense();

    orientation.resize(3);
    orientation = _output.subVector(0,2);
    linearAcceleration.resize(3);
    linearAcceleration = _output.subVector(3,5);
    angularVelocity.resize(3);
    angularVelocity = _output.subVector(6,8);
}
예제 #5
1
파일: main.cpp 프로젝트: xufango/contrib_bk
    void run()
    {
		Bottle *input = port.read();
		if (input!=NULL)
		{
			if (first_run)
			{
			    for (int i=0; i<input->size(); i++)
				{
                    previous.resize(input->size());
                    current.resize(input->size());
                    diff.resize(input->size());
                    for (int i=0; i<input->size(); i++) current[i]=previous[i]=input->get(i).asDouble();
				}
				first_run=false;
			}

            bool print = false;
            for (int i=0; i<input->size(); i++)
			{
                previous[i]=current[i];
                current[i]=input->get(i).asDouble();
                diff[i]=current[i]-previous[i];

                tolerance = 10/100;
                double percent = fabs(diff[i]/current[i]);
                if (percent > tolerance) 
                    {
                        fprintf(stdout,"ch: %d percent +6.6%f\n", i , percent);
                        print = true;    
                    }
	        }

            if (print == true)
            {
                for (int i=0; i<input->size(); i++)
			    { 
                    fprintf(stdout,"+6.6%f  ",diff[i]);
                }
            }
			fprintf (stdout,"\n");
		}

		/*
		static double time_old_wd=Time::now();
		double time_wd=Time::now();
		fprintf(stdout,"time%+3.3f       ", time_wd-time_old_wd);
		cout << "       " << output.toString().c_str() << endl;
		time_old_wd=time_wd;
		*/
	}
bool icubFinger::readEncoders(yarp::sig::Vector &encoderValues){
    bool ret = false;


    encoderValues.resize(3);
    encoderValues.zero();

    int pendingReads = _fingerEncoders->getPendingReads();
    Bottle *handEnc = NULL;

    for(int i = 0; i <= pendingReads; i++){
        handEnc = _fingerEncoders->read();
    }

    if(handEnc != NULL)
    {
        encoderValues[0] = handEnc->get(_proximalEncoderIndex).asDouble();
        encoderValues[1] = handEnc->get(_middleEncoderIndex).asDouble();
        encoderValues[2] = handEnc->get(_distalEncoderIndex).asDouble();
        ret = true;

        adjustMinMax(encoderValues[0], _minProximal, _maxProximal);
        adjustMinMax(encoderValues[1], _minMiddle, _maxMiddle);
        adjustMinMax(encoderValues[2], _minDistal, _maxDistal);
    }


    //cout << "Encoders: " << encoderValues.toString() << endl;
    return ret;
}
예제 #7
1
/**
 * Some helper function for converting between Yarp,KDL and Eigen
 * matrix types.
 *
 */
bool toYarp(const KDL::Wrench & ft, yarp::sig::Vector & ft_yrp)
{
    if( ft_yrp.size() != 6 ) { ft_yrp.resize(6); }
    for(int i=0; i < 6; i++ ) {
        ft_yrp[i] = ft[i];
    }
}
예제 #8
1
bool toYarp(const iDynTree::Position& iDynTreePosition, yarp::sig::Vector& yarpVector)
{
    if( yarpVector.size() != 3 )
    {
        yarpVector.resize(3);
    }

    memcpy(yarpVector.data(),iDynTreePosition.data(),3*sizeof(double));
    return true;
}
예제 #9
1
bool toYarp(const Vector3& iDynTreeVector3, yarp::sig::Vector& yarpVector)
{
    if( yarpVector.size() != 3 )
    {
        yarpVector.resize(3);
    }

    memcpy(yarpVector.data(),iDynTreeVector3.data(),3*sizeof(double));
    return true;
}
void MiddleFinger::getTactileDataComp(yarp::sig::Vector &tactileData){
    tactileData.resize(12);
    tactileData.zero();

    Bottle *tactileDataPort = _tactileDataComp_in->read();

    int startIndex = 12;
    if(tactileDataPort != NULL){
        for(int i = startIndex; i < startIndex + 12; ++i){
            tactileData[i - startIndex] = tactileDataPort->get(i).asDouble();
        }
    }

}
bool IndexFinger::getPositionHandFrame(yarp::sig::Vector &position, yarp::sig::Vector &fingerEncoders){

    bool ret = true;
    Vector joints;

    int nEncs;

    position.clear();
    position.resize(3); //x,y, z position


    ret = ret && _armEncoder->getAxes(&nEncs);
    Vector encs(nEncs);
    if(! (ret = ret && _armEncoder->getEncoders(encs.data())))
    {
        cerr << _dbgtag << "Failed to read arm encoder data" << endl;
    }


    //cout << encs.toString() << endl;

    ret = ret && _iCubFinger->getChainJoints(encs, joints);


    if(ret == false){
        cout << "failed to get chain joints" << endl;
        return false;
    }


    // Replace the joins with the encoder readings
    joints[0] = 20; // The index fingertip calibration procedure used this value.
    joints[1] = 90 * (1 - (fingerEncoders[0] - _minProximal) / (_maxProximal - _minProximal) );
    joints[2] = 90 * (1 - (fingerEncoders[1] - _minMiddle) / (_maxMiddle - _minMiddle) );
    joints[3] = 90 * (1 - (fingerEncoders[2] - _minDistal) / (_maxDistal - _minDistal) );

    //cout << joints.size() << endl;
    //Convert the joints to radians.
    for (int j = 0; j < joints.size(); j++)
        joints[j] *= DEG2RAD;


    yarp::sig::Matrix tipFrame = _iCubFinger->getH(joints);
    position = tipFrame.getCol(3); // Tip's position in the hand coordinate


    return ret;
}
예제 #12
0
bool yarp::dev::FrameTransformClient::transformPose(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_pose, yarp::sig::Vector &transformed_pose)
{
    if (input_pose.size() != 6)
    {
        yError() << "sorry.. only 6 dimensional vector (3 axes + roll pith and yaw) allowed, dear friend of mine..";
        return false;
    }
    if (transformed_pose.size() != 6)
    {
        yWarning("FrameTransformClient::transformPose() performance warning: size transformed_pose should be 6, resizing");
        transformed_pose.resize(6, 0.0);
    }
    yarp::sig::Matrix m(4, 4);
    if (!getTransform(target_frame_id, source_frame_id, m))
    {
        yError() << "no transform found between source '" << target_frame_id << "' and target '" << source_frame_id << "'";
        return false;
    }
    FrameTransform t;
    t.transFromVec(input_pose[0], input_pose[1], input_pose[2]);
    t.rotFromRPY(input_pose[3], input_pose[4], input_pose[5]);
    t.fromMatrix(m * t.toMatrix());
    transformed_pose[0] = t.translation.tX;
    transformed_pose[1] = t.translation.tY;
    transformed_pose[2] = t.translation.tZ;

    yarp::sig::Vector rot;
    rot = t.getRPYRot();
    transformed_pose[3] = rot[0];
    transformed_pose[4] = rot[1];
    transformed_pose[5] = rot[2];
    return true;
}
예제 #13
0
bool KinectDriverOpenNI::get3DPoint(int u, int v, yarp::sig::Vector &point3D)
{
    const XnDepthPixel* pDepthMap = depthGenerator.GetDepthMap();
    XnPoint3D p2D, p3D;
    int newU=u;
    int newV=v;
    //request arrives with respect to the 320x240 image, but the depth by default
    // is 640x480 (we resize it before send it to the server)
    if (device==KINECT_TAGS_DEVICE_KINECT)
    {
        newU=u*2;
        newV=v*2;
    }

    p2D.X = newU;
    p2D.Y = newV;
    p2D.Z = pDepthMap[newV*this->def_depth_width+newU];
    depthGenerator.ConvertProjectiveToRealWorld(1, &p2D, &p3D);

    //We provide the 3D point in meters
    point3D.resize(3,0.0);
    point3D[0]=p3D.X/1000;
    point3D[1]=p3D.Y/1000;
    point3D[2]=p3D.Z/1000;
    
    return true;
}
bool ObjectModelGrid::getNextSamplingPos(yarp::sig::Vector& nextSamplingPoint){

    bool ret = false;
    nextSamplingPoint.resize(3);
    // Fix the x coordinate, move along the y
    // increment x when y is at or beyon max

    if(_nextSamplingPoint[1] < _yMax){
        _nextSamplingPoint[1] += _stepSize;
        // cap at max
        if(_nextSamplingPoint[1] > _yMax){
            _nextSamplingPoint[1] = _yMax;

        }
        ret = true;
    }
    else if(_nextSamplingPoint[0] > _xMin){

        _nextSamplingPoint[0] -= _stepSize;
        if(_nextSamplingPoint[0] < _xMin){
            _nextSamplingPoint[0] = _xMin;
        }
        _nextSamplingPoint[1] = _yMin;
        ret = true;
    }
    else{
        ret = false;
    }

    nextSamplingPoint = _nextSamplingPoint;
    return ret;
}
void icubFinger::getAngels(yarp::sig::Vector &angles, yarp::sig::Vector fingerEncoders){

    angles.resize(3);
    angles[0] = 90 * (1 - (fingerEncoders[0] - _minProximal) / (_maxProximal - _minProximal) );
    angles[1] = 90 * (1 - (fingerEncoders[1] - _minMiddle) / (_maxMiddle - _minMiddle) );
    angles[2] = 90 * (1 - (fingerEncoders[2] - _minDistal) / (_maxDistal - _minDistal) );
}
예제 #16
0
/* Both ds and result should be of the same dimension */
bool TactileData::diff(TactileData &ds, yarp::sig::Vector &result, bool ifAbs)
{
	double *data = this->data();
	int size = this->size();
	//check if dimensions of two DataSample match (though it will be always ;) )
	if(ds.size() != size)
	{
		cout << "Dimensions do not match:   " << ds.size() << "   "  << size << endl;
		return false;
	}
	
	if(result.size() != size)
	{
		cout << "WARNING. Dimension of result vector is different. Reset it" << endl;
		result.resize(size);
	}
		
	if(ifAbs)
	{
		for(int i = 0; i < size; i++)
		{
			result[i] = std::fabs(data[i] - ds[i]);
			//result.push_back(this->data[i] - ds[i]);
		}
	}
	else
	{
		for(int i = 0; i < size; i++)
		{
			result[i] = data[i] - ds[i];
			//result.push_back(this->data[i] - ds[i]);
		}
	}
	return true;
}
bool Vector_read(const std::string file_name, yarp::sig::Vector & vec)
{
    FILE * fp;
    uint32_t len;
    double elem;
    
    fp = fopen(file_name.c_str(),"rb");
    if( fp == NULL ) return false;
    
    fread(&len,sizeof(uint32_t),1,fp);
    vec.resize(len);
    
    for(size_t i=0;i<len;i++) { 
        fread(&elem,sizeof(double),1,fp);
        vec[i]=elem;
    }
    
    /*
    if( gsl_vector_fread(fp,(gsl_vector*)vec.getGslVector()) == GSL_EFAILED ) {
        fclose(fp);
        return false;
    }*/
    fclose(fp);
    return true;
}
void DictionaryLearning::f_bow(std::vector<yarp::sig::Vector> & features, yarp::sig::Vector & code)
{
     fprintf(stdout, "Starting bow coding \n");
     double time=Time::now();
     


     code.resize(dictionarySize);
     code=0.0;

     int n_features=features.size();
     for (int feat_idx=0; feat_idx<n_features; feat_idx++)
     {
         float minNorm=(float)1e20;
         int winnerAtom;
         
         //Vector &A=features[feat_idx];
         double *A2 = features[feat_idx].data(); 

         //double time_atom=Time::now();
         for(int atom_idx=0; atom_idx<dictionarySize; atom_idx++)
         {
             //Vector &B=dictionaryBOW[atom_idx];  
             double *B2 = dictionaryBOW[atom_idx].data();        
             //double time_norm=Time::now();
             double norm=0.0;
             for (int i=0; i<featuresize; i++)
             {
                 //double d=A[i]-B[i];
                 double d=A2[i]-B2[i];
                 norm+=d*d;
                                  
                 if (norm > minNorm) 
                     goto ciao;
           
             }                

             if(norm<minNorm)
             {
                 minNorm=norm;
                 winnerAtom=atom_idx;
             }                 
             
             ciao:{}               
                 //time_norm=Time::now()-time_norm;
                 //fprintf(stdout, "norm %f \n",time_norm);
         }
         
         //time_atom=Time::now()-time_atom;
         //fprintf(stdout, "%f \n",time_atom);
         code[winnerAtom]++;
     }
     
     code=code/yarp::math::norm(code);
     time=Time::now()-time;
     fprintf(stdout, "%f \n",time);


}
예제 #19
0
bool toYarp(const Eigen::VectorXd & vec_eigen, yarp::sig::Vector & vec_yrp)
{
    if( vec_yrp.size() != vec_eigen.size() ) { vec_yrp.resize(vec_eigen.size()); }
    if( memcpy(vec_yrp.data(),vec_eigen.data(),sizeof(double)*vec_eigen.size()) != NULL ) {
        return true;
    } else {
        return false;
    }
}
예제 #20
0
void idyntreeMat2yarp(const iDynTreeVectorType & idyntreeVector,
                   yarp::sig::Vector & yarpVector)
{
    yarpVector.resize(idyntreeVector.size());
    for(size_t row=0; row < idyntreeVector.size(); row++)
    {
        yarpVector(row) = idyntreeVector(row);
    }
}
예제 #21
0
bool embObjFTsensor::getTemperatureSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
{
    GET_privData(mPriv).mutex.wait();
    out.resize(1);
    out[0] = GET_privData(mPriv).lastTemperature/10.0; //I need to convert from tenths of degree centigrade to degree centigrade
    timestamp =  GET_privData(mPriv).timestampTemperature;
    GET_privData(mPriv).mutex.post();
    return true;
}
예제 #22
0
void OrientationThread::getAngles(yarp::sig::Vector &angles, int nAngles)
{
    angles.resize(nAngles);
    double factor=360.0/nAngles;
    double tmp=0.0;
    for (int i=0; i<nAngles; i++)
    {
        angles[i]=tmp;
        tmp+=factor;
    }
}
예제 #23
0
bool Rangefinder2DInputPortProcessor::getData(yarp::sig::Vector &ranges)
{
    mutex.wait();
    if (lastBottle.size()==0) { mutex.post(); return false; }
    unsigned int size = lastBottle.get(0).asList()->size();
    ranges.resize(size);
    for (unsigned int i = 0; i < size; i++)
        ranges[i] = lastBottle.get(0).asList()->get(i).asDouble();
    mutex.post();
    return true;
}
예제 #24
0
/*! Read a vector from the sensor.
 * @param out a vector containing the sensor's last readings.
 * @return AS_OK or return code. AS_TIMEOUT if the sensor timed-out.
 **/
int comanFTsensor::read(yarp::sig::Vector &out)
{
    // This method gives data to the analogServer

    mutex.wait();
    status = AS_OK;

    // TODO
    int sensor_idx = 0;
    out.resize(numberOfBoards * _channels);

    for(int idx = 0; idx < numberOfBoards; idx++)
    {
        FtBoard *ftSensor = NULL;
        sensor_idx = FTmap[idx];
        ftSensor = _fts[sensor_idx]; // sensor_ids = boardId
        if( NULL == ftSensor)
        {
            //yError() << "Trying to read data from a non-existing FT sensor " << sensor_idx << ", check your config file";
            for(int tmp_idx = 0; tmp_idx < _channels; tmp_idx++)
                out[idx*_channels + tmp_idx] = 0;

//          Debug purpose, this way the output will be the boardId, to check if it is
//          trying to read from the right board
            out[idx*_channels + 0] = (double) sensor_idx;
            out[idx*_channels + 1] = (double) sensor_idx;
            out[idx*_channels + 2] = (double) sensor_idx;
            out[idx*_channels + 3] = (double) sensor_idx;
            out[idx*_channels + 4] = (double) sensor_idx;
            out[idx*_channels + 5] = (double) sensor_idx;
            continue;
        }


        ts_bc_data_t bc_data;
        ft_bc_data_t &data = bc_data.raw_bc_data.ft_bc_data;

        ftSensor->get_bc_data(bc_data);

        // ci sono 6 valori da leggere per ogni FT, per ora!!
        out[idx*_channels + 0] = (double)data.fx / scaleFactor[idx];
        out[idx*_channels + 1] = (double)data.fy / scaleFactor[idx];
        out[idx*_channels + 2] = (double)data.fz / scaleFactor[idx];
        out[idx*_channels + 3] = (double)data.tx / scaleFactor[idx];
        out[idx*_channels + 4] = (double)data.ty / scaleFactor[idx];
        out[idx*_channels + 5] = (double)data.tz / scaleFactor[idx];
    }
    mutex.post();

    return status;
}
/*! Read a vector from the sensor.
 * @param out a vector containing the sensor's last readings.
 * @return AS_OK or return code. AS_TIMEOUT if the sensor timed-out.
 **/
int embObjAnalogSensor::read(yarp::sig::Vector &out)
{
    // This method gives data to the analogServer

      mutex.wait();

      if (!data)
      {
          mutex.post();
          return false;
      }

      // errors are not handled for now... it'll always be OK!!
      if (status!=IAnalogSensor::AS_OK)
      {
          switch (status)
          {
              case IAnalogSensor::AS_OVF:
                  {
                      counterSat++;
                  }
                  break;
              case IAnalogSensor::AS_ERROR:
                  {
                      counterError++;
                  }
                  break;
              case IAnalogSensor::AS_TIMEOUT:
                  {
                     counterTimeout++;
                  }
                  break;
              default:
              {
                  counterError++;
              }
          }
          mutex.post();
          return status;
      }

      out.resize(data->size());
      for(int k=0;k<data->size();k++)
      {
          out[k]=(*data)[k];
      }

      mutex.post();
    
    return status;
}
예제 #26
0
/*! Read a vector from the sensor.
 * @param out a vector containing the sensor's last readings.
 * @return AS_OK or return code. AS_TIMEOUT if the sensor timed-out.
 **/
int embObjInertials::read(yarp::sig::Vector &out)
{
    // This method gives analogdata to the analogServer

    if(false == opened)
    {
        return false;
    }

    mutex.wait();


    // errors are not handled for now... it'll always be OK!!
    if (status != IAnalogSensor::AS_OK)
    {
        switch (status)
        {
            case IAnalogSensor::AS_OVF:
            {
              counterSat++;
            }  break;
            case IAnalogSensor::AS_ERROR:
            {
              counterError++;
            } break;
            case IAnalogSensor::AS_TIMEOUT:
            {
             counterTimeout++;
            } break;
            default:
            {
              counterError++;
            } break;
        }
        mutex.post();
        return status;
    }

    out.resize(analogdata.size());
    for(int k=0; k<analogdata.size(); k++)
    {
        out[k] = analogdata[k];
    }


    mutex.post();

    return status;

}
int GazeboYarpContactLoadCellArrayDriver::read(yarp::sig::Vector& out)
{
    yarp::os::LockGuard guard(m_dataMutex);

    if (!m_dataAvailable)
    {
        return AS_TIMEOUT;
    }

    out.resize(m_contactNormalForces.size());
    out = m_contactNormalForces;

    return AS_OK;
}
예제 #28
0
/*! Read a vector from the sensor.
 * @param out a vector containing the sensor's last readings.
 * @return AS_OK or return code. AS_TIMEOUT if the sensor timed-out.
 **/
int RobotranYarpForceTorqueDriver::read(yarp::sig::Vector &out)
{
    // This method gives data to the analogServer

    mutex.wait();
    status = AS_OK;

    //data=MBSdataStruct.Qq[];

    out.resize(data.size());

    out = data;
    mutex.post();
    return status;
}
예제 #29
0
파일: modHelp.cpp 프로젝트: arocchi/codyco
    //---------------------------------------------------------
    bool eigenToYarpVector(const Eigen::VectorXd &eigenVector, yarp::sig::Vector &yarpVector)
    {
        if(eigenVector.size() == 0)
        {
            cout<<"ERROR: input vector is empty (eigenToYarpVector)"<<endl;
            return false;
        }

        //resize and fill eigen vector with yarp vector elements
        yarpVector.resize(eigenVector.size());
        for(unsigned int i=0; i <eigenVector.size(); i++)
                yarpVector(i) = eigenVector(i);

        return true;
    };
bool MiddleFinger::getPositionHandFrame(yarp::sig::Vector &position, yarp::sig::Vector &fingerEncoders){
    bool ret = true;

    Vector joints;

    int nEncs;

    position.clear();
    position.resize(3); //x,y, z position


    ret = ret && _armEncoder->getAxes(&nEncs);
    Vector encs(nEncs);
    if(! (ret = ret && _armEncoder->getEncoders(encs.data())))
    {
        cerr << _dbgtag << "Failed to read arm encoder data" << endl;
    }


    //cout << encs.toString() << endl;
    //cout << fingerEncoders.toString() << endl;
    ret = ret && _iCubFinger->getChainJoints(encs, joints);


    if(ret == false){
        cout << "failed to get chain joints" << endl;
        return false;
    }


    //std::cout << "From iCubFinger: " << joints.toString() << endl;
    //This is where it is different from the index finger

    joints[0] = 90 * (1 - (fingerEncoders[0] - _minProximal) / (_maxProximal - _minProximal) );
    joints[1] = 90 * (1 - (fingerEncoders[1] - _minMiddle) / (_maxMiddle - _minMiddle) );
    joints[2] = 90 * (1 - (fingerEncoders[2] - _minDistal) / (_maxDistal - _minDistal) );

    //cout << "From mycalculat: " << joints.toString() << endl;
    //cout << joints.size() << endl;
    //Convert the joints to radians.
    for (int j = 0; j < joints.size(); j++)
        joints[j] *= M_PI/180;

    yarp::sig::Matrix tipFrame = _iCubFinger->getH(joints);
    position = tipFrame.getCol(3); // Tip's position in the hand coordinate

}