From 64d6d580ab6bca8305b9853dba20ee82a37ecea8 Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Wed, 12 Mar 2014 16:10:49 +0100
Subject: [PATCH] Computing the initial difference between the ZMPref and the
ZMPMB and addinng it into the lop.
---
.../ZMPVelocityReferencedQP.cpp | 90 ++++++++-------
.../ZMPVelocityReferencedQP.h | 108 ++++++++++++++++--
tests/TestHerdt2010DynamicFilter.cpp | 14 ++-
3 files changed, 157 insertions(+), 55 deletions(-)
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 4040bccf..2ad9a246 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -182,11 +182,11 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
// Initialization of the Kajita preview controls (ICRA 2003).
MAL_MATRIX_RESIZE(m_deltax,3,1); MAL_MATRIX_RESIZE(m_deltay,3,1);
m_PC = new PreviewControl(SPM,
- OptimalControllerSolver::MODE_WITHOUT_INITIALPOS,
+ OptimalControllerSolver::MODE_WITH_INITIALPOS,
true);
m_PC->SetPreviewControlTime (QP_T_*(QP_N_-1));
m_PC->SetSamplingPeriod (m_SamplingPeriod);
- m_PC->SetHeightOfCoM(Robot_->CoMHeight());
+ m_PC->SetHeightOfCoM(0);
// init of the debug files
ofstream aof;
@@ -201,8 +201,8 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
m_velocityTraj.resize( QP_N_ * m_numberOfSample );
m_accelerationTraj.resize( QP_N_ * m_numberOfSample );
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_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();
@@ -529,8 +529,8 @@ ZMPVelocityReferencedQP::OnLine(double time,
{
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[i] = FinalLeftFootTraj_deq[i] ;
+ //m_RightFootTraj_deq[i] = FinalRightFootTraj_deq[i] ;
}
m_LeftFootTraj_deq = FinalLeftFootTraj_deq ;
m_RightFootTraj_deq = FinalRightFootTraj_deq ;
@@ -584,14 +584,12 @@ ZMPVelocityReferencedQP::OnLine(double time,
Interpolate_trunk_orientation( currentIndex + i * m_numberOfSample, m_COMTraj_deq,
m_LeftFootTraj_deq, m_RightFootTraj_deq);
-//
-// if ( CurrentSupport.Phase == SS && CurrentSupport.StateChanged==true && solution.Solution_vec.size() >= (2*QP_N_ + 4))
-// {
-//// 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];
-//// solution.SupportOrientations_deq[0]=solution.SupportOrientations_deq[2];
-// cout << "passage from double to single support\n" ;
-// }
+
+ if ( CurrentSupport.Phase == SS && CurrentSupport.StateChanged==true && solution.Solution_vec.size() >= (2*QP_N_ + 4))
+ {
+ 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];
+ }
if (CurrentSupport.StateChanged==true )
{
solution.SupportOrientations_deq[0] = solution.SupportOrientations_deq[stateModified+1];
@@ -602,9 +600,6 @@ ZMPVelocityReferencedQP::OnLine(double time,
}
-
- cout << "changes = " << changes << endl ;
- cout << "solution.Solution_vec.size() = " << solution.Solution_vec.size() << endl ;
/// \brief Debug Purpose
/// --------------------
ofstream aof;
@@ -631,17 +626,17 @@ ZMPVelocityReferencedQP::OnLine(double time,
<< 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_COMTraj_deq[i].yaw[0]*180 / M_PI) << " " // 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_LeftFootTraj_deq[i].theta ) << " " // 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
+ << filterprecision( m_RightFootTraj_deq[i].theta ) << " " // 11
<< endl ;
}
aof.close();
-
+ cout << "time = " << time << endl ;
// DYNAMIC FILTER
// --------------
//DynamicFilter( m_ZMPTraj_deq, m_COMTraj_deq, m_LeftFootTraj_deq, m_RightFootTraj_deq, currentIndex );
@@ -679,13 +674,15 @@ ZMPVelocityReferencedQP::OnLine(double time,
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
aof << iteration*0.005 << " " // 1
- << FinalLeftFootTraj_deq[currentIndex].theta << " " // 14
- << FinalRightFootTraj_deq[currentIndex].theta << " " // 15
- << FinalCOMTraj_deq[currentIndex].roll[0] << " " // 19
- << FinalCOMTraj_deq[currentIndex].pitch[0] << " " // 19
- << FinalCOMTraj_deq[currentIndex].yaw[0] << " " // 19
- << FinalCOMTraj_deq[currentIndex].x[0] << " " // 19
- << FinalCOMTraj_deq[currentIndex].y[0] << " " // 19
+ << FinalLeftFootTraj_deq[0].theta << " " // 14
+ << FinalRightFootTraj_deq[0].theta << " " // 15
+ << FinalCOMTraj_deq[0].roll[0] << " " // 19
+ << FinalCOMTraj_deq[0].pitch[0] << " " // 19
+ << FinalCOMTraj_deq[0].yaw[0] << " " // 19
+ << FinalCOMTraj_deq[0].x[0] << " " // 19
+ << FinalCOMTraj_deq[0].y[0] << " " // 19
+ << FinalZMPTraj_deq[0].px << " " // 19
+ << FinalZMPTraj_deq[0].py << " " // 19
<< endl ;
aof.close();
@@ -703,7 +700,6 @@ ZMPVelocityReferencedQP::OnLine(double time,
//
//
//----------"Real-time" loop---------
-
}
@@ -875,6 +871,10 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
Force_HRP2_14 f4 ;
Force_HRP2_14 f1 ;
Force_HRP2_14 f2 ;
+
+ static bool once = true ;
+ static double dInitX(0), dInitY(0) ;
+
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++ )
@@ -892,9 +892,13 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
f3 = node.joint.f ;
f4 = node.body.iX0.applyInv (node.joint.f);
}
-
- m_deltaZMPMBPositions[i].px = ZMPPositions[currentIndex+i].px - ( - f1.n()[1] / f1.f()[2] ) ;
- m_deltaZMPMBPositions[i].py = ZMPPositions[currentIndex+i].py - ( f1.n()[0] / f1.f()[2] ) ;
+ if (once){
+ dInitX = ZMPPositions[currentIndex].px - ( - f1.n()[1] / f1.f()[2] ) ;
+ dInitY = ZMPPositions[currentIndex].py - ( f1.n()[0] / f1.f()[2] ) ;
+ once = 0 ;
+ }
+ m_deltaZMPMBPositions[i].px = ZMPPositions[currentIndex+i].px - ( - f1.n()[1] / f1.f()[2] ) - dInitX ;
+ m_deltaZMPMBPositions[i].py = ZMPPositions[currentIndex+i].py - ( f1.n()[0] / f1.f()[2] ) - dInitY ;
m_deltaZMPMBPositions[i].pz = 0.0 ;
m_deltaZMPMBPositions[i].theta = 0.0 ;
m_deltaZMPMBPositions[i].time = m_CurrentTime + i * m_SamplingPeriod ;
@@ -927,7 +931,7 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
m_PC->SetPreviewControlTime (QP_T_*(QP_N_-1));
m_PC->SetSamplingPeriod (m_SamplingPeriod);
m_PC->SetHeightOfCoM(0);
- m_PC->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITHOUT_INITIALPOS);
+ m_PC->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITH_INITIALPOS);
for(int j=0;j<3;j++)
{
m_deltax(j,0) = 0 ;
@@ -1006,17 +1010,21 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
<< filterprecision( FinalCOMTraj_deq[currentIndex].x[0] ) << " " // 18
<< filterprecision( FinalCOMTraj_deq[currentIndex].y[0] ) << " " // 19
+
+ << filterprecision( ( - f4.n()[1] / f4.f()[2] ) + dInitX ) << " " // 18
+ << filterprecision( ( f4.n()[0] / f4.f()[2] ) + dInitY ) << " " // 19
+
<< endl ;
aof.close();
-// for (unsigned int i = 0 ; i < m_numberOfSample ; i++)
-// {
-// for(int j=0;j<3;j++)
-// {
-// FinalCOMTraj_deq[currentIndex+i].x[j] -= COMStateBuffer[i].x[j] ;
-// FinalCOMTraj_deq[currentIndex+i].y[j] -= COMStateBuffer[i].y[j] ;
-// }
-// }
+ for (unsigned int i = 0 ; i < m_numberOfSample ; i++)
+ {
+ for(int j=0;j<3;j++)
+ {
+ FinalCOMTraj_deq[currentIndex+i].x[j] += COMStateBuffer[i].x[j] ;
+ FinalCOMTraj_deq[currentIndex+i].y[j] += COMStateBuffer[i].y[j] ;
+ }
+ }
iteration++;
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
index 28abca71..20deec87 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
@@ -30,18 +30,29 @@
#ifndef _ZMPVELOCITYREFERENCEDQP_WITH_CONSTRAINT_H_
#define _ZMPVELOCITYREFERENCEDQP_WITH_CONSTRAINT_H_
-
-#include <PreviewControl/LinearizedInvertedPendulum2D.h>
+#include <PreviewControl/PreviewControl.hh>
+#include <PreviewControl/LinearizedInvertedPendulum2D.hh>
#include <PreviewControl/rigid-body-system.hh>
#include <Mathematics/relative-feet-inequalities.hh>
-#include <ZMPRefTrajectoryGeneration/ZMPRefTrajectoryGeneration.h>
-#include <PreviewControl/SupportFSM.h>
-#include <ZMPRefTrajectoryGeneration/OrientationsPreview.h>
+#include <ZMPRefTrajectoryGeneration/ZMPRefTrajectoryGeneration.hh>
+#include <PreviewControl/SupportFSM.hh>
+#include <ZMPRefTrajectoryGeneration/OrientationsPreview.hh>
#include <ZMPRefTrajectoryGeneration/qp-problem.hh>
-#include <privatepgtypes.h>
+#include <privatepgtypes.hh>
#include <ZMPRefTrajectoryGeneration/generator-vel-ref.hh>
#include <Mathematics/intermediate-qp-matrices.hh>
#include <jrl/walkgen/pgtypes.hh>
+#include <MotionGeneration/ComAndFootRealizationByGeometry.hh>
+
+// metapod includes
+#include <metapod/models/hrp2_14/hrp2_14.hh>
+#ifndef METAPOD_TYPEDEF
+#define METAPOD_TYPEDEF
+ typedef double LocalFloatType;
+ typedef metapod::Spatial::ForceTpl<LocalFloatType> Force_HRP2_14;
+ typedef metapod::hrp2_14<LocalFloatType> Robot_Model;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::BODY >::type Node;
+#endif
namespace PatternGeneratorJRL
{
@@ -57,7 +68,7 @@ namespace PatternGeneratorJRL
public:
ZMPVelocityReferencedQP(SimplePluginManager *SPM, string DataFile,
- CjrlHumanoidDynamicRobot *aHS=0);
+ CjrlHumanoidDynamicRobot *aHS=0 );
~ZMPVelocityReferencedQP();
@@ -114,7 +125,7 @@ namespace PatternGeneratorJRL
}
inline bool Running()
- { return Running_; }
+ { return Running_; }
/// \brief Set the final-stage trigger
inline void EndingPhase(bool EndingPhase)
@@ -128,9 +139,17 @@ namespace PatternGeneratorJRL
inline const int & QP_N(void) const
{ return QP_N_; }
+
+ /// \brief Setter and getter for the ComAndZMPTrajectoryGeneration.
+ inline bool setComAndFootRealization(ComAndFootRealization * aCFR)
+ { ComAndFootRealization_ = aCFR; return true;};
+ inline ComAndFootRealization * getComAndFootRealization()
+ { return ComAndFootRealization_;};
+
/// \}
+
//
// Private members:
//
@@ -172,10 +191,11 @@ namespace PatternGeneratorJRL
int QP_N_;
/// \brief 2D LIPM to simulate the evolution of the robot's CoM.
- LinearizedInvertedPendulum2D CoM_;
+ LinearizedInvertedPendulum2D CoM_ ;
+ LinearizedInvertedPendulum2D CoM2_ ;
/// \brief Simplified robot model
- RigidBodySystem * Robot_;
+ RigidBodySystem * Robot_ ;
/// \brief Finite State Machine to simulate the evolution of the support states.
SupportFSM * SupportFSM_;
@@ -198,10 +218,54 @@ namespace PatternGeneratorJRL
/// \brief Previewed Solution
solution_t Solution_;
+ /// \brief Store a reference to the object to solve posture resolution.
+ ComAndFootRealization * ComAndFootRealization_;
+
+ /// \brief HDR allow the computation of the dynamic filter
+ CjrlHumanoidDynamicRobot * HDR_ ;
+
+ /// \brief Pointer to the Preview Control object.
+ PreviewControl *m_PC;
+
+ /// \brief State of the Preview control.
+ MAL_MATRIX( m_deltax,double);
+ MAL_MATRIX( m_deltay,double);
+ /// \brief Buffers for the Kajita's dynamic filter
+ deque<ZMPPosition> m_ZMPTraj_deq ;
+ 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 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 ;
+
+ /// \brief Set configuration vectors (q, dq, ddq, torques) to reference values.
+ Robot_Model::confVector m_torques, m_q, m_dq, m_ddq;
+
+ /// \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
void GetZMPDiscretization(std::deque<ZMPPosition> & ZMPPositions,
std::deque<COMState> & COMStates,
@@ -235,8 +299,30 @@ namespace PatternGeneratorJRL
deque<FootAbsolutePosition> &RightFootAbsolutePositions);
int ReturnOptimalTimeToRegenerateAStep();
+
+ int DynamicFilter(std::deque<ZMPPosition> & ZMPPositions,
+ std::deque<COMState> & COMStates,
+ std::deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
+ std::deque<FootAbsolutePosition> &RightFootAbsolutePositions,
+ unsigned currentIndex
+ );
+
+ void CallToComAndFootRealization(COMState &acomp,
+ FootAbsolutePosition &aLeftFAP,
+ FootAbsolutePosition &aRightFAP,
+ MAL_VECTOR_TYPE(double) &CurrentConfiguration,
+ MAL_VECTOR_TYPE(double) &CurrentVelocity,
+ MAL_VECTOR_TYPE(double) &CurrentAcceleration,
+ int IterationNumber
+ );
+
+ void Interpolate_trunk_orientation(int CurrentIndex,
+ deque<COMState> & FinalCOMTraj_deq,
+ deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
+ deque<FootAbsolutePosition> & FinalRightFootTraj_deq);
+
};
}
-#include <ZMPRefTrajectoryGeneration/ZMPDiscretization.h>
+#include <ZMPRefTrajectoryGeneration/ZMPDiscretization.hh>
#endif /* _ZMPQP_WITH_CONSTRAINT_H_ */
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index de3209e2..09b4ebe5 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -68,6 +68,7 @@ private:
double errZMP [2] ;
Robot_Model2 robot_ ;
ComAndFootRealization * ComAndFootRealization_;
+ SimplePluginManager * SPM ;
public:
TestHerdt2010(int argc, char *argv[], string &aString, int TestProfile):
@@ -77,9 +78,13 @@ private:
errZMP[0]=0.0;
errZMP[1]=0.0;
ComAndFootRealization_ = 0 ;
+ SimplePluginManager * SPM = 0 ;
};
- ~TestHerdt2010(){}
+ ~TestHerdt2010(){
+ delete ComAndFootRealization_ ;
+ delete SPM ;
+ }
typedef void (TestHerdt2010::* localeventHandler_t)(PatternGeneratorInterface &);
@@ -210,11 +215,14 @@ private:
MAL_VECTOR_RESIZE(m_PreviousVelocity, m_HDR->numberDof());
MAL_VECTOR_RESIZE(m_PreviousAcceleration, m_HDR->numberDof());
- ComAndFootRealization_ = new ComAndFootRealizationByGeometry( NULL );
+
+ SPM = new SimplePluginManager();
+
+ ComAndFootRealization_ = new ComAndFootRealizationByGeometry( (PatternGeneratorInterfacePrivate*) SPM );
ComAndFootRealization_->setHumanoidDynamicRobot(m_HDR);
ComAndFootRealization_->SetHeightOfTheCoM(0.814);
ComAndFootRealization_->setSamplingPeriod(0.1);
- ComAndFootRealization_->SetStepStackHandler(new StepStackHandler(new SimplePluginManager()));
+ ComAndFootRealization_->SetStepStackHandler(new StepStackHandler(SPM));
ComAndFootRealization_->Initialization();
}
--
GitLab