diff --git a/doc/images/intrinsicGeom_vs_extrinsicGeom01.jpg b/doc/images/intrinsicGeom_vs_extrinsicGeom01.jpg
index 317a53d258c6d514cfbf6bf69813d9f4d6b50f01..3fd11d46da03a10f4b8a25ef885db2fa6da8430b 100644
Binary files a/doc/images/intrinsicGeom_vs_extrinsicGeom01.jpg and b/doc/images/intrinsicGeom_vs_extrinsicGeom01.jpg differ
diff --git a/doc/images/linesearch01.png b/doc/images/linesearch01.png
index fdf393dac704c49eaad214a8fda6f0d98f4d0eb9..e7c18f8bb088d785178b038e2d60266b09bc5760 100755
Binary files a/doc/images/linesearch01.png and b/doc/images/linesearch01.png differ
diff --git a/doc/rcfs.pdf b/doc/rcfs.pdf
index 6b5784de458bf1cd3ea1f5bb7cc255dcb2c33552..0a656bd33ea11416c4d988c82d0c37e137eaaddd 100644
Binary files a/doc/rcfs.pdf and b/doc/rcfs.pdf differ
diff --git a/doc/rcfs.tex b/doc/rcfs.tex
index abda8708533461647d05bc376ed73787f7e3e2dc..2ce347cbebd4d877ff310a5f13e837ef05e9dd0d 100644
--- a/doc/rcfs.tex
+++ b/doc/rcfs.tex
@@ -1953,6 +1953,163 @@ In addition, the LQT formulation allows the resulting controller to be formalize
Moreover, the above problem formulation can be extended to iterative LQR, providing an opportunity to consider obstacle avoidance and constraints within the DMP formulation, as well as to describe costs in task space with a DMP acting in joint angle space.
+
+
+
+
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+\subsection{LQR with infinite time horizon}
+\begin{flushright}
+\filename{LQR\_infHor.*}
+\end{flushright}
+
+%https://math.stackexchange.com/questions/3119575/how-can-i-solve-the-discrete-algebraic-riccati-equations
+
+%A LQR problem with finite time horizon $T$ and homogeneous matrices $\bm{A}$, $\bm{B}$, $\bm{Q}$ and $\bm{R}$ corresponds to the minimization problem
+%\begin{equation}
+% \min_{\bm{u}} \sum_{t=1}^T \bm{x}_t^\trsp \bm{Q} \bm{x}_t + \bm{u}_t^\trsp \bm{R} \bm{u}_t
+% \quad\text{s.t.}\quad
+% \bm{x}_{t+1} = \bm{A} \bm{x}_t + \bm{B}\bm{u}_t,
+%\end{equation}
+%which requires the recursive computation of
+%\begin{equation}
+% \bm{P}_{t\!-\!1} = \bm{A}^\trsp \bm{P}_t \bm{A}
+% + \bm{Q} - \bm{A}^\trsp \bm{P}_t \bm{B} {( \bm{B}^\trsp \bm{P}_t \bm{B} + \bm{R})}^{-1} \bm{B}^\trsp \bm{P}_t \bm{A},
+% \label{eq:PT}
+%\end{equation}
+%to find the controller
+%\begin{align}
+% \bm{\hat{u}}_{t\!-\!1} &= -\bm{K}_{t\!-\!1} \, \bm{x}_{t\!-\!1},\\
+% \text{with}\quad
+% \bm{K}_{t\!-\!1} &= {( \bm{B}^\trsp \bm{P}_t \bm{B} + \bm{R})}^{-1} \bm{B}^\trsp \bm{P}_t \bm{A}.
+%\end{align}
+%In this problem, $\bm{P}_t$ converges to a steady state after a few backward recursions, starting from $\bm{P}_T$.
+
+We have seen in Section \ref{sec:LQRrecursive} that dynamic programming could be used to reduce the minimization in \eqref{eq:valueFunction} over the entire sequence of control commands $\bm{u}_{t:T-1}$ to a sequence of minimization problems over control commands at a single time step, by proceeding backwards in time. We obtained a backward recursion procedure in which $\bm{V}_t$ is evaluated recursively from $t=T-1$ to $t=1$, starting from $\bm{V}_T=\bm{Q}_T$, which corresponds to a Riccati equation, see Algorithm \ref{alg:LQRrecursive} for the summary of the overall procedure.
+
+An infinite time horizon corresponds to $T\rightarrow\infty$, characterized by the steady state
+\begin{equation}
+ \bm{P}_\infty = \bm{A}^\trsp \bm{P}_\infty \bm{A}
+ + \bm{Q} - \bm{A}^\trsp \bm{P}_\infty \bm{B} {( \bm{B}^\trsp \bm{P}_\infty \bm{B} + \bm{R} )}^{-1} \bm{B}^\trsp \bm{P}_\infty \bm{A}.
+\end{equation}
+The above equation can also be solved through a discrete algebraic Riccati equation (DARE).
+By rewriting the cost in \eqref{eq:PT} with a Lagragian formulation
+\begin{equation}
+ c = \sum_{t=1}^T \bm{x}_t^\trsp \bm{Q} \bm{x}_t + \bm{u}_t^\trsp \bm{R} \bm{u}_t
+ +\bm{\lambda}_t (\bm{A} \bm{x}_t + \bm{B}\bm{u}_t - \bm{x}_{t+1}),
+\end{equation}
+we can find the minimum by differentiating the cost and equating to zeros, providing
+\begin{align}
+ \frac{\partial c}{\partial\bm{\lambda}_t} &= \bm{A} \bm{x}_t + \bm{B}\bm{u}_t - \bm{x}_{t+1} = \bm{0},\\
+ \frac{\partial c}{\partial\bm{x}_t} &= \bm{Q} \bm{x}_t + \bm{A}^\trsp \bm{\lambda}_t - \bm{\lambda}_{t-1} = \bm{0},\\
+ \frac{\partial c}{\partial\bm{u}_t} &= \bm{R} \bm{u}_t + \bm{B}^\trsp \bm{\lambda}_t = \bm{0},
+\end{align}
+which can be rewritten as
+\begin{equation}
+ \begin{bmatrix} \bm{x}_{t+1} \\ \bm{\lambda}_t \end{bmatrix}
+ \!=\!
+ \underbrace{\begin{bmatrix} \bm{A} \!+\! \bm{B} \bm{R}^{-1} \! \bm{B}^\trsp \! \bm{A}^{-\trsp} \bm{Q} & -\bm{B} \bm{R}^{-1} \! \bm{B}^\trsp \! \bm{A}^{-\trsp} \\
+ -\bm{A}^{-\trsp} \bm{Q} & \bm{A}^{-\trsp} \end{bmatrix}}_{\bm{Z}} \!
+ \begin{bmatrix} \bm{x}_t \\ \bm{\lambda}_{t-1} \end{bmatrix},
+\end{equation}
+where $\bm{Z}$ is called a symplectic matrix.\footnote{For a continuous system, the same development would result in a Hamiltonian matrix.}
+
+The above DARE problem has multiple solutions, but only one results in a positive semi-definite $\bm{P}_\infty$. The symplectic matrix $\bm{Z}$ can be decomposed as $\bm{Z}=\bm{V}\bm{D}\bm{V}^\trsp$ (for example, with an eigendecomposition). For systems such as double integrators, it has a structure in which half of the eigenvalues are within the inner circle, which corresponds to the stable solution we are interested in.\footnote{For a continuous system, the corresponding eigenvalues have negative real part.}
+If we decompose the matrix $\bm{V}$ containing the eigenvectors as
+\begin{equation}
+ \bm{V} = \begin{bmatrix} \bm{V}_{11} & \bm{V}_{12} \\ \bm{V}_{21} & \bm{V}_{22} \end{bmatrix},
+\end{equation}
+the steady state solution of the Riccati equation can be described in terms of the corresponding eigenvectors as $\bm{P}_\infty = \bm{V}_{21} \bm{V}_{11}^{-1}$.
+
+
+%\subsubsection*{Continuous algebraic Riccati equation (CARE)}
+
+%For the infinite horizon setting, the cost
+%\begin{equation}
+% c = \sum_{t=1}^\infty \bm{x}_t^\trsp \bm{Q} \bm{x}_t + \bm{u}_t^\trsp \bm{R} \bm{u}_t
+%\end{equation}
+%can be minimized with the algebraic Riccati equation (ARE)
+%\begin{gather}
+% \bm{A}^\trsp \bm{P} + \bm{P} \bm{A} - \bm{P} \bm{B} \bm{R}^{-1}\!\bm{B}^\trsp\!\bm{P} + \bm{Q} = \bm{0}
+% \label{eq:Riccati2}
+% \\
+% \iff \begin{bmatrix}\bm{P} & -\bm{I} \end{bmatrix}
+% \underbrace{\begin{bmatrix} \bm{A} & -\bm{\bm{B} \bm{R}^{-1}\!\bm{B}^\trsp} \\ \bm{-Q} & \bm{-A^\trsp} \end{bmatrix}}_{\bm{H}} \begin{bmatrix}\bm{I} \\ \bm{P} \end{bmatrix}
+% = \bm{0} ,\label{eq:Riccati2b}
+%\end{gather}
+%where $\bm{H}$ is a Hamiltonian matrix.
+
+%Under suitable hypotheses about symmetry, stabilizability and detectability, \eqref{eq:Riccati2} has a unique positive semidefinite solution $\bm{P}$, which can be obtained by several methods. %, see \cite{Laub79} for details.
+
+%The key is to convert the problem to a stable invariant subspace problem of the Hamiltonian matrix, i.e., finding the invariant subspace corresponding to eigenvalues of $\bm{H}$ with negative real parts. This subspace can be found in several ways. The eigendecomposition approach (with ordered eigencomponents) decomposes $\bm{H}$ as
+%\begin{equation}
+% \bm{H} = \bm{V} \begin{bmatrix} \bm{\lambda}_1 & \bm{0} \\ \bm{0} & \bm{\lambda}_2 \end{bmatrix} \bm{V}^\trsp ,
+% \quad\mathrm{with}\quad
+% \bm{V} = \begin{bmatrix} \bm{V}_{11} & \bm{V}_{12} \\ \bm{V}_{21} & \bm{V}_{22} \end{bmatrix} ,
+% \label{eq:HEig}
+%\end{equation}
+%where $\begin{bmatrix}\bm{V}_{11}\\\bm{V}_{21}\end{bmatrix}$ forms the stable eigenspace of $\bm{H}$. We have that $\bm{H}\in\mathbb{R}^{4D\times 4D}$ for a double integrator as in \eqref{eq:doubleIntegrator}, which precisely has $2D$ eigenvalues with negative real parts. Together with \eqref{eq:Riccati2b}, it provides the ARE solution
+%\begin{equation}
+% \bm{P} = \bm{V}_{21} \bm{V}_{11}^{-1} .
+% \label{eq:PEig}
+%\end{equation}
+
+%%The eigenvector approach can be numerically unstable when the Hamiltonian matrix is close to defective, as can occur some cases in which H is close to a matrix whose Jordan form has ones above its main diagonal. In this case, the matrix $\bm{V}_1$ will be ill-conditioned. However, the ill-conditioning of $\bm{V}_1$ is independent of the conditioning of the Riccati equation.
+%%%\cite{Laub79,Paige81}.
+%%
+%%To circumvent the ill-conditioning problems associated with a defective Hamiltonian matrix, the same stable eigenspace of Hamiltonian can be spanned by Schur vectors. % \cite{Laub79}.
+%%In this case, an orthogonal transformation $\bm{U}$ is found to reduce $\bm{H}$ to real Schur form
+%%\begin{equation}
+%% \bm{H} = \bm{U} \begin{bmatrix} \bm{T}_1 & \bm{T}_{12} \\ \bm{0} & \bm{T}_2 \end{bmatrix} \bm{U}^\trsp ,\quad
+%% \mathrm{with}\quad
+%% \bm{U} = \begin{bmatrix} \bm{U}_1 & \bm{U}_{12} \\ \bm{U}_{21} & \bm{U}_2 \end{bmatrix} ,
+%% \label{eq:HSchur}
+%%\end{equation}
+%%ordered such that the eigenvalues of $\bm{T}_1$ are stable and those of $\bm{T}_2$ are unstable. This can be achieved with additional orthogonal transformations reordering the Schur form such that the negative eigenvalues precede the non-negative eigenvalues on the diagonal of the upper triangular matrices.
+%%
+%%In the above, $\begin{bmatrix}\bm{U}_1\\\bm{U}_{21}\end{bmatrix}$ spans for the same stable eigenspace of $\bm{H}$ as in \eqref{eq:HEig}, providing with \eqref{eq:Riccati2b} the ARE solution
+%%\begin{equation}
+%% \bm{P} = \bm{U}_{21} \bm{U}_1^{-1} .
+%% \label{eq:PSchur}
+%%\end{equation}
+%%
+%%Both \eqref{eq:PEig} and \eqref{eq:PSchur} solve \eqref{eq:Riccati2} with $\bm{P}=\bm{P}^\trsp$ positive definite.
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+\subsubsection*{Ideal damping ratio with DARE}
+
+As an example of application, in the case of diagonal stiffness and damping matrices, it is easy to show that the stiffness-damping ratio estimated by DARE corresponds to an ideal damping ratio.
+%, see for example the supplementary notes from \cite{Astrom08}.
+%
+%\begin{equation}
+% \begin{bmatrix}\dot{e}\\\ddot{e}\end{bmatrix}
+% =
+% \overbrace{\begin{bmatrix}0&1\\0&0\end{bmatrix}}^{\bm{A}}
+% \begin{bmatrix}e\\\dot{e}\end{bmatrix}
+% +
+% \overbrace{\begin{bmatrix}0\\1\end{bmatrix}}^{\bm{B}} u,
+% \label{eq:doubleIntegrator}
+%\end{equation}
+By defining a double integrator system with a state composed of 1D position and 1D velocity, and by setting the weighting terms
+\begin{equation}
+ \bm{Q}=\begin{bmatrix}c_1&0\\0&0\end{bmatrix}
+ \;,\quad R=c_2 ,
+\end{equation}
+with $c_1$ and $c_2$ constant scalars, the optimal feedback law found by the system is described by $u=-\kappa^\ty{P}e-\kappa^\ty{V}\dot{e}$, with $\kappa^\ty{V}=\sqrt{2\kappa^\ty{P}}$, which corresponds to the ideal underdamped damping ratio $x=\frac{\kappa^\ty{V}}{2\sqrt{\kappa^\ty{P}}} = \frac{1}{\sqrt{2}} \approx 0.7071$ in control design, optimizing the reaction speed while still allowing the system to asymptotically come back to its rest position.
+
+Similarly, for multivariate problems, the stiffness and damping matrices estimated by the system automatically satisfy the relation
+\begin{equation}
+ \bm{\hat{K}}^\ty{V} = \bm{V}(2\bm{D})^{\nicefrac{1}{2}}\bm{V}^\trsp,
+\end{equation}
+with $\bm{\hat{K}}^\ty{P}=\bm{V}\bm{D}\bm{V}^\trsp$ the eigendecomposition of $\bm{\hat{K}}^\ty{P}$, with unit eigenvectors represented as a matrix $\bm{V}$, and the squared eigenvalues represented in a diagonal matrix $\bm{D}$.
+
+
+
+
+
\newpage
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -3840,7 +3997,7 @@ The same principle can also be applied to task space by expressing all parts com
Since we used the term \emph{joint torques} in the previous section to refer to force commands at the joint angle level, we will use the term \emph{wrench} to refer to the forces at the level of the endeffector in task space. In the most generic case, the wrench $\bm{w}$ will be a 6D force vector by considering both translation and rotational parts (3D for each). The wrench $\bm{w}$ applied to the endeffector will then produce reaction torques at joint level, with $\bm{\tau}=\bm{J}(\bm{x})^\trsp \bm{w}$, corresponding to the principle of virtual work.
-By using the relations $\bm{\dot{f}}=\bm{J}(\bm{x})\bm{\dot{x}}$ and $\bm{\ddot{f}} = \bm{\dot{J}}(\bm{x}) \bm{\dot{x}} + \bm{J}(\bm{x}) \bm{\ddot{x}} \, \approx \bm{J}(\bm{x}) \bm{\ddot{x}}$, we can see that $\bm{\tau} = \bm{M}(\bm{x}) \, \bm{\ddot{x}}$ in the joint coordinate system becomes $\bm{w}=\bm{\Lambda}(\bm{x}) \, \bm{\ddot{f}}$, with $\bm{\Lambda}(\bm{x}) = {\Big( \bm{J}(\bm{x}) \, \bm{M}(\bm{x})^{-1} \bm{J}(\bm{x})^\trsp \Big)}^{-1}$ in the endeffector coordinate system.\footnote{We can see this with $\bm{J}^\trsp \bm{w} = \bm{M} \bm{\ddot{x}} \iff \bm{M}^{-1} \bm{J}^\trsp \bm{w} = \bm{\ddot{x}} \iff \bm{J} \bm{M}^{-1} \bm{J}^\trsp \bm{w} = \bm{J} \bm{\ddot{x}} \iff \bm{w} = {(\bm{J} \bm{M}^{-1} \bm{J}^\trsp)}^{-1} \bm{\ddot{f}}$.}
+By using the relations $\bm{\dot{f}}=\bm{J}(\bm{x})\bm{\dot{x}}$ and $\bm{\ddot{f}} = \bm{J}(\bm{x}) \bm{\ddot{x}} + \bm{\dot{J}}(\bm{x}) \bm{\dot{x}} \, \approx \bm{J}(\bm{x}) \bm{\ddot{x}}$, we can see that $\bm{\tau} = \bm{M}(\bm{x}) \, \bm{\ddot{x}}$ in the joint coordinate system becomes $\bm{w}=\bm{\Lambda}(\bm{x}) \, \bm{\ddot{f}}$, with $\bm{\Lambda}(\bm{x}) = {\Big( \bm{J}(\bm{x}) \, \bm{M}(\bm{x})^{-1} \bm{J}(\bm{x})^\trsp \Big)}^{-1}$ in the endeffector coordinate system.\footnote{We can see this with $\bm{J}^\trsp \bm{w} = \bm{M} \bm{\ddot{x}} \iff \bm{M}^{-1} \bm{J}^\trsp \bm{w} = \bm{\ddot{x}} \iff \bm{J} \bm{M}^{-1} \bm{J}^\trsp \bm{w} = \bm{J} \bm{\ddot{x}} \iff \bm{w} = {(\bm{J} \bm{M}^{-1} \bm{J}^\trsp)}^{-1} \bm{\ddot{f}}$.}
Similarly to \eqref{eq:impedcontroller}, we can then define a control law as
\begin{equation}
@@ -4135,10 +4292,12 @@ The movement in dashed lines show the baseline movements that would be produced
\end{figure*}
%\emph{Left:} Metric field constructed from a signed distance field, which allow to generate paths that naturally curve around obstacles when the obstacles are close. \emph{Right:} Metric field provided by inertia matrices to generate movements from one joint configuration to another while minimizing kinetic energy (in solid lines). The movement in dashed lines show the baseline movements that would be produced by ignoring inertia (corresponding to linear interpolation between the two robot poses).
-Manifolds with nonconstant curvature can also be employed, such as spaces endowed with a metric, characterized by a weighting matrix used to compute distances. Many problems in robotics can be formulated with such a smoothly varying matrix $\bm{G}(\bm{x})$ that can for example be used to evaluate displacements $\Delta\bm{x}$ as a quadratic error term $c(\Delta\bm{x})=\Delta\bm{x}^\trsp\bm{G}(\bm{x})\Delta\bm{x}$, forming a Riemannian metric that describes the underlying manifold (with non-homogeneous curvature). This weighting matrix can for example represent levels of kinetic energy or stiffness gains to model varying impedance behaviors. Computation is often more costly for these manifolds with nonconstant curvature, because it typically requires iterative algorithms instead of the direct analytic expressions typically provided by homogeneous manifolds.
+Manifolds with nonconstant curvature can also be employed, such as spaces endowed with a metric, characterized by a weighting matrix used to compute distances. Many problems in robotics can be formulated with such a smoothly varying matrix $\bm{G}(\bm{x})$ that can for example be used to evaluate velocity commands $\bm{u}$ %(or displacements $\Delta\bm{x}$)
+as a quadratic error term $c(\bm{u})=\bm{u}^\trsp\bm{G}(\bm{x})\bm{u}$, forming a Riemannian metric that describes the underlying manifold (with non-homogeneous curvature). This weighting matrix can for example represent levels of kinetic energy or stiffness gains to model varying impedance behaviors. Computation is often more costly for these manifolds with nonconstant curvature, because it typically requires iterative algorithms instead of the direct analytic expressions typically provided by homogeneous manifolds.
Figures \ref{fig:intrinsicGeom_vs_extrinsicGeom} and \ref{fig:nonhomogeneousManifolds} presents examples exploiting non-homogeneous Riemannian manifolds.
+%+computation with local optimization or wavefront propagation
\newpage
\bibliographystyle{plain}
diff --git a/matlab/FD.m b/matlab/FD.m
index 614d8605f5b6bd9d6b6fd6b866569dcd7896bc38..f7be71bb9fe5acb04508bdfc1324ab6369dfa4f7 100644
--- a/matlab/FD.m
+++ b/matlab/FD.m
@@ -12,7 +12,7 @@ function FD
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
param.dt = 1E-2; %Time step size
-param.nbData = 500; %Number of datapoints
+param.nbData = 2000; %Number of datapoints
param.nbPoints = 2; %Number of viapoints
param.nbVarX = 3; %Position space dimension (q1,q2,q3) --> State Space (q1,q2,q3,dq1,dq2,dq3)
param.nbVarU = 3; %Control space dimension (tau1,tau2,tau3)
@@ -20,12 +20,12 @@ param.nbVarF = 3; %Objective function dimension (x1,x2,o)
param.l = [1, 1, 1]; %Robot links lengths
param.m = [1, 1, 1]; %Robot links masses
param.g = 9.81; %gravity norm
-param.kv = 1*0; %Joint damping
+param.kv = 10.0; %Joint damping
%% Iterative LQR (iLQR)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tau = zeros(param.nbVarU*(param.nbData-1), 1); %Torque commands applied in each joint
-x = [-pi/2; 0; 0; zeros(param.nbVarX,1)]; %Initial robot state
+x = [-pi/2+pi/4; +pi/4; 0; zeros(param.nbVarX,1)]; %Initial robot state
%Auxiliary matrices
l = param.l;
@@ -35,7 +35,6 @@ nbData = (size(tau,1) / nbDOFs) + 1;
Lm = triu(ones(nbDOFs)) .* repmat(param.m, nbDOFs, 1);
L = tril(ones(nbDOFs));
-
%Forward Dynamics
for t=1:nbData-1
% %Elementwise computation of G, M, C
@@ -56,19 +55,16 @@ for t=1:nbData-1
C = L' * C;
M = L' * M * L;
- %Update pose
- tau_t = tau((t-1)*nbDOFs+1:t*nbDOFs);
-
- %To debug
- tau_t = zeros(nbDOFs,1);
- G = 0;
- C = 0;
-% M = 1;
- tau_t(1) = 1;
+% %Can be used as a test
+% tau = zeros(nbDOFs,1);
+% G = 0;
+% C = 0;
+%% M = 1;
+% tau(1) = 1;
- %ddq = M \ (tau + G + C * (q(nbDOFs+1:2*nbDOFs,t)).^2 - inv(L') * q(nbDOFs+1:2*nbDOFs,t) * param.kv) ; %With force damping
- ddx = M \ (tau_t + G + C * (L * x (nbDOFs+1:2*nbDOFs,t)).^2) - L * x(nbDOFs+1:2*nbDOFs,t) * param.kv ; %With joint damping
- x(:,t+1) = x(:,t) + [x(nbDOFs+1:2*nbDOFs,t); ddx] * dt;
+ tau_t = tau((t-1)*nbDOFs+1:t*nbDOFs);
+ ddx = M \ (tau_t + G + C * (L * x (nbDOFs+1:2*nbDOFs,t)).^2 - x(nbDOFs+1:2*nbDOFs,t) * param.kv);
+ x(:,t+1) = x(:,t) + [x(nbDOFs+1:2*nbDOFs,t); ddx] * dt; %Update pose
end
@@ -85,7 +81,7 @@ for t=1:param.nbData
set(h1, 'XData', f(1,:), 'YData', f(2,:));
set(h2, 'XData', f(1,:), 'YData', f(2,:));
drawnow;
- pause(param.dt * 1E-1); %Speeding up the visualization of the simulation
+ pause(param.dt * 1E-3); %Speeding up the visualization of the simulation
end
waitfor(h);
@@ -101,7 +97,6 @@ function f = fkin0(x, param)
f = [zeros(2,1), f];
end
-
%%%%%%%%%%%%%%%%%%%%%%
% heaviside function (not available in octave)
function y = heaviside(x)
diff --git a/matlab/LQR_infHor.m b/matlab/LQR_infHor.m
index 6604df34f29a7895993116cd5d8cc1f324f0158a..386c0afa0abc0f1b668be47f10343748fff4b4ae 100644
--- a/matlab/LQR_infHor.m
+++ b/matlab/LQR_infHor.m
@@ -1,7 +1,7 @@
-%% Point-mass LQR with infinite horizon
+%% Point-mass discrete LQR with infinite horizon
%%
-%% Copyright (c) 2023 Idiap Research Institute <https://www.idiap.ch/>
-%% Written by Sylvain Calinon <https://calinon.ch>
+%% Copyright (c) 2024 Idiap Research Institute <https://www.idiap.ch/>
+%% Written by Ante Marić <ante.maric@idiap.ch> and Sylvain Calinon <https://calinon.ch>
%%
%% This file is part of RCFS <https://robotics-codes-from-scratch.github.io/>
%% License: GPL-3.0-only
@@ -21,12 +21,15 @@ param.rfactor = 4E-2; %Control cost in LQR
%Control cost matrix
R = eye(param.nbVarPos) * param.rfactor;
-%Target and desired covariance
+%Target and desired covariance (for precision matrix on static features)
param.Mu = [randn(param.nbVarPos,1); zeros(param.nbVarPos*(param.nbDeriv-1),1)];
[Ar,~] = qr(randn(param.nbVarPos));
xCov = Ar * diag(rand(param.nbVarPos,1)) * Ar' * 1E-1;
+%Target and desired covariance (for precision matrix on static and dynamic features)
+%param.Mu = [randn(param.nbVarPos,1); zeros(param.nbVarPos*(param.nbDeriv-1),1)];
+%xCov = diag([rand(2,1); rand(2,1) / param.dt]) * 1E-1;
%% Discrete dynamical System settings
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -44,7 +47,12 @@ B = kron(B1d, eye(param.nbVarPos)); %Discrete nD
%% discrete LQR with infinite horizon
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-Q = blkdiag(inv(xCov), zeros(param.nbVarPos*(param.nbDeriv-1))); %Precision matrix
+% Precision matrix on static features
+Q = blkdiag(inv(xCov), zeros(param.nbVarPos*(param.nbDeriv-1)));
+
+% Precision matrix on static and dynamic features
+% Q = inv(xCov);
+
P = solveAlgebraicRiccati_eig_discrete(A, B*(R\B'), (Q+Q')/2);
L = (B' * P * B + R) \ B' * P * A; %Feedback gain (discrete version)
@@ -59,7 +67,7 @@ for n=1:param.nbRepros
x = [ones(param.nbVarPos,1)+randn(param.nbVarPos,1)*5E-1; zeros(param.nbVarPos*(param.nbDeriv-1),1)];
for t=1:param.nbData
r(n).Data(:,t) = x;
- u = L * (param.Mu - x); %Compute acceleration (with only feedback terms)
+ u = L * (param.Mu - x); %Compute control input (with only feedback terms)
x = A * x + B * u;
end
end
@@ -110,7 +118,7 @@ function X = solveAlgebraicRiccati_eig_discrete(A, G, Q)
n = size(A,1);
%Symplectic matrix (see https://en.wikipedia.org/wiki/Algebraic_Riccati_equation)
- %Z = [A+B*(R\B')/A'*Q, -B*(R\B')/A'; -A'\Q, A'^-1];
+ %Z = [A+B*(R\B')/A'*Q, -B*(R\B')/A'; -A'\Q, A'^-1];
Z = [A+G/A'*Q, -G/A'; -A'\Q, inv(A')];
%Since Z is symplectic, if it does not have any eigenvalues on the unit circle,
diff --git a/matlab/ergodic_control_SMC_2D.m b/matlab/ergodic_control_SMC_2D.m
index 4dd7ff8dcc2c3864282570e897187d0250abb591..5653622a8871ecf7df5544a4c8b2692e11d436b7 100644
--- a/matlab/ergodic_control_SMC_2D.m
+++ b/matlab/ergodic_control_SMC_2D.m
@@ -107,6 +107,7 @@ for t=1:nbData
% r.e(t) = sum(sum((w-w_hat).^2 .* Lambda)); %Reconstruction error evaluation
end
+
% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
h = figure('position',[10,10,1800,600]);
diff --git a/matlab/iLQR_manipulator.m b/matlab/iLQR_manipulator.m
index 47c5bc9629b6a37f83e34f261cb59a6911a3b575..5a69c5f996f48ddafc340c2f40a9513c8d011066 100644
--- a/matlab/iLQR_manipulator.m
+++ b/matlab/iLQR_manipulator.m
@@ -12,7 +12,7 @@ function iLQR_manipulator
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
param.dt = 1E-2; %Time step length
param.nbData = 50; %Number of datapoints
-param.nbIter = 300; %Maximum number of iterations for iLQR
+param.nbMaxIter = 100; %Maximum number of iterations for iLQR
param.nbPoints = 2; %Number of viapoints
param.nbVarX = 3; %State space dimension (x1,x2,x3)
param.nbVarU = 3; %Control space dimension (dx1,dx2,dx3)
@@ -48,7 +48,7 @@ Su0 = [zeros(param.nbVarX, param.nbVarX*(param.nbData-1)); tril(kron(ones(param.
Sx0 = kron(ones(param.nbData,1), eye(param.nbVarX));
Su = Su0(idx,:);
-for n=1:param.nbIter
+for n=1:param.nbMaxIter
x = reshape(Su0 * u + Sx0 * x0, param.nbVarX, param.nbData); %System evolution
[f, J] = f_reach(x(:,tl), param); %Residuals and Jacobians
du = (Su' * J' * Q * J * Su + R) \ (-Su' * J' * Q * f(:) - u * param.r); %Gauss-Newton update
diff --git a/matlab/iLQR_manipulator_basic.m b/matlab/iLQR_manipulator_basic.m
new file mode 100644
index 0000000000000000000000000000000000000000..4b915386d9e53fc27882ae1ed313e5572bc08f04
--- /dev/null
+++ b/matlab/iLQR_manipulator_basic.m
@@ -0,0 +1,131 @@
+%% iLQR applied to a planar manipulator for a viapoints task (batch formulation without orientation)
+%%
+%% Copyright (c) 2024 Idiap Research Institute <https://www.idiap.ch>
+%% Written by Sylvain Calinon <https://calinon.ch>
+%%
+%% This file is part of RCFS <https://rcfs.ch>
+%% License: GPL-3.0-only
+
+function iLQR_manipulator_basic
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+param.dt = 1E-2; %Time step length
+param.nbData = 50; %Number of datapoints
+param.nbMaxIter = 100; %Maximum number of iterations for iLQR
+param.nbPoints = 2; %Number of viapoints
+param.nbVarX = 3; %State space dimension (x1,x2,x3)
+param.nbVarU = 3; %Control space dimension (dx1,dx2,dx3)
+param.nbVarF = 2; %Objective function dimension (f1,f2)
+param.l = [2; 2; 1]; %Robot links lengths
+param.r = 1E-6; %Control weight term
+param.Mu = [[2; 1], [3; 2]]; %Viapoints
+
+R = speye((param.nbData-1)*param.nbVarU) * param.r; %Control weight matrix (at trajectory level)
+Q = speye(param.nbVarF * param.nbPoints) * 1E0; %Precision matrix
+
+%Time occurrence of viapoints
+tl = linspace(1, param.nbData, param.nbPoints+1);
+tl = round(tl(2:end));
+idx = (tl - 1) * param.nbVarX + [1:param.nbVarX]';
+
+
+%% Iterative LQR (iLQR)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+u = zeros(param.nbVarU*(param.nbData-1), 1); %Initial commands
+x0 = [3*pi/4; -pi/2; -pi/4]; %Initial robot pose
+
+%Transfer matrices (for linear system as single integrator)
+Su0 = [zeros(param.nbVarX, param.nbVarX*(param.nbData-1)); tril(kron(ones(param.nbData-1), eye(param.nbVarX)*param.dt))];
+Sx0 = kron(ones(param.nbData,1), eye(param.nbVarX));
+Su = Su0(idx,:);
+
+for n=1:param.nbMaxIter
+ x = reshape(Su0 * u + Sx0 * x0, param.nbVarX, param.nbData); %System evolution
+ [f, J] = f_reach(x(:,tl), param); %Residuals and Jacobians
+ du = (Su' * J' * Q * J * Su + R) \ (-Su' * J' * Q * f(:) - u * param.r); %Gauss-Newton update
+
+ %Estimate step size with backtracking line search method
+ alpha = 1;
+ cost0 = f(:)' * Q * f(:) + norm(u)^2 * param.r; %Cost
+ while 1
+ utmp = u + du * alpha;
+ xtmp = reshape(Su0 * utmp + Sx0 * x0, param.nbVarX, param.nbData); %System evolution
+ ftmp = f_reach(xtmp(:,tl), param); %Residuals
+ cost = ftmp(:)' * Q * ftmp(:) + norm(utmp)^2 * param.r; %Cost
+ if cost < cost0 || alpha < 1E-3
+ break;
+ end
+ alpha = alpha * 0.5;
+ end
+ u = u + du * alpha; %Update
+
+ if norm(du * alpha) < 1E-2
+ break; %Stop iLQR when solution is reached
+ end
+end
+disp(['iLQR converged in ' num2str(n) ' iterations.']);
+
+
+%% Plot state space
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+h = figure('position',[10,10,800,800],'color',[1,1,1]); hold on; axis off;
+colMat = lines(param.nbPoints);
+
+ftmp = fkin0(x(:,1), param);
+plot(ftmp(1,:), ftmp(2,:), '-','linewidth',4,'color',[.8 .8 .8]);
+
+ftmp = fkin0(x(:,tl(1)), param);
+plot(ftmp(1,:), ftmp(2,:), '-','linewidth',4,'color',[.6 .6 .6]);
+
+ftmp = fkin0(x(:,tl(2)), param);
+plot(ftmp(1,:), ftmp(2,:), '-','linewidth',4,'color',[.4 .4 .4]);
+
+for t=1:param.nbPoints
+ plot(param.Mu(1,t), param.Mu(2,t), '.','markersize',40,'color',colMat(t,:));
+end
+
+ftmp = fkin(x, param);
+plot(ftmp(1,:), ftmp(2,:), 'k-','linewidth',2);
+plot(ftmp(1,[1,tl]), ftmp(2,[1,tl]), 'k.','markersize',20);
+axis equal;
+
+waitfor(h);
+end
+
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Forward kinematics for end-effector (in robot coordinate system)
+function f = fkin(x, param)
+ L = tril(ones(size(x,1)));
+ f = [param.l' * cos(L * x); ...
+ param.l' * sin(L * x)]; %f1,f2,f3, where f3 is the orientation (single Euler angle for planar robot)
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Forward kinematics for all robot articulations (in robot coordinate system)
+function f = fkin0(x, param)
+ L = tril(ones(size(x,1)));
+ f = [L * diag(param.l) * cos(L * x), ...
+ L * diag(param.l) * sin(L * x)]';
+ f = [zeros(2,1), f];
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Jacobian with analytical computation (for single time step)
+function J = Jkin(x, param)
+ L = tril(ones(size(x,1)));
+ J = [-sin(L * x)' * diag(param.l) * L; ...
+ cos(L * x)' * diag(param.l) * L];
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Residuals f and Jacobians J for a viapoints reaching task (in object coordinate system)
+function [f, J] = f_reach(x, param)
+ f = fkin(x, param) - param.Mu; %Residuals by ignoring manifold
+ J = [];
+ for t=1:size(x,2)
+ Jtmp = Jkin(x(:,t), param);
+ J = blkdiag(J, Jtmp);
+ end
+end
diff --git a/matlab/iLQR_manipulator_dynamics.m b/matlab/iLQR_manipulator_dynamics.m
index d15bd3fddd58a31b7bf79510f1b81984307c1abe..8773823aa52b66c43ed902a8343c32f6c6dbce10 100644
--- a/matlab/iLQR_manipulator_dynamics.m
+++ b/matlab/iLQR_manipulator_dynamics.m
@@ -140,7 +140,7 @@ end
%%%%%%%%%%%%%%%%%%%%%%
%Jacobian with analytical computation (for single time step)
-function J = jkin(x, param)
+function J = Jkin(x, param)
L = tril(ones(size(x,1)));
J = [-sin(L * x)' * diag(param.l) * L; ...
cos(L * x)' * diag(param.l) * L; ...
@@ -158,7 +158,7 @@ function [f, J] = f_reach(x, param)
% f(:,t) = logmap(fkin(x(:,t), param), param.Mu(:,t));
f(1:2,t) = param.A(:,:,t)' * f(1:2,t); %Object-centered FK
- Jtmp = jkin(x(:,t), param);
+ Jtmp = Jkin(x(:,t), param);
Jtmp(1:2,:) = param.A(:,:,t)' * Jtmp(1:2,:); %Object-centered Jacobian
%Bounding boxes (optional)
@@ -194,7 +194,6 @@ function [x, Su] = fwdDyn(x, u,param)
S1(:,:,j) = repmat(J_index * eye(nbDOFs), nbDOFs, 1) - repmat(eye(nbDOFs) * J_index', 1, nbDOFs);
end
-
for t=1:nbData-1
%Computation in matrix form of J, G, M, C
G = -sum(Lm,2) .* l' .* cos(x(1:nbDOFs, t)) * g;
@@ -210,7 +209,7 @@ function [x, Su] = fwdDyn(x, u,param)
%Update pose
u_t = u((t-1) * nbDOFs + 1:t * nbDOFs);
- ddx = (M) \ (u_t + G + C * (x(nbDOFs+1:2*nbDOFs,t)).^2 - inv(L') * x(nbDOFs + 1:2 * nbDOFs,t) * kv); %With external force and joint damping
+ ddx = M \ (u_t + G + C * (x(nbDOFs+1:2*nbDOFs,t)).^2 - inv(L') * x(nbDOFs + 1:2 * nbDOFs,t) * kv); %With external force and joint damping
x(:,t+1) = x(:,t) + [x(nbDOFs + 1:2 * nbDOFs, t); ddx] * param.dt;
%Compute local linear systems
diff --git a/python/FD.py b/python/FD.py
index 60adc957963579c3b55a67f9159b8a7ee688f9ab..da9e3779f0641ee4e576d02304375fa3f8671206 100644
--- a/python/FD.py
+++ b/python/FD.py
@@ -49,13 +49,13 @@ def animate(i):
# ===============================
param = lambda: None # Lazy way to define an empty class in python
param.dt = 1E-2 # Time step length
-param.nbData = 20 # Number of datapoints
+param.nbData = 2000 # Number of datapoints
param.nbVarX = 3 # State space dimension (x1,x2,x3)
param.nbVarU = 3 # Control space dimension (dx1,dx2,dx3)
param.nbVarF = 3 # Objective function dimension (f1,f2,f3, with f3 as orientation)
param.l = np.array([1, 1, 1]) # Robot links lengths
param.m = np.array([1, 1, 1]) # Robot links masses
-param.damping = 1 # Viscous friction
+param.damping = 10.0 # Viscous friction
param.gravity = 9.81 # Gravity
# Auxiliary matrices
@@ -107,6 +107,6 @@ plt.gca().set_aspect('equal', adjustable='box')
#xdata, ydata = [], []
ln1, = plt.plot([], [], '-')
ln2, = plt.plot([], [], 'o-', linewidth=2, markersize=5, c="black")
-ani = animation.FuncAnimation(fig, animate, x.shape[1], init_func=init, interval = param.dt * 500, blit= True, repeat = False)
+ani = animation.FuncAnimation(fig, animate, x.shape[1], init_func=init, interval = param.dt * 10, blit= True, repeat = False)
plt.show()
diff --git a/python/IK_manipulator_manipulability.py b/python/IK_manipulator_manipulability.py
index ba110964de82621966d665a6aa733d3dadfc7b45..e971b29103af250bfb3946b04f4aedca6b5ead89 100644
--- a/python/IK_manipulator_manipulability.py
+++ b/python/IK_manipulator_manipulability.py
@@ -73,7 +73,6 @@ x[0] = x[0] + np.pi
## Inverse kinematics (IK)
# ===============================
-
ax.scatter(fh[0], fh[1], color='r', marker='.', s=10**2) #Plot target
for t in range(param.nbData):
f = fkin(x, param) # Forward kinematics (for end-effector)
@@ -83,72 +82,15 @@ for t in range(param.nbData):
f_rob = fkin0(x, param) # Forward kinematics (for all articulations, including end-effector)
ax.plot(f_rob[0,:], f_rob[1,:], color=str(1-t/param.nbData), linewidth=2) # Plot robot
-
-
### MANIPULABILITY ###
-
J = J[:2,:]
center = np.array([f[0], f[1]]) # end-effector position
+print(f"Center: {center}")
length, width, height = 1.8,1.5,1 # max joint velocities
size = np.array([length, width, height])
-refell = 130 * np.identity(2) # reference ellipsoid
-
-# Choice of the Jacobian matrix
-J1 = False
-J2 = False
-J3 = False
-J4 = False
-diffJac = [J1, J2, J3, J4]
-
-
-# 1. Robot manipulator
-if J1 == True:
- theta = 5*m.pi/6
- U = np.array([[m.cos(theta), -m.sin(theta)],[m.sin(theta), m.cos(theta)]])
- J = U.T @ J
- print(J)
-
-# 2. Bounded joint-space
-if J2 == True:
- jminlim = -np.ones(param.nbVarX)
- jmaxlim = np.ones(param.nbVarX)
- J = np.diag(1 - np.heaviside(x - jminlim,0)*np.heaviside(jmaxlim - x, 0))[:2,:]
- print(J)
-
-
-#Â 3. Bounded task-space
-if J3 == True:
- tminlim = -np.ones(2)
- tmaxlim = np.ones(2)
- J = np.diag(1 - np.heaviside(f[:2] - tminlim,0)*np.heaviside(tmaxlim - f[:2], 0)) @ J
- print(J)
-
-# 4. Object boundaries
-if J4 == True:
- theta = m.pi/4
- U = np.array([[m.cos(theta), -m.sin(theta)],[m.sin(theta), m.cos(theta)]])
- tminlim = -np.ones(2)
- tmaxlim = 2*np.ones(2)
- J = np.diag(1 - np.heaviside(U.T@(f[:2] - fh[:2]) - tminlim,0)*np.heaviside(tmaxlim - (U.T@(f[:2] - fh[:2])), 0)) @ J
- print(J)
-
-
-
-# Boundaries in joint-velocity space
-
-# 1. Rectangular cuboid
-showedges = False # Shows the mapping of the cube's edges
-
-# 2. Ellipse
-ellBound = True
-# 3. Superellipsoid
-superBound = False
-superVolume = False # Returns the fraction of the rectangular cuboid's volume covered by the superellipsoid
-
-
-# 1. Rectangular cuboid
+# initialize a rectangular cuboid for polytope
cube = np.zeros((2 ** param.nbVarX, param.nbVarX))
vertex = np.zeros(param.nbVarX)
# These two loops store the numbers 0 to 7 in binary (which can be seen as the coordinates of a cube)
@@ -159,12 +101,10 @@ for count1 in range(2 ** param.nbVarX):
# Rescaling so that the center of the cube is located at the origin
cube = cube * 2 - 1
-
-
for i in range(len(size)):
cube[:,i] = cube[:,i] * size[i]
-# Computation of the manipulability polytope
+# Computation of the manipulability polytope (blue)
polytope = np.zeros((2 ** param.nbVarX,2))
for count in range(2 ** param.nbVarX):
polytope[count] = J @ cube[count] + center
@@ -173,17 +113,13 @@ xpoints = polytope[:,0]
ypoints = polytope[:,1]
polytope = np.array([xpoints, ypoints]).T
-if not any(diffJac) == True:
- hull = scipy.spatial.ConvexHull(polytope)
-
- # vertices of the covex hull (might come in handy)
- vertices = np.zeros((len(hull.vertices),2))
- for i in range(len(hull.vertices)):
- vertices[i] = polytope[hull.vertices[i]]
- cube_norms = np.linalg.norm(vertices, axis = 1)
-
- for simplex in hull.simplices:
- plt.plot(polytope[simplex, 0], polytope[simplex, 1], 'b--')
+hull = scipy.spatial.ConvexHull(polytope)
+vertices = np.zeros((len(hull.vertices),2))
+for i in range(len(hull.vertices)):
+ vertices[i] = polytope[hull.vertices[i]]
+ax.plot(xpoints, ypoints, "kx")
+for simplex in hull.simplices:
+ plt.plot(polytope[simplex, 0], polytope[simplex, 1], 'b--')
def norm(vec, coeff, exp):
@@ -199,189 +135,26 @@ def sample(npoints, coeff, exp):
vecs[count] = vecs[count] / norm(vecs[count], coeff, exp)
return vecs
-# 2. Ellipsoid
-if ellBound == True:
- num_iter = 1000
- # coeff = np.array([1,1,1]) # these are the dimensions of the superellipsoid in joint-velocity space
- coeff = size # if one wants the superellipsoid to be contained in the cuboid
- exp = 2
+# plot the manipulability ellipsoid (red)
+num_iter = 1000
+coeff = np.array([1,1,1]) # these are the dimensions of the superellipsoid in joint-velocity space
+# coeff = size # if one wants the superellipsoid to be contained in the cuboid
- ell_jvlim = sample(num_iter, coeff, 2)
-
- ell_tvlim = np.zeros((num_iter,2))
+ell_jvlim = sample(num_iter, coeff, 2)
- for count in range(len(ell_jvlim)):
- ell_tvlim[count] = J @ ell_jvlim[count] + center
+ell_tvlim = np.zeros((num_iter,2))
- ell_x, ell_y = ell_tvlim.T
+for count in range(len(ell_jvlim)):
+ ell_tvlim[count] = J @ ell_jvlim[count] + center
- A = np.diag(coeff ** 2)
- Q = J @ A @ J.T
- eigenvals, eigenvecs = np.linalg.eig(Q)
-
- # Sort Eigenvalues and EigenVectors
- idx = eigenvals.argsort()[::-1]
- eigenvals = eigenvals[idx]
- eigenvecs = eigenvecs[idx]
+ell_x, ell_y = ell_tvlim.T
- # the sqrt of the eigenvalues give the length of the semi-axes
- print(f"Ellipsoid eigenvalues: {eigenvals}")
- vec1, vec2 = eigenvecs.T
- vec1 = vec1 * m.sqrt(eigenvals[0]) + center
- vec2 = vec2 * m.sqrt(eigenvals[1]) + center
+polytope = np.array([ell_x, ell_y]).T
+hull = scipy.spatial.ConvexHull(polytope)
+for simplex in hull.simplices:
+ plt.plot(polytope[simplex, 0], polytope[simplex, 1], 'r--')
- if not any(diffJac) == True:
- polytope = np.array([ell_x, ell_y]).T
- hull = scipy.spatial.ConvexHull(polytope)
- # vertices of the covex hull (might come in handy)
- #vertex = np.zeros((len(hull.vertices),2))
- #for i in range(len(hull.vertices)):
- # vertex[i] = polytope[hull.vertices[i]]
- #ax.plot(vertex[:,0], vertex[:,1], "gv")
-
- for simplex in hull.simplices:
-
- plt.plot(polytope[simplex, 0], polytope[simplex, 1], 'r--')
-
-
-
-# 3. Superellipsoid (rigorously this is not the most general form of a superellipsoid)
-if superBound == True:
- num_iter = 1000
- # coeff = np.array([1,1,1]) # these are the dimensions of the superellipsoid in joint-velocity space
- coeff = size # if one wants the superellipsoid to be contained in the cuboid
- exp = 4 # exp = 2 for an ellipse, exp = 4 for squircle, exp --> infty for rectangular cuboid
-
- if superVolume == True:
- vol = scipy.special.gamma(1/exp + 1)**param.nbVarX/scipy.special.gamma(param.nbVarX/exp + 1)
- print(f"fraction of the rectangular cuboid's volume: {vol}")
-
- jvlim = sample(num_iter, coeff, exp)
-
- tvlim = np.zeros((num_iter,2))
-
- for count in range(len(jvlim)):
- tvlim[count] = J @ jvlim[count] + center
-
-
- xpoints, ypoints = tvlim.T
-
- # Idea: approximate whatever shape I get with an ellipsoid, so that the reasoning on the eigenvalues apply!
- # Note: it does not just give the same ellipsoid as if exp = 2
-
- tvmax = tvlim[np.argmax(np.linalg.norm(tvlim-center, axis = 1))]
- cov_mat = np.cov(tvlim.T)
- eigenvals, eigenvecs = np.linalg.eig(cov_mat)
- idx = eigenvals.argsort()[::-1]
- eigenvals = eigenvals[idx]
- eigenvecs = eigenvecs[:,idx]
- eigenvecs = eigenvecs * np.sqrt(eigenvals)
- ratio = np.linalg.norm(tvmax-center)/m.sqrt(eigenvals[0])
- eigenvecs *= ratio
-
- vec1, vec2 = eigenvecs.T[0],eigenvecs.T[1]
-
- '''
- # Manipulability matrix
- Q = eigenvecs @ np.array([[eigenvals[0],0],[0,eigenvals[1]]]) @ np.linalg.inv(eigenvecs)
-
- # Riemannian distance
- A = np.linalg.inv(scipy.linalg.sqrtm(refell)) @ Q @ np.linalg.inv(scipy.linalg.sqrtm(refell))
- d = np.linalg.norm(scipy.linalg.logm(A))
- print(f"Riemannian distance: {d}")
- '''
-
- print(f"Superellipsoid eigenvalues: {(ratio * np.sqrt(eigenvals))**2}")
-
- phi = np.linspace(0, 2*m.pi,200)
- x = np.zeros((len(phi),2))
-
- for i in range(len(phi)):
- x[i] = center + vec1 * m.cos(phi[i]) + vec2 * m.sin(phi[i])
-
- super_norms = np.linalg.norm(x,axis = 1)
-
- if not any(diffJac) == True:
- ax.plot(x[:,0], x[:,1], "g1", label = "superellipsoid")
- vec1 += center
- vec2 += center
- ax.plot([center[0], tvmax[0]], [center[1],tvmax[1]])
- ax.plot([center[0], vec1[0]], [center[1],vec1[1]])
- ax.plot([center[0], vec2[0]], [center[1],vec2[1]])
-
-
-# Plots
-showhull = True # to show the convex hull of the superellipsoid
-showpoints = False # to show the image of all the sampled points
-
-
-if showhull == True and not any(diffJac) == True:
- polytope = np.array([xpoints, ypoints]).T
- hull = scipy.spatial.ConvexHull(polytope)
-
- # vertices of the covex hull (might come in handy)
- #vertex = np.zeros((len(hull.vertices),2))
- #for i in range(len(hull.vertices)):
- # vertex[i] = polytope[hull.vertices[i]]
- #ax.plot(vertex[:,0], vertex[:,1], "gv")
-
- for simplex in hull.simplices:
-
- plt.plot(polytope[simplex, 0], polytope[simplex, 1], 'g--')
-
-if showpoints == True:
- ax.plot(xpoints, ypoints, "kx")
-
-#fig = plt.figure()
-#ax2 = fig.add_subplot(projection='3d')
-
-#ax2.scatter(cube[:,0], cube[:,1], cube[:,2], c = "blue", label = "rectangular cuboid")
-
-#if ellBound == True:
-# ax2.scatter(ell_jvlim[:,0], ell_jvlim[:,1], ell_jvlim[:,2], c = "red", label = "ellipsoid")
-#
-#if superBound == True:
-# ax2.scatter(jvlim[:,0], jvlim[:,1], jvlim[:,2], c = "green", label = "superellipsoid")
-
-#legend = ax2.legend(loc='upper right')
-
-
-#if showedges == True:
-# num_points = 50
-# jvlim, tvlim = np.zeros((num_points,3)), np.zeros((num_points,2))
-#
-# edges = np.vstack((np.unique(cube[:,:2], axis = 0), np.unique(cube[:,1:3], axis = 0), np.unique(cube[:,0:3:2], axis = 0)))
-# for edge in edges[:4]:
-# for count in range(num_points):
-# z = (random.random() * 2 - 1) * height
-# jvlim[count] = np.array([edge[0],edge[1],z])
-# tvlim[count] = J @ jvlim[count] + center
-# ax2.scatter(jvlim[:,0], jvlim[:,1], jvlim[:,2], c = "blue")
-# ax.plot(tvlim[:,0], tvlim[:,1], "bx")
-
-# for edge in edges[4:8]:
-# for count in range(num_points):
-# x = (random.random() * 2 - 1) * length
-# jvlim[count] = np.array([x,edge[0],edge[1]])
-# tvlim[count] = J @ jvlim[count] + center
-# ax2.scatter(jvlim[:,0], jvlim[:,1], jvlim[:,2], c = "red")
-# ax.plot(tvlim[:,0], tvlim[:,1], "rx")
-#
-# for edge in edges[8:]:
-# for count in range(num_points):
-# y = (random.random() * 2 - 1) * width
-# jvlim[count] = np.array([edge[0],y,edge[1]])
-# tvlim[count] = J @ jvlim[count] + center
-# ax2.scatter(jvlim[:,0], jvlim[:,1], jvlim[:,2], c = "green")
-# ax.plot(tvlim[:,0], tvlim[:,1], "gx")
-#
-
-
-#ax.axis('off')
ax.axis('equal')
-#ax2.axis('equal')
-#plt.title(f"Length: {length}, width: {width}, height: {height}, p = {exp}")
-
plt.show()
diff --git a/python/LQR_infHor.py b/python/LQR_infHor.py
index 4ea88c1196d2ec66dfa2095c1a0ca9c950530e68..b125afa05c13777cd35c58d335b2fd73ec6114d5 100644
--- a/python/LQR_infHor.py
+++ b/python/LQR_infHor.py
@@ -1,8 +1,8 @@
'''
-Point-mass LQR with infinite horizon
+Point-mass discrete LQR with infinite horizon
-Copyright (c) 2023 Idiap Research Institute <https://www.idiap.ch/>
-Written by Jérémy Maceiras <jeremy.maceiras@idiap.ch> and
+Copyright (c) 2024 Idiap Research Institute <https://www.idiap.ch/>
+Written by Ante Marić <ante.maric@idiap.ch>, Jérémy Maceiras <jeremy.maceiras@idiap.ch> and
Sylvain Calinon <https://calinon.ch>
This file is part of RCFS <https://robotics-codes-from-scratch.github.io/>
@@ -11,83 +11,119 @@ License: GPL-3.0-only
import numpy as np
import copy
-from scipy.linalg import solve_discrete_are as solve_algebraic_riccati_discrete
-from scipy.stats import multivariate_normal
from math import factorial
import matplotlib.pyplot as plt
-
-# Plot a 2D Gaussians
-def plot_gaussian(mu, sigma):
- w, h = 100, 100
-
- std = [np.sqrt(sigma[0, 0]), np.sqrt(sigma[1, 1])]
- x = np.linspace(mu[0] - 3 * std[0], mu[0] + 3 * std[0], w)
- y = np.linspace(mu[1] - 3 * std[1], mu[1] + 3 * std[1], h)
-
- x, y = np.meshgrid(x, y)
-
- x_ = x.flatten()
- y_ = y.flatten()
- xy = np.vstack((x_, y_)).T
-
- normal_rv = multivariate_normal(mu, sigma)
- z = normal_rv.pdf(xy)
- z = z.reshape(w, h, order='F')
-
- plt.contourf(x, y, z.T,levels=1,colors=["white","red"],alpha=.5)
+import matplotlib.patches as patches
+
+def plot_gaussian(Mu, Sigma, color, valAlpha=1):
+ nbDrawingSeg = 100
+ darkcolor = color * 0.7
+ t = np.linspace(-np.pi, np.pi, nbDrawingSeg)
+
+ D, V = np.linalg.eig(Sigma)
+ R = np.real(V @ np.sqrt(np.diag(D)))
+ X = R @ np.array([np.cos(t), np.sin(t)]) + Mu.reshape(-1, 1)
+
+ if valAlpha < 1: # Plot with alpha transparency
+ patch = patches.Polygon(X.T, closed=True, facecolor=color, edgecolor=color, alpha=valAlpha, zorder=0)
+ plt.gca().add_patch(patch)
+ plt.plot(Mu[0], Mu[1], '.', markersize=10, color=darkcolor, zorder=1)
+ else: # Plot without transparency
+ patch = patches.Polygon(X.T, closed=True, facecolor=color, edgecolor=darkcolor, zorder=0)
+ plt.gca().add_patch(patch)
+ plt.plot(Mu[0], Mu[1], '.', markersize=10, color=darkcolor, zorder=1)
+
+ plt.axis('equal')
+
+def solve_algebraic_riccati_eig_discrete(A, G, Q):
+ n = A.shape[0]
+
+ # Symplectic matrix (see https://en.wikipedia.org/wiki/Algebraic_Riccati_equation)
+ Z = np.block([
+ [A + G @ np.linalg.inv(A).T @ Q, -G @ np.linalg.inv(A).T],
+ [-np.linalg.inv(A).T @ Q, np.linalg.inv(A).T]
+ ])
+
+ # Since Z is symplectic, if it does not have any eigenvalues on the unit circle,
+ # then exactly half of its eigenvalues are inside the unit circle.
+ eigvals, eigvecs = np.linalg.eig(Z)
+ U = []
+ for j in range(2 * n):
+ if np.abs(eigvals[j]) < 1: # inside unit circle
+ U.append(eigvecs[:, j])
+
+ U = np.array(U).T
+ X = np.real(U[n:, :] @ np.linalg.inv(U[:n, :]))
+ return X
# Parameters
# ===============================
-param = lambda: None # Lazy way to define an empty class in python
-param.nbData = 200 # Number of datapoints
-param.nbRepros = 4 #Number of reproductions
-param.nbVarPos = 2 #Dimension of position data (here: x1,x2)
-param.nbDeriv = 2 #Number of static & dynamic features (D=2 for [x,dx])
-param.nbVar = param.nbVarPos * param.nbDeriv #Dimension of state vector in the tangent space
-param.dt = 1e-2 #Time step duration
-param.rfactor = 4e-2 #Control cost in LQR
+param = lambda: None # Lazy way to define an empty class in python
+param.nbData = 200 # Number of datapoints
+param.nbRepros = 4 # Number of reproductions
+param.nbVarPos = 2 # Dimension of position data (here: x1,x2)
+param.nbDeriv = 2 # Number of static & dynamic features (D=2 for [x,dx])
+param.nbVar = param.nbVarPos * param.nbDeriv # Dimension of state vector in the tangent space
+param.dt = 1e-2 # Time step duration
+param.rfactor = 4e-2 # Control cost in LQR
# Control cost matrix
R = np.identity(param.nbVarPos) * param.rfactor
-# Target and desired covariance
-param.Mu = np.hstack(( np.random.uniform(size=param.nbVarPos) , np.zeros(param.nbVarPos) ))
-Ar,_ = np.linalg.qr(np.random.uniform(size=(param.nbVarPos,param.nbVarPos)))
+# Target and desired covariance (for precision matrix on static features)
+param.Mu = np.hstack((np.random.uniform(size=param.nbVarPos), np.zeros(param.nbVarPos)))
+Ar, _ = np.linalg.qr(np.random.uniform(size=(param.nbVarPos, param.nbVarPos)))
xCov = Ar @ np.diag(np.random.uniform(size=param.nbVarPos)) @ Ar.T * 1e-1
+## Target and desired covariance (for precision matrix on static and dynamic features)
+# param.Mu = np.hstack((np.random.randn(param.nbVarPos), np.zeros(param.nbVarPos * (param.nbDeriv - 1))))
+# xCov = np.diag(np.hstack((np.random.rand(2), np.random.rand(2) / param.dt))) * 1e-1
+
# Discrete dynamical System settings
# ===================================
-A1d = np.zeros((param.nbDeriv,param.nbDeriv))
-B1d = np.zeros((param.nbDeriv,1))
+A1d = np.zeros((param.nbDeriv, param.nbDeriv))
+B1d = np.zeros((param.nbDeriv, 1))
for i in range(param.nbDeriv):
- A1d += np.diag( np.ones(param.nbDeriv-i) ,i ) * param.dt**i * 1/factorial(i)
- B1d[param.nbDeriv-i-1] = param.dt**(i+1) * 1/factorial(i+1)
+ A1d += np.diag(np.ones(param.nbDeriv - i), i) * param.dt**i * 1 / factorial(i)
+ B1d[param.nbDeriv - i - 1] = param.dt**(i + 1) * 1 / factorial(i + 1)
-A = np.kron(A1d,np.identity(param.nbVarPos))
-B = np.kron(B1d,np.identity(param.nbVarPos))
+A = np.kron(A1d, np.identity(param.nbVarPos))
+B = np.kron(B1d, np.identity(param.nbVarPos))
# Discrete LQR with infinite horizon
# ===================================
-Q = np.zeros((param.nbVar,param.nbVar))
-Q[:param.nbVarPos,:param.nbVarPos] = np.linalg.inv(xCov) # Precision matrix
-P = solve_algebraic_riccati_discrete(A,B,Q,R)
-L = np.linalg.inv(B.T @ P @ B + R) @ B.T @ P @ A # Feedback gain (discrete version)
+# Precision matrix on static features
+Q = np.zeros((param.nbVar, param.nbVar))
+Q[:param.nbVarPos, :param.nbVarPos] = np.linalg.inv(xCov) # Precision matrix
+
+## Precision matrix on static and dynamic features
+# Q = np.linalg.inv(xCov)
+
+P = solve_algebraic_riccati_eig_discrete(A, B @ np.linalg.inv(R) @ B.T, (Q + Q.T) / 2)
+L = np.linalg.inv(B.T @ P @ B + R) @ B.T @ P @ A # Feedback gain (discrete version)
+
+# Test ratio between kp and kv
+if param.nbDeriv > 1:
+ kp = np.linalg.eigvals(L[:, :param.nbVarPos])
+ kv = np.linalg.eigvals(L[:, param.nbVarPos:])
+ ratio = kv / (2 * np.sqrt(kp)) # Corresponds to ideal damping ratio
+ print("Ratio:", ratio)
-reproducitons = []
+reproductions = []
for i in range(param.nbRepros):
xt = np.zeros(param.nbVar)
- xt[:param.nbVarPos] = 1+np.random.uniform(param.nbVarPos)*2
+ xt[:param.nbVarPos] = 1 + np.random.uniform(param.nbVarPos) * 2
xs = [copy.deepcopy(xt)]
for t in range(param.nbData):
u = L @ (param.Mu - xt)
xt = A @ xt + B @ u
xs += [copy.deepcopy(xt)]
- reproducitons += [ np.asarray(xs) ]
+ reproductions += [np.asarray(xs)]
# Plots
# ======
@@ -95,20 +131,20 @@ for i in range(param.nbRepros):
plt.figure()
plt.title("Position")
-for r in reproducitons:
- plt.plot(r[:,0],r[:,1],c="black")
-plot_gaussian(param.Mu[:param.nbVarPos],xCov)
+for r in reproductions:
+ plt.plot(r[:, 0], r[:, 1], c="black")
+plot_gaussian(param.Mu[:param.nbVarPos], xCov[:2, :2], color=np.array([.8, 0, 0]), valAlpha=0.3)
plt.axis("off")
plt.gca().set_aspect('equal', adjustable='box')
-fig,axs = plt.subplots(5)
+fig, axs = plt.subplots(5)
-for r in reproducitons:
- axs[0].plot(r[:,0],c="black",alpha=.4,linestyle="dashed")
- axs[1].plot(r[:,1],c="black",alpha=.4,linestyle="dashed")
- axs[2].plot(r[:,2],c="black",alpha=.4,linestyle="dashed")
- axs[3].plot(r[:,3],c="black",alpha=.4,linestyle="dashed")
- axs[4].plot(np.linalg.norm(r[:,2:4],axis=1),c="black",alpha=.4,linestyle="dashed")
+for r in reproductions:
+ axs[0].plot(r[:, 0], c="black", alpha=.4, linestyle="dashed")
+ axs[1].plot(r[:, 1], c="black", alpha=.4, linestyle="dashed")
+ axs[2].plot(r[:, 2], c="black", alpha=.4, linestyle="dashed")
+ axs[3].plot(r[:, 3], c="black", alpha=.4, linestyle="dashed")
+ axs[4].plot(np.linalg.norm(r[:, 2:4], axis=1), c="black", alpha=.4, linestyle="dashed")
axs[0].set_ylabel("$x_1$")
axs[1].set_ylabel("$x_2$")