27 #ifndef __embObjMotionControlh__
28 #define __embObjMotionControlh__
36 #include <yarp/os/Bottle.h>
37 #include <yarp/os/BufferedPort.h>
38 #include <yarp/os/Timer.h>
39 #include <yarp/dev/DeviceDriver.h>
40 #include <yarp/dev/PolyDriver.h>
41 #include <yarp/dev/ControlBoardHelper.h>
43 #include <yarp/dev/IVirtualAnalogSensor.h>
45 #include<yarp/dev/ImplementJointFault.h>
60 #ifdef NETWORK_PERFORMANCE_BENCHMARK
66 #undef VERIFY_ROP_SETIMPEDANCE
68 #undef VERIFY_ROP_SETPOSITIONRAW
76 #define EMBOBJMC_DONT_USE_MAIS
121 std::vector<BufferedPort<Bottle>*>
ports;
147 public IPidControlRaw,
148 public IControlCalibrationRaw,
149 public IAmplifierControlRaw,
150 public IEncodersTimedRaw,
151 public IMotorEncodersRaw,
152 public ImplementEncodersTimed,
153 public ImplementMotorEncoders,
155 public ImplementMotor,
156 public IPositionControlRaw,
157 public IVelocityControlRaw,
158 public IControlModeRaw,
159 public ImplementControlMode,
160 public IControlLimitsRaw,
161 public IImpedanceControlRaw,
162 public ImplementImpedanceControl,
163 public ImplementControlLimits,
164 public ImplementAmplifierControl,
165 public ImplementPositionControl,
166 public ImplementControlCalibration,
167 public ImplementPidControl,
168 public ImplementVelocityControl,
169 public ITorqueControlRaw,
170 public ImplementTorqueControl,
171 public IVirtualAnalogSensor,
172 public IPositionDirectRaw,
173 public ImplementPositionDirect,
174 public IInteractionModeRaw,
175 public ImplementInteractionMode,
176 public IRemoteVariablesRaw,
177 public ImplementRemoteVariables,
179 public ImplementAxisInfo,
180 public IPWMControlRaw,
181 public ImplementPWMControl,
182 public ICurrentControlRaw,
183 public ImplementCurrentControl,
185 public IJointFaultRaw,
186 public ImplementJointFault
194 ControlBoardHelper* _measureConverter;
205 double * _gearbox_M2J;
206 double * _gearbox_E2J;
208 std::vector<eomc::kalmanFilterParams_t> _kalman_params;
212 std::vector<eomc::encoder_t> _jointEncs;
213 std::vector<eomc::encoder_t> _motorEncs;
215 std::vector<eomc::rotorLimits_t> _rotorsLimits;
216 std::vector<eomc::jointLimits_t> _jointsLimits;
217 std::vector<eomc::motorCurrentLimits_t> _currentLimits;
219 std::vector<eomc::JointsSet> _jsets;
220 std::vector<int> _joint2set;
221 std::vector<eomc::timeouts_t> _timeouts;
223 std::vector<eomc::impedanceParameters_t> _impedance_params;
234 std::vector<eomc::axisInfo_t> _axesInfo;
240 #ifdef VERIFY_ROP_SETIMPEDANCE
241 uint32_t *impedanceSignature;
244 #ifdef VERIFY_ROP_SETPOSITIONRAW
245 uint32_t *refRawSignature;
246 bool *sendingDirects;
251 double SAFETY_THRESHOLD;
258 double *_ref_command_positions;
260 double *_ref_command_speeds;
261 double *_ref_positions;
263 double *_encodersStamp;
264 bool *checking_motiondone;
265 #define MAX_POSITION_MOVE_INTERVAL 0.080
266 double *_last_position_move_time;
267 eOmc_impedance_t *_cacheImpedance;
270 #ifdef NETWORK_PERFORMANCE_BENCHMARK
271 Tools:Emb_RensponseTimingVerifier m_responseTimingVerifier;
276 std::string getBoardInfo(
void);
277 bool askRemoteValue(eOprotID32_t id32,
void* value, uint16_t& size);
279 bool askRemoteValues(eOprotEndpoint_t ep, eOprotEntity_t entity, eOprotTag_t tag, std::vector<T>& values);
280 bool checkRemoteControlModeStatus(
int joint,
int target_mode);
285 bool verifyUserControlLawConsistencyInJointSet(
eomc::PidInfo *ipdInfo);
287 bool verifyTorquePidshasSameUnitTypes(yarp::dev::PidFeedbackUnitsEnum &fbk_pidunits, yarp::dev::PidOutputUnitsEnum& out_pidunits);
288 bool verifyUseMotorSpeedFbkInJointSet(
int useMotorSpeedFbk []);
289 bool updatedJointsetsCfgWithControlInfo(
void);
290 bool saveCouplingsData(
void);
291 void debugUtil_printJointsetInfo(
void);
293 bool isTorqueControlEnabled(
int joint);
294 bool isVelocityControlEnabled(
int joint);
296 bool iNeedCouplingsInfo(
void);
298 bool getJointConfiguration(
int joint, eOmc_joint_config_t *jntCfg_ptr);
299 bool getMotorConfiguration(
int axis, eOmc_motor_config_t *motCfg_ptr);
300 bool getGerabox_E2J(
int joint,
double *gearbox_E2J_ptr);
301 bool getJointEncTolerance(
int joint,
double *jEncTolerance_ptr);
302 bool getMotorEncTolerance(
int axis,
double *mEncTolerance_ptr);
303 void updateDeadZoneWithDefaultValues(
void);
304 bool getJointDeadZoneRaw(
int j,
double &jntDeadZone);
309 bool fromConfig(yarp::os::Searchable &config);
310 int fromConfig_NumOfJoints(yarp::os::Searchable &config);
311 bool fromConfig_getGeneralInfo(yarp::os::Searchable &config);
312 bool fromConfig_Step2(yarp::os::Searchable &config);
313 bool fromConfig_readServiceCfg(yarp::os::Searchable &config);
315 bool alloc(
int njoints);
322 bool helper_setPosPidRaw(
int j,
const Pid &pid);
323 bool helper_getPosPidRaw(
int j, Pid *pid);
324 bool helper_getPosPidsRaw(Pid *pid);
327 bool helper_setTrqPidRaw(
int j,
const Pid &pid);
328 bool helper_getTrqPidRaw(
int j, Pid *pid);
329 bool helper_getTrqPidsRaw(Pid *pid);
332 bool helper_setVelPidRaw(
int j,
const Pid &pid);
333 bool helper_getVelPidRaw(
int j, Pid *pid);
334 bool helper_getVelPidsRaw(Pid *pid);
337 bool helper_setCurPidRaw(
int j,
const Pid &pid);
338 bool helper_getCurPidRaw(
int j, Pid *pid);
339 bool helper_getCurPidsRaw(Pid *pid);
342 bool helper_setSpdPidRaw(
int j,
const Pid &pid);
343 bool helper_getSpdPidRaw(
int j, Pid *pid);
344 bool helper_getSpdPidsRaw(Pid *pid);
353 virtual bool open(yarp::os::Searchable &par);
354 virtual bool close();
356 virtual bool initialised();
358 virtual bool update(eOprotID32_t id32,
double timestamp,
void *rxdata);
361 virtual bool setPidRaw(
const PidControlTypeEnum& pidtype,
int j,
const Pid &pid)
override;
362 virtual bool setPidsRaw(
const PidControlTypeEnum& pidtype,
const Pid *pids)
override;
363 virtual bool setPidReferenceRaw(
const PidControlTypeEnum& pidtype,
int j,
double ref)
override;
364 virtual bool setPidReferencesRaw(
const PidControlTypeEnum& pidtype,
const double *refs)
override;
365 virtual bool setPidErrorLimitRaw(
const PidControlTypeEnum& pidtype,
int j,
double limit)
override;
366 virtual bool setPidErrorLimitsRaw(
const PidControlTypeEnum& pidtype,
const double *limits)
override;
367 virtual bool getPidErrorRaw(
const PidControlTypeEnum& pidtype,
int j,
double *err)
override;
368 virtual bool getPidErrorsRaw(
const PidControlTypeEnum& pidtype,
double *errs)
override;
369 virtual bool getPidOutputRaw(
const PidControlTypeEnum& pidtype,
int j,
double *
out)
override;
370 virtual bool getPidOutputsRaw(
const PidControlTypeEnum& pidtype,
double *outs)
override;
371 virtual bool getPidRaw(
const PidControlTypeEnum& pidtype,
int j, Pid *pid)
override;
372 virtual bool getPidsRaw(
const PidControlTypeEnum& pidtype, Pid *pids)
override;
373 virtual bool getPidReferenceRaw(
const PidControlTypeEnum& pidtype,
int j,
double *ref)
override;
374 virtual bool getPidReferencesRaw(
const PidControlTypeEnum& pidtype,
double *refs)
override;
375 virtual bool getPidErrorLimitRaw(
const PidControlTypeEnum& pidtype,
int j,
double *limit)
override;
376 virtual bool getPidErrorLimitsRaw(
const PidControlTypeEnum& pidtype,
double *limits)
override;
377 virtual bool resetPidRaw(
const PidControlTypeEnum& pidtype,
int j)
override;
378 virtual bool disablePidRaw(
const PidControlTypeEnum& pidtype,
int j)
override;
379 virtual bool enablePidRaw(
const PidControlTypeEnum& pidtype,
int j)
override;
380 virtual bool setPidOffsetRaw(
const PidControlTypeEnum& pidtype,
int j,
double v)
override;
381 virtual bool isPidEnabledRaw(
const PidControlTypeEnum& pidtype,
int j,
bool* enabled)
override;
384 virtual bool getAxes(
int *ax)
override;
385 virtual bool positionMoveRaw(
int j,
double ref)
override;
386 virtual bool positionMoveRaw(
const double *refs)
override;
387 virtual bool relativeMoveRaw(
int j,
double delta)
override;
388 virtual bool relativeMoveRaw(
const double *deltas)
override;
389 virtual bool checkMotionDoneRaw(
bool *flag)
override;
390 virtual bool checkMotionDoneRaw(
int j,
bool *flag)
override;
391 virtual bool setRefSpeedRaw(
int j,
double sp)
override;
392 virtual bool setRefSpeedsRaw(
const double *spds)
override;
393 virtual bool setRefAccelerationRaw(
int j,
double acc)
override;
394 virtual bool setRefAccelerationsRaw(
const double *accs)
override;
395 virtual bool getRefSpeedRaw(
int j,
double *ref)
override;
396 virtual bool getRefSpeedsRaw(
double *spds)
override;
397 virtual bool getRefAccelerationRaw(
int j,
double *
acc)
override;
398 virtual bool getRefAccelerationsRaw(
double *accs)
override;
399 virtual bool stopRaw(
int j)
override;
400 virtual bool stopRaw()
override;
404 virtual bool positionMoveRaw(
const int n_joint,
const int *joints,
const double *refs)
override;
405 virtual bool relativeMoveRaw(
const int n_joint,
const int *joints,
const double *deltas)
override;
406 virtual bool checkMotionDoneRaw(
const int n_joint,
const int *joints,
bool *flags)
override;
407 virtual bool setRefSpeedsRaw(
const int n_joint,
const int *joints,
const double *spds)
override;
408 virtual bool setRefAccelerationsRaw(
const int n_joint,
const int *joints,
const double *accs)
override;
409 virtual bool getRefSpeedsRaw(
const int n_joint,
const int *joints,
double *spds)
override;
410 virtual bool getRefAccelerationsRaw(
const int n_joint,
const int *joints,
double *accs)
override;
411 virtual bool stopRaw(
const int n_joint,
const int *joints)
override;
412 virtual bool getTargetPositionRaw(
const int joint,
double *ref)
override;
413 virtual bool getTargetPositionsRaw(
double *refs)
override;
414 virtual bool getTargetPositionsRaw(
const int n_joint,
const int *joints,
double *refs)
override;
417 virtual bool velocityMoveRaw(
int j,
double sp)
override;
418 virtual bool velocityMoveRaw(
const double *sp)
override;
422 virtual bool setCalibrationParametersRaw(
int axis,
const CalibrationParameters& params)
override;
423 virtual bool calibrateAxisWithParamsRaw(
int axis,
unsigned int type,
double p1,
double p2,
double p3)
override;
424 virtual bool calibrationDoneRaw(
int j)
override;
430 virtual bool getControlModeRaw(
int j,
int *v)
override;
431 virtual bool getControlModesRaw(
int *v)
override;
434 virtual bool getControlModesRaw(
const int n_joint,
const int *joints,
int *modes)
override;
435 virtual bool setControlModeRaw(
const int j,
const int mode)
override;
436 virtual bool setControlModesRaw(
const int n_joint,
const int *joints,
int *modes)
override;
437 virtual bool setControlModesRaw(
int *modes)
override;
440 virtual bool resetEncoderRaw(
int j)
override;
441 virtual bool resetEncodersRaw()
override;
442 virtual bool setEncoderRaw(
int j,
double val)
override;
443 virtual bool setEncodersRaw(
const double *vals)
override;
444 virtual bool getEncoderRaw(
int j,
double *v)
override;
445 virtual bool getEncodersRaw(
double *
encs)
override;
446 virtual bool getEncoderSpeedRaw(
int j,
double *sp)
override;
447 virtual bool getEncoderSpeedsRaw(
double *spds)
override;
448 virtual bool getEncoderAccelerationRaw(
int j,
double *spds)
override;
449 virtual bool getEncoderAccelerationsRaw(
double *accs)
override;
452 virtual bool getEncodersTimedRaw(
double *
encs,
double *stamps)
override;
453 virtual bool getEncoderTimedRaw(
int j,
double *
encs,
double *stamp)
override;
456 virtual bool getNumberOfMotorEncodersRaw(
int * num)
override;
457 virtual bool resetMotorEncoderRaw(
int m)
override;
458 virtual bool resetMotorEncodersRaw()
override;
459 virtual bool setMotorEncoderRaw(
int m,
const double val)
override;
460 virtual bool setMotorEncodersRaw(
const double *vals)
override;
461 virtual bool getMotorEncoderRaw(
int m,
double *v)
override;
462 virtual bool getMotorEncodersRaw(
double *
encs)
override;
463 virtual bool getMotorEncoderSpeedRaw(
int m,
double *sp)
override;
464 virtual bool getMotorEncoderSpeedsRaw(
double *spds)
override;
465 virtual bool getMotorEncoderAccelerationRaw(
int m,
double *spds)
override;
466 virtual bool getMotorEncoderAccelerationsRaw(
double *accs)
override;
467 virtual bool getMotorEncodersTimedRaw(
double *
encs,
double *stamps)
override;
468 virtual bool getMotorEncoderTimedRaw(
int m,
double *
encs,
double *stamp)
override;
469 virtual bool getMotorEncoderCountsPerRevolutionRaw(
int m,
double *v)
override;
470 virtual bool setMotorEncoderCountsPerRevolutionRaw(
int m,
const double cpr)
override;
474 virtual bool getRemoteVariableRaw(std::string key, yarp::os::Bottle& val)
override;
475 virtual bool setRemoteVariableRaw(std::string key,
const yarp::os::Bottle& val)
override;
476 virtual bool getRemoteVariablesListRaw(yarp::os::Bottle* listOfKeys)
override;
480 virtual bool getAxisNameRaw(
int axis, std::string& name)
override;
481 virtual bool getJointTypeRaw(
int axis, yarp::dev::JointTypeEnum& type)
override;
485 bool getRotorEncoderResolutionRaw(
int m,
double &rotres) ;
486 bool getJointEncoderResolutionRaw(
int m,
double &jntres) ;
487 bool getJointEncoderTypeRaw(
int j,
int &type) ;
488 bool getRotorEncoderTypeRaw(
int j,
int &type) ;
489 bool getKinematicMJRaw(
int j,
double &rotres) ;
490 bool getHasTempSensorsRaw(
int j,
int& ret) ;
491 bool getHasHallSensorRaw(
int j,
int& ret) ;
492 bool getHasRotorEncoderRaw(
int j,
int& ret) ;
493 bool getHasRotorEncoderIndexRaw(
int j,
int& ret) ;
494 bool getMotorPolesRaw(
int j,
int& poles) ;
495 bool getRotorIndexOffsetRaw(
int j,
double& rotorOffset) ;
496 bool getTorqueControlFilterType(
int j,
int& type) ;
497 bool getRotorLimitsRaw(
int j,
double *rotorMin,
double *rotorMax) ;
498 bool getWholeImpedanceRaw(
int j, eOmc_impedance_t &imped);
501 virtual bool enableAmpRaw(
int j)
override;
502 virtual bool disableAmpRaw(
int j)
override;
503 virtual bool getCurrentsRaw(
double *vals)
override;
504 virtual bool getCurrentRaw(
int j,
double *val)
override;
505 virtual bool setMaxCurrentRaw(
int j,
double val)
override;
506 virtual bool getMaxCurrentRaw(
int j,
double *val)
override;
507 virtual bool getAmpStatusRaw(
int *st)
override;
508 virtual bool getAmpStatusRaw(
int j,
int *st)
override;
509 virtual bool getPWMRaw(
int j,
double* val)
override;
510 virtual bool getPWMLimitRaw(
int j,
double* val)
override;
511 virtual bool setPWMLimitRaw(
int j,
const double val)
override;
512 virtual bool getPowerSupplyVoltageRaw(
int j,
double* val)
override;
516 virtual yarp::dev::VAS_status getVirtualAnalogSensorStatus(
int ch)
override;
517 virtual int getVirtualAnalogSensorChannels()
override;
518 virtual bool updateVirtualAnalogSensorMeasure(yarp::sig::Vector &fTorques)
override;
519 virtual bool updateVirtualAnalogSensorMeasure(
int j,
double &fTorque)
override;
521 #ifdef IMPLEMENT_DEBUG_INTERFACE
525 virtual bool setParameterRaw(
int j,
unsigned int type,
double value)
override;
526 virtual bool getParameterRaw(
int j,
unsigned int type,
double *value)
override;
527 virtual bool setDebugParameterRaw(
int j,
unsigned int index,
double value)
override;
528 virtual bool setDebugReferencePositionRaw(
int j,
double value)
override;
529 virtual bool getDebugParameterRaw(
int j,
unsigned int index,
double *value)
override;
530 virtual bool getDebugReferencePositionRaw(
int j,
double *value)
override;
534 virtual bool setLimitsRaw(
int axis,
double min,
double max)
override;
535 virtual bool getLimitsRaw(
int axis,
double *
min,
double *
max)
override;
537 virtual bool setVelLimitsRaw(
int axis,
double min,
double max)
override;
538 virtual bool getVelLimitsRaw(
int axis,
double *
min,
double *
max)
override;
541 virtual bool getTorqueRaw(
int j,
double *t)
override;
542 virtual bool getTorquesRaw(
double *t)
override;
543 virtual bool getTorqueRangeRaw(
int j,
double *
min,
double *
max)
override;
544 virtual bool getTorqueRangesRaw(
double *
min,
double *
max)
override;
545 virtual bool setRefTorquesRaw(
const double *t)
override;
546 virtual bool setRefTorqueRaw(
int j,
double t)
override;
547 virtual bool setRefTorquesRaw(
const int n_joint,
const int *joints,
const double *t)
override;
548 virtual bool getRefTorquesRaw(
double *t)
override;
549 virtual bool getRefTorqueRaw(
int j,
double *t)
override;
550 virtual bool getMotorTorqueParamsRaw(
int j, MotorTorqueParameters *params)
override;
551 virtual bool setMotorTorqueParamsRaw(
int j,
const MotorTorqueParameters params)
override;
555 virtual bool velocityMoveRaw(
const int n_joint,
const int *joints,
const double *spds)
override;
556 virtual bool getRefVelocityRaw(
const int joint,
double *ref)
override;
557 virtual bool getRefVelocitiesRaw(
double *refs)
override;
558 virtual bool getRefVelocitiesRaw(
const int n_joint,
const int *joints,
double *refs)
override;
562 virtual bool getImpedanceRaw(
int j,
double *stiffness,
double *damping)
override;
563 virtual bool setImpedanceRaw(
int j,
double stiffness,
double damping)
override;
564 virtual bool setImpedanceOffsetRaw(
int j,
double offset)
override;
565 virtual bool getImpedanceOffsetRaw(
int j,
double *
offset)
override;
566 virtual bool getCurrentImpedanceLimitRaw(
int j,
double *min_stiff,
double *max_stiff,
double *min_damp,
double *max_damp)
override;
569 virtual bool setPositionRaw(
int j,
double ref)
override;
570 virtual bool setPositionsRaw(
const int n_joint,
const int *joints,
const double *refs)
override;
571 virtual bool setPositionsRaw(
const double *refs)
override;
572 virtual bool getRefPositionRaw(
const int joint,
double *ref)
override;
573 virtual bool getRefPositionsRaw(
double *refs)
override;
574 virtual bool getRefPositionsRaw(
const int n_joint,
const int *joints,
double *refs)
override;
577 virtual bool getInteractionModeRaw(
int j, yarp::dev::InteractionModeEnum* _mode)
override;
578 virtual bool getInteractionModesRaw(
int n_joints,
int *joints, yarp::dev::InteractionModeEnum* modes)
override;
579 virtual bool getInteractionModesRaw(yarp::dev::InteractionModeEnum* modes)
override;
580 virtual bool setInteractionModeRaw(
int j, yarp::dev::InteractionModeEnum _mode)
override;
581 virtual bool setInteractionModesRaw(
int n_joints,
int *joints, yarp::dev::InteractionModeEnum* modes)
override;
582 virtual bool setInteractionModesRaw(yarp::dev::InteractionModeEnum* modes)
override;
585 virtual bool getNumberOfMotorsRaw(
int * num)
override;
586 virtual bool getTemperatureRaw(
int m,
double* val)
override;
587 virtual bool getTemperaturesRaw(
double *vals)
override;
588 virtual bool getTemperatureLimitRaw(
int m,
double *temp)
override;
589 virtual bool setTemperatureLimitRaw(
int m,
const double temp)
override;
590 virtual bool getPeakCurrentRaw(
int m,
double *val)
override;
591 virtual bool setPeakCurrentRaw(
int m,
const double val)
override;
592 virtual bool getNominalCurrentRaw(
int m,
double *val)
override;
593 virtual bool setNominalCurrentRaw(
int m,
const double val)
override;
594 virtual bool getGearboxRatioRaw(
int m,
double *gearbox)
override;
597 virtual bool setRefDutyCycleRaw(
int j,
double v)
override;
598 virtual bool setRefDutyCyclesRaw(
const double *v)
override;
599 virtual bool getRefDutyCycleRaw(
int j,
double *v)
override;
600 virtual bool getRefDutyCyclesRaw(
double *v)
override;
601 virtual bool getDutyCycleRaw(
int j,
double *v)
override;
602 virtual bool getDutyCyclesRaw(
double *v)
override;
608 virtual bool getCurrentRangeRaw(
int j,
double *
min,
double *
max)
override;
609 virtual bool getCurrentRangesRaw(
double *
min,
double *
max)
override;
610 virtual bool setRefCurrentsRaw(
const double *t)
override;
611 virtual bool setRefCurrentRaw(
int j,
double t)
override;
612 virtual bool setRefCurrentsRaw(
const int n_joint,
const int *joints,
const double *t)
override;
613 virtual bool getRefCurrentsRaw(
double *t)
override;
614 virtual bool getRefCurrentRaw(
int j,
double *t)
override;
620 virtual bool getLastJointFaultRaw(
int j,
int& fault, std::string& message)
override;
embObjMotionControl : driver for iCub motor control boards EMS on a ETH bus.
Copyright (C) 2008 RobotCub Consortium.
std::vector< BufferedPort< Bottle > * > ports
eOmn_serv_diagn_cfg_t config
bool useRawEncoderData
its value depends on environment variable "ETH_VERBOSEWHENOK"
bool pwmIsLimited
if true than do not use calibration data
int resolution
Num of error bits passable for joint encoder.
double tolerance
joint encoder type
vector< eOmc_jointset_configuration_t > jointset_cfgs
vector< vector< int > > set2joint