Robot.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_rbdyn/RobotData.h>
#include <mc_rbdyn/RobotFrame.h>
#include <mc_rbdyn/RobotModule.h>
#include <mc_rbdyn/Surface.h>
 
#include <mc_control/generic_gripper.h>
 
#include <mc_tvm/fwd.h>
 
#include <RBDyn/MultiBody.h>
#include <RBDyn/MultiBodyConfig.h>
#include <RBDyn/MultiBodyGraph.h>
 
#include <memory>
#include <optional>
#include <unordered_map>
 
namespace mc_rbdyn
{
 
struct Robots;
 
struct Robot;
 
struct LoadRobotParameters
{
  friend struct Robot;
 
private:
  std::optional<sva::PTransformd> base_tf_ = std::nullopt;
  std::optional<std::string> base_ = std::nullopt;
  bool warn_on_missing_files_ = false;
  RobotDataPtr data_ = nullptr;
 
public:
#define MAKE_LOAD_ROBOT_PARAMETER_SETTER(T, NAME)     \
  inline LoadRobotParameters & NAME(T value) noexcept \
  {                                                   \
    NAME##_ = value;                                  \
    return *this;                                     \
  }
 
  MAKE_LOAD_ROBOT_PARAMETER_SETTER(const sva::PTransformd &, base_tf)
  MAKE_LOAD_ROBOT_PARAMETER_SETTER(std::string_view, base)
  MAKE_LOAD_ROBOT_PARAMETER_SETTER(bool, warn_on_missing_files)
  MAKE_LOAD_ROBOT_PARAMETER_SETTER(RobotDataPtr, data)
 
#undef MAKE_LOAD_ROBOT_PARAMETER_SETTER
};
 
struct MC_RBDYN_DLLAPI Robot
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  friend struct Robots;
 
public:
  using convex_pair_t = std::pair<std::string, S_ObjectPtr>;
 
public:
  Robot(Robot &&);
  Robot & operator=(Robot &&);
 
  ~Robot();
 
  const std::string & name() const;
 
  const RobotModule & module() const;
 
  inline const BodySensor & bodySensor() const noexcept { return data_->bodySensors[0]; }
 
  void addBodySensor(const BodySensor & sensor);
 
  inline bool hasBodySensor(const std::string & name) const noexcept
  { return data_->bodySensorsIndex.count(name) != 0; }
 
  inline bool bodyHasBodySensor(const std::string & body) const noexcept
  { return data_->bodyBodySensors.count(body) != 0; }
 
  const BodySensor & bodySensor(const std::string & name) const;
 
  const BodySensor & bodyBodySensor(const std::string & name) const;
 
  inline const BodySensorVector & bodySensors() const noexcept { return data_->bodySensors; }
 
  /* End of Body sensors group */
 
  inline bool jointHasJointSensor(const std::string & joint) const noexcept
  { return data()->jointJointSensors.count(joint) != 0; }
 
  const JointSensor & jointJointSensor(const std::string & joint) const;
 
  inline const std::vector<JointSensor> & jointSensors() const noexcept { return data_->jointSensors; }
 
  /* End of Joint sensors group */
 
  bool hasJoint(const std::string & name) const;
 
  bool hasBody(const std::string & name) const;
 
  inline bool hasFrame(const std::string & name) const noexcept { return frames_.count(name) != 0; }
 
  inline const RobotFrame & frame(const std::string & name) const
  {
    auto it = frames_.find(name);
    if(it != frames_.end()) { return *it->second; }
    mc_rtc::log::error_and_throw("No frame named {} in {}", name, name_);
  }
 
  inline RobotFrame & frame(const std::string & name)
  { return const_cast<RobotFrame &>(static_cast<const Robot *>(this)->frame(name)); }
 
  std::vector<std::string> frames() const;
 
  RobotFrame & makeFrame(const std::string & name, RobotFrame & parent, sva::PTransformd X_p_f, bool baked = false);
 
  RobotFramePtr makeTemporaryFrame(const std::string & name,
                                   const RobotFrame & parent,
                                   sva::PTransformd X_p_f,
                                   bool baked = false) const;
 
  void makeFrames(std::vector<mc_rbdyn::RobotModule::FrameDescription> frames);
 
  unsigned int jointIndexByName(const std::string & name) const;
 
  int jointIndexInMBC(size_t jointIndex) const;
 
  unsigned int bodyIndexByName(const std::string & name) const;
 
  rbd::MultiBody & mb();
  const rbd::MultiBody & mb() const;
 
  rbd::MultiBodyConfig & mbc();
  const rbd::MultiBodyConfig & mbc() const;
 
  rbd::MultiBodyGraph & mbg();
  const rbd::MultiBodyGraph & mbg() const;
 
  const std::vector<std::vector<double>> & q() const;
  const std::vector<std::vector<double>> & alpha() const;
  const std::vector<std::vector<double>> & alphaD() const;
  const std::vector<std::vector<double>> & jointTorque() const;
  inline const std::vector<std::vector<double>> & controlTorque() const noexcept { return jointTorque(); }
  const std::vector<sva::PTransformd> & bodyPosW() const;
  const std::vector<sva::MotionVecd> & bodyVelW() const;
  const std::vector<sva::MotionVecd> & bodyVelB() const;
  const std::vector<sva::MotionVecd> & bodyAccB() const;
  std::vector<std::vector<double>> & q();
  std::vector<std::vector<double>> & alpha();
  std::vector<std::vector<double>> & alphaD();
  std::vector<std::vector<double>> & jointTorque();
  inline std::vector<std::vector<double>> & controlTorque() noexcept { return jointTorque(); }
  std::vector<sva::PTransformd> & bodyPosW();
  std::vector<sva::MotionVecd> & bodyVelW();
  std::vector<sva::MotionVecd> & bodyVelB();
  std::vector<sva::MotionVecd> & bodyAccB();
 
  const sva::PTransformd & bodyPosW(const std::string & name) const;
 
  sva::PTransformd X_b1_b2(const std::string & b1, const std::string & b2) const;
 
  const sva::MotionVecd & bodyVelW(const std::string & name) const;
 
  const sva::MotionVecd & bodyVelB(const std::string & name) const;
 
  const sva::MotionVecd & bodyAccB(const std::string & name) const;
 
  Eigen::Vector3d com() const;
  Eigen::Vector3d comVelocity() const;
  Eigen::Vector3d comAcceleration() const;
 
  sva::ForceVecd surfaceWrench(const std::string & surfaceName) const;
 
  sva::ForceVecd bodyWrench(const std::string & bodyName) const;
 
  Eigen::Vector2d cop(const std::string & frame, double min_pressure = 0.5) const;
 
  Eigen::Vector3d copW(const std::string & frame, double min_pressure = 0.5) const;
 
  sva::ForceVecd netWrench(const std::vector<std::string> & sensorNames) const;
 
  Eigen::Vector3d zmp(const sva::ForceVecd & netTotalWrench,
                      const Eigen::Vector3d & plane_p,
                      const Eigen::Vector3d & plane_n,
                      double minimalNetNormalForce = 1.) const;
 
  bool zmp(Eigen::Vector3d & zmpOut,
           const sva::ForceVecd & netTotalWrench,
           const Eigen::Vector3d & plane_p,
           const Eigen::Vector3d & plane_n,
           double minimalNetNormalForce = 1.) const noexcept;
 
  Eigen::Vector3d zmp(const sva::ForceVecd & netTotalWrench,
                      const sva::PTransformd & zmpFrame,
                      double minimalNetNormalForce = 1.) const;
 
  bool zmp(Eigen::Vector3d & zmpOut,
           const sva::ForceVecd & netTotalWrench,
           const sva::PTransformd & zmpFrame,
           double minimalNetNormalForce = 1.) const noexcept;
 
  Eigen::Vector3d zmp(const std::vector<std::string> & sensorNames,
                      const Eigen::Vector3d & plane_p,
                      const Eigen::Vector3d & plane_n,
                      double minimalNetNormalForce = 1.) const;
 
  bool zmp(Eigen::Vector3d & zmpOut,
           const std::vector<std::string> & sensorNames,
           const Eigen::Vector3d & plane_p,
           const Eigen::Vector3d & plane_n,
           double minimalNetNormalForce = 1.) const noexcept;
 
  Eigen::Vector3d zmp(const std::vector<std::string> & sensorNames,
                      const sva::PTransformd & zmpFrame,
                      double minimalNetNormalForce = 1.) const;
 
  bool zmp(Eigen::Vector3d & zmpOut,
           const std::vector<std::string> & sensorNames,
           const sva::PTransformd & zmpFrame,
           double minimalNetNormalForce = 1.) const noexcept;
 
  const std::vector<std::vector<double>> & ql() const;
  const std::vector<std::vector<double>> & qu() const;
  const std::vector<std::vector<double>> & vl() const;
  const std::vector<std::vector<double>> & vu() const;
  const std::vector<std::vector<double>> & al() const;
  const std::vector<std::vector<double>> & au() const;
  const std::vector<std::vector<double>> & jl() const;
  const std::vector<std::vector<double>> & ju() const;
  const std::vector<std::vector<double>> & tl() const;
  const std::vector<std::vector<double>> & tu() const;
  const std::vector<std::vector<double>> & tdl() const;
  const std::vector<std::vector<double>> & tdu() const;
  std::vector<std::vector<double>> & ql();
  std::vector<std::vector<double>> & qu();
  std::vector<std::vector<double>> & vl();
  std::vector<std::vector<double>> & vu();
  std::vector<std::vector<double>> & al();
  std::vector<std::vector<double>> & au();
  std::vector<std::vector<double>> & jl();
  std::vector<std::vector<double>> & ju();
  std::vector<std::vector<double>> & tl();
  std::vector<std::vector<double>> & tu();
  std::vector<std::vector<double>> & tdl();
  std::vector<std::vector<double>> & tdu();
 
  const std::vector<Flexibility> & flexibility() const;
  std::vector<Flexibility> & flexibility();
 
  void zmpTarget(const Eigen::Vector3d & zmp);
 
  const Eigen::Vector3d & zmpTarget() const;
 
  inline double mass() const noexcept { return mass_; }
 
  inline const std::vector<double> & encoderValues() const noexcept { return data_->encoderValues; }
 
  inline const std::vector<double> & encoderVelocities() const noexcept { return data_->encoderVelocities; }
 
  inline const std::vector<double> & jointTorques() const noexcept { return data_->jointTorques; }
 
  inline const std::vector<std::string> & refJointOrder() const { return data_->refJointOrder; }
 
  /* End Joints sensors section */
 
  void addForceSensor(const mc_rbdyn::ForceSensor & fs);
 
  inline bool hasForceSensor(const std::string & name) const noexcept
  { return data_->forceSensorsIndex.count(name) != 0; }
 
  inline bool bodyHasForceSensor(const std::string & body) const noexcept
  { return data_->bodyForceSensors_.count(body) != 0; }
 
  inline bool surfaceHasForceSensor(const std::string & surface) const
  { return bodyHasForceSensor(this->surface(surface).bodyName()); }
 
  inline bool bodyHasIndirectForceSensor(const std::string & body) const
  { return bodyHasForceSensor(body) || findIndirectForceSensorBodyName(body).empty(); }
 
  inline bool surfaceHasIndirectForceSensor(const std::string & surface) const
  { return bodyHasIndirectForceSensor(this->surface(surface).bodyName()); }
 
  const ForceSensor & forceSensor(const std::string & name) const;
 
  const ForceSensor & bodyForceSensor(const std::string & body) const;
 
  const ForceSensor * findBodyForceSensor(const std::string & body) const;
 
  const ForceSensor & surfaceForceSensor(const std::string & surfaceName) const;
 
  const ForceSensor & indirectBodyForceSensor(const std::string & body) const;
 
  const ForceSensor & indirectSurfaceForceSensor(const std::string & surface) const;
 
  inline const std::vector<ForceSensor> & forceSensors() const noexcept { return data_->forceSensors; }
 
  /* End of Force sensors group */
 
  template<typename T>
  bool hasDevice(const std::string & name) const noexcept;
 
  template<typename T>
  inline bool hasSensor(const std::string & name) const noexcept
  { return hasDevice<T>(name); }
 
  template<typename T>
  const T & device(const std::string & name) const;
 
  template<typename T>
  T & device(const std::string & name)
  { return const_cast<T &>(const_cast<const Robot *>(this)->device<T>(name)); }
 
  inline const DevicePtrVector & devices() const noexcept { return data_->devices; }
 
  template<typename T>
  inline const T & sensor(const std::string & name) const
  { return device<T>(name); }
 
  template<typename T>
  inline T & sensor(const std::string & name)
  { return device<T>(name); }
 
  void addDevice(DevicePtr device);
 
  inline void addSensor(SensorPtr sensor) { addDevice(std::move(sensor)); }
 
  /* End of Devices group */
 
  bool hasSurface(const std::string & surface) const;
 
  mc_rbdyn::Surface & surface(const std::string & sName);
  const mc_rbdyn::Surface & surface(const std::string & sName) const;
 
  sva::PTransformd surfacePose(const std::string & sName) const;
 
  mc_rbdyn::Surface & copySurface(const std::string & sName, const std::string & name);
 
  void addSurface(mc_rbdyn::SurfacePtr surface, bool doNotReplace = true);
 
  const std::map<std::string, mc_rbdyn::SurfacePtr> & surfaces() const;
 
  std::vector<std::string> availableSurfaces() const;
 
  bool hasConvex(const std::string & name) const;
 
  convex_pair_t & convex(const std::string & cName);
  const convex_pair_t & convex(const std::string & cName) const;
 
  const std::map<std::string, convex_pair_t> & convexes() const;
 
  void addConvex(const std::string & name,
                 const std::string & body,
                 S_ObjectPtr convex,
                 const sva::PTransformd & X_b_c = sva::PTransformd::Identity());
 
  void removeConvex(const std::string & name);
 
  const sva::PTransformd & bodyTransform(const std::string & bName) const;
 
  const sva::PTransformd & bodyTransform(int bodyIndex) const;
 
  const std::vector<sva::PTransformd> & bodyTransforms() const;
 
  const sva::PTransformd & collisionTransform(const std::string & cName) const;
 
  void loadRSDFFromDir(const std::string & surfaceDir);
 
  std::map<std::string, std::vector<double>> stance() const;
 
  inline mc_rbdyn::Robots & robots() noexcept { return *robots_; }
 
  inline const mc_rbdyn::Robots & robots() const noexcept { return *robots_; }
 
  unsigned int robotIndex() const;
 
  void forwardKinematics();
  void forwardKinematics(rbd::MultiBodyConfig & mbc) const;
 
  void forwardVelocity();
  void forwardVelocity(rbd::MultiBodyConfig & mbc) const;
 
  void forwardAcceleration(const sva::MotionVecd & A_0 = sva::MotionVecd(Eigen::Vector6d::Zero()));
  void forwardAcceleration(rbd::MultiBodyConfig & mbc,
                           const sva::MotionVecd & A_0 = sva::MotionVecd(Eigen::Vector6d::Zero())) const;
 
  void eulerIntegration(double step);
  void eulerIntegration(rbd::MultiBodyConfig & mbc, double step) const;
 
  const sva::PTransformd & posW() const;
  void posW(const sva::PTransformd & pt);
 
  void velW(const sva::MotionVecd & vel);
 
  const sva::MotionVecd & velW() const;
 
  void accW(const sva::MotionVecd & acc);
 
  const sva::MotionVecd accW() const;
 
  mc_control::Gripper & gripper(const std::string & gripper);
 
  const mc_control::Gripper & gripper(const std::string & gripper) const;
 
  bool hasGripper(const std::string & gripper) const;
 
  inline const std::unordered_map<std::string, mc_control::GripperPtr> & grippersByName() const noexcept
  { return data_->grippers; }
 
  inline const std::vector<mc_control::GripperRef> & grippers() const noexcept { return data_->grippersRef; }
 
  inline const RobotDataPtr data() const noexcept { return data_; }
 
  mc_tvm::Robot & tvmRobot() const;
 
  mc_tvm::Convex & tvmConvex(const std::string & name) const;
 
private:
  Robots * robots_;
  unsigned int robots_idx_;
  std::string name_;
  Eigen::Vector3d zmp_;
  std::vector<sva::PTransformd> bodyTransforms_;
  std::vector<std::vector<double>> ql_;
  std::vector<std::vector<double>> qu_;
  std::vector<std::vector<double>> vl_;
  std::vector<std::vector<double>> vu_;
  std::vector<std::vector<double>> al_;
  std::vector<std::vector<double>> au_;
  std::vector<std::vector<double>> jl_;
  std::vector<std::vector<double>> ju_;
  std::vector<std::vector<double>> tl_;
  std::vector<std::vector<double>> tu_;
  std::vector<std::vector<double>> tdl_;
  std::vector<std::vector<double>> tdu_;
  std::map<std::string, convex_pair_t> convexes_;
  std::map<std::string, sva::PTransformd> collisionTransforms_;
  std::map<std::string, mc_rbdyn::SurfacePtr> surfaces_;
  std::map<std::string, std::vector<double>> stance_;
  RobotDataPtr data_;
  std::vector<int> refJointIndexToMBCIndex_;
  Springs springs_;
  std::vector<Flexibility> flexibility_;
  std::unordered_map<std::string, RobotFramePtr> frames_;
  double mass_ = 0.0;
 
protected:
  struct NewRobotToken
  {
  };
 
public:
  Robot(NewRobotToken,
        const std::string & name,
        Robots & robots,
        unsigned int robots_idx,
        bool loadFiles,
        const LoadRobotParameters & params = {});
 
protected:
  void copyLoadedData(Robot & destination) const;
 
  void fixSurfaces();
 
  void fixSurface(Surface & surface);
 
  void fixCollisionTransforms();
 
  std::string findIndirectForceSensorBodyName(const std::string & bodyName) const;
 
private:
  Robot(const Robot &) = delete;
  Robot & operator=(const Robot &) = delete;
 
  /* mutable to allow initialization in const method */
  mutable mc_tvm::RobotPtr tvm_robot_;
 
  /* mutable to allow initialization in const method */
  mutable std::map<std::string, mc_tvm::ConvexPtr> tvm_convexes_;
 
  void name(const std::string & n);
 
  LoadRobotParameters load_params_;
};
 
MC_RBDYN_DLLAPI const mc_rbdyn::Robot & robotFromConfig(const mc_rtc::Configuration & config,
                                                        const mc_rbdyn::Robots & robots,
                                                        const std::string & prefix,
                                                        bool required = false,
                                                        const std::string & robotIndexKey = "robotIndex",
                                                        const std::string & robotNameKey = "robot",
                                                        const std::string & defaultRobotName = "");
 
std::string MC_RBDYN_DLLAPI robotNameFromConfig(const mc_rtc::Configuration & config,
                                                const mc_rbdyn::Robots & robots,
                                                const std::string & prefix = "",
                                                bool required = false,
                                                const std::string & robotIndexKey = "robotIndex",
                                                const std::string & robotNameKey = "robot",
                                                const std::string & defaultRobotName = "");
 
unsigned int MC_RBDYN_DLLAPI robotIndexFromConfig(const mc_rtc::Configuration & config,
                                                  const mc_rbdyn::Robots & robots,
                                                  const std::string & prefix = "",
                                                  bool required = false,
                                                  const std::string & robotIndexKey = "robotIndex",
                                                  const std::string & robotNameKey = "robot",
                                                  const std::string & defaultRobotName = "");
 
/*FIXME Not implemetend for now, only used for ATLAS
void loadPolyTorqueBoundsData(const std::string & file, Robot & robot);
*/
 
} // namespace mc_rbdyn
 
#include <mc_rbdyn/Robot.hpp>