[Collision] Improve loading/saving consistency

arntanguy · Jan 12, 2024 · 61cffbe · 61cffbe
1 parent 9ca231a
commit 61cffbe
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 13 deletions.
diff --git a/doc/_data/schemas/mc_rbdyn/Collision.json b/doc/_data/schemas/mc_rbdyn/Collision.json
@@ -5,10 +5,10 @@
   {
     "body1": { "type": "string", "description": "First convex body used, wildcards can be used" },
     "body2": { "type": "string", "description": "Second convex body used, wildcards can be used" },
-    "r1Joints": { "type": "array", "items": { "type": "string" }, "description": "Active joints used to avoid the collision (default: none, unspecified: all)" },
-    "r2Joints": { "type": "array", "items": { "type": "string" }, "description": "Active joints used to avoid the collision (default: none, unspecified: all)" },
-    "r1InactiveJoints": { "type": "array", "items": { "type": "string" }, "description": "Joints not used to avoid the collision (default: none). Has no effect if r1Joints isn't empty." },
-    "r2InactiveJoints": { "type": "array", "items": { "type": "string" }, "description": "Joints not used to avoid the collision (default: none). Has no effect if r2Joints isn't empty." },
+    "r1ActiveJoints": { "type": "array", "items": { "type": "string" }, "description": "Active joints used to avoid the collision (default: none, unspecified: all)" },
+    "r2ActiveJoints": { "type": "array", "items": { "type": "string" }, "description": "Active joints used to avoid the collision (default: none, unspecified: all)" },
+    "r1InactiveJoints": { "type": "array", "items": { "type": "string" }, "description": "Joints not used to avoid the collision (default: none). Has no effect if r1ActiveJoints is specified." },
+    "r2InactiveJoints": { "type": "array", "items": { "type": "string" }, "description": "Joints not used to avoid the collision (default: none). Has no effect if r2ActiveJoints is specified." },
     "iDist": { "type": "number", "default": 0.05, "description": "Interaction distance" },
     "sDist": { "type": "number", "default": 0.01, "description": "Safety distance" },
     "damping": { "type": "number", "default": 0.0, "description": "Damping, 0 enables automatic computation" }

diff --git a/src/mc_rbdyn/configuration_io.cpp b/src/mc_rbdyn/configuration_io.cpp
@@ -154,21 +154,28 @@ mc_rbdyn::Collision ConfigurationLoader<mc_rbdyn::Collision>::load(const mc_rtc:
   auto body2 = config("body2");
   auto loadActiveJoints = [&](std::string prefix) -> std::tuple<std::optional<std::vector<std::string>>, bool>
   {
-    if(auto cfg = config.find(prefix + "Joints")) { return {cfg->operator std::vector<std::string>(), false}; }
-    if(auto cfg = config.find(prefix + "ActiveJoints"))
+    if((config.has(prefix + "ActiveJoints") || config.has(prefix + "Joints")) && config.has(prefix + "InactiveJoints"))
     {
-      mc_rtc::log::deprecated(fmt::format("Collision ({} - {})", body1, body2), prefix + "ActiveJoints",
-                              prefix + "Joints");
+      mc_rtc::log::warning(
+          "Collision ({} - {}) has both {}ActiveJoints and {}InactiveJoints, {}ActiveJoints will be used", body1, body2,
+          prefix);
+    }
+
+    if(auto cfg = config.find(prefix + "Joints"))
+    {
+      mc_rtc::log::deprecated(fmt::format("Collision ({} - {})", body1, body2), prefix + "Joints",
+                              prefix + "ActiveJoints");
       auto joints = cfg->operator std::vector<std::string>();
       if(joints.empty())
       {
         mc_rtc::log::warning(
             "[Collision][breaking change] The meaning of an empty joint vector has changed from all joints active to "
-            "no joints active. Remove {}ActiveJoints from your configuration to restore the former behaviour.",
+            "no joints active. Remove {}Joints from your configuration to restore the former behaviour.",
             prefix);
       }
       return {cfg->operator std::vector<std::string>(), false};
     }
+    if(auto cfg = config.find(prefix + "ActiveJoints")) { return {cfg->operator std::vector<std::string>(), false}; }
     if(auto cfg = config.find(prefix + "InactiveJoints")) { return {cfg->operator std::vector<std::string>(), true}; }
     return {std::nullopt, false};
   };
@@ -186,10 +193,16 @@ mc_rtc::Configuration ConfigurationLoader<mc_rbdyn::Collision>::save(const mc_rb
   config.add("iDist", c.iDist);
   config.add("sDist", c.sDist);
   config.add("damping", c.damping);
-  if(c.r1Joints) { config.add("r1Joints", *c.r1Joints); }
-  if(c.r2Joints) { config.add("r2Joints", *c.r2Joints); }
-  config.add("r1JointsInactive", c.r1JointsInactive);
-  config.add("r2JointsInactive", c.r2JointsInactive);
+  auto saveActiveJoints = [&](std::string prefix, const std::optional<std::vector<std::string>> & joints, bool inactive)
+  {
+    if(joints)
+    {
+      if(inactive) { config.add(prefix + "InactiveJoints", *c.r1Joints); }
+      else { config.add(prefix + "ActiveJoints", *c.r1Joints); }
+    }
+  };
+  saveActiveJoints("r1", c.r1Joints, c.r1JointsInactive);
+  saveActiveJoints("r2", c.r2Joints, c.r2JointsInactive);
   return config;
 }