diff --git a/src/solver-op-space.cpp b/src/solver-op-space.cpp index b96f37a37d104568184b2b88656fb315d6aa857b..afdf26831e9e1065eee7df95bcfe4c79c168e4e4 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;