Trouble Computing Jacobian for 3R Robotic Manipulator

17 views (last 30 days)
Ibrahim
Ibrahim on 28 Mar 2024 at 4:56
Edited: Sam Chak on 28 Mar 2024 at 6:16
Hello all readers,
I am trying to compute the jacobian matrix of a 3R robotic manipulator using the velocity propagation method but I don't understand what I am doing wrong. I'll appreciate any input. and if this is not the place to post, please guide me where I can find a solution
%----------------------------------------------------
%% The Script
clear, clc
sympref('FloatingPointOutput', true); %for readability
syms q0 q1 q2 J
% Simple Implementation using Robotics Toolbox
L1 = 0.3; L2 = 0.3; L3=0.3; % Length of the robot link
%We can create each link like this:
%Link([theta d a alpha r/p)]
L(1) = Link([0 L1 0 pi/2 0]);
Unrecognized function or variable 'Link'.
L(2) = Link([0 0 L2 0]);
L(3) = Link([0 0 L3 0 0]);
robot=SerialLink(L,'name','Qrevo');
f_kine = robot.fkine([q0 q1 q2])
jacobian = robot.jacob0([q0 q1 q2])
dh_params = [0 pi/2 0.3 q0;
0.3 0 0 q1;
0.3 0 0 q2];
n = size(dh_params, 1);
T = eye(4);
% J = zeros(6,n);
T_0_6 = get_fk(dh_params);
point_end = T_0_6(1:3,3);
for i = 1:n
% Extract the DH parameters for the current link
a = dh_params(i, 1);
alpha = dh_params(i, 2);
d = dh_params(i, 3);
theta = dh_params(i, 4);
T_i = [cos(theta) -sin(theta)*cos(alpha) sin(theta)*sin(alpha) a*cos(theta);
sin(theta) cos(theta)*cos(alpha) -cos(theta)*sin(alpha) a*sin(theta);
0 sin(alpha) cos(alpha) d;
0 0 0 1];
T = T * T_i;
% Compute the Jacobian columns
z = T(1:3, 3);
p = T(1:3, 4);
r = point_end - p;
J(1:3,i) = cross(z, r);
J(4:6,i) = z;
end
simplify(jacobian)
simplify(J)
dh_test=[0 pi/2 0.3 1;
0.3 0 0 0;
0.3 0 0 3];
real_num = robot.jacob0([1 0 3])
real_num_2 = get_jacob(dh_test)
%----------------------------------------------------
%% get_jacob() function
function J = get_jacob(dh_params)
n = size(dh_params, 1);
T = eye(4);
J = zeros(6,n);
T_0_6 = get_fk(dh_params);
point_end = T_0_6(1:3,3);
for i = 1:n
% Extract the DH parameters for the current link
a = dh_params(i, 1);
alpha = dh_params(i, 2);
d = dh_params(i, 3);
theta = dh_params(i, 4);
T_i = [cos(theta) -sin(theta)*cos(alpha) sin(theta)*sin(alpha) a*cos(theta);
sin(theta) cos(theta)*cos(alpha) -cos(theta)*sin(alpha) a*sin(theta);
0 sin(alpha) cos(alpha) d;
0 0 0 1];
T = T * T_i;
% Compute the Jacobian columns
z = T(1:3, 3);
p = T(1:3, 4);
r = point_end - p;
J(1:3,i) = cross(z, r);
J(4:6,i) = z;
end
end
%----------------------------------------------------
%% get_fk() function
function T = get_fk(dh_params)
n = size(dh_params, 1);
T = eye(4);
for i = 1:n
a = dh_params(i, 1);
alpha = dh_params(i, 2);
d = dh_params(i, 3);
theta = dh_params(i, 4);
T_i = [cos(theta) -sin(theta)*cos(alpha) sin(theta)*sin(alpha) a*cos(theta);
sin(theta) cos(theta)*cos(alpha) -cos(theta)*sin(alpha) a*sin(theta);
0 sin(alpha) cos(alpha) d;
0 0 0 1];
T = T * T_i;
end
end
As you can see attached below, I am getting different results when I am comparing my solution to the toolbox's solution

Answers (0)

Products


Release

R2022b

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!