Commit bc2e5a26 authored by Milad Malekzadeh's avatar Milad Malekzadeh

Tab spacing (2 chars).

parent 68cb8b63
# pbdlib-matlab
### Compatibility
The codes have been tested with both Matlab and GNU Octave.
### Usage
Unzip the file and run 'demo_TPGMR_LQR01' (finite horizon LQR), 'demo_TPGMR_LQR02' (infinite horizon LQR) or
'demo_DSGMR01' (dynamical system with constant gains) in Matlab.
'demo_testLQR01', 'demo_testLQR02' and 'demo_testLQR03' can also be run as additional examples of LQR.
### Reference
Calinon, S., Bruno, D. and Caldwell, D.G. (2014). A task-parameterized probabilistic model with minimal intervention
control. Proc. of the IEEE Intl Conf. on Robotics and Automation (ICRA).
### Description
Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper systems
acting in multiple frames of reference. Each candidate coordinate system observes a set of demonstrations from its own
perspective, by extracting an attractor path whose variations depend on the relevance of the frame through the task.
This information is exploited to generate a new attractor path corresponding to new situations (new positions and
orientation of the frames), while the predicted covariances are exploited by a linear quadratic regulator (LQR) to
estimate the stiffness and damping feedback terms of the spring-damper systems, resulting in a minimal intervention
control strategy.
### Authors
Sylvain Calinon and Danilo Bruno, 2014
http://programming-by-demonstration.org/
This source code is given for free! In exchange, we would be grateful if you cite the following reference in any
academic publication that uses this code or part of it:
@inproceedings{Calinon14ICRA,
author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
title="A 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"
}
# pbdlib-matlab
### Compatibility
The codes have been tested with both Matlab and GNU Octave.
### Usage
Unzip the file and run 'demo_TPGMR_LQR01' (finite horizon LQR), 'demo_TPGMR_LQR02' (infinite horizon LQR) or
'demo_DSGMR01' (dynamical system with constant gains) in Matlab.
'demo_testLQR01', 'demo_testLQR02' and 'demo_testLQR03' can also be run as additional examples of LQR.
### Reference
Calinon, S., Bruno, D. and Caldwell, D.G. (2014). A task-parameterized probabilistic model with minimal intervention
control. Proc. of the IEEE Intl Conf. on Robotics and Automation (ICRA).
### Description
Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper systems
acting in multiple frames of reference. Each candidate coordinate system observes a set of demonstrations from its own
perspective, by extracting an attractor path whose variations depend on the relevance of the frame through the task.
This information is exploited to generate a new attractor path corresponding to new situations (new positions and
orientation of the frames), while the predicted covariances are exploited by a linear quadratic regulator (LQR) to
estimate the stiffness and damping feedback terms of the spring-damper systems, resulting in a minimal intervention
control strategy.
### Authors
Sylvain Calinon and Danilo Bruno, 2014
http://programming-by-demonstration.org/
This source code is given for free! In exchange, we would be grateful if you cite the following reference in any
academic publication that uses this code or part of it:
@inproceedings{Calinon14ICRA,
author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
title="A 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"
}
......@@ -7,16 +7,16 @@ function [Mu, Sigma] = productTPGMM(model, p)
% TP-GMM products
%See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
for i = 1:model.nbStates
% Reallocating
SigmaTmp = zeros(model.nbVar);
MuTmp = zeros(model.nbVar,1);
% Product of Gaussians
for m = 1 : model.nbFrames
MuP = p(m).A * model.Mu(:,m,i) + p(m).b;
SigmaP = p(m).A * model.Sigma(:,:,m,i) * p(m).A';
SigmaTmp = SigmaTmp + inv(SigmaP);
MuTmp = MuTmp + SigmaP\MuP;
end
Sigma(:,:,i) = inv(SigmaTmp);
Mu(:,i) = Sigma(:,:,i) * MuTmp;
% Reallocating
SigmaTmp = zeros(model.nbVar);
MuTmp = zeros(model.nbVar,1);
% Product of Gaussians
for m = 1 : model.nbFrames
MuP = p(m).A * model.Mu(:,m,i) + p(m).b;
SigmaP = p(m).A * model.Sigma(:,:,m,i) * p(m).A';
SigmaTmp = SigmaTmp + inv(SigmaP);
MuTmp = MuTmp + SigmaP\MuP;
end
Sigma(:,:,i) = inv(SigmaTmp);
Mu(:,i) = Sigma(:,:,i) * MuTmp;
end
......@@ -35,49 +35,49 @@ R = eye(nbVarOut) * rFactor;
tar = [r.currTar; zeros(nbVarOut,nbData)];
dtar = gradient(tar,1,2)/model.dt;
%% Reproduction with varying impedance parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x = currPos;
dx = zeros(nbVarOut,1);
for t=1:nbData
%Current weighting term
Q(1:nbVarOut,1:nbVarOut) = inv(r.currSigma(:,:,t));
%care() is a function from the Matlab Control Toolbox to solve the algebraic Riccati equation,
Q(1:nbVarOut,1:nbVarOut) = inv(r.currSigma(:,:,t));
%care() is a function from the Matlab Control Toolbox to solve the algebraic Riccati equation,
%also available with the control toolbox of GNU Octave
%P = care(A, B, (Q+Q')/2, R); %(Q+Q')/2 is used instead of Q to avoid warnings for the symmetry of Q
%Alternatively, the function below can be used for an implementation based on Schur decomposition
%P = solveAlgebraicRiccati_Schur(A, B/R*B', (Q+Q')/2);
%P = care(A, B, (Q+Q')/2, R); %(Q+Q')/2 is used instead of Q to avoid warnings for the symmetry of Q
%Alternatively, the function below can be used for an implementation based on Schur decomposition
%P = solveAlgebraicRiccati_Schur(A, B/R*B', (Q+Q')/2);
%Alternatively, the function below can be used for an implementation based on Eigendecomposition
P = solveAlgebraicRiccati_eig(A, B/R*B', (Q+Q')/2); %See Eq. (5.1.27) and Sec. (5.2) in doc/TechnicalReport.pdf
%Variable for feedforward term computation (optional for movements with low dynamics)
d = (P*B*(R\B')-A') \ (P*dtar(:,t) - P*A*tar(:,t)); %See Eq. (5.1.28) in doc/TechnicalReport.pdf
%Feedback term
L = R\B'*P; %See Eq. (5.1.30) in doc/TechnicalReport.pdf
%Feedforward term
M = R\B'*d; %See Eq. (5.1.30) in doc/TechnicalReport.pdf
%Compute acceleration (with only feedback terms)
%ddx = -L * [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
r.FB(:,t) = -L * [x-r.currTar(:,t); dx];
r.FF(:,t) = M;
%Update velocity and position
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
%Log data
r.Data(:,t) = [DataIn(:,t); x];
r.ddxNorm(t) = norm(ddx);
r.kpDet(t) = det(L(:,1:nbVarOut));
r.kvDet(t) = det(L(:,nbVarOut+1:end));
%Variable for feedforward term computation (optional for movements with low dynamics)
d = (P*B*(R\B')-A') \ (P*dtar(:,t) - P*A*tar(:,t)); %See Eq. (5.1.28) in doc/TechnicalReport.pdf
%Feedback term
L = R\B'*P; %See Eq. (5.1.30) in doc/TechnicalReport.pdf
%Feedforward term
M = R\B'*d; %See Eq. (5.1.30) in doc/TechnicalReport.pdf
%Compute acceleration (with only feedback terms)
%ddx = -L * [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
r.FB(:,t) = -L * [x-r.currTar(:,t); dx];
r.FF(:,t) = M;
%Update velocity and position
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
%Log data
r.Data(:,t) = [DataIn(:,t); x];
r.ddxNorm(t) = norm(ddx);
r.kpDet(t) = det(L(:,1:nbVarOut));
r.kvDet(t) = det(L(:,nbVarOut+1:end));
end
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment