33 lines
1.1 KiB
Matlab
33 lines
1.1 KiB
Matlab
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 |