Trouble plotting denavit hartenberg transformation matrix

Hi, i am currently working on a school project in Robotics, and i am to find the direct transformation of a RRPR robot in one of the problems. The robot in question can be seen in the picture below.

Where i have found the values for the dh table to be:

I have written this code, to create the homogeneos transformation matrices for each joint, and i tried making a model of it. The model however does not look anything like the robot itself, and i dont know why..

q1 = 0; %Angle

q2 = 0; %Angle

q3 = 0; %Length

q4 = 0; %Angle

a4 = 1.02;d1 = 1.5;

A1 = dh_transform(0,-pi/2,d1,q1); %Variable is theta

A2 = dh_transform(0,-pi/2,0,(pi/2+q2)); %Variable is theta

A3 = dh_transform(0,pi/2,(1.35+q3),pi); %Variable is d

A4 = dh_transform(a4,pi/2,0,(q4-pi/2)); %Variable is theta

T04 = A1*A2*A3*A4 %%Transformation matrix

dhparams = T04

robot = rigidBodyTree('DataFormat','row');

bodies = cell(4,1);

joints = cell(4,1);

for i = 1:4

if i == 3

bodies{i} = rigidBody(['body' num2str(i)]);

joints{i} = rigidBodyJoint(['jnt' num2str(i)],"prismatic");

setFixedTransform(joints{i},dhparams(i,:),"dh");

bodies{i}.Joint = joints{i};

else

bodies{i} = rigidBody(['body' num2str(i)]);

joints{i} = rigidBodyJoint(['jnt' num2str(i)],"revolute");

setFixedTransform(joints{i},dhparams(i,:),"dh");

bodies{i}.Joint = joints{i};

end

if i == 1 % Add first body to base

addBody(robot,bodies{i},"base")

else % Add current body to previous body by name

addBody(robot,bodies{i},bodies{i-1}.Name)

end

end

showdetails(robot)

show(robot)

Any help is appreciated!

Answer 1

[Umang Pandey]

Hi Kasper,

I understand that you want to construct an RRPR (Revolute-Revolute-Prismatic-Revolute) robot with specified Denavit-Hartenberg (DH) parameter values.

In your code, I noticed the use of "dh_transform" function which is not a built-in MATLAB function, and its definition wasn't provided in your message. However, given that you have the DH parameters, you can directly define the "dhparams" matrix as follows:

dhparams = [0 -pi/2 d1 q1;  
    0 -pi/2 0 pi/2+q2; 
    0 pi/2 1.35+q3 pi; 
    a4 pi/2 0 q4-pi/2]; 

For more information, you can refer to the following example on building PUMA560 manipulator robot using DH parameters:

https://www.mathworks.com/help/robotics/ug/build-manipulator-robot-using-kinematic-dh-parameters.html

Best,

Umang