packages/MATLAB/mr/BodyVelToLinearVel.m

@@ -1,8 +0,0 @@
-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);

packages/MATLAB/mr/FKinSpaceExpand.m

@@ -1,36 +0,0 @@
-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

packages/MATLAB/mr/FKinSpaceExpand_Sym.m

@@ -1,36 +0,0 @@
-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

packages/MATLAB/mr/FKinSpace_Sym.m

@@ -1,31 +0,0 @@
-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

packages/MATLAB/mr/MatrixExp6_Sym.m

@@ -1,23 +0,0 @@
-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

packages/MATLAB/mr/SpaceVelToLinearVel.m

@@ -1,9 +0,0 @@
-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);

packages/MATLAB/mr/SpatialInertia.m

@@ -1,10 +0,0 @@
-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