-
Notifications
You must be signed in to change notification settings - Fork 0
/
transformForOpenSimDLC.m
131 lines (115 loc) · 5.9 KB
/
transformForOpenSimDLC.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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [md_opensim] = transformForOpenSim(marker_data)
%
% This function transforms marker data from lab coordinates into OpenSim
% coordinates. This function is to be used in getTRCfromMarkers.
%
% INPUTS:
% marker_data : struct containing marker data, in robot coordinates
% Fields:
% pos - position of markers, aligned spatially to match handle
% movement
% t - times for video frames of marker capture, aligned to
% cerebus collection
% cds : CDS file to extract handle kinematics from
%
% OUTPUTS:
% md_opensim : struct of marker_data in OpenSim coordinates
% Fields:
% pos - position of markers, aligned to OpenSim coordinates
% t - times for video frames of marker capture, aligned to
% cerebus collection
% handle_opensim : position of the handle in OpenSim coordinates
%
%
% Written by Raeed Chowdhury and Joshua Glaser. Updated April 2017.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [md_opensim_pos,handle_opensim] = transformForOpenSimDLC(cds)
% Robot coordinates:
% Origin at shoulder joint center of robot, x is to right, y is towards screen, z is up
% OpenSim coordinates:
% Origin at shoulder joint center (marker 9), x is towards screen, y is up, z is to right
analog_idx = 0;
for i_an = 1:numel(cds.analog)
if(sum(strcmpi(cds.analog{i_an}.Properties.VariableNames,'shoulder1_x')) >= 1)
analog_idx = i_an;
elseif(sum(strcmpi(cds.analog{i_an}.Properties.VariableNames,'shoulder_x')) >= 1)
analog_idx = i_an;
end
end
md_opensim = cds.analog{analog_idx};
is_pos = ~cellfun(@isempty,strfind(md_opensim.Properties.VariableNames,'_x')) | ~cellfun(@isempty,strfind(md_opensim.Properties.VariableNames,'_y')) | ...
~cellfun(@isempty,strfind(md_opensim.Properties.VariableNames,'_z'));
is_point = ~cellfun(@isempty,strfind(md_opensim.Properties.VariableNames,'point'));
is_pos_and_not_point = is_pos & ~is_point;
md_opensim_pos = md_opensim(:,is_pos_and_not_point);
num_markers = size(md_opensim_pos,2)/3;
% extract and clean up shoulder jc data
if(any(strcmpi(md_opensim_pos.Properties.VariableNames,'shoulder_x')))
markername = 'shoulder';
shoulder_pos_lab = [md_opensim_pos.([markername,'_x']),md_opensim_pos.([markername,'_y']),md_opensim_pos.([markername,'_z'])];
elseif(any(strcmpi(md_opensim_pos.Properties.VariableNames,'shoulder1_x')))
markername = 'shoulder1';
shoulder_pos_lab = [md_opensim_pos.([markername,'_x']),md_opensim_pos.([markername,'_y']),md_opensim_pos.([markername,'_z'])];
end
marker_loss_points = find(diff(isnan(shoulder_pos_lab(:,1)))>0);
marker_reappear_points = find(diff(isnan(shoulder_pos_lab(:,1)))<0);
if isempty(marker_loss_points) && ~isempty(marker_reappear_points)
% means that marker was lost to start, reappeared and never lost again
% Dont know what to do here other than put a zero at start off loss
% marker_reappear_points(1) = [];
marker_loss_points = 0;
warning('Shoulder position marker not found at start of file. Replacing missing start points with first reappearance position')
elseif ~isempty(marker_loss_points) && isempty(marker_reappear_points)
% means that marker was lost only at end
% use last index
marker_reappear_points = length(shoulder_pos_lab);
elseif ~isempty(marker_loss_points) || ~isempty(marker_reappear_points)
if length(marker_loss_points)>length(marker_reappear_points) || marker_loss_points(end)>marker_reappear_points(end)
%Means that a marker was lost but never found again at end of file
%append last index
marker_reappear_points(end+1) = length(shoulder_pos_lab);
end
if length(marker_loss_points)<length(marker_reappear_points) || marker_loss_points(1)>marker_reappear_points(1)
%Means that a marker was lost to start
%Dont know what to do here other than put a zero at start off loss
% marker_reappear_points(1) = [];
marker_loss_points = [0;marker_loss_points];
warning('Shoulder position marker not found at start of file. Replacing missing start points with first reappearance position')
end
end
% fix marker loss
if ~isempty(marker_loss_points) && ~isempty(marker_reappear_points)
for i=1:length(marker_loss_points)
marker_loss = marker_loss_points(i);
marker_reappear = marker_reappear_points(i);
num_lost = marker_reappear-marker_loss;
if marker_loss~=0
rep_coord = repmat(shoulder_pos_lab(marker_loss,:),num_lost,1);
else
rep_coord = repmat(shoulder_pos_lab(marker_reappear,:),num_lost,1);
end
shoulder_pos_lab(marker_loss+1:marker_reappear,:) = rep_coord;
end
end
% Recenter all markers on shoulder position
rep_shoulder_pos = repmat(shoulder_pos_lab,[1 num_markers]);
dlc_pos_recenter = md_opensim_pos{:,:}-rep_shoulder_pos;
% switch axes of coordinate frame
% x->y, y->z, z->x
switch_mat = repmat([2,3,1],[1,num_markers]);
offset_mat = 3*reshape(repmat([0:1:num_markers-1],[3,1]),[1,size(md_opensim_pos,2)]);
switch_mat = switch_mat + offset_mat;
md_opensim_pos{:,:} = dlc_pos_recenter(:,switch_mat)/1000; % change from mm to meters
md_opensim_pos.t = md_opensim.t;
% do same for handle position
if(~isempty(cds.kin))
handle_pos = [cds.kin.x cds.kin.y zeros(height(cds.kin),1)];
handle_pos=interp1(cds.kin.t,handle_pos,md_opensim.t);
recenter_handle = handle_pos-shoulder_pos_lab;
handle_opensim = recenter_handle(:,[2 3 1])/100;
else
handle_opensim = [];
end
end