Commit 5001c83a by Joseph Mirabel Committed by Joseph Mirabel

### SimpleTimeParameterization accepts acceleration limit.

parent 7f3ebfc9
 ... ... @@ -66,6 +66,7 @@ namespace hpp { * - \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) * - \f$|P^{(2)}_n(t)| \le C \f$ (from the fifth order case only) * * The solutions are: * \li First order case: ... ... @@ -141,13 +142,34 @@ namespace hpp { * 6 \\ * \end{array}\right) * \f] * Then \f$P^{(1)}(t) = 30 \frac{s_1-s_0}{T} * Then \f$ 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 \f$so * \left(1 - \frac{t}{T}\right)^2 \ge 0 \f$ and * \f$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) \f$. * We have \f$\max{P_5^{(1)}(t)} = P_5^{(1)}(\frac{T}{2}) = \frac{30 (s_1 - s_0)}{16 T} \f$. * Let us compute \f$\max{P_5^{(2)}(t)} \f$. * * Let \f$q(u) = 2u (u+\frac{1}{2})(u-\frac{1}{2})\f$. * Then \f$P_5^{(2)}(t) = 60 \frac{s_1-s_0}{T^2} q(\frac{t}{T}-\frac{1}{2}) \f$. * For \f$u \in [0,\frac{1}{2}]\f$, we have \f$q(-u) = -q(u) \f$ and * \f$|q(u)| = -q(u) \le -q(\frac{1}{2\sqrt{3}}) = \frac{1}{6\sqrt{3}}\f$. So * \f[ \max{P_5^{(2)}(t)} = \frac{10}{\sqrt{3}} \frac{s_1-s_0}{T^2} \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} * \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*} \f] * * \f[ T = \max{ * \left\{ * \frac{15 (s_1 - s_0)}{8 B}, * \sqrt{\frac{10 (s_1 - s_0)}{\sqrt{3} B}} * \right\} * } \f] */ class HPP_CORE_DLLAPI SimpleTimeParameterization : public PathOptimizer ... ...
 ... ... @@ -36,7 +36,7 @@ namespace hpp { namespace { TimeParameterizationPtr_t computeTimeParameterizationFirstOrder ( const value_type& s0, const value_type& s1, const value_type& B, value_type& T) value_type& T) { vector_t a(2); T = (s1 - s0) / B; ... ... @@ -49,7 +49,7 @@ namespace hpp { TimeParameterizationPtr_t computeTimeParameterizationThirdOrder ( const value_type& s0, const value_type& s1, const value_type& B, value_type& T) value_type& T) { vector_t a(4); T = 3 * (s1 - s0) / (2 * B); ... ... @@ -64,10 +64,18 @@ namespace hpp { TimeParameterizationPtr_t computeTimeParameterizationFifthOrder ( const value_type& s0, const value_type& s1, const value_type& B, value_type& T) const value_type& C, value_type& T) { vector_t a(6); T = 30 * (s1 - s0) / (16 * B); if (C > 0) { T = std::max( 15 * (s1 - s0) / (8 * B) , // Velocity limit sqrt((10./std::sqrt(3))) * sqrt((s1 - s0) / C) // Acceleration limit ); } else { T = 15 * (s1 - s0) / (8 * B); } value_type Tpow = T*T*T; a[0] = s0; a[1] = 0; ... ... @@ -83,10 +91,10 @@ namespace hpp { } void checkTimeParameterization (const TimeParameterizationPtr_t tp, const size_type order, 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& C, const value_type T) { using std::fabs; const value_type thr = Eigen::NumTraits::dummy_precision(); const value_type thr = std::sqrt(Eigen::NumTraits::dummy_precision()); if ( fabs(tp->value(0) - sr.first) >= thr || fabs(tp->value(T) - sr.second) >= thr) { throw std::logic_error("Boundaries of TimeParameterization are not correct."); ... ... @@ -94,7 +102,7 @@ namespace hpp { if (order >= 1 && (fabs(tp->derivative(0, 1)) > thr || fabs(tp->derivative(T, 1)) > thr || fabs(tp->derivative(T/2, 1) - B) > thr) || ((C <= 0) && fabs(tp->derivative(T/2, 1) - B) > thr)) ) { HPP_THROW(std::logic_error, "Derivative of TimeParameterization are not correct:" ... ... @@ -103,6 +111,7 @@ namespace hpp { << "\ntp->derivative(T/2, 1) - B = " << tp->derivative(T/2, 1) - B << "\nT = " << T << "\nB = " << B << "\nC = " << C ); } if (order >= 2 ... ... @@ -131,6 +140,10 @@ namespace hpp { const value_type safety = problem().getParameter("SimpleTimeParameterization/safety").floatValue(); const size_type order = problem().getParameter("SimpleTimeParameterization/order").intValue(); const value_type maxAcc = problem().getParameter("SimpleTimeParameterization/maxAcceleration").floatValue(); if (order <= 1 && maxAcc) { throw std::invalid_argument ("Maximum acceleration cannot be set when order is <= to 1. Please set parameter SimpleTimeParameterization/maxAcceleration to a negative value."); } // Retrieve velocity limits const DevicePtr_t& robot = problem().robot(); ... ... @@ -186,14 +199,14 @@ namespace hpp { tp = computeTimeParameterizationThirdOrder (paramRange.first, paramRange.second, B, T); break; case 2: tp = computeTimeParameterizationFifthOrder (paramRange.first, paramRange.second, B, T); tp = computeTimeParameterizationFifthOrder (paramRange.first, paramRange.second, B, maxAcc, T); break; default: throw std::invalid_argument ("Parameter SimpleTimeParameterization/order should be in { 0, 1, 2 }"); break; } checkTimeParameterization (tp, order, paramRange, B, T); checkTimeParameterization (tp, order, paramRange, B, maxAcc, T); PathPtr_t pp = p->copy(); pp->timeParameterization (tp, interval_t (0, T)); ... ... @@ -217,6 +230,10 @@ namespace hpp { "SimpleTimeParameterization/order", "The desired continuity order.", Parameter((size_type)0))); Problem::declareParameter(ParameterDescription (Parameter::FLOAT, "SimpleTimeParameterization/maxAcceleration", "The maximum acceleration for each degree of freedom. Not considered if negative.", Parameter((value_type)-1))); HPP_END_PARAMETER_DECLARATION(SimpleTimeParameterization) } // namespace pathOptimization } // namespace core ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!