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

Inheritance diagram for hpp::core::pathOptimization::SimpleTimeParameterization:

Collaboration diagram for hpp::core::pathOptimization::SimpleTimeParameterization:

Public Member Functions
virtual PathVectorPtr_t	optimize (const PathVectorPtr_t &path)
	Optimize path. More...

Public Member Functions inherited from hpp::core::PathOptimizer
virtual	~PathOptimizer ()

const Problem &	problem () const
	Get problem. More...

void	interrupt ()
	Interrupt path optimization. More...

Static Public Member Functions
static SimpleTimeParameterizationPtr_t	create (const Problem &problem)
	Return shared pointer to new object. More...

Protected Member Functions
	SimpleTimeParameterization (const Problem &problem)

Protected Member Functions inherited from hpp::core::PathOptimizer
	PathOptimizer (const Problem &problem)

PathPtr_t	steer (ConfigurationIn_t q1, ConfigurationIn_t q2) const

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 Problem & problem )

protected

Member Function Documentation

◆ create()

static SimpleTimeParameterizationPtr_t hpp::core::pathOptimization::SimpleTimeParameterization::create ( const Problem & 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.

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

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

Public Member Functions

Static Public Member Functions