From 5648649890487e41d0558e7e11df3bf74fd62c52 Mon Sep 17 00:00:00 2001
From: Francois Keith <keith@lirmm.fr>
Date: Sun, 20 Apr 2014 12:54:05 +0200
Subject: [PATCH] Use waist rather than root to create the chain.
---
.../ComAndFootRealizationByGeometry.cpp | 11 ++++++++---
1 file changed, 8 insertions(+), 3 deletions(-)
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
index 343e958b..4436b500 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
@@ -149,7 +149,8 @@ InitializeMapsForAHand(CjrlHand * aHand,
std::vector<CjrlJoint *> FromRootToJoint2,FromRootToJoint;
FromRootToJoint.clear();
FromRootToJoint2.clear();
- FromRootToJoint = associatedWrist->jointsFromRootToThis();
+ CjrlJoint *waist = getHumanoidDynamicRobot()->waist();
+ FromRootToJoint = getHumanoidDynamicRobot()->jointsBetween(*waist, *associatedWrist);
std::vector<CjrlJoint *>::iterator itJoint = FromRootToJoint.begin();
bool startadding=false;
@@ -254,11 +255,13 @@ Initialization()
// to the VRML ID.
ODEBUG("Enter 5.0 ");
// Extract the indexes of the Right leg.
+ CjrlJoint *waist = getHumanoidDynamicRobot()->waist();
if (RightFoot->associatedAnkle()==0)
LTHROW("No right ankle");
+
std::vector<CjrlJoint *> FromRootToJoint2,FromRootToJoint =
- RightFoot->associatedAnkle()->jointsFromRootToThis();
+ getHumanoidDynamicRobot()->jointsBetween(*waist, *(RightFoot->associatedAnkle()));
std::vector<CjrlJoint *> ActuatedJoints =
getHumanoidDynamicRobot()->getActuatedJoints();
ODEBUG4("Size of ActuatedJoints"<<ActuatedJoints.size(),"DebugDataStartingCOM.dat");
@@ -276,7 +279,9 @@ Initialization()
InitializationMaps(FromRootToJoint,ActuatedJoints,
m_RightLegIndexinConfiguration);
FromRootToJoint.clear();
- FromRootToJoint = LeftFoot->associatedAnkle()->jointsFromRootToThis();
+ FromRootToJoint =
+ getHumanoidDynamicRobot()->jointsBetween(*waist, *(LeftFoot->associatedAnkle()));
+
FromRootToJoint.erase(FromRootToJoint.begin());
InitializationMaps(FromRootToJoint,ActuatedJoints,
m_LeftLegIndexinConfiguration);
--
GitLab