From ac3acd660bff4adb2ef2a7629a208719e87ce43a Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Tue, 17 Jun 2014 19:29:14 +0200
Subject: [PATCH] preparing the PG of Morisawa for the dynamic filter
---
src/PatternGeneratorInterfacePrivate.cpp | 5 +-
.../AnalyticalMorisawaCompact.cpp | 20 +-
.../AnalyticalMorisawaCompact.hh | 13 +-
.../DynamicFilter.cpp | 25 +-
.../DynamicFilter.hh | 6 +-
.../ZMPVelocityReferencedQP.hh | 2 -
tests/CMakeLists.txt | 5 +-
tests/TestHerdt2010DynamicFilter.cpp | 8 +-
tests/TestMorisawa2007.cpp | 517 ++++++++++++++++++
9 files changed, 575 insertions(+), 26 deletions(-)
diff --git a/src/PatternGeneratorInterfacePrivate.cpp b/src/PatternGeneratorInterfacePrivate.cpp
index b63a0e5d..28be9cb2 100644
--- a/src/PatternGeneratorInterfacePrivate.cpp
+++ b/src/PatternGeneratorInterfacePrivate.cpp
@@ -228,7 +228,7 @@ 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.resize(2);
+ m_ComAndFootRealization.resize(3);
m_ComAndFootRealization[0] = new ComAndFootRealizationByGeometry(this);
// Creates the foot trajectory generator.
@@ -249,8 +249,9 @@ namespace PatternGeneratorJRL {
m_ComAndFootRealization[1] = m_ZMPVRQP->getComAndFootRealization();
// ZMP and CoM generation using the analytical method proposed in Morisawa2007.
- m_ZMPM = new AnalyticalMorisawaCompact(this);
+ m_ZMPM = new AnalyticalMorisawaCompact(this,m_HumanoidDynamicRobot);
m_ZMPM->SetHumanoidSpecificities(m_HumanoidDynamicRobot);
+ m_ComAndFootRealization[2] = m_ZMPM->getComAndFootRealization();
// Preview control for a 3D Linear inverse pendulum
m_PC = new PreviewControl(this,OptimalControllerSolver::MODE_WITHOUT_INITIALPOS,true);
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index fdf71038..25c00c43 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -75,7 +75,7 @@ namespace PatternGeneratorJRL
{
- AnalyticalMorisawaCompact::AnalyticalMorisawaCompact(SimplePluginManager *lSPM)
+ AnalyticalMorisawaCompact::AnalyticalMorisawaCompact(SimplePluginManager *lSPM , CjrlHumanoidDynamicRobot *aHS)
: AnalyticalMorisawaAbstract(lSPM)
{
@@ -87,7 +87,7 @@ namespace PatternGeneratorJRL
memset(&m_CTIPY,0,sizeof(m_CTIPY));
m_OnLineChangeStepMode = ABSOLUTE_FRAME;
- m_HS = 0;
+ m_HS = aHS ;
m_FeetTrajectoryGenerator =
m_BackUpm_FeetTrajectoryGenerator = 0;
@@ -115,6 +115,8 @@ namespace PatternGeneratorJRL
m_NewStepInTheStackOfAbsolutePosition = false;
+ m_kajitaDynamicFilter = new DynamicFilter(lSPM,m_HS);
+
m_FilteringActivate = true;
RESETDEBUG4("Test.dat");
}
@@ -641,6 +643,14 @@ namespace PatternGeneratorJRL
/*! Recompute time when a new step should be added. */
m_UpperTimeLimitToUpdateStacks = m_AbsoluteTimeReference + m_DeltaTj[0] + m_Tdble + 0.45 * m_Tsingle;
+ m_kajitaDynamicFilter->init(m_CurrentTime,
+ m_SamplingPeriod,
+ m_SamplingPeriod,
+ m_SamplingPeriod,
+ m_PreviewControlTime,
+ lStartingCOMState.z[0]
+ );
+
return m_RelativeFootPositions.size();
}
@@ -684,6 +694,8 @@ namespace PatternGeneratorJRL
}
}
+ std::deque<COMState> deltaCoMTraj_deq (1);
+ //m_kajitaDynamicFilter->filter(,,,,deltaCoMTraj_deq);
/*! Feed the ZMPPositions. */
double lZMPPosx=0.0,lZMPPosy=0.0;
@@ -700,8 +712,8 @@ namespace PatternGeneratorJRL
m_AnalyticalZMPCoGTrajectoryX->ComputeCOMSpeed(time,lCOMPosdx,lIndexInterval);
m_AnalyticalZMPCoGTrajectoryY->ComputeCOM(time,lCOMPosy,lIndexInterval);
m_AnalyticalZMPCoGTrajectoryY->ComputeCOMSpeed(time,lCOMPosdy,lIndexInterval);
- aCOMPos.x[0] += lCOMPosx; aCOMPos.x[1] += lCOMPosdx;
- aCOMPos.y[0] += lCOMPosy; aCOMPos.y[1] += lCOMPosdy;
+ aCOMPos.x[0] += lCOMPosx + deltaCoMTraj_deq[0].x[0] ; aCOMPos.x[1] += lCOMPosdx + deltaCoMTraj_deq[0].x[1];
+ aCOMPos.y[0] += lCOMPosy + deltaCoMTraj_deq[0].y[0] ; aCOMPos.y[1] += lCOMPosdy + deltaCoMTraj_deq[0].y[1];
aCOMPos.z[0] = m_InitialPoseCoMHeight;
FinalCOMStates.push_back(aCOMPos);
/*! Feed the FootPositions. */
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh
index 3e72b26b..a97ff466 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh
@@ -26,7 +26,7 @@
\brief Compact form of the analytical solution to generate the ZMP and the CoM.
This object generate the reference value for the
- ZMP based on a polynomail representation
+ ZMP based on a polynomial representation
of the ZMP following
"Experimentation of Humanoid Walking Allowing Immediate
Modification of Foot Place Based on Analytical Solution"
@@ -46,7 +46,7 @@
#include <ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.hh>
#include <ZMPRefTrajectoryGeneration/FilteringAnalyticalTrajectoryByPreviewControl.hh>
#include <FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh>
-
+#include <ZMPRefTrajectoryGeneration/DynamicFilter.hh>
namespace PatternGeneratorJRL
{
@@ -106,7 +106,7 @@ namespace PatternGeneratorJRL
const static unsigned int RELATIVE_FRAME = 1;
/*! @} */
/*! Constructor */
- AnalyticalMorisawaCompact(SimplePluginManager * lSPM);
+ AnalyticalMorisawaCompact(SimplePluginManager * lSPM , CjrlHumanoidDynamicRobot *aHS);
/*! Destructor */
virtual ~AnalyticalMorisawaCompact();
@@ -414,6 +414,10 @@ namespace PatternGeneratorJRL
/*! Get the feet trajectory generator */
LeftAndRightFootTrajectoryGenerationMultiple * GetFeetTrajectoryGenerator();
+ /*! Setter and getter for the ComAndZMPTrajectoryGeneration. */
+ inline ComAndFootRealization * getComAndFootRealization()
+ { return m_kajitaDynamicFilter->getComAndFootRealization();};
+
/*! @} */
/*! Simple plugin interfaces
@@ -626,7 +630,8 @@ namespace PatternGeneratorJRL
/*! \brief Filtering the axis through a preview control. */
FilteringAnalyticalTrajectoryByPreviewControl * m_FilterXaxisByPC, * m_FilterYaxisByPC;
-
+ DynamicFilter * m_kajitaDynamicFilter ;
+
/*! \brief Activate or desactivate the filtering. */
bool m_FilteringActivate;
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index d36ddae8..293a5b25 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -97,12 +97,16 @@ void DynamicFilter::init(
PC_->SetSamplingPeriod (interpolationPeriod);
PC_->SetHeightOfCoM(CoMHeight_);
+ previousConfiguration_ = comAndFootRealization_->getHumanoidDynamicRobot()->currentConfiguration() ;
+ previousVelocity_ = comAndFootRealization_->getHumanoidDynamicRobot()->currentVelocity() ;
+ previousAcceleration_ = comAndFootRealization_->getHumanoidDynamicRobot()->currentAcceleration() ;
+
configurationTraj_.resize( PG_N_ * NbI_, previousConfiguration_ ); ;
velocityTraj_.resize( PG_N_ * NbI_, previousVelocity_ ); ;
accelerationTraj_.resize( PG_N_ * NbI_, previousAcceleration_ ); ;
deltaZMP_deq_.resize( PG_N_ * NbI_);
- ZMPMB_vec_.resize( PG_N_ * NbI_);
+ ZMPMB_vec_.resize( PG_N_ * NbI_ , vector<double>(2));
comAndFootRealization_->setSamplingPeriod(interpolationPeriod_);
comAndFootRealization_->Initialization();
@@ -129,9 +133,13 @@ int DynamicFilter::filter(
inputCOMTraj_deq_,
inputLeftFootTraj_deq_,
inputRightFootTraj_deq_);
+
+ printDebug();
+
InverseDynamics(inputZMPTraj_deq_);
+
int error = OptimalControl(outputDeltaCOMTraj_deq_);
- //printDebug();
+
return error ;
}
@@ -141,7 +149,7 @@ void DynamicFilter::InverseKinematics(
deque<FootAbsolutePosition> & inputRightFootTraj_deq_)
{
const int stage0 = 0 ;
- const unsigned int N = inputCOMTraj_deq_.size();
+ const unsigned int N = PG_N_ * NbI_ ;
int iteration = 2 ;
comAndFootRealization_->SetPreviousConfigurationStage0(
previousConfiguration_);
@@ -216,7 +224,7 @@ void DynamicFilter::InverseKinematics(
void DynamicFilter::InverseDynamics(
deque<ZMPPosition> inputZMPTraj_deq_)
{
- const unsigned int N = inputZMPTraj_deq_.size();
+ const unsigned int N = PG_N_ * NbI_ ;
for (unsigned int i = 0 ; i < N ; i++ )
{
// Copy the angular trajectory data from "Boost" to "Eigen"
@@ -258,6 +266,9 @@ void DynamicFilter::InverseDynamics(
int DynamicFilter::OptimalControl(
deque<COMState> & outputDeltaCOMTraj_deq_)
{
+ if(!PC_->IsCoherent())
+ PC_->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITH_INITIALPOS);
+
double aSxzmp (0) , aSyzmp(0);
double deltaZMPx (0) , deltaZMPy (0) ;
@@ -318,9 +329,9 @@ void DynamicFilter::printDebug()
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 );
+ int iteration100 = (int)iteration/100;
+ int iteration10 = (int)(iteration - iteration100*100)/10;
+ int iteration1 = (int)(iteration - iteration100*100 - iteration10*10 );
/// \brief Debug Purpose
/// --------------------
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
index b03b9dc6..46ca4830 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
@@ -19,6 +19,10 @@ namespace PatternGeneratorJRL
class DynamicFilter
{
public: // Public methods
+
+ // to use the vector of eigen used by metapod
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
/// \brief
DynamicFilter(SimplePluginManager *SPM,
CjrlHumanoidDynamicRobot *aHS);
@@ -140,7 +144,7 @@ namespace PatternGeneratorJRL
Force_HRP2_14 m_force ;
/// \brief Set of configuration vectors (q, dq, ddq, torques)
- Robot_Model::confVector m_torques, m_q, m_dq, m_ddq;
+ Robot_Model::confVector m_q, m_dq, m_ddq;
/// \brief Used to eliminate the initiale difference between
/// the zmp and the zmpmb
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index 97500fc2..f21e9854 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -42,7 +42,6 @@
#include <ZMPRefTrajectoryGeneration/generator-vel-ref.hh>
#include <Mathematics/intermediate-qp-matrices.hh>
#include <jrl/walkgen/pgtypes.hh>
-#include <MotionGeneration/ComAndFootRealizationByGeometry.hh>
#include <ZMPRefTrajectoryGeneration/DynamicFilter.hh>
namespace PatternGeneratorJRL
@@ -274,7 +273,6 @@ namespace PatternGeneratorJRL
DynamicFilter * dynamicFilter_ ;
public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW // to use the vector of eigen used by metapod
void GetZMPDiscretization(std::deque<ZMPPosition> & ZMPPositions,
std::deque<COMState> & COMStates,
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index a041a288..dc704fc0 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -73,6 +73,7 @@ MACRO(ADD_MORISAWA_2007 test_morisawa_arg)
)
TARGET_LINK_LIBRARIES(${testmorisawa2007} ${PROJECT_NAME})
PKG_CONFIG_USE_DEPENDENCY(${testmorisawa2007} jrl-dynamics)
+ PKG_CONFIG_USE_DEPENDENCY(${testmorisawa2007} hrp2-dynamics)
ADD_DEPENDENCIES(${testmorisawa2007} ${PROJECT_NAME})
MESSAGE(STATUS "jrl data dir: " ${JRL_DYNAMICS_PKGDATAROOTDIR})
@@ -95,8 +96,8 @@ MACRO(ADD_MORISAWA_2007 test_morisawa_arg)
${samplemodelpath} sample.wrl ${samplespec} ${sampleljr} ${sampleinitconfig})
ENDMACRO(ADD_MORISAWA_2007)
-#ADD_MORISAWA_2007(TestMorisawa2007OnLine)
-#ADD_MORISAWA_2007(TestMorisawa2007ShortWalk)
+ADD_MORISAWA_2007(TestMorisawa2007OnLine)
+ADD_MORISAWA_2007(TestMorisawa2007ShortWalk)
###################
# Test Herdt 2010 #
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index 7e0c82a8..5a8ca6df 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -60,8 +60,6 @@ enum Profiles_t {
PROFIL_HERDT_EMERGENCY_STOP // 2
};
-bool ONCE = true ;
-
class TestHerdt2010: public TestObject
{
@@ -73,6 +71,7 @@ private:
SimplePluginManager * SPM ;
double dInitX, dInitY;
int iteration,iteration_zmp ;
+ bool once ;
public:
TestHerdt2010(int argc, char *argv[], string &aString, int TestProfile):
@@ -91,6 +90,7 @@ private:
dInitY = 0 ;
iteration_zmp = 0 ;
iteration = 0 ;
+ once = true ;
};
~TestHerdt2010()
@@ -527,11 +527,11 @@ protected:
// cout << "ecartmaxY :" << ecartmaxY << endl ;
// Writing of the two zmps and the error.
- if (ONCE)
+ if (once)
{
aof.open("TestHerdt2010ErrorZMP.dat",ofstream::out);
aof.close();
- ONCE = false ;
+ once = false ;
}
aof.open("TestHerdt2010ErrorZMP.dat",ofstream::app);
aof.precision(8);
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index 93a494eb..3c423ec4 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -28,6 +28,26 @@
#include "CommonTools.hh"
#include "TestObject.hh"
+#include <hrp2-dynamics/hrp2OptHumanoidDynamicRobot.h>
+#include <MotionGeneration/ComAndFootRealizationByGeometry.hh>
+#include <metapod/models/hrp2_14/hrp2_14.hh>
+#ifndef METAPOD_INCLUDES
+#define METAPOD_INCLUDES
+// metapod includes
+#include <metapod/tools/print.hh>
+#include <metapod/tools/initconf.hh>
+#include <metapod/algos/rnea.hh>
+#include <Eigen/StdVector>
+#endif
+
+#ifndef METAPOD_TYPEDEF2
+#define METAPOD_TYPEDEF2
+typedef double LocalFloatType2;
+typedef metapod::Spatial::ForceTpl<LocalFloatType2> Force2;
+typedef metapod::hrp2_14<LocalFloatType2> Robot_Model2;
+typedef metapod::Nodes< Robot_Model2, Robot_Model2::BODY >::type Node2;
+#endif
+
using namespace::PatternGeneratorJRL;
using namespace::PatternGeneratorJRL::TestSuite;
using namespace std;
@@ -62,6 +82,16 @@ private:
unsigned long int m_NbStepsModified;
// New time between two steps.
double m_deltatime;
+
+ // buffer to save all the zmps positions
+ vector< vector<double> > errZMP_ ;
+ Robot_Model2 robot_ ;
+ ComAndFootRealization * ComAndFootRealization_;
+ SimplePluginManager * SPM ;
+ double dInitX, dInitY;
+ int iteration,iteration_zmp ;
+ bool once ;
+
public:
TestMorisawa2007(int argc, char*argv[], string &aString, int TestProfile):
TestObject(argc, argv, aString)
@@ -70,10 +100,497 @@ public:
m_TestChangeFoot = true;
m_NbStepsModified = 0;
m_deltatime = 0;
+
+ ComAndFootRealization_ = 0 ;
+ dInitX = 0 ;
+ dInitY = 0 ;
+ iteration_zmp = 0 ;
+ iteration = 0 ;
+ once = true ;
};
+ ~TestMorisawa2007()
+ {
+ delete ComAndFootRealization_ ;
+ delete SPM ;
+ }
+
+ bool doTest(ostream &os)
+ {
+ // Set time reference.
+ m_clock.startingDate();
+
+ /*! Open and reset appropriatly the debug files. */
+ prepareDebugFiles();
+ for (unsigned int lNbIt=0;lNbIt<m_OuterLoopNbItMax;lNbIt++)
+ {
+ os << "<===============================================================>"<<endl;
+ os << "Iteration nb: " << lNbIt << endl;
+
+ m_clock.startPlanning();
+
+ /*! According to test profile initialize the current profile. */
+ chooseTestProfile();
+
+ m_clock.endPlanning();
+
+ if (m_DebugHDR!=0)
+ {
+ m_DebugHDR->currentConfiguration(m_PreviousConfiguration);
+ m_DebugHDR->currentVelocity(m_PreviousVelocity);
+ m_DebugHDR->currentAcceleration(m_PreviousAcceleration);
+ m_DebugHDR->computeForwardKinematics();
+ }
+
+ bool ok = true;
+ while(ok)
+ {
+ m_clock.startOneIteration();
+ if (m_PGIInterface==0)
+ {
+ ok = m_PGI->RunOneStepOfTheControlLoop(m_CurrentConfiguration,
+ m_CurrentVelocity,
+ m_CurrentAcceleration,
+ m_OneStep.ZMPTarget,
+ m_OneStep.finalCOMPosition,
+ m_OneStep.LeftFootPosition,
+ m_OneStep.RightFootPosition);
+ }
+ else if (m_PGIInterface==1)
+ {
+ ok = m_PGI->RunOneStepOfTheControlLoop( m_CurrentConfiguration,
+ m_CurrentVelocity,
+ m_CurrentAcceleration,
+ m_OneStep.ZMPTarget);
+ }
+ m_OneStep.NbOfIt++;
+
+ m_clock.stopOneIteration();
+
+ m_PreviousConfiguration = m_CurrentConfiguration;
+ m_PreviousVelocity = m_CurrentVelocity;
+ m_PreviousAcceleration = m_CurrentAcceleration;
+
+ /*! Call the reimplemented method to generate events. */
+ if (ok)
+ {
+ m_clock.startModification();
+ generateEvent();
+ m_clock.stopModification();
+
+ m_clock.fillInStatistics();
+
+ /*! Fill the debug files with appropriate information. */
+ ComparingZMPs();
+ ComputeAndDisplayAverageError(false);
+ fillInDebugFiles();
+ }
+ else
+ {
+ cerr << "Nothing to dump after " << m_OneStep.NbOfIt << endl;
+ }
+ }
+
+ os << endl << "End of iteration " << lNbIt << endl;
+ os << "<===============================================================>"<<endl;
+ }
+ string lProfileOutput= m_TestName;
+ lProfileOutput +="TimeProfile.dat";
+ m_clock.writeBuffer(lProfileOutput);
+ m_clock.displayStatistics(os,m_OneStep);
+ // Compare debugging files
+ ComputeAndDisplayAverageError(true);
+ return compareDebugFiles();
+ }
+
+ void init()
+ {
+ // Instanciate and initialize.
+ string RobotFileName = m_VRMLPath + m_VRMLFileName;
+
+ bool fileExist = false;
+ {
+ std::ifstream file (RobotFileName.c_str ());
+ fileExist = !file.fail ();
+ }
+ if (!fileExist)
+ throw std::string ("failed to open robot model");
+
+ CreateAndInitializeHumanoidRobot(RobotFileName,
+ m_SpecificitiesFileName,
+ m_LinkJointRank,
+ m_InitConfig,
+ m_HDR, m_DebugHDR, m_PGI);
+
+ // Specify the walking mode: here the default one.
+ istringstream strm2(":walkmode 0");
+ m_PGI->ParseCmd(strm2);
+
+ MAL_VECTOR_RESIZE(m_CurrentConfiguration, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(m_CurrentVelocity, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(m_CurrentAcceleration, m_HDR->numberDof());
+
+ MAL_VECTOR_RESIZE(m_PreviousConfiguration, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(m_PreviousVelocity, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(m_PreviousAcceleration, m_HDR->numberDof());
+
+ SPM = new SimplePluginManager();
+
+ ComAndFootRealization_ = new ComAndFootRealizationByGeometry( (PatternGeneratorInterfacePrivate*) SPM );
+ ComAndFootRealization_->setHumanoidDynamicRobot(m_HDR);
+ ComAndFootRealization_->SetStepStackHandler(new StepStackHandler(SPM));
+ ComAndFootRealization_->SetHeightOfTheCoM(0.814);
+ ComAndFootRealization_->setSamplingPeriod(0.005);
+ ComAndFootRealization_->Initialization();
+
+ initIK();
+ }
+
protected:
+ double filterprecision(double adb)
+ {
+ if (fabs(adb)<1e-7)
+ return 0.0;
+
+ double ladb2 = adb * 1e7;
+ double lintadb2 = trunc(ladb2);
+ return lintadb2/1e7;
+ }
+
+ void initIK()
+ {
+ MAL_VECTOR_DIM(BodyAngles,double,MAL_VECTOR_SIZE(InitialPosition));
+ MAL_VECTOR_DIM(waist,double,6);
+ for (int i = 0 ; i < 6 ; ++i )
+ {
+ waist(i) = 0;
+ }
+ for (unsigned int i = 0 ; i < (m_HDR->numberDof()-6) ; ++i )
+ {
+ BodyAngles(i) = InitialPosition(i);
+ }
+ MAL_S3_VECTOR(lStartingCOMState,double);
+
+ lStartingCOMState(0) = m_OneStep.finalCOMPosition.x[0] ;
+ lStartingCOMState(1) = m_OneStep.finalCOMPosition.y[0] ;
+ lStartingCOMState(2) = m_OneStep.finalCOMPosition.z[0] ;
+ ComAndFootRealization_->SetHeightOfTheCoM(0.814);
+ ComAndFootRealization_->setSamplingPeriod(0.005);
+ ComAndFootRealization_->Initialization();
+
+ ComAndFootRealization_->InitializationCoM(BodyAngles,lStartingCOMState,
+ waist,
+ m_OneStep.LeftFootPosition, m_OneStep.RightFootPosition);
+ ComAndFootRealization_->Initialization();
+ }
+
+ void fillInDebugFiles( )
+ {
+ if (m_DebugFGPI)
+ {
+ ofstream aof;
+ string aFileName;
+ aFileName = m_TestName;
+ aFileName += "TestFGPI.dat";
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision(m_OneStep.NbOfIt*0.005 ) << " " // 1
+ << filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " " // 2
+ << filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 3
+ << filterprecision(m_OneStep.finalCOMPosition.z[0] ) << " " // 4
+ << filterprecision(m_OneStep.finalCOMPosition.yaw[0] ) << " " // 5
+ << filterprecision(m_OneStep.finalCOMPosition.x[1] ) << " " // 6
+ << filterprecision(m_OneStep.finalCOMPosition.y[1] ) << " " // 7
+ << filterprecision(m_OneStep.finalCOMPosition.z[1] ) << " " // 8
+ << filterprecision(m_OneStep.ZMPTarget(0) ) << " " // 9
+ << filterprecision(m_OneStep.ZMPTarget(1) ) << " " // 10
+ << filterprecision(m_OneStep.LeftFootPosition.x ) << " " // 11
+ << filterprecision(m_OneStep.LeftFootPosition.y ) << " " // 12
+ << filterprecision(m_OneStep.LeftFootPosition.z ) << " " // 13
+ << filterprecision(m_OneStep.LeftFootPosition.dx ) << " " // 14
+ << filterprecision(m_OneStep.LeftFootPosition.dy ) << " " // 15
+ << filterprecision(m_OneStep.LeftFootPosition.dz ) << " " // 16
+ << filterprecision(m_OneStep.LeftFootPosition.ddx ) << " " // 17
+ << filterprecision(m_OneStep.LeftFootPosition.ddy ) << " " // 18
+ << filterprecision(m_OneStep.LeftFootPosition.ddz ) << " " // 19
+ << 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
+ << filterprecision(m_OneStep.RightFootPosition.y ) << " " // 24
+ << filterprecision(m_OneStep.RightFootPosition.z ) << " " // 25
+ << filterprecision(m_OneStep.RightFootPosition.dx ) << " " // 26
+ << filterprecision(m_OneStep.RightFootPosition.dy ) << " " // 27
+ << filterprecision(m_OneStep.RightFootPosition.dz ) << " " // 28
+ << filterprecision(m_OneStep.RightFootPosition.ddx ) << " " // 29
+ << filterprecision(m_OneStep.RightFootPosition.ddy ) << " " // 30
+ << filterprecision(m_OneStep.RightFootPosition.ddz ) << " " // 31
+ << 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
+ << filterprecision(m_OneStep.ZMPTarget(0)*sin(m_CurrentConfiguration(5)) +
+ m_OneStep.ZMPTarget(1)*cos(m_CurrentConfiguration(5))
+ +m_CurrentConfiguration(1) ) << " " // 36
+ << filterprecision(m_CurrentConfiguration(0) ) << " " // 37
+ << filterprecision(m_CurrentConfiguration(1) ) << " "; // 38
+ for (unsigned int i = 0 ; i < m_HDR->currentConfiguration().size() ; i++)
+ {
+ aof << filterprecision(m_HDR->currentConfiguration()(i)) << " " ; // 39 - 74
+ }
+
+ aof << endl;
+ aof.close();
+ }
+
+
+ /// \brief Debug Purpose
+ /// --------------------
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+
+ if ( iteration == 0 ){
+ oss.str("/tmp/walk_mnaveau.pos");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
+ ///----
+ oss.str("/tmp/walk_mnaveau.pos");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision( iteration * 0.1 ) << " " ; // 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();
+
+ if ( iteration == 0 ){
+ oss.str("/tmp/walk_mnaveau.hip");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
+ oss.str("/tmp/walk_mnaveau.hip");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for(unsigned int j = 0 ; j < 20 ; j++){
+ aof << filterprecision( iteration * 0.1 ) << " " ; // 1
+ aof << filterprecision( 0.0 ) << " " ; // 1
+ aof << filterprecision( 0.0 ) << " " ; // 1
+ aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0] ) << " " ; // 1
+ aof << endl ;
+ }
+ aof.close();
+
+ iteration++;
+ }
+
+ void SpecializedRobotConstructor( CjrlHumanoidDynamicRobot *& aHDR, CjrlHumanoidDynamicRobot *& aDebugHDR )
+ {
+ dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
+ Chrp2OptHumanoidDynamicRobot *aHRP2HDR = new Chrp2OptHumanoidDynamicRobot( &aRobotDynamicsObjectConstructor );
+ aHDR = aHRP2HDR;
+ aDebugHDR = new Chrp2OptHumanoidDynamicRobot(&aRobotDynamicsObjectConstructor);
+ }
+
+ void ComparingZMPs()
+ {
+ const int stage0 = 0 ;
+ /// \brief calculate, from the CoM of computed by the preview control,
+ /// the corresponding articular position, velocity and acceleration
+ /// ------------------------------------------------------------------
+ MAL_VECTOR(CurrentConfiguration,double);
+ MAL_VECTOR(CurrentVelocity,double);
+ MAL_VECTOR(CurrentAcceleration,double);
+
+ MAL_VECTOR_RESIZE(CurrentConfiguration, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(CurrentVelocity, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(CurrentAcceleration, m_HDR->numberDof());
+
+ MAL_VECTOR_DIM(aCOMState,double,6);
+ MAL_VECTOR_DIM(aCOMSpeed,double,6);
+ MAL_VECTOR_DIM(aCOMAcc,double,6);
+ MAL_VECTOR_DIM(aLeftFootPosition,double,5);
+ MAL_VECTOR_DIM(aRightFootPosition,double,5);
+
+ aCOMState(0) = m_OneStep.finalCOMPosition.x[0]; aCOMSpeed(0) = m_OneStep.finalCOMPosition.x[1]; aCOMAcc(0) = m_OneStep.finalCOMPosition.x[2];
+ aCOMState(1) = m_OneStep.finalCOMPosition.y[0]; aCOMSpeed(1) = m_OneStep.finalCOMPosition.y[1]; aCOMAcc(1) = m_OneStep.finalCOMPosition.y[2];
+ aCOMState(2) = m_OneStep.finalCOMPosition.z[0]; aCOMSpeed(2) = m_OneStep.finalCOMPosition.z[1]; aCOMAcc(2) = m_OneStep.finalCOMPosition.z[2];
+ aCOMState(3) = m_OneStep.finalCOMPosition.roll[0]; aCOMSpeed(3) = m_OneStep.finalCOMPosition.roll[1]; aCOMAcc(3) = m_OneStep.finalCOMPosition.roll[2];
+ aCOMState(4) = m_OneStep.finalCOMPosition.pitch[0]; aCOMSpeed(4) = m_OneStep.finalCOMPosition.pitch[1]; aCOMAcc(4) = m_OneStep.finalCOMPosition.pitch[2];
+ aCOMState(5) = m_OneStep.finalCOMPosition.yaw[0]; aCOMSpeed(5) = m_OneStep.finalCOMPosition.yaw[1]; aCOMAcc(5) = m_OneStep.finalCOMPosition.yaw[2];
+
+ aLeftFootPosition(0) = m_OneStep.LeftFootPosition.x; aRightFootPosition(0) = m_OneStep.RightFootPosition.x;
+ aLeftFootPosition(1) = m_OneStep.LeftFootPosition.y; aRightFootPosition(1) = m_OneStep.RightFootPosition.y;
+ aLeftFootPosition(2) = m_OneStep.LeftFootPosition.z; aRightFootPosition(2) = m_OneStep.RightFootPosition.z;
+ aLeftFootPosition(3) = m_OneStep.LeftFootPosition.theta; aRightFootPosition(3) = m_OneStep.RightFootPosition.theta;
+ aLeftFootPosition(4) = m_OneStep.LeftFootPosition.omega; aRightFootPosition(4) = m_OneStep.RightFootPosition.omega;
+ ComAndFootRealization_->setSamplingPeriod(0.005);
+ ComAndFootRealization_->ComputePostureForGivenCoMAndFeetPosture(aCOMState, aCOMSpeed, aCOMAcc,
+ aLeftFootPosition,
+ aRightFootPosition,
+ CurrentConfiguration,
+ CurrentVelocity,
+ CurrentAcceleration,
+ m_OneStep.NbOfIt,
+ stage0);
+
+ /// \brief Debug Purpose
+ /// --------------------
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+ oss.str("TestHerdt2010DynamicART2.dat");
+ aFileName = oss.str();
+ if ( iteration_zmp == 0 )
+ {
+ 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 j = 0 ; j < CurrentConfiguration.size() ; ++j)
+ {
+ aof << filterprecision(CurrentConfiguration(j)) << " " ;
+ }
+ for (unsigned int j = 0 ; j < CurrentVelocity.size() ; ++j)
+ {
+ aof << filterprecision(CurrentVelocity(j)) << " " ;
+ }
+ for (unsigned int j = 0 ; j < CurrentAcceleration.size() ; ++j)
+ {
+ aof << filterprecision(CurrentAcceleration(j)) << " " ;
+ }
+ aof << endl ;
+
+
+ /// \brief rnea, calculation of the multi body ZMP
+ /// ----------------------------------------------
+ Robot_Model2::confVector q, dq, ddq;
+ for(unsigned int j = 0 ; j < CurrentConfiguration.size() ; j++ )
+ {
+ q(j,0) = CurrentConfiguration[j] ;
+ dq(j,0) = CurrentVelocity[j] ;
+ ddq(j,0) = CurrentAcceleration[j] ;
+ }
+ metapod::rnea< Robot_Model2, true >::run(robot_, q, dq, ddq);
+
+ Node2 anode = boost::fusion::at_c<Robot_Model2::BODY>(robot_.nodes);
+ Force2 aforce = anode.body.iX0.applyInv (anode.joint.f) ;
+
+ double ZMPMB[2];
+
+ ZMPMB[0] = - aforce.n()[1] / aforce.f()[2] ;
+ ZMPMB[1] = aforce.n()[0] / aforce.f()[2] ;
+
+
+ if (m_OneStep.NbOfIt<=10){
+ dInitX = m_OneStep.ZMPTarget(0) - ZMPMB[0] ;
+ dInitY = m_OneStep.ZMPTarget(1) - ZMPMB[1] ;
+ {
+ vector<double> tmp_zmp(2) ;
+ tmp_zmp[0] =0.0 ;
+ tmp_zmp[1] =0.0 ;
+ errZMP_.push_back(tmp_zmp);
+ }
+ }
+
+ if (m_OneStep.NbOfIt >= 10)
+ {
+ double errx = sqrt( ( m_OneStep.ZMPTarget(0) - ZMPMB[0] - dInitX )*( m_OneStep.ZMPTarget(0) - ZMPMB[0] - dInitX ) ) ;
+ double erry = sqrt( ( m_OneStep.ZMPTarget(1) - ZMPMB[1] - dInitY )*( m_OneStep.ZMPTarget(1) - ZMPMB[1] - dInitY ) ) ;
+ {
+ vector<double> tmp_zmp(2) ;
+ tmp_zmp[0] = errx ;
+ tmp_zmp[1] = erry ;
+ errZMP_.push_back(tmp_zmp);
+ }
+ }
+
+
+ static double ecartmaxX = 0 ;
+ static double ecartmaxY = 0 ;
+ if ( abs(errZMP_.back()[0]) > ecartmaxX )
+ ecartmaxX = abs(errZMP_.back()[0]) ;
+ if ( abs(errZMP_.back()[1]) > ecartmaxY )
+ ecartmaxY = abs(errZMP_.back()[1]) ;
+
+// cout << "ecartmaxX :" << ecartmaxX << endl ;
+// cout << "ecartmaxY :" << ecartmaxY << endl ;
+
+ // Writing of the two zmps and the error.
+ if (once)
+ {
+ aof.open("TestHerdt2010ErrorZMP.dat",ofstream::out);
+ aof.close();
+ once = false ;
+ }
+ aof.open("TestHerdt2010ErrorZMP.dat",ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision( iteration_zmp ) << " " // 1
+ << filterprecision( ZMPMB[0] + dInitX ) << " " // 2
+ << filterprecision( ZMPMB[1] + dInitY ) << " " // 3
+ << filterprecision(m_OneStep.ZMPTarget(0) ) << " " // 4
+ << filterprecision(m_OneStep.ZMPTarget(1) ) << " " // 5
+ << endl ;
+ aof.close();
+
+ iteration_zmp++;
+ return ;
+ }
+
+ void ComputeAndDisplayAverageError(bool display){
+ static int plot_it = 0 ;
+ double moyErrX = 0 ;
+ double moyErrY = 0 ;
+ for (unsigned int i = 0 ; i < errZMP_.size(); ++i)
+ {
+ moyErrX += errZMP_[i][0] ;
+ moyErrY += errZMP_[i][1] ;
+ }
+ moyErrX = moyErrX / errZMP_.size() ;
+ moyErrY = moyErrY / errZMP_.size() ;
+ if ( display )
+ {
+ cout << "moyErrX = " << moyErrX << endl
+ << "moyErrY = " << moyErrY << endl ;
+ }
+ ofstream aof;
+ string aFileName;
+ aFileName = m_TestName;
+ aFileName += "MoyZMP.dat";
+ if(plot_it==0){
+ 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);
+ aof << filterprecision(moyErrX ) << " " // 1
+ << filterprecision(moyErrY ) << " " // 2
+ << endl ;
+ aof.close();
+ plot_it++;
+ }
+
+
void StartAnalyticalOnLineWalking(PatternGeneratorInterface &aPGI)
{
CommonInitialization(aPGI);
--
GitLab