Commit 55931ef5 by Sylvain CALINON

New demo_manipulabilityTransfer examples added

parent a4047af2
......@@ -107,6 +107,21 @@ Did you find PbDLib useful for your research? Please acknowledge the authors in
}
```
#### Ref. [7]
**Manipulability ellispoids**<br>
[Link to publication](http://calinon.ch/papers/Rozo-IROS2017.pdf)
```
@inproceedings{Rozo17IROS,
author="Rozo, L. and Jaquier, N. and Calinon, S. and Caldwell, D. G.",
title="Learning Manipulability Ellipsoids for Task Compatibility in Robot Manipulation",
booktitle=IROS,
year="2017",
month="September",
address="Vancouver, Canada",
pages=""
}
```
### List of examples
All the examples are located in the main folder, and the functions are located in the `m_fcts` folder.
......@@ -159,6 +174,9 @@ All the examples are located in the main folder, and the functions are located i
| [demo_iterativeLQR_augmSigma_online01](./demos/demo_iterativeLQR_augmSigma_online01.m) | [[1]](#ref-1) | Same as demo_iterativeLQR_augmSigma01 but recomputed in an online manner |
| [demo_LQR_infHor01](./demos/demo_LQR_infHor01.m) | [[1]](#ref-1) | Continuous infinite horizon linear quadratic tracking, by relying on a GMM encoding of position and velocity data |
| [demo_LQR_infHor02](./demos/demo_LQR_infHor02.m) | [[1]](#ref-1) | Discrete infinite horizon linear quadratic tracking, by relying on a GMM encoding of position and velocity data |
| [demo_manipulabilityTransfer01](./demos/demo_manipulabilityTransfer01.m) | [[7]](#ref-7) | Use of robot redundancy to track desired manipulability ellipsoid |
| [demo_manipulabilityTransfer02](./demos/demo_manipulabilityTransfer02.m) | [[7]](#ref-7) | Learning and reproduction of manipulability ellipsoid profiles |
| [demo_manipulabilityTransfer02b](./demos/demo_manipulabilityTransfer02b.m) | [[7]](#ref-7) | Learning and reproduction of manipulability ellipsoid profiles (numerical version) |
| [demo_MFA01](./demos/demo_MFA01.m) | [[1]](#ref-1) | Mixture of factor analyzers (MFA) |
| [demo_MPPCA01](./demos/demo_MPPCA01.m) | [[1]](#ref-1) | Mixture of probabilistic principal component analyzers (MPPCA) |
| [demo_regularization01](./demos/demo_regularization01.m) | [[1]](#ref-1) | Regularization of GMM parameters with minimum admissible eigenvalue |
......
function demo_manipulabilityTransfer01
% Leonel Rozo, 2017
%
% First run 'startup_rvc' from the robotics toolbox
%
% This code shows how a robot can exploit its redundancy to modify its
% manipulability so that it matches, as close as possible, a desired
% manipulability ellipsoid (possibly obtained from another robot or a human)
% The approach evaluates a cost function that measures the similarity
% between manipulabilities and computes a nullspace velocity command
% designed to change the robot posture so that its manipulability ellipsoid
% becomes more similar to the desired one.
% Currently two cost functions are tested:
% 1. A. Ajoudani et al. ICRA'2015
% 2. Squared Stein divergence (metrics for manifolds of PD matrices)
%
addpath('./m_fcts/');
%% Auxiliar variables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
typeCost = 2; % Defines the cost to be minimized
dt = 1E-2; % Time step
%% Create robot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Robot parameters
nbDOFs = 4; %Nb of degrees of freedom
armLength = 4; % Links length
% Robot
L1 = Link('d', 0, 'a', armLength, 'alpha', 0);
robot = SerialLink(repmat(L1,nbDOFs,1));
q = sym('q', [1 nbDOFs]); % Symbolic robot joints
J = robot.jacob0(q'); % Symbolic Jacobian
%% Computation of cost function and its gradient
% Computation of the cost function
Me_c = J(1:2,:)*J(1:2,:)'; % Current VME
Me_d = [20 -40 ; -40 150]; % Desired VME
% Me_d = [103.09 80.07 ; 80.07 70.12]; % Desired VME
% Me_d = [10 1 ; 1 200]; % Desired VME
[eVc, ~] = eig(Me_d);
des_vec = eVc(:,1)/norm(eVc(:,1)); % Desired VME major axis for cost type 1
switch typeCost
case 1 % A. Ajoudani approach
% Minus sign is included here because the cost function is being
% minimized in the nullspace
C = -inv((des_vec)' * Me_c * (des_vec));
alpha = 100; % Gain for nullspace vector
% alpha = 850; % Gain for nullspace vector
% case 2 % Log-euclidean metrics
% 1st version -> Takes TOO LONG to be evaluated
% C = norm(logm(Me_d) - logm(Me_c) ,'fro');
% 2nd version -> Also takes TOO LONG to be evaluated
% Squared log-euclidean metrics
% Me_delta = logm(Me_d) - logm(Me_c);
% C = trace(Me_delta*ctranspose(Me_delta));
% 3rd version -> Also takes TOO LONG to be evaluated
% Squared log-euclidean metrics
% Me_delta = logm(Me_d) - logm(Me_c);
% C = sum(sum(Me_delta).^2);
case 2 % Squared Stein divergence
C = log(det(0.5*(Me_d+Me_c))) - 0.5*log(det(Me_d*Me_c));
alpha = 15.5e0; % Gain for nullspace vector
% alpha = 12.5e0; % Gain for nullspace vector
end
% Cost gradient computation (symbolically)
C_gradient = [];
for i = 1 : nbDOFs
C_gradient = [C_gradient ; diff(C,q(i))];
end
% Creating a MATLAB function from symbolic variable
Cgrad = matlabFunction(C_gradient);
% Cgrad = matlabFunction(C_gradient,'File','ManEllip_Cgradient.m');
%% Testing Manipulability Transfer
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initial conditions
dxh = [0; 0]; % Desired Cartesian velocity
% q0 = [0.0 ; pi/4 ; 0.0 ; 0.0]; % Initial robot configuration
% q0 =[0.5403 ; 0.9143 ; 4.4748 ; 1.0517]; % Initial robot configuration
q0 =[pi/2 ; -pi/6; -pi/2 ; -pi/2]; % Initial robot configuration
qt = q0;
nbIter = 30;
it = 1; % Iterations counter
h1 = [];
figure('position',[10 10 1000 450],'color',[1 1 1]);
% figure('position',[2000 250 1000 450],'color',[1 1 1]);
while( it < nbIter )
delete(h1);
Jt = robot.jacob0(qt); % Current Jacobian
Jt = Jt(1:2,:);
Htmp = robot.fkine(qt); % Forward Kinematics (needed for plots)
% Compatibility with 9.X and 10.X versions of robotics toolbox
if isobject(Htmp) % SE3 object verification
xt = Htmp.t(1:2);
else
xt = Htmp(1:2,end);
end
% Evaluating gradient at current configuration
Cgrad_t = Cgrad(qt(1),qt(2),qt(3),qt(4));
% Desired joint velocities
dq_T1 = pinv(Jt)*dxh; % Main task joint velocities (given desired dx)
dq_ns = -(eye(nbDOFs) - pinv(Jt)*Jt) * alpha * Cgrad_t; % Redundancy resolution
% Plotting robot and VMEs
subplot(1,2,1); hold on;
if(it == 1)
plotGMM(xt, 1E-2*Me_d, [0.2 0.8 0.2], .4); % Scaled matrix!
end
h1 = plotGMM(xt, 1E-2*(Jt*Jt'), [0.8 0.2 0.2], .4); % Scaled matrix!
colTmp = [1,1,1] - [.7,.7,.7] * it/nbIter;
plotArm(qt, ones(nbDOFs,1)*armLength, [0; 0; it*0.1], .2, colTmp);
axis square;
axis equal;
xlabel('x_1'); ylabel('x_2');
% Plotting costs
subplot (2,3,3); hold on;
Me_ct = (Jt*Jt');
C1t = inv((des_vec)' * Me_ct * (des_vec));
plot(it, C1t, '.b');
xlabel('t'); ylabel('c1_t');
subplot (2,3,6); hold on;
C2t = log(det(0.5*(Me_d+Me_ct))) - 0.5*log(det(Me_d*Me_ct));
plot(it, C2t, 'xr');
xlabel('t'); ylabel('c2_t');
drawnow;
% Updating joint position
qt = qt + (dq_T1 + dq_ns)*dt;
it = it + 1; % Iterations++
% pause;
end
\ No newline at end of file
function Cost = ManipulabilityCost(typeCost, Me_c, Me_d)
% Leonel Rozo, 2017
%
% This function computes a symbolic equation representing cost function
% that evaluates how similar two manipulability ellipsoids are.
%
% Parameters:
% - typeCost: Type of cost function to be used
% - Me_c: Manipulability ellipsoid at current time step (symbolic)
% - Me_d: Desired manipulability ellipsoid
%
% Returns:
% - Cost: Symbolic equation of the cost function
%% Computation of cost function and its gradient
% Computation of the cost function
[eVc, ~] = eig(Me_d);
des_vec = eVc(:,1)/norm(eVc(:,1)); % Desired VME major axis
switch typeCost
case 1 % A. Ajoudani approach
% Minus sign is included here because the cost function is being
% minimized in the nullspace
C = -inv((des_vec)' * Me_c * (des_vec));
case 2 % Squared Stein divergence
C = log(det(0.5*(Me_d+Me_c))) - 0.5*log(det(Me_d*Me_c));
end
% Creating a MATLAB function from symbolic variable
Cost = matlabFunction(C);
function Cgrad = ManipulabilityCostGradient(typeCost, Me_c, Me_d, q)
% Leonel Rozo, 2017
%
% This function computes a symbolic equation representing the gradient of a
% cost function that evaluates how similar two manipulability ellipsoids
% are.
%
% Parameters:
% - typeCost: Type of cost function to be used
% - Me_c: Manipulability ellipsoid at current time step (symbolic)
% - Me_d: Desired manipulability ellipsoid
% - q: Vector of symbolic variables representing the robot joints
%
% Returns:
% - Cgrad: Symbolic equation of the cost gradient
%% Computation of cost function and its gradient
% Computation of the cost function
[eVc, ~] = eig(Me_d);
des_vec = eVc(:,1)/norm(eVc(:,1)); % Desired VME major axis
switch typeCost
case 1 % A. Ajoudani approach (Major-axis alignment)
% Minus sign is included here because the cost function is being
% minimized in the nullspace
C = -inv((des_vec)' * Me_c * (des_vec));
case 2 % Squared Stein divergence
C = log(det(0.5*(Me_d+Me_c))) - 0.5*log(det(Me_d*Me_c));
end
% Symbolic cost gradient computation
C_gradient = [];
for i = 1 : length(q)
C_gradient = [C_gradient ; diff(C,q(i))];
end
% Creating a MATLAB function from symbolic variable
Cgrad = matlabFunction(C_gradient);
function q = Viterbi_HMM(Data, model)
% Viterbi path decoding (MAP estimate of best path) in HMM.
%
% Writing code takes time. Polishing it and making it available to others takes longer!
% If some parts of the code were useful for your research of for a better understanding
% of the algorithms, please reward the authors by citing the related publications,
% and consider making your own research available in this way.
%
% @article{Rozo16Frontiers,
% author="Rozo, L. and Silv\'erio, J. and Calinon, S. and Caldwell, D. G.",
% title="Learning Controllers for Reactive and Proactive Behaviors in Human-Robot Collaboration",
% journal="Frontiers in Robotics and {AI}",
% year="2016",
% month="June",
% volume="3",
% number="30",
% pages="1--11",
% doi="10.3389/frobt.2016.00030"
% }
%
% Copyright (c) 2015 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
%
% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
%
% PbDlib is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License version 3 as
% published by the Free Software Foundation.
%
% PbDlib is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
nbData = size(Data,2);
% %MPE estimate of best path (for comparison)
% H = computeGammaHMM(s(1), model);
% [~,q] = max(H);
% %Viterbi with scaling factor to avoid numerical underflow
% for i=1:model.nbStates
% B(i,:) = gaussPDF(Data, model.Mu(:,i), model.Sigma(:,:,i)); %Emission probability
% end
% %Viterbi forward pass
% DELTA(:,1) = model.StatesPriors .* B(:,1);
% DELTA(:,1) = DELTA(:,1) / norm(DELTA(:,1)); %Scaled version of Delta to avoid numerical underflow
% PSI(1:model.nbStates,1) = 0;
% for t=2:nbData
% for i=1:model.nbStates
% [maxTmp, PSI(i,t)] = max(DELTA(:,t-1) .* model.Trans(:,i));
% DELTA(i,t) = maxTmp * B(i,t);
% end
% DELTA(:,t) = DELTA(:,t) / norm(DELTA(:,t)); %Scaled version of Delta to avoid numerical underflow
% end
% %Backtracking
% q = [];
% [~,q(nbData)] = max(DELTA(:,nbData));
% for t=nbData-1:-1:1
% q(t) = PSI(q(t+1),t+1);
% end
%Viterbi with log computation
for i=1:model.nbStates
logB(i,:) = log(gaussPDF(Data, model.Mu(:,i), model.Sigma(:,:,i))); %Emission probability
end
%Viterbi forward pass
logDELTA(:,1) = log(model.StatesPriors) + logB(:,1);
PSI(1:model.nbStates,1) = 0;
for t=2:nbData
for i=1:model.nbStates
[maxTmp, PSI(i,t)] = max(logDELTA(:,t-1) + log(model.Trans(:,i)));
logDELTA(i,t) = maxTmp + logB(i,t);
end
end
%Backtracking
q = [];
[~,q(nbData)] = max(logDELTA(:,nbData));
for t=nbData-1:-1:1
q(t) = PSI(q(t+1),t+1);
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