Example added

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

python/iLQR_manipulator_boundary.py

+"""
+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)