Trouble Computing Jacobian for 3R Robotic Manipulator

17 ビュー (過去 30 日間)
Ibrahim
Ibrahim 2024 年 3 月 28 日 4:56
編集済み: Sam Chak 2024 年 3 月 28 日 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

回答 (0 件)

カテゴリ

Help Center および File ExchangeCode Generation についてさらに検索

製品


リリース

R2022b

Community Treasure Hunt

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

Start Hunting!

Translated by