Commit 0b665a37 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Astyle + emacs ident through script

Did not find a way to automatize the 80 cols policy.
parent cc1378b7
/*
* Copyright 2005, 2006, 2007, 2008, 2009, 2010,
* Copyright 2005, 2006, 2007, 2008, 2009, 2010,
*
* Olivier Stasse
*
......@@ -18,20 +18,20 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. 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 Clock.cpp
\brief This object allows to make time measurement on the code.
Copyright (c) 2008,
Olivier Stasse,
JRL-Japan, CNRS/AIST
Copyright (c) 2008,
Olivier Stasse,
All rights reserved.
JRL-Japan, CNRS/AIST
Please see license.txt for more information on license.
All rights reserved.
Please see license.txt for more information on license.
*/
#include <iostream>
......@@ -81,8 +81,10 @@ void Clock::StopTiming()
m_MaximumTime = m_MaximumTime < ltime ? ltime : m_MaximumTime;
m_TotalTime += ltime;
m_DataBuffer[(m_NbOfIterations*2)%3000000]=m_BeginTimeStamp.tv_sec +
0.000001 * m_BeginTimeStamp.tv_usec - m_StartingTime;
m_DataBuffer[(m_NbOfIterations*2)%3000000]=
m_BeginTimeStamp.tv_sec +
0.000001 * m_BeginTimeStamp.tv_usec -
m_StartingTime;
m_DataBuffer[(m_NbOfIterations*2+1)%3000000]=ltime;
}
......@@ -94,7 +96,7 @@ void Clock::IncIteration(int lNbOfIts)
void Clock::RecordDataBuffer(std::string filename)
{
std::ofstream aof(filename.c_str());
for(unsigned int i=0;i<2*m_NbOfIterations%300000;i+=2)
for(unsigned int i=0; i<2*m_NbOfIterations%300000; i+=2)
aof << m_DataBuffer[i]<< " " << m_DataBuffer[i+1] << std::endl;
aof.close();
}
......@@ -126,5 +128,5 @@ void Clock::Display()
std::cout << "Total Time : " << TotalTime() << std::endl;
std::cout << "Max Time : " << MaxTime() << std::endl;
std::cout << "Nb of iterations: " << NbOfIterations() << std::endl;
}
......@@ -24,7 +24,7 @@
*/
/*! \file Clock.h
\brief Defines an object for measuring the time spend in some code part.
\brief Defines an object for measuring the time spend in some code part.
*/
#ifndef _HWPG_CLOCK_H_
# define _HWPG_CLOCK_H_
......
/*
* Copyright 2005, 2006, 2007, 2008, 2009, 2010,
* Copyright 2005, 2006, 2007, 2008, 2009, 2010,
*
* Olivier Stasse
*
......@@ -18,7 +18,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. 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)
*/
/* @doc File for debugging messages. */
......@@ -26,64 +26,67 @@
#include <fstream>
#include <exception>
#define LTHROW(x) \
{ \
class Exception: public std::exception \
{ \
#define LTHROW(x) \
{ \
class Exception: public std::exception \
{ \
virtual const char * what() const throw() \
{ \
return x; \
} \
}; \
\
Exception almsg; \
throw almsg; }
{ \
return x; \
} \
}; \
\
Exception almsg; \
throw almsg; }
#define ODEBUG2(x)
#define ODEBUG3(x) std::cerr << __FILE__ << ":" \
<< __FUNCTION__ << "(#" \
<< __LINE__ << "):" << x << std::endl;
#define RESETDEBUG5(y) { std::ofstream DebugFile; \
DebugFile.open(y,ofstream::out); \
DebugFile.close();}
#define ODEBUG5(x,y) { std::ofstream DebugFile; \
DebugFile.open(y,ofstream::app); \
DebugFile << __FILE__ << ":" \
<< __FUNCTION__ << "(#" \
<< __LINE__ << "):" << x << std::endl; \
DebugFile.close();}
#define ODEBUG5SIMPLE(x,y) { std::ofstream DebugFile; \
DebugFile.open(y,ofstream::app); \
DebugFile << x << std::endl; \
DebugFile.close();}
<< __FUNCTION__ << "(#" \
<< __LINE__ << "):" << x << std::endl;
#define RESETDEBUG5(y) { std::ofstream DebugFile; \
DebugFile.open(y,ofstream::out); \
DebugFile.close();}
#define ODEBUG5(x,y) { std::ofstream DebugFile; \
DebugFile.open(y,ofstream::app); \
DebugFile << __FILE__ << ":" \
<< __FUNCTION__ << "(#" \
<< __LINE__ << "):" << x << std::endl; \
DebugFile.close();}
#define ODEBUG5SIMPLE(x,y) { std::ofstream DebugFile; \
DebugFile.open(y,ofstream::app); \
DebugFile << x << std::endl; \
DebugFile.close();}
#define ODEBUG3_NENDL(x) std::cerr << x
#ifdef _DEBUG_MODE_ON_
#define ODEBUG(x) std::cerr << __FILE__ << ":" \
<< __FUNCTION__ << "(#" \
<< __LINE__ << "):" << x << std::endl;
#define ODEBUG(x) std::cerr << __FILE__ << ":" \
<< __FUNCTION__ << "(#" \
<< __LINE__ << "):" << x << std::endl;
#define ODEBUG_NENDL(x) std::cerr << x
#define ODEBUG_CODE(x) x
#else
#define ODEBUG(x)
#define ODEBUG_NENDL(x)
#define ODEBUG_NENDL(x)
#define ODEBUG_CODE(x)
#endif
#ifdef _DEBUG_MODE_ON_
#define RESETDEBUG4(y) { std::ofstream DebugFile; DebugFile.open(y,ofstream::out); DebugFile.close();}
#define ODEBUG4(x,y) { std::ofstream DebugFile; DebugFile.open(y,ofstream::app); \
DebugFile << __FILE__ << ":" \
<< __FUNCTION__ << "(#" \
<< __LINE__ << "):" << x << std::endl; \
#define RESETDEBUG4(y) { std::ofstream DebugFile; \
DebugFile.open(y,ofstream::out); \
DebugFile.close();}
#define ODEBUG4(x,y) { std::ofstream DebugFile; \
DebugFile.open(y,ofstream::app); \
DebugFile << __FILE__ << ":" \
<< __FUNCTION__ << "(#" \
<< __LINE__ << "):" << x << std::endl; \
DebugFile.close();}
#define ODEBUG4SIMPLE(x,y) { std::ofstream DebugFile; \
DebugFile.open(y,ofstream::app); \
DebugFile << x << std::endl; \
DebugFile.close();}
#define ODEBUG4SIMPLE(x,y) { std::ofstream DebugFile; \
DebugFile.open(y,ofstream::app); \
DebugFile << x << std::endl; \
DebugFile.close();}
#define _DEBUG_4_ACTIVATED_ 1
#else
......
......@@ -50,14 +50,16 @@ FootTrajectoryGenerationAbstract
std::string aMethodName[5] =
{":omega",
{
":omega",
":stepheight",
":singlesupporttime",
":doublesupporttime",
":samplingperiod"
":stepstairon"};
":stepstairon"
};
for (int i=0;i<5;i++)
for (int i=0; i<5; i++)
{
if (!RegisterMethod(aMethodName[i]))
{
......@@ -100,12 +102,12 @@ void FootTrajectoryGenerationAbstract::CallMethod(std::string &Method,
void FootTrajectoryGenerationAbstract::
UpdateFootPosition
(std::deque<FootAbsolutePosition> & , //SupportFootAbsolutePositions,
(std::deque<FootAbsolutePosition> &, //SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> &, //NoneSupportFootAbsolutePositions,
int , //CurrentAbsoluteIndex,
int , //IndexInitial,
double , //ModulatedSingleSupportTime,
int , //StepType,
int, //CurrentAbsoluteIndex,
int, //IndexInitial,
double, //ModulatedSingleSupportTime,
int, //StepType,
int ) //LeftOrRight)
{
LTHROW("FootTrajectoryGenerationAbstract::UpdateFootPosition-1: \
......@@ -114,13 +116,13 @@ UpdateFootPosition
void FootTrajectoryGenerationAbstract::
UpdateFootPosition
(std::deque<FootAbsolutePosition> & ,//SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> & ,//NoneSupportFootAbsolutePositions,
int , // StartIndex,
int , //k,
double , //LocalInterpolationStartTime,
double , //ModulatedSingleSupportTime,
int , //StepType,
(std::deque<FootAbsolutePosition> &, //SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> &, //NoneSupportFootAbsolutePositions,
int, // StartIndex,
int, //k,
double, //LocalInterpolationStartTime,
double, //ModulatedSingleSupportTime,
int, //StepType,
int ) //LeftOrRight)
{
LTHROW("FootTrajectoryGenerationAbstract::UpdateFootPosition-2: \
......
......@@ -88,7 +88,7 @@ namespace PatternGeneratorJRL
/*! Default destructor. */
virtual ~FootTrajectoryGenerationAbstract(){};
virtual ~FootTrajectoryGenerationAbstract() {};
/*! This method computes the position of the swinging foot during single support phase,
and maintian a constant position for the support foot.
......@@ -105,14 +105,16 @@ namespace PatternGeneratorJRL
@param StepType: Type of steps (for book-keeping).
@param LeftOrRight: Specify if it is left (1) or right (-1).
*/
virtual void UpdateFootPosition(std::deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
virtual void UpdateFootPosition(std::deque<FootAbsolutePosition>
&SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int CurrentAbsoluteIndex,
int IndexInitial,
double ModulatedSingleSupportTime,
int StepType, int LeftOrRight);
virtual void UpdateFootPosition(std::deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
virtual void UpdateFootPosition(std::deque<FootAbsolutePosition>
&SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int StartIndex, int k,
double LocalInterpolationStartTime,
......@@ -135,11 +137,15 @@ namespace PatternGeneratorJRL
/*! \brief Set single support time */
void SetSingleSupportTime(double aTSingle)
{ m_TSingle =aTSingle;};
{
m_TSingle =aTSingle;
};
/*! \brief Get single support time */
double GetSingleSupportTime() const
{ return m_TSingle;};
{
return m_TSingle;
};
/*! @} */
/*! \name Double Support Time
......@@ -148,11 +154,15 @@ namespace PatternGeneratorJRL
/*! \brief Set double support time */
void SetDoubleSupportTime(double aTDouble)
{ m_TDouble =aTDouble;};
{
m_TDouble =aTDouble;
};
/*! \brief Get single support time */
double GetDoubleSupportTime() const
{ return m_TDouble;};
{
return m_TDouble;
};
/*! @}*/
......@@ -162,11 +172,15 @@ namespace PatternGeneratorJRL
/*! \brief Set single support time */
void SetSamplingPeriod(double aSamplingPeriod)
{ m_SamplingPeriod = aSamplingPeriod;};
{
m_SamplingPeriod = aSamplingPeriod;
};
/*! \brief Get single support time */
double GetSamplingPeriod() const
{ return m_SamplingPeriod;};
{
return m_SamplingPeriod;
};
/*!@}*/
......@@ -175,11 +189,15 @@ namespace PatternGeneratorJRL
/*! Get Omega */
double GetOmega() const
{ return m_Omega; };
{
return m_Omega;
};
/*! Set Omega */
void SetOmega(double anOmega)
{ m_Omega = anOmega; };
{
m_Omega = anOmega;
};
/*! \name StepHeight.
......@@ -187,11 +205,15 @@ namespace PatternGeneratorJRL
/*! Get StepHeight */
double GetStepHeight() const
{ return m_StepHeight; };
{
return m_StepHeight;
};
/*! Set Omega */
void SetStepHeight(double aStepHeight)
{ m_StepHeight = aStepHeight; };
{
m_StepHeight = aStepHeight;
};
/*! \name isStepStairOn.
......@@ -199,11 +221,15 @@ namespace PatternGeneratorJRL
/*! Get isStepStairOn */
int GetSetStepStairOn() const
{ return m_isStepStairOn; };
{
return m_isStepStairOn;
};
/*! Set isStepStairOn */
void SetStepStairOn(int aSSO)
{ m_isStepStairOn= aSSO;};
{
m_isStepStairOn= aSSO;
};
/*! @} */
/*! @} */
......
......@@ -61,7 +61,7 @@ SetNumberOfIntervals
(unsigned int)lNumberOfIntervals)
return;
for(unsigned int i=0;i<m_SetOfFootTrajectoryGenerationObjects.size();i++)
for(unsigned int i=0; i<m_SetOfFootTrajectoryGenerationObjects.size(); i++)
{
delete m_SetOfFootTrajectoryGenerationObjects[i];
}
......@@ -95,7 +95,7 @@ SetTimeIntervals
m_RefTime.resize(lDeltaTj.size());
double reftime=0.0;
for(unsigned int li=0;li<m_DeltaTj.size();li++)
for(unsigned int li=0; li<m_DeltaTj.size(); li++)
{
m_RefTime[li] = reftime;
ODEBUG(" m_RefTime["<< li <<"]: " << setprecision(12)
......@@ -123,7 +123,7 @@ Compute(int axis, double t, double &result)
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++)
for(unsigned int j=0; j<m_DeltaTj.size(); j++)
{
ODEBUG(" t: " << t << " reftime :" << reftime
<< " Tj["<<j << "]= "<< m_DeltaTj[j]);
......@@ -201,7 +201,7 @@ Compute
" m_Sensitivity: "<< m_Sensitivity
<<" m_DeltaTj.size(): "<< m_DeltaTj.size() );
for(unsigned int j=0;j<m_DeltaTj.size();j++)
for(unsigned int j=0; j<m_DeltaTj.size(); j++)
{
ODEBUG("t: " << t << " reftime :"
<< setprecision(12) << reftime <<
......@@ -445,7 +445,7 @@ CallMethod
int FootTrajectoryGenerationMultiple::
DisplayIntervals() const
{
for(unsigned int i=0;i<m_DeltaTj.size();i++)
for(unsigned int i=0; i<m_DeltaTj.size(); i++)
{
std::cout << "m_DeltaTj["<<i<<"]="<<m_DeltaTj[i] << std::endl;
}
......@@ -466,7 +466,7 @@ FootTrajectoryGenerationMultiple::operator=
SetTimeIntervals(lDeltaTj);
/* Copy nature of intervals */
for(unsigned int li=0;li<lDeltaTj.size();li++)
for(unsigned int li=0; li<lDeltaTj.size(); li++)
SetNatureInterval(li,aFTGM.GetNatureInterval(li));
/* Set absolute time reference */
......@@ -475,10 +475,10 @@ FootTrajectoryGenerationMultiple::operator=
/* Copy the parameters */
unsigned int li=0 ;
int lk=0 ;
for(li=0;li<lDeltaTj.size();li++)
for(li=0; li<lDeltaTj.size(); li++)
{
double TI, FP, IP, IS;
for(lk=0;lk<6;lk++)
for(lk=0; lk<6; lk++)
{
GetParametersWithInitPosInitSpeed(li,lk,TI,FP,IP,IS);
/*! Special case when TI is equal to zero */
......
......@@ -133,7 +133,8 @@ namespace PatternGeneratorJRL
@param[in] IndexInterval: Index of the interval to be used for the computation.
@param[out] aFootAbsolutePosition: a foot absolute position.
*/
bool Compute(double t, FootAbsolutePosition & aFootAbsolutePosition, unsigned int IndexInterval);
bool Compute(double t, FootAbsolutePosition & aFootAbsolutePosition,
unsigned int IndexInterval);
/*! 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
......@@ -214,12 +215,14 @@ namespace PatternGeneratorJRL
/*! @} */
FootTrajectoryGenerationMultiple & operator=(const FootTrajectoryGenerationMultiple & aFTGM);
FootTrajectoryGenerationMultiple & operator=(const
FootTrajectoryGenerationMultiple & aFTGM);
protected:
/*! \brief Handle a set of object allowing the generation of the foot trajectory.*/
std::vector<FootTrajectoryGenerationStandard *> m_SetOfFootTrajectoryGenerationObjects;
std::vector<FootTrajectoryGenerationStandard *>
m_SetOfFootTrajectoryGenerationObjects;
/*! \brief Reference of humanoid specificities. */
PRFoot * m_Foot;
......
......@@ -972,9 +972,15 @@ UpdateFootPosition
// COM Orientation
Eigen::Matrix3d Foot_R;
Foot_R(0,0) = c*co; Foot_R(0,1) = -s; Foot_R(0,2) = c*so;
Foot_R(1,0) = s*co; Foot_R(1,1) = c; Foot_R(1,2) = s*so;
Foot_R(2,0) = -so; Foot_R(2,1) = 0; Foot_R(2,2) = co;
Foot_R(0,0) = c*co;
Foot_R(0,1) = -s;
Foot_R(0,2) = c*so;
Foot_R(1,0) = s*co;
Foot_R(1,1) = c;
Foot_R(1,2) = s*so;
Foot_R(2,0) = -so;
Foot_R(2,1) = 0;
Foot_R(2,2) = co;
if (LeftOrRight==-1)
{
......@@ -1011,7 +1017,8 @@ UpdateFootPosition
double LocalInterpolationStartTime,
double ModulatedSingleSupportTime,
int StepType, int /* LeftOrRight */)
{//TODO 0:Update foot position needs to be verified and cleaned
{
//TODO 0:Update foot position needs to be verified and cleaned
// unsigned int k = CurrentAbsoluteIndex - IndexInitial;
// Local time
......@@ -1208,9 +1215,15 @@ UpdateFootPosition
// COM Orientation
Eigen::Matrix3d Foot_R;
Foot_R(0,0) = c*co; Foot_R(0,1) = -s; Foot_R(0,2) = c*so;
Foot_R(1,0) = s*co; Foot_R(1,1) = c; Foot_R(1,2) = s*so;
Foot_R(2,0) = -so; Foot_R(2,1) = 0; Foot_R(2,2) = co;
Foot_R(0,0) = c*co;
Foot_R(0,1) = -s;
Foot_R(0,2) = c*so;
Foot_R(1,0) = s*co;
Foot_R(1,1) = c;
Foot_R(1,2) = s*so;
Foot_R(2,0) = -so;
Foot_R(2,1) = 0;
Foot_R(2,2) = co;
if (LeftOrRight==-1)
{
......@@ -1260,22 +1273,24 @@ ComputingAbsFootPosFromQueueOfRelPos
Eigen::Matrix<double,2,1> v;;
Eigen::Matrix<double,2,1> v2;;
for(unsigned int i=0;i<RelativeFootPositions.size();i++)
for(unsigned int i=0; i<RelativeFootPositions.size(); i++)
{
/*! Compute Orientation matrix related to the relative orientation
of the support foot */
c = cos(RelativeFootPositions[i].theta*M_PI/180.0);
s = sin(RelativeFootPositions[i].theta*M_PI/180.0);
MM(0,0) = c; MM(0,1) = -s;
MM(1,0) = s; MM(1,1) = c;
MM(0,0) = c;
MM(0,1) = -s;
MM(1,0) = s;
MM(1,1) = c;
/*! Update the orientation */
CurrentAbsTheta+= RelativeFootPositions[i].theta;
CurrentAbsTheta = fmod(CurrentAbsTheta,180.0);
/*! Extract the current absolute orientation matrix. */
for(int k=0;k<2;k++)
for(int l=0;l<2;l++)
for(int k=0; k<2; k++)
for(int l=0; l<2; l++)
Orientation(k,l) = CurrentSupportFootPosition(k,l);
/*! Put in a vector form the translation of the relative foot. */
......@@ -1287,11 +1302,11 @@ ComputingAbsFootPosFromQueueOfRelPos
v2 = Orientation*v;
/*! Update the world coordinates of the support foot. */
for(int k=0;k<2;k++)
for(int l=0;l<2;l++)
for(int k=0; k<2; k++)
for(int l=0; l<2; l++)
CurrentSupportFootPosition(k,l) = Orientation(k,l);
for(int k=0;k<2;k++)
for(int k=0; k<2; k++)
CurrentSupportFootPosition(k,2) += v2(k,0);
AbsoluteFootPositions[i].x = v2(0,0);
......
......@@ -44,7 +44,8 @@ namespace PatternGeneratorJRL
orthogonal plan as well as the direction.For the height modification a 4th order polynome
is used. Finally a landing and take off phase using an angular value (\f$\omega\f$).
*/
class FootTrajectoryGenerationStandard : public FootTrajectoryGenerationAbstract
class FootTrajectoryGenerationStandard : public
FootTrajectoryGenerationAbstract
{
public:
......@@ -92,14 +93,16 @@ 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,
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,
virtual void UpdateFootPosition(deque<FootAbsolutePosition>
&SupportFootAbsolutePositions,
deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int StartIndex, int k,
double LocalInterpolationStartTime,
......@@ -227,7 +230,8 @@ namespace PatternGeneratorJRL
/*! Compute the absolute foot position from the queue of relative positions.
There is not direct dependency with time.
*/
void ComputingAbsFootPosFromQueueOfRelPos(deque<RelativeFootPosition> &RelativeFootPositions,
void ComputingAbsFootPosFromQueueOfRelPos(deque<RelativeFootPosition>
&RelativeFootPositions,
deque<FootAbsolutePosition> &AbsoluteFootPositions);
/*! Methods to compute a set of positions for the feet according to the discrete time given in param