Go to the documentation of this file.
29 #include <yarp/math/Math.h>
33 #define CARTCTRL_SERVER_VER 1.1
34 #define CARTCTRL_DEFAULT_PER 0.01 // [s]
35 #define CARTCTRL_DEFAULT_TASKVEL_PERFACTOR 4
36 #define CARTCTRL_DEFAULT_TOL 1e-2
37 #define CARTCTRL_DEFAULT_TRAJTIME 2.0 // [s]
38 #define CARTCTRL_DEFAULT_POSCTRL "on"
39 #define CARTCTRL_DEFAULT_MULJNTCTRL "on"
40 #define CARTCTRL_CONNECT_SOLVER_PING 1.0 // [s]
43 using namespace yarp::os;
45 using namespace yarp::sig;
46 using namespace yarp::math;
62 if (!
cmd.read(connection))
65 if (server->respond(
cmd,reply))
66 if (ConnectionWriter *writer=connection.getWriter())
88 int pose=command.get(1).asVocab32();
89 double t=command.get(2).asFloat64();
90 Bottle *
v=command.get(3).asList();
95 Vector od(
v->size()-xd.length());
97 for (
size_t i=0; i<xd.length(); i++)
98 xd[i]=
v->get(i).asFloat64();
100 for (
size_t i=0; i<od.length(); i++)
101 od[i]=
v->get(xd.length()+i).asFloat64();
103 server->goToPose(xd,od,t);
107 Vector xd(
v->size());
109 for (
int i=0; i<
v->size(); i++)
110 xd[i]=
v->get(i).asFloat64();
112 server->goToPosition(xd,t);
117 Bottle *
v=command.get(1).asList();
120 Vector odot(
v->size()-xdot.length());
122 for (
size_t i=0; i<xdot.length(); i++)
123 xdot[i]=
v->get(i).asFloat64();
125 for (
size_t i=0; i<odot.length(); i++)
126 odot[i]=
v->get(xdot.length()+i).asFloat64();
128 server->setTaskVelocities(xdot,odot);
138 yAssert(x0.length()>=7);
140 R=axis2dcm(x0.subVector(3,6));
147 yAssert(x0.length()>=7);
148 I->reset(x0.subVector(0,2));
149 R=axis2dcm(x0.subVector(3,6));
156 yAssert(vel.length()>=7);
157 Vector w=vel.subVector(3,6);
160 return cat(I->integrate(vel.subVector(0,2)),dcm2axis(R));
234 string prefixName=
"/";
240 portCmd->open(prefixName+
"/command:i");
243 portRpc.open(prefixName+
"/rpc:i");
292 switch (command.get(0).asVocab32())
305 if (command.size()>3)
307 int pose=command.get(1).asVocab32();
308 double t=command.get(2).asFloat64();
309 Bottle *
v=command.get(3).asList();
310 Vector xd(
v->size());
312 for (
int i=0; i<
v->size(); i++)
313 xd[i]=
v->get(i).asFloat64();
319 lock_guard<mutex> lck(
mtx);
325 lock_guard<mutex> lck(
mtx);
352 Bottle slvCommand=command;
356 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
381 if (command.size()>1)
383 int id=command.get(1).asInt32();
396 if (command.size()>1)
398 Bottle *ids=command.get(1).asList();
411 if (command.size()>1)
412 switch (command.get(1).asVocab32())
459 reply.addString(priority);
474 reply.addFloat64(
time);
489 reply.addFloat64(tol);
532 if (command.size()>2)
534 int axis=command.get(2).asInt32();
540 reply.addFloat64(
min);
541 reply.addFloat64(
max);
559 Bottle &dofPart=reply.addList();
561 for (
size_t i=0; i<curDof.length(); i++)
562 dofPart.addInt32((
int)curDof[i]);
577 reply.addList().read(curRestPos);
588 Vector curRestWeights;
592 reply.addList().read(curRestWeights);
629 reply.addList().read(qdot);
644 Bottle &xdotPart=reply.addList();
646 for (
size_t i=0; i<xdot.length(); i++)
647 xdotPart.addFloat64(xdot[i]);
649 for (
size_t i=0; i<odot.length(); i++)
650 xdotPart.addFloat64(odot[i]);
661 if (command.size()>2)
663 int axis=command.get(2).asInt32();
670 Bottle &posePart=reply.addList();
672 for (
size_t i=0; i<
x.length(); i++)
673 posePart.addFloat64(
x[i]);
675 for (
size_t i=0; i<o.length(); i++)
676 posePart.addFloat64(o[i]);
678 Bottle &stampPart=reply.addList();
679 stampPart.addInt32(stamp.getCount());
680 stampPart.addFloat64(stamp.getTime());
698 Bottle &tipPart=reply.addList();
700 for (
size_t i=0; i<
x.length(); i++)
701 tipPart.addFloat64(
x[i]);
703 for (
size_t i=0; i<o.length(); i++)
704 tipPart.addFloat64(o[i]);
719 reply.addList()=
info;
734 reply.addList()=options;
755 if (command.size()>2)
756 switch (command.get(1).asVocab32())
761 int mode=command.get(2).asVocab32();
780 int mode=command.get(2).asVocab32();
799 string priority=command.get(2).asString();
833 if (command.size()>4)
835 int axis=command.get(2).asInt32();
836 double min=command.get(3).asFloat64();
837 double max=command.get(4).asFloat64();
853 if (Bottle *b=command.get(2).asList())
855 Vector newDof(b->size());
858 for (
int i=0; i<b->size(); i++)
859 newDof[i]=b->get(i).asFloat64();
861 if (
setDOF(newDof,curDof))
864 Bottle &dofPart=reply.addList();
866 for (
size_t i=0; i<curDof.length(); i++)
867 dofPart.addInt32((
int)curDof[i]);
881 if (Bottle *b=command.get(2).asList())
883 Vector newRestPos(b->size());
886 for (
int i=0; i<b->size(); i++)
887 newRestPos[i]=b->get(i).asFloat64();
892 reply.addList().read(curRestPos);
906 if (Bottle *b=command.get(2).asList())
908 Vector newRestWeights(b->size());
909 Vector curRestWeights;
911 for (
int i=0; i<b->size(); i++)
912 newRestWeights[i]=b->get(i).asFloat64();
917 reply.addList().read(curRestWeights);
931 if (Bottle *b=command.get(2).asList())
936 Vector o(b->size()-
x.length());
938 for (
size_t i=0; i<
x.length(); i++)
939 x[i]=b->get(i).asFloat64();
941 for (
size_t i=0; i<o.length(); i++)
942 o[i]=b->get(i+
x.length()).asFloat64();
961 if (Bottle *options=command.get(2).asList())
987 if (command.size()>2)
988 switch (command.get(1).asVocab32())
993 int mode=command.get(2).asVocab32();
997 double checkPoint=command.get(3).asFloat64();
1012 int mode=command.get(2).asVocab32();
1016 double checkPoint=command.get(3).asFloat64();
1031 int mode=command.get(2).asVocab32();
1070 yInfo(
"%s: aligning joints bounds ...",
ctrlName.c_str());
1071 for (
size_t i=0; i<
lDsc.size(); i++)
1073 yInfo(
"part #%lu: %s",(
unsigned long)i,
lDsc[i].key.c_str());
1074 for (
int j=0; j<
lJnt[i]; j++)
1078 yError(
"joint #%d: failed getting limits!",cnt);
1082 yInfo(
"joint #%d: [%g, %g] deg",cnt,
min,
max);
1102 double timeStamp=-1.0;
1104 for (
int i=0; i<
numDrv; i++)
1109 ok=
lPid[i]->getPidReferences(VOCAB_PIDTYPE_POSITION,fbTmp.data());
1112 ok=
lEnt[i]->getEncodersTimed(fbTmp.data(),stamps.data());
1113 timeStamp=
std::max(timeStamp,findMax(stamps.subVector(0,
lJnt[i]-1)));
1116 ok=
lEnc[i]->getEncoders(fbTmp.data());
1120 for (
int j=0; j<
lJnt[i]; j++)
1135 else for (
int j=0; j<
lJnt[i]; j++)
1201 yWarning(
"%s: skipped message from solver due to invalid token (rx=%g)>(thr=%g)",
1213 yWarning(
"%s: skipped message from solver since controller has been stopped (rx=%g)<=(thr=%g)",
1223 Vector _xdes, _qdes;
1228 int l1=(int)b2->size();
1230 int len=l1<l2 ? l1 : l2;
1233 for (
int i=0; i<len; i++)
1234 _xdes[i]=b2->get(i).asFloat64();
1243 int l1=(int)b2->size();
1245 int len=l1<l2 ? l1 : l2;
1248 for (
int i=0; i<len; i++)
1253 yWarning(
"%s: skipped message from solver since does not match the controller dimension (qdes=%d)!=(ctrl=%d)",
1258 else if (!(_qdes==
qdes))
1289 jointsToSet.clear();
1292 lMod[i]->getControlModes(modes.data());
1293 for (
int j=0; j<
lJnt[i]; j++)
1298 if ((
mode==VOCAB_CM_HW_FAULT) || (
mode==VOCAB_CM_IDLE))
1302 if (
mode!=VOCAB_CM_POSITION_DIRECT)
1303 jointsToSet.push_back(chainCnt);
1305 else if (
mode!=VOCAB_CM_VELOCITY)
1306 jointsToSet.push_back(chainCnt);
1320 if (jointsToSet.size()==0)
1326 for (
int i=0; i<
numDrv; i++)
1330 for (
int j=0; j<
lJnt[i]; j++)
1332 if (chainCnt==jointsToSet[k])
1334 joints.push_back(
lRmp[i][j]);
1343 if (joints.size()>0)
1344 lMod[i]->setControlModes((
int)joints.size(),joints.data(),modes.data());
1359 info.addString(
"position");
1360 info.addString(
"multiple");
1368 int joint=
lRmp[j][k];
1371 joints.push_back(joint);
1372 refs.push_back(ref);
1375 ss<<
lDsc[j].key<<
"_"<<joint;
1376 info.addString(ss.str());
1377 info.addFloat64(ref);
1384 if (joints.size()>0)
1385 lPos[j]->setPositions((
int)joints.size(),joints.data(),refs.data());
1408 info.addString(
"velocity");
1409 info.addString(
"multiple");
1416 int joint=
lRmp[j][k];
1418 double thres=
lDsc[j].minAbsVels[k];
1421 if ((vel!=0.0) && (fabs(vel)<thres))
1422 vel=yarp::math::sign(
qdes[cnt]-
fb[cnt])*thres;
1424 joints.push_back(joint);
1425 vels.push_back(vel);
1428 ss<<
lDsc[j].key<<
"_"<<joint;
1429 info.addString(ss.str());
1430 info.addFloat64(vel);
1438 if (joints.size()>0)
1439 lVel[j]->velocityMove((
int)joints.size(),joints.data(),vels.data());
1460 info.addString(
"position");
1461 info.addString(
"single");
1469 int joint=
lRmp[j][k];
1471 lPos[j]->setPosition(joint,ref);
1474 ss<<
lDsc[j].key<<
"_"<<joint;
1475 info.addString(ss.str());
1476 info.addFloat64(ref);
1500 info.addString(
"velocity");
1501 info.addString(
"single");
1508 int joint=
lRmp[j][k];
1510 double thres=
lDsc[j].minAbsVels[k];
1513 if ((vel!=0.0) && (fabs(vel)<thres))
1514 vel=yarp::math::sign(
qdes[cnt]-
fb[cnt])*thres;
1516 lVel[j]->velocityMove(joint,vel);
1519 ss<<
lDsc[j].key<<
"_"<<joint;
1520 info.addString(ss.str());
1521 info.addFloat64(vel);
1545 info.addString(
"single");
1552 int joint=
lRmp[j][k];
1554 ss<<
lDsc[j].key<<
"_"<<joint;
1555 info.addString(ss.str());
1559 lStp[j]->stop(joint);
1560 info.addString(
"stop");
1565 lVel[j]->velocityMove(joint,0.0);
1566 info.addFloat64(0.0);
1594 yInfo(
"Starting %s at %d ms",
ctrlName.c_str(),(
int)(1000.0*getPeriod()));
1603 yInfo(
"%s started successfully",
ctrlName.c_str());
1605 yError(
"%s did not start!",
ctrlName.c_str());
1614 lock_guard<mutex> lck(
mtx);
1625 vector<int> jointsToSet;
1630 string event=
"none";
1663 event=
"motion-onset";
1697 event=
"motion-done";
1727 if (event==
"motion-onset")
1732 if (event==
"motion-done")
1757 yInfo(
"Stopping %s",
ctrlName.c_str());
1769 yInfo(
"***** Configuring cartesian controller *****");
1773 Bottle &optGeneral=config.findGroup(
"GENERAL");
1774 if (optGeneral.isNull())
1776 yError(
"GENERAL group is missing");
1781 yInfo(
"Acquiring options for group GENERAL...");
1784 if (optGeneral.check(
"SolverNameToConnect"))
1785 slvName=optGeneral.find(
"SolverNameToConnect").asString();
1788 yError(
"SolverNameToConnect option is missing");
1793 if (optGeneral.check(
"KinematicPart"))
1795 kinPart=optGeneral.find(
"KinematicPart").asString();
1798 yError(
"Attempt to instantiate an unknown kinematic part");
1799 yError(
"Available parts are: arm, leg, custom");
1806 yError(
"KinematicPart option is missing");
1811 string _partKinType;
1812 if (optGeneral.check(
"KinematicType"))
1814 kinType=optGeneral.find(
"KinematicType").asString();
1816 _partKinType=_kinType.substr(0,_kinType.find(
"_"));
1818 if ((_partKinType!=
"left") && (_partKinType!=
"right"))
1820 yError(
"Attempt to instantiate unknown kinematic type");
1821 yError(
"Available types are: left, right, left_v*, right_v*");
1828 yError(
"KinematicType option is missing");
1833 if (optGeneral.check(
"NumberOfDrivers"))
1835 numDrv=optGeneral.find(
"NumberOfDrivers").asInt32();
1838 yError(
"NumberOfDrivers shall be positive");
1845 yError(
"NumberOfDrivers option is missing");
1851 if (optGeneral.check(
"ControllerName"))
1852 ctrlName=optGeneral.find(
"ControllerName").asString();
1856 yWarning(
"default ControllerName assumed: %s",
ctrlName.c_str());
1859 if (optGeneral.check(
"ControllerPeriod"))
1860 setPeriod((
double)optGeneral.find(
"ControllerPeriod").asInt32()/1000.0);
1871 debugInfoEnabled=optGeneral.check(
"DebugInfo",Value(
"off")).asString()==
"on";
1873 yDebug(
"Commands to robot will be also streamed out on debug port");
1876 for (
int i=0; i<
numDrv; i++)
1878 ostringstream entry;
1879 entry<<
"DRIVER_"<<i;
1880 string entry_str(entry.str());
1882 Bottle &optDrv=config.findGroup(entry_str);
1883 if (optDrv.isNull())
1885 yError(
"%s group is missing",entry_str.c_str());
1890 yInfo(
"Acquiring options for group %s...",entry_str.c_str());
1894 if (optDrv.check(
"Key"))
1895 desc.
key=optDrv.find(
"Key").asString();
1898 yError(
"Key option is missing");
1903 if (optDrv.check(
"JointsOrder"))
1905 string jointsOrderType=optDrv.find(
"JointsOrder").asString();
1907 if (jointsOrderType==
"direct")
1909 else if (jointsOrderType==
"reversed")
1913 yError(
"Attempt to select an unknown mapping order");
1914 yError(
"Available orders are: direct, reversed");
1921 yError(
"JointsOrder option is missing");
1926 if (Bottle *pMinAbsVelsBottle=optDrv.find(
"MinAbsVels").asList())
1929 desc.
minAbsVels.resize(pMinAbsVelsBottle->size());
1931 for (
int j=0; j<pMinAbsVelsBottle->size(); j++)
1933 desc.
minAbsVels[j]=pMinAbsVelsBottle->get(j).asFloat64();
1941 yWarning(
"MinAbsVels option is missing ... using default values");
1945 lDsc.push_back(desc);
1949 Bottle &optPlantModel=config.findGroup(
"PLANT_MODEL");
1950 if (!optPlantModel.isNull())
1952 yInfo(
"PLANT_MODEL group detected");
1968 else if (optGeneral.check(
"CustomKinFile"))
1970 ResourceFinder rf_kin;
1971 if (optGeneral.check(
"CustomKinContext"))
1972 rf_kin.setDefaultContext(optGeneral.find(
"CustomKinContext").asString());
1973 rf_kin.configure(0,NULL);
1974 string pathToCustomKinFile=rf_kin.findFileByName(optGeneral.find(
"CustomKinFile").asString());
1976 Property linksOptions;
1982 yError(
"Invalid links parameters");
1989 yError(
"CustomKinFile option is missing");
1995 yInfo()<<
"DH Table: "<<DHTable.toString();
2037 yInfo(
"***** Attaching drivers to cartesian controller %s *****",
ctrlName.c_str());
2038 yInfo(
"Received list of %d driver(s)",nd);
2042 yError(
"Expected list of %d driver(s)",
numDrv);
2053 for (
int i=0; i<
numDrv; i++)
2055 yInfo(
"Acquiring info on driver %s...",
lDsc[i].key.c_str());
2059 for (j=0; j<
drivers.size(); j++)
2065 yError(
"None of provided drivers is of type %s",
lDsc[i].key.c_str());
2070 if (
drivers[j]->poly->isValid())
2072 yInfo(
"driver %s successfully open",
lDsc[i].key.c_str());
2076 IEncodersTimed *ent;
2078 IControlLimits *lim;
2079 IVelocityControl *vel;
2080 IPositionDirect *
pos;
2081 IPositionControl *stp;
2095 enc->getAxes(&joints);
2103 if (joints>remainingJoints)
2104 joints=remainingJoints;
2106 remainingJoints-=joints;
2108 int *rmpTmp=
new int[joints];
2109 for (
int k=0; k<joints; k++)
2110 rmpTmp[k]=
lDsc[i].jointsDirectOrder ? k : (joints-k-1);
2113 if (
lDsc[i].useDefaultMinAbsVel)
2114 lDsc[i].minAbsVels.resize(joints,0.0);
2115 else if (
lDsc[i].minAbsVels.length()<(
size_t)joints)
2117 Vector
tmp=
lDsc[i].minAbsVels;
2118 lDsc[i].minAbsVels.resize(joints,0.0);
2120 for (
size_t k=0; k<
tmp.length(); k++)
2124 lMod.push_back(mod);
2125 lEnc.push_back(enc);
2126 lEnt.push_back(ent);
2127 lPid.push_back(pid);
2128 lLim.push_back(lim);
2130 lStp.push_back(stp);
2131 lJnt.push_back(joints);
2132 lRmp.push_back(rmpTmp);
2133 lVel.push_back(vel);
2137 yError(
"unable to open driver %s",
lDsc[i].key.c_str());
2142 yInfo(
"%s: IControlMode %s",
ctrlName.c_str(),
2145 yInfo(
"%s: %s interface will be used",
ctrlName.c_str(),
2148 yInfo(
"%s: IPidControl %s",
ctrlName.c_str(),
2151 yInfo(
"%s: IPositionDirect %s",
ctrlName.c_str(),
2155 yInfo(
"%s: %s interface will be used",
ctrlName.c_str(),
2158 yInfo(
"%s: %s control will be used",
ctrlName.c_str(),
2175 yError(
"unable to retrieve joints limits");
2181 for (
int i=0; i<
numDrv; i++)
2182 for (
int j=0; j<
lJnt[i]; j++)
2221 for (
unsigned int i=0; i<
lRmp.size(); i++)
2250 string portSlvName=
"/";
2251 portSlvName=portSlvName+
slvName+
"/in";
2253 bool ok=Network::exists(portSlvName,
true);
2254 yInfo(
"%s: Checking if cartesian solver %s is alive... %s",
2266 yInfo(
"%s: Connecting to cartesian solver %s...",
ctrlName.c_str(),
slvName.c_str());
2268 string portSlvName=
"/";
2269 portSlvName=portSlvName+
slvName;
2273 ok&=Network::connect(portSlvName+
"/out",
portSlvIn.getName(),
"udp");
2274 ok&=Network::connect(
portSlvOut.getName(),portSlvName+
"/in",
"udp");
2275 ok&=Network::connect(
portSlvRpc.getName(),portSlvName+
"/rpc");
2278 yInfo(
"%s: Connections established with %s",
ctrlName.c_str(),
slvName.c_str());
2281 yError(
"%s: Problems detected while connecting to %s",
ctrlName.c_str(),
slvName.c_str());
2290 Bottle command, reply;
2297 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
2303 Bottle *rxDofPart=reply.get(1).asList();
2304 for (
int i=0; i<rxDofPart->size(); i++)
2306 if (rxDofPart->get(i).asInt32()!=0)
2328 const double t,
const bool latchToken)
2374 Bottle command, reply;
2382 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
2399 lock_guard<mutex> lck(
mtx);
2447 if (
connected && ((
p==
"position") || (
p==
"orientation")))
2449 lock_guard<mutex> lck(
mtx);
2451 Bottle command, reply;
2460 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
2473 Bottle command, reply;
2482 "position":
"orientation";
2485 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
2497 lock_guard<mutex> lck(
mtx);
2501 o.resize(pose.length()-
x.length());
2503 for (
size_t i=0; i<
x.length(); i++)
2506 for (
size_t i=0; i<o.length(); i++)
2507 o[i]=pose[
x.length()+i];
2521 Vector &o, Stamp *stamp)
2525 lock_guard<mutex> lck(
mtx);
2533 for (
size_t i=0; i<
x.length(); i++)
2558 lock_guard<mutex> lck(
mtx);
2560 Vector _xd(xd.length()+od.length());
2561 for (
size_t i=0; i<xd.length(); i++)
2564 for (
size_t i=0; i<od.length(); i++)
2565 _xd[xd.length()+i]=od[i];
2580 lock_guard<mutex> lck(
mtx);
2611 lock_guard<mutex> lck(
mtx);
2614 odhat.resize(
xdes.length()-3);
2616 for (
size_t i=0; i<xdhat.length(); i++)
2619 for (
size_t i=0; i<odhat.length(); i++)
2620 odhat[i]=
xdes[xdhat.length()+i];
2642 Vector &xdhat, Vector &odhat,
2648 lock_guard<mutex> lck(
mtx);
2650 Bottle command, reply;
2651 Vector tg(xd.length()+od.length());
2652 for (
size_t i=0; i<xd.length(); i++)
2655 for (
size_t i=0; i<od.length(); i++)
2656 tg[xd.length()+i]=od[i];
2667 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
2675 const Vector &od, Vector &xdhat,
2676 Vector &odhat, Vector &qdhat)
2681 lock_guard<mutex> lck(
mtx);
2683 Bottle command, reply;
2684 Vector tg(xd.length()+od.length());
2685 for (
size_t i=0; i<xd.length(); i++)
2688 for (
size_t i=0; i<od.length(); i++)
2689 tg[xd.length()+i]=od[i];
2701 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
2709 Vector &odhat, Vector &qdhat)
2714 lock_guard<mutex> lck(
mtx);
2716 Bottle command, reply;
2726 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
2734 Vector &xdhat, Vector &odhat,
2740 lock_guard<mutex> lck(
mtx);
2742 Bottle command, reply;
2753 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
2764 lock_guard<mutex> lck(
mtx);
2782 lock_guard<mutex> lck(
mtx);
2784 Bottle command, reply;
2787 Bottle &txDofPart=command.addList();
2788 for (
size_t i=0; i<newDof.length(); i++)
2789 txDofPart.addInt32((
int)newDof[i]);
2797 Bottle *rxDofPart=reply.get(1).asList();
2798 curDof.resize((
size_t)rxDofPart->size());
2799 for (
int i=0; i<rxDofPart->size(); i++)
2801 curDof[i]=rxDofPart->get(i).asInt32();
2819 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
2833 lock_guard<mutex> lck(
mtx);
2835 Bottle command, reply;
2843 Bottle *rxRestPart=reply.get(1).asList();
2844 curRestPos.resize(rxRestPart->size());
2845 for (
int i=0; i<rxRestPart->size(); i++)
2846 curRestPos[i]=rxRestPart->get(i).asFloat64();
2851 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
2866 lock_guard<mutex> lck(
mtx);
2868 Bottle command, reply;
2871 command.addList().read(newRestPos);
2877 Bottle *rxRestPart=reply.get(1).asList();
2878 curRestPos.resize(rxRestPart->size());
2879 for (
int i=0; i<rxRestPart->size(); i++)
2880 curRestPos[i]=rxRestPart->get(i).asFloat64();
2885 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
2899 lock_guard<mutex> lck(
mtx);
2901 Bottle command, reply;
2909 Bottle *rxRestPart=reply.get(1).asList();
2910 curRestWeights.resize(rxRestPart->size());
2911 for (
int i=0; i<rxRestPart->size(); i++)
2912 curRestWeights[i]=rxRestPart->get(i).asFloat64();
2917 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
2928 Vector &curRestWeights)
2932 lock_guard<mutex> lck(
mtx);
2934 Bottle command, reply;
2937 command.addList().read(newRestWeights);
2943 Bottle *rxRestPart=reply.get(1).asList();
2944 curRestWeights.resize(rxRestPart->size());
2945 for (
int i=0; i<rxRestPart->size(); i++)
2946 curRestWeights[i]=rxRestPart->get(i).asFloat64();
2951 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
2967 lock_guard<mutex> lck(
mtx);
2970 Bottle command, reply;
2973 command.addInt32(axis);
2977 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
2980 *
min=reply.get(1).asFloat64();
2981 *
max=reply.get(2).asFloat64();
2998 lock_guard<mutex> lck(
mtx);
3000 Bottle command, reply;
3003 command.addInt32(axis);
3004 command.addFloat64(
min);
3005 command.addFloat64(
max);
3011 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
3047 lock_guard<mutex> lck(
mtx);
3094 Matrix Des=axis2dcm(
xdes.subVector(3,6));
3095 Vector ax=dcm2axis(Des*SE3inv(
H));
3108 lock_guard<mutex> lck(
mtx);
3118 lock_guard<mutex> lck(
mtx);
3132 lock_guard<mutex> lck(
mtx);
3135 Vector taskVel(7,0.0);
3137 if ((J.rows()>0) && (J.cols()==
velCmd.length()))
3141 Vector _odot=taskVel.subVector(3,(
unsigned int)taskVel.length()-1);
3142 double thetadot=
norm(_odot);
3146 taskVel[3]=_odot[0];
3147 taskVel[4]=_odot[1];
3148 taskVel[5]=_odot[2];
3149 taskVel.push_back(thetadot);
3153 odot.resize(taskVel.length()-xdot.length());
3155 for (
size_t i=0; i<xdot.length(); i++)
3158 for (
size_t i=0; i<odot.length(); i++)
3159 odot[i]=taskVel[xdot.length()+i];
3174 lock_guard<mutex> lck(
mtx);
3176 for (
int i=0; i<3; i++)
3179 for (
size_t i=3; i<
xdot_set.length(); i++)
3205 if (
connected && (
x.length()>=3) || (o.length()>=4))
3207 lock_guard<mutex> lck(
mtx);
3209 Bottle command, reply;
3212 Bottle &txTipPart=command.addList();
3214 for (
int i=0; i<3; i++)
3215 txTipPart.addFloat64(
x[i]);
3217 for (
int i=0; i<4; i++)
3218 txTipPart.addFloat64(o[i]);
3226 Matrix HN=axis2dcm(o);
3235 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
3249 lock_guard<mutex> lck(
mtx);
3285 const double timeout)
3288 double t0=Time::now();
3292 Time::delay(period);
3336 lock_guard<mutex> lck(
mtx);
3350 Vector _dof,_restPos,_restWeights;
3351 Vector _tip_x,_tip_o;
3358 Matrix _limits(N,2);
3359 for (
unsigned int axis=0; axis<N; axis++)
3363 _limits(axis,0)=
min;
3364 _limits(axis,1)=
max;
3367 double _trajTime,_tol;
3368 bool _mode,_useReference;
3375 lock_guard<mutex> lck(
mtx);
3382 context.
tip_x=_tip_x;
3383 context.
tip_o=_tip_o;
3408 map<int,Context>::iterator itr=
contextMap.find(
id);
3412 context=itr->second;
3448 map<int,Context>::iterator itr=
contextMap.find(
id);
3462 if (contextIdList!=NULL)
3464 lock_guard<mutex> lck(
mtx);
3466 for (
int i=0; i<contextIdList->size(); i++)
3468 int id=contextIdList->get(i).asInt32();
3469 map<int,Context>::iterator itr=
contextMap.find(
id);
3488 Bottle &serverVer=
info.addList();
3489 serverVer.addString(
"server_version");
3495 size_t pos=type.find(
"_v");
3497 if (
pos!=string::npos)
3498 hwVer=strtod(type.substr(
pos+2).c_str(),NULL);
3500 Bottle &partVer=
info.addList();
3501 partVer.addString(kinPartStr+
"_version");
3502 partVer.addFloat64(hwVer);
3504 Bottle &partType=
info.addList();
3505 partType.addString(kinPartStr+
"_type");
3506 partType.addString(type);
3508 Bottle &events=
info.addList();
3509 events.addString(
"events");
3510 Bottle &eventsList=events.addList();
3511 eventsList.addString(
"motion-onset");
3512 eventsList.addString(
"motion-done");
3513 eventsList.addString(
"motion-ongoing");
3514 eventsList.addString(
"closing");
3515 eventsList.addString(
"*");
3526 const double checkPoint)
3529 map<string,CartesianEvent*>::iterator itr;
3536 ev.addString(event);
3537 ev.addFloat64(
time);
3538 if (checkPoint>=0.0)
3539 ev.addFloat64(checkPoint);
3550 if (itr->second!=NULL)
3552 CartesianEvent &Event=*itr->second;
3553 Event.cartesianEventVariables.type=event;
3554 Event.cartesianEventVariables.time=
time;
3556 if (checkPoint>=0.0)
3557 Event.cartesianEventVariables.motionOngoingCheckPoint=checkPoint;
3559 Event.cartesianEventCallback();
3563 string typeExtended=event;
3564 if (checkPoint>=0.0)
3567 ss<<
event<<
"-"<<checkPoint;
3568 typeExtended=ss.str();
3575 if (itr->second!=NULL)
3577 CartesianEvent &Event=*itr->second;
3578 Event.cartesianEventVariables.type=event;
3579 Event.cartesianEventVariables.time=
time;
3581 if (checkPoint>=0.0)
3582 Event.cartesianEventVariables.motionOngoingCheckPoint=checkPoint;
3584 Event.cartesianEventCallback();
3596 string type=
event.cartesianEventParameters.type;
3597 if (type==
"motion-ongoing")
3599 double checkPoint=
event.cartesianEventParameters.motionOngoingCheckPoint;
3603 ss<<type<<
"-"<<checkPoint;
3618 string type=
event.cartesianEventParameters.type;
3619 if (type==
"motion-ongoing")
3621 double checkPoint=
event.cartesianEventParameters.motionOngoingCheckPoint;
3625 ss<<type<<
"-"<<
event.cartesianEventParameters.motionOngoingCheckPoint;
3664 if ((checkPoint>=0.0) && (checkPoint<=1.0))
3666 lock_guard<mutex> lck(
mtx);
3679 if ((checkPoint>=0.0) && (checkPoint<=1.0))
3681 lock_guard<mutex> lck(
mtx);
3699 lock_guard<mutex> lck(
mtx);
3701 events.addFloat64(*itr);
3713 Bottle command, reply;
3724 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
3737 lock_guard<mutex> lck(
mtx);
3739 Bottle command, reply;
3748 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
3761 Bottle command, reply;
3769 options=*reply.get(1).asList();
3772 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
3785 lock_guard<mutex> lck(
mtx);
3787 Bottle command, reply;
3790 command.addList()=options;
3796 yError(
"%s: unable to get reply from solver!",
ctrlName.c_str());
3809 if (options.check(
"straightness"))
3813 if (options.check(
"task_2"))
3817 if (options.check(
"tol") || options.check(
"constr_tol") ||
3818 options.check(
"max_iter"))
3830 lock_guard<mutex> lck(
mtx);
3836 Bottle &straightness=options.addList();
3837 straightness.addString(
"straightness");
3845 Bottle &task2=options.addList();
3846 task2.addString(
"task_2");
bool getDesired(yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
CartesianCtrlCommandPort * portCmd
bool attachTipFrame(const yarp::sig::Vector &x, const yarp::sig::Vector &o)
iCub::iKin::iKinLimb * limbState
static yarp::os::Bottle * getEndEffectorPoseOption(const yarp::os::Bottle &b)
Retrieves the end-effector pose data.
yarp::os::Bottle sendCtrlCmdSingleJointVelocity()
CartesianCtrlRpcProcessor * rpcProcessor
#define IKINCARTCTRL_VOCAB_OPT_MODE
#define IKINCARTCTRL_VOCAB_OPT_POSE
#define IKINCARTCTRL_VOCAB_OPT_INFO
#define IKINCTRL_POSE_XYZ
yarp::os::Bottle solverConvergence
#define IKINCARTCTRL_VOCAB_OPT_TIME
#define IKINCARTCTRL_VOCAB_OPT_TWEAK
bool multipleJointsControlEnabled
#define IKINCARTCTRL_VOCAB_VAL_EVENT_ONGOING
virtual yarp::sig::Matrix get_J() const
Returns the actual Jacobian used in computation.
yarp::os::Stamp debugInfo
yarp::dev::PolyDriverList drivers
#define IKINSLV_VOCAB_REP_NACK
#define IKINCARTCTRL_VOCAB_CMD_EVENT
void setJointsCtrlMode(const std::vector< int > &jointsToSet)
#define IKINCARTCTRL_VOCAB_VAL_TRUE
#define IKINSLV_VOCAB_OPT_TIP_FRAME
#define IKINCARTCTRL_VOCAB_OPT_ISSOLVERON
#define CARTCTRL_DEFAULT_MULJNTCTRL
#define IKINCARTCTRL_VOCAB_OPT_REGISTER
#define IKINCARTCTRL_VOCAB_OPT_UNREGISTER
#define IKINCARTCTRL_VOCAB_OPT_REST_POS
double set_execTime(const double _execTime, const bool warn=false)
Sets the task execution time in seconds.
yarp::sig::Vector xdot_set
bool goTo(unsigned int _ctrlPose, const yarp::sig::Vector &xd, const double t, const bool latchToken=false)
yarp::os::Bottle sendCtrlCmdSingleJointPosition()
#define IKINCARTCTRL_VOCAB_CMD_DELETE
bool getReferenceMode(bool *f)
bool registerMotionOngoingEvent(const double checkPoint)
bool goToPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0)
bool unregisterEvent(yarp::dev::CartesianEvent &event)
std::multiset< double > motionOngoingEventsCurrent
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd, yarp::sig::Vector *xdot_set, const unsigned int verbose)
bool goToPosition(const yarp::sig::Vector &xd, const double t=0.0)
int taskRefVelPeriodFactor
virtual yarp::sig::Vector get_x() const
Returns the actual cartesian position of the End-Effector.
bool setTaskVelocities(const yarp::sig::Vector &xdot, const yarp::sig::Vector &odot)
#define CARTCTRL_CONNECT_SOLVER_PING
#define IKINSLV_VOCAB_VAL_PRIO_XYZ
bool setReferenceMode(const bool f)
yarp::os::Bottle(ServerCartesianController::* sendCtrlCmd)()
#define IKINCARTCTRL_VOCAB_CMD_RESTORE
std::deque< yarp::dev::IPositionControl * > lStp
#define IKINCTRL_POSE_ANG
bool getInTargetTol(double *tol)
#define IKINCARTCTRL_VOCAB_VAL_FALSE
#define IKINCARTCTRL_VOCAB_CMD_GO
yarp::os::Property plantModelProperties
#define CARTCTRL_SERVER_VER
void stopLimb(const bool execStopPosition=true)
#define IKINCARTCTRL_VOCAB_CMD_SET
yarp::os::Stamp eventInfo
yarp::sig::Vector restPos
bool askForPosition(const yarp::sig::Vector &xd, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
bool releaseLink(const unsigned int i)
Releases the ith Link.
#define CARTCTRL_DEFAULT_PER
bool open(yarp::os::Searchable &config)
std::deque< DriverDescriptor > lDsc
#define IKINCARTCTRL_VOCAB_OPT_XDOT
void onRead(yarp::os::Bottle &command)
yarp::sig::Vector restWeights
#define IKINSLV_VOCAB_CMD_ASK
#define IKINCARTCTRL_VOCAB_VAL_MODE_TRACK
yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false)
Returns the rigid roto-translation matrix from the root reference frame to the ith frame in Denavit-H...
unsigned int getDOF() const
Returns the current number of Chain's DOF.
bool tweakSet(const yarp::os::Bottle &options)
static yarp::os::Bottle * getJointsOption(const yarp::os::Bottle &b)
Retrieves the joints configuration data.
bool getJointsVelocities(yarp::sig::Vector &qdot)
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
virtual void setInTargetTol(double tol_x)
Sets tolerance for in-target check (5e-3 by default).
bool setDOF(const yarp::sig::Vector &newDof, yarp::sig::Vector &curDof)
bool checkMotionDone(bool *f)
std::map< int, Context > contextMap
std::multiset< double > motionOngoingEvents
#define IKINCTRL_POSE_FULL
bool areJointsHealthyAndSet(std::vector< int > &jointsToSet)
yarp::os::Bottle sendCtrlCmdMultipleJointsPosition()
bool setTrajTimeHelper(const double t)
bool getDOF(yarp::sig::Vector &curDof)
#define IKINCARTCTRL_VOCAB_OPT_LIST
#define IKINSLV_VOCAB_VAL_PRIO_ANG
bool restoreContext(const int id)
#define IKINCARTCTRL_VOCAB_OPT_TOL
iCub::iKin::iKinChain * chainPlan
yarp::sig::Vector getAng()
Returns the current free joint angles values.
std::deque< yarp::dev::IPositionDirect * > lPos
bool toLinksProperties(yarp::os::Property &options)
Provides the links attributes listed in a property object.
void motionOngoingEventsFlush()
std::deque< yarp::dev::IVelocityControl * > lVel
std::deque< yarp::dev::IEncodersTimed * > lEnt
virtual bool isInTarget()
Checks if the End-Effector is in target.
bool setHN(const yarp::sig::Matrix &_HN)
Sets HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
void reset(const yarp::sig::Vector &x0)
static bool getDesiredOption(const yarp::os::Bottle &reply, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
std::deque< yarp::dev::IControlLimits * > lLim
#define IKINCARTCTRL_VOCAB_CMD_STORE
bool attachAll(const yarp::dev::PolyDriverList &p)
#define IKINCARTCTRL_VOCAB_OPT_X
#define IKINCARTCTRL_VOCAB_OPT_LIM
yarp::os::RpcServer portRpc
servercartesiancontroller : implements the server part of the Cartesian Interface.
bool getLimits(const int axis, double *min, double *max)
ServerCartesianController()
#define IKINCARTCTRL_VOCAB_VAL_MODE_SINGLE
yarp::os::BufferedPort< yarp::os::Bottle > portEvent
bool getTrajTime(double *t)
double get_gamma() const
Returns the parameter gamma which is used to blend the contribute of the task controller versus the c...
SmithPredictor smithPredictor
bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
bool setTrajTime(const double t)
static void addVectorOption(yarp::os::Bottle &b, const int vcb, const yarp::sig::Vector &v)
#define IKINSLV_VOCAB_OPT_REST_WEIGHTS
bool setRestPos(const yarp::sig::Vector &newRestPos, yarp::sig::Vector &curRestPos)
virtual ~ServerCartesianController()
#define IKINCARTCTRL_VOCAB_REP_NACK
std::deque< yarp::dev::IEncoders * > lEnc
unsigned int getN() const
Returns the number of Links belonging to the Chain.
CartesianCtrlRpcProcessor(ServerCartesianController *server)
#define IKINSLV_VOCAB_OPT_REST_POS
yarp::sig::Vector minAbsVels
#define IKINCARTCTRL_VOCAB_OPT_PRIO
#define CARTCTRL_DEFAULT_TRAJTIME
bool deleteContext(const int id)
#define CARTCTRL_DEFAULT_TOL
bool setTrackingMode(const bool f)
double norm(const yarp::sig::Matrix &M, int col)
#define IKINSLV_VOCAB_OPT_XD
yarp::os::BufferedPort< yarp::sig::Vector > portState
#define IKINSLV_VOCAB_VAL_MODE_TRACK
#define IKINSLV_VOCAB_OPT_X
#define IKINCARTCTRL_VOCAB_OPT_REST_WEIGHTS
bool setInTargetTolHelper(const double tol)
std::deque< yarp::dev::IPidControl * > lPid
bool getRestPos(yarp::sig::Vector &curRestPos)
bool unregisterMotionOngoingEvent(const double checkPoint)
bool getRestWeights(yarp::sig::Vector &curRestWeights)
yarp::sig::Matrix getHN() const
Returns HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
#define IKINCARTCTRL_VOCAB_VAL_POSE_XYZ
#define IKINSLV_VOCAB_OPT_DOF
virtual ~TaskRefVelTargetGenerator()
yarp::os::BufferedPort< yarp::os::Bottle > portSlvIn
bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
unsigned int get_dim() const
Returns the number of Chain DOF.
#define IKINCARTCTRL_VOCAB_VAL_POSE_FULL
#define IKINSLV_VOCAB_CMD_SET
#define IKINSLV_VOCAB_CMD_GET
iCub::iKin::MultiRefMinJerkCtrl * ctrl
#define IKINSLV_VOCAB_OPT_TASK2
bool setRestWeights(const yarp::sig::Vector &newRestWeights, yarp::sig::Vector &curRestWeights)
bool askForPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
CartesianCtrlCommandPort(ServerCartesianController *server)
bool registerEvent(yarp::dev::CartesianEvent &event)
#define IKINCARTCTRL_VOCAB_OPT_Q
yarp::os::Bottle listMotionOngoingEvents()
bool setSolverConvergenceOptions(const yarp::os::Bottle &options)
yarp::os::BufferedPort< yarp::os::Bottle > portDebugInfo
void restart(const yarp::sig::Vector &y0)
void configure(const yarp::os::Property &options, iCub::iKin::iKinChain &chain)
bool getTask2ndOptions(yarp::os::Value &v)
double getFeedback(yarp::sig::Vector &_fb)
bool setLimits(const int axis, const double min, const double max)
void set_ctrlPose(unsigned int _ctrlPose)
Sets the state of Pose control settings.
yarp::sig::Vector computeCmd(const yarp::sig::Vector &u)
bool setBlockingValue(const unsigned int i, double Ang)
Changes the value of the ith blocked Link.
bool setPosePriority(const std::string &p)
bool goToPoseSync(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0)
yarp::os::BufferedPort< yarp::os::Bottle > portSlvOut
void motionOngoingEventsHandling()
#define IKINSLV_VOCAB_OPT_MODE
#define IKINCARTCTRL_VOCAB_OPT_MOTIONDONE
#define IKINSLV_VOCAB_OPT_LIM
#define IKINSLV_VOCAB_VAL_MODE_SINGLE
bool getSolverConvergenceOptions(yarp::os::Bottle &options)
#define CARTCTRL_DEFAULT_POSCTRL
#define CARTCTRL_DEFAULT_TASKVEL_PERFACTOR
bool read(yarp::os::ConnectionReader &connection)
#define IKINSLV_VOCAB_REP_ACK
#define IKINSLV_VOCAB_OPT_CONVERGENCE
std::map< std::string, yarp::dev::CartesianEvent * > eventsMap
bool waitMotionDone(const double period=0.1, const double timeout=0.0)
#define IKINCARTCTRL_VOCAB_REP_ACK
friend class CartesianCtrlRpcProcessor
static void addPoseOption(yarp::os::Bottle &b, const unsigned int pose)
Appends to a bottle all data needed to change the pose mode.
#define IKINSLV_VOCAB_OPT_Q
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
static void addTokenOption(yarp::os::Bottle &b, const double token)
Appends to a bottle a token to be exchanged with the solver.
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
double txTokenLatchedGoToRpc
static bool getTokenOption(const yarp::os::Bottle &b, double *token)
Retrieves the token from the bottle.
void setPlantParameters(const yarp::os::Property ¶meters, const std::string &entryTag="dimension")
Allows user to assign values to the parameters of plant under control (for the configuration space on...
friend class CartesianCtrlCommandPort
bool blockLink(const unsigned int i, double Ang)
Blocks the ith Link at the a certain value of its joint angle.
yarp::os::RpcClient portSlvRpc
virtual double getInTargetTol() const
Returns tolerance for in-target check.
bool getTrackingMode(bool *f)
bool isValid() const
Checks if the limb has been properly configured.
#define IKINCARTCTRL_VOCAB_OPT_DES
static void addTargetOption(yarp::os::Bottle &b, const yarp::sig::Vector &xd)
Appends to a bottle all data needed to command a target.
bool setInTargetTol(const double tol)
#define IKINCARTCTRL_VOCAB_OPT_TIP_FRAME
static void addModeOption(yarp::os::Bottle &b, const bool tracking)
Appends to a bottle all data needed to change the tracking mode.
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
void set_gamma(double _gamma)
Sets the parameter gamma which is used to blend the contribute of the task controller versus the cont...
yarp::sig::Vector integrate(const yarp::sig::Vector &vel)
std::string getType() const
Returns the Limb type as a string.
#define IKINCARTCTRL_VOCAB_CMD_STOP
bool getTaskVelocities(yarp::sig::Vector &xdot, yarp::sig::Vector &odot)
bool getTipFrame(yarp::sig::Vector &x, yarp::sig::Vector &o)
yarp::os::Bottle sendCtrlCmdMultipleJointsVelocity()
TaskRefVelTargetGenerator(const double Ts, const yarp::sig::Vector &x0)
std::condition_variable cv_syncEvent
void notifyEvent(const std::string &event, const double checkPoint=-1.0)
iCub::iKin::iKinLimb * limbPlan
virtual void set_q(const yarp::sig::Vector &q0)
Sets the joint angles values.
#define IKINCARTCTRL_VOCAB_OPT_QDOT
bool getPosePriority(std::string &p)
#define IKINSLV_VOCAB_OPT_PRIO
bool goToPositionSync(const yarp::sig::Vector &xd, const double t=0.0)
double txTokenLatchedStopControl
#define IKINCARTCTRL_VOCAB_CMD_ASK
bool storeContext(int *id)
#define IKINCARTCTRL_VOCAB_OPT_REFERENCE
bool setTrackingModeHelper(const bool f)
#define IKINCARTCTRL_VOCAB_OPT_DOF
bool tweakGet(yarp::os::Bottle &options)
bool setTask2ndOptions(const yarp::os::Value &v)
static string pathToCustomKinFile
void add_compensation(const yarp::sig::Vector &comp)
Adds to the controller input a further compensation term.
virtual yarp::sig::Vector get_q() const
Returns the actual joint angles values.
bool deleteContexts(yarp::os::Bottle *contextIdList)
#define IKINCARTCTRL_VOCAB_CMD_TASKVEL
std::deque< yarp::dev::IControlMode * > lMod
TaskRefVelTargetGenerator * taskRefVelTargetGen
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
iCub::iKin::iKinChain * chainState
bool getInfo(yarp::os::Bottle &info)