YARP
Yet Another Robot Platform
RPCMessagesParser.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
4  * All rights reserved.
5  *
6  * This software may be modified and distributed under the terms of the
7  * BSD-3-Clause license. See the accompanying LICENSE file for details.
8  */
9 
10 #include "RPCMessagesParser.h"
11 #include "ControlBoardWrapper.h"
12 #include "ControlBoardWrapperLogComponent.h"
13 
14 #include <iostream>
15 #include <yarp/os/LogStream.h>
16 
17 using namespace yarp::os;
18 using namespace yarp::dev;
19 using namespace yarp::dev::impl;
20 using namespace yarp::sig;
21 using namespace std;
22 
23 
24 inline void appendTimeStamp(Bottle &bot, Stamp &st)
25 {
26  int count=st.getCount();
27  double time=st.getTime();
28  bot.addVocab(VOCAB_TIMESTAMP);
29  bot.addInt32(count);
30  bot.addFloat64(time);
31 }
32 
33 void RPCMessagesParser::handleProtocolVersionRequest(const yarp::os::Bottle& cmd,
34  yarp::os::Bottle& response, bool *rec, bool *ok)
35 {
36  if (cmd.get(0).asVocab()!=VOCAB_GET)
37  {
38  *rec=false;
39  *ok=false;
40  return;
41  }
42 
43  response.addVocab(VOCAB_PROTOCOL_VERSION);
44  response.addInt32(PROTOCOL_VERSION_MAJOR);
45  response.addInt32(PROTOCOL_VERSION_MINOR);
46  response.addInt32(PROTOCOL_VERSION_TWEAK);
47 
48  *rec=true;
49  *ok=true;
50 }
51 
52 void RPCMessagesParser::handleImpedanceMsg(const yarp::os::Bottle& cmd,
53  yarp::os::Bottle& response, bool *rec, bool *ok)
54 {
55  if (ControlBoardWrapper_p->verbose())
56  yCDebug(CONTROLBOARDWRAPPER, "Handling IImpedance message");
57  if (!rpc_IImpedance)
58  {
59  yCError(CONTROLBOARDWRAPPER, "controlBoardWrapper: I do not have a valid interface");
60  *ok=false;
61  return;
62  }
63 
64  int code = cmd.get(0).asVocab();
65  *ok=false;
66  switch(code)
67  {
68  case VOCAB_SET:
69  {
70  if (ControlBoardWrapper_p->verbose())
71  yCDebug(CONTROLBOARDWRAPPER, "handleImpedanceMsg::VOCAB_SET command");
72  switch (cmd.get(2).asVocab())
73  {
74  case VOCAB_IMP_PARAM:
75  {
76  Bottle *b = cmd.get(4).asList();
77  if (b!=nullptr)
78  {
79  double stiff = b->get(0).asFloat64();
80  double damp = b->get(1).asFloat64();
81  *ok = rpc_IImpedance->setImpedance(cmd.get(3).asInt32(),stiff,damp);
82  *rec=true;
83  }
84  }
85  break;
86  case VOCAB_IMP_OFFSET:
87  {
88  Bottle *b = cmd.get(4).asList();
89  if (b!=nullptr)
90  {
91  double offs = b->get(0).asFloat64();
92  *ok = rpc_IImpedance->setImpedanceOffset(cmd.get(3).asInt32(),offs);
93  *rec=true;
94  }
95  }
96  break;
97  }
98  }
99  break;
100  case VOCAB_GET:
101  {
102  double stiff = 0;
103  double damp = 0;
104  double offs = 0;
105  if (ControlBoardWrapper_p->verbose())
106  yCDebug(CONTROLBOARDWRAPPER, "handleImpedanceMsg::VOCAB_GET command");
107 
108  response.addVocab(VOCAB_IS);
109  response.add(cmd.get(1));
110  switch (cmd.get(2).asVocab())
111  {
112  case VOCAB_IMP_PARAM:
113  {
114  *ok = rpc_IImpedance->getImpedance(cmd.get(3).asInt32(),&stiff, &damp);
115  Bottle& b = response.addList();
116  b.addFloat64(stiff);
117  b.addFloat64(damp);
118  *rec=true;
119  }
120  break;
121  case VOCAB_IMP_OFFSET:
122  {
123  *ok = rpc_IImpedance->getImpedanceOffset(cmd.get(3).asInt32(),&offs);
124  Bottle& b = response.addList();
125  b.addFloat64(offs);
126  *rec=true;
127  }
128  break;
129  case VOCAB_LIMITS:
130  {
131  double min_stiff = 0;
132  double max_stiff = 0;
133  double min_damp = 0;
134  double max_damp = 0;
135  *ok = rpc_IImpedance->getCurrentImpedanceLimit(cmd.get(3).asInt32(),&min_stiff, &max_stiff, &min_damp, &max_damp);
136  Bottle& b = response.addList();
137  b.addFloat64(min_stiff);
138  b.addFloat64(max_stiff);
139  b.addFloat64(min_damp);
140  b.addFloat64(max_damp);
141  *rec=true;
142  }
143  break;
144  }
145  }
146  lastRpcStamp.update();
147  appendTimeStamp(response, lastRpcStamp);
148  break; // case VOCAB_GET
149  default:
150  {
151  *rec=false;
152  }
153  break;
154  }
155 }
156 
157 void RPCMessagesParser::handleControlModeMsg(const yarp::os::Bottle& cmd,
158  yarp::os::Bottle& response, bool *rec, bool *ok)
159 {
160  if (ControlBoardWrapper_p->verbose())
161  yCDebug(CONTROLBOARDWRAPPER, "Handling IControlMode message");
162  if (! (rpc_iCtrlMode))
163  {
164  yCError(CONTROLBOARDWRAPPER, "ControlBoardWrapper: I do not have a valid iControlMode interface");
165  *ok=false;
166  return;
167  }
168 
169  //handle here messages about IControlMode interface
170  int code = cmd.get(0).asVocab();
171  *ok=true;
172  *rec=true; //or false
173 
174  switch(code)
175  {
176  case VOCAB_SET:
177  {
178  if (ControlBoardWrapper_p->verbose())
179  yCDebug(CONTROLBOARDWRAPPER, "handleControlModeMsg::VOCAB_SET command");
180 
181  int method = cmd.get(2).asVocab();
182 
183  switch(method)
184  {
185  case VOCAB_CM_CONTROL_MODE:
186  {
187  int axis = cmd.get(3).asInt32();
188  yCTrace(CONTROLBOARDWRAPPER) << "got VOCAB_CM_CONTROL_MODE";
189  if(rpc_iCtrlMode)
190  *ok = rpc_iCtrlMode->setControlMode(axis, cmd.get(4).asVocab());
191  else
192  {
193  yCError(CONTROLBOARDWRAPPER) << "ControlBoardWrapper: Unable to handle setControlMode request! This should not happen!";
194  *rec = false;
195  }
196  }
197  break;
198 
199  case VOCAB_CM_CONTROL_MODE_GROUP:
200  {
201  int n_joints = cmd.get(3).asInt32();
202  Bottle& jList = *(cmd.get(4).asList());
203  Bottle& modeList= *(cmd.get(5).asList());
204 
205  int *js = new int [n_joints];
206  int *modes = new int [n_joints];
207 
208  for(int i=0; i<n_joints; i++)
209  {
210  js[i] = jList.get(i).asInt32();
211  }
212 
213  for(int i=0; i<n_joints; i++)
214  {
215  modes[i] = modeList.get(i).asVocab();
216  }
217  if(rpc_iCtrlMode)
218  *ok = rpc_iCtrlMode->setControlModes(n_joints, js, modes);
219  else
220  {
221  *rec = false;
222  *ok = false;
223  }
224  delete [] js;
225  delete [] modes;
226  }
227  break;
228 
229  case VOCAB_CM_CONTROL_MODES:
230  {
231  yarp::os::Bottle *modeList;
232  modeList = cmd.get(3).asList();
233 
234  if(modeList->size() != (size_t) controlledJoints)
235  {
236  yCError(CONTROLBOARDWRAPPER, "received an invalid setControlMode message. Size of vector doesn´t match the number of controlled joints");
237  *ok = false;
238  break;
239  }
240  int *modes = new int [controlledJoints];
241  for( int i=0; i<controlledJoints; i++)
242  {
243  modes[i] = modeList->get(i).asVocab();
244  }
245  if(rpc_iCtrlMode)
246  *ok = rpc_iCtrlMode->setControlModes(modes);
247  else
248  {
249  *rec = false;
250  *ok = false;
251  }
252  delete [] modes;
253  }
254  break;
255 
256  default:
257  {
258  // if I´m here, someone is probably sending command using the old interface.
259  // try to be compatible as much as I can
260 
261  yCError(CONTROLBOARDWRAPPER) << " Error, received a set control mode message using a legacy version, trying to be handle the message anyway "
262  << " but please update your client to be compatible with the IControlMode2 interface";
263 
264  yCTrace(CONTROLBOARDWRAPPER) << " cmd.get(4).asVocab() is " << Vocab::decode(cmd.get(4).asVocab());
265  int axis = cmd.get(3).asInt32();
266 
267  switch (cmd.get(4).asVocab())
268  {
269  case VOCAB_CM_POSITION:
270  if(rpc_iCtrlMode)
271  *ok = rpc_iCtrlMode->setControlMode(axis, VOCAB_CM_POSITION);
272  break;
273 
274  case VOCAB_CM_POSITION_DIRECT:
275  if(rpc_iCtrlMode)
276  *ok = rpc_iCtrlMode->setControlMode(axis, VOCAB_CM_POSITION_DIRECT);
277  else
278  {
279  *rec = false;
280  *ok = false;
281  }
282  break;
283 
284 
285  case VOCAB_CM_VELOCITY:
286  if(rpc_iCtrlMode)
287  *ok = rpc_iCtrlMode->setControlMode(axis, VOCAB_CM_VELOCITY);
288  break;
289 
290  case VOCAB_CM_TORQUE:
291  if(rpc_iCtrlMode)
292  *ok = rpc_iCtrlMode->setControlMode(axis, VOCAB_CM_TORQUE);
293  break;
294 
295  case VOCAB_CM_IMPEDANCE_POS:
296  yCError(CONTROLBOARDWRAPPER) << "The 'impedancePosition' control mode is deprecated. \nUse setInteractionMode(axis, VOCAB_IM_COMPLIANT) + setControlMode(axis, VOCAB_CM_POSITION) instead";
297  break;
298 
299  case VOCAB_CM_IMPEDANCE_VEL:
300  yCError(CONTROLBOARDWRAPPER) << "The 'impedanceVelocity' control mode is deprecated. \nUse setInteractionMode(axis, VOCAB_IM_COMPLIANT) + setControlMode(axis, VOCAB_CM_VELOCITY) instead";
301  break;
302 
303  case VOCAB_CM_PWM:
304  if (rpc_iCtrlMode)
305  *ok = rpc_iCtrlMode->setControlMode(axis, VOCAB_CM_PWM);
306  else
307  {
308  *rec = false;
309  *ok = false;
310  }
311  break;
312 
313  case VOCAB_CM_CURRENT:
314  if (rpc_iCtrlMode)
315  *ok = rpc_iCtrlMode->setControlMode(axis, VOCAB_CM_CURRENT);
316  else
317  {
318  *rec = false;
319  *ok = false;
320  }
321  break;
322 
323  case VOCAB_CM_MIXED:
324  if(rpc_iCtrlMode)
325  *ok = rpc_iCtrlMode->setControlMode(axis, VOCAB_CM_MIXED);
326  else
327  {
328  *rec = false;
329  *ok = false;
330  }
331  break;
332 
333  case VOCAB_CM_FORCE_IDLE:
334  if(rpc_iCtrlMode)
335  *ok = rpc_iCtrlMode->setControlMode(axis, VOCAB_CM_FORCE_IDLE);
336  else
337  {
338  *rec = false;
339  *ok = false;
340  }
341  break;
342 
343  default:
344  // if (ControlBoardWrapper_p->verbose())
345  yCError(CONTROLBOARDWRAPPER, "SET unknown controlMode : %s ", cmd.toString().c_str());
346  *ok = false;
347  *rec = false;
348  break;
349  }
350  }
351  break; // close default case
352  }
353  }
354  break; // close SET case
355 
356  case VOCAB_GET:
357  {
358  if (ControlBoardWrapper_p->verbose())
359  yCDebug(CONTROLBOARDWRAPPER, "GET command");
360 
361  int method = cmd.get(2).asVocab();
362 
363  switch(method)
364  {
365  case VOCAB_CM_CONTROL_MODES:
366  {
367  if (ControlBoardWrapper_p->verbose())
368  yCDebug(CONTROLBOARDWRAPPER, "getControlModes");
369  int *p = new int[controlledJoints];
370  for (int i = 0; i < controlledJoints; ++i) {
371  p[i] = -1;
372  }
373  if(rpc_iCtrlMode)
374  *ok = rpc_iCtrlMode->getControlModes(p);
375 
376  response.addVocab(VOCAB_IS);
377  response.addVocab(VOCAB_CM_CONTROL_MODES);
378 
379  Bottle& b = response.addList();
380  int i;
381  for (i = 0; i < controlledJoints; i++)
382  b.addVocab(p[i]);
383  delete[] p;
384 
385  *rec=true;
386  }
387  break;
388 
389  case VOCAB_CM_CONTROL_MODE:
390  {
391  if (ControlBoardWrapper_p->verbose())
392  yCDebug(CONTROLBOARDWRAPPER, "getControlMode");
393 
394  int p=-1;
395  int axis = cmd.get(3).asInt32();
396  if(rpc_iCtrlMode)
397  *ok = rpc_iCtrlMode->getControlMode(axis, &p);
398 
399  response.addVocab(VOCAB_IS);
400  response.addInt32(axis);
401  response.addVocab(p);
402 
403  yCTrace(CONTROLBOARDWRAPPER, "Returning %d", p);
404  *rec=true;
405  }
406  break;
407 
408  case VOCAB_CM_CONTROL_MODE_GROUP:
409  {
410  if (ControlBoardWrapper_p->verbose())
411  yCDebug(CONTROLBOARDWRAPPER, "getControlMode group");
412 
413  int n_joints = cmd.get(3).asInt32();
414  Bottle& lIn = *(cmd.get(4).asList());
415 
416  int *js = new int [n_joints];
417  int *modes = new int [n_joints];
418  for(int i=0; i<n_joints; i++)
419  {
420  js[i] = lIn.get(i).asInt32();
421  modes[i] = -1;
422  }
423  if(rpc_iCtrlMode)
424  *ok = rpc_iCtrlMode->getControlModes(n_joints, js, modes);
425  else
426  {
427  *rec = false;
428  *ok = false;
429  }
430 
431  response.addVocab(VOCAB_IS);
432  response.addVocab(VOCAB_CM_CONTROL_MODE_GROUP);
433  Bottle& b = response.addList();
434  for(int i=0; i<n_joints; i++)
435  {
436  b.addVocab(modes[i]);
437  }
438 
439  delete[] js;
440  delete[] modes;
441 
442  *rec=true;
443  }
444  break;
445 
446  default:
447  yCError(CONTROLBOARDWRAPPER, "received a GET ICONTROLMODE command not understood");
448  break;
449  }
450  }
451 
452  lastRpcStamp.update();
453  appendTimeStamp(response, lastRpcStamp);
454  break; // case VOCAB_GET
455 
456  default:
457  {
458  *rec=false;
459  }
460  break;
461  }
462 }
463 
464 
465 void RPCMessagesParser::handleTorqueMsg(const yarp::os::Bottle& cmd,
466  yarp::os::Bottle& response, bool *rec, bool *ok)
467 {
468  if (ControlBoardWrapper_p->verbose())
469  yCDebug(CONTROLBOARDWRAPPER, "Handling ITorqueControl message");
470 
471  if (!rpc_ITorque)
472  {
473  yCError(CONTROLBOARDWRAPPER, "Error, I do not have a valid ITorque interface");
474  *ok=false;
475  return;
476  }
477 
478  int code = cmd.get(0).asVocab();
479  switch (code)
480  {
481  case VOCAB_SET:
482  {
483  *rec = true;
484  if (ControlBoardWrapper_p->verbose())
485  yCDebug(CONTROLBOARDWRAPPER, "set command received");
486 
487  switch(cmd.get(2).asVocab())
488  {
489  case VOCAB_REF:
490  {
491  *ok = rpc_ITorque->setRefTorque(cmd.get(3).asInt32(), cmd.get(4).asFloat64());
492  }
493  break;
494 
495  case VOCAB_MOTOR_PARAMS:
496  {
497  yarp::dev::MotorTorqueParameters params;
498  int joint = cmd.get(3).asInt32();
499  Bottle *b = cmd.get(4).asList();
500 
501  if (b==nullptr)
502  break;
503 
504  if (b->size() != 4)
505  {
506  yCError(CONTROLBOARDWRAPPER, "received a SET VOCAB_MOTOR_PARAMS command not understood, size!=4");
507  break;
508  }
509 
510  params.bemf = b->get(0).asFloat64();
511  params.bemf_scale = b->get(1).asFloat64();
512  params.ktau = b->get(2).asFloat64();
513  params.ktau_scale = b->get(3).asFloat64();
514 
515  *ok = rpc_ITorque->setMotorTorqueParams(joint, params);
516  }
517  break;
518 
519  case VOCAB_REFS:
520  {
521  Bottle *b = cmd.get(3).asList();
522  if (b==nullptr)
523  break;
524 
525  int i;
526  const int njs = b->size();
527  if (njs==controlledJoints)
528  {
529  auto* p = new double[njs]; // LATER: optimize to avoid allocation.
530  for (i = 0; i < njs; i++)
531  p[i] = b->get(i).asFloat64();
532  *ok = rpc_ITorque->setRefTorques (p);
533  delete[] p;
534  }
535  }
536  break;
537 
538  case VOCAB_TORQUE_MODE:
539  {
540  if(rpc_iCtrlMode)
541  {
542  int *modes = new int[controlledJoints];
543  for(int i=0; i<controlledJoints; i++)
544  modes[i] = VOCAB_CM_TORQUE;
545  *ok = rpc_iCtrlMode->setControlModes(modes);
546  delete [] modes;
547  }
548  else
549  {
550  *ok = false;
551  }
552  }
553  break;
554 
555  }
556  }
557  break;
558 
559  case VOCAB_GET:
560  {
561  *rec = true;
562  if (ControlBoardWrapper_p->verbose())
563  yCDebug(CONTROLBOARDWRAPPER, "get command received");
564  double dtmp = 0.0;
565  double dtmp2 = 0.0;
566  response.addVocab(VOCAB_IS);
567  response.add(cmd.get(1));
568 
569  switch(cmd.get(2).asVocab())
570  {
571  case VOCAB_AXES:
572  {
573  int tmp;
574  *ok = rpc_ITorque->getAxes(&tmp);
575  response.addInt32(tmp);
576  }
577  break;
578 
579  case VOCAB_TRQ:
580  {
581  *ok = rpc_ITorque->getTorque(cmd.get(3).asInt32(), &dtmp);
582  response.addFloat64(dtmp);
583  }
584  break;
585 
586  case VOCAB_MOTOR_PARAMS:
587  {
588  yarp::dev::MotorTorqueParameters params;
589  int joint = cmd.get(3).asInt32();
590 
591  // get the data
592  *ok = rpc_ITorque->getMotorTorqueParams(joint, &params);
593 
594  // convert it back to yarp message
595  Bottle& b = response.addList();
596 
597  b.addFloat64(params.bemf);
598  b.addFloat64(params.bemf_scale);
599  b.addFloat64(params.ktau);
600  b.addFloat64(params.ktau_scale);
601  }
602  break;
603  case VOCAB_RANGE:
604  {
605  *ok = rpc_ITorque->getTorqueRange(cmd.get(3).asInt32(), &dtmp, &dtmp2);
606  response.addFloat64(dtmp);
607  response.addFloat64(dtmp2);
608  }
609  break;
610 
611  case VOCAB_TRQS:
612  {
613  int i=0;
614  auto* p = new double[controlledJoints];
615  *ok = rpc_ITorque->getTorques(p);
616  Bottle& b = response.addList();
617  for (i = 0; i < controlledJoints; i++)
618  b.addFloat64(p[i]);
619  delete[] p;
620  }
621  break;
622 
623  case VOCAB_RANGES:
624  {
625  auto* p1 = new double[controlledJoints];
626  auto* p2 = new double[controlledJoints];
627  *ok = rpc_ITorque->getTorqueRanges(p1,p2);
628  Bottle& b1 = response.addList();
629  int i;
630  for (i = 0; i < controlledJoints; i++)
631  b1.addFloat64(p1[i]);
632  Bottle& b2 = response.addList();
633  for (i = 0; i < controlledJoints; i++)
634  b2.addFloat64(p2[i]);
635  delete[] p1;
636  delete[] p2;
637  }
638  break;
639 
640  case VOCAB_REFERENCE:
641  {
642  *ok = rpc_ITorque->getRefTorque(cmd.get(3).asInt32(), &dtmp);
643  response.addFloat64(dtmp);
644  }
645  break;
646 
647  case VOCAB_REFERENCES:
648  {
649  auto* p = new double[controlledJoints];
650  *ok = rpc_ITorque->getRefTorques(p);
651  Bottle& b = response.addList();
652  int i;
653  for (i = 0; i < controlledJoints; i++)
654  b.addFloat64(p[i]);
655  delete[] p;
656  }
657  break;
658  }
659  }
660  lastRpcStamp.update();
661  appendTimeStamp(response, lastRpcStamp);
662  break; // case VOCAB_GET
663  }
664 }
665 
666 void RPCMessagesParser::handleInteractionModeMsg(const yarp::os::Bottle& cmd,
667  yarp::os::Bottle& response, bool *rec, bool *ok)
668 {
669  if (ControlBoardWrapper_p->verbose())
670  yCDebug(CONTROLBOARDWRAPPER, "\nHandling IInteractionMode message");
671  if (!rpc_IInteract)
672  {
673  yCError(CONTROLBOARDWRAPPER, "Error I do not have a valid IInteractionMode interface");
674  *ok=false;
675  return;
676  }
677 
678  if (ControlBoardWrapper_p->verbose())
679  {
680  yCDebug(CONTROLBOARDWRAPPER) << "received command: " << cmd.toString();
681  }
682 
683  int action = cmd.get(0).asVocab();
684 
685  switch(action)
686  {
687  case VOCAB_SET:
688  {
689  switch (cmd.get(2).asVocab())
690  {
691  yarp::os::Bottle *jointList;
692  yarp::os::Bottle *modeList;
693  yarp::dev::InteractionModeEnum *modes;
694 
695  case VOCAB_INTERACTION_MODE:
696  {
697  *ok = rpc_IInteract->setInteractionMode(cmd.get(3).asInt32(), (yarp::dev::InteractionModeEnum) cmd.get(4).asVocab());
698  }
699  break;
700 
701  case VOCAB_INTERACTION_MODE_GROUP:
702  {
703  yCTrace(CONTROLBOARDWRAPPER) << "CBW.h set interactionMode GROUP";
704 
705  int n_joints = cmd.get(3).asInt32();
706  jointList = cmd.get(4).asList();
707  modeList = cmd.get(5).asList();
708  if( (jointList->size() != (size_t) n_joints) || (modeList->size() != (size_t) n_joints) )
709  {
710  if (ControlBoardWrapper_p->verbose()) {
711  yCError(CONTROLBOARDWRAPPER, "Received an invalid setInteractionMode message. Size of vectors doesn´t match");
712  }
713  *ok = false;
714  break;
715  }
716  int *joints = new int[n_joints];
717  modes = new yarp::dev::InteractionModeEnum [n_joints];
718  for( int i=0; i<n_joints; i++)
719  {
720  joints[i] = jointList->get(i).asInt32();
721  modes[i] = (yarp::dev::InteractionModeEnum) modeList->get(i).asVocab();
722  yCTrace(CONTROLBOARDWRAPPER) << "CBW.cpp received vocab " << yarp::os::Vocab::decode(modes[i]);
723  }
724  *ok = rpc_IInteract->setInteractionModes(n_joints, joints, modes);
725  delete [] joints;
726  delete [] modes;
727 
728  }
729  break;
730 
731  case VOCAB_INTERACTION_MODES:
732  {
733  yCTrace(CONTROLBOARDWRAPPER) << "CBW.c set interactionMode ALL";
734 
735  modeList = cmd.get(3).asList();
736  if(modeList->size() != (size_t) controlledJoints)
737  {
738  if (ControlBoardWrapper_p->verbose())
739  yCError(CONTROLBOARDWRAPPER, "Received an invalid setInteractionMode message. Size of vector doesn´t match the number of controlled joints");
740  *ok = false;
741  break;
742  }
743  modes = new yarp::dev::InteractionModeEnum [controlledJoints];
744  for( int i=0; i<controlledJoints; i++)
745  {
746  modes[i] = (yarp::dev::InteractionModeEnum) modeList->get(i).asVocab();
747  }
748  *ok = rpc_IInteract->setInteractionModes(modes);
749  delete [] modes;
750  }
751  break;
752 
753  default:
754  {
755  if (ControlBoardWrapper_p->verbose())
756  yCError(CONTROLBOARDWRAPPER, "Error while Handling IInteractionMode message, SET command not understood %s", cmd.get(2).asString().c_str());
757  *ok = false;
758  }
759  break;
760  }
761  *rec=true; //or false
762  }
763  break;
764 
765  case VOCAB_GET:
766  {
767  yarp::os::Bottle *jointList;
768 
769  switch (cmd.get(2).asVocab())
770  {
771  case VOCAB_INTERACTION_MODE:
772  {
773  yarp::dev::InteractionModeEnum mode;
774  *ok = rpc_IInteract->getInteractionMode(cmd.get(3).asInt32(), &mode);
775  response.addVocab(mode);
776  if (ControlBoardWrapper_p->verbose()) yCDebug(CONTROLBOARDWRAPPER) << " resp is " << response.toString();
777  }
778  break;
779 
780  case VOCAB_INTERACTION_MODE_GROUP:
781  {
782  yarp::dev::InteractionModeEnum* modes;
783 
784  int n_joints = cmd.get(3).asInt32();
785  jointList = cmd.get(4).asList();
786  if(jointList->size() != (size_t) n_joints )
787  {
788  yCError(CONTROLBOARDWRAPPER, "Received an invalid getInteractionMode message. Size of vectors doesn´t match");
789  *ok = false;
790  break;
791  }
792  int *joints = new int[n_joints];
793  modes = new yarp::dev::InteractionModeEnum [n_joints];
794  for( int i=0; i<n_joints; i++)
795  {
796  joints[i] = jointList->get(i).asInt32();
797  }
798  *ok = rpc_IInteract->getInteractionModes(n_joints, joints, modes);
799 
800  Bottle& c = response.addList();
801  for( int i=0; i<n_joints; i++)
802  {
803  c.addVocab(modes[i]);
804  }
805 
806  if (ControlBoardWrapper_p->verbose())
807  {
808  yCDebug(CONTROLBOARDWRAPPER, "got response bottle");
809  response.toString();
810  }
811  delete [] joints;
812  delete [] modes;
813  }
814  break;
815 
816  case VOCAB_INTERACTION_MODES:
817  {
818  yarp::dev::InteractionModeEnum* modes;
819  modes = new yarp::dev::InteractionModeEnum [controlledJoints];
820 
821  *ok = rpc_IInteract->getInteractionModes(modes);
822 
823  Bottle& b = response.addList();
824  for( int i=0; i<controlledJoints; i++)
825  {
826  b.addVocab(modes[i]);
827  }
828  if (ControlBoardWrapper_p->verbose())
829  {
830  yCDebug(CONTROLBOARDWRAPPER, "got response bottle");
831  response.toString();
832  }
833  delete [] modes;
834  }
835  break;
836  }
837  lastRpcStamp.update();
838  appendTimeStamp(response, lastRpcStamp);
839  }
840  break; // case VOCAB_GET
841 
842  default:
843  yCError(CONTROLBOARDWRAPPER, "Error while Handling IInteractionMode message, command was not SET nor GET");
844  *ok = false;
845  break;
846 
847  }
848 }
849 
850 void RPCMessagesParser::handleCurrentMsg(const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool *rec, bool *ok)
851 {
852  if (ControlBoardWrapper_p->verbose())
853  yCDebug(CONTROLBOARDWRAPPER, "Handling ICurrentControl message");
854 
855  if (!rpc_ICurrent)
856  {
857  yCError(CONTROLBOARDWRAPPER, "controlBoardWrapper: I do not have a valid ICurrentControl interface");
858  *ok = false;
859  return;
860  }
861 
862  int code = cmd.get(0).asVocab();
863  int action = cmd.get(2).asVocab();
864 
865  *ok = false;
866  *rec = true;
867  switch (code)
868  {
869  case VOCAB_SET:
870  {
871  switch (action)
872  {
873  case VOCAB_CURRENT_REF:
874  {
875  yCError(CONTROLBOARDWRAPPER, "VOCAB_CURRENT_REF methods is implemented as streaming");
876  *ok = false;
877  }
878  break;
879 
880  case VOCAB_CURRENT_REFS:
881  {
882  yCError(CONTROLBOARDWRAPPER, "VOCAB_CURRENT_REFS methods is implemented as streaming");
883  *ok = false;
884  }
885  break;
886 
887  case VOCAB_CURRENT_REF_GROUP:
888  {
889  yCError(CONTROLBOARDWRAPPER, "VOCAB_CURRENT_REF_GROUP methods is implemented as streaming");
890  *ok = false;
891  }
892  break;
893 
894  default:
895  {
896  yCError(CONTROLBOARDWRAPPER) << "Unknown handleCurrentMsg message received";
897  *rec = false;
898  *ok = false;
899  }
900  break;
901  }
902  }
903  break;
904 
905  case VOCAB_GET:
906  {
907  *rec = true;
908  if (ControlBoardWrapper_p->verbose())
909  yCDebug(CONTROLBOARDWRAPPER, "get command received");
910  double dtmp = 0.0;
911  double dtmp2 = 0.0;
912  response.addVocab(VOCAB_IS);
913  response.add(cmd.get(1));
914 
915  switch (action)
916  {
917  case VOCAB_CURRENT_REF:
918  {
919  *ok = rpc_ICurrent->getRefCurrent(cmd.get(3).asInt32(), &dtmp);
920  response.addFloat64(dtmp);
921  }
922  break;
923 
924  case VOCAB_CURRENT_REFS:
925  {
926  auto* p = new double[controlledJoints];
927  *ok = rpc_ICurrent->getRefCurrents(p);
928  Bottle& b = response.addList();
929  int i;
930  for (i = 0; i < controlledJoints; i++)
931  b.addFloat64(p[i]);
932  delete[] p;
933  }
934  break;
935 
936  case VOCAB_CURRENT_RANGE:
937  {
938 
939  *ok = rpc_ICurrent->getCurrentRange(cmd.get(3).asInt32(), &dtmp, &dtmp2);
940  response.addFloat64(dtmp);
941  response.addFloat64(dtmp2);
942  }
943  break;
944 
945  case VOCAB_CURRENT_RANGES:
946  {
947  auto* p1 = new double[controlledJoints];
948  auto* p2 = new double[controlledJoints];
949  *ok = rpc_ICurrent->getCurrentRanges(p1,p2);
950  Bottle& b1 = response.addList();
951  Bottle& b2 = response.addList();
952  int i;
953  for (i = 0; i < controlledJoints; i++)
954  {
955  b1.addFloat64(p1[i]);
956  }
957  for (i = 0; i < controlledJoints; i++)
958  {
959  b2.addFloat64(p2[i]);
960  }
961  delete[] p1;
962  delete[] p2;
963  }
964  break;
965 
966  default:
967  {
968  yCError(CONTROLBOARDWRAPPER) << "Unknown handleCurrentMsg message received";
969  *rec = false;
970  *ok = false;
971  }
972  break;
973  }
974  }
975  break;
976 
977  default:
978  {
979  yCError(CONTROLBOARDWRAPPER) << "Unknown handleCurrentMsg message received";
980  *rec = false;
981  *ok = false;
982  }
983  break;
984  }
985 }
986 
987 void RPCMessagesParser::handlePidMsg(const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool *rec, bool *ok)
988 {
989  if (ControlBoardWrapper_p->verbose())
990  yCDebug(CONTROLBOARDWRAPPER, "Handling IPidControl message");
991 
992  if (!rpc_IPid)
993  {
994  yCError(CONTROLBOARDWRAPPER, "controlBoardWrapper: I do not have a valid IPidControl interface");
995  *ok = false;
996  return;
997  }
998 
999  int code = cmd.get(0).asVocab();
1000  int action = cmd.get(2).asVocab();
1001  auto pidtype = static_cast<yarp::dev::PidControlTypeEnum>(cmd.get(3).asVocab());
1002 
1003  *ok = false;
1004  *rec = true;
1005  switch (code)
1006  {
1007  case VOCAB_SET:
1008  {
1009  *rec = true;
1010  if (ControlBoardWrapper_p->verbose())
1011  yCDebug(CONTROLBOARDWRAPPER, "set command received");
1012 
1013  switch(action)
1014  {
1015  case VOCAB_OFFSET:
1016  {
1017  double v;
1018  int j = cmd.get(4).asInt32();
1019  v=cmd.get(5).asFloat64();
1020  *ok = rpc_IPid->setPidOffset(pidtype, j, v);
1021  }
1022  break;
1023 
1024  case VOCAB_PID:
1025  {
1026  Pid p;
1027  int j = cmd.get(4).asInt32();
1028  Bottle *b = cmd.get(5).asList();
1029 
1030  if (b==nullptr)
1031  break;
1032 
1033  p.kp = b->get(0).asFloat64();
1034  p.kd = b->get(1).asFloat64();
1035  p.ki = b->get(2).asFloat64();
1036  p.max_int = b->get(3).asFloat64();
1037  p.max_output = b->get(4).asFloat64();
1038  p.offset = b->get(5).asFloat64();
1039  p.scale = b->get(6).asFloat64();
1040  p.stiction_up_val = b->get(7).asFloat64();
1041  p.stiction_down_val = b->get(8).asFloat64();
1042  p.kff = b->get(9).asFloat64();
1043  *ok = rpc_IPid->setPid(pidtype, j, p);
1044  }
1045  break;
1046 
1047  case VOCAB_PIDS:
1048  {
1049  Bottle *b = cmd.get(4).asList();
1050 
1051  if (b==nullptr)
1052  break;
1053 
1054  int i;
1055  const int njs = b->size();
1056  if (njs==controlledJoints)
1057  {
1058  Pid *p = new Pid[njs];
1059 
1060  bool allOK=true;
1061 
1062  for (i = 0; i < njs; i++)
1063  {
1064  Bottle *c = b->get(i).asList();
1065  if (c!=nullptr)
1066  {
1067  p[i].kp = c->get(0).asFloat64();
1068  p[i].kd = c->get(1).asFloat64();
1069  p[i].ki = c->get(2).asFloat64();
1070  p[i].max_int = c->get(3).asFloat64();
1071  p[i].max_output = c->get(4).asFloat64();
1072  p[i].offset = c->get(5).asFloat64();
1073  p[i].scale = c->get(6).asFloat64();
1074  p[i].stiction_up_val = c->get(7).asFloat64();
1075  p[i].stiction_down_val = c->get(8).asFloat64();
1076  p[i].kff = c->get(9).asFloat64();
1077  }
1078  else
1079  {
1080  allOK=false;
1081  }
1082  }
1083  if (allOK)
1084  *ok = rpc_IPid->setPids(pidtype, p);
1085  else
1086  *ok=false;
1087 
1088  delete[] p;
1089  }
1090  }
1091  break;
1092 
1093  case VOCAB_REF:
1094  {
1095  *ok = rpc_IPid->setPidReference (pidtype, cmd.get(4).asInt32(), cmd.get(5).asFloat64());
1096  }
1097  break;
1098 
1099  case VOCAB_REFS:
1100  {
1101  Bottle *b = cmd.get(4).asList();
1102 
1103  if (b==nullptr)
1104  break;
1105 
1106  int i;
1107  const int njs = b->size();
1108  if (njs==controlledJoints)
1109  {
1110  auto* p = new double[njs]; // LATER: optimize to avoid allocation.
1111  for (i = 0; i < njs; i++)
1112  p[i] = b->get(i).asFloat64();
1113  *ok = rpc_IPid->setPidReferences (pidtype, p);
1114  delete[] p;
1115  }
1116  }
1117  break;
1118 
1119  case VOCAB_LIM:
1120  {
1121  *ok = rpc_IPid->setPidErrorLimit (pidtype, cmd.get(4).asInt32(), cmd.get(5).asFloat64());
1122  }
1123  break;
1124 
1125  case VOCAB_LIMS:
1126  {
1127  Bottle *b = cmd.get(4).asList();
1128  int i;
1129 
1130  if (b==nullptr)
1131  break;
1132 
1133  const int njs = b->size();
1134  if (njs==controlledJoints)
1135  {
1136  auto* p = new double[njs]; // LATER: optimize to avoid allocation.
1137  for (i = 0; i < njs; i++)
1138  p[i] = b->get(i).asFloat64();
1139  *ok = rpc_IPid->setPidErrorLimits (pidtype, p);
1140  delete[] p;
1141  }
1142  }
1143  break;
1144 
1145  case VOCAB_RESET:
1146  {
1147  *ok = rpc_IPid->resetPid (pidtype, cmd.get(4).asInt32());
1148  }
1149  break;
1150 
1151  case VOCAB_DISABLE:
1152  {
1153  *ok = rpc_IPid->disablePid (pidtype, cmd.get(4).asInt32());
1154  }
1155  break;
1156 
1157  case VOCAB_ENABLE:
1158  {
1159  *ok = rpc_IPid->enablePid (pidtype, cmd.get(4).asInt32());
1160  }
1161  break;
1162  }
1163  }
1164  break;
1165 
1166  case VOCAB_GET:
1167  {
1168  *rec = true;
1169  if (ControlBoardWrapper_p->verbose())
1170  yCDebug(CONTROLBOARDWRAPPER, "get command received");
1171  double dtmp = 0.0;
1172  response.addVocab(VOCAB_IS);
1173  response.add(cmd.get(1));
1174 
1175  switch (action)
1176  {
1177  case VOCAB_LIMS:
1178  {
1179  auto* p = new double[controlledJoints];
1180  *ok = rpc_IPid->getPidErrorLimits(pidtype, p);
1181  Bottle& b = response.addList();
1182  int i;
1183  for (i = 0; i < controlledJoints; i++)
1184  b.addFloat64(p[i]);
1185  delete[] p;
1186  }
1187  break;
1188 
1189  case VOCAB_ENABLE:
1190  {
1191  bool booltmp=false;
1192  *ok = rpc_IPid->isPidEnabled(pidtype, cmd.get(4).asInt32(), &booltmp);
1193  response.addInt32(booltmp);
1194  }
1195  break;
1196 
1197  case VOCAB_ERR:
1198  {
1199  *ok = rpc_IPid->getPidError(pidtype, cmd.get(4).asInt32(), &dtmp);
1200  response.addFloat64(dtmp);
1201  }
1202  break;
1203 
1204  case VOCAB_ERRS:
1205  {
1206  auto* p = new double[controlledJoints];
1207  *ok = rpc_IPid->getPidErrors(pidtype, p);
1208  Bottle& b = response.addList();
1209  int i;
1210  for (i = 0; i < controlledJoints; i++)
1211  b.addFloat64(p[i]);
1212  delete[] p;
1213  }
1214  break;
1215 
1216  case VOCAB_OUTPUT:
1217  {
1218  *ok = rpc_IPid->getPidOutput(pidtype, cmd.get(4).asInt32(), &dtmp);
1219  response.addFloat64(dtmp);
1220  }
1221  break;
1222 
1223  case VOCAB_OUTPUTS:
1224  {
1225  auto* p = new double[controlledJoints];
1226  *ok = rpc_IPid->getPidOutputs(pidtype, p);
1227  Bottle& b = response.addList();
1228  int i;
1229  for (i = 0; i < controlledJoints; i++)
1230  b.addFloat64(p[i]);
1231  delete[] p;
1232  }
1233  break;
1234 
1235  case VOCAB_PID:
1236  {
1237  Pid p;
1238  *ok = rpc_IPid->getPid(pidtype, cmd.get(4).asInt32(), &p);
1239  Bottle& b = response.addList();
1240  b.addFloat64(p.kp);
1241  b.addFloat64(p.kd);
1242  b.addFloat64(p.ki);
1243  b.addFloat64(p.max_int);
1244  b.addFloat64(p.max_output);
1245  b.addFloat64(p.offset);
1246  b.addFloat64(p.scale);
1247  b.addFloat64(p.stiction_up_val);
1248  b.addFloat64(p.stiction_down_val);
1249  b.addFloat64(p.kff);
1250  }
1251  break;
1252 
1253  case VOCAB_PIDS:
1254  {
1255  Pid *p = new Pid[controlledJoints];
1256  *ok = rpc_IPid->getPids(pidtype, p);
1257  Bottle& b = response.addList();
1258  int i;
1259  for (i = 0; i < controlledJoints; i++)
1260  {
1261  Bottle& c = b.addList();
1262  c.addFloat64(p[i].kp);
1263  c.addFloat64(p[i].kd);
1264  c.addFloat64(p[i].ki);
1265  c.addFloat64(p[i].max_int);
1266  c.addFloat64(p[i].max_output);
1267  c.addFloat64(p[i].offset);
1268  c.addFloat64(p[i].scale);
1269  c.addFloat64(p[i].stiction_up_val);
1270  c.addFloat64(p[i].stiction_down_val);
1271  c.addFloat64(p[i].kff);
1272  }
1273  delete[] p;
1274  }
1275  break;
1276 
1277  case VOCAB_REFERENCE:
1278  {
1279  *ok = rpc_IPid->getPidReference(pidtype, cmd.get(4).asInt32(), &dtmp);
1280  response.addFloat64(dtmp);
1281  }
1282  break;
1283 
1284  case VOCAB_REFERENCES:
1285  {
1286  auto* p = new double[controlledJoints];
1287  *ok = rpc_IPid->getPidReferences(pidtype, p);
1288  Bottle& b = response.addList();
1289  int i;
1290  for (i = 0; i < controlledJoints; i++)
1291  b.addFloat64(p[i]);
1292  delete[] p;
1293  }
1294  break;
1295 
1296  case VOCAB_LIM:
1297  {
1298  *ok = rpc_IPid->getPidErrorLimit(pidtype, cmd.get(4).asInt32(), &dtmp);
1299  response.addFloat64(dtmp);
1300  }
1301  break;
1302  }
1303  }
1304  break;
1305 
1306  default:
1307  {
1308  yCError(CONTROLBOARDWRAPPER) << "Unknown handlePWMMsg message received";
1309  *rec = false;
1310  *ok = false;
1311  }
1312  break;
1313  }
1314 }
1315 
1316 void RPCMessagesParser::handlePWMMsg(const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool *rec, bool *ok)
1317 {
1318  if (ControlBoardWrapper_p->verbose())
1319  yCDebug(CONTROLBOARDWRAPPER, "Handling IPWMControl message");
1320 
1321  if (!rpc_IPWM)
1322  {
1323  yCError(CONTROLBOARDWRAPPER, "controlBoardWrapper: I do not have a valid IPWMControl interface");
1324  *ok = false;
1325  return;
1326  }
1327 
1328  int code = cmd.get(0).asVocab();
1329  int action = cmd.get(2).asVocab();
1330 
1331  *ok = false;
1332  *rec = true;
1333  switch (code)
1334  {
1335  case VOCAB_SET:
1336  {
1337  *rec = true;
1338  if (ControlBoardWrapper_p->verbose())
1339  yCDebug(CONTROLBOARDWRAPPER, "set command received");
1340 
1341  switch (action)
1342  {
1343  case VOCAB_PWMCONTROL_REF_PWM:
1344  {
1345  //handled as streaming!
1346  yCError(CONTROLBOARDWRAPPER) << "VOCAB_PWMCONTROL_REF_PWM handled as straming";
1347  *ok = false;
1348  }
1349  break;
1350 
1351  default:
1352  {
1353  yCError(CONTROLBOARDWRAPPER) << "Unknown handlePWMMsg message received";
1354  *ok = false;
1355  }
1356  break;
1357  }
1358  }
1359  break;
1360 
1361  case VOCAB_GET:
1362  {
1363  *rec = true;
1364  if (ControlBoardWrapper_p->verbose())
1365  yCDebug(CONTROLBOARDWRAPPER, "get command received");
1366  double dtmp = 0.0;
1367  response.addVocab(VOCAB_IS);
1368  response.add(cmd.get(1));
1369 
1370  switch (action)
1371  {
1372  case VOCAB_PWMCONTROL_REF_PWM:
1373  {
1374  *ok = rpc_IPWM->getRefDutyCycle(cmd.get(3).asInt32(), &dtmp);
1375  response.addFloat64(dtmp);
1376  }
1377  break;
1378 
1379  case VOCAB_PWMCONTROL_REF_PWMS:
1380  {
1381  auto* p = new double[controlledJoints];
1382  *ok = rpc_IPWM->getRefDutyCycles(p);
1383  Bottle& b = response.addList();
1384  int i;
1385  for (i = 0; i < controlledJoints; i++)
1386  b.addFloat64(p[i]);
1387  delete[] p;
1388  }
1389  break;
1390 
1391  case VOCAB_PWMCONTROL_PWM_OUTPUT:
1392  {
1393  *ok = rpc_IPWM->getDutyCycle(cmd.get(3).asInt32(), &dtmp);
1394  response.addFloat64(dtmp);
1395  }
1396  break;
1397 
1398  case VOCAB_PWMCONTROL_PWM_OUTPUTS:
1399  {
1400  auto* p = new double[controlledJoints];
1401  *ok = rpc_IPWM->getRefDutyCycles(p);
1402  Bottle& b = response.addList();
1403  int i;
1404  for (i = 0; i < controlledJoints; i++)
1405  b.addFloat64(p[i]);
1406  delete[] p;
1407  }
1408  break;
1409 
1410  default:
1411  {
1412  yCError(CONTROLBOARDWRAPPER) << "Unknown handlePWMMsg message received";
1413  *ok = false;
1414  }
1415  break;
1416  }
1417  }
1418  break;
1419 
1420  default:
1421  {
1422  yCError(CONTROLBOARDWRAPPER) << "Unknown handlePWMMsg message received";
1423  *rec = false;
1424  *ok = false;
1425  }
1426  break;
1427  }
1428 }
1429 
1430 void RPCMessagesParser::handleRemoteVariablesMsg(const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool *rec, bool *ok)
1431 {
1432  if (ControlBoardWrapper_p->verbose())
1433  yCDebug(CONTROLBOARDWRAPPER, "Handling IRemoteCalibrator message");
1434 
1435  if (!rpc_IRemoteCalibrator)
1436  {
1437  yCError(CONTROLBOARDWRAPPER, "controlBoardWrapper: I do not have a valid IRemoteCalibrator interface");
1438  *ok = false;
1439  return;
1440  }
1441 
1442  int code = cmd.get(0).asVocab();
1443  int action = cmd.get(2).asVocab();
1444 
1445  *ok = false;
1446  *rec = true;
1447  switch (code)
1448  {
1449  case VOCAB_SET:
1450  {
1451  switch (action)
1452  {
1453  case VOCAB_VARIABLE:
1454  {
1455  Bottle btail = cmd.tail().tail().tail().tail(); // remove the first four elements
1456  string s = btail.toString();
1457  *ok = rpc_IVar->setRemoteVariable(cmd.get(3).asString(), btail);
1458  }
1459  break;
1460 
1461  default:
1462  {
1463  *rec = false;
1464  *ok = false;
1465  } break;
1466  }
1467  }
1468  break;
1469 
1470  case VOCAB_GET:
1471  {
1472  response.clear();
1473  response.addVocab(VOCAB_IS);
1474  response.add(cmd.get(1));
1475  Bottle btmp;
1476 
1477  if (ControlBoardWrapper_p->verbose())
1478  yCDebug(CONTROLBOARDWRAPPER, "get command received");
1479 
1480  switch (action)
1481  {
1482  case VOCAB_VARIABLE:
1483  {
1484  *ok = rpc_IVar->getRemoteVariable(cmd.get(3).asString(), btmp);
1485  Bottle& b = response.addList();
1486  b = btmp;
1487  }
1488  break;
1489 
1490  case VOCAB_LIST_VARIABLES:
1491  {
1492  *ok = rpc_IVar->getRemoteVariablesList(&btmp);
1493  Bottle& b = response.addList();
1494  b = btmp;
1495  }
1496  break;
1497  }
1498  }
1499  } //end get/set switch
1500 }
1501 
1502 void RPCMessagesParser::handleRemoteCalibratorMsg(const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool *rec, bool *ok)
1503 {
1504  if(ControlBoardWrapper_p->verbose())
1505  yCDebug(CONTROLBOARDWRAPPER, "Handling IRemoteCalibrator message");
1506 
1507  if (!rpc_IRemoteCalibrator)
1508  {
1509  yCError(CONTROLBOARDWRAPPER, "controlBoardWrapper: I do not have a valid IRemoteCalibrator interface");
1510  *ok=false;
1511  return;
1512  }
1513 
1514  int code = cmd.get(0).asVocab();
1515  int action = cmd.get(2).asVocab();
1516 
1517  *ok=false;
1518  *rec=true;
1519  switch(code)
1520  {
1521  case VOCAB_SET:
1522  {
1523  switch(action)
1524  {
1525  case VOCAB_CALIBRATE_SINGLE_JOINT:
1526  {
1527  yCDebug(CONTROLBOARDWRAPPER) << "cmd is " << cmd.toString() << " joint is " << cmd.get(3).asInt32();
1528  if (ControlBoardWrapper_p->verbose())
1529  yCDebug(CONTROLBOARDWRAPPER, "Calling calibrate joint with no parameter");
1530  *ok = rpc_IRemoteCalibrator->calibrateSingleJoint(cmd.get(3).asInt32());
1531  } break;
1532 
1533  case VOCAB_CALIBRATE_WHOLE_PART:
1534  {
1535  if (ControlBoardWrapper_p->verbose())
1536  yCDebug(CONTROLBOARDWRAPPER, "Calling calibrate whole part");
1537  *ok = rpc_IRemoteCalibrator->calibrateWholePart();
1538  } break;
1539 
1540  case VOCAB_HOMING_SINGLE_JOINT:
1541  {
1542  if (ControlBoardWrapper_p->verbose())
1543  yCDebug(CONTROLBOARDWRAPPER, "Calling calibrate joint with no parameter");
1544  *ok = rpc_IRemoteCalibrator->homingSingleJoint(cmd.get(3).asInt32());
1545  } break;
1546 
1547  case VOCAB_HOMING_WHOLE_PART:
1548  {
1549  yCDebug(CONTROLBOARDWRAPPER) << "Received homing whole part";
1550  if (ControlBoardWrapper_p->verbose())
1551  yCDebug(CONTROLBOARDWRAPPER, "Calling calibrate whole part");
1552  *ok = rpc_IRemoteCalibrator->homingWholePart();
1553  } break;
1554 
1555  case VOCAB_PARK_SINGLE_JOINT:
1556  {
1557  if (ControlBoardWrapper_p->verbose())
1558  yCDebug(CONTROLBOARDWRAPPER, "Calling calibrate joint with no parameter");
1559  *ok = rpc_IRemoteCalibrator->parkSingleJoint(cmd.get(3).asInt32());
1560  } break;
1561 
1562  case VOCAB_PARK_WHOLE_PART:
1563  {
1564  if (ControlBoardWrapper_p->verbose())
1565  yCDebug(CONTROLBOARDWRAPPER, "Calling calibrate whole part");
1566  *ok = rpc_IRemoteCalibrator->parkWholePart();
1567  } break;
1568 
1569  case VOCAB_QUIT_CALIBRATE:
1570  {
1571  if (ControlBoardWrapper_p->verbose())
1572  yCDebug(CONTROLBOARDWRAPPER, "Calling quit calibrate");
1573  *ok = rpc_IRemoteCalibrator->quitCalibrate();
1574  } break;
1575 
1576  case VOCAB_QUIT_PARK:
1577  {
1578  if (ControlBoardWrapper_p->verbose())
1579  yCDebug(CONTROLBOARDWRAPPER, "Calling quit park");
1580  *ok = rpc_IRemoteCalibrator->quitPark();
1581  } break;
1582 
1583  default:
1584  {
1585  *rec = false;
1586  *ok = false;
1587  } break;
1588  }
1589  }break;
1590 
1591  case VOCAB_GET:
1592  {
1593  response.clear();
1594  response.addVocab(VOCAB_IS);
1595  response.add(cmd.get(1));
1596 
1597  switch(action)
1598  {
1599  case VOCAB_IS_CALIBRATOR_PRESENT:
1600  {
1601  bool tmp;
1602  if (ControlBoardWrapper_p->verbose())
1603  yCDebug(CONTROLBOARDWRAPPER, "Calling VOCAB_IS_CALIBRATOR_PRESENT");
1604  *ok = rpc_IRemoteCalibrator->isCalibratorDevicePresent(&tmp);
1605  response.addInt32(tmp);
1606  } break;
1607  }
1608  }
1609  } //end get/set switch
1610 }
1611 
1612 
1613 // rpc callback
1614 bool RPCMessagesParser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& response)
1615 {
1616  bool ok = false;
1617  bool rec = false; // Tells if the command is recognized!
1618 
1619  if (ControlBoardWrapper_p->verbose())
1620  yCDebug(CONTROLBOARDWRAPPER, "command received: %s", cmd.toString().c_str());
1621 
1622  int code = cmd.get(0).asVocab();
1623 
1624  if(cmd.size() < 2)
1625  {
1626  ok = false;
1627  }
1628  else
1629  {
1630  switch (cmd.get(1).asVocab())
1631  {
1632  case VOCAB_PID:
1633  handlePidMsg(cmd, response, &rec, &ok);
1634  break;
1635 
1636  case VOCAB_TORQUE:
1637  handleTorqueMsg(cmd, response, &rec, &ok);
1638  break;
1639 
1640  case VOCAB_ICONTROLMODE:
1641  handleControlModeMsg(cmd, response, &rec, &ok);
1642  break;
1643 
1644  case VOCAB_IMPEDANCE:
1645  handleImpedanceMsg(cmd, response, &rec, &ok);
1646  break;
1647 
1648  case VOCAB_INTERFACE_INTERACTION_MODE:
1649  handleInteractionModeMsg(cmd, response, &rec, &ok);
1650  break;
1651 
1652  case VOCAB_PROTOCOL_VERSION:
1653  handleProtocolVersionRequest(cmd, response, &rec, &ok);
1654  break;
1655 
1656  case VOCAB_REMOTE_CALIBRATOR_INTERFACE:
1657  handleRemoteCalibratorMsg(cmd, response, &rec, &ok);
1658  break;
1659 
1660  case VOCAB_REMOTE_VARIABILE_INTERFACE:
1661  handleRemoteVariablesMsg(cmd, response, &rec, &ok);
1662  break;
1663 
1664  case VOCAB_CURRENTCONTROL_INTERFACE:
1665  handleCurrentMsg(cmd, response, &rec, &ok);
1666  break;
1667 
1668  case VOCAB_PWMCONTROL_INTERFACE:
1669  handlePWMMsg(cmd, response, &rec, &ok);
1670  break;
1671 
1672  default:
1673  // fallback for old interfaces with no specific name
1674  switch (code)
1675  {
1676  case VOCAB_CALIBRATE_JOINT:
1677  {
1678  rec=true;
1679  if (ControlBoardWrapper_p->verbose())
1680  yCDebug(CONTROLBOARDWRAPPER, "Calling calibrate joint");
1681 
1682  int j=cmd.get(1).asInt32();
1683  int ui=cmd.get(2).asInt32();
1684  double v1=cmd.get(3).asFloat64();
1685  double v2=cmd.get(4).asFloat64();
1686  double v3=cmd.get(5).asFloat64();
1687  if (rpc_Icalib==nullptr)
1688  yCError(CONTROLBOARDWRAPPER, "Sorry I don't have a IControlCalibration2 interface");
1689  else
1690  ok=rpc_Icalib->calibrateAxisWithParams(j,ui,v1,v2,v3);
1691  }
1692  break;
1693 
1694  case VOCAB_CALIBRATE_JOINT_PARAMS:
1695  {
1696  rec = true;
1697  if (ControlBoardWrapper_p->verbose())
1698  yCDebug(CONTROLBOARDWRAPPER, "Calling calibrate joint");
1699 
1700  int j = cmd.get(1).asInt32();
1701  CalibrationParameters params;
1702  params.type = cmd.get(2).asInt32();
1703  params.param1 = cmd.get(3).asFloat64();
1704  params.param2 = cmd.get(4).asFloat64();
1705  params.param3 = cmd.get(5).asFloat64();
1706  params.param4 = cmd.get(6).asFloat64();
1707  if (rpc_Icalib == nullptr)
1708  yCError(CONTROLBOARDWRAPPER, "Sorry I don't have a IControlCalibration2 interface");
1709  else
1710  ok = rpc_Icalib->setCalibrationParameters(j, params);
1711  }
1712  break;
1713 
1714  case VOCAB_CALIBRATE:
1715  {
1716  rec=true;
1717  if (ControlBoardWrapper_p->verbose())
1718  yCDebug(CONTROLBOARDWRAPPER, "Calling calibrate");
1719  ok=rpc_Icalib->calibrateRobot();
1720  }
1721  break;
1722 
1723  case VOCAB_CALIBRATE_DONE:
1724  {
1725  rec=true;
1726  if (ControlBoardWrapper_p->verbose())
1727  yCDebug(CONTROLBOARDWRAPPER, "Calling calibrate done");
1728  int j=cmd.get(1).asInt32();
1729  ok=rpc_Icalib->calibrationDone(j);
1730  }
1731  break;
1732 
1733  case VOCAB_PARK:
1734  {
1735  rec=true;
1736  if (ControlBoardWrapper_p->verbose())
1737  yCDebug(CONTROLBOARDWRAPPER, "Calling park function");
1738  int flag=cmd.get(1).asInt32();
1739  if (flag)
1740  ok=rpc_Icalib->park(true);
1741  else
1742  ok=rpc_Icalib->park(false);
1743  ok=true; //client would get stuck if returning false
1744  }
1745  break;
1746 
1747  case VOCAB_SET:
1748  {
1749  rec = true;
1750  if (ControlBoardWrapper_p->verbose())
1751  yCDebug(CONTROLBOARDWRAPPER, "set command received");
1752 
1753  switch(cmd.get(1).asVocab())
1754  {
1755  case VOCAB_POSITION_MOVE:
1756  {
1757  ok = rpc_IPosCtrl->positionMove(cmd.get(2).asInt32(), cmd.get(3).asFloat64());
1758  }
1759  break;
1760 
1761  // this operation is also available on "command" port
1762  case VOCAB_POSITION_MOVES:
1763  {
1764  Bottle *b = cmd.get(2).asList();
1765  int i;
1766  if (b==nullptr)
1767  break;
1768  const int njs = b->size();
1769  if (njs!=controlledJoints)
1770  break;
1771  tmpVect.resize(njs);
1772  for (i = 0; i < njs; i++)
1773  tmpVect[i] = b->get(i).asFloat64();
1774 
1775  if (rpc_IPosCtrl!=nullptr)
1776  ok = rpc_IPosCtrl->positionMove(&tmpVect[0]);
1777  }
1778  break;
1779 
1780  case VOCAB_POSITION_MOVE_GROUP:
1781  {
1782  int len = cmd.get(2).asInt32();
1783  Bottle *jlut = cmd.get(3).asList();
1784  Bottle *pos_val= cmd.get(4).asList();
1785 
1786  if (rpc_IPosCtrl == nullptr)
1787  break;
1788 
1789  if (jlut==nullptr || pos_val==nullptr)
1790  break;
1791  if ((size_t) len!=jlut->size() || (size_t) len!=pos_val->size())
1792  break;
1793 
1794  int *j_tmp=new int [len];
1795  auto* pos_tmp=new double [len];
1796 
1797  for (int i = 0; i < len; i++)
1798  j_tmp[i] = jlut->get(i).asInt32();
1799 
1800  for (int i = 0; i < len; i++)
1801  pos_tmp[i] = pos_val->get(i).asFloat64();
1802 
1803  ok = rpc_IPosCtrl->positionMove(len, j_tmp, pos_tmp);
1804 
1805  delete [] j_tmp;
1806  delete [] pos_tmp;
1807  }
1808  break;
1809 
1810  // this operation is also available on "command" port
1811  case VOCAB_VELOCITY_MOVES:
1812  {
1813  Bottle *b = cmd.get(2).asList();
1814  int i;
1815  if (b==nullptr)
1816  break;
1817  const int njs = b->size();
1818  if (njs!=controlledJoints)
1819  break;
1820  tmpVect.resize(njs);
1821  for (i = 0; i < njs; i++)
1822  tmpVect[i] = b->get(i).asFloat64();
1823  if (rpc_IVelCtrl!=nullptr)
1824  ok = rpc_IVelCtrl->velocityMove(&tmpVect[0]);
1825 
1826  }
1827  break;
1828 
1829  case VOCAB_RELATIVE_MOVE:
1830  {
1831  ok = rpc_IPosCtrl->relativeMove(cmd.get(2).asInt32(), cmd.get(3).asFloat64());
1832  }
1833  break;
1834 
1835  case VOCAB_RELATIVE_MOVE_GROUP:
1836  {
1837  int len = cmd.get(2).asInt32();
1838  Bottle *jBottle_p = cmd.get(3).asList();
1839  Bottle *posBottle_p= cmd.get(4).asList();
1840 
1841  if (rpc_IPosCtrl == nullptr)
1842  break;
1843 
1844  if (jBottle_p==nullptr || posBottle_p==nullptr)
1845  break;
1846  if ((size_t) len!=jBottle_p->size() || (size_t) len!=posBottle_p->size())
1847  break;
1848 
1849  int *j_tmp=new int [len];
1850  auto* pos_tmp=new double [len];
1851 
1852  for (int i = 0; i < len; i++)
1853  j_tmp[i] = jBottle_p->get(i).asInt32();
1854 
1855  for (int i = 0; i < len; i++)
1856  pos_tmp[i] = posBottle_p->get(i).asFloat64();
1857 
1858  ok = rpc_IPosCtrl->relativeMove(len, j_tmp, pos_tmp);
1859 
1860  delete [] j_tmp;
1861  delete [] pos_tmp;
1862  }
1863  break;
1864 
1865  case VOCAB_RELATIVE_MOVES:
1866  {
1867  Bottle *b = cmd.get(2).asList();
1868 
1869  if (b==nullptr)
1870  break;
1871 
1872  int i;
1873  const int njs = b->size();
1874  if(njs!=controlledJoints)
1875  break;
1876  auto* p = new double[njs]; // LATER: optimize to avoid allocation.
1877  for (i = 0; i < njs; i++)
1878  p[i] = b->get(i).asFloat64();
1879  ok = rpc_IPosCtrl->relativeMove(p);
1880  delete[] p;
1881  }
1882  break;
1883 
1884  case VOCAB_REF_SPEED:
1885  {
1886  ok = rpc_IPosCtrl->setRefSpeed(cmd.get(2).asInt32(), cmd.get(3).asFloat64());
1887  }
1888  break;
1889 
1890  case VOCAB_REF_SPEED_GROUP:
1891  {
1892  int len = cmd.get(2).asInt32();
1893  Bottle *jBottle_p = cmd.get(3).asList();
1894  Bottle *velBottle_p= cmd.get(4).asList();
1895 
1896  if (rpc_IPosCtrl == nullptr)
1897  break;
1898 
1899  if (jBottle_p==nullptr || velBottle_p==nullptr)
1900  break;
1901  if ((size_t) len!=jBottle_p->size() || (size_t) len!=velBottle_p->size())
1902  break;
1903 
1904  int *j_tmp=new int [len];
1905  auto* spds_tmp=new double [len];
1906 
1907  for (int i = 0; i < len; i++)
1908  j_tmp[i] = jBottle_p->get(i).asInt32();
1909 
1910  for (int i = 0; i < len; i++)
1911  spds_tmp[i] = velBottle_p->get(i).asFloat64();
1912 
1913  ok = rpc_IPosCtrl->setRefSpeeds(len, j_tmp, spds_tmp);
1914  delete[] j_tmp;
1915  delete[] spds_tmp;
1916  }
1917  break;
1918 
1919  case VOCAB_REF_SPEEDS:
1920  {
1921  Bottle *b = cmd.get(2).asList();
1922 
1923  if (b==nullptr)
1924  break;
1925 
1926  int i;
1927  const int njs = b->size();
1928  if (njs!=controlledJoints)
1929  break;
1930  auto* p = new double[njs]; // LATER: optimize to avoid allocation.
1931  for (i = 0; i < njs; i++)
1932  p[i] = b->get(i).asFloat64();
1933  ok = rpc_IPosCtrl->setRefSpeeds(p);
1934  delete[] p;
1935  }
1936  break;
1937 
1938  case VOCAB_REF_ACCELERATION:
1939  {
1940  ok = rpc_IPosCtrl->setRefAcceleration(cmd.get(2).asInt32(), cmd.get(3).asFloat64());
1941  }
1942  break;
1943 
1944  case VOCAB_REF_ACCELERATION_GROUP:
1945  {
1946  int len = cmd.get(2).asInt32();
1947  Bottle *jBottle_p = cmd.get(3).asList();
1948  Bottle *accBottle_p = cmd.get(4).asList();
1949 
1950  if (rpc_IPosCtrl == nullptr)
1951  break;
1952 
1953  if (jBottle_p==nullptr || accBottle_p==nullptr)
1954  break;
1955  if ((size_t) len!=jBottle_p->size() || (size_t) len!=accBottle_p->size())
1956  break;
1957 
1958  int *j_tmp = new int [len];
1959  auto* accs_tmp = new double [len];
1960 
1961  for (int i = 0; i < len; i++)
1962  j_tmp[i] = jBottle_p->get(i).asInt32();
1963 
1964  for (int i = 0; i < len; i++)
1965  accs_tmp[i] = accBottle_p->get(i).asFloat64();
1966 
1967  ok = rpc_IPosCtrl->setRefAccelerations(len, j_tmp, accs_tmp);
1968  delete[] j_tmp;
1969  delete[] accs_tmp;
1970  }
1971  break;
1972 
1973  case VOCAB_REF_ACCELERATIONS:
1974  {
1975  Bottle *b = cmd.get(2).asList();
1976 
1977  if (b==nullptr)
1978  break;
1979 
1980  int i;
1981  const int njs = b->size();
1982  if(njs!=controlledJoints)
1983  break;
1984  auto* p = new double[njs]; // LATER: optimize to avoid allocation.
1985  for (i = 0; i < njs; i++)
1986  p[i] = b->get(i).asFloat64();
1987  ok = rpc_IPosCtrl->setRefAccelerations(p);
1988  delete[] p;
1989  }
1990  break;
1991 
1992  case VOCAB_STOP:
1993  {
1994  ok = rpc_IPosCtrl->stop(cmd.get(2).asInt32());
1995  }
1996  break;
1997 
1998  case VOCAB_STOP_GROUP:
1999  {
2000  int len = cmd.get(2).asInt32();
2001  Bottle *jBottle_p = cmd.get(3).asList();
2002 
2003  if (rpc_IPosCtrl == nullptr)
2004  break;
2005 
2006  if (jBottle_p==nullptr)
2007  break;
2008  if ((size_t) len!=jBottle_p->size())
2009  break;
2010 
2011  int *j_tmp = new int [len];
2012 
2013  for (int i = 0; i < len; i++)
2014  j_tmp[i] = jBottle_p->get(i).asInt32();
2015 
2016  ok = rpc_IPosCtrl->stop(len, j_tmp);
2017  delete[] j_tmp;
2018  }
2019  break;
2020 
2021  case VOCAB_STOPS:
2022  {
2023  ok = rpc_IPosCtrl->stop();
2024  }
2025  break;
2026 
2027  case VOCAB_E_RESET:
2028  {
2029  ok = rpc_IEncTimed->resetEncoder(cmd.get(2).asInt32());
2030  }
2031  break;
2032 
2033  case VOCAB_E_RESETS:
2034  {
2035  ok = rpc_IEncTimed->resetEncoders();
2036  }
2037  break;
2038 
2039  case VOCAB_ENCODER:
2040  {
2041  ok = rpc_IEncTimed->setEncoder(cmd.get(2).asInt32(), cmd.get(3).asFloat64());
2042  }
2043  break;
2044 
2045  case VOCAB_ENCODERS:
2046  {
2047  Bottle *b = cmd.get(2).asList();
2048 
2049  if (b==nullptr)
2050  break;
2051 
2052  int i;
2053  const int njs = b->size();
2054  if (njs!=controlledJoints)
2055  break;
2056  auto* p = new double[njs]; // LATER: optimize to avoid allocation.
2057  for (i = 0; i < njs; i++)
2058  p[i] = b->get(i).asFloat64();
2059  ok = rpc_IEncTimed->setEncoders(p);
2060  delete[] p;
2061  }
2062  break;
2063 
2064  case VOCAB_MOTOR_CPR:
2065  {
2066  ok = rpc_IMotEnc->setMotorEncoderCountsPerRevolution(cmd.get(2).asInt32(), cmd.get(3).asFloat64());
2067  }
2068  break;
2069 
2070  case VOCAB_MOTOR_E_RESET:
2071  {
2072  ok = rpc_IMotEnc->resetMotorEncoder(cmd.get(2).asInt32());
2073  }
2074  break;
2075 
2076  case VOCAB_MOTOR_E_RESETS:
2077  {
2078  ok = rpc_IMotEnc->resetMotorEncoders();
2079  }
2080  break;
2081 
2082  case VOCAB_MOTOR_ENCODER:
2083  {
2084  ok = rpc_IMotEnc->setMotorEncoder(cmd.get(2).asInt32(), cmd.get(3).asFloat64());
2085  }
2086  break;
2087 
2088  case VOCAB_MOTOR_ENCODERS:
2089  {
2090  Bottle *b = cmd.get(2).asList();
2091 
2092  if (b==nullptr)
2093  break;
2094 
2095  int i;
2096  const int njs = b->size();
2097  if (njs!=controlledJoints)
2098  break;
2099  auto* p = new double[njs]; // LATER: optimize to avoid allocation.
2100  for (i = 0; i < njs; i++)
2101  p[i] = b->get(i).asFloat64();
2102  ok = rpc_IMotEnc->setMotorEncoders(p);
2103  delete[] p;
2104  }
2105  break;
2106 
2107  case VOCAB_AMP_ENABLE:
2108  {
2109  ok = rcp_IAmp->enableAmp(cmd.get(2).asInt32());
2110  }
2111  break;
2112 
2113  case VOCAB_AMP_DISABLE:
2114  {
2115  ok = rcp_IAmp->disableAmp(cmd.get(2).asInt32());
2116  }
2117  break;
2118 
2119  case VOCAB_AMP_MAXCURRENT:
2120  {
2121  ok = rcp_IAmp->setMaxCurrent(cmd.get(2).asInt32(), cmd.get(3).asFloat64());
2122  }
2123  break;
2124 
2125  case VOCAB_AMP_PEAK_CURRENT:
2126  {
2127  ok = rcp_IAmp->setPeakCurrent(cmd.get(2).asInt32(), cmd.get(3).asFloat64());
2128  }
2129  break;
2130 
2131  case VOCAB_AMP_NOMINAL_CURRENT:
2132  {
2133  ok = rcp_IAmp->setNominalCurrent(cmd.get(2).asInt32(), cmd.get(3).asFloat64());
2134  }
2135  break;
2136 
2137  case VOCAB_AMP_PWM_LIMIT:
2138  {
2139  ok = rcp_IAmp->setPWMLimit(cmd.get(2).asInt32(), cmd.get(3).asFloat64());
2140  }
2141  break;
2142 
2143  case VOCAB_LIMITS:
2144  {
2145  ok = rcp_Ilim->setLimits(cmd.get(2).asInt32(), cmd.get(3).asFloat64(), cmd.get(4).asFloat64());
2146  }
2147  break;
2148 
2149 
2150  case VOCAB_TEMPERATURE_LIMIT:
2151  {
2152  ok = rpc_IMotor->setTemperatureLimit(cmd.get(2).asInt32(), cmd.get(3).asFloat64());
2153  }
2154  break;
2155 
2156  case VOCAB_GEARBOX_RATIO:
2157  {
2158  ok = rpc_IMotor->setGearboxRatio(cmd.get(2).asInt32(), cmd.get(3).asFloat64());
2159  }
2160  break;
2161 
2162  case VOCAB_VEL_LIMITS:
2163  {
2164  ok = rcp_Ilim->setVelLimits(cmd.get(2).asInt32(), cmd.get(3).asFloat64(), cmd.get(4).asFloat64());
2165  }
2166  break;
2167 
2168  default:
2169  {
2170  yCError(CONTROLBOARDWRAPPER, "received an unknown command after a VOCAB_SET (%s)", cmd.toString().c_str());
2171  }
2172  break;
2173  } //switch(cmd.get(1).asVocab()
2174  break;
2175  }
2176 
2177  case VOCAB_GET:
2178  {
2179  rec = true;
2180  if (ControlBoardWrapper_p->verbose())
2181  yCDebug(CONTROLBOARDWRAPPER, "get command received");
2182 
2183  double dtmp = 0.0;
2184  Bottle btmp;
2185  response.addVocab(VOCAB_IS);
2186  response.add(cmd.get(1));
2187 
2188  switch(cmd.get(1).asVocab())
2189  {
2190 
2191  case VOCAB_TEMPERATURE_LIMIT:
2192  {
2193  ok = rpc_IMotor->getTemperatureLimit(cmd.get(2).asInt32(), &dtmp);
2194  response.addFloat64(dtmp);
2195  }
2196  break;
2197 
2198  case VOCAB_TEMPERATURE:
2199  {
2200  ok = rpc_IMotor->getTemperature(cmd.get(2).asInt32(), &dtmp);
2201  response.addFloat64(dtmp);
2202  }
2203  break;
2204 
2205  case VOCAB_GEARBOX_RATIO:
2206  {
2207  ok = rpc_IMotor->getGearboxRatio(cmd.get(2).asInt32(), &dtmp);
2208  response.addFloat64(dtmp);
2209  }
2210  break;
2211 
2212  case VOCAB_TEMPERATURES:
2213  {
2214  auto* p = new double[controlledJoints];
2215  ok = rpc_IMotor->getTemperatures(p);
2216  Bottle& b = response.addList();
2217  int i;
2218  for (i = 0; i < controlledJoints; i++)
2219  b.addFloat64(p[i]);
2220  delete[] p;
2221  }
2222  break;
2223 
2224  case VOCAB_AMP_MAXCURRENT:
2225  {
2226  ok = rcp_IAmp->getMaxCurrent(cmd.get(2).asInt32(), &dtmp);
2227  response.addFloat64(dtmp);
2228  }
2229  break;
2230 
2231  case VOCAB_POSITION_MOVE:
2232  {
2233  if (ControlBoardWrapper_p->verbose())
2234  yCDebug(CONTROLBOARDWRAPPER, "getTargetPosition");
2235 
2236  ok = rpc_IPosCtrl->getTargetPosition(cmd.get(2).asInt32(), &dtmp);
2237 
2238  response.addFloat64(dtmp);
2239  rec=true;
2240  }
2241  break;
2242 
2243  case VOCAB_POSITION_MOVE_GROUP:
2244  {
2245  int len = cmd.get(2).asInt32();
2246  Bottle& in = *(cmd.get(3).asList());
2247  int *jointList = new int[len];
2248  auto* refs = new double[len];
2249 
2250  for(int j=0; j<len; j++)
2251  {
2252  jointList[j] = in.get(j).asInt32();
2253  }
2254  ok = rpc_IPosCtrl->getTargetPositions(len, jointList, refs);
2255 
2256  Bottle& b = response.addList();
2257  for (int i = 0; i < len; i++)
2258  b.addFloat64(refs[i]);
2259 
2260  delete[] jointList;
2261  delete[] refs;
2262  }
2263  break;
2264 
2265  case VOCAB_POSITION_MOVES:
2266  {
2267  auto* refs = new double[controlledJoints];
2268  ok = rpc_IPosCtrl->getTargetPositions(refs);
2269  Bottle& b = response.addList();
2270  int i;
2271  for (i = 0; i < controlledJoints; i++)
2272  b.addFloat64(refs[i]);
2273  delete[] refs;
2274  }
2275  break;
2276 
2277  case VOCAB_POSITION_DIRECT:
2278  {
2279  if (ControlBoardWrapper_p->verbose())
2280  yCDebug(CONTROLBOARDWRAPPER, "getRefPosition");
2281 
2282  ok = rpc_IPosDirect->getRefPosition(cmd.get(2).asInt32(), &dtmp);
2283 
2284  response.addFloat64(dtmp);
2285  rec=true;
2286  }
2287  break;
2288 
2289  case VOCAB_POSITION_DIRECT_GROUP:
2290  {
2291  int len = cmd.get(2).asInt32();
2292  Bottle& in = *(cmd.get(3).asList());
2293  int *jointList = new int[len];
2294  auto* refs = new double[len];
2295 
2296  for(int j=0; j<len; j++)
2297  {
2298  jointList[j] = in.get(j).asInt32();
2299  }
2300  ok = rpc_IPosDirect->getRefPositions(len, jointList, refs);
2301 
2302  Bottle& b = response.addList();
2303  for (int i = 0; i < len; i++)
2304  b.addFloat64(refs[i]);
2305 
2306  delete[] jointList;
2307  delete[] refs;
2308  }
2309  break;
2310 
2311  case VOCAB_POSITION_DIRECTS:
2312  {
2313  auto* refs = new double[controlledJoints];
2314  ok = rpc_IPosDirect->getRefPositions(refs);
2315  Bottle& b = response.addList();
2316  int i;
2317  for (i = 0; i < controlledJoints; i++)
2318  b.addFloat64(refs[i]);
2319  delete[] refs;
2320  }
2321  break;
2322 
2323  case VOCAB_VELOCITY_MOVE:
2324  {
2325  if (ControlBoardWrapper_p->verbose())
2326  yCDebug(CONTROLBOARDWRAPPER, "getVelocityMove - cmd: %s", cmd.toString().c_str());
2327 
2328  ok = rpc_IVelCtrl->getRefVelocity(cmd.get(2).asInt32(), &dtmp);
2329 
2330  response.addFloat64(dtmp);
2331  rec=true;
2332  }
2333  break;
2334 
2335  case VOCAB_VELOCITY_MOVE_GROUP:
2336  {
2337  if (ControlBoardWrapper_p->verbose())
2338  yCDebug(CONTROLBOARDWRAPPER, "getVelocityMove_group - cmd: %s", cmd.toString().c_str());
2339 
2340  int len = cmd.get(2).asInt32();
2341  Bottle& in = *(cmd.get(3).asList());
2342  int *jointList = new int[len];
2343  auto* refs = new double[len];
2344 
2345  for(int j=0; j<len; j++)
2346  {
2347  jointList[j] = in.get(j).asInt32();
2348  }
2349  ok = rpc_IVelCtrl->getRefVelocities(len, jointList, refs);
2350 
2351  Bottle& b = response.addList();
2352  for (int i = 0; i < len; i++)
2353  b.addFloat64(refs[i]);
2354 
2355  delete[] jointList;
2356  delete[] refs;
2357  }
2358  break;
2359 
2360  case VOCAB_VELOCITY_MOVES:
2361  {
2362  if (ControlBoardWrapper_p->verbose())
2363  yCDebug(CONTROLBOARDWRAPPER, "getVelocityMoves - cmd: %s", cmd.toString().c_str());
2364 
2365  auto* refs = new double[controlledJoints];
2366  ok = rpc_IVelCtrl->getRefVelocities(refs);
2367  Bottle& b = response.addList();
2368  int i;
2369  for (i = 0; i < controlledJoints; i++)
2370  b.addFloat64(refs[i]);
2371  delete[] refs;
2372  }
2373  break;
2374 
2375  case VOCAB_MOTORS_NUMBER:
2376  {
2377  int tmp;
2378  ok = rpc_IMotor->getNumberOfMotors(&tmp);
2379  response.addInt32(tmp);
2380  }
2381  break;
2382 
2383  case VOCAB_AXES:
2384  {
2385  int tmp;
2386  ok = rpc_IPosCtrl->getAxes(&tmp);
2387  response.addInt32(tmp);
2388  }
2389  break;
2390 
2391  case VOCAB_MOTION_DONE:
2392  {
2393  bool x = false;;
2394  ok = rpc_IPosCtrl->checkMotionDone(cmd.get(2).asInt32(), &x);
2395  response.addInt32(x);
2396  }
2397  break;
2398 
2399  case VOCAB_MOTION_DONE_GROUP:
2400  {
2401  bool x = false;
2402  int len = cmd.get(2).asInt32();
2403  Bottle& in = *(cmd.get(3).asList());
2404  int *jointList = new int[len];
2405  for(int j=0; j<len; j++)
2406  {
2407  jointList[j] = in.get(j).asInt32();
2408  }
2409  if(rpc_IPosCtrl!=nullptr)
2410  ok = rpc_IPosCtrl->checkMotionDone(len, jointList, &x);
2411  response.addInt32(x);
2412 
2413  delete[] jointList;
2414  }
2415  break;
2416 
2417  case VOCAB_MOTION_DONES:
2418  {
2419  bool x = false;
2420  ok = rpc_IPosCtrl->checkMotionDone(&x);
2421  response.addInt32(x);
2422  }
2423  break;
2424 
2425  case VOCAB_REF_SPEED:
2426  {
2427  ok = rpc_IPosCtrl->getRefSpeed(cmd.get(2).asInt32(), &dtmp);
2428  response.addFloat64(dtmp);
2429  }
2430  break;
2431 
2432  case VOCAB_REF_SPEED_GROUP:
2433  {
2434  int len = cmd.get(2).asInt32();
2435  Bottle& in = *(cmd.get(3).asList());
2436  int *jointList = new int[len];
2437  auto* speeds = new double[len];
2438 
2439  for(int j=0; j<len; j++)
2440  {
2441  jointList[j] = in.get(j).asInt32();
2442  }
2443  ok = rpc_IPosCtrl->getRefSpeeds(len, jointList, speeds);
2444 
2445  Bottle& b = response.addList();
2446  for (int i = 0; i < len; i++)
2447  b.addFloat64(speeds[i]);
2448 
2449  delete[] jointList;
2450  delete[] speeds;
2451  }
2452  break;
2453 
2454  case VOCAB_REF_SPEEDS:
2455  {
2456  auto* p = new double[controlledJoints];
2457  ok = rpc_IPosCtrl->getRefSpeeds(p);
2458  Bottle& b = response.addList();
2459  int i;
2460  for (i = 0; i < controlledJoints; i++)
2461  b.addFloat64(p[i]);
2462  delete[] p;
2463  }
2464  break;
2465 
2466  case VOCAB_REF_ACCELERATION:
2467  {
2468  ok = rpc_IPosCtrl->getRefAcceleration(cmd.get(2).asInt32(), &dtmp);
2469  response.addFloat64(dtmp);
2470  }
2471  break;
2472 
2473  case VOCAB_REF_ACCELERATION_GROUP:
2474  {
2475  int len = cmd.get(2).asInt32();
2476  Bottle& in = *(cmd.get(3).asList());
2477  int *jointList = new int[len];
2478  auto* accs = new double[len];
2479 
2480  for(int j=0; j<len; j++)
2481  {
2482  jointList[j] = in.get(j).asInt32();
2483  }
2484  ok = rpc_IPosCtrl->getRefAccelerations(len, jointList, accs);
2485 
2486  Bottle& b = response.addList();
2487  for (int i = 0; i < len; i++)
2488  b.addFloat64(accs[i]);
2489 
2490  delete[] jointList;
2491  delete[] accs;
2492  }
2493  break;
2494 
2495  case VOCAB_REF_ACCELERATIONS:
2496  {
2497  auto* p = new double[controlledJoints];
2498  ok = rpc_IPosCtrl->getRefAccelerations(p);
2499  Bottle& b = response.addList();
2500  int i;
2501  for (i = 0; i < controlledJoints; i++)
2502  b.addFloat64(p[i]);
2503  delete[] p;
2504  }
2505  break;
2506 
2507  case VOCAB_ENCODER:
2508  {
2509  ok = rpc_IEncTimed->getEncoder(cmd.get(2).asInt32(), &dtmp);
2510  response.addFloat64(dtmp);
2511  }
2512  break;
2513 
2514  case VOCAB_ENCODERS:
2515  {
2516  auto* p = new double[controlledJoints];
2517  ok = rpc_IEncTimed->getEncoders(p);
2518  Bottle& b = response.addList();
2519  int i;
2520  for (i = 0; i < controlledJoints; i++)
2521  b.addFloat64(p[i]);
2522  delete[] p;
2523  }
2524  break;
2525 
2526  case VOCAB_ENCODER_SPEED:
2527  {
2528  ok = rpc_IEncTimed->getEncoderSpeed(cmd.get(2).asInt32(), &dtmp);
2529  response.addFloat64(dtmp);
2530  }
2531  break;
2532 
2533  case VOCAB_ENCODER_SPEEDS:
2534  {
2535  auto* p = new double[controlledJoints];
2536  ok = rpc_IEncTimed->getEncoderSpeeds(p);
2537  Bottle& b = response.addList();
2538  int i;
2539  for (i = 0; i < controlledJoints; i++)
2540  b.addFloat64(p[i]);
2541  delete[] p;
2542  }
2543  break;
2544 
2545  case VOCAB_ENCODER_ACCELERATION:
2546  {
2547  ok = rpc_IEncTimed->getEncoderAcceleration(cmd.get(2).asInt32(), &dtmp);
2548  response.addFloat64(dtmp);
2549  }
2550  break;
2551 
2552  case VOCAB_ENCODER_ACCELERATIONS:
2553  {
2554  auto* p = new double[controlledJoints];
2555  ok = rpc_IEncTimed->getEncoderAccelerations(p);
2556  Bottle& b = response.addList();
2557  int i;
2558  for (i = 0; i < controlledJoints; i++)
2559  b.addFloat64(p[i]);
2560  delete[] p;
2561  }
2562  break;
2563 
2564  case VOCAB_MOTOR_CPR:
2565  {
2566  ok = rpc_IMotEnc->getMotorEncoderCountsPerRevolution(cmd.get(2).asInt32(), &dtmp);
2567  response.addFloat64(dtmp);
2568  }
2569  break;
2570 
2571  case VOCAB_MOTOR_ENCODER:
2572  {
2573  ok = rpc_IMotEnc->getMotorEncoder(cmd.get(2).asInt32(), &dtmp);
2574  response.addFloat64(dtmp);
2575  }
2576  break;
2577 
2578  case VOCAB_MOTOR_ENCODERS:
2579  {
2580  auto* p = new double[controlledJoints];
2581  ok = rpc_IMotEnc->getMotorEncoders(p);
2582  Bottle& b = response.addList();
2583  int i;
2584  for (i = 0; i < controlledJoints; i++)
2585  b.addFloat64(p[i]);
2586  delete[] p;
2587  }
2588  break;
2589 
2590  case VOCAB_MOTOR_ENCODER_SPEED:
2591  {
2592  ok = rpc_IMotEnc->getMotorEncoderSpeed(cmd.get(2).asInt32(), &dtmp);
2593  response.addFloat64(dtmp);
2594  }
2595  break;
2596 
2597  case VOCAB_MOTOR_ENCODER_SPEEDS:
2598  {
2599  auto* p = new double[controlledJoints];
2600  ok = rpc_IMotEnc->getMotorEncoderSpeeds(p);
2601  Bottle& b = response.addList();
2602  int i;
2603  for (i = 0; i < controlledJoints; i++)
2604  b.addFloat64(p[i]);
2605  delete[] p;
2606  }
2607  break;
2608 
2609  case VOCAB_MOTOR_ENCODER_ACCELERATION:
2610  {
2611  ok = rpc_IMotEnc->getMotorEncoderAcceleration(cmd.get(2).asInt32(), &dtmp);
2612  response.addFloat64(dtmp);
2613  }
2614  break;
2615 
2616  case VOCAB_MOTOR_ENCODER_ACCELERATIONS:
2617  {
2618  auto* p = new double[controlledJoints];
2619  ok = rpc_IMotEnc->getMotorEncoderAccelerations(p);
2620  Bottle& b = response.addList();
2621  int i;
2622  for (i = 0; i < controlledJoints; i++)
2623  b.addFloat64(p[i]);
2624  delete[] p;
2625  }
2626  break;
2627 
2628  case VOCAB_MOTOR_ENCODER_NUMBER:
2629  {
2630  int num=0;
2631  ok = rpc_IMotEnc->getNumberOfMotorEncoders(&num);
2632  response.addInt32(num);
2633  }
2634  break;
2635 
2636  case VOCAB_AMP_CURRENT:
2637  {
2638  ok = rcp_IAmp->getCurrent(cmd.get(2).asInt32(), &dtmp);
2639  response.addFloat64(dtmp);
2640  }
2641  break;
2642 
2643  case VOCAB_AMP_CURRENTS:
2644  {
2645  auto* p = new double[controlledJoints];
2646  ok = rcp_IAmp->getCurrents(p);
2647  Bottle& b = response.addList();
2648  int i;
2649  for (i = 0; i < controlledJoints; i++)
2650  b.addFloat64(p[i]);
2651  delete[] p;
2652  }
2653  break;
2654 
2655  case VOCAB_AMP_STATUS:
2656  {
2657  int *p = new int[controlledJoints];
2658  ok = rcp_IAmp->getAmpStatus(p);
2659  Bottle& b = response.addList();
2660  int i;
2661  for (i = 0; i < controlledJoints; i++)
2662  b.addInt32(p[i]);
2663  delete[] p;
2664  }
2665  break;
2666 
2667  case VOCAB_AMP_STATUS_SINGLE:
2668  {
2669  int j=cmd.get(2).asInt32();
2670  int itmp;
2671  ok = rcp_IAmp->getAmpStatus(j, &itmp);
2672  response.addInt32(itmp);
2673  }
2674  break;
2675 
2676  case VOCAB_AMP_NOMINAL_CURRENT:
2677  {
2678  int m=cmd.get(2).asInt32();
2679  ok = rcp_IAmp->getNominalCurrent(m, &dtmp);
2680  response.addFloat64(dtmp);
2681  }
2682  break;
2683 
2684  case VOCAB_AMP_PEAK_CURRENT:
2685  {
2686  int m=cmd.get(2).asInt32();
2687  ok = rcp_IAmp->getPeakCurrent(m, &dtmp);
2688  response.addFloat64(dtmp);
2689  }
2690  break;
2691 
2692  case VOCAB_AMP_PWM:
2693  {
2694  int m=cmd.get(2).asInt32();
2695  ok = rcp_IAmp->getPWM(m, &dtmp);
2696  yCTrace(CONTROLBOARDWRAPPER) << "RPC parser::getPWM: j" << m << " val " << dtmp;
2697  response.addFloat64(dtmp);
2698  }
2699  break;
2700 
2701  case VOCAB_AMP_PWM_LIMIT:
2702  {
2703  int m=cmd.get(2).asInt32();
2704  ok = rcp_IAmp->getPWMLimit(m, &dtmp);
2705  response.addFloat64(dtmp);
2706  }
2707  break;
2708 
2709  case VOCAB_AMP_VOLTAGE_SUPPLY:
2710  {
2711  int m=cmd.get(2).asInt32();
2712  ok = rcp_IAmp->getPowerSupplyVoltage(m, &dtmp);
2713  response.addFloat64(dtmp);
2714  }
2715  break;
2716 
2717  case VOCAB_LIMITS:
2718  {
2719  double min = 0.0, max = 0.0;
2720  ok = rcp_Ilim->getLimits(cmd.get(2).asInt32(), &min, &max);
2721  response.addFloat64(min);
2722  response.addFloat64(max);
2723  }
2724  break;
2725 
2726  case VOCAB_VEL_LIMITS:
2727  {
2728  double min = 0.0, max = 0.0;
2729  ok = rcp_Ilim->getVelLimits(cmd.get(2).asInt32(), &min, &max);
2730  response.addFloat64(min);
2731  response.addFloat64(max);
2732  }
2733  break;
2734 
2735  case VOCAB_INFO_NAME:
2736  {
2737  std::string name = "undocumented";
2738  ok = rpc_AxisInfo->getAxisName(cmd.get(2).asInt32(),name);
2739  response.addString(name.c_str());
2740  }
2741  break;
2742 
2743  case VOCAB_INFO_TYPE:
2744  {
2745  yarp::dev::JointTypeEnum type;
2746  ok = rpc_AxisInfo->getJointType(cmd.get(2).asInt32(), type);
2747  response.addInt32(type);
2748  }
2749  break;
2750 
2751  default:
2752  {
2753  yCError(CONTROLBOARDWRAPPER, "received an unknown request after a VOCAB_GET: %s", yarp::os::Vocab::decode(cmd.get(1).asVocab()).c_str());
2754  }
2755  break;
2756  } //switch cmd.get(1).asVocab())
2757 
2758  lastRpcStamp.update();
2759  appendTimeStamp(response, lastRpcStamp);
2760  } // case VOCAB_GET
2761  default:
2762  break;
2763  } //switch code
2764 
2765  if (!rec)
2766  {
2767  ok = DeviceResponder::respond(cmd,response);
2768  }
2769  }
2770 
2771  if (!ok)
2772  {
2773  // failed thus send only a VOCAB back.
2774  response.clear();
2775  response.addVocab(VOCAB_FAILED);
2776  }
2777  else
2778  response.addVocab(VOCAB_OK);
2779  }
2780 
2781  return ok;
2782 }
2783 
2784 bool RPCMessagesParser::initialize()
2785 {
2786  bool ok = false;
2787  if (rpc_IPosCtrl)
2788  {
2789  ok = rpc_IPosCtrl->getAxes(&controlledJoints);
2790  }
2791 
2792  DeviceResponder::makeUsage();
2793  addUsage("[get] [axes]", "get the number of axes");
2794  addUsage("[get] [name] $iAxisNumber", "get a human-readable name for an axis, if available");
2795  addUsage("[set] [pos] $iAxisNumber $fPosition", "command the position of an axis");
2796  addUsage("[set] [rel] $iAxisNumber $fPosition", "command the relative position of an axis");
2797  addUsage("[set] [vmo] $iAxisNumber $fVelocity", "command the velocity of an axis");
2798  addUsage("[get] [enc] $iAxisNumber", "get the encoder value for an axis");
2799 
2800  std::string args;
2801  for (int i=0; i<controlledJoints; i++) {
2802  if (i>0) {
2803  args += " ";
2804  }
2805  // removed dependency from yarp internals
2806  //args = args + "$f" + yarp::NetType::toString(i);
2807  }
2808  addUsage((std::string("[set] [poss] (")+args+")").c_str(),
2809  "command the position of all axes");
2810  addUsage((std::string("[set] [rels] (")+args+")").c_str(),
2811  "command the relative position of all axes");
2812  addUsage((std::string("[set] [vmos] (")+args+")").c_str(),
2813  "command the velocity of all axes");
2814 
2815  addUsage("[set] [aen] $iAxisNumber", "enable (amplifier for) the given axis");
2816  addUsage("[set] [adi] $iAxisNumber", "disable (amplifier for) the given axis");
2817  addUsage("[get] [acu] $iAxisNumber", "get current for the given axis");
2818  addUsage("[get] [acus]", "get current for all axes");
2819 
2820  return ok;
2821 }
2822 
2823 RPCMessagesParser::RPCMessagesParser() :
2824  ControlBoardWrapper_p(nullptr),
2825  rpc_IPid(nullptr),
2826  rpc_IPosCtrl(nullptr),
2827  rpc_IPosDirect(nullptr),
2828  rpc_IVelCtrl(nullptr),
2829  rpc_IEncTimed(nullptr),
2830  rpc_IMotEnc(nullptr),
2831  rcp_IAmp(nullptr),
2832  rcp_Ilim(nullptr),
2833  rpc_ITorque(nullptr),
2834  rpc_iCtrlMode(nullptr),
2835  rpc_AxisInfo(nullptr),
2836  rpc_IRemoteCalibrator(nullptr),
2837  rpc_Icalib(nullptr),
2838  rpc_IImpedance(nullptr),
2839  rpc_IInteract(nullptr),
2840  rpc_IMotor(nullptr),
2841  rpc_IVar(nullptr),
2842  rpc_ICurrent(nullptr),
2843  rpc_IPWM(nullptr),
2844  controlledJoints(0)
2845 {
2846 }
2847 
2848 void RPCMessagesParser::init(ControlBoardWrapper *x)
2849 {
2850  ControlBoardWrapper_p = x;
2851  rpc_IPid = dynamic_cast<yarp::dev::IPidControl *> (ControlBoardWrapper_p);
2852  rpc_IPosCtrl = dynamic_cast<yarp::dev::IPositionControl *> (ControlBoardWrapper_p);
2853  rpc_IPosDirect = dynamic_cast<yarp::dev::IPositionDirect *> (ControlBoardWrapper_p);
2854  rpc_IVelCtrl = dynamic_cast<yarp::dev::IVelocityControl *> (ControlBoardWrapper_p);
2855  rpc_IEncTimed = dynamic_cast<yarp::dev::IEncodersTimed *> (ControlBoardWrapper_p);
2856  rpc_IMotEnc = dynamic_cast<yarp::dev::IMotorEncoders *> (ControlBoardWrapper_p);
2857  rpc_IMotor = dynamic_cast<yarp::dev::IMotor *> (ControlBoardWrapper_p);
2858  rpc_IVar = dynamic_cast<yarp::dev::IRemoteVariables *> (ControlBoardWrapper_p);
2859  rcp_IAmp = dynamic_cast<yarp::dev::IAmplifierControl *> (ControlBoardWrapper_p);
2860  rcp_Ilim = dynamic_cast<yarp::dev::IControlLimits *> (ControlBoardWrapper_p);
2861  rpc_AxisInfo = dynamic_cast<yarp::dev::IAxisInfo *> (ControlBoardWrapper_p);
2862  rpc_IRemoteCalibrator = dynamic_cast<yarp::dev::IRemoteCalibrator *> (ControlBoardWrapper_p);
2863  rpc_Icalib = dynamic_cast<yarp::dev::IControlCalibration *> (ControlBoardWrapper_p);
2864  rpc_IImpedance = dynamic_cast<yarp::dev::IImpedanceControl *> (ControlBoardWrapper_p);
2865  rpc_ITorque = dynamic_cast<yarp::dev::ITorqueControl *> (ControlBoardWrapper_p);
2866  rpc_iCtrlMode = dynamic_cast<yarp::dev::IControlMode *> (ControlBoardWrapper_p);
2867  rpc_IInteract = dynamic_cast<yarp::dev::IInteractionMode *> (ControlBoardWrapper_p);
2868  rpc_ICurrent = dynamic_cast<yarp::dev::ICurrentControl *> (ControlBoardWrapper_p);
2869  rpc_IPWM = dynamic_cast<yarp::dev::IPWMControl *> (ControlBoardWrapper_p);
2870  controlledJoints = 0;
2871 }
VOCAB_PARK_SINGLE_JOINT
constexpr yarp::conf::vocab32_t VOCAB_PARK_SINGLE_JOINT
Definition: CalibratorVocabs.h:21
VOCAB_REMOTE_CALIBRATOR_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_REMOTE_CALIBRATOR_INTERFACE
Definition: CalibratorVocabs.h:15
VOCAB_HOMING_WHOLE_PART
constexpr yarp::conf::vocab32_t VOCAB_HOMING_WHOLE_PART
Definition: CalibratorVocabs.h:20
VOCAB_PARK_WHOLE_PART
constexpr yarp::conf::vocab32_t VOCAB_PARK_WHOLE_PART
Definition: CalibratorVocabs.h:22
VOCAB_QUIT_PARK
constexpr yarp::conf::vocab32_t VOCAB_QUIT_PARK
Definition: CalibratorVocabs.h:24
VOCAB_IS_CALIBRATOR_PRESENT
constexpr yarp::conf::vocab32_t VOCAB_IS_CALIBRATOR_PRESENT
Definition: CalibratorVocabs.h:16
VOCAB_CALIBRATE_SINGLE_JOINT
constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_SINGLE_JOINT
Definition: CalibratorVocabs.h:17
VOCAB_HOMING_SINGLE_JOINT
constexpr yarp::conf::vocab32_t VOCAB_HOMING_SINGLE_JOINT
Definition: CalibratorVocabs.h:19
VOCAB_CALIBRATE_WHOLE_PART
constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_WHOLE_PART
Definition: CalibratorVocabs.h:18
VOCAB_QUIT_CALIBRATE
constexpr yarp::conf::vocab32_t VOCAB_QUIT_CALIBRATE
Definition: CalibratorVocabs.h:23
VOCAB_TIMESTAMP
constexpr yarp::conf::vocab32_t VOCAB_TIMESTAMP
Definition: ControlBoardVocabs.h:18
VOCAB_VELOCITY_MOVE
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_MOVE
Definition: ControlBoardVocabs.h:36
VOCAB_REF_SPEED
constexpr yarp::conf::vocab32_t VOCAB_REF_SPEED
Definition: ControlBoardVocabs.h:27
VOCAB_STOPS
constexpr yarp::conf::vocab32_t VOCAB_STOPS
Definition: ControlBoardVocabs.h:32
VOCAB_MOTION_DONE
constexpr yarp::conf::vocab32_t VOCAB_MOTION_DONE
Definition: ControlBoardVocabs.h:16
VOCAB_STOP
constexpr yarp::conf::vocab32_t VOCAB_STOP
Definition: ControlBoardVocabs.h:31
VOCAB_REF_ACCELERATIONS
constexpr yarp::conf::vocab32_t VOCAB_REF_ACCELERATIONS
Definition: ControlBoardVocabs.h:30
VOCAB_MOTION_DONES
constexpr yarp::conf::vocab32_t VOCAB_MOTION_DONES
Definition: ControlBoardVocabs.h:17
VOCAB_PROTOCOL_VERSION
constexpr yarp::conf::vocab32_t VOCAB_PROTOCOL_VERSION
Definition: ControlBoardVocabs.h:40
VOCAB_POSITION_MOVE
constexpr yarp::conf::vocab32_t VOCAB_POSITION_MOVE
Definition: ControlBoardVocabs.h:22
VOCAB_REF_ACCELERATION
constexpr yarp::conf::vocab32_t VOCAB_REF_ACCELERATION
Definition: ControlBoardVocabs.h:29
VOCAB_VELOCITY_MOVES
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_MOVES
Definition: ControlBoardVocabs.h:37
VOCAB_RELATIVE_MOVE
constexpr yarp::conf::vocab32_t VOCAB_RELATIVE_MOVE
Definition: ControlBoardVocabs.h:24
VOCAB_REF_SPEEDS
constexpr yarp::conf::vocab32_t VOCAB_REF_SPEEDS
Definition: ControlBoardVocabs.h:28
VOCAB_RELATIVE_MOVES
constexpr yarp::conf::vocab32_t VOCAB_RELATIVE_MOVES
Definition: ControlBoardVocabs.h:26
VOCAB_POSITION_MOVES
constexpr yarp::conf::vocab32_t VOCAB_POSITION_MOVES
Definition: ControlBoardVocabs.h:23
CONTROLBOARDWRAPPER
const yarp::os::LogComponent & CONTROLBOARDWRAPPER()
Definition: ControlBoardWrapperLogComponent.cpp:11
PROTOCOL_VERSION_TWEAK
#define PROTOCOL_VERSION_TWEAK
Definition: ControlBoardWrapper.h:56
PROTOCOL_VERSION_MAJOR
#define PROTOCOL_VERSION_MAJOR
Definition: ControlBoardWrapper.h:54
PROTOCOL_VERSION_MINOR
#define PROTOCOL_VERSION_MINOR
Definition: ControlBoardWrapper.h:55
VOCAB_LIM
constexpr yarp::conf::vocab32_t VOCAB_LIM
Definition: GenericVocabs.h:30
VOCAB_OUTPUTS
constexpr yarp::conf::vocab32_t VOCAB_OUTPUTS
Definition: GenericVocabs.h:36
VOCAB_ENABLE
constexpr yarp::conf::vocab32_t VOCAB_ENABLE
Definition: GenericVocabs.h:34
VOCAB_DISABLE
constexpr yarp::conf::vocab32_t VOCAB_DISABLE
Definition: GenericVocabs.h:33
VOCAB_OK
constexpr yarp::conf::vocab32_t VOCAB_OK
Definition: GenericVocabs.h:18
VOCAB_REF
constexpr yarp::conf::vocab32_t VOCAB_REF
Definition: GenericVocabs.h:27
VOCAB_IS
constexpr yarp::conf::vocab32_t VOCAB_IS
Definition: GenericVocabs.h:17
VOCAB_ERRS
constexpr yarp::conf::vocab32_t VOCAB_ERRS
Definition: GenericVocabs.h:21
VOCAB_GET
constexpr yarp::conf::vocab32_t VOCAB_GET
Definition: GenericVocabs.h:16
VOCAB_OUTPUT
constexpr yarp::conf::vocab32_t VOCAB_OUTPUT
Definition: GenericVocabs.h:35
VOCAB_REFERENCE
constexpr yarp::conf::vocab32_t VOCAB_REFERENCE
Definition: GenericVocabs.h:37
VOCAB_FAILED
constexpr yarp::conf::vocab32_t VOCAB_FAILED
Definition: GenericVocabs.h:19
VOCAB_ERR
constexpr yarp::conf::vocab32_t VOCAB_ERR
Definition: GenericVocabs.h:20
VOCAB_REFS
constexpr yarp::conf::vocab32_t VOCAB_REFS
Definition: GenericVocabs.h:28
VOCAB_REFERENCES
constexpr yarp::conf::vocab32_t VOCAB_REFERENCES
Definition: GenericVocabs.h:38
VOCAB_RESET
constexpr yarp::conf::vocab32_t VOCAB_RESET
Definition: GenericVocabs.h:32
VOCAB_AXES
constexpr yarp::conf::vocab32_t VOCAB_AXES
Definition: GenericVocabs.h:39
VOCAB_SET
constexpr yarp::conf::vocab32_t VOCAB_SET
Definition: GenericVocabs.h:15
VOCAB_OFFSET
constexpr yarp::conf::vocab32_t VOCAB_OFFSET
Definition: GenericVocabs.h:26
VOCAB_LIMS
constexpr yarp::conf::vocab32_t VOCAB_LIMS
Definition: GenericVocabs.h:31
VOCAB_AMP_DISABLE
constexpr yarp::conf::vocab32_t VOCAB_AMP_DISABLE
Definition: IAmplifierControl.h:322
VOCAB_AMP_PEAK_CURRENT
constexpr yarp::conf::vocab32_t VOCAB_AMP_PEAK_CURRENT
Definition: IAmplifierControl.h:329
VOCAB_AMP_ENABLE
constexpr yarp::conf::vocab32_t VOCAB_AMP_ENABLE
Definition: IAmplifierControl.h:321
VOCAB_AMP_VOLTAGE_SUPPLY
constexpr yarp::conf::vocab32_t VOCAB_AMP_VOLTAGE_SUPPLY
Definition: IAmplifierControl.h:332
VOCAB_AMP_CURRENT
constexpr yarp::conf::vocab32_t VOCAB_AMP_CURRENT
Definition: IAmplifierControl.h:325
VOCAB_AMP_MAXCURRENT
constexpr yarp::conf::vocab32_t VOCAB_AMP_MAXCURRENT
Definition: IAmplifierControl.h:327
VOCAB_AMP_STATUS_SINGLE
constexpr yarp::conf::vocab32_t VOCAB_AMP_STATUS_SINGLE
Definition: IAmplifierControl.h:324
VOCAB_AMP_STATUS
constexpr yarp::conf::vocab32_t VOCAB_AMP_STATUS
Definition: IAmplifierControl.h:323
VOCAB_AMP_NOMINAL_CURRENT
constexpr yarp::conf::vocab32_t VOCAB_AMP_NOMINAL_CURRENT
Definition: IAmplifierControl.h:328
VOCAB_AMP_PWM
constexpr yarp::conf::vocab32_t VOCAB_AMP_PWM
Definition: IAmplifierControl.h:330
VOCAB_AMP_CURRENTS
constexpr yarp::conf::vocab32_t VOCAB_AMP_CURRENTS
Definition: IAmplifierControl.h:326
VOCAB_AMP_PWM_LIMIT
constexpr yarp::conf::vocab32_t VOCAB_AMP_PWM_LIMIT
Definition: IAmplifierControl.h:331
VOCAB_INFO_TYPE
constexpr yarp::conf::vocab32_t VOCAB_INFO_TYPE
Definition: IAxisInfo.h:93
VOCAB_INFO_NAME
constexpr yarp::conf::vocab32_t VOCAB_INFO_NAME
Definition: IAxisInfo.h:92
VOCAB_CALIBRATE_JOINT_PARAMS
constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_JOINT_PARAMS
Definition: IControlCalibration.h:145
VOCAB_CALIBRATE_DONE
constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_DONE
Definition: IControlCalibration.h:150
VOCAB_CALIBRATE_JOINT
constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_JOINT
Definition: IControlCalibration.h:144
VOCAB_CALIBRATE
constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE
Definition: IControlCalibration.h:146
VOCAB_PARK
constexpr yarp::conf::vocab32_t VOCAB_PARK
Definition: IControlCalibration.h:151
VOCAB_LIMITS
constexpr yarp::conf::vocab32_t VOCAB_LIMITS
Definition: IControlLimits.h:130
VOCAB_VEL_LIMITS
constexpr yarp::conf::vocab32_t VOCAB_VEL_LIMITS
Definition: IControlLimits.h:131
VOCAB_CM_CONTROL_MODE_GROUP
constexpr yarp::conf::vocab32_t VOCAB_CM_CONTROL_MODE_GROUP
Definition: IControlMode.h:121
VOCAB_CM_MIXED
constexpr yarp::conf::vocab32_t VOCAB_CM_MIXED
Definition: IControlMode.h:138
VOCAB_CM_TORQUE
constexpr yarp::conf::vocab32_t VOCAB_CM_TORQUE
Definition: IControlMode.h:127
VOCAB_CM_PWM
constexpr yarp::conf::vocab32_t VOCAB_CM_PWM
Definition: IControlMode.h:132
VOCAB_CM_IMPEDANCE_VEL
constexpr yarp::conf::vocab32_t VOCAB_CM_IMPEDANCE_VEL
Definition: IControlMode.h:134
VOCAB_CM_POSITION
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION
Definition: IControlMode.h:128
VOCAB_CM_CONTROL_MODES
constexpr yarp::conf::vocab32_t VOCAB_CM_CONTROL_MODES
Definition: IControlMode.h:122
VOCAB_CM_CURRENT
constexpr yarp::conf::vocab32_t VOCAB_CM_CURRENT
Definition: IControlMode.h:131
VOCAB_CM_IMPEDANCE_POS
constexpr yarp::conf::vocab32_t VOCAB_CM_IMPEDANCE_POS
Definition: IControlMode.h:133
VOCAB_CM_CONTROL_MODE
constexpr yarp::conf::vocab32_t VOCAB_CM_CONTROL_MODE
Definition: IControlMode.h:120
VOCAB_CM_POSITION_DIRECT
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION_DIRECT
Definition: IControlMode.h:129
VOCAB_ICONTROLMODE
constexpr yarp::conf::vocab32_t VOCAB_ICONTROLMODE
Definition: IControlMode.h:118
VOCAB_CM_VELOCITY
constexpr yarp::conf::vocab32_t VOCAB_CM_VELOCITY
Definition: IControlMode.h:130
VOCAB_CM_FORCE_IDLE
constexpr yarp::conf::vocab32_t VOCAB_CM_FORCE_IDLE
Definition: IControlMode.h:141
VOCAB_CURRENT_REF_GROUP
constexpr yarp::conf::vocab32_t VOCAB_CURRENT_REF_GROUP
Definition: ICurrentControl.h:201
VOCAB_CURRENT_REFS
constexpr yarp::conf::vocab32_t VOCAB_CURRENT_REFS
Definition: ICurrentControl.h:200
VOCAB_CURRENT_RANGE
constexpr yarp::conf::vocab32_t VOCAB_CURRENT_RANGE
Definition: ICurrentControl.h:202
VOCAB_CURRENTCONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_CURRENTCONTROL_INTERFACE
Definition: ICurrentControl.h:196
VOCAB_CURRENT_REF
constexpr yarp::conf::vocab32_t VOCAB_CURRENT_REF
Definition: ICurrentControl.h:199
VOCAB_CURRENT_RANGES
constexpr yarp::conf::vocab32_t VOCAB_CURRENT_RANGES
Definition: ICurrentControl.h:203
VOCAB_E_RESET
constexpr yarp::conf::vocab32_t VOCAB_E_RESET
Definition: IEncoders.h:209
VOCAB_ENCODER_ACCELERATIONS
constexpr yarp::conf::vocab32_t VOCAB_ENCODER_ACCELERATIONS
Definition: IEncoders.h:218
VOCAB_ENCODER
constexpr yarp::conf::vocab32_t VOCAB_ENCODER
Definition: IEncoders.h:211
VOCAB_ENCODER_SPEEDS
constexpr yarp::conf::vocab32_t VOCAB_ENCODER_SPEEDS
Definition: IEncoders.h:216
VOCAB_E_RESETS
constexpr yarp::conf::vocab32_t VOCAB_E_RESETS
Definition: IEncoders.h:210
VOCAB_ENCODER_SPEED
constexpr yarp::conf::vocab32_t VOCAB_ENCODER_SPEED
Definition: IEncoders.h:215
VOCAB_ENCODER_ACCELERATION
constexpr yarp::conf::vocab32_t VOCAB_ENCODER_ACCELERATION
Definition: IEncoders.h:217
VOCAB_ENCODERS
constexpr yarp::conf::vocab32_t VOCAB_ENCODERS
Definition: IEncoders.h:212
VOCAB_IMPEDANCE
constexpr yarp::conf::vocab32_t VOCAB_IMPEDANCE
Definition: IImpedanceControl.h:117
VOCAB_INTERACTION_MODES
constexpr yarp::conf::vocab32_t VOCAB_INTERACTION_MODES
Definition: IInteractionMode.h:32
VOCAB_INTERACTION_MODE_GROUP
constexpr yarp::conf::vocab32_t VOCAB_INTERACTION_MODE_GROUP
Definition: IInteractionMode.h:31
VOCAB_INTERACTION_MODE
constexpr yarp::conf::vocab32_t VOCAB_INTERACTION_MODE
Definition: IInteractionMode.h:30
VOCAB_INTERFACE_INTERACTION_MODE
constexpr yarp::conf::vocab32_t VOCAB_INTERFACE_INTERACTION_MODE
Definition: IInteractionMode.h:29
VOCAB_MOTOR_ENCODER_SPEED
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_SPEED
Definition: IMotorEncoders.h:285
VOCAB_MOTOR_ENCODER_ACCELERATIONS
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_ACCELERATIONS
Definition: IMotorEncoders.h:288
VOCAB_MOTOR_ENCODER_ACCELERATION
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_ACCELERATION
Definition: IMotorEncoders.h:287
VOCAB_MOTOR_ENCODER
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER
Definition: IMotorEncoders.h:279
VOCAB_MOTOR_ENCODER_SPEEDS
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_SPEEDS
Definition: IMotorEncoders.h:286
VOCAB_MOTOR_ENCODER_NUMBER
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_NUMBER
Definition: IMotorEncoders.h:284
VOCAB_MOTOR_E_RESETS
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_E_RESETS
Definition: IMotorEncoders.h:278
VOCAB_MOTOR_E_RESET
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_E_RESET
Definition: IMotorEncoders.h:277
VOCAB_MOTOR_ENCODERS
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODERS
Definition: IMotorEncoders.h:280
VOCAB_MOTOR_CPR
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_CPR
Definition: IMotorEncoders.h:281
VOCAB_TEMPERATURES
constexpr yarp::conf::vocab32_t VOCAB_TEMPERATURES
Definition: IMotor.h:167
VOCAB_TEMPERATURE_LIMIT
constexpr yarp::conf::vocab32_t VOCAB_TEMPERATURE_LIMIT
Definition: IMotor.h:168
VOCAB_TEMPERATURE
constexpr yarp::conf::vocab32_t VOCAB_TEMPERATURE
Definition: IMotor.h:165
VOCAB_GEARBOX_RATIO
constexpr yarp::conf::vocab32_t VOCAB_GEARBOX_RATIO
Definition: IMotor.h:166
VOCAB_MOTORS_NUMBER
constexpr yarp::conf::vocab32_t VOCAB_MOTORS_NUMBER
Definition: IMotor.h:164
VOCAB_PWMCONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_INTERFACE
Definition: IPWMControl.h:144
VOCAB_PWMCONTROL_PWM_OUTPUTS
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_PWM_OUTPUTS
Definition: IPWMControl.h:149
VOCAB_PWMCONTROL_REF_PWMS
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_REF_PWMS
Definition: IPWMControl.h:147
VOCAB_PWMCONTROL_REF_PWM
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_REF_PWM
Definition: IPWMControl.h:146
VOCAB_PWMCONTROL_PWM_OUTPUT
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_PWM_OUTPUT
Definition: IPWMControl.h:148
VOCAB_PIDS
constexpr yarp::conf::vocab32_t VOCAB_PIDS
Definition: IPidControl.h:385
VOCAB_PID
constexpr yarp::conf::vocab32_t VOCAB_PID
Definition: IPidControl.h:384
VOCAB_REF_ACCELERATION_GROUP
constexpr yarp::conf::vocab32_t VOCAB_REF_ACCELERATION_GROUP
Definition: IPositionControl.h:481
VOCAB_STOP_GROUP
constexpr yarp::conf::vocab32_t VOCAB_STOP_GROUP
Definition: IPositionControl.h:482
VOCAB_RELATIVE_MOVE_GROUP
constexpr yarp::conf::vocab32_t VOCAB_RELATIVE_MOVE_GROUP
Definition: IPositionControl.h:478
VOCAB_POSITION_MOVE_GROUP
constexpr yarp::conf::vocab32_t VOCAB_POSITION_MOVE_GROUP
Definition: IPositionControl.h:477
VOCAB_MOTION_DONE_GROUP
constexpr yarp::conf::vocab32_t VOCAB_MOTION_DONE_GROUP
Definition: IPositionControl.h:479
VOCAB_REF_SPEED_GROUP
constexpr yarp::conf::vocab32_t VOCAB_REF_SPEED_GROUP
Definition: IPositionControl.h:480
VOCAB_POSITION_DIRECTS
constexpr yarp::conf::vocab32_t VOCAB_POSITION_DIRECTS
Definition: IPositionDirect.h:194
VOCAB_POSITION_DIRECT
constexpr yarp::conf::vocab32_t VOCAB_POSITION_DIRECT
Definition: IPositionDirect.h:193
VOCAB_POSITION_DIRECT_GROUP
constexpr yarp::conf::vocab32_t VOCAB_POSITION_DIRECT_GROUP
Definition: IPositionDirect.h:195
VOCAB_LIST_VARIABLES
constexpr yarp::conf::vocab32_t VOCAB_LIST_VARIABLES
Definition: IRemoteVariables.h:67
VOCAB_VARIABLE
constexpr yarp::conf::vocab32_t VOCAB_VARIABLE
Definition: IRemoteVariables.h:66
VOCAB_REMOTE_VARIABILE_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_REMOTE_VARIABILE_INTERFACE
Definition: IRemoteVariables.h:65
VOCAB_TORQUE
constexpr yarp::conf::vocab32_t VOCAB_TORQUE
Definition: ITorqueControl.h:234
VOCAB_TRQ
constexpr yarp::conf::vocab32_t VOCAB_TRQ
Definition: ITorqueControl.h:237
VOCAB_TRQS
constexpr yarp::conf::vocab32_t VOCAB_TRQS
Definition: ITorqueControl.h:236
VOCAB_RANGE
constexpr yarp::conf::vocab32_t VOCAB_RANGE
Definition: ITorqueControl.h:241
VOCAB_IMP_OFFSET
constexpr yarp::conf::vocab32_t VOCAB_IMP_OFFSET
Definition: ITorqueControl.h:243
VOCAB_MOTOR_PARAMS
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_PARAMS
Definition: ITorqueControl.h:239
VOCAB_IMP_PARAM
constexpr yarp::conf::vocab32_t VOCAB_IMP_PARAM
Definition: ITorqueControl.h:242
VOCAB_RANGES
constexpr yarp::conf::vocab32_t VOCAB_RANGES
Definition: ITorqueControl.h:240
VOCAB_TORQUE_MODE
constexpr yarp::conf::vocab32_t VOCAB_TORQUE_MODE
Definition: ITorqueControl.h:235
VOCAB_VELOCITY_MOVE_GROUP
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_MOVE_GROUP
Definition: IVelocityControl.h:302
appendTimeStamp
void appendTimeStamp(Bottle &bot, Stamp &st)
Definition: RPCMessagesParser.cpp:24
RPCMessagesParser::handleRemoteCalibratorMsg
void handleRemoteCalibratorMsg(const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
Definition: RPCMessagesParser.cpp:1502
RPCMessagesParser::rpc_Icalib
yarp::dev::IControlCalibration * rpc_Icalib
Definition: RPCMessagesParser.h:72
RPCMessagesParser::rpc_IEncTimed
yarp::dev::IEncodersTimed * rpc_IEncTimed
Definition: RPCMessagesParser.h:64
RPCMessagesParser::rpc_iCtrlMode
yarp::dev::IControlMode * rpc_iCtrlMode
Definition: RPCMessagesParser.h:69
RPCMessagesParser::rpc_IRemoteCalibrator
yarp::dev::IRemoteCalibrator * rpc_IRemoteCalibrator
Definition: RPCMessagesParser.h:71
RPCMessagesParser::init
void init(ControlBoardWrapper *x)
Initialization.
Definition: RPCMessagesParser.cpp:2848
RPCMessagesParser::rpc_AxisInfo
yarp::dev::IAxisInfo * rpc_AxisInfo
Definition: RPCMessagesParser.h:70
RPCMessagesParser::rpc_ITorque
yarp::dev::ITorqueControl * rpc_ITorque
Definition: RPCMessagesParser.h:68
RPCMessagesParser::rpc_IPid
yarp::dev::IPidControl * rpc_IPid
Definition: RPCMessagesParser.h:60
RPCMessagesParser::handleProtocolVersionRequest
void handleProtocolVersionRequest(const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
Definition: RPCMessagesParser.cpp:33
RPCMessagesParser::handlePWMMsg
void handlePWMMsg(const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
Definition: RPCMessagesParser.cpp:1316
RPCMessagesParser::rpc_IVar
yarp::dev::IRemoteVariables * rpc_IVar
Definition: RPCMessagesParser.h:76
RPCMessagesParser::rpc_IImpedance
yarp::dev::IImpedanceControl * rpc_IImpedance
Definition: RPCMessagesParser.h:73
RPCMessagesParser::handleCurrentMsg
void handleCurrentMsg(const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
Definition: RPCMessagesParser.cpp:850
RPCMessagesParser::handleInteractionModeMsg
void handleInteractionModeMsg(const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
Definition: RPCMessagesParser.cpp:666
RPCMessagesParser::handleControlModeMsg
void handleControlModeMsg(const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
Definition: RPCMessagesParser.cpp:157
RPCMessagesParser::rpc_IPosDirect
yarp::dev::IPositionDirect * rpc_IPosDirect
Definition: RPCMessagesParser.h:62
RPCMessagesParser::rpc_ICurrent
yarp::dev::ICurrentControl * rpc_ICurrent
Definition: RPCMessagesParser.h:77
RPCMessagesParser::handleTorqueMsg
void handleTorqueMsg(const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
Definition: RPCMessagesParser.cpp:465
RPCMessagesParser::rpc_IMotEnc
yarp::dev::IMotorEncoders * rpc_IMotEnc
Definition: RPCMessagesParser.h:65
RPCMessagesParser::rpc_IInteract
yarp::dev::IInteractionMode * rpc_IInteract
Definition: RPCMessagesParser.h:74
RPCMessagesParser::handlePidMsg
void handlePidMsg(const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
Definition: RPCMessagesParser.cpp:987
RPCMessagesParser::RPCMessagesParser
RPCMessagesParser()
Constructor.
Definition: RPCMessagesParser.cpp:2823
RPCMessagesParser::initialize
virtual bool initialize()
Initialize the internal data.
Definition: RPCMessagesParser.cpp:2784
RPCMessagesParser::rpc_IPosCtrl
yarp::dev::IPositionControl * rpc_IPosCtrl
Definition: RPCMessagesParser.h:61
RPCMessagesParser::respond
bool respond(const yarp::os::Bottle &cmd, yarp::os::Bottle &response) override
Respond to a message.
Definition: RPCMessagesParser.cpp:1614
RPCMessagesParser::rpc_IVelCtrl
yarp::dev::IVelocityControl * rpc_IVelCtrl
Definition: RPCMessagesParser.h:63
RPCMessagesParser::rpc_IPWM
yarp::dev::IPWMControl * rpc_IPWM
Definition: RPCMessagesParser.h:78
RPCMessagesParser::rcp_IAmp
yarp::dev::IAmplifierControl * rcp_IAmp
Definition: RPCMessagesParser.h:66
RPCMessagesParser::ControlBoardWrapper_p
ControlBoardWrapper * ControlBoardWrapper_p
Definition: RPCMessagesParser.h:59
RPCMessagesParser::rpc_IMotor
yarp::dev::IMotor * rpc_IMotor
Definition: RPCMessagesParser.h:75
RPCMessagesParser::rcp_Ilim
yarp::dev::IControlLimits * rcp_Ilim
Definition: RPCMessagesParser.h:67
RPCMessagesParser::handleImpedanceMsg
void handleImpedanceMsg(const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
Definition: RPCMessagesParser.cpp:52
RPCMessagesParser::handleRemoteVariablesMsg
void handleRemoteVariablesMsg(const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
Definition: RPCMessagesParser.cpp:1430
yarp::dev::IAmplifierControl
Interface for control devices, amplifier commands.
Definition: IAmplifierControl.h:33
yarp::dev::IAxisInfo
Interface for getting information about specific axes, if available.
Definition: IAxisInfo.h:43
yarp::dev::IControlCalibration
Interface for control devices, calibration commands.
Definition: IControlCalibration.h:85
yarp::dev::IControlLimits
Interface for control devices, commands to get/set position and veloity limits.
Definition: IControlLimits.h:33
yarp::dev::IControlMode
Interface for setting control mode in control board.
Definition: IControlMode.h:28
yarp::dev::ICurrentControl
Interface for control boards implementing current control.
Definition: ICurrentControl.h:28
yarp::dev::IEncodersTimed
Control board, extend encoder interface with timestamps.
Definition: IEncodersTimed.h:59
yarp::dev::IImpedanceControl
Interface for control boards implementing impedance control.
Definition: IImpedanceControl.h:75
yarp::dev::IInteractionMode
Interface settings the way the robot interacts with the environment: basic interaction types are Stif...
Definition: IInteractionMode.h:43
yarp::dev::IMotorEncoders
Control board, encoder interface.
Definition: IMotorEncoders.h:155
yarp::dev::IMotor
Control board, encoder interface.
Definition: IMotor.h:99
yarp::dev::IPWMControl
Interface for controlling an axis, by sending directly a PWM reference signal to a motor.
Definition: IPWMControl.h:28
yarp::dev::IPidControl
Interface for a generic control board device implementing a PID controller, with scaled arguments.
Definition: IPidControl.h:211
yarp::dev::IPositionControl
Interface for a generic control board device implementing position control.
Definition: IPositionControl.h:257
yarp::dev::IPositionDirect
Interface for a generic control board device implementing position control.
Definition: IPositionDirect.h:32
yarp::dev::IRemoteCalibrator
IRemoteCalibrator interface is meant to remotize the access of the calibration device in order to all...
Definition: IRemoteCalibrator.h:30
yarp::dev::IRemoteVariables
IRemoteVariables interface.
Definition: IRemoteVariables.h:51
yarp::dev::ITorqueControl
Interface for control boards implementing torque control.
Definition: ITorqueControl.h:39
yarp::dev::IVelocityControl
Interface for control boards implementing velocity control.
Definition: IVelocityControl.h:160
yarp::dev::Pid
Contains the parameters for a PID.
Definition: ControlBoardPid.h:29
yarp::dev::Pid::kd
double kd
derivative gain
Definition: ControlBoardPid.h:32
yarp::dev::Pid::scale
double scale
scale for the pid output
Definition: ControlBoardPid.h:35
yarp::dev::Pid::offset
double offset
pwm offset added to the pid output
Definition: ControlBoardPid.h:37
yarp::dev::Pid::stiction_down_val
double stiction_down_val
down stiction offset added to the pid output
Definition: ControlBoardPid.h:39
yarp::dev::Pid::stiction_up_val
double stiction_up_val
up stiction offset added to the pid output
Definition: ControlBoardPid.h:38
yarp::dev::Pid::max_output
double max_output
max output
Definition: ControlBoardPid.h:36
yarp::dev::Pid::kff
double kff
feedforward gain
Definition: ControlBoardPid.h:40
yarp::dev::Pid::ki
double ki
integrative gain
Definition: ControlBoardPid.h:33
yarp::dev::Pid::max_int
double max_int
saturation threshold for the integrator
Definition: ControlBoardPid.h:34
yarp::dev::Pid::kp
double kp
proportional gain
Definition: ControlBoardPid.h:31
yarp::os::Bottle
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:73
yarp::os::Bottle::add
void add(const Value &value)
Add a Value to the bottle, at the end of the list.
Definition: Bottle.cpp:339
yarp::os::Bottle::addList
Bottle & addList()
Places an empty nested list in the bottle, at the end of the list.
Definition: Bottle.cpp:185
yarp::os::Bottle::size
size_type size() const
Gets the number of elements in the bottle.
Definition: Bottle.cpp:254
yarp::os::Bottle::addFloat64
void addFloat64(yarp::conf::float64_t x)
Places a 64-bit floating point number in the bottle, at the end of the list.
Definition: Bottle.cpp:161
yarp::os::Bottle::addVocab
void addVocab(int x)
Places a vocabulary item in the bottle, at the end of the list.
Definition: Bottle.cpp:167
yarp::os::Bottle::get
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:249
yarp::os::Bottle::tail
Bottle tail() const
Get all but the first element of a bottle.
Definition: Bottle.cpp:391
yarp::os::Bottle::clear
void clear()
Empties the bottle of any objects it contains.
Definition: Bottle.cpp:124
yarp::os::Bottle::addInt32
void addInt32(std::int32_t x)
Places a 32-bit integer in the bottle, at the end of the list.
Definition: Bottle.cpp:143
yarp::os::Bottle::addString
void addString(const char *str)
Places a string in the bottle, at the end of the list.
Definition: Bottle.cpp:173
yarp::os::Bottle::toString
std::string toString() const override
Gives a human-readable textual representation of the bottle.
Definition: Bottle.cpp:214
yarp::os::Stamp
An abstraction for a time stamp and/or sequence number.
Definition: Stamp.h:25
yarp::os::Stamp::getTime
double getTime() const
Get the time stamp.
Definition: Stamp.cpp:37
yarp::os::Stamp::getCount
int getCount() const
Get the sequence number.
Definition: Stamp.cpp:32
yarp::os::Value::asFloat64
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition: Value.cpp:225
yarp::os::Value::asInt32
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:207
yarp::os::Value::asList
virtual Bottle * asList() const
Get list value.
Definition: Value.cpp:243
yarp::os::Value::asVocab
virtual std::int32_t asVocab() const
Get vocabulary identifier as an integer.
Definition: Value.cpp:231
yarp::os::Value::asString
virtual std::string asString() const
Get string value.
Definition: Value.cpp:237
yCError
#define yCError(component,...)
Definition: LogComponent.h:157
yCTrace
#define yCTrace(component,...)
Definition: LogComponent.h:88
yCDebug
#define yCDebug(component,...)
Definition: LogComponent.h:112
yarp::dev
An interface for the device drivers.
Definition: audioBufferSizeData.cpp:17
yarp::dev::PidControlTypeEnum
PidControlTypeEnum
Definition: PidEnums.h:21
yarp::os::Vocab::decode
std::string decode(NetInt32 code)
Convert a vocabulary identifier into a string.
Definition: Vocab.cpp:36
yarp::os
An interface to the operating system, including Port based communication.
Definition: AbstractCarrier.h:17
yarp::sig
Signal processing.
Definition: Image.h:25