# How do I convert a set of coordinate and rotation values

I'm an electrical engineer who needs to use a robot for their first time. I have a UR5 that I'm controlling through the rtde interface via Python. It seems that what I'm trying to do must be something a million other people have done already but I've spent the morning reading and I'm very confused.

My robot has a "base" plane but it has a method to create a plane aligned with my workpiece that I've used to create a coordinate system from which it's easy to control the robot by writing values in the format [X, Y, Z, RX, RY, RZ]. The problem is that I can not read values from the robot in my custom plane, only in the "base" plane. It seems like it must be a common action to read the coordinates, rotate them by [RX, RY, RZ], then translate them by [X, Y, Z]. The help I've found on the web either does one or the other and often includes a lot of mathematical derivation. When I've tried to apply the solutions I don't get vectors of the same length after I've translated them so it feels like I must have done something wrong.

Is there any resource that can explain how to solve my problem simply? It feels like this must be a very common thing for people who use robots to do.

What you are looking for here is homogeneous transformation matrices. The purpose of these matrices is to seamlessly integrate rotation and translations into one matrix that acts as a change of coordinates. Consider some frame $$0$$ that has an origin (a zero) and three mutually perpendicular axes and some other frame $$n$$ with the same properties but not necessarily the same values. A homogeneous transformation matrix then is a matrix of the form:

$${}_{0}\mathbf{T}_{n} = \left[ {\begin{array}{cc} {}_{0}\mathbf{R}_{n} & {}_{0}\mathbf{x}_{n} \\ \mathbf{0} & 1 \\ \end{array} } \right] \in \mathbb{R}^{4 \times 4}$$

Where $${}_{0}\mathbf{R}_{n} \in \text{SO}(3)$$ is a $$3\times3$$ rotation matrix that rotates the coordinates of frame $$n$$ into frame $$0$$ and $${}_{0}\mathbf{x}_{n} \in \mathbb{R}^{3}$$ is the distance from the origin of frame $$0$$ to the origin of frame $$n$$. So, any vector $$\mathbf{v}_{n}$$ in frame $$n$$ can easily be expressed in frame $$0$$ as:

$$\mathbf{v}_{0} = {}_{0}\mathbf{T}_{n}\mathbf{v}_{n}$$

Of course, you might notice that $${}_{0}\mathbf{T}_{n}$$ has an incompatible size with respect to $$\mathbf{v} \in \mathbb{R}^{3}$$ - to solve this we simply append a $$1$$ to the end of $$\mathbf{v}$$, giving us $$\mathbf{v} = [v_1\ v_2\ v_3\ 1]^{T}$$.

Now that we have our formulation of frames in place, let's consider how we might transfer our control from frame $$0$$ (our base frame) to frame $$c$$, our "custom" frame. We know we can transform a vector from one frame to another via homogeneous transformation matrices, so, in a rush to the punchline - we'll say our final goal is some coordinate transformation $${}_{c}\mathbf{T}_{n}$$, where $$n$$ denotes the end-effector frame. Currently, we have the ability to "move" frame $$n$$ into frame $$0$$ - so we need to find a way to insert our $$c$$ frame into our model. Let's try establishing some connection between frames that don't change relative to any of our model parameters (i.e. robot joint values). The obvious choice here is frame $$0$$. Let's establish some coordinate transform $${}_{c}\mathbf{T}_{0}$$ - these should be the only measurements you will need to take.

Moving the frame $$0$$ origin to frame $$c$$ is as simple as measuring the distance from frame $$0$$ (your robot's base I assume) to the workpiece center (or whatever you are looking to define the workpiece origin by). The rotation part is a little more complicated. Perhaps first you could determine the $$\text{x}$$, $$\text{y}$$, and $$\text{z}$$ axes directions of your base frame. Then, determine what you would like your workpiece coordinate axes to look like (just make sure they are orthogonal to one another and satisfy the right-hand rule). From here, we want to make a rotation matrix $${}_{c}\mathbf{R}_{0}$$ to map frame $$0$$ orientations to frame $$c$$ orientations. An intuitive way to think about rotation matrices is:

$${}_{i}\mathbf{R}_{j} = [{}_{i}\mathbf{e}_{j}\ {}_{i}\mathbf{u}_{j}\ {}_{i}\mathbf{w}_{j}]$$

where $${}_{i}\mathbf{e}_{j} \in \mathbb{R}^{3}$$ is a column vector and represents the $$\text{x}$$-axis of frame $$j$$ represented in frame $$i$$ - and the same for $${}_{i}\mathbf{u}_{j}$$ and $${}_{i}\mathbf{w}_{j}$$ with the $$\text{y}$$ and $$\text{z}$$ axes respectively. At the end of the day, there's plenty of ways to form this matrix - so there will be many internet resources on how to do this if you find yourself lost.

So, we have $${}_{c}\mathbf{R}_{0}$$ and $${}_{c}\mathbf{x}_{0}$$ - let's say:

$${}_{c}\mathbf{T}_{0} = \left[ {\begin{array}{cc} {}_{c}\mathbf{R}_{0} & {}_{c}\mathbf{x}_{0} \\ \mathbf{0} & 1 \\ \end{array} } \right]$$

and now we can try moving frame $$n$$ to frame $$c$$ via $${}_{c}\mathbf{T}_{0} \cdot {}_{0}\mathbf{T}_{n}$$. This gives us:

$${}_{c}\mathbf{T}_{0} \cdot {}_{0}\mathbf{T}_{n} = \left[ {\begin{array}{cccc} {}_{c}\mathbf{R}_{0} \cdot {}_{0}\mathbf{e}_{n} & {}_{c}\mathbf{R}_{0} \cdot {}_{0}\mathbf{u}_{n} & {}_{c}\mathbf{R}_{0} \cdot {}_{0}\mathbf{w}_{n} & {}_{c}\mathbf{R}_{0} \cdot {}_{0}\mathbf{x}_{n} + {}_{c}\mathbf{x}_{0} \\ 0 & 0 & 0 & 1 \\ \end{array} } \right]$$

where

$${}_{c}\mathbf{R}_{n} = \left[ {\begin{array}{cccc} {}_{c}\mathbf{R}_{0} \cdot {}_{0}\mathbf{e}_{n} & {}_{c}\mathbf{R}_{0} \cdot {}_{0}\mathbf{u}_{n} & {}_{c}\mathbf{R}_{0} \cdot {}_{0}\mathbf{w}_{n} \\ \end{array} } \right]$$

and

$${}_{c}\mathbf{x}_{n} = {}_{c}\mathbf{R}_{0} \cdot {}_{0}\mathbf{x}_{n} + {}_{c}\mathbf{x}_{0}$$

Thus $${}_{c}\mathbf{T}_{n} = {}_{c}\mathbf{T}_{0} \cdot {}_{0}\mathbf{T}_{n}$$, and there we have the coordinate transformation we are looking for. Now to actually use this transformation, what I would recommend would be multiplying $${}_{c}\mathbf{T}_{n}$$ directly with the $$[x\ y\ z]$$ vector you mention (appended with a $$1$$ of course) to get the properly transformed end-effector location w.r.t. the "custom" frame. To get the orientation, I would recommend converting your $$[r_{x} \ r_{y} \ r_{y}]$$ vector to a rotation matrix - this will depend on what these values actually physically represent (here). Once in rotation matrix form, find the rotation in frame $$c$$ as:

$${}_{c}\mathbf{R}_{n} = {}_{c}\mathbf{R}_{0} \cdot \text{rot_mat}(\mathbf{r}_{x}, \mathbf{r}_{y}, \mathbf{r}_{z})$$

where $$\text{rot_mat}(\cdot)$$ represents a function to convert your orientation variables to a rotation matrix. After you do these conversions, you should have all you need to perform your tasks as you are wanting to.

• Thank you for this reply! I was worried people in this forum might be disgusted by my inexperienced question. I will try to implement this with an example and let you know if I succeed. Oct 24, 2022 at 17:33
• There's no shame in not knowing something, that's what SE is here for ;) hope this helps though! Oct 24, 2022 at 21:32
• @Bobby everyone has to start somewhere! Nobody was born knowing how to control robots <3
– Chuck
Oct 25, 2022 at 1:37
• There are libraries like the klampt library that already implement transformations. I suggest you use a library and stick the high level parts you care about. Oct 29, 2022 at 17:06
• @domo_arigato I apologize for not writing earlier (and I doubt you'd been left wondering) but I did successfully implement these transformation matrices and I can now both write and read coordinates in the same plane. Nov 5, 2022 at 17:31