0
$\begingroup$

I am trying to practice DH parameters and ran across the below configuration. Given the schematic in this picture without the addition of any other variables would it be correct to not acknowledge the highlighted area? I drew the x and z axis on the schematic and came up with the parameters in the picture but I am unsure if for the 0 and 1 frames I would have to acknowledge d1 (the variable in the schematic) or if in the second row I would have to acknowledge the highlighted area, can someone please help? Also as a general question where should the origin of each frame be if the frames don't intersect with one another?

First time posting, sorry for any formatting issues.
enter image description here

$\endgroup$

2 Answers 2

1
$\begingroup$

For the given diagram, it would be correct to ignore the highlighted area since it appears that L denotes the distance from the first joint to the second. Otherwise, you could equivalently draw frame 2 to be centered on the left edge of the highlighted line.

I think what you mean to represent in your solution is that frame 0 is fixed to the world, and frame 1 moves with the prismatic joint. In this case, the first row of your DH table, representing link 1, would have di = d1.

Frame origins can be drawn anywhere along the joint axis, and we usually choose the location based on ease of assigning DH values. You will get a hang of it with practice.

Here is some MATLAB code for visualizing links and joints based on given DH parameters. I find this helpful to check if my DH table is correct. It's from the foundational robotics course by UPenn on edX.

animate_lynx.m

%% Helper script to visualize forward kinematics of a lynx robot
% Fill in your code for the lynx_fk function before executing this script!

close all
pause on;  % Set this to on if you want to watch the animation
GraphingTimeDelay = 0.05; % The length of time that Matlab should pause between positions when graphing, if at all, in seconds.
totalTimeSteps = 100; % Number of steps in the animation

% Generate the starting and final joint angles, and gripper distance
% Set values manually, or randomise as shown below
q01 = 0;
q02 = 0;
q03 = 0;
q04 = 0;
q05 = 0;
g0 = 2;

q11 = pi/2;
q12 = 0;
q13 = 0;
q14 = 0;
q15 = 0;
g1 = 0;

% Use below code if you want to randomise
% q01 = rand(1) * 2 * pi;
% q02 = rand(1) * 2 * pi;
% q03 = rand(1) * 2 * pi;
% q04 = rand(1) * 2 * pi;
% q05 = rand(1) * 2 * pi;
% g0 = rand(1) * 2;
% 
% q11 = rand(1) * 2 * pi;
% q12 = rand(1) * 2 * pi;
% q13 = rand(1) * 2 * pi;
% q14 = rand(1) * 2 * pi;
% q15 = rand(1) * 2 * pi;
% g1 = 0;

% Setup plot
figure
scale_f = 20;
axis vis3d
axis(scale_f*[-1 1 -1 1 -1 1])
grid on
view(70,10)
xlabel('X (in.)')
ylabel('Y (in.)')
zlabel('Z (in.)')

% Plot robot initially
hold on
hrobot = plot3([0 0 10], [0 0 0], [0 6 6],'k.-','linewidth',2,'markersize',10);

% Animate the vector
pause(GraphingTimeDelay);
for i = 1:totalTimeSteps
   
    t = i/totalTimeSteps;
    pos = lynx_fk(q01*(1-t) + q11*(t), q02*(1-t) + q12*(t), q03*(1-t) + q13*(t), q04*(1-t) + q14*(t), q05*(1-t) + q15*(t), g0*(1-t) + g1*(t));
    
    set(hrobot,'xdata',[pos(1, 1) pos(2, 1) pos(3, 1) pos(4, 1) pos(5, 1) pos(6, 1) pos(7, 1) pos(9, 1) pos(7, 1) pos(8, 1) pos(10, 1)]',...
        'ydata',[pos(1, 2) pos(2, 2) pos(3, 2) pos(4, 2) pos(5, 2) pos(6, 2) pos(7, 2) pos(9, 2) pos(7, 2) pos(8, 2) pos(10, 2)]',...
        'zdata',[pos(1, 3) pos(2, 3) pos(3, 3) pos(4, 3) pos(5, 3) pos(6, 3) pos(7, 3) pos(9, 3) pos(7, 3) pos(8, 3) pos(10, 3)]');
    
    pause(GraphingTimeDelay);
end

compute_dh_matrix.m

function A = compute_dh_matrix(r, alpha, d, theta)

    %% Your code from the first part of this assignment goes here
    %% You can use this function in the lynx_fk function
    c_theta = cos(theta);
    s_theta = sin(theta);
    
    c_alpha = cos(alpha);
    s_alpha = sin(alpha);
    
    %A = eye(4);
    
    A(1, :) = [c_theta   -s_theta*c_alpha    s_theta*s_alpha    r*c_theta];
    A(2, :) = [s_theta    c_theta*c_alpha   -c_theta*s_alpha    r*s_theta];
    A(3, :) = [0          s_alpha            c_alpha            d];
    A(4, :) = [0          0                  0                  1];
    
    
end

lynx_fk.m

function [ pos ] = lynx_fk( theta1, theta2, theta3, theta4, theta5, g )
%LYNX_FK The input to the function will be the joint
%    angles of the robot in radians, and the distance between the gripper pads in inches.
%    The output must contain 10 positions of various points along the robot arm as specified
%    in the question.

    %% YOUR CODE GOES HERE
    
    % The physical link parameters are defined as given in the problem
    a = 3;
    b = 5.75;
    c = 7.375;
    d = 4.125;
    e = 1.125;

    % The various transformation matrices based on the DH parameters.
    % e.g. A_34 tranforms points from frame 4 to frame 3.
    A_01 = compute_dh_matrix(0, -pi/2, a, theta1);
    A_12 = compute_dh_matrix(b, 0, 0, theta2 - pi/2);
    A_23 = compute_dh_matrix(c, 0, 0, theta3 + pi/2);
    A_34 = compute_dh_matrix(0, -pi/2, 0, theta4 - pi/2);
    A_45 = compute_dh_matrix(0, 0, d, theta5);
    
    % Define a empty solution matrix
    pos = zeros(10, 4);
    
    % Find all the frame origin co-ordinates w.r.t. the world frame.
    pos(1, :) = [0 0 0 1];
    pos(2, :) = (A_01 * [0 0 0 1]')';
    pos(3, :) = (A_01 * A_12 * [0 0 0 1]')';
    pos(4, :) = (A_01 * A_12 * A_23 * [0 0 0 1]')';
    pos(5, :) = (A_01 * A_12 * A_23 * A_34 * [0 0 0 1]')';
    
    % The transformation matrix from the first to the last frame.
    T_05 = A_01 * A_12 * A_23 * A_34 * A_45;

    % Finding the gripper co-ordinates w.r.t the world frame.
    pos(6, :)  = (T_05 * [0    0 -e 1]')';
    pos(7, :)  = (T_05 * [g/2  0 -e 1]')';
    pos(8, :)  = (T_05 * [-g/2 0 -e 1]')';
    pos(9, :)  = (T_05 * [g/2  0  0 1]')';
    pos(10, :) = (T_05 * [-g/2 0  0 1]')';
    
    % Take the first 3 colums as the required solution. 4th column is all 1.
    pos = pos(:, 1:3);

end
$\endgroup$
2
  • $\begingroup$ Thank you for your answer, does this also mean that the world frame and frame 1 could have been drawn at the left edge and I could have ignored L completely? $\endgroup$
    – Tom
    Feb 18 at 5:06
  • $\begingroup$ @Tom That would defeat the purpose of the DH table. The reason we go through this exercise is to get transformation matrices from the world frame to each joint and the end effector. Given values for joint variables (d1, theta2), we can use the transformations obtained by the DH table to get the position of the arm end effector in the world frame (forward kinematics). If you were to draw frames 0 and 1 at the left edge, you would effectively ignore the prismatic joint and get forward kinematic relations in a "world" frame 0 that is moving with the arm. $\endgroup$
    – Srijal
    Feb 19 at 23:18
0
$\begingroup$

By definition, the (d) parameter in the Denavit-Hartenberg (DH) convention represents the distance/offset along the current frame's Z axis from the current frame's X axis to the next Frame's X axis. Some texts use previous and current instead of current and next. Both are equally correct.

As we can see from the configuration in the question, this distance depends on the variable (d) value of the prismatic joint added to the length highlighted in yellow. The answer to your question is no. We need to acknowledge the highlighted length. Let's call it (L_2).

The summation of d_1 + L_2 will be the distance along Z_0 between X_0 and X_1. It is redundant to have (L) length because the prismatic joint variable d_1 already represents the same length.

Notice: Each row in the DH table expresses a set of transformations to put the current frame on the next frame, which is the same as saying to transform current frame into next frame

I made small corrections to your frames. It is common to place a frame at the end to express the end-effector frame. It is counterintuitive to say frame 0 is at joint 1, and frame 1 at joint 2, and so on, but we get used to it with practice.

enter image description here

Then, the corresponding corrected DH table is as follows

Link (i) alpha_i a_i d_i theta_i
1 0 L_1 d_1 + L_2 0
2 0 0 L_3 theta_2

Let me, at the end, share with you a more intuitive way to figure out the parameters. Every time you look at a pair of frames starting from the robot base, refer to them as the (current) frame and the (next).

Theta: is the angle about the current frame's Z-axis between the current frame's X-axis and the next frame's X-axis.

d: is a distance along the current frame's Z-axis from the current frame's X-axis to the next frame's X-axis.

Alpha: is the angle about the next frame's X-axis between the current frame's Z-axis and the next frame's Z-axis.

a: is a distance along the next frame's X-axis from the current frame's Z-axis to the next frame's Z-axis.

There is a correspondence between those statements because the same transformations happen first with respect to the current frame's Z-axis and then with respect to the next frame's X-axis.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.