Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
rli
pbdlib-matlab
Commits
0f3045f4
Commit
0f3045f4
authored
Sep 21, 2014
by
Milad Malekzadeh
Browse files
Tab2.
parent
37a729bd
Changes
2
Hide whitespace changes
Inline
Side-by-side
demo_DSGMR01.m
View file @
0f3045f4
function
demo_DSGMR01
% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper
% 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
% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper
% 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).
%
% This demo presents the results for a dynamical system with constant gains.
%
%
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
%
% This source code is given for free! In exchange, I would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
% This source code is given for free! In exchange, I 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.",
...
...
@@ -28,18 +28,18 @@ function demo_DSGMR01
model
.
nbStates
=
3
;
%Number of Gaussians in the GMM
model
.
nbFrames
=
2
;
%Number of candidate frames of reference
model
.
nbVar
=
3
;
%Dimension of the datapoints in the dataset (here: t,x1,x2)
model
.
dt
=
0.01
;
%Time step
model
.
kP
=
100
;
%Stiffness gain
model
.
kV
=
(
2
*
model
.
kP
)
^.
5
;
%Damping gain
model
.
dt
=
0.01
;
%Time step
model
.
kP
=
100
;
%Stiffness gain
model
.
kV
=
(
2
*
model
.
kP
)
^.
5
;
%Damping gain
nbRepros
=
8
;
%Number of reproductions with new situations randomly generated
%% Load 3rd order tensor data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp
(
'Load 3rd order tensor data...'
);
% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for
% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and
% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations
% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=3 the dimension of a
% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for
% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and
% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations
% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=3 the dimension of a
% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory (200)
% multiplied by the number of demonstrations (5).
load
(
'data/DataLQR01.mat'
);
...
...
@@ -60,7 +60,7 @@ for n=1:nbSamples
DataTmp
=
s
(
n
)
.
Data0
(
2
:
end
,:);
DataTmp
=
[
s
(
n
)
.
Data0
(
1
,:);
K
*
[
DataTmp
;
DataTmp
*
D
;
DataTmp
*
D
*
D
]];
for
m
=
1
:
model
.
nbFrames
Data
(:,
m
,(
n
-
1
)
*
nbD
+
1
:
n
*
nbD
)
=
s
(
n
)
.
p
(
m
)
.
A
\
(
DataTmp
-
repmat
(
s
(
n
)
.
p
(
m
)
.
b
,
1
,
nbD
));
Data
(:,
m
,(
n
-
1
)
*
nbD
+
1
:
n
*
nbD
)
=
s
(
n
)
.
p
(
m
)
.
A
\
(
DataTmp
-
repmat
(
s
(
n
)
.
p
(
m
)
.
b
,
1
,
nbD
));
end
end
...
...
@@ -68,7 +68,7 @@ end
%% Tensor GMM learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf
(
'Parameters estimation of tensor GMM with EM:'
);
model
=
init_tensorGMM_timeBased
(
Data
,
model
);
%Initialization
model
=
init_tensorGMM_timeBased
(
Data
,
model
);
%Initialization
model
=
EM_tensorGMM
(
Data
,
model
);
...
...
@@ -79,16 +79,16 @@ DataIn = [1:s(1).nbData] * model.dt;
for
n
=
1
:
nbSamples
%Retrieval of attractor path through task-parameterized GMR
a
(
n
)
=
estimateAttractorPath
(
DataIn
,
model
,
s
(
n
));
r
(
n
)
=
reproduction_DS
(
DataIn
,
model
,
a
(
n
),
a
(
n
)
.
currTar
(:,
1
));
r
(
n
)
=
reproduction_DS
(
DataIn
,
model
,
a
(
n
),
a
(
n
)
.
currTar
(:,
1
));
end
%% Reproduction with DS-GMR for new task parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp
(
'New reproductions with DS-GMR...'
);
for
n
=
1
:
nbRepros
for
n
=
1
:
nbRepros
for
m
=
1
:
model
.
nbFrames
%Random generation of new task parameters
%Random generation of new task parameters
id
=
ceil
(
rand
(
2
,
1
)
*
nbSamples
);
w
=
rand
(
2
);
w
=
w
/
sum
(
w
);
rTmp
.
p
(
m
)
.
b
=
s
(
id
(
1
))
.
p
(
m
)
.
b
*
w
(
1
)
+
s
(
id
(
2
))
.
p
(
m
)
.
b
*
w
(
2
);
...
...
@@ -96,7 +96,7 @@ for n=1:nbRepros
end
%Retrieval of attractor path through task-parameterized GMR
anew
(
n
)
=
estimateAttractorPath
(
DataIn
,
model
,
rTmp
);
rnew
(
n
)
=
reproduction_DS
(
DataIn
,
model
,
anew
(
n
),
anew
(
n
)
.
currTar
(:,
1
));
rnew
(
n
)
=
reproduction_DS
(
DataIn
,
model
,
anew
(
n
),
anew
(
n
)
.
currTar
(:,
1
));
end
...
...
@@ -161,8 +161,8 @@ axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
%print('-dpng','outTest1.png');
%Plot additional information
figure
;
%Plot additional information
figure
;
%Plot norm of control commands
subplot
(
1
,
2
,
1
);
hold
on
;
for
n
=
1
:
nbRepros
...
...
demo_testLQR01.m
View file @
0f3045f4
...
...
@@ -4,8 +4,8 @@ function demo_testLQR01
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
%
% This source code is given for free! In exchange, I would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
% This source code is given for free! In exchange, I 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.",
...
...
@@ -20,16 +20,16 @@ function demo_testLQR01
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model
.
nbVar
=
2
;
%Dimension of the datapoints in the dataset (here: t,x1)
model
.
dt
=
0.01
;
%Time step
model
.
dt
=
0.01
;
%Time step
nbData
=
500
;
%Number of datapoints
nbRepros
=
3
;
%Number of reproductions with new situations randomly generated
rFactor
=
1E-2
;
%Weighting term for the minimization of control commands in LQR
%% Reproduction with LQR
%% Reproduction with LQR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp
(
'Reproductions with LQR...'
);
DataIn
=
[
1
:
nbData
]
*
model
.
dt
;
a
.
currTar
=
ones
(
1
,
nbData
);
a
.
currTar
=
ones
(
1
,
nbData
);
for
n
=
1
:
nbRepros
a
.
currSigma
=
ones
(
1
,
1
,
nbData
)
*
10
^
(
2
-
n
);
%r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor);
...
...
@@ -48,7 +48,7 @@ for n=1:nbRepros
end
xlabel
(
't'
);
ylabel
(
'x_1'
);
figure
;
figure
;
%Plot norm of control commands
subplot
(
1
,
2
,
1
);
hold
on
;
for
n
=
1
:
nbRepros
...
...
@@ -70,8 +70,8 @@ function demo_testLQR01
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
%
% This source code is given for free! In exchange, I would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
% This source code is given for free! In exchange, I 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.",
...
...
@@ -86,20 +86,20 @@ function demo_testLQR01
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model
.
nbVar
=
2
;
%Dimension of the datapoints in the dataset (here: t,x1)
model
.
dt
=
0.01
;
%Time step
model
.
dt
=
0.01
;
%Time step
nbData
=
500
;
%Number of datapoints
nbRepros
=
3
;
%Number of reproductions with new situations randomly generated
rFactor
=
1E-2
;
%Weighting term for the minimization of control commands in LQR
%% Reproduction with LQR
%% Reproduction with LQR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp
(
'Reproductions with LQR...'
);
DataIn
=
[
1
:
nbData
]
*
model
.
dt
;
a
.
currTar
=
ones
(
1
,
nbData
);
a
.
currTar
=
ones
(
1
,
nbData
);
for
n
=
1
:
nbRepros
a
.
currSigma
=
ones
(
1
,
1
,
nbData
)
*
10
^
(
2
-
n
);
%r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor);
r
(
n
)
=
reproduction_LQR_infiniteHorizon
(
DataIn
,
model
,
a
,
0
,
rFactor
);
a
.
currSigma
=
ones
(
1
,
1
,
nbData
)
*
10
^
(
2
-
n
);
%r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor);
r
(
n
)
=
reproduction_LQR_infiniteHorizon
(
DataIn
,
model
,
a
,
0
,
rFactor
);
end
%% Plots
...
...
@@ -109,22 +109,22 @@ hold on; box on;
%Plot target
plot
(
r
(
1
)
.
Data
(
1
,:),
a
.
currTar
,
'r-'
,
'linewidth'
,
2
);
for
n
=
1
:
nbRepros
%Plot trajectories
plot
(
r
(
n
)
.
Data
(
1
,:),
r
(
n
)
.
Data
(
2
,:),
'-'
,
'linewidth'
,
2
,
'color'
,
ones
(
3
,
1
)
*
(
n
-
1
)/
nbRepros
);
%Plot trajectories
plot
(
r
(
n
)
.
Data
(
1
,:),
r
(
n
)
.
Data
(
2
,:),
'-'
,
'linewidth'
,
2
,
'color'
,
ones
(
3
,
1
)
*
(
n
-
1
)/
nbRepros
);
end
xlabel
(
't'
);
ylabel
(
'x_1'
);
figure
;
figure
;
%Plot norm of control commands
subplot
(
1
,
2
,
1
);
hold
on
;
for
n
=
1
:
nbRepros
plot
(
DataIn
,
r
(
n
)
.
ddxNorm
,
'-'
,
'linewidth'
,
2
,
'color'
,
ones
(
3
,
1
)
*
(
n
-
1
)/
nbRepros
);
plot
(
DataIn
,
r
(
n
)
.
ddxNorm
,
'-'
,
'linewidth'
,
2
,
'color'
,
ones
(
3
,
1
)
*
(
n
-
1
)/
nbRepros
);
end
xlabel
(
't'
);
ylabel
(
'|ddx|'
);
%Plot stiffness
subplot
(
1
,
2
,
2
);
hold
on
;
for
n
=
1
:
nbRepros
plot
(
DataIn
,
r
(
n
)
.
kpDet
,
'-'
,
'linewidth'
,
2
,
'color'
,
ones
(
3
,
1
)
*
(
n
-
1
)/
nbRepros
);
plot
(
DataIn
,
r
(
n
)
.
kpDet
,
'-'
,
'linewidth'
,
2
,
'color'
,
ones
(
3
,
1
)
*
(
n
-
1
)/
nbRepros
);
end
xlabel
(
't'
);
ylabel
(
'kp'
);
...
...
Write
Preview
Supports
Markdown
0%
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!
Cancel
Please
register
or
sign in
to comment