From ec09b789a3a61bb19082cc890ac871a77ae0349c Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Fri, 25 Jul 2014 14:12:46 +0200
Subject: [PATCH] Adding zmp initialization in the Herdt2010 PG. Adding the
dynamic filter in the Herdt2010 PG.
---
.../CoMAndFootOnlyStrategy.cpp | 15 ++++
.../ComAndFootRealizationByGeometry.cpp | 6 +-
src/PreviewControl/PreviewControl.cpp | 8 +--
.../DynamicFilter.cpp | 60 ----------------
.../ZMPVelocityReferencedQP.cpp | 69 +++++++++++--------
.../ZMPVelocityReferencedQP.hh | 4 --
6 files changed, 62 insertions(+), 100 deletions(-)
diff --git a/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp b/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp
index c9d3f488..f63c8845 100644
--- a/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp
+++ b/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp
@@ -140,6 +140,8 @@ int CoMAndFootOnlyStrategy::EvaluateStartingState(MAL_VECTOR(&,double) BodyAngle
std::vector<ComAndFootRealization *>::iterator itCFR ;
for (itCFR = m_ComAndFootRealization.begin() ; itCFR != m_ComAndFootRealization.end() ; ++itCFR )
{
+ // here we use the analytical forward kinematics to initialise the position of the CoM of mass according to
+ // the articular position of the robot.
(*itCFR)->InitializationCoM(BodyAngles,lStartingCOMState,
lStartingWaistPose,
InitLeftFootPosition, InitRightFootPosition);
@@ -154,6 +156,19 @@ int CoMAndFootOnlyStrategy::EvaluateStartingState(MAL_VECTOR(&,double) BodyAngle
aStartingZMPPosition= (*itCFR)->GetCOGInitialAnkles();
}
+
+ // We assume that the robot is not moving at the beginning so the zmp is the projection of the com on the ground.
+ aStartingZMPPosition(0) = aStartingCOMState.x[0] ;
+ aStartingZMPPosition(1) = aStartingCOMState.y[0] ;
+ // The altitude of the zmp depend on the altitude of the support foot.
+ if (InitLeftFootPosition.stepType < 0)
+ {
+ aStartingZMPPosition(2) = InitLeftFootPosition.z ;
+ }else{
+ aStartingZMPPosition(2) = InitRightFootPosition.z ;
+ }
+
+
// cerr << "YOU SHOULD INITIALIZE PROPERLY aStartingZMPosition in CoMAndFootOnlyStrategy::EvaluateStartingState" <<endl;
return 0;
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
index f35273ea..7b32ff30 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
@@ -897,10 +897,8 @@ bool ComAndFootRealizationByGeometry::
int IterationNumber,
int Stage)
{
-
- MAL_VECTOR(lqr,double);
- MAL_VECTOR(lql,double);
-
+ MAL_VECTOR_DIM(lqr,double,6);
+ MAL_VECTOR_DIM(lql,double,6);
MAL_S3_VECTOR(AbsoluteWaistPosition,double);
// Kinematics for the legs.
diff --git a/src/PreviewControl/PreviewControl.cpp b/src/PreviewControl/PreviewControl.cpp
index 41bbc3ce..e510ab37 100644
--- a/src/PreviewControl/PreviewControl.cpp
+++ b/src/PreviewControl/PreviewControl.cpp
@@ -339,10 +339,10 @@ int PreviewControl::OneIterationOfPreview(MAL_MATRIX( &x, double),
ux = - r(0,0) + m_Ks * sxzmp ;
if(ZMPPositions.size()<m_SizeOfPreviewWindow)
- {
- LTHROW("ZMPPositions.size()<m_SizeOfPreviewWindow:" );
- }
- //cout << "Size preview = "<< ZMPPositions.size() << endl ;
+ {
+ LTHROW("ZMPPositions.size()<m_SizeOfPreviewWindow:" );
+ }
+
for(unsigned int i=0;i<m_SizeOfPreviewWindow;i++)
ux += m_F(i,0)* ZMPPositions[lindex+i].px;
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index b076aa52..ea1326ad 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -280,8 +280,6 @@ void DynamicFilter::InverseKinematics(
configuration, velocity, acceleration,
iteration, stage);
- static int it = 0 ;
-
if (walkingHeuristic_)
{
LankleNode & node_lankle = boost::fusion::at_c<Robot_Model::l_ankle>(robot_.nodes);
@@ -343,34 +341,6 @@ void DynamicFilter::InverseKinematics(
err1 = HDR->getSpecializedInverseKinematics( *left_shoulder ,*(HDR->leftWrist()), identity, leftHandPose, larm_q );
err2 = HDR->getSpecializedInverseKinematics( *right_shoulder ,*(HDR->rightWrist()), identity, rightHandPose, rarm_q );
- ofstream aof;
-
- string aFileName;
- if ( it == 0 ){
- aFileName = "larmrarm.dat";
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- }
- aFileName = "larmrarm.dat";
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
-
- aof << it * 0.005 << " " ; // 1
-
- for(unsigned int i = 0 ; i < larm_q.size() ; i++){
- aof << rarm_q(i) << " " ; // 2
- }
-
- for(unsigned int i = 0 ; i < rarm_q.size() ; i++){
- aof << larm_q(i) << " " ; // 8
- }
- aof << waistXrf.r()(0) << " "
- << waistXlf.r()(0) << " ";
-
- aof << endl ;
- aof.close();
-
// swinging arms
upperPartConfiguration_(upperPartIndex[0])= 0.0 ; // CHEST_JOINT0
upperPartConfiguration_(upperPartIndex[1])= 0.015 ; // CHEST_JOINT1
@@ -409,36 +379,6 @@ void DynamicFilter::InverseKinematics(
acceleration(i) = upperPartAcceleration_(i) ;
}
-
- /// \brief Create file .hip .pos .zmp
- /// --------------------
- ofstream aof;
- string aFileName;
-
- if ( it == 0 ){
- aFileName = "/tmp/goingDownWithWeightDFtest";
- aFileName+=".pos";
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- }
- ///----
- aFileName = "/tmp/goingDownWithWeightDFtest";
- aFileName+=".pos";
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- aof << it * 0.005 << " " ; // 1
- for(unsigned int i = 6 ; i < configuration.size() ; i++){
- aof << configuration(i) << " " ; // 2
- }
- for(unsigned int i = 0 ; i < 10 ; i++){
- aof << 0.0 << " " ;
- }
- aof << endl ;
- aof.close();
-
- ++it;
-
return;
}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 1902382e..372ea4e5 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -205,8 +205,6 @@ Solution_(),OFTG_DF_(0),OFTG_control_(0),dynamicFilter_(0)
// size = numberOfIterationOfThePreviewControl * NumberOfSample + Margin
ZMPTraj_deq_.resize( QP_N_ * NbSampleInterpolation_+20);
COMTraj_deq_.resize( QP_N_ * NbSampleInterpolation_+20);
- tmpCoM_.resize(QP_N_ * NbSampleControl_ + 20);
- tmpZMP_.resize(QP_N_ * NbSampleControl_ + 20);
}
@@ -435,7 +433,7 @@ int
FinalCurrentStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
dynamicFilter_->init(0.0,m_SamplingPeriod,InterpolationPeriod_,
- QP_T_,QP_N_*QP_T_,CoMHeight_,InitLeftFootAbsolutePosition,lStartingCOMState);
+ QP_T_, QP_N_*QP_T_ - QP_T_/m_SamplingPeriod * InterpolationPeriod_ ,CoMHeight_,InitLeftFootAbsolutePosition,lStartingCOMState);
return 0;
}
@@ -548,15 +546,19 @@ void
FinalRightFootTraj_deq, time) ;
// Computing the control ZMPMB
- unsigned int ControlIteration = time / QP_T_ ;
+ unsigned int ControlIteration = 0 ;
+ if ( time > m_SamplingPeriod )
+ {
+ ControlIteration = 2;
+ }
int stage0 = 0 ;
vector< vector<double> > zmpmb (NbSampleControl_,vector<double>(2,0.0));
for(unsigned int i = 0 ; i < NbSampleControl_ ; ++i)
{
dynamicFilter_->ComputeZMPMB(m_SamplingPeriod,
- FinalCOMTraj_deq[i+CurrentIndex_],
- FinalLeftFootTraj_deq[i+CurrentIndex_],
- FinalRightFootTraj_deq[i+CurrentIndex_],
+ FinalCOMTraj_deq[i],
+ FinalLeftFootTraj_deq[i],
+ FinalRightFootTraj_deq[i],
zmpmb[i],
stage0,
ControlIteration + i);
@@ -564,47 +566,56 @@ void
// Computing the interpolated ZMPMB
DynamicFilterInterpolation(time) ;
+
+ // computing the interpolated ZMPMB
int stage1 = 1 ;
- vector< vector<double> > zmpmb_i (NbSampleControl_,vector<double>(2,0.0));
- dynamicFilter_->stage0INstage1();
- for(unsigned int i = 0 ; i < NbSampleInterpolation_ ; ++i)
+ vector< vector<double> > zmpmb_i (QP_N_*NbSampleInterpolation_,vector<double>(2,0.0));
+
+ for(unsigned int i = 0 ; i < QP_N_*NbSampleInterpolation_ ; ++i)
{
dynamicFilter_->ComputeZMPMB(m_SamplingPeriod,
- COMTraj_deq_[i+CurrentIndex_],
- LeftFootTraj_deq_[i+CurrentIndex_],
- RightFootTraj_deq_[i+CurrentIndex_],
+ COMTraj_deq_[i],
+ LeftFootTraj_deq_[i],
+ RightFootTraj_deq_[i],
zmpmb_i[i],
stage1,
ControlIteration + i);
}
- deque<ZMPPosition> inputdeltaZMP_deq(N) ;
+ dynamicFilter_->stage0INstage1();
+
+ // Compute the delta ZMP
+ deque<ZMPPosition> inputdeltaZMP_deq(QP_N_*NbSampleInterpolation_) ;
deque<COMState> outputDeltaCOMTraj_deq ;
- for (unsigned int i = 0 ; i < NbSampleInterpolation_ ; ++i)
+ for (unsigned int i = 0 ; i < QP_N_*NbSampleInterpolation_ ; ++i)
{
- inputdeltaZMP_deq[i].px = ZMPTraj_deq_[i+CurrentIndex_].px - zmpmb_i[i][0] ;
- inputdeltaZMP_deq[i].py = ZMPTraj_deq_[i+CurrentIndex_].py - zmpmb_i[i][1] ;
+ inputdeltaZMP_deq[i].px = ZMPTraj_deq_[i].px - zmpmb_i[i][0] ;
+ inputdeltaZMP_deq[i].py = ZMPTraj_deq_[i].py - zmpmb_i[i][1] ;
inputdeltaZMP_deq[i].pz = 0.0 ;
inputdeltaZMP_deq[i].theta = 0.0 ;
inputdeltaZMP_deq[i].time = m_CurrentTime + i * m_SamplingPeriod ;
- inputdeltaZMP_deq[i].stepType = ZMPPositions[i].stepType ;
+ inputdeltaZMP_deq[i].stepType = ZMPTraj_deq_[i].stepType ;
}
- m_kajitaDynamicFilter->OptimalControl(inputdeltaZMP_deq,outputDeltaCOMTraj_deq) ;
+ // compute the delta CoM
+ dynamicFilter_->OptimalControl(inputdeltaZMP_deq,outputDeltaCOMTraj_deq) ;
+
+ // Correct the CoM.
deque<COMState> filteredCoM = FinalCOMTraj_deq ;
vector <vector<double> > filteredZMPMB (NbSampleControl_ , vector<double> (2,0.0)) ;
- for (unsigned int i = 0 ; i < n ; ++i)
+ int stage2 = 2 ;
+ for (unsigned int i = 0 ; i < NbSampleControl_ ; ++i)
{
for(int j=0;j<3;j++)
{
filteredCoM[i].x[j] += outputDeltaCOMTraj_deq[i].x[j] ;
filteredCoM[i].y[j] += outputDeltaCOMTraj_deq[i].y[j] ;
- COMStates[i].x[j] += outputDeltaCOMTraj_deq[i].x[j] ;
- COMStates[i].y[j] += outputDeltaCOMTraj_deq[i].y[j] ;
+ FinalCOMTraj_deq[i].x[j] += outputDeltaCOMTraj_deq[i].x[j] ;
+ FinalCOMTraj_deq[i].y[j] += outputDeltaCOMTraj_deq[i].y[j] ;
}
- m_kajitaDynamicFilter->ComputeZMPMB(m_SamplingPeriod, filteredCoM[i],
- LeftFootAbsolutePositions[i], RightFootAbsolutePositions[i],
- filteredZMPMB[i] , stage1, i);
+ dynamicFilter_->ComputeZMPMB(m_SamplingPeriod, filteredCoM[i],
+ FinalLeftFootTraj_deq[i], FinalRightFootTraj_deq[i],
+ filteredZMPMB[i] , stage2, ControlIteration + i);
}
//deque<COMState> tmp = FinalCOMTraj_deq ;
@@ -785,7 +796,7 @@ void
aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
- for (unsigned int i = CurrentIndex_ ; i < FinalCOMTraj_deq.size() ; ++i )
+ for (unsigned int i = 0 ; i < FinalCOMTraj_deq.size()-CurrentIndex_ ; ++i )
{
aof << filterprecision( FinalLeftFootTraj_deq[i].x ) << " " // 1
<< filterprecision( FinalLeftFootTraj_deq[i].y ) << " " // 2
@@ -837,10 +848,12 @@ void
<< filterprecision( FinalCOMTraj_deq[i].yaw[2] ) << " " // 48
<< filterprecision( FinalLeftFootTraj_deq[i].dddx ) << " " // 49
<< filterprecision( FinalRightFootTraj_deq[i].dddx ) << " " // 50
- << filterprecision( zmpmb[i-CurrentIndex_][0] ) << " " // 51
- << filterprecision( zmpmb[i-CurrentIndex_][1] ) << " " // 52
+ << filterprecision( zmpmb[i][0] ) << " " // 51
+ << filterprecision( zmpmb[i][1] ) << " " // 52
<< filterprecision( FinalZMPTraj_deq[i].px ) << " " // 53
<< filterprecision( FinalZMPTraj_deq[i].py ) << " " // 54
+ << filterprecision( filteredZMPMB[i][0] ) << " " // 55
+ << filterprecision( filteredZMPMB[i][1] ) << " " // 56
<< endl ;
}
aof.close();
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index f21e9854..6b8d34d1 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -225,10 +225,6 @@ namespace PatternGeneratorJRL
deque<FootAbsolutePosition> LeftFootTraj_deq_ ;
deque<FootAbsolutePosition> RightFootTraj_deq_ ;
- deque<COMState> tmpCoM_ ;
- deque<ZMPPosition> tmpZMP_ ;
- deque<FootAbsolutePosition> tmpRF_ ;
- deque<FootAbsolutePosition> tmpLF_ ;
vector< vector<double> > FootPrw_vec ;
/// \brief Index where to begin the interpolation
--
GitLab