31 #include <yarp/os/Bottle.h>
33 #include <yarp/os/Bottle.h>
34 #include <yarp/dev/ControlBoardPid.h>
35 #include <yarp/dev/ControlBoardHelper.h>
37 #include <yarp/dev/PidEnums.h>
38 #include <yarp/dev/ControlBoardInterfacesImpl.h>
40 #include "EoMotionControl.h"
41 #include <yarp/os/LogStream.h>
73 void setUnits(yarp::dev::PidFeedbackUnitsEnum fu, yarp::dev::PidOutputUnitsEnum ou)
79 virtual yarp::dev::Pid
getPID(
int joint) = 0;
92 pid =
new yarp::dev::Pid[nj];
100 yarp::dev::Pid
getPID(
int joint)
override
179 fbk_PidUnits = yarp::dev::PidFeedbackUnitsEnum::RAW_MACHINE_UNITS;
180 out_PidUnits = yarp::dev::PidOutputUnitsEnum::RAW_MACHINE_UNITS;
224 eOmc_jointset_configuration_t
cfg;
230 cfg.candotorquecontrol=0;
231 cfg.usespeedfeedbackfrommotors=0;
232 cfg.pidoutputtype=eomc_pidoutputtype_unknown;
234 cfg.constraints.type=eomc_jsetconstraint_unknown;
235 cfg.constraints.param1=0;
236 cfg.constraints.param2=0;
313 std::array<float32_t, 3>
x0;
314 std::array<float32_t, 3>
Q;
326 std::string _boardname;
329 std::map<std::string, Pid_Algorithm*> minjerkAlgoMap;
331 std::map<std::string, Pid_Algorithm*> torqueAlgoMap;
336 std::vector<std::string> _positionControlLaw;
337 std::vector<std::string> _velocityControlLaw;
338 std::vector<std::string> _mixedControlLaw;
341 std::vector<std::string> _torqueControlLaw;
342 std::vector<std::string> _currentControlLaw;
343 std::vector<std::string> _speedControlLaw;
358 bool parseControlsGroup(yarp::os::Searchable &config);
360 bool parseSelectedPositionControl(yarp::os::Searchable &config);
361 bool parseSelectedVelocityControl(yarp::os::Searchable &config);
362 bool parseSelectedMixedControl(yarp::os::Searchable &config);
365 bool parseSelectedTorqueControl(yarp::os::Searchable &config);
367 bool parseSelectedCurrentPid(yarp::os::Searchable &config,
bool pidisMandatory,
PidInfo *pids);
368 bool parseSelectedSpeedPid(yarp::os::Searchable &config,
bool pidisMandatory,
PidInfo *pids);
370 bool parsePid_minJerk_outPwm(yarp::os::Bottle &b_pid, std::string controlLaw);
371 bool parsePid_minJerk_outCur(yarp::os::Bottle &b_pid, std::string controlLaw);
372 bool parsePid_minJerk_outVel(yarp::os::Bottle &b_pid, std::string controlLaw);
378 bool parsePid_torque_outPwm(yarp::os::Bottle &b_pid, std::string controlLaw);
379 bool parsePid_torque_outCur(yarp::os::Bottle &b_pid, std::string controlLaw);
380 bool parsePid_torque_outVel(yarp::os::Bottle &b_pid, std::string controlLaw);
382 bool parsePidsGroup2FOC(yarp::os::Bottle& pidsGroup, Pid myPid[]);
383 bool parsePidsGroupSimple(yarp::os::Bottle& pidsGroup, Pid myPid[]);
384 bool parsePidsGroupExtended(yarp::os::Bottle& pidsGroup, Pid myPid[]);
385 bool parsePidsGroupDeluxe(yarp::os::Bottle& pidsGroup, Pid myPid[]);
387 bool parsePidsGroup(yarp::os::Bottle& pidsGroup, yarp::dev::Pid myPid[], std::string prefix);
389 bool parsePidUnitsType(yarp::os::Bottle &bPid, yarp::dev::PidFeedbackUnitsEnum &fbk_pidunits, yarp::dev::PidOutputUnitsEnum& out_pidunits);
391 bool checkJointTypes(
PidInfo *pids,
const std::string &pid_type);
392 bool checkSinglePid(
PidInfo &firstPid,
PidInfo ¤tPid,
const int &firstjoint,
const int ¤tjoint,
const std::string &pid_type);
394 bool convert(std::string
const &fromstring, eOmc_jsetconstraint_t &jsetconstraint,
bool& formaterror);
395 bool convert(yarp::os::Bottle &bottle, std::vector<double> &matrix,
bool &formaterror,
int targetsize);
398 bool extractGroup(yarp::os::Bottle &input, yarp::os::Bottle &
out,
const std::string &key1,
const std::string &txt,
int size,
bool mandatory=
true);
400 bool checkAndSetVectorSize(std::vector<T> &vec,
int size,
const std::string &funcName)
402 if(size > (
int)vec.capacity())
404 yError() <<
"embObjMC BOARD " << _boardname <<
" in " << funcName.c_str() <<
": try to insert " << size <<
"element in vector with " << vec.capacity() <<
" elements";
413 void debugUtil_printControlLaws(
void);
417 Parser(
int numofjoints, std::string boardname);
423 bool parseJointsetCfgGroup(yarp::os::Searchable &config, std::vector<JointsSet> &jsets, std::vector<int> &jointtoset);
424 bool parseTimeoutsGroup(yarp::os::Searchable &config, std::vector<timeouts_t> &timeouts,
int defaultVelocityTimeout);
425 bool parseCurrentLimits(yarp::os::Searchable &config, std::vector<motorCurrentLimits_t> &currLimits);
426 bool parseJointsLimits(yarp::os::Searchable &config, std::vector<jointLimits_t> &jointsLimits);
427 bool parseRotorsLimits(yarp::os::Searchable &config, std::vector<rotorLimits_t> &rotorsLimits);
430 bool parseBehaviourFalgs(yarp::os::Searchable &config,
bool &useRawEncoderData,
bool &pwmIsLimited );
432 bool parseAxisInfo(yarp::os::Searchable &config,
int axisMap[], std::vector<axisInfo_t> &axisInfo);
436 bool parseGearboxValues(yarp::os::Searchable &config,
double gearbox_M2J[],
double gearbox_E2J[]);
438 bool parseImpedanceGroup(yarp::os::Searchable &config,std::vector<impedanceParameters_t> &impedance);
439 bool parseDeadzoneValue(yarp::os::Searchable &config,
double deadzone[],
bool *found);
440 bool parseKalmanFilterParams(yarp::os::Searchable &config, std::vector<kalmanFilterParams_t> &kalmanFilterParams);
void setPidOutputType(eOmc_pidoutputtype_t type)
eOmc_jointset_configuration_t cfg
void setUseSpeedFeedbackFromMotors(bool flag)
int getNumberofJoints(void)
std::vector< int > joints
eOmc_jointset_configuration_t * getConfiguration(void)
void setCanDoTorqueControl(bool flag)
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[])
Parser(int numofjoints, std::string boardname)
bool parseMechanicalsFlags(yarp::os::Searchable &config, int useMotorSpeedFbk[])
bool parseMotioncontrolVersion(yarp::os::Searchable &config, int &version)
bool parseAxisInfo(yarp::os::Searchable &config, int axisMap[], std::vector< axisInfo_t > &axisInfo)
bool parseCouplingInfo(yarp::os::Searchable &config, couplingInfo_t &couplingInfo)
bool parsePids(yarp::os::Searchable &config, PidInfo *ppids, TrqPidInfo *tpids, PidInfo *cpids, PidInfo *spids, bool lowLevPidisMandatory)
bool parseFocGroup(yarp::os::Searchable &config, focBasedSpecificInfo_t *foc_based_info, std::string groupName)
bool parseDeadzoneValue(yarp::os::Searchable &config, double deadzone[], bool *found)
bool isVerboseEnabled(yarp::os::Searchable &config)
bool parseTimeoutsGroup(yarp::os::Searchable &config, std::vector< timeouts_t > &timeouts, int defaultVelocityTimeout)
bool parseCurrentLimits(yarp::os::Searchable &config, std::vector< motorCurrentLimits_t > &currLimits)
bool parseEncoderFactor(yarp::os::Searchable &config, double encoderFactor[])
bool parsefullscalePWM(yarp::os::Searchable &config, double dutycycleToPWM[])
bool parseRotorsLimits(yarp::os::Searchable &config, std::vector< rotorLimits_t > &rotorsLimits)
bool parseImpedanceGroup(yarp::os::Searchable &config, std::vector< impedanceParameters_t > &impedance)
bool parseKalmanFilterParams(yarp::os::Searchable &config, std::vector< kalmanFilterParams_t > &kalmanFilterParams)
bool parseGearboxValues(yarp::os::Searchable &config, double gearbox_M2J[], double gearbox_E2J[])
bool parseBehaviourFalgs(yarp::os::Searchable &config, bool &useRawEncoderData, bool &pwmIsLimited)
std::string usernamePidSelected
yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits
eOmc_ctrl_out_type_t out_type
yarp::dev::PidOutputUnitsEnum out_PidUnits
yarp::dev::Pid getPID(int joint) override
Pid_Algorithm_simple(int nj, eOmc_ctrl_out_type_t ot)
yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits
eOmc_ctrl_out_type_t getOutputType()
void setUnits(yarp::dev::PidFeedbackUnitsEnum fu, yarp::dev::PidOutputUnitsEnum ou)
eOmc_ctrl_out_type_t out_type
yarp::dev::PidOutputUnitsEnum out_PidUnits
virtual yarp::dev::Pid getPID(int joint)=0
double viscousPos
motor torque constant
double ktau
back-emf compensation parameter
Copyright (C) 2008 RobotCub Consortium.
std::vector< double > matrixM2J
std::vector< double > matrixE2J
std::vector< double > matrixJ2M
bool hasRotorEncoderIndex
double posHwMax
user joint limits, min
double posHwMin
hardaware joint limits, max
double velMax
hardaware joint limits, min
double posMax
user joint limits, max
std::array< float32_t, 3 > x0
std::array< float32_t, 3 > Q