# convert toolframe coordinate to world frame coordinates?

I am not sure how i should explain this, I am looking for a way to plot the trajectory an robot arm. An object is seen from the toolFrame frame, but how do I plot the position of each joint, such that the frame they uses are the same.

One way would be to use the world frame as reference, but how would i plot the position of the object related to the world frame?

• What environment are you drawing in? Matlab? Is there a mathematical model of the robot available in that environment? – Bending Unit 22 May 17 '16 at 9:11
• Matlab.. I have a mathmatical model, but not coded in matlab. At the moment I just multply different transformation matrices to get the point i want the desired transformation – James potter May 17 '16 at 9:17
• "An object is seen from the toolFrame frame" this is what makes it a little unclear to me what you want exactly. Do you want to visualize the trajectories in the tool frame? – Bending Unit 22 May 17 '16 at 10:26
• The position of the object is given relative to the tool frame. – James potter May 17 '16 at 10:57
• I would like to visualize trajectory of the whole robot arm, each joint.. – James potter May 17 '16 at 10:57

Assuming you have solved the inverse kinematics (IK) problem already...

I suppose you have a transformation matrix for each joint, build up from one line in the DH table (if you used DH to describe the robot). Form the IK you have obtained

$Q = [q_1, q_2, q_3, q_4, q_5, q_6]$

Having all Q values you can now write:

$H_{0,1} = A_1(q_1)$

$\Rightarrow$ $H_{0,1}(1:3, 4)$ will give you the $x$, $y$ and $z$ Coordinates of joint 1

$H_{0,2} = H_{0,1}*A_2(q_2)$

$\Rightarrow$ $H_{0,2}(1:3, 4)$ will give you the $x$, $y$ and $z$ Coordinates of joint 2

$H_{0,3} = H_{0,2}*A_3(q_3)$

$\Rightarrow$ $H_{0,3}(1:3, 4)$ will give you the $x$, $y$ and $z$ Coordinates of joint 3

$H_{0,4} = H_{0,3}*A_4(q_4)$

$\Rightarrow$ $H_{0,4}(1:3, 4)$ will give you the $x$, $y$ and $z$ Coordinates of joint 4

$H_{0,5} = H_{0,4}*A_5(q_5)$

$\Rightarrow$ $H_{0,5}(1:3, 4)$ will give you the $x$, $y$ and $z$ Coordinates of joint 5

$H_{0,6} = H_{0,5}*A_6(q_6)$

$\Rightarrow$ $H_{0,6}(1:3, 4)$ will give you the $x$, $y$ and $z$ Coordinates of joint 6

$H_{0,tool} = H_{0,6}*A_{tool}$

where

• $A_i$ is the DH-transformation matrix associated with joint $i$.
• the index $_0$ represents the world frame
• the intex $_{tool}$ represents the tool frame
• $H_{i,j}$ represent the transformation matrix from frame $i$ to frame $j$
• What about the position of the object related to the world? – James potter May 17 '16 at 12:11
• I don't know if thats wasn't clear, but my problem is the axis is misalligned, as the position of objected is logged according to the tool – James potter May 17 '16 at 12:13
• If you solved the inverse kinematics then it does not matter...that is why the answer starts with "asuming you have solved the IK" – 50k4 May 17 '16 at 12:14
• did you solve the IK? – 50k4 May 17 '16 at 12:15
• If I understand correctly, you have all the joint angles ($Q$). Those define the pose of the robot regardless of any other frames. Those are always expressed in their local frames. The transition from world frame to frame of joint 1 is $A_1$. The transition is dependent on joint angle and robot geometry only. You transition by multiplication and plot the frame origin (x, y and z). Then you go to the next joint and plot the frame origin and so on until you reach the end-effector. At the end you will get the location of each joint in world frame. connect the lines and you can draw your robot. – 50k4 May 17 '16 at 14:25