Add a TimeParameterization to paths so that the velocity does not exceeds the velocity limits. More...
#include <hpp/core/path-optimization/simple-time-parameterization.hh>
Public Member Functions | |
virtual PathVectorPtr_t | optimize (const PathVectorPtr_t &path) |
Optimize path. More... | |
![]() | |
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) | |
![]() | |
PathOptimizer (const Problem &problem) | |
PathPtr_t | steer (ConfigurationIn_t q1, ConfigurationIn_t q2) const |
Additional Inherited Members | |
![]() | |
bool | interrupt_ |
Whether to interrupt computation Set to false at start of optimize method, set to true by method interrupt. More... | |
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:
The solutions are:
\[ \begin{align*} a_0 &= s_0 \\ a_1 &= B \\ T &= \frac{s_1 - s_0}{B} \\ \end{align*} \]
\[ \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*} \]
|
protected |
|
static |
Return shared pointer to new object.
|
virtual |
Optimize path.
Implements hpp::core::PathOptimizer.