mc_global_controller.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_control/ControllerServer.h>
8 #include <mc_control/GlobalPlugin_fwd.h>
9 #include <mc_control/MCController.h>
10 #include <mc_control/api.h>
11 
12 #include <mc_rbdyn/RobotModule.h>
13 
14 #include <mc_rtc/loader.h>
15 #include <mc_rtc/log/Logger.h>
16 
17 #include <array>
18 
19 namespace mc_control
20 {
21 
22 struct MC_CONTROL_DLLAPI MCGlobalController
23 {
24 public:
25  struct GlobalConfiguration;
26 
27  using QuaternionMapAllocator = Eigen::aligned_allocator<std::pair<const std::string, Eigen::Quaterniond>>;
28  using QuaternionMap = std::map<std::string, Eigen::Quaterniond, std::less<std::string>, QuaternionMapAllocator>;
29 
30 private:
31  /* MCGlobalController is non-copyable */
32  MCGlobalController(const MCGlobalController &) = delete;
33  MCGlobalController & operator=(const MCGlobalController &) = delete;
34 
35  /* MCGlobalController is non-movable */
36  MCGlobalController(MCGlobalController &&) = delete;
37  MCGlobalController & operator=(MCGlobalController &&) = delete;
38 
39 public:
72  MCGlobalController(const std::string & conf = "", std::shared_ptr<mc_rbdyn::RobotModule> rm = nullptr);
73 
75  MCGlobalController(const GlobalConfiguration & conf);
76 
78  ~MCGlobalController();
79 
81  std::vector<std::string> enabled_controllers() const;
82 
86  std::vector<std::string> loaded_controllers() const;
87 
91  std::vector<std::string> loaded_robots() const;
92 
94  std::shared_ptr<mc_rbdyn::RobotModule> get_robot_module();
95 
97  std::string current_controller() const;
98 
118  void init(const std::vector<double> & initq);
119 
127  void init(const std::vector<double> & initq, const std::array<double, 7> & initAttitude);
128 
135  void init(const std::vector<double> & initq, const sva::PTransformd & initAttitude);
136 
149  void init(const std::map<std::string, std::vector<double>> & initqs = {},
150  const std::map<std::string, sva::PTransformd> & initAttitudes = {});
151 
166  void reset(const std::map<std::string, std::vector<double>> & resetqs = {},
167  const std::map<std::string, sva::PTransformd> & resetAttitudes = {});
168 
182  void setSensorPosition(const Eigen::Vector3d & pos);
183 
194  void setSensorPosition(const std::string & robotName, const Eigen::Vector3d & pos);
195 
201  void setSensorPositions(const std::map<std::string, Eigen::Vector3d> & poses);
202 
211  void setSensorPositions(const std::string & robotName, const std::map<std::string, Eigen::Vector3d> & poses);
212 
222  void setSensorOrientation(const Eigen::Quaterniond & ori);
223 
233  void setSensorOrientation(const std::string & robotName, const Eigen::Quaterniond & ori);
234 
242  void setSensorOrientations(const QuaternionMap & oris);
243 
253  void setSensorOrientations(const std::string & robotName, const QuaternionMap & oris);
254 
260  void setSensorLinearVelocity(const Eigen::Vector3d & vel);
270  void setSensorLinearVelocity(const std::string & robotName, const Eigen::Vector3d & vel);
271 
277  void setSensorLinearVelocities(const std::map<std::string, Eigen::Vector3d> & linearVels);
278 
289  void setSensorLinearVelocities(const std::string & robotName,
290  const std::map<std::string, Eigen::Vector3d> & linearVels);
291 
298  void setSensorAngularVelocity(const Eigen::Vector3d & vel);
299 
309  void setSensorAngularVelocity(const std::string & robotName, const Eigen::Vector3d & vel);
310 
316  void setSensorAngularVelocities(const std::map<std::string, Eigen::Vector3d> & angularVels);
317 
327  void setSensorAngularVelocities(const std::string & robotName,
328  const std::map<std::string, Eigen::Vector3d> & angularVels);
329 
334  MC_RTC_DEPRECATED void setSensorAcceleration(const Eigen::Vector3d & acc);
338  MC_RTC_DEPRECATED void setSensorAccelerations(const std::map<std::string, Eigen::Vector3d> & accels);
339 
344  void setSensorLinearAcceleration(const Eigen::Vector3d & acc);
354  void setSensorLinearAcceleration(const std::string & robotName, const Eigen::Vector3d & acc);
355 
361  void setSensorLinearAccelerations(const std::map<std::string, Eigen::Vector3d> & accels);
362 
372  void setSensorLinearAccelerations(const std::string & robotName,
373  const std::map<std::string, Eigen::Vector3d> & accels);
374 
381  void setSensorAngularAcceleration(const Eigen::Vector3d & acc);
382 
392  void setSensorAngularAcceleration(const std::string & robotName, const Eigen::Vector3d & acc);
393 
399  void setSensorAngularAccelerations(const std::map<std::string, Eigen::Vector3d> & accels);
400 
410  void setSensorAngularAccelerations(const std::string & robotName,
411  const std::map<std::string, Eigen::Vector3d> & accels);
412 
420  void setEncoderValues(const std::vector<double> & eValues);
421 
430  void setEncoderValues(const std::string & robotName, const std::vector<double> & eValues);
431 
441  void setEncoderVelocities(const std::vector<double> & eVelocities);
442 
443  /* Set the actual joint velocities for the specified robot (control + real)
444  *
445  * \param robotName Name of the robot to which the sensor values will be assigned.
446  * A robot with that name must exist in both robots() and realRobots() instances.
447  * \param eVelocities Actual joint velocities
448  *
449  * \throws If the specified robot does not exist
450  */
451  void setEncoderVelocities(const std::string & robotName, const std::vector<double> & eVelocities);
452 
460  void setJointTorques(const std::vector<double> & tValues);
469  void setJointTorques(const std::string & robotName, const std::vector<double> & tValues);
470 
476  void setWrenches(const std::map<std::string, sva::ForceVecd> & wrenches);
477 
487  void setWrenches(const std::string & robotName, const std::map<std::string, sva::ForceVecd> & wrenches);
491  MC_RTC_DEPRECATED void setWrenches(unsigned int robotIndex, const std::map<std::string, sva::ForceVecd> & wrenches);
492 
499  void setJointMotorTemperature(const std::string & joint, double temperature);
500 
510  void setJointMotorTemperature(const std::string & robotName, const std::string & joint, double temperature);
511 
517  void setJointMotorTemperatures(const std::map<std::string, double> & temperatures);
518 
527  void setJointMotorTemperatures(const std::string & robotName, const std::map<std::string, double> & temperatures);
528 
535  void setJointDriverTemperature(const std::string & joint, double temperature);
536 
546  void setJointDriverTemperature(const std::string & robotName, const std::string & joint, double temperature);
547 
553  void setJointDriverTemperatures(const std::map<std::string, double> & temperatures);
554 
563  void setJointDriverTemperatures(const std::string & robotName, const std::map<std::string, double> & temperatures);
564 
571  void setJointMotorCurrent(const std::string & joint, double current);
572 
582  void setJointMotorCurrent(const std::string & robotName, const std::string & joint, double current);
583 
589  void setJointMotorCurrents(const std::map<std::string, double> & currents);
590 
599  void setJointMotorCurrents(const std::string & robotName, const std::map<std::string, double> & currents);
600 
607  void setJointMotorStatus(const std::string & joint, bool status);
608 
618  void setJointMotorStatus(const std::string & robotName, const std::string & joint, bool status);
619 
625  void setJointMotorStatuses(const std::map<std::string, bool> & statuses);
626 
635  void setJointMotorStatuses(const std::string & robotName, const std::map<std::string, bool> & statuses);
638 protected:
649  void setSensorPositions(mc_rbdyn::Robot & robot, const std::map<std::string, Eigen::Vector3d> & poses);
651  void setSensorAngularAccelerations(mc_rbdyn::Robot & robot, const std::map<std::string, Eigen::Vector3d> & accels);
653  void setSensorLinearAccelerations(mc_rbdyn::Robot & robot, const std::map<std::string, Eigen::Vector3d> & accels);
654 
659  MC_RTC_DEPRECATED void setSensorAccelerations(mc_rbdyn::Robot & robot,
660  const std::map<std::string, Eigen::Vector3d> & accels);
662  void setSensorOrientations(mc_rbdyn::Robot & robot, const QuaternionMap & oris);
664  void setSensorLinearVelocities(mc_rbdyn::Robot & robot, const std::map<std::string, Eigen::Vector3d> & linearVels);
666  void setSensorAngularVelocities(mc_rbdyn::Robot & robot, const std::map<std::string, Eigen::Vector3d> & angularVels);
669 public:
674  bool run();
675 
677  ControllerServer & server();
678 
680  inline MCController & controller() noexcept
681  {
682  assert(controller_ != nullptr);
683  return *controller_;
684  }
685 
687  inline const MCController & controller() const noexcept
688  {
689  assert(controller_ != nullptr);
690  return *controller_;
691  }
692 
703 #define MAKE_ROBOTS_ACCESSOR(NAME, PTR) \
704  inline mc_rbdyn::Robots & NAME##s() noexcept { return *controller().PTR##s_; } \
705  inline const mc_rbdyn::Robots & NAME##s() const noexcept { return *controller().PTR##s_; } \
706  inline mc_rbdyn::Robot & NAME() noexcept { return NAME##s().robot(); } \
707  inline const mc_rbdyn::Robot & NAME() const noexcept { return NAME##s().robot(); } \
708  inline mc_rbdyn::Robot & NAME(const std::string & name) { return NAME##s().robot(name); } \
709  inline const mc_rbdyn::Robot & NAME(const std::string & name) const { return NAME##s().robot(name); }
710 
711  MAKE_ROBOTS_ACCESSOR(robot, outputRobot)
712  MAKE_ROBOTS_ACCESSOR(realRobot, outputRealRobot)
713 
714 #undef MAKE_ROBOTS_ACCESSOR
715 
719  double timestep() const;
720 
726  const std::vector<std::string> & ref_joint_order();
727 
729  const GlobalConfiguration & configuration() const;
730 
741  void add_controller_module_paths(const std::vector<std::string> & paths);
742 
752  bool AddController(const std::string & name);
753 
767  bool AddController(const std::string & name, std::shared_ptr<mc_control::MCController> controller);
768 
777  bool EnableController(const std::string & name);
778 
792  void setGripperTargetQ(const std::string & robot, const std::string & name, const std::vector<double> & q);
793 
798  void setGripperOpenPercent(const std::string & robot, double pOpen);
799 
805  void setGripperOpenPercent(const std::string & robot, const std::string & name, double pOpen);
823  bool GoToHalfSitPose_service();
825  bool GoToHalfSitPose();
826 
836  void refreshLog();
837 
838 private:
840  void init(const std::map<std::string, std::vector<double>> & initqs,
841  const std::map<std::string, sva::PTransformd> & initAttitudes,
842  bool reset);
843 
851  void initEncoders(mc_rbdyn::Robot & robot, const std::vector<double> & initq);
852 
858  void initEncoders(mc_rbdyn::Robot & robot);
859 
867  void initController(bool reset = false);
868 
869 public:
876  bool running = false;
877 
879  struct MC_CONTROL_DLLAPI GlobalConfiguration
880  {
889  GlobalConfiguration(const std::string & conf,
890  std::shared_ptr<mc_rbdyn::RobotModule> rm = nullptr,
891  bool conf_only = false);
892 
893  inline bool enabled(const std::string & ctrl);
894 
895  bool verbose_loader = true;
896 
897  bool init_attitude_from_sensor = false;
898  std::string init_attitude_sensor;
899 
900  std::vector<std::string> robot_module_paths;
901  std::shared_ptr<mc_rbdyn::RobotModule> main_robot_module;
902 
903  std::vector<std::string> observer_module_paths;
904 
905  std::vector<std::string> global_plugin_paths;
906  std::vector<std::string> global_plugins;
907  std::vector<std::string> global_plugins_autoload;
908  std::unordered_map<std::string, mc_rtc::Configuration> global_plugin_configs;
909 
910  std::vector<std::string> controller_module_paths;
911  std::vector<std::string> enabled_controllers;
912  std::string initial_controller;
913  std::unordered_map<std::string, mc_rtc::Configuration> controllers_configs;
914  double timestep = 0.002;
915  bool include_halfsit_controller = true;
916 
917  bool enable_log = true;
918  mc_rtc::Logger::Policy log_policy = mc_rtc::Logger::Policy::NON_THREADED;
919  std::string log_directory;
920  std::string log_template = "mc-control";
921 
922  bool enable_gui_server = true;
923  ControllerServerConfiguration gui_server_configuration;
924 
925  Configuration config;
926 
927  void load_controllers_configs();
928 
929  void load_plugin_configs();
930 
931  void load_controller_plugin_configs(const std::string & controller, const std::vector<std::string> & plugins);
932  };
933 
934 private:
935  using duration_ms = std::chrono::duration<double, std::milli>;
936  GlobalConfiguration config;
937  std::string current_ctrl;
938  std::string next_ctrl;
939  MCController * controller_ = nullptr;
940  MCController * next_controller_ = nullptr;
941  std::unique_ptr<mc_rtc::ObjectLoader<MCController>> controller_loader_;
942  std::map<std::string, std::shared_ptr<mc_control::MCController>> controllers;
943  std::vector<mc_observers::ObserverPtr> observers_;
944  std::map<std::string, mc_observers::ObserverPtr> observersByName_;
945 
946  std::unique_ptr<mc_control::ControllerServer> server_;
947 
948  std::unique_ptr<mc_rtc::ObjectLoader<GlobalPlugin>> plugin_loader_;
949  struct PluginHandle
950  {
951  PluginHandle(const std::string & name, GlobalPluginPtr plugin) : name(name), plugin(std::move(plugin)) {}
952  PluginHandle(const PluginHandle &) = delete;
953  PluginHandle & operator=(const PluginHandle &) = delete;
954  PluginHandle(PluginHandle &&) = default;
955  PluginHandle & operator=(PluginHandle &&) = default;
956  ~PluginHandle();
957  std::string name;
958  GlobalPluginPtr plugin;
959  };
960  std::vector<PluginHandle> plugins_;
961  std::vector<PluginHandle> controller_plugins_;
962  struct PluginBefore
963  {
964  GlobalPlugin * plugin;
965  duration_ms plugin_before_dt;
966  };
967  std::vector<PluginBefore> plugins_before_;
968  std::vector<GlobalPlugin *> plugins_before_always_;
969  struct PluginAfter
970  {
971  GlobalPlugin * plugin;
972  duration_ms plugin_after_dt;
973  };
974  std::vector<PluginAfter> plugins_after_;
975  std::vector<GlobalPlugin *> plugins_after_always_;
976 
977  void initGUI();
978 
979  void start_log();
980  void setup_log();
981  void setup_plugin_log();
982  std::map<std::string, bool> setup_logger_;
983 
985  duration_ms global_run_dt{0};
986  duration_ms controller_run_dt{0};
987  duration_ms observers_run_dt{0};
988  duration_ms log_dt{0};
989  duration_ms gui_dt{0};
990  double solver_build_and_solve_t = 0;
991  double solver_solve_t = 0;
992  double framework_cost = 0;
993 
999  void resetControllerPlugins();
1000 
1007  GlobalPlugin * loadPlugin(const std::string & name, const char * requiredBy);
1008 };
1009 
1010 } // namespace mc_control
mc_rtc::Configuration
Simplify access to values hold within a JSON file.
Definition: Configuration.h:165
ControllerServer.h
mc_control::MCGlobalController::GlobalConfiguration::controller_module_paths
std::vector< std::string > controller_module_paths
Definition: mc_global_controller.h:910
mc_control::MCGlobalController::GlobalConfiguration
Store the controller configuration.
Definition: mc_global_controller.h:879
MC_CONTROL_DLLAPI
#define MC_CONTROL_DLLAPI
Definition: api.h:50
mc_control::MCGlobalController::GlobalConfiguration::config
Configuration config
Definition: mc_global_controller.h:925
mc_control::MCGlobalController::GlobalConfiguration::initial_controller
std::string initial_controller
Definition: mc_global_controller.h:912
mc_rbdyn::Robot
Definition: Robot.h:62
mc_control::GlobalPluginPtr
std::unique_ptr< GlobalPlugin, mc_rtc::ObjectLoader< GlobalPlugin >::ObjectDeleter > GlobalPluginPtr
Definition: GlobalPlugin_fwd.h:15
GlobalPlugin_fwd.h
mc_control::MCGlobalController
Definition: mc_global_controller.h:22
mc_control::MCGlobalController::GlobalConfiguration::controllers_configs
std::unordered_map< std::string, mc_rtc::Configuration > controllers_configs
Definition: mc_global_controller.h:913
mc_control::MCGlobalController::controller
const MCController & controller() const noexcept
Const access to current controller.
Definition: mc_global_controller.h:687
RobotModule.h
api.h
Logger.h
MAKE_ROBOTS_ACCESSOR
#define MAKE_ROBOTS_ACCESSOR(NAME, PTR)
Definition: mc_global_controller.h:703
mc_control::MCGlobalController::GlobalConfiguration::main_robot_module
std::shared_ptr< mc_rbdyn::RobotModule > main_robot_module
Definition: mc_global_controller.h:901
mc_control::MCGlobalController::GlobalConfiguration::log_directory
std::string log_directory
Definition: mc_global_controller.h:919
mc_rtc::duration_ms
std::chrono::duration< double, std::milli > duration_ms
Definition: clock.h:13
mc_control::MCGlobalController::GlobalConfiguration::init_attitude_sensor
std::string init_attitude_sensor
Definition: mc_global_controller.h:898
mc_control::MCGlobalController::GlobalConfiguration::global_plugins_autoload
std::vector< std::string > global_plugins_autoload
Definition: mc_global_controller.h:907
mc_control::MCGlobalController::GlobalConfiguration::global_plugin_paths
std::vector< std::string > global_plugin_paths
Definition: mc_global_controller.h:905
mc_control::MCGlobalController::GlobalConfiguration::observer_module_paths
std::vector< std::string > observer_module_paths
Definition: mc_global_controller.h:903
MCController.h
mc_control::MCGlobalController::GlobalConfiguration::robot_module_paths
std::vector< std::string > robot_module_paths
Definition: mc_global_controller.h:900
std
Definition: Contact.h:66
mc_rtc::Logger::Policy
Policy
Defines available policies for the logger.
Definition: Logger.h:42
mc_control::MCGlobalController::GlobalConfiguration::global_plugins
std::vector< std::string > global_plugins
Definition: mc_global_controller.h:906
mc_control::MCGlobalController::GlobalConfiguration::gui_server_configuration
ControllerServerConfiguration gui_server_configuration
Definition: mc_global_controller.h:923
mc_control::ControllerServerConfiguration
Definition: ControllerServerConfiguration.h:40
mc_rtc::Logger::Policy::NON_THREADED
@ NON_THREADED
Non-threaded policy.
loader.h
ltdl wrapper for mc_rtc purpose
mc_control::MCGlobalController::controller
MCController & controller() noexcept
Access the current controller.
Definition: mc_global_controller.h:680
mc_control
Definition: CompletionCriteria.h:10
mc_control::MCGlobalController::GlobalConfiguration::enabled_controllers
std::vector< std::string > enabled_controllers
Definition: mc_global_controller.h:911
mc_control::MCController
MCController is the base class to implement all controllers. It assumes that at least two robots are ...
Definition: MCController.h:98
mc_control::MCGlobalController::GlobalConfiguration::global_plugin_configs
std::unordered_map< std::string, mc_rtc::Configuration > global_plugin_configs
Definition: mc_global_controller.h:908
mc_control::MCGlobalController::QuaternionMapAllocator
Eigen::aligned_allocator< std::pair< const std::string, Eigen::Quaterniond > > QuaternionMapAllocator
Definition: mc_global_controller.h:27
mc_control::MCGlobalController::QuaternionMap
std::map< std::string, Eigen::Quaterniond, std::less< std::string >, QuaternionMapAllocator > QuaternionMap
Definition: mc_global_controller.h:28