diff --git a/doc/images/LQT_smooth01.png b/doc/images/LQT_smooth01.png
index e56c16df00bede26da60c6c23c41dda2607fb667..a394260994d5b81bdc43fdfc8b023a63ad2b5672 100644
Binary files a/doc/images/LQT_smooth01.png and b/doc/images/LQT_smooth01.png differ
diff --git a/doc/images/LQT_smooth02.png b/doc/images/LQT_smooth02.png
index 6226d84757df570c768dd6c8782179c05435093c..702679a516478db9e337465bef4ab27614ef90b6 100644
Binary files a/doc/images/LQT_smooth02.png and b/doc/images/LQT_smooth02.png differ
diff --git a/doc/images/LQT_smooth03.png b/doc/images/LQT_smooth03.png
index c85e877376cfa5537eac6069572009fc8283d2aa..307d443d29778261910a8b5627e0497a1c10e4db 100644
Binary files a/doc/images/LQT_smooth03.png and b/doc/images/LQT_smooth03.png differ
diff --git a/doc/rcfs.pdf b/doc/rcfs.pdf
index c7239615d9077cbd89a3f057e68e7aabaedc94c2..41415821f7d2c86431767dd5d1214cc7c46a9074 100644
Binary files a/doc/rcfs.pdf and b/doc/rcfs.pdf differ
diff --git a/doc/rcfs.tex b/doc/rcfs.tex
index 5f0196725bc48205d4df8b341d9d3d2d20fed50e..e97e00aa5fc2524e5aceab889d6c0099a7f2f6aa 100644
--- a/doc/rcfs.tex
+++ b/doc/rcfs.tex
@@ -103,9 +103,10 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Introduction}
-This technical report presents several learning and optimal control techniques in robotics in the form of simple toy problems that can be easily coded. It comes with a set of standalone examples gathered as a git repository \textbf{Robotics Codes From Scratch (RCFS)}, accessed at:\\
-
-\noindent\texttt{\footnotesize https://gitlab.idiap.ch/rli/robotics-codes-from-scratch}\\
+This technical report presents several learning and optimal control techniques in robotics in the form of simple toy problems that can be easily coded. It comes with a set of standalone examples gathered as a git repository \textbf{Robotics Codes From Scratch (RCFS)}, accessed at:
+\begin{center}
+\texttt{https://gitlab.idiap.ch/rli/robotics-codes-from-scratch}
+\end{center}\\
Each section in this report lists the corresponding source codes in Python and Matlab (ensuring full compatibility with GNU Octave), as well as in C++ and Julia for some of the principal examples.
@@ -242,10 +243,10 @@ where we used the properties $\cos(0)=1$ and $\cos(-a)=\cos(a)$ of the cosine fu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Newton's method for minimization}\label{sec:Newton}
-\begin{wrapfigure}{r}{.36\textwidth}
+\begin{wrapfigure}{r}{.48\textwidth}
%\vspace{-20pt}
\centering
-\includegraphics[width=.34\textwidth]{images/NewtonOptim01.jpg}
+\includegraphics[width=.46\textwidth]{images/NewtonOptim01.jpg}
\caption{\footnotesize
Newton's method for minimization, starting from an initial estimate $x_1$ and converging to a local minimum (red point) after 5 iterations.
}
@@ -694,7 +695,7 @@ The approach can also be extended to model predictive control (MPC), iterative L
\begin{figure}
\centering
-\includegraphics[width=\columnwidth]{images/LQT_tennisServe01.jpg}
+\includegraphics[width=.7\columnwidth]{images/LQT_tennisServe01.jpg}
\caption{\footnotesize
Tennis serve problem solved by linear quadratic tracking (LQT).
}
@@ -724,16 +725,29 @@ In the above problem, $\bm{Q}$ is a precision matrix and $\bm{\mu}$ is a referen
\filename{LQT.*}
\end{flushright}
-\begin{figure}
+%\begin{figure}
+%\centering
+%\includegraphics[width=.32\columnwidth]{images/LQT_smooth01.png}
+%\includegraphics[width=.32\columnwidth]{images/LQT_smooth02.png}
+%\includegraphics[width=.32\columnwidth]{images/LQT_smooth03.png}
+%\caption{\footnotesize
+%Examples of linear quadratic tracking (LQT) applied to the task of reaching a set of viapoints (red dots) by starting from an initial position (black dot). \emph{Left:} With velocity commands, with a system described as a single integrator and a standard control cost. \emph{Center:} With velocity commands, with a system described as a single integrator and a control cost ensuring smoothness. \emph{Right:} With acceleration commands, with a system described as a double integrator and a standard control cost.
+%}
+%\label{fig:LQT_smooth}
+%\end{figure}
+
+\begin{wrapfigure}{r}{.42\textwidth}
+%\vspace{-20pt}
\centering
-\includegraphics[width=.32\columnwidth]{images/LQT_smooth01.png}
-\includegraphics[width=.32\columnwidth]{images/LQT_smooth02.png}
-\includegraphics[width=.32\columnwidth]{images/LQT_smooth03.png}
+\includegraphics[width=.16\columnwidth]{images/LQT_smooth01.png}\hspace{8mm}
+\includegraphics[width=.16\columnwidth]{images/LQT_smooth02.png}\\[8mm]
+\includegraphics[width=.16\columnwidth]{images/LQT_smooth03.png}
\caption{\footnotesize
-Examples of linear quadratic tracking (LQT) applied to the task of reaching a set of viapoints (red dots) by starting from an initial position (black dot). \emph{Left:} With velocity commands, with a system described as a single integrator and a standard control cost. \emph{Center:} With velocity commands, with a system described as a single integrator and a control cost ensuring smoothness. \emph{Right:} With acceleration commands, with a system described as a double integrator and a standard control cost.
+Examples of linear quadratic tracking (LQT) applied to the task of reaching a set of viapoints (red dots) by starting from an initial position (black dot). \emph{Top-left:} With velocity commands, with a system described as a single integrator and a standard control cost. \emph{Top-right:} With velocity commands, with a system described as a single integrator and a control cost ensuring smoothness. \emph{Bottom:} With acceleration commands, with a system described as a double integrator and a standard control cost.
}
\label{fig:LQT_smooth}
-\end{figure}
+%\vspace{-20pt}
+\end{wrapfigure}
Linear quadratic tracking (LQT) can be used with dynamical systems described as simple integrators. With single integrators, the states correspond to positions, with velocity control commands. With double integrators, the states correspond to positions and velocities, with acceleration control commands. With triple integrators, the states correspond to positions, velocities and accelerations, with jerk control commands, etc.
@@ -1015,14 +1029,14 @@ This least squares formulation of LQT (LQT-LS) yields the same controller as wit
\filename{LQT\_recursive\_LS\_multiAgents.*}
\end{flushright}
-\begin{figure}
+\begin{SCfigure}[50]
\centering
\includegraphics[width=.6\textwidth]{images/LQT_recursive_LS_multiAgents01.png}
\caption{\footnotesize
Least squares formulation of recursive LQR to control multiple agents (as point mass systems), where the task of each agent is to reach a desired target at the end of the motion, and to meet the other agent in the middle of the motion (e.g., for a handover task, see main text for the alternative option of nonzero offdiagonal elements at different time steps). We then test the adaptation capability of the agents by simulating a perturbation at 1/4 of the movement. The original and adapted movements are depicted in dashed and solid lines, respectively. The initial positions are represented with black points, the targets are in red, the optimized meeting points are in blue, and the perturbation is in green.
}
\label{fig:LQT_recursive_LS_multiAgents}
-\end{figure}
+\end{SCfigure}
Figure \ref{fig:LQT_recursive_LS_multiAgents} presents an example with the control of multiple agents, with a precision matrix involving nonzero offdiagonal elements. The corresponding example code presents two options: it either requests the two agents to meet in the middle of the motion (e.g., for a handover task), or it requests the two agents to find a location to reach at different time steps (e.g., to drop and pick-up an object), involving nonzero offdiagonal elements at different time steps.
@@ -1712,6 +1726,15 @@ Based on the above definitions, $\bm{f}(\bm{x})$ and $\bm{J}(\bm{x})$ are in thi
\end{align*}
%\bm{y}
+\begin{SCfigure}
+\centering
+\includegraphics[width=.6\textwidth]{images/iLQR_objectBoundaries01.jpg}
+\caption{\footnotesize
+Example of a viapoints task in which a planar robot with 3 joints needs to sequentially reach 2 objects, with object boundaries defining the allowed reaching points on the objects surfaces. \emph{Left:} Reaching task with two viapoints at $t=25$ and $t=50$. \emph{Right:} Corresponding values of the cost function for the end-effector space at $t=25$ and $t=50$.
+}
+\label{fig:iLQR_manipulator}
+\end{SCfigure}
+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsubsection{Reaching task with robot manipulator and prismatic object boundaries}\label{sec:reaching}
@@ -1719,15 +1742,6 @@ Based on the above definitions, $\bm{f}(\bm{x})$ and $\bm{J}(\bm{x})$ are in thi
\filename{iLQR\_manipulator.*}
\end{flushright}
-\begin{figure}
-\centering
-\includegraphics[width=.7\textwidth]{images/iLQR_objectBoundaries01.jpg}
-\caption{\footnotesize
-Example of a viapoints task in which a planar robot with 3 joints needs to sequentially reach 2 objects, with object boundaries defining the allowed reaching points on the objects surfaces. \emph{Left:} Reaching task with two viapoints at $t=25$ and $t=50$. \emph{Right:} Corresponding values of the cost function for the end-effector space at $t=25$ and $t=50$.
-}
-\label{fig:iLQR_manipulator}
-\end{figure}
-
The definition of $\bm{f}(\bm{x}_t)$ and $\bm{J}(\bm{x}_t)$ in \eqref{eq:fJU} can also be extended to objects/landmarks with boundaries by defining
\begin{align*}
\bm{f}(\bm{x}_t) &= \bm{f}^\tp{cut}\Big( \bm{e}(\bm{x}_t) \Big), \\
@@ -1762,25 +1776,25 @@ see also Fig.~\ref{fig:iLQR_manipulator}.
%\label{fig:iLQR_CoM}
%\end{figure}
-%\begin{wrapfigure}{r}{.46\textwidth}
-%\vspace{-20pt}
+%\begin{SCfigure}[50]
%\centering
-%\includegraphics[width=.44\textwidth]{images/iLQR_CoM01.png}
+%\includegraphics[width=.25\textwidth]{images/iLQR_CoM01.png}
%\caption{\footnotesize
%Reaching task with a humanoid (side view) by keeping the center of mass in an area defined by the feet location. The starting and ending poses are depicted in different grayscale levels. The corresponding center of mass is depicted by a darker semi-filled disc. The area allowed for the center of mass is depicted as a transparent red rectangle. The task consists of reaching the green object with the end-effector at the end of the movement, while always keeping the center of mass within the desired bounding box during the movement.
%}
%\label{fig:iLQR_CoM}
-%\vspace{-20pt}
-%\end{wrapfigure}
+%\end{SCfigure}
-\begin{SCfigure}[50]
+\begin{wrapfigure}{r}{.30\textwidth}
+%\vspace{-20pt}
\centering
-\includegraphics[width=.3\textwidth]{images/iLQR_CoM01.png}
+\includegraphics[width=.28\textwidth]{images/iLQR_CoM01.png}
\caption{\footnotesize
-Reaching task with a humanoid (side view) by keeping the center of mass in an area defined by the feet location. The starting and ending poses are depicted in different grayscale levels. The corresponding center of mass is depicted by a darker semi-filled disc. The area allowed for the center of mass is depicted as a transparent red rectangle. The task consists of reaching the green object with the end-effector at the end of the movement, while always keeping the center of mass within the desired bounding box during the movement.
+Reaching task with a humanoid (side view) by keeping the center of mass in an area defined by the feet location.
}
\label{fig:iLQR_CoM}
-\end{SCfigure}
+%\vspace{-20pt}
+\end{wrapfigure}
If we assume an equal mass for each link concentrated at the joint (i.e., assuming that the motors and gripper are heavier than the link structures), the forward kinematics function to determine the center of a mass of an articulated chain can simply be computed with
\begin{equation*}
@@ -1799,7 +1813,8 @@ The corresponding Jacobian matrix is
% Jtmp = [-sin(L * x(:,t))' * L * diag(model.l' * L); ...
% cos(L * x(:,t))' * L * diag(model.l' * L)] / model.nbVarX;
-The forward kinematics function $\bm{f}^\tp{CoM}$ can be used in tracking tasks similar to the ones considering the end-effector, as in Section \ref{sec:reaching}, see Fig.~\ref{fig:iLQR_CoM} for an example.
+The forward kinematics function $\bm{f}^\tp{CoM}$ can be used in tracking tasks similar to the ones considering the end-effector, as in Section \ref{sec:reaching}. Fig.~\ref{fig:iLQR_CoM} shows an example in which the starting and ending poses are depicted in different grayscale levels. The corresponding center of mass is depicted by a darker semi-filled disc. The area allowed for the center of mass is depicted as a transparent red rectangle. The task consists of reaching the green object with the end-effector at the end of the movement, while always keeping the center of mass within the desired bounding box during the movement.
+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -1808,14 +1823,14 @@ The forward kinematics function $\bm{f}^\tp{CoM}$ can be used in tracking tasks
\filename{iLQR\_bimanual.*}
\end{flushright}
-\begin{figure}
+\begin{SCfigure}
\centering
-\includegraphics[width=.48\columnwidth]{images/iLQR_bimanual02.png}
+\includegraphics[width=.48\columnwidth]{images/iLQR_bimanual02.png}\\
\includegraphics[width=.48\columnwidth]{images/iLQR_bimanual01.png}
\caption{\footnotesize
Reaching tasks with a bimanual robot (frontal view). \emph{Left:} with a target for each hand. \emph{Right:} with a target for the hand on the left, while keeping the center of mass at the same location during the whole movement.
}
-\label{fig:iLQR_bimanual}
+\label{SCfig:iLQR_bimanual}
\end{figure}
%\begin{wrapfigure}{r}{.36\textwidth}