I've been trying to write a script that allows me to read in force plate analog data via Qualisys QTM for Matlab. Qualisys sends real-time frame data for any variable, but in this case I'm only concerned with frame number and force on the left and right belts of a dual belt treadmill. You can select singular analog force input per video frame (100Hz) or randomly sized analog cell packets (2,n) where the top cell is the analog frame number (1000Hz) and the bottom cell is a 2x10, 2x15, 2x20, 2x30 which contain the vertical components of the left and right force plates. The size of the bottom cells are random and are pushed to matlab at the rate at which they develop from qualisys. The idea is that when an individual meets a preset force threshold by stepping with the right foot on the right treadmill belt, matlab will send a packet back to the treadmill via tcpip which rapidly decelerates the right belt speed for a fraction of a second.
for i = 1:nframes
% ### Fetch data from QTM
[frameinfo,Analog] = QCM;
try
Force = cell2mat(Analog(2,:)); analogData(:,i) = Analog; % Add current value
catch
isempty(Analog);
continue
end
if i > 5
for j = 1:length(Force)
RightFz = Force(2,j);
LeftFz = Force(1,j);
if j > 1
rightForce_previous = Force(2,j-1);
leftForce_previous = Force(1,j-1);
....
....
if ((stanceTrig == 1) && (RightFz > lowerthresh) && (RightFz < upperthresh) && (RightFz < rightForce_previous))
tripFrame(i) = current_frame;
tripframe = current_frame;
%tripTrigger(remote,lowspeed,accel,orispeed);
stanceTrig = 0;
tripTrig = 0;
end
function tripTrigger(remote,lowspeed,accel,orispeed)
fopen(remote);
tm_set(remote,lowspeed,accel);
pause(0.10);
tm_set(remote,highspeed,accel);
pause(0.05);
tm_set(remote,orispeed,accel);
pause(0.17);
end
This portion in real-time works with no issues. Where I ran into issues was attempting to insert a mechanism which would allow someone to turn off the tripTrigger by increasing their foot treadmill contact time. I started a second if loop > 5 because the first few cells can be 50k data points while matlab and QTM catch up with each other before delivering cells at a normalized size and pace {2,(10 or 15 or 20} per frame. Following the current force as RightFZ and LeftFz and the previous_right, previous_left to compare across thresholds is where I attempted to insert the mechanism I attempted to describe above.
...
if ((LeftFz > lowbound) && (leftForce_previous < lowbound) && (LeftFz > leftForce_previous))
HS_frame = double(current_frame);
hs_frame(i) = current_frame;
elseif ((LeftFz < lowbound) && (leftForce_previous > lowbound) && (LeftFz < leftForce_previous))
TO_frame = double(current_frame);
to_frame(i) = current_frame;
else
end
try
stanceTime = (TO_frame - HS_frame)/100;
catch
isempty(TO_frame);isempty(HS_frame);(HS_frame > TO_frame);
continue
end
if (stanceTime < stanceAvgSD)
stanceTrig = 1;
end
This causes about frame rate drops of around 10/30 which makes the original trip function unreliable. I've tried minimizing the loop iterations for the LeftFz rightFz and previous by attempting to mean sections of the cells but it does not work any more efficiently, actually less for some reason. I have attempted to disable Nagles algorithm for what it is worth and have attempted many seperate solutions with no luck. I am not sure if this is a tcpip issue or if my code is that inefficient. This is my first real programming attempt so I don't have enough experience with either. If anyone has any suggestions I would be most grateful.I will post the script in its entirety below.
clear; clc;
remote = tcpip('localhost', 4000);
%pause(7);
nframes = 1000;
QCM('connect', '127.0.0.1:22222','frameinfo','Analog:3,9'); % Connects to QTM and keeps the connection alive.
BW = 220; % Input bodyweight
stance = 0.95; %Add from previous session stanceTime avg
bodyweight = BW/2.2;
stanceAvgSD = stance/0.90; % Can alter denominator to define stance length requirement
highbound = (bodyweight * 9.8 * 0.9)/1000; % GRF Threshold for HS/TO Events
lowbound = (bodyweight * 9.8 * 0.25)/1000;
upperthresh = (bodyweight * 9.8 * 1.04)/1000; % GRF Threshold for Trip Onset
lowerthresh = (bodyweight * 9.8 * 0.98)/1000;
stanceTime = stance; % Preallocate StanceTime gets replaced in loop
stanceTrig = 0; % Preallocated condition state of StepTime threshold
% Preallocated for Analog Frame, GRF, and HS/TO Events
analogData=cell(2,nframes);
hs_frame = zeros(1,nframes);
to_frame = zeros(1,nframes);
frames = zeros(1,nframes);
tripFrame = zeros(1,nframes);
leftForce = [];
rightForce = [];
TO_frame = [];
HS_frame = [];
analog = [];
rightFz = [];
leftFz = [];
% Treadmill Trip Variables
cws = 0.72; % Input comfortable walking speed
accel = 5; % Input Acceleration of Trip
lowpoint = cws + 0.12 * accel * -1;
highpoint = cws + 0.10 * accel;
orispeed = [cws cws];
leftright = 'R';
if (leftright == 'L')
ForcePlateIndex = 1;
lowspeed = [cws lowpoint];
highspeed = [cws highpoint];
else
ForcePlateIndex = 2;
lowspeed = [lowpoint cws];
highspeed = [highpoint cws];
end
for i = 1:nframes
% ### Fetch data from QTM
[frameinfo Analog] = QCM;
current_frame=frameinfo(1);
frames(i) = current_frame;
try
Force = cell2mat(Analog(2,:)); analogData(:,i) = Analog; % Add current value
catch
isempty(Analog);
continue
end
if i > 5
for j = 1:length(Force)
RightFz = Force(2,j);
LeftFz = Force(1,j);
if j > 1
rightForce_previous = Force(2,j-1);
leftForce_previous = Force(1,j-1);
if ((LeftFz > lowbound) && (leftForce_previous < lowbound) && (LeftFz > leftForce_previous))
HS_frame = current_frame;
hs_frame(i) = current_frame;
elseif ((LeftFz < lowbound) && (leftForce_previous > lowbound) && (LeftFz < leftForce_previous))
TO_frame = current_frame;
to_frame(i) = current_frame;
try
stanceTime = ((TO_frame - HS_frame)/100);
catch
isempty(TO_frame);isempty(HS_frame);(HS_frame > TO_frame);
continue
end
if (stanceTime < stanceAvgSD)
stanceTrig = 1;
end
end
end
if ((stanceTrig ==1) && (RightFz > lowerthresh) && (RightFz < upperthresh) && (RightFz < rightForce_previous))
tripFrame(i) = current_frame;
tripframe = current_frame;
tripTrigger(remote,lowspeed,accel,orispeed);
stanceTrig = 0;
end
end
end
end
QCM('disconnect');
clear mex