Skip to content
Snippets Groups Projects
Commit 2dcdd554 authored by Jérémy MACEIRAS's avatar Jérémy MACEIRAS
Browse files

Merge branch 'misc/ilqr_manipulator_dynamics'

parents 53cc1c61 5954e7de
Branches
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
python/iLQR_manipulator.py
+ 2
− 0
View file @ 2dcdd554
......@@ -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
......
python/iLQR_manipulator_dynamics.py
+ 79
− 100
View file @ 2dcdd554
......@@ -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()
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment