From a455c9e5acbc6fce9edde3745b8e923d1bf81724 Mon Sep 17 00:00:00 2001
From: Sylvain CALINON <sylvain.calinon@idiap.ch>
Date: Mon, 14 Oct 2024 10:38:22 +0200
Subject: [PATCH] curvature example added
---
matlab/iLQR_curvature.m | 234 ++++++++++++++++++++++++++++++++++++++++
1 file changed, 234 insertions(+)
create mode 100644 matlab/iLQR_curvature.m
diff --git a/matlab/iLQR_curvature.m b/matlab/iLQR_curvature.m
new file mode 100644
index 0000000..a30ecd3
--- /dev/null
+++ b/matlab/iLQR_curvature.m
@@ -0,0 +1,234 @@
+%% Viapoint task for a point-mass system with a cost on curvature
+%%
+%% Copyright (c) 2024 Idiap Research Institute <https://www.idiap.ch/>
+%% Written by Sylvain Calinon <https://calinon.ch>
+%%
+%% This file is part of RCFS <https://robotics-codes-from-scratch.github.io/>
+%% License: GPL-3.0-only
+
+function iLQR_curvature
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+param.dt = 1E-2; %Time step size
+param.nbData = 100; %Number of datapoints
+param.nbPoints = 3; %Number of viapoints
+param.nbMinIter = 5; %Minimum number of iterations for iLQR
+param.nbMaxIter = 20; %Maximum number of iterations for iLQR
+param.nbVarPos = 2; %Dimension of position data
+param.nbDeriv = 3; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
+param.nbVarX = param.nbVarPos * param.nbDeriv; %State space dimension
+param.q = 1E1; %Tracking weight
+param.qc = 1E-6; %Curvature weight
+param.r = 1E-9; %Control weight
+
+idp = [0:param.nbPoints-1]*param.nbVarX + [1:param.nbVarPos]';
+idv = [0:param.nbPoints-1]*param.nbVarX + [param.nbVarPos+1:2*param.nbVarPos]';
+
+%Viapoints
+param.Mu = zeros(param.nbVarX*param.nbPoints,1);
+%param.Mu(idp(:)) = rand(param.nbVarPos*param.nbPoints,1);
+param.Mu(idp(:)) = [[1; 1]; [0.5; 0.2]; [0.8; 2]];
+
+%Control weight matrix (at trajectory level)
+R = speye((param.nbData-1) * param.nbVarPos) * param.r;
+
+%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]';
+%idPos = (tl - 1) * param.nbVarX + [1:param.nbVarPos]';
+%idVel = (tl - 1) * param.nbVarX + [param.nbVarPos+1:2*param.nbVarPos]';
+
+%Dynamical System settings (discrete version)
+A1d = zeros(param.nbDeriv);
+for i=0:param.nbDeriv-1
+ A1d = A1d + diag(ones(param.nbDeriv-i,1),i) * param.dt^i * 1/factorial(i); %Discrete 1D
+end
+B1d = zeros(param.nbDeriv,1);
+for i=1:param.nbDeriv
+ B1d(param.nbDeriv-i+1) = param.dt^i * 1/factorial(i); %Discrete 1D
+end
+A = repmat(kron(A1d, eye(param.nbVarPos)), [1 1 param.nbData-1]); %Discrete nD
+B = repmat(kron(B1d, eye(param.nbVarPos)), [1 1 param.nbData-1]); %Discrete nD
+
+[Su0, Sx0] = transferMatrices(A, B); %Constant Su and Sx for the proposed system
+Su = Su0(idx,:);
+
+
+%% Constraining the position of two consecutive keypoints to be same and crossing at given angle -> forming a loop
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%param.Q = eye(param.nbPoints*param.nbVarX);
+
+%Only cares about position part of the state variable
+param.Q = diag(kron(ones(param.nbPoints,1), [ones(param.nbVarPos,1); zeros(param.nbVarX-param.nbVarPos,1)]));
+
+param.Mu(idp(:,2)) = param.Mu(idp(:,1)); %Viapoints 1 and 2 form the loop
+
+a = pi/2; %desired crossing angle
+V = [cos(a) -sin(a); sin(a) cos(a)]; %rotation matrix
+
+%Impose cost on crossing angle
+param.Q(idv(:,1), idv(:,1)) = eye(param.nbVarPos)*1E0;
+param.Q(idv(:,2), idv(:,2)) = eye(param.nbVarPos)*1E0;
+param.Q(idv(:,1), idv(:,2)) = V; %-eye(nbVarPos)*1E0;
+param.Q(idv(:,2), idv(:,1)) = V'; %-eye(nbVarPos)*1E0;
+
+
+%% iLQR without curvature cost (for initialization)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+x0 = zeros(param.nbVarX,1); %Initial state
+u = zeros(param.nbVarPos*(param.nbData-1), 1); %Initial commands
+
+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 (viapoints tracking objective)
+ du = (Su' * J' * param.Q * J * Su + R) \ (-Su' * J' * param.Q * f - u * param.r); %Update
+
+ %Estimate step size with backtracking line search method
+ alpha = 1;
+ cost0 = costFct(f, 0, u, param); %Cost
+ while 1
+ utmp = u + du * alpha;
+ xtmp = reshape(Su0 * utmp + Sx0 * x0, param.nbVarX, param.nbData);
+ ftmp = f_reach(xtmp(:,tl), param); %Residuals (viapoints tracking objective)
+ cost = costFct(ftmp, 0, utmp, param); %Cost
+ if cost < cost0 || alpha < 1E-3
+ break;
+ end
+ alpha = alpha * 0.5;
+ end
+ u = u + du * alpha;
+
+ r(n).x = x; %Log data
+
+ if norm(du * alpha) < 5E-2 && n>param.nbMinIter
+ break; %Stop iLQR when solution is reached
+ end
+end
+disp(['iLQR converged in ' num2str(n) ' iterations.']);
+
+x1 = x; %Log data
+fc = f_curvature(x, param); %Residuals and Jacobians (motion objective)
+norm(fc(:))^2
+
+
+%% iLQR with curvature cost
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%x0 = zeros(param.nbVarX,1); %Initial state
+%u = zeros(param.nbVarPos*(param.nbData-1), 1); %Initial commands
+
+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 (viapoints tracking objective)
+ [fc, Jc] = f_curvature(x, param); %Residuals and Jacobians (curvature objective)
+ du = (Su' * J' * param.Q * J * Su + Su0' * Jc' * Jc * Su0 * param.qc + R) \ ...
+ (-Su' * J' * param.Q * f - Su0' * Jc' * fc(:) * param.qc - u * param.r); %Update
+
+ %Estimate step size with backtracking line search method
+ alpha = 1;
+ cost0 = costFct(f, fc, u, param); %Cost
+ while 1
+ utmp = u + du * alpha;
+ xtmp = reshape(Su0 * utmp + Sx0 * x0, param.nbVarX, param.nbData);
+ ftmp = f_reach(xtmp(:,tl), param); %Residuals (viapoints tracking objective)
+ fctmp = f_curvature(xtmp, param); %Residuals (curvature objective)
+ cost = costFct(ftmp, fctmp, utmp, param); %Cost
+ if cost < cost0 || alpha < 1E-3
+ break;
+ end
+ alpha = alpha * 0.5;
+ end
+ u = u + du * alpha;
+
+ if norm(du * alpha) < 5E-2 && n>param.nbMinIter
+ break; %Stop iLQR when solution is reached
+ end
+end
+disp(['iLQR converged in ' num2str(n) ' iterations.']);
+norm(fc(:))^2
+
+
+%% Plot state space
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+h = figure('position',[10,10,600,600],'color',[1,1,1]); hold on; axis off;
+hl(1) = plot(x1(1,:), x1(2,:), '-','linewidth',4,'color',[.7 .7 .7]);
+hl(2) = plot(x(1,:), x(2,:), '-','linewidth',4,'color',[0 0 0]);
+%hl(3) =
+plot(x(1,1), x(2,1), '.','markersize',30,'color',[0 0 0]);
+%Display curvature information
+%for t=1:param.nbData
+% v = [x(4,t); -x(3,t)]; %Normal to curve
+% r = 1 / (fc(t)+1E-12);
+%
+% if abs(r)>1E-2 && abs(r)<0.5
+% v = -r * v / norm(v);
+% plot([x(1,t), x(1,t)+v(1)], [x(2,t), x(2,t)+v(2)], '-','color',[0 .6 0]);
+% end
+%end
+plot(param.Mu(idp(1,:)), param.Mu(idp(2,:)), '.','markersize',30,'color',[.8 0 0]);
+axis equal;
+legend(hl,{'Without cost on curvature','With cost on curvature'}, 'fontsize',20,'location','southeast'); %,'Viapoints'
+%print('-dpng','graphs/iLQR_curvature01.png');
+
+waitfor(h);
+close all;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+function [Su, Sx] = transferMatrices(A, B)
+ [nbVarX, nbVarU, nbData] = size(B);
+ nbData = nbData+1;
+ Sx = kron(ones(nbData,1), speye(nbVarX));
+ Su = sparse(zeros(nbVarX*(nbData-1), nbVarU*(nbData-1)));
+ for t=1:nbData-1
+ id1 = (t-1)*nbVarX+1:t*nbVarX;
+ id2 = t*nbVarX+1:(t+1)*nbVarX;
+ id3 = (t-1)*nbVarU+1:t*nbVarU;
+ Sx(id2,:) = squeeze(A(:,:,t)) * Sx(id1,:);
+ Su(id2,:) = squeeze(A(:,:,t)) * Su(id1,:);
+ Su(id2,id3) = B(:,:,t);
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Cost function
+function c = costFct(f, fc, u, param)
+% c = norm(f(:))^2 * param.q + norm(fc(:))^2 * param.qc + norm(u)^2 * param.r;
+ c = f(:)' * param.Q * f(:) + norm(fc(:))^2 * param.qc + norm(u)^2 * param.r;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Residuals f and Jacobians J for a viapoints reaching task with point mass system
+function [f, J] = f_reach(x, param)
+ f = x(:) - param.Mu; %Residuals
+ J = eye(param.nbVarX * size(x,2)); %Jacobian
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Residuals f and Jacobians J for curvature minimization over path
+function [f, J] = f_curvature(x, param)
+% dx = x(:,2:end) - x(:,1:end-1);
+% l = sum(dx.^2,1).^.5; %Segment lengths
+% dx0 = dx ./ l; %Unit direction vectors
+% theta = acos(dx0(:,2:end)' * dx0(:,1:end-1)); %Angles
+
+ ddx = x(2*param.nbVarPos+1:3*param.nbVarPos,:); %Second derivative
+ dx = x(param.nbVarPos+1:2*param.nbVarPos,:); %First derivative
+ dxn = sum(dx.^2,1).^(3/2);
+ f = (dx(1,:) .* ddx(2,:) - dx(2,:) .* ddx(1,:)) ./ (dxn + 1E-8); %Curvature
+
+ s11 = zeros(param.nbVarX,1); s11(3) = 1; %Selection vector
+ s12 = zeros(param.nbVarX,1); s12(4) = 1; %Selection vector
+ s21 = zeros(param.nbVarX,1); s21(5) = 1; %Selection vector
+ s22 = zeros(param.nbVarX,1); s22(6) = 1; %Selection vector
+ Sa = s11 * s22' - s12 * s21'; %Selection matrix for numerator
+ Sb = s11 * s11' + s12 * s12'; %Selection matrix for denominator
+ J = [];
+ for t=1:param.nbData
+ a = x(:,t)' * Sa * x(:,t);
+ b = x(:,t)' * Sb * x(:,t) + 1E-8;
+ Jtmp = 2 * b^(-3/2) * Sa * x(:,t) - 3 * a * b^(-5/2) * Sb * x(:,t);
+ J = blkdiag(J, Jtmp');
+ end
+end
--
GitLab