iCub-main
Loading...
Searching...
No Matches
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
46using namespace yarp::dev;
47using 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
53void handleTorqueMsg(ITorqueControl *itq, IPidControl *ipid, const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool *rec, bool *ok);
54void handleImpedanceMsg(IImpedanceControl *iimp, const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool *rec, bool *ok);
55void handleControlModeMsg(IControlMode *icm, const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool *rec, bool *ok);
56void handleControlModeMsg_DEBUG(IControlMode *icm, const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool *rec, bool *ok);
57void handleInteractionModeMsg(IInteractionMode *_iInteract, const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool *rec, bool *ok);
58
59//
60int jnts = 0; // joint number
61
62int 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
1043ApplicationCleanQuit:
1044 dd.close();
1045 delete[] tmp;
1046 delete[] btmp;
1047 delete[] jtmp;
1048
1049 Network::fini();
1050 return 0;
1051}
1052
1053void 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
1383void 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
1459void 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
1512void 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
1603void 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
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
main
int main()
Definition main.cpp:67
fprintf
fprintf(fid,'\n')
p
p
Definition show_eyes_axes.m:23
offset
degrees offset
Definition sine.m:4
Generated on Wed Dec 18 2024 16:34:46 for iCub-main by doxygen 1.9.8