Commit cc1378b7 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Untabify and reindent everything.

parent def5f182
......@@ -24,10 +24,10 @@
*/
/*!\file FootTrajectoryGenerationAbstract.h
\brief This class determinate how it s generate all the values
for the foot trajectories.
\brief This class determinate how it s generate all the values
for the foot trajectories.
@ingroup foottrajectorygeneration
@ingroup foottrajectorygeneration
*/
#include <Debug.hh>
#include "FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh"
......@@ -60,15 +60,15 @@ FootTrajectoryGenerationAbstract
for (int i=0;i<5;i++)
{
if (!RegisterMethod(aMethodName[i]))
{
std::cerr << "Unable to register " << aMethodName << std::endl;
}
{
std::cerr << "Unable to register " << aMethodName << std::endl;
}
}
}
void FootTrajectoryGenerationAbstract::CallMethod(std::string &Method,
std::istringstream &strm)
std::istringstream &strm)
{
if (Method==":omega")
{
......
......@@ -23,9 +23,9 @@
*/
/*!\file FootTrajectoryGenerationAbstract.h
\brief This class determinate how it s generate all the values for the foot trajectories.
\brief This class determinate how it s generate all the values for the foot trajectories.
@ingroup foottrajectorygeneration
@ingroup foottrajectorygeneration
*/
......@@ -84,7 +84,7 @@ namespace PatternGeneratorJRL
/*! Constructor: In order to compute some appropriate strategies,
this class needs to extract specific details from the humanoid model. */
FootTrajectoryGenerationAbstract(SimplePluginManager *lSPM,
PRFoot *inFoot) ;
PRFoot *inFoot) ;
/*! Default destructor. */
......@@ -106,18 +106,18 @@ namespace PatternGeneratorJRL
@param LeftOrRight: Specify if it is left (1) or right (-1).
*/
virtual void UpdateFootPosition(std::deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int CurrentAbsoluteIndex,
int IndexInitial,
double ModulatedSingleSupportTime,
int StepType, int LeftOrRight);
std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int CurrentAbsoluteIndex,
int IndexInitial,
double ModulatedSingleSupportTime,
int StepType, int LeftOrRight);
virtual void UpdateFootPosition(std::deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int StartIndex, int k,
double LocalInterpolationStartTime,
double ModulatedSingleSupportTime,
int StepType, int LeftOrRight);
std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int StartIndex, int k,
double LocalInterpolationStartTime,
double ModulatedSingleSupportTime,
int StepType, int LeftOrRight);
/*! Initialize internal data structures. */
virtual void InitializeInternalDataStructures()=0;
......@@ -127,7 +127,7 @@ namespace PatternGeneratorJRL
/*! \name Setter and getter for parameters
@{
*/
*/
/*! \name Single Support Time
@{
......
......@@ -72,10 +72,10 @@ SetNumberOfIntervals
i++)
{
m_SetOfFootTrajectoryGenerationObjects[i] =
new FootTrajectoryGenerationStandard
(getSimplePluginManager(),m_Foot);
new FootTrajectoryGenerationStandard
(getSimplePluginManager(),m_Foot);
m_SetOfFootTrajectoryGenerationObjects[i]->
InitializeInternalDataStructures();
InitializeInternalDataStructures();
}
m_NatureOfIntervals.resize(lNumberOfIntervals);
}
......@@ -98,9 +98,9 @@ SetTimeIntervals
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];
}
......@@ -121,27 +121,27 @@ Compute(int axis, double t, double &result)
double reftime=0;
ODEBUG(" ====== CoM ====== ");
ODEBUG(" t: " << t << " reftime :" << reftime << " m_Sensitivity: "
<< m_Sensitivity <<" m_DeltaTj.size(): "<< m_DeltaTj.size() );
<< 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]);
<< " 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;
}
(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];
}
......@@ -174,16 +174,16 @@ Compute
aFootAbsolutePosition.stepType =
m_NatureOfIntervals[IndexInterval];
/* ofstream aof;
string aFileName;
/* 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.close();*/
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.close();*/
return true;
}
......@@ -197,71 +197,71 @@ Compute
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() );
<< " 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) );
<< 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))
{
double deltaj=0.0;
deltaj = t-reftime;
if (m_SetOfFootTrajectoryGenerationObjects[j]!=0)
{
//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("X: " << aFootAbsolutePosition.x <<
" Y: " << aFootAbsolutePosition.y <<
" Z: " << aFootAbsolutePosition.z <<
" Theta: " << aFootAbsolutePosition.theta <<
" Omega: " << aFootAbsolutePosition.omega <<
" stepType: " << aFootAbsolutePosition.stepType <<
" NI: " << m_NatureOfIntervals[j] <<
" interval : " << j);
return true;
}
(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);
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("X: " << aFootAbsolutePosition.x <<
" Y: " << aFootAbsolutePosition.y <<
" Z: " << aFootAbsolutePosition.z <<
" Theta: " << aFootAbsolutePosition.theta <<
" 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);
" m_AbsoluteReferenceTime" << m_AbsoluteTimeReference);
ODEBUG("t: " << setprecision(12) << t
<< " reftime :" << reftime
<< " m_Sensitivity: "<< m_Sensitivity
<< " m_DeltaTj.size(): "<< m_DeltaTj.size() );
<< " 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 Nature)
{
if (IntervalIndex>=m_NatureOfIntervals.size())
return -1;
......@@ -271,7 +271,7 @@ SetNatureInterval(unsigned int IntervalIndex,
}
/*! This method returns the nature of the interval.
*/
*/
int FootTrajectoryGenerationMultiple::
GetNatureInterval(unsigned int IntervalIndex) const
{
......@@ -479,14 +479,14 @@ FootTrajectoryGenerationMultiple::operator=
{
double TI, FP, IP, IS;
for(lk=0;lk<6;lk++)
{
GetParametersWithInitPosInitSpeed(li,lk,TI,FP,IP,IS);
/*! Special case when TI is equal to zero */
if (TI==0.0)
SetParameters(li,lk,TI,FP);
else
SetParametersWithInitPosInitSpeed(li,lk,TI,FP,IP,IS);
}
{
GetParametersWithInitPosInitSpeed(li,lk,TI,FP,IP,IS);
/*! Special case when TI is equal to zero */
if (TI==0.0)
SetParameters(li,lk,TI,FP);
else
SetParametersWithInitPosInitSpeed(li,lk,TI,FP,IP,IS);
}
}
return *this;
}
......@@ -42,25 +42,25 @@ 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
objects.
Thus it acts as a container, and allow a coherent interface to a set
of foot trajectory.
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
objects.
Thus it acts as a container, and allow a coherent interface to a set
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
two subcategories exist: support foot and flying.
Each interval is described by a time duration \f[ \Delta T_j \f],
its nature which can be double support and in single support
two subcategories exist: support foot and flying.
*/
*/
class FootTrajectoryGenerationMultiple : public SimplePlugin
{
public :
/*! \name Constants to define the nature of the foot trajectory.
@{
@{
*/
/*! Double support foot. */
const static int DOUBLE_SUPPORT=0;
......@@ -77,7 +77,7 @@ namespace PatternGeneratorJRL
/*! Constructor: In order to compute some appropriate strategies,
this class needs to extract specific details from the humanoid model. */
FootTrajectoryGenerationMultiple(SimplePluginManager *lSPM,
PRFoot *aFoot);
PRFoot *aFoot);
// Default destructor
~FootTrajectoryGenerationMultiple();
......@@ -91,7 +91,7 @@ namespace PatternGeneratorJRL
/*! \name Methods related to the handling of the intervals.
@{
*/
*/
/*! \brief Set number of intervals. */
void SetNumberOfIntervals(int lNumberOfIntervals);
......@@ -157,35 +157,15 @@ namespace PatternGeneratorJRL
@param InitPosition: Initial position when computing the polynome at t= m_AbsoluteTimeReference.
@param InitSpeed: Initial speed when computing the polynome at t=m_AbsoluteTimeReference.
*/
int SetParametersWithInitPosInitSpeed(unsigned int PolynomeIndex,
int AxisReference,
double TimeInterval,
double FinalPosition,
double InitPosition,
double InitSpeed,
vector<double> MiddlePos=vector<double>(3,-1));
/*! 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 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.
*/
int SetParametersWithInitPosInitSpeedInitAcc(unsigned int PolynomeIndex,
int AxisReference,
double TimeInterval,
double FinalPosition,
double InitPosition,
double InitSpeed,
double InitAcc,
vector<double> middlePos = vector<double>(3,-1));
int SetParametersWithInitPosInitSpeed(unsigned int PolynomeIndex,
int AxisReference,
double TimeInterval,
double FinalPosition,
double InitPosition,
double InitSpeed,
vector<double> MiddlePos=vector<double>(3,-1));
/*! This method gets the parameters for each of the polynome used by this
/*! 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.
......@@ -194,27 +174,47 @@ namespace PatternGeneratorJRL
@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.
*/
int GetParametersWithInitPosInitSpeed(unsigned int PolynomeIndex,
int AxisReference,
double &TimeInterval,
double &FinalPosition,
double &InitPosition,
double &InitSpeed);
int SetParametersWithInitPosInitSpeedInitAcc(unsigned int PolynomeIndex,
int AxisReference,
double TimeInterval,
double FinalPosition,
double InitPosition,
double InitSpeed,
double InitAcc,
vector<double> middlePos = vector<double>(3,-1));
/*! \name Methods related to the Absolute Time Reference.
This time specifies the beginning of the trajectory.
@{ */
/*! 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.
@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.
*/
int GetParametersWithInitPosInitSpeed(unsigned int PolynomeIndex,
int AxisReference,
double &TimeInterval,
double &FinalPosition,
double &InitPosition,
double &InitSpeed);
/*! \name Methods related to the Absolute Time Reference.
This time specifies the beginning of the trajectory.
@{ */
/*! Returns the time when the trajectory starts. */
double GetAbsoluteTimeReference() const;
/*! Returns the time when the trajectory starts. */
double GetAbsoluteTimeReference() const;
/*! Set the time when the trajectory starts. */
void SetAbsoluteTimeReference(double lAbsoluteTimeReference);
/*! Set the time when the trajectory starts. */
void SetAbsoluteTimeReference(double lAbsoluteTimeReference);
/*! @} */
/*! @} */
FootTrajectoryGenerationMultiple & operator=(const FootTrajectoryGenerationMultiple & aFTGM);
FootTrajectoryGenerationMultiple & operator=(const FootTrajectoryGenerationMultiple & aFTGM);
protected:
......
......@@ -24,7 +24,7 @@
*/
/*! \file FootTrajectoryGenerationStandard.h
\brief This object generate all the values for the foot trajectories.
@ingroup foottrajectorygeneration */
@ingroup foottrajectorygeneration */
#ifndef _FOOT_TRAJECTORY_GENERATION_STANDARD_H_
......@@ -92,150 +92,150 @@ namespace PatternGeneratorJRL
@param StepType: Type of steps (for book-keeping).
@param LeftOrRight: Specify if it is left (1) or right (-1).
*/
virtual void UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int CurrentAbsoluteIndex,
int IndexInitial,
double ModulatedSingleSupportTime,
int StepType,int LeftOrRight);
virtual void UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int StartIndex, int k,
double LocalInterpolationStartTime,
double ModulatedSingleSupportTime,
int StepType, int LeftOrRight);
/*! Initialize internal data structures.
In this specific case, it is in charge of creating the polynomial structures.
virtual void UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int CurrentAbsoluteIndex,
int IndexInitial,
double ModulatedSingleSupportTime,
int StepType,int LeftOrRight);
virtual void UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int StartIndex, int k,
double LocalInterpolationStartTime,
double ModulatedSingleSupportTime,
int StepType, int LeftOrRight);
/*! Initialize internal data structures.
In this specific case, it is in charge of creating the polynomial structures.
*/
virtual void InitializeInternalDataStructures();
virtual void InitializeInternalDataStructures();
/*! Free internal data structures.
In this specific case, it is in charge of freeing the polynomial data structures.
/*! Free internal data structures.
In this specific case, it is in charge of freeing the polynomial data structures.
*/
virtual void FreeInternalDataStructures();
/*! 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.
It assumes an initial position and an initial speed set to zero.
@param[in] AxisReference: Set to which axis the parameters will be applied.
@param[in] TimeInterval: Set the time base of the polynome.
@param[in] Position: Set the final position of the polynome at TimeInterval.
*/
int SetParameters(int AxisReference,
double TimeInterval,
double Position);
/*! 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[in] AxisReference: Set to which axis the parameters will be applied.
@param[in] TimeInterval: Set the time base of the polynome.
@param[in] FinalPosition: Set the final position of the polynome at TimeInterval.
@param[in] InitPosition: Initial position when computing the polynome at t=0.0.
@param[in] InitSpeed: Initial speed when computing the polynome at t=0.0.
*/
int SetParametersWithInitPosInitSpeed(int AxisReference,
double TimeInterval,
double FinalPosition,
double InitPosition,
double InitSpeed,
vector<double> MiddlePos=vector<double>(3,-1));