From 7383eaa3fe2fb44d7c9980662479d39f6f85f331 Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Fri, 18 Mar 2016 17:50:10 +0100
Subject: [PATCH] generalize the SQP period of recalculation
---
.../ZMPVelocityReferencedSQP.cpp | 78 ++++++++++---------
.../ZMPVelocityReferencedSQP.hh | 21 ++---
2 files changed, 54 insertions(+), 45 deletions(-)
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp
index 43eeea4f..d14899ca 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp
@@ -65,9 +65,11 @@ ZMPRefTrajectoryGeneration(SPM),OFTG_(NULL),dynamicFilter_(NULL),CurrentIndexUpp
m_SamplingPeriod = 0.005 ;
// Generator Management
- InterpolationPeriod_ = m_SamplingPeriod*10;
- previewSize_ = 9 ;
+ InterpolationPeriod_ = m_SamplingPeriod*7;
+ previewSize_ = 8 ;
previewDuration_ = (previewSize_-1)*SQP_T_ ;
+ outputPreviewDuration_ = m_SamplingPeriod ;
+ NbSampleOutput_ = (int)round(outputPreviewDuration_/m_SamplingPeriod) + 1 ;
NbSampleControl_ = (int)round(SQP_T_/m_SamplingPeriod) ;
NbSampleInterpolation_ = (int)round(SQP_T_/InterpolationPeriod_) ;
StepPeriod_ = 0.8 ;
@@ -144,6 +146,8 @@ ZMPRefTrajectoryGeneration(SPM),OFTG_(NULL),dynamicFilter_(NULL),CurrentIndexUpp
LeftFootTraj_deq_ctrl_ .resize( previewSize_ * NbSampleControl_ + CurrentIndexUpperBound_);
RightFootTraj_deq_ctrl_.resize( previewSize_ * NbSampleControl_ + CurrentIndexUpperBound_);
+ deltaCOMTraj_deq_.resize((int)round(outputPreviewDuration_/m_SamplingPeriod));
+
JerkX_ .clear();
JerkY_ .clear();
FootStepX_ .clear();
@@ -351,7 +355,7 @@ int ZMPVelocityReferencedSQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
dynamicFilter_->getComAndFootRealization()->ShiftFoot(true);
dynamicFilter_->init(m_SamplingPeriod,
InterpolationPeriod_,
- m_SamplingPeriod,
+ outputPreviewDuration_,
previewDuration_ ,
previewDuration_-SQP_T_,
lStartingCOMState);
@@ -384,14 +388,13 @@ void ZMPVelocityReferencedSQP::OnLine(double time,
}
// Test if the end of the online mode has been reached.
- if ((EndingPhase_) &&
- (time>=TimeToStopOnLineMode_))
+ if ((EndingPhase_) && (time>=TimeToStopOnLineMode_))
{ m_OnLineMode = false; }
// UPDATE WALKING TRAJECTORIES:
// ----------------------------
- //if(time + 0.00001 > UpperTimeLimitToUpdate_)
- //{
+ if(time + 0.00001 > UpperTimeLimitToUpdate_)
+ {
// UPDATE INTERNAL DATA:
// ---------------------
if(PerturbationOccured_ &&
@@ -427,24 +430,24 @@ void ZMPVelocityReferencedSQP::OnLine(double time,
double ltime = end.tv_sec-begin.tv_sec
+ 0.000001 * (end.tv_usec - begin.tv_usec);
-// bool endline = false ;
-// if(ltime >= 0.0005)
-// {
-// cout << ltime * 1000 << " "
-// << NMPCgenerator_->cput()*1000 << " "
-// << ltime * 1000 - NMPCgenerator_->cput()*1000 ;
-// endline = true;
-// }
-// if((ltime * 1000 - NMPCgenerator_->cput()*1000)>= 0.5)
-// {
-// ++warning;
-// cout << " : warning on cpu time ; " << warning ;
-// endline = true;
-// }
-// if(endline)
-// {
-// cout << endl;
-// }
+ bool endline = false ;
+ if(ltime >= 0.0005)
+ {
+ cout << ltime * 1000 << " "
+ << NMPCgenerator_->cput()*1000 << " "
+ << ltime * 1000 - NMPCgenerator_->cput()*1000 ;
+ endline = true;
+ }
+ if((ltime * 1000 - NMPCgenerator_->cput()*1000)>= 0.5)
+ {
+ ++warning;
+ cout << " : warning on cpu time ; " << warning ;
+ endline = true;
+ }
+ if(endline)
+ {
+ cout << endl;
+ }
// INITIALIZE INTERPOLATION:
// ------------------------
@@ -462,9 +465,9 @@ void ZMPVelocityReferencedSQP::OnLine(double time,
FullTrajectoryInterpolation(time);
// Take only the data that are actually used by the robot
- FinalZMPTraj_deq.resize(2); FinalLeftFootTraj_deq .resize(2);;
- FinalCOMTraj_deq.resize(2); FinalRightFootTraj_deq.resize(2);;
- for(unsigned i=0 ; i < 2 ; ++i)
+ FinalZMPTraj_deq.resize(NbSampleOutput_); FinalLeftFootTraj_deq .resize(NbSampleOutput_);;
+ FinalCOMTraj_deq.resize(NbSampleOutput_); FinalRightFootTraj_deq.resize(NbSampleOutput_);;
+ for(unsigned i=0 ; i < NbSampleOutput_ ; ++i)
{
FinalZMPTraj_deq [i] = ZMPTraj_deq_ctrl_ [i] ;
FinalCOMTraj_deq [i] = COMTraj_deq_ctrl_ [i] ;
@@ -506,17 +509,22 @@ void ZMPVelocityReferencedSQP::OnLine(double time,
{
if (EndingPhase_ == false)
{
- TimeToStopOnLineMode_ = UpperTimeLimitToUpdate_ + SQP_T_ * SQP_N_ + m_SamplingPeriod;
+ TimeToStopOnLineMode_ = UpperTimeLimitToUpdate_
+ + outputPreviewDuration_ * SQP_N_
+ + m_SamplingPeriod;
}
- UpperTimeLimitToUpdate_ = UpperTimeLimitToUpdate_ + SQP_T_ + m_SamplingPeriod ;
+ UpperTimeLimitToUpdate_ = UpperTimeLimitToUpdate_
+ + outputPreviewDuration_ + m_SamplingPeriod ;
}else{
if (EndingPhase_ == false)
{
- TimeToStopOnLineMode_ = UpperTimeLimitToUpdate_ + SQP_T_ * SQP_N_;
+ TimeToStopOnLineMode_ = UpperTimeLimitToUpdate_
+ + outputPreviewDuration_ * SQP_N_;
}
- UpperTimeLimitToUpdate_ = UpperTimeLimitToUpdate_ + SQP_T_;
+ UpperTimeLimitToUpdate_ = UpperTimeLimitToUpdate_
+ + outputPreviewDuration_;
}
- //}
+ }
//-----------------------------------
//
//
@@ -542,7 +550,7 @@ void ZMPVelocityReferencedSQP::FullTrajectoryInterpolation(double time)
LIPM_.setState(itCOM_);
CoMZMPInterpolation(JerkX_,JerkY_,&LIPM_,NbSampleControl_,0,CurrentIndex_,SupportStates_deq);
- itCOM_ = COMTraj_deq_ctrl_[1];
+ itCOM_ = COMTraj_deq_ctrl_[NbSampleOutput_-1];
support_state_t currentSupport = SupportStates_deq[0] ;
currentSupport.StepNumber=0;
@@ -627,7 +635,7 @@ void ZMPVelocityReferencedSQP::CoMZMPInterpolation(
const double tf = 0.75;
jx = 6/(tf*tf*tf)*(jx - tf*COMTraj_deq_ctrl_[i-1].x[1] - (tf*tf/2)*COMTraj_deq_ctrl_[i-1].x[2]);
jy = 6/(tf*tf*tf)*(jy - tf*COMTraj_deq_ctrl_[i-1].y[1] - (tf*tf/2)*COMTraj_deq_ctrl_[i-1].y[2]);
- LIPM->Interpolation( COMTraj_deq_ctrl_, ZMPTraj_deq_ctrl_, currentIndex + IterationNumber * numberOfSample, jx, jy);
+ LIPM->Interpolation( COMTraj_deq_ctrl_, ZMPTraj_deq_ctrl_, currentIndex, jx, jy);
}
else
{
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.hh
index f4e7e6de..b7cb13ed 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.hh
@@ -259,32 +259,31 @@ namespace PatternGeneratorJRL
FootAbsolutePosition initRightFoot_ ;
COMState itCOM_ ;
- /// \brief Duration of the preview for filtering
- double previewDuration_ ;
- /// \brief Size of the preview for filtering
- int previewSize_ ;
+ /// \brief Size of the output preview
+ unsigned NbSampleOutput_ ;
/// \brief Number of interpolated point needed for control computed during QP_T_
unsigned NbSampleControl_ ;
/// \brief Number of interpolated point needed for the dynamic filter computed during QP_T_
unsigned NbSampleInterpolation_ ;
+ /// \brief Size of the preview for filtering
+ unsigned previewSize_ ;
+
+ /// \brief Duration of the preview for filtering
+ double previewDuration_ ;
+ /// \brief Duration of the output preview
+ double outputPreviewDuration_ ;
/// \brief Interpolation Period for the dynamic filter
double InterpolationPeriod_ ;
-
/// \brief Step Period of the robot
double StepPeriod_ ;
-
/// \brief Period where the robot is on ONE feet
double SSPeriod_ ;
-
/// \brief Period where the robot is on TWO feet
double DSPeriod_ ;
-
/// \brief Maximum distance between the feet
double FeetDistance_ ;
-
/// \brief Maximum height of the feet
double StepHeight_ ;
-
/// \brief Height of the CoM
double CoMHeight_ ;
@@ -335,6 +334,8 @@ namespace PatternGeneratorJRL
const int IterationNumber, // INPUT
const unsigned int currentIndex, // INPUT
const std::deque<support_state_t> & SupportStates_deq );// INPUT
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
--
GitLab