From cc7bbca5031f08119b7eaad66ab04b971ffa0021 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Wed, 31 Jan 2024 18:09:38 +0900 Subject: [PATCH] Fix usage of FootSteps_Planner --- plugins/ExternalFootstepPlanner/src/HybridPlanner.cpp | 2 +- src/states/Initial.cpp | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/plugins/ExternalFootstepPlanner/src/HybridPlanner.cpp b/plugins/ExternalFootstepPlanner/src/HybridPlanner.cpp index 1dfe595f..761e2bc4 100644 --- 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("footsteps_planner::planner_config", conf); + ctl_.datastore().call("footsteps_planner::configure", conf); Tp_ = conf("Tp"); delta_ = conf("delta"); } diff --git a/src/states/Initial.cpp b/src/states/Initial.cpp index 13adbf9e..da1da761 100644 --- 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