Merge branch 'feature/Jacobian_sym' into 'main'

Feature/jacobian sym

See merge request robotics/modern-robotics!3

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);

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

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

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

packages/MATLAB/mr/JacobianBody_Sym.m

@@ -0,0 +1,33 @@
+function Jb = JacobianBody_Sym(Blist, thetalist)
+% *** CHAPTER 5: VELOCITY KINEMATICS AND STATICS ***
+% Takes Blist: The joint screw axes in the end-effector frame when the
+%              manipulator is at the home position, in the format of a 
+%              matrix with the screw axes as the columns,
+%       thetalist: A list of joint coordinates.
+% Returns the corresponding body Jacobian (6xn real numbers).
+% Example Input:
+% 
+% clear; clc;
+% Blist = [[0; 0; 1;   0; 0.2; 0.2], ...
+%        [1; 0; 0;   2;   0;   3], ...
+%        [0; 1; 0;   0;   2;   1], ...
+%        [1; 0; 0; 0.2; 0.3; 0.4]];
+% thetalist = [0.2; 1.1; 0.1; 1.2];
+% Jb = JacobianBody(Blist, thetalist)
+% 
+% Output:
+% Jb =
+%   -0.0453    0.9950         0    1.0000
+%    0.7436    0.0930    0.3624         0
+%   -0.6671    0.0362   -0.9320         0
+%    2.3259    1.6681    0.5641    0.2000
+%   -1.4432    2.9456    1.4331    0.3000
+%   -2.0664    1.8288   -1.5887    0.4000
+
+Jb = sym(Blist);
+T = sym(eye(4));
+for i = length(thetalist) - 1: -1: 1   
+    T = T * expm(VecTose3(-1 * Blist(:, i + 1) * thetalist(i + 1)));
+	Jb(:, i) = Adjoint(T) * Blist(:, i);
+end
+end

packages/MATLAB/mr/JacobianSpace_Sym.m

@@ -0,0 +1,33 @@
+function Js = JacobianSpace_Sym(Slist, thetalist)
+% *** CHAPTER 5: VELOCITY KINEMATICS AND STATICS ***
+% Takes Slist: The joint screw axes in the space frame when the manipulator
+%              is at the home position, in the format of a matrix with the
+%              screw axes as the columns,
+%       thetalist: A list of joint coordinates. 
+% Returns the corresponding space Jacobian (6xn real numbers).
+% Example Input:
+% 
+% clear; clc;
+% Slist = [[0; 0; 1;   0; 0.2; 0.2], ...
+%        [1; 0; 0;   2;   0;   3], ...
+%        [0; 1; 0;   0;   2;   1], ...
+%        [1; 0; 0; 0.2; 0.3; 0.4]];
+% thetalist = [0.2; 1.1; 0.1; 1.2];
+% Js = JacobianSpace(Slist, thetalist)
+% 
+% Output:
+% Js =
+%         0    0.9801   -0.0901    0.9575
+%         0    0.1987    0.4446    0.2849
+%    1.0000         0    0.8912   -0.0453
+%         0    1.9522   -2.2164   -0.5116
+%    0.2000    0.4365   -2.4371    2.7754
+%    0.2000    2.9603    3.2357    2.2251
+
+Js = sym(Slist);
+T = sym(eye(4));
+for i = 2: length(thetalist)
+    T = T * expm(VecTose3(Slist(:, i - 1) * thetalist(i - 1)));
+	Js(:, i) = Adjoint(T) * Slist(:, i);
+end
+end

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

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);

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