From 9dfa837e8cff5ed4d7a785ac05a1706b3835e966 Mon Sep 17 00:00:00 2001 From: Rui Date: Sun, 5 Nov 2023 02:04:04 +0000 Subject: [PATCH] Feature/add inertia and FK expand --- packages/MATLAB/mr/BodyVelToLinearVel.m | 8 ++++++ packages/MATLAB/mr/FKinSpaceExpand.m | 36 ++++++++++++++++++++++++ packages/MATLAB/mr/FKinSpaceExpand_Sym.m | 36 ++++++++++++++++++++++++ packages/MATLAB/mr/FKinSpace_Sym.m | 31 ++++++++++++++++++++ packages/MATLAB/mr/MatrixExp6_Sym.m | 23 +++++++++++++++ packages/MATLAB/mr/SpaceVelToLinearVel.m | 9 ++++++ packages/MATLAB/mr/SpatialInertia.m | 10 +++++++ 7 files changed, 153 insertions(+) create mode 100644 packages/MATLAB/mr/BodyVelToLinearVel.m create mode 100644 packages/MATLAB/mr/FKinSpaceExpand.m create mode 100644 packages/MATLAB/mr/FKinSpaceExpand_Sym.m create mode 100644 packages/MATLAB/mr/FKinSpace_Sym.m create mode 100644 packages/MATLAB/mr/MatrixExp6_Sym.m create mode 100644 packages/MATLAB/mr/SpaceVelToLinearVel.m create mode 100644 packages/MATLAB/mr/SpatialInertia.m diff --git a/packages/MATLAB/mr/BodyVelToLinearVel.m b/packages/MATLAB/mr/BodyVelToLinearVel.m new file mode 100644 index 0000000..59574a2 --- /dev/null +++ b/packages/MATLAB/mr/BodyVelToLinearVel.m @@ -0,0 +1,8 @@ +function Vlinear = BodyVelToLinearVel(V,G) +% *** CHAPTER x: DYNAMICS OF OPEN CHAINS *** +% Takes V: Body frame velocity, +% G: Spactial frame G +% Returns Glist: Spatial inertia matrices Gi of the links + +[R, p] = TransToRp(G); +Vlinear = R*V(4:6); diff --git a/packages/MATLAB/mr/FKinSpaceExpand.m b/packages/MATLAB/mr/FKinSpaceExpand.m new file mode 100644 index 0000000..15e8e39 --- /dev/null +++ b/packages/MATLAB/mr/FKinSpaceExpand.m @@ -0,0 +1,36 @@ +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,size(thetalist,1)); +Mi = eye(4); +for i = size(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 \ No newline at end of file diff --git a/packages/MATLAB/mr/FKinSpaceExpand_Sym.m b/packages/MATLAB/mr/FKinSpaceExpand_Sym.m new file mode 100644 index 0000000..95162fb --- /dev/null +++ b/packages/MATLAB/mr/FKinSpaceExpand_Sym.m @@ -0,0 +1,36 @@ +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 \ No newline at end of file diff --git a/packages/MATLAB/mr/FKinSpace_Sym.m b/packages/MATLAB/mr/FKinSpace_Sym.m new file mode 100644 index 0000000..fdae8dd --- /dev/null +++ b/packages/MATLAB/mr/FKinSpace_Sym.m @@ -0,0 +1,31 @@ +function T = FKinSpace(M, 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 + +T = M; +for i = size(thetalist): -1: 1 + T = MatrixExp6_Sym(VecTose3(Slist(:, i) * thetalist(i))) * T; +end +end \ No newline at end of file diff --git a/packages/MATLAB/mr/MatrixExp6_Sym.m b/packages/MATLAB/mr/MatrixExp6_Sym.m new file mode 100644 index 0000000..d17ae49 --- /dev/null +++ b/packages/MATLAB/mr/MatrixExp6_Sym.m @@ -0,0 +1,23 @@ +function T = MatrixExp6_Sym(se3mat) +% *** CHAPTER 3: RIGID-BODY MOTIONS *** +% Takes a se(3) representation of exponential coordinates. +% Returns a T matrix in SE(3) that is achieved by traveling along/about the +% screw axis S for a distance theta from an initial configuration T = I. +% Example Input: +% +% clear; clc; +% se3mat = [ 0, 0, 0, 0; +% 0, 0, -1.5708, 2.3562; +% 0, 1.5708, 0, 2.3562; +% 0, 0, 0, 0] +% T = MatrixExp6(se3mat) +% +% Output: +% T = +% 1.0000 0 0 0 +% 0 0.0000 -1.0000 -0.0000 +% 0 1.0000 0.0000 3.0000 +% 0 0 0 1.0000 + +T = expm(se3mat); +end \ No newline at end of file diff --git a/packages/MATLAB/mr/SpaceVelToLinearVel.m b/packages/MATLAB/mr/SpaceVelToLinearVel.m new file mode 100644 index 0000000..b85b33e --- /dev/null +++ b/packages/MATLAB/mr/SpaceVelToLinearVel.m @@ -0,0 +1,9 @@ +function Vlinear = BodyVelToLinearVel(V,G) +% *** CHAPTER x: DYNAMICS OF OPEN CHAINS *** +% Takes V: Body frame velocity, +% G: Space frame G +% Returns Glist: Spatial inertia matrices Gi of the links + +[R, p] = TransToRp(G); +Vlienar = se3ToVec(V)*[p;1]; +Vlienar = Vlienar(1:3); diff --git a/packages/MATLAB/mr/SpatialInertia.m b/packages/MATLAB/mr/SpatialInertia.m new file mode 100644 index 0000000..5a004e2 --- /dev/null +++ b/packages/MATLAB/mr/SpatialInertia.m @@ -0,0 +1,10 @@ +function Glist = SpatialInertia(G) +% *** CHAPTER x: DYNAMICS OF OPEN CHAINS *** +% Takes G: A list of inertia, +% Returns Glist: Spatial inertia matrices Gi of the links + +n = size(G,1); +Glist = zeros(6,6,n); +for i = 1:n + Glist(:,:,i) = diag(G(i,1:6)); +end