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

SimpleTimeParameterization can enforce continuous acceleration.

parent 26611a19
......@@ -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
......
......@@ -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