Commit 7ea05b8c authored by Sylvain CALINON's avatar Sylvain CALINON

Merge branch 'master' of gitlab.idiap.ch:rli/pbdlib-matlab

parents 17b2d1a5 ec4fb567
......@@ -138,6 +138,18 @@ Did you find PbDLib useful for your research? Please acknowledge the authors in
}
```
#### Ref. [9]
**Manipulability ellipsoids tracking**<br>
[Link to publication](http://calinon.ch/paper3066.htm)
```
@incollection{@article{Jaquier18RSS,
author="Jaquier, N and Rozo, L. and Caldwell, D. G. and Calinon, S.",
title="Geometry-aware Tracking of Manipulability Ellipsoids",
year="2018",
booktitle = "Robotics: Science and Systems ({R:SS})",
address = "Pittsburgh, USA"
}
```
### List of examples
......@@ -198,6 +210,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_manipulabilityTracking_mainTask01](./demos/demo_manipulabilityControl_mainTask01.m) | [[9]](#ref-9) | Tracking of a desired manipulability ellipsoid as the main task |
| [demo_manipulabilityTracking_mainTask02](./demos/demo_manipulabilityControl_mainTask02.m) | [[9]](#ref-9) | Tracking of a desired manipulability ellipsoid as the main task using precision matrice as controller gain |
| [demo_manipulabilityTracking_secondaryTask01](./demos/demo_manipulabilityControl_secondTask01.m) | [[9]](#ref-9) | Tracking of a desired manipulability ellipsoid as the secondary task with position tracking as main task |
| [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) |
......
function demo_manipulabilityTracking_mainTask01
% Matching of a desired manipulability ellipsoid as the main task (no
% desired position) using the formulation with the manipulability Jacobien
% (Mandel notation).
%
% 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 cite the related publications.
%
% @article{Jaquier18RSS,
% author="Jaquier, N and Rozo, L. and Caldwell, D. G. and Calinon, S.",
% title="Geometry-aware Tracking of Manipulability Ellipsoids",
% year="2018",
% booktitle = "Robotics: Science and Systems ({R:SS})",
% address = "Pittsburgh, USA"
% }
%
% Copyright (c) 2017 Idiap Research Institute, http://idiap.ch/
% Written by Noémie Jaquier and Leonel Rozo
%
% 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/>.
% First run 'startup_rvc' from the robotics toolbox
addpath('./m_fcts/');
%% Auxiliar variables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dt = 1E-2; % Time step
nbIter = 65; % Number of iterations
Km = 3; % Gain for manipulability control in task space
%% 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
% Define the desired manipulability
q_Me_d = [pi/16 ; pi/4 ; pi/8 ; -pi/8]; % task 1
% q_Me_d = [pi/2 ; -pi/6; -pi/2 ; -pi/2]; % task 2
J_Me_d = robot.jacob0(q_Me_d); % Current Jacobian
J_Me_d = J_Me_d(1:2,:);
Me_d = (J_Me_d*J_Me_d');
%% Testing Manipulability Transfer
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initial conditions
q0 = [pi/2 ; -pi/6; -pi/2 ; -pi/2]; % Initial robot configuration task 1
% q0 = [0.0 ; pi/4 ; pi/2 ; pi/8]; % Initial robot configuration task 2
qt = q0;
it = 1; % Iterations counter
h1 = [];
gmm_c = [];
% Initial end-effector position (compatibility with 9.X and 10.X versions
% of robotics toolbox)
Htmp = robot.fkine(q0); % Forward Kinematics
if isobject(Htmp) % SE3 object verification
x0 = Htmp.t(1:2);
else
x0 = Htmp(1:2,end);
end
figure('position',[10 10 1000 450],'color',[1 1 1]);
% Main control loop
while( it < nbIter )
delete(h1);
Jt = robot.jacob0(qt); % Current Jacobian
Jt_full = Jt;
Jt = Jt(1:2,:);
Htmp = robot.fkine(qt); % Forward Kinematics (needed for plots)
Me_ct = (Jt*Jt'); % Current manipulability
Me_track(:,:,it) = Me_ct;
qt_track(:,it) = qt;
% Current end-effector position
if isobject(Htmp) % SE3 object verification
xt = Htmp.t(1:2);
else
xt = Htmp(1:2,end);
end
% Compute manipulability Jacobian
Jm_t = compute_red_manipulability_Jacobian(Jt_full, 1:2);
% Compute desired joint velocities
M_diff = logmap(Me_d,Me_ct);
dq_T1 = pinv(Jm_t)*Km*symmat2vec(M_diff);
% Plotting robot and manipulability ellipsoids
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*Me_ct, [0.8 0.2 0.2], .4); % Scaled matrix!
colTmp = [1,1,1] - [.8,.8,.8] * (it+10)/nbIter;
plotArm(qt, ones(nbDOFs,1)*armLength, [0; 0; it*0.1], .2, colTmp);
axis square;
axis equal;
xlabel('x_1'); ylabel('x_2');
subplot(1,2,2);
hold on; axis equal;
delete(gmm_c);
gmm_c = plotGMM([0;0], 1E-2*Me_ct, [0.8 0.2 0.2], .1); % Scaled matrix!
if(it == 1)
plotGMM([0;0], 1E-2*Me_d, [0.2 0.8 0.2], .1); % Scaled matrix!
end
drawnow;
% Updating joint position
qt = qt + (dq_T1)*dt;
it = it + 1; % Iterations++
end
%% Final plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Robot and manipulability ellipsoids
figure('position',[10 10 900 900],'color',[1 1 1]);
hold on;
p = [];
for it = 1:2:nbIter-1
colTmp = [1,1,1] - [.8,.8,.8] * (it)/nbIter;
p = [p; plotArm(qt_track(:,it), ones(nbDOFs,1)*armLength, [0; 0; it*0.1], .2, colTmp)];
end
z = get(p,'ZData'); % to put arm plot down compared to GMM plot
for i = 1:size(z,1)
if isempty(z{i})
set(p,'ZData',z{i}-10)
end
end
plotGMM(xt, 1E-2*Me_d, [0.2 0.8 0.2], .4);
plotGMM(x0, 1E-2*Me_track(:,:,1), [0.2 0.2 0.8], .4);
plotGMM(xt, 1E-2*Me_ct, [0.8 0.2 0.2], .4);
axis equal
set(gca,'xtick',[],'ytick',[])
xlabel('$x_1$','fontsize',40,'Interpreter','latex'); ylabel('$x_2$','fontsize',40,'Interpreter','latex');
% Initial manipulability ellipsoid
figure('position',[10 10 450 450],'color',[1 1 1]);
hold on;
plotGMM([0;0], 1E-2*Me_d, [0.2 0.8 0.2], .4);
plotGMM([0;0], 1E-2*Me_track(:,:,1), [0.2 0.2 0.8], .4);
xlabel('$x_1$','fontsize',38,'Interpreter','latex'); ylabel('$x_2$','fontsize',38,'Interpreter','latex');
xlim([-1.8 1.8]);ylim([-1.8 1.8]);
set(gca,'xtick',[],'ytick',[]);
text(-.8,1,0,'Initial','FontSize',38,'Interpreter','latex')
axis equal;
% Final manipulability ellipsoid
figure('position',[10 10 450 450],'color',[1 1 1]);
hold on;
plotGMM([0;0], 1E-2*Me_d, [0.2 0.8 0.2], .4);
plotGMM([0;0], 1E-2*Me_ct, [0.8 0.2 0.2], .4);
xlabel('$x_1$','fontsize',38,'Interpreter','latex'); ylabel('$x_2$','fontsize',38,'Interpreter','latex');
xlim([-1.8 1.8]);ylim([-1.8 1.8]);
text(-.7,1,0,'Final','FontSize',38,'Interpreter','latex')
set(gca,'xtick',[],'ytick',[])
axis equal;
% Cost
figure()
hold on;
for it = 1:nbIter-1
cost(it) = norm(logm(Me_d^-.5*Me_track(:,:,it)*Me_d^-.5),'fro');
end
plot([1:nbIter-1].*dt, cost, '-','color',[0 0 .7],'Linewidth',3);
set(gca,'fontsize',14);
xlim([0 nbIter*dt])
xlabel('$t$','fontsize',22,'Interpreter','latex'); ylabel('$d$','fontsize',22,'Interpreter','latex');
end
function Jm_red = compute_red_manipulability_Jacobian(J, taskVar)
% Compute the force manipulability Jacobian (symbolic) in the form of a
% matrix using Mandel notation.
if nargin < 2
taskVar = 1:6;
end
Jm = compute_manipulability_Jacobian(J);
Jm_red = [];
for i = 1:size(Jm,3)
Jm_red = [Jm_red, symmat2vec(Jm(taskVar,taskVar,i))];
end
end
function Jm = compute_manipulability_Jacobian(J)
% Compute the force manipulability Jacobian (symbolic).
J_grad = compute_joint_derivative_Jacobian(J);
Jm = tmprod(J_grad,J,2) + tmprod(permute(J_grad,[2,1,3]),J,1);
% mat_mult = kdlJacToArma(J)*reshape( arma::mat(permDerivJ.memptr(), permDerivJ.n_elem, 1, false), columns, columns*rows);
% dJtdq_J = arma::cube(mat_mult.memptr(), rows, rows, columns);
end
function J_grad = compute_joint_derivative_Jacobian(J)
% Compute the Jacobian derivative w.r.t joint angles (hybrid Jacobian
% representation).
% Ref: H. Bruyninck and J. de Schutter, 1996
nb_rows = size(J,1); % task space dim.
nb_cols = size(J,2); % joint space dim.
J_grad = zeros(nb_rows, nb_cols, nb_cols);
for i = 1:nb_cols
for j = 1:nb_cols
J_i = J(:,i);
J_j = J(:,j);
if j < i
J_grad(1:3,i,j) = cross(J_j(4:6,:),J_i(1:3,:));
J_grad(4:6,i,j) = cross(J_j(4:6,:),J_i(4:6,:));
elseif j > i
J_grad(1:3,i,j) = -cross(J_j(1:3,:),J_i(4:6,:));
else
J_grad(1:3,i,j) = cross(J_i(4:6,:),J_i(1:3,:));
end
end
end
end
function U = logmap(X,S)
% Logarithm map (SPD manifold)
N = size(X,3);
for n = 1:N
% U(:,:,n) = S^.5 * logm(S^-.5 * X(:,:,n) * S^-.5) * S^.5;
% tic
% U(:,:,n) = S * logm(S\X(:,:,n));
% toc
% tic
[v,d] = eig(S\X(:,:,n));
U(:,:,n) = S * v*diag(log(diag(d)))*v^-1;
% toc
end
end
function v = symmat2vec(M)
% Vectorization of a symmetric matrix
N = size(M,1);
v = [];
v = diag(M);
for n = 1:N-1
v = [v; sqrt(2).*diag(M,n)]; % Mandel notation
end
end
function [S,iperm] = tmprod(T,U,mode)
% Mode-n tensor-matrix product
size_tens = ones(1,mode);
size_tens(1:ndims(T)) = size(T);
N = length(size_tens);
% Compute the complement of the set of modes.
bits = ones(1,N);
bits(mode) = 0;
modec = 1:N;
modec = modec(logical(bits(modec)));
% Permutation of the tensor
perm = [mode modec];
size_tens = size_tens(perm);
S = T;
if mode ~= 1
S = permute(S,perm);
end
% n-mode product
size_tens(1) = size(U,1);
S = reshape(U*reshape(S,size(S,1),[]),size_tens);
% Inverse permutation
iperm(perm(1:N)) = 1:N;
S = permute(S,iperm);
end
function demo_manipulabilityTracking_mainTask02
% Matching of a desired manipulability ellipsoid as the main task (no
% desired position) using the formulation with the manipulability Jacobien
% (Mandel notation).
% The matrix gain used for the manipulability tracking controller is now
% defined as the inverse of a 2nd-order covariance matrix representing the
% variability information obtained from a learning algorithm.
%
% 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 cite the related publications.
%
% @article{Jaquier18RSS,
% author="Jaquier, N and Rozo, L. and Caldwell, D. G. and Calinon, S.",
% title="Geometry-aware Tracking of Manipulability Ellipsoids",
% year="2018",
% booktitle = "Robotics: Science and Systems ({R:SS})",
% address = "Pittsburgh, USA"
% }
%
% Copyright (c) 2017 Idiap Research Institute, http://idiap.ch/
% Written by Noémie Jaquier and Leonel Rozo
%
% 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/>.
% First run 'startup_rvc' from the robotics toolbox
addpath('./m_fcts/');
%% Auxiliar variables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dt = 1E-2; % Time step
nbIter = 100; % Number of iterations
clrmap = lines(4);
% Initialisation of the covariance transformation
cov_2D(:,:,1) = [0.3 0.0 0.0; 0.0 0.3 0.0; 0.0 0.0 0.3]; % Unitary gain (baseline)
cov_2D(:,:,2) = [0.03 0.0 0.0; 0.0 0.3 0.0; 0.0 0.0 0.3]; % Priority on matching for 'x' axis
cov_2D(:,:,3) = [0.3 0.0 0.0; 0.0 0.03 0.0; 0.0 0.0 0.3]; % Priority on matching for 'y' axis
cov_2D(:,:,4) = [0.3 0.0 0.0; 0.0 0.3 0.0; 0.0 0.0 0.1]; % Correlation has highest priority for tracking
names = {'Baseline','Priority on x-axis','Priority on y-axis','Priority on xy-correltation'};
for n = 1:4
%% 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
% Define the desired manipulability
q_Me_d = [0.0 ; pi/4 ; pi/2 ; pi/8];
J_Me_d = robot.jacob0(q_Me_d); % Current Jacobian
J_Me_d = J_Me_d(1:2,:);
Me_d = (J_Me_d*J_Me_d');
%% Testing Manipulability Transfer
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initial conditions
q0 =[pi/2 ; -pi/6; -pi/2 ; -pi/2]; % Initial robot configuration
qt = q0;
it = 1; % Iterations counter
h1 = [];
while( it < nbIter )
delete(h1);
Jt = robot.jacob0(qt); % Current Jacobian
Jt_full = Jt;
Jt = Jt(1:2,:);
Htmp = robot.fkine(qt); % Forward Kinematics
Me_ct = (Jt*Jt'); % Current manipulability
Me_track(:,:,it,n)=Me_ct;
qt_track(:,it) = qt;
% 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
xt_track(:,it,n) = xt;
% Compute manipulability Jacobian
Jm_t = compute_red_manipulability_Jacobian(Jt_full, 1:2);
% Desired joint velocities
M_diff = logmap(Me_d,Me_ct);
dq_T1 = pinv(Jm_t)*(cov_2D(:,:,n)\symmat2vec(M_diff));
% Updating joint position
qt = qt + (dq_T1)*dt;
it = it + 1; % Iterations
end
%% Final plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Robot and manipulability ellipsoids
figure('position',[10 10 900 900],'color',[1 1 1]);
hold on;
p = [];
for it = 1:2:nbIter-1
colTmp = [.95,.95,.95] - [.8,.8,.8] * (it)/nbIter;
p = [p; plotArm(qt_track(:,it), ones(nbDOFs,1)*armLength, [0; 0; it*0.1], .2, colTmp)];
end
z = get(p,'ZData'); % to put arm plot down compared to GMM plot
for i = 1:size(z,1)
if isempty(z{i})
set(p,'ZData',z{i}-10)
end
end
plotGMM(xt(:,end), 1E-2*Me_d, [0.2 0.8 0.2], .4);
plotGMM(xt_track(:,1,n), 1E-2*Me_track(:,:,1,n), [0.2 0.2 0.8], .4);
plotGMM(xt(:,end), 1E-2*Me_ct, clrmap(n,:), .4);
axis equal
xlim([-3,9]); ylim([-4,8])
set(gca,'fontsize',22,'xtick',[],'ytick',[]);
xlabel('$x_1$','fontsize',38,'Interpreter','latex'); ylabel('$x_2$','fontsize',38,'Interpreter','latex');
title(names{n});
drawnow;
end
%% Final plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Manipulability ellipsoids
for n = 1:4
figure('position',[10 10 1200 300],'color',[1 1 1]);
hold on;
for it = 1:10:nbIter-1
plotGMM([it*dt;0], 2E-4*Me_d, [0.2 0.8 0.2], .1);
plotGMM([it*dt;0], 2E-4*Me_track(:,:,it,n), clrmap(n,:), .3);
end
axis equal;
set(gca,'fontsize',24,'ytick',[]);
xlabel('$t$','fontsize',42,'Interpreter','latex');
ylabel('$\mathbf{M}$','fontsize',42,'Interpreter','latex');
title(names{n});
end
figure('position',[10 10 900 900],'color',[1 1 1]);
hold on;
for it = 1:2:nbIter-1
for n = 1:4
plotGMM(xt_track(:,it,n), 1E-2*Me_track(:,:,it,n), clrmap(n,:), .15);
end
end
plotGMM(xt_track(:,1,1), 1E-2*Me_track(:,:,1,1), [0.1 0.1 0.4], .6);
axis equal;
set(gca,'fontsize',18);
xlabel('$x_1$','fontsize',30,'Interpreter','latex'); ylabel('$x_2$','fontsize',30,'Interpreter','latex');
title('End-effector and manipulability ellipsoids trajectories')
end
function Jm_red = compute_red_manipulability_Jacobian(J, taskVar)
% Compute the force manipulability Jacobian (symbolic) in the form of a
% matrix using Mandel notation.
if nargin < 2
taskVar = 1:6;
end
Jm = compute_manipulability_Jacobian(J);
Jm_red = [];
for i = 1:size(Jm,3)
Jm_red = [Jm_red, symmat2vec(Jm(taskVar,taskVar,i))];
end
end
function Jm = compute_manipulability_Jacobian(J)
% Compute the force manipulability Jacobian (symbolic).
J_grad = compute_joint_derivative_Jacobian(J);
Jm = tmprod(J_grad,J,2) + tmprod(permute(J_grad,[2,1,3]),J,1);
% mat_mult = kdlJacToArma(J)*reshape( arma::mat(permDerivJ.memptr(), permDerivJ.n_elem, 1, false), columns, columns*rows);
% dJtdq_J = arma::cube(mat_mult.memptr(), rows, rows, columns);
end
function J_grad = compute_joint_derivative_Jacobian(J)
% Compute the Jacobian derivative w.r.t joint angles (hybrid Jacobian
% representation).
% Ref: H. Bruyninck and J. de Schutter, 1996
nb_rows = size(J,1); % task space dim.
nb_cols = size(J,2); % joint space dim.
J_grad = zeros(nb_rows, nb_cols, nb_cols);
for i = 1:nb_cols
for j = 1:nb_cols
J_i = J(:,i);
J_j = J(:,j);
if j < i
J_grad(1:3,i,j) = cross(J_j(4:6,:),J_i(1:3,:));
J_grad(4:6,i,j) = cross(J_j(4:6,:),J_i(4:6,:));
elseif j > i
J_grad(1:3,i,j) = -cross(J_j(1:3,:),J_i(4:6,:));
else
J_grad(1:3,i,j) = cross(J_i(4:6,:),J_i(1:3,:));
end
end
end
end
function U = logmap(X,S)
% Logarithm map (SPD manifold)
N = size(X,3);
for n = 1:N
% U(:,:,n) = S^.5 * logm(S^-.5 * X(:,:,n) * S^-.5) * S^.5;
% tic
% U(:,:,n) = S * logm(S\X(:,:,n));
% toc
% tic
[v,d] = eig(S\X(:,:,n));
U(:,:,n) = S * v*diag(log(diag(d)))*v^-1;
% toc
end
end
function v = symmat2vec(M)
% Vectorization of a symmetric matrix
N = size(M,1);
v = [];
v = diag(M);
for n = 1:N-1
v = [v; sqrt(2).*diag(M,n)]; % Mandel notation
end
end
function [S,iperm] = tmprod(T,U,mode)
% Mode-n tensor-matrix product
size_tens = ones(1,mode);
size_tens(1:ndims(T)) = size(T);
N = length(size_tens);
% Compute the complement of the set of modes.
bits = ones(1,N);
bits(mode) = 0;
modec = 1:N;
modec = modec(logical(bits(modec)));
% Permutation of the tensor
perm = [mode modec];
size_tens = size_tens(perm);
S = T;
if mode ~= 1
S = permute(S,perm);
end
% n-mode product
size_tens(1) = size(U,1);
S = reshape(U*reshape(S,size(S,1),[]),size_tens);
% Inverse permutation
iperm(perm(1:N)) = 1:N;
S = permute(S,iperm);
end
function demo_manipulabilityTracking_secondaryTask01
% Holding position as main task, matching a desired manipulability
% ellipsoid as secondary task with the manipulability Jacobian formulation
% (Mandel notation).
%
% 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 cite the related publications.
%
% @article{Jaquier18RSS,
% author="Jaquier, N and Rozo, L. and Caldwell, D. G. and Calinon, S.",
% title="Geometry-aware Tracking of Manipulability Ellipsoids",
% year="2018",
% booktitle = "Robotics: Science and Systems ({R:SS})",
% address = "Pittsburgh, USA"
% }
%
% Copyright (c) 2017 Idiap Research Institute, http://idiap.ch/
% Written by Noémie Jaquier and Leonel Rozo
%
% 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/>.
% First run 'startup_rvc' from the robotics toolbox
addpath('./m_fcts/');
%% Auxiliar variables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dt = 1E-2; % Time step
nbIter = 50; % Number of iterations
Kp = 8; % Gain for position control in task space
Km = 5; % Gain for manipulability control in nullspace
%% 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
% Define the desired manipulability
Me_d = [20 -40 ; -40 150]; % task1