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