[config] Allow inheriting from another robot's plans

Since yaml-cpp does not support, nor intend to support YAML merge
anchors, this manually adds this ability to inherit from another robot's
plans configuration to avoid duplication

See jbeder/yaml-cpp#300

Author: arntanguy

etc/LIPMWalking.in.yaml

@@ -371,7 +371,7 @@ plans:
         - pose:
             translation: [0.035, 0.09, 0.0]
           surface: LeftFootCenter
-  hrp4j: &hrp4j
+  hrp4j:
     ashibumi: # stepping in place
       double_support_duration: 0.5
       single_support_duration: 0.8
@@ -476,7 +476,16 @@ plans:
         - pose:
             translation: [0.035, 0.07, 0.0]
           surface: LeftFootCenter
-  hrp4cr: *hrp4j
+  hrp4cr:
+    base: hrp4j
+    warmup:
+      com_height: 0.78
+    ashibumi:
+      com_height: 0.78
+    custom_forward:
+      com_height: 0.78
+    custom_backward:
+      com_height: 0.78
   hrp2_drc:
     ashibumi: # stepping in place
       double_support_duration: 0.2

src/Controller.cpp

@@ -36,13 +36,60 @@ using Color = mc_rtc::gui::Color;
 namespace lipm_walking
 {
 
+/**
+ * @brief Recursively merge plan configurations
+ * Allows for
+ * plans:
+ *   hrp4:
+ *    com_height: 0.75
+ *    contacts:
+ *    - ...
+ *   hrp4j:
+ *    base: hrp4
+ *    com_height: 0.78
+ *
+ *
+ * @param plans Configuration object containing all the plans
+ * @param robot Robot for which the plan should be merged
+ *
+ * @return Merged plan for the specified robot
+ */
+mc_rtc::Configuration mergePlanConfigs(const mc_rtc::Configuration & plans, const std::string & robot)
+{
+  if(plans(robot).has("base"))
+  {
+    std::string base = plans(robot)("base");
+    if(base == robot)
+    {
+      mc_rtc::log::error_and_throw<std::runtime_error>(
+          "[LIPMWalking] Plan \"{}\" cannot inherit from itself (base: \"{}\")", robot, base);
+    }
+    else if(!plans.has(base))
+    {
+      mc_rtc::log::error_and_throw<std::runtime_error>("[LIPMWalking] Plan \"{}\" has no base \"{}\"", robot, base);
+    }
+    else
+    {
+      mc_rtc::Configuration robotConf = plans(robot);
+      robotConf.remove("base");
+      auto res = mergePlanConfigs(plans, base);
+      res.load(robotConf);
+      return res;
+    }
+  }
+  else
+  {
+    return plans(robot);
+  }
+}
+
 Controller::Controller(std::shared_ptr<mc_rbdyn::RobotModule> robotModule,
                        double dt,
                        const mc_rtc::Configuration & config)
 : mc_control::fsm::Controller(robotModule, dt, config), planInterpolator(gui()), halfSitPose(controlRobot().mbc().q)
 {
   auto robotConfig = config("robot_models")(controlRobot().name());
-  auto planConfig = config("plans")(controlRobot().name());
+  auto planConfig = mergePlanConfigs(config("plans"), controlRobot().name());
 
   mc_rtc::log::info("Loading default stabilizer configuration");
   defaultStabilizerConfig_ = robot().module().defaultLIPMStabilizerConfiguration();