diff --git a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
index 5f5854d76b1c4171863b3e90f68306d192cf0502..caf332c3e71f385c08dfdd6634328f97b0da743e 100644
--- a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
+++ b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
@@ -321,9 +321,10 @@ void
);
}
+ int index_orientation = PrwSupportStates_deq[1].StepNumber ;
SetParameters(
FootTrajectoryGenerationStandard::THETA_AXIS,
- TimeInterval, PreviewedSupportAngles_deq[0]*180.0/M_PI,
+ TimeInterval, PreviewedSupportAngles_deq[index_orientation]*180.0/M_PI,
LastSFP->theta, LastSFP->dtheta, LastSFP->ddtheta);
SetParametersWithInitPosInitSpeed(
diff --git a/src/Mathematics/PolynomeFoot.cpp b/src/Mathematics/PolynomeFoot.cpp
index 527ed9e6f9ba219c33bdd2f6b1eaae78a5e162c0..4196213aeb7e258411d3511ebb9993efb3bf3886 100644
--- a/src/Mathematics/PolynomeFoot.cpp
+++ b/src/Mathematics/PolynomeFoot.cpp
@@ -127,6 +127,34 @@ void Polynome4::SetParameters(double FT, double MP, double FP)
/*Final Pos*/FP);
}
+void Polynome4::SetParameters(double FT,
+ double InitPos,
+ double InitSpeed,
+ double InitAcc,
+ double FinalSpeed,
+ double FinalAcc)
+{
+ MP_ = 0.0; // default value
+ FP_ = 0.0 ;
+
+ FT_ = FT;
+
+ double tmp;
+ m_Coefficients[0] = InitPos;
+ m_Coefficients[1] = InitSpeed;
+ m_Coefficients[2] = InitAcc/2;
+ tmp = FT*FT;
+ if(tmp==0.0)
+ {
+ m_Coefficients[3] = 0.0;
+ m_Coefficients[4] = 0.0;
+ }else{
+ m_Coefficients[3] = (- 5*InitAcc*FT - 2*FinalAcc*FT - 6.0*InitSpeed + 6.0*FinalSpeed)/(6*tmp);
+ tmp=tmp*FT;
+ m_Coefficients[4] = ( 3*InitAcc*FT + 2*FinalAcc*FT + 4.0*InitSpeed - 4.0*FinalSpeed)/(8*tmp);
+ }
+}
+
void Polynome4::SetParametersWithInitPosInitSpeed(double FT,
double MP,
double InitPos,
diff --git a/src/Mathematics/PolynomeFoot.hh b/src/Mathematics/PolynomeFoot.hh
index 41a5e842d6399596649ad7edee2c027fe62136f1..088b292f62f60225e03ec2cf6ab69aeeffa5408e 100644
--- a/src/Mathematics/PolynomeFoot.hh
+++ b/src/Mathematics/PolynomeFoot.hh
@@ -120,6 +120,20 @@ namespace PatternGeneratorJRL
// Final velocity and position are 0
void SetParameters(double FT, double MP, double FP=0.0);
+ /// Set the parameters
+ // time horizon
+ // Initial Position
+ // Initial velocity (IS)
+ // Initial Acceleration
+ // Final velocity
+ // Final Acceleration
+ void SetParameters(double FT,
+ double InitPos,
+ double InitSpeed,
+ double InitAcc,
+ double FinalSpeed,
+ double FinalAcc);
+
/*! Set the parameters such that
the initial position, and initial speed
are different from zero.
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index 61b53b7bba434d492d1ea8db79ef5584f0b6f61d..bc7ae7975ec9bd3300a3c271ed8dc54a178047d7 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -1,6 +1,6 @@
#include "DynamicFilter.hh"
#include <metapod/algos/rnea.hh>
-
+#include <iomanip>
using namespace std;
using namespace PatternGeneratorJRL;
using namespace metapod;
@@ -402,6 +402,77 @@ int DynamicFilter::OnLinefilter(
}
aof.close();
+ oss.str("/tmp/buffer_");
+ oss << setfill('0') << setw(3) << iteration_zmp << ".txt" ;
+ 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 < NbI_*PG_N_ ; ++i)
+ {
+ aof << i << " " ; // 0
+ aof << inputZMPTraj_deq_[i].px << " " ; // 1
+ aof << inputZMPTraj_deq_[i].py << " " ; // 2
+
+ aof << ZMPMB_vec_[i][0] << " " ; // 3
+ aof << ZMPMB_vec_[i][1] << " " ; // 4
+
+ aof << inputCOMTraj_deq_[i].x[0] << " " ; // 5
+ aof << inputCOMTraj_deq_[i].x[1] << " " ; // 6
+ aof << inputCOMTraj_deq_[i].x[2] << " " ; // 7
+
+ aof << inputLeftFootTraj_deq_[i].x << " " ; // 8
+ aof << inputLeftFootTraj_deq_[i].dx << " " ; // 9
+ aof << inputLeftFootTraj_deq_[i].ddx << " " ; // 10
+
+ aof << inputRightFootTraj_deq_[i].x << " " ; // 11
+ aof << inputRightFootTraj_deq_[i].dx << " " ; // 12
+ aof << inputRightFootTraj_deq_[i].ddx << " " ; // 13
+
+ aof << inputCOMTraj_deq_[i].y[0] << " " ; // 14
+ aof << inputCOMTraj_deq_[i].y[1] << " " ; // 15
+ aof << inputCOMTraj_deq_[i].y[2] << " " ; // 16
+
+ aof << inputLeftFootTraj_deq_[i].y << " " ; // 17
+ aof << inputLeftFootTraj_deq_[i].dy << " " ; // 18
+ aof << inputLeftFootTraj_deq_[i].ddy << " " ; // 19
+
+ aof << inputRightFootTraj_deq_[i].y << " " ; // 20
+ aof << inputRightFootTraj_deq_[i].dy << " " ; // 21
+ aof << inputRightFootTraj_deq_[i].ddy << " " ; // 22
+
+ aof << inputCOMTraj_deq_[i].yaw[0] << " " ; // 23
+ aof << inputCOMTraj_deq_[i].yaw[1] << " " ; // 24
+ aof << inputCOMTraj_deq_[i].yaw[2] << " " ; // 25
+
+ aof << inputLeftFootTraj_deq_[i].theta << " " ; // 26
+ aof << inputLeftFootTraj_deq_[i].dtheta << " " ; // 27
+ aof << inputLeftFootTraj_deq_[i].ddtheta << " " ; // 28
+
+ aof << inputRightFootTraj_deq_[i].theta << " " ; // 29
+ aof << inputRightFootTraj_deq_[i].dtheta << " " ; // 30
+ aof << inputRightFootTraj_deq_[i].ddtheta << " " ;// 31
+
+ aof << inputCOMTraj_deq_[i].z[0] << " " ; // 32
+ aof << inputCOMTraj_deq_[i].z[1] << " " ; // 33
+ aof << inputCOMTraj_deq_[i].z[2] << " " ; // 34
+
+ aof << inputLeftFootTraj_deq_[i].z << " " ; // 35
+ aof << inputLeftFootTraj_deq_[i].dz << " " ; // 36
+ aof << inputLeftFootTraj_deq_[i].ddz << " " ; // 37
+
+ aof << inputRightFootTraj_deq_[i].z << " " ; // 38
+ aof << inputRightFootTraj_deq_[i].dz << " " ; // 39
+ aof << inputRightFootTraj_deq_[i].ddz << " " ; // 40
+
+ aof << endl ;
+ }
+ aof.close();
+
+
iteration_zmp++;
return 0 ;
diff --git a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp
index 2800966f21b39de032cb144e6ba12244c492f7ee..ea547a568393d2a29021e4f192269c8bc9cddd1b 100644
--- a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp
+++ b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp
@@ -70,14 +70,15 @@ OrientationsPreview::OrientationsPreview(CjrlHumanoidDynamicRobot *aHS)
uLimitFeet_ = 5.0/180.0*M_PI;
// Polynome to interpolate the trunk orientation
- TrunkStateTheta_ = new Polynome5(0.0,0.0);
+ TrunkStateYaw_ = new Polynome4(0.0,0.0);
+
+ LastFirstPvwSol_ = 0.0 ;
}
OrientationsPreview::~OrientationsPreview() {
}
-void
- OrientationsPreview::preview_orientations(double Time,
+void OrientationsPreview::preview_orientations(double Time,
const reference_t & Ref,
double StepDuration,
const std::deque<FootAbsolutePosition> & LeftFootPositions_deq,
@@ -108,10 +109,25 @@ void
signRotVelTrunk_ = (TrunkStateT_.yaw[1] < 0.0)?-1.0:1.0;
- unsigned StepNumber = 0;
+ // compute the number of iteration before landing on the first previewed step
+ std::deque<support_state_t>::const_iterator SPTraj_it = Solution.SupportStates_deq.begin();
+ int ItBeforeLanding = 0 ;
+ while(SPTraj_it!=Solution.SupportStates_deq.end())
+ {
+ if ( SPTraj_it->StateChanged !=1 )
+ {
+ ++ItBeforeLanding ;
+ }
+ else
+ {
+ break;
+ }
+ ++SPTraj_it;
+ }
+ int ItBeforeLandingThresh = 2 ;
+ unsigned NbStepsPreviewed = Solution.SupportStates_deq.back().StepNumber;
- // Parameters of the trunk polynomial (fourth order)
- double a,b,c,d,e;
+ unsigned StepNumber = 0;
// Trunk angle at the end of the current support phase
double PreviewedTrunkAngleEnd;
@@ -134,14 +150,22 @@ void
TrunkAngleOK = false;
while(!TrunkAngleOK)
{
+ // order 4 polynomial P(t) = a + b t + 1/2 c t^2 + 1/3 d t^3 + 1/4 e t^4
+ // with the following cnstraint :
+ // - P(0) = InitAngle = TrunkState_.yaw[0]
+ // - d P(0) /dt = InitAngleVelocity = TrunkState_.yaw[0]
+ // - d² P(0) /dt² = 0
+ // - d P(T_) /dt = FinalAngleVelocity = TrunkStateT_.yaw[1]
+ // - d² P(T_) /dt² = 0
if (fabs(TrunkStateT_.yaw[1]-TrunkState_.yaw[1]) > EPS_)
{
- a = TrunkState_.yaw[0];
- b = TrunkState_.yaw[1];
- c = 0.0;
- d = 3.0*(TrunkStateT_.yaw[1]-TrunkState_.yaw[1]) / (T_*T_);
- e = -2.0*d/(3.0*T_);
- TrunkStateT_.yaw[0] = a + b*T_+1.0/2.0*c*T_*T_+1.0/3.0*d*T_*T_*T_+1.0/4.0*e*T_*T_*T_*T_;
+ TrunkStateYaw_->SetParameters(T_,
+ TrunkState_.yaw[0],
+ TrunkState_.yaw[1],
+ /*initAcc*/0.0,
+ /*finalSpeed*/TrunkStateT_.yaw[1],
+ /*finalAcc*/0.0);
+ TrunkStateT_.yaw[0] = TrunkStateYaw_->Compute(T_);
}
else
TrunkStateT_.yaw[0] = TrunkState_.yaw[0] + TrunkState_.yaw[1]*T_;
@@ -182,7 +206,10 @@ void
StepNumber++)
{
PreviewedSupportFoot = -PreviewedSupportFoot;
- //compute the optimal support orientation
+ // compute the optimal support orientation :
+ // the standard is that the orientation of the next support foot is the orientation of the trunk
+ // plus half of the angular displacement of the trunk. Which lead, more or less to the assumption
+ // the orientation of taht the orientation of the trunk is half of the orientation of the feet.
double PreviewedSupportAngle = PreviewedTrunkAngleEnd + TrunkStateT_.yaw[1]*SSPeriod_/2.0;
verify_velocity_hip_joint(Time,
@@ -209,9 +236,16 @@ void
TrunkVelOK = false;
break;
}
- else
- PreviewedSupportAngles_deq.push_back(PreviewedSupportAngle);
-
+ else{
+
+ if( ItBeforeLanding <= ItBeforeLandingThresh && ItBeforeLanding > 0 && Solution.SupportStates_deq.front().Phase == SS
+ && Solution.SupportStates_deq.front().StateChanged != 1 && NbStepsPreviewed > 0
+ && StepNumber == (unsigned) FirstFootPreviewed )
+ {
+ PreviewedSupportAngles_deq.push_back(LastFirstPvwSol_);
+ }
+ PreviewedSupportAngles_deq.push_back(PreviewedSupportAngle);
+ }
//Prepare for the next step
PreviewedTrunkAngleEnd = PreviewedTrunkAngleEnd + SSPeriod_*TrunkStateT_.yaw[1];
PreviousSupportAngle = PreviewedSupportAngle;
@@ -249,12 +283,11 @@ void
prwSS_it->Yaw = supportAngle;
prwSS_it++;
}
-
+ LastFirstPvwSol_ = PreviewedSupportAngles_deq[0];
}
-void
- OrientationsPreview::verify_acceleration_hip_joint(const reference_t & Ref,
+void OrientationsPreview::verify_acceleration_hip_joint(const reference_t & Ref,
const support_state_t & CurrentSupport)
{
if(CurrentSupport.Phase != DS)
@@ -271,8 +304,7 @@ void
}
-bool
- OrientationsPreview::verify_angle_hip_joint(const support_state_t & CurrentSupport,
+bool OrientationsPreview::verify_angle_hip_joint(const support_state_t & CurrentSupport,
double PreviewedTrunkAngleEnd,
const COMState &TrunkState_, COMState &TrunkStateT_,
double CurrentSupportFootAngle,
@@ -306,8 +338,7 @@ bool
}
-void
- OrientationsPreview::verify_velocity_hip_joint(double Time,
+void OrientationsPreview::verify_velocity_hip_joint(double Time,
double PreviewedSupportFoot, double PreviewedSupportAngle,
unsigned StepNumber, const support_state_t & CurrentSupport,
double CurrentRightFootAngle, double CurrentLeftFootAngle,
@@ -368,70 +399,105 @@ void
}
-void
- OrientationsPreview::interpolate_trunk_orientation(double Time, int CurrentIndex,
- double NewSamplingPeriod,
- const deque<support_state_t> & PrwSupportStates_deq,
- deque<COMState> & FinalCOMTraj_deq)
+void OrientationsPreview::interpolate_trunk_orientation(double Time, int CurrentIndex,
+ double NewSamplingPeriod,
+ const deque<support_state_t> & PrwSupportStates_deq,
+ deque<COMState> & FinalCOMTraj_deq)
{
support_state_t CurrentSupport = PrwSupportStates_deq.front();
+ double initPos = FinalCOMTraj_deq[CurrentIndex-1].yaw[0];
+ double initSpeed = FinalCOMTraj_deq[CurrentIndex-1].yaw[1];
+ double initAcc = FinalCOMTraj_deq[CurrentIndex-1].yaw[2];
+ double tT = 0.0 ;
if(CurrentSupport.Phase == SS && Time+1.5*T_ < CurrentSupport.TimeLimit) // CurrentSupport.Phase == SS && Time+3.0/2.0*T_ < CurrentSupport.TimeLimit
{
- //Fourth order polynomial parameters
- double a = TrunkState_.yaw[1];
- double c = 3.0*(TrunkStateT_.yaw[1]-TrunkState_.yaw[1])/(T_*T_);
- double d = -2.0*c/(3.0*T_);
-
- double tT;
- double Theta = TrunkState_.yaw[0];
-
- FinalCOMTraj_deq[CurrentIndex].yaw[0] = TrunkState_.yaw[0];
- FinalCOMTraj_deq[CurrentIndex].yaw[1] = TrunkState_.yaw[1];
- FinalCOMTraj_deq[CurrentIndex].yaw[2] = TrunkState_.yaw[2];
+ TrunkStateYaw_->SetParameters( T_,initPos ,initSpeed , initAcc , TrunkStateT_.yaw[1], 0.0);
//Interpolate the orientation of the trunk
for(int k = 0; k<(int)(T_/NewSamplingPeriod);k++)
{
tT = (double)(k+1)*NewSamplingPeriod;
- if(fabs(TrunkStateT_.yaw[1]-TrunkState_.yaw[1])-0.000001 > 0)
- {
- TrunkState_.yaw[0] = (((1.0/4.0*d*tT+1.0/3.0*c)*
- tT)*tT+a)*tT+Theta;
- TrunkState_.yaw[1] = ((d*tT+c)*tT)*tT+a;
- TrunkState_.yaw[2] = (3.0*d*tT+2.0*c)*tT;
- }
- else
- {
- TrunkState_.yaw[0] += NewSamplingPeriod*TrunkStateT_.yaw[1];
- }
- FinalCOMTraj_deq[CurrentIndex+k].yaw[0] = TrunkState_.yaw[0];
- FinalCOMTraj_deq[CurrentIndex+k].yaw[1] = TrunkState_.yaw[1];
- FinalCOMTraj_deq[CurrentIndex+k].yaw[2] = TrunkState_.yaw[2];
+ FinalCOMTraj_deq[CurrentIndex+k].yaw[0] = TrunkStateYaw_->Compute(tT) ;
+ FinalCOMTraj_deq[CurrentIndex+k].yaw[1] = TrunkStateYaw_->ComputeDerivative(tT) ;
+ FinalCOMTraj_deq[CurrentIndex+k].yaw[2] = TrunkStateYaw_->ComputeSecDerivative(tT) ;
}
}
else if (CurrentSupport.Phase == DS || Time+1.5*T_ > CurrentSupport.TimeLimit)
{
for(int k = 0; k<(int)(T_/NewSamplingPeriod);k++)
{
- FinalCOMTraj_deq[CurrentIndex+k].yaw[0] = TrunkState_.yaw[0];
- FinalCOMTraj_deq[CurrentIndex+k].yaw[1] = TrunkState_.yaw[1];
- FinalCOMTraj_deq[CurrentIndex+k].yaw[2] = TrunkState_.yaw[2];
+ FinalCOMTraj_deq[CurrentIndex+k].yaw[0] = initPos;
+ FinalCOMTraj_deq[CurrentIndex+k].yaw[1] = initSpeed;
+ FinalCOMTraj_deq[CurrentIndex+k].yaw[2] = initAcc;
}
}
-
}
+void OrientationsPreview::one_iteration(double Time,
+ const deque<support_state_t> & PrwSupportStates_deq)
+{
+ support_state_t CurrentSupport = PrwSupportStates_deq.front();
+
+ if(CurrentSupport.Phase == SS && Time+1.5*T_ < CurrentSupport.TimeLimit) // CurrentSupport.Phase == SS && Time+3.0/2.0*T_ < CurrentSupport.TimeLimit
+ {
+ //Fourth order polynomial parameters
+ //Interpolate the orientation of the trunk
+ if(fabs(TrunkStateT_.yaw[1]-TrunkState_.yaw[1])-0.000001 > 0)
+ {
+ double initPos = TrunkState_.yaw[0];
+ double initSpeed = TrunkState_.yaw[1];
+ double initAcc = 0.0;
+ double finalSpeed = TrunkStateT_.yaw[1];
+ double finalAcc = 0.0 ;
+ TrunkStateYaw_->SetParameters( T_, initPos , initSpeed , initAcc , finalSpeed , finalAcc );
+ TrunkState_.yaw[0] = TrunkStateYaw_->Compute(T_);
+ TrunkState_.yaw[1] = TrunkStateYaw_->ComputeDerivative(T_);
+ TrunkState_.yaw[2] = TrunkStateYaw_->ComputeSecDerivative(T_);
+ }
+ else
+ {
+ TrunkState_.yaw[0] += T_*TrunkStateT_.yaw[1];
+ }
+ }
+ else if (CurrentSupport.Phase == DS || Time+1.5*T_ > CurrentSupport.TimeLimit)
+ {
+ }
+}
-double
- OrientationsPreview::f(double a,double b,double c,double d,double x)
+double OrientationsPreview::f(double a,double b,double c,double d,double x)
{return a+b*x+c*x*x+d*x*x*x;}
-double
- OrientationsPreview::df(double ,double b,double c,double d,double x)
+double OrientationsPreview::df(double ,double b,double c,double d,double x)
{return b+2*c*x+3.0*d*x*x;}
+////Fourth order polynomial parameters
+//double initPos = FinalCOMTraj_deq[CurrentIndex-1].yaw[0];
+//double initSpeed = FinalCOMTraj_deq[CurrentIndex-1].yaw[1];
+//double initAcc = FinalCOMTraj_deq[CurrentIndex-1].yaw[2];
+
+//TrunkStateYaw_->SetParameters( T_,initPos ,initSpeed , initAcc , TrunkStateT_.yaw[1], 0.0);
+//FinalCOMTraj_deq[CurrentIndex].yaw[0] = TrunkState_.yaw[0];
+//FinalCOMTraj_deq[CurrentIndex].yaw[1] = TrunkState_.yaw[1];
+//FinalCOMTraj_deq[CurrentIndex].yaw[2] = TrunkState_.yaw[2];
+
+//cout << "interpolation\n" << endl ;
+////Interpolate the orientation of the trunk
+//for(int k = 0; k<(int)(T_/NewSamplingPeriod);k++)
+//{
+// if(fabs(TrunkStateT_.yaw[1]-TrunkState_.yaw[1])-0.000001 > 0)
+// {
+// FinalCOMTraj_deq[CurrentIndex+k].yaw[0] = TrunkStateYaw_->Compute(tT) ;
+// FinalCOMTraj_deq[CurrentIndex+k].yaw[1] = TrunkStateYaw_->ComputeDerivative(tT) ;
+// FinalCOMTraj_deq[CurrentIndex+k].yaw[2] = TrunkStateYaw_->ComputeSecDerivative(tT) ;
+// }
+// else
+// {
+// TrunkState_.yaw[0] += NewSamplingPeriod*TrunkStateT_.yaw[1];
+// }
+
+//}
\ No newline at end of file
diff --git a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh
index c1e5f96d65e78d030f63139a0c26c7ab11e8fe42..e690b9279093a217f803cb9c9627c2d34196b1d5 100644
--- a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh
+++ b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh
@@ -89,6 +89,17 @@ namespace PatternGeneratorJRL
const std::deque<support_state_t> & PrwSupportStates_deq,
std::deque<COMState> & FinalCOMTraj_deq);
+ /// \brief Compute the current state for the preview of the orientation
+ ///
+ /// \param[in] Time
+ /// \param[in] CurrentIndex
+ /// \param[in] NewSamplingPeriod
+ /// \param[in] PrwSupportStates_deq
+ /// \param[out] FinalCOMTraj_deq
+ void one_iteration(double Time,
+ const std::deque<support_state_t> & PrwSupportStates_deq);
+
+
/// \name Accessors
/// \{
inline COMState const & CurrentTrunkState() const
@@ -215,6 +226,9 @@ namespace PatternGeneratorJRL
/// \brief
double SupportTimePassed_;
+ /// \brief
+ double LastFirstPvwSol_ ;
+
/// \brief Numerical precision
const static double EPS_;
@@ -223,8 +237,7 @@ namespace PatternGeneratorJRL
/// \brief State of the trunk at the first previewed sampling
COMState TrunkStateT_;
- Polynome5 * TrunkStateTheta_ ;
-
+ Polynome4 * TrunkStateYaw_ ;
};
}
#endif /* ORIENTATIONSPREVIEW_H_ */
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index b117cead058943f9d00a5c49cd95c9ce23b72259..25ab1464e235e919e634b16e11b4382ac566faa9 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -477,7 +477,6 @@ void ZMPVelocityReferencedQP::OnLine(double time,
deque<FootAbsolutePosition> & FinalRightFootTraj_deq)
{
- cout << "time = " << time << endl ;
// If on-line mode not activated we go out.
if (!m_OnLineMode)
{
@@ -648,6 +647,8 @@ void ZMPVelocityReferencedQP::ControlInterpolation(
// INTERPOLATE TRUNK ORIENTATION:
// ------------------------------
+ OrientPrw_->one_iteration(time,Solution_.SupportStates_deq);
+
OrientPrw_->interpolate_trunk_orientation( time, CurrentIndex_,
m_SamplingPeriod, Solution_.SupportStates_deq,
FinalCOMTraj_deq );
@@ -656,17 +657,16 @@ void ZMPVelocityReferencedQP::ControlInterpolation(
// INTERPOLATE THE COMPUTED FOOT POSITIONS:
// ----------------------------------------
+ int index_orientation = Solution_.SupportStates_deq[1].StepNumber ;
+ if (time+1.5*QP_T_ > Solution_.SupportStates_deq.front().TimeLimit)
+ cout << "ds" << " " << Solution_.SupportStates_deq.front().StateChanged << " " << (bool)(time+1.5*QP_T_ > Solution_.SupportStates_deq.front().TimeLimit) << " " << Solution_.SupportOrientations_deq[0] << endl ;
+ else
+ cout << Solution_.SupportOrientations_deq[index_orientation] << " " << "orient foot sol = " << Solution_.SupportOrientations_deq[0] << endl ;
+
OFTG_control_->interpolate_feet_positions( time,
Solution_.SupportStates_deq, Solution_,
Solution_.SupportOrientations_deq,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq);
-
- for(unsigned int i = 0 ; i<FinalCOMTraj_deq.size() ; ++i)
- {
- FinalCOMTraj_deq[i].yaw[0] = 0.5*(FinalRightFootTraj_deq[i].theta +FinalLeftFootTraj_deq[i].theta );
- FinalCOMTraj_deq[i].yaw[1] = 0.5*(FinalRightFootTraj_deq[i].dtheta +FinalLeftFootTraj_deq[i].dtheta );
- FinalCOMTraj_deq[i].yaw[2] = 0.5*(FinalRightFootTraj_deq[i].ddtheta+FinalLeftFootTraj_deq[i].ddtheta );
- }
return ;
}
@@ -687,27 +687,24 @@ void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
// Copy the solution for the orientation interpolation function
OFTG_DF_->SetSamplingPeriod( m_SamplingPeriod );
- solution_t aSolution = Solution_ ;
+ solution_ = Solution_ ;
//solution_.SupportStates_deq.pop_front();
for ( int i = 0 ; i < QP_N_ ; i++ )
{
- aSolution.SupportOrientations_deq.clear();
- OrientPrw_DF_->preview_orientations( time + i * QP_T_, VelRef_,
- SupportFSM_->StepPeriod(),
- LeftFootTraj_deq_ctrl_, RightFootTraj_deq_ctrl_,
- aSolution);
-
- OrientPrw_DF_->interpolate_trunk_orientation( time + i * QP_T_,
- CurrentIndex_ + i * NbSampleControl_, m_SamplingPeriod,
- solution_.SupportStates_deq, COMTraj_deq_ctrl_ );
+ OrientPrw_->interpolate_trunk_orientation( time + i * QP_T_,
+ CurrentIndex_ + i * NbSampleControl_, m_SamplingPeriod,
+ solution_.SupportStates_deq, COMTraj_deq_ctrl_ );
// Modify a copy of the solution to allow "OFTG_DF_->interpolate_feet_positions"
// to use the correcte feet step previewed
PrepareSolution();
+
+ int index_orientation = Solution_.SupportStates_deq[1].StepNumber ;
+ cout << "Preview : orient foot sol = " << solution_.SupportOrientations_deq[index_orientation] << endl ;
OFTG_DF_->interpolate_feet_positions( time + i * QP_T_,
solution_.SupportStates_deq, solution_,
- aSolution.SupportOrientations_deq,
+ solution_.SupportOrientations_deq,
LeftFootTraj_deq_ctrl_, RightFootTraj_deq_ctrl_);
solution_.SupportStates_deq.pop_front();
}
@@ -717,12 +714,6 @@ void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
OFTG_DF_->copyPolynomesFromFTGS(OFTG_control_);
- for(unsigned int i = 0 ; i<QP_N_*NbSampleControl_ ; ++i)
- {
- COMTraj_deq_ctrl_[i].yaw[0] = 0.5*(LeftFootTraj_deq_ctrl_[i].theta +RightFootTraj_deq_ctrl_[i].theta );
- COMTraj_deq_ctrl_[i].yaw[1] = 0.5*(LeftFootTraj_deq_ctrl_[i].dtheta +RightFootTraj_deq_ctrl_[i].dtheta );
- COMTraj_deq_ctrl_[i].yaw[2] = 0.5*(LeftFootTraj_deq_ctrl_[i].ddtheta+RightFootTraj_deq_ctrl_[i].ddtheta );
- }
return ;
}
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index 5203b73fb389b17582bbb9f59d4b34af038acd2e..64972c7d0aa9f85cbe456ba173d93352df2460bb 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -300,7 +300,7 @@ protected:
<< 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[0]*M_PI/180 ) << " " // 5
+ << filterprecision(m_OneStep.finalCOMPosition.yaw[0] ) << " " // 5
<< filterprecision(m_OneStep.finalCOMPosition.x[1] ) << " " // 6
<< filterprecision(m_OneStep.finalCOMPosition.y[1] ) << " " // 7
<< filterprecision(m_OneStep.finalCOMPosition.z[1] ) << " " // 8
@@ -320,21 +320,25 @@ protected:
<< filterprecision(m_OneStep.LeftFootPosition.ddx ) << " " // 22
<< filterprecision(m_OneStep.LeftFootPosition.ddy ) << " " // 23
<< filterprecision(m_OneStep.LeftFootPosition.ddz ) << " " // 24
- << filterprecision(m_OneStep.LeftFootPosition.theta*M_PI/180 ) << " " // 25
- << filterprecision(m_OneStep.LeftFootPosition.omega ) << " " // 26
- << filterprecision(m_OneStep.LeftFootPosition.omega2 ) << " " // 27
- << filterprecision(m_OneStep.RightFootPosition.x ) << " " // 28
- << filterprecision(m_OneStep.RightFootPosition.y ) << " " // 29
- << filterprecision(m_OneStep.RightFootPosition.z ) << " " // 30
- << filterprecision(m_OneStep.RightFootPosition.dx ) << " " // 31
- << filterprecision(m_OneStep.RightFootPosition.dy ) << " " // 32
- << filterprecision(m_OneStep.RightFootPosition.dz ) << " " // 33
- << filterprecision(m_OneStep.RightFootPosition.ddx ) << " " // 34
- << filterprecision(m_OneStep.RightFootPosition.ddy ) << " " // 35
- << filterprecision(m_OneStep.RightFootPosition.ddz ) << " " // 36
- << filterprecision(m_OneStep.RightFootPosition.theta*M_PI/180 ) << " " // 37
- << filterprecision(m_OneStep.RightFootPosition.omega ) << " " // 38
- << filterprecision(m_OneStep.RightFootPosition.omega2 ) << " " ;// 39
+ << filterprecision(m_OneStep.LeftFootPosition.theta ) << " " // 25
+ << filterprecision(m_OneStep.LeftFootPosition.dtheta ) << " " // 26
+ << filterprecision(m_OneStep.LeftFootPosition.ddtheta ) << " " // 27
+ << filterprecision(m_OneStep.LeftFootPosition.omega ) << " " // 28
+ << filterprecision(m_OneStep.LeftFootPosition.omega2 ) << " " // 29
+ << filterprecision(m_OneStep.RightFootPosition.x ) << " " // 30
+ << filterprecision(m_OneStep.RightFootPosition.y ) << " " // 31
+ << filterprecision(m_OneStep.RightFootPosition.z ) << " " // 32
+ << filterprecision(m_OneStep.RightFootPosition.dx ) << " " // 33
+ << filterprecision(m_OneStep.RightFootPosition.dy ) << " " // 34
+ << filterprecision(m_OneStep.RightFootPosition.dz ) << " " // 35
+ << filterprecision(m_OneStep.RightFootPosition.ddx ) << " " // 36
+ << filterprecision(m_OneStep.RightFootPosition.ddy ) << " " // 37
+ << filterprecision(m_OneStep.RightFootPosition.ddz ) << " " // 38
+ << filterprecision(m_OneStep.RightFootPosition.theta ) << " " // 39
+ << filterprecision(m_OneStep.RightFootPosition.dtheta ) << " " // 40
+ << filterprecision(m_OneStep.RightFootPosition.ddtheta ) << " " // 41
+ << filterprecision(m_OneStep.RightFootPosition.omega ) << " " // 42
+ << filterprecision(m_OneStep.RightFootPosition.omega2 ) << " " ;// 43
aof << endl;
aof.close();
}