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?").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  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).asInt32();
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).asFloat64();
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).asFloat64()/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).asFloat64();
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).asFloat64();
224 
225  int mj_size = p.findGroup("GENERAL").check("Kinematic_mj_size",Value(0),"Default velocity").asInt32();
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).asFloat64();
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).asFloat64()*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).asFloat64()*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).asFloat64()*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  ImplementPositionControl::initialize(njoints, axisMap, angleToEncoder, zeros);
307  ImplementVelocityControl::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  ImplementControlMode::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 ) //fingers abduction
992  next_pos[axis] = limitsMax[axis]; //add explanation here
993  else
994  next_pos[axis] = limitsMin[axis];
995  }
996  else
997  {
998  next_pos[axis] = limitsMin[axis];
999  }
1000  ref_command_positions[axis] = next_pos[axis];
1001  }
1002  else if(ref > limitsMax[axis])
1003  {
1004  if (njoints == 16)
1005  {
1006  if ( axis == 7 ) //fingers abduction
1007  next_pos[axis] = fabs(limitsMax[axis] - limitsMax[axis]); //add explanation here
1008  else
1009  next_pos[axis] = limitsMax[axis];
1010  }
1011  else
1012  {
1013  next_pos[axis] = limitsMax[axis];
1014  }
1015  ref_command_positions[axis] = next_pos[axis];
1016  }
1017  else
1018  {
1019  ref_command_positions[axis] = ref;
1020  if (njoints == 16)
1021  {
1022  if ( axis == 10 || axis == 12 || axis == 14 )
1023  next_pos[axis] = ref/2;
1024  else if ( axis == 15 )
1025  next_pos[axis] = ref/3;
1026  else if ( axis == 7 )
1027  next_pos[axis] = fabs(limitsMax[axis] - ref);
1028  else
1029  next_pos[axis] = ref;
1030  }
1031  else
1032  {
1033  next_pos[axis] = ref;
1034  }
1035  }
1036 
1037  motor_on[axis]=true;
1038 
1039  if (verbosity)
1040  yDebug("moving joint %d of part %d to pos %f\n",axis, partSelec, next_pos[axis]);
1041  return true;
1042  }
1043  if (verbosity)
1044  yError("positionMoveRaw joint access too high %d \n",axis);
1045  return false;
1046 }
1047 
1049 {
1050  bool ret = true;
1051  for(int axis = 0; axis<njoints; axis++)
1052  {
1053  ret &= positionMoveRaw(axis, refs[axis]);
1054  }
1055  return ret;
1056 }
1057 
1058 bool iCubSimulationControl::relativeMoveRaw(int axis, double delta)
1059 {
1060  return positionMoveRaw(axis,next_pos[axis]+delta);
1061 }
1062 
1063 bool iCubSimulationControl::relativeMoveRaw(const double *deltas)
1064 {
1065  for(int axis = 0; axis<njoints; axis++) {
1066  relativeMoveRaw(axis,deltas[axis]);
1067  }
1068  return true;
1069 }
1070 
1072 {
1073  lock_guard<mutex> lck(_mutex);
1074  bool fin = true;
1075  for(int axis = 0;axis<njoints;axis++)
1076  {
1077  if(! (fabs( current_jnt_pos[axis]-next_pos[axis])<error_tol[axis]))
1078  {
1079  fin = false;
1080  }
1081  }
1082  if (verbosity)
1083  yDebug("motion finished error tol %f %f %f\n",error_tol[0],current_jnt_pos[0],next_pos[0]);
1084  *ret = fin;
1085  return true;
1086 }
1087 
1089 {
1090  if( (axis >=0) && (axis<njoints) )
1091  {
1092  lock_guard<mutex> lck(_mutex);
1093  if(fabs(current_jnt_pos[axis]-next_pos[axis])<error_tol[axis])
1094  *ret = true;
1095  else
1096  *ret = false;
1097  return true;
1098  }
1099  if (verbosity)
1100  yError("checkMotionDoneRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1101  return false;
1102 }
1103 bool iCubSimulationControl::setRefSpeedRaw(int axis, double sp)
1104 {
1105  if( (axis >=0) && (axis<njoints) )
1106  {
1107  lock_guard<mutex> lck(_mutex);
1108  //vel = sp;// *180/M_PI ;
1109  vels[axis] = sp;//vel/20;
1110  ref_speeds[axis] = sp;
1111  if (verbosity)
1112  yDebug("setting joint %d of part %d to reference velocity %f\n",axis,partSelec,vels[axis]);
1113  return true;
1114  }
1115  return false;
1116 }
1117 
1119 {
1120  bool ret = true;
1121  for (int axis = 0; axis<njoints; axis++)
1122  {
1123  ret &= velocityMoveRaw(axis, spds[axis]);
1124  }
1125  return ret;
1126 }
1128 {
1129  return NOT_YET_IMPLEMENTED("setRefAccelerationRaw");
1130 }
1132 {
1133  return NOT_YET_IMPLEMENTED("setRefAccelerationsRaw");
1134 }
1135 bool iCubSimulationControl::getRefSpeedRaw(int axis, double *ref)
1136 {
1137  if((axis>=0) && (axis<njoints)) {
1138  lock_guard<mutex> lck(_mutex);
1139  *ref = ref_speeds[axis];
1140  return true;
1141  }
1142  if (verbosity)
1143  yError("getRefSpeedRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1144  return false;
1145 }
1147 {
1148  bool ret = true;
1149  for (int i = 0; i<njoints; i++)
1150  {
1151  ret &= getRefSpeedRaw(i, &spds[i]);
1152  }
1153  return ret;
1154 }
1156 {
1157  return NOT_YET_IMPLEMENTED("getRefAccelerationRaw");
1158 }
1160 {
1161  return NOT_YET_IMPLEMENTED("getRefAccelerationsRaw");
1162 }
1164 {
1165  if( (axis>=0) && (axis<njoints) )
1166  {
1167  lock_guard<mutex> lck(_mutex);
1168  next_pos[axis] = current_jnt_pos[axis];
1169  next_vel[axis] = 0.0;
1170  return true;
1171  }
1172  if (verbosity)
1173  yError("stopRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1174  return false;
1175 }
1177 {
1178  lock_guard<mutex> lck(_mutex);
1179  for(int axis=0;axis<njoints;axis++)
1180  {
1181  next_pos[axis] = current_jnt_pos[axis];
1182  next_vel[axis] = 0.0;
1183  }
1184  return true;
1185 }
1186 
1188 {
1189  if ((axis >= 0) && (axis<njoints)) {
1190  lock_guard<mutex> lck(_mutex);
1191  *ref = ref_command_positions[axis];
1192  return true;
1193  }
1194  if (verbosity)
1195  yError("getTargetPositionRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1196  return false;
1197 }
1198 
1200 {
1201  bool ret = true;
1202  for (int i = 0; i<njoints; i++)
1203  {
1204  ret &= getTargetPositionRaw(i, &refs[i]);
1205  }
1206  return ret;
1207 }
1208 
1209 bool iCubSimulationControl::getTargetPositionsRaw(int nj, const int * jnts, double *refs)
1210 {
1211  bool ret = true;
1212  for (int i = 0; i<nj; i++)
1213  {
1214  ret &= getTargetPositionRaw(jnts[i], &refs[i]);
1215  }
1216  return ret;
1217 }
1218 
1219 bool iCubSimulationControl::getRefVelocityRaw(int axis, double *ref)
1220 {
1221  lock_guard<mutex> lck(_mutex);
1222  *ref = ref_command_speeds[axis];
1223  return true;
1224 }
1225 
1227 {
1228  bool ret = true;
1229  for (int i = 0; i<njoints; i++)
1230  {
1231  ret &= getRefVelocityRaw(i, &refs[i]);
1232  }
1233  return ret;
1234 }
1235 
1236 bool iCubSimulationControl::getRefVelocitiesRaw(int nj, const int * jnts, double *refs)
1237 {
1238  bool ret = true;
1239  for (int i = 0; i<nj; i++)
1240  {
1241  ret &= getRefVelocityRaw(jnts[i], &refs[i]);
1242  }
1243  return ret;
1244 }
1245 
1246 bool iCubSimulationControl::getRefPositionRaw(int axis, double *ref)
1247 {
1248  lock_guard<mutex> lck(_mutex);
1249  *ref = ref_positions[axis];
1250  return true;
1251 }
1252 
1254 {
1255  bool ret = true;
1256  for (int i = 0; i<njoints; i++)
1257  {
1258  ret &= getRefPositionRaw(i, &refs[i]);
1259  }
1260  return ret;
1261 }
1262 
1263 
1264 bool iCubSimulationControl::getRefPositionsRaw(int nj, const int * jnts, double *refs)
1265 {
1266  bool ret = true;
1267  for (int i = 0; i<nj; i++)
1268  {
1269  ret &= getRefPositionRaw(jnts[i], &refs[i]);
1270  }
1271  return ret;
1272 }
1273 
1274 bool iCubSimulationControl::positionMoveRaw(int nj, const int * jnts, const double *refs)
1275 {
1276  bool ret = true;
1277  for (int i = 0; i<nj; i++)
1278  {
1279  ret &= positionMoveRaw(jnts[i], refs[i]);
1280  }
1281  return ret;
1282 }
1283 
1284 bool iCubSimulationControl::relativeMoveRaw(int nj, const int * jnts, const double *refs)
1285 {
1286  bool ret = true;
1287  for (int i = 0; i<nj; i++)
1288  {
1289  ret &= relativeMoveRaw(jnts[i], refs[i]);
1290  }
1291  return ret;
1292 }
1293 
1294 bool iCubSimulationControl::checkMotionDoneRaw(int nj, const int * jnts, bool *done)
1295 {
1296  bool ret = true;
1297  bool jDone(true), tmpDone(true);
1298 
1299  for (int i = 0; i<nj; i++)
1300  {
1301  ret &= checkMotionDoneRaw(jnts[i], &jDone);
1302  tmpDone &= jDone;
1303  }
1304  *done = tmpDone;
1305  return ret;
1306 }
1307 
1308 bool iCubSimulationControl::setRefSpeedsRaw(const int nj, const int * jnts, const double *refs)
1309 {
1310  bool ret = true;
1311  for (int i = 0; i<nj; i++)
1312  {
1313  ret &= setRefSpeedRaw(jnts[i], refs[i]);
1314  }
1315  return ret;
1316 }
1317 
1318 bool iCubSimulationControl::getRefSpeedsRaw(const int nj, const int * jnts, double *refs)
1319 {
1320  bool ret = true;
1321  for (int i = 0; i<nj; i++)
1322  {
1323  ret &= getRefSpeedRaw(jnts[i], &refs[i]);
1324  }
1325  return ret;
1326 }
1327 
1328 // cmd is an array of double of length njoints specifying speed
1330 bool iCubSimulationControl::velocityMoveRaw (int axis, double sp)
1331 {
1332  if( (axis >=0) && (axis<njoints) )
1333  {
1334  int mode = 0;
1335  getControlModeRaw(axis, &mode);
1336  if (mode != VOCAB_CM_VELOCITY &&
1337  mode != VOCAB_CM_MIXED)
1338  {
1339  yError() << "velocityMoveRaw: skipping command because part " << partSelec << " joint " << axis << "is not in VOCAB_CM_VELOCITY mode";
1340  return false;
1341  }
1342  lock_guard<mutex> lck(_mutex);
1343  next_vel[axis] = sp;
1344  ref_command_speeds[axis] = sp;
1345  motor_on[axis] = true;
1346  return true;
1347  }
1348  if (verbosity)
1349  yError("velocityMoveRaw: joint with index %d does not exist, valis joints indices are between 0 and %d \n",axis,njoints);
1350  return false;
1351 }
1352 
1356 {
1357  bool ret = true;
1358  for (int axis=0; axis<njoints; axis++)
1359  {
1360  ret &= velocityMoveRaw(axis,sp[axis]);
1361  }
1362  return ret;
1363 }
1364 
1365 bool iCubSimulationControl::setEncoderRaw(int axis, double val)
1366 {
1367  return NOT_YET_IMPLEMENTED("setEncoderRaw");
1368 }
1369 
1371 {
1372  return NOT_YET_IMPLEMENTED("setEncodersRaw");
1373 }
1374 
1376 {
1377  return NOT_YET_IMPLEMENTED("resetEncoderRaw");
1378 }
1379 
1381 {
1382  return NOT_YET_IMPLEMENTED("resetEncodersRaw");
1383 }
1384 
1386 {
1387  lock_guard<mutex> lck(_mutex);
1388  for(int axis = 0;axis<njoints;axis++)
1389  {
1390  if ( axis == 10 || axis == 12 || axis == 14 )
1391  v[axis] = current_jnt_pos[axis]*2;
1392  else if ( axis == 15 )
1393  v[axis] = current_jnt_pos[axis]*3;
1394  else if ( axis == 7 )
1395  v[axis] = limitsMax[axis] - current_jnt_pos[axis];
1396  else
1397  v[axis] = current_jnt_pos[axis];
1398  }
1399  return true;
1400 }
1401 
1403 {
1404  if((axis>=0) && (axis<njoints)) {
1405  lock_guard<mutex> lck(_mutex);
1406 
1407  if ( axis == 10 || axis == 12 || axis == 14 )
1408  *v = current_jnt_pos[axis]*2;
1409  else if ( axis == 15 )
1410  *v = current_jnt_pos[axis]*3;
1411  else if ( axis == 7 )
1412  *v = limitsMax[axis] - current_jnt_pos[axis];
1413  else
1414  *v = current_jnt_pos[axis];
1415 
1416  return true;
1417  }
1418  if (verbosity)
1419  yError("getEncoderRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1420  return false;
1421 }
1422 
1424 {
1425  double timeNow = Time::now();
1426  for(int axis = 0;axis<njoints;axis++)
1427  {
1428  stamps[axis] = timeNow;
1429  }
1430  return getEncodersRaw(encs);
1431 }
1432 
1433 bool iCubSimulationControl::getEncoderTimedRaw(int axis, double *enc, double *stamp)
1434 {
1435  *stamp = Time::now();
1436  return getEncoderRaw(axis, enc);
1437 }
1438 
1439 
1441 {
1442  lock_guard<mutex> lck(_mutex);
1443  for(int axis = 0; axis<njoints; axis++)
1444  v[axis] = current_jnt_vel[axis];//* 10;
1445  return true;
1446 }
1447 
1449 {
1450  if( (axis>=0) && (axis<njoints) ) {
1451  lock_guard<mutex> lck(_mutex);
1452  *v = current_jnt_vel[axis];// * 10;
1453  return true;
1454  }
1455  if (verbosity)
1456  yError("getEncoderSpeedRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1457  return false;
1458 }
1459 
1461 {
1462  lock_guard<mutex> lck(_mutex);
1463  for (int axis = 0; axis<njoints; axis++)
1464  v[axis] = current_jnt_acc[axis];//* 10;
1465  return true;
1466 }
1467 
1469 {
1470  if ((axis >= 0) && (axis<njoints)) {
1471  lock_guard<mutex> lck(_mutex);
1472  *v = current_jnt_acc[axis];// * 10;
1473  return true;
1474  }
1475  if (verbosity)
1476  yError("getEncoderSpeedRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1477  return false;
1478 }
1479 
1480 bool iCubSimulationControl::setMotorEncoderRaw(int axis, const double val)
1481 {
1482  return NOT_YET_IMPLEMENTED("setMotorEncoderRaw");
1483 }
1484 
1486 {
1487  return NOT_YET_IMPLEMENTED("setMotorEncodersRaw");
1488 }
1489 
1491 {
1492  return NOT_YET_IMPLEMENTED("resetMotorEncoderRaw");
1493 }
1494 
1496 {
1497  return NOT_YET_IMPLEMENTED("resetMotorEncodersRaw");
1498 }
1499 
1501 {
1502  for(int axis = 0;axis<njoints;axis++)
1503  {
1504  getMotorEncoderRaw(axis,&v[axis]);
1505  }
1506  return true;
1507 }
1508 
1510 {
1511  if((axis>=0) && (axis<njoints))
1512  {
1513  lock_guard<mutex> lck(_mutex);
1514  *v = current_mot_pos[axis];
1515  return true;
1516  }
1517  if (verbosity)
1518  yError("getMotorEncoderRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1519  return false;
1520 }
1521 
1523 {
1524  *num=njoints;
1525  return true;
1526 }
1527 
1529 {
1530  return NOT_YET_IMPLEMENTED("setMotorEncoderCountsPerRevolutionRaw");
1531 }
1532 
1534 {
1535  return NOT_YET_IMPLEMENTED("getMotorEncoderCountsPerRevolutionRaw");
1536 }
1537 
1538 
1540 {
1541  double timeNow = Time::now();
1542  for(int axis = 0;axis<njoints;axis++)
1543  {
1544  stamps[axis] = timeNow;
1545  }
1546  return getMotorEncodersRaw(encs);
1547 }
1548 
1549 bool iCubSimulationControl::getMotorEncoderTimedRaw(int axis, double *enc, double *stamp)
1550 {
1551  *stamp = Time::now();
1552  return getMotorEncoderRaw(axis, enc);
1553 }
1554 
1555 
1557 {
1558  for(int axis = 0; axis<njoints; axis++)
1559  {
1560  getMotorEncoderSpeedRaw(axis,&v[axis]);
1561  }
1562  return true;
1563 }
1564 
1566 {
1567  if( (axis>=0) && (axis<njoints) ) {
1568  lock_guard<mutex> lck(_mutex);
1569  *v = current_mot_vel[axis];
1570  return true;
1571  }
1572  if (verbosity)
1573  yError("getEncoderSpeedRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1574  return false;
1575 }
1576 
1578 {
1579  for (int axis = 0; axis<njoints; axis++)
1580  {
1581  getMotorEncoderAccelerationRaw(axis, &v[axis]);
1582  }
1583  return true;
1584 }
1585 
1587 {
1588  if ((axis >= 0) && (axis<njoints)) {
1589  lock_guard<mutex> lck(_mutex);
1590  *v = current_mot_acc[axis];
1591  return true;
1592  }
1593  if (verbosity)
1594  yError("getEncoderSpeedRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1595  return false;
1596 }
1597 
1599 {
1600  return DEPRECATED ("disableAmpRaw");
1601 }
1602 
1604 {
1605  return DEPRECATED ("enableAmpRaw");
1606 }
1607 
1608 // bcast
1610 {
1611  lock_guard<mutex> lck(_mutex);
1612  for(int axis = 0; axis<njoints; axis++)
1613  cs[axis] = current_ampere[axis];
1614  return true;
1615 }
1616 
1617 // bcast currents
1618 bool iCubSimulationControl::getCurrentRaw(int axis, double *c)
1619 {
1620  if( (axis>=0) && (axis<njoints) )
1621  {
1622  lock_guard<mutex> lck(_mutex);
1623  *c = current_ampere[axis];
1624  }
1625  else
1626  {
1627  if (verbosity)
1628  yError("getCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1629  }
1630  return true;
1631 }
1632 
1634 {
1635  if( (axis>=0) && (axis<njoints) )
1636  {
1637  lock_guard<mutex> lck(_mutex);
1638  maxCurrent[axis]=v;
1639  }
1640  else
1641  {
1642  if (verbosity)
1643  yError("setMaxCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1644  }
1645  return true;
1646 }
1647 
1649 {
1650  if( (axis>=0) && (axis<njoints) )
1651  {
1652  lock_guard<mutex> lck(_mutex);
1653  *v=maxCurrent[axis];
1654  }
1655  else
1656  {
1657  if (verbosity)
1658  yError("getMaxCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1659  }
1660  return true;
1661 }
1662 
1663 bool iCubSimulationControl::calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3)
1664 {
1665  return NOT_YET_IMPLEMENTED("calibrateRaw");
1666 }
1667 
1669 {
1670  return NOT_YET_IMPLEMENTED("doneRaw");
1671 }
1672 
1673 
1675 {
1676  lock_guard<mutex> lck(_mutex);
1677  for(int axis =0;axis<njoints;axis++)
1678  st[axis] = (int)motor_on[axis];
1679  return true;
1680 }
1681 
1683 {
1684  if( (axis>=0) && (axis<njoints))
1685  {
1686  lock_guard<mutex> lck(_mutex);
1687  st[axis] = (int)motor_on[axis];
1688  return true;
1689  }
1690  if (verbosity)
1691  yError("getAmpStatusRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1692  return false;
1693 }
1694 
1695 bool iCubSimulationControl::setLimitsRaw(int axis, double min, double max)
1696 {
1697  if( (axis >=0) && (axis < njoints) ){
1698  lock_guard<mutex> lck(_mutex);
1699  limitsMax[axis] = max;
1700  limitsMin[axis] = min;
1701  return true;
1702  }
1703  if (verbosity)
1704  yError("setLimitsRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1705  return false;
1706 }
1707 
1708 bool iCubSimulationControl::getLimitsRaw(int axis, double *min, double *max)
1709 {
1710  if( (axis >=0) && (axis < njoints)) {
1711  lock_guard<mutex> lck(_mutex);
1712  *min = limitsMin[axis];
1713  *max = limitsMax[axis];
1714  return true;
1715  }
1716  //else
1717  return false;
1718 }
1719 
1720 // IRemoteVariables
1721 bool iCubSimulationControl::getRemoteVariableRaw(string key, yarp::os::Bottle& val)
1722 {
1723  val.clear();
1724  if (key == "kinematic_mj")
1725  {
1726  val.addString("1 2 3"); return true;
1727  }
1728  else if (key == "rotor")
1729  {
1730  Bottle& r = val.addList(); for (int i=0; i<njoints; i++) r.addFloat64(rotToEncoder[i]); return true;
1731  }
1732  else if (key == "gearbox")
1733  {
1734  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addFloat64(gearbox[i]); return true;
1735  }
1736  else if (key == "zeros")
1737  {
1738  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addFloat64(zeros[i]); return true;
1739  }
1740  else if (key == "hasHallSensor")
1741  {
1742  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addInt32(hasHallSensor[i]); return true;
1743  }
1744  else if (key == "hasTempSensor")
1745  {
1746  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addInt32(hasTempSensor[i]); return true;
1747  }
1748  else if (key == "hasRotorEncoder")
1749  {
1750  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addInt32(hasRotorEncoder[i]); return true;
1751  }
1752  else if (key == "rotorIndexOffset")
1753  {
1754  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addInt32(rotorIndexOffset[i]); return true;
1755  }
1756  else if (key == "motorPoles")
1757  {
1758  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addInt32(motorPoles[i]); return true;
1759  }
1760  else if (key == "pidCurrentKp")
1761  {
1762  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addFloat64(current_pid[i].kp); return true;
1763  }
1764  else if (key == "pidCurrentKi")
1765  {
1766  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addFloat64(current_pid[i].ki); return true;
1767  }
1768  else if (key == "pidCurrentShift")
1769  {
1770  Bottle& r = val.addList(); for (int i = 0; i<njoints; i++) r.addFloat64(current_pid[i].scale); return true;
1771  }
1772  yWarning("getRemoteVariable(): Unknown variable %s", key.c_str());
1773  return false;
1774 }
1775 
1776 bool iCubSimulationControl::setRemoteVariableRaw(string key, const yarp::os::Bottle& val)
1777 {
1778  string s1 = val.toString();
1779  Bottle* bval = val.get(0).asList();
1780  if (bval == 0)
1781  {
1782  yWarning("setRemoteVariable(): Protocol error %s", s1.c_str());
1783  return false;
1784  }
1785 
1786  string s2 = bval->toString();
1787  if (key == "kinematic_mj")
1788  {
1789  return true;
1790  }
1791  else if (key == "rotor")
1792  {
1793  for (int i = 0; i < njoints; i++)
1794  rotToEncoder[i] = bval->get(i).asFloat64();
1795  return true;
1796  }
1797  else if (key == "gearbox")
1798  {
1799  for (int i = 0; i < njoints; i++) gearbox[i] = bval->get(i).asFloat64(); return true;
1800  }
1801  else if (key == "zeros")
1802  {
1803  for (int i = 0; i < njoints; i++) zeros[i] = bval->get(i).asFloat64(); return true;
1804  }
1805  yWarning("setRemoteVariable(): Unknown variable %s", key.c_str());
1806  return false;
1807 }
1808 
1809 bool iCubSimulationControl::getRemoteVariablesListRaw(yarp::os::Bottle* listOfKeys)
1810 {
1811  listOfKeys->clear();
1812  listOfKeys->addString("kinematic_mj");
1813  listOfKeys->addString("rotor");
1814  listOfKeys->addString("gearbox");
1815  listOfKeys->addString("hasHallSensor");
1816  listOfKeys->addString("hasTempSensor");
1817  listOfKeys->addString("hasRotorEncoder");
1818  listOfKeys->addString("rotorIndexOffset");
1819  listOfKeys->addString("motorPoles");
1820  listOfKeys->addString("pidCurrentKp");
1821  listOfKeys->addString("pidCurrentKi");
1822  listOfKeys->addString("pidCurrentShift");
1823  return true;
1824 }
1825 
1826 bool iCubSimulationControl::setVelLimitsRaw(int axis, double min, double max)
1827 {
1828  if ((axis >= 0) && (axis < njoints)){
1829  lock_guard<mutex> lck(_mutex);
1830  velLimitsMax[axis] = max;
1831  velLimitsMin[axis] = min;
1832  return true;
1833  }
1834  if (verbosity)
1835  yError("setVelLimitsRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis, njoints);
1836  return false;
1837 }
1838 
1839 bool iCubSimulationControl::getVelLimitsRaw(int axis, double *min, double *max)
1840 {
1841  if ((axis >= 0) && (axis < njoints)) {
1842  lock_guard<mutex> lck(_mutex);
1843  *min = velLimitsMin[axis];
1844  *max = velLimitsMax[axis];
1845  return true;
1846  }
1847  //else
1848  return false;
1849 }
1850 
1851 bool iCubSimulationControl::velocityMoveRaw(const int n_joint, const int *joints, const double *spds)
1852 {
1853  bool ret = true;
1854  for(int j=0; j<n_joint; j++)
1855  {
1856  ret = ret && velocityMoveRaw(joints[j], spds[j]);
1857  }
1858  return ret;
1859 }
1860 
1861 bool iCubSimulationControl::setRefAccelerationsRaw(const int n_joint, const int *joints, const double *accs)
1862 {
1863  bool ret = true;
1864  for(int j=0; j<n_joint; j++)
1865  {
1866  ret = ret && setRefAccelerationRaw(joints[j], accs[j]);
1867  }
1868  return ret;
1869 }
1870 
1871 bool iCubSimulationControl::getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs)
1872 {
1873  bool ret = true;
1874  for(int j=0; j<n_joint; j++)
1875  {
1876  ret = ret && getRefAccelerationRaw(joints[j], &accs[j]);
1877  }
1878  return ret;
1879 }
1880 
1881 bool iCubSimulationControl::stopRaw(const int n_joint, const int *joints)
1882 {
1883  bool ret = true;
1884  for(int j=0; j<n_joint; j++)
1885  {
1886  ret = ret && stopRaw(joints[j]);
1887  }
1888  return ret;
1889 }
1890 
1891 bool iCubSimulationControl::getAxisNameRaw(int axis, string& name)
1892 {
1893  if ((axis >= 0) && (axis < njoints))
1894  {
1895  lock_guard<mutex> lck(_mutex);
1896  char buff[100];
1897  sprintf(buff,"JOINT%d", axis);
1898  name.assign(buff,strlen(buff));
1899  return true;
1900  }
1901 
1902  return false;
1903 }
1904 
1905 bool iCubSimulationControl::getJointTypeRaw(int axis, yarp::dev::JointTypeEnum& type)
1906 {
1907  if ((axis >= 0) && (axis < njoints))
1908  {
1909  lock_guard<mutex> lck(_mutex);
1910  type = yarp::dev::VOCAB_JOINTTYPE_REVOLUTE;
1911  return true;
1912  }
1913 
1914  return false;
1915 }
1916 
1917 bool iCubSimulationControl::getTorqueRaw(int axis, double *sp)
1918 {
1919  if( (axis >=0) && (axis < njoints)) {
1920  lock_guard<mutex> lck(_mutex);
1921  *sp = current_jnt_torques[axis];
1922  return true;
1923  }
1924  return false;
1925 }
1927 {
1928  lock_guard<mutex> lck(_mutex);
1929  for(int axis = 0;axis<njoints;axis++)
1930  sp[axis] = current_jnt_torques[axis];
1931  return true;
1932 }
1933 bool iCubSimulationControl::getTorqueRangeRaw(int axis, double *a,double *b)
1934 {
1935  return NOT_YET_IMPLEMENTED("getTorqueRangeRaw");
1936 }
1938 {
1939  return NOT_YET_IMPLEMENTED("getTorqueRangesRaw");
1940 }
1942 {
1943  bool ret = true;
1944  for (int i = 0; i<njoints; i++)
1945  {
1946  ret &= setRefTorqueRaw(i, sp[i]);
1947  }
1948  return ret;
1949 }
1950 
1952 {
1953  if( (axis >=0) && (axis<njoints) ) {
1954  lock_guard<mutex> lck(_mutex);
1955  next_torques[axis] = ref;
1956  ref_torques[axis] = ref;
1957  motor_on[axis] = true;
1958  return true;
1959  }
1960  if (verbosity)
1961  yError("setRefTorqueRaw: joint with index %d does not exist, valis joints indices are between 0 and %d \n",axis,njoints);
1962  return false;
1963 }
1964 
1965 bool iCubSimulationControl::setRefTorquesRaw(const int n_joint, const int *joints, const double *t)
1966 {
1967  bool ret = true;
1968  for (int j = 0; j<n_joint; j++)
1969  {
1970  ret = ret && setRefTorqueRaw(joints[j], t[j]);
1971  }
1972  return ret;
1973 }
1974 
1976 {
1977  bool ret = true;
1978  for (int i = 0; i<njoints; i++)
1979  {
1980  ret &= getRefTorqueRaw(i, &refs[i]);
1981  }
1982  return ret;
1983 }
1984 bool iCubSimulationControl::getRefTorqueRaw(int axis,double *ref)
1985 {
1986  if( (axis >=0) && (axis<njoints) ) {
1987  lock_guard<mutex> lck(_mutex);
1988  *ref = next_torques[axis];
1989  return true;
1990  }
1991  if (verbosity)
1992  yError("getRefTorqueRaw: joint with index %d does not exist, valis joints indices are between 0 and %d \n",axis,njoints);
1993  return false;
1994 }
1995 
1996 bool iCubSimulationControl::getMotorTorqueParamsRaw(int axis, MotorTorqueParameters *params)
1997 {
1998  if( (axis >=0) && (axis<njoints) )
1999  {
2000  params->bemf=motor_torque_params[axis].bemf;
2001  params->bemf_scale=motor_torque_params[axis].bemf_scale;
2002  params->ktau=motor_torque_params[axis].ktau;
2003  params->ktau_scale=motor_torque_params[axis].ktau_scale;
2004  }
2005  NOT_YET_IMPLEMENTED("getMotorTorqueParamsRaw");
2006  return true; //fake
2007 
2008 }
2009 bool iCubSimulationControl::setMotorTorqueParamsRaw(int axis, const MotorTorqueParameters params)
2010 {
2011  if( (axis >=0) && (axis<njoints) )
2012  {
2013  motor_torque_params[axis].bemf=params.bemf;
2014  motor_torque_params[axis].bemf_scale=params.bemf_scale;
2015  motor_torque_params[axis].ktau=params.ktau;
2016  motor_torque_params[axis].ktau_scale=params.ktau_scale;
2017  }
2018  NOT_YET_IMPLEMENTED("setMotorTorqueParamsRaw");
2019  return true; //fake
2020 }
2021 
2022 int iCubSimulationControl::ControlModes_yarp2iCubSIM(int yarpMode)
2023 {
2024  int ret = VOCAB_CM_UNKNOWN;
2025  switch(yarpMode)
2026  {
2027  case VOCAB_CM_FORCE_IDLE:
2028  case VOCAB_CM_IDLE:
2029  ret = MODE_IDLE;
2030  break;
2031  case VOCAB_CM_POSITION:
2032  ret = MODE_POSITION;
2033  break;
2034  case VOCAB_CM_VELOCITY:
2035  ret = MODE_VELOCITY;
2036  break;
2037  case VOCAB_CM_TORQUE:
2038  ret = MODE_TORQUE;
2039  break;
2040  case VOCAB_CM_IMPEDANCE_POS:
2041  ret = MODE_IMPEDANCE_POS;
2042  break;
2043  case VOCAB_CM_IMPEDANCE_VEL:
2044  ret = MODE_IMPEDANCE_VEL;
2045  break;
2046  case VOCAB_CM_PWM:
2047  ret = MODE_PWM;
2048  break;
2049  case VOCAB_CM_CURRENT:
2050  ret = MODE_CURRENT;
2051  break;
2052 
2053  // for new modes I´ll use directly the vocabs, so they are the same
2054  case VOCAB_CM_MIXED:
2055  case VOCAB_CM_POSITION_DIRECT:
2056  case VOCAB_CM_HW_FAULT:
2057  case VOCAB_CM_CALIBRATING:
2058  case VOCAB_CM_CALIB_DONE:
2059  case VOCAB_CM_NOT_CONFIGURED:
2060  case VOCAB_CM_CONFIGURED:
2061  ret = yarpMode;
2062  break;
2063 
2064  default:
2065  ret = MODE_UNKNOWN;
2066  break;
2067  }
2068  return ret;
2069 }
2070 
2071 int iCubSimulationControl::ControlModes_iCubSIM2yarp(int iCubMode)
2072 {
2073  int ret = VOCAB_CM_UNKNOWN;
2074  switch(iCubMode)
2075  {
2076  case MODE_IDLE:
2077  ret=VOCAB_CM_IDLE;
2078  break;
2079  case MODE_POSITION:
2080  ret=VOCAB_CM_POSITION;
2081  break;
2082  case MODE_VELOCITY:
2083  ret=VOCAB_CM_VELOCITY;
2084  break;
2085  case MODE_TORQUE:
2086  ret=VOCAB_CM_TORQUE;
2087  break;
2088  case MODE_IMPEDANCE_POS:
2089  ret=VOCAB_CM_IMPEDANCE_POS;
2090  break;
2091  case MODE_IMPEDANCE_VEL:
2092  ret=VOCAB_CM_IMPEDANCE_VEL;
2093  break;
2094  case MODE_PWM:
2095  ret=VOCAB_CM_PWM;
2096  break;
2097  case MODE_CURRENT:
2098  ret = VOCAB_CM_CURRENT;
2099  break;
2100 
2101  // for new modes I´ll use directly the vocabs, so they are the same
2102  case MODE_MIXED:
2103  case MODE_HW_FAULT:
2104  case VOCAB_CM_POSITION_DIRECT:
2105  case MODE_CALIBRATING:
2106  case MODE_CALIB_DONE:
2107  case MODE_NOT_CONFIGURED:
2108  case MODE_CONFIGURED:
2109  ret = iCubMode;
2110  break;
2111 
2112  default:
2113  ret=VOCAB_CM_UNKNOWN;
2114  break;
2115  }
2116  return ret;
2117 }
2118 
2120 {
2121  if( (j >=0) && (j < njoints)) {
2122  lock_guard<mutex> lck(_mutex);
2123  *mode = ControlModes_iCubSIM2yarp(controlMode[j]);
2124  return true;
2125  }
2126  if (verbosity)
2127  yError("getControlModeRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j, njoints);
2128  return false;
2129 }
2130 
2132 {
2133  lock_guard<mutex> lck(_mutex);
2134  for(int axis = 0;axis<njoints;axis++)
2135  modes[axis] = ControlModes_iCubSIM2yarp(controlMode[axis]);
2136  return true;
2137 }
2138 
2140 bool iCubSimulationControl::getControlModesRaw(const int n_joint, const int *joints, int *modes)
2141 {
2142  bool ret = true;
2143  for(int idx=0; idx< n_joint; idx++)
2144  ret = ret && getControlModeRaw(joints[idx], &modes[idx]);
2145  return ret;
2146 }
2147 
2149 {
2150  int tmp;
2151  if( (j >=0) && (j < njoints) )
2152  {
2153  tmp = ControlModes_yarp2iCubSIM(mode);
2154  if(tmp == MODE_UNKNOWN)
2155  {
2156  yError() << "setControlModeRaw: unknown control mode " << yarp::os::Vocab32::decode(mode);
2157  }
2158  else if (controlMode[j] == VOCAB_CM_HW_FAULT && mode != VOCAB_CM_FORCE_IDLE)
2159  {
2160  yError() << "setControlModeRaw: unable to reset an HW_FAULT without a VOCAB_CM_FORCE_IDLE command";
2161  }
2162  else
2163  {
2164  lock_guard<mutex> lck(_mutex);
2165  controlMode[j] = ControlModes_yarp2iCubSIM(mode);
2166  next_pos[j]=current_jnt_pos[j];
2167  if (controlMode[j] != MODE_PWM) pwm_ref[j] = 0;
2168  if (controlMode[j] != MODE_CURRENT) current_ampere_ref[j] = 0;
2169  }
2170  return true;
2171  }
2172  if (verbosity)
2173  yError("setControlModeRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j, njoints);
2174  return false;
2175 }
2176 
2177 bool iCubSimulationControl::setControlModesRaw(const int n_joint, const int *joints, int *modes)
2178 {
2179  bool ret = true;
2180  for(int idx=0; idx<n_joint; idx++)
2181  {
2182  ret = ret && setControlModeRaw(joints[idx], modes[idx]);
2183  }
2184  return ret;
2185 }
2186 
2188 {
2189  bool ret = true;
2190  for(int j=0; j<njoints; j++)
2191  {
2192  ret = ret && setControlModeRaw(j, modes[j]);
2193  }
2194  return ret;
2195 }
2196 
2198 bool iCubSimulationControl::getInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum* mode)
2199 {
2200  *mode = (yarp::dev::InteractionModeEnum) interactionMode[axis];
2201  return true;
2202 }
2203 
2204 bool iCubSimulationControl::getInteractionModesRaw(int n_joints, int *__joints, yarp::dev::InteractionModeEnum* modes)
2205 {
2206  bool ret = true;
2207  for(int idx=0; idx<n_joints; idx++)
2208  {
2209  ret = ret && getInteractionModeRaw(__joints[idx], &modes[idx]);
2210  }
2211  return ret;
2212 }
2213 
2214 bool iCubSimulationControl::getInteractionModesRaw(yarp::dev::InteractionModeEnum* modes)
2215 {
2216  bool ret = true;
2217  for(int j=0; j<njoints; j++)
2218  {
2219  ret = ret && getInteractionModeRaw(j, &modes[j]);
2220  }
2221  return ret;
2222 }
2223 
2224 bool iCubSimulationControl::setInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum mode)
2225 {
2226  interactionMode[axis] = (int)mode;
2227  next_pos[axis]=current_jnt_pos[axis];
2228  return true;
2229 }
2230 
2231 bool iCubSimulationControl::setInteractionModesRaw(int n_joints, int *__joints, yarp::dev::InteractionModeEnum* modes)
2232 {
2233  bool ret = true;
2234  for(int idx=0; idx<n_joints; idx++)
2235  {
2236  ret = ret && setInteractionModeRaw(__joints[idx], modes[idx]);
2237  }
2238  return ret;
2239 }
2240 
2241 bool iCubSimulationControl::setInteractionModesRaw(yarp::dev::InteractionModeEnum* modes)
2242 {
2243  bool ret = true;
2244  for(int j=0; j<njoints; j++)
2245  {
2246  ret = ret && setInteractionModeRaw(j, modes[j]);
2247  }
2248  return ret;
2249 }
2250 
2251 // Position direct interface
2252 bool iCubSimulationControl::setPositionRaw(int axis, double ref)
2253 {
2254  // This function has been copy pasted from positionMoveRaw
2255  if( (axis >=0) && (axis<njoints) )
2256  {
2257  int mode = 0;
2258  getControlModeRaw(axis, &mode);
2259  if (mode != VOCAB_CM_POSITION_DIRECT)
2260  {
2261  yError() << "setPositionRaw: skipping command because part " << partSelec << " joint" << axis << "is not in VOCAB_CM_POSITION_DIRECT mode";
2262  return false;
2263  }
2264  lock_guard<mutex> lck(_mutex);
2265  if(ref< limitsMin[axis])
2266  {
2267  if (njoints == 16)
2268  {
2269  if ( axis == 7 ) //fingers abduction
2270  next_pos[axis] = limitsMax[axis]; //add explanation here
2271  else
2272  next_pos[axis] = limitsMin[axis];
2273  }
2274  else
2275  {
2276  next_pos[axis] = limitsMin[axis];
2277  }
2278  ref_positions[axis] = next_pos[axis];
2279  }
2280  else if(ref > limitsMax[axis])
2281  {
2282  if (njoints == 16)
2283  {
2284  if ( axis == 7 ) //fingers abduction
2285  next_pos[axis] = fabs(limitsMax[axis] - limitsMax[axis]); //add explanation here
2286  else
2287  next_pos[axis] = limitsMax[axis];
2288  }
2289  else
2290  {
2291  next_pos[axis] = limitsMax[axis];
2292  }
2293  ref_positions[axis] = next_pos[axis];
2294  }
2295  else
2296  {
2297  ref_positions[axis] = ref;
2298  if (njoints == 16)
2299  {
2300  if ( axis == 10 || axis == 12 || axis == 14 )
2301  next_pos[axis] = ref/2;
2302  else if ( axis == 15 )
2303  next_pos[axis] = ref/3;
2304  else if ( axis == 7 )
2305  next_pos[axis] = fabs(limitsMax[axis] - ref);
2306  else
2307  next_pos[axis] = ref;
2308  }
2309  else
2310  {
2311  next_pos[axis] = ref;
2312  }
2313  }
2314 
2315  motor_on[axis]=true;
2316 
2317  if (verbosity)
2318  yDebug("moving joint %d of part %d to pos %f (pos direct)\n",axis, partSelec, next_pos[axis]);
2319  return true;
2320  }
2321  if (verbosity)
2322  yError("setPositionRaw joint access too high %d \n",axis);
2323  return false;
2324 }
2325 
2326 bool iCubSimulationControl::setPositionsRaw(const int n_joint, const int *joints, const double *refs)
2327 {
2328  bool ret = true;
2329  for(int i=0; i<n_joint; i++)
2330  {
2331  ret = ret && setPositionRaw(joints[i], refs[i]);
2332  }
2333  return ret;
2334 }
2335 
2337 {
2338  bool ret = true;
2339  for(int j=0; j<njoints; j++)
2340  {
2341  ret = ret && setPositionRaw(j, refs[j]);
2342  }
2343  return ret;
2344 }
2345 
2346 //PWM interface
2348 {
2349  if ((j >= 0) && (j<njoints))
2350  {
2351  lock_guard<mutex> lck(_mutex);
2352  pwm_ref[j] = v;
2353  return true;
2354  }
2355  if (verbosity)
2356  yError("setRefDutyCycleRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j, njoints);
2357  return false;
2358 }
2359 
2361 {
2362  lock_guard<mutex> lck(_mutex);
2363  for (int axis = 0; axis<njoints; axis++)
2364  pwm_ref[axis] = v[axis];
2365  return true;
2366 }
2367 
2369 {
2370  if ((j >= 0) && (j<njoints))
2371  {
2372  lock_guard<mutex> lck(_mutex);
2373  *v = pwm_ref[j];
2374  return true;
2375  }
2376  if (verbosity)
2377  yError("getRefDutyCycleRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j, njoints);
2378  return false;
2379 }
2380 
2382 {
2383  lock_guard<mutex> lck(_mutex);
2384  for (int axis = 0; axis<njoints; axis++)
2385  v[axis] = pwm_ref[axis];
2386  return true;
2387 }
2388 
2390 {
2391  if ((j >= 0) && (j<njoints))
2392  {
2393  lock_guard<mutex> lck(_mutex);
2394  *v = pwm[j];
2395  return true;
2396  }
2397  if (verbosity)
2398  yError("getDutyCycleRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j, njoints);
2399  return false;
2400 }
2401 
2403 {
2404  lock_guard<mutex> lck(_mutex);
2405  for (int axis = 0; axis<njoints; axis++)
2406  v[axis] = pwm[axis];
2407  return true;
2408 }
2409 
2410 // Current interface
2411 /*bool iCubSimulationControl::getCurrentRaw(int j, double *t)
2412 {
2413 return NOT_YET_IMPLEMENTED("getCurrentRaw");
2414 }
2415 
2416 bool iCubSimulationControl::getCurrentsRaw(double *t)
2417 {
2418 return NOT_YET_IMPLEMENTED("getCurrentsRaw");
2419 }
2420 */
2421 
2423 {
2424  if ((j >= 0) && (j<njoints))
2425  {
2426  lock_guard<mutex> lck(_mutex);
2427  *min = -3;
2428  *max = 3;
2429  return true;
2430  }
2431  if (verbosity)
2432  yError("setRefCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j, njoints);
2433  return false;
2434 }
2435 
2437 {
2438  lock_guard<mutex> lck(_mutex);
2439  for (int axis = 0; axis < njoints; axis++)
2440  {
2441  min[axis] = -3;
2442  max[axis] = 3;
2443  }
2444  return true;
2445 }
2446 
2448 {
2449  lock_guard<mutex> lck(_mutex);
2450  for (int axis = 0; axis<njoints; axis++)
2451  current_ampere_ref[axis] = t[axis];
2452  return true;
2453 }
2454 
2456 {
2457  if ((j >= 0) && (j<njoints))
2458  {
2459  lock_guard<mutex> lck(_mutex);
2460  current_ampere_ref[j] = t;
2461  return true;
2462  }
2463  if (verbosity)
2464  yError("setRefCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j, njoints);
2465  return false;
2466 }
2467 
2468 bool iCubSimulationControl::setRefCurrentsRaw(const int n_joint, const int *joints, const double *t)
2469 {
2470  bool ret = true;
2471  for (int j = 0; j<n_joint; j++)
2472  {
2473  ret = ret && setRefCurrentRaw(joints[j], t[j]);
2474  }
2475  return ret;
2476 }
2477 
2479 {
2480  lock_guard<mutex> lck(_mutex);
2481  for (int axis = 0; axis<njoints; axis++)
2482  t[axis] = current_ampere_ref[axis];
2483  return true;
2484 }
2485 
2487 {
2488  if ((j >= 0) && (j<njoints))
2489  {
2490  lock_guard<mutex> lck(_mutex);
2491  *t = current_ampere_ref[j];
2492  return true;
2493  }
2494  if (verbosity)
2495  yError("getRefCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j, njoints);
2496  return false;
2497 }
MODE_IMPEDANCE_VEL
#define MODE_IMPEDANCE_VEL
Definition: iCubSimulationControl.h:52
LogicalJoint::getTorque
virtual double getTorque()=0
this is just a fake torque interface for now
yarp::dev::iCubSimulationControl::getMotorEncodersRaw
virtual bool getMotorEncodersRaw(double *encs) override
Definition: iCubSimulationControl.cpp:1500
yarp::dev::iCubSimulationControl::stopRaw
virtual bool stopRaw() override
Definition: iCubSimulationControl.cpp:1176
yarp::dev::iCubSimulationControl::getAxisNameRaw
virtual bool getAxisNameRaw(int axis, std::string &name) override
Definition: iCubSimulationControl.cpp:1891
yarp::dev::iCubSimulationControl::getPeakCurrentRaw
virtual bool getPeakCurrentRaw(int m, double *val) override
Definition: iCubSimulationControl.cpp:861
LogicalJoint::getVelocity
virtual double getVelocity()=0
Get the angular velocity of an associated joint, in ICUB units and sign.
yarp::dev::iCubSimulationControl::setVelLimitsRaw
virtual bool setVelLimitsRaw(int axis, double min, double max) override
Definition: iCubSimulationControl.cpp:1826
yarp::dev::iCubSimulationControl::getEncoderSpeedsRaw
virtual bool getEncoderSpeedsRaw(double *spds) override
Definition: iCubSimulationControl.cpp:1440
python-motor-control.tmp
tmp
Definition: python-motor-control.py:43
yarp::dev::iCubSimulationControl::setPeakCurrentRaw
virtual bool setPeakCurrentRaw(int m, const double val) override
Definition: iCubSimulationControl.cpp:866
yarp::dev::iCubSimulationControl::getEncoderAccelerationsRaw
virtual bool getEncoderAccelerationsRaw(double *accs) override
Definition: iCubSimulationControl.cpp:1460
yarp::dev::iCubSimulationControl::torqueLimits
double * torqueLimits
Definition: iCubSimulationControl.h:466
iCub::ctrl::AWQuadEstimator
Definition: adaptWinPolyEstimator.h:207
yarp::dev::iCubSimulationControl::getPidsRaw
virtual bool getPidsRaw(const PidControlTypeEnum &pidtype, Pid *pids) override
Definition: iCubSimulationControl.cpp:641
yarp::dev::iCubSimulationControl::dutycycleToPwm
double * dutycycleToPwm
Definition: iCubSimulationControl.h:451
yarp::dev::iCubSimulationControl::setControlModesRaw
virtual bool setControlModesRaw(const int n_joint, const int *joints, int *modes) override
Definition: iCubSimulationControl.cpp:2177
yarp::dev::iCubSimulationControl::getDutyCycleRaw
virtual bool getDutyCycleRaw(int j, double *v) override
Definition: iCubSimulationControl.cpp:2389
yarp::dev::iCubSimulationControl::njoints
int njoints
Definition: iCubSimulationControl.h:434
yarp::dev::iCubSimulationControl::quadEstJnt
iCub::ctrl::AWQuadEstimator * quadEstJnt
Definition: iCubSimulationControl.h:411
yarp::dev::iCubSimulationControl::getJointTypeRaw
virtual bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
Definition: iCubSimulationControl.cpp:1905
yarp::dev::iCubSimulationControl::getTemperatureLimitRaw
virtual bool getTemperatureLimitRaw(int m, double *temp) override
Definition: iCubSimulationControl.cpp:851
yarp::dev::iCubSimulationControl::getRemoteVariablesListRaw
virtual bool getRemoteVariablesListRaw(yarp::os::Bottle *listOfKeys) override
Definition: iCubSimulationControl.cpp:1809
yarp::dev::iCubSimulationControl::ref_torques
double * ref_torques
Definition: iCubSimulationControl.h:427
yarp::dev::iCubSimulationControl::getRefVelocityRaw
virtual bool getRefVelocityRaw(const int joint, double *ref) override
Definition: iCubSimulationControl.cpp:1219
yarp::dev::iCubSimulationControl::getPowerSupplyVoltageRaw
virtual bool getPowerSupplyVoltageRaw(int j, double *val) override
Definition: iCubSimulationControl.cpp:896
yarp::dev::iCubSimulationControl::setPidErrorLimitsRaw
virtual bool setPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, const double *limits) override
Definition: iCubSimulationControl.cpp:707
yarp::dev::iCubSimulationControl::velLimitsMax
double * velLimitsMax
Definition: iCubSimulationControl.h:465
yarp::dev::iCubSimulationControl::getMotorEncoderCountsPerRevolutionRaw
virtual bool getMotorEncoderCountsPerRevolutionRaw(int m, double *cpr) override
Definition: iCubSimulationControl.cpp:1533
yarp::dev::iCubSimulationControl::zeros
double * zeros
Definition: iCubSimulationControl.h:448
yarp::dev::iCubSimulationControl::getTorqueRangeRaw
virtual bool getTorqueRangeRaw(int, double *, double *) override
Definition: iCubSimulationControl.cpp:1933
yarp::dev::iCubSimulationControl::getRefTorquesRaw
virtual bool getRefTorquesRaw(double *) override
Definition: iCubSimulationControl.cpp:1975
yarp::dev::iCubSimulationControl::getEncoderSpeedRaw
virtual bool getEncoderSpeedRaw(int j, double *sp) override
Definition: iCubSimulationControl.cpp:1448
yarp::dev::iCubSimulationControl::getMotorEncoderTimedRaw
virtual bool getMotorEncoderTimedRaw(int m, double *encs, double *stamp) override
Definition: iCubSimulationControl.cpp:1549
yarp::dev::iCubSimulationControl::getRefSpeedsRaw
virtual bool getRefSpeedsRaw(double *spds) override
Definition: iCubSimulationControl.cpp:1146
LogicalJoint
Abstract class for mapping from "physical" (simulated) joints in the model to control joints in the I...
Definition: LogicalJoint.h:33
e
e
Definition: compute_ekf_fast.m:13
MODE_TORQUE
#define MODE_TORQUE
Definition: iCubSimulationControl.h:50
OdeInit::verbosity
int verbosity
Definition: OdeInit.h:72
yarp::dev::iCubSimulationControl::setMaxCurrentRaw
virtual bool setMaxCurrentRaw(int j, double val) override
Definition: iCubSimulationControl.cpp:1633
yarp::dev::iCubSimulationControl::getMotorEncodersTimedRaw
virtual bool getMotorEncodersTimedRaw(double *encs, double *stamps) override
Definition: iCubSimulationControl.cpp:1539
yarp::dev::iCubSimulationControl::getNumberOfMotorEncodersRaw
virtual bool getNumberOfMotorEncodersRaw(int *num) override
Definition: iCubSimulationControl.cpp:1522
yarp::dev::iCubSimulationControl::setPidErrorLimitRaw
virtual bool setPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double limit) override
Definition: iCubSimulationControl.cpp:702
yarp::dev::iCubSimulationControl::gearbox
double * gearbox
Definition: iCubSimulationControl.h:469
yarp::dev::iCubSimulationControl::hasRotorEncoder
bool * hasRotorEncoder
Definition: iCubSimulationControl.h:475
yarp::dev::iCubSimulationControl::current_jnt_torques
double * current_jnt_torques
Definition: iCubSimulationControl.h:388
yarp::dev::iCubSimulationControl::ref_positions
double * ref_positions
Definition: iCubSimulationControl.h:418
yarp::dev::iCubSimulationControl::setRefDutyCyclesRaw
virtual bool setRefDutyCyclesRaw(const double *v) override
Definition: iCubSimulationControl.cpp:2360
out
out
Definition: sine.m:8
yarp::dev::iCubSimulationControl::setLimitsRaw
virtual bool setLimitsRaw(int axis, double min, double max) override
Definition: iCubSimulationControl.cpp:1695
yarp::dev::iCubSimulationControl::setPidReferenceRaw
virtual bool setPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double ref) override
cmd is a SingleAxis pointer with 1 double arg
Definition: iCubSimulationControl.cpp:660
yarp::dev::iCubSimulationControl::getPidReferencesRaw
virtual bool getPidReferencesRaw(const PidControlTypeEnum &pidtype, double *refs) override
Definition: iCubSimulationControl.cpp:934
yarp::dev::iCubSimulationControl::open
virtual bool open(yarp::os::Searchable &config)
Open the device driver and start communication with the hardware.
Definition: iCubSimulationControl.cpp:87
yarp::dev::iCubSimulationControl::enableAmpRaw
virtual bool enableAmpRaw(int j) override
Definition: iCubSimulationControl.cpp:1603
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
yarp::dev::iCubSimulationControl::calibrationDoneRaw
virtual bool calibrationDoneRaw(int j) override
Definition: iCubSimulationControl.cpp:1668
yarp::dev::iCubSimulationControl::input
int input
Definition: iCubSimulationControl.h:458
yarp::dev::iCubSimulationControl::~iCubSimulationControl
virtual ~iCubSimulationControl()
Destructor.
Definition: iCubSimulationControl.cpp:80
yarp::dev::iCubSimulationControl::joints
yarp::dev::PolyDriver joints
Definition: iCubSimulationControl.h:374
yarp::dev::iCubSimulationControl::linEstJnt
iCub::ctrl::AWLinEstimator * linEstJnt
Definition: iCubSimulationControl.h:410
yarp::dev::iCubSimulationControl::checkMotionDoneRaw
virtual bool checkMotionDoneRaw(bool *flag) override
Definition: iCubSimulationControl.cpp:1071
yarp::dev::iCubSimulationControl::enablePidRaw
virtual bool enablePidRaw(const PidControlTypeEnum &pidtype, int j) override
Definition: iCubSimulationControl.cpp:958
yarp::dev::iCubSimulationControl::getPidErrorsRaw
virtual bool getPidErrorsRaw(const PidControlTypeEnum &pidtype, double *errs) override
Definition: iCubSimulationControl.cpp:739
yarp::dev::iCubSimulationControl::setPositionsRaw
virtual bool setPositionsRaw(const int n_joint, const int *joints, const double *refs) override
Definition: iCubSimulationControl.cpp:2326
yarp::dev::iCubSimulationControl::vel
double vel
Definition: iCubSimulationControl.h:437
yarp::dev::iCubSimulationControl::jointStep
void jointStep()
Definition: iCubSimulationControl.cpp:476
yarp::dev::iCubSimulationControl::setPositionRaw
virtual bool setPositionRaw(int j, double ref) override
Definition: iCubSimulationControl.cpp:2252
yarp::dev::iCubSimulationControl::_mutex
std::mutex _mutex
Definition: iCubSimulationControl.h:377
yarp::dev::iCubSimulationControl::setMotorEncoderRaw
virtual bool setMotorEncoderRaw(int m, const double val) override
Definition: iCubSimulationControl.cpp:1480
MODE_CALIB_DONE
#define MODE_CALIB_DONE
Definition: iCubSimulationControl.h:59
yarp::dev::iCubSimulationControl::refSpeed
double * refSpeed
Definition: iCubSimulationControl.h:470
yarp::dev::iCubSimulationControl::inputs
int * inputs
Definition: iCubSimulationControl.h:459
yarp::dev::iCubSimulationControl::velLimitsMin
double * velLimitsMin
Definition: iCubSimulationControl.h:464
yarp::dev::iCubSimulationControl::getRefCurrentsRaw
virtual bool getRefCurrentsRaw(double *t) override
Definition: iCubSimulationControl.cpp:2478
yarp::dev::iCubSimulationControl::getMotorEncoderSpeedRaw
virtual bool getMotorEncoderSpeedRaw(int m, double *sp) override
Definition: iCubSimulationControl.cpp:1565
iCub::ctrl::AWPolyEstimator::estimate
yarp::sig::Vector estimate()
Execute the algorithm upon the elements list, with the max deviation threshold given by D.
Definition: adaptWinPolyEstimator.cpp:100
OdeInit::setSimulationControl
void setSimulationControl(iCubSimulationControl *control, int part)
Definition: OdeInit.cpp:125
yarp::dev::iCubSimulationControl::getCurrentsRaw
virtual bool getCurrentsRaw(double *vals) override
Definition: iCubSimulationControl.cpp:1609
yarp::dev::iCubSimulationControl::current_ampere_ref
double * current_ampere_ref
Definition: iCubSimulationControl.h:397
OdeInit::removeSimulationControl
void removeSimulationControl(int part)
Definition: OdeInit.cpp:182
yarp::dev::iCubSimulationControl::getPidOutputRaw
virtual bool getPidOutputRaw(const PidControlTypeEnum &pidtype, int j, double *out) override
Definition: iCubSimulationControl.cpp:749
yarp::dev::iCubSimulationControl::rotorIndexOffset
int * rotorIndexOffset
Definition: iCubSimulationControl.h:476
yarp::dev::iCubSimulationControl::linEstMot
iCub::ctrl::AWLinEstimator * linEstMot
Definition: iCubSimulationControl.h:412
yarp::dev::iCubSimulationControl::relativeMoveRaw
virtual bool relativeMoveRaw(int j, double delta) override
Definition: iCubSimulationControl.cpp:1058
yarp::dev::iCubSimulationControl::ErrorPos
double ErrorPos[100]
Definition: iCubSimulationControl.h:457
yarp::dev::iCubSimulationControl::controlMode
int * controlMode
Definition: iCubSimulationControl.h:479
MODE_NOT_CONFIGURED
#define MODE_NOT_CONFIGURED
Definition: iCubSimulationControl.h:60
yarp::dev::iCubSimulationControl::setPidsRaw
virtual bool setPidsRaw(const PidControlTypeEnum &pidtype, const Pid *pids) override
Definition: iCubSimulationControl.cpp:650
yarp::dev::iCubSimulationControl::torque_pid
Pid * torque_pid
Definition: iCubSimulationControl.h:483
MODE_UNKNOWN
#define MODE_UNKNOWN
Definition: iCubSimulationControl.h:62
yarp::dev::iCubSimulationControl::setRefSpeedRaw
virtual bool setRefSpeedRaw(int j, double sp) override
Definition: iCubSimulationControl.cpp:1103
yarp::dev
Definition: DebugInterfaces.h:52
MODE_CALIBRATING
#define MODE_CALIBRATING
Definition: iCubSimulationControl.h:58
yarp::dev::iCubSimulationControl::controlP
double * controlP
Definition: iCubSimulationControl.h:472
yarp::dev::iCubSimulationControl::setPWMLimitRaw
virtual bool setPWMLimitRaw(int j, const double val) override
Definition: iCubSimulationControl.cpp:891
p
p
Definition: show_eyes_axes.m:23
yarp::dev::iCubSimulationControl::getPidErrorLimitRaw
virtual bool getPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double *limit) override
Definition: iCubSimulationControl.cpp:943
yarp::dev::iCubSimulationControl::setControlModeRaw
virtual bool setControlModeRaw(const int j, const int mode) override
Definition: iCubSimulationControl.cpp:2148
yarp::dev::iCubSimulationControl::current_ampere
double * current_ampere
Definition: iCubSimulationControl.h:396
yarp::dev::iCubSimulationControl::getCurrentRangeRaw
virtual bool getCurrentRangeRaw(int j, double *min, double *max) override
Definition: iCubSimulationControl.cpp:2422
yarp::dev::iCubSimulationControl::current_mot_pos
double * current_mot_pos
Definition: iCubSimulationControl.h:385
OdeInit.h
This file is responsible for the initialisation of the world parameters that are controlled by ODE....
iCub::ctrl::AWPolyElement
Definition: adaptWinPolyEstimator.h:45
yarp::dev::iCubSimulationControl::velocity_pid
Pid * velocity_pid
Definition: iCubSimulationControl.h:485
yarp::dev::iCubSimulationControl::setPidOffsetRaw
virtual bool setPidOffsetRaw(const PidControlTypeEnum &pidtype, int j, double v) override
Definition: iCubSimulationControl.cpp:963
yarp::dev::iCubSimulationControl::setMotorEncoderCountsPerRevolutionRaw
virtual bool setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override
Definition: iCubSimulationControl.cpp:1528
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
yarp::dev::iCubSimulationControl::resetEncoderRaw
virtual bool resetEncoderRaw(int j) override
Definition: iCubSimulationControl.cpp:1375
yarp::dev::iCubSimulationControl::setRefAccelerationsRaw
virtual bool setRefAccelerationsRaw(const double *accs) override
Definition: iCubSimulationControl.cpp:1131
yarp::dev::iCubSimulationControl::close
virtual bool close(void)
Definition: iCubSimulationControl.cpp:351
yarp::dev::iCubSimulationControl::ref_speeds
double * ref_speeds
Definition: iCubSimulationControl.h:423
LogicalJoint::setTorque
virtual void setTorque(double target)=0
Set the reference torque.
MODE_HW_FAULT
#define MODE_HW_FAULT
Definition: iCubSimulationControl.h:57
yarp::dev::iCubSimulationControl::getTorquesRaw
virtual bool getTorquesRaw(double *) override
Definition: iCubSimulationControl.cpp:1926
yarp::dev::iCubSimulationControl::getRefAccelerationRaw
virtual bool getRefAccelerationRaw(int j, double *acc) override
Definition: iCubSimulationControl.cpp:1155
yarp::dev::iCubSimulationControl::setRefDutyCycleRaw
virtual bool setRefDutyCycleRaw(int j, double v) override
Definition: iCubSimulationControl.cpp:2347
MODE_CURRENT
#define MODE_CURRENT
Definition: iCubSimulationControl.h:54
yarp::dev::iCubSimulationControl::disableAmpRaw
virtual bool disableAmpRaw(int j) override
Definition: iCubSimulationControl.cpp:1598
yarp::dev::iCubSimulationControl::setNominalCurrentRaw
virtual bool setNominalCurrentRaw(int m, const double val) override
Definition: iCubSimulationControl.cpp:876
yarp::dev::iCubSimulationControl::newtonsToSensor
double * newtonsToSensor
encoder zeros
Definition: iCubSimulationControl.h:449
yarp::dev::iCubSimulationControl::setTemperatureLimitRaw
virtual bool setTemperatureLimitRaw(int m, const double temp) override
Definition: iCubSimulationControl.cpp:856
yarp::dev::iCubSimulationControl::getEncoderTimedRaw
virtual bool getEncoderTimedRaw(int j, double *encs, double *stamp)
Definition: iCubSimulationControl.cpp:1433
yarp::dev::iCubSimulationControl::getLimitsRaw
virtual bool getLimitsRaw(int axis, double *min, double *max) override
Definition: iCubSimulationControl.cpp:1708
OdeInit::get
static OdeInit & get()
Definition: OdeInit.cpp:189
MODE_IDLE
#define MODE_IDLE
Definition: iCubSimulationControl.h:47
mode
GLenum mode
Definition: rendering.cpp:48
yarp::dev::iCubSimulationControl::calibrateAxisWithParamsRaw
virtual bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override
Definition: iCubSimulationControl.cpp:1663
MODE_POSITION
#define MODE_POSITION
Definition: iCubSimulationControl.h:48
yarp::dev::iCubSimulationControl::estimated_jnt_vel
double * estimated_jnt_vel
Definition: iCubSimulationControl.h:402
yarp::dev::iCubSimulationControl::getNominalCurrentRaw
virtual bool getNominalCurrentRaw(int m, double *val) override
Definition: iCubSimulationControl.cpp:871
yarp::dev::iCubSimulationControl::getDutyCyclesRaw
virtual bool getDutyCyclesRaw(double *v) override
Definition: iCubSimulationControl.cpp:2402
yarp::dev::iCubSimulationControl::setEncoderRaw
virtual bool setEncoderRaw(int j, double val) override
Definition: iCubSimulationControl.cpp:1365
yarp::dev::iCubSimulationControl::next_pos
double * next_pos
Definition: iCubSimulationControl.h:416
yarp::dev::iCubSimulationControl::positionMoveRaw
virtual bool positionMoveRaw(int j, double ref) override
Definition: iCubSimulationControl.cpp:973
yarp::dev::iCubSimulationControl::setRefTorquesRaw
virtual bool setRefTorquesRaw(const double *) override
Definition: iCubSimulationControl.cpp:1941
yarp::dev::iCubSimulationControl::estimated_mot_acc
double * estimated_mot_acc
Definition: iCubSimulationControl.h:409
yarp::dev::iCubSimulationControl::getEncoderRaw
virtual bool getEncoderRaw(int j, double *v) override
Definition: iCubSimulationControl.cpp:1402
yarp::dev::iCubSimulationControl::getControlModesRaw
virtual bool getControlModesRaw(int *modes) override
Definition: iCubSimulationControl.cpp:2131
yarp::dev::iCubSimulationControl::getRefDutyCycleRaw
virtual bool getRefDutyCycleRaw(int j, double *v) override
Definition: iCubSimulationControl.cpp:2368
yarp::dev::iCubSimulationControl::getPWMRaw
virtual bool getPWMRaw(int j, double *val) override
Definition: iCubSimulationControl.cpp:881
yarp::dev::iCubSimulationControl::motor_torque_params
MotorTorqueParameters * motor_torque_params
Definition: iCubSimulationControl.h:486
yarp::dev::iCubSimulationControl::setEncodersRaw
virtual bool setEncodersRaw(const double *vals) override
Definition: iCubSimulationControl.cpp:1370
yarp::dev::iCubSimulationControl::setInteractionModeRaw
virtual bool setInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum mode) override
Definition: iCubSimulationControl.cpp:2224
yarp::dev::iCubSimulationControl::getRefCurrentRaw
virtual bool getRefCurrentRaw(int j, double *t) override
Definition: iCubSimulationControl.cpp:2486
yarp::dev::iCubSimulationControl::ampsToSensor
double * ampsToSensor
Definition: iCubSimulationControl.h:450
LogicalJoints::control
virtual LogicalJoint & control(int part, int axis)=0
Access the control for a logical joint, based on part and axis number.
yarp::dev::iCubSimulationControl::getAmpStatusRaw
virtual bool getAmpStatusRaw(int *st) override
Definition: iCubSimulationControl.cpp:1674
NOT_YET_IMPLEMENTED
static bool NOT_YET_IMPLEMENTED(const char *txt)
Definition: iCubSimulationControl.cpp:40
yarp::dev::iCubSimulationControl::quadEstMot
iCub::ctrl::AWQuadEstimator * quadEstMot
Definition: iCubSimulationControl.h:413
yarp::dev::iCubSimulationControl::getInteractionModeRaw
virtual bool getInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum *mode) override
Definition: iCubSimulationControl.cpp:2198
yarp::dev::iCubSimulationControl::getRefTorqueRaw
virtual bool getRefTorqueRaw(int, double *) override
Definition: iCubSimulationControl.cpp:1984
yarp::dev::iCubSimulationControl::getPidOutputsRaw
virtual bool getPidOutputsRaw(const PidControlTypeEnum &pidtype, double *outs) override
Definition: iCubSimulationControl.cpp:822
yarp::dev::iCubSimulationControl::verbosity
int verbosity
Definition: iCubSimulationControl.h:366
yarp::dev::iCubSimulationControl::setRefCurrentRaw
virtual bool setRefCurrentRaw(int j, double t) override
Definition: iCubSimulationControl.cpp:2455
yarp::dev::iCubSimulationControl::getTorqueRaw
virtual bool getTorqueRaw(int, double *) override
Definition: iCubSimulationControl.cpp:1917
yarp::dev::iCubSimulationControl::getPidErrorLimitsRaw
virtual bool getPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, double *limits) override
Definition: iCubSimulationControl.cpp:948
yarp::dev::iCubSimulationControl::next_vel
double * next_vel
Definition: iCubSimulationControl.h:421
jnts
int jnts
Definition: main.cpp:60
yarp::dev::iCubSimulationControl::_opened
bool _opened
Definition: iCubSimulationControl.h:381
LogicalJoint::setVelocity
virtual void setVelocity(double target)=0
Set velocity of joint (in ICUB units/sign).
yarp::dev::iCubSimulationControl::getMotorEncoderSpeedsRaw
virtual bool getMotorEncoderSpeedsRaw(double *spds) override
Definition: iCubSimulationControl.cpp:1556
yarp::dev::iCubSimulationControl::getTemperaturesRaw
virtual bool getTemperaturesRaw(double *vals) override
Definition: iCubSimulationControl.cpp:846
LogicalJoint::setControlParameters
virtual void setControlParameters(double vel, double acc)=0
Set velocity and acceleration control parameters.
yarp::dev::iCubSimulationControl::pwm_ref
double * pwm_ref
Definition: iCubSimulationControl.h:393
yarp::dev::iCubSimulationControl::getTemperatureRaw
virtual bool getTemperatureRaw(int m, double *val) override
Definition: iCubSimulationControl.cpp:841
yarp::dev::iCubSimulationControl::hasTempSensor
bool * hasTempSensor
Definition: iCubSimulationControl.h:474
yarp::dev::iCubSimulationControl::getEncodersRaw
virtual bool getEncodersRaw(double *encs) override
Definition: iCubSimulationControl.cpp:1385
MODE_VELOCITY
#define MODE_VELOCITY
Definition: iCubSimulationControl.h:49
MODE_PWM
#define MODE_PWM
Definition: iCubSimulationControl.h:53
yarp::dev::iCubSimulationControl::setRefCurrentsRaw
virtual bool setRefCurrentsRaw(const double *t) override
Definition: iCubSimulationControl.cpp:2447
yarp::dev::iCubSimulationControl::vels
double * vels
Definition: iCubSimulationControl.h:460
yarp::dev::iCubSimulationControl::getTargetPositionRaw
virtual bool getTargetPositionRaw(const int joint, double *ref) override
Definition: iCubSimulationControl.cpp:1187
yarp::dev::iCubSimulationControl::position_pid
Pid * position_pid
Definition: iCubSimulationControl.h:482
yarp::dev::iCubSimulationControl::getControlModeRaw
virtual bool getControlModeRaw(int j, int *mode) override
Definition: iCubSimulationControl.cpp:2119
MODE_CONFIGURED
#define MODE_CONFIGURED
Definition: iCubSimulationControl.h:61
yarp::dev::iCubSimulationControl::estimated_mot_vel
double * estimated_mot_vel
Definition: iCubSimulationControl.h:403
yarp::dev::iCubSimulationControl::getPidRaw
virtual bool getPidRaw(const PidControlTypeEnum &pidtype, int j, Pid *pid) override
Definition: iCubSimulationControl.cpp:616
yarp::dev::iCubSimulationControl::setPidReferencesRaw
virtual bool setPidReferencesRaw(const PidControlTypeEnum &pidtype, const double *refs) override
Definition: iCubSimulationControl.cpp:692
done
bool done
Definition: main.cpp:42
yarp::dev::iCubSimulationControl::setMotorEncodersRaw
virtual bool setMotorEncodersRaw(const double *vals) override
Definition: iCubSimulationControl.cpp:1485
yarp::dev::iCubSimulationControl::resetPidRaw
virtual bool resetPidRaw(const PidControlTypeEnum &pidtype, int j) override
Definition: iCubSimulationControl.cpp:953
yarp::dev::iCubSimulationControl::getRefSpeedRaw
virtual bool getRefSpeedRaw(int j, double *ref) override
Definition: iCubSimulationControl.cpp:1135
yarp::dev::iCubSimulationControl::getRefDutyCyclesRaw
virtual bool getRefDutyCyclesRaw(double *v) override
Definition: iCubSimulationControl.cpp:2381
yarp::dev::iCubSimulationControl::current_jnt_vel
double * current_jnt_vel
Definition: iCubSimulationControl.h:400
yarp::dev::iCubSimulationControl::getCurrentRaw
virtual bool getCurrentRaw(int j, double *val) override
Definition: iCubSimulationControl.cpp:1618
yarp::dev::iCubSimulationControl::getEncodersTimedRaw
virtual bool getEncodersTimedRaw(double *encs, double *stamps)
Definition: iCubSimulationControl.cpp:1423
yarp::dev::iCubSimulationControl::setRefTorqueRaw
virtual bool setRefTorqueRaw(int, double) override
Definition: iCubSimulationControl.cpp:1951
yarp::dev::iCubSimulationControl::setPidRaw
virtual bool setPidRaw(const PidControlTypeEnum &pidtype, int j, const Pid &pid) override
Definition: iCubSimulationControl.cpp:591
yarp::dev::iCubSimulationControl::pwm
double * pwm
Definition: iCubSimulationControl.h:392
yarp::dev::iCubSimulationControl::getRefVelocitiesRaw
virtual bool getRefVelocitiesRaw(double *refs) override
Definition: iCubSimulationControl.cpp:1226
yarp::dev::iCubSimulationControl::getPidErrorRaw
virtual bool getPidErrorRaw(const PidControlTypeEnum &pidtype, int j, double *err) override
Definition: iCubSimulationControl.cpp:712
yarp::dev::iCubSimulationControl::partSelec
int partSelec
Definition: iCubSimulationControl.h:432
v
static int v
Definition: iCub_Sim.cpp:42
yarp::dev::iCubSimulationControl::getNumberOfMotorsRaw
virtual bool getNumberOfMotorsRaw(int *m) override
IMotor.
Definition: iCubSimulationControl.cpp:835
yarp::dev::iCubSimulationControl::interactionMode
int * interactionMode
Definition: iCubSimulationControl.h:480
yarp::dev::iCubSimulationControl::getAxes
virtual bool getAxes(int *ax) override
POSITION CONTROL INTERFACE RAW.
Definition: iCubSimulationControl.cpp:585
yarp::dev::iCubSimulationControl::current_pid
Pid * current_pid
Definition: iCubSimulationControl.h:484
iCub::ctrl::AWLinEstimator
Definition: adaptWinPolyEstimator.h:184
yarp::dev::iCubSimulationControl::resetMotorEncodersRaw
virtual bool resetMotorEncodersRaw() override
Definition: iCubSimulationControl.cpp:1495
yarp::dev::iCubSimulationControl::disablePidRaw
virtual bool disablePidRaw(const PidControlTypeEnum &pidtype, int j) override
Definition: iCubSimulationControl.cpp:968
yarp::dev::iCubSimulationControl::getPWMLimitRaw
virtual bool getPWMLimitRaw(int j, double *val) override
Definition: iCubSimulationControl.cpp:886
yarp::dev::iCubSimulationControl::estimated_jnt_acc
double * estimated_jnt_acc
Definition: iCubSimulationControl.h:408
yarp::dev::iCubSimulationControl::error_tol
double * error_tol
Definition: iCubSimulationControl.h:453
yarp::dev::iCubSimulationControl::setRefSpeedsRaw
virtual bool setRefSpeedsRaw(const double *spds) override
Definition: iCubSimulationControl.cpp:1118
python-motor-control.encs
encs
Definition: python-motor-control.py:36
yarp::dev::iCubSimulationControl::refAccel
double * refAccel
Definition: iCubSimulationControl.h:471
yarp::dev::iCubSimulationControl::getEncoderAccelerationRaw
virtual bool getEncoderAccelerationRaw(int j, double *spds) override
Definition: iCubSimulationControl.cpp:1468
yarp::dev::iCubSimulationControl::getInteractionModesRaw
virtual bool getInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
Definition: iCubSimulationControl.cpp:2204
yarp::dev::iCubSimulationControl::getVelLimitsRaw
virtual bool getVelLimitsRaw(int axis, double *min, double *max) override
Definition: iCubSimulationControl.cpp:1839
yarp::dev::iCubSimulationControl::motor_on
bool * motor_on
Definition: iCubSimulationControl.h:455
yarp::dev::iCubSimulationControl::setRefAccelerationRaw
virtual bool setRefAccelerationRaw(int j, double acc) override
Definition: iCubSimulationControl.cpp:1127
yarp::dev::iCubSimulationControl::current_mot_acc
double * current_mot_acc
Definition: iCubSimulationControl.h:407
acc
_3f_vect_t acc
Definition: dataTypes.h:3
yarp::dev::iCubSimulationControl::getTorqueRangesRaw
virtual bool getTorqueRangesRaw(double *, double *) override
Definition: iCubSimulationControl.cpp:1937
yarp::dev::iCubSimulationControl::setInteractionModesRaw
virtual bool setInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
Definition: iCubSimulationControl.cpp:2231
yarp::dev::iCubSimulationControl::getMotorEncoderAccelerationsRaw
virtual bool getMotorEncoderAccelerationsRaw(double *accs) override
Definition: iCubSimulationControl.cpp:1577
yarp::dev::iCubSimulationControl::setMotorTorqueParamsRaw
virtual bool setMotorTorqueParamsRaw(int j, const MotorTorqueParameters params) override
Definition: iCubSimulationControl.cpp:2009
MODE_IMPEDANCE_POS
#define MODE_IMPEDANCE_POS
Definition: iCubSimulationControl.h:51
yarp::dev::iCubSimulationControl::getMotorEncoderRaw
virtual bool getMotorEncoderRaw(int m, double *v) override
Definition: iCubSimulationControl.cpp:1509
yarp::dev::iCubSimulationControl::getCurrentRangesRaw
virtual bool getCurrentRangesRaw(double *min, double *max) override
Definition: iCubSimulationControl.cpp:2436
yarp::dev::iCubSimulationControl::limitsMin
double * limitsMin
Definition: iCubSimulationControl.h:462
yarp::dev::iCubSimulationControl::velocityMoveRaw
virtual bool velocityMoveRaw(int j, double sp) override
for each axis
Definition: iCubSimulationControl.cpp:1330
yarp::dev::iCubSimulationControl::limitsMax
double * limitsMax
Definition: iCubSimulationControl.h:463
yarp::dev::iCubSimulationControl::current_jnt_acc
double * current_jnt_acc
Definition: iCubSimulationControl.h:406
yarp::dev::iCubSimulationControl::getRefPositionRaw
virtual bool getRefPositionRaw(const int joint, double *ref) override
Definition: iCubSimulationControl.cpp:1246
yarp::dev::iCubSimulationControl::axisMap
int * axisMap
Definition: iCubSimulationControl.h:444
M_PI
#define M_PI
Definition: XSensMTx.cpp:24
yarp::dev::iCubSimulationControl::angleToEncoder
double * angleToEncoder
Definition: iCubSimulationControl.h:446
yarp::dev::iCubSimulationControl::getRefPositionsRaw
virtual bool getRefPositionsRaw(double *refs) override
Definition: iCubSimulationControl.cpp:1253
yarp::dev::iCubSimulationControl::current_jnt_pos
double * current_jnt_pos
Definition: iCubSimulationControl.h:384
LogicalJoint::isValid
virtual bool isValid()=0
iCubSimulationControl.h
This is the header file for the yarp interface of the iCubSimulation.
yarp::dev::iCubSimulationControl::current_mot_vel
double * current_mot_vel
Definition: iCubSimulationControl.h:401
yarp::dev::iCubSimulationControl::ref_command_speeds
double * ref_command_speeds
Definition: iCubSimulationControl.h:422
MODE_MIXED
#define MODE_MIXED
Definition: iCubSimulationControl.h:55
yarp::dev::iCubSimulationControl::manager
LogicalJoints * manager
Definition: iCubSimulationControl.h:375
LogicalJoint::getAngle
virtual double getAngle()=0
Get the angle of an associated joint, in ICUB units and sign.
LogicalJoint::setPosition
virtual void setPosition(double target)=0
Drive towards an angle setpoint (in ICUB units/sign).
yarp::dev::iCubSimulationControl::isPidEnabledRaw
virtual bool isPidEnabledRaw(const PidControlTypeEnum &pidtype, int j, bool *enabled) override
Definition: iCubSimulationControl.cpp:830
yarp::dev::iCubSimulationControl::resetMotorEncoderRaw
virtual bool resetMotorEncoderRaw(int m) override
Definition: iCubSimulationControl.cpp:1490
yarp::dev::iCubSimulationControl::getMotorTorqueParamsRaw
virtual bool getMotorTorqueParamsRaw(int j, MotorTorqueParameters *params) override
Definition: iCubSimulationControl.cpp:1996
yarp::dev::iCubSimulationControl::getRemoteVariableRaw
virtual bool getRemoteVariableRaw(std::string key, yarp::os::Bottle &val) override
Definition: iCubSimulationControl.cpp:1721
OdeInit::mtx
std::mutex mtx
Definition: OdeInit.h:65
yarp::dev::iCubSimulationControl::getRefAccelerationsRaw
virtual bool getRefAccelerationsRaw(double *accs) override
Definition: iCubSimulationControl.cpp:1159
yarp::dev::iCubSimulationControl::getTargetPositionsRaw
virtual bool getTargetPositionsRaw(double *refs) override
Definition: iCubSimulationControl.cpp:1199
idx
static uint32_t idx[BOARD_NUM]
Definition: debugFunctions.cpp:34
yarp::dev::iCubSimulationControl::resetEncodersRaw
virtual bool resetEncodersRaw() override
Definition: iCubSimulationControl.cpp:1380
yarp::dev::iCubSimulationControl::getMaxCurrentRaw
virtual bool getMaxCurrentRaw(int j, double *val) override
Definition: iCubSimulationControl.cpp:1648
yarp::dev::iCubSimulationControl::getPidReferenceRaw
virtual bool getPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double *ref) override
Definition: iCubSimulationControl.cpp:901
yarp::dev::iCubSimulationControl::hasHallSensor
bool * hasHallSensor
Definition: iCubSimulationControl.h:473
yarp::dev::iCubSimulationControl::current_mot_torques
double * current_mot_torques
Definition: iCubSimulationControl.h:389
yarp::dev::iCubSimulationControl::next_torques
double * next_torques
Definition: iCubSimulationControl.h:426
yarp::dev::iCubSimulationControl::ref_command_positions
double * ref_command_positions
Definition: iCubSimulationControl.h:417
yarp::dev::iCubSimulationControl::kinematic_mj
yarp::sig::Matrix kinematic_mj
Definition: iCubSimulationControl.h:487
yarp::dev::iCubSimulationControl::motorPoles
int * motorPoles
Definition: iCubSimulationControl.h:477
DEPRECATED
static bool DEPRECATED(const char *txt)
Definition: iCubSimulationControl.cpp:47
OdeInit
ODE state information.
Definition: OdeInit.h:55
yarp::dev::iCubSimulationControl::setRemoteVariableRaw
virtual bool setRemoteVariableRaw(std::string key, const yarp::os::Bottle &val) override
Definition: iCubSimulationControl.cpp:1776
yarp::dev::iCubSimulationControl::maxCurrent
double * maxCurrent
Definition: iCubSimulationControl.h:467
yarp::dev::iCubSimulationControl::getMotorEncoderAccelerationRaw
virtual bool getMotorEncoderAccelerationRaw(int m, double *spds) override
Definition: iCubSimulationControl.cpp:1586
yarp::dev::iCubSimulationControl::rotToEncoder
double * rotToEncoder
Definition: iCubSimulationControl.h:468