How to generate a function which can give the transformation matrix for given DH parameters?

I want to generate a function which can give transformation matrix for given DH parameters. The cache is the function should valid for any number of links. It means number of links are variable. Like if I want take 4 ,5 or6 any no of links it should take dh parameters for all the links and should give the final transformation matrix.

Answer 1

[Karsh Tharyani]

Hi @Shashank Thakker,

If you are aware about the DH parameters for your robot, you can assemble a rigidBodyTree and use the getTransform function on the tree which provides the pose of a rigidBody on the tree with respect to the tree's base body. The tree doesn't limit the number of bodies (links) or joints you can add to it.

The following example in the MathWorks Documentation: https://www.mathworks.com/help/robotics/ug/build-manipulator-robot-using-kinematic-dh-parameters.html goes through this in great detail. Here are some snippets from that example that show how to use the setFixedTransform to specify the DH parameters of a rigidBodyJoint while assembling the rigidBodyTree representing a robot.

robot=rigidBodyTree(DataFormat="row");
dhparams = [0   	pi/2	0   	0;
            0.4318	0       0       0
            0.0203	-pi/2	0.15005	0;
            0   	pi/2	0.4318	0;
            0       -pi/2	0   	0;
            0       0       0       0];
bodies = cell(6,1);
joints = cell(6,1);
for i = 1:6
    bodies{i} = rigidBody(['body' num2str(i)]);
    joints{i} = rigidBodyJoint(['jnt' num2str(i)],"revolute");
    setFixedTransform(joints{i},dhparams(i,:),"dh");
    bodies{i}.Joint = joints{i};
    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)

--------------------
Robot: (6 bodies)

 Idx    Body Name   Joint Name   Joint Type    Parent Name(Idx)   Children Name(s)
 ---    ---------   ----------   ----------    ----------------   ----------------
   1        body1         jnt1     revolute             base(0)   body2(2)  
   2        body2         jnt2     revolute            body1(1)   body3(3)  
   3        body3         jnt3     revolute            body2(2)   body4(4)  
   4        body4         jnt4     revolute            body3(3)   body5(5)  
   5        body5         jnt5     revolute            body4(4)   body6(6)  
   6        body6         jnt6     revolute            body5(5)   
--------------------

% Find the pose (4-by-4 homogeneous transformation matrix) of 'body6' with
% respect to the 'base' if the first joint position is pi/2 rad, fifth joint is at -pi/4 rad, 
% and the rest are zero radians.
q=[pi/2,0,0,0,-pi/4,0];
disp(q)

    1.5708         0         0         0   -0.7854         0

Hbase_body6=getTransform(robot,q,'body6');
disp(Hbase_body6)

    0.0000   -1.0000    0.0000    0.1501
    0.7071    0.0000    0.7071    0.4521
   -0.7071   -0.0000    0.7071    0.4318
         0         0         0    1.0000

show(robot,q);

Answer 2

[Pushpendra Gupta]

syms L1 L2 L3 L4 L5 Q1 Q2 Q3 Q4 Q5
alphaa = [0,0,-90,0,+90]; % this is the alpha value for all  the link
a=[L1,L2, L3, L4, L5]; % Length of the Link
d=[0,0,0,0,0]; %Offset
Q=[Q1,Q2,Q3,Q4,Q5]; % joint angle variation

%% Transformation Matrices
for i = 1:5
switch i
    case 1
       T01= [cos(Q(1,i)),-sin(Q(1,i))*cosd(alphaa(1,i)),sind(alphaa(1,i))*sin(Q(1,i)),a(1,i)*cos(Q(1,i));sin(Q(1,i)),cos(Q(1,i)).*cosd(alphaa(1,i)),-sind(alphaa(1,i))*cos(Q(1,i)),sin(Q(1,i))*a(1,i);0,sind(alphaa(1,i)),cosd(alphaa(1,i)),d(1,i);0,0,0,1];
    case 2
        T12= [cos(Q(1,i)),-sin(Q(1,i))*cosd(alphaa(1,i)),sind(alphaa(1,i))*sin(Q(1,i)),a(1,i)*cos(Q(1,i));sin(Q(1,i)),cos(Q(1,i)).*cosd(alphaa(1,i)),-sind(alphaa(1,i))*cos(Q(1,i)),sin(Q(1,i))*a(1,i);0,sind(alphaa(1,i)),cosd(alphaa(1,i)),d(1,i);0,0,0,1];
    case 3
        T23= [cos(Q(1,i)),-sin(Q(1,i))*cosd(alphaa(1,i)),sind(alphaa(1,i))*sin(Q(1,i)),a(1,i)*cos(Q(1,i));sin(Q(1,i)),cos(Q(1,i)).*cosd(alphaa(1,i)),-sind(alphaa(1,i))*cos(Q(1,i)),sin(Q(1,i))*a(1,i);0,sind(alphaa(1,i)),cosd(alphaa(1,i)),d(1,i);0,0,0,1];
    case 4
        T34= [cos(Q(1,i)),-sin(Q(1,i))*cosd(alphaa(1,i)),sind(alphaa(1,i))*sin(Q(1,i)),a(1,i)*cos(Q(1,i));sin(Q(1,i)),cos(Q(1,i)).*cosd(alphaa(1,i)),-sind(alphaa(1,i))*cos(Q(1,i)),sin(Q(1,i))*a(1,i);0,sind(alphaa(1,i)),cosd(alphaa(1,i)),d(1,i);0,0,0,1];
    case 5
        T45= [cos(Q(1,i)),-sin(Q(1,i))*cosd(alphaa(1,i)),sind(alphaa(1,i))*sin(Q(1,i)),a(1,i)*cos(Q(1,i));sin(Q(1,i)),cos(Q(1,i)).*cosd(alphaa(1,i)),-sind(alphaa(1,i))*cos(Q(1,i)),sin(Q(1,i))*a(1,i);0,sind(alphaa(1,i)),cosd(alphaa(1,i)),d(1,i);0,0,0,1];
        
end
end

T01 % First Link with respect to base

T01 = 

T02 = T01*T12 % Second Link with respect to base

T02 = 

%% You can simplify it too
simplify(T02)

ans = 

%% you can find the position and Orientation of 5th Link with respect to Base Link using

T05 = T01*T12*T23*T34*T45;
simplify(T05)

ans = 

This is for symbolic form in case of numeric evaluation use makehgtform

https://in.mathworks.com/help/matlab/ref/makehgtform.html