diff --git a/matlab/iLQR_manipulator_boundary.m b/matlab/iLQR_manipulator_boundary.m
new file mode 100644
index 0000000000000000000000000000000000000000..1b449e66933bf0cb281c6e2ec352a01d93945ea0
--- /dev/null
+++ b/matlab/iLQR_manipulator_boundary.m
@@ -0,0 +1,214 @@
+%% iLQR applied to a planar manipulator for a viapoints task with bounding box on x (or u)
+%%
+%% Copyright (c) 2023 Idiap Research Institute <https://www.idiap.ch/>
+%% Written by Teguh Lembono <teguh.lembono@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
+
+function iLQR_manipulator_boundary
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+param.dt = 1E-2; %Time step size
+param.nbData = 100; %Number of datapoints
+param.nbIter = 100; %Maximum number of iterations for iLQR
+param.nbPoints = 2; %Number of viapoints
+param.nbDOFs = 3; % Number of articulated links
+param.nbVarX = 3; %State space dimension (x1,x2,x3)
+param.nbVarU = 3; %Control space dimension (dx1,dx2,dx3)
+param.nbVarF = 2; %Task space dimension (f1,f2)
+param.l = [3, 2, 1]; %Robot links lengths
+param.sz = [.2, .3]; %Size of objects
+param.ulim = [15, 5, 15]; %Control commands range
+param.xlim = [pi*2, pi*2, pi*.05]; %joint angles range
+param.q = 1E0; %Tracking weighting term
+param.rv = 1E3; %Bounding weighting term
+param.r = 1E-4; %Control weighting term
+param.Mu = [[2; 1], [3; 2]]; %Viapoints
+
+Q = speye(param.nbVarF * param.nbPoints) * param.q; %Precision matrix
+R = speye(param.nbVarU * (param.nbData-1)) * param.r; %Control weight matrix (at trajectory level)
+
+%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 = ones(param.nbVarU*(param.nbData-1), 1) * 0; %Initial control 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)); kron(tril(ones(param.nbData-1)), eye(param.nbVarX)*param.dt)];
+Sx0 = kron(ones(param.nbData,1), eye(param.nbVarX));
+Su = Su0(idx,:);
+
+X0 = reshape(Su0 * u + Sx0 * x0, param.nbVarX, param.nbData);
+for n=1:param.nbIter
+ x = reshape(Su0 * u + Sx0 * x0, param.nbVarX, param.nbData); %System evolution
+ [f, J] = f_reach(x(:,tl), param);
+% [v, Jv] = u_bound(u, model);
+ [v, Jv, idv] = x_bound(x(:), param);
+ Sv = Su0(idv,:);
+
+ du = (Su' * J' * Q * J * Su + Sv' * Jv' * Jv * Sv * param.rv + R) \ (-Su' * J' * Q * f(:) - Sv' * Jv' * v * param.rv - u * param.r); %Gradient
+% du = (Su' * J' * Q * J * Su + Sv' * Sv * param.rv + R) \ (-Su' * J' * Q * f(:) - Sv' * v * param.rv - u * param.r); %Gradient
+
+% du = (Su' * J' * Q * J * Su + Jv' * Jv * param.rv + R) \ (-Su' * J' * Q * f(:) - Jv' * v * param.rv - u * param.r); %Gradient
+% du = (Jv' * Su' * J' * Q * J * Su * Jv + Jv' * Jv * param.rv + R) \ (-Jv' * Su' * J' * Q * f(:) - Jv' * v * param.rv - u * param.r); %Gradient
+
+ %Estimate step size with backtracking line search method
+ alpha = 1;
+ cost0 = f(:)' * Q * f(:) + norm(v)^2 * param.rv + norm(u)^2 * param.r;
+ while 1
+ utmp = u + du * alpha;
+ xtmp = reshape(Su0 * utmp + Sx0 * x0, param.nbVarX, param.nbData);
+ ftmp = f_reach(xtmp(:,tl), param);
+ %vtmp = u_bound(utmp, param);
+ vtmp = x_bound(xtmp(:), param);
+ cost = ftmp(:)' * Q * ftmp(:) + norm(vtmp)^2 * param.rv + norm(utmp)^2 * param.r;
+ if cost < cost0 || alpha < 1E-3
+ break;
+ end
+ alpha = alpha * 0.5;
+ end
+ u = u + du * alpha;
+
+ 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
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,800,800],'color',[1,1,1]); hold on; axis off;
+
+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]);
+
+colMat = lines(param.nbPoints);
+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,:), '-','linewidth',2,'color',[0 0 0]);
+plot(ftmp(1,1), ftmp(2,1), '.','markersize',40,'color',[0 0 0]);
+plot(ftmp(1,tl), ftmp(2,tl), '.','markersize',30,'color',[0 0 0]);
+axis equal;
+
+%% Timeline plot
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+h = figure('position',[950 10 800 800],'color',[1 1 1]);
+%Plot x
+for j=1:param.nbVarX
+ subplot(param.nbVarX, 1, j); grid on; hold on; box on;
+ plot([1, param.nbData], [param.xlim(j), param.xlim(j)], 'r-');
+ plot([1, param.nbData], -[param.xlim(j), param.xlim(j)], 'r-');
+ plot(X0(j,:), '-','linewidth',3,'color',[.7 .7 .7]);
+ plot(x(j,:), '-','linewidth',3,'color',[0 0 0]);
+ ylabel(['$x_' num2str(j) '$'], 'interpreter','latex','fontsize',26);
+end
+xlabel('$t$', 'interpreter','latex','fontsize',26);
+
+waitfor(h);
+end
+
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Forward kinematics (in robot coordinate system)
+function f = fkin(x, param)
+ T = tril(ones(size(x,1)));
+ f = [param.l * cos(T * x); ...
+ param.l * sin(T * x)];
+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)
+ T = tril(ones(size(x,1)));
+ J = [-sin(T * x)' * diag(param.l) * T; ...
+ cos(T * x)' * diag(param.l) * T];
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%
+%%Jacobian with numerical computation (for single time step)
+%function J = jacob0_num(x, param)
+% e = 1E-6;
+% J = zeros(param.nbVarF, param.nbVarX);
+% for n=1:size(x,1)
+% xtmp = x;
+% xtmp(n) = xtmp(n) + e;
+% J(:,n) = (fkine0(xtmp, param) - fkine0(x, param)) / e;
+% end
+%end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Cost and gradient for a viapoints reaching task (in object coordinate system)
+function [f, J] = f_reach(x, param)
+ f = fkin(x, param) - param.Mu; %Error by ignoring manifold
+
+ J = [];
+ for t=1:size(x,2)
+% f(1:2,t) = param.A(:,:,t)' * f(1:2,t); %Object-centered FK
+
+ Jtmp = Jkin(x(:,t), param);
+ %Jtmp = jacob0_num(x(:,t), param);
+
+% Jtmp(1:2,:) = param.A(:,:,t)' * Jtmp(1:2,:); %Object-centered Jacobian
+
+% %Bounding boxes (optional)
+% for i=1:2
+% if abs(f(i,t)) < param.sz(i)
+% f(i,t) = 0;
+% Jtmp(i,:) = 0;
+% else
+% f(i,t) = f(i,t) - sign(f(i,t)) * param.sz(i);
+% end
+% end
+
+ J = blkdiag(J, Jtmp);
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%
+%%Cost and gradient for boundary on u
+%function [v, Jv, idv] = u_bound(u, param)
+% v = zeros(param.nbVarU*(param.nbData-1), 1);
+% ulim = repmat(param.ulim', param.nbData-1, 1);
+% idv = abs(u) > ulim; %Bounding boxes
+% Jv = diag(idv);
+% v(idv) = u(idv) - sign(u(idv)) .* ulim(idv);
+%end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Cost and gradient for boundary on x
+function [v, Jv, idv] = x_bound(x, param)
+ %v = zeros(param.nbVarU*param.nbData, 1);
+ xlim = repmat(param.xlim', param.nbData, 1);
+ idv = abs(x) > xlim; %Bounding boxes
+ Jv = eye(sum(idv));
+ v = x(idv) - sign(x(idv)) .* xlim(idv);
+end
diff --git a/python/iLQR_manipulator_boundary.py b/python/iLQR_manipulator_boundary.py
index d8f58e545efe0bbb117b4167c6055515174fae0a..d35cdb8d81662fb12b55c2ba99efe58f4e10b8e1 100644
--- a/python/iLQR_manipulator_boundary.py
+++ b/python/iLQR_manipulator_boundary.py
@@ -49,7 +49,6 @@ def f_reach(x, param):
J = Jtmp
return f, J
-
def x_bound(x, param):
"""Cost and gradient for a viapoints reaching task (in object coordinate system)"""
xlim = np.tile(param.xlim.T, (param.nbData))
@@ -71,84 +70,6 @@ def fkin0(x, params):
f = np.hstack((np.zeros((2, 1)), f))
return f
-
-def plot_fkin0_for_via_points(x0, x, tl, param):
- """Plot robot forward kinematics for initial configuration, via-points, and path."""
- _, ax = plt.subplots(figsize=(12, 8))
-
- fkin00 = fkin0(x0, param)
- ax.plot(
- fkin00[0, :],
- fkin00[1, :],
- color=(0.9, 0.9, 0.9),
- linewidth=5,
- label="Initial Configuration",
- )
- ax.scatter(fkin00[0, 1:], fkin00[1, 1:], color="skyblue", marker="o", s=100, zorder=2)
- ax.scatter(fkin00[0, 0], fkin00[1, 0], color="black", marker="s", s=100, zorder=2)
-
- for i, idx in enumerate(tl):
- fkin0_i = fkin0(x[:, idx], param)
- color_factor = len(param.Mu) / (len(param.Mu) - i)
- ax.plot(
- fkin0_i[0, :],
- fkin0_i[1, :],
- linewidth=5,
- color=(0.8 / color_factor, 0.8 / color_factor, 0.8 / color_factor),
- label=f"Via-point {i + 1} Configuration",
- )
- ax.scatter(fkin0_i[0, 1:], fkin0_i[1, 1:], color="skyblue", marker="o", s=100, zorder=2)
- ax.scatter(fkin0_i[0, 0], fkin0_i[1, 0], color="black", marker="s", s=100, zorder=2)
-
- ftmp0 = fkin(x, param)
- ax.plot(
- ftmp0[0, :],
- ftmp0[1, :],
- "--",
- linewidth=1,
- color="black",
- label="End-effector trajectory",
- )
- ax.plot(
- param.Mu[0, 0],
- param.Mu[1, 0],
- ".",
- markersize=10,
- color="darkred",
- label="Via-point 1 Marker",
- )
- ax.plot(
- param.Mu[0, 1],
- param.Mu[1, 1],
- ".",
- markersize=10,
- color="purple",
- label="Via-point 2 Marker",
- )
-
- ax.axis("off")
- ax.set_aspect("equal", adjustable="box")
- ax.legend(loc="upper left", bbox_to_anchor=(1.05, 1), borderaxespad=0.0)
- plt.show()
-
-
-def plot_x(x0, x, param):
- """Plot the change of x ( i.e. [x1, x2, x3]) over time"""
- _, axs = plt.subplots(3, 1, figsize=(10, 8))
-
- for i in range(3):
- axs[i].plot(x[i, :], color="black", label=f"x{i}")
- axs[i].axhline(y=x0[i], color="blue", linestyle="--", label=f"x{i}_0")
- axs[i].axhline(y=param.xlim[i], color="red", linestyle="--", label=f"x{i}_lim")
- axs[i].set_title(f"x{i} vs t")
- axs[i].set_xlabel("t")
- axs[i].set_ylabel(f"x{i}")
- axs[i].legend()
-
- plt.tight_layout()
- plt.show()
-
-
## Parameters
# ===============================
@@ -164,7 +85,7 @@ param.l = np.array([3, 2, 1]) # Robot links lengths
param.xlim = np.array([np.pi * 2, np.pi * 2, np.pi * 0.05]) # joint angles range
param.q = 1e0 # Tracking weighting term
param.rv = 1e3 # Bounding weighting term
-param.r = 1e-6 # Control weighting term
+param.r = 1e-4 # Control weighting term
param.Mu = np.array([[2, 3], [1, 2]]) # Viapoints
# Main program
@@ -240,5 +161,56 @@ print(f"iLQR converged in {i} iterations")
# Visualize
x = x.reshape([param.nbVarX, param.nbData], order="F")
-plot_fkin0_for_via_points(x0, x, tl, param)
-plot_x(x0, x, param)
+"""Plot robot forward kinematics for initial configuration, via-points, and path."""
+_, ax = plt.subplots(figsize=(12, 8))
+
+fkin00 = fkin0(x0, param)
+ax.plot(fkin00[0, :], fkin00[1, :], color=(0.9, 0.9, 0.9),
+ linewidth=5,label="Initial Configuration",)
+ax.scatter(fkin00[0, 1:], fkin00[1, 1:], color="skyblue",
+ marker="o", s=100, zorder=2)
+ax.scatter(fkin00[0, 0], fkin00[1, 0], color="black",
+ marker="s", s=100, zorder=2)
+
+for i, idx in enumerate(tl):
+ fkin0_i = fkin0(x[:, idx], param)
+ color_factor = len(param.Mu) / (len(param.Mu) - i)
+ ax.plot(fkin0_i[0, :], fkin0_i[1, :], linewidth=5,
+ color=(0.8 / color_factor, 0.8 / color_factor, 0.8 / color_factor),
+ label=f"Via-point {i + 1} Configuration",
+ )
+ ax.scatter(fkin0_i[0, 1:], fkin0_i[1, 1:], color="skyblue",
+ marker="o", s=100, zorder=2)
+ ax.scatter(fkin0_i[0, 0], fkin0_i[1, 0], color="black",
+ marker="s", s=100, zorder=2)
+
+ftmp0 = fkin(x, param)
+ax.plot(ftmp0[0, :], ftmp0[1, :], "--", linewidth=1,
+ color="black", label="End-effector trajectory",
+)
+ax.plot(param.Mu[0, 0], param.Mu[1, 0], ".", markersize=10, color="darkred",
+ label="Via-point 1 Marker",
+)
+ax.plot(param.Mu[0, 1], param.Mu[1, 1], ".", markersize=10,
+ color="purple", label="Via-point 2 Marker",
+)
+
+ax.axis("off")
+ax.set_aspect("equal", adjustable="box")
+ax.legend(loc="upper left", bbox_to_anchor=(1.05, 1), borderaxespad=0.0)
+plt.show()
+
+"""Plot the change of x ( i.e. [x1, x2, x3]) over time"""
+_, axs = plt.subplots(3, 1, figsize=(10, 8))
+
+for i in range(3):
+ axs[i].plot(x[i, :], color="black", label=f"x{i}")
+ axs[i].axhline(y=x0[i], color="blue", linestyle="--", label=f"x{i}_0")
+ axs[i].axhline(y=param.xlim[i], color="red", linestyle="--", label=f"x{i}_lim")
+ axs[i].set_title(f"x{i} vs t")
+ axs[i].set_xlabel("t")
+ axs[i].set_ylabel(f"x{i}")
+ axs[i].legend()
+
+plt.tight_layout()
+plt.show()