From c81e9c394438dfd042e756b8e2e666234a9a6d97 Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Mon, 17 Mar 2014 19:02:59 +0100
Subject: [PATCH] commit before switching branches
---
src/PreviewControl/PreviewControl.cpp | 136 +++----
.../ZMPVelocityReferencedQP.cpp | 376 ++++--------------
.../ZMPVelocityReferencedQP.hh | 13 +-
tests/TestHerdt2010DynamicFilter.cpp | 121 +++++-
tests/TestKajita2003.cpp | 115 ++++++
tests/TestObject.cpp | 45 +--
6 files changed, 391 insertions(+), 415 deletions(-)
diff --git a/src/PreviewControl/PreviewControl.cpp b/src/PreviewControl/PreviewControl.cpp
index f268883a..2c16ff02 100644
--- a/src/PreviewControl/PreviewControl.cpp
+++ b/src/PreviewControl/PreviewControl.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2006, 2007, 2008, 2009, 2010,
+ * Copyright 2006, 2007, 2008, 2009, 2010,
*
* Andrei Herdt
* Florent Lamiraux
@@ -20,11 +20,11 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
- * Research carried out within the scope of the
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
-/** Object to perform preview control on a cart model.
+/** Object to perform preview control on a cart model.
*/
#include <fstream>
@@ -48,7 +48,7 @@ PreviewControl::PreviewControl(SimplePluginManager *lSPM,
m_PreviewControlTime = 0.0;
m_Zc = 0.0;
m_SizeOfPreviewWindow = 0;
-
+
MAL_MATRIX_RESIZE(m_A,3,3);
MAL_MATRIX_RESIZE(m_B,3,1);
MAL_MATRIX_RESIZE(m_C,1,3);
@@ -58,11 +58,11 @@ PreviewControl::PreviewControl(SimplePluginManager *lSPM,
ODEBUG("Identification: " << this);
- std::string aMethodName[3] =
+ std::string aMethodName[3] =
{":samplingperiod",
":previewcontroltime",
":comheight"};
-
+
for(int i=0;i<3;i++)
{
if (!RegisterMethod(aMethodName[i]))
@@ -85,12 +85,12 @@ PreviewControl::~PreviewControl()
double PreviewControl::SamplingPeriod() const
{
- return m_SamplingPeriod;
+ return m_SamplingPeriod;
}
double PreviewControl::PreviewControlTime() const
-{
- return m_PreviewControlTime;
+{
+ return m_PreviewControlTime;
}
@@ -114,7 +114,7 @@ void PreviewControl::SetPreviewControlTime(double lPreviewControlTime)
{
if (m_PreviewControlTime != lPreviewControlTime)
m_Coherent = false;
-
+
m_PreviewControlTime = lPreviewControlTime;
if (m_AutoComputeWeights)
@@ -149,8 +149,8 @@ void PreviewControl::ReadPrecomputedFile(string aFileName)
aif >> m_Zc;
aif >> m_SamplingPeriod;
aif >> m_PreviewControlTime;
-
-
+
+
float r;
for(int i=0;i<3;i++)
{
@@ -161,7 +161,7 @@ void PreviewControl::ReadPrecomputedFile(string aFileName)
aif >> r;
m_Ks=r;
-
+
m_SizeOfPreviewWindow = (unsigned int)(m_PreviewControlTime/
m_SamplingPeriod);
MAL_MATRIX_RESIZE(m_F,m_SizeOfPreviewWindow,1);
@@ -185,14 +185,14 @@ void PreviewControl::ReadPrecomputedFile(string aFileName)
m_C(0,1) = 0.0;
m_C(0,2) = -m_Zc/9.81;
-
+
m_Coherent = true;
aif.close();
}
- else
+ else
cerr << "PreviewControl - Unable to open " << aFileName << endl;
-
+
}
void PreviewControl::ComputeOptimalWeights(unsigned int mode)
@@ -204,16 +204,16 @@ void PreviewControl::ComputeOptimalWeights(unsigned int mode)
m_A(0,0) = 1.0; m_A(0,1) = T; m_A(0,2) = T*T/2.0;
m_A(1,0) = 0.0; m_A(1,1) = 1.0; m_A(1,2) = T;
m_A(2,0) = 0.0; m_A(2,1) = 0.0; m_A(2,2) = 1.0;
-
+
m_B(0,0) = T*T*T/6.0;
m_B(1,0) = T*T/2.0;
m_B(2,0) = T;
-
+
m_C(0,0) = 1.0;
m_C(0,1) = 0.0;
m_C(0,2) = -m_Zc/9.81;
- ODEBUG(" m_Zc: " << m_Zc << " " << m_C(0,2));
-
+ ODEBUG(" m_Zc: " << m_Zc << " m_C(0,2)" << m_C(0,2));
+
MAL_MATRIX_TYPE( double) lF,lK;
double Q=0.0,R=0.0;
@@ -225,8 +225,8 @@ void PreviewControl::ComputeOptimalWeights(unsigned int mode)
return;
Nl = (int)(m_PreviewControlTime/T);
-
-
+
+
if (mode==OptimalControllerSolver::MODE_WITHOUT_INITIALPOS)
{
ODEBUG("COMPUTATION WITHOUT INITIALPOS !");
@@ -240,24 +240,24 @@ void PreviewControl::ComputeOptimalWeights(unsigned int mode)
MAL_MATRIX_DIM(bx,double,4,1);
MAL_MATRIX(tmpb,double);
MAL_MATRIX_DIM(cx,double,1,4);
-
+
tmpA = MAL_RET_A_by_B(m_C,m_A);
-
+
Ax(0,0)= 1.0;
for(int i=0;i<3;i++)
- {
- cx(0,i+1)=0.0;
- Ax(0,i+1) = tmpA(0,i);
- for(int j=0;j<3;j++)
- Ax(i+1,j+1) = m_A(i,j);
- }
-
+ {
+ cx(0,i+1)=0.0;
+ Ax(0,i+1) = tmpA(0,i);
+ for(int j=0;j<3;j++)
+ Ax(i+1,j+1) = m_A(i,j);
+ }
+
tmpb = MAL_RET_A_by_B(m_C,m_B);
bx(0,0) = tmpb(0,0);
for(int i=0;i<3;i++)
- {
- bx(i+1,0) = m_B(i,0);
- }
+ {
+ bx(i+1,0) = m_B(i,0);
+ }
cx(0,0) =1.0;
@@ -268,16 +268,16 @@ void PreviewControl::ComputeOptimalWeights(unsigned int mode)
ODEBUG("R:" << R);
ODEBUG("Nl:" << Nl);
anOCS = new PatternGeneratorJRL::OptimalControllerSolver(Ax,bx,cx,Q,R,Nl);
-
+
anOCS->ComputeWeights(OptimalControllerSolver::MODE_WITHOUT_INITIALPOS);
-
+
anOCS->GetF(m_F);
anOCS->GetK(lK);
-
+
m_Ks = lK(0,0);
for (int i=0;i<3;i++)
- m_Kx(0,i) = lK(0,i+1);
+ m_Kx(0,i) = lK(0,i+1);
delete anOCS;
}
@@ -287,17 +287,17 @@ void PreviewControl::ComputeOptimalWeights(unsigned int mode)
R = 1e-5;
ODEBUG("COMPUTATION WITH INITIALPOS !");
anOCS = new PatternGeneratorJRL::OptimalControllerSolver(m_A,m_B,m_C,Q,R,Nl);
-
+
anOCS->ComputeWeights(PatternGeneratorJRL::OptimalControllerSolver::MODE_WITH_INITIALPOS);
-
+
anOCS->GetF(m_F);
anOCS->GetK(lK);
-
+
m_Ks = lK(0,0);
for (int i=0;i<3;i++)
- m_Kx(0,i) = lK(0,i);
+ m_Kx(0,i) = lK(0,i);
delete anOCS;
@@ -309,7 +309,7 @@ void PreviewControl::ComputeOptimalWeights(unsigned int mode)
ODEBUG("m_Kx(0,0): " << m_Kx(0,0) << " " <<
"m_Kx(0,1): " << m_Kx(0,1) << " " <<
"m_Kx(0,2): " << m_Kx(0,2) );
-
+
ODEBUG("m_A" <<m_A);
ODEBUG("m_B" <<m_B);
ODEBUG("m_C" <<m_C);
@@ -317,11 +317,11 @@ void PreviewControl::ComputeOptimalWeights(unsigned int mode)
m_SizeOfPreviewWindow = (unsigned int)(m_PreviewControlTime/
m_SamplingPeriod);
MAL_MATRIX_RESIZE(m_F,m_SizeOfPreviewWindow,1);
-
+
m_Coherent = true;
}
-int PreviewControl::OneIterationOfPreview(MAL_MATRIX( &x, double),
+int PreviewControl::OneIterationOfPreview(MAL_MATRIX( &x, double),
MAL_MATRIX(& y, double),
double & sxzmp, double & syzmp,
deque<PatternGeneratorJRL::ZMPPosition> & ZMPPositions,
@@ -351,7 +351,7 @@ int PreviewControl::OneIterationOfPreview(MAL_MATRIX( &x, double),
for(unsigned int i=0;i<m_SizeOfPreviewWindow;i++)
uy += m_F(i,0)* ZMPPositions[lindex+i].py;
-
+
x = MAL_RET_A_by_B(m_A,x) + ux * m_B;
y = MAL_RET_A_by_B(m_A,y) + uy * m_B;
@@ -361,19 +361,19 @@ int PreviewControl::OneIterationOfPreview(MAL_MATRIX( &x, double),
zmpy2 = 0.0;
for(unsigned int i=0;i<MAL_MATRIX_NB_ROWS(y);i++)
zmpy2 += m_C(0,i)*y(i,0);
-
+
if (Simulation)
{
sxzmp += (ZMPPositions[lindex].px - zmpx2);
syzmp += (ZMPPositions[lindex].py - zmpy2);
}
-
-
+
+
return 0;
}
-int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
+int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
double & sxzmp,
deque<double> & ZMPPositions,
unsigned int lindex,
@@ -388,7 +388,7 @@ int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
// Compute the command.
r = MAL_RET_A_by_B(m_Kx,x);
ux = - r(0,0) + m_Ks * sxzmp ;
-
+
ODEBUG( "x: " << x);
ODEBUG(" ux phase 1: " << ux);
if(ZMPPositions.size()<m_SizeOfPreviewWindow)
@@ -397,22 +397,22 @@ int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
ZMPPositions.size()<< " " << m_SizeOfPreviewWindow);
exit(0);
}
-
+
for(unsigned int i=0;i<m_SizeOfPreviewWindow;i++)
ux += m_F(i,0)* ZMPPositions[lindex+i];
ODEBUG(" ux preview window phase: " << ux );
x = MAL_RET_A_by_B(m_A,x) + ux * m_B;
-
+
zmpx2 = 0.0;
for(unsigned int i=0;i<MAL_MATRIX_NB_ROWS(x);i++)
zmpx2 += m_C(0,i)*x(i,0);
-
+
if (Simulation)
{
sxzmp += (ZMPPositions[lindex] - zmpx2);
}
-
+
ODEBUG("zmpx: " << zmpx2 );
ODEBUG("sxzmp: " << sxzmp);
ODEBUG("********");
@@ -420,7 +420,7 @@ int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
return 0;
}
-int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
+int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
double & sxzmp,
vector<double> & ZMPPositions,
unsigned int lindex,
@@ -435,7 +435,7 @@ int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
// Compute the command.
r = MAL_RET_A_by_B(m_Kx,x);
ux = - r(0,0) + m_Ks * sxzmp ;
-
+
ODEBUG( "x: " << x);
ODEBUG(" ux phase 1: " << ux);
if(ZMPPositions.size()<m_SizeOfPreviewWindow)
@@ -444,7 +444,7 @@ int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
ZMPPositions.size()<< " " << m_SizeOfPreviewWindow);
exit(0);
}
-
+
int TestSize = ZMPPositions.size()-lindex - m_SizeOfPreviewWindow;
if (TestSize>=0)
@@ -454,8 +454,8 @@ int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
}
else
{
- ODEBUG("Case where TestSize<0 (lindex:" <<lindex <<
- " , ZMPPositions.size(): " << ZMPPositions.size() <<
+ ODEBUG("Case where TestSize<0 (lindex:" <<lindex <<
+ " , ZMPPositions.size(): " << ZMPPositions.size() <<
" , m_SizeOfPreviewWindow: " << m_SizeOfPreviewWindow);
for(unsigned int i=lindex;i<ZMPPositions.size();i++)
ux += m_F(i,0)* ZMPPositions[i];
@@ -466,16 +466,16 @@ int PreviewControl::OneIterationOfPreview1D(MAL_MATRIX( &x, double),
}
ODEBUG(" ux preview window phase: " << ux );
x = MAL_RET_A_by_B(m_A,x) + ux * m_B;
-
+
zmpx2 = 0.0;
for(unsigned int i=0;i<MAL_MATRIX_NB_ROWS(x);i++)
zmpx2 += m_C(0,i)*x(i,0);
-
+
if (Simulation)
{
sxzmp += (ZMPPositions[lindex] - zmpx2);
}
-
+
ODEBUG("zmpx: " << zmpx2 );
ODEBUG("sxzmp: " << sxzmp);
ODEBUG("********");
@@ -489,17 +489,17 @@ void PreviewControl::print()
cout << "Zc: " << m_Zc <<endl;
cout << "Sampling Period: " << m_SamplingPeriod <<endl;
cout << "Preview control time window: "<<m_PreviewControlTime<<endl;
-
+
for(int i=0;i<3;i++)
cout << m_Kx(0,i) << " ";
cout << endl;
-
+
cout << "Ks "<< m_Ks << endl;
-
+
cout << "F:"<<endl;
for(unsigned int i=0;i<m_SizeOfPreviewWindow;i++)
cout << m_F(i,0) << endl;
-
+
}
void PreviewControl::CallMethod(std::string & Method, std::istringstream &strm)
{
@@ -533,7 +533,7 @@ void PreviewControl::CallMethod(std::string & Method, std::istringstream &strm)
}
}
else if (Method==":computeweightsofpreview")
- {
+ {
std::string aws;
if (strm.good())
{
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index f1024441..1b24359d 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -186,7 +186,7 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
true);
m_PC->SetPreviewControlTime (QP_T_*(QP_N_-1));
m_PC->SetSamplingPeriod (m_SamplingPeriod);
- m_PC->SetHeightOfCoM(0);
+ m_PC->SetHeightOfCoM(0.814);
// init of the debug files
ofstream aof;
@@ -194,22 +194,27 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
aof.open(aFileName.c_str(),ofstream::out);
// init of the buffer for the kajita's dynamic filter
+ // number of sample inside one iteration of the preview control
m_numberOfSample = (unsigned)(QP_T_/m_SamplingPeriod);
+ // size = numberOfIterationOfThePreviewControl * NumberOfSample + Margin
m_ZMPTraj_deq.resize( QP_N_ * m_numberOfSample + 50 );
m_COMTraj_deq.resize( QP_N_ * m_numberOfSample + 50 );
m_configurationTraj.resize( QP_N_ * m_numberOfSample );
m_velocityTraj.resize( QP_N_ * m_numberOfSample );
m_accelerationTraj.resize( QP_N_ * m_numberOfSample );
m_deltaZMPMBPositions.resize ( QP_N_ * m_numberOfSample );
- //m_LeftFootTraj_deq.resize(QP_N_ * m_numberOfSample + 50);
- //m_RightFootTraj_deq.resize(QP_N_ * m_numberOfSample + 50);
-
+ // Initialization of the configuration vectors
m_previousConfiguration = aHS->currentConfiguration() ;
m_previousVelocity = aHS->currentVelocity();
m_previousAcceleration = aHS->currentAcceleration();
+ // Configuration vectors at the end of the previewControl of Herdt
m_QP_T_Configuration = aHS->currentConfiguration();
m_QP_T_previousVelocity = aHS->currentVelocity();
m_QP_T_previousAcceleration = aHS->currentAcceleration();
+
+ m_once = true ;
+ m_dInitX = 0 ;
+ m_dInitY = 0 ;
}
@@ -418,6 +423,7 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
CoM_(CoM);
IntermedData_->CoM(CoM_());
+ // initialisation of a second object that allow the interpolation along 1.6s
CoM2_.SetComHeight(lStartingCOMState.z[0]);
CoM2_.InitializeSystem();
CoM2_(CoM);
@@ -511,31 +517,28 @@ ZMPVelocityReferencedQP::OnLine(double time,
if(Solution_.Fail>0)
Problem_.dump( time );
- static int iteration = 0;
+ static int iteration = 0 ;
+ if (iteration == 269)
+ {
+ Solution_.print(cout);
+ }
+
// INTERPOLATE THE NEXT COMPUTED COM STATE:
// ----------------------------------------
- unsigned currentIndex = FinalCOMTraj_deq.size();
- solution_t solution (Solution_), solution2 (Solution_) ;
- deque<FootAbsolutePosition> m_LeftFootTraj ;
- deque<FootAbsolutePosition> m_RightFootTraj ;
- support_state_t CurrentSupport, PreviousSupport ;
- CurrentSupport = PreviousSupport = solution.SupportStates_deq.front() ;
-
- for (unsigned i = 0 ; i < currentIndex ; i++)
+ m_currentIndex = FinalCOMTraj_deq.size();
+ m_solution = Solution_ ;
+ for (unsigned i = 0 ; i < m_currentIndex ; i++)
{
m_ZMPTraj_deq[i] = FinalZMPTraj_deq[i] ;
m_COMTraj_deq[i] = FinalCOMTraj_deq[i] ;
- //m_LeftFootTraj_deq[i] = FinalLeftFootTraj_deq[i] ;
- //m_RightFootTraj_deq[i] = FinalRightFootTraj_deq[i] ;
}
m_LeftFootTraj_deq = FinalLeftFootTraj_deq ;
m_RightFootTraj_deq = FinalRightFootTraj_deq ;
- int stateModified = 0 ;
for ( int i = 0 ; i < QP_N_ ; i++ )
{
- if(solution.SupportStates_deq.size() && solution.SupportStates_deq[0].NbStepsLeft == 0)
+ if(m_solution.SupportStates_deq.size() && m_solution.SupportStates_deq[0].NbStepsLeft == 0)
{
double jx = (FinalLeftFootTraj_deq[0].x + FinalRightFootTraj_deq[0].x)/2 - FinalCOMTraj_deq[0].x[0];
double jy = (FinalLeftFootTraj_deq[0].y + FinalRightFootTraj_deq[0].y)/2 - FinalCOMTraj_deq[0].y[0];
@@ -547,7 +550,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
{
CoM2_.setState(CoM_());
}
- CoM2_.Interpolation( m_COMTraj_deq, m_ZMPTraj_deq, currentIndex + i * m_numberOfSample,
+ CoM2_.Interpolation( m_COMTraj_deq, m_ZMPTraj_deq, m_currentIndex + i * m_numberOfSample,
jx, jy);
CoM2_.OneIteration( jx, jy );
}
@@ -558,37 +561,34 @@ ZMPVelocityReferencedQP::OnLine(double time,
{
CoM2_.setState(CoM_());
}
- CoM2_.Interpolation( m_COMTraj_deq, m_ZMPTraj_deq, currentIndex + i * m_numberOfSample,
- solution.Solution_vec[i], solution.Solution_vec[QP_N_+i] );
- CoM2_.OneIteration( solution.Solution_vec[i], solution.Solution_vec[QP_N_+i] );
+ CoM2_.Interpolation( m_COMTraj_deq, m_ZMPTraj_deq, m_currentIndex + i * m_numberOfSample,
+ m_solution.Solution_vec[i], m_solution.Solution_vec[QP_N_+i] );
+ CoM2_.OneIteration( m_solution.Solution_vec[i], m_solution.Solution_vec[QP_N_+i] );
}
}
- CoM_.setState(m_COMTraj_deq[m_numberOfSample + currentIndex - 1]);
// INTERPOLATE TRUNK ORIENTATION AND THE COMPUTED FOOT POSITIONS :
// ---------------------------------------------------------------
- OrientPrw_->interpolate_trunk_orientation( time, currentIndex,
- m_SamplingPeriod, solution.SupportStates_deq,
+ OrientPrw_->interpolate_trunk_orientation( time, m_currentIndex,
+ m_SamplingPeriod, m_solution.SupportStates_deq,
m_COMTraj_deq );
COMState aCoMState = OrientPrw_->CurrentTrunkState();
- solution.SupportStates_deq.pop_front();
+ Robot_->generate_trajectories( time, m_solution,
+ m_solution.SupportStates_deq, m_solution.SupportOrientations_deq,
+ m_LeftFootTraj_deq, m_RightFootTraj_deq );
+ cout << "support X , Y = " << m_solution.SupportStates_deq.front().X << ", " << m_solution.SupportStates_deq.front().Y << endl ;
+ m_solution.SupportStates_deq.pop_front();
for ( int i = 1 ; i < QP_N_ ; i++ ){
- OrientPrw_->interpolate_trunk_orientation( time + i * QP_T_, currentIndex + i * m_numberOfSample,
- m_SamplingPeriod, solution.SupportStates_deq,
+ OrientPrw_->interpolate_trunk_orientation( time + i * QP_T_, m_currentIndex + i * m_numberOfSample,
+ m_SamplingPeriod, m_solution.SupportStates_deq,
m_COMTraj_deq );
- solution.SupportStates_deq.pop_front();
- }
- OrientPrw_->CurrentTrunkState(aCoMState);
-
- int compteur = 0 ;
- support_state_t currentSupport ;
- for ( int i = 0 ; i < QP_N_ ; i++ ){
- currentSupport = solution2.SupportStates_deq.front();
- Robot_->generate_trajectories( time + i * QP_T_, solution2,
- solution2.SupportStates_deq, solution2.SupportOrientations_deq,
+ Robot_->generate_trajectories( time + i * QP_T_, m_solution,
+ m_solution.SupportStates_deq, m_solution.SupportOrientations_deq,
m_LeftFootTraj_deq, m_RightFootTraj_deq );
- solution2.SupportStates_deq.pop_front();
+ cout << "support X , Y = " << m_solution.SupportStates_deq.front().X << ", " << m_solution.SupportStates_deq.front().Y << endl ;
+ m_solution.SupportStates_deq.pop_front();
}
+ OrientPrw_->CurrentTrunkState(aCoMState);
/// \brief Debug Purpose
/// --------------------
@@ -611,7 +611,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
- for(unsigned int i = 0 ; i < currentIndex + QP_N_ * m_numberOfSample ; i++){
+ for(unsigned int i = 0 ; i < m_currentIndex + QP_N_ * m_numberOfSample ; i++){
aof << m_COMTraj_deq[i].roll[0] << " " // 1
<< m_COMTraj_deq[i].pitch[0] << " " // 2
<< m_COMTraj_deq[i].yaw[0] << " " // 3
@@ -625,27 +625,31 @@ ZMPVelocityReferencedQP::OnLine(double time,
<< m_RightFootTraj_deq[i].x << " " // 11
<< m_LeftFootTraj_deq[i].y << " " // 12
<< m_RightFootTraj_deq[i].y << " " // 13
+ << m_LeftFootTraj_deq[i].z << " " // 14
+ << m_RightFootTraj_deq[i].z << " " // 15
<< endl ;
}
+ aof.close();
+ FinalZMPTraj_deq.resize( m_numberOfSample + m_currentIndex );
+ for (unsigned int i = m_currentIndex ; i < FinalZMPTraj_deq.size() ; i++ )
+ FinalZMPTraj_deq[i] = m_ZMPTraj_deq[i] ;
- aof.close();
// DYNAMIC FILTER
// --------------
- //DynamicFilter( m_ZMPTraj_deq, m_COMTraj_deq, m_LeftFootTraj_deq, m_RightFootTraj_deq, currentIndex );
+ DynamicFilter( m_ZMPTraj_deq, m_COMTraj_deq, m_LeftFootTraj_deq, m_RightFootTraj_deq, m_currentIndex );
+ CoM_.setState(m_COMTraj_deq[m_numberOfSample + m_currentIndex - 1]);
// RECOPIE DU BUFFER
// -----------------
- FinalCOMTraj_deq.resize( m_numberOfSample + currentIndex );
- FinalZMPTraj_deq.resize( m_numberOfSample + currentIndex );
- FinalLeftFootTraj_deq.resize( m_numberOfSample + currentIndex );
- FinalRightFootTraj_deq.resize( m_numberOfSample + currentIndex );
+ FinalCOMTraj_deq.resize( m_numberOfSample + m_currentIndex );
+ FinalLeftFootTraj_deq.resize( m_numberOfSample + m_currentIndex );
+ FinalRightFootTraj_deq.resize( m_numberOfSample + m_currentIndex );
- for (unsigned int i = currentIndex ; i < FinalZMPTraj_deq.size() ; i++ )
+ for (unsigned int i = m_currentIndex ; i < FinalZMPTraj_deq.size() ; i++ )
{
- FinalZMPTraj_deq[i] = m_ZMPTraj_deq[i] ;
FinalCOMTraj_deq[i] = m_COMTraj_deq[i] ;
FinalLeftFootTraj_deq[i] = m_LeftFootTraj_deq[i] ;
FinalRightFootTraj_deq[i] = m_RightFootTraj_deq[i] ;
@@ -666,7 +670,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
- aof << iteration*0.005 << " " // 1
+ aof << iteration*0.1 << " " // 1
<< FinalLeftFootTraj_deq[0].theta *M_PI/180 << " " // 2
<< FinalRightFootTraj_deq[0].theta *M_PI/180 << " " // 3
<< FinalCOMTraj_deq[0].roll[0] << " " // 4
@@ -760,20 +764,10 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
unsigned currentIndex
)
{
- /// \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 );
-
const unsigned int N = m_numberOfSample * QP_N_ ;
- /// \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
+ // ------------------------------------------------------------------
for(unsigned int i = 0 ; i < N ; i++ ){
CallToComAndFootRealization(
FinalCOMTraj_deq[currentIndex+i],
@@ -785,94 +779,9 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
i);
}
- /// \brief Debug Purpose
- /// --------------------
- oss.str("TestHerdt2010DynamicArticulation");
- oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- ///----
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- for(unsigned int i = 0 ; i < N ; i++){
- for(unsigned int j = 0 ; j < m_configurationTraj[i].size() ; j++){
- aof << filterprecision( m_configurationTraj[i](j) ) << " " ; // 1
- }
- aof << endl ;
- }
- aof.close();
-
- if ( iteration == 0 ){
- oss.str("/tmp/walkfwd_herdt.pos");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- }
- ///----
- oss.str("/tmp/walkfwd_herdt.pos");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
-
- for(unsigned int j = 0 ; j < m_numberOfSample ; j++){
- aof << filterprecision( iteration * QP_T_ + j * m_SamplingPeriod ) << " " ; // 1
- for(unsigned int i = 6 ; i < m_configurationTraj[j].size() ; i++){
- aof << filterprecision( m_configurationTraj[j](i) ) << " " ; // 1
- }
- for(unsigned int i = 0 ; i < 10; i++){
- aof << 0.0 << " " ;
- }
- aof << endl ;
- }
- aof.close();
-
- if ( iteration == 0 ){
- oss.str("/tmp/walkfwd_herdt.hip");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- }
- ///----
- oss.str("/tmp/walkfwd_herdt.hip");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
-
- for(unsigned int j = 0 ; j < m_numberOfSample ; j++){
- aof << filterprecision( iteration * QP_T_ + j * m_SamplingPeriod ) << " " ; // 1
- aof << filterprecision( FinalCOMTraj_deq[currentIndex+j].roll[0] ) << " " ; // 1
- aof << filterprecision( FinalCOMTraj_deq[currentIndex+j].pitch[0] ) << " " ; // 1
- aof << filterprecision( FinalCOMTraj_deq[currentIndex+j].yaw[0] ) << " " ; // 1
- aof << endl ;
- }
- aof.close();
-
- /// \brief Debug Purpose
- /// --------------------
- oss.str("TestHerdt2010DynamicDZMP");
- oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
-
- ///----
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- /// \brief rnea, calculation of the multi body ZMP
- /// ----------------------------------------------
- Force_HRP2_14 f3 ;
- Force_HRP2_14 f4 ;
- Force_HRP2_14 f1 ;
- Force_HRP2_14 f2 ;
-
- static bool once = true ;
- static double dInitX(0), dInitY(0) ;
-
+ // \brief rnea, calculation of the multi body ZMP
+ // ----------------------------------------------
+ // Initialize the force,
for (unsigned int i = 0 ; i < N ; i++ ){
// Apply the RNEA to the metapod multibody and print the result in a log file.
for(unsigned int j = 0 ; j < m_configurationTraj[i].size() ; j++ )
@@ -884,51 +793,26 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
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);
- f2 = node.joint.f ;
- f1 = node.body.iX0.applyInv (node.joint.f);
- if (i==0){
- f3 = node.joint.f ;
- f4 = node.body.iX0.applyInv (node.joint.f);
+ m_force = node.body.iX0.applyInv (node.joint.f);
+ if (m_once){
+ m_dInitX = ZMPPositions[currentIndex].px - ( - m_force.n()[1] / m_force.f()[2] ) ;
+ m_dInitY = ZMPPositions[currentIndex].py - ( m_force.n()[0] / m_force.f()[2] ) ;
+ m_once = false ;
}
- if (once){
- dInitX = ZMPPositions[currentIndex].px - ( - f1.n()[1] / f1.f()[2] ) ;
- dInitY = ZMPPositions[currentIndex].py - ( f1.n()[0] / f1.f()[2] ) ;
- once = 0 ;
- }
- m_deltaZMPMBPositions[i].px = ZMPPositions[currentIndex+i].px - ( - f1.n()[1] / f1.f()[2] ) - dInitX ;
- m_deltaZMPMBPositions[i].py = ZMPPositions[currentIndex+i].py - ( f1.n()[0] / f1.f()[2] ) - dInitY ;
+ m_deltaZMPMBPositions[i].px = ZMPPositions[currentIndex+i].px - ( - m_force.n()[1] / m_force.f()[2] ) - m_dInitX ;
+ m_deltaZMPMBPositions[i].py = ZMPPositions[currentIndex+i].py - ( m_force.n()[0] / m_force.f()[2] ) - m_dInitY ;
m_deltaZMPMBPositions[i].pz = 0.0 ;
m_deltaZMPMBPositions[i].theta = 0.0 ;
m_deltaZMPMBPositions[i].time = m_CurrentTime + i * m_SamplingPeriod ;
m_deltaZMPMBPositions[i].stepType = ZMPPositions[currentIndex+i].stepType ;
-
- aof << filterprecision( m_deltaZMPMBPositions[i].px ) << " " // 1
- << filterprecision( m_deltaZMPMBPositions[i].py ) << " " // 2
- << filterprecision( ZMPPositions[currentIndex+i].px ) << " " // 3
- << filterprecision( ZMPPositions[currentIndex+i].py ) << " " // 4
- << filterprecision( f2.f()[0] ) << " " // 5
- << filterprecision( f2.f()[1] ) << " " // 6
- << filterprecision( f2.f()[2] ) << " " // 7
- << filterprecision( f2.n()[0] ) << " " // 8
- << filterprecision( f2.n()[1] ) << " " // 9
- << filterprecision( f2.n()[2] ) << " " // 10
-
- << filterprecision( f1.f()[0] ) << " " // 5
- << filterprecision( f1.f()[1] ) << " " // 6
- << filterprecision( f1.f()[2] ) << " " // 7
- << filterprecision( f1.n()[0] ) << " " // 8
- << filterprecision( f1.n()[1] ) << " " // 9
- << filterprecision( f1.n()[2] ) << " " // 10
- << endl ;
}
- aof.close();
/// Preview control on the ZMPMBs computed
/// --------------------------------------
//init of the Kajita preview control
m_PC->SetPreviewControlTime (QP_T_*(QP_N_-1));
m_PC->SetSamplingPeriod (m_SamplingPeriod);
- m_PC->SetHeightOfCoM(0);
+ m_PC->SetHeightOfCoM(0.814);
m_PC->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITH_INITIALPOS);
for(int j=0;j<3;j++)
{
@@ -952,69 +836,6 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
}
}
- /// \brief Debug Purpose
- /// --------------------
- oss.str("TestHerdt2010DynamicFilterBufferPC");
- oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- ///----
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- for(unsigned int i = 0 ; i < COMStateBuffer.size() ; i++){
- aof << filterprecision( COMStateBuffer[i].x[0] ) << " " // 2
- << filterprecision( COMStateBuffer[i].y[0] ) << " " // 3
- << endl ;
- }
- aof.close();
-
- /// \brief Debug Purpose
- /// --------------------
- if ( iteration == 0 )
- {
- oss.str("TestHerdt2010DynamicDZMP.dat");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- }
- ///----
- oss.str("TestHerdt2010DynamicDZMP.dat");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- aof << filterprecision( iteration*0.005 ) << " " // 1
- << filterprecision( f3.f()[0] ) << " " // 2
- << filterprecision( f3.f()[1] ) << " " // 3
- << filterprecision( f3.f()[2] ) << " " // 4
- << filterprecision( f3.n()[0] ) << " " // 5
- << filterprecision( f3.n()[1] ) << " " // 6
- << filterprecision( f3.n()[2] ) << " " // 7
-
- << filterprecision( f4.f()[0] ) << " " // 8
- << filterprecision( f4.f()[1] ) << " " // 9
- << filterprecision( f4.f()[2] ) << " " // 10
- << filterprecision( f4.n()[0] ) << " " // 11
- << filterprecision( f4.n()[1] ) << " " // 12
- << filterprecision( f4.n()[2] ) << " " // 13
-
- << filterprecision( ZMPPositions[currentIndex].px ) << " " // 14
- << filterprecision( ZMPPositions[currentIndex].py ) << " " // 15
-
- << filterprecision( COMStateBuffer[0].x[0] ) << " " // 16
- << filterprecision( COMStateBuffer[0].y[0] ) << " " // 17
-
- << filterprecision( FinalCOMTraj_deq[currentIndex].x[0] ) << " " // 18
- << filterprecision( FinalCOMTraj_deq[currentIndex].y[0] ) << " " // 19
-
- << filterprecision( ( - f4.n()[1] / f4.f()[2] ) + dInitX ) << " " // 18
- << filterprecision( ( f4.n()[0] / f4.f()[2] ) + dInitY ) << " " // 19
-
- << endl ;
- aof.close();
-
for (unsigned int i = 0 ; i < m_numberOfSample ; i++)
{
for(int j=0;j<3;j++)
@@ -1023,9 +844,6 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
FinalCOMTraj_deq[currentIndex+i].y[j] += COMStateBuffer[i].y[j] ;
}
}
-
-
- iteration++;
return 0;
}
@@ -1036,7 +854,7 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(COMState &acomp,
MAL_VECTOR_TYPE(double) &CurrentConfiguration,
MAL_VECTOR_TYPE(double) &CurrentVelocity,
MAL_VECTOR_TYPE(double) &CurrentAcceleration,
- int IterationNumber)
+ unsigned IterationNumber)
{
// New scheme for WPG v3.0
@@ -1083,58 +901,14 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(COMState &acomp,
aRightFootPosition(4) = aRightFAP.omega;
if (IterationNumber == 0){
- CurrentConfiguration = m_QP_T_Configuration ;
- CurrentVelocity = m_QP_T_previousVelocity ;
- CurrentAcceleration = m_QP_T_previousAcceleration ;
+ CurrentConfiguration = HDR_->currentConfiguration();
+ CurrentVelocity = HDR_->currentConfiguration();
+ CurrentAcceleration = HDR_->currentConfiguration();
}else{
CurrentConfiguration = m_previousConfiguration ;
CurrentVelocity = m_previousVelocity ;
CurrentAcceleration = m_previousAcceleration ;
}
- /// \brief Debug Purpose
- /// --------------------
- ofstream aof6;
- 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 );
- if (IterationNumber == 0)
- {
- ofstream aof6;
- string aFileName;
- oss.str("TestHerdt2010DynamicFilterArt2");
-
- oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
- aFileName = oss.str();
- aof6.open(aFileName.c_str(),ofstream::out);
- aof6.close();
-
- oss.str("TestHerdt2010COMFeet");
- oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
- aFileName = oss.str();
- aof6.open(aFileName.c_str(),ofstream::out);
- aof6.close();
- }
- oss.str("TestHerdt2010COMFeet");
- oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
- aFileName = oss.str();
- aof6.open(aFileName.c_str(),ofstream::app);
- aof6.precision(8);
- aof6.setf(ios::scientific, ios::floatfield);
- for(unsigned int i = 0 ; i < aCOMState.size() ; i++){
- aof6 << filterprecision( aCOMState(i) ) << " " ; // 1;
- }
- for(unsigned int i = 0 ; i < aLeftFootPosition.size() ; i++){
- aof6 << filterprecision( aLeftFootPosition(i) ) << " " ; // 1;
- }
- for(unsigned int i = 0 ; i < aRightFootPosition.size() ; i++){
- aof6 << filterprecision( aRightFootPosition(i) ) << " " ; // 1;
- }
- aof6 << endl ;
- aof6.close();
-
ComAndFootRealization_->ComputePostureForGivenCoMAndFeetPosture(aCOMState, aCOMSpeed, aCOMAcc,
aLeftFootPosition,
aRightFootPosition,
@@ -1143,14 +917,10 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(COMState &acomp,
CurrentAcceleration,
IterationNumber,
0);
-
- if (IterationNumber==(int)m_numberOfSample*QP_N_-1)
- iteration++;
-
if(IterationNumber == m_numberOfSample-1 ){
- m_QP_T_Configuration = CurrentConfiguration ;
- m_QP_T_previousVelocity = CurrentVelocity ;
- m_QP_T_previousAcceleration = CurrentAcceleration ;
+ HDR_->currentConfiguration(CurrentConfiguration);
+ HDR_->currentConfiguration(CurrentVelocity);
+ HDR_->currentConfiguration(CurrentAcceleration);
}else{
m_previousConfiguration = CurrentConfiguration ;
m_previousVelocity = CurrentVelocity ;
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index f98bdd7d..6acc45c7 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -237,6 +237,10 @@ namespace PatternGeneratorJRL
deque<FootAbsolutePosition> m_LeftFootTraj_deq ;
deque<FootAbsolutePosition> m_RightFootTraj_deq ;
+
+ unsigned m_currentIndex ;
+ solution_t m_solution ;
+
/// \brief Number of interpolated point computed during QP_T_ (27/02/2014 :0.1)
unsigned m_numberOfSample ;
@@ -251,6 +255,13 @@ namespace PatternGeneratorJRL
MAL_VECTOR_TYPE(double) m_QP_T_previousVelocity ;
MAL_VECTOR_TYPE(double) m_QP_T_previousAcceleration ;
+ /// \brief force acting the CoM of the robot expressed in the Euclidean Frame
+ Force_HRP2_14 m_force ;
+
+ /// \brief Used to compute once, the initial difference between the ZMP and the ZMPMB
+ bool m_once ;
+ double m_dInitX, m_dInitY ;
+
/// \brief Buffer comtaimimg the difference between the ZMP computed from the Herdt controler
///and the ZMP Multibody computed from the articular trajectory
std::deque<ZMPPosition> m_deltaZMPMBPositions ;
@@ -313,7 +324,7 @@ namespace PatternGeneratorJRL
MAL_VECTOR_TYPE(double) &CurrentConfiguration,
MAL_VECTOR_TYPE(double) &CurrentVelocity,
MAL_VECTOR_TYPE(double) &CurrentAcceleration,
- int IterationNumber
+ unsigned IterationNumber
);
};
}
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index 09b4ebe5..ce8bb2aa 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -229,6 +229,121 @@ private:
protected:
+ void fillInDebugFiles( )
+ {
+ if (m_DebugFGPI)
+ {
+ ofstream aof;
+ string aFileName;
+ aFileName = m_TestName;
+ aFileName += "TestFGPI.dat";
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision(m_OneStep.NbOfIt*0.005 ) << " " // 1
+ << filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " " // 2
+ << filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 3
+ << filterprecision(m_OneStep.finalCOMPosition.z[0] ) << " " // 4
+ << filterprecision(m_OneStep.finalCOMPosition.yaw ) << " " // 5
+ << filterprecision(m_OneStep.finalCOMPosition.x[1] ) << " " // 6
+ << filterprecision(m_OneStep.finalCOMPosition.y[1] ) << " " // 7
+ << filterprecision(m_OneStep.finalCOMPosition.z[1] ) << " " // 8
+ << filterprecision(m_OneStep.ZMPTarget(0) ) << " " // 9
+ << filterprecision(m_OneStep.ZMPTarget(1) ) << " " // 10
+ << filterprecision(m_OneStep.LeftFootPosition.x ) << " " // 11
+ << filterprecision(m_OneStep.LeftFootPosition.y ) << " " // 12
+ << filterprecision(m_OneStep.LeftFootPosition.z ) << " " // 13
+ << filterprecision(m_OneStep.LeftFootPosition.dx ) << " " // 14
+ << filterprecision(m_OneStep.LeftFootPosition.dy ) << " " // 15
+ << filterprecision(m_OneStep.LeftFootPosition.dz ) << " " // 16
+ << filterprecision(m_OneStep.LeftFootPosition.ddx ) << " " // 17
+ << filterprecision(m_OneStep.LeftFootPosition.ddy ) << " " // 18
+ << filterprecision(m_OneStep.LeftFootPosition.ddz ) << " " // 19
+ << filterprecision(m_OneStep.LeftFootPosition.theta*M_PI/180 ) << " " // 20
+ << filterprecision(m_OneStep.LeftFootPosition.omega ) << " " // 21
+ << filterprecision(m_OneStep.LeftFootPosition.omega2 ) << " " // 22
+ << filterprecision(m_OneStep.RightFootPosition.x ) << " " // 23
+ << filterprecision(m_OneStep.RightFootPosition.y ) << " " // 24
+ << filterprecision(m_OneStep.RightFootPosition.z ) << " " // 25
+ << filterprecision(m_OneStep.RightFootPosition.dx ) << " " // 26
+ << filterprecision(m_OneStep.RightFootPosition.dy ) << " " // 27
+ << filterprecision(m_OneStep.RightFootPosition.dz ) << " " // 28
+ << 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.omega ) << " " // 33
+ << filterprecision(m_OneStep.RightFootPosition.omega2 ) << " " // 34
+ << filterprecision(m_OneStep.ZMPTarget(0)*cos(m_CurrentConfiguration(5)) -
+ m_OneStep.ZMPTarget(1)*sin(m_CurrentConfiguration(5))
+ +m_CurrentConfiguration(0) ) << " " // 35
+ << filterprecision(m_OneStep.ZMPTarget(0)*sin(m_CurrentConfiguration(5)) +
+ m_OneStep.ZMPTarget(1)*cos(m_CurrentConfiguration(5))
+ +m_CurrentConfiguration(1) ) << " " // 36
+ << filterprecision(m_CurrentConfiguration(0) ) << " " // 37
+ << filterprecision(m_CurrentConfiguration(1) ) << " "; // 38
+ for (unsigned int i = 0 ; i < m_HDR->currentConfiguration().size() ; i++)
+ {
+ aof << filterprecision(m_HDR->currentConfiguration()(i)) << " " ; // 39 - 74
+ }
+ aof << endl;
+ aof.close();
+ }
+
+
+ /// \brief Debug Purpose
+ /// --------------------
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+ static int iteration = 0;
+
+ if ( iteration == 0 ){
+ oss.str("/tmp/walk_mnaveau.pos");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
+ ///----
+ oss.str("/tmp/walk_mnaveau.pos");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision( iteration * 0.1 ) << " " ; // 1
+ for(unsigned int i = 6 ; i < m_CurrentConfiguration.size() ; i++){
+ aof << filterprecision( m_CurrentConfiguration(i) ) << " " ; // 1
+ }
+ for(unsigned int i = 0 ; i < 10 ; i++){
+ aof << 0.0 << " " ;
+ }
+ aof << endl ;
+ aof.close();
+
+ if ( iteration == 0 ){
+ oss.str("/tmp/walk_mnaveau.hip");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
+ oss.str("/tmp/walk_mnaveau.hip");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for(unsigned int j = 0 ; j < 20 ; j++){
+ aof << filterprecision( iteration * 0.1 ) << " " ; // 1
+ aof << filterprecision( 0.0 ) << " " ; // 1
+ aof << filterprecision( 0.0 ) << " " ; // 1
+ aof << filterprecision( m_OneStep.finalCOMPosition.yaw ) << " " ; // 1
+ aof << endl ;
+ }
+ aof.close();
+
+
+ iteration++;
+ }
+
void SpecializedRobotConstructor( CjrlHumanoidDynamicRobot *& aHDR, CjrlHumanoidDynamicRobot *& aDebugHDR )
{
dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
@@ -521,7 +636,7 @@ protected:
localeventHandler_t Handler ;
};
- #define localNbOfEvents 5
+ #define localNbOfEvents 12
struct localEvent events [localNbOfEvents] =
{ {5*200,&TestHerdt2010::walkForward},
// {10*200,&TestHerdt2010::walkSidewards},
@@ -536,7 +651,7 @@ protected:
{10*200,&TestHerdt2010::startTurningLeft2},
{15*200,&TestHerdt2010::startTurningRight2},
{25*200,&TestHerdt2010::stop},
- {27.5*200,&TestHerdt2010::stopOnLineWalking}
+ {30*200,&TestHerdt2010::stopOnLineWalking}
};
// Test when triggering event.
@@ -552,7 +667,7 @@ protected:
void generateEventEmergencyStop()
{
- #define localNbOfEventsEMS 4
+ #define localNbOfEventsEMS 5
struct localEvent events [localNbOfEventsEMS] =
{ {5*200,&TestHerdt2010::startTurningLeft2},
{10*200,&TestHerdt2010::startTurningRight2},
diff --git a/tests/TestKajita2003.cpp b/tests/TestKajita2003.cpp
index 84644446..24862a9f 100644
--- a/tests/TestKajita2003.cpp
+++ b/tests/TestKajita2003.cpp
@@ -65,6 +65,121 @@ protected:
}
+ void fillInDebugFiles( )
+ {
+ if (m_DebugFGPI)
+ {
+ ofstream aof;
+ string aFileName;
+ aFileName = m_TestName;
+ aFileName += "TestFGPI.dat";
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision(m_OneStep.NbOfIt*0.005 ) << " " // 1
+ << filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " " // 2
+ << filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 3
+ << filterprecision(m_OneStep.finalCOMPosition.z[0] ) << " " // 4
+ << filterprecision(m_OneStep.finalCOMPosition.yaw ) << " " // 5
+ << filterprecision(m_OneStep.finalCOMPosition.x[1] ) << " " // 6
+ << filterprecision(m_OneStep.finalCOMPosition.y[1] ) << " " // 7
+ << filterprecision(m_OneStep.finalCOMPosition.z[1] ) << " " // 8
+ << filterprecision(m_OneStep.ZMPTarget(0) ) << " " // 9
+ << filterprecision(m_OneStep.ZMPTarget(1) ) << " " // 10
+ << filterprecision(m_OneStep.LeftFootPosition.x ) << " " // 11
+ << filterprecision(m_OneStep.LeftFootPosition.y ) << " " // 12
+ << filterprecision(m_OneStep.LeftFootPosition.z ) << " " // 13
+ << filterprecision(m_OneStep.LeftFootPosition.dx ) << " " // 14
+ << filterprecision(m_OneStep.LeftFootPosition.dy ) << " " // 15
+ << filterprecision(m_OneStep.LeftFootPosition.dz ) << " " // 16
+ << filterprecision(m_OneStep.LeftFootPosition.ddx ) << " " // 17
+ << filterprecision(m_OneStep.LeftFootPosition.ddy ) << " " // 18
+ << filterprecision(m_OneStep.LeftFootPosition.ddz ) << " " // 19
+ << filterprecision(m_OneStep.LeftFootPosition.theta*M_PI/180 ) << " " // 20
+ << filterprecision(m_OneStep.LeftFootPosition.omega ) << " " // 21
+ << filterprecision(m_OneStep.LeftFootPosition.omega2 ) << " " // 22
+ << filterprecision(m_OneStep.RightFootPosition.x ) << " " // 23
+ << filterprecision(m_OneStep.RightFootPosition.y ) << " " // 24
+ << filterprecision(m_OneStep.RightFootPosition.z ) << " " // 25
+ << filterprecision(m_OneStep.RightFootPosition.dx ) << " " // 26
+ << filterprecision(m_OneStep.RightFootPosition.dy ) << " " // 27
+ << filterprecision(m_OneStep.RightFootPosition.dz ) << " " // 28
+ << 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.omega ) << " " // 33
+ << filterprecision(m_OneStep.RightFootPosition.omega2 ) << " " // 34
+ << filterprecision(m_OneStep.ZMPTarget(0)*cos(m_CurrentConfiguration(5)) -
+ m_OneStep.ZMPTarget(1)*sin(m_CurrentConfiguration(5))
+ +m_CurrentConfiguration(0) ) << " " // 35
+ << filterprecision(m_OneStep.ZMPTarget(0)*sin(m_CurrentConfiguration(5)) +
+ m_OneStep.ZMPTarget(1)*cos(m_CurrentConfiguration(5))
+ +m_CurrentConfiguration(1) ) << " " // 36
+ << filterprecision(m_CurrentConfiguration(0) ) << " " // 37
+ << filterprecision(m_CurrentConfiguration(1) ) << " "; // 38
+ for (unsigned int i = 0 ; i < m_HDR->currentConfiguration().size() ; i++)
+ {
+ aof << filterprecision(m_HDR->currentConfiguration()(i)) << " " ; // 39 - 74
+ }
+ aof << endl;
+ aof.close();
+ }
+
+
+ /// \brief Debug Purpose
+ /// --------------------
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+ static int iteration = 0;
+
+ if ( iteration == 0 ){
+ oss.str("/tmp/walk_Kajita.pos");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
+ ///----
+ oss.str("/tmp/walk_Kajita.pos");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision( iteration * 0.1 ) << " " ; // 1
+ for(unsigned int i = 6 ; i < m_CurrentConfiguration.size() ; i++){
+ aof << filterprecision( m_CurrentConfiguration(i) ) << " " ; // 1
+ }
+ for(unsigned int i = 0 ; i < 10 ; i++){
+ aof << 0.0 << " " ;
+ }
+ aof << endl ;
+ aof.close();
+
+ if ( iteration == 0 ){
+ oss.str("/tmp/walk_Kajita.hip");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
+ oss.str("/tmp/walk_Kajita.hip");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for(unsigned int j = 0 ; j < m_numberOfSample ; j++){
+ aof << filterprecision( iteration * 0.5 ) << " " ; // 1
+ aof << filterprecision( 0.0 ) << " " ; // 1
+ aof << filterprecision( 0.0 ) << " " ; // 1
+ aof << filterprecision( m_OneStep.finalCOMPosition.yaw ) << " " ; // 1
+ aof << endl ;
+ }
+ aof.close();
+
+
+ iteration++;
+ }
+
void TurningOnTheCircle(PatternGeneratorInterface &aPGI)
{
CommonInitialization(aPGI);
diff --git a/tests/TestObject.cpp b/tests/TestObject.cpp
index 67806b8b..b5113783 100644
--- a/tests/TestObject.cpp
+++ b/tests/TestObject.cpp
@@ -360,7 +360,7 @@ namespace PatternGeneratorJRL
<< filterprecision(m_OneStep.LeftFootPosition.ddx ) << " " // 17
<< filterprecision(m_OneStep.LeftFootPosition.ddy ) << " " // 18
<< filterprecision(m_OneStep.LeftFootPosition.ddz ) << " " // 19
- << filterprecision(m_OneStep.LeftFootPosition.theta ) << " " // 20
+ << filterprecision(m_OneStep.LeftFootPosition.theta*M_PI/180 ) << " " // 20
<< filterprecision(m_OneStep.LeftFootPosition.omega ) << " " // 21
<< filterprecision(m_OneStep.LeftFootPosition.omega2 ) << " " // 22
<< filterprecision(m_OneStep.RightFootPosition.x ) << " " // 23
@@ -372,7 +372,7 @@ namespace PatternGeneratorJRL
<< filterprecision(m_OneStep.RightFootPosition.ddx ) << " " // 29
<< filterprecision(m_OneStep.RightFootPosition.ddy ) << " " // 30
<< filterprecision(m_OneStep.RightFootPosition.ddz ) << " " // 31
- << filterprecision(m_OneStep.RightFootPosition.theta ) << " " // 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)) -
@@ -383,51 +383,16 @@ namespace PatternGeneratorJRL
+m_CurrentConfiguration(1) ) << " " // 36
<< filterprecision(m_CurrentConfiguration(0) ) << " " // 37
<< filterprecision(m_CurrentConfiguration(1) ) << " "; // 38
- for (unsigned int i = 0 ; i < m_CurrentConfiguration.size() ; i++)
+ for (unsigned int i = 0 ; i < m_HDR->currentConfiguration().size() ; i++)
{
- aof << filterprecision(m_CurrentConfiguration(i)) << " " ; // 39 - 74
+ aof << filterprecision(m_HDR->currentConfiguration()(i)) << " " ; // 39 - 74
}
aof << endl;
aof.close();
}
+ }
- /// \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 );
-
-
- if ( iteration == 0 ){
- oss.str("/tmp/walkfwd_kajita.pos");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- }
- ///----
- oss.str("/tmp/walkfwd_kajita.pos");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- aof << filterprecision( iteration * 0.005 ) << " " ; // 1
- for(unsigned int i = 6 ; i < m_CurrentConfiguration.size() ; i++){
- aof << filterprecision( m_CurrentConfiguration(i) ) << " " ; // 1
- }
- for(unsigned int i = 0 ; i < 10 ; i++){
- aof << 0.0 << " " ;
- }
- aof << endl ;
- aof.close();
-
- iteration++;
-
- }
bool TestObject::compareDebugFiles( )
--
GitLab