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
0f3045f4
Commit
0f3045f4
authored
Sep 21, 2014
by
Milad Malekzadeh
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Tab2.
parent
37a729bd
Changes
2
Hide whitespace changes
Inline
Sidebyside
Showing
2 changed files
with
41 additions
and
41 deletions
+41
41
demo_DSGMR01.m
demo_DSGMR01.m
+22
22
demo_testLQR01.m
demo_testLQR01.m
+19
19
No files found.
demo_DSGMR01.m
View file @
0f3045f4
function
demo_DSGMR01
% 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
% 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).
%
% This demo presents the results for a dynamical system with constant gains.
%
%
% Author: Sylvain Calinon, 2014
% http://programmingbydemonstration.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 mth 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 mth 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 taskparameterized 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 DSGMR for new task parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp
(
'New reproductions with DSGMR...'
);
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 taskparameterized 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://programmingbydemonstration.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
=
1E2
;
%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://programmingbydemonstration.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
=
1E2
;
%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
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