Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
robotics-codes-from-scratch
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
rli
robotics-codes-from-scratch
Commits
2dcdd554
Commit
2dcdd554
authored
3 years ago
by
Jérémy MACEIRAS
Browse files
Options
Downloads
Plain Diff
Merge branch 'misc/ilqr_manipulator_dynamics'
parents
53cc1c61
5954e7de
Branches
Branches containing commit
No related tags found
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
python/iLQR_manipulator.py
+2
-0
2 additions, 0 deletions
python/iLQR_manipulator.py
python/iLQR_manipulator_dynamics.py
+79
-100
79 additions, 100 deletions
python/iLQR_manipulator_dynamics.py
with
81 additions
and
100 deletions
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
...
...
This diff is collapsed.
Click to expand it.
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
j
kin
(
param
,
x
):
T
=
np
.
tril
(
np
.
ones
((
len
(
x
),
len
(
x
)))
)
def
J
kin
(
x
,
param
):
L
=
np
.
tril
(
np
.
ones
((
len
(
x
),
len
(
x
)))
)
J
=
np
.
vstack
((
-
np
.
sin
(
T
@x
).
T
@
np
.
diag
(
param
.
l
inkLengths
)
@
T
,
np
.
cos
(
T
@x
).
T
@
np
.
diag
(
param
.
l
inkLengths
)
@
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
.
m
u
)
J
=
np
.
zeros
((
len
(
x
)
*
param
.
nbVarF
,
len
(
x
)
*
param
.
nbVarX
))
def
f_reach
(
x
,
param
):
f
=
logmap
(
fkin
(
x
,
param
),
param
.
M
u
)
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
=
j
kin
(
param
,
x
[
t
]
)
Jtmp
=
J
kin
(
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 p
arameters
#
P
arameters
# ===============================
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
.
l
inkLengths
=
[
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
.
m
u
=
np
.
asarray
([[
2
,
1
,
-
np
.
pi
/
6
],
[
3
,
2
,
-
np
.
pi
/
3
]])
.
T
# Viapoints
param
.
M
u
=
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
.
m
u
[
i
,
-
1
]
orn_t
=
param
.
M
u
[
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
.
m
u
[
i
,:
2
]
-
param
.
A
[
i
]
@np.array
(
param
.
sizeObj
)
rect_orn
=
param
.
m
u
[
i
,
-
1
]
rect_origin
=
param
.
M
u
[
i
,:
2
]
-
param
.
A
[
i
]
@np.array
(
param
.
sizeObj
)
rect_orn
=
param
.
M
u
[
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
.
m
u
[
i
,
0
],
param
.
m
u
[
i
,
1
],
s
=
100
,
marker
=
"
X
"
,
c
=
color_map
[
i
])
plt
.
scatter
(
param
.
M
u
[
i
,
0
],
param
.
M
u
[
i
,
1
],
s
=
100
,
marker
=
"
X
"
,
c
=
color_map
[
i
])
plt
.
show
()
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment