curvature example added

matlab/iLQR_curvature.m

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