diff --git a/doc/images/iLQR_distMaintain01.png b/doc/images/iLQR_distMaintain01.png
new file mode 100644
index 0000000000000000000000000000000000000000..2302e21a6dac2b791c138faf75633d348bc33808
Binary files /dev/null and b/doc/images/iLQR_distMaintain01.png differ
diff --git a/doc/images/iLQR_distMaintain02.png b/doc/images/iLQR_distMaintain02.png
new file mode 100644
index 0000000000000000000000000000000000000000..c59b7a74b07de158109177d8e29c492c821c20f8
Binary files /dev/null and b/doc/images/iLQR_distMaintain02.png differ
diff --git a/doc/rcfs.pdf b/doc/rcfs.pdf
index dae5823cb4115b16f22e4e743723852df635bd4c..c96fca73b8b4027cba7eae639b2888b43e7a7ba0 100644
Binary files a/doc/rcfs.pdf and b/doc/rcfs.pdf differ
diff --git a/doc/rcfs.tex b/doc/rcfs.tex
index 945009c25e0683330dc455da47291d5525dda5e2..a5dc1117e5da929276c8290d395813620025ce73 100644
--- a/doc/rcfs.tex
+++ b/doc/rcfs.tex
@@ -3,6 +3,7 @@
\usepackage{graphicx,amsmath,amssymb,bm,xcolor,soul,nicefrac,listings,algorithm2e,dsfont,caption,subcaption,wrapfig,sidecap}
\usepackage[hidelinks]{hyperref}
\usepackage[makeroom]{cancel}
+\usepackage[export]{adjustbox} %for valign in figures
%pseudocode
\newcommand\mycommfont[1]{\footnotesize\ttfamily\textcolor{lightgray}{#1}}
@@ -2691,12 +2692,23 @@ A similar principle can be applied to robot manipulators by composing forward ki
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-\subsubsection{Maintaining a desired distance to an object} %(1-e'*e)'*(1-e'*e) version, where 1-e'*e is a scalar
+\subsubsection{Distance to an object} %(1-e'*e)'*(1-e'*e) version, where 1-e'*e is a scalar
\begin{flushright}
\filename{iLQR\_distMaintenance.*}
\end{flushright}
-The obstacle example above can easily be extended to the problem of maintaining a desired distance to an object, which can also typically used with other objectives. We first define a function and a gradient
+\begin{wrapfigure}{r}{.38\textwidth}
+%\vspace{-20pt}
+\centering
+\includegraphics[width=.14\textwidth,valign=c]{images/iLQR_distMaintain01.png}\hspace{6mm}
+\includegraphics[width=.14\textwidth,valign=c]{images/iLQR_distMaintain02.png}
+\caption{\footnotesize
+\emph{Left:} Optimal control problem to generate a motion by considering a point mass starting from an initial point (in black), with a cost asking to reach or maintain a desired distance to a point (in red). \emph{Right:} Similar problem for a contour to reach/maintain defined as a covariance matrix.
+}
+\label{fig:distMaintain}
+\end{wrapfigure}
+
+The obstacle example above can easily be extended to the problem of reaching/maintaining a desired distance to an object, which can also typically used with other objectives. We first define a function and a gradient
\begin{equation*}
f^\tp{dist}(d) = 1-d, \quad\quad
g^\tp{dist}(d) = -1,
@@ -2710,6 +2722,10 @@ that we exploit to define $\bm{f}(\bm{x}_t)$ and $\bm{J}(\bm{x}_t)$ in \eqref{eq
In the above, $\bm{f}(\bm{x})$ defines a continuous function that is $0$ on a sphere of radius $r$ centered on the object (defined by a center $\bm{\mu}$), and increasing/decreasing when moving away from this surface in one direction or the other.
+Similarly, the error can be weighted by a covariance matrix $\bm{\Sigma}$, by using $e(\bm{x}_t) = {(\bm{x}_t-\bm{\mu})}^\trsp \bm{\Sigma}^{-1} (\bm{x}_t-\bm{\mu})$ and $\bm{J}(\bm{x}_t) = -2 {(\bm{x}_t-\bm{\mu})}^\trsp \bm{\Sigma}^{-1}$, which corresponds to the problem of reaching/maintaining the contour of an ellipse.
+
+Figure \ref{fig:distMaintain} presents examples with a point mass.
+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsubsection{Manipulability tracking}
@@ -2800,7 +2816,7 @@ Figure \ref{fig:iLQR_decoupling} presents a simple example within a 2D reaching
\centering
\includegraphics[width=.32\textwidth]{images/iLQR_curvature01.png}
\caption{\footnotesize
-Point mass control problem starting form an initial point (in black), with a cost asking to pass through a set of viapoints (in red), together with a cost on curvature (black path).
+Optimal control problem to generate a motion by considering a point mass starting from an initial point (in black), with a cost asking to pass through a set of viapoints (in red), together with a cost on curvature (black path).
}
\label{fig:curvature}
\end{wrapfigure}
@@ -2840,6 +2856,8 @@ With this formulation, by using the derivative property $(fg)'=f'g+fg'$, the der
\label{eq:diff_curvature}
\end{equation}
+Figure \ref{fig:curvature} presents an example with a point mass.
+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{iLQR with control primitives}
diff --git a/matlab/iLQR_distMaintenance.m b/matlab/iLQR_distMaintenance.m
index dee8be9c27b91872b2278333d86341f5b44ac672..82a25b967a9c68f237e58b5c6b2b6093b8e81d79 100644
--- a/matlab/iLQR_distMaintenance.m
+++ b/matlab/iLQR_distMaintenance.m
@@ -17,7 +17,12 @@ param.nbIter = 100; %Maximum number of iterations for iLQR
param.nbVarX = 2; %State space dimension (x1,x2)
param.nbVarU = 2; %Control space dimension (dx1,dx2)
param.Mu = [1.0; 0.3]; %Object location
-param.dist = .4; %Distance to maintain
+
+%param.dist = .4; %Distance to maintain
+%param.Sigma = eye(param.nbVarX) * param.dist^2; %Covariance matrix
+vtmp = [1; 1]; %Main axis of covariance matrix
+param.Sigma = vtmp * vtmp' + eye(2) * 1E-1; %Covariance matrix
+
param.q = 1E0; %Distance maintenance weight term
param.r = 1E-3; %Control weight term
@@ -63,14 +68,19 @@ disp(['iLQR converged in ' num2str(n) ' iterations.']);
%% Plot state space
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-al = linspace(-pi, pi, 50);
h = figure('position',[10,10,800,800],'color',[1,1,1]); hold on; axis off;
-msh = param.dist * [cos(al); sin(al)] + repmat(param.Mu(1:2), 1, 50);
+al = linspace(-pi, pi, 50);
+%msh = param.dist * [cos(al); sin(al)] + repmat(param.Mu(1:2), 1, 50);
+[V,D] = eig(param.Sigma);
+msh = V * D.^.5 * [cos(al); sin(al)] + repmat(param.Mu(1:2), 1, 50);
+
patch(msh(1,:), msh(2,:), [1 .8 .8],'linewidth',2,'edgecolor',[.8 .4 .4]);
plot(param.Mu(1), param.Mu(2), '.','markersize',25,'color',[.8 0 0]);
plot(x(1,:), x(2,:), '-','linewidth',2,'color',[0 0 0]);
-plot(x(1,[1,end]), x(2,[1,end]), '.','markersize',25,'color',[0 0 0]);
+plot(x(1,1), x(2,1), '.','markersize',25,'color',[0 0 0]);
+plot(x(1,end), x(2,end), '.','markersize',25,'color',[0 .6 0]);
axis equal;
+%print('-dpng','iLQR_distMaintain02.png');
waitfor(h);
end
@@ -79,8 +89,11 @@ end
%Residuals f and Jacobians J for maintaining a desired distance to an object (fast version with computation in matrix form)
function [f, J] = f_dist(x, param)
e = x - repmat(param.Mu(1:2), 1, param.nbData);
- f = 1 - sum(e.^2, 1)' / param.dist^2; %Residuals
- Jtmp = repmat(-e'/param.dist^2, 1, param.nbData);
+% f = 1 - sum(e.^2, 1)' / param.dist^2; %Residuals
+% Jtmp = repmat(-e'/param.dist^2, 1, param.nbData);
+ f = 1 - sum(e .* (param.Sigma\e), 1)';
+ Jtmp = repmat(-param.Sigma\e, param.nbData,1 )';
+
J = Jtmp .* kron(eye(param.nbData), ones(1,param.nbVarU)); %Jacobians
end