From 280d11a14b87790708993c531d616a7198ab151c Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Wed, 26 Nov 2014 16:32:49 +0100
Subject: [PATCH] establish the dynamic filter for the Morisawa's offline PG
---
.../AnalyticalMorisawaCompact.cpp | 239 +------
.../DynamicFilter.cpp | 39 +-
.../DynamicFilter.hh | 36 +-
.../ZMPVelocityReferencedQP.cpp | 2 +-
tests/TestHirukawa2007.cpp | 590 ++++++++++++++++++
tests/TestInverseKinematics.cpp | 4 +-
tests/TestMorisawa2007.cpp | 2 +-
7 files changed, 640 insertions(+), 272 deletions(-)
create mode 100644 tests/TestHirukawa2007.cpp
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index 7627bf04..d7b34a98 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -563,7 +563,6 @@ computing the analytical trajectories. */
/*! Set the current time reference for the analytical trajectory. */
double TimeShift = m_Tsingle*2;
- //double TimeShift = m_Tsingle;
m_AbsoluteTimeReference = m_CurrentTime-TimeShift;
m_AnalyticalZMPCoGTrajectoryX->SetAbsoluteTimeReference(m_AbsoluteTimeReference);
m_AnalyticalZMPCoGTrajectoryY->SetAbsoluteTimeReference(m_AbsoluteTimeReference);
@@ -572,12 +571,13 @@ computing the analytical trajectories. */
/*! Compute the total size of the array related to the steps. */
FillQueues(m_CurrentTime,m_CurrentTime+m_PreviewControlTime-TimeShift,
ZMPPositions, COMStates,LeftFootAbsolutePositions, RightFootAbsolutePositions);
- deque<COMState> filteredCoM = COMStates ;
+ //deque<COMState> filteredCoM = COMStates ;
+
+ /*! initialize the dynamic filter */
unsigned int n = COMStates.size();
double KajitaPCpreviewWindow = 1.6 ;
- m_kajitaDynamicFilter->init( m_CurrentTime,
- m_SamplingPeriod,
+ m_kajitaDynamicFilter->init( m_SamplingPeriod,
m_SamplingPeriod,
(double)(n+1)*m_SamplingPeriod,
KajitaPCpreviewWindow,
@@ -585,6 +585,7 @@ computing the analytical trajectories. */
InitLeftFootAbsolutePosition,
lStartingCOMState );
+ /*! Set the upper body trajectory */
CjrlHumanoidDynamicRobot * aHDR = m_kajitaDynamicFilter->
getComAndFootRealization()->getHumanoidDynamicRobot();
MAL_VECTOR_TYPE(double) UpperConfig = aHDR->currentConfiguration() ;
@@ -635,8 +636,6 @@ computing the analytical trajectories. */
UpperAcc(i)=0.0;
}
-
-
m_kajitaDynamicFilter->setRobotUpperPart(UpperConfig,UpperVel,UpperAcc);
/*! Add "KajitaPCpreviewWindow" second to the buffers for fitering */
@@ -651,236 +650,34 @@ computing the analytical trajectories. */
LeftFootAbsolutePositions.push_back(lastLF);
RightFootAbsolutePositions.push_back(lastRF);
}
- unsigned int N = ZMPPositions.size();
- int stage0 = 0 ;
- int stage1 = 1 ;
- vector <vector<double> > ZMPMB (N , vector<double> (2,0.0)) ;
- for (unsigned int i = 0 ; i < N ; ++i)
- {
- m_kajitaDynamicFilter->ComputeZMPMB( m_SamplingPeriod, COMStates[i],
- LeftFootAbsolutePositions[i], RightFootAbsolutePositions[i],
- ZMPMB[i] , stage0 , i);
- }
- m_kajitaDynamicFilter->getClock()->Display();
- deque<ZMPPosition> inputdeltaZMP_deq(N) ;
deque<COMState> outputDeltaCOMTraj_deq ;
- for (unsigned int i = 0 ; i < N ; ++i)
- {
- inputdeltaZMP_deq[i].px = ZMPPositions[i].px - ZMPMB[i][0] ;
- inputdeltaZMP_deq[i].py = ZMPPositions[i].py - ZMPMB[i][1] ;
- inputdeltaZMP_deq[i].pz = 0.0 ;
- inputdeltaZMP_deq[i].theta = 0.0 ;
- inputdeltaZMP_deq[i].time = m_CurrentTime + i * m_SamplingPeriod ;
- inputdeltaZMP_deq[i].stepType = ZMPPositions[i].stepType ;
- }
- m_kajitaDynamicFilter->OptimalControl(inputdeltaZMP_deq,outputDeltaCOMTraj_deq) ;
+ m_kajitaDynamicFilter->OffLinefilter(
+ m_CurrentTime,
+ COMStates,
+ ZMPPositions,
+ LeftFootAbsolutePositions,
+ RightFootAbsolutePositions,
+ vector< MAL_VECTOR_TYPE(double) > (1,UpperConfig),
+ vector< MAL_VECTOR_TYPE(double) > (1,UpperConfig),
+ vector< MAL_VECTOR_TYPE(double) > (1,UpperConfig),
+ outputDeltaCOMTraj_deq);
vector <vector<double> > filteredZMPMB (n , vector<double> (2,0.0)) ;
for (unsigned int i = 0 ; i < n ; ++i)
{
for(int j=0;j<3;j++)
{
- filteredCoM[i].x[j] += outputDeltaCOMTraj_deq[i].x[j] ;
- filteredCoM[i].y[j] += outputDeltaCOMTraj_deq[i].y[j] ;
- COMStates[i].x[j] += outputDeltaCOMTraj_deq[i].x[j] ;
- COMStates[i].y[j] += outputDeltaCOMTraj_deq[i].y[j] ;
+ COMStates[i].x[j] += outputDeltaCOMTraj_deq[i].x[j] ;
+ COMStates[i].y[j] += outputDeltaCOMTraj_deq[i].y[j] ;
}
- m_kajitaDynamicFilter->ComputeZMPMB(m_SamplingPeriod, filteredCoM[i],
- LeftFootAbsolutePositions[i], RightFootAbsolutePositions[i],
- filteredZMPMB[i] , stage1, i);
}
- cout << "COMStates.size() = " << COMStates.size() << endl ;
- cout << "Buffer.size() = " << inputdeltaZMP_deq.size() << endl ;
- cout << "outputDeltaCOMTraj_deq.size() = " << outputDeltaCOMTraj_deq.size() << endl ;
-
m_UpperTimeLimitToUpdateStacks = m_CurrentTime;
for(int i=0;i<m_NumberOfIntervals;i++)
{
m_UpperTimeLimitToUpdateStacks += m_DeltaTj[i];
}
-//
-//
-// /// \brief Debug Purpose
-// /// --------------------
-// ifstream iof;
-// string aFileName;
-// aFileName = "/home/mnaveau/devel/HRP2Log/ClimbingWithTools-11072014-01-astate.log" ;
-// iof.open(aFileName.c_str(),std::ifstream::in);
-// string entete;
-// getline(iof,entete);
-// vector <vector <double> > Datas (4000,vector <double>(176));
-// for(unsigned int i = 0 ; i < 4000 ; ++i)
-// {
-// for (unsigned int j = 0 ; j < 176 ; ++j )
-// {
-// iof >> Datas[i][j] ;
-// }
-// }
-//
-// vector < MAL_VECTOR_TYPE(double) > POS (4000);
-// vector < MAL_VECTOR_TYPE(double) > VIT (4000);
-// vector < MAL_VECTOR_TYPE(double) > ACC (4000);
-// for(unsigned int i = 0 ; i < 4000 ; ++i)
-// {
-// MAL_VECTOR_RESIZE(POS[i], 36);
-// MAL_VECTOR_RESIZE(VIT[i], 36);
-// MAL_VECTOR_RESIZE(ACC[i], 36);
-// }
-//
-// for (unsigned int j = 0 ; j < 6 ; ++j )
-// {
-// POS[0](j+158) = Datas[i][j] ;
-// POS[1](j+158) = Datas[i][j] ;
-// }
-// for (unsigned int j = 0 ; j < 30 ; ++j )
-// {
-// POS[0](j+6) = Datas[i][j] ;
-// POS[1](j+6) = Datas[i][j] ;
-// }
-//
-// for(unsigned int i = 2 ; i < 4000 ; ++i)
-// {
-// for (unsigned int j = 0 ; j < 30 ; ++j )
-// {
-// m_CurrentConfiguration = Datas[i][j] ;
-// }
-// }
-//
-//
- double ecartMax_ZMP_ZMPMB=0.0;
- double ecartMax_ZMP_ZMPcorrected=0.0;
- double ecartMoy_ZMP_ZMPMB=0.0;
- double ecartMoy_ZMP_ZMPcorrected=0.0;
-
- for (unsigned int i = 0 ; i < n ; ++i )
- {
- double ecartZMP_ZMPMB = 0 ;
- double ecartZMP_ZMPcorrected = 0 ;
- ecartZMP_ZMPMB = (ZMPPositions[i].px - ZMPMB[i][0])*(ZMPPositions[i].px - ZMPMB[i][0])+
- (ZMPPositions[i].py - ZMPMB[i][1])*(ZMPPositions[i].py - ZMPMB[i][1]);
-
- ecartZMP_ZMPcorrected = (ZMPPositions[i].px - filteredZMPMB[i][0])*
- (ZMPPositions[i].px - filteredZMPMB[i][0])
- +
- (ZMPPositions[i].py - filteredZMPMB[i][1])*
- (ZMPPositions[i].py - filteredZMPMB[i][1]);
- ecartZMP_ZMPMB = sqrt(ecartZMP_ZMPMB);
- ecartZMP_ZMPcorrected = sqrt(ecartZMP_ZMPcorrected);
- if(ecartZMP_ZMPMB > ecartMax_ZMP_ZMPMB)
- {
- ecartMax_ZMP_ZMPMB = ecartZMP_ZMPMB ;
- }
- if(ecartZMP_ZMPcorrected > ecartMax_ZMP_ZMPcorrected)
- {
- ecartMax_ZMP_ZMPcorrected = ecartZMP_ZMPcorrected ;
- }
- ecartMoy_ZMP_ZMPMB += ecartZMP_ZMPMB ;
- ecartMoy_ZMP_ZMPcorrected += ecartZMP_ZMPcorrected ;
- }
- ecartMoy_ZMP_ZMPMB = ecartMoy_ZMP_ZMPMB/n ;
- ecartMoy_ZMP_ZMPcorrected = ecartMoy_ZMP_ZMPcorrected/n ;
-
- cout << "ecartMax_ZMP_ZMPMB = " << ecartMax_ZMP_ZMPMB << endl ;
- cout << "ecartMax_ZMP_ZMPcorrected = " << ecartMax_ZMP_ZMPcorrected << endl ;
- cout << "ecartMoy_ZMP_ZMPMB = " << ecartMoy_ZMP_ZMPMB << endl ;
- cout << "ecartMoy_ZMP_ZMPcorrected = " << ecartMoy_ZMP_ZMPcorrected << endl ;
-
- for (unsigned int i = 0 ; i < KajitaPCpreviewWindow/m_SamplingPeriod ; ++i)
- {
- ZMPPositions.pop_back();
- COMStates.pop_back();
- LeftFootAbsolutePositions.pop_back();
- RightFootAbsolutePositions.pop_back();
- }
-
- /// \brief Debug Purpose
- /// --------------------
- ofstream aof;
- string aFileName;
- ostringstream oss(std::ostringstream::ate);
- static int iteration = 0;
- /// \brief Debug Purpose
- /// --------------------
- oss.str("MorisawaData.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 )
- {
- aof << i*m_SamplingPeriod << " " // 1
- << COMStates[i].x[0] << " " // 2
- << COMStates[i].x[1] << " " // 3
- << COMStates[i].x[2] << " " // 4
- << COMStates[i].y[0] << " " // 5
- << COMStates[i].y[1] << " " // 6
- << COMStates[i].y[2] << " " // 7
- << COMStates[i].z[0] << " " // 8
- << COMStates[i].z[1] << " " // 9
- << COMStates[i].z[2] << " " // 10
- << COMStates[i].roll[0] << " " // 11
- << COMStates[i].roll[1] << " " // 12
- << COMStates[i].roll[2] << " " // 13
- << COMStates[i].pitch[0] << " " // 14
- << COMStates[i].pitch[1] << " " // 15
- << COMStates[i].pitch[2] << " " // 16
- << COMStates[i].yaw[0] << " " // 17
- << COMStates[i].yaw[1] << " " // 18
- << COMStates[i].yaw[2] << " " // 19
- << ZMPPositions[i].px << " " // 20
- << ZMPPositions[i].py << " " // 21
- << ZMPMB[i][0] << " " // 22
- << ZMPMB[i][1] << " " // 23
- << filteredZMPMB[i][0] << " " // 24
- << filteredZMPMB[i][1] << " " // 25
- << inputdeltaZMP_deq[i].px << " " // 26
- << inputdeltaZMP_deq[i].py << " " // 27
- << outputDeltaCOMTraj_deq[i].x[0] << " " // 28
- << outputDeltaCOMTraj_deq[i].x[1] << " " // 29
- << outputDeltaCOMTraj_deq[i].x[2] << " " // 30
- << outputDeltaCOMTraj_deq[i].y[0] << " " // 31
- << outputDeltaCOMTraj_deq[i].y[1] << " " // 32
- << outputDeltaCOMTraj_deq[i].y[2] << " " // 33
- << LeftFootAbsolutePositions[i].x << " " // 34
- << LeftFootAbsolutePositions[i].y << " " // 35
- << LeftFootAbsolutePositions[i].z << " " // 36
- << LeftFootAbsolutePositions[i].theta << " " // 37
- << LeftFootAbsolutePositions[i].omega << " " // 38
- << LeftFootAbsolutePositions[i].dx << " " // 39
- << LeftFootAbsolutePositions[i].dy << " " // 40
- << LeftFootAbsolutePositions[i].dz << " " // 41
- << LeftFootAbsolutePositions[i].dtheta << " " // 42
- << LeftFootAbsolutePositions[i].domega << " " // 43
- << LeftFootAbsolutePositions[i].ddx << " " // 44
- << LeftFootAbsolutePositions[i].ddy << " " // 45
- << LeftFootAbsolutePositions[i].ddz << " " // 46
- << LeftFootAbsolutePositions[i].ddtheta << " " // 47
- << LeftFootAbsolutePositions[i].ddomega << " " // 48
- << RightFootAbsolutePositions[i].x << " " // 49
- << RightFootAbsolutePositions[i].y << " " // 50
- << RightFootAbsolutePositions[i].z << " " // 51
- << RightFootAbsolutePositions[i].theta << " " // 52
- << RightFootAbsolutePositions[i].omega << " " // 53
- << RightFootAbsolutePositions[i].dx << " " // 54
- << RightFootAbsolutePositions[i].dy << " " // 55
- << RightFootAbsolutePositions[i].dz << " " // 56
- << RightFootAbsolutePositions[i].dtheta << " " // 57
- << RightFootAbsolutePositions[i].domega << " " // 58
- << RightFootAbsolutePositions[i].ddx << " " // 59
- << RightFootAbsolutePositions[i].ddy << " " // 60
- << RightFootAbsolutePositions[i].ddz << " " // 61
- << RightFootAbsolutePositions[i].ddtheta << " " // 62
- << RightFootAbsolutePositions[i].ddomega << " " // 63
- << endl ;
- }
- aof.close() ;
- ++iteration;
-
}
int AnalyticalMorisawaCompact::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
@@ -946,7 +743,7 @@ When the limit is reached, and the stack exhausted this method is called again.
/*! Recompute time when a new step should be added. */
m_UpperTimeLimitToUpdateStacks = m_AbsoluteTimeReference + m_DeltaTj[0] + m_Tdble + 0.45 * m_Tsingle;
- m_kajitaDynamicFilter->init( m_CurrentTime, m_SamplingPeriod, 0.04, m_SamplingPeriod, 1.6,
+ m_kajitaDynamicFilter->init( m_SamplingPeriod, 0.04, m_SamplingPeriod, 1.6,
lStartingCOMState.z[0], InitLeftFootAbsolutePosition, lStartingCOMState );
return m_RelativeFootPositions.size();
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index 1cfcb999..b10bff5d 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -10,7 +10,6 @@ DynamicFilter::DynamicFilter(
SimplePluginManager *SPM,
CjrlHumanoidDynamicRobot *aHS)
{
- currentTime_ = 0.0 ;
controlPeriod_ = 0.0 ;
interpolationPeriod_ = 0.0 ;
previewWindowSize_ = 0.0 ;
@@ -77,9 +76,9 @@ DynamicFilter::~DynamicFilter()
}
}
-void DynamicFilter::setRobotUpperPart(MAL_VECTOR_TYPE(double) & configuration,
- MAL_VECTOR_TYPE(double) & velocity,
- MAL_VECTOR_TYPE(double) & acceleration)
+void DynamicFilter::setRobotUpperPart(const MAL_VECTOR_TYPE(double) & configuration,
+ const MAL_VECTOR_TYPE(double) & velocity,
+ const MAL_VECTOR_TYPE(double) & acceleration)
{
for ( unsigned int i = 0 ; i < upperPartIndex.size() ; ++i )
{
@@ -92,7 +91,6 @@ void DynamicFilter::setRobotUpperPart(MAL_VECTOR_TYPE(double) & configuration,
/// \brief Initialse all objects, to be called just after the constructor
void DynamicFilter::init(
- double currentTime,
double controlPeriod,
double interpolationPeriod,
double PG_T,
@@ -101,7 +99,6 @@ void DynamicFilter::init(
FootAbsolutePosition inputLeftFoot,
COMState inputCoMState)
{
- currentTime_ = currentTime ;
controlPeriod_ = controlPeriod ;
interpolationPeriod_ = interpolationPeriod ;
PG_T_ = PG_T ;
@@ -189,16 +186,14 @@ void DynamicFilter::init(
}
int DynamicFilter::OffLinefilter(
- COMState & lastCtrlCoMState,
- FootAbsolutePosition & lastCtrlLeftFoot,
- FootAbsolutePosition & lastCtrlRightFoot,
- deque<COMState> & inputCOMTraj_deq_,
- deque<ZMPPosition> inputZMPTraj_deq_,
- deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
- deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
- vector< MAL_VECTOR_TYPE(double) > & UpperPart_q,
- vector< MAL_VECTOR_TYPE(double) > & UpperPart_dq,
- vector< MAL_VECTOR_TYPE(double) > & UpperPart_ddq,
+ const double currentTime,
+ const deque<COMState> &inputCOMTraj_deq_,
+ const deque<ZMPPosition> &inputZMPTraj_deq_,
+ const deque<FootAbsolutePosition> &inputLeftFootTraj_deq_,
+ const deque<FootAbsolutePosition> &inputRightFootTraj_deq_,
+ const vector< MAL_VECTOR_TYPE(double) > & UpperPart_q,
+ const vector< MAL_VECTOR_TYPE(double) > & UpperPart_dq,
+ const vector< MAL_VECTOR_TYPE(double) > & UpperPart_ddq,
deque<COMState> & outputDeltaCOMTraj_deq_)
{
unsigned int N = inputCOMTraj_deq_.size() ;
@@ -207,7 +202,7 @@ int DynamicFilter::OffLinefilter(
setRobotUpperPart(UpperPart_q[0],UpperPart_dq[0],UpperPart_ddq[0]);
- for(unsigned int i = 0 ; i < inputCOMTraj_deq_.size() ; ++i )
+ for(unsigned int i = 0 ; i < N ; ++i )
{
ComputeZMPMB(interpolationPeriod_,inputCOMTraj_deq_[i],inputLeftFootTraj_deq_[i],
inputRightFootTraj_deq_[i], ZMPMB_vec_[i] , stage0_ , i);
@@ -218,14 +213,10 @@ int DynamicFilter::OffLinefilter(
deltaZMP_deq_[i].py = inputZMPTraj_deq_[i].py - ZMPMB_vec_[i][1] ;
deltaZMP_deq_[i].pz = 0.0 ;
deltaZMP_deq_[i].theta = 0.0 ;
- deltaZMP_deq_[i].time = m_CurrentTime + i * interpolationPeriod_ ;
- deltaZMP_deq_[i].stepType = ZMPMB_vec_[i].stepType ;
+ deltaZMP_deq_[i].time = currentTime + i * interpolationPeriod_ ;
+ deltaZMP_deq_[i].stepType = inputZMPTraj_deq_[i].stepType ;
}
- m_kajitaDynamicFilter->OptimalControl(inputdeltaZMP_deq,outputDeltaCOMTraj_deq) ;
-
-
-
-
+ OptimalControl(deltaZMP_deq_,outputDeltaCOMTraj_deq_) ;
return 0;
}
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
index 4b97b268..b95dd88b 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
@@ -53,16 +53,15 @@ namespace PatternGeneratorJRL
);
~DynamicFilter();
/// \brief
- int OffLinefilter(COMState & lastCtrlCoMState,
- FootAbsolutePosition & lastCtrlLeftFoot,
- FootAbsolutePosition & lastCtrlRightFoot,
- deque<COMState> & inputCOMTraj_deq_,
- deque<ZMPPosition> inputZMPTraj_deq_,
- deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
- deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
- vector<boost_ublas::vector<double> > &UpperPart_q,
- vector<boost_ublas::vector<double> > &UpperPart_dq,
- vector<boost_ublas::vector<double> > &UpperPart_ddq,
+ int OffLinefilter(
+ const double currentTime,
+ const deque<COMState> & inputCOMTraj_deq_,
+ const deque<ZMPPosition> & inputZMPTraj_deq_,
+ const deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+ const deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
+ const vector<MAL_VECTOR_TYPE(double) > &UpperPart_q,
+ const vector<MAL_VECTOR_TYPE(double) > &UpperPart_dq,
+ const vector<MAL_VECTOR_TYPE(double) > &UpperPart_ddq,
deque<COMState> & outputDeltaCOMTraj_deq_);
int OnLinefilter(
@@ -76,7 +75,6 @@ namespace PatternGeneratorJRL
deque<COMState> & outputDeltaCOMTraj_deq_);
void init(
- double currentTime,
double controlPeriod,
double interpolationPeriod,
double PG_T,
@@ -139,9 +137,6 @@ namespace PatternGeneratorJRL
public: // The accessors
/// \brief setter :
- inline void setCurrentTime(double time)
- {currentTime_ = time ;}
-
inline void setControlPeriod(double controlPeriod)
{controlPeriod_ = controlPeriod ;}
@@ -154,17 +149,14 @@ namespace PatternGeneratorJRL
inline void setPreviewWindowSize_(double previewWindowSize)
{ previewWindowSize_ = previewWindowSize; }
- void setRobotUpperPart(MAL_VECTOR_TYPE(double) & configuration,
- MAL_VECTOR_TYPE(double) & velocity,
- MAL_VECTOR_TYPE(double) & acceleration);
+ void setRobotUpperPart(const MAL_VECTOR_TYPE(double) & configuration,
+ const MAL_VECTOR_TYPE(double) & velocity,
+ const MAL_VECTOR_TYPE(double) & acceleration);
/// \brief getter :
inline ComAndFootRealizationByGeometry * getComAndFootRealization()
{ return comAndFootRealization_;}
- inline double getCurrentTime()
- {return currentTime_ ;}
-
inline double getControlPeriod()
{return controlPeriod_ ;}
@@ -194,9 +186,7 @@ namespace PatternGeneratorJRL
/// \brief Time variables
/// -----------------------------------
- /// \brief Current time of the PG.
- double currentTime_ ;
-
+ ///
/// \brief control period of the PG host
double controlPeriod_;
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 92734569..5994514a 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -438,7 +438,7 @@ int
cout << "lStartingCOMState = " << std::scientific << lStartingCOMState << endl ;
cout << "lStartingZMPPosition = " << std::scientific << lStartingZMPPosition << endl ;
- dynamicFilter_->init(0.0,m_SamplingPeriod,InterpolationPeriod_,
+ dynamicFilter_->init(m_SamplingPeriod,InterpolationPeriod_,
QP_T_, QP_N_*QP_T_ - QP_T_/m_SamplingPeriod * InterpolationPeriod_ ,CoMHeight_,InitLeftFootAbsolutePosition,lStartingCOMState);
return 0;
}
diff --git a/tests/TestHirukawa2007.cpp b/tests/TestHirukawa2007.cpp
new file mode 100644
index 00000000..b5b01101
--- /dev/null
+++ b/tests/TestHirukawa2007.cpp
@@ -0,0 +1,590 @@
+/*
+ * Copyright 2010,
+ *
+ * Andrei Herdt
+ * Olivier Stasse
+ *
+ * JRL, CNRS/AIST
+ *
+ * This file is part of walkGenJrl.
+ * walkGenJrl is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * walkGenJrl is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Lesser Public License for more details.
+ * 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
+ * Joint Japanese-French Robotics Laboratory (JRL)
+ */
+/* \file This file tests A. Herdt's walking algorithm for
+ * automatic foot placement giving an instantaneous CoM velocity reference.
+ */
+#include "Debug.hh"
+#include "CommonTools.hh"
+#include "TestObject.hh"
+#include <hrp2-dynamics/hrp2OptHumanoidDynamicRobot.h>
+#include <ZMPRefTrajectoryGeneration/DynamicFilter.hh>
+#include <metapod/models/hrp2_14/hrp2_14.hh>
+#include <boost/fusion/algorithm/iteration/for_each.hpp>
+#include <boost/fusion/include/for_each.hpp>
+#include <boost/fusion/include/accumulate.hpp>
+#include <MultiContactRefTrajectoryGeneration/MultiContactHirukawa.hh>
+
+#ifndef METAPOD_TYPEDEF
+#define METAPOD_TYPEDEF
+ typedef double LocalFloatType;
+ typedef metapod::Spatial::ForceTpl<LocalFloatType> Force_HRP2_14;
+ typedef metapod::hrp2_14<LocalFloatType> Robot_Model;
+
+ typedef metapod::Nodes< Robot_Model, Robot_Model::BODY >::type RootNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::l_wrist >::type LhandNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::r_wrist >::type RhandNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::LARM_LINK0 >::type LshoulderNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::RARM_LINK0 >::type RshoulderNode;
+
+ typedef metapod::Nodes< Robot_Model, Robot_Model::LLEG_LINK0 >::type LhipNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::RLEG_LINK0 >::type RhipNode;
+ 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::LLEG_LINK0,0,true,false> Jac_LF;
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::r_ankle, Robot_Model::RLEG_LINK0,0,true,false> Jac_RF;
+
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::l_wrist, Robot_Model::LARM_LINK0,0,true,false> Jac_LH;
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::r_wrist, Robot_Model::RARM_LINK0,0,true,false> Jac_RH;
+#endif
+
+typedef metapod::Nodes< Robot_Model, Robot_Model::BODY >::type RootNode;
+
+using namespace::PatternGeneratorJRL;
+using namespace::PatternGeneratorJRL::TestSuite;
+using namespace std;
+
+typedef Eigen::Matrix<double,6,1> vector6d ;
+
+class TestHirukawa2007: public TestObject
+{
+
+private:
+
+ SimplePluginManager * SPM_ ;
+ double dInitX, dInitY;
+ bool once ;
+ MAL_VECTOR(InitialPosition,double);
+ double samplingPeriod ;
+
+ vector<COMState> comPos_ ;
+ vector< FootAbsolutePosition > rf_ ;
+ vector< FootAbsolutePosition > lf_ ;
+ vector< HandAbsolutePosition > rh_ ;
+ vector< HandAbsolutePosition > lh_ ;
+ vector<ZMPPosition> zmp_ ;
+
+ Robot_Model robot_ ;
+ Robot_Model::confVector q_init_ ;
+
+ vector< vector<double> > data_ ;
+
+ MultiContactHirukawa MCHpg ;
+
+public:
+ TestHirukawa2007(int argc, char *argv[], string &aString):
+ TestObject(argc,argv,aString)
+ {
+ SPM_ = NULL ;
+ once = true ;
+ MAL_VECTOR_RESIZE(InitialPosition,30);
+ samplingPeriod = 0.005 ;
+ }
+
+ ~TestHirukawa2007()
+ {
+ if ( SPM_ != 0 )
+ {
+ delete SPM_ ;
+ SPM_ = 0 ;
+ }
+ m_DebugHDR = 0;
+ }
+
+ typedef void (TestHirukawa2007::* localeventHandler_t)(PatternGeneratorInterface &);
+
+ struct localEvent
+ {
+ unsigned time;
+ localeventHandler_t Handler ;
+ };
+
+ bool doTest(ostream &os)
+ {
+ metapod::bcalc<Robot_Model>::run(robot_,q_init_);
+ metapod::jcalc<Robot_Model>::run(robot_,q_init_,Robot_Model::confVector::Zero());
+
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+ oss.str("/home/mnaveau/devel/matlab_scripts/step_generator/testMetapod.txt");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+
+ boost::fusion::for_each(robot_.nodes , print_iXo() );
+
+ double sum_mass = 0.0 ;
+ metapod::Vector3dTpl< LocalFloatType >::Type com (0.0,0.0,0.0);
+
+ sum_mass = boost::fusion::accumulate(robot_.nodes , sum_mass , MassSum() );
+ com = boost::fusion::accumulate(robot_.nodes , com , MassbyComSum() );
+ cout << "mass * com = \n" << com << endl ;
+ cout << "mass = \n" << sum_mass << endl ;
+ com = com / sum_mass ;
+ cout << "com = \n" << com << endl ;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ data_.clear() ;
+ std::string astateFile =
+"/home/mnaveau/devel/ros_unstable/src/jrl/jrl-walkgen/_build-RELEASE/tests/TestMorisawa2007ShortWalk32TestFGPI.dat" ;
+ std::ifstream dataStream ;
+ dataStream.open(astateFile.c_str(),std::ifstream::in);
+
+ // reading all the data file
+ while (dataStream.good()) {
+ vector<double> oneLine(74) ;
+ for (unsigned int i = 0 ; i < oneLine.size() ; ++i)
+ dataStream >> oneLine[i];
+ data_.push_back(oneLine);
+ }
+ dataStream.close();
+
+ comPos_.resize(data_.size()) ;
+ rf_.resize(data_.size()) ;
+ lf_.resize(data_.size()) ;
+ rh_.resize(data_.size()) ;
+ lh_.resize(data_.size()) ;
+ zmp_ .resize(data_.size()) ;
+
+ for (unsigned int i = 0 ; i < data_.size() ; ++i)
+ {
+ comPos_[i].x[0] = data_[i][1] ;
+ comPos_[i].y[0] = data_[i][2] ;
+ comPos_[i].z[0] = data_[i][3] ;
+ comPos_[i].yaw[0] = data_[i][4] ;
+ comPos_[i].x[1] = data_[i][5] ;
+ comPos_[i].y[1] = data_[i][6] ;
+ comPos_[i].z[1] = data_[i][7] ;
+
+ rf_[i].x = data_[i][22] ;
+ rf_[i].y = data_[i][23] ;
+ rf_[i].z = data_[i][24] ;
+ rf_[i].omega = 0.0 ;
+ rf_[i].omega2 = 0.0 ;
+ rf_[i].theta = 0.0 ;
+
+ rf_[i].dx = data_[i][25] ;
+ rf_[i].dy = data_[i][26] ;
+ rf_[i].dz = data_[i][27] ;
+ rf_[i].domega = 0.0 ;
+ rf_[i].domega2 = 0.0 ;
+ rf_[i].dtheta = 0.0 ;
+
+ lf_[i].x = data_[i][10] ;
+ lf_[i].y = data_[i][11] ;
+ lf_[i].z = data_[i][12] ;
+ lf_[i].omega = 0.0 ;
+ lf_[i].omega2 = 0.0 ;
+ lf_[i].theta = 0.0 ;
+
+ lf_[i].dx = data_[i][13] ;
+ lf_[i].dy = data_[i][14] ;
+ lf_[i].dz = data_[i][15] ;
+ lf_[i].domega = 0.0 ;
+ lf_[i].domega2 = 0.0 ;
+ lf_[i].dtheta = 0.0 ;
+
+ rh_[i].dx = data_[i][4] ;
+ rh_[i].dy = data_[i][5] ;
+ rh_[i].dz = data_[i][6] ;
+ rh_[i].domega = 0.0 ;
+ rh_[i].domega2 = 0.0 ;
+ rh_[i].dtheta = 0.0 ;
+ rh_[i].stepType = 1.0 ;
+
+
+ lh_[i].dx = data_[i][4] ;
+ lh_[i].dy = data_[i][5] ;
+ lh_[i].dz = data_[i][6] ;
+ lh_[i].domega = 0.0 ;
+ lh_[i].domega2 = 0.0 ;
+ lh_[i].dtheta = 0.0 ;
+ lh_[i].stepType = 1.0 ;
+ }
+
+ MCHpg.online(comPos_,rf_,lf_,rh_,lh_) ;
+
+ vector<vector<int> > test_vector ;
+ test_vector.clear();
+ unsigned int dimension = 5 ;
+ test_vector.resize(dimension);
+ for (unsigned int i = 0 ; i < dimension ; ++i)
+ {
+ test_vector[i].clear();
+ for (unsigned int j = 0 ; j < i ; ++j)
+ {
+ int nbr = j ;
+ test_vector[i].push_back(nbr);
+ }
+ }
+
+
+ for (unsigned int i = 0 ; i < test_vector.size() ; ++i)
+ {
+ for (unsigned int j = 0 ; j < test_vector[i].size() ; ++j)
+ {
+ cout << test_vector[i][j] << " ";
+ }
+ cout << endl ;
+ }
+
+
+ //fillInDebugFiles();
+ return true ;
+ }
+
+ struct print_iXo
+ {
+ template <typename T>
+ void operator()(T & x) const
+ {
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+ oss.str("/home/mnaveau/devel/matlab_scripts/step_generator/testMetapod.txt");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(7);
+ aof.setf(ios::fixed, ios::floatfield);
+ aof << Robot_Model::inertias[x.id] ;
+ }
+ };
+
+ struct MassSum
+ {
+ typedef LocalFloatType result_type;
+
+ template <typename T>
+ result_type operator()( const result_type sum_mass, T & node) const
+ {
+ return ( sum_mass + Robot_Model::inertias[node.id].m() ) ;
+ }
+ };
+
+ struct MassbyComSum
+ {
+ typedef metapod::Vector3dTpl< LocalFloatType >::Type result_type;
+
+ template <typename T>
+ result_type operator()( const result_type sum_h, const T & x) const
+ {
+ double mass = Robot_Model::inertias[x.id].m() ;
+ return ( sum_h + mass * x.body.iX0.r() + x.body.iX0.E() * Robot_Model::inertias[x.id].h() );
+ }
+ };
+
+ void init()
+ {
+ // Instanciate and initialize.
+ string RobotFileName = m_VRMLPath + m_VRMLFileName;
+
+ bool fileExist = false;
+ {
+ std::ifstream file (RobotFileName.c_str ());
+ fileExist = !file.fail ();
+ }
+ if (!fileExist)
+ throw std::string ("failed to open robot model");
+
+ // Creating the humanoid robot.
+ SpecializedRobotConstructor(m_HDR);
+ if(m_HDR==0)
+ {
+ if (m_HDR!=0) delete m_HDR;
+ dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
+ m_HDR = aRobotDynamicsObjectConstructor.createHumanoidDynamicRobot();
+ }
+ // Parsing the file.
+ dynamicsJRLJapan::parseOpenHRPVRMLFile(*m_HDR,RobotFileName,
+ m_LinkJointRank,
+ m_SpecificitiesFileName);
+ // Create Pattern Generator Interface
+ m_PGI = patternGeneratorInterfaceFactory(m_HDR);
+
+ unsigned int lNbActuatedJoints = 30;
+ double * dInitPos = new double[lNbActuatedJoints];
+ ifstream aif;
+ aif.open(m_InitConfig.c_str(),ifstream::in);
+ if (aif.is_open())
+ {
+ for(unsigned int i=0;i<lNbActuatedJoints;i++)
+ aif >> dInitPos[i];
+ }
+ aif.close();
+
+ bool DebugConfiguration = true;
+ ofstream aofq;
+ if (DebugConfiguration)
+ {
+ aofq.open("TestConfiguration.dat",ofstream::out);
+ if (aofq.is_open())
+ {
+ for(unsigned int k=0;k<30;k++)
+ {
+ aofq << dInitPos[k] << " ";
+ }
+ aofq << endl;
+ }
+
+ }
+
+ // This is a vector corresponding to the DOFs actuated of the robot.
+ bool conversiontoradneeded=true;
+ if (conversiontoradneeded)
+ for(unsigned int i=0;i<MAL_VECTOR_SIZE(InitialPosition);i++)
+ InitialPosition(i) = dInitPos[i]*M_PI/180.0;
+ else
+ for(unsigned int i=0;i<MAL_VECTOR_SIZE(InitialPosition);i++)
+ InitialPosition(i) = dInitPos[i];
+
+ q_init_(0) = 0.0 ;
+ q_init_(1) = 0.0 ;
+ q_init_(2) = 0.6487 ;
+ q_init_(3) = 0.0 ;
+ q_init_(4) = 0.0 ;
+ q_init_(5) = 0.0 ;
+ for(unsigned int i=0 ; i<MAL_VECTOR_SIZE(InitialPosition) ; i++)
+ q_init_(i+6,0) = InitialPosition(i) ;
+
+ MCHpg.q(q_init_) ;
+
+ // This is a vector corresponding to ALL the DOFS of the robot:
+ // free flyer + actuated DOFS.
+ unsigned int lNbDofs = 36 ;
+ MAL_VECTOR_DIM(CurrentConfiguration,double,lNbDofs);
+ MAL_VECTOR_DIM(CurrentVelocity,double,lNbDofs);
+ MAL_VECTOR_DIM(CurrentAcceleration,double,lNbDofs);
+ MAL_VECTOR_DIM(PreviousConfiguration,double,lNbDofs) ;
+ MAL_VECTOR_DIM(PreviousVelocity,double,lNbDofs);
+ MAL_VECTOR_DIM(PreviousAcceleration,double,lNbDofs);
+ for(int i=0;i<6;i++)
+ {
+ PreviousConfiguration[i] = PreviousVelocity[i] = PreviousAcceleration[i] = 0.0;
+ }
+
+ for(unsigned int i=6;i<lNbDofs;i++)
+ {
+ PreviousConfiguration[i] = InitialPosition[i-6];
+ PreviousVelocity[i] = PreviousAcceleration[i] = 0.0;
+ }
+
+ delete [] dInitPos;
+
+ MAL_VECTOR_RESIZE(m_CurrentConfiguration, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(m_CurrentVelocity, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(m_CurrentAcceleration, m_HDR->numberDof());
+
+ MAL_VECTOR_RESIZE(m_PreviousConfiguration, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(m_PreviousVelocity, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(m_PreviousAcceleration, m_HDR->numberDof());
+
+ SPM_ = new SimplePluginManager();
+ }
+
+protected:
+
+ void chooseTestProfile()
+ {return;}
+ void generateEvent()
+ {return;}
+
+ void SpecializedRobotConstructor(CjrlHumanoidDynamicRobot *& aHDR)
+ {
+ dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
+ Chrp2OptHumanoidDynamicRobot *aHRP2HDR = new Chrp2OptHumanoidDynamicRobot( &aRobotDynamicsObjectConstructor );
+ aHDR = aHRP2HDR;
+ }
+
+// void fillInDebugFiles( )
+// {
+// /// \brief Create file .hip .pos .zmp
+// /// --------------------
+// ofstream aof;
+// string aFileName;
+// static int iteration = 0 ;
+
+// if ( iteration == 0 ){
+// aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+// aFileName+=m_TestName;
+// aFileName+=".pos";
+// aof.open(aFileName.c_str(),ofstream::out);
+// aof.close();
+// }
+// ///----
+// aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+// aFileName+=m_TestName;
+// aFileName+=".pos";
+// 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) ) << " " ; // 2
+// }
+// for(unsigned int i = 0 ; i < 9 ; i++){
+// aof << 0.0 << " " ;
+// }
+// aof << 0.0 << endl ;
+// aof.close();
+
+// if ( iteration == 0 ){
+// aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+// aFileName+=m_TestName;
+// aFileName+=".hip";
+// aof.open(aFileName.c_str(),ofstream::out);
+// aof.close();
+// }
+// aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+// aFileName+=m_TestName;
+// aFileName+=".hip";
+// aof.open(aFileName.c_str(),ofstream::app);
+// aof.precision(8);
+// aof.setf(ios::scientific, ios::floatfield);
+// aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+// aof << filterprecision( m_OneStep.finalCOMPosition.roll[0] * M_PI /180) << " " ; // 2
+// aof << filterprecision( m_OneStep.finalCOMPosition.pitch[0] * M_PI /180 ) << " " ; // 3
+// aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0] * M_PI /180 ) ; // 4
+// aof << endl ;
+// aof.close();
+
+// if ( iteration == 0 ){
+// aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+// aFileName+=m_TestName;
+// aFileName+=".zmp";
+// aof.open(aFileName.c_str(),ofstream::out);
+// aof.close();
+// }
+
+// FootAbsolutePosition aSupportState;
+// if (m_OneStep.LeftFootPosition.stepType < 0 )
+// aSupportState = m_OneStep.LeftFootPosition ;
+// else
+// aSupportState = m_OneStep.RightFootPosition ;
+
+// aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+// aFileName+=m_TestName;
+// aFileName+=".zmp";
+// aof.open(aFileName.c_str(),ofstream::app);
+// aof.precision(8);
+// aof.setf(ios::scientific, ios::floatfield);
+// aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+// aof << filterprecision( m_OneStep.ZMPTarget(0) - m_CurrentConfiguration(0)) << " " ; // 2
+// aof << filterprecision( m_OneStep.ZMPTarget(1) - m_CurrentConfiguration(1) ) << " " ; // 3
+// aof << filterprecision( aSupportState.z - m_CurrentConfiguration(2)) ; // 4
+// aof << endl ;
+// aof.close();
+
+// aFileName = "/opt/grx3.0/HRP2LAAS/log/mnaveau/";
+// aFileName+="footpos";
+// aFileName+=".dat";
+// aof.open(aFileName.c_str(),ofstream::app);
+// aof.precision(8);
+// aof.setf(ios::scientific, ios::floatfield);
+// aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+// aof << filterprecision( lfFoot[iteration].x ) << " " ; // 2
+// aof << filterprecision( lfFoot[iteration].y ) << " " ; // 3
+// aof << endl ;
+// aof.close();
+
+// iteration++;
+// }
+
+ void SpecializedRobotConstructor( CjrlHumanoidDynamicRobot *& aHDR, CjrlHumanoidDynamicRobot *& aDebugHDR )
+ {
+ dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
+ Chrp2OptHumanoidDynamicRobot *aHRP2HDR = new Chrp2OptHumanoidDynamicRobot( &aRobotDynamicsObjectConstructor );
+ aHDR = aHRP2HDR;
+ aDebugHDR = new Chrp2OptHumanoidDynamicRobot(&aRobotDynamicsObjectConstructor);
+ }
+
+ double filterprecision(double adb)
+ {
+ if (fabs(adb)<1e-7)
+ return 0.0;
+
+ double ladb2 = adb * 1e7;
+ double lintadb2 = trunc(ladb2);
+ return lintadb2/1e7;
+ }
+};
+
+int PerformTests(int argc, char *argv[])
+{
+#define NB_PROFILES 1
+ std::string TestNames = "TestHirukawa2007" ;
+
+ TestHirukawa2007 aTH2007(argc,argv,TestNames);
+ aTH2007.init();
+ try{
+ if (!aTH2007.doTest(std::cout)){
+ cout << "Failed test " << endl;
+ return -1;
+ }
+ else
+ cout << "Passed test " << endl;
+ }
+ catch (const char * astr){
+ cerr << "Failed on following error " << astr << std::endl;
+ return -1;
+ }
+
+ return 0;
+}
+
+int main(int argc, char *argv[])
+{
+ try
+ {
+ int ret = PerformTests(argc,argv);
+ return ret ;
+ }
+ catch (const std::string& msg)
+ {
+ std::cerr << msg << std::endl;
+ }
+ return 1;
+}
+
+
+
diff --git a/tests/TestInverseKinematics.cpp b/tests/TestInverseKinematics.cpp
index f99295a6..273c7bc9 100644
--- a/tests/TestInverseKinematics.cpp
+++ b/tests/TestInverseKinematics.cpp
@@ -100,7 +100,7 @@ public:
int stage0 = 0 ;
int stage1 = 1 ;
double samplingPeriod = 0.005 ;
- dynamicfilter_->init( 0.0,
+ dynamicfilter_->init(
samplingPeriod,
samplingPeriod,
(double)(comPos.size()-320)*samplingPeriod,
@@ -268,7 +268,7 @@ public:
supportFoot = m_OneStep.RightFootPosition ;
}
double samplingPeriod = 0.005;
- dynamicfilter_->init(0.0,samplingPeriod,samplingPeriod,0.1,
+ dynamicfilter_->init(samplingPeriod,samplingPeriod,0.1,
1.6,0.814,supportFoot,m_OneStep.finalCOMPosition);
initIK();
MAL_VECTOR_TYPE(double) UpperConfig = m_HDR->currentConfiguration() ;
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index 5668936f..39b835b9 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -244,7 +244,7 @@ public:
else{
supportFoot = m_OneStep.RightFootPosition ;
}
- dynamicfilter_->init(0.0,samplingPeriod_,samplingPeriod_,samplingPeriod_,
+ dynamicfilter_->init(samplingPeriod_,samplingPeriod_,samplingPeriod_,
samplingPeriod_,0.814,supportFoot,m_OneStep.finalCOMPosition);
initIK();
MAL_VECTOR_TYPE(double) UpperConfig = m_HDR->currentConfiguration() ;
--
GitLab