diff --git a/README.md b/README.md index 214f45e347a4185fe408a529a627da7ff81b39e8..5850342369933d9943f5fa139f6368657328868d 100644 --- a/README.md +++ b/README.md @@ -1,51 +1,86 @@ # pbdlib-matlab +PbDLib is a set of tools combining statistical learning, dynamical systems and optimal control approaches for programming-by-demonstration applications. + +The Matlab/GNU Octave version is currently maintained by Sylvain Calinon, http://calinon.ch. + +More information can be found on: http://programming-by-demonstration.org/lib/ + ### Compatibility - The codes have been tested with both Matlab and GNU Octave. +The codes are compatible with both Matlab or GNU Octave. ### Usage - Examples starting with 'demo_' can be run from Matlab/GNU Octave. +Examples starting with `demo_` can be run from Matlab/GNU Octave. ### References - Calinon, S. (in preparation). A tutorial on task-parameterized movement learning and retrieval. - - Calinon, S., Bruno, D. and Caldwell, D.G. (2014). A task-parameterized probabilistic model with minimal intervention - control. Proc. of the IEEE Intl Conf. on Robotics and Automation (ICRA). - -### Description - - Demonstration of 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. - -### Datasets - - The folder "data" contains a dataset of 2D handwriting movements from LASA-EPFL (http://lasa.epfl.ch), collected within - the context of the AMARSI European Project. Reference: S.M. Khansari-Zadeh and A. Billard, "Learning Stable Non-Linear - Dynamical Systems with Gaussian Mixture Models", IEEE Transaction on Robotics, 2011. - -### Contact - - Sylvain Calinon - http://programming-by-demonstration.org/lib/ - - 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 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" - } +Did you find PbDLib useful for your research? Please acknowledge the authors in any academic publications that used some parts of these codes. + +``` +@article{Calinon15, + author="Calinon, S.", + title="A tutorial on task-parameterized movement learning and retrieval", + year="2015", +} +``` +``` +@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" +} +``` + +### Dataset + +The folder "data" contains a dataset of 2D handwriting movements from LASA-EPFL (http://lasa.epfl.ch), collected within the context of the AMARSI European Project. Reference: S.M. Khansari-Zadeh and A. Billard, "Learning Stable Non-Linear Dynamical Systems with Gaussian Mixture Models", IEEE Transaction on Robotics, 2011. + +### List of examples + +All the examples are located in the main folder, and the functions are located in the `m_fcts` folder. +| Filename | Description | +|----------|-------------| +| benchmark_DS_GP_GMM01 | Benchmark of task-parameterized model based on Gaussian process regression, with trajectory model (Gaussian mixture model encoding), and DS-GMR used for reproduction | +| benchmark_DS_GP_raw01 | Benchmark of task-parameterized model based on Gaussian process regression, with raw trajectory, and spring-damper system used for reproduction | +| benchmark_DS_PGMM01 | Benchmark of task-parameterized model based on parametric Gaussian mixture model, and DS-GMR used for reproduction | +| benchmark_DS_TP_GMM01 | Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction | +| benchmark_DS_TP_GP01 | Benchmark of task-parameterized Gaussian process (nonparametric task-parameterized method) | +| benchmark_DS_TP_LWR01 | Benchmark of task-parameterized locally weighted regression (nonparametric task-parameterized method) | +| benchmark_DS_TP_MFA01 | Benchmark of task-parameterized mixture of factor analyzers (TP-MFA), with DS-GMR used for reproduction | +| benchmark_DS_TP_trajGMM01 | Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction | +| demo_batchLQR01 | Controller retrieval through a batch solution of linear quadratic optimal control (unconstrained linear MPC), by relying on a Gaussian mixture model (GMM) encoding of position and velocity data (see also demo_iterativeLQR01) | +| demo_DSGMR01 | Gaussian mixture model (GMM), with Gaussian mixture regression(GMR) and dynamical systems used for reproduction, with decay variable used as input (as in DMP) | +| demo_DTW01 | Trajectory realignment through dynamic time warping (DTW) | +| demo_GMM01 | Gaussian mixture model (GMM) parameters estimation | +| demo_GMR01 | Gaussian mixture model (GMM) and time-based Gaussian mixture regression (GMR) used for reproduction | +| demo_GPR01 | Use of Gaussian process regression (GPR) as a task-parameterized model, with DS-GMR used to retrieve continuous movements | +| demo_HDDC01 | High Dimensional Data Clustering (HDDC, or HD-GMM) model from Bouveyron (2007) | +| demo_iterativeLQR01 | Controller retrieval through an iterative solution of linear quadratic optimal control (finite horizon, unconstrained linear MPC), by relying on a Gaussian mixture model (GMM) encoding of position and velocity data (see also demo_batchLQR01) | +| demo_MFA01 | Mixture of factor analyzers (MFA) | +| demo_MPPCA01 | Mixture of probabilistic principal component analyzers (MPPCA) | +| demo_stdPGMM01 | Parametric Gaussian mixture model (PGMM) used as a task-parameterized model, with DS-GMR employed to retrieve continuous movements | +| demo_testLQR01 | Test of the linear quadratic regulation with different variance in the data | +| demo_testLQR02 | Test of the linear quadratic regulation (LQR) with evaluation of the damping ratio found by the system | +| demo_testLQR03 | Comparison of linear quadratic regulators (LQR) with finite and infinite time horizons | +| demo_TPbatchLQR01 | Task-parameterized GMM encoding position and velocity data, combined with a batch solution of linear quadratic optimal control (unconstrained linear MPC) | +| demo_TPbatchLQR02 | Batch solution of a linear quadratic optimal control (unconstrained linear MPC) acting in multiple frames, which is equivalent to TP-GMM combined with LQR | +| demo_TPGMM01 | Task-parameterized Gaussian mixture model (TP-GMM) encoding | +| demo_TPGMR01 | Task-parameterized Gaussian mixture model (TP-GMM), with GMR used for reproduction (without dynamical system) | +| demo_TPGMR_DS01 | Dynamical system with constant gains used with a task-parameterized model | +| demo_TPGMR_LQR01 | Finite horizon LQR used with a task-parameterized model +| demo_TPGMR_LQR02 | Infinite horizon LQR used with a task-parameterized model +| demo_TPGP01 | Task-parameterized Gaussian process regression (TP-GPR) | +| demo_TPHDDC01 | Task-parameterized high dimensional data clustering (TP-HDDC) | +| demo_TPMFA01 | Task-parameterized mixture of factor analyzers (TP-MFA), without motion retrieval | +| demo_TPMPPCA01 | Task-parameterized mixture of probabilistic principal component analyzers (TP-MPPCA) | +| demo_TPtrajGMM01 | Task-parameterized model with trajectory-GMM encoding | +| demo_trajGMM01 | Reproduction of trajectory with a GMM with dynamic features (trajectory GMM) | +| demoIK_nullspace_TPGMM01 | IK with nullspace treated with task-parameterized GMM (bimanual tracking task, version with 4 frames) | +| demoIK_pointing_TPGMM01 | Task-parameterized GMM to encode pointing direction by considering nullspace constraint (4 frames) (example with two objects and robot frame, starting from the same initial pose (nullspace constraint), by using a single Euler orientation angle and 3 DOFs robot) | diff --git a/benchmark_DS_GP_GMM01.m b/benchmark_DS_GP_GMM01.m index ab3b9025a7ffa877b1269d37025f8836be4f8e93..5cef271592bbdc620ee63da384064115b440749e 100644 --- a/benchmark_DS_GP_GMM01.m +++ b/benchmark_DS_GP_GMM01.m @@ -1,10 +1,22 @@ function benchmark_DS_GP_GMM01 -%Benchmark of task-parameterized model based on Gaussian process regression, -%with trajectory model (Gaussian mixture model encoding), and DS-GMR used for reproduction -%Sylvain Calinon, 2015 +% Benchmark of task-parameterized model based on Gaussian process regression, +% with trajectory model (Gaussian mixture model encoding), and DS-GMR used for reproduction +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 3; %Number of Gaussians in the GMM @@ -14,8 +26,8 @@ model.dt = 0.01; %Time step model.kP = 100; %Stiffness gain model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio) nbRepros = 4; %Number of reproductions with new situations randomly generated -nbVarOut = model.nbVar-1; -L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; +nbVarOut = model.nbVar-1; %(here: x1,x2) +L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback gains %% Load 3rd order tensor data @@ -25,8 +37,8 @@ disp('Load 3rd order tensor data...'); % 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). +% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200) +% multiplied by the number of demonstrations (M=5). load('data/DataLQR01.mat'); @@ -40,7 +52,7 @@ 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, see Eq. (4.0.2) in doc/TechnicalReport.pdf +%Compute data with derivatives %Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples); Data = s(1).Data0(1,:); for n=1:nbSamples @@ -102,14 +114,14 @@ for n=1:nbSamples end % %Retrieval of attractor path through GMR -% currTar = GMR(r(n), DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf +% currTar = GMR(r(n), DataIn, 1, [2:model.nbVar]); %See Eq. (17)-(19) % % %Motion retrieval with spring-damper system % x = s(n).p(1).b(2:model.nbVar); % dx = zeros(nbVarOut,1); % for t=1:s(n).nbData % %Compute acceleration, velocity and position -% ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf +% ddx = -L * [x-currTar(:,t); dx]; % dx = dx + ddx * model.dt; % x = x + dx * model.dt; % r(n).Data(:,t) = x; @@ -146,14 +158,14 @@ for n=1:nbRepros end %Retrieval of attractor path through GMR - [rnew(n).currTar, rnew(n).currSigma] = GMR(rnew(n), DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf + [rnew(n).currTar, rnew(n).currSigma] = GMR(rnew(n), DataIn, 1, [2:model.nbVar]); %See Eq. (17)-(19) %Motion retrieval with spring-damper system x = rnew(n).p(1).b(2:model.nbVar); dx = zeros(nbVarOut,1); for t=1:nbD %Compute acceleration, velocity and position - ddx = -L * [x-rnew(n).currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf + ddx = -L * [x-rnew(n).currTar(:,t); dx]; dx = dx + ddx * model.dt; x = x + dx * model.dt; rnew(n).Data(:,t) = x; @@ -180,9 +192,10 @@ for n=1:nbSamples plotGMM(r(n).Mu(2:3,:),r(n).Sigma(2:3,2:3,:), [0 0 0], .04); end axis equal; axis(limAxes); -print('-dpng','-r600','graphs/benchmark_DS_GP_GMM01.png'); +%print('-dpng','-r600','graphs/benchmark_DS_GP_GMM01.png'); %Plot reproductions in new situations +disp('[Press enter to see next reproduction attempt]'); h=[]; for n=1:nbRepros delete(h); @@ -193,8 +206,8 @@ for n=1:nbRepros [1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)]; h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])]; axis equal; axis(limAxes); - print('-dpng','-r600',['graphs/benchmark_DS_GP_GMM' num2str(n+1,'%.2d') '.png']); - %pause + %print('-dpng','-r600',['graphs/benchmark_DS_GP_GMM' num2str(n+1,'%.2d') '.png']); + pause end pause; diff --git a/benchmark_DS_GP_raw01.m b/benchmark_DS_GP_raw01.m index fc04165a065c58d5ebf6781ea036b896a95abad3..32d2966949bad4eca4b44d731b102ddc025fe083 100644 --- a/benchmark_DS_GP_raw01.m +++ b/benchmark_DS_GP_raw01.m @@ -1,10 +1,22 @@ function benchmark_DS_GP_raw01 -%Benchmark of task-parameterized model based on Gaussian process regression, -%with raw trajectory, and spring-damper system used for reproduction -%Sylvain Calinon, 2015 +% Benchmark of task-parameterized model based on Gaussian process regression, +% with raw trajectory, and spring-damper system used for reproduction +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbFrames = 2; %Number of candidate frames of reference @@ -15,6 +27,7 @@ model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio) nbRepros = 4; %Number of reproductions with new situations randomly generated L = [eye(model.nbVar)*model.kP, eye(model.nbVar)*model.kV]; + %% Load 3rd order tensor data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% disp('Load 3rd order tensor data...'); @@ -22,8 +35,8 @@ disp('Load 3rd order tensor data...'); % 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). +% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200) +% multiplied by the number of demonstrations (M=5). load('data/DataLQR01.mat'); @@ -36,7 +49,7 @@ 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(model.nbVar)); -%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf +%Compute derivatives %Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples); Data = s(1).Data0(1,:); for n=1:nbSamples @@ -45,6 +58,7 @@ for n=1:nbSamples Data = [Data; s(n).Data]; %Data is a matrix of size M*D x T (stacking the different trajectory samples) end + %% GPR with raw trajectory encoding %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% fprintf('Parameters estimation of GPR with raw trajectory encoding:'); @@ -70,7 +84,7 @@ end % dx = zeros(model.nbVar,1); % for t=1:s(n).nbData % %Compute acceleration, velocity and position -% ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf +% ddx = -L * [x-currTar(:,t); dx]; % dx = dx + ddx * model.dt; % x = x + dx * model.dt; % r(n).Data(:,t) = x; @@ -101,7 +115,7 @@ for n=1:nbRepros dx = zeros(model.nbVar,1); for t=1:nbD %Compute acceleration, velocity and position - ddx = -L * [x-rnew(n).currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf + ddx = -L * [x-rnew(n).currTar(:,t); dx]; dx = dx + ddx * model.dt; x = x + dx * model.dt; rnew(n).Data(:,t) = x; @@ -125,9 +139,10 @@ for n=1:nbSamples [1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.04); end axis equal; axis(limAxes); -print('-dpng','-r600','graphs/benchmark_DS_GP_raw01.png'); +%print('-dpng','-r600','graphs/benchmark_DS_GP_raw01.png'); %Plot reproductions in new situations +disp('[Press enter to see next reproduction attempt]'); h=[]; for n=1:nbRepros delete(h); @@ -137,8 +152,8 @@ for n=1:nbRepros [1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)]; h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])]; axis equal; axis(limAxes); - print('-dpng','-r600',['graphs/benchmark_DS_GP_raw' num2str(n+1,'%.2d') '.png']); - %pause + %print('-dpng','-r600',['graphs/benchmark_DS_GP_raw' num2str(n+1,'%.2d') '.png']); + pause; end pause; diff --git a/benchmark_DS_PGMM01.m b/benchmark_DS_PGMM01.m index 6aaf9df7819cafd0e629d3f2162791627c689ebd..d33b277eeb98c42c3bd94fcaf696829e6816d8ca 100644 --- a/benchmark_DS_PGMM01.m +++ b/benchmark_DS_PGMM01.m @@ -1,9 +1,21 @@ function benchmark_DS_PGMM01 -%Benchmark of task-parameterized model based on parametric Gaussian mixture model, and DS-GMR used for reproduction -%Sylvain Calinon, 2015 +% Benchmark of task-parameterized model based on parametric Gaussian mixture model, and DS-GMR used for reproduction +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 3; %Number of Gaussians in the GMM @@ -13,8 +25,8 @@ model.dt = 0.01; %Time step model.kP = 100; %Stiffness gain model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio) nbRepros = 4; %Number of reproductions with new situations randomly generated -nbVarOut = model.nbVar - 1; -L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; +nbVarOut = model.nbVar-1; %(here, x1,x2) +L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback gains %% Load 3rd order tensor data @@ -24,8 +36,8 @@ disp('Load 3rd order tensor data...'); % 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). +% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200) +% multiplied by the number of demonstrations (M=5). load('data/DataLQR01.mat'); @@ -38,7 +50,7 @@ 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, see Eq. (4.0.2) in doc/TechnicalReport.pdf +%Compute derivatives Data = []; for n=1:nbSamples DataTmp = s(n).Data0(2:end,:); @@ -51,9 +63,10 @@ end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% fprintf('Parameters estimation of PGMM with EM:'); for n=1:nbSamples -% %Task parameters rearranged as a vector (position and orientation), see Eq. (7.1.4) in doc/TechnicalReport.pdf +% %Task parameters rearranged as a vector (position and orientation), see Eq. (48) % s(n).OmegaMu = [s(n).p(1).b(2:3); s(n).p(1).A(2:3,3); s(n).p(2).b(2:3); s(n).p(2).A(2:3,3); 1]; - %Task parameters rearranged as a vector (position only), see Eq. (7.1.4) in doc/TechnicalReport.pdf + + %Task parameters rearranged as a vector (position only), see Eq. (48) s(n).OmegaMu = [s(n).p(1).b(2:3); s(n).p(2).b(2:3); 1]; end @@ -61,10 +74,12 @@ end % %model = init_GMM_kmeans(Data, model); % model = init_GMM_timeBased(Data, model); % model = EM_GMM(Data, model); + for i=1:model.nbStates % %Initialization of parameters based on standard GMM % model.ZMu(:,:,i) = zeros(model.nbVar, size(s(1).OmegaMu,1)); % model.ZMu(:,end,i) = model.Mu(:,i); + %Random initialization of parameters model.ZMu(:,:,i) = rand(model.nbVar,size(s(1).OmegaMu,1)); model.Sigma(:,:,i) = eye(model.nbVar); @@ -83,17 +98,18 @@ nbVarOut = model.nbVar-1; for n=1:nbSamples %Computation of the resulting Gaussians (for display purpose) for i=1:model.nbStates - model.Mu(:,i) = model.ZMu(:,:,i) * s(n).OmegaMu; %Temporary Mu variable, see Eq. (7.1.4) in doc/TechnicalReport.pdf + model.Mu(:,i) = model.ZMu(:,:,i) * s(n).OmegaMu; %Temporary Mu variable, see Eq. (48) end r(n).Mu = model.Mu; + % %Retrieval of attractor path through GMR -% currTar = GMR(model, DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf +% currTar = GMR(model, DataIn, 1, [2:model.nbVar]); %See Eq. (17)-(19) % %Motion retrieval with spring-damper system % x = s(n).p(1).b(2:model.nbVar); % dx = zeros(nbVarOut,1); % for t=1:s(n).nbData % %Compute acceleration, velocity and position -% ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf +% ddx = -L * [x-currTar(:,t); dx]; % dx = dx + ddx * model.dt; % x = x + dx * model.dt; % r(n).Data(:,t) = x; @@ -110,6 +126,7 @@ for n=1:nbRepros % %Task parameters re-arranged as a vector (position and orientation), see Eq. (7.1.4) in doc/TechnicalReport.pdf % rnew(n).OmegaMu = [rnew(n).p(1).b(2:3); rnew(n).p(1).A(2:3,3); rnew(n).p(2).b(2:3); rnew(n).p(2).A(2:3,3); 1]; + %Task parameters re-arranged as a vector (position only), see Eq. (7.1.4) in doc/TechnicalReport.pdf rnew(n).OmegaMu = [rnew(n).p(1).b(2:3); rnew(n).p(2).b(2:3); 1]; @@ -152,9 +169,10 @@ for n=1:nbSamples plotGMM(r(n).Mu(2:3,:),model.Sigma(2:3,2:3,:), [0 0 0], .04); end axis equal; axis(limAxes); -print('-dpng','-r600','graphs/benchmark_DS_PGMM01.png'); +%print('-dpng','-r600','graphs/benchmark_DS_PGMM01.png'); %Plot reproductions in new situations +disp('[Press enter to see next reproduction attempt]'); h=[]; for n=1:nbRepros delete(h); @@ -165,8 +183,8 @@ for n=1:nbRepros [1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)]; h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])]; axis equal; axis(limAxes); - print('-dpng','-r600',['graphs/benchmark_DS_PGMM' num2str(n+1,'%.2d') '.png']); - %pause + %print('-dpng','-r600',['graphs/benchmark_DS_PGMM' num2str(n+1,'%.2d') '.png']); + pause; end pause; diff --git a/benchmark_DS_TP_GMM01.m b/benchmark_DS_TP_GMM01.m index b0df6d1d95db7246bb0b2a19ba5b97d95f7ff4ab..5a0dc70cbffc41b802ea58d5b82ab695b84220ed 100644 --- a/benchmark_DS_TP_GMM01.m +++ b/benchmark_DS_TP_GMM01.m @@ -1,6 +1,17 @@ function benchmark_DS_TP_GMM01 -%Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction -%Sylvain Calinon, 2015 +% Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); @@ -22,8 +33,8 @@ disp('Load 3rd order tensor data...'); % 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). +% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200) +% multiplied by the number of demonstrations (M=5). load('data/DataLQR01.mat'); @@ -37,7 +48,7 @@ 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, see Eq. (4.0.2) in doc/TechnicalReport.pdf +%Create 3rd order tensor data with XHAT instead of X Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples); for n=1:nbSamples DataTmp = s(n).Data0(2:end,:); @@ -97,9 +108,10 @@ for n=1:nbSamples plotGMM(r(n).Mu(2:3,:),r(n).Sigma(2:3,2:3,:), [0 0 0], .04); end axis equal; axis(limAxes); -print('-dpng','-r600','graphs/benchmark_DS_TP_GMM01.png'); +%print('-dpng','-r600','graphs/benchmark_DS_TP_GMM01.png'); %Plot reproductions in new situations +disp('[Press enter to see next reproduction attempt]'); h=[]; for n=1:nbRepros delete(h); @@ -110,8 +122,8 @@ for n=1:nbRepros [1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)]; h = [h plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[0 0 0])]; axis equal; axis(limAxes); - print('-dpng','-r600',['graphs/benchmark_DS_TP_GMM' num2str(n+1,'%.2d') '.png']); - %pause + %print('-dpng','-r600',['graphs/benchmark_DS_TP_GMM' num2str(n+1,'%.2d') '.png']); + pause; end pause; diff --git a/benchmark_DS_TP_GP01.m b/benchmark_DS_TP_GP01.m index a0714527dccb791488532a1f0722c9a394f76658..dfc4fe7058342bb2226f6078161d9e91a6b970ee 100644 --- a/benchmark_DS_TP_GP01.m +++ b/benchmark_DS_TP_GP01.m @@ -1,9 +1,21 @@ function benchmark_DS_TP_GP01 -%Benchmark of task-parameterized Gaussian process (nonparametric task-parameterized method) -%Sylvain Calinon, 2015 +% Benchmark of task-parameterized Gaussian process (nonparametric task-parameterized method) +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbFrames = 2; %Number of candidate frames of reference @@ -12,8 +24,8 @@ model.dt = 0.01; %Time step model.kP = 100; %Stiffness gain model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio) nbRepros = 4; %Number of reproductions with new situations randomly generated -nbVarOut = model.nbVar-1; -L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; +nbVarOut = model.nbVar-1; %(here, x1,x2) +L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback gains %% Load 3rd order tensor data @@ -23,8 +35,8 @@ disp('Load 3rd order tensor data...'); % 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). +% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200) +% multiplied by the number of demonstrations (M=5). load('data/DataLQR01.mat'); @@ -37,7 +49,7 @@ 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(model.nbVar-1)); -%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf +%Create 3rd order tensor data with XHAT instead of X Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples); for n=1:nbSamples DataTmp = s(n).Data0(2:end,:); @@ -64,6 +76,7 @@ for m=1:model.nbFrames model.Mu(:,m,:) = MuTmp; model.Sigma(:,:,m,:) = SigmaTmp; end + %Reproduction with spring-damper system % for n=1:nbSamples % currTar = productTPGMM0(model, s(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf @@ -121,9 +134,10 @@ for n=1:nbSamples [1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.04); end axis equal; axis(limAxes); -print('-dpng','-r600','graphs/benchmark_DS_TP_GP01.png'); +%print('-dpng','-r600','graphs/benchmark_DS_TP_GP01.png'); %Plot reproductions in new situations +disp('[Press enter to see next reproduction attempt]'); h=[]; for n=1:nbRepros delete(h); @@ -133,8 +147,8 @@ for n=1:nbRepros [1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)]; h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])]; axis equal; axis(limAxes); - print('-dpng','-r600',['graphs/benchmark_DS_TP_GP' num2str(n+1,'%.2d') '.png']); - %pause + %print('-dpng','-r600',['graphs/benchmark_DS_TP_GP' num2str(n+1,'%.2d') '.png']); + pause end pause; diff --git a/benchmark_DS_TP_LWR01.m b/benchmark_DS_TP_LWR01.m index e7f66818f2c640184bf015a6b5e2c8c09f5b21e3..a6cf26cb59fb344066dc288dda46b3d598e162d2 100644 --- a/benchmark_DS_TP_LWR01.m +++ b/benchmark_DS_TP_LWR01.m @@ -1,9 +1,21 @@ function benchmark_DS_TP_LWR01 -%Benchmark of task-parameterized locally weighted regression (nonparametric task-parameterized method) -%Sylvain Calinon, 2015 +% Benchmark of task-parameterized locally weighted regression (nonparametric task-parameterized method) +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbFrames = 2; %Number of candidate frames of reference @@ -12,8 +24,8 @@ model.dt = 0.01; %Time step model.kP = 100; %Stiffness gain model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio) nbRepros = 4; %Number of reproductions with new situations randomly generated -nbVarOut = model.nbVar-1; -L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; +nbVarOut = model.nbVar-1; %(here, x1,x2) +L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback gains %% Load 3rd order tensor data @@ -23,8 +35,8 @@ disp('Load 3rd order tensor data...'); % 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). +% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200) +% multiplied by the number of demonstrations (M=5). load('data/DataLQR01.mat'); @@ -37,7 +49,7 @@ 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(model.nbVar-1)); -%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf +%Compute derivatives Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples); for n=1:nbSamples DataTmp = s(n).Data0(2:end,:); @@ -75,6 +87,7 @@ for m=1:model.nbFrames model.Mu(:,m,:) = MuTmp; model.Sigma(:,:,m,:) = SigmaTmp; end + %Reproduction with spring-damper system % for n=1:nbSamples % currTar = productTPGMM0(model, s(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf @@ -132,9 +145,10 @@ for n=1:nbSamples [1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.04); end axis equal; axis(limAxes); -print('-dpng','-r600','graphs/benchmark_DS_TP_LWR01.png'); +%print('-dpng','-r600','graphs/benchmark_DS_TP_LWR01.png'); %Plot reproductions in new situations +disp('[Press enter to see next reproduction attempt]'); h=[]; for n=1:nbRepros delete(h); @@ -144,8 +158,8 @@ for n=1:nbRepros [1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)]; h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])]; axis equal; axis(limAxes); - print('-dpng','-r600',['graphs/benchmark_DS_TP_LWR' num2str(n+1,'%.2d') '.png']); - %pause + %print('-dpng','-r600',['graphs/benchmark_DS_TP_LWR' num2str(n+1,'%.2d') '.png']); + pause; end pause; diff --git a/benchmark_DS_TP_MFA01.m b/benchmark_DS_TP_MFA01.m index 92be712e06a149f6b8a2f9c280ec5d31dfb196d0..1f06468d68165ff459761124440646ff7c0dc3df 100644 --- a/benchmark_DS_TP_MFA01.m +++ b/benchmark_DS_TP_MFA01.m @@ -1,9 +1,21 @@ function benchmark_DS_TP_MFA01 -%Benchmark of task-parameterized mixture of factor analyzers (TP-MFA), with DS-GMR used for reproduction -%Sylvain Calinon, 2015 +% Benchmark of task-parameterized mixture of factor analyzers (TP-MFA), with DS-GMR used for reproduction +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 3; %Number of Gaussians in the GMM @@ -98,9 +110,10 @@ for n=1:nbSamples plotGMM(r(n).Mu(2:3,:),r(n).Sigma(2:3,2:3,:), [0 0 0], .04); end axis equal; axis(limAxes); -print('-dpng','-r600','graphs/benchmark_DS_TP_MFA01.png'); +%print('-dpng','-r600','graphs/benchmark_DS_TP_MFA01.png'); %Plot reproductions in new situations +disp('[Press enter to see next reproduction attempt]'); h=[]; for n=1:nbRepros delete(h); @@ -111,8 +124,8 @@ for n=1:nbRepros [1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)]; h = [h plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[0 0 0])]; axis equal; axis(limAxes); - print('-dpng','-r600',['graphs/benchmark_DS_TP_MFA' num2str(n+1,'%.2d') '.png']); - %pause + %print('-dpng','-r600',['graphs/benchmark_DS_TP_MFA' num2str(n+1,'%.2d') '.png']); + pause; end pause; diff --git a/benchmark_DS_TP_trajGMM01.m b/benchmark_DS_TP_trajGMM01.m index 9078fe8e876797b1d950c3dfb63a3e6fda2d4add..84053ddd25f5f1b3fbcc07d63f22c19995956412 100644 --- a/benchmark_DS_TP_trajGMM01.m +++ b/benchmark_DS_TP_trajGMM01.m @@ -1,20 +1,32 @@ function benchmark_DS_TP_trajGMM01 -%Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction -%Sylvain Calinon, 2015 +% Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS used for reproduction +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 3; %Number of Gaussians in the GMM model.nbFrames = 2; %Number of candidate frames of reference model.nbVarPos = 2; %Dimension of position data (here: x1,x2) model.nbDeriv = 3; %Number of static&dynamic features (D=2 for [x,dx], D=3 for [x,dx,ddx], etc.) -model.dt = .1; %Time step +model.dt = 0.1; %Time step model.kP = 10; %Stiffness gain model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio) nbRepros = 4; %Number of reproductions with new situations randomly generated -L = [eye(model.nbVarPos)*model.kP, eye(model.nbVarPos)*model.kV]; +L = [eye(model.nbVarPos)*model.kP, eye(model.nbVarPos)*model.kV]; %Feedback gains %% Load 3rd order tensor data @@ -24,8 +36,8 @@ disp('Load 3rd order tensor data...'); % 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). +% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200) +% multiplied by the number of demonstrations (M=5). load('data/DataLQR01.mat'); @@ -44,10 +56,6 @@ for n=1:nbSamples s(n).Data = [s(n).Data0(1,:); K * [DataTmp; DataTmp*D; DataTmp*D*D]]; end -% for n=1:nbSamples -% s(n).Data = s(n).Data0; -% end - %% Compute derivatives %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -215,9 +223,10 @@ for n=1:nbSamples plotGMM(r(n).Mu(1:2,:),r(n).Sigma(1:2,1:2,:), [0 0 0], .04); end axis equal; axis(limAxes); -print('-dpng','-r600','graphs/benchmark_DS_TP_trajGMM01.png'); +%print('-dpng','-r600','graphs/benchmark_DS_TP_trajGMM01.png'); %Plot reproductions in new situations +disp('[Press enter to see next reproduction attempt]'); h=[]; for n=1:nbRepros delete(h); @@ -228,8 +237,8 @@ for n=1:nbRepros [1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)]; h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])]; axis equal; axis(limAxes); - print('-dpng','-r600',['graphs/benchmark_DS_TP_trajGMM' num2str(n+1,'%.2d') '.png']); - %pause + %print('-dpng','-r600',['graphs/benchmark_DS_TP_trajGMM' num2str(n+1,'%.2d') '.png']); + pause; end pause; diff --git a/data/DataWithDeriv01.mat b/data/DataWithDeriv01.mat new file mode 100644 index 0000000000000000000000000000000000000000..397d036b1ce870343889574d148547f95f6e6453 Binary files /dev/null and b/data/DataWithDeriv01.mat differ diff --git a/data/DataWithDeriv02.mat b/data/DataWithDeriv02.mat new file mode 100644 index 0000000000000000000000000000000000000000..f8326ba6c8c6da8e7821d93698c69b4ea0a60cba Binary files /dev/null and b/data/DataWithDeriv02.mat differ diff --git a/data/TPGMMpointing_nullspace_data02.mat b/data/TPGMMpointing_nullspace_data02.mat index ec8e57e3ca33d94c3b66019d2756614fb7a5af63..9c69bb66915d7d6ae660fe58eba8a33ba3a54715 100644 Binary files a/data/TPGMMpointing_nullspace_data02.mat and b/data/TPGMMpointing_nullspace_data02.mat differ diff --git a/data/TPGMMpointing_nullspace_model02.mat b/data/TPGMMpointing_nullspace_model02.mat index 3de1f30862e922a00fef5da5ddf7917081c7e6f3..eb756ca7020afe5db1e2653c03769bc4bdb5965d 100644 Binary files a/data/TPGMMpointing_nullspace_model02.mat and b/data/TPGMMpointing_nullspace_model02.mat differ diff --git a/data/TPGMMpointing_nullspace_repro02.mat b/data/TPGMMpointing_nullspace_repro02.mat index 0c1cb16c33353933c22864c7ff9fc71018da3972..61131fa9078f2d8582547583ddc1e39ffb0d5d8b 100644 Binary files a/data/TPGMMpointing_nullspace_repro02.mat and b/data/TPGMMpointing_nullspace_repro02.mat differ diff --git a/data/TPGMMtmp.mat b/data/TPGMMtmp.mat index 157c0378aaee45aa2246073737837c9838daeb20..2cdd77e752fcee51fe871bd123ade07bfd463b57 100644 Binary files a/data/TPGMMtmp.mat and b/data/TPGMMtmp.mat differ diff --git a/demoIK_nullspace_TPGMM01.m b/demoIK_nullspace_TPGMM01.m index 01ea7fa0058eae3bd2182080c1236b5d2b52e573..380d98972d5a4b88c9394176ddc405f1327551fd 100644 --- a/demoIK_nullspace_TPGMM01.m +++ b/demoIK_nullspace_TPGMM01.m @@ -1,10 +1,23 @@ function demoIK_nullspace_TPGMM01 -%IK with nullspace treated with task-parameterized GMM (bimanual tracking task, version with 4 frames) -%First run 'startup_rvc' from the robotics toolbox -%Sylvain Calinon, 2015 +% Inverse kinematics with nullspace treated with task-parameterized GMM (bimanual tracking task, version with 4 frames). +% +% This example requires Peter Corke's robotics toolbox (run 'startup_rvc' from the robotics toolbox). +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 1; %Number of states @@ -22,12 +35,14 @@ pinvDampCoeff = 1e-8; %Coefficient for damped pseudoinverse needsModel = 1; + %% Create robot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% armLength = .5; L1 = Link('d', 0, 'a', armLength, 'alpha', 0); arm = SerialLink(repmat(L1,3,1)); + %% Generate demonstrations %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% if needsModel==1 @@ -46,6 +61,9 @@ for n=1:nbSamples %Objects moving on a line s(n).lxh = [linspace(s(n).lx(1,1),s(n).lx(1,1)-.6*armLength,nbData); linspace(s(n).lx(2,1),s(n).lx(2,1)+2*armLength,nbData)]; s(n).rxh = [linspace(s(n).rx(1,1),s(n).rx(1,1)+.6*armLength,nbData); linspace(s(n).rx(2,1),s(n).rx(2,1)+2*armLength,nbData)]; +% %Objects moving on a curve +% s(n).lxh = [-sin(linspace(0,pi,nbData))*0.4+s(n).lx(1,1); linspace(s(n).lx(2,1),s(n).lx(2,1)+2*armLength,nbData)]; +% s(n).rxh = [sin(linspace(0,pi,nbData))*0.4+s(n).rx(1,1); linspace(s(n).rx(2,1),s(n).rx(2,1)+2*armLength,nbData)]; end %Build Jacobians lJ = arm.jacob0(q(1:3),'trans'); @@ -116,7 +134,7 @@ model = init_TPGMM_timeBased(Data, model); %Initialization %model = init_TPGMM_kmeans(Data, model); %Initialization model = EM_TPGMM(Data, model); -model.nbVar = model.nbQ; %Update nbVar to use productTPGMM() +model.nbVar = model.nbQ; %Update of nbVar to later use productTPGMM() %% Reproduction @@ -137,6 +155,9 @@ for n=1:nbRepros %Objects moving on a line r(n).lxh = [linspace(r(n).lx(1,1),r(n).lx(1,1)-.6*armLength,nbData); linspace(r(n).lx(2,1),r(n).lx(2,1)+2*armLength,nbData)]; r(n).rxh = [linspace(r(n).rx(1,1),r(n).rx(1,1)+.6*armLength,nbData); linspace(r(n).rx(2,1),r(n).rx(2,1)+2*armLength,nbData)]; +% %Objects moving on a curve +% r(n).lxh = [-sin(linspace(0,pi,nbData))*0.3+r(n).lx(1,1); linspace(r(n).lx(2,1),r(n).lx(2,1)+2*armLength,nbData)]; +% r(n).rxh = [sin(linspace(0,pi,nbData))*0.3+r(n).rx(1,1); linspace(r(n).rx(2,1),r(n).rx(2,1)+2*armLength,nbData)]; end %IK controller ldx = (r(n).lxh(:,t) - r(n).lx(:,t)) / model.dt; @@ -198,6 +219,7 @@ save('data/TPGMMtmp.mat','s','r','model'); end %needsModel load('data/TPGMMtmp.mat'); + %% Plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure('position',[10,10,1000,450],'color',[1 1 1]); diff --git a/demoIK_pointing_nullspace_TPGMM01.m b/demoIK_pointing_TPGMM01.m similarity index 89% rename from demoIK_pointing_nullspace_TPGMM01.m rename to demoIK_pointing_TPGMM01.m index 720e38ba718f7e2f1ddbb65bbf5ff98c00d46681..cbf56d00163b6300754d5e25d2718b903dcf2164 100644 --- a/demoIK_pointing_nullspace_TPGMM01.m +++ b/demoIK_pointing_TPGMM01.m @@ -1,12 +1,25 @@ -function demoIK_pointing_nullspace_TPGMM01 -%Example of task-parameterized GMM to encode pointing direction by considering nullspace constraint (4 frames) -%(example with two objects and robot frame, starting from the same initial pose (nullspace constraint), -%by using a single Euler orientation angle and 3 DOFs robot) -%This example requires 'startup_rvc' to be run from the Robotics Toolbox. -%Sylvain Calinon, 2015 +function demoIK_pointing_TPGMM01 +% Task-parameterized GMM to encode pointing direction by considering nullspace constraint (4 frames) +% (example with two objects and robot frame, starting from the same initial pose (nullspace constraint), +% by using a single Euler orientation angle and 3 DOFs robot) +% +% This example requires Peter Corke's Robotics Toolbox (run 'startup_rvc' from the Robotics Toolbox). +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 6; %Number of states in the GMM @@ -24,9 +37,10 @@ eMax = 1; %Maximum error norm for stable Jacobian computation Kp = 0.15; %Amplification gain for error computation KpQ = 0.15; %Amplification gain for joint angle error computation -needsData = 0; -needsModel = 0; -needsRepro = 0; +needsData = 1; +needsModel = 1; +needsRepro = 1; + %% Create robot (requires the Robotics Toolbox) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -317,7 +331,7 @@ for n=1:nbRepros end end -%print('-dpng','graphs/demoIK_pointing_nullspace_TPGMM01.png'); +%print('-dpng','graphs/demoIK_pointing_TPGMM01.png'); %pause; %close all; diff --git a/demo_DSGMR01.m b/demo_DSGMR01.m index 350709f648a681966e174722e43adcf3264a1663..d7a415e23e3a8b6920d49c6d944fa2a9c50374b5 100644 --- a/demo_DSGMR01.m +++ b/demo_DSGMR01.m @@ -1,23 +1,35 @@ function demo_DSGMR01 -%Example of Gaussian mixture model (GMM), with Gaussian mixture regression(GMR) and dynamical systems used for reproduction, -%with decay variable used as input (as in DMP) -%Sylvain Calinon, 2015 +% Gaussian mixture model (GMM), with Gaussian mixture regression(GMR) and dynamical systems used for reproduction, +% with decay variable used as input (as in DMP). +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 5; %Number of states in the GMM model.nbVar = 3; %Number of variables [t,x1,x2] nbData = 200; %Length of each trajectory -model.dt = 0.001; %Time step +model.dt = 0.01; %Time step nbSamples = 5; %Number of demonstrations - -model.kP = 2000; %Stiffness gain +model.kP = 50; %Stiffness gain model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio) model.alpha = 1.0; %Decay factor -nbVarOut = model.nbVar-1; -L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; +nbVarOut = model.nbVar-1; %Dimension of spatial variables +L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback term + %% Load AMARSI data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -25,14 +37,21 @@ sIn(1) = 1; %Initialization of decay term for t=2:nbData sIn(t) = sIn(t-1) - model.alpha * sIn(t-1) * model.dt; %Update of decay term (ds/dt=-alpha s) end +% %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 transformation matrix to compute [X; DX; DDX] +% D = (diag(ones(1,nbData-1),-1)-eye(nbData)) / model.dt; +% D(end,end) = 0; -%Store data as [s; x] -demos=[]; -load('data/AMARSI/CShape.mat'); -Data=[]; +load('data/AMARSI/GShape.mat'); +Data=[]; Data0=[]; for n=1:nbSamples - s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling - Data = [Data [sIn; s(n).Data]]; + DataTmp = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling + %s(n).Data = K * [DataTmp; DataTmp*D; DataTmp*D*D]; + s(n).Data = DataTmp; + Data = [Data [sIn; s(n).Data]]; %Training data as [s;xhat] + Data0 = [Data0 [sIn; DataTmp]]; %Original data as [s;x] end @@ -41,34 +60,35 @@ end %model = init_GMM_timeBased(Data, model); model = init_GMM_kmeans(Data, model); model = EM_GMM(Data, model); -[currTar, currSigma] = GMR(model, sIn, 1, 2:3); +[currTar, currSigma] = GMR(model, sIn, 1, 2:model.nbVar); %see Eq. (17)-(19) %Motion retrieval with spring-damper system x = Data(2:model.nbVar,1); dx = zeros(nbVarOut,1); for t=1:nbData %Compute acceleration, velocity and position - ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf + ddx = L * [currTar(:,t)-x; -dx]; dx = dx + ddx * model.dt; x = x + dx * model.dt; DataOut(:,t) = x; end + %% Plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure('position',[10,10,1000,500]); %Plot GMM subplot(1,2,1); hold on; box on; title('GMM'); plotGMM(model.Mu(2:model.nbVar,:), model.Sigma(2:model.nbVar,2:model.nbVar,:), [.8 0 0]); -plot(Data(2,:),Data(3,:),'.','markersize',8,'color',[.7 .7 .7]); +plot(Data0(2,:),Data0(3,:),'.','markersize',8,'color',[.7 .7 .7]); axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]); %Plot DS-GMR subplot(1,2,2); hold on; box on; title('DS-GMR'); plotGMM(currTar, currSigma, [0 .8 0]); -plot(Data(2,:),Data(3,:),'.','markersize',8,'color',[.7 .7 .7]); +plot(Data0(2,:),Data0(3,:),'.','markersize',8,'color',[.7 .7 .7]); plot(currTar(1,:),currTar(2,:),'-','linewidth',1.5,'color',[0 .6 0]); plot(DataOut(1,:),DataOut(2,:),'-','linewidth',3,'color',[0 .3 0]); axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]); %print('-dpng','graphs/demo_DSGMR01.png'); -%pause; -%close all; +pause; +close all; diff --git a/demo_DTW01.m b/demo_DTW01.m new file mode 100644 index 0000000000000000000000000000000000000000..e324bc24570bf9c1cb72fa7716923acad47b78bd --- /dev/null +++ b/demo_DTW01.m @@ -0,0 +1,76 @@ +function demo_DTW01 +% Trajectory realignment through dynamic time warping (DTW). +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +nbData = 200; %Length of each trajectory +wMax = 50; %Warping time window +nbSamples = 5; %Number of demonstrations +nbVar = 2; %Number of dimensions (max 2 for AMARSI data) + + +%% Load AMARSI handwriting data +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +demos=[]; +load('data/AMARSI/GShape.mat'); +for n=1:nbSamples + s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos(1:nbVar,:), linspace(1,size(demos{n}.pos,2),nbData)); %Resampling +end + + +%% Dynamic time warping +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +r(1).Data = s(1).Data; +for n=2:nbSamples + [r(1).Data, r(n).Data, r(n-1).wPath] = DTW(r(1).Data, s(n).Data, wMax); + %Realign previous trajectories + p = r(n-1).wPath(1,:); + for m=2:n-1 + DataTmp = r(m).Data(:,p); + r(m).Data = spline(1:size(DataTmp,2), DataTmp, linspace(1,size(DataTmp,2),nbData)); %Resampling + end +end + + +%% Plots +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +figure('position',[10,10,1000,500]); +for k=1:nbVar + subplot(2,nbVar,(k-1)*2+1); hold on; if k==1 title('Before DTW'); end; + for n=1:nbSamples + plot(s(n).Data(k,:), '-','linewidth',1,'color',[0 0 0]); + end + xlabel('t'); ylabel(['x_' num2str(k)]); + axis tight; set(gca,'Xtick',[]); set(gca,'Ytick',[]); + subplot(2,nbVar,(k-1)*2+2); hold on; if k==1 title('After DTW'); end; + for n=1:nbSamples + plot(r(n).Data(k,:), '-','linewidth',1,'color',[0 0 0]); + end + xlabel('t'); ylabel(['x_' num2str(k)]); + axis tight; set(gca,'Xtick',[]); set(gca,'Ytick',[]); +end + +figure; hold on; +for n=1:nbSamples-1 + plot(r(n).wPath(1,:),r(n).wPath(2,:),'-','color',[0 0 0]); +end +xlabel('w_1'); ylabel('w_2'); + +%print('-dpng','graphs/demo_DTW01.png'); +%pause; +%close all; diff --git a/demo_GMM01.m b/demo_GMM01.m index 11aade306899848221683732fbcde5e69a0f0145..551e647bd895aa9c20e57491fb391eca593254ee 100644 --- a/demo_GMM01.m +++ b/demo_GMM01.m @@ -1,9 +1,21 @@ function demo_GMM01 -%Example of Gaussian mixture model (GMM) -%Sylvain Calinon, 2015 +% Gaussian mixture model (GMM) parameters estimation. +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 5; %Number of states in the GMM diff --git a/demo_GMR01.m b/demo_GMR01.m index afe5c267e73d0b5313110b05183918afd7bdb9e9..bc1e27cbc644233fd1e6fdc94900c7b30578d727 100644 --- a/demo_GMR01.m +++ b/demo_GMR01.m @@ -1,9 +1,21 @@ function demo_GMR01 -%Example of Gaussian mixture model (GMM) and time-based Gaussian mixture regression (GMR) used for reproduction -%Sylvain Calinon, 2015 +% Gaussian mixture model (GMM) with time-based Gaussian mixture regression (GMR) used for reproduction +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 5; %Number of states in the GMM @@ -12,7 +24,8 @@ model.dt = 0.001; %Time step duration nbData = 200; %Length of each trajectory nbSamples = 5; %Number of demonstrations -%% Load AMARSI data + +%% Load AMARSI handwriting data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% demos=[]; load('data/AMARSI/CShape.mat'); @@ -28,7 +41,7 @@ end %model = init_GMM_kmeans(Data, model); model = init_GMM_timeBased(Data, model); model = EM_GMM(Data, model); -[DataOut, SigmaOut] = GMR(model, [1:nbData]*model.dt, 1, 2:3); +[DataOut, SigmaOut] = GMR(model, [1:nbData]*model.dt, 1, 2:3); %see Eq. (17)-(19) %% Plots diff --git a/demo_GPR01.m b/demo_GPR01.m index 67670111af308af19bed0141ab4689233889a108..b981b5e07eb97414a190300a3931ba51961cd797 100644 --- a/demo_GPR01.m +++ b/demo_GPR01.m @@ -1,10 +1,21 @@ function demo_GPR01 -%Example of Gaussian processs regression (GPR) used as a task-parameterized model, -%with DS-GMR employed to retrieve continuous movements -%Sylvain Calinon, 2015 +% Gaussian processs regression (GPR) used for task adaptation, with DS-GMR employed to retrieve continuous movements +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 3; %Number of Gaussians in the GMM @@ -80,7 +91,7 @@ DataIn = [1:s(1).nbData] * model.dt; nbVarOut = model.nbVar-1; L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; for n=1:nbSamples - %Rebuild model parameters with GPR + %Rebuild model parameters with GPR, see Eq. (46) vOut = GPR(model.DataIn, model.DataOut, s(n).DataIn); %Re-arrange GPR output as GMM parameters @@ -95,15 +106,15 @@ for n=1:nbSamples r(n).Sigma(:,:,i) = V * max(D,5E-4) * V'; end - %Retrieval of attractor path through GMR - currTar = GMR(r(n), DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf + %Retrieval of attractor path through GMR, see Eq. (17)-(19) + currTar = GMR(r(n), DataIn, 1, [2:model.nbVar]); %Motion retrieval with spring-damper system x = s(n).p(1).b(2:model.nbVar); dx = zeros(nbVarOut,1); for t=1:s(n).nbData %Compute acceleration, velocity and position - ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf + ddx = -L * [x-currTar(:,t); dx]; dx = dx + ddx * model.dt; x = x + dx * model.dt; r(n).Data(:,t) = x; @@ -140,15 +151,15 @@ for n=1:nbRepros rnew(n).Sigma(:,:,i) = V * max(D,1E-3) * V'; end - %Retrieval of attractor path through GMR - currTar = GMR(rnew(n), DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf + %Retrieval of attractor path through GMR, see Eq. (17)-(19) + currTar = GMR(rnew(n), DataIn, 1, [2:model.nbVar]); %Motion retrieval with spring-damper system x = rnew(n).p(1).b(2:model.nbVar); dx = zeros(nbVarOut,1); for t=1:nbD %Compute acceleration, velocity and position - ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf + ddx = -L * [x-currTar(:,t); dx]; dx = dx + ddx * model.dt; x = x + dx * model.dt; rnew(n).Data(:,t) = x; diff --git a/demo_HDDC01.m b/demo_HDDC01.m index 358afbe14106f8ac957d4f9a03c33c67a09d65c8..0139babfe678dffad56a0ca66c755550944c2843 100644 --- a/demo_HDDC01.m +++ b/demo_HDDC01.m @@ -1,9 +1,21 @@ function demo_HDDC01 -%Example of High Dimensional Data Clustering (HDDC, or HD-GMM) model from Bouveyron (2007) -%Sylvain Calinon, 2015 +% Example of High Dimensional Data Clustering (HDDC, or HD-GMM) encoding from Bouveyron (2007) +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 4; %Number of states in the GMM @@ -13,7 +25,7 @@ nbData = 200; %Length of each trajectory nbSamples = 5; %Number of demonstrations -%% Load AMARSI data +%% Load AMARSI handwriting data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% demos=[]; load('data/AMARSI/GShape.mat'); %Load x1,x2 variables @@ -33,7 +45,8 @@ end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model = init_GMM_kmeans(Data, model); model0 = EM_GMM(Data, model); %for comparison -model = EM_HDGMM(Data, model); +model = EM_HDGMM(Data, model); + %% Plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/demo_MFA01.m b/demo_MFA01.m index f1f575c9cf6cc4f1965b988062132b49b331afd9..a9ca0913a2f86bd1d7d972b301bae7dcdcdf0b57 100644 --- a/demo_MFA01.m +++ b/demo_MFA01.m @@ -1,9 +1,21 @@ function demo_MFA01 -%Example of mixture of factor analysers (MFA) -%Sylvain Calinon, 2015 +% Mixture of factor analysers (MFA) encoding +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 4; %Number of states in the GMM @@ -13,7 +25,7 @@ nbData = 200; %Length of each trajectory nbSamples = 5; %Number of demonstrations -%% Load AMARSI data +%% Load AMARSI handwriting data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% demos=[]; load('data/AMARSI/GShape.mat'); %Load x1,x2 variables @@ -33,7 +45,7 @@ end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model = init_GMM_kmeans(Data, model); model0 = EM_GMM(Data, model); %for comparison -model = EM_MFA(Data, model); +model = EM_MFA(Data, model); %% Plots diff --git a/demo_MPPCA01.m b/demo_MPPCA01.m index 973ea06fd193fb9ea03f9ce1efbd6b875000eb28..5106a1bb29b3bbec3750dafd6c2af3a753254497 100644 --- a/demo_MPPCA01.m +++ b/demo_MPPCA01.m @@ -1,6 +1,17 @@ function demo_MPPCA01 -%Example of mixture of probabilistic principal component analyzers (MPPCA) -%Sylvain Calinon, 2015 +% Example of mixture of probabilistic principal component analyzers (MPPCA) encoding +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); @@ -13,7 +24,7 @@ nbData = 200; %Length of each trajectory nbSamples = 5; %Number of demonstrations -%% Load AMARSI data +%% Load AMARSI handwriting data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% demos=[]; load('data/AMARSI/GShape.mat'); %Load x1,x2 variables diff --git a/demo_TPGMM01.m b/demo_TPGMM01.m index c92750c904c73f984f23ba0e2d2c4dfb4bbcdf09..37b3fd7b253b4a5e20ac9cbce4cfdc0b365c8578 100644 --- a/demo_TPGMM01.m +++ b/demo_TPGMM01.m @@ -1,9 +1,21 @@ function demo_TPGMM01 -%Example of task-parameterized Gaussian mixture model (TP-GMM) -%Sylvain Calinon, 2015 +% Task-parameterized Gaussian mixture model (TP-GMM) encoding +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 3; %Number of Gaussians in the GMM @@ -18,8 +30,8 @@ disp('Load 3rd order tensor data...'); % 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=2 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 (nbSamples=5). +% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200) +% multiplied by the number of demonstrations (M=5). load('data/Data01.mat'); @@ -72,6 +84,30 @@ for m=1:model.nbFrames axis square; set(gca,'xtick',[0],'ytick',[0]); end + +% %% Saving of data for C++ version of pbdlib +% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% %Save model parameters +% for m=1:model.nbFrames +% M = []; +% N = []; +% for i=1:model.nbStates +% M = [M model.Sigma(:,:,m,i)]; +% N = [N model.Mu(:,m,i)]; +% end +% save(['data/Sigma' num2str(m) '.txt'], 'M', '-ascii'); +% save(['data/Mu' num2str(m) '.txt'], 'N', '-ascii'); +% end +% save(['data/Priors.txt'], 'model.Priors', '-ascii'); +% TPGMMparams = [model.nbVar; model.nbFrames; model.nbStates; model.dt]; +% save(['data/Tpgmm.txt'], 'TPGMMparams', '-ascii'); +% %Save task parameters +% for m=1:model.nbFrames +% Ab = [r(1).p(m).b'; r(1).p(m).A]; +% save(['data/Params' num2str(m) '.txt'], 'Ab', '-ascii'); +% end + + %print('-dpng','graphs/demo_TPGMM01.png'); %pause; %close all; diff --git a/demo_TPGMR01.m b/demo_TPGMR01.m index b0e10214e32404ab4506aff157ae639e32a2d26c..f253007dc6a07bc81b4aa72ab2b0f3d81ed74477 100644 --- a/demo_TPGMR01.m +++ b/demo_TPGMR01.m @@ -1,9 +1,21 @@ function demo_TPGMR01 -%Example of task-parameterized Gaussian mixture model (TP-GMM), with GMR used for reproduction -%Sylvain Calinon, 2015 +% Task-parameterized Gaussian mixture model (TP-GMM), with time-based GMR used for reproduction +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 3; %Number of Gaussians in the GMM @@ -19,8 +31,8 @@ disp('Load 3rd order tensor data...'); % 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 (nbSamples=5). +% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200) +% multiplied by the number of demonstrations (M=5). load('data/Data02.mat'); @@ -37,10 +49,10 @@ model = EM_tensorGMM(Data, model); disp('Reproductions with GMR...'); DataIn = s(1).Data(1,:); for n=1:nbSamples - [r(n).Mu, r(n).Sigma] = productTPGMM0(model, s(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf + [r(n).Mu, r(n).Sigma] = productTPGMM0(model, s(n).p); %See Eq. (6) r(n).Priors = model.Priors; r(n).nbStates = model.nbStates; - r(n).Data = [DataIn; GMR(r(n), DataIn, 1, 2:model.nbVar)]; %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf + r(n).Data = [DataIn; GMR(r(n), DataIn, 1, 2:model.nbVar)]; %See Eq. (17)-(19) end @@ -56,10 +68,10 @@ for n=1:nbRepros rnew(n).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 - [rnew(n).Mu, rnew(n).Sigma] = productTPGMM0(model, rnew(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf + [rnew(n).Mu, rnew(n).Sigma] = productTPGMM0(model, rnew(n).p); %See Eq. (6) rnew(n).Priors = model.Priors; rnew(n).nbStates = model.nbStates; - rnew(n).Data = [DataIn; GMR(rnew(n), DataIn, 1, 2:model.nbVar)]; %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf + rnew(n).Data = [DataIn; GMR(rnew(n), DataIn, 1, 2:model.nbVar)]; %See Eq. (17)-(19) end diff --git a/demo_TPGMR_DS01.m b/demo_TPGMR_DS01.m index 1d6a20fac2b99bbee1bea22f92697582ac55e2bf..1c45fe78473987e0f971e9d68d924669540d9abf 100644 --- a/demo_TPGMR_DS01.m +++ b/demo_TPGMR_DS01.m @@ -5,10 +5,11 @@ function demo_TPGMR_DS01 % 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. +% This example presents the results for a time-based GMR reference retrieval process combined with a tracking system +% with constant gains. % -% Author: Sylvain Calinon, 2014 -% http://programming-by-demonstration.org/SylvainCalinon +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ % % 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: @@ -25,6 +26,7 @@ function demo_TPGMR_DS01 addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 3; %Number of Gaussians in the GMM @@ -35,6 +37,7 @@ model.kP = 100; %Stiffness gain model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio) nbRepros = 8; %Number of reproductions with new situations randomly generated + %% Load 3rd order tensor data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% disp('Load 3rd order tensor data...'); @@ -42,8 +45,8 @@ disp('Load 3rd order tensor data...'); % 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). +% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200) +% multiplied by the number of demonstrations (M=5). load('data/DataLQR01.mat'); diff --git a/demo_TPGMR_LQR01.m b/demo_TPGMR_LQR01.m index 0ad618841e51dabb83c83b0c856f13ed7f580514..70b5025e2615d36506634ae8afe6104945804480 100644 --- a/demo_TPGMR_LQR01.m +++ b/demo_TPGMR_LQR01.m @@ -7,10 +7,10 @@ function demo_TPGMR_LQR01 % 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. +% This example presents the results for a time-based GMR reference retrieval process combined with a finite horizon LQR. % -% Author: Sylvain Calinon, 2014 -% http://programming-by-demonstration.org/SylvainCalinon +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ % % 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: @@ -27,6 +27,7 @@ function demo_TPGMR_LQR01 addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 3; %Number of Gaussians in the GMM @@ -63,7 +64,8 @@ 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); + r(n) = reproduction_LQR_finiteHorizon(model, a(n), a(n).currTar(:,1), rFactor); + r(n).Data = [DataIn; r(n).Data]; end @@ -80,7 +82,8 @@ for n=1:nbRepros 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); + rnew(n) = reproduction_LQR_finiteHorizon(model, anew(n), anew(n).currTar(:,1), rFactor); + rnew(n).Data = [DataIn; rnew(n).Data]; end diff --git a/demo_TPGMR_LQR02.m b/demo_TPGMR_LQR02.m index 4f733c716555361efc553179ba89cd753295f973..05294a1057d0a2da7e152e6fe1dd997d093dc5f7 100644 --- a/demo_TPGMR_LQR02.m +++ b/demo_TPGMR_LQR02.m @@ -7,10 +7,10 @@ function demo_TPGMR_LQR02 % 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 an infinite horizon LQR. +% This example presents the results for a time-based GMR reference retrieval process combined with an infinite horizon LQR. % -% Author: Sylvain Calinon, 2014 -% http://programming-by-demonstration.org/SylvainCalinon +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ % % 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: @@ -27,6 +27,7 @@ function demo_TPGMR_LQR02 addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 3; %Number of Gaussians in the GMM @@ -63,7 +64,8 @@ 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_infiniteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor); + r(n) = reproduction_LQR_infiniteHorizon(model, a(n), a(n).currTar(:,1), rFactor); + r(n).Data = [DataIn; r(n).Data]; end @@ -80,7 +82,8 @@ for n=1:nbRepros end %Retrieval of attractor path through task-parameterized GMR anew(n) = estimateAttractorPath(DataIn, model, rTmp); - rnew(n) = reproduction_LQR_infiniteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor); + rnew(n) = reproduction_LQR_infiniteHorizon(model, anew(n), anew(n).currTar(:,1), rFactor); + rnew(n).Data = [DataIn; rnew(n).Data]; end diff --git a/demo_TPGP01.m b/demo_TPGP01.m new file mode 100644 index 0000000000000000000000000000000000000000..a2b17933b61c9faa3b9869223bf401d711545056 --- /dev/null +++ b/demo_TPGP01.m @@ -0,0 +1,135 @@ +function demo_TPGP01 +% Task-parameterized Gaussian process regression (TP-GPR) +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +model.nbFrames = 2; %Number of candidate frames of reference +model.nbVar = 2; %Dimension of the datapoints in the dataset (here: x1,x2) +nbRepros = 8; %Number of reproductions with new situations randomly generated +nbData = 200; %Number of datapoints in a trajectory + + +%% 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=2 the dimension of a +% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200) +% multiplied by the number of demonstrations (M=5). +load('data/Data01.mat'); + + +%% Learning (pre-computation) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Learning...'); +model.nbStates = nbData; +model.Priors = ones(model.nbStates,1) / model.nbStates; +DataIn(1,:) = repmat(1:nbData,1,nbSamples); +for m=1:model.nbFrames + DataOut = squeeze(Data(:,m,:)); + [MuTmp, SigmaTmp] = GPR(DataIn, DataOut, DataIn, [1, 1E1, 1E-1]); + model.Mu(:,m,:) = MuTmp; + model.Sigma(:,:,m,:) = SigmaTmp; +end + + +%% Reproduction for the task parameters used to train the model +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Reproductions with TP-GP...'); +for n=1:nbSamples + r(n).Data = productTPGMM0(model, s(n).p); %See Eq. (6) +end + + +%% Reproduction for new task parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('New reproductions with TP-GP...'); +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); + rnew(n).p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2); + rnew(n).p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2); + end + rnew(n).Data = productTPGMM0(model, rnew(n).p); %See Eq. (6) +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(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:)); + plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','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'); +for n=1:nbSamples + %Plot frames + for m=1:model.nbFrames + plot([s(n).p(m).b(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:)); + plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]); + end +end +for n=1:nbSamples + %Plot trajectories + plot(r(n).Data(1,1), r(n).Data(2,1),'.','markersize',12,'color',clrmap(n,:)); + plot(r(n).Data(1,:), r(n).Data(2,:),'-','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'); +for n=1:nbRepros + %Plot frames + for m=1:model.nbFrames + plot([rnew(n).p(m).b(1) rnew(n).p(m).b(1)+rnew(n).p(m).A(1,2)], [rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:)); + plot(rnew(n).p(m).b(1), rnew(n).p(m).b(2), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]); + end +end +for n=1:nbRepros + %Plot trajectories + plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[.2 .2 .2]); + plot(rnew(n).Data(1,:), rnew(n).Data(2,:),'-','linewidth',1.5,'color',[.2 .2 .2]); +end +axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]); + +%print('-dpng','graphs/demo_TPGP01.png'); +%pause; +%close all; + + diff --git a/demo_TPHDDC01.m b/demo_TPHDDC01.m index 9dab570218fc6db9ea366af841e6a8c4e0f7eed5..7db387c26148aef2c807af67708df96e7a4cde29 100644 --- a/demo_TPHDDC01.m +++ b/demo_TPHDDC01.m @@ -1,9 +1,21 @@ function demo_TPHDDC01 -%Example of task-parameterized high dimensional data clustering (TP-HDDC) -%Sylvain Calinon, 2015 +% Task-parameterized high dimensional data clustering (TP-HDDC) +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 3; %Number of Gaussians in the GMM @@ -19,8 +31,8 @@ disp('Load 3rd order tensor data...'); % 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=2 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 (nbSamples=5). +% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200) +% multiplied by the number of demonstrations (M=5). load('data/Data01.mat'); @@ -32,9 +44,10 @@ model = EM_tensorHDGMM(Data, model); %Reconstruct GMM for each demonstration for n=1:nbSamples - [s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p); + [s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p); %See Eq. (6) end + %% Plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure('position',[20,50,1300,500]); diff --git a/demo_TPMFA01.m b/demo_TPMFA01.m index b47018794d21fa61572ae14dfc5d8fc7dda82f46..29fb85d616306e11abee04ae28bf3c6a072a7772 100644 --- a/demo_TPMFA01.m +++ b/demo_TPMFA01.m @@ -1,9 +1,21 @@ function demo_TPMFA01 -%Example of task-parameterized mixture of factor analyzers (TP-MFA) -%Sylvain Calinon, 2015 +% Task-parameterized mixture of factor analyzers (TP-MFA) +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 3; %Number of Gaussians in the GMM @@ -19,8 +31,8 @@ disp('Load 3rd order tensor data...'); % 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=2 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 (nbSamples=5). +% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200) +% multiplied by the number of demonstrations (M=5). load('data/Data01.mat'); @@ -32,9 +44,10 @@ model = EM_tensorMFA(Data, model); %Reconstruct GMM for each demonstration for n=1:nbSamples - [s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p); + [s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p); %See Eq. (6) end + %% Plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure('position',[20,50,1300,500]); diff --git a/demo_TPMPPCA01.m b/demo_TPMPPCA01.m index 4a9071307554e9a61bd427c270743b1815444d73..847f3135e282e3e50d7b41624191129580be336e 100644 --- a/demo_TPMPPCA01.m +++ b/demo_TPMPPCA01.m @@ -1,9 +1,21 @@ function demo_TPMPPCA01 -%Example of task-parameterized mixture of probabilistic principal component analyzers (MPPCA) (TP-GMM) -%Sylvain Calinon, 2015 +% Task-parameterized mixture of probabilistic principal component analyzers (TP-MPPCA) +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 3; %Number of Gaussians in the GMM @@ -19,8 +31,8 @@ disp('Load 3rd order tensor data...'); % 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=2 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 (nbSamples=5). +% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200) +% multiplied by the number of demonstrations (M=5). load('data/Data01.mat'); @@ -32,9 +44,10 @@ model = EM_tensorMPPCA(Data, model); %Reconstruct GMM for each demonstration for n=1:nbSamples - [s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p); + [s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p); %See Eq. (6) end + %% Plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure('position',[20,50,1300,500]); diff --git a/demo_TPbatchLQR01.m b/demo_TPbatchLQR01.m new file mode 100644 index 0000000000000000000000000000000000000000..0b3fd3225e30b8abf3f444f6e9a6c5beafb5f7b6 --- /dev/null +++ b/demo_TPbatchLQR01.m @@ -0,0 +1,230 @@ +function demo_TPbatchLQR01 +% Task-parameterized GMM encoding position and velocity data, combined with a batch solution +% of linear quadratic optimal control (unconstrained linear MPC) +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +model.nbStates = 3; %Number of Gaussians in the GMM +model.nbFrames = 2; %Number of candidate frames of reference +model.nbVarPos = 2; %Dimension of position data (here: x1,x2) +model.nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx]) +model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector +model.rfactor = 1E-1; %Control cost in LQR +model.dt = 0.01; %Time step duration +nbData = 200; %Number of datapoints in a trajectory +nbRepros = 5; %Number of reproductions + +%Dynamical System settings (discrete version), see Eq. (33) +A = kron([1, model.dt; 0, 1], eye(model.nbVarPos)); +B = kron([0; model.dt], eye(model.nbVarPos)); +%Control cost matrix +R = eye(model.nbVarPos) * model.rfactor; +R = kron(eye(nbData-1),R); + + +%% 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 DC x P x N, with D=2 the dimension of a +% datapoint, C=2 the number of derivatives (incl. position), P=2 the number of candidate frames, and N=TM +% the number of datapoints in a trajectory (T=200) multiplied by the number of demonstrations (M=5). +load('data/DataWithDeriv01.mat'); + +% %Convert position data to position+velocity data +% load('data/Data01.mat'); +% %Create transformation matrix to compute derivatives +% D = (diag(ones(1,nbData-1),-1)-eye(nbData)) / model.dt; +% D(end,end) = 0; +% %Create 3rd order tensor data and task parameters +% model.nbVar = model.nbVarPos * model.nbDeriv; +% Data = zeros(model.nbVar, model.nbFrames, nbSamples*nbData); +% for n=1:nbSamples +% s(n).Data = zeros(model.nbVar,model.nbFrames,nbData); +% s(n).Data0 = s(n).Data0(2:end,:); %Remove time +% DataTmp = s(n).Data0; +% for k=1:model.nbDeriv-1 +% DataTmp = [DataTmp; s(n).Data0*D^k]; %Compute derivatives +% end +% for m=1:model.nbFrames +% s(n).p(m).b = [s(n).p(m).b; zeros((model.nbDeriv-1)*model.nbVarPos,1)]; +% s(n).p(m).A = kron(eye(model.nbDeriv), s(n).p(m).A); +% s(n).Data(:,m,:) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbData)); +% Data(:,m,(n-1)*nbData+1:n*nbData) = s(n).Data(:,m,:); +% end +% end +% %Save new dataset including derivatives +% save('data/DataWithDeriv01.mat', 'Data','s','nbSamples'); + + +%% TP-GMM learning +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +fprintf('Parameters estimation of TP-GMM with EM...'); +%model = init_tensorGMM_kmeans(Data, model); %Initialization + +%Initialization based on position data +model0 = init_tensorGMM_kmeans(Data(1:model.nbVarPos,:,:), model); +[~,~,GAMMA2] = EM_tensorGMM(Data(1:model.nbVarPos,:,:), model0); +model.Priors = model0.Priors; +for i=1:model.nbStates + for m=1:model.nbFrames + DataTmp = squeeze(Data(:,m,:)); + model.Mu(:,m,i) = DataTmp * GAMMA2(i,:)'; + DataTmp = DataTmp - repmat(model.Mu(:,m,i),1,nbData*nbSamples); + model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp'; + end +end + +model = EM_tensorGMM(Data, model); + + +%% Reproduction with LQR for the task parameters used to train the model +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Reproductions with batch LQR...'); +%Construct Su and Sx matrices, see Eq. (35) +Su = zeros(model.nbVar*nbData, model.nbVarPos*(nbData-1)); +Sx = kron(ones(nbData,1),eye(model.nbVar)); +M = B; +for n=2:nbData + %Build Sx matrix + id1 = (n-1)*model.nbVar+1:nbData*model.nbVar; + Sx(id1,:) = Sx(id1,:) * A; + %Build Su matrix + id1 = (n-1)*model.nbVar+1:n*model.nbVar; + id2 = 1:(n-1)*model.nbVarPos; + Su(id1,id2) = M; + M = [A*M(:,1:model.nbVarPos), M]; +end + +%Reproductions +for n=1:nbSamples + %Reconstruct GMM + [s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p); + %Compute best path for the n-th demonstration + [~,s(n).q] = max(model.Pix(:,(n-1)*nbData+1:n*nbData),[],1); %works also for nbStates=1 + %Build stepwise reference trajectory, see Eq. (27) + MuQ = reshape(s(n).Mu(:,s(n).q), model.nbVar*nbData, 1); + SigmaQ = (kron(ones(nbData,1), eye(model.nbVar)) * reshape(s(n).Sigma(:,:,s(n).q), model.nbVar, model.nbVar*nbData)) ... + .* kron(eye(nbData), ones(model.nbVar)); + %Batch LQR (unconstrained linear MPC), see Eq. (37) + SuInvSigmaQ = Su' / SigmaQ; + Rq = SuInvSigmaQ * Su + R; + X = [s(1).Data0(:,1) + randn(model.nbVarPos,1)*0E0; zeros(model.nbVarPos,1)]; + rq = SuInvSigmaQ * (MuQ-Sx*X); + u = Rq \ rq; %can also be computed with u = lscov(Rq, rq); + r(n).Data = reshape(Sx*X+Su*u, model.nbVar, nbData); +end + + +%% Reproduction with LQR for new task parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('New reproductions with batch LQR...'); +for n=1:nbRepros + %Random generation of new task parameters + for m=1:model.nbFrames + id=ceil(rand(2,1)*nbSamples); + w=rand(2); w=w/sum(w); + rnew(n).p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2); + rnew(n).p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2); + end + %Reconstruct GMM + [rnew(n).Mu, rnew(n).Sigma] = productTPGMM0(model, rnew(n).p); + %Compute best path for the 1st demonstration (HSMM can alternatively be used here) + [~,rnew(n).q] = max(model.Pix(:,1:nbData),[],1); %works also for nbStates=1 + %Build stepwise reference trajectory, see Eq. (27) + MuQ = reshape(rnew(n).Mu(:,rnew(n).q), model.nbVar*nbData, 1); + SigmaQ = (kron(ones(nbData,1), eye(model.nbVar)) * reshape(rnew(n).Sigma(:,:,rnew(n).q), model.nbVar, model.nbVar*nbData)) ... + .* kron(eye(nbData), ones(model.nbVar)); + %Batch LQR (unconstrained linear MPC), see Eq. (37) + SuInvSigmaQ = Su' / SigmaQ; + Rq = SuInvSigmaQ * Su + R; + X = [s(1).Data0(:,1) + randn(model.nbVarPos,1)*0E0; zeros(model.nbVarPos,1)]; + rq = SuInvSigmaQ * (MuQ-Sx*X); + u = Rq \ rq; %can also be computed with u = lscov(Rq, rq); + rnew(n).Data = reshape(Sx*X+Su*u, model.nbVar, nbData); +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(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:)); + plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]); + end + %Plot trajectories + plot(s(n).Data0(1,1), s(n).Data0(2,1),'.','markersize',12,'color',clrmap(n,:)); + plot(s(n).Data0(1,:), s(n).Data0(2,:),'-','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'); +for n=1:nbSamples + %Plot frames + for m=1:model.nbFrames + plot([s(n).p(m).b(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:)); + plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]); + end +end +for n=1:nbSamples + %Plot trajectories + plot(r(n).Data(1,1), r(n).Data(2,1),'.','markersize',12,'color',clrmap(n,:)); + plot(r(n).Data(1,:), r(n).Data(2,:),'-','linewidth',1.5,'color',clrmap(n,:)); +end +for n=1:nbSamples + %Plot Gaussians + plotGMM(s(n).Mu(1:2,:,1), s(n).Sigma(1:2,1:2,:,1), [.5 .5 .5], .4); +end +axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]); + +%NEW REPROS +subplot(1,3,3); hold on; box on; title('New reproductions'); +for n=1:nbRepros + %Plot frames + for m=1:model.nbFrames + plot([rnew(n).p(m).b(1) rnew(n).p(m).b(1)+rnew(n).p(m).A(1,2)], [rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:)); + plot(rnew(n).p(m).b(1), rnew(n).p(m).b(2), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]); + end +end +for n=1:nbRepros + %Plot trajectories + plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[.2 .2 .2]); + plot(rnew(n).Data(1,:), rnew(n).Data(2,:),'-','linewidth',1.5,'color',[.2 .2 .2]); +end +for n=1:nbRepros + %Plot Gaussians + plotGMM(rnew(n).Mu(1:2,:,1), rnew(n).Sigma(1:2,1:2,:,1), [.5 .5 .5], .4); +end +axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]); + +%print('-dpng','graphs/demo_TPbatchLQR01.png'); +%pause; +%close all; diff --git a/demo_TPbatchLQR02.m b/demo_TPbatchLQR02.m new file mode 100644 index 0000000000000000000000000000000000000000..42a9176ca94b478bfbe711e61e38e1e8427be6bf --- /dev/null +++ b/demo_TPbatchLQR02.m @@ -0,0 +1,232 @@ +function demo_TPbatchLQR02 +% Batch solution of a linear quadratic optimal control (unconstrained linear MPC) acting in multiple frames, +% which is equivalent to TP-GMM combined with LQR +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +model.nbStates = 3; %Number of Gaussians in the GMM +model.nbFrames = 2; %Number of candidate frames of reference +model.nbVarPos = 2; %Dimension of position data (here: x1,x2) +model.nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx]) +model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector +model.rfactor = 1E-1; %Control cost in LQR +model.dt = 0.01; %Time step duration +nbData = 200; %Number of datapoints in a trajectory +nbRepros = 5; %Number of reproductions + +%Dynamical System settings (discrete version), see Eq. (33) +A = kron([1, model.dt; 0, 1], eye(model.nbVarPos)); +B = kron([0; model.dt], eye(model.nbVarPos)); +%Control cost matrix +R = eye(model.nbVarPos) * model.rfactor; +R = kron(eye(nbData-1),R); + + +%% 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 DC x P x N, with D=2 the dimension of a +% datapoint, C=2 the number of derivatives (incl. position), P=2 the number of candidate frames, and N=TM +% the number of datapoints in a trajectory (T=200) multiplied by the number of demonstrations (M=5). +load('data/DataWithDeriv01.mat'); + + +%% TP-GMM learning +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +fprintf('Parameters estimation of TP-GMM with EM...'); +%model = init_tensorGMM_kmeans(Data, model); %Initialization + +%Initialization based on position data +model0 = init_tensorGMM_kmeans(Data(1:model.nbVarPos,:,:), model); +[~,~,GAMMA2] = EM_tensorGMM(Data(1:model.nbVarPos,:,:), model0); +model.Priors = model0.Priors; +for i=1:model.nbStates + for m=1:model.nbFrames + DataTmp = squeeze(Data(:,m,:)); + model.Mu(:,m,i) = DataTmp * GAMMA2(i,:)'; + DataTmp = DataTmp - repmat(model.Mu(:,m,i),1,nbData*nbSamples); + model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp'; + end +end + +model = EM_tensorGMM(Data, model); + + +%% Reproduction with LQR for the task parameters used to train the model +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Reproductions with batch LQR...'); +%Construct Su and Sx matrices, see Eq. (35) +Su = zeros(model.nbVar*nbData, model.nbVarPos*(nbData-1)); +Sx = kron(ones(nbData,1),eye(model.nbVar)); +M = B; +for n=2:nbData + %Build Sx matrix + id1 = (n-1)*model.nbVar+1:nbData*model.nbVar; + Sx(id1,:) = Sx(id1,:) * A; + %Build Su matrix + id1 = (n-1)*model.nbVar+1:n*model.nbVar; + id2 = 1:(n-1)*model.nbVarPos; + Su(id1,id2) = M; + M = [A*M(:,1:model.nbVarPos), M]; +end + +%Reproductions +for n=1:nbSamples + + %GMM projection, see Eq. (5) + for i=1:model.nbStates + for m=1:model.nbFrames + s(n).p(m).Mu(:,i) = s(n).p(m).A * model.Mu(:,m,i) + s(n).p(m).b; + s(n).p(m).Sigma(:,:,i) = s(n).p(m).A * model.Sigma(:,:,m,i) * s(n).p(m).A'; + end + end + + %Compute best path for the n-th demonstration + [~,s(n).q] = max(model.Pix(:,(n-1)*nbData+1:n*nbData),[],1); %works also for nbStates=1 + + %Build a reference trajectory for each frame, see Eq. (27) + invSigmaQ = zeros(model.nbVar*nbData); + for m=1:model.nbFrames + s(n).p(m).MuQ = reshape(s(n).p(m).Mu(:,s(n).q), model.nbVar*nbData, 1); + s(n).p(m).SigmaQ = (kron(ones(nbData,1), eye(model.nbVar)) * reshape(s(n).p(m).Sigma(:,:,s(n).q), model.nbVar, model.nbVar*nbData)) ... + .* kron(eye(nbData), ones(model.nbVar)); + invSigmaQ = invSigmaQ + inv(s(n).p(m).SigmaQ); + end + + %Batch LQR (unconstrained linear MPC), see Eq. (37) + SuInvSigmaQ = Su' * invSigmaQ; + Rq = SuInvSigmaQ * Su + R; + X = [s(1).Data0(:,1) + randn(model.nbVarPos,1)*0E0; zeros(model.nbVarPos,1)]; + rq = zeros(model.nbVar*nbData,1); + for m=1:model.nbFrames + rq = rq + s(n).p(m).SigmaQ \ (s(n).p(m).MuQ - Sx*X); + end + rq = Su' * rq; + u = Rq \ rq; %can also be computed with u = lscov(Rq, rq); + r(n).Data = reshape(Sx*X+Su*u, model.nbVar, nbData); +end + + +%% Reproduction with LQR for new task parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('New reproductions with batch LQR...'); +for n=1:nbRepros + + %Random generation of new task parameters + for m=1:model.nbFrames + id=ceil(rand(2,1)*nbSamples); + w=rand(2); w=w/sum(w); + rnew(n).p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2); + rnew(n).p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2); + end + + %GMM projection, see Eq. (5) + for i=1:model.nbStates + for m=1:model.nbFrames + rnew(n).p(m).Mu(:,i) = rnew(n).p(m).A * model.Mu(:,m,i) + rnew(n).p(m).b; + rnew(n).p(m).Sigma(:,:,i) = rnew(n).p(m).A * model.Sigma(:,:,m,i) * rnew(n).p(m).A'; + end + end + + %Compute best path for the 1st demonstration (HSMM can alternatively be used here) + [~,rnew(n).q] = max(model.Pix(:,1:nbData),[],1); %works also for nbStates=1 + + %Build a reference trajectory for each frame, see Eq. (27) + invSigmaQ = zeros(model.nbVar*nbData); + for m=1:model.nbFrames + rnew(n).p(m).MuQ = reshape(rnew(n).p(m).Mu(:,rnew(n).q), model.nbVar*nbData, 1); + rnew(n).p(m).SigmaQ = (kron(ones(nbData,1), eye(model.nbVar)) * ... + reshape(rnew(n).p(m).Sigma(:,:,rnew(n).q), model.nbVar, model.nbVar*nbData)) .* kron(eye(nbData), ones(model.nbVar)); + invSigmaQ = invSigmaQ + inv(rnew(n).p(m).SigmaQ); + end + + %Batch LQR (unconstrained linear MPC), see Eq. (37) + SuInvSigmaQ = Su' * invSigmaQ; + Rq = SuInvSigmaQ * Su + R; + X = [s(1).Data0(:,1) + randn(model.nbVarPos,1)*0E0; zeros(model.nbVarPos,1)]; + rq = zeros(model.nbVar*nbData,1); + for m=1:model.nbFrames + rq = rq + rnew(n).p(m).SigmaQ \ (rnew(n).p(m).MuQ - Sx*X); + end + rq = Su' * rq; + u = Rq \ rq; %can also be computed with u = lscov(Rq, rq); + rnew(n).Data = reshape(Sx*X+Su*u, model.nbVar, nbData); +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(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:)); + plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]); + end + %Plot trajectories + plot(s(n).Data0(1,1), s(n).Data0(2,1),'.','markersize',12,'color',clrmap(n,:)); + plot(s(n).Data0(1,:), s(n).Data0(2,:),'-','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'); +for n=1:nbSamples + %Plot frames + for m=1:model.nbFrames + plot([s(n).p(m).b(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:)); + plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]); + end +end +for n=1:nbSamples + %Plot trajectories + plot(r(n).Data(1,1), r(n).Data(2,1),'.','markersize',12,'color',clrmap(n,:)); + plot(r(n).Data(1,:), r(n).Data(2,:),'-','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'); +for n=1:nbRepros + %Plot frames + for m=1:model.nbFrames + plot([rnew(n).p(m).b(1) rnew(n).p(m).b(1)+rnew(n).p(m).A(1,2)], [rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:)); + plot(rnew(n).p(m).b(1), rnew(n).p(m).b(2), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]); + end +end +for n=1:nbRepros + %Plot trajectories + plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[.2 .2 .2]); + plot(rnew(n).Data(1,:), rnew(n).Data(2,:),'-','linewidth',1.5,'color',[.2 .2 .2]); +end +axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]); + +%print('-dpng','graphs/demo_TPbatchLQR02.png'); +%pause; +%close all; diff --git a/demo_trajTPGMM01.m b/demo_TPtrajGMM01.m similarity index 60% rename from demo_trajTPGMM01.m rename to demo_TPtrajGMM01.m index 2ddf6e5e98a1ac6cbfecf2ef9a0ef6d2f4074289..81d49a47f5ea1d1d7f23f37d595a18883007c2e3 100644 --- a/demo_trajTPGMM01.m +++ b/demo_TPtrajGMM01.m @@ -1,17 +1,32 @@ -function demo_trajTPGMM01 -%Example of TP-GMM with trajectory GMM encoding -%Sylvain Calinon, 2015 +function demo_TPtrajGMM01 +% Task-parameterized model with trajectory-GMM encoding (GMM with dynamic features) +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 3; %Number of Gaussians in the GMM model.nbFrames = 2; %Number of candidate frames of reference model.nbVarPos = 2; %Dimension of position data (here: x1,x2) model.nbDeriv = 3; %Number of static&dynamic features (D=2 for [x,dx], D=3 for [x,dx,ddx], etc.) +model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector model.dt = 0.01; %Time step -nbRepros = 8; %Number of reproductions with new situations randomly generated +nbData = 200; %Number of datapoints in a trajectory +nbRepros = 5; %Number of reproductions with new situations randomly generated + %% Load 3rd order tensor data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -19,47 +34,64 @@ 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 -% multiplied by the number of demonstrations. -load('data/DataLQR01.mat'); - -%Compute derivatives -nbD = s(1).nbData; -%Create transformation matrix to compute [X; DX; DDX] -D = (diag(ones(1,nbD-1),-1)-eye(nbD)) / model.dt; -D(end,end) = 0; -%Create 3rd order tensor data with XHAT instead of X -model.nbVar = model.nbVarPos * model.nbDeriv; -Data = zeros(model.nbVar, model.nbFrames, nbD); -for n=1:nbSamples - DataTmp = s(n).Data0(2:end,:); - for k=1:model.nbDeriv-1 - DataTmp = [DataTmp; s(n).Data0(2:end,:)*D^k]; %Compute derivatives - end - for m=1:model.nbFrames - s(n).p(m).b = [s(n).p(m).b(2:end); zeros((model.nbDeriv-1)*model.nbVarPos,1)]; - s(n).p(m).A = kron(eye(model.nbDeriv), s(n).p(m).A(2:end,2:end)); - Data(:,m,(n-1)*nbD+1:n*nbD) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbD)); - end -end +% in the different frames. It is a 3rd order tensor of dimension DC x P x N, with D=2 the dimension of a +% datapoint, C=2 the number of derivatives (incl. position), P=2 the number of candidate frames, and N=TM +% the number of datapoints in a trajectory (T=200) multiplied by the number of demonstrations (M=5). +load('data/DataWithDeriv02.mat'); + +% %Convert position data to position+velocity data +% load('data/Data01.mat'); +% %Create transformation matrix to compute derivatives +% D = (diag(ones(1,nbData-1),-1)-eye(nbData)) / model.dt; +% D(end,end) = 0; +% %Create 3rd order tensor data and task parameters +% Data = zeros(model.nbVar, model.nbFrames, nbSamples*nbData); +% for n=1:nbSamples +% s(n).Data = zeros(model.nbVar,model.nbFrames,nbData); +% s(n).Data0 = s(n).Data0(2:end,:); %Remove time +% DataTmp = s(n).Data0; +% for k=1:model.nbDeriv-1 +% DataTmp = [DataTmp; s(n).Data0*D^k]; %Compute derivatives +% end +% for m=1:model.nbFrames +% s(n).p(m).b = [s(n).p(m).b; zeros((model.nbDeriv-1)*model.nbVarPos,1)]; +% s(n).p(m).A = kron(eye(model.nbDeriv), s(n).p(m).A); +% s(n).Data(:,m,:) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbData)); +% Data(:,m,(n-1)*nbData+1:n*nbData) = s(n).Data(:,m,:); +% end +% end +% %Save new dataset including derivatives +% save('data/DataWithDeriv02.mat', 'Data','s','nbSamples'); %Construct PHI operator (big sparse matrix) -[PHI,PHI1,T,T1] = constructPHI(model,nbD,nbSamples); +[PHI,PHI1] = constructPHI(model, nbData, nbSamples); %% TP-GMM learning %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -fprintf('Parameters estimation of TP-GMM with EM:'); +fprintf('Parameters estimation of TP-GMM with EM...'); %model = init_tensorGMM_timeBased(Data, model); %Initialization -model = init_tensorGMM_kmeans(Data, model); %Initialization +%model = init_tensorGMM_kmeans(Data, model); %Initialization + +%Initialization based on position data +model0 = init_tensorGMM_kmeans(Data(1:model.nbVarPos,:,:), model); +[~,~,GAMMA2] = EM_tensorGMM(Data(1:model.nbVarPos,:,:), model0); +model.Priors = model0.Priors; +for i=1:model.nbStates + for m=1:model.nbFrames + DataTmp = squeeze(Data(:,m,:)); + model.Mu(:,m,i) = DataTmp * GAMMA2(i,:)'; + DataTmp = DataTmp - repmat(model.Mu(:,m,i),1,nbData*nbSamples); + model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp'; + end +end + model = EM_tensorGMM(Data, model); %% Reproduction for the task parameters used to train the model %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% disp('Reproductions...'); -%DataIn = [1:s(1).nbData] * model.dt; for n=1:nbSamples %Products of linearly transformed Gaussians for i=1:model.nbStates @@ -74,19 +106,19 @@ for n=1:nbSamples r(n).Sigma(:,:,i) = inv(SigmaTmp); r(n).Mu(:,i) = r(n).Sigma(:,:,i) * MuTmp; end - %Create single Gaussian N(MuQ,SigmaQ) based on state sequence q - [~,r(n).q] = max(model.Pix(:,(n-1)*T1+1:n*T1),[],1); %works also for nbStates=1 - r(n).MuQ = reshape(r(n).Mu(:,r(n).q), model.nbVarPos*model.nbDeriv*T1, 1); - r(n).SigmaQ = zeros(model.nbVarPos*model.nbDeriv*T1); - for t=1:T1 + %Create single Gaussian N(MuQ,SigmaQ) based on state sequence q, see Eq. (27) + [~,r(n).q] = max(model.Pix(:,(n-1)*nbData+1:n*nbData),[],1); %works also for nbStates=1 + r(n).MuQ = reshape(r(n).Mu(:,r(n).q), model.nbVarPos*model.nbDeriv*nbData, 1); + r(n).SigmaQ = zeros(model.nbVarPos*model.nbDeriv*nbData); + for t=1:nbData id1 = (t-1)*model.nbVarPos*model.nbDeriv+1:t*model.nbVarPos*model.nbDeriv; r(n).SigmaQ(id1,id1) = r(n).Sigma(:,:,r(n).q(t)); end - %Retrieval of data with trajectory GMM + %Retrieval of data with trajectory GMM, see Eq. (30) PHIinvSigmaQ = PHI1'/r(n).SigmaQ; Rq = PHIinvSigmaQ * PHI1; rq = PHIinvSigmaQ * r(n).MuQ; - r(n).Data = reshape(Rq\rq, model.nbVarPos, T1); %Reshape data for plotting + r(n).Data = reshape(Rq\rq, model.nbVarPos, nbData); %Reshape data for plotting end @@ -114,19 +146,19 @@ for n=1:nbRepros rnew(n).Sigma(:,:,i) = inv(SigmaTmp); rnew(n).Mu(:,i) = rnew(n).Sigma(:,:,i) * MuTmp; end - %Create single Gaussian N(MuQ,SigmaQ) based on state sequence q - [~,rnew(n).q] = max(model.Pix(:,1:T1),[],1); %works also for nbStates=1 - rnew(n).MuQ = reshape(rnew(n).Mu(:,rnew(n).q), model.nbVarPos*model.nbDeriv*T1, 1); - rnew(n).SigmaQ = zeros(model.nbVarPos*model.nbDeriv*T1); - for t=1:T1 + %Create single Gaussian N(MuQ,SigmaQ) based on state sequence q, see Eq. (27) + [~,rnew(n).q] = max(model.Pix(:,1:nbData),[],1); %works also for nbStates=1 + rnew(n).MuQ = reshape(rnew(n).Mu(:,rnew(n).q), model.nbVarPos*model.nbDeriv*nbData, 1); + rnew(n).SigmaQ = zeros(model.nbVarPos*model.nbDeriv*nbData); + for t=1:nbData id1 = (t-1)*model.nbVarPos*model.nbDeriv+1:t*model.nbVarPos*model.nbDeriv; rnew(n).SigmaQ(id1,id1) = rnew(n).Sigma(:,:,rnew(n).q(t)); end - %Retrieval of data with trajectory GMM + %Retrieval of data with trajectory GMM, see Eq. (30) PHIinvSigmaQ = PHI1'/rnew(n).SigmaQ; Rq = PHIinvSigmaQ * PHI1; rq = PHIinvSigmaQ * rnew(n).MuQ; - rnew(n).Data = reshape(Rq\rq, model.nbVarPos, T1); %Reshape data for plotting + rnew(n).Data = reshape(Rq\rq, model.nbVarPos, nbData); %Reshape data for plotting end @@ -148,8 +180,8 @@ for n=1:nbSamples plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','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,:)); + plot(s(n).Data0(1,1), s(n).Data0(2,1),'.','markersize',12,'color',clrmap(n,:)); + plot(s(n).Data0(1,:), s(n).Data0(2,:),'-','linewidth',1.5,'color',clrmap(n,:)); end axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]); @@ -169,7 +201,7 @@ for n=1:nbSamples end for n=1:nbSamples %Plot Gaussians - plotGMM(r(n).Mu(1:2,:,1), r(n).Sigma(1:2,1:2,:,1), [.5 .5 .5],.8); + plotGMM(r(n).Mu(1:2,:,1), r(n).Sigma(1:2,1:2,:,1), [.5 .5 .5], .4); end axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]); @@ -189,11 +221,11 @@ for n=1:nbRepros end for n=1:nbRepros %Plot Gaussians - plotGMM(rnew(n).Mu(1:2,:,1), rnew(n).Sigma(1:2,1:2,:,1), [.5 .5 .5],.8); + plotGMM(rnew(n).Mu(1:2,:,1), rnew(n).Sigma(1:2,1:2,:,1), [.5 .5 .5], .4); end axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]); -%print('-dpng','graphs/demo_trajTPGMM01.png'); +%print('-dpng','graphs/demo_TPtrajGMM01.png'); %pause; %close all; diff --git a/demo_batchLQR01.m b/demo_batchLQR01.m new file mode 100644 index 0000000000000000000000000000000000000000..4ce8d4c15980591d8a04a5e95ef623c3fb75be94 --- /dev/null +++ b/demo_batchLQR01.m @@ -0,0 +1,147 @@ +function demo_batchLQR01 +% Controller retrieval through a batch solution of linear quadratic optimal control (unconstrained linear MPC), +% by relying on a Gaussian mixture model (GMM) encoding of position and velocity data. +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +nbSamples = 3; %Number of demonstrations +nbRepros = 5; %Number of reproductions +nbData = 100; %Number of datapoints + +model.nbStates = 5; %Number of Gaussians in the GMM +model.nbVarPos = 2; %Dimension of position data (here: x1,x2) +model.nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx]) +model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector +model.rfactor = 1E-6; %Control cost in LQR +model.dt = 0.01; %Time step duration + +%Dynamical System settings (discrete version), see Eq. (33) +A = kron([1, model.dt; 0, 1], eye(model.nbVarPos)); +B = kron([0; model.dt], eye(model.nbVarPos)); +C = kron([1, 0], eye(model.nbVarPos)); +%Control cost matrix +R = eye(model.nbVarPos) * model.rfactor; +R = kron(eye(nbData-1),R); + + +%% Load AMARSI handwriting data +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +load('data/AMARSI/GShape.mat') +Data=[]; +for n=1:nbSamples + s(n).Data=[]; + for m=1:model.nbDeriv + if m==1 + dTmp = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling + else + dTmp = gradient(dTmp) / model.dt; %Compute derivatives + end + s(n).Data = [s(n).Data; dTmp]; + end + Data = [Data s(n).Data]; +end + + +%% Learning +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%model = init_GMM_kmeans(Data,model); + +%Initialization based on position data +model0 = init_GMM_kmeans(Data(1:model.nbVarPos,:), model); +[~, GAMMA2] = EM_GMM(Data(1:model.nbVarPos,:), model0); +model.Priors = model0.Priors; +for i=1:model.nbStates + model.Mu(:,i) = Data * GAMMA2(i,:)'; + DataTmp = Data - repmat(model.Mu(:,i),1,nbData*nbSamples); + model.Sigma(:,:,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp'; +end + +%Refinement of parameters +[model, H] = EM_GMM(Data, model); + +%Set list of states according to first demonstration (alternatively, an HSMM can be used) +[~,qList] = max(H(:,1:nbData),[],1); %works also for nbStates=1 + +%Create single Gaussian N(MuQ,SigmaQ) based on optimal state sequence q, see Eq. (27) +MuQ = reshape(model.Mu(:,qList), model.nbVar*nbData, 1); +SigmaQ = zeros(model.nbVar*nbData); +for t=1:nbData + id = (t-1)*model.nbVar+1:t*model.nbVar; + %MuQ(id,1) = model.Mu(:,qList(t)); + SigmaQ(id,id) = model.Sigma(:,:,qList(t)); +end +%SigmaQ can alternatively be computed with: +%SigmaQ = (kron(ones(nbData,1), eye(model.nbVar)) * reshape(model.Sigma(:,:,qList), model.nbVar, model.nbVar*nbData)) .* kron(eye(nbData), ones(model.nbVar)); + + +%% Batch LQR reproduction +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +Su = zeros(model.nbVar*nbData, model.nbVarPos*(nbData-1)); +Sx = kron(ones(nbData,1),eye(model.nbVar)); +M = B; +for n=2:nbData + %Build Sx matrix, see Eq. (35) + id1 = (n-1)*model.nbVar+1:nbData*model.nbVar; + Sx(id1,:) = Sx(id1,:) * A; + %Build Su matrix, see Eq. (35) + id1 = (n-1)*model.nbVar+1:n*model.nbVar; + id2 = 1:(n-1)*model.nbVarPos; + Su(id1,id2) = M; + M = [A*M(:,1:model.nbVarPos), M]; +end + +%Set matrices to compute the damped weighted least squares estimate, see Eq. (37) +SuInvSigmaQ = Su' / SigmaQ; +Rq = SuInvSigmaQ * Su + R; +%M = (SuInvSigmaQ * Su + R) \ SuInvSigmaQ; +%M = inv(Su' / SigmaQ * Su + R) * Su' /SigmaQ; + +%Reproductions +for n=1:nbRepros + X = Data(:,1) + [randn(model.nbVarPos,1)*2E0; zeros(model.nbVarPos,1)]; + rq = SuInvSigmaQ * (MuQ-Sx*X); + u = Rq \ rq; %Can also be computed with u = lscov(Rq, rq); + %u = M * (MuQ-Sx*X); + r(n).Data = reshape(Sx*X+Su*u, model.nbVar, nbData); + +% %Simulate plant +% r(n).u = reshape(u, model.nbVarPos, nbData-1); +% for t=1:nbData-1 +% %id = (t-1)*model.nbVar+1:t*model.nbVar; +% %r(n).u(:,t) = M(id,id) * (MuQ(id) - X(1:model.nbVar)); +% X = A * X + B * r(n).u(:,t); +% r(n).Data(:,t) = X; +% end +end + + +%% Plot +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +figure('position',[10 10 700 700],'color',[1 1 1]); hold on; axis off; +plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [0.5 0.5 0.5]); +plot(Data(1,:), Data(2,:), 'k.'); +for n=1:nbRepros + plot(r(n).Data(1,:), r(n).Data(2,:), '-','linewidth',2,'color',[.8 0 0]); + plot(r(n).Data(1,:), r(n).Data(2,:), '.','markersize',16,'color',[1 0 0]); +end +axis equal; + +%print('-dpng','graphs/demo_batchLQR01.png'); +%pause; +%close all; + diff --git a/demo_iterativeLQR01.m b/demo_iterativeLQR01.m new file mode 100644 index 0000000000000000000000000000000000000000..b3db7f440d1894d53ca4b26b01a364d7ad20d1c7 --- /dev/null +++ b/demo_iterativeLQR01.m @@ -0,0 +1,115 @@ +function demo_iterativeLQR01 +% Example of controller retrieval through an iterative solution of linear quadratic optimal control (finite horizon, +% unconstrained linear MPC), by relying on a Gaussian mixture model (GMM) encoding of position and velocity data +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } + +addpath('./m_fcts/'); + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +nbSamples = 3; %Number of demonstrations +nbRepros = 5; %Number of reproductions +nbData = 100; %Number of datapoints + +model.nbStates = 5; %Number of Gaussians in the GMM +model.nbVarPos = 2; %Dimension of position data (here: x1,x2) +model.nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx]) +model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector +model.rfactor = 1E-6; %Control cost in LQR +model.dt = 0.01; %Time step duration + +%Dynamical System settings (discrete version) +A = kron([1, model.dt; 0, 1], eye(model.nbVarPos)); +B = kron([0; model.dt], eye(model.nbVarPos)); +C = kron([1, 0], eye(model.nbVarPos)); +%Control cost matrix +R = eye(model.nbVarPos) * model.rfactor; + + +%% Load Data +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +load('data/AMARSI/GShape.mat') +Data=[]; +for n=1:nbSamples + s(n).Data=[]; + for m=1:model.nbDeriv + if m==1 + dTmp = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling + else + dTmp = gradient(dTmp) / model.dt; %Compute derivatives + end + s(n).Data = [s(n).Data; dTmp]; + end + Data = [Data s(n).Data]; +end + + +%% Learning +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%model = init_GMM_kmeans(Data,model); + +%Initialization based on position data +model0 = init_GMM_kmeans(Data(1:model.nbVarPos,:), model); +[~, GAMMA2] = EM_GMM(Data(1:model.nbVarPos,:), model0); +model.Priors = model0.Priors; +for i=1:model.nbStates + model.Mu(:,i) = Data * GAMMA2(i,:)'; + DataTmp = Data - repmat(model.Mu(:,i),1,nbData*nbSamples); + model.Sigma(:,:,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp'; +end + +%Refinement of parameters +[model, H] = EM_GMM(Data, model); + +%Set list of states according to first demonstration (alternatively, an HSMM can be used) +[~,qList] = max(H(:,1:nbData),[],1); %works also for nbStates=1 + + +%% Iterative LQR reproduction (finite horizon) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +P = zeros(model.nbVar,model.nbVar,nbData); +P(:,:,end) = inv(model.Sigma(:,:,qList(nbData))); +for t=nbData-1:-1:1 + Q = inv(model.Sigma(:,:,qList(t))); + P(:,:,t) = Q - A' * (P(:,:,t+1) * B / (B' * P(:,:,t+1) * B + R) * B' * P(:,:,t+1) - P(:,:,t+1)) * A; +end +%Reproduction +for n=1:nbRepros + x = Data(1:model.nbVarPos,1) + randn(model.nbVarPos,1)*2E0; + dx = Data(model.nbVarPos+1:end,1); + for t=1:nbData + K = (B' * P(:,:,t) * B + R) \ B' * P(:,:,t) * A; + ddx = K * (model.Mu(:,qList(t)) - [x; dx]); + dx = dx + ddx * model.dt; + x = x + dx * model.dt; + r(n).Data(:,t) = [x;dx;ddx]; + end +end + + +%% Plot +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +figure('position',[10 10 700 700],'color',[1 1 1]); hold on; axis off; +plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [0.5 0.5 0.5]); +plot(Data(1,:), Data(2,:), 'k.'); +for n=1:nbRepros + plot(r(n).Data(1,:), r(n).Data(2,:), '-','linewidth',2,'color',[.8 0 0]); + plot(r(n).Data(1,:), r(n).Data(2,:), '.','markersize',16,'color',[1 0 0]); +end +axis equal; + +%print('-dpng','graphs/demo_iterativeLQR01.png'); +pause; +close all; + diff --git a/demo_stdPGMM01.m b/demo_stdPGMM01.m index 675377300dc9e175c91aa770e2b5e4e91e732f90..3faf987c5bc0056c89dc43b10da66c53ccb71c10 100644 --- a/demo_stdPGMM01.m +++ b/demo_stdPGMM01.m @@ -1,10 +1,22 @@ function demo_stdPGMM01 -%Example of parametric Gaussian mixture model (PGMM) used as a task-parameterized model, -%with DS-GMR employed to retrieve continuous movements -%Sylvain Calinon, 2015 +% Example of parametric Gaussian mixture model (PGMM) used for task adaptation, +% with DS-GMR employed to retrieve continuous movements +% +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ +% +% 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: +% +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", +% } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbStates = 3; %Number of Gaussians in the GMM @@ -23,8 +35,8 @@ disp('Load motion data...'); % 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 (nbSamples=5). +% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200) +% multiplied by the number of demonstrations (M=5). load('data/DataLQR01.mat'); @@ -38,7 +50,7 @@ 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 data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf +%Compute derivatives Data = []; for n=1:nbSamples DataTmp = s(n).Data0(2:end,:); @@ -51,9 +63,10 @@ end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% fprintf('Parameters estimation of PGMM with EM:'); for n=1:nbSamples - %Task parameters rearranged as a vector (position and orientation), see Eq. (7.1.4) in doc/TechnicalReport.pdf + %Task parameters rearranged as a vector (position and orientation), see Eq. (48) s(n).OmegaMu = [s(n).p(1).b(2:3); s(n).p(1).A(2:3,3); s(n).p(2).b(2:3); s(n).p(2).A(2:3,3); 1]; -% %Task parameters rearranged as a vector (position only), see Eq. (7.1.4) in doc/TechnicalReport.pdf + +% %Task parameters rearranged as a vector (position only), see Eq. (48) % s(n).OmegaMu = [s(n).p(1).b(2:3); s(n).p(2).b(2:3); 1]; end @@ -64,6 +77,7 @@ for i=1:model.nbStates %Initialization of parameters based on standard GMM model.ZMu(:,:,i) = zeros(model.nbVar, size(s(1).OmegaMu,1)); model.ZMu(:,end,i) = model.Mu(:,i); + % %Random initialization of parameters % model.ZMu(:,:,i) = rand(model.nbVar,size(s(1).OmegaMu,1)); % model.Sigma(:,:,i) = eye(model.nbVar); @@ -83,17 +97,17 @@ L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; for n=1:nbSamples %Computation of the resulting Gaussians (for display purpose) for i=1:model.nbStates - model.Mu(:,i) = model.ZMu(:,:,i) * s(n).OmegaMu; %Temporary Mu variable, see Eq. (7.1.4) in doc/TechnicalReport.pdf + model.Mu(:,i) = model.ZMu(:,:,i) * s(n).OmegaMu; %Temporary Mu variable, see Eq. (48) end r(n).Mu = model.Mu; - %Retrieval of attractor path through GMR - currTar = GMR(model, DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf + %Retrieval of attractor path through GMR, see Eq. (17)-(19) + currTar = GMR(model, DataIn, 1, [2:model.nbVar]); %Motion retrieval with spring-damper system x = s(n).p(1).b(2:model.nbVar); dx = zeros(nbVarOut,1); for t=1:s(n).nbData %Compute acceleration, velocity and position - ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf + ddx = -L * [x-currTar(:,t); dx]; dx = dx + ddx * model.dt; x = x + dx * model.dt; r(n).Data(:,t) = x; @@ -113,18 +127,19 @@ for n=1:nbRepros rnew(n).p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2); end - %Task parameters re-arranged as a vector (position and orientation), see Eq. (7.1.4) in doc/TechnicalReport.pdf + %Task parameters re-arranged as a vector (position and orientation), see Eq. (48) rnew(n).OmegaMu = [rnew(n).p(1).b(2:3); rnew(n).p(1).A(2:3,3); rnew(n).p(2).b(2:3); rnew(n).p(2).A(2:3,3); 1]; -% %Task parameters re-arranged as a vector (position only), see Eq. (7.1.4) in doc/TechnicalReport.pdf + +% %Task parameters re-arranged as a vector (position only), see Eq. (48) % rnew(n).OmegaMu = [rnew(n).p(1).b(2:3); rnew(n).p(2).b(2:3); 1]; %Computation of the resulting Gaussians (for display purpose) for i=1:model.nbStates - model.Mu(:,i) = model.ZMu(:,:,i) * rnew(n).OmegaMu; %Temporary Mu variable, see Eq. (7.1.4) in doc/TechnicalReport.pdf + model.Mu(:,i) = model.ZMu(:,:,i) * rnew(n).OmegaMu; %Temporary Mu variable, see Eq. (48) end rnew(n).Mu = model.Mu; - %Retrieval of attractor path through GMR - currTar = GMR(model, DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf + %Retrieval of attractor path through GMR, see Eq. (17)-(19) + currTar = GMR(model, DataIn, 1, [2:model.nbVar]); %Motion retrieval with spring-damper system x = rnew(n).p(1).b(2:model.nbVar); dx = zeros(nbVarOut,1); diff --git a/demo_testLQR01.m b/demo_testLQR01.m index 08e0e0750d42e4e6203b7193ad5613903facb8e8..5562cf2762ca754e0419b6c4b2251a4c2ed2b2aa 100644 --- a/demo_testLQR01.m +++ b/demo_testLQR01.m @@ -1,24 +1,21 @@ function demo_testLQR01 % Test of the linear quadratic regulation with different variance in the data % -% Author: Sylvain Calinon, 2014 -% http://programming-by-demonstration.org/SylvainCalinon +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ % % 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" +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", % } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1) @@ -27,6 +24,7 @@ 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 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% disp('Reproductions with LQR...'); @@ -34,10 +32,12 @@ DataIn = [1:nbData] * model.dt; 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); + %r(n) = reproduction_LQR_finiteHorizon(model, a, 0, rFactor); + r(n) = reproduction_LQR_infiniteHorizon(model, a, 0, rFactor); + r(n).Data = [DataIn; r(n).Data]; end + %% Plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure('position',[20,50,1300,500]); @@ -64,6 +64,7 @@ for n=1:nbRepros end xlabel('t'); ylabel('kp'); +%print('-dpng','graphs/demo_testLQR01.png'); %pause; %close all; diff --git a/demo_testLQR02.m b/demo_testLQR02.m index 787a1d7d72bf6cb8c22169dc4fee50a2a62407d7..93bb4982d1366eb97b3d3fd65352bfb8ee4019eb 100644 --- a/demo_testLQR02.m +++ b/demo_testLQR02.m @@ -1,24 +1,21 @@ function demo_testLQR02 % Test of the linear quadratic regulation (evaluation of the damping ratio found by the system) % -% Author: Sylvain Calinon, 2014 -% http://programming-by-demonstration.org/SylvainCalinon +% Sylvain Calinon, 2015 +% http://programming-by-demonstration.org/lib/ % % 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" +% @article{Calinon15, +% author="Calinon, S.", +% title="A tutorial on task-parameterized movement learning and retrieval", +% year="2015", % } addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1) @@ -27,6 +24,7 @@ nbData = 1000; %Number of datapoints nbRepros = 1; %Number of reproductions with new situations randomly generated rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR + %% Reproduction with LQR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% disp('Reproductions with LQR...'); @@ -34,10 +32,12 @@ DataIn = [1:nbData] * model.dt; a.currTar = ones(1,nbData); a.currSigma = ones(1,1,nbData); %-> LQR with cost X'X + u'u for n=1:nbRepros - %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor); - r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor); + %r(n) = reproduction_LQR_finiteHorizon(model, a, 0, rFactor); + r(n) = reproduction_LQR_infiniteHorizon(model, a, 0, rFactor); + r(n).Data = [DataIn; r(n).Data]; end + %% Plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure('position',[20,50,1300,500]); @@ -72,6 +72,7 @@ for n=1:nbRepros end xlabel('t'); ylabel('Damping ratio'); +%print('-dpng','graphs/demo_testLQR02.png'); %pause; %close all; diff --git a/demo_testLQR03.m b/demo_testLQR03.m index 294ea3183bf2e7f11f7f38f37625ec1fdd891816..165e4567d1f52aef937c1992df0538e28362d47c 100644 --- a/demo_testLQR03.m +++ b/demo_testLQR03.m @@ -19,6 +19,7 @@ function demo_testLQR03 addpath('./m_fcts/'); + %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1) @@ -27,6 +28,7 @@ nbData = 400; %Number of datapoints nbRepros = 2; %Number of reproductions with new situations randomly generated rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR + %% Reproduction with LQR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% disp('Reproductions with LQR...'); @@ -43,12 +45,14 @@ aFinal.currSigma = a.currSigma(:,:,end); for n=1:nbRepros if n==1 - r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor); + r(n) = reproduction_LQR_infiniteHorizon(model, a, 0, rFactor); + r(n).Data = [DataIn; r(n).Data]; else %First call to LQR to get an estimate of the final feedback terms - [~,Pfinal] = reproduction_LQR_infiniteHorizon(DataIn(end), model, aFinal, 0, rFactor); + [~,Pfinal] = reproduction_LQR_infiniteHorizon(model, aFinal, 0, rFactor); %Second call to LQR with finite horizon - r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor, Pfinal); + r(n) = reproduction_LQR_finiteHorizon(model, a, 0, rFactor, Pfinal); + r(n).Data = [DataIn; r(n).Data]; end end for n=1:nbRepros @@ -58,6 +62,7 @@ for n=1:nbRepros end end + %% Plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure('position',[20,50,1300,500]); @@ -96,5 +101,6 @@ for n=1:nbRepros end xlabel('t'); ylabel('kv'); +%print('-dpng','graphs/demo_testLQR03.png'); %pause; %close all; diff --git a/demo_trajGMM01.m b/demo_trajGMM01.m deleted file mode 100644 index 4ec4daaddbe2dc446133c080a31d08a743424c2f..0000000000000000000000000000000000000000 --- a/demo_trajGMM01.m +++ /dev/null @@ -1,189 +0,0 @@ -function demo_trajGMM01 -%Example of trajectory synthesis with a GMM with dynamic features (trajectory GMM) -%Sylvain Calinon, 2015 - -addpath('./m_fcts/'); - -%% Parameters -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -model.nbStates = 5; %Number of components in the GMM -model.nbVarPos = 2; %Dimension of position data (here: x1,x2) -model.nbDeriv = 3; %Number of static&dynamic features (D=2 for [x,dx], D=3 for [x,dx,ddx], etc.) -model.nbVar = model.nbVarPos * model.nbDeriv; -model.dt = 1; %Time step (without rescaling, large values such as 1 has the advantage of creating clusers based on position information) -nbSamples = 4; %Number of demonstrations -nbD = 200; %Number of datapoints in a trajectory - -[PHI,PHI1,T,T1] = constructPHI(model,nbD,nbSamples); %Construct PHI operator (big sparse matrix) - - -%% Load dataset -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -load('data/dataTrajGMM01.mat'); -%load('data/dataTrajGMM02.mat'); %periodic motion -%load('data/dataTrajGMM03.mat'); %motion with options -%load('data/dataTrajGMM04.mat'); %partial demonstrations - -%Resampling of dataset -nbD0 = size(Data,2)/nbSamples; %Original number of datapoints in a trajectory -DataTmp = []; -for n=1:nbSamples - DataTmp = [DataTmp spline(1:nbD0, Data(2:model.nbVarPos+1,(n-1)*nbD0+1:n*nbD0), linspace(1,nbD0,nbD))]; -end -Data = DataTmp; - -%Re-arrange data in vector form -x = reshape(Data(:,1:T), model.nbVarPos*T, 1) * 1E2; %Scale data to avoid numerical computation problem -zeta = PHI*x; %y is for example [x1(1), x2(1), x1d(1), x2d(1), x1(2), x2(2), x1d(2), x2d(2), ...], see Eq. (2.4.5) in doc/TechnicalReport.pdf -Data = reshape(zeta, model.nbVarPos*model.nbDeriv, T); %Include derivatives in Data - - -%% Parameters estimation -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%Initialization with kmeans -model = init_GMM_kmeans(Data, model); - -% %Initialization with homogeneous time intervals -% DataTS = [repmat([1:T1]*model.dt,1,nbSamples); Data]; -% modelTmp = init_GMM_timeBased(DataTS, model); -% model.Mu = modelTmp.Mu(2:end,:); -% model.Sigma = modelTmp.Sigma(2:end,2:end,:); -% model.Priors = modelTmp.Priors; - -%Refinement of parameters -[model, GAMMA2] = EM_GMM(Data, model); -%[model, GAMMA2] = EM_MFA(Data, model); -%[model, GAMMA2] = EM_MPPCA(Data, model); -%[model, GAMMA2] = EM_HDGMM(Data, model); - -%Precomputation of inverses (optional) -for i=1:model.nbStates - model.invSigma(:,:,i) = inv(model.Sigma(:,:,i)); -end - - -%% Reproduction -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -for n=1:1 %nbSamples - %Compute best path for the n-th demonstration - [~,r(n).q] = max(GAMMA2(:,(n-1)*T1+1:n*T1),[],1); %works also for nbStates=1 - - %Create single Gaussian N(MuQ,SigmaQ) based on optimal state sequence q, see Eq. (2.4.8) in doc/TechnicalReport.pdf - MuQ = reshape(model.Mu(:,r(n).q), model.nbVar*T1, 1); - %MuQ = zeros(model.nbVar*T1,1); - SigmaQ = zeros(model.nbVar*T1); - for t=1:T1 - id = (t-1)*model.nbVar+1:t*model.nbVar; - %MuQ(id) = model.Mu(:,r(n).q(t)); - SigmaQ(id,id) = model.Sigma(:,:,r(n).q(t)); - end - %SigmaQ can alternatively be computed with: - %r(n).SigmaQ = (kron(ones(T1,1), eye(model.nbVar)) * reshape(model.Sigma(:,:,r(1).q), model.nbVar, model.nbVar*T1)) .* kron(eye(T1), ones(model.nbVar)); - - - %Least squares computation method 1 (using lscov Matlab function), see Eq. (2.4.11) in doc/TechnicalReport.pdf - %%%%%%%%%%%%%%%%%%% - [zeta,~,mse,S] = lscov(PHI1, MuQ, SigmaQ,'chol'); %Retrieval of data with weighted least squares solution - r(n).Data = reshape(zeta, model.nbVarPos, T1); %Reshape data for plotting - - -% %Least squares computation method 2 (most readable but not optimized), see Eq. (2.4.11) in doc/TechnicalReport.pdf -% %%%%%%%%%%%%%%%%%%% -% PHIinvSigmaQ = PHI1' / SigmaQ; -% Rq = PHIinvSigmaQ * PHI1; -% rq = PHIinvSigmaQ * MuQ; -% zeta = Rq \ rq; %Can also be computed with c = lscov(Rq, rq) -% r(n).Data = reshape(zeta, model.nbVarPos, T1); %Reshape data for plotting -% %Covariance Matrix computation of ordinary least squares estimate, see Eq. (2.4.12) in doc/TechnicalReport.pdf -% mse = (MuQ'*inv(SigmaQ)*MuQ - rq'*inv(Rq)*rq) ./ ((model.nbVar-model.nbVarPos)*T1); -% S = inv(Rq) * mse; - - -% %Least squares computation method 3 (efficient computation using Cholesky and QR decompositions, inspired by lscov code) -% %%%%%%%%%%%%%%%%%%% -% T = chol(SigmaQ)'; %SigmaQ=T*T' -% TA = T \ PHI1; -% TMuQ = T \ MuQ; -% [Q, R, perm] = qr(TA,0); %PHI1(:,perm)=Q*R -% z = Q' * TMuQ; -% zeta(perm,:) = R \ z; %zeta=(TA'*TA)\(TA'*TMuQ), see Eq. (2.4.11) in doc/TechnicalReport.pdf -% r(n).Data = reshape(zeta, model.nbVarPos, T1); %Reshape data for plotting -% %Covariance Matrix computation of ordinary least squares estimate -% err = TMuQ - Q*z; -% mse = err'*err ./ (model.nbVar*T1 - model.nbVarPos*T1); -% Rinv = R \ eye(model.nbVarPos*T1); -% S(perm,perm) = Rinv*Rinv' .* mse; %See Eq. (2.4.12) in doc/TechnicalReport.pdf - - - %Rebuild covariance by reshaping S, see Eq. (2.4.12) in doc/TechnicalReport.pdf - for t=1:T1 - id = (t-1)*model.nbVarPos+1:t*model.nbVarPos; - r(n).expSigma(:,:,t) = S(id,id) * T1; - end - -end %nbSamples - - -%% Plot timeline -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -figure('position',[10 10 600 600]); -for m=1:model.nbVarPos - limAxes = [1, T1, min(Data(m,:))-1E0, max(Data(m,:))+1E0]; - subplot(model.nbVarPos,1,m); hold on; - for n=1:1 %nbSamples - msh=[]; x0=[]; - for t=1:T1-1 - if size(msh,2)==0 - msh(:,1) = [t; model.Mu(m,r(n).q(t))]; - end - if t==T1-1 || r(n).q(t+1)~=r(n).q(t) - msh(:,2) = [t+1; model.Mu(m,r(n).q(t))]; - sTmp = model.Sigma(m,m,r(n).q(t))^.5; - msh2 = [msh(:,1)+[0;sTmp], msh(:,2)+[0;sTmp], msh(:,2)-[0;sTmp], msh(:,1)-[0;sTmp], msh(:,1)+[0;sTmp]]; - patch(msh2(1,:), msh2(2,:), [.85 .85 .85],'edgecolor',[.7 .7 .7]); - plot(msh(1,:), msh(2,:), '-','linewidth',3,'color',[.7 .7 .7]); - plot([msh(1,1) msh(1,1)], limAxes(3:4), ':','linewidth',1,'color',[.7 .7 .7]); - x0 = [x0 msh]; - msh=[]; - end - end - msh = [1:T1, T1:-1:1; r(n).Data(m,:)-squeeze(r(n).expSigma(m,m,:).^.5)'*1, fliplr(r(n).Data(m,:)+squeeze(r(n).expSigma(m,m,:).^.5)'*1)]; - patch(msh(1,:), msh(2,:), [1 .4 .4],'edgecolor',[1 .7 .7],'edgealpha',.8,'facealpha',.8); - end - for n=1:nbSamples - plot(1:T1, Data(m,(n-1)*T1+1:n*T1), '-','lineWidth',2,'color',[.2 .2 .2]); - end - for n=1:1 - plot(1:T1, r(n).Data(m,:), '-','lineWidth',2.5,'color',[.8 0 0]); - end - - set(gca,'xtick',[],'ytick',[]); - xlabel('$t$','interpreter','latex','fontsize',18); - ylabel(['$x_' num2str(m) '$'],'interpreter','latex','fontsize',18); - axis(limAxes); -end - -%% Plot 2D -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -if model.nbVarPos>1 - figure('position',[620 10 600 600]); hold on; - for n=1:1 %nbSamples - plotGMM(r(n).Data([1,2],:), r(n).expSigma([1,2],[1,2],:)*2, [1 .2 .2]); - end - plotGMM(model.Mu([1,2],:), model.Sigma([1,2],[1,2],:)*2, [.5 .5 .5],.8); - for n=1:nbSamples - plot(Data(1,(n-1)*T1+1:n*T1), Data(2,(n-1)*T1+1:n*T1), '-','lineWidth',2,'color',[.2 .2 .2]); %-0.2+0.8*(n-1)/(nbSamples-1) - end - for n=1:1 - plot(r(n).Data(1,:), r(n).Data(2,:), '-','lineWidth',2.5,'color',[.8 0 0]); - end - set(gca,'xtick',[],'ytick',[]); axis equal; axis square; - xlabel(['$x_1$'],'interpreter','latex','fontsize',18); - ylabel(['$x_2$'],'interpreter','latex','fontsize',18); -end - -%print('-dpng','graphs/demo_trajGMM01.png'); -%pause; -%close all; - - diff --git a/m_fcts/DTW.m b/m_fcts/DTW.m new file mode 100644 index 0000000000000000000000000000000000000000..39ccd5e4fa030d0fa430ecce324bf02ec6c1014e --- /dev/null +++ b/m_fcts/DTW.m @@ -0,0 +1,52 @@ +function [x_new, y_new, p] = DTW(x, y, w) +%Trajectory realignment through dynamic time warping +%Sylvain Calinon, 2015 + +if nargin<3 + w = Inf; +end + +nx = size(x,2); +ny = size(y,2); + +w = max(w,abs(nx-ny)); + +%Initialization +D = ones(nx+1,ny+1) * Inf; +D(1,1) = 0; + +%DP loop +for i=1:nx + for j=max(i-w,1):min(i+w,ny) + D(i+1,j+1) = norm(x(:,i)-y(:,j)) + min([D(i,j+1), D(i+1,j), D(i,j)]); + end +end + +i=nx+1; j=ny+1; +p=[]; +while i>1 && j>1 + [~,id] = min([D(i,j-1), D(i-1,j), D(i-1,j-1)]); + if id==1 + j=j-1; + elseif id==2 + i=i-1; + else + i=i-1; + j=j-1; + end + p = [p [i;j]]; +end + +p = fliplr(p(:,1:end-1)-1); + +x_new = x(:,p(1,:)); +y_new = y(:,p(2,:)); + +%Resampling +x_new = spline(1:size(x_new,2), x_new, linspace(1,size(x_new,2),nx)); +y_new = spline(1:size(y_new,2), y_new, linspace(1,size(y_new,2),nx)); + + + + + diff --git a/m_fcts/EM_GMM.m b/m_fcts/EM_GMM.m index 5492118cd25b0131a93810804f76cd2d7f316c6b..8d9f60ed7b3fb7fd810af7f173f62a6cfaf32084 100644 --- a/m_fcts/EM_GMM.m +++ b/m_fcts/EM_GMM.m @@ -30,7 +30,7 @@ for nbIter=1:nbMaxSteps %Update Sigma, see Eq. (2.0.8) in doc/TechnicalReport.pdf (regularization term is optional, see Eq. (2.1.2)) DataTmp = Data - repmat(model.Mu(:,i),1,nbData); - model.Sigma(:,:,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar) * diagRegularizationFactor; + model.Sigma(:,:,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(size(Data,1)) * diagRegularizationFactor; end %Compute average log-likelihood diff --git a/m_fcts/EM_tensorGMM.m b/m_fcts/EM_tensorGMM.m index 15073fa59f606e49300d1e6e0a1f1e6b10962de1..8e7d31547d5d869bed4882317633d51ecd2d4ac8 100644 --- a/m_fcts/EM_tensorGMM.m +++ b/m_fcts/EM_tensorGMM.m @@ -1,4 +1,4 @@ -function [model, GAMMA0] = EM_tensorGMM(Data, model) +function [model, GAMMA0, GAMMA2] = EM_tensorGMM(Data, model) % Training of a task-parameterized Gaussian mixture model (GMM) with an expectation-maximization (EM) algorithm. % The approach allows the modulation of the centers and covariance matrices of the Gaussians with respect to % external parameters represented in the form of candidate coordinate systems. @@ -51,7 +51,7 @@ for nbIter=1:nbMaxSteps %Update Sigma (regularization term is optional), see Eq. (6.0.4) in doc/TechnicalReport.pdf DataTmp = DataMat - repmat(model.Mu(:,m,i),1,nbData); - model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar) * diagRegularizationFactor; + model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(size(DataTmp,1)) * diagRegularizationFactor; end end diff --git a/m_fcts/GPR.m b/m_fcts/GPR.m index 0044dd338624f41d8c317c896fad3710c640e3e9..7c5dd069e9f56f102117fc49c4b3f79caf60d845 100644 --- a/m_fcts/GPR.m +++ b/m_fcts/GPR.m @@ -33,7 +33,7 @@ yd = (Kd * invK * y')'; % + repmat(ymean,1,size(qd,2)); if nargout>1 SigmaOut = zeros(size(yd,1), size(yd,1), size(yd,2)); -% %Evaluate Sigma (as in Rasmussen 2006) +% %Evaluate Sigma (as in Rasmussen, 2006) % Mdd = pdist2(qd',qd'); % Kdd = exp(-Mdd.^2); % S = Kdd - Kd * invK * Kd'; @@ -41,7 +41,7 @@ if nargout>1 % SigmaOut(:,:,t) = eye(size(yd,1)) * S(t,t); % end - %Evaluate Sigma + %Evaluate Sigma (as in GMR) %nbSamples = size(y,2) / size(yd,2); %yd = repmat(yd,1,nbSamples); for t=1:size(yd,2) diff --git a/m_fcts/constructPHI.m b/m_fcts/constructPHI.m index a16a87f11160a1b3e930618f5c8720633eff9c32..d8a0f5c8eca8cab6441d1766fd0c7d55a4461d60 100644 --- a/m_fcts/constructPHI.m +++ b/m_fcts/constructPHI.m @@ -1,4 +1,4 @@ -function [PHI,PHI1,T,T1] = constructPHI(model,nbD,nbSamples) +function [PHI,PHI1,PHI0,T,T1] = constructPHI(model,nbD,nbSamples) %Construct PHI operator (big sparse matrix) used in trajectory-GMM, see Eq. (2.4.5) in doc/TechnicalReport.pdf %Sylvain Calinon, 2015 diff --git a/m_fcts/gaussPDF.m b/m_fcts/gaussPDF.m index 4bc34cb96d7c5fbd805514f2dd4a342ca8d48af5..38e6f464e46b8b99e9f39268aa4a88d2e93098e4 100644 --- a/m_fcts/gaussPDF.m +++ b/m_fcts/gaussPDF.m @@ -15,4 +15,4 @@ function prob = gaussPDF(Data, Mu, Sigma) %See Eq. (2.0.3) in doc/TechnicalReport.pdf Data = Data' - repmat(Mu',nbData,1); prob = sum((Data/Sigma).*Data, 2); -prob = exp(-0.5*prob) / sqrt((2*pi)^nbVar * (abs(det(Sigma))+realmin)); +prob = exp(-0.5*prob) / sqrt((2*pi)^nbVar * abs(det(Sigma)) + realmin); diff --git a/m_fcts/init_GMM_timeBased.m b/m_fcts/init_GMM_timeBased.m index 0677312ac265ebf01309e948adb1b8086d5a0ca7..574e8537537391bf0a9356a1e61d6d490750f257 100644 --- a/m_fcts/init_GMM_timeBased.m +++ b/m_fcts/init_GMM_timeBased.m @@ -22,7 +22,8 @@ function model = init_GMM_timeBased(Data, model) % Switzerland, http://lasa.epfl.ch [nbVar, nbData] = size(Data); -diagRegularizationFactor = 1E-2; +%diagRegularizationFactor = 1E-2; +diagRegularizationFactor = 1E-8; TimingSep = linspace(min(Data(1,:)), max(Data(1,:)), model.nbStates+1); diff --git a/m_fcts/init_tensorGMM_kmeans.m b/m_fcts/init_tensorGMM_kmeans.m index c0ea9ff61817d892d0ee0a0851be6358010ce5c2..50ec07ef95042e4fff9292eb6d969550da8ca831 100644 --- a/m_fcts/init_tensorGMM_kmeans.m +++ b/m_fcts/init_tensorGMM_kmeans.m @@ -5,6 +5,7 @@ function model = init_tensorGMM_kmeans(Data, model) diagRegularizationFactor = 1E-4; +nbVar = size(Data,1); DataAll = reshape(Data, size(Data,1)*size(Data,2), size(Data,3)); %Matricization/flattening of tensor %k-means clustering @@ -20,8 +21,8 @@ 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); + model.Mu(:,m,i) = Mu((m-1)*nbVar+1:m*nbVar,i); + model.Sigma(:,:,m,i) = Sigma((m-1)*nbVar+1:m*nbVar,(m-1)*nbVar+1:m*nbVar,i); end end diff --git a/m_fcts/kmeansClustering.m b/m_fcts/kmeansClustering.m index f69d560d3d0f8310c85e7140347eb4545192e803..4483814fc658bff18b2d3e513c8ab6b405eef1c9 100644 --- a/m_fcts/kmeansClustering.m +++ b/m_fcts/kmeansClustering.m @@ -8,7 +8,7 @@ cumdist_threshold = 1e-10; maxIter = 100; %Initialization of the parameters -[nbVar, nbData] = size(Data); +[~, nbData] = size(Data); cumdist_old = -realmax; nbStep = 0; @@ -35,10 +35,10 @@ while 1 end cumdist_old = cumdist; nbStep = nbStep+1; - % if nbStep>maxIter - % disp(['Maximum number of iterations, ' num2str(maxIter) 'is reached']); - % break; - % end + if nbStep>maxIter + disp(['Maximum number of kmeans iterations, ' num2str(maxIter) 'is reached']); + break; + end end %disp(['Kmeans stopped after ' num2str(nbStep) ' steps.']); diff --git a/m_fcts/plot2DArrow.m b/m_fcts/plot2DArrow.m index a39b3a019a29408ff70dd032826eb7254e16f7b7..aba232896c0cf32025746fbbfbb6e4c0e00cef92 100644 --- a/m_fcts/plot2DArrow.m +++ b/m_fcts/plot2DArrow.m @@ -2,7 +2,7 @@ function h = plot2DArrow(pos,dir,col) %Sylvain Calinon, 2015 h(1) = plot([pos(1) pos(1)+dir(1)], [pos(2) pos(2)+dir(2)], '-','linewidth',2,'color',col); -sz = 8E-2; +sz = 1E-1; pos = pos+dir; if norm(dir)>sz dir = dir/norm(dir); diff --git a/m_fcts/plotGMM.m b/m_fcts/plotGMM.m index 68e9a79535ad7e371c9e435f0041fe1728a62264..faebe32f2d877537131ea670639fc8ca3207f82a 100644 --- a/m_fcts/plotGMM.m +++ b/m_fcts/plotGMM.m @@ -11,7 +11,7 @@ function h = plotGMM(Mu, Sigma, color, valAlpha) nbStates = size(Mu,2); nbDrawingSeg = 35; -lightcolor = min(color+0.5,1); +darkcolor = color*0.5; %max(color-0.5,0); t = linspace(-pi, pi, nbDrawingSeg); h=[]; @@ -21,11 +21,12 @@ for i=1:nbStates R = real(V*D.^.5); X = R * [cos(t); sin(t)] + repmat(Mu(:,i), 1, nbDrawingSeg); if nargin>3 %Plot with alpha transparency - h = [h patch(X(1,:), X(2,:), lightcolor, 'lineWidth', 1, 'EdgeColor', color, 'facealpha', valAlpha,'edgealpha', valAlpha)]; - MuTmp = [cos(t); sin(t)] * 0.006 + repmat(Mu(:,i),1,nbDrawingSeg); - h = [h patch(MuTmp(1,:), MuTmp(2,:), color, 'LineStyle', 'none', 'facealpha', valAlpha)]; + h = [h patch(X(1,:), X(2,:), color, 'lineWidth', 1, 'EdgeColor', darkcolor, 'facealpha', valAlpha,'edgealpha', valAlpha)]; + %MuTmp = [cos(t); sin(t)] * 0.3 + repmat(Mu(:,i),1,nbDrawingSeg); + %h = [h patch(MuTmp(1,:), MuTmp(2,:), darkcolor, 'LineStyle', 'none', 'facealpha', valAlpha)]; + h = [h plot(Mu(1,:), Mu(2,:), '.', 'markersize', 6, 'color', darkcolor)]; else %Plot without transparency - h = [h patch(X(1,:), X(2,:), lightcolor, 'lineWidth', 1, 'EdgeColor', color)]; - h = [h plot(Mu(1,:), Mu(2,:), '.', 'lineWidth', 2, 'markersize', 6, 'color', color)]; + h = [h patch(X(1,:), X(2,:), color, 'lineWidth', 1, 'EdgeColor', darkcolor)]; + h = [h plot(Mu(1,:), Mu(2,:), '.', 'markersize', 6, 'color', darkcolor)]; end end diff --git a/m_fcts/reproduction_LQR_finiteHorizon.m b/m_fcts/reproduction_LQR_finiteHorizon.m index deb647ab929741693285a43078299f3c00edd34b..8808bcd41b7dc77a88284a158dc8b1e0d518d164 100644 --- a/m_fcts/reproduction_LQR_finiteHorizon.m +++ b/m_fcts/reproduction_LQR_finiteHorizon.m @@ -1,4 +1,4 @@ -function r = reproduction_LQR_finiteHorizon(DataIn, model, r, currPos, rFactor, Pfinal) +function r = reproduction_LQR_finiteHorizon(model, r, currPos, rFactor, Pfinal) % Reproduction with a linear quadratic regulator of finite horizon % % Authors: Sylvain Calinon and Danilo Bruno, 2014 @@ -17,8 +17,7 @@ function r = reproduction_LQR_finiteHorizon(DataIn, model, r, currPos, rFactor, % pages="3339--3344" % } -nbData = size(DataIn,2); -nbVarOut = model.nbVar - size(DataIn,1); +[nbVarOut,nbData] = size(r.currTar); %% LQR with cost = sum_t X(t)' Q(t) X(t) + u(t)' R u(t) (See Eq. (5.0.2) in doc/TechnicalReport.pdf) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -46,17 +45,10 @@ L = zeros(nbVarOut, nbVarOut*2, nbData); %Compute P_T from the desired final feedback gains L_T, P(:,:,nbData) = Pfinal; -%Compute derivative of target path -%dTar = diff(r.currTar, 1, 2); %For optional feedforward term computation - %Variables for feedforward term computation (optional for movements with low dynamics) +%tar = [r.currTar; gradient(r.currTar,1,2)/model.dt]; tar = [r.currTar; zeros(nbVarOut,nbData)]; dtar = gradient(tar,1,2)/model.dt; -%tar = [r.currTar; gradient(r.currTar,1,2)/model.dt]; -%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; - %Backward integration of the Riccati equation and additional equation for t=nbData-1:-1:1 @@ -74,19 +66,20 @@ 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]; + %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); %See Eq. (5.1.30) in doc/TechnicalReport.pdf + ddx = -L(:,:,t) * [x-r.currTar(:,t); dx] + M(:,t); %See Eq. (5.1.30) in doc/TechnicalReport.pdf %Update velocity and position dx = dx + ddx * model.dt; x = x + dx * model.dt; %Log data (with additional variables collected for analysis purpose) - r.Data(:,t) = [DataIn(:,t); x]; + r.Data(:,t) = x; r.ddxNorm(t) = norm(ddx); %r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx]; %r.FF(:,t) = M(:,t); @@ -97,5 +90,3 @@ for t=1:nbData %Note that if [V,D] = eigs(L(:,1:nbVarOut)), we have L(:,nbVarOut+1:end) = V * (2*D).^.5 * V' end - - diff --git a/m_fcts/reproduction_LQR_infiniteHorizon.m b/m_fcts/reproduction_LQR_infiniteHorizon.m index 2037c6a127d3bbd530001c1de7086799bd0a0059..5a6fd5e35d3c359d420760c565c037bd12efef63 100644 --- a/m_fcts/reproduction_LQR_infiniteHorizon.m +++ b/m_fcts/reproduction_LQR_infiniteHorizon.m @@ -1,4 +1,4 @@ -function [r,P] = reproduction_LQR_infiniteHorizon(DataIn, model, r, currPos, rFactor) +function [r,P] = reproduction_LQR_infiniteHorizon(model, r, currPos, rFactor) % Reproduction with a linear quadratic regulator of infinite horizon % % Authors: Sylvain Calinon and Danilo Bruno, 2014 @@ -17,8 +17,7 @@ function [r,P] = reproduction_LQR_infiniteHorizon(DataIn, model, r, currPos, rFa % pages="3339--3344" % } -nbData = size(DataIn,2); -nbVarOut = model.nbVar - size(DataIn,1); +[nbVarOut,nbData] = size(r.currTar); %% LQR with cost = sum_t X(t)' Q(t) X(t) + u(t)' R u(t) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -65,17 +64,17 @@ for t=1:nbData M = R\B'*d; %See Eq. (5.1.30) in doc/TechnicalReport.pdf %Compute acceleration (with only feedback terms) - %ddx = -L * [x-r.currTar(:,t); dx]; + ddx = -L * [x-r.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 + %ddx = -L * [x-r.currTar(:,t); dx] + M; %See Eq. (5.1.30) in doc/TechnicalReport.pdf %Update velocity and position dx = dx + ddx * model.dt; x = x + dx * model.dt; %Log data (with additional variables collected for analysis purpose) - r.Data(:,t) = [DataIn(:,t); x]; + r.Data(:,t) = x; r.ddxNorm(t) = norm(ddx); %r.FB(:,t) = -L * [x-r.currTar(:,t); dx]; %r.FF(:,t) = M;