diff --git a/TODO.txt b/TODO.txt index dd8d56b526651ed184ef92aac47abff26f00f3fa..226c753d043d130ecb9c707f19a0f3003bd2ff14 100644 --- a/TODO.txt +++ b/TODO.txt @@ -17,7 +17,7 @@ hard-coded optimizer (Graph-RandomShortcut) and then globally with the command ps.addPathOptimizer("Graph-RandomShortcut"). Due to how this works, this is like doing two times the same thing so the first should be removed. - However bugs have been observed when the global optimization is used (Core + However bugs have been observed when the global optimization is used (Core dumped). For unknown reasons, in GraphOptimizer::optimize, the dynamic cast of current->constraints() may fail on *some* 0-length paths. Ignoring empty paths (the "if" I have added) seems to make the problem disappear diff --git a/doc/Doxyfile.extra.in b/doc/Doxyfile.extra.in index 9ca64de3f434a2e125f080fe33b7d42657e5d5f8..233aa4ae5d6e4fe843bacd644dc9d21728247fc7 100644 --- a/doc/Doxyfile.extra.in +++ b/doc/Doxyfile.extra.in @@ -8,3 +8,5 @@ ALIASES += Link{1}="\ref \1" ALIASES += Link{2}="\ref \1 \"\2\"" ALIASES += LHPP{2}="\Link{hpp::\1::\2,\2}" ALIASES += LPinocchio{1}="\LHPP{pinocchio,\1}" + +USE_MATHJAX=YES diff --git a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh index 6454a1b134c08326e33115ceef823fd6ab2f4ae9..e99c498915d5c41ee243cc9c01d3ec52eb404449 100644 --- a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh +++ b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh @@ -56,10 +56,36 @@ class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory return ptr; } - /// Build a trajectory in SE(3). - /// \param points a Nx7 matrix whose rows corresponds to a pose. - /// \param weights a 6D vector, weights to be applied when computing - /// the distance between two SE3 points. + /** Build a trajectory in SE(3). + \param points a Nx7 matrix whose rows corresponds to poses. + \param weights a 6D vector, weights to be applied when computing + the distance between two SE3 points. + + The trajectory \f$T\f$ is defined as follows. Let \f$N\f$ be the number of + lines of matrix \c points, \f$p_i\f$ be the i-th line of \c points and + let \f$W\f$ be the + diagonal matrix with the coefficients of \c weights: + \f[ + W = \left(\begin{array}{cccccc} + w_1 & 0 & 0 & 0 & 0 & 0\\ + 0 & w_2 & 0 & 0 & 0 & 0\\ + 0 & 0 & w_3 & 0 & 0 & 0\\ + 0 & 0 & 0 & w_4 & 0 & 0\\ + 0 & 0 & 0 & 0 & w_5 & 0\\ + 0 & 0 & 0 & 0 & 0 & w_6\\ + \end{array}\right) + \f] + + \f{eqnarray*}{ + f(t) = \mathbf{p}_i \oplus \frac{t-t_i}{t_{i+1}-t_i} (\mathbf{p}_{i+1}-\mathbf{p}_i) && + \mbox{ for } t \in [t_i,t_{i+1}] + \f} + + where \f$t_0 = 0\f$ and + \f{eqnarray*}{ + t_{i+1}-t_i = \|W(\mathbf{p}_{i+1}-\mathbf{p}_i)\| && \mbox{for } i + \mbox{ such that } 1 \leq i \leq N-1 + \f} */ static PathPtr_t makePiecewiseLinearTrajectory(matrixIn_t points, vectorIn_t weights);