# Move relative to base coordinate system using 3D rotation vector

I am trying to move a UR robots tool center point(located at the end of the arm) relative to the base coordinate system (there is an internal function that does this but I am trying to pull it out due to latency issues). Points in the base coordinate system are described by [x, y, z, Rx, Ry, Rz] and the manual describes Rx, Ry, and Rz as "The rotation vector (Vector3d) in radians, also called the Axis-Angle vector (unit-axis of rotation multiplied by the rotation angle in radians)." I've looked online for how to translate points given the rotation vector, but most examples use a rotation matrix. If possible I would like to use the rotation vector to calculate the new points but if not, how do I convert between axis-angle and the rotation matrix? I've looked online and tried converting it based on examples but the numbers constantly come out wrong. I have added an example of a translation I programmed within the robot and the result that my function should give. I'm sorry for not knowing more details as I am not familiar with this kind of math. For reference I am writing the code in c# and if there are any libraries that do this for me, that would be even better! Thanks.

Image showing picture of robot for reference

To move the robot you feed it a point in space relative to the base. If the tool center point is rotated to account for a surface that isn't on the same plane as the base, I would like it to move relative to the rotation vector specified for that surface.

Example:

Reference point: [181.546, -439.057, 292.625, 0.793237, 2.904762, -0.216106]

Translate point: [0, 40, 0, 0, 0, 0]

Final point: [202.046, -404.968, 288.422, 0.793237, 2.904762, -0.216106]

Example code I tried for the rotation matrix:

double x = 181.546, y = -439.057, z = 292.625;
double Rx=0.793237, Ry = 2.904762, Rz = -0.216106;
double transX = 0, transY = 40, transZ = 0;

Matrix3D referenceMatrix = new Matrix3D(
Math.Cos(Ry) * Math.Cos(Rz), (-Math.Sin(Rz)*Math.Cos(Rx))+(Math.Sin(Rx)*Math.Cos(Rz)*Math.Sin(Ry)), (Math.Sin(Rz)*Math.Sin(Rx))+(Math.Cos(Rx)*Math.Cos(Rz)*Math.Sin(Ry)),
x, Math.Cos(Ry)*Math.Sin(Rz), (Math.Sin(Rx) * Math.Sin(Ry) * Math.Sin(Rz)) + (Math.Cos(Rx) * Math.Cos(Rz)), (-Math.Sin(Rx)*Math.Cos(Rz))+(Math.Cos(Rx)*Math.Sin(Ry)*Math.Sin(Rz)),
y, -Math.Sin(Ry), Math.Sin(Rx)*Math.Cos(Ry), Math.Cos(Rx)*Math.Cos(Ry),
z, 0, 0, 0, 1);

Matrix3D tranformMatrix = new Matrix3D(1, 0, 0, transX,
0, 1, 0, transY,
0, 0, 1, transZ,
0, 0, 0, 1);
Matrix3D finalMatrix = Matrix3D.Multiply(referenceMatrix, tranformMatrix);


Produced the points [194.095, -413.08, 264.92, ?, ?, ?] and I know the rotation vector would be the same since I only moved in the y-direction, but if I did adjust and angle, I am unsure how to find them.

• It would help a lot if you were to add a picture. – Adrian Keister Jun 6 at 21:33
• That picture's not really what I had in mind. Can you please post a diagram with the original point, a list of the transformations you want (translations, rotations, etc.), in the order that you want them done, as well as the final point? Also note that you can post pictures inline, instead of a link. That's almost always preferable. – Adrian Keister Jun 6 at 21:50

Premise on Rotation Matices

Let's first make clear some points about Rotation Matrices to try and avoid mistakes and misunderstandings very commonly happening in dealing with them.

Among all the possible definitions, let's base ourselves on this one. $${\bf R}_{\,{\bf x}} (\alpha ) = \left( {\matrix{ 1 & 0 & 0 \cr 0 & {\cos \alpha } & { - \sin \alpha } \cr 0 & {\sin \alpha } & {\cos \alpha } \cr } } \right)\quad {\bf R}_{\,{\bf y}} (\beta ) = \left( {\matrix{ {\cos \beta } & 0 & {\sin \beta } \cr 0 & 1 & 0 \cr { - \sin \beta } & 0 & {\cos \beta } \cr } } \right)\quad {\bf R}_{\,{\bf z}} (\gamma ) = \left( {\matrix{ {\cos \gamma } & { - \sin \gamma } & 0 \cr {\sin \gamma } & {\cos \gamma } & 0 \cr 0 & 0 & 1 \cr } } \right)$$ here:
- the angle sign is according to the "right hand" rule;
- when the matrix is (left) applied to a column vector, it restitutes a rotated column vector expressed in the original system;
- so that the successive application, e.g. $${\bf R}_{\,{\bf y}} (\beta ) \;{\bf R}_{\,{\bf z}} (\gamma )$$ means a rotation around the $$z$$ axis of $$\gamma$$, followed by a rotation around the original $$y$$ axis of $$\beta$$;
- a rotation around the $$z$$ axis of $$\gamma$$, followed by a rotation around the the new $$y$$ axis of $$\beta$$;
is instead given by the reverse order $${\bf R}_{\,{\bf z}} (\gamma )\;{\bf R}_{\,{\bf y'}} (\beta ) = \left( {{\bf R}_{\,{\bf z}} (\gamma )\;{\bf R}_{\,{\bf y'}} (\beta )\,{\bf R}_{\,{\bf z}} ^{\, - {\bf 1}} (\gamma )} \right){\bf R}_{\,{\bf z}} (\gamma )$$ (which is the case shown in the sketch).

Rotation around a given axis

That premised, let's pass to the expression of the rotation , by a given angle, right hand rule, around a given vector.
a) Take the cosine of the vector with the $$z$$ axis ($${\bf n}_z$$): its $$\arccos$$ will give $$0 \le \beta \le \pi$$:
b) Normalize the projection of the vector onto the $$x,y$$ plane: then you get $$\cos \gamma = {{n_{\,x} } \over {\sqrt {n_{\,x} ^{\,2} + n_{\,y} ^{\,2} } }}\;\quad \sin \gamma = {{n_{\,y} } \over {\sqrt {n_{\,x} ^{\,2} + n_{\,y} ^{\,2} } }}$$ to get $$- \pi < \gamma \le \pi$$ through the $$\arg$$ function ($$\arctan$$ over 4 quadrants).
c) Then the matrix shown above is the matrix that rotates axis $$z$$ to $$\bf n$$, and the other axes brought into the position shown in red.
d) Upon applying a rotation of the wanted angle (say $$\theta$$) around $${\bf n}=z'$$, we shall bring the reference axes back to the original position, because the rotation to reach to $$\bf n$$ is not part of the rotation we want to give.

In conclusion
\bbox[lightyellow] { \eqalign{ & {\bf R}_{\,{\bf n}} (\theta ) = \left( {{\bf R}_{\,{\bf z}} (\gamma )\;{\bf R}_{\,{\bf y}'} (\beta )} \right)\;{\bf R}_{\,{\bf z}'} (\theta )\; \left( {{\bf R}_{\,{\bf z}} (\gamma )\;{\bf R}_{\,{\bf y}'} (\beta )} \right)^{\, - {\bf 1}} = \cr & = {\bf R}_{\,{\bf z}} (\gamma )\;{\bf R}_{\,{\bf y}} (\beta )\;{\bf R}_{\,{\bf z}} (\theta )\;{\bf R}_{\,{\bf y}} ( - \beta )\;{\bf R}_{\,{\bf z}} ( - \gamma ) \cr} } is the matrix that applied to a column vector, will restitute that vector rotated by an angle $$\theta$$ around $$\bf n$$.

Note that finally we can omit the computation of the angles and write $${\bf R}_{\,{\bf z}} (\gamma )\;{\bf R}_{\,{\bf y}} (\beta ) = \left( {\matrix{ {{{n_{\,z} n_{\,x} } \over {\sqrt {n_{\,x} ^{\,2} + n_{\,y} ^{\,2} } }}} & { - {{n_{\,y} } \over {\sqrt {n_{\,x} ^{\,2} + n_{\,y} ^{\,2} } }}} & {n_{\,x} } \cr {{{n_{\,z} n_{\,y} } \over {\sqrt {n_{\,x} ^{\,2} + n_{\,y} ^{\,2} } }}} & {{{n_{\,x} } \over {\sqrt {n_{\,x} ^{\,2} + n_{\,y} ^{\,2} } }}} & {n_{\,y} } \cr { - \sqrt {n_{\,x} ^{\,2} + n_{\,y} ^{\,2} } } & 0 & {n_{\,z} } \cr } } \right)$$

Example:

$${\bf n} = {1 \over {\sqrt 3 }}\left( {\matrix{ 1 \cr 1 \cr 1 \cr } } \right) \quad \Rightarrow \quad \left\{ \matrix{ \beta = \arccos \left( {{{\sqrt 3 } \over 3}} \right) \approx 0.304\pi \hfill \cr \gamma = \arg \left( {1,1} \right) = {\pi \over 4} \hfill \cr} \right.$$ then \eqalign{ & {\bf R}_{\,{\bf z}} (\gamma )\;{\bf R}_{\,{\bf y}} (\beta ) = \left( {\matrix{ {\cos \gamma } & { - \sin \gamma } & 0 \cr {\sin \gamma } & {\cos \gamma } & 0 \cr 0 & 0 & 1 \cr } } \right)\left( {\matrix{ {\cos \beta } & 0 & {\sin \beta } \cr 0 & 1 & 0 \cr { - \sin \beta } & 0 & {\cos \beta } \cr } } \right) = \cr & = {{\sqrt 2 } \over 2}\left( {\matrix{ 1 & { - 1} & 0 \cr 1 & 1 & 0 \cr 0 & 0 & {\sqrt 2 } \cr } } \right)\left( {\matrix{ {{{\sqrt 3 } \over 3}} & 0 & {\sqrt {1 - 1/3} } \cr 0 & 1 & 0 \cr { - \sqrt {1 - 1/3} } & 0 & {{{\sqrt 3 } \over 3}} \cr } } \right) = \cr & = {{\sqrt 2 } \over 2}\left( {\matrix{ {{{\sqrt 3 } \over 3}} & { - 1} & {{{\sqrt 2 } \over {\sqrt 3 }}} \cr {{{\sqrt 3 } \over 3}} & 1 & {{{\sqrt 2 } \over {\sqrt 3 }}} \cr { - {2 \over {\sqrt 3 }}} & 0 & {{{\sqrt 2 } \over {\sqrt 3 }}} \cr } } \right) \cr} and you can see that the columns represent the red axes.
And for the inverse we can better take the transpose. $$\left( {{\bf R}_{\,{\bf z}} (\gamma )\;{\bf R}_{\,{\bf y}} (\beta )} \right)^{\, - {\bf 1}} = \left( {{\bf R}_{\,{\bf z}} (\gamma )\;{\bf R}_{\,{\bf y}} (\beta )} \right)^{\,T}$$

Thus our matrix is (eliding the factor $$\sqrt{2} /2$$). $${\bf R}_{\,{\bf n}} (\theta ) = \left( {\matrix{ {{{\sqrt 3 } \over 3}} & { - 1} & {{{\sqrt 2 } \over {\sqrt 3 }}} \cr {{{\sqrt 3 } \over 3}} & 1 & {{{\sqrt 2 } \over {\sqrt 3 }}} \cr { - {2 \over {\sqrt 3 }}} & 0 & {{{\sqrt 2 } \over {\sqrt 3 }}} \cr } } \right)\left( {\matrix{ {\cos \theta } & { - \sin \theta } & 0 \cr {\sin \theta } & {\cos \theta } & 0 \cr 0 & 0 & 1 \cr } } \right)\left( {\matrix{ {{{\sqrt 3 } \over 3}} & {{{\sqrt 3 } \over 3}} & { - {2 \over {\sqrt 3 }}} \cr { - 1} & 1 & 0 \cr {{{\sqrt 2 } \over {\sqrt 3 }}} & {{{\sqrt 2 } \over {\sqrt 3 }}} & {{{\sqrt 2 } \over {\sqrt 3 }}} \cr } } \right)$$

To verify that, lets put $${\bf x} = (x,y,z)^T \quad {\bf v} = {\bf R}_{\,{\bf n}} (\theta )\,{\bf x}$$ and we can check that $$\bbox[lightyellow] { \left\{ \matrix{ \left| {\bf x} \right| = \left| {\bf v} \right| \hfill \cr {\bf n} \cdot {\bf x} = {\bf n} \cdot {\bf v}\;\;({\rm indep}{\rm .}\,{\rm of}\,\theta ) \hfill \cr {{\left( {{\bf n} \times {\bf x}} \right) \cdot \left( {{\bf n} \times {\bf v}} \right)} \over {\left| {{\bf n} \times {\bf x}} \right|\;\left| {{\bf n} \times {\bf v}} \right|}} = \cos \theta \hfill \cr} \right. }$$

• So if I tried what you suggested above by rotating one matrix and then transforming using the rotation matrix, and that is working, but if I rotate more that one axis the numbers come out incorrect. I may have misunderstood you but to try it I calculated each of the 3 rotation matrices and multiplied them together, and then applied my transform but the numbers are incorrect. – vazqu133 Jun 7 at 15:51
• @vazqu133: I do not get how you applied what I told in the answer, but I know that the subject is intricated and very prone to lead to errors. To help you better I have included an example. Let me suggest that you follow carefully each step, making examples by yourself, to properly grasp the principles of rotation matrices. Let me know if you need other clarifications. – G Cab Jun 7 at 23:18