From d216cc70f0492167269e4e3823915e6dbd03e7fb Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Thu, 12 Jun 2014 19:49:30 +0200
Subject: [PATCH] Creating a class plugable on all PG. It has the purpose to
filter the com with the Kajita's dynamic filter
---
src/CMakeLists.txt | 1 +
.../OnLineFootTrajectoryGeneration.cpp | 1 -
src/PatternGeneratorInterfacePrivate.cpp | 2 +-
.../DynamicFilter.cpp | 353 ++++++++++++++++++
.../DynamicFilter.hh | 169 +++++++++
.../ZMPVelocityReferencedQP.cpp | 2 +-
.../generator-vel-ref.cpp | 23 +-
7 files changed, 534 insertions(+), 17 deletions(-)
create mode 100644 src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
create mode 100644 src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 4d900180..e109f0d5 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -73,6 +73,7 @@ SET(SOURCES
ZMPRefTrajectoryGeneration/qp-problem.cpp
ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp
+ ZMPRefTrajectoryGeneration/DynamicFilter.cpp
MotionGeneration/StepOverPlanner.cpp
MotionGeneration/CollisionDetector.cpp
MotionGeneration/WaistHeightVariation.cpp
diff --git a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
index 7248d6d1..8fc7948b 100644
--- a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
+++ b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
@@ -351,7 +351,6 @@ void
if (TimeInterval>=0.06499 && TimeInterval<=0.065999)
cout << endl ;
cout << endl ;
- cout << "########################################################\n" ;
SetParameters(
FootTrajectoryGenerationStandard::X_AXIS,
TimeInterval,FPx,
diff --git a/src/PatternGeneratorInterfacePrivate.cpp b/src/PatternGeneratorInterfacePrivate.cpp
index 00aa82d2..b63a0e5d 100644
--- a/src/PatternGeneratorInterfacePrivate.cpp
+++ b/src/PatternGeneratorInterfacePrivate.cpp
@@ -298,7 +298,7 @@ namespace PatternGeneratorJRL {
// Initialize the Preview Control general object.
m_DoubleStagePCStrategy->InitInterObjects(m_HumanoidDynamicRobot,
- (*m_ComAndFootRealization.begin()),
+ m_ComAndFootRealization[0],
m_StepStackHandler);
m_CoMAndFootOnlyStrategy->InitInterObjects(m_HumanoidDynamicRobot,
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
new file mode 100644
index 00000000..75a15a06
--- /dev/null
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -0,0 +1,353 @@
+#include "DynamicFilter.hh"
+#include <metapod/algos/rnea.hh>
+
+using namespace std;
+using namespace PatternGeneratorJRL;
+using namespace metapod;
+
+DynamicFilter::DynamicFilter(
+ SimplePluginManager *SPM,
+ CjrlHumanoidDynamicRobot *aHS
+ )
+{
+ currentTime_ = 0.0 ;
+ controlPeriod_ = 0.0 ;
+ interpolationPeriod_ = 0.0 ;
+ previewWindowSize_ = 0.0 ;
+ PG_T_ = 0.0 ;
+ NbI_ = 0.0 ;
+ NCtrl_ = 0.0;
+ NbI_ = 0.0 ;
+
+ comAndFootRealization_ = new ComAndFootRealizationByGeometry(
+ (PatternGeneratorInterfacePrivate*) SPM );
+ comAndFootRealization_->setHumanoidDynamicRobot(aHS);
+ comAndFootRealization_->SetHeightOfTheCoM(CoMHeight_);
+ comAndFootRealization_->setSamplingPeriod(interpolationPeriod_);
+ comAndFootRealization_->Initialization();
+
+ PC_ = new PreviewControl(
+ SPM,OptimalControllerSolver::MODE_WITH_INITIALPOS,false);
+ CoMHeight_ = 0.0 ;
+
+ configurationTraj_.clear();
+ velocityTraj_.clear();
+ accelerationTraj_.clear();
+ previousConfiguration_.clear();
+ deltaZMP_deq_.clear();
+ ZMPMB_vec_.clear();
+
+ MAL_VECTOR_RESIZE(aCoMState_,6);
+ MAL_VECTOR_RESIZE(aCoMSpeed_,6);
+ MAL_VECTOR_RESIZE(aCoMAcc_,6);
+ MAL_VECTOR_RESIZE(aLeftFootPosition_,5);
+ MAL_VECTOR_RESIZE(aRightFootPosition_,5);
+ MAL_MATRIX_RESIZE(deltax_,3,1);
+ MAL_MATRIX_RESIZE(deltay_,3,1);
+
+ previousConfiguration_ = aHS->currentConfiguration() ;
+ previousVelocity_ = aHS->currentVelocity() ;
+ previousAcceleration_ = aHS->currentAcceleration() ;
+
+ comAndFootRealization_->SetPreviousConfigurationStage0(
+ previousConfiguration_);
+ comAndFootRealization_->SetPreviousVelocityStage0(
+ previousVelocity_);
+
+ Once_ = true ;
+ DInitX_ = 0.0 ;
+ DInitY_ = 0.0 ;
+
+}
+
+/// \brief Initialse all objects, to be called just after the constructor
+void DynamicFilter::init(
+ double currentTime,
+ double controlPeriod,
+ double interpolationPeriod,
+ double PG_T,
+ double previewWindowSize,
+ double CoMHeight
+ )
+{
+ currentTime_ = currentTime ;
+ controlPeriod_ = controlPeriod ;
+ interpolationPeriod_ = interpolationPeriod ;
+ PG_T_ = PG_T ;
+ previewWindowSize_ = previewWindowSize ;
+
+ NbI_ = (int)(PG_T_/interpolationPeriod) ;
+ NCtrl_ = (int)(PG_T_/controlPeriod) ;
+ PG_N_ = (int)(previewWindowSize_/PG_T_) ;
+
+ CoMHeight_ = CoMHeight ;
+ PC_->SetPreviewControlTime (previewWindowSize_ - PG_T/controlPeriod * interpolationPeriod);
+ PC_->SetSamplingPeriod (interpolationPeriod);
+ PC_->SetHeightOfCoM(CoMHeight_);
+
+ 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_);
+
+ MAL_VECTOR_RESIZE(aCoMState_,6);
+ MAL_VECTOR_RESIZE(aCoMSpeed_,6);
+ MAL_VECTOR_RESIZE(aCoMAcc_,6);
+ MAL_VECTOR_RESIZE(aLeftFootPosition_,5);
+ MAL_VECTOR_RESIZE(aRightFootPosition_,5);
+ MAL_MATRIX_RESIZE(deltax_,3,1);
+ MAL_MATRIX_RESIZE(deltay_,3,1);
+ return ;
+}
+
+int DynamicFilter::filter(
+ deque<COMState> & inputCOMTraj_deq_,
+ deque<ZMPPosition> inputZMPTraj_deq_,
+ deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+ deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
+ deque<COMState> & outputDeltaCOMTraj_deq_
+ )
+{
+ InverseKinematics(
+ inputCOMTraj_deq_,
+ inputLeftFootTraj_deq_,
+ inputRightFootTraj_deq_);
+ InverseDynamics(inputZMPTraj_deq_);
+ int error = OptimalControl(outputDeltaCOMTraj_deq_);
+ //printDebug();
+ return error ;
+}
+
+void DynamicFilter::InverseKinematics(
+ deque<COMState> & inputCOMTraj_deq_,
+ deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+ deque<FootAbsolutePosition> & inputRightFootTraj_deq_)
+{
+ const int stage0 = 0 ;
+ const unsigned int N = inputCOMTraj_deq_.size();
+ int iteration = 2 ;
+ comAndFootRealization_->SetPreviousConfigurationStage0(
+ previousConfiguration_);
+ comAndFootRealization_->SetPreviousVelocityStage0(
+ previousVelocity_);
+
+ for(unsigned int i = 0 ; i < N ; i++ )
+ {
+ const COMState & acomp = inputCOMTraj_deq_[i] ;
+ const FootAbsolutePosition & aLeftFAP =
+ inputLeftFootTraj_deq_ [i] ;
+ const FootAbsolutePosition & aRightFAP =
+ inputRightFootTraj_deq_ [i] ;
+
+ aCoMState_(0) = acomp.x[0]; aCoMSpeed_(0) = acomp.x[1];
+ aCoMState_(1) = acomp.y[0]; aCoMSpeed_(1) = acomp.y[1];
+ aCoMState_(2) = acomp.z[0]; aCoMSpeed_(2) = acomp.z[1];
+ aCoMState_(3) = acomp.roll[0]; aCoMSpeed_(3) = acomp.roll[1];
+ aCoMState_(4) = acomp.pitch[0]; aCoMSpeed_(4) = acomp.pitch[1];
+ aCoMState_(5) = acomp.yaw[0]; aCoMSpeed_(5) = acomp.yaw[1];
+
+ aCoMAcc_(0) = acomp.x[2]; aLeftFootPosition_(0) = aLeftFAP.x;
+ aCoMAcc_(1) = acomp.y[2]; aLeftFootPosition_(1) = aLeftFAP.y;
+ aCoMAcc_(2) = acomp.z[2]; aLeftFootPosition_(2) = aLeftFAP.z;
+ aCoMAcc_(3) = acomp.roll[2]; aLeftFootPosition_(3) = aLeftFAP.theta;
+ aCoMAcc_(4) = acomp.pitch[2];aLeftFootPosition_(4) = aLeftFAP.omega;
+ aCoMAcc_(5) = acomp.yaw[2];
+
+ aRightFootPosition_(0) = aRightFAP.x;
+ aRightFootPosition_(1) = aRightFAP.y;
+ aRightFootPosition_(2) = aRightFAP.z;
+ aRightFootPosition_(3) = aRightFAP.theta;
+ aRightFootPosition_(4) = aRightFAP.omega;
+
+ comAndFootRealization_->ComputePostureForGivenCoMAndFeetPosture(
+ aCoMState_, aCoMSpeed_, aCoMAcc_,
+ aLeftFootPosition_, aRightFootPosition_,
+ configurationTraj_[i],
+ velocityTraj_[i],
+ accelerationTraj_[i],
+ iteration, stage0);
+ }
+
+// tmpConfigurationTraj_[0] = ( ConfigurationTraj_[1]+ConfigurationTraj_[0]+PreviousConfiguration_ )/3;
+// tmpVelocityTraj_[0] = ( VelocityTraj_[1]+VelocityTraj_[0]+PreviousVelocity_ )/3;
+// tmpAccelerationTraj_[0] = ( AccelerationTraj_[1]+AccelerationTraj_[0]+PreviousAcceleration_ )/3;
+
+ // saving the precedent state of the next QP_ computation
+ previousConfiguration_ = configurationTraj_[NbI_-1] ;
+ previousVelocity_ = velocityTraj_[NbI_-1] ;
+ previousAcceleration_ = accelerationTraj_[NbI_-1] ;
+
+// for (unsigned int i = 1 ; i < N-1 ; ++i )
+// {
+// tmpConfigurationTraj_[i] = ( ConfigurationTraj_[i+1] + ConfigurationTraj_[i] + ConfigurationTraj_[i-1] )/3;
+// tmpVelocityTraj_[i] = ( VelocityTraj_[i+1] + VelocityTraj_[i] + VelocityTraj_[i-1] )/3;
+// tmpAccelerationTraj_[i] = ( AccelerationTraj_[i+1] + AccelerationTraj_[i] + AccelerationTraj_[i-1] )/3;
+// }
+//
+// tmpConfigurationTraj_[N-1] = ( ConfigurationTraj_[N-1]+ConfigurationTraj_[N-2] )*0.5;
+// tmpVelocityTraj_[N-1] = ( VelocityTraj_[N-1]+VelocityTraj_[N-2] )*0.5;
+// tmpAccelerationTraj_[N-1] = ( AccelerationTraj_[N-1]+AccelerationTraj_[N-2] )*0.5;
+//
+//
+// ConfigurationTraj_ = tmpConfigurationTraj_ ;
+// VelocityTraj_ = tmpVelocityTraj_ ;
+// AccelerationTraj_ = tmpAccelerationTraj_ ;
+
+ return ;
+}
+
+void DynamicFilter::InverseDynamics(
+ deque<ZMPPosition> inputZMPTraj_deq_)
+{
+ const unsigned int N = inputZMPTraj_deq_.size();
+ for (unsigned int i = 0 ; i < N ; i++ )
+ {
+ // Copy the angular trajectory data from "Boost" to "Eigen"
+ for(unsigned int j = 0 ; j < configurationTraj_[i].size() ; j++ )
+ {
+ m_q(j,0) = configurationTraj_[i](j) ;
+ m_dq(j,0) = velocityTraj_[i](j) ;
+ m_ddq(j,0) = accelerationTraj_[i](j) ;
+ }
+
+ // Apply the RNEA on the robot model
+ metapod::rnea< Robot_Model, true >::run(m_robot, m_q, m_dq, m_ddq);
+
+ Node & node =
+ boost::fusion::at_c<Robot_Model::BODY>(m_robot.nodes);
+ m_force = node.body.iX0.applyInv (node.joint.f);
+
+ if (Once_){
+ DInitX_ = inputZMPTraj_deq_[0].px -
+ ( - m_force.n()[1] / m_force.f()[2] ) ;
+ DInitY_ = inputZMPTraj_deq_[0].py -
+ ( m_force.n()[0] / m_force.f()[2] ) ;
+ Once_ = false ;
+ }
+
+ ZMPMB_vec_[i][0] = - m_force.n()[1] / m_force.f()[2] + DInitX_ ;
+ ZMPMB_vec_[i][1] = m_force.n()[0] / m_force.f()[2] + DInitY_ ;
+
+ deltaZMP_deq_[i].px = inputZMPTraj_deq_[i].px - ZMPMB_vec_[i][0] ;
+ deltaZMP_deq_[i].py = inputZMPTraj_deq_[i].py - ZMPMB_vec_[i][1] ;
+ deltaZMP_deq_[i].pz = 0.0 ;
+ deltaZMP_deq_[i].theta = 0.0 ;
+ deltaZMP_deq_[i].time = currentTime_ + i * interpolationPeriod_ ;
+ deltaZMP_deq_[i].stepType = inputZMPTraj_deq_[i].stepType ;
+ }
+ return ;
+}
+
+int DynamicFilter::OptimalControl(
+ deque<COMState> & outputDeltaCOMTraj_deq_)
+{
+
+ double aSxzmp (0) , aSyzmp(0);
+ double deltaZMPx (0) , deltaZMPy (0) ;
+
+ // calcul of the preview control along the "deltaZMP_deq_"
+ for (unsigned i = 0 ; i < NCtrl_ ; i++ )
+ {
+ for(int j=0;j<3;j++)
+ {
+ deltax_(j,0) = 0 ;
+ deltay_(j,0) = 0 ;
+ }
+ PC_->OneIterationOfPreview(deltax_,deltay_,
+ aSxzmp,aSyzmp,
+ deltaZMP_deq_,i,
+ deltaZMPx, deltaZMPy, false);
+ for(int j=0;j<3;j++)
+ {
+ outputDeltaCOMTraj_deq_[i].x[j] = deltax_(j,0);
+ outputDeltaCOMTraj_deq_[i].y[j] = deltay_(j,0);
+ }
+ }
+
+ for (unsigned int i = 0 ; i < NCtrl_ ; i++)
+ {
+ for(int j=0;j<3;j++)
+ {
+ if ( outputDeltaCOMTraj_deq_[i].x[j] == outputDeltaCOMTraj_deq_[i].x[j] ||
+ outputDeltaCOMTraj_deq_[i].y[j] == outputDeltaCOMTraj_deq_[i].y[j] )
+ {}
+ else{
+ cout << "kajita2003 preview control diverged\n" ;
+ return -1 ;
+ }
+ }
+ }
+ return 0 ;
+}
+
+double DynamicFilter::filterprecision(double adb)
+{
+ if (fabs(adb)<1e-7)
+ return 0.0;
+
+ if (fabs(adb)>1e7)
+ return 1e7 ;
+
+ double ladb2 = adb * 1e7;
+ double lintadb2 = trunc(ladb2);
+ return lintadb2/1e7;
+}
+
+void DynamicFilter::printDebug()
+{
+ /// \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("TestHerdt2010DynamicArtDF.dat");
+ aFileName = oss.str();
+ if(iteration == 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 i = 0 ; i < NbI_ ; ++i )
+ {
+ aof << filterprecision( 0.0 ) << " " // 1
+ << filterprecision( 0.0 ) << " " ; // 2
+ for(unsigned int j = 0 ; j < configurationTraj_[i].size() ; j++ )
+ aof << filterprecision( configurationTraj_[i](j) ) << " " ;
+ for(unsigned int j = 0 ; j < velocityTraj_[i].size() ; j++ )
+ aof << filterprecision( velocityTraj_[i](j) ) << " " ;
+ for(unsigned int j = 0 ; j < accelerationTraj_[i].size() ; j++ )
+ aof << filterprecision( accelerationTraj_[i](j) ) << " " ;
+ aof << endl ;
+ }
+ aof.close();
+
+ ++iteration;
+
+ static double ecartmaxX = 0 ;
+ static double ecartmaxY = 0 ;
+
+ for (unsigned int i = 0 ; i < deltaZMP_deq_.size() ; i++ )
+ {
+ if ( abs(deltaZMP_deq_[i].px) > ecartmaxX )
+ ecartmaxX = abs(deltaZMP_deq_[i].px) ;
+ if ( abs(deltaZMP_deq_[i].py) > ecartmaxY )
+ ecartmaxY = abs(deltaZMP_deq_[i].py) ;
+ }
+ return ;
+}
+
+
+
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
new file mode 100644
index 00000000..6021e36a
--- /dev/null
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
@@ -0,0 +1,169 @@
+#ifndef DYNAMICFILTER_HH
+#define DYNAMICFILTER_HH
+
+// metapod includes
+#include <metapod/models/hrp2_14/hrp2_14.hh>
+#include <MotionGeneration/ComAndFootRealizationByGeometry.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
+{
+
+ class DynamicFilter
+ {
+ public: // Public methods
+ /// \brief
+ DynamicFilter(SimplePluginManager *SPM,
+ CjrlHumanoidDynamicRobot *aHS);
+
+ /// \brief
+ int filter(
+ deque<COMState> & inputCOMTraj_deq_,
+ deque<ZMPPosition> inputZMPTraj_deq_,
+ deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+ deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
+ deque<COMState> & outputDeltaCOMTraj_deq_
+ );
+
+ void init(
+ double currentTime,
+ double controlPeriod,
+ double interpolationPeriod,
+ double PG_T,
+ double previewWindowSize,
+ double CoMHeight
+ );
+
+ private: // Private methods
+
+ // \brief calculate, from the CoM computed by the preview control,
+ // the corresponding articular position, velocity and acceleration
+ void InverseKinematics(
+ deque<COMState> & inputCOMTraj_deq_,
+ deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+ deque<FootAbsolutePosition> & inputRightFootTraj_deq_
+ );
+
+ // Apply the RNEA on the robot model
+ void InverseDynamics(deque<ZMPPosition> inputZMPTraj_deq_);
+
+ /// Preview control on the ZMPMBs computed
+ /// --------------------------------------
+ int OptimalControl(std::deque<COMState> & outputDeltaCOMTraj_deq_);
+
+ void printDebug();
+ double filterprecision(double adb);
+
+
+ public: // The accessors
+
+ /// \brief setter :
+ inline void setCurrentTime(double time)
+ {currentTime_ = time ;}
+
+ inline void setControlPeriod(double controlPeriod)
+ {controlPeriod_ = controlPeriod ;}
+
+ inline void setInterpolationPeriod(double interpolationPeriod)
+ {interpolationPeriod_ = interpolationPeriod ; return ;}
+
+ inline void setPGPeriod(double PG_T)
+ {PG_T_ = PG_T ;}
+
+ /// \brief getter :
+ inline ComAndFootRealization * getComAndFootRealization()
+ { return comAndFootRealization_;};
+
+ private: // Private members
+
+ /// \brief Time variables
+ /// -----------------------------------
+ /// \brief Current time of the PG.
+ double currentTime_ ;
+
+ /// \brief control period of the PG host
+ double controlPeriod_;
+
+ /// \brief Interpolation Period for the dynamic filter
+ double interpolationPeriod_ ;
+
+ /// \brief Sampling period of the PG host
+ double PG_T_;
+
+ /// \brief size of the previw window in second
+ double previewWindowSize_ ;
+
+ //------------------------------------------------------
+ /// \brief Contain the number of control points
+ unsigned int NCtrl_;
+
+ /// \brief Contain the number of interpolation points
+ /// inside the Sampling period of the PG host
+ unsigned int NbI_ ;
+
+ /// \brief Nb. samplings inside preview window
+ unsigned int PG_N_ ;
+
+ /// \brief Inverse Kinematics variables
+ /// -----------------------------------
+ /// \brief Store a reference to the object to solve posture resolution.
+ ComAndFootRealizationByGeometry * comAndFootRealization_;
+
+ /// \brief Buffers for the Inverse Kinematics
+ std::vector <MAL_VECTOR_TYPE(double)> configurationTraj_ ;
+ std::vector <MAL_VECTOR_TYPE(double)> velocityTraj_ ;
+ std::vector <MAL_VECTOR_TYPE(double)> accelerationTraj_ ;
+ MAL_VECTOR_TYPE(double) previousConfiguration_ ;
+ MAL_VECTOR_TYPE(double) previousVelocity_ ;
+ MAL_VECTOR_TYPE(double) previousAcceleration_ ;
+ MAL_VECTOR_TYPE(double) aCoMState_;
+ MAL_VECTOR_TYPE(double) aCoMSpeed_;
+ MAL_VECTOR_TYPE(double) aCoMAcc_;
+ MAL_VECTOR_TYPE(double) aLeftFootPosition_;
+ MAL_VECTOR_TYPE(double) aRightFootPosition_;
+
+ /// \brief Inverse Dynamics variables
+ /// ---------------------------------
+ /// \brief Initialize the robot with the autogenerated files
+ /// by MetapodFromUrdf
+ Robot_Model m_robot;
+
+ /// \brief force acting on the CoM of the robot expressed
+ /// in the Euclidean Frame
+ 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;
+
+ /// \brief Used to eliminate the initiale difference between
+ /// the zmp and the zmpmb
+ bool Once_ ;
+ double DInitX_, DInitY_ ;
+
+ /// \brief Buffers the ZMP Multibody computed
+ /// from the inverse Dynamics, and the difference between
+ /// this zmp and the reference one.
+ deque< vector<double> > ZMPMB_vec_ ;
+ std::deque<ZMPPosition> deltaZMP_deq_ ;
+
+ /// \brief Optimal Control variables
+ /// --------------------------------
+ /// \brief Pointer to the Preview Control object.
+ PreviewControl *PC_;
+ double CoMHeight_ ;
+
+ /// \brief State of the Preview control.
+ MAL_MATRIX(deltax_,double);
+ MAL_MATRIX(deltay_,double);
+ };
+
+}
+
+#endif // DYNAMICFILTER_HH
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 30d49f5d..d4a01ffb 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -515,9 +515,9 @@ void
// UPDATE INTERNAL DATA:
// ---------------------
Problem_.reset_variant();
- VRQPGenerator_->LastFootSol( Solution_ );
VRQPGenerator_->CurrentTime( time );
Solution_.reset();
+ VRQPGenerator_->CurrentTime( time );
VelRef_=NewVelRef_;
SupportFSM_->update_vel_reference(VelRef_, IntermedData_->SupportState());
IntermedData_->Reference( VelRef_ );
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index 23bf08c4..066c178c 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -540,7 +540,7 @@ GeneratorVelRef::build_eq_constraints_feet( const std::deque<support_state_t> &
Pb.NbEqConstraints(2*NbStepsPreviewed);
for(unsigned int i = 0; i< NbStepsPreviewed; i++)
{
- EqualityMatrix(0,i) = 1.0; EqualityVector(0) = -SPTraj_it->X;
+ EqualityMatrix(0,i) = 1.0; EqualityVector(0) = -SPTraj_it->X;
EqualityMatrix(1,NbStepsPreviewed+i) = 1.0; EqualityVector(1) = -SPTraj_it->Y;
Pb.add_term_to( MATRIX_DU, EqualityMatrix, 2*i, 2*N_ );
Pb.add_term_to( VECTOR_DS, EqualityVector, 2*i );
@@ -567,29 +567,24 @@ void GeneratorVelRef::build_eq_constraints_limitPosFeet(const solution_t & Solut
}
if( ItBeforeLanding <= 3 && Solution.SupportStates_deq.front().Phase == SS )
- { unsigned nbStepsPreviewed = Solution.SupportStates_deq.back().StepNumber;
+ { unsigned NbStepsPreviewed = Solution.SupportStates_deq.back().StepNumber;
+
+ Pb.NbEqConstraints(2);
boost_ublas::matrix<double> EqualityMatrix;
boost_ublas::vector<double> EqualityVector;
- EqualityMatrix.resize(2,2*(N_+nbStepsPreviewed), false);
+ EqualityMatrix.resize(2,2*N_+2*NbStepsPreviewed, false);
EqualityMatrix.clear();
EqualityVector.resize(2, false);
EqualityVector.clear();
- EqualityMatrix(0,2*N_) = 1.0;
- EqualityMatrix(1,2*N_+nbStepsPreviewed) = 1.0;
- EqualityVector(0) = LastFootSolX_ ;
- EqualityVector(1) = LastFootSolY_ ;
-
- cout << "EqualityVector \n" << EqualityVector << endl;
- cout << "EqualityMatrix \n" << EqualityMatrix << endl;
-
- Pb.NbEqConstraints(2);
-
+ EqualityMatrix(0,2*N_) = 1.0; EqualityVector(0) = LastFootSolX_ ;
+ EqualityMatrix(1,2*N_+NbStepsPreviewed) = 1.0; EqualityVector(1) = LastFootSolY_ ;
Pb.add_term_to( MATRIX_DU, EqualityMatrix, 0, 0 );
Pb.add_term_to( VECTOR_DS, EqualityVector, 0 );
-
+ cout << "EqualityVector \n" << EqualityVector << endl;
+ cout << "EqualityMatrix \n" << EqualityMatrix << endl;
EqualityMatrix.clear();
EqualityVector.clear();
}
--
GitLab