diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
index 2ad68e2bb888ff713ff8c989150653e4750109b8..df80c80c619136849db5efa467070a19758b46c9 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
@@ -23,7 +23,8 @@
*/
/*!\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
*/
@@ -48,7 +49,8 @@ namespace PatternGeneratorJRL
{
/** @ingroup foottrajectorygeneration
- This class defines the abstract interface to interact with foot generation object.
+ This class defines the abstract interface to interact with
+ foot generation object.
Two parameters \f$ T_{DS} \f$ and \f$ T_{SS} \f$ defines respectively
the double support time and the single support time.
@@ -67,8 +69,8 @@ namespace PatternGeneratorJRL
should call InitializeInternalDataStructures() once all the internal
parameters of the object are set.
- The virtual function FreeInternalDataStructures() is used when changing some
- parameters and by the destructor.
+ The virtual function FreeInternalDataStructures() is used when changing
+ some parameters and by the destructor.
The most important function is UpdateFootPosition() which populates a
queue of foot absolute positions data structure.
@@ -90,36 +92,44 @@ namespace PatternGeneratorJRL
/*! Default destructor. */
virtual ~FootTrajectoryGenerationAbstract() {};
- /*! This method computes the position of the swinging foot during single support phase,
- and maintian a constant position for the support foot.
- @param SupportFootAbsolutePositions: Queue of absolute position for the support foot.
- This method will set the foot position at index CurrentAbsoluteIndex of the queue.
+ /*! This method computes the position of the swinging foot during
+ single support phase, and maintian a constant position
+ for the support foot.
+ @param SupportFootAbsolutePositions: Queue of absolute position for the
+ support foot.
+ This method will set the foot position at index CurrentAbsoluteIndex
+ of the queue.
This position is supposed to be constant.
- @param NoneSupportFootAbsolutePositions: Queue of absolute position for the swinging
- foot. This method will set the foot position at index NoneSupportFootAbsolutePositions
+ @param NoneSupportFootAbsolutePositions: Queue of absolute position
+ for the swinging
+ foot. This method will set the foot position at index
+ NoneSupportFootAbsolutePositions
of the queue.
- @param CurrentAbsoluteIndex: Index in the queues of the foot position to be set.
- @param IndexInitial: Index in the queues which correspond to the starting point
- of the current single support phase.
+ @param CurrentAbsoluteIndex: Index in the queues of the foot position to
+ be set.
+ @param IndexInitial: Index in the queues which correspond to the
+ starting point of the current single support phase.
@param ModulatedSingleSupportTime: Amount of time where the foot is flat.
@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,
- 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);
+ 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,
+ std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
+ int StartIndex, int k,
+ double LocalInterpolationStartTime,
+ double ModulatedSingleSupportTime,
+ int StepType, int LeftOrRight);
/*! Initialize internal data structures. */
virtual void InitializeInternalDataStructures()=0;
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.hh b/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.hh
index ea01be11d70095216603b226a5a73d3902166287..fea88281fb2cc870e7adc067d914580fe427cd82 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.hh
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.hh
@@ -67,9 +67,11 @@ namespace PatternGeneratorJRL
/*! \name Single support foot subcategories
@{ */
- /*! \brief The robot is in single support and the foot considered is the support foot */
+ /*! \brief The robot is in single support and the foot
+ considered is the support foot */
const static int SINGLE_SUPPORT_SUPPORT=1;
- /*! \brief The robot is in single support and the foot considered is flying. */
+ /*! \brief The robot is in single support and
+ the foot considered is flying. */
const static int SINGLE_SUPPORT_FLYING=2;
/*! @} */
@@ -130,15 +132,18 @@ namespace PatternGeneratorJRL
/*! \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.
+ @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);
- /*! 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.
@@ -148,53 +153,68 @@ namespace PatternGeneratorJRL
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
+ /*! 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 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 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
+ 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.
+ @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 SetParametersWithInitPosInitSpeedInitAcc
+ (unsigned int PolynomeIndex,
+ int AxisReference,
+ double TimeInterval,
+ double FinalPosition,
+ double InitPosition,
+ double InitSpeed,
+ double InitAcc,
+ vector<double> middlePos = vector<double>(3,-1));
+
/*! 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.
+ 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 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,
@@ -215,12 +235,13 @@ 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.*/
+ /*! \brief Handle a set of object allowing the generation of the foot
+ trajectory.*/
std::vector<FootTrajectoryGenerationStandard *>
m_SetOfFootTrajectoryGenerationObjects;
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
index 5b37afb0458f46e6d5424398ef201212661d9c6f..633ae110b9823716ec4a3cbe4cf66fe81a7f98d2 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
@@ -39,17 +39,22 @@ namespace PatternGeneratorJRL
{
/** @ingroup foottrajectorygeneration
- This class generates a trajectory for the swinging foot during single support phase.
- It uses a classical approach relying in polynome of 3rd orders for the position in the
- 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$).
+ This class generates a trajectory for the swinging foot during single
+ support phase.
+ It uses a classical approach relying in polynome of 3rd orders for the
+ position in the
+ 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
{
public:
- /*!\name Constants related to the direction for the generation of the polynomes.
+ /*!\name Constants related to the direction for the generation of the
+ polynomes.
@{ */
/*! \brief along the frontal direction */
@@ -74,82 +79,103 @@ namespace PatternGeneratorJRL
/*! Default destructor. */
virtual ~FootTrajectoryGenerationStandard();
- /*! This method computes the position of the swinging foot during single support phase,
+ /*! This method computes the position of the swinging foot during
+ single support phase,
and maintain a constant position for the support foot.
It uses polynomial of 3rd order for the X-axis, Y-axis,
orientation in the X-Z axis, and orientation in the X-Y axis,
and finally it uses a 4th order polynome for the Z-axis.
- @param SupportFootAbsolutePositions: Queue of absolute position for the support foot.
- This method will set the foot position at index CurrentAbsoluteIndex of the queue.
+ @param SupportFootAbsolutePositions: Queue of absolute position for
+ the support foot.
+ This method will set the foot position at index CurrentAbsoluteIndex
+ of the queue.
This position is supposed to be constant.
- @param NoneSupportFootAbsolutePositions: Queue of absolute position for the swinging
- foot. This method will set the foot position at index NoneSupportFootAbsolutePositions
+ @param NoneSupportFootAbsolutePositions: Queue of absolute position
+ for the swinging
+ foot. This method will set the foot position at index
+ NoneSupportFootAbsolutePositions
of the queue.
- @param CurrentAbsoluteIndex: Index in the queues of the foot position to be set.
- @param IndexInitial: Index in the queues which correspond to the starting point
- of the current single support phase.
+ @param CurrentAbsoluteIndex: Index in the queues of the foot
+ position to be set.
+ @param IndexInitial: Index in the queues which correspond to the
+ starting point of the current single support phase.
@param ModulatedSingleSupportTime: Amount of time where the foot is flat.
@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,
+ 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);
+ 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.
+ In this specific case, it is in charge of
+ creating the polynomial structures.
*/
virtual void InitializeInternalDataStructures();
/*! Free internal data structures.
- In this specific case, it is in charge of freeing the polynomial 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
+ /*! 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] 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.
+ @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
+ /*! 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] 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] 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));
+ int SetParametersWithInitPosInitSpeed
+ (int AxisReference,
+ double TimeInterval,
+ double FinalPosition,
+ double InitPosition,
+ double InitSpeed,
+ vector<double> MiddlePos=vector<double>(3,-1));
/*! Overloading -- BSPlines Init Function
- This method specifies the parameters for each of the Bsplines used by this object
+ This method specifies the parameters for each of the Bsplines used by
+ this object.
@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.
@@ -169,12 +195,16 @@ namespace PatternGeneratorJRL
double InitPosition);
/*! 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
+ 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] 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] 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 GetParametersWithInitPosInitSpeed(int AxisReference,
@@ -209,7 +239,8 @@ namespace PatternGeneratorJRL
/// \param[in] InitSpeed
/// \param[in] InitAcc
int SetParameters(int PolynomeIndex, double TimeInterval,
- double FinalPosition, double InitPosition, double InitSpeed, double InitAcc,
+ double FinalPosition, double InitPosition,
+ double InitSpeed, double InitAcc,
std::vector<double> MiddlePos=vector<double>(3,-1) );
/*! Fill an absolute foot position structure for a given time. */
@@ -230,12 +261,13 @@ 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,
- deque<FootAbsolutePosition> &AbsoluteFootPositions);
+ 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 parameters
- and the phase of walking.
+ /*! Methods to compute a set of positions for the feet according to the
+ discrete time given in parameters and the phase of walking.
@{
*/
diff --git a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
index 364043a5642c14e0c30daa81c5f286be25089ac4..b11e8db7f6c9347a56cdf77143e327bac668140b 100644
--- a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
+++ b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
@@ -630,8 +630,10 @@ InitializeFromRelativeSteps
<< LeftFootTmpFinalPos.dy << " , "
<< LeftFootTmpFinalPos.dz << " ) " );
- ODEBUG("RightFootTmpInitPos.stepType="<<RightFootTmpInitPos.stepType);
- ODEBUG("RightFootTmpFinalPos.stepType="<<RightFootTmpFinalPos.stepType);
+ ODEBUG("RightFootTmpInitPos.stepType=" <<
+ RightFootTmpInitPos.stepType);
+ ODEBUG("RightFootTmpFinalPos.stepType=" <<
+ RightFootTmpFinalPos.stepType);
ODEBUG("End of Single support phase");
SetAnInterval(IntervalIndex,m_RightFootTrajectory,
RightFootTmpInitPos,
@@ -663,7 +665,8 @@ InitializeFromRelativeSteps
/*! At this stage the phase of double support is dealt with */
unsigned int limitk=1;
- /*! If we are at the end a second double support phase has to be added. */
+ /*! If we are at the end a second double support phase
+ has to be added. */
if (i==RelativeFootPositions.size()-1)
limitk=2;
diff --git a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
index 2eac0f00301ab792f0aab428353ab0a53bc304c8..b124b95242639e14fbd9793df92b7f2fe38ef0e8 100644
--- a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
+++ b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
@@ -47,12 +47,12 @@ namespace PatternGeneratorJRL
foot trajectories.
It acts as a container for two FootTrajectoryGenerationMultiple objects
- which handle several polynomials trajectory generation for the right and left
- feet.
+ which handle several polynomials trajectory generation for the right
+ and left feet.
It provides an initialization of the underlying objects assuming that
- some basic informations have been provided: single support time, double support
- time, omega, step height.
+ some basic informations have been provided: single support time,
+ double support time, omega, step height.
The information used follow the previously defined script like language,
but it could be extended for a clear separation between the two feet.
@@ -62,13 +62,14 @@ namespace PatternGeneratorJRL
{
public:
- /*! \brief The constructor initialize the plugin part, and the data related to the humanoid. */
+ /*! \brief The constructor initialize the plugin part,
+ and the data related to the humanoid. */
LeftAndRightFootTrajectoryGenerationMultiple(SimplePluginManager * lSPM,
PRFoot * inFoot);
/*! \brief Copy constructor. */
- LeftAndRightFootTrajectoryGenerationMultiple(const
- LeftAndRightFootTrajectoryGenerationMultiple &);
+ LeftAndRightFootTrajectoryGenerationMultiple
+ (const LeftAndRightFootTrajectoryGenerationMultiple &);
/*! \brief Memory release. */
~LeftAndRightFootTrajectoryGenerationMultiple();
@@ -79,47 +80,63 @@ namespace PatternGeneratorJRL
*/
virtual void CallMethod(std::string &Method, std::istringstream &strm);
- /*! \brief Initialize the analytical feet trajectories from a set of relative step.
- It is based on the parameters given through the interface. All the settings are done internally.
+ /*! \brief Initialize the analytical feet trajectories
+ from a set of relative step.
+ It is based on the parameters given through the interface.
+ All the settings are done internally.
The only output is the set of absolute positions for the support foot.
- @param[in] RelativeFootPositions: The set of relative positions for the support foot.
+ @param[in] RelativeFootPositions: The set of relative positions
+ for the support foot.
@param[in] LeftFootInitialPosition: The initial position of the left foot.
- @param[in] RightFootInitialPosition: the initial position of the right foot.
- @param[out] SupportFootAbsoluteFootPositions: The set of absolute foot positions
- corresponding to the set of relative foot positions (i.e given step by step
- and not every sampled control time).
- @param[in] IgnoreFirst: Ignore the first double support phase, should be true at beginning of stepping.
- @param[in] Continuity: Should be true if more steps should be added, false to stop stepping.
+ @param[in] RightFootInitialPosition: the initial position of the right
+ foot.
+ @param[out] SupportFootAbsoluteFootPositions: The set of absolute foot
+ positions
+ corresponding to the set of relative foot positions
+ (i.e given step by step and not every sampled control time).
+ @param[in] IgnoreFirst: Ignore the first double support phase,
+ should be true at beginning of stepping.
+ @param[in] Continuity: Should be true if more steps should be added,
+ false to stop stepping.
*/
- void InitializeFromRelativeSteps(deque<RelativeFootPosition>
- &RelativeFootPositions,
- FootAbsolutePosition &LeftFootInitialPosition,
- FootAbsolutePosition &RightFootInitialPosition,
- deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions,
- bool IgnoreFirst, bool Continuity);
+ void InitializeFromRelativeSteps
+ (deque<RelativeFootPosition>
+ &RelativeFootPositions,
+ FootAbsolutePosition &LeftFootInitialPosition,
+ FootAbsolutePosition &RightFootInitialPosition,
+ deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions,
+ bool IgnoreFirst, bool Continuity);
/*! \brief Method to compute the absolute position of the foot.
- @param[in] LeftOrRight: -1 indicates the right foot, 1 indicates the left foot.
- @param[in] time: The absolute time to be compared with the absolute reference defining the start
+ @param[in] LeftOrRight: -1 indicates the right foot,
+ 1 indicates the left foot.
+ @param[in] time: The absolute time to be compared
+ with the absolute reference defining the start
of the trajectory.
- @param[out] aFootAbsolutePosition: The data structure to be filled with the information
+ @param[out] aFootAbsolutePosition:
+ The data structure to be filled with the information
\f$ (x,y,z,\omega, \omega_2, \theta) \f$.
*/
- bool ComputeAnAbsoluteFootPosition(int LeftOrRight, double time,
- FootAbsolutePosition & aFootAbsolutePosition);
+ bool ComputeAnAbsoluteFootPosition
+ (int LeftOrRight, double time,
+ FootAbsolutePosition & aFootAbsolutePosition);
/*! \brief Method to compute the absolute position of the foot.
- @param[in] LeftOrRight: -1 indicates the right foot, 1 indicates the left foot.
- @param[in] time: The absolute time to be compared with the absolute reference defining the start
+ @param[in] LeftOrRight: -1 indicates the right foot,
+ 1 indicates the left foot.
+ @param[in] time: The absolute time to be compared
+ with the absolute reference defining the start
of the trajectory.
- @param[out] aFootAbsolutePosition: The data structure to be filled with the information
+ @param[out] aFootAbsolutePosition: The data structure to
+ be filled with the information
\f$ (x,y,z,\omega, \omega_2, \theta) \f$.
@param[in] IndexInterval: On which interval to compute the foot position.
*/
- bool ComputeAnAbsoluteFootPosition(int LeftOrRight, double time,
- FootAbsolutePosition & aFootAbsolutePosition,
- unsigned int IndexInterval);
+ bool ComputeAnAbsoluteFootPosition
+ (int LeftOrRight, double time,
+ FootAbsolutePosition & aFootAbsolutePosition,
+ unsigned int IndexInterval);
/*
bool ComputeAnAbsoluteFootPosition(int LeftOrRight,
@@ -127,47 +144,61 @@ namespace PatternGeneratorJRL
std::deque<FootAbsolutePosition> & adFAP,
unsigned int IndexInterval);*/
- /*! \brief Method to compute absolute feet positions from a set of relative one.
- @param[in] RelativeFootPositions: The set of relative positions for the support foot.
+ /*! \brief Method to compute absolute feet positions from a set of
+ relative one.
+ @param[in] RelativeFootPositions: The set of relative positions for
+ the support foot.
@param[in] LeftFootInitialPosition: The initial position of the left foot.
- @param[in] RightFootInitialPosition: the initial position of the right foot.
- @param[out] SupportFootAbsoluteFootPositions: The set of absolute foot positions
- corresponding to the set of relative foot positions (i.e given step by step
- and not every sampled control time)
+ @param[in] RightFootInitialPosition: the initial position of the
+ right foot.
+ @param[out] SupportFootAbsoluteFootPositions: The set of absolute
+ foot positions
+ corresponding to the set of relative foot positions
+ (i.e given step by step and not every sampled control time)
*/
- void ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition>
- &RelativeFootPositions,
- FootAbsolutePosition &LeftFootInitialPosition,
- FootAbsolutePosition &RightFootInitialPosition,
- deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions);
-
- /*! \brief Method to compute absolute feet positions from a set of relative one.
- @param[in] RelativeFootPositions: The set of relative positions for the support foot.
- @param[in] SupportFootInitialPosition: The initial position of the support foot.
- @param[out] SupportFootAbsoluteFootPositions: The set of absolute foot positions
- corresponding to the set of relative foot positions (i.e given step by step
- and not every sampled control time).
+ void ComputeAbsoluteStepsFromRelativeSteps
+ (deque<RelativeFootPosition>
+ &RelativeFootPositions,
+ FootAbsolutePosition &LeftFootInitialPosition,
+ FootAbsolutePosition &RightFootInitialPosition,
+ deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions);
+
+ /*! \brief Method to compute absolute feet positions from a set of
+ relative one.
+ @param[in] RelativeFootPositions: The set of relative positions for the
+ support foot.
+ @param[in] SupportFootInitialPosition: The initial position of the
+ support foot.
+ @param[out] SupportFootAbsoluteFootPositions: The set of absolute foot
+ positions
+ corresponding to the set of relative foot positions
+ (i.e given step by step and not every sampled control time).
*/
- void ComputeAbsoluteStepsFromRelativeSteps(deque<RelativeFootPosition>
- &RelativeFootPositions,
- FootAbsolutePosition &SupportFootInitialPosition,
- deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions);
-
- /*! \brief Method to compute relative feet positions from a set of absolute one
- where one has changed.
- @param[in] RelativeFootPositions: The set of relative positions for the support foot.
- @param[in] ChangedInterval: The interval where the absolute foot position has been changed.
- @param[in] SupportFootInitialPosition: The absolute foot position of the initial step in the stack of steps.
- @param[out] SupportFootAbsoluteFootPositions: The set of absolute foot positions
- corresponding to the set of relative foot positions (i.e given step by step
- and not every sampled control time).
+ void ComputeAbsoluteStepsFromRelativeSteps
+ (deque<RelativeFootPosition>
+ &RelativeFootPositions,
+ FootAbsolutePosition &SupportFootInitialPosition,
+ deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions);
+
+ /*! \brief Method to compute relative feet positions from a set of absolute
+ one where one has changed.
+ @param[in] RelativeFootPositions: The set of relative positions for the
+ support foot.
+ @param[in] ChangedInterval: The interval where the absolute foot position
+ has been changed.
+ @param[in] SupportFootInitialPosition: The absolute foot position of the
+ initial step in the stack of steps.
+ @param[out] SupportFootAbsoluteFootPositions: The set of absolute foot
+ positions corresponding to the set of relative foot positions
+ (i.e given step by step and not every sampled control time).
*/
- void ChangeRelStepsFromAbsSteps(deque<RelativeFootPosition>
- &RelativeFootPositions,
- FootAbsolutePosition &SupportFootInitialPosition,
- deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions,
- unsigned int ChangedInterval);
-
+ void ChangeRelStepsFromAbsSteps
+ (deque<RelativeFootPosition>
+ &RelativeFootPositions,
+ FootAbsolutePosition &SupportFootInitialPosition,
+ deque<FootAbsolutePosition> &SupportFootAbsoluteFootPositions,
+ unsigned int ChangedInterval);
+
/*! Returns foot */
PRFoot *getFoot() const;
diff --git a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
index bfc0f7ab16eea1a0355fe1d1a9aafc78d90285fa..3012c6d8ec9c4cd7680fe5804bdf6d067a3b2086 100644
--- a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
+++ b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
@@ -431,7 +431,8 @@ interpolate_feet_positions
FinalRightFootTraj_deq [CurrentIndex+k].ddtheta = 0.0 ;
FinalLeftFootTraj_deq[CurrentIndex+k].time =
- FinalRightFootTraj_deq[CurrentIndex+k].time = Time+k*m_SamplingPeriod;
+ FinalRightFootTraj_deq[CurrentIndex+k].time =
+ Time+k*m_SamplingPeriod;
FinalLeftFootTraj_deq[CurrentIndex+k].stepType =
FinalRightFootTraj_deq[CurrentIndex+k].stepType = 10;
}
@@ -522,7 +523,8 @@ interpolate_feet_positions
StepType, 1);
}
FinalLeftFootTraj_deq[CurrentIndex+k].time =
- FinalRightFootTraj_deq[CurrentIndex+k].time = Time+k*m_SamplingPeriod;
+ FinalRightFootTraj_deq[CurrentIndex+k].time =
+ Time+k*m_SamplingPeriod;
}
}
else if (CurrentSupport.Phase == DS ||
@@ -564,7 +566,8 @@ interpolate_feet_positions
FinalRightFootTraj_deq [CurrentIndex+k].ddtheta = 0.0 ;
FinalLeftFootTraj_deq[CurrentIndex+k].time =
- FinalRightFootTraj_deq[CurrentIndex+k].time = Time+k*m_SamplingPeriod;
+ FinalRightFootTraj_deq[CurrentIndex+k].time =
+ Time+k*m_SamplingPeriod;
FinalLeftFootTraj_deq[CurrentIndex+k].stepType =
FinalRightFootTraj_deq[CurrentIndex+k].stepType = 10;
}
diff --git a/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.hh b/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.hh
index 53a0e39b25bcc684698a3ab6a0985a24e6f318a5..ebdf3b62660691da954b09739529ba3b7c24352c 100644
--- a/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.hh
+++ b/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.hh
@@ -56,16 +56,22 @@ namespace PatternGeneratorJRL
*/
/*! Perform a 5 ms step to generate the necessary information.
- \note{The meaning and the way to use this method depends on the child class}.
+ \note{The meaning and the way to use this method depends on the child
+ class}.
@param[out] LeftFootPosition: The position of the Left Foot position.
@param[out] RightFootPosition: The position of the Right Foot position.
- @param[out] ZMPRefPos: The ZMP position to be feed to the controller, in the waist
+ @param[out] ZMPRefPos: The ZMP position to be feed to the controller,
+ in the waist
frame reference.
- @param[out] finalCOMState: returns CoM state position, velocity and acceleration.
- @param[out] CurrentConfiguration: The results is a state vector containing the articular positions.
- @param[out] CurrentVelocity: The results is a state vector containing the speed.
- @param[out] CurrentAcceleration: The results is a state vector containing the acceleration.
+ @param[out] finalCOMState: returns CoM state position, velocity
+ and acceleration.
+ @param[out] CurrentConfiguration: The results is a state vector
+ containing the articular positions.
+ @param[out] CurrentVelocity: The results is a state vector
+ containing the speed.
+ @param[out] CurrentAcceleration: The results is a state vector
+ containing the acceleration.
*/
int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition,
@@ -78,14 +84,17 @@ namespace PatternGeneratorJRL
/*! Computes the COM of the robot with the Joint values given in BodyAngles,
- velocities set to zero, and returns the values of the COM in aStaringCOMState.
- Assuming that the waist is at (0,0,0)
+ velocities set to zero, and returns the values of the COM in
+ aStaringCOMState. Assuming that the waist is at (0,0,0)
it returns the associate initial values for the left and right foot.
- @param[in] BodyAngles: 4x4 matrix of the robot's root (most of the time, the waist)
+ @param[in] BodyAngles: 4x4 matrix of the robot's root (most of the time,
+ the waist)
pose (position + orientation).
- @param[out] aStartingCOMState: Returns the 3D position of the CoM for the current
+ @param[out] aStartingCOMState: Returns the 3D position of the CoM for
+ the current
position of the robot.
- @param[out] aStartingZMPPosition: Returns the 3D position of the ZMP for the current
+ @param[out] aStartingZMPPosition: Returns the 3D position of the ZMP for
+ the current
position of the robot.
@param[out] InitLeftFootPosition: Returns the position of the left foot in
the waist coordinates frame.
@@ -138,8 +147,8 @@ namespace PatternGeneratorJRL
/*! Count the number of successives hits on the bottom of the buffers. */
int m_NbOfHitBottom;
- /*! Keeps a link towards an object allowing to find a pose for a given CoM and
- foot position. */
+ /*! Keeps a link towards an object allowing to find a pose for a given CoM
+ and foot position. */
std::vector<ComAndFootRealization *>m_ComAndFootRealization;
/*! Set the position of the buffer size limit. */
diff --git a/src/GlobalStrategyManagers/DoubleStagePreviewControlStrategy.hh b/src/GlobalStrategyManagers/DoubleStagePreviewControlStrategy.hh
index 727728d4e17f46a3e5385c14344536397db83783..2e84a8a593710662425d4fadf7a77e4d55aebf0d 100644
--- a/src/GlobalStrategyManagers/DoubleStagePreviewControlStrategy.hh
+++ b/src/GlobalStrategyManagers/DoubleStagePreviewControlStrategy.hh
@@ -45,22 +45,29 @@ namespace PatternGeneratorJRL
public:
/*! Default constructor. */
- DoubleStagePreviewControlStrategy(SimplePluginManager *aSimplePluginManager);
+ DoubleStagePreviewControlStrategy
+ (SimplePluginManager *aSimplePluginManager);
/*! Default destructor. */
~DoubleStagePreviewControlStrategy();
/*! Perform a 5 ms step to generate the necessary information.
- \note{The meaning and the way to use this method depends on the child class}.
+ \note{The meaning and the way to use this method depends on the child
+ class}.
@param[out] LeftFootPosition: The position of the Left Foot position.
@param[out] RightFootPosition: The position of the Right Foot position.
- @param[out] ZMPRefPos: The ZMP position to be feed to the controller, in the waist
+ @param[out] ZMPRefPos: The ZMP position to be feed to the controller,
+ in the waist
frame reference.
- @param[out] COMState: returns position, velocity and acceleration of the CoM.
- @param[out] CurrentConfiguration: The results is a state vector containing the articular positions.
- @param[out] CurrentVelocity: The results is a state vector containing the speed.
- @param[out] CurrentAcceleration: The results is a state vector containing the acceleration.
+ @param[out] COMState: returns position, velocity and acceleration of the
+ CoM.
+ @param[out] CurrentConfiguration: The results is a state vector
+ containing the articular positions.
+ @param[out] CurrentVelocity: The results is a state vector containing
+ the speed.
+ @param[out] CurrentAcceleration: The results is a state vector containing
+ the acceleration.
*/
int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition,
@@ -73,14 +80,18 @@ namespace PatternGeneratorJRL
/*! Computes the COM of the robot with the Joint values given in BodyAngles,
- velocities set to zero, and returns the values of the COM in aStaringCOMState.
+ velocities set to zero, and returns the values of the COM in
+ aStaringCOMState.
Assuming that the waist is at (0,0,0)
it returns the associate initial values for the left and right foot.
- @param[in] BodyAngles: 4x4 matrix of the robot's root (most of the time, the waist)
+ @param[in] BodyAngles: 4x4 matrix of the robot's root (most of the time,
+ the waist)
pose (position + orientation).
- @param[out] aStartingCOMState: Returns the 3D position of the CoM for the current
+ @param[out] aStartingCOMState: Returns the 3D position of the CoM for
+ the current
position of the robot.
- @param[out] aStartingZMPPosition: Returns the 3D position of the ZMP for the current
+ @param[out] aStartingZMPPosition: Returns the 3D position of the ZMP for
+ the current
position of the robot.
@param[out] InitLeftFootPosition: Returns the position of the left foot in
the waist coordinates frame.
@@ -130,8 +141,10 @@ namespace PatternGeneratorJRL
/*! \brief Prepare the buffers at the beginning of the foot positions.
@param[out] aZMPositions: ZMP position reference
@param[out] aCOMBuffer: COM trajectory
- @param[out] aLeftFootAbsolutePositions: Trajectory of absolute positions for the left foot.
- @param[out] aRightFootAbsolutePositions: Trajectory of absolute positions for the right foot.
+ @param[out] aLeftFootAbsolutePositions: Trajectory of absolute positions
+ for the left foot.
+ @param[out] aRightFootAbsolutePositions: Trajectory of absolute positions
+ for the right foot.
*/
void Setup(deque<ZMPPosition> & aZMPositions,
deque<COMState> & aCOMBuffer,
diff --git a/src/GlobalStrategyManagers/GlobalStrategyManager.cpp b/src/GlobalStrategyManagers/GlobalStrategyManager.cpp
index 86b9afa78d5cbf4e551801d0f9905785583eed0c..b900fcf0e2d84d9b3144b148fe39ce40d2363828 100644
--- a/src/GlobalStrategyManagers/GlobalStrategyManager.cpp
+++ b/src/GlobalStrategyManagers/GlobalStrategyManager.cpp
@@ -22,8 +22,8 @@
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/*! \file GlobalStrategyManager.cpp
- \brief This object defines a global strategy abstract object to generate an output
- handled by the PatternGeneratorInterface object.
+ \brief This object defines a global strategy abstract object to generate
+ an output handled by the PatternGeneratorInterface object.
*/
#include <Debug.hh>
diff --git a/src/GlobalStrategyManagers/GlobalStrategyManager.hh b/src/GlobalStrategyManagers/GlobalStrategyManager.hh
index 3712f62aa98b143324d1d4da3747adb0002f0fec..60406031709f38fd8f4d5eab074e363f9e547987 100644
--- a/src/GlobalStrategyManagers/GlobalStrategyManager.hh
+++ b/src/GlobalStrategyManagers/GlobalStrategyManager.hh
@@ -22,8 +22,8 @@
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/*! \file GlobalStrategyManager.h
- \brief This object defines a global strategy abstract object to generate an output
- handled by the PatternGeneratorInterface object. */
+ \brief This object defines a global strategy abstract object to
+ generate an output handled by the PatternGeneratorInterface object. */
#include <deque>
@@ -70,15 +70,20 @@ namespace PatternGeneratorJRL
/*! Perform a 5 ms step to generate the necessary information.
- \note{The meaning and the way to use this method depends on the child class}.
+ \note{The meaning and the way to use this method depends on
+ the child class}.
@param[out] LeftFootPosition: The position of the Left Foot position.
@param[out] RightFootPosition: The position of the Right Foot position.
@param[out] ZMPRefPos: The ZMP position to be fed to the controller.
- @param[out] aCOMState: returns position, velocity and acceleration of the CoM.
- @param[out] CurrentConfiguration: The results is a state vector containing the articular positions.
- @param[out] CurrentVelocity: The results is a state vector containing the speed.
- @param[out] CurrentAcceleration: The results is a state vector containing the acceleration.
+ @param[out] aCOMState: returns position, velocity and acceleration of the
+ CoM.
+ @param[out] CurrentConfiguration: The results is a state vector containing
+ the articular positions.
+ @param[out] CurrentVelocity: The results is a state vector containing the
+ speed.
+ @param[out] CurrentAcceleration: The results is a state vector containing
+ the acceleration.
*/
virtual int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition,
@@ -91,27 +96,32 @@ namespace PatternGeneratorJRL
/*! Computes the COM of the robot with the Joint values given in BodyAngles,
- velocities set to zero, and returns the values of the COM in aStaringCOMPosition.
+ velocities set to zero, and returns the values of the COM in
+ aStaringCOMPosition.
Assuming that the waist is at (0,0,0)
it returns the associate initial values for the left and right foot.
- @param[in] BodyAngles: 4x4 matrix of the robot's root (most of the time, the waist)
+ @param[in] BodyAngles: 4x4 matrix of the robot's root (most of the time,
+ the waist)
pose (position + orientation).
- @param[out] aStartingCOMPosition: Returns the 3D position of the CoM for the current
+ @param[out] aStartingCOMPosition: Returns the 3D position of the CoM for
+ the current
position of the robot.
- @param[out] aStartingZMPPosition: Returns the 3D position of the ZMP for the current
+ @param[out] aStartingZMPPosition: Returns the 3D position of the ZMP for
+ the current
position of the robot.
@param[out] InitLeftFootPosition: Returns the position of the left foot in
the waist coordinates frame.
@param[out] InitRightFootPosition: Returns the position of the right foot
in the waist coordinates frame.
*/
- virtual int EvaluateStartingState(Eigen::VectorXd & BodyAngles,
- COMState & aStartingCOMState,
- Eigen::Vector3d & aStartingZMPPosition,
- Eigen::Matrix<double, 6, 1> & aStartingWaistPose,
- FootAbsolutePosition & InitLeftFootPosition,
- FootAbsolutePosition & InitRightFootPosition)=0;
-
+ virtual int EvaluateStartingState
+ (Eigen::VectorXd & BodyAngles,
+ COMState & aStartingCOMState,
+ Eigen::Vector3d & aStartingZMPPosition,
+ Eigen::Matrix<double, 6, 1> & aStartingWaistPose,
+ FootAbsolutePosition & InitLeftFootPosition,
+ FootAbsolutePosition & InitRightFootPosition)=0;
+
/*! \brief Method to detect the status regarding the end of the motion.
This method returns :
@@ -122,22 +132,28 @@ namespace PatternGeneratorJRL
virtual int EndOfMotion()=0;
/*! \brief Setting the pointers towards buffers of positions.
- \param[in] aZMPositions: Absolute frame positions buffer of the Zero Momentum Point reference.
- \param[in] aCOMBuffer: Absolute frame positions buffer of the CoM trajectory related to the previous
+ \param[in] aZMPositions: Absolute frame positions buffer of the
+ Zero Momentum Point reference.
+ \param[in] aCOMBuffer: Absolute frame positions buffer of the CoM
+ trajectory related to the previous
ZMP reference trajectory.
- \param[in] aLeftFootAbsolutePositions: Absolute frame positions buffer of the left foot.
- \param[in] aRightFootAbsolutePositions: Absolute frame positions buffer of the right foot.
+ \param[in] aLeftFootAbsolutePositions: Absolute frame positions
+ buffer of the left foot.
+ \param[in] aRightFootAbsolutePositions: Absolute frame positions
+ buffer of the right foot.
*/
- void SetBufferPositions(deque<ZMPPosition> * aZMPositions,
- deque<COMState> * aCOMBuffer,
- deque<FootAbsolutePosition> *aLeftFootAbsolutePositions,
- deque<FootAbsolutePosition> *aRightFootAbsolutePositions );
-
+ void SetBufferPositions
+ (deque<ZMPPosition> * aZMPositions,
+ deque<COMState> * aCOMBuffer,
+ deque<FootAbsolutePosition> *aLeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> *aRightFootAbsolutePositions );
+
/*! Prepare the buffers at the beginning of the foot positions. */
- virtual void Setup(deque<ZMPPosition> & aZMPositions,
- deque<COMState> & aCOMBuffer,
- deque<FootAbsolutePosition> & aLeftFootAbsolutePositions,
- deque<FootAbsolutePosition> & aRightFootAbsolutePositions )=0;
+ virtual void Setup
+ (deque<ZMPPosition> & aZMPositions,
+ deque<COMState> & aCOMBuffer,
+ deque<FootAbsolutePosition> & aLeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> & aRightFootAbsolutePositions )=0;
protected:
@@ -169,15 +185,16 @@ namespace PatternGeneratorJRL
public:
/*! \name Setter and getter for the jrlHumanoidDynamicRobot object. */
- /*! @param[in] aHumanoidDynamicRobot: an object able to compute dynamic parameters
- of the robot. */
+ /*! @param[in] aHumanoidDynamicRobot: an object able to compute dynamic
+ parameters of the robot. */
inline bool setHumanoidDynamicRobot(PinocchioRobot *aHumanoidDynamicRobot)
{
m_PinocchioRobot = aHumanoidDynamicRobot;
return true;
}
- /*! \brief Returns the object able to compute the dynamic parameters of the robot. */
+ /*! \brief Returns the object able to compute the dynamic parameters
+ of the robot. */
inline PinocchioRobot * getHumanoidDynamicRobot() const
{
return m_PinocchioRobot;
diff --git a/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp b/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp
index 22739f1e2770c702bd266a3ee14980b9a48945bb..3a1d19095bf817ea8d6ebf256b9a52b4dd7369ef 100644
--- a/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp
+++ b/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp
@@ -99,7 +99,8 @@ namespace PatternGeneratorJRL
{
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;
@@ -117,8 +118,9 @@ namespace PatternGeneratorJRL
double add2r;
add2r = m_ListOfCOGPolynomials[j]->Compute(deltaj);
ODEBUG( " add2r: " << add2r <<" " \
- << "Polynomial ("<<j<<") with degree (supposed:"<< m_PolynomialDegree[j] <<
- ")"<<endl \
+ << "Polynomial ("<<j
+ <<") with degree (supposed:"<< m_PolynomialDegree[j]
+ <<")"<<endl \
<< "Coefficients:");
// m_ListOfCOGPolynomials[j]->print();
r+=add2r;
@@ -144,7 +146,8 @@ namespace PatternGeneratorJRL
{
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;
@@ -161,8 +164,9 @@ namespace PatternGeneratorJRL
double add2r;
add2r = m_ListOfCOGPolynomials[j]->ComputeDerivative(deltaj);
ODEBUG( " add2r: " << add2r <<" " \
- << "Polynomial ("<<j<<") with degree (supposed:"<< m_PolynomialDegree[j] <<
- ")"<<endl \
+ << "Polynomial ("<<j
+ << ") with degree (supposed:"<< m_PolynomialDegree[j]
+ << ")"<<endl \
<< "Coefficients:");
// m_ListOfCOGPolynomials[j]->print();
r+=add2r;
@@ -220,7 +224,8 @@ namespace PatternGeneratorJRL
ODEBUG(" ====== ZMP ====== " );
for(unsigned int j=0; j<m_DeltaTj.size(); 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;
@@ -236,7 +241,8 @@ namespace PatternGeneratorJRL
double add2r;
add2r = m_ListOfZMPPolynomials[j]->Compute(deltaj);
ODEBUG( " add2r: " << add2r << " " );
- ODEBUG("Polynomial ("<<j<<") with degree (supposed:"<< m_PolynomialDegree[j] <<
+ ODEBUG("Polynomial ("<<j
+ <<") with degree (supposed:"<< m_PolynomialDegree[j] <<
")");
ODEBUG("Coefficients:");
// m_ListOfZMPPolynomials[j]->print();
@@ -261,7 +267,8 @@ namespace PatternGeneratorJRL
{
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;
@@ -277,8 +284,9 @@ namespace PatternGeneratorJRL
double add2r;
add2r = m_ListOfCOGPolynomials[j]->ComputeDerivative(deltaj);
ODEBUG( " add2r: " << add2r <<" " \
- << "Polynomial ("<<j<<") with degree (supposed:"<< m_PolynomialDegree[j] <<
- ")"<<endl \
+ << "Polynomial ("<<j
+ <<") with degree (supposed:"
+ << m_PolynomialDegree[j] << ")"<<endl \
<< "Coefficients:");
// m_ListOfCOGPolynomials[j]->print();
r+=add2r;
@@ -301,9 +309,10 @@ namespace PatternGeneratorJRL
return true;
}
- void AnalyticalZMPCOGTrajectory::SetCoGHyperbolicCoefficients(
- vector<double> &lV,
- vector<double> &lW)
+ void AnalyticalZMPCOGTrajectory::
+ SetCoGHyperbolicCoefficients
+ (vector<double> &lV,
+ vector<double> &lW)
{
if ((int)lV.size()==m_NbOfIntervals)
m_V = lV;
@@ -311,9 +320,10 @@ namespace PatternGeneratorJRL
m_W = lW;
}
- void AnalyticalZMPCOGTrajectory::SetStartingTimeIntervalsAndHeightVariation(
- vector<double> &lTj,
- vector<double> &lomegaj)
+ void AnalyticalZMPCOGTrajectory::
+ SetStartingTimeIntervalsAndHeightVariation
+ (vector<double> &lTj,
+ vector<double> &lomegaj)
{
if ((int)lTj.size()==m_NbOfIntervals)
{
@@ -327,7 +337,8 @@ namespace PatternGeneratorJRL
}
}
else
- cerr << "Pb while initializing the time intervals. " << lTj.size() << " " <<
+ cerr << "Pb while initializing the time intervals. "
+ << lTj.size() << " " <<
m_NbOfIntervals << endl;
if ((int)lomegaj.size()==m_NbOfIntervals)
m_omegaj = lomegaj;
@@ -359,35 +370,44 @@ namespace PatternGeneratorJRL
}
- void AnalyticalZMPCOGTrajectory::GetPolynomialDegrees(vector<unsigned int>
- &lPolynomialDegree) const
+ void AnalyticalZMPCOGTrajectory::
+ GetPolynomialDegrees
+ (vector<unsigned int>
+ &lPolynomialDegree) const
{
lPolynomialDegree=m_PolynomialDegree;
}
- void AnalyticalZMPCOGTrajectory::GetNumberOfIntervals(unsigned int
- &lNbOfIntervals) const
+ void AnalyticalZMPCOGTrajectory::
+ GetNumberOfIntervals
+ (unsigned int
+ &lNbOfIntervals) const
{
lNbOfIntervals = m_NbOfIntervals;
}
- void AnalyticalZMPCOGTrajectory::GetHyperbolicCoefficients(vector<double> &lV,
- vector<double> &lW) const
+ void AnalyticalZMPCOGTrajectory::
+ GetHyperbolicCoefficients
+ (vector<double> &lV,
+ vector<double> &lW) const
{
lV = m_V;
lW = m_W;
}
- void AnalyticalZMPCOGTrajectory::GetStartingPointAndHeightVariation(
- vector<double> &lTj,
- vector<double> &lomegaj)
+ void AnalyticalZMPCOGTrajectory::
+ GetStartingPointAndHeightVariation
+ (vector<double> &lTj,
+ vector<double> &lomegaj)
{
lTj = m_DeltaTj;
lomegaj = m_omegaj;
}
- bool AnalyticalZMPCOGTrajectory::GetFromListOfCOGPolynomials(unsigned int j,
- Polynome * &aPoly ) const
+ bool AnalyticalZMPCOGTrajectory::
+ GetFromListOfCOGPolynomials
+ (unsigned int j,
+ Polynome * &aPoly ) const
{
aPoly = 0;
@@ -399,8 +419,10 @@ namespace PatternGeneratorJRL
return false;
}
- bool AnalyticalZMPCOGTrajectory::GetFromListOfZMPPolynomials(unsigned int j,
- Polynome * &aPoly ) const
+ bool AnalyticalZMPCOGTrajectory::
+ GetFromListOfZMPPolynomials
+ (unsigned int j,
+ Polynome * &aPoly ) const
{
aPoly = 0;
@@ -412,10 +434,11 @@ namespace PatternGeneratorJRL
return false;
}
- void AnalyticalZMPCOGTrajectory::TransfertOneIntervalCoefficientsFromCOGTrajectoryToZMPOne(
- unsigned int intervalindex,
- double &lCoMZ,
- double &lZMPZ)
+ void AnalyticalZMPCOGTrajectory::
+ TransfertOneIntervalCoefficientsFromCOGTrajectoryToZMPOne
+ (unsigned int intervalindex,
+ double &lCoMZ,
+ double &lZMPZ)
{
vector<double> CoefsFromCOG;
@@ -442,24 +465,27 @@ namespace PatternGeneratorJRL
}
- void AnalyticalZMPCOGTrajectory::TransfertCoefficientsFromCOGTrajectoryToZMPOne(
- vector<double> &lCoMZ,
- vector<double> &lZMPZ)
+ void AnalyticalZMPCOGTrajectory::
+ TransfertCoefficientsFromCOGTrajectoryToZMPOne
+ (vector<double> &lCoMZ,
+ vector<double> &lZMPZ)
{
for(int j=0; j<m_NbOfIntervals; j++)
{
double lCoMZ2 = lCoMZ[j];
double lZMPZ2 = lZMPZ[j];
- TransfertOneIntervalCoefficientsFromCOGTrajectoryToZMPOne(j,lCoMZ2,lZMPZ2);
+ TransfertOneIntervalCoefficientsFromCOGTrajectoryToZMPOne
+ (j,lCoMZ2,lZMPZ2);
}
}
- void AnalyticalZMPCOGTrajectory::Building3rdOrderPolynomial(
- unsigned int anIntervalj,
- double pjTjm1,
- double pjTj)
+ void AnalyticalZMPCOGTrajectory::
+ Building3rdOrderPolynomial
+ (unsigned int anIntervalj,
+ double pjTjm1,
+ double pjTj)
{
vector<double> CoefsForZMP, CoefsForCOG;
CoefsForZMP.resize(4);
@@ -511,8 +537,10 @@ namespace PatternGeneratorJRL
return GetIntervalIndexFromTime(t, j, prev_j);
}
- bool AnalyticalZMPCOGTrajectory::GetIntervalIndexFromTime(double t,
- unsigned int &j, unsigned int &prev_j)
+ bool AnalyticalZMPCOGTrajectory::
+ GetIntervalIndexFromTime
+ (double t,
+ unsigned int &j, unsigned int &prev_j)
{
ODEBUG("Here "<< m_DeltaTj.size());
t -= m_AbsoluteTimeReference;
@@ -522,7 +550,8 @@ namespace PatternGeneratorJRL
{
ODEBUG("t: " << t << " reftime :" << reftime << " Tj["<<j << "]= "<<
m_DeltaTj[j]);
- if (((t+m_Sensitivity)>=reftime) && (t<=reftime+m_DeltaTj[lj]+m_Sensitivity))
+ if (((t+m_Sensitivity)>=reftime) &&
+ (t<=reftime+m_DeltaTj[lj]+m_Sensitivity))
{
j = lj;
return true;
diff --git a/src/Mathematics/AnalyticalZMPCOGTrajectory.hh b/src/Mathematics/AnalyticalZMPCOGTrajectory.hh
index 2b38170afa0d3e91735aed083b65320061a45528..468beda8171631033f2ed84f33638d7b8f35b42e 100644
--- a/src/Mathematics/AnalyticalZMPCOGTrajectory.hh
+++ b/src/Mathematics/AnalyticalZMPCOGTrajectory.hh
@@ -148,8 +148,9 @@ namespace PatternGeneratorJRL
std::vector<double> &lW);
/*! \brief Set the starting point and the height variation. */
- void SetStartingTimeIntervalsAndHeightVariation(std::vector<double> &lDeltaTj,
- std::vector<double> &lomegaj);
+ void SetStartingTimeIntervalsAndHeightVariation
+ (std::vector<double> &lDeltaTj,
+ std::vector<double> &lomegaj);
/*! \brief Set the degree of each polynomials for the CoG
Remark: the size of the vector of degrees should match the number
@@ -159,7 +160,8 @@ namespace PatternGeneratorJRL
/*! \brief Get the degree of each polynomials for the CoG.
*/
- void GetPolynomialDegrees(std::vector<unsigned int> &lPolynomialDegree) const;
+ void GetPolynomialDegrees
+ (std::vector<unsigned int> &lPolynomialDegree) const;
/*! \brief Set the number of Intervals for this
trajectory. */
@@ -187,23 +189,26 @@ namespace PatternGeneratorJRL
*/
bool GetFromListOfZMPPolynomials(unsigned int j, Polynome * & aPoly ) const;
- /*! \brief Transfert the coefficients from the COG trajectory to the ZMP for all intervals.
+ /*! \brief Transfert the coefficients from the COG trajectory
+ to the ZMP for all intervals.
@param lCOMZ: Profile of the height CoM for each interval.
@param lZMPZ: Profile of the height ZMP for each interval.
*/
- void TransfertCoefficientsFromCOGTrajectoryToZMPOne(std::vector<double> &lCOMZ,
- std::vector<double> &lZMPZ);
+ void TransfertCoefficientsFromCOGTrajectoryToZMPOne
+ (std::vector<double> &lCOMZ,
+ std::vector<double> &lZMPZ);
/*! \brief Transfert the coefficients from the COG trajectory to the ZMP.
@param IntervalIndex: Number of the interval.
@param lCOMZ: Value of the CoM height for this interval.
@param lZMPZ: Value of the ZMP height for this interval.
*/
- void TransfertOneIntervalCoefficientsFromCOGTrajectoryToZMPOne(
- unsigned int IntervalIndex,
- double &lCOMZ,
- double &lZMPZ);
-
+ void TransfertOneIntervalCoefficientsFromCOGTrajectoryToZMPOne
+ (
+ unsigned int IntervalIndex,
+ double &lCOMZ,
+ double &lZMPZ);
+
/*! Build the coefficients of a third order COG polynomial according
to the OmegaC and the value of the ZMP at the beginning and the end
@@ -214,7 +219,8 @@ namespace PatternGeneratorJRL
/*@} */
- /*! \brief Returns the maximal fluctuation for the first segment of this trajectory. */
+ /*! \brief Returns the maximal fluctuation for the first segment of this
+ trajectory. */
double FluctuationMaximal();
friend std::ostream& operator <<(std::ostream &os,
@@ -237,7 +243,8 @@ namespace PatternGeneratorJRL
/*! \brief Get the index of the interval according to the time,
and the previous value of the interval. */
- bool GetIntervalIndexFromTime(double t, unsigned int &j, unsigned int &prev_j);
+ bool GetIntervalIndexFromTime(double t, unsigned int &j,
+ unsigned int &prev_j);
protected:
diff --git a/src/Mathematics/Bsplines.cpp b/src/Mathematics/Bsplines.cpp
index 6691846c3095961f50cb0d81c4d4adbcf6590f44..bff361f08dfdb2af151d76a1e5320e64559b76c0 100644
--- a/src/Mathematics/Bsplines.cpp
+++ b/src/Mathematics/Bsplines.cpp
@@ -33,98 +33,6 @@ void Bsplines::GenerateDegree()
m_degree = (unsigned)degree ;
}
-//void Bsplines::GenerateKnotVector(std::string method)
-//{
-// /*Calculer set of parameters*/
-// unsigned int i,j;
-// vector<double> set_of_pam;
-
-// if (method == "centripetal")
-// {
-// //cout << "centripetal" << endl;
-// set_of_pam.clear();
-// set_of_pam.reserve(m_control_points.size());
-// cout << m_control_points.size() << endl;
-// double L = 0.0;
-// double D = 0.0;
-// for (i=0;i<m_control_points.size()-1;i++)
-// {
-// L += sqrt ( sqrt(((m_control_points[i].x - m_control_points[i+1].x)*(m_control_points[i].x - m_control_points[i+1].x)) +
-// ((m_control_points[i].y - m_control_points[i+1].y)*(m_control_points[i].y - m_control_points[i+1].y)) ) );
-// }
-// for (i=0;i<m_control_points.size();i++)
-// {
-// if (i == 0)
-// {
-// set_of_pam.push_back(0.0);
-// }
-// else if (i == m_control_points.size()-1)
-// {
-// set_of_pam.push_back(1.0);
-// }
-// else
-// {
-// D += sqrt ( sqrt(((m_control_points[i].x - m_control_points[i+1].x)*(m_control_points[i].x - m_control_points[i+1].x)) +
-// ((m_control_points[i].y - m_control_points[i+1].y)*(m_control_points[i].y - m_control_points[i+1].y)) ) );
-// set_of_pam.push_back(D/L);
-// }
-// }
-
-// m_knot.clear();
-// double U = 0.0;
-// for (i=0;i<=m_degree;i++)
-// {
-// m_knot.push_back(0.0);
-// }
-
-// if (m_control_points.size()-1>=m_degree)
-// {
-// for (j=1;j<=m_control_points.size()-1-m_degree;j++)
-// {
-// i=j;
-// U=0.0;
-// while (i<=m_degree-1+j)
-// {
-// U +=set_of_pam[i];
-// i++;
-// }
-// m_knot.push_back(U/double(m_degree));
-// }
-// }
-
-// for (i=0;i<=m_degree;i++)
-// {
-// m_knot.push_back(1.0);
-// }
-// if (m_knot.size() - 1 != m_control_points.size() + m_degree)
-// {
-// cout << "Knot vector cant be created. m_control_points.size()-1>=m_degree "<< endl;
-// m_knot.clear();
-// }
-
-// }
-
-// else if (method =="universal")
-// {
-// m_knot.clear();
-// //cout << "universal" << endl;
-// double U=0;
-// for (i=0;i<=m_degree;i++)
-// {
-// m_knot.push_back(0.0);
-// }
-// for (i=1;i<=m_control_points.size()-1-m_degree;i++)
-// {
-// U = double(i)/(m_control_points.size()-1-m_degree+1);
-// m_knot.push_back(U);
-// }
-// for (i=0;i<=m_degree;i++)
-// {
-// m_knot.push_back(1.0);
-// }
-
-// }
-//}
int Bsplines::ComputeBasisFunctions(double t)
{
@@ -161,13 +69,14 @@ int Bsplines::ComputeBasisFunctions(double t)
if ( m_knot[i] == m_knot[i+j] )
tmp1 = 0.0 ;
else
- tmp1 = (t - m_knot[i]) / (m_knot[i+j]-m_knot[i]) * m_basis_functions[j-1][i] ;
+ tmp1 = (t - m_knot[i]) / (m_knot[i+j]-m_knot[i]) *
+ m_basis_functions[j-1][i] ;
if (m_knot[i+j+1] == m_knot[i+1])
tmp2 = 0.0 ;
else
- tmp2 = (m_knot[i+j+1] - t) / (m_knot[i+j+1]-m_knot[i+1] ) * m_basis_functions[j
- -1][i+1] ;
+ tmp2 = (m_knot[i+j+1] - t) / (m_knot[i+j+1]-m_knot[i+1] ) *
+ m_basis_functions[j-1][i+1] ;
m_basis_functions[j][i] = tmp1 + tmp2 ;
}
@@ -196,7 +105,8 @@ int Bsplines::ComputeBasisFunctions(double t)
- //compute their time second derivative for the degree p=m_degree : dd Nip(t) /dt
+ // compute their time second derivative for the degree
+ // p=m_degree : dd Nip(t) /dt
m_basis_functions_sec_derivative.resize(m_basis_functions[m_degree].size());
double factor = (double)(m_degree*(m_degree-1));
double den1(0.0),den2(0.0),den3(0.0),den4(0.0);
@@ -252,8 +162,10 @@ int Bsplines::ComputeBasisFunctions(double t)
}
-int Bsplines::ComputeBasisFunctionsRecursively(double t,
- std::deque<double> &m_knot, unsigned int m_degree)
+int Bsplines::
+ComputeBasisFunctionsRecursively
+(double t,
+ std::deque<double> &m_knot, unsigned int m_degree)
{
vector<double> basis_functions (m_knot.size()-m_degree-1);
for (unsigned int i = 0 ; i < basis_functions.size() ; ++i)
@@ -282,14 +194,16 @@ double Bsplines::Nij_t(int i, int j, double t, deque<double> & m_knot)
if ( m_knot[i] == m_knot[i+j] )
tmp1 = 0.0 ;
else
- tmp1 = (t - m_knot[i]) / (m_knot[i+j]-m_knot[i]) * Bsplines::Nij_t(i,j-1,t,
- m_knot) ;
+ tmp1 = (t - m_knot[i]) / (m_knot[i+j]-m_knot[i]) *
+ Bsplines::Nij_t(i,j-1,t,
+ m_knot) ;
if (m_knot[i+j+1] == m_knot[i+1])
tmp2 = 0.0 ;
else
- tmp2 = (m_knot[i+j+1] - t) / (m_knot[i+j+1]-m_knot[i+1] ) * Bsplines::Nij_t(i+1,
- j-1,t,m_knot) ;
+ tmp2 = (m_knot[i+j+1] - t) / (m_knot[i+j+1]-m_knot[i+1] ) *
+ Bsplines::Nij_t(i+1,
+ j-1,t,m_knot) ;
Nij_t = tmp1 + tmp2 ;
}
@@ -331,8 +245,10 @@ Bsplines Bsplines::DerivativeBsplines()
}
else
{
- dB_control_points[i] = ((m_control_points[i+1] - m_control_points[i])*double(
- m_degree) )/ (m_knot[i+m_degree+1] - m_knot[i+1]);
+ dB_control_points[i] = ((m_control_points[i+1] -
+ m_control_points[i])*
+ double( m_degree) )/
+ (m_knot[i+m_degree+1] - m_knot[i+1]);
}
}
@@ -622,7 +538,8 @@ void BSplinesFoot::ComputeControlPointFrom2DataPoint(void)
*ddN3T-FS*ddN2T0*dN4T0*ddN3T+dN2T*ddN4T*ddN3T0*IS-dN2T*ddN4T0*ddN3T*IS
-ddN4T0*FP*dN5T*ddN3T*dN2T0+dN2T*ddN0T0*ddN4T*IP*dN3T0-IA*dN2T*ddN4T
*dN3T0+ddN2T0*dN3T*dN4T0*FA-FS*dN3T0*ddN4T0*ddN2T-ddN5T*ddN3T0*dN4T
- *FP*dN2T0+dN2T*dN3T0*ddN4T0*FA-ddN4T*ddN2T0*dN3T*IS+ddN4T*ddN2T0*IP*dN3T*dN0T0)
+ *FP*dN2T0+dN2T*dN3T0*ddN4T0*FA-ddN4T*ddN2T0*dN3T*IS+ddN4T*ddN2T0*IP*dN3T*
+ dN0T0)
;
m_control_points[2]=
-1.0/( dN2T*dN3T0*ddN1T*ddN4T0-dN3T*dN4T0*ddN2T*ddN1T0+dN1T*ddN4T0*ddN3T
@@ -634,13 +551,16 @@ void BSplinesFoot::ComputeControlPointFrom2DataPoint(void)
*dN1T0*ddN3T+ddN3T0*dN4T*ddN1T*dN2T0-dN4T*ddN3T*dN2T0*ddN1T0+ddN3T0
*dN1T*dN4T0*ddN2T-dN2T*ddN4T*dN3T0*ddN1T0-ddN3T0*dN4T*dN1T0*ddN2T
+ddN4T*dN3T*dN2T0*ddN1T0-ddN4T*ddN3T0*dN1T*dN2T0)
- *( ddN0T0*dN4T*dN1T0*IP*ddN3T+ddN5T*ddN3T0*dN1T*dN4T0*FP-dN1T*ddN4T0*ddN3T*IS
+ *( ddN0T0*dN4T*dN1T0*IP*ddN3T+ddN5T*ddN3T0*dN1T*dN4T0*FP-dN1T*ddN4T0*ddN3T*
+ IS
+dN1T*IP*ddN4T0*ddN3T*dN0T0+dN4T0*FP*dN5T*ddN3T*ddN1T0-ddN5T*ddN3T0*dN4T
*dN1T0*FP+dN1T*dN3T0*ddN4T0*FA-ddN4T*dN3T0*FP*dN5T*ddN1T0-FS*dN4T0*ddN3T
*ddN1T0-dN1T0*ddN4T0*FP*dN5T*ddN3T+ddN3T0*dN4T*IP*ddN1T*dN0T0-ddN0T0
*dN4T*IP*dN3T0*ddN1T-IA*ddN4T*dN1T*dN3T0+IA*dN4T*dN3T0*ddN1T+IA*dN1T
- *dN4T0*ddN3T-IP*dN3T*ddN1T*ddN4T0*dN0T0-ddN4T*ddN3T0*dN1T*IP*dN0T0+FS*ddN3T0
- *dN4T0*ddN1T-IA*dN4T*dN1T0*ddN3T+ddN4T*ddN3T0*dN1T0*FP*dN5T-FS*dN3T0*ddN1T
+ *dN4T0*ddN3T-IP*dN3T*ddN1T*ddN4T0*dN0T0-ddN4T*ddN3T0*dN1T*IP*dN0T0+FS*
+ ddN3T0
+ *dN4T0*ddN1T-IA*dN4T*dN1T0*ddN3T+ddN4T*ddN3T0*dN1T0*FP*dN5T-FS*dN3T0*
+ ddN1T
*ddN4T0+ddN4T*IP*dN3T*dN0T0*ddN1T0-dN1T0*dN3T*ddN4T0*FA+FS*ddN4T*dN3T0
*ddN1T0-ddN3T0*dN4T*ddN1T*IS-ddN0T0*dN1T*IP*dN4T0*ddN3T-ddN3T0*dN4T0
*ddN1T*FP*dN5T-ddN3T0*dN1T*dN4T0*FA-ddN5T*dN1T*dN3T0*ddN4T0*FP-dN4T
@@ -648,51 +568,81 @@ void BSplinesFoot::ComputeControlPointFrom2DataPoint(void)
*ddN4T0*IS+dN3T*dN4T0*FA*ddN1T0-dN4T*IP*ddN3T*dN0T0*ddN1T0+ddN4T*ddN3T0
*dN1T*IS+IA*ddN4T*dN1T0*dN3T+ddN0T0*ddN4T*dN1T*IP*dN3T0+dN3T0*ddN1T
*ddN4T0*FP*dN5T-ddN0T0*ddN4T*dN1T0*IP*dN3T-FS*ddN4T*ddN3T0*dN1T0
- -IA*dN3T*dN4T0*ddN1T+ddN5T*dN4T*dN3T0*FP*ddN1T0+ddN0T0*IP*dN3T*dN4T0*ddN1T
+ -IA*dN3T*dN4T0*ddN1T+ddN5T*dN4T*dN3T0*FP*ddN1T0+ddN0T0*IP*dN3T*dN4T0*
+ ddN1T
+dN4T*ddN3T*ddN1T0*IS-ddN5T*dN3T*dN4T0*FP*ddN1T0+FS*dN1T0*ddN4T0*ddN3T
+ddN5T*dN1T0*dN3T*ddN4T0*FP);
m_control_points[3]=
- -1.0/( dN2T*dN3T0*ddN1T*ddN4T0-dN3T*dN4T0*ddN2T*ddN1T0+dN1T*ddN4T0*ddN3T*dN2T0
- -dN2T*ddN3T0*dN4T0*ddN1T+ddN4T*dN1T*ddN2T0*dN3T0+dN1T0*dN3T*ddN4T0*ddN2T
- +dN4T*dN3T0*ddN2T*ddN1T0-dN1T*ddN2T0*dN4T0*ddN3T+dN2T*dN4T0*ddN3T*ddN1T0
- -dN1T*dN3T0*ddN4T0*ddN2T+ddN2T0*dN3T*dN4T0*ddN1T-ddN4T*ddN2T0*dN1T0*dN3T
- +dN2T*ddN4T*ddN3T0*dN1T0-dN3T*ddN1T*ddN4T0*dN2T0-dN4T*ddN2T0*dN3T0*ddN1T
- -dN2T*dN1T0*ddN4T0*ddN3T+dN4T*ddN2T0*dN1T0*ddN3T+ddN3T0*dN4T*ddN1T*dN2T0
- -dN4T*ddN3T*dN2T0*ddN1T0+ddN3T0*dN1T*dN4T0*ddN2T-dN2T*ddN4T*dN3T0*ddN1T0
- -ddN3T0*dN4T*dN1T0*ddN2T+ddN4T*dN3T*dN2T0*ddN1T0-ddN4T*ddN3T0*dN1T*dN2T0)
+ -1.0/( dN2T*dN3T0*ddN1T*ddN4T0-dN3T*dN4T0*ddN2T*ddN1T0+dN1T*ddN4T0*ddN3T*
+ dN2T0
+ -dN2T*ddN3T0*dN4T0*ddN1T+ddN4T*dN1T*ddN2T0*dN3T0+dN1T0*dN3T*ddN4T0*
+ ddN2T
+ +dN4T*dN3T0*ddN2T*ddN1T0-dN1T*ddN2T0*dN4T0*ddN3T+dN2T*dN4T0*ddN3T*
+ ddN1T0
+ -dN1T*dN3T0*ddN4T0*ddN2T+ddN2T0*dN3T*dN4T0*ddN1T-ddN4T*ddN2T0*dN1T0*
+ dN3T
+ +dN2T*ddN4T*ddN3T0*dN1T0-dN3T*ddN1T*ddN4T0*dN2T0-dN4T*ddN2T0*dN3T0*
+ ddN1T
+ -dN2T*dN1T0*ddN4T0*ddN3T+dN4T*ddN2T0*dN1T0*ddN3T+ddN3T0*dN4T*ddN1T*
+ dN2T0
+ -dN4T*ddN3T*dN2T0*ddN1T0+ddN3T0*dN1T*dN4T0*ddN2T-dN2T*ddN4T*dN3T0*
+ ddN1T0
+ -ddN3T0*dN4T*dN1T0*ddN2T+ddN4T*dN3T*dN2T0*ddN1T0-ddN4T*ddN3T0*dN1T*
+ dN2T0)
*( dN2T*ddN0T0*ddN4T*dN1T0*IP+dN4T*IP*dN0T0*ddN2T*ddN1T0
-ddN5T*dN4T*FP*dN2T0*ddN1T0
- -IA*dN1T*dN4T0*ddN2T+ddN5T*dN2T*dN4T0*FP*ddN1T0-ddN4T*ddN2T0*dN1T0*FP*dN5T
+ -IA*dN1T*dN4T0*ddN2T+ddN5T*dN2T*dN4T0*FP*ddN1T0-ddN4T*ddN2T0*dN1T0*FP*
+ dN5T
-dN1T*ddN4T0*FA*dN2T0-ddN4T*dN1T*ddN2T0*IS+ddN4T*FP*dN5T*dN2T0*ddN1T0
- +dN2T*IP*ddN1T*ddN4T0*dN0T0-ddN5T*dN2T*dN1T0*ddN4T0*FP+FS*ddN1T*ddN4T0*dN2T0
+ +dN2T*IP*ddN1T*ddN4T0*dN0T0-ddN5T*dN2T*dN1T0*ddN4T0*FP+FS*ddN1T*ddN4T0*
+ dN2T0
+IA*dN4T*dN1T0*ddN2T+ddN5T*dN4T*ddN2T0*dN1T0*FP+FS*ddN4T*ddN2T0*dN1T0
- +ddN2T0*dN4T0*ddN1T*FP*dN5T-dN2T*dN4T0*FA*ddN1T0-ddN1T*ddN4T0*FP*dN5T*dN2T0
+ +ddN2T0*dN4T0*ddN1T*FP*dN5T-dN2T*dN4T0*FA*ddN1T0-ddN1T*ddN4T0*FP*dN5T*
+ dN2T0
-dN2T*ddN1T*ddN4T0*IS-FS*ddN2T0*dN4T0*ddN1T+ddN5T*dN1T*ddN4T0*FP*dN2T0
- +dN1T*ddN2T0*dN4T0*FA-ddN0T0*dN4T*dN1T0*IP*ddN2T+dN1T0*ddN4T0*FP*dN5T*ddN2T
- -IA*dN4T*ddN1T*dN2T0+ddN0T0*dN4T*IP*ddN1T*dN2T0-dN1T*IP*ddN4T0*dN0T0*ddN2T
- +IA*ddN4T*dN1T*dN2T0-dN4T*ddN2T0*IP*ddN1T*dN0T0-dN2T*ddN4T*IP*dN0T0*ddN1T0
+ +dN1T*ddN2T0*dN4T0*FA-ddN0T0*dN4T*dN1T0*IP*ddN2T+dN1T0*ddN4T0*FP*dN5T*
+ ddN2T
+ -IA*dN4T*ddN1T*dN2T0+ddN0T0*dN4T*IP*ddN1T*dN2T0-dN1T*IP*ddN4T0*dN0T0*
+ ddN2T
+ +IA*ddN4T*dN1T*dN2T0-dN4T*ddN2T0*IP*ddN1T*dN0T0-dN2T*ddN4T*IP*dN0T0*
+ ddN1T0
-dN2T*ddN0T0*IP*dN4T0*ddN1T+dN4T*ddN2T0*ddN1T*IS+FS*dN4T0*ddN2T*ddN1T0
-FS*dN1T0*ddN4T0*ddN2T+ddN4T*dN1T*ddN2T0*IP*dN0T0+IA*dN2T*dN4T0*ddN1T
-dN4T*ddN2T0*dN1T0*FA-dN4T0*FP*dN5T*ddN2T*ddN1T0-dN4T*ddN2T*ddN1T0*IS
-IA*dN2T*ddN4T*dN1T0+dN2T*dN1T0*ddN4T0*FA+dN1T*ddN4T0*ddN2T*IS
- +ddN0T0*dN1T*IP*dN4T0*ddN2T-ddN5T*dN1T*ddN2T0*dN4T0*FP+dN4T*FA*dN2T0*ddN1T0
+ +ddN0T0*dN1T*IP*dN4T0*ddN2T-ddN5T*dN1T*ddN2T0*dN4T0*FP+dN4T*FA*dN2T0*
+ ddN1T0
+dN2T*ddN4T*ddN1T0*IS-FS*ddN4T*dN2T0*ddN1T0-ddN0T0*ddN4T*dN1T*IP*dN2T0);
m_control_points[4]=
- -1.0/( dN2T*dN3T0*ddN1T*ddN4T0-dN3T*dN4T0*ddN2T*ddN1T0+dN1T*ddN4T0*ddN3T*dN2T0
- -dN2T*ddN3T0*dN4T0*ddN1T+ddN4T*dN1T*ddN2T0*dN3T0+dN1T0*dN3T*ddN4T0*ddN2T
- +dN4T*dN3T0*ddN2T*ddN1T0-dN1T*ddN2T0*dN4T0*ddN3T+dN2T*dN4T0*ddN3T*ddN1T0
- -dN1T*dN3T0*ddN4T0*ddN2T+ddN2T0*dN3T*dN4T0*ddN1T-ddN4T*ddN2T0*dN1T0*dN3T
- +dN2T*ddN4T*ddN3T0*dN1T0-dN3T*ddN1T*ddN4T0*dN2T0-dN4T*ddN2T0*dN3T0*ddN1T
- -dN2T*dN1T0*ddN4T0*ddN3T+dN4T*ddN2T0*dN1T0*ddN3T+ddN3T0*dN4T*ddN1T*dN2T0
- -dN4T*ddN3T*dN2T0*ddN1T0+ddN3T0*dN1T*dN4T0*ddN2T-dN2T*ddN4T*dN3T0*ddN1T0
- -ddN3T0*dN4T*dN1T0*ddN2T+ddN4T*dN3T*dN2T0*ddN1T0-ddN4T*ddN3T0*dN1T*dN2T0)
- *( ddN2T0*dN1T0*dN3T*FA+ddN5T*dN1T*ddN2T0*dN3T0*FP-ddN0T0*IP*dN3T*ddN1T*dN2T0
- -IA*dN1T*ddN3T*dN2T0-ddN0T0*dN1T*IP*dN3T0*ddN2T-dN2T*ddN0T0*dN1T0*IP*ddN3T
+ -1.0/( dN2T*dN3T0*ddN1T*ddN4T0-dN3T*dN4T0*ddN2T*ddN1T0+dN1T*ddN4T0*ddN3T*
+ dN2T0
+ -dN2T*ddN3T0*dN4T0*ddN1T+ddN4T*dN1T*ddN2T0*dN3T0+dN1T0*dN3T*ddN4T0*
+ ddN2T
+ +dN4T*dN3T0*ddN2T*ddN1T0-dN1T*ddN2T0*dN4T0*ddN3T+dN2T*dN4T0*ddN3T*
+ ddN1T0
+ -dN1T*dN3T0*ddN4T0*ddN2T+ddN2T0*dN3T*dN4T0*ddN1T-ddN4T*ddN2T0*dN1T0*
+ dN3T
+ +dN2T*ddN4T*ddN3T0*dN1T0-dN3T*ddN1T*ddN4T0*dN2T0-dN4T*ddN2T0*dN3T0*
+ ddN1T
+ -dN2T*dN1T0*ddN4T0*ddN3T+dN4T*ddN2T0*dN1T0*ddN3T+ddN3T0*dN4T*ddN1T*
+ dN2T0
+ -dN4T*ddN3T*dN2T0*ddN1T0+ddN3T0*dN1T*dN4T0*ddN2T-dN2T*ddN4T*dN3T0*
+ ddN1T0
+ -ddN3T0*dN4T*dN1T0*ddN2T+ddN4T*dN3T*dN2T0*ddN1T0-ddN4T*ddN3T0*dN1T*
+ dN2T0)
+ *( ddN2T0*dN1T0*dN3T*FA+ddN5T*dN1T*ddN2T0*dN3T0*FP-ddN0T0*IP*dN3T*ddN1T*
+ dN2T0
+ -IA*dN1T*ddN3T*dN2T0-ddN0T0*dN1T*IP*dN3T0*ddN2T-dN2T*ddN0T0*dN1T0*IP*
+ ddN3T
+dN3T*ddN2T*ddN1T0*IS-FS*dN3T0*ddN2T*ddN1T0+ddN2T0*dN1T0*FP*dN5T*ddN3T
- -ddN2T0*dN3T0*ddN1T*FP*dN5T+dN1T*ddN2T0*ddN3T*IS+ddN0T0*dN1T0*IP*dN3T*ddN2T
- +IA*dN3T*ddN1T*dN2T0+ddN5T*dN2T*ddN3T0*dN1T0*FP+dN3T0*FP*dN5T*ddN2T*ddN1T0
- +IA*dN2T*dN1T0*ddN3T+dN2T*ddN0T0*IP*dN3T0*ddN1T+dN2T*IP*ddN3T*dN0T0*ddN1T0
- -ddN3T0*dN1T*ddN2T*IS-FP*dN5T*ddN3T*dN2T0*ddN1T0-dN1T*ddN2T0*IP*ddN3T*dN0T0
+ -ddN2T0*dN3T0*ddN1T*FP*dN5T+dN1T*ddN2T0*ddN3T*IS+ddN0T0*dN1T0*IP*dN3T*
+ ddN2T
+ +IA*dN3T*ddN1T*dN2T0+ddN5T*dN2T*ddN3T0*dN1T0*FP+dN3T0*FP*dN5T*ddN2T*
+ ddN1T0
+ +IA*dN2T*dN1T0*ddN3T+dN2T*ddN0T0*IP*dN3T0*ddN1T+dN2T*IP*ddN3T*dN0T0*
+ ddN1T0
+ -ddN3T0*dN1T*ddN2T*IS-FP*dN5T*ddN3T*dN2T0*ddN1T0-dN1T*ddN2T0*IP*ddN3T*
+ dN0T0
-dN3T*FA*dN2T0*ddN1T0-IA*dN2T*dN3T0*ddN1T-dN1T*ddN2T0*dN3T0*FA
+ddN3T0*dN1T*IP*dN0T0*ddN2T
-FS*ddN3T0*ddN1T*dN2T0+FS*ddN2T0*dN3T0*ddN1T-dN2T*ddN3T0*IP*ddN1T*dN0T0
@@ -770,7 +720,8 @@ void BSplinesFoot::ComputeControlPointFrom3DataPoint(void)
+ddN0T0*ddN4T
*IP*dN3T0*N2Tm*dN5T-ddN5T*MidP1*dN4T*ddN2T0*dN3T0
+ddN3T0*dN4T0*dN6T*FP*N5Tm*ddN2T
- -ddN5T*IA*N3Tm*dN4T*dN2T0+ddN0T0*ddN4T*IP*dN3T*N5Tm*dN2T0+FS*ddN2T0*dN4T0*ddN3T
+ -ddN5T*IA*N3Tm*dN4T*dN2T0+ddN0T0*ddN4T*IP*dN3T*N5Tm*dN2T0+FS*ddN2T0*dN4T0
+ *ddN3T
*N5Tm-ddN5T*IP*dN3T*ddN4T0*N2Tm*dN0T0-ddN5T*N4Tm*ddN2T0*dN3T0*dN6T*FP
-dN4T*ddN2T0
*ddN3T*N5Tm*IS-N3Tm*ddN4T0*dN5T*FA*dN2T0+ddN5T*IA*dN4T*dN3T0*N2Tm
@@ -779,15 +730,19 @@ void BSplinesFoot::ComputeControlPointFrom3DataPoint(void)
+ddN2T0*ddN6T
*dN3T*dN4T0*FP*N5Tm+ddN5T*FS*N4Tm*ddN2T0*dN3T0-IA*dN4T*dN3T0*N5Tm*ddN2T
+ddN5T*ddN0T0
- *N3Tm*dN4T*IP*dN2T0-ddN5T*dN2T*MidP1*ddN3T0*dN4T0+IA*ddN4T*N3Tm*dN5T*dN2T0
+ *N3Tm*dN4T*IP*dN2T0-ddN5T*dN2T*MidP1*ddN3T0*dN4T0+IA*ddN4T*N3Tm*dN5T*
+ dN2T0
+IA*dN3T
- *dN4T0*N5Tm*ddN2T+ddN5T*MidP1*ddN3T0*dN4T*dN2T0-MidP1*ddN2T0*dN4T0*dN5T*ddN3T
+ *dN4T0*N5Tm*ddN2T+ddN5T*MidP1*ddN3T0*dN4T*dN2T0-MidP1*ddN2T0*dN4T0*dN5T*
+ ddN3T
-N3Tm
*ddN2T0*ddN6T*dN4T0*FP*dN5T+ddN0T0*N4Tm*IP*dN5T*ddN3T*dN2T0
+N4Tm*ddN2T0*ddN6T*dN3T0
*FP*dN5T-ddN5T*ddN3T0*dN4T*N2Tm*IS-dN2T*ddN3T0*ddN6T*dN4T0*FP*N5Tm
-ddN3T0*dN4T*IP
- *dN0T0*N5Tm*ddN2T-ddN5T*FS*N3Tm*ddN2T0*dN4T0+ddN5T*IA*N4Tm*dN3T*dN2T0-ddN5T*dN2T
+ *dN0T0*N5Tm*ddN2T-ddN5T*FS*N3Tm*ddN2T0*dN4T0+ddN5T*IA*N4Tm*dN3T*dN2T0-
+ ddN5T
+ *dN2T
*ddN3T0*N4Tm*IP*dN0T0-ddN3T0*dN4T*FA*N5Tm*dN2T0-dN3T*ddN4T0*N5Tm*ddN2T*IS
+N3Tm*ddN2T0
*dN4T0*dN5T*FA+ddN5T*IA*dN2T*N3Tm*dN4T0-FS*ddN3T0*dN4T0*N5Tm*ddN2T
@@ -798,9 +753,11 @@ void BSplinesFoot::ComputeControlPointFrom3DataPoint(void)
-ddN2T0*dN3T*dN4T0
*FA*N5Tm-ddN6T*dN3T0*ddN4T0*FP*N2Tm*dN5T-ddN5T*FS*dN3T0*ddN4T0*N2Tm
-ddN6T*dN3T*ddN4T0
- *FP*N5Tm*dN2T0-ddN0T0*IP*dN3T*dN4T0*N5Tm*ddN2T+ddN3T0*dN4T*ddN6T*FP*N5Tm*dN2T0
+ *FP*N5Tm*dN2T0-ddN0T0*IP*dN3T*dN4T0*N5Tm*ddN2T+ddN3T0*dN4T*ddN6T*FP*N5Tm
+ *dN2T0
-ddN5T
- *IA*dN2T*N4Tm*dN3T0-dN3T0*dN6T*ddN4T0*FP*N5Tm*ddN2T+ddN3T0*dN4T*N5Tm*ddN2T*IS
+ *IA*dN2T*N4Tm*dN3T0-dN3T0*dN6T*ddN4T0*FP*N5Tm*ddN2T+ddN3T0*dN4T*N5Tm*
+ ddN2T*IS
-ddN5T
*ddN0T0*dN4T*IP*dN3T0*N2Tm+ddN3T0*N4Tm*dN5T*FA*dN2T0
+IP*ddN4T0*N2Tm*dN5T*ddN3T*dN0T0
@@ -844,7 +801,8 @@ void BSplinesFoot::ComputeControlPointFrom3DataPoint(void)
-ddN0T0*N4Tm*IP*dN3T0*dN5T*ddN2T
+dN2T*ddN3T0*dN4T0*FA*N5Tm-ddN5T*ddN3T0*dN4T0*dN6T*FP*N2Tm
-ddN4T*ddN3T0*IP*N2Tm*dN5T*dN0T0
- +dN3T0*ddN4T0*N2Tm*dN5T*FA+ddN5T*FS*N3Tm*ddN4T0*dN2T0-IA*dN2T*dN4T0*ddN3T*N5Tm
+ +dN3T0*ddN4T0*N2Tm*dN5T*FA+ddN5T*FS*N3Tm*ddN4T0*dN2T0-IA*dN2T*dN4T0*
+ ddN3T*N5Tm
-ddN3T0*dN4T0
*N2Tm*dN5T*FA+ddN4T*ddN3T0*N2Tm*dN5T*IS-IA*N4Tm*dN5T*ddN3T*dN2T0
-dN2T*ddN4T*ddN3T0*N5Tm*IS
@@ -854,7 +812,8 @@ void BSplinesFoot::ComputeControlPointFrom3DataPoint(void)
-ddN5T*N1Tm*ddN3T0*dN4T*dN2T0+dN4T
*dN3T0*N5Tm*ddN2T*ddN1T0-dN3T*dN4T0*N5Tm*ddN2T*ddN1T0
+ddN5T*N1Tm*dN4T*ddN2T0*dN3T0-N1Tm*ddN4T0
- *dN5T*ddN3T*dN2T0-ddN4T*N3Tm*dN5T*dN2T0*ddN1T0+ddN5T*N4Tm*ddN2T0*dN1T0*dN3T
+ *dN5T*ddN3T*dN2T0-ddN4T*N3Tm*dN5T*dN2T0*ddN1T0+ddN5T*N4Tm*ddN2T0*dN1T0*
+ dN3T
-N1Tm*ddN3T0*dN4T0
*dN5T*ddN2T-ddN5T*N1Tm*ddN2T0*dN3T*dN4T0-N4Tm*dN3T0*dN5T*ddN2T*ddN1T0
+ddN5T*ddN3T0*dN4T*dN1T0
@@ -864,9 +823,11 @@ void BSplinesFoot::ComputeControlPointFrom3DataPoint(void)
+ddN5T*N3Tm*dN4T*dN2T0*ddN1T0+ddN5T
*dN2T*N3Tm*dN1T0*ddN4T0-N3Tm*dN1T0*ddN4T0*dN5T*ddN2T
-N4Tm*ddN2T0*dN1T0*dN5T*ddN3T+dN1T0*dN3T
- *ddN4T0*N5Tm*ddN2T+N1Tm*ddN2T0*dN4T0*dN5T*ddN3T+dN2T*dN4T0*ddN3T*N5Tm*ddN1T0
+ *ddN4T0*N5Tm*ddN2T+N1Tm*ddN2T0*dN4T0*dN5T*ddN3T+dN2T*dN4T0*ddN3T*N5Tm*
+ ddN1T0
-ddN4T*ddN3T0
- *dN1T0*N2Tm*dN5T-ddN5T*dN1T0*dN3T*ddN4T0*N2Tm+N4Tm*dN5T*ddN3T*dN2T0*ddN1T0
+ *dN1T0*N2Tm*dN5T-ddN5T*dN1T0*dN3T*ddN4T0*N2Tm+N4Tm*dN5T*ddN3T*dN2T0*
+ ddN1T0
+dN2T*ddN4T*ddN3T0
*dN1T0*N5Tm-ddN5T*dN2T*ddN3T0*N4Tm*dN1T0-ddN5T*dN2T*N1Tm*dN3T0*ddN4T0
+ddN5T*dN2T*N1Tm*ddN3T0
@@ -874,9 +835,11 @@ void BSplinesFoot::ComputeControlPointFrom3DataPoint(void)
-dN2T*dN1T0*ddN4T0*ddN3T*N5Tm
+ddN4T*dN3T0*N2Tm*dN5T*ddN1T0-dN4T0*N2Tm*dN5T*ddN3T*ddN1T0
+dN4T*ddN2T0*dN1T0*ddN3T*N5Tm-ddN3T0
- *dN4T*dN1T0*N5Tm*ddN2T-ddN5T*N4Tm*dN3T*dN2T0*ddN1T0+ddN4T*dN3T*N5Tm*dN2T0*ddN1T0
+ *dN4T*dN1T0*N5Tm*ddN2T-ddN5T*N4Tm*dN3T*dN2T0*ddN1T0+ddN4T*dN3T*N5Tm*dN2T0
+ *ddN1T0
+dN1T0*ddN4T0
- *N2Tm*dN5T*ddN3T-N1Tm*ddN4T*ddN2T0*dN3T0*dN5T+ddN5T*dN2T*N4Tm*dN3T0*ddN1T0
+ *N2Tm*dN5T*ddN3T-N1Tm*ddN4T*ddN2T0*dN3T0*dN5T+ddN5T*dN2T*N4Tm*dN3T0*
+ ddN1T0
+ddN3T0*N4Tm*dN1T0
*dN5T*ddN2T-ddN5T*N3Tm*dN4T*ddN2T0*dN1T0-dN2T*ddN4T*dN3T0*N5Tm*ddN1T0
+N3Tm*dN4T0*dN5T*ddN2T*ddN1T0);
@@ -907,8 +870,10 @@ void BSplinesFoot::ComputeControlPointFrom3DataPoint(void)
-N4Tm*IP*dN5T*ddN3T*dN0T0*ddN1T0
+IA*dN4T*dN1T0*ddN3T*N5Tm-FS*ddN4T*dN3T0*N5Tm*ddN1T0
-dN4T0*dN6T*FP*ddN3T*N5Tm*ddN1T0
- +N1Tm*ddN4T*ddN3T0*dN5T*IS-FS*dN1T0*ddN4T0*ddN3T*N5Tm+ddN5T*N1Tm*dN3T*ddN4T0*IS
- -ddN5T*IA*N3Tm*dN4T*dN1T0-ddN4T*N3Tm*dN5T*ddN1T0*IS+N3Tm*dN4T0*dN5T*FA*ddN1T0
+ +N1Tm*ddN4T*ddN3T0*dN5T*IS-FS*dN1T0*ddN4T0*ddN3T*N5Tm+ddN5T*N1Tm*dN3T*
+ ddN4T0*IS
+ -ddN5T*IA*N3Tm*dN4T*dN1T0-ddN4T*N3Tm*dN5T*ddN1T0*IS+N3Tm*dN4T0*dN5T*FA*
+ ddN1T0
+dN1T0*dN3T*ddN4T0*FA*N5Tm+ddN4T*N3Tm*IP*dN5T*dN0T0*ddN1T0
+ddN5T*IA*N1Tm*dN4T*dN3T0
+ddN4T*dN3T*N5Tm*ddN1T0*IS-ddN5T*N4Tm*dN3T*ddN1T0*IS
@@ -970,7 +935,8 @@ void BSplinesFoot::ComputeControlPointFrom3DataPoint(void)
-ddN5T*N3Tm*dN4T*ddN2T0*dN1T0-dN2T*ddN4T*dN3T0*N5Tm*ddN1T0
+N3Tm*dN4T0*dN5T*ddN2T*ddN1T0);
m_control_points[3]=
- -( FS*ddN4T*ddN2T0*dN1T0*N5Tm+N1Tm*ddN4T0*dN5T*FA*dN2T0+ddN5T*IA*dN2T*N4Tm*dN1T0
+ -( FS*ddN4T*ddN2T0*dN1T0*N5Tm+N1Tm*ddN4T0*dN5T*FA*dN2T0+ddN5T*IA*dN2T*N4Tm*
+ dN1T0
+ddN5T*dN2T*N4Tm*IP*dN0T0*ddN1T0+dN1T0*ddN6T*ddN4T0*FP*N2Tm*dN5T
+MidP1*dN1T0*ddN4T0*dN5T*ddN2T
+dN1T0*dN6T*ddN4T0*FP*N5Tm*ddN2T+ddN5T*dN2T*MidP1*dN4T0*ddN1T0
@@ -983,7 +949,9 @@ void BSplinesFoot::ComputeControlPointFrom3DataPoint(void)
+ddN5T*dN4T0*dN6T*FP*N2Tm*ddN1T0
+ddN5T*IA*N1Tm*dN4T*dN2T0+ddN5T*N4Tm*ddN2T0*dN1T0*dN6T*FP
-ddN5T*N1Tm*ddN2T0*dN4T0*dN6T*FP
- +dN2T*ddN4T*N5Tm*ddN1T0*IS-FS*dN1T0*ddN4T0*N5Tm*ddN2T+FS*dN4T0*N5Tm*ddN2T*ddN1T0
+ +dN2T*ddN4T*N5Tm*ddN1T0*IS-FS*dN1T0*ddN4T0*N5Tm*ddN2T+FS*dN4T0*N5Tm*
+ ddN2T*
+ ddN1T0
-N1Tm*ddN2T0*dN4T0*dN5T*FA-N4Tm*ddN2T0*dN1T0*ddN6T*FP*dN5T
+ddN4T*dN6T*FP*N5Tm*dN2T0*ddN1T0
-ddN5T*dN2T*ddN0T0*N4Tm*dN1T0*IP+ddN0T0*N4Tm*dN1T0*IP*dN5T*ddN2T
@@ -996,7 +964,8 @@ void BSplinesFoot::ComputeControlPointFrom3DataPoint(void)
+ddN4T*MidP1*dN5T*dN2T0*ddN1T0
-N1Tm*ddN4T*ddN2T0*IP*dN5T*dN0T0+IA*ddN4T*dN1T0*N2Tm*dN5T
-N4Tm*IP*dN5T*dN0T0*ddN2T*ddN1T0
- -ddN5T*N1Tm*FS*ddN4T0*dN2T0+N4Tm*dN5T*ddN2T*ddN1T0*IS-ddN5T*IA*dN4T*dN1T0*N2Tm
+ -ddN5T*N1Tm*FS*ddN4T0*dN2T0+N4Tm*dN5T*ddN2T*ddN1T0*IS-ddN5T*IA*dN4T*dN1T0
+ *N2Tm
+dN2T*ddN0T0*ddN4T*dN1T0*IP*N5Tm-MidP1*dN4T0*dN5T*ddN2T*ddN1T0
-ddN0T0*N1Tm*IP*dN4T0*dN5T*ddN2T
+ddN5T*N1Tm*dN4T*ddN2T0*IP*dN0T0-N1Tm*ddN4T0*dN5T*ddN2T*IS
@@ -1085,7 +1054,8 @@ void BSplinesFoot::ComputeControlPointFrom3DataPoint(void)
+IP*N2Tm*dN5T*ddN3T*dN0T0*ddN1T0
-ddN0T0*dN1T0*IP*dN3T*N5Tm*ddN2T+ddN5T*FS*ddN3T0*dN1T0*N2Tm
+ddN5T*N1Tm*ddN2T0*IP*dN3T*dN0T0
- +IA*N1Tm*dN3T0*dN5T*ddN2T-ddN2T0*dN1T0*dN3T*FA*N5Tm-ddN5T*FS*N3Tm*ddN2T0*dN1T0
+ +IA*N1Tm*dN3T0*dN5T*ddN2T-ddN2T0*dN1T0*dN3T*FA*N5Tm-ddN5T*FS*N3Tm*ddN2T0
+ *dN1T0
-N1Tm*ddN3T0*dN5T*ddN2T*IS+dN6T*FP*ddN3T*N5Tm*dN2T0*ddN1T0
+ddN5T*N3Tm*ddN2T0*dN1T0*dN6T*FP
-ddN5T*MidP1*dN3T*dN2T0*ddN1T0-ddN5T*dN2T*MidP1*ddN3T0*dN1T0
@@ -1094,7 +1064,8 @@ void BSplinesFoot::ComputeControlPointFrom3DataPoint(void)
+IP*dN3T*dN0T0*N5Tm*ddN2T*ddN1T0
-dN2T*dN3T0*FA*N5Tm*ddN1T0-dN3T*N5Tm*ddN2T*ddN1T0*IS
+N3Tm*ddN6T*FP*dN5T*dN2T0*ddN1T0
- +IA*dN1T0*N2Tm*dN5T*ddN3T+dN2T*ddN3T*N5Tm*ddN1T0*IS-IA*N3Tm*dN1T0*dN5T*ddN2T
+ +IA*dN1T0*N2Tm*dN5T*ddN3T+dN2T*ddN3T*N5Tm*ddN1T0*IS-IA*N3Tm*dN1T0*dN5T
+ *ddN2T
+dN2T*ddN0T0*dN1T0*IP*ddN3T*N5Tm+ddN5T*dN3T0*dN6T*FP*N2Tm*ddN1T0
+N3Tm*ddN2T0*dN1T0*dN5T*FA
-ddN5T*N1Tm*ddN2T0*dN3T*IS+ddN0T0*N3Tm*dN1T0*IP*dN5T*ddN2T
@@ -1163,13 +1134,15 @@ void BSplinesFoot::ComputeControlPointFrom3DataPoint(void)
-FS*ddN4T*N3Tm*ddN2T0
*dN1T0+FS*N4Tm*ddN2T0*dN1T0*ddN3T+N4Tm*dN3T*FA*dN2T0*ddN1T0
-FS*N4Tm*ddN3T*dN2T0*ddN1T0
- -dN2T*N3Tm*dN1T0*ddN4T0*FA+N3Tm*dN4T*ddN2T0*dN1T0*FA+N3Tm*dN4T*ddN2T*ddN1T0*IS
+ -dN2T*N3Tm*dN1T0*ddN4T0*FA+N3Tm*dN4T*ddN2T0*dN1T0*FA+N3Tm*dN4T*ddN2T*
+ ddN1T0*IS
+dN2T
*ddN0T0*N1Tm*ddN4T*IP*dN3T0-N1Tm*ddN4T*ddN2T0*dN3T0*dN6T*FP
+dN4T*IP*N2Tm*ddN3T*dN0T0
*ddN1T0+N1Tm*dN4T*ddN2T0*ddN6T*dN3T0*FP+N1Tm*ddN4T*ddN3T0*dN6T*FP*dN2T0
-dN2T*N1Tm
- *ddN4T*ddN3T0*IP*dN0T0+N4Tm*IP*dN3T*dN0T0*ddN2T*ddN1T0-N4Tm*dN3T*ddN2T*ddN1T0*IS
+ *ddN4T*ddN3T0*IP*dN0T0+N4Tm*IP*dN3T*dN0T0*ddN2T*ddN1T0-N4Tm*dN3T*ddN2T*
+ ddN1T0*IS
-MidP1
*dN4T*ddN2T0*dN1T0*ddN3T-N1Tm*FS*dN3T0*ddN4T0*ddN2T
-dN1T0*ddN6T*dN3T*ddN4T0*FP*N2Tm
@@ -1187,7 +1160,8 @@ void BSplinesFoot::ComputeControlPointFrom3DataPoint(void)
-dN2T*ddN0T0*ddN4T
*N3Tm*dN1T0*IP-ddN3T0*dN4T*dN1T0*N2Tm*FA-N4Tm*dN3T0*dN6T*FP*ddN2T*ddN1T0
-N1Tm*dN6T
- *ddN4T0*FP*ddN3T*dN2T0+IA*N1Tm*dN4T*dN3T0*ddN2T+N1Tm*dN3T0*dN6T*ddN4T0*FP*ddN2T
+ *ddN4T0*FP*ddN3T*dN2T0+IA*N1Tm*dN4T*dN3T0*ddN2T+N1Tm*dN3T0*dN6T*ddN4T0*
+ FP*ddN2T
-N1Tm
*FS*ddN2T0*dN4T0*ddN3T+dN2T*N1Tm*ddN3T0*ddN6T*dN4T0*FP
+ddN4T*dN3T0*dN6T*FP*N2Tm*ddN1T0
@@ -1195,9 +1169,11 @@ void BSplinesFoot::ComputeControlPointFrom3DataPoint(void)
+ddN4T*MidP1*ddN2T0*dN1T0*dN3T-N1Tm
*ddN3T0*dN4T*ddN6T*FP*dN2T0-dN4T0*dN6T*FP*N2Tm*ddN3T*ddN1T0
+IA*N4Tm*dN1T0*dN3T*ddN2T
- -dN4T*N2Tm*ddN3T*ddN1T0*IS-N1Tm*dN3T*ddN4T0*FA*dN2T0-dN2T*N4Tm*dN3T0*FA*ddN1T0
+ -dN4T*N2Tm*ddN3T*ddN1T0*IS-N1Tm*dN3T*ddN4T0*FA*dN2T0-dN2T*N4Tm*dN3T0*FA*
+ ddN1T0
-dN2T*N3Tm
- *ddN6T*dN4T0*FP*ddN1T0+IA*N1Tm*ddN4T*dN3T*dN2T0-ddN0T0*N1Tm*ddN4T*IP*dN3T*dN2T0
+ *ddN6T*dN4T0*FP*ddN1T0+IA*N1Tm*ddN4T*dN3T*dN2T0-ddN0T0*N1Tm*ddN4T*IP*dN3T
+ *dN2T0
+dN2T
*ddN4T*MidP1*dN3T0*ddN1T0-dN2T*N4Tm*IP*ddN3T*dN0T0*ddN1T0
+dN2T*N1Tm*IP*ddN4T0*ddN3T*dN0T0
@@ -1337,97 +1313,116 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
m_control_points[0]=IP;
m_control_points[1]=
- -1.0/( N3Tm1*ddN6T*dN5T*ddN1T0*N2Tm2*dN4T0+N4Tm1*N5Tm2*dN6T*dN2T0*ddN3T*ddN1T0
+ -1.0/( N3Tm1*ddN6T*dN5T*ddN1T0*N2Tm2*dN4T0+N4Tm1*N5Tm2*dN6T*dN2T0*ddN3T*
+ ddN1T0
-ddN2T0
- *ddN6T*N5Tm2*dN3T*N1Tm1*dN4T0+dN3T0*ddN2T0*N6Tm2*dN5T*N1Tm1*ddN4T-ddN2T0*N4Tm1
- *N6Tm2*dN3T*dN1T0*ddN5T-N2Tm1*N5Tm2*dN6T*ddN3T*ddN1T0*dN4T0-ddN3T0*dN6T*dN2T0
- *N1Tm1*ddN5T*N4Tm2-ddN2T0*N4Tm1*ddN6T*dN5T*dN1T0*N3Tm2-N4Tm1*ddN6T*N5Tm2*dN3T
+ *ddN6T*N5Tm2*dN3T*N1Tm1*dN4T0+dN3T0*ddN2T0*N6Tm2*dN5T*N1Tm1*ddN4T-
+ ddN2T0*N4Tm1
+ *N6Tm2*dN3T*dN1T0*ddN5T-N2Tm1*N5Tm2*dN6T*ddN3T*ddN1T0*dN4T0-ddN3T0*
+ dN6T*dN2T0
+ *N1Tm1*ddN5T*N4Tm2-ddN2T0*N4Tm1*ddN6T*dN5T*dN1T0*N3Tm2-N4Tm1*ddN6T*
+ N5Tm2*dN3T
*dN2T0*ddN1T0-dN3T0*ddN2T0*ddN6T*dN5T*N1Tm1*N4Tm2
-ddN3T0*dN4T*N6Tm2*N2Tm1*dN1T0*ddN5T
- -dN3T0*dN4T*N2Tm1*ddN6T*N5Tm2*ddN1T0+ddN3T0*N6Tm2*N2Tm1*dN5T*dN1T0*ddN4T
+ -dN3T0*dN4T*N2Tm1*ddN6T*N5Tm2*ddN1T0+ddN3T0*N6Tm2*N2Tm1*dN5T*dN1T0*
+ ddN4T
+N3Tm1*ddN2T0
*N5Tm2*dN6T*dN1T0*ddN4T-N5Tm1*dN4T*ddN2T0*N6Tm2*dN1T0*ddN3T
-dN3T0*dN6T*ddN4T0*N1Tm1
*ddN5T*N2Tm2+ddN2T0*N6Tm2*dN3T*N1Tm1*ddN5T*dN4T0
+N6Tm2*ddN4T0*dN2T0*dN5T*ddN3T*N1Tm1
- +N5Tm1*ddN6T*dN3T*dN2T0*ddN1T0*N4Tm2+dN6T*ddN4T0*dN2T0*N1Tm1*N3Tm2*ddN5T
+ +N5Tm1*ddN6T*dN3T*dN2T0*ddN1T0*N4Tm2+dN6T*ddN4T0*dN2T0*N1Tm1*N3Tm2*
+ ddN5T
-N3Tm1*dN4T
*ddN2T0*ddN6T*N5Tm2*dN1T0+N3Tm1*dN6T*dN2T0*ddN1T0*ddN5T*N4Tm2
-N3Tm1*ddN2T0*N6Tm2*dN5T
*dN1T0*ddN4T+N2Tm1*N5Tm2*dN6T*ddN4T0*dN1T0*ddN3T
+N5Tm1*ddN2T0*N6Tm2*dN3T*dN1T0*ddN4T
- +N5Tm1*dN6T*ddN3T*ddN1T0*N2Tm2*dN4T0+dN3T0*dN4T*N6Tm2*N2Tm1*ddN1T0*ddN5T
+ +N5Tm1*dN6T*ddN3T*ddN1T0*N2Tm2*dN4T0+dN3T0*dN4T*N6Tm2*N2Tm1*ddN1T0*
+ ddN5T
-dN3T0*N6Tm2
*N2Tm1*dN5T*ddN1T0*ddN4T-ddN3T0*N6Tm2*dN2T0*dN5T*N1Tm1*ddN4T
+ddN6T*N5Tm2*dN3T*ddN4T0
*dN2T0*N1Tm1+N5Tm1*dN6T*dN2T0*ddN1T0*N3Tm2*ddN4T
+ddN2T0*ddN6T*dN5T*N1Tm1*N3Tm2*dN4T0
- -ddN2T0*N6Tm2*dN5T*ddN3T*N1Tm1*dN4T0+dN3T0*ddN2T0*dN6T*N1Tm1*ddN5T*N4Tm2
+ -ddN2T0*N6Tm2*dN5T*ddN3T*N1Tm1*dN4T0+dN3T0*ddN2T0*dN6T*N1Tm1*ddN5T*
+ N4Tm2
+ddN3T0*N2Tm1
*dN6T*dN1T0*ddN5T*N4Tm2+N3Tm1*N6Tm2*dN2T0*dN5T*ddN1T0*ddN4T
-ddN3T0*N4Tm1*dN6T*dN1T0
*ddN5T*N2Tm2+N4Tm1*N6Tm2*dN3T*dN2T0*ddN1T0*ddN5T
+N4Tm1*ddN6T*dN2T0*dN5T*ddN1T0*N3Tm2
- +N3Tm1*ddN2T0*ddN6T*dN5T*dN1T0*N4Tm2-ddN3T0*ddN6T*dN5T*N1Tm1*N2Tm2*dN4T0
+ +N3Tm1*ddN2T0*ddN6T*dN5T*dN1T0*N4Tm2-ddN3T0*ddN6T*dN5T*N1Tm1*N2Tm2*
+ dN4T0
-N3Tm1*dN6T
*ddN1T0*ddN5T*N2Tm2*dN4T0+N5Tm1*ddN6T*dN3T*ddN4T0*dN1T0*N2Tm2
-dN3T0*ddN2T0*N5Tm2*dN6T
*N1Tm1*ddN4T-N5Tm1*ddN2T0*ddN6T*dN3T*dN1T0*N4Tm2
+ddN3T0*N5Tm1*dN6T*dN1T0*N2Tm2*ddN4T
- -ddN6T*ddN4T0*dN2T0*dN5T*N1Tm1*N3Tm2+dN3T0*N2Tm1*N5Tm2*dN6T*ddN1T0*ddN4T
+ -ddN6T*ddN4T0*dN2T0*dN5T*N1Tm1*N3Tm2+dN3T0*N2Tm1*N5Tm2*dN6T*ddN1T0*
+ ddN4T
+ddN3T0*dN4T
*N2Tm1*ddN6T*N5Tm2*dN1T0+N2Tm1*ddN6T*N5Tm2*dN3T*ddN1T0*dN4T0
-N5Tm1*ddN6T*dN3T*ddN1T0
*N2Tm2*dN4T0-N3Tm1*N5Tm2*dN6T*dN2T0*ddN1T0*ddN4T
+ddN3T0*N4Tm1*ddN6T*dN5T*dN1T0*N2Tm2
- -N3Tm1*ddN2T0*dN6T*dN1T0*ddN5T*N4Tm2-N2Tm1*dN6T*ddN4T0*dN1T0*N3Tm2*ddN5T
+ -N3Tm1*ddN2T0*dN6T*dN1T0*ddN5T*N4Tm2-N2Tm1*dN6T*ddN4T0*dN1T0*N3Tm2*
+ ddN5T
-N5Tm2*dN6T
*ddN4T0*dN2T0*ddN3T*N1Tm1+ddN3T0*N5Tm2*dN6T*dN2T0*N1Tm1*ddN4T
-N5Tm1*dN6T*dN2T0*ddN3T
*ddN1T0*N4Tm2+N3Tm1*dN4T*ddN6T*N5Tm2*dN2T0*ddN1T0
-ddN3T0*N2Tm1*ddN6T*dN5T*dN1T0*N4Tm2
- +dN3T0*ddN6T*ddN4T0*dN5T*N1Tm1*N2Tm2-N2Tm1*ddN6T*dN5T*ddN1T0*N3Tm2*dN4T0
+ +dN3T0*ddN6T*ddN4T0*dN5T*N1Tm1*N2Tm2-N2Tm1*ddN6T*dN5T*ddN1T0*N3Tm2*
+ dN4T0
+dN3T0*N5Tm1
*dN4T*ddN6T*ddN1T0*N2Tm2-N6Tm2*N2Tm1*dN3T*ddN1T0*ddN5T*dN4T0
+N3Tm1*dN6T*ddN4T0*dN1T0
*ddN5T*N2Tm2-N6Tm2*dN3T*ddN4T0*dN2T0*N1Tm1*ddN5T
-ddN2T0*N4Tm1*N5Tm2*dN6T*dN1T0*ddN3T
- +N3Tm1*dN4T*ddN2T0*N6Tm2*dN1T0*ddN5T-N6Tm2*N2Tm1*ddN4T0*dN5T*dN1T0*ddN3T
+ +N3Tm1*dN4T*ddN2T0*N6Tm2*dN1T0*ddN5T-N6Tm2*N2Tm1*ddN4T0*dN5T*dN1T0*
+ ddN3T
-N5Tm1*ddN2T0
*dN6T*dN1T0*N3Tm2*ddN4T-ddN3T0*dN4T*ddN6T*N5Tm2*dN2T0*N1Tm1
+ddN2T0*N5Tm2*dN6T*ddN3T
*N1Tm1*dN4T0+ddN2T0*N4Tm1*N6Tm2*dN5T*dN1T0*ddN3T
+dN3T0*dN4T*ddN2T0*ddN6T*N5Tm2*N1Tm1
- -N5Tm1*dN4T*ddN6T*dN2T0*ddN1T0*N3Tm2-ddN3T0*N5Tm1*dN4T*ddN6T*dN1T0*N2Tm2
+ -N5Tm1*dN4T*ddN6T*dN2T0*ddN1T0*N3Tm2-ddN3T0*N5Tm1*dN4T*ddN6T*dN1T0*
+ N2Tm2
+ddN3T0*dN4T
*N6Tm2*dN2T0*N1Tm1*ddN5T-N4Tm1*dN6T*dN2T0*ddN1T0*N3Tm2*ddN5T
-N2Tm1*ddN6T*N5Tm2*dN3T
*ddN4T0*dN1T0+ddN2T0*N4Tm1*dN6T*dN1T0*N3Tm2*ddN5T
+dN3T0*N2Tm1*ddN6T*dN5T*ddN1T0*N4Tm2
- +N5Tm1*ddN2T0*dN6T*dN1T0*ddN3T*N4Tm2+ddN3T0*dN6T*N1Tm1*ddN5T*N2Tm2*dN4T0
+ +N5Tm1*ddN2T0*dN6T*dN1T0*ddN3T*N4Tm2+ddN3T0*dN6T*N1Tm1*ddN5T*N2Tm2*
+ dN4T0
-dN3T0*N4Tm1
*ddN6T*dN5T*ddN1T0*N2Tm2-ddN2T0*dN6T*N1Tm1*N3Tm2*ddN5T*dN4T0
-N5Tm1*N6Tm2*dN3T*dN2T0
*ddN1T0*ddN4T-N3Tm1*ddN6T*ddN4T0*dN5T*dN1T0*N2Tm2
-ddN3T0*N2Tm1*N5Tm2*dN6T*dN1T0*ddN4T
- +ddN2T0*N4Tm1*ddN6T*N5Tm2*dN3T*dN1T0-N3Tm1*dN4T*N6Tm2*dN2T0*ddN1T0*ddN5T
+ +ddN2T0*N4Tm1*ddN6T*N5Tm2*dN3T*dN1T0-N3Tm1*dN4T*N6Tm2*dN2T0*ddN1T0*
+ ddN5T
-dN3T0*N5Tm1
*dN6T*ddN1T0*N2Tm2*ddN4T-N5Tm1*dN6T*ddN4T0*dN1T0*ddN3T*N2Tm2
+N6Tm2*N2Tm1*dN5T*ddN3T
*ddN1T0*dN4T0-N3Tm1*ddN6T*dN2T0*dN5T*ddN1T0*N4Tm2
+N2Tm1*ddN6T*ddN4T0*dN5T*dN1T0*N3Tm2
- +N5Tm1*dN4T*ddN2T0*ddN6T*dN1T0*N3Tm2+N2Tm1*dN6T*ddN1T0*N3Tm2*ddN5T*dN4T0
+ +N5Tm1*dN4T*ddN2T0*ddN6T*dN1T0*N3Tm2+N2Tm1*dN6T*ddN1T0*N3Tm2*ddN5T*
+ dN4T0
+dN3T0*N4Tm1
*dN6T*ddN1T0*ddN5T*N2Tm2-dN3T0*dN4T*ddN2T0*N6Tm2*N1Tm1*ddN5T
+ddN3T0*ddN6T*dN2T0*dN5T
*N1Tm1*N4Tm2+N5Tm1*dN4T*N6Tm2*dN2T0*ddN3T*ddN1T0
-dN3T0*N2Tm1*dN6T*ddN1T0*ddN5T*N4Tm2
- -N4Tm1*N6Tm2*dN2T0*dN5T*ddN3T*ddN1T0+N6Tm2*N2Tm1*dN3T*ddN4T0*dN1T0*ddN5T)
+ -N4Tm1*N6Tm2*dN2T0*dN5T*ddN3T*ddN1T0+N6Tm2*N2Tm1*dN3T*ddN4T0*dN1T0*
+ ddN5T)
*( N3Tm1*IA*dN6T*ddN5T*N2Tm2*dN4T0+ddN3T0*N2Tm1*dN6T*IP*dN0T0*ddN5T*N4Tm2
-ddN3T0*N2Tm1
*N5Tm2*dN6T*FA*dN4T0+N2Tm1*N5Tm2*dN6T*ddN4T0*IP*ddN3T*dN0T0
-N3Tm1*IS*dN6T*ddN4T0*ddN5T
- *N2Tm2+N5Tm1*ddN2T0*N6Tm2*ddN3T*dN7T*FP*dN4T0+N6Tm2*N2Tm1*IS*ddN4T0*dN5T*ddN3T
+ *N2Tm2+N5Tm1*ddN2T0*N6Tm2*ddN3T*dN7T*FP*dN4T0+N6Tm2*N2Tm1*IS*ddN4T0*dN5T
+ *ddN3T
+N3Tm1
*dN6T*ddN4T0*IP*dN0T0*ddN5T*N2Tm2-ddN2T0*N5Tm2*dN6T*ddN3T*MidP1*dN4T0
-N4Tm1*N6Tm2*IA
@@ -1455,7 +1450,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+ddN3T0*N2Tm1*dN6T*ddN5T
*MidP2*dN4T0-N3Tm1*ddN6T*N5Tm2*ddN4T0*dN2T0*dN7T*FP
+N5Tm1*dN6T*IP*dN2T0*N3Tm2*ddN0T0
- *ddN4T+ddN2T0*N4Tm1*ddN6T*N5Tm2*dN3T*IP*dN0T0-ddN3T0*N4Tm1*ddN6T*IS*dN5T*N2Tm2
+ *ddN4T+ddN2T0*N4Tm1*ddN6T*N5Tm2*dN3T*IP*dN0T0-ddN3T0*N4Tm1*ddN6T*IS*dN5T
+ *N2Tm2
-N3Tm1
*dN4T*ddN6T*N5Tm2*IA*dN2T0+ddN3T0*N4Tm1*N6Tm2*dN2T0*FS*ddN5T
+dN3T0*dN4T*N2Tm1*ddN6T
@@ -1475,7 +1471,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-ddN3T0*N2Tm1*ddN6T*N5Tm2
*dN7T*FP*dN4T0+N3Tm1*N6Tm2*IP*dN2T0*dN5T*ddN0T0*ddN4T
-N5Tm1*dN4T*ddN2T0*N6Tm2*IP*ddN3T
- *dN0T0-N3Tm1*N5Tm2*dN6T*IP*dN2T0*ddN0T0*ddN4T-N3Tm1*ddN2T0*N5Tm2*IS*dN6T*ddN4T
+ *dN0T0-N3Tm1*N5Tm2*dN6T*IP*dN2T0*ddN0T0*ddN4T-N3Tm1*ddN2T0*N5Tm2*IS*dN6T
+ *ddN4T
-N3Tm1*N5Tm2
*dN6T*ddN4T0*dN2T0*FA-dN3T0*N5Tm1*dN4T*ddN2T0*N6Tm2*FA
+ddN3T0*N5Tm1*dN4T*N6Tm2*dN2T0*FA
@@ -1499,7 +1496,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-dN3T0*N4Tm1*IA*dN6T*ddN5T*N2Tm2-ddN3T0
*N5Tm1*ddN6T*dN2T0*dN7T*FP*N4Tm2-N5Tm1*dN4T*N6Tm2*IA*dN2T0*ddN3T
-dN3T0*dN4T*N6Tm2*N2Tm1
- *IA*ddN5T-N2Tm1*ddN6T*IS*ddN4T0*dN5T*N3Tm2-dN3T0*N2Tm1*N5Tm2*IA*dN6T*ddN4T
+ *IA*ddN5T-N2Tm1*ddN6T*IS*ddN4T0*dN5T*N3Tm2-dN3T0*N2Tm1*N5Tm2*IA*dN6T*
+ ddN4T
+N5Tm1*IA*dN6T
*dN2T0*ddN3T*N4Tm2+ddN3T0*ddN7T*N5Tm1*dN6T*dN2T0*FP*N4Tm2
+dN3T0*N5Tm1*IA*dN6T*N2Tm2*ddN4T
@@ -1507,7 +1505,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-ddN3T0*N2Tm1*IS
*dN6T*ddN5T*N4Tm2+dN3T0*N2Tm1*ddN6T*N5Tm2*ddN4T0*dN7T*FP
-ddN3T0*N2Tm1*ddN6T*IP*dN5T*dN0T0
- *N4Tm2-N5Tm1*ddN2T0*dN6T*IP*dN0T0*N3Tm2*ddN4T-ddN3T0*N5Tm1*ddN6T*FS*N2Tm2*dN4T0
+ *N4Tm2-N5Tm1*ddN2T0*dN6T*IP*dN0T0*N3Tm2*ddN4T-ddN3T0*N5Tm1*ddN6T*FS*N2Tm2
+ *dN4T0
-N5Tm1*N6Tm2
*dN3T*ddN4T0*dN2T0*FA-ddN2T0*N4Tm1*N6Tm2*IS*dN5T*ddN3T
-N2Tm1*N5Tm2*dN6T*IP*ddN3T*ddN0T0
@@ -1515,17 +1514,21 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+N6Tm2*N2Tm1*IP*dN5T*ddN3T*ddN0T0*dN4T0+N3Tm1
*ddN6T*IS*ddN4T0*dN5T*N2Tm2-ddN3T0*N4Tm1*N6Tm2*dN2T0*dN7T*FP*ddN5T
-ddN2T0*ddN6T*dN5T*N3Tm2
- *MidP1*dN4T0-ddN3T0*N4Tm1*ddN6T*N5Tm2*dN2T0*FS+ddN3T0*N2Tm1*ddN6T*N5Tm2*FS*dN4T0
+ *MidP1*dN4T0-ddN3T0*N4Tm1*ddN6T*N5Tm2*dN2T0*FS+ddN3T0*N2Tm1*ddN6T*N5Tm2
+ *FS
+ *dN4T0
-N5Tm1
*ddN6T*ddN4T0*dN2T0*FS*N3Tm2-N3Tm1*ddN6T*ddN4T0*IP*dN5T*dN0T0*N2Tm2
-N5Tm1*ddN2T0*N6Tm2*dN3T
- *IS*ddN4T+N5Tm1*dN4T*ddN2T0*N6Tm2*IS*ddN3T-ddN3T0*N5Tm1*IS*dN6T*N2Tm2*ddN4T
+ *IS*ddN4T+N5Tm1*dN4T*ddN2T0*N6Tm2*IS*ddN3T-ddN3T0*N5Tm1*IS*dN6T*N2Tm2*
+ ddN4T
+ddN2T0*N4Tm1
*dN6T*IP*dN0T0*N3Tm2*ddN5T-N3Tm1*dN6T*IP*ddN5T*N2Tm2*ddN0T0*dN4T0
-N5Tm1*ddN2T0*ddN6T*dN3T
*IP*dN0T0*N4Tm2-N3Tm1*ddN6T*IP*dN2T0*dN5T*N4Tm2*ddN0T0
-N5Tm1*ddN2T0*N6Tm2*ddN3T*FS*dN4T0
- +dN3T0*ddN7T*ddN2T0*N4Tm1*N5Tm2*dN6T*FP+dN3T0*dN4T*N6Tm2*N2Tm1*IP*ddN5T*ddN0T0
+ +dN3T0*ddN7T*ddN2T0*N4Tm1*N5Tm2*dN6T*FP+dN3T0*dN4T*N6Tm2*N2Tm1*IP*ddN5T*
+ ddN0T0
-N3Tm1*ddN2T0
*N6Tm2*dN7T*FP*ddN5T*dN4T0+dN3T0*ddN7T*N6Tm2*N2Tm1*ddN4T0*dN5T*FP
+ddN3T0*N4Tm1*IS*dN6T
@@ -1535,11 +1538,13 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+N3Tm1*dN4T*ddN6T*N5Tm2
*IP*dN2T0*ddN0T0+dN3T0*ddN7T*N5Tm1*dN4T*ddN2T0*N6Tm2*FP
-N5Tm1*dN6T*ddN4T0*IP*ddN3T*dN0T0
- *N2Tm2+dN3T0*N5Tm1*dN4T*ddN6T*IP*N2Tm2*ddN0T0+N3Tm1*ddN2T0*IS*dN6T*ddN5T*N4Tm2
+ *N2Tm2+dN3T0*N5Tm1*dN4T*ddN6T*IP*N2Tm2*ddN0T0+N3Tm1*ddN2T0*IS*dN6T*ddN5T*
+ N4Tm2
+ddN6T*ddN4T0
*dN2T0*dN5T*N3Tm2*MidP1+ddN3T0*N5Tm1*ddN6T*dN7T*FP*N2Tm2*dN4T0
+N5Tm1*ddN2T0*N6Tm2*dN3T*FA
- *dN4T0-N2Tm1*ddN6T*IP*dN5T*N3Tm2*ddN0T0*dN4T0+N4Tm1*IA*dN6T*dN2T0*N3Tm2*ddN5T
+ *dN4T0-N2Tm1*ddN6T*IP*dN5T*N3Tm2*ddN0T0*dN4T0+N4Tm1*IA*dN6T*dN2T0*N3Tm2*
+ ddN5T
+N4Tm1*ddN6T
*N5Tm2*IA*dN3T*dN2T0+ddN3T0*N4Tm1*ddN6T*N5Tm2*dN2T0*dN7T*FP
-N6Tm2*N2Tm1*IA*dN5T*ddN3T*dN4T0
@@ -1549,15 +1554,18 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+ddN3T0*dN4T*N2Tm1*ddN6T*N5Tm2*IP*dN0T0
-ddN3T0*N6Tm2*N2Tm1*IS*dN5T*ddN4T-N2Tm1*IA*dN6T*N3Tm2*ddN5T*dN4T0
-ddN3T0*N5Tm1*dN4T*ddN6T*IP
- *dN0T0*N2Tm2+ddN2T0*N4Tm1*ddN6T*IS*dN5T*N3Tm2-dN3T0*N2Tm1*ddN6T*N5Tm2*ddN4T0*FS
+ *dN0T0*N2Tm2+ddN2T0*N4Tm1*ddN6T*IS*dN5T*N3Tm2-dN3T0*N2Tm1*ddN6T*N5Tm2*
+ ddN4T0*FS
-ddN3T0*N4Tm1
*dN6T*dN2T0*ddN5T*MidP2-dN3T0*dN4T*ddN2T0*ddN6T*N5Tm2*MidP1
-dN3T0*N5Tm1*ddN2T0*N6Tm2*dN7T*FP
- *ddN4T-N5Tm1*dN6T*IP*dN2T0*ddN3T*N4Tm2*ddN0T0-N3Tm1*dN4T*ddN2T0*N6Tm2*IS*ddN5T
+ *ddN4T-N5Tm1*dN6T*IP*dN2T0*ddN3T*N4Tm2*ddN0T0-N3Tm1*dN4T*ddN2T0*N6Tm2*
+ IS*ddN5T
-N5Tm1*N6Tm2
*ddN4T0*dN2T0*ddN3T*dN7T*FP-dN3T0*N5Tm1*dN4T*ddN6T*IA*N2Tm2
+ddN3T0*N5Tm1*dN6T*IP*dN0T0*N2Tm2
- *ddN4T+ddN3T0*N5Tm1*ddN6T*dN2T0*FS*N4Tm2-ddN7T*N5Tm1*dN6T*ddN4T0*dN2T0*FP*N3Tm2
+ *ddN4T+ddN3T0*N5Tm1*ddN6T*dN2T0*FS*N4Tm2-ddN7T*N5Tm1*dN6T*ddN4T0*dN2T0*
+ FP*N3Tm2
+dN3T0*N4Tm1
*ddN6T*IA*dN5T*N2Tm2-dN3T0*ddN2T0*N6Tm2*dN5T*MidP1*ddN4T
-dN3T0*ddN2T0*N4Tm1*ddN6T*N5Tm2*dN7T
@@ -1569,7 +1577,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+ddN3T0*ddN7T*N4Tm1
*N6Tm2*dN2T0*dN5T*FP+ddN3T0*N6Tm2*N2Tm1*dN7T*FP*ddN5T*dN4T0
+dN3T0*N5Tm1*ddN2T0*N6Tm2*FS*ddN4T
- +ddN3T0*N4Tm1*ddN6T*dN2T0*dN5T*MidP2-ddN7T*N3Tm1*ddN2T0*N5Tm2*dN6T*FP*dN4T0
+ +ddN3T0*N4Tm1*ddN6T*dN2T0*dN5T*MidP2-ddN7T*N3Tm1*ddN2T0*N5Tm2*dN6T*
+ FP*dN4T0
+N6Tm2*dN3T*ddN4T0
*dN2T0*ddN5T*MidP1-ddN3T0*N4Tm1*dN6T*IP*dN0T0*ddN5T*N2Tm2
+N5Tm1*ddN2T0*ddN6T*FS*N3Tm2*dN4T0
@@ -1583,7 +1592,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+dN3T0*N4Tm1*dN6T
*IP*ddN5T*N2Tm2*ddN0T0+N3Tm1*dN4T*N6Tm2*IA*dN2T0*ddN5T
+N5Tm1*ddN2T0*IS*dN6T*N3Tm2*ddN4T
- +N4Tm1*N6Tm2*dN3T*IP*dN2T0*ddN5T*ddN0T0+dN3T0*N5Tm1*ddN2T0*ddN6T*dN7T*FP*N4Tm2
+ +N4Tm1*N6Tm2*dN3T*IP*dN2T0*ddN5T*ddN0T0+dN3T0*N5Tm1*ddN2T0*ddN6T*dN7T*FP
+ *N4Tm2
+ddN2T0*dN6T
*N3Tm2*ddN5T*MidP1*dN4T0+N3Tm1*N6Tm2*ddN4T0*dN2T0*dN7T*FP*ddN5T
-dN3T0*N2Tm1*ddN6T*IA*dN5T
@@ -1617,11 +1627,13 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-ddN3T0*dN6T*ddN5T
*MidP1*N2Tm2*dN4T0-dN3T0*N4Tm1*ddN6T*IP*dN5T*N2Tm2*ddN0T0
+ddN2T0*N6Tm2*dN5T*ddN3T*MidP1
- *dN4T0+ddN3T0*N2Tm1*N5Tm2*IS*dN6T*ddN4T+N4Tm1*N5Tm2*dN6T*IP*dN2T0*ddN3T*ddN0T0
+ *dN4T0+ddN3T0*N2Tm1*N5Tm2*IS*dN6T*ddN4T+N4Tm1*N5Tm2*dN6T*IP*dN2T0*ddN3T*
+ ddN0T0
+dN3T0*N5Tm1
*dN4T*ddN2T0*ddN6T*MidP2+ddN3T0*N6Tm2*dN2T0*dN5T*MidP1*ddN4T
-N4Tm1*N6Tm2*IP*dN2T0*dN5T*ddN3T
- *ddN0T0-N5Tm1*dN4T*ddN2T0*ddN6T*IS*N3Tm2-ddN3T0*N6Tm2*N2Tm1*FS*ddN5T*dN4T0
+ *ddN0T0-N5Tm1*dN4T*ddN2T0*ddN6T*IS*N3Tm2-ddN3T0*N6Tm2*N2Tm1*FS*ddN5T*
+ dN4T0
+dN3T0*N2Tm1*ddN6T
*ddN4T0*dN5T*MidP2+dN3T0*N6Tm2*N2Tm1*ddN4T0*FS*ddN5T
-N3Tm1*ddN2T0*ddN6T*IS*dN5T*N4Tm2+dN3T0
@@ -1629,7 +1641,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+ddN3T0*dN4T*ddN6T
*N5Tm2*dN2T0*MidP1+N2Tm1*ddN6T*N5Tm2*dN3T*IP*ddN0T0*dN4T0
+dN3T0*ddN2T0*N4Tm1*N6Tm2*dN7T*FP
- *ddN5T+N5Tm1*ddN2T0*dN6T*ddN3T*MidP2*dN4T0-dN3T0*ddN2T0*N4Tm1*N6Tm2*FS*ddN5T
+ *ddN5T+N5Tm1*ddN2T0*dN6T*ddN3T*MidP2*dN4T0-dN3T0*ddN2T0*N4Tm1*N6Tm2*FS*
+ ddN5T
+ddN3T0*N6Tm2
*N2Tm1*dN5T*FA*dN4T0-dN6T*ddN4T0*dN2T0*N3Tm2*ddN5T*MidP1
-ddN2T0*N4Tm1*N5Tm2*dN6T*IP*ddN3T
@@ -1637,11 +1650,13 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-N3Tm1*ddN6T*IA
*dN5T*N2Tm2*dN4T0+N3Tm1*dN4T*ddN2T0*N6Tm2*IP*dN0T0*ddN5T
+N5Tm1*ddN2T0*N6Tm2*dN3T*IP*dN0T0
- *ddN4T+dN3T0*dN4T*ddN2T0*N6Tm2*ddN5T*MidP1-dN3T0*ddN2T0*dN6T*ddN5T*N4Tm2*MidP1
+ *ddN4T+dN3T0*dN4T*ddN2T0*N6Tm2*ddN5T*MidP1-dN3T0*ddN2T0*dN6T*ddN5T*N4Tm2*
+ MidP1
+N5Tm1*dN6T
*ddN4T0*dN2T0*FA*N3Tm2-ddN3T0*N2Tm1*N5Tm2*dN6T*IP*dN0T0*ddN4T
-dN3T0*N2Tm1*dN6T*ddN4T0*ddN5T
- *MidP2+ddN3T0*N5Tm1*dN6T*FA*N2Tm2*dN4T0+ddN3T0*N6Tm2*N2Tm1*IP*dN5T*dN0T0*ddN4T
+ *MidP2+ddN3T0*N5Tm1*dN6T*FA*N2Tm2*dN4T0+ddN3T0*N6Tm2*N2Tm1*IP*dN5T*dN0T0*
+ ddN4T
+N5Tm1*ddN6T
*IA*dN3T*N2Tm2*dN4T0-N3Tm1*dN4T*N6Tm2*IP*dN2T0*ddN5T*ddN0T0
-ddN3T0*N5Tm1*dN4T*ddN6T*dN2T0
@@ -1663,7 +1678,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+N5Tm1*dN4T*N6Tm2*IS*ddN3T*ddN1T0+N5Tm2*IA
*dN6T*ddN3T*N1Tm1*dN4T0-dN3T0*N6Tm2*IP*dN5T*N1Tm1*ddN0T0*ddN4T
+ddN3T0*N4Tm1*N5Tm2*dN6T*dN1T0
- *FA+dN3T0*N4Tm1*N6Tm2*dN7T*ddN1T0*FP*ddN5T+ddN3T0*N5Tm2*IS*dN6T*N1Tm1*ddN4T
+ *FA+dN3T0*N4Tm1*N6Tm2*dN7T*ddN1T0*FP*ddN5T+ddN3T0*N5Tm2*IS*dN6T*N1Tm1*
+ ddN4T
+ddN3T0*dN6T*IP
*dN0T0*N1Tm1*ddN5T*N4Tm2+ddN3T0*dN4T*ddN6T*N5Tm2*IP*dN0T0*N1Tm1
+ddN7T*N5Tm1*N6Tm2*dN3T*ddN4T0
@@ -1679,7 +1695,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+ddN3T0*ddN6T*IS*dN5T*N1Tm1*N4Tm2+ddN3T0
*N4Tm1*ddN6T*N5Tm2*dN1T0*dN7T*FP+ddN6T*N5Tm2*dN3T*IS*ddN4T0*N1Tm1
-N5Tm1*dN4T*N6Tm2*IA*dN1T0
- *ddN3T-dN3T0*ddN6T*N5Tm2*ddN4T0*FS*N1Tm1+N6Tm2*dN3T*ddN4T0*IP*dN0T0*N1Tm1*ddN5T
+ *ddN3T-dN3T0*ddN6T*N5Tm2*ddN4T0*FS*N1Tm1+N6Tm2*dN3T*ddN4T0*IP*dN0T0*N1Tm1*
+ ddN5T
+dN3T0*N5Tm1
*dN6T*FA*ddN1T0*N4Tm2+dN3T0*N5Tm2*dN6T*IP*N1Tm1*ddN0T0*ddN4T
+N4Tm1*ddN6T*N5Tm2*dN3T*IP*dN0T0
@@ -1697,29 +1714,35 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+ddN3T0*dN4T*N6Tm2*IS*N1Tm1*ddN5T-ddN3T0*N5Tm1
*dN6T*dN1T0*FA*N4Tm2-dN3T0*ddN6T*IA*dN5T*N1Tm1*N4Tm2
+dN3T0*N5Tm1*ddN6T*dN7T*ddN1T0*FP*N4Tm2
- +N5Tm1*dN6T*IP*dN1T0*N3Tm2*ddN0T0*ddN4T+ddN3T0*dN6T*N1Tm1*ddN5T*MidP2*dN4T0
+ +N5Tm1*dN6T*IP*dN1T0*N3Tm2*ddN0T0*ddN4T+ddN3T0*dN6T*N1Tm1*ddN5T*MidP2*
+ dN4T0
-ddN7T*N5Tm1*dN6T
*ddN4T0*dN1T0*FP*N3Tm2-N5Tm1*N6Tm2*ddN3T*FS*ddN1T0*dN4T0
+ddN3T0*N5Tm1*ddN6T*dN1T0*FS*N4Tm2-N5Tm1
*dN4T*ddN6T*IS*ddN1T0*N3Tm2+N6Tm2*IP*dN5T*ddN3T*N1Tm1*ddN0T0*dN4T0
+N6Tm2*dN3T*ddN4T0*dN1T0*ddN5T
- *MidP1+IS*dN6T*ddN4T0*N1Tm1*N3Tm2*ddN5T+N4Tm1*dN6T*IP*dN0T0*ddN1T0*N3Tm2*ddN5T
+ *MidP1+IS*dN6T*ddN4T0*N1Tm1*N3Tm2*ddN5T+N4Tm1*dN6T*IP*dN0T0*ddN1T0*N3Tm2*
+ ddN5T
-dN3T0*dN6T*ddN4T0
*N1Tm1*ddN5T*MidP2-N3Tm1*dN6T*IP*dN0T0*ddN1T0*ddN5T*N4Tm2
+N6Tm2*IA*dN3T*N1Tm1*ddN5T*dN4T0+ddN6T
*ddN4T0*dN5T*dN1T0*N3Tm2*MidP1-ddN6T*N5Tm2*IA*dN3T*N1Tm1*dN4T0
+ddN3T0*ddN6T*N5Tm2*FS*N1Tm1*dN4T0
- +dN6T*ddN1T0*N3Tm2*ddN5T*MidP1*dN4T0-N5Tm1*dN4T*N6Tm2*IP*ddN3T*dN0T0*ddN1T0
+ +dN6T*ddN1T0*N3Tm2*ddN5T*MidP1*dN4T0-N5Tm1*dN4T*N6Tm2*IP*ddN3T*dN0T0*
+ ddN1T0
-N4Tm1*N6Tm2*IA*dN3T
- *dN1T0*ddN5T-N5Tm1*dN6T*FA*ddN1T0*N3Tm2*dN4T0+ddN3T0*N6Tm2*dN5T*FA*N1Tm1*dN4T0
+ *dN1T0*ddN5T-N5Tm1*dN6T*FA*ddN1T0*N3Tm2*dN4T0+ddN3T0*N6Tm2*dN5T*FA*N1Tm1
+ *dN4T0
-N6Tm2*dN3T*IS
*ddN4T0*N1Tm1*ddN5T-N5Tm1*dN4T*ddN6T*IP*dN1T0*N3Tm2*ddN0T0
+dN3T0*N5Tm2*dN6T*ddN4T0*FA*N1Tm1
- -ddN3T0*N4Tm1*N6Tm2*dN1T0*dN7T*FP*ddN5T+N4Tm1*N5Tm2*dN6T*IP*dN1T0*ddN3T*ddN0T0
+ -ddN3T0*N4Tm1*N6Tm2*dN1T0*dN7T*FP*ddN5T+N4Tm1*N5Tm2*dN6T*IP*dN1T0*ddN3T
+ *ddN0T0
-ddN7T*N3Tm1*N5Tm2
*dN6T*ddN1T0*FP*dN4T0-N4Tm1*ddN6T*N5Tm2*dN3T*IS*ddN1T0
+dN3T0*ddN6T*IP*dN5T*N1Tm1*N4Tm2*ddN0T0
- +dN3T0*ddN6T*N5Tm2*ddN4T0*dN7T*FP*N1Tm1+ddN7T*N3Tm1*N5Tm2*dN6T*ddN4T0*dN1T0*FP
+ +dN3T0*ddN6T*N5Tm2*ddN4T0*dN7T*FP*N1Tm1+ddN7T*N3Tm1*N5Tm2*dN6T*ddN4T0*
+ dN1T0*FP
+N5Tm1*ddN6T*FS
*ddN1T0*N3Tm2*dN4T0-ddN3T0*dN4T*N6Tm2*dN1T0*ddN5T*MidP1
-N3Tm1*ddN6T*N5Tm2*FS*ddN1T0*dN4T0-ddN6T
@@ -1731,7 +1754,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+N5Tm2*dN6T*ddN4T0*dN1T0*ddN3T*MidP1+ddN3T0
*N6Tm2*dN5T*dN1T0*MidP1*ddN4T+ddN6T*IA*dN5T*N1Tm1*N3Tm2*dN4T0
-N4Tm1*N6Tm2*IP*dN5T*dN1T0*ddN3T
- *ddN0T0-ddN3T0*N4Tm1*N6Tm2*dN5T*dN1T0*FA-N6Tm2*dN3T*IP*N1Tm1*ddN5T*ddN0T0*dN4T0
+ *ddN0T0-ddN3T0*N4Tm1*N6Tm2*dN5T*dN1T0*FA-N6Tm2*dN3T*IP*N1Tm1*ddN5T*ddN0T0
+ *dN4T0
-N5Tm2*dN6T*IP*
ddN3T*N1Tm1*ddN0T0*dN4T0+N3Tm1*N5Tm2*IA*dN6T*dN1T0*ddN4T
+N6Tm2*dN5T*ddN3T*ddN1T0*MidP1*dN4T0
@@ -1749,7 +1773,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-dN3T0*ddN7T*N5Tm1*dN6T*ddN1T0*FP*N4Tm2-
ddN6T*N5Tm2*dN3T*ddN4T0*dN1T0*MidP1-N3Tm1*dN4T*N6Tm2*IP*dN1T0*ddN5T*ddN0T0
-N5Tm1*ddN6T*IA*dN3T
- *dN1T0*N4Tm2+dN3T0*N4Tm1*N6Tm2*dN5T*FA*ddN1T0+dN3T0*N4Tm1*ddN6T*N5Tm2*FS*ddN1T0
+ *dN1T0*N4Tm2+dN3T0*N4Tm1*N6Tm2*dN5T*FA*ddN1T0+dN3T0*N4Tm1*ddN6T*N5Tm2*FS
+ *ddN1T0
+ddN6T*N5Tm2*dN3T
*IP*N1Tm1*ddN0T0*dN4T0-ddN3T0*N5Tm1*ddN6T*dN1T0*dN7T*FP*N4Tm2
-N4Tm1*ddN6T*N5Tm2*dN3T*IP*dN1T0
@@ -1763,7 +1788,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+N3Tm1*N6Tm2*IP*dN5T*dN1T0*ddN0T0*ddN4T-N3Tm1
*ddN6T*N5Tm2*ddN4T0*dN1T0*dN7T*FP-ddN3T0*N6Tm2*FS*N1Tm1*ddN5T*dN4T0
+ddN3T0*N6Tm2*dN7T*FP*N1Tm1
- *ddN5T*dN4T0+N3Tm1*N6Tm2*ddN4T0*dN5T*dN1T0*FA-N3Tm1*IA*dN6T*dN1T0*ddN5T*N4Tm2
+ *ddN5T*dN4T0+N3Tm1*N6Tm2*ddN4T0*dN5T*dN1T0*FA-N3Tm1*IA*dN6T*dN1T0*ddN5T
+ *N4Tm2
+N3Tm1*N5Tm2*dN6T
*FA*ddN1T0*dN4T0-dN3T0*ddN7T*N5Tm2*dN6T*ddN4T0*FP*N1Tm1
+N5Tm1*dN4T*ddN6T*IP*dN0T0*ddN1T0*N3Tm2
@@ -1775,7 +1801,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+dN3T0*dN4T*ddN6T*N5Tm2*IA*N1Tm1+N3Tm1
*N6Tm2*ddN4T0*dN1T0*dN7T*FP*ddN5T-N5Tm1*dN6T*ddN4T0*dN1T0*ddN3T*MidP2
+N5Tm1*dN6T*ddN3T*ddN1T0*MidP2
- *dN4T0-N5Tm1*dN6T*IP*dN0T0*ddN1T0*N3Tm2*ddN4T+N4Tm1*ddN6T*IS*dN5T*ddN1T0*N3Tm2
+ *dN4T0-N5Tm1*dN6T*IP*dN0T0*ddN1T0*N3Tm2*ddN4T+N4Tm1*ddN6T*IS*dN5T*ddN1T0*
+ N3Tm2
+ddN3T0*N6Tm2*IP*dN5T
*dN0T0*N1Tm1*ddN4T+N4Tm1*N5Tm2*IS*dN6T*ddN3T*ddN1T0
+N3Tm1*N6Tm2*IS*dN5T*ddN1T0*ddN4T+ddN7T*N3Tm1*N6Tm2
@@ -1783,27 +1810,32 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-N3Tm1*N5Tm2*dN6T*IP*dN1T0*ddN0T0*ddN4T
-ddN6T*dN5T*ddN1T0*N3Tm2*MidP1*dN4T0-N5Tm2*IS*dN6T*ddN4T0*ddN3T*N1Tm1
-N5Tm1*IS*dN6T*ddN3T*ddN1T0*N4Tm2
- +N5Tm1*dN4T*N6Tm2*IP*dN1T0*ddN3T*ddN0T0-N3Tm1*ddN6T*ddN4T0*dN5T*dN1T0*MidP2
+ +N5Tm1*dN4T*N6Tm2*IP*dN1T0*ddN3T*ddN0T0-N3Tm1*ddN6T*ddN4T0*dN5T*dN1T0*
+ MidP2
+N4Tm1*N6Tm2*IA*dN5T*dN1T0
- *ddN3T-dN3T0*N5Tm1*ddN6T*FS*ddN1T0*N4Tm2-ddN6T*N5Tm2*dN3T*ddN4T0*IP*dN0T0*N1Tm1
+ *ddN3T-dN3T0*N5Tm1*ddN6T*FS*ddN1T0*N4Tm2-ddN6T*N5Tm2*dN3T*ddN4T0*IP*dN0T0
+ *N1Tm1
+ddN3T0*dN4T*ddN6T
*N5Tm2*dN1T0*MidP1+N3Tm1*dN4T*N6Tm2*IP*dN0T0*ddN1T0*ddN5T
-dN3T0*N5Tm1*N6Tm2*dN7T*ddN1T0*FP*ddN4T
-ddN7T*N3Tm1*N6Tm2*ddN4T0*dN5T*dN1T0*FP+dN3T0*N6Tm2*IA*dN5T*N1Tm1*ddN4T
-dN3T0*N4Tm1*ddN6T*dN5T*ddN1T0
- *MidP2+N4Tm1*ddN6T*N5Tm2*IA*dN3T*dN1T0+ddN6T*ddN4T0*IP*dN5T*dN0T0*N1Tm1*N3Tm2
+ *MidP2+N4Tm1*ddN6T*N5Tm2*IA*dN3T*dN1T0+ddN6T*ddN4T0*IP*dN5T*dN0T0*N1Tm1
+ *N3Tm2
-N4Tm1*N5Tm2*dN6T*IP
*ddN3T*dN0T0*ddN1T0-N6Tm2*ddN4T0*dN5T*dN1T0*ddN3T*MidP1
-dN3T0*dN4T*N6Tm2*IA*N1Tm1*ddN5T+N5Tm2*dN6T
*ddN4T0*IP*ddN3T*dN0T0*N1Tm1-ddN3T0*N5Tm2*dN6T*dN1T0*MidP1*ddN4T
-N5Tm1*IA*dN6T*dN1T0*N3Tm2*ddN4T
- -ddN3T0*N5Tm2*dN6T*IP*dN0T0*N1Tm1*ddN4T-N5Tm1*ddN6T*dN3T*IP*dN0T0*ddN1T0*N4Tm2
+ -ddN3T0*N5Tm2*dN6T*IP*dN0T0*N1Tm1*ddN4T-N5Tm1*ddN6T*dN3T*IP*dN0T0*ddN1T0
+ *N4Tm2
+ddN3T0*N4Tm1*N6Tm2
*dN1T0*FS*ddN5T-N5Tm1*N6Tm2*ddN4T0*dN1T0*ddN3T*dN7T*FP
-N5Tm1*dN6T*IP*dN1T0*ddN3T*N4Tm2*ddN0T0+ddN3T0
*ddN7T*N5Tm2*dN6T*FP*N1Tm1*dN4T0-ddN3T0*N5Tm1*dN4T*ddN6T*dN1T0*MidP2
+N5Tm1*ddN6T*dN3T*IS*ddN1T0
- *N4Tm2+dN6T*IP*N1Tm1*N3Tm2*ddN5T*ddN0T0*dN4T0-dN3T0*N6Tm2*ddN4T0*dN5T*FA*N1Tm1
+ *N4Tm2+dN6T*IP*N1Tm1*N3Tm2*ddN5T*ddN0T0*dN4T0-dN3T0*N6Tm2*ddN4T0*dN5T*FA
+ *N1Tm1
-ddN3T0*dN4T*ddN6T
*N5Tm2*IS*N1Tm1-dN6T*ddN4T0*IP*dN0T0*N1Tm1*N3Tm2*ddN5T
-N5Tm2*dN6T*ddN3T*ddN1T0*MidP1*dN4T0+N5Tm1
@@ -1821,7 +1853,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+N5Tm1*ddN6T*ddN4T0*dN1T0*dN7T*FP*N3Tm2
-IA*dN6T*N1Tm1*N3Tm2*ddN5T*dN4T0+dN3T0*N5Tm1*dN4T*ddN6T*ddN1T0*MidP2
+N5Tm1*N6Tm2*IA*dN3T*dN1T0*ddN4T
- +dN3T0*ddN6T*ddN4T0*dN5T*N1Tm1*MidP2-ddN3T0*ddN7T*N5Tm1*dN4T*N6Tm2*dN1T0*FP
+ +dN3T0*ddN6T*ddN4T0*dN5T*N1Tm1*MidP2-ddN3T0*ddN7T*N5Tm1*dN4T*N6Tm2*dN1T0*
+ FP
+N3Tm1*N5Tm2*dN6T*IP*dN0T0
*ddN1T0*ddN4T+ddN3T0*N5Tm1*N6Tm2*dN1T0*dN7T*FP*ddN4T
-dN3T0*dN4T*ddN6T*N5Tm2*IP*N1Tm1*ddN0T0+N3Tm1
@@ -1837,7 +1870,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+N3Tm1*N6Tm2*FS*ddN1T0*ddN5T*dN4T0+dN3T0
*dN4T*N6Tm2*IP*N1Tm1*ddN5T*ddN0T0-N6Tm2*dN3T*ddN1T0*ddN5T*MidP1*dN4T0
-N5Tm1*ddN6T*ddN4T0*dN1T0*FS
- *N3Tm2-dN3T0*N6Tm2*dN5T*ddN1T0*MidP1*ddN4T+N3Tm1*ddN6T*dN5T*ddN1T0*MidP2*dN4T0
+ *N3Tm2-dN3T0*N6Tm2*dN5T*ddN1T0*MidP1*ddN4T+N3Tm1*ddN6T*dN5T*ddN1T0*MidP2*
+ dN4T0
-N4Tm1*N6Tm2*dN3T*IP
*dN0T0*ddN1T0*ddN5T-ddN3T0*dN4T*N6Tm2*IP*dN0T0*N1Tm1*ddN5T
-N4Tm1*IS*dN6T*ddN1T0*N3Tm2*ddN5T)
@@ -1897,108 +1931,126 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+ddN2T0*N5Tm2*dN6T*ddN3T*N1Tm1*dN4T0+ddN2T0
*N4Tm1*N6Tm2*dN5T*dN1T0*ddN3T+dN3T0*dN4T*ddN2T0*ddN6T*N5Tm2*N1Tm1
-N5Tm1*dN4T*ddN6T*dN2T0*ddN1T0
- *N3Tm2-ddN3T0*N5Tm1*dN4T*ddN6T*dN1T0*N2Tm2+ddN3T0*dN4T*N6Tm2*dN2T0*N1Tm1*ddN5T
+ *N3Tm2-ddN3T0*N5Tm1*dN4T*ddN6T*dN1T0*N2Tm2+ddN3T0*dN4T*N6Tm2*dN2T0*N1Tm1*
+ ddN5T
-N4Tm1*dN6T*dN2T0
*ddN1T0*N3Tm2*ddN5T-N2Tm1*ddN6T*N5Tm2*dN3T*ddN4T0*dN1T0
+ddN2T0*N4Tm1*dN6T*dN1T0*N3Tm2*ddN5T+dN3T0
*N2Tm1*ddN6T*dN5T*ddN1T0*N4Tm2+N5Tm1*ddN2T0*dN6T*dN1T0*ddN3T*N4Tm2
+ddN3T0*dN6T*N1Tm1*ddN5T*N2Tm2
- *dN4T0-dN3T0*N4Tm1*ddN6T*dN5T*ddN1T0*N2Tm2-ddN2T0*dN6T*N1Tm1*N3Tm2*ddN5T*dN4T0
+ *dN4T0-dN3T0*N4Tm1*ddN6T*dN5T*ddN1T0*N2Tm2-ddN2T0*dN6T*N1Tm1*N3Tm2*ddN5T*
+ dN4T0
-N5Tm1*N6Tm2*dN3T
*dN2T0*ddN1T0*ddN4T-N3Tm1*ddN6T*ddN4T0*dN5T*dN1T0*N2Tm2
-ddN3T0*N2Tm1*N5Tm2*dN6T*dN1T0*ddN4T+ddN2T0
*N4Tm1*ddN6T*N5Tm2*dN3T*dN1T0-N3Tm1*dN4T*N6Tm2*dN2T0*ddN1T0*ddN5T
-dN3T0*N5Tm1*dN6T*ddN1T0*N2Tm2
- *ddN4T-N5Tm1*dN6T*ddN4T0*dN1T0*ddN3T*N2Tm2+N6Tm2*N2Tm1*dN5T*ddN3T*ddN1T0*dN4T0
+ *ddN4T-N5Tm1*dN6T*ddN4T0*dN1T0*ddN3T*N2Tm2+N6Tm2*N2Tm1*dN5T*ddN3T*ddN1T0*
+ dN4T0
-N3Tm1*ddN6T*dN2T0
*dN5T*ddN1T0*N4Tm2+N2Tm1*ddN6T*ddN4T0*dN5T*dN1T0*N3Tm2
+N5Tm1*dN4T*ddN2T0*ddN6T*dN1T0*N3Tm2+N2Tm1
*dN6T*ddN1T0*N3Tm2*ddN5T*dN4T0+dN3T0*N4Tm1*dN6T*ddN1T0*ddN5T*N2Tm2
-dN3T0*dN4T*ddN2T0*N6Tm2*N1Tm1
- *ddN5T+ddN3T0*ddN6T*dN2T0*dN5T*N1Tm1*N4Tm2+N5Tm1*dN4T*N6Tm2*dN2T0*ddN3T*ddN1T0
+ *ddN5T+ddN3T0*ddN6T*dN2T0*dN5T*N1Tm1*N4Tm2+N5Tm1*dN4T*N6Tm2*dN2T0*ddN3T*
+ ddN1T0
-dN3T0*N2Tm1*dN6T
*ddN1T0*ddN5T*N4Tm2-N4Tm1*N6Tm2*dN2T0*dN5T*ddN3T*ddN1T0
+N6Tm2*N2Tm1*dN3T*ddN4T0*dN1T0*ddN5T);
m_control_points[3]=
- -1.0/( N3Tm1*ddN6T*dN5T*ddN1T0*N2Tm2*dN4T0+N4Tm1*N5Tm2*dN6T*dN2T0*ddN3T*ddN1T0
+ -1.0/( N3Tm1*ddN6T*dN5T*ddN1T0*N2Tm2*dN4T0+N4Tm1*N5Tm2*dN6T*dN2T0*ddN3T*
+ ddN1T0
-ddN2T0*ddN6T*N5Tm2*dN3T
*N1Tm1*dN4T0+dN3T0*ddN2T0*N6Tm2*dN5T*N1Tm1*ddN4T
-ddN2T0*N4Tm1*N6Tm2*dN3T*dN1T0*ddN5T-N2Tm1*N5Tm2
*dN6T*ddN3T*ddN1T0*dN4T0-ddN3T0*dN6T*dN2T0*N1Tm1*ddN5T*N4Tm2
-ddN2T0*N4Tm1*ddN6T*dN5T*dN1T0*N3Tm2
- -N4Tm1*ddN6T*N5Tm2*dN3T*dN2T0*ddN1T0-dN3T0*ddN2T0*ddN6T*dN5T*N1Tm1*N4Tm2
+ -N4Tm1*ddN6T*N5Tm2*dN3T*dN2T0*ddN1T0-dN3T0*ddN2T0*ddN6T*dN5T*N1Tm1*
+ N4Tm2
-ddN3T0*dN4T*N6Tm2*N2Tm1
*dN1T0*ddN5T-dN3T0*dN4T*N2Tm1*ddN6T*N5Tm2*ddN1T0
+ddN3T0*N6Tm2*N2Tm1*dN5T*dN1T0*ddN4T+N3Tm1*ddN2T0
*N5Tm2*dN6T*dN1T0*ddN4T-N5Tm1*dN4T*ddN2T0*N6Tm2*dN1T0*ddN3T
-dN3T0*dN6T*ddN4T0*N1Tm1*ddN5T*N2Tm2
- +ddN2T0*N6Tm2*dN3T*N1Tm1*ddN5T*dN4T0+N6Tm2*ddN4T0*dN2T0*dN5T*ddN3T*N1Tm1
+ +ddN2T0*N6Tm2*dN3T*N1Tm1*ddN5T*dN4T0+N6Tm2*ddN4T0*dN2T0*dN5T*ddN3T*
+ N1Tm1
+N5Tm1*ddN6T*dN3T*dN2T0
*ddN1T0*N4Tm2+dN6T*ddN4T0*dN2T0*N1Tm1*N3Tm2*ddN5T
-N3Tm1*dN4T*ddN2T0*ddN6T*N5Tm2*dN1T0+N3Tm1*dN6T
*dN2T0*ddN1T0*ddN5T*N4Tm2-N3Tm1*ddN2T0*N6Tm2*dN5T*dN1T0*ddN4T
+N2Tm1*N5Tm2*dN6T*ddN4T0*dN1T0*ddN3T
- +N5Tm1*ddN2T0*N6Tm2*dN3T*dN1T0*ddN4T+N5Tm1*dN6T*ddN3T*ddN1T0*N2Tm2*dN4T0
+ +N5Tm1*ddN2T0*N6Tm2*dN3T*dN1T0*ddN4T+N5Tm1*dN6T*ddN3T*ddN1T0*N2Tm2*
+ dN4T0
+dN3T0*dN4T*N6Tm2*N2Tm1
*ddN1T0*ddN5T-dN3T0*N6Tm2*N2Tm1*dN5T*ddN1T0*ddN4T
-ddN3T0*N6Tm2*dN2T0*dN5T*N1Tm1*ddN4T+ddN6T*N5Tm2
*dN3T*ddN4T0*dN2T0*N1Tm1+N5Tm1*dN6T*dN2T0*ddN1T0*N3Tm2*ddN4T
+ddN2T0*ddN6T*dN5T*N1Tm1*N3Tm2*dN4T0
- -ddN2T0*N6Tm2*dN5T*ddN3T*N1Tm1*dN4T0+dN3T0*ddN2T0*dN6T*N1Tm1*ddN5T*N4Tm2
+ -ddN2T0*N6Tm2*dN5T*ddN3T*N1Tm1*dN4T0+dN3T0*ddN2T0*dN6T*N1Tm1*ddN5T*
+ N4Tm2
+ddN3T0*N2Tm1*dN6T*dN1T0
*ddN5T*N4Tm2+N3Tm1*N6Tm2*dN2T0*dN5T*ddN1T0*ddN4T
-ddN3T0*N4Tm1*dN6T*dN1T0*ddN5T*N2Tm2+N4Tm1*N6Tm2
*dN3T*dN2T0*ddN1T0*ddN5T+N4Tm1*ddN6T*dN2T0*dN5T*ddN1T0*N3Tm2
+N3Tm1*ddN2T0*ddN6T*dN5T*dN1T0*N4Tm2
- -ddN3T0*ddN6T*dN5T*N1Tm1*N2Tm2*dN4T0-N3Tm1*dN6T*ddN1T0*ddN5T*N2Tm2*dN4T0
+ -ddN3T0*ddN6T*dN5T*N1Tm1*N2Tm2*dN4T0-N3Tm1*dN6T*ddN1T0*ddN5T*N2Tm2*
+ dN4T0
+N5Tm1*ddN6T*dN3T*ddN4T0
*dN1T0*N2Tm2-dN3T0*ddN2T0*N5Tm2*dN6T*N1Tm1*ddN4T
-N5Tm1*ddN2T0*ddN6T*dN3T*dN1T0*N4Tm2+ddN3T0*N5Tm1
*dN6T*dN1T0*N2Tm2*ddN4T-ddN6T*ddN4T0*dN2T0*dN5T*N1Tm1*N3Tm2
+dN3T0*N2Tm1*N5Tm2*dN6T*ddN1T0*ddN4T
- +ddN3T0*dN4T*N2Tm1*ddN6T*N5Tm2*dN1T0+N2Tm1*ddN6T*N5Tm2*dN3T*ddN1T0*dN4T0
+ +ddN3T0*dN4T*N2Tm1*ddN6T*N5Tm2*dN1T0+N2Tm1*ddN6T*N5Tm2*dN3T*ddN1T0*
+ dN4T0
-N5Tm1*ddN6T*dN3T*ddN1T0
*N2Tm2*dN4T0-N3Tm1*N5Tm2*dN6T*dN2T0*ddN1T0*ddN4T
+ddN3T0*N4Tm1*ddN6T*dN5T*dN1T0*N2Tm2-N3Tm1*ddN2T0
*dN6T*dN1T0*ddN5T*N4Tm2-N2Tm1*dN6T*ddN4T0*dN1T0*N3Tm2*ddN5T
-N5Tm2*dN6T*ddN4T0*dN2T0*ddN3T*N1Tm1
- +ddN3T0*N5Tm2*dN6T*dN2T0*N1Tm1*ddN4T-N5Tm1*dN6T*dN2T0*ddN3T*ddN1T0*N4Tm2
+ +ddN3T0*N5Tm2*dN6T*dN2T0*N1Tm1*ddN4T-N5Tm1*dN6T*dN2T0*ddN3T*ddN1T0*
+ N4Tm2
+N3Tm1*dN4T*ddN6T*N5Tm2
*dN2T0*ddN1T0-ddN3T0*N2Tm1*ddN6T*dN5T*dN1T0*N4Tm2
+dN3T0*ddN6T*ddN4T0*dN5T*N1Tm1*N2Tm2-N2Tm1*ddN6T
*dN5T*ddN1T0*N3Tm2*dN4T0+dN3T0*N5Tm1*dN4T*ddN6T*ddN1T0*N2Tm2
-N6Tm2*N2Tm1*dN3T*ddN1T0*ddN5T*dN4T0
- +N3Tm1*dN6T*ddN4T0*dN1T0*ddN5T*N2Tm2-N6Tm2*dN3T*ddN4T0*dN2T0*N1Tm1*ddN5T
+ +N3Tm1*dN6T*ddN4T0*dN1T0*ddN5T*N2Tm2-N6Tm2*dN3T*ddN4T0*dN2T0*N1Tm1*
+ ddN5T
-ddN2T0*N4Tm1*N5Tm2*dN6T
*dN1T0*ddN3T+N3Tm1*dN4T*ddN2T0*N6Tm2*dN1T0*ddN5T
-N6Tm2*N2Tm1*ddN4T0*dN5T*dN1T0*ddN3T-N5Tm1*ddN2T0
*dN6T*dN1T0*N3Tm2*ddN4T-ddN3T0*dN4T*ddN6T*N5Tm2*dN2T0*N1Tm1
+ddN2T0*N5Tm2*dN6T*ddN3T*N1Tm1*dN4T0
- +ddN2T0*N4Tm1*N6Tm2*dN5T*dN1T0*ddN3T+dN3T0*dN4T*ddN2T0*ddN6T*N5Tm2*N1Tm1
+ +ddN2T0*N4Tm1*N6Tm2*dN5T*dN1T0*ddN3T+dN3T0*dN4T*ddN2T0*ddN6T*N5Tm2*
+ N1Tm1
-N5Tm1*dN4T*ddN6T*dN2T0*
ddN1T0*N3Tm2-ddN3T0*N5Tm1*dN4T*ddN6T*dN1T0*N2Tm2
+ddN3T0*dN4T*N6Tm2*dN2T0*N1Tm1*ddN5T-N4Tm1*dN6T
*dN2T0*ddN1T0*N3Tm2*ddN5T-N2Tm1*ddN6T*N5Tm2*dN3T*ddN4T0*dN1T0
+ddN2T0*N4Tm1*dN6T*dN1T0*N3Tm2*ddN5T
- +dN3T0*N2Tm1*ddN6T*dN5T*ddN1T0*N4Tm2+N5Tm1*ddN2T0*dN6T*dN1T0*ddN3T*N4Tm2
+ +dN3T0*N2Tm1*ddN6T*dN5T*ddN1T0*N4Tm2+N5Tm1*ddN2T0*dN6T*dN1T0*ddN3T*
+ N4Tm2
+ddN3T0*dN6T*N1Tm1*ddN5T
*N2Tm2*dN4T0-dN3T0*N4Tm1*ddN6T*dN5T*ddN1T0*N2Tm2
-ddN2T0*dN6T*N1Tm1*N3Tm2*ddN5T*dN4T0-N5Tm1*N6Tm2
*dN3T*dN2T0*ddN1T0*ddN4T-N3Tm1*ddN6T*ddN4T0*dN5T*dN1T0*N2Tm2
-ddN3T0*N2Tm1*N5Tm2*dN6T*dN1T0*ddN4T
- +ddN2T0*N4Tm1*ddN6T*N5Tm2*dN3T*dN1T0-N3Tm1*dN4T*N6Tm2*dN2T0*ddN1T0*ddN5T
+ +ddN2T0*N4Tm1*ddN6T*N5Tm2*dN3T*dN1T0-N3Tm1*dN4T*N6Tm2*dN2T0*ddN1T0*
+ ddN5T
-dN3T0*N5Tm1*dN6T*ddN1T0
*N2Tm2*ddN4T-N5Tm1*dN6T*ddN4T0*dN1T0*ddN3T*N2Tm2
+N6Tm2*N2Tm1*dN5T*ddN3T*ddN1T0*dN4T0-N3Tm1*ddN6T
*dN2T0*dN5T*ddN1T0*N4Tm2+N2Tm1*ddN6T*ddN4T0*dN5T*dN1T0*N3Tm2
+N5Tm1*dN4T*ddN2T0*ddN6T*dN1T0*N3Tm2
- +N2Tm1*dN6T*ddN1T0*N3Tm2*ddN5T*dN4T0+dN3T0*N4Tm1*dN6T*ddN1T0*ddN5T*N2Tm2
+ +N2Tm1*dN6T*ddN1T0*N3Tm2*ddN5T*dN4T0+dN3T0*N4Tm1*dN6T*ddN1T0*ddN5T*
+ N2Tm2
-dN3T0*dN4T*ddN2T0*N6Tm2
*N1Tm1*ddN5T+ddN3T0*ddN6T*dN2T0*dN5T*N1Tm1*N4Tm2
+N5Tm1*dN4T*N6Tm2*dN2T0*ddN3T*ddN1T0-dN3T0*N2Tm1
*dN6T*ddN1T0*ddN5T*N4Tm2-N4Tm1*N6Tm2*dN2T0*dN5T*ddN3T*ddN1T0
+N6Tm2*N2Tm1*dN3T*ddN4T0*dN1T0*ddN5T)
- *( ddN6T*IP*dN2T0*dN5T*N1Tm1*N4Tm2*ddN0T0+N5Tm2*dN6T*dN2T0*ddN1T0*MidP1*ddN4T
+ *( ddN6T*IP*dN2T0*dN5T*N1Tm1*N4Tm2*ddN0T0+N5Tm2*dN6T*dN2T0*ddN1T0*MidP1*
+ ddN4T
+ddN7T*N5Tm1*dN6T*ddN1T0*FP
*N2Tm2*dN4T0-ddN6T*dN5T*ddN1T0*MidP1*N2Tm2*dN4T0
+N4Tm1*N6Tm2*dN2T0*dN5T*FA*ddN1T0+ddN7T*N2Tm1*N5Tm2
@@ -2006,13 +2058,17 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-N5Tm1*dN4T*N6Tm2*dN2T0*FA*ddN1T0-ddN7T*ddN2T0
*N6Tm2*dN5T*FP*N1Tm1*dN4T0+N6Tm2*N2Tm1*IP*dN5T*dN1T0*ddN0T0*ddN4T
-N2Tm1*N5Tm2*dN6T*IP*dN1T0*ddN0T0
- *ddN4T-N2Tm1*ddN6T*N5Tm2*FS*ddN1T0*dN4T0-ddN2T0*N5Tm2*dN6T*IP*dN0T0*N1Tm1*ddN4T
+ *ddN4T-N2Tm1*ddN6T*N5Tm2*FS*ddN1T0*dN4T0-ddN2T0*N5Tm2*dN6T*IP*dN0T0*
+ N1Tm1*
+ ddN4T
-dN6T*ddN4T0*dN1T0
*ddN5T*MidP1*N2Tm2+N6Tm2*N2Tm1*ddN4T0*dN1T0*dN7T*FP*ddN5T
-N5Tm1*ddN2T0*N6Tm2*dN1T0*FS*ddN4T-ddN7T
*N2Tm1*N5Tm2*dN6T*ddN1T0*FP*dN4T0-ddN7T*N5Tm1*dN4T*ddN2T0*N6Tm2*dN1T0*FP
-dN4T*ddN2T0*N6Tm2*dN1T0
- *ddN5T*MidP1-N6Tm2*N2Tm1*ddN4T0*dN1T0*FS*ddN5T+N5Tm1*dN6T*ddN4T0*dN1T0*FA*N2Tm2
+ *ddN5T*MidP1-N6Tm2*N2Tm1*ddN4T0*dN1T0*FS*ddN5T+N5Tm1*dN6T*ddN4T0*dN1T0*
+ FA*
+ N2Tm2
-N6Tm2*N2Tm1*dN5T*FA
*ddN1T0*dN4T0-IA*dN6T*N1Tm1*ddN5T*N2Tm2*dN4T0
-N4Tm1*ddN6T*N5Tm2*dN2T0*dN7T*ddN1T0*FP-N2Tm1*ddN6T*N5Tm2
@@ -2020,17 +2076,20 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-ddN6T*IA*dN2T0*dN5T*N1Tm1*N4Tm2+N2Tm1*ddN6T
*IA*dN5T*dN1T0*N4Tm2+ddN2T0*N6Tm2*IP*dN5T*dN0T0*N1Tm1*ddN4T
-ddN7T*N5Tm2*dN6T*ddN4T0*dN2T0*FP*N1Tm1
- +ddN2T0*N4Tm1*ddN6T*dN5T*dN1T0*MidP2-dN4T*ddN6T*N5Tm2*IP*dN2T0*N1Tm1*ddN0T0
+ +ddN2T0*N4Tm1*ddN6T*dN5T*dN1T0*MidP2-dN4T*ddN6T*N5Tm2*IP*dN2T0*N1Tm1*
+ ddN0T0
+N2Tm1*IS*dN6T*ddN1T0*ddN5T
*N4Tm2+N5Tm1*ddN2T0*ddN6T*dN1T0*FS*N4Tm2+N2Tm1*N5Tm2*IA*dN6T*dN1T0*ddN4T
-dN4T*N6Tm2*IA*dN2T0*N1Tm1
- *ddN5T-N5Tm1*ddN2T0*dN6T*dN1T0*FA*N4Tm2+ddN2T0*N6Tm2*dN7T*FP*N1Tm1*ddN5T*dN4T0
+ *ddN5T-N5Tm1*ddN2T0*dN6T*dN1T0*FA*N4Tm2+ddN2T0*N6Tm2*dN7T*FP*N1Tm1*ddN5T*
+ dN4T0
-dN4T*N6Tm2*N2Tm1*IP
*dN1T0*ddN5T*ddN0T0+N2Tm1*ddN6T*N5Tm2*ddN4T0*dN1T0*FS
+dN6T*IP*N1Tm1*ddN5T*N2Tm2*ddN0T0*dN4T0-ddN6T
*IS*ddN4T0*dN5T*N1Tm1*N2Tm2+N5Tm1*dN4T*ddN2T0*N6Tm2*dN1T0*FA
+N4Tm1*ddN6T*IP*dN5T*dN1T0*N2Tm2*ddN0T0
- -dN4T*N2Tm1*ddN6T*N5Tm2*IP*dN0T0*ddN1T0-N4Tm1*ddN6T*IP*dN5T*dN0T0*ddN1T0*N2Tm2
+ -dN4T*N2Tm1*ddN6T*N5Tm2*IP*dN0T0*ddN1T0-N4Tm1*ddN6T*IP*dN5T*dN0T0*ddN1T0*
+ N2Tm2
+dN4T*N2Tm1*ddN6T*N5Tm2
*IS*ddN1T0-N2Tm1*N5Tm2*IS*dN6T*ddN1T0*ddN4T
-ddN6T*IP*dN5T*N1Tm1*N2Tm2*ddN0T0*dN4T0+N2Tm1*N5Tm2*dN6T
@@ -2058,7 +2117,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-ddN2T0*N5Tm2*dN6T*dN1T0*MidP1*ddN4T
-N5Tm1*ddN2T0*ddN6T*dN1T0*dN7T*FP*N4Tm2+N4Tm1*IA*dN6T*dN1T0*ddN5T*N2Tm2
+ddN7T*N5Tm1*ddN2T0*dN6T*dN1T0
- *FP*N4Tm2+dN4T*ddN2T0*N6Tm2*IS*N1Tm1*ddN5T-N5Tm1*IA*dN6T*dN1T0*N2Tm2*ddN4T
+ *FP*N4Tm2+dN4T*ddN2T0*N6Tm2*IS*N1Tm1*ddN5T-N5Tm1*IA*dN6T*dN1T0*N2Tm2*
+ ddN4T
+N5Tm1*ddN2T0*dN6T*dN1T0
*MidP2*ddN4T+N2Tm1*ddN6T*N5Tm2*dN7T*ddN1T0*FP*dN4T0
+N5Tm1*dN4T*ddN6T*IA*dN1T0*N2Tm2-N5Tm1*dN4T*ddN6T
@@ -2068,7 +2128,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-N5Tm1*dN6T*FA*ddN1T0*N2Tm2*dN4T0-N6Tm2
*N2Tm1*dN7T*ddN1T0*FP*ddN5T*dN4T0+ddN6T*dN2T0*dN5T*ddN1T0*N4Tm2*MidP1
+N5Tm1*IS*dN6T*ddN1T0*N2Tm2*ddN4T
- -N2Tm1*dN6T*IP*dN0T0*ddN1T0*ddN5T*N4Tm2-ddN2T0*ddN6T*N5Tm2*dN7T*FP*N1Tm1*dN4T0
+ -N2Tm1*dN6T*IP*dN0T0*ddN1T0*ddN5T*N4Tm2-ddN2T0*ddN6T*N5Tm2*dN7T*FP*N1Tm1*
+ dN4T0
-N5Tm1*ddN6T*dN7T*ddN1T0
*FP*N2Tm2*dN4T0+ddN2T0*N6Tm2*dN5T*FA*N1Tm1*dN4T0
+N5Tm1*ddN6T*FS*ddN1T0*N2Tm2*dN4T0+ddN6T*ddN4T0*dN2T0
@@ -2076,7 +2137,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+dN4T*ddN2T0*ddN6T*N5Tm2*IP*dN0T0*N1Tm1+dN4T
*N2Tm1*ddN6T*N5Tm2*IP*dN1T0*ddN0T0+ddN7T*ddN2T0*N4Tm1*N6Tm2*dN5T*dN1T0*FP
-N5Tm1*ddN6T*ddN4T0*dN1T0*FS
- *N2Tm2+N2Tm1*dN6T*IP*dN1T0*ddN5T*N4Tm2*ddN0T0-N4Tm1*IS*dN6T*ddN1T0*ddN5T*N2Tm2
+ *N2Tm2+N2Tm1*dN6T*IP*dN1T0*ddN5T*N4Tm2*ddN0T0-N4Tm1*IS*dN6T*ddN1T0*ddN5T*
+ N2Tm2
+ddN2T0*ddN6T*N5Tm2*FS
*N1Tm1*dN4T0+N5Tm2*dN6T*IP*dN2T0*N1Tm1*ddN0T0*ddN4T
+ddN2T0*N4Tm1*ddN6T*N5Tm2*dN1T0*dN7T*FP-N6Tm2*IP
@@ -2084,7 +2146,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+N4Tm1*ddN6T*N5Tm2*dN2T0*FS*ddN1T0
-ddN7T*N5Tm1*dN6T*dN2T0*ddN1T0*FP*N4Tm2-N6Tm2*N2Tm1*IA*dN5T*dN1T0*ddN4T
+N2Tm1*ddN6T*dN5T*ddN1T0*MidP2
- *dN4T0-dN6T*IP*dN2T0*N1Tm1*ddN5T*N4Tm2*ddN0T0+ddN6T*IA*dN5T*N1Tm1*N2Tm2*dN4T0
+ *dN4T0-dN6T*IP*dN2T0*N1Tm1*ddN5T*N4Tm2*ddN0T0+ddN6T*IA*dN5T*N1Tm1*N2Tm2*
+ dN4T0
-ddN2T0*ddN6T*dN5T*dN1T0
*N4Tm2*MidP1+N5Tm1*dN6T*IP*dN1T0*N2Tm2*ddN0T0*ddN4T
-dN4T*N2Tm1*ddN6T*N5Tm2*IA*dN1T0+ddN2T0*dN6T*IP*dN0T0
@@ -2098,7 +2161,9 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+N6Tm2*N2Tm1*ddN4T0*dN5T*dN1T0
*FA+ddN6T*ddN4T0*IP*dN5T*dN0T0*N1Tm1*N2Tm2
-ddN2T0*ddN6T*IP*dN5T*dN0T0*N1Tm1*N4Tm2+N6Tm2*IA*dN2T0*dN5T
- *N1Tm1*ddN4T-ddN2T0*N4Tm1*dN6T*dN1T0*ddN5T*MidP2+dN4T*N6Tm2*N2Tm1*IA*dN1T0*ddN5T
+ *N1Tm1*ddN4T-ddN2T0*N4Tm1*dN6T*dN1T0*ddN5T*MidP2+dN4T*N6Tm2*N2Tm1*IA*
+ dN1T0*
+ ddN5T
+N6Tm2*ddN4T0*dN2T0*FS
*N1Tm1*ddN5T+ddN2T0*N4Tm1*N5Tm2*dN6T*dN1T0*FA
-N2Tm1*dN6T*ddN1T0*ddN5T*MidP2*dN4T0+ddN2T0*N5Tm2*IS*dN6T
@@ -2112,7 +2177,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+N5Tm1*ddN2T0*N6Tm2*dN1T0*dN7T*FP*ddN4T+dN4T
*N6Tm2*N2Tm1*IP*dN0T0*ddN1T0*ddN5T-ddN7T*N6Tm2*N2Tm1*ddN4T0*dN5T*dN1T0*FP
+ddN2T0*dN6T*N1Tm1*ddN5T*MidP2
- *dN4T0+ddN6T*ddN4T0*dN5T*dN1T0*MidP1*N2Tm2+N5Tm1*N6Tm2*dN2T0*FS*ddN1T0*ddN4T
+ *dN4T0+ddN6T*ddN4T0*dN5T*dN1T0*MidP1*N2Tm2+N5Tm1*N6Tm2*dN2T0*FS*ddN1T0*
+ ddN4T
-N2Tm1*ddN6T*ddN4T0*dN5T
*dN1T0*MidP2-N6Tm2*dN2T0*dN5T*ddN1T0*MidP1*ddN4T
-ddN7T*ddN2T0*N4Tm1*N5Tm2*dN6T*dN1T0*FP+N5Tm1*dN4T*ddN6T
@@ -2145,9 +2211,11 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-N2Tm1*IS*dN6T*ddN1T0*N3Tm2*ddN5T
-N5Tm1*ddN2T0*ddN6T*dN1T0*FS*N3Tm2-ddN6T*IP*dN2T0*dN5T*N1Tm1*N3Tm2*ddN0T0
-N3Tm1*ddN6T*IP*dN5T*dN1T0
- *N2Tm2*ddN0T0+N5Tm1*N6Tm2*dN3T*dN2T0*FA*ddN1T0-N3Tm1*ddN6T*N5Tm2*dN2T0*FS*ddN1T0
+ *N2Tm2*ddN0T0+N5Tm1*N6Tm2*dN3T*dN2T0*FA*ddN1T0-N3Tm1*ddN6T*N5Tm2*dN2T0*FS
+ *ddN1T0
-N2Tm1*ddN6T*IA*dN5T
- *dN1T0*N3Tm2+ddN6T*IA*dN2T0*dN5T*N1Tm1*N3Tm2+ddN2T0*ddN6T*N5Tm2*dN3T*IS*N1Tm1
+ *dN1T0*N3Tm2+ddN6T*IA*dN2T0*dN5T*N1Tm1*N3Tm2+ddN2T0*ddN6T*N5Tm2*dN3T*IS*
+ N1Tm1
+N3Tm1*ddN6T*N5Tm2*dN2T0
*dN7T*ddN1T0*FP-N2Tm1*ddN6T*N5Tm2*dN3T*IP*dN1T0*ddN0T0
-N3Tm1*ddN2T0*N6Tm2*dN1T0*FS*ddN5T+dN3T0*ddN2T0
@@ -2165,7 +2233,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+dN3T0*N5Tm1*dN6T*FA*ddN1T0*N2Tm2
+ddN3T0*N6Tm2*N2Tm1*dN1T0*FS*ddN5T-N5Tm1*dN6T*IP*dN1T0*ddN3T*N2Tm2*ddN0T0
+N3Tm1*ddN6T*IP*dN5T*dN0T0*ddN1T0
- *N2Tm2-N5Tm2*dN6T*dN2T0*ddN3T*ddN1T0*MidP1-ddN2T0*ddN6T*N5Tm2*dN3T*dN1T0*MidP1
+ *N2Tm2-N5Tm2*dN6T*dN2T0*ddN3T*ddN1T0*MidP1-ddN2T0*ddN6T*N5Tm2*dN3T*dN1T0*
+ MidP1
-ddN3T0*N5Tm2*dN6T*dN2T0*FA
*N1Tm1-ddN3T0*ddN7T*N6Tm2*dN2T0*dN5T*FP*N1Tm1
-N3Tm1*dN6T*dN2T0*ddN1T0*ddN5T*MidP2+N3Tm1*ddN2T0*N6Tm2*dN1T0
@@ -2179,13 +2248,15 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-N6Tm2*N2Tm1*dN3T*IP*dN0T0*ddN1T0
*ddN5T+N6Tm2*N2Tm1*IA*dN5T*dN1T0*ddN3T+ddN2T0*N6Tm2*IS*dN5T*ddN3T*N1Tm1
-ddN7T*N5Tm1*ddN2T0*dN6T*dN1T0
- *FP*N3Tm2+ddN3T0*ddN6T*IS*dN5T*N1Tm1*N2Tm2+N5Tm1*ddN2T0*ddN6T*dN3T*dN1T0*MidP2
+ *FP*N3Tm2+ddN3T0*ddN6T*IS*dN5T*N1Tm1*N2Tm2+N5Tm1*ddN2T0*ddN6T*dN3T*dN1T0*
+ MidP2
+N2Tm1*ddN6T*N5Tm2*IA*dN3T
*dN1T0-N5Tm1*dN6T*dN2T0*FA*ddN1T0*N3Tm2-N5Tm1*N6Tm2*dN2T0*ddN3T*FS*ddN1T0
+N3Tm1*N5Tm2*dN6T*dN2T0*FA*ddN1T0
-N3Tm1*ddN6T*IS*dN5T*ddN1T0*N2Tm2-N3Tm1*ddN2T0*ddN6T*dN5T*dN1T0*MidP2
-ddN3T0*ddN6T*N5Tm2*dN2T0*dN7T*FP
- *N1Tm1-dN3T0*N2Tm1*N5Tm2*dN6T*FA*ddN1T0-N5Tm1*ddN2T0*N6Tm2*dN1T0*ddN3T*dN7T*FP
+ *N1Tm1-dN3T0*N2Tm1*N5Tm2*dN6T*FA*ddN1T0-N5Tm1*ddN2T0*N6Tm2*dN1T0*ddN3T*
+ dN7T*FP
+N5Tm1*ddN6T*dN2T0*FS*ddN1T0
*N3Tm2+ddN3T0*dN6T*dN1T0*ddN5T*MidP1*N2Tm2
+ddN3T0*dN6T*IP*dN0T0*N1Tm1*ddN5T*N2Tm2-ddN2T0*dN6T*dN1T0*N3Tm2
@@ -2201,7 +2272,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+ddN6T*N5Tm2*dN3T*IP*dN2T0*N1Tm1*ddN0T0+dN3T0
*N6Tm2*N2Tm1*dN5T*FA*ddN1T0+ddN3T0*N2Tm1*ddN6T*dN5T*dN1T0*MidP2
+N2Tm1*ddN6T*N5Tm2*dN3T*IP*dN0T0*ddN1T0
- +N5Tm1*dN6T*dN2T0*ddN3T*ddN1T0*MidP2-N2Tm1*ddN6T*IP*dN5T*dN0T0*ddN1T0*N3Tm2
+ +N5Tm1*dN6T*dN2T0*ddN3T*ddN1T0*MidP2-N2Tm1*ddN6T*IP*dN5T*dN0T0*ddN1T0*
+ N3Tm2
-ddN2T0*dN6T*IP*dN0T0*N1Tm1
*N3Tm2*ddN5T+ddN3T0*ddN7T*N5Tm2*dN6T*dN2T0*FP*N1Tm1
+N5Tm1*dN6T*IP*ddN3T*dN0T0*ddN1T0*N2Tm2+ddN3T0*N6Tm2
@@ -2209,7 +2281,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+ddN7T*N3Tm1*ddN2T0*N5Tm2*dN6T*dN1T0*FP+ddN2T0
*N6Tm2*dN3T*dN1T0*ddN5T*MidP1+dN3T0*ddN6T*IP*dN5T*N1Tm1*N2Tm2*ddN0T0
+N3Tm1*ddN2T0*ddN6T*N5Tm2*dN1T0*FS
- +N6Tm2*dN2T0*dN5T*ddN3T*ddN1T0*MidP1+ddN7T*N5Tm1*dN6T*dN2T0*ddN1T0*FP*N3Tm2
+ +N6Tm2*dN2T0*dN5T*ddN3T*ddN1T0*MidP1+ddN7T*N5Tm1*dN6T*dN2T0*ddN1T0*FP*
+ N3Tm2
+dN3T0*ddN2T0*N5Tm2*dN6T*FA
*N1Tm1+ddN2T0*N5Tm2*dN6T*dN1T0*ddN3T*MidP1
+ddN3T0*ddN7T*N5Tm1*dN6T*dN1T0*FP*N2Tm2+N6Tm2*IP*dN2T0*dN5T
@@ -2241,7 +2314,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-N5Tm1*ddN2T0*N6Tm2*dN3T*dN1T0*FA+ddN7T
*N3Tm1*N6Tm2*dN2T0*dN5T*ddN1T0*FP-IA*dN6T*dN2T0*N1Tm1*N3Tm2*ddN5T
-N3Tm1*N6Tm2*dN2T0*dN7T*ddN1T0*FP*ddN5T
- +ddN3T0*N6Tm2*dN2T0*dN7T*FP*N1Tm1*ddN5T-ddN2T0*ddN6T*N5Tm2*dN3T*IP*dN0T0*N1Tm1
+ +ddN3T0*N6Tm2*dN2T0*dN7T*FP*N1Tm1*ddN5T-ddN2T0*ddN6T*N5Tm2*dN3T*IP*dN0T0*
+ N1Tm1
+N3Tm1*ddN2T0*N6Tm2
*dN5T*dN1T0*FA-ddN3T0*ddN7T*N2Tm1*N5Tm2*dN6T*dN1T0*FP
+N2Tm1*N5Tm2*dN6T*IP*dN1T0*ddN3T*ddN0T0-N6Tm2
@@ -2251,7 +2325,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-ddN3T0*ddN6T*dN2T0*dN5T*N1Tm1*MidP2)
/( N3Tm1*ddN6T*dN5T*ddN1T0*N2Tm2*dN4T0+N4Tm1*N5Tm2*dN6T*dN2T0*ddN3T*ddN1T0
-ddN2T0*ddN6T*N5Tm2*dN3T*N1Tm1
- *dN4T0+dN3T0*ddN2T0*N6Tm2*dN5T*N1Tm1*ddN4T-ddN2T0*N4Tm1*N6Tm2*dN3T*dN1T0*ddN5T
+ *dN4T0+dN3T0*ddN2T0*N6Tm2*dN5T*N1Tm1*ddN4T-ddN2T0*N4Tm1*N6Tm2*dN3T*dN1T0*
+ ddN5T
-N2Tm1*N5Tm2*dN6T*ddN3T
*ddN1T0*dN4T0-ddN3T0*dN6T*dN2T0*N1Tm1*ddN5T*N4Tm2
-ddN2T0*N4Tm1*ddN6T*dN5T*dN1T0*N3Tm2-N4Tm1*ddN6T
@@ -2261,7 +2336,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+N3Tm1*ddN2T0*N5Tm2*dN6T*dN1T0*ddN4T
-N5Tm1*dN4T*ddN2T0*N6Tm2*dN1T0*ddN3T-dN3T0*dN6T*ddN4T0*N1Tm1*ddN5T*N2Tm2
+ddN2T0*N6Tm2*dN3T*N1Tm1*ddN5T
- *dN4T0+N6Tm2*ddN4T0*dN2T0*dN5T*ddN3T*N1Tm1+N5Tm1*ddN6T*dN3T*dN2T0*ddN1T0*N4Tm2
+ *dN4T0+N6Tm2*ddN4T0*dN2T0*dN5T*ddN3T*N1Tm1+N5Tm1*ddN6T*dN3T*dN2T0*ddN1T0*
+ N4Tm2
+dN6T*ddN4T0*dN2T0*N1Tm1
*N3Tm2*ddN5T-N3Tm1*dN4T*ddN2T0*ddN6T*N5Tm2*dN1T0
+N3Tm1*dN6T*dN2T0*ddN1T0*ddN5T*N4Tm2-N3Tm1*ddN2T0*N6Tm2
@@ -2279,7 +2355,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+N4Tm1*ddN6T*dN2T0*dN5T*ddN1T0*N3Tm2
+N3Tm1*ddN2T0*ddN6T*dN5T*dN1T0*N4Tm2-ddN3T0*ddN6T*dN5T*N1Tm1*N2Tm2*dN4T0
-N3Tm1*dN6T*ddN1T0*ddN5T*N2Tm2
- *dN4T0+N5Tm1*ddN6T*dN3T*ddN4T0*dN1T0*N2Tm2-dN3T0*ddN2T0*N5Tm2*dN6T*N1Tm1*ddN4T
+ *dN4T0+N5Tm1*ddN6T*dN3T*ddN4T0*dN1T0*N2Tm2-dN3T0*ddN2T0*N5Tm2*dN6T*N1Tm1*
+ ddN4T
-N5Tm1*ddN2T0*ddN6T*dN3T
*dN1T0*N4Tm2+ddN3T0*N5Tm1*dN6T*dN1T0*N2Tm2*ddN4T
-ddN6T*ddN4T0*dN2T0*dN5T*N1Tm1*N3Tm2+dN3T0*N2Tm1*N5Tm2
@@ -2291,7 +2368,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-N5Tm2*dN6T*ddN4T0*dN2T0*ddN3T*N1Tm1
+ddN3T0*N5Tm2*dN6T*dN2T0*N1Tm1*ddN4T-N5Tm1*dN6T*dN2T0*ddN3T*ddN1T0*N4Tm2
+N3Tm1*dN4T*ddN6T*N5Tm2*dN2T0*
- ddN1T0-ddN3T0*N2Tm1*ddN6T*dN5T*dN1T0*N4Tm2+dN3T0*ddN6T*ddN4T0*dN5T*N1Tm1*N2Tm2
+ ddN1T0-ddN3T0*N2Tm1*ddN6T*dN5T*dN1T0*N4Tm2+dN3T0*ddN6T*ddN4T0*dN5T*N1Tm1*
+ N2Tm2
-N2Tm1*ddN6T*dN5T*ddN1T0
*N3Tm2*dN4T0+dN3T0*N5Tm1*dN4T*ddN6T*ddN1T0*N2Tm2
-N6Tm2*N2Tm1*dN3T*ddN1T0*ddN5T*dN4T0+N3Tm1*dN6T*ddN4T0
@@ -2303,7 +2381,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+ddN2T0*N4Tm1*N6Tm2*dN5T*dN1T0*ddN3T+
dN3T0*dN4T*ddN2T0*ddN6T*N5Tm2*N1Tm1-N5Tm1*dN4T*ddN6T*dN2T0*ddN1T0*N3Tm2
-ddN3T0*N5Tm1*dN4T*ddN6T*dN1T0*
- N2Tm2+ddN3T0*dN4T*N6Tm2*dN2T0*N1Tm1*ddN5T-N4Tm1*dN6T*dN2T0*ddN1T0*N3Tm2*ddN5T
+ N2Tm2+ddN3T0*dN4T*N6Tm2*dN2T0*N1Tm1*ddN5T-N4Tm1*dN6T*dN2T0*ddN1T0*N3Tm2*
+ ddN5T
-N2Tm1*ddN6T*N5Tm2*dN3T*
ddN4T0*dN1T0+ddN2T0*N4Tm1*dN6T*dN1T0*N3Tm2*ddN5T
+dN3T0*N2Tm1*ddN6T*dN5T*ddN1T0*N4Tm2+N5Tm1*ddN2T0*dN6T
@@ -2315,7 +2394,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-N3Tm1*dN4T*N6Tm2*dN2T0*ddN1T0*ddN5T
-dN3T0*N5Tm1*dN6T*ddN1T0*N2Tm2*ddN4T-N5Tm1*dN6T*ddN4T0*dN1T0*ddN3T*N2Tm2
+N6Tm2*N2Tm1*dN5T*ddN3T*ddN1T0
- *dN4T0-N3Tm1*ddN6T*dN2T0*dN5T*ddN1T0*N4Tm2+N2Tm1*ddN6T*ddN4T0*dN5T*dN1T0*N3Tm2
+ *dN4T0-N3Tm1*ddN6T*dN2T0*dN5T*ddN1T0*N4Tm2+N2Tm1*ddN6T*ddN4T0*dN5T*dN1T0*
+ N3Tm2
+N5Tm1*dN4T*ddN2T0*ddN6T
*dN1T0*N3Tm2+N2Tm1*dN6T*ddN1T0*N3Tm2*ddN5T*dN4T0
+dN3T0*N4Tm1*dN6T*ddN1T0*ddN5T*N2Tm2-dN3T0*dN4T*ddN2T0
@@ -2336,7 +2416,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+dN3T0*dN4T*N2Tm1*ddN6T*ddN1T0*MidP2+dN4T
*N6Tm2*N2Tm1*IP*dN1T0*ddN3T*ddN0T0-ddN3T0*N6Tm2*N2Tm1*dN1T0*FS*ddN4T
+dN3T0*N2Tm1*ddN6T*dN7T*ddN1T0*FP*
- N4Tm2-dN4T*ddN2T0*N6Tm2*IS*ddN3T*N1Tm1+ddN3T0*ddN7T*dN6T*FP*N1Tm1*N2Tm2*dN4T0
+ N4Tm2-dN4T*ddN2T0*N6Tm2*IS*ddN3T*N1Tm1+ddN3T0*ddN7T*dN6T*FP*N1Tm1*N2Tm2*
+ dN4T0
+dN4T*ddN2T0*ddN6T*IS
*N1Tm1*N3Tm2-ddN6T*dN3T*ddN4T0*dN2T0*N1Tm1*MidP2
-N3Tm1*dN6T*dN2T0*FA*ddN1T0*N4Tm2+ddN3T0*N2Tm1*dN6T
@@ -2390,7 +2471,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+dN4T*N6Tm2*N2Tm1*IS*ddN3T*ddN1T0
-ddN3T0*dN6T*dN2T0*N1Tm1*MidP2*ddN4T-ddN6T*IA*dN3T*N1Tm1*N2Tm2*dN4T0
-ddN3T0*dN4T*N2Tm1*ddN6T*dN1T0*
- MidP2+ddN2T0*ddN6T*dN3T*dN1T0*N4Tm2*MidP1-ddN3T0*dN4T*N6Tm2*dN2T0*FA*N1Tm1
+ MidP2+ddN2T0*ddN6T*dN3T*dN1T0*N4Tm2*MidP1-ddN3T0*dN4T*N6Tm2*dN2T0*FA*
+ N1Tm1
-ddN2T0*N4Tm1*ddN6T*dN3T*
dN1T0*MidP2-N3Tm1*ddN6T*dN2T0*dN7T*ddN1T0*FP*N4Tm2
-N2Tm1*ddN6T*IA*dN3T*dN1T0*N4Tm2+N4Tm1*ddN6T*dN2T0
@@ -2398,7 +2480,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+dN4T*N6Tm2*IA*dN2T0*ddN3T*N1Tm1+ddN3T0*
ddN7T*N2Tm1*dN6T*dN1T0*FP*N4Tm2+ddN3T0*ddN6T*dN2T0*dN7T*FP*N1Tm1*N4Tm2
+N2Tm1*dN6T*ddN4T0*dN1T0*FA*N3Tm2
- -ddN3T0*N2Tm1*ddN6T*dN1T0*dN7T*FP*N4Tm2-ddN2T0*N6Tm2*dN3T*IP*dN0T0*N1Tm1*ddN4T
+ -ddN3T0*N2Tm1*ddN6T*dN1T0*dN7T*FP*N4Tm2-ddN2T0*N6Tm2*dN3T*IP*dN0T0*N1Tm1*
+ ddN4T
+dN4T*ddN2T0*N6Tm2*IP
*ddN3T*dN0T0*N1Tm1-N2Tm1*dN6T*FA*ddN1T0*N3Tm2*dN4T0
-N4Tm1*dN6T*dN2T0*ddN3T*ddN1T0*MidP2+N3Tm1*dN4T
@@ -2426,9 +2509,12 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+ddN2T0*N6Tm2*ddN3T*FS*N1Tm1*dN4T0
-N2Tm1*IS*dN6T*ddN3T*ddN1T0*N4Tm2-ddN3T0*dN6T*FA*N1Tm1*N2Tm2*dN4T0
-ddN6T*ddN4T0*dN2T0*dN7T*FP*N1Tm1
- *N3Tm2+dN6T*dN2T0*ddN3T*ddN1T0*N4Tm2*MidP1-ddN2T0*IS*dN6T*N1Tm1*N3Tm2*ddN4T
+ *N3Tm2+dN6T*dN2T0*ddN3T*ddN1T0*N4Tm2*MidP1-ddN2T0*IS*dN6T*N1Tm1*N3Tm2*
+ ddN4T
-dN3T0*N2Tm1*dN6T*ddN1T0
- *MidP2*ddN4T+N4Tm1*N6Tm2*dN2T0*ddN3T*FS*ddN1T0-IS*dN6T*ddN4T0*ddN3T*N1Tm1*N2Tm2
+ *MidP2*ddN4T+N4Tm1*N6Tm2*dN2T0*ddN3T*FS*ddN1T0-IS*dN6T*ddN4T0*ddN3T*
+ N1Tm1*
+ N2Tm2
-ddN3T0*dN6T*IP*dN0T0
*N1Tm1*N2Tm2*ddN4T+N4Tm1*dN6T*dN2T0*FA*ddN1T0*N3Tm2
+ddN2T0*dN6T*dN1T0*N3Tm2*MidP1*ddN4T+ddN2T0*N6Tm2
@@ -2440,13 +2526,15 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+N3Tm1*ddN2T0*ddN6T*dN1T0*dN7T*FP*N4Tm2+N2Tm1
*ddN6T*ddN4T0*dN1T0*dN7T*FP*N3Tm2+ddN3T0*IS*dN6T*N1Tm1*N2Tm2*ddN4T
+ddN3T0*N6Tm2*dN2T0*FS*N1Tm1*ddN4T
- -dN4T*N6Tm2*IP*dN2T0*ddN3T*N1Tm1*ddN0T0-dN3T0*N6Tm2*N2Tm1*dN7T*ddN1T0*FP*ddN4T
+ -dN4T*N6Tm2*IP*dN2T0*ddN3T*N1Tm1*ddN0T0-dN3T0*N6Tm2*N2Tm1*dN7T*ddN1T0*FP*
+ ddN4T
+N6Tm2*N2Tm1*ddN3T*dN7T
*ddN1T0*FP*dN4T0+dN6T*ddN4T0*dN2T0*ddN3T*N1Tm1*MidP2
-ddN6T*dN3T*IP*dN2T0*N1Tm1*N4Tm2*ddN0T0-ddN3T0*
N2Tm1*dN6T*dN1T0*FA*N4Tm2-ddN2T0*N6Tm2*ddN3T*dN7T*FP*N1Tm1*dN4T0
+ddN3T0*N2Tm1*ddN6T*dN1T0*FS*N4Tm2
- +ddN2T0*ddN6T*dN3T*N1Tm1*MidP2*dN4T0-dN3T0*ddN7T*dN4T*ddN2T0*N6Tm2*FP*N1Tm1
+ +ddN2T0*ddN6T*dN3T*N1Tm1*MidP2*dN4T0-dN3T0*ddN7T*dN4T*ddN2T0*N6Tm2*FP*
+ N1Tm1
+ddN3T0*ddN7T*dN4T*N6Tm2
*dN2T0*FP*N1Tm1-N3Tm1*dN4T*ddN6T*dN2T0*ddN1T0*MidP2
+N4Tm1*dN6T*IP*dN1T0*ddN3T*N2Tm2*ddN0T0-dN3T0*N4Tm1
@@ -2462,9 +2550,11 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-N2Tm1*dN6T*ddN4T0*dN1T0*ddN3T*MidP2+dN3T0
*dN4T*ddN6T*IA*N1Tm1*N2Tm2+N3Tm1*dN6T*IP*dN0T0*ddN1T0*N2Tm2*ddN4T
+N4Tm1*ddN6T*dN3T*dN2T0*ddN1T0*MidP2
- -dN3T0*N4Tm1*ddN6T*dN7T*ddN1T0*FP*N2Tm2-dN4T*ddN2T0*ddN6T*IP*dN0T0*N1Tm1*N3Tm2
+ -dN3T0*N4Tm1*ddN6T*dN7T*ddN1T0*FP*N2Tm2-dN4T*ddN2T0*ddN6T*IP*dN0T0*N1Tm1*
+ N3Tm2
+ddN3T0*N4Tm1*dN6T*dN1T0
- *FA*N2Tm2+ddN2T0*N4Tm1*N6Tm2*dN3T*dN1T0*FA-dN3T0*dN4T*ddN2T0*ddN6T*N1Tm1*MidP2
+ *FA*N2Tm2+ddN2T0*N4Tm1*N6Tm2*dN3T*dN1T0*FA-dN3T0*dN4T*ddN2T0*ddN6T*N1Tm1*
+ MidP2
+dN6T*ddN4T0*IP*ddN3T
*dN0T0*N1Tm1*N2Tm2+dN3T0*ddN2T0*ddN6T*FS*N1Tm1*N4Tm2
-dN3T0*dN4T*N6Tm2*N2Tm1*FA*ddN1T0+ddN3T0*N6Tm2
@@ -2486,24 +2576,28 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-dN3T0*ddN7T*N2Tm1*dN6T*ddN1T0*FP*N4Tm2
-dN4T*N6Tm2*N2Tm1*IP*ddN3T*dN0T0*ddN1T0+N3Tm1*ddN6T*ddN4T0*dN1T0*FS*N2Tm2
-dN3T0*IA*dN6T*N1Tm1*N2Tm2
- *ddN4T+N2Tm1*IA*dN6T*dN1T0*ddN3T*N4Tm2+ddN3T0*dN4T*ddN6T*dN1T0*MidP1*N2Tm2
+ *ddN4T+N2Tm1*IA*dN6T*dN1T0*ddN3T*N4Tm2+ddN3T0*dN4T*ddN6T*dN1T0*MidP1*
+ N2Tm2
-ddN3T0*dN6T*dN1T0*MidP1
*N2Tm2*ddN4T-N3Tm1*ddN6T*FS*ddN1T0*N2Tm2*dN4T0
+ddN7T*N2Tm1*dN6T*ddN1T0*FP*N3Tm2*dN4T0+ddN6T*dN3T
*ddN1T0*MidP1*N2Tm2*dN4T0+ddN3T0*N4Tm1*ddN6T*dN1T0*dN7T*FP*N2Tm2
-N2Tm1*dN6T*IP*dN1T0*ddN3T*N4Tm2
- *ddN0T0-N2Tm1*ddN6T*dN3T*IP*dN0T0*ddN1T0*N4Tm2+ddN6T*IA*dN3T*dN2T0*N1Tm1*N4Tm2
+ *ddN0T0-N2Tm1*ddN6T*dN3T*IP*dN0T0*ddN1T0*N4Tm2+ddN6T*IA*dN3T*dN2T0*N1Tm1*
+ N4Tm2
+dN3T0*ddN7T*N4Tm1
*dN6T*ddN1T0*FP*N2Tm2+ddN7T*ddN2T0*N6Tm2*dN3T*FP*N1Tm1*dN4T0
+N4Tm1*ddN6T*IA*dN3T*dN1T0*N2Tm2-N3Tm1
*ddN2T0*dN6T*dN1T0*MidP2*ddN4T-dN4T*N2Tm1*ddN6T*IP*dN1T0*N3Tm2*ddN0T0
+dN3T0*dN6T*ddN1T0*MidP1*N2Tm2
- *ddN4T+N3Tm1*dN6T*FA*ddN1T0*N2Tm2*dN4T0+ddN2T0*dN6T*IP*dN0T0*N1Tm1*N3Tm2*ddN4T
+ *ddN4T+N3Tm1*dN6T*FA*ddN1T0*N2Tm2*dN4T0+ddN2T0*dN6T*IP*dN0T0*N1Tm1*N3Tm2*
+ ddN4T
-N3Tm1*dN4T*ddN6T*IA
*dN1T0*N2Tm2)
/( N3Tm1*ddN6T*dN5T*ddN1T0*N2Tm2*dN4T0+N4Tm1*N5Tm2*dN6T*dN2T0*ddN3T*ddN1T0
-ddN2T0*ddN6T*N5Tm2*dN3T*N1Tm1
- *dN4T0+dN3T0*ddN2T0*N6Tm2*dN5T*N1Tm1*ddN4T-ddN2T0*N4Tm1*N6Tm2*dN3T*dN1T0*ddN5T
+ *dN4T0+dN3T0*ddN2T0*N6Tm2*dN5T*N1Tm1*ddN4T-ddN2T0*N4Tm1*N6Tm2*dN3T*dN1T0*
+ ddN5T
-N2Tm1*N5Tm2*dN6T*ddN3T
*ddN1T0*dN4T0-ddN3T0*dN6T*dN2T0*N1Tm1*ddN5T*N4Tm2
-ddN2T0*N4Tm1*ddN6T*dN5T*dN1T0*N3Tm2-N4Tm1*ddN6T*N5Tm2
@@ -2515,7 +2609,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+ddN2T0*N6Tm2*dN3T*N1Tm1*ddN5T*dN4T0
+N6Tm2*ddN4T0*dN2T0*dN5T*ddN3T*N1Tm1+N5Tm1*ddN6T*dN3T*dN2T0*ddN1T0*N4Tm2
+dN6T*ddN4T0*dN2T0*N1Tm1*N3Tm2
- *ddN5T-N3Tm1*dN4T*ddN2T0*ddN6T*N5Tm2*dN1T0+N3Tm1*dN6T*dN2T0*ddN1T0*ddN5T*N4Tm2
+ *ddN5T-N3Tm1*dN4T*ddN2T0*ddN6T*N5Tm2*dN1T0+N3Tm1*dN6T*dN2T0*ddN1T0*ddN5T*
+ N4Tm2
-N3Tm1*ddN2T0*N6Tm2*dN5T
*dN1T0*ddN4T+N2Tm1*N5Tm2*dN6T*ddN4T0*dN1T0*ddN3T
+N5Tm1*ddN2T0*N6Tm2*dN3T*dN1T0*ddN4T+N5Tm1*dN6T*ddN3T
@@ -2527,7 +2622,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+dN3T0*ddN2T0*dN6T*N1Tm1*ddN5T*N4Tm2
+ddN3T0*N2Tm1*dN6T*dN1T0*ddN5T*N4Tm2+N3Tm1*N6Tm2*dN2T0*dN5T*ddN1T0*ddN4T
-ddN3T0*N4Tm1*dN6T*dN1T0*ddN5T
- *N2Tm2+N4Tm1*N6Tm2*dN3T*dN2T0*ddN1T0*ddN5T+N4Tm1*ddN6T*dN2T0*dN5T*ddN1T0*N3Tm2
+ *N2Tm2+N4Tm1*N6Tm2*dN3T*dN2T0*ddN1T0*ddN5T+N4Tm1*ddN6T*dN2T0*dN5T*ddN1T0*
+ N3Tm2
+N3Tm1*ddN2T0*ddN6T*dN5T
*dN1T0*N4Tm2-ddN3T0*ddN6T*dN5T*N1Tm1*N2Tm2*dN4T0
-N3Tm1*dN6T*ddN1T0*ddN5T*N2Tm2*dN4T0+N5Tm1*ddN6T*dN3T
@@ -2539,7 +2635,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-N5Tm1*ddN6T*dN3T*ddN1T0*N2Tm2*dN4T0
-N3Tm1*N5Tm2*dN6T*dN2T0*ddN1T0*ddN4T+ddN3T0*N4Tm1*ddN6T*dN5T*dN1T0*N2Tm2
-N3Tm1*ddN2T0*dN6T*dN1T0*ddN5T
- *N4Tm2-N2Tm1*dN6T*ddN4T0*dN1T0*N3Tm2*ddN5T-N5Tm2*dN6T*ddN4T0*dN2T0*ddN3T*N1Tm1
+ *N4Tm2-N2Tm1*dN6T*ddN4T0*dN1T0*N3Tm2*ddN5T-N5Tm2*dN6T*ddN4T0*dN2T0*ddN3T*
+ N1Tm1
+ddN3T0*N5Tm2*dN6T*dN2T0
*N1Tm1*ddN4T-N5Tm1*dN6T*dN2T0*ddN3T*ddN1T0*N4Tm2
+N3Tm1*dN4T*ddN6T*N5Tm2*dN2T0*ddN1T0-ddN3T0*N2Tm1*ddN6T
@@ -2551,7 +2648,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+N3Tm1*dN4T*ddN2T0*N6Tm2*dN1T0*ddN5T
-N6Tm2*N2Tm1*ddN4T0*dN5T*dN1T0*ddN3T-N5Tm1*ddN2T0*dN6T*dN1T0*N3Tm2*ddN4T
-ddN3T0*dN4T*ddN6T*N5Tm2*dN2T0
- *N1Tm1+ddN2T0*N5Tm2*dN6T*ddN3T*N1Tm1*dN4T0+ddN2T0*N4Tm1*N6Tm2*dN5T*dN1T0*ddN3T
+ *N1Tm1+ddN2T0*N5Tm2*dN6T*ddN3T*N1Tm1*dN4T0+ddN2T0*N4Tm1*N6Tm2*dN5T*dN1T0*
+ ddN3T
+dN3T0*dN4T*ddN2T0*ddN6T
*N5Tm2*N1Tm1-N5Tm1*dN4T*ddN6T*dN2T0*ddN1T0*N3Tm2
-ddN3T0*N5Tm1*dN4T*ddN6T*dN1T0*N2Tm2+ddN3T0*dN4T*N6Tm2
@@ -2563,7 +2661,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-ddN2T0*dN6T*N1Tm1*N3Tm2*ddN5T*dN4T0
-N5Tm1*N6Tm2*dN3T*dN2T0*ddN1T0*ddN4T-N3Tm1*ddN6T*ddN4T0*dN5T*dN1T0*N2Tm2
-ddN3T0*N2Tm1*N5Tm2*dN6T*dN1T0
- *ddN4T+ddN2T0*N4Tm1*ddN6T*N5Tm2*dN3T*dN1T0-N3Tm1*dN4T*N6Tm2*dN2T0*ddN1T0*ddN5T
+ *ddN4T+ddN2T0*N4Tm1*ddN6T*N5Tm2*dN3T*dN1T0-N3Tm1*dN4T*N6Tm2*dN2T0*ddN1T0*
+ ddN5T
-dN3T0*N5Tm1*dN6T*ddN1T0
*N2Tm2*ddN4T-N5Tm1*dN6T*ddN4T0*dN1T0*ddN3T*N2Tm2
+N6Tm2*N2Tm1*dN5T*ddN3T*ddN1T0*dN4T0-N3Tm1*ddN6T*dN2T0
@@ -2576,7 +2675,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
*ddN5T*N4Tm2-N4Tm1*N6Tm2*dN2T0*dN5T*ddN3T*ddN1T0
+N6Tm2*N2Tm1*dN3T*ddN4T0*dN1T0*ddN5T);
m_control_points[6]=
- -( ddN7T*N5Tm2*dN3T*ddN4T0*dN2T0*FP*N1Tm1+dN3T0*ddN7T*dN4T*ddN2T0*N5Tm2*FP*N1Tm1
+ -( ddN7T*N5Tm2*dN3T*ddN4T0*dN2T0*FP*N1Tm1+dN3T0*ddN7T*dN4T*ddN2T0*N5Tm2*FP*
+ N1Tm1
+IP*dN5T*ddN3T*N1Tm1
*N2Tm2*ddN0T0*dN4T0+dN3T*ddN4T0*dN2T0*N1Tm1*ddN5T*MidP2
-N5Tm1*dN2T0*FS*ddN1T0*N3Tm2*ddN4T+ddN3T0
@@ -2606,7 +2706,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+ddN3T0*ddN7T*N4Tm1*dN5T*dN1T0*FP
*N2Tm2-N3Tm1*dN5T*FA*ddN1T0*N2Tm2*dN4T0-N5Tm1*dN4T*ddN2T0*dN1T0*FA*N3Tm2
+dN3T0*dN4T*N2Tm1*N5Tm2*FA
- *ddN1T0-dN3T0*dN5T*ddN1T0*MidP1*N2Tm2*ddN4T+N4Tm1*IA*dN5T*dN1T0*ddN3T*N2Tm2
+ *ddN1T0-dN3T0*dN5T*ddN1T0*MidP1*N2Tm2*ddN4T+N4Tm1*IA*dN5T*dN1T0*ddN3T*
+ N2Tm2
-dN3T0*ddN7T*ddN2T0*dN5T
*FP*N1Tm1*N4Tm2-N4Tm1*N5Tm2*dN2T0*ddN3T*FS*ddN1T0
-ddN3T0*ddN7T*N2Tm1*dN5T*dN1T0*FP*N4Tm2-ddN3T0*dN4T
@@ -2622,7 +2723,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-N3Tm1*ddN2T0*N5Tm2*dN1T0*FS*ddN4T
-N5Tm1*dN3T*ddN4T0*dN1T0*FA*N2Tm2-ddN7T*N5Tm1*dN4T*dN2T0*ddN1T0*FP*N3Tm2
-ddN2T0*N5Tm2*ddN3T*FS*N1Tm1
- *dN4T0-dN3T0*IP*dN5T*N1Tm1*N2Tm2*ddN0T0*ddN4T-N3Tm1*dN2T0*FS*ddN1T0*ddN5T*N4Tm2
+ *dN4T0-dN3T0*IP*dN5T*N1Tm1*N2Tm2*ddN0T0*ddN4T-N3Tm1*dN2T0*FS*ddN1T0*ddN5T
+ *N4Tm2
+ddN3T0*dN4T*IS*N1Tm1
*ddN5T*N2Tm2+N2Tm1*ddN4T0*dN5T*dN1T0*ddN3T*MidP2
+N3Tm1*N5Tm2*dN2T0*FS*ddN1T0*ddN4T-N3Tm1*ddN4T0*dN1T0
@@ -2634,7 +2736,9 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+ddN7T*N3Tm1*dN4T*N5Tm2*dN2T0*ddN1T0*FP-dN3T0
*N5Tm1*dN7T*ddN1T0*FP*N2Tm2*ddN4T-dN3T0*N2Tm1*N5Tm2*FS*ddN1T0*ddN4T
-ddN2T0*dN5T*dN1T0*N3Tm2*MidP1
- *ddN4T-N5Tm1*ddN2T0*dN1T0*ddN3T*FS*N4Tm2-ddN2T0*IP*dN5T*dN0T0*N1Tm1*N3Tm2*ddN4T
+ *ddN4T-N5Tm1*ddN2T0*dN1T0*ddN3T*FS*N4Tm2-ddN2T0*IP*dN5T*dN0T0*N1Tm1*
+ N3Tm2*
+ ddN4T
+dN4T*N2Tm1*N5Tm2*IP
*ddN3T*dN0T0*ddN1T0+N2Tm1*IP*dN5T*dN1T0*ddN3T*N4Tm2*ddN0T0
+dN3T0*N4Tm1*dN7T*ddN1T0*FP*ddN5T*N2Tm2-N2Tm1
@@ -2642,7 +2746,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-ddN2T0*dN7T*FP*N1Tm1*N3Tm2*ddN5T*dN4T0
+dN4T*ddN2T0*N5Tm2*IS*ddN3T*N1Tm1+N5Tm1*dN4T*IP*dN1T0*ddN3T*N2Tm2*ddN0T0
+dN3T0*ddN7T*N5Tm1*dN4T*ddN1T0
- *FP*N2Tm2+N2Tm1*IA*dN3T*dN1T0*ddN5T*N4Tm2-N5Tm2*dN3T*ddN4T0*dN2T0*FA*N1Tm1
+ *FP*N2Tm2+N2Tm1*IA*dN3T*dN1T0*ddN5T*N4Tm2-N5Tm2*dN3T*ddN4T0*dN2T0*FA*
+ N1Tm1
-ddN7T*ddN2T0*N5Tm2*dN3T*FP
*N1Tm1*dN4T0+ddN3T0*dN5T*FA*N1Tm1*N2Tm2*dN4T0
+ddN2T0*N4Tm1*dN1T0*dN7T*FP*N3Tm2*ddN5T+N3Tm1*dN4T*IP*dN0T0
@@ -2666,7 +2771,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+dN3T0*dN4T*ddN2T0*N1Tm1*ddN5T
*MidP2-dN3T0*N4Tm1*FS*ddN1T0*ddN5T*N2Tm2-N5Tm1*dN3T*IS*ddN1T0*N2Tm2*ddN4T
+N4Tm1*N5Tm2*dN3T*dN2T0*FA
- *ddN1T0+dN3T0*N2Tm1*FS*ddN1T0*ddN5T*N4Tm2-ddN4T0*dN5T*dN1T0*ddN3T*MidP1*N2Tm2
+ *ddN1T0+dN3T0*N2Tm1*FS*ddN1T0*ddN5T*N4Tm2-ddN4T0*dN5T*dN1T0*ddN3T*MidP1*
+ N2Tm2
-dN4T*ddN2T0*N5Tm2*dN1T0
*ddN3T*MidP1+ddN7T*N5Tm1*dN3T*ddN4T0*dN1T0*FP*N2Tm2
-ddN3T0*N5Tm1*dN1T0*FS*N2Tm2*ddN4T+N2Tm1*ddN4T0
@@ -2686,7 +2792,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-dN4T*N2Tm1*N5Tm2*IS*ddN3T*ddN1T0+N3Tm1
*ddN2T0*dN1T0*FS*ddN5T*N4Tm2-dN2T0*dN5T*ddN3T*ddN1T0*N4Tm2*MidP1
-N5Tm1*ddN4T0*dN1T0*ddN3T*dN7T*FP*N2Tm2
- +N2Tm1*N5Tm2*ddN4T0*dN1T0*ddN3T*dN7T*FP+N4Tm1*dN3T*IP*dN1T0*ddN5T*N2Tm2*ddN0T0
+ +N2Tm1*N5Tm2*ddN4T0*dN1T0*ddN3T*dN7T*FP+N4Tm1*dN3T*IP*dN1T0*ddN5T*N2Tm2*
+ ddN0T0
+ddN7T*N2Tm1*N5Tm2*dN3T
*ddN1T0*FP*dN4T0+dN4T*N2Tm1*IS*ddN1T0*N3Tm2*ddN5T
-ddN2T0*N4Tm1*N5Tm2*dN1T0*ddN3T*dN7T*FP+dN5T*ddN3T
@@ -2698,21 +2805,26 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-N3Tm1*dN7T*ddN1T0*FP*ddN5T*N2Tm2*dN4T0+ddN7T*N2Tm1
*ddN4T0*dN5T*dN1T0*FP*N3Tm2+ddN4T0*dN2T0*dN7T*FP*N1Tm1*N3Tm2*ddN5T
+N3Tm1*dN2T0*dN5T*FA*ddN1T0*N4Tm2
- -ddN7T*N2Tm1*N5Tm2*dN3T*ddN4T0*dN1T0*FP+IP*dN2T0*dN5T*N1Tm1*N3Tm2*ddN0T0*ddN4T
+ -ddN7T*N2Tm1*N5Tm2*dN3T*ddN4T0*dN1T0*FP+IP*dN2T0*dN5T*N1Tm1*N3Tm2*ddN0T0*
+ ddN4T
+N5Tm1*ddN2T0*dN1T0
*ddN3T*dN7T*FP*N4Tm2+ddN2T0*dN5T*ddN3T*N1Tm1*MidP2*dN4T0
-N5Tm1*dN2T0*ddN3T*dN7T*ddN1T0*FP*N4Tm2
+N3Tm1*ddN2T0*dN5T*dN1T0*MidP2*ddN4T-ddN2T0*N4Tm1*N5Tm2*dN3T*dN1T0*FA
+dN4T*N2Tm1*N5Tm2*IA*dN1T0
- *ddN3T-dN3T0*ddN7T*N4Tm1*dN5T*ddN1T0*FP*N2Tm2-dN3T*IS*ddN4T0*N1Tm1*ddN5T*N2Tm2
+ *ddN3T-dN3T0*ddN7T*N4Tm1*dN5T*ddN1T0*FP*N2Tm2-dN3T*IS*ddN4T0*N1Tm1*ddN5T*
+ N2Tm2
-dN4T*N2Tm1*N5Tm2
*IP*dN1T0*ddN3T*ddN0T0+ddN7T*N3Tm1*dN5T*ddN1T0*FP*N2Tm2*dN4T0
-ddN7T*N3Tm1*ddN4T0*dN5T*dN1T0*FP*N2Tm2
- -dN3T0*ddN4T0*dN7T*FP*N1Tm1*ddN5T*N2Tm2+ddN3T0*ddN7T*dN4T*N2Tm1*N5Tm2*dN1T0*FP
+ -dN3T0*ddN4T0*dN7T*FP*N1Tm1*ddN5T*N2Tm2+ddN3T0*ddN7T*dN4T*N2Tm1*N5Tm2*
+ dN1T0*FP
+N5Tm1*ddN2T0*dN3T*dN1T0
- *FA*N4Tm2+N4Tm1*dN2T0*FS*ddN1T0*N3Tm2*ddN5T-N2Tm1*dN3T*ddN4T0*dN1T0*ddN5T*MidP2
+ *FA*N4Tm2+N4Tm1*dN2T0*FS*ddN1T0*N3Tm2*ddN5T-N2Tm1*dN3T*ddN4T0*dN1T0*
+ ddN5T*MidP2
+IA*dN2T0*dN5T*ddN3T
- *N1Tm1*N4Tm2+N5Tm1*dN3T*FA*ddN1T0*N2Tm2*dN4T0+N2Tm1*N5Tm2*ddN3T*FS*ddN1T0*dN4T0
+ *N1Tm1*N4Tm2+N5Tm1*dN3T*FA*ddN1T0*N2Tm2*dN4T0+N2Tm1*N5Tm2*ddN3T*FS*ddN1T0
+ *dN4T0
+dN3T*dN2T0*ddN1T0
*ddN5T*N4Tm2*MidP1-dN3T*IP*N1Tm1*ddN5T*N2Tm2*ddN0T0*dN4T0
+ddN3T0*ddN7T*dN2T0*dN5T*FP*N1Tm1*N4Tm2
@@ -2734,7 +2846,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-ddN7T*N3Tm1*dN4T*ddN2T0*N5Tm2*dN1T0*FP-N3Tm1
*N5Tm2*dN2T0*dN7T*ddN1T0*FP*ddN4T-ddN4T0*dN2T0*dN5T*ddN3T*N1Tm1*MidP2
-ddN4T0*IP*dN5T*ddN3T*dN0T0*N1Tm1
- *N2Tm2+ddN3T0*N2Tm1*dN5T*dN1T0*FA*N4Tm2-N3Tm1*dN4T*IP*dN1T0*ddN5T*N2Tm2*ddN0T0
+ *N2Tm2+ddN3T0*N2Tm1*dN5T*dN1T0*FA*N4Tm2-N3Tm1*dN4T*IP*dN1T0*ddN5T*N2Tm2*
+ ddN0T0
-N2Tm1*IP*dN5T*dN1T0*N3Tm2
*ddN0T0*ddN4T+ddN7T*N4Tm1*dN2T0*dN5T*ddN1T0*FP*N3Tm2
-N2Tm1*dN3T*IS*ddN1T0*ddN5T*N4Tm2-ddN3T0*FS*N1Tm1
@@ -2750,11 +2863,14 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+ddN2T0*N5Tm2*dN3T*FA*N1Tm1*dN4T0
+N2Tm1*dN3T*IP*dN0T0*ddN1T0*ddN5T*N4Tm2-IA*dN3T*dN2T0*N1Tm1*ddN5T*N4Tm2
-IP*dN2T0*dN5T*ddN3T*N1Tm1*N4Tm2
- *ddN0T0-N2Tm1*ddN4T0*dN5T*dN1T0*FA*N3Tm2-dN4T*dN2T0*ddN1T0*N3Tm2*ddN5T*MidP1
+ *ddN0T0-N2Tm1*ddN4T0*dN5T*dN1T0*FA*N3Tm2-dN4T*dN2T0*ddN1T0*N3Tm2*ddN5T*
+ MidP1
-N4Tm1*IA*dN3T*dN1T0*ddN5T
- *N2Tm2-dN4T*N5Tm2*IA*dN2T0*ddN3T*N1Tm1+ddN2T0*dN5T*dN1T0*ddN3T*N4Tm2*MidP1
+ *N2Tm2-dN4T*N5Tm2*IA*dN2T0*ddN3T*N1Tm1+ddN2T0*dN5T*dN1T0*ddN3T*N4Tm2*
+ MidP1
+N4Tm1*IP*dN5T*ddN3T*dN0T0*ddN1T0
- *N2Tm2+dN3T0*N2Tm1*dN5T*ddN1T0*MidP2*ddN4T+ddN2T0*IS*dN5T*N1Tm1*N3Tm2*ddN4T
+ *N2Tm2+dN3T0*N2Tm1*dN5T*ddN1T0*MidP2*ddN4T+ddN2T0*IS*dN5T*N1Tm1*N3Tm2*
+ ddN4T
-ddN3T0*dN2T0*dN7T*FP*N1Tm1
*ddN5T*N4Tm2-ddN4T0*dN2T0*FS*N1Tm1*N3Tm2*ddN5T
-N5Tm2*ddN4T0*dN2T0*ddN3T*dN7T*FP*N1Tm1-N5Tm1*dN3T*IP*dN1T0
@@ -2774,7 +2890,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-ddN3T0*ddN7T*dN4T*N5Tm2*dN2T0*FP*N1Tm1
-ddN3T0*IS*dN5T*N1Tm1*N2Tm2*ddN4T+N3Tm1*dN4T*IA*dN1T0*ddN5T*N2Tm2
-N4Tm1*dN3T*dN2T0*ddN1T0*ddN5T*MidP2
- -ddN3T0*dN4T*dN1T0*ddN5T*MidP1*N2Tm2-dN3T0*ddN2T0*N5Tm2*dN7T*FP*N1Tm1*ddN4T
+ -ddN3T0*dN4T*dN1T0*ddN5T*MidP1*N2Tm2-dN3T0*ddN2T0*N5Tm2*dN7T*FP*N1Tm1*
+ ddN4T
+N3Tm1*ddN4T0*dN1T0*dN7T*FP
*ddN5T*N2Tm2+N2Tm1*IA*dN5T*dN1T0*N3Tm2*ddN4T
-ddN2T0*dN3T*IP*dN0T0*N1Tm1*ddN5T*N4Tm2+N2Tm1*dN7T*ddN1T0*FP
@@ -2785,7 +2902,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
*N5Tm1*dN4T*dN1T0*FP*N2Tm2)
/( N3Tm1*ddN6T*dN5T*ddN1T0*N2Tm2*dN4T0+N4Tm1*N5Tm2*dN6T*dN2T0*ddN3T*ddN1T0
-ddN2T0*ddN6T*N5Tm2*dN3T*N1Tm1
- *dN4T0+dN3T0*ddN2T0*N6Tm2*dN5T*N1Tm1*ddN4T-ddN2T0*N4Tm1*N6Tm2*dN3T*dN1T0*ddN5T
+ *dN4T0+dN3T0*ddN2T0*N6Tm2*dN5T*N1Tm1*ddN4T-ddN2T0*N4Tm1*N6Tm2*dN3T*dN1T0*
+ ddN5T
-N2Tm1*N5Tm2*dN6T*ddN3T
*ddN1T0*dN4T0-ddN3T0*dN6T*dN2T0*N1Tm1*ddN5T*N4Tm2
-ddN2T0*N4Tm1*ddN6T*dN5T*dN1T0*N3Tm2-N4Tm1*ddN6T*N5Tm2
@@ -2797,7 +2915,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+ddN2T0*N6Tm2*dN3T*N1Tm1*ddN5T*dN4T0
+N6Tm2*ddN4T0*dN2T0*dN5T*ddN3T*N1Tm1+N5Tm1*ddN6T*dN3T*dN2T0*ddN1T0*N4Tm2
+dN6T*ddN4T0*dN2T0*N1Tm1*N3Tm2
- *ddN5T-N3Tm1*dN4T*ddN2T0*ddN6T*N5Tm2*dN1T0+N3Tm1*dN6T*dN2T0*ddN1T0*ddN5T*N4Tm2
+ *ddN5T-N3Tm1*dN4T*ddN2T0*ddN6T*N5Tm2*dN1T0+N3Tm1*dN6T*dN2T0*ddN1T0*ddN5T
+ *N4Tm2
-N3Tm1*ddN2T0*N6Tm2*dN5T
*dN1T0*ddN4T+N2Tm1*N5Tm2*dN6T*ddN4T0*dN1T0*ddN3T
+N5Tm1*ddN2T0*N6Tm2*dN3T*dN1T0*ddN4T+N5Tm1*dN6T*ddN3T*
@@ -2809,7 +2928,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+dN3T0*ddN2T0*dN6T*N1Tm1*ddN5T*N4Tm2+
ddN3T0*N2Tm1*dN6T*dN1T0*ddN5T*N4Tm2+N3Tm1*N6Tm2*dN2T0*dN5T*ddN1T0*ddN4T
-ddN3T0*N4Tm1*dN6T*dN1T0*ddN5T*
- N2Tm2+N4Tm1*N6Tm2*dN3T*dN2T0*ddN1T0*ddN5T+N4Tm1*ddN6T*dN2T0*dN5T*ddN1T0*N3Tm2
+ N2Tm2+N4Tm1*N6Tm2*dN3T*dN2T0*ddN1T0*ddN5T+N4Tm1*ddN6T*dN2T0*dN5T*ddN1T0
+ *N3Tm2
+N3Tm1*ddN2T0*ddN6T*dN5T*
dN1T0*N4Tm2-ddN3T0*ddN6T*dN5T*N1Tm1*N2Tm2*dN4T0
-N3Tm1*dN6T*ddN1T0*ddN5T*N2Tm2*dN4T0+N5Tm1*ddN6T*dN3T*
@@ -2821,7 +2941,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-N5Tm1*ddN6T*dN3T*ddN1T0*N2Tm2*dN4T0-
N3Tm1*N5Tm2*dN6T*dN2T0*ddN1T0*ddN4T+ddN3T0*N4Tm1*ddN6T*dN5T*dN1T0*N2Tm2
-N3Tm1*ddN2T0*dN6T*dN1T0*ddN5T
- *N4Tm2-N2Tm1*dN6T*ddN4T0*dN1T0*N3Tm2*ddN5T-N5Tm2*dN6T*ddN4T0*dN2T0*ddN3T*N1Tm1
+ *N4Tm2-N2Tm1*dN6T*ddN4T0*dN1T0*N3Tm2*ddN5T-N5Tm2*dN6T*ddN4T0*dN2T0*ddN3T*
+ N1Tm1
+ddN3T0*N5Tm2*dN6T*dN2T0
*N1Tm1*ddN4T-N5Tm1*dN6T*dN2T0*ddN3T*ddN1T0*N4Tm2
+N3Tm1*dN4T*ddN6T*N5Tm2*dN2T0*ddN1T0-ddN3T0*N2Tm1*ddN6T
@@ -2833,7 +2954,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
+N3Tm1*dN4T*ddN2T0*N6Tm2*dN1T0*ddN5T
-N6Tm2*N2Tm1*ddN4T0*dN5T*dN1T0*ddN3T-N5Tm1*ddN2T0*dN6T*dN1T0*N3Tm2*ddN4T
-ddN3T0*dN4T*ddN6T*N5Tm2*dN2T0
- *N1Tm1+ddN2T0*N5Tm2*dN6T*ddN3T*N1Tm1*dN4T0+ddN2T0*N4Tm1*N6Tm2*dN5T*dN1T0*ddN3T
+ *N1Tm1+ddN2T0*N5Tm2*dN6T*ddN3T*N1Tm1*dN4T0+ddN2T0*N4Tm1*N6Tm2*dN5T*dN1T0
+ *ddN3T
+dN3T0*dN4T*ddN2T0*ddN6T
*N5Tm2*N1Tm1-N5Tm1*dN4T*ddN6T*dN2T0*ddN1T0*N3Tm2
-ddN3T0*N5Tm1*dN4T*ddN6T*dN1T0*N2Tm2+ddN3T0*dN4T*N6Tm2
@@ -2845,7 +2967,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-ddN2T0*dN6T*N1Tm1*N3Tm2*ddN5T*dN4T0
-N5Tm1*N6Tm2*dN3T*dN2T0*ddN1T0*ddN4T-N3Tm1*ddN6T*ddN4T0*dN5T*dN1T0*N2Tm2
-ddN3T0*N2Tm1*N5Tm2*dN6T*dN1T0
- *ddN4T+ddN2T0*N4Tm1*ddN6T*N5Tm2*dN3T*dN1T0-N3Tm1*dN4T*N6Tm2*dN2T0*ddN1T0*ddN5T
+ *ddN4T+ddN2T0*N4Tm1*ddN6T*N5Tm2*dN3T*dN1T0-N3Tm1*dN4T*N6Tm2*dN2T0*ddN1T0
+ *ddN5T
-dN3T0*N5Tm1*dN6T*ddN1T0
*N2Tm2*ddN4T-N5Tm1*dN6T*ddN4T0*dN1T0*ddN3T*N2Tm2
+N6Tm2*N2Tm1*dN5T*ddN3T*ddN1T0*dN4T0-N3Tm1*ddN6T*dN2T0
@@ -2855,7 +2978,8 @@ void BSplinesFoot::ComputeControlPointFrom4DataPoint(void)
-dN3T0*dN4T*ddN2T0*N6Tm2*N1Tm1*ddN5T+ddN3T0
*ddN6T*dN2T0*dN5T*N1Tm1*N4Tm2+N5Tm1*dN4T*N6Tm2*dN2T0*ddN3T*ddN1T0
-dN3T0*N2Tm1*dN6T*ddN1T0*ddN5T*N4Tm2
- -N4Tm1*N6Tm2*dN2T0*dN5T*ddN3T*ddN1T0+N6Tm2*N2Tm1*dN3T*ddN4T0*dN1T0*ddN5T);
+ -N4Tm1*N6Tm2*dN2T0*dN5T*ddN3T*ddN1T0+N6Tm2*N2Tm1*dN3T*ddN4T0*dN1T0*
+ ddN5T);
m_control_points[7]=FP;
return ;
diff --git a/src/Mathematics/Bsplines.hh b/src/Mathematics/Bsplines.hh
index e9419c79738d46c3bc53760d0e69e96b8f9630d6..4e6ea8b1b2e0249ba18509897edb400a3fe8c314 100644
--- a/src/Mathematics/Bsplines.hh
+++ b/src/Mathematics/Bsplines.hh
@@ -1,5 +1,6 @@
/** \file Bsplines.h
- \brief Bsplines object for generating trajectoire from set of Points given. */
+ \brief Bsplines object for generating trajectoire
+ from set of Points given. */
#ifndef _BSPLINES_H_
diff --git a/src/Mathematics/ConvexHull.hh b/src/Mathematics/ConvexHull.hh
index 526d0ebd7327cd2587ed9f4499b54c7022b802a1..8a32adb3e831f554b0f6ef8b31f87a1253c575a3 100644
--- a/src/Mathematics/ConvexHull.hh
+++ b/src/Mathematics/ConvexHull.hh
@@ -52,8 +52,10 @@ namespace PatternGeneratorJRL
/*! Compute the convex hull
by applying Graham's algorithm.
- @param aVecOfPoints: The set of 2D points on which the convex hull is computed.
- @param TheConvexHull: The set of 2D points which give the convex hull. */
+ @param aVecOfPoints:
+ The set of 2D points on which the convex hull is computed.
+ @param TheConvexHull:
+ The set of 2D points which give the convex hull. */
void DoComputeConvexHull(std::vector<CH_Point> aVecOfPoints,
std::vector<CH_Point> &TheConvexHull);
diff --git a/src/Mathematics/FootConstraintsAsLinearSystem.cpp b/src/Mathematics/FootConstraintsAsLinearSystem.cpp
index c23f3f7ba6306666536e1f975fc9999fedd9f770..e2852ed997311ad373794ac917150b59b4f69402 100644
--- a/src/Mathematics/FootConstraintsAsLinearSystem.cpp
+++ b/src/Mathematics/FootConstraintsAsLinearSystem.cpp
@@ -40,9 +40,10 @@ using namespace PatternGeneratorJRL;
-FootConstraintsAsLinearSystem::FootConstraintsAsLinearSystem(
- SimplePluginManager *aSPM,
- PinocchioRobot *aPR) :
+FootConstraintsAsLinearSystem::
+FootConstraintsAsLinearSystem
+(SimplePluginManager *aSPM,
+ PinocchioRobot *aPR) :
SimplePlugin(aSPM)
{
m_PR = aPR;
@@ -53,8 +54,10 @@ FootConstraintsAsLinearSystem::~FootConstraintsAsLinearSystem()
{
}
-int FootConstraintsAsLinearSystem::FindSimilarConstraints(Eigen::MatrixXd &A,
- vector<int> &SimilarConstraints)
+int FootConstraintsAsLinearSystem::
+FindSimilarConstraints
+(Eigen::MatrixXd &A,
+ vector<int> &SimilarConstraints)
{
SimilarConstraints.resize(A.rows());
@@ -194,8 +197,9 @@ ComputeLinearSystem
C(1) /= (double)n;
- ODEBUG("(x["<< n-1 << "],y["<< n-1 << "]): " << aVecOfPoints[n-1].col << " " <<
- aVecOfPoints[n-1].row << " "
+ ODEBUG("(x["<< n-1 << "],y["<< n-1 << "]): "
+ << aVecOfPoints[n-1].col << " "
+ << aVecOfPoints[n-1].row << " "
<< aVecOfPoints[0].col << " " << aVecOfPoints[0].row );
if (fabs(aVecOfPoints[0].col-aVecOfPoints[n-1].col)>1e-7)
@@ -268,15 +272,16 @@ ComputeLinearSystem
return 0;
}
-int FootConstraintsAsLinearSystem::BuildLinearConstraintInequalities(
- deque<FootAbsolutePosition>
- &LeftFootAbsolutePositions,
- deque<FootAbsolutePosition>
- &RightFootAbsolutePositions,
- deque<LinearConstraintInequality_t *> &
- QueueOfLConstraintInequalities,
- double ConstraintOnX,
- double ConstraintOnY)
+int FootConstraintsAsLinearSystem::
+BuildLinearConstraintInequalities
+(deque<FootAbsolutePosition>
+ &LeftFootAbsolutePositions,
+ deque<FootAbsolutePosition>
+ &RightFootAbsolutePositions,
+ deque<LinearConstraintInequality_t *> &
+ QueueOfLConstraintInequalities,
+ double ConstraintOnX,
+ double ConstraintOnY)
{
// Find the convex hull for each of the position,
// in order to create the corresponding trajectory.
@@ -410,13 +415,18 @@ int FootConstraintsAsLinearSystem::BuildLinearConstraintInequalities(
// Computes the maxima.
- xmin = aVecOfPoints[j].col < xmin ? aVecOfPoints[j].col : xmin;
- xmax = aVecOfPoints[j].col > xmax ? aVecOfPoints[j].col : xmax;
- ymin = aVecOfPoints[j].row < ymin ? aVecOfPoints[j].row : ymin;
- ymax = aVecOfPoints[j].row > ymax ? aVecOfPoints[j].row : ymax;
+ xmin = aVecOfPoints[j].col < xmin ?
+ aVecOfPoints[j].col : xmin;
+ xmax = aVecOfPoints[j].col > xmax ?
+ aVecOfPoints[j].col : xmax;
+ ymin = aVecOfPoints[j].row < ymin ?
+ aVecOfPoints[j].row : ymin;
+ ymax = aVecOfPoints[j].row > ymax ?
+ aVecOfPoints[j].row : ymax;
}
- ODEBUG("State 3-1 " << xmin << " " << xmax << " " << ymin << " " << ymax);
+ ODEBUG("State 3-1 " << xmin << " " << xmax
+ << " " << ymin << " " << ymax);
lx=RightFootAbsolutePositions[i].x;
ly=RightFootAbsolutePositions[i].y;
@@ -428,21 +438,28 @@ int FootConstraintsAsLinearSystem::BuildLinearConstraintInequalities(
RightFootAbsolutePositions[i].theta);
for(unsigned j=0; j<4; j++)
{
- aVecOfPoints[j+4].col = lx + ( lxcoefs[j] * lRightFootHalfWidth
- * c_t - lycoefs[j] *
- lRightFootHalfHeight * s_t );
- aVecOfPoints[j+4].row = ly + ( lxcoefs[j] * lRightFootHalfWidth
- * s_t + lycoefs[j] *
- lRightFootHalfHeight * c_t );
+ aVecOfPoints[j+4].col =
+ lx + ( lxcoefs[j] * lRightFootHalfWidth
+ * c_t - lycoefs[j] *
+ lRightFootHalfHeight * s_t );
+ aVecOfPoints[j+4].row =
+ ly + ( lxcoefs[j] * lRightFootHalfWidth
+ * s_t + lycoefs[j] *
+ lRightFootHalfHeight * c_t );
// Computes the maxima.
- xmin = aVecOfPoints[j+4].col < xmin ? aVecOfPoints[j+4].col : xmin;
- xmax = aVecOfPoints[j+4].col > xmax ? aVecOfPoints[j+4].col : xmax;
- ymin = aVecOfPoints[j+4].row < ymin ? aVecOfPoints[j+4].row : ymin;
- ymax = aVecOfPoints[j+4].row > ymax ? aVecOfPoints[j+4].row : ymax;
+ xmin = aVecOfPoints[j+4].col < xmin ?
+ aVecOfPoints[j+4].col : xmin;
+ xmax = aVecOfPoints[j+4].col > xmax ?
+ aVecOfPoints[j+4].col : xmax;
+ ymin = aVecOfPoints[j+4].row < ymin ?
+ aVecOfPoints[j+4].row : ymin;
+ ymax = aVecOfPoints[j+4].row > ymax ?
+ aVecOfPoints[j+4].row : ymax;
}
- ODEBUG("State 3-2" << xmin << " " << xmax << " " << ymin << " " << ymax);
+ ODEBUG("State 3-2" << xmin << " " << xmax
+ << " " << ymin << " " << ymax);
aCH.DoComputeConvexHull(aVecOfPoints,TheConvexHull);
}
// In the second case, it is necessary to compute
@@ -453,7 +470,8 @@ int FootConstraintsAsLinearSystem::BuildLinearConstraintInequalities(
TheConvexHull.resize(4);
// Who is support foot ?
- if (LeftFootAbsolutePositions[i].z < RightFootAbsolutePositions[i].z)
+ if (LeftFootAbsolutePositions[i].z <
+ RightFootAbsolutePositions[i].z)
{
lx=LeftFootAbsolutePositions[i].x;
ly=LeftFootAbsolutePositions[i].y;
@@ -469,10 +487,14 @@ int FootConstraintsAsLinearSystem::BuildLinearConstraintInequalities(
( lxcoefs[j] * lLeftFootHalfWidth * s_t +
lycoefs[j] * lLeftFootHalfHeight * c_t );
// Computes the maxima.
- xmin = TheConvexHull[j].col < xmin ? TheConvexHull[j].col : xmin;
- xmax = TheConvexHull[j].col > xmax ? TheConvexHull[j].col : xmax;
- ymin = TheConvexHull[j].row < ymin ? TheConvexHull[j].row : ymin;
- ymax = TheConvexHull[j].row > ymax ? TheConvexHull[j].row : ymax;
+ xmin = TheConvexHull[j].col < xmin ?
+ TheConvexHull[j].col : xmin;
+ xmax = TheConvexHull[j].col > xmax ?
+ TheConvexHull[j].col : xmax;
+ ymin = TheConvexHull[j].row < ymin ?
+ TheConvexHull[j].row : ymin;
+ ymax = TheConvexHull[j].row > ymax ?
+ TheConvexHull[j].row : ymax;
}
ODEBUG("Left support foot");
@@ -485,29 +507,37 @@ int FootConstraintsAsLinearSystem::BuildLinearConstraintInequalities(
c_t = cos(RightFootAbsolutePositions[i].theta*M_PI/180.0);
for(unsigned j=0; j<4; j++)
{
- TheConvexHull[j].col = lx + ( lxcoefs[j] *
- lRightFootHalfWidth * c_t -
- lycoefs[j] *
- lRightFootHalfHeight * s_t );
- TheConvexHull[j].row = ly + ( lxcoefs[j] *
- lRightFootHalfWidth * s_t +
- lycoefs[j] *
- lRightFootHalfHeight * c_t );
+ TheConvexHull[j].col =
+ lx + ( lxcoefs[j] *
+ lRightFootHalfWidth * c_t -
+ lycoefs[j] *
+ lRightFootHalfHeight * s_t );
+ TheConvexHull[j].row =
+ ly + ( lxcoefs[j] *
+ lRightFootHalfWidth * s_t +
+ lycoefs[j] *
+ lRightFootHalfHeight * c_t );
// Computes the maxima.
- xmin = TheConvexHull[j].col < xmin ? TheConvexHull[j].col : xmin;
- xmax = TheConvexHull[j].col > xmax ? TheConvexHull[j].col : xmax;
- ymin = TheConvexHull[j].row < ymin ? TheConvexHull[j].row : ymin;
- ymax = TheConvexHull[j].row > ymax ? TheConvexHull[j].row : ymax;
+ xmin = TheConvexHull[j].col < xmin ?
+ TheConvexHull[j].col : xmin;
+ xmax = TheConvexHull[j].col > xmax ?
+ TheConvexHull[j].col : xmax;
+ ymin = TheConvexHull[j].row < ymin ?
+ TheConvexHull[j].row : ymin;
+ ymax = TheConvexHull[j].row > ymax ?
+ TheConvexHull[j].row : ymax;
}
ODEBUG("Right support foot");
}
- ODEBUG("State !=3 " << xmin << " " << xmax << " " << ymin << " " << ymax);
+ ODEBUG("State !=3 " << xmin << " " << xmax << " "
+ << ymin << " " << ymax);
}
// Linear Constraint Inequality
- LinearConstraintInequality_t * aLCI = new LinearConstraintInequality_t;
+ LinearConstraintInequality_t * aLCI =
+ new LinearConstraintInequality_t;
// Building those constraints.
ComputeLinearSystem(TheConvexHull,aLCI->A, aLCI->B, aLCI->Center);
// Finding the similar one (i.e. Ai identical).
@@ -518,8 +548,10 @@ int FootConstraintsAsLinearSystem::BuildLinearConstraintInequalities(
{
QueueOfLConstraintInequalities.back()->EndingTime =
LeftFootAbsolutePositions[i].time;
- ODEBUG4( QueueOfLConstraintInequalities.back()->StartingTime << " " <<
- QueueOfLConstraintInequalities.back()->EndingTime << " " <<
+ ODEBUG4( QueueOfLConstraintInequalities.back()->StartingTime
+ << " " <<
+ QueueOfLConstraintInequalities.back()->EndingTime
+ << " " <<
prev_xmin << " " <<
prev_xmax << " " <<
prev_ymin << " " <<
@@ -542,8 +574,10 @@ int FootConstraintsAsLinearSystem::BuildLinearConstraintInequalities(
{
QueueOfLConstraintInequalities.back()->EndingTime =
LeftFootAbsolutePositions[i].time;
- ODEBUG4( QueueOfLConstraintInequalities.back()->StartingTime << " " <<
- QueueOfLConstraintInequalities.back()->EndingTime << " " <<
+ ODEBUG4( QueueOfLConstraintInequalities.back()->StartingTime
+ << " " <<
+ QueueOfLConstraintInequalities.back()->EndingTime
+ << " " <<
prev_xmin << " " <<
prev_xmax << " " <<
prev_ymin << " " <<
diff --git a/src/Mathematics/FootConstraintsAsLinearSystem.hh b/src/Mathematics/FootConstraintsAsLinearSystem.hh
index 6187997cca8182b1b25ae72a1eb5291b28984cdf..617ac0d7c7aef99dc26e58af3032af132c083018 100644
--- a/src/Mathematics/FootConstraintsAsLinearSystem.hh
+++ b/src/Mathematics/FootConstraintsAsLinearSystem.hh
@@ -60,8 +60,10 @@ namespace PatternGeneratorJRL
/*! Destructor */
~FootConstraintsAsLinearSystem();
- /*! Compute the linear system \f${\bf A}{\bf x} \geq {\bf b}\f$ associated with the
- set of points specified by aVecOfPoints. aVecOfPoints is supposed to represent
+ /*! Compute the linear system \f${\bf A}{\bf x} \geq {\bf b}\f$
+ associated with the
+ set of points specified by aVecOfPoints. aVecOfPoints is
+ supposed to represent
the convex hull of the robot contact points with the ground.
*/
int ComputeLinearSystem(std::vector<CH_Point> aVecOfPoints,
@@ -69,25 +71,28 @@ namespace PatternGeneratorJRL
Eigen::MatrixXd &B,
Eigen::VectorXd &C);
- /*! Build a queue of constraint Inequalities based on a list of Foot Absolute
- Position.
+ /*! Build a queue of constraint Inequalities based on a list of
+ Foot Absolute Position.
*/
- int BuildLinearConstraintInequalities(std::deque< FootAbsolutePosition>
- &LeftFootAbsolutePositions,
- std::deque<FootAbsolutePosition> &RightFootAbsolutePositions,
- std::deque<LinearConstraintInequality_t *> &
- QueueOfLConstraintInequalities,
- double ConstraintOnX,
- double ConstraintOnY);
-
- /*! Build a queue of constraint Inequalities based on a list of Foot Absolute Position. */
- int BuildLinearConstraintInequalities2(std::deque< FootAbsolutePosition>
- &LeftFootAbsolutePositions,
- std::deque<FootAbsolutePosition> &RightFootAbsolutePositions,
- std::deque<LinearConstraintInequality_t *> &
- QueueOfLConstraintInequalities,
- double ConstraintOnX,
- double ConstraintOnY);
+ int BuildLinearConstraintInequalities
+ (std::deque< FootAbsolutePosition>
+ &LeftFootAbsolutePositions,
+ std::deque<FootAbsolutePosition> &RightFootAbsolutePositions,
+ std::deque<LinearConstraintInequality_t *> &
+ QueueOfLConstraintInequalities,
+ double ConstraintOnX,
+ double ConstraintOnY);
+
+ /*! Build a queue of constraint Inequalities based on a list
+ of Foot Absolute Position. */
+ int BuildLinearConstraintInequalities2
+ (std::deque< FootAbsolutePosition>
+ &LeftFootAbsolutePositions,
+ std::deque<FootAbsolutePosition> &RightFootAbsolutePositions,
+ std::deque<LinearConstraintInequality_t *> &
+ QueueOfLConstraintInequalities,
+ double ConstraintOnX,
+ double ConstraintOnY);
/*! Find Similar Constraints */
int FindSimilarConstraints(Eigen::MatrixXd &A,
diff --git a/src/Mathematics/OptCholesky.cpp b/src/Mathematics/OptCholesky.cpp
index 791b88979ce3578e5b1774f1ea5c12511e4cc417..a1362ee445bb7cdd0c346c3f0ab8f81fa08d0512 100644
--- a/src/Mathematics/OptCholesky.cpp
+++ b/src/Mathematics/OptCholesky.cpp
@@ -59,7 +59,9 @@ void OptCholesky::SetToZero()
if (m_NbMaxOfConstraints!=0)
{
if (m_L!=0)
- for(unsigned int i=0; i<m_NbMaxOfConstraints * m_NbMaxOfConstraints; i++)
+ for(unsigned int i=0;
+ i<m_NbMaxOfConstraints * m_NbMaxOfConstraints;
+ i++)
m_L[i]=0.0;
diff --git a/src/Mathematics/PLDPHerdt.cpp b/src/Mathematics/PLDPHerdt.cpp
index ede071dc274659dc9f0f2210834b5a41157988ec..639b2afaba1ed6cf6e60e096dfb5141761e0a896 100644
--- a/src/Mathematics/PLDPHerdt.cpp
+++ b/src/Mathematics/PLDPHerdt.cpp
@@ -636,15 +636,6 @@ ComputeAlpha
//Check if we can not reuse an already computed result
{
bool ToBeComputed=true;
- // if (SimilarConstraint[li]!=0)
- // {
- // int lindex = li+SimilarConstraint[li];
- // if (m_ConstraintsValueComputed[lindex])
- // {
- // m_tmp1[li] = -m_tmp1[lindex];
- // ToBeComputed=false;
- // }
- // }
if(ToBeComputed)
for(unsigned lj=0; lj<2*(m_CardV+NumberSteps); lj++)
@@ -664,15 +655,6 @@ ComputeAlpha
// Check if we can not reuse an already computed result
{
bool ToBeComputed=true;
- // if (SimilarConstraint[li]!=0)
- // {
- // int lindex = li+SimilarConstraint[li];
- // if (m_ConstraintsValueComputed[lindex+m_NbOfConstraints])
- // {
- // m_tmp2[li] += -m_tmp2[lindex]-m_b[lindex];
- // ToBeComputed=false;
- // }
- // }
if(ToBeComputed)
for(unsigned lj=0; lj<2*(m_CardV+NumberSteps); lj++)
@@ -684,9 +666,11 @@ ComputeAlpha
if (m_tmp2[li]>m_tol)
{
- std::cerr << "PB ON constraint "<<li<< " at time " << m_InternalTime << endl;
+ std::cerr << "PB ON constraint "<<li
+ << " at time " << m_InternalTime << endl;
std::cerr << " Check current V k="<<m_ItNb<< endl;
- std::cerr << " should be feasible : " << m_tmp2[li]<< " " << -m_v2[li] << endl;
+ std::cerr << " should be feasible : " << m_tmp2[li]
+ << " " << -m_v2[li] << endl;
}
else if (m_tmp2[li]>0.0)
m_tmp2[li] = -m_tol;
@@ -716,18 +700,23 @@ ComputeAlpha
return Alpha;
}
-int PLDPSolverHerdt::SolveProblem(deque<LinearConstraintInequalityFreeFeet_t> &
- QueueOfLConstraintInequalitiesFreeFeet,
- deque<SupportFeet_t> & QueueOfSupportFeet,
- double *CstPartOfTheCostFunction,
- unsigned int NbOfConstraints,
- double *LinearPartOfConstraints,
- double *CstPartOfConstraints,
- double *XkYk,
- double *X,
- unsigned int NumberOfRemovedConstraints, unsigned int NbRemovedFootCstr,
- bool StartingSequence,unsigned int NumberSteps, bool CurrentStateChanged,
- double time)
+int PLDPSolverHerdt::
+SolveProblem
+(deque<LinearConstraintInequalityFreeFeet_t> &
+ QueueOfLConstraintInequalitiesFreeFeet,
+ deque<SupportFeet_t> & QueueOfSupportFeet,
+ double *CstPartOfTheCostFunction,
+ unsigned int NbOfConstraints,
+ double *LinearPartOfConstraints,
+ double *CstPartOfConstraints,
+ double *XkYk,
+ double *X,
+ unsigned int NumberOfRemovedConstraints,
+ unsigned int NbRemovedFootCstr,
+ bool StartingSequence,
+ unsigned int NumberSteps,
+ bool CurrentStateChanged,
+ double time)
{
vector<unsigned int> NewActivatedConstraints;
if (StartingSequence)
@@ -837,12 +826,14 @@ int PLDPSolverHerdt::SolveProblem(deque<LinearConstraintInequalityFreeFeet_t> &
{
if(m_PreviouslyActivatedConstraints[i] < 4*m_CardV)//ZMP constraints
{
- lindex=m_PreviouslyActivatedConstraints[i]-NumberOfRemovedConstraints;
+ lindex=m_PreviouslyActivatedConstraints[i]-
+ NumberOfRemovedConstraints;
}
else if(m_PreviouslyActivatedConstraints[i] >= 4*m_CardV
&& CurrentStateChanged == true)//Foot placement constraints
{
- IndexFootCstr = m_PreviouslyActivatedConstraints[i]-NbRemovedFootCstr;
+ IndexFootCstr = m_PreviouslyActivatedConstraints[i]-
+ NbRemovedFootCstr;
}
m_ActivatedConstraints.push_back(lindex);
@@ -918,7 +909,8 @@ int PLDPSolverHerdt::SolveProblem(deque<LinearConstraintInequalityFreeFeet_t> &
if (ContinueAlgo)
{
- ODEBUG("Nb of activated constraints: " << NewActivatedConstraints.size());
+ ODEBUG("Nb of activated constraints: "
+ << NewActivatedConstraints.size());
m_OptCholesky->AddActiveConstraints(NewActivatedConstraints);
for(unsigned int i=0; i<NewActivatedConstraints.size(); i++)
m_ActivatedConstraints.push_back(NewActivatedConstraints[i]);
@@ -975,10 +967,12 @@ int PLDPSolverHerdt::SolveProblem(deque<LinearConstraintInequalityFreeFeet_t> &
ODEBUG("AR (" << lTime <<") :" );
for( unsigned int i=0; i<m_ActivatedConstraints.size(); i++)
{
- ODEBUG( "( " << m_ActivatedConstraints[i] << " , " << m_v2[i] << " ) ");
+ ODEBUG( "( " << m_ActivatedConstraints[i] << " , "
+ << m_v2[i] << " ) ");
if (m_v2[i]<0.0)
{
- m_PreviouslyActivatedConstraints.push_back(m_ActivatedConstraints[i]);
+ m_PreviouslyActivatedConstraints.
+ push_back(m_ActivatedConstraints[i]);
ODEBUG(m_ActivatedConstraints[i] << " " );
}
}
@@ -1063,42 +1057,15 @@ int PLDPSolverHerdt::SolveProblem(deque<LinearConstraintInequalityFreeFeet_t> &
ODEBUG6(m_ActivatedConstraints.size() << " "
<< NbOfConstraints << " "
- << m_ActivatedConstraints.size() - m_PreviouslyActivatedConstraints.size() <<
- " "
+ << m_ActivatedConstraints.size() -
+ m_PreviouslyActivatedConstraints.size()
+ << " "
<< m_ItNb,(char*)Buffer.c_str());
m_InternalTime += 0.02;
return 0;
}
-/* Store the ZMP solution for hotstart. */
-// void PLDPSolverHerdt::StoreCurrentZMPSolution(double *XkYk)
-// {
-// // The current solution is Vk,
-// // but its graphical representation is better understood
-// // when transformed in ZMP ref trajectory.
-
-// for(unsigned int i=0;i<m_CardV;i++)
-// {
-// m_InitialZMPSolution[i] = 0.0; // X axis
-// m_InitialZMPSolution[i+m_CardV] =0.0; // Y axis
-// for(unsigned int j=0;j<m_CardV;j++)
-// {
-// m_InitialZMPSolution[i] += m_Pu[j*m_CardV+i] * m_Vk[j];
-// m_InitialZMPSolution[i+m_CardV] += m_Pu[j*m_CardV+i] * m_Vk[j+m_CardV];
-// }
-// for(unsigned int j=0;j<3;j++)
-// {
-// m_InitialZMPSolution[i] += m_Px[i*3+j] * XkYk[j];
-// // lZMP[i] += m_Px[i*3+j] * XkYk[j+3];
-// // lZMP[i+m_CardV] += m_Px[i*3+j] * XkYk[j];
-// m_InitialZMPSolution[i+m_CardV] += m_Px[i*3+j] * XkYk[j+3];
-// }
-// // aof << lZMP[i] << " " << lZMP[i+m_CardV] << endl;
-// }
-
-// // aof.close();
-// }
/* Write the current solution for debugging. */
void PLDPSolverHerdt::WriteCurrentZMPSolution(string filename,
diff --git a/src/Mathematics/PLDPSolver.cpp b/src/Mathematics/PLDPSolver.cpp
index b1e51904c12103a5e0c29b4265f1f189ace2041a..b27a189d4f8595f8aaed95e000732771192903d1 100644
--- a/src/Mathematics/PLDPSolver.cpp
+++ b/src/Mathematics/PLDPSolver.cpp
@@ -48,7 +48,8 @@
#define isnan _isnan
//definition of isinf for win32
-//src: http://www.gnu.org/software/libtool/manual/autoconf/Function-Portability.html
+//src: http://www.gnu.org/software/libtool/manual/autoconf/
+//Function-Portability.html
inline int isinf (double x)
{
return isnan (x - x);
@@ -95,9 +96,10 @@ PLDPSolver::PLDPSolver(unsigned int CardU,
m_NbMaxOfConstraints = 8*m_CardV;
m_NbOfConstraints = 0;
- m_OptCholesky = new PatternGeneratorJRL::OptCholesky(m_NbMaxOfConstraints,
- 2*m_CardV,
- OptCholesky::MODE_FORTRAN);
+ m_OptCholesky = new PatternGeneratorJRL::
+ OptCholesky(m_NbMaxOfConstraints,
+ 2*m_CardV,
+ OptCholesky::MODE_FORTRAN);
string Buffer("InfosPLDP");
if (m_HotStart)
@@ -317,8 +319,10 @@ int PLDPSolver::ComputeInitialSolution(double *ZMPRef,
m_Vk[i]+= m_iPu[(m_CardV-1)*m_CardV+i] * ZMPRef[m_CardV-1];
for(unsigned int j=0; j<m_CardV-1; j++)
- m_Vk[i+m_CardV]+= m_iPu[j*m_CardV+i] * m_PreviousZMPSolution[j+m_CardV+1];
- m_Vk[i+m_CardV]+= m_iPu[(m_CardV-1)*m_CardV+i] * ZMPRef[m_CardV-1+m_CardV];
+ m_Vk[i+m_CardV]+= m_iPu[j*m_CardV+i] *
+ m_PreviousZMPSolution[j+m_CardV+1];
+ m_Vk[i+m_CardV]+= m_iPu[(m_CardV-1)*m_CardV+i] *
+ ZMPRef[m_CardV-1+m_CardV];
}
@@ -626,9 +630,11 @@ double PLDPSolver::ComputeAlpha(vector<unsigned int> & NewActivatedConstraints,
if (m_tmp2[li]>m_tol)
{
- std::cerr << "PB ON constraint "<<li<< " at time " << m_InternalTime << endl;
+ std::cerr << "PB ON constraint "<<li<< " at time "
+ << m_InternalTime << endl;
std::cerr << " Check current V k="<<m_ItNb<< endl;
- std::cerr << " should be faisable : " << m_tmp2[li]<< " " << -m_v2[li] << endl;
+ std::cerr << " should be faisable : " << m_tmp2[li]<< " "
+ << -m_v2[li] << endl;
}
else if (m_tmp2[li]>0.0)
m_tmp2[li] = -m_tol;
@@ -789,7 +795,8 @@ int PLDPSolver::SolveProblem(double *CstPartOfTheCostFunction,
{
for(unsigned int i=0; i<m_PreviouslyActivatedConstraints.size(); i++)
{
- int lindex=m_PreviouslyActivatedConstraints[i]-NumberOfRemovedConstraints;
+ int lindex=m_PreviouslyActivatedConstraints[i]-
+ NumberOfRemovedConstraints;
if (lindex>=0)
{
m_ActivatedConstraints.push_back(lindex);
@@ -864,7 +871,8 @@ int PLDPSolver::SolveProblem(double *CstPartOfTheCostFunction,
if (ContinueAlgo)
{
- ODEBUG("Nb of activated constraints: " << NewActivatedConstraints.size());
+ ODEBUG("Nb of activated constraints: " <<
+ NewActivatedConstraints.size());
m_OptCholesky->AddActiveConstraints(NewActivatedConstraints);
for(unsigned int i=0; i<NewActivatedConstraints.size(); i++)
m_ActivatedConstraints.push_back(NewActivatedConstraints[i]);
@@ -921,10 +929,12 @@ int PLDPSolver::SolveProblem(double *CstPartOfTheCostFunction,
ODEBUG("AR (" << lTime <<") :") ;
for( unsigned int i=0; i<m_ActivatedConstraints.size(); i++)
{
- ODEBUG( "( " << m_ActivatedConstraints[i] << " , " << m_v2[i] << " ) ");
+ ODEBUG( "( " << m_ActivatedConstraints[i] << " , "
+ << m_v2[i] << " ) ");
if (m_v2[i]<0.0)
{
- m_PreviouslyActivatedConstraints.push_back(m_ActivatedConstraints[i]);
+ m_PreviouslyActivatedConstraints.
+ push_back(m_ActivatedConstraints[i]);
ODEBUG3( m_ActivatedConstraints[i] << " " );
}
}
@@ -1008,7 +1018,8 @@ int PLDPSolver::SolveProblem(double *CstPartOfTheCostFunction,
ODEBUG6(m_ActivatedConstraints.size() << " "
<< NbOfConstraints << " "
- << m_ActivatedConstraints.size() - m_PreviouslyActivatedConstraints.size() <<
+ << m_ActivatedConstraints.size() -
+ m_PreviouslyActivatedConstraints.size() <<
" "
<< m_ItNb,(char*)Buffer.c_str());
@@ -1030,7 +1041,8 @@ void PLDPSolver::StoreCurrentZMPSolution(double *XkYk)
for(unsigned int j=0; j<m_CardV; j++)
{
m_PreviousZMPSolution[i] += m_Pu[j*m_CardV+i] * m_Vk[j];
- m_PreviousZMPSolution[i+m_CardV] += m_Pu[j*m_CardV+i] * m_Vk[j+m_CardV];
+ m_PreviousZMPSolution[i+m_CardV] += m_Pu[j*m_CardV+i] *
+ m_Vk[j+m_CardV];
}
for(unsigned int j=0; j<3; j++)
{
diff --git a/src/Mathematics/PLDPSolver.hh b/src/Mathematics/PLDPSolver.hh
index 8823ea642d190e0518e4473518a20d53fc2a1c4b..9bd422eee6c01e84128ca9fecde5aa47d1848996 100644
--- a/src/Mathematics/PLDPSolver.hh
+++ b/src/Mathematics/PLDPSolver.hh
@@ -39,7 +39,8 @@ namespace Optimization
{
namespace Solver
{
- /*! This class implements a two stage strategy to solve the following optimal problem:
+ /*! This class implements a two stage strategy to solve the
+ following optimal problem:
*/
class PLDPSolver
{
diff --git a/src/Mathematics/PLDPSolverHerdt.hh b/src/Mathematics/PLDPSolverHerdt.hh
index 4986571664b5038d813bdadd78ac51e36dc1858a..04270b1a7899d7a1c67cac068fe5b1018397beaa 100644
--- a/src/Mathematics/PLDPSolverHerdt.hh
+++ b/src/Mathematics/PLDPSolverHerdt.hh
@@ -40,7 +40,8 @@ namespace Optimization
{
namespace Solver
{
- /*! This class implements a two stage strategy to solve the following optimal problem:
+ /*! This class implements a two stage strategy to solve the following
+ optimal problem:
*/
class PLDPSolverHerdt
{
@@ -57,31 +58,35 @@ namespace Optimization
/*! \brief Solve the optimization problem
*/
- int SolveProblem(
- std::deque<PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_t> &
- QueueOfLConstraintInequalitiesFreeFeet,
- std::deque<PatternGeneratorJRL::SupportFeet_t> & QueueOfSupportFeet,
- double *CstPartOfTheCostFunction,
- unsigned int NbOfConstraints,
- double *LinearPartOfConstraints,
- double *CstPartOfConstraints,
- double *XkYk,
- double *X,
- unsigned int NumberOfRemovedConstraints, unsigned int NbRemovedFootCstr,
- bool StartingSequence, unsigned int NumberSteps, bool CurrentStateChanged,
- double time);
+ int SolveProblem
+ (std::deque<PatternGeneratorJRL::
+ LinearConstraintInequalityFreeFeet_t> &
+ QueueOfLConstraintInequalitiesFreeFeet,
+ std::deque<PatternGeneratorJRL::SupportFeet_t> & QueueOfSupportFeet,
+ double *CstPartOfTheCostFunction,
+ unsigned int NbOfConstraints,
+ double *LinearPartOfConstraints,
+ double *CstPartOfConstraints,
+ double *XkYk,
+ double *X,
+ unsigned int NumberOfRemovedConstraints,
+ unsigned int NbRemovedFootCstr,
+ bool StartingSequence,
+ unsigned int NumberSteps,
+ bool CurrentStateChanged,
+ double time);
protected:
/*! \name Initial solution methods related
@{
*/
/*! Compute the initial solution */
- int ComputeInitialSolution(
- std::deque<PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_t> &
- QueueOfLConstraintInequalitiesFreeFeet,
- std::deque<PatternGeneratorJRL::SupportFeet_t> & QueueOfSupportFeet,
- unsigned int NumberSteps,
- double *XkYk);
+ int ComputeInitialSolution
+ (std::deque<PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_t> &
+ QueueOfLConstraintInequalitiesFreeFeet,
+ std::deque<PatternGeneratorJRL::SupportFeet_t> & QueueOfSupportFeet,
+ unsigned int NumberSteps,
+ double *XkYk);
/*! Precompite iPuPx */
int PrecomputeiPuPx(unsigned int NumberSteps);
@@ -124,8 +129,8 @@ namespace Optimization
double ComputeAlpha(vector<unsigned int> &NewActivatedConstraints,
unsigned int NumberSteps);
- /* /\*! Store the current ZMP solution for hot start purposes. *\/ */
- /* void StoreCurrentZMPSolution(double *XkYk); */
+ /*! Store the current ZMP solution for hot start purposes. *\/ */
+ /* void StoreCurrentZMPSolution(double *XkYk); */
/*! Write current ZMP ref trajectory associated with
current value of m_Vk. */
diff --git a/src/Mathematics/PolynomeFoot.cpp b/src/Mathematics/PolynomeFoot.cpp
index dc77a9ea60c29abf1f33772d518cbf6497341a2c..fe318ef1e469649a2de21301448648fc766fdf96 100644
--- a/src/Mathematics/PolynomeFoot.cpp
+++ b/src/Mathematics/PolynomeFoot.cpp
@@ -219,11 +219,14 @@ void Polynome4::SetParametersWithInitPosInitSpeed(double FT,
}
else
{
- m_Coefficients[2] = (-4.0*InitSpeed*FT - 11.0*InitPos + 16.0*MP - 5*FP)/tmp;
+ m_Coefficients[2] = (-4.0*InitSpeed*FT - 11.0*InitPos +
+ 16.0*MP - 5*FP)/tmp;
tmp=tmp*FT;
- m_Coefficients[3] = ( 5.0*InitSpeed*FT + 18.0*InitPos - 32.0*MP + 14*FP)/tmp;
+ m_Coefficients[3] = ( 5.0*InitSpeed*FT + 18.0*InitPos -
+ 32.0*MP + 14*FP)/tmp;
tmp=tmp*FT;
- m_Coefficients[4] = (-2.0*InitSpeed*FT - 8.0 *InitPos + 16.0*MP - 8*FP)/tmp;
+ m_Coefficients[4] = (-2.0*InitSpeed*FT - 8.0 *InitPos +
+ 16.0*MP - 8*FP)/tmp;
}
}
void Polynome4::GetParametersWithInitPosInitSpeed(double &FT,
@@ -281,8 +284,9 @@ void Polynome5::GetParametersWithInitPosInitSpeed(double &FT,
FP = FinalPos_;
}
-void Polynome5::SetParameters(double FT, double FP,
- double InitPos, double InitSpeed, double InitAcc, double)
+void Polynome5::SetParameters
+(double FT, double FP,
+ double InitPos, double InitSpeed, double InitAcc, double)
{
double tmp;
m_Coefficients[0] = InitPos_ = InitPos;
@@ -300,20 +304,24 @@ void Polynome5::SetParameters(double FT, double FP,
}
else
{
- m_Coefficients[3] = (-3.0/2.0*InitAcc*FT*FT-6.0*InitSpeed*FT - 10.0*InitPos +
+ m_Coefficients[3] = (-3.0/2.0*InitAcc*FT*FT-6.0*InitSpeed*FT -
+ 10.0*InitPos +
10.0*FP)/tmp;
tmp=tmp*FT;
- m_Coefficients[4] = ( 3.0/2.0*InitAcc*FT*FT + 8.0*InitSpeed*FT + 15.0*InitPos -
+ m_Coefficients[4] = ( 3.0/2.0*InitAcc*FT*FT + 8.0*InitSpeed*FT +
+ 15.0*InitPos -
15.0*FP)/tmp;
tmp=tmp*FT;
- m_Coefficients[5] = ( -1.0/2.0*InitAcc*FT*FT - 3.0*InitSpeed*FT - 6.0*InitPos +
+ m_Coefficients[5] = ( -1.0/2.0*InitAcc*FT*FT - 3.0*InitSpeed*FT -
+ 6.0*InitPos +
6.0*FP)/tmp;
}
}
-void Polynome5::SetParameters(double FT,
- double InitPos, double InitSpeed, double InitAcc,
- double FinalPos, double FinalSpeed, double FinalAcc)
+void Polynome5::SetParameters
+(double FT,
+ double InitPos, double InitSpeed, double InitAcc,
+ double FinalPos, double FinalSpeed, double FinalAcc)
{
double tmp;
m_Coefficients[0] = InitPos_ = InitPos;
@@ -331,14 +339,20 @@ void Polynome5::SetParameters(double FT,
}
else
{
- m_Coefficients[3] = -(1.5*InitAcc*FT*FT - 0.5*FinalAcc*FT*FT + 6.0*InitSpeed*FT
- + 4.0*FinalSpeed*FT + 10.0*InitPos - 10.0*FinalPos)/tmp;
+ m_Coefficients[3] = -(1.5*InitAcc*FT*FT - 0.5*FinalAcc*FT*FT +
+ 6.0*InitSpeed*FT
+ + 4.0*FinalSpeed*FT + 10.0*InitPos -
+ 10.0*FinalPos)/tmp;
tmp=tmp*FT;
- m_Coefficients[4] = (1.5*InitAcc*FT*FT - FinalAcc*FT*FT + 8.0*InitSpeed*FT
- + 7.0*FinalSpeed*FT + 15.0*InitPos - 15.0*FinalPos)/tmp;
+ m_Coefficients[4] = (1.5*InitAcc*FT*FT - FinalAcc*FT*FT +
+ 8.0*InitSpeed*FT
+ + 7.0*FinalSpeed*FT + 15.0*InitPos -
+ 15.0*FinalPos)/tmp;
tmp=tmp*FT;
- m_Coefficients[5] = -(0.5*InitAcc*FT*FT - 0.5*FinalAcc*FT*FT + 3.0*InitSpeed*FT
- + 3.0*FinalSpeed*FT + 6.0*InitPos - 6.0*FinalPos)/tmp;
+ m_Coefficients[5] = -(0.5*InitAcc*FT*FT - 0.5*FinalAcc*FT*FT
+ + 3.0*InitSpeed*FT
+ + 3.0*FinalSpeed*FT + 6.0*InitPos
+ - 6.0*FinalPos)/tmp;
}
}
@@ -359,10 +373,11 @@ void Polynome6::SetParameters(double FT, double MP, double FP)
}
-void Polynome6::SetParametersWithMiddlePos(
- double FT, double MP,
- double InitPos, double InitSpeed, double InitAcc,
- double FP)
+void Polynome6::
+SetParametersWithMiddlePos
+(double FT, double MP,
+ double InitPos, double InitSpeed, double InitAcc,
+ double FP)
{
FT_=FT;
MP_=MP;
@@ -382,13 +397,17 @@ void Polynome6::SetParametersWithMiddlePos(
}
else
{
- m_Coefficients[3] = -0.5*( 5*FT*FT*InitAcc+32*FT*InitSpeed-128*MP+ 84*InitPos+
+ m_Coefficients[3] = -0.5*( 5*FT*FT*InitAcc+32*FT*InitSpeed-128*MP+
+ 84*InitPos+
44*FP )/(FT*FT*FT);
- m_Coefficients[4] = 0.5*( 9*FT*FT*InitAcc+76*FT*InitSpeed-384*MP+222*InitPos
+ m_Coefficients[4] = 0.5*( 9*FT*FT*InitAcc+76*FT*InitSpeed-384*MP+
+ 222*InitPos
+162*FP )/(FT*FT*FT*FT);
- m_Coefficients[5] = -0.5*( 7*FT*FT*InitAcc+66*FT*InitSpeed-384*MP+204*InitPos
+ m_Coefficients[5] = -0.5*( 7*FT*FT*InitAcc+66*FT*InitSpeed-384*MP+
+ 204*InitPos
+180*FP )/(FT*FT*FT*FT*FT);
- m_Coefficients[6] = ( FT*FT*InitAcc+10*FT*InitSpeed- 64*MP+ 32*InitPos+
+ m_Coefficients[6] = ( FT*FT*InitAcc+10*FT*InitSpeed- 64*MP+
+ 32*InitPos+
32*FP )/(FT*FT*FT*FT*FT*FT);
}
@@ -450,8 +469,9 @@ void Polynome7::GetParametersWithInitPosInitSpeed(double &FT,
FP = FP_;
}
-void Polynome7::SetParameters(double FT, double FP,
- double InitPos, double InitSpeed, double InitAcc, double InitJerk)
+void Polynome7::SetParameters
+(double FT, double FP,
+ double InitPos, double InitSpeed, double InitAcc, double InitJerk)
{
double tmp;
FT_ = FT;
@@ -473,16 +493,20 @@ void Polynome7::SetParameters(double FT, double FP,
}
else
{
- m_Coefficients[4] = -(2*InitJerk*FT*FT*FT + 15*InitAcc*FT*FT + 60*InitSpeed_*FT
+ m_Coefficients[4] = -(2*InitJerk*FT*FT*FT + 15*InitAcc*FT*FT +
+ 60*InitSpeed_*FT
+ 105*InitPos_ - 105*FP)/(3*tmp);
tmp *=FT;
- m_Coefficients[5] = (InitJerk*FT*FT*FT + 10*InitAcc*FT*FT + 45*InitSpeed_*FT +
+ m_Coefficients[5] = (InitJerk*FT*FT*FT + 10*InitAcc*FT*FT +
+ 45*InitSpeed_*FT +
84*InitPos_ - 84*FP)/tmp;
tmp*=FT;
- m_Coefficients[6] = -(4*InitJerk*FT*FT*FT + 45*InitAcc*FT*FT + 216*InitSpeed_*FT
+ m_Coefficients[6] = -(4*InitJerk*FT*FT*FT + 45*InitAcc*FT*FT +
+ 216*InitSpeed_*FT
+ 420*InitPos_ - 420*FP)/(6*tmp);
tmp*=FT;
- m_Coefficients[7] = (InitJerk*FT*FT*FT + 12*InitAcc*FT*FT + 60*InitSpeed_*FT +
+ m_Coefficients[7] = (InitJerk*FT*FT*FT + 12*InitAcc*FT*FT +
+ 60*InitSpeed_*FT +
120*InitPos_ - 120*FP)/(6*tmp);
}
}
diff --git a/src/Mathematics/PolynomeFoot.hh b/src/Mathematics/PolynomeFoot.hh
index 6952024fe3540575cc142b2a618ab083849934af..dd2f15f1fc089d3728297e5ec2c1b0d260d1ad10 100644
--- a/src/Mathematics/PolynomeFoot.hh
+++ b/src/Mathematics/PolynomeFoot.hh
@@ -206,11 +206,15 @@ namespace PatternGeneratorJRL
double &InitPos,
double &InitSpeed);
- /// \brief Set parameters considering initial position, velocity, acceleration
- void SetParameters(double FT, double FP,
- double InitPos, double InitSpeed, double InitAcc, double InitJerk = 0.0);
-
- /// \brief Set parameters considering initial position, velocity, acceleration,
+ /// \brief Set parameters considering initial position, velocity,
+ /// acceleration
+ void SetParameters
+ (double FT, double FP,
+ double InitPos, double InitSpeed, double InitAcc,
+ double InitJerk = 0.0);
+
+ /// \brief Set parameters considering initial position, velocity,
+ /// acceleration,
/// and final poistion, velocity and acceleration
void SetParameters(double FT,
double InitPos, double InitSpeed, double InitAcc,
@@ -236,8 +240,10 @@ namespace PatternGeneratorJRL
// Initial acceleration, velocity and position by default 0
// Final acceleration, velocity and position are 0
void SetParameters(double FT, double MP, double FP = 0.0);
- void SetParametersWithMiddlePos(double FT, double MP,
- double InitPos, double InitSpeed, double InitAcc=0.0, double FP = 0.0);
+ void SetParametersWithMiddlePos
+ (double FT, double MP,
+ double InitPos, double InitSpeed, double InitAcc=0.0, double FP = 0.0);
+
void GetParametersWithInitPosInitSpeed(double &TimeInterval,
double &MiddlePosition,
double &FinalPosition,
@@ -270,10 +276,13 @@ namespace PatternGeneratorJRL
double InitPos,
double InitSpeed);
- /// \brief Set parameters considering initial position, velocity, acceleration, jerk
- void SetParameters(double FT, double FP,
- double InitPos, double InitSpeed, double InitAcc, double InitJerk=0.0);
-
+ /// \brief Set parameters considering initial position,
+ /// velocity, acceleration, jerk
+ void SetParameters
+ (double FT, double FP,
+ double InitPos, double InitSpeed, double InitAcc,
+ double InitJerk=0.0);
+
/*! Set the parameters such that
the initial position, and initial speed
diff --git a/src/Mathematics/StepOverPolynome.cpp b/src/Mathematics/StepOverPolynome.cpp
index 9ff17f8d9ae3ab5dac2d45ba705ae5ec85e9b086..890a2b50d6bb74d4a7178408f72d7446ed643eb2 100644
--- a/src/Mathematics/StepOverPolynome.cpp
+++ b/src/Mathematics/StepOverPolynome.cpp
@@ -23,7 +23,8 @@
* Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
-/* Polynomes object for generating foot and hip trajectories while stepping over. */
+/* Polynomes object for generating foot and hip trajectories
+while stepping over. */
#include <iostream>
#include <vector>
@@ -166,7 +167,7 @@ StepOverPolynomeFoot::~StepOverPolynomeFoot()
{
}
-//////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////
StepOverPolynomeFootZtoX::StepOverPolynomeFootZtoX() :Polynome(3)
{
// SetParameters(boundCond,timeDistr);
@@ -233,7 +234,7 @@ StepOverPolynomeFootZtoX::~StepOverPolynomeFootZtoX()
{
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////
StepOverPolynomeFootXtoTime::StepOverPolynomeFootXtoTime() :Polynome(4)
{
@@ -383,11 +384,7 @@ StepOverPolynomeHip4::~StepOverPolynomeHip4()
{
}
-
-
-
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
+//////////////////////////////////////////////////////
StepOverSpline::StepOverSpline()
@@ -440,9 +437,10 @@ void StepOverSpline::SetParameters(Eigen::VectorXd Points)
{
m_Coefficients(0,i) = Points(i);
m_Coefficients(1,i) = TempSol(i,0);
- m_Coefficients(2,i) = 3.0*(Points(i+1)-Points(i))-2.0*TempSol(i,0)-TempSol(i+1,
- 0);
- m_Coefficients(3,i) = 2.0*(Points(i)-Points(i+1))+TempSol(i,0)+TempSol(i+1,0);
+ m_Coefficients(2,i) = 3.0*(Points(i+1)-Points(i))-
+ 2.0*TempSol(i,0)-TempSol(i+1, 0);
+ m_Coefficients(3,i) = 2.0*(Points(i)-Points(i+1))+
+ TempSol(i,0)+TempSol(i+1,0);
}
}
@@ -515,7 +513,8 @@ void StepOverSpline::print()
for (unsigned int i=0; i<m_number-1; i++)
{
- cout << " Spline m_number " << i+1 << ":" << endl << " ";
+ cout << " Spline m_number " << i+1 << ":"
+ << endl << " ";
for (unsigned int j=0; j<4; j++)
cout << m_Coefficients(j,i) << " " ;
cout << endl;
@@ -531,16 +530,18 @@ StepOverSpline::~StepOverSpline()
}
-////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////
StepOverClampedCubicSpline::StepOverClampedCubicSpline()
{
// SetParameters(boundCond,timeDistr);
}
-void StepOverClampedCubicSpline::SetParameters(Eigen::VectorXd Points,
- Eigen::VectorXd TimePoints,
- Eigen::VectorXd DerivativeEndPoints)
+void StepOverClampedCubicSpline::
+SetParameters
+(Eigen::VectorXd Points,
+ Eigen::VectorXd TimePoints,
+ Eigen::VectorXd DerivativeEndPoints)
{
Eigen::Matrix<double,4,4> LhSide;;
Eigen::Matrix<double,4,1> RhSide;;
@@ -567,12 +568,13 @@ void StepOverClampedCubicSpline::SetParameters(Eigen::VectorXd Points,
for (unsigned int i=1; i<m_number; i++)
{
- RhSide(i,0)=3.0/h(i)*(Points(i+1)-Points(i))-3.0/h(i-1)*(Points(i)-Points(i-1));
+ RhSide(i,0)=3.0/h(i)*(Points(i+1)-Points(i))-3.0/
+ h(i-1)*(Points(i)-Points(i-1));
}
RhSide(0,0)=3.0/h(0)*(Points(1)-Points(0))-3.0*DerivativeEndPoints(0);
- RhSide(m_number,0)=-3.0/h(m_number-1)*(Points(m_number)-Points(
- m_number-1))+3.0*DerivativeEndPoints(1);
+ RhSide(m_number,0)=-3.0/h(m_number-1)*
+ (Points(m_number)-Points(m_number-1))+3.0*DerivativeEndPoints(1);
for (unsigned int i=1; i<m_number; i++)
{
@@ -594,7 +596,8 @@ void StepOverClampedCubicSpline::SetParameters(Eigen::VectorXd Points,
for (unsigned int i=0; i<m_number; i++)
{
m_Coefficients(0,i) = Points(i);
- m_Coefficients(1,i) = (Points(i+1)-Points(i)-(2.0*TempSol(i,0)+TempSol(i+1,0))
+ m_Coefficients(1,i) = (Points(i+1)-Points(i)-
+ (2.0*TempSol(i,0)+TempSol(i+1,0))
*h(i)*h(i)/3.0)/h(i);
m_Coefficients(2,i) = TempSol(i,0);
m_Coefficients(3,i) = (TempSol(i+1,0)-TempSol(i,0))/(3.0*h(i));
@@ -612,7 +615,8 @@ GetValueSpline
numberComp = TimePoints.size();
if (!(numberComp=m_number))
{
- cout << "CCS: number of intervals is not the same as for the calculation of the spline coefficients !"
+ cout << "CCS: number of intervals is not the same "
+ << "as for the calculation of the spline coefficients !"
<< endl;
return 0.0;
}
@@ -635,7 +639,8 @@ GetValueSpline
} // for
if (CurrentLocalTime>=TimePoints(m_number))
- // if the CurrentLocalTime is out of range it will return the end position if t>tmax
+ // if the CurrentLocalTime is out of range it will return
+ //the end position if t>tmax
{
double Time0to1 =0.0;
double Time0to1_2 =0.0;
@@ -644,18 +649,20 @@ GetValueSpline
Time0to1_2=Time0to1*Time0to1;
Time0to1_3=Time0to1_2*Time0to1;
- ValueSpline = m_Coefficients(0,m_number-1)+m_Coefficients(1,m_number-1)*Time0to1
- +m_Coefficients(2,m_number-1)*Time0to1_2+m_Coefficients(3,
- m_number-1)*Time0to1_3;
+ ValueSpline = m_Coefficients(0,m_number-1)+m_Coefficients(1,m_number-1)*
+ Time0to1 + m_Coefficients(2,m_number-1)*
+ Time0to1_2+m_Coefficients(3, m_number-1)*Time0to1_3;
// cout << "CCS: timevalue is beyond the range of given time intervals,
// last position is returned" << endl;
}
if (CurrentLocalTime<=TimePoints(0))
- // if the CurrentLocalTime is out of range it will return the start position if t<tmin
+ // if the CurrentLocalTime is out of range it will
+ // return the start position if t<tmin
{
ValueSpline = m_Coefficients(0,0);
- // cout << "CCS: timevalue falls before the range of given time intervals, first position is returned" << endl;
+ // cout << "CCS: timevalue falls before the range
+ // of given time intervals, first position is returned" << endl;
}
return ValueSpline;
diff --git a/src/Mathematics/StepOverPolynome.hh b/src/Mathematics/StepOverPolynome.hh
index c3447ba1f69e79db3cf420329e933eabfa5db89d..6dec5a7c2ebf87d33d0aac64eeafecf71395356d 100644
--- a/src/Mathematics/StepOverPolynome.hh
+++ b/src/Mathematics/StepOverPolynome.hh
@@ -25,7 +25,8 @@
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/** \file StepOverPolynome.h
- \brief Polynomes object for generating foot and hip trajectories while stepping over. */
+ \brief Polynomes object for generating foot and hip trajectories
+ while stepping over. */
#ifndef _STEPOVER_POLYNOME_H_
@@ -45,8 +46,10 @@ namespace PatternGeneratorJRL
{
public:
/*! Constructor:
- boundCond: the different boundary conditions begin, intermediate and end of polynomial
- timeDistr: vector with time instants for intermediate boundary conditions and end time */
+ boundCond: the different boundary conditions begin,
+ intermediate and end of polynomial
+ timeDistr: vector with time instants for
+ intermediate boundary conditions and end time */
StepOverPolynomeFoot();
/*! Set the parameters */
@@ -103,8 +106,10 @@ namespace PatternGeneratorJRL
{
public:
/*! Constructor:
- boundCond: the different boundary conditions begin, intermediate and end of polynomial
- timeDistr: vector with time instants for intermediate boundary conditions and end time
+ boundCond: the different boundary conditions begin,
+ intermediate and end of polynomial
+ timeDistr: vector with time instants for intermediate
+ boundary conditions and end time
*/
StepOverPolynomeHip4();
diff --git a/src/Mathematics/intermediate-qp-matrices.cpp b/src/Mathematics/intermediate-qp-matrices.cpp
index 5da95aeb20270313e131a7aa7c1b293f9eceff62..712171d1deb0de2f20fa559bef284824973a2213 100644
--- a/src/Mathematics/intermediate-qp-matrices.cpp
+++ b/src/Mathematics/intermediate-qp-matrices.cpp
@@ -22,7 +22,8 @@
* Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
-/*! \brief Custom (value based) container providing intermediate elements for the construction of a QP.
+/*! \brief Custom (value based) container providing intermediate elements
+ for the construction of a QP.
*/
#include <Mathematics/intermediate-qp-matrices.hh>
diff --git a/src/Mathematics/intermediate-qp-matrices.hh b/src/Mathematics/intermediate-qp-matrices.hh
index 11df7539e2aa1e046b69026afc305639cb09234a..139de7214007a8f67dab1accc4a3309c25ce2680 100644
--- a/src/Mathematics/intermediate-qp-matrices.hh
+++ b/src/Mathematics/intermediate-qp-matrices.hh
@@ -37,7 +37,8 @@
namespace PatternGeneratorJRL
{
- /// \brief Custom (value based) container providing intermediate elements for the construction of a QP.
+ /// \brief Custom (value based) container providing intermediate elements
+ /// for the construction of a QP.
class IntermedQPMat
{
//
@@ -68,7 +69,8 @@ namespace PatternGeneratorJRL
Eigen::MatrixXd VT;
/// \brief Selection matrix multiplied with the current foot position
Eigen::VectorXd VcX, VcY;
- /// \brief Shifted selection matrix multiplied with the current feet position
+ /// \brief Shifted selection matrix multiplied with the current feet
+ /// position
Eigen::VectorXd VcshiftX, VcshiftY;
/// \brief Selection matrix for the current foot position
Eigen::VectorXd Vc_fX, Vc_fY;
diff --git a/src/Mathematics/qld.cpp b/src/Mathematics/qld.cpp
index 7adbf7a3131ba1aa85187bdcde23ed03e5ac1850..378a741c58d23e90e244b4a4b58d95eef411a69f 100644
--- a/src/Mathematics/qld.cpp
+++ b/src/Mathematics/qld.cpp
@@ -659,8 +659,10 @@ int ql0002_(integer *n,integer *m,integer *meq,integer *mmax,
doublereal *diag, doublereal *w,
integer * /* lw */)
#else
- /* Subroutine */ int ql0002_(n, m, meq, mmax, mn, mnn, nmax, lql, a, b, grad,
- g, xl, xu, x, nact, iact, maxit, vsmall, info, diag, w, lw)
+ /* Subroutine */
+ int ql0002_
+ (n, m, meq, mmax, mn, mnn, nmax, lql, a, b, grad,
+ g, xl, xu, x, nact, iact, maxit, vsmall, info, diag, w, lw)
integer *n, *m, *meq, *mmax, *mn, *mnn, *nmax;
logical *lql;
doublereal *a, *b, *grad, *g, *xl, *xu, *x;
@@ -872,8 +874,8 @@ integer *lw;
/* Computing MIN */
d__1 = w[id], d__2 = g[j + j * g_dim1];
ga = -min(d__1,d__2);
- gb = (d__1 = w[id] - g[j + j * g_dim1], abs(d__1)) + (d__2 = g[i
- + j * g_dim1], abs(d__2));
+ gb = (d__1 = w[id] - g[j + j * g_dim1], abs(d__1)) +
+ (d__2 = g[i + j * g_dim1], abs(d__2));
if (gb > zero)
{
/* Computing 2nd power */
@@ -2255,8 +2257,9 @@ integer *lw;
i__2 = *n;
for (i = 1; i <= i__2; ++i)
{
- sum += (d__1 = x[i], abs(d__1)) * vfact * ((d__2 = grad[i], abs(d__2))
- + (d__3 = g[i + i * g_dim1] * x[i], abs(d__3)));
+ sum += (d__1 = x[i], abs(d__1)) *
+ vfact * ((d__2 = grad[i], abs(d__2))
+ + (d__3 = g[i + i * g_dim1] * x[i], abs(d__3)));
if (*lql)
{
goto L920;
diff --git a/src/Mathematics/relative-feet-inequalities.cpp b/src/Mathematics/relative-feet-inequalities.cpp
index 4b815b45a76d9371ee4dd0e987f0a0f96b38ea3d..f1a36b14050d2f09392e0dbd479d7c04373672cc 100644
--- a/src/Mathematics/relative-feet-inequalities.cpp
+++ b/src/Mathematics/relative-feet-inequalities.cpp
@@ -25,7 +25,8 @@
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/** \file FootConstraintAsLinearSystemForVelRef.cpp
- \brief This object builds linear constraints relative to the current and the previewed feet positions.
+ \brief This object builds linear constraints relative to the current
+ and the previewed feet positions.
*/
#include <iostream>
@@ -39,8 +40,9 @@ using namespace PatternGeneratorJRL;
#include <Debug.hh>
-RelativeFeetInequalities::RelativeFeetInequalities( SimplePluginManager *aSPM,
- PinocchioRobot *aPR ) :
+RelativeFeetInequalities::RelativeFeetInequalities
+( SimplePluginManager *aSPM,
+ PinocchioRobot *aPR ) :
SimplePlugin(aSPM)
{
@@ -102,10 +104,10 @@ RelativeFeetInequalities::init_convex_hulls()
FootPosEdges_.LeftSS.resize( nbVertFeet );
FootPosEdges_.RightDS.resize( nbVertFeet );
FootPosEdges_.RightSS.resize( nbVertFeet );
- FootPosEdges_.LeftDS.set_vertices ( LeftFPosEdgesX_, LeftFPosEdgesY_ );
- FootPosEdges_.LeftSS.set_vertices ( LeftFPosEdgesX_, LeftFPosEdgesY_ );
- FootPosEdges_.RightDS.set_vertices( RightFPosEdgesX_, RightFPosEdgesY_ );
- FootPosEdges_.RightSS.set_vertices( RightFPosEdgesX_, RightFPosEdgesY_ );
+ FootPosEdges_.LeftDS.set_vertices ( LeftFPosEdgesX_, LeftFPosEdgesY_ );
+ FootPosEdges_.LeftSS.set_vertices ( LeftFPosEdgesX_, LeftFPosEdgesY_ );
+ FootPosEdges_.RightDS.set_vertices( RightFPosEdgesX_, RightFPosEdgesY_);
+ FootPosEdges_.RightSS.set_vertices( RightFPosEdgesX_, RightFPosEdgesY_);
// ZMP polygonal hulls:
@@ -122,28 +124,42 @@ RelativeFeetInequalities::init_convex_hulls()
for( unsigned j = 0; j < nbVertCoP; j++ )
{
//Left single support phase
- ZMPPosEdges_.LeftSS.X_vec[j] = lxcoefsLeft[j]*LeftFootSize_.getHalfWidth();
- ZMPPosEdges_.LeftSS.Y_vec[j] = lycoefsLeft[j]*LeftFootSize_.getHalfHeight();
+ ZMPPosEdges_.LeftSS.X_vec[j] = lxcoefsLeft[j]*
+ LeftFootSize_.getHalfWidth();
+ ZMPPosEdges_.LeftSS.Y_vec[j] = lycoefsLeft[j]*
+ LeftFootSize_.getHalfHeight();
//Right single support phase
- ZMPPosEdges_.RightSS.X_vec[j] = lxcoefsRight[j]*RightFootSize_.getHalfWidth();
- ZMPPosEdges_.RightSS.Y_vec[j] = lycoefsRight[j]*RightFootSize_.getHalfHeight();
+ ZMPPosEdges_.RightSS.X_vec[j] = lxcoefsRight[j]*
+ RightFootSize_.getHalfWidth();
+ ZMPPosEdges_.RightSS.Y_vec[j] = lycoefsRight[j]*
+ RightFootSize_.getHalfHeight();
//Left DS phase
- ZMPPosEdges_.LeftDS.X_vec[j] = lxcoefsLeft[j]*LeftFootSize_.getHalfWidth();
- ZMPPosEdges_.LeftDS.Y_vec[j] = lycoefsLeft[j]*LeftFootSize_.getHalfHeightDS()
+ ZMPPosEdges_.LeftDS.X_vec[j] = lxcoefsLeft[j]*
+ LeftFootSize_.getHalfWidth();
+ ZMPPosEdges_.LeftDS.Y_vec[j] = lycoefsLeft[j]*
+ LeftFootSize_.getHalfHeightDS()
-DSFeetDistance_/2.0;
//Right DS phase
- ZMPPosEdges_.RightDS.X_vec[j] = lxcoefsRight[j]*RightFootSize_.getHalfWidth();
- ZMPPosEdges_.RightDS.Y_vec[j] = lycoefsRight[j]*RightFootSize_.getHalfHeightDS()
+ ZMPPosEdges_.RightDS.X_vec[j] = lxcoefsRight[j]*
+ RightFootSize_.getHalfWidth();
+ ZMPPosEdges_.RightDS.Y_vec[j] =lycoefsRight[j]*
+ RightFootSize_.getHalfHeightDS()
+DSFeetDistance_/2.0;
}
// CoM polyhedric hull:
// --------------------
- double IneqCoMA_a[nbIneqCoM] = { -0.6, -0.6, -0.6, -0.6, -0.6, -0.3, -0.3, -0.3, -0.3, -0.3};
- double IneqCoMB_a[nbIneqCoM] = {-0.3, -0.175, -0.05, 0.075, 0.2, -0.3, -0.175, -0.05, 0.075, 0.2};
- double IneqCoMC_a[nbIneqCoM] = {-0.8544, -0.818917, -0.801561, -0.803508, -0.824621, -1, -0.969858, -0.955249, -0.956883, -0.974679};
- double IneqCoMD_a[nbIneqCoM] = {-0.862318, -0.836995, -0.818542, -0.807405, -0.803531, -0.9175, -0.894201, -0.876789, -0.865534, -0.860404};
+ double IneqCoMA_a[nbIneqCoM] =
+ { -0.6, -0.6, -0.6, -0.6, -0.6, -0.3, -0.3, -0.3, -0.3, -0.3};
+ double IneqCoMB_a[nbIneqCoM] =
+ {-0.3, -0.175, -0.05, 0.075, 0.2, -0.3, -0.175, -0.05, 0.075, 0.2};
+ double IneqCoMC_a[nbIneqCoM] =
+ {-0.8544, -0.818917, -0.801561, -0.803508, -0.824621,
+ -1, -0.969858, -0.955249, -0.956883, -0.974679};
+ double IneqCoMD_a[nbIneqCoM] =
+ {-0.862318, -0.836995, -0.818542, -0.807405, -0.803531,
+ -0.9175, -0.894201, -0.876789, -0.865534, -0.860404};
CoMHull_.resize(0, nbIneqCoM);
CoMHull_.set_inequalities( IneqCoMA_a, IneqCoMB_a, IneqCoMC_a, IneqCoMD_a );
@@ -193,8 +209,10 @@ RelativeFeetInequalities::set_feet_dimensions( PinocchioRobot *aPR )
void
-RelativeFeetInequalities::set_vertices( convex_hull_t & ConvexHull,
- const support_state_t & Support, ineq_e type)
+RelativeFeetInequalities::
+set_vertices
+( convex_hull_t & ConvexHull,
+ const support_state_t & Support, ineq_e type)
{
edges_s * ConvexHull_p = 0;
@@ -245,8 +263,10 @@ RelativeFeetInequalities::set_vertices( convex_hull_t & ConvexHull,
void
-RelativeFeetInequalities::set_inequalities( convex_hull_t & ConvexHull,
- const support_state_t &, ineq_e type)
+RelativeFeetInequalities::
+set_inequalities
+( convex_hull_t & ConvexHull,
+ const support_state_t &, ineq_e type)
{
convex_hull_t * ConvexHull_p = 0;
@@ -272,8 +292,10 @@ RelativeFeetInequalities::set_inequalities( convex_hull_t & ConvexHull,
void
-RelativeFeetInequalities::compute_linear_system ( convex_hull_t & ConvexHull,
- const support_state_t & PrwSupport ) const
+RelativeFeetInequalities::
+compute_linear_system
+( convex_hull_t & ConvexHull,
+ const support_state_t & PrwSupport ) const
{
double dx,dy,dc,x1,y1,x2,y2;
unsigned int nbRows = (unsigned int)ConvexHull.X_vec.size();
diff --git a/src/Mathematics/relative-feet-inequalities.hh b/src/Mathematics/relative-feet-inequalities.hh
index b1b4bfbae7243ab3970e0076be4192a2d4771c59..453fa5767df3f1eff7d292ba0afe8924a164e5f7 100644
--- a/src/Mathematics/relative-feet-inequalities.hh
+++ b/src/Mathematics/relative-feet-inequalities.hh
@@ -45,7 +45,8 @@
namespace PatternGeneratorJRL
{
- /// \brief Generate a stack of inequalities relative to feet centers for the whole preview window.
+ /// \brief Generate a stack of inequalities relative to feet centers for
+ /// the whole preview window.
class RelativeFeetInequalities:public SimplePlugin
{
@@ -83,7 +84,8 @@ namespace PatternGeneratorJRL
void set_inequalities( convex_hull_t & ConvexHull,
const support_state_t & Support, ineq_e type);
- /// \brief Compute the linear inequalities \f${\bf A}{\bf x} \geq {\bf b}\f$ associated with the
+ /// \brief Compute the linear inequalities \f${\bf A}{\bf x} \geq
+ /// {\bf b}\f$ associated with the
/// convex hull specified by a vector of points.
///
/// \param[out] aVecOfPoints a vector of vertices
diff --git a/src/MotionGeneration/CollisionDetector.cpp b/src/MotionGeneration/CollisionDetector.cpp
index 0ca6355f064ae41d6ea0584abb72d119bc7dd220..534f91dac4e72016d9e6275beb5bd2913e576fe4 100644
--- a/src/MotionGeneration/CollisionDetector.cpp
+++ b/src/MotionGeneration/CollisionDetector.cpp
@@ -52,10 +52,10 @@ void CollisionDetector::SetObstacleCoordinates(ObstaclePar aObstacleInfo)
/*! ObstacleInfo contains x, y, z position of obstacle in worldframe
(point taken on the front plan of the obstacle on the floor and in
the middel of the width and also the orientation of the obstacle
- in the X,Y plane of the world and the obstacle dimensions: depth, Width and height
+ in the X,Y plane of the world and the obstacle dimensions:
+ depth, Width and height
*/
-
//double ObstacleAngle;
//double ObstacleWidth, ObstacleHeight, ObstacleDepth;
double c,s;
@@ -66,14 +66,12 @@ void CollisionDetector::SetObstacleCoordinates(ObstaclePar aObstacleInfo)
m_ObstaclePosition(1) = aObstacleInfo.y;
m_ObstaclePosition(2) = aObstacleInfo.z;
-
// ObstacleAngle = m_ObstacleInfo.theta;
c = cos(aObstacleInfo.theta*M_PI/180.0);
s = sin(aObstacleInfo.theta*M_PI/180.0);
-
-
- //this matrix transformes coordinates in the obstacle frame into the world frame
+ // this matrix transformes coordinates in the obstacle frame
+ // into the world frame
m_ObstacleRot(0,0) = c;
m_ObstacleRot(0,1) =-s;
m_ObstacleRot(0,2) = 0;
@@ -84,7 +82,8 @@ void CollisionDetector::SetObstacleCoordinates(ObstaclePar aObstacleInfo)
m_ObstacleRot(2,1) = 0;
m_ObstacleRot(2,2) = 1;
- //this matrix transformes coordinates in the world frame into the obstacle frame
+ // this matrix transformes coordinates in the world
+ // frame into the obstacle frame
m_ObstacleRotInv(0,0) = c;
m_ObstacleRotInv(0,1) = s;
m_ObstacleRotInv(0,2) = 0;
@@ -100,7 +99,8 @@ void CollisionDetector::SetObstacleCoordinates(ObstaclePar aObstacleInfo)
//ObstacleHeight = ObstacleInfo.h;
// obstacle points are the four corner points at one side of the obstacle
- // in the obstacle coordinate frame...the other side have the same coordinates,
+ // in the obstacle coordinate frame...the other side have
+ // the same coordinates,
// except for the Y coordinate which has opposite sign
// point 0:
m_ObstaclePoints(0,0) = 0.0;
@@ -119,17 +119,20 @@ void CollisionDetector::SetObstacleCoordinates(ObstaclePar aObstacleInfo)
m_ObstaclePoints(1,3) = aObstacleInfo.w/2.0;
m_ObstaclePoints(2,3) = 0.0;
- //cout << "I have set the obstacle info in the colission detection class" << endl;
+ // cout << "I have set the obstacle info in the colission detection class"
+ // << endl;
}
-void CollisionDetector::WorldFrameToObstacleFrame(Eigen::Vector3d
- &WorldFrameCoord,
- Eigen::Vector3d &ObstacleFrameCoord)
+void CollisionDetector::WorldFrameToObstacleFrame
+(Eigen::Vector3d
+ &WorldFrameCoord,
+ Eigen::Vector3d &ObstacleFrameCoord)
{
// This function transforms the coordinates of a point
// expressed in the world frame to the local coordinates in the obstacle frame
- ObstacleFrameCoord = m_ObstacleRotInv * (WorldFrameCoord - m_ObstaclePosition);
+ ObstacleFrameCoord = m_ObstacleRotInv *
+ (WorldFrameCoord - m_ObstaclePosition);
/*
cout << "X WorldFrameCoord: " << WorldFrameCoord(0,0) << endl;
cout << "Y WorldFrameCoord: " << WorldFrameCoord(1,0) << endl;
@@ -139,18 +142,23 @@ void CollisionDetector::WorldFrameToObstacleFrame(Eigen::Vector3d
cout << "Y obstacle position: " << m_ObstaclePosition(1,0) << endl;
cout << "Z obstacle position: " << m_ObstaclePosition(2,0) << endl;
- cout << "X obstacle frame coordinate: " << ObstacleFrameCoord(0,0) << endl;
- cout << "Y obstacle frame coordinate: " << ObstacleFrameCoord(1,0) << endl;
- cout << "Z obstacle frame coordinate: " << ObstacleFrameCoord(2,0) << endl;
+ cout << "X obstacle frame coordinate: "
+ << ObstacleFrameCoord(0,0) << endl;
+ cout << "Y obstacle frame coordinate: "
+ << ObstacleFrameCoord(1,0) << endl;
+ cout << "Z obstacle frame coordinate: "
+ << ObstacleFrameCoord(2,0) << endl;
*/
}
-void CollisionDetector::CalcCoordShankLowerLegPoint(Eigen::Vector3d RelCoord,
- Eigen::Vector3d &AbsCoord,
- Eigen::VectorXd LegAngles,
- Eigen::Matrix3d WaistRot,
- Eigen::Vector3d WaistPos,
- int WhichLeg)
+void CollisionDetector::
+CalcCoordShankLowerLegPoint
+(Eigen::Vector3d RelCoord,
+ Eigen::Vector3d &AbsCoord,
+ Eigen::VectorXd LegAngles,
+ Eigen::Matrix3d WaistRot,
+ Eigen::Vector3d WaistPos,
+ int WhichLeg)
{
Eigen::Matrix3d Rotation;
Eigen::Vector3d TempCoord;
@@ -243,7 +251,8 @@ void CollisionDetector::CalcCoordShankLowerLegPoint(Eigen::Vector3d RelCoord,
Translation(0) = 0.0;
- //this is currenlty hard coded but has to be solved when using functions in dynamic multi body class
+ // this is currenlty hard coded but has to be solved
+ // when using functions in dynamic multi body class
if(WhichLeg>0)
Translation(1) = 0.09;
else
@@ -279,9 +288,10 @@ bool CollisionDetector::CollisionTwoLines(vector<double> p1,
vector<double> v1,
vector<double> v2)
{
- //this function checks for intersection of two line segments p1p2 and v1v2.
- //since this is a 2D problem the coordinates are the respective planar coordinates
- //it returns true if a collision occurs, else false
+ // this function checks for intersection of two line segments p1p2 and v1v2.
+ // since this is a 2D problem the coordinates are the respective
+ // planar coordinates
+ // it returns true if a collision occurs, else false
double Ap1p2v1, Ap1p2v2, Av1v2p1,Av1v2p2;
@@ -299,13 +309,16 @@ bool CollisionDetector::CollisionTwoLines(vector<double> p1,
}
-bool CollisionDetector::CollisionLineObstaclePlane(Eigen::Vector3d &LegPoint1,
- Eigen::Vector3d &LegPoint2,
- int PlaneNumber)
+bool CollisionDetector::
+CollisionLineObstaclePlane
+(Eigen::Vector3d &LegPoint1,
+ Eigen::Vector3d &LegPoint2,
+ int PlaneNumber)
{
/*! This function checks for intersection of a linesegment p1p2
of the robot, expressed in the obstacle frame, with one of
- the three planes of the obstacle: plane 0: front; plane 1: top; plane 2:rear plane
+ the three planes of the obstacle: plane 0: front; plane 1: top;
+ plane 2:rear plane
it returns true if a collision occurs, else false
for the intersection of a linesegment with an obstacle plane,
@@ -313,8 +326,6 @@ bool CollisionDetector::CollisionLineObstaclePlane(Eigen::Vector3d &LegPoint1,
and the XY plane or YZ plane depending planenumber
indicates a side plan og the obstacle or not. */
-
-
bool CollisionStatusXZ, CollisionStatusXY,CollisionStatusYZ;
vector<double> p1, p2, v1, v2;
@@ -328,8 +339,6 @@ bool CollisionDetector::CollisionLineObstaclePlane(Eigen::Vector3d &LegPoint1,
CollisionStatusXZ = 1;
CollisionStatusYZ = 1;
-
-
if (PlaneNumber==3)
{
diff --git a/src/MotionGeneration/ComAndFootRealization.hh b/src/MotionGeneration/ComAndFootRealization.hh
index 92006f4f4e56606d1d747df7ba5ed48d7f6f1b7a..3ebf26fbcb2cb63af0f85e4da82a7c5fb7831ec8 100644
--- a/src/MotionGeneration/ComAndFootRealization.hh
+++ b/src/MotionGeneration/ComAndFootRealization.hh
@@ -97,21 +97,29 @@ namespace PatternGeneratorJRL
\a ZMPPreviewControlWithMultiBodyZMP. */
/*! Compute the robot state for a given CoM and feet posture.
- Each posture is given by a 3D position and two Euler angles \f$ (\theta, \omega) \f$.
+ Each posture is given by a 3D position and two Euler angles
+ \f$ (\theta, \omega) \f$.
Very important: This method is assume to set correctly the body angles of
its \a HumanoidDynamicRobot and a subsequent call to the ZMP position
will return the associated ZMP vector.
- @param CoMPosition: a 5 dimensional vector with the first dimension for position,
+ @param CoMPosition: a 5 dimensional vector with the first dimension
+ for position,
and the last two for the orientation (Euler angle).
- @param CoMSpeed: a 5 dimensional vector: 3 for the linear velocity in X,Y,Z,
+ @param CoMSpeed: a 5 dimensional vector: 3 for the linear velocity in
+ X,Y,Z,
and two for the angular velocity.
- @param CoMAcc: a 5 dimensional vector: 3 for the linear acceleration in X,Y,Z,
+ @param CoMAcc: a 5 dimensional vector: 3 for the linear acceleration in
+ X,Y,Z,
and two for the angular acceleration.
- @param LeftFoot: a 5 dimensional following the same convention than for \a CoMPosition.
+ @param LeftFoot: a 5 dimensional following the same convention than for
+ \a CoMPosition.
@param RightFoot: idem.
- @param CurrentConfiguration: The result is a state vector containing the position which are put inside this parameter.
- @param CurrentVelocity: The result is a state vector containing the speed which are put inside this parameter.x
- @param CurrentAcceleration: The result is a state vector containing the acceleration which are put inside this parameter.x
+ @param CurrentConfiguration: The result is a state vector containing the
+ position which are put inside this parameter.
+ @param CurrentVelocity: The result is a state vector containing the speed
+ which are put inside this parameter.x
+ @param CurrentAcceleration: The result is a state vector containing the
+ acceleration which are put inside this parameter.x
@param IterationNumber: Number of iteration.
@param Stage: indicates which stage is reach by the Pattern Generator.
*/
@@ -132,7 +140,8 @@ namespace PatternGeneratorJRL
/*! \name Setter and getter for the jrlHumanoidDynamicRobot object. */
- /*! @param aHumanoidDynamicRobot: an object able to compute dynamic parameters
+ /*! @param aHumanoidDynamicRobot: an object able to
+ compute dynamic parameters
of the robot. */
inline virtual bool setPinocchioRobot(PinocchioRobot *aPinocchioRobot)
{
@@ -158,18 +167,20 @@ namespace PatternGeneratorJRL
IMPORTANT: The jrlHumanoidDynamicRobot must have been properly set up.
*/
- virtual bool InitializationCoM(Eigen::VectorXd &BodyAnglesIni,
- Eigen::Vector3d & lStartingCOMPosition,
- Eigen::Matrix<double,6,1> & lStartingWaistPose,
- FootAbsolutePosition & InitLeftFootAbsPos,
- FootAbsolutePosition & InitRightFootAbsPos)=0;
+ virtual bool InitializationCoM
+ (Eigen::VectorXd &BodyAnglesIni,
+ Eigen::Vector3d & lStartingCOMPosition,
+ Eigen::Matrix<double,6,1> & lStartingWaistPose,
+ FootAbsolutePosition & InitLeftFootAbsPos,
+ FootAbsolutePosition & InitRightFootAbsPos)=0;
/*! This initialization phase, make sure that the needed buffers
for the upper body motion are correctly setup.
*/
- virtual bool InitializationUpperBody(deque<ZMPPosition> &inZMPPositions,
- deque<COMPosition> &inCOMBuffer,
- deque<RelativeFootPosition> lRelativeFootPositions)=0;
+ virtual bool InitializationUpperBody
+ (deque<ZMPPosition> &inZMPPositions,
+ deque<COMPosition> &inCOMBuffer,
+ deque<RelativeFootPosition> lRelativeFootPositions)=0;
/* @} */
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
index e320745da106ee15594db8774a1c2392c330ff21..46dc3ba94be297604727d2cb1e2da681101254c1 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
@@ -141,9 +141,10 @@ InitializationMaps(std::vector<pinocchio::Index> &FromRootToJoint,
{
// -1 because pinocchio uses quaternion instead of roll pitch yaw
// assuming the model possess only revolute joint
- IndexinConfiguration[lindex] = pinocchio::idx_q(
- actuatedJoints[FromRootToJoint[i]])-1 ;
- IndexinVelocity[lindex] = pinocchio::idx_v(actuatedJoints[FromRootToJoint[i]]) ;
+ IndexinConfiguration[lindex] =
+ pinocchio::idx_q(actuatedJoints[FromRootToJoint[i]])-1 ;
+ IndexinVelocity[lindex] =
+ pinocchio::idx_v(actuatedJoints[FromRootToJoint[i]]) ;
lindex++;
}
}
@@ -189,25 +190,7 @@ InitializeMapForChest(pinocchio::JointModelVector & ActuatedJoints)
std::vector<pinocchio::JointIndex> /*FromRootToJoint2,*/FromRootToJoint;
FromRootToJoint = getPinocchioRobot()->fromRootToIt(Chest);
// erase the 1rst element as it is the root
- FromRootToJoint.erase (FromRootToJoint.begin());/*
- std::vector<pinocchio::JointIndex>::iterator itJoint = FromRootToJoint.begin();
- bool startadding=false;
- while(itJoint!=FromRootToJoint.end())
- {
- std::vector<pinocchio::JointIndex>::iterator current = itJoint;
-
- if (*current==Chest)
- {
- startadding=true;
-
- }
- else
- {
- if (startadding)
- FromRootToJoint2.push_back(*itJoint);
- }
- itJoint++;
- }*/
+ FromRootToJoint.erase (FromRootToJoint.begin());
InitializationMaps(FromRootToJoint,ActuatedJoints,
m_ChestIndexinConfiguration,m_ChestIndexinVelocity);
}
@@ -269,8 +252,8 @@ Initialization()
// Remove the first element
std::vector<pinocchio::JointIndex> FromRootToJoint =
getPinocchioRobot()->jointsBetween(waist, RightFoot->associatedAnkle);
- FromRootToJoint.erase(
- FromRootToJoint.begin()); // be careful with that line potential bug
+ FromRootToJoint.erase(FromRootToJoint.begin());
+ // be careful with that line potential bug
InitializationMaps(FromRootToJoint,ActuatedJoints,
m_RightLegIndexinConfiguration,
m_RightLegIndexinVelocity);
@@ -446,7 +429,8 @@ InitializationCoM
lStartingCOMPosition.setZero();
lStartingWaistPose.setZero();
- // Compute the forward dynamics from the configuration vector provided by the user.
+ // Compute the forward dynamics from the configuration vector
+ // provided by the user.
// Initialize waist pose.
InitializationHumanoid(BodyAnglesIni,lStartingWaistPose);
@@ -483,9 +467,9 @@ InitializationCoM
lStartingCOMPosition[2] -= InitRightFootPosition.z;
// Vector to go from CoM to Waist.
- m_DiffBetweenComAndWaist[0] = lStartingWaistPose(0) - lStartingCOMPosition[0];
- m_DiffBetweenComAndWaist[1] = lStartingWaistPose(1) - lStartingCOMPosition[1];
- m_DiffBetweenComAndWaist[2] = lStartingWaistPose(2) - lStartingCOMPosition[2];
+ m_DiffBetweenComAndWaist[0] = lStartingWaistPose(0)-lStartingCOMPosition[0];
+ m_DiffBetweenComAndWaist[1] = lStartingWaistPose(1)-lStartingCOMPosition[1];
+ m_DiffBetweenComAndWaist[2] = lStartingWaistPose(2)-lStartingCOMPosition[2];
ODEBUG("lFootPosition[2]: " <<InitRightFootPosition.z);
ODEBUG( "Diff between Com and Waist" << m_DiffBetweenComAndWaist[0] << " "
<< m_DiffBetweenComAndWaist[1] << " "
@@ -542,7 +526,8 @@ InitializationUpperBody
// Check pre-condition.
if (getPinocchioRobot()==0)
{
- cerr << "ComAndFootRealizationByGeometry::InitializationUpperBody " << endl
+ cerr << "ComAndFootRealizationByGeometry::InitializationUpperBody "
+ << endl
<< "No Humanoid Specificites class given " << endl;
return false;
}
@@ -560,16 +545,20 @@ InitializationUpperBody
ODEBUG4("FinishAndRealizeStepSequence() - 3 ","DebugGMFKW.dat");
gettimeofday(&time2,0);
- //this function calculates a buffer with COM values after a first preview round,
- // currently required to calculate the arm swing before "onglobal step of control"
- // in order to take the arm swing motion into account in the second preview loop
+ //this function calculates a buffer with COM values after a first
+ // preview round,
+ // currently required to calculate the arm swing before "one global step
+ // of control"
+ // in order to take the arm swing motion into account in the second
+ // preview loop
deque<ZMPPosition> aZMPBuffer;
aZMPBuffer.resize(inCOMBuffer.size());
ODEBUG4("FinishAndRealizeStepSequence() - 4 ","DebugGMFKW.dat");
- // read upperbody data which has to be included in the patterngeneration second preview loop stability
+ // read upperbody data which has to be included in the patterngeneration
+ // second preview loop stability
string BodyDat = "UpperBodyDataFile.dat";
ODEBUG4("FinishAndRealizeStepSequence() - 5 ","DebugGMFKW.dat");
@@ -601,35 +590,7 @@ InitializationUpperBody
// Create the conversion array between the Upper body indexes
// and the Joint indexes.
- /*
- int UpperBodyJointNb = getHumanoidDynamicRobot()->GetUpperBodyJointNb();
- m_ConversionForUpperBodyFromLocalIndexToRobotDOFs = getHumanoidDynamicRobot()->GetUpperBodyJoints();
-
- for(unsigned int i=0;i<m_ConversionForUpperBodyFromLocalIndexToRobotDOFs.size();i++)
- {
-
- m_ConversionForUpperBodyFromLocalIndexToRobotDOFs[i] = i;
- }
-
- for(unsigned int i=0;i<m_UpperBodyPositionsBuffer.size();i++)
- {
- m_UpperBodyPositionsBuffer[i].Joints.resize(UpperBodyJointNb);
-
- Eigen::VectorXd currentConfiguration;
- currentConfiguration = getHumanoidDynamicRobot()->currentConfiguration();
-
- if (i==0)
- {
- for(int j=0;j<UpperBodyJointNb;j++)
- m_UpperBodyPositionsBuffer[i].Joints[j] = currentConfiguration
- [m_ConversionForUpperBodyFromLocalIndexToRobotDOFs[j]];
- }
- // Initialize the upper body motion to the current stored value.
- for(int j=0;j<UpperBodyJointNb;j++)
- m_UpperBodyPositionsBuffer[i].Joints[j] = m_UpperBodyPositionsBuffer[0].Joints[j];
- }
- */
-
+
}
ODEBUG4("FinishAndRealizeStepSequence() - 6 ","DebugCG.ctx");
@@ -764,8 +725,9 @@ KinematicsForOneLeg
ODEBUG("Typeid of humanoid: " << typeid(getHumanoidDynamicRobot()).name() );
// Call specialized dynamics.
- getPinocchioRobot()->ComputeSpecializedInverseKinematics(
- Waist,Ankle,BodyPose,FootPose,lq);
+ getPinocchioRobot()->
+ ComputeSpecializedInverseKinematics
+ (Waist,Ankle,BodyPose,FootPose,lq);
ODEBUG4("lq " << lq,"DebugDataIK.dat");
return true;
}
@@ -1017,7 +979,7 @@ ComputePostureForGivenCoMAndFeetPosture
for(unsigned int i=0; i<CurrentAcceleration.size(); i++)
{
CurrentVelocity[i]=0.0;
- CurrentAcceleration[i] = 0.0;
+ CurrentAcceleration[i] = 0.0;
/* Keep the new value for the legs. */
}
@@ -1041,7 +1003,8 @@ ComputePostureForGivenCoMAndFeetPosture
if (IterationNumber>1)
{
for(unsigned int i=6; i<m_prev_Velocity.size(); i++)
- CurrentAcceleration[i] = (CurrentVelocity[i] - m_prev_Velocity[i])/ ldt;
+ CurrentAcceleration[i] =
+ (CurrentVelocity[i] - m_prev_Velocity[i])/ ldt;
}
}
else
@@ -1073,7 +1036,8 @@ ComputePostureForGivenCoMAndFeetPosture
if (IterationNumber>1)
{
for(unsigned int i=6; i<m_prev_Velocity1.size(); i++)
- CurrentAcceleration[i] = (CurrentVelocity[i] - m_prev_Velocity1[i])/ ldt;
+ CurrentAcceleration[i] =
+ (CurrentVelocity[i] - m_prev_Velocity1[i])/ ldt;
}
}
else
@@ -1143,12 +1107,12 @@ ComputePostureForGivenCoMAndFeetPosture
aCoMSpeed(4)*waistCom(1) +
aCoMSpeed(5)*waistCom(2) ;
- coriolis(0) = waistCom(0) * omega_dot_omega - aCoMSpeed(
- 3) * omega_dot_waistCom ;
- coriolis(1) = waistCom(1) * omega_dot_omega - aCoMSpeed(
- 4) * omega_dot_waistCom ;
- coriolis(2) = waistCom(2) * omega_dot_omega - aCoMSpeed(
- 5) * omega_dot_waistCom ;
+ coriolis(0) = waistCom(0) * omega_dot_omega -
+ aCoMSpeed( 3) * omega_dot_waistCom ;
+ coriolis(1) = waistCom(1) * omega_dot_omega -
+ aCoMSpeed(4) * omega_dot_waistCom ;
+ coriolis(2) = waistCom(2) * omega_dot_omega -
+ aCoMSpeed(5) * omega_dot_waistCom ;
// a_waist = a_com + waist-com x d omega/dt + (omega x waist-com) x omega
CurrentAcceleration[0] = aCoMAcc(0) +
@@ -1277,14 +1241,14 @@ ComputeUpperBodyHeuristicForNormalWalking(Eigen::VectorXd & qArmr,
TempCos = cos(aCOMPosition(5)*M_PI/180.0);
TempSin = sin(aCOMPosition(5)*M_PI/180.0);
- TempXR = TempCos * (RFP(0) +m_AnklePositionRight[0] - aCOMPosition(
- 0) - m_COGInitialAnkles(0)) +
- TempSin * (RFP(1) +m_AnklePositionRight[1] - aCOMPosition(
- 1) - m_COGInitialAnkles(1));
- TempXL = TempCos * (LFP(0) +m_AnklePositionRight[0] - aCOMPosition(
- 0) - m_COGInitialAnkles(0)) +
- TempSin * (LFP(1) +m_AnklePositionRight[1] - aCOMPosition(
- 1) - m_COGInitialAnkles(1));
+ TempXR = TempCos * (RFP(0) +m_AnklePositionRight[0] -
+ aCOMPosition(0) - m_COGInitialAnkles(0)) +
+ TempSin * (RFP(1) +m_AnklePositionRight[1] -
+ aCOMPosition( 1) - m_COGInitialAnkles(1));
+ TempXL = TempCos * (LFP(0) +m_AnklePositionRight[0] -
+ aCOMPosition(0) - m_COGInitialAnkles(0)) +
+ TempSin * (LFP(1) +m_AnklePositionRight[1] -
+ aCOMPosition(1) - m_COGInitialAnkles(1));
ODEBUG4(aCOMPosition(0) << " " << aCOMPosition(1) << " " << aCOMPosition(3),
"DebugDataIKArms.txt");
@@ -1312,12 +1276,12 @@ ComputeUpperBodyHeuristicForNormalWalking(Eigen::VectorXd & qArmr,
TempALeft * m_GainFactor / 0.2;
jointEndPosition(2,3) = m_ZARM;
- getPinocchioRobot()->ComputeSpecializedInverseKinematics(
- m_LeftShoulder,
- getPinocchioRobot()->leftWrist(),
- jointRootPosition,
- jointEndPosition,
- qArml);
+ getPinocchioRobot()->
+ ComputeSpecializedInverseKinematics
+ (m_LeftShoulder, getPinocchioRobot()->leftWrist(),
+ jointRootPosition,
+ jointEndPosition,
+ qArml);
ODEBUG4("ComputeHeuristicArm: Step 2 ","DebugDataIKArms.txt");
ODEBUG4( "IK Left arm p:" << qArml(0)<< " " << qArml(1) << " " << qArml(2)
<< " " << qArml(3) << " " << qArml(4) << " " << qArml(5),
@@ -1326,12 +1290,13 @@ ComputeUpperBodyHeuristicForNormalWalking(Eigen::VectorXd & qArmr,
jointEndPosition(0,3) = TempARight;
jointEndPosition(2,3) = m_ZARM;
- getPinocchioRobot()->ComputeSpecializedInverseKinematics(
- m_RightShoulder,
- getPinocchioRobot()->rightWrist(),
- jointRootPosition,
- jointEndPosition,
- qArmr);
+ getPinocchioRobot()->
+ ComputeSpecializedInverseKinematics
+ (m_RightShoulder,
+ getPinocchioRobot()->rightWrist(),
+ jointRootPosition,
+ jointEndPosition,
+ qArmr);
ODEBUG4( "IK Right arm p:" << qArmr(0)<< " " << qArmr(1) << " " << qArmr(2)
<< " " << qArmr(3) << " " << qArmr(4) << " " << qArmr(5),
"DebugDataIKArms.txt" );
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.hh b/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
index 6a7a51dc6d656681178b6f1372ea55080e5c5f91..9fab62904af68850ef2f4659b946c255d50250d9 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
@@ -67,29 +67,38 @@ namespace PatternGeneratorJRL
~ComAndFootRealizationByGeometry();
/** @} */
- /*! Initialization which should be done after setting the HumanoidDynamicRobot member. */
+ /*! Initialization which should be done after setting the
+ HumanoidDynamicRobot member. */
void Initialization();
/*! Compute the robot state for a given CoM and feet posture.
- Each posture is given by a 3D position and two Euler angles \f$ (\theta, \omega) \f$.
+ Each posture is given by a 3D position and two Euler angles
+ \f$ (\theta, \omega) \f$.
Very important: This method is assume to set correctly the body angles of
its \a HumanoidDynamicRobot and a subsequent call to the ZMP position
will return the associated ZMP vector.
- @param[in] CoMPosition a 6 dimensional vector with the first 3 dimension for position,
+ @param[in] CoMPosition a 6 dimensional vector with the first 3 dimension
+ for position,
and the last two for the orientation (Euler angle).
- @param[in] aCoMSpeed a 6 dimensional vector with the first 3 dimension for linear velocity,
+ @param[in] aCoMSpeed a 6 dimensional vector with the first 3 dimension
+ for linear velocity,
and the last two for the angular velocity.
- @param[in] aCoMAcc a 6 dimensional vector with the first 3 dimension for linear velocity,
+ @param[in] aCoMAcc a 6 dimensional vector with the first 3 dimension
+ for linear velocity,
and the last two for the angular velocity.
- @param[in] LeftFoot a 6 dimensional following the same convention than for \a CoMPosition.
+ @param[in] LeftFoot a 6 dimensional following the same convention than
+ for \a CoMPosition.
@param[in] RightFoot idem.
@param[out] CurrentConfiguration The result is a state vector containing
the position which are put inside this parameter.
- @param[out] CurrentVelocity The result is a state vector containing the speed which are put inside this parameter.
- @param[out] CurrentAcceleration The result is a state vector containing the acceleratio which are put inside this parameter.
+ @param[out] CurrentVelocity The result is a state vector containing
+ the speed which are put inside this parameter.
+ @param[out] CurrentAcceleration The result is a state vector containing
+ the acceleratio which are put inside this parameter.
@param[in] IterationNumber Number of iteration.
- @param[in] Stage indicates which stage is reach by the Pattern Generator. If this is the
+ @param[in] Stage indicates which stage is reach by the Pattern Generator.
+ If this is the
last stage, we store some information.
*/
@@ -113,10 +122,12 @@ namespace PatternGeneratorJRL
/*! \brief Initialize the humanoid model considering the current
configuration set by the user.
\param[in] BodyAnglesIni: The configuration vector provided by the user.
- \param[out] lStartingWaistPose: The waist pose according to the user configuration vector.
+ \param[out] lStartingWaistPose: The waist pose according to the user
+ configuration vector.
*/
- bool InitializationHumanoid(Eigen::VectorXd &BodyAnglesIni,
- Eigen::Matrix<double, 6, 1> &lStartingWaistPose);
+ bool InitializationHumanoid
+ (Eigen::VectorXd &BodyAnglesIni,
+ Eigen::Matrix<double, 6, 1> &lStartingWaistPose);
/*! \brief Initialize the foot position.
\param[in] aFoot: Pointer to the foot to be updated.
@@ -146,24 +157,23 @@ namespace PatternGeneratorJRL
/*! This initialization phase, make sure that the needed buffers
for the upper body motion are correctly setup.
*/
- bool InitializationUpperBody(deque<ZMPPosition> &inZMPPositions,
- deque<COMPosition> &inCOMBuffer,
- deque<RelativeFootPosition> lRelativeFootPositions);
+ bool InitializationUpperBody
+ (deque<ZMPPosition> &inZMPPositions,
+ deque<COMPosition> &inCOMBuffer,
+ deque<RelativeFootPosition> lRelativeFootPositions);
/* @} */
-
-
-
/*! Evaluate CoM for a given position.
Assuming that the waist is at (0,0,0)
It returns the associate initial values for the left and right foot.
*/
- int EvaluateCOMForStartingPosition(Eigen::VectorXd &BodyAngles,
- double omega, double theta,
- Eigen::Vector3d &lCOMPosition,
- FootAbsolutePosition & LeftFootPosition,
- FootAbsolutePosition & RightFootPosition);
+ int EvaluateCOMForStartingPosition
+ (Eigen::VectorXd &BodyAngles,
+ double omega, double theta,
+ Eigen::Vector3d &lCOMPosition,
+ FootAbsolutePosition & LeftFootPosition,
+ FootAbsolutePosition & RightFootPosition);
/*! Evaluate CoM for a given position.
Assuming that the waist is at (0,0,0)
@@ -181,13 +191,15 @@ namespace PatternGeneratorJRL
FootAbsolutePosition & InitRightFootPosition);
/*! Method to compute the heuristic for the arms. */
- void ComputeUpperBodyHeuristicForNormalWalking(Eigen::VectorXd & qArmr,
- Eigen::VectorXd & qArml,
- Eigen::VectorXd & aCOMPosition,
- Eigen::VectorXd & RFP,
- Eigen::VectorXd & LFP);
-
- /*! This method returns the final COM pose matrix after the second stage of control. */
+ void ComputeUpperBodyHeuristicForNormalWalking
+ (Eigen::VectorXd & qArmr,
+ Eigen::VectorXd & qArml,
+ Eigen::VectorXd & aCOMPosition,
+ Eigen::VectorXd & RFP,
+ Eigen::VectorXd & LFP);
+
+ /*! This method returns the final COM pose matrix
+ after the second stage of control. */
Eigen::MatrixXd GetFinalDesiredCOMPose();
/*! Returns the position of the Waist in the the COM Frame . */
@@ -198,11 +210,13 @@ namespace PatternGeneratorJRL
/*! Reimplementation of the setter of the HumanoidDynamicRobot. */
bool setPinocchioRobot(PinocchioRobot * aHumanoidDynamicRobot);
- /*! Compute the angles values considering a 6DOF leg for a given configuration
+ /*! Compute the angles values considering a 6DOF leg for a given
+ configuration
of the waist and the foot of the leg:
@param[in] Body_R : orientation of the Waist.
@param[in] Body_P: position of the waist.
- @param[in] aFoot: A vector giving the foot configuration (x,y,z, theta, omega).
+ @param[in] aFoot: A vector giving the foot configuration
+ (x,y,z, theta, omega).
@param[in] lDt: Vector describing the hip configuration.
@param[in] aCoMPosition: Position of the CoM.
@param[in] ToTheHip: Vector to go from the Waist to the Hip.
@@ -219,7 +233,8 @@ namespace PatternGeneratorJRL
Eigen::VectorXd &lq,
int Stage);
- /*! Compute the angles values considering two 6DOF legs for a given configuration
+ /*! Compute the angles values considering two 6DOF legs for a given
+ configuration
of the waist and of the feet:
@param aCoMPosition: Position of the CoM (x,y,z,theta, omega, phi).
@param aLeftFoot: Position of the foot (x,y,z, theta, omega).
@@ -248,8 +263,10 @@ namespace PatternGeneratorJRL
*/
Eigen::Matrix4d GetCurrentPositionofWaistInCOMFrame();
- /*! \brief Getter and setter for the previous configurations and velocities */
- inline void SetPreviousConfigurationStage0(Eigen::VectorXd & prev_Configuration)
+ /*! \brief Getter and setter for the previous
+ configurations and velocities */
+ inline void SetPreviousConfigurationStage0
+ (Eigen::VectorXd & prev_Configuration)
{
m_prev_Configuration = prev_Configuration ;
}
@@ -258,7 +275,8 @@ namespace PatternGeneratorJRL
m_prev_Velocity = prev_Velocity ;
}
- inline void SetPreviousConfigurationStage1(Eigen::VectorXd & prev_Configuration)
+ inline void SetPreviousConfigurationStage1
+ (Eigen::VectorXd & prev_Configuration)
{
m_prev_Configuration1 = prev_Configuration ;
}
@@ -267,7 +285,8 @@ namespace PatternGeneratorJRL
m_prev_Velocity1 = prev_Velocity ;
}
- inline void SetPreviousConfigurationStage2(Eigen::VectorXd & prev_Configuration)
+ inline void SetPreviousConfigurationStage2
+ (Eigen::VectorXd & prev_Configuration)
{
m_prev_Configuration2 = prev_Configuration ;
}
@@ -276,7 +295,8 @@ namespace PatternGeneratorJRL
m_prev_Velocity2 = prev_Velocity ;
}
- /*! \brief Getter and setter for the previous configurations and velocities */
+ /*! \brief Getter and setter for the
+ previous configurations and velocities */
inline void SetPreviousConfigurationStage0(const Eigen::VectorXd &
prev_Configuration)
{
@@ -307,7 +327,8 @@ namespace PatternGeneratorJRL
m_prev_Velocity2 = prev_Velocity;
}
- /*! \brief Getter and setter for the previous configurations and velocities */
+ /*! \brief Getter and setter for the previous
+ configurations and velocities */
inline Eigen::VectorXd & GetPreviousConfigurationStage0()
{
return m_prev_Configuration ;
@@ -328,19 +349,23 @@ namespace PatternGeneratorJRL
return m_prev_Velocity1 ;
};
- inline void leftLegIndexinConfiguration(std::vector<int> & leftLegMaps) const
+ inline void leftLegIndexinConfiguration
+ (std::vector<int> & leftLegMaps) const
{
leftLegMaps = m_LeftLegIndexinConfiguration ;
}
- inline void rightLegIndexinConfiguration(std::vector<int> & rightLegMaps) const
+ inline void rightLegIndexinConfiguration
+ (std::vector<int> & rightLegMaps) const
{
rightLegMaps = m_RightLegIndexinConfiguration ;
}
- inline void leftArmIndexinConfiguration(std::vector<int> & leftArmMaps) const
+ inline void leftArmIndexinConfiguration
+ (std::vector<int> & leftArmMaps) const
{
leftArmMaps = m_LeftArmIndexinConfiguration ;
}
- inline void rightArmIndexinConfiguration(std::vector<int> & rightArmMaps) const
+ inline void rightArmIndexinConfiguration
+ (std::vector<int> & rightArmMaps) const
{
rightArmMaps = m_RightArmIndexinConfiguration ;
}
diff --git a/src/MotionGeneration/GenerateMotionFromKineoWorks.cpp b/src/MotionGeneration/GenerateMotionFromKineoWorks.cpp
index eb7db8d849f33067af5b42c8b3b59c4d84273ddc..66fa40349ad19597b0df4e26656fc439165a955a 100644
--- a/src/MotionGeneration/GenerateMotionFromKineoWorks.cpp
+++ b/src/MotionGeneration/GenerateMotionFromKineoWorks.cpp
@@ -95,7 +95,8 @@ namespace PatternGeneratorJRL
aif >> version;
if (version!=2.0)
{
- cerr << "Not version 2.0. Do not understand the information. Aborted. " << endl;
+ cerr << "Not version 2.0. Do not understand the information. "
+ << "Aborted. " << endl;
return -1;
}
@@ -193,8 +194,9 @@ namespace PatternGeneratorJRL
}
- void GenerateMotionFromKineoWorks::CreateBufferFirstPreview(
- deque<ZMPPosition> &ZMPRefBuffer)
+ void GenerateMotionFromKineoWorks::
+ CreateBufferFirstPreview
+ (deque<ZMPPosition> &ZMPRefBuffer)
{
deque<ZMPPosition> aFIFOZMPRefPositions;
Eigen::MatrixXd aPC1x;
@@ -295,14 +297,16 @@ namespace PatternGeneratorJRL
m_NL = (unsigned int)(m_PreviewControlTime/m_SamplingPeriod);
}
- void GenerateMotionFromKineoWorks::ComputeUpperBodyPosition(
- deque< KWNode >& UpperBodyPositionsBuffer,
- vector<int> &ConversionFromLocalToRobotDOFsIndex)
+ void GenerateMotionFromKineoWorks::
+ ComputeUpperBodyPosition
+ (deque< KWNode >& UpperBodyPositionsBuffer,
+ vector<int> &ConversionFromLocalToRobotDOFsIndex)
{
vector<int> ConversionFromLocalToKW;
int count=0;
//! Find the sizes for the buffer, and the conversion array..
- //! First dimension: count the number of DOFs which are -1 inside the array of indexes.
+ //! First dimension: count the number of DOFs which are -1
+ // inside the array of indexes.
int NbOfUsedDOFs=0;
KWNode deltaJoints;
@@ -313,14 +317,16 @@ namespace PatternGeneratorJRL
deltaJoints.Joints.resize(NbOfUsedDOFs);
ConversionFromLocalToKW.resize(NbOfUsedDOFs);
- //! Second dimension: directly the number of elements inside the COM's buffer of position.
+ //! Second dimension: directly the number of elements inside
+ // the COM's buffer of position.
UpperBodyPositionsBuffer.resize(m_COMBuffer.size());
for(unsigned int i=0; i<UpperBodyPositionsBuffer.size(); i++)
{
UpperBodyPositionsBuffer[i].Joints.resize(NbOfUsedDOFs);
}
- //! First initialize the first upper body position at the beginning of the motion.
+ //! First initialize the first upper body position at the
+ // beginning of the motion.
//! and fill the conversion array.
int k=0;
for(unsigned int i=0; i<m_Path[0].Joints.size(); i++)
@@ -349,7 +355,8 @@ namespace PatternGeneratorJRL
lY = m_Path[IdWayPoint].Joints[7];
lZ = m_Path[IdWayPoint].Joints[8];
- //! Find the closest (X,Y,Z) position in the remaining part of the CoM buffer.
+ //! Find the closest (X,Y,Z) position in the remaining
+ // part of the CoM buffer.
for(unsigned int i=count; i<m_COMBuffer.size(); i++)
{
double ldist= (lX-m_COMBuffer[i].x[0])*(lX - m_COMBuffer[i].x[0])
@@ -376,13 +383,15 @@ namespace PatternGeneratorJRL
int k = ConversionFromLocalToKW[i];
deltaJoints.Joints[i]= (m_Path[IdWayPoint].Joints[k] -
- m_Path[IdWayPoint-1].Joints[k])/(double)(CountTarget-count);
+ m_Path[IdWayPoint-1].Joints[k])/(double)(CountTarget-count);
}
//! Fill the buffer with linear interpolation.
while(count<=CountTarget)
{
- for(unsigned int i=0; i<ConversionFromLocalToRobotDOFsIndex.size(); i++)
+ for(unsigned int i=0;
+ i<ConversionFromLocalToRobotDOFsIndex.size();
+ i++)
{
UpperBodyPositionsBuffer[count].Joints[i]=
UpperBodyPositionsBuffer[count-1].Joints[i]
diff --git a/src/MotionGeneration/StepOverPlanner.cpp b/src/MotionGeneration/StepOverPlanner.cpp
index 0a3027e09dc6680724f13291db7915363d1867d9..77d45ee18120ffbcbb4f4b77b2eb56ef28879a59 100644
--- a/src/MotionGeneration/StepOverPlanner.cpp
+++ b/src/MotionGeneration/StepOverPlanner.cpp
@@ -60,8 +60,8 @@ StepOverPlanner::StepOverPlanner(ObstaclePar &ObstacleParameters,
else
{
lWidth = 0.2;
- cerr << "WARNING: no object with humanoid specificities properly defined." <<
- endl;
+ cerr << "WARNING: no object with humanoid specificities properly defined."
+ << endl;
m_AnkleSoilDistance = 0.1;
m_tipToAnkle = 0.1;
m_heelToAnkle = 0.1;
@@ -82,7 +82,8 @@ StepOverPlanner::StepOverPlanner(ObstaclePar &ObstacleParameters,
m_nominalStepWidth = 0.19;
- //this angle is used to limit the position during feasibility in double support over the obstacle
+ //this angle is used to limit the position during feasibility in
+ // double support over the obstacle
m_KneeAngleBound=15.0*M_PI/180.0;
m_Tsingle = 0.78;
@@ -130,7 +131,8 @@ StepOverPlanner::StepOverPlanner(ObstaclePar &ObstacleParameters,
m_StaticToTheRightHip(1) = -0.1;
m_StaticToTheRightHip(2) = m_DiffBetweenComAndWaist;
- // defining the points on the shank to set the boundary lines of the leg layout
+ // defining the points on the shank to set the boundary lines
+ // of the leg layout
// for the values of the coordinates see paper guan san IROS 2004
// 'feasibility of humanoid robots stepping over obstacles'
@@ -231,7 +233,8 @@ void StepOverPlanner::CalculateFootHolds(deque<RelativeFootPosition>
/// Returns the double support time.
float GetTDoubleSupport();
- DoubleSupportFeasibility(); //perform this function to set m_StepOverStepLenght and m_StepOverHipHeight;
+ DoubleSupportFeasibility();
+ //perform this function to set m_StepOverStepLenght and m_StepOverHipHeight;
double ankleDistToObstacle;
@@ -251,13 +254,13 @@ void StepOverPlanner::CalculateFootHolds(deque<RelativeFootPosition>
double walkStepLenght;
- walkStepLenght=m_nominalStepLenght+(footDistLeftToMove
- -m_nominalStepLenght*numberOfSteps)/numberOfSteps;
+ walkStepLenght=m_nominalStepLenght+
+ (footDistLeftToMove-m_nominalStepLenght*numberOfSteps)/numberOfSteps;
- cout << "Obstacle height with safety boundary:" << m_ObstacleParameters.h <<
- endl;
- cout << "Obstacle thickness with safety boundary:" << m_ObstacleParameters.d <<
- endl;
+ cout << "Obstacle height with safety boundary:"
+ << m_ObstacleParameters.h << endl;
+ cout << "Obstacle thickness with safety boundary:"
+ << m_ObstacleParameters.d << endl;
cout << "Distance to Obstacle:" << m_ObstacleParameters.x << endl;
cout << "StepOver steplenght:" << m_StepOverStepLenght << endl;
cout << "StepOver COMHeight:" << m_StepOverHipHeight << endl;
@@ -283,7 +286,8 @@ void StepOverPlanner::CalculateFootHolds(deque<RelativeFootPosition>
for (int i=0; i<numberOfSteps-1; i++)
{
tempPos.sx=walkStepLenght;
- tempPos.sy=(-1.0)*(tempPos.sy)/std::fabs((double)tempPos.sy)*m_nominalStepWidth;
+ tempPos.sy=(-1.0)*(tempPos.sy)/
+ std::fabs((double)tempPos.sy)*m_nominalStepWidth;
tempPos.theta=0.0;
tempPos.SStime=m_Tsingle;
tempPos.DStime=m_Tdble;
@@ -294,7 +298,8 @@ void StepOverPlanner::CalculateFootHolds(deque<RelativeFootPosition>
// one step before stepover obstacle
tempPos.sx=walkStepLenght;
- tempPos.sy=(-1.0)*(tempPos.sy)/std::fabs((double)tempPos.sy)*m_nominalStepWidth;
+ tempPos.sy=(-1.0)*(tempPos.sy)
+ /std::fabs((double)tempPos.sy)*m_nominalStepWidth;
tempPos.theta=0.0;
tempPos.SStime=m_TsingleStepOverBeforeAfter;
tempPos.DStime=m_TdbleStepOverBeforeAfter;
@@ -304,7 +309,8 @@ void StepOverPlanner::CalculateFootHolds(deque<RelativeFootPosition>
// first leg over the obsacle
tempPos.sx=m_StepOverStepLenght;
- tempPos.sy=(-1.0)*(tempPos.sy)/std::fabs((double)tempPos.sy)*m_nominalStepWidth;
+ tempPos.sy=(-1.0)*(tempPos.sy)
+ /std::fabs((double)tempPos.sy)*m_nominalStepWidth;
//*cos(m_WaistRotationStepOver*M_PI/180.0);
tempPos.theta=0.0;
tempPos.SStime=m_TsingleStepOver;
@@ -315,7 +321,8 @@ void StepOverPlanner::CalculateFootHolds(deque<RelativeFootPosition>
// second leg over the obsacle
tempPos.sx=m_nominalStepLenght;
- tempPos.sy=(-1.0)*(tempPos.sy)/std::fabs((double)tempPos.sy)*m_nominalStepWidth;
+ tempPos.sy=(-1.0)*(tempPos.sy)
+ /std::fabs((double)tempPos.sy)*m_nominalStepWidth;
//*cos(m_WaistRotationStepOver*M_PI/180.0);
tempPos.theta=0.0;
tempPos.SStime=m_TsingleStepOver;
@@ -325,7 +332,8 @@ void StepOverPlanner::CalculateFootHolds(deque<RelativeFootPosition>
//one step after the obstacle stepping over
tempPos.sx=m_nominalStepLenght;
- tempPos.sy=(-1.0)*(tempPos.sy)/std::fabs((double)tempPos.sy)*m_nominalStepWidth;
+ tempPos.sy=(-1.0)*(tempPos.sy)
+ /std::fabs((double)tempPos.sy)*m_nominalStepWidth;
tempPos.theta=0.0;
tempPos.SStime=m_TsingleStepOverBeforeAfter;
tempPos.DStime=m_TdbleStepOverBeforeAfter;
@@ -334,7 +342,8 @@ void StepOverPlanner::CalculateFootHolds(deque<RelativeFootPosition>
//one extra regular step
tempPos.sx=m_nominalStepLenght;
- tempPos.sy=(-1.0)*(tempPos.sy)/std::fabs((double)tempPos.sy)*m_nominalStepWidth;
+ tempPos.sy=(-1.0)*(tempPos.sy)
+ /std::fabs((double)tempPos.sy)*m_nominalStepWidth;
tempPos.theta=0.0;
tempPos.SStime=m_Tsingle;
tempPos.DStime=m_Tdble;
@@ -344,7 +353,8 @@ void StepOverPlanner::CalculateFootHolds(deque<RelativeFootPosition>
//last step
tempPos.sx=0;
- tempPos.sy=(-1.0)*(tempPos.sy)/std::fabs((double)tempPos.sy)*m_nominalStepWidth;
+ tempPos.sy=(-1.0)*(tempPos.sy)
+ /std::fabs((double)tempPos.sy)*m_nominalStepWidth;
tempPos.theta=0.0;
tempPos.SStime=m_Tsingle;
tempPos.DStime=m_Tdble;
@@ -361,7 +371,8 @@ void StepOverPlanner::DoubleSupportFeasibility()
double StepOverStepLenght, StepOverStepLenghtMin, StepOverStepLenghtMax;
double StepOverCOMHeight, StepOverCOMHeightMin, StepOverCOMHeightMax;
double OrientationFeetToObstacle = 0.0, OmegaAngleFeet = 0.0;
- //this is the factor determining aproximately the COM position due to preview control during double support
+ //this is the factor determining
+ // aproximately the COM position due to preview control during double support
double DoubleSupportCOMPosFactor;
@@ -400,50 +411,57 @@ void StepOverPlanner::DoubleSupportFeasibility()
bool CollisionStatus, FinalCollisionStatus;
- StepOverStepLenghtMin = m_ObstacleParameters.d + m_heelToAnkle + m_tipToAnkle +
- m_heelDistAfter + m_tipDistBefore;
+ StepOverStepLenghtMin = m_ObstacleParameters.d + m_heelToAnkle +
+ m_tipToAnkle + m_heelDistAfter + m_tipDistBefore;
StepOverStepLenghtMax = 0.6;
- //0.4 - m_DiffBetweenComAndWaist + m_soleToAnkle;0.6 * cos(90.0*M_PI/180.0/2.0)
+ //0.4 - m_DiffBetweenComAndWaist + m_soleToAnkle;0.6 *cos(90.0*M_PI/180.0/2.0)
StepOverCOMHeightMin = 0.4- m_DiffBetweenComAndWaist + m_soleToAnkle;
- //m_NominalCOMStepHeight;//0.6 * cos(m_KneeAngleBound/2.0) - m_DiffBetweenComAndWaist + m_soleToAnkle;
+ //m_NominalCOMStepHeight;//0.6 * cos(m_KneeAngleBound/2.0) -
+ // m_DiffBetweenComAndWaist + m_soleToAnkle;
StepOverCOMHeightMax =0.75-m_DeltaStepOverCOMHeightMax;
IncrementStepLenght = double ((StepOverStepLenghtMax -
StepOverStepLenghtMin)/((EvaluationNumber)));
- IncrementCOMHeight = double ((StepOverCOMHeightMax - StepOverCOMHeightMin)/((
- EvaluationNumber)));
-
- /*! this angle can be used to extend the steplength during stepover but currently it is set to 0 convinience*/
+ IncrementCOMHeight = double ((StepOverCOMHeightMax - StepOverCOMHeightMin)
+ /((EvaluationNumber)));
+ /*! this angle can be used to extend the steplength
+ during stepover but currently it is set to 0 convinience*/
- /*! this parameter should be evaluated and checked and in the end to be retreieved
+ /*! this parameter should be evaluated and checked and in the end
+ to be retreieved
from a table containing these values for different step situations ...
for which a first round of preview control has been performed */
DoubleSupportCOMPosFactor = 0.50;
CollisionStatus = 1;
FinalCollisionStatus = 1;
- /*! we suppose that both feet have the same orentation with respect to the obstacle */
+ /*! we suppose that both feet have the same orentation with respect
+ to the obstacle */
for (int i=0; i<EvaluationNumber+1; i++)
{
for (int j=0; j<EvaluationNumber+1; j++)
{
StepOverStepLenght = StepOverStepLenghtMin + i*IncrementStepLenght;
- StepOverCOMHeight = StepOverCOMHeightMax - (double(j*IncrementCOMHeight));
+ StepOverCOMHeight = StepOverCOMHeightMax -
+ (double(j*IncrementCOMHeight));
StepOverStepWidth = m_nominalStepWidth;
//cout << "StepOverStepcd ../Lenght: " << StepOverStepLenght <<
- //" StepOverStepWidth: " << StepOverStepWidth << " StepOverCOMHeight: " << StepOverCOMHeight << endl;
+ //" StepOverStepWidth: " << StepOverStepWidth
+ // << " StepOverCOMHeight: " << StepOverCOMHeight << endl;
//coordinates ankles in obstacle frame
//assuming the left foot is in front of the obstacle
- //and that in the Y direction of the obstacle the feet are symmetrical with respect to the obstacle origin
+ //and that in the Y direction of the obstacle the feet
+ // are symmetrical with respect to the obstacle origin
- AnkleBeforeObst(0) =-( StepOverStepLenght-m_heelToAnkle-m_heelDistAfter
+ AnkleBeforeObst(0) =-( StepOverStepLenght-m_heelToAnkle-
+ m_heelDistAfter
-m_ObstacleParameters.d);
AnkleBeforeObst(1) = StepOverStepWidth/2.0;
AnkleBeforeObst(2) = m_soleToAnkle;
@@ -453,14 +471,17 @@ void StepOverPlanner::DoubleSupportFeasibility()
AnkleAfterObst(2) = m_soleToAnkle;
- //position left foot in front of the obstacle to world frame coordinates
+ //position left foot in front of the obstacle
+ //to world frame coordinates
Foot_P = m_ObstaclePosition + m_ObstacleRot*AnkleBeforeObst;
TempCOMState(0) = AnkleBeforeObst(0)+ DoubleSupportCOMPosFactor *
StepOverStepLenght;
TempCOMState(1) =
- 0.0; //suppose the preview control sets Y coordinate in the middel of the dubbel support
+ 0.0;
+ //suppose the preview control sets Y coordinate
+ // in the middle of the double support
TempCOMState(2) = StepOverCOMHeight;
//to worldframe
@@ -471,7 +492,8 @@ void StepOverPlanner::DoubleSupportFeasibility()
aCOMState.z[0] = TempCOMState(2);
aCOMState.yaw[0] = -
- m_WaistRotationStepOver;//m_ObstacleParameters.theta + OrientationHipToObstacle;
+ m_WaistRotationStepOver;
+ //m_ObstacleParameters.theta + OrientationHipToObstacle;
c = cos(aCOMState.yaw[0]*M_PI/180.0);
@@ -499,7 +521,9 @@ void StepOverPlanner::DoubleSupportFeasibility()
c = cos(m_ObstacleParameters.theta*M_PI/180.0);
s = sin(m_ObstacleParameters.theta*M_PI/180.0);
- co = cos(0.0*M_PI/180.0); //at the moment the feet stand flat on the ground when in double support phase
+ co = cos(0.0*M_PI/180.0);
+ //at the moment the feet stand flat on
+ // the ground when in double support phase
so = sin(0.0*M_PI/180.0);
Foot_R(0,0) = c*co;
@@ -570,7 +594,10 @@ void StepOverPlanner::DoubleSupportFeasibility()
- ///TO DO a check on all the maximum values for the angles after the inverse kinematics....or implement a check in the inverskinematics claas itself...at this moments there is only a protection against knee overstretch built in
+ ///TO DO a check on all the maximum values for the angles
+ //after the inverse kinematics....or implement a check in the
+ // inverskinematics claas itself...at this moments there is only a
+ // protection against knee overstretch built in
if (!((LeftLegAngles(3)<m_KneeAngleBound)
||(RightLegAngles(3)<m_KneeAngleBound)))
{
@@ -581,8 +608,10 @@ void StepOverPlanner::DoubleSupportFeasibility()
WaistRot = Body_R;
- //check collision : for the leg in front of the obstacle only lines (points 1, 2, 3, 4) on the shin
- // for the leg behind the obstacle only lines (point 5, 6, 7) on the calf
+ // check collision : for the leg in front of the obstacle
+ // only lines (points 1, 2, 3, 4) on the shin
+ // for the leg behind the obstacle only lines
+ // (point 5, 6, 7) on the calf
CollisionStatus = 0;
FinalCollisionStatus = 0;
@@ -593,25 +622,32 @@ void StepOverPlanner::DoubleSupportFeasibility()
PointOnLeg[0] = m_LegLayoutPoint(0,k);
PointOnLeg[1] = m_LegLayoutPoint(1,k);
PointOnLeg[2] = m_LegLayoutPoint(2,k);
- // MAL_MATRIX_C_eq_EXTRACT_A(PointOnLeg,m_LegLayoutPoint,double,0,k,3,1);
+ // MAL_MATRIX_C_eq_EXTRACT_A(PointOnLeg,
+ // m_LegLayoutPoint,double,0,k,3,1);
ODEBUG("PointOnLeg : " << k << endl << PointOnLeg << endl );
- m_CollDet->CalcCoordShankLowerLegPoint(PointOnLeg,AbsCoord,
- LeftLegAngles,WaistRot,WaistPos,1);
- m_CollDet->WorldFrameToObstacleFrame(AbsCoord, ObstFrameCoord1);
+ m_CollDet->CalcCoordShankLowerLegPoint
+ (PointOnLeg,AbsCoord, LeftLegAngles,WaistRot,WaistPos,1);
+ m_CollDet->WorldFrameToObstacleFrame
+ (AbsCoord, ObstFrameCoord1);
//PointOnLeg = m_LegLayoutPoint.GetNColumns(k+1,1);
PointOnLeg[0] = m_LegLayoutPoint(0,k+1);
PointOnLeg[1] = m_LegLayoutPoint(1,k+1);
PointOnLeg[2] = m_LegLayoutPoint(2,k+1);
- // MAL_MATRIX_C_eq_EXTRACT_A(PointOnLeg,m_LegLayoutPoint,double,0,k+1,3,1);
+ // MAL_MATRIX_C_eq_EXTRACT_A(PointOnLeg,
+ // m_LegLayoutPoint,double,0,k+1,3,1);
ODEBUG("PointOnLeg : " << k+1 << endl << PointOnLeg << endl );
- m_CollDet->CalcCoordShankLowerLegPoint(PointOnLeg,AbsCoord,
- LeftLegAngles,WaistRot,WaistPos,1);
- m_CollDet->WorldFrameToObstacleFrame(AbsCoord, ObstFrameCoord2);
- CollisionStatus = m_CollDet->CollisionLineObstacleComplete(ObstFrameCoord1,
- ObstFrameCoord2);
- //cout << "collision status for line with starting point " << k+1 << " is : " << CollisionStatus << endl;
- FinalCollisionStatus = FinalCollisionStatus || CollisionStatus;
+ m_CollDet->CalcCoordShankLowerLegPoint
+ (PointOnLeg,AbsCoord, LeftLegAngles,WaistRot,WaistPos,1);
+ m_CollDet->WorldFrameToObstacleFrame
+ (AbsCoord, ObstFrameCoord2);
+ CollisionStatus =
+ m_CollDet->CollisionLineObstacleComplete(ObstFrameCoord1,
+ ObstFrameCoord2);
+ //cout << "collision status for line with starting point "
+ // << k+1 << " is : " << CollisionStatus << endl;
+ FinalCollisionStatus = FinalCollisionStatus ||
+ CollisionStatus;
}
for (unsigned int k=4; k<6; k++)
{
@@ -620,24 +656,36 @@ void StepOverPlanner::DoubleSupportFeasibility()
PointOnLeg[0] = m_LegLayoutPoint(0,k);
PointOnLeg[1] = m_LegLayoutPoint(1,k);
PointOnLeg[2] = m_LegLayoutPoint(2,k);
- //MAL_MATRIX_C_eq_EXTRACT_A(PointOnLeg,m_LegLayoutPoint,double,0,k,3,1);
- m_CollDet->CalcCoordShankLowerLegPoint(PointOnLeg,AbsCoord,LeftLegAngles,
- WaistRot,WaistPos,1);
- m_CollDet->WorldFrameToObstacleFrame(AbsCoord, ObstFrameCoord1);
+ //MAL_MATRIX_C_eq_EXTRACT_A(PointOnLeg,
+ // m_LegLayoutPoint,double,0,k,3,1);
+ m_CollDet->CalcCoordShankLowerLegPoint
+ (PointOnLeg,AbsCoord,
+ LeftLegAngles,
+ WaistRot,WaistPos,1);
+ m_CollDet->WorldFrameToObstacleFrame
+ (AbsCoord,
+ ObstFrameCoord1);
//PointOnLeg = m_LegLayoutPoint.GetNColumns(k+1,1);
PointOnLeg[0] = m_LegLayoutPoint(0,k+1);
PointOnLeg[1] = m_LegLayoutPoint(1,k+1);
PointOnLeg[2] = m_LegLayoutPoint(2,k+1);
- //MAL_MATRIX_C_eq_EXTRACT_A(PointOnLeg,m_LegLayoutPoint,double,0,k+1,3,1);
+ //MAL_MATRIX_C_eq_EXTRACT_A(PointOnLeg,m_LegLayoutPoint,
+ // double,0,k+1,3,1);
ODEBUG("PointOnLeg : " << k+1 << endl << PointOnLeg << endl );
- m_CollDet->CalcCoordShankLowerLegPoint(PointOnLeg,AbsCoord,LeftLegAngles,
- WaistRot,WaistPos,1);
- m_CollDet->WorldFrameToObstacleFrame(AbsCoord, ObstFrameCoord2);
- CollisionStatus = m_CollDet->CollisionLineObstacleComplete(ObstFrameCoord1,
- ObstFrameCoord2);
- //cout << "collision status for line with starting point " << k+1 << " is : " << CollisionStatus << endl;
- FinalCollisionStatus = FinalCollisionStatus || CollisionStatus;
+ m_CollDet->CalcCoordShankLowerLegPoint
+ (PointOnLeg,AbsCoord,LeftLegAngles,
+ WaistRot,WaistPos,1);
+ m_CollDet->
+ WorldFrameToObstacleFrame(AbsCoord, ObstFrameCoord2);
+ CollisionStatus = m_CollDet->
+ CollisionLineObstacleComplete
+ (ObstFrameCoord1,
+ ObstFrameCoord2);
+ //cout << "collision status for line with starting point "
+ // << k+1 << " is : " << CollisionStatus << endl;
+ FinalCollisionStatus = FinalCollisionStatus ||
+ CollisionStatus;
}
}
//cout << "FinalCollisionStatus is " << FinalCollisionStatus << endl;
@@ -649,17 +697,22 @@ void StepOverPlanner::DoubleSupportFeasibility()
{
m_StepOverStepLenght = StepOverStepLenght;
m_StepOverHipHeight = StepOverCOMHeight;
- //cout << "feasibility selected StepOverStepLenght : " << StepOverStepLenght << " and StepOverCOMHeight : " << StepOverCOMHeight << endl;
- // cout << "while the nominal steplength is : " << m_nominalStepLenght << " and the nominal COMHeight is " << m_NominalCOMStepHeight << endl;;
+ //cout << "feasibility selected StepOverStepLenght : " <<
+ // StepOverStepLenght << " and StepOverCOMHeight : "
+ // << StepOverCOMHeight << endl;
+ // cout << "while the nominal steplength is : "
+ // << m_nominalStepLenght << " and the nominal COMHeight is "
+ // << m_NominalCOMStepHeight << endl;;
break;
}
}
}
-void StepOverPlanner::PolyPlanner(deque<COMState> &aCOMBuffer,
- deque<FootAbsolutePosition> & aLeftFootBuffer,
- deque<FootAbsolutePosition> & aRightFootBuffer,
- deque<ZMPPosition> & aZMPPositions)
+void StepOverPlanner::PolyPlanner
+(deque<COMState> &aCOMBuffer,
+ deque<FootAbsolutePosition> & aLeftFootBuffer,
+ deque<FootAbsolutePosition> & aRightFootBuffer,
+ deque<ZMPPosition> & aZMPPositions)
{
m_RightFootBuffer = aRightFootBuffer;
m_LeftFootBuffer = aLeftFootBuffer;
@@ -694,11 +747,13 @@ void StepOverPlanner::PolyPlanner(deque<COMState> &aCOMBuffer,
if ((std::fabs((double)m_LeftFootBuffer[u].stepType)==13)&
(m_EndPrevStepOver==0))
m_EndPrevStepOver = u;
- if ((std::fabs((double)m_LeftFootBuffer[u].stepType)==3)&(m_StartStepOver==0))
+ if ((std::fabs((double)m_LeftFootBuffer[u].stepType)==3)&
+ (m_StartStepOver==0))
m_StartStepOver = u;
if ((m_LeftFootBuffer[u].stepType==14)&(m_StartDoubleSupp==0))
m_StartDoubleSupp = u;
- if ((std::fabs((double)m_LeftFootBuffer[u].stepType)==4)&(m_StartSecondStep==0))
+ if ((std::fabs((double)m_LeftFootBuffer[u].stepType)==4)&
+ (m_StartSecondStep==0))
m_StartSecondStep = u;
if ((m_LeftFootBuffer[u].stepType==15)&(m_EndStepOver==0))
m_EndStepOver = u;
@@ -825,19 +880,22 @@ void StepOverPlanner::PolyPlannerFirstStep(deque<FootAbsolutePosition>
//m_ModulationSupportCoefficient=0.8;// MOET ERGENS ANDERS GEDEFINIEERD WORDEN
- //for now it is only in the 2D and with the obstacle perpendicular to absolute x direction
+ //for now it is only in the 2D and with the obstacle
+ //perpendicular to absolute x direction
Point1X = StepLenght-m_heelToAnkle-m_ObstacleParameters.d-xOffset
-m_tipToAnkle*cos(Omega1*M_PI/180.0);
Point1Y = 0.00;
Point1Z = m_ObstacleParameters.h-m_tipToAnkle*sin(Omega1*M_PI/180.0);
- Point2X = StepLenght-m_heelToAnkle+xOffset+m_heelToAnkle*cos(Omega2*M_PI/180.0);
+ Point2X = StepLenght-m_heelToAnkle+xOffset+
+ m_heelToAnkle*cos(Omega2*M_PI/180.0);
Point2Y = 0.00;
Point2Z = m_ObstacleParameters.h-m_tipToAnkle*sin(Omega2*M_PI/180.0);
Point3Z= Point1Z
- +0.04;// m_ObstacleParameters.h+zOffset+0.04+m_tipToAnkle*sin(Omega2*M_PI/180.0);
+ +0.04;
+ // m_ObstacleParameters.h+zOffset+0.04+m_tipToAnkle*sin(Omega2*M_PI/180.0);
vector<double> aTimeDistr,aTimeDistrModulated;
@@ -853,7 +911,9 @@ void StepOverPlanner::PolyPlannerFirstStep(deque<FootAbsolutePosition>
aTimeDistr[2]=StepTime;
- //this time schedule is used for the X and Y coordinate of the foot in order to make sure the foot lifts the ground (Z) before moving the X and Y direction
+ // this time schedule is used for the X and Y coordinate of the foot in order
+ // to make sure the foot lifts the ground (Z) before moving
+ // the X and Y direction
aTimeDistrModulated.resize(3);
@@ -901,18 +961,20 @@ void StepOverPlanner::PolyPlannerFirstStep(deque<FootAbsolutePosition>
for (int i=1; i<=NumberIntermediate; i++)
{
- SpeedWeightZ[i-1] = (EndSpeedZ-PreviousSpeedZ)*((double (i-1)/(double (
- NumberIntermediate))))+PreviousSpeedZ;
+ SpeedWeightZ[i-1] = (EndSpeedZ-PreviousSpeedZ)*
+ ((double (i-1)/
+ (double (NumberIntermediate))))+PreviousSpeedZ;
// cout << "SpeedWeightZ[i-1]" << SpeedWeightZ[i-1] << endl;
SpeedAccZ = SpeedAccZ + SpeedWeightZ[i-1];
}
for (int i=1; i<=NumberIntermediate; i++)
{
- IntermediateZAcc = IntermediateZAcc + (Point3Z-Point1Z)*SpeedWeightZ[i
- -1]/SpeedAccZ;
+ IntermediateZAcc = IntermediateZAcc + (Point3Z-Point1Z)*
+ SpeedWeightZ[i-1]/SpeedAccZ;
ZfootPos(Counter+i) = IntermediateZAcc + ZfootPos(Counter);
- TimeIntervalsZ(Counter+i) = TimeIntervalsZ(Counter)+i*IntermediateTimeStep;
+ TimeIntervalsZ(Counter+i) =
+ TimeIntervalsZ(Counter)+i*IntermediateTimeStep;
CounterTemp = i;
}
@@ -927,21 +989,24 @@ void StepOverPlanner::PolyPlannerFirstStep(deque<FootAbsolutePosition>
for (int i=1; i<=NumberIntermediate; i++)
{
- SpeedWeightZ[i-1] = (EndSpeedZ-PreviousSpeedZ)*((double (i-1)/(double (
- NumberIntermediate))))+PreviousSpeedZ;
+ SpeedWeightZ[i-1] = (EndSpeedZ-PreviousSpeedZ)*
+ ((double (i-1)/
+ (double (NumberIntermediate))))+PreviousSpeedZ;
SpeedAccZ = SpeedAccZ + SpeedWeightZ[i-1];
}
for (int i=1; i<=NumberIntermediate; i++)
{
- IntermediateZAcc = IntermediateZAcc + (Point2Z-Point3Z)*SpeedWeightZ[i
- -1]/SpeedAccZ;
+ IntermediateZAcc = IntermediateZAcc +
+ (Point2Z-Point3Z)*SpeedWeightZ[i-1]/SpeedAccZ;
ZfootPos(Counter+i) = IntermediateZAcc + ZfootPos(Counter);
- TimeIntervalsZ(Counter+i) = TimeIntervalsZ(Counter)+i*IntermediateTimeStep;
+ TimeIntervalsZ(Counter+i) =
+ TimeIntervalsZ(Counter)+i*IntermediateTimeStep;
CounterTemp = i;
}
- //going down from point2Z to the ground with smooth velocity profile at touch down
+ //going down from point2Z to the ground with smooth velocity profile
+ //at touch down
IntermediateTimeStep = (aTimeDistr[2]-aTimeDistr[1])/(NumberIntermediate);
@@ -956,17 +1021,19 @@ void StepOverPlanner::PolyPlannerFirstStep(deque<FootAbsolutePosition>
for (int i=1; i<=NumberIntermediate; i++)
{
- SpeedWeightZ[i-1] = (EndSpeedZ-PreviousSpeedZ)*pow((double (i-1)/(double (
- NumberIntermediate))),1)+PreviousSpeedZ;
+ SpeedWeightZ[i-1] = (EndSpeedZ-PreviousSpeedZ)*
+ pow((double (i-1)/(double (NumberIntermediate))),1)+PreviousSpeedZ;
SpeedAccZ = SpeedAccZ + SpeedWeightZ[i-1];
}
for (int i=1; i<=NumberIntermediate; i++)
{
- IntermediateZAcc = IntermediateZAcc + (-Point2Z)*SpeedWeightZ[i-1]/SpeedAccZ;
+ IntermediateZAcc = IntermediateZAcc + (-Point2Z)*SpeedWeightZ[i-1]/
+ SpeedAccZ;
ZfootPos(Counter+i) = IntermediateZAcc + ZfootPos(Counter);
- TimeIntervalsZ(Counter+i) = TimeIntervalsZ(Counter)+i*IntermediateTimeStep;
+ TimeIntervalsZ(Counter+i) = TimeIntervalsZ(Counter)+
+ i*IntermediateTimeStep;
}
@@ -1029,11 +1096,10 @@ void StepOverPlanner::PolyPlannerFirstStep(deque<FootAbsolutePosition>
OmegaImpactfootSpeedBound(0)=0.0;
OmegaImpactfootSpeedBound(1)=0.0;
- m_ClampedCubicSplineStepOverFootOmegaImpact->SetParameters(OmegaImpactfootPos,
- TimeIntervalsOmegaImpact,
- OmegaImpactfootSpeedBound);
-
-
+ m_ClampedCubicSplineStepOverFootOmegaImpact->
+ SetParameters(OmegaImpactfootPos,
+ TimeIntervalsOmegaImpact,
+ OmegaImpactfootSpeedBound);
vector<double> aTimeDistrModulatedYSide;
Eigen::Matrix<double,5,1> aBoundCondYSide;
@@ -1105,28 +1171,39 @@ void StepOverPlanner::PolyPlannerFirstStep(deque<FootAbsolutePosition>
{
aStepOverFootBuffer[i+aStart].x=aStepOverFootBuffer[i+aStart-1].x;
aStepOverFootBuffer[i+aStart].y=aStepOverFootBuffer[i+aStart-1].y;
- aStepOverFootBuffer[i+aStart].theta=aStepOverFootBuffer[i+aStart-1].theta;
+ aStepOverFootBuffer[i+aStart].theta=
+ aStepOverFootBuffer[i+aStart-1].theta;
aStepOverFootBuffer[i+aStart].omega=
- m_ClampedCubicSplineStepOverFootOmegaImpact->GetValueSpline(
- TimeIntervalsOmegaImpact,
- LocalTime-TouchDownTime)+aStepOverFootBuffer[aStart].omega;
+ m_ClampedCubicSplineStepOverFootOmegaImpact->
+ GetValueSpline
+ (TimeIntervalsOmegaImpact,
+ LocalTime-TouchDownTime)
+ +aStepOverFootBuffer[aStart].omega;
}
else
{
aStepOverFootBuffer[i+aStart].x =
- m_ClampedCubicSplineStepOverFootX->GetValueSpline(TimeIntervalsX,
- LocalTime-LiftOffTime)+aStepOverFootBuffer[aStart].x;
+ m_ClampedCubicSplineStepOverFootX->
+ GetValueSpline
+ (TimeIntervalsX,
+ LocalTime-LiftOffTime)+aStepOverFootBuffer[aStart].x;
aStepOverFootBuffer[i+aStart].y =
- m_ClampedCubicSplineStepOverFootY->GetValueSpline(TimeIntervalsY,
- LocalTime-LiftOffTime)+aStepOverFootBuffer[aStart].y;
+ m_ClampedCubicSplineStepOverFootY->
+ GetValueSpline
+ (TimeIntervalsY,
+ LocalTime-LiftOffTime)+aStepOverFootBuffer[aStart].y;
aStepOverFootBuffer[i+aStart].omega=
- m_ClampedCubicSplineStepOverFootOmega->GetValueSpline(TimeIntervalsOmega,
- LocalTime-LiftOffTime)+aStepOverFootBuffer[aStart].omega;
+ m_ClampedCubicSplineStepOverFootOmega->
+ GetValueSpline
+ (TimeIntervalsOmega,
+ LocalTime-LiftOffTime)+aStepOverFootBuffer[aStart].omega;
aStepOverFootBuffer[i+aStart].theta=aStepOverFootBuffer[aStart].theta;
}
aStepOverFootBuffer[i+aStart].z =
- m_ClampedCubicSplineStepOverFootZ->GetValueSpline(TimeIntervalsZ,
- LocalTime)+aStepOverFootBuffer[aStart].z;
+ m_ClampedCubicSplineStepOverFootZ->
+ GetValueSpline
+ (TimeIntervalsZ,
+ LocalTime)+aStepOverFootBuffer[aStart].z;
}
}
@@ -1164,10 +1241,12 @@ void StepOverPlanner::PolyPlannerSecondStep(deque<FootAbsolutePosition>
Point1Y = 0.0;
Point1Z = m_ObstacleParameters.h+m_tipToAnkle*sin(Omega1*M_PI/180.0);
- Point2X = m_StepOverStepLenght-m_heelToAnkle+xOffset+m_heelToAnkle*cos(
- Omega2*M_PI/180.0);
+ Point2X = m_StepOverStepLenght-
+ m_heelToAnkle+xOffset+m_heelToAnkle*
+ cos(Omega2*M_PI/180.0);
Point2Y = 0.0;
- Point2Z = Point1Z;// m_ObstacleParameters.h+0.04;//-m_tipToAnkle*sin(Omega2*M_PI/180.0);
+ Point2Z = Point1Z;
+ // m_ObstacleParameters.h+0.04;//-m_tipToAnkle*sin(Omega2*M_PI/180.0);
vector<double> aTimeDistr,aTimeDistrModulated;
double ModulatedStepTime = StepTime * m_ModulationSupportCoefficient;
@@ -1181,7 +1260,9 @@ void StepOverPlanner::PolyPlannerSecondStep(deque<FootAbsolutePosition>
aTimeDistr[1]=m_TimeDistrFactor[3]*StepTime/5.0;
aTimeDistr[2]=StepTime;
- //this time schedule is used for the X and Y coordinate of the foot in order to make sure the foot lifts the ground (Z) before moving the X and Y direction
+ // this time schedule is used for the X and Y coordinate of the foot in
+ // order to make sure the foot lifts the ground (Z) before
+ // moving the X and Y direction
aTimeDistrModulated.resize(3);
@@ -1225,8 +1306,10 @@ void StepOverPlanner::PolyPlannerSecondStep(deque<FootAbsolutePosition>
TimeIntervalsZ(1+Counter+1) = aTimeDistr[1];
TimeIntervalsZ(1+Counter+2) = aTimeDistr[2];
- m_ClampedCubicSplineStepOverFootZ->SetParameters(ZfootPos,TimeIntervalsZ,
- ZfootSpeedBound);
+ m_ClampedCubicSplineStepOverFootZ->
+ SetParameters
+ (ZfootPos,TimeIntervalsZ,
+ ZfootSpeedBound);
Eigen::Matrix<double,4,1> XfootPos;
Eigen::Matrix<double,4,1> TimeIntervalsX;
@@ -1242,7 +1325,8 @@ void StepOverPlanner::PolyPlannerSecondStep(deque<FootAbsolutePosition>
XfootPos.resize(4+NumberIntermediate);
TimeIntervalsX.resize(4+NumberIntermediate);
- //Use of speed to weight the extra points for the last interval on X to prevent overshoot of the spline on X
+ // Use of speed to weight the extra points for
+ // the last interval on X to prevent overshoot of the spline on X
double PreviousSpeedX,EndSpeedX,SpeedAccX;
vector<double> SpeedWeightX;
@@ -1269,7 +1353,8 @@ void StepOverPlanner::PolyPlannerSecondStep(deque<FootAbsolutePosition>
for (int i=1; i<=NumberIntermediate; i++)
{
- XfootPos(2+i) = XfootPos(2+i-1)+(StepLenght-Point2X)*SpeedWeightX[i]/SpeedAccX;
+ XfootPos(2+i) = XfootPos(2+i-1)+
+ (StepLenght-Point2X)*SpeedWeightX[i]/SpeedAccX;
TimeIntervalsX(2+i) = aTimeDistrModulated[1]+i*IntermediateTimeStep;
Counter = i;
}
@@ -1326,8 +1411,8 @@ void StepOverPlanner::PolyPlannerSecondStep(deque<FootAbsolutePosition>
TimeIntervalsOmega(2) = aTimeDistrModulated[1];
TimeIntervalsOmega(3) = aTimeDistrModulated[2];
*/
- m_ClampedCubicSplineStepOverFootOmega->SetParameters(OmegafootPos,
- TimeIntervalsOmega,OmegafootSpeedBound);
+ m_ClampedCubicSplineStepOverFootOmega->
+ SetParameters(OmegafootPos,TimeIntervalsOmega,OmegafootSpeedBound);
Eigen::Matrix<double,2,1> OmegaImpactfootPos;
Eigen::Matrix<double,2,1> TimeIntervalsOmegaImpact;
@@ -1342,10 +1427,10 @@ void StepOverPlanner::PolyPlannerSecondStep(deque<FootAbsolutePosition>
OmegaImpactfootSpeedBound(0)=0.0;
OmegaImpactfootSpeedBound(1)=0.0;
- m_ClampedCubicSplineStepOverFootOmegaImpact->SetParameters(OmegaImpactfootPos,
- TimeIntervalsOmegaImpact,
- OmegaImpactfootSpeedBound);
-
+ m_ClampedCubicSplineStepOverFootOmegaImpact->
+ SetParameters(OmegaImpactfootPos,
+ TimeIntervalsOmegaImpact,
+ OmegaImpactfootSpeedBound);
Eigen::Matrix<double,4,1> YfootPos;
Eigen::Matrix<double,4,1> TimeIntervalsY;
@@ -1366,8 +1451,9 @@ void StepOverPlanner::PolyPlannerSecondStep(deque<FootAbsolutePosition>
YfootSpeedBound(0)=0.0;
YfootSpeedBound(1)=0.0;
- m_ClampedCubicSplineStepOverFootY->SetParameters(YfootPos,TimeIntervalsY,
- YfootSpeedBound);
+ m_ClampedCubicSplineStepOverFootY->
+ SetParameters(YfootPos,TimeIntervalsY,
+ YfootSpeedBound);
//update the footbuffers with the new calculated polynomials for stepping over
unsigned int diff = m_EndStepOver-m_StartSecondStep;
@@ -1389,33 +1475,40 @@ void StepOverPlanner::PolyPlannerSecondStep(deque<FootAbsolutePosition>
{
aStepOverFootBuffer[i+aStart].x=aStepOverFootBuffer[i+aStart-1].x;
aStepOverFootBuffer[i+aStart].y=aStepOverFootBuffer[i+aStart-1].y;
- aStepOverFootBuffer[i+aStart].theta=aStepOverFootBuffer[i+aStart-1].theta;
+ aStepOverFootBuffer[i+aStart].theta=
+ aStepOverFootBuffer[i+aStart-1].theta;
aStepOverFootBuffer[i+aStart].omega=
- m_ClampedCubicSplineStepOverFootOmegaImpact->GetValueSpline(
- TimeIntervalsOmegaImpact,
- LocalTime-TouchDownTime)+
+ m_ClampedCubicSplineStepOverFootOmegaImpact->
+ GetValueSpline
+ (TimeIntervalsOmegaImpact,LocalTime-TouchDownTime)+
aStepOverFootBuffer[aStart].omega;
}
else
{
aStepOverFootBuffer[i+aStart].x =
- m_ClampedCubicSplineStepOverFootX->GetValueSpline(TimeIntervalsX,
- LocalTime-LiftOffTime)
+ m_ClampedCubicSplineStepOverFootX->
+ GetValueSpline(TimeIntervalsX,
+ LocalTime-LiftOffTime)
+aStepOverFootBuffer[aStart].x;
aStepOverFootBuffer[i+aStart].y =
- m_ClampedCubicSplineStepOverFootY->GetValueSpline(TimeIntervalsY,
- LocalTime-LiftOffTime)
+ m_ClampedCubicSplineStepOverFootY->
+ GetValueSpline(TimeIntervalsY,
+ LocalTime-LiftOffTime)
+aStepOverFootBuffer[aStart].y;
/* aStepOverFootBuffer[i+aStart].y=
- m_PolynomeStepOverY->Compute(LocalTime-LiftOffTime)+aStepOverFootBuffer[aStart].y; */
+ m_PolynomeStepOverY->Compute(LocalTime-LiftOffTime)
+ +aStepOverFootBuffer[aStart].y; */
aStepOverFootBuffer[i+aStart].omega=
- m_ClampedCubicSplineStepOverFootOmega->GetValueSpline(TimeIntervalsOmega,
- LocalTime-LiftOffTime)
+ m_ClampedCubicSplineStepOverFootOmega->
+ GetValueSpline
+ (TimeIntervalsOmega,
+ LocalTime-LiftOffTime)
+aStepOverFootBuffer[aStart].omega;
aStepOverFootBuffer[i+aStart].theta=aStepOverFootBuffer[aStart].theta;
}
aStepOverFootBuffer[i+aStart].z =
- m_ClampedCubicSplineStepOverFootZ->GetValueSpline(TimeIntervalsZ,LocalTime)
+ m_ClampedCubicSplineStepOverFootZ->
+ GetValueSpline(TimeIntervalsZ,LocalTime)
+aStepOverFootBuffer[aStart].z;
}
@@ -1433,8 +1526,9 @@ void StepOverPlanner::PolyPlannerHip()
StepTime = m_LeftFootBuffer[m_EndPrevStepOver].time
-m_LeftFootBuffer[m_StartPrevStepOver].time;
- //StepTime = m_LeftFootBuffer[m_StartDoubleSupp].time-m_LeftFootBuffer[m_StartStepOver].time;
- //we take foot buffer since this contains the time course
+ // StepTime = m_LeftFootBuffer[m_StartDoubleSupp].time-
+ // m_LeftFootBuffer[m_StartStepOver].time;
+ // we take foot buffer since this contains the time course
HeightDifference =m_StepOverHipHeight-m_COMBuffer[m_StartPrevStepOver].z[0];
vector<double> aTimeDistr;
@@ -1448,16 +1542,11 @@ void StepOverPlanner::PolyPlannerHip()
aTimeDistr.resize(1);
aTimeDistr[0]=diff*m_SamplingPeriod;
-
-
-
-
aBoundCond(0)=0.0;
aBoundCond(1)=0.0;
aBoundCond(2)=HeightDifference;
aBoundCond(3)=0.0;
-
m_PolynomeStepOverHipStep2->SetParameters(aBoundCond,aTimeDistr);
aBoundCond(0)=0.0;
@@ -1472,19 +1561,21 @@ void StepOverPlanner::PolyPlannerHip()
{
LocalTime=(i)*m_SamplingPeriod;
{
- m_COMBuffer[i+aStart].z[0]=m_PolynomeStepOverHipStep2->Compute(
- LocalTime)+m_COMBuffer[aStart].z[0];
+ m_COMBuffer[i+aStart].z[0]=
+ m_PolynomeStepOverHipStep2->
+ Compute(LocalTime)+m_COMBuffer[aStart].z[0];
- m_COMBuffer[i+aStart].z[1]=(m_COMBuffer[i+aStart].z[0]-m_COMBuffer[i+aStart
- -1].z[0])/m_SamplingPeriod;
- m_COMBuffer[i+aStart].z[2]=(m_COMBuffer[i+aStart].z[1]-m_COMBuffer[i+aStart
- -1].z[1])/m_SamplingPeriod;
+ m_COMBuffer[i+aStart].z[1]=
+ (m_COMBuffer[i+aStart].z[0]-
+ m_COMBuffer[i+aStart-1].z[0])/m_SamplingPeriod;
+ m_COMBuffer[i+aStart].z[2]=
+ (m_COMBuffer[i+aStart].z[1]-
+ m_COMBuffer[i+aStart-1].z[1])/m_SamplingPeriod;
m_COMBuffer[i+aStart].yaw[0]=
- m_PolynomeStepOverHipRotation->Compute(LocalTime)+m_COMBuffer[aStart].yaw[0];
+ m_PolynomeStepOverHipRotation->
+ Compute(LocalTime)+m_COMBuffer[aStart].yaw[0];
}
-
-
}
@@ -1496,10 +1587,13 @@ void StepOverPlanner::PolyPlannerHip()
{
m_COMBuffer[i+aStart].z[0]=m_COMBuffer[i+aStart-1].z[0];
- m_COMBuffer[i+aStart].z[1]=(m_COMBuffer[i+aStart].z[0]-m_COMBuffer[i+aStart
- -1].z[0])/m_SamplingPeriod;
- m_COMBuffer[i+aStart].z[2]=(m_COMBuffer[i+aStart].z[1]-m_COMBuffer[i+aStart
- -1].z[1])/m_SamplingPeriod;
+ m_COMBuffer[i+aStart].z[1]=
+ (m_COMBuffer[i+aStart].z[0]-
+ m_COMBuffer[i+aStart-1].z[0])
+ /m_SamplingPeriod;
+ m_COMBuffer[i+aStart].z[2]=
+ (m_COMBuffer[i+aStart].z[1]-m_COMBuffer[i+aStart-1].z[1])
+ /m_SamplingPeriod;
m_COMBuffer[i+aStart].yaw[0]=m_COMBuffer[i+aStart-1].yaw[0];
}
@@ -1533,25 +1627,31 @@ void StepOverPlanner::PolyPlannerHip()
{
LocalTime=(i)*m_SamplingPeriod;
{
- m_COMBuffer[i+aStart].z[0]=m_PolynomeStepOverHipStep2->Compute(
- LocalTime)+m_COMBuffer[aStart].z[0];
+ m_COMBuffer[i+aStart].z[0]=
+ m_PolynomeStepOverHipStep2->Compute(LocalTime)
+ +m_COMBuffer[aStart].z[0];
- m_COMBuffer[i+aStart].z[1]=(m_COMBuffer[i+aStart].z[0]-m_COMBuffer[i+aStart
- -1].z[0])/m_SamplingPeriod;
- m_COMBuffer[i+aStart].z[2]=(m_COMBuffer[i+aStart].z[1]-m_COMBuffer[i+aStart
- -1].z[1])/m_SamplingPeriod;
+ m_COMBuffer[i+aStart].z[1]=
+ (m_COMBuffer[i+aStart].z[0]-m_COMBuffer[i+aStart-1].z[0])
+ /m_SamplingPeriod;
+ m_COMBuffer[i+aStart].z[2]=
+ (m_COMBuffer[i+aStart].z[1]-m_COMBuffer[i+aStart-1].z[1])
+ /m_SamplingPeriod;
- m_COMBuffer[i+aStart].yaw[0]=m_PolynomeStepOverHipRotation->Compute(
- LocalTime)+m_COMBuffer[aStart].yaw[0];
+ m_COMBuffer[i+aStart].yaw[0]=
+ m_PolynomeStepOverHipRotation->
+ Compute(LocalTime)+m_COMBuffer[aStart].yaw[0];
}
}
}
-void StepOverPlanner::SetExtraBuffer(deque<COMState> aExtraCOMBuffer,
- deque<FootAbsolutePosition> aExtraRightFootBuffer,
- deque<FootAbsolutePosition> aExtraLeftFootBuffer)
+void StepOverPlanner::
+SetExtraBuffer
+(deque<COMState> aExtraCOMBuffer,
+ deque<FootAbsolutePosition> aExtraRightFootBuffer,
+ deque<FootAbsolutePosition> aExtraLeftFootBuffer)
{
m_ExtraCOMBuffer=aExtraCOMBuffer;
m_ExtraRightFootBuffer = aExtraRightFootBuffer;
@@ -1560,42 +1660,51 @@ void StepOverPlanner::SetExtraBuffer(deque<COMState> aExtraCOMBuffer,
-void StepOverPlanner::GetExtraBuffer(deque<COMState> &aExtraCOMBuffer,
- deque<FootAbsolutePosition> &aExtraRightFootBuffer,
- deque<FootAbsolutePosition> &aExtraLeftFootBuffer)
+void StepOverPlanner::
+GetExtraBuffer
+(deque<COMState> &aExtraCOMBuffer,
+ deque<FootAbsolutePosition> &aExtraRightFootBuffer,
+ deque<FootAbsolutePosition> &aExtraLeftFootBuffer)
{
aExtraCOMBuffer = m_ExtraCOMBuffer;
aExtraRightFootBuffer = m_ExtraRightFootBuffer;
aExtraLeftFootBuffer = m_ExtraLeftFootBuffer;
}
-void StepOverPlanner::SetFootBuffers(deque<FootAbsolutePosition>
- aLeftFootBuffer,
- deque<FootAbsolutePosition> aRightFootBuffer)
+void StepOverPlanner::
+SetFootBuffers
+(deque<FootAbsolutePosition>
+ aLeftFootBuffer,
+ deque<FootAbsolutePosition> aRightFootBuffer)
{
m_RightFootBuffer = aRightFootBuffer;
m_LeftFootBuffer = aLeftFootBuffer;
}
-void StepOverPlanner::GetFootBuffers(deque<FootAbsolutePosition> &
- aRightFootBuffer,
- deque<FootAbsolutePosition> & aLeftFootBuffer)
+void StepOverPlanner::
+GetFootBuffers
+(deque<FootAbsolutePosition> &
+ aRightFootBuffer,
+ deque<FootAbsolutePosition> & aLeftFootBuffer)
{
aRightFootBuffer = m_RightFootBuffer;
aLeftFootBuffer = m_LeftFootBuffer;
}
-void StepOverPlanner::SetObstacleInformation(ObstaclePar ObstacleParameters)
+void StepOverPlanner::
+SetObstacleInformation
+(ObstaclePar ObstacleParameters)
{
- //add safety boundaries to the obstacle , the safety bounderies at the moment are chosen
- //but they can vary in the fuuter in function of the vision information uncertainty
+ // add safety boundaries to the obstacle ,
+ // the safety bounderies at the moment are chosen
+ // but they can vary in the fuuter in function
+ // of the vision information uncertainty
double safeBoundWidth=0.03;
double safeBoundHeight=0.03;
double safeBoundDepth=0.03;
-
ObstacleParameters.h+=safeBoundHeight;
ObstacleParameters.w+=2.0*safeBoundWidth;
ObstacleParameters.d+=2.0*safeBoundDepth;
@@ -1604,8 +1713,6 @@ void StepOverPlanner::SetObstacleInformation(ObstaclePar ObstacleParameters)
//m_obstacles is visible and requered in the rest of the class
m_ObstacleParameters = ObstacleParameters;
-
-
m_ObstaclePosition(0) = m_ObstacleParameters.x;
m_ObstaclePosition(1) = m_ObstacleParameters.y;
m_ObstaclePosition(2) = m_ObstacleParameters.z;
@@ -1615,9 +1722,8 @@ void StepOverPlanner::SetObstacleInformation(ObstaclePar ObstacleParameters)
c = cos(m_ObstacleParameters.theta*M_PI/180.0);
s = sin(m_ObstacleParameters.theta*M_PI/180.0);
-
-
- //this matrix transformes coordinates in the obstacle frame into the world frame
+ // this matrix transformes coordinates in the obstacle
+ // frame into the world frame
m_ObstacleRot(0,0) = c;
m_ObstacleRot(0,1) =-s;
m_ObstacleRot(0,2) = 0;
@@ -1628,7 +1734,8 @@ void StepOverPlanner::SetObstacleInformation(ObstaclePar ObstacleParameters)
m_ObstacleRot(2,1) = 0;
m_ObstacleRot(2,2) = 1;
- //this matrix transformes coordinates in the world frame into the obstacle frame
+ // this matrix transformes coordinates in the world
+ // frame into the obstacle frame
m_ObstacleRotInv(0,0) = c;
m_ObstacleRotInv(0,1) = s;
m_ObstacleRotInv(0,2) = 0;
@@ -1681,17 +1788,17 @@ void StepOverPlanner::TimeDistributeFactor(vector<double> &TimeDistrFactor)
}
-void StepOverPlanner::SetDeltaStepOverCOMHeightMax(double
- aDeltaStepOverCOMHeightMax)
+void StepOverPlanner::SetDeltaStepOverCOMHeightMax
+(double aDeltaStepOverCOMHeightMax)
{
-
m_DeltaStepOverCOMHeightMax = aDeltaStepOverCOMHeightMax;
}
-void StepOverPlanner::CreateBufferFirstPreview(deque<COMState> &m_COMBuffer,
- deque<ZMPPosition> &m_ZMPBuffer,
- deque<ZMPPosition> &m_ZMPRefBuffer)
+void StepOverPlanner::CreateBufferFirstPreview
+(deque<COMState> &m_COMBuffer,
+ deque<ZMPPosition> &m_ZMPBuffer,
+ deque<ZMPPosition> &m_ZMPRefBuffer)
{
deque<ZMPPosition> aFIFOZMPRefPositions;
Eigen::MatrixXd aPC1x;
@@ -1848,7 +1955,8 @@ void StepOverPlanner::m_SetObstacleParameters(istringstream &strm)
}
else
{
- cout << "Not enough inputs for completion of obstacle information structure!" <<
+ cout << "Not enough inputs for completion of "
+ << "obstacle information structure!" <<
endl;
break;
}
diff --git a/src/MotionGeneration/StepOverPlanner.hh b/src/MotionGeneration/StepOverPlanner.hh
index cc2fd19ed3993947c6b1ce971daa607e0da4d3e0..54294cc123d3e794326fd1e91758c98bdfba1b51 100644
--- a/src/MotionGeneration/StepOverPlanner.hh
+++ b/src/MotionGeneration/StepOverPlanner.hh
@@ -65,7 +65,8 @@ namespace PatternGeneratorJRL
typedef struct ObstaclePar_t ObstaclePar;
/*!\ingroup steppingover
- \brief Object to compute new foot trajectories to step over obstacle dynamically*/
+ \brief Object to compute new foot trajectories to step over obstacle
+ dynamically*/
class StepOverPlanner
{
public :
@@ -81,31 +82,34 @@ namespace PatternGeneratorJRL
footholds to be set in function of an obstacle in front. */
void CalculateFootHolds(deque<RelativeFootPosition> &FootHolds);
- /*! \brief Call for polynomial planning of both steps during the obstacle stepover */
+ /*! \brief Call for polynomial planning of both steps during the obstacle
+ stepover */
void PolyPlanner(deque<COMState> &aCOMBuffer,
deque<FootAbsolutePosition> & aLeftFootBuffer,
deque<FootAbsolutePosition> & aRightFootBuffer,
deque<ZMPPosition> & aZMPPositions);
- /*! function which calculates the polynomial coeficients for the first step*/
+ /*! function which calculates the polynomial coeficients
+ for the first step*/
void PolyPlannerFirstStep(deque<FootAbsolutePosition>
&aFirstStepOverFootBuffer);
- /*! function which calculates the polynomial coeficients for the first step*/
+ /*! function which calculates the polynomial coeficients
+ for the first step*/
void PolyPlannerSecondStep(deque<FootAbsolutePosition>
&aSecondStepOverFootBuffer);
- /*! function which calculates the polynomial coeficients for the changing COM height*/
+ /*! function which calculates the polynomial coeficients
+ for the changing COM height*/
void PolyPlannerHip();
-
-
/*! this sets the extra COM buffer calculated in the ZMPMultybody class*/
void SetExtraBuffer(deque<COMState> aExtraCOMBuffer,
deque<FootAbsolutePosition> aExtraRightFootBuffer,
deque<FootAbsolutePosition> aExtraLeftFootBuffer);
- /*! this gets the extra COM buPreviewControlffer calculated in the ZMPMultybody class*/
+ /*! this gets the extra COM buPreviewControlffer calculated
+ in the ZMPMultybody class*/
void GetExtraBuffer(deque<COMState> &aExtraCOMBuffer,
deque<FootAbsolutePosition> &aExtraRightFootBuffer,
deque<FootAbsolutePosition> &aExtraLeftFootBuffer);
diff --git a/src/MotionGeneration/WaistHeightVariation.cpp b/src/MotionGeneration/WaistHeightVariation.cpp
index 14d154e30cb4f084b99b121a6492aab9c5e83209..4f0044cd39e046fc2cd4bdc26960345ccd85532c 100644
--- a/src/MotionGeneration/WaistHeightVariation.cpp
+++ b/src/MotionGeneration/WaistHeightVariation.cpp
@@ -70,11 +70,6 @@ void WaistHeightVariation::PolyPlanner(deque<COMPosition> &aCOMBuffer,
aTimeDistr[0]=aFootHolds[stepnumber].SStime+aFootHolds[stepnumber].DStime;
- /*
- cout << "Time distributed computed from "
- << m_SamplingPeriod << " "
- << aFootHolds[stepnumber].SStime << " " << aFootHolds[stepnumber].DStime << endl;
- */
aBoundCond(0)=aCOMBuffer[0].z[0];
aBoundCond(1)=0.0;
aBoundCond(2)=aCOMBuffer[0].z[0]+aFootHolds[stepnumber].DeviationHipHeight;
@@ -92,18 +87,20 @@ void WaistHeightVariation::PolyPlanner(deque<COMPosition> &aCOMBuffer,
{
if ((aFootHolds[stepnumber].sx>0)|(stepnumber==0))
{
- if ((aZMPPosition[u-1].stepType==11)&(std::fabs((double)
- aZMPPosition[u].stepType)==1))
+ if ((aZMPPosition[u-1].stepType==11)&
+ (std::fabs((double) aZMPPosition[u].stepType)==1))
{
- ODEBUG("Deviation Hip Height: " << aFootHolds[stepnumber].DeviationHipHeight);
+ ODEBUG("Deviation Hip Height: "
+ << aFootHolds[stepnumber].DeviationHipHeight);
stepnumber++;
-
- aTimeDistr[0]=aFootHolds[stepnumber].SStime+aFootHolds[stepnumber].DStime;
+
+ aTimeDistr[0]=
+ aFootHolds[stepnumber].SStime+aFootHolds[stepnumber].DStime;
u_start=u;
- aBoundCond(0)=aCOMBuffer[u_start].z[0]+aFootHolds[stepnumber
- -1].DeviationHipHeight;
+ aBoundCond(0)=aCOMBuffer[u_start].z[0]+
+ aFootHolds[stepnumber-1].DeviationHipHeight;
aBoundCond(1)=0.0;
aBoundCond(2)=aCOMBuffer[u_start].z[0]
+aFootHolds[stepnumber].DeviationHipHeight;
@@ -113,11 +110,13 @@ void WaistHeightVariation::PolyPlanner(deque<COMPosition> &aCOMBuffer,
m_PolynomeHip->SetParameters(aBoundCond,aTimeDistr);
//m_PolynomeHip->print();
- aCOMBuffer[u_start].z[0]=m_PolynomeHip->Compute(0);//+aCOMBuffer[u_start].z[0];
- aCOMBuffer[u_start].z[1]=(aCOMBuffer[u_start].z[0]-aCOMBuffer[u_start
- -1].z[0])/m_SamplingPeriod;
- aCOMBuffer[u_start].z[2]=(aCOMBuffer[u_start].z[1]-aCOMBuffer[u_start
- -1].z[1])/m_SamplingPeriod;
+ aCOMBuffer[u_start].z[0]=m_PolynomeHip->Compute(0);
+ aCOMBuffer[u_start].z[1]=
+ (aCOMBuffer[u_start].z[0]-aCOMBuffer[u_start-1].z[0])
+ /m_SamplingPeriod;
+ aCOMBuffer[u_start].z[2]=
+ (aCOMBuffer[u_start].z[1]-
+ aCOMBuffer[u_start-1].z[1])/m_SamplingPeriod;
}
else
@@ -126,10 +125,12 @@ void WaistHeightVariation::PolyPlanner(deque<COMPosition> &aCOMBuffer,
double LocalTime;
LocalTime=(u-u_start)*m_SamplingPeriod;
- aCOMBuffer[u].z[0]=m_PolynomeHip->Compute(
- LocalTime);//+aCOMBuffer[u_start].z[0];
- aCOMBuffer[u].z[1]=(aCOMBuffer[u].z[0]-aCOMBuffer[u-1].z[0])/m_SamplingPeriod;
- aCOMBuffer[u].z[2]=(aCOMBuffer[u].z[1]-aCOMBuffer[u-1].z[1])/m_SamplingPeriod;
+ aCOMBuffer[u].z[0]=
+ m_PolynomeHip->Compute(LocalTime);//+aCOMBuffer[u_start].z[0];
+ aCOMBuffer[u].z[1]=
+ (aCOMBuffer[u].z[0]-aCOMBuffer[u-1].z[0])/m_SamplingPeriod;
+ aCOMBuffer[u].z[2]=
+ (aCOMBuffer[u].z[1]-aCOMBuffer[u-1].z[1])/m_SamplingPeriod;
}
}
diff --git a/src/MotionGeneration/WaistHeightVariation.hh b/src/MotionGeneration/WaistHeightVariation.hh
index 84fc07b609691802177e5ccd8280753f11ab3628..3e5a459a1c5d5d4f31321934deb7c3a8d948c7ea 100644
--- a/src/MotionGeneration/WaistHeightVariation.hh
+++ b/src/MotionGeneration/WaistHeightVariation.hh
@@ -63,8 +63,10 @@ namespace PatternGeneratorJRL
{
public:
/// Constructor:
- /// boundCond: the different boundary conditions begin, intermediate and end of polynomial
- /// timeDistr: vector with time instants for intermediate boundary conditions and end time
+ /// boundCond: the different boundary conditions begin,
+ /// intermediate and end of polynomial
+ /// timeDistr: vector with time instants for intermediate
+ /// boundary conditions and end time
WaistPolynome();
@@ -80,7 +82,8 @@ namespace PatternGeneratorJRL
- /// Object to compute new foot trajectories for the height of the waist with waist differnces as input for each step
+ /// Object to compute new foot trajectories for the height of the waist
+ /// with waist differnces as input for each step
class WaistHeightVariation
{
public :
diff --git a/src/PreviewControl/LinearizedInvertedPendulum2D.cpp b/src/PreviewControl/LinearizedInvertedPendulum2D.cpp
index 57aa2ec01e27226afd89eac7bb84ae4c84f3c049..cc16f0f23feaea05bdff731b9f00ed861db12bc2 100644
--- a/src/PreviewControl/LinearizedInvertedPendulum2D.cpp
+++ b/src/PreviewControl/LinearizedInvertedPendulum2D.cpp
@@ -191,17 +191,22 @@ int LinearizedInvertedPendulum2D::InitializeSystem()
-int LinearizedInvertedPendulum2D::Interpolation(deque<COMState> &COMStates,
- deque<ZMPPosition> &ZMPRefPositions,
- int CurrentPosition,
- double CX, double CY)
+int LinearizedInvertedPendulum2D::
+Interpolation
+(deque<COMState> &COMStates,
+ deque<ZMPPosition> &ZMPRefPositions,
+ int CurrentPosition,
+ double CX, double CY)
{
int lCurrentPosition = CurrentPosition;
// Fill the queues with the interpolated CoM values.
- //TODO: with TestHerdt, it is mandatory to use COMStates.size()-1, or it will crash.
+ // TODO: with TestHerdt, it is mandatory to use COMStates.size()-1, or it will
+ // crash.
// Is it the same for the other PG ? Please check.
- // TODO: with TestHerdt, it is mandatory to use m_InterpolationInterval-1 to interpolate correctly
- // along the whole preview window will it be still fine with the reste of the PG?
+ // TODO: with TestHerdt, it is mandatory to use m_InterpolationInterval-1 to
+ // interpolate correctly
+ // along the whole preview window will it be still fine with the reste of the
+ // PG ?
int loopEnd = std::min<int>( m_InterpolationInterval-1,
((int)COMStates.size())-1-CurrentPosition);
for(int lk=0; lk<=loopEnd; lk++,lCurrentPosition++)
@@ -256,8 +261,10 @@ int LinearizedInvertedPendulum2D::Interpolation(deque<COMState> &COMStates,
aZMPPos.pz = 0.0 ;
- ODEBUG4(aCOMPos.x[0] << " " << aCOMPos.x[1] << " " << aCOMPos.x[2] << " " <<
- aCOMPos.y[0] << " " << aCOMPos.y[1] << " " << aCOMPos.y[2] << " " <<
+ ODEBUG4(aCOMPos.x[0] << " " << aCOMPos.x[1] << " "
+ << aCOMPos.x[2] << " " <<
+ aCOMPos.y[0] << " " << aCOMPos.y[1] << " "
+ << aCOMPos.y[2] << " " <<
aCOMPos.yaw << " " <<
aZMPPos.px << " " << aZMPPos.py << " " << aZMPPos.theta << " " <<
CX << " " << CY << " " <<
diff --git a/src/PreviewControl/LinearizedInvertedPendulum2D.hh b/src/PreviewControl/LinearizedInvertedPendulum2D.hh
index 6e97a809fa18e219a63a8bb2f0c79caecb0414c8..a3bde6fbb976c2a36a722afcaa7222c448d183f8 100644
--- a/src/PreviewControl/LinearizedInvertedPendulum2D.hh
+++ b/src/PreviewControl/LinearizedInvertedPendulum2D.hh
@@ -60,9 +60,12 @@ namespace PatternGeneratorJRL
/*! \brief Interpolation during a simulation period with control parameters.
\param[out]: NewFinalZMPPositions: queue of ZMP positions interpolated.
\param[out]: COMStates: queue of COM positions interpolated.
- \param[in]: ZMPRefPositions: Reference positions of ZMP (Kajita's heuristic every 5 ms).
- \param[in]: CurrentPosition: index of the current position of the ZMP reference
- (this allow to propagate some parameters define by a heuristic to the CoM positions).
+ \param[in]: ZMPRefPositions: Reference positions of ZMP
+ (Kajita's heuristic every 5 ms).
+ \param[in]: CurrentPosition: index of the current position of
+ the ZMP reference
+ (this allow to propagate some parameters define by a heuristic
+ to the CoM positions).
\param[in]: CX: command parameter in the forward direction.
\param[in]: CY: command parameter in the perpendicular direction.
*/
diff --git a/src/PreviewControl/OptimalControllerSolver.cpp b/src/PreviewControl/OptimalControllerSolver.cpp
index e638a0c777685f2bbbd3ac9378ac8e6e7cbea507..9366f6c46d26fea708f053aa970e374d4d3c2574 100644
--- a/src/PreviewControl/OptimalControllerSolver.cpp
+++ b/src/PreviewControl/OptimalControllerSolver.cpp
@@ -43,11 +43,12 @@ typedef int integer ;
extern "C" {
extern doublereal dlapy2_(doublereal *, doublereal *);
extern double dlamch_ (char *);
- extern /* Subroutine */ int dgges_(char *, char *, char *, L_fp, integer *
- , doublereal *, integer *, doublereal *, integer *, integer *,
- doublereal *, doublereal *, doublereal *, doublereal *, integer *,
- doublereal *, integer *, doublereal *, integer *, logical *,
- integer *);
+ extern /* Subroutine */
+ int dgges_(char *, char *, char *, L_fp, integer * ,
+ doublereal *, integer *, doublereal *, integer *, integer *,
+ doublereal *, doublereal *, doublereal *, doublereal *, integer *,
+ doublereal *, integer *, doublereal *, integer *, logical *,
+ integer *);
}
@@ -268,12 +269,14 @@ void OptimalControllerSolver::ComputeWeights(unsigned int Mode)
MatrixRXd ZE(2*n,2*n); // The matrix of Schur vectors.
Eigen::VectorXd WR(2*n);
Eigen::VectorXd WI(
- 2*n); // The eigenvalues ( a matrix to handle complex eigenvalues).
+ 2*n);
+ // The eigenvalues ( a matrix to handle complex eigenvalues).
Eigen::VectorXd GS(2*n);
if (!GeneralizedSchur(H,E,WR,WI,GS,ZH,ZE))
{
- std::cerr << "Something is wrong with the weights for the preview control !"
+ std::cerr << "Something is wrong with the weights "
+ << "for the preview control !"
<< std::endl;
}
diff --git a/src/PreviewControl/OptimalControllerSolver.hh b/src/PreviewControl/OptimalControllerSolver.hh
index 840b63ab6ddfc1a1dd750db31ed88aedbfaff8b6..177bdf37a8d83150aa2d45b4436c1be6d45585a8 100644
--- a/src/PreviewControl/OptimalControllerSolver.hh
+++ b/src/PreviewControl/OptimalControllerSolver.hh
@@ -66,11 +66,13 @@ namespace PatternGeneratorJRL
\f{eqnarray*}
{\bf K} & \equiv & (R + {\bf b}^T{\bf Pb})^{-1}{\bf b}^T{\bf PA} \\
- K_p(i) & \equiv & (R + {\bf b}^T{\bf Pb})^{-1}{\bf b}^T({\bf A}-{\bf bK})^{T*(i-1)}{\bf c}^TQ \\
+ K_p(i) & \equiv & (R + {\bf b}^T{\bf Pb})^{-1}{\bf b}^T({\bf A}-
+ {\bf bK})^{T*(i-1)}{\bf c}^TQ \ \
\f}
where \f$ {\bf P} \f$ is solution of the following Riccati equation:
\f[
- {\bf P} = {\bf A}^T {\bf PA} + {\bf c}^TQ{\bf c} - {\bf A}^T{\bf Pb}(R + {\bf b}^T{\bf Pb})^{-1}{\bf b}^T{\bf PA}
+ {\bf P} = {\bf A}^T {\bf PA} + {\bf c}^TQ{\bf c} -
+ {\bf A}^T{\bf Pb}(R + {\bf b}^T{\bf Pb})^{-1}{\bf b}^T{\bf PA}
\f]
@@ -81,13 +83,15 @@ namespace PatternGeneratorJRL
we can reformulate the discrete problem by posing the following:
\f{eqnarray*}
\begin{matrix}
- {\bf x}^*_{k+1} &= \widetilde{\bf A} {\bf x}^*_{k} + \widetilde{\bf b}\Delta u_k \\
+ {\bf x}^*_{k+1} &= \widetilde{\bf A} {\bf x}^*_{k} +
+ \widetilde{\bf b}\Delta u_k \ \
p_k &= \widetilde{\bf c}{\bf x}^*_{k}
\end{matrix}
\f}
with
\f{eqnarray*}
- \Delta u_k \equiv u_k - u_{k-1} & \Delta {\bf x}_k \equiv {\bf x}_k - {\bf x}_{k-1}\\
+ \Delta u_k \equiv u_k - u_{k-1} & \Delta {\bf x}_k \equiv {\bf x}_k
+ - {\bf x}_{k-1}\ \
{\bf x}_k \equiv \left[
\begin{matrix}
p_k\\
@@ -123,16 +127,19 @@ namespace PatternGeneratorJRL
the solution is then:
\f{eqnarray*}
- u_j = - K_1 \sum_{i=0}^k e(i) - {\bf K}_2 x(k) - \sum_{j=1}^{N_L} K_p(j)p^{ref}_j(k+j)
+ u_j = - K_1 \sum_{i=0}^k e(i) - {\bf K}_2 x(k) - \sum_{j=1}^{N_L}
+ K_p(j)p^{ref}_j(k+j)
\f}
where \f{eqnarray*}
- \left[ \begin{matrix} K_1 \\ {\bf K}_2 \\ \end{matrix} \right]= \widetilde{\bf K}
+ \left[ \begin{matrix} K_1 \\ {\bf K}_2 \\ \end{matrix} \right]=
+ \widetilde{\bf K}
\f}
\anchor Laub1979
- Alan J. Laub A Schur method for solving Algebraic Riccati Equations, IEEE Transaction on Automatic Control,
+ Alan J. Laub A Schur method for solving Algebraic Riccati Equations,
+ IEEE Transaction on Automatic Control,
Vol AC-24, No.6 December 1979
*/
class OptimalControllerSolver
@@ -170,7 +177,8 @@ namespace PatternGeneratorJRL
protected:
- typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,Eigen::RowMajor>
+ typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>
MatrixRXd;
/*! The matrices needed for the dynamical system such as
\f{eqnarray*}
@@ -210,7 +218,8 @@ namespace PatternGeneratorJRL
};
/*!
- \defgroup pageexampleoptimalweights Computing optimal weights for the preview control.
+ \defgroup pageexampleoptimalweights Computing optimal weights
+ for the preview control.
\ingroup codesourceexamples
\dontinclude TestRiccatiEquation.cpp
@@ -275,12 +284,14 @@ namespace PatternGeneratorJRL
To suppress the problem of the initial CoM position,
we can reformulate the discrete problem by posing the following:
\f{eqnarray*}
- {\bf x}^*_{k+1} = \widetilde{\bf A} {\bf x}^*_{k} + \widetilde{\bf b}\Delta u_k
+ {\bf x}^*_{k+1} = \widetilde{\bf A} {\bf x}^*_{k} + \widetilde{\bf b}
+ \Delta u_k
p_k = \widetilde{\bf c}{\bf x}^*_{k}
\f}
with
\f{eqnarray*
- \Delta u_k \equiv u_k - u_{k-1} & \Delta {\bf x}_k \equiv {\bf x}_k - {\bf x}_{k-1}
+ \Delta u_k \equiv u_k - u_{k-1} & \Delta {\bf x}_k \equiv {\bf x}_k -
+ {\bf x}_{k-1}
{\bf x}_k \equiv \left[
\begin{matrix}
p_k\\
@@ -321,7 +332,8 @@ namespace PatternGeneratorJRL
The computation of the weights is done by calling ComputeWeights().
There is only one parameter to specify, but it is important
as the weights are computed differently according to this parameter.
- If you use the mode without initial position please uses MODE_WITH_INITIALPOS.
+ If you use the mode without initial position please uses
+ MODE_WITH_INITIALPOS.
\skipline PatternGeneratorJRL::OptimalControllerSolver
To display the weights in the standard output
diff --git a/src/PreviewControl/PreviewControl.cpp b/src/PreviewControl/PreviewControl.cpp
index ac3d982fb14685b524df90cbb03ec5e47dae8f29..8cc55fc73eb73bfd597ddb027969fa6ebc9946bd 100644
--- a/src/PreviewControl/PreviewControl.cpp
+++ b/src/PreviewControl/PreviewControl.cpp
@@ -305,10 +305,11 @@ void PreviewControl::ComputeOptimalWeights(unsigned int mode)
Q = 1.0;
R = 1e-5;
ODEBUG("COMPUTATION WITH INITIALPOS !");
- anOCS = new PatternGeneratorJRL::OptimalControllerSolver(m_A,m_B,m_C,Q,R,Nl);
+ anOCS = new PatternGeneratorJRL::
+ OptimalControllerSolver(m_A,m_B,m_C,Q,R,Nl);
- anOCS->ComputeWeights(
- PatternGeneratorJRL::OptimalControllerSolver::MODE_WITH_INITIALPOS);
+ anOCS->ComputeWeights
+ (PatternGeneratorJRL::OptimalControllerSolver::MODE_WITH_INITIALPOS);
anOCS->GetF(m_F);
diff --git a/src/PreviewControl/PreviewControl.hh b/src/PreviewControl/PreviewControl.hh
index df02fa8007f65221cd0ba50d847cabeeacdbd9d0..eb0d8e99d67541868a684d54b7935ed79cad2d9c 100644
--- a/src/PreviewControl/PreviewControl.hh
+++ b/src/PreviewControl/PreviewControl.hh
@@ -54,9 +54,10 @@ namespace PatternGeneratorJRL
public:
/*! Constructor */
- PreviewControl(SimplePluginManager *lSPM,
- unsigned int defaultMode = OptimalControllerSolver::MODE_WITH_INITIALPOS,
- bool computeWeightsAutomatically = false);
+ PreviewControl
+ (SimplePluginManager *lSPM,
+ unsigned int defaultMode = OptimalControllerSolver::MODE_WITH_INITIALPOS,
+ bool computeWeightsAutomatically = false);
/*! Destructor */
~PreviewControl();
@@ -68,16 +69,18 @@ namespace PatternGeneratorJRL
/*! \brief One iteration of the preview control. */
- int OneIterationOfPreview(Eigen::MatrixXd &x,
- Eigen::MatrixXd &y,
- double & sxzmp, double & syzmp,
- deque<PatternGeneratorJRL::ZMPPosition> & ZMPPositions,
- unsigned long int lindex,
- double & zmpx2, double & zmpy2,
- bool Simulation);
-
-
- /*! \brief One iteration of the preview control along one axis (using queues)*/
+ int OneIterationOfPreview
+ (Eigen::MatrixXd &x,
+ Eigen::MatrixXd &y,
+ double & sxzmp, double & syzmp,
+ deque<PatternGeneratorJRL::ZMPPosition> & ZMPPositions,
+ unsigned long int lindex,
+ double & zmpx2, double & zmpy2,
+ bool Simulation);
+
+
+ /*! \brief One iteration of the preview control
+ along one axis (using queues)*/
int OneIterationOfPreview1D(Eigen::MatrixXd &x,
double & sxzmp,
deque<double> & ZMPPositions,
@@ -85,11 +88,13 @@ namespace PatternGeneratorJRL
double & zmpx2,
bool Simulation);
- /*! \brief One iteration of the preview control along one axis (using vectors)
+ /*! \brief One iteration of the preview control along one axis
+ (using vectors)
\param [in][out] x: Current state of the CoM along the axis.
\param [in][out] sxzmp: Summed error.
\param [in] ZMPPositions: Vector of ZMP reference positions.
- \param [in] lindex: Starting index in the array of ZMP reference positions.
+ \param [in] lindex: Starting index in the array of ZMP reference
+ positions.
\param [out] zmpx2: Resulting ZMP value.
\param [in] Simulation: This should be set to false.
*/
@@ -126,8 +131,10 @@ namespace PatternGeneratorJRL
/*! @} */
/*! \brief Compute optimal weights.
- \param [in] mode: with initial pos (OptimalControllerSolver::MODE_WITH_INITIALPOS),
- without initial position (OptimalControllerSolver::MODE_WITHOUT_INITIALPOS).
+ \param [in] mode: with initial pos
+ (OptimalControllerSolver::MODE_WITH_INITIALPOS),
+ without initial position (OptimalControllerSolver::
+ MODE_WITHOUT_INITIALPOS).
*/
void ComputeOptimalWeights(unsigned int mode);
diff --git a/src/PreviewControl/SupportFSM.cpp b/src/PreviewControl/SupportFSM.cpp
index 51b368f436fee814a29eb4b8f2c1323bde1b3c3a..22aac914d47f670f9cce064d386e58563a83cec8 100644
--- a/src/PreviewControl/SupportFSM.cpp
+++ b/src/PreviewControl/SupportFSM.cpp
@@ -107,8 +107,9 @@ SupportFSM::update_vel_reference(reference_t & Ref,
void
-SupportFSM::set_support_state(double time, unsigned int pi,
- support_state_t & Support, const reference_t & Ref) const
+SupportFSM::set_support_state
+(double time, unsigned int pi,
+ support_state_t & Support, const reference_t & Ref) const
{
Support.StateChanged = false;
diff --git a/src/PreviewControl/SupportFSM.hh b/src/PreviewControl/SupportFSM.hh
index 9463cf98a144b2e6812f54a5fe7f5a8a0265ef98..084066c274db44d88d5f3c8b8dd554eec2960751 100644
--- a/src/PreviewControl/SupportFSM.hh
+++ b/src/PreviewControl/SupportFSM.hh
@@ -59,7 +59,8 @@ namespace PatternGeneratorJRL
/// \brief Initialize the previewed state
///
/// \param[in] time Current time
- /// \param[in] pi Number of (p)reviewed sampling (i)nstant inside the preview period
+ /// \param[in] pi Number of (p)reviewed sampling (i)nstant
+ /// inside the preview period
/// \param[out] Support Support state to be actualized
/// \param[in] Ref Trajectory reference
void set_support_state( double time,
diff --git a/src/PreviewControl/ZMPPreviewControlWithMultiBodyZMP.cpp b/src/PreviewControl/ZMPPreviewControlWithMultiBodyZMP.cpp
index 2f0b1c46f6e439d15503b6debbfe91bee31131c7..70047184dfc6c962b3abdeedc529da622c015f68 100644
--- a/src/PreviewControl/ZMPPreviewControlWithMultiBodyZMP.cpp
+++ b/src/PreviewControl/ZMPPreviewControlWithMultiBodyZMP.cpp
@@ -641,9 +641,11 @@ SetupIterativePhase
" COMStates.size()=" <<COMStates.size());
FirstStageOfControl(LeftFootPositions[localindex],
RightFootPositions[localindex],COMStates[localindex]);
- ODEBUG("m_FIFOCOMStates["<<localindex<<"]=" << m_FIFOCOMStates[localindex].x[0]
+ ODEBUG("m_FIFOCOMStates["<<localindex<<"]="
+ << m_FIFOCOMStates[localindex].x[0]
<< " " <<
- m_FIFOCOMStates[localindex].y[0] << " " << m_FIFOCOMStates[localindex].z[0] <<
+ m_FIFOCOMStates[localindex].y[0] << " "
+ << m_FIFOCOMStates[localindex].z[0] <<
" m_FIFOCOMStates.size()=" <<m_FIFOCOMStates.size());
//COMState acompos = m_FIFOCOMStates[localindex];
//FootAbsolutePosition aLeftFAP = m_FIFOLeftFootPosition[localindex];
@@ -809,7 +811,9 @@ GetCurrentPositionofWaistInCOMFrame()
m_ComAndFootRealization->
GetCurrentPositionofWaistInCOMFrame();
- // cerr << " Should implement: ZMPPreviewControlWithMultiBodyZMP::GetCurrentPositionOfWaistInCOMFrame()" << endl;
+ // cerr << " Should implement:
+ // ZMPPreviewControlWithMultiBodyZMP::
+ // GetCurrentPositionOfWaistInCOMFrame()" << endl;
return PosOfWaistInCoMFrame;
}
diff --git a/src/PreviewControl/ZMPPreviewControlWithMultiBodyZMP.hh b/src/PreviewControl/ZMPPreviewControlWithMultiBodyZMP.hh
index dcda2cd10b92ed0bc2e2c5f451721b838d87fa45..a8c438022843633d9512eda363438a8cb76f93bc 100644
--- a/src/PreviewControl/ZMPPreviewControlWithMultiBodyZMP.hh
+++ b/src/PreviewControl/ZMPPreviewControlWithMultiBodyZMP.hh
@@ -206,20 +206,25 @@ namespace PatternGeneratorJRL
/*! \name Implementation of the GlobalStrategyManager interface.
@{ */
/*! Set the algorithm used for ZMP and CoM trajectory.
- @param[in] anAlgo: The algorithm to be used for ZMP and CoM trajectory generation.
+ @param[in] anAlgo:
+ The algorithm to be used for ZMP and CoM trajectory generation.
They are 3 possible values:
\li ZMPCOM_TRAJECTORY_FULL: Two preview control are computed. The first
- to generate a CoM trajectory based on the cart model. The second to correct
+ to generate a CoM trajectory based on the cart model.
+ The second to correct
this trajectory using the multibody ZMP.
\li ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY: Only the second stage is used.
The first CoM trajectory is used by a different process. This allow
- to mix different algorithms (notable the quadratic problem with constraints).
+ to mix different algorithms (notable the quadratic problem with
+ constraints).
- \li ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY: Use only the first stage to generate
- the CoM trajectory. It is strongly adviced in this case, to not use
- the geometrical ZMP and CoM trajectory generation but an external CoM task.
+ \li ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY: Use only the first stage
+ to generate the CoM trajectory.
+ It is strongly adviced in this case, to not use
+ the geometrical ZMP and CoM trajectory generation but an external
+ CoM task.
@return Returns false if this is not possible.
*/
@@ -230,25 +235,32 @@ namespace PatternGeneratorJRL
int GetStrategyForStageActivation();
/*! @} */
- /*! Returns the difference between the Waist and the CoM for a starting position. */
+ /*! Returns the difference between the Waist and the CoM
+ for a starting position. */
void GetDifferenceBetweenComAndWaist(double lComAndWaist[3]);
/*! Perform a 5 ms step to generate the full set of angular positions.
The main point of the preview control is to use the future to compute
the current state needed for the robot. Therefore knowing that
- the future window needed is of size NL=SamplingPeriod * PreviewControlWindow,
+ the future window needed is of size NL=SamplingPeriod *
+ PreviewControlWindow,
and that the algorithm is a two stages preview control,
the foot position needs to be provided at k+NL, and the ZMP references
at k+2*NL.
@param[in] LeftFootPosition: The position of the k+NL Left Foot position.
- @param[in] RightFootPosition: The position of the k+NL Right Foot position.
+ @param[in] RightFootPosition: The position of the k+NL
+ Right Foot position.
@param[in] NewZMPRefPos: The ZMP position at k + 2*NL.
- @param[out] finalCOMState: returns position, velocity and acceleration of the CoM
+ @param[out] finalCOMState: returns position, velocity and acceleration of
+ the CoM
after the second stage of control, i.e. the final value.
- @param[out] CurrentConfiguration: The results is a state vector containing the articular positions.
- @param[out] CurrentVelocity: The results is a state vector containing the speed.
- @param[out] CurrentAcceleration: The results is a state vector containing the acceleration.
+ @param[out] CurrentConfiguration: The results is a state vector
+ containing the articular positions.
+ @param[out] CurrentVelocity: The results is a state vector containing the
+ speed.
+ @param[out] CurrentAcceleration: The results is a state vector containing
+ the acceleration.
*/
int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition,
@@ -265,7 +277,8 @@ namespace PatternGeneratorJRL
aCOMState will be updated with the new value of the COM computed by
the card model.
@param[in] LeftFootPosition: The position of the k+NL Left Foot position.
- @param[in] RightFootPosition: The position of the k+NL Right Foot position.
+ @param[in] RightFootPosition: The position of the k+NL
+ Right Foot position.
@param[in] afCOMState: A COM position of reference, in this context,
this will be the height of the waist.
@@ -296,14 +309,18 @@ namespace PatternGeneratorJRL
int SecondStageOfControl(COMState &refandfinal);
/*! Compute the COM of the robot with the Joint values given in BodyAngles,
- velocities set to zero, and returns the values of the COM in aStaringCOMState.
+ velocities set to zero, and returns the values of the COM in
+ aStaringCOMState.
Assuming that the waist is at (0,0,0)
it returns the associate initial values for the left and right foot.
- @param[in] BodyAngles: 4x4 matrix of the robot's root (most of the time, the waist)
+ @param[in] BodyAngles: 4x4 matrix of the robot's root (most of the time,
+ the waist)
pose (position + orientation).
- @param[out] aStartingCOMState: Returns the 3D position of the CoM for the current
+ @param[out] aStartingCOMState: Returns the 3D position of the CoM for the
+ current
position of the robot.
- @param[out] aStartingZMPPosition: Returns the 3D position of the ZMP for the current
+ @param[out] aStartingZMPPosition: Returns the 3D position of the ZMP for
+ the current
position of the robot.
@param[out] InitLeftFootPosition: Returns the position of the left foot in
the waist coordinates frame.
@@ -318,15 +335,17 @@ namespace PatternGeneratorJRL
FootAbsolutePosition & InitRightFootPosition);
/*! Compute the COM of the robot with the Joint values given in BodyAngles,
- velocities set to zero, and returns the values of the COM in aStaringCOMState.
+ velocities set to zero, and returns the values of the COM in
+ aStaringCOMState.
Assuming that the waist is at (0,0,0)
it returns the associate initial values for the left and right foot.
@param BodyAngles: Vector of the joint values for the robot.
@param[out] aStartingCOMState: Position of the CoM.
@param[out] aWaistPosition: Position of the Waist.
- @param[out] InitLeftFootPosition: Position of the left foot in the waist coordinates frame.
- @param[out] InitRightFootPosition: Position of the right foot in the waist coordinates
- frame.
+ @param[out] InitLeftFootPosition: Position of the left foot in the
+ waist coordinates frame.
+ @param[out] InitRightFootPosition: Position of the right foot in the
+ waist coordinates frame.
*/
int EvaluateStartingCoM(Eigen::MatrixXd BodyAngles,
Eigen::Vector3d & aStartingCOMState,
@@ -339,7 +358,8 @@ namespace PatternGeneratorJRL
@{
*/
- /*! Setup (Frontal Global), compute internally all the steps to get NL ZMP multibody values.
+ /*! Setup (Frontal Global), compute internally all the steps to get
+ NL ZMP multibody values.
@param[in] ZMPRefPositions: FIFO of the ZMP reference values.
@param[out] COMStates: FIFO of the COM reference positions
@@ -355,8 +375,9 @@ namespace PatternGeneratorJRL
deque<FootAbsolutePosition> &LeftFootPositions,
deque<FootAbsolutePosition> &RightFootPositions);
- /*! Method to perform the First Phase. It initializes properly the internal fields
- of ZMPPreviewControlWithMultiBodyZMP for the setup phase.
+ /*! Method to perform the First Phase. It initializes properly
+ the internal fields of ZMPPreviewControlWithMultiBodyZMP
+ for the setup phase.
@param[in] ZMPRefPositions: FIFO of the ZMP reference values.
@param[in] COMStates: FIFO of the COM reference positions
@@ -386,11 +407,14 @@ namespace PatternGeneratorJRL
trajectories).
@param[in] RightFootPositions: idem than the previous one but for the
right foot.
- @param[out] CurrentConfiguration: The position part of the state vector realizing the current CoM and
+ @param[out] CurrentConfiguration: The position part of the state vector
+ realizing the current CoM and
feet position instance.
- @param[out] CurrentVelocity: The velocity part of the state vector realizing the current CoM and
+ @param[out] CurrentVelocity: The velocity part of the state vector
+ realizing the current CoM and
feet position instance.
- @param[out] CurrentAcceleration: The acceleration part of the state vector realizing the current CoM and
+ @param[out] CurrentAcceleration: The acceleration part of the state
+ vector realizing the current CoM and
feet position instance.
@param[in] localindex: Value of the index which goes from 0 to 2 * m_NL.
*/
@@ -408,7 +432,8 @@ namespace PatternGeneratorJRL
used by the stepover planner.
@param[out] ExtraCOMBuffer: Extra FIFO for the CoM positions.
- @param[out] ExtraZMPBuffer: Extra FIFO for the ZMP positions (for the stepping over
+ @param[out] ExtraZMPBuffer: Extra FIFO for the ZMP positions
+ (for the stepping over
first preview control).
@param[out] ExtraZMPRefBuffer: Extra FIFO for the ZMP ref positions.
*/
@@ -419,8 +444,8 @@ namespace PatternGeneratorJRL
/*! Evaluate Starting CoM for a given position.
@param[in] BodyAnglesInit: The state vector used to compute the CoM.
@param[out] aStartingCOMState: The CoM of the position specified.
- @param[out] InitLeftFootPosition: Position of the InitLeftFootPosition in the same
- reference frame than the waist.
+ @param[out] InitLeftFootPosition: Position of the InitLeftFootPosition
+ in the same reference frame than the waist.
@param[out] InitRightFootPosition: Position of the InitRightFootPosition
in the same reference frame than the waist
*/
@@ -430,18 +455,22 @@ namespace PatternGeneratorJRL
FootAbsolutePosition & InitLeftFootPosition,
FootAbsolutePosition & InitRightFootPosition);
- /*! This method returns the final COM pose matrix after the second stage of control.
- @return A 4x4 matrix of double which includes the desired final CoM position and orientation.*/
+ /*! This method returns the final COM pose matrix after the second
+ stage of control.
+ @return A 4x4 matrix of double which includes the desired final
+ CoM position and orientation.*/
Eigen::Matrix4d GetFinalDesiredCOMPose();
/*! This method returns the current waist position in the COM reference
frame. This can be used with the previous method to get the final Waist
position.
- @return A 4x4 matrix of double which includes the desired final Waist in the CoM
+ @return A 4x4 matrix of double which includes the desired final
+ Waist in the CoM
phase position and orientation.*/
Eigen::Matrix4d GetCurrentPositionofWaistInCOMFrame();
- /*! Returns the last element of the COM FIFO in the first stage of control */
+ /*! Returns the last element of the COM FIFO in the first stage of
+ control */
COMState GetLastCOMFromFirstStage();
/*! Update the queue of ZMP reference value. */
@@ -463,13 +492,17 @@ namespace PatternGeneratorJRL
and the feet positions will be used.
@param[in] acomp : COM position,
@param[in] aLeftFAP: Pose of the left foot (3D position + 2 euler angles)
- @param[in] aRightFAP: Pose of the right foot (3D position + 2 euler angles)
- @param[out] CurrentConfiguration: Returns the part of state vector corresponding
+ @param[in] aRightFAP: Pose of the right foot (3D position + 2 euler
+ angles)
+ @param[out] CurrentConfiguration: Returns the part of state vector
+ corresponding
to the position of the free floating, and the articular values.
- @param[out] CurrentVelocity: Returns the part of state vector corresponding
- to the velocity of the free floating and the articular values.
- @param[out] CurrentAcceleration: Returns the part of state vector corresponding
- to the acceleration of the free floating, and the articular values.
+ @param[out] CurrentVelocity: Returns the part of state vector
+ corresponding to the velocity of the free floating and the
+ articular values.
+ @param[out] CurrentAcceleration: Returns the part of state vector
+ corresponding to the acceleration of the free floating,
+ and the articular values.
@param[in] IterationNumber: Number of time slot realized so far.
@param[in] StageOfTheAlgorithm: Indicates if this is the second stage of
the preview control or the first one.
@@ -488,8 +521,8 @@ namespace PatternGeneratorJRL
void SetPreviewControl(PreviewControl *aPC);
/*! \name Setter and getter for the jrlHumanoidDynamicRobot object. */
- /*! @param[in] aHumanoidDynamicRobot: an object able to compute dynamic parameters
- of the robot. */
+ /*! @param[in] aHumanoidDynamicRobot: an object able to compute dynamic
+ parameters of the robot. */
inline bool setPinocchioRobot(PinocchioRobot *aPinocchioRobot)
{
m_PinocchioRobot = aPinocchioRobot;
diff --git a/src/PreviewControl/rigid-body-system.cpp b/src/PreviewControl/rigid-body-system.cpp
index 4ea2533fd9a0110f72de3bc358db7948ae5f3eda..534bd509d252129e04a7ca9736b8bb386b7a3ab7 100644
--- a/src/PreviewControl/rigid-body-system.cpp
+++ b/src/PreviewControl/rigid-body-system.cpp
@@ -253,9 +253,10 @@ precompute_trajectories
int
-RigidBodySystem::update( const std::deque<support_state_t> & SupportStates_deq,
- const std::deque<FootAbsolutePosition> & LeftFootTraj_deq,
- const std::deque<FootAbsolutePosition> & RightFootTraj_deq )
+RigidBodySystem::update
+( const std::deque<support_state_t> & SupportStates_deq,
+ const std::deque<FootAbsolutePosition> & LeftFootTraj_deq,
+ const std::deque<FootAbsolutePosition> & RightFootTraj_deq )
{
unsigned nbStepsPreviewed = SupportStates_deq.back().StepNumber;
@@ -346,7 +347,8 @@ RigidBodySystem::compute_dyn_cop( unsigned nbSteps )
LeftFoot_.Trajectory().begin();
std::deque<rigid_body_state_t>::iterator RFTraj_it =
RightFoot_.Trajectory().begin();
- std::deque<rigid_body_state_t>::iterator CoMTraj_it = CoM_.Trajectory().begin();
+ std::deque<rigid_body_state_t>::iterator CoMTraj_it =
+ CoM_.Trajectory().begin();
// std::deque<double>::iterator GRF_it = GRF_deq_.begin();
double GRF = 0.0;
@@ -356,46 +358,61 @@ RigidBodySystem::compute_dyn_cop( unsigned nbSteps )
LeftFoot_.Mass()*(LFTraj_it->Z[2]+GRAVITY)+
RightFoot_.Mass()*(RFTraj_it->Z[2]+GRAVITY);
- CoPDynamicsJerk_.S.row( i ) += CoM_.Dynamics(POSITION).S.row(
- i ) * CoM_.Mass()*( CoMTraj_it->Z[2]+GRAVITY )/GRF;
- CoPDynamicsJerk_.U.row(i ) += CoM_.Dynamics(POSITION).U.row(
- i ) * CoM_.Mass()*( CoMTraj_it->Z[2]+GRAVITY )/GRF;
- CoPDynamicsJerk_.UT.col( i ) += CoM_.Dynamics(POSITION).U.row(
- i ) * CoM_.Mass()*( CoMTraj_it->Z[2]+GRAVITY )/GRF;
- CoPDynamicsJerk_.S.row( i ) -= CoM_.Dynamics(ACCELERATION).S.row(
- i ) * CoM_.Mass()*( CoMTraj_it->Z[0] )/GRF;
- CoPDynamicsJerk_.U.row( i ) -= CoM_.Dynamics(ACCELERATION).U.row(
- i ) * CoM_.Mass()*( CoMTraj_it->Z[0] )/GRF;
- CoPDynamicsJerk_.UT.col( i ) -= CoM_.Dynamics(ACCELERATION).U.row(
- i ) * CoM_.Mass()*( CoMTraj_it->Z[0] )/GRF;
+ CoPDynamicsJerk_.S.row( i ) +=
+ CoM_.Dynamics(POSITION).S.row( i ) * CoM_.Mass()*
+ ( CoMTraj_it->Z[2]+GRAVITY )/GRF;
+ CoPDynamicsJerk_.U.row(i ) +=
+ CoM_.Dynamics(POSITION).U.row( i ) * CoM_.Mass()*
+ ( CoMTraj_it->Z[2]+GRAVITY )/GRF;
+ CoPDynamicsJerk_.UT.col( i ) +=
+ CoM_.Dynamics(POSITION).U.row( i ) * CoM_.Mass()*
+ ( CoMTraj_it->Z[2]+GRAVITY )/GRF;
+ CoPDynamicsJerk_.S.row( i ) -= CoM_.Dynamics(ACCELERATION).S.row( i )
+ * CoM_.Mass()*( CoMTraj_it->Z[0] )/GRF;
+ CoPDynamicsJerk_.U.row( i ) -= CoM_.Dynamics(ACCELERATION).U.row(i )
+ * CoM_.Mass()*( CoMTraj_it->Z[0] )/GRF;
+ CoPDynamicsJerk_.UT.col( i ) -= CoM_.Dynamics(ACCELERATION).U.row( i )
+ * CoM_.Mass()*( CoMTraj_it->Z[0] )/GRF;
if(multiBody_)
{
- LeftFoot_.Dynamics(COP_POSITION).S.row( i ) += LeftFoot_.Dynamics(
- POSITION).S.row( i ) * LeftFoot_.Mass()*( LFTraj_it->Z[2]+GRAVITY )/GRF;
- LeftFoot_.Dynamics(COP_POSITION).U.row( i ) += LeftFoot_.Dynamics(
- POSITION).U.row( i ) * LeftFoot_.Mass()*( LFTraj_it->Z[2]+GRAVITY )/GRF;
- LeftFoot_.Dynamics(COP_POSITION).UT.col( i ) += LeftFoot_.Dynamics(
- POSITION).U.row( i ) * LeftFoot_.Mass()*( LFTraj_it->Z[2]+GRAVITY )/GRF;
- LeftFoot_.Dynamics(COP_POSITION).S.row( i ) -= LeftFoot_.Dynamics(
- ACCELERATION).S.row( i ) * LeftFoot_.Mass()*( LFTraj_it->Z[0] )/GRF;
- LeftFoot_.Dynamics(COP_POSITION).U.row( i ) -= LeftFoot_.Dynamics(
- ACCELERATION).U.row( i ) * LeftFoot_.Mass()*( LFTraj_it->Z[0] )/GRF;
- LeftFoot_.Dynamics(COP_POSITION).UT.col( i ) -= LeftFoot_.Dynamics(
- ACCELERATION).U.row( i ) * LeftFoot_.Mass()*( LFTraj_it->Z[0] )/GRF;
-
- RightFoot_.Dynamics(COP_POSITION).S.row( i ) += RightFoot_.Dynamics(
- POSITION).S.row( i ) * RightFoot_.Mass()*( RFTraj_it->Z[2]+GRAVITY )/GRF;
- RightFoot_.Dynamics(COP_POSITION).U.row( i ) += RightFoot_.Dynamics(
- POSITION).U.row( i ) * RightFoot_.Mass()*( RFTraj_it->Z[2]+GRAVITY )/GRF;
- RightFoot_.Dynamics(COP_POSITION).UT.col( i ) += RightFoot_.Dynamics(
- POSITION).U.row( i ) * RightFoot_.Mass()*( RFTraj_it->Z[2]+GRAVITY )/GRF;
- RightFoot_.Dynamics(COP_POSITION).S.row( i ) -= RightFoot_.Dynamics(
- ACCELERATION).S.row( i ) * RightFoot_.Mass()*( RFTraj_it->Z[0] )/GRF;
- RightFoot_.Dynamics(COP_POSITION).U.row( i ) -= RightFoot_.Dynamics(
- ACCELERATION).U.row( i ) * RightFoot_.Mass()*( RFTraj_it->Z[0] )/GRF;
- RightFoot_.Dynamics(COP_POSITION).UT.col( i ) -= RightFoot_.Dynamics(
- ACCELERATION).U.row( i ) * RightFoot_.Mass()*( RFTraj_it->Z[0] )/GRF;
+ LeftFoot_.Dynamics(COP_POSITION).S.row( i ) +=
+ LeftFoot_.Dynamics(POSITION).S.row( i ) *
+ LeftFoot_.Mass()*( LFTraj_it->Z[2]+GRAVITY )/GRF;
+ LeftFoot_.Dynamics(COP_POSITION).U.row( i ) +=
+ LeftFoot_.Dynamics(POSITION).U.row( i ) *
+ LeftFoot_.Mass()*( LFTraj_it->Z[2]+GRAVITY )/GRF;
+ LeftFoot_.Dynamics(COP_POSITION).UT.col( i ) +=
+ LeftFoot_.Dynamics(POSITION).U.row( i ) *
+ LeftFoot_.Mass()*( LFTraj_it->Z[2]+GRAVITY )/GRF;
+ LeftFoot_.Dynamics(COP_POSITION).S.row( i ) -=
+ LeftFoot_.Dynamics(ACCELERATION).S.row( i ) *
+ LeftFoot_.Mass()*( LFTraj_it->Z[0] )/GRF;
+ LeftFoot_.Dynamics(COP_POSITION).U.row( i ) -=
+ LeftFoot_.Dynamics(ACCELERATION).U.row( i ) *
+ LeftFoot_.Mass()*( LFTraj_it->Z[0] )/GRF;
+ LeftFoot_.Dynamics(COP_POSITION).UT.col( i ) -=
+ LeftFoot_.Dynamics(ACCELERATION).U.row( i ) *
+ LeftFoot_.Mass()*( LFTraj_it->Z[0] )/GRF;
+
+ RightFoot_.Dynamics(COP_POSITION).S.row( i ) +=
+ RightFoot_.Dynamics(POSITION).S.row( i ) *
+ RightFoot_.Mass()*( RFTraj_it->Z[2]+GRAVITY )/GRF;
+ RightFoot_.Dynamics(COP_POSITION).U.row( i ) +=
+ RightFoot_.Dynamics(POSITION).U.row( i ) *
+ RightFoot_.Mass()*( RFTraj_it->Z[2]+GRAVITY )/GRF;
+ RightFoot_.Dynamics(COP_POSITION).UT.col( i ) +=
+ RightFoot_.Dynamics(POSITION).U.row( i ) *
+ RightFoot_.Mass()*( RFTraj_it->Z[2]+GRAVITY )/GRF;
+ RightFoot_.Dynamics(COP_POSITION).S.row( i ) -=
+ RightFoot_.Dynamics(ACCELERATION).S.row( i ) *
+ RightFoot_.Mass()*( RFTraj_it->Z[0] )/GRF;
+ RightFoot_.Dynamics(COP_POSITION).U.row( i ) -=
+ RightFoot_.Dynamics(ACCELERATION).U.row( i ) *
+ RightFoot_.Mass()*( RFTraj_it->Z[0] )/GRF;
+ RightFoot_.Dynamics(COP_POSITION).UT.col( i ) -=
+ RightFoot_.Dynamics(ACCELERATION).U.row( i ) *
+ RightFoot_.Mass()*( RFTraj_it->Z[0] )/GRF;
}
CoMTraj_it++;
@@ -450,7 +467,8 @@ RigidBodySystem::compute_dyn_cjerk( linear_dynamics_t & Dynamics )
Dynamics.S(i,2) = ((i+1)*T_)*((i+1)*T_)/2;
for(unsigned int j=0; j<N_; j++)
if (j<=i)
- Dynamics.U(i,j) = Dynamics.UT(j,i) =(1+3*(i-j)+3*(i-j)*(i-j))*(T_*T_*T_)/6 ;
+ Dynamics.U(i,j) = Dynamics.UT(j,i)=
+ (1+3*(i-j)+3*(i-j)*(i-j))*(T_*T_*T_)/6 ;
else
Dynamics.U(i,j) = Dynamics.UT(j,i) = 0.0;
}
@@ -502,8 +520,10 @@ RigidBodySystem::compute_dyn_cjerk( linear_dynamics_t & Dynamics )
Dynamics.S(i,2) = (i+1)*(i+1)*T_*T_*0.5-CoMHeight_/9.81;
for(unsigned int j=0; j<N_; j++)
if (j<=i)
- Dynamics.U(i,j) = Dynamics.UT(j,
- i) = (1 + 3*(i-j) + 3*(i-j)*(i-j)) * T_*T_*T_/6.0 - T_*CoMHeight_/9.81;
+ Dynamics.U(i,j) =
+ Dynamics.UT(j,i) =
+ (1 + 3*(i-j) + 3*(i-j)*(i-j)) * T_*T_*T_/6.0 -
+ T_*CoMHeight_/9.81;
else
Dynamics.U(i,j) = Dynamics.UT(j,i) = 0.0;
}
@@ -519,9 +539,12 @@ RigidBodySystem::compute_dyn_cjerk( linear_dynamics_t & Dynamics )
int
-RigidBodySystem::compute_foot_zero_dynamics( const std::deque<support_state_t> &
- SupportStates_deq,
- linear_dynamics_t & LeftFootDynamics, linear_dynamics_t & RightFootDynamics )
+RigidBodySystem::
+compute_foot_zero_dynamics
+( const std::deque<support_state_t> &
+ SupportStates_deq,
+ linear_dynamics_t & LeftFootDynamics,
+ linear_dynamics_t & RightFootDynamics )
{
// Resize the matrices:
@@ -649,9 +672,12 @@ RigidBodySystem::compute_foot_zero_dynamics( const std::deque<support_state_t> &
int
-RigidBodySystem::compute_foot_pol_dynamics( const std::deque<support_state_t> &
- SupportStates_deq,
- linear_dynamics_t & LeftFootDynamics, linear_dynamics_t & RightFootDynamics )
+RigidBodySystem::
+compute_foot_pol_dynamics
+( const std::deque<support_state_t> &
+ SupportStates_deq,
+ linear_dynamics_t & LeftFootDynamics,
+ linear_dynamics_t & RightFootDynamics )
{
// Resize the matrices:
@@ -717,16 +743,20 @@ RigidBodySystem::compute_foot_pol_dynamics( const std::deque<support_state_t> &
FFDynamics->S(i,2) = FFDynamics->S(i-1,2);
for(unsigned int SNb = 0; SNb < nbSteps; SNb++)
{
- SFDynamics->U(i,SNb) = SFDynamics->UT(SNb,i) = SFDynamics->U(i-1,SNb);
- FFDynamics->U(i,SNb) = FFDynamics->UT(SNb,i) = FFDynamics->U(i-1,SNb);
+ SFDynamics->U(i,SNb) = SFDynamics->UT(SNb,i) =
+ SFDynamics->U(i-1,SNb);
+ FFDynamics->U(i,SNb) = FFDynamics->UT(SNb,i) =
+ FFDynamics->U(i-1,SNb);
}
}
}
else
{
- compute_sbar( Spbar, Sabar, (SS_it->NbInstants)*T_, FSM_->StepPeriod()-T_ );
- compute_ubar( Upbar, Uabar, (SS_it->NbInstants)*T_, FSM_->StepPeriod()-T_ );
+ compute_sbar( Spbar, Sabar, (SS_it->NbInstants)*T_,
+ FSM_->StepPeriod()-T_ );
+ compute_ubar( Upbar, Uabar, (SS_it->NbInstants)*T_,
+ FSM_->StepPeriod()-T_ );
if(SS_it->StepNumber == 0 && SS_it->StepNumber < nbSteps)
{
if(FFDynamics->Type == POSITION)
@@ -737,9 +767,11 @@ RigidBodySystem::compute_foot_pol_dynamics( const std::deque<support_state_t> &
SFDynamics->S(i,1) = 0.0;
FFDynamics->S(i,2) = Spbar[2];
SFDynamics->S(i,2) = 0.0;
- FFDynamics->U(i,SS_it->StepNumber) = FFDynamics->UT(SS_it->StepNumber,
- i) = Upbar[0];
- SFDynamics->U(i,SS_it->StepNumber) = SFDynamics->UT(SS_it->StepNumber,i) = 0.0;
+ FFDynamics->U(i,SS_it->StepNumber) =
+ FFDynamics->UT(SS_it->StepNumber,
+ i) = Upbar[0];
+ SFDynamics->U(i,SS_it->StepNumber) =
+ SFDynamics->UT(SS_it->StepNumber,i) = 0.0;
}
else if(FFDynamics->Type == ACCELERATION)
{
@@ -749,11 +781,14 @@ RigidBodySystem::compute_foot_pol_dynamics( const std::deque<support_state_t> &
SFDynamics->S(i,1) = 0.0;
FFDynamics->S(i,2) = Sabar[2];
SFDynamics->S(i,2) = 1.0;
- FFDynamics->U(i,SS_it->StepNumber) = FFDynamics->UT(SS_it->StepNumber,
- i) = Uabar[0];
- SFDynamics->U(i,SS_it->StepNumber) = SFDynamics->UT(SS_it->StepNumber,i) = 0.0;
+ FFDynamics->U(i,SS_it->StepNumber) =
+ FFDynamics->UT(SS_it->StepNumber,
+ i) = Uabar[0];
+ SFDynamics->U(i,SS_it->StepNumber) =
+ SFDynamics->UT(SS_it->StepNumber,i) = 0.0;
}
- if(((SS_it->NbInstants)*T_ > FSM_->StepPeriod()-T_) && (SS_it->StepNumber != 0))
+ if(((SS_it->NbInstants)*T_ > FSM_->StepPeriod()-T_) &&
+ (SS_it->StepNumber != 0))
{
// DS phase
FFDynamics->S(i,0) = FFDynamics->S(i-1,0);
@@ -762,35 +797,45 @@ RigidBodySystem::compute_foot_pol_dynamics( const std::deque<support_state_t> &
SFDynamics->S(i,1) = SFDynamics->S(i-1,1);
FFDynamics->S(i,2) = FFDynamics->S(i-1,2);
SFDynamics->S(i,2) = SFDynamics->S(i-1,2);
- FFDynamics->U(i,SS_it->StepNumber) = FFDynamics->UT(SS_it->StepNumber,
- i) = FFDynamics->U(i-1,SS_it->StepNumber);
- SFDynamics->U(i,SS_it->StepNumber) = SFDynamics->UT(SS_it->StepNumber,
- i) = SFDynamics->U(i-1,SS_it->StepNumber);
+ FFDynamics->U(i,SS_it->StepNumber) =
+ FFDynamics->UT(SS_it->StepNumber,
+ i) = FFDynamics->U(i-1,SS_it->StepNumber);
+ SFDynamics->U(i,SS_it->StepNumber) =
+ SFDynamics->UT(SS_it->StepNumber,
+ i) = SFDynamics->U(i-1,SS_it->StepNumber);
}
}
else if(SS_it->StepNumber == 1 && SS_it->StepNumber < nbSteps)
{
if(FFDynamics->Type == POSITION)
{
- FFDynamics->S(i,0) = Spbar[0]; //SFDynamics->S(i,0) = SFDynamics->S(i-1,0);
- FFDynamics->S(i,1) = Spbar[1]; //SFDynamics->S(i,1) = SFDynamics->S(i-1,1);
- FFDynamics->S(i,2) = Spbar[2]; //SFDynamics->S(i,2) = SFDynamics->S(i-1,2);
- FFDynamics->U(i,SS_it->StepNumber) = FFDynamics->UT(SS_it->StepNumber,
- i) = Upbar[0];
- SFDynamics->U(i,SS_it->StepNumber-1) = SFDynamics->UT(SS_it->StepNumber-1,
- i) = 1.0;
+ FFDynamics->S(i,0) = Spbar[0];
+ //SFDynamics->S(i,0) = SFDynamics->S(i-1,0);
+ FFDynamics->S(i,1) = Spbar[1];
+ //SFDynamics->S(i,1) = SFDynamics->S(i-1,1);
+ FFDynamics->S(i,2) = Spbar[2];
+ //SFDynamics->S(i,2) = SFDynamics->S(i-1,2);
+ FFDynamics->U(i,SS_it->StepNumber) =
+ FFDynamics->UT(SS_it->StepNumber,
+ i) = Upbar[0];
+ SFDynamics->U(i,SS_it->StepNumber-1) =
+ SFDynamics->UT(SS_it->StepNumber-1,
+ i) = 1.0;
}
else if(FFDynamics->Type == ACCELERATION)
{
FFDynamics->S(i,0) = Sabar[0];
FFDynamics->S(i,1) = Sabar[1];
FFDynamics->S(i,2) = Sabar[2];
- FFDynamics->U(i,SS_it->StepNumber) = FFDynamics->UT(SS_it->StepNumber,
- i) = Uabar[0];
- SFDynamics->U(i,SS_it->StepNumber-1) = SFDynamics->UT(SS_it->StepNumber-1,
- i) = 1.0;
+ FFDynamics->U(i,SS_it->StepNumber) =
+ FFDynamics->UT(SS_it->StepNumber,
+ i) = Uabar[0];
+ SFDynamics->U(i,SS_it->StepNumber-1) =
+ SFDynamics->UT(SS_it->StepNumber-1,
+ i) = 1.0;
}
- // The foot has touched the ground, the support phase has not switched yet
+ // The foot has touched the ground, the support phase has not
+ // switched yet
if((SS_it->NbInstants)*T_ > FSM_->StepPeriod()-T_)
{
FFDynamics->S(i,0) = FFDynamics->S(i-1,0);
@@ -799,10 +844,12 @@ RigidBodySystem::compute_foot_pol_dynamics( const std::deque<support_state_t> &
SFDynamics->S(i,1) = SFDynamics->S(i-1,1);
FFDynamics->S(i,2) = FFDynamics->S(i-1,2);
SFDynamics->S(i,2) = SFDynamics->S(i-1,2);
- FFDynamics->U(i,SS_it->StepNumber) = FFDynamics->UT(SS_it->StepNumber,
- i) = FFDynamics->U(i-1,SS_it->StepNumber);
- SFDynamics->U(i,SS_it->StepNumber-1) = SFDynamics->UT(SS_it->StepNumber-1,
- i) = SFDynamics->U(i-1,SS_it->StepNumber-1);
+ FFDynamics->U(i,SS_it->StepNumber) =
+ FFDynamics->UT(SS_it->StepNumber,
+ i) = FFDynamics->U(i-1,SS_it->StepNumber);
+ SFDynamics->U(i,SS_it->StepNumber-1) =
+ SFDynamics->UT(SS_it->StepNumber-1,
+ i) = SFDynamics->U(i-1,SS_it->StepNumber-1);
}
}
else if(SS_it->StepNumber == 2)
@@ -815,8 +862,10 @@ RigidBodySystem::compute_foot_pol_dynamics( const std::deque<support_state_t> &
SFDynamics->S(i,2) = SFDynamics->S(i-1,2);
for(unsigned int j = 0; j<nbSteps; j++)
{
- FFDynamics->U(i,j) = FFDynamics->UT(j,i) = FFDynamics->U(i-1,j);
- SFDynamics->U(i,j) = SFDynamics->UT(j,i) = SFDynamics->U(i-1,j);
+ FFDynamics->U(i,j) = FFDynamics->UT(j,i) =
+ FFDynamics->U(i-1,j);
+ SFDynamics->U(i,j) = SFDynamics->UT(j,i) =
+ SFDynamics->U(i-1,j);
}
}
}
@@ -828,163 +877,19 @@ RigidBodySystem::compute_foot_pol_dynamics( const std::deque<support_state_t> &
}
-//int
-//RigidBodySystem::compute_foot_cjerk_dynamics( const std::deque<support_state_t> & SupportStates_deq,
-// linear_dynamics_t & LeftFootDynamics, linear_dynamics_t & RightFootDynamics )
-//{
-//
-// // Resize the matrices:
-// // --------------------
-// unsigned int nbSteps = SupportStates_deq.back().StepNumber;
-// LeftFootDynamics.U.resize(N_,nbSteps);
-// LeftFootDynamics.U.setZero();
-// LeftFootDynamics.UT.resize(nbSteps,N_);
-// LeftFootDynamics.UT.setZero();
-// LeftFootDynamics.S.setZero();
-// RightFootDynamics.U.resize(N_,nbSteps);
-// RightFootDynamics.U.setZero();
-// RightFootDynamics.UT.resize(nbSteps,N_);
-// RightFootDynamics.UT.setZero();
-// RightFootDynamics.S.setZero();
-//
-// // Fill the matrices:
-// // ------------------
-// linear_dynamics_t * SFDynamics;
-// linear_dynamics_t * FFDynamics;
-// double Spbar[3], Sabar[3];
-// double Upbar[2], Uabar[2];
-// unsigned int SwitchInstant = 0;
-// std::deque<support_state_t>::const_iterator SS_it =
-// SupportStates_deq.begin();
-// SS_it++;
-// for(unsigned int i=0;i<N_;i++)
-// {
-// if(SS_it->Foot == LEFT)
-// {
-// SFDynamics = & LeftFootDynamics;
-// FFDynamics = & RightFootDynamics;
-// }
-// else
-// {
-// SFDynamics = & RightFootDynamics;
-// FFDynamics = & LeftFootDynamics;
-// }
-// if(SS_it->StateChanged == true)
-// {
-// SwitchInstant = i+1;
-// }
-// if(SS_it->Phase == DS)
-// {
-// // The previous row is copied in DS phase
-// if(i==0)
-// {
-// if(SFDynamics->Type == POSITION)
-// {
-// SFDynamics->S(i,0) = 1.0;FFDynamics->S(i,0) = 1.0;
-// SFDynamics->S(i,1) = 0.0;FFDynamics->S(i,1) = 0.0;
-// SFDynamics->S(i,2) = 0.0;FFDynamics->S(i,2) = 0.0;
-// }
-// }
-// if(i>0)
-// {
-// SFDynamics->S(i,0) = SFDynamics->S(i-1,0);FFDynamics->S(i,0) = FFDynamics->S(i-1,0);
-// SFDynamics->S(i,1) = SFDynamics->S(i-1,1);FFDynamics->S(i,1) = FFDynamics->S(i-1,1);
-// SFDynamics->S(i,2) = SFDynamics->S(i-1,2);FFDynamics->S(i,2) = FFDynamics->S(i-1,2);
-// for(unsigned int SNb = 0; SNb < nbSteps; SNb++)
-// {
-// SFDynamics->U(i,SNb) = SFDynamics->UT(SNb,i) = SFDynamics->U(i-1,SNb);
-// FFDynamics->U(i,SNb) = FFDynamics->UT(SNb,i) = FFDynamics->U(i-1,SNb);
-// }
-// }
-// }
-// else
-// {
-//
-// compute_sbar( Spbar, Sabar, (SS_it->NbInstants+1)*T_, FSM_->StepPeriod()-T_ );
-// compute_ubar( Upbar, Uabar, (SS_it->NbInstants+1)*T_, FSM_->StepPeriod()-T_ );
-// if(SS_it->StepNumber == 0 && SS_it->StepNumber < nbSteps)
-// {
-// if(FFDynamics->Type == POSITION)
-// {
-// FFDynamics->S(i,0) = Spbar[0];SFDynamics->S(i,0) = 1.0;
-// FFDynamics->S(i,1) = Spbar[1];SFDynamics->S(i,1) = 0.0;
-// FFDynamics->S(i,2) = Spbar[2];SFDynamics->S(i,2) = 0.0;
-// FFDynamics->U(i,SS_it->StepNumber) = FFDynamics->UT(SS_it->StepNumber,i) = Upbar[0];
-// SFDynamics->U(i,SS_it->StepNumber) = SFDynamics->UT(SS_it->StepNumber,i) = 0.0;
-// }
-// else if(FFDynamics->Type == ACCELERATION)
-// {
-// FFDynamics->S(i,0) = Sabar[0];SFDynamics->S(i,0) = 0.0;
-// FFDynamics->S(i,1) = Sabar[1];SFDynamics->S(i,1) = 0.0;
-// FFDynamics->S(i,2) = Sabar[2];SFDynamics->S(i,2) = 1.0;
-// FFDynamics->U(i,SS_it->StepNumber) = FFDynamics->UT(SS_it->StepNumber,i) = Uabar[0];
-// SFDynamics->U(i,SS_it->StepNumber) = SFDynamics->UT(SS_it->StepNumber,i) = 0.0;
-// }
-// if((SS_it->NbInstants+1)*T_ > FSM_->StepPeriod()-T_)
-// {
-// FFDynamics->S(i,0) = FFDynamics->S(i-1,0);SFDynamics->S(i,0) = SFDynamics->S(i-1,0);
-// FFDynamics->S(i,1) = FFDynamics->S(i-1,1);SFDynamics->S(i,1) = SFDynamics->S(i-1,1);
-// FFDynamics->S(i,2) = FFDynamics->S(i-1,2);SFDynamics->S(i,2) = SFDynamics->S(i-1,2);
-// FFDynamics->U(i,SS_it->StepNumber) = FFDynamics->UT(SS_it->StepNumber,i) = FFDynamics->U(i-1,SS_it->StepNumber);
-// SFDynamics->U(i,SS_it->StepNumber) = SFDynamics->UT(SS_it->StepNumber,i) = SFDynamics->U(i-1,SS_it->StepNumber);
-// }
-// }
-// else if(SS_it->StepNumber == 1 && SS_it->StepNumber < nbSteps)
-// {
-// if(FFDynamics->Type == POSITION)
-// {
-// FFDynamics->S(i,0) = Spbar[0]; //SFDynamics->S(i,0) = SFDynamics->S(i-1,0);
-// FFDynamics->S(i,1) = Spbar[1]; //SFDynamics->S(i,1) = SFDynamics->S(i-1,1);
-// FFDynamics->S(i,2) = Spbar[2]; //SFDynamics->S(i,2) = SFDynamics->S(i-1,2);
-// FFDynamics->U(i,SS_it->StepNumber) = FFDynamics->UT(SS_it->StepNumber,i) = Upbar[0];
-// SFDynamics->U(i,SS_it->StepNumber-1) = SFDynamics->UT(SS_it->StepNumber-1,i) = 1.0;
-// }
-// else if(FFDynamics->Type == ACCELERATION)
-// {
-// FFDynamics->S(i,0) = Sabar[0];
-// FFDynamics->S(i,1) = Sabar[1];
-// FFDynamics->S(i,2) = Sabar[2];
-// FFDynamics->U(i,SS_it->StepNumber) = FFDynamics->UT(SS_it->StepNumber,i) = Uabar[0];
-// SFDynamics->U(i,SS_it->StepNumber-1) = SFDynamics->UT(SS_it->StepNumber-1,i) = 1.0;
-// }
-// if((SS_it->NbInstants+1)*T_ > FSM_->StepPeriod()-T_)
-// {
-// FFDynamics->S(i,0) = FFDynamics->S(i-1,0);SFDynamics->S(i,0) = SFDynamics->S(i-1,0);
-// FFDynamics->S(i,1) = FFDynamics->S(i-1,1);SFDynamics->S(i,1) = SFDynamics->S(i-1,1);
-// FFDynamics->S(i,2) = FFDynamics->S(i-1,2);SFDynamics->S(i,2) = SFDynamics->S(i-1,2);
-// FFDynamics->U(i,SS_it->StepNumber) = FFDynamics->UT(SS_it->StepNumber,i) = FFDynamics->U(i-1,SS_it->StepNumber);
-// SFDynamics->U(i,SS_it->StepNumber-1) = SFDynamics->UT(SS_it->StepNumber-1,i) = SFDynamics->U(i-1,SS_it->StepNumber-1);
-// }
-// }
-// else if(SS_it->StepNumber == 2)
-// {
-// FFDynamics->S(i,0) = FFDynamics->S(i-1,0);SFDynamics->S(i,0) = SFDynamics->S(i-1,0);
-// FFDynamics->S(i,1) = FFDynamics->S(i-1,1);SFDynamics->S(i,1) = SFDynamics->S(i-1,1);
-// FFDynamics->S(i,2) = FFDynamics->S(i-1,2);SFDynamics->S(i,2) = SFDynamics->S(i-1,2);
-// for(unsigned int j = 0; j<nbSteps; j++)
-// {
-// FFDynamics->U(i,j) = FFDynamics->UT(j,i) = FFDynamics->U(i-1,j);
-// SFDynamics->U(i,j) = SFDynamics->UT(j,i) = SFDynamics->U(i-1,j);
-// }
-// }
-// }
-// SS_it++;
-// }
-//
-// return 0;
-//
-//}
int
-RigidBodySystem::generate_trajectories( double Time,
- const solution_t & Solution,
- const std::deque<support_state_t> & PrwSupportStates_deq,
- const std::deque<double> & PreviewedSupportAngles_deq,
- std::deque<FootAbsolutePosition> & LeftFootTraj_deq,
- std::deque<FootAbsolutePosition> & RightFootTraj_deq )
+RigidBodySystem::
+generate_trajectories
+( double Time,
+ const solution_t & Solution,
+ const std::deque<support_state_t> & PrwSupportStates_deq,
+ const std::deque<double> & PreviewedSupportAngles_deq,
+ std::deque<FootAbsolutePosition> & LeftFootTraj_deq,
+ std::deque<FootAbsolutePosition> & RightFootTraj_deq )
{
-
+
OFTG_->interpolate_feet_positions(Time, PrwSupportStates_deq,
Solution, PreviewedSupportAngles_deq,
LeftFootTraj_deq, RightFootTraj_deq);
diff --git a/src/PreviewControl/rigid-body-system.hh b/src/PreviewControl/rigid-body-system.hh
index 01e8fec7cede2480ee428c57b0b219f1223babe9..02ee16f073c274d75af8240b26ee8a71d42ad4e8 100644
--- a/src/PreviewControl/rigid-body-system.hh
+++ b/src/PreviewControl/rigid-body-system.hh
@@ -93,11 +93,12 @@ namespace PatternGeneratorJRL
/// \param[out] RightFootTraj_deq
///
/// return 0
- int generate_trajectories( double time, const solution_t & Result,
- const std::deque<support_state_t> & SupportStates_deq,
- const std::deque<double> & PreviewedSupportAngles_deq,
- std::deque<FootAbsolutePosition> & LeftFootTraj_deq,
- std::deque<FootAbsolutePosition> & RightFootTraj_deq);
+ int generate_trajectories
+ ( double time, const solution_t & Result,
+ const std::deque<support_state_t> & SupportStates_deq,
+ const std::deque<double> & PreviewedSupportAngles_deq,
+ std::deque<FootAbsolutePosition> & LeftFootTraj_deq,
+ std::deque<FootAbsolutePosition> & RightFootTraj_deq);
/// \name Accessors and mutators
/// \{
@@ -232,10 +233,12 @@ namespace PatternGeneratorJRL
/// \param[out] RightFootDynamics
///
/// return 0
- int compute_foot_zero_dynamics( const std::deque<support_state_t> &
- SupportStates_deq,
- linear_dynamics_t & LeftFootDynamics, linear_dynamics_t & RightFootDynamics);
-
+ int compute_foot_zero_dynamics
+ ( const std::deque<support_state_t> &
+ SupportStates_deq,
+ linear_dynamics_t & LeftFootDynamics,
+ linear_dynamics_t & RightFootDynamics);
+
/// \brief Compute foot dynamics based on polynomial interpolation
///
/// \param[in] SupportStates_deq Previewed support states
@@ -243,10 +246,12 @@ namespace PatternGeneratorJRL
/// \param[out] RightFootDynamics
///
/// return 0
- int compute_foot_pol_dynamics( const std::deque<support_state_t> &
- SupportStates_deq,
- linear_dynamics_t & LeftFootDynamics, linear_dynamics_t & RightFootDynamics);
-
+ int compute_foot_pol_dynamics
+ ( const std::deque<support_state_t> &
+ SupportStates_deq,
+ linear_dynamics_t & LeftFootDynamics,
+ linear_dynamics_t & RightFootDynamics);
+
/// \brief Compute foot dynamics based on "piecewise constant jerk" splines
///
/// \param[in] SupportStates_deq Previewed support states
@@ -254,10 +259,12 @@ namespace PatternGeneratorJRL
/// \param[out] RightFootDynamics
///
/// return 0
- int compute_foot_cjerk_dynamics( const std::deque<support_state_t> &
- SupportStates_deq,
- linear_dynamics_t & LeftFootDynamics, linear_dynamics_t & RightFootDynamics);
-
+ int compute_foot_cjerk_dynamics
+ ( const std::deque<support_state_t> &
+ SupportStates_deq,
+ linear_dynamics_t & LeftFootDynamics,
+ linear_dynamics_t & RightFootDynamics);
+
/// \brief Initialize static trajectories
int initialize_trajectories();
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.cpp
index af2f1f609dd249004809373bff2dd6cc012af1cf..2e1a0d1200c5fe684a53c2558d8272c3ad473cfc 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.cpp
@@ -61,8 +61,9 @@ namespace PatternGeneratorJRL
}
- bool AnalyticalMorisawaAbstract::SetNumberOfStepsInAdvance(
- int lNumberOfStepsInAdvance)
+ bool AnalyticalMorisawaAbstract::
+ SetNumberOfStepsInAdvance
+ (int lNumberOfStepsInAdvance)
{
m_NumberOfStepsInAdvance = lNumberOfStepsInAdvance;
return true;
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index 047765e78ddf92c365960c5a4023116e22282154..64b37b15d3b0bf3e82cb5b865c24d2c5d3d3a074 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -78,8 +78,10 @@ namespace PatternGeneratorJRL
{
- AnalyticalMorisawaCompact::AnalyticalMorisawaCompact(SimplePluginManager *lSPM,
- PinocchioRobot *aPR)
+ AnalyticalMorisawaCompact::
+ AnalyticalMorisawaCompact
+ (SimplePluginManager *lSPM,
+ PinocchioRobot *aPR)
: AnalyticalMorisawaAbstract(lSPM)
{
@@ -98,24 +100,31 @@ namespace PatternGeneratorJRL
m_NeedToReset = true;
m_AbsoluteTimeReference = 0.0;
- m_PreviewControl = new PreviewControl(lSPM,
- OptimalControllerSolver::MODE_WITH_INITIALPOS,
- true);
+ m_PreviewControl =
+ new PreviewControl
+ (lSPM,
+ OptimalControllerSolver::MODE_WITH_INITIALPOS,
+ true);
- /*! Dynamic allocation of the analytical trajectories for the ZMP and the COG */
+ /*! Dynamic allocation of the analytical trajectories for the ZMP
+ and the COG */
m_AnalyticalZMPCoGTrajectoryX = new AnalyticalZMPCOGTrajectory(7);
m_AnalyticalZMPCoGTrajectoryY = new AnalyticalZMPCOGTrajectory(7);
// m_AnalyticalZMPCoGTrajectoryZ = new AnalyticalZMPCOGTrajectory(7);
/*! Dynamic allocation of the filters. */
- m_FilterXaxisByPC = new FilteringAnalyticalTrajectoryByPreviewControl(lSPM,
- m_AnalyticalZMPCoGTrajectoryX,
- m_PreviewControl);
-
- m_FilterYaxisByPC = new FilteringAnalyticalTrajectoryByPreviewControl(lSPM,
- m_AnalyticalZMPCoGTrajectoryY,
- m_PreviewControl);
-
+ m_FilterXaxisByPC =
+ new FilteringAnalyticalTrajectoryByPreviewControl
+ (lSPM,
+ m_AnalyticalZMPCoGTrajectoryX,
+ m_PreviewControl);
+
+ m_FilterYaxisByPC =
+ new FilteringAnalyticalTrajectoryByPreviewControl
+ (lSPM,
+ m_AnalyticalZMPCoGTrajectoryY,
+ m_PreviewControl);
+
m_kajitaDynamicFilter = new DynamicFilter(lSPM,m_PR);
m_VerboseLevel=0;
@@ -137,11 +146,13 @@ namespace PatternGeneratorJRL
if (m_VerboseLevel>2)
{
// Display the clock for some part of the code.
- cout << "Part of the foot position computation + queue handling." << endl;
+ cout << "Part of the foot position computation + queue handling."
+ << endl;
m_Clock1.Display();
cout << "Part of the foot change landing position" << endl;
m_Clock2.Display();
- cout << "Part on the analytical ZMP COG trajectories and foot polynomial computation"
+ cout << "Part on the analytical ZMP COG trajectories and "
+ << "foot polynomial computation"
<< endl;
m_Clock3.Display();
}
@@ -266,15 +277,8 @@ namespace PatternGeneratorJRL
m_AF.resize(SizeOfZ,2*SizeOfZ);
m_IPIV.resize(SizeOfZ);
-
- {
- for(unsigned int i=0; i<m_AF.rows(); i++) for(unsigned int j=0; j<m_AF.cols();
- j++)
- m_AF(i,j)=0;
- };
- {
- for(unsigned int i=0; i<m_IPIV.size(); m_IPIV[i++]=0);
- };
+ m_AF.setZero();
+ m_IPIV.setZero();
m_NeedToReset = true;
}
@@ -376,12 +380,13 @@ namespace PatternGeneratorJRL
}
- int AnalyticalMorisawaCompact::BuildAndSolveCOMZMPForASetOfSteps(
- Eigen::Matrix3d &lStartingCOMState,
- FootAbsolutePosition &LeftFootInitialPosition,
- FootAbsolutePosition &RightFootInitialPosition,
- bool IgnoreFirstRelativeFoot,
- bool DoNotPrepareLastFoot)
+ int AnalyticalMorisawaCompact::
+ BuildAndSolveCOMZMPForASetOfSteps
+ (Eigen::Matrix3d &lStartingCOMState,
+ FootAbsolutePosition &LeftFootInitialPosition,
+ FootAbsolutePosition &RightFootInitialPosition,
+ bool IgnoreFirstRelativeFoot,
+ bool DoNotPrepareLastFoot)
{
if (m_RelativeFootPositions.size()==0)
@@ -419,10 +424,10 @@ namespace PatternGeneratorJRL
m_AnalyticalZMPCoGTrajectoryX->SetPolynomialDegrees(m_PolynomialDegrees);
m_AnalyticalZMPCoGTrajectoryY->SetPolynomialDegrees(m_PolynomialDegrees);
- m_AnalyticalZMPCoGTrajectoryX->SetStartingTimeIntervalsAndHeightVariation(
- m_DeltaTj,m_Omegaj);
- m_AnalyticalZMPCoGTrajectoryY->SetStartingTimeIntervalsAndHeightVariation(
- m_DeltaTj,m_Omegaj);
+ m_AnalyticalZMPCoGTrajectoryX->
+ SetStartingTimeIntervalsAndHeightVariation(m_DeltaTj,m_Omegaj);
+ m_AnalyticalZMPCoGTrajectoryY->
+ SetStartingTimeIntervalsAndHeightVariation(m_DeltaTj,m_Omegaj);
/* Build the profil for the X and Y axis. */
double InitialCoMX=0.0;
@@ -455,11 +460,12 @@ namespace PatternGeneratorJRL
if (m_FeetTrajectoryGenerator!=0)
{
m_FeetTrajectoryGenerator->SetDeltaTj(m_DeltaTj);
- m_FeetTrajectoryGenerator->InitializeFromRelativeSteps(m_RelativeFootPositions,
- LeftFootInitialPosition,
- RightFootInitialPosition,
- m_AbsoluteSupportFootPositions,
- IgnoreFirstRelativeFoot, false);
+ m_FeetTrajectoryGenerator->InitializeFromRelativeSteps
+ (m_RelativeFootPositions,
+ LeftFootInitialPosition,
+ RightFootInitialPosition,
+ m_AbsoluteSupportFootPositions,
+ IgnoreFirstRelativeFoot, false);
unsigned int i=0,j=1;
for(i=0,j=1; i<m_AbsoluteSupportFootPositions.size(); i++,j+=2)
@@ -474,7 +480,8 @@ namespace PatternGeneratorJRL
// Strategy for the final CoM pos: middle of the segment
// between the two final steps, in order to be statically stable.
- unsigned int lindex = (unsigned int)(m_AbsoluteSupportFootPositions.size()-1);
+ unsigned int lindex = (unsigned int)
+ (m_AbsoluteSupportFootPositions.size()-1);
if (DoNotPrepareLastFoot)
FinalCoMPosX = m_AbsoluteSupportFootPositions[lindex].x;
@@ -482,7 +489,8 @@ namespace PatternGeneratorJRL
FinalCoMPosX = 0.5 *(m_AbsoluteSupportFootPositions[lindex-1].x +
m_AbsoluteSupportFootPositions[lindex].x);
if (DoNotPrepareLastFoot)
- (*lZMPX)[j-2] = (*lZMPX)[j-1] = m_AbsoluteSupportFootPositions[lindex].x;
+ (*lZMPX)[j-2] = (*lZMPX)[j-1] =
+ m_AbsoluteSupportFootPositions[lindex].x;
else
(*lZMPX)[j-2] = (*lZMPX)[j-1] = FinalCoMPosX;
@@ -493,7 +501,8 @@ namespace PatternGeneratorJRL
m_AbsoluteSupportFootPositions[lindex].y);
if (DoNotPrepareLastFoot)
- (*lZMPY)[j-2] = (*lZMPY)[j-1] = m_AbsoluteSupportFootPositions[lindex].y;
+ (*lZMPY)[j-2] = (*lZMPY)[j-1] =
+ m_AbsoluteSupportFootPositions[lindex].y;
else
(*lZMPY)[j-2] = (*lZMPY)[j-1] = FinalCoMPosY;
}
@@ -507,10 +516,12 @@ namespace PatternGeneratorJRL
/*! Build 3rd order polynomials. */
for(int i=1; i<NbOfIntervals-1; i++)
{
- m_AnalyticalZMPCoGTrajectoryX->Building3rdOrderPolynomial(i,(*lZMPX)[i-1],
- (*lZMPX)[i]);
- m_AnalyticalZMPCoGTrajectoryY->Building3rdOrderPolynomial(i,(*lZMPY)[i-1],
- (*lZMPY)[i]);
+ m_AnalyticalZMPCoGTrajectoryX->
+ Building3rdOrderPolynomial(i,(*lZMPX)[i-1],
+ (*lZMPX)[i]);
+ m_AnalyticalZMPCoGTrajectoryY->
+ Building3rdOrderPolynomial(i,(*lZMPY)[i-1],
+ (*lZMPY)[i]);
}
// Block for X trajectory
@@ -536,17 +547,19 @@ namespace PatternGeneratorJRL
}
- void AnalyticalMorisawaCompact::GetZMPDiscretization(deque<ZMPPosition> &
- ZMPPositions,
- deque<COMState> & COMStates,
- deque<RelativeFootPosition> &RelativeFootPositions,
- deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
- deque<FootAbsolutePosition> &RightFootAbsolutePositions,
- double,
- COMState & lStartingCOMState,
- Eigen::Vector3d &,
- FootAbsolutePosition & InitLeftFootAbsolutePosition,
- FootAbsolutePosition & InitRightFootAbsolutePosition)
+ void AnalyticalMorisawaCompact::
+ GetZMPDiscretization
+ (deque<ZMPPosition> &
+ ZMPPositions,
+ deque<COMState> & COMStates,
+ deque<RelativeFootPosition> &RelativeFootPositions,
+ deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> &RightFootAbsolutePositions,
+ double,
+ COMState & lStartingCOMState,
+ Eigen::Vector3d &,
+ FootAbsolutePosition & InitLeftFootAbsolutePosition,
+ FootAbsolutePosition & InitRightFootAbsolutePosition)
{
// INITIALIZE FEET POSITIONS:
// --------------------------
@@ -568,7 +581,8 @@ namespace PatternGeneratorJRL
->GetCurrentPositionofWaistInCOMFrame();
m_RelativeFootPositions = RelativeFootPositions;
- /* This part computes the CoM and ZMP trajectory giving the foot position information.
+ /* This part computes the CoM and ZMP trajectory giving
+ the foot position information.
It also creates the analytical feet trajectories.
*/
Eigen::Matrix3d lMStartingCOMState;
@@ -606,15 +620,17 @@ namespace PatternGeneratorJRL
/*! Set the current time reference for the analytical trajectory. */
double TimeShift = m_Tsingle*2;
m_AbsoluteTimeReference = m_CurrentTime-TimeShift;
- m_AnalyticalZMPCoGTrajectoryX->SetAbsoluteTimeReference(
- m_AbsoluteTimeReference);
- m_AnalyticalZMPCoGTrajectoryY->SetAbsoluteTimeReference(
- m_AbsoluteTimeReference);
- m_FeetTrajectoryGenerator->SetAbsoluteTimeReference(m_AbsoluteTimeReference);
+ m_AnalyticalZMPCoGTrajectoryX->
+ SetAbsoluteTimeReference(m_AbsoluteTimeReference);
+ m_AnalyticalZMPCoGTrajectoryY->
+ SetAbsoluteTimeReference(m_AbsoluteTimeReference);
+ m_FeetTrajectoryGenerator->
+ SetAbsoluteTimeReference(m_AbsoluteTimeReference);
/*! Compute the total size of the array related to the steps. */
FillQueues(m_CurrentTime,m_CurrentTime+m_PreviewControlTime-TimeShift,
- ZMPPositions, COMStates,LeftFootAbsolutePositions, RightFootAbsolutePositions);
+ ZMPPositions, COMStates,LeftFootAbsolutePositions,
+ RightFootAbsolutePositions);
bool filterOn_ = true ;
if(filterOn_)
@@ -625,7 +641,8 @@ namespace PatternGeneratorJRL
m_kajitaDynamicFilter->init( m_SamplingPeriod,
m_SamplingPeriod,
n*m_SamplingPeriod,
- m_PreviewControlTime-TimeShift+KajitaPCpreviewWindow,
+ m_PreviewControlTime-
+ TimeShift+KajitaPCpreviewWindow,
KajitaPCpreviewWindow,
lStartingCOMState );
/*! Set the upper body trajectory */
@@ -686,7 +703,9 @@ namespace PatternGeneratorJRL
COMState lastCoM = COMStates.back();
FootAbsolutePosition lastLF = LeftFootAbsolutePositions.back();
FootAbsolutePosition lastRF = RightFootAbsolutePositions.back();
- for (unsigned int i = 0 ; i < KajitaPCpreviewWindow/m_SamplingPeriod ; ++i)
+ for (unsigned int i = 0 ;
+ i < KajitaPCpreviewWindow/m_SamplingPeriod ;
+ ++i)
{
ZMPPositions.push_back(lastZMP);
COMStates.push_back(lastCoM);
@@ -703,16 +722,17 @@ namespace PatternGeneratorJRL
// Filter the trajectory
deque<COMState> outputDeltaCOMTraj_deq (n) ;
- m_kajitaDynamicFilter->OffLinefilter(
- COMStates,
- ZMPPositions,
- LeftFootAbsolutePositions,
- RightFootAbsolutePositions,
- vector< Eigen::VectorXd > (1,UpperConfig),
- vector< Eigen::VectorXd > (1,UpperVel),
- vector< Eigen::VectorXd > (1,UpperAcc),
- outputDeltaCOMTraj_deq);
-
+ m_kajitaDynamicFilter->
+ OffLinefilter
+ (COMStates,
+ ZMPPositions,
+ LeftFootAbsolutePositions,
+ RightFootAbsolutePositions,
+ vector< Eigen::VectorXd > (1,UpperConfig),
+ vector< Eigen::VectorXd > (1,UpperVel),
+ vector< Eigen::VectorXd > (1,UpperAcc),
+ outputDeltaCOMTraj_deq);
+
#ifdef DEBUG
m_kajitaDynamicFilter->Debug(COMStates,LeftFootAbsolutePositions,
RightFootAbsolutePositions,
@@ -731,7 +751,9 @@ namespace PatternGeneratorJRL
}
}
- for (unsigned int i = 0 ; i < KajitaPCpreviewWindow/m_SamplingPeriod ; ++i)
+ for (unsigned int i = 0 ;
+ i < KajitaPCpreviewWindow/m_SamplingPeriod ;
+ ++i)
{
ZMPPositions.pop_back();
COMStates.pop_back();
@@ -748,16 +770,18 @@ namespace PatternGeneratorJRL
}
}
- std::size_t AnalyticalMorisawaCompact::InitOnLine(deque<ZMPPosition> &
- FinalZMPPositions,
- deque<COMState> & FinalCoMPositions,
- deque<FootAbsolutePosition> & FinalLeftFootAbsolutePositions,
- deque<FootAbsolutePosition> & FinalRightFootAbsolutePositions,
- FootAbsolutePosition & InitLeftFootAbsolutePosition,
- FootAbsolutePosition & InitRightFootAbsolutePosition,
- deque<RelativeFootPosition> &RelativeFootPositions,
- COMState & lStartingCOMState,
- Eigen::Vector3d &)
+ std::size_t AnalyticalMorisawaCompact::
+ InitOnLine
+ (deque<ZMPPosition> &
+ FinalZMPPositions,
+ deque<COMState> & FinalCoMPositions,
+ deque<FootAbsolutePosition> & FinalLeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> & FinalRightFootAbsolutePositions,
+ FootAbsolutePosition & InitLeftFootAbsolutePosition,
+ FootAbsolutePosition & InitRightFootAbsolutePosition,
+ deque<RelativeFootPosition> &RelativeFootPositions,
+ COMState & lStartingCOMState,
+ Eigen::Vector3d &)
{
m_OnLineMode = true;
m_RelativeFootPositions.clear();
@@ -816,7 +840,8 @@ namespace PatternGeneratorJRL
else
m_AbsoluteCurrentSupportFootPosition = InitLeftFootAbsolutePosition;
- /* This part computes the CoM and ZMP trajectory giving the foot position information.
+ /* This part computes the CoM and ZMP trajectory
+ giving the foot position information.
It also creates the analytical feet trajectories.
*/
if (BuildAndSolveCOMZMPForASetOfSteps(lMStartingCOMState,
@@ -828,14 +853,16 @@ namespace PatternGeneratorJRL
}
m_AbsoluteTimeReference = m_CurrentTime-m_Tsingle*2;
- m_AnalyticalZMPCoGTrajectoryX->SetAbsoluteTimeReference(
- m_AbsoluteTimeReference);
- m_AnalyticalZMPCoGTrajectoryY->SetAbsoluteTimeReference(
- m_AbsoluteTimeReference);
- m_FeetTrajectoryGenerator->SetAbsoluteTimeReference(m_AbsoluteTimeReference);
+ m_AnalyticalZMPCoGTrajectoryX->
+ SetAbsoluteTimeReference(m_AbsoluteTimeReference);
+ m_AnalyticalZMPCoGTrajectoryY->
+ SetAbsoluteTimeReference(m_AbsoluteTimeReference);
+ m_FeetTrajectoryGenerator->
+ SetAbsoluteTimeReference(m_AbsoluteTimeReference);
/* Current strategy : add 2 values, and update at each iteration the stack.
- When the limit is reached, and the stack exhausted this method is called again. */
+ When the limit is reached, and the stack exhausted this method is called
+ again. */
FillQueues(m_CurrentTime,
m_CurrentTime+2*m_SamplingPeriod,
FinalZMPPositions,
@@ -872,17 +899,19 @@ namespace PatternGeneratorJRL
}
- void AnalyticalMorisawaCompact::OnLine(double time,
- deque<ZMPPosition> & FinalZMPPositions,
- deque<COMState> & FinalCOMStates,
- deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
- deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions)
+ void AnalyticalMorisawaCompact::
+ OnLine(double time,
+ deque<ZMPPosition> & FinalZMPPositions,
+ deque<COMState> & FinalCOMStates,
+ deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions)
{
unsigned int lIndexInterval;
if (time<m_UpperTimeLimitToUpdateStacks)
{
- if (m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(time,
- lIndexInterval))
+ if (m_AnalyticalZMPCoGTrajectoryX->
+ GetIntervalIndexFromTime(time,
+ lIndexInterval))
{
ZMPPosition aZMPPos;
@@ -895,7 +924,8 @@ namespace PatternGeneratorJRL
double FZmpX=0, FComX=0,FComdX=0;
// Should we filter ?
- bool r = m_FilterXaxisByPC->UpdateOneStep(time,FZmpX, FComX, FComdX);
+ bool r = m_FilterXaxisByPC->
+ UpdateOneStep(time,FZmpX, FComX, FComdX);
if (r)
{
double FZmpY=0, FComY=0,FComdY=0;
@@ -917,19 +947,25 @@ namespace PatternGeneratorJRL
/*! Feed the ZMPPositions. */
double lZMPPosx=0.0,lZMPPosy=0.0;
- m_AnalyticalZMPCoGTrajectoryX->ComputeZMP(time,lZMPPosx,lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryX->
+ ComputeZMP(time,lZMPPosx,lIndexInterval);
aZMPPos.px += lZMPPosx;
- m_AnalyticalZMPCoGTrajectoryY->ComputeZMP(time,lZMPPosy,lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryY->
+ ComputeZMP(time,lZMPPosy,lIndexInterval);
aZMPPos.py += lZMPPosy;
FinalZMPPositions.push_back(aZMPPos);
/*! Feed the COMStates. */
double lCOMPosx=0.0, lCOMPosdx=0.0;
double lCOMPosy=0.0, lCOMPosdy=0.0;
- m_AnalyticalZMPCoGTrajectoryX->ComputeCOM(time,lCOMPosx,lIndexInterval);
- m_AnalyticalZMPCoGTrajectoryX->ComputeCOMSpeed(time,lCOMPosdx,lIndexInterval);
- m_AnalyticalZMPCoGTrajectoryY->ComputeCOM(time,lCOMPosy,lIndexInterval);
- m_AnalyticalZMPCoGTrajectoryY->ComputeCOMSpeed(time,lCOMPosdy,lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryX->
+ ComputeCOM(time,lCOMPosx,lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryX->
+ ComputeCOMSpeed(time,lCOMPosdx,lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryY->
+ ComputeCOM(time,lCOMPosy,lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryY->
+ ComputeCOMSpeed(time,lCOMPosdy,lIndexInterval);
aCOMPos.x[0] += lCOMPosx;
aCOMPos.x[1] += lCOMPosdx;
aCOMPos.y[0] += lCOMPosy;
@@ -942,15 +978,19 @@ namespace PatternGeneratorJRL
/*! Left */
FootAbsolutePosition LeftFootAbsPos;
memset(&LeftFootAbsPos,0,sizeof(LeftFootAbsPos));
- m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(1,time,LeftFootAbsPos,
- lIndexInterval);
+ m_FeetTrajectoryGenerator->
+ ComputeAnAbsoluteFootPosition
+ (1,time,LeftFootAbsPos,
+ lIndexInterval);
FinalLeftFootAbsolutePositions.push_back(LeftFootAbsPos);
/*! Right */
FootAbsolutePosition RightFootAbsPos;
memset(&RightFootAbsPos,0,sizeof(RightFootAbsPos));
- m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(-1,time,
- RightFootAbsPos,lIndexInterval);
+ m_FeetTrajectoryGenerator->
+ ComputeAnAbsoluteFootPosition
+ (-1,time,
+ RightFootAbsPos,lIndexInterval);
FinalRightFootAbsolutePositions.push_back(RightFootAbsPos);
}
@@ -962,106 +1002,21 @@ namespace PatternGeneratorJRL
m_OnLineMode = false;
}
}
- // void AnalyticalMorisawaCompact::OnLine(double time,
- // deque<ZMPPosition> & FinalZMPPositions,
- // deque<COMState> & FinalCOMStates,
- // deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
- // deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions)
- // {
- // unsigned int lIndexInterval;
- // if (time<m_UpperTimeLimitToUpdateStacks)
- // {
- // if (m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(time,lIndexInterval))
- // {
- // ZMPPosition aZMPPos;
- // memset(&aZMPPos,0,sizeof(aZMPPos));
- // COMState aCOMPos;
- // memset(&aCOMPos,0,sizeof(aCOMPos));
- // if (m_FilteringActivate)
- // {
- // double FZmpX=0, FComX=0,FComdX=0;
- // // Should we filter ?
- // bool r = m_FilterXaxisByPC->UpdateOneStep(time,FZmpX, FComX, FComdX);
- // if (r)
- // {
- // double FZmpY=0, FComY=0,FComdY=0;
- // // Yes we should.
- // m_FilterYaxisByPC->UpdateOneStep(time,FZmpY, FComY, FComdY);
- // /*! Feed the ZMPPositions. */
- // aZMPPos.px = FZmpX;
- // aZMPPos.py = FZmpY;
- // /*! Feed the COMStates. */
- // aCOMPos.x[0] = FComX; aCOMPos.x[1] = FComdX;
- // aCOMPos.y[0] = FComY; aCOMPos.y[1] = FComdY;
- // }
- // }
-
- // /*! Feed the ZMP, CoMP, and feet positions, velocities and accelerations. */
- // ctrlLF_ .clear() ;
- // ctrlRF_ .clear() ;
- // ctrlCoM_.clear() ;
- // ctrlZMP_.clear() ;
- // double previewWindowSize = 0.8 ;
- // if(time>=0.8)
- // {
- // cout << "timeOnLine = " << time << endl ;
- // }
- // FillQueues(m_SamplingPeriod, time, time + previewWindowSize,
- // ctrlZMP_, ctrlCoM_, ctrlLF_, ctrlRF_);
-
- // double intPeriod = m_kajitaDynamicFilter->getInterpolationPeriod() ;
- // unsigned int IndexMax = (int)round(previewWindowSize / intPeriod );
- // intCoM_.resize(IndexMax);
- // intLF_ .resize(IndexMax);
- // intRF_ .resize(IndexMax);
- // int inc = (int)round( intPeriod / m_SamplingPeriod) ;
- // for (unsigned int i = 0 , j = 0 ; j < IndexMax ; i = i + inc , ++j )
- // {
- // intCoM_[j] = ctrlCoM_[i] ;
- // intLF_ [j] = ctrlLF_ [i] ;
- // intRF_ [j] = ctrlRF_ [i] ;
- // }
-
- // //m_kajitaDynamicFilter->OnLinefilter(intCoM_,ctrlZMP_,intLF_,intRF_,outputDeltaCoM_);
- // outputDeltaCoM_[0].x[0]=0.0;
- // outputDeltaCoM_[0].x[1]=0.0;
- // outputDeltaCoM_[0].x[2]=0.0;
- // outputDeltaCoM_[0].y[0]=0.0;
- // outputDeltaCoM_[0].y[1]=0.0;
- // outputDeltaCoM_[0].y[2]=0.0;
- // //m_kajitaDynamicFilter->Debug(ctrlCoM_,ctrlLF_,ctrlRF_, intCoM_, ctrlZMP_, intLF_, intRF_, outputDeltaCoM_);
- // ctrlCoM_[0].x[0] += aCOMPos.x[0] + outputDeltaCoM_[0].x[0];
- // ctrlCoM_[0].x[1] += aCOMPos.x[1] + outputDeltaCoM_[0].x[1];
- // ctrlCoM_[0].x[2] += outputDeltaCoM_[0].x[2];
- // ctrlCoM_[0].y[0] += aCOMPos.y[0] + outputDeltaCoM_[0].y[0];
- // ctrlCoM_[0].y[1] += aCOMPos.y[1] + outputDeltaCoM_[0].y[1];
- // ctrlCoM_[0].y[2] += outputDeltaCoM_[0].y[2];
- // FinalCOMStates.push_back(ctrlCoM_[0]);
- // FinalZMPPositions.push_back(ctrlZMP_[0]);
- // FinalLeftFootAbsolutePositions.push_back(ctrlLF_[0]);
- // FinalRightFootAbsolutePositions.push_back(ctrlRF_[0]);
- // }
- // }
- // else
- // {
- // /*! We reached the end of the trajectory generated
- // and no foot steps have been added. */
- // m_OnLineMode = false;
- // }
- // }
-
- void AnalyticalMorisawaCompact::OnLineAddFoot(RelativeFootPosition &
- NewRelativeFootPosition,
- deque<ZMPPosition> & FinalZMPPositions,
- deque<COMState> & FinalCoMPositions,
- deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
- deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions,
- bool )
+
+ void AnalyticalMorisawaCompact::
+ OnLineAddFoot(RelativeFootPosition &
+ NewRelativeFootPosition,
+ deque<ZMPPosition> & FinalZMPPositions,
+ deque<COMState> & FinalCoMPositions,
+ deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions,
+ bool )
{
ODEBUG("****************** Begin OnLineAddFoot **************************");
unsigned int StartingIndexInterval;
- m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(m_CurrentTime,
- StartingIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryX->
+ GetIntervalIndexFromTime(m_CurrentTime,
+ StartingIndexInterval);
unsigned int IndexInterval = (unsigned int)(m_CTIPX.ZMPProfil.size()-1);
@@ -1083,11 +1038,12 @@ namespace PatternGeneratorJRL
deque<FootAbsolutePosition> aQAFP;
- m_FeetTrajectoryGenerator->ComputeAbsoluteStepsFromRelativeSteps(
- m_RelativeFootPositions,
- FinalLeftFootAbsolutePositions[0],
- FinalRightFootAbsolutePositions[0],
- aQAFP);
+ m_FeetTrajectoryGenerator->
+ ComputeAbsoluteStepsFromRelativeSteps
+ (m_RelativeFootPositions,
+ FinalLeftFootAbsolutePositions[0],
+ FinalRightFootAbsolutePositions[0],
+ aQAFP);
vector<FootAbsolutePosition> aNewFootAbsPos;
aNewFootAbsPos.resize(1);
@@ -1134,7 +1090,8 @@ namespace PatternGeneratorJRL
m_Clock3.StartTiming();
/* Current strategy : add 2 values, and update at each iteration the stack.
- When the limit is reached, and the stack exhausted this method is called again. */
+ When the limit is reached, and the stack exhausted this method is called
+ again. */
FillQueues(m_AbsoluteTimeReference,
m_AbsoluteTimeReference+2*m_SamplingPeriod,
FinalZMPPositions,
@@ -1504,13 +1461,14 @@ namespace PatternGeneratorJRL
}
- void AnalyticalMorisawaCompact::TransfertTheCoefficientsToTrajectories(
- AnalyticalZMPCOGTrajectory &aAZCT,
- vector<double> &lCoMZ,
- vector<double> &lZMPZ,
- double &lZMPInit,
- double &lZMPEnd,
- bool )
+ void AnalyticalMorisawaCompact::
+ TransfertTheCoefficientsToTrajectories
+ (AnalyticalZMPCOGTrajectory &aAZCT,
+ vector<double> &lCoMZ,
+ vector<double> &lZMPZ,
+ double &lZMPInit,
+ double &lZMPEnd,
+ bool )
{
vector<double> lV;
vector<double> lW;
@@ -1552,8 +1510,8 @@ namespace PatternGeneratorJRL
}
coeff[1] = coeff[3] * 6.0/(m_Omegaj[m_NumberOfIntervals
-1]*m_Omegaj[m_NumberOfIntervals-1]);
- coeff[0] = lZMPEnd + coeff[2] * 2.0/(m_Omegaj[m_NumberOfIntervals
- -1]*m_Omegaj[m_NumberOfIntervals-1]);
+ coeff[0] = lZMPEnd + coeff[2] * 2.0/
+ (m_Omegaj[m_NumberOfIntervals-1]*m_Omegaj[m_NumberOfIntervals-1]);
aAZCT.GetFromListOfCOGPolynomials(m_NumberOfIntervals-1,aPolynome);
@@ -1572,15 +1530,16 @@ namespace PatternGeneratorJRL
lZMPZ[0]);
// and the last interval
- aAZCT.TransfertOneIntervalCoefficientsFromCOGTrajectoryToZMPOne(
- m_NumberOfIntervals-1,
- lCoMZ[m_NumberOfIntervals-1],
- lZMPZ[m_NumberOfIntervals-1]);
+ aAZCT.TransfertOneIntervalCoefficientsFromCOGTrajectoryToZMPOne
+ (m_NumberOfIntervals-1,
+ lCoMZ[m_NumberOfIntervals-1],
+ lZMPZ[m_NumberOfIntervals-1]);
}
- void AnalyticalMorisawaCompact::ComputeTrajectory(
- CompactTrajectoryInstanceParameters &aCTIP,
- AnalyticalZMPCOGTrajectory &aAZCT)
+ void AnalyticalMorisawaCompact::
+ ComputeTrajectory
+ (CompactTrajectoryInstanceParameters &aCTIP,
+ AnalyticalZMPCOGTrajectory &aAZCT)
{
ComputeW(aCTIP.InitialCoM,
aCTIP.InitialCoMSpeed,
@@ -1588,21 +1547,24 @@ namespace PatternGeneratorJRL
aCTIP.FinalCoMPos,
aAZCT);
ComputePolynomialWeights2();
- TransfertTheCoefficientsToTrajectories(aAZCT,
- aCTIP.CoMZ,
- aCTIP.ZMPZ,
- aCTIP.ZMPProfil[0],
- aCTIP.ZMPProfil[m_NumberOfIntervals-1],
- false);
-
+ TransfertTheCoefficientsToTrajectories
+ (aAZCT,
+ aCTIP.CoMZ,
+ aCTIP.ZMPZ,
+ aCTIP.ZMPProfil[0],
+ aCTIP.ZMPProfil[m_NumberOfIntervals-1],
+ false);
+
}
- int AnalyticalMorisawaCompact::TimeChange(double LocalTime,
- unsigned int IndexStep,
- unsigned int &IndexStartingInterval,
- double &FinalTime,
- double &NewTj)
+ int AnalyticalMorisawaCompact::
+ TimeChange
+ (double LocalTime,
+ unsigned int IndexStep,
+ unsigned int &IndexStartingInterval,
+ double &FinalTime,
+ double &NewTj)
{
// The Index Step can be equal to m_NumberOfIntervals.
@@ -1618,8 +1580,9 @@ namespace PatternGeneratorJRL
FinalTime+=m_DeltaTj[j];
/* Find from which interval we are starting. */
- m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(
- LocalTime+m_AbsoluteTimeReference,IndexStartingInterval);
+ m_AnalyticalZMPCoGTrajectoryX->
+ GetIntervalIndexFromTime
+ (LocalTime+m_AbsoluteTimeReference,IndexStartingInterval);
double reftime=0.0;
for(unsigned int j=0; j<IndexStartingInterval; j++)
@@ -1729,7 +1692,8 @@ namespace PatternGeneratorJRL
long int r;
/* Test if there is enough step in the stack of.
- We have to remove one, because there is still the last foot added.
+ We have to remove one, because there is
+ still the last foot added.
*/
if ((r=(long int)aStepStackHandler->ReturnStackSize()-1-
(long int)NeededSteps)<0)
@@ -1778,17 +1742,19 @@ namespace PatternGeneratorJRL
}
}
- /* Add the relative foot position inside the internal stack as well as
+ /* Add the relative foot position inside the internal
+ stack as well as
the absolute foot position. It also removes the steps
inside the StepStackHandler object taken into account.*/
for(int li=0; li<NeededSteps; li++)
{
m_RelativeFootPositions.push_back(lRelativeFootPositions[li]);
- m_AbsoluteSupportFootPositions.push_back(lAbsoluteSupportFootPositions[li]);
+ m_AbsoluteSupportFootPositions.
+ push_back(lAbsoluteSupportFootPositions[li]);
aStepStackHandler->RemoveFirstStepInTheStack();
}
- /*! Remove the corresponding step from the stack of relative and absolute
- foot positions. */
+ /*! Remove the corresponding step from the stack of
+ relative and absolute foot positions. */
for(unsigned int li=0; li<IndexStartingInterval/2; li++)
{
m_RelativeFootPositions.pop_front();
@@ -1812,9 +1778,10 @@ namespace PatternGeneratorJRL
}
- double AnalyticalMorisawaCompact::TimeCompensationForZMPFluctuation(
- FluctuationParameters &aFP,
- double DeltaTInit)
+ double AnalyticalMorisawaCompact::
+ TimeCompensationForZMPFluctuation
+ (FluctuationParameters &aFP,
+ double DeltaTInit)
{
double r=0.0;
double DeltaTNew=0.0;
@@ -1849,8 +1816,10 @@ namespace PatternGeneratorJRL
else r = rnum/rden;
;
- /* r2 = ( m_Omegaj[0]*(aFP.CoMInit - aFP.ZMPInit) + (aFP.CoMSpeedInit - aFP.ZMPSpeedInit) )/
- (m_Omegaj[0] * ( aFP.CoMNew - aFP.ZMPNew) + (aFP.CoMSpeedNew - aFP.ZMPSpeedNew)); */
+ /* r2 = ( m_Omegaj[0]*(aFP.CoMInit - aFP.ZMPInit)
+ + (aFP.CoMSpeedInit - aFP.ZMPSpeedInit) )/
+ (m_Omegaj[0] * ( aFP.CoMNew - aFP.ZMPNew) +
+ (aFP.CoMSpeedNew - aFP.ZMPSpeedNew)); */
if (r<0.0)
DeltaTNew = DeltaTInit + m_Tsingle*0.5;
@@ -1863,14 +1832,17 @@ namespace PatternGeneratorJRL
}
- void AnalyticalMorisawaCompact::ChangeZMPProfil(vector<unsigned int> &
- IndexStep,
- vector<FootAbsolutePosition> &NewFootAbsPos,
- CompactTrajectoryInstanceParameters &aCTIPX,
- CompactTrajectoryInstanceParameters &aCTIPY)
+ void AnalyticalMorisawaCompact::
+ ChangeZMPProfil
+ (vector<unsigned int> &
+ IndexStep,
+ vector<FootAbsolutePosition> &NewFootAbsPos,
+ CompactTrajectoryInstanceParameters &aCTIPX,
+ CompactTrajectoryInstanceParameters &aCTIPY)
{
- /* Change in the constraints, i.e. modify aCTIPX and aCTIPY appropriatly . */
+ /* Change in the constraints, i.e. modify
+ aCTIPX and aCTIPY appropriatly . */
for(unsigned int i=0; i<IndexStep.size(); i++)
{
unsigned int lIndexStep=IndexStep[i];
@@ -1887,13 +1859,17 @@ namespace PatternGeneratorJRL
if (lIndexStep<aCTIPX.ZMPProfil.size())
{
- aCTIPX.ZMPProfil[lIndexStep] = NewFootAbsPos[lIndexForFootPrintInterval].x;
- aCTIPY.ZMPProfil[lIndexStep] = NewFootAbsPos[lIndexForFootPrintInterval].y;
+ aCTIPX.ZMPProfil[lIndexStep] =
+ NewFootAbsPos[lIndexForFootPrintInterval].x;
+ aCTIPY.ZMPProfil[lIndexStep] =
+ NewFootAbsPos[lIndexForFootPrintInterval].y;
}
if (lIndexStep<aCTIPX.ZMPProfil.size()-1)
{
- aCTIPX.ZMPProfil[lIndexStep+1] = NewFootAbsPos[lIndexForFootPrintInterval].x;
- aCTIPY.ZMPProfil[lIndexStep+1] = NewFootAbsPos[lIndexForFootPrintInterval].y;
+ aCTIPX.ZMPProfil[lIndexStep+1] =
+ NewFootAbsPos[lIndexForFootPrintInterval].x;
+ aCTIPY.ZMPProfil[lIndexStep+1] =
+ NewFootAbsPos[lIndexForFootPrintInterval].y;
}
/* If the end condition has been changed... */
@@ -1905,9 +1881,11 @@ namespace PatternGeneratorJRL
}
}
- int AnalyticalMorisawaCompact::ChangeFootLandingPosition(double t,
- vector<unsigned int> & IndexStep,
- vector<FootAbsolutePosition> & NewFootAbsPos)
+ int AnalyticalMorisawaCompact::
+ ChangeFootLandingPosition
+ (double t,
+ vector<unsigned int> & IndexStep,
+ vector<FootAbsolutePosition> & NewFootAbsPos)
{
int r=0;
r=ChangeFootLandingPosition(t,IndexStep,
@@ -1919,17 +1897,19 @@ namespace PatternGeneratorJRL
return r;
}
- int AnalyticalMorisawaCompact::ChangeFootLandingPosition(double t,
- vector<unsigned int> & IndexStep,
- vector<FootAbsolutePosition> & NewFootAbsPos,
- AnalyticalZMPCOGTrajectory &aAZCTX,
- CompactTrajectoryInstanceParameters &aCTIPX,
- AnalyticalZMPCOGTrajectory &aAZCTY,
- CompactTrajectoryInstanceParameters &aCTIPY,
- bool TemporalShift,
- bool ResetFilters,
- StepStackHandler * aStepStackHandler,
- bool AddingAFootStep)
+ int AnalyticalMorisawaCompact::
+ ChangeFootLandingPosition
+ (double t,
+ vector<unsigned int> & IndexStep,
+ vector<FootAbsolutePosition> & NewFootAbsPos,
+ AnalyticalZMPCOGTrajectory &aAZCTX,
+ CompactTrajectoryInstanceParameters &aCTIPX,
+ AnalyticalZMPCOGTrajectory &aAZCTY,
+ CompactTrajectoryInstanceParameters &aCTIPY,
+ bool TemporalShift,
+ bool ResetFilters,
+ StepStackHandler * aStepStackHandler,
+ bool AddingAFootStep)
{
double LocalTime = t - m_AbsoluteTimeReference;
double FinalTime=0.0;
@@ -1956,17 +1936,20 @@ namespace PatternGeneratorJRL
/* Store the current position and speed of each foot. */
FootAbsolutePosition InitAbsLeftFootPos,InitAbsRightFootPos;
- m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(1,t,
- InitAbsLeftFootPos);
- m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(-1,t,
- InitAbsRightFootPos);
-
+ m_FeetTrajectoryGenerator->
+ ComputeAnAbsoluteFootPosition(1,t,
+ InitAbsLeftFootPos);
+ m_FeetTrajectoryGenerator->
+ ComputeAnAbsoluteFootPosition(-1,t,
+ InitAbsRightFootPos);
+
/* ! This part of the code is not used if we are just trying to add
a foot step. */
// if ((int)IndexStep[0]<m_NumberOfIntervals)
if (!AddingAFootStep)
{
- /* Compute the time of maximal fluctuation for the initial solution along the X axis.*/
+ /* Compute the time of maximal fluctuation
+ for the initial solution along the X axis.*/
aAZCTX.FluctuationMaximal();
aAZCTX.ComputeCOM(t,aFPX.CoMInit,IndexStartingInterval);
@@ -1975,7 +1958,8 @@ namespace PatternGeneratorJRL
aAZCTX.ComputeZMPSpeed(t,aFPX.ZMPSpeedInit);
- /* Compute the time of maximal fluctuation for the initial solution along the Y axis.*/
+ /* Compute the time of maximal fluctuation
+ for the initial solution along the Y axis.*/
aAZCTY.ComputeCOM(t,aFPY.CoMInit,IndexStartingInterval);
aAZCTY.ComputeCOMSpeed(t,aFPY.CoMSpeedInit);
aAZCTY.ComputeZMP(t,aFPY.ZMPInit,IndexStartingInterval);
@@ -1990,8 +1974,10 @@ namespace PatternGeneratorJRL
/* Recompute the coefficient of the ZMP/COG trajectories objects. */
for(int i=1; i<m_NumberOfIntervals-1; i++)
{
- aAZCTX.Building3rdOrderPolynomial(i,aCTIPX.ZMPProfil[i-1],aCTIPX.ZMPProfil[i]);
- aAZCTY.Building3rdOrderPolynomial(i,aCTIPY.ZMPProfil[i-1],aCTIPY.ZMPProfil[i]);
+ aAZCTX.Building3rdOrderPolynomial
+ (i,aCTIPX.ZMPProfil[i-1],aCTIPX.ZMPProfil[i]);
+ aAZCTY.Building3rdOrderPolynomial
+ (i,aCTIPY.ZMPProfil[i-1],aCTIPY.ZMPProfil[i]);
}
/* Compute the trajectories */
@@ -2017,7 +2003,8 @@ namespace PatternGeneratorJRL
else
{
// For a proper initialization of the analytical trajectories
- // through constraint changes, the Fluctuation structure has to be changed
+ // through constraint changes,
+ // the Fluctuation structure has to be changed
// approriatly.
aAZCTX.ComputeCOM(t,aFPX.CoMInit);
aAZCTX.ComputeCOMSpeed(t,aFPX.CoMSpeedInit);
@@ -2068,14 +2055,13 @@ namespace PatternGeneratorJRL
m_FeetTrajectoryGenerator->SetDeltaTj(m_DeltaTj);
/* Modify the feet trajectory */
- ODEBUG("***************** Begin Change Foot Landing Position ********************");
- m_FeetTrajectoryGenerator->ComputeAbsoluteStepsFromRelativeSteps(
- m_RelativeFootPositions,
- InitAbsLeftFootPos,
- InitAbsRightFootPos,
- m_AbsoluteSupportFootPositions);
-
- ODEBUG("***************** End of Change Foot Landing Position ********************");
+ ODEBUG("***** Begin Change Foot Landing Position **************");
+ m_FeetTrajectoryGenerator->ComputeAbsoluteStepsFromRelativeSteps
+ (m_RelativeFootPositions,
+ InitAbsLeftFootPos,InitAbsRightFootPos,
+ m_AbsoluteSupportFootPositions);
+
+ ODEBUG("***** End of Change Foot Landing Position *************");
}
/* Shift the ZMP profil, the initial
@@ -2086,12 +2072,13 @@ namespace PatternGeneratorJRL
IndexStartingInterval,
aStepStackHandler);
- m_FeetTrajectoryGenerator->InitializeFromRelativeSteps(m_RelativeFootPositions,
- InitAbsLeftFootPos,
- InitAbsRightFootPos,
- m_AbsoluteSupportFootPositions,
- false,true);
-
+ m_FeetTrajectoryGenerator->InitializeFromRelativeSteps
+ (m_RelativeFootPositions,
+ InitAbsLeftFootPos,
+ InitAbsRightFootPos,
+ m_AbsoluteSupportFootPositions,
+ false,true);
+
// Initialize and modify the aAZCT trajectories' Tj and Omegaj.
aAZCTX.SetStartingTimeIntervalsAndHeightVariation(m_DeltaTj,m_Omegaj);
aAZCTY.SetStartingTimeIntervalsAndHeightVariation(m_DeltaTj,m_Omegaj);
@@ -2099,8 +2086,10 @@ namespace PatternGeneratorJRL
/* Recompute the coefficient of the ZMP/COG trajectories objects. */
for(int i=1; i<m_NumberOfIntervals-1; i++)
{
- aAZCTX.Building3rdOrderPolynomial(i,aCTIPX.ZMPProfil[i-1],aCTIPX.ZMPProfil[i]);
- aAZCTY.Building3rdOrderPolynomial(i,aCTIPY.ZMPProfil[i-1],aCTIPY.ZMPProfil[i]);
+ aAZCTX.Building3rdOrderPolynomial
+ (i,aCTIPX.ZMPProfil[i-1],aCTIPX.ZMPProfil[i]);
+ aAZCTY.Building3rdOrderPolynomial
+ (i,aCTIPY.ZMPProfil[i-1],aCTIPY.ZMPProfil[i]);
}
aAZCTX.SetAbsoluteTimeReference(t);
@@ -2162,11 +2151,12 @@ namespace PatternGeneratorJRL
return m_PreviewControl;
}
- void AnalyticalMorisawaCompact::FilterOutOrthogonalDirection(
- AnalyticalZMPCOGTrajectory & aAZCT,
- CompactTrajectoryInstanceParameters &aCTIP,
- deque<double> & ZMPTrajectory,
- deque<double> & CoGTrajectory)
+ void AnalyticalMorisawaCompact::
+ FilterOutOrthogonalDirection
+ (AnalyticalZMPCOGTrajectory & aAZCT,
+ CompactTrajectoryInstanceParameters &aCTIP,
+ deque<double> & ZMPTrajectory,
+ deque<double> & CoGTrajectory)
{
/* Initiliazing the Preview Control according to the trajectory
to filter. */
@@ -2217,16 +2207,17 @@ namespace PatternGeneratorJRL
}
- void AnalyticalMorisawaCompact::SetFeetTrajectoryGenerator(
- LeftAndRightFootTrajectoryGenerationMultiple *
- aFeetTrajectoryGenerator)
+ void AnalyticalMorisawaCompact::
+ SetFeetTrajectoryGenerator
+ (LeftAndRightFootTrajectoryGenerationMultiple *
+ aFeetTrajectoryGenerator)
{
m_FeetTrajectoryGenerator = aFeetTrajectoryGenerator;
if (m_BackUpm_FeetTrajectoryGenerator==0)
m_BackUpm_FeetTrajectoryGenerator=
- new LeftAndRightFootTrajectoryGenerationMultiple(
- m_FeetTrajectoryGenerator->getSimplePluginManager(),
- m_FeetTrajectoryGenerator->getFoot());
+ new LeftAndRightFootTrajectoryGenerationMultiple
+ (m_FeetTrajectoryGenerator->getSimplePluginManager(),
+ m_FeetTrajectoryGenerator->getFoot());
}
@@ -2236,13 +2227,15 @@ namespace PatternGeneratorJRL
return m_FeetTrajectoryGenerator;
}
- int AnalyticalMorisawaCompact::OnLineFootChange(double time,
- FootAbsolutePosition &aFootAbsolutePosition,
- deque<ZMPPosition> & ZMPPositions,
- deque<COMState> & CoMPositions,
- deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
- deque<FootAbsolutePosition> &RightFootAbsolutePositions,
- StepStackHandler *aStepStackHandler)
+ int AnalyticalMorisawaCompact::
+ OnLineFootChange
+ (double time,
+ FootAbsolutePosition &aFootAbsolutePosition,
+ deque<ZMPPosition> & ZMPPositions,
+ deque<COMState> & CoMPositions,
+ deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> &RightFootAbsolutePositions,
+ StepStackHandler *aStepStackHandler)
{
deque<FootAbsolutePosition> NewFeetAbsolutePosition;
NewFeetAbsolutePosition.push_back(aFootAbsolutePosition);
@@ -2256,16 +2249,18 @@ namespace PatternGeneratorJRL
}
- int AnalyticalMorisawaCompact::OnLineFootChanges(double time,
- deque<FootAbsolutePosition> &aFootAbsolutePosition,
- deque<ZMPPosition> & ZMPPositions,
- deque<COMState> & CoMPositions,
- deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
- deque<FootAbsolutePosition> &RightFootAbsolutePositions,
- StepStackHandler *aStepStackHandler)
+ int AnalyticalMorisawaCompact::
+ OnLineFootChanges
+ (double time,
+ deque<FootAbsolutePosition> &aFootAbsolutePosition,
+ deque<ZMPPosition> & ZMPPositions,
+ deque<COMState> & CoMPositions,
+ deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> &RightFootAbsolutePositions,
+ StepStackHandler *aStepStackHandler)
{
-
- ODEBUG("****************** Begin OnLineFootChange **************************");
+
+ ODEBUG("***** Begin OnLineFootChange *****");
int IndexInterval=-1;
/* Trying to find the index interval where the change should operate. */
@@ -2334,7 +2329,8 @@ namespace PatternGeneratorJRL
vector<unsigned int> IndexIntervals;
vector<FootAbsolutePosition> NewRelFootAbsolutePositions;
- /*! Recompute relative or absolute foot-steps positions depending on the mode. */
+ /*! Recompute relative or absolute foot-steps positions
+ depending on the mode. */
if (m_OnLineChangeStepMode==ABSOLUTE_FRAME)
{
IndexIntervals.resize(1);
@@ -2344,11 +2340,14 @@ namespace PatternGeneratorJRL
m_AbsoluteSupportFootPositions[i+lChangedIntervalFoot] =
aFootAbsolutePosition[i];
- /* From the new foot landing position computes the new relative set of positions. */
- m_FeetTrajectoryGenerator->ChangeRelStepsFromAbsSteps(m_RelativeFootPositions,
- m_AbsoluteCurrentSupportFootPosition,
- m_AbsoluteSupportFootPositions,
- lChangedIntervalFoot);
+ /* From the new foot landing position computes the new relative set of
+ positions. */
+ m_FeetTrajectoryGenerator->
+ ChangeRelStepsFromAbsSteps
+ (m_RelativeFootPositions,
+ m_AbsoluteCurrentSupportFootPosition,
+ m_AbsoluteSupportFootPositions,
+ lChangedIntervalFoot);
NewRelFootAbsolutePositions.resize(aFootAbsolutePosition.size());
for(unsigned int i=0; i<aFootAbsolutePosition.size(); i++)
@@ -2380,12 +2379,13 @@ namespace PatternGeneratorJRL
}
deque<FootAbsolutePosition> lAbsoluteSupportFootPositions;
- m_FeetTrajectoryGenerator->ComputeAbsoluteStepsFromRelativeSteps(
- m_RelativeFootPositions,
- LeftFootAbsolutePositions[0],
- RightFootAbsolutePositions[0],
- lAbsoluteSupportFootPositions);
-
+ m_FeetTrajectoryGenerator->
+ ComputeAbsoluteStepsFromRelativeSteps
+ (m_RelativeFootPositions,
+ LeftFootAbsolutePositions[0],
+ RightFootAbsolutePositions[0],
+ lAbsoluteSupportFootPositions);
+
for(unsigned int j=0,k=lChangedIntervalFoot;
j<NewRelFootAbsolutePositions.size(); j++,k++)
{
@@ -2394,7 +2394,8 @@ namespace PatternGeneratorJRL
for(unsigned int k=lChangedIntervalFoot;
k<m_AbsoluteSupportFootPositions.size(); k++)
{
- m_AbsoluteSupportFootPositions[k] = lAbsoluteSupportFootPositions[k];
+ m_AbsoluteSupportFootPositions[k] =
+ lAbsoluteSupportFootPositions[k];
}
}
@@ -2453,12 +2454,14 @@ namespace PatternGeneratorJRL
m_AbsoluteTimeReference+2*m_SamplingPeriod,
ZMPPositions,CoMPositions, LeftFootAbsolutePositions,
RightFootAbsolutePositions);
- ODEBUG("****************** End OnLineFootChange **************************");
+ ODEBUG("***** End OnLineFootChange *****");
return 0;
}
- /*! \brief Return the time at which it is optimal to regenerate a step in online mode.
- This time is given in the size of the Left Foot Positions queue under which such
+ /*! \brief Return the time at which it is optimal to
+ regenerate a step in online mode.
+ This time is given in the size of the Left Foot Positions
+ queue under which such
new step has to be generate.
*/
int AnalyticalMorisawaCompact::ReturnOptimalTimeToRegenerateAStep()
@@ -2467,11 +2470,13 @@ namespace PatternGeneratorJRL
return r;
}
- void AnalyticalMorisawaCompact::EndPhaseOfTheWalking(deque<ZMPPosition>
- &FinalZMPPositions,
- deque<COMState> &FinalCoMPositions,
- deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
- deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions)
+ void AnalyticalMorisawaCompact::
+ EndPhaseOfTheWalking
+ (deque<ZMPPosition>
+ &FinalZMPPositions,
+ deque<COMState> &FinalCoMPositions,
+ deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions)
{
m_OnLineMode = true;
@@ -2482,10 +2487,11 @@ namespace PatternGeneratorJRL
/* Update the relative and absolute foot positions. */
m_RelativeFootPositions.pop_front();
- ODEBUG("****************** Begin EndPhaseOfTheWalking **************************");
+ ODEBUG("***** Begin EndPhaseOfTheWalking *****");
// Strategy for the final CoM pos: middle of the segment
// between the two final steps, in order to be statically stable.
- unsigned int lindex = (unsigned int)(m_AbsoluteSupportFootPositions.size()-1);
+ unsigned int lindex = (unsigned int)
+ (m_AbsoluteSupportFootPositions.size()-1);
vector<double> * lZMPX=0;
double FinalCoMPosX=0.6;
@@ -2511,8 +2517,9 @@ namespace PatternGeneratorJRL
/*! Build 3rd order polynomials. */
for(int i=1; i<NbOfIntervals-1; i++)
{
- m_AnalyticalZMPCoGTrajectoryX->Building3rdOrderPolynomial(i,(*lZMPX)[i-1],
- (*lZMPX)[i]);
+ m_AnalyticalZMPCoGTrajectoryX->
+ Building3rdOrderPolynomial(i,(*lZMPX)[i-1],
+ (*lZMPX)[i]);
}
/*! Compute trajectory for CoM along X axis. */
@@ -2536,15 +2543,17 @@ namespace PatternGeneratorJRL
/*! Build 3rd order polynomials. */
for(int i=1; i<NbOfIntervals-1; i++)
{
- m_AnalyticalZMPCoGTrajectoryY->Building3rdOrderPolynomial(i,(*lZMPY)[i-1],
- (*lZMPY)[i]);
+ m_AnalyticalZMPCoGTrajectoryY->
+ Building3rdOrderPolynomial(i,(*lZMPY)[i-1],
+ (*lZMPY)[i]);
}
/*! Compute the analytical trajectory*/
ComputeTrajectory(m_CTIPY,*m_AnalyticalZMPCoGTrajectoryY);
/* Specify when a new step should be asked for. */
- m_UpperTimeLimitToUpdateStacks = m_AbsoluteTimeReference + m_PreviewControlTime;
+ m_UpperTimeLimitToUpdateStacks = m_AbsoluteTimeReference +
+ m_PreviewControlTime;
/* Put two positions from the new polynomials in the queues. */
FillQueues(m_CurrentTime,
@@ -2555,7 +2564,7 @@ namespace PatternGeneratorJRL
FinalRightFootAbsolutePositions);
m_EndPhase=true;
- ODEBUG("****************** End of EndPhaseOfTheWalking **************************");
+ ODEBUG("**** End of EndPhaseOfTheWalking *******");
}
void AnalyticalMorisawaCompact::RegisterMethods()
@@ -2625,31 +2634,37 @@ namespace PatternGeneratorJRL
m_FeetTrajectoryGenerator->SetAbsoluteTimeReference(x);
}
- void AnalyticalMorisawaCompact::FillQueues(double samplingPeriod,
- double StartingTime,
- double EndTime,
- deque<ZMPPosition> & FinalZMPPositions,
- deque<COMState> & FinalCoMPositions,
- deque<FootAbsolutePosition> & FinalLeftFootAbsolutePositions,
- deque<FootAbsolutePosition> & FinalRightFootAbsolutePositions)
+ void AnalyticalMorisawaCompact::
+ FillQueues
+ (double samplingPeriod,
+ double StartingTime,
+ double EndTime,
+ deque<ZMPPosition> & FinalZMPPositions,
+ deque<COMState> & FinalCoMPositions,
+ deque<FootAbsolutePosition> & FinalLeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> & FinalRightFootAbsolutePositions)
{
unsigned int lIndexInterval,lPrevIndexInterval;
- m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(m_AbsoluteTimeReference,
- lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryX->
+ GetIntervalIndexFromTime(m_AbsoluteTimeReference,
+ lIndexInterval);
lPrevIndexInterval = lIndexInterval;
/*! Fill in the stacks: minimal strategy only 1 reference. */
for(double t=StartingTime; t<=EndTime; t+= samplingPeriod)
{
- m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(t,lIndexInterval,
- lPrevIndexInterval);
-
+ m_AnalyticalZMPCoGTrajectoryX->
+ GetIntervalIndexFromTime(t,lIndexInterval,
+ lPrevIndexInterval);
+
/*! Feed the ZMPPositions. */
ZMPPosition aZMPPos;
- if (!m_AnalyticalZMPCoGTrajectoryX->ComputeZMP(t,aZMPPos.px,lIndexInterval))
+ if (!m_AnalyticalZMPCoGTrajectoryX->
+ ComputeZMP(t,aZMPPos.px,lIndexInterval))
LTHROW("Unable to compute ZMP along X-Axis in EndPhaseOfWalking");
- if (!m_AnalyticalZMPCoGTrajectoryY->ComputeZMP(t,aZMPPos.py,lIndexInterval))
+ if (!m_AnalyticalZMPCoGTrajectoryY->
+ ComputeZMP(t,aZMPPos.py,lIndexInterval))
LTHROW("Unable to compute ZMP along Y-Axis in EndPhaseOfWalking");
ComputeZMPz(t,aZMPPos,lIndexInterval);
@@ -2661,8 +2676,10 @@ namespace PatternGeneratorJRL
/*! Left */
FootAbsolutePosition LeftFootAbsPos;
memset(&LeftFootAbsPos,0,sizeof(LeftFootAbsPos));
- if (!m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(1,t,
- LeftFootAbsPos,lIndexInterval))
+ if (!m_FeetTrajectoryGenerator->
+ ComputeAnAbsoluteFootPosition
+ (1,t,
+ LeftFootAbsPos,lIndexInterval))
{
LTHROW("Unable to compute left foot position in EndPhaseOfWalking");
}
@@ -2671,29 +2688,36 @@ namespace PatternGeneratorJRL
/*! Right */
FootAbsolutePosition RightFootAbsPos;
memset(&RightFootAbsPos,0,sizeof(RightFootAbsPos));
- if (!m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(-1,t,
- RightFootAbsPos,lIndexInterval))
+ if (!m_FeetTrajectoryGenerator->
+ ComputeAnAbsoluteFootPosition
+ (-1,t,
+ RightFootAbsPos,lIndexInterval))
{
- LTHROW("Unable to compute right foot position in EndPhaseOfWalking");
+ LTHROW("Unable to compute right foot"
+ " position in EndPhaseOfWalking");
}
FinalRightFootAbsolutePositions.push_back(RightFootAbsPos);
/*! Feed the COMStates. */
COMState aCOMPos;
memset(&aCOMPos,0,sizeof(aCOMPos));
- if (!m_AnalyticalZMPCoGTrajectoryX->ComputeCOM(t,aCOMPos.x[0],lIndexInterval))
+ if (!m_AnalyticalZMPCoGTrajectoryX->
+ ComputeCOM(t,aCOMPos.x[0],lIndexInterval))
{
LTHROW("COM out of bound along X axis.");
}
- m_AnalyticalZMPCoGTrajectoryX->ComputeCOMSpeed(t,aCOMPos.x[1],lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryX->
+ ComputeCOMSpeed(t,aCOMPos.x[1],lIndexInterval);
m_AnalyticalZMPCoGTrajectoryX->ComputeCOMAcceleration(t,aCOMPos.x[2],
lIndexInterval);
- if (!m_AnalyticalZMPCoGTrajectoryY->ComputeCOM(t,aCOMPos.y[0],lIndexInterval))
+ if (!m_AnalyticalZMPCoGTrajectoryY->
+ ComputeCOM(t,aCOMPos.y[0],lIndexInterval))
{
LTHROW("COM out of bound along Y axis.");
}
- m_AnalyticalZMPCoGTrajectoryY->ComputeCOMSpeed(t,aCOMPos.y[1],lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryY->
+ ComputeCOMSpeed(t,aCOMPos.y[1],lIndexInterval);
m_AnalyticalZMPCoGTrajectoryY->ComputeCOMAcceleration(t,aCOMPos.y[2],
lIndexInterval);
@@ -2706,9 +2730,12 @@ namespace PatternGeneratorJRL
FinalCoMPositions.push_back(aCOMPos);
ODEBUG4(aZMPPos.px << " " << aZMPPos.py << " " <<
- aCOMPos.x[0] << " " << aCOMPos.y[0] << " " << aCOMPos.z[0] <<" "<<
- LeftFootAbsPos.x << " " << LeftFootAbsPos.y << " " << LeftFootAbsPos.z << " " <<
- RightFootAbsPos.x << " " << RightFootAbsPos.y << " " << RightFootAbsPos.z << " "
+ aCOMPos.x[0] << " " << aCOMPos.y[0] << " " << aCOMPos.z[0]
+ <<" "<<
+ LeftFootAbsPos.x << " " << LeftFootAbsPos.y << " " <<
+ LeftFootAbsPos.z << " " <<
+ RightFootAbsPos.x << " " << RightFootAbsPos.y << " " <<
+ RightFootAbsPos.z << " "
<<
samplingPeriod,"Test.dat");
}
@@ -2718,17 +2745,6 @@ namespace PatternGeneratorJRL
FootAbsolutePosition & LeftFoot,
FootAbsolutePosition & )
{
- // vector<double> barriere (500,0.0) ;
- // COMState & c = FinalCoMPositions.front() ;
- // FootAbsolutePosition & lf = FinalLeftFootAbsolutePositions .front() ;
- // FootAbsolutePosition & rf = FinalRightFootAbsolutePositions.front() ;
- // double lb = 0.7;
- // double ub = 0.9;
- // barriere[0] =
- // exp( sqrt((c.x[0]-lf.x)*(c.x[0]-lf.x)+(c.y[0]-lf.y)*(c.y[0]-lf.y)+(c.z[0]-lf.z)*(c.z[0]-lf.z)) - lb)
- // + exp(-sqrt((c.x[0]-lf.x)*(c.x[0]-lf.x)+(c.y[0]-lf.y)*(c.y[0]-lf.y)+(c.z[0]-lf.z)*(c.z[0]-lf.z)) + ub)
- // + exp( sqrt((c.x[0]-rf.x)*(c.x[0]-rf.x)+(c.y[0]-rf.y)*(c.y[0]-rf.y)+(c.z[0]-rf.z)*(c.z[0]-rf.z)) - lb)
- // + exp(-sqrt((c.x[0]-rf.x)*(c.x[0]-rf.x)+(c.y[0]-rf.y)*(c.y[0]-rf.y)+(c.z[0]-rf.z)*(c.z[0]-rf.z)) + ub);
CoM.z[0] = ( (LeftFoot.z + 0.7) + (LeftFoot.z + 0.85) + (LeftFoot.z + 0.7) +
(LeftFoot.z + 0.85) ) * 0.25 ;
@@ -2737,47 +2753,14 @@ namespace PatternGeneratorJRL
CoM.z[0] = ( (LeftFoot.ddz + 0.7) + (LeftFoot.ddz + 0.85) +
(LeftFoot.ddz + 0.7) + (LeftFoot.ddz + 0.85) ) * 0.25 ;
- //
- // x = expt(sqrt(- (4 l - 8 h l + 4 h + 8) u
- // 3 2 2 3 3
- // - (- 8 l + 8 h l + (8 h - 12) l - 8 h - 20 h) u
- // 4 3 2 2 3 4 2 2
- // - (4 l + 8 h l + (8 - 24 h ) l + (8 h + 20 h) l + 4 h + 20 h + 13) u
- // 4 2 3 3 2 4 2
- // - (- 8 h l + (8 h - 12) l + (8 h + 20 h) l + (- 8 h - 40 h - 22) l
- // 2 4 3 3 4 2 2
- // - 4 h) u - (4 h + 8) l - (- 8 h - 20 h) l - (4 h + 20 h + 13) l + 4 h l
- // 2 3/2 3 2 2
- // - 4 h - 16)/(4 3 ) + (4 u + h (- 6 u + 24 l u - 6 l + 18)
- // 2 2 2 3 3 1
- // + l (- 6 u - 9) - 6 l u - 9 u + h (- 6 u - 6 l) + 4 l + 4 h )/108, -)
- // 3
- // 2 2 2
- // + (u - l u + h (- u - l) + l + h + 3)
- // 2 2 4 3 2 2 3 3
- // /(9 expt(sqrt(- (4 l - 8 h l + 4 h + 8) u - (- 8 l + 8 h l + (8 h - 12) l - 8 h - 20 h) u
- // 4 3 2 2 3 4 2 2
- // - (4 l + 8 h l + (8 - 24 h ) l + (8 h + 20 h) l + 4 h + 20 h + 13) u
- // 4 2 3 3 2 4 2
- // - (- 8 h l + (8 h - 12) l + (8 h + 20 h) l + (- 8 h - 40 h - 22) l
- // 2 4 3 3 4 2 2
- // - 4 h) u - (4 h + 8) l - (- 8 h - 20 h) l - (4 h + 20 h + 13) l + 4 h l
- // 2 3/2 3 2 2
- // - 4 h - 16)/(4 3 ) + (4 u + h (- 6 u + 24 l u - 6 l + 18)
- // 2 2 2 3 3
- // + l (- 6 u - 9) - 6 l u - 9 u + h (- 6 u - 6 l) + 4 l + 4 h )/108,
- // 1 - u - l - h
- // -)) - -----------]
- // 3 3
- //double l =
- //double u =
-
-
- //CoM.z[0];
}
- void AnalyticalMorisawaCompact::ComputeCoMz(double t,
- unsigned int lIndexInterval, COMState &CoM, deque<COMState> & FinalCoMPositions)
+ void AnalyticalMorisawaCompact::
+ ComputeCoMz
+ (double t,
+ unsigned int lIndexInterval,
+ COMState &CoM,
+ deque<COMState> & FinalCoMPositions)
{
double* CoMz = CoM.z ;
double moving_time = m_RelativeFootPositions[0].SStime +
@@ -2792,14 +2775,16 @@ namespace PatternGeneratorJRL
LTHROW("No foot");
Eigen::Vector3d corrZ ;
corrZ = aFoot->anklePosition ;
- corrZ(2) = 0; //foot height no more equal to ankle height; TODO : remove corrZ
+ corrZ(2) = 0; //foot height no more equal to ankle height; TODO :
+ // remove corrZ
// after the final step we keep the same position for a while
if( Index >= m_AbsoluteSupportFootPositions.size() )
{
if(FinalCoMPositions.size()==0)
{
- CoMz[0] = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index].z -
+ CoMz[0] = m_InitialPoseCoMHeight +
+ m_AbsoluteSupportFootPositions[Index].z -
corrZ(2);
CoMz[1] = 0.0 ;
CoMz[2] = 0.0 ;
@@ -2809,10 +2794,12 @@ namespace PatternGeneratorJRL
COMState LastCoM = FinalCoMPositions.back();
double higherPoseCoMz = m_InitialPoseCoMHeight +
m_AbsoluteSupportFootPositions.back().z - corrZ(2);
- double ft = m_RelativeFootPositions.back().SStime-(t-Index*moving_time) ;
+ double ft = m_RelativeFootPositions.back().SStime-
+ (t-Index*moving_time) ;
- m_CoMbsplinesZ->SetParameters(ft,LastCoM.z[0],higherPoseCoMz,
- vector<double>(), vector<double>(), LastCoM.z[1], LastCoM.z[2]) ;
+ m_CoMbsplinesZ->SetParameters
+ (ft,LastCoM.z[0],higherPoseCoMz,
+ vector<double>(), vector<double>(), LastCoM.z[1], LastCoM.z[2]) ;
m_CoMbsplinesZ->Compute(m_SamplingPeriod,CoMz[0],CoMz[1],CoMz[2]) ;
return ;
}
@@ -2855,14 +2842,16 @@ namespace PatternGeneratorJRL
interpolationTime = t-Index*moving_time ;
FinalPos = initCoMheight ;
FinalTime = SStime - interpolationTime ;
- if(sx*sx+sy*sy > 0.22*0.22 && sz*sz+sz*sz < 0.00001 && sx*sx+sx*sx > 0.00001)
+ if(sx*sx+sy*sy > 0.22*0.22 && sz*sz+sz*sz < 0.00001 && sx*sx+sx*sx >
+ 0.00001)
{
FinalPos = lowerCoMheight ;
}
InitPos = LastCoM.z[0];
InitSpeed = LastCoM.z[1];
InitAcc = LastCoM.z[2];
- m_CoMbsplinesZ->SetParameters(FinalTime,InitPos,FinalPos,ToMP,MP,InitSpeed,
+ m_CoMbsplinesZ->SetParameters(FinalTime,InitPos,FinalPos,ToMP,MP,
+ InitSpeed,
InitAcc) ;
m_CoMbsplinesZ->Compute(m_SamplingPeriod,CoMz[0],CoMz[1],CoMz[2]) ;
return ;
@@ -2885,14 +2874,16 @@ namespace PatternGeneratorJRL
// climbing
// put first leg on the stairs with decrease of CoM //up// of stair height
- // the CoM line will decrease between an //upLeft to upRight// interval of SStime.
+ // the CoM line will decrease between an //upLeft to upRight
+ // interval of SStime.
// the CoM line will go up between an //upLeft1 to upRight1//
// interval of SStime while 2nd leg moving up on the stairs.
if (absFootz_0 > absFootz_1) // first leg
{
deltaZ = absFootz_0 - absFootz_1;
if (Index>1)
- InitPos = m_InitialPoseCoMHeight + absFootz_2 - up*(absFootz_1 - absFootz_2) ;
+ InitPos = m_InitialPoseCoMHeight + absFootz_2 -
+ up*(absFootz_1 - absFootz_2) ;
else // Special case: starting the motion.
InitPos = m_InitialPoseCoMHeight ;
@@ -2915,13 +2906,15 @@ namespace PatternGeneratorJRL
}
// going down
// the CoM line will decrease an //1+down// stair height
- // between an //downLeft to downRight// interval of SStime while moving first leg down
+ // between an //downLeft to downRight// interval of SStime while
+ // moving first leg down
// put the 2nd leg down while standing up the CoM.
else if (absFootz_0 < absFootz_1 )
{
deltaZ = absFootz_1 - absFootz_0;
if (Index>1)
- InitPos = m_InitialPoseCoMHeight + absFootz_1 - down*(absFootz_2 - absFootz_1) ;
+ InitPos = m_InitialPoseCoMHeight + absFootz_1 -
+ down*(absFootz_2 - absFootz_1) ;
else // Special case: starting the motion.
InitPos = m_InitialPoseCoMHeight ;
@@ -2951,7 +2944,8 @@ namespace PatternGeneratorJRL
InitPos = initCoMheight ;
FinalPos = initCoMheight ;
- if(sx*sx+sy*sy > 0.22*0.22 && sz*sz+sz*sz < 0.00001 && sx*sx+sx*sx > 0.00001)
+ if(sx*sx+sy*sy > 0.22*0.22 && sz*sz+sz*sz < 0.00001 &&
+ sx*sx+sx*sx > 0.00001)
{
if(LastCoM.z[0] >= lowerCoMheight + 0.00001
|| LastCoM.z[0] <= lowerCoMheight - 0.00001)
@@ -3032,23 +3026,28 @@ namespace PatternGeneratorJRL
}
else
{
- double absFootz_0 = m_AbsoluteSupportFootPositions[stepNumber].z - corrZ(2) ;
- double absFootz_1 = m_AbsoluteSupportFootPositions[stepNumber-1].z - corrZ(2) ;
- m_ZMPpolynomeZ->SetParametersWithInitPosInitSpeed(
- m_RelativeFootPositions[0].DStime,
- absFootz_0,absFootz_1,0.0);
+ double absFootz_0 = m_AbsoluteSupportFootPositions[stepNumber].z
+ - corrZ(2) ;
+ double absFootz_1 = m_AbsoluteSupportFootPositions[stepNumber-1].z
+ - corrZ(2) ;
+ m_ZMPpolynomeZ->
+ SetParametersWithInitPosInitSpeed
+ (m_RelativeFootPositions[0].DStime,
+ absFootz_0,absFootz_1,0.0);
ZMPz.pz = m_ZMPpolynomeZ->Compute(t-stepNumber*moving_time
-m_RelativeFootPositions[0].SStime);
}
return ;
}
- void AnalyticalMorisawaCompact::FillQueues(double StartingTime,
- double EndTime,
- deque<ZMPPosition> & FinalZMPPositions,
- deque<COMState> & FinalCoMPositions,
- deque<FootAbsolutePosition> & FinalLeftFootAbsolutePositions,
- deque<FootAbsolutePosition> & FinalRightFootAbsolutePositions)
+ void AnalyticalMorisawaCompact::
+ FillQueues
+ (double StartingTime,
+ double EndTime,
+ deque<ZMPPosition> & FinalZMPPositions,
+ deque<COMState> & FinalCoMPositions,
+ deque<FootAbsolutePosition> & FinalLeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> & FinalRightFootAbsolutePositions)
{
FillQueues(m_SamplingPeriod, StartingTime, EndTime, FinalZMPPositions,
FinalCoMPositions, FinalLeftFootAbsolutePositions,
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index f6f0f1dc0aacf32191b294fa87d27873bc372905..09e609dcf3f51b7a98e779fb047a86b630be7119 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -21,8 +21,8 @@ DynamicFilter::DynamicFilter(
PR_ = aPR ;
- comAndFootRealization_ = new ComAndFootRealizationByGeometry((
- PatternGeneratorInterfacePrivate*) SPM );
+ comAndFootRealization_ = new
+ ComAndFootRealizationByGeometry((PatternGeneratorInterfacePrivate*) SPM );
comAndFootRealization_->setPinocchioRobot(PR_);
comAndFootRealization_->SetHeightOfTheCoM(CoMHeight_);
comAndFootRealization_->ShiftFoot(true);
@@ -42,11 +42,10 @@ DynamicFilter::DynamicFilter(
deltax_.resize(3,1);
deltay_.resize(3,1);
- comAndFootRealization_->SetPreviousConfigurationStage0(
- PR_->currentRPYConfiguration());
- comAndFootRealization_->SetPreviousVelocityStage0(
- PR_->currentRPYVelocity());
-
+ comAndFootRealization_->
+ SetPreviousConfigurationStage0(PR_->currentRPYConfiguration());
+ comAndFootRealization_->
+ SetPreviousVelocityStage0(PR_->currentRPYVelocity());
sxzmp_.clear();
syzmp_.clear();
@@ -139,13 +138,14 @@ void DynamicFilter::setRobotUpperPart(const Eigen::VectorXd & configuration,
/// \brief Initialise all objects, to be called just after the constructor
/// the filter takes a subsampled previewWindow,
/// interpolate it and use the kajita preview control on it
-void DynamicFilter::init(
- double controlPeriod,
- double interpolationPeriod,
- double controlWindowSize,
- double previewWindowSize,
- double kajitaPCwindowSize,
- COMState inputCoMState)
+void DynamicFilter::
+init
+(double controlPeriod,
+ double interpolationPeriod,
+ double controlWindowSize,
+ double previewWindowSize,
+ double kajitaPCwindowSize,
+ COMState inputCoMState)
{
controlPeriod_ = controlPeriod ;
interpolationPeriod_ = interpolationPeriod ;
@@ -227,15 +227,16 @@ void DynamicFilter::init(
return ;
}
-int DynamicFilter::OffLinefilter(
- const deque<COMState> &inputCOMTraj_deq_,
- const deque<ZMPPosition> &inputZMPTraj_deq_,
- const deque<FootAbsolutePosition> &inputLeftFootTraj_deq_,
- const deque<FootAbsolutePosition> &inputRightFootTraj_deq_,
- const vector< Eigen::VectorXd > & UpperPart_q,
- const vector< Eigen::VectorXd > & UpperPart_dq,
- const vector< Eigen::VectorXd > & UpperPart_ddq,
- deque<COMState> & outputDeltaCOMTraj_deq)
+int DynamicFilter::
+OffLinefilter
+( const deque<COMState> &inputCOMTraj_deq_,
+ const deque<ZMPPosition> &inputZMPTraj_deq_,
+ const deque<FootAbsolutePosition> &inputLeftFootTraj_deq_,
+ const deque<FootAbsolutePosition> &inputRightFootTraj_deq_,
+ const vector< Eigen::VectorXd > & UpperPart_q,
+ const vector< Eigen::VectorXd > & UpperPart_dq,
+ const vector< Eigen::VectorXd > & UpperPart_ddq,
+ deque<COMState> & outputDeltaCOMTraj_deq)
{
unsigned int N = (unsigned int)inputCOMTraj_deq_.size() ;
deltaZMP_deq_.resize(N);
@@ -269,12 +270,13 @@ int DynamicFilter::OffLinefilter(
return 0;
}
-int DynamicFilter::OnLinefilter(
- const deque<COMState> & inputCOMTraj_deq_,
- const deque<ZMPPosition> & inputZMPTraj_deq_,
- const deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
- const deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
- deque<COMState> & outputDeltaCOMTraj_deq_)
+int DynamicFilter::
+OnLinefilter
+(const deque<COMState> & inputCOMTraj_deq_,
+ const deque<ZMPPosition> & inputZMPTraj_deq_,
+ const deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+ const deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
+ deque<COMState> & outputDeltaCOMTraj_deq_)
{
unsigned int N = (unsigned int)inputRightFootTraj_deq_.size() ;
int inc = (int)round(interpolationPeriod_/controlPeriod_) ;
@@ -291,17 +293,10 @@ int DynamicFilter::OnLinefilter(
stage1_,
//currentIteration
i);
- // cout << "DF : " ;
- // cout << ZMPMBAcceleration_(3) << " "
- // << ZMPMBAcceleration_(4) << " "
- // << ZMPMBAcceleration_(5) << endl ;
}
ZMPMB_vec_[0][0]=inputZMPTraj_deq_[0].px;
ZMPMB_vec_[0][1]=inputZMPTraj_deq_[0].py;
ZMPMB_vec_[0][2]=0.0;
- // ZMPMB_vec_[1][0]=inputZMPTraj_deq_[0].px;
- // ZMPMB_vec_[1][1]=inputZMPTraj_deq_[0].py;
- // ZMPMB_vec_[1][2]=0.0;
if(false)
{
zmpmb_i_.resize( N1 ) ;
@@ -340,8 +335,10 @@ int DynamicFilter::OnLinefilter(
dZMPMB_vec[N-1][1] = (ZMPMB_vec_[N-1][1]-ZMPMB_vec_[N-2][1])/inc;
for(unsigned i=1 ; i<N-2 ; ++i)
{
- dZMPMB_vec[i][0] = (ZMPMB_vec_[i+1][0]-ZMPMB_vec_[i-1][0])/(2*inc);
- dZMPMB_vec[i][1] = (ZMPMB_vec_[i+1][1]-ZMPMB_vec_[i-1][1])/(2*inc);
+ dZMPMB_vec[i][0] =
+ (ZMPMB_vec_[i+1][0]-ZMPMB_vec_[i-1][0])/(2*inc);
+ dZMPMB_vec[i][1] =
+ (ZMPMB_vec_[i+1][1]-ZMPMB_vec_[i-1][1])/(2*inc);
}
for(unsigned i=0 ; i<N-1 ; ++i)
{
@@ -383,11 +380,12 @@ int DynamicFilter::OnLinefilter(
}
// #############################
-int DynamicFilter::zmpmb(
- Eigen::VectorXd& configuration,
- Eigen::VectorXd& velocity,
- Eigen::VectorXd& acceleration,
- Eigen::Vector3d & zmpmb)
+int DynamicFilter::
+zmpmb
+(Eigen::VectorXd& configuration,
+ Eigen::VectorXd& velocity,
+ Eigen::VectorXd& acceleration,
+ Eigen::Vector3d & zmpmb)
{
PR_->computeInverseDynamics(configuration,velocity,acceleration);
PR_->zeroMomentumPoint(zmpmb);
@@ -395,16 +393,17 @@ int DynamicFilter::zmpmb(
}
//##################################
-void DynamicFilter::InverseKinematics(
- const COMState & inputCoMState,
- const FootAbsolutePosition & inputLeftFoot,
- const FootAbsolutePosition & inputRightFoot,
- Eigen::VectorXd& configuration,
- Eigen::VectorXd& velocity,
- Eigen::VectorXd& acceleration,
- double samplingPeriod,
- int stage,
- int iteration)
+void DynamicFilter::
+InverseKinematics
+(const COMState & inputCoMState,
+ const FootAbsolutePosition & inputLeftFoot,
+ const FootAbsolutePosition & inputRightFoot,
+ Eigen::VectorXd& configuration,
+ Eigen::VectorXd& velocity,
+ Eigen::VectorXd& acceleration,
+ double samplingPeriod,
+ int stage,
+ int iteration)
{
// lower body !!!!! the angular quantities are set in degree !!!!!!
@@ -445,12 +444,13 @@ void DynamicFilter::InverseKinematics(
<< " aCoMAcc_ :" << aCoMAcc_ << std::endl;
*/
comAndFootRealization_->setSamplingPeriod(samplingPeriod);
- comAndFootRealization_->ComputePostureForGivenCoMAndFeetPosture(
- aCoMState_, aCoMSpeed_, aCoMAcc_,
- aLeftFootPosition_, aRightFootPosition_,
- configuration, velocity, acceleration,
- iteration, stage);
-
+ comAndFootRealization_->
+ ComputePostureForGivenCoMAndFeetPosture
+ (aCoMState_, aCoMSpeed_, aCoMAcc_,
+ aLeftFootPosition_, aRightFootPosition_,
+ configuration, velocity, acceleration,
+ iteration, stage);
+
// std::cout << " configuration:" << configuration << std::endl;
ODEBUG5SIMPLE(configuration,"/tmp/test_configuration.dat");
@@ -491,20 +491,24 @@ void DynamicFilter::InverseKinematics(
void DynamicFilter::stage0INstage1()
{
- comAndFootRealization_->SetPreviousConfigurationStage1(
- comAndFootRealization_->GetPreviousConfigurationStage0());
- comAndFootRealization_->SetPreviousVelocityStage1(
- comAndFootRealization_->GetPreviousVelocityStage0());
+ comAndFootRealization_->
+ SetPreviousConfigurationStage1
+ (comAndFootRealization_->GetPreviousConfigurationStage0());
+ comAndFootRealization_->
+ SetPreviousVelocityStage1
+ (comAndFootRealization_->GetPreviousVelocityStage0());
return ;
}
-void DynamicFilter::ComputeZMPMB(double samplingPeriod,
- const COMState & inputCoMState,
- const FootAbsolutePosition & inputLeftFoot,
- const FootAbsolutePosition & inputRightFoot,
- Eigen::Vector3d &ZMPMB,
- unsigned int stage,
- unsigned int iteration)
+void DynamicFilter::
+ComputeZMPMB
+(double samplingPeriod,
+ const COMState & inputCoMState,
+ const FootAbsolutePosition & inputLeftFoot,
+ const FootAbsolutePosition & inputRightFoot,
+ Eigen::Vector3d &ZMPMB,
+ unsigned int stage,
+ unsigned int iteration)
{
InverseKinematics( inputCoMState, inputLeftFoot, inputRightFoot,
ZMPMBConfiguration_, ZMPMBVelocity_, ZMPMBAcceleration_,
@@ -522,9 +526,10 @@ void DynamicFilter::ComputeZMPMB(double samplingPeriod,
return ;
}
-int DynamicFilter::OptimalControl(
- deque<ZMPPosition> & inputdeltaZMP_deq,
- deque<COMState> & outputDeltaCOMTraj_deq_)
+int DynamicFilter::
+OptimalControl
+( deque<ZMPPosition> & inputdeltaZMP_deq,
+ deque<COMState> & outputDeltaCOMTraj_deq_)
{
assert(PC_->IsCoherent());
std::size_t Nctrl = (int)round(controlWindowSize_/controlPeriod_) ;
@@ -583,12 +588,14 @@ int DynamicFilter::OptimalControl(
// // compute the speed and acceleration of the waist in the world frame
// if (PreviousSupportFoot_)
// {
-// Jac_LF::run(robot_, prev_q_, Vector3dTpl<LocalFloatType>::Type(0,0,0), jacobian_lf_);
+// Jac_LF::run(robot_, prev_q_, Vector3dTpl<LocalFloatType>::Type(0,0,0),
+// jacobian_lf_);
// waist_speed = jacobian_lf_ * prev_dq_ ;
// waist_acc = jacobian_lf_ * prev_ddq_ /* + d_jacobian_lf_ * prev_dq_*/ ;
// }else
// {
-// Jac_RF::run(robot_, prev_q_, Vector3dTpl<LocalFloatType>::Type(0,0,0), jacobian_rf_);
+// Jac_RF::run(robot_, prev_q_, Vector3dTpl<LocalFloatType>::Type(0,0,0),
+// jacobian_rf_);
// waist_speed = jacobian_rf_ * prev_dq_ ;
// waist_acc = jacobian_rf_ * prev_ddq_ /*+ d_jacobian_rf_ * prev_dq_*/ ;
// }
@@ -598,7 +605,8 @@ int DynamicFilter::OptimalControl(
// ddq_(i,0) = waist_acc(i,0);
// }
// // compute the position of the waist in the world frame
-// RootNode & node_waist = boost::fusion::at_c<Robot_Model::BODY>(robot_.nodes);
+// RootNode & node_waist = boost::fusion::at_c<Robot_Model::BODY>
+// (robot_.nodes);
// waist_theta(0,0) = prev_q_(3,0) ;
// waist_theta(1,0) = prev_dq_(4,0) ;
// waist_theta(2,0) = prev_ddq_(5,0) ;
@@ -636,8 +644,10 @@ int DynamicFilter::OptimalControl(
// }
//
// metapod::rnea< Robot_Model, true >::run(robot_2, q, dq, ddq);
-// LankleNode & node_lankle = boost::fusion::at_c<Robot_Model::l_ankle>(robot_2.nodes);
-// RankleNode & node_rankle = boost::fusion::at_c<Robot_Model::r_ankle>(robot_2.nodes);
+// LankleNode & node_lankle =
+// boost::fusion::at_c<Robot_Model::l_ankle>(robot_2.nodes);
+// RankleNode & node_rankle =
+// boost::fusion::at_c<Robot_Model::r_ankle>(robot_2.nodes);
//
// CWx = node_waist.body.iX0.r()(0,0) - inputCoMState.x[0] ;
// CWy = node_waist.body.iX0.r()(1,0) - inputCoMState.y[0] ;
@@ -747,14 +757,16 @@ int DynamicFilter::OptimalControl(
// return ;
//}
-void DynamicFilter::Debug(const deque<COMState> & ctrlCoMState,
- const deque<FootAbsolutePosition> & ctrlLeftFoot,
- const deque<FootAbsolutePosition> & ctrlRightFoot,
- const deque<COMState> & inputCOMTraj_deq_,
- const deque<ZMPPosition> inputZMPTraj_deq_,
- const deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
- const deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
- const deque<COMState> & outputDeltaCOMTraj_deq_)
+void DynamicFilter::
+Debug
+(const deque<COMState> & ctrlCoMState,
+ const deque<FootAbsolutePosition> & ctrlLeftFoot,
+ const deque<FootAbsolutePosition> & ctrlRightFoot,
+ const deque<COMState> & inputCOMTraj_deq_,
+ const deque<ZMPPosition> inputZMPTraj_deq_,
+ const deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+ const deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
+ const deque<COMState> & outputDeltaCOMTraj_deq_)
{
deque<COMState> CoM_tmp = ctrlCoMState ;
int Nctrl = (int)round(controlWindowSize_/controlPeriod_) ;
@@ -779,7 +791,8 @@ void DynamicFilter::Debug(const deque<COMState> & ctrlCoMState,
com.pitch[0] = com.pitch[0] * 180 / M_PI ;
com.yaw[0] = com.yaw[0] * 180 / M_PI ;
InverseKinematics( com, ctrlLeftFoot[i], ctrlRightFoot[i],
- ZMPMBConfiguration_, ZMPMBVelocity_, ZMPMBAcceleration_,
+ ZMPMBConfiguration_, ZMPMBVelocity_,
+ ZMPMBAcceleration_,
controlPeriod_, 2, 20) ;
PR_->computeInverseDynamics(ZMPMBConfiguration_,
@@ -872,18 +885,26 @@ void DynamicFilter::Debug(const deque<COMState> & ctrlCoMState,
aof << inputRightFootTraj_deq_[i].dz << " " ; // 39
aof << inputRightFootTraj_deq_[i].ddz << " " ; // 40
- aof << CoM_tmp[i*(int)round(interpolationPeriod_/controlPeriod_)].x[0] << " "
+ aof << CoM_tmp[i*(int)round(interpolationPeriod_/
+ controlPeriod_)].x[0] << " "
; // 41
- aof << CoM_tmp[i*(int)round(interpolationPeriod_/controlPeriod_)].x[1] << " "
+ aof << CoM_tmp[i*(int)round(interpolationPeriod_/
+ controlPeriod_)].x[1] << " "
; // 42
- aof << CoM_tmp[i*(int)round(interpolationPeriod_/controlPeriod_)].x[2] << " "
+ aof << CoM_tmp[i*(int)round(interpolationPeriod_/
+ controlPeriod_)].x[2] << " "
; // 43
- aof << CoM_tmp[i*(int)round(interpolationPeriod_/controlPeriod_)].y[0] << " "
+ aof << CoM_tmp[i*(int)round(interpolationPeriod_/
+ controlPeriod_)].y[0] << " "
; // 44
- aof << CoM_tmp[i*(int)round(interpolationPeriod_/controlPeriod_)].y[1] << " "
+ aof << CoM_tmp[i*(int)round(interpolationPeriod_/
+ controlPeriod_)].y[1]
+ << " "
; // 45
- aof << CoM_tmp[i*(int)round(interpolationPeriod_/controlPeriod_)].y[2] << " "
+ aof << CoM_tmp[i*(int)round(interpolationPeriod_/
+ controlPeriod_)].y[2]
+ << " "
; // 46
//47
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
index e98279fc3bfe0f77ed52583ea93ce1045d143b7c..979e95b655def785400028973112ed0a4780ab17 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
@@ -45,64 +45,67 @@ namespace PatternGeneratorJRL
);
~DynamicFilter();
/// \brief
- int OffLinefilter(
- const deque<COMState> & inputCOMTraj_deq_,
- const deque<ZMPPosition> & inputZMPTraj_deq_,
- const deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
- const deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
- const vector<Eigen::VectorXd > &UpperPart_q,
- const vector<Eigen::VectorXd > &UpperPart_dq,
- const vector<Eigen::VectorXd > &UpperPart_ddq,
- deque<COMState> & outputDeltaCOMTraj_deq_);
-
- int OnLinefilter(const deque<COMState> & inputCOMTraj_deq_,
- const deque<ZMPPosition> & inputZMPTraj_deq_,
- const deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
- const deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
- deque<COMState> & outputDeltaCOMTraj_deq_);
-
- void init(
- double controlPeriod,
- double interpolationPeriod,
- double controlWindowSize,
- double previewWindowSize,
- double kajitaPCwindowSize,
- COMState inputCoMState);
+ int OffLinefilter
+ (const deque<COMState> & inputCOMTraj_deq_,
+ const deque<ZMPPosition> & inputZMPTraj_deq_,
+ const deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+ const deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
+ const vector<Eigen::VectorXd > &UpperPart_q,
+ const vector<Eigen::VectorXd > &UpperPart_dq,
+ const vector<Eigen::VectorXd > &UpperPart_ddq,
+ deque<COMState> & outputDeltaCOMTraj_deq_);
+
+ int OnLinefilter
+ (const deque<COMState> & inputCOMTraj_deq_,
+ const deque<ZMPPosition> & inputZMPTraj_deq_,
+ const deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+ const deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
+ deque<COMState> & outputDeltaCOMTraj_deq_);
+
+ void init
+ (double controlPeriod,
+ double interpolationPeriod,
+ double controlWindowSize,
+ double previewWindowSize,
+ double kajitaPCwindowSize,
+ COMState inputCoMState);
/// \brief atomic function
- void InverseKinematics(
- const COMState & inputCoMState,
- const FootAbsolutePosition & inputLeftFoot,
- const FootAbsolutePosition & inputRightFoot,
- Eigen::VectorXd & configuration,
- Eigen::VectorXd & velocity,
- Eigen::VectorXd & acceleration,
- double samplingPeriod,
- int stage,
- int iteration);
-
+ void InverseKinematics
+ ( const COMState & inputCoMState,
+ const FootAbsolutePosition & inputLeftFoot,
+ const FootAbsolutePosition & inputRightFoot,
+ Eigen::VectorXd & configuration,
+ Eigen::VectorXd & velocity,
+ Eigen::VectorXd & acceleration,
+ double samplingPeriod,
+ int stage,
+ int iteration);
+
/// \brief atomic function allow to compute
- void ComputeZMPMB(
- double samplingPeriod,
- const COMState & inputCoMState,
- const FootAbsolutePosition & inputLeftFoot,
- const FootAbsolutePosition & inputRightFoot,
- Eigen::Vector3d & ZMPMB,
- unsigned int stage,
- unsigned int iteration);
+ void ComputeZMPMB
+ (double samplingPeriod,
+ const COMState & inputCoMState,
+ const FootAbsolutePosition & inputLeftFoot,
+ const FootAbsolutePosition & inputRightFoot,
+ Eigen::Vector3d & ZMPMB,
+ unsigned int stage,
+ unsigned int iteration);
void stage0INstage1();
/// \brief Preview control on the ZMPMBs computed
- int OptimalControl(deque<ZMPPosition> &inputdeltaZMP_deq,
- deque<COMState> & outputDeltaCOMTraj_deq_);
-
+ int OptimalControl
+ (deque<ZMPPosition> &inputdeltaZMP_deq,
+ deque<COMState> & outputDeltaCOMTraj_deq_);
+
/// \brief compute the zmpmb from articulated pos vel and acc
- int zmpmb(Eigen::VectorXd& configuration,
- Eigen::VectorXd& velocity,
- Eigen::VectorXd& acceleration,
- Eigen::Vector3d & zmpmb);
-
+ int zmpmb
+ (Eigen::VectorXd& configuration,
+ Eigen::VectorXd& velocity,
+ Eigen::VectorXd& acceleration,
+ Eigen::Vector3d & zmpmb);
+
void CallMethod(std::string & Method, std::istringstream &strm);
private: // Private methods
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 93b6f7b68ddec232bc6213aea4f8075b1b51f12c..284d5a7397210744a2bc4b2b2f5e9ab4acea877a 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -477,11 +477,12 @@ InitOnLine
-void ZMPVelocityReferencedQP::OnLine(double time,
- deque<ZMPPosition> & FinalZMPTraj_deq,
- deque<COMState> & FinalCOMTraj_deq,
- deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> & FinalRightFootTraj_deq)
+void ZMPVelocityReferencedQP::OnLine
+(double time,
+ deque<ZMPPosition> & FinalZMPTraj_deq,
+ deque<COMState> & FinalCOMTraj_deq,
+ deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
+ deque<FootAbsolutePosition> & FinalRightFootTraj_deq)
{
// If on-line mode not activated we go out.
@@ -516,16 +517,20 @@ void ZMPVelocityReferencedQP::OnLine(double time,
// PREVIEW SUPPORT STATES FOR THE WHOLE PREVIEW WINDOW:
// ----------------------------------------------------
- VRQPGenerator_->preview_support_states( time, SupportFSM_,
- FinalLeftFootTraj_deq, FinalRightFootTraj_deq, Solution_.SupportStates_deq );
+ VRQPGenerator_->preview_support_states
+ ( time, SupportFSM_,
+ FinalLeftFootTraj_deq,
+ FinalRightFootTraj_deq,
+ Solution_.SupportStates_deq );
// COMPUTE ORIENTATIONS OF FEET FOR WHOLE PREVIEW PERIOD:
// ------------------------------------------------------
InitStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
- OrientPrw_->preview_orientations( time, VelRef_,
- SupportFSM_->StepPeriod(),
- FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
- Solution_ );
+ OrientPrw_->preview_orientations
+ ( time, VelRef_,
+ SupportFSM_->StepPeriod(),
+ FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
+ Solution_ );
// UPDATE THE DYNAMICS:
@@ -576,7 +581,9 @@ void ZMPVelocityReferencedQP::OnLine(double time,
// INTERPOLATION
FinalZMPTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
FinalCOMTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
- ControlInterpolation( FinalCOMTraj_deq, FinalZMPTraj_deq, FinalLeftFootTraj_deq,
+ ControlInterpolation( FinalCOMTraj_deq,
+ FinalZMPTraj_deq,
+ FinalLeftFootTraj_deq,
FinalRightFootTraj_deq, time) ;
DynamicFilterInterpolation(time);
@@ -633,7 +640,8 @@ void ZMPVelocityReferencedQP::OnLine(double time,
TimeToStopOnLineMode_ = UpperTimeLimitToUpdate_ + QP_T_ * QP_N_ +
m_SamplingPeriod;
}
- UpperTimeLimitToUpdate_ = UpperTimeLimitToUpdate_ + QP_T_ + m_SamplingPeriod ;
+ UpperTimeLimitToUpdate_ = UpperTimeLimitToUpdate_ + QP_T_ +
+ m_SamplingPeriod ;
}
else
{
@@ -652,15 +660,16 @@ void ZMPVelocityReferencedQP::OnLine(double time,
//----------"Real-time" loop---------
}
-void ZMPVelocityReferencedQP::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
+void ZMPVelocityReferencedQP::
+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
{
InitStateLIPM_ = LIPM_.GetState() ;
-
+
// INTERPOLATE CoM AND ZMP TRAJECTORIES:
// -------------------------------------
CoMZMPInterpolation(FinalZMPTraj_deq, FinalCOMTraj_deq,
@@ -671,18 +680,20 @@ void ZMPVelocityReferencedQP::ControlInterpolation(
// ------------------------------
OrientPrw_->one_iteration(time,Solution_.SupportStates_deq);
- OrientPrw_->interpolate_trunk_orientation( time, CurrentIndex_,
- m_SamplingPeriod, Solution_.SupportStates_deq,
- FinalCOMTraj_deq );
+ OrientPrw_->interpolate_trunk_orientation
+ ( time, CurrentIndex_,
+ m_SamplingPeriod, Solution_.SupportStates_deq,
+ FinalCOMTraj_deq );
FinalCurrentStateOrientPrw_ = OrientPrw_->CurrentTrunkState();
FinalPreviewStateOrientPrw_ = OrientPrw_->PreviewTrunkState();
// INTERPOLATE THE COMPUTED FOOT POSITIONS:
// ----------------------------------------
- OFTG_control_->interpolate_feet_positions( time,
- Solution_.SupportStates_deq, Solution_,
- Solution_.SupportOrientations_deq,
- FinalLeftFootTraj_deq, FinalRightFootTraj_deq);
+ OFTG_control_->
+ interpolate_feet_positions( time,
+ Solution_.SupportStates_deq, Solution_,
+ Solution_.SupportOrientations_deq,
+ FinalLeftFootTraj_deq, FinalRightFootTraj_deq);
return ;
}
@@ -701,18 +712,22 @@ void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
&Solution_, &LIPM_subsampled_,
NbSampleControl_, i, CurrentIndex_);
- OrientPrw_->interpolate_trunk_orientation( time + i * QP_T_,
- CurrentIndex_ + i * NbSampleControl_, m_SamplingPeriod,
- solution_.SupportStates_deq, COMTraj_deq_ctrl_ );
-
- // Modify a copy of the solution to allow "OFTG_DF_->interpolate_feet_positions"
+ OrientPrw_->
+ interpolate_trunk_orientation
+ ( time + i * QP_T_,
+ CurrentIndex_ + i * NbSampleControl_, m_SamplingPeriod,
+ solution_.SupportStates_deq, COMTraj_deq_ctrl_ );
+
+ // Modify a copy of the solution to allow
+ // "OFTG_DF_->interpolate_feet_positions"
// to use the correcte feet step previewed
PrepareSolution();
- OFTG_DF_->interpolate_feet_positions( time + i * QP_T_,
- solution_.SupportStates_deq, solution_,
- solution_.SupportOrientations_deq,
- LeftFootTraj_deq_ctrl_, RightFootTraj_deq_ctrl_);
+ OFTG_DF_->interpolate_feet_positions
+ ( time + i * QP_T_,
+ solution_.SupportStates_deq, solution_,
+ solution_.SupportOrientations_deq,
+ LeftFootTraj_deq_ctrl_, RightFootTraj_deq_ctrl_);
solution_.SupportStates_deq.pop_front();
}
@@ -724,16 +739,17 @@ void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
return ;
}
-void ZMPVelocityReferencedQP::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 * aSolutionReference, // INPUT
- LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
- const unsigned numberOfSample, // INPUT
- const int IterationNumber, // INPUT
- const unsigned int currentIndex) // INPUT
+void ZMPVelocityReferencedQP::
+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 * aSolutionReference, // INPUT
+ LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
+ const unsigned numberOfSample, // INPUT
+ const int IterationNumber, // INPUT
+ const unsigned int currentIndex) // INPUT
{
if(aSolutionReference->SupportStates_deq.size()
&& aSolutionReference->SupportStates_deq[IterationNumber].NbStepsLeft == 0)
@@ -748,10 +764,10 @@ void ZMPVelocityReferencedQP::CoMZMPInterpolation(
Running_ = false;
}
const double tf = 0.75;
- jx = 6/(tf*tf*tf)*(jx - tf*COMTraj_deq[i-1].x[1] - (tf*tf/2)*COMTraj_deq[i
- -1].x[2]);
- jy = 6/(tf*tf*tf)*(jy - tf*COMTraj_deq[i-1].y[1] - (tf*tf/2)*COMTraj_deq[i
- -1].y[2]);
+ jx = 6/(tf*tf*tf)*(jx - tf*COMTraj_deq[i-1].x[1] -
+ (tf*tf/2)*COMTraj_deq[i-1].x[2]);
+ jy = 6/(tf*tf*tf)*(jy - tf*COMTraj_deq[i-1].y[1] -
+ (tf*tf/2)*COMTraj_deq[i-1].y[2]);
LIPM->Interpolation( COMTraj_deq, ZMPPositions,
currentIndex + IterationNumber * numberOfSample,
jx, jy);
@@ -760,12 +776,14 @@ void ZMPVelocityReferencedQP::CoMZMPInterpolation(
else
{
Running_ = true;
- LIPM->Interpolation( COMTraj_deq, ZMPPositions,
- currentIndex + IterationNumber * numberOfSample,
- aSolutionReference->Solution_vec[IterationNumber],
- aSolutionReference->Solution_vec[IterationNumber+QP_N_] );
- LIPM->OneIteration( aSolutionReference->Solution_vec[IterationNumber],
- aSolutionReference->Solution_vec[IterationNumber+QP_N_] );
+ LIPM->Interpolation
+ ( COMTraj_deq, ZMPPositions,
+ currentIndex + IterationNumber * numberOfSample,
+ aSolutionReference->Solution_vec[IterationNumber],
+ aSolutionReference->Solution_vec[IterationNumber+QP_N_] );
+ LIPM->OneIteration
+ ( aSolutionReference->Solution_vec[IterationNumber],
+ aSolutionReference->Solution_vec[IterationNumber+QP_N_] );
}
return ;
}
@@ -802,15 +820,18 @@ void ZMPVelocityReferencedQP::InterpretSolutionVector()
if ( nbSteps > 0 )
{
// center of the feet of the last preview double support phase :
- double middleX = (FootPrw_vec[size_vec_sol-2][0] + FootPrw_vec[size_vec_sol
- -3][0])*0.5 ;
- double middleY = (FootPrw_vec[size_vec_sol-2][1] + FootPrw_vec[size_vec_sol
- -3][1])*0.5 ;
-
- FootPrw_vec[size_vec_sol-1][0] = FootPrw_vec[size_vec_sol-2][0] + 2*( (
- middleX + Vx*StepPeriod_) - FootPrw_vec[size_vec_sol-2][0] );
- FootPrw_vec[size_vec_sol-1][1] = FootPrw_vec[size_vec_sol-2][1] + 2*( (
- middleY + Vy*StepPeriod_) - FootPrw_vec[size_vec_sol-2][1] );
+ double middleX = (FootPrw_vec[size_vec_sol-2][0] +
+ FootPrw_vec[size_vec_sol
+ -3][0])*0.5 ;
+ double middleY = (FootPrw_vec[size_vec_sol-2][1] +
+ FootPrw_vec[size_vec_sol
+ -3][1])*0.5 ;
+
+ FootPrw_vec[size_vec_sol-1][0] =
+ FootPrw_vec[size_vec_sol-2][0] +
+ 2*( (middleX + Vx*StepPeriod_) - FootPrw_vec[size_vec_sol-2][0] );
+ FootPrw_vec[size_vec_sol-1][1] = FootPrw_vec[size_vec_sol-2][1] +
+ 2*( (middleY + Vy*StepPeriod_) - FootPrw_vec[size_vec_sol-2][1] );
}
else
{
@@ -819,10 +840,10 @@ void ZMPVelocityReferencedQP::InterpretSolutionVector()
Sign = 1.0;
else
Sign = -1.0;
- FootPrw_vec[size_vec_sol-1][0] = CurrentSupport.X + Sign*sin(
- FirstSupport.Yaw)*FeetDistance_;
- FootPrw_vec[size_vec_sol-1][1] = CurrentSupport.Y - Sign*cos(
- FirstSupport.Yaw)*FeetDistance_;
+ FootPrw_vec[size_vec_sol-1][0] =
+ CurrentSupport.X + Sign*sin(FirstSupport.Yaw)*FeetDistance_;
+ FootPrw_vec[size_vec_sol-1][1] =
+ CurrentSupport.Y - Sign*cos(FirstSupport.Yaw)*FeetDistance_;
}
}
for(unsigned int i = 0 ; i < SupportStates.size() ; ++i)
@@ -832,7 +853,8 @@ void ZMPVelocityReferencedQP::InterpretSolutionVector()
return ;
}
-// Modify a copy of the solution to allow "OFTG_DF_->interpolate_feet_positions()"
+// Modify a copy of the solution to allow
+// "OFTG_DF_->interpolate_feet_positions()"
// to use the correct foot step previewed
void ZMPVelocityReferencedQP::PrepareSolution()
{
@@ -842,55 +864,65 @@ void ZMPVelocityReferencedQP::PrepareSolution()
if(CurrentSupport.Phase!=DS && nbSteps!=0)
{
- solution_.Solution_vec[2*QP_N_] = FootPrw_vec[CurrentSupport.StepNumber+1][0] ;
- solution_.Solution_vec[2*QP_N_+nbSteps] = FootPrw_vec[CurrentSupport.StepNumber
+ solution_.Solution_vec[2*QP_N_] =
+ FootPrw_vec[CurrentSupport.StepNumber+1][0] ;
+ solution_.Solution_vec[2*QP_N_+nbSteps] =
+ FootPrw_vec[CurrentSupport.StepNumber
+1][1];
}
return ;
}
// TODO: New parent class needed
-void ZMPVelocityReferencedQP::GetZMPDiscretization(deque<ZMPPosition> &,
- deque<COMState> &,
- deque<RelativeFootPosition> &,
- deque<FootAbsolutePosition> &,
- deque<FootAbsolutePosition> &,
- double,
- COMState &,
- Eigen::Vector3d &,
- FootAbsolutePosition &,
- FootAbsolutePosition & )
+void ZMPVelocityReferencedQP::
+GetZMPDiscretization
+(deque<ZMPPosition> &,
+ deque<COMState> &,
+ deque<RelativeFootPosition> &,
+ deque<FootAbsolutePosition> &,
+ deque<FootAbsolutePosition> &,
+ double,
+ COMState &,
+ Eigen::Vector3d &,
+ FootAbsolutePosition &,
+ FootAbsolutePosition & )
{
cout << "To be removed" << endl;
}
-void ZMPVelocityReferencedQP::OnLineAddFoot(RelativeFootPosition &,
- deque<ZMPPosition> &,
- deque<COMState> &,
- deque<FootAbsolutePosition> &,
- deque<FootAbsolutePosition> &,
- bool)
+void ZMPVelocityReferencedQP::
+OnLineAddFoot
+(RelativeFootPosition &,
+ deque<ZMPPosition> &,
+ deque<COMState> &,
+ deque<FootAbsolutePosition> &,
+ deque<FootAbsolutePosition> &,
+ bool)
{
cout << "To be removed" << endl;
}
-int ZMPVelocityReferencedQP::OnLineFootChange(double,
- FootAbsolutePosition &,
- deque<ZMPPosition> &,
- deque<COMState> &,
- deque<FootAbsolutePosition> &,
- deque<FootAbsolutePosition> &,
- StepStackHandler *)
+int ZMPVelocityReferencedQP::
+OnLineFootChange
+(double,
+ FootAbsolutePosition &,
+ deque<ZMPPosition> &,
+ deque<COMState> &,
+ deque<FootAbsolutePosition> &,
+ deque<FootAbsolutePosition> &,
+ StepStackHandler *)
{
cout << "To be removed" << endl;
return -1;
}
-void ZMPVelocityReferencedQP::EndPhaseOfTheWalking(deque<ZMPPosition> &,
- deque<COMState> &,
- deque<FootAbsolutePosition> &,
- deque<FootAbsolutePosition> &)
+void ZMPVelocityReferencedQP::
+EndPhaseOfTheWalking
+(deque<ZMPPosition> &,
+ deque<COMState> &,
+ deque<FootAbsolutePosition> &,
+ deque<FootAbsolutePosition> &)
{
cout << "To be removed" << endl;
}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp
index 42c9b5bfc2f91f9452bedc3d195981948c196c97..95c8e2dc399637dfc161d0e79a30cff3af1aaa19 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp
@@ -569,18 +569,6 @@ OnLine(double time,
FinalLeftFootTraj_deq [i] = LeftFootTraj_deq_ctrl_ [i] ;
FinalRightFootTraj_deq[i] = RightFootTraj_deq_ctrl_[i] ;
}
- // if(outputPreviewDuration_==m_SamplingPeriod)
- // {
- // dynamicFilter_->InverseKinematics(initCOM_,initLeftFoot_,initRightFoot_,
- // m_CurrentConfiguration_,
- // m_CurrentVelocity_,
- // m_CurrentAcceleration_,
- // m_SamplingPeriod,2,20);
- // dynamicFilter_->getComAndFootRealization()
- // ->SetPreviousConfigurationStage1(m_CurrentConfiguration_);
- // dynamicFilter_->getComAndFootRealization()
- // ->SetPreviousVelocityStage1(m_CurrentVelocity_);
- // }
dynamicFilter_->OnLinefilter(COMTraj_deq_,ZMPTraj_deq_ctrl_,
LeftFootTraj_deq_,
RightFootTraj_deq_,
@@ -647,9 +635,11 @@ void ZMPVelocityReferencedSQP::FullTrajectoryInterpolation(double time)
+ CurrentIndexUpperBound_,itCOM_);
ZMPTraj_deq_ctrl_.resize(CurrentIndex_ + previewSize_ * NbSampleControl_
+ CurrentIndexUpperBound_,initZMP_);
- LeftFootTraj_deq_ctrl_.resize(CurrentIndex_ + previewSize_ * NbSampleControl_
+ LeftFootTraj_deq_ctrl_.resize(CurrentIndex_ + previewSize_ *
+ NbSampleControl_
+ CurrentIndexUpperBound_,initLeftFoot_);
- RightFootTraj_deq_ctrl_.resize(CurrentIndex_ + previewSize_ * NbSampleControl_
+ RightFootTraj_deq_ctrl_.resize(CurrentIndex_ + previewSize_ *
+ NbSampleControl_
+ CurrentIndexUpperBound_,initRightFoot_);
}
@@ -742,14 +732,15 @@ void ZMPVelocityReferencedSQP::FullTrajectoryInterpolation(double time)
return ;
}
-void ZMPVelocityReferencedSQP::CoMZMPInterpolation(
- std::vector<double> & JerkX, // INPUT
- std::vector<double> & JerkY, // INPUT
- LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
- const unsigned, // INPUT
- const int IterationNumber, // INPUT
- const unsigned int currentIndex, // INPUT
- const std::deque<support_state_t> & SupportStates_deq )// INPUT
+void ZMPVelocityReferencedSQP::
+CoMZMPInterpolation
+(std::vector<double> & JerkX, // INPUT
+ std::vector<double> & JerkY, // INPUT
+ LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
+ const unsigned, // INPUT
+ const int IterationNumber, // INPUT
+ const unsigned int currentIndex, // INPUT
+ const std::deque<support_state_t> & SupportStates_deq )// INPUT
{
if(SupportStates_deq[0].Phase==DS && SupportStates_deq[0].NbStepsLeft == 0)
{
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.hh
index 6f1cdc9936dbd7f7e5a364b5a2761fc797d1744c..5cbc4a324ef269e0585d807df31c7cd5fa702ae8 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.hh
@@ -36,8 +36,6 @@
#include <ZMPRefTrajectoryGeneration/nmpc_generator.hh>
#include <ZMPRefTrajectoryGeneration/DynamicFilter.hh>
-//#include </home/mnaveau/devel/ros_unstable/src/jrl/jrl-walkgen/tests/ClockCPUTime.hh>
-
namespace PatternGeneratorJRL
{
@@ -59,14 +57,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
@@ -133,7 +133,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;
@@ -224,8 +225,6 @@ namespace PatternGeneratorJRL
/// \brief Nb. foot support inside preview window
int SQP_nf_;
-
-
/// \brief 2D LIPM to simulate the evolution of the robot's CoM.
LinearizedInvertedPendulum2D LIPM_ ;
@@ -277,9 +276,11 @@ namespace PatternGeneratorJRL
/// \brief Size of the output preview
unsigned NbSampleOutput_ ;
- /// \brief Number of interpolated point needed for control computed during SQP_T_
+ /// \brief Number of interpolated point needed for control
+ /// computed during SQP_T_
unsigned NbSampleControl_ ;
- /// \brief Number of interpolated point needed for the dynamic filter computed during SQP_T_
+ /// \brief Number of interpolated point needed for the dynamic
+ /// filter computed during SQP_T_
unsigned NbSampleInterpolation_ ;
/// \brief Size of the preview for filtering
unsigned previewSize_ ;
@@ -305,23 +306,25 @@ 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);
+ 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,
@@ -331,25 +334,28 @@ namespace PatternGeneratorJRL
deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
StepStackHandler * aStepStackHandler);
- void EndPhaseOfTheWalking(deque<ZMPPosition> & ZMPPositions,
- deque<COMState> & FinalCOMTraj_deq,
- deque<FootAbsolutePosition> & LeftFootAbsolutePositions,
- deque<FootAbsolutePosition> & RightFootAbsolutePositions);
-
+ void EndPhaseOfTheWalking
+ (deque<ZMPPosition> & ZMPPositions,
+ deque<COMState> & FinalCOMTraj_deq,
+ deque<FootAbsolutePosition> & LeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> & RightFootAbsolutePositions);
+
int ReturnOptimalTimeToRegenerateAStep();
- /// \brief Interpolate just enough data to pilot the robot (period of interpolation = QP_T_)
+ /// \brief Interpolate just enough data to pilot the robot (period of
+ /// interpolation = QP_T_)
/// uses
void FullTrajectoryInterpolation(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::vector<double> &JerkX, // INPUT
- std::vector<double> &JerkY, // INPUT
- LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
- const unsigned numberOfSample, // INPUT
- const int IterationNumber, // INPUT
- const unsigned int currentIndex, // INPUT
- const std::deque<support_state_t> & SupportStates_deq );// INPUT
+ /// \brief Interpolation form the com jerk the position of the com
+ /// and the zmp corresponding to the kart table model
+ void CoMZMPInterpolation
+ (std::vector<double> &JerkX, // INPUT
+ std::vector<double> &JerkY, // INPUT
+ LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
+ const unsigned numberOfSample, // INPUT
+ const int IterationNumber, // INPUT
+ const unsigned int currentIndex, // INPUT
+ const std::deque<support_state_t> & SupportStates_deq );// INPUT
};
}
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index 60922d6705f5e7dd286b95a61fda6d295548257b..6ceeb2fa9058163f99a567ece537d58049f70b7e 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -33,8 +33,10 @@ using namespace std;
using namespace PatternGeneratorJRL;
-GeneratorVelRef::GeneratorVelRef(SimplePluginManager *lSPM,
- IntermedQPMat * Data, RigidBodySystem * Robot, RelativeFeetInequalities * RFI )
+GeneratorVelRef::GeneratorVelRef
+(SimplePluginManager *lSPM,
+ IntermedQPMat * Data, RigidBodySystem * Robot,
+ RelativeFeetInequalities * RFI )
: MPCTrajectoryGeneration(lSPM)
, IntermedData_ (Data)
, Robot_(Robot)
@@ -63,18 +65,19 @@ void
GeneratorVelRef::Ponderation( double weight, objective_e type)
{
- IntermedQPMat::objective_variant_t & Objective = IntermedData_->Objective(
- type );
+ IntermedQPMat::objective_variant_t & Objective =
+ IntermedData_->Objective(type );
Objective.weight = weight;
}
void
-GeneratorVelRef::preview_support_states( double time, const SupportFSM * FSM,
- const deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
- const deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
- deque<support_state_t> & SupportStates_deq )
+GeneratorVelRef::preview_support_states
+( double time, const SupportFSM * FSM,
+ const deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
+ const deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
+ deque<support_state_t> & SupportStates_deq )
{
const FootAbsolutePosition * FAP = NULL;
@@ -138,8 +141,9 @@ GeneratorVelRef::preview_support_states( double time, const SupportFSM * FSM,
void
-GeneratorVelRef::generate_selection_matrices( const std::deque<support_state_t>
- & SupportStates_deq )
+GeneratorVelRef::generate_selection_matrices
+( const std::deque<support_state_t>
+ & SupportStates_deq )
{
IntermedQPMat::state_variant_t & State = IntermedData_->State();
@@ -166,8 +170,10 @@ GeneratorVelRef::generate_selection_matrices( const std::deque<support_state_t>
{
if(SS_it->StepNumber>0)
{
- State.V(i,SS_it->StepNumber-1) = State.VT(SS_it->StepNumber-1,i) = 1.0;
- if( SS_it->StepNumber==1 && SS_it->StateChanged && SS_it->Phase == SS )
+ State.V(i,SS_it->StepNumber-1) =
+ State.VT(SS_it->StepNumber-1,i) = 1.0;
+ if( SS_it->StepNumber==1 &&
+ SS_it->StateChanged && SS_it->Phase == SS )
{
--SS_it;
State.Vc_fX(0) = SS_it->X;
@@ -286,8 +292,10 @@ GeneratorVelRef::initialize_matrices( linear_inequality_t & Inequalities)
void
-GeneratorVelRef::build_inequalities_cop(linear_inequality_t & Inequalities,
- const std::deque<support_state_t> & SupportStates_deq) const
+GeneratorVelRef::
+build_inequalities_cop
+(linear_inequality_t & Inequalities,
+ const std::deque<support_state_t> & SupportStates_deq) const
{
deque<support_state_t>::const_iterator prwSS_it = SupportStates_deq.begin();
@@ -313,7 +321,8 @@ GeneratorVelRef::build_inequalities_cop(linear_inequality_t & Inequalities,
// cout << "RIGHT \n" ;
// cout << "convex hull : \n";
// for(unsigned int k = 0 ; k < CoPHull.X_vec.size() ; ++k)
- // cout << CoPHull.X_vec[k] << " " << CoPHull.Y_vec[k] << endl ;
+ // cout << CoPHull.X_vec[k] << " " << CoPHull.Y_vec[k]
+ // << endl ;
}
RFI_->compute_linear_system( CoPHull, *prwSS_it );
// cout << "linear system \n";
@@ -322,7 +331,8 @@ GeneratorVelRef::build_inequalities_cop(linear_inequality_t & Inequalities,
Inequalities.D.X_mat.insert( i*nbEdges+j, i)= CoPHull.A_vec[j] ;
Inequalities.D.Y_mat.insert( i*nbEdges+j, i)= CoPHull.B_vec[j] ;
Inequalities.Dc_vec( i*nbEdges+j ) = CoPHull.D_vec[j];
- // cout << CoPHull.A_vec[j] << " " << CoPHull.B_vec[j] << " " << CoPHull.D_vec[j] << endl;
+ // cout << CoPHull.A_vec[j] << " " << CoPHull.B_vec[j]
+ // << " " << CoPHull.D_vec[j] << endl;
}
++prwSS_it;
@@ -332,8 +342,10 @@ GeneratorVelRef::build_inequalities_cop(linear_inequality_t & Inequalities,
void
-GeneratorVelRef::build_inequalities_feet( linear_inequality_t & Inequalities,
- const std::deque<support_state_t> & SupportStates_deq ) const
+GeneratorVelRef::
+build_inequalities_feet
+( linear_inequality_t & Inequalities,
+ const std::deque<support_state_t> & SupportStates_deq ) const
{
// Arrays for the generated set of inequalities
@@ -351,7 +363,8 @@ GeneratorVelRef::build_inequalities_feet( linear_inequality_t & Inequalities,
for( unsigned i=0; i<N_; i++ )
{
//foot positioning constraints
- if( prwSS_it->StateChanged && prwSS_it->StepNumber>0 && prwSS_it->Phase != DS)
+ if( prwSS_it->StateChanged &&
+ prwSS_it->StepNumber>0 && prwSS_it->Phase != DS)
{
prwSS_it--;//Take the support state before
RFI_->set_vertices( FeetHull, *prwSS_it, INEQ_FEET );
@@ -360,7 +373,8 @@ GeneratorVelRef::build_inequalities_feet( linear_inequality_t & Inequalities,
// else
// cout << "RIGHT \n" ;
// for(unsigned int k = 0 ; k < FeetHull.X_vec.size() ; ++k)
- // cout << FeetHull.X_vec[k] << " " << FeetHull.Y_vec[k] << endl ;
+ // cout << FeetHull.X_vec[k] << " " << FeetHull.Y_vec[k]
+ // << endl ;
prwSS_it++;
@@ -368,12 +382,16 @@ GeneratorVelRef::build_inequalities_feet( linear_inequality_t & Inequalities,
//cout << "linear system \n";
for( unsigned j = 0; j < nbEdges; j++ )
{
- Inequalities.D.X_mat.insert ((prwSS_it->StepNumber-1)*nbEdges+j,
- (prwSS_it->StepNumber-1))= FeetHull.A_vec[j] ;
- Inequalities.D.Y_mat.insert ((prwSS_it->StepNumber-1)*nbEdges+j,
- (prwSS_it->StepNumber-1))= FeetHull.B_vec[j] ;
- Inequalities.Dc_vec( (prwSS_it->StepNumber-1)*nbEdges+j ) = FeetHull.D_vec[j];
- // cout << FeetHull.A_vec[j] << " " << FeetHull.B_vec[j] << " " << FeetHull.D_vec[j] << endl;
+ Inequalities.D.X_mat.insert
+ ((prwSS_it->StepNumber-1)*nbEdges+j,
+ (prwSS_it->StepNumber-1))= FeetHull.A_vec[j] ;
+ Inequalities.D.Y_mat.insert
+ ((prwSS_it->StepNumber-1)*nbEdges+j,
+ (prwSS_it->StepNumber-1))= FeetHull.B_vec[j] ;
+ Inequalities.Dc_vec
+ ( (prwSS_it->StepNumber-1)*nbEdges+j ) = FeetHull.D_vec[j];
+ // cout << FeetHull.A_vec[j] << " " << FeetHull.B_vec[j]
+ // << " " << FeetHull.D_vec[j] << endl;
}
}
@@ -384,8 +402,10 @@ GeneratorVelRef::build_inequalities_feet( linear_inequality_t & Inequalities,
void
-GeneratorVelRef::build_inequalities_com(linear_inequality_t & Inequalities,
- const std::deque<support_state_t> & SupportStates_deq) const
+GeneratorVelRef::
+build_inequalities_com
+(linear_inequality_t & Inequalities,
+ const std::deque<support_state_t> & SupportStates_deq) const
{
deque<support_state_t>::const_iterator prwSS_it = SupportStates_deq.begin();
@@ -422,60 +442,69 @@ GeneratorVelRef::build_inequalities_com(linear_inequality_t & Inequalities,
void
-GeneratorVelRef::build_constraints_cop(const linear_inequality_t & IneqCoP,
- unsigned int NbStepsPreviewed, QPProblem & Pb)
+GeneratorVelRef::
+build_constraints_cop
+(const linear_inequality_t & IneqCoP,
+ unsigned int NbStepsPreviewed, QPProblem & Pb)
{
unsigned int NbConstraints = Pb.NbConstraints();
// -D*U
- compute_term ( MM_, -1.0, IneqCoP.D.X_mat, Robot_->DynamicsCoPJerk().U );
- Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, 0 );
- compute_term ( MM_, -1.0, IneqCoP.D.Y_mat, Robot_->DynamicsCoPJerk().U );
- Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, N_ );
+ compute_term ( MM_, -1.0, IneqCoP.D.X_mat,
+ Robot_->DynamicsCoPJerk().U );
+ Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, 0 );
+ compute_term ( MM_, -1.0, IneqCoP.D.Y_mat,
+ Robot_->DynamicsCoPJerk().U );
+ Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, N_ );
// +D*V
- compute_term ( MM_, 1.0, IneqCoP.D.X_mat,
- IntermedData_->State().V );
- // + Robot_->LeftFoot().Dynamics(COP).U + Robot_->RightFoot().Dynamics(COP).U );
- Pb.add_term_to( MATRIX_DU, MM_, NbConstraints,
- 2*N_ );
-
- // cout << "IntermedData_->State().V = " << IntermedData_->State().V << endl ;
- compute_term ( MM_, 1.0, IneqCoP.D.Y_mat,
- IntermedData_->State().V );
- // + Robot_->LeftFoot().Dynamics(COP).U + Robot_->RightFoot().Dynamics(COP).U );
- Pb.add_term_to( MATRIX_DU, MM_, NbConstraints,
- 2*N_+NbStepsPreviewed );
+ compute_term ( MM_, 1.0, IneqCoP.D.X_mat, IntermedData_->State().V );
+ // + Robot_->LeftFoot().Dynamics(COP).U +
+ // Robot_->RightFoot().Dynamics(COP).U );
+ Pb.add_term_to( MATRIX_DU, MM_, NbConstraints,2*N_);
+
+ // cout << "IntermedData_->State().V = "
+ // << IntermedData_->State().V << endl ;
+ compute_term ( MM_, 1.0, IneqCoP.D.Y_mat, IntermedData_->State().V );
+ // + Robot_->LeftFoot().Dynamics(COP).U +
+ // Robot_->RightFoot().Dynamics(COP).U );
+ Pb.add_term_to( MATRIX_DU, MM_, NbConstraints,2*N_+NbStepsPreviewed );
//constant part
// +dc
- Pb.add_term_to( VECTOR_DS,IneqCoP.Dc_vec, NbConstraints );
+ Pb.add_term_to( VECTOR_DS,IneqCoP.Dc_vec, NbConstraints);
// -D*S_z*x
compute_term ( MV_, -1.0, IneqCoP.D.X_mat, Robot_->DynamicsCoPJerk().S,
- IntermedData_->State().CoM.x );
- Pb.add_term_to( VECTOR_DS, MV_,
- NbConstraints );
+ IntermedData_->State().CoM.x );
+ Pb.add_term_to( VECTOR_DS, MV_,NbConstraints);
/*
* Usefull for multibody dynamics
*
- compute_term ( MV_, -1.0, IneqCoP.D.x, Robot_->LeftFoot().Dynamics(COP).S, Robot_->LeftFoot().State().X );
- Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
- compute_term ( MV_, -1.0, IneqCoP.D.x, Robot_->RightFoot().Dynamics(COP).S, Robot_->RightFoot().State().X );
- Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
+ compute_term ( MV_, -1.0, IneqCoP.D.x, Robot_->LeftFoot().Dynamics(COP).S,
+ Robot_->LeftFoot().State().X );
+ Pb.add_term_to( VECTOR_DS, MV_,
+ NbConstraints );
+ compute_term ( MV_, -1.0, IneqCoP.D.x, Robot_->RightFoot().Dynamics(COP).S,
+ Robot_->RightFoot().State().X );
+ Pb.add_term_to( VECTOR_DS, MV_,
+ NbConstraints );
*/
compute_term ( MV_, -1.0, IneqCoP.D.Y_mat, Robot_->DynamicsCoPJerk().S,
IntermedData_->State().CoM.y );
- Pb.add_term_to( VECTOR_DS, MV_,
- NbConstraints );
+ Pb.add_term_to( VECTOR_DS, MV_,NbConstraints);
/*
* Usefull for multibody dynamics
*
- compute_term ( MV_, -1.0, IneqCoP.D.y, Robot_->LeftFoot().Dynamics(COP).S, Robot_->LeftFoot().State().Y );
- Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
- compute_term ( MV_, -1.0, IneqCoP.D.y, Robot_->RightFoot().Dynamics(COP).S, Robot_->RightFoot().State().Y );
- Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
+ compute_term ( MV_, -1.0, IneqCoP.D.y, Robot_->LeftFoot().Dynamics(COP).S,
+ Robot_->LeftFoot().State().Y );
+ Pb.add_term_to( VECTOR_DS, MV_,
+ NbConstraints );
+ compute_term ( MV_, -1.0, IneqCoP.D.y, Robot_->RightFoot().Dynamics(COP).S,
+ Robot_->RightFoot().State().Y );
+ Pb.add_term_to( VECTOR_DS, MV_,
+ NbConstraints );
*/
// +D*Vc*FP
@@ -487,9 +516,11 @@ GeneratorVelRef::build_constraints_cop(const linear_inequality_t & IneqCoP,
void
-GeneratorVelRef::build_constraints_feet(const linear_inequality_t & IneqFeet,
- const IntermedQPMat::state_variant_t & State,
- int NbStepsPreviewed, QPProblem & Pb)
+GeneratorVelRef::
+build_constraints_feet
+(const linear_inequality_t & IneqFeet,
+ const IntermedQPMat::state_variant_t & State,
+ int NbStepsPreviewed, QPProblem & Pb)
{
unsigned int NbConstraints = Pb.NbConstraints();
@@ -513,8 +544,10 @@ GeneratorVelRef::build_constraints_feet(const linear_inequality_t & IneqFeet,
void
-GeneratorVelRef::build_constraints_com( const linear_inequality_t & IneqCoM,
- const support_state_t & CurrentSupport, QPProblem & Pb )
+GeneratorVelRef::
+build_constraints_com
+( const linear_inequality_t & IneqCoM,
+ const support_state_t & CurrentSupport, QPProblem & Pb )
{
const linear_dynamics_t & CoMDyn = Robot_->CoM().Dynamics( POSITION );
@@ -540,29 +573,33 @@ GeneratorVelRef::build_constraints_com( const linear_inequality_t & IneqCoM,
Pb.add_term_to( MATRIX_DU, MM_, nbConstraints, 2*N_ );//X
// +Dx*(S*cx-Vc*pcx)
- compute_term ( MV_, 1.0, IneqCoM.D.X_mat, CoMDyn.S, CoM.x );
- Pb.add_term_to( VECTOR_DS, MV_, nbConstraints );
- compute_term ( MV_, -CurrentSupport.X, IneqCoM.D.X_mat, State.VcshiftX );
- Pb.add_term_to( VECTOR_DS, MV_, nbConstraints );
+ compute_term ( MV_, 1.0, IneqCoM.D.X_mat, CoMDyn.S, CoM.x );
+ Pb.add_term_to( VECTOR_DS, MV_, nbConstraints );
+ compute_term ( MV_, -CurrentSupport.X, IneqCoM.D.X_mat,
+ State.VcshiftX );
+ Pb.add_term_to( VECTOR_DS, MV_, nbConstraints );
// +Dy*(S*cy-Vc*pcy)
- compute_term ( MV_, 1.0, IneqCoM.D.Y_mat, CoMDyn.S, CoM.y );
- Pb.add_term_to( VECTOR_DS, MV_, nbConstraints );
- compute_term ( MV_, -CurrentSupport.Y, IneqCoM.D.Y_mat, State.VcshiftY );
- Pb.add_term_to( VECTOR_DS, MV_, nbConstraints );
+ compute_term ( MV_, 1.0, IneqCoM.D.Y_mat, CoMDyn.S, CoM.y );
+ Pb.add_term_to( VECTOR_DS, MV_, nbConstraints );
+ compute_term ( MV_, -CurrentSupport.Y, IneqCoM.D.Y_mat,
+ State.VcshiftY );
+ Pb.add_term_to( VECTOR_DS, MV_, nbConstraints);
// +Dz*cz
MM_ = IneqCoM.D.Z_mat*Robot_->CoMHeight();
- Pb.add_term_to( VECTOR_DS, MM_, nbConstraints, 0 );
+ Pb.add_term_to( VECTOR_DS, MM_, nbConstraints, 0 );
// +dc
- Pb.add_term_to( VECTOR_DS, IneqCoM.Dc_vec, nbConstraints );
+ Pb.add_term_to( VECTOR_DS, IneqCoM.Dc_vec, nbConstraints );
}
void
-GeneratorVelRef::build_eq_constraints_feet( const std::deque<support_state_t> &
- SupportStates_deq,
- unsigned int NbStepsPreviewed, QPProblem & Pb )
+GeneratorVelRef::
+build_eq_constraints_feet
+( const std::deque<support_state_t> &
+ SupportStates_deq,
+ unsigned int NbStepsPreviewed, QPProblem & Pb )
{
if(SupportStates_deq.front().StateChanged)
@@ -592,8 +629,9 @@ GeneratorVelRef::build_eq_constraints_feet( const std::deque<support_state_t> &
}
-void GeneratorVelRef::build_eq_constraints_limitPosFeet(
- const solution_t & Solution,QPProblem & Pb)
+void GeneratorVelRef::
+build_eq_constraints_limitPosFeet
+(const solution_t & Solution,QPProblem & Pb)
{
std::deque<support_state_t>::const_iterator SPTraj_it =
Solution.SupportStates_deq.begin();
@@ -651,7 +689,8 @@ GeneratorVelRef::build_constraints( QPProblem & Pb, solution_t & Solution )
//Equality constraints:
//---------------------
- //build_eq_constraints_feet( Solution.SupportStates_deq, nbStepsPreviewed, Pb );
+ //build_eq_constraints_feet( Solution.SupportStates_deq, nbStepsPreviewed,
+ //Pb );
build_eq_constraints_limitPosFeet( Solution, Pb);
// Polygonal constraints:
@@ -671,7 +710,8 @@ GeneratorVelRef::build_constraints( QPProblem & Pb, solution_t & Solution )
// -----------------------
// linear_inequality_t & IneqCoM = IntermedData_->Inequalities( INEQ_COM );
// build_inequalities_com( IneqCoM, Solution.SupportStates_deq );
- // const support_state_t & CurrentSupport = Solution.SupportStates_deq.front();
+ // const support_state_t & CurrentSupport =
+ // Solution.SupportStates_deq.front();
// build_constraints_com( IneqCoM, CurrentSupport, Pb );
if (Solution.useWarmStart)
@@ -690,37 +730,37 @@ GeneratorVelRef::build_invariant_part( QPProblem & Pb )
//Constant terms in the Hessian
// +a*U'*U
- const IntermedQPMat::objective_variant_t & Jerk = IntermedData_->Objective(
- JERK_MIN );
+ const IntermedQPMat::objective_variant_t & Jerk =
+ IntermedData_->Objective(JERK_MIN );
const linear_dynamics_t & JerkDynamics = CoM.Dynamics( JERK );
compute_term ( MM_, Jerk.weight, JerkDynamics.UT, JerkDynamics.U );
Pb.add_term_to( MATRIX_Q, MM_, 0, 0 );
Pb.add_term_to( MATRIX_Q, MM_, N_, N_ );
// +a*U'*U
- const IntermedQPMat::objective_variant_t & InstVel = IntermedData_->Objective(
- INSTANT_VELOCITY );
+ const IntermedQPMat::objective_variant_t & InstVel =
+ IntermedData_->Objective(INSTANT_VELOCITY );
const linear_dynamics_t & VelDynamics = CoM.Dynamics( VELOCITY );
compute_term ( MM_, InstVel.weight, VelDynamics.UT, VelDynamics.U );
Pb.add_term_to( MATRIX_Q, MM_, 0, 0 );
Pb.add_term_to( MATRIX_Q, MM_, N_, N_ );
// +a*U'*U
- const IntermedQPMat::objective_variant_t & COPCent = IntermedData_->Objective(
- COP_CENTERING );
+ const IntermedQPMat::objective_variant_t & COPCent =
+ IntermedData_->Objective(COP_CENTERING );
compute_term ( MM_, COPCent.weight, Robot_->DynamicsCoPJerk().UT,
Robot_->DynamicsCoPJerk().U );
- Pb.add_term_to( MATRIX_Q, MM_, 0,
- 0 );
- Pb.add_term_to( MATRIX_Q, MM_, N_,
- N_ );
+ Pb.add_term_to( MATRIX_Q, MM_, 0,0);
+ Pb.add_term_to( MATRIX_Q, MM_, N_,N_);
}
void
-GeneratorVelRef::update_problem( QPProblem & Pb,
- const std::deque<support_state_t> & SupportStates_deq )
+GeneratorVelRef::
+update_problem
+( QPProblem & Pb,
+ const std::deque<support_state_t> & SupportStates_deq )
{
Pb.clear(VECTOR_D);
@@ -730,8 +770,8 @@ GeneratorVelRef::update_problem( QPProblem & Pb,
const RigidBody & CoM = Robot_->CoM();
// Instant velocity terms
- const IntermedQPMat::objective_variant_t & InstVel = IntermedData_->Objective(
- INSTANT_VELOCITY );
+ const IntermedQPMat::objective_variant_t & InstVel =
+ IntermedData_->Objective(INSTANT_VELOCITY );
const linear_dynamics_t & VelDynamics = CoM.Dynamics( VELOCITY );
// Linear part
// +a*U'*S*x
@@ -744,16 +784,18 @@ GeneratorVelRef::update_problem( QPProblem & Pb,
Pb.add_term_to( VECTOR_D, MV_,
N_ );
// +a*U'*ref
- compute_term ( MV_, -InstVel.weight, VelDynamics.UT, State.Ref.Global.X_vec );
- Pb.add_term_to( VECTOR_D, MV_, 0 );
- compute_term ( MV_, -InstVel.weight, VelDynamics.UT, State.Ref.Global.Y_vec );
- Pb.add_term_to( VECTOR_D, MV_, N_ );
+ compute_term ( MV_, -InstVel.weight, VelDynamics.UT,
+ State.Ref.Global.X_vec);
+ Pb.add_term_to( VECTOR_D, MV_, 0 );
+ compute_term ( MV_, -InstVel.weight,
+ VelDynamics.UT, State.Ref.Global.Y_vec );
+ Pb.add_term_to( VECTOR_D, MV_, N_ );
// COP - centering terms
- const IntermedQPMat::objective_variant_t & COPCent = IntermedData_->Objective(
- COP_CENTERING );
+ const IntermedQPMat::objective_variant_t & COPCent =
+ IntermedData_->Objective(COP_CENTERING );
const linear_dynamics_t & CoPDynamics = Robot_->DynamicsCoPJerk( );
// const linear_dynamics_t & LFCoP = Robot_->LeftFoot().Dynamics(COP);
// const linear_dynamics_t & RFCoP = Robot_->RightFoot().Dynamics(COP);
@@ -764,20 +806,20 @@ GeneratorVelRef::update_problem( QPProblem & Pb,
Pb.add_term_to( MATRIX_Q, MM_, N_, 2*N_+nbStepsPreviewed );
// -a*V*U
- compute_term ( MM_, -COPCent.weight, State.VT, CoPDynamics.U );
- Pb.add_term_to( MATRIX_Q, MM_, 2*N_, 0 );
- Pb.add_term_to( MATRIX_Q, MM_, 2*N_+nbStepsPreviewed, N_ );
+ compute_term ( MM_, -COPCent.weight, State.VT, CoPDynamics.U );
+ Pb.add_term_to( MATRIX_Q, MM_, 2*N_, 0 );
+ Pb.add_term_to( MATRIX_Q, MM_, 2*N_+nbStepsPreviewed, N_ );
//+a*V'*V
- compute_term ( MM_, COPCent.weight, State.VT, State.V );
- Pb.add_term_to( MATRIX_Q, MM_, 2*N_, 2*N_ );
- Pb.add_term_to( MATRIX_Q, MM_, 2*N_+nbStepsPreviewed, 2*N_+nbStepsPreviewed );
+ compute_term ( MM_, COPCent.weight, State.VT, State.V );
+ Pb.add_term_to( MATRIX_Q, MM_, 2*N_, 2*N_);
+ Pb.add_term_to( MATRIX_Q, MM_, 2*N_+nbStepsPreviewed, 2*N_+nbStepsPreviewed);
//Linear part
// -a*V'*S*x
- compute_term ( MV_, -COPCent.weight, State.VT, CoPDynamics.S, State.CoM.x );
- Pb.add_term_to( VECTOR_D, MV_, 2*N_ );
- compute_term ( MV_, -COPCent.weight, State.VT, CoPDynamics.S, State.CoM.y );
- Pb.add_term_to( VECTOR_D, MV_, 2*N_+nbStepsPreviewed );
+ compute_term ( MV_, -COPCent.weight, State.VT, CoPDynamics.S, State.CoM.x );
+ Pb.add_term_to( VECTOR_D, MV_, 2*N_ );
+ compute_term ( MV_, -COPCent.weight, State.VT, CoPDynamics.S, State.CoM.y );
+ Pb.add_term_to( VECTOR_D, MV_, 2*N_+nbStepsPreviewed);
// +a*V'*Vc*x
compute_term ( MV_, COPCent.weight, State.VT, State.VcX );
Pb.add_term_to( VECTOR_D, MV_, 2*N_ );
@@ -808,7 +850,8 @@ GeneratorVelRef::compute_warm_start( solution_t & Solution )
// Compute initial ZMP and foot positions:
// ---------------------------------------
- deque<support_state_t>::iterator prwSS_it = Solution.SupportStates_deq.begin();
+ deque<support_state_t>::iterator prwSS_it =
+ Solution.SupportStates_deq.begin();
prwSS_it++;//Point at the first previewed support state
unsigned int j = 0;
for(unsigned int i=0; i<N_; i++)
@@ -861,8 +904,10 @@ GeneratorVelRef::compute_warm_start( solution_t & Solution )
void
-GeneratorVelRef::compute_term(Eigen::MatrixXd &weightMM, double weight,
- const Eigen::MatrixXd &M1, const Eigen::MatrixXd &M2)
+GeneratorVelRef::
+compute_term
+(Eigen::MatrixXd &weightMM, double weight,
+ const Eigen::MatrixXd &M1, const Eigen::MatrixXd &M2)
{
weightMM = M1*M2;
weightMM *= weight;
@@ -870,8 +915,10 @@ GeneratorVelRef::compute_term(Eigen::MatrixXd &weightMM, double weight,
void
-GeneratorVelRef::compute_term(Eigen::VectorXd &weightMV, double weight,
- const Eigen::MatrixXd &M, const Eigen::VectorXd &V)
+GeneratorVelRef::
+compute_term
+(Eigen::VectorXd &weightMV, double weight,
+ const Eigen::MatrixXd &M, const Eigen::VectorXd &V)
{
weightMV = M*V;
weightMV *= weight;
@@ -879,9 +926,11 @@ GeneratorVelRef::compute_term(Eigen::VectorXd &weightMV, double weight,
void
-GeneratorVelRef::compute_term(Eigen::VectorXd &weightMV,
- double weight, const Eigen::MatrixXd &M,
- const Eigen::VectorXd &V, double scalar)
+GeneratorVelRef::
+compute_term
+(Eigen::VectorXd &weightMV,
+ double weight, const Eigen::MatrixXd &M,
+ const Eigen::VectorXd &V, double scalar)
{
weightMV = M*V;
weightMV *= weight*scalar;
@@ -889,9 +938,11 @@ GeneratorVelRef::compute_term(Eigen::VectorXd &weightMV,
void
-GeneratorVelRef::compute_term(Eigen::VectorXd &weightMV,
- double weight, const Eigen::MatrixXd &M1,
- const Eigen::MatrixXd &M2, const Eigen::VectorXd &V2)
+GeneratorVelRef::
+compute_term
+(Eigen::VectorXd &weightMV,
+ double weight, const Eigen::MatrixXd &M1,
+ const Eigen::MatrixXd &M2, const Eigen::VectorXd &V2)
{
MV2_ = M2*V2;
weightMV = M1*MV2_;
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
index 88332cae0334da7df450a99fbdd1ee74a9973cc8..ea183de65b7433fe1593c81c302359433059ecac 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
@@ -70,12 +70,14 @@ namespace PatternGeneratorJRL
/// \param[in] FinalLeftFootTraj_deq
/// \param[in] FinalRightFootTraj_deq
/// \param[out] SupportStates_deq
- void preview_support_states( double Time, const SupportFSM * FSM,
- const deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
- const deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
- deque<support_state_t> & SupportStates_deq );
-
- /// \brief Set the global reference from the local one and the orientation of the trunk frame
+ void preview_support_states
+ ( double Time, const SupportFSM * FSM,
+ const deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
+ const deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
+ deque<support_state_t> & SupportStates_deq );
+
+ /// \brief Set the global reference from the local one and the
+ /// orientation of the trunk frame
/// for the whole preview window
///
/// \param[in] Solution
@@ -101,7 +103,8 @@ namespace PatternGeneratorJRL
/// \param[in] Pb
/// \param[in] SupportStates_deq
void update_problem( QPProblem & Pb,
- const std::deque<support_state_t> & SupportStates_deq );
+ const std::deque<support_state_t> &
+ SupportStates_deq );
/// \brief Compute the initial solution vector for warm start
///
@@ -150,8 +153,9 @@ namespace PatternGeneratorJRL
///
/// \param[out] Inequalities
/// \param[in] SupportStates_deq
- void build_inequalities_feet(linear_inequality_t & Inequalities,
- const std::deque<support_state_t> & SupportStates_deq) const;
+ void build_inequalities_feet
+ (linear_inequality_t & Inequalities,
+ const std::deque<support_state_t> & SupportStates_deq) const;
//
// Protected methods
@@ -164,12 +168,14 @@ namespace PatternGeneratorJRL
void generate_selection_matrices( const std::deque<support_state_t> &
SupportStates_deq);
- /// \brief Generate a queue of inequalities with respect to the centers of the feet
+ /// \brief Generate a queue of inequalities with respect to the centers of
+ /// the feet
///
/// \param[out] Inequalities
/// \param[in] SupportStates_deq
void build_inequalities_cop(linear_inequality_t & Inequalities,
- const std::deque<support_state_t> & SupportStates_deq) const;
+ const std::deque<support_state_t> &
+ SupportStates_deq) const;
/// \brief Generate a queue of inequality constraints on
/// the feet positions with respect to previous foot positions
@@ -177,7 +183,8 @@ namespace PatternGeneratorJRL
/// \param[out] Inequalities In matrix form
/// \param[in] SupportStates_deq
void build_inequalities_com(linear_inequality_t & Inequalities,
- const std::deque<support_state_t> & SupportStates_deq) const;
+ const std::deque<support_state_t> &
+ SupportStates_deq) const;
/// \brief Compute CoP constraints corresponding to the set of inequalities
///
@@ -204,7 +211,8 @@ namespace PatternGeneratorJRL
/// \param[in] CurrentSupport
/// \param[out] Pb
void build_constraints_com( const linear_inequality_t & IneqCoM,
- const support_state_t & CurrentSupport, QPProblem & Pb );
+ const support_state_t & CurrentSupport,
+ QPProblem & Pb );
/// \brief Compute feet equality constraints from a trajectory
///
@@ -213,9 +221,11 @@ namespace PatternGeneratorJRL
/// \param[out] Pb
void build_eq_constraints_feet( const std::deque<support_state_t> &
SupportStates_deq,
- unsigned int NbStepsPreviewed, QPProblem & Pb );
+ unsigned int NbStepsPreviewed,
+ QPProblem & Pb );
- /// \brief Compute feet equality constraints to restrain the previewed foot position
+ /// \brief Compute feet equality constraints to restrain the previewed foot
+ /// position
/// some iteration before landing
/// \param[in] Solution
/// \param[out] Pb
diff --git a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp b/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp
index 94da0f0d7374cedf6e4dfa9bab8986154a84fe47..b7493f82cb2724b4423b62816ce16d534aba8fa1 100644
--- a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp
+++ b/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp
@@ -23,7 +23,8 @@
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* \file mpc-trajectory-generation.cpp
- \brief Abstract object for trajectory generation via model predictive control.*/
+ \brief Abstract object for trajectory generation
+ via model predictive control.*/
#include <Debug.hh>
#include <ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh>
diff --git a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh b/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh
index cc8a48d7d2c0d235b9fe7adedfcc554c14528af2..0fd898408c39e9a2e90bcd583ec7ae4144d432c5 100644
--- a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh
+++ b/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh
@@ -23,7 +23,8 @@
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* \file mpc-trajectory-generation.cpp
- \brief Abstract object for trajectory generation via model predictive control.*/
+ \brief Abstract object for trajectory generation via model
+ predictive control.*/
#ifndef _TRAJ_GEN_H_
#define _TRAJ_GEN_H_
@@ -38,7 +39,8 @@
namespace PatternGeneratorJRL
{
- /*! This class defines an abstract interface to generate low-dimensional walking trajectories (CoM/CdP/Feet).
+ /*! This class defines an abstract interface to generate
+ low-dimensional walking trajectories (CoM/CdP/Feet).
*/
class MPCTrajectoryGeneration : public SimplePlugin
{
@@ -83,7 +85,8 @@ namespace PatternGeneratorJRL
/// \name Members related to the generation of feet trajectories.
/// @{
- /// \brief ModulationSupportCoefficient coeeficient to wait a little before foot is of the ground
+ /// \brief ModulationSupportCoefficient coeeficient to wait
+ /// a little before foot is of the ground
double ModulationSupportCoefficient_;
/// \brief The foot orientation for the lift off and the landing
@@ -165,7 +168,8 @@ namespace PatternGeneratorJRL
Tdble_ = Tdble;
};
- /// \brief Get the sampling period for the control, set to 0.005 by default. */
+ /// \brief Get the sampling period for the control, set
+ /// to 0.005 by default. */
inline const double & SamplingPeriodControl() const
{
return Tctr_;
diff --git a/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp b/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
index e42aa53465a5cfaad98a2bcd3f00ce828b9f9f7b..0e98cabf83eb29299fc76f92c36c10856bdcac6b 100644
--- a/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
+++ b/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
@@ -175,12 +175,13 @@ NMPCgenerator::~NMPCgenerator()
}
}
-void NMPCgenerator::initNMPCgenerator(
- bool useLineSearch,
- support_state_t & currentSupport,
- COMState &lStartingCOMState,
- reference_t &local_vel_ref,
- unsigned N, unsigned nf, double T, double T_step)
+void NMPCgenerator::
+initNMPCgenerator
+(bool useLineSearch,
+ support_state_t & currentSupport,
+ COMState &lStartingCOMState,
+ reference_t &local_vel_ref,
+ unsigned N, unsigned nf, double T, double T_step)
{
N_ = N ;
nf_ = nf ;
@@ -417,11 +418,16 @@ void NMPCgenerator::updateConstraint()
// Global Jacobian for all constraints
// Constraints
- // qp_lbJ_ = (qp_lbJ_cop_ ) < qp_J_ * X = (qp_J_cop_ ) * X < qp_ubJ_ = (qp_ubJ_cop_ )
- // (qp_lbJ_foot_ ) < (qp_J_foot_ ) < (qp_ubJ_foot_ )
- // (qp_lbJ_vel_ ) < (qp_J_vel_ ) < (qp_ubJ_vel_ )
- // (qp_lbJ_obs_ ) < (qp_J_obs_ ) < (qp_ubJ_obs_ )
- // (qp_lbJ_theta_) < (qp_J_theta_) < (qp_ubJ_theta_)
+ // qp_lbJ_ = (qp_lbJ_cop_ ) < qp_J_ * X = (qp_J_cop_ ) * X < qp_ubJ_ =
+ // (qp_ubJ_cop_ )
+ // (qp_lbJ_foot_ ) < (qp_J_foot_ ) <
+ // (qp_ubJ_foot_ )
+ // (qp_lbJ_vel_ ) < (qp_J_vel_ ) <
+ // (qp_ubJ_vel_ )
+ // (qp_lbJ_obs_ ) < (qp_J_obs_ ) <
+ // (qp_ubJ_obs_ )
+ // (qp_lbJ_theta_) < (qp_J_theta_) <
+ // (qp_ubJ_theta_)
//
// Jacobians
// qp_J_cop_ = (Acop_xy_ , Acop_theta_ )
@@ -576,11 +582,13 @@ void NMPCgenerator::evalConstraint(Eigen::VectorXd & U)
return ;
}
-void NMPCgenerator::updateInitialCondition(double time,
- FootAbsolutePosition & currentLeftFootAbsolutePosition,
- FootAbsolutePosition & currentRightFootAbsolutePosition,
- COMState & currentCOMState,
- reference_t & local_vel_ref)
+void NMPCgenerator::
+updateInitialCondition
+(double time,
+ FootAbsolutePosition & currentLeftFootAbsolutePosition,
+ FootAbsolutePosition & currentRightFootAbsolutePosition,
+ COMState & currentCOMState,
+ reference_t & local_vel_ref)
{
time_ = time ;
@@ -1087,9 +1095,10 @@ getSolution
}
}
-void NMPCgenerator::updateCurrentSupport(double time,
- FootAbsolutePosition &FinalLeftFoot,
- FootAbsolutePosition &FinalRightFoot)
+void NMPCgenerator::updateCurrentSupport
+(double time,
+ FootAbsolutePosition &FinalLeftFoot,
+ FootAbsolutePosition &FinalRightFoot)
{
#ifdef DEBUG_COUT
cout << "previous support : \n"
@@ -1122,9 +1131,10 @@ void NMPCgenerator::updateCurrentSupport(double time,
}
}
-void NMPCgenerator::updateSupportdeque(double time,
- FootAbsolutePosition &FinalLeftFoot,
- FootAbsolutePosition &FinalRightFoot)
+void NMPCgenerator::updateSupportdeque
+(double time,
+ FootAbsolutePosition &FinalLeftFoot,
+ FootAbsolutePosition &FinalRightFoot)
{
SupportStates_deq_[0] = currentSupport_ ;
const FootAbsolutePosition * FAP = NULL;
@@ -1521,8 +1531,10 @@ void NMPCgenerator::updateCoPconstraint(Eigen::VectorXd &U)
{
for(unsigned j=0 ; j<Acop_theta_.cols() ; ++j)
{
- Acop_theta_(i,j) = derv_Acop_map2_(i,
- j) * Acop_theta_dummy1_(i); // warning this is the real jacobian
+ Acop_theta_(i,j) =
+ derv_Acop_map2_(i,
+ j) *
+ Acop_theta_dummy1_(i); // warning this is the real jacobian
//Acop_theta_(i,j) = 0.0 ; // WARNING this is not the real jacobian !
}
}
@@ -1664,7 +1676,8 @@ void NMPCgenerator::updateFootPoseConstraint(Eigen::VectorXd &U)
// if( desiredNextSupportFootRelativePosition.size()!=0 )
// {
- // unsigned nbFoot = std::min(desiredNextSupportFootRelativePosition.size(),nf_);
+ // unsigned nbFoot =
+ // std::min(desiredNextSupportFootRelativePosition.size(),nf_);
// ignoreFirstStep = nbFoot ;
// }
@@ -1722,7 +1735,8 @@ void NMPCgenerator::updateFootPoseConstraint(Eigen::VectorXd &U)
if(n!=0)
{
deltaF_[n](0)=U(N_+n)-U(N_+n-1) ;//F_kp1_x_[n]-F_kp1_x_[n-1];
- deltaF_[n](1)=U(2*N_+nf_+n)-U(2*N_+nf_+n-1) ;//F_kp1_y_[n]-F_kp1_y_[n-1];
+ deltaF_[n](1)=U(2*N_+nf_+n)-U(2*N_+nf_+n-1) ;
+ //F_kp1_y_[n]-F_kp1_y_[n-1];
AdRdF_[n] = A0f_theta_[n]*deltaF_[n];
double sum = 0.0 ;
for (unsigned j=0 ; j<n_vertices_ ; ++j)
@@ -1732,8 +1746,10 @@ void NMPCgenerator::updateFootPoseConstraint(Eigen::VectorXd &U)
for (unsigned j=0 ; j<n_vertices_ ; ++j)
{
//Afoot_theta_[n](j,n-1) = sum;
- Afoot_theta_[n](j,n-1) = AdRdF_[n](j); // This is the real jacobian!
- //Afoot_theta_[n](j,n-1) = 0.0; // WARNING this is not the real jacobian!
+ Afoot_theta_[n](j,n-1) = AdRdF_[n](j);
+ // This is the real jacobian!
+ //Afoot_theta_[n](j,n-1) = 0.0;
+ // WARNING this is not the real jacobian!
}
for(unsigned i=0 ; i<n_vertices_ ; ++i)
for(unsigned j=0 ; j<nf_ ; ++j)
@@ -2172,8 +2188,10 @@ void NMPCgenerator::initializeCostFunction()
// are computed in the update function
// p_xy_ = ( p_xy_X_, p_xy_Fx_, p_xy_Y_, p_xy_Fy_ )
- // p_theta_ = ( 0.5 * a * (-2) * [ f_k_theta+T_step*dTheta^ref f_k_theta+2*T_step*dTheta^ref ] )
- // Those are time dependant matrices so they are computed in the update function
+ // p_theta_ = ( 0.5 * a * (-2) * [ f_k_theta+T_step*dTheta^ref
+ // f_k_theta+2*T_step*dTheta^ref ] )
+ // Those are time dependant matrices so they are computed in
+ // the update function
}
void NMPCgenerator::updateCostFunction()
@@ -2314,7 +2332,8 @@ void NMPCgenerator::updateCostFunction()
for(unsigned i=0 ; i<nf_ ; ++i)
p_(index+i) = p_xy_Fy_(i) ;
index+=nf_;
- // p_theta_ = ( 0.5 * a * [ f_k_theta+T_step*dTheta^ref f_k_theta+2*T_step*dTheta^ref ] )
+ // p_theta_ = ( 0.5 * a * [ f_k_theta+T_step*dTheta^ref
+ // f_k_theta+2*T_step*dTheta^ref ] )
for(unsigned i=0 ; i<nf_ ; ++i)
p_(index+i) = - alpha_theta_ * ( currentSupport_.Yaw +
(i+1) * T_step_* vel_ref_.Global.Yaw )
diff --git a/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh b/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh
index 2a9fd6edec85e836501a870f244e10508c9ef323..84b981284f1b84262af53760038468751ddb92e1 100644
--- a/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh
+++ b/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh
@@ -48,12 +48,13 @@ namespace PatternGeneratorJRL
COMState & lStartingCOMState,
reference_t & local_vel_ref,
unsigned N, unsigned nf, double T, double T_step);
- void updateInitialCondition(double time,
- FootAbsolutePosition ¤tLeftFootAbsolutePosition,
- FootAbsolutePosition ¤tRightFootAbsolutePosition,
- COMState & currentCOMState,
- reference_t & local_vel_ref
- );
+ void updateInitialCondition
+ (double time,
+ FootAbsolutePosition ¤tLeftFootAbsolutePosition,
+ FootAbsolutePosition ¤tRightFootAbsolutePosition,
+ COMState & currentCOMState,
+ reference_t & local_vel_ref
+ );
void solve();
private:
@@ -283,7 +284,8 @@ namespace PatternGeneratorJRL
Eigen::VectorXd UBcop_ ;
Eigen::MatrixXd D_kp1_xy_, D_kp1_theta_, Pzuv_, derv_Acop_map_ ;
Eigen::MatrixXd derv_Acop_map2_ ;
- Eigen::VectorXd b_kp1_, Pzsc_, Pzsc_x_, Pzsc_y_, v_kp1f_, v_kp1f_x_, v_kp1f_y_ ;
+ Eigen::VectorXd b_kp1_, Pzsc_, Pzsc_x_, Pzsc_y_, v_kp1f_,
+ v_kp1f_x_, v_kp1f_y_ ;
Eigen::VectorXd v_kf_x_, v_kf_y_ ;
Eigen::MatrixXd diffMat_ ;
Eigen::MatrixXd rotMat_xy_, rotMat_theta_, rotMat_;
diff --git a/src/ZMPRefTrajectoryGeneration/problem-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/problem-vel-ref.cpp
index 011175339b8aaf6b89d32497cc46105e6408f4c9..34a3da753737f572fcf6a80d466bb3a4907e18ba 100644
--- a/src/ZMPRefTrajectoryGeneration/problem-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/problem-vel-ref.cpp
@@ -105,7 +105,8 @@ void ProblemVelRef_s::AllocateMemory()
Q=new double[4*(m_QP_N+m_stepNumber)*(m_QP_N
- +m_stepNumber)]; //Quadratic part of the objective function
+ +m_stepNumber)];
+ //Quadratic part of the objective function
D=new double[2*(m_QP_N
+m_stepNumber)]; // Linear part of the objective function
XL=new double[2*(m_QP_N+m_stepNumber)]; // Lower bound of the jerk.
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
index 93485876ebd11ad97a4db66a4db7dba6a59c8507..760be1538db779fde163e0e9cb09d42757eb534d 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
@@ -277,7 +277,8 @@ QPProblem::solve( solver_e Solver, solution_t & Result, const tests_e & tests )
case QLD:
ql0001_(&m_, &me_, &mmax_, &n_, &nmax_, &mnn_,
- Q_dense_.Array_, D_.Array_, DU_dense_.Array_, DS_.Array_, XL_.Array_,
+ Q_dense_.Array_, D_.Array_, DU_dense_.Array_,
+ DS_.Array_, XL_.Array_,
XU_.Array_,
X_.Array_, U_.Array_, &iout_, &ifail_, &iprint_,
war_.Array_, &lwar_, iwar_.Array_, &liwar_, &eps_);
@@ -376,8 +377,9 @@ QPProblem::solve( solver_e Solver, solution_t & Result, const tests_e & tests )
{
if (tmp(i)+DS(i)<-1e-6)
{
- std::cout << "Unrespected constraint " << i << " : " << tmp(i) << " < " << -DS(
- i) << std::endl;
+ std::cout << "Unrespected constraint " << i
+ << " : " << tmp(i) << " < "
+ << -DS(i) << std::endl;
++nb_ctr;
}
}
@@ -393,9 +395,6 @@ QPProblem::solve( solver_e Solver, solution_t & Result, const tests_e & tests )
&inform_, &iter_, &obj_, clamda_,
iwar_.Array_, &liwar_, war_.Array_, &lwar_);
-
-
-
lastSolution_.resize(n_);
for(int i = 0; i < n_; i++)
{
@@ -440,15 +439,18 @@ QPProblem::add_term_to( qp_element_e Type, const Eigen::MatrixXd &Mat,
{
case MATRIX_Q:
Array_p = &Q_;
- NbVariables_ = (unsigned int)((col+Mat.cols()>NbVariables_) ? col+Mat.cols() :
- NbVariables_);
+ NbVariables_ =
+ (unsigned int)((col+Mat.cols()>NbVariables_) ?
+ col+Mat.cols() :
+ NbVariables_);
break;
case MATRIX_DU:
Array_p = &DU_;
NbConstraints_ = (unsigned int)(( row+Mat.rows() > NbConstraints_ ) ? row
+Mat.rows() : NbConstraints_);
- NbVariables_ = (unsigned int)(( col+Mat.cols() > NbVariables_ ) ? col+Mat.cols()
+ NbVariables_ = (unsigned int)(( col+Mat.cols() > NbVariables_ ) ?
+ col+Mat.cols()
: NbVariables_);
row++;//The first rows of DU,DS are empty
break;
@@ -514,25 +516,29 @@ QPProblem::add_term_to( qp_element_e Type, const Eigen::VectorXd &Vec,
{
case VECTOR_D:
Array_p = &D_;
- NbVariables_ = (unsigned int)((row+Vec.size()>NbVariables_) ? row+Vec.size() :
+ NbVariables_ = (unsigned int)((row+Vec.size()>NbVariables_) ?
+ row+Vec.size() :
NbVariables_);
break;
case VECTOR_XL:
Array_p = &XL_;
- NbVariables_ = (unsigned int)((row+Vec.size()>NbVariables_) ? row+Vec.size() :
+ NbVariables_ = (unsigned int)((row+Vec.size()>NbVariables_) ?
+ row+Vec.size() :
NbVariables_);
break;
case VECTOR_XU:
Array_p = &XU_;
- NbVariables_ = (unsigned int)((row+Vec.size()>NbVariables_) ? row+Vec.size() :
+ NbVariables_ = (unsigned int)((row+Vec.size()>NbVariables_) ?
+ row+Vec.size() :
NbVariables_);
break;
case VECTOR_DS:
Array_p = &DS_;
- NbConstraints_ = (unsigned int)((row+Vec.size()>NbConstraints_) ? row+Vec.size()
+ NbConstraints_ = (unsigned int)((row+Vec.size()>NbConstraints_) ?
+ row+Vec.size()
: NbConstraints_);
row++;//The first rows of DU,DS are empty
break;
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.hh b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
index 03e73e8deb08117f9260e53d01a02aee0d30e247..752f7a359e545e694fe6ea1d8dd62fd305fd647d 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.hh
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
@@ -325,7 +325,8 @@ namespace PatternGeneratorJRL
/// \name ql-parameters
/// \{
int m_, me_, mmax_, n_, nmax_, mnn_;
- array_s<double> Q_, Q_dense_, D_, DU_, DU_dense_, DS_, XL_, XU_, X_, U_, war_;
+ array_s<double> Q_, Q_dense_, D_, DU_, DU_dense_, DS_, XL_,
+ XU_, X_, U_, war_;
array_s<int> iwar_;
int iout_, ifail_, iprint_, lwar_, liwar_;
double eps_;
diff --git a/src/patterngeneratorinterfaceprivate.hh b/src/patterngeneratorinterfaceprivate.hh
index ab4e4d60c1ae61b51943210f50fa16af08f9b6c3..b9998d7c0cf3f6cb6ba048064404a707984f4a21 100644
--- a/src/patterngeneratorinterfaceprivate.hh
+++ b/src/patterngeneratorinterfaceprivate.hh
@@ -25,7 +25,8 @@
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/*! \file PatternGeneratorInterface.h
- \brief This object provides a unified interface to access the pattern generator.
+ \brief This object provides a unified interface to access
+ the pattern generator.
It allows to hide all the computation and hacking to the user.
*/
@@ -67,7 +68,8 @@ namespace PatternGeneratorJRL
/** @ingroup Interface
This class is the interface between the Pattern Generator and the
- external world. In addition to the classical setter and getter for various parameters
+ external world. In addition to the classical setter and getter
+ for various parameters
there is the possibility to pass commands a string of stream to the method
\a ParseCmd().
@@ -85,30 +87,35 @@ namespace PatternGeneratorJRL
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/*! Constructor
@param strm: Should provide the file to initialize the preview control,
- the path to the VRML model, and the name of the file containing the VRML model.
+ the path to the VRML model, and the name of the file containing
+ the VRML model.
*/
PatternGeneratorInterfacePrivate(PinocchioRobot *aPinocchioRobotRobot);
/*! Destructor */
~PatternGeneratorInterfacePrivate();
- /*! \brief Function to specify steps in the stack of the walking pattern generator.
- This method is different AddOnLineStep which is the default step add when there is no policy,
- or no step available.
+ /*! \brief Function to specify steps in the stack of the walking pattern
+ generator.
+ This method is different AddOnLineStep which is the default step add when
+ there is no policy, or no step available.
*/
void AddStepInStack(double dx, double dy, double theta);
- /*! \name High levels function to create automatically stack of steps following specific motions.
+ /*! \name High levels function to create automatically stack of steps
+ following specific motions.
@{
*/
- /*! \brief This methods generate a stack of steps which make the robot follows an arc.
+ /*! \brief This methods generate a stack of steps which make the robot
+ follows an arc.
The direction of the robot is tangential to the arc.
@param[in] x: Position of the center of the circle along the X-axis.
@param[in] y: Position of the center of the circle along the Y-axis.
@param[in] R: Ray of the circle.
@param[in] arc_deg: Arc in degrees along which the robot walks.
- @param[in] SupportFoot: Indicates which is the first support foot (1) Left or (-1) Right.
+ @param[in] SupportFoot: Indicates which is the first support foot (1)
+ Left or (-1) Right.
*/
void CreateArcInStepStack( double x,
double y,
@@ -116,63 +123,79 @@ namespace PatternGeneratorJRL
double arc_deg,
int SupportFoot);
- /*! \brief This methods generate a stack of steps which make the robot follows an arc.
+ /*! \brief This methods generate a stack of steps which make the robot
+ follows an arc.
The direction of the robot is towards the center of the arc.
The robot is therefore expected to move sideways.
@param[in] R: Ray of the circle.
@param[in] arc_deg: Arc in degrees along which the robot walks.
- @param[in] SupportFoot: Indicates which is the first support foot (1) Left or (-1) Right.
+ @param[in] SupportFoot: Indicates which is the first support foot (1)
+ Left or (-1) Right.
*/
void CreateArcCenteredInStepStack( double R,
double arc_deg,
int SupportFoot);
- /*! \brief This specifies which foot will be used as the first support of the motion. */
+ /*! \brief This specifies which foot will be used as the first support
+ of the motion. */
void PrepareForSupportFoot(int SupportFoot);
- /*! \brief This method precomputes all the buffers necessary for walking according to the chosen strategy. */
+ /*! \brief This method precomputes all the buffers necessary for walking
+ according to the chosen strategy. */
void FinishAndRealizeStepSequence();
/*! @} */
/*! Common Initialization of walking.
- @param[out] lStartingCOMPosition: For the starting position on the articular space, returns
+ @param[out] lStartingCOMPosition: For the starting position on the
+ articular space, returns
the COM position.
- @param[out] lStartingZMPPosition: For the starting position on the articular space, returns
+ @param[out] lStartingZMPPosition: For the starting position on the
+ articular space, returns
the ZMP position.
- @param[out] BodyAnglesIni: Basically it is a copy of CurrentJointValues but as a vector.
- @param[out] InitLeftFootAbsPos: Returns the current absolute position of the left foot for
+ @param[out] BodyAnglesIni: Basically it is a copy of CurrentJointValues
+ but as a vector.
+ @param[out] InitLeftFootAbsPos: Returns the current absolute position
+ of the left foot for
the given posture of the robot.
- @param[out] InitRightFootAbsPos: Returns the current absolute position of the right foot
+ @param[out] InitRightFootAbsPos: Returns the current absolute position
+ of the right foot
for the given posture of the robot.
- @param[out] lRelativeFootPositions: List of relative positions for the support foot still in the
+ @param[out] lRelativeFootPositions: List of relative positions for
+ the support foot still in the
stack of steps.
- @param[in] lCurrentJointValues: The vector of articular values in classical C++ style.
+ @param[in] lCurrentJointValues: The vector of articular values in
+ classical C++ style.
@param[in] ClearStepStackHandler: Clean the stack of steps after copy.
*/
- void CommonInitializationOfWalking(COMState & lStartingCOMState,
- Eigen::Vector3d & lStartingZMPPosition,
- Eigen::VectorXd & BodyAnglesIni,
- FootAbsolutePosition & InitLeftFootAbsPos,
- FootAbsolutePosition & InitRightFootAbsPos,
- deque<RelativeFootPosition> & lRelativeFootPositions,
- std::vector<double> & lCurrentJointValues,
- bool ClearStepStackHandler);
+ void CommonInitializationOfWalking
+ (COMState & lStartingCOMState,
+ Eigen::Vector3d & lStartingZMPPosition,
+ Eigen::VectorXd & BodyAnglesIni,
+ FootAbsolutePosition & InitLeftFootAbsPos,
+ FootAbsolutePosition & InitRightFootAbsPos,
+ deque<RelativeFootPosition> & lRelativeFootPositions,
+ std::vector<double> & lCurrentJointValues,
+ bool ClearStepStackHandler);
/*! \name Methods for the control part.
@{
*/
- /*! \brief Run One Step of the global control loop aka The Main Method To Be Used.
- @param[out] CurrentConfiguration The current configuration of the robot according to
- the implementation of dynamic-JRLJapan. This should be first position and orientation
+ /*! \brief Run One Step of the global control loop aka The Main Method
+ To Be Used.
+ @param[out] CurrentConfiguration The current configuration of the robot
+ according to the implementation of Pinocchio.
+ This should be first position and orientation
of the waist, and then all the DOFs of your robot.
- @param[out] CurrentVelocity The current velocity of the robot according to the
- the implementation of dynamic-JRLJapan.
- @param[out] CurrentAcceleration The current acceleration of the robot according to the
- the implementation of dynamic-JRLJapan.
+ @param[out] CurrentVelocity The current velocity of the robot
+ according to the
+ the implementation of Pinocchio.
+ @param[out] CurrentAcceleration The current acceleration of the robot
+ according to the
+ the implementation of Pinocchio.
@param[out] ZMPTarget The target ZMP in the waist reference frame.
@return True is there is still some data to send, false otherwise.
*/
@@ -181,14 +204,19 @@ namespace PatternGeneratorJRL
Eigen::VectorXd & CurrentAcceleration,
Eigen::VectorXd & ZMPTarget);
- /*! \brief Run One Step of the global control loop aka The Main Method To Be Used.
- @param[out] CurrentConfiguration The current configuration of the robot according to
- the implementation of dynamic-JRLJapan. This should be first position and orientation
+ /*! \brief Run One Step of the global control loop aka The Main
+ Method To Be Used.
+ @param[out] CurrentConfiguration The current configuration of the robot
+ according to
+ the implementation of Pinocchio. This should be first position and
+ orientation
of the waist, and then all the DOFs of your robot.
- @param[out] CurrentVelocity The current velocity of the robot according to the
- the implementation of dynamic-JRLJapan.
- @param[out] CurrentAcceleration The current acceleration of the robot according to the
- the implementation of dynamic-JRLJapan.
+ @param[out] CurrentVelocity The current velocity of the robot according
+ to the
+ the implementation of Pinocchio.
+ @param[out] CurrentAcceleration The current acceleration of the robot
+ according to the
+ the implementation of Pinocchio.
@param[out] ZMPTarget The target ZMP in the waist reference frame.
@param[out] COMState The CoM state for this motion.
@param[out] LeftFootPosition: Absolute position of the left foot.
@@ -203,14 +231,19 @@ namespace PatternGeneratorJRL
FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition);
- /*! \brief Run One Step of the global control loop aka The Main Method To Be Used.
- @param[out] CurrentConfiguration The current configuration of the robot according to
- the implementation of dynamic-JRLJapan. This should be first position and orientation
+ /*! \brief Run One Step of the global control loop
+ aka The Main Method To Be Used.
+ @param[out] CurrentConfiguration The current configuration of the robot
+ according to
+ the implementation of Pinocchio. This should be first position and
+ orientation
of the waist, and then all the DOFs of your robot.
- @param[out] CurrentVelocity The current velocity of the robot according to the
- the implementation of dynamic-JRLJapan.
- @param[out] CurrentAcceleration The current acceleration of the robot according to the
- the implementation of dynamic-JRLJapan.
+ @param[out] CurrentVelocity The current velocity of the robot according
+ to the
+ the implementation of Pinocchio.
+ @param[out] CurrentAcceleration The current acceleration of the robot
+ according to the
+ the implementation of Pinocchio.
@param[out] ZMPTarget The target ZMP in the waist reference frame.
@param[out] aCOMPosition The CoM position for this motion.
@param[out] LeftFootPosition: Absolute position of the left foot.
@@ -226,7 +259,8 @@ namespace PatternGeneratorJRL
FootAbsolutePosition &RightFootPosition);
- /*! \brief Run One Step of the global control loop aka The Main Method To Be Used.
+ /*! \brief Run One Step of the global control loop aka
+ The Main Method To Be Used.
@param[out] LeftFootPosition: Absolute position of the left foot.
@param[out] RightFootPosition: Absolute position of the right foot.
@param[out] ZMPRefPos: ZMP position new reference
@@ -283,16 +317,20 @@ namespace PatternGeneratorJRL
/*! \brief Change online step.
The strategy is the following: the step in single support phase at time t
has its landing position changed to \f$ (X,Y,\theta) \f$ in absolute
- coordinates (i.e. in the world reference frame of the free flyer of the robot).
+ coordinates (i.e. in the world reference frame of the free flyer
+ of the robot).
For stability reason there is no guarantee that this method can
- realized the operation. Please see the documentation of the walking pattern generator
+ realized the operation. Please see the documentation of the walking
+ pattern generator
algorithm used.
- If the time falls during a double support phase, the next single support phase is chosen.
+ If the time falls during a double support phase, the next single support
+ phase is chosen.
@param[in] Time: Time information of the step.
@param[in] aFootAbsolutePosition: Absolute position of the foot.
- @return If the operation failed the method returns a negative number related
+ @return If the operation failed the method returns a negative number
+ related
to an error, 0 otherwise.
*/
int ChangeOnLineStep(double Time,
@@ -353,16 +391,19 @@ namespace PatternGeneratorJRL
@{
*/
- /*! \brief Parse a command (to be used out of a plugin) and call all objects which registered the method. */
+ /*! \brief Parse a command (to be used out of a plugin) and call all
+ objects which registered the method. */
int ParseCmd(std::istringstream &strm);
- /*! \brief This method register a method to a specific object which derivates from SimplePlugin class. */
+ /*! \brief This method register a method to a specific object which
+ derivates from SimplePlugin class. */
bool RegisterMethod(string &MethodName, SimplePlugin *aSP);
/*! @} */
- /*! \brief Returns the ZMP, CoM, left foot absolute position, and right foot absolute position
+ /*! \brief Returns the ZMP, CoM, left foot absolute position, and
+ right foot absolute position
for the initiale pose.*/
void EvaluateStartingState(COMState & lStartingCOMState,
Eigen::Vector3d & lStartingZMPPosition,
@@ -635,16 +676,19 @@ namespace PatternGeneratorJRL
/*! Constants
@{ */
- /*! Using Preview Control with 2 stages proposed by Shuuji Kajita in 2003. */
+ /*! Using Preview Control with 2 stages proposed by Shuuji Kajita in 2003.
+ */
static const int ZMPCOM_KAJITA_2003=1;
- /*! Using the preview control with 2 stages proposed by Pierre-Brice Wieber in 2006. */
+ /*! Using the preview control with 2 stages proposed by Pierre-Brice
+ Wieber in 2006. */
static const int ZMPCOM_WIEBER_2006=2;
/*! Using the analytical solution proposed by Morisawa in 2007. */
static const int ZMPCOM_MORISAWA_2007=3;
- /*! Using the QP constrained problem resolution proposed by Dimitrov in 2008. */
+ /*! Using the QP constrained problem resolution proposed by Dimitrov
+ in 2008. */
static const int ZMPCOM_DIMITROV_2008=4;
/*! Using the velocity referenced QP proposed by Herdt in 2010. */
@@ -657,7 +701,8 @@ namespace PatternGeneratorJRL
/*! Humanoid Dynamic robot */
PinocchioRobot * m_PinocchioRobot ;
- //CjrlHumanoidDynamicRobot * m_HumanoidDynamicRobot, * m_2HumanoidDynamicRobot;
+ //CjrlHumanoidDynamicRobot * m_HumanoidDynamicRobot, *
+ // m_2HumanoidDynamicRobot;
/*! Speed of the leg. */
Eigen::VectorXd m_dqr,m_dql;
@@ -716,7 +761,8 @@ namespace PatternGeneratorJRL
virtual void CallMethod(string &MethodName,
istringstream &istrm);
- /*! \brief Register the methods handled by the SimplePlugin part of this object. */
+ /*! \brief Register the methods handled by the SimplePlugin part of
+ this object. */
void RegisterPluginMethods();
/*! \brief Start FPE trapping. */
@@ -733,12 +779,14 @@ namespace PatternGeneratorJRL
as well as Upper Body Positions. */
void ExpandCOMPositionsQueues(int aNumber);
- /*! \brief Compute the COM, left and right foot position for a given BodyAngle position */
+ /*! \brief Compute the COM, left and right foot position for a
+ given BodyAngle position */
void EvaluateStartingCOM(Eigen::VectorXd &Configuration,
Eigen::Vector3d &lStartingCOMPosition);
- /*! \brief Fill the internal buffer with the appropriate information depending on the strategy.
+ /*! \brief Fill the internal buffer with the appropriate information
+ depending on the strategy.
The behavior of this method depends on \a m_AlgorithmforZMPCOM.
*/
int CreateZMPReferences(deque<RelativeFootPosition> &lRelativeFootPositions,
@@ -747,7 +795,8 @@ namespace PatternGeneratorJRL
FootAbsolutePosition & InitLeftFootAbsPos,
FootAbsolutePosition & InitRightFootAbsPos);
- /*! \brief Create automatically a new step for a ZMP based stability criteria */
+ /*! \brief Create automatically a new step for a ZMP based stability
+ criteria */
void AutomaticallyAddFirstStep(deque<RelativeFootPosition> &
lRelativeFootPositions,
FootAbsolutePosition & InitLeftFootAbsPos,