diff --git a/python/iLQR_manipulator.py b/python/iLQR_manipulator.py
index e216452cc017ec53960efc4fbadfc71322528c9c..2aa963d47157c32745eb03efae5692997e361aa9 100644
--- a/python/iLQR_manipulator.py
+++ b/python/iLQR_manipulator.py
@@ -85,6 +85,8 @@ def f_reach(x, param):
return f,J
## Parameters
+# ===============================
+
param = lambda: None # Lazy way to define an empty class in python
param.dt = 1e-2 # Time step length
param.nbData = 50 # Number of datapoints
diff --git a/python/iLQR_manipulator_dynamics.py b/python/iLQR_manipulator_dynamics.py
index a0db108edde5fa223635a85301bb8d0afcc5f998..471e2f3e018106fa36eb8a4e4c4b3d620303e5d3 100644
--- a/python/iLQR_manipulator_dynamics.py
+++ b/python/iLQR_manipulator_dynamics.py
@@ -21,7 +21,6 @@
'''
import numpy as np
-import numpy.matlib
import matplotlib.pyplot as plt
import matplotlib.patches as patches
@@ -36,46 +35,43 @@ def logmap(f,f0):
return error
# Forward kinematics for E-E
-def fkin(param,x):
- x = x.T
- A = np.tril(np.ones([param.nbVarX,param.nbVarX]))
- f = np.vstack((param.linkLengths @ np.cos(A @ x),
- param.linkLengths @ np.sin(A @ x),
- np.mod(np.sum(x,0)+np.pi, 2*np.pi) - np.pi)) #x1,x2,o (orientation as single Euler angle for planar robot)
- return f.T
+def fkin(x,param):
+ L = np.tril(np.ones([param.nbVarX, param.nbVarX]))
+ f = np.vstack([
+ param.l @ np.cos(L @ x.T),
+ param.l @ np.sin(L @ x.T),
+ np.mod(np.sum(x.T,0)+np.pi, 2*np.pi) - np.pi
+ ]) # f1,f2,f3, where f3 is the orientation (single Euler angle for planar robot)
+ return f.T
# Forward Kinematics for all joints
-def fkin0(param,x):
- T = np.tril(np.ones([param.nbVarX,param.nbVarX]))
- T2 = np.tril(np.matlib.repmat(param.linkLengths,len(x),1))
- f = np.vstack((
- T2 @ np.cos(T@x),
- T2 @ np.sin(T@x)
- )).T
- f = np.vstack((
- np.zeros(2),
- f
- ))
- return f
+def fkin0(x,param):
+ L = np.tril(np.ones([param.nbVarX, param.nbVarX]))
+ f = np.vstack([
+ L @ np.diag(param.l) @ np.cos(L @ x),
+ L @ np.diag(param.l) @ np.sin(L @ x)
+ ])
+ f = np.hstack([np.zeros([2,1]), f])
+ return f.T
# Jacobian with analytical computation (for single time step)
-def jkin(param,x):
- T = np.tril( np.ones((len(x),len(x))) )
+def Jkin(x,param):
+ L = np.tril( np.ones((len(x),len(x))) )
J = np.vstack((
- -np.sin(T@x).T @ np.diag(param.linkLengths) @ T,
- np.cos(T@x).T @ np.diag(param.linkLengths) @ T,
+ -np.sin(L@x).T @ np.diag(param.l) @ L,
+ np.cos(L@x).T @ np.diag(param.l) @ L,
np.ones(len(x))
))
return J
# Residual and Jacobian
-def f_reach(param,x):
- f = logmap(fkin(param,x),param.mu)
- J = np.zeros(( len(x) * param.nbVarF , len(x) * param.nbVarX ))
+def f_reach(x,param):
+ f = logmap(fkin(x,param),param.Mu)
+ J = np.zeros(( param.nbPoints * param.nbVarF , param.nbPoints * param.nbVarX ))
- for t in range(x.shape[0]):
+ for t in range(param.nbPoints ):
f[t,:2] = param.A[t].T @ f[t,:2] # Object oriented fk
- Jtmp = jkin(param,x[t])
+ Jtmp = Jkin(x[t],param)
Jtmp[:2] = param.A[t].T @ Jtmp[:2] # Object centered jacobian
if param.useBoundingBox:
@@ -91,74 +87,69 @@ def f_reach(param,x):
# Forward dynamic to compute
def forward_dynamics(x, u, param):
- g = 9.81 #Gravity norm
- kv = 1 #Joints Damping
- l = np.reshape( param.linkLengths, [1,param.nbVarX] )
+ l = np.reshape( param.l, [1,param.nbVarX] )
m = np.reshape( param.linkMasses, [1,param.nbVarX] )
- dt = param.dt
- nbDOFs = l.shape[1]
- nbData = int(u.shape[0]/nbDOFs + 1)
- Tm = np.multiply(np.triu(np.ones([nbDOFs,nbDOFs])), np.repeat(m, nbDOFs,0))
- T = np.tril(np.ones([nbDOFs, nbDOFs]))
- Su = np.zeros([2 * nbDOFs * nbData, nbDOFs * (nbData - 1)])
+ Tm = np.multiply(np.triu(np.ones([param.nbVarX,param.nbVarX])), np.repeat(m, param.nbVarX,0))
+ T = np.tril(np.ones([param.nbVarX, param.nbVarX]))
+ Su = np.zeros([2 * param.nbVarX * param.nbData , param.nbVarX * (param.nbData - 1)])
#Precomputation of mask (in tensor form)
- S1= np.zeros([nbDOFs, nbDOFs, nbDOFs])
- J_index = np.ones([1, nbDOFs])
- for j in range(nbDOFs):
+ S1= np.zeros([param.nbVarX, param.nbVarX, param.nbVarX])
+ J_index = np.ones([1, param.nbVarX])
+ for j in range(param.nbVarX):
J_index[0,:j] = np.zeros([j])
- S1[:,:,j] = np.repeat(J_index @ np.eye(nbDOFs), nbDOFs, 0) - np.transpose(np.repeat(J_index @ np.eye(nbDOFs), nbDOFs, 0))
+ S1[:,:,j] = np.repeat(J_index @ np.eye(param.nbVarX), param.nbVarX, 0) - np.transpose(np.repeat(J_index @ np.eye(param.nbVarX), param.nbVarX, 0))
#Initialization of dM and dC tensors and A21 matrix
- dM = np.zeros([nbDOFs, nbDOFs, nbDOFs])
- dC = np.zeros([nbDOFs, nbDOFs, nbDOFs])
- A21 = np.zeros([nbDOFs, nbDOFs])
+ dM = np.zeros([param.nbVarX, param.nbVarX, param.nbVarX])
+ dC = np.zeros([param.nbVarX, param.nbVarX, param.nbVarX])
+ A21 = np.zeros([param.nbVarX, param.nbVarX])
- for t in range(nbData-1):
+ for t in range(param.nbData -1):
# Computation in matrix form of G,M, and C
- G =-np.reshape(np.sum(Tm,1), [nbDOFs,1]) * l.T * np.cos(T @ np.reshape(x[t,0:nbDOFs], [nbDOFs,1])) * g
+ G =-np.reshape(np.sum(Tm,1), [param.nbVarX,1]) * l.T * np.cos(T @ np.reshape(x[t,0:param.nbVarX], [param.nbVarX,1])) * param.g
G = T.T @ G
- M = (l.T * l) * np.cos(np.reshape(T @ x[t,:nbDOFs], [nbDOFs,1]) - T @ x[t,:nbDOFs]) * (Tm ** .5 @ ((Tm ** .5).T))
+ M = (l.T * l) * np.cos(np.reshape(T @ x[t,:param.nbVarX], [param.nbVarX,1]) - T @ x[t,:param.nbVarX]) * (Tm ** .5 @ ((Tm ** .5).T))
M = T.T @ M @ T
- C = -(l.T * l) * np.sin(np.reshape(T @ x[t,:nbDOFs], [nbDOFs,1]) - T @ x[t,:nbDOFs]) * (Tm ** .5 @ ((Tm ** .5).T))
+ C = -(l.T * l) * np.sin(np.reshape(T @ x[t,:param.nbVarX], [param.nbVarX,1]) - T @ x[t,:param.nbVarX]) * (Tm ** .5 @ ((Tm ** .5).T))
# Computation in tensor form of derivatives dG,dM, and dC
- dG = np.diagflat(np.reshape(np.sum(Tm,1), [nbDOFs,1]) * l.T * np.sin(T @ np.reshape(x[t,0:nbDOFs], [nbDOFs,1])) * g) @ T
- dM_tmp = (l.T * l) * np.sin(np.reshape(T @ x[t,:nbDOFs], [nbDOFs,1]) - T @ x[t,:nbDOFs]) * (Tm ** .5 @ ((Tm ** .5).T))
+ dG = np.diagflat(np.reshape(np.sum(Tm,1), [param.nbVarX,1]) * l.T * np.sin(T @ np.reshape(x[t,0:param.nbVarX], [param.nbVarX,1])) * param.g) @ T
+ dM_tmp = (l.T * l) * np.sin(np.reshape(T @ x[t,:param.nbVarX], [param.nbVarX,1]) - T @ x[t,:param.nbVarX]) * (Tm ** .5 @ ((Tm ** .5).T))
for j in range(dM.shape[2]):
dM[:,:,j] = T.T @ (dM_tmp * S1[:,:,j]) @ T
- dC_tmp = (l.T * l) * np.cos(np.reshape( T @ x[t,:nbDOFs], [nbDOFs,1]) - T @ x[t,:nbDOFs]) * (Tm ** .5 @ ((Tm ** .5).T))
+ dC_tmp = (l.T * l) * np.cos(np.reshape( T @ x[t,:param.nbVarX], [param.nbVarX,1]) - T @ x[t,:param.nbVarX]) * (Tm ** .5 @ ((Tm ** .5).T))
for j in range(dC.shape[2]):
dC[:,:,j] = dC_tmp * S1[:,:,j]
# update pose
- tau = np.reshape(u[(t) * nbDOFs:(t + 1) * nbDOFs], [nbDOFs, 1])
+ tau = np.reshape(u[(t) * param.nbVarX:(t + 1) * param.nbVarX], [param.nbVarX, 1])
inv_M = np.linalg.inv(M)
- ddq =inv_M @ (tau + G + T.T @ C @ (T @ np.reshape(x[t,nbDOFs:], [nbDOFs,1])) ** 2) - T @ np.reshape(x[t,nbDOFs:], [nbDOFs,1]) * kv
+ ddq =inv_M @ (tau + G + T.T @ C @ (T @ np.reshape(x[t,param.nbVarX:], [param.nbVarX,1])) ** 2) - T @ np.reshape(x[t,param.nbVarX:], [param.nbVarX,1]) * param.kv
# compute local linear systems
- x[t+1,:] = x[t,:] + np.hstack([x[t,nbDOFs:], np.reshape(ddq, [nbDOFs,])]) * dt
- A11 = np.eye(nbDOFs)
- A12 = A11 * dt
- A22 = np.eye(nbDOFs) + (2 * inv_M @ T.T @ C @ np.diagflat(T @ x[t,nbDOFs:]) @ T - T * kv) * dt
- for j in range(nbDOFs):
- A21[:,j] = (-inv_M @ dM[:,:,j] @ inv_M @ (tau + G + T.T @ C @ (T @ np.reshape(x[t,nbDOFs:], [nbDOFs,1])) ** 2)
- + np.reshape(inv_M @ T.T @ dG[:,j], [nbDOFs,1]) + inv_M @ T .T @ dC[:,:,j] @ (T @ np.reshape(x[t,nbDOFs:], [nbDOFs,1])) ** 2).flatten()
- A = np.vstack((np.hstack((A11, A12)), np.hstack((A21 * dt, A22))))
- B = np.vstack((np.zeros([nbDOFs, nbDOFs]), inv_M * dt))
+ x[t+1,:] = x[t,:] + np.hstack([x[t,param.nbVarX:], np.reshape(ddq, [param.nbVarX,])]) * param.dt
+ A11 = np.eye(param.nbVarX)
+ A12 = A11 * param.dt
+ A22 = np.eye(param.nbVarX) + (2 * inv_M @ T.T @ C @ np.diagflat(T @ x[t,param.nbVarX:]) @ T - T * param.kv) * param.dt
+ for j in range(param.nbVarX):
+ A21[:,j] = (-inv_M @ dM[:,:,j] @ inv_M @ (tau + G + T.T @ C @ (T @ np.reshape(x[t,param.nbVarX:], [param.nbVarX,1])) ** 2)
+ + np.reshape(inv_M @ T.T @ dG[:,j], [param.nbVarX,1]) + inv_M @ T .T @ dC[:,:,j] @ (T @ np.reshape(x[t,param.nbVarX:], [param.nbVarX,1])) ** 2).flatten()
+ A = np.vstack((np.hstack((A11, A12)), np.hstack((A21 * param.dt, A22))))
+ B = np.vstack((np.zeros([param.nbVarX, param.nbVarX]), inv_M * param.dt))
# compute transformation matrix
- Su[2 * nbDOFs * (t + 1):2 * nbDOFs * (t + 2),:] = A @ Su[2 * nbDOFs * t:2 * nbDOFs * (t + 1),:]
- Su[2 * nbDOFs * (t + 1):2 * nbDOFs * (t + 2), nbDOFs * t:nbDOFs * (t + 1)] =B
+ Su[2 * param.nbVarX * (t + 1):2 * param.nbVarX * (t + 2),:] = A @ Su[2 * param.nbVarX * t:2 * param.nbVarX * (t + 1),:]
+ Su[2 * param.nbVarX * (t + 1):2 * param.nbVarX * (t + 2), param.nbVarX * t:param.nbVarX * (t + 1)] =B
return x, Su
-# General param parameters
+# Parameters
# ===============================
param = lambda: None # Lazy way to define an empty class in python
@@ -169,66 +160,54 @@ 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 = 3 # Objective function dimension (f1,f2,f3, with f3 as orientation)
-param.linkLengths = [2,2,1] # Robot links lengths
+param.l = [2,2,1] # Robot links lengths
param.linkMasses = [1,1,1]
-param.sizeObj = [.2,.3] # Size of objects
+param.g = 9.81 # Gravity norm
+param.kv = 1 # Joint damping
+param.sz = [.2,.3] # Size of objects
param.r = 1e-6 # Control weight term
-param.mu = np.asarray([[2,1,-np.pi/6], [3,2,-np.pi/3]]).T # Viapoints
+param.Mu = np.asarray([[2,1,-np.pi/3], [3,2,-np.pi/3]]) # Viapoints
param.useBoundingBox = False
+param.A = np.zeros( (param.nbPoints,2,2) ) # Object orientation matrices
-# Task parameters
+# Main program
# ===============================
-# Targets
-param.mu = np.asarray([
- [2,1,-np.pi/3], # x , y , orientation
- [3,2,-np.pi/3]
-])
-
# Transformation matrices
-param.A = np.zeros( (param.nbPoints,2,2) )
for i in range(param.nbPoints):
- orn_t = param.mu[i,-1]
+ orn_t = param.Mu[i,-1]
param.A[i,:,:] = np.asarray([
[np.cos(orn_t) , -np.sin(orn_t)],
[np.sin(orn_t) , np.cos(orn_t)]
])
-# Regularization matrix
-R = np.identity( (param.nbData-1) * param.nbVarU ) * param.r
-
# Precision matrix
Q = np.identity( param.nbVarF * param.nbPoints)*1e5
-# System parameters
-# ===============================
+# Regularization matrix
+R = np.identity( (param.nbData-1) * param.nbVarU ) * param.r
# Time occurence 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* 2* param.nbVarX)])
+# 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 position (in joint space)
-v0 = np.array([0,0,0])#initial velocity (in joint space)
+v0 = np.array([0,0,0]) #initial velocity (in joint space)
x = np.zeros([param.nbData, 2*param.nbVarX])
x[0,:param.nbVarX] = x0
x[0,param.nbVarX:] = v0
-# 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
- # We remove the lines that are out of interest
-
-# Solving iLQR
-# ===============================
for i in range( param.nbIter ):
# system evolution and Transfer matrix (computed from forward dynamics)
x, Su0 = forward_dynamics(x, u, param)
Su = Su0[idx.flatten()]
- f, J = f_reach(param,x[tl,:param.nbVarX])
+ f, J = f_reach(x[tl,:param.nbVarX],param)
du = np.linalg.inv(Su.T @ J.T @ Q @ J @ Su + R) @ (-Su.T @ J.T @ Q @ f.flatten() - u * param.r)
# Perform line search
@@ -238,7 +217,7 @@ for i in range( param.nbIter ):
while True:
utmp = u + du * alpha
xtmp, _ = forward_dynamics(x, utmp, param)
- ftmp, _ = f_reach(param,xtmp[tl,:param.nbVarX])
+ ftmp, _ = f_reach(xtmp[tl,:param.nbVarX],param)
cost = ftmp.flatten() @ Q @ ftmp.flatten() + np.linalg.norm(utmp) * param.r
if cost < cost0 or alpha < 1e-3:
@@ -258,9 +237,9 @@ plt.axis("off")
plt.gca().set_aspect('equal', adjustable='box')
# Get points of interest
-f = fkin(param,x[:,:param.nbVarX])
-f00 = fkin0(param,x[0,:param.nbVarX])
-fT0 = fkin0(param,x[-1,:param.nbVarX])
+f = fkin(x[:,:param.nbVarX],param)
+f00 = fkin0(x[0,:param.nbVarX],param)
+fT0 = fkin0(x[-1,:param.nbVarX],param)
plt.plot( f00[:,0] , f00[:,1],c='black',linewidth=5,alpha=.2)
plt.plot( fT0[:,0] , fT0[:,1],c='black',linewidth=5,alpha=.6)
@@ -273,12 +252,12 @@ color_map = ["deepskyblue","darkorange"]
for i in range(param.nbPoints):
if param.useBoundingBox:
- rect_origin = param.mu[i,:2] - param.A[i]@np.array(param.sizeObj)
- rect_orn = param.mu[i,-1]
+ rect_origin = param.Mu[i,:2] - param.A[i]@np.array(param.sizeObj)
+ rect_orn = param.Mu[i,-1]
rect = patches.Rectangle(rect_origin,param.sizeObj[0]*2,param.sizeObj[1]*2,np.degrees(rect_orn),color=color_map[i])
ax.add_patch(rect)
else:
- plt.scatter(param.mu[i,0],param.mu[i,1],s=100,marker="X",c=color_map[i])
+ plt.scatter(param.Mu[i,0],param.Mu[i,1],s=100,marker="X",c=color_map[i])
plt.show()