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