hpp-core: hpp::core::pathOptimization::SimpleTimeParameterization Class Reference

hpp-core 9.0.2 Implement basic classes for canonical path planning for kinematic chains.

Loading...
Searching...
No Matches
Public Member Functions | Static Public Member Functions | Public Attributes | Protected Member Functions | List of all members
hpp::core::pathOptimization::SimpleTimeParameterization Class Reference
Path Optimization

`#include <hpp/core/path-optimization/simple-time-parameterization.hh>`

Inheritance diagram for hpp::core::pathOptimization::SimpleTimeParameterization:
[legend]
Collaboration diagram for hpp::core::pathOptimization::SimpleTimeParameterization:
[legend]

## Public Member Functions
virtual PathVectorPtr_t | optimize (const PathVectorPtr_t &path)
| Optimize path.
Public Member Functions inherited from hpp::core::PathOptimizer
virtual | ~PathOptimizer ()
ProblemConstPtr_t | problem () const
| Get problem.
void | interrupt ()
| Interrupt path optimization.
void | maxIterations (const unsigned long int &n)
| Set maximal number of iterations.
void | timeOut (const double &timeOut)
| set time out (in seconds)

## Static Public Member Functions
static SimpleTimeParameterizationPtr_t | create (const ProblemConstPtr_t &problem)
| Return shared pointer to new object.

## Public Attributes
value_type | safety
| A scaling factor for the velocity bounds.
size_type | order
| The desired continuity order (0, 1, or 2).
value_type | maxAcceleration

## Protected Member Functions
| SimpleTimeParameterization (const ProblemConstPtr_t &problem)
Protected Member Functions inherited from hpp::core::PathOptimizer
| PathOptimizer (const ProblemConstPtr_t &problem)
PathPtr_t | steer (ConfigurationIn_t q1, ConfigurationIn_t q2) const
void | monitorExecution ()
void | endIteration ()
bool | shouldStop () const
void | initFromParameters ()

## Additional Inherited Members
Protected Attributes inherited from hpp::core::PathOptimizer
bool | interrupt_

## Detailed Description

Add a TimeParameterization to paths so that the velocity does not exceeds the velocity limits.

Parameter SimpleTimeParameterization/safety (value_type) defines is used to rescale the velocity limit.

Parameter SimpleTimeParameterization/velocity (bool) defines whether to use first or third order Polynomial.

Let the initial path be

\[ \begin{align*} q \colon [s_0, s_1] & \to \mathcal{C} \\ s & \mapsto q(s) \end{align*} \]

We want to find a increasing Polynomial

\[ \begin{align*} P_n \colon [0, T] & \to [s_0, s_1] \\ t & \mapsto s \end{align*} \]

where \(n\) is the polynom degree. The time parameterized path should satisfy the velocity limits so:

\[ l_i \le \dot q_i (P_n(t)) = P'_n(t) \frac{dq_i }{ds} (P_n(t)) \le u_i \\ P'_n(t) | \frac{dq_i }{ds} (P_n(t)) | \le \min u_i, -l_i \\ \]

where \( u_i > 0 \) and \( l_i < 0 \) are the velocity limits.

Let \( v \) be a velocity bound (Path::velocityBound) of \( q \) on \( [s_0, s_1] \). Then \( P'_n(t) \le B \) where

\[ B = \min_i \frac{u_i}{v_i}, \frac{-l_i}{v_i} \]

The constraints on \( P_n \) are:

- \( P_n(0) = s_0 \)
- \( P_n(T) = s_1 \)
- \( P^{(1)}_n(t) \le B \)
- \( P^{(1)}_n(0) = 0 \) (from the third order case only)
- \( P^{(1)}_n(T) = 0 \) (from the third order case only)
- \( P^{(2)}_n(0) = 0 \) (from the fifth order case only)
- \( P^{(2)}_n(T) = 0 \) (from the fifth order case only)
- \( |P^{(2)}_n(t)| \le C \) (from the fifth order case only)

The solutions are:

- First order case:

  \[ \begin{align*} a_0 &= s_0 \\ a_1 &= B \\ T &= \frac{s_1 - s_0}{B} \\ \end{align*} \]

- Third order case:

  \[ \begin{align*} T &= 3 \frac{s_1 - s_0}{2 B} \\ a_0 &= s_0 \\ a_1 &= 0 \\ a_2 &= 3 \frac{s_1 - s_0}{T^2} \\ a_3 &= - \frac{2 a_2}{3 T} \\ \end{align*} \]

  In this case, \( P^{(1)}_3(t) = 6 \frac{s_1-s_0}{T} \frac{t}{T} \left(1 - \frac{t}{T}\right) \ge 0 \).

- Fifth order case:
  Let \( P_5(t) = \sum_i a_i t^i \). Trivially, \( a_0 = s_0 \), \( a_1 = 0 \) and \( a_2 = 0 \).
  Then, \( P_5(T) = s_1 \), \( P^{(1)}_5(T) = 0 \) and \( P^{(2)}_5(T) = 0 \) implies:

  \[ \left(\begin{array}{ccc} T^3 & T^4 & T^5 \\ 3 T^2 & 4 T^3 & 5 T^4 \\ 6 T & 12 T^2 & 20 T^3 \\ \end{array}\right) \times \left(\begin{array}{c} a_3 \\ a_4 \\ a_5 \\ \end{array}\right) = \left(\begin{array}{c} s_1 - s_0 \\ 0 \\ 0 \\ \end{array}\right) \]

  \[ \left(\begin{array}{c} a_3 \\ a_4 \\ a_5 \\ \end{array}\right) = \frac{s_1-s_0}{\det(M)} \times \left(\begin{array}{c} 80 T^6 - 60 T^6 \\ 30 T^5 - 60 T^5 \\ 36 T^4 - 24 T^4 \\ \end{array}\right) \]

  And thus:

  \[ \left(\begin{array}{c} a_3 \\ a_4 \\ a_5 \\ \end{array}\right) = \frac{s_1-s_0}{T^5} \times \left(\begin{array}{c} 10 T^2 \\ -15 T \\ 6 \\ \end{array}\right) \]

  Then \( P_5^{(1)}(t) = 30 \frac{s_1-s_0}{T} \left(\frac{t}{T}\right)^2 \left(1 - \frac{t}{T}\right)^2 \ge 0 \) and \( P_5^{(2)}(t) = 60 \frac{s_1-s_0}{T^2} \left(\frac{t}{T}\right) \left(1 - \frac{t}{T}\right) \left(1 - 2\frac{t}{T}\right) \). We have \( \max{P_5^{(1)}(t)} = P_5^{(1)}(\frac{T}{2}) = \frac{30 (s_1 - s_0)}{16 T} \). Let us compute \( \max{P_5^{(2)}(t)} \).

Let \( q(u) = 2u (u+\frac{1}{2})(u-\frac{1}{2})\). Then \( P_5^{(2)}(t) = 60 \frac{s_1-s_0}{T^2} q(\frac{t}{T}-\frac{1}{2}) \). For \( u \in [0,\frac{1}{2}]\), we have \( q(-u) = -q(u) \) and \( |q(u)| = -q(u) \le -q(\frac{1}{2\sqrt{3}}) = \frac{1}{6\sqrt{3}}\). So

\[ \max{P_5^{(2)}(t)} = \frac{10}{\sqrt{3}} \frac{s_1-s_0}{T^2} \]

So

\[ \begin{align*} \frac{15 (s_1 - s_0)}{8 T} &\le B \\ \frac{10}{\sqrt{3}} \frac{s_1-s_0}{T^2} &\le C \\ \end{align*} \]

\[ T = \max{ \left\{ \frac{15 (s_1 - s_0)}{8 B}, \sqrt{\frac{10 (s_1 - s_0)}{\sqrt{3} B}} \right\} } \]

## Constructor & Destructor Documentation

## ◆ SimpleTimeParameterization()

hpp::core::pathOptimization::SimpleTimeParameterization::SimpleTimeParameterization | ( | const ProblemConstPtr_t & | problem | ) |
| protected

## Member Function Documentation

## ◆ create()

static SimpleTimeParameterizationPtr_t hpp::core::pathOptimization::SimpleTimeParameterization::create | ( | const ProblemConstPtr_t & | problem | ) |
| static

Return shared pointer to new object.

## ◆ optimize()

virtual PathVectorPtr_t hpp::core::pathOptimization::SimpleTimeParameterization::optimize | ( | const PathVectorPtr_t & | path | ) |
| virtual

Optimize path.

Implements hpp::core::PathOptimizer.

## Member Data Documentation

## ◆ maxAcceleration

value_type hpp::core::pathOptimization::SimpleTimeParameterization::maxAcceleration

The maximum acceleration for each degree of freedom. Not considered if negative.

## ◆ order

size_type hpp::core::pathOptimization::SimpleTimeParameterization::order

The desired continuity order (0, 1, or 2).

## ◆ safety

value_type hpp::core::pathOptimization::SimpleTimeParameterization::safety

A scaling factor for the velocity bounds.

The documentation for this class was generated from the following file:

- include/hpp/core/path-optimization/simple-time-parameterization.hh