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>
38 #include <yarp/dev/ControlBoardInterfaces.h>
39 #include <yarp/dev/PolyDriver.h>
41 #include <yarp/os/LogStream.h>
42 #include <yarp/os/Log.h>
47 using namespace yarp::os;
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')
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);
57 void handleInteractionModeMsg(IInteractionMode *_iInteract,
const yarp::os::Bottle&
cmd, yarp::os::Bottle& response,
bool *rec,
bool *ok);
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());
77 options.fromCommand(
argc, argv);
78 if (!options.check(
"robot") || !options.check(
"part")) {
79 printf(
"Missing either --robot or --part options\n");
86 Value& v = options.find(
"robot");
87 Value& part = options.find(
"part");
90 if (!options.check(
"device", val)) {
91 options.put(
"device",
"remote_controlboard");
93 if (!options.check(
"local", val)) {
94 name=
"/"+std::string(v.asString().c_str())+
"/"+std::string(part.asString().c_str())+
"/simpleclient";
96 options.put(
"local", name.c_str());
98 if (!options.check(
"remote", val)) {
99 name=
"/"+std::string(v.asString().c_str())+
"/"+std::string(part.asString().c_str());
101 options.put(
"remote", name.c_str());
104 fprintf(stderr,
"%s", options.toString().c_str());
108 PolyDriver dd(options);
110 printf(
"Device not available. Here are the known devices:\n");
111 printf(
"%s", Drivers::factory().
toString().c_str());
116 IPositionControl *ipos=0;
117 IPositionDirect *iposDir=0;
118 IVelocityControl *vel=0;
121 IAmplifierControl *amp=0;
122 IControlLimits *lim=0;
123 IControlMode *iMode2=0;
125 ITorqueControl *itorque=0;
127 IImpedanceControl *iimp=0;
128 IInteractionMode *iInteract=0;
129 IMotorEncoders *iMotEnc=0;
130 IAxisInfo *iInfo = 0;
140 ok &= dd.view(itorque);
143 ok &= dd.view(iposDir);
144 ok &= dd.view(iMode2);
145 ok &= dd.view(iInteract);
148 yError(
"Problems acquiring mandatory interfaces, quitting\n");
152 ok &=dd.view(iMotEnc);
154 yWarning(
"Problems acquiring optional interface IMotorEncoders\n");
158 yWarning(
"Problems acquiring optional interface iMotor\n");
160 ok &= dd.view(iInfo);
162 yWarning(
"Problems acquiring optional interface iInfo\n");
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;
172 printf(
"Device active...\n");
173 while (dd.isValid()) {
181 c = (char)fgetc(stdin);
190 p.fromString(s.c_str());
191 printf(
"Bottle: %s\n",
p.toString().c_str());
193 switch(
p.get(0).asVocab32()) {
196 printf(
"Available commands:\n");
197 printf(
"-------------------\n\n");
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());
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());
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());
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());
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());
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());
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());
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());
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());
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");
326 if (options.check(
"debug"))
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");
344 goto ApplicationCleanQuit;
347 case VOCAB_ICONTROLMODE:
350 printf(
"%s\n", response.toString().c_str());
357 printf(
"%s\n", response.toString().c_str());
361 case VOCAB_IMPEDANCE:
364 printf(
"%s\n", response.toString().c_str());
371 printf(
"%s\n", response.toString().c_str());
375 case VOCAB_INTERFACE_INTERACTION_MODE:
378 printf(
"%s\n", response.toString().c_str());
383 switch(
p.get(1).asVocab32())
385 case VOCAB_POSITION_MOVE:
389 printf (
"unavailable interface\n");
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");
399 case VOCAB_POSITION_MOVES:
403 printf (
"unavailable interface\n");
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");
414 case VOCAB_POSITION_DIRECT:
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");
422 case VOCAB_POSITION_DIRECTS:
424 ok = iposDir->getRefPositions(
tmp);
425 Bottle& b = response.addList();
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");
435 case VOCAB_VELOCITY_MOVE:
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");
443 case VOCAB_VELOCITY_MOVES:
445 ok = vel->getRefVelocities(
tmp);
446 Bottle& b = response.addList();
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");
456 case VOCAB_INFO_NAME:
458 int j =
p.get(2).asInt32();
459 if (iInfo == 0) { printf(
"unavailable interface\n");
break; }
461 iInfo->getAxisName(j,tmp_str);
462 printf(
"%s: %d %s\n", Vocab32::decode(VOCAB_INFO_NAME).c_str(), j, tmp_str.c_str());
469 printf (
"%s: %d\n", Vocab32::decode(VOCAB_AXES).c_str(), nj);
473 case VOCAB_MOTION_DONE:
475 if (ipos==0) {yError (
"unavailable interface iPos\n");
break;}
477 int j =
p.get(2).asInt32();
478 ipos->checkMotionDone(j,&b);
479 if (b==
true) printf(
"1");
484 case VOCAB_MOTION_DONES:
486 if (ipos==0) {yError (
"unavailable interface iPos\n");
break;}
488 ipos->checkMotionDone(&b);
489 if (b==
true) printf(
"1");
494 case VOCAB_MOTION_DONE_GROUP:
496 if (ipos==0) {yError (
"unavailable interface iPos\n");
break;}
498 ipos->checkMotionDone(
jnts, jtmp ,&b);
499 if (b==
true) printf(
"1");
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]);
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]);
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]);
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]);
543 case VOCAB_MOTOR_CPR:
545 if (iMotEnc==0) {printf (
"unavailable interface\n");
break;}
546 int j =
p.get(2).asInt32();
548 iMotEnc->getMotorEncoderCountsPerRevolution(j, &v);
549 printf(
"%s: ", Vocab32::decode(VOCAB_MOTOR_CPR).c_str());
555 case VOCAB_AMP_MAXCURRENT:
557 if (amp==0) {printf (
"unavailable interface\n");
break;}
558 int j =
p.get(2).asInt32();
560 amp->getMaxCurrent(j, &v);
561 printf(
"%s: ", Vocab32::decode(VOCAB_AMP_MAXCURRENT).c_str());
567 case VOCAB_MOTORS_NUMBER:
569 if (imot==0) {printf (
"unavailable interface\n");
break;}
571 imot->getNumberOfMotors(&v);
572 printf(
"%s: ", Vocab32::decode(VOCAB_MOTORS_NUMBER).c_str());
578 case VOCAB_MOTOR_ENCODER_NUMBER:
580 if (iMotEnc==0) {printf (
"unavailable interface\n");
break;}
582 iMotEnc->getNumberOfMotorEncoders(&v);
583 printf(
"%s: ", Vocab32::decode(VOCAB_MOTOR_ENCODER_NUMBER).c_str());
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);
606 Pid *
p =
new Pid[
jnts];
607 ok = pid->getPids(VOCAB_PIDTYPE_POSITION,
p);
608 Bottle& b = response.addList();
610 for (i = 0; i <
jnts; i++)
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);
619 c.addFloat64(
p[i].scale);
621 printf(
"%s\n", b.toString().c_str());
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);
635 case VOCAB_VEL_LIMITS: {
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);
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]);
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]);
663 case VOCAB_TEMPERATURE: {
664 if (imot==0) {printf (
"unavailable interface\n");
break;}
665 int j =
p.get(2).asInt32();
667 imot->getTemperature(j, &v);
668 printf(
"%s: ", Vocab32::decode(VOCAB_TEMPERATURE).c_str());
674 case VOCAB_TEMPERATURE_LIMIT: {
675 if (imot==0) {printf (
"unavailable interface\n");
break;}
676 int j =
p.get(2).asInt32();
678 imot->getTemperatureLimit(j, &v);
679 printf(
"%s: ", Vocab32::decode(VOCAB_TEMPERATURE_LIMIT).c_str());
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]);
695 case VOCAB_PWMCONTROL_PWM_OUTPUT:
698 if (ipwm == 0) { printf(
"unavailable interface\n");
break; }
699 int j =
p.get(2).asInt32();
701 ipwm->getDutyCycle(j, &v);
702 printf(
"%s: ", Vocab32::decode(VOCAB_PWMCONTROL_PWM_OUTPUT).c_str());
708 case VOCAB_REFERENCE: {
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);
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]);
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]);
736 case VOCAB_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);
746 case VOCAB_REF_ACCELERATION: {
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);
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]);
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]);
774 case VOCAB_AMP_PWM_LIMIT:
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]);
783 case VOCAB_AMP_PEAK_CURRENT:
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]);
792 case VOCAB_AMP_NOMINAL_CURRENT:
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]);
801 case VOCAB_AMP_VOLTAGE_SUPPLY:
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]);
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);
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);
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);
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);
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);
854 printf(
"%s: moving all joints\n", Vocab32::decode(VOCAB_POSITION_MOVES).c_str());
855 ipos->positionMove(
tmp);
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();
864 printf(
"%s: moving all joints\n", Vocab32::decode(VOCAB_VELOCITY_MOVES).c_str());
865 vel->velocityMove(
tmp);
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();
874 printf(
"%s: setting speed for all joints\n", Vocab32::decode(VOCAB_REF_SPEEDS).c_str());
875 ipos->setRefSpeeds(
tmp);
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();
884 printf(
"%s: setting acceleration for all joints\n", Vocab32::decode(VOCAB_REF_ACCELERATIONS).c_str());
885 ipos->setRefAccelerations(
tmp);
890 int j =
p.get(2).asInt32();
891 printf(
"%s: stopping axis %d\n", Vocab32::decode(VOCAB_STOP).c_str(), j);
897 printf(
"%s: stopping all axes\n", Vocab32::decode(VOCAB_STOPS).c_str());
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());
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();
915 iposDir->setPositions(
tmp);
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);
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();
932 printf(
"%s: setting the encoder value for all joints\n", Vocab32::decode(VOCAB_ENCODERS).c_str());
933 enc->setEncoders(
tmp);
939 int j =
p.get(2).asInt32();
940 Bottle *l =
p.get(3).asList();
943 printf(
"Check you specify a 7 elements list, e.g. set pid 0 (2000 20 1 300 300 0 0)\n");
950 pd.kp = l->get(0).asFloat64();
951 pd.kd = l->get(1).asFloat64();
952 pd.ki = l->get(2).asFloat64();
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();
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);
965 printf(
"Error, check you specify at least 7 elements, e.g. set pid 0 (2000 20 1 300 300 0 0)\n");
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);
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);
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);
997 int j =
p.get(2).asInt32();
998 printf(
"%s: enabling control for axis %d\n", Vocab32::decode(VOCAB_ENABLE).c_str(), j);
1000 pid->enablePid(VOCAB_PIDTYPE_POSITION,j);
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());
1012 case VOCAB_AMP_PWM_LIMIT:
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);
1021 case VOCAB_AMP_PEAK_CURRENT:
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);
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);
1043 ApplicationCleanQuit:
1054 yarp::os::Bottle& response,
bool *rec,
bool *ok)
1056 fprintf(stderr,
"Handling ITorque messages\n");
1060 fprintf(stderr,
"Error, I do not have a valid ITorque interface\n");
1065 int controlledJoints;
1066 torque->getAxes(&controlledJoints);
1068 int code =
cmd.get(1).asVocab32();
1075 switch(
cmd.get(2).asVocab32())
1079 *
ok = torque->setRefTorque(
cmd.get(3).asInt32(),
cmd.get(4).asFloat64());
1085 Bottle& b = *(
cmd.get(3).asList());
1087 const int njs = b.size();
1088 if (njs==controlledJoints)
1090 double *
p =
new double[njs];
1091 for (i = 0; i < njs; i++)
1092 p[i] = b.get(i).asFloat64();
1093 *
ok = torque->setRefTorques (
p);
1102 int n_joint =
cmd.get(3).asInt32();
1104 Bottle& jointList = *(
cmd.get(4).asList());
1105 Bottle& refList = *(
cmd.get(5).asList());
1107 const int joint_size = jointList.size();
1108 const int refs_size = refList.size();
1110 if( (joint_size != n_joint) || (refs_size != n_joint) )
1112 yError() <<
"command malformed, size of joint list and reference list must match the number of joints (" << n_joint <<
")";
1117 if (n_joint > controlledJoints)
1119 yError() <<
"command malformed, number of joints inserted is bigger than number of available joints (" << controlledJoints <<
")";
1124 int *joints =
new int[n_joint];
1125 double *refs =
new double[n_joint];
1127 for (i = 0; i < n_joint; i++)
1129 joints[i] = jointList.get(i).asInt32();
1130 refs[i] = refList.get(i).asFloat64();
1132 *
ok = torque->setRefTorques(n_joint, joints, refs);
1140 *
ok = ipid->setPidErrorLimit (VOCAB_PIDTYPE_POSITION,
cmd.get(3).asInt32(),
cmd.get(4).asFloat64());
1146 Bottle& b = *(
cmd.get(3).asList());
1148 const int njs = b.size();
1149 if (njs==controlledJoints)
1151 double *
p =
new double[njs];
1152 for (i = 0; i < njs; i++)
1153 p[i] = b.get(i).asFloat64();
1154 *
ok = ipid->setPidErrorLimits (VOCAB_PIDTYPE_POSITION,
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);
1178 Bottle& b = *(
cmd.get(3).asList());
1180 const int njs = b.size();
1181 if (njs==controlledJoints)
1183 Pid *
p =
new Pid[njs];
1184 for (i = 0; i < njs; i++)
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();
1195 *
ok = ipid->setPids(VOCAB_PIDTYPE_TORQUE,
p);
1203 *
ok = ipid->resetPid (VOCAB_PIDTYPE_TORQUE,
cmd.get(3).asInt32());
1209 *
ok = ipid->disablePid (VOCAB_PIDTYPE_TORQUE,
cmd.get(3).asInt32());
1215 *
ok = ipid->enablePid (VOCAB_PIDTYPE_TORQUE,
cmd.get(3).asInt32());
1229 response.addVocab32(VOCAB_IS);
1230 response.add(
cmd.get(1));
1232 switch(
cmd.get(2).asVocab32())
1237 *
ok = torque->getAxes(&
tmp);
1238 response.addInt32(
tmp);
1244 *
ok = torque->getTorque(
cmd.get(3).asInt32(), &dtmp);
1245 response.addFloat64(dtmp);
1251 double *
p =
new double[controlledJoints];
1252 *
ok = torque->getTorques(
p);
1253 Bottle& b = response.addList();
1255 for (i = 0; i < controlledJoints; i++)
1263 *
ok = ipid->getPidError(VOCAB_PIDTYPE_TORQUE,
cmd.get(3).asInt32(), &dtmp);
1264 response.addFloat64(dtmp);
1270 double *
p =
new double[controlledJoints];
1271 *
ok = ipid->getPidErrors(VOCAB_PIDTYPE_TORQUE,
p);
1272 Bottle& b = response.addList();
1274 for (i = 0; i < controlledJoints; i++)
1282 *
ok = ipid->getPidOutput(VOCAB_PIDTYPE_TORQUE,
cmd.get(3).asInt32(), &dtmp);
1283 response.addFloat64(dtmp);
1289 double *
p =
new double[controlledJoints];
1290 *
ok = ipid->getPidOutputs(VOCAB_PIDTYPE_TORQUE,
p);
1291 Bottle& b = response.addList();
1293 for (i = 0; i < controlledJoints; i++)
1302 *
ok = ipid->getPid(VOCAB_PIDTYPE_TORQUE,
cmd.get(3).asInt32(), &
p);
1303 Bottle& b = response.addList();
1307 b.addFloat64(
p.max_int);
1308 b.addFloat64(
p.max_output);
1309 b.addFloat64(
p.offset);
1310 b.addFloat64(
p.scale);
1316 Pid *
p =
new Pid[controlledJoints];
1317 *
ok = ipid->getPids(VOCAB_PIDTYPE_TORQUE,
p);
1318 Bottle& b = response.addList();
1320 for (i = 0; i < controlledJoints; i++)
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);
1329 c.addFloat64(
p[i].scale);
1335 case VOCAB_REFERENCE:
1337 *
ok = torque->getRefTorque(
cmd.get(3).asInt32(), &dtmp);
1338 response.addFloat64(dtmp);
1342 case VOCAB_REFERENCES:
1344 double *
p =
new double[controlledJoints];
1345 *
ok = torque->getRefTorques(
p);
1346 Bottle& b = response.addList();
1348 for (i = 0; i < controlledJoints; i++)
1356 *
ok = ipid->getPidErrorLimit(VOCAB_PIDTYPE_TORQUE,
cmd.get(3).asInt32(), &dtmp);
1357 response.addFloat64(dtmp);
1363 double *
p =
new double[controlledJoints];
1364 *
ok = ipid->getPidErrorLimits(VOCAB_PIDTYPE_TORQUE,
p);
1365 Bottle& b = response.addList();
1367 for (i = 0; i < controlledJoints; i++)
1384 yarp::os::Bottle& response,
bool *rec,
bool *ok)
1386 fprintf(stderr,
"Handling IImpedance messages\n");
1390 fprintf(stderr,
"Error, I do not have a valid IImpedance interface\n");
1395 int controlledJoints;
1396 iimp->getAxes(&controlledJoints);
1398 int code =
cmd.get(1).asVocab32();
1405 switch(
cmd.get(2).asVocab32())
1407 case VOCAB_IMP_PARAM:
1409 *
ok = iimp->setImpedance(
cmd.get(3).asInt32(),
cmd.get(4).asFloat64(),
cmd.get(5).asFloat64());
1412 case VOCAB_IMP_OFFSET:
1414 *
ok = iimp->setImpedanceOffset (
cmd.get(3).asInt32(),
cmd.get(4).asFloat64());
1429 response.addVocab32(VOCAB_IS);
1430 response.add(
cmd.get(1));
1432 switch(
cmd.get(2).asVocab32())
1435 case VOCAB_IMP_PARAM:
1437 *
ok = iimp->getImpedance(
cmd.get(3).asInt32(), &dtmp0, &dtmp1);
1438 response.addFloat64(dtmp0);
1439 response.addFloat64(dtmp1);
1443 case VOCAB_IMP_OFFSET:
1445 *
ok = iimp->getImpedanceOffset(
cmd.get(3).asInt32(), &dtmp0);
1446 response.addFloat64(dtmp0);
1460 yarp::os::Bottle& response,
bool *rec,
bool *ok)
1464 fprintf(stderr,
"Handling IControlMode message %s, DEBUG MODE\n",
cmd.toString().c_str());
1467 fprintf(stderr,
"Error I do not have a valid interface\n");
1472 int code =
cmd.get(1).asVocab32();
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);
1487 if (
cmd.get(2).asVocab32()==VOCAB_CM_CONTROL_MODE)
1490 int axis =
cmd.get(3).asInt32();
1491 fprintf(stderr,
"Calling getControlMode\n");
1492 *
ok = iMode->getControlMode(axis, &
p);
1494 response.addVocab32(VOCAB_IS);
1495 response.addInt32(axis);
1496 response.addVocab32(VOCAB_CM_CONTROL_MODE);
1497 response.addVocab32(
p);
1513 yarp::os::Bottle& response,
bool *rec,
bool *ok)
1515 fprintf(stderr,
"Handling IControlMode message %s\n",
cmd.toString().c_str());
1518 fprintf(stderr,
"Error I do not have a valid interface\n");
1523 int code =
cmd.get(1).asVocab32();
1530 int axis =
cmd.get(3).asInt32();
1531 int mode=
cmd.get(2).asVocab32();
1534 case VOCAB_CM_POSITION:
1535 *
ok = iMode->setControlMode(axis, VOCAB_CM_POSITION);
1537 case VOCAB_CM_POSITION_DIRECT:
1538 *
ok = iMode->setControlMode(axis, VOCAB_CM_POSITION_DIRECT);
1540 case VOCAB_CM_VELOCITY:
1541 *
ok = iMode->setControlMode(axis, VOCAB_CM_VELOCITY);
1543 case VOCAB_CM_MIXED:
1544 *
ok = iMode->setControlMode(axis, VOCAB_CM_MIXED);
1546 case VOCAB_CM_TORQUE:
1547 *
ok = iMode->setControlMode(axis, VOCAB_CM_TORQUE);
1550 *
ok = iMode->setControlMode(axis, VOCAB_CM_PWM);
1552 case VOCAB_CM_CURRENT:
1553 *
ok = iMode->setControlMode(axis, VOCAB_CM_CURRENT);
1556 *
ok = iMode->setControlMode(axis, VOCAB_CM_IDLE);
1558 case VOCAB_CM_FORCE_IDLE:
1559 *
ok = iMode->setControlMode(axis, VOCAB_CM_FORCE_IDLE);
1561 case VOCAB_CM_IMPEDANCE_POS:
1562 *
ok = iMode->setControlMode(axis, VOCAB_CM_IMPEDANCE_POS);
1564 case VOCAB_CM_IMPEDANCE_VEL:
1565 *
ok = iMode->setControlMode(axis, VOCAB_CM_IMPEDANCE_VEL);
1576 if (
cmd.get(2).asVocab32()==VOCAB_CM_CONTROL_MODE)
1579 int axis =
cmd.get(3).asInt32();
1580 fprintf(stderr,
"Calling getControlMode\n");
1581 *
ok = iMode->getControlMode(axis, &
p);
1583 response.addVocab32(VOCAB_IS);
1584 response.addInt32(axis);
1585 response.addVocab32(VOCAB_CM_CONTROL_MODE);
1586 response.addVocab32(
p);
1604 yarp::os::Bottle& response,
bool *rec,
bool *ok)
1606 fprintf(stderr,
"Handling IInteractionMode message %s\n",
cmd.toString().c_str());
1609 fprintf(stderr,
"Error I do not have a valid interface\n");
1614 int code =
cmd.get(1).asVocab32();
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);
1630 int which =
cmd.get(2).asVocab32();
1633 case VOCAB_INTERACTION_MODE:
1635 int axis =
cmd.get(3).asInt32();
1636 yarp::dev::InteractionModeEnum mode;
1637 *
ok = iInteract->getInteractionMode(axis, &mode);
1641 response.addVocab32(VOCAB_IS);
1642 response.addInt32(axis);
1643 response.addVocab32(VOCAB_INTERACTION_MODE);
1644 response.addVocab32(mode);
1649 response.addVocab32(VOCAB_FAILED);
1655 case VOCAB_INTERACTION_MODES:
1657 int axis =
cmd.get(3).asInt32();
1658 yarp::dev::InteractionModeEnum *modes;
1659 modes =
new yarp::dev::InteractionModeEnum[
jnts];
1661 *
ok = iInteract->getInteractionMode(axis, modes);
1665 response.addVocab32(VOCAB_IS);
1666 response.addVocab32(VOCAB_INTERACTION_MODES);
1667 for(
int i=0; i<
jnts; i++)
1668 response.addVocab32(modes[i]);
1673 response.addVocab32(VOCAB_FAILED);
1681 fprintf(stderr,
"get command not understood");
1691 fprintf(stderr,
"type of command not understood");
int main(int argc, char *argv[])
void handleControlModeMsg(IControlMode *icm, const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
#define VOCAB_ICONTROLMODE_DEBUG
void handleControlModeMsg_DEBUG(IControlMode *icm, const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
void handleImpedanceMsg(IImpedanceControl *iimp, const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
void handleTorqueMsg(ITorqueControl *itq, IPidControl *ipid, const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
void handleInteractionModeMsg(IInteractionMode *_iInteract, const yarp::os::Bottle &cmd, yarp::os::Bottle &response, bool *rec, bool *ok)
std::string toString(const T &t)