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;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
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));
}