From 23e0a9304c44687c8b6efc5319d035c40a2f1789 Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Thu, 27 Feb 2014 18:38:22 +0100
Subject: [PATCH] Creation d une nouvelle instance de
CoMandFootRealizationByGeometrie pour le calcul du filtre dynamique dans
Herdt.
---
.gitignore | 7 +
src/CMakeLists.txt | 18 +-
.../CoMAndFootOnlyStrategy.cpp | 36 +-
.../CoMAndFootOnlyStrategy.hh | 28 +-
.../ComAndFootRealizationByGeometry.cpp | 167 +++--
.../ComAndFootRealizationByGeometry.hh | 113 +--
src/PatternGeneratorInterfacePrivate.cpp | 32 +-
.../ZMPVelocityReferencedQP.cpp | 704 +++++++++---------
.../ZMPVelocityReferencedQP.hh | 32 +-
src/patterngeneratorinterfaceprivate.hh | 2 +-
tests/CMakeLists.txt | 1 +
tests/TestHerdt2010DynamicFilter.cpp | 19 +-
tests/TestKajita2003.cpp | 18 +-
tests/TestObject.cpp | 53 +-
14 files changed, 697 insertions(+), 533 deletions(-)
diff --git a/.gitignore b/.gitignore
index 4767b4f8..6e0f6858 100644
--- a/.gitignore
+++ b/.gitignore
@@ -30,3 +30,10 @@ build/
+committed/
*~
*.swp
+_build-DEBUG-CB/
+_build-RELEASE-CB/
+_build-RELEASE/
+cmake_metapod_configure_hrp2_14.cmake
+cmake_metapod_configure_simple_humanoid.cmake
+src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp_dirty
+
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index cca88d4c..4d900180 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -89,7 +89,23 @@ SET(SOURCES
privatepgtypes.cpp
)
-ADD_LIBRARY(jrl-walkgen SHARED ${SOURCES})
+# prefix and suffix each element of list by ${prefix}elemnt${suffix}
+macro(ADDPREFIX newlist prefix list_name)
+ # create empty list - necessary?
+ SET(${newlist})
+
+ # prefix and suffix elements
+ foreach(l ${${list_name}})
+ list(APPEND ${newlist} ${prefix}${l} )
+ endforeach()
+
+endmacro(ADDPREFIX)
+
+IF ( '${CMAKE_EXTRA_GENERATOR}' STREQUAL 'CodeBlocks' )
+ ADDPREFIX(${PROJECT_NAME}_ABSOLUTE_HEADERS "${CMAKE_SOURCE_DIR}/" ${PROJECT_NAME}_HEADERS)
+ENDIF ( '${CMAKE_EXTRA_GENERATOR}' STREQUAL 'CodeBlocks' )
+
+ADD_LIBRARY(jrl-walkgen SHARED ${SOURCES} ${${PROJECT_NAME}_ABSOLUTE_HEADERS})
SET_TARGET_PROPERTIES(jrl-walkgen PROPERTIES SOVERSION ${PROJECT_VERSION})
# Define dependencies
diff --git a/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp b/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp
index 34db64aa..7c16adeb 100644
--- a/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp
+++ b/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp
@@ -1,9 +1,9 @@
/*
- * Copyright 2007, 2008, 2009, 2010,
+ * Copyright 2007, 2008, 2009, 2010,
*
* Fumio Kanehiro
* Francois Keith
- * Florent Lamiraux
+ * Florent Lamiraux
* Anthony Mallet
* Olivier Stasse
*
@@ -22,12 +22,12 @@
* You should have received a copy of the GNU Lesser General Public License
* along with jrl-walkgen. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/*! \file CoMAndFootOnlyStrategy.h
- \brief This object defines a global strategy object to generate
+ \brief This object defines a global strategy object to generate
only foot, ZMP reference and CoM trajectories position every 5 ms. */
#include <Debug.hh>
@@ -46,7 +46,7 @@ CoMAndFootOnlyStrategy::~CoMAndFootOnlyStrategy()
}
int CoMAndFootOnlyStrategy::InitInterObjects(CjrlHumanoidDynamicRobot * /* aHDR */,
- ComAndFootRealization * aCFR,
+ std::vector<ComAndFootRealization *> aCFR,
StepStackHandler * /* aSSH */)
{
m_ComAndFootRealization = aCFR;
@@ -61,12 +61,12 @@ int CoMAndFootOnlyStrategy::OneGlobalStepOfControl(FootAbsolutePosition &LeftFoo
MAL_VECTOR_TYPE(double) & ,//CurrentVelocity,
MAL_VECTOR_TYPE(double) & )//CurrentAcceleration)
{
- ODEBUG("Begin OneGlobalStepOfControl "
- << m_LeftFootPositions->size() << " "
+ ODEBUG("Begin OneGlobalStepOfControl "
+ << m_LeftFootPositions->size() << " "
<< m_RightFootPositions->size() << " "
<< m_COMBuffer->size() << " "
<< m_ZMPPositions->size());
-
+
/* The strategy of this class is simply to pull off values from the buffers. */
if (m_LeftFootPositions->size()>0)
{
@@ -89,7 +89,7 @@ int CoMAndFootOnlyStrategy::OneGlobalStepOfControl(FootAbsolutePosition &LeftFoo
ODEBUG3("Problem on the right foot position queue: empty");
return -3;
}
-
+
if (m_COMBuffer->size()>0)
{
finalCOMPosition = (*m_COMBuffer)[0];
@@ -116,7 +116,7 @@ int CoMAndFootOnlyStrategy::OneGlobalStepOfControl(FootAbsolutePosition &LeftFoo
}
ODEBUG("End of OneGlobalStepOfControl"
- << m_LeftFootPositions->size() << " "
+ << m_LeftFootPositions->size() << " "
<< m_RightFootPositions->size() << " "
<< m_COMBuffer->size() << " "
<< m_ZMPPositions->size());
@@ -137,15 +137,19 @@ int CoMAndFootOnlyStrategy::EvaluateStartingState(MAL_VECTOR(&,double) BodyAngle
lStartingCOMState(1) = aStartingCOMState.y[0];
lStartingCOMState(2) = aStartingCOMState.z[0];
- m_ComAndFootRealization->InitializationCoM(BodyAngles,lStartingCOMState,
+ std::vector<ComAndFootRealization *>::iterator itCFR ;
+ for (itCFR = m_ComAndFootRealization.begin() ; itCFR == m_ComAndFootRealization.end() ; ++itCFR )
+ {
+ (*itCFR)->InitializationCoM(BodyAngles,lStartingCOMState,
lStartingWaistPose,
- InitLeftFootPosition, InitRightFootPosition);
+ InitLeftFootPosition, InitRightFootPosition);
+ }
ODEBUG("EvaluateStartingCOM: m_StartingCOMState: " << lStartingCOMState);
aStartingCOMState.x[0] = lStartingCOMState(0);
aStartingCOMState.y[0] = lStartingCOMState(1);
aStartingCOMState.z[0] = lStartingCOMState(2);
- aStartingZMPPosition= m_ComAndFootRealization->GetCOGInitialAnkles();
+ aStartingZMPPosition= (*m_ComAndFootRealization.begin())->GetCOGInitialAnkles();
// cerr << "YOU SHOULD INITIALIZE PROPERLY aStartingZMPosition in CoMAndFootOnlyStrategy::EvaluateStartingState" <<endl;
return 0;
@@ -160,7 +164,7 @@ int CoMAndFootOnlyStrategy::EndOfMotion()
{
if (m_LeftFootPositions->size()==m_BufferSizeLimit+1)
{
- ODEBUG("LeftFootPositions position ( "<< (*m_LeftFootPositions)[0].x
+ ODEBUG("LeftFootPositions position ( "<< (*m_LeftFootPositions)[0].x
<< " , " << (*m_LeftFootPositions)[0].y << " ) " );
}
@@ -170,7 +174,7 @@ int CoMAndFootOnlyStrategy::EndOfMotion()
else if ((m_LeftFootPositions->size()==m_BufferSizeLimit) &&
(m_NbOfHitBottom==0))
{
- ODEBUG("LeftFootPositions size : "<< m_LeftFootPositions->size()
+ ODEBUG("LeftFootPositions size : "<< m_LeftFootPositions->size()
<< "Buffer size limit: " << m_BufferSizeLimit);
m_NbOfHitBottom++;
@@ -191,7 +195,7 @@ void CoMAndFootOnlyStrategy::Setup(deque<ZMPPosition> &, // aZMPPositio
{
}
-void CoMAndFootOnlyStrategy::CallMethod(std::string &,//Method,
+void CoMAndFootOnlyStrategy::CallMethod(std::string &,//Method,
std::istringstream &)// astrm)
{
}
diff --git a/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.hh b/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.hh
index d333447f..594ed9e5 100644
--- a/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.hh
+++ b/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.hh
@@ -1,5 +1,5 @@
/*
- * Copyright 2007, 2008, 2009, 2010,
+ * Copyright 2007, 2008, 2009, 2010,
*
* Olivier Stasse
*
@@ -18,11 +18,11 @@
* You should have received a copy of the GNU Lesser General Public License
* along with jrl-walkgen. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/*! \file CoMAndFootOnlyStrategy.h
- \brief This object defines a global strategy object to generate
+ \brief This object defines a global strategy object to generate
only foot, ZMP reference and CoM trajectories position every 5 ms.
*/
@@ -42,7 +42,7 @@ namespace PatternGeneratorJRL
*/
class CoMAndFootOnlyStrategy: public GlobalStrategyManager
{
-
+
public:
/*! Default constructor. */
@@ -51,7 +51,7 @@ namespace PatternGeneratorJRL
/*! Default destructor. */
~CoMAndFootOnlyStrategy();
- /*! \name Reimplement the interface inherited from Global Strategy Manager
+ /*! \name Reimplement the interface inherited from Global Strategy Manager
@{
*/
@@ -60,7 +60,7 @@ namespace PatternGeneratorJRL
@param[out] LeftFootPosition: The position of the Left Foot position.
@param[out] RightFootPosition: The position of the Right Foot position.
- @param[out] ZMPRefPos: The ZMP position to be feed to the controller, in the waist
+ @param[out] ZMPRefPos: The ZMP position to be feed to the controller, in the waist
frame reference.
@param[out] finalCOMState: returns CoM state position, velocity and acceleration.
@param[out] CurrentConfiguration: The results is a state vector containing the articular positions.
@@ -74,9 +74,9 @@ namespace PatternGeneratorJRL
MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration);
-
-
+
+
/*! Computes the COM of the robot with the Joint values given in BodyAngles,
velocities set to zero, and returns the values of the COM in aStaringCOMState.
Assuming that the waist is at (0,0,0)
@@ -103,7 +103,7 @@ namespace PatternGeneratorJRL
/*! This method returns :
- \li -1 if there is no more motion to realize
+ \li -1 if there is no more motion to realize
\li 0 if a new step is needed,
\li 1 if there is still enough steps inside the internal stack.
*/
@@ -112,17 +112,17 @@ namespace PatternGeneratorJRL
/*! @} */
/*! Methods related to the end of the motion.
- @{
+ @{
*/
/* Fix the end of the buffer to be tested. */
void SetTheLimitOfTheBuffer(unsigned int lBufferSizeLimit);
-
+
/*! @} */
/*! Reimplement the Call method for SimplePlugin part */
void CallMethod(std::string &Method, std::istringstream &astrm);
-
+
/*! */
void Setup(deque<ZMPPosition> & aZMPPositions,
deque<COMState> & aCOMBuffer,
@@ -131,7 +131,7 @@ namespace PatternGeneratorJRL
/*! \brief Initialization of the inter objects relationship. */
int InitInterObjects(CjrlHumanoidDynamicRobot * aHDR,
- ComAndFootRealization * aCFR,
+ std::vector<ComAndFootRealization *> aCFR,
StepStackHandler * aSSH);
protected:
@@ -140,7 +140,7 @@ namespace PatternGeneratorJRL
/*! Keeps a link towards an object allowing to find a pose for a given CoM and
foot position. */
- ComAndFootRealization *m_ComAndFootRealization;
+ std::vector<ComAndFootRealization *>m_ComAndFootRealization;
/*! Set the position of the buffer size limit. */
unsigned m_BufferSizeLimit;
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
index b482409d..1dea09af 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
+ * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
*
* Olivier Stasse
*
@@ -18,7 +18,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
#include <iostream>
@@ -48,7 +48,7 @@ ComAndFootRealizationByGeometry(PatternGeneratorInterfacePrivate *aPGI)
m_ZARM = -1.0;
m_LeftShoulder = 0;
m_RightShoulder = 0;
-
+
RegisterMethods();
for(unsigned int i=0;i<3;i++)
@@ -117,9 +117,9 @@ InitializationMaps(std::vector<CjrlJoint *> &FromRootToJoint,
IndexInVRML.resize(FromRootToJoint.size());
IndexinConfiguration.resize(FromRootToJoint.size());
- ODEBUG("Enter 6.1 " );
+ ODEBUG("Enter 6.1 " );
int lindex =0;
-
+
// Here we assume that they are in a decending order.
for(unsigned int i=0;i<FromRootToJoint.size();i++)
{
@@ -131,7 +131,7 @@ InitializationMaps(std::vector<CjrlJoint *> &FromRootToJoint,
break;
}
}
-
+
IndexinConfiguration[lindex] = FromRootToJoint[i]->rankInConfiguration();
lindex++;
}
@@ -153,7 +153,7 @@ InitializeMapsForAHand(CjrlHand * aHand,
CjrlJoint *Chest = getHumanoidDynamicRobot()->chest();
if (Chest==0)
return;
-
+
const CjrlJoint * associatedWrist = aHand->associatedWrist();
if (associatedWrist==0)
return;
@@ -177,7 +177,7 @@ InitializeMapsForAHand(CjrlHand * aHand,
FromRootToJoint2.push_back(*itJoint);
}
itJoint++;
-
+
}
associateShoulder = FromRootToJoint2[0];
InitializationMaps(FromRootToJoint2,ActuatedJoints,
@@ -199,11 +199,11 @@ InitializeMapForChest(std::vector<CjrlJoint *> &ActuatedJoints)
while(itJoint!=FromRootToJoint.end())
{
std::vector<CjrlJoint *>::iterator current = itJoint;
-
+
if (*current==Chest)
{
startadding=true;
-
+
}
else
{
@@ -228,7 +228,7 @@ Initialization()
m_UpBody = new UpperBodyMotion();
ODEBUG("Enter 1.0 ");
-
+
ODEBUG("Enter 2.0 ");
// Take the right ankle position (should be equivalent)
vector3d lAnklePositionRight,lAnklePositionLeft;
@@ -240,7 +240,7 @@ Initialization()
RightFoot = getHumanoidDynamicRobot()->rightFoot();
if (RightFoot==0)
LTHROW("No right foot");
-
+
RightFoot->getAnklePositionInLocalFrame(lAnklePositionRight);
LeftFoot->getAnklePositionInLocalFrame(lAnklePositionLeft);
@@ -254,7 +254,7 @@ Initialization()
m_AnklePositionLeft[1] = lAnklePositionLeft[1];
m_AnklePositionRight[2] = lAnklePositionRight[2];
m_AnklePositionLeft[2] = lAnklePositionRight[2];
-
+
ODEBUG4("m_AnklePositionRight: "<< m_AnklePositionRight[0] << " " <<
m_AnklePositionRight[1] << " " <<
m_AnklePositionRight[2],"DebugDataStartingCOM.dat");
@@ -266,12 +266,12 @@ Initialization()
// to the VRML ID.
ODEBUG("Enter 5.0 ");
// Extract the indexes of the Right leg.
-
+
if (RightFoot->associatedAnkle()==0)
LTHROW("No right ankle");
- std::vector<CjrlJoint *> FromRootToJoint2,FromRootToJoint =
+ std::vector<CjrlJoint *> FromRootToJoint2,FromRootToJoint =
RightFoot->associatedAnkle()->jointsFromRootToThis();
- std::vector<CjrlJoint *> ActuatedJoints =
+ std::vector<CjrlJoint *> ActuatedJoints =
getHumanoidDynamicRobot()->getActuatedJoints();
ODEBUG4("Size of ActuatedJoints"<<ActuatedJoints.size(),"DebugDataStartingCOM.dat");
// Build global map.
@@ -298,18 +298,18 @@ Initialization()
ActuatedJoints,
m_LeftArmIndexInVRML, m_LeftArmIndexinConfiguration,
m_LeftShoulder);
-
+
// Create maps for the right hand.
InitializeMapsForAHand(getHumanoidDynamicRobot()->rightHand(),
ActuatedJoints,
m_RightArmIndexInVRML, m_RightArmIndexinConfiguration,
m_RightShoulder);
-
+
FromRootToJoint.clear();
FromRootToJoint2.clear();
-
+
ODEBUG("RightLegIndex: "
<< m_RightLegIndexInVRML[0] << " "
<< m_RightLegIndexInVRML[1] << " "
@@ -336,9 +336,9 @@ Initialization()
<< m_LeftLegIndexinConfiguration[3] << " "
<< m_LeftLegIndexinConfiguration[4] << " "
<< m_LeftLegIndexinConfiguration[5] );
-
+
ODEBUG("Enter 12.0 ");
-
+
MAL_VECTOR_FILL(m_prev_Configuration,0.0);
MAL_VECTOR_FILL(m_prev_Configuration1,0.0);
MAL_VECTOR_FILL(m_prev_Velocity,0.0);
@@ -362,7 +362,7 @@ InitializationHumanoid(MAL_VECTOR_TYPE(double) &BodyAnglesIni,
{
// For initialization we read the current value inside
// the model. But we do not use it.
- MAL_VECTOR_TYPE(double) CurrentConfig =
+ MAL_VECTOR_TYPE(double) CurrentConfig =
getHumanoidDynamicRobot()->currentConfiguration();
// Set to zero the free floating root.
@@ -373,12 +373,12 @@ InitializationHumanoid(MAL_VECTOR_TYPE(double) &BodyAnglesIni,
CurrentConfig[3] = 0.0;
CurrentConfig[4] = 0.0;
CurrentConfig[5] = 0.0;
-
+
// Initialize the configuration vector.
for(unsigned int i=0;i<m_GlobalVRMLIDtoConfiguration.size();i++)
{
CurrentConfig[m_GlobalVRMLIDtoConfiguration[i]] = BodyAnglesIni[i];
- }
+ }
CjrlHumanoidDynamicRobot *aDMB = getHumanoidDynamicRobot();
aDMB->currentConfiguration(CurrentConfig);
@@ -425,7 +425,7 @@ InitializationFoot(CjrlFoot * aFoot,
for(unsigned int i=0;i<3;i++)
MAL_S4x4_MATRIX_ACCESS_I_J(FootTranslation, i,3) = -
m_AnklePosition[i];
-
+
lFootPose = MAL_S4x4_RET_A_by_B(lFootPose,FootTranslation);
InitFootPosition.x = MAL_S4x4_MATRIX_ACCESS_I_J(lFootPose, 0,3);
@@ -489,7 +489,7 @@ InitializationCoM(MAL_VECTOR_TYPE(double) &BodyAnglesIni,
/* Initialize the waist pose.*/
MAL_VECTOR_RESIZE(lStartingWaistPose,6);
- // Compute the forward dynamics from the configuration vector provided by the user.
+ // Compute the forward dynamics from the configuration vector provided by the user.
// Initialize waist pose.
InitializationHumanoid(BodyAnglesIni,lStartingWaistPose);
@@ -500,23 +500,23 @@ InitializationCoM(MAL_VECTOR_TYPE(double) &BodyAnglesIni,
// Initialize Feet.
InitializationFoot(RightFoot, m_AnklePositionRight,InitRightFootPosition);
- ODEBUG("InitRightFootPosition : " << InitRightFootPosition.x
- << " " << InitRightFootPosition.y
+ ODEBUG("InitRightFootPosition : " << InitRightFootPosition.x
+ << " " << InitRightFootPosition.y
<< " " << InitRightFootPosition.z << std::endl
<< " Ankle: " << m_AnklePositionRight);
InitializationFoot(LeftFoot, m_AnklePositionLeft, InitLeftFootPosition);
- ODEBUG("InitLeftFootPosition : " << InitLeftFootPosition.x
- << " " << InitLeftFootPosition.y
+ ODEBUG("InitLeftFootPosition : " << InitLeftFootPosition.x
+ << " " << InitLeftFootPosition.y
<< " " << InitLeftFootPosition.z << std::endl
<< " Ankle: " << m_AnklePositionLeft);
-
+
// Compute the center of gravity between the ankles.
- MAL_S3_VECTOR_ACCESS(m_COGInitialAnkles,0) =
+ MAL_S3_VECTOR_ACCESS(m_COGInitialAnkles,0) =
0.5 * (InitRightFootPosition.x + InitLeftFootPosition.x);
- MAL_S3_VECTOR_ACCESS(m_COGInitialAnkles,1) =
+ MAL_S3_VECTOR_ACCESS(m_COGInitialAnkles,1) =
0.5 * (InitRightFootPosition.y + InitLeftFootPosition.y);
- MAL_S3_VECTOR_ACCESS(m_COGInitialAnkles,2) =
+ MAL_S3_VECTOR_ACCESS(m_COGInitialAnkles,2) =
0.5 * (InitRightFootPosition.z + InitLeftFootPosition.z);
ODEBUG("COGInitialAnkle : "<<m_COGInitialAnkles);
@@ -539,8 +539,8 @@ InitializationCoM(MAL_VECTOR_TYPE(double) &BodyAnglesIni,
m_DiffBetweenComAndWaist[1] = lStartingWaistPose(1) - lStartingCOMPosition[1];
m_DiffBetweenComAndWaist[2] = lStartingWaistPose(2) - lStartingCOMPosition[2];
ODEBUG("lFootPosition[2]: " <<InitRightFootPosition.z);
- ODEBUG( "Diff between Com and Waist" << m_DiffBetweenComAndWaist[0] << " "
- << m_DiffBetweenComAndWaist[1] << " "
+ ODEBUG( "Diff between Com and Waist" << m_DiffBetweenComAndWaist[0] << " "
+ << m_DiffBetweenComAndWaist[1] << " "
<< m_DiffBetweenComAndWaist[2]);
// This term is usefull if
@@ -551,9 +551,9 @@ InitializationCoM(MAL_VECTOR_TYPE(double) &BodyAnglesIni,
// The one which initialize correctly the height of the pattern generator.
for(int i=0;i<3;i++)
{
- m_TranslationToTheLeftHip(i) =
+ m_TranslationToTheLeftHip(i) =
m_StaticToTheLeftHip(i) + m_DiffBetweenComAndWaist[i];
- m_TranslationToTheRightHip(i) =
+ m_TranslationToTheRightHip(i) =
m_StaticToTheRightHip(i) + m_DiffBetweenComAndWaist[i];
}
@@ -591,7 +591,7 @@ InitializationUpperBody(deque<ZMPPosition> &inZMPPositions,
ODEBUG6("FinishAndRealizeStepSequence() - 1","DebugGMFKW.dat");
cout << __FUNCTION__ << ":"<< __LINE__ << ": You should implement something here"
<< endl;
-
+
ODEBUG6("FinishAndRealizeStepSequence() - 3 ","DebugGMFKW.dat");
gettimeofday(&time2,0);
@@ -755,7 +755,7 @@ KinematicsForOneLeg(MAL_S3x3_MATRIX_TYPE(double) & Body_R,
Body_P[2] << " " <<
Foot_P[0] << " " <<
Foot_P[1] << " " <<
- Foot_P[2] << " "
+ Foot_P[2] << " "
,"DebugDataIK.dat");
}
@@ -770,9 +770,9 @@ KinematicsForOneLeg(MAL_S3x3_MATRIX_TYPE(double) & Body_R,
{
for(unsigned int j=0;j<3;j++)
{
- MAL_S4x4_MATRIX_ACCESS_I_J(BodyPose,i,j) =
+ MAL_S4x4_MATRIX_ACCESS_I_J(BodyPose,i,j) =
MAL_S3x3_MATRIX_ACCESS_I_J(Body_R,i,j);
- MAL_S4x4_MATRIX_ACCESS_I_J(FootPose,i,j) =
+ MAL_S4x4_MATRIX_ACCESS_I_J(FootPose,i,j) =
MAL_S3x3_MATRIX_ACCESS_I_J(Foot_R,i,j);
}
MAL_S4x4_MATRIX_ACCESS_I_J(BodyPose,i,3) = MAL_S3_VECTOR_ACCESS(Body_P,i);
@@ -781,12 +781,12 @@ KinematicsForOneLeg(MAL_S3x3_MATRIX_TYPE(double) & Body_R,
CjrlJoint *Waist = getHumanoidDynamicRobot()->waist();
-
+
ODEBUG("Typeid of humanoid: " << typeid(getHumanoidDynamicRobot()).name() );
// Call specialized dynamics.
getHumanoidDynamicRobot()->getSpecializedInverseKinematics(*Waist,*Ankle,BodyPose,FootPose,lq);
-
+
return true;
}
@@ -848,7 +848,7 @@ KinematicsForTheLegs(MAL_VECTOR_TYPE(double) & aCoMPosition,
Body_P(1) = aCoMPosition(1) + ToTheHip(1);
Body_P(2) = aCoMPosition(2) + ToTheHip(2);
- ODEBUG(aCoMPosition(2) <<
+ ODEBUG(aCoMPosition(2) <<
" Body_P : " << Body_P << std::endl <<
" aLeftFoot : " << aLeftFoot);
/* If this is the second call, (stage =1)
@@ -918,7 +918,7 @@ ComputePostureForGivenCoMAndFeetPosture(MAL_VECTOR_TYPE(double) & aCoMPosition,
int IterationNumber,
int Stage)
{
-
+
MAL_VECTOR(lqr,double);
MAL_VECTOR(lql,double);
@@ -950,9 +950,9 @@ ComputePostureForGivenCoMAndFeetPosture(MAL_VECTOR_TYPE(double) & aCoMPosition,
lAbsoluteWaistPosition(i) = MAL_S3_VECTOR_ACCESS(AbsoluteWaistPosition,i);
lAbsoluteWaistPosition(i+3) = aCoMPosition(i+3);
}
- ODEBUG("AbsoluteWaistPosition:" << lAbsoluteWaistPosition <<
+ ODEBUG("AbsoluteWaistPosition:" << lAbsoluteWaistPosition <<
" ComPosition" << aCoMPosition);
-
+
ComputeUpperBodyHeuristicForNormalWalking(qArmr,
qArml,
lAbsoluteWaistPosition,
@@ -1027,7 +1027,7 @@ ComputePostureForGivenCoMAndFeetPosture(MAL_VECTOR_TYPE(double) & aCoMPosition,
CurrentAcceleration[i] = 0.0;
/* Keep the new value for the legs. */
}
-
+
double ldt = getSamplingPeriod();
if (Stage==0)
@@ -1095,7 +1095,7 @@ ComputePostureForGivenCoMAndFeetPosture(MAL_VECTOR_TYPE(double) & aCoMPosition,
for(int i=0;i<6;i++)
CurrentVelocity[i] = aCoMSpeed(i);
-
+
for(int i=0;i<6;i++)
CurrentAcceleration[i] = aCoMAcc(i);
@@ -1162,7 +1162,7 @@ EvaluateStartingCoM(MAL_VECTOR(&BodyAngles,double),
int ComAndFootRealizationByGeometry::
EvaluateCOMForStartingPosition( MAL_VECTOR( &BodyAngles,double),
- double , // omega,
+ double , // omega,
double , // theta,
MAL_S3_VECTOR( &lCOMPosition,double),
FootAbsolutePosition & InitLeftFootPosition,
@@ -1193,7 +1193,7 @@ ComputeUpperBodyHeuristicForNormalWalking(MAL_VECTOR_TYPE(double) & qArmr,
MAL_VECTOR_TYPE(double) & RFP,
MAL_VECTOR_TYPE(double) & LFP)
{
-
+
ODEBUG4("aCOMPosition:" << aCOMPosition << endl <<
"Right Foot Position:" << RFP << endl <<
"Left Foot Position:" << LFP << endl, "DebugDataIKArms.txt");
@@ -1219,24 +1219,24 @@ ComputeUpperBodyHeuristicForNormalWalking(MAL_VECTOR_TYPE(double) & qArmr,
TempARight = TempXR*-1.0;
TempALeft = TempXL*-1.0;
- ODEBUG4("Values: TL " << TempALeft <<
- " TR " << TempARight <<
- " " << m_ZARM <<
+ ODEBUG4("Values: TL " << TempALeft <<
+ " TR " << TempARight <<
+ " " << m_ZARM <<
" " << m_Xmax ,"DebugDataIKArms.txt");
- ODEBUG("Values: TL " << TempALeft <<
- " TR " << TempARight <<
- " " << m_ZARM <<
+ ODEBUG("Values: TL " << TempALeft <<
+ " TR " << TempARight <<
+ " " << m_ZARM <<
" " << m_Xmax );
// Compute angles using inverse kinematics and the computed hand position.
matrix4d jointRootPosition,jointEndPosition;
MAL_S4x4_MATRIX_SET_IDENTITY(jointRootPosition);
MAL_S4x4_MATRIX_SET_IDENTITY(jointEndPosition);
-
- MAL_S4x4_MATRIX_ACCESS_I_J(jointEndPosition,0,3) =
+
+ MAL_S4x4_MATRIX_ACCESS_I_J(jointEndPosition,0,3) =
TempALeft * m_GainFactor / 0.2;
MAL_S4x4_MATRIX_ACCESS_I_J(jointEndPosition,2,3) = m_ZARM;
-
+
getHumanoidDynamicRobot()->getSpecializedInverseKinematics(*m_LeftShoulder,
*getHumanoidDynamicRobot()->leftWrist(),
jointRootPosition,
@@ -1249,7 +1249,7 @@ ComputeUpperBodyHeuristicForNormalWalking(MAL_VECTOR_TYPE(double) & qArmr,
MAL_S4x4_MATRIX_ACCESS_I_J(jointEndPosition,0,3) = TempARight;
MAL_S4x4_MATRIX_ACCESS_I_J(jointEndPosition,2,3) = m_ZARM;
-
+
getHumanoidDynamicRobot()->getSpecializedInverseKinematics(*m_RightShoulder,
*getHumanoidDynamicRobot()->rightWrist(),
jointRootPosition,
@@ -1329,6 +1329,47 @@ GetCurrentPositionofWaistInCOMFrame()
MAL_S3_VECTOR_TYPE(double) ComAndFootRealizationByGeometry::GetCOGInitialAnkles()
{
-
+
return m_COGInitialAnkles;
}
+
+ostream& PatternGeneratorJRL::operator<<(ostream &os,const ComAndFootRealization &obj)
+{
+
+ SimplePluginManager * SPM = obj.getSimplePluginManager() ;
+ os << "The Simple Plugin Manager is :\n" ;
+ os << SPM << endl ;
+
+// MAL_S3_VECTOR_TYPE(double) COGInitialAnkles = obj.GetCOGInitialAnkles() ;
+// os << "The initial COG of ankles is :\n" ;
+// os << COGInitialAnkles(0,0) << endl
+// << COGInitialAnkles(1,0) << endl
+// << COGInitialAnkles(2,0) << endl ;
+
+ StepStackHandler* stepStackHandler = obj.GetStepStackHandler() ;
+ os << "The Step Stack Handler pointer is :\n" ;
+ os << stepStackHandler << endl ;
+
+ double SamplingPeriod = obj.getSamplingPeriod() ;
+ os << "The sampling period is :\n" ;
+ os << SamplingPeriod << endl ;
+
+// MAL_S4x4_MATRIX_TYPE(double) P = obj.GetCurrentPositionofWaistInCOMFrame();
+// os<< "The current position of waist in COM frame is :\n" ;
+// << MAL_S4x4_MATRIX_ACCESS_I_J(P, 0,0) << " " << MAL_S4x4_MATRIX_ACCESS_I_J(P, 0,1) << " "
+// << MAL_S4x4_MATRIX_ACCESS_I_J(P, 0,2) << " " << MAL_S4x4_MATRIX_ACCESS_I_J(P, 0,3) << endl
+// << MAL_S4x4_MATRIX_ACCESS_I_J(P, 1,0) << " " << MAL_S4x4_MATRIX_ACCESS_I_J(P, 1,1) << " "
+// << MAL_S4x4_MATRIX_ACCESS_I_J(P, 1,2) << " " << MAL_S4x4_MATRIX_ACCESS_I_J(P, 1,3) << endl
+// << MAL_S4x4_MATRIX_ACCESS_I_J(P, 2,0) << " " << MAL_S4x4_MATRIX_ACCESS_I_J(P, 2,1) << " "
+// << MAL_S4x4_MATRIX_ACCESS_I_J(P, 2,2) << " " << MAL_S4x4_MATRIX_ACCESS_I_J(P, 2,3) << endl
+// << MAL_S4x4_MATRIX_ACCESS_I_J(P, 3,0) << " " << MAL_S4x4_MATRIX_ACCESS_I_J(P, 3,1) << " "
+// << MAL_S4x4_MATRIX_ACCESS_I_J(P, 3,2) << " " << MAL_S4x4_MATRIX_ACCESS_I_J(P, 3,3) << endl ;
+
+ CjrlHumanoidDynamicRobot * HumanoidDynamicRobot = obj.getHumanoidDynamicRobot();
+ os << "The HumanoidDynamicRobot is at the adress :\n" << HumanoidDynamicRobot << endl ;
+
+ double HeightOfTheCoM = obj.GetHeightOfTheCoM();
+ os << "The height of the robot is :\n" << HeightOfTheCoM << endl << endl ;
+
+ return os;
+}
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.hh b/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
index fb6fe70b..8b0f94de 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
@@ -1,5 +1,5 @@
/*
- * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
+ * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
*
* Francois Keith
* Olivier Stasse
@@ -19,20 +19,20 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* \file ComAndFootRealizationByGeometry.h
\brief Realizes the CoM and Foot position by assuming that the robot
has 6 DoFs legs. It is then a simple matter of inverse geometry,
- relying on the inverse kinematics object.
+ relying on the inverse kinematics object.
*/
#ifndef _COM_AND_FOOT_REALIZATION_BY_GEOMETRY_H_
#define _COM_AND_FOOT_REALIZATION_BY_GEOMETRY_H_
#include <jrl/walkgen/pgtypes.hh>
-#include <MotionGeneration/ComAndFootRealization.hh>
+#include <MotionGeneration/ComAndFootRealization.hh>
#include <MotionGeneration/StepOverPlanner.hh>
#include <MotionGeneration/WaistHeightVariation.hh>
#include <MotionGeneration/UpperBodyMotion.hh>
@@ -42,20 +42,20 @@ namespace PatternGeneratorJRL
{
/* @ingroup motiongeneration
- This object realizes different kind of motion: stepping over,
+ This object realizes different kind of motion: stepping over,
execution of planned trajectory, lowering the waist, but they all
assume that the upper body position is separated from the legs.
- It also assumes that the robot has 6 DoFs legs.
+ It also assumes that the robot has 6 DoFs legs.
It is then a simple matter of inverse geometry,
relying on the inverse kinematics object.
-
+
The different strategies can by modified by changing the walking mode
parameter.
-
+
*/
class ComAndFootRealizationByGeometry: public ComAndFootRealization
{
-
+
public:
/*! \name Constructor and destructor */
@@ -69,11 +69,11 @@ namespace PatternGeneratorJRL
/*! Initialization which should be done after setting the HumanoidDynamicRobot member. */
void Initialization();
-
+
/*! Compute the robot state for a given CoM and feet posture.
Each posture is given by a 3D position and two Euler angles \f$ (\theta, \omega) \f$.
Very important: This method is assume to set correctly the body angles of
- its \a HumanoidDynamicRobot and a subsequent call to the ZMP position
+ its \a HumanoidDynamicRobot and a subsequent call to the ZMP position
will return the associated ZMP vector.
@param[in] CoMPosition a 6 dimensional vector with the first 3 dimension for position,
and the last two for the orientation (Euler angle).
@@ -83,14 +83,14 @@ namespace PatternGeneratorJRL
and the last two for the angular velocity.
@param[in] LeftFoot a 6 dimensional following the same convention than for \a CoMPosition.
@param[in] RightFoot idem.
- @param[out] CurrentConfiguration The result is a state vector containing
+ @param[out] CurrentConfiguration The result is a state vector containing
the position which are put inside this parameter.
@param[out] CurrentVelocity The result is a state vector containing the speed which are put inside this parameter.
@param[out] CurrentAcceleration The result is a state vector containing the acceleratio which are put inside this parameter.
@param[in] IterationNumber Number of iteration.
- @param[in] Stage indicates which stage is reach by the Pattern Generator. If this is the
+ @param[in] Stage indicates which stage is reach by the Pattern Generator. If this is the
last stage, we store some information.
-
+
*/
bool ComputePostureForGivenCoMAndFeetPosture(MAL_VECTOR_TYPE(double) &CoMPosition,
MAL_VECTOR_TYPE(double) & aCoMSpeed,
@@ -101,16 +101,16 @@ namespace PatternGeneratorJRL
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration,
int IterationNumber,
- int Stage);
+ int Stage);
- /*! \name Initialization of the walking.
+ /*! \name Initialization of the walking.
@{
*/
-
-
+
+
/*! \brief Initialize the humanoid model considering the current
- configuration set by the user.
- \param[in] BodyAnglesIni: The configuration vector provided by the user.
+ configuration set by the user.
+ \param[in] BodyAnglesIni: The configuration vector provided by the user.
\param[out] lStartingWaistPose: The waist pose according to the user configuration vector.
*/
bool InitializationHumanoid(MAL_VECTOR_TYPE(double) &BodyAnglesIni,
@@ -119,7 +119,7 @@ namespace PatternGeneratorJRL
/*! \brief Initialize the foot position.
\param[in] aFoot: Pointer to the foot to be updated.
\param[in] m_AnklePosition: Translation from the ankle to the soil.
- \param[out] InitFootPosition: The foot position according to the
+ \param[out] InitFootPosition: The foot position according to the
free flyer (set to 0.0 0.0 0.0)
*/
bool InitializationFoot(CjrlFoot * aFoot,
@@ -130,15 +130,15 @@ namespace PatternGeneratorJRL
1/ we take the current state of the robot
to compute the current CoM value.
2/ We deduce the difference between the CoM and the waist,
- which is suppose to be constant for the all duration of the motion.
+ which is suppose to be constant for the all duration of the motion.
IMPORTANT: The jrlHumanoidDynamicRobot must have been properly set up.
-
+
*/
bool InitializationCoM(MAL_VECTOR_TYPE(double) &BodyAnglesIni,
MAL_S3_VECTOR_TYPE(double) & lStartingCOMPosition,
MAL_VECTOR_TYPE(double) & lStartingWaistPosition,
- FootAbsolutePosition & InitLeftFootAbsPos,
+ FootAbsolutePosition & InitLeftFootAbsPos,
FootAbsolutePosition & InitRightFootAbsPos);
/*! This initialization phase, make sure that the needed buffers
@@ -149,7 +149,7 @@ namespace PatternGeneratorJRL
deque<RelativeFootPosition> lRelativeFootPositions);
/* @} */
-
+
@@ -162,7 +162,7 @@ namespace PatternGeneratorJRL
MAL_S3_VECTOR( &lCOMPosition,double),
FootAbsolutePosition & LeftFootPosition,
FootAbsolutePosition & RightFootPosition);
-
+
/*! Evaluate CoM for a given position.
Assuming that the waist is at (0,0,0)
It returns the associate initial values for the left and right foot.*/
@@ -179,7 +179,7 @@ namespace PatternGeneratorJRL
FootAbsolutePosition & InitRightFootPosition);
/*! Method to compute the heuristic for the arms. */
- void ComputeUpperBodyHeuristicForNormalWalking(MAL_VECTOR_TYPE(double) & qArmr,
+ void ComputeUpperBodyHeuristicForNormalWalking(MAL_VECTOR_TYPE(double) & qArmr,
MAL_VECTOR_TYPE(double) & qArml,
MAL_VECTOR_TYPE(double) & aCOMPosition,
MAL_VECTOR_TYPE(double) & RFP,
@@ -215,7 +215,7 @@ namespace PatternGeneratorJRL
int LeftOrRight,
MAL_VECTOR_TYPE(double) &lq,
int Stage);
-
+
/*! Compute the angles values considering two 6DOF legs for a given configuration
of the waist and of the feet:
@param aCoMPosition: Position of the CoM (x,y,z,theta, omega, phi).
@@ -234,12 +234,12 @@ namespace PatternGeneratorJRL
MAL_VECTOR_TYPE(double) & qr,
MAL_S3_VECTOR_TYPE(double) & AbsoluteWaistPosition);
- /*! \brief Implement the Plugin part to receive information from
+ /*! \brief Implement the Plugin part to receive information from
PatternGeneratorInterface.
*/
void CallMethod(string &Method, istringstream &istrm);
- /*! Get the current position of the waist in the COM reference frame
+ /*! Get the current position of the waist in the COM reference frame
@return a 4x4 matrix which contains the pose and the position of the waist
in the CoM reference frame.
*/
@@ -248,15 +248,17 @@ namespace PatternGeneratorJRL
/*! \brief Get the COG of the ankles at the starting position. */
virtual MAL_S3_VECTOR_TYPE(double) GetCOGInitialAnkles();
+ friend ostream& operator <<(ostream &os,const ComAndFootRealization &obj);
+
protected:
-
+
/*! \brief Initialization of internal maps of indexes */
void InitializationMaps(std::vector<CjrlJoint *> &FromRootToFoot,
std::vector<CjrlJoint *> &ActuatedJoints,
std::vector<int> &IndexInVRML,
std::vector<int> &IndexinConfiguration);
- /*! Map shoulders and wrist
+ /*! Map shoulders and wrist
\param[in] aHand: The hand to be used for extraction of data.
\param[in] ActuatedJoints: The vector of actuated joints.
\param[out] IndexesInVRML: The kinematic chain from the shoulder
@@ -264,7 +266,7 @@ namespace PatternGeneratorJRL
\param[out] IndexesInConfiguration: The kinematic chain
from the shoulder given with the depth-first ordering.
\param[out] associateShoulder: The shoulder extracted from
- the kinematic chain.
+ the kinematic chain.
*/
void InitializeMapsForAHand(CjrlHand * aHand,
std::vector<CjrlJoint *> &ActuatedJoints,
@@ -280,14 +282,14 @@ namespace PatternGeneratorJRL
private:
- /*! \name Objects for stepping over.
+ /*! \name Objects for stepping over.
@{
*/
-
+
/*! Planner for the waist variation for stepping
over an obstacle. */
- WaistHeightVariation *m_WaistPlanner;
-
+ WaistHeightVariation *m_WaistPlanner;
+
/*! Planner for the upper body motion. */
UpperBodyMotion * m_UpBody;
@@ -295,15 +297,15 @@ namespace PatternGeneratorJRL
/*! Pointer related to Kineoworks planner. */
GenerateMotionFromKineoWorks * m_GMFKW;
-
-
+
+
/*! \brief Displacement between the hip and the foot. @{*/
/*! \brief For the right foot. */
MAL_S3_VECTOR(m_DtRight,double);
/*! \brief For the left foot. */
MAL_S3_VECTOR(m_DtLeft,double);
/*! @} */
-
+
/*! \name Vector from the Waist to the left and right hip. */
/*! Static part from the waist to the left hip.. */
@@ -314,12 +316,12 @@ namespace PatternGeneratorJRL
MAL_S3_VECTOR(m_TranslationToTheLeftHip,double);
/*! Dynamic part form the waist to the right hip. */
MAL_S3_VECTOR( m_TranslationToTheRightHip,double);
-
+
/*! @} */
-
+
/*! \name Previous joint values. */
- //@{
+ //@{
/*! \brief For the speed (stage 0). */
MAL_VECTOR(m_prev_Configuration,double);
@@ -333,29 +335,29 @@ namespace PatternGeneratorJRL
MAL_VECTOR( m_prev_Velocity1,double);
//@}
-
+
/*! COM Starting position. */
MAL_S3_VECTOR(m_StartingCOMPosition,double);
-
+
/*! Final COM pose. */
MAL_S4x4_MATRIX(m_FinalDesiredCOMPose,double);
-
+
/*! Store the position of the ankle in the right feet. */
MAL_S3_VECTOR(m_AnklePositionRight,double);
/*! Store the position of the ankle in the left feet. */
MAL_S3_VECTOR(m_AnklePositionLeft,double);
-
- /*! Difference between the CoM and the Waist
+
+ /*! Difference between the CoM and the Waist
from the initialization phase,
i.e. not reevaluated while walking. */
MAL_S3_VECTOR(m_DiffBetweenComAndWaist,double);
- /*! Difference between the CoM and the Waist
+ /*! Difference between the CoM and the Waist
in the CoM reference frame. */
MAL_S3_VECTOR(m_ComAndWaistInRefFrame,double);
-
+
/*! Maximal distance along the X axis for the hand motion */
double m_Xmax;
@@ -367,7 +369,7 @@ namespace PatternGeneratorJRL
/*! Conversion between the index of the plan and the robot DOFs. */
std::vector<int> m_ConversionForUpperBodyFromLocalIndexToRobotDOFs;
-
+
/*! Keep the indexes for the legs of the robot in the VRML numbering system. */
std::vector<int> m_LeftLegIndexInVRML;
std::vector<int> m_RightLegIndexInVRML;
@@ -375,7 +377,7 @@ namespace PatternGeneratorJRL
std::vector<int> m_RightArmIndexInVRML;
std::vector<int> m_ChestIndexInVRML;
- /*! \name Keep the indexes into the Configuration numbering system.
+ /*! \name Keep the indexes into the Configuration numbering system.
@{
*/
/*! \brief For the left leg, Specific for the Inverse Kinematics. */
@@ -386,7 +388,7 @@ namespace PatternGeneratorJRL
std::vector<int> m_LeftArmIndexinConfiguration;
/*! \brief For the right leg, Specific for the Inverse Kinematics. */
std::vector<int> m_RightArmIndexinConfiguration;
-
+
/*! \brief For the chest. */
std::vector<int> m_ChestIndexinConfiguration;
@@ -399,7 +401,7 @@ namespace PatternGeneratorJRL
/*! Buffer of current Upper Body motion. */
std::vector<double> m_UpperBodyMotion;
- /*! COG of the ankles in the waist reference frame
+ /*! COG of the ankles in the waist reference frame
when evaluating the initial position.
*/
MAL_S3_VECTOR_TYPE(double) m_COGInitialAnkles;
@@ -408,6 +410,7 @@ namespace PatternGeneratorJRL
CjrlJoint *m_LeftShoulder, *m_RightShoulder;
};
+ ostream & operator <<(ostream &os,const ComAndFootRealization &obj);
}
-#endif
+#endif
diff --git a/src/PatternGeneratorInterfacePrivate.cpp b/src/PatternGeneratorInterfacePrivate.cpp
index 51a508fa..37b8dcb0 100644
--- a/src/PatternGeneratorInterfacePrivate.cpp
+++ b/src/PatternGeneratorInterfacePrivate.cpp
@@ -228,7 +228,8 @@ namespace PatternGeneratorJRL {
// INFO: This where you should instanciate your own
// INFO: object for Com and Foot realization.
// INFO: The default one is based on a geometrical approach.
- m_ComAndFootRealization = new ComAndFootRealizationByGeometry(this);
+ m_ComAndFootRealization.resize(2);
+ m_ComAndFootRealization[0] = new ComAndFootRealizationByGeometry(this);
// Creates the foot trajectory generator.
m_FeetTrajectoryGenerator = new LeftAndRightFootTrajectoryGenerationMultiple(this,
@@ -245,6 +246,7 @@ namespace PatternGeneratorJRL {
// ZMP and CoM generation using the method proposed in Herdt2010.
m_ZMPVRQP = new ZMPVelocityReferencedQP(this,"",m_HumanoidDynamicRobot);
+ m_ComAndFootRealization[1] = m_ZMPVRQP->getComAndFootRealization();
// ZMP and CoM generation using the analytical method proposed in Morisawa2007.
m_ZMPM = new AnalyticalMorisawaCompact(this);
@@ -282,7 +284,7 @@ namespace PatternGeneratorJRL {
&m_LeftFootPositions,
&m_RightFootPositions);
- // Defa`<ult handler :DoubleStagePreviewControl.
+ // Default handler :DoubleStagePreviewControl.
m_GlobalStrategyManager = m_DoubleStagePCStrategy;
// End of the creation of the fundamental objects.
@@ -296,7 +298,7 @@ namespace PatternGeneratorJRL {
// Initialize the Preview Control general object.
m_DoubleStagePCStrategy->InitInterObjects(m_HumanoidDynamicRobot,
- m_ComAndFootRealization,
+ (*m_ComAndFootRealization.begin()),
m_StepStackHandler);
m_CoMAndFootOnlyStrategy->InitInterObjects(m_HumanoidDynamicRobot,
@@ -335,15 +337,13 @@ namespace PatternGeneratorJRL {
// Read the robot VRML file model.
- m_ComAndFootRealization->setHumanoidDynamicRobot(m_HumanoidDynamicRobot);
+ m_ComAndFootRealization[0]->setHumanoidDynamicRobot(m_HumanoidDynamicRobot);
+ m_ComAndFootRealization[0]->SetHeightOfTheCoM(m_PC->GetHeightOfCoM());
+ m_ComAndFootRealization[0]->setSamplingPeriod(m_PC->SamplingPeriod());
+ m_ComAndFootRealization[0]->SetStepStackHandler(m_StepStackHandler);
+ m_ComAndFootRealization[0]->Initialization();
- m_ComAndFootRealization->SetHeightOfTheCoM(m_PC->GetHeightOfCoM());
-
- m_ComAndFootRealization->setSamplingPeriod(m_PC->SamplingPeriod());
-
- m_ComAndFootRealization->SetStepStackHandler(m_StepStackHandler);
-
- m_ComAndFootRealization->Initialization();
+ m_ComAndFootRealization[1]->SetStepStackHandler(m_StepStackHandler);
m_StOvPl->SetPreviewControl(m_PC);
m_StOvPl->SetDynamicMultiBodyModel(m_HumanoidDynamicRobot);
@@ -353,6 +353,7 @@ namespace PatternGeneratorJRL {
m_StepStackHandler->SetStepOverPlanner(m_StOvPl);
m_StepStackHandler->SetWalkMode(0);
// End of the initialization of the fundamental object.
+
}
PatternGeneratorInterfacePrivate::~PatternGeneratorInterfacePrivate()
@@ -396,8 +397,8 @@ namespace PatternGeneratorJRL {
delete m_ZMPM;
ODEBUG4("Destructor: did m_ZMPM","DebugPGI.txt");
- if (m_ComAndFootRealization!=0)
- delete m_ComAndFootRealization;
+ if ((*m_ComAndFootRealization.begin())!=0)
+ delete (*m_ComAndFootRealization.begin());
if (m_FeetTrajectoryGenerator!=0)
delete m_FeetTrajectoryGenerator;
@@ -1319,7 +1320,6 @@ namespace PatternGeneratorJRL {
{
ODEBUG("InternalClock:" <<m_InternalClock <<
" SamplingPeriod: "<<m_SamplingPeriod);
- m_ZMPVRQP->setComAndFootRealization(m_ComAndFootRealization);
m_ZMPVRQP->OnLine(m_InternalClock,
m_ZMPPositions,
m_COMBuffer,
@@ -1512,6 +1512,10 @@ namespace PatternGeneratorJRL {
// to be done only when the robot has finish a motion.
UpdateAbsolutePosition(UpdateAbsMotionOrNot);
ODEBUG("Return true");
+
+ //cout << "CoM 0 :\n" << *m_ComAndFootRealization[0] << endl ;
+ //cout << "CoM 1 :\n" << *m_ComAndFootRealization[1] << endl ;
+
return m_Running;
}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index d2edef9c..62f021f4 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -75,7 +75,7 @@ double filterprecision(double adb)
}
ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
- string , CjrlHumanoidDynamicRobot *aHS , ComAndFootRealization * ComAndFootRealization ) :
+ string , CjrlHumanoidDynamicRobot *aHS ) :
ZMPRefTrajectoryGeneration(SPM),
Robot_(0),SupportFSM_(0),OrientPrw_(0),VRQPGenerator_(0),IntermedData_(0),RFI_(0),Problem_(),Solution_(),OFTG_(0)
{
@@ -157,9 +157,13 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
OFTG_->FeetDistance( 0.2 );
OFTG_->StepHeight( 0.05 );
- setComAndFootRealization(ComAndFootRealization);
- if (ComAndFootRealization_)
- ComAndFootRealization_->Initialization();
+ // Create and initialize the class that compute the simplify inverse kinematics :
+ // ------------------------------------------------------------------------------
+ ComAndFootRealization_ = new ComAndFootRealizationByGeometry( (PatternGeneratorInterfacePrivate*) SPM );
+ ComAndFootRealization_->setHumanoidDynamicRobot(aHS);
+ ComAndFootRealization_->SetHeightOfTheCoM(0.0);
+ ComAndFootRealization_->setSamplingPeriod(0.005);
+ ComAndFootRealization_->Initialization();
// Register method to handle
const unsigned int NbMethods = 3;
@@ -201,6 +205,13 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
m_deltaZMPMBPositions.resize ( QP_N_ * m_numberOfSample );
m_LeftFootTraj_deq.resize(QP_N_ * m_numberOfSample + 50);
m_RightFootTraj_deq.resize(QP_N_ * m_numberOfSample + 50);
+
+ m_previousConfiguration = aHS->currentConfiguration() ;
+ m_previousVelocity = aHS->currentVelocity();
+ m_previousAcceleration = aHS->currentAcceleration();
+ m_QP_T_Configuration = aHS->currentConfiguration();
+ m_QP_T_previousVelocity = aHS->currentVelocity();
+ m_QP_T_previousAcceleration = aHS->currentAcceleration();
}
@@ -247,6 +258,17 @@ ZMPVelocityReferencedQP::~ZMPVelocityReferencedQP()
delete OFTG_;
OFTG_ = 0 ;
}
+
+ if (SSH_!=0){
+ delete SSH_;
+ SSH_ = 0 ;
+ }
+
+ if (ComAndFootRealization_!=0){
+ delete ComAndFootRealization_;
+ ComAndFootRealization_ = 0 ;
+ }
+
}
@@ -441,171 +463,210 @@ ZMPVelocityReferencedQP::OnLine(double time,
// UPDATE WALKING TRAJECTORIES:
// ----------------------------
if(time + 0.00001 > UpperTimeLimitToUpdate_)
- {
+ {
- // UPDATE INTERNAL DATA:
- // ---------------------
- Problem_.reset_variant();
- Solution_.reset();
- VRQPGenerator_->CurrentTime( time );
- VelRef_=NewVelRef_;
- SupportFSM_->update_vel_reference(VelRef_, IntermedData_->SupportState());
- IntermedData_->Reference( VelRef_ );
- IntermedData_->CoM( CoM_() );
+ // UPDATE INTERNAL DATA:
+ // ---------------------
+ Problem_.reset_variant();
+ Solution_.reset();
+ VRQPGenerator_->CurrentTime( time );
+ VelRef_=NewVelRef_;
+ SupportFSM_->update_vel_reference(VelRef_, IntermedData_->SupportState());
+ IntermedData_->Reference( VelRef_ );
+ IntermedData_->CoM( CoM_() );
- // PREVIEW SUPPORT STATES FOR THE WHOLE PREVIEW WINDOW:
- // ----------------------------------------------------
- VRQPGenerator_->preview_support_states( time, SupportFSM_,
- FinalLeftFootTraj_deq, FinalRightFootTraj_deq, Solution_.SupportStates_deq );
+ // PREVIEW SUPPORT STATES FOR THE WHOLE PREVIEW WINDOW:
+ // ----------------------------------------------------
+ VRQPGenerator_->preview_support_states( time, SupportFSM_,
+ FinalLeftFootTraj_deq, FinalRightFootTraj_deq, Solution_.SupportStates_deq );
- // COMPUTE ORIENTATIONS OF FEET FOR WHOLE PREVIEW PERIOD:
- // ------------------------------------------------------
- OrientPrw_->preview_orientations( time, VelRef_,
- SupportFSM_->StepPeriod(),
- FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
- Solution_ );
+ // COMPUTE ORIENTATIONS OF FEET FOR WHOLE PREVIEW PERIOD:
+ // ------------------------------------------------------
+ OrientPrw_->preview_orientations( time, VelRef_,
+ SupportFSM_->StepPeriod(),
+ FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
+ Solution_ );
- // UPDATE THE DYNAMICS:
- // --------------------
- Robot_->update( Solution_.SupportStates_deq,
- FinalLeftFootTraj_deq, FinalRightFootTraj_deq );
+ // UPDATE THE DYNAMICS:
+ // --------------------
+ Robot_->update( Solution_.SupportStates_deq,
+ FinalLeftFootTraj_deq, FinalRightFootTraj_deq );
- // COMPUTE REFERENCE IN THE GLOBAL FRAME:
- // --------------------------------------
- VRQPGenerator_->compute_global_reference( Solution_ );
+ // COMPUTE REFERENCE IN THE GLOBAL FRAME:
+ // --------------------------------------
+ VRQPGenerator_->compute_global_reference( Solution_ );
- // BUILD VARIANT PART OF THE OBJECTIVE:
- // ------------------------------------
- VRQPGenerator_->update_problem( Problem_, Solution_.SupportStates_deq );
+ // BUILD VARIANT PART OF THE OBJECTIVE:
+ // ------------------------------------
+ VRQPGenerator_->update_problem( Problem_, Solution_.SupportStates_deq );
- // BUILD CONSTRAINTS:
- // ------------------
- VRQPGenerator_->build_constraints( Problem_, Solution_ );
+ // BUILD CONSTRAINTS:
+ // ------------------
+ VRQPGenerator_->build_constraints( Problem_, Solution_ );
- // SOLVE PROBLEM:
- // --------------
- if (Solution_.useWarmStart)
- VRQPGenerator_->compute_warm_start( Solution_ );//TODO: Move to update_problem or build_constraints?
- Problem_.solve( QLD, Solution_, NONE );
- if(Solution_.Fail>0)
- Problem_.dump( time );
+ // SOLVE PROBLEM:
+ // --------------
+ if (Solution_.useWarmStart)
+ VRQPGenerator_->compute_warm_start( Solution_ );//TODO: Move to update_problem or build_constraints?
+ Problem_.solve( QLD, Solution_, NONE );
+ if(Solution_.Fail>0)
+ Problem_.dump( time );
- // INTERPOLATE THE NEXT COMPUTED COM STATE:
- // ----------------------------------------
- unsigned currentIndex = FinalCOMTraj_deq.size();
- deque<support_state_t> prwSupportStates_deq = Solution_.SupportStates_deq ;
- deque<double> previewedSupportAngles_deq = Solution_.SupportOrientations_deq ;
- deque<FootAbsolutePosition> m_LeftFootTraj ;
- deque<FootAbsolutePosition> m_RightFootTraj ;
+ // INTERPOLATE THE NEXT COMPUTED COM STATE:
+ // ----------------------------------------
+ unsigned currentIndex = FinalCOMTraj_deq.size();
+ deque<support_state_t> prwSupportStates_deq = Solution_.SupportStates_deq ;
+ deque<double> previewedSupportAngles_deq = Solution_.SupportOrientations_deq ;
+ deque<FootAbsolutePosition> m_LeftFootTraj ;
+ deque<FootAbsolutePosition> m_RightFootTraj ;
- for (unsigned i = 0 ; i < currentIndex ; i++)
- {
- m_ZMPTraj_deq[i] = FinalZMPTraj_deq[i] ;
- m_COMTraj_deq[i] = FinalCOMTraj_deq[i] ;
- m_LeftFootTraj_deq[i] = FinalLeftFootTraj_deq[i] ;
- m_RightFootTraj_deq[i] = FinalRightFootTraj_deq[i] ;
- }
- m_LeftFootTraj_deq = FinalLeftFootTraj_deq ;
- m_RightFootTraj_deq = FinalRightFootTraj_deq ;
- for ( int i = 0 ; i < QP_N_ ; i++ )
+ for (unsigned i = 0 ; i < currentIndex ; i++)
+ {
+ m_ZMPTraj_deq[i] = FinalZMPTraj_deq[i] ;
+ m_COMTraj_deq[i] = FinalCOMTraj_deq[i] ;
+ m_LeftFootTraj_deq[i] = FinalLeftFootTraj_deq[i] ;
+ m_RightFootTraj_deq[i] = FinalRightFootTraj_deq[i] ;
+ }
+ m_LeftFootTraj_deq = FinalLeftFootTraj_deq ;
+ m_RightFootTraj_deq = FinalRightFootTraj_deq ;
+ bool firstStep = 0 ;
+
+ for ( int i = 0 ; i < QP_N_ ; i++ )
+ {
+ if(Solution_.SupportStates_deq.size() && Solution_.SupportStates_deq[0].NbStepsLeft == 0)
{
- if(Solution_.SupportStates_deq.size() && Solution_.SupportStates_deq[0].NbStepsLeft == 0)
+ double jx = (FinalLeftFootTraj_deq[0].x + FinalRightFootTraj_deq[0].x)/2 - FinalCOMTraj_deq[0].x[0];
+ double jy = (FinalLeftFootTraj_deq[0].y + FinalRightFootTraj_deq[0].y)/2 - FinalCOMTraj_deq[0].y[0];
+ if(fabs(jx) < 1e-3 && fabs(jy) < 1e-3) { Running_ = false; }
+ const double tf = 0.75;
+ jx = 6/(tf*tf*tf)*(jx - tf*FinalCOMTraj_deq[0].x[1] - (tf*tf/2)*FinalCOMTraj_deq[0].x[2]);
+ jy = 6/(tf*tf*tf)*(jy - tf*FinalCOMTraj_deq[0].y[1] - (tf*tf/2)*FinalCOMTraj_deq[0].y[2]);
+ if(i == 0)
{
- double jx = (FinalLeftFootTraj_deq[0].x + FinalRightFootTraj_deq[0].x)/2 - FinalCOMTraj_deq[0].x[0];
- double jy = (FinalLeftFootTraj_deq[0].y + FinalRightFootTraj_deq[0].y)/2 - FinalCOMTraj_deq[0].y[0];
- if(fabs(jx) < 1e-3 && fabs(jy) < 1e-3) { Running_ = false; }
- const double tf = 0.75;
- jx = 6/(tf*tf*tf)*(jx - tf*FinalCOMTraj_deq[0].x[1] - (tf*tf/2)*FinalCOMTraj_deq[0].x[2]);
- jy = 6/(tf*tf*tf)*(jy - tf*FinalCOMTraj_deq[0].y[1] - (tf*tf/2)*FinalCOMTraj_deq[0].y[2]);
- if(i == 0)
- {
- CoM2_.setState(CoM_());
- CoM2_.Interpolation( m_COMTraj_deq, m_ZMPTraj_deq, currentIndex + i * m_numberOfSample,
- jx, jy);
- CoM2_.OneIteration( jx, jy );
- }else{
- CoM2_.Interpolation( m_COMTraj_deq, m_ZMPTraj_deq, currentIndex + 1 + i * m_numberOfSample,
- jx, jy);
- CoM2_.OneIteration( jx, jy );
- }
-
+ CoM2_.setState(CoM_());
}
- else
+ CoM2_.Interpolation( m_COMTraj_deq, m_ZMPTraj_deq, currentIndex + i * m_numberOfSample,
+ jx, jy);
+ CoM2_.OneIteration( jx, jy );
+ }
+ else
+ {
+ Running_ = true;
+ if(i == 0)
{
- Running_ = true;
- if(i == 0)
- {
- CoM2_.setState(CoM_());
- CoM2_.Interpolation( m_COMTraj_deq, m_ZMPTraj_deq, currentIndex + i * m_numberOfSample,
- Solution_.Solution_vec[i], Solution_.Solution_vec[QP_N_+i] );
- CoM2_.OneIteration( Solution_.Solution_vec[i], Solution_.Solution_vec[QP_N_+i] );
- }else{
- CoM2_.Interpolation( m_COMTraj_deq, m_ZMPTraj_deq, currentIndex + 1 + i * m_numberOfSample,
- Solution_.Solution_vec[i], Solution_.Solution_vec[QP_N_+i] );
- CoM2_.OneIteration( Solution_.Solution_vec[i], Solution_.Solution_vec[QP_N_+i] );
- }
+ CoM2_.setState(CoM_());
}
+ CoM2_.Interpolation( m_COMTraj_deq, m_ZMPTraj_deq, currentIndex + i * m_numberOfSample,
+ Solution_.Solution_vec[i], Solution_.Solution_vec[QP_N_+i] );
+ CoM2_.OneIteration( Solution_.Solution_vec[i], Solution_.Solution_vec[QP_N_+i] );
+ }
+
+ // INTERPOLATE TRUNK ORIENTATION:
+ // ------------------------------
+ OrientPrw_->interpolate_trunk_orientation( time + i * QP_T_, currentIndex + i * m_numberOfSample,
+ m_SamplingPeriod, prwSupportStates_deq,
+ m_COMTraj_deq );
- // INTERPOLATE TRUNK ORIENTATION:
- // ------------------------------
- OrientPrw_->interpolate_trunk_orientation( time + i * QP_T_, currentIndex + i * m_numberOfSample,
- m_SamplingPeriod, Solution_.SupportStates_deq,
- m_COMTraj_deq );
-
- // INTERPOLATE THE COMPUTED FOOT POSITIONS:
- // ----------------------------------------
- OFTG_->interpolate_feet_positions(time + i * QP_T_,
- prwSupportStates_deq,
- Solution_,
- previewedSupportAngles_deq,
- m_LeftFootTraj_deq,
- m_RightFootTraj_deq);
- if (i > QP_N_/2.0 && Solution_.Solution_vec.size() >= (unsigned int)(2*QP_N_+3) )
+ // INTERPOLATE THE COMPUTED FOOT POSITIONS:
+ // ----------------------------------------
+ OFTG_->interpolate_feet_positions(time + i * QP_T_,
+ prwSupportStates_deq,
+ Solution_,
+ previewedSupportAngles_deq,
+ m_LeftFootTraj_deq,
+ m_RightFootTraj_deq);
+
+ support_state_t &CurrentSupport = prwSupportStates_deq.front();
+ if (CurrentSupport.StateChanged==true && CurrentSupport.Phase == SS && Solution_.Solution_vec.size() >= (unsigned int)(2*QP_N_+3) )
+ {
+ if (firstStep == 1)
{
Solution_.Solution_vec[2*QP_N_] = Solution_.Solution_vec[2*QP_N_+1] ;
Solution_.Solution_vec[2*QP_N_+2] = Solution_.Solution_vec[2*QP_N_+3];
+ previewedSupportAngles_deq[0]=previewedSupportAngles_deq[2];
}
- prwSupportStates_deq.pop_front();
- previewedSupportAngles_deq.pop_front();
+ firstStep = 1 ;
}
+ prwSupportStates_deq.pop_front();
+ }
- // DYNAMIC FILTER
- // --------------
- DynamicFilter( m_ZMPTraj_deq, m_COMTraj_deq, m_LeftFootTraj_deq, m_RightFootTraj_deq, currentIndex );
+ /// \brief Debug Purpose
+ /// --------------------
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+ static int iteration = 0;
+ int iteration100 = (int)iteration/100;
+ int iteration10 = (int)(iteration - iteration100*100)/10;
+ int iteration1 = (int)(iteration - iteration100*100 - iteration10*10 );
+
+ /// \brief Debug Purpose
+ /// --------------------
+ oss.str("TestHerdt2010DynamicBuffers");
+ oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ ///----
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for(unsigned int i = 0 ; i < currentIndex + QP_N_ * m_numberOfSample ; i++){
+ aof << filterprecision( m_ZMPTraj_deq[i].px ) << " " // 1
+ << filterprecision( m_ZMPTraj_deq[i].py ) << " " // 2
+ << filterprecision( m_COMTraj_deq[i].x[0] ) << " " // 3
+ << filterprecision( m_COMTraj_deq[i].y[0] ) << " " // 4
+ << filterprecision( m_COMTraj_deq[i].yaw[0] ) << " " // 5
+ << filterprecision( m_LeftFootTraj_deq[i].x ) << " " // 6
+ << filterprecision( m_LeftFootTraj_deq[i].y ) << " " // 7
+ << filterprecision( m_LeftFootTraj_deq[i].theta * M_PI / 180 ) << " " // 8
+ << filterprecision( m_RightFootTraj_deq[i].x ) << " " // 9
+ << filterprecision( m_RightFootTraj_deq[i].y ) << " " // 10
+ << filterprecision( m_RightFootTraj_deq[i].theta * M_PI / 180 ) << " " // 11
+ << endl ;
+ }
+ aof.close();
- CoM_.setState(m_COMTraj_deq[m_numberOfSample + currentIndex - 1]);
+ iteration++;
- // RECOPIE DU BUFFER
- FinalCOMTraj_deq.resize( m_numberOfSample + currentIndex );
- FinalZMPTraj_deq.resize( m_numberOfSample + currentIndex );
- FinalLeftFootTraj_deq.resize( m_numberOfSample + currentIndex );
- FinalRightFootTraj_deq.resize( m_numberOfSample + currentIndex );
+ // DYNAMIC FILTER
+ // --------------
+ //DynamicFilter( m_ZMPTraj_deq, m_COMTraj_deq, m_LeftFootTraj_deq, m_RightFootTraj_deq, currentIndex );
- for (unsigned int i = currentIndex ; i < FinalZMPTraj_deq.size() ; i++ )
- {
- FinalZMPTraj_deq[i] = m_ZMPTraj_deq[i] ;
- FinalCOMTraj_deq[i] = m_COMTraj_deq[i] ;
- FinalLeftFootTraj_deq[i] = m_LeftFootTraj_deq[i] ;
- FinalRightFootTraj_deq[i] = m_RightFootTraj_deq[i] ;
- }
+ CoM_.setState(m_COMTraj_deq[m_numberOfSample + currentIndex - 1]);
- // Specify that we are in the ending phase.
- if (EndingPhase_ == false)
- {
- TimeToStopOnLineMode_ = UpperTimeLimitToUpdate_ + QP_T_ * QP_N_;
- }
- UpperTimeLimitToUpdate_ = UpperTimeLimitToUpdate_ + QP_T_;
+ // RECOPIE DU BUFFER
+ // -----------------
+ FinalCOMTraj_deq.resize( m_numberOfSample + currentIndex );
+ FinalZMPTraj_deq.resize( m_numberOfSample + currentIndex );
+ FinalLeftFootTraj_deq.resize( m_numberOfSample + currentIndex );
+ FinalRightFootTraj_deq.resize( m_numberOfSample + currentIndex );
+
+ for (unsigned int i = currentIndex ; i < FinalZMPTraj_deq.size() ; i++ )
+ {
+ FinalZMPTraj_deq[i] = m_ZMPTraj_deq[i] ;
+ FinalCOMTraj_deq[i] = m_COMTraj_deq[i] ;
+ FinalLeftFootTraj_deq[i] = m_LeftFootTraj_deq[i] ;
+ FinalRightFootTraj_deq[i] = m_RightFootTraj_deq[i] ;
+ }
+ // Specify that we are in the ending phase.
+ if (EndingPhase_ == false)
+ {
+ TimeToStopOnLineMode_ = UpperTimeLimitToUpdate_ + QP_T_ * QP_N_;
}
+ UpperTimeLimitToUpdate_ = UpperTimeLimitToUpdate_ + QP_T_;
+
+ }
//-----------------------------------
//
//
- //----------"Real-time" loop--------
+ //----------"Real-time" loop---------
}
@@ -669,6 +730,16 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
unsigned currentIndex
)
{
+ /// \brief Debug Purpose
+ /// --------------------
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+ static int iteration = 0;
+ int iteration100 = (int)iteration/100;
+ int iteration10 = (int)(iteration - iteration100*100)/10;
+ int iteration1 = (int)(iteration - iteration100*100 - iteration10*10 );
+
const unsigned int N = m_numberOfSample * QP_N_ ;
/// \brief calculate, from the CoM computed by the preview control,
/// the corresponding articular position, velocity and acceleration
@@ -686,187 +757,146 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
/// \brief Debug Purpose
/// --------------------
- ofstream aof6;
- string aFileName;
- static int iteration = 0;
- if (iteration == 0)
- {
- ofstream aof6;
- string aFileName;
- aFileName = "TestHerdt2010DynamicFilterArt.dat";
- aof6.open(aFileName.c_str(),ofstream::out);
- aof6.close();
- }
- aFileName = "TestHerdt2010DynamicFilterArt.dat";
- aof6.open(aFileName.c_str(),ofstream::app);
- aof6.precision(8);
- aof6.setf(ios::scientific, ios::floatfield);
- for(unsigned int i = 0 ; i < LeftFootAbsolutePositions.size() ; i++){
- aof6 << filterprecision( LeftFootAbsolutePositions [i].x ) << " " ; // 1;
- aof6 << filterprecision( LeftFootAbsolutePositions [i].y ) << " " ; // 1;
- aof6 << filterprecision( LeftFootAbsolutePositions [i].theta ) << " " ; // 1;
- aof6 << filterprecision( RightFootAbsolutePositions [i].x ) << " " ; // 1;
- aof6 << filterprecision( RightFootAbsolutePositions [i].y ) << " " ; // 1;
- aof6 << filterprecision( RightFootAbsolutePositions [i].theta ) << " " ; // 1;
- }
- aof6 << endl ;
- aof6.close();
-
- /// \brief rnea, calculation of the multi body ZMP
- /// ----------------------------------------------
- vector< vector<double> > ZMPMB ( N , vector<double> (2) );
- for (unsigned int i = 0 ; i < N ; i++ ){
- // Apply the RNEA to the metapod multibody and print the result in a log file.
- for(unsigned int j = 0 ; j < m_configurationTraj[i].size() ; j++ )
- {
- m_q(j,0) = m_configurationTraj[i](j) ;
- m_dq(j,0) = m_velocityTraj[i](j) ;
- m_ddq(j,0) = m_accelerationTraj[i](j) ;
+ oss.str("TestHerdt2010DynamicArticulation");
+ oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ ///----
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for(unsigned int i = 0 ; i < N ; i++){
+ for(unsigned int j = 0 ; j < m_configurationTraj[i].size() ; j++){
+ aof << filterprecision( m_configurationTraj[i](j) ) << " " ; // 1
}
- metapod::rnea< Robot_Model, true >::run(m_robot, m_q, m_dq, m_ddq);
- getTorques(m_robot, m_torques);
- m_allTorques[i] = m_torques ;
-
- Node & node = boost::fusion::at_c<Robot_Model::BODY>(m_robot.nodes);
- Force & aforce = node.joint.f ;
-
- ZMPMB[i][0] = - aforce.n()[1] / aforce.f()[2];
- ZMPMB[i][1] = aforce.n()[0] / aforce.f()[2] ;
- m_deltaZMPMBPositions[i].px = ZMPPositions[currentIndex+i].px - ( - aforce.n()[1] / aforce.f()[2] ) ;
- m_deltaZMPMBPositions[i].py = ZMPPositions[currentIndex+i].py - ( aforce.n()[0] / aforce.f()[2] ) ;
- m_deltaZMPMBPositions[i].pz = 0.0 ;
- m_deltaZMPMBPositions[i].theta = 0.0 ;
- m_deltaZMPMBPositions[i].time = m_CurrentTime + i * m_SamplingPeriod ;
- m_deltaZMPMBPositions[i].stepType = ZMPPositions[currentIndex+i].stepType ;
+ aof << endl ;
}
+ aof.close();
- /// \brief Debug Purpose
- /// --------------------
- ofstream aof5;
- if (iteration == 0)
- {
- ofstream aof5;
- string aFileName;
- aFileName = "TestHerdt2010DynamicDZMP.dat";
- aof5.open(aFileName.c_str(),ofstream::out);
- aof5.close();
- }
- aFileName = "TestHerdt2010DynamicDZMP.dat";
- aof5.open(aFileName.c_str(),ofstream::app);
- aof5.precision(8);
- aof5.setf(ios::scientific, ios::floatfield);
- for(unsigned int i = 0 ; i < m_deltaZMPMBPositions.size(); i++){
- aof5 << filterprecision( ZMPPositions[currentIndex+i].px ) << " " // 1
- << filterprecision( ZMPPositions[currentIndex+i].py ) << " " // 2
- << filterprecision( ZMPMB[i][0] ) << " " // 1
- << filterprecision( ZMPMB[i][1] ) << " " // 2
- << filterprecision( FinalCOMTraj_deq[currentIndex+i].x[0] ) << " " // 1
- << filterprecision( FinalCOMTraj_deq[currentIndex+i].y[0] ) << " " // 2
- << filterprecision( m_deltaZMPMBPositions[i].px ) << " " // 2
- << filterprecision( m_deltaZMPMBPositions[i].py ) << " " // 3
- << endl ;
- }
- aof5.close();
- /// \brief Debug Purpose
- /// --------------------
- if (iteration == 0)
- {
- ofstream aof5;
- string aFileName;
- aFileName = "TestHerdt2010DynamicFilterZMPPC.dat";
- aof5.open(aFileName.c_str(),ofstream::out);
- aof5.close();
+ if ( iteration == 0 ){
+ oss.str("/tmp/walkfwd_herdt.pos");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
}
- aFileName = "TestHerdt2010DynamicFilterZMPPC.dat";
- aof5.open(aFileName.c_str(),ofstream::app);
- aof5.precision(8);
- aof5.setf(ios::scientific, ios::floatfield);
- aof5 << filterprecision(ZMPPositions[currentIndex+N-1].px ) << " " // 1
- << filterprecision(ZMPPositions[currentIndex+N-1].py ) << " " // 2
- << filterprecision(ZMPMB[0][0] ) << " " // 1
- << filterprecision(ZMPMB[0][1]) << " " // 2
- << endl ;
- aof5.close();
-
- /// \brief Debug Purpose
- /// --------------------
- ofstream aof;
- if (iteration == 0)
- {
- ofstream aof;
- string aFileName;
- aFileName = "TestHerdt2010DynamicFilterPC.dat";
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
+ ///----
+ oss.str("/tmp/walkfwd_herdt.pos");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision( iteration * m_SamplingPeriod ) << " " ; // 1
+ for(unsigned int j = 6 ; j < m_numberOfSample ; j++){
+ for(unsigned int i = 6 ; i < m_configurationTraj[j].size() ; i++){
+ aof << filterprecision( m_configurationTraj[j](i) ) << " " ; // 1
+ }
}
- aFileName = "TestHerdt2010DynamicFilterPC.dat";
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- aof << filterprecision(iteration*0.005 ) << " " // 1
- << filterprecision(m_deltaZMPMBPositions[0].px ) << " " // 2
- << filterprecision(m_deltaZMPMBPositions[0].py ) << " " // 3
- << filterprecision(FinalCOMTraj_deq[currentIndex].x[0]) << " " // 4
- << filterprecision(FinalCOMTraj_deq[currentIndex].y[0]) << " " // 5
- << filterprecision(ZMPPositions[currentIndex].px) << " " // 6
- << filterprecision(ZMPPositions[currentIndex].py) << " " ; // 7
-
- /// Preview control on the ZMPMBs computed
- /// --------------------------------------
- //init of the Kajita preview control
- m_PC->SetPreviewControlTime (QP_T_*(QP_N_-1));
- m_PC->SetSamplingPeriod (m_SamplingPeriod);
- m_PC->SetHeightOfCoM(Robot_->CoMHeight());
- m_PC->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITHOUT_INITIALPOS);
- for(int j=0;j<3;j++)
- {
- m_deltax(j,0) = 0 ;
- m_deltay(j,0) = 0 ;
+ for(unsigned int i = 0 ; i < 10 ; i++){
+ aof << 0 << " " ;
}
+ aof << endl ;
+ aof.close();
- double aSxzmp (0) , aSyzmp(0);
- double deltaZMPx (0) , deltaZMPy (0) ;
- std::deque<COMState> COMStateBuffer (m_numberOfSample);
-
- // calcul of the preview control along the "ZMPPositions"
- for (unsigned i = 0 ; i < m_numberOfSample ; i++ )
- {
- m_PC->OneIterationOfPreview(m_deltax,m_deltay,
- aSxzmp,aSyzmp,
- m_deltaZMPMBPositions,i,
- deltaZMPx, deltaZMPy, false);
- for(int j=0;j<3;j++)
- {
- COMStateBuffer[i].x[j] = m_deltax(j,0);
- COMStateBuffer[i].y[j] = m_deltay(j,0);
- }
- }
+// /// \brief rnea, calculation of the multi body ZMP
+// /// ----------------------------------------------
+// vector< vector<double> > ZMPMB ( N , vector<double> (2) );
+// for (unsigned int i = 0 ; i < N ; i++ ){
+// // Apply the RNEA to the metapod multibody and print the result in a log file.
+// for(unsigned int j = 0 ; j < m_configurationTraj[i].size() ; j++ )
+// {
+// m_q(j,0) = m_configurationTraj[i](j) ;
+// m_dq(j,0) = m_velocityTraj[i](j) ;
+// m_ddq(j,0) = m_accelerationTraj[i](j) ;
+// }
+// metapod::rnea< Robot_Model, true >::run(m_robot, m_q, m_dq, m_ddq);
+// getTorques(m_robot, m_torques);
+// m_allTorques[i] = m_torques ;
+//
+// Node & node = boost::fusion::at_c<Robot_Model::BODY>(m_robot.nodes);
+// Force & aforce = node.joint.f ;
+//
+// ZMPMB[i][0] = - aforce.n()[1] / aforce.f()[2];
+// ZMPMB[i][1] = aforce.n()[0] / aforce.f()[2] ;
+// m_deltaZMPMBPositions[i].px = ZMPPositions[currentIndex+i].px - ( - aforce.n()[1] / aforce.f()[2] ) ;
+// m_deltaZMPMBPositions[i].py = ZMPPositions[currentIndex+i].py - ( aforce.n()[0] / aforce.f()[2] ) ;
+// m_deltaZMPMBPositions[i].pz = 0.0 ;
+// m_deltaZMPMBPositions[i].theta = 0.0 ;
+// m_deltaZMPMBPositions[i].time = m_CurrentTime + i * m_SamplingPeriod ;
+// m_deltaZMPMBPositions[i].stepType = ZMPPositions[currentIndex+i].stepType ;
+// }
- if (iteration == 0)
- {
- ofstream aof9;
- string aFileName;
- aFileName = "TestHerdt2010DynamicFilterBufferPC.dat";
- aof9.open(aFileName.c_str(),ofstream::out);
- aof9.close();
- }
- aFileName = "TestHerdt2010DynamicFilterBufferPC.dat";
- aof5.open(aFileName.c_str(),ofstream::app);
- aof5.precision(8);
- aof5.setf(ios::scientific, ios::floatfield);
- for(unsigned int i = 0 ; i < COMStateBuffer.size(); i++){
- aof5 << filterprecision( COMStateBuffer[i].x[0] ) << " " // 1
- << filterprecision( COMStateBuffer[i].x[1] ) << " " // 1
- << filterprecision( COMStateBuffer[i].x[2] ) << " " // 1
- << filterprecision( COMStateBuffer[i].y[0] ) << " " // 1
- << filterprecision( COMStateBuffer[i].y[1] ) << " " // 1
- << filterprecision( COMStateBuffer[i].y[2] ) << " " // 1
- << endl ;
- }
- aof5.close();
+// /// \brief Debug Purpose
+// /// --------------------
+// oss.str("TestHerdt2010DynamicDZMP");
+// oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
+// aFileName = oss.str();
+// aof.open(aFileName.c_str(),ofstream::out);
+// aof.close();
+// ///----
+// aof.open(aFileName.c_str(),ofstream::app);
+// aof.precision(8);
+// aof.setf(ios::scientific, ios::floatfield);
+// for(unsigned int i = 0 ; i < m_deltaZMPMBPositions.size() ; i++){
+// aof << filterprecision( m_deltaZMPMBPositions[i].px ) << " " // 1
+// << filterprecision( m_deltaZMPMBPositions[i].py ) << " " // 2
+// << filterprecision( ZMPPositions[currentIndex+i].px ) << " " // 3
+// << filterprecision( ZMPPositions[currentIndex+i].py ) << " " // 4
+// << filterprecision( ZMPMB[i][0] ) << " " // 5
+// << filterprecision( ZMPMB[i][1] ) << " " // 6
+// << endl ;
+// }
+// aof.close();
+
+// /// Preview control on the ZMPMBs computed
+// /// --------------------------------------
+// //init of the Kajita preview control
+// m_PC->SetPreviewControlTime (QP_T_*(QP_N_-1));
+// m_PC->SetSamplingPeriod (m_SamplingPeriod);
+// m_PC->SetHeightOfCoM(Robot_->CoMHeight());
+// m_PC->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITHOUT_INITIALPOS);
+// for(int j=0;j<3;j++)
+// {
+// m_deltax(j,0) = 0 ;
+// m_deltay(j,0) = 0 ;
+// }
+// double aSxzmp (0) , aSyzmp(0);
+// double deltaZMPx (0) , deltaZMPy (0) ;
+// std::deque<COMState> COMStateBuffer (m_numberOfSample);
+// // calcul of the preview control along the "ZMPPositions"
+// for (unsigned i = 0 ; i < m_numberOfSample ; i++ )
+// {
+// m_PC->OneIterationOfPreview(m_deltax,m_deltay,
+// aSxzmp,aSyzmp,
+// m_deltaZMPMBPositions,i,
+// deltaZMPx, deltaZMPy, false);
+// for(int j=0;j<3;j++)
+// {
+// COMStateBuffer[i].x[j] = m_deltax(j,0);
+// COMStateBuffer[i].y[j] = m_deltay(j,0);
+// }
+// }
+//
+// /// \brief Debug Purpose
+// /// --------------------
+// oss.str("TestHerdt2010DynamicFilterBufferPC");
+// oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
+// aFileName = oss.str();
+// aof.open(aFileName.c_str(),ofstream::out);
+// aof.close();
+// ///----
+// aof.open(aFileName.c_str(),ofstream::app);
+// aof.precision(8);
+// aof.setf(ios::scientific, ios::floatfield);
+// for(unsigned int i = 0 ; i < COMStateBuffer.size() ; i++){
+// aof << filterprecision( COMStateBuffer[i].x[0] ) << " " // 2
+// << filterprecision( COMStateBuffer[i].y[0] ) << " " // 3
+// << endl ;
+// }
+// aof.close();
// for (unsigned int i = 0 ; i < m_numberOfSample ; i++)
// {
@@ -876,12 +906,7 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
// FinalCOMTraj_deq[currentIndex+i].y[j] -= COMStateBuffer[i].y[j] ;
// }
// }
- aof << filterprecision(COMStateBuffer[0].x[0]) << " " // 8
- << filterprecision(COMStateBuffer[0].x[0]) << " " // 9
- << filterprecision(FinalCOMTraj_deq[currentIndex].x[0]) << " " // 10
- << filterprecision(FinalCOMTraj_deq[currentIndex].y[0]) << " " // 11
- << endl ;
- aof.close();
+
iteration++;
return 0;
@@ -940,39 +965,43 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(COMState &acomp,
aRightFootPosition(3) = aRightFAP.theta;
aRightFootPosition(4) = aRightFAP.omega;
- /* Get the current configuration vector */
- CurrentConfiguration = HDR_->currentConfiguration();
-
- /* Get the current velocity vector */
- CurrentVelocity = HDR_->currentVelocity();
-
- /* Get the current acceleration vector */
- CurrentAcceleration = HDR_->currentAcceleration();
-
+ if (IterationNumber == 0){
+ CurrentConfiguration = m_QP_T_Configuration ;
+ CurrentVelocity = m_QP_T_previousVelocity ;
+ CurrentAcceleration = m_QP_T_previousAcceleration ;
+ }else{
+ CurrentConfiguration = m_previousConfiguration ;
+ CurrentVelocity = m_previousVelocity ;
+ CurrentAcceleration = m_previousAcceleration ;
+ }
/// \brief Debug Purpose
/// --------------------
ofstream aof6;
string aFileName;
ostringstream oss(std::ostringstream::ate);
static int iteration = 0;
+ int iteration100 = (int)iteration/100;
+ int iteration10 = (int)(iteration - iteration100*100)/10;
+ int iteration1 = (int)(iteration - iteration100*100 - iteration10*10 );
if (IterationNumber == 0)
{
ofstream aof6;
string aFileName;
oss.str("TestHerdt2010DynamicFilterArt2");
- oss << "_" << iteration << ".dat";
+
+ oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
aFileName = oss.str();
aof6.open(aFileName.c_str(),ofstream::out);
aof6.close();
oss.str("TestHerdt2010COMFeet");
- oss << "_" << iteration << ".dat";
+ oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
aFileName = oss.str();
aof6.open(aFileName.c_str(),ofstream::out);
aof6.close();
}
oss.str("TestHerdt2010COMFeet");
- oss << "_" << iteration<< ".dat";
+ oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
aFileName = oss.str();
aof6.open(aFileName.c_str(),ofstream::app);
aof6.precision(8);
@@ -989,15 +1018,6 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(COMState &acomp,
aof6 << endl ;
aof6.close();
- static int StageOfTheAlgorithm = 0 ;
- if (StageOfTheAlgorithm == 0)
- {
- ComAndFootRealization_->setHumanoidDynamicRobot(HDR_);
- ComAndFootRealization_->SetHeightOfTheCoM(0.814);
- ComAndFootRealization_->setSamplingPeriod(0.005);
- ComAndFootRealization_->SetStepStackHandler(new StepStackHandler(new SimplePluginManager()));
- ComAndFootRealization_->Initialization();
- }
ComAndFootRealization_->ComputePostureForGivenCoMAndFeetPosture(aCOMState, aCOMSpeed, aCOMAcc,
aLeftFootPosition,
aRightFootPosition,
@@ -1005,22 +1025,18 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(COMState &acomp,
CurrentVelocity,
CurrentAcceleration,
IterationNumber,
- 1);
- StageOfTheAlgorithm = 1 ;
-
- oss.str("TestHerdt2010DynamicFilterArt2");
- oss << "_" << iteration << ".dat";
- aFileName = oss.str();
- aof6.open(aFileName.c_str(),ofstream::app);
- aof6.precision(8);
- aof6.setf(ios::scientific, ios::floatfield);
- for(unsigned int i = 0 ; i < CurrentConfiguration.size() ; i++){
- aof6 << filterprecision( CurrentConfiguration(i) ) << " " ; // 1;
- }
- aof6 << endl ;
- aof6.close();
+ 0);
if (IterationNumber==(int)m_numberOfSample*QP_N_-1)
iteration++;
+ if(IterationNumber == m_numberOfSample-1 ){
+ m_QP_T_Configuration = CurrentConfiguration ;
+ m_QP_T_previousVelocity = CurrentVelocity ;
+ m_QP_T_previousAcceleration = CurrentAcceleration ;
+ }else{
+ m_previousConfiguration = CurrentConfiguration ;
+ m_previousVelocity = CurrentVelocity ;
+ m_previousAcceleration = CurrentAcceleration ;
+ }
}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index 8802de6d..ace4550d 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -67,8 +67,7 @@ namespace PatternGeneratorJRL
public:
ZMPVelocityReferencedQP(SimplePluginManager *SPM, string DataFile,
- CjrlHumanoidDynamicRobot *aHS=0,
- ComAndFootRealization * ComAndFootRealization=0);
+ CjrlHumanoidDynamicRobot *aHS=0 );
~ZMPVelocityReferencedQP();
@@ -191,7 +190,8 @@ namespace PatternGeneratorJRL
int QP_N_;
/// \brief 2D LIPM to simulate the evolution of the robot's CoM.
- LinearizedInvertedPendulum2D CoM_, CoM2_;
+ LinearizedInvertedPendulum2D CoM_ ;
+ LinearizedInvertedPendulum2D CoM2_ ;
/// \brief Simplified robot model
RigidBodySystem * Robot_ ;
@@ -217,6 +217,9 @@ namespace PatternGeneratorJRL
/// \brief Previewed Solution
solution_t Solution_;
+ /// \brief StepStackHandler for ComAndFootRealization object
+ StepStackHandler* SSH_;
+
/// \brief Store a reference to the object to solve posture resolution.
ComAndFootRealization * ComAndFootRealization_;
@@ -235,19 +238,38 @@ namespace PatternGeneratorJRL
deque<COMState> m_COMTraj_deq ;
deque<FootAbsolutePosition> m_LeftFootTraj_deq ;
deque<FootAbsolutePosition> m_RightFootTraj_deq ;
+
+ /// \brief Number of interpolated point computed during QP_T_ (27/02/2014 :0.1)
unsigned m_numberOfSample ;
+
+ /// \brief Buffers for the CoM an Feet computation, i.e. the simplify inverse kinematics.
vector <MAL_VECTOR_TYPE(double)> m_configurationTraj ;
vector <MAL_VECTOR_TYPE(double)> m_velocityTraj ;
vector <MAL_VECTOR_TYPE(double)> m_accelerationTraj ;
+ MAL_VECTOR_TYPE(double) m_previousConfiguration ;
+ MAL_VECTOR_TYPE(double) m_previousVelocity ;
+ MAL_VECTOR_TYPE(double) m_previousAcceleration ;
+ MAL_VECTOR_TYPE(double) m_QP_T_Configuration ;
+ MAL_VECTOR_TYPE(double) m_QP_T_previousVelocity ;
+ MAL_VECTOR_TYPE(double) m_QP_T_previousAcceleration ;
+
+ /// \brief Buffer for the torques computed by RNEA algorithme from Featherstone
vector < Robot_Model::confVector,Eigen::aligned_allocator<Robot_Model::confVector> > m_allTorques ;
+
+ /// \brief Buffer comtaimimg the difference between the ZMP computed from the Herdt controler\
+ and the ZMP Multibody computed from the articular trajectory
std::deque<ZMPPosition> m_deltaZMPMBPositions ;
- // Set configuration vectors (q, dq, ddq, torques) to reference values.
+
+ /// \brief Set configuration vectors (q, dq, ddq, torques) to reference values.
Robot_Model::confVector m_torques, m_q, m_dq, m_ddq;
- // Initialize the robot with the autogenerated files by MetapodFromUrdf
+
+ /// \brief Initialize the robot with the autogenerated files by MetapodFromUrdf
Robot_Model m_robot;
+
/// \brief Standard polynomial trajectories for the feet.
OnLineFootTrajectoryGeneration * OFTG_;
+
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // to use the vector of eigen used by metapod
diff --git a/src/patterngeneratorinterfaceprivate.hh b/src/patterngeneratorinterfaceprivate.hh
index b80f7ed5..3baa976d 100644
--- a/src/patterngeneratorinterfaceprivate.hh
+++ b/src/patterngeneratorinterfaceprivate.hh
@@ -647,7 +647,7 @@ namespace PatternGeneratorJRL
/*! Objet to realize the generate the posture for given CoM
and feet positions. */
- ComAndFootRealization * m_ComAndFootRealization;
+ std::vector<ComAndFootRealization *> m_ComAndFootRealization;
/* \name Object related to stepping over.
@{
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index dca437ca..a041a288 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -114,6 +114,7 @@ MACRO(ADD_HERDT_2010 test_herdt2010_arg)
)
TARGET_LINK_LIBRARIES(${test_herdt2010_arg} ${PROJECT_NAME})
PKG_CONFIG_USE_DEPENDENCY(${test_herdt2010_arg} jrl-dynamics)
+ PKG_CONFIG_USE_DEPENDENCY(${test_herdt2010_arg} hrp2-dynamics)
ADD_DEPENDENCIES(${test_herdt2010_arg} ${PROJECT_NAME})
MESSAGE(STATUS "jrl data dir: " ${JRL_DYNAMICS_PKGDATAROOTDIR})
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index c7b4aee6..241f90ce 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -29,6 +29,8 @@
#include "CommonTools.hh"
#include "TestObject.hh"
#include <jrl/walkgen/pgtypes.hh>
+#include <hrp2-dynamics/hrp2OptHumanoidDynamicRobot.h>
+#include <MotionGeneration/ComAndFootRealizationByGeometry.hh>
#include <metapod/models/hrp2_14/hrp2_14.hh>
#ifndef METAPOD_INCLUDES
@@ -48,7 +50,6 @@ typedef metapod::hrp2_14<LocalFloatType2> Robot_Model2;
typedef metapod::Nodes< Robot_Model2, Robot_Model2::BODY >::type Node2;
#endif
-#include <MotionGeneration/ComAndFootRealizationByGeometry.hh>
using namespace::PatternGeneratorJRL;
using namespace::PatternGeneratorJRL::TestSuite;
@@ -220,6 +221,14 @@ private:
protected:
+ void SpecializedRobotConstructor( CjrlHumanoidDynamicRobot *& aHDR, CjrlHumanoidDynamicRobot *& aDebugHDR )
+ {
+ dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
+ Chrp2OptHumanoidDynamicRobot *aHRP2HDR = new Chrp2OptHumanoidDynamicRobot( &aRobotDynamicsObjectConstructor );
+ aHDR = aHRP2HDR;
+ aDebugHDR = new Chrp2OptHumanoidDynamicRobot(&aRobotDynamicsObjectConstructor);
+ }
+
double filterprecision(double adb)
{
if (fabs(adb)<1e-7)
@@ -506,9 +515,9 @@ protected:
#define localNbOfEvents 12
struct localEvent events [localNbOfEvents] =
- { { 5*200,&TestHerdt2010::walkForward},
+ { {5*200,&TestHerdt2010::walkForward},
// {10*200,&TestHerdt2010::walkSidewards},
- // {25*200,&TestHerdt2010::startTurningRightOnSpot},
+ {15*200,&TestHerdt2010::startTurningRightOnSpot},
// {35*200,&TestHerdt2010::walkForward},
// {45*200,&TestHerdt2010::startTurningLeftOnSpot},
// {55*200,&TestHerdt2010::walkForward},
@@ -516,8 +525,8 @@ protected:
// {75*200,&TestHerdt2010::walkForward},
// {85*200,&TestHerdt2010::startTurningLeft},
// {95*200,&TestHerdt2010::startTurningRight},
- {15*200,&TestHerdt2010::stop},
- {20.5*200,&TestHerdt2010::stopOnLineWalking}
+ {25*200,&TestHerdt2010::stop},
+ {27.5*200,&TestHerdt2010::stopOnLineWalking}
};
// Test when triggering event.
diff --git a/tests/TestKajita2003.cpp b/tests/TestKajita2003.cpp
index 3ca9261d..84644446 100644
--- a/tests/TestKajita2003.cpp
+++ b/tests/TestKajita2003.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2010,
+ * Copyright 2010,
*
* Olivier Stasse
*
@@ -18,7 +18,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* \file This file tests A. Herdt's walking algorithm for
@@ -78,7 +78,7 @@ protected:
istringstream strm2(":arc 0.0 0.75 30.0 -1");
aPGI.ParseCmd(strm2);
}
-
+
{
istringstream strm2(":lastsupport");
aPGI.ParseCmd(strm2);
@@ -99,7 +99,7 @@ protected:
istringstream strm2(":SetAlgoForZmpTrajectory Kajita");
aPGI.ParseCmd(strm2);
}
-
+
{
istringstream strm2(":stepseq 0.0 -0.105 0.0 \
0.2 0.21 0.0 \
@@ -219,13 +219,13 @@ protected:
0 -0.2 0 ");
aPGI.ParseCmd(strm2);
}
-
+
}
void chooseTestProfile()
{
-
+
switch(m_TestProfile)
{
@@ -246,7 +246,7 @@ protected:
break;
}
}
-
+
void generateEvent()
{
}
@@ -256,11 +256,11 @@ int PerformTests(int argc, char *argv[])
{
std::string TestNames[4] = { "TestKajita2003StraightWalking",
- "TestKajita2003Circle",
+ "TestKajita2003Circle",
"TestKajita2003PbFlorentSeq1",
"TestKajita2003PbFlorentSeq2"};
int TestProfiles[4] = { PROFIL_STRAIGHT_WALKING,
- PROFIL_CIRCLE,
+ PROFIL_CIRCLE,
PROFIL_PB_FLORENT_SEQ1,
PROFIL_PB_FLORENT_SEQ2};
diff --git a/tests/TestObject.cpp b/tests/TestObject.cpp
index a19271f2..3e3aefd0 100644
--- a/tests/TestObject.cpp
+++ b/tests/TestObject.cpp
@@ -360,7 +360,7 @@ namespace PatternGeneratorJRL
<< filterprecision(m_OneStep.LeftFootPosition.ddx ) << " " // 17
<< filterprecision(m_OneStep.LeftFootPosition.ddy ) << " " // 18
<< filterprecision(m_OneStep.LeftFootPosition.ddz ) << " " // 19
- << filterprecision(m_OneStep.LeftFootPosition.theta ) << " " // 20
+ << filterprecision(m_OneStep.LeftFootPosition.theta * M_PI / 180 ) << " " // 20
<< filterprecision(m_OneStep.LeftFootPosition.omega ) << " " // 21
<< filterprecision(m_OneStep.LeftFootPosition.omega2 ) << " " // 22
<< filterprecision(m_OneStep.RightFootPosition.x ) << " " // 23
@@ -372,20 +372,61 @@ namespace PatternGeneratorJRL
<< filterprecision(m_OneStep.RightFootPosition.ddx ) << " " // 29
<< filterprecision(m_OneStep.RightFootPosition.ddy ) << " " // 30
<< filterprecision(m_OneStep.RightFootPosition.ddz ) << " " // 31
- << filterprecision(m_OneStep.RightFootPosition.theta ) << " " // 32
+ << filterprecision(m_OneStep.RightFootPosition.theta* M_PI / 180 ) << " " // 32
<< filterprecision(m_OneStep.RightFootPosition.omega ) << " " // 33
<< filterprecision(m_OneStep.RightFootPosition.omega2 ) << " " // 34
<< filterprecision(m_OneStep.ZMPTarget(0)*cos(m_CurrentConfiguration(5)) -
m_OneStep.ZMPTarget(1)*sin(m_CurrentConfiguration(5))
- +m_CurrentConfiguration(0) ) << " " // 35
+ +m_CurrentConfiguration(0) ) << " " // 35
<< filterprecision(m_OneStep.ZMPTarget(0)*sin(m_CurrentConfiguration(5)) +
m_OneStep.ZMPTarget(1)*cos(m_CurrentConfiguration(5))
- +m_CurrentConfiguration(1) ) << " " // 36
+ +m_CurrentConfiguration(1) ) << " " // 36
<< filterprecision(m_CurrentConfiguration(0) ) << " " // 37
- << filterprecision(m_CurrentConfiguration(1) ) << " " // 38
- << endl;
+ << filterprecision(m_CurrentConfiguration(1) ) << " "; // 38
+ for (unsigned int i = 0 ; i < m_CurrentConfiguration.size() ; i++)
+ {
+ aof << filterprecision(m_CurrentConfiguration(i)) << " " ; // 39 - 74
+ }
+ aof << endl;
aof.close();
}
+
+
+// /// \brief Debug Purpose
+// /// --------------------
+// ofstream aof;
+// string aFileName;
+// ostringstream oss(std::ostringstream::ate);
+// static int iteration = 0;
+// int iteration100 = (int)iteration/100;
+// int iteration10 = (int)(iteration - iteration100*100)/10;
+// int iteration1 = (int)(iteration - iteration100*100 - iteration10*10 );
+//
+//
+// if ( iteration == 0 ){
+// oss.str("/tmp/walkfwd_kajita.pos");
+// aFileName = oss.str();
+// aof.open(aFileName.c_str(),ofstream::out);
+// aof.close();
+// }
+// ///----
+// oss.str("/tmp/walkfwd_kajita.pos");
+// aFileName = oss.str();
+// aof.open(aFileName.c_str(),ofstream::app);
+// aof.precision(8);
+// aof.setf(ios::scientific, ios::floatfield);
+// aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+// for(unsigned int i = 6 ; i < m_CurrentConfiguration.size() ; i++){
+// aof << filterprecision( m_CurrentConfiguration(i) ) << " " ; // 1
+// }
+// for(unsigned int i = 0 ; i < 10 ; i++){
+// aof << 0.0 << " " ;
+// }
+// aof << endl ;
+// aof.close();
+//
+// iteration++;
+
}
--
GitLab