Example #1
1
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;
}
Example #2
1
/* 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;
}
Example #3
1
bool toiDynTree(const yarp::sig::Vector& yarpVector, VectorDynSize& iDynTreeVector)
{
    iDynTreeVector.resize(yarpVector.size());
    memcpy(iDynTreeVector.data(),yarpVector.data(),yarpVector.size()*sizeof(double));
    return true;

}
Example #4
1
bool YarptoiDynTree(const yarp::sig::Vector & yarpVector, iDynTree::Wrench & iDynTreeWrench)
{
    if( yarpVector.size() != 6 ) return false;
    memcpy(iDynTreeWrench.getLinearVec3().data(),yarpVector.data(),3*sizeof(double));
    memcpy(iDynTreeWrench.getAngularVec3().data(),yarpVector.data()+3,3*sizeof(double));
    return true;
}
Example #5
1
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;
}
Example #6
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)));
        }
    }
}
/**
 * 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];
    }
}
Example #8
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];
}
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;
}
//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;
}
Example #11
1
bool LinearMean::setHyperparameters(const yarp::sig::Vector newHyperPar)
{
	if (newHyperPar.size()!=this->getNumberOfHyperparameters())
		return false;
    meanLinearHyperparam=newHyperPar.subVector(0, newHyperPar.size()-2);
    meanOffset=newHyperPar(newHyperPar.size()-1);
    return true;
}
void ObjectModelGrid::addContactPoint(const yarp::sig::Vector fingertipPosition){

    assert(fingertipPosition.size() == 3);
    cout << "Contact position: " << fingertipPosition.toString() << endl;
    _contactPoints.push_back(fingertipPosition);


}
Example #13
1
bool toEigen(const yarp::sig::Vector & vec_yrp, Eigen::VectorXd & vec_eigen)
{
 if( vec_yrp.size() != vec_eigen.size() ) { vec_eigen.resize(vec_yrp.size()); }
    if( memcpy(vec_eigen.data(),vec_yrp.data(),sizeof(double)*vec_eigen.size()) != NULL ) {
        return true;
    } else {
        return false;
    }
}
Example #14
1
void iCubShoulderConstr::appendVectorValue(yarp::sig::Vector &dest, double val)
{
    yarp::sig::Vector tmp(dest.length()+1);
    for (size_t i=0; i<dest.length(); i++)
        tmp[i]=dest[i];

    dest=tmp;
    dest[dest.length()-1]=val;
}
Example #15
1
bool YarpJointDev::getDOF ( yarp::sig::Vector& curDof ) {

    curDof.clear();

    for ( unsigned i = 0; i < _chain->GetNumberOfJoints(); ++i )
        curDof.push_back ( 1 );

    return true;

}
Example #16
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;
}
Example #17
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;
}
Example #18
1
bool toiDynTree(const yarp::sig::Vector& yarpVector, iDynTree::Vector3& iDynTreeVector3)
{
    if( yarpVector.size() != 3 )
    {
        return false;
    }

    memcpy(iDynTreeVector3.data(),yarpVector.data(),3*sizeof(double));
    return true;
}
Example #19
1
void OpenSoT::tasks::velocity::Postural::setReference(const yarp::sig::Vector &x_desired,
                                                      const yarp::sig::Vector &xdot_desired)
{
    assert(x_desired.size() == _x_size);
    assert(xdot_desired.size() == _x_size);

    _x_desired = x_desired;
    _xdot_desired = xdot_desired;
    this->update_b();
}
bool reactCtrlThread::controlArm(const yarp::sig::Vector &_vels)
{   
    VectorOf<int> jointsToSetA;
    VectorOf<int> jointsToSetT;
    if (!areJointsHealthyAndSet(jointsToSetA,"arm","velocity"))
    {
        yWarning("[reactCtrlThread]Stopping control because arm joints are not healthy!");
        stopControlHelper();
        return false;
    }

    if (useTorso)
    {
        if (!areJointsHealthyAndSet(jointsToSetT,"torso","velocity"))
        {
            yWarning("[reactCtrlThread]Stopping control because torso joints are not healthy!");
            stopControlHelper();
            return false;
        }
    }

    if (!setCtrlModes(jointsToSetA,"arm","velocity"))
    {
        yError("[reactCtrlThread]I am not able to set the arm joints to velocity mode!");
        return false;
    }   

    if (useTorso)
    {
        if (!setCtrlModes(jointsToSetT,"torso","velocity"))
        {
            yError("[reactCtrlThread]I am not able to set the torso joints to velocity mode!");
            return false;
        }
    }

    printMessage(1,"Moving the robot with velocities: %s\n",_vels.toString(3,3).c_str());
    if (useTorso)
    {
        Vector velsT(3,0.0);
        velsT[0] = _vels[2]; //swapping pitch and yaw as per iKin vs. motor interface convention
        velsT[1] = _vels[1];
        velsT[2] = _vels[0]; //swapping pitch and yaw as per iKin vs. motor interface convention
        
        ivelT->velocityMove(velsT.data());
        ivelA->velocityMove(_vels.subVector(3,9).data()); //indexes 3 to 9 are the arm joints velocities
    }
    else
    {
        ivelA->velocityMove(_vels.data()); //if there is not torso, _vels has only the 7 arm joints
    }

    return true;
}
Example #21
1
bool yarpWholeBodyModelV1::convertGeneralizedTorques(yarp::sig::Vector idyntree_base_force, yarp::sig::Vector idyntree_torques, double * tau)
{
    if( idyntree_base_force.size() != 6 && (int)idyntree_torques.size() != p_model->getNrOfDOFs() ) { return false; }
    for(int j = 0; j < 6; j++ ) {
        tau[j] = idyntree_base_force[j];
    }
    for(int wbi_joint_numeric_id=0; wbi_joint_numeric_id < this->dof; wbi_joint_numeric_id++ )
    {
        tau[wbi_joint_numeric_id+6] = idyntree_torques[wbiToiDynTreeJointId[wbi_joint_numeric_id]];
    }

    return true;
}
bool isEqual(const yarp::sig::Vector& a, const yarp::sig::Vector& b, const double& tolerance)
{
    if (a.size() != b.size()) return false;

    for (size_t i = 0; i < a.size(); i++)
    {
        if (fabs(a[i] - b[i]) > tolerance)
        {
            return false;
        }
    }
    return true;
}
Example #23
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);
}
Example #24
1
    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;
		*/
	}
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();
        }
    }

}
Example #26
1
bool toiDynTree(const yarp::sig::Vector& yarpVector, Direction& direction)
{
    if( yarpVector.size() != 3 )
    {
        return false;
    }

    memcpy(direction.data(),yarpVector.data(),3*sizeof(double));

    // normalize
    direction.Normalize();

    return true;
}
void MultiTaskLinearGPRLearner::setWeightsStandardDeviation(const yarp::sig::Vector& s) {
if( s.size() != this->getDomainCols() ) {
        throw std::runtime_error("MultiTaskLinearGPRLearner: wrong dimension of noise std deviation");
    }
    
    inv_Sigma_w = zeros(this->getDomainCols(),this->getDomainCols());
    
    for(unsigned i=0; i < s.size(); i++ ) {
        inv_Sigma_w(i,i) = 1/(s[i]*s[i]);
    }
    
    this->weight_prior_indefinite = false;
    
    this->reset();
}
Example #28
1
    //---------------------------------------------------------
    bool yarpToEigenVector(const yarp::sig::Vector &yarpVector, Eigen::VectorXd &eigenVector)
    {
        if(yarpVector.size() == 0)
        {
            cout<<"ERROR: input vector is empty (yarpToEigenVector)"<<endl;
            return false;
        }

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

        return true;
    };
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;
}
Example #30
1
bool RobotUtils::setReferenceSpeeds(const yarp::sig::Vector &maximum_velocity)
{
    assert(maximum_velocity.size() == this->getNumberOfJoints());

    if(!bodyIsInPositionMode()) {
        std::cout << "Trying to set reference speeds for the whole coman "
                  << "but the robot is not entirely in Position Mode";
        return false;
    }

    yarp::sig::Vector velocity_torso,
                      velocity_right_arm,
                      velocity_left_arm,
                      velocity_right_leg,
                      velocity_left_leg,
                      velocity_head;
    idynutils.fromIDynToRobot(maximum_velocity, velocity_torso, idynutils.torso);
    idynutils.fromIDynToRobot(maximum_velocity, velocity_right_arm, idynutils.right_arm);
    idynutils.fromIDynToRobot(maximum_velocity, velocity_left_arm, idynutils.left_arm);
    idynutils.fromIDynToRobot(maximum_velocity, velocity_right_leg, idynutils.right_leg);
    idynutils.fromIDynToRobot(maximum_velocity, velocity_left_leg, idynutils.left_leg);
    if(head.isAvailable) idynutils.fromIDynToRobot(maximum_velocity, velocity_head, idynutils.head);
    return  torso.setReferenceSpeeds(velocity_torso) &&
            right_arm.setReferenceSpeeds(velocity_right_arm) &&
            left_arm.setReferenceSpeeds(velocity_left_arm) &&
            right_leg.setReferenceSpeeds(velocity_right_leg) &&
            left_leg.setReferenceSpeeds(velocity_left_leg) &&
            (head.isAvailable || head.setReferenceSpeeds(velocity_head));
}