RobotModule.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_rbdyn/BodySensor.h>
#include <mc_rbdyn/Collision.h>
#include <mc_rbdyn/CompoundJointConstraintDescription.h>
#include <mc_rbdyn/Flexibility.h>
#include <mc_rbdyn/ForceSensor.h>
#include <mc_rbdyn/JointSensor.h>
#include <mc_rbdyn/Mimic.h>
#include <mc_rbdyn/RobotConverterConfig.h>
#include <mc_rbdyn/Springs.h>
#include <mc_rbdyn/api.h>
#include <mc_rbdyn/lipm_stabilizer/StabilizerConfiguration.h>
 
#include <mc_rtc/constants.h>
#include <mc_rtc/deprecated.h>
 
#include <RBDyn/parsers/common.h>
 
#include <sch/S_Object/S_Object.h>
 
/* This is an interface designed to provide additionnal information about a robot */
 
namespace mc_rbdyn
{
 
using S_ObjectPtr = std::shared_ptr<sch::S_Object>;
 
struct DevicePtrVector : public std::vector<DevicePtr>
{
  inline DevicePtrVector() = default;
 
  MC_RBDYN_DLLAPI DevicePtrVector(const DevicePtrVector & v);
  MC_RBDYN_DLLAPI DevicePtrVector & operator=(const DevicePtrVector & v);
 
  inline DevicePtrVector(DevicePtrVector && v) = default;
  inline DevicePtrVector & operator=(DevicePtrVector && v) = default;
};
 
struct VisualMap : public std::map<std::string, std::vector<rbd::parsers::Visual>>
{
  inline VisualMap() = default;
 
  inline VisualMap(const VisualMap & v) = default;
  inline VisualMap & operator=(const VisualMap & v) = default;
 
  inline VisualMap(VisualMap && v) = default;
  inline VisualMap & operator=(VisualMap && v) = default;
 
  using std::map<std::string, std::vector<rbd::parsers::Visual>>::operator=;
};
 
struct MC_RBDYN_DLLAPI RobotModule
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  using bounds_t = std::vector<std::map<std::string, std::vector<double>>>;
 
  using accelerationBounds_t = std::vector<std::map<std::string, std::vector<double>>>;
 
  using jerkBounds_t = std::vector<std::map<std::string, std::vector<double>>>;
 
  using torqueDerivativeBounds_t = std::vector<std::map<std::string, std::vector<double>>>;
 
  struct MC_RBDYN_DLLAPI Gripper
  {
    struct MC_RBDYN_DLLAPI Safety
    {
      static constexpr double DEFAULT_PERCENT_VMAX = 0.25;
      static constexpr double DEFAULT_ACTUAL_COMMAND_DIFF_TRIGGER = mc_rtc::constants::toRad(8.);
      static constexpr double DEFAULT_RELEASE_OFFSET = mc_rtc::constants::toRad(2);
      static constexpr unsigned int DEFAULT_OVER_COMMAND_LIMIT_ITER_N = 5;
 
      // FIXME These constructors are only needed to facilitate initialization and not required in C++14
      inline Safety() : Safety(DEFAULT_PERCENT_VMAX) {}
 
      inline Safety(double percentVMax,
                    double actualCommandDiffTrigger = DEFAULT_ACTUAL_COMMAND_DIFF_TRIGGER,
                    double releaseSafetyOffset = DEFAULT_RELEASE_OFFSET,
                    unsigned int overCommandLimitIterN = DEFAULT_OVER_COMMAND_LIMIT_ITER_N)
      : percentVMax(percentVMax), actualCommandDiffTrigger(actualCommandDiffTrigger),
        releaseSafetyOffset(releaseSafetyOffset), overCommandLimitIterN(overCommandLimitIterN)
      {
      }
 
      void load(const mc_rtc::Configuration & config);
      mc_rtc::Configuration save() const;
 
      double percentVMax;
      double actualCommandDiffTrigger;
      double releaseSafetyOffset;
      unsigned int overCommandLimitIterN;
    };
 
    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);
 
    Gripper(const std::string & name,
            const std::vector<std::string> & joints,
            bool reverse_limits,
            const Safety & safety,
            const std::vector<Mimic> & mimics);
 
    std::string name;
    std::vector<std::string> joints;
    bool reverse_limits;
 
    // FIXME In C++17 std::optional is a better semantic
    inline const Safety * safety() const { return hasSafety_ ? &safety_ : nullptr; }
 
    inline const std::vector<Mimic> * mimics() const { return hasMimics_ ? &mimics_ : nullptr; }
 
  private:
    bool hasSafety_ = false;
    Safety safety_;
    bool hasMimics_ = false;
    std::vector<Mimic> mimics_;
    Gripper(const std::string & name,
            const std::vector<std::string> & joints,
            bool reverse_limits,
            const Safety * safety,
            const std::vector<Mimic> * mimics);
  };
 
  struct MC_RBDYN_DLLAPI FrameDescription
  {
    FrameDescription(const std::string & n, const std::string & p, const sva::PTransformd & pt, bool baked = false)
    : name(n), parent(p), X_p_f(pt), baked(baked)
    {
    }
 
    std::string name;
    std::string parent;
    sva::PTransformd X_p_f;
    bool baked = false;
  };
 
  RobotModule(const std::string & path, const std::string & name)
  : RobotModule(path, name, path + "/urdf/" + name + ".urdf")
  {
  }
 
  RobotModule(const std::string & path, const std::string & name, const std::string & urdf_path)
  : path(path), name(name), urdf_path(urdf_path), convex_dir(""), rsdf_dir(path + "/rsdf/" + name),
    calib_dir(path + "/calib/" + name), _real_urdf(urdf_path)
  {
  }
 
  RobotModule(const std::string & name, const rbd::parsers::ParserResult & res);
 
  struct ConnectionParameters
  {
 
#define CONNECTION_PROPERTY(NAME, TYPE, DEFAULT)              \
  TYPE NAME##_ = DEFAULT;                                     \
  inline ConnectionParameters & NAME(const TYPE & b) noexcept \
  {                                                           \
    NAME##_ = b;                                              \
    return *this;                                             \
  }                                                           \
  inline auto & NAME() noexcept { return NAME##_; }
 
    CONNECTION_PROPERTY(path, std::string, "")
    CONNECTION_PROPERTY(name, std::string, "")
    CONNECTION_PROPERTY(urdf_path, std::string, "")
    CONNECTION_PROPERTY(rsdf_dir, std::string, "")
    CONNECTION_PROPERTY(calib_dir, std::string, "")
    CONNECTION_PROPERTY(useGripperSafetyFromThis, bool, true)
    CONNECTION_PROPERTY(useLIPMStabilizerConfigFromThis, bool, true)
 
    /* End General parameters section */
 
    CONNECTION_PROPERTY(jointType, rbd::Joint::Type, rbd::Joint::Type::Fixed)
 
    CONNECTION_PROPERTY(jointAxis, Eigen::Vector3d, Eigen::Vector3d::UnitZ())
    CONNECTION_PROPERTY(jointForward, bool, true)
    CONNECTION_PROPERTY(jointName, std::string, "")
    CONNECTION_PROPERTY(jointStance, std::vector<double>, {})
    using JointLimits = std::array<std::vector<double>, 6>;
    CONNECTION_PROPERTY(jointLimits, JointLimits, {})
    using JointAccelerationLimits = std::array<std::vector<double>, 2>;
    CONNECTION_PROPERTY(jointAccelerationLimits, JointAccelerationLimits, {})
    using JointJerkLimits = std::array<std::vector<double>, 2>;
    CONNECTION_PROPERTY(jointJerkLimits, JointJerkLimits, {})
    using JointTorqueDerivativeLimits = std::array<std::vector<double>, 2>;
    CONNECTION_PROPERTY(jointTorqueDerivativeLimits, JointTorqueDerivativeLimits, {})
    CONNECTION_PROPERTY(X_this_connection, sva::PTransformd, sva::PTransformd::Identity())
    CONNECTION_PROPERTY(X_other_connection, sva::PTransformd, sva::PTransformd::Identity())
 
    /* End Connection joint section */
 
    using mapping_t = std::unordered_map<std::string, std::string>;
 
    CONNECTION_PROPERTY(bodyMapping, mapping_t, {})
 
    CONNECTION_PROPERTY(jointMapping, mapping_t, {})
 
    CONNECTION_PROPERTY(convexMapping, mapping_t, {})
 
    CONNECTION_PROPERTY(gripperMapping, mapping_t, {})
 
    CONNECTION_PROPERTY(surfaceMapping, mapping_t, {})
 
    /* Remap frame names */
    CONNECTION_PROPERTY(frameMapping, mapping_t, {})
 
    CONNECTION_PROPERTY(forceSensorMapping, mapping_t, {})
 
    CONNECTION_PROPERTY(bodySensorMapping, mapping_t, {})
 
    CONNECTION_PROPERTY(deviceMapping, mapping_t, {})
 
#undef CONNECTION_PROPERTY
    /* End Name mapping section */
  };
 
  RobotModule connect(const mc_rbdyn::RobotModule & other,
                      const std::string & this_body,
                      const std::string & other_body,
                      const std::string & prefix,
                      const ConnectionParameters & params) const;
 
  RobotModule disconnect(const mc_rbdyn::RobotModule & other,
                         const std::string & this_body,
                         const std::string & other_body,
                         const std::string & prefix,
                         const ConnectionParameters & params) const;
 
  void init(const rbd::parsers::ParserResult & res);
 
  void generate_convexes(bool regenerate = false, unsigned int sampling_point = 4000);
 
  void bind_convexes();
 
  const std::vector<std::map<std::string, std::vector<double>>> & bounds() const { return _bounds; }
 
  const std::vector<std::map<std::string, std::vector<double>>> & accelerationBounds() const
  {
    return _accelerationBounds;
  }
 
  const std::vector<std::map<std::string, std::vector<double>>> & jerkBounds() const { return _jerkBounds; }
 
  const std::vector<std::map<std::string, std::vector<double>>> & torqueDerivativeBounds() const
  {
    return _torqueDerivativeBounds;
  }
 
  const std::map<std::string, std::vector<double>> & stance() const { return _stance; }
 
  const std::map<std::string, std::pair<std::string, std::string>> & convexHull() const { return _convexHull; }
 
  const std::map<std::string, std::pair<std::string, S_ObjectPtr>> & collisionObjects() const
  {
    return _collisionObjects;
  }
 
  const std::map<std::string, std::pair<std::string, std::string>> & stpbvHull() const { return _stpbvHull; }
 
  const std::map<std::string, sva::PTransformd> & collisionTransforms() const { return _collisionTransforms; }
 
  const std::vector<Flexibility> & flexibility() const { return _flexibility; }
 
  const std::vector<ForceSensor> & forceSensors() const { return _forceSensors; }
 
  const BodySensorVector & bodySensors() const { return _bodySensors; }
 
  const std::vector<JointSensor> & jointSensors() const { return _jointSensors; }
 
  const Springs & springs() const { return _springs; }
 
  const std::vector<mc_rbdyn::Collision> & minimalSelfCollisions() const { return _minimalSelfCollisions; }
 
  const std::vector<mc_rbdyn::Collision> & commonSelfCollisions() const { return _commonSelfCollisions; }
 
  const std::vector<Gripper> & grippers() const { return _grippers; }
 
  inline const Gripper::Safety & gripperSafety() const { return _gripperSafety; }
 
  const std::vector<std::string> & ref_joint_order() const { return _ref_joint_order; }
 
  const std::array<double, 7> & default_attitude() const { return _default_attitude; }
 
  const mc_rbdyn::lipm_stabilizer::StabilizerConfiguration & defaultLIPMStabilizerConfiguration() const
  {
    return _lipmStabilizerConfig;
  }
 
  void boundsFromURDF(const rbd::parsers::Limits & limits);
 
  void expand_stance();
 
  void make_default_ref_joint_order();
 
  inline const CompoundJointConstraintDescriptionVector & compoundJoints() const { return _compoundJoints; }
 
  inline const std::vector<std::string> & parameters() const { return _parameters; }
 
  inline const std::vector<std::string> & canonicalParameters() const { return _canonicalParameters; }
 
  RobotConverterConfig controlToCanonicalConfig;
 
  /* Post-processing for control to canonical
   *
   * The default implementation does nothing
   *
   * This function is called automatically by mc_rtc after each iteration of MCGlobalController::run()
   *
   * It is called last, after the controller/observer/grippers have run and before the plugins/log/GUI
   */
  std::function<void(const mc_rbdyn::Robot & control, mc_rbdyn::Robot & canonical)> controlToCanonicalPostProcess =
      [](const mc_rbdyn::Robot &, mc_rbdyn::Robot &) {};
 
  std::string real_urdf() const { return _real_urdf; }
 
  inline const DevicePtrVector & devices() const { return _devices; }
 
  inline const std::vector<FrameDescription> & frames() const noexcept { return _frames; }
 
public:
  std::string path;
  std::string name;
  std::string urdf_path;
  std::string convex_dir;
  std::string rsdf_dir;
  std::string calib_dir;
  rbd::MultiBody mb;
  rbd::MultiBodyConfig mbc;
  rbd::MultiBodyGraph mbg;
  bounds_t _bounds;
  accelerationBounds_t _accelerationBounds;
  jerkBounds_t _jerkBounds;
  torqueDerivativeBounds_t _torqueDerivativeBounds;
  std::map<std::string, std::vector<double>> _stance;
  std::map<std::string, std::pair<std::string, std::string>> _convexHull;
  std::map<std::string, std::pair<std::string, S_ObjectPtr>> _collisionObjects;
  std::map<std::string, std::pair<std::string, std::string>> _stpbvHull;
  VisualMap _visual;
  VisualMap _collision;
  std::map<std::string, sva::PTransformd> _collisionTransforms;
  std::vector<Flexibility> _flexibility;
  std::vector<ForceSensor> _forceSensors;
  BodySensorVector _bodySensors;
  std::vector<JointSensor> _jointSensors;
  Springs _springs;
  std::vector<mc_rbdyn::Collision> _minimalSelfCollisions;
  std::vector<mc_rbdyn::Collision> _commonSelfCollisions;
  std::vector<Gripper> _grippers;
  Gripper::Safety _gripperSafety;
  std::vector<std::string> _ref_joint_order;
  std::array<double, 7> _default_attitude = {{1., 0., 0., 0., 0., 0., 0.}};
  CompoundJointConstraintDescriptionVector _compoundJoints;
  std::vector<std::string> _parameters;
  std::vector<std::string> _canonicalParameters;
  mc_rbdyn::lipm_stabilizer::StabilizerConfiguration _lipmStabilizerConfig;
  std::string _real_urdf;
  DevicePtrVector _devices;
  std::vector<FrameDescription> _frames;
};
 
typedef std::shared_ptr<RobotModule> RobotModulePtr;
 
MC_RBDYN_DLLAPI RobotModule::bounds_t urdf_limits_to_bounds(const rbd::parsers::Limits & limits);
 
using RobotModuleVector = std::vector<RobotModule, Eigen::aligned_allocator<RobotModule>>;
 
MC_RBDYN_DLLAPI bool check_module_compatibility(const RobotModule & lhs, const RobotModule & rhs);
 
inline bool operator==(const RobotModule::Gripper::Safety & lhs, const RobotModule::Gripper::Safety & rhs)
{
  return lhs.percentVMax == rhs.percentVMax && lhs.actualCommandDiffTrigger == rhs.actualCommandDiffTrigger
         && lhs.releaseSafetyOffset == rhs.releaseSafetyOffset
         && lhs.overCommandLimitIterN == rhs.overCommandLimitIterN;
}
 
inline bool operator==(const RobotModule::Gripper & lhs, const RobotModule::Gripper & rhs)
{
  auto compareMimics = [&]()
  {
    auto lmimics = lhs.mimics();
    auto rmimics = rhs.mimics();
    if(lmimics == nullptr && rmimics == nullptr) { return true; }
    if(lmimics == nullptr || rmimics == nullptr) { return false; }
    return *lmimics == *rmimics;
  };
  auto compareSafety = [&]()
  {
    auto lsafety = lhs.safety();
    auto rsafety = rhs.safety();
    if(lsafety == nullptr && rsafety == nullptr) { return true; }
    if(lsafety == nullptr || rsafety == nullptr) { return false; }
    return *lsafety == *rsafety;
  };
  return lhs.name == rhs.name && lhs.joints == rhs.joints && lhs.reverse_limits == rhs.reverse_limits && compareSafety()
         && compareMimics();
}
 
} // namespace mc_rbdyn
 
#ifdef WIN32
#  define ROBOT_MODULE_API __declspec(dllexport)
#else
#  if __GNUC__ >= 4
#    define ROBOT_MODULE_API __attribute__((visibility("default")))
#  else
#    define ROBOT_MODULE_API
#  endif
#endif