iCub-main
embObjMotionControl.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) 2012 iCub Facility, Istituto Italiano di Tecnologia
5 * Authors: Alberto Cardellino
6 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
7 *
8 */
9 
10 //#include <yarp/dev/CanBusInterface.h>
11 #include <yarp/os/Bottle.h>
12 #include <yarp/os/Time.h>
13 #include <string.h>
14 #include <iostream>
15 
16 #include "embObjMotionControl.h"
17 #include <ethManager.h>
18 #include <FeatureInterface.h>
19 #include <yarp/conf/environment.h>
20 
21 #include <yarp/os/LogStream.h>
22 
23 #include "EoCommon.h"
24 #include "EOarray.h"
25 #include "EoProtocol.h"
26 #include "EoProtocolMN.h"
27 #include "EoProtocolMC.h"
28 #include "EoProtocolAS.h"
30 
31 #include <yarp/os/NetType.h>
32 #include <yarp/dev/ControlBoardHelper.h>
33 
34 
35 #include "eomcUtils.h"
36 
37 using namespace yarp::dev;
38 using namespace yarp::os;
39 using namespace yarp::os::impl;
40 
41 
42 
43 using namespace yarp::dev::eomc;
44 
45 
46 // macros
47 #define ASK_REFERENCE_TO_FIRMWARE 1
48 
49 #define PARSER_MOTION_CONTROL_VERSION 6
50 
51 
52 
53 
54 
55 static inline bool NOT_YET_IMPLEMENTED(const char *txt)
56 {
57  yError() << txt << " is not yet implemented for embObjMotionControl";
58  return true;
59 }
60 
61 static inline bool DEPRECATED(const char *txt)
62 {
63  yError() << txt << " has been deprecated for embObjMotionControl";
64  return true;
65 }
66 
67 #define NV_NOT_FOUND return nv_not_found();
68 
69 static bool nv_not_found(void)
70 {
71  yError () << " nv_not_found!! This may mean that this variable is not handled by this EMS\n";
72  return false;
73 }
74 
75 
76 
77 
78 
79 
80 
81 
82 
83 std::string embObjMotionControl::getBoardInfo(void)
84 {
85  if(nullptr == res)
86  {
87  return " BOARD name_unknown (IP unknown) ";
88  }
89  else
90  {
91  return ("BOARD " + res->getProperties().boardnameString + " (IP " + res->getProperties().ipv4addrString + ") ");
92  }
93 }
94 
95 
96 bool embObjMotionControl::alloc(int nj)
97 {
98  _axisMap = allocAndCheck<int>(nj);
99 
100  _encodersStamp = allocAndCheck<double>(nj);
101  _gearbox_M2J = allocAndCheck<double>(nj);
102  _gearbox_E2J = allocAndCheck<double>(nj);
103  _deadzone = allocAndCheck<double>(nj);
104  _foc_based_info=allocAndCheck<eomc::focBasedSpecificInfo_t>(nj);
105  _trj_pids= new eomc::PidInfo[nj];
106  //_dir_pids= new eomc::PidInfo[nj];
107  _trq_pids= new eomc::TrqPidInfo [nj];
108  _cur_pids= new eomc::PidInfo[nj];
109  _spd_pids= new eomc::PidInfo[nj];
110  _impedance_limits=allocAndCheck<eomc::impedanceLimits_t>(nj);
111  checking_motiondone=allocAndCheck<bool>(nj);
112  _last_position_move_time=allocAndCheck<double>(nj);
113 
114  // Reserve space for data stored locally. values are initialize to 0
115  _ref_command_positions = allocAndCheck<double>(nj);
116  _ref_positions = allocAndCheck<double>(nj);
117  _ref_command_speeds = allocAndCheck<double>(nj);
118  _ref_speeds = allocAndCheck<double>(nj);
119  _ref_accs = allocAndCheck<double>(nj);
120 
121  _enabledAmp = allocAndCheck<bool>(nj);
122  _enabledPid = allocAndCheck<bool>(nj);
123  _calibrated = allocAndCheck<bool>(nj);
124  _cacheImpedance = allocAndCheck<eOmc_impedance_t>(nj);
125 
126 
127  _rotorsLimits.resize(nj);
128  _jointsLimits.resize(nj);
129  _currentLimits.resize(nj);
130  _jsets.resize(nj);
131  _joint2set.resize(nj);
132  _timeouts.resize(nj);
133  _impedance_params.resize(nj);
134  _axesInfo.resize(nj);
135  _jointEncs.resize(nj);
136  _motorEncs.resize(nj);
137  _kalman_params.resize(nj);
138 
139  //debug purpose
140 
141  return true;
142 }
143 
144 bool embObjMotionControl::dealloc()
145 {
146  checkAndDestroy(_axisMap);
147  checkAndDestroy(_encodersStamp);
148  checkAndDestroy(_gearbox_M2J);
149  checkAndDestroy(_gearbox_E2J);
150  checkAndDestroy(_deadzone);
151  checkAndDestroy(_impedance_limits);
152  checkAndDestroy(checking_motiondone);
153  checkAndDestroy(_ref_command_positions);
154  checkAndDestroy(_ref_positions);
155  checkAndDestroy(_ref_command_speeds);
156  checkAndDestroy(_ref_speeds);
157  checkAndDestroy(_ref_accs);
158 
159  checkAndDestroy(_enabledAmp);
160  checkAndDestroy(_enabledPid);
161  checkAndDestroy(_calibrated);
162  checkAndDestroy(_foc_based_info);
163 
164 
165  if(_trj_pids)
166  delete [] _trj_pids;
167 
168  //if(_dir_pids)
169  // delete [] _dir_pids;
170 
171  if(_trq_pids)
172  delete [] _trq_pids;
173 
174  if(_cur_pids)
175  delete [] _cur_pids;
176 
177  if (_spd_pids)
178  delete[] _spd_pids;
179 
180 
181  return true;
182 }
183 
185  ImplementControlCalibration(this),
186  ImplementAmplifierControl(this),
187  ImplementPidControl(this),
188  ImplementEncodersTimed(this),
189  ImplementPositionControl(this),
190  ImplementVelocityControl(this),
191  ImplementControlMode(this),
192  ImplementImpedanceControl(this),
193  ImplementMotorEncoders(this),
194 #ifdef IMPLEMENT_DEBUG_INTERFACE
196 #endif
197  ImplementTorqueControl(this),
198  ImplementControlLimits(this),
199  ImplementPositionDirect(this),
200  ImplementInteractionMode(this),
201  ImplementMotor(this),
202  ImplementRemoteVariables(this),
203  ImplementAxisInfo(this),
204  ImplementPWMControl(this),
205  ImplementCurrentControl(this),
206  ImplementJointFault(this),
207  SAFETY_THRESHOLD(2.0),
208  _rotorsLimits(0),
209  _jointsLimits(0),
210  _currentLimits(0),
211  _jsets(0),
212  _joint2set(0),
213  _timeouts(0),
214  _impedance_params(0),
215  _axesInfo(0),
216  _jointEncs(0),
217  _motorEncs(0),
218  _kalman_params(0)
219 {
220  _gearbox_M2J = 0;
221  _gearbox_E2J = 0;
222  _deadzone = 0;
223  opened = 0;
224  _trj_pids = NULL;
225  //_dir_pids = NULL;
226  _trq_pids = NULL;
227  _cur_pids = NULL;
228  _spd_pids = NULL;
229  res = NULL;
230  _njoints = 0;
231  _axisMap = NULL;
232  _encodersStamp = NULL;
233  _foc_based_info = NULL;
234  _cacheImpedance = NULL;
235  _impedance_limits = NULL;
236  _ref_accs = NULL;
237  _ref_command_speeds = NULL;
238  _ref_command_positions= NULL;
239  _ref_positions = NULL;
240  _ref_speeds = NULL;
241  _measureConverter = NULL;
242 
243  checking_motiondone = NULL;
244  // debug connection
245  //tot_packet_recv = 0;
246  //errors = 0;
247  //start = 0;
248  //end = 0;
249 
250  // Check status of joints
251  _enabledPid = NULL;
252  _enabledAmp = NULL;
253  _calibrated = NULL;
254  _last_position_move_time = NULL;
255 
256  behFlags.useRawEncoderData = false;
257  behFlags.pwmIsLimited = false;
258 
259  std::string tmp = yarp::conf::environment::get_string("ETH_VERBOSEWHENOK");
260  if (tmp != "")
261  {
262  behFlags.verbosewhenok = (bool)yarp::conf::numeric::from_string(tmp, 0U);
263  }
264  else
265  {
266  behFlags.verbosewhenok = false;
267  }
268  parser = NULL;
269  _mcparser = NULL;
270 
271 #ifdef NETWORK_PERFORMANCE_BENCHMARK
272  /* We would like to verify if the round trimp of request and answer from embedded board is about 3 milliseconds, with a tollerance 0f 0.250 milliseconds.
273  The m_responseTimingVerifier object, after 3 seconds, prints an istogram with values from 1 to 10 millisec with a step of 0.5 millisec
274  */
275  m_responseTimingVerifier.init(0.003, 0.00025, 0.001, 0.01, 0.0005, 30);
276 #endif
277 
278 }
279 
280 
282 {
283  yTrace() << "embObjMotionControl::~embObjMotionControl()";
284 
285  if(NULL != parser)
286  {
287  delete parser;
288  parser = NULL;
289  }
290 
291  if(NULL != _mcparser)
292  {
293  delete _mcparser;
294  _mcparser = NULL;
295  }
296 
297  dealloc();
298 }
299 
300 
302 {
303  return opened;
304 }
305 
306 bool embObjMotionControl::initializeInterfaces(measureConvFactors &f)
307 {
308 
309  ImplementControlCalibration::initialize(_njoints, _axisMap, f.angleToEncoder, NULL);
310  ImplementAmplifierControl::initialize(_njoints, _axisMap, f.angleToEncoder, NULL,f.ampsToSensor);
311  ImplementEncodersTimed::initialize(_njoints, _axisMap, f.angleToEncoder, NULL);
312  ImplementMotorEncoders::initialize(_njoints, _axisMap, f.angleToEncoder, NULL);
313  ImplementPositionControl::initialize(_njoints, _axisMap, f.angleToEncoder, NULL);
314  ImplementPidControl::initialize(_njoints, _axisMap, f.angleToEncoder, NULL, f.newtonsToSensor, f.ampsToSensor, f.dutycycleToPWM);
315  ImplementControlMode::initialize(_njoints, _axisMap);
316  ImplementVelocityControl::initialize(_njoints, _axisMap, f.angleToEncoder, NULL);
317  ImplementControlLimits::initialize(_njoints, _axisMap, f.angleToEncoder, NULL);
318  ImplementImpedanceControl::initialize(_njoints, _axisMap, f.angleToEncoder, NULL, f.newtonsToSensor);
319  ImplementTorqueControl::initialize(_njoints, _axisMap, f.angleToEncoder, NULL, f.newtonsToSensor, f.ampsToSensor, f.dutycycleToPWM, f.bemf2raw, f.ktau2raw);
320  ImplementPositionDirect::initialize(_njoints, _axisMap, f.angleToEncoder, NULL);
321  ImplementInteractionMode::initialize(_njoints, _axisMap, f.angleToEncoder, NULL);
322  ImplementMotor::initialize(_njoints, _axisMap);
323  ImplementRemoteVariables::initialize(_njoints, _axisMap);
324  ImplementAxisInfo::initialize(_njoints, _axisMap);
325  ImplementCurrentControl::initialize(_njoints, _axisMap, f.ampsToSensor);
326  ImplementPWMControl::initialize(_njoints, _axisMap, f.dutycycleToPWM);
327  ImplementJointFault::initialize(_njoints, _axisMap);
328 
329  return true;
330 
331 }
332 
333 bool embObjMotionControl::open(yarp::os::Searchable &config)
334 {
335  // - first thing to do is verify if the eth manager is available. then i parse info about the eth board.
336 
337  ethManager = eth::TheEthManager::instance();
338  if(NULL == ethManager)
339  {
340  yFatal() << "embObjMotionControl::open() fails to instantiate ethManager";
341  return false;
342  }
343 
344  eOipv4addr_t ipv4addr;
345  string boardIPstring;
346  string boardName;
347  if(false == ethManager->verifyEthBoardInfo(config, ipv4addr, boardIPstring, boardName))
348  {
349  yError() << "embObjMotionControl::open(): object TheEthManager fails in parsing ETH propertiex from xml file";
350  return false;
351  }
352  // add specific info about this device ...
353 
354 
355  if(NULL == parser)
356  {
357  parser = new ServiceParser;
358  }
359 
360 
361  // - now all other things
362 
363  // -- instantiate EthResource etc.
364 
365  res = ethManager->requestResource2(this, config);
366  if(NULL == res)
367  {
368  yError() << "embObjMotionControl::open() fails because could not instantiate the ethResource for " << getBoardInfo() << " ... unable to continue";
369  return false;
370  }
371  // READ CONFIGURATION
372  if(!fromConfig(config))
373  {
374  yError() << getBoardInfo() << "Missing motion control parameters in config file";
375  return false;
376  }
377 
378  if(!res->verifyEPprotocol(eoprot_endpoint_motioncontrol))
379  {
380  yError() << "embObjMotionControl: failed verifyEPprotocol. Cannot continue!";
381  cleanup();
382  return false;
383  }
384 
385 
386  const eOmn_serv_parameter_t* servparam = &serviceConfig.ethservice;
387  if(eomn_serv_MC_generic == serviceConfig.ethservice.configuration.type)
388  {
389  servparam = NULL;
390  }
391 
392  // in here ...we open ports where to print AMO data
393  mcdiagnostics.config.mode = serviceConfig.ethservice.configuration.diagnosticsmode;
394  mcdiagnostics.config.par16 = serviceConfig.ethservice.configuration.diagnosticsparam;
395  if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.config.mode)
396  {
397  // prepare the ports
398  mcdiagnostics.ports.resize(2);
399  for(size_t i=0; i<mcdiagnostics.ports.size(); i++)
400  {
401  mcdiagnostics.ports[i] = new BufferedPort<Bottle>;
402  mcdiagnostics.ports[i]->open("/amo/" + res->getProperties().boardnameString + "/j" + std::to_string(i));
403  }
404  }
405 
406  // Initialize the downsampler timer
407 
408  event_downsampler = new mced::mcEventDownsampler();
409  event_downsampler->config.period = 0.01;
410  event_downsampler->config.threshold = 5;
411  event_downsampler->config.subcomponent = "[mc.skipped-cmd-wrong-mode]";
412  event_downsampler->config.info = getBoardInfo();
413  event_downsampler->start();
414 
415  if(false == res->serviceVerifyActivate(eomn_serv_category_mc, servparam))
416  {
417  yError() << "embObjMotionControl::open() has an error in call of ethResources::serviceVerifyActivate() for" << getBoardInfo();
418  cleanup();
419  return false;
420  }
421 
422  yDebug() << "embObjMotionControl:serviceVerifyActivate OK!";
423 
424 
425  if(!init() )
426  {
427  yError() << "embObjMotionControl::open() has an error in call of embObjMotionControl::init() for" << getBoardInfo();
428  return false;
429  }
430  else
431  {
432  if(behFlags.verbosewhenok)
433  {
434  yDebug() << "embObjMotionControl::init() has succesfully initted" << getBoardInfo();
435  }
436  }
437 
438 
439  if(false == res->serviceStart(eomn_serv_category_mc))
440  {
441  yError() << "embObjMotionControl::open() fails to start mc service for" << getBoardInfo() << ": cannot continue";
442  cleanup();
443  return false;
444  }
445  else
446  {
447  if(behFlags.verbosewhenok)
448  {
449  yDebug() << "embObjMotionControl::open() correctly starts mc service of" << getBoardInfo();
450  }
451  }
452 
453 
454  opened = true;
455 
456 
457  if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.config.mode)
458  {
459  // marco.accame on 10 june 2011.
460  // in here we wait some time so that yarprobotinterface may receive amo streaming before any
461  // further module (e.g., the calibrator) sends an active control mode and starts moving the motor.
462  //
463  // how does it work?
464  //
465  // when the ETH board receives command from res->serviceStart() it enters the control loop and it
466  // waits for a non IDLE control mode etc. but it also starts streaming AMO data. It is the thread
467  // inside EthReceiver whuch process UDP pckets, so a wait inside this current thread will do the job.
468  //
469  // moreover, if mcdiagnostics.config.par16 is > 0, then we also have a mechanism such that the AMO
470  // are streamed in a conditio of the motor not initiaized yet. in such way we can observe what is
471  // the effect of the configuration of the motor on the AMO reading.
472  //
473  // how does it work?
474  //
475  // it is the function embObjMotionControl::init() which configures the motors by sending messages
476  // with tag eoprot_tag_mc_joint_config.
477  // the ETH board, if it sees eomn_serv_diagn_mode_MC_AMOyarp and par16 > 0, it applies the config of
478  // the motor with a delay of mcdiagnostics.config.par16 milliseconds. so, in here if we wait
479  // the same amount of time, we:
480  // - can read amo values with the motors off,
481  // - we are sure that no other module (e.g., the calibrator) will attempt to move a motor which is
482  // not yet configured.
483  // I KNOW: IT WOULD BE MUCH BETTER to wait in here until the ETH board tells us that it has configured
484  // the motor (rather then just applying a delay and hoping the ETH boards works fine). but that would
485  // require some effort which for now i prefer to postpone because "l'ottimo e' il nemico del bene"
486  // and we need a quick solution to test icub3.
487 
488  SystemClock::delaySystem(0.001*mcdiagnostics.config.par16);
489  }
490 
491  return true;
492 }
493 
494 
495 int embObjMotionControl::fromConfig_NumOfJoints(yarp::os::Searchable &config)
496 {
497  //
498  // Read Configuration params from file
499  //
500  int jn = config.findGroup("GENERAL").check("Joints", Value(1), "Number of degrees of freedom").asInt32();
501 
502  return(jn);
503 }
504 
505 
506 
507 void embObjMotionControl::debugUtil_printJointsetInfo(void)
508 {
509 
510  yError() << "****** DEBUG PRINTS **********";
511  yError() << "joint to set:";
512  for(int x=0; x< _njoints; x++)
513  yError() << " /t j " << x << ": set " <<_joint2set[x];
514  yError() << "jointmap:";
515 
516  yError() << " number of sets" << _jsets.size();
517  for(size_t x=0; x< _jsets.size(); x++)
518  {
519  yError() << "set " << x<< "has size " <<_jsets[x].getNumberofJoints();
520  for(int y=0; y<_jsets[x].getNumberofJoints(); y++)
521  yError() << "set " << x << ": " << _jsets[x].joints[y];
522  }
523  yError() << "********* END ****************";
524 
525 }
526 
527 
528 
529 
530 bool embObjMotionControl::verifyUserControlLawConsistencyInJointSet(eomc::PidInfo *pidInfo)
531 {
532 
533  for(size_t s=0; s<_jsets.size(); s++)
534  {
535  int numofjoints = _jsets[s].getNumberofJoints();
536 
537  if(numofjoints== 0 )
538  {
539  yError() << "embObjMC" << getBoardInfo() << "Jointsset " << s << "hasn't joints!!! I should be never stay here!!!";
540  return false;
541  }
542  int firstjoint = _jsets[s].joints[0];//get firts joint of set s
543 
544  for(int k=1; k<numofjoints; k++)
545  {
546  int otherjoint = _jsets[s].joints[k];
547 
548  if(pidInfo[firstjoint].usernamePidSelected != pidInfo[otherjoint].usernamePidSelected)
549  {
550  yError() << "embObjMC "<< getBoardInfo() << "Joints beloning to same set must be have same control law. Joint " << otherjoint << " differs from " << firstjoint << "Set num " << s ;
551  yError() << pidInfo[firstjoint].usernamePidSelected << "***" << pidInfo[otherjoint].usernamePidSelected;
552  return false;
553  }
554  }
555  }
556  return true;
557 }
558 
559 
560 
561 
562 
563 bool embObjMotionControl::verifyUserControlLawConsistencyInJointSet(eomc::TrqPidInfo *pidInfo)
564 {
565  for(size_t s=0; s<_jsets.size(); s++)
566  {
567  int numofjoints = _jsets[s].getNumberofJoints();
568 
569  if(numofjoints== 0 )
570  {
571  yError() << "embObjMC "<< getBoardInfo() << "Jointsset " << s << "hasn't joints!!! I should be never stay here!!!";
572  return false;
573  }
574  int firstjoint = _jsets[s].joints[0];//get firts joint of set s
575 
576  for(int k=1; k<numofjoints; k++)
577  {
578  int otherjoint = _jsets[s].joints[k];
579 
580  if(pidInfo[firstjoint].usernamePidSelected != pidInfo[otherjoint].usernamePidSelected)
581  {
582  yError() << "embObjMC"<< getBoardInfo() << "Joints beloning to same set must be have same control law. Joint " << otherjoint << " differs from " << firstjoint << "Set num " << s ;
583  yError() << pidInfo[firstjoint].usernamePidSelected << "***" << pidInfo[otherjoint].usernamePidSelected;
584  return false;
585  }
586  }
587  }
588  return true;
589 }
590 
591 
592 bool embObjMotionControl::updatedJointsetsCfgWithControlInfo()
593 {
594  //#error ALE
595 
596  for(size_t s=0; s<_jsets.size(); s++)
597  {
598  if(_jsets[s].getNumberofJoints() == 0)
599  {
600  yError() << "embObjMC"<< getBoardInfo() << "Jointsset " << s << "hasn't joints!!! I should be never stay here!!!";
601  return false;
602  }
603 
604  int joint = _jsets[s].joints[0];
605  //eOmc_pidoutputtype_t pid_out_type = pidOutputTypeConver_eomc2fw(_trj_pids[joint].controlLaw);
606  //if(eomc_pidoutputtype_unknown == pid_out_type)
607  //{
608  // yError() << "embObjMC"<< getBoardInfo() << "pid output type is unknown for joint " << joint;
609  // return false;
610  //}
611  //_jsets[s].setPidOutputType(pid_out_type);
612  //_jsets[s].setCanDoTorqueControl(isTorqueControlEnabled(joint));
613 
614  _jsets[s].cfg.pid_output_types.postrj_ctrl_out_type = _trj_pids[joint].out_type;
615  _jsets[s].cfg.pid_output_types.veltrj_ctrl_out_type = _trj_pids[joint].out_type;
616  _jsets[s].cfg.pid_output_types.mixtrj_ctrl_out_type = _trj_pids[joint].out_type;
617 
618  //_jsets[s].cfg.pid_output_types.posdir_ctrl_out_type = _dir_pids[joint].out_type;
619  //_jsets[s].cfg.pid_output_types.veldir_ctrl_out_type = _dir_pids[joint].out_type;
620  _jsets[s].cfg.pid_output_types.posdir_ctrl_out_type = _trj_pids[joint].out_type;
621  _jsets[s].cfg.pid_output_types.veldir_ctrl_out_type = _trj_pids[joint].out_type;
622 
623  _jsets[s].cfg.pid_output_types.torque_ctrl_out_type = _trq_pids[joint].out_type;
624 
625  _jsets[s].cfg.pid_output_types.pwm_ctrl_out_type = eomc_ctrl_out_type_pwm;
626 
627  if (_cur_pids[joint].enabled)
628  {
629  _jsets[s].cfg.pid_output_types.cur_ctrl_out_type = eomc_ctrl_out_type_cur;
630  }
631  else
632  {
633  _jsets[s].cfg.pid_output_types.cur_ctrl_out_type = eomc_ctrl_out_type_n_a;
634  }
635  }
636  return true;
637 }
638 
639 
640 
641 
642 bool embObjMotionControl::saveCouplingsData(void)
643 {
644  eOmc_4jomo_coupling_t *jc_dest;
645 
646  static eOmc_4jomo_coupling_t dummyjomocoupling = {};
647 
648  switch(serviceConfig.ethservice.configuration.type)
649  {
650  case eomn_serv_MC_foc:
651  {
652  jc_dest = &(serviceConfig.ethservice.configuration.data.mc.foc_based.jomocoupling);
653  } break;
654 
655  case eomn_serv_MC_mc4plus:
656  {
657  jc_dest = &(serviceConfig.ethservice.configuration.data.mc.mc4plus_based.jomocoupling);
658  } break;
659 
660  case eomn_serv_MC_mc4plusmais:
661  {
662  jc_dest = &(serviceConfig.ethservice.configuration.data.mc.mc4plusmais_based.jomocoupling);
663 
664  } break;
665  case eomn_serv_MC_mc2pluspsc:
666  {
667  jc_dest = &(serviceConfig.ethservice.configuration.data.mc.mc2pluspsc.jomocoupling);
668 
669  } break;
670  case eomn_serv_MC_mc4plusfaps:
671  {
672  jc_dest = &(serviceConfig.ethservice.configuration.data.mc.mc4plusfaps.jomocoupling);
673 
674  } break;
675  case eomn_serv_MC_mc4pluspmc:
676  {
677  jc_dest = &dummyjomocoupling; // this mode does not have a coupling as it is w/ 7 independent joints
678 
679  } break;
680  case eomn_serv_MC_mc4:
681  {
682  return true;
683  } break;
684 
685  case eomn_serv_MC_generic:
686  {
687  return true;
688  } break;
689  default:
690  {
691  return false;
692  }
693  }
694 
695 
696  // this mode does not use jointsets but only eOmc_jointset_configuration_t
697  if(eomn_serv_MC_mc4pluspmc == serviceConfig.ethservice.configuration.type)
698  {
699  eOmc_arrayof_7jointsetconfig_t *arrayof7jset = &(serviceConfig.ethservice.configuration.data.mc.mc4pluspmc.arrayof7jointsets);
700  EOarray *array = eo_array_New(7, sizeof(eOmc_jointset_configuration_t), arrayof7jset);
701  // must initialise each entry of array
702 
703  //for(size_t s=0; s< _jsets.size(); s++)
704  for(size_t s=0; s<4; s++)
705  {
706  eOmc_jointset_configuration_t* cfg_ptr = _jsets[s].getConfiguration();
707  eo_array_PushBack(array, cfg_ptr);
708  }
709  for(size_t e=0; e<3; e++)
710  {
711  eOmc_jointset_configuration_t cfg = {0};
712  eo_array_PushBack(array, &cfg);
713  }
714 
715  return true;
716  }
717 
718 
719  memset(jc_dest, 0, sizeof(eOmc_4jomo_coupling_t));
720 
721  //I need to initialize all elements of joint2set with "eomc_jointSetNum_none": it is used by fw to get num of setBemfParamRaw
722  //4 is teh satic dimension of joint2set. see definition of type eOmc_4jomo_coupling_t
723  for(int i=0; i<4; i++)
724  {
725  jc_dest->joint2set[i] = eomc_jointSetNum_none;
726  }
727 
728  if(_joint2set.size() > 4 )
729  {
730  yError() << "embObjMC "<< getBoardInfo() << "Jointsset size is bigger than 4. I can't send jointset information to fw.";
731  return false;
732  }
733 
734  for(size_t i=0; i<_joint2set.size(); i++)
735  {
736  jc_dest->joint2set[i] = _joint2set[i];
737  }
738 
739  for(int i=0; i<4; i++)
740  {
741  for(int j=0; j<4; j++)
742  {
743  jc_dest->joint2motor[i][j] = eo_common_float_to_Q17_14((float)_couplingInfo.matrixJ2M[4*i+j]);
744  jc_dest->motor2joint[i][j] = eo_common_float_to_Q17_14((float)_couplingInfo.matrixM2J[4*i+j]);
745  }
746  }
747 
748 
749  for(int r=0; r<4; r++)
750  {
751  for(int c=0; c<6; c++)
752  {
753  jc_dest->encoder2joint[r][c] = eo_common_float_to_Q17_14((float)_couplingInfo.matrixE2J[6*r+c]);
754  }
755  }
756 
757  for(size_t s=0; s< _jsets.size(); s++)
758  {
759  eOmc_jointset_configuration_t* cfg_ptr = _jsets[s].getConfiguration();
760  memcpy(&(jc_dest->jsetcfg[s]), cfg_ptr, sizeof(eOmc_jointset_configuration_t));
761  }
762 
763  return true;
764 
765 }
766 
767 
768 bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config)
769 {
770  Bottle xtmp;
771  int i,j;
772 
773  measureConvFactors measConvFactors (_njoints);
774 
775  if(iNeedCouplingsInfo())
776  {
777 
779  if(!_mcparser->parseCouplingInfo(config, _couplingInfo))
780  return false;
781 
782 
784  if(!_mcparser->parseJointsetCfgGroup(config, _jsets, _joint2set))
785  return false;
786 
787  //debugUtil_printJointsetInfo();
788  }
789 
790 
792 
793 
794  {
795  if(!_mcparser->parseAxisInfo(config, _axisMap, _axesInfo))
796  return false;
797 
799  if(behFlags.useRawEncoderData)
800  {
801  for (i = 0; i < _njoints; i++)
802  {
803  measConvFactors.angleToEncoder[i] = 1;
804  }
805  }
806  else
807  {
808  if(!_mcparser->parseEncoderFactor(config, measConvFactors.angleToEncoder))
809  return false;
810  }
811 
812  if (!_mcparser->parsefullscalePWM(config, measConvFactors.dutycycleToPWM))
813  return false;
814 
815  if (!_mcparser->parseAmpsToSensor(config, measConvFactors.ampsToSensor))
816  return false;
817 
818  //VALE: i have to parse GeneralMecGroup after parsing jointsetcfg, because inside generalmec group there is useMotorSpeedFbk that needs jointset info.
819 
820  if(!_mcparser->parseGearboxValues(config, _gearbox_M2J, _gearbox_E2J))
821  return false;
822 
823  // useMotorSpeedFbk
824  if(eomn_serv_MC_mc4 != (eOmn_serv_type_t)serviceConfig.ethservice.configuration.type)
825  {
826  int* useMotorSpeedFbk = 0;
827  useMotorSpeedFbk = new int[_njoints];
828  if (!_mcparser->parseMechanicalsFlags(config, useMotorSpeedFbk))
829  {
830  delete[] useMotorSpeedFbk;
831  return false;
832  }
833  //Note: currently in eth protocol this parameter belongs to jointset configuration. So
834  // i need to check that every joint belong to same set has the same value
835  if (!verifyUseMotorSpeedFbkInJointSet(useMotorSpeedFbk))
836  {
837  delete[] useMotorSpeedFbk;
838  return false;
839  }
840  delete[] useMotorSpeedFbk;
841  }
842  bool deadzoneIsAvailable;
843  if(!_mcparser->parseDeadzoneValue(config, _deadzone, &deadzoneIsAvailable))
844  return false;
845  if(!deadzoneIsAvailable) // if parameter is not written in configuration files then use default values
846  {
847  updateDeadZoneWithDefaultValues();
848  }
849 
850  if(!_mcparser->parseKalmanFilterParams(config, _kalman_params))
851  {
852  return false;
853  }
854  }
855 
856 
858  {
859  bool lowLevPidisMandatory = false;
860 
861  if(serviceConfig.ethservice.configuration.type == eomn_serv_MC_foc)
862  {
863  lowLevPidisMandatory = true;
864  }
865 
866  if(!_mcparser->parsePids(config, _trj_pids/*, _dir_pids*/, _trq_pids, _cur_pids, _spd_pids, lowLevPidisMandatory))
867  return false;
868 
869  // 1) verify joint belonging to same set has same control law
870  //if(!verifyUserControlLawConsistencyInJointSet(_ppids))
871  // return false;
872  //if(!verifyUserControlLawConsistencyInJointSet(_vpids))
873  // return false;
874  //if(!verifyUserControlLawConsistencyInJointSet(_tpids))
875  // return false;
876 
877  //yarp::dev::PidFeedbackUnitsEnum fbk_TrqPidUnits;
878  //yarp::dev::PidOutputUnitsEnum out_TrqPidUnits;
879  //if(!verifyTorquePidshasSameUnitTypes(fbk_TrqPidUnits, out_TrqPidUnits))
880  // return false;
881 
882  //2) since some joint sets configuration info is in control and ids group, get that info and save them in jointset data struct.
883  updatedJointsetsCfgWithControlInfo();
884  }
885 
886  for (i = 0; i < _njoints; i++)
887  {
888  measConvFactors.newtonsToSensor[i] = 1000000.0f; // conversion from Nm into microNm
889 
890  measConvFactors.bemf2raw[i] = measConvFactors.newtonsToSensor[i] / measConvFactors.angleToEncoder[i];
891  if (_trq_pids->out_PidUnits == yarp::dev::PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT)
892  {
893  measConvFactors.ktau2raw[i] = measConvFactors.dutycycleToPWM[i] / measConvFactors.newtonsToSensor[i];
894  }
895  else if (_trq_pids->out_PidUnits == yarp::dev::PidOutputUnitsEnum::RAW_MACHINE_UNITS)
896  {
897  measConvFactors.ktau2raw[i] = 1.0 / measConvFactors.newtonsToSensor[i];
898  }
899  else
900  {
901  yError() << "Invalid ktau units"; return false;
902  }
903  }
904 
906  _measureConverter = new ControlBoardHelper(_njoints, _axisMap, measConvFactors.angleToEncoder, NULL, measConvFactors.newtonsToSensor, measConvFactors.ampsToSensor, nullptr, measConvFactors.dutycycleToPWM , measConvFactors.bemf2raw, measConvFactors.ktau2raw);
907  _measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, _trj_pids->fbk_PidUnits, _trj_pids->out_PidUnits);
908  //_measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_DIRECT, _dir_pids->fbk_PidUnits, _dir_pids->out_PidUnits);
909  _measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE, _trq_pids->fbk_PidUnits, _trq_pids->out_PidUnits);
910  _measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, _cur_pids->fbk_PidUnits, _cur_pids->out_PidUnits);
911  _measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY, _spd_pids->fbk_PidUnits, _spd_pids->out_PidUnits);
912  /*
913  void ControlBoardHelper::set_pid_conversion_units(const PidControlTypeEnum& pidtype, const PidFeedbackUnitsEnum fbk_conv_units, const PidOutputUnitsEnum out_conv_units)
914  {
915  ControlBoardHelper* cb_helper = this;
916  int nj = cb_helper->axes();
917  for (int i = 0; i < nj; i++)
918  {
919  mPriv->pid_units[pidtype][i].fbk_units = fbk_conv_units;
920  mPriv->pid_units[pidtype][i].out_units = out_conv_units;
921  }
922  }
923  */
924  initializeInterfaces(measConvFactors);
925  ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, _trj_pids->fbk_PidUnits, _trj_pids->out_PidUnits);
926  //ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_DIRECT, _dir_pids->fbk_PidUnits, _dir_pids->out_PidUnits);
927  ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE, _trq_pids->fbk_PidUnits, _trq_pids->out_PidUnits);
928  ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, _cur_pids->fbk_PidUnits, _cur_pids->out_PidUnits);
929  ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY, _spd_pids->fbk_PidUnits, _spd_pids->out_PidUnits);
930 
931 
932  //Now save in data in structures EmbObj protocol compatible
933  if(!saveCouplingsData())
934  return false;
935 
936 
938  if(! _mcparser->parseImpedanceGroup(config,_impedance_params))
939  {
940  yError() << "embObjMC " << getBoardInfo() << "IMPEDANCE section: error detected in parameters syntax";
941  return false;
942  }
943 
944 
946  for(j=0; j<_njoints; j++)
947  {
948  // got from canBusMotionControl, ask to Randazzo Marco
949  _impedance_limits[j].min_damp= 0.001;
950  _impedance_limits[j].max_damp= 9.888;
951  _impedance_limits[j].min_stiff= 0.002;
952  _impedance_limits[j].max_stiff= 9.889;
953  _impedance_limits[j].param_a= 0.011;
954  _impedance_limits[j].param_b= 0.012;
955  _impedance_limits[j].param_c= 0.013;
956  }
957 
958 
959 
961  {
962  if(!_mcparser->parseCurrentLimits(config, _currentLimits))
963  return false;
964 
965  if(!_mcparser->parseJointsLimits(config, _jointsLimits))
966  return false;
967 
968  if(!_mcparser->parseRotorsLimits(config, _rotorsLimits))
969  return false;
970  }
971 
973  if(serviceConfig.ethservice.configuration.type == eomn_serv_MC_foc)
974  {
975  std::string groupName = (static_cast<eObrd_type_t>(serviceConfig.ethservice.configuration.data.mc.foc_based.type) == eobrd_foc) ? "2FOC" : "AMCBLDC";
976  if(!_mcparser->parseFocGroup(config, _foc_based_info, groupName))
977  return false;
978  }
979 
980 
982  if(! _mcparser->parseTimeoutsGroup(config, _timeouts, 1000 /*defaultVelocityTimeout*/))
983  return false;
984 
985 
986  return true;
987 }
988 
989 
990 
991 bool embObjMotionControl::verifyUseMotorSpeedFbkInJointSet(int useMotorSpeedFbk [])
992 {
993  for(size_t s=0; s< _jsets.size(); s++)
994  {
995  int numofjointsinset = _jsets[s].getNumberofJoints();
996  if(numofjointsinset == 0 )
997  {
998  yError() << "embObjMC " << getBoardInfo() << "Jointsset " << s << "hasn't joints!!! I should be never stay here!!!";
999  return false;
1000  }
1001 
1002  int firstjointofset = _jsets[s].joints[0];
1003  for(int j=1; j<numofjointsinset; j++)
1004  {
1005  int joint = _jsets[s].joints[j];
1006  if(useMotorSpeedFbk[firstjointofset] != useMotorSpeedFbk[joint])
1007  {
1008  yError() << "embObjMC " << getBoardInfo() << ". Param useMotorSpeedFbk should have same value for joints belong same set. See joint " << firstjointofset << " and " << joint;
1009  return false;
1010  }
1011  }
1012 
1013  _jsets[s].setUseSpeedFeedbackFromMotors(useMotorSpeedFbk[firstjointofset]);
1014  }
1015 
1016  return true;
1017 
1018 }
1019 
1020 bool embObjMotionControl::verifyTorquePidshasSameUnitTypes(yarp::dev::PidFeedbackUnitsEnum &fbk_pidunits, yarp::dev::PidOutputUnitsEnum& out_pidunits)
1021 {
1022  fbk_pidunits = yarp::dev::PidFeedbackUnitsEnum::RAW_MACHINE_UNITS;
1023  out_pidunits = yarp::dev::PidOutputUnitsEnum::RAW_MACHINE_UNITS;
1024  //get first joint with enabled torque
1025  int firstjoint = -1;
1026  for(int i=0; i<_njoints; i++)
1027  {
1028  if(_trq_pids[i].enabled)
1029  firstjoint = i;
1030  }
1031 
1032  if(firstjoint==-1)
1033  {
1034  // no joint has torque enabed
1035  return true;
1036  }
1037 
1038  for(int i=firstjoint+1; i<_njoints; i++)
1039  {
1040  if(_trq_pids[i].enabled)
1041  {
1042  if(_trq_pids[firstjoint].fbk_PidUnits != _trq_pids[i].fbk_PidUnits ||
1043  _trq_pids[firstjoint].out_PidUnits != _trq_pids[i].out_PidUnits)
1044  {
1045  yError() << "embObjMC " << getBoardInfo() << "all joints with torque enabled should have same controlunits type. Joint " << firstjoint << " differs from joint " << i;
1046  return false;
1047  }
1048  }
1049  }
1050 
1051  fbk_pidunits = _trq_pids[firstjoint].fbk_PidUnits;
1052  out_pidunits = _trq_pids[firstjoint].out_PidUnits;
1053  return true;
1054 }
1055 
1056 bool embObjMotionControl::isTorqueControlEnabled(int joint)
1057 {
1058  return (_trq_pids[joint].enabled);
1059 }
1060 
1061 bool embObjMotionControl::isVelocityControlEnabled(int joint)
1062 {
1063  //return (_dir_pids[joint].enabled);
1064  return (_trj_pids[joint].enabled);
1065 }
1066 
1067 
1068 void embObjMotionControl::updateDeadZoneWithDefaultValues(void)
1069 {
1070  for(int i=0; i<_njoints; i++)
1071  {
1072  switch(_jointEncs[i].type)
1073  {
1074  case eomc_enc_aea:
1075  _deadzone[i] = eomc_defaultValue::DeadZone::jointWithAEA;// 0.0494;
1076  break;
1077  case eomc_enc_aea3:
1078  _deadzone[i] = eomc_defaultValue::DeadZone::jointWithAEA3;// TODO: temporary equal to 0.0
1079  break;
1080  case eomc_enc_aksim2:
1082  case eomc_enc_amo:
1083  _deadzone[i] = eomc_defaultValue::DeadZone::jointWithAMO;// 0.0055;
1084  break;
1085  case eomc_enc_roie:
1086  case eomc_enc_absanalog:
1087  case eomc_enc_mais:
1088  case eomc_enc_qenc:
1089  case eomc_enc_hallmotor:
1090  case eomc_enc_spichainof2:
1091  case eomc_enc_spichainof3:
1092  default:
1093  _deadzone[i] = 0.0;
1094 
1095  }
1096  }
1097 }
1098 
1099 // use this one for ... service configuration
1100 bool embObjMotionControl::fromConfig_readServiceCfg(yarp::os::Searchable &config)
1101 {
1102 
1103  if(false == parser->parseService(config, serviceConfig))
1104  {
1105  yError() << "embObjMC " << getBoardInfo() << "cannot parse service" ;
1106  return false;
1107  }
1108 
1109  if(eomn_serv_MC_generic == serviceConfig.ethservice.configuration.type)
1110  {
1111  yError() << "embObjMC " << getBoardInfo() << "it is no longer possible use eomn_serv_MC_generic, because firmware cannot configure itself!" ;
1112  return false;
1113  }
1114 
1115  //now parser read encoders' resolutions also.
1116  //so here I save in embObMotioncontrol memory encoders's resolution
1117  servMC_encoder_t * jointEncoder_ptr = NULL;
1118  servMC_encoder_t * motorEncoder_ptr = NULL;
1119  for(int i=0; i<_njoints; i++)
1120  {
1121  jointEncoder_ptr = parser->getEncoderAtJoint(i);
1122  motorEncoder_ptr = parser->getEncoderAtMotor(i);
1123 
1124  if(NULL == jointEncoder_ptr)
1125  {
1126  _jointEncs[i].resolution = 1;
1127  _jointEncs[i].type = eomc_enc_none;
1128  _jointEncs[i].tolerance = 0;
1129  }
1130  else
1131  {
1132  _jointEncs[i].resolution = jointEncoder_ptr->resolution;
1133  _jointEncs[i].type = (eOmc_encoder_t)jointEncoder_ptr->desc.type; //Here I'm sure that type belong to eOmc_encoder_t enum.It is filled by eomc_string2encoder function
1134  _jointEncs[i].tolerance = jointEncoder_ptr->tolerance;
1135  }
1136 
1137 
1138  if(NULL == motorEncoder_ptr)
1139  {
1140  _motorEncs[i].resolution = 1;
1141  _motorEncs[i].type = eomc_enc_none;
1142  _motorEncs[i].tolerance = 0;
1143  }
1144  else
1145  {
1146  _motorEncs[i].resolution = motorEncoder_ptr->resolution;
1147  _motorEncs[i].type = (eOmc_encoder_t)motorEncoder_ptr->desc.type; //Here I'm sure that type belong to eOmc_encoder_t enum.It is filled by eomc_string2encoder function
1148  _motorEncs[i].tolerance = motorEncoder_ptr->tolerance;
1149  }
1150 
1151 
1152  }
1153 
1154 
1155  return true;
1156 }
1157 
1158 
1159 
1160 bool embObjMotionControl::fromConfig(yarp::os::Searchable &config)
1161 {
1162 
1163  _njoints = fromConfig_NumOfJoints(config);
1164 
1165  if(0 == _njoints)
1166  {
1167  yError() << "embObjMC"<< getBoardInfo() << "fromConfig(): detected _njoints = " << _njoints;
1168  return false;
1169  }
1170 
1171  // we have number of joints inside _njoints. we allocate all required buffers
1172  if(!alloc(_njoints))
1173  {
1174  yError() << "embObjMC"<< getBoardInfo() << "fromConfig(): alloc() failed for _njoints = " << _njoints;
1175  return false;
1176  }
1177 
1178 
1179  _mcparser = new eomc::Parser(_njoints, string(res->getProperties().boardnameString));
1180 
1182  int currentMCversion =0;
1183  if(!_mcparser->parseMotioncontrolVersion(config, currentMCversion))
1184  return false;
1185 
1186  if (currentMCversion != PARSER_MOTION_CONTROL_VERSION)
1187  {
1188  yError() << "embObjMC" << getBoardInfo() << "------ ATTENTION!!!! Wrong value of <MotioncontrolVersion> parameter !!!! ---------------------------------------------------------------------------------------";
1189  yError() << "embObjMC" << getBoardInfo() << "------ This means that the configuration files of this device are not compatible with my parser, so I cannot start. ";
1190  yError() << "embObjMC" << getBoardInfo() << "------ I need version " << PARSER_MOTION_CONTROL_VERSION << ", but in configuration files have version " << currentMCversion << ".";
1191  yError() << "embObjMC" << getBoardInfo() << "------ Please update configuration files in robots-configuration repository. (see http://wiki.icub.org/wiki/Robot_configuration for more information). ";
1192  yError() << "embObjMC" << getBoardInfo() << "------ If the problem persists contact icub-support@iit.it DO NOT DO IT BY YOURSELF.";
1193  yError() << "embObjMC" << getBoardInfo() << "----------------------------------------------------------------------------------------------------------------------------------------------------------------";
1194  return false;
1195  }
1196 
1197  //print verbose info
1198  if(_mcparser->isVerboseEnabled(config))
1199  yTrace() << config.toString().c_str();
1200 
1201 
1202 
1203  // first step of configuration
1204  if(false == fromConfig_readServiceCfg(config))
1205  {
1206  return false;
1207  }
1208 
1209  if(!_mcparser->parseBehaviourFalgs(config, behFlags.useRawEncoderData, behFlags.pwmIsLimited ))//in general info group
1210  {
1211  return false;
1212  }
1213 
1214 
1215  // second step of configuration
1216  if(false == fromConfig_Step2(config))
1217  {
1218  return false;
1219  }
1220 
1221  // third step of configuration
1222 
1223 
1224  return true;
1225 }
1226 
1227 
1228 bool embObjMotionControl::init()
1229 {
1230  eOprotID32_t protid = 0;
1231 
1233  //SEND DISABLE TO ALL JOINTS
1235 
1236  for(int logico=0; logico< _njoints; logico++)
1237  {
1238  int fisico = _axisMap[logico];
1239  protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, fisico, eoprot_tag_mc_joint_cmmnds_controlmode);
1240  eOenum08_t controlMode = eomc_controlmode_cmd_idle;
1241 
1242  if(false == res->setRemoteValue(protid, &controlMode))
1243  {
1244  yError() << "embObjMotionControl::init() had an error while setting eomc_controlmode_cmd_idle in "<< getBoardInfo();
1245  // return(false); i dont return false. because even if a failure, that is not a severe error.
1246  // MOREOVER: to verify we must read the status of the joint and NOT the command ... THINK OF IT
1247  }
1248  }
1249 
1250  SystemClock::delaySystem(0.010);
1251 
1252 
1254  // configure the regular rops
1256 
1257  vector<eOprotID32_t> id32v(0);
1258  for(int n=0; n<_njoints; n++)
1259  {
1260  protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, n, eoprot_tag_mc_joint_status_core);
1261  id32v.push_back(protid);
1262  protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, n, eoprot_tag_mc_motor_status);
1263  id32v.push_back(protid);
1264  }
1265 
1266  if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.config.mode)
1267  {
1268  protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, 0, eoprot_tag_mc_joint_status_debug);
1269  id32v.push_back(protid);
1270  protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, 1, eoprot_tag_mc_joint_status_debug);
1271  id32v.push_back(protid);
1272  }
1273 
1274 
1275  if(false == res->serviceSetRegulars(eomn_serv_category_mc, id32v))
1276  {
1277  yError() << "embObjMotionControl::init() fails to add its variables to regulars in "<< getBoardInfo() << ": cannot proceed any further";
1278  return false;
1279  }
1280  else
1281  {
1282  if(behFlags.verbosewhenok)
1283  {
1284  yDebug() << "embObjMotionControl::init() added" << id32v.size() << "regular rops to "<< getBoardInfo();
1285  char nvinfo[128];
1286  for(unsigned int r=0; r<id32v.size(); r++)
1287  {
1288  uint32_t id32 = id32v.at(r);
1289  eoprot_ID2information(id32, nvinfo, sizeof(nvinfo));
1290  yDebug() << "\t it added regular rop for" << nvinfo;
1291  }
1292  }
1293  }
1294 
1295  SystemClock::delaySystem(0.005);
1296 
1297 
1298 
1300  // invia la configurazione dei GIUNTI //
1302  for(int logico=0; logico< _njoints; logico++)
1303  {
1304  int fisico = _axisMap[logico];
1305  protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, fisico, eoprot_tag_mc_joint_config);
1306 
1307  eOmc_joint_config_t jconfig = {0};
1308  memset(&jconfig, 0, sizeof(eOmc_joint_config_t));
1309  yarp::dev::Pid tmp;
1310  tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_POSITION,_trj_pids[logico].pid, fisico);
1311  copyPid_iCub2eo(&tmp, &jconfig.pidtrajectory);
1312  //tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_DIRECT, _dir_pids[logico].pid, fisico);
1313  //copyPid_iCub2eo(&tmp, &jconfig.piddirect);
1314  tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_TORQUE, _trq_pids[logico].pid, fisico);
1315  copyPid_iCub2eo(&tmp, &jconfig.pidtorque);
1316 
1317  //stiffness and damping read in xml file are in Nm/deg and Nm/(Deg/sec), so we need to convert before send to fw.
1318  jconfig.impedance.damping = (eOmeas_damping_t) _measureConverter->impN2S(_impedance_params[logico].damping, fisico);
1319  jconfig.impedance.stiffness = (eOmeas_stiffness_t) _measureConverter->impN2S(_impedance_params[logico].stiffness, fisico);
1320  jconfig.impedance.offset = 0;
1321 
1322  _cacheImpedance[logico].stiffness = jconfig.impedance.stiffness;
1323  _cacheImpedance[logico].damping = jconfig.impedance.damping;
1324  _cacheImpedance[logico].offset = jconfig.impedance.offset;
1325 
1326  jconfig.userlimits.max = (eOmeas_position_t) S_32(_measureConverter->posA2E(_jointsLimits[logico].posMax, fisico));
1327  jconfig.userlimits.min = (eOmeas_position_t) S_32(_measureConverter->posA2E(_jointsLimits[logico].posMin, fisico));
1328 
1329  jconfig.hardwarelimits.max = (eOmeas_position_t) S_32(_measureConverter->posA2E(_jointsLimits[logico].posHwMax, fisico));
1330  jconfig.hardwarelimits.min = (eOmeas_position_t) S_32(_measureConverter->posA2E(_jointsLimits[logico].posHwMin, fisico));
1331 
1332 
1333  jconfig.maxvelocityofjoint = S_32(_measureConverter->posA2E(_jointsLimits[logico].velMax, fisico)); //icubdeg/s
1334  jconfig.velocitysetpointtimeout = (eOmeas_time_t) U_16(_timeouts[logico].velocity);
1335 
1336  jconfig.jntEncoderResolution = _jointEncs[logico].resolution;
1337  jconfig.jntEncoderType = _jointEncs[logico].type;
1338  jconfig.jntEncTolerance = _jointEncs[logico].tolerance;
1339 
1340  jconfig.motor_params.bemf_value = _measureConverter->bemf_user2raw(_trq_pids[logico].kbemf, fisico);
1341  jconfig.motor_params.bemf_scale = 0;
1342  jconfig.motor_params.ktau_value = _measureConverter->ktau_user2raw(_trq_pids[logico].ktau, fisico);
1343  jconfig.motor_params.ktau_scale = 0;
1344  jconfig.motor_params.friction.viscous_pos_val = _measureConverter->viscousPos_user2raw(_trq_pids[logico].viscousPos, fisico);
1345  jconfig.motor_params.friction.viscous_neg_val = _measureConverter->viscousNeg_user2raw(_trq_pids[logico].viscousNeg, fisico);
1346  jconfig.motor_params.friction.coulomb_pos_val = _measureConverter->coulombPos_user2raw(_trq_pids[logico].coulombPos, fisico);
1347  jconfig.motor_params.friction.coulomb_neg_val = _measureConverter->coulombNeg_user2raw(_trq_pids[logico].coulombNeg, fisico);
1348 
1349  jconfig.gearbox_E2J = _gearbox_E2J[logico];
1350 
1351  jconfig.deadzone = _measureConverter->posA2E(_deadzone[logico], fisico);
1352 
1353  jconfig.tcfiltertype=_trq_pids[logico].filterType;
1354 
1355  jconfig.kalman_params.enabled = _kalman_params[logico].enabled;
1356  for(int i=0; i<_kalman_params[logico].x0.size(); i++) jconfig.kalman_params.x0[i] = _kalman_params[logico].x0.at(i);
1357  for(int i=0; i<_kalman_params[logico].Q.size(); i++) jconfig.kalman_params.Q[i] = _kalman_params[logico].Q.at(i);
1358  jconfig.kalman_params.R = _kalman_params[logico].R;
1359  jconfig.kalman_params.P0 = _kalman_params[logico].P0;
1360 
1361  if(false == res->setcheckRemoteValue(protid, &jconfig, 10, 0.010, 0.050))
1362  {
1363  yError() << "FATAL: embObjMotionControl::init() had an error while calling setcheckRemoteValue() for joint config fisico #" << fisico << "in "<< getBoardInfo();
1364  return false;
1365  }
1366  else
1367  {
1368  if(behFlags.verbosewhenok)
1369  {
1370  yDebug() << "embObjMotionControl::init() correctly configured joint config fisico #" << fisico << "in "<< getBoardInfo();
1371  }
1372  }
1373  }
1374 
1375 
1377  // invia la configurazione dei MOTORI //
1379 
1380 
1381  for(int logico=0; logico<_njoints; logico++)
1382  {
1383  int fisico = _axisMap[logico];
1384 
1385  protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, fisico, eoprot_tag_mc_motor_config);
1386  eOmc_motor_config_t motor_cfg = {0};
1387  motor_cfg.maxvelocityofmotor = 0;//_maxMotorVelocity[logico]; //unused yet!
1388  motor_cfg.currentLimits.nominalCurrent = _currentLimits[logico].nominalCurrent;
1389  motor_cfg.currentLimits.overloadCurrent = _currentLimits[logico].overloadCurrent;
1390  motor_cfg.currentLimits.peakCurrent = _currentLimits[logico].peakCurrent;
1391  motor_cfg.gearbox_M2J = _gearbox_M2J[logico];
1392  motor_cfg.rotorEncoderResolution = _motorEncs[logico].resolution;
1393  motor_cfg.rotEncTolerance = _motorEncs[logico].tolerance;
1394  motor_cfg.hasHallSensor = _foc_based_info[logico].hasHallSensor;
1395  motor_cfg.hasRotorEncoder = _foc_based_info[logico].hasRotorEncoder;
1396  motor_cfg.hasTempSensor = _foc_based_info[logico].hasTempSensor;
1397  motor_cfg.hasRotorEncoderIndex = _foc_based_info[logico].hasRotorEncoderIndex;
1398  motor_cfg.hasSpeedEncoder = _foc_based_info[logico].hasSpeedEncoder;
1399  motor_cfg.verbose = _foc_based_info[logico].verbose;
1400  motor_cfg.motorPoles = _foc_based_info[logico].motorPoles;
1401  motor_cfg.rotorIndexOffset = _foc_based_info[logico].rotorIndexOffset;
1402  motor_cfg.rotorEncoderType = _motorEncs[logico].type;
1403  motor_cfg.pwmLimit =_rotorsLimits[logico].pwmMax;
1404  motor_cfg.limitsofrotor.max = (eOmeas_position_t) S_32(_measureConverter->posA2E(_rotorsLimits[logico].posMax, fisico ));
1405  motor_cfg.limitsofrotor.min = (eOmeas_position_t) S_32(_measureConverter->posA2E(_rotorsLimits[logico].posMin, fisico ));
1406 
1407  yarp::dev::Pid tmp;
1408  tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_CURRENT, _cur_pids[logico].pid, fisico);
1409  copyPid_iCub2eo(&tmp, &motor_cfg.pidcurrent);
1410 
1411  tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_VELOCITY, _spd_pids[logico].pid, fisico);
1412  copyPid_iCub2eo(&tmp, &motor_cfg.pidspeed);
1413 
1414  if (false == res->setcheckRemoteValue(protid, &motor_cfg, 10, 0.010, 0.050))
1415  {
1416  yError() << "FATAL: embObjMotionControl::init() had an error while calling setcheckRemoteValue() for motor config fisico #" << fisico << "in "<< getBoardInfo();
1417  return false;
1418  }
1419  else
1420  {
1421  if (behFlags.verbosewhenok)
1422  {
1423  yDebug() << "embObjMotionControl::init() correctly configured motor config fisico #" << fisico << "in "<< getBoardInfo();
1424  }
1425  }
1426  }
1427 
1429  // invia la configurazione del controller //
1431 
1432  //to be done
1433 
1434  yTrace() << "embObjMotionControl::init(): correctly instantiated for " << getBoardInfo();
1435  return true;
1436 }
1437 
1438 
1439 
1441 {
1442  yTrace() << " embObjMotionControl::close()";
1443 
1444  ImplementControlMode::uninitialize();
1445  ImplementEncodersTimed::uninitialize();
1446  ImplementMotorEncoders::uninitialize();
1447  ImplementPositionControl::uninitialize();
1448  ImplementVelocityControl::uninitialize();
1449  ImplementPidControl::uninitialize();
1450  ImplementControlCalibration::uninitialize();
1451  ImplementAmplifierControl::uninitialize();
1452  ImplementImpedanceControl::uninitialize();
1453  ImplementControlLimits::uninitialize();
1454  ImplementTorqueControl::uninitialize();
1455  ImplementPositionDirect::uninitialize();
1456  ImplementInteractionMode::uninitialize();
1457  ImplementRemoteVariables::uninitialize();
1458  ImplementAxisInfo::uninitialize();
1459  ImplementCurrentControl::uninitialize();
1460  ImplementPWMControl::uninitialize();
1461  ImplementJointFault::uninitialize();
1462 
1463  if (_measureConverter) {delete _measureConverter; _measureConverter=0;}
1464 
1465 
1466  if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.config.mode)
1467  {
1468  // close the ports
1469  for(size_t i=0; i<mcdiagnostics.ports.size(); i++)
1470  {
1471  mcdiagnostics.ports[i]->close();
1472  delete mcdiagnostics.ports[i];
1473  }
1474  mcdiagnostics.ports.clear();
1475 
1476  mcdiagnostics.config.mode = eomn_serv_diagn_mode_NONE;
1477  mcdiagnostics.config.par16 = 0;
1478  }
1479 
1480  delete event_downsampler;
1481  // in cleanup, at date of 23feb2016 there is a call to ethManager->releaseResource() which ...
1482  // send to config all the boards and stops tx and rx treads.
1483  // thus, in here we cannot call serviceStop(mc) because there will be tx/rx activity only for the first call of ::close().
1484  // i termporarily put serviceStop(eomn_serv_category_all) inside releaseResource()
1485  // todo: later on: clear regulars of mc, stop(mc), inside releaseresource() DO NOT stop tx/rx activity and DO NOT stop all services
1486  // res->serviceStop(eomn_serv_category_mc);
1487  // #warning TODO: clear the regulars imposed by motion-control.
1488 
1489  cleanup();
1490 
1491  return true;
1492 }
1493 
1494 void embObjMotionControl::cleanup(void)
1495 {
1496  if(ethManager == NULL) return;
1497 
1498  int ret = ethManager->releaseResource2(res, this);
1499  res = NULL;
1500  if(ret == -1)
1501  ethManager->killYourself();
1502 }
1503 
1504 
1505 
1507 {
1509 }
1510 
1511 bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxdata)
1512 {
1513  // use this function to update the values cached in the class using data received by the remote boards via the network callbacks
1514  // in embObjMotionControl it is updated only the timestamp of the encoders, thuus i dont used rxdata
1515  size_t joint = eoprot_ID2index(id32);
1516 
1517  // rxdata = rxdata;
1518 
1519  // marco.accame: pay attention using rxdata. the rxdata depends on the id32.
1520  // now the function update() is called with rxdata of different types.
1521  // if the tag is eoprot_tag_mc_joint_status, then rxdata is of type eOmc_joint_status_t*
1522  // if the tag is eoprot_tag_mc_joint_status_basic, then rxdata is of type eOmc_joint_status_basic_t*
1523 
1524 
1525  // for the case of id32 which contains an encoder value .... we refresh the timestamp of that encoder
1526 
1527  if(true == initialised())
1528  { // do it only if we already have opened the device
1529  std::lock_guard<std::mutex> lck(_mutex);
1530  _encodersStamp[joint] = timestamp;
1531  }
1532 
1533 
1534  if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.config.mode)
1535  {
1536  eOprotEntity_t ent = eoprot_ID2entity(id32);
1537  eOprotTag_t tag = eoprot_ID2tag(id32);
1538 
1539  char str[128] = "boh";
1540 
1541  eoprot_ID2information(id32, str, sizeof(str));
1542 
1543  if((eoprot_entity_mc_joint == ent) && (eoprot_tag_mc_joint_status_debug == tag) && (joint < mcdiagnostics.ports.size()))
1544  {
1545 
1546  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joint, eoprot_tag_mc_joint_status_core);
1547  eOmc_joint_status_core_t jcore = {};
1548 
1549  res->getLocalValue(id32, &jcore);
1550 
1551  int32_t *debug32 = reinterpret_cast<int32_t*>(rxdata);
1552  // write into relevant port
1553 
1554  Bottle& output = mcdiagnostics.ports[joint]->prepare();
1555  output.clear();
1556  //output.addString("[yt, amo, reg, pos]"); // but we must get the joint and the motor as well
1557  output.addString("[yt, amo, reg, pos]");
1558  output.addFloat64(timestamp);
1559  output.addInt32(debug32[0]);
1560  output.addInt32(debug32[1]);
1561  output.addInt32(jcore.measures.meas_position);
1562  mcdiagnostics.ports[joint]->write();
1563  }
1564  }
1565 
1566  return true;
1567 }
1568 
1569 
1571 bool embObjMotionControl::setPidRaw(const PidControlTypeEnum& pidtype, int j, const Pid &pid)
1572 {
1573  switch (pidtype)
1574  {
1575  case VOCAB_PIDTYPE_POSITION:
1576  helper_setPosPidRaw(j,pid);
1577  break;
1578  case VOCAB_PIDTYPE_VELOCITY:
1579  //helper_setVelPidRaw(j,pid);
1580  helper_setSpdPidRaw(j, pid);
1581  break;
1582  case VOCAB_PIDTYPE_TORQUE:
1583  helper_setTrqPidRaw(j, pid);
1584  break;
1585  case VOCAB_PIDTYPE_CURRENT:
1586  helper_setCurPidRaw(j,pid);
1587  break;
1588  default:
1589  yError()<<"Invalid pidtype:"<<pidtype;
1590  break;
1591  }
1592  return true;
1593 }
1594 
1595 bool embObjMotionControl::getPidRaw (const PidControlTypeEnum& pidtype, int axis, Pid *pid)
1596 {
1597  switch (pidtype)
1598  {
1599  case VOCAB_PIDTYPE_POSITION:
1600  helper_getPosPidRaw(axis,pid);
1601  break;
1602  case VOCAB_PIDTYPE_VELOCITY:
1603  //helper_getVelPidRaw(axis,pid);
1604  helper_getSpdPidRaw(axis, pid);
1605  break;
1606  case VOCAB_PIDTYPE_TORQUE:
1607  helper_getTrqPidRaw(axis, pid);
1608  break;
1609  case VOCAB_PIDTYPE_CURRENT:
1610  helper_getCurPidRaw(axis,pid);
1611  break;
1612  default:
1613  yError()<<"Invalid pidtype:"<<pidtype;
1614  break;
1615  }
1616  return true;
1617 }
1618 
1619 bool embObjMotionControl::helper_setPosPidRaw(int j, const Pid &pid)
1620 {
1621  eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_pidtrajectory);
1622  eOmc_PID_t outPid;
1623  Pid hwPid = pid;
1624 
1625  //printf("helper_setPosPid: kp=%f ki=%f kd=%f\n", hwPid.kp, hwPid.ki, hwPid.kd);
1626  copyPid_iCub2eo(&hwPid, &outPid);
1627 
1628  if(false == res->setRemoteValue(protoId, &outPid))
1629  {
1630  yError() << "while setting position PIDs for " << getBoardInfo() << " joint " << j;
1631  return false;
1632  }
1633 
1634  return true;
1635 }
1636 
1637 bool embObjMotionControl::setPidsRaw(const PidControlTypeEnum& pidtype, const Pid *pids)
1638 {
1639  bool ret = true;
1640  for(int j=0; j< _njoints; j++)
1641  {
1642  ret &= setPidRaw(pidtype, j, pids[j]);
1643  }
1644  return ret;
1645 }
1646 
1647 bool embObjMotionControl::setPidReferenceRaw(const PidControlTypeEnum& pidtype, int j, double ref)
1648 {
1649  return NOT_YET_IMPLEMENTED("setPidReferenceRaw");
1650 }
1651 
1652 bool embObjMotionControl::setPidReferencesRaw(const PidControlTypeEnum& pidtype, const double *refs)
1653 {
1654  bool ret = true;
1655  for(int j=0, index=0; j< _njoints; j++, index++)
1656  {
1657  ret &= setPidReferenceRaw(pidtype, j, refs[index]);
1658  }
1659  return ret;
1660 }
1661 
1662 bool embObjMotionControl::setPidErrorLimitRaw(const PidControlTypeEnum& pidtype, int j, double limit)
1663 {
1664  // print_debug(AC_trace_file, "embObjMotionControl::setErrorLimitRaw()");
1665  return NOT_YET_IMPLEMENTED("setErrorLimitRaw");
1666 }
1667 
1668 bool embObjMotionControl::setPidErrorLimitsRaw(const PidControlTypeEnum& pidtype, const double *limits)
1669 {
1670  // print_debug(AC_trace_file, "embObjMotionControl::setErrorLimitsRaw()");
1671  return NOT_YET_IMPLEMENTED("setErrorLimitsRaw");
1672 }
1673 
1674 bool embObjMotionControl::getPidErrorRaw(const PidControlTypeEnum& pidtype, int j, double *err)
1675 {
1676  uint16_t size = 0;
1677  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
1678  eOmc_joint_status_core_t jcore = {0};
1679  *err = 0;
1680  if(!res->getLocalValue(id32, &jcore))
1681  return false;
1682 
1683  switch(pidtype)
1684  {
1685  case VOCAB_PIDTYPE_POSITION:
1686  {
1687  if((eomc_controlmode_torque == jcore.modes.controlmodestatus) ||
1688  (eomc_controlmode_openloop == jcore.modes.controlmodestatus) ||
1689  (eomc_controlmode_current == jcore.modes.controlmodestatus))
1690  return true;
1691  else
1692  *err = (double) jcore.ofpid.generic.error1;
1693  }
1694  break;
1695  /*
1696  case VOCAB_PIDTYPE_DIRECT:
1697  {
1698  *err=0; //not yet implemented
1699  NOT_YET_IMPLEMENTED("getPidErrorRaw VOCAB_PIDTYPE_DIRECT");
1700  }
1701  break;
1702  */
1703  case VOCAB_PIDTYPE_TORQUE:
1704  {
1705  if ((eOmc_interactionmode_compliant == jcore.modes.interactionmodestatus) &&
1706  (eomc_controlmode_position == jcore.modes.controlmodestatus))
1707  {
1708  *err = (double) jcore.ofpid.complpos.errtrq;
1709  }
1710 
1711  if(eomc_controlmode_torque == jcore.modes.controlmodestatus)
1712  {
1713  *err = (double) jcore.ofpid.torque.errtrq;
1714  }
1715  }
1716  break;
1717  case VOCAB_PIDTYPE_VELOCITY:
1718  {
1719  *err = 0; //not yet implemented
1720  NOT_YET_IMPLEMENTED("getPidErrorRaw VOCAB_PIDTYPE_LLSPEED");
1721  }
1722  break;
1723  case VOCAB_PIDTYPE_CURRENT:
1724  {
1725  *err = 0; //not yet implemented
1726  NOT_YET_IMPLEMENTED("getPidErrorRaw VOCAB_PIDTYPE_CURRENT");
1727  }
1728  break;
1729  default:
1730  {
1731  yError()<<"Invalid pidtype:"<<pidtype;
1732  }
1733  break;
1734  }
1735  return true;
1736 }
1737 
1738 bool embObjMotionControl::getPidErrorsRaw(const PidControlTypeEnum& pidtype, double *errs)
1739 {
1740  bool ret = true;
1741  for(int j=0; j< _njoints; j++)
1742  {
1743  ret &= getPidErrorRaw(pidtype, j, &errs[j]);
1744  }
1745  return ret;
1746 }
1747 
1748 bool embObjMotionControl::helper_getPosPidRaw(int j, Pid *pid)
1749 {
1750  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_pidtrajectory);
1751 
1752  uint16_t size;
1753  eOmc_PID_t eoPID = {0};
1754 
1755 
1756 #ifdef NETWORK_PERFORMANCE_BENCHMARK
1757  double start = yarp::os::Time::now();
1758 #endif
1759 
1760  bool ret = askRemoteValue(protid, &eoPID, size);
1761 
1762 #ifdef NETWORK_PERFORMANCE_BENCHMARK
1763  double end = yarp::os::Time::now();
1764  m_responseTimingVerifier.tick(end-start, start);
1765 #endif
1766 
1767  if(!ret)
1768  return false;
1769 
1770  copyPid_eo2iCub(&eoPID, pid);
1771 
1772  //printf("helper_getPosPid: kp=%f ki=%f kd=%f\n", pid->kp, pid->ki, pid->kd);
1773 
1774  return true;
1775 }
1776 
1777 bool embObjMotionControl::helper_getPosPidsRaw(Pid *pid)
1778 {
1779  std::vector<eOmc_PID_t> eoPIDList(_njoints);
1780  bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_config_pidtrajectory, eoPIDList);
1781  if(!ret)
1782  {
1783  yError() << "failed helper_getPosPidsRaw for" << getBoardInfo();
1784  return false;
1785  }
1786 
1787  for(int j=0; j<_njoints; j++)
1788  {
1789  copyPid_eo2iCub(&eoPIDList[j], &pid[j]);
1790 
1791  //printf("helper_getPosPid: kp=%f ki=%f kd=%f\n", pid->kp, pid->ki, pid->kd);
1792  }
1793  return true;
1794 }
1795 
1796 
1797 bool embObjMotionControl::getPidsRaw(const PidControlTypeEnum& pidtype, Pid *pids)
1798 {
1799  switch (pidtype)
1800  {
1801  case VOCAB_PIDTYPE_POSITION:
1802  helper_getPosPidsRaw(pids);
1803  break;
1804  //case VOCAB_PIDTYPE_DIRECT:
1805  // helper_getVelPidsRaw(pids);
1806  // break;
1807  case VOCAB_PIDTYPE_TORQUE:
1808  helper_getTrqPidsRaw(pids);
1809  break;
1810  case VOCAB_PIDTYPE_CURRENT:
1811  helper_getCurPidsRaw(pids);
1812  break;
1813  case VOCAB_PIDTYPE_VELOCITY:
1814  helper_getSpdPidsRaw(pids);
1815  break;
1816  default:
1817  yError()<<"Invalid pidtype:"<<pidtype;
1818  break;
1819  }
1820  return true;
1821 }
1822 
1823 bool embObjMotionControl::getPidReferenceRaw(const PidControlTypeEnum& pidtype, int j, double *ref)
1824 {
1825  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
1826  eOmc_joint_status_core_t jcore = {0};
1827  *ref = 0;
1828  if(!res->getLocalValue(id32, &jcore))
1829  return false;
1830 
1831  switch (pidtype)
1832  {
1833  case VOCAB_PIDTYPE_POSITION:
1834  {
1835  if((eomc_controlmode_torque == jcore.modes.controlmodestatus) ||
1836  (eomc_controlmode_openloop == jcore.modes.controlmodestatus) ||
1837  (eomc_controlmode_current == jcore.modes.controlmodestatus))
1838  { *ref = 0; yError() << "Invalid getPidReferenceRaw() request for current control mode"; return true; }
1839  *ref = (double) jcore.ofpid.generic.reference1;
1840  }
1841  break;
1842  //case VOCAB_PIDTYPE_DIRECT:
1843  //{
1844  // *ref=0;
1845  // NOT_YET_IMPLEMENTED("getPidReferenceRaw VOCAB_PIDTYPE_DIRECT");
1846  //}
1847  //break;
1848  case VOCAB_PIDTYPE_TORQUE:
1849  {
1850  *ref = 0;
1851  NOT_YET_IMPLEMENTED("getPidReferenceRaw VOCAB_PIDTYPE_TORQUE");
1852  }
1853  break;
1854  case VOCAB_PIDTYPE_CURRENT:
1855  {
1856  *ref=0;
1857  NOT_YET_IMPLEMENTED("getPidReferenceRaw VOCAB_PIDTYPE_CURRENT");
1858  }
1859  break;
1860  case VOCAB_PIDTYPE_VELOCITY:
1861  {
1862  *ref = 0;
1863  NOT_YET_IMPLEMENTED("getPidReferenceRaw VOCAB_PIDTYPE_VELOCITY");
1864  }
1865  break;
1866  default:
1867  {
1868  *ref=0;
1869  yError()<<"Invalid pidtype:"<<pidtype;
1870  }
1871  break;
1872  }
1873  return true;
1874 }
1875 
1876 bool embObjMotionControl::getPidReferencesRaw(const PidControlTypeEnum& pidtype, double *refs)
1877 {
1878  bool ret = true;
1879 
1880  // just one joint at time, wait answer before getting to the next.
1881  // This is because otherwise too many msg will be placed into can queue
1882  for(int j=0; j< _njoints; j++)
1883  {
1884  ret &= getPidReferenceRaw(pidtype, j, &refs[j]);
1885  }
1886  return ret;
1887 }
1888 
1889 bool embObjMotionControl::getPidErrorLimitRaw(const PidControlTypeEnum& pidtype, int j, double *limit)
1890 {
1891  return NOT_YET_IMPLEMENTED("getErrorLimit");
1892 }
1893 
1894 bool embObjMotionControl::getPidErrorLimitsRaw(const PidControlTypeEnum& pidtype, double *limits)
1895 {
1896  return NOT_YET_IMPLEMENTED("getErrorLimit");
1897 }
1898 
1899 bool embObjMotionControl::resetPidRaw(const PidControlTypeEnum& pidtype, int j)
1900 {
1901  return NOT_YET_IMPLEMENTED("resetPid");
1902 }
1903 
1904 bool embObjMotionControl::disablePidRaw(const PidControlTypeEnum& pidtype, int j)
1905 {
1906  return DEPRECATED("disablePidRaw");
1907 }
1908 
1909 bool embObjMotionControl::enablePidRaw(const PidControlTypeEnum& pidtype, int j)
1910 {
1911  return DEPRECATED("enablePidRaw");
1912 }
1913 
1914 bool embObjMotionControl::setPidOffsetRaw(const PidControlTypeEnum& pidtype, int j, double v)
1915 {
1916  return NOT_YET_IMPLEMENTED("setOffset");
1917 }
1918 
1920 // Velocity control interface raw //
1922 
1924 {
1925  int mode=0;
1926  getControlModeRaw(j, &mode);
1927  if( (mode != VOCAB_CM_VELOCITY) &&
1928  (mode != VOCAB_CM_MIXED) &&
1929  (mode != VOCAB_CM_IMPEDANCE_VEL) &&
1930  (mode != VOCAB_CM_IDLE))
1931  {
1932  if(event_downsampler->canprint())
1933  {
1934  yError() << "velocityMoveRaw: skipping command because " << getBoardInfo() << " joint " << j << " is not in VOCAB_CM_VELOCITY mode";
1935  }
1936  return true;
1937  }
1938 
1939  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
1940 
1941  _ref_command_speeds[j] = sp ; // save internally the new value of speed.
1942 
1943  eOmc_setpoint_t setpoint;
1944  setpoint.type = eomc_setpoint_velocity;
1945  setpoint.to.velocity.value = (eOmeas_velocity_t) S_32(_ref_command_speeds[j]);
1946  setpoint.to.velocity.withacceleration = (eOmeas_acceleration_t) S_32(_ref_accs[j]);
1947 
1948 
1949  if(false == res->setRemoteValue(protid, &setpoint))
1950  {
1951  yError() << "while setting velocity mode";
1952  return false;
1953  }
1954  return true;
1955 }
1956 
1958 {
1959  bool ret = true;
1960  eOmc_setpoint_t setpoint;
1961 
1962  setpoint.type = eomc_setpoint_velocity;
1963 
1964  for(int j=0; j<_njoints; j++)
1965  {
1966  ret = velocityMoveRaw(j, sp[j]) && ret;
1967  }
1968 
1969  return ret;
1970 }
1971 
1972 
1974 // Calibration control interface //
1976 
1977 bool embObjMotionControl::setCalibrationParametersRaw(int j, const CalibrationParameters& params)
1978 {
1979  yTrace() << "setCalibrationParametersRaw for " << getBoardInfo() << "joint" << j;
1980 
1981  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_calibration);
1982  eOmc_calibrator_t calib;
1983  memset(&calib, 0x00, sizeof(calib));
1984  calib.type = params.type;
1985 
1986  switch (calib.type)
1987  {
1988  // muove -> amp+pid, poi calib
1989  case eomc_calibration_type0_hard_stops:
1990  calib.params.type0.pwmlimit = (int16_t)S_16(params.param1);
1991  calib.params.type0.velocity = (eOmeas_velocity_t)S_32(params.param2);
1992  calib.params.type0.calibrationZero = (int32_t)S_32(_measureConverter->posA2E(params.paramZero, j));
1993  break;
1994 
1995  // fermo
1996  case eomc_calibration_type1_abs_sens_analog:
1997  calib.params.type1.position = (int16_t)S_16(params.param1);
1998  calib.params.type1.velocity = (eOmeas_velocity_t)S_32(params.param2);
1999  calib.params.type1.calibrationZero = (int32_t)S_32(_measureConverter->posA2E(params.paramZero, j));
2000  break;
2001 
2002  // muove
2003  case eomc_calibration_type2_hard_stops_diff:
2004  calib.params.type2.pwmlimit = (int16_t)S_16(params.param1);
2005  calib.params.type2.velocity = (eOmeas_velocity_t)S_32(params.param2);
2006  calib.params.type2.calibrationZero = (int32_t)S_32(_measureConverter->posA2E(params.paramZero, j));
2007  break;
2008 
2009  // muove
2010  case eomc_calibration_type3_abs_sens_digital:
2011  calib.params.type3.position = (int16_t)S_16(params.param1);
2012  calib.params.type3.velocity = (eOmeas_velocity_t)S_32(params.param2);
2013  calib.params.type3.offset = (int32_t)S_32(params.param3);
2014  calib.params.type3.calibrationZero = (int32_t)S_32(_measureConverter->posA2E(params.paramZero, j));
2015  break;
2016 
2017  // muove
2018  case eomc_calibration_type4_abs_and_incremental:
2019  calib.params.type4.position = (int16_t)S_16(params.param1);
2020  calib.params.type4.velocity = (eOmeas_velocity_t)S_32(params.param2);
2021  calib.params.type4.maxencoder = (int32_t)S_32(params.param3);
2022  calib.params.type4.calibrationZero = (int32_t)S_32(_measureConverter->posA2E(params.paramZero, j));
2023  break;
2024 
2025  // muove
2026  case eomc_calibration_type5_hard_stops:
2027  calib.params.type5.pwmlimit = (int32_t) S_32(params.param1);
2028  calib.params.type5.final_pos = (int32_t) S_32(params.param2);
2029  calib.params.type5.calibrationZero = (int32_t)S_32(_measureConverter->posA2E(params.paramZero, j));
2030  break;
2031 
2032  // muove
2033  case eomc_calibration_type6_mais:
2034  calib.params.type6.position = (int32_t)S_32(params.param1);
2035  calib.params.type6.velocity = (int32_t)S_32(params.param2);
2036  calib.params.type6.current = (int32_t)S_32(params.param3);
2037  calib.params.type6.vmin = (int32_t)S_32(params.param4);
2038  calib.params.type6.vmax = (int32_t)S_32(params.param5);
2039  calib.params.type6.calibrationZero = (int32_t)S_32(_measureConverter->posA2E(params.paramZero, j));
2040  break;
2041 
2042  // muove
2043  case eomc_calibration_type7_hall_sensor:
2044  calib.params.type7.position = (int32_t)S_32(params.param1);
2045  calib.params.type7.velocity = (int32_t)S_32(params.param2);
2046  //param3 is not used
2047  calib.params.type7.vmin = (int32_t)S_32(params.param4);
2048  calib.params.type7.vmax = (int32_t)S_32(params.param5);
2049  calib.params.type7.calibrationZero = (int32_t)S_32(_measureConverter->posA2E(params.paramZero, j));
2050  break;
2051 
2052  //muove
2053  case eomc_calibration_type8_tripod_internal_hard_stop:
2054  calib.params.type8.pwmlimit = (int32_t) S_32(params.param1);
2055  calib.params.type8.max_delta = (int32_t) S_32(params.param2);
2056  calib.params.type8.calibrationZero = (int32_t)S_32(params.paramZero /* * _angleToEncoder[j] */);
2057  break;
2058 
2059  case eomc_calibration_type9_tripod_external_hard_stop:
2060  calib.params.type9.pwmlimit = (int32_t) S_32(params.param1);
2061  calib.params.type9.max_delta = (int32_t) S_32(params.param2);
2062  calib.params.type9.calibrationZero = (int32_t)S_32(params.paramZero /* * _angleToEncoder[j] */);
2063  break;
2064 
2065  case eomc_calibration_type10_abs_hard_stop:
2066  calib.params.type10.pwmlimit = (int32_t) S_32(params.param1);
2067  calib.params.type10.calibrationZero = (int32_t)S_32(_measureConverter->posA2E(params.paramZero, j));
2068  break;
2069 
2070  case eomc_calibration_type11_cer_hands:
2071  calib.params.type11.offset0 = (int32_t)S_32(params.param1);
2072  calib.params.type11.offset1 = (int32_t)S_32(params.param2);
2073  calib.params.type11.offset2 = (int32_t)S_32(params.param3);
2074  calib.params.type11.cable_range = (int32_t)S_32(params.param4);
2075  calib.params.type11.pwm = (int32_t)S_32(params.param5);
2076  //calib.params.type11.calibrationZero = 32767;//(int32_t)S_32(params.paramZero * _angleToEncoder[j]);
2077  break;
2078 
2079  case eomc_calibration_type12_absolute_sensor:
2080  calib.params.type12.rawValueAtZeroPos = (int32_t)S_32(params.param1);
2081  calib.params.type12.calibrationDelta = (int32_t)S_32(_measureConverter->posA2E(params.paramZero, j));
2082  break;
2083 
2084  case eomc_calibration_type13_cer_hands_2:
2085  calib.params.type13.rawValueAtZeroPos0 = (int32_t)S_32(params.param1);
2086  calib.params.type13.rawValueAtZeroPos1 = (int32_t)S_32(params.param2);
2087  calib.params.type13.rawValueAtZeroPos2 = (int32_t)S_32(params.param3);
2088  calib.params.type13.rawValueAtZeroPos3 = (int32_t)S_32(params.param4);
2089  break;
2090 
2091  default:
2092  yError() << "Calibration type unknown!! (embObjMotionControl)\n";
2093  return false;
2094  break;
2095  }
2096 
2097  if (false == res->setRemoteValue(protid, &calib))
2098  {
2099  yError() << "while setting velocity mode";
2100  return false;
2101  }
2102 
2103  _calibrated[j] = true;
2104 
2105  return true;
2106 }
2107 
2108 bool embObjMotionControl::calibrateAxisWithParamsRaw(int j, unsigned int type, double p1, double p2, double p3)
2109 {
2110  yTrace() << "calibrateRaw for" << getBoardInfo() << "joint" << j;
2111 
2112  // Tenere il check o forzare questi sottostati?
2113 // if(!_enabledAmp[j ] )
2114 // {
2115 // yWarning () << "Called calibrate for joint " << j << "with PWM(AMP) not enabled, forcing it!!";
2116 // // return false;
2117 // }
2118 
2119 // if(!_enabledPid[j ])
2120 // {
2121 // yWarning () << "Called calibrate for joint " << j << "with PID not enabled, forcing it!!";
2122 // // return false;
2123 // }
2124 
2125  // There is no explicit command "go to calibration mode" but it is implicit in the calibration command
2126 
2127 
2128  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_calibration);
2129  eOmc_calibrator_t calib;
2130  memset(&calib, 0x00, sizeof(calib));
2131  calib.type = type;
2132 
2133  switch(type)
2134  {
2135  // muove -> amp+pid, poi calib
2136  case eomc_calibration_type0_hard_stops:
2137  calib.params.type0.pwmlimit = (int16_t) S_16(p1);
2138  calib.params.type0.velocity = (eOmeas_velocity_t) S_32(p2);
2139  break;
2140 
2141  // fermo
2142  case eomc_calibration_type1_abs_sens_analog:
2143  calib.params.type1.position = (int16_t) S_16(p1);
2144  calib.params.type1.velocity = (eOmeas_velocity_t) S_32(p2);
2145  break;
2146 
2147  // muove
2148  case eomc_calibration_type2_hard_stops_diff:
2149  calib.params.type2.pwmlimit = (int16_t) S_16(p1);
2150  calib.params.type2.velocity = (eOmeas_velocity_t) S_32(p2);
2151  break;
2152 
2153  // muove
2154  case eomc_calibration_type3_abs_sens_digital:
2155  calib.params.type3.position = (int16_t) S_16(p1);
2156  calib.params.type3.velocity = (eOmeas_velocity_t) S_32(p2);
2157  calib.params.type3.offset = (int32_t) S_32(p3);
2158  break;
2159 
2160  // muove
2161  case eomc_calibration_type4_abs_and_incremental:
2162  calib.params.type4.position = (int16_t) S_16(p1);
2163  calib.params.type4.velocity = (eOmeas_velocity_t) S_32(p2);
2164  calib.params.type4.maxencoder = (int32_t) S_32(p3);
2165  break;
2166 
2167  default:
2168  yError () << "Calibration type unknown!! (embObjMotionControl)\n";
2169  return false;
2170  break;
2171  }
2172 
2173  if(false == res->setRemoteValue(protid, &calib))
2174  {
2175  yError() << "while setting velocity mode";
2176  return false;
2177  }
2178 
2179  _calibrated[j ] = true;
2180 
2181  return true;
2182 }
2183 
2184 
2186 {
2187  bool result = false;
2188  eOenum08_t temp = 0;
2189  uint16_t size = 0;
2190  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_status_core_modes_controlmodestatus);
2191  if(false == askRemoteValue(id32, &temp, size))
2192  {
2193  yError () << "Failure of askRemoteValue() inside embObjMotionControl::doneRaw(axis=" << axis << ") for " << getBoardInfo();
2194  return false;
2195  }
2196 
2197  eOmc_controlmode_t type = (eOmc_controlmode_t) temp;
2198 
2199 
2200  // if the control mode is no longer a calibration type, it means calibration ended
2201  if (eomc_controlmode_idle == type)
2202  {
2203  result = false;
2204  }
2205  else if (eomc_controlmode_calib == type)
2206  {
2207  result = false;
2208  }
2209  else if (eomc_controlmode_hwFault == type)
2210  {
2211  yError("unable to complete calibration: joint %d in 'hw_fault status' inside doneRaw() function", axis);
2212  result = false;
2213  }
2214  else if (eomc_controlmode_notConfigured == type)
2215  {
2216  yError("unable to complete calibration: joint %d in 'not_configured' status inside doneRaw() function", axis);
2217  result = false;
2218  }
2219  else if (eomc_controlmode_unknownError == type)
2220  {
2221  yError("unable to complete calibration: joint %d in 'unknownError' status inside doneRaw() function", axis);
2222  result = false;
2223  }
2224  else if (eomc_controlmode_configured == type)
2225  {
2226  yError("unable to complete calibration: joint %d in 'configured' status inside doneRaw() function", axis);
2227  result = false;
2228  }
2229  else
2230  {
2231  result = true;
2232  }
2233  return result;
2234 }
2235 
2237 // Position control interface //
2239 
2241 {
2242  *ax=_njoints;
2243 
2244  return true;
2245 }
2246 
2248 {
2249  if (yarp::os::Time::now()-_last_position_move_time[j]<MAX_POSITION_MOVE_INTERVAL)
2250  {
2251  yWarning() << "Performance warning: You are using positionMove commands at high rate (<"<< MAX_POSITION_MOVE_INTERVAL*1000.0 <<" ms). Probably position control mode is not the right control mode to use.";
2252  }
2253  _last_position_move_time[j] = yarp::os::Time::now();
2254 
2255  int mode = 0;
2256  getControlModeRaw(j, &mode);
2257  if( (mode != VOCAB_CM_POSITION) &&
2258  (mode != VOCAB_CM_MIXED) &&
2259  (mode != VOCAB_CM_IMPEDANCE_POS) &&
2260  (mode != VOCAB_CM_IDLE))
2261  {
2262  if (event_downsampler->canprint())
2263  {
2264  yError() << "positionMoveRaw: skipping command because " << getBoardInfo() << " joint " << j << " is not in VOCAB_CM_POSITION mode";
2265  }
2266  return true;
2267  }
2268 
2269  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
2270  _ref_command_positions[j] = ref; // save internally the new value of pos.
2271 
2272  eOmc_setpoint_t setpoint;
2273 
2274  setpoint.type = (eOenum08_t) eomc_setpoint_position;
2275  setpoint.to.position.value = (eOmeas_position_t) S_32(_ref_command_positions[j]);
2276  setpoint.to.position.withvelocity = (eOmeas_velocity_t) S_32(_ref_speeds[j]);
2277 
2278  return res->setRemoteValue(protid, &setpoint);
2279 }
2280 
2282 {
2283  bool ret = true;
2284 
2285  for(int j=0, index=0; j< _njoints; j++, index++)
2286  {
2287  ret &= positionMoveRaw(j, refs[index]);
2288  }
2289  return ret;
2290 }
2291 
2292 bool embObjMotionControl::relativeMoveRaw(int j, double delta)
2293 {
2294  return NOT_YET_IMPLEMENTED("positionRelative");
2295 }
2296 
2297 bool embObjMotionControl::relativeMoveRaw(const double *deltas)
2298 {
2299  return NOT_YET_IMPLEMENTED("positionRelative");
2300 }
2301 
2302 
2304 {
2305  eObool_t ismotiondone = eobool_false;
2306  uint16_t size = 0;
2307 
2308  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core_modes_ismotiondone);
2309  if(false == askRemoteValue(id32, &ismotiondone, size))
2310  {
2311  yError () << "Failure of askRemoteValue() inside embObjMotionControl::checkMotionDoneRaw(j=" << j << ") for " << getBoardInfo();
2312  return false;
2313  }
2314 
2315 
2316  *flag = ismotiondone; // eObool_t can have values only amongst: eobool_true (1) or eobool_false (0).
2317 
2318  return true;
2319 }
2320 
2322 {
2323  std::vector <eObool_t> ismotiondoneList(_njoints);
2324  bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_core_modes_ismotiondone, ismotiondoneList);
2325  if(false == ret)
2326  {
2327  yError () << "Failure of askRemoteValues() inside embObjMotionControl::checkMotionDoneRaw for all joints of" << getBoardInfo();
2328  return false;
2329  }
2330  *flag=true;
2331  for(int j=0; j<_njoints; j++)
2332  {
2333  *flag &= ismotiondoneList[j]; // eObool_t can have values only amongst: eobool_true (1) or eobool_false (0).
2334  }
2335  return true;
2336 }
2337 
2339 {
2340  // Velocity is expressed in iDegrees/s
2341  // save internally the new value of speed; it'll be used in the positionMove
2342  int index = j ;
2343  _ref_speeds[index] = sp;
2344  return true;
2345 }
2346 
2348 {
2349  // Velocity is expressed in iDegrees/s
2350  // save internally the new value of speed; it'll be used in the positionMove
2351  for(int j=0, index=0; j< _njoints; j++, index++)
2352  {
2353  _ref_speeds[index] = spds[index];
2354  }
2355  return true;
2356 }
2357 
2359 {
2360  // Acceleration is expressed in iDegrees/s^2
2361  // save internally the new value of the acceleration; it'll be used in the velocityMove command
2362 
2363  if (acc > 1e6)
2364  {
2365  _ref_accs[j ] = 1e6;
2366  }
2367  else if (acc < -1e6)
2368  {
2369  _ref_accs[j ] = -1e6;
2370  }
2371  else
2372  {
2373  _ref_accs[j ] = acc;
2374  }
2375 
2376  return true;
2377 }
2378 
2380 {
2381  // Acceleration is expressed in iDegrees/s^2
2382  // save internally the new value of the acceleration; it'll be used in the velocityMove command
2383  for(int j=0, index=0; j< _njoints; j++, index++)
2384  {
2385  if (accs[j] > 1e6)
2386  {
2387  _ref_accs[index] = 1e6;
2388  }
2389  else if (accs[j] < -1e6)
2390  {
2391  _ref_accs[index] = -1e6;
2392  }
2393  else
2394  {
2395  _ref_accs[index] = accs[j];
2396  }
2397  }
2398  return true;
2399 }
2400 
2401 bool embObjMotionControl::getRefSpeedRaw(int j, double *spd)
2402 {
2403  if (j<0 || j>_njoints) return false;
2404 #if ASK_REFERENCE_TO_FIRMWARE
2405  *spd = _ref_speeds[j];
2406  //return NOT_YET_IMPLEMENTED("getRefSpeedRaw");
2407 #else
2408  *spd = _ref_speeds[j];
2409 #endif
2410  return true;
2411 }
2412 
2414 {
2415  memcpy(spds, _ref_speeds, sizeof(double) * _njoints);
2416  return true;
2417 }
2418 
2420 {
2421  *acc = _ref_accs[j];
2422  return true;
2423 }
2424 
2426 {
2427  memcpy(accs, _ref_accs, sizeof(double) * _njoints);
2428  return true;
2429 }
2430 
2432 {
2433  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_stoptrajectory);
2434 
2435  eObool_t stop = eobool_true;
2436 
2437  return res->setRemoteValue(protid, &stop);
2438 }
2439 
2441 {
2442  bool ret = true;
2443  for(int j=0; j< _njoints; j++)
2444  {
2445  ret &= stopRaw(j);
2446  }
2447  return ret;
2448 }
2450 
2452 // Position control2 interface //
2454 
2455 bool embObjMotionControl::positionMoveRaw(const int n_joint, const int *joints, const double *refs)
2456 {
2457  bool ret = true;
2458  for(int j=0; j<n_joint; j++)
2459  {
2460  ret = ret &&positionMoveRaw(joints[j], refs[j]);
2461  }
2462  return ret;
2463 }
2464 
2465 bool embObjMotionControl::relativeMoveRaw(const int n_joint, const int *joints, const double *deltas)
2466 {
2467  bool ret = true;
2468  for(int j=0; j<n_joint; j++)
2469  {
2470  ret = ret &&relativeMoveRaw(joints[j], deltas[j]);
2471  }
2472  return ret;
2473 }
2474 
2475 bool embObjMotionControl::checkMotionDoneRaw(const int n_joint, const int *joints, bool *flag)
2476 {
2477 
2478  //1) first of all, check if all joints number are ok
2479  for(int j=0; j<n_joint; j++)
2480  {
2481  if(joints[j] >= _njoints)
2482  {
2483  yError() << getBoardInfo() << ":checkMotionDoneRaw required for not existing joint ( " << joints[j] << ")";
2484  return false;
2485  }
2486  }
2487 
2488  //2) ask check motion done for all my joints
2489  std::vector <eObool_t> ismotiondoneList(_njoints);
2490  bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_core_modes_ismotiondone, ismotiondoneList);
2491  if(false == ret)
2492  {
2493  yError () << getBoardInfo() << "Failure of askRemoteValues() inside embObjMotionControl::checkMotionDoneRaw for a group of joint"; getBoardInfo();
2494  return false;
2495  }
2496 
2497  //3) verify only the given joints
2498  bool tot_val = true;
2499  for(int j=0; j<n_joint; j++)
2500  {
2501  tot_val &= ismotiondoneList[joints[j]];
2502  }
2503 
2504  *flag = tot_val;
2505  return true;
2506 
2507 }
2508 
2509 bool embObjMotionControl::setRefSpeedsRaw(const int n_joint, const int *joints, const double *spds)
2510 {
2511  bool ret = true;
2512  for(int j=0; j<n_joint; j++)
2513  {
2514  ret = ret &&setRefSpeedRaw(joints[j], spds[j]);
2515  }
2516  return ret;
2517 }
2518 
2519 bool embObjMotionControl::setRefAccelerationsRaw(const int n_joint, const int *joints, const double *accs)
2520 {
2521  bool ret = true;
2522  for(int j=0; j<n_joint; j++)
2523  {
2524  ret = ret &&setRefAccelerationRaw(joints[j], accs[j]);
2525  }
2526  return ret;
2527 }
2528 
2529 bool embObjMotionControl::getRefSpeedsRaw(const int n_joint, const int *joints, double *spds)
2530 {
2531  bool ret = true;
2532  for(int j=0; j<n_joint; j++)
2533  {
2534  ret = ret && getRefSpeedRaw(joints[j], &spds[j]);
2535  }
2536  return ret;
2537 }
2538 
2539 bool embObjMotionControl::getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs)
2540 {
2541  bool ret = true;
2542  for(int j=0; j<n_joint; j++)
2543  {
2544  ret = ret && getRefAccelerationRaw(joints[j], &accs[j]);
2545  }
2546  return ret;
2547 }
2548 
2549 bool embObjMotionControl::stopRaw(const int n_joint, const int *joints)
2550 {
2551  bool ret = true;
2552  for(int j=0; j<n_joint; j++)
2553  {
2554  ret = ret &&stopRaw(joints[j]);
2555  }
2556  return ret;
2557 }
2558 
2560 
2561 // ControlMode
2562 
2564 {
2565  eOmc_joint_status_core_t jcore = {0};
2566  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
2567  if(! res->getLocalValue(protid, &jcore))
2568  return false;
2569 
2570  eOmc_controlmode_t type = (eOmc_controlmode_t) jcore.modes.controlmodestatus;
2571 
2573  return true;
2574 }
2575 
2576 // IControl Mode 2
2578 {
2579  bool ret = true;
2580  for(int j=0; j< _njoints; j++)
2581  {
2582  ret = ret && getControlModeRaw(j, &v[j]);
2583  }
2584  return ret;
2585 }
2586 
2587 bool embObjMotionControl::getControlModesRaw(const int n_joint, const int *joints, int *modes)
2588 {
2589  bool ret = true;
2590  for(int j=0; j< n_joint; j++)
2591  {
2592  ret = ret && getControlModeRaw(joints[j], &modes[j]);
2593  }
2594  return ret;
2595 }
2596 
2597 
2598 
2599 // marco.accame: con alberto cardellino abbiamo parlato della correttezza di effettuare la verifica di quanto imposto (in setControlModeRaw() ed affini)
2600 // andando a rileggere il valore nella scheda eth fino a che esso non sia quello atteso. si deve fare oppure no?
2601 // con il control mode il can ora lo fa ma e' giusto? era cosi' anche in passato?
2602 bool embObjMotionControl::setControlModeRaw(const int j, const int _mode)
2603 {
2604  bool ret = true;
2605  eOenum08_t controlmodecommand = 0;
2606 
2607  if((_mode == VOCAB_CM_TORQUE) && (_trq_pids[j].enabled == false))
2608  {
2609  yError()<<"Torque control is disabled. Check your configuration parameters";
2610  return false;
2611  }
2612 
2613  if(!controlModeCommandConvert_yarp2embObj(_mode, controlmodecommand) )
2614  {
2615  yError() << "SetControlMode: received unknown control mode for " << getBoardInfo() << " joint " << j << " mode " << Vocab32::decode(_mode);
2616  return false;
2617  }
2618 
2619  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_controlmode);
2620  if(false == res->setRemoteValue(protid, &controlmodecommand) )
2621  {
2622  yError() << "setControlModeRaw failed for " << getBoardInfo() << " joint " << j << " mode " << Vocab32::decode(_mode);
2623  return false;
2624  }
2625 
2626 
2627  ret = checkRemoteControlModeStatus(j, _mode);
2628 
2629  if(false == ret)
2630  {
2631  yError() << "In embObjMotionControl::setControlModeRaw(j=" << j << ", mode=" << yarp::os::Vocab32::decode(_mode).c_str() << ") for " << getBoardInfo() << " has failed checkRemoteControlModeStatus()";
2632  }
2633 
2634  return ret;
2635 }
2636 
2637 
2638 bool embObjMotionControl::setControlModesRaw(const int n_joint, const int *joints, int *modes)
2639 {
2640  bool ret = true;
2641  eOenum08_t controlmodecommand = 0;
2642 
2643 
2644  for(int i=0; i<n_joint; i++)
2645  {
2646  if ((modes[i] == VOCAB_CM_TORQUE) && (_trq_pids[i].enabled == false)) {yError()<<"Torque control is disabled. Check your configuration parameters"; continue;}
2647 
2648  if(!controlModeCommandConvert_yarp2embObj(modes[i], controlmodecommand) )
2649  {
2650  yError() << "SetControlModesRaw(): received unknown control mode for " << getBoardInfo() << " joint " << joints[i] << " mode " << Vocab32::decode(modes[i]);
2651 
2652  return false;
2653  }
2654 
2655  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joints[i], eoprot_tag_mc_joint_cmmnds_controlmode);
2656  if(false == res->setRemoteValue(protid, &controlmodecommand) )
2657  {
2658  yError() << "setControlModesRaw() could not send set<cmmnds_controlmode> for " << getBoardInfo() << " joint " << joints[i] << " mode " << Vocab32::decode(modes[i]);
2659 
2660  return false;
2661  }
2662 
2663  bool tmpresult = checkRemoteControlModeStatus(joints[i], modes[i]);
2664  if(false == tmpresult)
2665  {
2666  yError() << "setControlModesRaw(const int n_joint, const int *joints, int *modes) could not check with checkRemoteControlModeStatus() for " << getBoardInfo() << " joint " << joints[i] << " mode " << Vocab32::decode(modes[i]);
2667  }
2668 
2669  ret = ret && tmpresult;
2670 
2671  }
2672 
2673  return ret;
2674 }
2675 
2677 {
2678  bool ret = true;
2679  eOenum08_t controlmodecommand = 0;
2680 
2681  for(int i=0; i<_njoints; i++)
2682  {
2683 
2684  if ((modes[i] == VOCAB_CM_TORQUE) && (_trq_pids[i].enabled == false))
2685  {
2686  yError()<<"Torque control is disabled. Check your configuration parameters";
2687  continue;
2688  }
2689 
2690  if(!controlModeCommandConvert_yarp2embObj(modes[i], controlmodecommand) )
2691  {
2692  yError() << "SetControlMode: received unknown control mode for" << getBoardInfo() << " joint " << i << " mode " << Vocab32::decode(modes[i]);
2693  return false;
2694  }
2695 
2696  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, i, eoprot_tag_mc_joint_cmmnds_controlmode);
2697  if(false == res->setRemoteValue(protid, &controlmodecommand) )
2698  {
2699  yError() << "setControlModesRaw failed for " << getBoardInfo() << " joint " << i << " mode " << Vocab32::decode(modes[i]);
2700  return false;
2701  }
2702 
2703  bool tmpresult = checkRemoteControlModeStatus(i, modes[i]);
2704  if(false == tmpresult)
2705  {
2706  yError() << "setControlModesRaw(int *modes) could not check with checkRemoteControlModeStatus() for" << getBoardInfo() << " joint " << i << " mode " << Vocab32::decode(modes[i]);
2707  }
2708 
2709  ret = ret && tmpresult;
2710 
2711  }
2712 
2713 
2714  return ret;
2715 }
2716 
2717 
2719 
2720 bool embObjMotionControl::setEncoderRaw(int j, double val)
2721 {
2722  return NOT_YET_IMPLEMENTED("setEncoder");
2723 }
2724 
2725 bool embObjMotionControl::setEncodersRaw(const double *vals)
2726 {
2727  return NOT_YET_IMPLEMENTED("setEncoders");
2728 }
2729 
2731 {
2732  return NOT_YET_IMPLEMENTED("resetEncoder");
2733 }
2734 
2736 {
2737  return NOT_YET_IMPLEMENTED("resetEncoders");
2738 }
2739 
2740 bool embObjMotionControl::getEncoderRaw(int j, double *value)
2741 {
2742  eOmc_joint_status_core_t core;
2743  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
2744 
2745  bool ret = res->getLocalValue(protid, &core);
2746 
2747  if(ret)
2748  {
2749  *value = (double) core.measures.meas_position;
2750  }
2751  else
2752  {
2753  yError() << "embObjMotionControl while reading encoder";
2754  *value = 0;
2755  }
2756 
2757  return ret;
2758 }
2759 
2761 {
2762  bool ret = true;
2763  for(int j=0; j< _njoints; j++)
2764  {
2765  ret &= getEncoderRaw(j, &encs[j]);
2766 
2767  }
2768  return ret;
2769 }
2770 
2772 {
2773  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
2774  eOmc_joint_status_core_t core;
2775  *sp = 0;
2776  if(!res->getLocalValue(protid, &core))
2777  {
2778  return false;
2779  }
2780  // extract requested data from status
2781  *sp = (double) core.measures.meas_velocity;
2782  return true;
2783 }
2784 
2786 {
2787  bool ret = true;
2788  for(int j=0; j< _njoints; j++)
2789  {
2790  ret &= getEncoderSpeedRaw(j, &spds[j]);
2791  }
2792  return ret;
2793 }
2794 
2796 {
2797  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
2798  eOmc_joint_status_core_t core;
2799  *acc = 0;
2800  if(! res->getLocalValue(protid, &core))
2801  {
2802  return false;
2803  }
2804  *acc = (double) core.measures.meas_acceleration;
2805  return true;
2806 }
2807 
2809 {
2810  bool ret = true;
2811  for(int j=0; j< _njoints; j++)
2812  {
2813  ret &= getEncoderAccelerationRaw(j, &accs[j]);
2814  }
2815  return ret;
2816 }
2817 
2819 
2820 bool embObjMotionControl::getEncodersTimedRaw(double *encs, double *stamps)
2821 {
2822  bool ret = getEncodersRaw(encs);
2823  std::lock_guard<std::mutex> lck(_mutex);
2824  for(int i=0; i<_njoints; i++)
2825  stamps[i] = _encodersStamp[i];
2826  return ret;
2827 }
2828 
2829 bool embObjMotionControl::getEncoderTimedRaw(int j, double *encs, double *stamp)
2830 {
2831  bool ret = getEncoderRaw(j, encs);
2832  std::lock_guard<std::mutex> lck(_mutex);
2833  *stamp = _encodersStamp[j];
2834  return ret;
2835 }
2836 
2838 
2840 {
2841  *num=_njoints;
2842  return true;
2843 }
2844 
2845 bool embObjMotionControl::setMotorEncoderRaw(int m, const double val)
2846 {
2847  return NOT_YET_IMPLEMENTED("setMotorEncoder");
2848 }
2849 
2851 {
2852  return NOT_YET_IMPLEMENTED("setMotorEncoders");
2853 }
2854 
2856 {
2857  return NOT_YET_IMPLEMENTED("setMotorEncoderCountsPerRevolutionRaw");
2858 }
2859 
2861 {
2862  return NOT_YET_IMPLEMENTED("getMotorEncoderCountsPerRevolutionRaw");
2863 }
2864 
2866 {
2867  return NOT_YET_IMPLEMENTED("resetMotorEncoder");
2868 }
2869 
2871 {
2872  return NOT_YET_IMPLEMENTED("reseMotortEncoders");
2873 }
2874 
2876 {
2877  eOmc_motor_status_basic_t status;
2878  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);
2879 
2880  bool ret = res->getLocalValue(protid, &status);
2881  if(ret)
2882  {
2883  *value = (double) status.mot_position;
2884  }
2885  else
2886  {
2887  yError() << "embObjMotionControl while reading motor encoder position";
2888  *value = 0;
2889  }
2890 
2891  return ret;
2892 }
2893 
2895 {
2896  bool ret = true;
2897  for(int j=0; j< _njoints; j++)
2898  {
2899  ret &= getMotorEncoderRaw(j, &encs[j]);
2900 
2901  }
2902  return ret;
2903 }
2904 
2906 {
2907  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);
2908  eOmc_motor_status_basic_t tmpMotorStatus;
2909  bool ret = res->getLocalValue(protid, &tmpMotorStatus);
2910  if(ret)
2911  {
2912  *sp = (double) tmpMotorStatus.mot_velocity;
2913  }
2914  else
2915  {
2916  yError() << "embObjMotionControl while reading motor encoder speed";
2917  *sp = 0;
2918  }
2919  return true;
2920 }
2921 
2923 {
2924  bool ret = true;
2925  for(int j=0; j< _njoints; j++)
2926  {
2927  ret &= getMotorEncoderSpeedRaw(j, &spds[j]);
2928  }
2929  return ret;
2930 }
2931 
2933 {
2934  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);
2935  eOmc_motor_status_basic_t tmpMotorStatus;
2936  bool ret = res->getLocalValue(protid, &tmpMotorStatus);
2937  if(ret)
2938  {
2939  *acc = (double) tmpMotorStatus.mot_acceleration;
2940  }
2941  else
2942  {
2943  yError() << "embObjMotionControl while reading motor encoder acceleration";
2944  *acc = 0;
2945  }
2946  return true;
2947 }
2948 
2950 {
2951  bool ret = true;
2952  for(int j=0; j< _njoints; j++)
2953  {
2954  ret &= getMotorEncoderAccelerationRaw(j, &accs[j]);
2955  }
2956  return ret;
2957 }
2958 
2960 {
2961  bool ret = getMotorEncodersRaw(encs);
2962  std::lock_guard<std::mutex> lck(_mutex);
2963  for(int i=0; i<_njoints; i++)
2964  stamps[i] = _encodersStamp[i];
2965  return ret;
2966 }
2967 
2968 bool embObjMotionControl::getMotorEncoderTimedRaw(int m, double *encs, double *stamp)
2969 {
2970  bool ret = getMotorEncoderRaw(m, encs);
2971  std::lock_guard<std::mutex> lck(_mutex);
2972  *stamp = _encodersStamp[m];
2973  return ret;
2974 }
2976 
2978 
2980 {
2981  return DEPRECATED("enableAmpRaw");
2982 }
2983 
2985 {
2986  return DEPRECATED("disableAmpRaw");
2987 }
2988 
2989 bool embObjMotionControl::getCurrentRaw(int j, double *value)
2990 {
2991  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_status_basic);
2992  eOmc_motor_status_basic_t tmpMotorStatus;
2993  bool ret = res->getLocalValue(protid, &tmpMotorStatus);
2994 
2995  *value = (double) tmpMotorStatus.mot_current;
2996  return true;
2997 }
2998 
3000 {
3001  bool ret = true;
3002  for(int j=0; j< _njoints; j++)
3003  {
3004  ret &= getCurrentRaw(j, &vals[j]);
3005  }
3006  return ret;
3007 }
3008 
3010 {
3011  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_currentlimits);
3012  uint16_t size;
3013  eOmc_current_limits_params_t currentlimits = {0};
3014 
3015  if(!askRemoteValue(protid, &currentlimits, size))
3016  {
3017  yError() << "embObjMotionControl::setMaxCurrentRaw() could not read max current for " << getBoardInfo() << "joint " << j;
3018  return false;
3019  }
3020 
3021  //set current overload
3022  currentlimits.overloadCurrent = (eOmeas_current_t) S_16(val);
3023 
3024  //send new values
3025  return res->setRemoteValue(protid, &currentlimits);
3026 }
3027 
3029 {
3030  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_currentlimits);
3031  uint16_t size;
3032  eOmc_current_limits_params_t currentlimits = {0};
3033  *val = 0;
3034 
3035  if(!askRemoteValue(protid, &currentlimits, size))
3036  {
3037  yError() << "embObjMotionControl::getMaxCurrentRaw() could not read max current for " << getBoardInfo() << "joint " << j;
3038  return false;
3039  }
3040 
3041  *val = (double) currentlimits.overloadCurrent;
3042 
3043  return true;
3044 }
3045 
3047 {
3048  //VALE: can i set this func like deprecated? none sets _enabledAmp!!
3049  (_enabledAmp[j ]) ? *st = 1 : *st = 0;
3050  return true;
3051 }
3052 
3054 {
3055  bool ret = true;
3056  for(int j=0; j<_njoints; j++)
3057  {
3058  sts[j] = _enabledAmp[j];
3059  }
3060 
3061  return ret;
3062 }
3063 
3064 #ifdef IMPLEMENT_DEBUG_INTERFACE
3065 //----------------------------------------------\\
3066 // Debug interface
3067 //----------------------------------------------\\
3068 
3069 bool embObjMotionControl::setParameterRaw(int j, unsigned int type, double value) { return NOT_YET_IMPLEMENTED("setParameterRaw"); }
3070 bool embObjMotionControl::getParameterRaw(int j, unsigned int type, double* value) { return NOT_YET_IMPLEMENTED("getParameterRaw"); }
3071 bool embObjMotionControl::getDebugParameterRaw(int j, unsigned int index, double* value) { return NOT_YET_IMPLEMENTED("getDebugParameterRaw"); }
3072 bool embObjMotionControl::setDebugParameterRaw(int j, unsigned int index, double value) { return NOT_YET_IMPLEMENTED("setDebugParameterRaw"); }
3073 bool embObjMotionControl::setDebugReferencePositionRaw(int j, double value) { return NOT_YET_IMPLEMENTED("setDebugReferencePositionRaw"); }
3074 bool embObjMotionControl::getDebugReferencePositionRaw(int j, double* value) { return NOT_YET_IMPLEMENTED("getDebugReferencePositionRaw");}
3075 
3076 #endif //IMPLEMENT_DEBUG_INTERFACE
3077 
3078 // Limit interface
3079 bool embObjMotionControl::setLimitsRaw(int j, double min, double max)
3080 {
3081  bool ret = true;
3082  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_userlimits);
3083 
3084  eOmeas_position_limits_t limits;
3085  limits.max = (eOmeas_position_t) S_32(max);
3086  limits.min = (eOmeas_position_t) S_32(min);
3087 
3088  ret = res->setRemoteValue(protid, &limits);
3089 
3090 
3091  if(!ret)
3092  {
3093  yError() << "while setting position limits for joint" << j << " \n";
3094  }
3095  return ret;
3096 }
3097 
3098 bool embObjMotionControl::getLimitsRaw(int j, double *min, double *max)
3099 {
3100  eOmeas_position_limits_t limits;
3101  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_userlimits);
3102  uint16_t size;
3103 
3104  if(! askRemoteValue(protoid, &limits, size))
3105  return false;
3106 
3107  *min = (double)limits.min + SAFETY_THRESHOLD;
3108  *max = (double)limits.max - SAFETY_THRESHOLD;
3109  return true;
3110 }
3111 
3112 bool embObjMotionControl::getGearboxRatioRaw(int j, double *gearbox)
3113 {
3114  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3115  uint16_t size;
3116  eOmc_motor_config_t motor_cfg;
3117  if(! askRemoteValue(protoid, &motor_cfg, size))
3118  return false;
3119 
3120  // refresh cached value when reading data from the EMS
3121  *gearbox = (double)motor_cfg.gearbox_M2J;
3122 
3123  return true;
3124 }
3125 
3126 bool embObjMotionControl::getRotorLimitsRaw(int j, double *rotorMin, double *rotorMax)
3127 {
3128  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3129  uint16_t size;
3130  eOmc_motor_config_t motor_cfg;
3131  if(! askRemoteValue(protoid, &motor_cfg, size))
3132  return false;
3133  *rotorMax = (double)( motor_cfg.limitsofrotor.max);
3134  *rotorMin = (double)( motor_cfg.limitsofrotor.min);
3135  return true;
3136 }
3137 
3139 {
3140  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config);
3141  uint16_t size;
3142  eOmc_joint_config_t joint_cfg;
3143  if(! askRemoteValue(protoid, &joint_cfg, size))
3144  return false;
3145 
3146  // refresh cached value when reading data from the EMS
3147  type = (int)joint_cfg.tcfiltertype;
3148  return true;
3149 }
3150 
3152 {
3153  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3154  uint16_t size;
3155  eOmc_motor_config_t motor_cfg;
3156  if(! askRemoteValue(protoid, &motor_cfg, size))
3157  return false;
3158 
3159  // refresh cached value when reading data from the EMS
3160  rotres = (double)motor_cfg.rotorEncoderResolution;
3161 
3162  return true;
3163 }
3164 
3166 {
3167  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config);
3168  uint16_t size;
3169  eOmc_joint_config_t joint_cfg;
3170  if(! askRemoteValue(protoid, &joint_cfg, size))
3171  return false;
3172 
3173  // refresh cached value when reading data from the EMS
3174  jntres = (double)joint_cfg.jntEncoderResolution;
3175 
3176  return true;
3177 }
3178 
3180 {
3181  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config);
3182  uint16_t size;
3183  eOmc_joint_config_t joint_cfg;
3184  if(! askRemoteValue(protoid, &joint_cfg, size))
3185  return false;
3186 
3187  // refresh cached value when reading data from the EMS
3188  type = (int)joint_cfg.jntEncoderType;
3189 
3190  return true;
3191 }
3192 
3194 {
3195  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3196  uint16_t size;
3197  eOmc_motor_config_t motor_cfg;
3198  if(! askRemoteValue(protoid, &motor_cfg, size))
3199  return false;
3200 
3201  // refresh cached value when reading data from the EMS
3202  type = (int)motor_cfg.rotorEncoderType;
3203 
3204  return true;
3205 }
3206 
3207 bool embObjMotionControl::getKinematicMJRaw(int j, double &rotres)
3208 {
3209  yError("getKinematicMJRaw not yet implemented");
3210  return false;
3211 }
3212 
3214 {
3215  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3216  uint16_t size;
3217  eOmc_motor_config_t motor_cfg;
3218  if(! askRemoteValue(protoid, &motor_cfg, size))
3219  return false;
3220 
3221  // refresh cached value when reading data from the EMS
3222  ret = (int)motor_cfg.hasTempSensor;
3223 
3224  return true;
3225 }
3226 
3228 {
3229  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3230  uint16_t size;
3231  eOmc_motor_config_t motor_cfg;
3232  if(! askRemoteValue(protoid, &motor_cfg, size))
3233  return false;
3234 
3235  // refresh cached value when reading data from the EMS
3236  ret = (int)motor_cfg.hasHallSensor;
3237 
3238  return true;
3239 }
3240 
3242 {
3243  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3244  uint16_t size;
3245  eOmc_motor_config_t motor_cfg;
3246  if(! askRemoteValue(protoid, &motor_cfg, size))
3247  return false;
3248 
3249  // refresh cached value when reading data from the EMS
3250  ret = (int)motor_cfg.hasRotorEncoder;
3251 
3252  return true;
3253 }
3254 
3256 {
3257  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3258  uint16_t size;
3259  eOmc_motor_config_t motor_cfg;
3260  if(! askRemoteValue(protoid, &motor_cfg, size))
3261  return false;
3262 
3263  // refresh cached value when reading data from the EMS
3264  ret = (int)motor_cfg.hasRotorEncoderIndex;
3265 
3266  return true;
3267 }
3268 
3270 {
3271  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3272 
3273  uint16_t size;
3274  eOmc_motor_config_t motor_cfg;
3275  if(! askRemoteValue(protoid, &motor_cfg, size))
3276  return false;
3277 
3278  // refresh cached value when reading data from the EMS
3279  poles = (int)motor_cfg.motorPoles;
3280 
3281  return true;
3282 }
3283 
3284 bool embObjMotionControl::getRotorIndexOffsetRaw(int j, double& rotorOffset)
3285 {
3286  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3287  uint16_t size;
3288  eOmc_motor_config_t motor_cfg;
3289  if(! askRemoteValue(protoid, &motor_cfg, size))
3290  return false;
3291 
3292  // refresh cached value when reading data from the EMS
3293  rotorOffset = (double)motor_cfg.rotorIndexOffset;
3294 
3295  return true;
3296 }
3297 
3298 bool embObjMotionControl::getAxisNameRaw(int axis, std::string& name)
3299 {
3300  if (axis >= 0 && axis < _njoints)
3301  {
3302  name = _axesInfo[axis].name;
3303  return true;
3304  }
3305  else
3306  {
3307  name = "ERROR";
3308  return false;
3309  }
3310 }
3311 
3312 bool embObjMotionControl::getJointTypeRaw(int axis, yarp::dev::JointTypeEnum& type)
3313 {
3314  if (axis >= 0 && axis < _njoints)
3315  {
3316  type = _axesInfo[axis].type;
3317  return true;
3318  }
3319  else
3320  {
3321  return false;
3322  }
3323 }
3324 
3325 bool embObjMotionControl::getJointDeadZoneRaw(int j, double &jntDeadZone)
3326 {
3327  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config);
3328  uint16_t size;
3329  eOmc_joint_config_t joint_cfg;
3330  if(! askRemoteValue(protoid, &joint_cfg, size))
3331  return false;
3332 
3333  // refresh cached value when reading data from the EMS
3334  jntDeadZone = _measureConverter->posE2A((double)joint_cfg.deadzone, _axisMap[j]);
3335 
3336  return true;
3337 }
3338 
3339 // IRemoteVariables
3340 bool embObjMotionControl::getRemoteVariableRaw(std::string key, yarp::os::Bottle& val)
3341 {
3342  val.clear();
3343  if (key == "kinematic_mj")
3344  {
3345  // Return the reduced kinematic_mj matrix considering only the joints actually exposed to the user
3346  Bottle& ret = val.addList();
3347 
3348  eOmn_serv_type_t mc_serv_type = (eOmn_serv_type_t)serviceConfig.ethservice.configuration.type;
3349  if(iNeedCouplingsInfo())
3350  {
3351  for (int r=0; r<_njoints; r++)
3352  {
3353  for (int c = 0; c < _njoints; c++)
3354  {
3355  // matrixJ2M is stored as row major in the eomc_couplingInfo_t,
3356  // and kinematic_mj is returned as a row major serialization as well
3357  ret.addFloat64(_couplingInfo.matrixJ2M[4 * r + c]);
3358  }
3359  }
3360  }
3361  else
3362  {
3363  ret.addFloat64(0.0);
3364  }
3365  return true;
3366  }
3367  else if (key == "encoders")
3368  {
3369  Bottle& r = val.addList(); for (int i = 0; i < _njoints; i++) { r.addFloat64(_measureConverter->posA2E(1.0, i)); }
3370  return true;
3371  }
3372  else if (key == "rotorEncoderResolution")
3373  {
3374  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { double tmp = 0; getRotorEncoderResolutionRaw(i, tmp); r.addFloat64(tmp); }
3375  return true;
3376  }
3377  else if (key == "jointEncoderResolution")
3378  {
3379  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { double tmp = 0; getJointEncoderResolutionRaw(i, tmp); r.addFloat64(tmp); }
3380  return true;
3381  }
3382  else if (key == "gearbox_M2J")
3383  {
3384  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { double tmp=0; getGearboxRatioRaw(i, &tmp); r.addFloat64(tmp); }
3385  return true;
3386  }
3387  else if (key == "gearbox_E2J")
3388  {
3389  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { double tmp=0; getGerabox_E2J(i, &tmp); r.addFloat64(tmp); }
3390  return true;
3391  }
3392  else if (key == "hasHallSensor")
3393  {
3394  Bottle& r = val.addList(); for (int i = 0; i < _njoints; i++) { int tmp = 0; getHasHallSensorRaw(i, tmp); r.addInt32(tmp); }
3395  return true;
3396  }
3397  else if (key == "hasTempSensor")
3398  {
3399  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { int tmp = 0; getHasTempSensorsRaw(i, tmp); r.addInt32(tmp); }
3400  return true;
3401  }
3402  else if (key == "hasRotorEncoder")
3403  {
3404  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { int tmp = 0; getHasRotorEncoderRaw(i, tmp); r.addInt32(tmp); }
3405  return true;
3406  }
3407  else if (key == "hasRotorEncoderIndex")
3408  {
3409  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { int tmp = 0; getHasRotorEncoderIndexRaw(i, tmp); r.addInt32(tmp); }
3410  return true;
3411  }
3412  else if (key == "rotorIndexOffset")
3413  {
3414  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { double tmp = 0; getRotorIndexOffsetRaw(i, tmp); r.addFloat64(tmp); }
3415  return true;
3416  }
3417  else if (key == "motorPoles")
3418  {
3419  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { int tmp = 0; getMotorPolesRaw(i, tmp); r.addInt32(tmp); }
3420  return true;
3421  }
3422  else if (key == "pidCurrentKp")
3423  {
3424  Bottle& r = val.addList(); for (int i = 0; i < _njoints; i++) { Pid p; getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &p); r.addFloat64(p.kp); }
3425  return true;
3426  }
3427  else if (key == "pidCurrentKi")
3428  {
3429  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { Pid p; getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &p); r.addFloat64(p.ki); }
3430  return true;
3431  }
3432  else if (key == "pidCurrentShift")
3433  {
3434  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { Pid p; getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &p); r.addFloat64(p.scale); }
3435  return true;
3436  }
3437  else if (key == "pidCurrentOutput")
3438  {
3439  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { Pid p; getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &p); r.addFloat64(p.max_output); }
3440  return true;
3441  }
3442  else if (key == "jointEncoderType")
3443  {
3444  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++)
3445  {
3446  int t; string s;
3447  getJointEncoderTypeRaw(i, t); uint8_t tt = t; bool b = EncoderType_eo2iCub(&tt, &s);
3448  if (b == false)
3449  {
3450  yError("Invalid jointEncoderType");
3451  }
3452  r.addString(s);
3453  }
3454  return true;
3455  }
3456  else if (key == "rotorEncoderType")
3457  {
3458  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++)
3459  {
3460  int t; string s;
3461  getRotorEncoderTypeRaw(i, t); uint8_t tt = t; bool b = EncoderType_eo2iCub(&tt, &s);
3462  if (b == false)
3463  {
3464  yError("Invalid motorEncoderType");
3465  }
3466  r.addString(s);
3467  }
3468  return true;
3469  }
3470  else if (key == "coulombThreshold")
3471  {
3472  val.addString("not implemented yet");
3473  return true;
3474  }
3475  else if (key == "torqueControlFilterType")
3476  {
3477  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { int t; getTorqueControlFilterType(i, t); r.addFloat64(t); }
3478  return true;
3479  }
3480  else if (key == "torqueControlEnabled")
3481  {
3482 
3483  Bottle& r = val.addList();
3484  for(int i = 0; i<_njoints; i++)
3485  {
3486  r.addInt32((int)_trq_pids[i].enabled );
3487  }
3488  return true;
3489  }
3490  else if (key == "PWMLimit")
3491  {
3492  Bottle& r = val.addList(); for (int i = 0; i< _njoints; i++) { double tmp = 0; getPWMLimitRaw(i, &tmp); r.addFloat64(tmp); }
3493  return true;
3494  }
3495  else if (key == "motOverloadCurr")
3496  {
3497  Bottle& r = val.addList(); for (int i = 0; i< _njoints; i++) { double tmp = 0; getMaxCurrentRaw(i, &tmp); r.addFloat64(tmp); }
3498  return true;
3499  }
3500  else if (key == "motNominalCurr")
3501  {
3502  Bottle& r = val.addList(); for (int i = 0; i< _njoints; i++) { double tmp = 0; getNominalCurrentRaw(i, &tmp); r.addFloat64(tmp); }
3503  return true;
3504  }
3505  else if (key == "motPeakCurr")
3506  {
3507  Bottle& r = val.addList(); for (int i = 0; i< _njoints; i++) { double tmp = 0; getPeakCurrentRaw(i, &tmp); r.addFloat64(tmp); }
3508  return true;
3509  }
3510  else if (key == "PowerSuppVoltage")
3511  {
3512  Bottle& r = val.addList(); for (int i = 0; i< _njoints; i++) { double tmp = 0; getPowerSupplyVoltageRaw(i, &tmp); r.addFloat64(tmp); }
3513  return true;
3514  }
3515  else if (key == "rotorMax")
3516  {
3517  double tmp1, tmp2;
3518  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { double tmp = 0; getRotorLimitsRaw(i, &tmp1, &tmp2); r.addFloat64(tmp2); }
3519  return true;
3520  }
3521  else if (key == "rotorMin")
3522  {
3523  double tmp1, tmp2;
3524  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { double tmp = 0; getRotorLimitsRaw(i, &tmp1, &tmp2); r.addFloat64(tmp1); }
3525  return true;
3526  }
3527  else if (key == "jointMax")
3528  {
3529  double tmp1, tmp2;
3530  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { double tmp = 0; getLimitsRaw(i, &tmp1, &tmp2); r.addFloat64(tmp2); }
3531  return true;
3532  }
3533  else if (key == "jointMin")
3534  {
3535  double tmp1, tmp2;
3536  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { double tmp = 0; getLimitsRaw(i, &tmp1, &tmp2); r.addFloat64(tmp1); }
3537  return true;
3538  }
3539  else if (key == "jointEncTolerance")
3540  {
3541  double tmp1;
3542  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { double tmp = 0; getJointEncTolerance(i, &tmp1); r.addFloat64(tmp1); }
3543  return true;
3544  }
3545  else if (key == "motorEncTolerance")
3546  {
3547  double tmp1;
3548  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { double tmp = 0; getMotorEncTolerance(i, &tmp1); r.addFloat64(tmp1); }
3549  return true;
3550  }
3551  else if (key == "jointDeadZone")
3552  {
3553  double tmp1;
3554  Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { double tmp = 0; getJointDeadZoneRaw(i, tmp1); r.addFloat64(tmp1); }
3555  return true;
3556  }
3557  else if (key == "readonly_position_PIDraw")
3558  {
3559  Bottle& r = val.addList();
3560  for (int i = 0; i < _njoints; i++)
3561  { Pid p;
3562  getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, i, &p);
3563  char buff[1000];
3564  snprintf(buff, 1000, "J %d : kp %+3.3f ki %+3.3f kd %+3.3f maxint %+3.3f maxout %+3.3f off %+3.3f scale %+3.3f up %+3.3f dwn %+3.3f kff %+3.3f", i, p.kp, p.ki, p.kd, p.max_int, p.max_output, p.offset, p.scale, p.stiction_up_val, p.stiction_down_val, p.kff);
3565  r.addString(buff);
3566  }
3567  return true;
3568  }
3569  else if (key == "readonly_velocity_PIDraw")
3570  {
3571  Bottle& r = val.addList();
3572  for (int i = 0; i < _njoints; i++)
3573  { Pid p; getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY, i, &p);
3574  char buff[1000];
3575  snprintf(buff, 1000, "J %d : kp %+3.3f ki %+3.3f kd %+3.3f maxint %+3.3f maxout %+3.3f off %+3.3f scale %+3.3f up %+3.3f dwn %+3.3f kff %+3.3f", i, p.kp, p.ki, p.kd, p.max_int, p.max_output, p.offset, p.scale, p.stiction_up_val, p.stiction_down_val, p.kff);
3576  r.addString(buff);
3577  }
3578  return true;
3579  }
3580  else if (key == "readonly_torque_PIDraw")
3581  {
3582  Bottle& r = val.addList();
3583  for (int i = 0; i < _njoints; i++)
3584  { Pid p; getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE, i, &p);
3585  char buff[1000];
3586  snprintf(buff, 1000, "J %d : kp %+3.3f ki %+3.3f kd %+3.3f maxint %+3.3f maxout %+3.3f off %+3.3f scale %+3.3f up %+3.3f dwn %+3.3f kff %+3.3f", i, p.kp, p.ki, p.kd, p.max_int, p.max_output, p.offset, p.scale, p.stiction_up_val, p.stiction_down_val, p.kff);
3587  r.addString(buff);
3588  }
3589  return true;
3590  }
3591  else if (key == "readonly_current_PIDraw")
3592  {
3593  Bottle& r = val.addList();
3594  for (int i = 0; i < _njoints; i++)
3595  { Pid p; getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &p);
3596  char buff[1000];
3597  snprintf(buff, 1000, "J %d : kp %+3.3f ki %+3.3f kd %+3.3f maxint %+3.3f maxout %+3.3f off %+3.3f scale %+3.3f up %+3.3f dwn %+3.3f kff %+3.3f", i, p.kp, p.ki, p.kd, p.max_int, p.max_output, p.offset, p.scale, p.stiction_up_val, p.stiction_down_val, p.kff);
3598  r.addString(buff);
3599  }
3600  return true;
3601  }
3602  else if (key == "readonly_llspeed_PIDraw")
3603  {
3604  Bottle& r = val.addList();
3605  for (int i = 0; i < _njoints; i++)
3606  {
3607  Pid p; getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY, i, &p);
3608  char buff[1000];
3609  snprintf(buff, 1000, "J %d : kp %+3.3f ki %+3.3f kd %+3.3f maxint %+3.3f maxout %+3.3f off %+3.3f scale %+3.3f up %+3.3f dwn %+3.3f kff %+3.3f", i, p.kp, p.ki, p.kd, p.max_int, p.max_output, p.offset, p.scale, p.stiction_up_val, p.stiction_down_val, p.kff);
3610  r.addString(buff);
3611  }
3612  return true;
3613  }
3614  else if (key == "readonly_motor_torque_params_raw")
3615  {
3616  Bottle& r = val.addList();
3617  for (int i = 0; i < _njoints; i++)
3618  {
3619  MotorTorqueParameters params;
3620  getMotorTorqueParamsRaw(i, &params);
3621  char buff[1000];
3622  snprintf(buff, 1000, "J %d : bemf %+3.3f bemf_scale %+3.3f ktau %+3.3f ktau_scale %+3.3f viscousPos %+3.3f viscousNeg %+3.3f coulombPos %+3.3f coulombNeg %+3.3f", i, params.bemf, params.bemf_scale, params.ktau, params.ktau_scale, params.viscousPos, params.viscousNeg, params.coulombPos, params.coulombNeg);
3623  r.addString(buff);
3624  }
3625  return true;
3626  }
3627  yWarning("getRemoteVariable(): Unknown variable %s", key.c_str());
3628  return false;
3629 }
3630 
3631 bool embObjMotionControl::setRemoteVariableRaw(std::string key, const yarp::os::Bottle& val)
3632 {
3633  string s1 = val.toString();
3634  if (val.size() != _njoints)
3635  {
3636  yWarning("setRemoteVariable(): Protocol error %s", s1.c_str());
3637  return false;
3638  }
3639 
3640  if (key == "kinematic_mj")
3641  {
3642  yWarning("setRemoteVariable(): Impossible to set kinematic_mj parameter at runtime.");
3643  return false;
3644  }
3645 // else if (key == "rotor")
3646 // {
3647 // for (int i = 0; i < _njoints; i++) _rotorEncoderRes[i] = val.get(i).asInt32();//this operation has none effect on motor controlelr, so i remove it
3648 // return true;
3649 // }
3650 // else if (key == "gearbox_M2J")
3651 // {
3652 // for (int i = 0; i < _njoints; i++) _gearbox_M2J[i] = val.get(i).asFloat64();//this operation has none effect on motor controlelr, so i remove it
3653 // return true;
3654 // }
3655  else if (key == "PWMLimit")
3656  {
3657  for (int i = 0; i < _njoints; i++) setPWMLimitRaw(i, val.get(i).asFloat64());
3658  return true;
3659  }
3660  //disabled for used safety
3661 #if 0
3662  else if (key == "jointMax")
3663  {
3664  double min, max;
3665  for (int i = 0; i < _njoints; i++)
3666  {
3667  getLimitsRaw(i, &min, &max);
3668  setLimitsRaw(i, min, val.get(i).asFloat64());
3669  }
3670  return true;
3671  }
3672  else if (key == "jointMin")
3673  {
3674  double min, max;
3675  for (int i = 0; i < _njoints; i++)
3676  {
3677  getLimitsRaw(i, &min, &max);
3678  setLimitsRaw(i, val.get(i).asFloat64(), max);
3679  }
3680  }
3681 #endif
3682  yWarning("setRemoteVariable(): Unknown variable %s", key.c_str());
3683  return false;
3684 }
3685 
3686 bool embObjMotionControl::getRemoteVariablesListRaw(yarp::os::Bottle* listOfKeys)
3687 {
3688  listOfKeys->clear();
3689  listOfKeys->addString("kinematic_mj");
3690  listOfKeys->addString("encoders");
3691  listOfKeys->addString("gearbox_M2J");
3692  listOfKeys->addString("gearbox_E2J");
3693  listOfKeys->addString("hasHallSensor");
3694  listOfKeys->addString("hasTempSensor");
3695  listOfKeys->addString("hasRotorEncoder");
3696  listOfKeys->addString("hasRotorEncoderIndex");
3697  listOfKeys->addString("rotorIndexOffset");
3698  listOfKeys->addString("rotorEncoderResolution");
3699  listOfKeys->addString("jointEncoderResolution");
3700  listOfKeys->addString("motorPoles");
3701  listOfKeys->addString("pidCurrentKp");
3702  listOfKeys->addString("pidCurrentKi");
3703  listOfKeys->addString("pidCurrentShift");
3704  listOfKeys->addString("pidCurrentOutput");
3705  listOfKeys->addString("coulombThreshold");
3706  listOfKeys->addString("torqueControlFilterType");
3707  listOfKeys->addString("jointEncoderType");
3708  listOfKeys->addString("rotorEncoderType");
3709  listOfKeys->addString("PWMLimit");
3710  listOfKeys->addString("motOverloadCurr");
3711  listOfKeys->addString("motNominalCurr");
3712  listOfKeys->addString("motPeakCurr");
3713  listOfKeys->addString("PowerSuppVoltage");
3714  listOfKeys->addString("rotorMax");
3715  listOfKeys->addString("rotorMin");
3716  listOfKeys->addString("jointMax");
3717  listOfKeys->addString("jointMin");
3718  listOfKeys->addString("jointEncTolerance");
3719  listOfKeys->addString("motorEncTolerance");
3720  listOfKeys->addString("jointDeadZone");
3721  listOfKeys->addString("readonly_position_PIDraw");
3722  listOfKeys->addString("readonly_velocity_PIDraw");
3723  listOfKeys->addString("readonly_current_PIDraw");
3724  listOfKeys->addString("readonly_torque_PIDraw");
3725  listOfKeys->addString("readonly_motor_torque_params_raw");
3726  return true;
3727 }
3728 
3729 // IControlLimits2
3730 bool embObjMotionControl::setVelLimitsRaw(int axis, double min, double max)
3731 {
3732  return NOT_YET_IMPLEMENTED("setVelLimitsRaw");
3733 }
3734 
3735 bool embObjMotionControl::getVelLimitsRaw(int axis, double *min, double *max)
3736 {
3737  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_config);
3738  uint16_t size;
3739  eOmc_joint_config_t joint_cfg;
3740  if(! askRemoteValue(protoid, &joint_cfg, size))
3741  return false;
3742 
3743  *max = joint_cfg.maxvelocityofjoint;
3744  *min = 0;
3745 
3746  return true;
3747 }
3748 
3749 
3750 /*
3751  * IVirtualAnalogSensor Interface
3752  *
3753  * DEPRECATED!! WILL BE REMOVED IN THE NEAR FUTURE!!
3754  *
3755  */
3756 
3758 {
3759  return VAS_status::VAS_OK;
3760 };
3761 
3763 {
3764  return _njoints;
3765 };
3766 
3768 {
3769  bool ret = true;
3770 
3771  for(int j=0; j< _njoints; j++)
3772  {
3773  ret = ret && updateVirtualAnalogSensorMeasure(j, fTorques[j]);
3774  }
3775  return ret;
3776 }
3777 
3778 bool embObjMotionControl::updateVirtualAnalogSensorMeasure(int userLevel_jointNumber, double &fTorque)
3779 {
3780  int j = _axisMap[userLevel_jointNumber];
3781 
3782  eOmeas_torque_t meas_torque = 0;
3783  static double curr_time = Time::now();
3784  static int count_saturation=0;
3785 
3786  meas_torque = (eOmeas_torque_t) S_32(_measureConverter->trqN2S(fTorque, j));
3787 
3788  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_inputs_externallymeasuredtorque);
3789 // We don't need anymore to cache locally because ems board broadcast its torque value in joint status core
3790 // // i write also locally because i want to read it back later on inside getTorqueRaw()
3791 // res->setLocalValue(protoid, &meas_torque);
3792 
3793  // and i want also to send it to the board
3794  return res->setRemoteValue(protoid, &meas_torque);
3795 }
3796 
3797 // end IVirtualAnalogSensor //
3798 
3799 
3800 // Torque control
3802 {
3803  eOmc_joint_status_core_t jstatus;
3804  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
3805  bool ret = res->getLocalValue(protoid, &jstatus);
3806  *t = (double) _measureConverter->trqS2N(jstatus.measures.meas_torque, j);
3807  return ret;
3808 }
3809 
3811 {
3812  bool ret = true;
3813  for(int j=0; j<_njoints; j++)
3814  ret = ret && getTorqueRaw(j, &t[j]);
3815  return true;
3816 }
3817 
3818 bool embObjMotionControl::getTorqueRangeRaw(int j, double *min, double *max)
3819 {
3820  return NOT_YET_IMPLEMENTED("getTorqueRangeRaw");
3821 }
3822 
3824 {
3825  return NOT_YET_IMPLEMENTED("getTorqueRangesRaw");
3826 }
3827 
3829 {
3830  bool ret = true;
3831  for(int j=0; j<_njoints && ret; j++)
3832  ret &= setRefTorqueRaw(j, t[j]);
3833  return ret;
3834 }
3835 
3837 {
3838  eOmc_setpoint_t setpoint;
3839  setpoint.type = (eOenum08_t) eomc_setpoint_torque;
3840  setpoint.to.torque.value = (eOmeas_torque_t) S_32(t);
3841 
3842  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
3843  return res->setRemoteValue(protid, &setpoint);
3844 }
3845 
3846 bool embObjMotionControl::setRefTorquesRaw(const int n_joint, const int *joints, const double *t)
3847 {
3848  bool ret = true;
3849  for(int j=0; j< n_joint; j++)
3850  {
3851  ret &= setRefTorqueRaw(joints[j], t[j]);
3852  }
3853  return ret;
3854 }
3855 
3857 {
3858  bool ret = true;
3859  for(int j=0; j<_njoints && ret; j++)
3860  ret &= getRefTorqueRaw(j, &t[j]);
3861  return ret;
3862 }
3863 
3865 {
3866  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
3867  eOmc_joint_status_core_t jcore = {0};
3868  *t =0 ;
3869 
3870 
3871  if(!res->getLocalValue(id32, &jcore))
3872  {
3873  yError() << "embObjMotionControl::getRefTorqueRaw() could not read pid torque reference pos for " << getBoardInfo() << "joint " << j;
3874  return false;
3875  }
3876 
3877  if ((eOmc_interactionmode_compliant == jcore.modes.interactionmodestatus) &&
3878  (eomc_controlmode_position == jcore.modes.controlmodestatus))
3879  {
3880  *t = (double) jcore.ofpid.complpos.reftrq;
3881  }
3882 
3883  if(eomc_controlmode_torque == jcore.modes.controlmodestatus)
3884  {
3885  *t = (double) jcore.ofpid.torque.reftrq;
3886  }
3887 
3888  return true;
3889 }
3890 
3891 bool embObjMotionControl::helper_setTrqPidRaw(int j, const Pid &pid)
3892 {
3893  eOmc_PID_t outPid;
3894  Pid hwPid = pid;
3895 
3896  //printf("DEBUG setTorquePidRaw: %f %f %f %f %f\n",hwPid.kp , hwPid.ki, hwPid.kd , hwPid.stiction_up_val , hwPid.stiction_down_val );
3897 
3898  copyPid_iCub2eo(&hwPid, &outPid);
3899  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_pidtorque);
3900  return res->setRemoteValue(protid, &outPid);
3901 }
3902 
3903 bool embObjMotionControl::helper_getTrqPidRaw(int j, Pid *pid)
3904 {
3905  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_pidtorque);
3906 
3907  uint16_t size;
3908  eOmc_PID_t eoPID;
3909  if(! askRemoteValue(protoid, &eoPID, size))
3910  return false;
3911 
3912  copyPid_eo2iCub(&eoPID, pid);
3913  //printf("DEBUG getTorquePidRaw: %f %f %f %f %f\n",pid->kp , pid->ki, pid->kd , pid->stiction_up_val , pid->stiction_down_val );
3914 
3915  return true;
3916 }
3917 
3918 bool embObjMotionControl::helper_getTrqPidsRaw(Pid *pid)
3919 {
3920  std::vector<eOmc_PID_t> eoPIDList (_njoints);
3921  bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_config_pidtorque, eoPIDList);
3922  if(! ret)
3923  return false;
3924  for(int j=0; j< _njoints; j++)
3925  {
3926  copyPid_eo2iCub(&eoPIDList[j], &pid[j]);
3927  //printf("DEBUG getTorquePidRaw: %f %f %f %f %f\n",pid->kp , pid->ki, pid->kd , pid->stiction_up_val , pid->stiction_down_val );
3928  }
3929  return true;
3930 }
3931 
3932 
3933 bool embObjMotionControl::getImpedanceRaw(int j, double *stiffness, double *damping)
3934 {
3935  // first set is done in the open function because the whole joint config is sent to the EMSs
3936  eOmc_impedance_t val;
3937 
3938  if(!getWholeImpedanceRaw(j, val))
3939  return false;
3940 
3941  *stiffness = (double) (val.stiffness);
3942  *damping = (double) (val.damping);
3943  return true;
3944 }
3945 
3946 bool embObjMotionControl::getWholeImpedanceRaw(int j, eOmc_impedance_t &imped)
3947 {
3948  // first set is done in the open function because the whole joint config is sent to the EMSs
3949 
3950  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_impedance);
3951  uint16_t size;
3952  if(! askRemoteValue(protoid, &imped, size))
3953  return false;
3954 
3955  // refresh cached value when reading data from the EMS
3956  _cacheImpedance->damping = imped.damping;
3957  _cacheImpedance->stiffness = imped.stiffness;
3958  _cacheImpedance->offset = imped.offset;
3959  return true;
3960 }
3961 
3962 bool embObjMotionControl::setImpedanceRaw(int j, double stiffness, double damping)
3963 {
3964  bool ret = true;
3965  eOmc_impedance_t val;
3966 
3967  // Need to read the whole struct and modify just 2 of them -> now aching the old values and re-using them.
3968  // first set is done in the open function because the whole joint config is sent to the EMSs
3969  // cleaner solution, split the impedance structure into 2 separeted nework variables
3970 // if(!getWholeImpedanceRaw(j, val))
3971 // return false;
3972 
3973  _cacheImpedance[j].stiffness = (eOmeas_stiffness_t) stiffness;
3974  _cacheImpedance[j].damping = (eOmeas_damping_t) damping;
3975 
3976  val.stiffness = _cacheImpedance[j].stiffness;
3977  val.damping = _cacheImpedance[j].damping;
3978  val.offset = _cacheImpedance[j].offset;
3979 
3980  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_impedance);
3981 
3982 
3983  ret &= res->setRemoteValue(protid, &val);
3984  return ret;
3985 }
3986 
3988 {
3989  bool ret = true;
3990  eOmc_impedance_t val;
3991 
3992  // first set is done in the open function because the whole joint config is sent to the EMSs
3993 // if(!getWholeImpedanceRaw(j, val))
3994 // return false;
3995 
3996  _cacheImpedance[j].offset = (eOmeas_torque_t) S_32(offset);
3997  val.stiffness = _cacheImpedance[j].stiffness;
3998  val.damping = _cacheImpedance[j].damping;
3999  val.offset = _cacheImpedance[j].offset;
4000 
4001  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_impedance);
4002 
4003 
4004  ret &= res->setRemoteValue(protid, &val);
4005 
4006  return ret;
4007 }
4008 
4010 {
4011  eOmc_impedance_t val;
4012 
4013  if(!getWholeImpedanceRaw(j, val))
4014  return false;
4015 
4016  *offset = val.offset;
4017  return true;
4018 }
4019 
4020 bool embObjMotionControl::getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp)
4021 {
4022  *min_stiff = _impedance_limits[j].min_stiff;
4023  *max_stiff = _impedance_limits[j].max_stiff;
4024  *min_damp = _impedance_limits[j].min_damp;
4025  *max_damp = _impedance_limits[j].max_damp;
4026  return true;
4027 }
4028 
4029 bool embObjMotionControl::getMotorTorqueParamsRaw(int j, MotorTorqueParameters *params)
4030 {
4031  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_motor_params);
4032 
4033  uint16_t size;
4034  eOmc_motor_params_t eo_params = {0};
4035  if(! askRemoteValue(protoid, &eo_params, size))
4036  return false;
4037 
4038  params->bemf = eo_params.bemf_value;
4039  params->bemf_scale = eo_params.bemf_scale;
4040  params->ktau = eo_params.ktau_value;
4041  params->ktau_scale = eo_params.ktau_scale;
4042  params->viscousPos = eo_params.friction.viscous_pos_val;
4043  params->viscousNeg = eo_params.friction.viscous_neg_val ;
4044  params->coulombPos = eo_params.friction.coulomb_pos_val;
4045  params->coulombNeg = eo_params.friction.coulomb_neg_val;
4046 
4047  //printf("debug getMotorTorqueParamsRaw %f %f %f %f %f %f %f %f\n", params->bemf, params->bemf_scale, params->ktau,params->ktau_scale, params->viscousPos, params->viscousNeg, params->coulombPos, params->coulombNeg);
4048 
4049  return true;
4050 }
4051 
4052 bool embObjMotionControl::setMotorTorqueParamsRaw(int j, const MotorTorqueParameters params)
4053 {
4054  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_motor_params);
4055  eOmc_motor_params_t eo_params = {0};
4056 
4057  //printf("setMotorTorqueParamsRaw for j %d(INPUT): benf=%f ktau=%f viscousPos=%f viscousNeg=%f coulombPos=%f coulombNeg=%f\n",j, params.bemf, params.ktau, params.viscousPos, params.viscousNeg, params.coulombPos, params.coulombNeg);
4058 
4059  eo_params.bemf_value = (float) params.bemf;
4060  eo_params.bemf_scale = (uint8_t) params.bemf_scale;
4061  eo_params.ktau_value = (float) params.ktau;
4062  eo_params.ktau_scale = (uint8_t) params.ktau_scale;
4063  eo_params.friction.viscous_pos_val = static_cast<float32_t>(params.viscousPos);
4064  eo_params.friction.viscous_neg_val = static_cast<float32_t>(params.viscousNeg);
4065  eo_params.friction.coulomb_pos_val = static_cast<float32_t>(params.coulombPos);
4066  eo_params.friction.coulomb_neg_val = static_cast<float32_t>(params.coulombNeg);
4067 
4068 
4069  if(false == res->setRemoteValue(id32, &eo_params))
4070  {
4071  yError() << "embObjMotionControl::setMotorTorqueParamsRaw() could not send set message for" << getBoardInfo() << "joint " << j;
4072  return false;
4073  }
4074 
4075  return true;
4076 }
4077 
4078 // IVelocityControl2
4079 bool embObjMotionControl::velocityMoveRaw(const int n_joint, const int *joints, const double *spds)
4080 {
4081  bool ret = true;
4082 
4083  for(int j=0; j< n_joint; j++)
4084  {
4085  ret &= velocityMoveRaw(joints[j], spds[j]);
4086  }
4087  return ret;
4088 }
4089 
4090 /*
4091 bool embObjMotionControl::helper_setVelPidRaw(int j, const Pid &pid)
4092 {
4093  eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_piddirect);
4094  eOmc_PID_t outPid;
4095  Pid hwPid = pid;
4096 
4097  if(!_dir_pids[j].enabled)
4098  {
4099  yError() << "eoMc " << getBoardInfo() << ": it is not possible set direct pid for joint " << j <<", because velocity pid is enabled in xml files";
4100  return false;
4101  }
4102 
4103  copyPid_iCub2eo(&hwPid, &outPid);
4104 
4105  if (false == res->setRemoteValue(protoId, &outPid))
4106  {
4107  yError() << "while setting direct PIDs for" << getBoardInfo() << " joint " << j;
4108  return false;
4109  }
4110 
4111  return true;
4112 
4113  //return NOT_YET_IMPLEMENTED("Our boards do not have a Velocity Pid");
4114 }
4115 */
4116 
4117 bool embObjMotionControl::helper_getVelPidRaw(int j, Pid *pid)
4118 {
4119  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_piddirect);
4120  uint16_t size;
4121  eOmc_PID_t eoPID;
4122  if(! askRemoteValue(protoid, &eoPID, size))
4123  return false;
4124 
4125  copyPid_eo2iCub(&eoPID, pid);
4126 
4127  return true;
4128 
4129  //return NOT_YET_IMPLEMENTED("Our boards do not have a Velocity Pid");
4130 }
4131 
4132 bool embObjMotionControl::helper_getVelPidsRaw(Pid *pid)
4133 {
4134  std::vector <eOmc_PID_t> eoPIDList (_njoints);
4135  bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_config_piddirect, eoPIDList);
4136  if(!ret)
4137  return false;
4138 
4139  for(int j=0; j<_njoints; j++)
4140  {
4141  copyPid_eo2iCub(&eoPIDList[j], &pid[j]);
4142  }
4143 
4144  return true;
4145 
4146  //return NOT_YET_IMPLEMENTED("Our boards do not have a Velocity Pid");
4147 }
4148 
4149 // PositionDirect Interface
4151 {
4152  int mode = 0;
4153  getControlModeRaw(j, &mode);
4154  if (mode != VOCAB_CM_POSITION_DIRECT &&
4155  mode != VOCAB_CM_IDLE)
4156  {
4157  if(event_downsampler->canprint())
4158  {
4159  yError() << "setReferenceRaw: skipping command because" << getBoardInfo() << " joint " << j << " is not in VOCAB_CM_POSITION_DIRECT mode";
4160  }
4161  return true;
4162  }
4163 
4164  eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
4165  eOmc_setpoint_t setpoint = {0};
4166 
4167  _ref_positions[j] = ref; // save internally the new value of pos.
4168  setpoint.type = (eOenum08_t) eomc_setpoint_positionraw;
4169  setpoint.to.position.value = (eOmeas_position_t) S_32(ref);
4170  setpoint.to.position.withvelocity = 0;
4171 
4172  return res->setRemoteValue(protoId, &setpoint);
4173 }
4174 
4175 bool embObjMotionControl::setPositionsRaw(const int n_joint, const int *joints, const double *refs)
4176 {
4177  bool ret = true;
4178  for(int i=0; i<n_joint; i++)
4179  {
4180  ret &= setPositionRaw(joints[i], refs[i]);
4181  }
4182  return ret;
4183 }
4184 
4186 {
4187  bool ret = true;
4188  for (int i = 0; i<_njoints; i++)
4189  {
4190  ret &= setPositionRaw(i, refs[i]);
4191  }
4192  return ret;
4193 }
4194 
4195 
4197 {
4198  if (axis<0 || axis>_njoints) return false;
4199 #if ASK_REFERENCE_TO_FIRMWARE
4200  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_status_target);
4201  *ref = 0;
4202 
4203  // Get the value
4204  uint16_t size;
4205  eOmc_joint_status_target_t target = {0};
4206  if(!askRemoteValue(id32, &target, size))
4207  {
4208  yError() << "embObjMotionControl::getTargetPositionRaw() could not read reference pos for " << getBoardInfo() << "joint " << axis;
4209  return false;
4210  }
4211 
4212  *ref = (double) target.trgt_position;
4213  //yError() << "embObjMotionControl::getTargetPositionRaw() BOARD" << _fId.boardNumber << "joint " << axis << "pos=" << target.trgt_position;
4214  return true;
4215 #else
4216  *ref = _ref_command_positions[axis];
4217  return true;
4218 #endif
4219 }
4220 
4222 {
4223  bool ret = true;
4224  for (int i = 0; i<_njoints; i++)
4225  {
4226  ret &= getTargetPositionRaw(i, &refs[i]);
4227  }
4228  return ret;
4229 }
4230 
4231 bool embObjMotionControl::getTargetPositionsRaw(int nj, const int * jnts, double *refs)
4232 {
4233  bool ret = true;
4234  for (int i = 0; i<nj; i++)
4235  {
4236  ret &= getTargetPositionRaw(jnts[i], &refs[i]);
4237  }
4238  return ret;
4239 }
4240 
4241 bool embObjMotionControl::getRefVelocityRaw(int axis, double *ref)
4242 {
4243  if (axis<0 || axis>_njoints) return false;
4244 #if ASK_REFERENCE_TO_FIRMWARE
4245  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_status_target);
4246  *ref = 0;
4247 
4248  // Get the value
4249  uint16_t size;
4250  eOmc_joint_status_target_t target = {0};
4251  if(!askRemoteValue(id32, &target, size))
4252  {
4253  yError() << "embObjMotionControl::getRefVelocityRaw() could not read reference vel for " << getBoardInfo() << "joint " << axis;
4254  return false;
4255  }
4256  *ref = (double) target.trgt_velocity;
4257  return true;
4258 #else
4259  *ref = _ref_command_speeds[axis];
4260  return true;
4261 #endif
4262 }
4263 
4265 {
4266  #if ASK_REFERENCE_TO_FIRMWARE
4267  std::vector <eOmc_joint_status_target_t> targetList(_njoints);
4268  bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_target, targetList);
4269  if(!ret)
4270  {
4271  yError() << "embObjMotionControl::getRefVelocitiesRaw() could not read reference vel for " << getBoardInfo() ;
4272  return false;
4273  }
4274  // Get the value
4275  for(int j=0; j<_njoints; j++)
4276  {
4277  refs[j] = (double) targetList[j].trgt_velocity;
4278  }
4279  return true;
4280  #else
4281  for(int j=0; j<_njoints; j++)
4282  {
4283  refs[j] = _ref_command_speeds[j];
4284  }
4285  return true;
4286  #endif
4287 }
4288 
4289 bool embObjMotionControl::getRefVelocitiesRaw(int nj, const int * jnts, double *refs)
4290 {
4291  std::vector <double> refsList(_njoints);
4292  if(!getRefVelocitiesRaw(refsList.data()))
4293  return false;
4294 
4295  for (int i = 0; i<nj; i++)
4296  {
4297  if(jnts[i]>= _njoints)
4298  {
4299  yError() << getBoardInfo() << "getRefVelocitiesRaw: joint " << jnts[i] << "doesn't exist";
4300  return false;
4301  }
4302  refs[i] = refsList[jnts[i]];
4303  }
4304  return true;
4305 }
4306 
4307 bool embObjMotionControl::getRefPositionRaw(int axis, double *ref)
4308 {
4309  if (axis<0 || axis>_njoints) return false;
4310 #if ASK_REFERENCE_TO_FIRMWARE
4311  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_status_target);
4312  *ref = 0;
4313  // Get the value
4314  uint16_t size;
4315  eOmc_joint_status_target_t target = {0};
4316  if(!askRemoteValue(id32, &target, size))
4317  {
4318  yError() << "embObjMotionControl::getRefPositionRaw() could not read reference pos for " << getBoardInfo() << "joint " << axis;
4319  return false;
4320  }
4321 
4322  *ref = (double) target.trgt_positionraw;
4323  return true;
4324 #else
4325  *ref = _ref_positions[axis];
4326  return true;
4327 #endif
4328 }
4329 
4331 {
4332  #if ASK_REFERENCE_TO_FIRMWARE
4333  std::vector <eOmc_joint_status_target_t> targetList(_njoints);
4334  bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_target, targetList);
4335  if(!ret)
4336  {
4337  yError() << "embObjMotionControl::getRefPositionRaw() could not read reference pos for " << getBoardInfo();
4338  return false;
4339  }
4340  // Get the value
4341  for(int j=0; j< _njoints; j++)
4342  refs[j] = (double) targetList[j].trgt_positionraw;
4343  return true;
4344  #else
4345  for(int j=0; j< _njoints; j++)
4346  refs[j] = _ref_positions[j];
4347  return true;
4348  #endif
4349 }
4350 
4351 bool embObjMotionControl::getRefPositionsRaw(int nj, const int * jnts, double *refs)
4352 {
4353  bool ret = true;
4354  for (int i = 0; i<nj; i++)
4355  {
4356  ret &= getRefPositionRaw(jnts[i], &refs[i]);
4357  }
4358  return ret;
4359 }
4360 
4361 // InteractionMode
4362 
4363 
4364 
4365 bool embObjMotionControl::getInteractionModeRaw(int j, yarp::dev::InteractionModeEnum* _mode)
4366 {
4367  eOenum08_t interactionmodestatus;
4368 // std::cout << "eoMC getInteractionModeRaw SINGLE joint " << j << std::endl;
4369 
4370  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core_modes_interactionmodestatus);
4371  if(! res->getLocalValue(protid, &interactionmodestatus)) // it is broadcasted toghether with the jointStatus full
4372  return false;
4373 
4374  int tmp = (int) *_mode;
4375  if(!interactionModeStatusConvert_embObj2yarp(interactionmodestatus, tmp) )
4376  return false;
4377 
4378  *_mode = (yarp::dev::InteractionModeEnum) tmp;
4379  return true;
4380 }
4381 
4382 bool embObjMotionControl::getInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum* modes)
4383 {
4384 // std::cout << "eoMC getInteractionModeRaw GROUP joints" << std::endl;
4385  bool ret = true;
4386  for(int idx=0; idx<n_joints; idx++)
4387  {
4388  ret = getInteractionModeRaw(joints[idx], &modes[idx]);
4389  }
4390  return ret;
4391 }
4392 
4393 bool embObjMotionControl::getInteractionModesRaw(yarp::dev::InteractionModeEnum* modes)
4394 {
4395 // std::cout << "eoMC getInteractionModeRaw ALL joints" << std::endl;
4396  bool ret = true;
4397  for(int j=0; j<_njoints; j++)
4398  ret = ret && getInteractionModeRaw(j, &modes[j]);
4399  return ret;
4400 }
4401 
4402 // marco.accame: con alberto cardellino abbiamo parlato della correttezza di effettuare la verifica di quanto imposto (in setInteractionModeRaw() ed affini)
4403 // andando a rileggere il valore nella scheda eth fino a che esso non sia quello atteso. si deve fare oppure no?
4404 // con il interaction mode il can ora non lo fa. mentre lo fa per il control mode. perche' diverso?
4405 bool embObjMotionControl::setInteractionModeRaw(int j, yarp::dev::InteractionModeEnum _mode)
4406 {
4407  eOenum08_t interactionmodecommand = 0;
4408 
4409 
4410  // yDebug() << "received setInteractionModeRaw command (SINGLE) for" << getBoardInfo() << " joint " << j << " mode " << Vocab32::decode(_mode);
4411 
4412  if (_mode == VOCAB_IM_COMPLIANT && _trq_pids[j].enabled == false) {yError()<<"Torque control is disabled. Check your configuration parameters"; return false;}
4413 
4414  if(!interactionModeCommandConvert_yarp2embObj(_mode, interactionmodecommand) )
4415  {
4416  yError() << "setInteractionModeRaw: received unknown mode for" << getBoardInfo() << " joint " << j << " mode " << Vocab32::decode(_mode);
4417  }
4418 
4419  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_interactionmode);
4420 
4421  if(false == res->setRemoteValue(protid, &interactionmodecommand) )
4422  {
4423  yError() << "setInteractionModeRaw failed for" << getBoardInfo() << " joint " << j << " mode " << Vocab32::decode(_mode);
4424  return false;
4425  }
4426 
4427  // marco.accame: use the following if you want to check the value of interactionmode on the remote board
4428 #if 0
4429  eOenum08_t interactionmodestatus = 0;
4430  uint16_t size = 0;
4431  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_interactionmodestatus);
4432  bool ret = askRemoteValue(id32, &interactionmodestatus, size);
4433 
4434  if((false == ret) || (interactionmodecommand != interactionmodestatus))
4435  {
4436  yError() << "check of embObjMotionControl::setInteractionModeRaw() failed for" << getBoardInfo() << " joint " << j << " mode " << Vocab32::decode(_mode);
4437  return false;
4438  }
4439 #endif
4440 
4441  return true;
4442 }
4443 
4444 
4445 bool embObjMotionControl::setInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum* modes)
4446 {
4447 // std::cout << "setInteractionModeRaw GROUP " << std::endl;
4448 
4449  eOenum08_t interactionmodecommand = 0;
4450 
4451  for(int j=0; j<n_joints; j++)
4452  {
4453  if (modes[j] == VOCAB_IM_COMPLIANT && _trq_pids[j].enabled == false) {yError()<<"Torque control is disabled. Check your configuration parameters"; continue;}
4454 
4455  if(!interactionModeCommandConvert_yarp2embObj(modes[j], interactionmodecommand) )
4456  {
4457  yError() << "embObjMotionControl::setInteractionModesRaw(): received unknown interactionMode for" << getBoardInfo() << " joint " << j << " mode " << Vocab32::decode(modes[j]) << " " << modes[j];
4458  return false;
4459  }
4460 
4461  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_interactionmode);
4462  if(false == res->setRemoteValue(protid, &interactionmodecommand) )
4463  {
4464  yError() << "embObjMotionControl::setInteractionModesRaw() failed for" << getBoardInfo() << " joint " << j << " mode " << Vocab32::decode(modes[j]);
4465  return false;
4466  }
4467 
4468  // marco.accame: use the following if you want to check the value of interactionmode on the remote board
4469 #if 0
4470  eOenum08_t interactionmodestatus = 0;
4471  uint16_t size = 0;
4472  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_interactionmodestatus);
4473  bool ret = askRemoteValue(id32, &interactionmodestatus, size);
4474 
4475  if((false == ret) || (interactionmodecommand != interactionmodestatus))
4476  {
4477  if(false == ret)
4478  {
4479  yError() << "check of embObjMotionControl::setInteractionModesRaw() failed for" << getBoardInfo() << " joint " << j << " mode " << Vocab32::decode(modes[j]);
4480  return false;
4481  }
4482 
4483  int tmp;
4484  if(interactionModeStatusConvert_embObj2yarp(interactionmodestatus, tmp) )
4485  yError() << "setInteractionModeRaw failed for" << getBoardInfo() << " joint " << j << " because of interactionMode mismatching \n\tSet " \
4486  << Vocab32::decode(modes[j]) << " Got " << Vocab32::decode(tmp);
4487  else
4488  yError() << "setInteractionModeRaw failed for" << getBoardInfo() << " joint " << j << " because of interactionMode mismatching \n\tSet " \
4489  << Vocab32::decode(modes[j]) << " Got an unknown value!";
4490  return false;
4491  }
4492 #endif
4493 
4494  }
4495 
4496  return true;
4497 }
4498 
4499 bool embObjMotionControl::setInteractionModesRaw(yarp::dev::InteractionModeEnum* modes)
4500 {
4501 
4502  eOenum08_t interactionmodecommand = 0;
4503 
4504  for(int j=0; j<_njoints; j++)
4505  {
4506  if ((modes[j] == VOCAB_IM_COMPLIANT) && (_trq_pids[j].enabled == false))
4507  {
4508  yError()<<"Torque control is disabled. Check your configuration parameters";
4509  continue;
4510  }
4511 
4512  if(!interactionModeCommandConvert_yarp2embObj(modes[j], interactionmodecommand) )
4513  {
4514  yError() << "setInteractionModeRaw: received unknown interactionMode for" << getBoardInfo() << " joint " << j << " mode " << Vocab32::decode(modes[j]);
4515  return false;
4516  }
4517 
4518  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_interactionmode);
4519  if(false == res->setRemoteValue(protid, &interactionmodecommand) )
4520  {
4521  yError() << "setInteractionModeRaw failed for" << getBoardInfo() << " joint " << j << " mode " << Vocab32::decode(modes[j]);
4522  return false;
4523  }
4524 
4525  // marco.accame: use the following if you want to check the value of interactionmode on the remote board
4526 #if 0
4527  eOenum08_t interactionmodestatus = 0;
4528  uint16_t size = 0;
4529  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_interactionmodestatus);
4530  bool ret = askRemoteValue(id32, &interactionmodestatus, size);
4531 
4532  if((false == ret) || (interactionmodecommand != interactionmodestatus))
4533  {
4534  if(false == ret)
4535  {
4536  yError() << "check of embObjMotionControl::setInteractionModesRaw() failed for" << getBoardInfo() << " joint " << j << " mode " << Vocab32::decode(modes[j]);
4537  return false;
4538  }
4539 
4540  int tmp;
4541  if(interactionModeStatusConvert_embObj2yarp(interactionmodestatus, tmp) )
4542  yError() << "setInteractionModeRaw failed for" << getBoardInfo() << " joint " << j << " because of interactionMode mismatching \n\tSet " \
4543  << Vocab32::decode(modes[j]) << " Got " << Vocab32::decode(tmp);
4544  else
4545  yError() << "setInteractionModeRaw failed for" << getBoardInfo() << " joint " << j << " because of interactionMode mismatching \n\tSet " \
4546  << Vocab32::decode(modes[j]) << " Got an unknown value!";
4547  return false;
4548  }
4549 #endif
4550 
4551  }
4552 
4553  return true;
4554 }
4555 
4556 
4557 bool embObjMotionControl::getPidOutputRaw(const PidControlTypeEnum& pidtype, int j, double *out)
4558 {
4559  eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
4560  eOmc_joint_status_core_t jcore = {0};
4561  *out = 0;
4562  if(!res->getLocalValue(protoId, &jcore) )
4563  return false;
4564 
4565  switch (pidtype)
4566  {
4567  case VOCAB_PIDTYPE_POSITION:
4568  if((eomc_controlmode_torque == jcore.modes.controlmodestatus) || (eomc_controlmode_current == jcore.modes.controlmodestatus))
4569  *out=0;
4570  else
4571  *out = (double) jcore.ofpid.generic.output;
4572  break;
4573  //case VOCAB_PIDTYPE_DIRECT:
4574  // *out=0;
4575  //break;
4576  case VOCAB_PIDTYPE_TORQUE:
4577  if ((eomc_controlmode_torque == jcore.modes.controlmodestatus) ||
4578  ((eomc_controlmode_position == jcore.modes.controlmodestatus) && (eOmc_interactionmode_compliant == jcore.modes.interactionmodestatus)))
4579  *out = jcore.ofpid.generic.output;
4580  else
4581  *out = 0;
4582  break;
4583  case VOCAB_PIDTYPE_CURRENT:
4584  *out=0;
4585  break;
4586  case VOCAB_PIDTYPE_VELOCITY:
4587  *out = 0;
4588  break;
4589  default:
4590  yError()<<"Invalid pidtype:"<<pidtype;
4591  break;
4592  }
4593  return true;
4594 }
4595 
4596 bool embObjMotionControl::getPidOutputsRaw(const PidControlTypeEnum& pidtype, double *outs)
4597 {
4598  bool ret = true;
4599  for(int j=0; j< _njoints; j++)
4600  {
4601  ret &= getPidOutputRaw(pidtype, j, &outs[j]);
4602  }
4603  return ret;
4604 }
4605 
4606 bool embObjMotionControl::isPidEnabledRaw(const PidControlTypeEnum& pidtype, int j, bool* enabled)
4607 {
4608  return NOT_YET_IMPLEMENTED("isPidEnabled");
4609 }
4610 
4612 {
4613  *num=_njoints;
4614  return true;
4615 }
4616 
4618 {
4619  eOmc_motor_status_basic_t status;
4620  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);
4621 
4622  bool ret = res->getLocalValue(protid, &status);
4623  if(ret)
4624  {
4625  *val = (double) status.mot_temperature;
4626  }
4627  else
4628  {
4629  yError() << "embObjMotionControl::getTemperatureRaw failed for" << getBoardInfo() << " motor " << m ;
4630  *val = 0;
4631  }
4632 
4633  return ret;
4634 }
4635 
4637 {
4638  bool ret = true;
4639  for(int j=0; j< _njoints; j++)
4640  {
4641  ret &= getTemperatureRaw(j, &vals[j]);
4642  }
4643  return ret;
4644 }
4645 
4647 {
4648  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_temperaturelimit);
4649  uint16_t size;
4650  eOmeas_temperature_t temperaturelimit = {0};
4651  *temp = 0;
4652  if(!askRemoteValue(protid, &temperaturelimit, size))
4653  {
4654  yError() << "embObjMotionControl::getTemperatureLimitRaw() can't read temperature limits for" << getBoardInfo() << " motor " << m;
4655  return false;
4656  }
4657 
4658  *temp = (double) temperaturelimit;
4659 
4660  return true;
4661 }
4662 
4663 bool embObjMotionControl::setTemperatureLimitRaw(int m, const double temp)
4664 {
4665  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_temperaturelimit);
4666  eOmeas_temperature_t temperatureLimit = (eOmeas_pwm_t) S_16(temp);
4667  return res->setRemoteValue(protid, &temperatureLimit);
4668 
4669 }
4670 
4672 {
4673  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_currentlimits);
4674  uint16_t size;
4675  eOmc_current_limits_params_t currentlimits = {0};
4676  *val = 0;
4677  if(!askRemoteValue(protid, &currentlimits, size))
4678  {
4679  yError() << "embObjMotionControl::getPeakCurrentRaw() can't read current limits for" << getBoardInfo() << " motor " << m;
4680  return false;
4681  }
4682 
4683  *val = (double) currentlimits.peakCurrent ;
4684  return true;
4685 }
4686 
4687 bool embObjMotionControl::setPeakCurrentRaw(int m, const double val)
4688 {
4689  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_currentlimits);
4690  //get current limit params
4691  uint16_t size;
4692  eOmc_current_limits_params_t currentlimits = {0};
4693  if(!askRemoteValue(protid, &currentlimits, size))
4694  {
4695  yError() << "embObjMotionControl::setPeakCurrentRaw can't read current limits for" << getBoardInfo() << " motor " << m ;
4696  return false;
4697  }
4698 
4699  //set current overload
4700  currentlimits.peakCurrent = (eOmeas_current_t) S_16(val);
4701 
4702  //send new values
4703  bool ret = res->setRemoteValue(protid, &currentlimits);
4704  if(!ret)
4705  {
4706  yError() << "embObjMotionControl::setPeakCurrentRaw failed sending new value for" << getBoardInfo() << " motor " << m ;
4707  }
4708  return ret;
4709 }
4710 
4712 {
4713  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_currentlimits);
4714  uint16_t size;
4715  eOmc_current_limits_params_t currentlimits = {0};
4716  *val = 0;
4717  if(!askRemoteValue(protid, &currentlimits, size))
4718  {
4719  yError() << "embObjMotionControl::getNominalCurrentRaw() can't read current limits for" << getBoardInfo() << " motor " << m;
4720  return false;
4721  }
4722 
4723  *val = (double) currentlimits.nominalCurrent ;
4724  return true;
4725 }
4726 
4727 bool embObjMotionControl::setNominalCurrentRaw(int m, const double val)
4728 {
4729  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_currentlimits);
4730 
4731  //get current limit params
4732  uint16_t size;
4733  eOmc_current_limits_params_t currentlimits = {0};
4734  if(!askRemoteValue(protid, &currentlimits, size))
4735  {
4736  yError() << "embObjMotionControl::setNominalCurrentRaw can't read current limits for" << getBoardInfo() << " motor " << m ;
4737  return false;
4738  }
4739 
4740  //set current overload
4741  currentlimits.nominalCurrent = (eOmeas_current_t) S_16(val);
4742 
4743  //send new values
4744  bool ret = res->setRemoteValue(protid, &currentlimits);
4745  if(!ret)
4746  {
4747  yError() << "embObjMotionControl::setNominalCurrentRaw failed sending new value for" << getBoardInfo() << " motor " << m ;
4748  }
4749 
4750  return ret;
4751 }
4752 
4753 bool embObjMotionControl::getPWMRaw(int j, double* val)
4754 {
4755  eOmc_motor_status_basic_t status;
4756  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_status_basic);
4757 
4758  bool ret = res->getLocalValue(protid, &status);
4759  if(ret)
4760  {
4761  *val = (double) status.mot_pwm;
4762  }
4763  else
4764  {
4765  yError() << "embObjMotionControl::getPWMRaw failed for" << getBoardInfo() << " motor " << j ;
4766  *val = 0;
4767  }
4768 
4769  return ret;
4770 }
4771 
4772 bool embObjMotionControl::getPWMLimitRaw(int j, double* val)
4773 {
4774  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_pwmlimit);
4775  uint16_t size;
4776  eOmeas_pwm_t motorPwmLimit;
4777 
4778  bool ret = askRemoteValue(protid, &motorPwmLimit, size);
4779  if(ret)
4780  {
4781  *val = (double) motorPwmLimit;
4782  }
4783  else
4784  {
4785  yError() << "embObjMotionControl::getPWMLimitRaw failed for" << getBoardInfo() << " motor " << j ;
4786  *val = 0;
4787  }
4788 
4789  return ret;
4790 }
4791 
4792 bool embObjMotionControl::setPWMLimitRaw(int j, const double val)
4793 {
4794  if (val < 0)
4795  {
4796  yError() << "embObjMotionControl::setPWMLimitRaw failed because pwmLimit is negative for" << getBoardInfo() << " motor " << j ;
4797  return true; //return true because the error ios not due to communication error
4798  }
4799  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_pwmlimit);
4800  eOmeas_pwm_t motorPwmLimit = (eOmeas_pwm_t) S_16(val);
4801  return res->setRemoteValue(protid, &motorPwmLimit);
4802 }
4803 
4805 {
4806  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_controller, 0, eoprot_tag_mc_controller_status);
4807  uint16_t size;
4808  eOmc_controller_status_t controllerStatus;
4809 
4810  bool ret = askRemoteValue(protid, &controllerStatus, size);
4811  if(ret)
4812  {
4813  *val = (double) controllerStatus.supplyVoltage;
4814  }
4815  else
4816  {
4817  yError() << "embObjMotionControl::getPowerSupplyVoltageRaw failed for" << getBoardInfo() << " motor " << j ;
4818  *val = 0;
4819  }
4820 
4821  return ret;
4822 }
4823 
4824 bool embObjMotionControl::askRemoteValue(eOprotID32_t id32, void* value, uint16_t& size)
4825 {
4826  return res->getRemoteValue(id32, value, 0.200, 0);
4827 }
4828 
4829 
4830 template <class T>
4831 bool embObjMotionControl::askRemoteValues(eOprotEndpoint_t ep, eOprotEntity_t entity, eOprotTag_t tag, std::vector<T>& values)
4832 {
4833  std::vector<eOprotID32_t> idList;
4834  std::vector<void*> valueList;
4835  idList.clear();
4836  valueList.clear();
4837  for(int j=0; j<_njoints; j++)
4838  {
4839  eOprotID32_t protoId = eoprot_ID_get(ep, entity, j, tag);
4840  idList.push_back(protoId);
4841  valueList.push_back((void*)&values[j]);
4842  }
4843 
4844  bool ret = res->getRemoteValues(idList, valueList);
4845  if(!ret)
4846  {
4847  yError() << "embObjMotionControl::askRemoteValues failed for all joints of" << getBoardInfo();
4848  }
4849 
4850  return ret;
4851 }
4852 
4853 
4854 
4855 
4856 bool embObjMotionControl::checkRemoteControlModeStatus(int joint, int target_mode)
4857 {
4858  bool ret = false;
4859  eOenum08_t temp = 0;
4860  uint16_t size = 0;
4861 
4862  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joint, eoprot_tag_mc_joint_status_core_modes_controlmodestatus);
4863  const double timeout = 0.250; // 250 msec
4864  const int maxretries = 25;
4865  const double delaybetweenqueries = 0.010; // 10 msec
4866 
4867  // now i repeat the query until i am satisfied. how many times? for maximum time timeout seconds and with a gap of delaybetweenqueries
4868 
4869  double timeofstart = yarp::os::Time::now();
4870  int attempt = 0;
4871 
4872  for( attempt = 0; attempt < maxretries; attempt++)
4873  {
4874  ret = askRemoteValue(id32, &temp, size);
4875  if(ret == false)
4876  {
4877  yError ("An error occurred inside embObjMotionControl::checkRemoteControlModeStatus(j=%d, targetmode=%s) for BOARD %s IP %s", joint, yarp::os::Vocab32::decode(target_mode).c_str(), res->getProperties().boardnameString.c_str(), res->getProperties().ipv4addrString.c_str());
4878  break;
4879  }
4880  int current_mode = controlModeStatusConvert_embObj2yarp(temp);
4881  if(current_mode == target_mode)
4882  {
4883  ret = true;
4884  break;
4885  }
4886  if((current_mode == VOCAB_CM_IDLE) && (target_mode == VOCAB_CM_FORCE_IDLE))
4887  {
4888  ret = true;
4889  break;
4890  }
4891  if(current_mode == VOCAB_CM_HW_FAULT)
4892  {
4893  if(target_mode != VOCAB_CM_FORCE_IDLE) { yError ("embObjMotionControl::checkRemoteControlModeStatus(%d, %d) is unable to check the control mode of BOARD %s IP %s because it is now in HW_FAULT", joint, target_mode, res->getProperties().boardnameString.c_str(), res->getProperties().ipv4addrString.c_str()); }
4894  ret = true;
4895  break;
4896  }
4897 
4898  if((yarp::os::Time::now()-timeofstart) > timeout)
4899  {
4900  ret = false;
4901  yError ("A %f sec timeout occured in embObjMotionControl::checkRemoteControlModeStatus(), BOARD %s IP %s, joint %d, current mode: %s, requested: %s", timeout, res->getProperties().boardnameString.c_str(), res->getProperties().ipv4addrString.c_str(), joint, yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(target_mode).c_str());
4902  break;
4903  }
4904  if(attempt > 0)
4905  { // i print the warning only after at least one retry.
4906  yWarning ("embObjMotionControl::checkRemoteControlModeStatus() has done %d attempts and will retry again after a %f sec delay. (BOARD %s IP %s, joint %d) -> current mode = %s, requested = %s", attempt+1, delaybetweenqueries, res->getProperties().boardnameString.c_str() , res->getProperties().ipv4addrString.c_str(), joint, yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(target_mode).c_str());
4907  }
4908  SystemClock::delaySystem(delaybetweenqueries);
4909  }
4910 
4911  if(false == ret)
4912  {
4913  yError("failure of embObjMotionControl::checkRemoteControlModeStatus(j=%d, targetmode=%s) for BOARD %s IP %s after %d attempts and %f seconds", joint, yarp::os::Vocab32::decode(target_mode).c_str(), res->getProperties().boardnameString.c_str(), res->getProperties().ipv4addrString.c_str(), attempt, yarp::os::Time::now()-timeofstart);
4914  }
4915 
4916 
4917  return ret;
4918 }
4919 
4920 //the device needs coupling info if it manages joints controlled by 2foc and mc4plus.
4921 bool embObjMotionControl::iNeedCouplingsInfo(void)
4922 {
4923  eOmn_serv_type_t mc_serv_type = (eOmn_serv_type_t)serviceConfig.ethservice.configuration.type;
4924  if( (mc_serv_type == eomn_serv_MC_foc) ||
4925  (mc_serv_type == eomn_serv_MC_mc4plus) ||
4926  (mc_serv_type == eomn_serv_MC_mc4plusmais) ||
4927  (mc_serv_type == eomn_serv_MC_mc2pluspsc) ||
4928  (mc_serv_type == eomn_serv_MC_mc4plusfaps)
4929  || (mc_serv_type == eomn_serv_MC_mc4pluspmc)
4930  )
4931  return true;
4932  else
4933  return false;
4934 }
4935 
4936 //PWM interface
4938 {
4939  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
4940 
4941  eOmc_setpoint_t setpoint;
4942 
4943  setpoint.type = (eOenum08_t)eomc_setpoint_openloop;
4944  setpoint.to.openloop.value = (eOmeas_pwm_t)S_16(v);
4945 
4946  return res->setRemoteValue(protid, &setpoint);
4947 }
4948 
4950 {
4951  bool ret = true;
4952  for (int j = 0; j<_njoints; j++)
4953  {
4954  ret = ret && setRefDutyCycleRaw(j, v[j]);
4955  }
4956  return ret;
4957 }
4958 
4960 {
4961  eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_target);
4962  uint16_t size = 0;
4963  *v = 0;
4964  eOmc_joint_status_target_t target = { 0 };
4965 
4966 
4967  if (!askRemoteValue(protoId, &target, size))
4968  {
4969  yError() << "embObjMotionControl::getRefDutyCycleRaw() could not read openloop reference for " << getBoardInfo() << "joint " << j;
4970  return false;
4971  }
4972 
4973  *v = (double)target.trgt_pwm;
4974 
4975  return true;
4976 }
4977 
4979 {
4980  std::vector <eOmc_joint_status_target_t> targetList(_njoints);
4981  bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_target, targetList);
4982  if(!ret)
4983  {
4984  yError() << "embObjMotionControl::getDutyCyclesRaw failed for all joints of" << getBoardInfo();
4985  }
4986 
4987  for (int j = 0; j<_njoints; j++)
4988  {
4989  v[j]= targetList[j].trgt_pwm;
4990  }
4991  return ret;
4992 }
4993 
4995 {
4996  eOmc_motor_status_basic_t status;
4997  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_status_basic);
4998 
4999  bool ret = res->getLocalValue(protid, &status);
5000  if (ret)
5001  {
5002  *v = (double)status.mot_pwm;
5003  }
5004  else
5005  {
5006  yError() << "embObjMotionControl::getDutyCycleRaw failed for" << getBoardInfo() << " motor " << j;
5007  *v = 0;
5008  }
5009 
5010  return ret;
5011 }
5012 
5014 {
5015  bool ret = true;
5016  for (int j = 0; j< _njoints; j++)
5017  {
5018  ret &= getDutyCycleRaw(j, &v[j]);
5019  }
5020  return ret;
5021 }
5022 
5023 // Current interface
5024 
5025 bool embObjMotionControl::getCurrentRangeRaw(int j, double *min, double *max)
5026 {
5027  //this should be completed with numbers obtained from configuration files.
5028  //some caveats: currently current limits are expressed in robot configuration files in milliAmperes. Amperes should be used instead.
5029  //yarp does not perform any conversion on these numbers. Should it?
5030  *min = -10000.0;
5031  *max = 10000.0;
5032  return true;
5033 }
5034 
5036 {
5037  bool ret = true;
5038  for (int j = 0; j< _njoints; j++)
5039  {
5040  ret &= getCurrentRangeRaw(j, &min[j], &max[j]);
5041  }
5042  return ret;
5043 }
5044 
5046 {
5047  bool ret = true;
5048  for (int j = 0; j<_njoints; j++)
5049  {
5050  ret = ret && setRefCurrentRaw(j, t[j]);
5051  }
5052  return ret;
5053 }
5054 
5056 {
5057  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
5058 
5059  eOmc_setpoint_t setpoint;
5060 
5061  setpoint.type = (eOenum08_t)eomc_setpoint_current;
5062  setpoint.to.current.value = (eOmeas_pwm_t)S_16(t);
5063 
5064  return res->setRemoteValue(protid, &setpoint);
5065 }
5066 
5067 bool embObjMotionControl::setRefCurrentsRaw(const int n_joint, const int *joints, const double *t)
5068 {
5069  bool ret = true;
5070  for (int j = 0; j<n_joint; j++)
5071  {
5072  ret = ret && setRefCurrentRaw(joints[j], t[j]);
5073  }
5074  return ret;
5075 }
5076 
5078 {
5079  std::vector <eOmc_joint_status_target_t> targetList(_njoints);
5080  bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_target, targetList);
5081  if (!ret)
5082  {
5083  yError() << "embObjMotionControl::getDutyCyclesRaw failed for all joints of" << getBoardInfo();
5084  }
5085 
5086  for (int j = 0; j<_njoints; j++)
5087  {
5088  t[j] = targetList[j].trgt_current;
5089  }
5090  return ret;
5091 }
5092 
5094 {
5095  eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_target);
5096  uint16_t size = 0;
5097  *t = 0;
5098  eOmc_joint_status_target_t target = { 0 };
5099 
5100 
5101  if (!askRemoteValue(protoId, &target, size))
5102  {
5103  yError() << "embObjMotionControl::getRefDutyCycleRaw() could not read openloop reference for " << getBoardInfo() << "joint " << j;
5104  return false;
5105  }
5106 
5107  *t = (double)target.trgt_current;
5108 
5109  return true;
5110 }
5111 
5112 bool embObjMotionControl::helper_setCurPidRaw(int j, const Pid &pid)
5113 {
5114  eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_pidcurrent);
5115  eOmc_PID_t outPid;
5116  Pid hwPid = pid;
5117 
5118  if (!_cur_pids[j].enabled)
5119  {
5120  yError() << "eoMc " << getBoardInfo() << ": it is not possible set current pid for motor " << j << ", because current pid is not enabled in xml files";
5121  return false;
5122  }
5123 
5124  copyPid_iCub2eo(&hwPid, &outPid);
5125 
5126  if (false == res->setRemoteValue(protoId, &outPid))
5127  {
5128  yError() << "while setting velocity PIDs for" << getBoardInfo() << " joint " << j;
5129  return false;
5130  }
5131 
5132  return true;
5133 
5134  //return NOT_YET_IMPLEMENTED("Our boards do not have a Velocity Pid");
5135 }
5136 
5137 bool embObjMotionControl::helper_setSpdPidRaw(int j, const Pid &pid)
5138 {
5139  eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_pidspeed);
5140  eOmc_PID_t outPid;
5141  Pid hwPid = pid;
5142 
5143  if (!_cur_pids[j].enabled)
5144  {
5145  yError() << "eoMc " << getBoardInfo() << ": it is not possible set speed pid for motor " << j << ", because speed pid is not enabled in xml files";
5146  return false;
5147  }
5148 
5149  copyPid_iCub2eo(&hwPid, &outPid);
5150 
5151  if (false == res->setRemoteValue(protoId, &outPid))
5152  {
5153  yError() << "while setting velocity PIDs for" << getBoardInfo() << " joint " << j;
5154  return false;
5155  }
5156 
5157  return true;
5158 
5159  //return NOT_YET_IMPLEMENTED("Our boards do not have a Velocity Pid");
5160 }
5161 
5162 bool embObjMotionControl::helper_getCurPidRaw(int j, Pid *pid)
5163 {
5164  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
5165  uint16_t size;
5166  eOmc_motor_config_t motor_cfg;
5167  if(! askRemoteValue(protoid, &motor_cfg, size))
5168  return false;
5169 
5170  // refresh cached value when reading data from the EMS
5171  eOmc_PID_t tmp = (eOmc_PID_t)motor_cfg.pidcurrent;
5172  copyPid_eo2iCub(&tmp, pid);
5173 
5174  return true;
5175 }
5176 
5177 bool embObjMotionControl::helper_getSpdPidRaw(int j, Pid *pid)
5178 {
5179  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
5180  uint16_t size;
5181  eOmc_motor_config_t motor_cfg;
5182  if (!askRemoteValue(protoid, &motor_cfg, size))
5183  return false;
5184 
5185  // refresh cached value when reading data from the EMS
5186  eOmc_PID_t tmp = (eOmc_PID_t)motor_cfg.pidspeed;
5187  copyPid_eo2iCub(&tmp, pid);
5188 
5189  return true;
5190 }
5191 
5192 bool embObjMotionControl::helper_getCurPidsRaw(Pid *pid)
5193 {
5194  std::vector <eOmc_motor_config_t> motor_cfg_list(_njoints);
5195  bool ret = askRemoteValues<eOmc_motor_config_t>(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, eoprot_tag_mc_motor_config, motor_cfg_list);
5196  if(! ret)
5197  return false;
5198 
5199  for(int j=0; j<_njoints; j++)
5200  {
5201  eOmc_PID_t tmp = (eOmc_PID_t)motor_cfg_list[j].pidcurrent;
5202  copyPid_eo2iCub(&tmp, &pid[j]);
5203  }
5204  return true;
5205 }
5206 
5207 bool embObjMotionControl::helper_getSpdPidsRaw(Pid *pid)
5208 {
5209  std::vector <eOmc_motor_config_t> motor_cfg_list(_njoints);
5210  bool ret = askRemoteValues<eOmc_motor_config_t>(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, eoprot_tag_mc_motor_config, motor_cfg_list);
5211  if (!ret)
5212  return false;
5213 
5214  for (int j = 0; j<_njoints; j++)
5215  {
5216  eOmc_PID_t tmp = (eOmc_PID_t)motor_cfg_list[j].pidspeed;
5217  copyPid_eo2iCub(&tmp, &pid[j]);
5218  }
5219  return true;
5220 }
5221 
5222 bool embObjMotionControl::getJointConfiguration(int joint, eOmc_joint_config_t *jntCfg_ptr)
5223 {
5224  uint32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joint, eoprot_tag_mc_joint_config);
5225  uint16_t size;
5226  if(!askRemoteValue(protoid, jntCfg_ptr, size))
5227  {
5228  yError ("Failure of askRemoteValue() inside embObjMotionControl::getJointConfiguration(axis=%d) for BOARD %s IP %s", joint, res->getProperties().boardnameString.c_str(), res->getProperties().ipv4addrString.c_str());
5229  return false;
5230  }
5231  return true;
5232 }
5233 
5234 bool embObjMotionControl::getMotorConfiguration(int axis, eOmc_motor_config_t *motCfg_ptr)
5235 {
5236  uint32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, axis, eoprot_tag_mc_motor_config);
5237  uint16_t size;
5238  if(!askRemoteValue(protoid, motCfg_ptr, size))
5239  {
5240  yError ("Failure of askRemoteValue() inside embObjMotionControl::getMotorConfiguration(axis=%d) for BOARD %s IP %s", axis, res->getProperties().boardnameString.c_str(), res->getProperties().ipv4addrString.c_str());
5241  return false;
5242  }
5243  return true;
5244 }
5245 
5246 
5247 bool embObjMotionControl::getGerabox_E2J(int joint, double *gearbox_E2J_ptr)
5248 {
5249  eOmc_joint_config_t jntCfg;
5250 
5251  if(!getJointConfiguration(joint, &jntCfg))
5252  {
5253  yError ("Failure embObjMotionControl::getGerabox_E2J(axis=%d) for BOARD %s IP %s", joint, res->getProperties().boardnameString.c_str(), res->getProperties().ipv4addrString.c_str());
5254  return false;
5255  }
5256  *gearbox_E2J_ptr = jntCfg.gearbox_E2J;
5257  return true;
5258 }
5259 
5260 bool embObjMotionControl::getJointEncTolerance(int joint, double *jEncTolerance_ptr)
5261 {
5262  eOmc_joint_config_t jntCfg;
5263 
5264  if(!getJointConfiguration(joint, &jntCfg))
5265  {
5266  yError ("Failure embObjMotionControl::getJointEncTolerance(axis=%d) for BOARD %s IP %s", joint, res->getProperties().boardnameString.c_str(), res->getProperties().ipv4addrString.c_str());
5267  return false;
5268  }
5269  *jEncTolerance_ptr = jntCfg.jntEncTolerance;
5270  return true;
5271 }
5272 
5273 bool embObjMotionControl::getMotorEncTolerance(int axis, double *mEncTolerance_ptr)
5274 {
5275  eOmc_motor_config_t motorCfg;
5276  if(!getMotorConfiguration(axis, &motorCfg))
5277  {
5278  yError ("Failure embObjMotionControl::getMotorEncTolerance(axis=%d) for BOARD %s IP %s", axis, res->getProperties().boardnameString.c_str(), res->getProperties().ipv4addrString.c_str());
5279  return false;
5280  }
5281  *mEncTolerance_ptr = motorCfg.rotEncTolerance;
5282  return true;
5283 }
5284 
5285 bool embObjMotionControl::getLastJointFaultRaw(int j, int& fault, std::string& message)
5286 {
5287  eOmc_motor_status_t status;
5288 
5289  eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol,
5290  eoprot_entity_mc_motor, j,
5291  eoprot_tag_mc_motor_status);
5292 
5293  bool ret = res->getLocalValue(protid, &status);
5294 
5295  message.clear();
5296 
5297  if (!ret)
5298  {
5299  fault = -1;
5300  message = "Could not retrieve the fault state.";
5301  return false;
5302  }
5303 
5304  if (status.mc_fault_state == EOERROR_CODE_DUMMY)
5305  {
5306  fault = EOERROR_CODE_DUMMY;
5307  message = "No fault detected.";
5308 
5309  return true;
5310  }
5311 
5312  fault = eoerror_code2value(status.mc_fault_state);
5313  message = eoerror_code2string(status.mc_fault_state);
5314 
5315  return true;
5316 }
5317 
5318 // eof
#define MAX_POSITION_MOVE_INTERVAL
bool parseService(yarp::os::Searchable &config, servConfigMais_t &maisconfig)
servMC_encoder_t * getEncoderAtMotor(int index)
servMC_encoder_t * getEncoderAtJoint(int index)
virtual const Properties & getProperties()=0
virtual bool setcheckRemoteValue(const eOprotID32_t id32, void *value, const unsigned int retries=10, const double waitbeforecheck=0.001, const double timeout=0.050)=0
virtual bool serviceVerifyActivate(eOmn_serv_category_t category, const eOmn_serv_parameter_t *param, double timeout=0.500)=0
virtual bool setRemoteValue(const eOprotID32_t id32, void *value)=0
virtual bool verifyEPprotocol(eOprot_endpoint_t ep)=0
virtual bool getLocalValue(const eOprotID32_t id32, void *value)=0
virtual bool serviceSetRegulars(eOmn_serv_category_t category, vector< eOprotID32_t > &id32vector, double timeout=0.500)=0
virtual bool serviceStart(eOmn_serv_category_t category, double timeout=0.500)=0
virtual bool getRemoteValue(const eOprotID32_t id32, void *value, const double timeout=0.100, const unsigned int retries=0)=0
virtual bool getRemoteValues(const std::vector< eOprotID32_t > &id32s, const std::vector< void * > &values, const double timeout=0.500)=0
int releaseResource2(eth::AbstractEthResource *ethresource, IethResource *interface)
Definition: ethManager.cpp:409
bool verifyEthBoardInfo(yarp::os::Searchable &cfgtotal, eOipv4addr_t &boardipv4, string boardipv4string, string boardname)
Definition: ethManager.cpp:283
static bool killYourself()
Definition: ethManager.cpp:178
static TheEthManager * instance()
Definition: ethManager.cpp:159
eth::AbstractEthResource * requestResource2(IethResource *interface, yarp::os::Searchable &cfgtotal)
Definition: ethManager.cpp:336
bool start()
Instantiates the yarp::os::Timer object and starts it.
bool canprint()
Called by the object that implements the downsampler.
virtual bool getPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double *ref) override
virtual bool setVelLimitsRaw(int axis, double min, double max) override
virtual bool setMaxCurrentRaw(int j, double val) override
virtual bool getMotorEncoderRaw(int m, double *v) override
virtual bool enablePidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool setPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, const double *limits) override
virtual bool setRefSpeedsRaw(const double *spds) override
virtual bool getTorqueRangesRaw(double *min, double *max) override
virtual bool getControlModesRaw(int *v) override
virtual bool getRefCurrentRaw(int j, double *t) override
virtual bool setInteractionModeRaw(int j, yarp::dev::InteractionModeEnum _mode) override
virtual bool getPidOutputRaw(const PidControlTypeEnum &pidtype, int j, double *out) override
virtual bool getInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
virtual bool setInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
bool getRotorEncoderTypeRaw(int j, int &type)
virtual bool setRefAccelerationsRaw(const double *accs) override
virtual bool disablePidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool getImpedanceRaw(int j, double *stiffness, double *damping) override
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
virtual bool setPidOffsetRaw(const PidControlTypeEnum &pidtype, int j, double v) override
virtual bool getPowerSupplyVoltageRaw(int j, double *val) override
virtual bool setPidsRaw(const PidControlTypeEnum &pidtype, const Pid *pids) override
virtual bool getPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double *limit) override
virtual bool setPeakCurrentRaw(int m, const double val) override
virtual bool enableAmpRaw(int j) override
bool getHasRotorEncoderRaw(int j, int &ret)
virtual bool setRefTorqueRaw(int j, double t) override
virtual bool relativeMoveRaw(int j, double delta) override
virtual bool disableAmpRaw(int j) override
virtual bool getRefVelocityRaw(const int joint, double *ref) override
virtual bool getEncodersRaw(double *encs) override
virtual bool getLastJointFaultRaw(int j, int &fault, std::string &message) override
virtual bool getDutyCyclesRaw(double *v) override
virtual bool getPidReferencesRaw(const PidControlTypeEnum &pidtype, double *refs) override
virtual bool getMotorEncoderAccelerationsRaw(double *accs) override
virtual bool resetMotorEncoderRaw(int m) override
virtual bool checkMotionDoneRaw(bool *flag) override
virtual bool getPWMLimitRaw(int j, double *val) override
virtual bool getControlModeRaw(int j, int *v) override
virtual bool getPWMRaw(int j, double *val) override
bool getRotorLimitsRaw(int j, double *rotorMin, double *rotorMax)
virtual bool getTorqueRaw(int j, double *t) override
virtual bool getMotorEncodersRaw(double *encs) override
virtual bool open(yarp::os::Searchable &par)
virtual bool getEncodersTimedRaw(double *encs, double *stamps) override
bool getHasHallSensorRaw(int j, int &ret)
bool getJointEncoderResolutionRaw(int m, double &jntres)
virtual bool getRefSpeedsRaw(double *spds) override
virtual bool getInteractionModeRaw(int j, yarp::dev::InteractionModeEnum *_mode) override
virtual eth::iethresType_t type()
virtual bool getCurrentRangesRaw(double *min, double *max) override
virtual bool getLimitsRaw(int axis, double *min, double *max) override
virtual bool setPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double ref) override
virtual bool updateVirtualAnalogSensorMeasure(yarp::sig::Vector &fTorques) override
virtual bool setImpedanceRaw(int j, double stiffness, double damping) override
virtual bool setRemoteVariableRaw(std::string key, const yarp::os::Bottle &val) override
virtual bool getCurrentsRaw(double *vals) override
virtual bool getTorqueRangeRaw(int j, double *min, double *max) override
virtual bool getTorquesRaw(double *t) override
virtual bool getAmpStatusRaw(int *st) override
virtual yarp::dev::VAS_status getVirtualAnalogSensorStatus(int ch) override
bool getHasTempSensorsRaw(int j, int &ret)
virtual bool setRefCurrentsRaw(const double *t) override
virtual bool getTargetPositionRaw(const int joint, double *ref) override
virtual bool setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override
virtual bool getGearboxRatioRaw(int m, double *gearbox) override
virtual bool resetEncodersRaw() override
virtual bool getTemperatureLimitRaw(int m, double *temp) override
virtual bool setCalibrationParametersRaw(int axis, const CalibrationParameters &params) override
virtual bool setNominalCurrentRaw(int m, const double val) override
virtual bool setMotorTorqueParamsRaw(int j, const MotorTorqueParameters params) override
virtual bool setRefCurrentRaw(int j, double t) override
virtual bool setTemperatureLimitRaw(int m, const double temp) override
virtual bool getRemoteVariableRaw(std::string key, yarp::os::Bottle &val) override
virtual bool calibrationDoneRaw(int j) override
virtual bool getEncoderTimedRaw(int j, double *encs, double *stamp) override
virtual bool getAxisNameRaw(int axis, std::string &name) override
virtual bool setPWMLimitRaw(int j, const double val) override
virtual bool getRefAccelerationRaw(int j, double *acc) override
virtual bool resetPidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool getPidRaw(const PidControlTypeEnum &pidtype, int j, Pid *pid) override
virtual bool setPositionRaw(int j, double ref) override
virtual bool getRefSpeedRaw(int j, double *ref) override
virtual bool setPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double limit) override
virtual bool setControlModesRaw(const int n_joint, const int *joints, int *modes) override
bool getRotorEncoderResolutionRaw(int m, double &rotres)
virtual bool setRefDutyCycleRaw(int j, double v) override
virtual bool getImpedanceOffsetRaw(int j, double *offset) override
bool getWholeImpedanceRaw(int j, eOmc_impedance_t &imped)
virtual bool getEncoderSpeedsRaw(double *spds) override
virtual bool setPositionsRaw(const int n_joint, const int *joints, const double *refs) override
virtual bool resetMotorEncodersRaw() override
bool getJointEncoderTypeRaw(int j, int &type)
virtual bool getNumberOfMotorEncodersRaw(int *num) override
virtual bool getPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, double *limits) override
virtual bool setMotorEncoderRaw(int m, const double val) override
virtual bool getMotorEncoderTimedRaw(int m, double *encs, double *stamp) override
virtual bool setEncoderRaw(int j, double val) override
virtual bool resetEncoderRaw(int j) override
virtual bool setPidRaw(const PidControlTypeEnum &pidtype, int j, const Pid &pid) override
virtual bool getRefAccelerationsRaw(double *accs) override
virtual bool getPidOutputsRaw(const PidControlTypeEnum &pidtype, double *outs) override
virtual bool getRefPositionsRaw(double *refs) override
virtual bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
virtual bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override
virtual bool getNominalCurrentRaw(int m, double *val) override
virtual bool setRefTorquesRaw(const double *t) override
virtual bool getRefDutyCyclesRaw(double *v) override
virtual bool getCurrentRaw(int j, double *val) override
bool getMotorPolesRaw(int j, int &poles)
virtual bool getCurrentRangeRaw(int j, double *min, double *max) override
virtual bool getRefTorquesRaw(double *t) override
virtual bool setEncodersRaw(const double *vals) override
virtual bool getEncoderRaw(int j, double *v) override
virtual bool setControlModeRaw(const int j, const int mode) override
virtual bool positionMoveRaw(int j, double ref) override
bool getKinematicMJRaw(int j, double &rotres)
virtual bool setLimitsRaw(int axis, double min, double max) override
virtual bool getEncoderAccelerationRaw(int j, double *spds) override
virtual bool getMotorTorqueParamsRaw(int j, MotorTorqueParameters *params) override
virtual bool getTemperatureRaw(int m, double *val) override
virtual bool setPidReferencesRaw(const PidControlTypeEnum &pidtype, const double *refs) override
virtual bool getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override
virtual bool getVelLimitsRaw(int axis, double *min, double *max) override
virtual bool getRefTorqueRaw(int j, double *t) override
virtual bool getNumberOfMotorsRaw(int *num) override
virtual bool setRefSpeedRaw(int j, double sp) override
virtual bool getMotorEncoderSpeedsRaw(double *spds) override
virtual bool getMaxCurrentRaw(int j, double *val) override
virtual bool getTemperaturesRaw(double *vals) override
virtual bool getMotorEncoderCountsPerRevolutionRaw(int m, double *v) override
virtual bool getEncoderAccelerationsRaw(double *accs) override
virtual bool getPidsRaw(const PidControlTypeEnum &pidtype, Pid *pids) override
virtual bool getRemoteVariablesListRaw(yarp::os::Bottle *listOfKeys) override
bool getTorqueControlFilterType(int j, int &type)
virtual bool getRefCurrentsRaw(double *t) override
virtual bool getPidErrorRaw(const PidControlTypeEnum &pidtype, int j, double *err) override
virtual bool setImpedanceOffsetRaw(int j, double offset) override
virtual bool getRefDutyCycleRaw(int j, double *v) override
virtual bool getMotorEncodersTimedRaw(double *encs, double *stamps) override
virtual int getVirtualAnalogSensorChannels() override
bool getRotorIndexOffsetRaw(int j, double &rotorOffset)
virtual bool setMotorEncodersRaw(const double *vals) override
virtual bool isPidEnabledRaw(const PidControlTypeEnum &pidtype, int j, bool *enabled) override
virtual bool setRefAccelerationRaw(int j, double acc) override
virtual bool getEncoderSpeedRaw(int j, double *sp) override
virtual bool getMotorEncoderSpeedRaw(int m, double *sp) override
virtual bool getMotorEncoderAccelerationRaw(int m, double *spds) override
virtual bool getAxes(int *ax) override
virtual bool getRefPositionRaw(const int joint, double *ref) override
virtual bool setRefDutyCyclesRaw(const double *v) override
virtual bool velocityMoveRaw(int j, double sp) override
virtual bool getPeakCurrentRaw(int m, double *val) override
virtual bool getPidErrorsRaw(const PidControlTypeEnum &pidtype, double *errs) override
virtual bool getRefVelocitiesRaw(double *refs) override
virtual bool getTargetPositionsRaw(double *refs) override
bool getHasRotorEncoderIndexRaw(int j, int &ret)
virtual bool getDutyCycleRaw(int j, double *v) override
bool parseJointsetCfgGroup(yarp::os::Searchable &config, std::vector< JointsSet > &jsets, std::vector< int > &jointtoset)
bool parseJointsLimits(yarp::os::Searchable &config, std::vector< jointLimits_t > &jointsLimits)
bool parseAmpsToSensor(yarp::os::Searchable &config, double ampsToSensor[])
bool parseMechanicalsFlags(yarp::os::Searchable &config, int useMotorSpeedFbk[])
bool parseMotioncontrolVersion(yarp::os::Searchable &config, int &version)
bool parseAxisInfo(yarp::os::Searchable &config, int axisMap[], std::vector< axisInfo_t > &axisInfo)
bool parseCouplingInfo(yarp::os::Searchable &config, couplingInfo_t &couplingInfo)
bool parsePids(yarp::os::Searchable &config, PidInfo *ppids, TrqPidInfo *tpids, PidInfo *cpids, PidInfo *spids, bool lowLevPidisMandatory)
Definition: eomcParser.cpp:77
bool parseFocGroup(yarp::os::Searchable &config, focBasedSpecificInfo_t *foc_based_info, std::string groupName)
bool parseDeadzoneValue(yarp::os::Searchable &config, double deadzone[], bool *found)
bool isVerboseEnabled(yarp::os::Searchable &config)
bool parseTimeoutsGroup(yarp::os::Searchable &config, std::vector< timeouts_t > &timeouts, int defaultVelocityTimeout)
bool parseCurrentLimits(yarp::os::Searchable &config, std::vector< motorCurrentLimits_t > &currLimits)
bool parseEncoderFactor(yarp::os::Searchable &config, double encoderFactor[])
bool parsefullscalePWM(yarp::os::Searchable &config, double dutycycleToPWM[])
bool parseRotorsLimits(yarp::os::Searchable &config, std::vector< rotorLimits_t > &rotorsLimits)
bool parseImpedanceGroup(yarp::os::Searchable &config, std::vector< impedanceParameters_t > &impedance)
bool parseKalmanFilterParams(yarp::os::Searchable &config, std::vector< kalmanFilterParams_t > &kalmanFilterParams)
bool parseGearboxValues(yarp::os::Searchable &config, double gearbox_M2J[], double gearbox_E2J[])
bool parseBehaviourFalgs(yarp::os::Searchable &config, bool &useRawEncoderData, bool &pwmIsLimited)
std::string usernamePidSelected
Definition: eomcParser.h:170
yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits
Definition: eomcParser.h:164
eOmc_ctrl_out_type_t out_type
Definition: eomcParser.h:166
yarp::dev::PidOutputUnitsEnum out_PidUnits
Definition: eomcParser.h:165
_3f_vect_t acc
Definition: dataTypes.h:1
int n
static uint32_t idx[BOARD_NUM]
static bool nv_not_found(void)
static bool NOT_YET_IMPLEMENTED(const char *txt)
#define PARSER_MOTION_CONTROL_VERSION
static bool DEPRECATED(const char *txt)
int jnts
Definition: main.cpp:60
iethresType_t
Definition: IethResource.h:61
@ iethres_motioncontrol
Definition: IethResource.h:66
const FSC max
Definition: strain.h:48
const FSC min
Definition: strain.h:49
int S_32(double x)
Definition: eomcUtils.h:70
bool EncoderType_eo2iCub(const uint8_t *in, string *out)
Definition: eomcUtils.h:125
int controlModeStatusConvert_embObj2yarp(eOenum08_t embObjMode)
Definition: eomcUtils.h:258
bool interactionModeStatusConvert_embObj2yarp(eOenum08_t embObjMode, int &vocabOut)
Definition: eomcUtils.h:384
bool controlModeCommandConvert_yarp2embObj(int vocabMode, eOenum08_t &embOut)
Definition: eomcUtils.h:201
void copyPid_eo2iCub(eOmc_PID_t *in, Pid *out)
Definition: eomcUtils.h:186
short S_16(double x)
Definition: eomcUtils.h:43
void copyPid_iCub2eo(const Pid *in, eOmc_PID_t *out)
Definition: eomcUtils.h:171
bool interactionModeCommandConvert_yarp2embObj(int vocabMode, eOenum08_t &embOut)
Definition: eomcUtils.h:362
int U_16(double x)
Definition: eomcUtils.h:31
out
Definition: sine.m:8
degrees offset
Definition: sine.m:4
std::vector< BufferedPort< Bottle > * > ports
eOmn_serv_diagn_cfg_t config
eOmn_serv_parameter_t ethservice
Definition: serviceParser.h:98
eOmc_encoder_descriptor_t desc
bool useRawEncoderData
its value depends on environment variable "ETH_VERBOSEWHENOK"
bool pwmIsLimited
if true than do not use calibration data
std::vector< double > matrixM2J
Definition: eomcParser.h:280
std::vector< double > matrixE2J
Definition: eomcParser.h:281
std::vector< double > matrixJ2M
Definition: eomcParser.h:279