diff --git a/doc/rcfs.pdf b/doc/rcfs.pdf
index 0103c220c8070f0a22935d03adf128895f82f1b7..b3def33d782aad8d0a3ec87e6c1f2916ab4ebe6b 100644
Binary files a/doc/rcfs.pdf and b/doc/rcfs.pdf differ
diff --git a/doc/rcfs.tex b/doc/rcfs.tex
index a87f986420d75ca537f188fc9424c2473310e7f9..aa904d1faf4a733a363879e385c9d41122b5f49d 100644
--- a/doc/rcfs.tex
+++ b/doc/rcfs.tex
@@ -1928,22 +1928,22 @@ with the corresponding Jacobian matrix $\bm{J}^\tp{CoM}\in\mathbb{R}^{2\times 5}
Reaching task with obstacle avoidance, by starting from $\bm{x}_1$ and reaching $\bm{\mu}_T$ while avoiding the two obstacles in red.
}
\label{fig:iLQR_obstacle}
-\vspace{-80pt}
+\vspace{-20pt}
\end{wrapfigure}
By taking as example a point-mass system, iLQR can be used to solve an ellipsoidal obstacle avoidance problem with the cost (typically used with other costs, see Fig.~\ref{fig:iLQR_obstacle} for an example). We first define a ReLU-like function and its gradient as (see also Fig.~\ref{fig:fcut}-\emph{right})
-\begin{align*}
- f^\tp{avoid}(d) &= \begin{cases} 0, & \text{if}\ d>1 \\ 1-d, & \text{otherwise} \end{cases}, \\
- \bm{g}^\tp{avoid}(d) &= \begin{cases} 0, \hspace{6mm} & \text{if}\ d>1 \\ -1, & \text{otherwise} \end{cases},
-\end{align*}
+\begin{equation*}
+ f^\tp{avoid}(d) = \begin{cases} 0, & \text{if}\ d>1 \\ 1-d, & \text{otherwise} \end{cases}, \quad\quad
+ g^\tp{avoid}(d) = \begin{cases} 0, \hspace{6mm} & \text{if}\ d>1 \\ -1, & \text{otherwise} \end{cases},
+\end{equation*}
that we exploit to define $\bm{f}(\bm{x}_t)$ and $\bm{J}(\bm{x}_t)$ in \eqref{eq:du} as
\begin{align*}
- \bm{f}(\bm{x}_t) &= f^\tp{avoid}\Big( e(\bm{x}_t) \Big), \\
- \bm{J}(\bm{x}_t) &= 2 \; \bm{g}^\tp{avoid}\Big( e(\bm{x}_t) \Big) \; {(\bm{x}_t-\bm{\mu})}^\trsp \; \bm{\Sigma}^{-1} ,\\
+ \bm{f}(\bm{x}_t) &= f^\tp{avoid}\Big( e(\bm{x}_t) \Big), \quad\quad
+ \bm{J}(\bm{x}_t) = 2 \; g^\tp{avoid}\Big( e(\bm{x}_t) \Big) \; {(\bm{x}_t-\bm{\mu})}^\trsp \; \bm{\Sigma}^{-1} ,\\
\text{with}\quad e(\bm{x}_t) &= {(\bm{x}_t-\bm{\mu})}^\trsp \bm{\Sigma}^{-1} (\bm{x}_t-\bm{\mu}).
\end{align*}
-In the above, $\bm{f}(\bm{x})$ defines a continuous and smooth function that is $0$ outside the obstacle boundaries and $1$ at the center of the obstacle. The ellipsoid is defined by a center $\bm{\mu}$ and a shape $\bm{\Sigma}=\bm{V}\bm{V}^\trsp$, where $\bm{V}$ is a horizontal concatenation of vectors $\bm{V}_i$ describing the principal axes of the ellipsoid, see Fig.~\ref{fig:iLQR_obstacle}.
+In the above, $\bm{f}(\bm{x})$ defines a continuous function that is $0$ outside the obstacle boundaries and $1$ at the center of the obstacle. The ellipsoid is defined by a center $\bm{\mu}$ and a shape $\bm{\Sigma}=\bm{V}\bm{V}^\trsp$, where $\bm{V}$ is a horizontal concatenation of vectors $\bm{V}_i$ describing the principal axes of the ellipsoid, see Fig.~\ref{fig:iLQR_obstacle}.
%A similar decomposition on the corresponding precision matrix is expressed as $\bm{\Sigma}^{-1}=\bm{U}\bm{U}^\trsp$.
A similar principle can be applied to robot manipulators by composing forward kinematics and obstacle avoidance functions.
@@ -1954,6 +1954,27 @@ A similar principle can be applied to robot manipulators by composing forward ki
% Alternatively, an avoidance cost c = (1 - f'*f) * wQ (with wQ a scalar weigh) can be used in practice because 0<=(1 - f'*f)<=1
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+\subsubsection{Maintaining a desired 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{equation*}
+ f^\tp{dist}(d) = 1-d, \quad\quad
+ g^\tp{dist}(d) = -1,
+\end{equation*}
+that we exploit to define $\bm{f}(\bm{x}_t)$ and $\bm{J}(\bm{x}_t)$ in \eqref{eq:du} as
+\begin{align*}
+ \bm{f}(\bm{x}_t) &= f^\tp{dist}\Big( e(\bm{x}_t) \Big), \quad\quad
+ \bm{J}(\bm{x}_t) = -\frac{2}{r} {(\bm{x}_t-\bm{\mu})}^\trsp ,\\ %\; g^\tp{dist}\Big( e(\bm{x}_t) \Big)
+ \text{with}\quad e(\bm{x}_t) &= \frac{1}{r^2} {(\bm{x}_t-\bm{\mu})}^\trsp (\bm{x}_t-\bm{\mu}).
+\end{align*}
+
+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.
+
+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsubsection{Manipulability tracking}
\begin{flushright}
diff --git a/matlab/iLQR_distMaintenance.m b/matlab/iLQR_distMaintenance.m
index 13439f408932042ad704ed19d5ab344cfe5594c7..46130fa7ca491aff1474a31f079189ec13fc1cc7 100644
--- a/matlab/iLQR_distMaintenance.m
+++ b/matlab/iLQR_distMaintenance.m
@@ -89,9 +89,9 @@ 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)) / param.dist;
- f = 1 - sum(e.^2, 1)'; %Residuals
- Jtmp = -e' * repmat(eye(param.nbVarU)/param.dist, 1, param.nbData);
+ 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);
J = Jtmp .* kron(eye(param.nbData), ones(1,param.nbVarU)); %Jacobians
end
@@ -100,10 +100,10 @@ end
%function [f, J] = f_dist(x, param)
% f=[]; J=[];
% for t=1:size(x,2)
-% e = (x(:,t) - param.Mu(1:2)) / param.dist;
-% ftmp = 1 - e' * e;
+% e = x(:,t) - param.Mu(1:2);
+% ftmp = 1 - e' * e / param.dist^2;
% f = [f; ftmp]; %Residuals
-% Jtmp = -e' / param.dist;
+% Jtmp = -e' / param.dist^2;
% J = blkdiag(J, Jtmp); %Jacobians
% end
%end