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}

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)

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]); 

matlab/iLQR_manipulator_basic.m

+%% 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

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

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()