Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
pbdlib-matlab
Project overview
Project overview
Details
Activity
Releases
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Analytics
Analytics
CI / CD
Repository
Value Stream
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
rli
pbdlib-matlab
Commits
43f2e613
Commit
43f2e613
authored
Jul 30, 2014
by
Sylvain Calinon
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Notational changes in LQR files and update of kmeans init
parent
edcab51a
Changes
5
Hide whitespace changes
Inline
Side-by-side
Showing
5 changed files
with
54 additions
and
68 deletions
+54
-68
demo_testLQR03.m
demo_testLQR03.m
+2
-2
init_tensorGMM_kmeans.m
init_tensorGMM_kmeans.m
+16
-17
kmeansClustering.m
kmeansClustering.m
+1
-1
reproduction_LQR_finiteHorizon.m
reproduction_LQR_finiteHorizon.m
+29
-33
reproduction_LQR_infiniteHorizon.m
reproduction_LQR_infiniteHorizon.m
+6
-15
No files found.
demo_testLQR03.m
View file @
43f2e613
...
...
@@ -44,9 +44,9 @@ for n=1:nbRepros
r
(
n
)
=
reproduction_LQR_infiniteHorizon
(
DataIn
,
model
,
a
,
0
,
rFactor
);
else
%First call to LQR to get an estimate of the final feedback terms
[
~
,
S
final
]
=
reproduction_LQR_infiniteHorizon
(
DataIn
(
end
),
model
,
aFinal
,
0
,
rFactor
);
[
~
,
P
final
]
=
reproduction_LQR_infiniteHorizon
(
DataIn
(
end
),
model
,
aFinal
,
0
,
rFactor
);
%Second call to LQR with finite horizon
r
(
n
)
=
reproduction_LQR_finiteHorizon
(
DataIn
,
model
,
a
,
0
,
rFactor
,
S
final
);
r
(
n
)
=
reproduction_LQR_finiteHorizon
(
DataIn
,
model
,
a
,
0
,
rFactor
,
P
final
);
end
end
for
n
=
1
:
nbRepros
...
...
init_tensorGMM_kmeans.m
View file @
43f2e613
function
model
=
init_tensorGMM_kmeans
(
Data
,
model
)
%
Author: Leonel Rozo, 2014
%
http://programming-by-demonstration.org/LeonelRozo
%
%
Initialization of the model with k-means.
%
Authors: Sylvain Calinon, Tohid Alizadeh, 2013
%
http://programming-by-demonstration.org/
diagRegularizationFactor
=
1E-4
;
%Matricization/flattening of tensor
DataAll
=
reshape
(
Data
,
size
(
Data
,
1
)
*
size
(
Data
,
2
),
size
(
Data
,
3
));
DataAll
=
reshape
(
Data
,
size
(
Data
,
1
)
*
size
(
Data
,
2
),
size
(
Data
,
3
));
%Matricization/flattening of tensor
%
The function 'kmeans' below is from the Matlab Statistics Toolbox (see note above)
[
Data_id
,
Centers
]
=
kmeans
(
DataAll
'
,
model
.
nbStates
);
%
k-means clustering
[
Data_id
,
Mu
]
=
kmeansClustering
(
DataAll
,
model
.
nbStates
);
% Setting means and covariance matrices
Mu
=
Centers
'
;
Sigma
=
zeros
(
model
.
nbFrames
*
model
.
nbVar
,
model
.
nbFrames
*
model
.
nbVar
,
model
.
nbStates
);
for
i
=
1
:
model
.
nbStates
for
i
=
1
:
model
.
nbStates
idtmp
=
find
(
Data_id
==
i
);
model
.
Priors
(
i
)
=
length
(
idtmp
);
Sigma
(:,:,
i
)
=
cov
(
DataAll
(:,
idtmp
)
'
)
+
eye
(
size
(
DataAll
,
1
))
*
diagRegularizationFactor
;
Sigma
(:,:,
i
)
=
cov
(
[
DataAll
(:,
idtmp
)
DataAll
(:,
idtmp
)]
'
)
+
eye
(
size
(
DataAll
,
1
))
*
diagRegularizationFactor
;
end
model
.
Priors
=
model
.
Priors
/
sum
(
model
.
Priors
);
%Reshape GMM parameters into a tensor
for
m
=
1
:
model
.
nbFrames
for
i
=
1
:
model
.
nbStates
model
.
Mu
(:,
m
,
i
)
=
Mu
((
m
-
1
)
*
model
.
nbVar
+
1
:
m
*
model
.
nbVar
,
i
);
model
.
Sigma
(:,:,
m
,
i
)
=
Sigma
((
m
-
1
)
*
model
.
nbVar
+
1
:
m
*
model
.
nbVar
,(
m
-
1
)
*
model
.
nbVar
+
1
:
m
*
model
.
nbVar
,
i
);
%Reshape GMM parameters into a tensor
for
m
=
1
:
model
.
nbFrames
for
i
=
1
:
model
.
nbStates
model
.
Mu
(:,
m
,
i
)
=
Mu
((
m
-
1
)
*
model
.
nbVar
+
1
:
m
*
model
.
nbVar
,
i
);
model
.
Sigma
(:,:,
m
,
i
)
=
Sigma
((
m
-
1
)
*
model
.
nbVar
+
1
:
m
*
model
.
nbVar
,(
m
-
1
)
*
model
.
nbVar
+
1
:
m
*
model
.
nbVar
,
i
);
end
end
kmeansClustering.m
View file @
43f2e613
function
[
idList
,
Mu
]
=
kmeansClustering
(
Data
,
nbStates
)
% Initialization of the model with k-means.
% Author: Sylvain Calinon, Tohid Alizadeh, 2013
% Author
s
: Sylvain Calinon, Tohid Alizadeh, 2013
% http://programming-by-demonstration.org/
%Criterion to stop the EM iterative update
...
...
reproduction_LQR_finiteHorizon.m
View file @
43f2e613
function
r
=
reproduction_LQR_finiteHorizon
(
DataIn
,
model
,
r
,
currPos
,
rFactor
,
S
final
)
function
r
=
reproduction_LQR_finiteHorizon
(
DataIn
,
model
,
r
,
currPos
,
rFactor
,
P
final
)
% Reproduction with a linear quadratic regulator of finite horizon
%
% Authors: Sylvain Calinon and Danilo Bruno, 2014
...
...
@@ -28,23 +28,23 @@ B = kron([0; 1], eye(nbVarOut));
%Initialize Q and R weighting matrices
Q
=
zeros
(
nbVarOut
*
2
,
nbVarOut
*
2
,
nbData
);
for
t
=
1
:
nbData
Q
(
1
:
nbVarOut
,
1
:
nbVarOut
,
t
)
=
inv
(
r
.
currSigma
(:,:,
t
));
Q
(
1
:
nbVarOut
,
1
:
nbVarOut
,
t
)
=
inv
(
r
.
currSigma
(:,:,
t
));
end
R
=
eye
(
nbVarOut
)
*
rFactor
;
if
nargin
<
6
%S
final = B*R*[eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]
S
final
=
B
*
R
*
[
eye
(
nbVarOut
)
*
0
,
eye
(
nbVarOut
)
*
0
];
%final feedback terms (boundary conditions)
%P
final = B*R*[eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]
P
final
=
B
*
R
*
[
eye
(
nbVarOut
)
*
0
,
eye
(
nbVarOut
)
*
0
];
%final feedback terms (boundary conditions)
end
%Auxiliary variables to minimize the cost function
S
=
zeros
(
nbVarOut
*
2
,
nbVarOut
*
2
,
nbData
);
P
=
zeros
(
nbVarOut
*
2
,
nbVarOut
*
2
,
nbData
);
d
=
zeros
(
nbVarOut
*
2
,
nbData
);
%For optional feedforward term computation
%Feedback term
L
=
zeros
(
nbVarOut
,
nbVarOut
*
2
,
nbData
);
%Compute
S
_T from the desired final feedback gains L_T,
S
(:,:,
nbData
)
=
S
final
;
%Compute
P
_T from the desired final feedback gains L_T,
P
(:,:,
nbData
)
=
P
final
;
%Compute derivative of target path
%dTar = diff(r.currTar, 1, 2); %For optional feedforward term computation
...
...
@@ -62,14 +62,14 @@ dtar = gradient(tar,1,2)/model.dt;
%Backward integration of the Riccati equation and additional equation
for
t
=
nbData
-
1
:
-
1
:
1
S
(:,:,
t
)
=
S
(:,:,
t
+
1
)
+
model
.
dt
*
(
A
'*S(:,:,t+1) + S(:,:,t+1)*A - S(:,:,t+1) * B * (R\B'
)
*
S
(:,:,
t
+
1
)
+
Q
(:,:,
t
+
1
));
%Optional feedforward term computation
d
(:,
t
)
=
d
(:,
t
+
1
)
+
model
.
dt
*
((
A
'-
S(:,:,t+1)*B*(R\B'
))
*
d
(:,
t
+
1
)
+
S
(:,:,
t
+
1
)
*
dtar
(:,
t
+
1
)
-
S
(:,:,
t
+
1
)
*
A
*
tar
(:,
t
+
1
));
P
(:,:,
t
)
=
P
(:,:,
t
+
1
)
+
model
.
dt
*
(
A
'*P(:,:,t+1) + P(:,:,t+1)*A - P(:,:,t+1)*B*(R\B'
)
*
P
(:,:,
t
+
1
)
+
Q
(:,:,
t
+
1
));
%Optional feedforward term computation
d
(:,
t
)
=
d
(:,
t
+
1
)
+
model
.
dt
*
((
A
'-
P(:,:,t+1)*B*(R\B'
))
*
d
(:,
t
+
1
)
+
P
(:,:,
t
+
1
)
*
dtar
(:,
t
+
1
)
-
P
(:,:,
t
+
1
)
*
A
*
tar
(:,
t
+
1
));
end
%Computation of the feedback term L in u=-LX+M
%Computation of the feedback term L
and feedforward term M
in u=-LX+M
for
t
=
1
:
nbData
L
(:,:,
t
)
=
R
\
B
'
*
S
(:,:,
t
);
M
(:,
t
)
=
R
\
B
'
*
d
(:,
t
);
%Optional feedforward term computation
L
(:,:,
t
)
=
R
\
B
'
*
P
(:,:,
t
);
M
(:,
t
)
=
R
\
B
'
*
d
(:,
t
);
%Optional feedforward term computation
end
%% Reproduction with varying impedance parameters
...
...
@@ -77,26 +77,22 @@ end
x
=
currPos
;
dx
=
zeros
(
nbVarOut
,
1
);
for
t
=
1
:
nbData
%Compute acceleration (with only feedback term)
%ddx = -L(:,:,t) * [x-r.currTar(:,t); dx];
%Compute acceleration (with both feedback and feedforward terms)
ddx
=
-
L
(:,:,
t
)
*
[
x
-
r
.
currTar
(:,
t
);
dx
]
+
M
(:,
t
);
r
.
FB
(:,
t
)
=
-
L
(:,:,
t
)
*
[
x
-
r
.
currTar
(:,
t
);
dx
];
r
.
FF
(:,
t
)
=
M
(:,
t
);
%ddx = -L * ([x;dx]-tar(:,t)) + M;
%r.FB(:,t) = -L * ([x;dx]-tar(:,t));
%r.FF(:,t) = M;
%Update velocity and position
dx
=
dx
+
ddx
*
model
.
dt
;
x
=
x
+
dx
*
model
.
dt
;
%Log data
r
.
Data
(:,
t
)
=
[
DataIn
(:,
t
);
x
];
r
.
ddxNorm
(
t
)
=
norm
(
ddx
);
r
.
kpDet
(
t
)
=
det
(
L
(:,
1
:
nbVarOut
,
t
));
r
.
kvDet
(
t
)
=
det
(
L
(:,
nbVarOut
+
1
:
end
,
t
));
%Compute acceleration (with only feedback term)
%ddx = -L(:,:,t) * [x-r.currTar(:,t); dx];
%Compute acceleration (with both feedback and feedforward terms)
ddx
=
-
L
(:,:,
t
)
*
[
x
-
r
.
currTar
(:,
t
);
dx
]
+
M
(:,
t
);
%r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx];
%r.FF(:,t) = M(:,t);
%Update velocity and position
dx
=
dx
+
ddx
*
model
.
dt
;
x
=
x
+
dx
*
model
.
dt
;
%Log data
r
.
Data
(:,
t
)
=
[
DataIn
(:,
t
);
x
];
r
.
ddxNorm
(
t
)
=
norm
(
ddx
);
r
.
kpDet
(
t
)
=
det
(
L
(:,
1
:
nbVarOut
,
t
));
r
.
kvDet
(
t
)
=
det
(
L
(:,
nbVarOut
+
1
:
end
,
t
));
end
...
...
reproduction_LQR_infiniteHorizon.m
View file @
43f2e613
function
[
r
,
S
]
=
reproduction_LQR_infiniteHorizon
(
DataIn
,
model
,
r
,
currPos
,
rFactor
)
function
[
r
,
P
]
=
reproduction_LQR_infiniteHorizon
(
DataIn
,
model
,
r
,
currPos
,
rFactor
)
% Reproduction with a linear quadratic regulator of infinite horizon
%
% Authors: Sylvain Calinon and Danilo Bruno, 2014
...
...
@@ -32,14 +32,9 @@ R = eye(nbVarOut) * rFactor;
%Variables for feedforward term computation (optional for movements with low dynamics)
%tar = [r.currTar; gradient(r.currTar,1,2)/model.dt];
%dtar = gradient(tar,1,2)/model.dt;
tar
=
[
r
.
currTar
;
zeros
(
nbVarOut
,
nbData
)];
dtar
=
gradient
(
tar
,
1
,
2
)/
model
.
dt
;
%tar = [r.currTar; diff([r.currTar(:,1) r.currTar],1,2)/model.dt];
%dtar = diff([tar tar(:,1)],1,2)/model.dt;
%% Reproduction with varying impedance parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
...
...
@@ -51,16 +46,16 @@ for t=1:nbData
%care() is a function from the Matlab Control Toolbox to solve the algebraic Riccati equation,
%also available with the control toolbox of GNU Octave
S
=
care
(
A
,
B
,
(
Q
+
Q
')/2, R); %(Q+Q'
)/
2
is
used
instead
of
Q
to
avoid
warnings
for
the
symmetry
of
Q
P
=
care
(
A
,
B
,
(
Q
+
Q
')/2, R); %(Q+Q'
)/
2
is
used
instead
of
Q
to
avoid
warnings
for
the
symmetry
of
Q
%Alternatively the function below can be used for an implementation based on Schur decomposition.
%
S
= solveAlgebraicRiccati(A, B/R*B', (Q+Q')/2);
%
P
= solveAlgebraicRiccati(A, B/R*B', (Q+Q')/2);
%Variable for feedforward term computation (optional for movements with low dynamics)
d
=
inv
(
S
*
B
*
(
R
\
B
')-A'
)
*
(
S
*
dtar
(:,
t
)
-
S
*
A
*
tar
(:,
t
));
d
=
(
P
*
B
*
(
R
\
B
')-A'
)
\
(
P
*
dtar
(:,
t
)
-
P
*
A
*
tar
(:,
t
));
%Feedback term
L
=
R
\
B
'*
S
;
L
=
R
\
B
'*
P
;
%Feedforward term
M
=
R
\
B
'*
d
;
...
...
@@ -69,11 +64,7 @@ for t=1:nbData
%Compute acceleration (with feedback and feedforward terms)
ddx
=
-
L
*
[
x
-
r
.
currTar
(:,
t
);
dx
]
+
M
;
r
.
FB
(:,
t
)
=
-
L
*
[
x
-
r
.
currTar
(:,
t
);
dx
];
r
.
FF
(:,
t
)
=
M
;
%ddx = -L * ([x;dx]-tar(:,t)) + M;
%r.FB(:,t) = -L * ([x;dx]-tar(:,t));
%r.FB(:,t) = -L * [x-r.currTar(:,t); dx];
%r.FF(:,t) = M;
%Update velocity and position
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment