22 #include <RBDyn/parsers/common.h>
24 #include <sch/S_Object/S_Object.h>
56 struct VisualMap :
public std::map<std::string, std::vector<rbd::parsers::Visual>>
66 using std::map<std::string, std::vector<rbd::parsers::Visual>>::operator=;
71 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
82 using bounds_t = std::vector<std::map<std::string, std::vector<double>>>;
100 using jerkBounds_t = std::vector<std::map<std::string, std::vector<double>>>;
118 static constexpr
double DEFAULT_PERCENT_VMAX = 0.25;
124 static constexpr
unsigned int DEFAULT_OVER_COMMAND_LIMIT_ITER_N = 5;
130 double actualCommandDiffTrigger = DEFAULT_ACTUAL_COMMAND_DIFF_TRIGGER,
131 double releaseSafetyOffset = DEFAULT_RELEASE_OFFSET,
132 unsigned int overCommandLimitIterN = DEFAULT_OVER_COMMAND_LIMIT_ITER_N)
133 : percentVMax(percentVMax), actualCommandDiffTrigger(actualCommandDiffTrigger),
134 releaseSafetyOffset(releaseSafetyOffset), overCommandLimitIterN(overCommandLimitIterN)
154 Gripper(
const std::string & name,
const std::vector<std::string> & joints,
bool reverse_limits);
158 const std::vector<std::string> & joints,
164 const std::vector<std::string> & joints,
167 const std::vector<Mimic> & mimics);
178 inline const Safety *
safety()
const {
return hasSafety_ ? &safety_ :
nullptr; }
181 inline const std::vector<Mimic> *
mimics()
const {
return hasMimics_ ? &mimics_ :
nullptr; }
185 bool hasSafety_ =
false;
189 bool hasMimics_ =
false;
191 std::vector<Mimic> mimics_;
193 Gripper(
const std::string & name,
194 const std::vector<std::string> & joints,
196 const Safety * safety,
197 const std::vector<Mimic> * mimics);
211 FrameDescription(
const std::string & n,
const std::string & p,
const sva::PTransformd & pt,
bool baked =
false)
212 : name(n), parent(p), X_p_f(pt), baked(baked)
242 :
RobotModule(path, name, path +
"/urdf/" + name +
".urdf")
258 RobotModule(
const std::string & path,
const std::string & name,
const std::string & urdf_path)
259 : path(path), name(name), urdf_path(urdf_path), rsdf_dir(path +
"/rsdf/" + name), calib_dir(path +
"/calib/" + name),
260 _real_urdf(urdf_path)
265 RobotModule(
const std::string & name,
const rbd::parsers::ParserResult & res);
276 void init(
const rbd::parsers::ParserResult & res);
289 const std::vector<std::map<std::string, std::vector<double>>> &
bounds()
const {
return _bounds; }
302 return _accelerationBounds;
314 const std::vector<std::map<std::string, std::vector<double>>> &
jerkBounds()
const {
return _jerkBounds; }
327 return _torqueDerivativeBounds;
340 const std::map<std::string, std::vector<double>> &
stance()
const {
return _stance; }
355 const std::map<std::string, std::pair<std::string, std::string>> &
convexHull()
const {
return _convexHull; }
370 const std::map<std::string, std::pair<std::string, S_ObjectPtr>> &
collisionObjects()
const
372 return _collisionObjects;
386 const std::map<std::string, std::pair<std::string, std::string>> &
stpbvHull()
const {
return _stpbvHull; }
394 const std::map<std::string, sva::PTransformd> &
collisionTransforms()
const {
return _collisionTransforms; }
400 const std::vector<Flexibility> &
flexibility()
const {
return _flexibility; }
406 const std::vector<ForceSensor> &
forceSensors()
const {
return _forceSensors; }
418 const std::vector<JointSensor> &
jointSensors()
const {
return _jointSensors; }
449 const std::vector<Gripper> &
grippers()
const {
return _grippers; }
475 return _lipmStabilizerConfig;
511 inline const std::vector<std::string> &
parameters()
const {
return _parameters; }
553 inline const std::vector<FrameDescription> &
frames() const noexcept {
return _frames; }
581 std::map<std::string, std::vector<double>>
_stance;
583 std::map<std::string, std::pair<std::string, std::string>>
_convexHull;
587 std::map<std::string, std::pair<std::string, std::string>>
_stpbvHull;
615 std::array<double, 7> _default_attitude = {{1., 0., 0., 0., 0., 0., 0.}};
666 auto compareMimics = [&]()
668 auto lmimics = lhs.
mimics();
669 auto rmimics = rhs.
mimics();
670 if(lmimics ==
nullptr && rmimics ==
nullptr) {
return true; }
671 if(lmimics ==
nullptr || rmimics ==
nullptr) {
return false; }
672 return *lmimics == *rmimics;
674 auto compareSafety = [&]()
676 auto lsafety = lhs.
safety();
677 auto rsafety = rhs.
safety();
678 if(lsafety ==
nullptr && rsafety ==
nullptr) {
return true; }
679 if(lsafety ==
nullptr || rsafety ==
nullptr) {
return false; }
680 return *lsafety == *rsafety;
689 # define ROBOT_MODULE_API __declspec(dllexport)
692 # define ROBOT_MODULE_API __attribute__((visibility("default")))
694 # define ROBOT_MODULE_API
#define MC_RBDYN_DLLAPI
Definition: api.h:50
Definition: generic_gripper.h:15
std::vector< BodySensor, Eigen::aligned_allocator< BodySensor > > BodySensorVector
Definition: BodySensor.h:145
MC_RBDYN_DLLAPI bool check_module_compatibility(const RobotModule &lhs, const RobotModule &rhs)
std::vector< RobotModule, Eigen::aligned_allocator< RobotModule > > RobotModuleVector
Definition: RobotModule.h:641
std::vector< CompoundJointConstraintDescription, Eigen::aligned_allocator< CompoundJointConstraintDescription > > CompoundJointConstraintDescriptionVector
Definition: CompoundJointConstraintDescription.h:33
std::shared_ptr< sch::S_Object > S_ObjectPtr
Definition: RobotModule.h:35
bool operator==(const mc_rbdyn::BodySensor &lhs, const mc_rbdyn::BodySensor &rhs)
Definition: BodySensor.h:147
MC_RBDYN_DLLAPI RobotModule::bounds_t urdf_limits_to_bounds(const rbd::parsers::Limits &limits)
Converts limits provided by RBDyn parsers to bounds.
std::shared_ptr< RobotModule > RobotModulePtr
Definition: RobotModule.h:632
constexpr double toRad(const double degrees)
Converts degrees to radians.
Definition: constants.h:39
Definition: RobotModule.h:42
DevicePtrVector & operator=(DevicePtrVector &&v)=default
DevicePtrVector()=default
DevicePtrVector(DevicePtrVector &&v)=default
MC_RBDYN_DLLAPI DevicePtrVector(const DevicePtrVector &v)
MC_RBDYN_DLLAPI DevicePtrVector & operator=(const DevicePtrVector &v)
Configuration for mc_rbdyn::RobotConverter.
Definition: RobotConverterConfig.h:16
Definition: RobotModule.h:209
std::string name
Definition: RobotModule.h:217
std::string parent
Definition: RobotModule.h:219
FrameDescription(const std::string &n, const std::string &p, const sva::PTransformd &pt, bool baked=false)
Definition: RobotModule.h:211
sva::PTransformd X_p_f
Definition: RobotModule.h:221
Definition: RobotModule.h:116
double actualCommandDiffTrigger
Definition: RobotModule.h:146
Safety()
Definition: RobotModule.h:127
mc_rtc::Configuration save() const
double percentVMax
Definition: RobotModule.h:144
Safety(double percentVMax, double actualCommandDiffTrigger=DEFAULT_ACTUAL_COMMAND_DIFF_TRIGGER, double releaseSafetyOffset=DEFAULT_RELEASE_OFFSET, unsigned int overCommandLimitIterN=DEFAULT_OVER_COMMAND_LIMIT_ITER_N)
Definition: RobotModule.h:129
unsigned int overCommandLimitIterN
Definition: RobotModule.h:150
void load(const mc_rtc::Configuration &config)
double releaseSafetyOffset
Definition: RobotModule.h:148
Definition: RobotModule.h:113
Gripper(const std::string &name, const std::vector< std::string > &joints, bool reverse_limits)
Gripper(const std::string &name, const std::vector< std::string > &joints, bool reverse_limits, const Safety &safety)
std::string name
Definition: RobotModule.h:170
bool reverse_limits
Definition: RobotModule.h:174
std::vector< std::string > joints
Definition: RobotModule.h:172
Gripper(const std::string &name, const std::vector< std::string > &joints, bool reverse_limits, const Safety &safety, const std::vector< Mimic > &mimics)
const std::vector< Mimic > * mimics() const
Definition: RobotModule.h:181
const Safety * safety() const
Definition: RobotModule.h:178
Definition: RobotModule.h:70
std::vector< mc_rbdyn::Collision > _commonSelfCollisions
Definition: RobotModule.h:607
std::map< std::string, std::pair< std::string, std::string > > _stpbvHull
Definition: RobotModule.h:587
CompoundJointConstraintDescriptionVector _compoundJoints
Definition: RobotModule.h:617
std::vector< ForceSensor > _forceSensors
Definition: RobotModule.h:597
const std::vector< Flexibility > & flexibility() const
Definition: RobotModule.h:400
std::vector< std::string > _parameters
Definition: RobotModule.h:619
std::vector< std::map< std::string, std::vector< double > >> accelerationBounds_t
Definition: RobotModule.h:91
const std::vector< Gripper > & grippers() const
Definition: RobotModule.h:449
RobotConverterConfig controlToCanonicalConfig
Definition: RobotModule.h:527
const std::vector< ForceSensor > & forceSensors() const
Definition: RobotModule.h:406
void make_default_ref_joint_order()
RobotModule(const std::string &name, const rbd::parsers::ParserResult &res)
std::vector< std::map< std::string, std::vector< double > >> torqueDerivativeBounds_t
Definition: RobotModule.h:109
const Springs & springs() const
Definition: RobotModule.h:424
const CompoundJointConstraintDescriptionVector & compoundJoints() const
Definition: RobotModule.h:508
const std::array< double, 7 > & default_attitude() const
Definition: RobotModule.h:470
VisualMap _visual
Definition: RobotModule.h:589
std::string calib_dir
Definition: RobotModule.h:565
const std::vector< std::map< std::string, std::vector< double > > > & torqueDerivativeBounds() const
Definition: RobotModule.h:325
void init(const rbd::parsers::ParserResult &res)
accelerationBounds_t _accelerationBounds
Definition: RobotModule.h:575
const DevicePtrVector & devices() const
Definition: RobotModule.h:550
const BodySensorVector & bodySensors() const
Definition: RobotModule.h:412
const std::vector< mc_rbdyn::Collision > & minimalSelfCollisions() const
Definition: RobotModule.h:433
std::map< std::string, sva::PTransformd > _collisionTransforms
Definition: RobotModule.h:593
std::string _real_urdf
Definition: RobotModule.h:625
Gripper::Safety _gripperSafety
Definition: RobotModule.h:611
std::vector< std::map< std::string, std::vector< double > >> bounds_t
Definition: RobotModule.h:82
std::string path
Definition: RobotModule.h:557
const std::map< std::string, std::pair< std::string, S_ObjectPtr > > & collisionObjects() const
Definition: RobotModule.h:370
std::map< std::string, std::pair< std::string, std::string > > _convexHull
Definition: RobotModule.h:583
std::vector< Flexibility > _flexibility
Definition: RobotModule.h:595
rbd::MultiBodyGraph mbg
Definition: RobotModule.h:571
std::vector< FrameDescription > _frames
Definition: RobotModule.h:629
const std::map< std::string, std::pair< std::string, std::string > > & stpbvHull() const
Definition: RobotModule.h:386
const std::vector< JointSensor > & jointSensors() const
Definition: RobotModule.h:418
const std::vector< std::map< std::string, std::vector< double > > > & accelerationBounds() const
Definition: RobotModule.h:300
std::string urdf_path
Definition: RobotModule.h:561
void boundsFromURDF(const rbd::parsers::Limits &limits)
const std::vector< FrameDescription > & frames() const noexcept
Definition: RobotModule.h:553
mc_rbdyn::lipm_stabilizer::StabilizerConfiguration _lipmStabilizerConfig
Definition: RobotModule.h:623
std::map< std::string, std::vector< double > > _stance
Definition: RobotModule.h:581
const std::vector< std::string > & parameters() const
Definition: RobotModule.h:511
BodySensorVector _bodySensors
Definition: RobotModule.h:599
std::vector< std::string > _canonicalParameters
Definition: RobotModule.h:621
std::vector< std::map< std::string, std::vector< double > >> jerkBounds_t
Definition: RobotModule.h:100
std::vector< JointSensor > _jointSensors
Definition: RobotModule.h:601
std::string rsdf_dir
Definition: RobotModule.h:563
std::map< std::string, std::pair< std::string, S_ObjectPtr > > _collisionObjects
Definition: RobotModule.h:585
const std::vector< std::string > & canonicalParameters() const
Definition: RobotModule.h:517
std::string real_urdf() const
Definition: RobotModule.h:547
VisualMap _collision
Definition: RobotModule.h:591
rbd::MultiBody mb
Definition: RobotModule.h:567
DevicePtrVector _devices
Definition: RobotModule.h:627
const std::map< std::string, sva::PTransformd > & collisionTransforms() const
Definition: RobotModule.h:394
const std::vector< std::map< std::string, std::vector< double > > > & jerkBounds() const
Definition: RobotModule.h:314
std::vector< mc_rbdyn::Collision > _minimalSelfCollisions
Definition: RobotModule.h:605
std::string name
Definition: RobotModule.h:559
const std::vector< std::string > & ref_joint_order() const
Definition: RobotModule.h:464
const std::map< std::string, std::vector< double > > & stance() const
Definition: RobotModule.h:340
const std::vector< std::map< std::string, std::vector< double > > > & bounds() const
Definition: RobotModule.h:289
std::vector< std::string > _ref_joint_order
Definition: RobotModule.h:613
RobotModule(const std::string &path, const std::string &name)
Definition: RobotModule.h:241
rbd::MultiBodyConfig mbc
Definition: RobotModule.h:569
Springs _springs
Definition: RobotModule.h:603
const Gripper::Safety & gripperSafety() const
Definition: RobotModule.h:457
jerkBounds_t _jerkBounds
Definition: RobotModule.h:577
const std::map< std::string, std::pair< std::string, std::string > > & convexHull() const
Definition: RobotModule.h:355
RobotModule(const std::string &path, const std::string &name, const std::string &urdf_path)
Definition: RobotModule.h:258
const mc_rbdyn::lipm_stabilizer::StabilizerConfiguration & defaultLIPMStabilizerConfiguration() const
Definition: RobotModule.h:473
bounds_t _bounds
Definition: RobotModule.h:573
const std::vector< mc_rbdyn::Collision > & commonSelfCollisions() const
Definition: RobotModule.h:443
torqueDerivativeBounds_t _torqueDerivativeBounds
Definition: RobotModule.h:579
std::vector< Gripper > _grippers
Definition: RobotModule.h:609
Definition: RobotModule.h:57
VisualMap & operator=(VisualMap &&v)=default
VisualMap & operator=(const VisualMap &v)=default
VisualMap(VisualMap &&v)=default
VisualMap(const VisualMap &v)=default
Configuration of the LIPMStabilizer. This configuration is meant to be overriden from the RobotModule...
Definition: StabilizerConfiguration.h:435
Simplify access to values hold within a JSON file.
Definition: Configuration.h:166