diff --git a/matlab/iLQR_distMaintenance.m b/matlab/iLQR_distMaintenance.m
index f8cdb1df044a18998cfbc98595d29e120d94bf33..1db55be76006b282ea7554eb057e59276d666a89 100644
--- a/matlab/iLQR_distMaintenance.m
+++ b/matlab/iLQR_distMaintenance.m
@@ -20,11 +20,11 @@ param.Mu = [1.0; 0.3]; %Object location
%param.dist = .4; %Distance to maintain
%param.Sigma = eye(param.nbVarX) * param.dist^2; %Covariance matrix
-vtmp = [1; 1]; %Main axis of covariance matrix
-param.Sigma = vtmp * vtmp' + eye(2) * 1E-1; %Covariance matrix
+vtmp = [.8; .8]; %Main axis of covariance matrix
+param.Sigma = vtmp * vtmp' + eye(2) * 2E-2; %Covariance matrix
param.q = 1E0; %Distance maintenance weight term
-param.r = 1E-3; %Control weight term
+param.r = 1E-6; %Control weight term
R = speye((param.nbData-1) * param.nbVarU) * param.r; %Control weight matrix (at trajectory level)
@@ -73,8 +73,8 @@ al = linspace(-pi, pi, 50);
%msh = param.dist * [cos(al); sin(al)] + repmat(param.Mu(1:2), 1, 50);
[V,D] = eig(param.Sigma);
msh = V * D.^.5 * [cos(al); sin(al)] + repmat(param.Mu(1:2), 1, 50);
-
patch(msh(1,:), msh(2,:), [1 .8 .8],'linewidth',2,'edgecolor',[.8 .4 .4]);
+
plot(param.Mu(1), param.Mu(2), '.','markersize',25,'color',[.8 0 0]);
plot(x(1,:), x(2,:), '-','linewidth',2,'color',[0 0 0]);
plot(x(1,1), x(2,1), '.','markersize',25,'color',[0 0 0]);
diff --git a/python/iLQR_manipulator_boundary.py b/python/iLQR_manipulator_boundary.py
new file mode 100644
index 0000000000000000000000000000000000000000..d8f58e545efe0bbb117b4167c6055515174fae0a
--- /dev/null
+++ b/python/iLQR_manipulator_boundary.py
@@ -0,0 +1,244 @@
+"""
+iLQR applied to a planar manipulator for a viapoints task with bounding on x
+
+Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/
+Written by Ekansh Sharma <ekanshh.sharma@gmail.com> and
+Sylvain Calinon <https://calinon.ch>
+
+This file is part of RCFS <https://robotics-codes-from-scratch.github.io/>
+License: GPL-3.0-only
+"""
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+
+def fkin(x, param):
+ """Forward kinematics for end-effector (in robot coordinate system)"""
+ T = np.tril(np.ones(len(x)))
+ f = np.vstack([param.l @ np.cos(T @ x), param.l @ np.sin(T @ x)])
+ return f
+
+
+def jacob0(x, param):
+ """Jacobian with analytical computation (for single time step)"""
+ T = np.tril(np.ones(len(x)))
+ J = np.array(
+ [
+ -np.sin(T @ x).T @ np.diag((param.l)) @ T,
+ np.cos(T @ x).T @ np.diag(param.l) @ T,
+ ]
+ )
+ return J
+
+
+def f_reach(x, param):
+ """Cost and gradient for a viapoints reaching task (in object coordinate system)"""
+ f = fkin(x, param) - param.Mu
+ J = np.array([])
+ for t in range(x.shape[1]):
+ Jtmp = jacob0(x[:, t], param)
+ if np.any(J):
+ J = np.block(
+ [
+ [J, np.zeros((J.shape[0], Jtmp.shape[1]))],
+ [np.zeros((Jtmp.shape[0], J.shape[1])), Jtmp],
+ ]
+ )
+ else:
+ 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))
+ idv = np.abs(x) > xlim
+ Jv = np.eye(np.sum(idv))
+ v = x[idv] - np.sign(x[idv]) * xlim[idv].flatten()
+ return v, Jv, idv
+
+
+def fkin0(x, params):
+ """Compute forward kinematics for all joints (in robot coordinate system)"""
+ L = np.tril(np.ones(len(x)))
+ f = np.vstack(
+ [
+ L @ np.diag(params.l) @ np.cos(L @ x),
+ L @ np.diag(params.l) @ np.sin(L @ x),
+ ]
+ )
+ 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
+# ===============================
+
+param = lambda: None
+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.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 = 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.Mu = np.array([[2, 3], [1, 2]]) # Viapoints
+
+# Main program
+# ===============================
+
+# Precision matrix
+Q = np.eye(param.nbVarF * param.nbPoints)
+
+# Control weight matrix
+R = np.eye((param.nbData - 1) * param.nbVarU) * param.r
+
+# Time occurrence of viapoints
+tl = np.rint(np.linspace(0, param.nbData - 1, param.nbPoints + 1)[1:]).astype(np.int64)
+idx = np.array([i + np.arange(0, param.nbVarX, 1) for i in (tl * param.nbVarX)])
+idx = idx.flatten()
+
+# Transfer matrices (for linear system as single integrator)
+Su0 = np.vstack(
+ [
+ np.zeros([param.nbVarX, param.nbVarX * (param.nbData - 1)]),
+ np.kron(np.tril(np.ones(param.nbData - 1)), np.eye(param.nbVarX) * param.dt),
+ ]
+)
+Sx0 = np.kron(np.ones(param.nbData), np.eye(param.nbVarX)).T
+Su = Su0[idx, :] # We remove the lines that are out of interest
+
+# iLQR
+# ===============================
+
+u = np.zeros(param.nbVarU * (param.nbData - 1)) # Initial control command
+x0 = np.array([3 * np.pi / 4, -np.pi / 2, np.pi / 4]) # Initial state
+
+for i in range(param.nbIter):
+ x = Sx0 @ x0 + Su0 @ u # System evolution
+ x = x.reshape([param.nbVarX, param.nbData], order="F")
+
+ f, J = f_reach(x[:, tl], param) # Residuals and Jacobians for reaching task
+ x = x.flatten(order="F")
+ v, Jv, idv = x_bound(x, param) # Residuals and Jacobians for boundary on x
+
+ Sv = Su0[idv, :]
+
+ du = np.linalg.inv(Su.T @ J.T @ Q @ J @ Su + Sv.T @ Jv.T @ Jv @ Sv * param.rv + R) @ (
+ -Su.T @ J.T @ Q @ f.flatten("F") - Sv.T @ Jv.T @ v * param.rv - u * param.r
+ ) # Gauss-Newton update
+
+ # Estimate step size with backtracking line search method
+ alpha = 1
+ cost0 = f.flatten("F").T @ Q @ f.flatten("F") + np.linalg.norm(v) ** 2 * param.rv + np.linalg.norm(u) ** 2 * param.r
+ while True:
+ utmp = u + du * alpha
+ xtmp = Su0 @ utmp + Sx0 @ x0 # System evolution
+ xtmp = xtmp.reshape([param.nbVarX, param.nbData], order="F")
+ ftmp, _ = f_reach(xtmp[:, tl], param) # Residuals
+ xtmp = xtmp.flatten(order="F")
+ vtmp, _, _ = x_bound(xtmp, param)
+ cost = (
+ ftmp.flatten("F").T @ Q @ ftmp.flatten("F")
+ + np.linalg.norm(vtmp) ** 2 * param.rv
+ + np.linalg.norm(utmp) ** 2 * param.r
+ )
+ if cost < cost0 or alpha < 1e-4:
+ print("Iteration {}, cost: {}, alpha: {}".format(i, cost, alpha))
+ break
+ alpha /= 2
+
+ u = u + du * alpha
+
+ if np.linalg.norm(du * alpha) < 1e-2:
+ break # Stop iLQR iterations when solution is reached
+
+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)