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