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