Commit 8a778207 authored by Olivier Stasse's avatar Olivier Stasse

Fix tabs and 80cols convention.

parent 8777573f
Pipeline #5072 failed with stage
in 4 minutes and 47 seconds
......@@ -67,7 +67,8 @@ namespace PatternGeneratorJRL
{
for(size_t i = 0; i < 3; ++i)
{
os << "x[" << i << "] " << aCp.x[i] << " y[" << i << "] " << aCp.y[i] << " z[" << i << "] " << aCp.z[i] << std::endl;
os << "x[" << i << "] " << aCp.x[i] << " y[" << i << "] "
<< aCp.y[i] << " z[" << i << "] " << aCp.z[i] << std::endl;
}
os << "yaw " << aCp.yaw << " pitch " << aCp.pitch << " roll " << aCp.roll;
return os;
......@@ -90,7 +91,8 @@ namespace PatternGeneratorJRL
COMState_s();
friend std::ostream & operator<<(std::ostream &os, const struct COMState_s & acs);
friend std::ostream & operator<<(std::ostream &os,
const struct COMState_s & acs);
};
......@@ -105,7 +107,8 @@ namespace PatternGeneratorJRL
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 6 :stepping stair
//3:first leg over obstacle 4:second leg over obstacle
// 5:one step after obstacle 6 :stepping stair
double DeviationHipHeight;
RelativeFootPosition_s();
};
......@@ -166,14 +169,16 @@ namespace PatternGeneratorJRL
/*! Time at which this position should be reached. */
double time;
/*! 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
+10 if double support phase
(-1) if support foot */
int stepType;
};
typedef struct FootAbsolutePosition_t FootAbsolutePosition;
inline std::ostream & operator<<(std::ostream & os, const FootAbsolutePosition & fap)
inline std::ostream & operator<<(std::ostream & os,
const FootAbsolutePosition & fap)
{
os << "x " << fap.x
<< " y " << fap.y
......@@ -218,7 +223,8 @@ namespace PatternGeneratorJRL
};
typedef struct HandAbsolutePosition_t HandAbsolutePosition;
inline std::ostream & operator<<(std::ostream & os, const HandAbsolutePosition& hap)
inline std::ostream & operator<<(std::ostream & os,
const HandAbsolutePosition& hap)
{
os << "x " << hap.x
<< " y " << hap.y
......
......@@ -68,7 +68,8 @@ namespace PatternGeneratorJRL
/// Compute RNEA algorithm from
/// \param[in] q: \f$q=[r,\theta,\hat{q}]\f$ with \f$r\f$ the position
/// of the free floating base (usually the waist), \f$theta\f$ the free floating
/// of the free floating base (usually the waist), \f$theta\f$ the
/// free floating
/// orientation in RPY format, $\hat{q}$ the motor angles position.
void computeInverseDynamics(Eigen::VectorXd & q,
Eigen::VectorXd & v,
......@@ -88,7 +89,8 @@ namespace PatternGeneratorJRL
/// compute POSITION (not velocity) of the joints from end effector pose
/// This is the implementation of the analitycal inverse kinematic extracted
/// from the book of Kajita
/// Authors Shuuji Kajita ; Hirohisa Hirukawa ; Kensuke Harada ; Kazuhito Yokoi
/// Authors Shuuji Kajita ; Hirohisa Hirukawa ; Kensuke Harada ;
/// Kazuhito Yokoi
/// ISBN 9782287877162 ; 9782287877155
/// It needs a specific distribution of the joint to work.
/// a test at the initialization of the class is [should be] done to check
......
......@@ -22,6 +22,8 @@ ENDIF()
CONFIGURE_FILE(configJRLWPG.hh.in ${CMAKE_BINARY_DIR}/include/jrl/walkgen/configJRLWPG.hh)
# Make sure to find Debug.h
INCLUDE_DIRECTORIES(BEFORE ${PROJECT_SOURCE_DIR}/src)
INCLUDE_DIRECTORIES(BEFORE ${PROJECT_SOURCE_DIR}/src/ZMPRefTrajectoryGeneration)
INCLUDE_DIRECTORIES(BEFORE ${PROJECT_SOURCE_DIR}/src/FootTrajectoryGeneration)
# Add Boost path to include directories.
INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
......
......@@ -21,16 +21,18 @@
* Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/*! \file This object filters an analytical ZMP trajectory through preview control. */
/*! \file This object filters an analytical ZMP trajectory through
preview control. */
#include <iostream>
#include <fstream>
#include "Debug.hh"
#include <ZMPRefTrajectoryGeneration/FilteringAnalyticalTrajectoryByPreviewControl.hh>
#include "FilteringAnalyticalTrajectoryByPreviewControl.hh"
using namespace PatternGeneratorJRL;
FilteringAnalyticalTrajectoryByPreviewControl::FilteringAnalyticalTrajectoryByPreviewControl
FilteringAnalyticalTrajectoryByPreviewControl::
FilteringAnalyticalTrajectoryByPreviewControl
(SimplePluginManager *lSPM,
AnalyticalZMPCOGTrajectory* lAnalyticalZMPCOGTrajectory,
PreviewControl * lPreviewControl) : SimplePlugin(lSPM)
......@@ -75,15 +77,17 @@ FilteringAnalyticalTrajectoryByPreviewControl::FilteringAnalyticalTrajectoryByPr
}
void FilteringAnalyticalTrajectoryByPreviewControl::SetAnalyticalTrajectory(
AnalyticalZMPCOGTrajectory *lAZCT)
void FilteringAnalyticalTrajectoryByPreviewControl::
SetAnalyticalTrajectory
(AnalyticalZMPCOGTrajectory *lAZCT)
{
m_AnalyticalZMPCOGTrajectory = lAZCT;
}
void FilteringAnalyticalTrajectoryByPreviewControl::SetPreviewControl(
PreviewControl *lPreviewControl)
void FilteringAnalyticalTrajectoryByPreviewControl::
SetPreviewControl
( PreviewControl *lPreviewControl)
{
m_PreviewControl = lPreviewControl;
......@@ -107,19 +111,22 @@ void FilteringAnalyticalTrajectoryByPreviewControl::Resize()
unsigned int DataBufferSize = (unsigned int ) ((m_Tsingle
+m_PreviewControlTime)/
m_SamplingPeriod);
ODEBUG3("m_Tsingle: " << m_Tsingle << " DataBufferSize:" << DataBufferSize);
ODEBUG3("m_Tsingle: " << m_Tsingle << " DataBufferSize:"
<< DataBufferSize);
m_DataBuffer.resize(DataBufferSize);
}
#endif
}
FilteringAnalyticalTrajectoryByPreviewControl::~FilteringAnalyticalTrajectoryByPreviewControl()
FilteringAnalyticalTrajectoryByPreviewControl::
~FilteringAnalyticalTrajectoryByPreviewControl()
{
}
bool FilteringAnalyticalTrajectoryByPreviewControl::FillInWholeBuffer(
double FirstValueofZMPProfil,
double DeltaTj0 )
bool FilteringAnalyticalTrajectoryByPreviewControl::
FillInWholeBuffer
(double FirstValueofZMPProfil,
double DeltaTj0 )
{
ODEBUG("m_PreviewControl : " << m_PreviewControl <<
" m_AnalyticalZMPCOGTrajectory : " << m_AnalyticalZMPCOGTrajectory);
......@@ -128,8 +135,8 @@ bool FilteringAnalyticalTrajectoryByPreviewControl::FillInWholeBuffer(
return false;
if (!m_PreviewControl->IsCoherent())
m_PreviewControl->ComputeOptimalWeights(
OptimalControllerSolver::MODE_WITH_INITIALPOS);
m_PreviewControl->ComputeOptimalWeights
( OptimalControllerSolver::MODE_WITH_INITIALPOS);
m_Duration = DeltaTj0;
......@@ -137,7 +144,8 @@ bool FilteringAnalyticalTrajectoryByPreviewControl::FillInWholeBuffer(
m_StartingTime = m_AnalyticalZMPCOGTrajectory->GetAbsoluteTimeReference();
double DeltaT = m_PreviewControl->SamplingPeriod();
unsigned int SizeOfBuffer = (unsigned int)((DeltaTj0+PreviewWindowTime)/DeltaT);
unsigned int SizeOfBuffer = (unsigned int)((DeltaTj0+PreviewWindowTime)
/DeltaT);
ODEBUG("SizeOfBuffer: " <<SizeOfBuffer<< " Duration : "<<m_Duration);
if (m_DataBuffer.size()!=SizeOfBuffer)
m_DataBuffer.resize(SizeOfBuffer);
......@@ -182,14 +190,16 @@ bool FilteringAnalyticalTrajectoryByPreviewControl::FillInWholeBuffer(
return true;
}
bool FilteringAnalyticalTrajectoryByPreviewControl::UpdateOneStep(double t,
double &ZMPValue,
double &CoMValue,
double &CoMSpeedValue)
bool FilteringAnalyticalTrajectoryByPreviewControl::
UpdateOneStep
(double t,
double &ZMPValue,
double &CoMValue,
double &CoMSpeedValue)
{
ODEBUG("time:" << t << " m_StartingTime: " <<
m_StartingTime << " " << m_Duration + m_StartingTime << " ( " << m_Duration <<
" ) "
m_StartingTime << " " << m_Duration + m_StartingTime << " ( "
<< m_Duration << " ) "
<< " LBI:" << m_LocalBufferIndex);
if ((t<m_StartingTime) || (t>m_Duration+m_StartingTime) || (m_Duration==0.0))
return false;
......@@ -210,9 +220,10 @@ bool FilteringAnalyticalTrajectoryByPreviewControl::UpdateOneStep(double t,
}
/*! \brief Overloading method of SimplePlugin */
void FilteringAnalyticalTrajectoryByPreviewControl::CallMethod(
std::string &Method,
std::istringstream &strm)
void FilteringAnalyticalTrajectoryByPreviewControl::
CallMethod
(std::string &Method,
std::istringstream &strm)
{
if (Method==":samplingperiod")
{
......
......@@ -21,7 +21,8 @@
* Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/*! \file This object filters an analytical ZMP trajectory through preview control. */
/*! \file This object filters an analytical ZMP trajectory through preview
control. */
#ifndef _FILTERING_ANALYTICAL_TRAJECTORY_BY_PREVIEW_CONTROL_H_
#define _FILTERING_ANALYTICAL_TRAJECTORY_BY_PREVIEW_CONTROL_H_
......@@ -40,10 +41,11 @@ namespace PatternGeneratorJRL
public:
/*! \brief Default constructor */
FilteringAnalyticalTrajectoryByPreviewControl(SimplePluginManager * lSPM,
AnalyticalZMPCOGTrajectory * lAnalyticalZMPCOGTrajectory=0,
PreviewControl * lPreviewControl=0);
FilteringAnalyticalTrajectoryByPreviewControl
(SimplePluginManager * lSPM,
AnalyticalZMPCOGTrajectory * lAnalyticalZMPCOGTrajectory=0,
PreviewControl * lPreviewControl=0);
/*! \brief Set Analytical trajectory */
void SetAnalyticalTrajectory(AnalyticalZMPCOGTrajectory *lAZCT);
......@@ -54,7 +56,8 @@ namespace PatternGeneratorJRL
This has to be done if the analytical trajectory has been changed,
and that the first interval has been changed.
\param FistValueOfZMPProfil: The first value of the desired ZMP interval.
\param DeltaTj0: Value of the time interval during which the filter is applied.
\param DeltaTj0: Value of the time interval during which the filter
is applied.
\return false if a problem occured, true otherwise.
*/
bool FillInWholeBuffer(double FirstValueofZMPProfil,
......
......@@ -58,11 +58,16 @@ namespace PatternGeneratorJRL
/// \}
/// \brief Preview feet and trunk orientations inside the preview window
/// The orientations of the feet are adapted to the previewed orientation of the hip.
/// The resulting velocities accelerations and orientations are verified against the limits.
/// If the constraints can not be satisfied the rotational velocity of the trunk is reduced.
/// The trunk is rotating with a constant speed after a constant acceleration phase of T_ length.
/// During the initial double support phase the trunk is not rotating contrary to the following.
/// The orientations of the feet are adapted to the previewed orientation
/// of the hip.
/// The resulting velocities accelerations and orientations are verified
/// against the limits.
/// If the constraints can not be satisfied the rotational velocity
/// of the trunk is reduced.
/// The trunk is rotating with a constant speed after a constant
/// acceleration phase of T_ length.
/// During the initial double support phase the trunk is not rotating
/// contrary to the following.
///
/// \param[in] Time
/// \param[in] Ref
......@@ -70,12 +75,13 @@ namespace PatternGeneratorJRL
/// \param[in] LeftFootPositions_deq
/// \param[in] RightFootPositions_deq
/// \param[out] Solution Trunk and Foot orientations
void preview_orientations(double Time,
const reference_t & Ref,
double StepDuration,
const std::deque<FootAbsolutePosition> & LeftFootPositions_deq,
const std::deque<FootAbsolutePosition> & RightFootPositions_deq,
solution_t & Solution);
void preview_orientations
(double Time,
const reference_t & Ref,
double StepDuration,
const std::deque<FootAbsolutePosition> & LeftFootPositions_deq,
const std::deque<FootAbsolutePosition> & RightFootPositions_deq,
solution_t & Solution);
/// \brief Interpolate previewed orientation of the trunk
///
......@@ -84,11 +90,12 @@ namespace PatternGeneratorJRL
/// \param[in] NewSamplingPeriod
/// \param[in] PrwSupportStates_deq
/// \param[out] FinalCOMTraj_deq
void interpolate_trunk_orientation(double Time,
int CurrentIndex,
double NewSamplingPeriod,
const std::deque<support_state_t> & PrwSupportStates_deq,
std::deque<COMState> & FinalCOMTraj_deq);
void interpolate_trunk_orientation
(double Time,
int CurrentIndex,
double NewSamplingPeriod,
const std::deque<support_state_t> & PrwSupportStates_deq,
std::deque<COMState> & FinalCOMTraj_deq);
/// \brief Compute the current state for the preview of the orientation
///
......@@ -97,9 +104,10 @@ namespace PatternGeneratorJRL
/// \param[in] NewSamplingPeriod
/// \param[in] PrwSupportStates_deq
/// \param[out] FinalCOMTraj_deq
void one_iteration(double Time,
const std::deque<support_state_t> & PrwSupportStates_deq);
void one_iteration
(double Time,
const std::deque<support_state_t> & PrwSupportStates_deq);
/// \name Accessors
/// \{
......@@ -150,9 +158,13 @@ namespace PatternGeneratorJRL
//
private:
/// \brief Verify and eventually reduce the maximal acceleration of the hip joint necessary to attain the velocity reference in one sampling T_.
/// The verification is based on the supposition that the final joint trajectory is composed by
/// a fourth-order polynomial acceleration phase inside T_ and a constant velocity phase for the rest of the preview horizon.
/// \brief Verify and eventually reduce the maximal acceleration of
/// the hip joint necessary to attain the velocity reference in one
/// sampling T_.
/// The verification is based on the supposition that the final joint
/// trajectory is composed by
/// a fourth-order polynomial acceleration phase inside T_ and
/// a constant velocity phase for the rest of the preview horizon.
///
/// \param[in] Ref
/// \param[in] CurrentSupport
......@@ -161,7 +173,8 @@ namespace PatternGeneratorJRL
/// \brief Verify velocity of hip joint
/// The velocity is verified only between previewed supports.
/// The verification is based on the supposition that the final joint trajectory is a third-order polynomial.
/// The verification is based on the supposition that the final joint
/// trajectory is a third-order polynomial.
///
/// \param[in] Time
/// \param[in] PreviewedSupportFoot
......
......@@ -66,14 +66,16 @@ namespace PatternGeneratorJRL
/// \brief Handle plugins (SimplePlugin interface)
void CallMethod(std::string &Method, std::istringstream &strm);
/*! \name Call method to handle on-line generation of ZMP reference trajectory.
/*! \name Call method to handle on-line generation of ZMP
reference trajectory.
@{*/
/*! Methods for on-line generation. (First version!)
The queues will be updated as follows:
- The first values necessary to start walking will be inserted.
- The initial positions of the feet will be taken into account
according to InitLeftFootAbsolutePosition and InitRightFootAbsolutePosition.
according to InitLeftFootAbsolutePosition and
InitRightFootAbsolutePosition.
- The RelativeFootPositions stack will NOT be taken into account,
- The starting COM Position will NOT be taken into account.
Returns the number of steps which has been completely put inside
......@@ -100,7 +102,8 @@ namespace PatternGeneratorJRL
/// \name Accessors and mutators
/// \{
/// \brief Set the reference (velocity only as for now) through the Interface (slow)
/// \brief Set the reference (velocity only as for now) through
/// the Interface (slow)
void Reference(istringstream &strm)
{
strm >> NewVelRef_.Local.X;
......@@ -209,10 +212,12 @@ namespace PatternGeneratorJRL
/// \brief Simplified robot model
RigidBodySystem * Robot_ ;
/// \brief Finite State Machine to simulate the evolution of the support states.
/// \brief Finite State Machine to simulate the evolution of
/// the support states.
SupportFSM * SupportFSM_;
/// \brief Decoupled optimization problem to compute the evolution of feet angles.
/// \brief Decoupled optimization problem to compute the evolution
/// of feet angles.
OrientationsPreview * OrientPrw_;
OrientationsPreview * OrientPrw_DF_;
......@@ -278,10 +283,12 @@ namespace PatternGeneratorJRL
/// \brief Height of the CoM
double CoMHeight_ ;
/// \brief Number of interpolated point needed for control computed during QP_T_
/// \brief Number of interpolated point needed for control computed
/// during QP_T_
unsigned NbSampleControl_ ;
/// \brief Number of interpolated point needed for the dynamic filter computed during QP_T_
/// \brief Number of interpolated point needed for the dynamic filter
/// computed during QP_T_
unsigned NbSampleInterpolation_ ;
COMState InitStateLIPM_ ;
......@@ -297,63 +304,70 @@ namespace PatternGeneratorJRL
public:
void GetZMPDiscretization(std::deque<ZMPPosition> & ZMPPositions,
std::deque<COMState> & COMStates,
std::deque<RelativeFootPosition> &RelativeFootPositions,
std::deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
std::deque<FootAbsolutePosition> &RightFootAbsolutePositions,
double Xmax,
COMState & lStartingCOMState,
Eigen::Vector3d & lStartingZMPPosition,
FootAbsolutePosition & InitLeftFootAbsolutePosition,
FootAbsolutePosition & InitRightFootAbsolutePosition);
void OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosition,
std::deque<ZMPPosition> & FinalZMPPositions,
std::deque<COMState> & COMStates,
std::deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
std::deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
bool EndSequence);
int OnLineFootChange(double time,
FootAbsolutePosition & aFootAbsolutePosition,
deque<ZMPPosition> & FinalZMPPositions,
deque<COMState> & CoMPositions,
deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
StepStackHandler * aStepStackHandler);
void EndPhaseOfTheWalking(deque<ZMPPosition> & ZMPPositions,
deque<COMState> & FinalCOMTraj_deq,
deque<FootAbsolutePosition> & LeftFootAbsolutePositions,
deque<FootAbsolutePosition> & RightFootAbsolutePositions);
void GetZMPDiscretization
(std::deque<ZMPPosition> & ZMPPositions,
std::deque<COMState> & COMStates,
std::deque<RelativeFootPosition> &RelativeFootPositions,
std::deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
std::deque<FootAbsolutePosition> &RightFootAbsolutePositions,
double Xmax,
COMState & lStartingCOMState,
Eigen::Vector3d & lStartingZMPPosition,
FootAbsolutePosition & InitLeftFootAbsolutePosition,
FootAbsolutePosition & InitRightFootAbsolutePosition);
void OnLineAddFoot
(RelativeFootPosition & NewRelativeFootPosition,
std::deque<ZMPPosition> & FinalZMPPositions,
std::deque<COMState> & COMStates,
std::deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
std::deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
bool EndSequence);
int OnLineFootChange
(double time,
FootAbsolutePosition & aFootAbsolutePosition,
deque<ZMPPosition> & FinalZMPPositions,
deque<COMState> & CoMPositions,
deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
StepStackHandler * aStepStackHandler);
void EndPhaseOfTheWalking
(deque<ZMPPosition> & ZMPPositions,
deque<COMState> & FinalCOMTraj_deq,
deque<FootAbsolutePosition> & LeftFootAbsolutePositions,
deque<FootAbsolutePosition> & RightFootAbsolutePositions);
int ReturnOptimalTimeToRegenerateAStep();
/// \brief Interpolation form the com jerk the position of the com and the zmp corresponding to the kart table model
void CoMZMPInterpolation(
std::deque<ZMPPosition> & ZMPPositions, // OUTPUT
std::deque<COMState> & COMTraj_deq, // OUTPUT
const std::deque<FootAbsolutePosition> & LeftFootTraj_deq, // INPUT
const std::deque<FootAbsolutePosition> & RightFootTraj_deq, // INPUT
const solution_t * Solution, // INPUT
LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
const unsigned numberOfSample, // INPUT
const int IterationNumber, // INPUT
const unsigned int currentIndex); // INPUT
/// \brief Interpolate just enough data to pilot the robot (period of interpolation = QP_T_)
void ControlInterpolation(
std::deque<COMState> & FinalCOMTraj_deq, // OUTPUT
std::deque<ZMPPosition> & FinalZMPTraj_deq, // OUTPUT
std::deque<FootAbsolutePosition> & FinalLeftFootTraj_deq, // OUTPUT
std::deque<FootAbsolutePosition> & FinalRightFootTraj_deq, // OUTPUT
double time); // INPUT
/// \brief Interpolation form the com jerk the position of the com and the
/// zmp corresponding to the kart table model
void CoMZMPInterpolation
(std::deque<ZMPPosition> & ZMPPositions, // OUTPUT
std::deque<COMState> & COMTraj_deq, // OUTPUT
const std::deque<FootAbsolutePosition> & LeftFootTraj_deq, // INPUT
const std::deque<FootAbsolutePosition> & RightFootTraj_deq, // INPUT
const solution_t * Solution, // INPUT
LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
const unsigned numberOfSample, // INPUT
const int IterationNumber, // INPUT
const unsigned int currentIndex); // INPUT
/// \brief Interpolate just enough data to pilot the robot (period of
/// interpolation = QP_T_)
void ControlInterpolation
(std::deque<COMState> & FinalCOMTraj_deq, // OUTPUT
std::deque<ZMPPosition> & FinalZMPTraj_deq, // OUTPUT
std::deque<FootAbsolutePosition> & FinalLeftFootTraj_deq, // OUTPUT
std::deque<FootAbsolutePosition> & FinalRightFootTraj_deq, // OUTPUT
double time); // INPUT
/// \brief Interpolation everything on the whole preview
void DynamicFilterInterpolation(double time);
/// \brief Define the position of an additionnal foot step outside the preview to interpolate the position of the swinging feet in 3D
/// \brief Define the position of an additionnal foot step outside
/// the preview to interpolate the position of the swinging feet in 3D
void InterpretSolutionVector();
/// \brief Prepare the vecteur containing the solution for the interpolation
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment