Skip to content

GitLab

Commit b74e2204 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

SimpleTimeParameterization can enforce continuous acceleration.

parent 26611a19
Hide whitespace changes
Inline Side-by-side
include/hpp/core/path-optimization/simple-time-parameterization.hh
View file @ b74e2204
......@@ -61,9 +61,11 @@ namespace hpp {
* The constraints on \f$ P_n \f$ are:
* - \f$ P_n(0) = s_0 \f$
* - \f$ P_n(T) = s_1 \f$
* - \f$ P'_n(t) \le B \f$
* - \f$ P'_n(0) = 0 \f$ (in the third order case only)
* - \f$ P'_n(T) = 0 \f$ (in the third order case only)
* - \f$ P^{(1)}_n(t) \le B \f$
* - \f$ P^{(1)}_n(0) = 0 \f$ (from the third order case only)
* - \f$ P^{(1)}_n(T) = 0 \f$ (from the third order case only)
* - \f$ P^{(2)}_n(0) = 0 \f$ (from the fifth order case only)
* - \f$ P^{(2)}_n(T) = 0 \f$ (from the fifth order case only)
*
* The solutions are:
* \li First order case:
......@@ -81,6 +83,71 @@ namespace hpp {
* a_2 &= 3 \frac{s_1 - s_0}{T^2} \\
* a_3 &= - \frac{2 a_2}{3 T} \\
* \end{align*} \f]
* In this case, \f$ P^{(1)}_3(t) = 6 \frac{s_1-s_0}{T} \frac{t}{T}
* \left(1 - \frac{t}{T}\right) \ge 0 \f$.
*
* \li Fifth order case:\n
* Let \f$ P_5(t) = \sum_i a_i t^i \f$.
* Trivially, \f$ a_0 = s_0 \f$, \f$ a_1 = 0 \f$ and \f$ a_2 = 0 \f$. \n
* Then, \f$ P_5(T) = s_1 \f$, \f$ P^{(1)}_5(T) = 0 \f$ and
* \f$ P^{(2)}_5(T) = 0 \f$ implies:
* \f[
* \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)
* \f]
* \f[
* \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)
* \f]
* And thus:
* \f[
* \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)
* \f]
* Then \f$ P^{(1)}(t) = 30 \frac{s_1-s_0}{T}
* \left(\frac{t}{T}\right)^2
* \left(1 - \frac{t}{T}\right)^2 \ge 0 \f$ so
* \f[ \begin{align*}
* B &= \max{P^{(1)}(t)} = P^{(1)}(\frac{T}{2}) = \frac{30 (s_1 - s_0)}{16 T} \\
* T &= \frac{30 (s_1 - s_0)}{16 B}
* \end{align*} \f]
*/
class HPP_CORE_DLLAPI SimpleTimeParameterization : public PathOptimizer
......
src/path-optimization/simple-time-parameterization.cc
View file @ b74e2204
......@@ -62,8 +62,28 @@ namespace hpp {
return TimeParameterizationPtr_t (new Polynomial (a));
}
TimeParameterizationPtr_t computeTimeParameterizationFifthOrder (
const value_type& s0, const value_type& s1, const value_type& B,
value_type& T)
{
vector_t a(6);
T = 30 * (s1 - s0) / (16 * B);
value_type Tpow = T*T*T;
a[0] = s0;
a[1] = 0;
a[2] = 0;
a[3] = 10 * (s1 - s0) / Tpow;
Tpow *= T;
a[4] = -15 * (s1 - s0) / Tpow;
Tpow *= T;
a[5] = 6 * (s1 - s0) / Tpow;
hppDout (info, "Time parametrization returned " << a.transpose()
<< ", " << T);
return TimeParameterizationPtr_t (new Polynomial (a));
}
void checkTimeParameterization (const TimeParameterizationPtr_t tp,
const bool velocity, const interval_t sr, const value_type B, const value_type T)
const size_type order, const interval_t sr, const value_type B, const value_type T)
{
using std::fabs;
const value_type thr = Eigen::NumTraits<value_type>::dummy_precision();
......@@ -71,7 +91,7 @@ namespace hpp {
|| fabs(tp->value(T) - sr.second) >= thr) {
throw std::logic_error("Boundaries of TimeParameterization are not correct.");
}
if (velocity
if (order >= 1
&& (fabs(tp->derivative(0, 1)) > thr
|| fabs(tp->derivative(T, 1)) > thr
|| fabs(tp->derivative(T/2, 1) - B) > thr)
......@@ -85,6 +105,17 @@ namespace hpp {
<< "\nB = " << B
);
}
if (order >= 2
&& (fabs(tp->derivative(0, 2)) > thr
|| fabs(tp->derivative(T, 2)) > thr)
) {
HPP_THROW(std::logic_error,
"Derivative of TimeParameterization are not correct:"
<< "\ntp->derivative(0, 2) = " << tp->derivative(0, 2)
<< "\ntp->derivative(T, 2) = " << tp->derivative(T, 2)
<< "\nT = " << T
);
}
}
}
......@@ -99,7 +130,7 @@ namespace hpp {
const value_type infinity = std::numeric_limits<value_type>::infinity();
const value_type safety = problem().getParameter("SimpleTimeParameterization/safety", (value_type)1);
const bool velocity = problem().getParameter("SimpleTimeParameterization/velocity", false);
const size_type order = problem().getParameter("SimpleTimeParameterization/order", (size_type)0);
// Retrieve velocity limits
const DevicePtr_t& robot = problem().robot();
......@@ -147,12 +178,22 @@ namespace hpp {
// Compute the polynom and total time
value_type T;
TimeParameterizationPtr_t tp;
if (velocity)
tp = computeTimeParameterizationThirdOrder (paramRange.first, paramRange.second, B, T);
else
tp = computeTimeParameterizationFirstOrder (paramRange.first, paramRange.second, B, T);
switch (order) {
case 0:
tp = computeTimeParameterizationFirstOrder (paramRange.first, paramRange.second, B, T);
break;
case 1:
tp = computeTimeParameterizationThirdOrder (paramRange.first, paramRange.second, B, T);
break;
case 2:
tp = computeTimeParameterizationFifthOrder (paramRange.first, paramRange.second, B, T);
break;
default:
throw std::invalid_argument ("Parameter SimpleTimeParameterization/order should be in { 0, 1, 2 }");
break;
}
checkTimeParameterization (tp, velocity, paramRange, B, T);
checkTimeParameterization (tp, order, paramRange, B, T);
PathPtr_t pp = p->copy();
pp->timeParameterization (tp, interval_t (0, T));
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment