mc_rtc  2.13.0
RobotModule.h
Go to the documentation of this file.
1 /*
2  * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
3  */
4 
5 #pragma once
6 
7 #include <mc_rbdyn/BodySensor.h>
8 #include <mc_rbdyn/Collision.h>
10 #include <mc_rbdyn/Flexibility.h>
11 #include <mc_rbdyn/ForceSensor.h>
12 #include <mc_rbdyn/JointSensor.h>
13 #include <mc_rbdyn/Mimic.h>
15 #include <mc_rbdyn/Springs.h>
16 #include <mc_rbdyn/api.h>
18 
19 #include <mc_rtc/constants.h>
20 #include <mc_rtc/deprecated.h>
21 
22 #include <RBDyn/parsers/common.h>
23 
24 #include <sch/S_Object/S_Object.h>
25 
26 #include <array>
27 #include <map>
28 #include <vector>
29 
30 /* This is an interface designed to provide additionnal information about a robot */
31 
32 namespace mc_rbdyn
33 {
34 
35 using S_ObjectPtr = std::shared_ptr<sch::S_Object>;
36 
41 struct DevicePtrVector : public std::vector<DevicePtr>
42 {
43  inline DevicePtrVector() = default;
44 
47 
48  inline DevicePtrVector(DevicePtrVector && v) = default;
49  inline DevicePtrVector & operator=(DevicePtrVector && v) = default;
50 };
51 
56 struct VisualMap : public std::map<std::string, std::vector<rbd::parsers::Visual>>
57 {
58  inline VisualMap() = default;
59 
60  inline VisualMap(const VisualMap & v) = default;
61  inline VisualMap & operator=(const VisualMap & v) = default;
62 
63  inline VisualMap(VisualMap && v) = default;
64  inline VisualMap & operator=(VisualMap && v) = default;
65 
66  using std::map<std::string, std::vector<rbd::parsers::Visual>>::operator=;
67 };
68 
70 {
71  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
72 
82  using bounds_t = std::vector<std::map<std::string, std::vector<double>>>;
83 
91  using accelerationBounds_t = std::vector<std::map<std::string, std::vector<double>>>;
92 
100  using jerkBounds_t = std::vector<std::map<std::string, std::vector<double>>>;
101 
109  using torqueDerivativeBounds_t = std::vector<std::map<std::string, std::vector<double>>>;
110 
113  {
116  {
118  static constexpr double DEFAULT_PERCENT_VMAX = 0.25;
120  static constexpr double DEFAULT_ACTUAL_COMMAND_DIFF_TRIGGER = mc_rtc::constants::toRad(8.);
122  static constexpr double DEFAULT_RELEASE_OFFSET = mc_rtc::constants::toRad(2);
124  static constexpr unsigned int DEFAULT_OVER_COMMAND_LIMIT_ITER_N = 5;
125 
126  // FIXME These constructors are only needed to facilitate initialization and not required in C++14
127  inline Safety() : Safety(DEFAULT_PERCENT_VMAX) {}
128 
129  inline Safety(double percentVMax,
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)
135  {
136  }
137 
139  void load(const mc_rtc::Configuration & config);
142 
144  double percentVMax;
150  unsigned int overCommandLimitIterN;
151  };
152 
154  Gripper(const std::string & name, const std::vector<std::string> & joints, bool reverse_limits);
155 
157  Gripper(const std::string & name,
158  const std::vector<std::string> & joints,
159  bool reverse_limits,
160  const Safety & safety);
161 
163  Gripper(const std::string & name,
164  const std::vector<std::string> & joints,
165  bool reverse_limits,
166  const Safety & safety,
167  const std::vector<Mimic> & mimics);
168 
170  std::string name;
172  std::vector<std::string> joints;
175 
176  // FIXME In C++17 std::optional is a better semantic
178  inline const Safety * safety() const { return hasSafety_ ? &safety_ : nullptr; }
179 
181  inline const std::vector<Mimic> * mimics() const { return hasMimics_ ? &mimics_ : nullptr; }
182 
183  private:
185  bool hasSafety_ = false;
187  Safety safety_;
189  bool hasMimics_ = false;
191  std::vector<Mimic> mimics_;
193  Gripper(const std::string & name,
194  const std::vector<std::string> & joints,
195  bool reverse_limits,
196  const Safety * safety,
197  const std::vector<Mimic> * mimics);
198  };
199 
209  {
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)
213  {
214  }
215 
217  std::string name;
219  std::string parent;
221  sva::PTransformd X_p_f;
223  bool baked = false;
224  };
225 
241  RobotModule(const std::string & path, const std::string & name)
242  : RobotModule(path, name, path + "/urdf/" + name + ".urdf")
243  {
244  }
245 
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)
261  {
262  }
263 
265  RobotModule(const std::string & name, const rbd::parsers::ParserResult & res);
266 
276  void init(const rbd::parsers::ParserResult & res);
277 
289  const std::vector<std::map<std::string, std::vector<double>>> & bounds() const { return _bounds; }
290 
300  const std::vector<std::map<std::string, std::vector<double>>> & accelerationBounds() const
301  {
302  return _accelerationBounds;
303  }
304 
314  const std::vector<std::map<std::string, std::vector<double>>> & jerkBounds() const { return _jerkBounds; }
315 
325  const std::vector<std::map<std::string, std::vector<double>>> & torqueDerivativeBounds() const
326  {
327  return _torqueDerivativeBounds;
328  }
329 
340  const std::map<std::string, std::vector<double>> & stance() const { return _stance; }
341 
355  const std::map<std::string, std::pair<std::string, std::string>> & convexHull() const { return _convexHull; }
356 
370  const std::map<std::string, std::pair<std::string, S_ObjectPtr>> & collisionObjects() const
371  {
372  return _collisionObjects;
373  }
374 
386  const std::map<std::string, std::pair<std::string, std::string>> & stpbvHull() const { return _stpbvHull; }
387 
394  const std::map<std::string, sva::PTransformd> & collisionTransforms() const { return _collisionTransforms; }
395 
400  const std::vector<Flexibility> & flexibility() const { return _flexibility; }
401 
406  const std::vector<ForceSensor> & forceSensors() const { return _forceSensors; }
407 
412  const BodySensorVector & bodySensors() const { return _bodySensors; }
413 
418  const std::vector<JointSensor> & jointSensors() const { return _jointSensors; }
419 
424  const Springs & springs() const { return _springs; }
425 
433  const std::vector<mc_rbdyn::Collision> & minimalSelfCollisions() const { return _minimalSelfCollisions; }
434 
443  const std::vector<mc_rbdyn::Collision> & commonSelfCollisions() const { return _commonSelfCollisions; }
444 
449  const std::vector<Gripper> & grippers() const { return _grippers; }
450 
457  inline const Gripper::Safety & gripperSafety() const { return _gripperSafety; }
458 
464  const std::vector<std::string> & ref_joint_order() const { return _ref_joint_order; }
465 
470  const std::array<double, 7> & default_attitude() const { return _default_attitude; }
471 
474  {
475  return _lipmStabilizerConfig;
476  }
477 
485  void boundsFromURDF(const rbd::parsers::Limits & limits);
486 
495 
503 
508  inline const CompoundJointConstraintDescriptionVector & compoundJoints() const { return _compoundJoints; }
509 
511  inline const std::vector<std::string> & parameters() const { return _parameters; }
512 
517  inline const std::vector<std::string> & canonicalParameters() const { return _canonicalParameters; }
518 
528 
529  /* Post-processing for control to canonical
530  *
531  * The default implementation does nothing
532  *
533  * This function is called automatically by mc_rtc after each iteration of MCGlobalController::run()
534  *
535  * It is called last, after the controller/observer/grippers have run and before the plugins/log/GUI
536  */
537  std::function<void(const mc_rbdyn::Robot & control, mc_rbdyn::Robot & canonical)> controlToCanonicalPostProcess =
538  [](const mc_rbdyn::Robot &, mc_rbdyn::Robot &) {};
539 
547  std::string real_urdf() const { return _real_urdf; }
548 
550  inline const DevicePtrVector & devices() const { return _devices; }
551 
553  inline const std::vector<FrameDescription> & frames() const noexcept { return _frames; }
554 
555 public:
557  std::string path;
559  std::string name;
561  std::string urdf_path;
563  std::string rsdf_dir;
565  std::string calib_dir;
567  rbd::MultiBody mb;
569  rbd::MultiBodyConfig mbc;
571  rbd::MultiBodyGraph mbg;
581  std::map<std::string, std::vector<double>> _stance;
583  std::map<std::string, std::pair<std::string, std::string>> _convexHull;
585  std::map<std::string, std::pair<std::string, S_ObjectPtr>> _collisionObjects;
587  std::map<std::string, std::pair<std::string, std::string>> _stpbvHull;
593  std::map<std::string, sva::PTransformd> _collisionTransforms;
595  std::vector<Flexibility> _flexibility;
597  std::vector<ForceSensor> _forceSensors;
601  std::vector<JointSensor> _jointSensors;
605  std::vector<mc_rbdyn::Collision> _minimalSelfCollisions;
607  std::vector<mc_rbdyn::Collision> _commonSelfCollisions;
609  std::vector<Gripper> _grippers;
613  std::vector<std::string> _ref_joint_order;
615  std::array<double, 7> _default_attitude = {{1., 0., 0., 0., 0., 0., 0.}};
619  std::vector<std::string> _parameters;
621  std::vector<std::string> _canonicalParameters;
625  std::string _real_urdf;
629  std::vector<FrameDescription> _frames;
630 };
631 
632 typedef std::shared_ptr<RobotModule> RobotModulePtr;
633 
639 MC_RBDYN_DLLAPI RobotModule::bounds_t urdf_limits_to_bounds(const rbd::parsers::Limits & limits);
640 
641 using RobotModuleVector = std::vector<RobotModule, Eigen::aligned_allocator<RobotModule>>;
642 
656 
658 {
662 }
663 
664 inline bool operator==(const RobotModule::Gripper & lhs, const RobotModule::Gripper & rhs)
665 {
666  auto compareMimics = [&]()
667  {
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;
673  };
674  auto compareSafety = [&]()
675  {
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;
681  };
682  return lhs.name == rhs.name && lhs.joints == rhs.joints && lhs.reverse_limits == rhs.reverse_limits && compareSafety()
683  && compareMimics();
684 }
685 
686 } // namespace mc_rbdyn
687 
688 #ifdef WIN32
689 # define ROBOT_MODULE_API __declspec(dllexport)
690 #else
691 # if __GNUC__ >= 4
692 # define ROBOT_MODULE_API __attribute__((visibility("default")))
693 # else
694 # define ROBOT_MODULE_API
695 # endif
696 #endif
#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(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: Robot.h:63
Definition: Springs.h:17
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