iCub-main
iCubSimulationControl.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) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
5 * Author: Vadim Tikhanoff, Paul Fitzpatrick, Giorgio Metta
6 * email: vadim.tikhanoff@iit.it, paulfitz@alum.mit.edu, giorgio.metta@iit.it
7 * website: www.robotcub.org
8 * Permission is granted to copy, distribute, and/or modify this program
9 * under the terms of the GNU General Public License, version 2 or any
10 * later version published by the Free Software Foundation.
11 *
12 * A copy of the license can be found at
13 * http://www.robotcub.org/icub/license/gpl.txt
14 *
15 * This program is distributed in the hope that it will be useful, but
16 * WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
18 * Public License for more details
19 */
20 
21 #ifdef _MSC_VER
22 #pragma warning(disable:4355) //for VC++, no precision loss complaints
23 #endif
24 #include <yarp/os/Time.h>
26 
27 #include <cmath>
28 #include <string>
29 
31 #include "iCubSimulationControl.h"
32 #include "OdeInit.h"
33 #include <yarp/dev/ControlBoardHelper.h>
34 #include <yarp/dev/ControlBoardInterfacesImpl.h>
35 #include <yarp/os/LogStream.h>
36 
37 using namespace yarp::os;
38 using namespace yarp::dev;
39 
40 static bool NOT_YET_IMPLEMENTED(const char *txt)
41 {
42  yError("%s not yet implemented for iCubSimulationControl\n", txt);
43 
44  return false;
45 }
46 
47 static inline bool DEPRECATED(const char *txt)
48 {
49  yError() << txt << " has been deprecated for embObjMotionControl";
50  return true;
51 }
52 
54 
55 iCubSimulationControl::iCubSimulationControl() :
56  //PeriodicThread(0.01),
57  ImplementPositionControl(this),
58  ImplementVelocityControl(this),
59  ImplementPidControl(this),
60  ImplementEncodersTimed(this),
61  ImplementTorqueControl(this),
62  ImplementControlMode(this),
63  ImplementControlCalibration(this),
64  ImplementAmplifierControl(this),
65  ImplementControlLimits(this),
66  ImplementInteractionMode(this),
67  ImplementPositionDirect(this),
68  ImplementMotorEncoders(this),
69  ImplementRemoteVariables(this),
70  ImplementAxisInfo(this),
71  ImplementMotor(this),
72  ImplementPWMControl(this),
73  ImplementCurrentControl(this)
74 {
75  _opened = false;
76  manager = NULL;
77 }
78 
79 
81 {
82  OdeInit& odeinit = OdeInit::get();
83  lock_guard<mutex> lck(odeinit.mtx);
85 }
86 
87 bool iCubSimulationControl::open(yarp::os::Searchable& config) {
88 
89  Searchable& p = config;
90 
91  if (!p.check("GENERAL","section for general motor control parameters")) {
92  yError("Cannot understand configuration parameters");
93  return false;
94  }
95 
96  int TypeArm = p.findGroup("GENERAL").check("Type",Value(1),
97  "what did the user select?").asInt();
98 
99  int numTOTjoints = p.findGroup("GENERAL").check("TotalJoints",Value(1),
100  "Number of total joints").asInt();
101 
102  double velocity = p.findGroup("GENERAL").check("Vel",Value(1),
103  "Default velocity").asDouble();
104  _mutex.lock();
105  partSelec = TypeArm;
106 
107  njoints = numTOTjoints;
108 
109  vel = velocity;
110 
111  angleToEncoder = allocAndCheck<double>(njoints);
112  zeros = allocAndCheck<double>(njoints);
113  newtonsToSensor = allocAndCheck<double>(njoints);
114  ampsToSensor = allocAndCheck<double>(njoints);
115  dutycycleToPwm = allocAndCheck<double>(njoints);
116 
117  controlMode = allocAndCheck<int>(njoints);
118  interactionMode = allocAndCheck<int>(njoints);
119  maxCurrent = allocAndCheck<double>(njoints);
120 
121  limitsMin = allocAndCheck<double>(njoints);
122  limitsMax = allocAndCheck<double>(njoints);
123  velLimitsMin = allocAndCheck<double>(njoints);
124  velLimitsMax = allocAndCheck<double>(njoints);
125  torqueLimits = allocAndCheck<double>(njoints);
126 
127  refSpeed = allocAndCheck<double>(njoints);
128  refAccel = allocAndCheck<double>(njoints);
129  controlP = allocAndCheck<double>(njoints);
130 
131  axisMap = allocAndCheck<int>(njoints);
132 // jointNames = new string[njoints];
133 
134  current_jnt_pos = allocAndCheck<double>(njoints);
135  current_jnt_vel = allocAndCheck<double>(njoints);
136  current_jnt_acc = allocAndCheck<double>(njoints);
137  estimated_jnt_vel = allocAndCheck<double>(njoints);
138  estimated_jnt_acc = allocAndCheck<double>(njoints);
139  current_jnt_torques = allocAndCheck<double>(njoints);
140  current_mot_pos = allocAndCheck<double>(njoints);
141  current_mot_vel = allocAndCheck<double>(njoints);
142  current_mot_acc = allocAndCheck<double>(njoints);
143  estimated_mot_vel = allocAndCheck<double>(njoints);
144  estimated_mot_acc = allocAndCheck<double>(njoints);
145  current_mot_torques = allocAndCheck<double>(njoints);
146  pwm = allocAndCheck<double>(njoints);
147  pwm_ref = allocAndCheck<double>(njoints);
148  current_ampere = allocAndCheck<double>(njoints);
149  current_ampere_ref = allocAndCheck<double>(njoints);
150  next_pos = allocAndCheck<double>(njoints);
151  next_vel = allocAndCheck<double>(njoints);
152  next_torques = allocAndCheck<double>(njoints);
153  inputs = allocAndCheck<int>(njoints);
154  vels = allocAndCheck<double>(njoints);
155  ref_command_positions = allocAndCheck<double>(njoints);
156  ref_positions = allocAndCheck<double>(njoints);
157  ref_command_speeds = allocAndCheck<double>(njoints);
158  ref_speeds = allocAndCheck<double>(njoints);
159  ref_torques = allocAndCheck<double>(njoints);
160 
161  error_tol = allocAndCheck<double>(njoints);
162  position_pid = allocAndCheck <Pid>(njoints);
163  torque_pid = allocAndCheck <Pid>(njoints);
164  current_pid = allocAndCheck <Pid>(njoints);
165  velocity_pid = allocAndCheck <Pid>(njoints);
166  motor_torque_params = allocAndCheck <MotorTorqueParameters>(njoints);
167  rotToEncoder = allocAndCheck<double>(njoints);
168  gearbox = allocAndCheck<double>(njoints);
169  hasHallSensor = allocAndCheck<bool>(njoints);
170  hasTempSensor = allocAndCheck<bool>(njoints);
171  hasRotorEncoder = allocAndCheck<bool>(njoints);
172  rotorIndexOffset = allocAndCheck<int>(njoints);
173  motorPoles = allocAndCheck<int>(njoints);
174  linEstJnt = new iCub::ctrl::AWLinEstimator(25, 2.0);
176  linEstMot = new iCub::ctrl::AWLinEstimator(25, 2.0);
178 
179 // joint_dev = new DeviceTag[njoints];
180 
181  motor_on = allocAndCheck<bool>(njoints);
182  for(int axis = 0;axis<njoints;axis++)
183  motor_on[axis] = false;
184 
186  /* GENERAL */
188 
189  Bottle& xtmp = p.findGroup("GENERAL").findGroup("AxisMap","a list of reordered indices for the axes");
190 
191  if (xtmp.size() != njoints+1) {
192  yError("AxisMap does not have the right number of entries\n");
193  return false;
194  }
195  for (int i = 1; i < xtmp.size(); i++) axisMap[i-1] = xtmp.get(i).asInt();
196 
197  xtmp = p.findGroup("GENERAL").findGroup("Encoder","a list of scales for the encoders");
198  if (xtmp.size() != njoints+1) {
199  yError("Encoder does not have the right number of entries\n");
200  return false;
201  }
202  for (int i = 1; i < xtmp.size(); i++) angleToEncoder[i-1] = xtmp.get(i).asDouble();
203 
204  xtmp = p.findGroup("GENERAL").findGroup("fullscalePWM", "a list of scales for the fullscalePWM param");
205  if (xtmp.size() != njoints + 1) {
206  yError("fullscalePWM does not have the right number of entries\n");
207  return false;
208  }
209  for (int i = 1; i < xtmp.size(); i++) dutycycleToPwm[i - 1] = xtmp.get(i).asDouble()/100.0;
210 
211  xtmp = p.findGroup("GENERAL").findGroup("ampsToSensor", "a list of scales for the ampsToSensor param");
212  if (xtmp.size() != njoints + 1) {
213  yError("ampsToSensor does not have the right number of entries\n");
214  return false;
215  }
216  for (int i = 1; i < xtmp.size(); i++) ampsToSensor[i - 1] = xtmp.get(i).asDouble();
217 
218  xtmp = p.findGroup("GENERAL").findGroup("Zeros","a list of offsets for the zero point");
219  if (xtmp.size() != njoints+1) {
220  yError("Zeros does not have the right number of entries\n");
221  return false;
222  }
223  for (int i = 1; i < xtmp.size(); i++) zeros[i-1] = xtmp.get(i).asDouble();
224 
225  int mj_size = p.findGroup("GENERAL").check("Kinematic_mj_size",Value(0),"Default velocity").asInt();
226  if (mj_size>0)
227  {
228  xtmp = p.findGroup("GENERAL").findGroup("Kinematic_mj");
229  if (xtmp.size() != mj_size*mj_size) {
230  yError("Invalid Kinematic_mj size\n");
231  }
232  kinematic_mj.resize(mj_size,mj_size);
233  for (int c = 0; c < mj_size; c++)
234  for (int r = 0; r < mj_size; r++)
235  {
236  int e=r+c*mj_size+1;
237  kinematic_mj[r][c] = xtmp.get(e).asDouble();
238  }
239  }
240 
241  //fake position pid
242  for (int i = 1; i < njoints + 1; i++) position_pid[i - 1].max_output = 100;
243 
244  //torque sensor
245  for (int i = 1; i < njoints+1; i++) newtonsToSensor[i-1] = 1.0;
246 
248  /* LIMITS */
250  xtmp = p.findGroup("LIMITS").findGroup("jntPosMax","access the joint limits max");
251  if(xtmp.size() != njoints+1)
252  {
253  yError("Not enough max joint limits\n");
254  return false;
255  }
256  for( int i =1;i<xtmp.size();i++ )
257  limitsMax[i-1] = xtmp.get(i).asDouble()*angleToEncoder[i-1];
258 
259  //max velocity
260  for (int i = 1; i < xtmp.size(); i++)
261  {
262  velLimitsMin[i - 1] = 0;
263  velLimitsMax[i - 1] = 100 * angleToEncoder[i - 1];
264  }
265 
266  xtmp = p.findGroup("LIMITS").findGroup("jntPosMin","access the joint limits min");
267  if(xtmp.size() != njoints+1)
268  {
269  yError("Not enough min joint limits\n");
270  return false;
271  }
272  for(int i =1;i<xtmp.size();i++)
273  limitsMin[i-1] = xtmp.get(i).asDouble()*angleToEncoder[i-1];
274 
275  xtmp = p.findGroup("LIMITS").findGroup("error_tol","error tolerance during tracking");
276  if(xtmp.size() != njoints+1)
277  {
278  yError("Not enough error_tol\n");
279  return false;
280  }
281  for(int i=1;i<xtmp.size();i++)
282  error_tol[i-1] = xtmp.get(i).asDouble()*angleToEncoder[i-1];
283 
284  for(int axis =0;axis<njoints;axis++)
285  {
286  current_jnt_pos[axis] = 0.0;
287  current_mot_pos[axis] = 0.0;
288  ErrorPos[axis] = 0.0;
289  double v = 0.0;
290  if (v<limitsMin[axis]) v = limitsMin[axis];
291  if (v>limitsMax[axis]) v = limitsMax[axis];
292  //else v = next_pos[i];
293  next_pos[axis] = M_PI*(v/angleToEncoder[axis])/180;//removed (v/angleToEncoder[i]-1)/180
294  next_vel[axis] = 10.0*angleToEncoder[axis];
295  next_torques[axis] = 0.0;
296  ref_speeds[axis] = 10.0*angleToEncoder[axis];
297  ref_command_speeds[axis] = 0.0;
298  input = 0;
299  inputs[axis] = 0;
300  vels[axis] = 1;
301  maxCurrent[axis] = 1000;
302  controlMode[axis] = MODE_POSITION;
303  interactionMode[axis] = VOCAB_IM_STIFF;
304  }
305 
306  ImplementPositionControl2::initialize(njoints, axisMap, angleToEncoder, zeros);
307  ImplementVelocityControl2::initialize(njoints, axisMap, angleToEncoder, zeros);
308  ImplementPidControl::initialize(njoints, axisMap, angleToEncoder, zeros,newtonsToSensor,ampsToSensor, dutycycleToPwm);
309  ImplementEncodersTimed::initialize(njoints, axisMap, angleToEncoder, zeros);
310  ImplementMotorEncoders::initialize(njoints, axisMap, angleToEncoder, zeros);
311  ImplementControlCalibration::initialize(njoints, axisMap, angleToEncoder, zeros);
312  ImplementAmplifierControl::initialize(njoints, axisMap, angleToEncoder, zeros);
313  ImplementControlLimits::initialize(njoints, axisMap, angleToEncoder, zeros);
314  ImplementTorqueControl::initialize(njoints, axisMap, angleToEncoder, zeros, newtonsToSensor, ampsToSensor, dutycycleToPwm,nullptr,nullptr);
315  ImplementControlMode2::initialize(njoints, axisMap);
316  ImplementInteractionMode::initialize(njoints, axisMap);
317  ImplementPositionDirect::initialize(njoints, axisMap, angleToEncoder, zeros);
318  ImplementRemoteVariables::initialize(njoints, axisMap);
319  ImplementAxisInfo::initialize(njoints, axisMap);
320  ImplementMotor::initialize(njoints, axisMap);
321  ImplementCurrentControl::initialize(njoints, axisMap, ampsToSensor);
322  ImplementPWMControl::initialize(njoints, axisMap, dutycycleToPwm);
323 
324  if (!p.check("joint_device")) {
325  yError("Need a device to access the joints\n");
326  return false;
327  }
328  if (!joints.open(p.find("joint_device").asString().c_str())) {
329  yError("Failed to create a device to access the joints\n");
330  return false;
331  }
332  manager = NULL;
333  joints.view(manager);
334  if (manager==NULL) {
335  yError("Wrong type for device to access the joints\n");
336  return false;
337  }
338 
339  OdeInit& odeinit = OdeInit::get();
340  odeinit.mtx.lock();
341  odeinit.setSimulationControl(this, partSelec);
342  odeinit.mtx.unlock();
343  //PeriodicThread::start();
344  _mutex.unlock();
345  _opened = true;
346 
347  verbosity = odeinit.verbosity;
348  return true;
349 }
350 
352 {
353  if (_opened) {
354 
355  //if (PeriodicThread::isRunning())
356  // {
357  // }
358 
359  //PeriodicThread::stop();/// stops the thread first (joins too).
360 
361  ImplementPositionControl::uninitialize();
362  ImplementVelocityControl::uninitialize();
363  ImplementTorqueControl::uninitialize();
364  ImplementPidControl::uninitialize();
365  ImplementEncodersTimed::uninitialize();
366  ImplementMotorEncoders::uninitialize();
367  ImplementControlCalibration::uninitialize();
368  ImplementAmplifierControl::uninitialize();
369  ImplementControlLimits::uninitialize();
370  ImplementControlMode::uninitialize();
371  ImplementInteractionMode::uninitialize();
372  ImplementPositionDirect::uninitialize();
373  ImplementRemoteVariables::uninitialize();
374  ImplementAxisInfo::uninitialize();
375  ImplementMotor::uninitialize();
376  ImplementCurrentControl::uninitialize();
377  ImplementPWMControl::uninitialize();
378  }
379 
380  checkAndDestroy<double>(current_jnt_pos);
381  checkAndDestroy<double>(current_jnt_torques);
382  checkAndDestroy<double>(current_mot_pos);
383  checkAndDestroy<double>(current_mot_torques);
384  checkAndDestroy<double>(pwm);
385  checkAndDestroy<double>(pwm_ref);
386  checkAndDestroy<double>(current_ampere);
387  checkAndDestroy<double>(current_ampere_ref);
388  checkAndDestroy<double>(current_jnt_vel);
389  checkAndDestroy<double>(current_mot_vel);
390  checkAndDestroy<double>(current_jnt_acc);
391  checkAndDestroy<double>(current_mot_acc);
392  checkAndDestroy<double>(estimated_jnt_vel);
393  checkAndDestroy<double>(estimated_mot_vel);
394  checkAndDestroy<double>(estimated_jnt_acc);
395  checkAndDestroy<double>(estimated_mot_acc);
396  checkAndDestroy<double>(next_pos);
397  checkAndDestroy<double>(next_vel);
398  checkAndDestroy<double>(next_torques);
399  // delete[] joint_dev;
400  checkAndDestroy<double>(ref_command_positions);
401  checkAndDestroy<double>(ref_positions);
402  checkAndDestroy<double>(ref_command_speeds);
403  checkAndDestroy<double>(ref_speeds);
404  checkAndDestroy<double>(ref_torques);
405  checkAndDestroy<double>(angleToEncoder);
406  checkAndDestroy<double>(zeros);
407  checkAndDestroy<double>(newtonsToSensor);
408  checkAndDestroy<int>(controlMode);
409  checkAndDestroy<int>(interactionMode);
410  checkAndDestroy<double>(limitsMin);
411  checkAndDestroy<double>(limitsMax);
412  checkAndDestroy<double>(velLimitsMin);
413  checkAndDestroy<double>(velLimitsMax);
414  checkAndDestroy<int>(axisMap);
415  checkAndDestroy<int>(inputs);
416  checkAndDestroy<double>(vels);
417  checkAndDestroy<double>(torqueLimits);
418  checkAndDestroy<double>(maxCurrent);
419 
420  checkAndDestroy<double>(refSpeed);
421  checkAndDestroy<double>(refAccel);
422  checkAndDestroy<double>(controlP);
423 
424  checkAndDestroy<double>(error_tol);
425  checkAndDestroy<bool>(motor_on);
426  checkAndDestroy<Pid>(position_pid);
427  checkAndDestroy<Pid>(torque_pid);
428  checkAndDestroy<Pid>(velocity_pid);
429  checkAndDestroy<MotorTorqueParameters>(motor_torque_params);
430  checkAndDestroy<double>(rotToEncoder);
431  checkAndDestroy<double>(gearbox);
432  checkAndDestroy<bool>(hasHallSensor);
433  checkAndDestroy<bool>(hasTempSensor);
434  checkAndDestroy<bool>(hasRotorEncoder);
435  checkAndDestroy<int>(rotorIndexOffset);
436  checkAndDestroy<int>(motorPoles);
437  checkAndDestroy<double>(ampsToSensor);
438  checkAndDestroy<double>(dutycycleToPwm);
439  // delete[] jointNames;
440 
441  delete linEstJnt;
442  delete quadEstJnt;
443  delete linEstMot;
444  delete quadEstMot;
445 
446  _opened = false;
447  return true;
448 }
449 
450 void iCubSimulationControl::compute_mot_pos_from_jnt_pos(double *mot_pos, const double *jnt_pos, int size_joints)
451 {
452  for (int i = 0; i < size_joints; i++)
453  {
454  mot_pos[i] = jnt_pos[i]; //use coupling matrix here
455  }
456 }
457 
458 void iCubSimulationControl::compute_mot_vel_and_acc(double *mot_vel, double *mot_acc, const double *mot_pos, int size_joints)
459 {
460  iCub::ctrl::AWPolyElement el(yarp::sig::Vector(size_joints, mot_pos), Time::now());
461  yarp::sig::Vector speeds = linEstMot->estimate(el);
462  yarp::sig::Vector accs = quadEstMot->estimate(el);
463  for (int i = 0; i < size_joints; i++) mot_vel[i] = speeds[i];
464  for (int i = 0; i < size_joints; i++) mot_acc[i] = accs[i];
465 }
466 
467 void iCubSimulationControl::compute_jnt_vel_and_acc(double *jnt_vel, double *jnt_acc, const double *jnt_pos, int size_joints)
468 {
469  iCub::ctrl::AWPolyElement el(yarp::sig::Vector(size_joints, jnt_pos), Time::now());
470  yarp::sig::Vector speeds = linEstJnt->estimate(el);
471  yarp::sig::Vector accs = quadEstJnt->estimate(el);
472  for (int i = 0; i < size_joints; i++) jnt_vel[i] = speeds[i];
473  for (int i = 0; i < size_joints; i++) jnt_acc[i] = accs[i];
474 }
475 
477  lock_guard<mutex> lck(_mutex);
478  if (manager==NULL) {
479  return;
480  }
481  if (partSelec<=6)
482  {
483  for (int axis=0; axis<njoints; axis++)
484  {
485  LogicalJoint& ctrl = manager->control(partSelec,axis);
486  if (!ctrl.isValid()) continue;
487  current_jnt_pos[axis] = ctrl.getAngle();
488  current_jnt_vel[axis] = ctrl.getVelocity();
489  current_jnt_torques[axis] = (controlMode[axis]==MODE_TORQUE) ? ctrl.getTorque() : 0.0; // if not torque ctrl, set torque feedback to 0
490  current_mot_torques[axis]=0;
491 
492  if (maxCurrent[axis]<=0)
493  {
494  controlMode[axis]= VOCAB_CM_HW_FAULT;
495  motor_on[axis] = false;
496  }
497 
498  //motor_on[axis] = true; // no reason to turn motors off, for now
499 
500  if (controlMode[axis]==MODE_VELOCITY || controlMode[axis]==VOCAB_CM_MIXED || controlMode[axis]==MODE_IMPEDANCE_VEL)
501  {
502  if(((current_jnt_pos[axis]<limitsMin[axis])&&(next_vel[axis]<0)) || ((current_jnt_pos[axis]>limitsMax[axis])&&(next_vel[axis]>0)))
503  {
504  ctrl.setVelocity(0.0);
505  }
506  else
507  {
508  ctrl.setVelocity(next_vel[axis]);
509  }
510  }
511  else if (controlMode[axis]==MODE_POSITION || controlMode[axis]==MODE_IMPEDANCE_POS)
512  {
513  ctrl.setControlParameters(vels[axis],1);
514  ctrl.setPosition(next_pos[axis]);
515  }
516  else if (controlMode[axis]==VOCAB_CM_POSITION_DIRECT)
517  {
518  ctrl.setControlParameters(5,1);
519  ctrl.setPosition(next_pos[axis]);
520  }
521  else if (controlMode[axis]==MODE_TORQUE)
522  {
523  ctrl.setTorque(next_torques[axis]);
524  }
525  else if (controlMode[axis]==MODE_PWM)
526  {
527  pwm[axis] = pwm_ref[axis];
528  //currently identical to velocity control, with fixed velocity
529  if (((current_jnt_pos[axis]<limitsMin[axis]) && (pwm_ref[axis]<0)) || ((current_jnt_pos[axis]>limitsMax[axis]) && (pwm_ref[axis]>0)))
530  {
531  ctrl.setVelocity(0.0);
532  }
533  else
534  {
535  if (pwm_ref[axis]>0.001)
536  {
537  ctrl.setVelocity(3);
538  }
539  else if (pwm_ref[axis]<-0.001)
540  {
541  ctrl.setVelocity(-3);
542  }
543  else
544  {
545  ctrl.setVelocity(0.0);
546  }
547  }
548  }
549  else if (controlMode[axis] == MODE_CURRENT)
550  {
551  current_ampere[axis] = current_ampere_ref[axis];
552  //currently identical to velocity control, with fixed velocity
553  if (((current_jnt_pos[axis]<limitsMin[axis]) && (current_ampere_ref[axis]<0)) || ((current_jnt_pos[axis]>limitsMax[axis]) && (current_ampere_ref[axis]>0)))
554  {
555  ctrl.setVelocity(0.0);
556  }
557  else
558  {
559  if (current_ampere_ref[axis]>0.001)
560  {
561  ctrl.setVelocity(3);
562  }
563  else if (current_ampere_ref[axis]<-0.001)
564  {
565  ctrl.setVelocity(-3);
566  }
567  else
568  {
569  ctrl.setVelocity(0.0);
570  }
571  }
572  }
573  }
574  compute_mot_pos_from_jnt_pos(current_mot_pos, current_jnt_pos, njoints);
575  compute_mot_vel_and_acc(estimated_mot_vel, estimated_mot_acc, current_mot_pos, njoints);
576  compute_jnt_vel_and_acc(estimated_jnt_vel, estimated_jnt_acc, current_jnt_pos, njoints);
577  for (int axis = 0; axis < njoints; axis++)
578  {
579  current_mot_acc[axis] = estimated_mot_acc[axis];
580  current_jnt_acc[axis] = estimated_jnt_acc[axis];
581  }
582  }
583 }
584 
586 {
587  *ax = njoints;
588  return true;
589 }
590 
591 bool iCubSimulationControl::setPidRaw (const PidControlTypeEnum& pidtype, int j, const Pid &pid)
592 {
593  if( (j>=0) && (j<njoints) )
594  {
595  switch (pidtype)
596  {
597  case VOCAB_PIDTYPE_POSITION:
598  position_pid[j]=pid;
599  break;
600  case VOCAB_PIDTYPE_VELOCITY:
601  velocity_pid[j]=pid;
602  break;
603  case VOCAB_PIDTYPE_CURRENT:
604  current_pid[j]=pid;
605  break;
606  case VOCAB_PIDTYPE_TORQUE:
607  torque_pid[j]=pid;
608  break;
609  default:
610  break;
611  }
612  }
613  return true;
614 }
615 
616 bool iCubSimulationControl::getPidRaw (const PidControlTypeEnum& pidtype, int j, Pid *out)
617 {
618  if( (j>=0) && (j<njoints) )
619  {
620  switch (pidtype)
621  {
622  case VOCAB_PIDTYPE_POSITION:
623  *out=position_pid[j];
624  break;
625  case VOCAB_PIDTYPE_VELOCITY:
626  *out=velocity_pid[j];
627  break;
628  case VOCAB_PIDTYPE_CURRENT:
629  *out=current_pid[j];
630  break;
631  case VOCAB_PIDTYPE_TORQUE:
632  *out=torque_pid[j];
633  break;
634  default:
635  break;
636  }
637  }
638  return true;
639 }
640 
641 bool iCubSimulationControl::getPidsRaw (const PidControlTypeEnum& pidtype, Pid *out)
642 {
643  for (int i=0; i<njoints; i++)
644  {
645  getPidRaw(pidtype, i, &out[i]);
646  }
647  return true;
648 }
649 
650 bool iCubSimulationControl::setPidsRaw(const PidControlTypeEnum& pidtype, const Pid *pids)
651 {
652  for (int i=0; i<njoints; i++)
653  {
654  setPidRaw(pidtype,i,pids[i]);
655  }
656  return true;
657 }
658 
660 bool iCubSimulationControl::setPidReferenceRaw (const PidControlTypeEnum& pidtype, int axis, double ref)
661 {
662  if( (axis>=0) && (axis<njoints) )
663  {
664  int mode = 0;
665  getControlModeRaw(axis, &mode);
666  switch (pidtype)
667  {
668  case VOCAB_PIDTYPE_POSITION:
669  NOT_YET_IMPLEMENTED("setPidReferenceRaw");
670  break;
671  case VOCAB_PIDTYPE_VELOCITY:
672  NOT_YET_IMPLEMENTED("setPidReferenceRaw");
673  break;
674  case VOCAB_PIDTYPE_CURRENT:
675  NOT_YET_IMPLEMENTED("setPidReferenceRaw");
676  break;
677  case VOCAB_PIDTYPE_TORQUE:
678  NOT_YET_IMPLEMENTED("setPidReferenceRaw");
679  break;
680  default:
681  break;
682  }
683  return true;
684  }
685  if (verbosity)
686  {
687  yError("setReferenceRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
688  }
689  return false;
690 }
691 
692 bool iCubSimulationControl::setPidReferencesRaw (const PidControlTypeEnum& pidtype, const double *refs)
693 {
694  bool ret = true;
695  for(int axis = 0; axis<njoints; axis++)
696  {
697  ret &= setPidReferenceRaw(pidtype, axis, refs[axis]);
698  }
699  return ret;
700 }
701 
702 bool iCubSimulationControl::setPidErrorLimitRaw(const PidControlTypeEnum& pidtype, int axis, double limit)
703 {
704  return NOT_YET_IMPLEMENTED("setErrorLimitRaw");
705 }
706 
707 bool iCubSimulationControl::setPidErrorLimitsRaw(const PidControlTypeEnum& pidtype, const double *limit)
708 {
709  return NOT_YET_IMPLEMENTED("setErrorLimitsRaw");
710 }
711 
712 bool iCubSimulationControl::getPidErrorRaw(const PidControlTypeEnum& pidtype, int axis, double *err)
713 {
714  if ((axis >= 0) && (axis<njoints))
715  {
716  lock_guard<mutex> lck(_mutex);
717  switch (pidtype)
718  {
719  case VOCAB_PIDTYPE_POSITION:
720  *err = current_jnt_pos[axis] - next_pos[axis];
721  break;
722  case VOCAB_PIDTYPE_TORQUE:
723  *err = current_jnt_torques[axis] - next_torques[axis];
724  break;
725  case VOCAB_PIDTYPE_VELOCITY:
726  *err = current_jnt_vel[axis] - next_vel[axis];
727  break;
728  case VOCAB_PIDTYPE_CURRENT:
729  *err = current_ampere[axis] - current_ampere_ref[axis];
730  break;
731  }
732  return true;
733  }
734  if (verbosity)
735  yError("getErrorRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
736  return false;
737 }
738 
739 bool iCubSimulationControl::getPidErrorsRaw(const PidControlTypeEnum& pidtype, double *errs)
740 {
741  bool ret = true;
742  for (int axis = 0; axis<njoints; axis++)
743  {
744  ret &= getPidErrorRaw(pidtype, axis, &errs[axis]);
745  }
746  return ret;
747 }
748 
749 bool iCubSimulationControl::getPidOutputRaw(const PidControlTypeEnum& pidtype, int axis, double *out)
750 {
751  if( (axis>=0) && (axis<njoints) )
752  {
753  int mode = 0;
754  getControlModeRaw(axis, &mode);
755  switch (pidtype)
756  {
757  case VOCAB_PIDTYPE_POSITION:
758  if (mode == VOCAB_CM_POSITION_DIRECT ||
759  mode == VOCAB_CM_POSITION ||
760  mode == VOCAB_CM_MIXED)
761  {
762  lock_guard<mutex> lck(_mutex);
763  *out = pwm[axis];
764  }
765  else
766  {
767  lock_guard<mutex> lck(_mutex);
768  *out = 0;
769  }
770  break;
771  case VOCAB_PIDTYPE_VELOCITY:
772  if (mode == VOCAB_CM_VELOCITY)
773  {
774  lock_guard<mutex> lck(_mutex);
775  *out = pwm[axis];
776  }
777  else
778  {
779  lock_guard<mutex> lck(_mutex);
780  *out = 0;
781  }
782  break;
783  case VOCAB_PIDTYPE_CURRENT:
784  if (mode == VOCAB_CM_CURRENT)
785  {
786  lock_guard<mutex> lck(_mutex);
787  *out = pwm[axis];
788  }
789  else
790  {
791  lock_guard<mutex> lck(_mutex);
792  *out = 0;
793  }
794  break;
795  case VOCAB_PIDTYPE_TORQUE:
796  if (mode == VOCAB_CM_TORQUE)
797  {
798  lock_guard<mutex> lck(_mutex);
799  *out = pwm[axis];
800  }
801  else
802  {
803  lock_guard<mutex> lck(_mutex);
804  *out = 0;
805  }
806  break;
807  default:
808  {
809  lock_guard<mutex> lck(_mutex);
810  *out = 0;
811  }
812  break;
813  }
814  }
815  if (verbosity)
816  {
817  yError("getOutputRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
818  }
819  return false;
820 }
821 
822 bool iCubSimulationControl::getPidOutputsRaw(const PidControlTypeEnum& pidtype, double *outs)
823 {
824  lock_guard<mutex> lck(_mutex);
825  for(int axis = 0; axis<njoints; axis++)
826  outs[axis] = pwm[axis];
827  return true;
828 }
829 
830 bool iCubSimulationControl::isPidEnabledRaw(const PidControlTypeEnum& pidtype, int j, bool* enabled)
831 {
832  return NOT_YET_IMPLEMENTED("isPidEnabled");
833 }
834 
836 {
837  *num = njoints;
838  return true;
839 }
840 
842 {
843  return NOT_YET_IMPLEMENTED("getTemperatureRaw");
844 }
845 
847 {
848  return NOT_YET_IMPLEMENTED("getTemperaturesRaw");
849 }
850 
852 {
853  return NOT_YET_IMPLEMENTED("getTemperatureLimitRaw");
854 }
855 
856 bool iCubSimulationControl::setTemperatureLimitRaw(int m, const double temp)
857 {
858  return NOT_YET_IMPLEMENTED("setTemperatureLimitRaw");
859 }
860 
862 {
863  return NOT_YET_IMPLEMENTED("getPeakCurrentRaw");
864 }
865 
866 bool iCubSimulationControl::setPeakCurrentRaw(int m, const double val)
867 {
868  return NOT_YET_IMPLEMENTED("setPeakCurrentRaw");
869 }
870 
872 {
873  return NOT_YET_IMPLEMENTED("getNominalCurrentRaw");
874 }
875 
876 bool iCubSimulationControl::setNominalCurrentRaw(int m, const double val)
877 {
878  return NOT_YET_IMPLEMENTED("setNominalCurrentRaw");
879 }
880 
881 bool iCubSimulationControl::getPWMRaw(int j, double* val)
882 {
883  return NOT_YET_IMPLEMENTED("getPWMRaw");
884 }
885 
887 {
888  return NOT_YET_IMPLEMENTED("getPWMLimitRaw");
889 }
890 
891 bool iCubSimulationControl::setPWMLimitRaw(int j, const double val)
892 {
893  return NOT_YET_IMPLEMENTED("setPWMLimitRaw");
894 }
895 
897 {
898  return NOT_YET_IMPLEMENTED("getPowerSupplyVoltageRaw");
899 }
900 
901 bool iCubSimulationControl::getPidReferenceRaw(const PidControlTypeEnum& pidtype, int axis, double *ref)
902 {
903  if( (axis>=0) && (axis<njoints) )
904  {
905  int mode = 0;
906  getControlModeRaw(axis, &mode);
907  lock_guard<mutex> lck(_mutex);
908  switch (pidtype)
909  {
910  case VOCAB_PIDTYPE_POSITION:
911  *ref = next_pos[axis];
912  break;
913  case VOCAB_PIDTYPE_VELOCITY:
914  *ref = next_vel[axis];
915  break;
916  case VOCAB_PIDTYPE_CURRENT:
917  *ref = current_ampere_ref[axis];
918  break;
919  case VOCAB_PIDTYPE_TORQUE:
920  *ref = next_torques[axis];
921  break;
922  default:
923  break;
924  }
925  return true;
926  }
927  if (verbosity)
928  {
929  yError("getReferenceRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
930  }
931  return false;
932 }
933 
934 bool iCubSimulationControl::getPidReferencesRaw(const PidControlTypeEnum& pidtype, double *ref)
935 {
936  for(int axis = 0; axis<njoints; axis++)
937  {
938  getPidReferenceRaw(pidtype, axis, &ref[axis]);
939  }
940  return true;
941 }
942 
943 bool iCubSimulationControl::getPidErrorLimitRaw(const PidControlTypeEnum& pidtype, int axis, double *err)
944 {
945  return NOT_YET_IMPLEMENTED("getErrorLimitRaw");
946 }
947 
948 bool iCubSimulationControl::getPidErrorLimitsRaw(const PidControlTypeEnum& pidtype, double *errs)
949 {
950  return NOT_YET_IMPLEMENTED("getErrorLimitsRaw");
951 }
952 
953 bool iCubSimulationControl::resetPidRaw(const PidControlTypeEnum& pidtype, int axis)
954 {
955  return NOT_YET_IMPLEMENTED("resetPidRaw");
956 }
957 
958 bool iCubSimulationControl::enablePidRaw(const PidControlTypeEnum& pidtype, int axis)
959 {
960  return DEPRECATED("enablePidRaw");
961 }
962 
963 bool iCubSimulationControl::setPidOffsetRaw(const PidControlTypeEnum& pidtype, int axis, double v)
964 {
965  return NOT_YET_IMPLEMENTED("setOffsetRaw");
966 }
967 
968 bool iCubSimulationControl::disablePidRaw(const PidControlTypeEnum& pidtype, int axis)
969 {
970  return DEPRECATED("disablePidRaw");
971 }
972 
973 bool iCubSimulationControl::positionMoveRaw(int axis, double ref)
974 {
975  if( (axis >=0) && (axis<njoints) )
976  {
977  int mode = 0;
978  getControlModeRaw(axis, &mode);
979  if (mode != VOCAB_CM_POSITION &&
980  mode != VOCAB_CM_MIXED )
981  {
982  yError() << "positionMoveRaw: skipping command because part " << partSelec << " joint " << axis << "is not in VOCAB_CM_POSITION mode";
983  return false;
984  }
985 
986  lock_guard<mutex> lck(_mutex);
987  if(ref< limitsMin[axis])
988  {
989  if (njoints == 16)
990  {
991  if ( axis == 7 )
992  next_pos[axis] = limitsMax[axis];
993  }
994  else
995  {
996  next_pos[axis] = limitsMin[axis];
997  }
998  ref_command_positions[axis] = next_pos[axis];
999  }
1000  else if(ref > limitsMax[axis])
1001  {
1002  if (njoints == 16)
1003  {
1004  if ( axis == 7 )
1005  next_pos[axis] = fabs(limitsMax[axis] - limitsMax[axis]);
1006  }
1007  else
1008  {
1009  next_pos[axis] = limitsMax[axis];
1010  }
1011  ref_command_positions[axis] = next_pos[axis];
1012  }
1013  else
1014  {
1015  ref_command_positions[axis] = ref;
1016  if (njoints == 16)
1017  {
1018  if ( axis == 10 || axis == 12 || axis == 14 )
1019  next_pos[axis] = ref/2;
1020  else if ( axis == 15 )
1021  next_pos[axis] = ref/3;
1022  else if ( axis == 7 )
1023  next_pos[axis] = fabs(limitsMax[axis] - ref);
1024  else
1025  next_pos[axis] = ref;
1026  }
1027  else
1028  {
1029  next_pos[axis] = ref;
1030  }
1031  }
1032 
1033  motor_on[axis]=true;
1034 
1035  if (verbosity)
1036  yDebug("moving joint %d of part %d to pos %f\n",axis, partSelec, next_pos[axis]);
1037  return true;
1038  }
1039  if (verbosity)
1040  yError("positionMoveRaw joint access too high %d \n",axis);
1041  return false;
1042 }
1043 
1045 {
1046  bool ret = true;
1047  for(int axis = 0; axis<njoints; axis++)
1048  {
1049  ret &= positionMoveRaw(axis, refs[axis]);
1050  }
1051  return ret;
1052 }
1053 
1054 bool iCubSimulationControl::relativeMoveRaw(int axis, double delta)
1055 {
1056  return positionMoveRaw(axis,next_pos[axis]+delta);
1057 }
1058 
1059 bool iCubSimulationControl::relativeMoveRaw(const double *deltas)
1060 {
1061  for(int axis = 0; axis<njoints; axis++) {
1062  relativeMoveRaw(axis,deltas[axis]);
1063  }
1064  return true;
1065 }
1066 
1068 {
1069  lock_guard<mutex> lck(_mutex);
1070  bool fin = true;
1071  for(int axis = 0;axis<njoints;axis++)
1072  {
1073  if(! (fabs( current_jnt_pos[axis]-next_pos[axis])<error_tol[axis]))
1074  {
1075  fin = false;
1076  }
1077  }
1078  if (verbosity)
1079  yDebug("motion finished error tol %f %f %f\n",error_tol[0],current_jnt_pos[0],next_pos[0]);
1080  *ret = fin;
1081  return true;
1082 }
1083 
1085 {
1086  if( (axis >=0) && (axis<njoints) )
1087  {
1088  lock_guard<mutex> lck(_mutex);
1089  if(fabs(current_jnt_pos[axis]-next_pos[axis])<error_tol[axis])
1090  *ret = true;
1091  else
1092  *ret = false;
1093  return true;
1094  }
1095  if (verbosity)
1096  yError("checkMotionDoneRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1097  return false;
1098 }
1099 bool iCubSimulationControl::setRefSpeedRaw(int axis, double sp)
1100 {
1101  if( (axis >=0) && (axis<njoints) )
1102  {
1103  lock_guard<mutex> lck(_mutex);
1104  //vel = sp;// *180/M_PI ;
1105  vels[axis] = sp;//vel/20;
1106  ref_speeds[axis] = sp;
1107  if (verbosity)
1108  yDebug("setting joint %d of part %d to reference velocity %f\n",axis,partSelec,vels[axis]);
1109  return true;
1110  }
1111  return false;
1112 }
1113 
1115 {
1116  bool ret = true;
1117  for (int axis = 0; axis<njoints; axis++)
1118  {
1119  ret &= velocityMoveRaw(axis, spds[axis]);
1120  }
1121  return ret;
1122 }
1124 {
1125  return NOT_YET_IMPLEMENTED("setRefAccelerationRaw");
1126 }
1128 {
1129  return NOT_YET_IMPLEMENTED("setRefAccelerationsRaw");
1130 }
1131 bool iCubSimulationControl::getRefSpeedRaw(int axis, double *ref)
1132 {
1133  if((axis>=0) && (axis<njoints)) {
1134  lock_guard<mutex> lck(_mutex);
1135  *ref = ref_speeds[axis];
1136  return true;
1137  }
1138  if (verbosity)
1139  yError("getRefSpeedRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1140  return false;
1141 }
1143 {
1144  bool ret = true;
1145  for (int i = 0; i<njoints; i++)
1146  {
1147  ret &= getRefSpeedRaw(i, &spds[i]);
1148  }
1149  return ret;
1150 }
1152 {
1153  return NOT_YET_IMPLEMENTED("getRefAccelerationRaw");
1154 }
1156 {
1157  return NOT_YET_IMPLEMENTED("getRefAccelerationsRaw");
1158 }
1160 {
1161  if( (axis>=0) && (axis<njoints) )
1162  {
1163  lock_guard<mutex> lck(_mutex);
1164  next_pos[axis] = current_jnt_pos[axis];
1165  next_vel[axis] = 0.0;
1166  return true;
1167  }
1168  if (verbosity)
1169  yError("stopRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1170  return false;
1171 }
1173 {
1174  lock_guard<mutex> lck(_mutex);
1175  for(int axis=0;axis<njoints;axis++)
1176  {
1177  next_pos[axis] = current_jnt_pos[axis];
1178  next_vel[axis] = 0.0;
1179  }
1180  return true;
1181 }
1182 
1184 {
1185  if ((axis >= 0) && (axis<njoints)) {
1186  lock_guard<mutex> lck(_mutex);
1187  *ref = ref_command_positions[axis];
1188  return true;
1189  }
1190  if (verbosity)
1191  yError("getTargetPositionRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1192  return false;
1193 }
1194 
1196 {
1197  bool ret = true;
1198  for (int i = 0; i<njoints; i++)
1199  {
1200  ret &= getTargetPositionRaw(i, &refs[i]);
1201  }
1202  return ret;
1203 }
1204 
1205 bool iCubSimulationControl::getTargetPositionsRaw(int nj, const int * jnts, double *refs)
1206 {
1207  bool ret = true;
1208  for (int i = 0; i<nj; i++)
1209  {
1210  ret &= getTargetPositionRaw(jnts[i], &refs[i]);
1211  }
1212  return ret;
1213 }
1214 
1215 bool iCubSimulationControl::getRefVelocityRaw(int axis, double *ref)
1216 {
1217  lock_guard<mutex> lck(_mutex);
1218  *ref = ref_command_speeds[axis];
1219  return true;
1220 }
1221 
1223 {
1224  bool ret = true;
1225  for (int i = 0; i<njoints; i++)
1226  {
1227  ret &= getRefVelocityRaw(i, &refs[i]);
1228  }
1229  return ret;
1230 }
1231 
1232 bool iCubSimulationControl::getRefVelocitiesRaw(int nj, const int * jnts, double *refs)
1233 {
1234  bool ret = true;
1235  for (int i = 0; i<nj; i++)
1236  {
1237  ret &= getRefVelocityRaw(jnts[i], &refs[i]);
1238  }
1239  return ret;
1240 }
1241 
1242 bool iCubSimulationControl::getRefPositionRaw(int axis, double *ref)
1243 {
1244  lock_guard<mutex> lck(_mutex);
1245  *ref = ref_positions[axis];
1246  return true;
1247 }
1248 
1250 {
1251  bool ret = true;
1252  for (int i = 0; i<njoints; i++)
1253  {
1254  ret &= getRefPositionRaw(i, &refs[i]);
1255  }
1256  return ret;
1257 }
1258 
1259 
1260 bool iCubSimulationControl::getRefPositionsRaw(int nj, const int * jnts, double *refs)
1261 {
1262  bool ret = true;
1263  for (int i = 0; i<nj; i++)
1264  {
1265  ret &= getRefPositionRaw(jnts[i], &refs[i]);
1266  }
1267  return ret;
1268 }
1269 
1270 bool iCubSimulationControl::positionMoveRaw(int nj, const int * jnts, const double *refs)
1271 {
1272  bool ret = true;
1273  for (int i = 0; i<nj; i++)
1274  {
1275  ret &= positionMoveRaw(jnts[i], refs[i]);
1276  }
1277  return ret;
1278 }
1279 
1280 bool iCubSimulationControl::relativeMoveRaw(int nj, const int * jnts, const double *refs)
1281 {
1282  bool ret = true;
1283  for (int i = 0; i<nj; i++)
1284  {
1285  ret &= relativeMoveRaw(jnts[i], refs[i]);
1286  }
1287  return ret;
1288 }
1289 
1290 bool iCubSimulationControl::checkMotionDoneRaw(int nj, const int * jnts, bool *done)
1291 {
1292  bool ret = true;
1293  bool jDone(true), tmpDone(true);
1294 
1295  for (int i = 0; i<nj; i++)
1296  {
1297  ret &= checkMotionDoneRaw(jnts[i], &jDone);
1298  tmpDone &= jDone;
1299  }
1300  *done = tmpDone;
1301  return ret;
1302 }
1303 
1304 bool iCubSimulationControl::setRefSpeedsRaw(const int nj, const int * jnts, const double *refs)
1305 {
1306  bool ret = true;
1307  for (int i = 0; i<nj; i++)
1308  {
1309  ret &= setRefSpeedRaw(jnts[i], refs[i]);
1310  }
1311  return ret;
1312 }
1313 
1314 bool iCubSimulationControl::getRefSpeedsRaw(const int nj, const int * jnts, double *refs)
1315 {
1316  bool ret = true;
1317  for (int i = 0; i<nj; i++)
1318  {
1319  ret &= getRefSpeedRaw(jnts[i], &refs[i]);
1320  }
1321  return ret;
1322 }
1323 
1324 // cmd is an array of double of length njoints specifying speed
1326 bool iCubSimulationControl::velocityMoveRaw (int axis, double sp)
1327 {
1328  if( (axis >=0) && (axis<njoints) )
1329  {
1330  int mode = 0;
1331  getControlModeRaw(axis, &mode);
1332  if (mode != VOCAB_CM_VELOCITY &&
1333  mode != VOCAB_CM_MIXED)
1334  {
1335  yError() << "velocityMoveRaw: skipping command because part " << partSelec << " joint " << axis << "is not in VOCAB_CM_VELOCITY mode";
1336  return false;
1337  }
1338  lock_guard<mutex> lck(_mutex);
1339  next_vel[axis] = sp;
1340  ref_command_speeds[axis] = sp;
1341  motor_on[axis] = true;
1342  return true;
1343  }
1344  if (verbosity)
1345  yError("velocityMoveRaw: joint with index %d does not exist, valis joints indices are between 0 and %d \n",axis,njoints);
1346  return false;
1347 }
1348 
1352 {
1353  bool ret = true;
1354  for (int axis=0; axis<njoints; axis++)
1355  {
1356  ret &= velocityMoveRaw(axis,sp[axis]);
1357  }
1358  return ret;
1359 }
1360 
1361 bool iCubSimulationControl::setEncoderRaw(int axis, double val)
1362 {
1363  return NOT_YET_IMPLEMENTED("setEncoderRaw");
1364 }
1365 
1367 {
1368  return NOT_YET_IMPLEMENTED("setEncodersRaw");
1369 }
1370 
1372 {
1373  return NOT_YET_IMPLEMENTED("resetEncoderRaw");
1374 }
1375 
1377 {
1378  return NOT_YET_IMPLEMENTED("resetEncodersRaw");
1379 }
1380 
1382 {
1383  lock_guard<mutex> lck(_mutex);
1384  for(int axis = 0;axis<njoints;axis++)
1385  {
1386  if ( axis == 10 || axis == 12 || axis == 14 )
1387  v[axis] = current_jnt_pos[axis]*2;
1388  else if ( axis == 15 )
1389  v[axis] = current_jnt_pos[axis]*3;
1390  else if ( axis == 7 )
1391  v[axis] = limitsMax[axis] - current_jnt_pos[axis];
1392  else
1393  v[axis] = current_jnt_pos[axis];
1394  }
1395  return true;
1396 }
1397 
1399 {
1400  if((axis>=0) && (axis<njoints)) {
1401  lock_guard<mutex> lck(_mutex);
1402 
1403  if ( axis == 10 || axis == 12 || axis == 14 )
1404  *v = current_jnt_pos[axis]*2;
1405  else if ( axis == 15 )
1406  *v = current_jnt_pos[axis]*3;
1407  else if ( axis == 7 )
1408  *v = limitsMax[axis] - current_jnt_pos[axis];
1409  else
1410  *v = current_jnt_pos[axis];
1411 
1412  return true;
1413  }
1414  if (verbosity)
1415  yError("getEncoderRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1416  return false;
1417 }
1418 
1420 {
1421  double timeNow = Time::now();
1422  for(int axis = 0;axis<njoints;axis++)
1423  {
1424  stamps[axis] = timeNow;
1425  }
1426  return getEncodersRaw(encs);
1427 }
1428 
1429 bool iCubSimulationControl::getEncoderTimedRaw(int axis, double *enc, double *stamp)
1430 {
1431  *stamp = Time::now();
1432  return getEncoderRaw(axis, enc);
1433 }
1434 
1435 
1437 {
1438  lock_guard<mutex> lck(_mutex);
1439  for(int axis = 0; axis<njoints; axis++)
1440  v[axis] = current_jnt_vel[axis];//* 10;
1441  return true;
1442 }
1443 
1445 {
1446  if( (axis>=0) && (axis<njoints) ) {
1447  lock_guard<mutex> lck(_mutex);
1448  *v = current_jnt_vel[axis];// * 10;
1449  return true;
1450  }
1451  if (verbosity)
1452  yError("getEncoderSpeedRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1453  return false;
1454 }
1455 
1457 {
1458  lock_guard<mutex> lck(_mutex);
1459  for (int axis = 0; axis<njoints; axis++)
1460  v[axis] = current_jnt_acc[axis];//* 10;
1461  return true;
1462 }
1463 
1465 {
1466  if ((axis >= 0) && (axis<njoints)) {
1467  lock_guard<mutex> lck(_mutex);
1468  *v = current_jnt_acc[axis];// * 10;
1469  return true;
1470  }
1471  if (verbosity)
1472  yError("getEncoderSpeedRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1473  return false;
1474 }
1475 
1476 bool iCubSimulationControl::setMotorEncoderRaw(int axis, const double val)
1477 {
1478  return NOT_YET_IMPLEMENTED("setMotorEncoderRaw");
1479 }
1480 
1482 {
1483  return NOT_YET_IMPLEMENTED("setMotorEncodersRaw");
1484 }
1485 
1487 {
1488  return NOT_YET_IMPLEMENTED("resetMotorEncoderRaw");
1489 }
1490 
1492 {
1493  return NOT_YET_IMPLEMENTED("resetMotorEncodersRaw");
1494 }
1495 
1497 {
1498  for(int axis = 0;axis<njoints;axis++)
1499  {
1500  getMotorEncoderRaw(axis,&v[axis]);
1501  }
1502  return true;
1503 }
1504 
1506 {
1507  if((axis>=0) && (axis<njoints))
1508  {
1509  lock_guard<mutex> lck(_mutex);
1510  *v = current_mot_pos[axis];
1511  return true;
1512  }
1513  if (verbosity)
1514  yError("getMotorEncoderRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1515  return false;
1516 }
1517 
1519 {
1520  *num=njoints;
1521  return true;
1522 }
1523 
1525 {
1526  return NOT_YET_IMPLEMENTED("setMotorEncoderCountsPerRevolutionRaw");
1527 }
1528 
1530 {
1531  return NOT_YET_IMPLEMENTED("getMotorEncoderCountsPerRevolutionRaw");
1532 }
1533 
1534 
1536 {
1537  double timeNow = Time::now();
1538  for(int axis = 0;axis<njoints;axis++)
1539  {
1540  stamps[axis] = timeNow;
1541  }
1542  return getMotorEncodersRaw(encs);
1543 }
1544 
1545 bool iCubSimulationControl::getMotorEncoderTimedRaw(int axis, double *enc, double *stamp)
1546 {
1547  *stamp = Time::now();
1548  return getMotorEncoderRaw(axis, enc);
1549 }
1550 
1551 
1553 {
1554  for(int axis = 0; axis<njoints; axis++)
1555  {
1556  getMotorEncoderSpeedRaw(axis,&v[axis]);
1557  }
1558  return true;
1559 }
1560 
1562 {
1563  if( (axis>=0) && (axis<njoints) ) {
1564  lock_guard<mutex> lck(_mutex);
1565  *v = current_mot_vel[axis];
1566  return true;
1567  }
1568  if (verbosity)
1569  yError("getEncoderSpeedRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1570  return false;
1571 }
1572 
1574 {
1575  for (int axis = 0; axis<njoints; axis++)
1576  {
1577  getMotorEncoderAccelerationRaw(axis, &v[axis]);
1578  }
1579  return true;
1580 }
1581 
1583 {
1584  if ((axis >= 0) && (axis<njoints)) {
1585  lock_guard<mutex> lck(_mutex);
1586  *v = current_mot_acc[axis];
1587  return true;
1588  }
1589  if (verbosity)
1590  yError("getEncoderSpeedRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1591  return false;
1592 }
1593 
1595 {
1596  return DEPRECATED ("disableAmpRaw");
1597 }
1598 
1600 {
1601  return DEPRECATED ("enableAmpRaw");
1602 }
1603 
1604 // bcast
1606 {
1607  lock_guard<mutex> lck(_mutex);
1608  for(int axis = 0; axis<njoints; axis++)
1609  cs[axis] = current_ampere[axis];
1610  return true;
1611 }
1612 
1613 // bcast currents
1614 bool iCubSimulationControl::getCurrentRaw(int axis, double *c)
1615 {
1616  if( (axis>=0) && (axis<njoints) )
1617  {
1618  lock_guard<mutex> lck(_mutex);
1619  *c = current_ampere[axis];
1620  }
1621  else
1622  {
1623  if (verbosity)
1624  yError("getCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1625  }
1626  return true;
1627 }
1628 
1630 {
1631  if( (axis>=0) && (axis<njoints) )
1632  {
1633  lock_guard<mutex> lck(_mutex);
1634  maxCurrent[axis]=v;
1635  }
1636  else
1637  {
1638  if (verbosity)
1639  yError("setMaxCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1640  }
1641  return true;
1642 }
1643 
1645 {
1646  if( (axis>=0) && (axis<njoints) )
1647  {
1648  lock_guard<mutex> lck(_mutex);
1649  *v=maxCurrent[axis];
1650  }
1651  else
1652  {
1653  if (verbosity)
1654  yError("getMaxCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1655  }
1656  return true;
1657 }
1658 
1659 bool iCubSimulationControl::calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3)
1660 {
1661  return NOT_YET_IMPLEMENTED("calibrateRaw");
1662 }
1663 
1665 {
1666  return NOT_YET_IMPLEMENTED("doneRaw");
1667 }
1668 
1669 
1671 {
1672  lock_guard<mutex> lck(_mutex);
1673  for(int axis =0;axis<njoints;axis++)
1674  st[axis] = (int)motor_on[axis];
1675  return true;
1676 }
1677 
1679 {
1680  if( (axis>=0) && (axis<njoints))
1681  {
1682  lock_guard<mutex> lck(_mutex);
1683  st[axis] = (int)motor_on[axis];
1684  return true;
1685  }
1686  if (verbosity)
1687  yError("getAmpStatusRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1688  return false;
1689 }
1690 
1691 bool iCubSimulationControl::setLimitsRaw(int axis, double min, double max)
1692 {
1693  if( (axis >=0) && (axis < njoints) ){
1694  lock_guard<mutex> lck(_mutex);
1695  limitsMax[axis] = max;
1696  limitsMin[axis] = min;
1697  return true;
1698  }
1699  if (verbosity)
1700  yError("setLimitsRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1701  return false;
1702 }
1703 
1704 bool iCubSimulationControl::getLimitsRaw(int axis, double *min, double *max)
1705 {
1706  if( (axis >=0) && (axis < njoints)) {
1707  lock_guard<mutex> lck(_mutex);
1708  *min = limitsMin[axis];
1709  *max = limitsMax[axis];
1710  return true;
1711  }
1712  //else
1713  return false;
1714 }
1715 
1716 // IRemoteVariables
1717 bool iCubSimulationControl::getRemoteVariableRaw(string key, yarp::os::Bottle& val)
1718 {
1719  val.clear();
1720  if (key == "kinematic_mj")
1721  {
1722  val.addString("1 2 3"); return true;
1723  }
1724  else if (key == "rotor")
1725  {
1726  Bottle& r = val.addList(); for (int i=0; i<njoints; i++) r.addDouble(rotToEncoder[i]); return true;
1727  }
1728  else if (key == "gearbox")
1729  {
1730  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addDouble(gearbox[i]); return true;
1731  }
1732  else if (key == "zeros")
1733  {
1734  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addDouble(zeros[i]); return true;
1735  }
1736  else if (key == "hasHallSensor")
1737  {
1738  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addInt(hasHallSensor[i]); return true;
1739  }
1740  else if (key == "hasTempSensor")
1741  {
1742  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addInt(hasTempSensor[i]); return true;
1743  }
1744  else if (key == "hasRotorEncoder")
1745  {
1746  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addInt(hasRotorEncoder[i]); return true;
1747  }
1748  else if (key == "rotorIndexOffset")
1749  {
1750  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addInt(rotorIndexOffset[i]); return true;
1751  }
1752  else if (key == "motorPoles")
1753  {
1754  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addInt(motorPoles[i]); return true;
1755  }
1756  else if (key == "pidCurrentKp")
1757  {
1758  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addDouble(current_pid[i].kp); return true;
1759  }
1760  else if (key == "pidCurrentKi")
1761  {
1762  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addDouble(current_pid[i].ki); return true;
1763  }
1764  else if (key == "pidCurrentShift")
1765  {
1766  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addDouble(current_pid[i].scale); return true;
1767  }
1768  yWarning("getRemoteVariable(): Unknown variable %s", key.c_str());
1769  return false;
1770 }
1771 
1772 bool iCubSimulationControl::setRemoteVariableRaw(string key, const yarp::os::Bottle& val)
1773 {
1774  string s1 = val.toString();
1775  Bottle* bval = val.get(0).asList();
1776  if (bval == 0)
1777  {
1778  yWarning("setRemoteVariable(): Protocol error %s", s1.c_str());
1779  return false;
1780  }
1781 
1782  string s2 = bval->toString();
1783  if (key == "kinematic_mj")
1784  {
1785  return true;
1786  }
1787  else if (key == "rotor")
1788  {
1789  for (int i = 0; i < njoints; i++)
1790  rotToEncoder[i] = bval->get(i).asDouble();
1791  return true;
1792  }
1793  else if (key == "gearbox")
1794  {
1795  for (int i = 0; i < njoints; i++) gearbox[i] = bval->get(i).asDouble(); return true;
1796  }
1797  else if (key == "zeros")
1798  {
1799  for (int i = 0; i < njoints; i++) zeros[i] = bval->get(i).asDouble(); return true;
1800  }
1801  yWarning("setRemoteVariable(): Unknown variable %s", key.c_str());
1802  return false;
1803 }
1804 
1805 bool iCubSimulationControl::getRemoteVariablesListRaw(yarp::os::Bottle* listOfKeys)
1806 {
1807  listOfKeys->clear();
1808  listOfKeys->addString("kinematic_mj");
1809  listOfKeys->addString("rotor");
1810  listOfKeys->addString("gearbox");
1811  listOfKeys->addString("hasHallSensor");
1812  listOfKeys->addString("hasTempSensor");
1813  listOfKeys->addString("hasRotorEncoder");
1814  listOfKeys->addString("rotorIndexOffset");
1815  listOfKeys->addString("motorPoles");
1816  listOfKeys->addString("pidCurrentKp");
1817  listOfKeys->addString("pidCurrentKi");
1818  listOfKeys->addString("pidCurrentShift");
1819  return true;
1820 }
1821 
1822 bool iCubSimulationControl::setVelLimitsRaw(int axis, double min, double max)
1823 {
1824  if ((axis >= 0) && (axis < njoints)){
1825  lock_guard<mutex> lck(_mutex);
1826  velLimitsMax[axis] = max;
1827  velLimitsMin[axis] = min;
1828  return true;
1829  }
1830  if (verbosity)
1831  yError("setVelLimitsRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1832  return false;
1833 }
1834 
1835 bool iCubSimulationControl::getVelLimitsRaw(int axis, double *min, double *max)
1836 {
1837  if ((axis >= 0) && (axis < njoints)) {
1838  lock_guard<mutex> lck(_mutex);
1839  *min = velLimitsMin[axis];
1840  *max = velLimitsMax[axis];
1841  return true;
1842  }
1843  //else
1844  return false;
1845 }
1846 
1847 bool iCubSimulationControl::velocityMoveRaw(const int n_joint, const int *joints, const double *spds)
1848 {
1849  bool ret = true;
1850  for(int j=0; j<n_joint; j++)
1851  {
1852  ret = ret && velocityMoveRaw(joints[j], spds[j]);
1853  }
1854  return ret;
1855 }
1856 
1857 bool iCubSimulationControl::setRefAccelerationsRaw(const int n_joint, const int *joints, const double *accs)
1858 {
1859  bool ret = true;
1860  for(int j=0; j<n_joint; j++)
1861  {
1862  ret = ret && setRefAccelerationRaw(joints[j], accs[j]);
1863  }
1864  return ret;
1865 }
1866 
1867 bool iCubSimulationControl::getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs)
1868 {
1869  bool ret = true;
1870  for(int j=0; j<n_joint; j++)
1871  {
1872  ret = ret && getRefAccelerationRaw(joints[j], &accs[j]);
1873  }
1874  return ret;
1875 }
1876 
1877 bool iCubSimulationControl::stopRaw(const int n_joint, const int *joints)
1878 {
1879  bool ret = true;
1880  for(int j=0; j<n_joint; j++)
1881  {
1882  ret = ret && stopRaw(joints[j]);
1883  }
1884  return ret;
1885 }
1886 
1887 bool iCubSimulationControl::getAxisNameRaw(int axis, string& name)
1888 {
1889  if ((axis >= 0) && (axis < njoints))
1890  {
1891  lock_guard<mutex> lck(_mutex);
1892  char buff[100];
1893  sprintf(buff,"JOINT%d", axis);
1894  name.assign(buff,strlen(buff));
1895  return true;
1896  }
1897 
1898  return false;
1899 }
1900 
1901 bool iCubSimulationControl::getJointTypeRaw(int axis, yarp::dev::JointTypeEnum& type)
1902 {
1903  if ((axis >= 0) && (axis < njoints))
1904  {
1905  lock_guard<mutex> lck(_mutex);
1906  type = yarp::dev::VOCAB_JOINTTYPE_REVOLUTE;
1907  return true;
1908  }
1909 
1910  return false;
1911 }
1912 
1913 bool iCubSimulationControl::getTorqueRaw(int axis, double *sp)
1914 {
1915  if( (axis >=0) && (axis < njoints)) {
1916  lock_guard<mutex> lck(_mutex);
1917  *sp = current_jnt_torques[axis];
1918  return true;
1919  }
1920  return false;
1921 }
1923 {
1924  lock_guard<mutex> lck(_mutex);
1925  for(int axis = 0;axis<njoints;axis++)
1926  sp[axis] = current_jnt_torques[axis];
1927  return true;
1928 }
1929 bool iCubSimulationControl::getTorqueRangeRaw(int axis, double *a,double *b)
1930 {
1931  return NOT_YET_IMPLEMENTED("getTorqueRangeRaw");
1932 }
1934 {
1935  return NOT_YET_IMPLEMENTED("getTorqueRangesRaw");
1936 }
1938 {
1939  bool ret = true;
1940  for (int i = 0; i<njoints; i++)
1941  {
1942  ret &= setRefTorqueRaw(i, sp[i]);
1943  }
1944  return ret;
1945 }
1946 
1948 {
1949  if( (axis >=0) && (axis<njoints) ) {
1950  lock_guard<mutex> lck(_mutex);
1951  next_torques[axis] = ref;
1952  ref_torques[axis] = ref;
1953  motor_on[axis] = true;
1954  return true;
1955  }
1956  if (verbosity)
1957  yError("setRefTorqueRaw: joint with index %d does not exist, valis joints indices are between 0 and %d \n",axis,njoints);
1958  return false;
1959 }
1960 
1961 bool iCubSimulationControl::setRefTorquesRaw(const int n_joint, const int *joints, const double *t)
1962 {
1963  bool ret = true;
1964  for (int j = 0; j<n_joint; j++)
1965  {
1966  ret = ret && setRefTorqueRaw(joints[j], t[j]);
1967  }
1968  return ret;
1969 }
1970 
1972 {
1973  bool ret = true;
1974  for (int i = 0; i<njoints; i++)
1975  {
1976  ret &= getRefTorqueRaw(i, &refs[i]);
1977  }
1978  return ret;
1979 }
1980 bool iCubSimulationControl::getRefTorqueRaw(int axis,double *ref)
1981 {
1982  if( (axis >=0) && (axis<njoints) ) {
1983  lock_guard<mutex> lck(_mutex);
1984  *ref = next_torques[axis];
1985  return true;
1986  }
1987  if (verbosity)
1988  yError("getRefTorqueRaw: joint with index %d does not exist, valis joints indices are between 0 and %d \n",axis,njoints);
1989  return false;
1990 }
1991 
1992 bool iCubSimulationControl::getMotorTorqueParamsRaw(int axis, MotorTorqueParameters *params)
1993 {
1994  if( (axis >=0) && (axis<njoints) )
1995  {
1996  params->bemf=motor_torque_params[axis].bemf;
1997  params->bemf_scale=motor_torque_params[axis].bemf_scale;
1998  params->ktau=motor_torque_params[axis].ktau;
1999  params->ktau_scale=motor_torque_params[axis].ktau_scale;
2000  }
2001  NOT_YET_IMPLEMENTED("getMotorTorqueParamsRaw");
2002  return true; //fake
2003 
2004 }
2005 bool iCubSimulationControl::setMotorTorqueParamsRaw(int axis, const MotorTorqueParameters params)
2006 {
2007  if( (axis >=0) && (axis<njoints) )
2008  {
2009  motor_torque_params[axis].bemf=params.bemf;
2010  motor_torque_params[axis].bemf_scale=params.bemf_scale;
2011  motor_torque_params[axis].ktau=params.ktau;
2012  motor_torque_params[axis].ktau_scale=params.ktau_scale;
2013  }
2014  NOT_YET_IMPLEMENTED("setMotorTorqueParamsRaw");
2015  return true; //fake
2016 }
2017 
2018 int iCubSimulationControl::ControlModes_yarp2iCubSIM(int yarpMode)
2019 {
2020  int ret = VOCAB_CM_UNKNOWN;
2021  switch(yarpMode)
2022  {
2023  case VOCAB_CM_FORCE_IDLE:
2024  case VOCAB_CM_IDLE:
2025  ret = MODE_IDLE;
2026  break;
2027  case VOCAB_CM_POSITION:
2028  ret = MODE_POSITION;
2029  break;
2030  case VOCAB_CM_VELOCITY:
2031  ret = MODE_VELOCITY;
2032  break;
2033  case VOCAB_CM_TORQUE:
2034  ret = MODE_TORQUE;
2035  break;
2036  case VOCAB_CM_IMPEDANCE_POS:
2037  ret = MODE_IMPEDANCE_POS;
2038  break;
2039  case VOCAB_CM_IMPEDANCE_VEL:
2040  ret = MODE_IMPEDANCE_VEL;
2041  break;
2042  case VOCAB_CM_PWM:
2043  ret = MODE_PWM;
2044  break;
2045  case VOCAB_CM_CURRENT:
2046  ret = MODE_CURRENT;
2047  break;
2048 
2049  // for new modes I´ll use directly the vocabs, so they are the same
2050  case VOCAB_CM_MIXED:
2051  case VOCAB_CM_POSITION_DIRECT:
2052  case VOCAB_CM_HW_FAULT:
2053  case VOCAB_CM_CALIBRATING:
2054  case VOCAB_CM_CALIB_DONE:
2055  case VOCAB_CM_NOT_CONFIGURED:
2056  case VOCAB_CM_CONFIGURED:
2057  ret = yarpMode;
2058  break;
2059 
2060  default:
2061  ret = MODE_UNKNOWN;
2062  break;
2063  }
2064  return ret;
2065 }
2066 
2067 int iCubSimulationControl::ControlModes_iCubSIM2yarp(int iCubMode)
2068 {
2069  int ret = VOCAB_CM_UNKNOWN;
2070  switch(iCubMode)
2071  {
2072  case MODE_IDLE:
2073  ret=VOCAB_CM_IDLE;
2074  break;
2075  case MODE_POSITION:
2076  ret=VOCAB_CM_POSITION;
2077  break;
2078  case MODE_VELOCITY:
2079  ret=VOCAB_CM_VELOCITY;
2080  break;
2081  case MODE_TORQUE:
2082  ret=VOCAB_CM_TORQUE;
2083  break;
2084  case MODE_IMPEDANCE_POS:
2085  ret=VOCAB_CM_IMPEDANCE_POS;
2086  break;
2087  case MODE_IMPEDANCE_VEL:
2088  ret=VOCAB_CM_IMPEDANCE_VEL;
2089  break;
2090  case MODE_PWM:
2091  ret=VOCAB_CM_PWM;
2092  break;
2093  case MODE_CURRENT:
2094  ret = VOCAB_CM_CURRENT;
2095  break;
2096 
2097  // for new modes I´ll use directly the vocabs, so they are the same
2098  case MODE_MIXED:
2099  case MODE_HW_FAULT:
2100  case VOCAB_CM_POSITION_DIRECT:
2101  case MODE_CALIBRATING:
2102  case MODE_CALIB_DONE:
2103  case MODE_NOT_CONFIGURED:
2104  case MODE_CONFIGURED:
2105  ret = iCubMode;
2106  break;
2107 
2108  default:
2109  ret=VOCAB_CM_UNKNOWN;
2110  break;
2111  }
2112  return ret;
2113 }
2114 
2116 {
2117  if( (j >=0) && (j < njoints)) {
2118  lock_guard<mutex> lck(_mutex);
2119  *mode = ControlModes_iCubSIM2yarp(controlMode[j]);
2120  return true;
2121  }
2122  if (verbosity)
2123  yError("getControlModeRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j, njoints);
2124  return false;
2125 }
2126 
2128 {
2129  lock_guard<mutex> lck(_mutex);
2130  for(int axis = 0;axis<njoints;axis++)
2131  modes[axis] = ControlModes_iCubSIM2yarp(controlMode[axis]);
2132  return true;
2133 }
2134 
2136 bool iCubSimulationControl::getControlModesRaw(const int n_joint, const int *joints, int *modes)
2137 {
2138  bool ret = true;
2139  for(int idx=0; idx< n_joint; idx++)
2140  ret = ret && getControlModeRaw(joints[idx], &modes[idx]);
2141  return ret;
2142 }
2143 
2145 {
2146  int tmp;
2147  if( (j >=0) && (j < njoints) )
2148  {
2149  tmp = ControlModes_yarp2iCubSIM(mode);
2150  if(tmp == MODE_UNKNOWN)
2151  {
2152  yError() << "setControlModeRaw: unknown control mode " << yarp::os::Vocab::decode(mode);
2153  }
2154  else if (controlMode[j] == VOCAB_CM_HW_FAULT && mode != VOCAB_CM_FORCE_IDLE)
2155  {
2156  yError() << "setControlModeRaw: unable to reset an HW_FAULT without a VOCAB_CM_FORCE_IDLE command";
2157  }
2158  else
2159  {
2160  lock_guard<mutex> lck(_mutex);
2161  controlMode[j] = ControlModes_yarp2iCubSIM(mode);
2162  next_pos[j]=current_jnt_pos[j];
2163  if (controlMode[j] != MODE_PWM) pwm_ref[j] = 0;
2164  if (controlMode[j] != MODE_CURRENT) current_ampere_ref[j] = 0;
2165  }
2166  return true;
2167  }
2168  if (verbosity)
2169  yError("setControlModeRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j, njoints);
2170  return false;
2171 }
2172 
2173 bool iCubSimulationControl::setControlModesRaw(const int n_joint, const int *joints, int *modes)
2174 {
2175  bool ret = true;
2176  for(int idx=0; idx<n_joint; idx++)
2177  {
2178  ret = ret && setControlModeRaw(joints[idx], modes[idx]);
2179  }
2180  return ret;
2181 }
2182 
2184 {
2185  bool ret = true;
2186  for(int j=0; j<njoints; j++)
2187  {
2188  ret = ret && setControlModeRaw(j, modes[j]);
2189  }
2190  return ret;
2191 }
2192 
2194 bool iCubSimulationControl::getInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum* mode)
2195 {
2196  *mode = (yarp::dev::InteractionModeEnum) interactionMode[axis];
2197  return true;
2198 }
2199 
2200 bool iCubSimulationControl::getInteractionModesRaw(int n_joints, int *__joints, yarp::dev::InteractionModeEnum* modes)
2201 {
2202  bool ret = true;
2203  for(int idx=0; idx<n_joints; idx++)
2204  {
2205  ret = ret && getInteractionModeRaw(__joints[idx], &modes[idx]);
2206  }
2207  return ret;
2208 }
2209 
2210 bool iCubSimulationControl::getInteractionModesRaw(yarp::dev::InteractionModeEnum* modes)
2211 {
2212  bool ret = true;
2213  for(int j=0; j<njoints; j++)
2214  {
2215  ret = ret && getInteractionModeRaw(j, &modes[j]);
2216  }
2217  return ret;
2218 }
2219 
2220 bool iCubSimulationControl::setInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum mode)
2221 {
2222  interactionMode[axis] = (int)mode;
2223  next_pos[axis]=current_jnt_pos[axis];
2224  return true;
2225 }
2226 
2227 bool iCubSimulationControl::setInteractionModesRaw(int n_joints, int *__joints, yarp::dev::InteractionModeEnum* modes)
2228 {
2229  bool ret = true;
2230  for(int idx=0; idx<n_joints; idx++)
2231  {
2232  ret = ret && setInteractionModeRaw(__joints[idx], modes[idx]);
2233  }
2234  return ret;
2235 }
2236 
2237 bool iCubSimulationControl::setInteractionModesRaw(yarp::dev::InteractionModeEnum* modes)
2238 {
2239  bool ret = true;
2240  for(int j=0; j<njoints; j++)
2241  {
2242  ret = ret && setInteractionModeRaw(j, modes[j]);
2243  }
2244  return ret;
2245 }
2246 
2247 // Position direct interface
2248 bool iCubSimulationControl::setPositionRaw(int axis, double ref)
2249 {
2250  // This function has been copy pasted from positionMoveRaw
2251  if( (axis >=0) && (axis<njoints) )
2252  {
2253  int mode = 0;
2254  getControlModeRaw(axis, &mode);
2255  if (mode != VOCAB_CM_POSITION_DIRECT)
2256  {
2257  yError() << "setPositionRaw: skipping command because part " << partSelec << " joint" << axis << "is not in VOCAB_CM_POSITION_DIRECT mode";
2258  return false;
2259  }
2260  lock_guard<mutex> lck(_mutex);
2261  if(ref< limitsMin[axis])
2262  {
2263  if (njoints == 16)
2264  {
2265  if ( axis == 7 )
2266  next_pos[axis] = limitsMax[axis];
2267  }
2268  else
2269  {
2270  next_pos[axis] = limitsMin[axis];
2271  }
2272  ref_positions[axis] = next_pos[axis];
2273  }
2274  else if(ref > limitsMax[axis])
2275  {
2276  if (njoints == 16)
2277  {
2278  if ( axis == 7 )
2279  next_pos[axis] = fabs(limitsMax[axis] - limitsMax[axis]);
2280  }
2281  else
2282  {
2283  next_pos[axis] = limitsMax[axis];
2284  }
2285  ref_positions[axis] = next_pos[axis];
2286  }
2287  else
2288  {
2289  ref_positions[axis] = ref;
2290  if (njoints == 16)
2291  {
2292  if ( axis == 10 || axis == 12 || axis == 14 )
2293  next_pos[axis] = ref/2;
2294  else if ( axis == 15 )
2295  next_pos[axis] = ref/3;
2296  else if ( axis == 7 )
2297  next_pos[axis] = fabs(limitsMax[axis] - ref);
2298  else
2299  next_pos[axis] = ref;
2300  }
2301  else
2302  {
2303  next_pos[axis] = ref;
2304  }
2305  }
2306 
2307  motor_on[axis]=true;
2308 
2309  if (verbosity)
2310  yDebug("moving joint %d of part %d to pos %f (pos direct)\n",axis, partSelec, next_pos[axis]);
2311  return true;
2312  }
2313  if (verbosity)
2314  yError("setPositionRaw joint access too high %d \n",axis);
2315  return false;
2316 }
2317 
2318 bool iCubSimulationControl::setPositionsRaw(const int n_joint, const int *joints, const double *refs)
2319 {
2320  bool ret = true;
2321  for(int i=0; i<n_joint; i++)
2322  {
2323  ret = ret && setPositionRaw(joints[i], refs[i]);
2324  }
2325  return ret;
2326 }
2327 
2329 {
2330  bool ret = true;
2331  for(int j=0; j<njoints; j++)
2332  {
2333  ret = ret && setPositionRaw(j, refs[j]);
2334  }
2335  return ret;
2336 }
2337 
2338 //PWM interface
2340 {
2341  if ((j >= 0) && (j<njoints))
2342  {
2343  lock_guard<mutex> lck(_mutex);
2344  pwm_ref[j] = v;
2345  return true;
2346  }
2347  if (verbosity)
2348  yError("setRefDutyCycleRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j, njoints);
2349  return false;
2350 }
2351 
2353 {
2354  lock_guard<mutex> lck(_mutex);
2355  for (int axis = 0; axis<njoints; axis++)
2356  pwm_ref[axis] = v[axis];
2357  return true;
2358 }
2359 
2361 {
2362  if ((j >= 0) && (j<njoints))
2363  {
2364  lock_guard<mutex> lck(_mutex);
2365  *v = pwm_ref[j];
2366  return true;
2367  }
2368  if (verbosity)
2369  yError("getRefDutyCycleRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j, njoints);
2370  return false;
2371 }
2372 
2374 {
2375  lock_guard<mutex> lck(_mutex);
2376  for (int axis = 0; axis<njoints; axis++)
2377  v[axis] = pwm_ref[axis];
2378  return true;
2379 }
2380 
2382 {
2383  if ((j >= 0) && (j<njoints))
2384  {
2385  lock_guard<mutex> lck(_mutex);
2386  *v = pwm[j];
2387  return true;
2388  }
2389  if (verbosity)
2390  yError("getDutyCycleRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j, njoints);
2391  return false;
2392 }
2393 
2395 {
2396  lock_guard<mutex> lck(_mutex);
2397  for (int axis = 0; axis<njoints; axis++)
2398  v[axis] = pwm[axis];
2399  return true;
2400 }
2401 
2402 // Current interface
2403 /*bool iCubSimulationControl::getCurrentRaw(int j, double *t)
2404 {
2405 return NOT_YET_IMPLEMENTED("getCurrentRaw");
2406 }
2407 
2408 bool iCubSimulationControl::getCurrentsRaw(double *t)
2409 {
2410 return NOT_YET_IMPLEMENTED("getCurrentsRaw");
2411 }
2412 */
2413 
2415 {
2416  if ((j >= 0) && (j<njoints))
2417  {
2418  lock_guard<mutex> lck(_mutex);
2419  *min = -3;
2420  *max = 3;
2421  return true;
2422  }
2423  if (verbosity)
2424  yError("setRefCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j, njoints);
2425  return false;
2426 }
2427 
2429 {
2430  lock_guard<mutex> lck(_mutex);
2431  for (int axis = 0; axis < njoints; axis++)
2432  {
2433  min[axis] = -3;
2434  max[axis] = 3;
2435  }
2436  return true;
2437 }
2438 
2440 {
2441  lock_guard<mutex> lck(_mutex);
2442  for (int axis = 0; axis<njoints; axis++)
2443  current_ampere_ref[axis] = t[axis];
2444  return true;
2445 }
2446 
2448 {
2449  if ((j >= 0) && (j<njoints))
2450  {
2451  lock_guard<mutex> lck(_mutex);
2452  current_ampere_ref[j] = t;
2453  return true;
2454  }
2455  if (verbosity)
2456  yError("setRefCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j, njoints);
2457  return false;
2458 }
2459 
2460 bool iCubSimulationControl::setRefCurrentsRaw(const int n_joint, const int *joints, const double *t)
2461 {
2462  bool ret = true;
2463  for (int j = 0; j<n_joint; j++)
2464  {
2465  ret = ret && setRefCurrentRaw(joints[j], t[j]);
2466  }
2467  return ret;
2468 }
2469 
2471 {
2472  lock_guard<mutex> lck(_mutex);
2473  for (int axis = 0; axis<njoints; axis++)
2474  t[axis] = current_ampere_ref[axis];
2475  return true;
2476 }
2477 
2479 {
2480  if ((j >= 0) && (j<njoints))
2481  {
2482  lock_guard<mutex> lck(_mutex);
2483  *t = current_ampere_ref[j];
2484  return true;
2485  }
2486  if (verbosity)
2487  yError("getRefCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j, njoints);
2488  return false;
2489 }
virtual bool getTorqueRangesRaw(double *, double *) override
virtual bool getEncodersRaw(double *encs) override
virtual bool getLimitsRaw(int axis, double *min, double *max) override
virtual bool getPeakCurrentRaw(int m, double *val) override
virtual bool resetMotorEncodersRaw() override
virtual bool getCurrentRangesRaw(double *min, double *max) override
virtual bool setInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
virtual bool setPidReferencesRaw(const PidControlTypeEnum &pidtype, const double *refs) override
#define M_PI
Definition: XSensMTx.cpp:24
virtual bool getMotorEncoderSpeedRaw(int m, double *sp) override
virtual bool velocityMoveRaw(int j, double sp) override
for each axis
#define MODE_TORQUE
virtual bool getTorqueRaw(int, double *) override
virtual bool getMotorTorqueParamsRaw(int j, MotorTorqueParameters *params) override
Basic element for adaptive polynomial fitting.
virtual bool setRefCurrentsRaw(const double *t) override
virtual bool setRefDutyCycleRaw(int j, double v) override
virtual bool getMotorEncoderAccelerationRaw(int m, double *spds) override
int verbosity
Definition: OdeInit.h:72
virtual bool getEncoderAccelerationRaw(int j, double *spds) override
static uint32_t idx[BOARD_NUM]
iCub::ctrl::AWQuadEstimator * quadEstJnt
virtual bool getPidRaw(const PidControlTypeEnum &pidtype, int j, Pid *pid) override
virtual bool getPidReferencesRaw(const PidControlTypeEnum &pidtype, double *refs) override
virtual bool setEncoderRaw(int j, double val) override
virtual void setTorque(double target)=0
Set the reference torque.
virtual bool relativeMoveRaw(int j, double delta) override
#define MODE_IDLE
#define MODE_CALIBRATING
virtual bool getPidsRaw(const PidControlTypeEnum &pidtype, Pid *pids) override
virtual bool getPidOutputRaw(const PidControlTypeEnum &pidtype, int j, double *out) override
virtual bool getPWMLimitRaw(int j, double *val) override
virtual bool getRefPositionsRaw(double *refs) override
out
Definition: sine.m:8
virtual bool setMotorTorqueParamsRaw(int j, const MotorTorqueParameters params) override
#define MODE_IMPEDANCE_VEL
virtual bool getPidErrorRaw(const PidControlTypeEnum &pidtype, int j, double *err) override
virtual bool setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override
std::mutex mtx
Definition: OdeInit.h:65
virtual bool getDutyCycleRaw(int j, double *v) override
virtual bool setMotorEncoderRaw(int m, const double val) override
virtual bool setRefAccelerationsRaw(const double *accs) override
virtual bool getRefTorquesRaw(double *) override
int jnts
Definition: main.cpp:60
MotorTorqueParameters * motor_torque_params
virtual double getTorque()=0
this is just a fake torque interface for now
virtual bool setPidRaw(const PidControlTypeEnum &pidtype, int j, const Pid &pid) override
#define MODE_POSITION
virtual double getVelocity()=0
Get the angular velocity of an associated joint, in ICUB units and sign.
virtual bool setControlModesRaw(const int n_joint, const int *joints, int *modes) override
virtual bool getCurrentsRaw(double *vals) override
virtual bool getCurrentRangeRaw(int j, double *min, double *max) override
virtual bool setInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum mode) override
virtual bool getRefTorqueRaw(int, double *) override
virtual bool getAxes(int *ax) override
POSITION CONTROL INTERFACE RAW.
virtual bool getMotorEncoderCountsPerRevolutionRaw(int m, double *cpr) override
virtual bool isPidEnabledRaw(const PidControlTypeEnum &pidtype, int j, bool *enabled) override
virtual bool getPWMRaw(int j, double *val) override
virtual bool setPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, const double *limits) override
virtual bool getRefSpeedsRaw(double *spds) override
virtual bool checkMotionDoneRaw(bool *flag) override
virtual bool resetMotorEncoderRaw(int m) override
virtual bool getRefDutyCyclesRaw(double *v) override
virtual bool getTemperatureLimitRaw(int m, double *temp) override
virtual bool getControlModeRaw(int j, int *mode) override
virtual bool getRefVelocitiesRaw(double *refs) override
virtual bool setPidsRaw(const PidControlTypeEnum &pidtype, const Pid *pids) override
virtual bool getMotorEncodersRaw(double *encs) override
virtual bool getRefSpeedRaw(int j, double *ref) override
virtual bool getTorqueRangeRaw(int, double *, double *) override
virtual bool getEncodersTimedRaw(double *encs, double *stamps)
virtual bool getVelLimitsRaw(int axis, double *min, double *max) override
#define MODE_HW_FAULT
virtual bool setRefTorquesRaw(const double *) override
virtual bool getMotorEncoderTimedRaw(int m, double *encs, double *stamp) override
#define MODE_CURRENT
virtual bool setEncodersRaw(const double *vals) override
iCub::ctrl::AWQuadEstimator * quadEstMot
virtual bool getMotorEncoderRaw(int m, double *v) override
virtual bool resetEncoderRaw(int j) override
const FSC max
Definition: strain.h:48
virtual bool getEncoderSpeedsRaw(double *spds) override
virtual bool getRefPositionRaw(const int joint, double *ref) override
virtual bool setPositionRaw(int j, double ref) override
virtual bool getAxisNameRaw(int axis, std::string &name) override
static int v
Definition: iCub_Sim.cpp:42
_3f_vect_t acc
Definition: dataTypes.h:123
static OdeInit & get()
Definition: OdeInit.cpp:189
virtual bool setRefAccelerationRaw(int j, double acc) override
virtual bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
virtual bool setRefDutyCyclesRaw(const double *v) override
virtual bool setRefSpeedsRaw(const double *spds) override
iCub::ctrl::AWLinEstimator * linEstMot
virtual bool setMaxCurrentRaw(int j, double val) override
virtual void setVelocity(double target)=0
Set velocity of joint (in ICUB units/sign).
virtual void setControlParameters(double vel, double acc)=0
Set velocity and acceleration control parameters.
virtual bool getTorquesRaw(double *) override
const FSC min
Definition: strain.h:49
#define MODE_CALIB_DONE
virtual bool getMaxCurrentRaw(int j, double *val) override
virtual bool enableAmpRaw(int j) override
virtual bool getPidOutputsRaw(const PidControlTypeEnum &pidtype, double *outs) override
This file is responsible for the initialisation of the world parameters that are controlled by ODE...
ODE state information.
Definition: OdeInit.h:55
virtual bool getRemoteVariableRaw(std::string key, yarp::os::Bottle &val) override
virtual bool getRemoteVariablesListRaw(yarp::os::Bottle *listOfKeys) override
virtual bool getPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double *limit) override
virtual bool calibrationDoneRaw(int j) override
virtual bool getTargetPositionsRaw(double *refs) override
virtual bool getTemperatureRaw(int m, double *val) override
virtual bool setLimitsRaw(int axis, double min, double max) override
virtual bool getRefCurrentsRaw(double *t) override
virtual bool isValid()=0
virtual bool getInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
virtual bool setNominalCurrentRaw(int m, const double val) override
yarp::sig::Vector estimate()
Execute the algorithm upon the elements list, with the max deviation threshold given by D...
virtual bool getInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum *mode) override
virtual bool getRefAccelerationRaw(int j, double *acc) override
virtual bool getNumberOfMotorEncodersRaw(int *num) override
virtual bool getNominalCurrentRaw(int m, double *val) override
#define MODE_VELOCITY
virtual bool getNumberOfMotorsRaw(int *m) override
IMotor.
virtual bool resetPidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool setRemoteVariableRaw(std::string key, const yarp::os::Bottle &val) override
virtual bool getPowerSupplyVoltageRaw(int j, double *val) override
static bool DEPRECATED(const char *txt)
#define MODE_MIXED
static bool NOT_YET_IMPLEMENTED(const char *txt)
virtual bool setPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double limit) override
iCub::ctrl::AWLinEstimator * linEstJnt
#define MODE_IMPEDANCE_POS
virtual void setPosition(double target)=0
Drive towards an angle setpoint (in ICUB units/sign).
virtual bool getEncoderSpeedRaw(int j, double *sp) override
virtual bool getTargetPositionRaw(const int joint, double *ref) override
virtual bool setPWMLimitRaw(int j, const double val) override
#define MODE_NOT_CONFIGURED
Adaptive window linear fitting to estimate the first derivative.
Abstract class for mapping from "physical" (simulated) joints in the model to control joints in the I...
Definition: LogicalJoint.h:33
virtual bool setRefSpeedRaw(int j, double sp) override
virtual bool getAmpStatusRaw(int *st) override
virtual LogicalJoint & control(int part, int axis)=0
Access the control for a logical joint, based on part and axis number.
virtual double getAngle()=0
Get the angle of an associated joint, in ICUB units and sign.
virtual bool getDutyCyclesRaw(double *v) override
virtual bool getPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, double *limits) override
virtual bool getRefVelocityRaw(const int joint, double *ref) override
virtual bool setVelLimitsRaw(int axis, double min, double max) override
virtual bool setPidOffsetRaw(const PidControlTypeEnum &pidtype, int j, double v) override
virtual bool setRefCurrentRaw(int j, double t) override
virtual bool getRefCurrentRaw(int j, double *t) override
virtual bool getControlModesRaw(int *modes) override
virtual bool getPidErrorsRaw(const PidControlTypeEnum &pidtype, double *errs) override
Adaptive window quadratic fitting to estimate the second derivative.
virtual bool enablePidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool setRefTorqueRaw(int, double) override
#define MODE_CONFIGURED
virtual bool getRefDutyCycleRaw(int j, double *v) override
This is the header file for the yarp interface of the iCubSimulation.
void setSimulationControl(iCubSimulationControl *control, int part)
Definition: OdeInit.cpp:125
virtual ~iCubSimulationControl()
Destructor.
void removeSimulationControl(int part)
Definition: OdeInit.cpp:182
virtual bool setMotorEncodersRaw(const double *vals) override
virtual bool setPositionsRaw(const int n_joint, const int *joints, const double *refs) override
#define MODE_PWM
bool done
Definition: main.cpp:42
virtual bool setTemperatureLimitRaw(int m, const double temp) override
virtual bool setPeakCurrentRaw(int m, const double val) override
virtual bool setControlModeRaw(const int j, const int mode) override
#define MODE_UNKNOWN
virtual bool getRefAccelerationsRaw(double *accs) override
virtual bool getMotorEncodersTimedRaw(double *encs, double *stamps) override
virtual bool getTemperaturesRaw(double *vals) override
virtual bool positionMoveRaw(int j, double ref) override
GLenum mode
Definition: rendering.cpp:48
virtual bool open(yarp::os::Searchable &config)
Open the device driver and start communication with the hardware.
virtual bool getCurrentRaw(int j, double *val) override
virtual bool getPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double *ref) override
virtual bool getEncoderRaw(int j, double *v) override
virtual bool disablePidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool getMotorEncoderAccelerationsRaw(double *accs) override
virtual bool getEncoderTimedRaw(int j, double *encs, double *stamp)
virtual bool disableAmpRaw(int j) override
virtual bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override
virtual bool getEncoderAccelerationsRaw(double *accs) override
virtual bool setPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double ref) override
cmd is a SingleAxis pointer with 1 double arg
virtual bool getMotorEncoderSpeedsRaw(double *spds) override