From 0d67b9a55c63325aa7d79d84e0f5fcc86613c47f Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Tue, 30 Aug 2011 19:01:51 +0200
Subject: [PATCH] Corrected the bug in the altitude of the foot.
---
src/solver-op-space.cpp | 4 +++-
1 file changed, 3 insertions(+), 1 deletion(-)
diff --git a/src/solver-op-space.cpp b/src/solver-op-space.cpp
index b96f37a..afdf268 100644
--- a/src/solver-op-space.cpp
+++ b/src/solver-op-space.cpp
@@ -304,7 +304,9 @@ namespace dynamicgraph
/* Ci = [ X_c^0_{z,Rx,Ry} -Psi_{z,Rx,Ry}];
* with X_c^0_{z,Rx,Ry} = [ 0 0 1 0 0 0; 0 -z 0 1 0 0; z 0 0 0 1 0 ]
* and Psi_{z,Rx,Ry} = [ 1 1 1 1; y_1 y_2 y_3 y_4; -x_1 -x_2 -x_3 -x_4 ]. */
- const double z = positions(2,0);
+ //const double z = positions(2,0);
+ /* position(3) is the position of the ground wrt the ankle. We need the opposite. */
+ const double z = -positions(2,0); // DEBUG TRIAL
Ci.setZero();
Ci(0,2)=1;
Ci(1,1)=-z; Ci(1,3)=1;
--
GitLab