Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
pbdlibmatlab
Project overview
Project overview
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
rli
pbdlibmatlab
Commits
bc2e5a26
Commit
bc2e5a26
authored
Sep 18, 2014
by
Milad Malekzadeh
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Tab spacing (2 chars).
parent
68cb8b63
Changes
3
Hide whitespace changes
Inline
Sidebyside
Showing
3 changed files
with
90 additions
and
90 deletions
+90
90
README.md
README.md
+45
45
productTPGMM.m
productTPGMM.m
+12
12
reproduction_LQR_infiniteHorizon.m
reproduction_LQR_infiniteHorizon.m
+33
33
No files found.
README.md
View file @
bc2e5a26
# pbdlibmatlab
### Compatibility
The codes have been tested with both Matlab and GNU Octave.
### Usage
Unzip the file and run 'demo_TPGMR_LQR01' (finite horizon LQR), 'demo_TPGMR_LQR02' (infinite horizon LQR) or
'demo_DSGMR01' (dynamical system with constant gains) in Matlab.
'demo_testLQR01', 'demo_testLQR02' and 'demo_testLQR03' can also be run as additional examples of LQR.
### Reference
Calinon, S., Bruno, D. and Caldwell, D.G. (2014). A taskparameterized probabilistic model with minimal intervention
control. Proc. of the IEEE Intl Conf. on Robotics and Automation (ICRA).
### Description
Demonstration a taskparameterized probabilistic model encoding movements in the form of virtual springdamper systems
acting in multiple frames of reference. Each candidate coordinate system observes a set of demonstrations from its own
perspective, by extracting an attractor path whose variations depend on the relevance of the frame through the task.
This information is exploited to generate a new attractor path corresponding to new situations (new positions and
orientation of the frames), while the predicted covariances are exploited by a linear quadratic regulator (LQR) to
estimate the stiffness and damping feedback terms of the springdamper systems, resulting in a minimal intervention
control strategy.
### Authors
Sylvain Calinon and Danilo Bruno, 2014
http://programmingbydemonstration.org/
This source code is given for free! In exchange, we would be grateful if you cite the following reference in any
academic publication that uses this code or part of it:
@inproceedings{Calinon14ICRA,
author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
title="A taskparameterized probabilistic model with minimal intervention control",
booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
year="2014",
month="MayJune",
address="Hong Kong, China",
pages="33393344"
}
# pbdlibmatlab
### Compatibility
The codes have been tested with both Matlab and GNU Octave.
### Usage
Unzip the file and run 'demo_TPGMR_LQR01' (finite horizon LQR), 'demo_TPGMR_LQR02' (infinite horizon LQR) or
'demo_DSGMR01' (dynamical system with constant gains) in Matlab.
'demo_testLQR01', 'demo_testLQR02' and 'demo_testLQR03' can also be run as additional examples of LQR.
### Reference
Calinon, S., Bruno, D. and Caldwell, D.G. (2014). A taskparameterized probabilistic model with minimal intervention
control. Proc. of the IEEE Intl Conf. on Robotics and Automation (ICRA).
### Description
Demonstration a taskparameterized probabilistic model encoding movements in the form of virtual springdamper systems
acting in multiple frames of reference. Each candidate coordinate system observes a set of demonstrations from its own
perspective, by extracting an attractor path whose variations depend on the relevance of the frame through the task.
This information is exploited to generate a new attractor path corresponding to new situations (new positions and
orientation of the frames), while the predicted covariances are exploited by a linear quadratic regulator (LQR) to
estimate the stiffness and damping feedback terms of the springdamper systems, resulting in a minimal intervention
control strategy.
### Authors
Sylvain Calinon and Danilo Bruno, 2014
http://programmingbydemonstration.org/
This source code is given for free! In exchange, we would be grateful if you cite the following reference in any
academic publication that uses this code or part of it:
@inproceedings{Calinon14ICRA,
author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
title="A taskparameterized probabilistic model with minimal intervention control",
booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
year="2014",
month="MayJune",
address="Hong Kong, China",
pages="33393344"
}
productTPGMM.m
View file @
bc2e5a26
...
...
@@ 7,16 +7,16 @@ function [Mu, Sigma] = productTPGMM(model, p)
% TPGMM products
%See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
for
i
=
1
:
model
.
nbStates
% Reallocating
SigmaTmp
=
zeros
(
model
.
nbVar
);
MuTmp
=
zeros
(
model
.
nbVar
,
1
);
% Product of Gaussians
for
m
=
1
:
model
.
nbFrames
MuP
=
p
(
m
)
.
A
*
model
.
Mu
(:,
m
,
i
)
+
p
(
m
)
.
b
;
SigmaP
=
p
(
m
)
.
A
*
model
.
Sigma
(:,:,
m
,
i
)
*
p
(
m
)
.
A
'
;
SigmaTmp
=
SigmaTmp
+
inv
(
SigmaP
);
MuTmp
=
MuTmp
+
SigmaP
\
MuP
;
end
Sigma
(:,:,
i
)
=
inv
(
SigmaTmp
);
Mu
(:,
i
)
=
Sigma
(:,:,
i
)
*
MuTmp
;
% Reallocating
SigmaTmp
=
zeros
(
model
.
nbVar
);
MuTmp
=
zeros
(
model
.
nbVar
,
1
);
% Product of Gaussians
for
m
=
1
:
model
.
nbFrames
MuP
=
p
(
m
)
.
A
*
model
.
Mu
(:,
m
,
i
)
+
p
(
m
)
.
b
;
SigmaP
=
p
(
m
)
.
A
*
model
.
Sigma
(:,:,
m
,
i
)
*
p
(
m
)
.
A
'
;
SigmaTmp
=
SigmaTmp
+
inv
(
SigmaP
);
MuTmp
=
MuTmp
+
SigmaP
\
MuP
;
end
Sigma
(:,:,
i
)
=
inv
(
SigmaTmp
);
Mu
(:,
i
)
=
Sigma
(:,:,
i
)
*
MuTmp
;
end
reproduction_LQR_infiniteHorizon.m
View file @
bc2e5a26
...
...
@@ 35,49 +35,49 @@ R = eye(nbVarOut) * rFactor;
tar
=
[
r
.
currTar
;
zeros
(
nbVarOut
,
nbData
)];
dtar
=
gradient
(
tar
,
1
,
2
)/
model
.
dt
;
%% Reproduction with varying impedance parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x
=
currPos
;
dx
=
zeros
(
nbVarOut
,
1
);
for
t
=
1
:
nbData
%Current weighting term
Q
(
1
:
nbVarOut
,
1
:
nbVarOut
)
=
inv
(
r
.
currSigma
(:,:,
t
));
%care() is a function from the Matlab Control Toolbox to solve the algebraic Riccati equation,
Q
(
1
:
nbVarOut
,
1
:
nbVarOut
)
=
inv
(
r
.
currSigma
(:,:,
t
));
%care() is a function from the Matlab Control Toolbox to solve the algebraic Riccati equation,
%also available with the control toolbox of GNU Octave
%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
%P = solveAlgebraicRiccati_Schur(A, B/R*B', (Q+Q')/2);
%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
%P = solveAlgebraicRiccati_Schur(A, B/R*B', (Q+Q')/2);
%Alternatively, the function below can be used for an implementation based on Eigendecomposition
P
=
solveAlgebraicRiccati_eig
(
A
,
B
/
R
*
B
', (Q+Q'
)/
2
);
%See Eq. (5.1.27) and Sec. (5.2) in doc/TechnicalReport.pdf
%Variable for feedforward term computation (optional for movements with low dynamics)
d
=
(
P
*
B
*
(
R
\
B
')A'
)
\
(
P
*
dtar
(:,
t
)

P
*
A
*
tar
(:,
t
));
%See Eq. (5.1.28) in doc/TechnicalReport.pdf
%Feedback term
L
=
R
\
B
'*
P
;
%See Eq. (5.1.30) in doc/TechnicalReport.pdf
%Feedforward term
M
=
R
\
B
'*
d
;
%See Eq. (5.1.30) in doc/TechnicalReport.pdf
%Compute acceleration (with only feedback terms)
%ddx = L * [xr.currTar(:,t); dx];
%Compute acceleration (with feedback and feedforward terms)
ddx
=

L
*
[
x

r
.
currTar
(:,
t
);
dx
]
+
M
;
%See Eq. (5.1.30) in doc/TechnicalReport.pdf
r
.
FB
(:,
t
)
=

L
*
[
x

r
.
currTar
(:,
t
);
dx
];
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
));
r
.
kvDet
(
t
)
=
det
(
L
(:,
nbVarOut
+
1
:
end
));
%Variable for feedforward term computation (optional for movements with low dynamics)
d
=
(
P
*
B
*
(
R
\
B
')A'
)
\
(
P
*
dtar
(:,
t
)

P
*
A
*
tar
(:,
t
));
%See Eq. (5.1.28) in doc/TechnicalReport.pdf
%Feedback term
L
=
R
\
B
'*
P
;
%See Eq. (5.1.30) in doc/TechnicalReport.pdf
%Feedforward term
M
=
R
\
B
'*
d
;
%See Eq. (5.1.30) in doc/TechnicalReport.pdf
%Compute acceleration (with only feedback terms)
%ddx = L * [xr.currTar(:,t); dx];
%Compute acceleration (with feedback and feedforward terms)
ddx
=

L
*
[
x

r
.
currTar
(:,
t
);
dx
]
+
M
;
%See Eq. (5.1.30) in doc/TechnicalReport.pdf
r
.
FB
(:,
t
)
=

L
*
[
x

r
.
currTar
(:,
t
);
dx
];
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
));
r
.
kvDet
(
t
)
=
det
(
L
(:,
nbVarOut
+
1
:
end
));
end
...
...
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