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
ca1eeab4
Commit
ca1eeab4
authored
3 years ago
by
Jérémy MACEIRAS
Browse files
Options
Downloads
Patches
Plain Diff
[misc] Fixed bad rebase
parent
00a00db7
Branches
Branches containing commit
No related tags found
1 merge request
!9
Merge branch example/lqt nullspace on master
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
python/iLQR_bimanual_manipulability.py
+0
-231
0 additions, 231 deletions
python/iLQR_bimanual_manipulability.py
with
0 additions
and
231 deletions
python/iLQR_bimanual_manipulability.py
deleted
100644 → 0
+
0
−
231
View file @
00a00db7
'''
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
()
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