-
Notifications
You must be signed in to change notification settings - Fork 0
/
demo_manipulabilityTracking_mainTask02.m
executable file
·292 lines (241 loc) · 8.76 KB
/
demo_manipulabilityTracking_mainTask02.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
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) 2018 Idiap Research Institute, http://idiap.ch/,
% and Italian Institute of Technology, https://iit.it/
% Written by Noémie Jaquier and Leonel Rozo
%
% This file is also 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