mc_global_controller.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_control/ControllerServer.h>
#include <mc_control/GlobalPlugin_fwd.h>
#include <mc_control/MCController.h>
#include <mc_control/api.h>
 
#include <mc_rbdyn/RobotModule.h>
 
#include <mc_rtc/loader.h>
#include <mc_rtc/log/Logger.h>
 
#include <array>
 
namespace mc_control
{
 
struct MC_CONTROL_DLLAPI MCGlobalController
{
public:
  struct GlobalConfiguration;
 
  using QuaternionMapAllocator = Eigen::aligned_allocator<std::pair<const std::string, Eigen::Quaterniond>>;
  using QuaternionMap = std::map<std::string, Eigen::Quaterniond, std::less<std::string>, QuaternionMapAllocator>;
 
private:
  /* MCGlobalController is non-copyable */
  MCGlobalController(const MCGlobalController &) = delete;
  MCGlobalController & operator=(const MCGlobalController &) = delete;
 
  /* MCGlobalController is non-movable */
  MCGlobalController(MCGlobalController &&) = delete;
  MCGlobalController & operator=(MCGlobalController &&) = delete;
 
public:
  MCGlobalController(const std::string & conf = "", std::shared_ptr<mc_rbdyn::RobotModule> rm = nullptr);
 
  MCGlobalController(const GlobalConfiguration & conf);
 
  ~MCGlobalController();
 
  std::vector<std::string> enabled_controllers() const;
 
  std::vector<std::string> loaded_controllers() const;
 
  std::vector<std::string> loaded_robots() const;
 
  std::shared_ptr<mc_rbdyn::RobotModule> get_robot_module();
 
  std::string current_controller() const;
 
  void init(const std::vector<double> & initq);
 
  void init(const std::vector<double> & initq, const std::array<double, 7> & initAttitude);
 
  void init(const std::vector<double> & initq, const sva::PTransformd & initAttitude);
 
  void init(const std::map<std::string, std::vector<double>> & initqs = {},
            const std::map<std::string, sva::PTransformd> & initAttitudes = {});
 
  void reset(const std::map<std::string, std::vector<double>> & resetqs = {},
             const std::map<std::string, sva::PTransformd> & resetAttitudes = {});
 
  void setSensorPosition(const Eigen::Vector3d & pos);
 
  void setSensorPosition(const std::string & robotName, const Eigen::Vector3d & pos);
 
  void setSensorPositions(const std::map<std::string, Eigen::Vector3d> & poses);
 
  void setSensorPositions(const std::string & robotName, const std::map<std::string, Eigen::Vector3d> & poses);
 
  void setSensorOrientation(const Eigen::Quaterniond & ori);
 
  void setSensorOrientation(const std::string & robotName, const Eigen::Quaterniond & ori);
 
  void setSensorOrientations(const QuaternionMap & oris);
 
  void setSensorOrientations(const std::string & robotName, const QuaternionMap & oris);
 
  void setSensorLinearVelocity(const Eigen::Vector3d & vel);
  void setSensorLinearVelocity(const std::string & robotName, const Eigen::Vector3d & vel);
 
  void setSensorLinearVelocities(const std::map<std::string, Eigen::Vector3d> & linearVels);
 
  void setSensorLinearVelocities(const std::string & robotName,
                                 const std::map<std::string, Eigen::Vector3d> & linearVels);
 
  void setSensorAngularVelocity(const Eigen::Vector3d & vel);
 
  void setSensorAngularVelocity(const std::string & robotName, const Eigen::Vector3d & vel);
 
  void setSensorAngularVelocities(const std::map<std::string, Eigen::Vector3d> & angularVels);
 
  void setSensorAngularVelocities(const std::string & robotName,
                                  const std::map<std::string, Eigen::Vector3d> & angularVels);
 
  MC_RTC_DEPRECATED void setSensorAcceleration(const Eigen::Vector3d & acc);
  MC_RTC_DEPRECATED void setSensorAccelerations(const std::map<std::string, Eigen::Vector3d> & accels);
 
  void setSensorLinearAcceleration(const Eigen::Vector3d & acc);
  void setSensorLinearAcceleration(const std::string & robotName, const Eigen::Vector3d & acc);
 
  void setSensorLinearAccelerations(const std::map<std::string, Eigen::Vector3d> & accels);
 
  void setSensorLinearAccelerations(const std::string & robotName,
                                    const std::map<std::string, Eigen::Vector3d> & accels);
 
  void setSensorAngularAcceleration(const Eigen::Vector3d & acc);
 
  void setSensorAngularAcceleration(const std::string & robotName, const Eigen::Vector3d & acc);
 
  void setSensorAngularAccelerations(const std::map<std::string, Eigen::Vector3d> & accels);
 
  void setSensorAngularAccelerations(const std::string & robotName,
                                     const std::map<std::string, Eigen::Vector3d> & accels);
 
  void setEncoderValues(const std::vector<double> & eValues);
 
  void setEncoderValues(const std::string & robotName, const std::vector<double> & eValues);
 
  void setEncoderVelocities(const std::vector<double> & eVelocities);
 
  /* Set the actual joint velocities for the specified robot (control + real)
   *
   * \param robotName Name of the robot to which the sensor values will be assigned.
   * A robot with that name must exist in both robots() and realRobots() instances.
   * \param eVelocities Actual joint velocities
   *
   * \throws If the specified robot does not exist
   */
  void setEncoderVelocities(const std::string & robotName, const std::vector<double> & eVelocities);
 
  void setJointTorques(const std::vector<double> & tValues);
  void setJointTorques(const std::string & robotName, const std::vector<double> & tValues);
 
  void setWrenches(const std::map<std::string, sva::ForceVecd> & wrenches);
 
  void setWrenches(const std::string & robotName, const std::map<std::string, sva::ForceVecd> & wrenches);
  MC_RTC_DEPRECATED void setWrenches(unsigned int robotIndex, const std::map<std::string, sva::ForceVecd> & wrenches);
 
  void setJointMotorTemperature(const std::string & joint, double temperature);
 
  void setJointMotorTemperature(const std::string & robotName, const std::string & joint, double temperature);
 
  void setJointMotorTemperatures(const std::map<std::string, double> & temperatures);
 
  void setJointMotorTemperatures(const std::string & robotName, const std::map<std::string, double> & temperatures);
 
  void setJointDriverTemperature(const std::string & joint, double temperature);
 
  void setJointDriverTemperature(const std::string & robotName, const std::string & joint, double temperature);
 
  void setJointDriverTemperatures(const std::map<std::string, double> & temperatures);
 
  void setJointDriverTemperatures(const std::string & robotName, const std::map<std::string, double> & temperatures);
 
  void setJointMotorCurrent(const std::string & joint, double current);
 
  void setJointMotorCurrent(const std::string & robotName, const std::string & joint, double current);
 
  void setJointMotorCurrents(const std::map<std::string, double> & currents);
 
  void setJointMotorCurrents(const std::string & robotName, const std::map<std::string, double> & currents);
 
  void setJointMotorStatus(const std::string & joint, bool status);
 
  void setJointMotorStatus(const std::string & robotName, const std::string & joint, bool status);
 
  void setJointMotorStatuses(const std::map<std::string, bool> & statuses);
 
  void setJointMotorStatuses(const std::string & robotName, const std::map<std::string, bool> & statuses);
protected:
  void setSensorPositions(mc_rbdyn::Robot & robot, const std::map<std::string, Eigen::Vector3d> & poses);
  void setSensorAngularAccelerations(mc_rbdyn::Robot & robot, const std::map<std::string, Eigen::Vector3d> & accels);
  void setSensorLinearAccelerations(mc_rbdyn::Robot & robot, const std::map<std::string, Eigen::Vector3d> & accels);
 
  MC_RTC_DEPRECATED void setSensorAccelerations(mc_rbdyn::Robot & robot,
                                                const std::map<std::string, Eigen::Vector3d> & accels);
  void setSensorOrientations(mc_rbdyn::Robot & robot, const QuaternionMap & oris);
  void setSensorLinearVelocities(mc_rbdyn::Robot & robot, const std::map<std::string, Eigen::Vector3d> & linearVels);
  void setSensorAngularVelocities(mc_rbdyn::Robot & robot, const std::map<std::string, Eigen::Vector3d> & angularVels);
public:
  bool run();
 
  ControllerServer & server();
 
  inline MCController & controller() noexcept
  {
    assert(controller_ != nullptr);
    return *controller_;
  }
 
  inline const MCController & controller() const noexcept
  {
    assert(controller_ != nullptr);
    return *controller_;
  }
 
#define MAKE_ROBOTS_ACCESSOR(NAME, PTR)                                                      \
  inline mc_rbdyn::Robots & NAME##s() noexcept { return *controller().PTR##s_; }             \
  inline const mc_rbdyn::Robots & NAME##s() const noexcept { return *controller().PTR##s_; } \
  inline mc_rbdyn::Robot & NAME() noexcept { return NAME##s().robot(); }                     \
  inline const mc_rbdyn::Robot & NAME() const noexcept { return NAME##s().robot(); }         \
  inline mc_rbdyn::Robot & NAME(const std::string & name) { return NAME##s().robot(name); }  \
  inline const mc_rbdyn::Robot & NAME(const std::string & name) const { return NAME##s().robot(name); }
 
  MAKE_ROBOTS_ACCESSOR(robot, outputRobot)
  MAKE_ROBOTS_ACCESSOR(realRobot, outputRealRobot)
 
#undef MAKE_ROBOTS_ACCESSOR
 
  double timestep() const;
 
  const std::vector<std::string> & ref_joint_order();
 
  const GlobalConfiguration & configuration() const;
 
  void add_controller_module_paths(const std::vector<std::string> & paths);
 
  bool AddController(const std::string & name);
 
  bool AddController(const std::string & name, std::shared_ptr<mc_control::MCController> controller);
 
  bool EnableController(const std::string & name);
 
  void setGripperTargetQ(const std::string & robot, const std::string & name, const std::vector<double> & q);
 
  void setGripperOpenPercent(const std::string & robot, double pOpen);
 
  void setGripperOpenPercent(const std::string & robot, const std::string & name, double pOpen);
  bool GoToHalfSitPose_service();
  bool GoToHalfSitPose();
 
  void refreshLog();
 
private:
  void init(const std::map<std::string, std::vector<double>> & initqs,
            const std::map<std::string, sva::PTransformd> & initAttitudes,
            bool reset);
 
  void initEncoders(mc_rbdyn::Robot & robot, const std::vector<double> & initq);
 
  void initEncoders(mc_rbdyn::Robot & robot);
 
  void initController(bool reset = false);
 
public:
  bool running = false;
 
  struct MC_CONTROL_DLLAPI GlobalConfiguration
  {
    GlobalConfiguration(const std::string & conf,
                        std::shared_ptr<mc_rbdyn::RobotModule> rm = nullptr,
                        bool conf_only = false);
 
    inline bool enabled(const std::string & ctrl);
 
    bool verbose_loader = true;
 
    bool init_attitude_from_sensor = false;
    std::string init_attitude_sensor;
 
    std::vector<std::string> robot_module_paths;
    std::shared_ptr<mc_rbdyn::RobotModule> main_robot_module;
 
    std::vector<std::string> observer_module_paths;
 
    std::vector<std::string> global_plugin_paths;
    std::vector<std::string> global_plugins;
    std::vector<std::string> global_plugins_autoload;
    std::unordered_map<std::string, mc_rtc::Configuration> global_plugin_configs;
 
    std::vector<std::string> controller_module_paths;
    std::vector<std::string> enabled_controllers;
    std::string initial_controller;
    std::unordered_map<std::string, mc_rtc::Configuration> controllers_configs;
    double timestep = 0.002;
    bool include_halfsit_controller = true;
 
    bool enable_log = true;
    mc_rtc::Logger::Policy log_policy = mc_rtc::Logger::Policy::NON_THREADED;
    std::string log_directory;
    std::string log_template = "mc-control";
 
    bool enable_gui_server = true;
    ControllerServerConfiguration gui_server_configuration;
 
    Configuration config;
 
    void load_controllers_configs();
 
    void load_plugin_configs();
 
    void load_controller_plugin_configs(const std::string & controller, const std::vector<std::string> & plugins);
  };
 
private:
  using duration_ms = std::chrono::duration<double, std::milli>;
  GlobalConfiguration config;
  std::string current_ctrl;
  std::string next_ctrl;
  MCController * controller_ = nullptr;
  MCController * next_controller_ = nullptr;
  std::unique_ptr<mc_rtc::ObjectLoader<MCController>> controller_loader_;
  std::map<std::string, std::shared_ptr<mc_control::MCController>> controllers;
  std::vector<mc_observers::ObserverPtr> observers_;
  std::map<std::string, mc_observers::ObserverPtr> observersByName_;
 
  std::unique_ptr<mc_control::ControllerServer> server_;
 
  std::unique_ptr<mc_rtc::ObjectLoader<GlobalPlugin>> plugin_loader_;
  struct PluginHandle
  {
    PluginHandle(const std::string & name, GlobalPluginPtr plugin) : name(name), plugin(std::move(plugin)) {}
    PluginHandle(const PluginHandle &) = delete;
    PluginHandle & operator=(const PluginHandle &) = delete;
    PluginHandle(PluginHandle &&) = default;
    PluginHandle & operator=(PluginHandle &&) = default;
    ~PluginHandle();
    std::string name;
    GlobalPluginPtr plugin;
  };
  std::vector<PluginHandle> plugins_;
  std::vector<PluginHandle> controller_plugins_;
  struct PluginBefore
  {
    GlobalPlugin * plugin;
    duration_ms plugin_before_dt;
  };
  std::vector<PluginBefore> plugins_before_;
  std::vector<GlobalPlugin *> plugins_before_always_;
  struct PluginAfter
  {
    GlobalPlugin * plugin;
    duration_ms plugin_after_dt;
  };
  std::vector<PluginAfter> plugins_after_;
  std::vector<GlobalPlugin *> plugins_after_always_;
 
  void initGUI();
 
  void start_log();
  void setup_log();
  void setup_plugin_log();
  std::map<std::string, bool> setup_logger_;
 
  duration_ms global_run_dt{0};
  duration_ms controller_run_dt{0};
  duration_ms observers_run_dt{0};
  duration_ms log_dt{0};
  duration_ms gui_dt{0};
  double solver_build_and_solve_t = 0;
  double solver_solve_t = 0;
  double framework_cost = 0;
 
  void resetControllerPlugins();
 
  GlobalPlugin * loadPlugin(const std::string & name, const char * requiredBy);
};
 
} // namespace mc_control