Forward kinematics equations

Question

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}$

Answer 1

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.

Answer 2

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.

Answer 3

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.