Fix usage of FootSteps_Planner

jrl-umi3218 · Jan 31, 2024 · cc7bbca · cc7bbca
1 parent ed1fee6
commit cc7bbca
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 3 deletions.
diff --git a/plugins/ExternalFootstepPlanner/src/HybridPlanner.cpp b/plugins/ExternalFootstepPlanner/src/HybridPlanner.cpp
@@ -24,7 +24,7 @@ void HybridPlanner::configure(const mc_rtc::Configuration & config)
   auto conf = config("HybridPlanner", mc_rtc::Configuration{});
   conf.load(ctl_.config()("HybridPlanner"));
   mc_rtc::log::info("[HybridPlanner] Requesting configuration: {}", conf.dump(true));
-  ctl_.datastore().assign<mc_rtc::Configuration>("footsteps_planner::planner_config", conf);
+  ctl_.datastore().call<void, const mc_rtc::Configuration &>("footsteps_planner::configure", conf);
   Tp_ = conf("Tp");
   delta_ = conf("delta");
 }

diff --git a/src/states/Initial.cpp b/src/states/Initial.cpp
@@ -137,9 +137,12 @@ void states::Initial::internalReset()
   {
     auto X_0_fb = ctl.supportContact().robotTransform(ctl.controlRobot());
     ctl.controlRobot().posW(X_0_fb);
-    ctl.controlRobot().velW(sva::MotionVecd::Zero());
     ctl.realRobot().posW(X_0_fb);
-    ctl.realRobot().velW(sva::MotionVecd::Zero());
+    if(ctl.controlRobot().mb().joint(0).dof() == 6)
+    {
+      ctl.controlRobot().velW(sva::MotionVecd::Zero());
+      ctl.realRobot().velW(sva::MotionVecd::Zero());
+    }
   }
 
   // (2) update contact frames to coincide with surface ones