Merge pull request #423 from antodld/topic/RobotFromControllerYAML

[MCControler] Update main robot name

Author: gergondet

.pre-commit-config.yaml

@@ -41,7 +41,8 @@ repos:
           src/mc_control/samples/ExternalForces/etc/ExternalForces.in.yaml|
           src/mc_control/samples/LIPMStabilizer/etc/LIPMStabilizer.in.yaml|
           src/mc_control/fsm/states/data/StabilizerStanding.yaml|
-          tests/controllers/TestPythonState.in.yaml
+          tests/controllers/TestPythonState.in.yaml|
+          tests/global_controller_configuration/mc_rtc.in.yaml
       )$
   - id: debug-statements
   - id: destroyed-symlinks

doc/_i18n/en.yml

@@ -178,3 +178,4 @@ tutorials:
       title: Supporting MC_RTC_BUILD_STATIC in your code
     debug-lssol-output-6:
       title: Debugging LSSOL output 6
+or: or

doc/_i18n/en/tutorials/introduction/configuration.html

@@ -30,7 +30,13 @@
       </th>
       <td colspan="2">These entries cover most needs you might have</td>
     </tr>
-    {% include mc_rtc_configuration_row.html entry="MainRobot" desc="This entry dictates the main robot used by all controllers. Most interface cannot infer the correct module to use based on the simulation environment so this is the user's responsibility to make sure the two match." example="MainRobot: JVRC1" %}
+{% capture MainRobot_example2 %}
+# Support renaming the robot
+MainRobot:
+  name: MyRobot
+  module: JVRC1
+{% endcapture %}
+    {% include mc_rtc_configuration_row.html entry="MainRobot" desc="This entry dictates the main robot used by all controllers. Most interface cannot infer the correct module to use based on the simulation environment so this is the user's responsibility to make sure the two match." example="MainRobot: JVRC1" example2=MainRobot_example2%}
     <tr>
       <th scope="row">Enabled</th>
       <td>Provides a list of enabled controllers. See the <a href="{{site.baseurl}}/tutorials/samples/list-of-samples.html">list of all available sample controllers</a>.</td>

doc/_i18n/jp.yml

@@ -179,3 +179,4 @@ tutorials:
       title: MC_RTC_BUILD_STATICをプログラムでサポートする方法
     debug-lssol-output-6:
       title: LSSOL output 6のデバッグ
+or: または

doc/_i18n/jp/tutorials/introduction/configuration.html

@@ -30,7 +30,13 @@
       </th>
       <td colspan="2">このエントリは、通常考えられるほとんどのニーズをカバーします。</td>
     </tr>
-    {% include mc_rtc_configuration_row.html entry="MainRobot" desc="このエントリは、すべてのコントローラーで使用されるメインロボットを指定します。ほとんどのインターフェイスは、どのモジュールを使用すべきかをシミュレーション環境に基づいて推測することはできません。そのため、シミュレーション環境に応じてユーザーがモジュールを指定する必要があります。" example="MainRobot: JVRC1" %}
+{% capture MainRobot_example2 %}
+# ロボット名の変更をサポート
+MainRobot:
+  name: MyRobot
+  module: JVRC1
+{% endcapture %}
+    {% include mc_rtc_configuration_row.html entry="MainRobot" desc="このエントリは、すべてのコントローラーで使用されるメインロボットを指定します。ほとんどのインターフェイスは、どのモジュールを使用すべきかをシミュレーション環境に基づいて推測することはできません。そのため、シミュレーション環境に応じてユーザーがモジュールを指定する必要があります。" example="MainRobot: JVRC1" example2=MainRobot_example2 %}
     <tr>
       <th scope="row">Enabled</th>
       <td>有効なコントローラーのリストを提供します。<a href="{{site.baseurl}}/tutorials/samples/list-of-samples.html">利用可能なサンプルコントローラーのリスト</a>を参照してください。</a>.</td>

doc/_includes/mc_rtc_configuration_row.html

@@ -1,5 +1,11 @@
 <tr>
   <th scope="row">{{include.entry}}</th>
   <td>{{include.desc}}</td>
-  <td>{% highlight yaml %}{{include.example}}{% endhighlight %}</td>
+  <td>
+    {% highlight yaml %}{{include.example}}{% endhighlight %}
+    {% if include.example2 %}
+      <p>{% t or %}</p>
+      {% highlight yaml %}{{include.example2}}{% endhighlight %}
+    {% endif %}
+  </td>
 </tr>

include/mc_control/mc_global_controller.h

@@ -15,9 +15,6 @@
 #include <mc_rtc/log/Logger.h>
 
 #include <array>
-#include <fstream>
-#include <sstream>
-#include <thread>
 
 namespace mc_control
 {
@@ -878,7 +875,6 @@ struct MC_CONTROL_DLLAPI MCGlobalController
    */
   bool running = false;
 
-public:
   /*! \brief Store the controller configuration */
   struct MC_CONTROL_DLLAPI GlobalConfiguration
   {
@@ -887,29 +883,33 @@ struct MC_CONTROL_DLLAPI MCGlobalController
      * \param conf Configuration file that should be loaded
      *
      * \param Main robot module, if null use the MainRobot entry in conf to initialize it
+     *
+     * \param conf_only If true, only load the specified configuration file
      */
-    GlobalConfiguration(const std::string & conf, std::shared_ptr<mc_rbdyn::RobotModule> rm = nullptr);
+    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::string init_attitude_sensor;
 
-    std::vector<std::string> robot_module_paths = {};
+    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> observer_module_paths;
 
-    std::vector<std::string> global_plugin_paths = {};
-    std::vector<std::string> global_plugins = {};
-    std::vector<std::string> global_plugins_autoload = {};
+    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::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;
@@ -921,8 +921,8 @@ struct MC_CONTROL_DLLAPI MCGlobalController
 
     bool enable_gui_server = true;
     double gui_timestep = 0.05;
-    std::vector<std::string> gui_server_pub_uris{};
-    std::vector<std::string> gui_server_rep_uris{};
+    std::vector<std::string> gui_server_pub_uris;
+    std::vector<std::string> gui_server_rep_uris;
 
     Configuration config;
 
@@ -936,16 +936,16 @@ struct MC_CONTROL_DLLAPI MCGlobalController
 private:
   using duration_ms = std::chrono::duration<double, std::milli>;
   GlobalConfiguration config;
-  std::string current_ctrl = "";
-  std::string next_ctrl = "";
+  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_ = nullptr;
+  std::unique_ptr<mc_control::ControllerServer> server_;
 
   std::unique_ptr<mc_rtc::ObjectLoader<GlobalPlugin>> plugin_loader_;
   struct PluginHandle
@@ -981,7 +981,7 @@ struct MC_CONTROL_DLLAPI MCGlobalController
   void start_log();
   void setup_log();
   void setup_plugin_log();
-  std::map<std::string, bool> setup_logger_ = {};
+  std::map<std::string, bool> setup_logger_;
 
   /** Timers and performance measure */
   duration_ms global_run_dt{0};

include/mc_rbdyn/RobotLoader.h

@@ -132,6 +132,7 @@ struct MC_RBDYN_DLLAPI RobotLoader
     std::lock_guard<std::mutex> guard{mtx};
     init(true);
     robot_loader->clear();
+    aliases.clear();
   }
 
   /** Check if a robot is available

src/mc_control/MCController.cpp

@@ -115,7 +115,7 @@ static inline std::shared_ptr<mc_solver::QPSolver> make_solver(double dt, MCCont
   }
 }
 
-MCController::MCController(const std::vector<std::shared_ptr<mc_rbdyn::RobotModule>> & robots_modules,
+MCController::MCController(const std::vector<std::shared_ptr<mc_rbdyn::RobotModule>> & robot_modules,
                            double dt,
                            const mc_rtc::Configuration & config,
                            ControllerParameters params)
@@ -129,7 +129,14 @@ MCController::MCController(const std::vector<std::shared_ptr<mc_rbdyn::RobotModu
   qpsolver->logger(logger_);
   qpsolver->gui(gui_);
   qpsolver->controller(this);
-  for(auto rm : robots_modules) { loadRobot(rm, rm->name); }
+
+  std::string main_robot_name = config.find<std::string>("MainRobot", "name").value_or(robot_modules[0]->name);
+  loadRobot(robot_modules[0], main_robot_name);
+  for(auto it = std::next(robot_modules.cbegin()); it != robot_modules.end(); ++it)
+  {
+    const auto & rm = *it;
+    loadRobot(rm, rm->name);
+  }
   /* Load robot-specific configuration depending on parameters */
   auto load_robot_config_into = config;
   if(params.load_robot_config_)
@@ -192,7 +199,7 @@ MCController::MCController(const std::vector<std::shared_ptr<mc_rbdyn::RobotModu
   dynamicsConstraint.reset(new mc_solver::DynamicsConstraint(robots(), 0, dt, damper, 0.5));
   kinematicsConstraint.reset(new mc_solver::KinematicsConstraint(robots(), 0, dt, damper, 0.5));
   selfCollisionConstraint.reset(new mc_solver::CollisionsConstraint(robots(), 0, 0, dt));
-  selfCollisionConstraint->addCollisions(solver(), robots_modules[0]->minimalSelfCollisions());
+  selfCollisionConstraint->addCollisions(solver(), robot_modules[0]->minimalSelfCollisions());
   compoundJointConstraint.reset(new mc_solver::CompoundJointConstraint(robots(), 0, timeStep));
   postureTask = std::make_shared<mc_tasks::PostureTask>(solver(), 0, 10.0, 5.0);
   /** Load additional robots from the configuration */
@@ -247,19 +254,15 @@ MCController::MCController(const std::vector<std::shared_ptr<mc_rbdyn::RobotModu
       }
       else
       {
-        std::string module = rconfig("module");
-        auto params = rconfig("params", std::vector<std::string>{});
-        mc_rbdyn::RobotModulePtr rm = nullptr;
-        if(params.size() == 0) { rm = mc_rbdyn::RobotLoader::get_robot_module(module); }
-        else if(params.size() == 1) { rm = mc_rbdyn::RobotLoader::get_robot_module(module, params.at(0)); }
-        else if(params.size() == 2)
+        auto params = [&]() -> std::vector<std::string>
         {
-          rm = mc_rbdyn::RobotLoader::get_robot_module(module, params.at(0), params.at(1));
-        }
-        else
-        {
-          mc_rtc::log::error_and_throw("Controller only handles robot modules that require two parameters at most");
-        }
+          auto module = rconfig("module");
+          if(module.isArray()) { return module.operator std::vector<std::string>(); }
+          std::vector<std::string> params = rconfig("params", std::vector<std::string>{});
+          params.insert(params.begin(), module.operator std::string());
+          return params;
+        }();
+        auto rm = mc_rbdyn::RobotLoader::get_robot_module(params);
         if(!rm) { mc_rtc::log::error_and_throw("Failed to load {} as specified in configuration", rname); }
         auto & robot = loadRobot(rm, rname);
         load_robot_config(robot);

src/mc_control/Ticker.cpp

@@ -4,6 +4,8 @@
 
 #include <mc_control/Ticker.h>
 
+#include <thread>
+
 #include <boost/filesystem.hpp>
 namespace bfs = boost::filesystem;