MCController.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_control/Configuration.h>
#include <mc_control/Contact.h>
 
#include <mc_observers/ObserverPipeline.h>
 
#include <mc_rbdyn/RobotConverter.h>
#include <mc_rbdyn/Robots.h>
 
#include <mc_rtc/DataStore.h>
#include <mc_rtc/gui.h>
#include <mc_rtc/log/Logger.h>
#include <mc_rtc/unique_ptr.h>
 
#include <mc_solver/CollisionsConstraint.h>
#include <mc_solver/CompoundJointConstraint.h>
#include <mc_solver/ContactConstraint.h>
#include <mc_solver/DynamicsConstraint.h>
#include <mc_solver/KinematicsConstraint.h>
#include <mc_solver/QPSolver.h>
 
#include <mc_tasks/PostureTask.h>
 
namespace mc_rbdyn
{
struct Contact;
}
 
#include <mc_control/api.h>
 
namespace mc_control
{
 
struct MC_CONTROL_DLLAPI ControllerResetData
{
  const std::vector<std::vector<double>> q;
};
 
struct MC_CONTROL_DLLAPI ControllerParameters
{
  inline ControllerParameters() = default;
  inline ControllerParameters(const ControllerParameters &) = default;
  inline ControllerParameters(ControllerParameters &&) = default;
  inline ControllerParameters & operator=(const ControllerParameters &) = default;
  inline ControllerParameters & operator=(ControllerParameters &&) = default;
 
#define ADD_PARAMETER(TYPE, NAME, DEFAULT) \
  TYPE NAME##_ = DEFAULT;                  \
  ControllerParameters & NAME(TYPE value)  \
  {                                        \
    NAME##_ = value;                       \
    return *this;                          \
  }
  ADD_PARAMETER(mc_solver::QPSolver::Backend, backend, mc_solver::QPSolver::Backend::Tasks)
  ADD_PARAMETER(bool, load_robot_config, true)
  ADD_PARAMETER(bool, overwrite_config, false)
  ADD_PARAMETER(bool, load_robot_config_with_module_name, true)
  ADD_PARAMETER(std::vector<std::string>, load_robot_config_into, {"robots"})
  ADD_PARAMETER(std::vector<std::string>, extra_configurations, {})
 
  inline ControllerParameters(mc_solver::QPSolver::Backend backend) : backend_(backend) {}
};
 
struct MCGlobalController;
 
struct MC_CONTROL_DLLAPI MCController
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  friend struct MCGlobalController;
 
public:
  using Backend = mc_solver::QPSolver::Backend;
 
  virtual ~MCController();
  virtual bool run();
 
  virtual void createObserverPipelines(const mc_rtc::Configuration & config);
 
  virtual bool runObserverPipelines();
 
  virtual bool resetObserverPipelines();
 
  bool hasObserverPipeline(const std::string & name) const;
 
  bool hasObserverPipeline() const;
 
  const std::vector<mc_observers::ObserverPipeline> & observerPipelines() const;
  std::vector<mc_observers::ObserverPipeline> & observerPipelines();
 
  const mc_observers::ObserverPipeline & observerPipeline(const std::string & name) const;
  mc_observers::ObserverPipeline & observerPipeline(const std::string & name);
 
  const mc_observers::ObserverPipeline & observerPipeline() const;
  mc_observers::ObserverPipeline & observerPipeline();
 
  bool run(mc_solver::FeedbackType fType);
 
  virtual void stop();
 
  virtual void reset(const ControllerResetData & reset_data);
 
  void addCollisions(const std::string & r1,
                     const std::string & r2,
                     const std::vector<mc_rbdyn::Collision> & collisions);
 
  bool hasCollision(const std::string & r1, const std::string & r2, const mc_rbdyn::Collision & col) const noexcept;
 
  bool hasCollision(const std::string & r1,
                    const std::string & r2,
                    const std::string & c1,
                    const std::string & c2) const noexcept;
 
  void removeCollisions(const std::string & r1,
                        const std::string & r2,
                        const std::vector<mc_rbdyn::Collision> & collisions);
 
  void removeCollisions(const std::string & r1, const std::string & r2);
 
  void addContact(const Contact & c);
 
  void removeContact(const Contact & c);
 
  void clearContacts();
 
  const ContactSet & contacts() const;
 
  bool hasContact(const Contact & c) const;
 
  inline bool hasRobot(const std::string & robot) const noexcept { return robots().hasRobot(robot); }
 
  inline const mc_rbdyn::Robots & robots() const noexcept { return solver().robots(); }
 
  inline mc_rbdyn::Robots & robots() noexcept { return solver().robots(); }
 
  inline const mc_rbdyn::Robot & robot() const noexcept { return robots().robot(); }
 
  inline mc_rbdyn::Robot & robot() noexcept { return robots().robot(); }
 
  inline const mc_rbdyn::Robot & robot(const std::string & name) const { return robots().robot(name); }
 
  inline mc_rbdyn::Robot & robot(const std::string & name) { return robots().robot(name); }
 
  inline const mc_rbdyn::Robot & env() const noexcept { return robots().env(); }
 
  inline mc_rbdyn::Robot & env() noexcept { return robots().env(); }
  inline const mc_solver::QPSolver & solver() const noexcept { return *qpsolver; }
 
  inline mc_solver::QPSolver & solver() noexcept { return *qpsolver; }
 
  inline mc_rtc::Logger & logger() noexcept { return *logger_; }
 
  inline std::shared_ptr<mc_rtc::gui::StateBuilder> gui() const noexcept { return gui_; }
 
  inline mc_rtc::DataStore & datastore() noexcept { return datastore_; }
 
  const mc_rtc::DataStore & datastore() const noexcept { return datastore_; }
 
  inline const mc_rbdyn::Robots & realRobots() const noexcept { return solver().realRobots(); }
  inline mc_rbdyn::Robots & realRobots() noexcept { return solver().realRobots(); }
 
  inline const mc_rbdyn::Robot & realRobot() const noexcept { return realRobots().robot(); }
  inline mc_rbdyn::Robot & realRobot() noexcept { return realRobots().robot(); }
 
  inline const mc_rbdyn::Robot & realRobot(const std::string & name) const { return realRobots().robot(name); }
 
  inline mc_rbdyn::Robot & realRobot(const std::string & name) { return realRobots().robot(name); }
  inline const mc_rbdyn::Robots & outputRobots() const noexcept { return *outputRobots_; }
  inline mc_rbdyn::Robots & outputRobots() noexcept { return *outputRobots_; }
 
  inline const mc_rbdyn::Robot & outputRobot() const noexcept { return outputRobots_->robot(); }
  inline mc_rbdyn::Robot & outputRobot() noexcept { return outputRobots_->robot(); }
 
  inline const mc_rbdyn::Robot & outputRobot(const std::string & name) const { return outputRobots_->robot(name); }
 
  inline mc_rbdyn::Robot & outputRobot(const std::string & name) { return outputRobots_->robot(name); }
  inline const mc_rbdyn::Robots & outputRealRobots() const noexcept { return *outputRealRobots_; }
  inline mc_rbdyn::Robots & outputRealRobots() noexcept { return *outputRealRobots_; }
 
  inline const mc_rbdyn::Robot & outputRealRobot() const noexcept { return outputRealRobots_->robot(); }
  inline mc_rbdyn::Robot & outputRealRobot() noexcept { return outputRealRobots_->robot(); }
 
  inline const mc_rbdyn::Robot & outputRealRobot(const std::string & name) const
  {
    return outputRealRobots_->robot(name);
  }
 
  inline mc_rbdyn::Robot & outputRealRobot(const std::string & name) { return outputRealRobots_->robot(name); }
  virtual void supported_robots(std::vector<std::string> & out) const;
 
  mc_rbdyn::Robot & loadRobot(mc_rbdyn::RobotModulePtr rm, const std::string & name);
 
  void removeRobot(const std::string & name);
 
  mc_rtc::Configuration & config() { return config_; }
 
  const mc_rtc::Configuration & config() const { return config_; }
 
  Gripper & gripper(const std::string & robot, const std::string & gripper);
 
  template<typename T>
  struct DeprecatedAnchorFrame : public std::false_type
  {
  };
 
  template<typename T = void> // void for shorter error message
  inline void anchorFrame(const sva::PTransformd &)
  {
    static_assert(DeprecatedAnchorFrame<T>::value,
                  "[MC_RTC_DEPRECATED] The anchorFrame and anchorFrameReal accessors are no longer supported, please "
                  "remove calls to these functions from your code and replace it with a datastore callback. For "
                  "further information please refer to the observer's tutorial: "
                  "https://jrl-umi3218.github.io/mc_rtc/tutorials/recipes/observers.html");
  }
 
  template<typename T = void> // void for shorter error message
  inline void anchorFrameReal(const sva::PTransformd &)
  {
    static_assert(DeprecatedAnchorFrame<T>::value,
                  "[MC_RTC_DEPRECATED] The anchorFrame and anchorFrameReal accessors are no longer supported, please "
                  "remove calls to these functions from your code and replace it with a datastore callback. For "
                  "further information please refer to the observer's tutorial: "
                  "https://jrl-umi3218.github.io/mc_rtc/tutorials/recipes/observers.html");
  }
  template<typename T = void> // void for shorter error message
  inline const sva::PTransformd & anchorFrame() const
  {
    static_assert(DeprecatedAnchorFrame<T>::value,
                  "[MC_RTC_DEPRECATED] The anchorFrame and anchorFrameReal accessors are no longer supported, please "
                  "remove calls to these functions from your code and replace it with a datastore callback. For "
                  "further information please refer to the observer's tutorial: "
                  "https://jrl-umi3218.github.io/mc_rtc/tutorials/recipes/observers.html");
    return robot().posW();
  }
 
  template<typename T = void> // void for shorter error message
  inline const sva::PTransformd & anchorFrameReal() const
  {
    static_assert(DeprecatedAnchorFrame<T>::value,
                  "[MC_RTC_DEPRECATED] The anchorFrame and anchorFrameReal accessors are no longer supported, please "
                  "remove calls to these functions from your code and replace it with a datastore callback. For "
                  "further information please refer to the observer's tutorial: "
                  "https://jrl-umi3218.github.io/mc_rtc/tutorials/recipes/observers.html");
    return robot().posW();
  }
 
protected:
  MCController(std::shared_ptr<mc_rbdyn::RobotModule> robot, double dt, ControllerParameters params = {});
 
  MCController(std::shared_ptr<mc_rbdyn::RobotModule> robot,
               double dt,
               const mc_rtc::Configuration & config,
               ControllerParameters params = {});
 
  MCController(const std::vector<std::shared_ptr<mc_rbdyn::RobotModule>> & robot_modules,
               double dt,
               ControllerParameters params = {});
 
  MCController(const std::vector<std::shared_ptr<mc_rbdyn::RobotModule>> & robot_modules,
               double dt,
               const mc_rtc::Configuration & config,
               ControllerParameters params = {});
 
  mc_rbdyn::Robot & loadRobot(mc_rbdyn::RobotModulePtr rm,
                              const std::string & name,
                              mc_rbdyn::Robots & robots,
                              const mc_rbdyn::LoadRobotParameters & params);
 
  void addRobotToLog(const mc_rbdyn::Robot & robot);
 
  void addRobotToGUI(const mc_rbdyn::Robot & robot);
 
  void updateContacts();
 
protected:
  std::shared_ptr<mc_solver::QPSolver> qpsolver;
 
  mc_rbdyn::RobotsPtr outputRobots_;
  mc_rbdyn::RobotsPtr outputRealRobots_;
  std::vector<mc_rbdyn::RobotConverter> converters_;
 
  std::vector<mc_observers::ObserverPipeline> observerPipelines_;
 
  std::shared_ptr<mc_rtc::Logger> logger_;
  std::shared_ptr<mc_rtc::gui::StateBuilder> gui_;
 
  mc_rtc::Configuration config_;
 
  mc_rtc::DataStore datastore_;
 
  std::vector<mc_solver::ConstraintSetPtr> constraints_;
 
  std::shared_ptr<mc_solver::ContactConstraint> contact_constraint_ = nullptr;
 
  std::map<std::pair<std::string, std::string>, std::shared_ptr<mc_solver::CollisionsConstraint>> collision_constraints_;
 
  ContactSet contacts_;
  bool contacts_changed_;
  using ContactTableDataT = std::tuple<std::string, std::string, std::string, std::string, std::string, double>;
  std::vector<ContactTableDataT> contacts_table_;
 
  using duration_ms = std::chrono::duration<double, std::milli>;
  duration_ms updateContacts_dt_{0};
 
public:
  const double timeStep;
  mc_rtc::unique_ptr<mc_solver::ContactConstraint> contactConstraint;
  mc_rtc::unique_ptr<mc_solver::DynamicsConstraint> dynamicsConstraint;
  mc_rtc::unique_ptr<mc_solver::KinematicsConstraint> kinematicsConstraint;
  mc_rtc::unique_ptr<mc_solver::CollisionsConstraint> selfCollisionConstraint;
  mc_rtc::unique_ptr<mc_solver::CompoundJointConstraint> compoundJointConstraint;
  std::shared_ptr<mc_tasks::PostureTask> postureTask;
  /* Controller's name */
  const std::string name_;
  const std::string loading_location_;
 
  mc_rtc::Configuration robot_config(const std::string & robot_name) const;
 
  mc_rtc::Configuration robot_config(const mc_rbdyn::Robot & robot) const;
 
  static void set_loading_location(std::string_view location);
 
  static void set_name(std::string_view name);
};
 
namespace details
{
 
template<MCController::Backend backend, typename SolverT>
struct BackendSpecificController : public MCController
{
  BackendSpecificController(mc_rbdyn::RobotModulePtr robot, double dt, const mc_rtc::Configuration & config = {})
  : MCController(robot, dt, config, backend)
  {
  }
 
  BackendSpecificController(const std::vector<mc_rbdyn::RobotModulePtr> & robots,
                            double dt,
                            const mc_rtc::Configuration & config = {},
                            ControllerParameters params = {})
  : MCController(robots, dt, config, params.backend(backend))
  {
  }
 
  const SolverT & solver() const noexcept { return SolverT::from_solver(MCController::solver()); }
 
  SolverT & solver() noexcept { return SolverT::from_solver(MCController::solver()); }
};
 
} // namespace details
 
} // namespace mc_control
 
#ifdef WIN32
#  define CONTROLLER_MODULE_API __declspec(dllexport)
#else
#  if __GNUC__ >= 4
#    define CONTROLLER_MODULE_API __attribute__((visibility("default")))
#  else
#    define CONTROLLER_MODULE_API
#  endif
#endif