From e03881c4e968b0ae9e74f4ba6401a5b652cbe451 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Sat, 27 Apr 2019 09:42:47 +0200
Subject: [PATCH] [FootTrajectoryGeneraton] Enforce 80 lines column Remove
warnings
---
include/jrl/walkgen/pinocchiorobot.hh | 6 +-
.../FootTrajectoryGenerationMultiple.cpp | 311 +++++----
.../FootTrajectoryGenerationStandard.cpp | 633 ++++++++++--------
...dRightFootTrajectoryGenerationMultiple.cpp | 317 +++++----
4 files changed, 750 insertions(+), 517 deletions(-)
diff --git a/include/jrl/walkgen/pinocchiorobot.hh b/include/jrl/walkgen/pinocchiorobot.hh
index 5717748a..71dc6c33 100644
--- a/include/jrl/walkgen/pinocchiorobot.hh
+++ b/include/jrl/walkgen/pinocchiorobot.hh
@@ -186,11 +186,11 @@ namespace PatternGeneratorJRL
inline pinocchio::JointModelVector & getActuatedJoints()
{return m_robotModel->joints;}
- inline Eigen::VectorXd currentConfiguration()
+ inline Eigen::VectorXd & currentConfiguration()
{return m_qmal;}
- inline Eigen::VectorXd currentVelocity()
+ inline Eigen::VectorXd & currentVelocity()
{return m_vmal;}
- inline Eigen::VectorXd currentAcceleration()
+ inline Eigen::VectorXd & currentAcceleration()
{return m_amal;}
inline unsigned numberDof()
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
index 1edd36cc..1caa2ebd 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
@@ -32,25 +32,33 @@
using namespace PatternGeneratorJRL;
-FootTrajectoryGenerationMultiple::FootTrajectoryGenerationMultiple(SimplePluginManager *lSPM,
- PRFoot *aFoot)
+FootTrajectoryGenerationMultiple::
+FootTrajectoryGenerationMultiple
+(SimplePluginManager *lSPM,
+ PRFoot *aFoot)
: SimplePlugin(lSPM)
{
m_Foot = aFoot ;
m_Sensitivity=0.0;
}
-FootTrajectoryGenerationMultiple::~FootTrajectoryGenerationMultiple()
+FootTrajectoryGenerationMultiple::
+~FootTrajectoryGenerationMultiple()
{
- for(unsigned int i=0;i<m_SetOfFootTrajectoryGenerationObjects.size();i++)
+ for(unsigned int i=0;
+ i<m_SetOfFootTrajectoryGenerationObjects.size();
+ i++)
{
delete m_SetOfFootTrajectoryGenerationObjects[i];
}
}
-void FootTrajectoryGenerationMultiple::SetNumberOfIntervals(int lNumberOfIntervals)
+void FootTrajectoryGenerationMultiple::
+SetNumberOfIntervals
+(int lNumberOfIntervals)
{
- if (m_SetOfFootTrajectoryGenerationObjects.size()==(unsigned int)lNumberOfIntervals)
+ if (m_SetOfFootTrajectoryGenerationObjects.size()==
+ (unsigned int)lNumberOfIntervals)
return;
for(unsigned int i=0;i<m_SetOfFootTrajectoryGenerationObjects.size();i++)
@@ -59,21 +67,29 @@ void FootTrajectoryGenerationMultiple::SetNumberOfIntervals(int lNumberOfInterva
}
m_SetOfFootTrajectoryGenerationObjects.resize(lNumberOfIntervals);
- for(unsigned int i=0;i<m_SetOfFootTrajectoryGenerationObjects.size();i++)
+ for(unsigned int i=0;
+ i<m_SetOfFootTrajectoryGenerationObjects.size();
+ i++)
{
- m_SetOfFootTrajectoryGenerationObjects[i] = new FootTrajectoryGenerationStandard(getSimplePluginManager(),m_Foot);
- m_SetOfFootTrajectoryGenerationObjects[i]->InitializeInternalDataStructures();
+ m_SetOfFootTrajectoryGenerationObjects[i] =
+ new FootTrajectoryGenerationStandard
+ (getSimplePluginManager(),m_Foot);
+ m_SetOfFootTrajectoryGenerationObjects[i]->
+ InitializeInternalDataStructures();
}
m_NatureOfIntervals.resize(lNumberOfIntervals);
}
-int FootTrajectoryGenerationMultiple::GetNumberOfIntervals() const
+int FootTrajectoryGenerationMultiple::
+GetNumberOfIntervals() const
{
return static_cast<int>(m_SetOfFootTrajectoryGenerationObjects.size());
}
-void FootTrajectoryGenerationMultiple::SetTimeIntervals(const vector<double> &lDeltaTj)
+void FootTrajectoryGenerationMultiple::
+SetTimeIntervals
+(const vector<double> &lDeltaTj)
{
m_DeltaTj = lDeltaTj;
m_RefTime.resize(lDeltaTj.size());
@@ -82,37 +98,47 @@ void FootTrajectoryGenerationMultiple::SetTimeIntervals(const vector<double> &lD
for(unsigned int li=0;li<m_DeltaTj.size();li++)
{
m_RefTime[li] = reftime;
- ODEBUG(" m_RefTime["<< li <<"]: " << setprecision(12) << m_RefTime[li] << " reftime: "<< setprecision(12) << reftime );
+ ODEBUG(" m_RefTime["<< li <<"]: " << setprecision(12)
+ << m_RefTime[li] << " reftime: "
+ << setprecision(12) << reftime );
reftime+=m_DeltaTj[li];
}
}
-void FootTrajectoryGenerationMultiple::GetTimeIntervals(vector<double> &lDeltaTj) const
+void FootTrajectoryGenerationMultiple::
+GetTimeIntervals(vector<double> &lDeltaTj)
+ const
{
lDeltaTj = m_DeltaTj;
}
-bool FootTrajectoryGenerationMultiple::Compute(int axis, double t, double &result)
+bool FootTrajectoryGenerationMultiple::
+Compute(int axis, double t, double &result)
{
t -= m_AbsoluteTimeReference;
result = -1.0;
double reftime=0;
ODEBUG(" ====== CoM ====== ");
- ODEBUG(" t: " << t << " reftime :" << reftime << " m_Sensitivity: "<< m_Sensitivity <<" m_DeltaTj.size(): "<< m_DeltaTj.size() );
+ ODEBUG(" t: " << t << " reftime :" << reftime << " m_Sensitivity: "
+ << m_Sensitivity <<" m_DeltaTj.size(): "<< m_DeltaTj.size() );
for(unsigned int j=0;j<m_DeltaTj.size();j++)
{
- ODEBUG(" t: " << t << " reftime :" << reftime << " Tj["<<j << "]= "<< m_DeltaTj[j]);
+ ODEBUG(" t: " << t << " reftime :" << reftime
+ << " Tj["<<j << "]= "<< m_DeltaTj[j]);
- if (((t+m_Sensitivity)>=reftime) && (t<=reftime+m_DeltaTj[j]+m_Sensitivity))
+ if (((t+m_Sensitivity)>=reftime) &&
+ (t<=reftime+m_DeltaTj[j]+m_Sensitivity))
{
double deltaj=0.0;
deltaj = t-reftime;
if (m_SetOfFootTrajectoryGenerationObjects[j]!=0)
{
- result = m_SetOfFootTrajectoryGenerationObjects[j]->Compute(axis,deltaj);
+ result =
+ m_SetOfFootTrajectoryGenerationObjects[j]->
+ Compute(axis,deltaj);
}
return true;
}
@@ -125,62 +151,88 @@ bool FootTrajectoryGenerationMultiple::Compute(int axis, double t, double &resul
}
-bool FootTrajectoryGenerationMultiple::Compute(double t, FootAbsolutePosition & aFootAbsolutePosition, unsigned int IndexInterval)
+bool FootTrajectoryGenerationMultiple::
+Compute
+(double t,
+ FootAbsolutePosition & aFootAbsolutePosition,
+ unsigned int IndexInterval)
{
- double deltaj = t - m_AbsoluteTimeReference - m_RefTime[IndexInterval];
+ double deltaj =
+ t - m_AbsoluteTimeReference - m_RefTime[IndexInterval];
ODEBUG("IndexInterval : " << IndexInterval );
// Use polynoms
- //m_SetOfFootTrajectoryGenerationObjects[IndexInterval]->ComputeAllWithPolynom(aFootAbsolutePosition,deltaj);
+ //m_SetOfFootTrajectoryGenerationObjects[IndexInterval]->
+ // ComputeAllWithPolynom(aFootAbsolutePosition,deltaj);
// Use BSplines
- m_SetOfFootTrajectoryGenerationObjects[IndexInterval]->ComputeAllWithBSplines(aFootAbsolutePosition,deltaj);
+ m_SetOfFootTrajectoryGenerationObjects[IndexInterval]->
+ ComputeAllWithBSplines(aFootAbsolutePosition,deltaj);
- aFootAbsolutePosition.stepType = m_NatureOfIntervals[IndexInterval];
+ aFootAbsolutePosition.stepType =
+ m_NatureOfIntervals[IndexInterval];
/* ofstream aof;
string aFileName;
aFileName ="ex.dat";
aof.open(aFileName.c_str(),ofstream::app);
- aof << "deltaj " << deltaj << " t " << t << " m_Absoulute " << m_AbsoluteTimeReference << " Ref " << m_RefTime[IndexInterval] << " j " << IndexInterval << " foot "<< aFootAbsolutePosition.x << " "<< aFootAbsolutePosition.z << endl;
+ aof << "deltaj " << deltaj << " t " << t << " m_Absoulute "
+ << m_AbsoluteTimeReference << " Ref " << m_RefTime[IndexInterval]
+ << " j " << IndexInterval << " foot "<< aFootAbsolutePosition.x
+ << " "<< aFootAbsolutePosition.z << endl;
aof.close();*/
return true;
}
-bool FootTrajectoryGenerationMultiple::Compute(double t, FootAbsolutePosition & aFootAbsolutePosition)
+bool FootTrajectoryGenerationMultiple::
+Compute
+(double t, FootAbsolutePosition & aFootAbsolutePosition)
{
t -= m_AbsoluteTimeReference;
double reftime=0;
ODEBUG(" ====== Foot ====== " << m_DeltaTj.size());
- ODEBUG("t: " << setprecision(12) << t << " reftime :" << reftime <<
- " m_Sensitivity: "<< m_Sensitivity <<" m_DeltaTj.size(): "<< m_DeltaTj.size() );
+ ODEBUG("t: " << setprecision(12) << t
+ << " reftime :" << reftime <<
+ " m_Sensitivity: "<< m_Sensitivity
+ <<" m_DeltaTj.size(): "<< m_DeltaTj.size() );
for(unsigned int j=0;j<m_DeltaTj.size();j++)
{
- ODEBUG("t: " << t << " reftime :" << setprecision(12) << reftime <<
- " Tj["<<j << "]= " << setprecision(12) << m_DeltaTj[j]
- <<" max limit: " << setprecision(12) << (reftime+m_DeltaTj[j]+m_Sensitivity) );
+ ODEBUG("t: " << t << " reftime :"
+ << setprecision(12) << reftime <<
+ " Tj["<<j << "]= " << setprecision(12)
+ << m_DeltaTj[j]
+ <<" max limit: " << setprecision(12)
+ << (reftime+m_DeltaTj[j]+m_Sensitivity) );
+
ODEBUG(" Tj["<<j << "]= " << setprecision(12) << m_DeltaTj[j] );
- if (((t+m_Sensitivity)>=reftime) && (t<=reftime+m_DeltaTj[j]+m_Sensitivity))
+
+ if (((t+m_Sensitivity)>=reftime) &&
+ (t<=reftime+m_DeltaTj[j]+m_Sensitivity))
{
double deltaj=0.0;
deltaj = t-reftime;
if (m_SetOfFootTrajectoryGenerationObjects[j]!=0)
{
- //m_SetOfFootTrajectoryGenerationObjects[j]->ComputeAllWithPolynom(aFootAbsolutePosition,deltaj);
- m_SetOfFootTrajectoryGenerationObjects[j]->ComputeAllWithBSplines(aFootAbsolutePosition,deltaj);
+ //m_SetOfFootTrajectoryGenerationObjects[j]->
+ //ComputeAllWithPolynom(aFootAbsolutePosition,deltaj);
+ m_SetOfFootTrajectoryGenerationObjects[j]->
+ ComputeAllWithBSplines(aFootAbsolutePosition,deltaj);
aFootAbsolutePosition.stepType = m_NatureOfIntervals[j];
}
- ODEBUG("t: " << t << " reftime :" << setprecision(12) << reftime <<
- " AbsoluteTimeReference : " << m_AbsoluteTimeReference <<
- " Tj["<<j << "]= " << setprecision(12) << m_DeltaTj[j]
- <<" max limit: " << setprecision(12) << (reftime+m_DeltaTj[j]+m_Sensitivity) );
+ ODEBUG("t: " << t << " reftime :" << setprecision(12)
+ << reftime
+ << " AbsoluteTimeReference : "
+ << m_AbsoluteTimeReference
+ << " Tj["<<j << "]= " << setprecision(12) << m_DeltaTj[j]
+ <<" max limit: " << setprecision(12)
+ << (reftime+m_DeltaTj[j]+m_Sensitivity) );
ODEBUG("X: " << aFootAbsolutePosition.x <<
" Y: " << aFootAbsolutePosition.y <<
" Z: " << aFootAbsolutePosition.z <<
@@ -197,16 +249,19 @@ bool FootTrajectoryGenerationMultiple::Compute(double t, FootAbsolutePosition &
}
ODEBUG(" reftime :" << reftime <<
" m_AbsoluteReferenceTime" << m_AbsoluteTimeReference);
- ODEBUG("t: " << setprecision(12) << t << " reftime :" << reftime <<
- " m_Sensitivity: "<< m_Sensitivity <<" m_DeltaTj.size(): "<< m_DeltaTj.size() );
+ ODEBUG("t: " << setprecision(12) << t
+ << " reftime :" << reftime
+ << " m_Sensitivity: "<< m_Sensitivity
+ << " m_DeltaTj.size(): "<< m_DeltaTj.size() );
return false;
}
/*! This method specifies the nature of the interval.
*/
-int FootTrajectoryGenerationMultiple::SetNatureInterval(unsigned int IntervalIndex,
- int Nature)
+int FootTrajectoryGenerationMultiple::
+SetNatureInterval(unsigned int IntervalIndex,
+ int Nature)
{
if (IntervalIndex>=m_NatureOfIntervals.size())
return -1;
@@ -217,7 +272,8 @@ int FootTrajectoryGenerationMultiple::SetNatureInterval(unsigned int IntervalInd
/*! This method returns the nature of the interval.
*/
-int FootTrajectoryGenerationMultiple::GetNatureInterval(unsigned int IntervalIndex) const
+int FootTrajectoryGenerationMultiple::
+GetNatureInterval(unsigned int IntervalIndex) const
{
if (IntervalIndex>=m_NatureOfIntervals.size())
return -100;
@@ -226,34 +282,39 @@ int FootTrajectoryGenerationMultiple::GetNatureInterval(unsigned int IntervalInd
}
-/*! This method specifies the parameters for each of the polynome used by this
- object. In this case, as it is used for the 3rd order polynome. The polynome to
- which those parameters are set is specified with PolynomeIndex.
+/*! This method specifies the parameters for each of the polynome
+ used by this object. In this case, as it is used for the 3rd
+ order polynome. The polynome to which those parameters are set
+ is specified with PolynomeIndex.
@param PolynomeIndex: Set to which axis the parameters will be applied.
@param TimeInterval: Set the time base of the polynome.
@param Position: Set the final position of the polynome at TimeInterval.
@param InitPosition: Initial position when computing the polynome at t=0.0.
@param InitSpeed: Initial speed when computing the polynome at t=0.0.
*/
-int FootTrajectoryGenerationMultiple::SetParametersWithInitPosInitSpeed(unsigned int IntervalIndex,
- int AxisReference,
- double TimeInterval,
- double FinalPosition,
- double InitPosition,
- double InitSpeed,
- vector<double> middlePos
- )
+int FootTrajectoryGenerationMultiple::
+SetParametersWithInitPosInitSpeed
+(unsigned int IntervalIndex,
+ int AxisReference,
+ double TimeInterval,
+ double FinalPosition,
+ double InitPosition,
+ double InitSpeed,
+ vector<double> middlePos
+ )
{
- //std::cout << "Entro Pol " << std::endl;
- if (IntervalIndex>=m_SetOfFootTrajectoryGenerationObjects.size())
+ if (IntervalIndex>=
+ m_SetOfFootTrajectoryGenerationObjects.size())
return -1;
- m_SetOfFootTrajectoryGenerationObjects[IntervalIndex]->SetParametersWithInitPosInitSpeed(AxisReference,
- TimeInterval,
- FinalPosition,
- InitPosition,
- InitSpeed,
- middlePos);
+ m_SetOfFootTrajectoryGenerationObjects[IntervalIndex]->
+ SetParametersWithInitPosInitSpeed
+ (AxisReference,
+ TimeInterval,
+ FinalPosition,
+ InitPosition,
+ InitSpeed,
+ middlePos);
return 0;
}
@@ -265,105 +326,128 @@ int FootTrajectoryGenerationMultiple::SetParametersWithInitPosInitSpeed(unsigned
@param TimeInterval: Set the time base of the polynome.
@param Position: Set the final position of the polynome at TimeInterval.
*/
-int FootTrajectoryGenerationMultiple::SetParameters(unsigned int IntervalIndex,
- int AxisReference,
- double TimeInterval,
- double FinalPosition)
+int FootTrajectoryGenerationMultiple::
+SetParameters
+(unsigned int IntervalIndex,
+ int AxisReference,
+ double TimeInterval,
+ double FinalPosition)
{
- if (IntervalIndex>=m_SetOfFootTrajectoryGenerationObjects.size())
+ if (IntervalIndex>=
+ m_SetOfFootTrajectoryGenerationObjects.size())
return -1;
- return SetParametersWithInitPosInitSpeedInitAcc( IntervalIndex,
- AxisReference,
- TimeInterval,
- FinalPosition,
- 0.0,0.0,0.0 );
+ return SetParametersWithInitPosInitSpeedInitAcc
+ ( IntervalIndex,
+ AxisReference,
+ TimeInterval,
+ FinalPosition,
+ 0.0,0.0,0.0 );
}
/*! This method specifies the parameters for each of the polynome used by this
- object. In this case, as it is used for the 3rd order polynome. The polynome to
- which those parameters are set is specified with PolynomeIndex.
+ object. In this case, as it is used for the 3rd order polynome. The polynome
+ to which those parameters are set is specified with PolynomeIndex.
@param PolynomeIndex: Set to which axis the parameters will be applied.
@param AxisReference: Index to the axis to be used.
@param TimeInterval: Set the time base of the polynome.
@param FinalPosition: Set the final position of the polynome at TimeInterval.
- @param InitPosition: Initial position when computing the polynome at t= m_AbsoluteTimeReference.
- @param InitSpeed: Initial speed when computing the polynome at t=m_AbsoluteTimeReference.
- @param InitAcc: Initial speed when computing the polynome at t=m_AbsoluteTimeReference.
+ @param InitPosition: Initial position when computing the polynome at
+ t= m_AbsoluteTimeReference.
+ @param InitSpeed: Initial speed when computing the polynome at
+ t=m_AbsoluteTimeReference.
+ @param InitAcc: Initial speed when computing the polynome at
+ t=m_AbsoluteTimeReference.
*/
-int FootTrajectoryGenerationMultiple::SetParametersWithInitPosInitSpeedInitAcc(unsigned int IntervalIndex,
- int AxisReference,
- double TimeInterval,
- double FinalPosition,
- double InitPosition,
- double InitSpeed,
- double InitAcc,
- vector<double> middlePos
- )
+int FootTrajectoryGenerationMultiple::
+SetParametersWithInitPosInitSpeedInitAcc
+(unsigned int IntervalIndex,
+ int AxisReference,
+ double TimeInterval,
+ double FinalPosition,
+ double InitPosition,
+ double InitSpeed,
+ double InitAcc,
+ vector<double> middlePos)
{
- if (IntervalIndex>=m_SetOfFootTrajectoryGenerationObjects.size())
+ if (IntervalIndex>=
+ m_SetOfFootTrajectoryGenerationObjects.size())
return -1;
- m_SetOfFootTrajectoryGenerationObjects[IntervalIndex]->SetParameters(AxisReference,
- TimeInterval,
- FinalPosition,
- InitPosition,
- InitSpeed,
- InitAcc,
- middlePos);
+ m_SetOfFootTrajectoryGenerationObjects[IntervalIndex]->
+ SetParameters
+ (AxisReference,
+ TimeInterval,
+ FinalPosition,
+ InitPosition,
+ InitSpeed,
+ InitAcc,
+ middlePos);
return 0;
}
/*! This method get the parameters for each of the polynome used by this
- object. In this case, as it is used for the 3rd order polynome. The polynome to
- which those parameters are set is specified with PolynomeIndex.
+ object. In this case, as it is used for the 3rd order polynome. The polynome
+ to which those parameters are set is specified with PolynomeIndex.
@param PolynomeIndex: Set to which axis the parameters will be applied.
@param TimeInterval: Set the time base of the polynome.
@param Position: Set the final position of the polynome at TimeInterval.
@param InitPosition: Initial position when computing the polynome at t=0.0.
@param InitSpeed: Initial speed when computing the polynome at t=0.0.
*/
-int FootTrajectoryGenerationMultiple::GetParametersWithInitPosInitSpeed(unsigned int IntervalIndex,
- int AxisReference,
- double &TimeInterval,
- double &FinalPosition,
- double &InitPosition,
- double &InitSpeed)
+int FootTrajectoryGenerationMultiple::
+GetParametersWithInitPosInitSpeed
+(unsigned int IntervalIndex,
+ int AxisReference,
+ double &TimeInterval,
+ double &FinalPosition,
+ double &InitPosition,
+ double &InitSpeed)
{
- if (IntervalIndex>=m_SetOfFootTrajectoryGenerationObjects.size())
+ if (IntervalIndex>=
+ m_SetOfFootTrajectoryGenerationObjects.size())
return -1;
- m_SetOfFootTrajectoryGenerationObjects[IntervalIndex]->GetParametersWithInitPosInitSpeed(AxisReference,
- TimeInterval,
- FinalPosition,
- InitPosition,
- InitSpeed);
+ m_SetOfFootTrajectoryGenerationObjects[IntervalIndex]->
+ GetParametersWithInitPosInitSpeed
+ (AxisReference,
+ TimeInterval,
+ FinalPosition,
+ InitPosition,
+ InitSpeed);
+
return 0;
}
-double FootTrajectoryGenerationMultiple::GetAbsoluteTimeReference() const
+double FootTrajectoryGenerationMultiple::
+GetAbsoluteTimeReference() const
{
return m_AbsoluteTimeReference;
}
-void FootTrajectoryGenerationMultiple::SetAbsoluteTimeReference(double lAbsoluteTimeReference)
+void FootTrajectoryGenerationMultiple::
+SetAbsoluteTimeReference
+(double lAbsoluteTimeReference)
{
m_AbsoluteTimeReference = lAbsoluteTimeReference;
}
-void FootTrajectoryGenerationMultiple::CallMethod(std::string &, //Method,
- std::istringstream & ) //strm)
+void FootTrajectoryGenerationMultiple::
+CallMethod
+(std::string &, //Method,
+ std::istringstream & ) //strm)
{
}
-int FootTrajectoryGenerationMultiple::DisplayIntervals() const
+int FootTrajectoryGenerationMultiple::
+DisplayIntervals() const
{
for(unsigned int i=0;i<m_DeltaTj.size();i++)
{
- ODEBUG3("m_DeltaTj["<<i<<"]="<<m_DeltaTj[i]);
+ std::cout << "m_DeltaTj["<<i<<"]="<<m_DeltaTj[i] << std::endl;
}
return 0;
}
@@ -406,4 +490,3 @@ FootTrajectoryGenerationMultiple::operator=
}
return *this;
}
-
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
index ad9ca5e7..409fbf60 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
@@ -33,8 +33,10 @@
using namespace PatternGeneratorJRL;
-FootTrajectoryGenerationStandard::FootTrajectoryGenerationStandard(SimplePluginManager *lSPM,
- PRFoot *aFoot)
+FootTrajectoryGenerationStandard::
+FootTrajectoryGenerationStandard
+(SimplePluginManager *lSPM,
+ PRFoot *aFoot)
: FootTrajectoryGenerationAbstract(lSPM,aFoot)
{
/* Initialize the pointers to polynomes. */
@@ -55,9 +57,9 @@ FootTrajectoryGenerationStandard::FootTrajectoryGenerationStandard(SimplePluginM
from humanoid specific informations. */
double lWidth=0.0,lDepth=0.0;
if (m_Foot->associatedAnkle!=0)
- {
- lWidth = m_Foot->soleWidth ;
- }
+ {
+ lWidth = m_Foot->soleWidth ;
+ }
else
{
cerr << "Pb no ref Foot." << endl;
@@ -85,10 +87,10 @@ FootTrajectoryGenerationStandard::FootTrajectoryGenerationStandard(SimplePluginM
/* Compute Left foot coordinates */
if (m_Foot->associatedAnkle!=0)
- {
- lWidth = m_Foot->soleWidth ;
- AnklePosition = m_Foot->anklePosition;
- }
+ {
+ lWidth = m_Foot->soleWidth ;
+ AnklePosition = m_Foot->anklePosition;
+ }
else
{
cerr << "Pb no ref Foot." << endl;
@@ -101,7 +103,8 @@ FootTrajectoryGenerationStandard::FootTrajectoryGenerationStandard(SimplePluginM
RESETDEBUG5("GeneratedFoot.dat");
}
-FootTrajectoryGenerationStandard::~FootTrajectoryGenerationStandard()
+FootTrajectoryGenerationStandard::
+~FootTrajectoryGenerationStandard()
{
if (m_PolynomeX!=0)
delete m_PolynomeX;
@@ -131,7 +134,8 @@ FootTrajectoryGenerationStandard::~FootTrajectoryGenerationStandard()
delete m_PolynomeTheta;
}
-void FootTrajectoryGenerationStandard::InitializeInternalDataStructures()
+void FootTrajectoryGenerationStandard::
+InitializeInternalDataStructures()
{
FreeInternalDataStructures();
@@ -148,7 +152,8 @@ void FootTrajectoryGenerationStandard::InitializeInternalDataStructures()
m_PolynomeTheta = new Polynome5(0,0);
}
-void FootTrajectoryGenerationStandard::FreeInternalDataStructures()
+void FootTrajectoryGenerationStandard::
+FreeInternalDataStructures()
{
if (m_PolynomeX!=0)
delete m_PolynomeX;
@@ -180,27 +185,35 @@ void FootTrajectoryGenerationStandard::FreeInternalDataStructures()
}
// Initizialize the parameter to use Polynoms
-int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex,
- double TimeInterval,
- double Position)
+int FootTrajectoryGenerationStandard::
+SetParameters
+(int PolynomeIndex,
+ double TimeInterval,
+ double Position)
{
vector<double> MP (1, Position + m_StepHeight) ;
vector<double> ToMP (1, TimeInterval/3.0) ;
switch (PolynomeIndex)
- {
+ {
case X_AXIS:
m_PolynomeX->SetParameters(TimeInterval,Position);
- //m_BsplinesX->SetParameters(TimeInterval,Position,TimeInterval/3.0,Position + m_StepHeight);
+ //m_BsplinesX->SetParameters(TimeInterval,Position,
+ // TimeInterval/3.0,Position + m_StepHeight);
break;
case Y_AXIS:
m_PolynomeY->SetParameters(TimeInterval,Position);
- //-m_BsplinesY->SetParameters(TimeInterval,Position,TimeInterval/3.0,Position + m_StepHeight);
+ //-m_BsplinesY->SetParameters(TimeInterval,Position,
+ // TimeInterval/3.0,Position + m_StepHeight);
break;
case Z_AXIS:
- m_PolynomeZ->SetParameters(TimeInterval,Position+m_StepHeight,Position);
- m_BsplinesZ->SetParameters(TimeInterval,0.0,Position,ToMP,MP);
+ ODEBUG3("Position: " << Position <<
+ " m_StepHeight: " << m_StepHeight);
+ m_PolynomeZ->SetParameters
+ (TimeInterval,Position+m_StepHeight,Position);
+ m_BsplinesZ->SetParameters
+ (TimeInterval,0.0,Position,ToMP,MP);
break;
case THETA_AXIS:
@@ -218,18 +231,20 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex,
default:
return -1;
break;
- }
+ }
return 0;
}
// Polynoms
-int FootTrajectoryGenerationStandard::SetParametersWithInitPosInitSpeed(int PolynomeIndex,
- double TimeInterval,
- double FinalPosition,
- double InitPosition,
- double InitSpeed,
- vector<double> MiddlePos)
+int FootTrajectoryGenerationStandard::
+SetParametersWithInitPosInitSpeed
+(int PolynomeIndex,
+ double TimeInterval,
+ double FinalPosition,
+ double InitPosition,
+ double InitSpeed,
+ vector<double> MiddlePos)
{
double epsilon = 0.0001;
double WayPoint_x = MiddlePos[0] ;
@@ -239,50 +254,56 @@ int FootTrajectoryGenerationStandard::SetParametersWithInitPosInitSpeed(int Poly
vector<double> MP ;
vector<double> ToMP ;
- bool isWayPointSet = WayPoint_x != WayPoint_y &&
- WayPoint_y != WayPoint_z &&
- WayPoint_x != WayPoint_z ;
-
- bool isFootMoving = abs(m_BsplinesY->FP() - m_BsplinesY->IP())>epsilon ||
- abs(m_BsplinesX->FP() - m_BsplinesX->IP())>epsilon ;
+ bool isWayPointSet =
+ WayPoint_x != WayPoint_y &&
+ WayPoint_y != WayPoint_z &&
+ WayPoint_x != WayPoint_z ;
+
+ bool isFootMoving =
+ abs(m_BsplinesY->FP() - m_BsplinesY->IP())>epsilon ||
+ abs(m_BsplinesX->FP() - m_BsplinesX->IP())>epsilon ;
switch (PolynomeIndex)
- {
+ {
case X_AXIS:
ODEBUG2("Initspeed: " << InitSpeed << " ");
// Init polynom
- m_PolynomeX->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,0.0);
+ m_PolynomeX->SetParameters
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed,0.0);
// Init BSpline
if( isWayPointSet )
- {
- ToMP.push_back(0.20*TimeInterval);
- ToMP.push_back(0.75*TimeInterval);
- MP.push_back(InitPosition) ;
- MP.push_back(FinalPosition) ;
- }
+ {
+ ToMP.push_back(0.20*TimeInterval);
+ ToMP.push_back(0.75*TimeInterval);
+ MP.push_back(InitPosition) ;
+ MP.push_back(FinalPosition) ;
+ }
else
- {
+ {
ToMP.clear();
MP.clear();
- }
- m_BsplinesX->SetParameters(TimeInterval,InitPosition,FinalPosition,ToMP,MP,InitSpeed);
+ }
+ m_BsplinesX->SetParameters
+ (TimeInterval,InitPosition,FinalPosition,ToMP,MP,InitSpeed);
break;
case Y_AXIS:
- m_PolynomeY->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,0.0);
+ m_PolynomeY->SetParameters
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed,0.0);
if(isWayPointSet)
- {
- ToMP.push_back(0.20*TimeInterval);
- ToMP.push_back(0.75*TimeInterval);
- MP.push_back(WayPoint_y);
- MP.push_back(WayPoint_y);
- }
+ {
+ ToMP.push_back(0.20*TimeInterval);
+ ToMP.push_back(0.75*TimeInterval);
+ MP.push_back(WayPoint_y);
+ MP.push_back(WayPoint_y);
+ }
else
- {
- ToMP.clear();
- MP.clear();
- }
- m_BsplinesY->SetParameters(TimeInterval,InitPosition,FinalPosition,ToMP,MP,InitSpeed);
+ {
+ ToMP.clear();
+ MP.clear();
+ }
+ m_BsplinesY->SetParameters
+ (TimeInterval,InitPosition,FinalPosition,ToMP,MP,InitSpeed);
break;
case Z_AXIS:
@@ -290,8 +311,9 @@ int FootTrajectoryGenerationStandard::SetParametersWithInitPosInitSpeed(int Poly
{
WayPoint_z = 0.0;
}
- m_PolynomeZ->SetParametersWithMiddlePos(TimeInterval, FinalPosition+m_StepHeight,
- InitPosition, InitSpeed, 0.0, FinalPosition);
+ m_PolynomeZ->SetParametersWithMiddlePos
+ (TimeInterval, FinalPosition+m_StepHeight,
+ InitPosition, InitSpeed, 0.0, FinalPosition);
// Check the final and the initial position to decide what to do
if (FinalPosition - InitPosition > epsilon )
@@ -299,7 +321,8 @@ int FootTrajectoryGenerationStandard::SetParametersWithInitPosInitSpeed(int Poly
ToMP.push_back(0.4*TimeInterval);
MP.push_back(FinalPosition+WayPoint_z);
}
- else if (FinalPosition - InitPosition <= epsilon && FinalPosition - InitPosition >= -epsilon )
+ else if (FinalPosition - InitPosition <= epsilon &&
+ FinalPosition - InitPosition >= -epsilon )
{
ToMP.push_back(0.5*TimeInterval);
MP.push_back(FinalPosition+WayPoint_z);
@@ -309,31 +332,45 @@ int FootTrajectoryGenerationStandard::SetParametersWithInitPosInitSpeed(int Poly
ToMP.push_back(0.6*TimeInterval);
MP.push_back(InitPosition+WayPoint_z);
}
- m_BsplinesZ->SetParameters(TimeInterval,InitPosition,FinalPosition,ToMP,MP,InitSpeed);
+ m_BsplinesZ->SetParameters
+ (TimeInterval,InitPosition,FinalPosition,ToMP,MP,InitSpeed);
break;
case THETA_AXIS:
- m_PolynomeTheta->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ m_PolynomeTheta->
+ SetParametersWithInitPosInitSpeed
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed);
break;
case OMEGA_AXIS:
- m_PolynomeOmega->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ m_PolynomeOmega->
+ SetParametersWithInitPosInitSpeed
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed);
break;
case OMEGA2_AXIS:
- m_PolynomeOmega2->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ m_PolynomeOmega2->
+ SetParametersWithInitPosInitSpeed
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed);
break;
default:
return -1;
break;
- }
+ }
return 0;
}
// allow C^2 continuity in the interpolation
-int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex, double TimeInterval,
- double FinalPosition, double InitPosition, double InitSpeed, double InitAcc, vector<double> MiddlePos)
+int FootTrajectoryGenerationStandard::
+SetParameters
+(int PolynomeIndex,
+ double TimeInterval,
+ double FinalPosition,
+ double InitPosition,
+ double InitSpeed,
+ double InitAcc,
+ vector<double> MiddlePos)
{
double epsilon = 0.0001;
double WayPoint_x = MiddlePos[0] ;
@@ -343,50 +380,56 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex, double Ti
vector<double> MP ;
vector<double> ToMP ;
- bool isWayPointSet = WayPoint_y!=WayPoint_x &&
- (WayPoint_x*WayPoint_x >= epsilon ||
- WayPoint_y*WayPoint_y >= epsilon) ;
-
- bool isFootMoving = abs(m_BsplinesY->FP() - m_BsplinesY->IP())>epsilon ||
- abs(m_BsplinesX->FP() - m_BsplinesX->IP())>epsilon ;
+ bool isWayPointSet =
+ WayPoint_y!=WayPoint_x &&
+ (WayPoint_x*WayPoint_x >= epsilon ||
+ WayPoint_y*WayPoint_y >= epsilon) ;
+
+ bool isFootMoving =
+ abs(m_BsplinesY->FP() - m_BsplinesY->IP())>epsilon ||
+ abs(m_BsplinesX->FP() - m_BsplinesX->IP())>epsilon ;
switch (PolynomeIndex)
- {
+ {
case X_AXIS:
ODEBUG2("Initspeed: " << InitSpeed << " ");
// Init polynom
- m_PolynomeX->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc);
+ m_PolynomeX->SetParameters
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc);
// Init BSpline
if( isWayPointSet )
- {
- ToMP.push_back(0.20*TimeInterval);
- ToMP.push_back(0.75*TimeInterval);
- MP.push_back(InitPosition) ;
- MP.push_back(FinalPosition) ;
- }
+ {
+ ToMP.push_back(0.20*TimeInterval);
+ ToMP.push_back(0.75*TimeInterval);
+ MP.push_back(InitPosition) ;
+ MP.push_back(FinalPosition) ;
+ }
else
- {
+ {
ToMP.clear();
MP.clear();
- }
- m_BsplinesX->SetParametersWithInitFinalPose(TimeInterval,InitPosition,FinalPosition,ToMP,MP);
+ }
+ m_BsplinesX->SetParametersWithInitFinalPose
+ (TimeInterval,InitPosition,FinalPosition,ToMP,MP);
break;
case Y_AXIS:
- m_PolynomeY->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc);
+ m_PolynomeY->SetParameters
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc);
if(isWayPointSet)
- {
- ToMP.push_back(0.20*TimeInterval);
- ToMP.push_back(0.75*TimeInterval);
- MP.push_back(WayPoint_y);
- MP.push_back(WayPoint_y);
- }
+ {
+ ToMP.push_back(0.20*TimeInterval);
+ ToMP.push_back(0.75*TimeInterval);
+ MP.push_back(WayPoint_y);
+ MP.push_back(WayPoint_y);
+ }
else
- {
- ToMP.clear();
- MP.clear();
- }
- m_BsplinesY->SetParametersWithInitFinalPose(TimeInterval,InitPosition,FinalPosition,ToMP,MP);
+ {
+ ToMP.clear();
+ MP.clear();
+ }
+ m_BsplinesY->SetParametersWithInitFinalPose
+ (TimeInterval,InitPosition,FinalPosition,ToMP,MP);
break;
case Z_AXIS:
@@ -394,155 +437,205 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex, double Ti
{
WayPoint_z = 0.0;
}
- m_PolynomeZ->SetParametersWithMiddlePos(TimeInterval, FinalPosition+m_StepHeight,
- InitPosition, InitSpeed, InitAcc, FinalPosition);
+ m_PolynomeZ->SetParametersWithMiddlePos
+ (TimeInterval, FinalPosition+m_StepHeight,
+ InitPosition, InitSpeed, InitAcc, FinalPosition);
// Check the final and the initial position to decide what to do
if(InitSpeed*InitSpeed > 0.00001)
- {
- ToMP.clear();
- MP.clear();
- }
+ {
+ ToMP.clear();
+ MP.clear();
+ }
else if (FinalPosition - InitPosition > epsilon )
{
ToMP.push_back(0.4*TimeInterval);
MP.push_back(FinalPosition+WayPoint_z);
- m_BsplinesZ->SetParametersWithInitFinalPose(TimeInterval,InitPosition,FinalPosition,ToMP,MP);
+ m_BsplinesZ->SetParametersWithInitFinalPose
+ (TimeInterval,InitPosition,FinalPosition,ToMP,MP);
m_BsplinesZ->GenerateDegree();
- m_BsplinesZ->PrintControlPoints();
- m_BsplinesZ->PrintDegree();
- m_BsplinesZ->PrintKnotVector();
-
+
+ if (0)
+ {
+ m_BsplinesZ->PrintControlPoints();
+ m_BsplinesZ->PrintDegree();
+ m_BsplinesZ->PrintKnotVector();
+ }
}
- else if ( sqrt((FinalPosition - InitPosition)*(FinalPosition - InitPosition)) <= epsilon )
+ else if ( sqrt((FinalPosition - InitPosition)*
+ (FinalPosition - InitPosition)) <= epsilon )
{
ToMP.push_back(0.5*TimeInterval);
MP.push_back(FinalPosition+WayPoint_z);
- m_BsplinesZ->SetParameters(TimeInterval,InitPosition,FinalPosition,ToMP,MP,InitSpeed,InitAcc);
+ m_BsplinesZ->SetParameters
+ (TimeInterval,InitPosition,
+ FinalPosition,ToMP,MP,InitSpeed,InitAcc);
}
else if (FinalPosition - InitPosition < -epsilon )
{
ToMP.push_back(0.65*TimeInterval);
MP.push_back(InitPosition+WayPoint_z*0.45);
- m_BsplinesZ->SetParametersWithInitFinalPose(TimeInterval,InitPosition,FinalPosition,ToMP,MP);
+ m_BsplinesZ->
+ SetParametersWithInitFinalPose
+ (TimeInterval,InitPosition,FinalPosition,ToMP,MP);
m_BsplinesZ->GenerateDegree();
- m_BsplinesZ->PrintControlPoints();
- m_BsplinesZ->PrintDegree();
- m_BsplinesZ->PrintKnotVector();
+ if (0)
+ {
+ m_BsplinesZ->PrintControlPoints();
+ m_BsplinesZ->PrintDegree();
+ m_BsplinesZ->PrintKnotVector();
+ }
}
- //m_BsplinesZ->SetParameters(TimeInterval,InitPosition,FinalPosition,ToMP,MP,InitSpeed,InitAcc);
+ //m_BsplinesZ->SetParameters(TimeInterval,
+ // InitPosition,FinalPosition,ToMP,MP,InitSpeed,InitAcc);
break;
case THETA_AXIS:
- m_PolynomeTheta->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc);
+ m_PolynomeTheta->SetParameters
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc);
break;
case OMEGA_AXIS:
- m_PolynomeOmega->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ m_PolynomeOmega->SetParametersWithInitPosInitSpeed
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed);
break;
case OMEGA2_AXIS:
- m_PolynomeOmega2->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ m_PolynomeOmega2->SetParametersWithInitPosInitSpeed
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed);
break;
default:
return -1;
break;
- }
+ }
return 0;
}
// allow C^2 continuity in the interpolation
-int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex, double TimeInterval,
- double FinalPosition, double InitPosition, double InitSpeed, double InitAcc, double InitJerk)
+int FootTrajectoryGenerationStandard::
+SetParameters
+(int PolynomeIndex,
+ double TimeInterval,
+ double FinalPosition,
+ double InitPosition,
+ double InitSpeed,
+ double InitAcc,
+ double InitJerk)
{
- switch (PolynomeIndex)
- {
+ switch (PolynomeIndex)
+ {
- case X_AXIS:
- m_PolynomeX->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc,InitJerk);
- break;
+ case X_AXIS:
+ m_PolynomeX->SetParameters
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc,InitJerk);
+ break;
- case Y_AXIS:
- m_PolynomeY->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc,InitJerk);
- break;
+ case Y_AXIS:
+ m_PolynomeY->SetParameters
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc,InitJerk);
+ break;
- case Z_AXIS:
- m_PolynomeZ->SetParametersWithMiddlePos(TimeInterval,FinalPosition+m_StepHeight,InitPosition,InitSpeed,
- InitAcc,FinalPosition);
- break;
+ case Z_AXIS:
+ m_PolynomeZ->
+ SetParametersWithMiddlePos
+ (TimeInterval,FinalPosition+m_StepHeight,InitPosition,InitSpeed,
+ InitAcc,FinalPosition);
+ break;
- case THETA_AXIS:
- m_PolynomeTheta->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc);
- break;
+ case THETA_AXIS:
+ m_PolynomeTheta->
+ SetParameters
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc);
+ break;
- case OMEGA_AXIS:
- m_PolynomeOmega->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
- break;
+ case OMEGA_AXIS:
+ m_PolynomeOmega->
+ SetParametersWithInitPosInitSpeed
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ break;
- case OMEGA2_AXIS:
- m_PolynomeOmega2->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
- break;
+ case OMEGA2_AXIS:
+ m_PolynomeOmega2->
+ SetParametersWithInitPosInitSpeed
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ break;
- default:
- return -1;
- break;
- }
- return 0;
+ default:
+ return -1;
+ break;
+ }
+ return 0;
}
-int FootTrajectoryGenerationStandard::GetParametersWithInitPosInitSpeed(int PolynomeIndex,
- double &TimeInterval,
- double &FinalPosition,
- double &InitPosition,
- double &InitSpeed)
+int FootTrajectoryGenerationStandard::
+GetParametersWithInitPosInitSpeed
+(int PolynomeIndex,
+ double &TimeInterval,
+ double &FinalPosition,
+ double &InitPosition,
+ double &InitSpeed)
{
double MiddlePosition = 0.0 ; // for polynome4
switch (PolynomeIndex)
- {
+ {
- case X_AXIS:
- ODEBUG2("Initspeed: " << InitSpeed << " ");
- m_PolynomeX->GetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
- break;
+ case X_AXIS:
+ ODEBUG2("Initspeed: " << InitSpeed << " ");
+ m_PolynomeX->
+ GetParametersWithInitPosInitSpeed
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ break;
- case Y_AXIS:
- m_PolynomeY->GetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
- break;
+ case Y_AXIS:
+ m_PolynomeY->
+ GetParametersWithInitPosInitSpeed
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ break;
- case Z_AXIS:
- m_PolynomeZ->GetParametersWithInitPosInitSpeed(TimeInterval,MiddlePosition,FinalPosition,InitPosition,InitSpeed);
+ case Z_AXIS:
+ m_PolynomeZ->
+ GetParametersWithInitPosInitSpeed
+ (TimeInterval,MiddlePosition,FinalPosition,InitPosition,InitSpeed);
- break;
+ break;
- case THETA_AXIS:
- m_PolynomeTheta->GetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
- break;
+ case THETA_AXIS:
+ m_PolynomeTheta->
+ GetParametersWithInitPosInitSpeed
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ break;
- case OMEGA_AXIS:
- m_PolynomeOmega->GetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
- break;
+ case OMEGA_AXIS:
+ m_PolynomeOmega->
+ GetParametersWithInitPosInitSpeed
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ break;
- case OMEGA2_AXIS:
- m_PolynomeOmega2->GetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
- break;
+ case OMEGA2_AXIS:
+ m_PolynomeOmega2->
+ GetParametersWithInitPosInitSpeed
+ (TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ break;
- default:
- return -1;
- break;
- }
- return 0;
+ default:
+ return -1;
+ break;
+ }
+ return 0;
}
// Compute the trajectory from init point to end point using polynom
-double FootTrajectoryGenerationStandard::ComputeAllWithPolynom(FootAbsolutePosition & aFootAbsolutePosition,
- double Time)
+double
+FootTrajectoryGenerationStandard::
+ComputeAllWithPolynom
+(FootAbsolutePosition & aFootAbsolutePosition,
+ double Time)
{
- //std::cout << "------------ Using Polynom ---------" << std::endl;
aFootAbsolutePosition.x = m_PolynomeX->Compute(Time);
aFootAbsolutePosition.dx = m_PolynomeX->ComputeDerivative(Time);
@@ -577,7 +670,7 @@ double FootTrajectoryGenerationStandard::ComputeAllWithPolynom(FootAbsolutePosit
// Compute the trajectory from init point to end point using B-Splines
double FootTrajectoryGenerationStandard::ComputeAllWithBSplines(FootAbsolutePosition & aFootAbsolutePosition,
- double Time)
+ double Time)
{
double initz(0.0),initdz(0.0),initddz(0.0) ;
m_BsplinesZ->Compute(0.0,
@@ -590,31 +683,27 @@ double FootTrajectoryGenerationStandard::ComputeAllWithBSplines(FootAbsolutePosi
double EndOfLiftOff = (ss_time-UnlockedSwingPeriod)*0.5;
double StartLanding = EndOfLiftOff + UnlockedSwingPeriod;
- //cout << UnlockedSwingPeriod << " " << ss_time << " " << " " << UnlockedSwingPeriod/ss_time << endl ;
-
double timeOfInterpolation = 0.0 ;
//double timeOfInterpolation = Time - EndOfLiftOff ;
if(initdz*initdz > 0.000001)
- {
- timeOfInterpolation = Time ;
- }
+ {
+ timeOfInterpolation = Time ;
+ }
else
- {
- if(Time < EndOfLiftOff)
- {
- timeOfInterpolation = 0.0 ;
- }
- else if (Time < StartLanding)
- {
- timeOfInterpolation = Time - EndOfLiftOff ;
- }
- else
- {
- timeOfInterpolation = UnlockedSwingPeriod ;
- }
- }
-// cout << "timeOfInterpolation = " << timeOfInterpolation << endl ;
-// cout << "time = " << Time << endl ;
+ {
+ if(Time < EndOfLiftOff)
+ {
+ timeOfInterpolation = 0.0 ;
+ }
+ else if (Time < StartLanding)
+ {
+ timeOfInterpolation = Time - EndOfLiftOff ;
+ }
+ else
+ {
+ timeOfInterpolation = UnlockedSwingPeriod ;
+ }
+ }
// Trajectory of the foot compute in the X domain (plane X of t)
m_BsplinesX->Compute(timeOfInterpolation,
@@ -654,14 +743,6 @@ double FootTrajectoryGenerationStandard::ComputeAllWithBSplines(FootAbsolutePosi
ODEBUG2("t: " << Time << " : " << aFootAbsolutePosition.omega2);
ODEBUG2("t: " << Time << " : " << aFootAbsolutePosition.theta);
-// cout << "FT_z = " << m_BsplinesZ->FT() << " " << m_BsplinesZ->Compute(Time-0.005) << endl ;
-// cout << "FT_z = " << m_BsplinesZ->Compute(Time) << " " << m_BsplinesZ->Compute(Time+0.005) << endl ;
-// cout << "t: " << Time << " " << timeOfInterpolation << " : " << aFootAbsolutePosition.x << endl
-// << "t: " << Time << " " << timeOfInterpolation << " : " << aFootAbsolutePosition.y << endl
-// << "t: " << Time << " " << timeOfInterpolation << " : " << aFootAbsolutePosition.z << endl
-// << "t: " << Time << " " << timeOfInterpolation << " : " << aFootAbsolutePosition.omega << endl
-// << "t: " << Time << " " << timeOfInterpolation << " : " << aFootAbsolutePosition.omega2 << endl
-// << "t: " << Time << " " << timeOfInterpolation << " : " << aFootAbsolutePosition.theta << endl;
return Time;
}
@@ -671,37 +752,37 @@ double FootTrajectoryGenerationStandard::Compute(unsigned int PolynomeIndex, dou
double r=0.0;
switch (PolynomeIndex)
- {
+ {
- case X_AXIS:
- r=m_PolynomeX->Compute(Time);
- break;
+ case X_AXIS:
+ r=m_PolynomeX->Compute(Time);
+ break;
- case Y_AXIS:
- r=m_PolynomeY->Compute(Time);
- break;
+ case Y_AXIS:
+ r=m_PolynomeY->Compute(Time);
+ break;
- case Z_AXIS:
- r=m_PolynomeZ->Compute(Time);
- break;
+ case Z_AXIS:
+ r=m_PolynomeZ->Compute(Time);
+ break;
- case THETA_AXIS:
- r=m_PolynomeTheta->Compute(Time);
- break;
+ case THETA_AXIS:
+ r=m_PolynomeTheta->Compute(Time);
+ break;
- case OMEGA_AXIS:
- r=m_PolynomeOmega->Compute(Time);
- break;
+ case OMEGA_AXIS:
+ r=m_PolynomeOmega->Compute(Time);
+ break;
- case OMEGA2_AXIS:
- r=m_PolynomeOmega2->Compute(Time);
- break;
+ case OMEGA2_AXIS:
+ r=m_PolynomeOmega2->Compute(Time);
+ break;
- default:
- return -1.0;
- break;
- }
- return r;
+ default:
+ return -1.0;
+ break;
+ }
+ return r;
}
double FootTrajectoryGenerationStandard::ComputeSecDerivative(unsigned int PolynomeIndex, double Time)
@@ -709,37 +790,37 @@ double FootTrajectoryGenerationStandard::ComputeSecDerivative(unsigned int Polyn
double r=0.0;
switch (PolynomeIndex)
- {
+ {
- case X_AXIS:
- r=m_PolynomeX->ComputeSecDerivative(Time);
- break;
+ case X_AXIS:
+ r=m_PolynomeX->ComputeSecDerivative(Time);
+ break;
- case Y_AXIS:
- r=m_PolynomeY->ComputeSecDerivative(Time);
- break;
+ case Y_AXIS:
+ r=m_PolynomeY->ComputeSecDerivative(Time);
+ break;
- case Z_AXIS:
- r=m_PolynomeZ->ComputeSecDerivative(Time);
- break;
+ case Z_AXIS:
+ r=m_PolynomeZ->ComputeSecDerivative(Time);
+ break;
- case THETA_AXIS:
- r=m_PolynomeTheta->ComputeSecDerivative(Time);
- break;
+ case THETA_AXIS:
+ r=m_PolynomeTheta->ComputeSecDerivative(Time);
+ break;
- case OMEGA_AXIS:
- r=m_PolynomeOmega->ComputeSecDerivative(Time);
- break;
+ case OMEGA_AXIS:
+ r=m_PolynomeOmega->ComputeSecDerivative(Time);
+ break;
- case OMEGA2_AXIS:
- r=m_PolynomeOmega2->ComputeSecDerivative(Time);
- break;
+ case OMEGA2_AXIS:
+ r=m_PolynomeOmega2->ComputeSecDerivative(Time);
+ break;
- default:
- return -1.0;
- break;
- }
- return r;
+ default:
+ return -1.0;
+ break;
+ }
+ return r;
}
void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
@@ -891,11 +972,11 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
#endif
ODEBUG4( "Foot Step:" << StepType << "Foot Shift: "<< Foot_Shift
- << " ( " << dFX<< " , " << dFY<< " , " << " , " << dFZ << " )"
- << curr_NSFAP.x << " "
- << curr_NSFAP.y << " "
- << curr_NSFAP.z << " "
- ,"GeneratedFoot.dat");
+ << " ( " << dFX<< " , " << dFY<< " , " << " , " << dFZ << " )"
+ << curr_NSFAP.x << " "
+ << curr_NSFAP.y << " "
+ << curr_NSFAP.z << " "
+ ,"GeneratedFoot.dat");
}
@@ -943,24 +1024,24 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
// x, dx
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
m_PolynomeX->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
- // NoneSupportFootAbsolutePositions[StartIndex-1].x;
+ // NoneSupportFootAbsolutePositions[StartIndex-1].x;
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dx =
m_PolynomeX->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
- // NoneSupportFootAbsolutePositions[StartIndex-1].dx;
+ // NoneSupportFootAbsolutePositions[StartIndex-1].dx;
//y, dy
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
m_PolynomeY->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
- // NoneSupportFootAbsolutePositions[StartIndex-1].y;
+ // NoneSupportFootAbsolutePositions[StartIndex-1].y;
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dy =
m_PolynomeY->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff); // +
- // NoneSupportFootAbsolutePositions[StartIndex-1].dy;
+ // NoneSupportFootAbsolutePositions[StartIndex-1].dy;
//theta, dtheta
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
m_PolynomeTheta->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
- //NoneSupportFootAbsolutePositions[StartIndex].theta;
+ //NoneSupportFootAbsolutePositions[StartIndex].theta;
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dtheta =
m_PolynomeTheta->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
- // +NoneSupportFootAbsolutePositions[StartIndex].dtheta;
+ // +NoneSupportFootAbsolutePositions[StartIndex].dtheta;
}
else
{
@@ -991,10 +1072,10 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
- //m_AnklePositionRight[2];
+ //m_AnklePositionRight[2];
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
- //m_AnklePositionRight[2];
+ //m_AnklePositionRight[2];
//bool ProtectionNeeded=false;
@@ -1004,11 +1085,11 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
{
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
m_PolynomeOmega->Compute(InterpolationTime); // +
- // NoneSupportFootAbsolutePositions[StartIndex-1].omega;
+ // NoneSupportFootAbsolutePositions[StartIndex-1].omega;
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].domega =
m_PolynomeOmega->Compute(InterpolationTime);// +
- // NoneSupportFootAbsolutePositions[StartIndex-1].domega;
+ // NoneSupportFootAbsolutePositions[StartIndex-1].domega;
//ProtectionNeeded=true;
}
@@ -1102,11 +1183,11 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
#endif
ODEBUG4( "Foot Step:" << StepType << "Foot Shift: "<< Foot_Shift
- << " ( " << dFX<< " , " << dFY<< " , " << " , " << dFZ << " )"
- << NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x << " "
- << NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y << " "
- << NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z << " "
- ,"GeneratedFoot.dat");
+ << " ( " << dFX<< " , " << dFY<< " , " << " , " << dFZ << " )"
+ << NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x << " "
+ << NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y << " "
+ << NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z << " "
+ ,"GeneratedFoot.dat");
}
diff --git a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
index a4d040e4..b81baeaa 100644
--- a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
+++ b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
@@ -67,12 +67,16 @@ LeftAndRightFootTrajectoryGenerationMultiple(SimplePluginManager *lSPM,
}
-PRFoot * LeftAndRightFootTrajectoryGenerationMultiple::getFoot() const
+PRFoot *
+LeftAndRightFootTrajectoryGenerationMultiple::
+getFoot() const
{
return m_Foot;
}
+
LeftAndRightFootTrajectoryGenerationMultiple::
-LeftAndRightFootTrajectoryGenerationMultiple(const LeftAndRightFootTrajectoryGenerationMultiple & aLRFTGM):
+LeftAndRightFootTrajectoryGenerationMultiple
+(const LeftAndRightFootTrajectoryGenerationMultiple & aLRFTGM):
SimplePlugin(aLRFTGM.getSimplePluginManager())
{
@@ -81,7 +85,8 @@ LeftAndRightFootTrajectoryGenerationMultiple(const LeftAndRightFootTrajectoryGen
*this = aLRFTGM;
}
-LeftAndRightFootTrajectoryGenerationMultiple::~LeftAndRightFootTrajectoryGenerationMultiple()
+LeftAndRightFootTrajectoryGenerationMultiple::
+~LeftAndRightFootTrajectoryGenerationMultiple()
{
if (m_LeftFootTrajectory!=0)
@@ -93,7 +98,8 @@ LeftAndRightFootTrajectoryGenerationMultiple::~LeftAndRightFootTrajectoryGenerat
}
/*! Handling methods for the plugin mecanism. */
-void LeftAndRightFootTrajectoryGenerationMultiple::CallMethod(std::string & Method, std::istringstream &strm)
+void LeftAndRightFootTrajectoryGenerationMultiple::
+CallMethod(std::string & Method, std::istringstream &strm)
{
if (Method==":omega")
{
@@ -117,87 +123,106 @@ void LeftAndRightFootTrajectoryGenerationMultiple::CallMethod(std::string & Meth
}
}
-void LeftAndRightFootTrajectoryGenerationMultiple::SetAnInterval(unsigned int IntervalIndex,
- FootTrajectoryGenerationMultiple * aFTGM,
- FootAbsolutePosition &FootInitialPosition,
- FootAbsolutePosition &FootFinalPosition,
- vector<double> MiddlePos)
+void LeftAndRightFootTrajectoryGenerationMultiple::
+SetAnInterval
+(unsigned int IntervalIndex,
+ FootTrajectoryGenerationMultiple * aFTGM,
+ FootAbsolutePosition &FootInitialPosition,
+ FootAbsolutePosition &FootFinalPosition,
+ vector<double> MiddlePos)
{
- ODEBUG("Set interval " << IntervalIndex << "/" << m_DeltaTj.size() << " : " << m_DeltaTj[IntervalIndex] << " X: ("
- << FootFinalPosition.x << "," << FootInitialPosition.x << "," << FootInitialPosition.dx << ")("
- << FootFinalPosition.y << "," << FootInitialPosition.y << "," << FootInitialPosition.dy << ")("
- << FootFinalPosition.z << "," << FootInitialPosition.z << "," << FootInitialPosition.dz << ")");
+ ODEBUG("Set interval " << IntervalIndex << "/"
+ << m_DeltaTj.size() << " : " << m_DeltaTj[IntervalIndex] << " X: ("
+ << FootFinalPosition.x << ","
+ << FootInitialPosition.x << ","
+ << FootInitialPosition.dx << ")("
+ << FootFinalPosition.y << ","
+ << FootInitialPosition.y << ","
+ << FootInitialPosition.dy << ")("
+ << FootFinalPosition.z << ","
+ << FootInitialPosition.z << ","
+ << FootInitialPosition.dz << ")");
aFTGM->SetNatureInterval(IntervalIndex,FootFinalPosition.stepType);
double ModulationSupportCoefficient = 0.7;
- double UnlockedSwingPeriod = m_DeltaTj[IntervalIndex] * ModulationSupportCoefficient;
+ double UnlockedSwingPeriod = m_DeltaTj[IntervalIndex] *
+ ModulationSupportCoefficient;
// X axis.
- aFTGM->SetParametersWithInitPosInitSpeedInitAcc(IntervalIndex,
- FootTrajectoryGenerationStandard::X_AXIS,
- UnlockedSwingPeriod,
- FootFinalPosition.x,
- FootInitialPosition.x,
- FootInitialPosition.dx,
- FootInitialPosition.ddx,
- MiddlePos);
+ aFTGM->SetParametersWithInitPosInitSpeedInitAcc
+ (IntervalIndex,
+ FootTrajectoryGenerationStandard::X_AXIS,
+ UnlockedSwingPeriod,
+ FootFinalPosition.x,
+ FootInitialPosition.x,
+ FootInitialPosition.dx,
+ FootInitialPosition.ddx,
+ MiddlePos);
// Y axis.
- aFTGM->SetParametersWithInitPosInitSpeedInitAcc(IntervalIndex,
- FootTrajectoryGenerationStandard::Y_AXIS,
- UnlockedSwingPeriod,
- FootFinalPosition.y,
- FootInitialPosition.y,
- FootInitialPosition.dy,
- FootInitialPosition.ddy,
- MiddlePos);
+ aFTGM->SetParametersWithInitPosInitSpeedInitAcc
+ (IntervalIndex,
+ FootTrajectoryGenerationStandard::Y_AXIS,
+ UnlockedSwingPeriod,
+ FootFinalPosition.y,
+ FootInitialPosition.y,
+ FootInitialPosition.dy,
+ FootInitialPosition.ddy,
+ MiddlePos);
// Z axis.
- aFTGM->SetParametersWithInitPosInitSpeedInitAcc(IntervalIndex,
- FootTrajectoryGenerationStandard::Z_AXIS,
- m_DeltaTj[IntervalIndex],
- FootFinalPosition.z,
- FootInitialPosition.z,
- FootInitialPosition.dz,
- FootInitialPosition.ddz,
- MiddlePos);
+ aFTGM->SetParametersWithInitPosInitSpeedInitAcc
+ (IntervalIndex,
+ FootTrajectoryGenerationStandard::Z_AXIS,
+ m_DeltaTj[IntervalIndex],
+ FootFinalPosition.z,
+ FootInitialPosition.z,
+ FootInitialPosition.dz,
+ FootInitialPosition.ddz,
+ MiddlePos);
// THETA
- aFTGM->SetParametersWithInitPosInitSpeedInitAcc(IntervalIndex,
- FootTrajectoryGenerationStandard::THETA_AXIS,
- UnlockedSwingPeriod,
- FootFinalPosition.theta,
- FootInitialPosition.theta,
- FootInitialPosition.dtheta,
- FootInitialPosition.ddtheta);
+ aFTGM->SetParametersWithInitPosInitSpeedInitAcc
+ (IntervalIndex,
+ FootTrajectoryGenerationStandard::THETA_AXIS,
+ UnlockedSwingPeriod,
+ FootFinalPosition.theta,
+ FootInitialPosition.theta,
+ FootInitialPosition.dtheta,
+ FootInitialPosition.ddtheta);
// Omega
- aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
- FootTrajectoryGenerationStandard::OMEGA_AXIS,
- UnlockedSwingPeriod,
- FootFinalPosition.omega,
- FootInitialPosition.omega,
- FootInitialPosition.domega);
+ aFTGM->SetParametersWithInitPosInitSpeed
+ (IntervalIndex,
+ FootTrajectoryGenerationStandard::OMEGA_AXIS,
+ UnlockedSwingPeriod,
+ FootFinalPosition.omega,
+ FootInitialPosition.omega,
+ FootInitialPosition.domega);
// Omega 2
- aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
- FootTrajectoryGenerationStandard::OMEGA2_AXIS,
- UnlockedSwingPeriod,
- FootFinalPosition.omega2,
- FootInitialPosition.omega2,
- FootInitialPosition.domega2);
+ aFTGM->SetParametersWithInitPosInitSpeed
+ (IntervalIndex,
+ FootTrajectoryGenerationStandard::OMEGA2_AXIS,
+ UnlockedSwingPeriod,
+ FootFinalPosition.omega2,
+ FootInitialPosition.omega2,
+ FootInitialPosition.domega2);
}
void LeftAndRightFootTrajectoryGenerationMultiple::
-InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
- FootAbsolutePosition &LeftFootInitialPosition,
- FootAbsolutePosition &RightFootInitialPosition,
- deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions,
- bool IgnoreFirst, bool Continuity)
+InitializeFromRelativeSteps
+(deque<RelativeFootPosition> &RelativeFootPositions,
+ FootAbsolutePosition &LeftFootInitialPosition,
+ FootAbsolutePosition &RightFootInitialPosition,
+ deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions,
+ bool IgnoreFirst, bool Continuity)
{
- ODEBUG("LeftFootInitialPosition.stepType: "<< LeftFootInitialPosition.stepType
- << " RightFootInitialPosition.stepType: "<< RightFootInitialPosition.stepType);
+ ODEBUG("LeftFootInitialPosition.stepType: "
+ << LeftFootInitialPosition.stepType
+ << " RightFootInitialPosition.stepType: "
+ << RightFootInitialPosition.stepType);
/*! Makes sure the size of the SupportFootAbsolutePositions is the same than
the relative foot positions. */
if (SupportFootAbsoluteFootPositions.size()!=
@@ -266,7 +291,8 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
CurrentSupportFootPosition(2,2) = LeftFootInitialPosition.z;
// v2(2,0) = LeftFootInitialPosition.z;
}
- ODEBUG("Support Foot : " << v2(0,0) << " " << v2(1,0) << " " << CurrentAbsTheta);
+ ODEBUG("Support Foot : " << v2(0,0) << " "
+ << v2(1,0) << " " << CurrentAbsTheta);
// Initial Position of the current support foot.
c = cos(CurrentAbsTheta*M_PI/180.0);
@@ -316,7 +342,8 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
{
/*! At this stage the phase of double support is dealt with */
ODEBUG("Double support phase");
- //LeftFootTmpInitPos.z = CurrentSupportFootPosition(2,2);// 0.0;//LeftFootTmpInitPos.z + LeftFootTmpFinalPos.z/1.5;
+ //LeftFootTmpInitPos.z = CurrentSupportFootPosition(2,2);
+ // 0.0;//LeftFootTmpInitPos.z + LeftFootTmpFinalPos.z/1.5;
LeftFootTmpInitPos.dz = 0;
LeftFootTmpInitPos.stepType=11;
@@ -402,7 +429,10 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
AbsoluteFootPositions[i].z = CurrentSupportFootPosition(2,2);
AbsoluteFootPositions[i].theta = CurrentAbsTheta;
- ODEBUG("CSFP:" << CurrentSupportFootPosition(0,2) << " " << CurrentSupportFootPosition(1,2) <<" " << CurrentSupportFootPosition(2,2) << " "<< CurrentAbsTheta);
+ ODEBUG("CSFP:" << CurrentSupportFootPosition(0,2) << " "
+ << CurrentSupportFootPosition(1,2) << " "
+ << CurrentSupportFootPosition(2,2) << " "
+ << CurrentAbsTheta);
/*! We deal with the single support phase,
i.e. the target of the next single support phase
@@ -514,16 +544,20 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
dx = InitPos(1) - FinalPos(1) ;
dy = FinalPos(0) - InitPos(0) ;
m_MiddleWayPoint.resize(2);
- { for(unsigned int i=0;i<m_MiddleWayPoint.size();m_MiddleWayPoint[i++]=0.0);};
+ { for(unsigned int i=0;
+ i<m_MiddleWayPoint.size();
+ m_MiddleWayPoint[i++]=0.0);};
if ( dx*dx>=1e-6 || dy*dy>=1e-6 )// not moving implies no collision
{
dc = -(dx * InitPos(0) + dy *InitPos(1)) ;
- distSquareToLine = (dx*currSupp(0) + dy*currSupp(1) + dc)*(dx*currSupp(0) + dy*currSupp(1) + dc)/(dx*dx + dy*dy);
+ distSquareToLine = (dx*currSupp(0) + dy*currSupp(1) + dc)*
+ (dx*currSupp(0) + dy*currSupp(1) + dc)/(dx*dx + dy*dy);
if( distSquareToLine < m_WayPointThreshold )
{
double x(currSupp(0)), y(currSupp(1)),
- x0((FinalPos(0)+InitPos(0))*0.5), y0((FinalPos(1)+InitPos(1))*0.5),
- R2((dx*dx+dy+dy)*0.5*0.5);
+ x0((FinalPos(0)+InitPos(0))*0.5),
+ y0((FinalPos(1)+InitPos(1))*0.5),
+ R2((dx*dx+dy+dy)*0.5*0.5);
bool autocollision = (x-x0)*(x-x0)+(y-y0)*(y-y0)<=R2;
if( autocollision )
{
@@ -658,13 +692,18 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
/*! This part initializes correctly the last two intervals
if the system is in real-time foot modification. In this case,
the representation of the intervals shift from:
- ONE DOUBLE SUPPORT STARTING PHASE - 1st foot single support phase - double support phase - 2nd foot single support phase
- - double support phase - 3rd single support phase - ending double support phase
+ ONE DOUBLE SUPPORT STARTING PHASE - 1st foot single support phase -
+ double support phase - 2nd foot single support phase
+ - double support phase - 3rd single support phase -
+ ending double support phase
to
- 1st foot single support phase - double support phase - 2nd foot single support phase
+ 1st foot single support phase - double support phase -
+ 2nd foot single support phase
- double support phase - 3rd single support phase
- Two intervals are missing and should be set by default to the end position of the feet
- if Continuity is set to true, and if the number of intervals so far is the number of
+ Two intervals are missing and should be set by
+ default to the end position of the feet
+ if Continuity is set to true, and if the number of intervals
+ so far is the number of
intervals minus 2.
*/
if ((Continuity) && (IntervalIndex==(int)(m_DeltaTj.size()-2)))
@@ -689,10 +728,11 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
}
void LeftAndRightFootTrajectoryGenerationMultiple::
-ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
- FootAbsolutePosition &LeftFootInitialPosition,
- FootAbsolutePosition &RightFootInitialPosition,
- deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions)
+ComputeAbsoluteStepsFromRelativeSteps
+(deque<RelativeFootPosition> &RelativeFootPositions,
+ FootAbsolutePosition &LeftFootInitialPosition,
+ FootAbsolutePosition &RightFootInitialPosition,
+ deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions)
{
FootAbsolutePosition aSupportFootAbsolutePosition;
@@ -715,9 +755,10 @@ ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootP
}
void LeftAndRightFootTrajectoryGenerationMultiple::
-ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
- FootAbsolutePosition &SupportFootInitialAbsolutePosition,
- deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions)
+ComputeAbsoluteStepsFromRelativeSteps
+(deque<RelativeFootPosition> &RelativeFootPositions,
+ FootAbsolutePosition &SupportFootInitialAbsolutePosition,
+ deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions)
{
/*! Makes sure the size of the SupportFootAbsolutePositions is the same than
the relative foot positions. */
@@ -815,7 +856,8 @@ ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootP
AbsoluteFootPositions[i].z = CurrentSupportFootPosition(2,2);
AbsoluteFootPositions[i].theta = CurrentAbsTheta;
- ODEBUG("CSFP:" << CurrentSupportFootPosition(0,2) << " " << CurrentSupportFootPosition(1,2));
+ ODEBUG("CSFP:" << CurrentSupportFootPosition(0,2) << " "
+ << CurrentSupportFootPosition(1,2));
/* Populate the set of support foot absolute positions */
SupportFootAbsoluteFootPositions[i].x = CurrentSupportFootPosition(0,2);
@@ -828,10 +870,11 @@ ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootP
}
void LeftAndRightFootTrajectoryGenerationMultiple::
-ChangeRelStepsFromAbsSteps(deque<RelativeFootPosition> &RelativeFootPositions,
- FootAbsolutePosition &SupportFootInitialPosition,
- deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions,
- unsigned int ChangedInterval)
+ChangeRelStepsFromAbsSteps
+(deque<RelativeFootPosition> &RelativeFootPositions,
+ FootAbsolutePosition &SupportFootInitialPosition,
+ deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions,
+ unsigned int ChangedInterval)
{
if (ChangedInterval>=SupportFootAbsoluteFootPositions.size())
{
@@ -887,7 +930,8 @@ ChangeRelStepsFromAbsSteps(deque<RelativeFootPosition> &RelativeFootPositions,
RelativeFootPositions[ChangedInterval].sx = relMotionM1(0,2);
RelativeFootPositions[ChangedInterval].sy = relMotionM1(1,2);
- RelativeFootPositions[ChangedInterval].theta = atan2(relMotionM1(1,0),relMotionM1(0,0));
+ RelativeFootPositions[ChangedInterval].theta =
+ atan2(relMotionM1(1,0),relMotionM1(0,0));
double thetakp1,xkp1,ykp1;
@@ -911,17 +955,21 @@ ChangeRelStepsFromAbsSteps(deque<RelativeFootPosition> &RelativeFootPositions,
RelativeFootPositions[ChangedInterval+1].sx = relMotionP1(0,2);
RelativeFootPositions[ChangedInterval+1].sy = relMotionP1(1,2);
- RelativeFootPositions[ChangedInterval+1].theta = atan2(relMotionP1(1,0),relMotionP1(0,0));
+ RelativeFootPositions[ChangedInterval+1].theta =
+ atan2(relMotionP1(1,0),relMotionP1(0,0));
}
ODEBUG("KP1 position: " << xkp1 << " " << ykp1 << " " << thetakp1 );
- ODEBUG("Changed intervals : " << ChangedInterval-1 << " " << ChangedInterval << " " << ChangedInterval + 1);
+ ODEBUG("Changed intervals : " << ChangedInterval-1 << " "
+ << ChangedInterval << " " << ChangedInterval + 1);
}
-bool LeftAndRightFootTrajectoryGenerationMultiple::ComputeAnAbsoluteFootPosition(int LeftOrRight,
- double time,
- FootAbsolutePosition & aFAP)
+bool LeftAndRightFootTrajectoryGenerationMultiple::
+ComputeAnAbsoluteFootPosition
+(int LeftOrRight,
+ double time,
+ FootAbsolutePosition & aFAP)
{
ODEBUG("Left (1) or right (-1) : " << LeftOrRight);
@@ -932,9 +980,11 @@ bool LeftAndRightFootTrajectoryGenerationMultiple::ComputeAnAbsoluteFootPosition
//aFAP.LeftOrRightFoot = 0; // Left Foot
bool r = m_LeftFootTrajectory->Compute(time,aFAP);
if (!r)
- {ODEBUG3("Unable to compute left foot abs pos at time " <<time);
- LTHROW("Pb in computing absolute foot position");
- }
+ {
+ std::cerr << "Unable to compute left foot abs pos at time "
+ <<time << std::endl;
+ LTHROW("Pb in computing absolute foot position");
+ }
return r;
}
else
@@ -943,22 +993,28 @@ bool LeftAndRightFootTrajectoryGenerationMultiple::ComputeAnAbsoluteFootPosition
//aFAP.LeftOrRightFoot = 1; // Right Foot
bool r = m_RightFootTrajectory->Compute(time,aFAP);
if (!r)
- {ODEBUG3("Unable to compute right foot abs pos at time " <<time);
- LTHROW("Pb in computing absolute foot position");
- }
+ {
+ std::cerr
+ << "Unable to compute right foot abs pos at time " <<time
+ << std::endl;
+ LTHROW("Pb in computing absolute foot position");
+ }
return r;
}
return false;
}
-bool LeftAndRightFootTrajectoryGenerationMultiple::ComputeAnAbsoluteFootPosition(int LeftOrRight,
- double time,
- FootAbsolutePosition & aFAP,
- unsigned int IndexInterval)
+bool LeftAndRightFootTrajectoryGenerationMultiple::
+ComputeAnAbsoluteFootPosition
+(int LeftOrRight,
+ double time,
+ FootAbsolutePosition & aFAP,
+ unsigned int IndexInterval)
{
- ODEBUG(this << " " << m_LeftFootTrajectory << " " << m_RightFootTrajectory);
+ ODEBUG(this << " " << m_LeftFootTrajectory << " "
+ << m_RightFootTrajectory);
if (LeftOrRight==1)
return m_LeftFootTrajectory->Compute(time,aFAP,IndexInterval);
@@ -968,10 +1024,12 @@ bool LeftAndRightFootTrajectoryGenerationMultiple::ComputeAnAbsoluteFootPosition
return false;
}
/*
-bool LeftAndRightFootTrajectoryGenerationMultiple::ComputeAnAbsoluteFootPosition(int LeftOrRight,
- double time,
- deque<FootAbsolutePosition> & adFAP,
- unsigned int IndexInterval)
+bool LeftAndRightFootTrajectoryGenerationMultiple::
+ComputeAnAbsoluteFootPosition
+(int LeftOrRight,
+double time,
+deque<FootAbsolutePosition> & adFAP,
+unsigned int IndexInterval)
{
ODEBUG(this << " " << m_LeftFootTrajectory << " " << m_RightFootTrajectory);
@@ -984,7 +1042,8 @@ bool LeftAndRightFootTrajectoryGenerationMultiple::ComputeAnAbsoluteFootPosition
return false;
}*/
-void LeftAndRightFootTrajectoryGenerationMultiple::SetDeltaTj(vector<double> &aDeltaTj)
+void LeftAndRightFootTrajectoryGenerationMultiple::
+SetDeltaTj(vector<double> &aDeltaTj)
{
ODEBUG("SetDeltaTj :" << aDeltaTj.size() << " " << aDeltaTj[0]);
@@ -996,40 +1055,47 @@ void LeftAndRightFootTrajectoryGenerationMultiple::SetDeltaTj(vector<double> &aD
}
-void LeftAndRightFootTrajectoryGenerationMultiple::DisplayIntervals()
+void LeftAndRightFootTrajectoryGenerationMultiple::
+DisplayIntervals()
{
- ODEBUG3("Left intervals");
+ ODEBUG("Left intervals");
m_LeftFootTrajectory->DisplayIntervals();
- ODEBUG3("Right intervals");
+ ODEBUG("Right intervals");
m_RightFootTrajectory->DisplayIntervals();
}
-void LeftAndRightFootTrajectoryGenerationMultiple::GetDeltaTj(vector<double> &aDeltaTj) const
+void LeftAndRightFootTrajectoryGenerationMultiple::
+GetDeltaTj(vector<double> &aDeltaTj) const
{
aDeltaTj = m_DeltaTj;
}
-void LeftAndRightFootTrajectoryGenerationMultiple::SetStepHeight(double aStepHeight)
+void LeftAndRightFootTrajectoryGenerationMultiple::
+SetStepHeight(double aStepHeight)
{
m_StepHeight = aStepHeight;
}
-double LeftAndRightFootTrajectoryGenerationMultiple::GetStepHeight() const
+double LeftAndRightFootTrajectoryGenerationMultiple::
+GetStepHeight() const
{
return m_StepHeight;
}
-void LeftAndRightFootTrajectoryGenerationMultiple::SetStepCurving(double aStepCurving)
+void LeftAndRightFootTrajectoryGenerationMultiple::
+SetStepCurving(double aStepCurving)
{
m_StepCurving = aStepCurving;
}
-double LeftAndRightFootTrajectoryGenerationMultiple::GetStepCurving() const
+double LeftAndRightFootTrajectoryGenerationMultiple::
+GetStepCurving() const
{
return m_StepCurving;
}
-double LeftAndRightFootTrajectoryGenerationMultiple::GetAbsoluteTimeReference() const
+double LeftAndRightFootTrajectoryGenerationMultiple::
+GetAbsoluteTimeReference() const
{
double res=0.0;
double LeftATR=0.0,RightATR=0.0;
@@ -1045,7 +1111,8 @@ double LeftAndRightFootTrajectoryGenerationMultiple::GetAbsoluteTimeReference()
return res;
}
-void LeftAndRightFootTrajectoryGenerationMultiple::SetAbsoluteTimeReference(double anATR)
+void LeftAndRightFootTrajectoryGenerationMultiple::
+SetAbsoluteTimeReference(double anATR)
{
if (m_LeftFootTrajectory!=0)
m_LeftFootTrajectory->SetAbsoluteTimeReference(anATR);
@@ -1075,14 +1142,16 @@ LeftAndRightFootTrajectoryGenerationMultiple::operator=
}
-FootTrajectoryGenerationMultiple * LeftAndRightFootTrajectoryGenerationMultiple::getLeftFootTrajectory() const
+FootTrajectoryGenerationMultiple *
+LeftAndRightFootTrajectoryGenerationMultiple::
+getLeftFootTrajectory() const
{
return m_LeftFootTrajectory;
}
-FootTrajectoryGenerationMultiple * LeftAndRightFootTrajectoryGenerationMultiple::getRightFootTrajectory() const
+FootTrajectoryGenerationMultiple *
+LeftAndRightFootTrajectoryGenerationMultiple::
+getRightFootTrajectory() const
{
return m_RightFootTrajectory;
}
-
-
--
GitLab