Modern_Robotics/packages/MATLAB/mr/FKinSpaceExpand_Sym.m

function Tlist = FKinSpaceExpand_Sym(Mlist, Slist, thetalist)
% *** CHAPTER 4: FORWARD KINEMATICS ***
% Takes M: the home configuration (position and orientation) of the 
%          end-effector,
%       Slist: The joint screw axes in the space frame when the manipulator
%              is at the home position,
%       thetalist: A list of joint coordinates.
% Returns T in SE(3) representing the end-effector frame, when the joints 
% are at the specified coordinates (i.t.o Space Frame).
% Example Inputs:
% 
% clear; clc;
% M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]];
% Slist = [[0; 0;  1;  4; 0;    0], ...
%        [0; 0;  0;  0; 1;    0], ...
%        [0; 0; -1; -6; 0; -0.1]];
% thetalist =[pi / 2; 3; pi];
% T = FKinSpace(M, Slist, thetalist)
% 
% Output:
% T =
%   -0.0000    1.0000         0   -5.0000
%    1.0000    0.0000         0    4.0000
%         0         0   -1.0000    1.6858
%         0         0         0    1.0000

Tlist = sym(zeros(4,4,size(thetalist,1)));
Mi = sym(eye(4));
for i = size(thetalist): -1: 1
    for j = 1:i
        Mi = Mi * Mlist(:, :, j);
    end
    Tlist(:,:,i) = FKinSpace_Sym(Mi, Slist(:,1:i), thetalist(1:i));
    Mi = sym(eye(4));
end
end