diff --git a/doc/rcfs.pdf b/doc/rcfs.pdf
index dfc85ea92f1683a19f68595fbd48ea552398ce2a..c61f486fad35a89c1ddabb8eb46155ed1555dd85 100644
Binary files a/doc/rcfs.pdf and b/doc/rcfs.pdf differ
diff --git a/doc/rcfs.tex b/doc/rcfs.tex
index e1f06259e99040adfe0ca70225e6e7eb443b1e9f..1664acca51b3270d1eba27ba73f76765d18e1bae 100644
--- a/doc/rcfs.tex
+++ b/doc/rcfs.tex
@@ -356,21 +356,21 @@ By starting from an initial estimate $\bm{x}_1$ and by recursively refining the
%https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm (section Derivation from Newton's method)
%http://homepages.laas.fr/nmansard/teach/robotics2015/textbook_draft.pdf
-The Gauss--Newton algorithm is a special case of Newton's method in which the cost is quadratic (sum of squared function values), with $c(\bm{x})=\sum_{i=1}^R r_i^2(\bm{x})=\bm{r}^\trsp\bm{r}=\|r\|^2$, where $\bm{r}$ are residual vectors. We neglect in this case the second-order derivative terms of the Hessian. The gradient and Hessian can in this case be computed with
+The Gauss--Newton algorithm is a special case of Newton's method in which the cost is quadratic (sum of squared function values), with the scalar cost $c(\bm{x})=\|\bm{f}(\bm{x})\|^2=\bm{f}(\bm{x})^\trsp\bm{f}(\bm{x})=\sum_{i=1}^R f_i^2(\bm{x})$, where $\bm{f}(\bm{x})\in\mathbb{R}^R$ is a residual vector. By neglecting the second-order derivative terms, the gradient and Hessian can be computed with
\begin{equation*}
- \bm{g} = 2 \bm{J}_{\bm{r}}^\trsp \bm{r} ,\quad
- \bm{H} \approx 2 \bm{J}_{\bm{r}}^\trsp \bm{J}_{\bm{r}},
+ \bm{g}(\bm{x}) = 2 \bm{J}(\bm{x})^\trsp \bm{f}(\bm{x}) ,\quad
+ \bm{H}(\bm{x}) \approx 2 \bm{J}(\bm{x})^\trsp \bm{J}(\bm{x}),
\end{equation*}
-where $\bm{J}_{\bm{r}}\in\mathbb{R}^{R\times D}$ is the Jacobian matrix of $\bm{r}\in\mathbb{R}^R$. This definition of the Hessian matrix makes it positive definite, which is useful to solve minimization problems as for well conditioned Jacobian matrices, we do not need to verify the positive definiteness of the Hessian matrix at each iteration.
+where $\bm{J}(\bm{x})\in\mathbb{R}^{R\times D}$ is the Jacobian matrix of $\bm{f}(\bm{x})$. This definition of the Hessian matrix makes it positive definite, which is useful to solve minimization problems as for well conditioned Jacobian matrices, we do not need to verify the positive definiteness of the Hessian matrix at each iteration.
The update rule in \eqref{eq:Taylor_nD_update} then becomes
\begin{align}
- \bm{x}_{k+1} &= \bm{x}_k - {\big(\bm{J}_{\bm{r}}^\trsp(\bm{x}_k) \bm{J}_{\bm{r}}(\bm{x}_k)\big)}^{-1} \,
- \bm{J}_{\bm{r}}^\trsp(\bm{x}_k) \, \bm{r}(\bm{x}_k) \nonumber\\
- &= \bm{x}_k - \bm{J}_{\bm{r}}^\psin(\bm{x}_k) \, \bm{r}(\bm{x}_k),
+ \bm{x}_{k+1} &= \bm{x}_k - {\big(\bm{J}^\trsp(\bm{x}_k) \bm{J}(\bm{x}_k)\big)}^{-1} \,
+ \bm{J}^\trsp(\bm{x}_k) \, \bm{f}(\bm{x}_k) \nonumber\\
+ &= \bm{x}_k - \bm{J}^\psin(\bm{x}_k) \, \bm{f}(\bm{x}_k),
\label{eq:GaussNewtonUpdate}
\end{align}
-where $\bm{J}_{\bm{r}}^\psin$ denotes the pseudoinverse of $\bm{J}_{\bm{r}}$.
+where $\bm{J}^\psin$ denotes the pseudoinverse of $\bm{J}$.
The Gauss--Newton algorithm is the workhorse of many robotics problems, including inverse kinematics and optimal control, as we will see in the next sections.
@@ -391,7 +391,7 @@ deriving $c(\bm{x})$ w.r.t.\ $\bm{x}$ and equating to zero yields
\end{equation}
which corresponds to the standard analytic least squares estimate. We will see later in the inverse kinematics section that for the complete solution can also include a nullspace structure.
-In contrast to the Gauss--Newton algorithm presented in Section \ref{sec:GaussNewton}, the optimization problem in \eqref{eq:costquadratic}, described by a quadratic cost on the decision variable $\bm{x}$, admits the solution \eqref{eq:costquadraticsolution} that can be computed directly without relying on an iterative procedure. We can observe that if we follow the Gauss--Newton procedure, the residual vector corresponding to the cost \eqref{eq:costquadratic} is $\bm{r}=\bm{A}\bm{x}-\bm{b}$ and its Jacobian matrix is $\bm{J}_{\bm{r}}=\bm{A}$. By starting from an initial estimate $\bm{x}_0$, the Gauss--Newton update in \eqref{eq:GaussNewtonUpdate} then takes the form
+In contrast to the Gauss--Newton algorithm presented in Section \ref{sec:GaussNewton}, the optimization problem in \eqref{eq:costquadratic}, described by a quadratic cost on the decision variable $\bm{x}$, admits the solution \eqref{eq:costquadraticsolution} that can be computed directly without relying on an iterative procedure. We can observe that if we follow the Gauss--Newton procedure, the residual vector corresponding to the cost \eqref{eq:costquadratic} is $\bm{f}=\bm{A}\bm{x}-\bm{b}$ and its Jacobian matrix is $\bm{J}=\bm{A}$. By starting from an initial estimate $\bm{x}_0$, the Gauss--Newton update in \eqref{eq:GaussNewtonUpdate} then takes the form
\begin{align*}
\bm{x}_{k+1} &= \bm{x}_k - \bm{A}^\psin (\bm{A}\bm{x}_k-\bm{b})\\
&= \bm{x}_k - (\bm{A}^\trsp \bm{A})^{-1} \bm{A}^\trsp (\bm{A}\bm{x}_k-\bm{b})\\
@@ -501,7 +501,7 @@ Forward kinematics for a planar robot with two links.
The \emph{forward kinematics} (FK) function of a planar robot manipulator is defined as
\begin{align*}
- \bm{f} &= \begin{bmatrix} \bm{\ell}^\trsp \cos(\bm{L}\bm{x}) \\ \bm{\ell}^\trsp \sin(\bm{L}\bm{x}) \\ \bm{1}^\trsp \bm{x} \end{bmatrix} \\
+ \bm{f}^\tp{ee} &= \begin{bmatrix} \bm{\ell}^\trsp \cos(\bm{L}\bm{x}) \\ \bm{\ell}^\trsp \sin(\bm{L}\bm{x}) \\ \bm{1}^\trsp \bm{x} \end{bmatrix} \\
&= \begin{bmatrix}
\ell_1 \!\cos(x_1) \!+\! \ell_2 \!\cos(x_1\!+\!x_2) \!+\! \ell_3 \!\cos(x_1\!+\!x_2\!+\!x_3) \!+\! \ldots\\
\ell_1 \sin(x_1) \!+\! \ell_2 \!\sin(x_1\!+\!x_2) \!+\! \ell_3 \!\sin(x_1\!+\!x_2\!+\!x_3) \!+\! \ldots\\
@@ -512,30 +512,30 @@ The \emph{forward kinematics} (FK) function of a planar robot manipulator is def
% f = [model.l * cos(T * x); ...
% model.l * sin(T * x); ...
% mod(sum(x,1)+pi, 2*pi) - pi];
-with $\bm{x}$ the state of the robot (joint angles), $\bm{f}$ the position of the robot endeffector, $\bm{\ell}\,$ a vector of robot links lengths, $\bm{L}$ a lower triangular matrix with unit elements, and $\bm{1}$ a vector of unit elements, see Fig.~\ref{fig:FK}.
+with $\bm{x}$ the state of the robot (joint angles), $\bm{f}^\tp{ee}$ the position of the robot endeffector, $\bm{\ell}\,$ a vector of robot links lengths, $\bm{L}$ a lower triangular matrix with unit elements, and $\bm{1}$ a vector of unit elements, see Fig.~\ref{fig:FK}.
The position and orientation of all articulations can similarly be computed with the forward kinematics function
\begin{align}
- \bm{\tilde{f}} &= {\Big[ \bm{L}\; \diag(\bm{\ell}) \cos(\bm{L}\bm{x}) ,\quad \bm{L}\; \diag(\bm{\ell}) \sin(\bm{L}\bm{x}) ,\quad \bm{L} \bm{x} \Big]}^\trsp
+ \bm{\tilde{f}}^\tp{ee} &= {\Big[ \bm{L}\; \diag(\bm{\ell}) \cos(\bm{L}\bm{x}) ,\quad \bm{L}\; \diag(\bm{\ell}) \sin(\bm{L}\bm{x}) ,\quad \bm{L} \bm{x} \Big]}^\trsp
\nonumber\\
&= \begin{bmatrix}
- \tilde{f}_{1,1} & \tilde{f}_{1,2} & \tilde{f}_{1,3} & \ldots\\
- \tilde{f}_{2,1} & \tilde{f}_{2,2} & \tilde{f}_{2,3} & \ldots\\
- \tilde{f}_{3,1} & \tilde{f}_{3,2} & \tilde{f}_{3,3} & \ldots\\
+ \tilde{f}^\tp{ee}_{1,1} & \tilde{f}^\tp{ee}_{1,2} & \tilde{f}^\tp{ee}_{1,3} & \ldots\\
+ \tilde{f}^\tp{ee}_{2,1} & \tilde{f}^\tp{ee}_{2,2} & \tilde{f}^\tp{ee}_{2,3} & \ldots\\
+ \tilde{f}^\tp{ee}_{3,1} & \tilde{f}^\tp{ee}_{3,2} & \tilde{f}^\tp{ee}_{3,3} & \ldots\\
\end{bmatrix} \!\!,
\label{eq:FKall}
\end{align}
\begin{alignat*}{3}
- & \tilde{f}_{1,1} = \ell_1 \!\cos(x_1),\quad &&
- \tilde{f}_{1,2} = \ell_1 \!\cos(x_1) \!+\! \ell_2 \!\cos(x_1\!+\!x_2),\quad &&
- \tilde{f}_{1,3} = \ell_1 \!\cos(x_1) \!+\! \ell_2 \!\cos(x_1\!+\!x_2) \!+\! \ell_3 \!\cos(x_1\!+\!x_2\!+\!x_3),\\
+ & \tilde{f}^\tp{ee}_{1,1} = \ell_1 \!\cos(x_1),\quad &&
+ \tilde{f}^\tp{ee}_{1,2} = \ell_1 \!\cos(x_1) \!+\! \ell_2 \!\cos(x_1\!+\!x_2),\quad &&
+ \tilde{f}^\tp{ee}_{1,3} = \ell_1 \!\cos(x_1) \!+\! \ell_2 \!\cos(x_1\!+\!x_2) \!+\! \ell_3 \!\cos(x_1\!+\!x_2\!+\!x_3),\\
\text{with}\quad
- & \tilde{f}_{2,1} = \ell_1 \sin(x_1), &&
- \tilde{f}_{2,2} = \ell_1 \sin(x_1) \!+\! \ell_2 \!\sin(x_1\!+\!x_2), &&
- \tilde{f}_{2,3} = \ell_1 \sin(x_1) \!+\! \ell_2 \!\sin(x_1\!+\!x_2) \!+\! \ell_3 \!\sin(x_1\!+\!x_2\!+\!x_3), \quad\ldots\\
- & \tilde{f}_{3,1} = x_1, &&
- \tilde{f}_{3,2} = x_1 + x_2, &&
- \tilde{f}_{3,3} = x_1 + x_2 + x_3.
+ & \tilde{f}^\tp{ee}_{2,1} = \ell_1 \sin(x_1), &&
+ \tilde{f}^\tp{ee}_{2,2} = \ell_1 \sin(x_1) \!+\! \ell_2 \!\sin(x_1\!+\!x_2), &&
+ \tilde{f}^\tp{ee}_{2,3} = \ell_1 \sin(x_1) \!+\! \ell_2 \!\sin(x_1\!+\!x_2) \!+\! \ell_3 \!\sin(x_1\!+\!x_2\!+\!x_3), \quad\ldots\\
+ & \tilde{f}^\tp{ee}_{3,1} = x_1, &&
+ \tilde{f}^\tp{ee}_{3,2} = x_1 + x_2, &&
+ \tilde{f}^\tp{ee}_{3,3} = x_1 + x_2 + x_3.
\end{alignat*}
%\begin{align*}
@@ -571,12 +571,63 @@ f = np.array([L @ np.diag(l) @ np.cos(L @ x), L @ np.diag(l) @ np.sin(L @ x)]) #
\filename{IK\_manipulator.*}
\end{flushright}
-By differentiating the forward kinematics function, a least norm inverse kinematics solution can be computed with
+\begin{figure}
+\centering
+\includegraphics[width=.5\columnwidth]{images/transformations01.jpg}
+\caption{\footnotesize
+Typical transformations involved in a manipulation task involving a robot, a vision system, a visual marker on the object, and a desired grasping location on the object.
+}
+\label{fig:transformations}
+\end{figure}
+
+We define a manipulation task involving a set of transformations as in Fig.~\ref{fig:transformations}. By relying on these transformation operators, we will describe all variables in the robot frame of reference (defined by $\bm{0}$, $\bm{e}_1$ and $\bm{e}_2$ in the figure).
+
+We first define $\bm{f}(\bm{x})$ as the residual vector between a target $\bm{\mu}$ and the endeffector position $\bm{f}^\tp{ee}(\bm{x})$ computed by the forward kinematics function, namely
+\begin{align*}
+ \bm{f}(\bm{x}) &= \bm{f}^\tp{ee}(\bm{x}) - \bm{\mu}. %\\
+ %\bm{J}(\bm{x}_t) &= \bm{J}^\tp{ee}(\bm{x}_t).
+\end{align*}
+
+The \emph{inverse kinematics} (IK) problem consists of finding a robot pose to reach a target with the robot endeffector. The underlying optimization problem consists of minimizing a cost $c(\bm{x})=\|\bm{f}(\bm{x})\|^2=\bm{f}(\bm{x})^\trsp\bm{f}(\bm{x})$, which can be solved iteratively with a Gauss--Newton method.
+
+As seen in Section \ref{sec:GaussNewton}, by differentiating $c(\bm{x})$ with respect to $\bm{x}$ and equating to 0, we get an update rule as in \eqref{eq:Taylor_nD_update}, namely
+\begin{align}
+ \bm{x}_{k+1} &= \bm{x}_k - {\big(\bm{J}^\trsp(\bm{x}_k) \bm{J}(\bm{x}_k)\big)}^{-1} \,
+ \bm{J}^\trsp(\bm{x}_k) \, \bm{f}(\bm{x}_k) \nonumber\\
+ &= \bm{x}_k - \bm{J}^\psin(\bm{x}_k) \, \big(\bm{f}^\tp{ee}(\bm{x}_k) - \bm{\mu}\big),
+ \label{eq:GaussNewtonUpdateIK}
+\end{align}
+where $\bm{J}\in\mathbb{R}^{R\times D}$ is the Jacobian matrix of $\bm{f}\in\mathbb{R}^R$, and $\bm{J}^\psin$ denotes the pseudoinverse of $\bm{J}$.
+
+For the orientation part of the data (if considered), the residual vector $\bm{f}(\bm{x}) = \bm{f}^\tp{ee}(\bm{x}) - \bm{\mu}$ is replaced by a geodesic residual computed with the logarithmic map $\bm{f}(\bm{x}) = \mathrm{Log}_{\bm{\mu}}\!\big(\bm{f}^\tp{ee}(\bm{x})\big)$, see \cite{Calinon20RAM} for details.
+
+The approach can also be extended to target objects/landmarks with positions $\bm{\mu}$ and rotation matrices $\bm{A}$, as depicted in Fig.~\ref{fig:transformations}. %, whose columns are basis vectors forming a coordinate system
+We can then define an error between the robot endeffector and an object/landmark expressed in the object/landmark coordinate system as
\begin{equation}
- \bm{\dot{\bm{x}}} = \bm{J}^\psin\!(\bm{x}) \; \bm{\dot{f}},
- \label{eq:IK_LS}
+\begin{aligned}
+ \bm{f}(\bm{x}) &= \bm{A}^\trsp \big(\bm{f}^\tp{ee}(\bm{x}) - \bm{\mu}\big), \\
+ \bm{J}(\bm{x}) &= \bm{A}^\trsp \bm{J}^\tp{ee}(\bm{x}).
+\end{aligned}
+\label{eq:fJU}
+\end{equation}
+
+The inverse kinematics update in \eqref{eq:GaussNewtonUpdateIK} can also be used to compute controllers.
+
+For a manipulator controlled by joint angle velocity commands $\bm{u}=\bm{\dot{x}} \approx \frac{\bm{\Delta}\bm{x}}{\Delta t}$, we can for example define a controller
+\begin{equation}
+ \bm{u} = \bm{J}^\psin(\bm{x}) \, (\bm{f}^\tp{ee}(\bm{x}) - \bm{\mu})\frac{\alpha}{\Delta t},
\end{equation}
-where the Jacobian $\bm{J}(\bm{x})$ corresponding to the endeffector forward kinematics function $\bm{f}(\bm{x})$ can be computed as (with a simplification for the orientation part by ignoring the manifold aspect)
+where $\alpha$ can typically be set as a small constant value, by taking into account the desired reactivity and the velocity capability of the robot.
+
+%the evolution of the system is described by $\bm{x}_{t+1} = \bm{x}_t + \bm{u}_t \Delta t$.
+
+%By differentiating the forward kinematics function, a least norm inverse kinematics solution can be computed with
+%\begin{equation}
+% \bm{\dot{\bm{x}}} = \bm{J}^\psin\!(\bm{x}) \; \bm{\dot{f}},
+% \label{eq:IK_LS}
+%\end{equation}
+
+For the planar robot manipulator example shown in Fig.~\ref{fig:FK}, the Jacobian $\bm{J}(\bm{x})$ of the endeffector forward kinematics function $\bm{f}^\tp{ee}(\bm{x})$ can be computed as %(with a simplification for the orientation part by ignoring the manifold aspect)
\begin{align*}
\bm{J} &= \begin{bmatrix}
-\sin(\bm{L}\bm{x})^\trsp \diag(\bm{\ell}) \bm{L} \\
@@ -626,7 +677,6 @@ In Python, this can be coded for the endeffector position part as
J = np.array([-np.sin(L @ x).T @ np.diag(l) @ L, np.cos(L @ x).T @ np.diag(l) @ L]) #Jacobian (for endeffector)
\end{lstlisting}
-This Jacobian can be used to solve the \emph{inverse kinematics} (IK) problem that consists of finding a robot pose to reach a target with the robot endeffector. The underlying optimization problem consists of minimizing a quadratic cost $c=\|\bm{f}-\bm{\mu}\|^2$, where $\bm{f}$ is the position of the endeffector and $\bm{\mu}$ is a target to reach with this endeffector. An optimization problem with a quadratic cost can be solved iteratively with a Gauss--Newton method, requiring to compute Jacobian pseudoinverses $\bm{J}^\psin$, see Section \ref{sec:GaussNewton} for details.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -688,7 +738,7 @@ Examples of nullspace controllers. The red paths represent the trajectories of m
\label{fig:nullspace}
\end{figure}
-In \eqref{eq:IK_LS}, the pseudoinverse provides a single least norm solution. This result can be generalized to obtain all solutions of the linear system with
+In \eqref{eq:GaussNewtonUpdateIK}, the pseudoinverse provides a single least norm solution. This result can be generalized to obtain all solutions of the linear system with
\begin{equation}
\bm{\dot{\bm{x}}} = \bm{J}^\psin\!(\bm{x}) \; \bm{\dot{f}} + \bm{N}\!(\bm{x}) \; \bm{g}(\bm{x}),
\label{eq:IK_nullspace}
@@ -1217,15 +1267,15 @@ The above equations can be used recursively, with $\bm{B}_\new^{-1}$ and $\bm{w}
%By following the notation in Section \ref{sec:GaussNewton}, the above objective can be written as a sum of squared functions that can be solved by Gauss--Newton optimization, with residuals and Jacobians given by
%\begin{alignat*}{3}
-% & \bm{r}_1 = \bm{\Psi}\bm{w} - \bm{x}, \qquad
+% & \bm{f}_1 = \bm{\Psi}\bm{w} - \bm{x}, \qquad
% && \bm{J}_1 = \bm{\Psi}, \\
-% & \bm{r}_2 = \bm{\nabla}_{\!\bm{t}}\bm{x}^\trsp \, \bm{\nabla}_{\!\bm{t}}\bm{x} - 1, \qquad\qquad
+% & \bm{f}_2 = \bm{\nabla}_{\!\bm{t}}\bm{x}^\trsp \, \bm{\nabla}_{\!\bm{t}}\bm{x} - 1, \qquad\qquad
% && \bm{J}_2 = 2 \, \bm{\nabla}_{\!\bm{t}}\bm{x} \, \bm{\nabla}_{\!\bm{t}}\bm{\Psi},
%\end{alignat*}
%which provide at each iteration step $k$ the Gauss--Newton update rule
%\begin{equation*}
% \bm{w}_{k+1} = \bm{w}_k - \alpha {\big( \bm{J}_1^\trsp \bm{J}_1 + \lambda \bm{J}_2^\trsp \bm{J}_2 \big)}^{-1} \,
-% \big( \bm{J}_1^\trsp \, \bm{r}_1 + \lambda \bm{J}_2^\trsp \, \bm{r}_2 \big),\\
+% \big( \bm{J}_1^\trsp \, \bm{f}_1 + \lambda \bm{J}_2^\trsp \, \bm{f}_2 \big),\\
%\end{equation*}
%as detailed in Section \ref{sec:GaussNewton}, where the learning rate $\alpha$ can be determined by line search.
@@ -1261,13 +1311,13 @@ The first component of the objective in \eqref{eq:wEikonal} is to find a SDF tha
By following the notation in Section \ref{sec:GaussNewton}, the above objective can be written as a sum of squared functions that can be solved by Gauss--Newton optimization, with residuals and Jacobians given by
\begin{align*}
- r_{1,m} &= \bm{\Psi}(\bm{t}_m) \, \bm{w} - x_m,
+ f_{1,m} &= \bm{\Psi}(\bm{t}_m) \, \bm{w} - x_m,
\quad &
\bm{J}_{1,m} &= \bm{\Psi}(\bm{t}_m),
\quad &
\forall m\in\{1,\ldots,M\},
\\
- r_{2,n} &= \bm{\nabla}x_n^\trsp \, \bm{\nabla}x_n - 1,
+ f_{2,n} &= \bm{\nabla}x_n^\trsp \, \bm{\nabla}x_n - 1,
\quad &
\bm{J}_{2,n} &= 2 \, {\bm{\nabla}x_n}^{\!\trsp} \, \bm{\nabla}\bm{\Psi}(\bm{t}_n),
\quad &
@@ -1277,7 +1327,7 @@ which provide at each iteration step $k$ the Gauss--Newton update rule
\begin{equation*}
\bm{w}_{k+1} \;\leftarrow\;
\bm{w}_k - \alpha {\Bigg( \sum_{m=1}^M \bm{J}_{1,m}^\trsp \bm{J}_{1,m} + \lambda \sum_{n=1}^N \bm{J}_{2,n}^\trsp \bm{J}_{2,n} \Bigg)}^{\!-1} \,
- \Bigg( \sum_{m=1}^M \bm{J}_{1,m}^\trsp \, r_{1,m} + \lambda \sum_{n=1}^N \bm{J}_{2,n}^\trsp \, r_{2,n} \bigg),\\
+ \Bigg( \sum_{m=1}^M \bm{J}_{1,m}^\trsp \, f_{1,m} + \lambda \sum_{n=1}^N \bm{J}_{2,n}^\trsp \, f_{2,n} \bigg),\\
\end{equation*}
as detailed in Section \ref{sec:GaussNewton}, where the learning rate $\alpha$ can be determined by line search.
@@ -1285,7 +1335,7 @@ Note that the above computation can be rewritten with concatenated vectors and m
\begin{equation*}
\bm{w}_{k+1} \;\leftarrow\;
\bm{w}_k - \alpha {\Bigg( \bm{J}_{1}^\trsp \bm{J}_{1} + \lambda \bm{J}_{2}^\trsp \bm{J}_{2} \Bigg)}^{\!-1} \,
- \Bigg( \bm{J}_{1}^\trsp \, \bm{r}_{1} + \lambda \bm{J}_{2}^\trsp \, \bm{r}_{2} \bigg).
+ \Bigg( \bm{J}_{1}^\trsp \, \bm{f}_{1} + \lambda \bm{J}_{2}^\trsp \, \bm{f}_{2} \bigg).
\end{equation*}
Figure \ref{fig:Bezier_2D_eikonal} presents an example in 2D.
@@ -2335,16 +2385,16 @@ In the next sections, we show examples of functions $\bm{f}(\bm{x})$ that can re
\filename{iLQR\_manipulator.*}
\end{flushright}
-\begin{figure}
-\centering
-\includegraphics[width=.5\columnwidth]{images/transformations01.jpg}
-\caption{\footnotesize
-Typical transformations involved in a manipulation task involving a robot, a vision system, a visual marker on the object, and a desired grasping location on the object.
-}
-\label{fig:transformations}
-\end{figure}
+%\begin{figure}
+%\centering
+%\includegraphics[width=.5\columnwidth]{images/transformations01.jpg}
+%\caption{\footnotesize
+%Typical transformations involved in a manipulation task involving a robot, a vision system, a visual marker on the object, and a desired grasping location on the object.
+%}
+%\label{fig:transformations}
+%\end{figure}
-We define a manipulation task involving a set of transformations as in Fig.~\ref{fig:transformations}. By relying on these transformation operators, we will next describe all variables in the robot frame of reference (defined by $\bm{0}$, $\bm{e}_1$ and $\bm{e}_2$ in the figure).
+%We define a manipulation task involving a set of transformations as in Fig.~\ref{fig:transformations}. By relying on these transformation operators, we will next describe all variables in the robot frame of reference (defined by $\bm{0}$, $\bm{e}_1$ and $\bm{e}_2$ in the figure).
For a manipulator controlled by joint angle velocity commands $\bm{u}=\bm{\dot{x}}$, the evolution of the system is described by $\bm{x}_{t+1} = \bm{x}_t + \bm{u}_t \Delta t$, with the Taylor expansion \eqref{eq:DS} simplifying to $\bm{A}_t=\frac{\partial\bm{g}}{\partial\bm{x}_t}=\bm{I}$ and $\bm{B}_t=\frac{\partial\bm{g}}{\partial\bm{u}_t}=\bm{I}\Delta t$. Similarly, a double integrator can alternatively be considered, with acceleration commands $\bm{u}=\bm{\ddot{x}}$ and states composed of both positions and velocities.
@@ -2354,18 +2404,6 @@ For a robot manipulator, $\bm{f}(\bm{x}_t)$ in \eqref{eq:du} typically represent
\bm{J}(\bm{x}_t) &= \bm{J}^\tp{ee}(\bm{x}_t).
\end{align*}
-For the orientation part of the data (if considered), the Euclidean distance vector $\bm{f}^\tp{ee}(\bm{x}_t) - \bm{\mu}_t$ is replaced by a geodesic distance measure computed with the logarithmic map $\log_{\bm{\mu}_t}\!\big(\bm{f}^\tp{ee}(\bm{x}_t)\big)$, see \cite{Calinon20RAM} for details.
-
-The approach can similarly be extended to target objects/landmarks with positions $\bm{\mu}_t$ and rotation matrices $\bm{U}_t$, whose columns are basis vectors forming a coordinate system, see Fig.~\ref{fig:iLQR_manipulator}. We can then define an error between the robot endeffector and an object/landmark expressed in the object/landmark coordinate system as
-\begin{equation}
-\begin{aligned}
- \bm{f}(\bm{x}_t) &= \bm{U}_t^\trsp \big(\bm{f}^\tp{ee}(\bm{x}_t) - \bm{\mu}_t\big), \\
- \bm{J}(\bm{x}_t) &= \bm{U}_t^\trsp \bm{J}^\tp{ee}(\bm{x}_t).
-\end{aligned}
-\label{eq:fJU}
-\end{equation}
-
-
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%\subsubsection{Bounded joint space}
@@ -2803,7 +2841,7 @@ For a 2 DOFs robot controlled at each time step $t$ with two control command $u_
\end{equation*}
with corresponding residuals and Jacobian given by
\begin{equation*}
- r_t = u_{1,t} \, u_{2,t},
+ f_t = u_{1,t} \, u_{2,t},
\quad
\bm{J}_t = \big[ u_{2,t}, \, u_{1,t} \big],
\quad
@@ -2813,9 +2851,9 @@ with corresponding residuals and Jacobian given by
The Gauss--Newton update rule in concatenated vector form is given by
\begin{equation*}
\bm{u}_{k+1} \;\leftarrow\;
- \bm{u}_k - \alpha \, {\big( \bm{J}^\trsp \bm{J} \big)}^{\!-1} \, \bm{J}^\trsp \, \bm{r},
+ \bm{u}_k - \alpha \, {\big( \bm{J}^\trsp \bm{J} \big)}^{\!-1} \, \bm{J}^\trsp \, \bm{f},
\end{equation*}
-where $\alpha$ is a line search parameter, $\bm{r}$ is a vector concatenating vertically all residuals $r_t$, and $\bm{J}$ is a Jacobian matrix with $\bm{J}_t$ as block diagonal elements. With some linear algebra machinery, this can also be computed in batch form using $\bm{r} = \bm{u}_1 \odot \bm{u}_2$, and $\bm{J} = (\bm{I}_{T-1} \otimes \bm{1}_{1\times 2}) \; \text{diag}\Big(\big(\bm{I}_{T-1} \otimes (\bm{1}_{2\times 2}-\bm{I}_2)\big)\bm{u} \Big)$, with $\odot$ and $\otimes$ the elementwise product and Kronecker product operators, respectively.
+where $\alpha$ is a line search parameter, $\bm{f}$ is a vector concatenating vertically all residuals $f_t$, and $\bm{J}$ is a Jacobian matrix with $\bm{J}_t$ as block diagonal elements. With some linear algebra machinery, this can also be computed in batch form using $\bm{f} = \bm{u}_1 \odot \bm{u}_2$, and $\bm{J} = (\bm{I}_{T-1} \otimes \bm{1}_{1\times 2}) \; \text{diag}\Big(\big(\bm{I}_{T-1} \otimes (\bm{1}_{2\times 2}-\bm{I}_2)\big)\bm{u} \Big)$, with $\odot$ and $\otimes$ the elementwise product and Kronecker product operators, respectively.
Figure \ref{fig:iLQR_decoupling} presents a simple example within a 2D reaching problem with a point mass agent and velocity commands.
@@ -3898,7 +3936,7 @@ For a set of $N$ datapoints, this geometric mean corresponds to the minimization
\begin{equation*}
\min_{\bm{\mu}} \sum_{n=1}^N {\text{Log}_{\bm{\mu}}\!(\bm{x}_n)}^\trsp \; \text{Log}_{\bm{\mu}}\!(\bm{x}_n),
\end{equation*}
-which can be solved by a simple and fast Gauss-Newton iterative algorithm. The algorithm starts from an initial estimate on the manifold and an associated tangent space. The datapoints $\{\bm{x}_n\}_{n=1}^N$ are projected in this tangent space to compute a direction vector, which provides an updated estimate of the mean. This process is repeated by iterating
+which can be solved by a simple and fast Gauss--Newton iterative algorithm. The algorithm starts from an initial estimate on the manifold and an associated tangent space. The datapoints $\{\bm{x}_n\}_{n=1}^N$ are projected in this tangent space to compute a direction vector, which provides an updated estimate of the mean. This process is repeated by iterating
\begin{equation*}
\bm{u} = \frac{1}{N} \sum_{n=1}^N \text{Log}_{\bm{\mu}}\!(\bm{x}_n),
\qquad