36 lines
1.2 KiB
Mathematica
36 lines
1.2 KiB
Mathematica
|
function Tlist = FKinSpaceExpand(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 = zeros(4,4,length(thetalist));
|
||
|
|
Mi = eye(4);
|
||
|
|
for i = length(thetalist): -1: 1
|
||
|
|
for j = 1:i
|
||
|
|
Mi = Mi * Mlist(:, :, j);
|
||
|
|
end
|
||
|
|
Tlist(:,:,i) = FKinSpace(Mi, Slist(:,1:i), thetalist(1:i));
|
||
|
|
Mi = eye(4);
|
||
|
|
end
|
||
|
|
end
|