Skip to content
Snippets Groups Projects
Commit b7b93de0 authored by mnaveau's avatar mnaveau
Browse files

Creation of independant interpolation function for decoupling the dynamic filter from the pg

parent 47d2aee8
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 303 additions and 119 deletions
src/PreviewControl/LinearizedInvertedPendulum2D.cpp
+ 15
− 0
View file @ b7b93de0
...@@ -127,6 +127,21 @@ void LinearizedInvertedPendulum2D::setState(COMState aCoM) ...@@ -127,6 +127,21 @@ void LinearizedInvertedPendulum2D::setState(COMState aCoM)
m_CoM.z[2] = aCoM.z[2]; m_CoM.z[2] = aCoM.z[2];
} }
COMState LinearizedInvertedPendulum2D::GetState()
{
COMState aCoM ;
aCoM.x[0] = m_CoM.x[0];
aCoM.x[1] = m_CoM.x[1];
aCoM.x[2] = m_CoM.x[2];
aCoM.y[0] = m_CoM.y[0];
aCoM.y[1] = m_CoM.y[1];
aCoM.y[2] = m_CoM.y[2];
aCoM.z[0] = m_CoM.z[0];
aCoM.z[1] = m_CoM.z[1];
aCoM.z[2] = m_CoM.z[2];
return aCoM ;
}
void LinearizedInvertedPendulum2D::setState(com_t aCoM) void LinearizedInvertedPendulum2D::setState(com_t aCoM)
{ {
m_CoM = aCoM ; m_CoM = aCoM ;
......
src/PreviewControl/LinearizedInvertedPendulum2D.hh
+ 3
− 1
View file @ b7b93de0
...@@ -151,7 +151,9 @@ namespace PatternGeneratorJRL ...@@ -151,7 +151,9 @@ namespace PatternGeneratorJRL
/*! Get state. */ /*! Get state. */
void GetState(MAL_VECTOR_TYPE(double) &lxk); void GetState(MAL_VECTOR_TYPE(double) &lxk);
inline com_t GetState() COMState GetState();
inline com_t getState()
{return m_CoM ;} {return m_CoM ;}
/*! Set state. */ /*! Set state. */
......
src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+ 226
− 93
View file @ b7b93de0
...@@ -76,14 +76,15 @@ double filterprecision(double adb) ...@@ -76,14 +76,15 @@ double filterprecision(double adb)
ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM, ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
string , CjrlHumanoidDynamicRobot *aHS ) : string , CjrlHumanoidDynamicRobot *aHS ) :
ZMPRefTrajectoryGeneration(SPM), ZMPRefTrajectoryGeneration(SPM),
Robot_(0),SupportFSM_(0),OrientPrw_(0),VRQPGenerator_(0),IntermedData_(0),RFI_(0),Problem_(),Solution_(),EPS_(1e-6),OFTG_(0) Robot_(0),SupportFSM_(0),OrientPrw_(0),VRQPGenerator_(0),IntermedData_(0),
RFI_(0),Problem_(),Solution_(),OFTG_DF_(0),OFTG_control_(0)
{ {
Running_ = false ; Running_ = false ;
TimeBuffer_ = 0.04 ; TimeBuffer_ = 0.04 ;
QP_T_ = 0.1 ; QP_T_ = 0.1 ;
QP_N_ = 16 ; QP_N_ = 16 ;
m_SamplingPeriod = 0.005 ; m_SamplingPeriod = 0.005 ;
InterpolationPeriod_ = 0.005 ; InterpolationPeriod_ = QP_T_/20 ;
StepPeriod_ = 0.8 ; StepPeriod_ = 0.8 ;
SSPeriod = 0.7 ; SSPeriod = 0.7 ;
DSPeriod = 0.1 ; DSPeriod = 0.1 ;
...@@ -123,9 +124,19 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM, ...@@ -123,9 +124,19 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
LIPM_control_.InitializeSystem(); LIPM_control_.InitializeSystem();
// Initialize the 2D LIPM // Initialize the 2D LIPM
LIPM_.SetSimulationControlPeriod( QP_T_ ); LIPM_control_subsampled_.SetSimulationControlPeriod( QP_T_ );
LIPM_.SetRobotControlPeriod( InterpolationPeriod_ ); LIPM_control_subsampled_.SetRobotControlPeriod( InterpolationPeriod_ );
LIPM_.InitializeSystem(); LIPM_control_subsampled_.InitializeSystem();
// Initialize the 2D LIPM
LIPM_DF_.SetSimulationControlPeriod( QP_T_ );
LIPM_DF_.SetRobotControlPeriod( m_SamplingPeriod );
LIPM_DF_.InitializeSystem();
// Initialize the 2D LIPM
LIPM_DF_subsampled_.SetSimulationControlPeriod( QP_T_ );
LIPM_DF_subsampled_.SetRobotControlPeriod( InterpolationPeriod_ );
LIPM_DF_subsampled_.InitializeSystem();
// Create and initialize simplified robot model // Create and initialize simplified robot model
Robot_ = new RigidBodySystem( SPM, aHS, SupportFSM_ ); Robot_ = new RigidBodySystem( SPM, aHS, SupportFSM_ );
...@@ -153,21 +164,21 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM, ...@@ -153,21 +164,21 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
// Create and initialize online interpolation of feet trajectories: // Create and initialize online interpolation of feet trajectories:
// ---------------------------------------------------------------- // ----------------------------------------------------------------
OFTG_ = new OnLineFootTrajectoryGeneration(SPM,HDR_->leftFoot()); OFTG_DF_ = new OnLineFootTrajectoryGeneration(SPM,HDR_->leftFoot());
OFTG_->InitializeInternalDataStructures(); OFTG_DF_->InitializeInternalDataStructures();
OFTG_->SetSingleSupportTime( SSPeriod ); OFTG_DF_->SetSingleSupportTime( SSPeriod );
OFTG_->SetDoubleSupportTime( DSPeriod ); OFTG_DF_->SetDoubleSupportTime( DSPeriod );
OFTG_->SetSamplingPeriod( m_SamplingPeriod ); OFTG_DF_->SetSamplingPeriod( InterpolationPeriod_ );
OFTG_->QPSamplingPeriod( QP_T_ ); OFTG_DF_->QPSamplingPeriod( QP_T_ );
OFTG_->NbSamplingsPreviewed( QP_N_ ); OFTG_DF_->NbSamplingsPreviewed( QP_N_ );
OFTG_->FeetDistance( FeetDistance ); OFTG_DF_->FeetDistance( FeetDistance );
OFTG_->StepHeight( StepHeight ); OFTG_DF_->StepHeight( StepHeight );
OFTG_control_ = new OnLineFootTrajectoryGeneration(SPM,HDR_->leftFoot()); OFTG_control_ = new OnLineFootTrajectoryGeneration(SPM,HDR_->leftFoot());
OFTG_control_->InitializeInternalDataStructures(); OFTG_control_->InitializeInternalDataStructures();
OFTG_control_->SetSingleSupportTime( SSPeriod ); OFTG_control_->SetSingleSupportTime( SSPeriod );
OFTG_control_->SetDoubleSupportTime( DSPeriod ); OFTG_control_->SetDoubleSupportTime( DSPeriod );
OFTG_control_->SetSamplingPeriod( InterpolationPeriod_ ); OFTG_control_->SetSamplingPeriod( m_SamplingPeriod );
OFTG_control_->QPSamplingPeriod( QP_T_ ); OFTG_control_->QPSamplingPeriod( QP_T_ );
OFTG_control_->NbSamplingsPreviewed( QP_N_ ); OFTG_control_->NbSamplingsPreviewed( QP_N_ );
OFTG_control_->FeetDistance( FeetDistance ); OFTG_control_->FeetDistance( FeetDistance );
...@@ -199,31 +210,34 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM, ...@@ -199,31 +210,34 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
// Initialization of the Kajita preview controls (ICRA 2003). // Initialization of the Kajita preview controls (ICRA 2003).
MAL_MATRIX_RESIZE(m_deltax,3,1); MAL_MATRIX_RESIZE(m_deltay,3,1); MAL_MATRIX_RESIZE(m_deltax,3,1); MAL_MATRIX_RESIZE(m_deltay,3,1);
PC_ = new PreviewControl(SPM, OptimalControllerSolver::MODE_WITH_INITIALPOS, false); PC_ = new PreviewControl(SPM, OptimalControllerSolver::MODE_WITH_INITIALPOS, false);
PC_->SetPreviewControlTime (QP_T_*QP_N_ - QP_T_/InterpolationPeriod_*m_SamplingPeriod); PC_->SetPreviewControlTime (QP_T_*QP_N_ - QP_T_/m_SamplingPeriod * InterpolationPeriod_);
PC_->SetSamplingPeriod (m_SamplingPeriod); PC_->SetSamplingPeriod (InterpolationPeriod_);
PC_->SetHeightOfCoM(CoMHeight_); PC_->SetHeightOfCoM(CoMHeight_);
// init of the buffer for the kajita's dynamic filter // init of the buffer for the kajita's dynamic filter
// number of sample inside one iteration of the preview control // number of sample inside one iteration of the preview control
NumberOfSample_ = (unsigned)(QP_T_/m_SamplingPeriod); NbSampleControl_ = (unsigned)(QP_T_/m_SamplingPeriod) ;
NbSampleInterpolation_ = (unsigned)(QP_T_/InterpolationPeriod_) ;
// size = numberOfIterationOfThePreviewControl * NumberOfSample + Margin // size = numberOfIterationOfThePreviewControl * NumberOfSample + Margin
ZMPTraj_deq_.resize( QP_N_ * NumberOfSample_+20); ZMPTraj_deq_.resize( QP_N_ * NbSampleInterpolation_+20);
COMTraj_deq_.resize( QP_N_ * NumberOfSample_+20); COMTraj_deq_.resize( QP_N_ * NbSampleInterpolation_+20);
tmpCoM_.resize(QP_N_ * NbSampleInterpolation_ + 20);
tmpZMP_.resize(QP_N_ * NbSampleInterpolation_ + 20);
ConfigurationTraj_.resize( QP_N_ * NumberOfSample_ ); ConfigurationTraj_.resize( QP_N_ * NbSampleInterpolation_ );
VelocityTraj_.resize( QP_N_ * NumberOfSample_ ); VelocityTraj_.resize( QP_N_ * NbSampleInterpolation_ );
AccelerationTraj_.resize( QP_N_ * NumberOfSample_ ); AccelerationTraj_.resize( QP_N_ * NbSampleInterpolation_ );
DeltaZMPMBPositions_.resize ( QP_N_ * NumberOfSample_ ); DeltaZMPMBPositions_.resize ( QP_N_ * NbSampleInterpolation_ );
// Initialization of the configuration vectors // Initialization of the configuration vectors
PreviousConfiguration_ = aHS->currentConfiguration() ; PreviousConfiguration_ = aHS->currentConfiguration() ;
PreviousVelocity_ = aHS->currentVelocity(); PreviousVelocity_ = aHS->currentVelocity();
PreviousAcceleration_ = aHS->currentAcceleration(); PreviousAcceleration_ = aHS->currentAcceleration();
ComStateBuffer_.resize(QP_T_/InterpolationPeriod_); ComStateBuffer_.resize(NbSampleControl_);
Once_ = true ; Once_ = true ;
DInitX_ = 0 ; DInitX_ = 0 ;
...@@ -422,13 +436,21 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq, ...@@ -422,13 +436,21 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
LIPM_control_.SetComHeight(lStartingCOMState.z[0]); LIPM_control_.SetComHeight(lStartingCOMState.z[0]);
LIPM_control_.InitializeSystem(); LIPM_control_.InitializeSystem();
LIPM_control_(CoM); LIPM_control_(CoM);
//--
LIPM_control_subsampled_.SetComHeight(lStartingCOMState.z[0]);
LIPM_control_subsampled_.InitializeSystem();
LIPM_control_subsampled_(CoM);
//--
LIPM_DF_.SetComHeight(lStartingCOMState.z[0]);
LIPM_DF_.InitializeSystem();
LIPM_DF_(CoM);
//--
LIPM_DF_subsampled_.SetComHeight(lStartingCOMState.z[0]);
LIPM_DF_subsampled_.InitializeSystem();
LIPM_DF_subsampled_(CoM);
//--
IntermedData_->CoM(LIPM_control_()); IntermedData_->CoM(LIPM_control_());
// initialisation of a second object that allow the interpolation along 1.6s
LIPM_.SetComHeight(lStartingCOMState.z[0]);
LIPM_.InitializeSystem();
LIPM_(CoM);
// Initialize preview of orientations // Initialize preview of orientations
OrientPrw_->CurrentTrunkState( lStartingCOMState ); OrientPrw_->CurrentTrunkState( lStartingCOMState );
...@@ -439,6 +461,11 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq, ...@@ -439,6 +461,11 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
Problem_.nbInvariantCols(2*QP_N_); Problem_.nbInvariantCols(2*QP_N_);
VRQPGenerator_->build_invariant_part( Problem_ ); VRQPGenerator_->build_invariant_part( Problem_ );
// initialize intermed data needed during the interpolation
InitStateLIPMcontrol_ = LIPM_control_.GetState() ;
InitStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
FinalStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
return 0; return 0;
} }
...@@ -532,44 +559,29 @@ ZMPVelocityReferencedQP::OnLine(double time, ...@@ -532,44 +559,29 @@ ZMPVelocityReferencedQP::OnLine(double time,
solution_ = Solution_ ; solution_ = Solution_ ;
for (unsigned i = 0 ; i < CurrentIndex_ ; i++) for (unsigned i = 0 ; i < CurrentIndex_ ; i++)
{ {
ZMPTraj_deq_[i] = FinalZMPTraj_deq[ i ] ; ZMPTraj_deq_[i] = FinalZMPTraj_deq[i] ;
COMTraj_deq_[i] = FinalCOMTraj_deq[ i ] ; COMTraj_deq_[i] = tmpCoM_[i] = FinalCOMTraj_deq[i] ;
}
FinalZMPTraj_deq.resize( QP_T_/m_SamplingPeriod + CurrentIndex_ );
FinalCOMTraj_deq.resize( QP_T_/m_SamplingPeriod + CurrentIndex_ );
// interpolation on the first QP_T_ second
Interpolation(FinalZMPTraj_deq, FinalCOMTraj_deq ,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
&solution_, &LIPM_control_, OrientPrw_, OFTG_control_,
CurrentIndex_, time, 0 ) ;
for (unsigned i = 0 ; i < NumberOfSample_ ; i++)
{
ZMPTraj_deq_[ CurrentIndex_ + i] = FinalZMPTraj_deq[ CurrentIndex_ + i * InterpolationPeriod_ / m_SamplingPeriod ] ;
COMTraj_deq_[ CurrentIndex_ + i] = FinalCOMTraj_deq[ CurrentIndex_ + i * InterpolationPeriod_ / m_SamplingPeriod ] ;
} }
LeftFootTraj_deq_ = FinalLeftFootTraj_deq ; LeftFootTraj_deq_ = FinalLeftFootTraj_deq ;
RightFootTraj_deq_ = FinalRightFootTraj_deq ; RightFootTraj_deq_ = FinalRightFootTraj_deq ;
FinalZMPTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
FinalCOMTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
LIPM_.setState(LIPM_control_.GetState()); // INTERPOLATION
for ( int i = 1 ; i < QP_N_ ; i++ ){ ControlInterpolation( FinalZMPTraj_deq, FinalLeftFootTraj_deq,
Interpolation(ZMPTraj_deq_, COMTraj_deq_ , FinalRightFootTraj_deq, time) ;
LeftFootTraj_deq_, RightFootTraj_deq_, DynamicFilterInterpolation( FinalCOMTraj_deq, time) ;
&solution_, &LIPM_, OrientPrw_, OFTG_,
CurrentIndex_, time, i ) ;
}
// DYNAMIC FILTER // DYNAMIC FILTER
// -------------- // --------------
DynamicFilter( ZMPTraj_deq_, COMTraj_deq_, LeftFootTraj_deq_, RightFootTraj_deq_, CurrentIndex_, time ); DynamicFilter( ZMPTraj_deq_, COMTraj_deq_, LeftFootTraj_deq_, RightFootTraj_deq_, CurrentIndex_, time );
//LIPM_control_.setState(COMTraj_deq_[NumberOfSample_ + CurrentIndex_ - 1]);
OrientPrw_->CurrentTrunkState(COMTraj_deq_[NumberOfSample_ + CurrentIndex_ - 1]);
// RECOPIE DU BUFFER // COPY THE RESULTS
// ----------------- // ----------------
for (unsigned int i = CurrentIndex_ ; i < FinalZMPTraj_deq.size() ; i++ ) for (unsigned int i = CurrentIndex_ ; i < FinalCOMTraj_deq.size() ; i++ )
FinalCOMTraj_deq[i] = COMTraj_deq_[i] ; FinalCOMTraj_deq[i] = COMTraj_deq_[i] ;
LIPM_DF_.setState(FinalCOMTraj_deq[NbSampleControl_ + CurrentIndex_ - 1]);
LIPM_DF_subsampled_.setState(FinalCOMTraj_deq[NbSampleControl_ + CurrentIndex_ - 1]) ;
// Specify that we are in the ending phase. // Specify that we are in the ending phase.
if (EndingPhase_ == false) if (EndingPhase_ == false)
...@@ -638,6 +650,88 @@ int ZMPVelocityReferencedQP::ReturnOptimalTimeToRegenerateAStep() ...@@ -638,6 +650,88 @@ int ZMPVelocityReferencedQP::ReturnOptimalTimeToRegenerateAStep()
return 2*r; return 2*r;
} }
void ZMPVelocityReferencedQP::ControlInterpolation(
std::deque<ZMPPosition> & FinalZMPTraj_deq,
std::deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
std::deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
double time)
{
InitStateLIPMcontrol_ = LIPM_control_.GetState() ;
// INTERPOLATE CoM AND ZMP TRAJECTORIES:
// -------------------------------------
CoMZMPInterpolation(FinalZMPTraj_deq, tmpCoM_,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
&solution_, &LIPM_control_, NbSampleControl_, 0);
// INTERPOLATE TRUNK ORIENTATION:
// ------------------------------
InitStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
OrientPrw_->interpolate_trunk_orientation( time, CurrentIndex_,
m_SamplingPeriod, solution_.SupportStates_deq,
tmpCoM_ );
// INTERPOLATE THE COMPUTED FOOT POSITIONS:
// ----------------------------------------
OFTG_control_->interpolate_feet_positions( time,
solution_.SupportStates_deq, solution_,
solution_.SupportOrientations_deq,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq);
return ;
}
void ZMPVelocityReferencedQP::DynamicFilterInterpolation(
std::deque<COMState> & FinalCOMTraj_deq,
double time)
{
LIPM_control_subsampled_.setState(InitStateLIPMcontrol_) ;
for ( int i = 0 ; i < QP_N_ ; i++ )
{
// INTERPOLATE ZMP TRAJECTORIES:
// -----------------------------
CoMZMPInterpolation(ZMPTraj_deq_, tmpCoM_,
LeftFootTraj_deq_, RightFootTraj_deq_,
&solution_, &LIPM_control_subsampled_,
NbSampleInterpolation_, i);
}
// INTERPOLATE COM TRAJECTORIES:
// -----------------------------
for ( int i = 0 ; i < QP_N_ ; i++ )
{
CoMZMPInterpolation(tmpZMP_, COMTraj_deq_,
LeftFootTraj_deq_, RightFootTraj_deq_,
&solution_, &LIPM_DF_subsampled_,
NbSampleInterpolation_, i);
}
CoMZMPInterpolation(tmpZMP_, FinalCOMTraj_deq,
LeftFootTraj_deq_, RightFootTraj_deq_,
&solution_, &LIPM_DF_, NbSampleControl_, 0);
// INTERPOLATE TRUNK ORIENTATION and FEET POSITIONS :
// --------------------------------------------------
OrientPrw_->CurrentTrunkState(InitStateOrientPrw_) ;
OrientPrw_->interpolate_trunk_orientation( time, CurrentIndex_,
m_SamplingPeriod, solution_.SupportStates_deq,
FinalCOMTraj_deq );
OrientPrw_->CurrentTrunkState(InitStateOrientPrw_) ;
for ( int i = 0 ; i < QP_N_ ; i++ )
{
OrientPrw_->interpolate_trunk_orientation( time + i * QP_T_,
CurrentIndex_ + i * NbSampleInterpolation_, InterpolationPeriod_,
solution_.SupportStates_deq, COMTraj_deq_ );
OFTG_DF_->interpolate_feet_positions( time + i * QP_T_,
solution_.SupportStates_deq, solution_,
solution_.SupportOrientations_deq,
LeftFootTraj_deq_, RightFootTraj_deq_);
solution_.SupportStates_deq.pop_front();
}
return ;
}
int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions, int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions,
std::deque<COMState> & COMTraj_deq , std::deque<COMState> & COMTraj_deq ,
std::deque<FootAbsolutePosition>& LeftFootAbsolutePositions, std::deque<FootAbsolutePosition>& LeftFootAbsolutePositions,
...@@ -646,7 +740,7 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions ...@@ -646,7 +740,7 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions
double time double time
) )
{ {
const unsigned int N = NumberOfSample_ * QP_N_ ; const unsigned int N = NbSampleInterpolation_ * QP_N_ ;
// \brief calculate, from the CoM computed by the preview control, // \brief calculate, from the CoM computed by the preview control,
// the corresponding articular position, velocity and acceleration // the corresponding articular position, velocity and acceleration
// ------------------------------------------------------------------ // ------------------------------------------------------------------
...@@ -690,9 +784,9 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions ...@@ -690,9 +784,9 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions
/// Preview control on the ZMPMBs computed /// Preview control on the ZMPMBs computed
/// -------------------------------------- /// --------------------------------------
//init of the Kajita preview control //init of the Kajita preview control
PC_->SetPreviewControlTime (QP_T_*QP_N_ - QP_T_/InterpolationPeriod_*m_SamplingPeriod); PC_->SetPreviewControlTime (QP_T_*QP_N_ - NbSampleControl_*InterpolationPeriod_);
PC_->SetSamplingPeriod (m_SamplingPeriod); PC_->SetSamplingPeriod (InterpolationPeriod_);
PC_->SetHeightOfCoM(0.814); PC_->SetHeightOfCoM(CoMHeight_);
PC_->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITH_INITIALPOS); PC_->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITH_INITIALPOS);
for(int j=0;j<3;j++) for(int j=0;j<3;j++)
{ {
...@@ -703,7 +797,7 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions ...@@ -703,7 +797,7 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions
double deltaZMPx (0) , deltaZMPy (0) ; double deltaZMPx (0) , deltaZMPy (0) ;
// calcul of the preview control along the "ZMPPositions" // calcul of the preview control along the "ZMPPositions"
for (unsigned i = 0 ; i < QP_T_/InterpolationPeriod_ ; i++ ) for (unsigned i = 0 ; i < NbSampleControl_ ; i++ )
{ {
PC_->OneIterationOfPreview(m_deltax,m_deltay, PC_->OneIterationOfPreview(m_deltax,m_deltay,
aSxzmp,aSyzmp, aSxzmp,aSyzmp,
...@@ -716,16 +810,73 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions ...@@ -716,16 +810,73 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions
} }
} }
for (unsigned int i = 0 ; i < QP_T_/InterpolationPeriod_ ; i++) for (unsigned int i = 0 ; i < NbSampleControl_ ; i++)
{ {
for(int j=0;j<3;j++) for(int j=0;j<3;j++)
{ {
if ( ComStateBuffer_[i].x[j] == ComStateBuffer_[i].x[j] ) if ( ComStateBuffer_[i].x[j] == ComStateBuffer_[i].x[j] ||
ComStateBuffer_[i].y[j] == ComStateBuffer_[i].y[j] )
{
COMTraj_deq[currentIndex+i].x[j] += ComStateBuffer_[i].x[j] ; COMTraj_deq[currentIndex+i].x[j] += ComStateBuffer_[i].x[j] ;
if ( ComStateBuffer_[i].y[j] == ComStateBuffer_[i].y[j] )
COMTraj_deq[currentIndex+i].y[j] += ComStateBuffer_[i].y[j] ; COMTraj_deq[currentIndex+i].y[j] += ComStateBuffer_[i].y[j] ;
}
else
{
//cout << "kajita2003 preview control diverged\n" ;
}
} }
} }
/// \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("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 < DeltaZMPMBPositions_.size() ; ++i )
{
aof << filterprecision( DeltaZMPMBPositions_[i].px ) << " " // 1
<< filterprecision( DeltaZMPMBPositions_[i].py ) << " " // 2
<< endl ;
}
/// \brief Debug Purpose
/// --------------------
oss.str("TestHerdt2010DynamicDCOM");
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] ) << " " // 1
<< filterprecision( ComStateBuffer_[i].y[0] ) << " " // 2
<< filterprecision( ComStateBuffer_[i].x[1] ) << " " // 1
<< filterprecision( ComStateBuffer_[i].y[1] ) << " " // 2
<< filterprecision( ComStateBuffer_[i].x[2] ) << " " // 1
<< filterprecision( ComStateBuffer_[i].y[2] ) << " " // 2
<< endl ;
}
iteration++;
return 0; return 0;
} }
...@@ -799,7 +950,7 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(COMState & acomp, ...@@ -799,7 +950,7 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(COMState & acomp,
CurrentAcceleration, CurrentAcceleration,
IterationNumber, IterationNumber,
0); 0);
if(IterationNumber == NumberOfSample_-1 ){ if(IterationNumber == NbSampleInterpolation_-1 ){
HDR_->currentConfiguration(CurrentConfiguration); HDR_->currentConfiguration(CurrentConfiguration);
HDR_->currentConfiguration(CurrentVelocity); HDR_->currentConfiguration(CurrentVelocity);
HDR_->currentConfiguration(CurrentAcceleration); HDR_->currentConfiguration(CurrentAcceleration);
...@@ -812,18 +963,15 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(COMState & acomp, ...@@ -812,18 +963,15 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(COMState & acomp,
return ; return ;
} }
void ZMPVelocityReferencedQP::Interpolation(std::deque<ZMPPosition> & ZMPPositions, void ZMPVelocityReferencedQP::CoMZMPInterpolation(
std::deque<ZMPPosition> & ZMPPositions,
std::deque<COMState> & COMTraj_deq , std::deque<COMState> & COMTraj_deq ,
std::deque<FootAbsolutePosition> & LeftFootTraj_deq, std::deque<FootAbsolutePosition> & LeftFootTraj_deq,
std::deque<FootAbsolutePosition> & RightFootTraj_deq, std::deque<FootAbsolutePosition> & RightFootTraj_deq,
solution_t * Solution, solution_t * Solution,
LinearizedInvertedPendulum2D * LIPM, LinearizedInvertedPendulum2D * LIPM,
OrientationsPreview * OrientPrw, unsigned numberOfSample,
OnLineFootTrajectoryGeneration * OFTG, int IterationNumber)
unsigned currentIndex,
double time,
int IterationNumber
)
{ {
if(Solution->SupportStates_deq.size() && Solution->SupportStates_deq[0].NbStepsLeft == 0) if(Solution->SupportStates_deq.size() && Solution->SupportStates_deq[0].NbStepsLeft == 0)
{ {
...@@ -833,32 +981,17 @@ void ZMPVelocityReferencedQP::Interpolation(std::deque<ZMPPosition> & ZMPPositio ...@@ -833,32 +981,17 @@ void ZMPVelocityReferencedQP::Interpolation(std::deque<ZMPPosition> & ZMPPositio
const double tf = 0.75; const double tf = 0.75;
jx = 6/(tf*tf*tf)*(jx - tf*COMTraj_deq[0].x[1] - (tf*tf/2)*COMTraj_deq[0].x[2]); jx = 6/(tf*tf*tf)*(jx - tf*COMTraj_deq[0].x[1] - (tf*tf/2)*COMTraj_deq[0].x[2]);
jy = 6/(tf*tf*tf)*(jy - tf*COMTraj_deq[0].y[1] - (tf*tf/2)*COMTraj_deq[0].y[2]); jy = 6/(tf*tf*tf)*(jy - tf*COMTraj_deq[0].y[1] - (tf*tf/2)*COMTraj_deq[0].y[2]);
LIPM->Interpolation( COMTraj_deq, ZMPPositions, currentIndex + IterationNumber * NumberOfSample_, LIPM->Interpolation( COMTraj_deq, ZMPPositions, CurrentIndex_ + IterationNumber * numberOfSample,
jx, jy); jx, jy);
LIPM->OneIteration( jx, jy ); LIPM->OneIteration( jx, jy );
} }
else else
{ {
Running_ = true; Running_ = true;
LIPM->Interpolation( COMTraj_deq, ZMPPositions, currentIndex + IterationNumber * NumberOfSample_, LIPM->Interpolation( COMTraj_deq, ZMPPositions, CurrentIndex_ + IterationNumber * numberOfSample,
Solution->Solution_vec[IterationNumber], Solution->Solution_vec[IterationNumber+QP_N_] ); Solution->Solution_vec[IterationNumber], Solution->Solution_vec[IterationNumber+QP_N_] );
LIPM->OneIteration( Solution->Solution_vec[IterationNumber],Solution->Solution_vec[IterationNumber+QP_N_] ); LIPM->OneIteration( Solution->Solution_vec[IterationNumber],Solution->Solution_vec[IterationNumber+QP_N_] );
} }
// INTERPOLATE TRUNK ORIENTATION:
// ------------------------------
OrientPrw->interpolate_trunk_orientation( time + IterationNumber * QP_T_, currentIndex + IterationNumber * NumberOfSample_,
InterpolationPeriod_, Solution->SupportStates_deq,
COMTraj_deq_ );
// INTERPOLATE THE COMPUTED FOOT POSITIONS:
// ----------------------------------------
OFTG->interpolate_feet_positions( time + IterationNumber * QP_T_,
Solution->SupportStates_deq, *Solution,
Solution->SupportOrientations_deq,
LeftFootTraj_deq, RightFootTraj_deq);
// WARNING the interpolation modifie the solution send a copy as argument
Solution->SupportStates_deq.pop_front();
return ; return ;
} }
src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
+ 22
− 5
View file @ b7b93de0
...@@ -243,6 +243,9 @@ namespace PatternGeneratorJRL ...@@ -243,6 +243,9 @@ namespace PatternGeneratorJRL
deque<FootAbsolutePosition> LeftFootTraj_deq_ ; deque<FootAbsolutePosition> LeftFootTraj_deq_ ;
deque<FootAbsolutePosition> RightFootTraj_deq_ ; deque<FootAbsolutePosition> RightFootTraj_deq_ ;
deque<COMState> tmpCoM_ ;
deque<ZMPPosition> tmpZMP_ ;
/// \brief Index where to begin the interpolation /// \brief Index where to begin the interpolation
unsigned CurrentIndex_ ; unsigned CurrentIndex_ ;
...@@ -267,8 +270,11 @@ namespace PatternGeneratorJRL ...@@ -267,8 +270,11 @@ namespace PatternGeneratorJRL
/// \brief Height of the CoM /// \brief Height of the CoM
double CoMHeight_ ; double CoMHeight_ ;
/// \brief Number of interpolated point computed during QP_T_ (27/02/2014 :0.1) /// \brief Number of interpolated point needed for control computed during QP_T_
unsigned NumberOfSample_ ; unsigned NbSampleControl_ ;
/// \brief Number of interpolated point needed for the dynamic filter computed during QP_T_
unsigned NbSampleInterpolation_ ;
/// \brief Buffers for the CoM an Feet computation, i.e. the simplify inverse kinematics. /// \brief Buffers for the CoM an Feet computation, i.e. the simplify inverse kinematics.
vector <MAL_VECTOR_TYPE(double)> ConfigurationTraj_ ; vector <MAL_VECTOR_TYPE(double)> ConfigurationTraj_ ;
...@@ -287,7 +293,6 @@ namespace PatternGeneratorJRL ...@@ -287,7 +293,6 @@ namespace PatternGeneratorJRL
/// \brief Used to compute once, the initial difference between the ZMP and the ZMPMB /// \brief Used to compute once, the initial difference between the ZMP and the ZMPMB
bool Once_ ; bool Once_ ;
double DInitX_, DInitY_ ; double DInitX_, DInitY_ ;
const double EPS_ ;
/// \brief Buffer comtaimimg the difference between the ZMP computed from the Herdt controler /// \brief Buffer comtaimimg the difference between the ZMP computed from the Herdt controler
///and the ZMP Multibody computed from the articular trajectory ///and the ZMP Multibody computed from the articular trajectory
...@@ -301,6 +306,7 @@ namespace PatternGeneratorJRL ...@@ -301,6 +306,7 @@ namespace PatternGeneratorJRL
/// \brief Standard polynomial trajectories for the feet. /// \brief Standard polynomial trajectories for the feet.
OnLineFootTrajectoryGeneration * OFTG_; OnLineFootTrajectoryGeneration * OFTG_;
OnLineFootTrajectoryGeneration * OFTG_control_ ;
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // to use the vector of eigen used by metapod EIGEN_MAKE_ALIGNED_OPERATOR_NEW // to use the vector of eigen used by metapod
...@@ -355,7 +361,6 @@ namespace PatternGeneratorJRL ...@@ -355,7 +361,6 @@ namespace PatternGeneratorJRL
unsigned IterationNumber unsigned IterationNumber
); );
// WARNING the interpolation modifie the solution_t, send a copy as argument
void Interpolation(std::deque<ZMPPosition> & ZMPPositions, void Interpolation(std::deque<ZMPPosition> & ZMPPositions,
std::deque<COMState> & COMTraj_deq , std::deque<COMState> & COMTraj_deq ,
std::deque<FootAbsolutePosition> & LeftFootTraj_deq, std::deque<FootAbsolutePosition> & LeftFootTraj_deq,
...@@ -366,8 +371,20 @@ namespace PatternGeneratorJRL ...@@ -366,8 +371,20 @@ namespace PatternGeneratorJRL
OnLineFootTrajectoryGeneration * OFTG, OnLineFootTrajectoryGeneration * OFTG,
unsigned currentIndex, unsigned currentIndex,
double time, double time,
int IterationNumber int IterationNumber,
unsigned numberOfSample
); );
void CoMZMPInterpolation(
std::deque<ZMPPosition> & ZMPPositions,
std::deque<COMState> & COMTraj_deq ,
std::deque<FootAbsolutePosition> & LeftFootTraj_deq,
std::deque<FootAbsolutePosition> & RightFootTraj_deq,
solution_t * Solution,
LinearizedInvertedPendulum2D * LIPM,
unsigned currentIndex,
int IterationNumber,
unsigned numberOfSample);
}; };
} }
......
src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+ 36
− 20
View file @ b7b93de0
...@@ -90,7 +90,7 @@ namespace PatternGeneratorJRL ...@@ -90,7 +90,7 @@ namespace PatternGeneratorJRL
the queue of ZMP, and foot positions. the queue of ZMP, and foot positions.
*/ */
int InitOnLine(deque<ZMPPosition> & FinalZMPPositions, int InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
deque<COMState> & CoMStates, deque<COMState> & FinalCoMPositions_deq,
deque<FootAbsolutePosition> & FinalLeftFootTraj_deq, deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
deque<FootAbsolutePosition> & FinalRightFootTraj_deq, deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
FootAbsolutePosition & InitLeftFootAbsolutePosition, FootAbsolutePosition & InitLeftFootAbsolutePosition,
...@@ -103,7 +103,7 @@ namespace PatternGeneratorJRL ...@@ -103,7 +103,7 @@ namespace PatternGeneratorJRL
/// \brief Update the stacks on-line /// \brief Update the stacks on-line
void OnLine(double time, void OnLine(double time,
deque<ZMPPosition> & FinalZMPPositions, deque<ZMPPosition> & FinalZMPPositions,
deque<COMState> & CoMStates, deque<COMState> & FinalCOMTraj_deq,
deque<FootAbsolutePosition> &FinalLeftFootTraj_deq, deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
deque<FootAbsolutePosition> &FinalRightFootTraj_deq); deque<FootAbsolutePosition> &FinalRightFootTraj_deq);
...@@ -195,7 +195,9 @@ namespace PatternGeneratorJRL ...@@ -195,7 +195,9 @@ namespace PatternGeneratorJRL
/// \brief 2D LIPM to simulate the evolution of the robot's CoM. /// \brief 2D LIPM to simulate the evolution of the robot's CoM.
LinearizedInvertedPendulum2D LIPM_control_ ; LinearizedInvertedPendulum2D LIPM_control_ ;
LinearizedInvertedPendulum2D LIPM_ ; LinearizedInvertedPendulum2D LIPM_control_subsampled_ ;
LinearizedInvertedPendulum2D LIPM_DF_ ;
LinearizedInvertedPendulum2D LIPM_DF_subsampled_ ;
/// \brief Simplified robot model /// \brief Simplified robot model
RigidBodySystem * Robot_ ; RigidBodySystem * Robot_ ;
...@@ -243,6 +245,9 @@ namespace PatternGeneratorJRL ...@@ -243,6 +245,9 @@ namespace PatternGeneratorJRL
deque<FootAbsolutePosition> LeftFootTraj_deq_ ; deque<FootAbsolutePosition> LeftFootTraj_deq_ ;
deque<FootAbsolutePosition> RightFootTraj_deq_ ; deque<FootAbsolutePosition> RightFootTraj_deq_ ;
deque<COMState> tmpCoM_ ;
deque<ZMPPosition> tmpZMP_ ;
/// \brief Index where to begin the interpolation /// \brief Index where to begin the interpolation
unsigned CurrentIndex_ ; unsigned CurrentIndex_ ;
...@@ -267,8 +272,11 @@ namespace PatternGeneratorJRL ...@@ -267,8 +272,11 @@ namespace PatternGeneratorJRL
/// \brief Height of the CoM /// \brief Height of the CoM
double CoMHeight_ ; double CoMHeight_ ;
/// \brief Number of interpolated point computed during QP_T_ (27/02/2014 :0.1) /// \brief Number of interpolated point needed for control computed during QP_T_
unsigned NumberOfSample_ ; unsigned NbSampleControl_ ;
/// \brief Number of interpolated point needed for the dynamic filter computed during QP_T_
unsigned NbSampleInterpolation_ ;
/// \brief Buffers for the CoM an Feet computation, i.e. the simplify inverse kinematics. /// \brief Buffers for the CoM an Feet computation, i.e. the simplify inverse kinematics.
vector <MAL_VECTOR_TYPE(double)> ConfigurationTraj_ ; vector <MAL_VECTOR_TYPE(double)> ConfigurationTraj_ ;
...@@ -287,7 +295,9 @@ namespace PatternGeneratorJRL ...@@ -287,7 +295,9 @@ namespace PatternGeneratorJRL
/// \brief Used to compute once, the initial difference between the ZMP and the ZMPMB /// \brief Used to compute once, the initial difference between the ZMP and the ZMPMB
bool Once_ ; bool Once_ ;
double DInitX_, DInitY_ ; double DInitX_, DInitY_ ;
const double EPS_ ; COMState InitStateLIPMcontrol_ ;
COMState InitStateOrientPrw_ ;
COMState FinalStateOrientPrw_ ;
/// \brief Buffer comtaimimg the difference between the ZMP computed from the Herdt controler /// \brief Buffer comtaimimg the difference between the ZMP computed from the Herdt controler
///and the ZMP Multibody computed from the articular trajectory ///and the ZMP Multibody computed from the articular trajectory
...@@ -300,7 +310,7 @@ namespace PatternGeneratorJRL ...@@ -300,7 +310,7 @@ namespace PatternGeneratorJRL
Robot_Model m_robot; Robot_Model m_robot;
/// \brief Standard polynomial trajectories for the feet. /// \brief Standard polynomial trajectories for the feet.
OnLineFootTrajectoryGeneration * OFTG_; OnLineFootTrajectoryGeneration * OFTG_DF_ ;
OnLineFootTrajectoryGeneration * OFTG_control_ ; OnLineFootTrajectoryGeneration * OFTG_control_ ;
public: public:
...@@ -356,19 +366,25 @@ namespace PatternGeneratorJRL ...@@ -356,19 +366,25 @@ namespace PatternGeneratorJRL
unsigned IterationNumber unsigned IterationNumber
); );
// WARNING the interpolation modifie the solution_t, send a copy as argument void CoMZMPInterpolation(
void Interpolation(std::deque<ZMPPosition> & ZMPPositions, std::deque<ZMPPosition> & ZMPPositions, // OUTPUT
std::deque<COMState> & COMTraj_deq , std::deque<COMState> & COMTraj_deq , // OUTPUT
std::deque<FootAbsolutePosition> & LeftFootTraj_deq, std::deque<FootAbsolutePosition> & LeftFootTraj_deq, // INPUT
std::deque<FootAbsolutePosition> & RightFootTraj_deq, std::deque<FootAbsolutePosition> & RightFootTraj_deq, // INPUT
solution_t * Solution, solution_t * Solution, // INPUT
LinearizedInvertedPendulum2D * LIPM, LinearizedInvertedPendulum2D * LIPM, // INPUT
OrientationsPreview * OrientPrw, unsigned numberOfSample, // INPUT
OnLineFootTrajectoryGeneration * OFTG, int IterationNumber); // INPUT
unsigned currentIndex,
double time, void ControlInterpolation(
int IterationNumber std::deque<ZMPPosition> & FinalZMPTraj_deq,
); std::deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
std::deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
double time);
void DynamicFilterInterpolation(
std::deque<COMState> & FinalCOMTraj_deq,
double time);
}; };
} }
......
tests/TestHerdt2010DynamicFilter.cpp
+ 1
− 0
View file @ b7b93de0
...@@ -316,6 +316,7 @@ protected: ...@@ -316,6 +316,7 @@ protected:
{ {
aof << filterprecision(m_HDR->currentConfiguration()(i)) << " " ; // 39 - 74 aof << filterprecision(m_HDR->currentConfiguration()(i)) << " " ; // 39 - 74
} }
aof << endl; aof << endl;
aof.close(); aof.close();
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment