Matlab Functions FKinBody and FKinSpace Output Incorrect Values

[Kilian-Olen]

I've been noticing many complaints, by students using this library, insisting that the FKinBody and FKinSpace functions do not output the correct values. Although the functions do output an appropriate matrix of the expected dimensions, when we compare the values with calculations done by hand we see that they do not agree. After looking into both functions, it suprised me to find that the fix was quite trivial.

Below is the original code for FKinBody.m (this reports no issues however the values are incorrect)
T = M;
for i = 1: size(thetalist)
T = T * MatrixExp6(VecTose3(Blist(:, i) * thetalist(i)));
end
end

Below is my correction which does output the correct values
T = M;
for i = 1: length(thetalist)
T = T * MatrixExp6(VecTose3(Blist(:, i) * thetalist(i)));
end
end

Pay close attention to the logic in the for loop. The function size() has been incorrectly used when instead we should be using length(). I believe that this issue arose from someone translating the library from one programming language to another, and they were not aware of the difference between these Matlab functions. Please update the Matlab library functions to prevent headaches for future students.