-
Notifications
You must be signed in to change notification settings - Fork 10
/
GetLieParameters.m
50 lines (41 loc) · 2.24 KB
/
GetLieParameters.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
%% Extract Lie parameters from SE(3)
% made by Terry Taewoong Um ([email protected])
% Adaptive Systems Lab., University of Waterloo
function [mdl_subject] = GetLieParameters(mdl_subject)
CLOSE_ZERO = 10^(-13);
nData = size(mdl_subject{1,1}.EulerAngle,1);
% LieVec for root position
for jj=1:mdl_subject{1,1}.nLink-1
for ii=1:nData
% mdl_subject{1,1}.LieVec_xyz(jj,:,ii) = (mdl_subject{1,1}.T_Moving_Home(1:3,4,jj,ii+1)-mdl_subject{1,1}.T_Moving_Home(1:3,4,jj,ii-1))';
% mdl_subject{1,1}.LieVec_xyz(jj,:,ii) = mdl_subject{1,1}.LieVec_xyz(jj,:,ii)*60; % velocity of the root
mdl_subject{1,1}.LieVec_xyz(jj,:,ii) = mdl_subject{1,1}.T_Moving_Home(1:3,4,jj,ii);
mdl_subject{1,1}.LieMag_xyz(jj,ii) = norm(mdl_subject{1,1}.LieVec_xyz(jj,:,ii));
if mdl_subject{1,1}.LieMag_xyz(jj,ii) < CLOSE_ZERO
mdl_subject{1,1}.LieVec_xyz(jj,:,ii) = [0 0 0];
mdl_subject{1,1}.LieVecNormed_xyz(jj,:,ii) = mdl_subject{1,1}.LieVecNormed_xyz(jj,:,ii+1);
mdl_subject{1,1}.LieMag_xyz(jj,ii) = 0;
else
mdl_subject{1,1}.LieVecNormed_xyz(jj,:,ii) = mdl_subject{1,1}.LieVec_xyz(jj,:,ii)/mdl_subject{1,1}.LieMag_xyz(jj,ii);
end
end
end
for ii=1:nData
mdl_subject{1,1}.LieVec(1,:,ii) = SO3toso3(mdl_subject{1,1}.T_Moving_Home(1:3,1:3,1,ii));
end
for kk=2:6
for jj=1:mdl_subject{kk,1}.nLink-1
for ii=1:nData
mdl_subject{kk,1}.LieVec(jj,:,ii) = SO3toso3(mdl_subject{kk,1}.T_Moving_Local(1:3,1:3,jj,ii));
mdl_subject{kk,1}.LieMag(jj,ii) = norm(mdl_subject{kk,1}.LieVec(jj,:,ii));
if mdl_subject{kk,1}.LieMag(jj,ii) < CLOSE_ZERO
mdl_subject{kk,1}.LieVec(jj,:,ii) = [0 0 0];
mdl_subject{kk,1}.LieVecNormed(jj,:,ii) = mdl_subject{kk,1}.LieVec(jj,:,ii);
mdl_subject{kk,1}.LieMag(jj,ii) = 0;
else
mdl_subject{kk,1}.LieVecNormed(jj,:,ii) = mdl_subject{kk,1}.LieVec(jj,:,ii)/mdl_subject{kk,1}.LieMag(jj,ii);
end
end
end
end
end