From 0eec54616408c8d63686c8c396f5ccd87f811381 Mon Sep 17 00:00:00 2001
From: Mehdi Benallegue <mehdi.benallegue@gmail.com>
Date: Tue, 3 Jun 2014 18:29:39 +0200
Subject: [PATCH] Correct computations of the X component of the ZMP
Correct also the linear force part of the Y component of the ZMP.
---
src/zmp-from-forces.cpp | 7 +++----
1 file changed, 3 insertions(+), 4 deletions(-)
diff --git a/src/zmp-from-forces.cpp b/src/zmp-from-forces.cpp
index 81ea9b9..ca1d34d 100644
--- a/src/zmp-from-forces.cpp
+++ b/src/zmp-from-forces.cpp
@@ -97,10 +97,9 @@ namespace sot {
const MatrixHomogeneous& M = sensorPositionsSIN_ [i]->access (time);
fz [i] = M (2,0) * f (0) + M(2,1) * f (1) + M (2,2) * f (2);
if (fz[i] > 0) {
- double Mx = M (0,0)*f(3) + M (0,1)*f(4) + M (0,2)*f(5) +
- M (1,3)*(M(2,0)*f(0)+M(2,1)*f(1)+M(2,2)*f(2));
- double My = M (1,0)*f(3) + M (1,1)*f(4) + M (1,2)*f(5) -
- M (1,3)*(M(1,0)*f(0)+M(1,1)*f(1)+M(1,2)*f(2));
+ /// we assume M (2,3)==0 for moments computations
+ double Mx = M (0,0)*f(3) + M (0,1)*f(4) + M (0,2)*f(5) + M (1,3)*(fz [i]);
+ double My = M (1,0)*f(3) + M (1,1)*f(4) + M (1,2)*f(5) - M (0,3)*(fz [i]);
fnormal += fz[i];
sumZmpx -= My;
sumZmpy += Mx;
--
GitLab