Commit 977ce2be authored by student's avatar student
Browse files

Update Algo Stepping stair Morisawa

parent 7bfdc238
......@@ -90,8 +90,8 @@ namespace PatternGeneratorJRL
friend std::ostream & operator<<(std::ostream &os, const struct COMState_s & acs);
};
typedef struct COMState_s COMState;
......@@ -99,11 +99,11 @@ namespace PatternGeneratorJRL
a sequence of relative positions. */
struct RelativeFootPosition_s
{
double sx,sy,theta;
double sx,sy,sz,theta;
double SStime;
double DStime;
int stepType; //1:normal walking 2:one step before obstacle
//3:first leg over obstacle 4:second leg over obstacle 5:one step after obstacle
//3:first leg over obstacle 4:second leg over obstacle 5:one step after obstacle 6 :stepping stair
double DeviationHipHeight;
};
typedef struct RelativeFootPosition_s RelativeFootPosition;
......
......@@ -41,6 +41,7 @@ FootTrajectoryGenerationAbstract::FootTrajectoryGenerationAbstract(SimplePluginM
m_Foot= aFoot;
m_SamplingPeriod = 0.005;
m_isStepStairOn = 1;
m_stairs = 0.0;
std::string aMethodName[5] =
{":omega",
......@@ -105,3 +106,14 @@ void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolut
{
LTHROW("FootTrajectoryGenerationAbstract::UpdateFootPosition-2: To be implemented ");
}
double FootTrajectoryGenerationAbstract::GetStairs()
{
return m_stairs;
}
void FootTrajectoryGenerationAbstract::SetStairs(double stairs)
{
m_stairs = stairs;
}
......@@ -192,6 +192,10 @@ namespace PatternGeneratorJRL
*/
virtual void CallMethod(std::string &Method, std::istringstream &strm);
double GetStairs();
void SetStairs(double stairs);
protected:
/*! Sampling period of the control. */
......@@ -210,6 +214,8 @@ namespace PatternGeneratorJRL
double m_Omega;
int m_isStepStairOn;
double m_stairs;
};
......
/*
* Copyright 2008, 2009, 2010,
* Copyright 2008, 2009, 2010,
*
* Torea Foissotte
* Olivier Stasse
......@@ -19,7 +19,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with jrl-walkgen. If not, see <http://www.gnu.org/licenses/>.
*
* Research carried out within the scope of the
* Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
......@@ -52,21 +52,21 @@ void FootTrajectoryGenerationMultiple::SetNumberOfIntervals(int lNumberOfInterva
{
if (m_SetOfFootTrajectoryGenerationObjects.size()==(unsigned int)lNumberOfIntervals)
return;
for(unsigned int i=0;i<m_SetOfFootTrajectoryGenerationObjects.size();i++)
{
delete m_SetOfFootTrajectoryGenerationObjects[i];
}
m_SetOfFootTrajectoryGenerationObjects.resize(lNumberOfIntervals);
for(unsigned int i=0;i<m_SetOfFootTrajectoryGenerationObjects.size();i++)
{
m_SetOfFootTrajectoryGenerationObjects[i] = new FootTrajectoryGenerationStandard(getSimplePluginManager(),m_Foot);
m_SetOfFootTrajectoryGenerationObjects[i]->InitializeInternalDataStructures();
}
}
m_NatureOfIntervals.resize(lNumberOfIntervals);
}
int FootTrajectoryGenerationMultiple::GetNumberOfIntervals() const
{
return m_SetOfFootTrajectoryGenerationObjects.size();
......@@ -78,14 +78,14 @@ void FootTrajectoryGenerationMultiple::SetTimeIntervals(const vector<double> &lD
m_DeltaTj = lDeltaTj;
m_RefTime.resize(lDeltaTj.size());
double reftime=0.0;
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 );
reftime+=m_DeltaTj[li];
}
}
void FootTrajectoryGenerationMultiple::GetTimeIntervals(vector<double> &lDeltaTj) const
......@@ -100,27 +100,27 @@ bool FootTrajectoryGenerationMultiple::Compute(int axis, double t, double &resul
double reftime=0;
ODEBUG(" ====== CoM ====== ");
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]);
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);
}
return true;
}
reftime+=m_DeltaTj[j];
}
ODEBUG(" reftime :" << reftime );
return false;
}
......@@ -129,62 +129,79 @@ bool FootTrajectoryGenerationMultiple::Compute(double t, FootAbsolutePosition &
{
double deltaj = t - m_AbsoluteTimeReference - m_RefTime[IndexInterval];
ODEBUG("IndexInterval : " << IndexInterval );
m_SetOfFootTrajectoryGenerationObjects[IndexInterval]->ComputeAll(aFootAbsolutePosition,deltaj);
aFootAbsolutePosition.stepType = m_NatureOfIntervals[IndexInterval];
return true;
}
/*bool FootTrajectoryGenerationMultiple::Compute(double t, deque<FootAbsolutePosition> &adFootAbsolutePosition, unsigned int IndexInterval)
{
double deltaj = t - m_AbsoluteTimeReference - m_RefTime[IndexInterval];
ODEBUG("IndexInterval : " << IndexInterval );
// m_SetOfFootTrajectoryGenerationObjects[IndexInterval]->m_BsplinesZ->SetParametersWithInitPos(adFootAbsolutePosition.back().z,m_SetOfFootTrajectoryGenerationObjects[IndexInterval]->m_BsplinesZ->GetFT(),m_SetOfFootTrajectoryGenerationObjects[IndexInterval]->m_BsplinesZ->GetMP()/1.5,m_SetOfFootTrajectoryGenerationObjects[IndexInterval]->m_BsplinesZ->GetMP(),m_SetOfFootTrajectoryGenerationObjects[IndexInterval]->m_BsplinesZ->GetFT()/3.0);
m_SetOfFootTrajectoryGenerationObjects[IndexInterval]->ComputeAll(adFootAbsolutePosition,deltaj);
// cout << IndexInterval<< " "<<m_DeltaTj[IndexInterval] << " "<< deltaj << " " << t << " "<< adFootAbsolutePosition.back().z<<" " <<m_AbsoluteTimeReference << " " << m_RefTime[IndexInterval] << endl;
//m_SetOfFootTrajectoryGenerationObjects[IndexInterval+1]->SetStairs( (m_SetOfFootTrajectoryGenerationObjects[IndexInterval]->GetStairs()));
adFootAbsolutePosition[IndexInterval].stepType = m_NatureOfIntervals[IndexInterval];
return true;
}*/
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 <<
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]
" 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))
{
double deltaj=0.0;
deltaj = t-reftime;
if (m_SetOfFootTrajectoryGenerationObjects[j]!=0)
{
{
m_SetOfFootTrajectoryGenerationObjects[j]->ComputeAll(aFootAbsolutePosition,deltaj);
aFootAbsolutePosition.stepType = m_NatureOfIntervals[j];
}
ODEBUG("t: " << t << " reftime :" << setprecision(12) << reftime <<
" AbsoluteTimeReference : " << m_AbsoluteTimeReference <<
" Tj["<<j << "]= " << setprecision(12) << m_DeltaTj[j]
" 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 <<
ODEBUG("X: " << aFootAbsolutePosition.x <<
" Y: " << aFootAbsolutePosition.y <<
" Z: " << aFootAbsolutePosition.z <<
" Theta: " << aFootAbsolutePosition.theta <<
" Omega: " << aFootAbsolutePosition.omega <<
" stepType: " << aFootAbsolutePosition.stepType <<
" Omega: " << aFootAbsolutePosition.omega <<
" stepType: " << aFootAbsolutePosition.stepType <<
" NI: " << m_NatureOfIntervals[j] <<
" interval : " << j);
return true;
}
reftime+=m_DeltaTj[j];
}
ODEBUG(" reftime :" << reftime <<
" m_AbsoluteReferenceTime" << m_AbsoluteTimeReference);
ODEBUG("t: " << setprecision(12) << t << " reftime :" << reftime <<
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.
/*! This method specifies the nature of the interval.
*/
int FootTrajectoryGenerationMultiple::SetNatureInterval(unsigned int IntervalIndex,
int Nature)
......@@ -193,24 +210,24 @@ int FootTrajectoryGenerationMultiple::SetNatureInterval(unsigned int IntervalInd
return -1;
m_NatureOfIntervals[IntervalIndex] = Nature;
return 0;
}
/*! This method returns the nature of the interval.
/*! This method returns the nature of the interval.
*/
int FootTrajectoryGenerationMultiple::GetNatureInterval(unsigned int IntervalIndex) const
{
if (IntervalIndex>=m_NatureOfIntervals.size())
return -100;
return m_NatureOfIntervals[IntervalIndex];
}
/*! 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.
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.
......@@ -237,8 +254,8 @@ int FootTrajectoryGenerationMultiple::SetParametersWithInitPosInitSpeed(unsigned
/*! 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.
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.
*/
......@@ -291,8 +308,8 @@ int FootTrajectoryGenerationMultiple::SetParametersWithInitPosInitSpeedInitAcc(u
/*! 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.
@param PolynomeIndex: Set to which axis the parameters will be applied.
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.
......@@ -332,7 +349,7 @@ void FootTrajectoryGenerationMultiple::SetAbsoluteTimeReference(double lAbsolute
void FootTrajectoryGenerationMultiple::CallMethod(std::string &, //Method,
std::istringstream & ) //strm)
{
}
int FootTrajectoryGenerationMultiple::DisplayIntervals() const
{
......@@ -343,7 +360,7 @@ int FootTrajectoryGenerationMultiple::DisplayIntervals() const
return 0;
}
FootTrajectoryGenerationMultiple &
FootTrajectoryGenerationMultiple &
FootTrajectoryGenerationMultiple::operator=
(const FootTrajectoryGenerationMultiple & aFTGM)
{
......
/*
* Copyright 2008, 2009, 2010,
* Copyright 2008, 2009, 2010,
*
* Torea Foissotte
* Olivier Stasse
......@@ -19,13 +19,13 @@
* You should have received a copy of the GNU Lesser General Public License
* along with jrl-walkgen. If not, see <http://www.gnu.org/licenses/>.
*
* Research carried out within the scope of the
* Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/*! \file FootTrajectoryGenerationMultiple.h
\brief This object is in charge of maintaining the foot trajectory
generation for several intervals.
generation for several intervals.
It relies on the FootTrajectoryGenerationStandard class.
@ingroup foottrajectorygeneration
......@@ -41,7 +41,7 @@ namespace PatternGeneratorJRL
{
/*! @ingroup foottrajectorygeneration
This class generates a trajectory for a complete leg relying on a
set of description of the intervals.
More precisely this object handles a set of FootTrajectoryGenerationStandard
......@@ -50,10 +50,10 @@ namespace PatternGeneratorJRL
of foot trajectory.
Each interval is described by a time duration \f[ \Delta T_j \f],
its nature which can be double support and in single support
its nature which can be double support and in single support
two subcategories exist: support foot and flying.
*/
class FootTrajectoryGenerationMultiple : public SimplePlugin
{
......@@ -81,14 +81,14 @@ namespace PatternGeneratorJRL
// Default destructor
~FootTrajectoryGenerationMultiple();
/*! \brief Reimplementation of the call method for the plugin manager.
/*! \brief Reimplementation of the call method for the plugin manager.
More explicitly this object will deal with the call which initialize
the feet behaviors (\f$omega\f$, \f$ stepheight \f$) .
*/
virtual void CallMethod(std::string &Method, std::istringstream &strm);
/*! \name Methods related to the handling of the intervals.
@{
*/
......@@ -100,7 +100,7 @@ namespace PatternGeneratorJRL
/*! \brief Set the time for each interval. */
void SetTimeIntervals(const std::vector<double> & lDeltaTj);
/*! \brief Get the time for each interval */
void GetTimeIntervals(std::vector<double> & lDeltaTj) const;
......@@ -112,22 +112,26 @@ namespace PatternGeneratorJRL
/*! \brief Display intervals time. */
int DisplayIntervals() const;
/*! @} */
/*! \brief Compute the value asked for according to :
@param[in] axis: the axis along which the computation is done,
@param[in] t: the time,
@param[out] r: the result.
*/
bool Compute(int axis, double t, double & r);
/*! \brief Compute the value asked for according to :
@param[in] t: the time,
@param[out] aFootAbsolutePosition: a foot absolute position.
*/
bool Compute(double t, FootAbsolutePosition & aFootAbsolutePosition);
/*
bool Compute(double t, std::deque<FootAbsolutePosition> & adFootAbsolutePosition, unsigned int IndexInterval);
*/
/*! \brief Compute the value asked for according to :
@param[in] t: the time,
@param[in] IndexInterval: Index of the interval to be used for the computation.
......@@ -137,8 +141,8 @@ namespace PatternGeneratorJRL
/*! 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.
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.
*/
......@@ -146,11 +150,11 @@ namespace PatternGeneratorJRL
int AxisReference,
double TimeInterval,
double FinalPosition);
/*! 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.
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.
......@@ -186,8 +190,8 @@ namespace PatternGeneratorJRL
/*! This method gets 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.
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.
......@@ -201,8 +205,8 @@ namespace PatternGeneratorJRL
double &InitPosition,
double &InitSpeed);
/*! \name Methods related to the Absolute Time Reference.
This time specifies the beginning of the trajectory.
/*! \name Methods related to the Absolute Time Reference.
This time specifies the beginning of the trajectory.
@{ */
/*! Returns the time when the trajectory starts. */
......@@ -210,9 +214,9 @@ namespace PatternGeneratorJRL
/*! Set the time when the trajectory starts. */
void SetAbsoluteTimeReference(double lAbsoluteTimeReference);
/*! @} */
FootTrajectoryGenerationMultiple & operator=(const FootTrajectoryGenerationMultiple & aFTGM);
protected:
......@@ -220,7 +224,7 @@ namespace PatternGeneratorJRL
/*! \brief Handle a set of object allowing the generation of the foot trajectory.*/
std::vector<FootTrajectoryGenerationStandard *> m_SetOfFootTrajectoryGenerationObjects;
/*! \brief Reference of humanoid specificities. */
/*! \brief Reference of humanoid specificities. */
CjrlFoot * m_Foot;
/*! \brief Set the absolute reference time for this set of intervals. */
......@@ -234,7 +238,7 @@ namespace PatternGeneratorJRL
/*! \brief Reference time for the polynomials. */
std::vector<double> m_RefTime;
/*! \brief Sensitivity to numerical unstability when using time. */
double m_Sensitivity;
......
......@@ -177,8 +177,8 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex,
}
else
{
m_BsplinesZ->SetParameters(TimeInterval,Position/1.5,TimeInterval/3.0,Position);
cout << "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXxx" << endl;
m_BsplinesZ->SetParameters(TimeInterval,Position,TimeInterval/3.0,Position*1.5);
// cout << "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXxx" << endl;
}
break;
......@@ -226,8 +226,11 @@ int FootTrajectoryGenerationStandard::SetParametersWithInitPosInitSpeed(int Poly
}
else
{
m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition/1.5,TimeInterval/3.0,FinalPosition);
}
//if (InitPosition < FinalPosition)
m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition,TimeInterval/3.0,InitPosition+(FinalPosition-InitPosition)*1.5);
// else if (InitPosition == FinalPosition)
// m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition,TimeInterval/3.0,FinalPosition);
}
break;
case THETA_AXIS:
......@@ -270,7 +273,10 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex, double Ti
}
else
{
m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition/1.5,TimeInterval/3.0,FinalPosition);
//if (InitPosition <= FinalPosition)
m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition,TimeInterval/3.0,InitPosition+(FinalPosition-InitPosition)*1.5);
// else if (InitPosition == FinalPosition)
// m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition,TimeInterval/3.0,FinalPosition);
}
break;
......@@ -342,6 +348,73 @@ int FootTrajectoryGenerationStandard::GetParametersWithInitPosInitSpeed(int Poly
return 0;
}
double FootTrajectoryGenerationStandard::ComputeAll(std::deque<FootAbsolutePosition> & adFootAbsolutePosition,double Time)
{
FootAbsolutePosition aFootAbsolutePosition;
aFootAbsolutePosition.x = m_PolynomeX->Compute(Time);
aFootAbsolutePosition.dx = m_PolynomeX->ComputeDerivative(Time);
// aFootAbsolutePosition.ddx = m_PolynomeX->ComputeSecDerivative(Time);
ODEBUG2("t: " << Time << " : " << aFootAbsolutePosition.x);
aFootAbsolutePosition.y = m_PolynomeY->Compute(Time);
aFootAbsolutePosition.dy = m_PolynomeY->ComputeDerivative(Time);
// aFootAbsolutePosition.ddy = m_PolynomeY->ComputeSecDerivative(Time);
if (m_isStepStairOn == 0)
{
aFootAbsolutePosition.z = m_PolynomeZ->Compute(Time);
aFootAbsolutePosition.dz = m_PolynomeZ->ComputeDerivative(Time);
// aFootAbsolutePosition.ddz = m_PolynomeZ->ComputeSecDerivative(Time);
}
else
{
/* if (m_BsplinesZ->ZComputePosition(Time)==0)
{
// cout << m_BsplinesZ->ZComputePosition(Time) << endl;
if (adFootAbsolutePosition.size() > 0)
aFootAbsolutePosition.z = adFootAbsolutePosition.back().z;
else
aFootAbsolutePosition.z =0.0;
// m_BsplinesZ->SetParametersWithInitPos(adFootAbsolutePosition.back().z,m_BsplinesZ->GetFT(),m_BsplinesZ->GetMP()/1.5,m_BsplinesZ->GetMP(),m_BsplinesZ->GetFT()/3.0);
// m_BsplinesZ->SetParametersWithInitPos(aFootAbsolutePosition.z,m_BsplinesZ->GetFT(),m_BsplinesZ->GetMP()/1.5,m_BsplinesZ->GetMP(),m_BsplinesZ->GetFT()/3.0);
// cout << m_BsplinesZ->ZComputePosition(Time) << endl;
}
else
{
// cout << adFootAbsolutePosition.back().z << " " << m_BsplinesZ->GetFT() << " " << m_BsplinesZ->GetMP()/1.5 << " " << m_BsplinesZ->GetMP() << " " << m_BsplinesZ->GetFT()/3.0 << endl;
// m_BsplinesZ->SetParametersWithInitPos(aFootAbsolutePosition.z,m_BsplinesZ->GetFT(),m_BsplinesZ->GetMP()/1.5,m_BsplinesZ->GetMP(),m_BsplinesZ->GetFT()/3.0);
aFootAbsolutePosition.z = m_BsplinesZ->ZComputePosition(Time);
// cout << "ooooooooooo "<<m_stairs<< endl;
}*/
aFootAbsolutePosition.z = m_BsplinesZ->ZComputePosition(Time);
aFootAbsolutePosition.dz = m_BsplinesZ->ZComputeVelocity(Time);
}
aFootAbsolutePosition.theta = m_PolynomeTheta->Compute(Time);
aFootAbsolutePosition.dtheta = m_PolynomeTheta->ComputeDerivative(Time);
aFootAbsolutePosition.omega = m_PolynomeOmega->Compute(Time);
aFootAbsolutePosition.domega = m_PolynomeOmega->ComputeDerivative(Time);
aFootAbsolutePosition.omega2 = m_PolynomeOmega2->Compute(Time);
aFootAbsolutePosition.domega2 = m_PolynomeOmega2->ComputeDerivative(Time);
//
adFootAbsolutePosition.push_back(aFootAbsolutePosition);
// cout << "AAAAAAAAAAAAAAAA "<< Time << " "<< m_BsplinesZ->GetFT() <<" "<< m_BsplinesZ->GetMP() <<" "<< adFootAbsolutePosition.back().z << endl;
return Time;
}