diff --git a/doc/images/iLQR_decoupling01.png b/doc/images/iLQR_decoupling01.png
new file mode 100644
index 0000000000000000000000000000000000000000..5c56c6ce98df31484010617ede31267f6879d3a2
Binary files /dev/null and b/doc/images/iLQR_decoupling01.png differ
diff --git a/doc/rcfs.pdf b/doc/rcfs.pdf
index aa8884db8e95f29a490fb2335ca36b1d122fc34d..e238ae870f5d9163188ea71e9bd84663d2cbdbf8 100644
Binary files a/doc/rcfs.pdf and b/doc/rcfs.pdf differ
diff --git a/doc/rcfs.tex b/doc/rcfs.tex
index 697d078716bc81b8b28858eaea72d8a06a22a703..7b1b9a1744a68e625f8809f9f5efdd591d66f102 100644
--- a/doc/rcfs.tex
+++ b/doc/rcfs.tex
@@ -911,15 +911,15 @@ When using splines of the form $\bm{\phi}(t)=\bm{T}(t)\bm{B}\bm{C}$, the derivat
By modeling a signed distance function as $x = f(\bm{t})$, the derivatives can for example be used to find a point on the contour (with distance zero). Such problem can be solved with Gauss--Newton optimization with the cost $c(\bm{t})= \frac{1}{2} f(\bm{t})^2$, by starting from an initial guess $\bm{t}_0$. In the above example, the Jacobian is then given by
\begin{equation}
\bm{J}(\bm{t}) = \begin{bmatrix} \frac{\partial\bm{\Psi}(\bm{t})}{\partial t_1} \,\bm{w}
- \\[2mm] \frac{\partial\bm{\Psi}(\bm{t})}{\partial t_2} \,\bm{w} \end{bmatrix},
+ \\[2mm] \frac{\partial\bm{\Psi}(\bm{t})}{\partial t_2} \,\bm{w} \\ \vdots \end{bmatrix},
\label{eq:J_SDF}
\end{equation}
and the update can be computed recursively with
\begin{equation}
- \bm{t}_{k+1} \;\leftarrow\; \bm{t}_k - \bm{J}(\bm{t}_k)^\psin f(\bm{t}_k),
+ \bm{t}_{k+1} \;\leftarrow\; \bm{t}_k - \alpha \bm{J}(\bm{t}_k)^\psin f(\bm{t}_k),
\label{eq:update_SDF}
\end{equation}
-as in Equation \eqref{eq:GaussNewtonUpdate}, where $\bm{J}(\bm{t}_k)^\trsp f(\bm{t}_k)$ is a gradient and $\bm{J}(\bm{t}_k)^\trsp \bm{J}(\bm{t}_k)$ a Hessian matrix.\newline
+as in Equation \eqref{eq:GaussNewtonUpdate}, where $\bm{J}(\bm{t}_k)^\trsp f(\bm{t}_k)$ is a gradient and $\bm{J}(\bm{t}_k)^\trsp \bm{J}(\bm{t}_k)$ a Hessian matrix, and $\alpha$ is a line search parameter.\newline
%%%%%%%%%%%%%%%%%%
@@ -937,19 +937,20 @@ where $\bm{J}_1(\bm{t}_1)$ and $\bm{J}_2(\bm{t}_2)$ are the derivatives of the t
Similarly to \eqref{eq:update_SDF}, the Gauss--Newton update step is then given by
\begin{equation}
- \bm{t}_{k+1} \;\leftarrow\; \bm{t}_k - \bm{J}(\bm{t}_k)^\psin
+ \bm{t}_{k+1} \;\leftarrow\; \bm{t}_k - \alpha \bm{J}(\bm{t}_k)^\psin
\begin{bmatrix} f_1(\bm{t}_{1,k}) \\ f_2(\bm{t}_{2,k}) \\ \bm{A} \, \bm{t}_{2,k} + \bm{b} - \bm{t}_{1,k} \end{bmatrix}.
\end{equation}
A prioritized optimization scheme can also be used to have the two points on the shape boundaries as primary objective and to move these points closer as secondary objective, with a corresponding Gauss--Newton update step given by
\begin{align}
- \bm{t}_{k+1} &\leftarrow\; \bm{t}_k - \bm{J}_{12}^\psin \begin{bmatrix} f_1(\bm{t}_{1,k}) \\ f_2(\bm{t}_{2,k}) \end{bmatrix}
- + \bm{N}_{12} \; \bm{J}_{3}^\psin \; (\bm{A} \, \bm{t}_{2,k} + \bm{b} - \bm{t}_{1,k}), \\
+ \bm{t}_{k+1} &\leftarrow\; \bm{t}_k - \alpha_1 \bm{J}_{12}^\psin \begin{bmatrix} f_1(\bm{t}_{1,k}) \\ f_2(\bm{t}_{2,k}) \end{bmatrix}
+ + \alpha_2 \bm{N}_{12} \; \bm{J}_{3}^\psin \; (\bm{A} \, \bm{t}_{2,k} + \bm{b} - \bm{t}_{1,k}), \\
\text{where}\quad
\bm{J}_{12} &= \begin{bmatrix} \bm{J}_1(\bm{t}_1) & \bm{0} \\ \bm{0} & \bm{J}_2(\bm{t}_2) \end{bmatrix}, \quad
\bm{N}_{12} = \bm{I} - \bm{J}_{12}^\psin \bm{J}_{12}, \quad
- \bm{J}_{3} = \begin{bmatrix} -\bm{I} & \bm{A} \end{bmatrix}.
+ \bm{J}_{3} = \begin{bmatrix} -\bm{I} & \bm{A} \end{bmatrix},
\end{align}
+with $\alpha_1$ and $\alpha_2$ are two line search parameters.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -1182,7 +1183,14 @@ which provide at each iteration step $k$ the Gauss--Newton update rule
\end{equation*}
as detailed in Section \ref{sec:GaussNewton}, where the learning rate $\alpha$ can be determined by line search.
-Note that the above computation can be rewritten with concatenated vectors and matrices for more efficient computation exploiting linear algebra. Fig.~\ref{fig:Bezier_2D_eikonal} presents an example in 2D.
+Note that the above computation can be rewritten with concatenated vectors and matrices for more efficient computation exploiting linear algebra. By concatenating vertically the Jacobian matrices and residuals, we have the equivalent Gauss--Newton update rule
+\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).
+\end{equation*}
+
+Figure \ref{fig:Bezier_2D_eikonal} presents an example in 2D.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -2597,6 +2605,44 @@ The above iLQR approach, with a geodesic cost on SPD manifolds, has been present
The approach can also be extended to other forms of symmetric positive definite matrices, such as kernel matrices used in machine learning algorithms to compute similarities, or graph graph Laplacians (a matrix representation of a graph that can for example be used to construct a low dimensional embedding of a graph).
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+\subsubsection{Decoupling of control commands}
+
+\begin{wrapfigure}{r}{.22\textwidth}
+%\vspace{-20pt}
+\centering
+\includegraphics[width=.20\textwidth]{images/iLQR_decoupling01.png}
+\caption{\footnotesize
+2D reaching task problem (initial point in black and target point in red), with an additional cost to favor the decoupling of the control commands.
+}
+\label{fig:iLQR_decoupling}
+\end{wrapfigure}
+
+In some situations, we would like to control a robot by avoiding that all degrees of freedom are controlled at the same time. For example, we might in some cases prefer that a mobile manipulator stops moving the arm while the platform is moving and vice versa.
+
+For a 2 DOFs robot controlled at each time step $t$ with two control command $u_{1,t}$ and $u_{2,t}$, the corresponding cost to find the full sequence of control commands for a duration $T$ (gathered in a concatenated vector $\bm{u}$) is
+\begin{equation*}
+ \min_{\bm{u}} \; \sum_{t=1}^T \, {(u_{1,t} \, u_{2,t})}^2,
+\end{equation*}
+with corresponding residuals and Jacobian given by
+\begin{equation*}
+ r_t = u_{1,t} \, u_{2,t},
+ \quad
+ \bm{J}_t = \big[ u_{2,t}, \, u_{1,t} \big],
+ \quad
+ \forall t\in\{1,\ldots,T\}.
+\end{equation*}
+
+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},
+\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.
+
+Figure \ref{fig:iLQR_decoupling} presents a simple example within a 2D reaching problem with a point mass agent and velocity commands.
+
+\newpage
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{iLQR with control primitives}
@@ -2676,6 +2722,7 @@ with Jacobian matrices $\bm{A}_t \!=\! \frac{\partial\bm{d}}{\partial\bm{x}_t}$,
Figure \ref{fig:iLQR_spacetime} shows a viapoints task example in which both path and duration are optimized (convergence after 10 iterations when starting from zero commands). The example uses a double integrator as linear system defined by constant $\mathbf{A}_t$ and $\mathbf{B}_t$ matrices.
+\newpage
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{iLQR with offdiagonal elements in the precision matrix}
@@ -3021,6 +3068,7 @@ Figure \ref{fig:ergodic_SMC} shows a 2D example of ergodic control to create a m
%The downside of the original ergodic control approach is that it does not scale well to problems requiring exploration in search space of more than two dimensions. In particular, the original approach would be too slow to consider full distributions in 6D spaces, which would be ideally required. Indeed, both position and orientation of endeffector(s) matter in most robot problems, including manipulation, insertion, welding, or the use of tools at the robot endeffectors.
%In \cite{Shetty21TRO}, we demonstrated that the original problem formulation can be conserved \textbf{by efficiently compressing the Fourier series decomposition with tensor train (TT) factorization}. The proposed solution is efficient both computationally and storage-wise, hence making it suitable for online implementations, as well as to tackle robot learning problems with a low quantity of training data. The above figure shows an overview of an insertion experiment conducted with the Siemens gears benchmark requiring full 6D endeffector poses. Further work is required to extend the approach to online active sensing applications in which the distributions can change based on the data collected by ergodic control.
+\newpage
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{Heat equation driven area coverage (HEDAC)}\label{sec:HEDAC}
@@ -3061,7 +3109,7 @@ The heat source is then diffused over the domain to propagate the information ab
\frac{d u(\bm{x},t)}{dt}=\alpha\Delta u(\bm{x},t) + s(\bm{x},t), %\beta
\label{eq:heat}
\end{equation}
-where $u(\bm{x},t)$ corresponds to the temperature field, $\alpha$ is a thermal diffusivity parameter, and $\Delta$ is the Laplacian operator.
+where $u(\bm{x},t)$ corresponds to the temperature field, $\alpha$ is a thermal diffusivity parameter, and $\Delta u= \frac{\partial^2 u}{\partial x_1^2} + \frac{\partial^2 u}{\partial x_2^2} + \ldots$ is the Laplacian of the function $u$.
%$\beta$ is the source strength
In order to control the agents, the potential field exerts a fictitious force on each of the $i$-th agent, based on the gradient of the temperature field