From 20f560a32f9d1bc38427321ded8cb9c313cd2bb6 Mon Sep 17 00:00:00 2001
From: pFernbach <pierre.fernbach@gmail.com>
Date: Mon, 18 Mar 2019 10:47:24 +0100
Subject: [PATCH] [centroidal] in geometric init guess : take com height found
during contact planning
---
scripts/hpp_wholebody_motion/centroidal/geometric.py | 4 +++-
1 file changed, 3 insertions(+), 1 deletion(-)
diff --git a/scripts/hpp_wholebody_motion/centroidal/geometric.py b/scripts/hpp_wholebody_motion/centroidal/geometric.py
index 1f14cc6..1702d95 100644
--- a/scripts/hpp_wholebody_motion/centroidal/geometric.py
+++ b/scripts/hpp_wholebody_motion/centroidal/geometric.py
@@ -28,7 +28,9 @@ def generateCentroidalTrajectory(cs_origin):
com_x += phase.RH_patch.placement.translation[0,0]
com_y += phase.RH_patch.placement.translation[1,0]
com_x /= phase.numActivePatches()
- com_y /= phase.numActivePatches()
+ com_y /= phase.numActivePatches()
+ # test : take com height from config found from planning :
+ com_z = cs.contact_phases[pid].init_state[2,0]
state[0] = com_x
state[1] = com_y
state[2] = com_z
--
GitLab