iCub-main
ClientCartesianController.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 <algorithm>
23 #include <sstream>
24 
27 
28 #include <yarp/math/Math.h>
29 
30 #include <iCub/iKin/iKinInv.h>
31 
32 #define CARTCTRL_CLIENT_VER 1.1
33 #define CARTCTRL_DEFAULT_TMO 0.1 // [s]
34 
35 using namespace std;
36 using namespace yarp::os;
37 using namespace yarp::dev;
38 using namespace yarp::sig;
39 using namespace yarp::math;
40 
41 
42 /************************************************************************/
43 void CartesianEventHandler::onRead(Bottle &event)
44 {
45  if (interface!=NULL)
46  interface->eventHandling(event);
47 }
48 
49 
50 /************************************************************************/
52 {
53  this->interface=interface;
54  setStrict();
55  useCallback();
56 }
57 
58 
59 /************************************************************************/
61 {
62  init();
63 }
64 
65 
66 /************************************************************************/
68 {
69  init();
70  open(config);
71 }
72 
73 
74 /************************************************************************/
76 {
77  connected=false;
78  closed=true;
79 
80  timeout=CARTCTRL_DEFAULT_TMO;
81  lastPoseMsgArrivalTime=0.0;
82 
83  pose.resize(7,0.0);
84 
85  portEvents.setInterface(this);
86 }
87 
88 
89 /************************************************************************/
90 bool ClientCartesianController::open(Searchable &config)
91 {
92  string remote, local, carrier;
93 
94  if (config.check("remote"))
95  remote=config.find("remote").asString();
96  else
97  return false;
98 
99  if (config.check("local"))
100  local=config.find("local").asString();
101  else
102  return false;
103 
104  closed=false;
105  carrier=config.check("carrier",Value("udp")).asString();
106 
107  if (config.check("timeout"))
108  timeout=config.find("timeout").asFloat64();
109 
110  portCmd.open(local+"/command:o");
111  portState.open(local+"/state:i");
112  portEvents.open(local+"/events:i");
113  portRpc.open(local+"/rpc:o");
114 
115  bool ok=true;
116  ok&=Network::connect(portRpc.getName(),remote+"/rpc:i");
117  if (ok)
118  {
119  Bottle info;
120  getInfoHelper(info);
121  if (info.check("server_version"))
122  {
123  double server_version=info.find("server_version").asFloat64();
124  if (server_version!=CARTCTRL_CLIENT_VER)
125  {
126  yError("version mismatch => server(%g) != client(%g); please update accordingly",
127  server_version,CARTCTRL_CLIENT_VER);
128  close();
129  return false;
130  }
131  }
132  else
133  yWarning("unable to retrieve server version; please update the server");
134  }
135  else
136  {
137  yError("unable to connect to the server rpc port!");
138  close();
139  return false;
140  }
141 
142  ok&=Network::connect(portCmd.getName(),remote+"/command:i",carrier);
143  ok&=Network::connect(remote+"/state:o",portState.getName(),carrier);
144  ok&=Network::connect(remote+"/events:o",portEvents.getName(),carrier);
145 
146  // check whether the solver is alive and connected
147  if (ok)
148  {
149  Bottle command, reply;
150 
151  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
152  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_ISSOLVERON);
153 
154  if (!portRpc.write(command,reply))
155  {
156  yError("unable to get reply from server!");
157  close();
158  return false;
159  }
160 
161  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
162  if (reply.size()>1)
163  if (reply.get(1).asVocab32()==IKINCARTCTRL_VOCAB_VAL_TRUE)
164  return connected=true;
165 
166  yError("unable to connect to solver!");
167  close();
168  return false;
169  }
170  else
171  {
172  yError("unable to connect to server!");
173  close();
174  return false;
175  }
176 }
177 
178 
179 /************************************************************************/
181 {
182  if (closed)
183  return true;
184 
185  deleteContexts();
186 
187  while (eventsMap.size()>0)
188  unregisterEvent(*eventsMap.begin()->second);
189 
190  portCmd.interrupt();
191  portState.interrupt();
192  portEvents.interrupt();
193  portRpc.interrupt();
194 
195  portCmd.close();
196  portState.close();
197  portEvents.close();
198  portRpc.close();
199 
200  connected=false;
201  return closed=true;
202 }
203 
204 
205 /************************************************************************/
207 {
208  if (!connected)
209  return false;
210 
211  Bottle command, reply;
212  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_SET);
213  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_MODE);
214 
216 
217  if (!portRpc.write(command,reply))
218  {
219  yError("unable to get reply from server!");
220  return false;
221  }
222 
223  return (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK);
224 }
225 
226 
227 /************************************************************************/
229 {
230  if (!connected || (f==NULL))
231  return false;
232 
233  Bottle command, reply;
234  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
235  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_MODE);
236 
237  if (!portRpc.write(command,reply))
238  {
239  yError("unable to get reply from server!");
240  return false;
241  }
242 
243  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
244  {
245  int mode=reply.get(1).asVocab32();
246 
248  *f=true;
250  *f=false;
251  else
252  return false;
253 
254  return true;
255  }
256  else
257  return false;
258 }
259 
260 
261 /************************************************************************/
263 {
264  if (!connected)
265  return false;
266 
267  Bottle command, reply;
268  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_SET);
269  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_REFERENCE);
270 
272 
273  if (!portRpc.write(command,reply))
274  {
275  yError("unable to get reply from server!");
276  return false;
277  }
278 
279  return (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK);
280 }
281 
282 
283 /************************************************************************/
285 {
286  if (!connected || (f==NULL))
287  return false;
288 
289  Bottle command, reply;
290  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
291  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_REFERENCE);
292 
293  if (!portRpc.write(command,reply))
294  {
295  yError("unable to get reply from server!");
296  return false;
297  }
298 
299  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
300  {
301  int mode=reply.get(1).asVocab32();
302 
304  *f=true;
306  *f=false;
307  else
308  return false;
309 
310  return true;
311  }
312  else
313  return false;
314 }
315 
316 
317 /************************************************************************/
319 {
320  if (!connected)
321  return false;
322 
323  Bottle command, reply;
324  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_SET);
325  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_PRIO);
326  command.addString(p);
327 
328  if (!portRpc.write(command,reply))
329  {
330  yError("unable to get reply from server!");
331  return false;
332  }
333 
334  return (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK);
335 }
336 
337 
338 /************************************************************************/
340 {
341  if (!connected)
342  return false;
343 
344  Bottle command, reply;
345  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
346  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_PRIO);
347 
348  if (!portRpc.write(command,reply))
349  {
350  yError("unable to get reply from server!");
351  return false;
352  }
353 
354  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
355  {
356  p=reply.get(1).asString();
357  return true;
358  }
359  else
360  return false;
361 }
362 
363 
364 /************************************************************************/
365 bool ClientCartesianController::getPose(Vector &x, Vector &o, Stamp *stamp)
366 {
367  if (!connected)
368  return false;
369 
370  double now=Time::now();
371 
372  // receive from network in streaming mode (non-blocking)
373  if (Vector *v=portState.read(false))
374  {
375  pose=*v;
376  portState.getEnvelope(poseStamp);
377  lastPoseMsgArrivalTime=now;
378  }
379 
380  x.resize(3);
381  o.resize(pose.length()-x.length());
382 
383  for (size_t i=0; i<x.length(); i++)
384  x[i]=pose[i];
385 
386  for (size_t i=0; i<o.length(); i++)
387  o[i]=pose[x.length()+i];
388 
389  if (stamp!=NULL)
390  *stamp=poseStamp;
391 
392  return (now-lastPoseMsgArrivalTime<timeout);
393 }
394 
395 
396 /************************************************************************/
397 bool ClientCartesianController::getPose(const int axis, Vector &x, Vector &o,
398  Stamp *stamp)
399 {
400  if (!connected)
401  return false;
402 
403  Bottle command, reply;
404  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
405  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_POSE);
406  command.addInt32(axis);
407 
408  if (!portRpc.write(command,reply))
409  {
410  yError("unable to get reply from server!");
411  return false;
412  }
413 
414  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
415  {
416  if (Bottle *posePart=reply.get(1).asList())
417  {
418  x.resize(3);
419  o.resize(posePart->size()-x.length());
420 
421  for (size_t i=0; i<x.length(); i++)
422  x[i]=posePart->get(i).asFloat64();
423 
424  for (size_t i=0; i<o.length(); i++)
425  o[i]=posePart->get(x.length()+i).asFloat64();
426 
427  if ((reply.size()>2) && (stamp!=NULL))
428  {
429  if (Bottle *stampPart=reply.get(2).asList())
430  {
431  Stamp tmpStamp(stampPart->get(0).asInt32(),
432  stampPart->get(1).asFloat64());
433 
434  *stamp=tmpStamp;
435  }
436  }
437 
438  return true;
439  }
440  }
441 
442  return false;
443 }
444 
445 
446 /************************************************************************/
447 bool ClientCartesianController::goToPose(const Vector &xd, const Vector &od,
448  const double t)
449 {
450  if (!connected || (xd.length()<3) || (od.length()<4))
451  return false;
452 
453  Bottle &command=portCmd.prepare();
454  command.clear();
455 
456  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GO);
457  command.addVocab32(IKINCARTCTRL_VOCAB_VAL_POSE_FULL);
458  command.addFloat64(t);
459  Bottle &xdesPart=command.addList();
460 
461  for (int i=0; i<3; i++)
462  xdesPart.addFloat64(xd[i]);
463 
464  for (int i=0; i<4; i++)
465  xdesPart.addFloat64(od[i]);
466 
467  // send command
468  portCmd.writeStrict();
469  return true;
470 }
471 
472 
473 /************************************************************************/
474 bool ClientCartesianController::goToPosition(const Vector &xd, const double t)
475 {
476  if (!connected || (xd.length()<3))
477  return false;
478 
479  Bottle &command=portCmd.prepare();
480  command.clear();
481 
482  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GO);
483  command.addVocab32(IKINCARTCTRL_VOCAB_VAL_POSE_XYZ);
484  command.addFloat64(t);
485  Bottle &xdesPart=command.addList();
486 
487  for (int i=0; i<3; i++)
488  xdesPart.addFloat64(xd[i]);
489 
490  // send command
491  portCmd.writeStrict();
492  return true;
493 }
494 
495 
496 /************************************************************************/
497 bool ClientCartesianController::goToPoseSync(const Vector &xd, const Vector &od,
498  const double t)
499 {
500  if (!connected || (xd.length()<3) || (od.length()<4))
501  return false;
502 
503  Bottle command, reply;
504  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GO);
505  command.addVocab32(IKINCARTCTRL_VOCAB_VAL_POSE_FULL);
506  command.addFloat64(t);
507  Bottle &xdesPart=command.addList();
508 
509  for (int i=0; i<3; i++)
510  xdesPart.addFloat64(xd[i]);
511 
512  for (int i=0; i<4; i++)
513  xdesPart.addFloat64(od[i]);
514 
515  if (!portRpc.write(command,reply))
516  {
517  yError("unable to get reply from server!");
518  return false;
519  }
520 
521  return (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK);
522 }
523 
524 
525 /************************************************************************/
526 bool ClientCartesianController::goToPositionSync(const Vector &xd, const double t)
527 {
528  if (!connected || (xd.length()<3))
529  return false;
530 
531  Bottle command, reply;
532  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GO);
533  command.addVocab32(IKINCARTCTRL_VOCAB_VAL_POSE_XYZ);
534  command.addFloat64(t);
535  Bottle &xdesPart=command.addList();
536 
537  for (int i=0; i<3; i++)
538  xdesPart.addFloat64(xd[i]);
539 
540  if (!portRpc.write(command,reply))
541  {
542  yError("unable to get reply from server!");
543  return false;
544  }
545 
546  return (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK);
547 }
548 
549 
550 /************************************************************************/
551 bool ClientCartesianController::getDesired(Vector &xdhat, Vector &odhat,
552  Vector &qdhat)
553 {
554  if (!connected)
555  return false;
556 
557  Bottle command, reply;
558  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
559  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_DES);
560 
561  if (!portRpc.write(command,reply))
562  {
563  yError("unable to get reply from server!");
564  return false;
565  }
566 
567  return getDesiredOption(reply,xdhat,odhat,qdhat);
568 }
569 
570 
571 /************************************************************************/
572 bool ClientCartesianController::askForPose(const Vector &xd, const Vector &od,
573  Vector &xdhat, Vector &odhat,
574  Vector &qdhat)
575 {
576  if (!connected)
577  return false;
578 
579  Bottle command, reply;
580  Vector tg(xd.length()+od.length());
581  for (size_t i=0; i<xd.length(); i++)
582  tg[i]=xd[i];
583 
584  for (size_t i=0; i<od.length(); i++)
585  tg[xd.length()+i]=od[i];
586 
587  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_ASK);
588  addVectorOption(command,IKINCARTCTRL_VOCAB_OPT_XD,tg);
589  addPoseOption(command,IKINCTRL_POSE_FULL);
590 
591  if (!portRpc.write(command,reply))
592  {
593  yError("unable to get reply from server!");
594  return false;
595  }
596 
597  return getDesiredOption(reply,xdhat,odhat,qdhat);
598 }
599 
600 
601 /************************************************************************/
602 bool ClientCartesianController::askForPose(const Vector &q0, const Vector &xd,
603  const Vector &od, Vector &xdhat,
604  Vector &odhat, Vector &qdhat)
605 {
606  if (!connected)
607  return false;
608 
609  Bottle command, reply;
610  Vector tg(xd.length()+od.length());
611  for (size_t i=0; i<xd.length(); i++)
612  tg[i]=xd[i];
613 
614  for (size_t i=0; i<od.length(); i++)
615  tg[xd.length()+i]=od[i];
616 
617  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_ASK);
618  addVectorOption(command,IKINCARTCTRL_VOCAB_OPT_XD,tg);
619  addVectorOption(command,IKINCARTCTRL_VOCAB_OPT_Q,q0);
620  addPoseOption(command,IKINCTRL_POSE_FULL);
621 
622  if (!portRpc.write(command,reply))
623  {
624  yError("unable to get reply from server!");
625  return false;
626  }
627 
628  return getDesiredOption(reply,xdhat,odhat,qdhat);
629 }
630 
631 
632 /************************************************************************/
633 bool ClientCartesianController::askForPosition(const Vector &xd, Vector &xdhat,
634  Vector &odhat, Vector &qdhat)
635 {
636  if (!connected)
637  return false;
638 
639  Bottle command, reply;
640  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_ASK);
641  addVectorOption(command,IKINCARTCTRL_VOCAB_OPT_XD,xd);
642  addPoseOption(command,IKINCTRL_POSE_XYZ);
643 
644  if (!portRpc.write(command,reply))
645  {
646  yError("unable to get reply from server!");
647  return false;
648  }
649 
650  return getDesiredOption(reply,xdhat,odhat,qdhat);
651 }
652 
653 
654 /************************************************************************/
655 bool ClientCartesianController::askForPosition(const Vector &q0, const Vector &xd,
656  Vector &xdhat, Vector &odhat,
657  Vector &qdhat)
658 {
659  if (!connected)
660  return false;
661 
662  Bottle command, reply;
663  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_ASK);
664  addVectorOption(command,IKINCARTCTRL_VOCAB_OPT_XD,xd);
665  addVectorOption(command,IKINCARTCTRL_VOCAB_OPT_Q,q0);
666  addPoseOption(command,IKINCTRL_POSE_XYZ);
667 
668  if (!portRpc.write(command,reply))
669  {
670  yError("unable to get reply from server!");
671  return false;
672  }
673 
674  return getDesiredOption(reply,xdhat,odhat,qdhat);
675 }
676 
677 
678 /************************************************************************/
680 {
681  if (!connected)
682  return false;
683 
684  Bottle command, reply;
685  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
686  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_DOF);
687 
688  if (!portRpc.write(command,reply))
689  {
690  yError("unable to get reply from server!");
691  return false;
692  }
693 
694  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
695  {
696  if (Bottle *dofPart=reply.get(1).asList())
697  {
698  curDof.resize(dofPart->size());
699 
700  for (size_t i=0; i<curDof.length(); i++)
701  curDof[i]=dofPart->get(i).asFloat64();
702 
703  return true;
704  }
705  }
706 
707  return false;
708 }
709 
710 
711 /************************************************************************/
712 bool ClientCartesianController::setDOF(const Vector &newDof, Vector &curDof)
713 {
714  if (!connected)
715  return false;
716 
717  Bottle command, reply;
718  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_SET);
719  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_DOF);
720  Bottle &dofPart=command.addList();
721 
722  for (size_t i=0; i<newDof.length(); i++)
723  dofPart.addInt32((int)newDof[i]);
724 
725  if (!portRpc.write(command,reply))
726  {
727  yError("unable to get reply from server!");
728  return false;
729  }
730 
731  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
732  {
733  if (Bottle *dofPart=reply.get(1).asList())
734  {
735  curDof.resize(dofPart->size());
736 
737  for (size_t i=0; i<curDof.length(); i++)
738  curDof[i]=dofPart->get(i).asFloat64();
739 
740  return true;
741  }
742  }
743 
744  return false;
745 }
746 
747 
748 /************************************************************************/
750 {
751  if (!connected)
752  return false;
753 
754  Bottle command, reply;
755  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
756  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_REST_POS);
757 
758  if (!portRpc.write(command,reply))
759  {
760  yError("unable to get reply from server!");
761  return false;
762  }
763 
764  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
765  {
766  if (Bottle *restPart=reply.get(1).asList())
767  {
768  curRestPos.resize(restPart->size());
769 
770  for (size_t i=0; i<curRestPos.length(); i++)
771  curRestPos[i]=restPart->get(i).asFloat64();
772 
773  return true;
774  }
775  }
776 
777  return false;
778 }
779 
780 
781 /************************************************************************/
782 bool ClientCartesianController::setRestPos(const Vector &newRestPos,
783  Vector &curRestPos)
784 {
785  if (!connected)
786  return false;
787 
788  Bottle command, reply;
789  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_SET);
790  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_REST_POS);
791  command.addList().read(newRestPos);
792 
793  if (!portRpc.write(command,reply))
794  {
795  yError("unable to get reply from server!");
796  return false;
797  }
798 
799  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
800  {
801  if (Bottle *restPart=reply.get(1).asList())
802  {
803  curRestPos.resize(restPart->size());
804  for (size_t i=0; i<curRestPos.length(); i++)
805  curRestPos[i]=restPart->get(i).asFloat64();
806  return true;
807  }
808  }
809 
810  return false;
811 }
812 
813 
814 /************************************************************************/
815 bool ClientCartesianController::getRestWeights(Vector &curRestWeights)
816 {
817  if (!connected)
818  return false;
819 
820  Bottle command, reply;
821  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
822  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_REST_WEIGHTS);
823 
824  if (!portRpc.write(command,reply))
825  {
826  yError("unable to get reply from server!");
827  return false;
828  }
829 
830  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
831  {
832  if (Bottle *restPart=reply.get(1).asList())
833  {
834  curRestWeights.resize(restPart->size());
835 
836  for (size_t i=0; i<curRestWeights.length(); i++)
837  curRestWeights[i]=restPart->get(i).asFloat64();
838 
839  return true;
840  }
841  }
842 
843  return false;
844 }
845 
846 
847 /************************************************************************/
848 bool ClientCartesianController::setRestWeights(const Vector &newRestWeights,
849  Vector &curRestWeights)
850 {
851  if (!connected)
852  return false;
853 
854  Bottle command, reply;
855  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_SET);
856  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_REST_WEIGHTS);
857  command.addList().read(newRestWeights);
858 
859  if (!portRpc.write(command,reply))
860  {
861  yError("unable to get reply from server!");
862  return false;
863  }
864 
865  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
866  {
867  if (Bottle *restPart=reply.get(1).asList())
868  {
869  curRestWeights.resize(restPart->size());
870  for (size_t i=0; i<curRestWeights.length(); i++)
871  curRestWeights[i]=restPart->get(i).asFloat64();
872  return true;
873  }
874  }
875 
876  return false;
877 }
878 
879 
880 /************************************************************************/
881 bool ClientCartesianController::getLimits(const int axis, double *min,
882  double *max)
883 {
884  if (!connected || (min==NULL) || (max==NULL))
885  return false;
886 
887  Bottle command, reply;
888  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
889  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_LIM);
890  command.addInt32(axis);
891 
892  if (!portRpc.write(command,reply))
893  {
894  yError("unable to get reply from server!");
895  return false;
896  }
897 
898  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
899  {
900  if (reply.size()>2)
901  {
902  *min=reply.get(1).asFloat64();
903  *max=reply.get(2).asFloat64();
904  return true;
905  }
906  }
907 
908  return false;
909 }
910 
911 
912 /************************************************************************/
913 bool ClientCartesianController::setLimits(const int axis, const double min,
914  const double max)
915 {
916  if (!connected)
917  return false;
918 
919  Bottle command, reply;
920  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_SET);
921  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_LIM);
922  command.addInt32(axis);
923  command.addFloat64(min);
924  command.addFloat64(max);
925 
926  if (!portRpc.write(command,reply))
927  {
928  yError("unable to get reply from server!");
929  return false;
930  }
931 
932  return (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK);
933 }
934 
935 
936 /************************************************************************/
938 {
939  if (!connected || (t==NULL))
940  return false;
941 
942  Bottle command, reply;
943  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
944  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_TIME);
945 
946  if (!portRpc.write(command,reply))
947  {
948  yError("unable to get reply from server!");
949  return false;
950  }
951 
952  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
953  {
954  if (reply.size()>1)
955  {
956  *t=reply.get(1).asFloat64();
957  return true;
958  }
959  }
960 
961  return false;
962 }
963 
964 
965 /************************************************************************/
967 {
968  if (!connected)
969  return false;
970 
971  Bottle command, reply;
972  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_SET);
973  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_TIME);
974  command.addFloat64(t);
975 
976  if (!portRpc.write(command,reply))
977  {
978  yError("unable to get reply from server!");
979  return false;
980  }
981 
982  return (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK);
983 }
984 
985 
986 /************************************************************************/
988 {
989  if (!connected || (tol==NULL))
990  return false;
991 
992  Bottle command, reply;
993  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
994  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_TOL);
995 
996  if (!portRpc.write(command,reply))
997  {
998  yError("unable to get reply from server!");
999  return false;
1000  }
1001 
1002  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
1003  {
1004  if (reply.size()>1)
1005  {
1006  *tol=reply.get(1).asFloat64();
1007  return true;
1008  }
1009  }
1010 
1011  return false;
1012 }
1013 
1014 
1015 /************************************************************************/
1017 {
1018  if (!connected)
1019  return false;
1020 
1021  Bottle command, reply;
1022  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_SET);
1023  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_TOL);
1024  command.addFloat64(tol);
1025 
1026  if (!portRpc.write(command,reply))
1027  {
1028  yError("unable to get reply from server!");
1029  return false;
1030  }
1031 
1032  return (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK);
1033 }
1034 
1035 
1036 /************************************************************************/
1038 {
1039  if (!connected)
1040  return false;
1041 
1042  Bottle command, reply;
1043  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
1044  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_QDOT);
1045 
1046  if (!portRpc.write(command,reply))
1047  {
1048  yError("unable to get reply from server!");
1049  return false;
1050  }
1051 
1052  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
1053  {
1054  if (Bottle *qdotPart=reply.get(1).asList())
1055  {
1056  qdot.resize(qdotPart->size());
1057 
1058  for (size_t i=0; i<qdot.length(); i++)
1059  qdot[i]=qdotPart->get(i).asFloat64();
1060 
1061  return true;
1062  }
1063  }
1064 
1065  return false;
1066 }
1067 
1068 
1069 /************************************************************************/
1070 bool ClientCartesianController::getTaskVelocities(Vector &xdot, Vector &odot)
1071 {
1072  if (!connected)
1073  return false;
1074 
1075  Bottle command, reply;
1076  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
1077  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_XDOT);
1078 
1079  if (!portRpc.write(command,reply))
1080  {
1081  yError("unable to get reply from server!");
1082  return false;
1083  }
1084 
1085  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
1086  {
1087  if (Bottle *xdotPart=reply.get(1).asList())
1088  {
1089  xdot.resize(3);
1090  odot.resize(xdotPart->size()-xdot.length());
1091 
1092  for (size_t i=0; i<xdot.length(); i++)
1093  xdot[i]=xdotPart->get(i).asFloat64();
1094 
1095  for (size_t i=0; i<odot.length(); i++)
1096  odot[i]=xdotPart->get(xdot.length()+i).asFloat64();
1097 
1098  return true;
1099  }
1100  }
1101 
1102  return false;
1103 }
1104 
1105 
1106 /************************************************************************/
1108  const Vector &odot)
1109 {
1110  if (!connected || (xdot.length()<3) || (odot.length()<4))
1111  return false;
1112 
1113  Bottle &command=portCmd.prepare();
1114  command.clear();
1115 
1116  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_TASKVEL);
1117  Bottle &xdotPart=command.addList();
1118 
1119  for (int i=0; i<3; i++)
1120  xdotPart.addFloat64(xdot[i]);
1121 
1122  for (int i=0; i<4; i++)
1123  xdotPart.addFloat64(odot[i]);
1124 
1125  // send command
1126  portCmd.writeStrict();
1127  return true;
1128 }
1129 
1130 
1131 /************************************************************************/
1132 bool ClientCartesianController::attachTipFrame(const Vector &x, const Vector &o)
1133 {
1134  if (!connected || (x.length()<3) || (o.length()<4))
1135  return false;
1136 
1137  Bottle command, reply;
1138  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_SET);
1139  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_TIP_FRAME);
1140  Bottle &tipPart=command.addList();
1141 
1142  for (int i=0; i<3; i++)
1143  tipPart.addFloat64(x[i]);
1144 
1145  for (int i=0; i<4; i++)
1146  tipPart.addFloat64(o[i]);
1147 
1148  if (!portRpc.write(command,reply))
1149  {
1150  yError("unable to get reply from server!");
1151  return false;
1152  }
1153 
1154  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
1155  {
1156  // mandatory refresh of the local pose (blocking)
1157  if (Vector *v=portState.read(true))
1158  {
1159  pose=*v;
1160  portState.getEnvelope(poseStamp);
1161  lastPoseMsgArrivalTime=Time::now();
1162  }
1163 
1164  return true;
1165  }
1166  else
1167  return false;
1168 }
1169 
1170 
1171 /************************************************************************/
1173 {
1174  if (!connected)
1175  return false;
1176 
1177  Bottle command, reply;
1178  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
1179  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_TIP_FRAME);
1180 
1181  if (!portRpc.write(command,reply))
1182  {
1183  yError("unable to get reply from server!");
1184  return false;
1185  }
1186 
1187  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
1188  {
1189  if (Bottle *tipPart=reply.get(1).asList())
1190  {
1191  x.resize(3);
1192  o.resize(tipPart->size()-x.length());
1193 
1194  for (size_t i=0; i<x.length(); i++)
1195  x[i]=tipPart->get(i).asFloat64();
1196 
1197  for (size_t i=0; i<o.length(); i++)
1198  o[i]=tipPart->get(x.length()+i).asFloat64();
1199 
1200  return true;
1201  }
1202  }
1203 
1204  return false;
1205 }
1206 
1207 
1208 /************************************************************************/
1210 {
1211  return attachTipFrame(Vector(3,0.0),Vector(4,0.0));
1212 }
1213 
1214 
1215 /************************************************************************/
1217 {
1218  if (!connected || (f==NULL))
1219  return false;
1220 
1221  Bottle command, reply;
1222  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
1223  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_MOTIONDONE);
1224 
1225  if (!portRpc.write(command,reply))
1226  {
1227  yError("unable to get reply from server!");
1228  return false;
1229  }
1230 
1231  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
1232  {
1233  if (reply.size()>1)
1234  {
1235  int flag=reply.get(1).asVocab32();
1236 
1237  if (flag==IKINCARTCTRL_VOCAB_VAL_TRUE)
1238  *f=true;
1239  else if (flag==IKINCARTCTRL_VOCAB_VAL_FALSE)
1240  *f=false;
1241 
1242  return true;
1243  }
1244  }
1245 
1246  return false;
1247 }
1248 
1249 
1250 /************************************************************************/
1252  const double timeout)
1253 {
1254  bool done=false;
1255  double t0=Time::now();
1256 
1257  while (!done)
1258  {
1259  Time::delay(period);
1260 
1261  if (!checkMotionDone(&done) || ((timeout>0.0) && ((Time::now()-t0)>timeout)))
1262  return false;
1263  }
1264 
1265  return true;
1266 }
1267 
1268 
1269 /************************************************************************/
1271 {
1272  if (!connected)
1273  return false;
1274 
1275  Bottle command, reply;
1276  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_STOP);
1277 
1278  if (!portRpc.write(command,reply))
1279  {
1280  yError("unable to get reply from server!");
1281  return false;
1282  }
1283 
1284  return (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK);
1285 }
1286 
1287 
1288 /************************************************************************/
1290 {
1291  if (!connected || (id==NULL))
1292  return false;
1293 
1294  Bottle command, reply;
1295  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_STORE);
1296 
1297  if (!portRpc.write(command,reply))
1298  {
1299  yError("unable to get reply from server!");
1300  return false;
1301  }
1302 
1303  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
1304  {
1305  if (reply.size()>1)
1306  {
1307  contextIdList.insert(*id=reply.get(1).asInt32());
1308  return true;
1309  }
1310  }
1311 
1312  return false;
1313 }
1314 
1315 
1316 /************************************************************************/
1318 {
1319  if (!connected || ((contextIdList.find(id)==contextIdList.end()) && (id!=0)))
1320  return false;
1321 
1322  Bottle command, reply;
1323  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_RESTORE);
1324  command.addInt32(id);
1325 
1326  if (!portRpc.write(command,reply))
1327  {
1328  yError("unable to get reply from server!");
1329  return false;
1330  }
1331 
1332  return (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK);
1333 }
1334 
1335 
1336 /************************************************************************/
1338 {
1339  if (!connected || ((contextIdList.find(id)==contextIdList.end()) && (id!=0)))
1340  return false;
1341 
1342  Bottle command, reply;
1343  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_DELETE);
1344  command.addList().addInt32(id);
1345 
1346  if (!portRpc.write(command,reply))
1347  {
1348  yError("unable to get reply from server!");
1349  return false;
1350  }
1351 
1352  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
1353  {
1354  contextIdList.erase(id);
1355  return true;
1356  }
1357  else
1358  return false;
1359 }
1360 
1361 
1362 /************************************************************************/
1364 {
1365  if (!connected)
1366  return false;
1367 
1368  Bottle command, reply;
1369  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_DELETE);
1370  Bottle &ids=command.addList();
1371  for (set<int>::iterator itr=contextIdList.begin(); itr!=contextIdList.end(); itr++)
1372  ids.addInt32(*itr);
1373 
1374  if (!portRpc.write(command,reply))
1375  {
1376  yError("unable to get reply from server!");
1377  return false;
1378  }
1379 
1380  contextIdList.clear();
1381 
1382  return (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK);
1383 }
1384 
1385 
1386 /************************************************************************/
1388 {
1389  Bottle command, reply;
1390  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
1391  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_INFO);
1392 
1393  if (!portRpc.write(command,reply))
1394  {
1395  yError("unable to get reply from server!");
1396  return false;
1397  }
1398 
1399  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
1400  {
1401  if (reply.size()>1)
1402  {
1403  if (Bottle *infoPart=reply.get(1).asList())
1404  info=*infoPart;
1405 
1406  return true;
1407  }
1408  }
1409 
1410  return false;
1411 }
1412 
1413 
1414 /************************************************************************/
1416 {
1417  if (connected)
1418  return getInfoHelper(info);
1419  else
1420  return false;
1421 }
1422 
1423 
1424 /************************************************************************/
1426 {
1427  string type=event.get(0).asString();
1428  double time=event.get(1).asFloat64();
1429  double checkPoint=(type=="motion-ongoing")?event.get(2).asFloat64():-1.0;
1430  map<string,CartesianEvent*>::iterator itr;
1431 
1432  // rise the all-events callback
1433  itr=eventsMap.find("*");
1434  if (itr!=eventsMap.end())
1435  {
1436  if (itr->second!=NULL)
1437  {
1438  CartesianEvent &Event=*itr->second;
1439  Event.cartesianEventVariables.type=type;
1440  Event.cartesianEventVariables.time=time;
1441 
1442  if (checkPoint>=0.0)
1443  Event.cartesianEventVariables.motionOngoingCheckPoint=checkPoint;
1444 
1445  Event.cartesianEventCallback();
1446  }
1447  }
1448 
1449  string typeExtended=type;
1450  if (checkPoint>=0.0)
1451  {
1452  ostringstream ss;
1453  ss<<type<<"-"<<checkPoint;
1454  typeExtended=ss.str();
1455  }
1456 
1457  // rise the event specific callback
1458  itr=eventsMap.find(typeExtended);
1459  if (itr!=eventsMap.end())
1460  {
1461  if (itr->second!=NULL)
1462  {
1463  CartesianEvent &Event=*itr->second;
1464  Event.cartesianEventVariables.type=type;
1465  Event.cartesianEventVariables.time=time;
1466 
1467  if (checkPoint>=0.0)
1468  Event.cartesianEventVariables.motionOngoingCheckPoint=checkPoint;
1469 
1470  Event.cartesianEventCallback();
1471  }
1472  }
1473 }
1474 
1475 
1476 /************************************************************************/
1477 bool ClientCartesianController::registerEvent(CartesianEvent &event)
1478 {
1479  if (!connected)
1480  return false;
1481 
1482  string type=event.cartesianEventParameters.type;
1483  if (type=="motion-ongoing")
1484  {
1485  double checkPoint=event.cartesianEventParameters.motionOngoingCheckPoint;
1486 
1487  Bottle command, reply;
1488  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_EVENT);
1489  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_REGISTER);
1490  command.addVocab32(IKINCARTCTRL_VOCAB_VAL_EVENT_ONGOING);
1491  command.addFloat64(checkPoint);
1492 
1493  if (!portRpc.write(command,reply))
1494  {
1495  yError("unable to get reply from server!");
1496  return false;
1497  }
1498 
1499  if (reply.get(0).asVocab32()!=IKINCARTCTRL_VOCAB_REP_ACK)
1500  return false;
1501 
1502  ostringstream ss;
1503  ss<<type<<"-"<<checkPoint;
1504  type=ss.str();
1505  }
1506 
1507  eventsMap[type]=&event;
1508  return true;
1509 }
1510 
1511 
1512 /************************************************************************/
1514 {
1515  if (!connected)
1516  return false;
1517 
1518  string type=event.cartesianEventParameters.type;
1519  if (type=="motion-ongoing")
1520  {
1521  double checkPoint=event.cartesianEventParameters.motionOngoingCheckPoint;
1522 
1523  Bottle command, reply;
1524  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_EVENT);
1525  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_UNREGISTER);
1526  command.addVocab32(IKINCARTCTRL_VOCAB_VAL_EVENT_ONGOING);
1527  command.addFloat64(checkPoint);
1528 
1529  if (!portRpc.write(command,reply))
1530  {
1531  yError("unable to get reply from server!");
1532  return false;
1533  }
1534 
1535  if (reply.get(0).asVocab32()!=IKINCARTCTRL_VOCAB_REP_ACK)
1536  return false;
1537 
1538  ostringstream ss;
1539  ss<<type<<"-"<<checkPoint;
1540  type=ss.str();
1541  }
1542 
1543  eventsMap.erase(type);
1544  return true;
1545 }
1546 
1547 
1548 /************************************************************************/
1549 bool ClientCartesianController::tweakSet(const Bottle &options)
1550 {
1551  if (!connected)
1552  return false;
1553 
1554  Bottle command, reply;
1555  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_SET);
1556  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_TWEAK);
1557  command.addList()=options;
1558 
1559  if (!portRpc.write(command,reply))
1560  {
1561  yError("unable to get reply from server!");
1562  return false;
1563  }
1564 
1565  return (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK);
1566 }
1567 
1568 
1569 /************************************************************************/
1571 {
1572  if (!connected)
1573  return false;
1574 
1575  Bottle command, reply;
1576  command.addVocab32(IKINCARTCTRL_VOCAB_CMD_GET);
1577  command.addVocab32(IKINCARTCTRL_VOCAB_OPT_TWEAK);
1578 
1579  if (!portRpc.write(command,reply))
1580  {
1581  yError("unable to get reply from server!");
1582  return false;
1583  }
1584 
1585  if (reply.get(0).asVocab32()==IKINCARTCTRL_VOCAB_REP_ACK)
1586  {
1587  if (reply.size()>1)
1588  {
1589  if (Bottle *optionsPart=reply.get(1).asList())
1590  options=*optionsPart;
1591 
1592  return true;
1593  }
1594  }
1595 
1596  return false;
1597 }
1598 
1599 
1600 /************************************************************************/
1602 {
1603  close();
1604 }
1605 
1606 
CARTCTRL_DEFAULT_TMO
#define CARTCTRL_DEFAULT_TMO
Definition: ClientCartesianController.cpp:33
CARTCTRL_CLIENT_VER
#define CARTCTRL_CLIENT_VER
Definition: ClientCartesianController.cpp:32
ClientCartesianController::getTaskVelocities
bool getTaskVelocities(yarp::sig::Vector &xdot, yarp::sig::Vector &odot)
Definition: ClientCartesianController.cpp:1070
ClientCartesianController::getDOF
bool getDOF(yarp::sig::Vector &curDof)
Definition: ClientCartesianController.cpp:679
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
IKINCARTCTRL_VOCAB_OPT_TIME
#define IKINCARTCTRL_VOCAB_OPT_TIME
Definition: CommonCartesianController.h:37
IKINCARTCTRL_VOCAB_OPT_TWEAK
#define IKINCARTCTRL_VOCAB_OPT_TWEAK
Definition: CommonCartesianController.h:54
ClientCartesianController::ClientCartesianController
ClientCartesianController()
Definition: ClientCartesianController.cpp:60
IKINCARTCTRL_VOCAB_VAL_EVENT_ONGOING
#define IKINCARTCTRL_VOCAB_VAL_EVENT_ONGOING
Definition: CommonCartesianController.h:64
ClientCartesianController::getTrajTime
bool getTrajTime(double *t)
Definition: ClientCartesianController.cpp:937
ClientCartesianController::checkMotionDone
bool checkMotionDone(bool *f)
Definition: ClientCartesianController.cpp:1216
IKINCARTCTRL_VOCAB_CMD_EVENT
#define IKINCARTCTRL_VOCAB_CMD_EVENT
Definition: CommonCartesianController.h:33
IKINCARTCTRL_VOCAB_VAL_TRUE
#define IKINCARTCTRL_VOCAB_VAL_TRUE
Definition: CommonCartesianController.h:62
IKINCARTCTRL_VOCAB_OPT_ISSOLVERON
#define IKINCARTCTRL_VOCAB_OPT_ISSOLVERON
Definition: CommonCartesianController.h:51
iCub::action::log::info
@ info
Definition: actionPrimitives.cpp:64
ClientCartesianController::getPose
bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
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
iKinInv.h
IKINCARTCTRL_VOCAB_OPT_REST_POS
#define IKINCARTCTRL_VOCAB_OPT_REST_POS
Definition: CommonCartesianController.h:40
IKINCARTCTRL_VOCAB_CMD_DELETE
#define IKINCARTCTRL_VOCAB_CMD_DELETE
Definition: CommonCartesianController.h:29
ClientCartesianController::setTaskVelocities
bool setTaskVelocities(const yarp::sig::Vector &xdot, const yarp::sig::Vector &odot)
Definition: ClientCartesianController.cpp:1107
ClientCartesianController::getRestWeights
bool getRestWeights(yarp::sig::Vector &curRestWeights)
Definition: ClientCartesianController.cpp:815
IKINCARTCTRL_VOCAB_CMD_RESTORE
#define IKINCARTCTRL_VOCAB_CMD_RESTORE
Definition: CommonCartesianController.h:28
IKINCARTCTRL_VOCAB_CMD_GET
#define IKINCARTCTRL_VOCAB_CMD_GET
Definition: CommonCartesianController.h:24
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
ClientCartesianController::setTrajTime
bool setTrajTime(const double t)
Definition: ClientCartesianController.cpp:966
ClientCartesianController::getJointsVelocities
bool getJointsVelocities(yarp::sig::Vector &qdot)
Definition: ClientCartesianController.cpp:1037
IKINCARTCTRL_VOCAB_CMD_SET
#define IKINCARTCTRL_VOCAB_CMD_SET
Definition: CommonCartesianController.h:25
IKINCARTCTRL_VOCAB_OPT_XDOT
#define IKINCARTCTRL_VOCAB_OPT_XDOT
Definition: CommonCartesianController.h:47
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
ClientCartesianController::setRestWeights
bool setRestWeights(const yarp::sig::Vector &newRestWeights, yarp::sig::Vector &curRestWeights)
Definition: ClientCartesianController.cpp:848
ClientCartesianController::removeTipFrame
bool removeTipFrame()
Definition: ClientCartesianController.cpp:1209
IKINCARTCTRL_VOCAB_VAL_MODE_TRACK
#define IKINCARTCTRL_VOCAB_VAL_MODE_TRACK
Definition: CommonCartesianController.h:60
ClientCartesianController::close
bool close()
Definition: ClientCartesianController.cpp:180
ClientCartesianController::askForPosition
bool askForPosition(const yarp::sig::Vector &xd, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
yarp::dev
Definition: DebugInterfaces.h:52
ClientCartesianController::deleteContexts
bool deleteContexts()
Definition: ClientCartesianController.cpp:1363
ClientCartesianController::unregisterEvent
bool unregisterEvent(yarp::dev::CartesianEvent &event)
Definition: ClientCartesianController.cpp:1513
ClientCartesianController::getInTargetTol
bool getInTargetTol(double *tol)
Definition: ClientCartesianController.cpp:987
ClientCartesianController::goToPoseSync
bool goToPoseSync(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0)
Definition: ClientCartesianController.cpp:497
p
p
Definition: show_eyes_axes.m:23
ClientCartesianController::storeContext
bool storeContext(int *id)
Definition: ClientCartesianController.cpp:1289
IKINCTRL_POSE_FULL
#define IKINCTRL_POSE_FULL
Definition: iKinInv.h:37
ClientCartesianController
clientcartesiancontroller : implements the client part of the Cartesian Interface.
Definition: ClientCartesianController.h:63
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
IKINCARTCTRL_VOCAB_OPT_TOL
#define IKINCARTCTRL_VOCAB_OPT_TOL
Definition: CommonCartesianController.h:38
ClientCartesianController::getLimits
bool getLimits(const int axis, double *min, double *max)
Definition: ClientCartesianController.cpp:881
ClientCartesianController::getDesired
bool getDesired(yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
Definition: ClientCartesianController.cpp:551
CartesianEventHandler::setInterface
void setInterface(ClientCartesianController *interface)
Definition: ClientCartesianController.cpp:51
ClientCartesianController::goToPositionSync
bool goToPositionSync(const yarp::sig::Vector &xd, const double t=0.0)
Definition: ClientCartesianController.cpp:526
IKINCARTCTRL_VOCAB_CMD_STORE
#define IKINCARTCTRL_VOCAB_CMD_STORE
Definition: CommonCartesianController.h:27
IKINCARTCTRL_VOCAB_OPT_LIM
#define IKINCARTCTRL_VOCAB_OPT_LIM
Definition: CommonCartesianController.h:43
IKINCARTCTRL_VOCAB_VAL_MODE_SINGLE
#define IKINCARTCTRL_VOCAB_VAL_MODE_SINGLE
Definition: CommonCartesianController.h:61
mode
GLenum mode
Definition: rendering.cpp:48
ClientCartesianController::tweakSet
bool tweakSet(const yarp::os::Bottle &options)
Definition: ClientCartesianController.cpp:1549
ClientCartesianController::attachTipFrame
bool attachTipFrame(const yarp::sig::Vector &x, const yarp::sig::Vector &o)
Definition: ClientCartesianController.cpp:1132
ClientCartesianController::init
void init()
Definition: ClientCartesianController.cpp:75
ClientCartesianController::setRestPos
bool setRestPos(const yarp::sig::Vector &newRestPos, yarp::sig::Vector &curRestPos)
Definition: ClientCartesianController.cpp:782
ClientCartesianController::~ClientCartesianController
virtual ~ClientCartesianController()
Definition: ClientCartesianController.cpp:1601
CartesianEventHandler::onRead
void onRead(yarp::os::Bottle &event)
Definition: ClientCartesianController.cpp:43
IKINCARTCTRL_VOCAB_OPT_XD
#define IKINCARTCTRL_VOCAB_OPT_XD
Definition: CommonCartesianController.h:44
IKINCARTCTRL_VOCAB_OPT_PRIO
#define IKINCARTCTRL_VOCAB_OPT_PRIO
Definition: CommonCartesianController.h:36
ClientCartesianController::setDOF
bool setDOF(const yarp::sig::Vector &newDof, yarp::sig::Vector &curDof)
Definition: ClientCartesianController.cpp:712
ClientCartesianController::stopControl
bool stopControl()
Definition: ClientCartesianController.cpp:1270
x
x
Definition: compute_ekf_sym.m:21
state::ok
@ ok
IKINCARTCTRL_VOCAB_OPT_REST_WEIGHTS
#define IKINCARTCTRL_VOCAB_OPT_REST_WEIGHTS
Definition: CommonCartesianController.h:41
IKINCARTCTRL_VOCAB_VAL_POSE_XYZ
#define IKINCARTCTRL_VOCAB_VAL_POSE_XYZ
Definition: CommonCartesianController.h:59
IKINCARTCTRL_VOCAB_VAL_POSE_FULL
#define IKINCARTCTRL_VOCAB_VAL_POSE_FULL
Definition: CommonCartesianController.h:58
ClientCartesianController::setInTargetTol
bool setInTargetTol(const double tol)
Definition: ClientCartesianController.cpp:1016
ClientCartesianController::eventHandling
void eventHandling(yarp::os::Bottle &event)
Definition: ClientCartesianController.cpp:1425
ClientCartesianController::setTrackingMode
bool setTrackingMode(const bool f)
Definition: ClientCartesianController.cpp:206
ClientCartesianController::getTrackingMode
bool getTrackingMode(bool *f)
Definition: ClientCartesianController.cpp:228
IKINCARTCTRL_VOCAB_OPT_Q
#define IKINCARTCTRL_VOCAB_OPT_Q
Definition: CommonCartesianController.h:46
f
f
Definition: compute_ekf_sym.m:22
ClientCartesianController::getInfoHelper
bool getInfoHelper(yarp::os::Bottle &info)
Definition: ClientCartesianController.cpp:1387
ClientCartesianController.h
done
bool done
Definition: main.cpp:42
IKINCARTCTRL_VOCAB_OPT_MOTIONDONE
#define IKINCARTCTRL_VOCAB_OPT_MOTIONDONE
Definition: CommonCartesianController.h:50
ClientCartesianController::getPosePriority
bool getPosePriority(std::string &p)
Definition: ClientCartesianController.cpp:339
ClientCartesianController::setPosePriority
bool setPosePriority(const std::string &p)
Definition: ClientCartesianController.cpp:318
ClientCartesianController::getTipFrame
bool getTipFrame(yarp::sig::Vector &x, yarp::sig::Vector &o)
Definition: ClientCartesianController.cpp:1172
CommonCartesianController.h
v
static int v
Definition: iCub_Sim.cpp:42
ClientCartesianController::tweakGet
bool tweakGet(yarp::os::Bottle &options)
Definition: ClientCartesianController.cpp:1570
IKINCARTCTRL_VOCAB_REP_ACK
#define IKINCARTCTRL_VOCAB_REP_ACK
Definition: CommonCartesianController.h:65
ClientCartesianController::setLimits
bool setLimits(const int axis, const double min, const double max)
Definition: ClientCartesianController.cpp:913
ClientCartesianController::registerEvent
bool registerEvent(yarp::dev::CartesianEvent &event)
Definition: ClientCartesianController.cpp:1477
ClientCartesianController::getRestPos
bool getRestPos(yarp::sig::Vector &curRestPos)
Definition: ClientCartesianController.cpp:749
IKINCARTCTRL_VOCAB_OPT_DES
#define IKINCARTCTRL_VOCAB_OPT_DES
Definition: CommonCartesianController.h:42
ClientCartesianController::waitMotionDone
bool waitMotionDone(const double period=0.1, const double timeout=0.0)
Definition: ClientCartesianController.cpp:1251
ClientCartesianController::goToPose
bool goToPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0)
Definition: ClientCartesianController.cpp:447
IKINCARTCTRL_VOCAB_OPT_TIP_FRAME
#define IKINCARTCTRL_VOCAB_OPT_TIP_FRAME
Definition: CommonCartesianController.h:49
IKINCARTCTRL_VOCAB_CMD_STOP
#define IKINCARTCTRL_VOCAB_CMD_STOP
Definition: CommonCartesianController.h:32
ClientCartesianController::getReferenceMode
bool getReferenceMode(bool *f)
Definition: ClientCartesianController.cpp:284
IKINCARTCTRL_VOCAB_OPT_QDOT
#define IKINCARTCTRL_VOCAB_OPT_QDOT
Definition: CommonCartesianController.h:48
ClientCartesianController::goToPosition
bool goToPosition(const yarp::sig::Vector &xd, const double t=0.0)
Definition: ClientCartesianController.cpp:474
IKINCARTCTRL_VOCAB_CMD_ASK
#define IKINCARTCTRL_VOCAB_CMD_ASK
Definition: CommonCartesianController.h:26
time
degrees time
Definition: sine.m:5
ClientCartesianController::deleteContext
bool deleteContext(const int id)
Definition: ClientCartesianController.cpp:1337
ClientCartesianController::open
bool open(yarp::os::Searchable &config)
Definition: ClientCartesianController.cpp:90
IKINCARTCTRL_VOCAB_OPT_REFERENCE
#define IKINCARTCTRL_VOCAB_OPT_REFERENCE
Definition: CommonCartesianController.h:35
IKINCARTCTRL_VOCAB_OPT_DOF
#define IKINCARTCTRL_VOCAB_OPT_DOF
Definition: CommonCartesianController.h:39
ClientCartesianController::restoreContext
bool restoreContext(const int id)
Definition: ClientCartesianController.cpp:1317
IKINCARTCTRL_VOCAB_CMD_TASKVEL
#define IKINCARTCTRL_VOCAB_CMD_TASKVEL
Definition: CommonCartesianController.h:31
ClientCartesianController::setReferenceMode
bool setReferenceMode(const bool f)
Definition: ClientCartesianController.cpp:262
ClientCartesianController::getInfo
bool getInfo(yarp::os::Bottle &info)
Definition: ClientCartesianController.cpp:1415
ClientCartesianController::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)