Inverse Kinematics does not give the desired joint angles

Question

I have a location of a tool with respect to the base. That location is described by an x,y,z coordinates and three rotations. I call that $Target$. I want to make a geometrical approach on the first three angles. I first have to remove some parts of my target to get it to the wrist and also remove the base. This. First I remove the base, then the wrist to tool(TWT) and lastly the frame 6 wrist(T6T). Then you end up with transformation from 0 to 6 (T60). There are 6 frames.

Essentially we did this

$T06=TB0^{-1}\cdot target \cdot TWT^{-1} \cdot T6W^{-1}$

When I as shown in the figure, when theta 1 is calculated for it makes athis equation.

$\theta_1=tan^{-1}(y/x)$

This gave the wrong answer. Since the trigonometry was straightforward, my hunch is that the above statement where we changed the transformation was at fault. Did we do it correctly is my question.

Answer 1

I hope introducing yet another variation on the notation does not confuse things but I strongly prefer the following layout for a transform because it makes chaining transformations the wrong way around pretty much impossible.
${}^aT_b$ describes the position of b in frame a.
${}^aT_{b} * {}^bT_c$ -> correct chaining
${}^aT_b * {}^cT_b$ -> clearly wrong

Your transformations:
T06 -> ${}^0T_6$ frame 6 described in frame 0
TB0 -> ${}^BT_0$ frame 0 described in frame 'base'
target -> ${}^BT_{tool}$ frame 'tool' described in frame 'base'
TWT -> ${}^{wrist}T_{tool}$ frame 'tool' described in frame 'wrist'
T6W -> ${}^{6}T_{wrist}$ frame 'wrist' described in frame 6
(btw you refer to 'frame 6 wrist(T6T)' first in the description and and then 'T6W' in the equation - is this intentional?)

So if the above is all correct, then we have:
${}^0T_6 = ({}^BT_0)^{-1} * {}^BT_{tool} * ({}^{wrist}T_{tool})^{-1} * ({}^{6}T_{wrist})^{-1}$
Let's flip the frames on the inverses to make it more readable
${}^0T_6 = {}^0T_B * {}^BT_{tool} * {}^{tool}T_{wrist} * {}^{wrist}T_{6}$
Which we can easily see now looks correct so I don't think there is a problem with this part (assuming those transformations are all set up correctly).

I assume you are taking y and x from this ${}^0T_6$? In which case, based on the layout of this robot and the particular configuration you show in the picture, I would indeed expect $\theta_1 = \tan^{-1}(y/x)$.
Note that this equation will not always be true for ${}^0T_6$ but should always be true for ${}^0T_5$ (or ${}^0T_{wrist}$).
However, don't forget that there may be two solutions! They will be $\pi$ apart.

If, as per your question, it does not, I think there is probably something wrong with one of your transformations.

How wrong is the answer you get?

To debug this I would set the joint angles to known values (as I guess you've already done as per the picture), then use forward kinematics to calculate the position of every frame, ${}^BT_0$, ${}^0T_1$, ${}^1T_2$ etc. If you have a way to visualise them that's ideal. Now work along each frame from the base and check that each one is where you expect.

Answer 2

I think your transforms are assembled in reverse order maybe? I didn't really follow your explanation of what the frames are (and they aren't labeled on your drawing), so I'll say that you can compose a transform matrix the following way:

$$ T_0^6 = T_0^1 T_1^2T_2^3 ... T_5^6 \\ $$

That is, to get from Frame 0 to Frame 6, you multiply together the transforms from 0 to 1, then 1 to 2, then 2 to 3, and so on.

This seems to be kind of similar to what you wrote:

$$ T06=TB0^{−1}⋅target⋅TWT^{−1}⋅T6W^{−1} \\ $$

But I'm not sure where your missing frames are (there are 6, right?) or what $target$ is (a point or a frame or a transform?)

But if you had some point in the end-effector frame (frame 6), then you would represent that via the base-to-frame-6 transform:

$$ T_B^6 = T_B^1T_1^2...T_5^6 \\ $$

And then

$$ \overrightarrow{x}_B = T_B^6 \overrightarrow{x}_6 \\ $$

If you wanted to express a point given in base coordinates in the end effector's frame, just invert the transform:

$$ \overrightarrow{x}_6 = (T_B^6)^{-1} \overrightarrow{x}_B \\ $$

because: $$ \begin{array}{ccc} \overrightarrow{x}_B &=& T_B^6 \overrightarrow{x}_6 \\ (T_B^6)^{-1}\overrightarrow{x}_B &=& (T_B^6)^{-1}T_B^6 \overrightarrow{x}_6 \\ (T_B^6)^{-1}\overrightarrow{x}_B &=& I \overrightarrow{x}_6 \\ (T_B^6)^{-1}\overrightarrow{x}_B &=& \overrightarrow{x}_6 \end{array} $$

Answer 3

The equation

$T06=TB0^{-1}\cdot target \cdot TWT^{-1} \cdot T6W^{-1}$

seems to be the probem.

The first equality which can be written is:

$$ T_{robot} = T_{TCP} $$

Two transformation matrices which transform the same base frame to the same TCP Frame, but use different atomic transformations. $T_{robot}$ uses joint angles and the $T_{TCP}$ matrix uses the end effector coordinates as atomic transformations.

$$T_{robot} = T_{base} \cdot T_{0,6} \cdot T_{tool}$$ $$T_{TCP} = T_x \cdot T_y \cdot T_z \cdot R_{X} \cdot R_{Y} \cdot R_{Z}$$ $$ T_{0,6} = T_{0,1} \cdot T_{1,2} \cdot ...\cdot T_{5,6}\\ $$

If the inverse kinematics problem is addressed, only $T_{0,6}$ has unknowns, every other matrix is known and fully defined.

$$ T_{base} ^{-1} \cdot T_{robot} \cdot T_{tool}^{-1} = T_{0,6}$$

The first step for solving inverse kinematics for robots with a wrist is finding the angle of the first joint. This is done by expressing the wirst point coordinates on both sides of the equation

$$ T_{base} ^{-1} \cdot T_{robot} \cdot T_{tool}^{-1} = T_{0,1} \cdot T_{1,2} \cdot ...\cdot T_{5,6}$$

$$ T_{base} ^{-1} \cdot T_{robot} \cdot T_{tool}^{-1} \cdot T_{5,6}^{-1} \cdot \cdot T_{4,5}^{-1} = T_{0,1} \cdot T_{1,2} \cdot ...\cdot T_{3,4}$$

In the matrix on the left hand side the last columnt should only contain known paramters, this is the X, Y, Z coordinate of the wrist point. Using the $atan2()$ function, the $q_1$ joint angle of the first axis can be calculated.

Answer 4

Your atan probably returns radians. Were you expecting degrees?

Forgive me if this seems so basic as to be insulting, but just yesterday it snagged two engineers pair programming (one was me)!