Robotics
/
Modern_Robotics
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add FK at each joint

This commit is contained in:
cosmic_power 2023-11-05 10:02:25 +08:00
parent 9274ea0a1a
commit 837813a88e
2 changed files with 14 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
packages/MATLAB/mr/FKinSpaceExpand.m
View File

@ -25,9 +25,12 @@ function Tlist = FKinSpaceExpand(Mlist, Slist, thetalist)
% 0 0 0 1.0000
Tlist = zeros(4,4,size(thetalist,1));
% Tlist(:,:,size(thetalist,1)) = M;
% T=M;
Mi = eye(4);
for i = size(thetalist): -1: 1
Tlist(:,:,i) = FKinSpace(Mlist(:,:,i), Slist(:,1:i), thetalist(1:i));
for j = 1:i
Mi = Mi * Mlist(:, :, j);
end
Tlist(:,:,i) = FKinSpace(Mi, Slist(:,1:i), thetalist(1:i));
Mi = eye(4);
end
end

13
packages/MATLAB/mr/FKinSpaceExpand_Sym.m
View File

@ -1,4 +1,4 @@
function Tlist = FKinSpaceExpand(Mlist, Slist, thetalist)
function Tlist = FKinSpaceExpand_Sym(Mlist, Slist, thetalist)
% *** CHAPTER 4: FORWARD KINEMATICS ***
% Takes M: the home configuration (position and orientation) of the
% end-effector,
@ -24,10 +24,13 @@ function Tlist = FKinSpaceExpand(Mlist, Slist, thetalist)
% 0 0 -1.0000 1.6858
% 0 0 0 1.0000
Tlist = zeros(4,4,size(thetalist,1));
% Tlist(:,:,size(thetalist,1)) = M;
% T=M;
Tlist = sym(zeros(4,4,size(thetalist,1)));
Mi = sym(eye(4));
for i = size(thetalist): -1: 1
Tlist(:,:,i) = FKinSpace_Sym(Mlist(:,:,i), Slist(:,1:i), thetalist(1:i));
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