diff --git a/doc/rcfs.pdf b/doc/rcfs.pdf index c96fca73b8b4027cba7eae639b2888b43e7a7ba0..803d7f85f609cfaa0b7721b47fac5022daa30f10 100644 Binary files a/doc/rcfs.pdf and b/doc/rcfs.pdf differ diff --git a/doc/rcfs.tex b/doc/rcfs.tex index a5dc1117e5da929276c8290d395813620025ce73..3a03dc88cbba575094520bd41178b6d0daade9c0 100644 --- a/doc/rcfs.tex +++ b/doc/rcfs.tex @@ -2692,7 +2692,7 @@ A similar principle can be applied to robot manipulators by composing forward ki %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -\subsubsection{Distance to an object} %(1-e'*e)'*(1-e'*e) version, where 1-e'*e is a scalar +\subsubsection{Distance to a target} %(1-e'*e)'*(1-e'*e) version, where 1-e'*e is a scalar \begin{flushright} \filename{iLQR\_distMaintenance.*} \end{flushright} @@ -2703,12 +2703,12 @@ A similar principle can be applied to robot manipulators by composing forward ki \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. +\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 target 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 +The obstacle example above can easily be extended to the problem of reaching/maintaining a desired distance to a target, 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, @@ -2720,7 +2720,7 @@ that we exploit to define $\bm{f}(\bm{x}_t)$ and $\bm{J}(\bm{x}_t)$ in \eqref{eq \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. +In the above, $\bm{f}(\bm{x})$ defines a continuous function that is $0$ on a sphere of radius $r$ centered on the target (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. diff --git a/matlab/iLQR_distMaintenance.m b/matlab/iLQR_distMaintenance.m index 82a25b967a9c68f237e58b5c6b2b6093b8e81d79..f8cdb1df044a18998cfbc98595d29e120d94bf33 100644 --- a/matlab/iLQR_distMaintenance.m +++ b/matlab/iLQR_distMaintenance.m @@ -80,7 +80,6 @@ plot(x(1,:), x(2,:), '-','linewidth',2,'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