iCub-main
iKinSlv.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
4  * All rights reserved.
5  *
6  * This software may be modified and distributed under the terms
7  * of the BSD-3-Clause license. See the accompanying LICENSE file for
8  * details.
9 */
10 
11 #include <cstdlib>
12 #include <cmath>
13 #include <algorithm>
14 
15 #include <yarp/os/LogStream.h>
16 #include <yarp/os/Network.h>
17 #include <yarp/os/Time.h>
18 
19 #include <iCub/iKin/iKinVocabs.h>
20 #include <iCub/iKin/iKinSlv.h>
21 
22 #define CARTSLV_DEFAULT_PER 20 // [ms]
23 #define CARTSLV_DEFAULT_TOL 1e-4
24 #define CARTSLV_DEFAULT_CONSTR_TOL 1e-6
25 #define CARTSLV_DEFAULT_MAXITER 200
26 #define CARTSLV_WEIGHT_2ND_TASK 0.01
27 #define CARTSLV_WEIGHT_3RD_TASK 0.01
28 #define CARTSLV_UNCTRLEDJNTS_THRES 1.0 // [deg]
29 
30 using namespace std;
31 using namespace yarp::os;
32 using namespace yarp::sig;
33 using namespace yarp::dev;
34 using namespace yarp::math;
35 using namespace iCub::ctrl;
36 using namespace iCub::iKin;
37 
38 
39 /************************************************************************/
40 bool RpcProcessor::read(ConnectionReader &connection)
41 {
42  if (!slv->isClosed())
43  {
44  Bottle cmd, reply;
45  if (!cmd.read(connection))
46  return false;
47 
48  slv->respond(cmd,reply);
49  if (ConnectionWriter *writer=connection.getWriter())
50  reply.write(*writer);
51 
52  return true;
53  }
54  else
55  return false;
56 }
57 
58 
59 /************************************************************************/
60 InputPort::InputPort(CartesianSolver *_slv)
61 {
62  slv=_slv;
63 
64  maxLen=7;
65  xd.resize(maxLen,0.0);
66  dof.resize(1,0.0);
67 
68  pose=IKINCTRL_POSE_FULL;
69  contMode=false;
70  isNew=false;
71  token=0.0;
72  pToken=NULL;
73 }
74 
75 
76 /************************************************************************/
77 void InputPort::set_dof(const Vector &_dof)
78 {
79  mutex.lock();
80  dof=_dof;
81  mutex.unlock();
82 }
83 
84 
85 /************************************************************************/
86 Vector InputPort::get_dof()
87 {
88  mutex.lock();
89  Vector _dof=dof;
90  mutex.unlock();
91  return _dof;
92 }
93 
94 
95 /************************************************************************/
96 Vector InputPort::get_xd()
97 {
98  mutex.lock();
99  Vector _xd=xd;
100  mutex.unlock();
101  return _xd;
102 }
103 
104 
105 /************************************************************************/
106 void InputPort::reset_xd(const Vector &_xd)
107 {
108  mutex.lock();
109  xd=_xd;
110  mutex.unlock();
111  isNew=false;
112 }
113 
114 
115 /************************************************************************/
116 bool InputPort::isNewDataEvent()
117 {
118  if (isNew)
119  {
120  isNew=false;
121  return true;
122  }
123  else
124  return false;
125 }
126 
127 
128 /************************************************************************/
129 bool InputPort::handleTarget(Bottle *b)
130 {
131  if (b!=NULL)
132  {
133  mutex.lock();
134  size_t len=std::min(b->size(),maxLen);
135  for (size_t i=0; i<len; i++)
136  xd[i]=b->get(i).asDouble();
137  mutex.unlock();
138 
139  return isNew=true;
140  }
141  else
142  return false;
143 }
144 
145 
146 /************************************************************************/
147 bool InputPort::handleDOF(Bottle *b)
148 {
149  if (b!=NULL)
150  {
151  slv->lock();
152 
153  mutex.lock();
154  dof.resize(b->size());
155  for (int i=0; i<b->size(); i++)
156  dof[i]=b->get(i).asInt();
157  mutex.unlock();
158 
159  slv->unlock();
160 
161  return true;
162  }
163  else
164  return false;
165 }
166 
167 
168 /************************************************************************/
169 bool InputPort::handlePose(const int newPose)
170 {
171  if (newPose==IKINSLV_VOCAB_VAL_POSE_FULL)
172  {
173  if (pose==IKINCTRL_POSE_XYZ)
174  isNew=true;
175 
176  pose=IKINCTRL_POSE_FULL;
177  return true;
178  }
179  else if (newPose==IKINSLV_VOCAB_VAL_POSE_XYZ)
180  {
181  if (pose==IKINCTRL_POSE_FULL)
182  isNew=true;
183 
184  pose=IKINCTRL_POSE_XYZ;
185  return true;
186  }
187  else
188  return false;
189 }
190 
191 
192 /************************************************************************/
193 bool InputPort::handleMode(const int newMode)
194 {
195  if (newMode==IKINSLV_VOCAB_VAL_MODE_TRACK)
196  {
197  slv->lock();
198  contMode=true;
199  slv->unlock();
200  return true;
201  }
202  else if (newMode==IKINSLV_VOCAB_VAL_MODE_SINGLE)
203  {
204  slv->lock();
205  contMode=false;
206  slv->unlock();
207  return true;
208  }
209  else
210  return false;
211 }
212 
213 
214 /************************************************************************/
215 void InputPort::onRead(Bottle &b)
216 {
217  bool xdOptIn =b.check(Vocab::decode(IKINSLV_VOCAB_OPT_XD));
218  bool dofOptIn =b.check(Vocab::decode(IKINSLV_VOCAB_OPT_DOF));
219  bool poseOptIn=b.check(Vocab::decode(IKINSLV_VOCAB_OPT_POSE));
220  bool modeOptIn=b.check(Vocab::decode(IKINSLV_VOCAB_OPT_MODE));
221 
222  if (xdOptIn)
223  {
224  if (CartesianHelper::getTokenOption(b,&token))
225  pToken=&token;
226  else
227  pToken=NULL;
228  }
229 
230  if (modeOptIn)
231  if (!handleMode(b.find(Vocab::decode(IKINSLV_VOCAB_OPT_MODE)).asVocab()))
232  yWarning("%s: got incomplete %s command",slv->slvName.c_str(),
233  Vocab::decode(IKINSLV_VOCAB_OPT_MODE).c_str());
234 
235  if (dofOptIn)
236  if (!handleDOF(b.find(Vocab::decode(IKINSLV_VOCAB_OPT_DOF)).asList()))
237  yWarning("%s: expected %s data",slv->slvName.c_str(),
238  Vocab::decode(IKINSLV_VOCAB_OPT_DOF).c_str());
239 
240  if (poseOptIn)
241  if (!handlePose(b.find(Vocab::decode(IKINSLV_VOCAB_OPT_POSE)).asVocab()))
242  yWarning("%s: got incomplete %s command",slv->slvName.c_str(),
243  Vocab::decode(IKINSLV_VOCAB_OPT_POSE).c_str());
244 
245  if (slv->handleJointsRestPosition(b.find(Vocab::decode(IKINSLV_VOCAB_OPT_REST_POS)).asList()) ||
246  slv->handleJointsRestWeights(b.find(Vocab::decode(IKINSLV_VOCAB_OPT_REST_WEIGHTS)).asList()))
247  {
248  slv->lock();
249  slv->prepareJointsRestTask();
250  slv->unlock();
251  }
252 
253  // shall be the last handling
254  if (xdOptIn)
255  {
256  if (!handleTarget(b.find(Vocab::decode(IKINSLV_VOCAB_OPT_XD)).asList()))
257  yWarning("%s: expected %s data",slv->slvName.c_str(),
258  Vocab::decode(IKINSLV_VOCAB_OPT_XD).c_str());
259  }
260  else
261  yWarning("%s: missing %s data; it shall be present",
262  slv->slvName.c_str(),Vocab::decode(IKINSLV_VOCAB_OPT_XD).c_str());
263 }
264 
265 
266 /************************************************************************/
267 void SolverCallback::exec(const Vector &xd, const Vector &q)
268 {
269  slv->send(xd,slv->prt->chn->EndEffPose(),CTRL_RAD2DEG*q,slv->pToken);
270 }
271 
272 
273 /************************************************************************/
274 CartesianSolver::CartesianSolver(const string &_slvName) :
275  PeriodicThread((double)CARTSLV_DEFAULT_PER/1000.0)
276 {
277  // initialization
278  slvName=_slvName;
279  configured=false;
280  closing=false;
281  closed=false;
282  interrupting=false;
283  verbosity=false;
284  timeout_detected=false;
285  maxPartJoints=0;
286  unctrlJointsNum=0;
287  ping_robot_tmo=0.0;
288 
289  prt=NULL;
290  slv=NULL;
291  clb=NULL;
292  inPort=NULL;
293  outPort=NULL;
294 
295  // open rpc port
296  rpcPort=new Port;
297  cmdProcessor=new RpcProcessor(this);
298  rpcPort->setReader(*cmdProcessor);
299  rpcPort->open("/"+slvName+"/rpc");
300 
301  // token
302  token=0.0;
303  pToken=NULL;
304 }
305 
306 
307 /************************************************************************/
308 PolyDriver *CartesianSolver::waitPart(const Property &partOpt)
309 {
310  string partName=partOpt.find("part").asString();
311  PolyDriver *pDrv=NULL;
312 
313  double t0=Time::now();
314  while (Time::now()-t0<ping_robot_tmo)
315  {
316  delete pDrv;
317 
318  pDrv=new PolyDriver(const_cast<Property&>(partOpt));
319  bool ok=pDrv->isValid();
320 
321  if (ok)
322  {
323  yInfo("%s: Checking if %s part is active ... ok",
324  slvName.c_str(),partName.c_str());
325  return pDrv;
326  }
327  else
328  {
329  double dt=ping_robot_tmo-(Time::now()-t0);
330  yInfo("%s: Checking if %s part is active ... not yet: still %.1f [s] to timeout expiry",
331  slvName.c_str(),partName.c_str(),dt>0.0?dt:0.0);
332 
333  double t1=Time::now();
334  while (Time::now()-t1<1.0)
335  Time::delay(0.1);
336  }
337 
338  if (interrupting)
339  break;
340  }
341 
342  return pDrv;
343 }
344 
345 
346 /************************************************************************/
348 {
349  hwLimits.resize(prt->chn->getN(),2);
350  double min, max;
351  int cnt=0;
352 
353  yInfo("%s: aligning joints bounds ...",slvName.c_str());
354  for (int i=0; i<prt->num; i++)
355  {
356  yInfo("part #%d: %s",i,prt->prp[i].find("part").asString().c_str());
357  for (int j=0; j<jnt[i]; j++)
358  {
359  if (!lim[i]->getLimits(rmp[i][j],&min,&max))
360  {
361  yError("joint #%d: failed getting limits!",cnt);
362  return false;
363  }
364 
365  yInfo("joint #%d: [%g, %g] deg",cnt,min,max);
366  (*prt->chn)[cnt].setMin(CTRL_DEG2RAD*min);
367  (*prt->chn)[cnt].setMax(CTRL_DEG2RAD*max);
368 
369  hwLimits(cnt,0)=min;
370  hwLimits(cnt,1)=max;
371 
372  cnt++;
373  }
374  }
375 
377  return true;
378 }
379 
380 
381 /************************************************************************/
382 bool CartesianSolver::setLimits(int axis, double min, double max)
383 {
384  if (axis>=(int)prt->chn->getN())
385  yError("#%d>#%d: requested out of range axis!",
386  axis,prt->chn->getN());
387  else if (min>max)
388  yError("joint #%d: requested wrong bounds [%g,%g]!",
389  axis,min,max);
390  else if ((min>=hwLimits(axis,0)) && (max<=hwLimits(axis,1)))
391  {
392  (*prt->chn)[axis].setMin(CTRL_DEG2RAD*min);
393  (*prt->chn)[axis].setMax(CTRL_DEG2RAD*max);
394 
395  swLimits(axis,0)=min;
396  swLimits(axis,1)=max;
397 
398  return true;
399  }
400  else
401  yWarning("joint #%d: requested out of range limit!",axis);
402 
403  return false;
404 }
405 
406 
407 /************************************************************************/
409 {
411 }
412 
413 
414 /************************************************************************/
416 {
417  if (unctrlJointsNum>0)
418  {
419  joints.resize(unctrlJointsNum);
420  int j=0;
421 
422  for (unsigned int i=0; i<prt->chn->getN(); i++)
423  if ((*prt->chn)[i].isBlocked())
424  joints[j++]=prt->chn->getAng(i);
425  }
426  else
427  joints.resize(1);
428 }
429 
430 
431 /************************************************************************/
432 void CartesianSolver::getFeedback(const bool wait)
433 {
434  Vector fbTmp(maxPartJoints);
435  int chainCnt=0;
436 
437  for (int i=0; i<prt->num; i++)
438  {
439  bool ok=enc[i]->getEncoders(fbTmp.data());
440  while (wait && !closing && !ok)
441  {
442  ok=enc[i]->getEncoders(fbTmp.data());
443  Time::yield();
444  }
445 
446  if (ok)
447  {
448  for (int j=0; j<jnt[i]; j++)
449  {
450  double tmp=CTRL_DEG2RAD*fbTmp[rmp[i][j]];
451 
452  if ((*prt->chn)[chainCnt].isBlocked())
453  prt->chn->setBlockingValue(chainCnt,tmp);
454  else
455  prt->chn->setAng(chainCnt,tmp);
456 
457  chainCnt++;
458  }
459  }
460  else
461  {
462  yWarning("%s: timeout detected on part %s!",
463  slvName.c_str(),prt->prp[i].find("part").asString().c_str());
464 
465  timeout_detected=true;
466  chainCnt+=jnt[i];
467  }
468  }
469 }
470 
471 
472 /************************************************************************/
474 {
475  lock();
476  getFeedback(true); // wait until all joints are acquired
477  unlock();
478 
480  inPort->reset_xd(prt->chn->EndEffPose());
481 }
482 
483 
484 /************************************************************************/
486 {
487  mutex.lock();
488 }
489 
490 
491 /************************************************************************/
493 {
494  mutex.unlock();
495 }
496 
497 
498 /************************************************************************/
500 {
501  dofEvent.reset();
502  dofEvent.wait();
503 }
504 
505 
506 /************************************************************************/
508 {
509  dofEvent.signal();
510 }
511 
512 
513 /************************************************************************/
514 void CartesianSolver::fillDOFInfo(Bottle &reply)
515 {
516  for (unsigned int i=0; i<prt->chn->getN(); i++)
517  reply.addInt(int(!(*prt->chn)[i].isBlocked()));
518 }
519 
520 
521 /************************************************************************/
522 void CartesianSolver::respond(const Bottle &command, Bottle &reply)
523 {
524  if (command.size())
525  {
526  int vcb=command.get(0).asVocab();
527 
528  if (!configured && vcb!=IKINSLV_VOCAB_CMD_CFG)
529  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
530  else switch (vcb)
531  {
532  //-----------------
534  {
535  if (command.size()>1)
536  {
537  switch (command.get(1).asVocab())
538  {
539  //-----------------
541  {
542  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
543 
545  reply.addVocab(IKINSLV_VOCAB_VAL_POSE_FULL);
546  else
547  reply.addVocab(IKINSLV_VOCAB_VAL_POSE_XYZ);
548 
549  break;
550  }
551 
552  //-----------------
554  {
555  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
556 
557  string priority=slv->get_posePriority();
558  if (priority=="position")
559  reply.addVocab(IKINSLV_VOCAB_VAL_PRIO_XYZ);
560  else
561  reply.addVocab(IKINSLV_VOCAB_VAL_PRIO_ANG);
562 
563  break;
564  }
565 
566  //-----------------
568  {
569  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
570 
571  if (inPort->get_contMode())
572  reply.addVocab(IKINSLV_VOCAB_VAL_MODE_TRACK);
573  else
574  reply.addVocab(IKINSLV_VOCAB_VAL_MODE_SINGLE);
575 
576  break;
577  }
578 
579  //-----------------
581  {
582  if (command.size()>2)
583  {
584  int axis=command.get(2).asInt();
585 
586  if (axis<(int)prt->chn->getN())
587  {
588  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
589  reply.addDouble(swLimits(axis,0));
590  reply.addDouble(swLimits(axis,1));
591  }
592  else
593  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
594  }
595  else
596  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
597 
598  break;
599  }
600 
601  //-----------------
603  {
604  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
605 
606  if (verbosity)
607  reply.addVocab(IKINSLV_VOCAB_VAL_ON);
608  else
609  reply.addVocab(IKINSLV_VOCAB_VAL_OFF);
610 
611  break;
612  }
613 
614  //-----------------
616  {
617  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
618  Bottle &dofPart=reply.addList();
619  fillDOFInfo(dofPart);
620  break;
621  }
622 
623  //-----------------
625  {
626  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
627  handleJointsRestPosition(NULL,&reply);
628  break;
629  }
630 
631  //-----------------
633  {
634  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
635  handleJointsRestWeights(NULL,&reply);
636  break;
637  }
638 
639  //-----------------
641  {
642  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
643  Bottle &payLoad=reply.addList();
644  payLoad.addInt(slv->get2ndTaskChain().getN());
645 
646  Bottle &posPart=payLoad.addList();
647  for (size_t i=0; i<xd_2ndTask.length(); i++)
648  posPart.addDouble(xd_2ndTask[i]);
649 
650  Bottle &weightsPart=payLoad.addList();
651  for (size_t i=0; i<w_2ndTask.length(); i++)
652  weightsPart.addDouble(w_2ndTask[i]);
653 
654  break;
655  }
656 
657  //-----------------
659  {
660  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
661  Bottle &payLoad=reply.addList();
662 
663  Bottle &tol=payLoad.addList();
664  tol.addString("tol");
665  tol.addDouble(slv->getTol());
666 
667  Bottle &constr_tol=payLoad.addList();
668  constr_tol.addString("constr_tol");
669  constr_tol.addDouble(slv->getConstrTol());
670 
671  Bottle &maxIter=payLoad.addList();
672  maxIter.addString("max_iter");
673  maxIter.addInt(slv->getMaxIter());
674 
675  break;
676  }
677 
678  //-----------------
679  default:
680  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
681  }
682  }
683  else
684  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
685 
686  break;
687  }
688 
689  //-----------------
691  {
692  if (command.size()>2)
693  {
694  switch (command.get(1).asVocab())
695  {
696  //-----------------
698  {
699  if (inPort->handlePose(command.get(2).asVocab()))
700  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
701  else
702  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
703 
704  break;
705  }
706 
707  //-----------------
709  {
710  int type=command.get(2).asVocab();
711  if (type==IKINSLV_VOCAB_VAL_PRIO_XYZ)
712  {
713  slv->set_posePriority("position");
714  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
715  }
716  else if (type==IKINSLV_VOCAB_VAL_PRIO_ANG)
717  {
718  slv->set_posePriority("orientation");
719  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
720  }
721  else
722  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
723 
724  break;
725  }
726 
727  //-----------------
729  {
730  if (inPort->handleMode(command.get(2).asVocab()))
731  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
732  else
733  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
734 
735  break;
736  }
737 
738  //-----------------
740  {
741  if (command.size()>4)
742  {
743  int axis=command.get(2).asInt();
744  double min=command.get(3).asDouble();
745  double max=command.get(4).asDouble();
746 
747  if (setLimits(axis,min,max))
748  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
749  else
750  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
751  }
752  else
753  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
754 
755  break;
756  }
757 
758  //-----------------
760  {
761  int sw=command.get(2).asVocab();
762 
763  if (sw==IKINSLV_VOCAB_VAL_ON)
764  {
765  verbosity=true;
766  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
767  }
768  else if (sw==IKINSLV_VOCAB_VAL_OFF)
769  {
770  verbosity=false;
771  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
772  }
773 
774  break;
775  }
776 
777  //-----------------
779  {
780  if (inPort->handleDOF(command.get(2).asList()))
781  {
782  waitDOFHandling(); // sleep till dof handling is done
783 
784  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
785  Bottle &dofPart=reply.addList();
786  fillDOFInfo(dofPart);
787  }
788  else
789  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
790 
791  break;
792  }
793 
794  //-----------------
796  {
797  Bottle restPart;
798 
799  if (handleJointsRestPosition(command.get(2).asList(),&restPart))
800  {
801  lock();
803  unlock();
804 
805  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
806  reply.append(restPart);
807  }
808  else
809  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
810 
811  break;
812  }
813 
814  //-----------------
816  {
817  Bottle restPart;
818 
819  if (handleJointsRestWeights(command.get(2).asList(),&restPart))
820  {
821  lock();
823  unlock();
824 
825  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
826  reply.append(restPart);
827  }
828  else
829  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
830 
831  break;
832  }
833 
834  //-----------------
836  {
837  if (Bottle *tipPart=command.get(2).asList())
838  {
839  if (tipPart->size()>=7)
840  {
841  Vector x(3);
842  for (size_t i=0; i<x.length(); i++)
843  x[i]=tipPart->get(i).asDouble();
844 
845  Vector o(4);
846  for (size_t i=0; i<o.length(); i++)
847  o[i]=tipPart->get(i+x.length()).asDouble();
848 
849  Matrix HN=axis2dcm(o);
850  HN.setSubcol(x,0,3);
851 
852  lock();
853  prt->chn->setHN(HN);
854  unlock();
855 
856  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
857  break;
858  }
859  }
860 
861  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
862  break;
863  }
864 
865  //-----------------
867  {
868  if (Bottle *payLoad=command.get(2).asList())
869  {
870  if (payLoad->size()>=3)
871  {
872  int n=payLoad->get(0).asInt();
873  Bottle *posPart=payLoad->get(1).asList();
874  Bottle *weightsPart=payLoad->get(2).asList();
875 
876  if ((posPart!=NULL) && (weightsPart!=NULL))
877  {
878  if ((posPart->size()>=3) && (weightsPart->size()>=3))
879  {
880  for (size_t i=0; i<xd_2ndTask.length(); i++)
881  xd_2ndTask[i]=posPart->get(i).asDouble();
882 
883  for (size_t i=0; i<w_2ndTask.length(); i++)
884  w_2ndTask[i]=weightsPart->get(i).asDouble();
885 
886  if (n>=0)
888  else
890 
891  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
892  break;
893  }
894  }
895  }
896  }
897 
898  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
899  break;
900  }
901 
902  //-----------------
904  {
905  if (Bottle *payLoad=command.get(2).asList())
906  {
907  int cnt=0;
908  if (payLoad->check("tol"))
909  {
910  slv->setTol(payLoad->find("tol").asDouble());
911  cnt++;
912  }
913 
914  if (payLoad->check("constr_tol"))
915  {
916  slv->setConstrTol(payLoad->find("constr_tol").asDouble());
917  cnt++;
918  }
919 
920  if (payLoad->check("max_iter"))
921  {
922  slv->setMaxIter(payLoad->find("max_iter").asInt());
923  cnt++;
924  }
925 
926  reply.addVocab(cnt>0?IKINSLV_VOCAB_REP_ACK:IKINSLV_VOCAB_REP_NACK);
927  }
928  else
929  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
930 
931  break;
932  }
933 
934  //-----------------
935  default:
936  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
937  }
938  }
939  else
940  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
941 
942  break;
943  }
944 
945  //-----------------
947  {
948  Bottle *b_xd=getTargetOption(command);
949  Bottle *b_q=getJointsOption(command);
950 
951  // some integrity checks
952  if (b_xd==NULL)
953  {
954  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
955  break;
956  }
957  else if (b_xd->size()<3) // at least the positional part must be given
958  {
959  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
960  break;
961  }
962 
963  lock();
964 
965  // get the target
966  Vector xd(b_xd->size());
967  for (size_t i=0; i<xd.length(); i++)
968  xd[i]=b_xd->get(i).asDouble();
969 
970  // accounts for the starting DOF
971  // if different from the actual one
972  if (b_q!=NULL)
973  {
974  size_t len=std::min((size_t)b_q->size(),(size_t)prt->chn->getDOF());
975  for (size_t i=0; i<len; i++)
976  (*prt->chn)(i).setAng(CTRL_DEG2RAD*b_q->get(i).asDouble());
977  }
978  else
979  getFeedback(); // otherwise get the current configuration
980 
981  // account for the pose
982  if (command.check(Vocab::decode(IKINSLV_VOCAB_OPT_POSE)))
983  {
984  int pose=command.find(Vocab::decode(IKINSLV_VOCAB_OPT_POSE)).asVocab();
985 
986  if (pose==IKINSLV_VOCAB_VAL_POSE_FULL)
988  else if (pose==IKINSLV_VOCAB_VAL_POSE_XYZ)
990  }
991 
992  // set things for the 3rd task
993  for (unsigned int i=0; i<prt->chn->getDOF(); i++)
994  if (idx_3rdTask[i]!=0.0)
995  qd_3rdTask[i]=(*prt->chn)(i).getAng();
996 
997  // call the solver to converge
998  double t0=Time::now();
999  Vector q=solve(xd);
1000  double t1=Time::now();
1001 
1002  Vector x=prt->chn->EndEffPose(q);
1003 
1004  // change to degrees
1005  q*=CTRL_RAD2DEG;
1006 
1007  // dump on screen
1008  if (verbosity)
1009  printInfo("ask",xd,x,q,t1-t0);
1010 
1011  // prepare the complete joints configuration
1012  Vector _q(prt->chn->getN());
1013  for (unsigned int i=0; i<prt->chn->getN(); i++)
1014  _q[i]=CTRL_RAD2DEG*prt->chn->getAng(i);
1015 
1016  // fill the reply accordingly
1017  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
1020 
1021  unlock();
1022 
1023  break;
1024  }
1025 
1026  //-----------------
1028  {
1029  suspend();
1030  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
1031  break;
1032  }
1033 
1034  //-----------------
1035  case IKINSLV_VOCAB_CMD_RUN:
1036  {
1037  if (!isRunning())
1038  {
1039  initPos();
1040  start();
1041  }
1042  else
1043  resume();
1044 
1045  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
1046  break;
1047  }
1048 
1049  //-----------------
1051  {
1052  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
1053  if (isSuspended())
1054  reply.addString("suspended");
1055  else if (isRunning())
1056  reply.addString("running");
1057  else
1058  reply.addString("not_started");
1059  break;
1060  }
1061 
1062  //-----------------
1063  case IKINSLV_VOCAB_CMD_CFG:
1064  {
1065  Property options(command.toString().c_str());
1066  yInfo("Configuring with options: %s",options.toString().c_str());
1067 
1068  if (open(options))
1069  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
1070  else
1071  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
1072 
1073  break;
1074  }
1075 
1076  //-----------------
1078  {
1079  reply.addVocab(Vocab::encode("many"));
1080  reply.addString("***** commands");
1081  reply.addVocab(IKINSLV_VOCAB_CMD_GET);
1082  reply.addVocab(IKINSLV_VOCAB_CMD_SET);
1083  reply.addVocab(IKINSLV_VOCAB_CMD_ASK);
1084  reply.addVocab(IKINSLV_VOCAB_CMD_SUSP);
1085  reply.addVocab(IKINSLV_VOCAB_CMD_RUN);
1086  reply.addVocab(IKINSLV_VOCAB_CMD_STATUS);
1087  reply.addVocab(IKINSLV_VOCAB_CMD_CFG);
1088  reply.addVocab(IKINSLV_VOCAB_CMD_HELP);
1089  reply.addVocab(IKINSLV_VOCAB_CMD_QUIT);
1090  reply.addString("***** options");
1091  reply.addVocab(IKINSLV_VOCAB_OPT_MODE);
1092  reply.addVocab(IKINSLV_VOCAB_OPT_POSE);
1093  reply.addVocab(IKINSLV_VOCAB_OPT_DOF);
1094  reply.addVocab(IKINSLV_VOCAB_OPT_LIM);
1095  reply.addVocab(IKINSLV_VOCAB_OPT_VERB);
1096  reply.addVocab(IKINSLV_VOCAB_OPT_TOKEN);
1097  reply.addVocab(IKINSLV_VOCAB_OPT_REST_POS);
1098  reply.addVocab(IKINSLV_VOCAB_OPT_REST_WEIGHTS);
1099  reply.addVocab(IKINSLV_VOCAB_OPT_TIP_FRAME);
1100  reply.addVocab(IKINSLV_VOCAB_OPT_TASK2);
1101  reply.addVocab(IKINSLV_VOCAB_OPT_XD);
1102  reply.addVocab(IKINSLV_VOCAB_OPT_X);
1103  reply.addVocab(IKINSLV_VOCAB_OPT_Q);
1104  reply.addString("***** values");
1105  reply.addVocab(IKINSLV_VOCAB_VAL_POSE_FULL);
1106  reply.addVocab(IKINSLV_VOCAB_VAL_POSE_XYZ);
1107  reply.addVocab(IKINSLV_VOCAB_VAL_MODE_TRACK);
1108  reply.addVocab(IKINSLV_VOCAB_VAL_MODE_SINGLE);
1109  reply.addVocab(IKINSLV_VOCAB_VAL_ON);
1110  reply.addVocab(IKINSLV_VOCAB_VAL_OFF);
1111  break;
1112  }
1113 
1114  //-----------------
1116  {
1117  reply.addVocab(IKINSLV_VOCAB_REP_ACK);
1118  close();
1119  break;
1120  }
1121 
1122  //-----------------
1123  default:
1124  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
1125  }
1126  }
1127  else
1128  reply.addVocab(IKINSLV_VOCAB_REP_NACK);
1129 }
1130 
1131 
1132 /************************************************************************/
1133 void CartesianSolver::send(const Vector &xd, const Vector &x, const Vector &q,
1134  double *tok)
1135 {
1136  Bottle &b=outPort->prepare();
1137  b.clear();
1138 
1142 
1143  if (tok!=NULL)
1144  addTokenOption(b,*tok);
1145 
1146  outPort->writeStrict();
1147 }
1148 
1149 
1150 /************************************************************************/
1151 void CartesianSolver::printInfo(const string &typ, const Vector &xd,
1152  const Vector &x, const Vector &q,
1153  const double t)
1154 {
1155  // ensure same length of vectors
1156  Vector x_=x.subVector(0,(unsigned int)xd.length()-1);
1157 
1158  printf(" Request type = %s\n",typ.c_str());
1159  printf(" Target rxPose [m] = %s\n",xd.toString().c_str());
1160  printf(" Target txPose [m] = %s\n",x_.toString().c_str());
1161  printf("Target txJoints [deg] = %s\n",q.toString().c_str());
1162  printf(" computed in [s] = %g\n",t);
1163 }
1164 
1165 
1166 /************************************************************************/
1168 {
1169  dof.resize(prt->chn->getN());
1170  bool fullness=true;
1171 
1172  for (unsigned int i=0; i<prt->chn->getN(); i++)
1173  if (!(dof[i]=!(*prt->chn)[i].isBlocked()))
1174  fullness=false;
1175 
1176  fullDOF=fullness;
1177  return dof;
1178 }
1179 
1180 
1181 /************************************************************************/
1182 bool CartesianSolver::decodeDOF(const Vector &_dof)
1183 {
1184  unsigned int len=std::min(prt->chn->getN(),(unsigned int)_dof.length());
1185  for (unsigned int i=0; i<len; i++)
1186  {
1187  if (_dof[i]>1.0)
1188  continue;
1189  else if (_dof[i]!=0.0)
1190  prt->chn->releaseLink(i);
1191  else
1192  prt->chn->blockLink(i);
1193  }
1194 
1196 
1197  return true;
1198 }
1199 
1200 
1201 /************************************************************************/
1202 bool CartesianSolver::handleJointsRestPosition(const Bottle *options, Bottle *reply)
1203 {
1204  bool ret=false;
1205 
1206  if (options!=NULL)
1207  {
1208  size_t len=std::min((size_t)options->size(),restJntPos.length());
1209  for (size_t i=0; i<len; i++)
1210  {
1211  double val=CTRL_DEG2RAD*options->get(i).asDouble();
1212  double min=(*prt->chn)[i].getMin();
1213  double max=(*prt->chn)[i].getMax();
1214 
1215  restJntPos[i]=std::min(std::max(val,min),max);
1216  }
1217 
1218  ret=true;
1219  }
1220 
1221  if (reply!=NULL)
1222  {
1223  Bottle &b=reply->addList();
1224  for (size_t i=0; i<restJntPos.length(); i++)
1225  b.addDouble(CTRL_RAD2DEG*restJntPos[i]);
1226  }
1227 
1228  return ret;
1229 }
1230 
1231 
1232 /************************************************************************/
1233 bool CartesianSolver::handleJointsRestWeights(const Bottle *options, Bottle *reply)
1234 {
1235  bool ret=false;
1236 
1237  if (options!=NULL)
1238  {
1239  size_t len=std::min((size_t)options->size(),restWeights.length());
1240  for (size_t i=0; i<len; i++)
1241  {
1242  double val=options->get(i).asInt();
1243  restWeights[i]=std::max(val,0.0);
1244  }
1245 
1246  ret=true;
1247  }
1248 
1249  if (reply!=NULL)
1250  {
1251  Bottle &b=reply->addList();
1252  for (size_t i=0; i<restWeights.length(); i++)
1253  b.addDouble(restWeights[i]);
1254  }
1255 
1256  return ret;
1257 }
1258 
1259 
1260 /************************************************************************/
1261 bool CartesianSolver::isNewDOF(const Vector &_dof)
1262 {
1263  size_t len=std::min((size_t)prt->chn->getN(),_dof.length());
1264  for (size_t i=0; i<len; i++)
1265  {
1266  if (_dof[i]>1.0)
1267  continue;
1268  else if (_dof[i]!=dof[i])
1269  return true;
1270  }
1271 
1272  return false;
1273 }
1274 
1275 
1276 /************************************************************************/
1277 bool CartesianSolver::open(Searchable &options)
1278 {
1279  if (configured)
1280  {
1281  yWarning("%s already configured",slvName.c_str());
1282  return true;
1283  }
1284 
1285  prt=getPartDesc(options);
1286  if (prt==NULL)
1287  {
1288  yError("Detected errors while processing parts description!");
1289  return false;
1290  }
1291 
1292  Property DHTable; prt->lmb->toLinksProperties(DHTable);
1293  yInfo()<<"DH Table: "<<DHTable.toString(); // stream version to prevent long strings truncation
1294 
1295  if (options.check("ping_robot_tmo"))
1296  ping_robot_tmo=options.find("ping_robot_tmo").asDouble();
1297 
1298  // open drivers
1299  int remainingJoints=prt->chn->getN();
1300  for (int i=0; i<prt->num; i++)
1301  {
1302  yInfo("Allocating device driver for %s ...",
1303  prt->prp[i].find("part").asString().c_str());
1304 
1305  PolyDriver *pDrv=(ping_robot_tmo>0.0)?
1306  waitPart(prt->prp[i]):
1307  new PolyDriver(prt->prp[i]);
1308 
1309  if (!pDrv->isValid())
1310  {
1311  delete pDrv;
1312  yError("Device driver not available!");
1313  close();
1314  return false;
1315  }
1316 
1317  // create interfaces
1318  IControlLimits *pLim;
1319  IEncoders *pEnc;
1320  int joints;
1321 
1322  if (!pDrv->view(pLim) || !pDrv->view(pEnc))
1323  {
1324  delete pDrv;
1325  yError("Problems acquiring interfaces!");
1326  close();
1327  return false;
1328  }
1329 
1330  pEnc->getAxes(&joints);
1331 
1332  // this is for allocating vector
1333  // to read data from the interface
1334  if (joints>maxPartJoints)
1335  maxPartJoints=joints;
1336 
1337  // handle chain's bounds
1338  if (joints>remainingJoints)
1339  joints=remainingJoints;
1340 
1341  remainingJoints-=joints;
1342 
1343  int *rmpTmp=new int[joints];
1344  for (int j=0; j<joints; j++)
1345  rmpTmp[j]=prt->rvs[i]?(joints-j-1):j;
1346 
1347  drv.push_back(pDrv);
1348  lim.push_back(pLim);
1349  enc.push_back(pEnc);
1350  jnt.push_back(joints);
1351  rmp.push_back(rmpTmp);
1352  }
1353 
1354  // handle joints rest position and weights
1355  restJntPos.resize(prt->chn->getN(),0.0);
1356  restWeights.resize(prt->chn->getN(),0.0);
1357  handleJointsRestPosition(options.find("rest_pos").asList());
1358  handleJointsRestWeights(options.find("rest_weights").asList());
1360 
1361  // dof here is not defined yet, so define it
1362  encodeDOF();
1363 
1364  // handle dof from options
1365  if (Bottle *v=options.find("dof").asList())
1366  {
1367  Vector _dof(v->size());
1368  for (size_t i=0; i<_dof.length(); i++)
1369  _dof[i]=v->get(i).asInt();
1370 
1371  decodeDOF(_dof);
1372  encodeDOF();
1373  }
1374 
1375  // joints bounds alignment
1376  if (!alignJointsBounds())
1377  {
1378  yError("Unable to retrieve joints limits!");
1379  close();
1380  return false;
1381  }
1382 
1383  // parse configuration options
1384  period=options.check("period",Value(CARTSLV_DEFAULT_PER)).asInt();
1385  setPeriod((double)period/1000.0);
1386 
1388  if (options.check(Vocab::decode(IKINSLV_VOCAB_OPT_POSE)))
1389  if (options.find(Vocab::decode(IKINSLV_VOCAB_OPT_POSE)).asVocab()==IKINSLV_VOCAB_VAL_POSE_XYZ)
1391 
1392  bool mode=false;
1393  if (options.check(Vocab::decode(IKINSLV_VOCAB_OPT_MODE)))
1394  if (options.find(Vocab::decode(IKINSLV_VOCAB_OPT_MODE)).asVocab()==IKINSLV_VOCAB_VAL_MODE_TRACK)
1395  mode=true;
1396 
1397  if (options.check("verbosity"))
1398  if (options.find("verbosity").asVocab()==IKINSLV_VOCAB_VAL_ON)
1399  verbosity=true;
1400 
1401  double tol=options.check("tol",Value(CARTSLV_DEFAULT_TOL)).asDouble();
1402  double constr_tol=options.check("constr_tol",Value(CARTSLV_DEFAULT_CONSTR_TOL)).asDouble();
1403  int maxIter=options.check("maxIter",Value(CARTSLV_DEFAULT_MAXITER)).asInt();
1404 
1405  // instantiate the optimizer
1406  slv=new iKinIpOptMin(*prt->chn,ctrlPose,tol,constr_tol,maxIter);
1407 
1408  // instantiate solver callback object if required
1409  if (options.check("interPoints"))
1410  if (options.find("interPoints").asVocab()==IKINSLV_VOCAB_VAL_ON)
1411  clb=new SolverCallback(this);
1412 
1413  // enable scaling
1414  slv->setUserScaling(true,100.0,100.0,100.0);
1415 
1416  // enforce linear inequalities constraints, if any
1417  if (prt->cns!=NULL)
1418  {
1419  slv->attachLIC(*prt->cns);
1420 
1421  double lower_bound_inf, upper_bound_inf;
1422  slv->getBoundsInf(lower_bound_inf,upper_bound_inf);
1423  slv->getLIC().getLowerBoundInf()=2.0*lower_bound_inf;
1424  slv->getLIC().getUpperBoundInf()=2.0*upper_bound_inf;
1425  slv->getLIC().update(NULL);
1426  }
1427 
1428  // set up 2nd task
1429  xd_2ndTask.resize(3,0.0);
1430  w_2ndTask.resize(3,0.0);
1431 
1432  // define input port
1433  inPort=new InputPort(this);
1434  inPort->useCallback();
1435 
1436  // init input port data
1437  inPort->set_dof(dof);
1438  inPort->get_pose()=ctrlPose;
1439  inPort->get_contMode()=contModeOld=mode;
1440 
1441  // define output port
1442  outPort=new BufferedPort<Bottle>;
1443 
1444  // count uncontrolled joints
1446 
1447  // get starting position
1448  initPos();
1449 
1450  configured=true;
1451 
1452  start();
1453 
1454  // open ports as very last thing, so that
1455  // the solver is completely operative
1456  // when it becomes yarp-visible
1457  inPort->open("/"+slvName+"/in");
1458  outPort->open("/"+slvName+"/out");
1459 
1460  return true;
1461 }
1462 
1463 
1464 /************************************************************************/
1465 bool CartesianSolver::changeDOF(const Vector &_dof)
1466 {
1467  if (isNewDOF(_dof))
1468  {
1469  // dof stuff
1470  Vector curDOF=dof;
1471  decodeDOF(_dof);
1472  inPort->set_dof(encodeDOF());
1473 
1474  if (dof==curDOF)
1475  return false;
1476 
1477  // update LinIneqConstr if any
1478  if (prt->cns!=NULL)
1479  prt->cns->update(NULL);
1480 
1481  // count uncontrolled joints
1483 
1484  // get starting position
1485  getFeedback();
1487 
1488  return true;
1489  }
1490  else
1491  return false;
1492 }
1493 
1494 
1495 /************************************************************************/
1497 {
1498  unsigned int nDOF=prt->chn->getDOF();
1499  int offs=0;
1500 
1501  qd_3rdTask.resize(nDOF);
1502  w_3rdTask.resize(nDOF,1.0);
1503  idx_3rdTask.resize(nDOF,1.0);
1504 
1505  for (unsigned int i=0; i<prt->chn->getN(); i++)
1506  {
1507  if (!(*prt->chn)[i].isBlocked())
1508  {
1509  qd_3rdTask[offs]=restJntPos[i];
1510  if (restWeights[i]!=0.0)
1511  {
1512  w_3rdTask[offs]=restWeights[i];
1513  idx_3rdTask[offs]=0.0;
1514  }
1515  offs++;
1516  }
1517  }
1518 }
1519 
1520 
1521 /************************************************************************/
1522 Vector CartesianSolver::solve(Vector &xd)
1523 {
1524  return slv->solve(prt->chn->getAng(),xd,
1527  NULL,NULL,clb);
1528 }
1529 
1530 
1531 /************************************************************************/
1533 {
1534  interrupting=true;
1535 }
1536 
1537 
1538 /************************************************************************/
1540 {
1541  if (closed)
1542  return;
1543 
1544  closing=true;
1545 
1546  if (isRunning())
1547  stop();
1548 
1549  if (inPort!=NULL)
1550  {
1551  inPort->interrupt();
1552  inPort->close();
1553  delete inPort;
1554  inPort=NULL;
1555  }
1556 
1557  if (outPort!=NULL)
1558  {
1559  outPort->interrupt();
1560  outPort->close();
1561  delete outPort;
1562  outPort=NULL;
1563  }
1564 
1565  delete slv;
1566  delete clb;
1567  slv=NULL;
1568  clb=NULL;
1569 
1570  for (size_t i=0; i<drv.size(); i++)
1571  {
1572  delete drv[i];
1573  drv[i]=NULL;
1574  }
1575 
1576  for (size_t i=0; i<rmp.size(); i++)
1577  {
1578  delete[] rmp[i];
1579  rmp[i]=NULL;
1580  }
1581 
1582  drv.clear();
1583  lim.clear();
1584  enc.clear();
1585  jnt.clear();
1586  rmp.clear();
1587 
1588  if (prt!=NULL)
1589  {
1590  delete prt->lmb;
1591  delete prt->cns;
1592  delete prt;
1593  prt=NULL;
1594  }
1595 
1596  yInfo("%s closed",slvName.c_str());
1597  closed=true;
1598 }
1599 
1600 
1601 /************************************************************************/
1603 {
1604  if (!configured)
1605  yError("%s not configured!",slvName.c_str());
1606  else
1607  yInfo("Starting %s at %d ms",slvName.c_str(),period);
1608 
1609  return configured;
1610 }
1611 
1612 
1613 /************************************************************************/
1615 {
1616  if (s)
1617  yInfo("%s started successfully",slvName.c_str());
1618  else
1619  yError("%s did not start!",slvName.c_str());
1620 }
1621 
1622 
1623 /************************************************************************/
1625 {
1626  if (isSuspended())
1627  yWarning("%s is already suspended",slvName.c_str());
1628  else
1629  {
1630  PeriodicThread::suspend();
1631  yInfo("%s suspended",slvName.c_str());
1632  }
1633 }
1634 
1635 
1636 /************************************************************************/
1638 {
1639  if (isSuspended())
1640  {
1641  initPos();
1642  PeriodicThread::resume();
1643  yInfo("%s resumed",slvName.c_str());
1644  }
1645  else
1646  yWarning("%s is already running",slvName.c_str());
1647 }
1648 
1649 
1650 /************************************************************************/
1652 {
1653  lock();
1654 
1655  // init conditions
1656  bool doSolve=false;
1657 
1658  // handle changeDOF()
1659  changeDOF(inPort->get_dof());
1660 
1661  // wake up sleeping threads
1662  postDOFHandling();
1663 
1664  // get the current configuration
1665  getFeedback();
1666 
1667  // acquire uncontrolled joints configuration
1668  Vector unctrlJoints;
1669  if (!fullDOF)
1670  {
1671  latchUncontrolledJoints(unctrlJoints);
1672 
1673  // upon setting the mode to continuous,
1674  // latch the old state so that we skip any
1675  // adjustment
1676  if (inPort->get_contMode() && !contModeOld)
1677  unctrlJointsOld=unctrlJoints;
1678 
1679  // detect movements of uncontrolled joints
1680  double distExtMoves=CTRL_RAD2DEG*norm(unctrlJoints-unctrlJointsOld);
1681 
1682  // run the solver if movements of uncontrolled joints
1683  // are detected and we are in continuous mode
1684  doSolve|=inPort->get_contMode() && (distExtMoves>CARTSLV_UNCTRLEDJNTS_THRES);
1685  if (doSolve && verbosity)
1686  yInfo("%s: detected movements on uncontrolled joints (norm=%g>%g deg)",
1687  slvName.c_str(),distExtMoves,CARTSLV_UNCTRLEDJNTS_THRES);
1688  }
1689 
1690  // run the solver if any input is received
1691  if (inPort->isNewDataEvent())
1692  {
1693  // update solver condition
1694  doSolve=true;
1695  }
1696 
1697  // solver part
1698  if (doSolve)
1699  {
1700  // point to the desired pose
1701  Vector xd=inPort->get_xd();
1702  if (inPort->get_tokenPtr()) // latch token
1703  {
1704  token=*inPort->get_tokenPtr();
1705  pToken=&token;
1706  }
1707  else
1708  pToken=NULL;
1709 
1710  // set things for the 3rd task
1711  for (unsigned int i=0; i<prt->chn->getDOF(); i++)
1712  if (idx_3rdTask[i]!=0.0)
1713  qd_3rdTask[i]=(*prt->chn)(i).getAng();
1714 
1715  // update optimizer's options
1716  slv->set_ctrlPose(ctrlPose=inPort->get_pose());
1717 
1718  // call the solver to converge
1719  double t0=Time::now();
1720  Vector q=solve(xd);
1721  double t1=Time::now();
1722 
1723  // q is the estimation of the real qd,
1724  // so that x is the actual achieved pose
1725  Vector x=prt->chn->EndEffPose(q);
1726 
1727  // change to degrees
1728  q*=CTRL_RAD2DEG;
1729 
1730  // send data
1731  send(xd,x,q,pToken);
1732 
1733  // dump on screen
1734  if (verbosity)
1735  printInfo("go",xd,x,q,t1-t0);
1736 
1737  // save the values of uncontrolled joints
1738  if (!fullDOF)
1739  unctrlJointsOld=unctrlJoints;
1740  }
1741 
1742  contModeOld=inPort->get_contMode();
1743 
1744  unlock();
1745 }
1746 
1747 
1748 /************************************************************************/
1750 {
1751  yInfo("Stopping %s ...",slvName.c_str());
1752 }
1753 
1754 
1755 /************************************************************************/
1757 {
1758  close();
1759 
1760  rpcPort->interrupt();
1761  rpcPort->close();
1762  delete rpcPort;
1763  delete cmdProcessor;
1764 }
1765 
1766 
1767 /************************************************************************/
1769 {
1770  type="right";
1771  string part_type=type;
1772  double version=1.0;
1773  if (options.check("type"))
1774  {
1775  type=options.find("type").asString();
1776  part_type=type.substr(0,type.find("_"));
1777  if ((part_type!="left") && (part_type!="right"))
1778  type=part_type="right";
1779 
1780  size_t underscore=type.find('_');
1781  if (underscore!=string::npos)
1782  version=strtod(type.substr(underscore+2).c_str(),NULL);
1783  }
1784 
1785  string robot=options.check("robot",Value("icub")).asString();
1786  Property optTorso("(device remote_controlboard)");
1787  Property optArm("(device remote_controlboard)");
1788 
1789  string partTorso ="torso";
1790  string remoteTorso="/"+robot+"/"+partTorso;
1791  string localTorso ="/"+slvName+"/"+partTorso;
1792  optTorso.put("remote",remoteTorso);
1793  optTorso.put("local",localTorso);
1794  optTorso.put("robot",robot);
1795  optTorso.put("part",partTorso);
1796 
1797  string partArm =part_type=="left"?"left_arm":"right_arm";
1798  string remoteArm="/"+robot+"/"+partArm;
1799  string localArm ="/"+slvName+"/"+partArm;
1800  optArm.put("remote",remoteArm);
1801  optArm.put("local",localArm);
1802  optArm.put("robot",robot);
1803  optArm.put("part",partArm);
1804 
1806  p->lmb=new iCubArm(type);
1807  p->chn=p->lmb->asChain();
1808  p->cns=new iCubShoulderConstr(*static_cast<iCubArm*>(p->lmb));
1809  p->prp.push_back(optTorso);
1810  p->prp.push_back(optArm);
1811  p->rvs.push_back(version<3.0); // torso
1812  p->rvs.push_back(false); // arm
1813  p->num=2; // number of parts
1814 
1815  return p;
1816 }
1817 
1818 
1819 /************************************************************************/
1820 bool iCubArmCartesianSolver::open(Searchable &options)
1821 {
1822  // call father's open() method
1823  if (CartesianSolver::open(options))
1824  {
1825  // Identify the elbow xyz position to be used as 2nd task
1827 
1828  // try to keep elbow as low as possible
1829  xd_2ndTask[2]=-1.0;
1830  w_2ndTask[2]=1.0;
1831  }
1832 
1833  return configured;
1834 }
1835 
1836 
1837 /************************************************************************/
1838 bool iCubArmCartesianSolver::decodeDOF(const Vector &_dof)
1839 {
1840  // latch current status
1841  Vector newDOF=dof;
1842 
1843  // update desired status
1844  size_t len=std::min((size_t)prt->chn->getN(),_dof.length());
1845  for (size_t i=0; i<len; i++)
1846  newDOF[i]=_dof[i];
1847 
1848  // check whether shoulder's axes are treated properly
1849  if (!(((newDOF[3]!=0.0) && (newDOF[4]!=0) && (newDOF[5]!=0)) ||
1850  ((newDOF[3]==0.0) && (newDOF[4]==0.0) && (newDOF[5]==0.0))))
1851  {
1852  // restore previous condition:
1853  // they all shall be on or off
1854  newDOF[3]=newDOF[4]=newDOF[5]=dof[3];
1855 
1856  yWarning("%s: attempt to set one shoulder's joint differently from others",slvName.c_str());
1857  }
1858 
1859  return CartesianSolver::decodeDOF(newDOF);
1860 }
1861 
1862 
1863 /************************************************************************/
1865 {
1866  type="right";
1867  string part_type=type;
1868  double version=1.0;
1869  if (options.check("type"))
1870  {
1871  type=options.find("type").asString();
1872  part_type=type.substr(0,type.find("_"));
1873  if ((part_type!="left") && (part_type!="right"))
1874  type=part_type="right";
1875 
1876  size_t underscore=type.find('_');
1877  if (underscore!=string::npos)
1878  version=strtod(type.substr(underscore+2).c_str(),NULL);
1879  }
1880 
1881  string robot=options.check("robot",Value("icub")).asString();
1882  Property optLeg("(device remote_controlboard)");
1883 
1884  string partLeg =part_type=="left"?"left_leg":"right_leg";
1885  string remoteLeg="/"+robot+"/"+partLeg;
1886  string localLeg ="/"+slvName+"/"+partLeg;
1887 
1888  optLeg.put("remote",remoteLeg);
1889  optLeg.put("local",localLeg);
1890  optLeg.put("robot",robot);
1891  optLeg.put("part",partLeg);
1892 
1894  p->lmb=new iCubLeg(type);
1895  p->chn=p->lmb->asChain();
1896  p->cns=NULL;
1897  p->prp.push_back(optLeg);
1898  p->rvs.push_back(false);
1899  p->num=1;
1900 
1901  return p;
1902 }
1903 
1904 
1905 
#define IKINSLV_VOCAB_OPT_CONVERGENCE
Definition: iKinVocabs.h:39
virtual bool handleJointsRestPosition(const yarp::os::Bottle *options, yarp::os::Bottle *reply=NULL)
Definition: iKinSlv.cpp:1202
void fillDOFInfo(yarp::os::Bottle &reply)
Definition: iKinSlv.cpp:514
yarp::sig::Vector idx_3rdTask
Definition: iKinSlv.h:382
yarp::os::Event dofEvent
Definition: iKinSlv.h:384
virtual yarp::sig::Vector & encodeDOF()
Definition: iKinSlv.cpp:1167
#define IKINSLV_VOCAB_REP_NACK
Definition: iKinVocabs.h:49
std::deque< yarp::dev::IEncoders * > enc
Definition: iKinSlv.h:337
SolverCallback * clb
Definition: iKinSlv.h:342
#define CARTSLV_WEIGHT_3RD_TASK
Definition: iKinSlv.cpp:27
virtual bool open(yarp::os::Searchable &options)
Configure the solver and start it up.
Definition: iKinSlv.cpp:1820
#define CARTSLV_DEFAULT_TOL
Definition: iKinSlv.cpp:23
double & getLowerBoundInf()
Returns a reference to the internal representation of -inf.
Definition: iKinIpOpt.h:136
#define IKINSLV_VOCAB_CMD_SUSP
Definition: iKinVocabs.h:19
yarp::dev::PolyDriver * waitPart(const yarp::os::Property &partOpt)
Definition: iKinSlv.cpp:308
#define CARTSLV_UNCTRLEDJNTS_THRES
Definition: iKinSlv.cpp:28
#define IKINSLV_VOCAB_CMD_ASK
Definition: iKinVocabs.h:18
#define CARTSLV_WEIGHT_2ND_TASK
Definition: iKinSlv.cpp:26
std::deque< yarp::os::Property > prp
Definition: iKinSlv.h:319
bool setHN(const yarp::sig::Matrix &_HN)
Sets HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
Definition: iKinFwd.cpp:579
#define IKINSLV_VOCAB_VAL_PRIO_XYZ
Definition: iKinVocabs.h:42
iKinIpOptMin * slv
Definition: iKinSlv.h:341
virtual void threadRelease()
Definition: iKinSlv.cpp:1749
yarp::os::Port * rpcPort
Definition: iKinSlv.h:345
static int stop
Definition: iCub_Sim.cpp:46
#define IKINSLV_VOCAB_CMD_CFG
Definition: iKinVocabs.h:23
virtual bool decodeDOF(const yarp::sig::Vector &_dof)
Definition: iKinSlv.cpp:1838
std::deque< yarp::dev::IControlLimits * > lim
Definition: iKinSlv.h:336
RpcProcessor * cmdProcessor
Definition: iKinSlv.h:344
std::deque< bool > rvs
Definition: iKinSlv.h:320
A class for defining the iCub Leg.
Definition: iKinFwd.h:1235
bool setLimits(int axis, double min, double max)
Definition: iKinSlv.cpp:382
void getFeedback(const bool wait=false)
Definition: iKinSlv.cpp:432
#define IKINSLV_VOCAB_OPT_TOKEN
Definition: iKinVocabs.h:33
PartDescriptor * prt
Definition: iKinSlv.h:334
STL namespace.
void setMaxIter(const int max_iter)
Sets Maximum Iteration.
Definition: iKinIpOpt.cpp:869
iKinChain & get2ndTaskChain()
Retrieves the 2nd task&#39;s chain.
Definition: iKinIpOpt.cpp:862
bool changeDOF(const yarp::sig::Vector &_dof)
Definition: iKinSlv.cpp:1465
unsigned int getDOF() const
Returns the current number of Chain&#39;s DOF.
Definition: iKinFwd.h:557
bool isNewDOF(const yarp::sig::Vector &_dof)
Definition: iKinSlv.cpp:1261
#define IKINSLV_VOCAB_VAL_PRIO_ANG
Definition: iKinVocabs.h:43
virtual ~CartesianSolver()
Default destructor.
Definition: iKinSlv.cpp:1756
virtual PartDescriptor * getPartDesc(yarp::os::Searchable &options)=0
#define IKINSLV_VOCAB_CMD_SET
Definition: iKinVocabs.h:17
virtual void respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
Definition: iKinSlv.cpp:522
int n
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
Definition: iKinFwd.cpp:849
void specify2ndTaskEndEff(const unsigned int n)
Selects the End-Effector of the 2nd task by giving the ordinal number n of last joint pointing at it...
Definition: iKinIpOpt.cpp:845
void attachLIC(iKinLinIneqConstr &lic)
Attach a iKinLinIneqConstr object in order to impose constraints of the form lB <= C*q <= uB...
Definition: iKinIpOpt.h:287
yarp::os::Mutex mutex
Definition: iKinSlv.h:348
int getMaxIter() const
Retrieves the current value of Maximum Iteration.
Definition: iKinIpOpt.cpp:881
static void addTokenOption(yarp::os::Bottle &b, const double token)
Appends to a bottle a token to be exchanged with the solver.
Definition: iKinHlp.cpp:140
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:25
const FSC max
Definition: strain.h:48
friend class InputPort
Definition: iKinSlv.h:426
virtual yarp::sig::Vector solve(yarp::sig::Vector &xd)
Definition: iKinSlv.cpp:1522
#define IKINCTRL_POSE_FULL
Definition: iKinInv.h:37
#define IKINSLV_VOCAB_REP_ACK
Definition: iKinVocabs.h:48
virtual void suspend()
Suspend the solver&#39;s main loop.
Definition: iKinSlv.cpp:1624
friend class SolverCallback
Definition: iKinSlv.h:427
static int v
Definition: iCub_Sim.cpp:47
double getTol() const
Retrieves cost function tolerance.
Definition: iKinIpOpt.cpp:915
yarp::sig::Vector getAng()
Returns the current free joint angles values.
Definition: iKinFwd.cpp:610
#define IKINSLV_VOCAB_VAL_ON
Definition: iKinVocabs.h:46
virtual void update(void *)
Updates internal state.
Definition: iKinIpOpt.h:162
#define IKINSLV_VOCAB_OPT_Q
Definition: iKinVocabs.h:32
void getBoundsInf(double &lower, double &upper)
Returns the lower and upper bounds to represent -inf and +inf.
Definition: iKinIpOpt.cpp:1000
bool setBlockingValue(const unsigned int i, double Ang)
Changes the value of the ith blocked Link.
Definition: iKinFwd.cpp:413
virtual void interrupt()
Interrupt the open() method waiting for motor parts to be ready.
Definition: iKinSlv.cpp:1532
#define IKINSLV_VOCAB_CMD_STATUS
Definition: iKinVocabs.h:21
const FSC min
Definition: strain.h:49
yarp::sig::Vector w_3rdTask
Definition: iKinSlv.h:381
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
#define IKINSLV_VOCAB_OPT_REST_POS
Definition: iKinVocabs.h:35
void setConstrTol(const double constr_tol)
Sets constraints tolerance.
Definition: iKinIpOpt.cpp:924
#define IKINSLV_VOCAB_VAL_MODE_SINGLE
Definition: iKinVocabs.h:45
yarp::sig::Vector unctrlJointsOld
Definition: iKinSlv.h:371
cmd
Definition: dataTypes.h:30
yarp::sig::Vector w_2ndTask
Definition: iKinSlv.h:378
virtual yarp::sig::Vector solve(const yarp::sig::Vector &q0, yarp::sig::Vector &xd, double weight2ndTask, yarp::sig::Vector &xd_2nd, yarp::sig::Vector &w_2nd, double weight3rdTask, yarp::sig::Vector &qd_3rd, yarp::sig::Vector &w_3rd, int *exit_code=NULL, bool *exhalt=NULL, iKinIterateCallback *iterate=NULL)
Executes the IpOpt algorithm trying to converge on target.
Definition: iKinIpOpt.cpp:1019
yarp::sig::Vector qd_3rdTask
Definition: iKinSlv.h:380
std::deque< int > jnt
Definition: iKinSlv.h:338
#define IKINCTRL_POSE_XYZ
Definition: iKinInv.h:38
yarp::sig::Matrix hwLimits
Definition: iKinSlv.h:368
std::string get_posePriority() const
Returns the Pose priority settings.
Definition: iKinIpOpt.h:279
#define IKINSLV_VOCAB_CMD_HELP
Definition: iKinVocabs.h:22
Definition: main.cpp:52
#define IKINSLV_VOCAB_OPT_PRIO
Definition: iKinVocabs.h:27
yarp::sig::Matrix swLimits
Definition: iKinSlv.h:369
double & getUpperBoundInf()
Returns a reference to the internal representation of +inf.
Definition: iKinIpOpt.h:142
#define IKINSLV_VOCAB_VAL_POSE_FULL
Definition: iKinVocabs.h:40
virtual bool open(yarp::os::Searchable &options)
Configure the solver and start it up.
Definition: iKinSlv.cpp:1277
#define IKINSLV_VOCAB_VAL_OFF
Definition: iKinVocabs.h:47
void setTol(const double tol)
Sets cost function tolerance.
Definition: iKinIpOpt.cpp:907
iKinLinIneqConstr * cns
Definition: iKinSlv.h:318
virtual void prepareJointsRestTask()
Definition: iKinSlv.cpp:1496
virtual PartDescriptor * getPartDesc(yarp::os::Searchable &options)
Definition: iKinSlv.cpp:1768
#define CARTSLV_DEFAULT_MAXITER
Definition: iKinSlv.cpp:25
GtkWidget * sw
Definition: main.cpp:170
#define IKINSLV_VOCAB_OPT_VERB
Definition: iKinVocabs.h:34
void latchUncontrolledJoints(yarp::sig::Vector &joints)
Definition: iKinSlv.cpp:415
virtual bool decodeDOF(const yarp::sig::Vector &_dof)
Definition: iKinSlv.cpp:1182
bool read(ConnectionReader &connection)
Definition: main.cpp:1466
#define IKINSLV_VOCAB_VAL_MODE_TRACK
Definition: iKinVocabs.h:44
virtual bool threadInit()
Definition: iKinSlv.cpp:1602
#define IKINSLV_VOCAB_CMD_RUN
Definition: iKinVocabs.h:20
void send(const yarp::sig::Vector &xd, const yarp::sig::Vector &x, const yarp::sig::Vector &q, double *tok)
Definition: iKinSlv.cpp:1133
#define IKINSLV_VOCAB_OPT_X
Definition: iKinVocabs.h:31
yarp::os::BufferedPort< yarp::os::Bottle > * outPort
Definition: iKinSlv.h:347
#define IKINSLV_VOCAB_CMD_GET
Definition: iKinVocabs.h:16
yarp::sig::Vector restWeights
Definition: iKinSlv.h:375
static void addVectorOption(yarp::os::Bottle &b, const int vcb, const yarp::sig::Vector &v)
Definition: iKinHlp.cpp:26
void setUserScaling(const bool useUserScaling, const double _obj_scaling, const double _x_scaling, const double _g_scaling)
Enables/disables user scaling factors.
Definition: iKinIpOpt.cpp:962
#define IKINSLV_VOCAB_OPT_REST_WEIGHTS
Definition: iKinVocabs.h:36
virtual void close()
Stop the solver and dispose it.
Definition: iKinSlv.cpp:1539
virtual bool handleJointsRestWeights(const yarp::os::Bottle *options, yarp::os::Bottle *reply=NULL)
Definition: iKinSlv.cpp:1233
friend class RpcProcessor
Definition: iKinSlv.h:425
static yarp::os::Bottle * getTargetOption(const yarp::os::Bottle &b)
Retrieves commanded target data from a bottle.
Definition: iKinHlp.cpp:149
std::deque< yarp::dev::PolyDriver * > drv
Definition: iKinSlv.h:335
yarp::sig::Vector dof
Definition: iKinSlv.h:372
Class for dealing with iCub shoulder&#39;s constraints due to the cables length.
Definition: iKinIpOpt.h:172
yarp::sig::Vector xd_2ndTask
Definition: iKinSlv.h:377
#define CARTSLV_DEFAULT_CONSTR_TOL
Definition: iKinSlv.cpp:24
bool toLinksProperties(yarp::os::Property &options)
Provides the links attributes listed in a property object.
Definition: iKinFwd.cpp:1442
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
Definition: iKinFwd.h:1013
#define CARTSLV_DEFAULT_PER
Definition: iKinSlv.cpp:22
#define IKINSLV_VOCAB_OPT_DOF
Definition: iKinVocabs.h:28
static yarp::os::Bottle * getJointsOption(const yarp::os::Bottle &b)
Retrieves the joints configuration data.
Definition: iKinHlp.cpp:163
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
A class for defining the iCub Arm.
Definition: iKinFwd.h:1081
iKinLinIneqConstr & getLIC()
Returns a reference to the attached Linear Inequality Constraints object.
Definition: iKinIpOpt.h:295
virtual void resume()
Resume the solver&#39;s main loop.
Definition: iKinSlv.cpp:1637
#define IKINSLV_VOCAB_OPT_LIM
Definition: iKinVocabs.h:29
#define IKINSLV_VOCAB_OPT_TASK2
Definition: iKinVocabs.h:38
bool releaseLink(const unsigned int i)
Releases the ith Link.
Definition: iKinFwd.cpp:462
void set_ctrlPose(const unsigned int _ctrlPose)
Sets the state of Pose control settings.
Definition: iKinIpOpt.cpp:822
virtual void afterStart(bool)
Definition: iKinSlv.cpp:1614
void printInfo(const std::string &typ, const yarp::sig::Vector &xd, const yarp::sig::Vector &x, const yarp::sig::Vector &q, const double t)
Definition: iKinSlv.cpp:1151
bool set_posePriority(const std::string &priority)
Sets the Pose priority for weighting more either position or orientation while reaching in full pose...
Definition: iKinIpOpt.cpp:832
Class for inverting chain&#39;s kinematics based on IpOpt lib.
Definition: iKinIpOpt.h:197
virtual PartDescriptor * getPartDesc(yarp::os::Searchable &options)
Definition: iKinSlv.cpp:1864
double getConstrTol() const
Retrieves constraints tolerance.
Definition: iKinIpOpt.cpp:932
GLenum mode
Definition: rendering.cpp:49
yarp::sig::Vector restJntPos
Definition: iKinSlv.h:374
bool blockLink(const unsigned int i, double Ang)
Blocks the ith Link at the a certain value of its joint angle.
Definition: iKinFwd.cpp:393
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
#define IKINSLV_VOCAB_OPT_XD
Definition: iKinVocabs.h:30
std::deque< int * > rmp
Definition: iKinSlv.h:339
#define IKINSLV_VOCAB_VAL_POSE_XYZ
Definition: iKinVocabs.h:41
void clear()
Removes all Links.
Definition: iKinFwd.cpp:352
#define IKINSLV_VOCAB_OPT_MODE
Definition: iKinVocabs.h:25
#define IKINSLV_VOCAB_CMD_QUIT
Definition: iKinVocabs.h:24
#define IKINSLV_VOCAB_OPT_POSE
Definition: iKinVocabs.h:26
#define IKINSLV_VOCAB_OPT_TIP_FRAME
Definition: iKinVocabs.h:37
Abstract class defining the core of on-line solvers.
Definition: iKinSlv.h:330
#define CTRL_RAD2DEG
Definition: XSensMTx.cpp:24