Example #1
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;
}
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 #3
0
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
bool dynContact::setForceMoment(const yarp::sig::Vector &_FMu){
    if(!checkVectorDim(_FMu, 6, "force moment"))
        return false;
    bool res = setForce(_FMu.subVector(0,2));
    return res && setMoment(_FMu.subVector(3,5));
}