Commit b74e2204 by Joseph Mirabel Committed by Joseph Mirabel

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::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::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