Trouble Computing Jacobian for 3R Robotic Manipulator
17 ビュー (過去 30 日間)
古いコメントを表示
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]);
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 件のコメント
回答 (0 件)
参考
カテゴリ
Help Center および File Exchange で Code Generation についてさらに検索
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!