From ecb1ccdc5665219f9ee5eff0d8b26cdaa8e91f53 Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Thu, 19 Jun 2014 20:53:40 +0200
Subject: [PATCH] debugging the dynamic filter
---
.../ComAndFootRealizationByGeometry.hh | 13 +
.../AnalyticalMorisawaCompact.cpp | 63 ++-
.../DynamicFilter.cpp | 425 ++++++++++--------
.../DynamicFilter.hh | 48 +-
.../ZMPVelocityReferencedQP.cpp | 11 +-
tests/TestMorisawa2007.cpp | 396 ++++++++--------
6 files changed, 525 insertions(+), 431 deletions(-)
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.hh b/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
index c2655db3..bf4518ac 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
@@ -258,6 +258,19 @@ namespace PatternGeneratorJRL
inline void SetPreviousVelocityStage1(MAL_VECTOR_TYPE(double) & prev_Velocity1)
{ m_prev_Velocity1 = prev_Velocity1 ;};
+ /*! \brief Getter and setter for the previous configurations and velocities */
+ inline void SetPreviousConfigurationStage0(const MAL_VECTOR_TYPE(double) & prev_Configuration)
+ { m_prev_Configuration = prev_Configuration ;};
+
+ inline void SetPreviousConfigurationStage1(const MAL_VECTOR_TYPE(double) & prev_Configuration1)
+ { m_prev_Configuration1 = prev_Configuration1 ;};
+
+ inline void SetPreviousVelocityStage0(const MAL_VECTOR_TYPE(double) & prev_Velocity)
+ { m_prev_Velocity = prev_Velocity ;};
+
+ inline void SetPreviousVelocityStage1(const MAL_VECTOR_TYPE(double) & prev_Velocity1)
+ { m_prev_Velocity1 = prev_Velocity1 ;};
+
/*! \brief Getter and setter for the previous configurations and velocities */
inline MAL_VECTOR_TYPE(double) & GetPreviousConfigurationStage0()
{ return m_prev_Configuration ;};
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index 003fffdb..2a21ccaf 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -712,9 +712,12 @@ namespace PatternGeneratorJRL
for (unsigned i = 0 ; i < N ; ++i)
{
current_time += m_kajitaDynamicFilter->getInterpolationPeriod() ;
-
- if (m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(current_time,lIndexInterval,lPrevIndexInterval))
+ bool setTime = !m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(current_time,lIndexInterval,lPrevIndexInterval);
+ if( setTime == 1 )
{
+ current_time -= m_kajitaDynamicFilter->getInterpolationPeriod();
+ m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(time,lIndexInterval);
+ }
/*! Feed the ZMPPositions. */
m_AnalyticalZMPCoGTrajectoryX->ComputeZMP(current_time,ZMPPos_deq[i].px,lIndexInterval);
m_AnalyticalZMPCoGTrajectoryY->ComputeZMP(current_time,ZMPPos_deq[i].py,lIndexInterval);
@@ -734,32 +737,52 @@ namespace PatternGeneratorJRL
/*! Left */
m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(1,current_time,LeftFootAbsPos_deq[i],lIndexInterval);
m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(-1,current_time,RightFootAbsPos_deq[i],lIndexInterval);
- }else
- {
- ZMPPos_deq[i]=ZMPPos_deq[i-1];
- COMPos_deq[i]=COMPos_deq[i-1];
- LeftFootAbsPos_deq[i]=LeftFootAbsPos_deq[i-1];
- RightFootAbsPos_deq[i]=RightFootAbsPos_deq[i-1];
- LeftFootAbsPos_deq[i].time = RightFootAbsPos_deq[i].time = ZMPPos_deq[i].time = current_time ;
- }
}
+ m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(time,lIndexInterval);
+ COMState lastCtrlCoMState; memset(&lastCtrlCoMState,0,sizeof(lastCtrlCoMState));
+ ZMPPosition lastCtrlZMP ; memset(&lastCtrlZMP,0,sizeof(lastCtrlZMP));
+ FootAbsolutePosition lastCtrlLeftFoot; memset(&lastCtrlLeftFoot,0,sizeof(lastCtrlLeftFoot));
+ FootAbsolutePosition lastCtrlRightFoot; memset(&lastCtrlRightFoot,0,sizeof(lastCtrlRightFoot));
+
+ m_AnalyticalZMPCoGTrajectoryX->ComputeZMP(time,lastCtrlZMP.px,lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryY->ComputeZMP(time,lastCtrlZMP.py,lIndexInterval);
+
+ m_AnalyticalZMPCoGTrajectoryX->ComputeCOM(time,lastCtrlCoMState.x[0],lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryX->ComputeCOMSpeed(time,lastCtrlCoMState.x[1],lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryX->ComputeCOMAcceleration(time,lastCtrlCoMState.x[2],lIndexInterval);
+
+ m_AnalyticalZMPCoGTrajectoryY->ComputeCOM(time,lastCtrlCoMState.y[0],lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryY->ComputeCOMSpeed(time,lastCtrlCoMState.y[1],lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryY->ComputeCOMAcceleration(time,lastCtrlCoMState.y[2],lIndexInterval);
+ lastCtrlCoMState.z[0] = m_InitialPoseCoMHeight;
+
+ m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(1,time,lastCtrlLeftFoot,lIndexInterval);
+ m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(-1,time,lastCtrlRightFoot,lIndexInterval);
std::deque<COMState> deltaCoMTraj_deq (1);
memset(&deltaCoMTraj_deq[0],0,sizeof(deltaCoMTraj_deq[0]));
- m_kajitaDynamicFilter->filter(COMPos_deq,ZMPPos_deq,LeftFootAbsPos_deq,RightFootAbsPos_deq,deltaCoMTraj_deq);
+
+ m_kajitaDynamicFilter->filter(
+ lastCtrlCoMState,
+ lastCtrlLeftFoot,
+ lastCtrlRightFoot,
+ COMPos_deq,
+ ZMPPos_deq,
+ LeftFootAbsPos_deq,
+ RightFootAbsPos_deq,
+ deltaCoMTraj_deq);
- COMState aCOMState (COMPos_deq[0]) ;
for(unsigned i = 0 ; i < 3 ; ++i)
{
- aCOMState.x[i] += deltaCoMTraj_deq[0].x[i] ;
- aCOMState.y[i] += deltaCoMTraj_deq[0].y[i] ;
+ lastCtrlCoMState.x[i] += deltaCoMTraj_deq[0].x[i] + aCOMStateToFilter.x[i] ;
+ lastCtrlCoMState.y[i] += deltaCoMTraj_deq[0].y[i] + aCOMStateToFilter.y[i] ;
}
- ZMPPos_deq[0].px += aZMPPositionToFilter.px;
- ZMPPos_deq[0].py += aZMPPositionToFilter.py;
- FinalZMPPositions.push_back(ZMPPos_deq[0]);
- FinalCOMStates.push_back(aCOMState);
- FinalLeftFootAbsolutePositions.push_back(LeftFootAbsPos_deq[0]);
- FinalRightFootAbsolutePositions.push_back(RightFootAbsPos_deq[0]);
+ lastCtrlZMP.px += aZMPPositionToFilter.px;
+ lastCtrlZMP.py += aZMPPositionToFilter.py;
+ FinalZMPPositions.push_back(lastCtrlZMP);
+ FinalCOMStates.push_back(lastCtrlCoMState);
+ FinalLeftFootAbsolutePositions.push_back(lastCtrlLeftFoot);
+ FinalRightFootAbsolutePositions.push_back(lastCtrlRightFoot);
}
}
else
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index 7a28653e..d31fd240 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -7,8 +7,7 @@ using namespace metapod;
DynamicFilter::DynamicFilter(
SimplePluginManager *SPM,
- CjrlHumanoidDynamicRobot *aHS
- )
+ CjrlHumanoidDynamicRobot *aHS)
{
currentTime_ = 0.0 ;
controlPeriod_ = 0.0 ;
@@ -79,8 +78,7 @@ void DynamicFilter::init(
double interpolationPeriod,
double PG_T,
double previewWindowSize,
- double CoMHeight
- )
+ double CoMHeight)
{
currentTime_ = currentTime ;
controlPeriod_ = controlPeriod ;
@@ -88,10 +86,10 @@ void DynamicFilter::init(
PG_T_ = PG_T ;
previewWindowSize_ = previewWindowSize ;
- if (PG_T>interpolationPeriod_)
+ if (interpolationPeriod_>PG_T)
{NbI_=1;}
else
- {NbI_ = PG_T/interpolationPeriod_;}
+ {NbI_ = (int)(PG_T/interpolationPeriod_);}
NCtrl_ = (int)(PG_T_/controlPeriod_) ;
PG_N_ = (int)(previewWindowSize_/interpolationPeriod_) ;
@@ -126,150 +124,156 @@ void DynamicFilter::init(
}
int DynamicFilter::filter(
+ COMState & lastCtrlCoMState,
+ FootAbsolutePosition & lastCtrlLeftFoot,
+ FootAbsolutePosition & lastCtrlRightFoot,
deque<COMState> & inputCOMTraj_deq_,
deque<ZMPPosition> inputZMPTraj_deq_,
deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
- deque<COMState> & outputDeltaCOMTraj_deq_
- )
+ deque<COMState> & outputDeltaCOMTraj_deq_)
{
InverseKinematics(
+ lastCtrlCoMState,
+ lastCtrlLeftFoot,
+ lastCtrlRightFoot,
inputCOMTraj_deq_,
inputLeftFootTraj_deq_,
inputRightFootTraj_deq_);
+ InverseDynamics(inputZMPTraj_deq_);
- //InverseDynamics(inputZMPTraj_deq_);
-
- //int error = OptimalControl(outputDeltaCOMTraj_deq_);
+ int error = OptimalControl(outputDeltaCOMTraj_deq_);
printBuffers(inputCOMTraj_deq_,
inputZMPTraj_deq_,
inputLeftFootTraj_deq_,
inputRightFootTraj_deq_,
outputDeltaCOMTraj_deq_);
- int error = 0 ;
+ printAlongTime(inputCOMTraj_deq_,
+ inputZMPTraj_deq_,
+ inputLeftFootTraj_deq_,
+ inputRightFootTraj_deq_,
+ outputDeltaCOMTraj_deq_);
return error ;
}
void DynamicFilter::InverseKinematics(
+ COMState & lastCtrlCoMState,
+ FootAbsolutePosition & lastCtrlLeftFoot,
+ FootAbsolutePosition & lastCtrlRightFoot,
deque<COMState> & inputCOMTraj_deq_,
deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
deque<FootAbsolutePosition> & inputRightFootTraj_deq_)
{
- const int stage0 = 0 ;
- int iteration = 2 ;
- comAndFootRealization_->SetPreviousConfigurationStage0(
- previousConfiguration_);
- comAndFootRealization_->SetPreviousVelocityStage0(
- previousVelocity_);
+ int stage0 = 0 ;
+ int stage1 = 1 ;
+ comAndFootRealization_->SetPreviousConfigurationStage0(previousConfiguration_);
+ comAndFootRealization_->SetPreviousVelocityStage0(previousVelocity_);
+ comAndFootRealization_->setSamplingPeriod(interpolationPeriod_);
for(unsigned int i = 0 ; i < PG_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);
+ InverseKinematics(inputCOMTraj_deq_[i],inputLeftFootTraj_deq_ [i], inputRightFootTraj_deq_ [i],
+ configurationTraj_[i],velocityTraj_[i],accelerationTraj_[i],
+ interpolationPeriod_, 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_ ;
-
+ InverseKinematics(lastCtrlCoMState, lastCtrlLeftFoot, lastCtrlRightFoot,
+ previousConfiguration_,previousVelocity_,previousAcceleration_,
+ controlPeriod_, stage1);
return ;
}
-void DynamicFilter::InverseDynamics(
- deque<ZMPPosition> inputZMPTraj_deq_)
+void DynamicFilter::InverseKinematics(
+ COMState & inputCoMState,
+ FootAbsolutePosition & inputLeftFoot,
+ FootAbsolutePosition & inputRightFoot,
+ MAL_VECTOR_TYPE(double)& configuration,
+ MAL_VECTOR_TYPE(double)& velocity,
+ MAL_VECTOR_TYPE(double)& acceleration,
+ double samplingPeriod,
+ int stage)
+{
+ int iteration = 2 ;
+ aCoMState_(0) = inputCoMState.x[0]; aCoMSpeed_(0) = inputCoMState.x[1];
+ aCoMState_(1) = inputCoMState.y[0]; aCoMSpeed_(1) = inputCoMState.y[1];
+ aCoMState_(2) = inputCoMState.z[0]; aCoMSpeed_(2) = inputCoMState.z[1];
+ aCoMState_(3) = inputCoMState.roll[0]; aCoMSpeed_(3) = inputCoMState.roll[1];
+ aCoMState_(4) = inputCoMState.pitch[0]; aCoMSpeed_(4) = inputCoMState.pitch[1];
+ aCoMState_(5) = inputCoMState.yaw[0]; aCoMSpeed_(5) = inputCoMState.yaw[1];
+
+ aCoMAcc_(0) = inputCoMState.x[2]; aLeftFootPosition_(0) = inputLeftFoot.x;
+ aCoMAcc_(1) = inputCoMState.y[2]; aLeftFootPosition_(1) = inputLeftFoot.y;
+ aCoMAcc_(2) = inputCoMState.z[2]; aLeftFootPosition_(2) = inputLeftFoot.z;
+ aCoMAcc_(3) = inputCoMState.roll[2]; aLeftFootPosition_(3) = inputLeftFoot.theta;
+ aCoMAcc_(4) = inputCoMState.pitch[2];aLeftFootPosition_(4) = inputLeftFoot.omega;
+ aCoMAcc_(5) = inputCoMState.yaw[2];
+
+ aRightFootPosition_(0) = inputRightFoot.x;
+ aRightFootPosition_(1) = inputRightFoot.y;
+ aRightFootPosition_(2) = inputRightFoot.z;
+ aRightFootPosition_(3) = inputRightFoot.theta;
+ aRightFootPosition_(4) = inputRightFoot.omega;
+
+ comAndFootRealization_->setSamplingPeriod(samplingPeriod);
+ comAndFootRealization_->ComputePostureForGivenCoMAndFeetPosture(
+ aCoMState_, aCoMSpeed_, aCoMAcc_,
+ aLeftFootPosition_, aRightFootPosition_,
+ configuration, velocity, acceleration,
+ iteration, stage);
+ return;
+}
+
+void DynamicFilter::InverseDynamics(deque<ZMPPosition> inputZMPTraj_deq)
{
for (unsigned int i = 0 ; i < PG_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);
+ ComputeZMPMB(configurationTraj_[i],velocityTraj_[i],accelerationTraj_[i],ZMPMB_vec_[i]);
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] ) ;
+ DInitX_ = inputZMPTraj_deq[0].px - ZMPMB_vec_[i][0];
+ DInitY_ = inputZMPTraj_deq[0].py - ZMPMB_vec_[i][1];
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].px = inputZMPTraj_deq[i].px - ZMPMB_vec_[i][0] - DInitX_ ;
+ deltaZMP_deq_[i].py = inputZMPTraj_deq[i].py - ZMPMB_vec_[i][1] - DInitY_ ;
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 ;
+ deltaZMP_deq_[i].stepType = inputZMPTraj_deq[i].stepType ;
}
return ;
}
+void DynamicFilter::ComputeZMPMB(
+ MAL_VECTOR_TYPE(double) & configuration,
+ MAL_VECTOR_TYPE(double) & velocity,
+ MAL_VECTOR_TYPE(double) & acceleration,
+ vector<double> & ZMPMB)
+{
+ // Copy the angular trajectory data from "Boost" to "Eigen"
+ for(unsigned int j = 0 ; j < configuration.size() ; j++ )
+ {
+ m_q(j,0) = configuration(j) ;
+ m_dq(j,0) = velocity(j) ;
+ m_ddq(j,0) = acceleration(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);
+
+ ZMPMB.resize(2);
+ ZMPMB[0] = - m_force.n()[1] / m_force.f()[2] ;
+ ZMPMB[1] = m_force.n()[0] / m_force.f()[2] ;
+
+ return ;
+}
+
int DynamicFilter::OptimalControl(
deque<COMState> & outputDeltaCOMTraj_deq_)
{
@@ -332,8 +336,7 @@ void DynamicFilter::printAlongTime(deque<COMState> & inputCOMTraj_deq_,
deque<ZMPPosition> inputZMPTraj_deq_,
deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
- deque<COMState> & outputDeltaCOMTraj_deq_
- )
+ deque<COMState> & outputDeltaCOMTraj_deq_)
{
// Debug Purpose
// -------------
@@ -341,12 +344,9 @@ void DynamicFilter::printAlongTime(deque<COMState> & inputCOMTraj_deq_,
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 );
// --------------------
- oss.str("DynamicFilterAllVariablesAlongInTime.dat");
+ oss.str("DynamicFilterAllVariablesNoisyAlongInTime.dat");
aFileName = oss.str();
if(iteration == 0)
{
@@ -357,88 +357,83 @@ void DynamicFilter::printAlongTime(deque<COMState> & inputCOMTraj_deq_,
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( iteration*controlPeriod_ + i*interpolationPeriod_ ) << " " // 0
- << filterprecision( inputCOMTraj_deq_[i].x[0] ) << " " // 1
- << filterprecision( inputCOMTraj_deq_[i].x[1] ) << " " // 2
- << filterprecision( inputCOMTraj_deq_[i].x[2] ) << " " // 3
- << filterprecision( inputCOMTraj_deq_[i].y[0] ) << " " // 4
- << filterprecision( inputCOMTraj_deq_[i].y[1] ) << " " // 5
- << filterprecision( inputCOMTraj_deq_[i].y[2] ) << " " // 6
- << filterprecision( inputCOMTraj_deq_[i].z[0] ) << " " // 7
- << filterprecision( inputCOMTraj_deq_[i].z[1] ) << " " // 8
- << filterprecision( inputCOMTraj_deq_[i].z[2] ) << " " // 9
- << filterprecision( inputCOMTraj_deq_[i].roll[0] ) << " " // 10
- << filterprecision( inputCOMTraj_deq_[i].roll[1] ) << " " // 11
- << filterprecision( inputCOMTraj_deq_[i].roll[2] ) << " " // 12
- << filterprecision( inputCOMTraj_deq_[i].pitch[0] ) << " "// 13
- << filterprecision( inputCOMTraj_deq_[i].pitch[1] ) << " "// 14
- << filterprecision( inputCOMTraj_deq_[i].pitch[2] ) << " "// 15
- << filterprecision( inputCOMTraj_deq_[i].yaw[0] ) << " " // 16
- << filterprecision( inputCOMTraj_deq_[i].yaw[1] ) << " " // 17
- << filterprecision( inputCOMTraj_deq_[i].yaw[2] ) << " " // 18
-
- << filterprecision( inputZMPTraj_deq_[i].px ) << " " // 19
- << filterprecision( inputZMPTraj_deq_[i].py ) << " " // 20
-
- << filterprecision( ZMPMB_vec_[i][0] ) << " " // 21
- << filterprecision( ZMPMB_vec_[i][1] ) << " " // 22
-
- << filterprecision( inputLeftFootTraj_deq_[i].x ) << " " // 23
- << filterprecision( inputLeftFootTraj_deq_[i].y ) << " " // 24
- << filterprecision( inputLeftFootTraj_deq_[i].z ) << " " // 25
- << filterprecision( inputLeftFootTraj_deq_[i].theta ) << " " // 26
- << filterprecision( inputLeftFootTraj_deq_[i].omega ) << " " // 27
- << filterprecision( inputLeftFootTraj_deq_[i].dx ) << " " // 28
- << filterprecision( inputLeftFootTraj_deq_[i].dy ) << " " // 29
- << filterprecision( inputLeftFootTraj_deq_[i].dz ) << " " // 30
- << filterprecision( inputLeftFootTraj_deq_[i].dtheta ) << " " // 31
- << filterprecision( inputLeftFootTraj_deq_[i].domega ) << " " // 32
- << filterprecision( inputLeftFootTraj_deq_[i].ddx ) << " " // 33
- << filterprecision( inputLeftFootTraj_deq_[i].ddy ) << " " // 34
- << filterprecision( inputLeftFootTraj_deq_[i].ddz ) << " " // 35
- << filterprecision( inputLeftFootTraj_deq_[i].ddtheta ) << " " // 36
- << filterprecision( inputLeftFootTraj_deq_[i].ddomega ) << " " // 37
-
- << filterprecision( inputRightFootTraj_deq_[i].x ) << " " // 38
- << filterprecision( inputRightFootTraj_deq_[i].y ) << " " // 39
- << filterprecision( inputRightFootTraj_deq_[i].z ) << " " // 40
- << filterprecision( inputRightFootTraj_deq_[i].theta ) << " " // 41
- << filterprecision( inputRightFootTraj_deq_[i].omega ) << " " // 42
- << filterprecision( inputRightFootTraj_deq_[i].dx ) << " " // 43
- << filterprecision( inputRightFootTraj_deq_[i].dy ) << " " // 44
- << filterprecision( inputRightFootTraj_deq_[i].dz ) << " " // 45
- << filterprecision( inputRightFootTraj_deq_[i].dtheta ) << " " // 46
- << filterprecision( inputRightFootTraj_deq_[i].domega ) << " " // 47
- << filterprecision( inputRightFootTraj_deq_[i].ddx ) << " " // 48
- << filterprecision( inputRightFootTraj_deq_[i].ddy ) << " " // 49
- << filterprecision( inputRightFootTraj_deq_[i].ddz ) << " " // 50
- << filterprecision( inputRightFootTraj_deq_[i].ddtheta ) << " "// 51
- << filterprecision( inputRightFootTraj_deq_[i].ddomega ) << " ";// 52
-
- 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 << filterprecision( iteration*controlPeriod_) << " " // 0
+ << filterprecision( inputCOMTraj_deq_[0].x[0] ) << " " // 1
+ << filterprecision( inputCOMTraj_deq_[0].x[1] ) << " " // 2
+ << filterprecision( inputCOMTraj_deq_[0].x[2] ) << " " // 3
+ << filterprecision( inputCOMTraj_deq_[0].y[0] ) << " " // 4
+ << filterprecision( inputCOMTraj_deq_[0].y[1] ) << " " // 5
+ << filterprecision( inputCOMTraj_deq_[0].y[2] ) << " " // 6
+ << filterprecision( inputCOMTraj_deq_[0].z[0] ) << " " // 7
+ << filterprecision( inputCOMTraj_deq_[0].z[1] ) << " " // 8
+ << filterprecision( inputCOMTraj_deq_[0].z[2] ) << " " // 9
+ << filterprecision( inputCOMTraj_deq_[0].roll[0] ) << " " // 10
+ << filterprecision( inputCOMTraj_deq_[0].roll[1] ) << " " // 11
+ << filterprecision( inputCOMTraj_deq_[0].roll[2] ) << " " // 12
+ << filterprecision( inputCOMTraj_deq_[0].pitch[0] ) << " "// 13
+ << filterprecision( inputCOMTraj_deq_[0].pitch[1] ) << " "// 14
+ << filterprecision( inputCOMTraj_deq_[0].pitch[2] ) << " "// 15
+ << filterprecision( inputCOMTraj_deq_[0].yaw[0] ) << " " // 16
+ << filterprecision( inputCOMTraj_deq_[0].yaw[1] ) << " " // 17
+ << filterprecision( inputCOMTraj_deq_[0].yaw[2] ) << " " // 18
+
+ << filterprecision( inputZMPTraj_deq_[0].px ) << " " // 19
+ << filterprecision( inputZMPTraj_deq_[0].py ) << " " // 20
+
+ << filterprecision( ZMPMB_vec_[0][0] ) << " " // 21
+ << filterprecision( ZMPMB_vec_[0][1] ) << " " // 22
+
+ << filterprecision( inputLeftFootTraj_deq_[0].x ) << " " // 23
+ << filterprecision( inputLeftFootTraj_deq_[0].y ) << " " // 24
+ << filterprecision( inputLeftFootTraj_deq_[0].z ) << " " // 25
+ << filterprecision( inputLeftFootTraj_deq_[0].theta ) << " " // 26
+ << filterprecision( inputLeftFootTraj_deq_[0].omega ) << " " // 27
+ << filterprecision( inputLeftFootTraj_deq_[0].dx ) << " " // 28
+ << filterprecision( inputLeftFootTraj_deq_[0].dy ) << " " // 29
+ << filterprecision( inputLeftFootTraj_deq_[0].dz ) << " " // 30
+ << filterprecision( inputLeftFootTraj_deq_[0].dtheta ) << " " // 31
+ << filterprecision( inputLeftFootTraj_deq_[0].domega ) << " " // 32
+ << filterprecision( inputLeftFootTraj_deq_[0].ddx ) << " " // 33
+ << filterprecision( inputLeftFootTraj_deq_[0].ddy ) << " " // 34
+ << filterprecision( inputLeftFootTraj_deq_[0].ddz ) << " " // 35
+ << filterprecision( inputLeftFootTraj_deq_[0].ddtheta ) << " " // 36
+ << filterprecision( inputLeftFootTraj_deq_[0].ddomega ) << " " // 37
+
+ << filterprecision( inputRightFootTraj_deq_[0].x ) << " " // 38
+ << filterprecision( inputRightFootTraj_deq_[0].y ) << " " // 39
+ << filterprecision( inputRightFootTraj_deq_[0].z ) << " " // 40
+ << filterprecision( inputRightFootTraj_deq_[0].theta ) << " " // 41
+ << filterprecision( inputRightFootTraj_deq_[0].omega ) << " " // 42
+ << filterprecision( inputRightFootTraj_deq_[0].dx ) << " " // 43
+ << filterprecision( inputRightFootTraj_deq_[0].dy ) << " " // 44
+ << filterprecision( inputRightFootTraj_deq_[0].dz ) << " " // 45
+ << filterprecision( inputRightFootTraj_deq_[0].dtheta ) << " " // 46
+ << filterprecision( inputRightFootTraj_deq_[0].domega ) << " " // 47
+ << filterprecision( inputRightFootTraj_deq_[0].ddx ) << " " // 48
+ << filterprecision( inputRightFootTraj_deq_[0].ddy ) << " " // 49
+ << filterprecision( inputRightFootTraj_deq_[0].ddz ) << " " // 50
+ << filterprecision( inputRightFootTraj_deq_[0].ddtheta ) << " "// 51
+ << filterprecision( inputRightFootTraj_deq_[0].ddomega ) << " ";// 52
+
+ for(unsigned int j = 0 ; j < previousConfiguration_.size() ; j++ )
+ aof << filterprecision( previousConfiguration_(j) ) << " " ;
+ for(unsigned int j = 0 ; j < previousVelocity_.size() ; j++ )
+ aof << filterprecision( previousVelocity_(j) ) << " " ;
+ for(unsigned int j = 0 ; j < previousAcceleration_.size() ; j++ )
+ aof << filterprecision( accelerationTraj_[0](j) ) << " " ;
+
+ aof << filterprecision( outputDeltaCOMTraj_deq_[0].x[0] ) << " "
+ << filterprecision( outputDeltaCOMTraj_deq_[0].x[1] ) << " "
+ << filterprecision( outputDeltaCOMTraj_deq_[0].x[2] ) << " "
+ << filterprecision( outputDeltaCOMTraj_deq_[0].y[0] ) << " "
+ << filterprecision( outputDeltaCOMTraj_deq_[0].y[1] ) << " "
+ << filterprecision( outputDeltaCOMTraj_deq_[0].y[2] ) << " ";
+
+ 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 ;
}
@@ -446,8 +441,7 @@ void DynamicFilter::printBuffers(deque<COMState> & inputCOMTraj_deq_,
deque<ZMPPosition> inputZMPTraj_deq_,
deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
- deque<COMState> & outputDeltaCOMTraj_deq_
- )
+ deque<COMState> & outputDeltaCOMTraj_deq_)
{
// Debug Purpose
// -------------
@@ -527,10 +521,6 @@ void DynamicFilter::printBuffers(deque<COMState> & inputCOMTraj_deq_,
<< filterprecision( inputRightFootTraj_deq_[i].ddtheta ) << " " // 49
<< filterprecision( inputRightFootTraj_deq_[i].ddomega ) << " ";// 50
- // << filterprecision( ZMPMB_vec_[i][0] ) << " " // 21
- // << filterprecision( ZMPMB_vec_[i][1] ) << " " // 22
-
-
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++ )
@@ -538,11 +528,62 @@ void DynamicFilter::printBuffers(deque<COMState> & inputCOMTraj_deq_,
for(unsigned int j = 0 ; j < accelerationTraj_[i].size() ; j++ )
aof << filterprecision( accelerationTraj_[i](j) ) << " " ;
+ aof << filterprecision( ZMPMB_vec_[i][0] ) << " " // 159
+ << filterprecision( ZMPMB_vec_[i][1] ) << " "; // 160
+
+ aof << filterprecision( deltaZMP_deq_[i].px ) << " " // 161
+ << filterprecision( deltaZMP_deq_[i].py ) << " "; // 162
+
+
aof << endl ;
}
aof.close();
+ static double maxErrX = 0 ;
+ static double maxErrY = 0 ;
+ for (unsigned int i = 0 ; i < deltaZMP_deq_.size() ; ++i )
+ {
+ if ( deltaZMP_deq_[i].px > maxErrX )
+ {
+ maxErrX = deltaZMP_deq_[i].px ;
+ }
+ if ( deltaZMP_deq_[i].py > maxErrY )
+ {
+ maxErrY = deltaZMP_deq_[i].py ;
+ }
+ }
+
+ static double moyErrX = 0 ;
+ static double moyErrY = 0 ;
+ static double sumErrX = 0 ;
+ static double sumErrY = 0 ;
+ static int nbRNEAcomputed = 0 ;
+ for (unsigned int i = 0 ; i < deltaZMP_deq_.size(); ++i)
+ {
+ sumErrX += deltaZMP_deq_[i].px ;
+ sumErrY += deltaZMP_deq_[i].py ;
+ }
+ nbRNEAcomputed += deltaZMP_deq_.size() ;
+ moyErrX = sumErrX / nbRNEAcomputed ;
+ moyErrY = sumErrY / nbRNEAcomputed ;
+
+ aFileName = "TestMorisawa2007OnLine32MoyNoisyZMP.dat" ;
+ 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);
+ aof << filterprecision(moyErrX ) << " " // 1
+ << filterprecision(moyErrY ) << " " // 2
+ << filterprecision(maxErrX ) << " " // 3
+ << filterprecision(maxErrY ) << " " // 4
+ << endl ;
+ aof.close();
+
+
++iteration;
return ;
}
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
index 0df47c46..6f87f7d7 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
@@ -29,6 +29,9 @@ namespace PatternGeneratorJRL
~DynamicFilter();
/// \brief
int filter(
+ COMState & lastCtrlCoMState,
+ FootAbsolutePosition & lastCtrlLeftFoot,
+ FootAbsolutePosition & lastCtrlRightFoot,
deque<COMState> & inputCOMTraj_deq_,
deque<ZMPPosition> inputZMPTraj_deq_,
deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
@@ -45,37 +48,60 @@ namespace PatternGeneratorJRL
double CoMHeight
);
+ /// \brief atomic function
+ void InverseKinematics(
+ COMState & inputCoMState,
+ FootAbsolutePosition & inputLeftFoot,
+ FootAbsolutePosition & inputRightFoot,
+ MAL_VECTOR_TYPE(double)& configuration,
+ MAL_VECTOR_TYPE(double)& velocity,
+ MAL_VECTOR_TYPE(double)& acceleration,
+ double samplingPeriod,
+ int stage);
+
+ /// \brief atomic function
+ void ComputeZMPMB(
+ MAL_VECTOR_TYPE(double)& configuration,
+ MAL_VECTOR_TYPE(double)& velocity,
+ MAL_VECTOR_TYPE(double)& acceleration,
+ vector<double> & ZMPMB);
+
private: // Private methods
- // \brief calculate, from the CoM computed by the preview control,
- // the corresponding articular position, velocity and acceleration
+ /// \brief calculate, from the CoM computed by the preview control,
+ /// the corresponding articular position, velocity and acceleration
void InverseKinematics(
+ COMState & lastCtrlCoMState,
+ FootAbsolutePosition & lastCtrlLeftFoot,
+ FootAbsolutePosition & lastCtrlRightFoot,
deque<COMState> & inputCOMTraj_deq_,
deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
- deque<FootAbsolutePosition> & inputRightFootTraj_deq_
- );
+ deque<FootAbsolutePosition> & inputRightFootTraj_deq_);
- // Apply the RNEA on the robot model
- void InverseDynamics(deque<ZMPPosition> inputZMPTraj_deq_);
+ /// \brief Apply the RNEA on the robot model and over the whole trajectory
+ /// given by the function "filter"
+ void InverseDynamics(deque<ZMPPosition> inputZMPTraj_deq);
- /// Preview control on the ZMPMBs computed
- /// --------------------------------------
+ /// \brief Preview control on the ZMPMBs computed
int OptimalControl(std::deque<COMState> & outputDeltaCOMTraj_deq_);
+ // -------------------------------------------------------------------
+
+ /// \brief Debug function
void printAlongTime(deque<COMState> & inputCOMTraj_deq_,
deque<ZMPPosition> inputZMPTraj_deq_,
deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
deque<COMState> & outputDeltaCOMTraj_deq_
);
-
+ /// \brief Debug function
void printBuffers(deque<COMState> & inputCOMTraj_deq_,
deque<ZMPPosition> inputZMPTraj_deq_,
deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
deque<COMState> & outputDeltaCOMTraj_deq_
);
-
+ /// \brief Debug function
double filterprecision(double adb);
@@ -98,7 +124,7 @@ namespace PatternGeneratorJRL
{ previewWindowSize_ = previewWindowSize; }
/// \brief getter :
- inline ComAndFootRealization * getComAndFootRealization()
+ inline ComAndFootRealizationByGeometry * getComAndFootRealization()
{ return comAndFootRealization_;};
inline double getCurrentTime()
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index f6aba5d0..36583b43 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -555,8 +555,15 @@ void
// DYNAMIC FILTER
// --------------
//DynamicFilter( time, tmp );
- dynamicFilter_->filter(COMTraj_deq_,ZMPTraj_deq_,LeftFootTraj_deq_,
- RightFootTraj_deq_,deltaCOMTraj_deq_);
+ dynamicFilter_->filter(
+ FinalCOMTraj_deq.back(),
+ FinalLeftFootTraj_deq.back(),
+ FinalRightFootTraj_deq.back(),
+ COMTraj_deq_,
+ ZMPTraj_deq_,
+ LeftFootTraj_deq_,
+ RightFootTraj_deq_,
+ deltaCOMTraj_deq_);
for (unsigned int i = 0 ; i < deltaCOMTraj_deq_.size() ; ++i )
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index da442ba6..b9ff8494 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -27,26 +27,8 @@
#include "CommonTools.hh"
#include "TestObject.hh"
-
#include <hrp2-dynamics/hrp2OptHumanoidDynamicRobot.h>
-#include <MotionGeneration/ComAndFootRealizationByGeometry.hh>
-#include <metapod/models/hrp2_14/hrp2_14.hh>
-#ifndef METAPOD_INCLUDES
-#define METAPOD_INCLUDES
-// metapod includes
-#include <metapod/tools/print.hh>
-#include <metapod/tools/initconf.hh>
-#include <metapod/algos/rnea.hh>
-#include <Eigen/StdVector>
-#endif
-
-#ifndef METAPOD_TYPEDEF2
-#define METAPOD_TYPEDEF2
-typedef double LocalFloatType2;
-typedef metapod::Spatial::ForceTpl<LocalFloatType2> Force2;
-typedef metapod::hrp2_14<LocalFloatType2> Robot_Model2;
-typedef metapod::Nodes< Robot_Model2, Robot_Model2::BODY >::type Node2;
-#endif
+#include <ZMPRefTrajectoryGeneration/DynamicFilter.hh>
using namespace::PatternGeneratorJRL;
using namespace::PatternGeneratorJRL::TestSuite;
@@ -84,35 +66,40 @@ private:
double m_deltatime;
// buffer to save all the zmps positions
- vector< vector<double> > errZMP_ ;
- Robot_Model2 robot_ ;
- ComAndFootRealization * ComAndFootRealization_;
- SimplePluginManager * SPM ;
- double dInitX, dInitY;
+ vector< vector<double> > deltaZMP_vec ;
+ vector< vector<double> > ZMPMB_vec ;
+ SimplePluginManager * SPM_ ;
+ DynamicFilter* dynamicfilter_;
+ double dInitX_, dInitY_;
int iteration,iteration_zmp ;
bool once ;
+ double samplingPeriod_;
public:
TestMorisawa2007(int argc, char*argv[], string &aString, int TestProfile):
- TestObject(argc, argv, aString)
+ TestObject(argc, argv, aString)
{
m_TestProfile = TestProfile;
m_TestChangeFoot = true;
m_NbStepsModified = 0;
m_deltatime = 0;
- ComAndFootRealization_ = 0 ;
- dInitX = 0 ;
- dInitY = 0 ;
+ SPM_ = NULL ;
+ dynamicfilter_ = NULL ;
+ dInitX_ = 0 ;
+ dInitY_ = 0 ;
iteration_zmp = 0 ;
iteration = 0 ;
once = true ;
+ ZMPMB_vec.resize(2);
+ deltaZMP_vec.clear();
+ samplingPeriod_ = 0.005 ;
};
~TestMorisawa2007()
{
- delete ComAndFootRealization_ ;
- delete SPM ;
+ delete dynamicfilter_ ;
+ delete SPM_ ;
}
bool doTest(ostream &os)
@@ -149,30 +136,30 @@ public:
if (m_PGIInterface==0)
{
ok = m_PGI->RunOneStepOfTheControlLoop(m_CurrentConfiguration,
- m_CurrentVelocity,
- m_CurrentAcceleration,
- m_OneStep.ZMPTarget,
- m_OneStep.finalCOMPosition,
- m_OneStep.LeftFootPosition,
- m_OneStep.RightFootPosition);
+ m_CurrentVelocity,
+ m_CurrentAcceleration,
+ m_OneStep.ZMPTarget,
+ m_OneStep.finalCOMPosition,
+ m_OneStep.LeftFootPosition,
+ m_OneStep.RightFootPosition);
}
else if (m_PGIInterface==1)
{
ok = m_PGI->RunOneStepOfTheControlLoop( m_CurrentConfiguration,
- m_CurrentVelocity,
- m_CurrentAcceleration,
- m_OneStep.ZMPTarget);
+ m_CurrentVelocity,
+ m_CurrentAcceleration,
+ m_OneStep.ZMPTarget);
}
- m_OneStep.NbOfIt++;
+ m_OneStep.NbOfIt++;
- m_clock.stopOneIteration();
+ m_clock.stopOneIteration();
- m_PreviousConfiguration = m_CurrentConfiguration;
- m_PreviousVelocity = m_CurrentVelocity;
- m_PreviousAcceleration = m_CurrentAcceleration;
+ m_PreviousConfiguration = m_CurrentConfiguration;
+ m_PreviousVelocity = m_CurrentVelocity;
+ m_PreviousAcceleration = m_CurrentAcceleration;
/*! Call the reimplemented method to generate events. */
- if (ok)
+ if (ok)
{
m_clock.startModification();
generateEvent();
@@ -182,10 +169,9 @@ public:
/*! Fill the debug files with appropriate information. */
ComparingZMPs();
- ComputeAndDisplayAverageError(false);
fillInDebugFiles();
}
- else
+ else
{
cerr << "Nothing to dump after " << m_OneStep.NbOfIt << endl;
}
@@ -234,16 +220,10 @@ public:
MAL_VECTOR_RESIZE(m_PreviousVelocity, m_HDR->numberDof());
MAL_VECTOR_RESIZE(m_PreviousAcceleration, m_HDR->numberDof());
- SPM = new SimplePluginManager();
-
- ComAndFootRealization_ = new ComAndFootRealizationByGeometry( (PatternGeneratorInterfacePrivate*) SPM );
- ComAndFootRealization_->setHumanoidDynamicRobot(m_HDR);
- ComAndFootRealization_->SetStepStackHandler(new StepStackHandler(SPM));
- ComAndFootRealization_->SetHeightOfTheCoM(0.814);
- ComAndFootRealization_->setSamplingPeriod(0.005);
- ComAndFootRealization_->Initialization();
-
- initIK();
+ SPM_ = new SimplePluginManager();
+ dynamicfilter_ = new DynamicFilter(SPM_,m_HDR);
+ dynamicfilter_->init(0.0,samplingPeriod_,samplingPeriod_,samplingPeriod_,samplingPeriod_,0.814);
+ initIK();
}
protected:
@@ -275,14 +255,17 @@ protected:
lStartingCOMState(0) = m_OneStep.finalCOMPosition.x[0] ;
lStartingCOMState(1) = m_OneStep.finalCOMPosition.y[0] ;
lStartingCOMState(2) = m_OneStep.finalCOMPosition.z[0] ;
- ComAndFootRealization_->SetHeightOfTheCoM(0.814);
- ComAndFootRealization_->setSamplingPeriod(0.005);
- ComAndFootRealization_->Initialization();
-
- ComAndFootRealization_->InitializationCoM(BodyAngles,lStartingCOMState,
- waist,
- m_OneStep.LeftFootPosition, m_OneStep.RightFootPosition);
- ComAndFootRealization_->Initialization();
+ ComAndFootRealizationByGeometry * CaFR = dynamicfilter_->getComAndFootRealization() ;
+ CaFR->SetHeightOfTheCoM(0.814);
+ CaFR->setSamplingPeriod(samplingPeriod_);
+ CaFR->SetStepStackHandler(new StepStackHandler(SPM_));
+ CaFR->Initialization();
+ CaFR->InitializationCoM(BodyAngles,lStartingCOMState,
+ waist, m_OneStep.LeftFootPosition,
+ m_OneStep.RightFootPosition);
+ CaFR->Initialization();
+ CaFR->SetPreviousConfigurationStage0(m_HDR->currentConfiguration());
+ CaFR->SetPreviousVelocityStage0(m_HDR->currentVelocity());
}
void fillInDebugFiles( )
@@ -296,7 +279,7 @@ protected:
aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
- aof << filterprecision(m_OneStep.NbOfIt*0.005 ) << " " // 1
+ aof << filterprecision(m_OneStep.NbOfIt*samplingPeriod_ ) << " " // 1
<< filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " " // 2
<< filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 3
<< filterprecision(m_OneStep.finalCOMPosition.z[0] ) << " " // 4
@@ -327,7 +310,7 @@ protected:
<< filterprecision(m_OneStep.RightFootPosition.ddx ) << " " // 29
<< filterprecision(m_OneStep.RightFootPosition.ddy ) << " " // 30
<< filterprecision(m_OneStep.RightFootPosition.ddz ) << " " // 31
- << filterprecision(m_OneStep.RightFootPosition.theta*M_PI/180 ) << " " // 32
+ << filterprecision(m_OneStep.RightFootPosition.theta*M_PI/180 ) << " " // 32
<< filterprecision(m_OneStep.RightFootPosition.omega ) << " " // 33
<< filterprecision(m_OneStep.RightFootPosition.omega2 ) << " " // 34
<< filterprecision(m_OneStep.ZMPTarget(0)*cos(m_CurrentConfiguration(5)) -
@@ -409,10 +392,13 @@ protected:
void ComparingZMPs()
{
- const int stage0 = 0 ;
- /// \brief calculate, from the CoM of computed by the preview control,
- /// the corresponding articular position, velocity and acceleration
- /// ------------------------------------------------------------------
+ // Debug Purpose
+ // -------------
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+ static int iteration = 0;
+
MAL_VECTOR(CurrentConfiguration,double);
MAL_VECTOR(CurrentVelocity,double);
MAL_VECTOR(CurrentAcceleration,double);
@@ -421,172 +407,170 @@ protected:
MAL_VECTOR_RESIZE(CurrentVelocity, m_HDR->numberDof());
MAL_VECTOR_RESIZE(CurrentAcceleration, m_HDR->numberDof());
- MAL_VECTOR_DIM(aCOMState,double,6);
- MAL_VECTOR_DIM(aCOMSpeed,double,6);
- MAL_VECTOR_DIM(aCOMAcc,double,6);
- MAL_VECTOR_DIM(aLeftFootPosition,double,5);
- MAL_VECTOR_DIM(aRightFootPosition,double,5);
-
- aCOMState(0) = m_OneStep.finalCOMPosition.x[0]; aCOMSpeed(0) = m_OneStep.finalCOMPosition.x[1]; aCOMAcc(0) = m_OneStep.finalCOMPosition.x[2];
- aCOMState(1) = m_OneStep.finalCOMPosition.y[0]; aCOMSpeed(1) = m_OneStep.finalCOMPosition.y[1]; aCOMAcc(1) = m_OneStep.finalCOMPosition.y[2];
- aCOMState(2) = m_OneStep.finalCOMPosition.z[0]; aCOMSpeed(2) = m_OneStep.finalCOMPosition.z[1]; aCOMAcc(2) = m_OneStep.finalCOMPosition.z[2];
- aCOMState(3) = m_OneStep.finalCOMPosition.roll[0]; aCOMSpeed(3) = m_OneStep.finalCOMPosition.roll[1]; aCOMAcc(3) = m_OneStep.finalCOMPosition.roll[2];
- aCOMState(4) = m_OneStep.finalCOMPosition.pitch[0]; aCOMSpeed(4) = m_OneStep.finalCOMPosition.pitch[1]; aCOMAcc(4) = m_OneStep.finalCOMPosition.pitch[2];
- aCOMState(5) = m_OneStep.finalCOMPosition.yaw[0]; aCOMSpeed(5) = m_OneStep.finalCOMPosition.yaw[1]; aCOMAcc(5) = m_OneStep.finalCOMPosition.yaw[2];
-
- aLeftFootPosition(0) = m_OneStep.LeftFootPosition.x; aRightFootPosition(0) = m_OneStep.RightFootPosition.x;
- aLeftFootPosition(1) = m_OneStep.LeftFootPosition.y; aRightFootPosition(1) = m_OneStep.RightFootPosition.y;
- aLeftFootPosition(2) = m_OneStep.LeftFootPosition.z; aRightFootPosition(2) = m_OneStep.RightFootPosition.z;
- aLeftFootPosition(3) = m_OneStep.LeftFootPosition.theta; aRightFootPosition(3) = m_OneStep.RightFootPosition.theta;
- aLeftFootPosition(4) = m_OneStep.LeftFootPosition.omega; aRightFootPosition(4) = m_OneStep.RightFootPosition.omega;
- ComAndFootRealization_->setSamplingPeriod(0.005);
- ComAndFootRealization_->ComputePostureForGivenCoMAndFeetPosture(aCOMState, aCOMSpeed, aCOMAcc,
- aLeftFootPosition,
- aRightFootPosition,
- CurrentConfiguration,
- CurrentVelocity,
- CurrentAcceleration,
- m_OneStep.NbOfIt,
- stage0);
-
- /// \brief Debug Purpose
- /// --------------------
- ofstream aof;
- string aFileName;
- ostringstream oss(std::ostringstream::ate);
- oss.str("TestHerdt2010DynamicART2.dat");
- aFileName = oss.str();
- if ( iteration_zmp == 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 j = 0 ; j < CurrentConfiguration.size() ; ++j)
- {
- aof << filterprecision(CurrentConfiguration(j)) << " " ;
- }
- for (unsigned int j = 0 ; j < CurrentVelocity.size() ; ++j)
- {
- aof << filterprecision(CurrentVelocity(j)) << " " ;
- }
- for (unsigned int j = 0 ; j < CurrentAcceleration.size() ; ++j)
- {
- aof << filterprecision(CurrentAcceleration(j)) << " " ;
- }
- aof << endl ;
-
-
- /// \brief rnea, calculation of the multi body ZMP
- /// ----------------------------------------------
- Robot_Model2::confVector q, dq, ddq;
- for(unsigned int j = 0 ; j < CurrentConfiguration.size() ; j++ )
- {
- q(j,0) = CurrentConfiguration[j] ;
- dq(j,0) = CurrentVelocity[j] ;
- ddq(j,0) = CurrentAcceleration[j] ;
- }
- metapod::rnea< Robot_Model2, true >::run(robot_, q, dq, ddq);
-
- Node2 anode = boost::fusion::at_c<Robot_Model2::BODY>(robot_.nodes);
- Force2 aforce = anode.body.iX0.applyInv (anode.joint.f) ;
-
- double ZMPMB[2];
-
- ZMPMB[0] = - aforce.n()[1] / aforce.f()[2] ;
- ZMPMB[1] = aforce.n()[0] / aforce.f()[2] ;
+ dynamicfilter_->InverseKinematics(
+ m_OneStep.finalCOMPosition,m_OneStep.LeftFootPosition,m_OneStep.RightFootPosition,
+ CurrentConfiguration,CurrentVelocity,CurrentAcceleration,samplingPeriod_,iteration);
+ vector<double> dZMPtmp (2);
+ vector<double> ZMPMBtmp (2);
+ dynamicfilter_->ComputeZMPMB(CurrentConfiguration,CurrentVelocity,CurrentAcceleration,ZMPMBtmp);
if (m_OneStep.NbOfIt<=10){
- dInitX = m_OneStep.ZMPTarget(0) - ZMPMB[0] ;
- dInitY = m_OneStep.ZMPTarget(1) - ZMPMB[1] ;
- {
- vector<double> tmp_zmp(2) ;
- tmp_zmp[0] =0.0 ;
- tmp_zmp[1] =0.0 ;
- errZMP_.push_back(tmp_zmp);
- }
+ dInitX_ = m_OneStep.ZMPTarget(0) - ZMPMBtmp[0] ;
+ dInitY_ = m_OneStep.ZMPTarget(1) - ZMPMBtmp[1] ;
}
- if (m_OneStep.NbOfIt >= 10)
- {
- double errx = sqrt( ( m_OneStep.ZMPTarget(0) - ZMPMB[0] - dInitX )*( m_OneStep.ZMPTarget(0) - ZMPMB[0] - dInitX ) ) ;
- double erry = sqrt( ( m_OneStep.ZMPTarget(1) - ZMPMB[1] - dInitY )*( m_OneStep.ZMPTarget(1) - ZMPMB[1] - dInitY ) ) ;
- {
- vector<double> tmp_zmp(2) ;
- tmp_zmp[0] = errx ;
- tmp_zmp[1] = erry ;
- errZMP_.push_back(tmp_zmp);
- }
- }
+ dZMPtmp[0] = m_OneStep.ZMPTarget(0) - ZMPMBtmp[0] - dInitX_ ;
+ dZMPtmp[1] = m_OneStep.ZMPTarget(1) - ZMPMBtmp[1] - dInitY_ ;
+ deltaZMP_vec.push_back(dZMPtmp);
+ ZMPMB_vec.push_back(ZMPMBtmp);
- static double ecartmaxX = 0 ;
- static double ecartmaxY = 0 ;
- if ( abs(errZMP_.back()[0]) > ecartmaxX )
- ecartmaxX = abs(errZMP_.back()[0]) ;
- if ( abs(errZMP_.back()[1]) > ecartmaxY )
- ecartmaxY = abs(errZMP_.back()[1]) ;
-// cout << "ecartmaxX :" << ecartmaxX << endl ;
-// cout << "ecartmaxY :" << ecartmaxY << endl ;
+ // --------------------
+ oss.str("DynamicFilterAllVariablesFiltered.dat");
+ aFileName = oss.str();
- // Writing of the two zmps and the error.
- if (once)
+ if ( iteration == 0 )
{
- aof.open("TestHerdt2010ErrorZMP.dat",ofstream::out);
+ aof.open(aFileName.c_str(),ofstream::out);
aof.close();
- once = false ;
}
- aof.open("TestHerdt2010ErrorZMP.dat",ofstream::app);
+ ///----
+ aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
- aof << filterprecision( iteration_zmp ) << " " // 1
- << filterprecision( ZMPMB[0] + dInitX ) << " " // 2
- << filterprecision( ZMPMB[1] + dInitY ) << " " // 3
- << filterprecision(m_OneStep.ZMPTarget(0) ) << " " // 4
- << filterprecision(m_OneStep.ZMPTarget(1) ) << " " // 5
- << endl ;
+ aof << filterprecision( iteration*samplingPeriod_ ) << " " // 0
+ << filterprecision( m_OneStep.finalCOMPosition.x[0] ) << " " // 1
+ << filterprecision( m_OneStep.finalCOMPosition.x[1] ) << " " // 2
+ << filterprecision( m_OneStep.finalCOMPosition.x[2] ) << " " // 3
+ << filterprecision( m_OneStep.finalCOMPosition.y[0] ) << " " // 4
+ << filterprecision( m_OneStep.finalCOMPosition.y[1] ) << " " // 5
+ << filterprecision( m_OneStep.finalCOMPosition.y[2] ) << " " // 6
+ << filterprecision( m_OneStep.finalCOMPosition.z[0] ) << " " // 7
+ << filterprecision( m_OneStep.finalCOMPosition.z[1] ) << " " // 8
+ << filterprecision( m_OneStep.finalCOMPosition.z[2] ) << " " // 9
+ << filterprecision( m_OneStep.finalCOMPosition.roll[0] ) << " " // 10
+ << filterprecision( m_OneStep.finalCOMPosition.roll[1] ) << " " // 11
+ << filterprecision( m_OneStep.finalCOMPosition.roll[2] ) << " " // 12
+ << filterprecision( m_OneStep.finalCOMPosition.pitch[0] ) << " "// 13
+ << filterprecision( m_OneStep.finalCOMPosition.pitch[1] ) << " "// 14
+ << filterprecision( m_OneStep.finalCOMPosition.pitch[2] ) << " "// 15
+ << filterprecision( m_OneStep.finalCOMPosition.yaw[0] ) << " " // 16
+ << filterprecision( m_OneStep.finalCOMPosition.yaw[1] ) << " " // 17
+ << filterprecision( m_OneStep.finalCOMPosition.yaw[2] ) << " " // 18
+
+ << filterprecision( m_OneStep.ZMPTarget[0] ) << " " // 19
+ << filterprecision( m_OneStep.ZMPTarget[1] ) << " " // 20
+
+ << filterprecision( m_OneStep.LeftFootPosition.x ) << " " // 21
+ << filterprecision( m_OneStep.LeftFootPosition.y ) << " " // 22
+ << filterprecision( m_OneStep.LeftFootPosition.z ) << " " // 23
+ << filterprecision( m_OneStep.LeftFootPosition.theta ) << " " // 24
+ << filterprecision( m_OneStep.LeftFootPosition.omega ) << " " // 25
+ << filterprecision( m_OneStep.LeftFootPosition.dx ) << " " // 26
+ << filterprecision( m_OneStep.LeftFootPosition.dy ) << " " // 27
+ << filterprecision( m_OneStep.LeftFootPosition.dz ) << " " // 28
+ << filterprecision( m_OneStep.LeftFootPosition.dtheta ) << " " // 29
+ << filterprecision( m_OneStep.LeftFootPosition.domega ) << " " // 30
+ << filterprecision( m_OneStep.LeftFootPosition.ddx ) << " " // 31
+ << filterprecision( m_OneStep.LeftFootPosition.ddy ) << " " // 32
+ << filterprecision( m_OneStep.LeftFootPosition.ddz ) << " " // 33
+ << filterprecision( m_OneStep.LeftFootPosition.ddtheta ) << " " // 34
+ << filterprecision( m_OneStep.LeftFootPosition.ddomega ) << " " // 35
+
+ << filterprecision( m_OneStep.RightFootPosition.x ) << " " // 36
+ << filterprecision( m_OneStep.RightFootPosition.y ) << " " // 37
+ << filterprecision( m_OneStep.RightFootPosition.z ) << " " // 38
+ << filterprecision( m_OneStep.RightFootPosition.theta ) << " " // 39
+ << filterprecision( m_OneStep.RightFootPosition.omega ) << " " // 40
+ << filterprecision( m_OneStep.RightFootPosition.dx ) << " " // 41
+ << filterprecision( m_OneStep.RightFootPosition.dy ) << " " // 42
+ << filterprecision( m_OneStep.RightFootPosition.dz ) << " " // 43
+ << filterprecision( m_OneStep.RightFootPosition.dtheta ) << " " // 44
+ << filterprecision( m_OneStep.RightFootPosition.domega ) << " " // 45
+ << filterprecision( m_OneStep.RightFootPosition.ddx ) << " " // 46
+ << filterprecision( m_OneStep.RightFootPosition.ddy ) << " " // 47
+ << filterprecision( m_OneStep.RightFootPosition.ddz ) << " " // 48
+ << filterprecision( m_OneStep.RightFootPosition.ddtheta ) << " " // 49
+ << filterprecision( m_OneStep.RightFootPosition.ddomega ) << " ";// 50
+
+ for(unsigned int j = 0 ; j < CurrentConfiguration.size() ; j++ )
+ aof << filterprecision( CurrentConfiguration(j) ) << " " ;
+ for(unsigned int j = 0 ; j < CurrentVelocity.size() ; j++ )
+ aof << filterprecision( CurrentVelocity(j) ) << " " ;
+ for(unsigned int j = 0 ; j < CurrentAcceleration.size() ; j++ )
+ aof << filterprecision( CurrentAcceleration(j) ) << " " ;
+
+ aof << filterprecision( ZMPMB_vec.back()[0] ) << " " // 159
+ << filterprecision( ZMPMB_vec.back()[1] ) << " "; // 160
+
+ aof << filterprecision( deltaZMP_vec.back()[0] ) << " " // 161
+ << filterprecision( deltaZMP_vec.back()[1] ) << " "; // 162
+
+
+ aof << endl ;
+
aof.close();
- iteration_zmp++;
+ ++iteration;
return ;
}
void ComputeAndDisplayAverageError(bool display){
static int plot_it = 0 ;
- double moyErrX = 0 ;
- double moyErrY = 0 ;
- for (unsigned int i = 0 ; i < errZMP_.size(); ++i)
+
+ static double maxErrX = 0 ;
+ static double maxErrY = 0 ;
+ for (unsigned int i = 0 ; i < deltaZMP_vec.size() ; ++i )
+ {
+ if ( abs(deltaZMP_vec[i][0]) > maxErrX )
+ {
+ maxErrX = abs(deltaZMP_vec[i][0]) ;
+ }
+ if ( abs(deltaZMP_vec[i][1]) > maxErrY )
+ {
+ maxErrY = abs(deltaZMP_vec[i][1]) ;
+ }
+ }
+
+ static double moyErrX = 0 ;
+ static double moyErrY = 0 ;
+ static double sumErrX = 0 ;
+ static double sumErrY = 0 ;
+ for (unsigned int i = 0 ; i < deltaZMP_vec.size(); ++i)
{
- moyErrX += errZMP_[i][0] ;
- moyErrY += errZMP_[i][1] ;
+ sumErrX += abs(deltaZMP_vec[i][0]) ;
+ sumErrY += abs(deltaZMP_vec[i][1]) ;
}
- moyErrX = moyErrX / errZMP_.size() ;
- moyErrY = moyErrY / errZMP_.size() ;
- if ( display )
+ moyErrX = sumErrX / deltaZMP_vec.size() ;
+ moyErrY = sumErrY / deltaZMP_vec.size() ;
+
+ if(display)
{
- cout << "moyErrX = " << moyErrX << endl
- << "moyErrY = " << moyErrY << endl ;
+ cout << "moyErrX = " << moyErrX ;
+ cout << "moyErrY = " << moyErrY ;
+ cout << "maxErrX = " << maxErrX ;
+ cout << "maxErrY = " << maxErrY ;
}
+
ofstream aof;
- string aFileName;
- aFileName = m_TestName;
- aFileName += "MoyZMP.dat";
- if(plot_it==0){
+ string aFileName;
+ aFileName = "TestMorisawa2007OnLine32MoyFilteredZMP.dat" ;
+ if(plot_it==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);
- aof << filterprecision(moyErrX ) << " " // 1
+ }
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision(moyErrX ) << " " // 1
<< filterprecision(moyErrY ) << " " // 2
+ << filterprecision(maxErrX ) << " " // 3
+ << filterprecision(maxErrY ) << " " // 4
<< endl ;
aof.close();
+
plot_it++;
}
--
GitLab