diff --git a/src/angle-estimator.cpp b/src/angle-estimator.cpp
index 1ab34c4a9e9201595fbe0ca2f7c16df949c3f4bb..05df2e011e6e9e37a72e5e8d448df00a0bf81954 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;
 }