From f740fe2336cc81fa31a33b4f5e488e9aaf789231 Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Mon, 8 Dec 2014 15:25:32 +0100
Subject: [PATCH] use the dynamic filter of kajita on different sampling
---
.../ZMPVelocityReferencedQP.cpp | 6 ++--
.../ZMPVelocityReferencedQP.hh | 6 ++++
tests/TestHerdt2010DynamicFilter.cpp | 31 +++++++++++++------
3 files changed, 31 insertions(+), 12 deletions(-)
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index d2108971..5eb23f18 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -79,8 +79,10 @@ Solution_(),OFTG_DF_(0),OFTG_control_(0),dynamicFilter_(0)
QP_N_ = 16 ;
m_SamplingPeriod = 0.005 ;
InterpolationPeriod_ = QP_T_/10;
+ previewDuration_ = QP_N_/2*QP_T_ ;
NbSampleControl_ = (int)round(QP_T_/m_SamplingPeriod) ;
NbSampleInterpolation_ = (int)round(QP_T_/InterpolationPeriod_) ;
+ previewSize_ = previewDuration_/NbSampleInterpolation_ ;
StepPeriod_ = 0.8 ;
SSPeriod_ = 0.7 ;
DSPeriod_ = 0.1 ;
@@ -464,7 +466,7 @@ int ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
FinalCurrentStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
dynamicFilter_->init(m_SamplingPeriod,InterpolationPeriod_,
- QP_T_, QP_N_*QP_T_/2 - QP_T_/m_SamplingPeriod * InterpolationPeriod_ ,CoMHeight_,InitLeftFootAbsolutePosition,lStartingCOMState);
+ QP_T_, previewDuration_ - NbSampleControl_*InterpolationPeriod_ ,CoMHeight_,InitLeftFootAbsolutePosition,lStartingCOMState);
return 0;
}
@@ -572,7 +574,7 @@ void ZMPVelocityReferencedQP::OnLine(double time,
DynamicFilterInterpolation(time);
- unsigned int IndexMax = (int)round(QP_N_*QP_T_*0.5 / InterpolationPeriod_);
+ unsigned int IndexMax = (int)round(previewDuration_ / InterpolationPeriod_);
ZMPTraj_deq_.resize(IndexMax);
COMTraj_deq_.resize(IndexMax);
LeftFootTraj_deq_.resize(IndexMax);
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index bfb8484d..de075511 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -181,6 +181,12 @@ namespace PatternGeneratorJRL
/// \brief Nb. samplings inside preview window
int QP_N_;
+ /// \brief Size of the preview for filtering
+ double previewDuration_ ;
+
+ /// \brief Duration of the preview for filtering
+ int previewSize_ ;
+
/// \brief 2D LIPM to simulate the evolution of the robot's CoM.
LinearizedInvertedPendulum2D LIPM_ ;
LinearizedInvertedPendulum2D LIPM_subsampled_ ;
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index 64972c7d..780c2ad1 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -355,9 +355,9 @@ protected:
aCOMState(0) = m_OneStep.finalCOMPosition.x[0]; aCOMSpeed(0) = m_OneStep.finalCOMPosition.x[1]; aCOMAcc(0) = m_OneStep.finalCOMPosition.x[2];
aCOMState(1) = m_OneStep.finalCOMPosition.y[0]; aCOMSpeed(1) = m_OneStep.finalCOMPosition.y[1]; aCOMAcc(1) = m_OneStep.finalCOMPosition.y[2];
aCOMState(2) = m_OneStep.finalCOMPosition.z[0]; aCOMSpeed(2) = m_OneStep.finalCOMPosition.z[1]; aCOMAcc(2) = m_OneStep.finalCOMPosition.z[2];
- aCOMState(3) = m_OneStep.finalCOMPosition.roll[0]; aCOMSpeed(3) = m_OneStep.finalCOMPosition.roll[1]; aCOMAcc(3) = m_OneStep.finalCOMPosition.roll[2];
- aCOMState(4) = m_OneStep.finalCOMPosition.pitch[0]; aCOMSpeed(4) = m_OneStep.finalCOMPosition.pitch[1]; aCOMAcc(4) = m_OneStep.finalCOMPosition.pitch[2];
- aCOMState(5) = m_OneStep.finalCOMPosition.yaw[0]; aCOMSpeed(5) = m_OneStep.finalCOMPosition.yaw[1]; aCOMAcc(5) = m_OneStep.finalCOMPosition.yaw[2];
+ aCOMState(3) = m_OneStep.finalCOMPosition.roll[0]*180/M_PI; aCOMSpeed(3) = m_OneStep.finalCOMPosition.roll[1]*180/M_PI; aCOMAcc(3) = m_OneStep.finalCOMPosition.roll[2]*180/M_PI;
+ aCOMState(4) = m_OneStep.finalCOMPosition.pitch[0]*180/M_PI; aCOMSpeed(4) = m_OneStep.finalCOMPosition.pitch[1]*180/M_PI; aCOMAcc(4) = m_OneStep.finalCOMPosition.pitch[2]*180/M_PI;
+ aCOMState(5) = m_OneStep.finalCOMPosition.yaw[0]*180/M_PI; aCOMSpeed(5) = m_OneStep.finalCOMPosition.yaw[1]*180/M_PI; aCOMAcc(5) = m_OneStep.finalCOMPosition.yaw[2]*180/M_PI;
aLeftFootPosition(0) = m_OneStep.LeftFootPosition.x; aRightFootPosition(0) = m_OneStep.RightFootPosition.x;
aLeftFootPosition(1) = m_OneStep.LeftFootPosition.y; aRightFootPosition(1) = m_OneStep.RightFootPosition.y;
@@ -374,6 +374,9 @@ protected:
20,
0);
+ m_CurrentConfiguration(28)= 0.174532925 ; // RARM_JOINT6
+ m_CurrentConfiguration(35)= 0.174532925 ; // LARM_JOINT6
+
/// \brief Create file .hip .pos .zmp
/// --------------------
ofstream aof ;
@@ -572,7 +575,6 @@ protected:
}
}
-
void stop(PatternGeneratorInterface &aPGI)
{
{
@@ -583,14 +585,22 @@ protected:
void walkForward(PatternGeneratorInterface &aPGI)
{
{
- istringstream strm2(":setVelReference 0.2 0.0 0.0");
+ istringstream strm2(":setVelReference 0.3 0.0 0.0");
aPGI.ParseCmd(strm2);
}
}
void walkSidewards(PatternGeneratorInterface &aPGI)
{
{
- istringstream strm2(":setVelReference 0.0 0.2 0.0");
+ istringstream strm2(":setVelReference 0.0 -0.3 0.0");
+ aPGI.ParseCmd(strm2);
+ }
+ }
+
+ void startWalkInDiagonal(PatternGeneratorInterface &aPGI)
+ {
+ {
+ istringstream strm2(":setVelReference 0.2 0.2 0.0");
aPGI.ParseCmd(strm2);
}
}
@@ -633,8 +643,9 @@ protected:
};
#define localNbOfEvents 12
struct localEvent events [localNbOfEvents] =
- { { 5*200,&TestHerdt2010::walkForward},
- {10*200,&TestHerdt2010::startTurningRightOnSpot},
+ { { 5*200,&TestHerdt2010::startWalkInDiagonal},
+ {10*200,&TestHerdt2010::walkSidewards},
+ {15*200,&TestHerdt2010::startTurningRightOnSpot},
/* {25*200,&TestHerdt2010::startTurningRightOnSpot},
{35*200,&TestHerdt2010::walkForward},
{45*200,&TestHerdt2010::startTurningLeftOnSpot},*//*
@@ -645,8 +656,8 @@ protected:
{95*200,&TestHerdt2010::startTurningRight},*/
// {105*200,&TestHerdt2010::stop},
// {110*200,&TestHerdt2010::stopOnLineWalking}};
- {15*200,&TestHerdt2010::stop},
- {20*200,&TestHerdt2010::stopOnLineWalking}};
+ {20*200,&TestHerdt2010::stop},
+ {25*200,&TestHerdt2010::stopOnLineWalking}};
// Test when triggering event.
for(unsigned int i=0;i<localNbOfEvents;i++)
{
--
GitLab