From a8e018a1c42ebf365cc05f3ffdb829b95e3d3415 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Fri, 7 Jul 2023 14:55:06 +0200
Subject: [PATCH] [steeringMethod::EndEffectorTrajectory] Improve
documentation.
---
TODO.txt | 2 +-
doc/Doxyfile.extra.in | 2 ++
.../end-effector-trajectory.hh | 34 ++++++++++++++++---
3 files changed, 33 insertions(+), 5 deletions(-)
diff --git a/TODO.txt b/TODO.txt
index dd8d56b5..226c753d 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 9ca64de3..233aa4ae 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 6454a1b1..e99c4989 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);
--
GitLab