iCub-main
ServerCartesianController.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Ugo Pattacini
4  * email: ugo.pattacini@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
19 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
20 // Developed by Ugo Pattacini
21 
22 #include <sstream>
23 #include <limits>
24 #include <algorithm>
25 
28 
29 #include <yarp/math/Math.h>
30 
31 #include <iCub/iKin/iKinVocabs.h>
32 
33 #define CARTCTRL_SERVER_VER 1.1
34 #define CARTCTRL_DEFAULT_PER 0.01 // [s]
35 #define CARTCTRL_DEFAULT_TASKVEL_PERFACTOR 4
36 #define CARTCTRL_DEFAULT_TOL 1e-2
37 #define CARTCTRL_DEFAULT_TRAJTIME 2.0 // [s]
38 #define CARTCTRL_DEFAULT_POSCTRL "on"
39 #define CARTCTRL_DEFAULT_MULJNTCTRL "on"
40 #define CARTCTRL_CONNECT_SOLVER_PING 1.0 // [s]
41 
42 using namespace std;
43 using namespace yarp::os;
44 using namespace yarp::dev;
45 using namespace yarp::sig;
46 using namespace yarp::math;
47 using namespace iCub::ctrl;
48 using namespace iCub::iKin;
49 
50 
51 /************************************************************************/
53 {
54  this->server=server;
55 }
56 
57 
58 /************************************************************************/
59 bool CartesianCtrlRpcProcessor::read(ConnectionReader &connection)
60 {
61  Bottle cmd, reply;
62  if (!cmd.read(connection))
63  return false;
64 
65  if (server->respond(cmd,reply))
66  if (ConnectionWriter *writer=connection.getWriter())
67  reply.write(*writer);
68 
69  return true;
70 }
71 
72 
73 /************************************************************************/
75 {
76  this->server=server;
77  useCallback();
78 }
79 
80 
81 /************************************************************************/
82 void CartesianCtrlCommandPort::onRead(Bottle &command)
83 {
84  if (command.size())
85  {
86  if ((command.get(0).asVocab32()==IKINCARTCTRL_VOCAB_CMD_GO) && (command.size()>3))
87  {
88  int pose=command.get(1).asVocab32();
89  double t=command.get(2).asFloat64();
90  Bottle *v=command.get(3).asList();
91 
93  {
94  Vector xd(3);
95  Vector od(v->size()-xd.length());
96 
97  for (size_t i=0; i<xd.length(); i++)
98  xd[i]=v->get(i).asFloat64();
99 
100  for (size_t i=0; i<od.length(); i++)
101  od[i]=v->get(xd.length()+i).asFloat64();
102 
103  server->goToPose(xd,od,t);
104  }
105  else if (pose==IKINCARTCTRL_VOCAB_VAL_POSE_XYZ)
106  {
107  Vector xd(v->size());
108 
109  for (int i=0; i<v->size(); i++)
110  xd[i]=v->get(i).asFloat64();
111 
112  server->goToPosition(xd,t);
113  }
114  }
115  else if ((command.get(0).asVocab32()==IKINCARTCTRL_VOCAB_CMD_TASKVEL) && (command.size()>1))
116  {
117  Bottle *v=command.get(1).asList();
118 
119  Vector xdot(3);
120  Vector odot(v->size()-xdot.length());
121 
122  for (size_t i=0; i<xdot.length(); i++)
123  xdot[i]=v->get(i).asFloat64();
124 
125  for (size_t i=0; i<odot.length(); i++)
126  odot[i]=v->get(xdot.length()+i).asFloat64();
127 
128  server->setTaskVelocities(xdot,odot);
129  }
130  }
131 }
132 
133 
134 /************************************************************************/
136  const Vector &x0)
137 {
138  yAssert(x0.length()>=7);
139  I=new Integrator(Ts,x0.subVector(0,2));
140  R=axis2dcm(x0.subVector(3,6));
141 }
142 
143 
144 /************************************************************************/
145 void TaskRefVelTargetGenerator::reset(const Vector &x0)
146 {
147  yAssert(x0.length()>=7);
148  I->reset(x0.subVector(0,2));
149  R=axis2dcm(x0.subVector(3,6));
150 }
151 
152 
153 /************************************************************************/
154 Vector TaskRefVelTargetGenerator::integrate(const Vector &vel)
155 {
156  yAssert(vel.length()>=7);
157  Vector w=vel.subVector(3,6);
158  w[3]*=I->getTs();
159  R=axis2dcm(w)*R;
160  return cat(I->integrate(vel.subVector(0,2)),dcm2axis(R));
161 }
162 
163 
164 /************************************************************************/
166 {
167  delete I;
168 }
169 
170 
171 /************************************************************************/
173  PeriodicThread(CARTCTRL_DEFAULT_PER)
174 {
175  init();
176 }
177 
178 
179 /************************************************************************/
181  PeriodicThread(CARTCTRL_DEFAULT_PER)
182 {
183  init();
184  open(config);
185 }
186 
187 
188 /************************************************************************/
190 {
191  // initialization
192  limbState=limbPlan=NULL;
193  chainState=chainPlan=NULL;
194  ctrl=NULL;
195 
196  portCmd =NULL;
197  rpcProcessor=NULL;
198 
199  attached =false;
200  connected =false;
201  closed =true;
202  trackingMode =false;
203  executingTraj=false;
204  taskVelModeOn=false;
205  motionDone =true;
206  useReferences=false;
207  jointsHealthy=true;
208 
209  connectCnt=0;
211  maxPartJoints=0;
214  pathPerc=0.0;
215 
216  txToken=0.0;
217  rxToken=0.0;
220  skipSlvRes=false;
221  syncEventEnabled=false;
222 
223  contextIdCnt=0;
224 }
225 
226 
227 /************************************************************************/
229 {
232  portRpc.setReader(*rpcProcessor);
233 
234  string prefixName="/";
235  prefixName=prefixName+ctrlName;
236 
237  portSlvIn.open(prefixName+"/"+slvName+"/in");
238  portSlvOut.open(prefixName+"/"+slvName+"/out");
239  portSlvRpc.open(prefixName+"/"+slvName+"/rpc");
240  portCmd->open(prefixName+"/command:i");
241  portState.open(prefixName+"/state:o");
242  portEvent.open(prefixName+"/events:o");
243  portRpc.open(prefixName+"/rpc:i");
244 
245  if (debugInfoEnabled)
246  portDebugInfo.open(prefixName+"/dbg:o");
247 }
248 
249 
250 /************************************************************************/
252 {
253  // close this first since the
254  // callback does quite a lot
255  if (portCmd!=NULL)
256  {
257  portCmd->disableCallback();
258  portCmd->interrupt();
259  portCmd->close();
260  delete portCmd;
261  }
262 
263  portSlvIn.interrupt();
264  portSlvOut.interrupt();
265  portSlvRpc.interrupt();
266  portState.interrupt();
267  portEvent.interrupt();
268  portRpc.interrupt();
269 
270  portSlvIn.close();
271  portSlvOut.close();
272  portSlvRpc.close();
273  portState.close();
274  portEvent.close();
275  portRpc.close();
276 
277  if (debugInfoEnabled)
278  {
279  portDebugInfo.interrupt();
280  portDebugInfo.close();
281  }
282 
283  delete rpcProcessor;
284  connected=false;
285 }
286 
287 
288 /************************************************************************/
289 bool ServerCartesianController::respond(const Bottle &command, Bottle &reply)
290 {
291  if (command.size())
292  switch (command.get(0).asVocab32())
293  {
294  //-----------------
296  {
297  stopControl();
298  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
299  break;
300  }
301 
302  //-----------------
304  {
305  if (command.size()>3)
306  {
307  int pose=command.get(1).asVocab32();
308  double t=command.get(2).asFloat64();
309  Bottle *v=command.get(3).asList();
310  Vector xd(v->size());
311 
312  for (int i=0; i<v->size(); i++)
313  xd[i]=v->get(i).asFloat64();
314 
315  bool ret=false;
316 
318  {
319  lock_guard<mutex> lck(mtx);
320  taskVelModeOn=false;
321  ret=goTo(IKINCTRL_POSE_FULL,xd,t,true);
322  }
323  else if (pose==IKINCARTCTRL_VOCAB_VAL_POSE_XYZ)
324  {
325  lock_guard<mutex> lck(mtx);
326  taskVelModeOn=false;
327  ret=goTo(IKINCTRL_POSE_XYZ,xd,t,true);
328  }
329 
330  if (ret)
331  {
332  // wait for the solver
333  syncEventEnabled=true;
334  unique_lock<mutex> lck(mtx_syncEvent);
335  cv_syncEvent.wait(lck);
336 
337  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
338  }
339  else
340  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
341  }
342  else
343  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
344 
345  break;
346  }
347 
348  //-----------------
350  {
351  // just behave as a relay
352  Bottle slvCommand=command;
353 
354  if (!portSlvRpc.write(slvCommand,reply))
355  {
356  yError("%s: unable to get reply from solver!",ctrlName.c_str());
357  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
358  }
359 
360  break;
361  }
362 
363  //-----------------
365  {
366  int id;
367  if (storeContext(&id))
368  {
369  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
370  reply.addInt32(id);
371  }
372  else
373  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
374 
375  break;
376  }
377 
378  //-----------------
380  {
381  if (command.size()>1)
382  {
383  int id=command.get(1).asInt32();
384  if (restoreContext(id))
385  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
386  else
387  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
388  }
389 
390  break;
391  }
392 
393  //-----------------
395  {
396  if (command.size()>1)
397  {
398  Bottle *ids=command.get(1).asList();
399  if (deleteContexts(ids))
400  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
401  else
402  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
403  }
404 
405  break;
406  }
407 
408  //-----------------
410  {
411  if (command.size()>1)
412  switch (command.get(1).asVocab32())
413  {
414  //-----------------
416  {
417  bool flag;
418  if (getTrackingMode(&flag))
419  {
420  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
421 
422  if (flag)
423  reply.addVocab32(IKINCARTCTRL_VOCAB_VAL_MODE_TRACK);
424  else
425  reply.addVocab32(IKINCARTCTRL_VOCAB_VAL_MODE_SINGLE);
426  }
427  else
428  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
429 
430  break;
431  }
432 
433  //-----------------
435  {
436  bool flag;
437  if (getReferenceMode(&flag))
438  {
439  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
440 
441  if (flag)
442  reply.addVocab32(IKINCARTCTRL_VOCAB_VAL_TRUE);
443  else
444  reply.addVocab32(IKINCARTCTRL_VOCAB_VAL_FALSE);
445  }
446  else
447  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
448 
449  break;
450  }
451 
452  //-----------------
454  {
455  string priority;
456  if (getPosePriority(priority))
457  {
458  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
459  reply.addString(priority);
460  }
461  else
462  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
463 
464  break;
465  }
466 
467  //-----------------
469  {
470  double time;
471  if (getTrajTime(&time))
472  {
473  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
474  reply.addFloat64(time);
475  }
476  else
477  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
478 
479  break;
480  }
481 
482  //-----------------
484  {
485  double tol;
486  if (getInTargetTol(&tol))
487  {
488  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
489  reply.addFloat64(tol);
490  }
491  else
492  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
493 
494  break;
495  }
496 
497  //-----------------
499  {
500  bool flag;
501  if (checkMotionDone(&flag))
502  {
503  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
504 
505  if (flag)
506  reply.addVocab32(IKINCARTCTRL_VOCAB_VAL_TRUE);
507  else
508  reply.addVocab32(IKINCARTCTRL_VOCAB_VAL_FALSE);
509  }
510  else
511  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
512 
513  break;
514  }
515 
516  //-----------------
518  {
519  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
520 
521  if (connected)
522  reply.addVocab32(IKINCARTCTRL_VOCAB_VAL_TRUE);
523  else
524  reply.addVocab32(IKINCARTCTRL_VOCAB_VAL_FALSE);
525 
526  break;
527  }
528 
529  //-----------------
531  {
532  if (command.size()>2)
533  {
534  int axis=command.get(2).asInt32();
535  double min, max;
536 
537  if (getLimits(axis,&min,&max))
538  {
539  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
540  reply.addFloat64(min);
541  reply.addFloat64(max);
542  }
543  else
544  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
545  }
546  else
547  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
548 
549  break;
550  }
551 
552  //-----------------
554  {
555  Vector curDof;
556  if (getDOF(curDof))
557  {
558  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
559  Bottle &dofPart=reply.addList();
560 
561  for (size_t i=0; i<curDof.length(); i++)
562  dofPart.addInt32((int)curDof[i]);
563  }
564  else
565  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
566 
567  break;
568  }
569 
570  //-----------------
572  {
573  Vector curRestPos;
574  if (getRestPos(curRestPos))
575  {
576  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
577  reply.addList().read(curRestPos);
578  }
579  else
580  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
581 
582  break;
583  }
584 
585  //-----------------
587  {
588  Vector curRestWeights;
589  if (getRestWeights(curRestWeights))
590  {
591  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
592  reply.addList().read(curRestWeights);
593  }
594  else
595  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
596 
597  break;
598  }
599 
600  //-----------------
602  {
603  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
604 
605  Vector q(chainState->getN());
606  int cnt=0;
607 
608  for (unsigned int i=0; i<chainState->getN(); i++)
609  {
610  if ((*chainState)[i].isBlocked())
611  q[i]=CTRL_RAD2DEG*chainState->getAng(i);
612  else
613  q[i]=CTRL_RAD2DEG*qdes[cnt++];
614  }
615 
618 
619  break;
620  }
621 
622  //-----------------
624  {
625  Vector qdot;
626  if (getJointsVelocities(qdot))
627  {
628  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
629  reply.addList().read(qdot);
630  }
631  else
632  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
633 
634  break;
635  }
636 
637  //-----------------
639  {
640  Vector xdot, odot;
641  if (getTaskVelocities(xdot,odot))
642  {
643  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
644  Bottle &xdotPart=reply.addList();
645 
646  for (size_t i=0; i<xdot.length(); i++)
647  xdotPart.addFloat64(xdot[i]);
648 
649  for (size_t i=0; i<odot.length(); i++)
650  xdotPart.addFloat64(odot[i]);
651  }
652  else
653  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
654 
655  break;
656  }
657 
658  //-----------------
660  {
661  if (command.size()>2)
662  {
663  int axis=command.get(2).asInt32();
664  Vector x,o;
665  Stamp stamp;
666 
667  if (getPose(axis,x,o,&stamp))
668  {
669  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
670  Bottle &posePart=reply.addList();
671 
672  for (size_t i=0; i<x.length(); i++)
673  posePart.addFloat64(x[i]);
674 
675  for (size_t i=0; i<o.length(); i++)
676  posePart.addFloat64(o[i]);
677 
678  Bottle &stampPart=reply.addList();
679  stampPart.addInt32(stamp.getCount());
680  stampPart.addFloat64(stamp.getTime());
681  }
682  else
683  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
684  }
685  else
686  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
687 
688  break;
689  }
690 
691  //-----------------
693  {
694  Vector x,o;
695  if (getTipFrame(x,o))
696  {
697  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
698  Bottle &tipPart=reply.addList();
699 
700  for (size_t i=0; i<x.length(); i++)
701  tipPart.addFloat64(x[i]);
702 
703  for (size_t i=0; i<o.length(); i++)
704  tipPart.addFloat64(o[i]);
705  }
706  else
707  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
708 
709  break;
710  }
711 
712  //-----------------
714  {
715  Bottle info;
716  if (getInfo(info))
717  {
718  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
719  reply.addList()=info;
720  }
721  else
722  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
723 
724  break;
725  }
726 
727  //-----------------
729  {
730  Bottle options;
731  if (tweakGet(options))
732  {
733  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
734  reply.addList()=options;
735  }
736  else
737  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
738 
739  break;
740  }
741 
742  //-----------------
743  default:
744  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
745  }
746  else
747  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
748 
749  break;
750  }
751 
752  //-----------------
754  {
755  if (command.size()>2)
756  switch (command.get(1).asVocab32())
757  {
758  //-----------------
760  {
761  int mode=command.get(2).asVocab32();
762  bool ret=false;
763 
765  ret=setTrackingMode(true);
767  ret=setTrackingMode(false);
768 
769  if (ret)
770  reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
771  else
772  reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
773 
774  break;
775  }
776 
777  //-----------------
779  {
780  int mode=command.get(2).asVocab32();
781  bool ret=false;
782 
784  ret=setReferenceMode(true);
786  ret=setReferenceMode(false);
787 
788  if (ret)
789  reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
790  else
791  reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
792 
793  break;
794  }
795 
796  //-----------------
798  {
799  string priority=command.get(2).asString();
800  if (setPosePriority(priority))
801  reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
802  else
803  reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
804 
805  break;
806  }
807 
808  //-----------------
810  {
811  if (setTrajTime(command.get(2).asFloat64()))
812  reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
813  else
814  reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
815 
816  break;
817  }
818 
819  //-----------------
821  {
822  if (setInTargetTol(command.get(2).asFloat64()))
823  reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
824  else
825  reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
826 
827  break;
828  }
829 
830  //-----------------
832  {
833  if (command.size()>4)
834  {
835  int axis=command.get(2).asInt32();
836  double min=command.get(3).asFloat64();
837  double max=command.get(4).asFloat64();
838 
839  if (setLimits(axis,min,max))
840  reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
841  else
842  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
843  }
844  else
845  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
846 
847  break;
848  }
849 
850  //-----------------
852  {
853  if (Bottle *b=command.get(2).asList())
854  {
855  Vector newDof(b->size());
856  Vector curDof;
857 
858  for (int i=0; i<b->size(); i++)
859  newDof[i]=b->get(i).asFloat64();
860 
861  if (setDOF(newDof,curDof))
862  {
863  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
864  Bottle &dofPart=reply.addList();
865 
866  for (size_t i=0; i<curDof.length(); i++)
867  dofPart.addInt32((int)curDof[i]);
868  }
869  else
870  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
871  }
872  else
873  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
874 
875  break;
876  }
877 
878  //-----------------
880  {
881  if (Bottle *b=command.get(2).asList())
882  {
883  Vector newRestPos(b->size());
884  Vector curRestPos;
885 
886  for (int i=0; i<b->size(); i++)
887  newRestPos[i]=b->get(i).asFloat64();
888 
889  if (setRestPos(newRestPos,curRestPos))
890  {
891  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
892  reply.addList().read(curRestPos);
893  }
894  else
895  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
896  }
897  else
898  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
899 
900  break;
901  }
902 
903  //-----------------
905  {
906  if (Bottle *b=command.get(2).asList())
907  {
908  Vector newRestWeights(b->size());
909  Vector curRestWeights;
910 
911  for (int i=0; i<b->size(); i++)
912  newRestWeights[i]=b->get(i).asFloat64();
913 
914  if (setRestWeights(newRestWeights,curRestWeights))
915  {
916  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
917  reply.addList().read(curRestWeights);
918  }
919  else
920  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
921  }
922  else
923  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
924 
925  break;
926  }
927 
928  //-----------------
930  {
931  if (Bottle *b=command.get(2).asList())
932  {
933  if (b->size()>=7)
934  {
935  Vector x(3);
936  Vector o(b->size()-x.length());
937 
938  for (size_t i=0; i<x.length(); i++)
939  x[i]=b->get(i).asFloat64();
940 
941  for (size_t i=0; i<o.length(); i++)
942  o[i]=b->get(i+x.length()).asFloat64();
943 
944  if (attachTipFrame(x,o))
945  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
946  else
947  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
948  }
949  else
950  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
951  }
952  else
953  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
954 
955  break;
956  }
957 
958  //-----------------
960  {
961  if (Bottle *options=command.get(2).asList())
962  {
963  if (tweakSet(*options))
964  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_ACK);
965  else
966  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
967  }
968  else
969  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
970 
971  break;
972  }
973 
974  //-----------------
975  default:
976  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
977  }
978  else
979  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
980 
981  break;
982  }
983 
984  //-----------------
986  {
987  if (command.size()>2)
988  switch (command.get(1).asVocab32())
989  {
990  //-----------------
992  {
993  int mode=command.get(2).asVocab32();
995  (command.size()>3))
996  {
997  double checkPoint=command.get(3).asFloat64();
998  if (registerMotionOngoingEvent(checkPoint))
999  {
1000  reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
1001  break;
1002  }
1003  }
1004 
1005  reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
1006  break;
1007  }
1008 
1009  //-----------------
1011  {
1012  int mode=command.get(2).asVocab32();
1014  (command.size()>3))
1015  {
1016  double checkPoint=command.get(3).asFloat64();
1017  if (unregisterMotionOngoingEvent(checkPoint))
1018  {
1019  reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
1020  break;
1021  }
1022  }
1023 
1024  reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
1025  break;
1026  }
1027 
1028  //-----------------
1030  {
1031  int mode=command.get(2).asVocab32();
1033  {
1034  reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
1035  reply.addList()=listMotionOngoingEvents();
1036  }
1037  else
1038  reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
1039 
1040  break;
1041  }
1042 
1043  //-----------------
1044  default:
1045  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
1046  }
1047  else
1048  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
1049 
1050  break;
1051  }
1052 
1053  //-----------------
1054  default:
1055  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
1056  }
1057  else
1058  reply.addVocab32(IKINCARTCTRL_VOCAB_REP_NACK);
1059 
1060  return true;
1061 }
1062 
1063 
1064 /************************************************************************/
1066 {
1067  double min, max;
1068  int cnt=0;
1069 
1070  yInfo("%s: aligning joints bounds ...",ctrlName.c_str());
1071  for (size_t i=0; i<lDsc.size(); i++)
1072  {
1073  yInfo("part #%lu: %s",(unsigned long)i,lDsc[i].key.c_str());
1074  for (int j=0; j<lJnt[i]; j++)
1075  {
1076  if (!lLim[i]->getLimits(lRmp[i][j],&min,&max))
1077  {
1078  yError("joint #%d: failed getting limits!",cnt);
1079  return false;
1080  }
1081 
1082  yInfo("joint #%d: [%g, %g] deg",cnt,min,max);
1083  (*chainState)[cnt].setMin(CTRL_DEG2RAD*min);
1084  (*chainState)[cnt].setMax(CTRL_DEG2RAD*max);
1085  (*chainPlan)[cnt].setMin(CTRL_DEG2RAD*min);
1086  (*chainPlan)[cnt].setMax(CTRL_DEG2RAD*max);
1087  cnt++;
1088  }
1089  }
1090 
1091  return true;
1092 }
1093 
1094 
1095 /************************************************************************/
1097 {
1098  Vector fbTmp(maxPartJoints);
1099  Vector stamps(maxPartJoints);
1100  int chainCnt=0;
1101  int _fbCnt=0;
1102  double timeStamp=-1.0;
1103 
1104  for (int i=0; i<numDrv; i++)
1105  {
1106  bool ok;
1107 
1108  if (useReferences)
1109  ok=lPid[i]->getPidReferences(VOCAB_PIDTYPE_POSITION,fbTmp.data());
1110  else if (encTimedEnabled)
1111  {
1112  ok=lEnt[i]->getEncodersTimed(fbTmp.data(),stamps.data());
1113  timeStamp=std::max(timeStamp,findMax(stamps.subVector(0,lJnt[i]-1)));
1114  }
1115  else
1116  ok=lEnc[i]->getEncoders(fbTmp.data());
1117 
1118  if (ok)
1119  {
1120  for (int j=0; j<lJnt[i]; j++)
1121  {
1122  double tmp=CTRL_DEG2RAD*fbTmp[lRmp[i][j]];
1123 
1124  if ((*chainState)[chainCnt].isBlocked())
1125  {
1126  chainState->setBlockingValue(chainCnt,tmp);
1127  chainPlan->setBlockingValue(chainCnt,tmp);
1128  }
1129  else
1130  _fb[_fbCnt++]=tmp;
1131 
1132  chainCnt++;
1133  }
1134  }
1135  else for (int j=0; j<lJnt[i]; j++)
1136  {
1137  if (!(*chainState)[chainCnt++].isBlocked())
1138  _fbCnt++;
1139  }
1140  }
1141 
1142  return timeStamp;
1143 }
1144 
1145 
1146 /************************************************************************/
1148 {
1149  // guard
1150  if (chainState==NULL)
1151  return;
1152 
1154 
1155  // destroy old controller
1156  delete ctrl;
1157 
1158  // update quantities
1159  fb.resize(chainState->getDOF());
1160  getFeedback(fb);
1161  chainState->setAng(fb);
1162  chainPlan->setAng(fb);
1163  velCmd.resize(chainState->getDOF(),0.0);
1165  qdes=chainState->getAng();
1166  q0=qdes;
1167 
1168  // instantiate new controller
1169  if (posDirectEnabled)
1170  ctrl=new MultiRefMinJerkCtrl(*chainPlan,ctrlPose,getPeriod());
1171  else if (plantModelProperties.check("plant_compensator",Value("off")).asString()=="on")
1172  {
1173  ctrl=new MultiRefMinJerkCtrl(*chainState,ctrlPose,getPeriod(),true);
1175  }
1176  else
1177  ctrl=new MultiRefMinJerkCtrl(*chainState,ctrlPose,getPeriod());
1178 
1179  // set tolerance
1181 
1182  // set task execution time
1184 
1185  // configure the Smith Predictor
1186  if (!posDirectEnabled)
1188 }
1189 
1190 
1191 /************************************************************************/
1193 {
1194  if (Bottle *b1=portSlvIn.read(false))
1195  {
1196  bool tokened=getTokenOption(*b1,&rxToken);
1197 
1198  // token shall be not greater than the trasmitted one
1199  if (tokened && (rxToken>txToken))
1200  {
1201  yWarning("%s: skipped message from solver due to invalid token (rx=%g)>(thr=%g)",
1202  ctrlName.c_str(),rxToken,txToken);
1203 
1204  return false;
1205  }
1206 
1207  // if we stopped the controller then we skip
1208  // any message with token smaller than the threshold
1209  if (skipSlvRes)
1210  {
1211  if (tokened && !trackingMode && (rxToken<=txTokenLatchedStopControl))
1212  {
1213  yWarning("%s: skipped message from solver since controller has been stopped (rx=%g)<=(thr=%g)",
1215 
1216  return false;
1217  }
1218  else
1219  skipSlvRes=false;
1220  }
1221 
1222  bool isNew=false;
1223  Vector _xdes, _qdes;
1224 
1225  if (b1->check(Vocab32::decode(IKINSLV_VOCAB_OPT_X)))
1226  {
1227  Bottle *b2=getEndEffectorPoseOption(*b1);
1228  int l1=(int)b2->size();
1229  int l2=7;
1230  int len=l1<l2 ? l1 : l2;
1231  _xdes.resize(len);
1232 
1233  for (int i=0; i<len; i++)
1234  _xdes[i]=b2->get(i).asFloat64();
1235 
1236  if (!(_xdes==xdes))
1237  isNew=true;
1238  }
1239 
1240  if (b1->check(Vocab32::decode(IKINSLV_VOCAB_OPT_Q)))
1241  {
1242  Bottle *b2=getJointsOption(*b1);
1243  int l1=(int)b2->size();
1244  int l2=chainState->getDOF();
1245  int len=l1<l2 ? l1 : l2;
1246  _qdes.resize(len);
1247 
1248  for (int i=0; i<len; i++)
1249  _qdes[i]=CTRL_DEG2RAD*b2->get(i).asFloat64();
1250 
1251  if (_qdes.length()!=ctrl->get_dim())
1252  {
1253  yWarning("%s: skipped message from solver since does not match the controller dimension (qdes=%d)!=(ctrl=%d)",
1254  ctrlName.c_str(),(int)_qdes.length(),ctrl->get_dim());
1255 
1256  return false;
1257  }
1258  else if (!(_qdes==qdes))
1259  isNew=true;
1260  }
1261 
1262  // update target
1263  if (isNew)
1264  {
1265  xdes=_xdes;
1266  qdes=_qdes;
1267  }
1268 
1269  // wake up rpc
1270  if (tokened && syncEventEnabled && (rxToken>=txTokenLatchedGoToRpc))
1271  {
1272  syncEventEnabled=false;
1273  cv_syncEvent.notify_all();
1274  }
1275 
1276  return isNew;
1277  }
1278  else
1279  return false;
1280 }
1281 
1282 
1283 /************************************************************************/
1285 {
1286  vector<int> modes(maxPartJoints);
1287  int chainCnt=0;
1288 
1289  jointsToSet.clear();
1290  for (int i=0; (i<numDrv) && ctrlModeAvailable; i++)
1291  {
1292  lMod[i]->getControlModes(modes.data());
1293  for (int j=0; j<lJnt[i]; j++)
1294  {
1295  if (!(*chainState)[chainCnt].isBlocked())
1296  {
1297  int mode=modes[lRmp[i][j]];
1298  if ((mode==VOCAB_CM_HW_FAULT) || (mode==VOCAB_CM_IDLE))
1299  return false;
1300  else if (posDirectEnabled)
1301  {
1302  if (mode!=VOCAB_CM_POSITION_DIRECT)
1303  jointsToSet.push_back(chainCnt);
1304  }
1305  else if (mode!=VOCAB_CM_VELOCITY)
1306  jointsToSet.push_back(chainCnt);
1307  }
1308 
1309  chainCnt++;
1310  }
1311  }
1312 
1313  return true;
1314 }
1315 
1316 
1317 /************************************************************************/
1318 void ServerCartesianController::setJointsCtrlMode(const vector<int> &jointsToSet)
1319 {
1320  if (jointsToSet.size()==0)
1321  return;
1322 
1323  int chainCnt=0;
1324  int k=0;
1325 
1326  for (int i=0; i<numDrv; i++)
1327  {
1328  vector<int> joints;
1329  vector<int> modes;
1330  for (int j=0; j<lJnt[i]; j++)
1331  {
1332  if (chainCnt==jointsToSet[k])
1333  {
1334  joints.push_back(lRmp[i][j]);
1335  modes.push_back(posDirectEnabled?VOCAB_CM_POSITION_DIRECT:
1336  VOCAB_CM_VELOCITY);
1337  k++;
1338  }
1339 
1340  chainCnt++;
1341  }
1342 
1343  if (joints.size()>0)
1344  lMod[i]->setControlModes((int)joints.size(),joints.data(),modes.data());
1345  }
1346 }
1347 
1348 
1349 /************************************************************************/
1351 {
1352  Bottle info;
1353  vector<int> joints;
1354  Vector refs;
1355  int cnt=0;
1356  int j=0;
1357  int k=0;
1358 
1359  info.addString("position");
1360  info.addString("multiple");
1361 
1362  Vector q=CTRL_RAD2DEG*ctrl->get_q();
1364  for (unsigned int i=0; i<chainState->getN(); i++)
1365  {
1366  if (!(*chainState)[i].isBlocked())
1367  {
1368  int joint=lRmp[j][k];
1369  double ref=q[cnt];
1370 
1371  joints.push_back(joint);
1372  refs.push_back(ref);
1373 
1374  ostringstream ss;
1375  ss<<lDsc[j].key<<"_"<<joint;
1376  info.addString(ss.str());
1377  info.addFloat64(ref);
1378 
1379  cnt++;
1380  }
1381 
1382  if (++k>=lJnt[j])
1383  {
1384  if (joints.size()>0)
1385  lPos[j]->setPositions((int)joints.size(),joints.data(),refs.data());
1386 
1387  joints.clear();
1388  refs.clear();
1389  j++;
1390  k=0;
1391  }
1392  }
1393 
1394  return info;
1395 }
1396 
1397 
1398 /************************************************************************/
1400 {
1401  Bottle info;
1402  vector<int> joints;
1403  Vector vels;
1404  int cnt=0;
1405  int j=0;
1406  int k=0;
1407 
1408  info.addString("velocity");
1409  info.addString("multiple");
1410 
1411  Vector v=CTRL_RAD2DEG*ctrl->get_qdot();
1412  for (unsigned int i=0; i<chainState->getN(); i++)
1413  {
1414  if (!(*chainState)[i].isBlocked())
1415  {
1416  int joint=lRmp[j][k];
1417  double vel=v[cnt];
1418  double thres=lDsc[j].minAbsVels[k];
1419 
1420  // apply bang-bang control to compensate for unachievable low velocities
1421  if ((vel!=0.0) && (fabs(vel)<thres))
1422  vel=yarp::math::sign(qdes[cnt]-fb[cnt])*thres;
1423 
1424  joints.push_back(joint);
1425  vels.push_back(vel);
1426 
1427  ostringstream ss;
1428  ss<<lDsc[j].key<<"_"<<joint;
1429  info.addString(ss.str());
1430  info.addFloat64(vel);
1431 
1432  velCmd[cnt]=vel;
1433  cnt++;
1434  }
1435 
1436  if (++k>=lJnt[j])
1437  {
1438  if (joints.size()>0)
1439  lVel[j]->velocityMove((int)joints.size(),joints.data(),vels.data());
1440 
1441  joints.clear();
1442  vels.clear();
1443  j++;
1444  k=0;
1445  }
1446  }
1447 
1448  return info;
1449 }
1450 
1451 
1452 /************************************************************************/
1454 {
1455  Bottle info;
1456  int cnt=0;
1457  int j=0;
1458  int k=0;
1459 
1460  info.addString("position");
1461  info.addString("single");
1462 
1463  Vector q=CTRL_RAD2DEG*ctrl->get_q();
1465  for (unsigned int i=0; i<chainState->getN(); i++)
1466  {
1467  if (!(*chainState)[i].isBlocked())
1468  {
1469  int joint=lRmp[j][k];
1470  double ref=q[cnt];
1471  lPos[j]->setPosition(joint,ref);
1472 
1473  ostringstream ss;
1474  ss<<lDsc[j].key<<"_"<<joint;
1475  info.addString(ss.str());
1476  info.addFloat64(ref);
1477 
1478  cnt++;
1479  }
1480 
1481  if (++k>=lJnt[j])
1482  {
1483  j++;
1484  k=0;
1485  }
1486  }
1487 
1488  return info;
1489 }
1490 
1491 
1492 /************************************************************************/
1494 {
1495  Bottle info;
1496  int cnt=0;
1497  int j=0;
1498  int k=0;
1499 
1500  info.addString("velocity");
1501  info.addString("single");
1502 
1503  Vector v=CTRL_RAD2DEG*ctrl->get_qdot();
1504  for (unsigned int i=0; i<chainState->getN(); i++)
1505  {
1506  if (!(*chainState)[i].isBlocked())
1507  {
1508  int joint=lRmp[j][k];
1509  double vel=v[cnt];
1510  double thres=lDsc[j].minAbsVels[k];
1511 
1512  // apply bang-bang control to compensate for unachievable low velocities
1513  if ((vel!=0.0) && (fabs(vel)<thres))
1514  vel=yarp::math::sign(qdes[cnt]-fb[cnt])*thres;
1515 
1516  lVel[j]->velocityMove(joint,vel);
1517 
1518  ostringstream ss;
1519  ss<<lDsc[j].key<<"_"<<joint;
1520  info.addString(ss.str());
1521  info.addFloat64(vel);
1522 
1523  velCmd[cnt]=vel;
1524  cnt++;
1525  }
1526 
1527  if (++k>=lJnt[j])
1528  {
1529  j++;
1530  k=0;
1531  }
1532  }
1533 
1534  return info;
1535 }
1536 
1537 
1538 /************************************************************************/
1539 void ServerCartesianController::stopLimb(const bool execStopPosition)
1540 {
1541  if (!posDirectEnabled || execStopPosition)
1542  {
1543  Bottle info;
1544  info.addString(posDirectEnabled?"position":"velocity");
1545  info.addString("single");
1546 
1547  int j=0; int k=0;
1548  for (unsigned int i=0; i<chainState->getN(); i++)
1549  {
1550  if (!(*chainState)[i].isBlocked())
1551  {
1552  int joint=lRmp[j][k];
1553  ostringstream ss;
1554  ss<<lDsc[j].key<<"_"<<joint;
1555  info.addString(ss.str());
1556 
1557  if (posDirectEnabled)
1558  {
1559  lStp[j]->stop(joint);
1560  info.addString("stop");
1561  }
1562  else
1563  {
1564  // vel==0.0 is always achievable
1565  lVel[j]->velocityMove(joint,0.0);
1566  info.addFloat64(0.0);
1567  }
1568  }
1569 
1570  if (++k>=lJnt[j])
1571  {
1572  j++;
1573  k=0;
1574  }
1575  }
1576 
1577  if (debugInfoEnabled && (portDebugInfo.getOutputCount()>0))
1578  {
1579  portDebugInfo.prepare()=info;
1580  debugInfo.update(txInfo.getTime());
1581  portDebugInfo.setEnvelope(debugInfo);
1582  portDebugInfo.writeStrict();
1583  }
1584  }
1585 
1586  velCmd=0.0;
1587  xdot_set=0.0;
1588 }
1589 
1590 
1591 /************************************************************************/
1593 {
1594  yInfo("Starting %s at %d ms",ctrlName.c_str(),(int)(1000.0*getPeriod()));
1595  return true;
1596 }
1597 
1598 
1599 /************************************************************************/
1601 {
1602  if (s)
1603  yInfo("%s started successfully",ctrlName.c_str());
1604  else
1605  yError("%s did not start!",ctrlName.c_str());
1606 }
1607 
1608 
1609 /************************************************************************/
1611 {
1612  if (connected)
1613  {
1614  lock_guard<mutex> lck(mtx);
1615 
1616  // read the feedback
1617  double stamp=getFeedback(fb);
1618 
1619  // update the stamp anyway
1620  if (stamp>=0.0)
1621  txInfo.update(stamp);
1622  else
1623  txInfo.update();
1624 
1625  vector<int> jointsToSet;
1627  if (!jointsHealthy)
1629 
1630  string event="none";
1631 
1632  // iff in position then update just chainState
1633  // and make the chainPlan evolve freely without
1634  // constraining it with the feedback
1635  if (posDirectEnabled)
1636  chainState->setAng(fb);
1637  else
1638  ctrl->set_q(fb);
1639 
1640  // manage the virtual target yielded by a
1641  // request for a task-space reference velocity
1643  {
1644  Vector xdot_set_int=taskRefVelTargetGen->integrate(xdot_set);
1645  goTo(IKINCTRL_POSE_FULL,xdot_set_int,0.0);
1647  }
1648 
1649  // get the current target pose
1650  if (getNewTarget())
1651  {
1652  if (jointsHealthy)
1653  {
1654  setJointsCtrlMode(jointsToSet);
1655  if (!executingTraj)
1656  {
1657  ctrl->restart(fb);
1659  }
1660 
1661  // onset of new trajectory
1662  executingTraj=true;
1663  event="motion-onset";
1664 
1666  q0=fb;
1667  }
1668  }
1669 
1670  // compute current point [%] in the path
1671  double dist=norm(qdes-q0);
1672  pathPerc=(dist>1e-6)?norm(fb-q0)/dist:1.0;
1673  pathPerc=std::min(std::max(pathPerc,0.0),1.0);
1674 
1675  if (executingTraj)
1676  {
1677  // add the contribution of the Smith Predictor block
1679 
1680  // limb control loop
1681  if (taskVelModeOn)
1683  else
1684  ctrl->iterate(xdes,qdes);
1685 
1686  // handle the end-trajectory event
1687  bool inTarget=ctrl->isInTarget();
1688  if (inTarget && posDirectEnabled)
1689  inTarget=isInTargetHelper();
1690 
1691  if (inTarget && !taskVelModeOn)
1692  {
1693  executingTraj=false;
1694  motionDone =true;
1695 
1696  stopLimb(false);
1697  event="motion-done";
1698 
1699  // switch the solver status to one shot mode
1700  // if that's the case
1701  if (!trackingMode && (rxToken==txToken))
1702  setTrackingModeHelper(false);
1703  }
1704  else
1705  {
1706  // send commands to the robot
1707  if (debugInfoEnabled && (portDebugInfo.getOutputCount()>0))
1708  {
1709  portDebugInfo.prepare()=(this->*sendCtrlCmd)();
1710  debugInfo.update(txInfo.getTime());
1711  portDebugInfo.setEnvelope(debugInfo);
1712  portDebugInfo.writeStrict();
1713  }
1714  else
1715  (this->*sendCtrlCmd)();
1716  }
1717  }
1718 
1719  // stream out the end-effector pose
1720  if (portState.getOutputCount()>0)
1721  {
1722  portState.prepare()=chainState->EndEffPose();
1723  portState.setEnvelope(txInfo);
1724  portState.write();
1725  }
1726 
1727  if (event=="motion-onset")
1728  notifyEvent(event);
1729 
1731 
1732  if (event=="motion-done")
1733  {
1735  notifyEvent(event);
1736  }
1737  }
1738  else if ((++connectCnt)*getPeriod()>CARTCTRL_CONNECT_SOLVER_PING)
1739  {
1740  if (connectToSolver())
1741  {
1742  createController();
1743 
1744  // reserve id==0 for start-up context
1745  int id0;
1746  storeContext(&id0);
1747  }
1748 
1749  connectCnt=0;
1750  }
1751 }
1752 
1753 
1754 /************************************************************************/
1756 {
1757  yInfo("Stopping %s",ctrlName.c_str());
1758 
1759  if (connected)
1760  stopLimb();
1761 
1762  notifyEvent("closing");
1763 }
1764 
1765 
1766 /************************************************************************/
1767 bool ServerCartesianController::open(Searchable &config)
1768 {
1769  yInfo("***** Configuring cartesian controller *****");
1770  closed=false;
1771 
1772  // GENERAL group
1773  Bottle &optGeneral=config.findGroup("GENERAL");
1774  if (optGeneral.isNull())
1775  {
1776  yError("GENERAL group is missing");
1777  close();
1778  return false;
1779  }
1780 
1781  yInfo("Acquiring options for group GENERAL...");
1782 
1783  // scan for required params
1784  if (optGeneral.check("SolverNameToConnect"))
1785  slvName=optGeneral.find("SolverNameToConnect").asString();
1786  else
1787  {
1788  yError("SolverNameToConnect option is missing");
1789  close();
1790  return false;
1791  }
1792 
1793  if (optGeneral.check("KinematicPart"))
1794  {
1795  kinPart=optGeneral.find("KinematicPart").asString();
1796  if ((kinPart!="arm") && (kinPart!="leg") && (kinPart!="custom"))
1797  {
1798  yError("Attempt to instantiate an unknown kinematic part");
1799  yError("Available parts are: arm, leg, custom");
1800  close();
1801  return false;
1802  }
1803  }
1804  else
1805  {
1806  yError("KinematicPart option is missing");
1807  close();
1808  return false;
1809  }
1810 
1811  string _partKinType;
1812  if (optGeneral.check("KinematicType"))
1813  {
1814  kinType=optGeneral.find("KinematicType").asString();
1815  string _kinType=kinType;
1816  _partKinType=_kinType.substr(0,_kinType.find("_"));
1817 
1818  if ((_partKinType!="left") && (_partKinType!="right"))
1819  {
1820  yError("Attempt to instantiate unknown kinematic type");
1821  yError("Available types are: left, right, left_v*, right_v*");
1822  close();
1823  return false;
1824  }
1825  }
1826  else if (kinPart!="custom")
1827  {
1828  yError("KinematicType option is missing");
1829  close();
1830  return false;
1831  }
1832 
1833  if (optGeneral.check("NumberOfDrivers"))
1834  {
1835  numDrv=optGeneral.find("NumberOfDrivers").asInt32();
1836  if (numDrv<=0)
1837  {
1838  yError("NumberOfDrivers shall be positive");
1839  close();
1840  return false;
1841  }
1842  }
1843  else
1844  {
1845  yError("NumberOfDrivers option is missing");
1846  close();
1847  return false;
1848  }
1849 
1850  // optional params
1851  if (optGeneral.check("ControllerName"))
1852  ctrlName=optGeneral.find("ControllerName").asString();
1853  else
1854  {
1855  ctrlName="cartController/"+kinPart+_partKinType;
1856  yWarning("default ControllerName assumed: %s",ctrlName.c_str());
1857  }
1858 
1859  if (optGeneral.check("ControllerPeriod"))
1860  setPeriod((double)optGeneral.find("ControllerPeriod").asInt32()/1000.0);
1861 
1862  taskRefVelPeriodFactor=optGeneral.check("TaskRefVelPeriodFactor",
1863  Value(CARTCTRL_DEFAULT_TASKVEL_PERFACTOR)).asInt32();
1864 
1865  posDirectEnabled=optGeneral.check("PositionControl",
1866  Value(CARTCTRL_DEFAULT_POSCTRL)).asString()=="on";
1867 
1868  multipleJointsControlEnabled=optGeneral.check("MultipleJointsControl",
1869  Value(CARTCTRL_DEFAULT_MULJNTCTRL)).asString()=="on";
1870 
1871  debugInfoEnabled=optGeneral.check("DebugInfo",Value("off")).asString()=="on";
1872  if (debugInfoEnabled)
1873  yDebug("Commands to robot will be also streamed out on debug port");
1874 
1875  // scan DRIVER groups
1876  for (int i=0; i<numDrv; i++)
1877  {
1878  ostringstream entry;
1879  entry<<"DRIVER_"<<i;
1880  string entry_str(entry.str());
1881 
1882  Bottle &optDrv=config.findGroup(entry_str);
1883  if (optDrv.isNull())
1884  {
1885  yError("%s group is missing",entry_str.c_str());
1886  close();
1887  return false;
1888  }
1889 
1890  yInfo("Acquiring options for group %s...",entry_str.c_str());
1891 
1892  DriverDescriptor desc;
1893 
1894  if (optDrv.check("Key"))
1895  desc.key=optDrv.find("Key").asString();
1896  else
1897  {
1898  yError("Key option is missing");
1899  close();
1900  return false;
1901  }
1902 
1903  if (optDrv.check("JointsOrder"))
1904  {
1905  string jointsOrderType=optDrv.find("JointsOrder").asString();
1906 
1907  if (jointsOrderType=="direct")
1908  desc.jointsDirectOrder=true;
1909  else if (jointsOrderType=="reversed")
1910  desc.jointsDirectOrder=false;
1911  else
1912  {
1913  yError("Attempt to select an unknown mapping order");
1914  yError("Available orders are: direct, reversed");
1915  close();
1916  return false;
1917  }
1918  }
1919  else
1920  {
1921  yError("JointsOrder option is missing");
1922  close();
1923  return false;
1924  }
1925 
1926  if (Bottle *pMinAbsVelsBottle=optDrv.find("MinAbsVels").asList())
1927  {
1928  desc.useDefaultMinAbsVel=false;
1929  desc.minAbsVels.resize(pMinAbsVelsBottle->size());
1930 
1931  for (int j=0; j<pMinAbsVelsBottle->size(); j++)
1932  {
1933  desc.minAbsVels[j]=pMinAbsVelsBottle->get(j).asFloat64();
1934 
1935  if (desc.minAbsVels[j]<0.0)
1936  desc.minAbsVels[j]=-desc.minAbsVels[j];
1937  }
1938  }
1939  else
1940  {
1941  yWarning("MinAbsVels option is missing ... using default values");
1942  desc.useDefaultMinAbsVel=true;
1943  }
1944 
1945  lDsc.push_back(desc);
1946  }
1947 
1948  // acquire options for Plant modeling
1949  Bottle &optPlantModel=config.findGroup("PLANT_MODEL");
1950  if (!optPlantModel.isNull())
1951  {
1952  yInfo("PLANT_MODEL group detected");
1953  plantModelProperties.fromString(optPlantModel.toString());
1954 
1955  // append information about the predictor's period,
1956  // that must match the controller's period
1957  plantModelProperties.unput("Ts");
1958  plantModelProperties.put("Ts",getPeriod());
1959  }
1960  else
1961  plantModelProperties.clear();
1962 
1963  // instantiate kinematic object
1964  if (kinPart=="arm")
1965  limbState=new iCubArm(kinType);
1966  else if (kinPart=="leg")
1967  limbState=new iCubLeg(kinType);
1968  else if (optGeneral.check("CustomKinFile")) // custom kinematics case
1969  {
1970  ResourceFinder rf_kin;
1971  if (optGeneral.check("CustomKinContext"))
1972  rf_kin.setDefaultContext(optGeneral.find("CustomKinContext").asString());
1973  rf_kin.configure(0,NULL);
1974  string pathToCustomKinFile=rf_kin.findFileByName(optGeneral.find("CustomKinFile").asString());
1975 
1976  Property linksOptions;
1977  linksOptions.fromConfigFile(pathToCustomKinFile);
1978 
1979  limbState=new iKinLimb(linksOptions);
1980  if (!limbState->isValid())
1981  {
1982  yError("Invalid links parameters");
1983  close();
1984  return false;
1985  }
1986  }
1987  else
1988  {
1989  yError("CustomKinFile option is missing");
1990  close();
1991  return false;
1992  }
1993 
1994  Property DHTable; limbState->toLinksProperties(DHTable);
1995  yInfo()<<"DH Table: "<<DHTable.toString(); // stream version to prevent long strings truncation
1996 
1997  // duplicate the limb for planning purpose
1998  limbPlan=new iKinLimb(*limbState);
1999 
2002 
2003  openPorts();
2004 
2005  return true;
2006 }
2007 
2008 
2009 /************************************************************************/
2011 {
2012  if (closed)
2013  return true;
2014 
2015  detachAll();
2016 
2017  delete limbState;
2018  delete limbPlan;
2019 
2020  while (eventsMap.size()>0)
2021  unregisterEvent(*eventsMap.begin()->second);
2022 
2023  closePorts();
2024 
2025  contextMap.clear();
2026 
2027  return closed=true;
2028 }
2029 
2030 
2031 /************************************************************************/
2032 bool ServerCartesianController::attachAll(const PolyDriverList &p)
2033 {
2034  drivers=p;
2035  int nd=drivers.size();
2036 
2037  yInfo("***** Attaching drivers to cartesian controller %s *****",ctrlName.c_str());
2038  yInfo("Received list of %d driver(s)",nd);
2039 
2040  if (nd!=numDrv)
2041  {
2042  yError("Expected list of %d driver(s)",numDrv);
2043  return false;
2044  }
2045 
2046  int remainingJoints=chainState->getN();
2047 
2048  ctrlModeAvailable=true;
2049  encTimedEnabled=true;
2050  pidAvailable=true;
2051  posDirectAvailable=true;
2052 
2053  for (int i=0; i<numDrv; i++)
2054  {
2055  yInfo("Acquiring info on driver %s...",lDsc[i].key.c_str());
2056 
2057  // check if what we require is present within the given list
2058  int j;
2059  for (j=0; j<drivers.size(); j++)
2060  if (lDsc[i].key==drivers[j]->key)
2061  break;
2062 
2063  if (j>=drivers.size())
2064  {
2065  yError("None of provided drivers is of type %s",lDsc[i].key.c_str());
2066  return false;
2067  }
2068 
2069  // acquire interfaces and driver's info
2070  if (drivers[j]->poly->isValid())
2071  {
2072  yInfo("driver %s successfully open",lDsc[i].key.c_str());
2073 
2074  IControlMode *mod;
2075  IEncoders *enc;
2076  IEncodersTimed *ent;
2077  IPidControl *pid;
2078  IControlLimits *lim;
2079  IVelocityControl *vel;
2080  IPositionDirect *pos;
2081  IPositionControl *stp;
2082  int joints;
2083 
2084  drivers[j]->poly->view(enc);
2085  drivers[j]->poly->view(lim);
2086 
2087  ctrlModeAvailable&=drivers[j]->poly->view(mod);
2088  drivers[j]->poly->view(vel);
2089 
2090  encTimedEnabled&=drivers[j]->poly->view(ent);
2091  pidAvailable&=drivers[j]->poly->view(pid);
2092  posDirectAvailable&=drivers[j]->poly->view(pos);
2093  posDirectAvailable&=drivers[j]->poly->view(stp);
2094 
2095  enc->getAxes(&joints);
2096 
2097  // this is for allocating vector
2098  // to read data from the interface
2099  if (joints>maxPartJoints)
2100  maxPartJoints=joints;
2101 
2102  // handle chain's bounds
2103  if (joints>remainingJoints)
2104  joints=remainingJoints;
2105 
2106  remainingJoints-=joints;
2107 
2108  int *rmpTmp=new int[joints];
2109  for (int k=0; k<joints; k++)
2110  rmpTmp[k]=lDsc[i].jointsDirectOrder ? k : (joints-k-1);
2111 
2112  // prepare references for minimum achievable absolute velocities
2113  if (lDsc[i].useDefaultMinAbsVel)
2114  lDsc[i].minAbsVels.resize(joints,0.0);
2115  else if (lDsc[i].minAbsVels.length()<(size_t)joints)
2116  {
2117  Vector tmp=lDsc[i].minAbsVels;
2118  lDsc[i].minAbsVels.resize(joints,0.0);
2119 
2120  for (size_t k=0; k<tmp.length(); k++)
2121  lDsc[i].minAbsVels[k]=tmp[k];
2122  }
2123 
2124  lMod.push_back(mod);
2125  lEnc.push_back(enc);
2126  lEnt.push_back(ent);
2127  lPid.push_back(pid);
2128  lLim.push_back(lim);
2129  lPos.push_back(pos);
2130  lStp.push_back(stp);
2131  lJnt.push_back(joints);
2132  lRmp.push_back(rmpTmp);
2133  lVel.push_back(vel);
2134  }
2135  else
2136  {
2137  yError("unable to open driver %s",lDsc[i].key.c_str());
2138  return false;
2139  }
2140  }
2141 
2142  yInfo("%s: IControlMode %s",ctrlName.c_str(),
2143  ctrlModeAvailable?"available":"not available");
2144 
2145  yInfo("%s: %s interface will be used",ctrlName.c_str(),
2146  encTimedEnabled?"IEncodersTimed":"IEncoders");
2147 
2148  yInfo("%s: IPidControl %s",ctrlName.c_str(),
2149  pidAvailable?"available":"not available");
2150 
2151  yInfo("%s: IPositionDirect %s",ctrlName.c_str(),
2152  posDirectAvailable?"available":"not available");
2153 
2155  yInfo("%s: %s interface will be used",ctrlName.c_str(),
2156  posDirectEnabled?"IPositionDirect":"IVelocityControl");
2157 
2158  yInfo("%s: %s control will be used",ctrlName.c_str(),
2159  multipleJointsControlEnabled?"multiple joints":"single joint");
2160 
2162  {
2163  if (posDirectEnabled)
2165  else
2167  }
2168  else if (posDirectEnabled)
2170  else
2172 
2173  if (!alignJointsBounds())
2174  {
2175  yError("unable to retrieve joints limits");
2176  return false;
2177  }
2178 
2179  // exclude acceleration constraints by fixing
2180  // thresholds at high values
2181  for (int i=0; i<numDrv; i++)
2182  for (int j=0; j<lJnt[i]; j++)
2183  lVel[i]->setRefAcceleration(j,std::numeric_limits<double>::max());
2184 
2185  // init task-space reference velocity
2186  xdot_set.resize(7,0.0);
2187 
2188  // this line shall be put before any
2189  // call to attached-dependent methods
2190  attached=true;
2191 
2192  connectToSolver();
2193 
2194  // create controller
2195  createController();
2196 
2197  // reserve id==0 for start-up context
2198  int id0;
2199  storeContext(&id0);
2200 
2201  // create the target generator for
2202  // task-space reference velocity
2205 
2206  start();
2207 
2208  return true;
2209 }
2210 
2211 
2212 /************************************************************************/
2214 {
2215  if (isRunning())
2216  stop();
2217 
2218  delete taskRefVelTargetGen;
2219  taskRefVelTargetGen=NULL;
2220 
2221  for (unsigned int i=0; i<lRmp.size(); i++)
2222  {
2223  delete[] lRmp[i];
2224  lRmp[i]=NULL;
2225  }
2226 
2227  lDsc.clear();
2228  lMod.clear();
2229  lEnc.clear();
2230  lEnt.clear();
2231  lPid.clear();
2232  lLim.clear();
2233  lVel.clear();
2234  lPos.clear();
2235  lStp.clear();
2236  lJnt.clear();
2237  lRmp.clear();
2238 
2239  delete ctrl;
2240  ctrl=NULL;
2241 
2242  attached=false;
2243  return true;
2244 }
2245 
2246 
2247 /************************************************************************/
2249 {
2250  string portSlvName="/";
2251  portSlvName=portSlvName+slvName+"/in";
2252 
2253  bool ok=Network::exists(portSlvName,true);
2254  yInfo("%s: Checking if cartesian solver %s is alive... %s",
2255  ctrlName.c_str(),slvName.c_str(),ok?"ready":"not yet");
2256 
2257  return ok;
2258 }
2259 
2260 
2261 /************************************************************************/
2263 {
2264  if (attached && !connected && pingSolver())
2265  {
2266  yInfo("%s: Connecting to cartesian solver %s...",ctrlName.c_str(),slvName.c_str());
2267 
2268  string portSlvName="/";
2269  portSlvName=portSlvName+slvName;
2270 
2271  bool ok=true;
2272 
2273  ok&=Network::connect(portSlvName+"/out",portSlvIn.getName(),"udp");
2274  ok&=Network::connect(portSlvOut.getName(),portSlvName+"/in","udp");
2275  ok&=Network::connect(portSlvRpc.getName(),portSlvName+"/rpc");
2276 
2277  if (ok)
2278  yInfo("%s: Connections established with %s",ctrlName.c_str(),slvName.c_str());
2279  else
2280  {
2281  yError("%s: Problems detected while connecting to %s",ctrlName.c_str(),slvName.c_str());
2282  return false;
2283  }
2284 
2285  // this line shall be put before any
2286  // call to connected-dependent methods
2287  connected=true;
2288 
2289  // keep solver and controller status aligned at start-up
2290  Bottle command, reply;
2291  command.addVocab32(IKINSLV_VOCAB_CMD_GET);
2292  command.addVocab32(IKINSLV_VOCAB_OPT_DOF);
2293 
2294  // send command to solver and wait for reply
2295  if (!portSlvRpc.write(command,reply))
2296  {
2297  yError("%s: unable to get reply from solver!",ctrlName.c_str());
2298  return false;
2299  }
2300 
2301  // update chain's links
2302  // skip the first ack/nack vocab
2303  Bottle *rxDofPart=reply.get(1).asList();
2304  for (int i=0; i<rxDofPart->size(); i++)
2305  {
2306  if (rxDofPart->get(i).asInt32()!=0)
2307  {
2308  chainState->releaseLink(i);
2309  chainPlan->releaseLink(i);
2310  }
2311  else
2312  {
2313  chainState->blockLink(i);
2314  chainPlan->blockLink(i);
2315  }
2316  }
2317 
2319  return true;
2320  }
2321  else
2322  return false;
2323 }
2324 
2325 
2326 /************************************************************************/
2327 bool ServerCartesianController::goTo(unsigned int _ctrlPose, const Vector &xd,
2328  const double t, const bool latchToken)
2329 {
2330  if (connected && (ctrl->get_dim()!=0) && jointsHealthy)
2331  {
2332  motionDone=false;
2333 
2334  ctrl->set_ctrlPose(ctrlPose=_ctrlPose);
2335 
2336  // update trajectory execution time just if required
2337  if (t>0.0)
2338  setTrajTimeHelper(t);
2339 
2340  Bottle &b=portSlvOut.prepare();
2341  b.clear();
2342 
2343  // xd part
2344  addTargetOption(b,xd);
2345  // pose part
2347  // always put solver in continuous mode
2348  // before commanding a new desired pose
2349  // in order to compensate for movements
2350  // of uncontrolled joints
2351  // correct solver status will be reinstated
2352  // accordingly at the end of trajectory
2353  addModeOption(b,true);
2354  // token part
2355  addTokenOption(b,txToken=Time::now());
2356 
2357  skipSlvRes=false;
2358  if (latchToken)
2360 
2361  portSlvOut.writeStrict();
2362  return true;
2363  }
2364  else
2365  return false;
2366 }
2367 
2368 
2369 /************************************************************************/
2371 {
2372  if (connected)
2373  {
2374  Bottle command, reply;
2375  command.addVocab32(IKINSLV_VOCAB_CMD_SET);
2376  command.addVocab32(IKINSLV_VOCAB_OPT_MODE);
2378 
2379  // send command to solver and wait for reply
2380  bool ret=false;
2381  if (!portSlvRpc.write(command,reply))
2382  yError("%s: unable to get reply from solver!",ctrlName.c_str());
2383  else if (reply.get(0).asVocab32()==IKINSLV_VOCAB_REP_ACK)
2384  {
2385  trackingMode=f;
2386  ret=true;
2387  }
2388 
2389  return ret;
2390  }
2391  else
2392  return false;
2393 }
2394 
2395 
2396 /************************************************************************/
2398 {
2399  lock_guard<mutex> lck(mtx);
2400  return setTrackingModeHelper(f);
2401 }
2402 
2403 
2404 /************************************************************************/
2406 {
2407  if (attached && (f!=NULL))
2408  {
2409  *f=trackingMode;
2410  return true;
2411  }
2412  else
2413  return false;
2414 }
2415 
2416 
2417 /************************************************************************/
2419 {
2420  if (attached && pidAvailable)
2421  {
2422  useReferences=f;
2423  return true;
2424  }
2425  else
2426  return false;
2427 }
2428 
2429 
2430 /************************************************************************/
2432 {
2433  if (attached && (f!=NULL))
2434  {
2435  *f=useReferences;
2436  return true;
2437  }
2438  else
2439  return false;
2440 }
2441 
2442 
2443 /************************************************************************/
2445 {
2446  bool ret=false;
2447  if (connected && ((p=="position") || (p=="orientation")))
2448  {
2449  lock_guard<mutex> lck(mtx);
2450 
2451  Bottle command, reply;
2452  command.addVocab32(IKINSLV_VOCAB_CMD_SET);
2453  command.addVocab32(IKINSLV_VOCAB_OPT_PRIO);
2454  command.addVocab32(p=="position"?IKINSLV_VOCAB_VAL_PRIO_XYZ:IKINSLV_VOCAB_VAL_PRIO_ANG);
2455 
2456  // send command to solver and wait for reply
2457  if (portSlvRpc.write(command,reply))
2458  ret=(reply.get(0).asVocab32()==IKINSLV_VOCAB_REP_ACK);
2459  else
2460  yError("%s: unable to get reply from solver!",ctrlName.c_str());
2461  }
2462 
2463  return ret;
2464 }
2465 
2466 
2467 /************************************************************************/
2469 {
2470  bool ret=false;
2471  if (connected)
2472  {
2473  Bottle command, reply;
2474  command.addVocab32(IKINSLV_VOCAB_CMD_GET);
2475  command.addVocab32(IKINSLV_VOCAB_OPT_PRIO);
2476 
2477  // send command to solver and wait for reply
2478  if (portSlvRpc.write(command,reply))
2479  {
2480  if (ret=(reply.get(0).asVocab32()==IKINSLV_VOCAB_REP_ACK))
2481  p=(reply.get(1).asVocab32()==IKINSLV_VOCAB_VAL_PRIO_XYZ)?
2482  "position":"orientation";
2483  }
2484  else
2485  yError("%s: unable to get reply from solver!",ctrlName.c_str());
2486  }
2487 
2488  return ret;
2489 }
2490 
2491 
2492 /************************************************************************/
2493 bool ServerCartesianController::getPose(Vector &x, Vector &o, Stamp *stamp)
2494 {
2495  if (attached)
2496  {
2497  lock_guard<mutex> lck(mtx);
2498  Vector pose=chainState->EndEffPose();
2499 
2500  x.resize(3);
2501  o.resize(pose.length()-x.length());
2502 
2503  for (size_t i=0; i<x.length(); i++)
2504  x[i]=pose[i];
2505 
2506  for (size_t i=0; i<o.length(); i++)
2507  o[i]=pose[x.length()+i];
2508 
2509  if (stamp!=NULL)
2510  *stamp=txInfo;
2511 
2512  return true;
2513  }
2514  else
2515  return false;
2516 }
2517 
2518 
2519 /************************************************************************/
2520 bool ServerCartesianController::getPose(const int axis, Vector &x,
2521  Vector &o, Stamp *stamp)
2522 {
2523  if (attached)
2524  {
2525  lock_guard<mutex> lck(mtx);
2526 
2527  bool ret=false;
2528  if (axis<(int)chainState->getN())
2529  {
2530  Matrix H=chainState->getH(axis,true);
2531 
2532  x.resize(3);
2533  for (size_t i=0; i<x.length(); i++)
2534  x[i]=H(i,3);
2535 
2536  o=dcm2axis(H);
2537 
2538  poseInfo.update(txInfo.getTime());
2539  if (stamp!=NULL)
2540  *stamp=poseInfo;
2541 
2542  ret=true;
2543  }
2544 
2545  return ret;
2546  }
2547  else
2548  return false;
2549 }
2550 
2551 
2552 /************************************************************************/
2553 bool ServerCartesianController::goToPose(const Vector &xd, const Vector &od,
2554  const double t)
2555 {
2556  if (connected)
2557  {
2558  lock_guard<mutex> lck(mtx);
2559 
2560  Vector _xd(xd.length()+od.length());
2561  for (size_t i=0; i<xd.length(); i++)
2562  _xd[i]=xd[i];
2563 
2564  for (size_t i=0; i<od.length(); i++)
2565  _xd[xd.length()+i]=od[i];
2566 
2567  taskVelModeOn=false;
2568  return goTo(IKINCTRL_POSE_FULL,_xd,t);
2569  }
2570  else
2571  return false;
2572 }
2573 
2574 
2575 /************************************************************************/
2576 bool ServerCartesianController::goToPosition(const Vector &xd, const double t)
2577 {
2578  if (connected)
2579  {
2580  lock_guard<mutex> lck(mtx);
2581 
2582  taskVelModeOn=false;
2583  return goTo(IKINCTRL_POSE_XYZ,xd,t);
2584  }
2585  else
2586  return false;
2587 }
2588 
2589 
2590 /************************************************************************/
2591 bool ServerCartesianController::goToPoseSync(const Vector &xd, const Vector &od,
2592  const double t)
2593 {
2594  return goToPose(xd,od,t);
2595 }
2596 
2597 
2598 /************************************************************************/
2599 bool ServerCartesianController::goToPositionSync(const Vector &xd, const double t)
2600 {
2601  return goToPosition(xd,t);
2602 }
2603 
2604 
2605 /************************************************************************/
2606 bool ServerCartesianController::getDesired(Vector &xdhat, Vector &odhat,
2607  Vector &qdhat)
2608 {
2609  if (connected)
2610  {
2611  lock_guard<mutex> lck(mtx);
2612 
2613  xdhat.resize(3);
2614  odhat.resize(xdes.length()-3);
2615 
2616  for (size_t i=0; i<xdhat.length(); i++)
2617  xdhat[i]=xdes[i];
2618 
2619  for (size_t i=0; i<odhat.length(); i++)
2620  odhat[i]=xdes[xdhat.length()+i];
2621 
2622  qdhat.resize(chainState->getN());
2623  int cnt=0;
2624 
2625  for (unsigned int i=0; i<chainState->getN(); i++)
2626  {
2627  if ((*chainState)[i].isBlocked())
2628  qdhat[i]=CTRL_RAD2DEG*chainState->getAng(i);
2629  else
2630  qdhat[i]=CTRL_RAD2DEG*qdes[cnt++];
2631  }
2632 
2633  return true;
2634  }
2635  else
2636  return false;
2637 }
2638 
2639 
2640 /************************************************************************/
2641 bool ServerCartesianController::askForPose(const Vector &xd, const Vector &od,
2642  Vector &xdhat, Vector &odhat,
2643  Vector &qdhat)
2644 {
2645  if (!connected)
2646  return false;
2647 
2648  lock_guard<mutex> lck(mtx);
2649 
2650  Bottle command, reply;
2651  Vector tg(xd.length()+od.length());
2652  for (size_t i=0; i<xd.length(); i++)
2653  tg[i]=xd[i];
2654 
2655  for (size_t i=0; i<od.length(); i++)
2656  tg[xd.length()+i]=od[i];
2657 
2658  command.addVocab32(IKINSLV_VOCAB_CMD_ASK);
2661 
2662  // send command and wait for reply
2663  bool ret=false;
2664  if (portSlvRpc.write(command,reply))
2665  ret=getDesiredOption(reply,xdhat,odhat,qdhat);
2666  else
2667  yError("%s: unable to get reply from solver!",ctrlName.c_str());
2668 
2669  return ret;
2670 }
2671 
2672 
2673 /************************************************************************/
2674 bool ServerCartesianController::askForPose(const Vector &q0, const Vector &xd,
2675  const Vector &od, Vector &xdhat,
2676  Vector &odhat, Vector &qdhat)
2677 {
2678  if (!connected)
2679  return false;
2680 
2681  lock_guard<mutex> lck(mtx);
2682 
2683  Bottle command, reply;
2684  Vector tg(xd.length()+od.length());
2685  for (size_t i=0; i<xd.length(); i++)
2686  tg[i]=xd[i];
2687 
2688  for (size_t i=0; i<od.length(); i++)
2689  tg[xd.length()+i]=od[i];
2690 
2691  command.addVocab32(IKINSLV_VOCAB_CMD_ASK);
2695 
2696  // send command and wait for reply
2697  bool ret=false;
2698  if (portSlvRpc.write(command,reply))
2699  ret=getDesiredOption(reply,xdhat,odhat,qdhat);
2700  else
2701  yError("%s: unable to get reply from solver!",ctrlName.c_str());
2702 
2703  return ret;
2704 }
2705 
2706 
2707 /************************************************************************/
2708 bool ServerCartesianController::askForPosition(const Vector &xd, Vector &xdhat,
2709  Vector &odhat, Vector &qdhat)
2710 {
2711  if (!connected)
2712  return false;
2713 
2714  lock_guard<mutex> lck(mtx);
2715 
2716  Bottle command, reply;
2717  command.addVocab32(IKINSLV_VOCAB_CMD_ASK);
2720 
2721  // send command and wait for reply
2722  bool ret=false;
2723  if (portSlvRpc.write(command,reply))
2724  ret=getDesiredOption(reply,xdhat,odhat,qdhat);
2725  else
2726  yError("%s: unable to get reply from solver!",ctrlName.c_str());
2727 
2728  return ret;
2729 }
2730 
2731 
2732 /************************************************************************/
2733 bool ServerCartesianController::askForPosition(const Vector &q0, const Vector &xd,
2734  Vector &xdhat, Vector &odhat,
2735  Vector &qdhat)
2736 {
2737  if (!connected)
2738  return false;
2739 
2740  lock_guard<mutex> lck(mtx);
2741 
2742  Bottle command, reply;
2743  command.addVocab32(IKINSLV_VOCAB_CMD_ASK);
2747 
2748  // send command and wait for reply
2749  bool ret=false;
2750  if (portSlvRpc.write(command,reply))
2751  ret=getDesiredOption(reply,xdhat,odhat,qdhat);
2752  else
2753  yError("%s: unable to get reply from solver!",ctrlName.c_str());
2754 
2755  return ret;
2756 }
2757 
2758 
2759 /************************************************************************/
2761 {
2762  if (connected)
2763  {
2764  lock_guard<mutex> lck(mtx);
2765 
2766  curDof.resize(chainState->getN());
2767  for (unsigned int i=0; i<chainState->getN(); i++)
2768  curDof[i]=!(*chainState)[i].isBlocked();
2769 
2770  return true;
2771  }
2772  else
2773  return false;
2774 }
2775 
2776 
2777 /************************************************************************/
2778 bool ServerCartesianController::setDOF(const Vector &newDof, Vector &curDof)
2779 {
2780  if (connected)
2781  {
2782  lock_guard<mutex> lck(mtx);
2783 
2784  Bottle command, reply;
2785  command.addVocab32(IKINSLV_VOCAB_CMD_SET);
2786  command.addVocab32(IKINSLV_VOCAB_OPT_DOF);
2787  Bottle &txDofPart=command.addList();
2788  for (size_t i=0; i<newDof.length(); i++)
2789  txDofPart.addInt32((int)newDof[i]);
2790 
2791  // send command to solver and wait for reply
2792  bool ret=false;
2793  if (portSlvRpc.write(command,reply))
2794  {
2795  // update chain's links
2796  // skip the first ack/nack vocab
2797  Bottle *rxDofPart=reply.get(1).asList();
2798  curDof.resize((size_t)rxDofPart->size());
2799  for (int i=0; i<rxDofPart->size(); i++)
2800  {
2801  curDof[i]=rxDofPart->get(i).asInt32();
2802  if (curDof[i]!=0.0)
2803  {
2804  chainState->releaseLink(i);
2805  chainPlan->releaseLink(i);
2806  }
2807  else
2808  {
2809  chainState->blockLink(i);
2810  chainPlan->blockLink(i);
2811  }
2812  }
2813 
2814  // update controller
2815  createController();
2816  ret=true;
2817  }
2818  else
2819  yError("%s: unable to get reply from solver!",ctrlName.c_str());
2820 
2821  return ret;
2822  }
2823  else
2824  return false;
2825 }
2826 
2827 
2828 /************************************************************************/
2830 {
2831  if (connected)
2832  {
2833  lock_guard<mutex> lck(mtx);
2834 
2835  Bottle command, reply;
2836  command.addVocab32(IKINSLV_VOCAB_CMD_GET);
2837  command.addVocab32(IKINSLV_VOCAB_OPT_REST_POS);
2838 
2839  // send command to solver and wait for reply
2840  bool ret=false;
2841  if (portSlvRpc.write(command,reply))
2842  {
2843  Bottle *rxRestPart=reply.get(1).asList();
2844  curRestPos.resize(rxRestPart->size());
2845  for (int i=0; i<rxRestPart->size(); i++)
2846  curRestPos[i]=rxRestPart->get(i).asFloat64();
2847 
2848  ret=true;
2849  }
2850  else
2851  yError("%s: unable to get reply from solver!",ctrlName.c_str());
2852 
2853  return ret;
2854  }
2855  else
2856  return false;
2857 }
2858 
2859 
2860 /************************************************************************/
2861 bool ServerCartesianController::setRestPos(const Vector &newRestPos,
2862  Vector &curRestPos)
2863 {
2864  if (connected)
2865  {
2866  lock_guard<mutex> lck(mtx);
2867 
2868  Bottle command, reply;
2869  command.addVocab32(IKINSLV_VOCAB_CMD_SET);
2870  command.addVocab32(IKINSLV_VOCAB_OPT_REST_POS);
2871  command.addList().read(newRestPos);
2872 
2873  // send command to solver and wait for reply
2874  bool ret=false;
2875  if (portSlvRpc.write(command,reply))
2876  {
2877  Bottle *rxRestPart=reply.get(1).asList();
2878  curRestPos.resize(rxRestPart->size());
2879  for (int i=0; i<rxRestPart->size(); i++)
2880  curRestPos[i]=rxRestPart->get(i).asFloat64();
2881 
2882  ret=true;
2883  }
2884  else
2885  yError("%s: unable to get reply from solver!",ctrlName.c_str());
2886 
2887  return ret;
2888  }
2889  else
2890  return false;
2891 }
2892 
2893 
2894 /************************************************************************/
2895 bool ServerCartesianController::getRestWeights(Vector &curRestWeights)
2896 {
2897  if (connected)
2898  {
2899  lock_guard<mutex> lck(mtx);
2900 
2901  Bottle command, reply;
2902  command.addVocab32(IKINSLV_VOCAB_CMD_GET);
2903  command.addVocab32(IKINSLV_VOCAB_OPT_REST_WEIGHTS);
2904 
2905  // send command to solver and wait for reply
2906  bool ret=false;
2907  if (portSlvRpc.write(command,reply))
2908  {
2909  Bottle *rxRestPart=reply.get(1).asList();
2910  curRestWeights.resize(rxRestPart->size());
2911  for (int i=0; i<rxRestPart->size(); i++)
2912  curRestWeights[i]=rxRestPart->get(i).asFloat64();
2913 
2914  ret=true;
2915  }
2916  else
2917  yError("%s: unable to get reply from solver!",ctrlName.c_str());
2918 
2919  return ret;
2920  }
2921  else
2922  return false;
2923 }
2924 
2925 
2926 /************************************************************************/
2927 bool ServerCartesianController::setRestWeights(const Vector &newRestWeights,
2928  Vector &curRestWeights)
2929 {
2930  if (connected)
2931  {
2932  lock_guard<mutex> lck(mtx);
2933 
2934  Bottle command, reply;
2935  command.addVocab32(IKINSLV_VOCAB_CMD_SET);
2936  command.addVocab32(IKINSLV_VOCAB_OPT_REST_WEIGHTS);
2937  command.addList().read(newRestWeights);
2938 
2939  // send command to solver and wait for reply
2940  bool ret=false;
2941  if (portSlvRpc.write(command,reply))
2942  {
2943  Bottle *rxRestPart=reply.get(1).asList();
2944  curRestWeights.resize(rxRestPart->size());
2945  for (int i=0; i<rxRestPart->size(); i++)
2946  curRestWeights[i]=rxRestPart->get(i).asFloat64();
2947 
2948  ret=true;
2949  }
2950  else
2951  yError("%s: unable to get reply from solver!",ctrlName.c_str());
2952 
2953  return ret;
2954  }
2955  else
2956  return false;
2957 }
2958 
2959 
2960 /************************************************************************/
2961 bool ServerCartesianController::getLimits(const int axis, double *min,
2962  double *max)
2963 {
2964  bool ret=false;
2965  if (connected && (min!=NULL) && (max!=NULL))
2966  {
2967  lock_guard<mutex> lck(mtx);
2968  if (axis<(int)chainState->getN())
2969  {
2970  Bottle command, reply;
2971  command.addVocab32(IKINSLV_VOCAB_CMD_GET);
2972  command.addVocab32(IKINSLV_VOCAB_OPT_LIM);
2973  command.addInt32(axis);
2974 
2975  // send command to solver and wait for reply
2976  if (!portSlvRpc.write(command,reply))
2977  yError("%s: unable to get reply from solver!",ctrlName.c_str());
2978  else if (reply.get(0).asVocab32()==IKINSLV_VOCAB_REP_ACK)
2979  {
2980  *min=reply.get(1).asFloat64();
2981  *max=reply.get(2).asFloat64();
2982  ret=true;
2983  }
2984  }
2985  }
2986 
2987  return ret;
2988 }
2989 
2990 
2991 /************************************************************************/
2992 bool ServerCartesianController::setLimits(const int axis, const double min,
2993  const double max)
2994 {
2995  bool ret=false;
2996  if (connected)
2997  {
2998  lock_guard<mutex> lck(mtx);
2999 
3000  Bottle command, reply;
3001  command.addVocab32(IKINSLV_VOCAB_CMD_SET);
3002  command.addVocab32(IKINSLV_VOCAB_OPT_LIM);
3003  command.addInt32(axis);
3004  command.addFloat64(min);
3005  command.addFloat64(max);
3006 
3007  // send command to solver and wait for reply
3008  if (portSlvRpc.write(command,reply))
3009  ret=(reply.get(0).asVocab32()==IKINSLV_VOCAB_REP_ACK);
3010  else
3011  yError("%s: unable to get reply from solver!",ctrlName.c_str());
3012  }
3013 
3014  return ret;
3015 }
3016 
3017 
3018 /************************************************************************/
3020 {
3021  if (attached && (t!=NULL))
3022  {
3023  *t=trajTime;
3024  return true;
3025  }
3026  else
3027  return false;
3028 }
3029 
3030 
3031 /************************************************************************/
3033 {
3034  if (attached)
3035  {
3036  trajTime=ctrl->set_execTime(t,true);
3037  return true;
3038  }
3039  else
3040  return false;
3041 }
3042 
3043 
3044 /************************************************************************/
3046 {
3047  lock_guard<mutex> lck(mtx);
3048  return setTrajTimeHelper(t);
3049 }
3050 
3051 
3052 /************************************************************************/
3054 {
3055  if (attached && (tol!=NULL))
3056  {
3057  *tol=targetTol;
3058  return true;
3059  }
3060  else
3061  return false;
3062 }
3063 
3064 
3065 /************************************************************************/
3067 {
3068  if (attached)
3069  {
3070  ctrl->setInTargetTol(tol);
3072  return true;
3073  }
3074  else
3075  return false;
3076 }
3077 
3078 
3079 /************************************************************************/
3081 {
3082  Matrix H=chainState->getH();
3083  Vector e(6,0.0);
3084 
3086  {
3087  e[0]=xdes[0]-H(0,3);
3088  e[1]=xdes[1]-H(1,3);
3089  e[2]=xdes[2]-H(2,3);
3090  }
3091 
3093  {
3094  Matrix Des=axis2dcm(xdes.subVector(3,6));
3095  Vector ax=dcm2axis(Des*SE3inv(H));
3096  e[3]=ax[3]*ax[0];
3097  e[4]=ax[3]*ax[1];
3098  e[5]=ax[3]*ax[2];
3099  }
3100 
3101  return (norm(e)<targetTol);
3102 }
3103 
3104 
3105 /************************************************************************/
3107 {
3108  lock_guard<mutex> lck(mtx);
3109  return setInTargetTolHelper(tol);
3110 }
3111 
3112 
3113 /************************************************************************/
3115 {
3116  if (connected)
3117  {
3118  lock_guard<mutex> lck(mtx);
3119  qdot=velCmd;
3120  return true;
3121  }
3122  else
3123  return false;
3124 }
3125 
3126 
3127 /************************************************************************/
3128 bool ServerCartesianController::getTaskVelocities(Vector &xdot, Vector &odot)
3129 {
3130  if (connected)
3131  {
3132  lock_guard<mutex> lck(mtx);
3133 
3134  Matrix J=ctrl->get_J();
3135  Vector taskVel(7,0.0);
3136 
3137  if ((J.rows()>0) && (J.cols()==velCmd.length()))
3138  {
3139  taskVel=J*(CTRL_DEG2RAD*velCmd);
3140 
3141  Vector _odot=taskVel.subVector(3,(unsigned int)taskVel.length()-1);
3142  double thetadot=norm(_odot);
3143  if (thetadot>0.0)
3144  _odot/=thetadot;
3145 
3146  taskVel[3]=_odot[0];
3147  taskVel[4]=_odot[1];
3148  taskVel[5]=_odot[2];
3149  taskVel.push_back(thetadot);
3150  }
3151 
3152  xdot.resize(3);
3153  odot.resize(taskVel.length()-xdot.length());
3154 
3155  for (size_t i=0; i<xdot.length(); i++)
3156  xdot[i]=taskVel[i];
3157 
3158  for (size_t i=0; i<odot.length(); i++)
3159  odot[i]=taskVel[xdot.length()+i];
3160 
3161  return true;
3162  }
3163  else
3164  return false;
3165 }
3166 
3167 
3168 /************************************************************************/
3170  const Vector &odot)
3171 {
3172  if (connected)
3173  {
3174  lock_guard<mutex> lck(mtx);
3175 
3176  for (int i=0; i<3; i++)
3177  xdot_set[i]=xdot[i];
3178 
3179  for (size_t i=3; i<xdot_set.length(); i++)
3180  xdot_set[i]=odot[i-3];
3181 
3182  if (norm(xdot_set)>0.0)
3183  {
3184  if (!taskVelModeOn)
3185  {
3188  }
3189 
3190  taskVelModeOn=true;
3191  }
3192  else
3194 
3195  return true;
3196  }
3197  else
3198  return false;
3199 }
3200 
3201 
3202 /************************************************************************/
3203 bool ServerCartesianController::attachTipFrame(const Vector &x, const Vector &o)
3204 {
3205  if (connected && (x.length()>=3) || (o.length()>=4))
3206  {
3207  lock_guard<mutex> lck(mtx);
3208 
3209  Bottle command, reply;
3210  command.addVocab32(IKINSLV_VOCAB_CMD_SET);
3211  command.addVocab32(IKINSLV_VOCAB_OPT_TIP_FRAME);
3212  Bottle &txTipPart=command.addList();
3213 
3214  for (int i=0; i<3; i++)
3215  txTipPart.addFloat64(x[i]);
3216 
3217  for (int i=0; i<4; i++)
3218  txTipPart.addFloat64(o[i]);
3219 
3220  // send command to solver and wait for reply
3221  bool ret=false;
3222  if (portSlvRpc.write(command,reply))
3223  {
3224  if (ret=(reply.get(0).asVocab32()==IKINSLV_VOCAB_REP_ACK))
3225  {
3226  Matrix HN=axis2dcm(o);
3227  HN(0,3)=x[0];
3228  HN(1,3)=x[1];
3229  HN(2,3)=x[2];
3230  chainState->setHN(HN);
3231  chainPlan->setHN(HN);
3232  }
3233  }
3234  else
3235  yError("%s: unable to get reply from solver!",ctrlName.c_str());
3236 
3237  return ret;
3238  }
3239  else
3240  return false;
3241 }
3242 
3243 
3244 /************************************************************************/
3246 {
3247  if (connected)
3248  {
3249  lock_guard<mutex> lck(mtx);
3250  Matrix HN=chainState->getHN();
3251 
3252  x=HN.getCol(3);
3253  x.pop_back();
3254 
3255  o=dcm2axis(HN);
3256  return true;
3257  }
3258  else
3259  return false;
3260 }
3261 
3262 
3263 /************************************************************************/
3265 {
3266  return attachTipFrame(Vector(3,0.0),Vector(4,0.0));
3267 }
3268 
3269 
3270 /************************************************************************/
3272 {
3273  if (attached && (f!=NULL))
3274  {
3275  *f=motionDone;
3276  return true;
3277  }
3278  else
3279  return false;
3280 }
3281 
3282 
3283 /************************************************************************/
3285  const double timeout)
3286 {
3287  bool done=false;
3288  double t0=Time::now();
3289 
3290  while (!done)
3291  {
3292  Time::delay(period);
3293 
3294  if (!checkMotionDone(&done) || ((timeout>0.0) && ((Time::now()-t0)>timeout)))
3295  return false;
3296  }
3297 
3298  return true;
3299 }
3300 
3301 
3302 /************************************************************************/
3304 {
3305  if (connected)
3306  {
3307  if (executingTraj)
3308  {
3309  executingTraj=false;
3310  taskVelModeOn=false;
3311  motionDone =true;
3312 
3313  stopLimb();
3314 
3316  skipSlvRes=true;
3317 
3318  notifyEvent("motion-done");
3319 
3320  // switch the solver status to one shot mode
3321  // if that's the case
3322  if (!trackingMode)
3323  setTrackingModeHelper(false);
3324  }
3325 
3326  return true;
3327  }
3328  else
3329  return false;
3330 }
3331 
3332 
3333 /************************************************************************/
3335 {
3336  lock_guard<mutex> lck(mtx);
3337  return stopControlHelper();
3338 }
3339 
3340 
3341 /************************************************************************/
3343 {
3344  if (connected && (id!=NULL))
3345  {
3346  mtx.lock();
3347  unsigned int N=chainState->getN();
3348  mtx.unlock();
3349 
3350  Vector _dof,_restPos,_restWeights;
3351  Vector _tip_x,_tip_o;
3352 
3353  getDOF(_dof);
3354  getRestPos(_restPos);
3355  getRestWeights(_restWeights);
3356  getTipFrame(_tip_x,_tip_o);
3357 
3358  Matrix _limits(N,2);
3359  for (unsigned int axis=0; axis<N; axis++)
3360  {
3361  double min,max;
3362  getLimits(axis,&min,&max);
3363  _limits(axis,0)=min;
3364  _limits(axis,1)=max;
3365  }
3366 
3367  double _trajTime,_tol;
3368  bool _mode,_useReference;
3369 
3370  getTrajTime(&_trajTime);
3371  getInTargetTol(&_tol);
3372  getTrackingMode(&_mode);
3373  getReferenceMode(&_useReference);
3374 
3375  lock_guard<mutex> lck(mtx);
3376 
3377  Context &context=contextMap[contextIdCnt];
3378  context.dof=_dof;
3379  context.restPos=_restPos;
3380  context.restWeights=_restWeights;
3381  context.limits=_limits;
3382  context.tip_x=_tip_x;
3383  context.tip_o=_tip_o;
3384  context.trajTime=_trajTime;
3385  context.tol=_tol;
3386  context.mode=_mode;
3387  context.useReferences=_useReference;
3388  context.straightness=ctrl->get_gamma();
3389  getPosePriority(context.posePriority);
3390  getTask2ndOptions(context.task_2);
3392 
3393  *id=contextIdCnt++;
3394  return true;
3395  }
3396  else
3397  return false;
3398 }
3399 
3400 
3401 /************************************************************************/
3403 {
3404  if (attached)
3405  {
3406  mtx.lock();
3407 
3408  map<int,Context>::iterator itr=contextMap.find(id);
3409  bool valid=(itr!=contextMap.end());
3410  Context context;
3411  if (valid)
3412  context=itr->second;
3413 
3414  mtx.unlock();
3415 
3416  if (valid)
3417  {
3418  setDOF(context.dof,context.dof);
3419  setRestPos(context.restPos,context.restPos);
3420  setRestWeights(context.restWeights,context.restWeights);
3421  attachTipFrame(context.tip_x,context.tip_o);
3422 
3423  for (unsigned int axis=0; axis<chainState->getN(); axis++)
3424  setLimits(axis,context.limits(axis,0),context.limits(axis,1));
3425 
3426  setTrackingMode(context.mode);
3428  setTrajTime(context.trajTime);
3429  setInTargetTol(context.tol);
3430  ctrl->set_gamma(context.straightness);
3431  setPosePriority(context.posePriority);
3432  setTask2ndOptions(context.task_2);
3434 
3435  return true;
3436  }
3437  else
3438  return false;
3439  }
3440  else
3441  return false;
3442 }
3443 
3444 
3445 /************************************************************************/
3447 {
3448  map<int,Context>::iterator itr=contextMap.find(id);
3449  if (itr!=contextMap.end())
3450  {
3451  contextMap.erase(itr);
3452  return true;
3453  }
3454  else
3455  return false;
3456 }
3457 
3458 
3459 /************************************************************************/
3461 {
3462  if (contextIdList!=NULL)
3463  {
3464  lock_guard<mutex> lck(mtx);
3465 
3466  for (int i=0; i<contextIdList->size(); i++)
3467  {
3468  int id=contextIdList->get(i).asInt32();
3469  map<int,Context>::iterator itr=contextMap.find(id);
3470  if (itr!=contextMap.end())
3471  contextMap.erase(itr);
3472  }
3473 
3474  return true;
3475  }
3476  else
3477  return false;
3478 }
3479 
3480 
3481 /************************************************************************/
3483 {
3484  if (attached)
3485  {
3486  info.clear();
3487 
3488  Bottle &serverVer=info.addList();
3489  serverVer.addString("server_version");
3490  serverVer.addFloat64(CARTCTRL_SERVER_VER);
3491 
3492  string kinPartStr(kinPart);
3493  string type=limbState->getType();
3494 
3495  size_t pos=type.find("_v");
3496  double hwVer=1.0;
3497  if (pos!=string::npos)
3498  hwVer=strtod(type.substr(pos+2).c_str(),NULL);
3499 
3500  Bottle &partVer=info.addList();
3501  partVer.addString(kinPartStr+"_version");
3502  partVer.addFloat64(hwVer);
3503 
3504  Bottle &partType=info.addList();
3505  partType.addString(kinPartStr+"_type");
3506  partType.addString(type);
3507 
3508  Bottle &events=info.addList();
3509  events.addString("events");
3510  Bottle &eventsList=events.addList();
3511  eventsList.addString("motion-onset");
3512  eventsList.addString("motion-done");
3513  eventsList.addString("motion-ongoing");
3514  eventsList.addString("closing");
3515  eventsList.addString("*");
3516 
3517  return true;
3518  }
3519  else
3520  return false;
3521 }
3522 
3523 
3524 /************************************************************************/
3526  const double checkPoint)
3527 {
3528  double time=txInfo.getTime();
3529  map<string,CartesianEvent*>::iterator itr;
3530 
3531  if (portEvent.getOutputCount()>0)
3532  {
3533  Bottle &ev=portEvent.prepare();
3534  ev.clear();
3535 
3536  ev.addString(event);
3537  ev.addFloat64(time);
3538  if (checkPoint>=0.0)
3539  ev.addFloat64(checkPoint);
3540 
3541  eventInfo.update(time);
3542  portEvent.setEnvelope(eventInfo);
3543  portEvent.writeStrict();
3544  }
3545 
3546  // rise the all-events callback
3547  itr=eventsMap.find("*");
3548  if (itr!=eventsMap.end())
3549  {
3550  if (itr->second!=NULL)
3551  {
3552  CartesianEvent &Event=*itr->second;
3553  Event.cartesianEventVariables.type=event;
3554  Event.cartesianEventVariables.time=time;
3555 
3556  if (checkPoint>=0.0)
3557  Event.cartesianEventVariables.motionOngoingCheckPoint=checkPoint;
3558 
3559  Event.cartesianEventCallback();
3560  }
3561  }
3562 
3563  string typeExtended=event;
3564  if (checkPoint>=0.0)
3565  {
3566  ostringstream ss;
3567  ss<<event<<"-"<<checkPoint;
3568  typeExtended=ss.str();
3569  }
3570 
3571  // rise the event specific callback
3572  itr=eventsMap.find(typeExtended);
3573  if (itr!=eventsMap.end())
3574  {
3575  if (itr->second!=NULL)
3576  {
3577  CartesianEvent &Event=*itr->second;
3578  Event.cartesianEventVariables.type=event;
3579  Event.cartesianEventVariables.time=time;
3580 
3581  if (checkPoint>=0.0)
3582  Event.cartesianEventVariables.motionOngoingCheckPoint=checkPoint;
3583 
3584  Event.cartesianEventCallback();
3585  }
3586  }
3587 }
3588 
3589 
3590 /************************************************************************/
3591 bool ServerCartesianController::registerEvent(CartesianEvent &event)
3592 {
3593  if (!connected)
3594  return false;
3595 
3596  string type=event.cartesianEventParameters.type;
3597  if (type=="motion-ongoing")
3598  {
3599  double checkPoint=event.cartesianEventParameters.motionOngoingCheckPoint;
3600  registerMotionOngoingEvent(checkPoint);
3601 
3602  ostringstream ss;
3603  ss<<type<<"-"<<checkPoint;
3604  type=ss.str();
3605  }
3606 
3607  eventsMap[type]=&event;
3608  return true;
3609 }
3610 
3611 
3612 /************************************************************************/
3614 {
3615  if (!connected)
3616  return false;
3617 
3618  string type=event.cartesianEventParameters.type;
3619  if (type=="motion-ongoing")
3620  {
3621  double checkPoint=event.cartesianEventParameters.motionOngoingCheckPoint;
3622  unregisterMotionOngoingEvent(checkPoint);
3623 
3624  ostringstream ss;
3625  ss<<type<<"-"<<event.cartesianEventParameters.motionOngoingCheckPoint;
3626  type=ss.str();
3627  }
3628 
3629  eventsMap.erase(type);
3630  return true;
3631 }
3632 
3633 
3634 /************************************************************************/
3636 {
3637  if (motionOngoingEventsCurrent.size()!=0)
3638  {
3639  double curCheckPoint=*motionOngoingEventsCurrent.begin();
3640  if (pathPerc>=curCheckPoint)
3641  {
3642  notifyEvent("motion-ongoing",curCheckPoint);
3643  motionOngoingEventsCurrent.erase(curCheckPoint);
3644  }
3645  }
3646 }
3647 
3648 
3649 /************************************************************************/
3651 {
3652  while (motionOngoingEventsCurrent.size()!=0)
3653  {
3654  double curCheckPoint=*motionOngoingEventsCurrent.begin();
3655  notifyEvent("motion-ongoing",curCheckPoint);
3656  motionOngoingEventsCurrent.erase(curCheckPoint);
3657  }
3658 }
3659 
3660 
3661 /************************************************************************/
3663 {
3664  if ((checkPoint>=0.0) && (checkPoint<=1.0))
3665  {
3666  lock_guard<mutex> lck(mtx);
3667  motionOngoingEvents.insert(checkPoint);
3668  return true;
3669  }
3670  else
3671  return false;
3672 }
3673 
3674 
3675 /************************************************************************/
3677 {
3678  bool ret=false;
3679  if ((checkPoint>=0.0) && (checkPoint<=1.0))
3680  {
3681  lock_guard<mutex> lck(mtx);
3682  multiset<double>::iterator itr=motionOngoingEvents.find(checkPoint);
3683  if (itr!=motionOngoingEvents.end())
3684  {
3685  motionOngoingEvents.erase(itr);
3686  ret=true;
3687  }
3688  }
3689 
3690  return ret;
3691 }
3692 
3693 
3694 /************************************************************************/
3696 {
3697  Bottle events;
3698 
3699  lock_guard<mutex> lck(mtx);
3700  for (multiset<double>::iterator itr=motionOngoingEvents.begin(); itr!=motionOngoingEvents.end(); itr++)
3701  events.addFloat64(*itr);
3702 
3703  return events;
3704 }
3705 
3706 
3707 /************************************************************************/
3709 {
3710  bool ret=false;
3711  if (connected)
3712  {
3713  Bottle command, reply;
3714  command.addVocab32(IKINSLV_VOCAB_CMD_GET);
3715  command.addVocab32(IKINSLV_VOCAB_OPT_TASK2);
3716 
3717  // send command to solver and wait for reply
3718  if (portSlvRpc.write(command,reply))
3719  {
3720  if (ret=(reply.get(0).asVocab32()==IKINSLV_VOCAB_REP_ACK))
3721  v=reply.get(1);
3722  }
3723  else
3724  yError("%s: unable to get reply from solver!",ctrlName.c_str());
3725  }
3726 
3727  return ret;
3728 }
3729 
3730 
3731 /************************************************************************/
3733 {
3734  bool ret=false;
3735  if (connected)
3736  {
3737  lock_guard<mutex> lck(mtx);
3738 
3739  Bottle command, reply;
3740  command.addVocab32(IKINSLV_VOCAB_CMD_SET);
3741  command.addVocab32(IKINSLV_VOCAB_OPT_TASK2);
3742  command.add(v);
3743 
3744  // send command to solver and wait for reply
3745  if (portSlvRpc.write(command,reply))
3746  ret=(reply.get(0).asVocab32()==IKINSLV_VOCAB_REP_ACK);
3747  else
3748  yError("%s: unable to get reply from solver!",ctrlName.c_str());
3749  }
3750 
3751  return ret;
3752 }
3753 
3754 
3755 /************************************************************************/
3757 {
3758  bool ret=false;
3759  if (connected)
3760  {
3761  Bottle command, reply;
3762  command.addVocab32(IKINSLV_VOCAB_CMD_GET);
3763  command.addVocab32(IKINSLV_VOCAB_OPT_CONVERGENCE);
3764 
3765  // send command to solver and wait for reply
3766  if (portSlvRpc.write(command,reply))
3767  {
3768  if (ret=(reply.get(0).asVocab32()==IKINSLV_VOCAB_REP_ACK))
3769  options=*reply.get(1).asList();
3770  }
3771  else
3772  yError("%s: unable to get reply from solver!",ctrlName.c_str());
3773  }
3774 
3775  return ret;
3776 }
3777 
3778 
3779 /************************************************************************/
3781 {
3782  bool ret=false;
3783  if (connected)
3784  {
3785  lock_guard<mutex> lck(mtx);
3786 
3787  Bottle command, reply;
3788  command.addVocab32(IKINSLV_VOCAB_CMD_SET);
3789  command.addVocab32(IKINSLV_VOCAB_OPT_CONVERGENCE);
3790  command.addList()=options;
3791 
3792  // send command to solver and wait for reply
3793  if (portSlvRpc.write(command,reply))
3794  ret=(reply.get(0).asVocab32()==IKINSLV_VOCAB_REP_ACK);
3795  else
3796  yError("%s: unable to get reply from solver!",ctrlName.c_str());
3797  }
3798 
3799  return ret;
3800 }
3801 
3802 
3803 /************************************************************************/
3804 bool ServerCartesianController::tweakSet(const Bottle &options)
3805 {
3806  bool ret=true;
3807 
3808  // straightness
3809  if (options.check("straightness"))
3810  ctrl->set_gamma(options.find("straightness").asFloat64());
3811 
3812  // secondary task
3813  if (options.check("task_2"))
3814  ret&=setTask2ndOptions(options.find("task_2"));
3815 
3816  // solver convergence options
3817  if (options.check("tol") || options.check("constr_tol") ||
3818  options.check("max_iter"))
3819  ret&=setSolverConvergenceOptions(options);
3820 
3821  return ret;
3822 }
3823 
3824 
3825 /************************************************************************/
3827 {
3828  if (attached)
3829  {
3830  lock_guard<mutex> lck(mtx);
3831  options.clear();
3832 
3833  bool ret=true;
3834 
3835  // straightness
3836  Bottle &straightness=options.addList();
3837  straightness.addString("straightness");
3838  straightness.addFloat64(ctrl->get_gamma());
3839 
3840  // secondary task
3841  Value v;
3842  ret&=getTask2ndOptions(v);
3843  if (ret)
3844  {
3845  Bottle &task2=options.addList();
3846  task2.addString("task_2");
3847  task2.add(v);
3848  }
3849 
3850  // solver convergence options
3851  Bottle b;
3853  if (ret)
3854  options.append(b);
3855 
3856  return ret;
3857  }
3858  else
3859  return false;
3860 }
3861 
3862 
3863 /************************************************************************/
3865 {
3866  close();
3867 }
3868 
3869 
3870 
ServerCartesianController::getDesired
bool getDesired(yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
Definition: ServerCartesianController.cpp:2606
ServerCartesianController::encTimedEnabled
bool encTimedEnabled
Definition: ServerCartesianController.h:121
ServerCartesianController::portCmd
CartesianCtrlCommandPort * portCmd
Definition: ServerCartesianController.h:196
ServerCartesianController::attachTipFrame
bool attachTipFrame(const yarp::sig::Vector &x, const yarp::sig::Vector &o)
Definition: ServerCartesianController.cpp:3203
ServerCartesianController::limbState
iCub::iKin::iKinLimb * limbState
Definition: ServerCartesianController.h:136
ServerCartesianController::pathPerc
double pathPerc
Definition: ServerCartesianController.h:161
iCub::iKin::CartesianHelper::getEndEffectorPoseOption
static yarp::os::Bottle * getEndEffectorPoseOption(const yarp::os::Bottle &b)
Retrieves the end-effector pose data.
Definition: iKinHlp.cpp:156
ServerCartesianController::threadRelease
void threadRelease()
Definition: ServerCartesianController.cpp:1755
ServerCartesianController::sendCtrlCmdSingleJointVelocity
yarp::os::Bottle sendCtrlCmdSingleJointVelocity()
Definition: ServerCartesianController.cpp:1493
python-motor-control.tmp
tmp
Definition: python-motor-control.py:43
ServerCartesianController::rpcProcessor
CartesianCtrlRpcProcessor * rpcProcessor
Definition: ServerCartesianController.h:197
IKINCARTCTRL_VOCAB_OPT_MODE
#define IKINCARTCTRL_VOCAB_OPT_MODE
Definition: CommonCartesianController.h:34
IKINCARTCTRL_VOCAB_OPT_POSE
#define IKINCARTCTRL_VOCAB_OPT_POSE
Definition: CommonCartesianController.h:52
IKINCARTCTRL_VOCAB_OPT_INFO
#define IKINCARTCTRL_VOCAB_OPT_INFO
Definition: CommonCartesianController.h:53
IKINCTRL_POSE_XYZ
#define IKINCTRL_POSE_XYZ
Definition: iKinInv.h:38
ServerCartesianController::Context::solverConvergence
yarp::os::Bottle solverConvergence
Definition: ServerCartesianController.h:214
IKINCARTCTRL_VOCAB_OPT_TIME
#define IKINCARTCTRL_VOCAB_OPT_TIME
Definition: CommonCartesianController.h:37
ServerCartesianController::jointsHealthy
bool jointsHealthy
Definition: ServerCartesianController.h:127
IKINCARTCTRL_VOCAB_OPT_TWEAK
#define IKINCARTCTRL_VOCAB_OPT_TWEAK
Definition: CommonCartesianController.h:54
iCub::ctrl::Integrator
Definition: pids.h:47
ServerCartesianController::multipleJointsControlEnabled
bool multipleJointsControlEnabled
Definition: ServerCartesianController.h:124
IKINCARTCTRL_VOCAB_VAL_EVENT_ONGOING
#define IKINCARTCTRL_VOCAB_VAL_EVENT_ONGOING
Definition: CommonCartesianController.h:64
ServerCartesianController::getNewTarget
bool getNewTarget()
Definition: ServerCartesianController.cpp:1192
H
H
Definition: compute_ekf_fast.m:27
ServerCartesianController::run
void run()
Definition: ServerCartesianController.cpp:1610
iCub::iKin::iKinCtrl::get_J
virtual yarp::sig::Matrix get_J() const
Returns the actual Jacobian used in computation.
Definition: iKinInv.h:387
ServerCartesianController::debugInfo
yarp::os::Stamp debugInfo
Definition: ServerCartesianController.h:178
ServerCartesianController::drivers
yarp::dev::PolyDriverList drivers
Definition: ServerCartesianController.h:111
ServerCartesianController::removeTipFrame
bool removeTipFrame()
Definition: ServerCartesianController.cpp:3264
IKINSLV_VOCAB_REP_NACK
#define IKINSLV_VOCAB_REP_NACK
Definition: iKinVocabs.h:49
IKINCARTCTRL_VOCAB_CMD_EVENT
#define IKINCARTCTRL_VOCAB_CMD_EVENT
Definition: CommonCartesianController.h:33
ServerCartesianController::setJointsCtrlMode
void setJointsCtrlMode(const std::vector< int > &jointsToSet)
Definition: ServerCartesianController.cpp:1318
IKINCARTCTRL_VOCAB_VAL_TRUE
#define IKINCARTCTRL_VOCAB_VAL_TRUE
Definition: CommonCartesianController.h:62
IKINSLV_VOCAB_OPT_TIP_FRAME
#define IKINSLV_VOCAB_OPT_TIP_FRAME
Definition: iKinVocabs.h:37
IKINCARTCTRL_VOCAB_OPT_ISSOLVERON
#define IKINCARTCTRL_VOCAB_OPT_ISSOLVERON
Definition: CommonCartesianController.h:51
CARTCTRL_DEFAULT_MULJNTCTRL
#define CARTCTRL_DEFAULT_MULJNTCTRL
Definition: ServerCartesianController.cpp:39
iCub::action::log::info
@ info
Definition: actionPrimitives.cpp:64
IKINCARTCTRL_VOCAB_OPT_REGISTER
#define IKINCARTCTRL_VOCAB_OPT_REGISTER
Definition: CommonCartesianController.h:55
IKINCARTCTRL_VOCAB_OPT_UNREGISTER
#define IKINCARTCTRL_VOCAB_OPT_UNREGISTER
Definition: CommonCartesianController.h:56
IKINCARTCTRL_VOCAB_OPT_REST_POS
#define IKINCARTCTRL_VOCAB_OPT_REST_POS
Definition: CommonCartesianController.h:40
iCub::iKin::MultiRefMinJerkCtrl::set_execTime
double set_execTime(const double _execTime, const bool warn=false)
Sets the task execution time in seconds.
Definition: iKinInv.cpp:942
ServerCartesianController::xdot_set
yarp::sig::Vector xdot_set
Definition: ServerCartesianController.h:182
ServerCartesianController::goTo
bool goTo(unsigned int _ctrlPose, const yarp::sig::Vector &xd, const double t, const bool latchToken=false)
Definition: ServerCartesianController.cpp:2327
ServerCartesianController::sendCtrlCmdSingleJointPosition
yarp::os::Bottle sendCtrlCmdSingleJointPosition()
Definition: ServerCartesianController.cpp:1453
IKINCARTCTRL_VOCAB_CMD_DELETE
#define IKINCARTCTRL_VOCAB_CMD_DELETE
Definition: CommonCartesianController.h:29
ServerCartesianController::getReferenceMode
bool getReferenceMode(bool *f)
Definition: ServerCartesianController.cpp:2431
ServerCartesianController::poseInfo
yarp::os::Stamp poseInfo
Definition: ServerCartesianController.h:176
ServerCartesianController::registerMotionOngoingEvent
bool registerMotionOngoingEvent(const double checkPoint)
Definition: ServerCartesianController.cpp:3662
ServerCartesianController::goToPose
bool goToPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0)
Definition: ServerCartesianController.cpp:2553
ServerCartesianController::Context::trajTime
double trajTime
Definition: ServerCartesianController.h:207
ServerCartesianController::unregisterEvent
bool unregisterEvent(yarp::dev::CartesianEvent &event)
Definition: ServerCartesianController.cpp:3613
ServerCartesianController::motionOngoingEventsCurrent
std::multiset< double > motionOngoingEventsCurrent
Definition: ServerCartesianController.h:222
ServerCartesianController::attached
bool attached
Definition: ServerCartesianController.h:113
iCub::iKin::MultiRefMinJerkCtrl::iterate
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd, yarp::sig::Vector *xdot_set, const unsigned int verbose)
ServerCartesianController::goToPosition
bool goToPosition(const yarp::sig::Vector &xd, const double t=0.0)
Definition: ServerCartesianController.cpp:2576
ServerCartesianController::taskRefVelPeriodFactor
int taskRefVelPeriodFactor
Definition: ServerCartesianController.h:162
iCub::iKin::iKinCtrl::get_x
virtual yarp::sig::Vector get_x() const
Returns the actual cartesian position of the End-Effector.
Definition: iKinInv.h:357
ServerCartesianController::setTaskVelocities
bool setTaskVelocities(const yarp::sig::Vector &xdot, const yarp::sig::Vector &odot)
Definition: ServerCartesianController.cpp:3169
pos
const dReal * pos
Definition: iCub_Sim.cpp:62
CARTCTRL_CONNECT_SOLVER_PING
#define CARTCTRL_CONNECT_SOLVER_PING
Definition: ServerCartesianController.cpp:40
e
e
Definition: compute_ekf_fast.m:13
IKINSLV_VOCAB_VAL_PRIO_XYZ
#define IKINSLV_VOCAB_VAL_PRIO_XYZ
Definition: iKinVocabs.h:42
ServerCartesianController::setReferenceMode
bool setReferenceMode(const bool f)
Definition: ServerCartesianController.cpp:2418
ServerCartesianController::trajTime
double trajTime
Definition: ServerCartesianController.h:160
ServerCartesianController::sendCtrlCmd
yarp::os::Bottle(ServerCartesianController::* sendCtrlCmd)()
Definition: ServerCartesianController.h:228
IKINCARTCTRL_VOCAB_CMD_RESTORE
#define IKINCARTCTRL_VOCAB_CMD_RESTORE
Definition: CommonCartesianController.h:28
ServerCartesianController::lJnt
std::deque< int > lJnt
Definition: ServerCartesianController.h:153
ServerCartesianController::lStp
std::deque< yarp::dev::IPositionControl * > lStp
Definition: ServerCartesianController.h:152
IKINCTRL_POSE_ANG
#define IKINCTRL_POSE_ANG
Definition: iKinInv.h:39
ServerCartesianController::getInTargetTol
bool getInTargetTol(double *tol)
Definition: ServerCartesianController.cpp:3053
IKINCARTCTRL_VOCAB_VAL_FALSE
#define IKINCARTCTRL_VOCAB_VAL_FALSE
Definition: CommonCartesianController.h:63
IKINCARTCTRL_VOCAB_CMD_GO
#define IKINCARTCTRL_VOCAB_CMD_GO
Definition: CommonCartesianController.h:30
ServerCartesianController::plantModelProperties
yarp::os::Property plantModelProperties
Definition: ServerCartesianController.h:141
cmd
cmd
Definition: dataTypes.h:30
CARTCTRL_SERVER_VER
#define CARTCTRL_SERVER_VER
Definition: ServerCartesianController.cpp:33
ServerCartesianController::stopLimb
void stopLimb(const bool execStopPosition=true)
Definition: ServerCartesianController.cpp:1539
IKINCARTCTRL_VOCAB_CMD_SET
#define IKINCARTCTRL_VOCAB_CMD_SET
Definition: CommonCartesianController.h:25
ServerCartesianController::eventInfo
yarp::os::Stamp eventInfo
Definition: ServerCartesianController.h:177
ServerCartesianController::Context::restPos
yarp::sig::Vector restPos
Definition: ServerCartesianController.h:202
CTRL_DEG2RAD
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:26
ServerCartesianController::kinPart
std::string kinPart
Definition: ServerCartesianController.h:132
ServerCartesianController::useReferences
bool useReferences
Definition: ServerCartesianController.h:126
ServerCartesianController::askForPosition
bool askForPosition(const yarp::sig::Vector &xd, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
iCub::iKin::iKinChain::releaseLink
bool releaseLink(const unsigned int i)
Releases the ith Link.
Definition: iKinFwd.cpp:471
CARTCTRL_DEFAULT_PER
#define CARTCTRL_DEFAULT_PER
Definition: ServerCartesianController.cpp:34
ServerCartesianController::open
bool open(yarp::os::Searchable &config)
Definition: ServerCartesianController.cpp:1767
ServerCartesianController::lDsc
std::deque< DriverDescriptor > lDsc
Definition: ServerCartesianController.h:144
IKINCARTCTRL_VOCAB_OPT_XDOT
#define IKINCARTCTRL_VOCAB_OPT_XDOT
Definition: CommonCartesianController.h:47
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
ServerCartesianController::pingSolver
bool pingSolver()
Definition: ServerCartesianController.cpp:2248
ServerCartesianController::slvName
std::string slvName
Definition: ServerCartesianController.h:131
CartesianCtrlCommandPort::onRead
void onRead(yarp::os::Bottle &command)
Definition: ServerCartesianController.cpp:82
ServerCartesianController::init
void init()
Definition: ServerCartesianController.cpp:189
ServerCartesianController::Context::restWeights
yarp::sig::Vector restWeights
Definition: ServerCartesianController.h:203
IKINSLV_VOCAB_CMD_ASK
#define IKINSLV_VOCAB_CMD_ASK
Definition: iKinVocabs.h:18
IKINCARTCTRL_VOCAB_VAL_MODE_TRACK
#define IKINCARTCTRL_VOCAB_VAL_MODE_TRACK
Definition: CommonCartesianController.h:60
ServerCartesianController::taskVelModeOn
bool taskVelModeOn
Definition: ServerCartesianController.h:118
TaskRefVelTargetGenerator
Definition: ServerCartesianController.h:77
iCub::iKin::iKinChain::getH
yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false)
Returns the rigid roto-translation matrix from the root reference frame to the ith frame in Denavit-H...
Definition: iKinFwd.cpp:740
ServerCartesianController::connected
bool connected
Definition: ServerCartesianController.h:114
ServerCartesianController::connectToSolver
bool connectToSolver()
Definition: ServerCartesianController.cpp:2262
iCub::iKin::iKinChain::getDOF
unsigned int getDOF() const
Returns the current number of Chain's DOF.
Definition: iKinFwd.h:557
ServerCartesianController::tweakSet
bool tweakSet(const yarp::os::Bottle &options)
Definition: ServerCartesianController.cpp:3804
iCub::iKin::CartesianHelper::getJointsOption
static yarp::os::Bottle * getJointsOption(const yarp::os::Bottle &b)
Retrieves the joints configuration data.
Definition: iKinHlp.cpp:163
stop
static int stop
Definition: iCub_Sim.cpp:41
ServerCartesianController::fb
yarp::sig::Vector fb
Definition: ServerCartesianController.h:184
ServerCartesianController::stopControlHelper
bool stopControlHelper()
Definition: ServerCartesianController.cpp:3303
ServerCartesianController::getJointsVelocities
bool getJointsVelocities(yarp::sig::Vector &qdot)
Definition: ServerCartesianController.cpp:3114
yarp::dev
Definition: DebugInterfaces.h:52
ServerCartesianController::pidAvailable
bool pidAvailable
Definition: ServerCartesianController.h:125
iCub::iKin::iKinChain::setAng
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
iCub::iKin::iKinCtrl::setInTargetTol
virtual void setInTargetTol(double tol_x)
Sets tolerance for in-target check (5e-3 by default).
Definition: iKinInv.h:278
ServerCartesianController::setDOF
bool setDOF(const yarp::sig::Vector &newDof, yarp::sig::Vector &curDof)
Definition: ServerCartesianController.cpp:2778
ServerCartesianController::checkMotionDone
bool checkMotionDone(bool *f)
Definition: ServerCartesianController.cpp:3271
ServerCartesianController::Context::posePriority
std::string posePriority
Definition: ServerCartesianController.h:212
ServerCartesianController::syncEventEnabled
bool syncEventEnabled
Definition: ServerCartesianController.h:170
p
p
Definition: show_eyes_axes.m:23
iCub::iKin::iCubArm
Definition: iKinFwd.h:1081
ServerCartesianController::contextMap
std::map< int, Context > contextMap
Definition: ServerCartesianController.h:218
ServerCartesianController::stopControl
bool stopControl()
Definition: ServerCartesianController.cpp:3334
ServerCartesianController::txInfo
yarp::os::Stamp txInfo
Definition: ServerCartesianController.h:175
ServerCartesianController::ctrlName
std::string ctrlName
Definition: ServerCartesianController.h:130
DriverDescriptor
Definition: ServerCartesianController.h:45
ServerCartesianController::motionOngoingEvents
std::multiset< double > motionOngoingEvents
Definition: ServerCartesianController.h:221
ServerCartesianController::numDrv
int numDrv
Definition: ServerCartesianController.h:134
IKINCTRL_POSE_FULL
#define IKINCTRL_POSE_FULL
Definition: iKinInv.h:37
ServerCartesianController::Context::useReferences
bool useReferences
Definition: ServerCartesianController.h:210
ServerCartesianController::areJointsHealthyAndSet
bool areJointsHealthyAndSet(std::vector< int > &jointsToSet)
Definition: ServerCartesianController.cpp:1284
ServerCartesianController::sendCtrlCmdMultipleJointsPosition
yarp::os::Bottle sendCtrlCmdMultipleJointsPosition()
Definition: ServerCartesianController.cpp:1350
ServerCartesianController::setTrajTimeHelper
bool setTrajTimeHelper(const double t)
Definition: ServerCartesianController.cpp:3032
ServerCartesianController::getDOF
bool getDOF(yarp::sig::Vector &curDof)
Definition: ServerCartesianController.cpp:2760
IKINCARTCTRL_VOCAB_OPT_LIST
#define IKINCARTCTRL_VOCAB_OPT_LIST
Definition: CommonCartesianController.h:57
ServerCartesianController::Context::mode
bool mode
Definition: ServerCartesianController.h:209
IKINSLV_VOCAB_VAL_PRIO_ANG
#define IKINSLV_VOCAB_VAL_PRIO_ANG
Definition: iKinVocabs.h:43
ServerCartesianController::restoreContext
bool restoreContext(const int id)
Definition: ServerCartesianController.cpp:3402
ServerCartesianController::taskRefVelPeriodCnt
int taskRefVelPeriodCnt
Definition: ServerCartesianController.h:163
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
IKINCARTCTRL_VOCAB_OPT_TOL
#define IKINCARTCTRL_VOCAB_OPT_TOL
Definition: CommonCartesianController.h:38
iCub::iKin::iKinLimb
Definition: iKinFwd.h:873
ServerCartesianController::Context::tip_x
yarp::sig::Vector tip_x
Definition: ServerCartesianController.h:204
ServerCartesianController::targetTol
double targetTol
Definition: ServerCartesianController.h:159
ServerCartesianController::chainPlan
iCub::iKin::iKinChain * chainPlan
Definition: ServerCartesianController.h:137
iCub::iKin::iKinChain::getAng
yarp::sig::Vector getAng()
Returns the current free joint angles values.
Definition: iKinFwd.cpp:619
ServerCartesianController::lPos
std::deque< yarp::dev::IPositionDirect * > lPos
Definition: ServerCartesianController.h:151
iCub::iKin::iKinLimb::toLinksProperties
bool toLinksProperties(yarp::os::Property &options)
Provides the links attributes listed in a property object.
Definition: iKinFwd.cpp:1451
CTRL_RAD2DEG
#define CTRL_RAD2DEG
Definition: XSensMTx.cpp:25
ServerCartesianController::motionOngoingEventsFlush
void motionOngoingEventsFlush()
Definition: ServerCartesianController.cpp:3650
ServerCartesianController::lVel
std::deque< yarp::dev::IVelocityControl * > lVel
Definition: ServerCartesianController.h:150
ServerCartesianController::mtx
std::mutex mtx
Definition: ServerCartesianController.h:172
ServerCartesianController::lEnt
std::deque< yarp::dev::IEncodersTimed * > lEnt
Definition: ServerCartesianController.h:147
iCub::iKin::iKinCtrl::isInTarget
virtual bool isInTarget()
Checks if the End-Effector is in target.
Definition: iKinInv.h:315
iCub::iKin::iKinChain::setHN
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:588
TaskRefVelTargetGenerator::reset
void reset(const yarp::sig::Vector &x0)
Definition: ServerCartesianController.cpp:145
iCub::iKin::CartesianHelper::getDesiredOption
static bool getDesiredOption(const yarp::os::Bottle &reply, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
Definition: iKinHlp.cpp:38
ServerCartesianController::lLim
std::deque< yarp::dev::IControlLimits * > lLim
Definition: ServerCartesianController.h:149
ServerCartesianController::Context::dof
yarp::sig::Vector dof
Definition: ServerCartesianController.h:201
IKINCARTCTRL_VOCAB_CMD_STORE
#define IKINCARTCTRL_VOCAB_CMD_STORE
Definition: CommonCartesianController.h:27
ServerCartesianController::attachAll
bool attachAll(const yarp::dev::PolyDriverList &p)
Definition: ServerCartesianController.cpp:2032
IKINCARTCTRL_VOCAB_OPT_X
#define IKINCARTCTRL_VOCAB_OPT_X
Definition: CommonCartesianController.h:45
ServerCartesianController::kinType
std::string kinType
Definition: ServerCartesianController.h:133
iCub::iKin
Definition: iKinFwd.h:71
iCub::iKin::MultiRefMinJerkCtrl
Definition: iKinInv.h:759
ServerCartesianController::closed
bool closed
Definition: ServerCartesianController.h:115
IKINCARTCTRL_VOCAB_OPT_LIM
#define IKINCARTCTRL_VOCAB_OPT_LIM
Definition: CommonCartesianController.h:43
ServerCartesianController::velCmd
yarp::sig::Vector velCmd
Definition: ServerCartesianController.h:183
ServerCartesianController::portRpc
yarp::os::RpcServer portRpc
Definition: ServerCartesianController.h:194
ServerCartesianController::afterStart
void afterStart(bool s)
Definition: ServerCartesianController.cpp:1600
ServerCartesianController
servercartesiancontroller : implements the server part of the Cartesian Interface.
Definition: ServerCartesianController.h:104
ServerCartesianController::xdes
yarp::sig::Vector xdes
Definition: ServerCartesianController.h:180
ServerCartesianController::getLimits
bool getLimits(const int axis, double *min, double *max)
Definition: ServerCartesianController.cpp:2961
ServerCartesianController::ServerCartesianController
ServerCartesianController()
Definition: ServerCartesianController.cpp:172
ServerCartesianController::txToken
double txToken
Definition: ServerCartesianController.h:165
IKINCARTCTRL_VOCAB_VAL_MODE_SINGLE
#define IKINCARTCTRL_VOCAB_VAL_MODE_SINGLE
Definition: CommonCartesianController.h:61
mode
GLenum mode
Definition: rendering.cpp:48
ServerCartesianController::ctrlModeAvailable
bool ctrlModeAvailable
Definition: ServerCartesianController.h:120
ServerCartesianController::portEvent
yarp::os::BufferedPort< yarp::os::Bottle > portEvent
Definition: ServerCartesianController.h:192
ServerCartesianController::getTrajTime
bool getTrajTime(double *t)
Definition: ServerCartesianController.cpp:3019
iCub::iKin::MultiRefMinJerkCtrl::get_gamma
double get_gamma() const
Returns the parameter gamma which is used to blend the contribute of the task controller versus the c...
Definition: iKinInv.h:901
ServerCartesianController::smithPredictor
SmithPredictor smithPredictor
Definition: ServerCartesianController.h:142
ServerCartesianController::respond
bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
Definition: ServerCartesianController.cpp:289
ServerCartesianController::setTrajTime
bool setTrajTime(const double t)
Definition: ServerCartesianController.cpp:3045
iCub::iKin::CartesianHelper::addVectorOption
static void addVectorOption(yarp::os::Bottle &b, const int vcb, const yarp::sig::Vector &v)
Definition: iKinHlp.cpp:26
IKINSLV_VOCAB_OPT_REST_WEIGHTS
#define IKINSLV_VOCAB_OPT_REST_WEIGHTS
Definition: iKinVocabs.h:36
ServerCartesianController::lRmp
std::deque< int * > lRmp
Definition: ServerCartesianController.h:154
ServerCartesianController::setRestPos
bool setRestPos(const yarp::sig::Vector &newRestPos, yarp::sig::Vector &curRestPos)
Definition: ServerCartesianController.cpp:2861
ServerCartesianController::executingTraj
bool executingTraj
Definition: ServerCartesianController.h:117
ServerCartesianController::q0
yarp::sig::Vector q0
Definition: ServerCartesianController.h:185
ServerCartesianController::Context
Definition: ServerCartesianController.h:199
ServerCartesianController::~ServerCartesianController
virtual ~ServerCartesianController()
Definition: ServerCartesianController.cpp:3864
IKINCARTCTRL_VOCAB_REP_NACK
#define IKINCARTCTRL_VOCAB_REP_NACK
Definition: CommonCartesianController.h:66
ServerCartesianController::lEnc
std::deque< yarp::dev::IEncoders * > lEnc
Definition: ServerCartesianController.h:146
iCub::iKin::iKinChain::getN
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
CartesianCtrlRpcProcessor::CartesianCtrlRpcProcessor
CartesianCtrlRpcProcessor(ServerCartesianController *server)
Definition: ServerCartesianController.cpp:52
IKINSLV_VOCAB_OPT_REST_POS
#define IKINSLV_VOCAB_OPT_REST_POS
Definition: iKinVocabs.h:35
DriverDescriptor::minAbsVels
yarp::sig::Vector minAbsVels
Definition: ServerCartesianController.h:50
IKINCARTCTRL_VOCAB_OPT_PRIO
#define IKINCARTCTRL_VOCAB_OPT_PRIO
Definition: CommonCartesianController.h:36
CARTCTRL_DEFAULT_TRAJTIME
#define CARTCTRL_DEFAULT_TRAJTIME
Definition: ServerCartesianController.cpp:37
ServerCartesianController::deleteContext
bool deleteContext(const int id)
Definition: ServerCartesianController.cpp:3446
CARTCTRL_DEFAULT_TOL
#define CARTCTRL_DEFAULT_TOL
Definition: ServerCartesianController.cpp:36
ServerCartesianController::setTrackingMode
bool setTrackingMode(const bool f)
Definition: ServerCartesianController.cpp:2397
ServerCartesianController::skipSlvRes
bool skipSlvRes
Definition: ServerCartesianController.h:169
iCub::ctrl::norm
double norm(const yarp::sig::Matrix &M, int col)
IKINSLV_VOCAB_OPT_XD
#define IKINSLV_VOCAB_OPT_XD
Definition: iKinVocabs.h:30
ServerCartesianController::Context::tol
double tol
Definition: ServerCartesianController.h:208
x
x
Definition: compute_ekf_sym.m:21
ServerCartesianController::portState
yarp::os::BufferedPort< yarp::sig::Vector > portState
Definition: ServerCartesianController.h:191
state::ok
@ ok
IKINSLV_VOCAB_VAL_MODE_TRACK
#define IKINSLV_VOCAB_VAL_MODE_TRACK
Definition: iKinVocabs.h:44
IKINSLV_VOCAB_OPT_X
#define IKINSLV_VOCAB_OPT_X
Definition: iKinVocabs.h:31
ServerCartesianController::trackingMode
bool trackingMode
Definition: ServerCartesianController.h:116
IKINCARTCTRL_VOCAB_OPT_REST_WEIGHTS
#define IKINCARTCTRL_VOCAB_OPT_REST_WEIGHTS
Definition: CommonCartesianController.h:41
ServerCartesianController::setInTargetTolHelper
bool setInTargetTolHelper(const double tol)
Definition: ServerCartesianController.cpp:3066
ServerCartesianController::lPid
std::deque< yarp::dev::IPidControl * > lPid
Definition: ServerCartesianController.h:148
ServerCartesianController::getRestPos
bool getRestPos(yarp::sig::Vector &curRestPos)
Definition: ServerCartesianController.cpp:2829
ServerCartesianController::unregisterMotionOngoingEvent
bool unregisterMotionOngoingEvent(const double checkPoint)
Definition: ServerCartesianController.cpp:3676
ServerCartesianController::getRestWeights
bool getRestWeights(yarp::sig::Vector &curRestWeights)
Definition: ServerCartesianController.cpp:2895
iCub::iKin::iKinChain::getHN
yarp::sig::Matrix getHN() const
Returns HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
Definition: iKinFwd.h:579
IKINCARTCTRL_VOCAB_VAL_POSE_XYZ
#define IKINCARTCTRL_VOCAB_VAL_POSE_XYZ
Definition: CommonCartesianController.h:59
IKINSLV_VOCAB_OPT_DOF
#define IKINSLV_VOCAB_OPT_DOF
Definition: iKinVocabs.h:28
TaskRefVelTargetGenerator::~TaskRefVelTargetGenerator
virtual ~TaskRefVelTargetGenerator()
Definition: ServerCartesianController.cpp:165
ServerCartesianController::portSlvIn
yarp::os::BufferedPort< yarp::os::Bottle > portSlvIn
Definition: ServerCartesianController.h:187
ServerCartesianController::getPose
bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
iCub::iKin::iKinCtrl::get_dim
unsigned int get_dim() const
Returns the number of Chain DOF.
Definition: iKinInv.h:345
IKINCARTCTRL_VOCAB_VAL_POSE_FULL
#define IKINCARTCTRL_VOCAB_VAL_POSE_FULL
Definition: CommonCartesianController.h:58
IKINSLV_VOCAB_CMD_SET
#define IKINSLV_VOCAB_CMD_SET
Definition: iKinVocabs.h:17
IKINSLV_VOCAB_CMD_GET
#define IKINSLV_VOCAB_CMD_GET
Definition: iKinVocabs.h:16
ServerCartesianController::ctrl
iCub::iKin::MultiRefMinJerkCtrl * ctrl
Definition: ServerCartesianController.h:138
ServerCartesianController::Context::tip_o
yarp::sig::Vector tip_o
Definition: ServerCartesianController.h:205
ServerCartesianController::connectCnt
unsigned int connectCnt
Definition: ServerCartesianController.h:156
ServerCartesianController::contextIdCnt
int contextIdCnt
Definition: ServerCartesianController.h:217
IKINSLV_VOCAB_OPT_TASK2
#define IKINSLV_VOCAB_OPT_TASK2
Definition: iKinVocabs.h:38
ServerCartesianController::setRestWeights
bool setRestWeights(const yarp::sig::Vector &newRestWeights, yarp::sig::Vector &curRestWeights)
Definition: ServerCartesianController.cpp:2927
ServerCartesianController::askForPose
bool askForPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
CartesianCtrlCommandPort::CartesianCtrlCommandPort
CartesianCtrlCommandPort(ServerCartesianController *server)
Definition: ServerCartesianController.cpp:74
ServerCartesianController::registerEvent
bool registerEvent(yarp::dev::CartesianEvent &event)
Definition: ServerCartesianController.cpp:3591
IKINCARTCTRL_VOCAB_OPT_Q
#define IKINCARTCTRL_VOCAB_OPT_Q
Definition: CommonCartesianController.h:46
f
f
Definition: compute_ekf_sym.m:22
ServerCartesianController::listMotionOngoingEvents
yarp::os::Bottle listMotionOngoingEvents()
Definition: ServerCartesianController.cpp:3695
ServerCartesianController::setSolverConvergenceOptions
bool setSolverConvergenceOptions(const yarp::os::Bottle &options)
Definition: ServerCartesianController.cpp:3780
ServerCartesianController::portDebugInfo
yarp::os::BufferedPort< yarp::os::Bottle > portDebugInfo
Definition: ServerCartesianController.h:193
SmithPredictor::restart
void restart(const yarp::sig::Vector &y0)
Definition: SmithPredictor.cpp:166
SmithPredictor::configure
void configure(const yarp::os::Property &options, iCub::iKin::iKinChain &chain)
Definition: SmithPredictor.cpp:69
DriverDescriptor::jointsDirectOrder
bool jointsDirectOrder
Definition: ServerCartesianController.h:48
ServerCartesianController::threadInit
bool threadInit()
Definition: ServerCartesianController.cpp:1592
ServerCartesianController::getTask2ndOptions
bool getTask2ndOptions(yarp::os::Value &v)
Definition: ServerCartesianController.cpp:3708
ServerCartesianController::getFeedback
double getFeedback(yarp::sig::Vector &_fb)
Definition: ServerCartesianController.cpp:1096
ServerCartesianController::setLimits
bool setLimits(const int axis, const double min, const double max)
Definition: ServerCartesianController.cpp:2992
iCub::iKin::iKinCtrl::set_ctrlPose
void set_ctrlPose(unsigned int _ctrlPose)
Sets the state of Pose control settings.
Definition: iKinInv.cpp:64
SmithPredictor::computeCmd
yarp::sig::Vector computeCmd(const yarp::sig::Vector &u)
Definition: SmithPredictor.cpp:193
iCub::iKin::iKinChain::setBlockingValue
bool setBlockingValue(const unsigned int i, double Ang)
Changes the value of the ith blocked Link.
Definition: iKinFwd.cpp:422
ServerCartesianController::setPosePriority
bool setPosePriority(const std::string &p)
Definition: ServerCartesianController.cpp:2444
ServerCartesianController::goToPoseSync
bool goToPoseSync(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0)
Definition: ServerCartesianController.cpp:2591
ServerCartesianController::portSlvOut
yarp::os::BufferedPort< yarp::os::Bottle > portSlvOut
Definition: ServerCartesianController.h:188
ServerCartesianController::motionOngoingEventsHandling
void motionOngoingEventsHandling()
Definition: ServerCartesianController.cpp:3635
IKINSLV_VOCAB_OPT_MODE
#define IKINSLV_VOCAB_OPT_MODE
Definition: iKinVocabs.h:25
done
bool done
Definition: main.cpp:42
ServerCartesianController::ctrlPose
unsigned int ctrlPose
Definition: ServerCartesianController.h:157
IKINCARTCTRL_VOCAB_OPT_MOTIONDONE
#define IKINCARTCTRL_VOCAB_OPT_MOTIONDONE
Definition: CommonCartesianController.h:50
IKINSLV_VOCAB_OPT_LIM
#define IKINSLV_VOCAB_OPT_LIM
Definition: iKinVocabs.h:29
IKINSLV_VOCAB_VAL_MODE_SINGLE
#define IKINSLV_VOCAB_VAL_MODE_SINGLE
Definition: iKinVocabs.h:45
ServerCartesianController::getSolverConvergenceOptions
bool getSolverConvergenceOptions(yarp::os::Bottle &options)
Definition: ServerCartesianController.cpp:3756
CARTCTRL_DEFAULT_POSCTRL
#define CARTCTRL_DEFAULT_POSCTRL
Definition: ServerCartesianController.cpp:38
CARTCTRL_DEFAULT_TASKVEL_PERFACTOR
#define CARTCTRL_DEFAULT_TASKVEL_PERFACTOR
Definition: ServerCartesianController.cpp:35
ServerCartesianController::Context::task_2
yarp::os::Value task_2
Definition: ServerCartesianController.h:213
CartesianCtrlRpcProcessor::read
bool read(yarp::os::ConnectionReader &connection)
Definition: ServerCartesianController.cpp:59
IKINSLV_VOCAB_REP_ACK
#define IKINSLV_VOCAB_REP_ACK
Definition: iKinVocabs.h:48
IKINSLV_VOCAB_OPT_CONVERGENCE
#define IKINSLV_VOCAB_OPT_CONVERGENCE
Definition: iKinVocabs.h:39
CommonCartesianController.h
ServerCartesianController::eventsMap
std::map< std::string, yarp::dev::CartesianEvent * > eventsMap
Definition: ServerCartesianController.h:219
v
static int v
Definition: iCub_Sim.cpp:42
ServerCartesianController::Context::straightness
double straightness
Definition: ServerCartesianController.h:211
ServerCartesianController::waitMotionDone
bool waitMotionDone(const double period=0.1, const double timeout=0.0)
Definition: ServerCartesianController.cpp:3284
ServerCartesianController::maxPartJoints
int maxPartJoints
Definition: ServerCartesianController.h:158
IKINCARTCTRL_VOCAB_REP_ACK
#define IKINCARTCTRL_VOCAB_REP_ACK
Definition: CommonCartesianController.h:65
ServerCartesianController::CartesianCtrlRpcProcessor
friend class CartesianCtrlRpcProcessor
Definition: ServerCartesianController.h:255
iCub::iKin::CartesianHelper::addPoseOption
static void addPoseOption(yarp::os::Bottle &b, const unsigned int pose)
Appends to a bottle all data needed to change the pose mode.
Definition: iKinHlp.cpp:114
IKINSLV_VOCAB_OPT_Q
#define IKINSLV_VOCAB_OPT_Q
Definition: iKinVocabs.h:32
ServerCartesianController::close
bool close()
Definition: ServerCartesianController.cpp:2010
ServerCartesianController::Context::limits
yarp::sig::Matrix limits
Definition: ServerCartesianController.h:206
ServerCartesianController::isInTargetHelper
bool isInTargetHelper()
Definition: ServerCartesianController.cpp:3080
ServerCartesianController::motionDone
bool motionDone
Definition: ServerCartesianController.h:119
iCub::iKin::iKinChain::EndEffPose
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
Definition: iKinFwd.cpp:858
ServerCartesianController::createController
void createController()
Definition: ServerCartesianController.cpp:1147
iCub::iKin::CartesianHelper::addTokenOption
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
iCub::iKin::MultiRefMinJerkCtrl::get_qdot
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
Definition: iKinInv.h:913
ServerCartesianController::posDirectEnabled
bool posDirectEnabled
Definition: ServerCartesianController.h:122
ServerCartesianController::txTokenLatchedGoToRpc
double txTokenLatchedGoToRpc
Definition: ServerCartesianController.h:168
iCub::iKin::CartesianHelper::getTokenOption
static bool getTokenOption(const yarp::os::Bottle &b, double *token)
Retrieves the token from the bottle.
Definition: iKinHlp.cpp:170
iCub::iKin::MultiRefMinJerkCtrl::setPlantParameters
void setPlantParameters(const yarp::os::Property &parameters, const std::string &entryTag="dimension")
Allows user to assign values to the parameters of plant under control (for the configuration space on...
Definition: iKinInv.cpp:965
ServerCartesianController::CartesianCtrlCommandPort
friend class CartesianCtrlCommandPort
Definition: ServerCartesianController.h:256
iCub::iKin::iKinChain::blockLink
bool blockLink(const unsigned int i, double Ang)
Blocks the ith Link at the a certain value of its joint angle.
Definition: iKinFwd.cpp:402
ServerCartesianController::portSlvRpc
yarp::os::RpcClient portSlvRpc
Definition: ServerCartesianController.h:189
iCub::iKin::iKinCtrl::getInTargetTol
virtual double getInTargetTol() const
Returns tolerance for in-target check.
Definition: iKinInv.h:284
ServerCartesianController::getTrackingMode
bool getTrackingMode(bool *f)
Definition: ServerCartesianController.cpp:2405
iCub::iKin::iKinLimb::isValid
bool isValid() const
Checks if the limb has been properly configured.
Definition: iKinFwd.h:998
ServerCartesianController::posDirectAvailable
bool posDirectAvailable
Definition: ServerCartesianController.h:123
IKINCARTCTRL_VOCAB_OPT_DES
#define IKINCARTCTRL_VOCAB_OPT_DES
Definition: CommonCartesianController.h:42
iCub::iKin::CartesianHelper::addTargetOption
static void addTargetOption(yarp::os::Bottle &b, const yarp::sig::Vector &xd)
Appends to a bottle all data needed to command a target.
Definition: iKinHlp.cpp:86
ServerCartesianController::qdes
yarp::sig::Vector qdes
Definition: ServerCartesianController.h:181
ServerCartesianController::setInTargetTol
bool setInTargetTol(const double tol)
Definition: ServerCartesianController.cpp:3106
iKinVocabs.h
IKINCARTCTRL_VOCAB_OPT_TIP_FRAME
#define IKINCARTCTRL_VOCAB_OPT_TIP_FRAME
Definition: CommonCartesianController.h:49
iCub::iKin::CartesianHelper::addModeOption
static void addModeOption(yarp::os::Bottle &b, const bool tracking)
Appends to a bottle all data needed to change the tracking mode.
Definition: iKinHlp.cpp:127
ServerCartesianController::detachAll
bool detachAll()
Definition: ServerCartesianController.cpp:2213
iCub::iKin::iCubLeg
Definition: iKinFwd.h:1236
iCub::iKin::iKinLimb::asChain
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
Definition: iKinFwd.h:1013
iCub::iKin::MultiRefMinJerkCtrl::set_gamma
void set_gamma(double _gamma)
Sets the parameter gamma which is used to blend the contribute of the task controller versus the cont...
Definition: iKinInv.h:934
TaskRefVelTargetGenerator::integrate
yarp::sig::Vector integrate(const yarp::sig::Vector &vel)
Definition: ServerCartesianController.cpp:154
iCub::iKin::iKinLimb::getType
std::string getType() const
Returns the Limb type as a string.
Definition: iKinFwd.h:1019
IKINCARTCTRL_VOCAB_CMD_STOP
#define IKINCARTCTRL_VOCAB_CMD_STOP
Definition: CommonCartesianController.h:32
DriverDescriptor::useDefaultMinAbsVel
bool useDefaultMinAbsVel
Definition: ServerCartesianController.h:51
ServerCartesianController::getTaskVelocities
bool getTaskVelocities(yarp::sig::Vector &xdot, yarp::sig::Vector &odot)
Definition: ServerCartesianController.cpp:3128
ServerCartesianController::getTipFrame
bool getTipFrame(yarp::sig::Vector &x, yarp::sig::Vector &o)
Definition: ServerCartesianController.cpp:3245
ServerCartesianController.h
ServerCartesianController::sendCtrlCmdMultipleJointsVelocity
yarp::os::Bottle sendCtrlCmdMultipleJointsVelocity()
Definition: ServerCartesianController.cpp:1399
TaskRefVelTargetGenerator::TaskRefVelTargetGenerator
TaskRefVelTargetGenerator(const double Ts, const yarp::sig::Vector &x0)
Definition: ServerCartesianController.cpp:135
ServerCartesianController::alignJointsBounds
bool alignJointsBounds()
Definition: ServerCartesianController.cpp:1065
ServerCartesianController::cv_syncEvent
std::condition_variable cv_syncEvent
Definition: ServerCartesianController.h:174
ServerCartesianController::notifyEvent
void notifyEvent(const std::string &event, const double checkPoint=-1.0)
Definition: ServerCartesianController.cpp:3525
ServerCartesianController::limbPlan
iCub::iKin::iKinLimb * limbPlan
Definition: ServerCartesianController.h:136
iCub::iKin::MultiRefMinJerkCtrl::set_q
virtual void set_q(const yarp::sig::Vector &q0)
Sets the joint angles values.
Definition: iKinInv.cpp:934
ServerCartesianController::debugInfoEnabled
bool debugInfoEnabled
Definition: ServerCartesianController.h:128
IKINCARTCTRL_VOCAB_OPT_QDOT
#define IKINCARTCTRL_VOCAB_OPT_QDOT
Definition: CommonCartesianController.h:48
ServerCartesianController::getPosePriority
bool getPosePriority(std::string &p)
Definition: ServerCartesianController.cpp:2468
IKINSLV_VOCAB_OPT_PRIO
#define IKINSLV_VOCAB_OPT_PRIO
Definition: iKinVocabs.h:27
ServerCartesianController::goToPositionSync
bool goToPositionSync(const yarp::sig::Vector &xd, const double t=0.0)
Definition: ServerCartesianController.cpp:2599
ServerCartesianController::txTokenLatchedStopControl
double txTokenLatchedStopControl
Definition: ServerCartesianController.h:167
IKINCARTCTRL_VOCAB_CMD_ASK
#define IKINCARTCTRL_VOCAB_CMD_ASK
Definition: CommonCartesianController.h:26
time
degrees time
Definition: sine.m:5
ServerCartesianController::storeContext
bool storeContext(int *id)
Definition: ServerCartesianController.cpp:3342
IKINCARTCTRL_VOCAB_OPT_REFERENCE
#define IKINCARTCTRL_VOCAB_OPT_REFERENCE
Definition: CommonCartesianController.h:35
ServerCartesianController::setTrackingModeHelper
bool setTrackingModeHelper(const bool f)
Definition: ServerCartesianController.cpp:2370
IKINCARTCTRL_VOCAB_OPT_DOF
#define IKINCARTCTRL_VOCAB_OPT_DOF
Definition: CommonCartesianController.h:39
DriverDescriptor::key
std::string key
Definition: ServerCartesianController.h:47
ServerCartesianController::tweakGet
bool tweakGet(yarp::os::Bottle &options)
Definition: ServerCartesianController.cpp:3826
ServerCartesianController::setTask2ndOptions
bool setTask2ndOptions(const yarp::os::Value &v)
Definition: ServerCartesianController.cpp:3732
pathToCustomKinFile
static string pathToCustomKinFile
Definition: main.cpp:163
iCub::iKin::MultiRefMinJerkCtrl::add_compensation
void add_compensation(const yarp::sig::Vector &comp)
Adds to the controller input a further compensation term.
Definition: iKinInv.cpp:956
ServerCartesianController::openPorts
void openPorts()
Definition: ServerCartesianController.cpp:228
iCub::iKin::iKinCtrl::get_q
virtual yarp::sig::Vector get_q() const
Returns the actual joint angles values.
Definition: iKinInv.h:375
ServerCartesianController::rxToken
double rxToken
Definition: ServerCartesianController.h:166
ServerCartesianController::deleteContexts
bool deleteContexts(yarp::os::Bottle *contextIdList)
Definition: ServerCartesianController.cpp:3460
IKINCARTCTRL_VOCAB_CMD_TASKVEL
#define IKINCARTCTRL_VOCAB_CMD_TASKVEL
Definition: CommonCartesianController.h:31
ServerCartesianController::lMod
std::deque< yarp::dev::IControlMode * > lMod
Definition: ServerCartesianController.h:145
ServerCartesianController::taskRefVelTargetGen
TaskRefVelTargetGenerator * taskRefVelTargetGen
Definition: ServerCartesianController.h:139
iCub::iKin::MultiRefMinJerkCtrl::restart
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
Definition: iKinInv.cpp:885
ServerCartesianController::chainState
iCub::iKin::iKinChain * chainState
Definition: ServerCartesianController.h:137
ServerCartesianController::getInfo
bool getInfo(yarp::os::Bottle &info)
Definition: ServerCartesianController.cpp:3482
ServerCartesianController::mtx_syncEvent
std::mutex mtx_syncEvent
Definition: ServerCartesianController.h:173
ServerCartesianController::closePorts
void closePorts()
Definition: ServerCartesianController.cpp:251