I calculated the forward kinematics for an articulated manipulator robot in ROS, but I am not getting the right answer. Here is the diagram for the robot configuration I used-
When Q1,Q2,and Q3 are zero this calculation is supposed to give the dx,dy and dz as 2,0,1 if I consider all the link lengths as 1.But after I substitute these values of Q1=0,Q2=0, and Q3=0 I get dx,dy, and dz as 2,0,0. I double checked the DH parameters and calculations in Matlab, but the values are still 2,0,0. I am not sure why I am not getting the right answer. Maybe I am getting the right answer, but I don't understand it.
My Matlab calculation after substituting Q1, Q2,and Q3 as 0,0,0-
>> a1
a1 =
1 0 0 0
0 0 -1 0
0 1 0 0
0 0 0 1
>> a2
a2 =
1 0 0 1
0 1 0 0
0 0 1 0
0 0 0 1
>> a3
a3 =
1 0 0 1
0 1 0 0
0 0 1 0
0 0 0 1
>> t06=a1*a2*a3
t03 =
1 0 0 2
0 0 -1 0
0 1 0 0
0 0 0 1
>>
In my inverse kinematics code when I enter the coordinates of end effector as 2,0,1, I get correct Q values i.e 0,0,0 or 0,0,2Pi. Where am I going wrong?