From a56d01bf344982a338524a441ca6d809607dca8b Mon Sep 17 00:00:00 2001
From: florent <florent@laas.fr>
Date: Tue, 4 Jan 2011 17:03:44 +0100
Subject: [PATCH] Fix computation of WaistWorldPositionSOUT.
* src/angle-estimator.cpp: previous implementation did not take into account
contactWorldPositionSIN and returned position of the waist in contact
joint frame.
New implementation returns position of the waist in the global frame.
---
src/angle-estimator.cpp | 8 +++++---
1 file changed, 5 insertions(+), 3 deletions(-)
diff --git a/src/angle-estimator.cpp b/src/angle-estimator.cpp
index 1ab34c4..05df2e0 100644
--- a/src/angle-estimator.cpp
+++ b/src/angle-estimator.cpp
@@ -289,18 +289,20 @@ computeWaistWorldPosition( MatrixHomogeneous& res,
sotDEBUGIN(15);
const MatrixHomogeneous & waistMleg = contactEmbeddedPositionSIN( time );
+ const MatrixHomogeneous& contactPos = contactWorldPositionSIN( time );
MatrixHomogeneous legMwaist; waistMleg.inverse( legMwaist );
-
+ MatrixHomogeneous tmpRes;
if( fromSensor_ )
{
const MatrixRotation & Rflex = flexibilitySOUT( time ); // footRleg
ml::Vector zero(3); zero.fill(0.);
MatrixHomogeneous footMleg; footMleg.buildFrom( Rflex,zero );
- footMleg.multiply( legMwaist,res );
+ footMleg.multiply( legMwaist,tmpRes );
}
- else { res = legMwaist; }
+ else { tmpRes = legMwaist; }
+ contactPos.multiply( tmpRes, res );
sotDEBUGOUT(15);
return res;
}
--
GitLab