iCub-main
Loading...
Searching...
No Matches
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/Log.h>
16#include <yarp/os/Network.h>
17#include <yarp/os/Time.h>
18
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
30using namespace std;
31using namespace yarp::os;
32using namespace yarp::sig;
33using namespace yarp::dev;
34using namespace yarp::math;
35using namespace iCub::ctrl;
36using namespace iCub::iKin;
37
38
39/************************************************************************/
40bool 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/************************************************************************/
61{
62 slv=_slv;
63
64 maxLen=7;
65 xd.resize(maxLen,0.0);
66 dof.resize(1,0.0);
67
69 contMode=false;
70 isNew=false;
71 token=0.0;
72 pToken=NULL;
73}
74
75
76/************************************************************************/
77void InputPort::set_dof(const Vector &_dof)
78{
79 lock_guard<mutex> lck(mtx);
80 dof=_dof;
81}
82
83
84/************************************************************************/
86{
87 lock_guard<mutex> lck(mtx);
88 Vector _dof=dof;
89 return _dof;
90}
91
92
93/************************************************************************/
95{
96 lock_guard<mutex> lck(mtx);
97 Vector _xd=xd;
98 return _xd;
99}
100
101
102/************************************************************************/
103void InputPort::reset_xd(const Vector &_xd)
104{
105 lock_guard<mutex> lck(mtx);
106 xd=_xd;
107 isNew=false;
108}
109
110
111/************************************************************************/
113{
114 if (isNew)
115 {
116 isNew=false;
117 return true;
118 }
119 else
120 return false;
121}
122
123
124/************************************************************************/
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).asFloat64();
133 return isNew=true;
134 }
135 else
136 return false;
137}
138
139
140/************************************************************************/
141bool 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).asInt32();
151 mtx.unlock();
152
153 slv->unlock();
154 return true;
155 }
156 else
157 return false;
158}
159
160
161/************************************************************************/
162bool InputPort::handlePose(const int newPose)
163{
164 if (newPose==IKINSLV_VOCAB_VAL_POSE_FULL)
165 {
167 isNew=true;
168
170 return true;
171 }
172 else if (newPose==IKINSLV_VOCAB_VAL_POSE_XYZ)
173 {
175 isNew=true;
176
178 return true;
179 }
180 else
181 return false;
182}
183
184
185/************************************************************************/
186bool 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/************************************************************************/
208void InputPort::onRead(Bottle &b)
209{
210 bool xdOptIn =b.check(Vocab32::decode(IKINSLV_VOCAB_OPT_XD));
211 bool dofOptIn =b.check(Vocab32::decode(IKINSLV_VOCAB_OPT_DOF));
212 bool poseOptIn=b.check(Vocab32::decode(IKINSLV_VOCAB_OPT_POSE));
213 bool modeOptIn=b.check(Vocab32::decode(IKINSLV_VOCAB_OPT_MODE));
214
215 if (xdOptIn)
216 {
218 pToken=&token;
219 else
220 pToken=NULL;
221 }
222
223 if (modeOptIn)
224 if (!handleMode(b.find(Vocab32::decode(IKINSLV_VOCAB_OPT_MODE)).asVocab32()))
225 yWarning("%s: got incomplete %s command",slv->slvName.c_str(),
226 Vocab32::decode(IKINSLV_VOCAB_OPT_MODE).c_str());
227
228 if (dofOptIn)
229 if (!handleDOF(b.find(Vocab32::decode(IKINSLV_VOCAB_OPT_DOF)).asList()))
230 yWarning("%s: expected %s data",slv->slvName.c_str(),
231 Vocab32::decode(IKINSLV_VOCAB_OPT_DOF).c_str());
232
233 if (poseOptIn)
234 if (!handlePose(b.find(Vocab32::decode(IKINSLV_VOCAB_OPT_POSE)).asVocab32()))
235 yWarning("%s: got incomplete %s command",slv->slvName.c_str(),
236 Vocab32::decode(IKINSLV_VOCAB_OPT_POSE).c_str());
237
238 if (slv->handleJointsRestPosition(b.find(Vocab32::decode(IKINSLV_VOCAB_OPT_REST_POS)).asList()) ||
239 slv->handleJointsRestWeights(b.find(Vocab32::decode(IKINSLV_VOCAB_OPT_REST_WEIGHTS)).asList()))
240 {
241 slv->lock();
243 slv->unlock();
244 }
245
246 // shall be the last handling
247 if (xdOptIn)
248 {
249 if (!handleTarget(b.find(Vocab32::decode(IKINSLV_VOCAB_OPT_XD)).asList()))
250 yWarning("%s: expected %s data",slv->slvName.c_str(),
251 Vocab32::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(),Vocab32::decode(IKINSLV_VOCAB_OPT_XD).c_str());
256}
257
258
259/************************************************************************/
260void SolverCallback::exec(const Vector &xd, const Vector &q)
261{
263}
264
265
266/************************************************************************/
267CartesianSolver::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;
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/************************************************************************/
301PolyDriver *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/************************************************************************/
375bool 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/************************************************************************/
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/************************************************************************/
425void 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/************************************************************************/
508{
509 for (unsigned int i=0; i<prt->chn->getN(); i++)
510 reply.addInt32(int(!(*prt->chn)[i].isBlocked()));
511}
512
513
514/************************************************************************/
515void CartesianSolver::respond(const Bottle &command, Bottle &reply)
516{
517 if (command.size())
518 {
519 int vcb=command.get(0).asVocab32();
520
522 reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
523 else switch (vcb)
524 {
525 //-----------------
527 {
528 if (command.size()>1)
529 {
530 switch (command.get(1).asVocab32())
531 {
532 //-----------------
534 {
535 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
536
538 reply.addVocab32(IKINSLV_VOCAB_VAL_POSE_FULL);
539 else
540 reply.addVocab32(IKINSLV_VOCAB_VAL_POSE_XYZ);
541
542 break;
543 }
544
545 //-----------------
547 {
548 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
549
550 string priority=slv->get_posePriority();
551 if (priority=="position")
552 reply.addVocab32(IKINSLV_VOCAB_VAL_PRIO_XYZ);
553 else
554 reply.addVocab32(IKINSLV_VOCAB_VAL_PRIO_ANG);
555
556 break;
557 }
558
559 //-----------------
561 {
562 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
563
564 if (inPort->get_contMode())
565 reply.addVocab32(IKINSLV_VOCAB_VAL_MODE_TRACK);
566 else
567 reply.addVocab32(IKINSLV_VOCAB_VAL_MODE_SINGLE);
568
569 break;
570 }
571
572 //-----------------
574 {
575 if (command.size()>2)
576 {
577 int axis=command.get(2).asInt32();
578
579 if (axis<(int)prt->chn->getN())
580 {
581 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
582 reply.addFloat64(swLimits(axis,0));
583 reply.addFloat64(swLimits(axis,1));
584 }
585 else
586 reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
587 }
588 else
589 reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
590
591 break;
592 }
593
594 //-----------------
596 {
597 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
598
599 if (verbosity)
600 reply.addVocab32(IKINSLV_VOCAB_VAL_ON);
601 else
602 reply.addVocab32(IKINSLV_VOCAB_VAL_OFF);
603
604 break;
605 }
606
607 //-----------------
609 {
610 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
611 Bottle &dofPart=reply.addList();
612 fillDOFInfo(dofPart);
613 break;
614 }
615
616 //-----------------
618 {
619 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
620 handleJointsRestPosition(NULL,&reply);
621 break;
622 }
623
624 //-----------------
626 {
627 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
628 handleJointsRestWeights(NULL,&reply);
629 break;
630 }
631
632 //-----------------
634 {
635 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
636 Bottle &payLoad=reply.addList();
637 payLoad.addInt32(slv->get2ndTaskChain().getN());
638
639 Bottle &posPart=payLoad.addList();
640 for (size_t i=0; i<xd_2ndTask.length(); i++)
641 posPart.addFloat64(xd_2ndTask[i]);
642
643 Bottle &weightsPart=payLoad.addList();
644 for (size_t i=0; i<w_2ndTask.length(); i++)
645 weightsPart.addFloat64(w_2ndTask[i]);
646
647 break;
648 }
649
650 //-----------------
652 {
653 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
654 Bottle &payLoad=reply.addList();
655
656 Bottle &tol=payLoad.addList();
657 tol.addString("tol");
658 tol.addFloat64(slv->getTol());
659
660 Bottle &constr_tol=payLoad.addList();
661 constr_tol.addString("constr_tol");
662 constr_tol.addFloat64(slv->getConstrTol());
663
664 Bottle &maxIter=payLoad.addList();
665 maxIter.addString("max_iter");
666 maxIter.addInt32(slv->getMaxIter());
667
668 break;
669 }
670
671 //-----------------
672 default:
673 reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
674 }
675 }
676 else
677 reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
678
679 break;
680 }
681
682 //-----------------
684 {
685 if (command.size()>2)
686 {
687 switch (command.get(1).asVocab32())
688 {
689 //-----------------
691 {
692 if (inPort->handlePose(command.get(2).asVocab32()))
693 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
694 else
695 reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
696
697 break;
698 }
699
700 //-----------------
702 {
703 int type=command.get(2).asVocab32();
705 {
706 slv->set_posePriority("position");
707 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
708 }
710 {
711 slv->set_posePriority("orientation");
712 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
713 }
714 else
715 reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
716
717 break;
718 }
719
720 //-----------------
722 {
723 if (inPort->handleMode(command.get(2).asVocab32()))
724 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
725 else
726 reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
727
728 break;
729 }
730
731 //-----------------
733 {
734 if (command.size()>4)
735 {
736 int axis=command.get(2).asInt32();
737 double min=command.get(3).asFloat64();
738 double max=command.get(4).asFloat64();
739
740 if (setLimits(axis,min,max))
741 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
742 else
743 reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
744 }
745 else
746 reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
747
748 break;
749 }
750
751 //-----------------
753 {
754 int sw=command.get(2).asVocab32();
755
756 if (sw==IKINSLV_VOCAB_VAL_ON)
757 {
758 verbosity=true;
759 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
760 }
761 else if (sw==IKINSLV_VOCAB_VAL_OFF)
762 {
763 verbosity=false;
764 reply.addVocab32(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.addVocab32(IKINSLV_VOCAB_REP_ACK);
778 Bottle &dofPart=reply.addList();
779 fillDOFInfo(dofPart);
780 }
781 else
782 reply.addVocab32(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.addVocab32(IKINSLV_VOCAB_REP_ACK);
799 reply.append(restPart);
800 }
801 else
802 reply.addVocab32(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.addVocab32(IKINSLV_VOCAB_REP_ACK);
819 reply.append(restPart);
820 }
821 else
822 reply.addVocab32(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).asFloat64();
837
838 Vector o(4);
839 for (size_t i=0; i<o.length(); i++)
840 o[i]=tipPart->get(i+x.length()).asFloat64();
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.addVocab32(IKINSLV_VOCAB_REP_ACK);
850 break;
851 }
852 }
853
854 reply.addVocab32(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).asInt32();
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).asFloat64();
875
876 for (size_t i=0; i<w_2ndTask.length(); i++)
877 w_2ndTask[i]=weightsPart->get(i).asFloat64();
878
879 if (n>=0)
881 else
883
884 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
885 break;
886 }
887 }
888 }
889 }
890
891 reply.addVocab32(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").asFloat64());
904 cnt++;
905 }
906
907 if (payLoad->check("constr_tol"))
908 {
909 slv->setConstrTol(payLoad->find("constr_tol").asFloat64());
910 cnt++;
911 }
912
913 if (payLoad->check("max_iter"))
914 {
915 slv->setMaxIter(payLoad->find("max_iter").asInt32());
916 cnt++;
917 }
918
919 reply.addVocab32(cnt>0?IKINSLV_VOCAB_REP_ACK:IKINSLV_VOCAB_REP_NACK);
920 }
921 else
922 reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
923
924 break;
925 }
926
927 //-----------------
928 default:
929 reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
930 }
931 }
932 else
933 reply.addVocab32(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.addVocab32(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.addVocab32(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).asFloat64();
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 (unsigned int i=0; i<len; i++)
969 (*prt->chn)(i).setAng(CTRL_DEG2RAD*b_q->get(i).asFloat64());
970 }
971 else
972 getFeedback(); // otherwise get the current configuration
973
974 // account for the pose
975 if (command.check(Vocab32::decode(IKINSLV_VOCAB_OPT_POSE)))
976 {
977 int pose=command.find(Vocab32::decode(IKINSLV_VOCAB_OPT_POSE)).asVocab32();
978
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.addVocab32(IKINSLV_VOCAB_REP_ACK);
1013
1014 unlock();
1015
1016 break;
1017 }
1018
1019 //-----------------
1021 {
1022 suspend();
1023 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
1024 break;
1025 }
1026
1027 //-----------------
1029 {
1030 if (!isRunning())
1031 {
1032 initPos();
1033 start();
1034 }
1035 else
1036 resume();
1037
1038 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
1039 break;
1040 }
1041
1042 //-----------------
1044 {
1045 reply.addVocab32(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 //-----------------
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.addVocab32(IKINSLV_VOCAB_REP_ACK);
1063 else
1064 reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
1065
1066 break;
1067 }
1068
1069 //-----------------
1071 {
1072 reply.addVocab32("many");
1073 reply.addString("***** commands");
1074 reply.addVocab32(IKINSLV_VOCAB_CMD_GET);
1075 reply.addVocab32(IKINSLV_VOCAB_CMD_SET);
1076 reply.addVocab32(IKINSLV_VOCAB_CMD_ASK);
1077 reply.addVocab32(IKINSLV_VOCAB_CMD_SUSP);
1078 reply.addVocab32(IKINSLV_VOCAB_CMD_RUN);
1079 reply.addVocab32(IKINSLV_VOCAB_CMD_STATUS);
1080 reply.addVocab32(IKINSLV_VOCAB_CMD_CFG);
1081 reply.addVocab32(IKINSLV_VOCAB_CMD_HELP);
1082 reply.addVocab32(IKINSLV_VOCAB_CMD_QUIT);
1083 reply.addString("***** options");
1084 reply.addVocab32(IKINSLV_VOCAB_OPT_MODE);
1085 reply.addVocab32(IKINSLV_VOCAB_OPT_POSE);
1086 reply.addVocab32(IKINSLV_VOCAB_OPT_DOF);
1087 reply.addVocab32(IKINSLV_VOCAB_OPT_LIM);
1088 reply.addVocab32(IKINSLV_VOCAB_OPT_VERB);
1089 reply.addVocab32(IKINSLV_VOCAB_OPT_TOKEN);
1090 reply.addVocab32(IKINSLV_VOCAB_OPT_REST_POS);
1091 reply.addVocab32(IKINSLV_VOCAB_OPT_REST_WEIGHTS);
1092 reply.addVocab32(IKINSLV_VOCAB_OPT_TIP_FRAME);
1093 reply.addVocab32(IKINSLV_VOCAB_OPT_TASK2);
1094 reply.addVocab32(IKINSLV_VOCAB_OPT_XD);
1095 reply.addVocab32(IKINSLV_VOCAB_OPT_X);
1096 reply.addVocab32(IKINSLV_VOCAB_OPT_Q);
1097 reply.addString("***** values");
1098 reply.addVocab32(IKINSLV_VOCAB_VAL_POSE_FULL);
1099 reply.addVocab32(IKINSLV_VOCAB_VAL_POSE_XYZ);
1100 reply.addVocab32(IKINSLV_VOCAB_VAL_MODE_TRACK);
1101 reply.addVocab32(IKINSLV_VOCAB_VAL_MODE_SINGLE);
1102 reply.addVocab32(IKINSLV_VOCAB_VAL_ON);
1103 reply.addVocab32(IKINSLV_VOCAB_VAL_OFF);
1104 break;
1105 }
1106
1107 //-----------------
1109 {
1110 reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
1111 close();
1112 break;
1113 }
1114
1115 //-----------------
1116 default:
1117 reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
1118 }
1119 }
1120 else
1121 reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
1122}
1123
1124
1125/************************************************************************/
1126void 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/************************************************************************/
1144void 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/************************************************************************/
1175bool 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/************************************************************************/
1195bool 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 (unsigned int i=0; i<len; i++)
1203 {
1204 double val=CTRL_DEG2RAD*options->get(i).asFloat64();
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.addFloat64(CTRL_RAD2DEG*restJntPos[i]);
1219 }
1220
1221 return ret;
1222}
1223
1224
1225/************************************************************************/
1226bool 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).asInt32();
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.addFloat64(restWeights[i]);
1247 }
1248
1249 return ret;
1250}
1251
1252
1253/************************************************************************/
1254bool 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/************************************************************************/
1270bool 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").asFloat64();
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).asInt32();
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)).asInt32();
1378 setPeriod((double)period/1000.0);
1379
1381 if (options.check(Vocab32::decode(IKINSLV_VOCAB_OPT_POSE)))
1382 if (options.find(Vocab32::decode(IKINSLV_VOCAB_OPT_POSE)).asVocab32()==IKINSLV_VOCAB_VAL_POSE_XYZ)
1384
1385 bool mode=false;
1386 if (options.check(Vocab32::decode(IKINSLV_VOCAB_OPT_MODE)))
1387 if (options.find(Vocab32::decode(IKINSLV_VOCAB_OPT_MODE)).asVocab32()==IKINSLV_VOCAB_VAL_MODE_TRACK)
1388 mode=true;
1389
1390 if (options.check("verbosity"))
1391 if (options.find("verbosity").asVocab32()==IKINSLV_VOCAB_VAL_ON)
1392 verbosity=true;
1393
1394 double tol=options.check("tol",Value(CARTSLV_DEFAULT_TOL)).asFloat64();
1395 double constr_tol=options.check("constr_tol",Value(CARTSLV_DEFAULT_CONSTR_TOL)).asFloat64();
1396 int maxIter=options.check("maxIter",Value(CARTSLV_DEFAULT_MAXITER)).asInt32();
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").asVocab32()==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/************************************************************************/
1458bool 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/************************************************************************/
1515Vector 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
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 iKinLimbVersion 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=iKinLimbVersion(type.substr(underscore+2));
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<iKinLimbVersion("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/************************************************************************/
1813bool 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/************************************************************************/
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 iKinLimbVersion 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=iKinLimbVersion(type.substr(underscore+2));
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 CTRL_RAD2DEG
Definition XSensMTx.cpp:25
#define CTRL_DEG2RAD
Definition XSensMTx.cpp:26
bool read(ConnectionReader &connection)
Definition main.cpp:1467
static void addVectorOption(yarp::os::Bottle &b, const int vcb, const yarp::sig::Vector &v)
Definition iKinHlp.cpp:26
static yarp::os::Bottle * getJointsOption(const yarp::os::Bottle &b)
Retrieves the joints configuration data.
Definition iKinHlp.cpp:163
static yarp::os::Bottle * getTargetOption(const yarp::os::Bottle &b)
Retrieves commanded target data from a bottle.
Definition iKinHlp.cpp:149
static bool getTokenOption(const yarp::os::Bottle &b, double *token)
Retrieves the token from the bottle.
Definition iKinHlp.cpp:170
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
Abstract class defining the core of on-line solvers.
Definition iKinSlv.h:345
yarp::sig::Vector idx_3rdTask
Definition iKinSlv.h:395
std::deque< yarp::dev::IControlLimits * > lim
Definition iKinSlv.h:349
yarp::os::BufferedPort< yarp::os::Bottle > * outPort
Definition iKinSlv.h:360
virtual yarp::sig::Vector & encodeDOF()
Definition iKinSlv.cpp:1160
virtual PartDescriptor * getPartDesc(yarp::os::Searchable &options)=0
virtual bool handleJointsRestWeights(const yarp::os::Bottle *options, yarp::os::Bottle *reply=NULL)
Definition iKinSlv.cpp:1226
bool setLimits(int axis, double min, double max)
Definition iKinSlv.cpp:375
std::deque< int > jnt
Definition iKinSlv.h:351
virtual bool open(yarp::os::Searchable &options)
Configure the solver and start it up.
Definition iKinSlv.cpp:1270
std::condition_variable cv_dofEvent
Definition iKinSlv.h:398
yarp::sig::Vector xd_2ndTask
Definition iKinSlv.h:390
bool changeDOF(const yarp::sig::Vector &_dof)
Definition iKinSlv.cpp:1458
virtual yarp::sig::Vector solve(yarp::sig::Vector &xd)
Definition iKinSlv.cpp:1515
virtual void respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
Definition iKinSlv.cpp:515
yarp::sig::Vector w_3rdTask
Definition iKinSlv.h:394
RpcProcessor * cmdProcessor
Definition iKinSlv.h:357
virtual void interrupt()
Interrupt the open() method waiting for motor parts to be ready.
Definition iKinSlv.cpp:1525
yarp::sig::Vector restWeights
Definition iKinSlv.h:388
virtual void resume()
Resume the solver's main loop.
Definition iKinSlv.cpp:1630
CartesianSolver(const std::string &_slvName)
Constructor.
Definition iKinSlv.cpp:267
std::deque< int * > rmp
Definition iKinSlv.h:352
yarp::sig::Matrix swLimits
Definition iKinSlv.h:382
std::deque< yarp::dev::IEncoders * > enc
Definition iKinSlv.h:350
friend class RpcProcessor
Definition iKinSlv.h:439
yarp::sig::Matrix hwLimits
Definition iKinSlv.h:381
virtual void afterStart(bool)
Definition iKinSlv.cpp:1607
virtual bool decodeDOF(const yarp::sig::Vector &_dof)
Definition iKinSlv.cpp:1175
yarp::sig::Vector restJntPos
Definition iKinSlv.h:387
virtual void threadRelease()
Definition iKinSlv.cpp:1742
yarp::sig::Vector unctrlJointsOld
Definition iKinSlv.h:384
SolverCallback * clb
Definition iKinSlv.h:355
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
virtual bool handleJointsRestPosition(const yarp::os::Bottle *options, yarp::os::Bottle *reply=NULL)
Definition iKinSlv.cpp:1195
void latchUncontrolledJoints(yarp::sig::Vector &joints)
Definition iKinSlv.cpp:408
virtual void suspend()
Suspend the solver's main loop.
Definition iKinSlv.cpp:1617
yarp::sig::Vector dof
Definition iKinSlv.h:385
virtual ~CartesianSolver()
Default destructor.
Definition iKinSlv.cpp:1749
friend class SolverCallback
Definition iKinSlv.h:441
virtual bool isClosed() const
To be called to check whether the solver has received a [quit] request.
Definition iKinSlv.h:548
yarp::dev::PolyDriver * waitPart(const yarp::os::Property &partOpt)
Definition iKinSlv.cpp:301
void getFeedback(const bool wait=false)
Definition iKinSlv.cpp:425
virtual void close()
Stop the solver and dispose it.
Definition iKinSlv.cpp:1532
void fillDOFInfo(yarp::os::Bottle &reply)
Definition iKinSlv.cpp:507
yarp::sig::Vector w_2ndTask
Definition iKinSlv.h:391
virtual void prepareJointsRestTask()
Definition iKinSlv.cpp:1489
void send(const yarp::sig::Vector &xd, const yarp::sig::Vector &x, const yarp::sig::Vector &q, double *tok)
Definition iKinSlv.cpp:1126
std::deque< yarp::dev::PolyDriver * > drv
Definition iKinSlv.h:348
yarp::os::Port * rpcPort
Definition iKinSlv.h:358
yarp::sig::Vector qd_3rdTask
Definition iKinSlv.h:393
PartDescriptor * prt
Definition iKinSlv.h:347
bool isNewDOF(const yarp::sig::Vector &_dof)
Definition iKinSlv.cpp:1254
yarp::sig::Vector get_xd()
Definition iKinSlv.cpp:94
bool handleTarget(yarp::os::Bottle *b)
Definition iKinSlv.cpp:125
yarp::sig::Vector dof
Definition iKinSlv.h:291
InputPort(CartesianSolver *_slv)
Definition iKinSlv.cpp:60
CartesianSolver * slv
Definition iKinSlv.h:279
bool handlePose(const int newPose)
Definition iKinSlv.cpp:162
bool handleDOF(yarp::os::Bottle *b)
Definition iKinSlv.cpp:141
yarp::sig::Vector get_dof()
Definition iKinSlv.cpp:85
bool handleMode(const int newMode)
Definition iKinSlv.cpp:186
void set_dof(const yarp::sig::Vector &_dof)
Definition iKinSlv.cpp:77
virtual void onRead(yarp::os::Bottle &b)
Definition iKinSlv.cpp:208
yarp::sig::Vector xd
Definition iKinSlv.h:292
void reset_xd(const yarp::sig::Vector &_xd)
Definition iKinSlv.cpp:103
CartesianSolver * slv
Definition iKinSlv.h:267
CartesianSolver * slv
Definition iKinSlv.h:318
virtual void exec(const yarp::sig::Vector &xd, const yarp::sig::Vector &q)
Defines the callback body to be called at each iteration.
Definition iKinSlv.cpp:260
Class for dealing with additional iCub arm's constraints.
Definition iKinIpOpt.h:171
virtual PartDescriptor * getPartDesc(yarp::os::Searchable &options)
Definition iKinSlv.cpp:1761
virtual bool decodeDOF(const yarp::sig::Vector &_dof)
Definition iKinSlv.cpp:1831
virtual bool open(yarp::os::Searchable &options)
Configure the solver and start it up.
Definition iKinSlv.cpp:1813
A class for defining the iCub Arm.
Definition iKinFwd.h:1193
virtual PartDescriptor * getPartDesc(yarp::os::Searchable &options)
Definition iKinSlv.cpp:1857
A class for defining the iCub Leg.
Definition iKinFwd.h:1348
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:580
bool releaseLink(const unsigned int i)
Releases the ith Link.
Definition iKinFwd.cpp:463
void clear()
Removes all Links.
Definition iKinFwd.cpp:353
bool setBlockingValue(const unsigned int i, double Ang)
Changes the value of the ith blocked Link.
Definition iKinFwd.cpp:414
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
Definition iKinFwd.cpp:850
yarp::sig::Vector getAng()
Returns the current free joint angles values.
Definition iKinFwd.cpp:611
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition iKinFwd.h:549
unsigned int getDOF() const
Returns the current number of Chain's DOF.
Definition iKinFwd.h:556
bool blockLink(const unsigned int i, double Ang)
Blocks the ith Link at the a certain value of its joint angle.
Definition iKinFwd.cpp:394
Class for inverting chain's kinematics based on IpOpt lib.
Definition iKinIpOpt.h:198
void attachLIC(iKinLinIneqConstr &lic)
Attach a iKinLinIneqConstr object in order to impose constraints of the form lB <= C*q <= uB.
Definition iKinIpOpt.h:287
void setTol(const double tol)
Sets cost function tolerance.
double getConstrTol() const
Retrieves constraints tolerance.
iKinLinIneqConstr & getLIC()
Returns a reference to the attached Linear Inequality Constraints object.
Definition iKinIpOpt.h:295
std::string get_posePriority() const
Returns the Pose priority settings.
Definition iKinIpOpt.h:279
void setConstrTol(const double constr_tol)
Sets constraints tolerance.
void setUserScaling(const bool useUserScaling, const double _obj_scaling, const double _x_scaling, const double _g_scaling)
Enables/disables user scaling factors.
bool set_posePriority(const std::string &priority)
Sets the Pose priority for weighting more either position or orientation while reaching in full pose.
int getMaxIter() const
Retrieves the current value of Maximum Iteration.
double getTol() const
Retrieves cost function tolerance.
void setMaxIter(const int max_iter)
Sets Maximum Iteration.
void getBoundsInf(double &lower, double &upper)
Returns the lower and upper bounds to represent -inf and +inf.
iKinChain & get2ndTaskChain()
Retrieves the 2nd task's chain.
void set_ctrlPose(const unsigned int _ctrlPose)
Sets the state of Pose control settings.
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.
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.
A class for defining the versions of the iCub limbs.
Definition iKinFwd.h:1045
bool toLinksProperties(yarp::os::Property &options)
Provides the links attributes listed in a property object.
Definition iKinFwd.cpp:1443
double & getUpperBoundInf()
Returns a reference to the internal representation of +inf.
Definition iKinIpOpt.h:141
virtual void update(void *)
Updates internal state.
Definition iKinIpOpt.h:161
double & getLowerBoundInf()
Returns a reference to the internal representation of -inf.
Definition iKinIpOpt.h:135
cmd
Definition dataTypes.h:30
int n
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
#define IKINCTRL_POSE_FULL
Definition iKinInv.h:37
#define IKINCTRL_POSE_XYZ
Definition iKinInv.h:38
#define CARTSLV_DEFAULT_TOL
Definition iKinSlv.cpp:23
#define CARTSLV_DEFAULT_PER
Definition iKinSlv.cpp:22
#define CARTSLV_WEIGHT_3RD_TASK
Definition iKinSlv.cpp:27
#define CARTSLV_DEFAULT_MAXITER
Definition iKinSlv.cpp:25
#define CARTSLV_DEFAULT_CONSTR_TOL
Definition iKinSlv.cpp:24
#define CARTSLV_UNCTRLEDJNTS_THRES
Definition iKinSlv.cpp:28
#define CARTSLV_WEIGHT_2ND_TASK
Definition iKinSlv.cpp:26
#define IKINSLV_VOCAB_OPT_DOF
Definition iKinVocabs.h:28
#define IKINSLV_VOCAB_CMD_QUIT
Definition iKinVocabs.h:24
#define IKINSLV_VOCAB_VAL_POSE_FULL
Definition iKinVocabs.h:40
#define IKINSLV_VOCAB_CMD_STATUS
Definition iKinVocabs.h:21
#define IKINSLV_VOCAB_OPT_PRIO
Definition iKinVocabs.h:27
#define IKINSLV_VOCAB_VAL_PRIO_XYZ
Definition iKinVocabs.h:42
#define IKINSLV_VOCAB_VAL_OFF
Definition iKinVocabs.h:47
#define IKINSLV_VOCAB_CMD_SUSP
Definition iKinVocabs.h:19
#define IKINSLV_VOCAB_VAL_MODE_TRACK
Definition iKinVocabs.h:44
#define IKINSLV_VOCAB_VAL_MODE_SINGLE
Definition iKinVocabs.h:45
#define IKINSLV_VOCAB_OPT_POSE
Definition iKinVocabs.h:26
#define IKINSLV_VOCAB_OPT_Q
Definition iKinVocabs.h:32
#define IKINSLV_VOCAB_OPT_CONVERGENCE
Definition iKinVocabs.h:39
#define IKINSLV_VOCAB_OPT_MODE
Definition iKinVocabs.h:25
#define IKINSLV_VOCAB_CMD_SET
Definition iKinVocabs.h:17
#define IKINSLV_VOCAB_CMD_ASK
Definition iKinVocabs.h:18
#define IKINSLV_VOCAB_REP_ACK
Definition iKinVocabs.h:48
#define IKINSLV_VOCAB_VAL_ON
Definition iKinVocabs.h:46
#define IKINSLV_VOCAB_CMD_RUN
Definition iKinVocabs.h:20
#define IKINSLV_VOCAB_OPT_TOKEN
Definition iKinVocabs.h:33
#define IKINSLV_VOCAB_OPT_REST_POS
Definition iKinVocabs.h:35
#define IKINSLV_VOCAB_CMD_HELP
Definition iKinVocabs.h:22
#define IKINSLV_VOCAB_CMD_CFG
Definition iKinVocabs.h:23
#define IKINSLV_VOCAB_OPT_VERB
Definition iKinVocabs.h:34
#define IKINSLV_VOCAB_VAL_POSE_XYZ
Definition iKinVocabs.h:41
#define IKINSLV_VOCAB_OPT_TIP_FRAME
Definition iKinVocabs.h:37
#define IKINSLV_VOCAB_CMD_GET
Definition iKinVocabs.h:16
#define IKINSLV_VOCAB_OPT_LIM
Definition iKinVocabs.h:29
#define IKINSLV_VOCAB_OPT_TASK2
Definition iKinVocabs.h:38
#define IKINSLV_VOCAB_OPT_REST_WEIGHTS
Definition iKinVocabs.h:36
#define IKINSLV_VOCAB_REP_NACK
Definition iKinVocabs.h:49
#define IKINSLV_VOCAB_OPT_X
Definition iKinVocabs.h:31
#define IKINSLV_VOCAB_VAL_PRIO_ANG
Definition iKinVocabs.h:43
#define IKINSLV_VOCAB_OPT_XD
Definition iKinVocabs.h:30
iKinLinIneqConstr * cns
Definition iKinSlv.h:331
std::deque< bool > rvs
Definition iKinSlv.h:333
std::deque< yarp::os::Property > prp
Definition iKinSlv.h:332