# Forward kinematics equations

I'm stuck at computing forward kinematics equations.

I have configuration of the first two joints like on the following image:

Transformation from the origin to the first joint basis is trivial: just translation by $\vec{OO_{1}}$.

The second transform from joint 1 to joint 2 basis makes me nervous throughout this day. First of all it is a rotation around $Z$ axis. So rotational part will look like this:

$R_{12}= \begin{pmatrix} cos(q_{1}) & -sin(q_{1}) & 0\\ sin(q_{1}) & cos(q_{1}) & 0\\ 0 & 0 & 1 \end{pmatrix}$

Problems are all about the translation part. I see two approaches here. Since angle between $\vec{O_{1}O_{2}}$ and plane $X_{1}O_{1}Y_{1}$ is constant because rotation is performed around $Z$ axis, length of projection of $\vec{O_{1}O_{2}}$ onto $X_{1}O_{1}Y_{1}$ is constant. Here it is:

$\vec{v} = O_{2} - O_{1} = \begin{pmatrix} v_{x}\\ v_{y}\\ v_{z} \end{pmatrix}$ Its' projection onto $X_{1}O_{1}Y_{1}$ is $\vec{v_{p}} = \begin{pmatrix} v_{x}\\ v_{y}\\ 0 \end{pmatrix}$ and it's magnitude is $m=\sqrt{v_{x}^2 + v_{y}^2}=const$.

Now let's look at what happens after rotation:

So the translation matrix looks like: $S_{12}= \begin{pmatrix} m\cdot cos(\alpha+q_{1})\\ m\cdot sin(\alpha+q_{1})\\ v_{z} \end{pmatrix}$

And full transformation matrix from joint 1 to joint 2 basis is:

$T_{12}= \begin{pmatrix} R_{12} & S_{12}\\ 0 & 1 \end{pmatrix}$

Unfortunately it gives me wrong results even when $q_{1}=0$. Can not see where my reasoning is wrong.

Second approach is more straightforward. Being able to calculate $\vec{O_{1}O_{2}}$ in initial configuration makes it possible just to rotate this vector by $q_{1}$ around $Z$ axis and this has to be our translation vector. Nevertheless I can't make it work.

$R_{z}= \begin{pmatrix} cos(q_{1}) & -sin(q_{1}) & 0\\ sin(q_{1}) & cos(q_{1}) & 0\\ 0 & 0 & 1 \end{pmatrix}\\ \vec{v} = O_{2} - O_{1} = \begin{pmatrix} v_{x}\\ v_{y}\\ v_{z} \end{pmatrix}\\ R_{z}\vec{v}= \begin{pmatrix} v_{x}cos(q_{1})-v_{y}sin(q_{1})\\ v_{x}sin(q_{1})+v_{y}cos(q_{1})\\ v_{z} \end{pmatrix}\\ T_{12}= \begin{pmatrix} R_{12} & R_{z}\vec{v}\\ 0 & 1 \end{pmatrix}$

It works until I rotate the first joint(i.e. only when $q_{1}=0$).

Under works I mean "calculates position of joint 2 origin right". This is done by multiplying transformation matrix $T_{02} = T_{01}T_{12}$ by $\begin{pmatrix} 0 & 0 & 0 & 1\end{pmatrix}^{T}$

• Unfortunately, from the description it is not clear what rotation should the structure execute. If the Z axis in the sketsc is pointing upwards, then it is unclear why is it summed up with alpha (assuming alpha is the tilt angle - y axis rotation - of the upper part). Furthermore, it is uncleat why do you summ up alpha with q1 even if they are rotations in the same direction. The q rotation is part of the rotation matrix, it should not be also part of the translation matrix. – 50k4 Apr 7 '17 at 13:22
• Please add axis labels to all coordinate systems. Furthermore, sketching the rotational joint would really help understand the orientation of the rotations. Also it is unclear what is the significance of "does not move" in the image. – 50k4 Apr 7 '17 at 13:24
• @50k4 Yes, I agree sketch is poor I'm going to draw new one. However why do you claim that The q rotation is part of the rotation matrix, it should not be also part of the translation matrix? I saw plenty of examples when q is used in rotation matrix and then in translation to find a projections onto axes. – Long Smith Apr 7 '17 at 13:35
• @50k4 Take a look, I have loaded new image. You can see on this image joint 1 origin and joint 2 origin. To find a translation I calculate vector O1O2 when q1 = 0 and after q1 is changed I rotate this vector by q1 and get x and y translations, z translation is const. If this is not the way to do it, it would be glad if you explain why. – Long Smith Apr 7 '17 at 13:57

You are complicating the translation part of the transformation matrix way to much.

The rotational part:

$R_{12}= \begin{pmatrix} cos(q_{1}) & -sin(q_{1}) & 0\\ sin(q_{1}) & cos(q_{1}) & 0\\ 0 & 0 & 1 \end{pmatrix}$

takes care of the q1 rotation. There is no further need to include q1 in the translation part. In the rotated coordinate system you make a translation on the x and y axis which is constant. You can express it in function of alpha, but there is no need to, since it is constant, you can use:

$Tr_{12} = \begin{pmatrix} v_{x}\\ 0 \\ v_{z} \end{pmatrix}$ as the translation part.

So the transformation Matrix will look like:

$T_{12}= \begin{pmatrix} R_{12} & Tr_{12}\\ 0 & 1 \end{pmatrix}$

Also please note that the coordinate system definitions you posted are not conform the Denavit-Hartenberg convention.

• I agree with this answer. The rotation/translation and transform matrices exist so you can neatly side-step any of the intermediate projections. Translate $ds$, rotate $q$, translate $ds$, rotate $q$, etc. until you're done with all of your links and joints. Don't bother with anything else. – Chuck Apr 7 '17 at 14:15
• I fixed update. Now I get the same result for q = 0 and q = pi/2. I feel like the problem is in the vector (0, 0, 0 ,1). – Long Smith Apr 7 '17 at 15:04
• Maybe this is a problem because I can not actually rotate vector with zero coordinates? Then should I calculate J2 position with T01 and O1O2? – Long Smith Apr 7 '17 at 15:07
• Think I can not do it because rotation won't be incorporated in this transformation. How then can I calculate J2 position? – Long Smith Apr 7 '17 at 15:15
• For the coordinate system, I know that. However my modelling environment has exactly such a coordinate system. I don't know why but it does. – Long Smith Apr 7 '17 at 15:21

In general, when you apply "rotation around a point," you construct that matrix by doing:

1. translate the center of rotation to world origin
2. apply rotation around world origin
3. translate the world origin back to the center of rotation

Also note that there are two matrix conventions, and nobody agrees which one is "standard": row vectors on the left, with translation in the bottom row of the matrix, or column vectors on the right, with translation in the right column of the matrix. As long as you know which one you're using, and rigorously stick to it, and know to translate if you import data/formulas from another source, you'll do fine. (The translation of a matrix between conventions is a simple transpose.)

Separately, there's the "left handed" versus "right handed" world coordinate interpretation conventions that may also vary, but luckily, the math is the same for both, so it doesn't change the matrices. A 90 degree rotation of unit X around the Z axis yields the unit Y vector, in either convention; the different only happens on translation to/from world coordinate space.

Firstly, I have to say that all the problems was because of my inattentiveness. Nothing was wrong with rotating $O_{1}O_{2}$ by $q_{1}$. The problem was about rotating first joint about global coordinates $Z$ axis instead of its' axis. Therefore I got an error between calculation and position in my modelling environment.

Several times I was told that I have not to include $q$ in the translation part. Not only here but also in some other sources. However it is not true and I wonder why so many people have told me about that. Let us look at what actually happens here. For simplicity we will look at the planar case.

Assume initial configuration like pictured below:

Let's describe two transformation matrices: one which includes $q_{1}$ in translation part and one that does not.

$T_{01}=\begin{pmatrix} cos(q_{1}) & -sin(q_{1}) & a_{1}\\ sin(q_{1}) & cos(q_{1}) & 0\\ 0 & 0 & 1 \end{pmatrix}\\ T_{01}'=\begin{pmatrix} cos(q_{1}) & -sin(q_{1}) & a_{1}cos(q_{1})\\ sin(q_{1}) & cos(q_{1}) & a_{1}sin(q_{1})\\ 0 & 0 & 1 \end{pmatrix}\\$

Vector $\vec{a_{2}}$ in $O_{1}$ frame is $\vec{a_{2}} = \begin{pmatrix} x_{0}'\\y_{0}'\\1\end{pmatrix}$

Now let's rotate our $\vec{a_{1}}$ by some angle $q_{1}$

What coordinates in origin frame vector $\vec{a_{2}}$ has? Geometrically easy to find that $\vec{a_{2}^{0}} = \begin{pmatrix} cos(q_{1})x_{0}'-sin(q_{1})y_{0}'+a_{1}cos(q_{1})\\ sin(q_{1})x_{0}'+cos(q_{1})y_{0}'+a_{1}sin(q_{1})\\ 1 \end{pmatrix}$

Now let's multiply our transformation matrices by $a_{2}$ and we should get the same result.

$T_{01}\vec{a_{2}}=\begin{pmatrix} cos(q_{1})x_{0}'-sin(q_{1})y_{0}'+a_{1}\\ sin(q_{1})x_{0}'+cos(q_{1})y_{0}'\\ 1 \end{pmatrix}\\\\ T_{01}'\vec{a_{2}}=\begin{pmatrix} cos(q_{1})x_{0}'-sin(q_{1})y_{0}'+a_{1}cos(q_{1})\\ sin(q_{1})x_{0}'+cos(q_{1})y_{0}'+a_{1}sin(q_{1})\\ 1 \end{pmatrix}$

And you see the difference: first transformation does not consider $O_{1}$ coordinate changes introduced by $q_{1}$ and it shifts our vector by the whole $a_{1}$ along $X$ and by 0 along $Y$. Therefore IF next joint coordinates are changed by current joint angle, you have to find projections in your translation part or if you know initial vector between their origins, you can rotate it by $q_{1}$. However the latter approach is more computationally expensive. Otherwise if coordinates are not going to be changed, you can throw out unnecessary calculations.