### Comments added.

parent 10deffa2
 function r = reproduction_LQR_finiteHorizon(DataIn, model, r, currPos, rFactor, Pfinal) % Reproduction with a linear quadratic regulator of finite horizon % % 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" % } nbData = size(DataIn,2); nbVarOut = model.nbVar - size(DataIn,1); %% LQR with cost = sum_t X(t)' Q(t) X(t) + u(t)' R u(t) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Definition of a double integrator system (DX = A X + B u with X = [x; dx]) A = kron([0 1; 0 0], eye(nbVarOut)); B = kron([0; 1], eye(nbVarOut)); %Initialize Q and R weighting matrices Q = zeros(nbVarOut*2,nbVarOut*2,nbData); for t=1:nbData Q(1:nbVarOut,1:nbVarOut,t) = inv(r.currSigma(:,:,t)); end R = eye(nbVarOut) * rFactor; if nargin<6 %Pfinal = B*R*[eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV] Pfinal = B*R*[eye(nbVarOut)*0, eye(nbVarOut)*0]; %final feedback terms (boundary conditions) end %Auxiliary variables to minimize the cost function P = zeros(nbVarOut*2, nbVarOut*2, nbData); d = zeros(nbVarOut*2, nbData); %For optional feedforward term computation %Feedback term 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; 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 P(:,:,t) = P(:,:,t+1) + model.dt * (A'*P(:,:,t+1) + P(:,:,t+1)*A - P(:,:,t+1)*B*(R\B')*P(:,:,t+1) + Q(:,:,t+1)); %Optional feedforward term computation d(:,t) = d(:,t+1) + model.dt * ((A'-P(:,:,t+1)*B*(R\B'))*d(:,t+1) + P(:,:,t+1)*dtar(:,t+1) - P(:,:,t+1)*A*tar(:,t+1)); end %Computation of the feedback term L and feedforward term M in u=-LX+M for t=1:nbData L(:,:,t) = R\B' * P(:,:,t); M(:,t) = R\B' * d(:,t); %Optional feedforward term computation end %% Reproduction with varying impedance parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% x = currPos; dx = zeros(nbVarOut,1); for t=1:nbData %Compute acceleration (with only feedback term) %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); r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx]; r.FF(:,t) = M(:,t); %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,t)); r.kvDet(t) = det(L(:,nbVarOut+1:end,t)); end function r = reproduction_LQR_finiteHorizon(DataIn, model, r, currPos, rFactor, Pfinal) % Reproduction with a linear quadratic regulator of finite horizon % % 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" % } nbData = size(DataIn,2); nbVarOut = model.nbVar - size(DataIn,1); %% 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) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Definition of a double integrator system (DX = A X + B u with X = [x; dx]) A = kron([0 1; 0 0], eye(nbVarOut)); %See Eq. (5.1.1) in doc/TechnicalReport.pdf B = kron([0; 1], eye(nbVarOut)); %See Eq. (5.1.1) in doc/TechnicalReport.pdf %Initialize Q and R weighting matrices Q = zeros(nbVarOut*2,nbVarOut*2,nbData); for t=1:nbData Q(1:nbVarOut,1:nbVarOut,t) = inv(r.currSigma(:,:,t)); end R = eye(nbVarOut) * rFactor; if nargin<6 %Pfinal = B*R*[eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV] Pfinal = B*R*[eye(nbVarOut)*0, eye(nbVarOut)*0]; %final feedback terms (boundary conditions) end %Auxiliary variables to minimize the cost function P = zeros(nbVarOut*2, nbVarOut*2, nbData); d = zeros(nbVarOut*2, nbData); %For optional feedforward term computation %Feedback term 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; 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 P(:,:,t) = P(:,:,t+1) + model.dt * (A'*P(:,:,t+1) + P(:,:,t+1)*A - P(:,:,t+1)*B*(R\B')*P(:,:,t+1) + Q(:,:,t+1)); %See Eq. (5.1.11) in doc/TechnicalReport.pdf %Optional feedforward term computation d(:,t) = d(:,t+1) + model.dt * ((A'-P(:,:,t+1)*B*(R\B'))*d(:,t+1) + P(:,:,t+1)*dtar(:,t+1) - P(:,:,t+1)*A*tar(:,t+1)); %See Eq. (5.1.29) in doc/TechnicalReport.pdf end %Computation of the feedback term L and feedforward term M in u=-LX+M for t=1:nbData L(:,:,t) = R\B' * P(:,:,t); %See Eq. (5.1.30) in doc/TechnicalReport.pdf M(:,t) = R\B' * d(:,t); %Optional feedforward term computation (See Eq. (5.1.30) in doc/TechnicalReport.pdf) end %% Reproduction with varying impedance parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% x = currPos; dx = zeros(nbVarOut,1); for t=1:nbData %Compute acceleration (with only feedback term) %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 r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx]; r.FF(:,t) = M(:,t); %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,t)); r.kvDet(t) = det(L(:,nbVarOut+1:end,t)); end
 function [r,P] = reproduction_LQR_infiniteHorizon(DataIn, model, r, currPos, rFactor) % Reproduction with a linear quadratic regulator of infinite horizon % % 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" % } nbData = size(DataIn,2); nbVarOut = model.nbVar - size(DataIn,1); %% LQR with cost = sum_t X(t)' Q(t) X(t) + u(t)' R u(t) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Definition of a double integrator system (DX = A X + B u with X = [x; dx]) A = kron([0 1; 0 0], eye(nbVarOut)); B = kron([0; 1], eye(nbVarOut)); %Initialize Q and R weighting matrices Q = zeros(nbVarOut*2,nbVarOut*2); R = eye(nbVarOut) * rFactor; %Variables for feedforward term computation (optional for movements with low dynamics) %tar = [r.currTar; gradient(r.currTar,1,2)/model.dt]; %dtar = gradient(tar,1,2)/model.dt; 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, %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); %Alternatively, the function below can be used for an implementation based on Eigendecomposition P = solveAlgebraicRiccati_eig(A, B/R*B', (Q+Q')/2); %Variable for feedforward term computation (optional for movements with low dynamics) d = (P*B*(R\B')-A') \ (P*dtar(:,t) - P*A*tar(:,t)); %Feedback term L = R\B'*P; %Feedforward term M = R\B'*d; %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; 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 function [r,P] = reproduction_LQR_infiniteHorizon(DataIn, model, r, currPos, rFactor) % Reproduction with a linear quadratic regulator of infinite horizon % % 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" % } nbData = size(DataIn,2); nbVarOut = model.nbVar - size(DataIn,1); %% LQR with cost = sum_t X(t)' Q(t) X(t) + u(t)' R u(t) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Definition of a double integrator system (DX = A X + B u with X = [x; dx]) A = kron([0 1; 0 0], eye(nbVarOut)); %See Eq. (5.1.1) in doc/TechnicalReport.pdf B = kron([0; 1], eye(nbVarOut)); %See Eq. (5.1.1) in doc/TechnicalReport.pdf %Initialize Q and R weighting matrices Q = zeros(nbVarOut*2,nbVarOut*2); R = eye(nbVarOut) * rFactor; %Variables for feedforward term computation (optional for movements with low dynamics) %tar = [r.currTar; gradient(r.currTar,1,2)/model.dt]; %dtar = gradient(tar,1,2)/model.dt; 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, %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); %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)); end
 ... ... @@ -11,8 +11,8 @@ function X = solveAlgebraicRiccati_Schur(A, G, Q) %} n = size(A,1); Z = [A -G; -Q -A']; Z = [A -G; -Q -A']; %See Eq. (5.2.3) in doc/TechnicalReport.pdf [U0, S0] = schur(Z); U = ordschur(U0, S0, 'lhp'); %Not yet available in GNU Octave X = U(n+1:end,1:n) / U(1:n,1:n); [U0, S0] = schur(Z); %See Eq. (5.2.6) in doc/TechnicalReport.pdf U = ordschur(U0, S0, 'lhp'); %Not yet available in GNU Octave (See Eq. (5.2.6) in doc/TechnicalReport.pdf) X = U(n+1:end,1:n) / U(1:n,1:n); %See Eq. (5.2.7) in doc/TechnicalReport.pdf
 ... ... @@ -3,9 +3,9 @@ function X = solveAlgebraicRiccati_eig(A, G, Q) %Danilo Bruno, 2014 n = size(A,1); Z = [A -G; -Q -A']; Z = [A -G; -Q -A']; %See Eq. (5.2.3) in doc/TechnicalReport.pdf [V,D] = eig(Z); [V,D] = eig(Z); %See Eq. (5.2.4) in doc/TechnicalReport.pdf U = []; for j=1:2*n if real(D(j,j)) < 0 ... ... @@ -13,5 +13,5 @@ for j=1:2*n end end X = U(n+1:end,1:n) / U(1:n,1:n); X = U(n+1:end,1:n) / U(1:n,1:n); %See Eq. (5.2.5) in doc/TechnicalReport.pdf X = real(X); \ No newline at end of file
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