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