diff --git a/packages/MATLAB/mr/JacobianBody_Sym.m b/packages/MATLAB/mr/JacobianBody_Sym.m new file mode 100644 index 0000000..064383e --- /dev/null +++ b/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 \ No newline at end of file diff --git a/packages/MATLAB/mr/JacobianSpace_Sym.m b/packages/MATLAB/mr/JacobianSpace_Sym.m new file mode 100644 index 0000000..27e8140 --- /dev/null +++ b/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 \ No newline at end of file