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.tex b/doc/rcfs.tex
index abda8708533461647d05bc376ed73787f7e3e2dc..44ada561e8759e5a6175fc5d060814052d327a1d 100644
--- a/doc/rcfs.tex
+++ b/doc/rcfs.tex
@@ -3840,7 +3840,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 +4135,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/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_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()