Skip to content
Snippets Groups Projects
Commit dd90a48b authored by Sylvain CALINON's avatar Sylvain CALINON
Browse files

Merge branch 'master' of gitlab.idiap.ch:rli/robotics-codes-from-scratch

parents 485b62d2 53cc1c61
Branches
No related tags found
No related merge requests found
......@@ -12,3 +12,4 @@ register_executable(iLQR_bimanual)
register_executable(iLQR_car)
register_executable(iLQR_manipulator)
register_executable(iLQR_manipulator_obstacle)
register_executable(iLQR_obstacle_GPIS)
\ No newline at end of file
This diff is collapsed.
'''
iLQR applied to a planar bimanual robot problem with a cost on tracking a desired
manipulability ellipsoid at the center of mass (batch formulation)
Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/
Written by Boyang Ti <https://www.idiap.ch/~bti/> and
......@@ -118,19 +119,19 @@ def f_manipulability(x, param):
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()
param = lambda: None # Lazy way to define an empty class in python
param.dt = 1e0 # Time step length
param.nbIter = 100 # Maximum number of iterations for iLQR
param.nbPoints = 1 # Number of viapoints
param.nbData = 10 # Number of datapoints
param.nbVarX = 5 # State space dimension ([q1,q2,q3] for left arm, [q1,q4,q5] for right arm)
param.nbVarU = param.nbVarX # Control space dimension ([dq1, dq2, dq3, dq4, dq5])
param.nbVarF = 4 # Objective function dimension ([x1,x2] for left arm and [x3,x4] for right arm)
param.l = np.ones((param.nbVarX, 1)) * 2. # Robot links lengths
param.r = 1e-6 # Control weight term
param.MuS = np.asarray([[10, 2], [2, 4]])
# Precision matrix
Q = np.kron(np.identity(param.nbPoints), np.diag([0., 0., 0., 0.]))
# Control weight matrix
......@@ -145,7 +146,6 @@ 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
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment