29 #include <yarp/math/Math.h>
33 #define CARTCTRL_SERVER_VER "2.0"
34 #define CARTCTRL_DEFAULT_PER 0.01
35 #define CARTCTRL_DEFAULT_TASKVEL_PERFACTOR 4
36 #define CARTCTRL_DEFAULT_TOL 1e-2
37 #define CARTCTRL_DEFAULT_TRAJTIME 2.0
38 #define CARTCTRL_DEFAULT_POSCTRL "on"
39 #define CARTCTRL_DEFAULT_MULJNTCTRL "on"
40 #define CARTCTRL_CONNECT_SOLVER_PING 1.0
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)
3500 Bottle &partVer=
info.addList();
3501 partVer.addString(kinPartStr+
"_version");
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");
#define IKINCARTCTRL_VOCAB_OPT_Q
#define IKINCARTCTRL_VOCAB_OPT_INFO
#define IKINCARTCTRL_VOCAB_VAL_POSE_XYZ
#define IKINCARTCTRL_VOCAB_OPT_PRIO
#define IKINCARTCTRL_VOCAB_OPT_UNREGISTER
#define IKINCARTCTRL_VOCAB_VAL_TRUE
#define IKINCARTCTRL_VOCAB_CMD_GO
#define IKINCARTCTRL_VOCAB_OPT_REST_POS
#define IKINCARTCTRL_VOCAB_OPT_REFERENCE
#define IKINCARTCTRL_VOCAB_OPT_DES
#define IKINCARTCTRL_VOCAB_OPT_LIM
#define IKINCARTCTRL_VOCAB_OPT_QDOT
#define IKINCARTCTRL_VOCAB_OPT_LIST
#define IKINCARTCTRL_VOCAB_OPT_X
#define IKINCARTCTRL_VOCAB_REP_ACK
#define IKINCARTCTRL_VOCAB_VAL_FALSE
#define IKINCARTCTRL_VOCAB_CMD_STORE
#define IKINCARTCTRL_VOCAB_OPT_TIME
#define IKINCARTCTRL_VOCAB_CMD_DELETE
#define IKINCARTCTRL_VOCAB_OPT_MODE
#define IKINCARTCTRL_VOCAB_CMD_TASKVEL
#define IKINCARTCTRL_VOCAB_VAL_POSE_FULL
#define IKINCARTCTRL_VOCAB_VAL_MODE_TRACK
#define IKINCARTCTRL_VOCAB_OPT_DOF
#define IKINCARTCTRL_VOCAB_OPT_TOL
#define IKINCARTCTRL_VOCAB_VAL_MODE_SINGLE
#define IKINCARTCTRL_VOCAB_CMD_ASK
#define IKINCARTCTRL_VOCAB_CMD_SET
#define IKINCARTCTRL_VOCAB_OPT_REST_WEIGHTS
#define IKINCARTCTRL_VOCAB_CMD_RESTORE
#define IKINCARTCTRL_VOCAB_OPT_ISSOLVERON
#define IKINCARTCTRL_VOCAB_OPT_TIP_FRAME
#define IKINCARTCTRL_VOCAB_CMD_EVENT
#define IKINCARTCTRL_VOCAB_OPT_XDOT
#define IKINCARTCTRL_VOCAB_OPT_TWEAK
#define IKINCARTCTRL_VOCAB_OPT_POSE
#define IKINCARTCTRL_VOCAB_CMD_STOP
#define IKINCARTCTRL_VOCAB_OPT_REGISTER
#define IKINCARTCTRL_VOCAB_VAL_EVENT_ONGOING
#define IKINCARTCTRL_VOCAB_REP_NACK
#define IKINCARTCTRL_VOCAB_OPT_MOTIONDONE
#define CARTCTRL_DEFAULT_TRAJTIME
#define CARTCTRL_DEFAULT_POSCTRL
#define CARTCTRL_CONNECT_SOLVER_PING
#define CARTCTRL_DEFAULT_TASKVEL_PERFACTOR
#define CARTCTRL_SERVER_VER
#define CARTCTRL_DEFAULT_PER
#define CARTCTRL_DEFAULT_MULJNTCTRL
#define CARTCTRL_DEFAULT_TOL
void onRead(yarp::os::Bottle &command)
CartesianCtrlCommandPort(ServerCartesianController *server)
CartesianCtrlRpcProcessor(ServerCartesianController *server)
bool read(yarp::os::ConnectionReader &connection)
servercartesiancontroller : implements the server part of the Cartesian Interface.
bool tweakSet(const yarp::os::Bottle &options)
yarp::os::BufferedPort< yarp::os::Bottle > portEvent
bool setTrackingMode(const bool f)
iCub::iKin::MultiRefMinJerkCtrl * ctrl
iCub::iKin::iKinLimb * limbState
double txTokenLatchedStopControl
bool deleteContexts(yarp::os::Bottle *contextIdList)
yarp::os::Property plantModelProperties
std::deque< yarp::dev::IVelocityControl * > lVel
yarp::os::Bottle sendCtrlCmdSingleJointPosition()
yarp::os::Bottle listMotionOngoingEvents()
virtual ~ServerCartesianController()
yarp::os::BufferedPort< yarp::sig::Vector > portState
void notifyEvent(const std::string &event, const double checkPoint=-1.0)
friend class CartesianCtrlRpcProcessor
std::deque< yarp::dev::IPositionControl * > lStp
yarp::os::Bottle sendCtrlCmdMultipleJointsVelocity()
bool askForPosition(const yarp::sig::Vector &xd, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
std::map< int, Context > contextMap
bool getDOF(yarp::sig::Vector &curDof)
yarp::os::Bottle sendCtrlCmdMultipleJointsPosition()
yarp::os::RpcClient portSlvRpc
std::deque< DriverDescriptor > lDsc
bool getRestWeights(yarp::sig::Vector &curRestWeights)
bool setTrajTimeHelper(const double t)
yarp::os::Stamp debugInfo
double txTokenLatchedGoToRpc
yarp::os::BufferedPort< yarp::os::Bottle > portDebugInfo
yarp::sig::Vector xdot_set
CartesianCtrlRpcProcessor * rpcProcessor
bool getTipFrame(yarp::sig::Vector &x, yarp::sig::Vector &o)
friend class CartesianCtrlCommandPort
bool getTrajTime(double *t)
bool goToPosition(const yarp::sig::Vector &xd, const double t=0.0)
bool attachAll(const yarp::dev::PolyDriverList &p)
void motionOngoingEventsHandling()
std::deque< yarp::dev::IEncodersTimed * > lEnt
iCub::iKin::iKinChain * chainState
bool setLimits(const int axis, const double min, const double max)
double getFeedback(yarp::sig::Vector &_fb)
bool multipleJointsControlEnabled
bool checkMotionDone(bool *f)
bool open(yarp::os::Searchable &config)
bool setTrackingModeHelper(const bool f)
yarp::os::Bottle sendCtrlCmdSingleJointVelocity()
bool getReferenceMode(bool *f)
bool getPosePriority(std::string &p)
bool getInTargetTol(double *tol)
bool setRestWeights(const yarp::sig::Vector &newRestWeights, yarp::sig::Vector &curRestWeights)
yarp::os::Bottle(ServerCartesianController::* sendCtrlCmd)()
std::multiset< double > motionOngoingEventsCurrent
bool attachTipFrame(const yarp::sig::Vector &x, const yarp::sig::Vector &o)
yarp::os::BufferedPort< yarp::os::Bottle > portSlvOut
bool setTrajTime(const double t)
bool getTask2ndOptions(yarp::os::Value &v)
std::deque< yarp::dev::IControlMode * > lMod
bool registerEvent(yarp::dev::CartesianEvent &event)
bool getLimits(const int axis, double *min, double *max)
bool unregisterMotionOngoingEvent(const double checkPoint)
yarp::os::RpcServer portRpc
std::deque< yarp::dev::IControlLimits * > lLim
void setJointsCtrlMode(const std::vector< int > &jointsToSet)
bool tweakGet(yarp::os::Bottle &options)
bool waitMotionDone(const double period=0.1, const double timeout=0.0)
bool setTask2ndOptions(const yarp::os::Value &v)
bool setInTargetTolHelper(const double tol)
bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
bool getSolverConvergenceOptions(yarp::os::Bottle &options)
bool setPosePriority(const std::string &p)
bool askForPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
bool getTaskVelocities(yarp::sig::Vector &xdot, yarp::sig::Vector &odot)
int taskRefVelPeriodFactor
bool goToPoseSync(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0)
bool getInfo(yarp::os::Bottle &info)
bool setSolverConvergenceOptions(const yarp::os::Bottle &options)
iCub::iKin::iKinLimb * limbPlan
TaskRefVelTargetGenerator * taskRefVelTargetGen
bool goToPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0)
std::map< std::string, yarp::dev::CartesianEvent * > eventsMap
bool unregisterEvent(yarp::dev::CartesianEvent &event)
bool goTo(unsigned int _ctrlPose, const yarp::sig::Vector &xd, const double t, const bool latchToken=false)
bool areJointsHealthyAndSet(std::vector< int > &jointsToSet)
void motionOngoingEventsFlush()
yarp::os::BufferedPort< yarp::os::Bottle > portSlvIn
bool setTaskVelocities(const yarp::sig::Vector &xdot, const yarp::sig::Vector &odot)
std::multiset< double > motionOngoingEvents
bool goToPositionSync(const yarp::sig::Vector &xd, const double t=0.0)
CartesianCtrlCommandPort * portCmd
bool setRestPos(const yarp::sig::Vector &newRestPos, yarp::sig::Vector &curRestPos)
iCub::iKin::iKinChain * chainPlan
bool storeContext(int *id)
ServerCartesianController()
bool restoreContext(const int id)
bool getTrackingMode(bool *f)
std::deque< yarp::dev::IPositionDirect * > lPos
bool setReferenceMode(const bool f)
std::deque< yarp::dev::IEncoders * > lEnc
yarp::os::Stamp eventInfo
void stopLimb(const bool execStopPosition=true)
yarp::dev::PolyDriverList drivers
std::condition_variable cv_syncEvent
bool setInTargetTol(const double tol)
bool deleteContext(const int id)
bool setDOF(const yarp::sig::Vector &newDof, yarp::sig::Vector &curDof)
bool getRestPos(yarp::sig::Vector &curRestPos)
bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
bool getDesired(yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
bool getJointsVelocities(yarp::sig::Vector &qdot)
SmithPredictor smithPredictor
bool registerMotionOngoingEvent(const double checkPoint)
std::deque< yarp::dev::IPidControl * > lPid
void configure(const yarp::os::Property &options, iCub::iKin::iKinChain &chain)
yarp::sig::Vector computeCmd(const yarp::sig::Vector &u)
void restart(const yarp::sig::Vector &y0)
void reset(const yarp::sig::Vector &x0)
yarp::sig::Vector integrate(const yarp::sig::Vector &vel)
virtual ~TaskRefVelTargetGenerator()
TaskRefVelTargetGenerator(const double Ts, const yarp::sig::Vector &x0)
A class for defining a saturated integrator based on Tustin formula: .
static yarp::os::Bottle * getEndEffectorPoseOption(const yarp::os::Bottle &b)
Retrieves the end-effector pose data.
static void addVectorOption(yarp::os::Bottle &b, const int vcb, const yarp::sig::Vector &v)
static yarp::os::Bottle * getJointsOption(const yarp::os::Bottle &b)
Retrieves the joints configuration data.
static void addModeOption(yarp::os::Bottle &b, const bool tracking)
Appends to a bottle all data needed to change the tracking mode.
static void addTargetOption(yarp::os::Bottle &b, const yarp::sig::Vector &xd)
Appends to a bottle all data needed to command a target.
static bool getDesiredOption(const yarp::os::Bottle &reply, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
static bool getTokenOption(const yarp::os::Bottle &b, double *token)
Retrieves the token from the bottle.
static void addTokenOption(yarp::os::Bottle &b, const double token)
Appends to a bottle a token to be exchanged with the solver.
static void addPoseOption(yarp::os::Bottle &b, const unsigned int pose)
Appends to a bottle all data needed to change the pose mode.
A class derived from iKinCtrl implementing the multi-referential approach (pdf).
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...
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd, yarp::sig::Vector *xdot_set, const unsigned int verbose)
void add_compensation(const yarp::sig::Vector &comp)
Adds to the controller input a further compensation term.
double set_execTime(const double _execTime, const bool warn=false)
Sets the task execution time in seconds.
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
virtual void set_q(const yarp::sig::Vector &q0)
Sets the joint angles values.
double get_gamma() const
Returns the parameter gamma which is used to blend the contribute of the task controller versus the c...
void set_gamma(double _gamma)
Sets the parameter gamma which is used to blend the contribute of the task controller versus the cont...
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
A class for defining the iCub Arm.
A class for defining the iCub Leg.
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...
yarp::sig::Matrix getHN() const
Returns HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
bool setHN(const yarp::sig::Matrix &_HN)
Sets HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
bool releaseLink(const unsigned int i)
Releases the ith Link.
bool setBlockingValue(const unsigned int i, double Ang)
Changes the value of the ith blocked Link.
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
yarp::sig::Vector getAng()
Returns the current free joint angles values.
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
unsigned int getN() const
Returns the number of Links belonging to the Chain.
unsigned int getDOF() const
Returns the current number of Chain's DOF.
bool blockLink(const unsigned int i, double Ang)
Blocks the ith Link at the a certain value of its joint angle.
virtual bool isInTarget()
Checks if the End-Effector is in target.
virtual yarp::sig::Matrix get_J() const
Returns the actual Jacobian used in computation.
virtual yarp::sig::Vector get_q() const
Returns the actual joint angles values.
virtual void setInTargetTol(double tol_x)
Sets tolerance for in-target check (5e-3 by default).
unsigned int get_dim() const
Returns the number of Chain DOF.
void set_ctrlPose(unsigned int _ctrlPose)
Sets the state of Pose control settings.
virtual yarp::sig::Vector get_x() const
Returns the actual cartesian position of the End-Effector.
virtual double getInTargetTol() const
Returns tolerance for in-target check.
A class for defining the versions of the iCub limbs.
std::string get_version() const
Return the version string.
A class for defining generic Limb.
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
std::string getType() const
Returns the Limb type as a string.
bool toLinksProperties(yarp::os::Property &options)
Provides the links attributes listed in a property object.
bool isValid() const
Checks if the limb has been properly configured.
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
#define IKINCTRL_POSE_FULL
#define IKINCTRL_POSE_XYZ
#define IKINCTRL_POSE_ANG
#define IKINSLV_VOCAB_OPT_DOF
#define IKINSLV_VOCAB_OPT_PRIO
#define IKINSLV_VOCAB_VAL_PRIO_XYZ
#define IKINSLV_VOCAB_VAL_MODE_TRACK
#define IKINSLV_VOCAB_VAL_MODE_SINGLE
#define IKINSLV_VOCAB_OPT_Q
#define IKINSLV_VOCAB_OPT_CONVERGENCE
#define IKINSLV_VOCAB_OPT_MODE
#define IKINSLV_VOCAB_CMD_SET
#define IKINSLV_VOCAB_CMD_ASK
#define IKINSLV_VOCAB_REP_ACK
#define IKINSLV_VOCAB_OPT_REST_POS
#define IKINSLV_VOCAB_OPT_TIP_FRAME
#define IKINSLV_VOCAB_CMD_GET
#define IKINSLV_VOCAB_OPT_LIM
#define IKINSLV_VOCAB_OPT_TASK2
#define IKINSLV_VOCAB_OPT_REST_WEIGHTS
#define IKINSLV_VOCAB_REP_NACK
#define IKINSLV_VOCAB_OPT_X
#define IKINSLV_VOCAB_VAL_PRIO_ANG
#define IKINSLV_VOCAB_OPT_XD
static string pathToCustomKinFile
yarp::sig::Vector minAbsVels
yarp::os::Bottle solverConvergence
yarp::sig::Vector restPos
yarp::sig::Vector restWeights