iCub-main
main.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4  * Copyright (C) 2006 RobotCub Consortium, European Commission FP6 Project IST-004370
5  * Author: Giorgio Metta and Lorenzo Natale
6  * website: www.robotcub.org
7  * Permission is granted to copy, distribute, and/or modify this program
8  * under the terms of the GNU General Public License, version 2 or any
9  * later version published by the Free Software Foundation.
10  *
11  * A copy of the license can be found at
12  * http://www.robotcub.org/icub/license/gpl.txt
13  *
14  * This program is distributed in the hope that it will be useful, but
15  * WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
17  * Public License for more details
18 */
19 
32 #include <yarp/os/Network.h>
33 #include <yarp/os/Port.h>
34 #include <yarp/os/Bottle.h>
35 #include <yarp/os/Time.h>
36 #include <yarp/os/Vocab.h>
37 
38 #include <yarp/dev/ControlBoardInterfaces.h>
39 #include <yarp/dev/PolyDriver.h>
40 
41 #include <yarp/os/LogStream.h>
42 #include <yarp/os/Log.h>
43 
44 #include <string>
45 
46 using namespace yarp::dev;
47 using namespace yarp::os;
48 
49 #define VOCAB_HELP yarp::os::createVocab32('h','e','l','p')
50 #define VOCAB_QUIT yarp::os::createVocab32('q','u','i','t')
51 #define VOCAB_ICONTROLMODE_DEBUG yarp::os::createVocab32('i','c','d','d')
52 
53 void handleTorqueMsg(ITorqueControl *itq, IPidControl *ipid, const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool *rec, bool *ok);
54 void handleImpedanceMsg(IImpedanceControl *iimp, const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool *rec, bool *ok);
55 void handleControlModeMsg(IControlMode *icm, const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool *rec, bool *ok);
56 void handleControlModeMsg_DEBUG(IControlMode *icm, const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool *rec, bool *ok);
57 void handleInteractionModeMsg(IInteractionMode *_iInteract, const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool *rec, bool *ok);
58 
59 //
60 int jnts = 0; // joint number
61 
62 int main(int argc, char *argv[])
63 {
64  // just list the devices if no argument given
65  if (argc <= 2) {
66  printf("You can call %s like this:\n", argv[0]);
67  printf(" %s --robot ROBOTNAME --OPTION VALUE ...\n", argv[0]);
68  printf("For example:\n");
69  printf(" %s --robot icub --local /talkto/james --remote /controlboard/rpc\n", argv[0]);
70  printf("Here are devices listed for your system:\n");
71  printf("%s", Drivers::factory().toString().c_str());
72  return 0;
73  }
74 
75  // get command line options
76  Property options;
77  options.fromCommand(argc, argv);
78  if (!options.check("robot") || !options.check("part")) {
79  printf("Missing either --robot or --part options\n");
80  return 1;
81  }
82 
83  Network::init();
84 
85  std::string name;
86  Value& v = options.find("robot");
87  Value& part = options.find("part");
88 
89  Value *val;
90  if (!options.check("device", val)) {
91  options.put("device", "remote_controlboard");
92  }
93  if (!options.check("local", val)) {
94  name="/"+std::string(v.asString().c_str())+"/"+std::string(part.asString().c_str())+"/simpleclient";
95  //sprintf(&name[0], "/%s/%s/client", v.asString().c_str(), part.asString().c_str());
96  options.put("local", name.c_str());
97  }
98  if (!options.check("remote", val)) {
99  name="/"+std::string(v.asString().c_str())+"/"+std::string(part.asString().c_str());
100  //sprintf(&name[0], "/%s/%s", v.asString().c_str(), part.asString().c_str());
101  options.put("remote", name.c_str());
102  }
103 
104  fprintf(stderr, "%s", options.toString().c_str());
105 
106 
107  // create a device
108  PolyDriver dd(options);
109  if (!dd.isValid()) {
110  printf("Device not available. Here are the known devices:\n");
111  printf("%s", Drivers::factory().toString().c_str());
112  Network::fini();
113  return 1;
114  }
115 
116  IPositionControl *ipos=0;
117  IPositionDirect *iposDir=0;
118  IVelocityControl *vel=0;
119  IEncoders *enc=0;
120  IPidControl *pid=0;
121  IAmplifierControl *amp=0;
122  IControlLimits *lim=0;
123  IControlMode *iMode2=0;
124  IMotor *imot=0;
125  ITorqueControl *itorque=0;
126  IPWMControl *ipwm=0;
127  IImpedanceControl *iimp=0;
128  IInteractionMode *iInteract=0;
129  IMotorEncoders *iMotEnc=0;
130  IAxisInfo *iInfo = 0;
131 
132  bool ok;
133  ok = dd.view(ipos);
134  ok &= dd.view(vel);
135  ok &= dd.view(enc);
136  ok &= dd.view(pid);
137  ok &= dd.view(amp);
138  ok &= dd.view(lim);
139 // ok &= dd.view(icm);
140  ok &= dd.view(itorque);
141  ok &= dd.view(ipwm);
142  ok &= dd.view(iimp);
143  ok &= dd.view(iposDir);
144  ok &= dd.view(iMode2);
145  ok &= dd.view(iInteract);
146 
147  if (!ok) {
148  yError("Problems acquiring mandatory interfaces, quitting\n");
149  return 1;
150  }
151 
152  ok &=dd.view(iMotEnc);
153  if (!ok) {
154  yWarning("Problems acquiring optional interface IMotorEncoders\n");
155  }
156  ok &=dd.view(imot);
157  if (!ok) {
158  yWarning("Problems acquiring optional interface iMotor\n");
159  }
160  ok &= dd.view(iInfo);
161  if (!ok) {
162  yWarning("Problems acquiring optional interface iInfo\n");
163  }
164 
165  ipos->getAxes(&jnts);
166  printf("Working with %d axes\n", jnts);
167  double *tmp = new double[jnts];
168  bool *btmp = new bool[jnts];
169  int *jtmp = new int[jnts];
170  for (int i=0; i<jnts; i++) jtmp[i]=i;
171 
172  printf("Device active...\n");
173  while (dd.isValid()) {
174  std::string s;
175  s.resize(1024);
176 
177  printf("-> ");
178  char c = 0;
179  int i = 0;
180  while (c != '\n') {
181  c = (char)fgetc(stdin);
182  s[i++] = c;
183  }
184  s[i-1] = s[i] = 0;
185 
186  Bottle p;
187  Bottle response;
188  bool ok=false;
189  bool rec=false;
190  p.fromString(s.c_str());
191  printf("Bottle: %s\n", p.toString().c_str());
192 
193  switch(p.get(0).asVocab32()) {
194  case VOCAB_HELP:
195  printf("\n\n");
196  printf("Available commands:\n");
197  printf("-------------------\n\n");
198 
199  printf("IPWMControl:\ntype [%s] and one of the following:\n", Vocab32::decode(VOCAB_PWMCONTROL_INTERFACE).c_str());
200  printf(" [set] [%s] <int> <float>\n", Vocab32::decode(VOCAB_PWMCONTROL_REF_PWM).c_str());
201  printf(" [get] [%s] <int>\n", Vocab32::decode(VOCAB_PWMCONTROL_REF_PWM).c_str());
202  printf(" [get] [%s]\n", Vocab32::decode(VOCAB_PWMCONTROL_PWM_OUTPUT).c_str());
203  printf("\n");
204 
205  printf("IControlMode:\ntype [%s] and one of the following:\n", Vocab32::decode(VOCAB_ICONTROLMODE).c_str());
206  printf(" [set] [%s]|[%s]|[%s]|[%s]|[%s]|[%s]|[%s]|[%s]|[%s][%s]|[%s]\n",
207  Vocab32::decode(VOCAB_CM_POSITION).c_str(),
208  Vocab32::decode(VOCAB_CM_POSITION_DIRECT).c_str(),
209  Vocab32::decode(VOCAB_CM_VELOCITY).c_str(),
210  Vocab32::decode(VOCAB_CM_MIXED).c_str(),
211  Vocab32::decode(VOCAB_CM_TORQUE).c_str(),
212  Vocab32::decode(VOCAB_CM_PWM).c_str(),
213  Vocab32::decode(VOCAB_CM_CURRENT).c_str(),
214  Vocab32::decode(VOCAB_CM_IDLE).c_str(),
215  Vocab32::decode(VOCAB_CM_FORCE_IDLE).c_str(),
216  Vocab32::decode(VOCAB_CM_IMPEDANCE_POS).c_str(),
217  Vocab32::decode(VOCAB_CM_IMPEDANCE_VEL).c_str());
218  printf(" [get] [%s] <int>\n", Vocab32::decode(VOCAB_CM_CONTROL_MODE).c_str());
219  printf("\n");
220 
221  printf("ITorqueControl:\ntype [%s] and one of the following:\n", Vocab32::decode(VOCAB_TORQUE).c_str());
222  printf(" [get] [%s] <int> to read the measured torque for a single axis\n", Vocab32::decode(VOCAB_TRQ).c_str());
223  printf(" [get] [%s] to read the measured torque for all axes\n", Vocab32::decode(VOCAB_TRQS).c_str());
224  printf(" [set] [%s] <int> <float> to set the reference torque for a single axis\n", Vocab32::decode(VOCAB_REF).c_str());
225  printf(" [set] [%s] <float list> to set the reference torque for all axes\n", Vocab32::decode(VOCAB_REFS).c_str());
226  printf(" [set] [%s] int '('<int list>')' '('<float list>')' to set the reference torque for a subset of axes axes\n", Vocab32::decode(VOCAB_REFG).c_str());
227  printf(" [get] [%s] <int> to read the reference torque for a single axis\n", Vocab32::decode(VOCAB_REF).c_str());
228  printf(" [get] [%s] to read the reference torque for all axes\n", Vocab32::decode(VOCAB_REFS).c_str());
229  printf("\n");
230 
231  printf("IImpedanceControl:\ntype [%s] and one of the following:\n", Vocab32::decode(VOCAB_IMPEDANCE).c_str());
232  printf(" [set] [%s] <int> <float> <float> \n", Vocab32::decode(VOCAB_IMP_PARAM).c_str());
233  printf(" [set] [%s] <int> <float>\n\n", Vocab32::decode(VOCAB_IMP_OFFSET).c_str());
234  printf(" [get] [%s] <int>\n", Vocab32::decode(VOCAB_IMP_PARAM).c_str());
235  printf(" [get] [%s] <int>\n", Vocab32::decode(VOCAB_IMP_OFFSET).c_str());
236  printf("\n");
237 
238  printf("IInteractionMode:\ntype [%s] and one of the following:\n", Vocab32::decode(VOCAB_INTERFACE_INTERACTION_MODE).c_str());
239  printf(" [set] [%s]|[%s] <int>\n", Vocab32::decode(VOCAB_IM_STIFF).c_str(), Vocab32::decode(VOCAB_IM_COMPLIANT).c_str());
240  printf(" [get] [%s] <int>\n", Vocab32::decode(VOCAB_INTERACTION_MODE).c_str());
241  printf(" [get] [%s] \n", Vocab32::decode(VOCAB_INTERACTION_MODES).c_str());
242  printf("\n");
243 
244  printf("IMotor Interfaces:\n");
245  printf("type [get] and one of the following:\n");
246  printf(" [%s] to read get the number of motors\n", Vocab32::decode(VOCAB_MOTORS_NUMBER).c_str());
247  printf(" [%s] <int> to read the temperature value for a single motor\n", Vocab32::decode(VOCAB_TEMPERATURE).c_str());
248  printf(" [%s] <int> to read the temperatures of all motors\n", Vocab32::decode(VOCAB_TEMPERATURES).c_str());
249  printf(" [%s] <int> to read the temperature limit for a single motor\n", Vocab32::decode(VOCAB_TEMPERATURE_LIMIT).c_str());
250  printf("type [set] and one of the following:\n");
251  printf(" [%s] <int> to set the temperature limit for a single motor\n", Vocab32::decode(VOCAB_TEMPERATURE_LIMIT).c_str());
252  printf("\n");
253 
254  printf("IMotorEncoder Interfaces:\n");
255  printf("type [get] and one of the following:\n");
256  printf(" [%s] <int> to read the cpr value for a single motor\n", Vocab32::decode(VOCAB_MOTOR_CPR).c_str());
257  printf(" [%s] to read get the number of motor encoders\n", Vocab32::decode(VOCAB_MOTOR_ENCODER_NUMBER).c_str());
258  printf(" [%s] to read the motor encoder positions for all motors\n", Vocab32::decode(VOCAB_MOTOR_ENCODERS).c_str());
259  printf(" [%s] to read the motor encoder speeds for all motors\n", Vocab32::decode(VOCAB_MOTOR_ENCODER_SPEEDS).c_str());
260  printf(" [%s] to read the motor encoder accelerations for all motors\n", Vocab32::decode(VOCAB_MOTOR_ENCODER_ACCELERATIONS).c_str());
261  printf("\n");
262 
263  printf("Standard Interfaces:\n");
264  printf("type [get] and one of the following:\n");
265  printf(" [%s] to read the number of controlled axes\n", Vocab32::decode(VOCAB_AXES).c_str());
266  printf(" [%s] <int> to read the name of a single axis\n", Vocab32::decode(VOCAB_INFO_NAME).c_str());
267  printf(" [%s] <int> to read target position for a single axis (iPositionControl)\n", Vocab32::decode(VOCAB_POSITION_MOVE).c_str());
268  printf(" [%s] to read target positions for all axes (iPositionControl)\n", Vocab32::decode(VOCAB_POSITION_MOVES).c_str());
269  printf(" [%s] <int> to read reference position for single axis (iPositionDirect)\n", Vocab32::decode(VOCAB_POSITION_DIRECT).c_str());
270  printf(" [%s] to read reference position for all axes (iPositionDirect)\n", Vocab32::decode(VOCAB_POSITION_DIRECTS).c_str());
271  printf(" [%s] <int> to read reference velocity for single axis (for velocityMove)\n", Vocab32::decode(VOCAB_VELOCITY_MOVE).c_str());
272  printf(" [%s] to read reference velocities for all axes (for velocityMove)\n", Vocab32::decode(VOCAB_VELOCITY_MOVES).c_str());
273  printf(" [%s] to read the encoder value for all axes\n", Vocab32::decode(VOCAB_ENCODERS).c_str());
274  printf(" [%s] to read the PID values for all axes\n", Vocab32::decode(VOCAB_PIDS).c_str());
275  printf(" [%s] <int> to read the PID values for a single axis\n", Vocab32::decode(VOCAB_PID).c_str());
276  printf(" [%s] <int> to read the position limits for a single axis\n", Vocab32::decode(VOCAB_LIMITS).c_str());
277  printf(" [%s] <int> to read the velocity limits for a single axis\n", Vocab32::decode(VOCAB_VEL_LIMITS).c_str());
278  printf(" [%s] to read the PID error for all axes\n", Vocab32::decode(VOCAB_ERRS).c_str());
279  printf(" [%s] to read the PID output for all axes\n", Vocab32::decode(VOCAB_OUTPUTS).c_str());
280  printf(" [%s] to read the reference position for all axes\n", Vocab32::decode(VOCAB_REFERENCES).c_str());
281  printf(" [%s] <int> to read the reference position for a single axis\n", Vocab32::decode(VOCAB_REFERENCE).c_str());
282  printf(" [%s] to read the reference speed for all axes\n", Vocab32::decode(VOCAB_REF_SPEEDS).c_str());
283  printf(" [%s] <int> to read the reference speed for a single axis\n", Vocab32::decode(VOCAB_REF_SPEED).c_str());
284  printf(" [%s] to read the reference acceleration for all axes\n", Vocab32::decode(VOCAB_REF_ACCELERATIONS).c_str());
285  printf(" [%s] <int> to read the reference acceleration for a single axis\n", Vocab32::decode(VOCAB_REF_ACCELERATION).c_str());
286  printf(" [%s] to read the current consumption for all axes\n", Vocab32::decode(VOCAB_AMP_CURRENTS).c_str());
287  printf(" [%s] <int> to get the nominal current for a motor\n", Vocab32::decode(VOCAB_AMP_NOMINAL_CURRENT).c_str());
288  printf(" [%s] <int> to get the peak current for a motor\n", Vocab32::decode(VOCAB_AMP_PEAK_CURRENT).c_str());
289  printf(" [%s] <int> to get the PWM output for a motor\n", Vocab32::decode(VOCAB_AMP_PWM).c_str());
290  printf(" [%s] <int> to get the PWM limit for a motor\n", Vocab32::decode(VOCAB_AMP_PWM_LIMIT).c_str());
291  printf(" [%s] <int> to get the power supply voltage for a single motor\n", Vocab32::decode(VOCAB_AMP_VOLTAGE_SUPPLY).c_str());
292  printf(" [%s] <int> to get the current limit of single motor\n", Vocab32::decode(VOCAB_AMP_MAXCURRENT).c_str());
293  printf(" [%s] <int> to check motionDone on a single motor\n", Vocab32::decode(VOCAB_MOTION_DONE).c_str());
294  printf(" [%s] to check motionDone on all motors \n", Vocab32::decode(VOCAB_MOTION_DONES).c_str());
295  printf(" [%s] to check motionDone on all motors using multiple functions\n", Vocab32::decode(VOCAB_MOTION_DONE_GROUP).c_str());
296  printf("\n");
297 
298  printf("type [set] and one of the following:\n");
299  printf(" [%s] <int> <double> to move a single axis\n", Vocab32::decode(VOCAB_POSITION_MOVE).c_str());
300  printf(" [%s] <int> <double> to accelerate a single axis to a given speed\n", Vocab32::decode(VOCAB_VELOCITY_MOVE).c_str());
301  printf(" [%s] <int> <double> to set the reference speed for a single axis\n", Vocab32::decode(VOCAB_REF_SPEED).c_str());
302  printf(" [%s] <int> <double> to set the reference acceleration for a single axis\n", Vocab32::decode(VOCAB_REF_ACCELERATION).c_str());
303  printf(" [%s] <list> to move multiple axes\n", Vocab32::decode(VOCAB_POSITION_MOVES).c_str());
304  printf(" [%s] <list> to accelerate multiple axes to a given speed\n", Vocab32::decode(VOCAB_VELOCITY_MOVES).c_str());
305  printf(" [%s] <list> to set the reference speed for all axes\n", Vocab32::decode(VOCAB_REF_SPEEDS).c_str());
306  printf(" [%s] <list> to set the reference acceleration for all axes\n", Vocab32::decode(VOCAB_REF_ACCELERATIONS).c_str());
307  printf(" [%s] <int> to stop a single axis\n", Vocab32::decode(VOCAB_STOP).c_str());
308  printf(" [%s] <int> to stop all axes\n", Vocab32::decode(VOCAB_STOPS).c_str());
309  printf(" [%s] <int> <double> to move single axis using position direct\n", Vocab32::decode(VOCAB_POSITION_DIRECT).c_str());
310  printf(" [%s] <list> to move multiple axes using position direct\n", Vocab32::decode(VOCAB_POSITION_DIRECTS).c_str());
311  printf(" [%s] <int> <list> to set the PID values for a single axis\n", Vocab32::decode(VOCAB_PID).c_str());
312  printf(" [%s] <int> <list> to set the limits for a single axis\n", Vocab32::decode(VOCAB_LIMITS).c_str());
313  printf(" [%s] <int> to disable the PID control for a single axis\n", Vocab32::decode(VOCAB_DISABLE).c_str());
314  printf(" [%s] <int> to enable the PID control for a single axis\n", Vocab32::decode(VOCAB_ENABLE).c_str());
315  printf(" [%s] <int> <double> to set the encoder value for a single axis\n", Vocab32::decode(VOCAB_ENCODER).c_str());
316  printf(" [%s] <list> to set the encoder value for all axes\n", Vocab32::decode(VOCAB_ENCODERS).c_str());
317  printf(" [%s] <int> <double> to set the current limit for single motor\n", Vocab32::decode(VOCAB_AMP_MAXCURRENT).c_str());
318  printf(" [%s] <int> <double> to set the peak current for a motor\n", Vocab32::decode(VOCAB_AMP_PEAK_CURRENT).c_str());
319  printf(" [%s] <int> <double> to set the PWM limit for a motor\n", Vocab32::decode(VOCAB_AMP_PWM_LIMIT).c_str());
320  printf("\n");
321 
322  printf("NOTES: - A list is a sequence of numbers in parenthesis, e.g. (10 2 1 10)\n");
323  printf(" - Pids are expressed as a list of 7 numbers, type get pid <int> to see an example\n");
324  printf("\n");
325 
326  if (options.check("debug"))
327  {
328  //#define VOCAB_CM_HW_FAULT yarp::os::createVocab32('h','w','f','a')
329  //#define VOCAB_CM_CALIBRATING yarp::os::createVocab32('c','a','l') // the joint is calibrating
330  //#define VOCAB_CM_CALIB_DONE yarp::os::createVocab32('c','a','l','d') // calibration succesfully completed
331  //#define VOCAB_CM_NOT_CONFIGURED yarp::os::createVocab32('c','f','g','n') // missing initial configuration (default value at start-up)
332  //#define VOCAB_CM_CONFIGURED yarp::os::createVocab32('c','f','g','y') // initial configuration completed, if any
333  printf("DEBUG NOTES: (hidden debug commands wich may break the robot! do not use!\n");
334  printf("- icdd set hwfa 1 : will try to force joint 1 in hw fault\n");
335  printf("- icdd set cal 1 : \n");
336  printf("- icdd set cald 1 : \n");
337  printf("- icdd set cfgy 1 : \n");
338  printf("- icdd set cfgn 1 : \n");
339  printf("- icdd set cmuk 1 : \n");
340  }
341  break;
342 
343  case VOCAB_QUIT:
344  goto ApplicationCleanQuit;
345  break;
346 
347  case VOCAB_ICONTROLMODE:
348  {
349  handleControlModeMsg(iMode2, p, response, &rec, &ok);
350  printf("%s\n", response.toString().c_str());
351  break;
352  }
353 
354  case VOCAB_ICONTROLMODE_DEBUG:
355  {
356  handleControlModeMsg_DEBUG(iMode2, p, response, &rec, &ok);
357  printf("%s\n", response.toString().c_str());
358  break;
359  }
360 
361  case VOCAB_IMPEDANCE:
362  {
363  handleImpedanceMsg(iimp, p, response, &rec, &ok);
364  printf("%s\n", response.toString().c_str());
365  break;
366  }
367 
368  case VOCAB_TORQUE:
369  {
370  handleTorqueMsg(itorque, pid, p, response, &rec, &ok);
371  printf("%s\n", response.toString().c_str());
372  break;
373  }
374 
375  case VOCAB_INTERFACE_INTERACTION_MODE:
376  {
377  handleInteractionModeMsg(iInteract, p, response, &rec, &ok);
378  printf("%s\n", response.toString().c_str());
379  break;
380  }
381 
382  case VOCAB_GET:
383  switch(p.get(1).asVocab32())
384  {
385  case VOCAB_POSITION_MOVE:
386  {
387  if(!ipos)
388  {
389  printf ("unavailable interface\n");
390  break;
391  }
392  double ref;
393  int j = p.get(2).asInt32();
394  bool ret = ipos->getTargetPosition(j, &ref);
395  printf("Ref joint %d is %.2f - [ret val is %s]\n", j, ref, ret?"true":"false");
396  }
397  break;
398 
399  case VOCAB_POSITION_MOVES:
400  {
401  if(!ipos)
402  {
403  printf ("unavailable interface\n");
404  break;
405  }
406  bool ret = ipos->getTargetPositions(tmp);
407  printf ("%s: (", Vocab32::decode(VOCAB_POSITION_MOVES).c_str());
408  for(i = 0; i < jnts; i++)
409  printf ("%.2f ", tmp[i]);
410  printf (") - ret is [%s]\n", ret? "true":"false");
411  }
412  break;
413 
414  case VOCAB_POSITION_DIRECT:
415  {
416  ok = iposDir->getRefPosition(p.get(3).asInt32(), tmp);
417  response.addFloat64(tmp[0]);
418  printf("Ref joint %d is %.2f - [ret val is %s]\n", p.get(3).asInt32(), tmp[0], ok?"true":"false");
419  }
420  break;
421 
422  case VOCAB_POSITION_DIRECTS:
423  {
424  ok = iposDir->getRefPositions(tmp);
425  Bottle& b = response.addList();
426  int i;
427  for (i = 0; i < jnts; i++)
428  b.addFloat64(tmp[i]);
429  for(i = 0; i < jnts; i++)
430  printf ("%.2f ", tmp[i]);
431  printf (" - ret is [%s]\n", ok? "true":"false");
432  }
433  break;
434 
435  case VOCAB_VELOCITY_MOVE:
436  {
437  ok = vel->getRefVelocity(p.get(2).asInt32(), tmp);
438  response.addFloat64(tmp[0]);
439  printf("Ref vel joint %d is %.2f - [ret val is %s]\n", p.get(3).asInt32(), tmp[0], ok?"true":"false");
440  }
441  break;
442 
443  case VOCAB_VELOCITY_MOVES:
444  {
445  ok = vel->getRefVelocities(tmp);
446  Bottle& b = response.addList();
447  int i;
448  for (i = 0; i < jnts; i++)
449  b.addFloat64(tmp[i]);
450  for(i = 0; i < jnts; i++)
451  printf ("%.2f ", tmp[i]);
452  printf (" - ret is [%s]\n", ok? "true":"false");
453  }
454  break;
455 
456  case VOCAB_INFO_NAME:
457  {
458  int j = p.get(2).asInt32();
459  if (iInfo == 0) { printf("unavailable interface\n"); break; }
460  std::string tmp_str;
461  iInfo->getAxisName(j,tmp_str);
462  printf("%s: %d %s\n", Vocab32::decode(VOCAB_INFO_NAME).c_str(), j, tmp_str.c_str());
463  }
464  break;
465 
466  case VOCAB_AXES: {
467  int nj = 0;
468  enc->getAxes(&nj);
469  printf ("%s: %d\n", Vocab32::decode(VOCAB_AXES).c_str(), nj);
470  }
471  break;
472 
473  case VOCAB_MOTION_DONE:
474  {
475  if (ipos==0) {yError ("unavailable interface iPos\n"); break;}
476  bool b=false;
477  int j = p.get(2).asInt32();
478  ipos->checkMotionDone(j,&b);
479  if (b==true) printf("1");
480  else printf ("0");
481  }
482  break;
483 
484  case VOCAB_MOTION_DONES:
485  {
486  if (ipos==0) {yError ("unavailable interface iPos\n"); break;}
487  bool b=false;
488  ipos->checkMotionDone(&b);
489  if (b==true) printf("1");
490  else printf ("0");
491  }
492  break;
493 
494  case VOCAB_MOTION_DONE_GROUP:
495  {
496  if (ipos==0) {yError ("unavailable interface iPos\n"); break;}
497  bool b=false;
498  ipos->checkMotionDone(jnts, jtmp ,&b);
499  if (b==true) printf("1");
500  else printf ("0");
501  }
502  break;
503 
504  case VOCAB_ENCODERS: {
505  enc->getEncoders(tmp);
506  printf ("%s: (", Vocab32::decode(VOCAB_ENCODERS).c_str());
507  for(i = 0; i < jnts; i++)
508  printf ("%.2f ", tmp[i]);
509  printf (")\n");
510  }
511  break;
512 
513  case VOCAB_MOTOR_ENCODERS: {
514  if (iMotEnc==0) {printf ("unavailable interface\n"); break;}
515  iMotEnc->getMotorEncoders(tmp);
516  printf ("%s: (", Vocab32::decode(VOCAB_MOTOR_ENCODERS).c_str());
517  for(i = 0; i < jnts; i++)
518  printf ("%.2f ", tmp[i]);
519  printf (")\n");
520  }
521  break;
522 
523  case VOCAB_MOTOR_ENCODER_SPEEDS: {
524  if (iMotEnc==0) {printf ("unavailable interface\n"); break;}
525  iMotEnc->getMotorEncoderSpeeds(tmp);
526  printf ("%s: (", Vocab32::decode(VOCAB_MOTOR_ENCODER_SPEEDS).c_str());
527  for(i = 0; i < jnts; i++)
528  printf ("%.2f ", tmp[i]);
529  printf (")\n");
530  }
531  break;
532 
533  case VOCAB_MOTOR_ENCODER_ACCELERATIONS: {
534  if (iMotEnc==0) {printf ("unavailable interface\n"); break;}
535  iMotEnc->getMotorEncoderAccelerations(tmp);
536  printf ("%s: (", Vocab32::decode(VOCAB_MOTOR_ENCODER_ACCELERATIONS).c_str());
537  for(i = 0; i < jnts; i++)
538  printf ("%.2f ", tmp[i]);
539  printf (")\n");
540  }
541  break;
542 
543  case VOCAB_MOTOR_CPR:
544  {
545  if (iMotEnc==0) {printf ("unavailable interface\n"); break;}
546  int j = p.get(2).asInt32();
547  double v;
548  iMotEnc->getMotorEncoderCountsPerRevolution(j, &v);
549  printf("%s: ", Vocab32::decode(VOCAB_MOTOR_CPR).c_str());
550  printf("%.2f ", v);
551  printf("\n");
552  }
553  break;
554 
555  case VOCAB_AMP_MAXCURRENT:
556  {
557  if (amp==0) {printf ("unavailable interface\n"); break;}
558  int j = p.get(2).asInt32();
559  double v;
560  amp->getMaxCurrent(j, &v);
561  printf("%s: ", Vocab32::decode(VOCAB_AMP_MAXCURRENT).c_str());
562  printf("%.2f ", v);
563  printf("\n");
564  }
565  break;
566 
567  case VOCAB_MOTORS_NUMBER:
568  {
569  if (imot==0) {printf ("unavailable interface\n"); break;}
570  int v;
571  imot->getNumberOfMotors(&v);
572  printf("%s: ", Vocab32::decode(VOCAB_MOTORS_NUMBER).c_str());
573  printf("%d ", v);
574  printf("\n");
575  }
576  break;
577 
578  case VOCAB_MOTOR_ENCODER_NUMBER:
579  {
580  if (iMotEnc==0) {printf ("unavailable interface\n"); break;}
581  int v;
582  iMotEnc->getNumberOfMotorEncoders(&v);
583  printf("%s: ", Vocab32::decode(VOCAB_MOTOR_ENCODER_NUMBER).c_str());
584  printf("%d ", v);
585  printf("\n");
586  }
587  break;
588 
589  case VOCAB_PID: {
590  Pid pd;
591  int j = p.get(2).asInt32();
592  pid->getPid(VOCAB_PIDTYPE_POSITION, j, &pd);
593  printf("%s: ", Vocab32::decode(VOCAB_PID).c_str());
594  printf("kp %.2f ", pd.kp);
595  printf("kd %.2f ", pd.kd);
596  printf("ki %.2f ", pd.ki);
597  printf("maxi %.2f ", pd.max_int);
598  printf("maxo %.2f ", pd.max_output);
599  printf("off %.2f ", pd.offset);
600  printf("scale %.2f ", pd.scale);
601  printf("\n");
602  }
603  break;
604 
605  case VOCAB_PIDS: {
606  Pid *p = new Pid[jnts];
607  ok = pid->getPids(VOCAB_PIDTYPE_POSITION, p);
608  Bottle& b = response.addList();
609  int i;
610  for (i = 0; i < jnts; i++)
611  {
612  Bottle& c = b.addList();
613  c.addFloat64(p[i].kp);
614  c.addFloat64(p[i].kd);
615  c.addFloat64(p[i].ki);
616  c.addFloat64(p[i].max_int);
617  c.addFloat64(p[i].max_output);
618  c.addFloat64(p[i].offset);
619  c.addFloat64(p[i].scale);
620  }
621  printf("%s\n", b.toString().c_str());
622  delete[] p;
623  }
624  break;
625 
626  case VOCAB_LIMITS: {
627  double min, max;
628  int j = p.get(2).asInt32();
629  lim->getLimits(j, &min, &max);
630  printf("%s: ", Vocab32::decode(VOCAB_LIMITS).c_str());
631  printf("limits: (%.2f %.2f)\n", min, max);
632  }
633  break;
634 
635  case VOCAB_VEL_LIMITS: {
636  double min, max;
637  int j = p.get(2).asInt32();
638  lim->getVelLimits(j, &min, &max);
639  printf("%s: ", Vocab32::decode(VOCAB_VEL_LIMITS).c_str());
640  printf("limits: (%.2f %.2f)\n", min, max);
641  }
642  break;
643 
644  case VOCAB_ERRS: {
645  pid->getPidErrors(VOCAB_PIDTYPE_POSITION, tmp);
646  printf ("%s: (", Vocab32::decode(VOCAB_ERRS).c_str());
647  for(i = 0; i < jnts; i++)
648  printf ("%.2f ", tmp[i]);
649  printf (")\n");
650  }
651  break;
652 
653  case VOCAB_TEMPERATURES: {
654  if (imot==0) {printf ("unavailable interface\n"); break;}
655  imot->getTemperatures(tmp);
656  printf ("%s: (", Vocab32::decode(VOCAB_TEMPERATURES).c_str());
657  for(i = 0; i < jnts; i++)
658  printf ("%.2f ", tmp[i]);
659  printf (")\n");
660  }
661  break;
662 
663  case VOCAB_TEMPERATURE: {
664  if (imot==0) {printf ("unavailable interface\n"); break;}
665  int j = p.get(2).asInt32();
666  double v;
667  imot->getTemperature(j, &v);
668  printf("%s: ", Vocab32::decode(VOCAB_TEMPERATURE).c_str());
669  printf("%.2f ", v);
670  printf("\n");
671  }
672  break;
673 
674  case VOCAB_TEMPERATURE_LIMIT: {
675  if (imot==0) {printf ("unavailable interface\n"); break;}
676  int j = p.get(2).asInt32();
677  double v;
678  imot->getTemperatureLimit(j, &v);
679  printf("%s: ", Vocab32::decode(VOCAB_TEMPERATURE_LIMIT).c_str());
680  printf("%.2f ", v);
681  printf("\n");
682  }
683  break;
684 
685  case VOCAB_PWMCONTROL_PWM_OUTPUTS: {
686  if (ipwm==0) {printf ("unavailable interface\n"); break;}
687  ipwm->getDutyCycles(tmp);
688  printf("%s: (", Vocab32::decode(VOCAB_PWMCONTROL_PWM_OUTPUTS).c_str());
689  for(i = 0; i < jnts; i++)
690  printf ("%.2f ", tmp[i]);
691  printf (")\n");
692  }
693  break;
694 
695  case VOCAB_PWMCONTROL_PWM_OUTPUT:
696  //case VOCAB_AMP_PWM:
697  {
698  if (ipwm == 0) { printf("unavailable interface\n"); break; }
699  int j = p.get(2).asInt32();
700  double v;
701  ipwm->getDutyCycle(j, &v);
702  printf("%s: ", Vocab32::decode(VOCAB_PWMCONTROL_PWM_OUTPUT).c_str());
703  printf("%.2f ", v);
704  printf("\n");
705  }
706  break;
707 
708  case VOCAB_REFERENCE: {
709  double ref_pos;
710  int j = p.get(2).asInt32();
711  pid->getPidReference(VOCAB_PIDTYPE_POSITION, j,&ref_pos);
712  printf ("%s: (", Vocab32::decode(VOCAB_REFERENCE).c_str());
713  printf ("%.2f ", ref_pos);
714  printf (")\n");
715  }
716  break;
717 
718  case VOCAB_REFERENCES: {
719  pid->getPidReferences(VOCAB_PIDTYPE_POSITION, tmp);
720  printf ("%s: (", Vocab32::decode(VOCAB_REFERENCES).c_str());
721  for(i = 0; i < jnts; i++)
722  printf ("%.2f ", tmp[i]);
723  printf (")\n");
724  }
725  break;
726 
727  case VOCAB_REF_SPEEDS: {
728  ipos->getRefSpeeds(tmp);
729  printf ("%s: (", Vocab32::decode(VOCAB_REF_SPEEDS).c_str());
730  for(i = 0; i < jnts; i++)
731  printf ("%.2f ", tmp[i]);
732  printf (")\n");
733  }
734  break;
735 
736  case VOCAB_REF_SPEED: {
737  double ref_speed;
738  int j = p.get(2).asInt32();
739  ipos->getRefSpeed(j,&ref_speed);
740  printf ("%s: (", Vocab32::decode(VOCAB_REF_SPEED).c_str());
741  printf ("%.2f ", ref_speed);
742  printf (")\n");
743  }
744  break;
745 
746  case VOCAB_REF_ACCELERATION: {
747  double ref_acc;
748  int j = p.get(2).asInt32();
749  ipos->getRefAcceleration(j,&ref_acc);
750  printf ("%s: (", Vocab32::decode(VOCAB_REF_ACCELERATION).c_str());
751  printf ("%.2f ", ref_acc);
752  printf (")\n");
753  }
754  break;
755 
756  case VOCAB_REF_ACCELERATIONS: {
757  ipos->getRefAccelerations(tmp);
758  printf ("%s: (", Vocab32::decode(VOCAB_REF_ACCELERATIONS).c_str());
759  for(i = 0; i < jnts; i++)
760  printf ("%.2f ", tmp[i]);
761  printf (")\n");
762  }
763  break;
764 
765  case VOCAB_AMP_CURRENTS: {
766  amp->getCurrents(tmp);
767  printf ("%s: (", Vocab32::decode(VOCAB_AMP_CURRENTS).c_str());
768  for(i = 0; i < jnts; i++)
769  printf ("%.2f ", tmp[i]);
770  printf (")\n");
771  }
772  break;
773 
774  case VOCAB_AMP_PWM_LIMIT:
775  {
776  int j = p.get(2).asInt32();
777  amp->getPWMLimit(j, tmp);
778  printf ("%s: (", Vocab32::decode(VOCAB_AMP_PWM_LIMIT).c_str());
779  printf ("%.2f )\n", tmp[0]);
780  }
781  break;
782 
783  case VOCAB_AMP_PEAK_CURRENT:
784  {
785  int j = p.get(2).asInt32();
786  amp->getPeakCurrent(j, tmp);
787  printf ("%s: (", Vocab32::decode(VOCAB_AMP_PEAK_CURRENT).c_str());
788  printf ("%.2f )\n", tmp[0]);
789  }
790  break;
791 
792  case VOCAB_AMP_NOMINAL_CURRENT:
793  {
794  int j = p.get(2).asInt32();
795  amp->getNominalCurrent(j, tmp);
796  printf ("%s: (", Vocab32::decode(VOCAB_AMP_NOMINAL_CURRENT).c_str());
797  printf ("%.2f )\n", tmp[0]);
798  }
799  break;
800 
801  case VOCAB_AMP_VOLTAGE_SUPPLY:
802  {
803  int j = p.get(2).asInt32();
804  amp->getPowerSupplyVoltage(j, tmp);
805  printf ("%s: (", Vocab32::decode(VOCAB_AMP_VOLTAGE_SUPPLY).c_str());
806  printf ("%.2f )\n", tmp[0]);
807  }
808  break;
809  }
810  break;
811 
812  case VOCAB_SET:
813  switch(p.get(1).asVocab32()) {
814  case VOCAB_POSITION_MOVE: {
815  int j = p.get(2).asInt32();
816  double ref = p.get(3).asFloat64();
817  printf("%s: moving %d to %.2f\n", Vocab32::decode(VOCAB_POSITION_MOVE).c_str(), j, ref);
818  ipos->positionMove(j, ref);
819  }
820  break;
821 
822  case VOCAB_VELOCITY_MOVE: {
823  int j = p.get(2).asInt32();
824  double ref = p.get(3).asFloat64();
825  printf("%s: accelerating %d to %.2f\n", Vocab32::decode(VOCAB_VELOCITY_MOVE).c_str(), j, ref);
826  vel->velocityMove(j, ref);
827  }
828  break;
829 
830  case VOCAB_REF_SPEED: {
831  int j = p.get(2).asInt32();
832  double ref = p.get(3).asFloat64();
833  printf("%s: setting speed for %d to %.2f\n", Vocab32::decode(VOCAB_REF_SPEED).c_str(), j, ref);
834  ipos->setRefSpeed(j, ref);
835  }
836  break;
837 
838  case VOCAB_REF_ACCELERATION: {
839  int j = p.get(2).asInt32();
840  double ref = p.get(3).asFloat64();
841  printf("%s: setting acceleration for %d to %.2f\n", Vocab32::decode(VOCAB_REF_ACCELERATION).c_str(), j, ref);
842  ipos->setRefAcceleration(j, ref);
843  }
844  break;
845 
846  case VOCAB_POSITION_MOVES: {
847  Bottle *l = p.get(2).asList();
848  printf("received %s\n", l->toString().c_str());
849  for (i = 0; i < jnts; i++) {
850  printf("%d - ", i); fflush(stdout);
851  tmp[i] = l->get(i).asFloat64();
852  printf("tmp[i] %f\n", tmp[i]); fflush(stdout);
853  }
854  printf("%s: moving all joints\n", Vocab32::decode(VOCAB_POSITION_MOVES).c_str());
855  ipos->positionMove(tmp);
856  }
857  break;
858 
859  case VOCAB_VELOCITY_MOVES: {
860  Bottle *l = p.get(2).asList();
861  for (i = 0; i < jnts; i++) {
862  tmp[i] = l->get(i).asFloat64();
863  }
864  printf("%s: moving all joints\n", Vocab32::decode(VOCAB_VELOCITY_MOVES).c_str());
865  vel->velocityMove(tmp);
866  }
867  break;
868 
869  case VOCAB_REF_SPEEDS: {
870  Bottle *l = p.get(2).asList();
871  for (i = 0; i < jnts; i++) {
872  tmp[i] = l->get(i).asFloat64();
873  }
874  printf("%s: setting speed for all joints\n", Vocab32::decode(VOCAB_REF_SPEEDS).c_str());
875  ipos->setRefSpeeds(tmp);
876  }
877  break;
878 
879  case VOCAB_REF_ACCELERATIONS: {
880  Bottle *l = p.get(2).asList();
881  for (i = 0; i < jnts; i++) {
882  tmp[i] = l->get(i).asFloat64();
883  }
884  printf("%s: setting acceleration for all joints\n", Vocab32::decode(VOCAB_REF_ACCELERATIONS).c_str());
885  ipos->setRefAccelerations(tmp);
886  }
887  break;
888 
889  case VOCAB_STOP: {
890  int j = p.get(2).asInt32();
891  printf("%s: stopping axis %d\n", Vocab32::decode(VOCAB_STOP).c_str(), j);
892  ipos->stop(j);
893  }
894  break;
895 
896  case VOCAB_STOPS: {
897  printf("%s: stopping all axes\n", Vocab32::decode(VOCAB_STOPS).c_str());
898  ipos->stop();
899  }
900  break;
901 
902 
903  case VOCAB_POSITION_DIRECT: {
904  printf("%s: setting position direct reference\n", Vocab32::decode(VOCAB_POSITION_DIRECT).c_str());
905  iposDir->setPosition(p.get(2).asInt32(), p.get(3).asFloat64());
906  }
907  break;
908 
909  case VOCAB_POSITION_DIRECTS: {
910  printf("%s: setting position direct references to %s\n", Vocab32::decode(VOCAB_POSITION_DIRECTS).c_str(), p.toString().c_str() );
911  Bottle *l = p.get(2).asList();
912  for (i = 0; i < jnts; i++) {
913  tmp[i] = l->get(i).asFloat64();
914  }
915  iposDir->setPositions(tmp);
916  }
917  break;
918 
919  case VOCAB_ENCODER: {
920  int j = p.get(2).asInt32();
921  double ref = p.get(3).asFloat64();
922  printf("%s: setting the encoder value for %d to %.2f\n", Vocab32::decode(VOCAB_ENCODER).c_str(), j, ref);
923  enc->setEncoder(j, ref);
924  }
925  break;
926 
927  case VOCAB_ENCODERS: {
928  Bottle *l = p.get(2).asList();
929  for (i = 0; i < jnts; i++) {
930  tmp[i] = l->get(i).asFloat64();
931  }
932  printf("%s: setting the encoder value for all joints\n", Vocab32::decode(VOCAB_ENCODERS).c_str());
933  enc->setEncoders(tmp);
934  }
935  break;
936 
937  case VOCAB_PID: {
938  Pid pd;
939  int j = p.get(2).asInt32();
940  Bottle *l = p.get(3).asList();
941  if (l==0)
942  {
943  printf("Check you specify a 7 elements list, e.g. set pid 0 (2000 20 1 300 300 0 0)\n");
944  }
945  else
946  {
947  int elems=l->size();
948  if (elems>=3)
949  {
950  pd.kp = l->get(0).asFloat64();
951  pd.kd = l->get(1).asFloat64();
952  pd.ki = l->get(2).asFloat64();
953  if (elems>=7)
954  {
955  pd.max_int = l->get(3).asFloat64();
956  pd.max_output = l->get(4).asFloat64();
957  pd.offset = l->get(5).asFloat64();
958  pd.scale = l->get(6).asFloat64();
959  }
960  printf("%s: setting PID values for axis %d\n", Vocab32::decode(VOCAB_PID).c_str(), j);
961  pid->setPid(VOCAB_PIDTYPE_POSITION, j, pd);
962  }
963  else
964  {
965  printf("Error, check you specify at least 7 elements, e.g. set pid 0 (2000 20 1 300 300 0 0)\n");
966  }
967  }
968  }
969  break;
970 
971  case VOCAB_TEMPERATURE_LIMIT: {
972  if (imot==0) {printf ("unavailable interface\n"); break;}
973  int j=p.get(2).asInt32();
974  double v=p.get(3).asFloat64();
975  imot->setTemperatureLimit(j,v);
976  printf("%s: setting temperature limit for axis %d to %f\n", Vocab32::decode(VOCAB_TEMPERATURE_LIMIT).c_str(), j, v);
977  }
978  break;
979 
980  case VOCAB_AMP_MAXCURRENT: {
981  int j=p.get(2).asInt32();
982  double v=p.get(3).asFloat64();
983  amp->setMaxCurrent(j,v);
984  printf("%s: setting max current for motor %d to %f\n", Vocab32::decode(VOCAB_AMP_MAXCURRENT).c_str(), j, v);
985  }
986  break;
987 
988  case VOCAB_DISABLE: {
989  int j = p.get(2).asInt32();
990  printf("%s: disabling control for axis %d\n", Vocab32::decode(VOCAB_DISABLE).c_str(), j);
991  pid->disablePid(VOCAB_PIDTYPE_POSITION,j);
992  amp->disableAmp(j);
993  }
994  break;
995 
996  case VOCAB_ENABLE: {
997  int j = p.get(2).asInt32();
998  printf("%s: enabling control for axis %d\n", Vocab32::decode(VOCAB_ENABLE).c_str(), j);
999  amp->enableAmp(j);
1000  pid->enablePid(VOCAB_PIDTYPE_POSITION,j);
1001  }
1002  break;
1003 
1004  case VOCAB_LIMITS: {
1005  int j = p.get(2).asInt32();
1006  printf("%s: setting limits for axis %d\n", Vocab32::decode(VOCAB_LIMITS).c_str(), j);
1007  Bottle *l = p.get(3).asList();
1008  lim->setLimits(j, l->get(0).asFloat64(), l->get(1).asFloat64());
1009  }
1010  break;
1011 
1012  case VOCAB_AMP_PWM_LIMIT:
1013  {
1014  int j = p.get(2).asInt32();
1015  double lim = p.get(3).asFloat64();
1016  bool ret = amp->setPWMLimit(j, lim);
1017  printf("%s: setting PWM limits for axis %d to %0.2f - ret: %d\n", Vocab32::decode(VOCAB_AMP_PWM_LIMIT).c_str(), j, lim, ret);
1018  }
1019  break;
1020 
1021  case VOCAB_AMP_PEAK_CURRENT:
1022  {
1023  int j = p.get(2).asInt32();
1024  double lim = p.get(3).asFloat64();
1025  bool ret = amp->setPeakCurrent(j, lim);
1026  printf("%s: setting peak current for axis %d to %0.2f - ret: %d\n", Vocab32::decode(VOCAB_AMP_PEAK_CURRENT).c_str(), j, lim, ret);
1027  }
1028  break;
1029 
1030  case VOCAB_PWMCONTROL_REF_PWM: {
1031  int j=p.get(2).asInt32();
1032  double v=p.get(3).asFloat64();
1033  ipwm->setRefDutyCycle(j,v);
1034  printf("%s: setting pwm for axis %d to %f\n", Vocab32::decode(VOCAB_PWMCONTROL_REF_PWM).c_str(), j, v);
1035  }
1036  break;
1037  }
1038  break;
1039  } /* switch get(0) */
1040 
1041  } /* while () */
1042 
1043 ApplicationCleanQuit:
1044  dd.close();
1045  delete[] tmp;
1046  delete[] btmp;
1047  delete[] jtmp;
1048 
1049  Network::fini();
1050  return 0;
1051 }
1052 
1053 void handleTorqueMsg(ITorqueControl *torque, IPidControl *ipid, const yarp::os::Bottle& cmd,
1054  yarp::os::Bottle& response, bool *rec, bool *ok)
1055 {
1056  fprintf(stderr, "Handling ITorque messages\n");
1057 
1058  if (!torque)
1059  {
1060  fprintf(stderr, "Error, I do not have a valid ITorque interface\n");
1061  *ok=false;
1062  return;
1063  }
1064 
1065  int controlledJoints;
1066  torque->getAxes(&controlledJoints);
1067 
1068  int code = cmd.get(1).asVocab32();
1069  switch (code)
1070  {
1071  case VOCAB_SET:
1072  {
1073  *rec = true;
1074 
1075  switch(cmd.get(2).asVocab32())
1076  {
1077  case VOCAB_REF:
1078  {
1079  *ok = torque->setRefTorque(cmd.get(3).asInt32(), cmd.get(4).asFloat64());
1080  }
1081  break;
1082 
1083  case VOCAB_REFS:
1084  {
1085  Bottle& b = *(cmd.get(3).asList());
1086  int i;
1087  const int njs = b.size();
1088  if (njs==controlledJoints)
1089  {
1090  double *p = new double[njs]; // LATER: optimize to avoid allocation.
1091  for (i = 0; i < njs; i++)
1092  p[i] = b.get(i).asFloat64();
1093  *ok = torque->setRefTorques (p);
1094  delete[] p;
1095  }
1096  }
1097  break;
1098 
1099 
1100  case VOCAB_REFG:
1101  {
1102  int n_joint = cmd.get(3).asInt32();
1103 
1104  Bottle& jointList = *(cmd.get(4).asList());
1105  Bottle& refList = *(cmd.get(5).asList());
1106  int i;
1107  const int joint_size = jointList.size();
1108  const int refs_size = refList.size();
1109 
1110  if( (joint_size != n_joint) || (refs_size != n_joint) )
1111  {
1112  yError() << "command malformed, size of joint list and reference list must match the number of joints (" << n_joint << ")";
1113  *rec = false;
1114  break;
1115  }
1116 
1117  if (n_joint > controlledJoints)
1118  {
1119  yError() << "command malformed, number of joints inserted is bigger than number of available joints (" << controlledJoints << ")";
1120  *rec = false;
1121  break;
1122  }
1123 
1124  int *joints = new int[n_joint]; // LATER: optimize to avoid allocation.
1125  double *refs = new double[n_joint]; // LATER: optimize to avoid allocation.
1126 
1127  for (i = 0; i < n_joint; i++)
1128  {
1129  joints[i] = jointList.get(i).asInt32();
1130  refs[i] = refList.get(i).asFloat64();
1131  }
1132  *ok = torque->setRefTorques(n_joint, joints, refs);
1133  delete[] joints;
1134  delete[] refs;
1135  }
1136  break;
1137 
1138  case VOCAB_LIM:
1139  {
1140  *ok = ipid->setPidErrorLimit (VOCAB_PIDTYPE_POSITION, cmd.get(3).asInt32(), cmd.get(4).asFloat64());
1141  }
1142  break;
1143 
1144  case VOCAB_LIMS:
1145  {
1146  Bottle& b = *(cmd.get(3).asList());
1147  int i;
1148  const int njs = b.size();
1149  if (njs==controlledJoints)
1150  {
1151  double *p = new double[njs]; // LATER: optimize to avoid allocation.
1152  for (i = 0; i < njs; i++)
1153  p[i] = b.get(i).asFloat64();
1154  *ok = ipid->setPidErrorLimits (VOCAB_PIDTYPE_POSITION, p);
1155  delete[] p;
1156  }
1157  }
1158  break;
1159 
1160  case VOCAB_PID:
1161  {
1162  Pid p;
1163  int j = cmd.get(3).asInt32();
1164  Bottle& b = *(cmd.get(4).asList());
1165  p.kp = b.get(0).asFloat64();
1166  p.kd = b.get(1).asFloat64();
1167  p.ki = b.get(2).asFloat64();
1168  p.max_int = b.get(3).asFloat64();
1169  p.max_output = b.get(4).asFloat64();
1170  p.offset = b.get(5).asFloat64();
1171  p.scale = b.get(6).asFloat64();
1172  *ok = ipid->setPid(VOCAB_PIDTYPE_TORQUE,j, p);
1173  }
1174  break;
1175 
1176  case VOCAB_PIDS:
1177  {
1178  Bottle& b = *(cmd.get(3).asList());
1179  int i;
1180  const int njs = b.size();
1181  if (njs==controlledJoints)
1182  {
1183  Pid *p = new Pid[njs];
1184  for (i = 0; i < njs; i++)
1185  {
1186  Bottle& c = *(b.get(i).asList());
1187  p[i].kp = c.get(0).asFloat64();
1188  p[i].kd = c.get(1).asFloat64();
1189  p[i].ki = c.get(2).asFloat64();
1190  p[i].max_int = c.get(3).asFloat64();
1191  p[i].max_output = c.get(4).asFloat64();
1192  p[i].offset = c.get(5).asFloat64();
1193  p[i].scale = c.get(6).asFloat64();
1194  }
1195  *ok = ipid->setPids(VOCAB_PIDTYPE_TORQUE,p);
1196  delete[] p;
1197  }
1198  }
1199  break;
1200 
1201  case VOCAB_RESET:
1202  {
1203  *ok = ipid->resetPid (VOCAB_PIDTYPE_TORQUE,cmd.get(3).asInt32());
1204  }
1205  break;
1206 
1207  case VOCAB_DISABLE:
1208  {
1209  *ok = ipid->disablePid (VOCAB_PIDTYPE_TORQUE,cmd.get(3).asInt32());
1210  }
1211  break;
1212 
1213  case VOCAB_ENABLE:
1214  {
1215  *ok = ipid->enablePid (VOCAB_PIDTYPE_TORQUE,cmd.get(3).asInt32());
1216  }
1217  break;
1218 
1219  }
1220  }
1221  break;
1222 
1223  case VOCAB_GET:
1224  {
1225  *rec = true;
1226 
1227  int tmp = 0;
1228  double dtmp = 0.0;
1229  response.addVocab32(VOCAB_IS);
1230  response.add(cmd.get(1));
1231 
1232  switch(cmd.get(2).asVocab32())
1233  {
1234  case VOCAB_AXES:
1235  {
1236  int tmp;
1237  *ok = torque->getAxes(&tmp);
1238  response.addInt32(tmp);
1239  }
1240  break;
1241 
1242  case VOCAB_TRQ:
1243  {
1244  *ok = torque->getTorque(cmd.get(3).asInt32(), &dtmp);
1245  response.addFloat64(dtmp);
1246  }
1247  break;
1248 
1249  case VOCAB_TRQS:
1250  {
1251  double *p = new double[controlledJoints];
1252  *ok = torque->getTorques(p);
1253  Bottle& b = response.addList();
1254  int i;
1255  for (i = 0; i < controlledJoints; i++)
1256  b.addFloat64(p[i]);
1257  delete[] p;
1258  }
1259  break;
1260 
1261  case VOCAB_ERR:
1262  {
1263  *ok = ipid->getPidError(VOCAB_PIDTYPE_TORQUE,cmd.get(3).asInt32(), &dtmp);
1264  response.addFloat64(dtmp);
1265  }
1266  break;
1267 
1268  case VOCAB_ERRS:
1269  {
1270  double *p = new double[controlledJoints];
1271  *ok = ipid->getPidErrors(VOCAB_PIDTYPE_TORQUE,p);
1272  Bottle& b = response.addList();
1273  int i;
1274  for (i = 0; i < controlledJoints; i++)
1275  b.addFloat64(p[i]);
1276  delete[] p;
1277  }
1278  break;
1279 
1280  case VOCAB_OUTPUT:
1281  {
1282  *ok = ipid->getPidOutput(VOCAB_PIDTYPE_TORQUE,cmd.get(3).asInt32(), &dtmp);
1283  response.addFloat64(dtmp);
1284  }
1285  break;
1286 
1287  case VOCAB_OUTPUTS:
1288  {
1289  double *p = new double[controlledJoints];
1290  *ok = ipid->getPidOutputs(VOCAB_PIDTYPE_TORQUE,p);
1291  Bottle& b = response.addList();
1292  int i;
1293  for (i = 0; i < controlledJoints; i++)
1294  b.addFloat64(p[i]);
1295  delete[] p;
1296  }
1297  break;
1298 
1299  case VOCAB_PID:
1300  {
1301  Pid p;
1302  *ok = ipid->getPid(VOCAB_PIDTYPE_TORQUE,cmd.get(3).asInt32(), &p);
1303  Bottle& b = response.addList();
1304  b.addFloat64(p.kp);
1305  b.addFloat64(p.kd);
1306  b.addFloat64(p.ki);
1307  b.addFloat64(p.max_int);
1308  b.addFloat64(p.max_output);
1309  b.addFloat64(p.offset);
1310  b.addFloat64(p.scale);
1311  }
1312  break;
1313 
1314  case VOCAB_PIDS:
1315  {
1316  Pid *p = new Pid[controlledJoints];
1317  *ok = ipid->getPids(VOCAB_PIDTYPE_TORQUE,p);
1318  Bottle& b = response.addList();
1319  int i;
1320  for (i = 0; i < controlledJoints; i++)
1321  {
1322  Bottle& c = b.addList();
1323  c.addFloat64(p[i].kp);
1324  c.addFloat64(p[i].kd);
1325  c.addFloat64(p[i].ki);
1326  c.addFloat64(p[i].max_int);
1327  c.addFloat64(p[i].max_output);
1328  c.addFloat64(p[i].offset);
1329  c.addFloat64(p[i].scale);
1330  }
1331  delete[] p;
1332  }
1333  break;
1334 
1335  case VOCAB_REFERENCE:
1336  {
1337  *ok = torque->getRefTorque(cmd.get(3).asInt32(), &dtmp);
1338  response.addFloat64(dtmp);
1339  }
1340  break;
1341 
1342  case VOCAB_REFERENCES:
1343  {
1344  double *p = new double[controlledJoints];
1345  *ok = torque->getRefTorques(p);
1346  Bottle& b = response.addList();
1347  int i;
1348  for (i = 0; i < controlledJoints; i++)
1349  b.addFloat64(p[i]);
1350  delete[] p;
1351  }
1352  break;
1353 
1354  case VOCAB_LIM:
1355  {
1356  *ok = ipid->getPidErrorLimit(VOCAB_PIDTYPE_TORQUE,cmd.get(3).asInt32(), &dtmp);
1357  response.addFloat64(dtmp);
1358  }
1359  break;
1360 
1361  case VOCAB_LIMS:
1362  {
1363  double *p = new double[controlledJoints];
1364  *ok = ipid->getPidErrorLimits(VOCAB_PIDTYPE_TORQUE,p);
1365  Bottle& b = response.addList();
1366  int i;
1367  for (i = 0; i < controlledJoints; i++)
1368  b.addFloat64(p[i]);
1369  delete[] p;
1370  }
1371  break;
1372 
1373  }
1374  }
1375  break;
1376  }
1377  //rec --> true se il comando e' riconosciuto
1378  //ok --> contiene il return value della chiamata all'interfaccia
1379  // ...*ok=torque->setPid();
1380  //torque->
1381 }
1382 
1383 void handleImpedanceMsg(IImpedanceControl *iimp, const yarp::os::Bottle& cmd,
1384  yarp::os::Bottle& response, bool *rec, bool *ok)
1385 {
1386  fprintf(stderr, "Handling IImpedance messages\n");
1387 
1388  if (!iimp)
1389  {
1390  fprintf(stderr, "Error, I do not have a valid IImpedance interface\n");
1391  *ok=false;
1392  return;
1393  }
1394 
1395  int controlledJoints;
1396  iimp->getAxes(&controlledJoints);
1397 
1398  int code = cmd.get(1).asVocab32();
1399  switch (code)
1400  {
1401  case VOCAB_SET:
1402  {
1403  *rec = true;
1404 
1405  switch(cmd.get(2).asVocab32())
1406  {
1407  case VOCAB_IMP_PARAM:
1408  {
1409  *ok = iimp->setImpedance(cmd.get(3).asInt32(), cmd.get(4).asFloat64(),cmd.get(5).asFloat64());
1410  }
1411  break;
1412  case VOCAB_IMP_OFFSET:
1413  {
1414  *ok = iimp->setImpedanceOffset (cmd.get(3).asInt32(), cmd.get(4).asFloat64());
1415  }
1416  break;
1417  }
1418  }
1419  break;
1420 
1421  case VOCAB_GET:
1422  {
1423  *rec = true;
1424 
1425  int tmp = 0;
1426  double dtmp0 = 0.0;
1427  double dtmp1 = 0.0;
1428  double dtmp2 = 0.0;
1429  response.addVocab32(VOCAB_IS);
1430  response.add(cmd.get(1));
1431 
1432  switch(cmd.get(2).asVocab32())
1433  {
1434 
1435  case VOCAB_IMP_PARAM:
1436  {
1437  *ok = iimp->getImpedance(cmd.get(3).asInt32(), &dtmp0, &dtmp1);
1438  response.addFloat64(dtmp0);
1439  response.addFloat64(dtmp1);
1440  }
1441  break;
1442 
1443  case VOCAB_IMP_OFFSET:
1444  {
1445  *ok = iimp->getImpedanceOffset(cmd.get(3).asInt32(), &dtmp0);
1446  response.addFloat64(dtmp0);
1447  }
1448  break;
1449  }
1450  }
1451  break;
1452  }
1453  //rec --> true se il comando e' riconosciuto
1454  //ok --> contiene il return value della chiamata all'interfaccia
1455  // ...*ok=torque->setPid();
1456  //torque->
1457 }
1458 
1459 void handleControlModeMsg_DEBUG(IControlMode *iMode, const yarp::os::Bottle& cmd,
1460  yarp::os::Bottle& response, bool *rec, bool *ok)
1461 {
1462  //THE PURPOSE OF THIS FUCTION IS BEING ABLE TO SET ALL POSSIBILE CONTROL MODES, ALSO THE ONES THAT CANNOT BE NORMALLY SET (e.g. HW_FAULT)
1463  //THIS IS USEFUL FOR DEBUG PURPOSES e.g. IN THE SIMULATOR
1464  fprintf(stderr, "Handling IControlMode message %s, DEBUG MODE\n", cmd.toString().c_str());
1465  if (!iMode)
1466  {
1467  fprintf(stderr, "Error I do not have a valid interface\n");
1468  *ok=false;
1469  return;
1470  }
1471 
1472  int code = cmd.get(1).asVocab32();
1473  *ok=true;
1474 
1475  switch(code)
1476  {
1477  case VOCAB_SET:
1478  {
1479  int axis = cmd.get(3).asInt32();
1480  yarp::os::Value mode_vocab=cmd.get(2);
1481  int mode = mode_vocab.asInt32();
1482  printf ("setting mode: %s (%d)",mode_vocab.toString().c_str(), mode);
1483  *ok = iMode->setControlMode(axis, mode);
1484  }
1485  case VOCAB_GET:
1486  {
1487  if (cmd.get(2).asVocab32()==VOCAB_CM_CONTROL_MODE)
1488  {
1489  int p=-1;
1490  int axis = cmd.get(3).asInt32();
1491  fprintf(stderr, "Calling getControlMode\n");
1492  *ok = iMode->getControlMode(axis, &p);
1493 
1494  response.addVocab32(VOCAB_IS);
1495  response.addInt32(axis);
1496  response.addVocab32(VOCAB_CM_CONTROL_MODE);
1497  response.addVocab32(p);
1498 
1499  //fprintf(stderr, "Returning %d\n", p);
1500  *rec=true;
1501  }
1502  }
1503  break;
1504  default:
1505  {
1506  *rec=false;
1507  }
1508  break;
1509  }
1510 }
1511 
1512 void handleControlModeMsg(IControlMode *iMode, const yarp::os::Bottle& cmd,
1513  yarp::os::Bottle& response, bool *rec, bool *ok)
1514 {
1515  fprintf(stderr, "Handling IControlMode message %s\n", cmd.toString().c_str());
1516  if (!iMode)
1517  {
1518  fprintf(stderr, "Error I do not have a valid interface\n");
1519  *ok=false;
1520  return;
1521  }
1522 
1523  int code = cmd.get(1).asVocab32();
1524  *ok=true;
1525 
1526  switch(code)
1527  {
1528  case VOCAB_SET:
1529  {
1530  int axis = cmd.get(3).asInt32();
1531  int mode=cmd.get(2).asVocab32();
1532  switch (mode)
1533  {
1534  case VOCAB_CM_POSITION:
1535  *ok = iMode->setControlMode(axis, VOCAB_CM_POSITION);
1536  break;
1537  case VOCAB_CM_POSITION_DIRECT:
1538  *ok = iMode->setControlMode(axis, VOCAB_CM_POSITION_DIRECT);
1539  break;
1540  case VOCAB_CM_VELOCITY:
1541  *ok = iMode->setControlMode(axis, VOCAB_CM_VELOCITY);
1542  break;
1543  case VOCAB_CM_MIXED:
1544  *ok = iMode->setControlMode(axis, VOCAB_CM_MIXED);
1545  break;
1546  case VOCAB_CM_TORQUE:
1547  *ok = iMode->setControlMode(axis, VOCAB_CM_TORQUE);
1548  break;
1549  case VOCAB_CM_PWM:
1550  *ok = iMode->setControlMode(axis, VOCAB_CM_PWM);
1551  break;
1552  case VOCAB_CM_CURRENT:
1553  *ok = iMode->setControlMode(axis, VOCAB_CM_CURRENT);
1554  break;
1555  case VOCAB_CM_IDLE:
1556  *ok = iMode->setControlMode(axis, VOCAB_CM_IDLE);
1557  break;
1558  case VOCAB_CM_FORCE_IDLE:
1559  *ok = iMode->setControlMode(axis, VOCAB_CM_FORCE_IDLE);
1560  break;
1561  case VOCAB_CM_IMPEDANCE_POS:
1562  *ok = iMode->setControlMode(axis, VOCAB_CM_IMPEDANCE_POS);
1563  break;
1564  case VOCAB_CM_IMPEDANCE_VEL:
1565  *ok = iMode->setControlMode(axis, VOCAB_CM_IMPEDANCE_VEL);
1566  break;
1567  default:
1568  *ok = false;
1569  break;
1570  }
1571  *rec=true; //or false
1572  }
1573  break;
1574  case VOCAB_GET:
1575  {
1576  if (cmd.get(2).asVocab32()==VOCAB_CM_CONTROL_MODE)
1577  {
1578  int p=-1;
1579  int axis = cmd.get(3).asInt32();
1580  fprintf(stderr, "Calling getControlMode\n");
1581  *ok = iMode->getControlMode(axis, &p);
1582 
1583  response.addVocab32(VOCAB_IS);
1584  response.addInt32(axis);
1585  response.addVocab32(VOCAB_CM_CONTROL_MODE);
1586  response.addVocab32(p);
1587 
1588  //fprintf(stderr, "Returning %d\n", p);
1589  *rec=true;
1590  }
1591  }
1592  break;
1593  default:
1594  {
1595  *rec=false;
1596  }
1597  break;
1598  }
1599 }
1600 
1601 
1602 
1603 void handleInteractionModeMsg(IInteractionMode *iInteract, const yarp::os::Bottle& cmd,
1604  yarp::os::Bottle& response, bool *rec, bool *ok)
1605 {
1606  fprintf(stderr, "Handling IInteractionMode message %s\n", cmd.toString().c_str());
1607  if (!iInteract)
1608  {
1609  fprintf(stderr, "Error I do not have a valid interface\n");
1610  *ok=false;
1611  return;
1612  }
1613 
1614  int code = cmd.get(1).asVocab32();
1615  *ok=true;
1616 
1617  switch(code)
1618  {
1619  case VOCAB_SET:
1620  {
1621  int axis = cmd.get(3).asInt32();
1622  yarp::dev::InteractionModeEnum mode = (yarp::dev::InteractionModeEnum) cmd.get(2).asVocab32();
1623  *ok = iInteract->setInteractionMode(axis, mode);
1624  *rec=true; //or false
1625  }
1626  break;
1627 
1628  case VOCAB_GET:
1629  {
1630  int which = cmd.get(2).asVocab32();
1631  switch(which)
1632  {
1633  case VOCAB_INTERACTION_MODE:
1634  {
1635  int axis = cmd.get(3).asInt32();
1636  yarp::dev::InteractionModeEnum mode;
1637  *ok = iInteract->getInteractionMode(axis, &mode);
1638  // create response
1639  if(*ok)
1640  {
1641  response.addVocab32(VOCAB_IS);
1642  response.addInt32(axis);
1643  response.addVocab32(VOCAB_INTERACTION_MODE);
1644  response.addVocab32(mode);
1645  *rec=true;
1646  }
1647  else
1648  {
1649  response.addVocab32(VOCAB_FAILED);
1650  *rec = false;
1651  }
1652  }
1653  break;
1654 
1655  case VOCAB_INTERACTION_MODES:
1656  {
1657  int axis = cmd.get(3).asInt32();
1658  yarp::dev::InteractionModeEnum *modes;
1659  modes = new yarp::dev::InteractionModeEnum[jnts];
1660 
1661  *ok = iInteract->getInteractionMode(axis, modes);
1662  // create response
1663  if(*ok)
1664  {
1665  response.addVocab32(VOCAB_IS);
1666  response.addVocab32(VOCAB_INTERACTION_MODES);
1667  for(int i=0; i<jnts; i++)
1668  response.addVocab32(modes[i]);
1669  *rec=true;
1670  }
1671  else
1672  {
1673  response.addVocab32(VOCAB_FAILED);
1674  *rec = false;
1675  }
1676  }
1677  break;
1678 
1679  default:
1680  {
1681  fprintf(stderr, "get command not understood");
1682  *rec=false;
1683  break;
1684  }
1685  break;
1686  }
1687  }
1688  break;
1689  default:
1690  {
1691  fprintf(stderr, "type of command not understood");
1692  *rec=false;
1693  }
1694  break;
1695  }
1696 }
cmd
cmd
Definition: dataTypes.h:30
main
int main(int argc, char *argv[])
Definition: main.cpp:31
VOCAB_HELP
#define VOCAB_HELP
Definition: main.cpp:49
VOCAB_QUIT
#define VOCAB_QUIT
Definition: main.cpp:50
handleControlModeMsg
void handleControlModeMsg(IControlMode *icm, const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
Definition: main.cpp:1512
VOCAB_ICONTROLMODE_DEBUG
#define VOCAB_ICONTROLMODE_DEBUG
Definition: main.cpp:51
handleControlModeMsg_DEBUG
void handleControlModeMsg_DEBUG(IControlMode *icm, const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
Definition: main.cpp:1459
jnts
int jnts
Definition: main.cpp:60
handleImpedanceMsg
void handleImpedanceMsg(IImpedanceControl *iimp, const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
Definition: main.cpp:1383
handleTorqueMsg
void handleTorqueMsg(ITorqueControl *itq, IPidControl *ipid, const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
Definition: main.cpp:1053
handleInteractionModeMsg
void handleInteractionModeMsg(IInteractionMode *_iInteract, const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
Definition: main.cpp:1603
iCub::skinManager::toString
std::string toString(const T &t)
Definition: compensator.h:200
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
fprintf
fprintf(fid,'\n')
p
p
Definition: show_eyes_axes.m:23
offset
degrees offset
Definition: sine.m:4
Generated on Wed Nov 13 2024 21:11:40 for iCub-main by doxygen 1.9.1