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
191b8988
Commit
191b8988
authored
Aug 06, 2014
by
Sylvain Calinon
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Division of demo01.m into 3 distinct examples
parent
6e3eeead
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
with
375 additions
and
41 deletions
+375
-41
README.md
README.md
+2
-2
demo_DSGMR01.m
demo_DSGMR01.m
+193
-0
demo_TPGMR_LQR01.m
demo_TPGMR_LQR01.m
+173
-0
demo_TPGMR_LQR02.m
demo_TPGMR_LQR02.m
+7
-39
No files found.
README.md
View file @
191b8988
...
@@ -6,8 +6,8 @@
...
@@ -6,8 +6,8 @@
### Usage
### Usage
Unzip the file and run 'demo
01' in Matlab. Several reproduction algorithms can be selected by commenting/uncommenting
Unzip the file and run 'demo
_TPGMR_LQR01' (finite horizon LQR), 'demo_TPGMR_LQR02' (infinite horizon LQR) or
lines 89-91 and 110-112 in demo01.m (finite/infinite horizon LQR or dynamical system with constant gains).
'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.
'demo_testLQR01', 'demo_testLQR02' and 'demo_testLQR03' can also be run as additional examples of LQR.
### Reference
### Reference
...
...
demo_DSGMR01.m
0 → 100644
View file @
191b8988
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
% 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:
%
% @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
% title="A task-parameterized probabilistic model with minimal intervention control",
% booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
% year="2014",
% month="May-June",
% address="Hong Kong, China",
% pages="3339--3344"
% }
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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 (required only if LQR is not used for reproduction)
model
.
kV
=
(
2
*
model
.
kP
)
^.
5
;
%Damping gain (required only if LQR is not used for reproduction)
nbRepros
=
8
;
%Number of reproductions with new situations randomly generated
rFactor
=
1E-1
;
%Weighting term for the minimization of control commands in LQR
%% 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
% 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'
);
%% Transformation of 'Data' to learn the path of the spring-damper system
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbD
=
s
(
1
)
.
nbData
;
nbVarOut
=
model
.
nbVar
-
1
;
%Create transformation matrix to compute [X; DX; DDX]
D
=
(
diag
(
ones
(
1
,
nbD
-
1
),
-
1
)
-
eye
(
nbD
))
/
model
.
dt
;
D
(
end
,
end
)
=
0
;
%Create transformation matrix to compute XHAT = X + DX*kV/kP + DDX/kP
K1d
=
[
1
,
model
.
kV
/
model
.
kP
,
1
/
model
.
kP
];
K
=
kron
(
K1d
,
eye
(
nbVarOut
));
%Create 3rd order tensor data with XHAT instead of X
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
));
end
end
%% Tensor GMM learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf
(
'Parameters estimation of tensor GMM with EM:'
);
model
=
init_tensorGMM_timeBased
(
Data
,
model
);
%Initialization
model
=
EM_tensorGMM
(
Data
,
model
);
%% Reproduction with LQR for the task parameters used to train the model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp
(
'Reproductions with LQR...'
);
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
));
end
%% Reproduction with LQR for new task parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp
(
'New reproductions with LQR...'
);
for
n
=
1
:
nbRepros
for
m
=
1
:
model
.
nbFrames
%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
);
rTmp
.
p
(
m
)
.
A
=
s
(
id
(
1
))
.
p
(
m
)
.
A
*
w
(
1
)
+
s
(
id
(
2
))
.
p
(
m
)
.
A
*
w
(
2
);
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
));
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
(
'position'
,[
20
,
50
,
1300
,
500
]);
xx
=
round
(
linspace
(
1
,
64
,
nbSamples
));
clrmap
=
colormap
(
'jet'
);
clrmap
=
min
(
clrmap
(
xx
,:),
.
95
);
limAxes
=
[
-
1.2
0.8
-
1.1
0.9
];
colPegs
=
[[
.
9
,
.
5
,
.
9
];[
.
5
,
.
9
,
.
5
]];
%DEMOS
subplot
(
1
,
3
,
1
);
hold
on
;
box
on
;
title
(
'Demonstrations'
);
for
n
=
1
:
nbSamples
%Plot frames
for
m
=
1
:
model
.
nbFrames
plot
([
s
(
n
)
.
p
(
m
)
.
b
(
2
)
s
(
n
)
.
p
(
m
)
.
b
(
2
)
+
s
(
n
)
.
p
(
m
)
.
A
(
2
,
3
)],
[
s
(
n
)
.
p
(
m
)
.
b
(
3
)
s
(
n
)
.
p
(
m
)
.
b
(
3
)
+
s
(
n
)
.
p
(
m
)
.
A
(
3
,
3
)],
'-'
,
'linewidth'
,
6
,
'color'
,
colPegs
(
m
,:));
plot
(
s
(
n
)
.
p
(
m
)
.
b
(
2
),
s
(
n
)
.
p
(
m
)
.
b
(
3
),
'.'
,
'markersize'
,
30
,
'color'
,
colPegs
(
m
,:)
-
[
.
05
,
.
05
,
.
05
]);
end
%Plot trajectories
plot
(
s
(
n
)
.
Data0
(
2
,
1
),
s
(
n
)
.
Data0
(
3
,
1
),
'.'
,
'markersize'
,
12
,
'color'
,
clrmap
(
n
,:));
plot
(
s
(
n
)
.
Data0
(
2
,:),
s
(
n
)
.
Data0
(
3
,:),
'-'
,
'linewidth'
,
1.5
,
'color'
,
clrmap
(
n
,:));
end
axis
(
limAxes
);
axis
square
;
set
(
gca
,
'xtick'
,[],
'ytick'
,[]);
%REPROS
subplot
(
1
,
3
,
2
);
hold
on
;
box
on
;
title
(
'Reproductions with DS-GMR'
);
for
n
=
1
:
nbSamples
%Plot frames
for
m
=
1
:
model
.
nbFrames
plot
([
s
(
n
)
.
p
(
m
)
.
b
(
2
)
s
(
n
)
.
p
(
m
)
.
b
(
2
)
+
s
(
n
)
.
p
(
m
)
.
A
(
2
,
3
)],
[
s
(
n
)
.
p
(
m
)
.
b
(
3
)
s
(
n
)
.
p
(
m
)
.
b
(
3
)
+
s
(
n
)
.
p
(
m
)
.
A
(
3
,
3
)],
'-'
,
'linewidth'
,
6
,
'color'
,
colPegs
(
m
,:));
plot
(
s
(
n
)
.
p
(
m
)
.
b
(
2
),
s
(
n
)
.
p
(
m
)
.
b
(
3
),
'.'
,
'markersize'
,
30
,
'color'
,
colPegs
(
m
,:)
-
[
.
05
,
.
05
,
.
05
]);
end
%Plot Gaussians
plotGMM
(
r
(
n
)
.
Mu
(
2
:
3
,:,
1
),
r
(
n
)
.
Sigma
(
2
:
3
,
2
:
3
,:,
1
),
[
.
7
.
7
.
7
]);
end
for
n
=
1
:
nbSamples
%Plot trajectories
plot
(
r
(
n
)
.
Data
(
2
,
1
),
r
(
n
)
.
Data
(
3
,
1
),
'.'
,
'markersize'
,
12
,
'color'
,
clrmap
(
n
,:));
plot
(
r
(
n
)
.
Data
(
2
,:),
r
(
n
)
.
Data
(
3
,:),
'-'
,
'linewidth'
,
1.5
,
'color'
,
clrmap
(
n
,:));
end
axis
(
limAxes
);
axis
square
;
set
(
gca
,
'xtick'
,[],
'ytick'
,[]);
%NEW REPROS
subplot
(
1
,
3
,
3
);
hold
on
;
box
on
;
title
(
'New reproductions with DS-GMR'
);
for
n
=
1
:
nbRepros
%Plot frames
for
m
=
1
:
model
.
nbFrames
plot
([
rnew
(
n
)
.
p
(
m
)
.
b
(
2
)
rnew
(
n
)
.
p
(
m
)
.
b
(
2
)
+
rnew
(
n
)
.
p
(
m
)
.
A
(
2
,
3
)],
[
rnew
(
n
)
.
p
(
m
)
.
b
(
3
)
rnew
(
n
)
.
p
(
m
)
.
b
(
3
)
+
rnew
(
n
)
.
p
(
m
)
.
A
(
3
,
3
)],
'-'
,
'linewidth'
,
6
,
'color'
,
colPegs
(
m
,:));
plot
(
rnew
(
n
)
.
p
(
m
)
.
b
(
2
),
rnew
(
n
)
.
p
(
m
)
.
b
(
3
),
'.'
,
'markersize'
,
30
,
'color'
,
colPegs
(
m
,:)
-
[
.
05
,
.
05
,
.
05
]);
end
%Plot Gaussians
plotGMM
(
rnew
(
n
)
.
Mu
(
2
:
3
,:,
1
),
rnew
(
n
)
.
Sigma
(
2
:
3
,
2
:
3
,:,
1
),
[
.
7
.
7
.
7
]);
end
for
n
=
1
:
nbRepros
%Plot trajectories
plot
(
rnew
(
n
)
.
Data
(
2
,
1
),
rnew
(
n
)
.
Data
(
3
,
1
),
'.'
,
'markersize'
,
12
,
'color'
,[
.
2
.
2
.
2
]);
plot
(
rnew
(
n
)
.
Data
(
2
,:),
rnew
(
n
)
.
Data
(
3
,:),
'-'
,
'linewidth'
,
1.5
,
'color'
,[
.
2
.
2
.
2
]);
end
axis
(
limAxes
);
axis
square
;
set
(
gca
,
'xtick'
,[],
'ytick'
,[]);
%print('-dpng','outTest1.png');
%Plot additional information
figure
;
%Plot norm of control commands
subplot
(
1
,
2
,
1
);
hold
on
;
for
n
=
1
:
nbRepros
plot
(
DataIn
,
rnew
(
n
)
.
ddxNorm
,
'k-'
,
'linewidth'
,
2
);
end
xlabel
(
't'
);
ylabel
(
'|ddx|'
);
%Plot strength of the stiffness term
subplot
(
1
,
2
,
2
);
hold
on
;
for
n
=
1
:
nbRepros
plot
(
DataIn
,
rnew
(
n
)
.
kpDet
,
'k-'
,
'linewidth'
,
2
);
end
xlabel
(
't'
);
ylabel
(
'|Kp|'
);
%Plot accelerations due to feedback and feedforward terms
figure
;
hold
on
;
n
=
1
;
k
=
1
;
plot
(
r
(
n
)
.
FB
(
k
,:),
'r-'
,
'linewidth'
,
2
);
plot
(
r
(
n
)
.
FF
(
k
,:),
'b-'
,
'linewidth'
,
2
);
legend
(
'ddx feedback'
,
'ddx feedforward'
);
xlabel
(
't'
);
ylabel
([
'ddx_'
num2str
(
k
)]);
%print('-dpng','outTest2.png');
%pause;
%close all;
demo_TPGMR_LQR01.m
0 → 100644
View file @
191b8988
function
demo_TPGMR_LQR01
% 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), while the predicted covariances
% are exploited by a linear quadratic regulator (LQR) to estimate the stiffness and damping feedback terms of
% the spring-damper systems, resulting in a minimal intervention control strategy.
%
% This demo presents the results for a finite horizon LQR.
%
% 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:
%
% @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
% title="A task-parameterized probabilistic model with minimal intervention control",
% booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
% year="2014",
% month="May-June",
% address="Hong Kong, China",
% pages="3339--3344"
% }
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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
nbRepros
=
8
;
%Number of reproductions with new situations randomly generated
rFactor
=
1E-1
;
%Weighting term for the minimization of control commands in LQR
%% 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
% 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'
);
%% Tensor GMM learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf
(
'Parameters estimation of tensor GMM with EM:'
);
model
=
init_tensorGMM_timeBased
(
Data
,
model
);
%Initialization
model
=
EM_tensorGMM
(
Data
,
model
);
%% Reproduction with LQR for the task parameters used to train the model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp
(
'Reproductions with LQR...'
);
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_LQR_finiteHorizon
(
DataIn
,
model
,
a
(
n
),
a
(
n
)
.
currTar
(:,
1
),
rFactor
);
end
%% Reproduction with LQR for new task parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp
(
'New reproductions with LQR...'
);
for
n
=
1
:
nbRepros
for
m
=
1
:
model
.
nbFrames
%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
);
rTmp
.
p
(
m
)
.
A
=
s
(
id
(
1
))
.
p
(
m
)
.
A
*
w
(
1
)
+
s
(
id
(
2
))
.
p
(
m
)
.
A
*
w
(
2
);
end
%Retrieval of attractor path through task-parameterized GMR
anew
(
n
)
=
estimateAttractorPath
(
DataIn
,
model
,
rTmp
);
rnew
(
n
)
=
reproduction_LQR_finiteHorizon
(
DataIn
,
model
,
anew
(
n
),
anew
(
n
)
.
currTar
(:,
1
),
rFactor
);
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
(
'position'
,[
20
,
50
,
1300
,
500
]);
xx
=
round
(
linspace
(
1
,
64
,
nbSamples
));
clrmap
=
colormap
(
'jet'
);
clrmap
=
min
(
clrmap
(
xx
,:),
.
95
);
limAxes
=
[
-
1.2
0.8
-
1.1
0.9
];
colPegs
=
[[
.
9
,
.
5
,
.
9
];[
.
5
,
.
9
,
.
5
]];
%DEMOS
subplot
(
1
,
3
,
1
);
hold
on
;
box
on
;
title
(
'Demonstrations'
);
for
n
=
1
:
nbSamples
%Plot frames
for
m
=
1
:
model
.
nbFrames
plot
([
s
(
n
)
.
p
(
m
)
.
b
(
2
)
s
(
n
)
.
p
(
m
)
.
b
(
2
)
+
s
(
n
)
.
p
(
m
)
.
A
(
2
,
3
)],
[
s
(
n
)
.
p
(
m
)
.
b
(
3
)
s
(
n
)
.
p
(
m
)
.
b
(
3
)
+
s
(
n
)
.
p
(
m
)
.
A
(
3
,
3
)],
'-'
,
'linewidth'
,
6
,
'color'
,
colPegs
(
m
,:));
plot
(
s
(
n
)
.
p
(
m
)
.
b
(
2
),
s
(
n
)
.
p
(
m
)
.
b
(
3
),
'.'
,
'markersize'
,
30
,
'color'
,
colPegs
(
m
,:)
-
[
.
05
,
.
05
,
.
05
]);
end
%Plot trajectories
plot
(
s
(
n
)
.
Data0
(
2
,
1
),
s
(
n
)
.
Data0
(
3
,
1
),
'.'
,
'markersize'
,
12
,
'color'
,
clrmap
(
n
,:));
plot
(
s
(
n
)
.
Data0
(
2
,:),
s
(
n
)
.
Data0
(
3
,:),
'-'
,
'linewidth'
,
1.5
,
'color'
,
clrmap
(
n
,:));
end
axis
(
limAxes
);
axis
square
;
set
(
gca
,
'xtick'
,[],
'ytick'
,[]);
%REPROS
subplot
(
1
,
3
,
2
);
hold
on
;
box
on
;
title
(
'Reproductions with finite horizon LQR'
);
for
n
=
1
:
nbSamples
%Plot frames
for
m
=
1
:
model
.
nbFrames
plot
([
s
(
n
)
.
p
(
m
)
.
b
(
2
)
s
(
n
)
.
p
(
m
)
.
b
(
2
)
+
s
(
n
)
.
p
(
m
)
.
A
(
2
,
3
)],
[
s
(
n
)
.
p
(
m
)
.
b
(
3
)
s
(
n
)
.
p
(
m
)
.
b
(
3
)
+
s
(
n
)
.
p
(
m
)
.
A
(
3
,
3
)],
'-'
,
'linewidth'
,
6
,
'color'
,
colPegs
(
m
,:));
plot
(
s
(
n
)
.
p
(
m
)
.
b
(
2
),
s
(
n
)
.
p
(
m
)
.
b
(
3
),
'.'
,
'markersize'
,
30
,
'color'
,
colPegs
(
m
,:)
-
[
.
05
,
.
05
,
.
05
]);
end
%Plot Gaussians
plotGMM
(
r
(
n
)
.
Mu
(
2
:
3
,:,
1
),
r
(
n
)
.
Sigma
(
2
:
3
,
2
:
3
,:,
1
),
[
.
7
.
7
.
7
]);
end
for
n
=
1
:
nbSamples
%Plot trajectories
plot
(
r
(
n
)
.
Data
(
2
,
1
),
r
(
n
)
.
Data
(
3
,
1
),
'.'
,
'markersize'
,
12
,
'color'
,
clrmap
(
n
,:));
plot
(
r
(
n
)
.
Data
(
2
,:),
r
(
n
)
.
Data
(
3
,:),
'-'
,
'linewidth'
,
1.5
,
'color'
,
clrmap
(
n
,:));
end
axis
(
limAxes
);
axis
square
;
set
(
gca
,
'xtick'
,[],
'ytick'
,[]);
%NEW REPROS
subplot
(
1
,
3
,
3
);
hold
on
;
box
on
;
title
(
'New reproductions with finite horizon LQR'
);
for
n
=
1
:
nbRepros
%Plot frames
for
m
=
1
:
model
.
nbFrames
plot
([
rnew
(
n
)
.
p
(
m
)
.
b
(
2
)
rnew
(
n
)
.
p
(
m
)
.
b
(
2
)
+
rnew
(
n
)
.
p
(
m
)
.
A
(
2
,
3
)],
[
rnew
(
n
)
.
p
(
m
)
.
b
(
3
)
rnew
(
n
)
.
p
(
m
)
.
b
(
3
)
+
rnew
(
n
)
.
p
(
m
)
.
A
(
3
,
3
)],
'-'
,
'linewidth'
,
6
,
'color'
,
colPegs
(
m
,:));
plot
(
rnew
(
n
)
.
p
(
m
)
.
b
(
2
),
rnew
(
n
)
.
p
(
m
)
.
b
(
3
),
'.'
,
'markersize'
,
30
,
'color'
,
colPegs
(
m
,:)
-
[
.
05
,
.
05
,
.
05
]);
end
%Plot Gaussians
plotGMM
(
rnew
(
n
)
.
Mu
(
2
:
3
,:,
1
),
rnew
(
n
)
.
Sigma
(
2
:
3
,
2
:
3
,:,
1
),
[
.
7
.
7
.
7
]);
end
for
n
=
1
:
nbRepros
%Plot trajectories
plot
(
rnew
(
n
)
.
Data
(
2
,
1
),
rnew
(
n
)
.
Data
(
3
,
1
),
'.'
,
'markersize'
,
12
,
'color'
,[
.
2
.
2
.
2
]);
plot
(
rnew
(
n
)
.
Data
(
2
,:),
rnew
(
n
)
.
Data
(
3
,:),
'-'
,
'linewidth'
,
1.5
,
'color'
,[
.
2
.
2
.
2
]);
end
axis
(
limAxes
);
axis
square
;
set
(
gca
,
'xtick'
,[],
'ytick'
,[]);
%print('-dpng','outTest1.png');
%Plot additional information
figure
;
%Plot norm of control commands
subplot
(
1
,
2
,
1
);
hold
on
;
for
n
=
1
:
nbRepros
plot
(
DataIn
,
rnew
(
n
)
.
ddxNorm
,
'k-'
,
'linewidth'
,
2
);
end
xlabel
(
't'
);
ylabel
(
'|ddx|'
);
%Plot strength of the stiffness term
subplot
(
1
,
2
,
2
);
hold
on
;
for
n
=
1
:
nbRepros
plot
(
DataIn
,
rnew
(
n
)
.
kpDet
,
'k-'
,
'linewidth'
,
2
);
end
xlabel
(
't'
);
ylabel
(
'|Kp|'
);
%Plot accelerations due to feedback and feedforward terms
figure
;
hold
on
;
n
=
1
;
k
=
1
;
plot
(
r
(
n
)
.
FB
(
k
,:),
'r-'
,
'linewidth'
,
2
);
plot
(
r
(
n
)
.
FF
(
k
,:),
'b-'
,
'linewidth'
,
2
);
legend
(
'ddx feedback'
,
'ddx feedforward'
);
xlabel
(
't'
);
ylabel
([
'ddx_'
num2str
(
k
)]);
%print('-dpng','outTest2.png');
%pause;
%close all;
demo
01
.m
→
demo
_TPGMR_LQR02
.m
View file @
191b8988
function
demo
01
function
demo
_TPGMR_LQR02
% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper
% 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
% 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
% demonstrations from its own perspective, by extracting an attractor path whose variations depend on the
...
@@ -7,8 +7,7 @@ function demo01
...
@@ -7,8 +7,7 @@ function demo01
% are exploited by a linear quadratic regulator (LQR) to estimate the stiffness and damping feedback terms of
% are exploited by a linear quadratic regulator (LQR) to estimate the stiffness and damping feedback terms of
% the spring-damper systems, resulting in a minimal intervention control strategy.
% the spring-damper systems, resulting in a minimal intervention control strategy.
%
%
% Several reproduction algorithms can be selected by commenting/uncommenting lines 89-91 and 110-112
% This demo presents the results for an infinite horizon LQR.
% (finite/infinite horizon LQR or dynamical system with constant gains).
%
%
% Author: Sylvain Calinon, 2014
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
% http://programming-by-demonstration.org/SylvainCalinon
...
@@ -48,28 +47,6 @@ disp('Load 3rd order tensor data...');
...
@@ -48,28 +47,6 @@ disp('Load 3rd order tensor data...');
load
(
'data/DataLQR01.mat'
);
load
(
'data/DataLQR01.mat'
);
% %% Optional recomputation of 'Data' (only required when using reproduction_DS)
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% model.kP = 100; %Stiffness gain (required only if LQR is not used for reproduction)
% model.kV = (2*model.kP)^.5; %Damping gain (required only if LQR is not used for reproduction)
% nbD = s(1).nbData;
% nbVarOut = model.nbVar - 1;
% %Create transformation matrix to compute [X; DX; DDX]
% D = (diag(ones(1,nbD-1),-1)-eye(nbD)) / model.dt;
% D(end,end) = 0;
% %Create transformation matrix to compute XHAT = X + DX*kV/kP + DDX/kP
% K1d = [1, model.kV/model.kP, 1/model.kP];
% K = kron(K1d,eye(nbVarOut));
% %Create 3rd order tensor data with XHAT instead of X
% 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));
% end
% end
%% Tensor GMM learning
%% Tensor GMM learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf
(
'Parameters estimation of tensor GMM with EM:'
);
fprintf
(
'Parameters estimation of tensor GMM with EM:'
);
...
@@ -83,12 +60,8 @@ disp('Reproductions with LQR...');
...
@@ -83,12 +60,8 @@ disp('Reproductions with LQR...');
DataIn
=
[
1
:
s
(
1
)
.
nbData
]
*
model
.
dt
;
DataIn
=
[
1
:
s
(
1
)
.
nbData
]
*
model
.
dt
;
for
n
=
1
:
nbSamples
for
n
=
1
:
nbSamples
%Retrieval of attractor path through task-parameterized GMR
%Retrieval of attractor path through task-parameterized GMR
a
(
n
)
=
estimateAttractorPath
(
DataIn
,
model
,
s
(
n
));
a
(
n
)
=
estimateAttractorPath
(
DataIn
,
model
,
s
(
n
));
%Reproduction with one of the selected approach
%r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
r
(
n
)
=
reproduction_LQR_infiniteHorizon
(
DataIn
,
model
,
a
(
n
),
a
(
n
)
.
currTar
(:,
1
),
rFactor
);
r
(
n
)
=
reproduction_LQR_infiniteHorizon
(
DataIn
,
model
,
a
(
n
),
a
(
n
)
.
currTar
(:,
1
),
rFactor
);
%r(n) = reproduction_DS(DataIn, model, a(n), a(n).currTar(:,1)); %This function requires to define model.kP and model.kV (see lines 38-39)
end
end
...
@@ -105,11 +78,7 @@ for n=1:nbRepros
...
@@ -105,11 +78,7 @@ for n=1:nbRepros
end
end
%Retrieval of attractor path through task-parameterized GMR
%Retrieval of attractor path through task-parameterized GMR
anew
(
n
)
=
estimateAttractorPath
(
DataIn
,
model
,
rTmp
);
anew
(
n
)
=
estimateAttractorPath
(
DataIn
,
model
,
rTmp
);
%Reproduction with one of the selected approach
%rnew(n) = reproduction_LQR_finiteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
rnew
(
n
)
=
reproduction_LQR_infiniteHorizon
(
DataIn
,
model
,
anew
(
n
),
anew
(
n
)
.
currTar
(:,
1
),
rFactor
);
rnew
(
n
)
=
reproduction_LQR_infiniteHorizon
(
DataIn
,
model
,
anew
(
n
),
anew
(
n
)
.
currTar
(:,
1
),
rFactor
);
%rnew(n) = reproduction_DS(DataIn, model, anew(n), anew(n).currTar(:,1)); %The fct requires to define model.kP and model.kV (see lines 38-39)
end
end
...
@@ -137,7 +106,7 @@ end
...
@@ -137,7 +106,7 @@ end
axis
(
limAxes
);
axis
square
;
set
(
gca
,
'xtick'
,[],
'ytick'
,[]);
axis
(
limAxes
);
axis
square
;
set
(
gca
,
'xtick'
,[],
'ytick'
,[]);
%REPROS
%REPROS
subplot
(
1
,
3
,
2
);
hold
on
;
box
on
;
title
(
'Reproductions with LQR'
);
subplot
(
1
,
3
,
2
);
hold
on
;
box
on
;
title
(
'Reproductions with
infinite horizon
LQR'
);
for
n
=
1
:
nbSamples
for
n
=
1
:
nbSamples
%Plot frames
%Plot frames
for
m
=
1
:
model
.
nbFrames
for
m
=
1
:
model
.
nbFrames
...
@@ -155,7 +124,7 @@ end
...
@@ -155,7 +124,7 @@ end
axis
(
limAxes
);
axis
square
;
set
(
gca
,
'xtick'
,[],
'ytick'
,[]);
axis
(
limAxes
);
axis
square
;
set
(
gca
,
'xtick'
,[],
'ytick'
,[]);
%NEW REPROS
%NEW REPROS
subplot
(
1
,
3
,
3
);
hold
on
;
box
on
;
title
(
'New reproductions with LQR'
);
subplot
(
1
,
3
,
3
);
hold
on
;
box
on
;
title
(
'New reproductions with
infinite horizon
LQR'
);
for
n
=
1
:
nbRepros
for
n
=
1
:
nbRepros
%Plot frames
%Plot frames
for
m
=
1
:
model
.
nbFrames
for
m
=
1
:
model
.
nbFrames
...
@@ -172,7 +141,7 @@ for n=1:nbRepros
...
@@ -172,7 +141,7 @@ for n=1:nbRepros
end
end
axis
(
limAxes
);
axis
square
;
set
(
gca
,
'xtick'
,[],
'ytick'
,[]);
axis
(
limAxes
);
axis
square
;
set
(
gca
,
'xtick'
,[],
'ytick'
,[]);
print
(
'-dpng'
,
'outTest1.png'
);
%
print('-dpng','outTest1.png');
%Plot additional information
%Plot additional information
figure
;
figure
;
...
@@ -197,8 +166,7 @@ plot(r(n).FF(k,:),'b-','linewidth',2);
...
@@ -197,8 +166,7 @@ plot(r(n).FF(k,:),'b-','linewidth',2);
legend
(
'ddx feedback'
,
'ddx feedforward'
);
legend
(
'ddx feedback'
,
'ddx feedforward'
);
xlabel
(
't'
);
ylabel
([
'ddx_'
num2str
(
k
)]);
xlabel
(
't'
);
ylabel
([
'ddx_'
num2str
(
k
)]);
print
(
'-dpng'
,
'outTest2.png'
);
%print('-dpng','outTest2.png');
%pause;
%pause;
%close all;
%close all;
...
...
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