# inverse kinematics for 6 jointed robots

I am a uncertain about how to compute the right homogeneous transformation matrix to compute an inverse kinematic Q-configuration.

Looking at robot like this Where at the end of this robot I have a camera mounted on to it.

The purpose of my application is to make the robot follow an object, so basically tracking it. The camera provide me with an X,Y,Z coordinate, which the position i want place my robot arm.

First question - How do i set up the desired homogenous transformation matrix?

The way i see it, I have 2 transformation matrices being T_tool_base and T_world_tool which become T_world_base = (T_tool_base) (T_world_tool)

My question is that how do i compute my desired transformation matrix. I think i know how i should setup the transformation matrix for the camera which would be like this

T_world_tool = 0 0 0 x 0 0 0 y 0 0 0 z 0 0 0 1

(Second question is regarding the rotation matrix, how do prescribe such that rotation in arbitrary as long the endpoint has the desired position in the world frame?)

but what should t_tool_base entail? should it entail the transformation of its current state or the desired transformation, and if so how do i extract the desired t_tool_base transformation?...

I would recommend changing the naming convention since it is a bit misleading. In robotics the world Coordinate system (CS) is usually your fixed, absolute coordinate system. Lets call the transformation matrix from your camera to your object $T_{Object,Tool}$ If it cannot include any rotation, then you are right is should have the form as you specified. You are also right with your multiplications, but I would advise keeping the two thing separate. I actually do not think that you need the $T_{Object,Tool}$ matrix

The $T_{Tool,Base}$ is a transformation matrix form your tool to your base. This is what you can use to solve the inverse kinematics problem.

1. Define your coordinate systems (with the Denavit Hartenberg convention)
2. Build your transformation matrix as $T_{Tool,Base} = A1(q1) \cdot A2(q2) \cdot A3(q3)\cdot A4(q4)\cdot A5(q5)\cdot A6(q6)$
3. Build the Cartesian transformation Matrix as $T_{TCP} = X_{trans}\cdot Y_{trans}\cdot Z_{trans}\cdot X_{rot}\cdot Y_{rot}\cdot Z_{rot}$
4. Solve the inverse kinematics problem from

$T_{TCP} = T_{Tool,Base}$

as presented in this description. After this step you should have a function that for a given Cartesian coordinates of the TCP as input $X = (x, y, z, a, b, c)$ calculates the corresponding joint position as output $Q = (q_1, q_2, q_3, q_4, q_5, q_6)$

1. Your camera will measure the relative position of the object. Lets define the vector of measurments as $X_{Object} = (x, y, z, 0, 0, 0)$. The current Cartesian position of the robot is known (after solving the IK problem). Lets call this $X_{TCP} = (x_r, y_r, z_r, a_r, b_r, c_r)$. If you only want to track positions then $X_{new} = X_{Object} + X_{TCP}$ will give you the coordinates you want to position your TCP. Use this as input for the inverse kinematic function and get the $Q$ angles which correspond to this position. If you want to orient the TCP toward the object calculate also the new realative angles geometrically then add those also (instead of the $0, 0, 0$). That is it!