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
485b62d2
Commit
485b62d2
authored
3 years ago
by
Hakan GIRGIN
Browse files
Options
Downloads
Patches
Plain Diff
Uploading python version of demo_OC_LQT_nullspace01.m
parent
5f699061
Branches
Branches containing commit
Tags
Tags containing commit
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
python/LQT_nullspace.py
+153
-0
153 additions, 0 deletions
python/LQT_nullspace.py
with
153 additions
and
0 deletions
python/LQT_nullspace.py
0 → 100644
+
153
−
0
View file @
485b62d2
'''
Linear Quadratic tracker applied on a via point example
Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/
Written by Jeremy Maceiras <jeremy.maceiras@idiap.ch>,
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
from
math
import
factorial
import
matplotlib.pyplot
as
plt
from
scipy.linalg
import
block_diag
# Parameters
# ===============================
dt
=
1e-1
# Time step length
nbPoints
=
1
# Number of targets
nbDeriv
=
1
# Order of the dynamical system
nbVarPos
=
2
# Number of position variable
nbData
=
50
# Number of datapoints
rfactor
=
1e-4
# Control weight term
nbRepros
=
60
# Number of stochastic reproductions
nb_var
=
nbVarPos
*
nbDeriv
# Dimension of state vector
# Dynamical System settings (discrete)
# =====================================
A1d
=
np
.
zeros
((
nbDeriv
,
nbDeriv
))
B1d
=
np
.
zeros
((
nbDeriv
,
1
))
for
i
in
range
(
nbDeriv
):
A1d
+=
np
.
diag
(
np
.
ones
(
nbDeriv
-
i
)
,
i
)
*
dt
**
i
*
1
/
factorial
(
i
)
B1d
[
nbDeriv
-
i
-
1
]
=
dt
**
(
i
+
1
)
*
1
/
factorial
(
i
+
1
)
A
=
np
.
kron
(
A1d
,
np
.
identity
(
nbVarPos
))
B
=
np
.
kron
(
B1d
,
np
.
identity
(
nbVarPos
))
# Build Sx and Su transfer matrices
Su
=
np
.
zeros
((
nb_var
*
nbData
,
nbVarPos
*
(
nbData
-
1
)))
Sx
=
np
.
kron
(
np
.
ones
((
nbData
,
1
)),
np
.
eye
(
nb_var
,
nb_var
))
M
=
B
for
i
in
range
(
1
,
nbData
):
Sx
[
i
*
nb_var
:
nbData
*
nb_var
,:]
=
np
.
dot
(
Sx
[
i
*
nb_var
:
nbData
*
nb_var
,:],
A
)
Su
[
nb_var
*
i
:
nb_var
*
i
+
M
.
shape
[
0
],
0
:
M
.
shape
[
1
]]
=
M
M
=
np
.
hstack
((
np
.
dot
(
A
,
M
),
B
))
# [0,nb_state_var-1]
# Cost function settings
# =====================================
R
=
np
.
identity
(
(
nbData
-
1
)
*
nbVarPos
)
*
rfactor
# Control cost matrix
t_list
=
np
.
stack
([
nbData
-
1
])
# viapoint time list
mu_list
=
np
.
stack
([
np
.
array
([
20
,
10.
])])
# viapoint list
Q_list
=
np
.
stack
([
np
.
eye
(
nb_var
)
*
1e3
])
# viapoint precision list
mus
=
np
.
zeros
((
nbData
,
nb_var
))
Qs
=
np
.
zeros
((
nbData
,
nb_var
,
nb_var
))
for
i
,
t
in
enumerate
(
t_list
):
mus
[
t
]
=
mu_list
[
i
]
Qs
[
t
]
=
Q_list
[
i
]
muQ
=
mus
.
flatten
()
Q
=
block_diag
(
*
Qs
)
# Change here off block diagonals of Q
# Batch LQR Reproduction
# =====================================
## Precomputation of basis functions to generate structured stochastic u through Bezier curves
# Building Bernstein basis functions
def
build_phi_bernstein
(
nb_data
,
nb_fct
):
t
=
np
.
linspace
(
0
,
1
,
nb_data
)
phi
=
np
.
zeros
((
nb_data
,
nb_fct
))
for
i
in
range
(
nb_fct
):
phi
[:,
i
]
=
factorial
(
nb_fct
-
1
)
/
(
factorial
(
i
)
*
factorial
(
nb_fct
-
1
-
i
))
*
(
1
-
t
)
**
(
nb_fct
-
1
-
i
)
*
t
**
i
return
phi
nbRBF
=
10
H
=
build_phi_bernstein
(
nbData
-
1
,
nbRBF
)
J
=
Q
@
Su
N
=
np
.
eye
(
J
.
shape
[
-
1
])
-
np
.
linalg
.
pinv
(
J
)
@
J
# nullspace operator of LQT
# Principal Task
x0
=
np
.
zeros
(
nb_var
)
u1
=
np
.
linalg
.
solve
(
Su
.
T
@
Q
@
Su
+
R
,
Su
.
T
@
Q
@
(
muQ
-
Sx
@
x0
))
x1
=
(
Sx
@
x0
+
Su
@
u1
).
reshape
((
-
1
,
nb_var
))
# Secondary Task
repr_x
=
np
.
zeros
((
nbRepros
,
nbData
,
nb_var
))
for
n
in
range
(
nbRepros
):
w
=
np
.
random
.
randn
(
nbRBF
,
nbVarPos
)
*
1E1
# Random weights
u2
=
H
@
w
# Reconstruction of control signals by a weighted superposition of basis functions
u
=
u1
+
N
@
u2
.
flatten
()
repr_x
[
n
]
=
np
.
reshape
(
Sx
@
x0
+
Su
@
u
,
(
-
1
,
nb_var
))
# Reshape data for plotting
# Plotting
# =========
plt
.
figure
()
plt
.
title
(
"
2D Trajectory
"
)
plt
.
scatter
(
x1
[
0
,
0
],
x1
[
0
,
1
],
c
=
'
black
'
,
s
=
100
)
for
mu
in
mu_list
:
plt
.
scatter
(
mu
[
0
],
mu
[
1
],
c
=
'
red
'
,
s
=
100
)
plt
.
plot
(
x1
[:,
0
]
,
x1
[:,
1
],
c
=
'
black
'
)
for
i
in
range
(
nbRepros
):
plt
.
plot
(
repr_x
[
i
,
:,
0
],
repr_x
[
i
,
:,
1
],
c
=
'
blue
'
,
alpha
=
0.1
,
zorder
=
0
)
plt
.
axis
(
"
off
"
)
plt
.
gca
().
set_aspect
(
'
equal
'
,
adjustable
=
'
box
'
)
fig
,
axs
=
plt
.
subplots
(
2
,
1
)
for
i
,
t
in
enumerate
(
t_list
):
axs
[
0
].
scatter
(
t
,
mu_list
[
i
][
0
],
c
=
'
red
'
)
for
i
in
range
(
nbRepros
):
axs
[
0
].
plot
(
repr_x
[
i
,
:,
0
],
c
=
'
blue
'
,
alpha
=
0.1
,
zorder
=
0
)
axs
[
0
].
plot
(
x1
[:,
0
],
c
=
'
black
'
)
axs
[
0
].
set_ylabel
(
"
$x_1$
"
)
axs
[
0
].
set_xticks
([
0
,
nbData
])
axs
[
0
].
set_xticklabels
([
"
0
"
,
"
T
"
])
for
i
,
t
in
enumerate
(
t_list
):
axs
[
1
].
scatter
(
t
,
mu_list
[
i
][
1
],
c
=
'
red
'
)
for
i
in
range
(
nbRepros
):
axs
[
1
].
plot
(
repr_x
[
i
,
:,
1
],
c
=
'
blue
'
,
alpha
=
0.1
,
zorder
=
0
)
axs
[
1
].
plot
(
x1
[:,
1
],
c
=
'
black
'
)
axs
[
1
].
set_ylabel
(
"
$x_2$
"
)
axs
[
1
].
set_xlabel
(
"
$t$
"
)
axs
[
1
].
set_xticks
([
0
,
nbData
])
axs
[
1
].
set_xticklabels
([
"
0
"
,
"
T
"
])
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