From 837813a88e9544f9c4cfa7f1853dab76e9aafe3e Mon Sep 17 00:00:00 2001 From: cosmic_power Date: Sun, 5 Nov 2023 10:02:25 +0800 Subject: [PATCH] add FK at each joint --- packages/MATLAB/mr/FKinSpaceExpand.m | 9 ++++++--- packages/MATLAB/mr/FKinSpaceExpand_Sym.m | 13 ++++++++----- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/packages/MATLAB/mr/FKinSpaceExpand.m b/packages/MATLAB/mr/FKinSpaceExpand.m index b554af0..15e8e39 100644 --- a/packages/MATLAB/mr/FKinSpaceExpand.m +++ b/packages/MATLAB/mr/FKinSpaceExpand.m @@ -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 \ No newline at end of file diff --git a/packages/MATLAB/mr/FKinSpaceExpand_Sym.m b/packages/MATLAB/mr/FKinSpaceExpand_Sym.m index 296a6c6..95162fb 100644 --- a/packages/MATLAB/mr/FKinSpaceExpand_Sym.m +++ b/packages/MATLAB/mr/FKinSpaceExpand_Sym.m @@ -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 \ No newline at end of file