From 34a245184204b0afafc9cd511c9c7671c51f1f04 Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Mon, 30 Jun 2014 14:06:23 +0200
Subject: [PATCH] include the computation of the orientation of the waist in
the Morisawa PG
---
.../FootTrajectoryGenerationStandard.cpp | 1 +
.../AnalyticalZMPCOGTrajectory.cpp | 3 +-
.../AnalyticalMorisawaCompact.cpp | 159 +++++++++---------
.../DynamicFilter.cpp | 139 +++++++++++++--
.../DynamicFilter.hh | 4 +-
.../ZMPVelocityReferencedQP.cpp | 59 ++++---
.../generator-vel-ref.cpp | 3 +-
tests/TestHerdt2010DynamicFilter.cpp | 6 +-
tests/TestMorisawa2007.cpp | 2 +-
9 files changed, 257 insertions(+), 119 deletions(-)
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
index dd691dec..052e48dd 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
@@ -321,6 +321,7 @@ double FootTrajectoryGenerationStandard::ComputeAll(FootAbsolutePosition & aFoot
aFootAbsolutePosition.theta = m_PolynomeTheta->Compute(Time);
aFootAbsolutePosition.dtheta = m_PolynomeTheta->ComputeDerivative(Time);
+ aFootAbsolutePosition.ddtheta = m_PolynomeTheta->ComputeSecDerivative(Time);
aFootAbsolutePosition.omega = m_PolynomeOmega->Compute(Time);
aFootAbsolutePosition.domega = m_PolynomeOmega->ComputeDerivative(Time);
diff --git a/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp b/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp
index c64f995f..01e08ec8 100644
--- a/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp
+++ b/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp
@@ -489,6 +489,7 @@ namespace PatternGeneratorJRL
bool AnalyticalZMPCOGTrajectory::GetIntervalIndexFromTime(double t, unsigned int &j, unsigned int &prev_j)
{
+ cout << "m_Sensitivity = " << m_Sensitivity << endl ;
ODEBUG("Here "<< m_DeltaTj.size());
t -= m_AbsoluteTimeReference;
double reftime=0;
@@ -496,7 +497,7 @@ namespace PatternGeneratorJRL
for(unsigned int lj=prev_j;lj<m_DeltaTj.size();lj++)
{
ODEBUG("t: " << t << " reftime :" << reftime << " Tj["<<j << "]= "<< m_DeltaTj[j]);
- if (((t+m_Sensitivity)>=reftime) && (t<=reftime+m_DeltaTj[lj]+m_Sensitivity))
+ if (((t+m_Sensitivity)>=reftime) && (t<reftime+m_DeltaTj[lj]+m_Sensitivity))
{
j = lj;
return true;
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index a25c53b9..d6a46dfd 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -665,27 +665,27 @@ namespace PatternGeneratorJRL
COMState aCOMPos;
memset(&aCOMPos,0,sizeof(aCOMPos));
- if (m_FilteringActivate)
- {
- double FZmpX=0, FComX=0,FComdX=0;
-
- // Should we filter ?
- bool r = m_FilterXaxisByPC->UpdateOneStep(time,FZmpX, FComX, FComdX);
- if (r)
- {
- double FZmpY=0, FComY=0,FComdY=0;
- // Yes we should.
- m_FilterYaxisByPC->UpdateOneStep(time,FZmpY, FComY, FComdY);
-
- /*! Feed the ZMPPositions. */
- aZMPPos.px = FZmpX;
- aZMPPos.py = FZmpY;
-
- /*! Feed the COMStates. */
- aCOMPos.x[0] = FComX; aCOMPos.x[1] = FComdX;
- aCOMPos.y[0] = FComY; aCOMPos.y[1] = FComdY;
- }
- }
+// if (m_FilteringActivate)
+// {
+// double FZmpX=0, FComX=0,FComdX=0;
+//
+// // Should we filter ?
+// bool r = m_FilterXaxisByPC->UpdateOneStep(time,FZmpX, FComX, FComdX);
+// if (r)
+// {
+// double FZmpY=0, FComY=0,FComdY=0;
+// // Yes we should.
+// m_FilterYaxisByPC->UpdateOneStep(time,FZmpY, FComY, FComdY);
+//
+// /*! Feed the ZMPPositions. */
+// aZMPPos.px = FZmpX;
+// aZMPPos.py = FZmpY;
+//
+// /*! Feed the COMStates. */
+// aCOMPos.x[0] = FComX; aCOMPos.x[1] = FComdX;
+// aCOMPos.y[0] = FComY; aCOMPos.y[1] = FComdY;
+// }
+// }
/*! Feed the ZMPPositions. */
@@ -708,10 +708,10 @@ namespace PatternGeneratorJRL
aCOMPos.x[0] += lCOMPosx; aCOMPos.x[1] += lCOMPosdx; aCOMPos.x[2] = lCOMPosddx;
aCOMPos.y[0] += lCOMPosy; aCOMPos.y[1] += lCOMPosdy; aCOMPos.y[2] = lCOMPosddy;
aCOMPos.z[0] = m_InitialPoseCoMHeight;
- FinalCOMStates.push_back(aCOMPos);
- /*! Feed the FootPositions. */
+ /*! Feed the FootPositions. */
+
/*! Left */
FootAbsolutePosition LeftFootAbsPos;
memset(&LeftFootAbsPos,0,sizeof(LeftFootAbsPos));
@@ -724,6 +724,13 @@ namespace PatternGeneratorJRL
m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(-1,time,RightFootAbsPos,lIndexInterval);
FinalRightFootAbsolutePositions.push_back(RightFootAbsPos);
+ /*! Feed the Waist orientation */
+ aCOMPos.yaw[0] = 0.5*(LeftFootAbsPos.theta + RightFootAbsPos.theta);
+ aCOMPos.yaw[1] = 0.5*(LeftFootAbsPos.dtheta + RightFootAbsPos.dtheta);
+ aCOMPos.yaw[2] = 0.5*(LeftFootAbsPos.ddtheta + RightFootAbsPos.ddtheta);
+ FinalCOMStates.push_back(aCOMPos);
+
+ /*! Feed the ZMPMB */
vector<double> ZMPMB (2) ;
static int iteration = 0;
m_kajitaDynamicFilter->ComputeZMPMB( m_SamplingPeriod, aCOMPos, LeftFootAbsPos, RightFootAbsPos, ZMPMB , iteration);
@@ -744,59 +751,59 @@ namespace PatternGeneratorJRL
aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
- aof << iteration*m_SamplingPeriod << " " // 0
- << aCOMPos.x[0] << " " // 1
- << aCOMPos.x[1] << " " // 2
- << aCOMPos.x[2] << " " // 3
- << aCOMPos.y[0] << " " // 4
- << aCOMPos.y[1] << " " // 5
- << aCOMPos.y[2] << " " // 6
- << aCOMPos.z[0] << " " // 7
- << aCOMPos.z[1] << " " // 8
- << aCOMPos.z[2] << " " // 9
- << aCOMPos.roll[0] << " " // 10
- << aCOMPos.roll[1] << " " // 11
- << aCOMPos.roll[2] << " " // 12
- << aCOMPos.pitch[0] << " "// 13
- << aCOMPos.pitch[1] << " "// 14
- << aCOMPos.pitch[2] << " "// 15
- << aCOMPos.yaw[0] << " " // 16
- << aCOMPos.yaw[1] << " " // 17
- << aCOMPos.yaw[2] << " " // 18
- << aZMPPos.px << " " // 19
- << aZMPPos.py << " " // 20
- << ZMPMB[0] << " " // 21
- << ZMPMB[1] << " " // 22
- << LeftFootAbsPos.x << " " // 23
- << LeftFootAbsPos.y << " " // 24
- << LeftFootAbsPos.z << " " // 25
- << LeftFootAbsPos.theta << " " // 26
- << LeftFootAbsPos.omega << " " // 27
- << LeftFootAbsPos.dx << " " // 28
- << LeftFootAbsPos.dy << " " // 29
- << LeftFootAbsPos.dz << " " // 30
- << LeftFootAbsPos.dtheta << " " // 31
- << LeftFootAbsPos.domega << " " // 32
- << LeftFootAbsPos.ddx << " " // 33
- << LeftFootAbsPos.ddy << " " // 34
- << LeftFootAbsPos.ddz << " " // 35
- << LeftFootAbsPos.ddtheta << " " // 36
- << LeftFootAbsPos.ddomega << " " // 37
- << RightFootAbsPos.x << " " // 38
- << RightFootAbsPos.y << " " // 39
- << RightFootAbsPos.z << " " // 40
- << RightFootAbsPos.theta << " " // 41
- << RightFootAbsPos.omega << " " // 42
- << RightFootAbsPos.dx << " " // 43
- << RightFootAbsPos.dy << " " // 44
- << RightFootAbsPos.dz << " " // 45
- << RightFootAbsPos.dtheta << " " // 46
- << RightFootAbsPos.domega << " " // 47
- << RightFootAbsPos.ddx << " " // 48
- << RightFootAbsPos.ddy << " " // 49
- << RightFootAbsPos.ddz << " " // 50
- << RightFootAbsPos.ddtheta << " "// 51
- << RightFootAbsPos.ddomega << " ";// 52
+ aof << iteration*m_SamplingPeriod << " " // 1
+ << aCOMPos.x[0] << " " // 2
+ << aCOMPos.x[1] << " " // 3
+ << aCOMPos.x[2] << " " // 4
+ << aCOMPos.y[0] << " " // 5
+ << aCOMPos.y[1] << " " // 6
+ << aCOMPos.y[2] << " " // 7
+ << aCOMPos.z[0] << " " // 8
+ << aCOMPos.z[1] << " " // 9
+ << aCOMPos.z[2] << " " // 10
+ << aCOMPos.roll[0] << " " // 11
+ << aCOMPos.roll[1] << " " // 12
+ << aCOMPos.roll[2] << " " // 13
+ << aCOMPos.pitch[0] << " " // 14
+ << aCOMPos.pitch[1] << " " // 15
+ << aCOMPos.pitch[2] << " " // 16
+ << aCOMPos.yaw[0] << " " // 17
+ << aCOMPos.yaw[1] << " " // 18
+ << aCOMPos.yaw[2] << " " // 19
+ << aZMPPos.px << " " // 20
+ << aZMPPos.py << " " // 21
+ << ZMPMB[0] << " " // 22
+ << ZMPMB[1] << " " // 23
+ << LeftFootAbsPos.x << " " // 24
+ << LeftFootAbsPos.y << " " // 25
+ << LeftFootAbsPos.z << " " // 26
+ << LeftFootAbsPos.theta << " " // 27
+ << LeftFootAbsPos.omega << " " // 28
+ << LeftFootAbsPos.dx << " " // 29
+ << LeftFootAbsPos.dy << " " // 30
+ << LeftFootAbsPos.dz << " " // 31
+ << LeftFootAbsPos.dtheta << " " // 32
+ << LeftFootAbsPos.domega << " " // 33
+ << LeftFootAbsPos.ddx << " " // 34
+ << LeftFootAbsPos.ddy << " " // 35
+ << LeftFootAbsPos.ddz << " " // 36
+ << LeftFootAbsPos.ddtheta << " " // 37
+ << LeftFootAbsPos.ddomega << " " // 38
+ << RightFootAbsPos.x << " " // 39
+ << RightFootAbsPos.y << " " // 40
+ << RightFootAbsPos.z << " " // 41
+ << RightFootAbsPos.theta << " " // 42
+ << RightFootAbsPos.omega << " " // 43
+ << RightFootAbsPos.dx << " " // 44
+ << RightFootAbsPos.dy << " " // 45
+ << RightFootAbsPos.dz << " " // 46
+ << RightFootAbsPos.dtheta << " " // 47
+ << RightFootAbsPos.domega << " " // 48
+ << RightFootAbsPos.ddx << " " // 49
+ << RightFootAbsPos.ddy << " " // 50
+ << RightFootAbsPos.ddz << " " // 51
+ << RightFootAbsPos.ddtheta << " " // 52
+ << RightFootAbsPos.ddomega << " "; // 53
aof << endl ;
aof.close();
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index c91be8f3..195a00af 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -130,6 +130,7 @@ void DynamicFilter::init(
prev_dq_ = dq_ ;
prev_ddq_ = ddq_ ;
bcalc<Robot_Model>::run(robot_, prev_q_);
+ bcalc<Robot_Model>::run(robot_2,prev_q_);
deltaZMP_deq_.resize( PG_N_);
ZMPMB_vec_.resize( PG_N_, vector<double>(2));
@@ -220,19 +221,19 @@ void DynamicFilter::InverseKinematics(
int stage,
int iteration)
{
- 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];bcalc<Robot_Model>::run(robot_, prev_q_);
+ 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;
@@ -284,6 +285,7 @@ void DynamicFilter::ComputeZMPMB(
ZMPMBConfiguration_, ZMPMBVelocity_, ZMPMBAcceleration_,
samplingPeriod, stage, iteration) ;
+
// Copy the angular trajectory data from "Boost" to "Eigen"
for(unsigned int j = 0 ; j < ZMPMBConfiguration_.size() ; j++ )
{
@@ -304,6 +306,117 @@ void DynamicFilter::ComputeZMPMB(
ZMPMB[0] = - m_force.n()[1] / m_force.f()[2] ;
ZMPMB[1] = m_force.n()[0] / m_force.f()[2] ;
+
+
+
+
+
+ Robot_Model::confVector q, dq, ddq;
+ for(unsigned int j = 0 ; j < 6 ; j++ )
+ {
+ q(j,0) = 0 ;
+ dq(j,0) = 0 ;
+ ddq(j,0) = 0 ;
+ }
+ for(unsigned int j = 6 ; j < ZMPMBConfiguration_.size() ; j++ )
+ {
+ q(j,0) = ZMPMBConfiguration_(j) ;
+ dq(j,0) = ZMPMBVelocity_(j) ;
+ ddq(j,0) = ZMPMBAcceleration_(j) ;
+ }
+
+ metapod::rnea< Robot_Model, true >::run(robot_2, q, dq, ddq);
+ LankleNode & node_lankle = boost::fusion::at_c<Robot_Model::l_ankle>(robot_2.nodes);
+ RankleNode & node_rankle = boost::fusion::at_c<Robot_Model::r_ankle>(robot_2.nodes);
+
+ // Debug Purpose
+ // -------------
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+ static int it = 0;
+
+ // --------------------
+ oss.str("DynamicFilterMetapodAccWaistSupportFoot.dat");
+ aFileName = oss.str();
+ if(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( it*samplingPeriod) << " " ; // 1
+
+ if (inputLeftFoot.stepType < 0)
+ {
+ aof << filterprecision( node_lankle.body.ai.v()(0,0) ) << " " // 2
+ << filterprecision( node_lankle.body.ai.v()(1,0) ) << " " // 3
+ << filterprecision( node_lankle.body.ai.v()(2,0) ) << " " // 4
+ << filterprecision( node_lankle.body.ai.w()(0,0) ) << " " // 5
+ << filterprecision( node_lankle.body.ai.w()(1,0) ) << " " // 6
+ << filterprecision( node_lankle.body.ai.w()(2,0) ) << " "; // 7
+ }else
+ {
+ aof << filterprecision( node_rankle.body.ai.v()(0,0) ) << " " // 2
+ << filterprecision( node_rankle.body.ai.v()(1,0) ) << " " // 3
+ << filterprecision( node_rankle.body.ai.v()(2,0) ) << " " // 4
+ << filterprecision( node_rankle.body.ai.w()(0,0) ) << " " // 5
+ << filterprecision( node_rankle.body.ai.w()(1,0) ) << " " // 6
+ << filterprecision( node_rankle.body.ai.w()(2,0) ) << " " ;// 7
+ }
+
+ aof << filterprecision( inputCoMState.x[2] ) << " " // 8
+ << filterprecision( inputCoMState.y[2] ) << " " // 9
+ << filterprecision( inputCoMState.z[2] ) << " " // 10
+ << filterprecision( inputCoMState.roll[2] ) << " " // 11
+ << filterprecision( inputCoMState.pitch[2] ) << " " // 12
+ << filterprecision( inputCoMState.yaw[2] ) << " " ; // 13
+
+ if (inputLeftFoot.stepType < 0)
+ {
+ aof << filterprecision( node_lankle.body.vi.v()(0,0) ) << " " // 14
+ << filterprecision( node_lankle.body.vi.v()(1,0) ) << " " // 15
+ << filterprecision( node_lankle.body.vi.v()(2,0) ) << " " // 16
+ << filterprecision( node_lankle.body.vi.w()(0,0) ) << " " // 17
+ << filterprecision( node_lankle.body.vi.w()(1,0) ) << " " // 18
+ << filterprecision( node_lankle.body.vi.w()(2,0) ) << " " ;// 19
+ }else
+ {
+ aof << filterprecision( node_rankle.body.vi.v()(0,0) ) << " " // 14
+ << filterprecision( node_rankle.body.vi.v()(1,0) ) << " " // 15
+ << filterprecision( node_rankle.body.vi.v()(2,0) ) << " " // 16
+ << filterprecision( node_rankle.body.vi.w()(0,0) ) << " " // 17
+ << filterprecision( node_rankle.body.vi.w()(1,0) ) << " " // 18
+ << filterprecision( node_rankle.body.vi.w()(2,0) ) << " "; // 19
+ }
+ aof << filterprecision( inputCoMState.x[1] ) << " " // 20
+ << filterprecision( inputCoMState.y[1] ) << " " // 21
+ << filterprecision( inputCoMState.z[1] ) << " " // 22
+ << filterprecision( inputCoMState.roll[1] ) << " " // 23
+ << filterprecision( inputCoMState.pitch[1] ) << " " // 24
+ << filterprecision( inputCoMState.yaw[1] ) << " " ; // 25
+
+ for(unsigned int j = 0 ; j < ZMPMBConfiguration_.size() ; j++ )//26
+ aof << filterprecision( ZMPMBConfiguration_(j) ) << " " ;
+ for(unsigned int j = 0 ; j < ZMPMBVelocity_.size() ; j++ ) //62
+ aof << filterprecision( ZMPMBVelocity_(j) ) << " " ;
+ for(unsigned int j = 0 ; j < ZMPMBAcceleration_.size() ; j++ )//98
+ aof << filterprecision( ZMPMBAcceleration_(j) ) << " " ;
+
+ aof << filterprecision( node_waist.body.vi.v()(0,0) ) << " " // 133
+ << filterprecision( node_waist.body.vi.v()(1,0) ) << " " // 134
+ << filterprecision( node_waist.body.vi.v()(2,0) ) << " " // 135
+ << filterprecision( node_waist.body.vi.w()(0,0) ) << " " // 136
+ << filterprecision( node_waist.body.vi.w()(1,0) ) << " " // 137
+ << filterprecision( node_waist.body.vi.w()(2,0) ) << " "; // 138
+
+ aof << endl ;
+ aof.close();
+ ++it;
+
return ;
}
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
index c24d24aa..831fbefa 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
@@ -12,6 +12,8 @@
typedef metapod::Spatial::ForceTpl<LocalFloatType> Force_HRP2_14;
typedef metapod::hrp2_14<LocalFloatType> Robot_Model;
typedef metapod::Nodes< Robot_Model, Robot_Model::BODY >::type RootNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::l_ankle >::type LankleNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::r_ankle >::type RankleNode;
typedef metapod::jac_point_chain < Robot_Model,
Robot_Model::l_ankle, Robot_Model::BODY,0,true,false> Jacobian_LF;
@@ -224,7 +226,7 @@ namespace PatternGeneratorJRL
/// ---------------------------------
/// \brief Initialize the robot with the autogenerated files
/// by MetapodFromUrdf
- Robot_Model robot_;
+ Robot_Model robot_,robot_2;
/// \brief Initialize the robot leg jacobians with the
/// autogenerated files by MetapodFromUrdf
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index f03ecba6..7c1f8ca8 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -538,7 +538,8 @@ void
RightFootTraj_deq_ = FinalRightFootTraj_deq ;
FinalZMPTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
FinalCOMTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
- deltaCOMTraj_deq_.resize(NbSampleControl_);
+ vector < vector<double> > ZMPMB_deq (NbSampleControl_,vector<double>(2));
+ //deltaCOMTraj_deq_.resize(NbSampleControl_);
// INTERPRET THE SOLUTION VECTOR :
// -------------------------------
InterpretSolutionVector();
@@ -555,25 +556,33 @@ void
// DYNAMIC FILTER
// --------------
//DynamicFilter( time, tmp );
- 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 )
- {
- for(int j=0;j<3;j++)
- {
- FinalCOMTraj_deq[CurrentIndex_+i].x[j] = deltaCOMTraj_deq_[i].x[j];
- FinalCOMTraj_deq[CurrentIndex_+i].y[j] = deltaCOMTraj_deq_[i].y[j];
- }
- }
+// 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 = CurrentIndex_ ; i < FinalCOMTraj_deq.size() ; ++ i )
+ dynamicFilter_->ComputeZMPMB(m_SamplingPeriod,
+ FinalCOMTraj_deq[i],
+ FinalLeftFootTraj_deq[i],
+ FinalRightFootTraj_deq[i],
+ ZMPMB_deq[i-CurrentIndex_],
+ (int)time/QP_T_ + i*m_SamplingPeriod
+ );
+
+
+// for (unsigned int i = 0 ; i < deltaCOMTraj_deq_.size() ; ++i )
+// {
+// for(int j=0;j<3;j++)
+// {
+// FinalCOMTraj_deq[CurrentIndex_+i].x[j] = deltaCOMTraj_deq_[i].x[j];
+// FinalCOMTraj_deq[CurrentIndex_+i].y[j] = deltaCOMTraj_deq_[i].y[j];
+// }
+// }
@@ -587,9 +596,9 @@ void
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 );
+// int iteration100 = (int)iteration/100;
+// int iteration10 = (int)(iteration - iteration100*100)/10;
+// int iteration1 = (int)(iteration - iteration100*100 - iteration10*10 );
//
// /// \brief Debug Purpose
// /// --------------------
@@ -770,6 +779,10 @@ void
<< filterprecision( FinalCOMTraj_deq[i].yaw[2] ) << " " // 48
<< filterprecision( FinalLeftFootTraj_deq[i].dddx ) << " " // 49
<< filterprecision( FinalRightFootTraj_deq[i].dddx ) << " " // 50
+ << filterprecision( ZMPMB_deq[i-CurrentIndex_][0] ) << " " // 51
+ << filterprecision( ZMPMB_deq[i-CurrentIndex_][1] ) << " " // 52
+ << filterprecision( FinalZMPTraj_deq[i].px ) << " " // 53
+ << filterprecision( FinalZMPTraj_deq[i].py ) << " " // 54
<< endl ;
}
aof.close();
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index b9894439..ed1ebc80 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -567,7 +567,8 @@ 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);
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index 5a8ca6df..c6a71fb1 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -759,9 +759,9 @@ void startOnLineWalking(PatternGeneratorInterface &aPGI)
{65*200,&TestHerdt2010::startTurningRightOnSpot},
{75*200,&TestHerdt2010::walkForward},
{85*200,&TestHerdt2010::startTurningLeft},
- {95*200,&TestHerdt2010::startTurningRight},
- {105*200,&TestHerdt2010::stop},
- {110*200,&TestHerdt2010::stopOnLineWalking}};
+ {95*200,&TestHerdt2010::startTurningRight},
+ {105*200,&TestHerdt2010::stop},
+ {110*200,&TestHerdt2010::stopOnLineWalking}};
// #define localNbOfEvents 6
// struct localEvent events [localNbOfEvents] =
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index 73650a36..06695545 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -168,7 +168,7 @@ public:
m_clock.fillInStatistics();
/*! Fill the debug files with appropriate information. */
- ComparingZMPs();
+ //ComparingZMPs();
fillInDebugFiles();
}
else
--
GitLab