From 9112d11fc45df06e6ec428306862c9d8974e9b69 Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Fri, 28 Feb 2014 19:36:09 +0100
Subject: [PATCH] Writting down the articulation configurations for openHRP
---
.../CoMAndFootOnlyStrategy.cpp | 16 +++--
.../ZMPVelocityReferencedQP.cpp | 44 +++++++++---
.../ZMPVelocityReferencedQP.hh | 2 +-
tests/TestHerdt2010DynamicFilter.cpp | 4 +-
tests/TestObject.cpp | 68 +++++++++----------
5 files changed, 81 insertions(+), 53 deletions(-)
diff --git a/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp b/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp
index 7c16adeb..40dba95a 100644
--- a/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp
+++ b/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp
@@ -138,20 +138,22 @@ int CoMAndFootOnlyStrategy::EvaluateStartingState(MAL_VECTOR(&,double) BodyAngle
lStartingCOMState(2) = aStartingCOMState.z[0];
std::vector<ComAndFootRealization *>::iterator itCFR ;
- for (itCFR = m_ComAndFootRealization.begin() ; itCFR == m_ComAndFootRealization.end() ; ++itCFR )
+ for (itCFR = m_ComAndFootRealization.begin() ; itCFR != m_ComAndFootRealization.end() ; ++itCFR )
{
(*itCFR)->InitializationCoM(BodyAngles,lStartingCOMState,
lStartingWaistPose,
InitLeftFootPosition, InitRightFootPosition);
- }
- ODEBUG("EvaluateStartingCOM: m_StartingCOMState: " << lStartingCOMState);
- aStartingCOMState.x[0] = lStartingCOMState(0);
- aStartingCOMState.y[0] = lStartingCOMState(1);
- aStartingCOMState.z[0] = lStartingCOMState(2);
- aStartingZMPPosition= (*m_ComAndFootRealization.begin())->GetCOGInitialAnkles();
+ ODEBUG("EvaluateStartingCOM: m_StartingCOMState: " << lStartingCOMState);
+ aStartingCOMState.x[0] = lStartingCOMState(0);
+ aStartingCOMState.y[0] = lStartingCOMState(1);
+ aStartingCOMState.z[0] = lStartingCOMState(2);
+ aStartingZMPPosition= (*itCFR)->GetCOGInitialAnkles();
+
+ }
// cerr << "YOU SHOULD INITIALIZE PROPERLY aStartingZMPosition in CoMAndFootOnlyStrategy::EvaluateStartingState" <<endl;
+
return 0;
}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 62f021f4..bc850aca 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -517,6 +517,9 @@ ZMPVelocityReferencedQP::OnLine(double time,
if(Solution_.Fail>0)
Problem_.dump( time );
+ static int iteration = 0;
+ if (iteration == 51)
+ iteration = 51;
// INTERPOLATE THE NEXT COMPUTED COM STATE:
// ----------------------------------------
unsigned currentIndex = FinalCOMTraj_deq.size();
@@ -566,6 +569,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
CoM2_.OneIteration( Solution_.Solution_vec[i], Solution_.Solution_vec[QP_N_+i] );
}
+
// INTERPOLATE TRUNK ORIENTATION:
// ------------------------------
OrientPrw_->interpolate_trunk_orientation( time + i * QP_T_, currentIndex + i * m_numberOfSample,
@@ -600,7 +604,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
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 );
@@ -636,7 +640,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
// 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, currentIndex );
CoM_.setState(m_COMTraj_deq[m_numberOfSample + currentIndex - 1]);
@@ -774,8 +778,6 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
}
aof.close();
-
-
if ( iteration == 0 ){
oss.str("/tmp/walkfwd_herdt.pos");
aFileName = oss.str();
@@ -788,18 +790,40 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
- aof << filterprecision( iteration * m_SamplingPeriod ) << " " ; // 1
- for(unsigned int j = 6 ; j < m_numberOfSample ; j++){
+
+ 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 ;
}
- for(unsigned int i = 0 ; i < 10 ; i++){
- aof << 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 rnea, calculation of the multi body ZMP
// /// ----------------------------------------------
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index ace4550d..3a94aef9 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -253,7 +253,7 @@ namespace PatternGeneratorJRL
MAL_VECTOR_TYPE(double) m_QP_T_previousVelocity ;
MAL_VECTOR_TYPE(double) m_QP_T_previousAcceleration ;
- /// \brief Buffer for the torques computed by RNEA algorithme from Featherstone
+ /// \brief Buffer for the torques computed by RNEA algorithm from Featherstone
vector < Robot_Model::confVector,Eigen::aligned_allocator<Robot_Model::confVector> > m_allTorques ;
/// \brief Buffer comtaimimg the difference between the ZMP computed from the Herdt controler\
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index 241f90ce..2e2103d2 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -517,7 +517,7 @@ protected:
struct localEvent events [localNbOfEvents] =
{ {5*200,&TestHerdt2010::walkForward},
// {10*200,&TestHerdt2010::walkSidewards},
- {15*200,&TestHerdt2010::startTurningRightOnSpot},
+ // {15*200,&TestHerdt2010::startTurningRightOnSpot},
// {35*200,&TestHerdt2010::walkForward},
// {45*200,&TestHerdt2010::startTurningLeftOnSpot},
// {55*200,&TestHerdt2010::walkForward},
@@ -525,6 +525,8 @@ protected:
// {75*200,&TestHerdt2010::walkForward},
// {85*200,&TestHerdt2010::startTurningLeft},
// {95*200,&TestHerdt2010::startTurningRight},
+ {10*200,&TestHerdt2010::startTurningLeft2},
+ {15*200,&TestHerdt2010::startTurningRight2},
{25*200,&TestHerdt2010::stop},
{27.5*200,&TestHerdt2010::stopOnLineWalking}
};
diff --git a/tests/TestObject.cpp b/tests/TestObject.cpp
index 3e3aefd0..ec8c39d4 100644
--- a/tests/TestObject.cpp
+++ b/tests/TestObject.cpp
@@ -392,40 +392,40 @@ namespace PatternGeneratorJRL
}
-// /// \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++;
+ /// \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++;
}
--
GitLab