Commit 5001c83a authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

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<value_type>::dummy_precision();
const value_type thr = std::sqrt(Eigen::NumTraits<value_type>::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!
Please register or to comment