diff --git a/python/iLQR_bimanual_manipulability.py b/python/iLQR_bimanual_manipulability.py deleted file mode 100644 index e446e3de085e6a382907cd22644e66eba8fb6d86..0000000000000000000000000000000000000000 --- a/python/iLQR_bimanual_manipulability.py +++ /dev/null @@ -1,231 +0,0 @@ -''' - iLQR applied to a planar bimanual robot problem with a cost on tracking a desired - - Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/ - Written by Boyang Ti <https://www.idiap.ch/~bti/> and - Sylvain Calinon <https://calinon.ch> - - This file is part of RCFS. - - RCFS is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License version 3 as - published by the Free Software Foundation. - - RCFS is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with RCFS. If not, see <http://www.gnu.org/licenses/>. -''' - -import numpy as np -import matplotlib.pyplot as plt -import matplotlib.patches as patches -import scipy.linalg -from scipy.linalg import fractional_matrix_power - -import numpy.matlib -# Helper functions -# =============================== - -# Forward kinematics for end-effector (in robot coordinate system) -def fkin(x, param): - L = np.tril(np.ones(3)) - f = np.vstack(( - param.l[0:3].T @ np.cos(L @ x[0:3]), - param.l[0:3].T @ np.sin(L @ x[0:3]), - param.l[[0, 3, 4]].T @ np.cos(L @ x[[0, 3, 4]]), - param.l[[0, 3, 4]].T @ np.sin(L @ x[[0, 3, 4]]) - )) # f1,f2 - return f - -# Forward kinematics for end-effector (in robot coordinate system) -def fkin0(x, param): - L = np.tril(np.ones(3)) - fl = np.vstack(( - L @ np.diag(param.l[0:3].flatten()) @ np.cos(L @ x[0:3]), - L @ np.diag(param.l[0:3].flatten()) @ np.sin(L @ x[0:3]) - )) - fr = np.vstack(( - L @ np.diag(param.l[[0, 3, 4]].flatten()) @ np.cos(L @ x[[0, 3, 4]]), - L @ np.diag(param.l[[0, 3, 4]].flatten()) @ np.sin(L @ x[[0, 3, 4]]) - )) - f = np.hstack((fl[:, ::-1], np.zeros([2, 1]), fr)) - return f - -# Forward kinematics for center of mass of a bimanual robot (in robot coordinate system) -def fkin_CoM(x, param): - L = np.tril(np.ones(3)) - f = np.vstack((param.l[0:3].T @ L @ np.cos(L @ x[0:3]) + param.l[[0, 3, 4]].T @ L @ np.cos(L @ x[[0, 3, 4]]), - param.l[0:3].T @ L @ np.sin(L @ x[0:3]) + param.l[[0, 3, 4]].T @ L @ np.sin(L @ x[[0, 3, 4]]))) / 6 - - return f - -# Jacobian with analytical computation (for single time step) -def Jkin(x, param): - L = np.tril(np.ones([param.nbVarX, param.nbVarX])) - J = np.vstack([ - -np.sin(L @ x).T @ np.diag(param.l) @ L, - np.cos(L @ x).T @ np.diag(param.l) @ L, - np.ones([1, param.nbVarX]) - ]) - return J - -# Jacobian for center of mass of a robot (in robot coordinate system) -def Jkin_CoM(x, param): - L = np.tril(np.ones(3)) - Jl = np.vstack((-np.sin(L @ x[0:3].reshape(-1, 1)).T @ L @ np.diag((param.l[0:3].T @ L).flatten()), - np.cos(L @ x[0:3].reshape(-1, 1)).T @ L @ np.diag((param.l[0:3].T @ L).flatten()))) / 6 - Jr = np.vstack((-np.sin(L @ x[[0, 3, 4]].reshape(-1, 1)).T @ L @ np.diag((param.l[[0, 3, 4]].T @ L).flatten()), - np.cos(L @ x[[0, 3, 4]].reshape(-1, 1)).T @ L @ np.diag((param.l[[0, 3, 4]].T @ L).flatten()))) / 6 - J = np.hstack(((Jl[:, 0] + Jr[:, 0]).reshape(-1, 1), Jl[:, 1:], Jr[:, 1:])) - return J - -def rman(x, param): - G = fractional_matrix_power(param.MuS, -0.5) - f = np.zeros((3, np.size(x, 1))) - for i in range(np.size(x, 1)): - Jt = Jkin_CoM(x[:, i], param) # Jacobian for center of mass - St = Jt @ Jt.T # manipulability matrix - - D, V = np.linalg.eig(G @ St @ G) - E = V @ np.diag(np.log(D)) @ np.linalg.pinv(V) - - E = np.tril(E) * (np.eye(2) + np.tril(np.ones(2), -1) * np.sqrt(2)) - f[:, i] = E[np.where(E!=0)] - return f - -# Jacobian for manipulability tracking with numerical computation -def Jman_num(x, param): - e = 1E-6 - X = np.matlib.repmat(x, 1, param.nbVarX) - F1 = rman(X, param) - F2 = rman(X + np.eye(param.nbVarX) * e, param) - J = (F2 - F1) / e - return J - -# Residuals f and Jacobians J for manipulability tracking -# (c=f'*f is the cost, g=J'*f is the gradient, H=J'*J is the approximated Hessian) -def f_manipulability(x, param): - f = rman(x, param) # Residuals - for t in range(np.size(x, 1)): - if t == 0: - J = Jman_num(x[:, t].reshape(-1, 1), param) - else: - J = scipy.linalg.block_diag(J, Jman_num(x[:, t].reshape(-1, 1), param)) # Jacobians - return f, J - -## Parameters -class Param: - def __init__(self): - self.dt = 1e0 # Time step length - self.nbIter = 100 # Maximum number of iterations for iLQR - self.nbPoints = 1 # Number of viapoints - self.nbData = 10 # Number of datapoints - self.nbVarX = 5 # State space dimension ([q1,q2,q3] for left arm, [q1,q4,q5] for right arm) - self.nbVarU = self.nbVarX # Control space dimension ([dq1, dq2, dq3, dq4, dq5]) - self.nbVarF = 4 # Objective function dimension ([x1,x2] for left arm and [x3,x4] for right arm) - self.l = np.ones((self.nbVarX, 1)) * 2. # Robot links lengths - self.r = 1e-6 # Control weight term - self.MuS = np.asarray([[3, 2], [2, 4]]) -param = Param() -# Precision matrix -Q = np.kron(np.identity(param.nbPoints), np.diag([0., 0., 0., 0.])) -# Control weight matrix -R = np.identity((param.nbData-1) * param.nbVarU) * param.r -# Precision matrix for continuous CoM tracking -Qc = np.kron(np.identity(param.nbData), np.diag([0., 0.])) - -# Time occurrence of viapoints -tl = np.linspace(0, param.nbData, param.nbPoints+1) -tl = np.rint(tl[1:]).astype(np.int64) - 1 -idx = np.array([i + np.arange(0, param.nbVarX, 1) for i in (tl*param.nbVarX)]) - -# iLQR -# =============================== - -u = np.zeros(param.nbVarU * (param.nbData-1)) # Initial control command -x0 = np.array([np.pi/3, np.pi/2, np.pi/3, -np.pi/3, -np.pi/4]) # Initial state - -# Transfer matrices (for linear system as single integrator) -Su0 = np.vstack([ - np.zeros([param.nbVarX, param.nbVarX*(param.nbData-1)]), - np.tril(np.kron(np.ones([param.nbData-1, param.nbData-1]), np.eye(param.nbVarX) * param.dt)) -]) -Sx0 = np.kron(np.ones(param.nbData), np.identity(param.nbVarX)).T -Su = Su0[idx.flatten()] # We remove the lines that are out of interest -for i in range(param.nbIter): - x = Su0 @ u + Sx0 @ x0 # System evolution - x = x.reshape([param.nbVarX, param.nbData], order='F') - f, J = f_manipulability(x[:, tl], param) # Residuals and Jacobians - du = np.linalg.inv(Su.T @ J.T @ J @ Su + R) @ (-Su.T @ J.T @ f.flatten('F') - u * param.r) # Gauss-Newton update - # Estimate step size with backtracking line search method - alpha = 1 - cost0 = f.flatten('F').T @ f.flatten('F') + np.linalg.norm(u)**2 * param.r # Cost - while True: - utmp = u + du * alpha - xtmp = Su0 @ utmp + Sx0 @ x0 # System evolution - xtmp = xtmp.reshape([param.nbVarX, param.nbData], order='F') - ftmp, _ = f_manipulability(xtmp[:, tl], param) # Residuals - cost = ftmp.flatten('F').T @ ftmp.flatten('F') + np.linalg.norm(utmp)**2 * param.r # Cost - if cost < cost0 or alpha < 1e-3: - u = utmp - print("Iteration {}, cost: {}".format(i,cost)) - break - alpha /= 2 - if np.linalg.norm(du * alpha) < 1E-2: - break # Stop iLQR iterations when solution is reached - -# Plots -# =============================== -plt.figure() -plt.axis('off') -plt.gca().set_aspect('equal', adjustable='box') - -fc = fkin_CoM(x, param) -al = np.linspace(-np.pi, np.pi, 50) -ax = plt.gca() - -# Plot desired manipulability ellipsoid -D1, V1 = np.linalg.eig(param.MuS) -D1 = np.diag(D1) -R1 = np.real(V1@np.sqrt(D1+0j)) -msh1 = (R1 @ np.array([np.cos(al), np.sin(al)]) * 0.52).T + np.matlib.repmat(fc[:, -1].reshape(-1, 1), 1, 50).T -p1 = patches.Polygon(msh1, closed=True, alpha=0.9) -p1.set_facecolor([1, 0.7, 0.7]) -p1.set_edgecolor([1, 0.6, 0.6]) -ax.add_patch(p1) - -# Plot robot manipulability ellipsoid -J = Jkin_CoM(x[:, -1], param) -S = J @ J.T -D2, V2 = np.linalg.eig(S) -D2 = np.diag(D2) -R2 = np.real(V2@np.sqrt(D2+0j)) -msh2 = (R2 @ np.array([np.cos(al), np.sin(al)]) * 0.5).T + np.matlib.repmat(fc[:, -1].reshape(-1, 1), 1, 50).T -p2 = patches.Polygon(msh2, closed=True, alpha=0.9) -p2.set_facecolor([0.4, 0.4, 0.4]) -p2.set_edgecolor([0.3, 0.3, 0.3]) -ax.add_patch(p2) - -# Plot CoM -fc = fkin_CoM(x, param) # Forward kinematics for center of mass -plt.plot(fc[0, 0], fc[1, 0], marker='o', markerfacecolor='none', markeredgewidth=4, markersize=10, markeredgecolor=[0.5, 0.5, 0.5]) # Plot CoM -plt.plot(fc[0, tl[-1]], fc[1, tl[-1]], marker='o', markerfacecolor='none', markeredgewidth=4, markersize=10, markeredgecolor=[0.2, 0.2, 0.2]) # Plot CoM - -# Plot end-effectors paths -f01 = fkin0(x[:, 0], param) -f02 = fkin0(x[:, tl[0]], param) - -# Get points of interest -f = fkin(x, param) -plt.plot(f01[0, :], f01[1, :], c='black', linewidth=4, alpha=.2) -plt.plot(f02[0, :], f02[1, :], c='black', linewidth=4, alpha=.4) - -plt.plot(f[0, :], f[1, :], c='black', marker='o', markevery=[0]+tl.tolist()) -plt.plot(f[2, :], f[3, :], c='black', marker='o', markevery=[0]+tl.tolist()) - - -plt.show()