Rotations & Transformations
The most common representations for 3D rotations are implemented in RobWork. In this page we will consider the Rotation Matrix, Axis-Angle (EAA), Roll Pitch Yaw Angle (RPY), and Quaternion representations, and how to convert between these representations. Finally, Transformations are considered.
Rotation Matrix
A 3x3 rotation matrix can be constructed using the Rotation3D type. The default Rotation3D<> type uses double precision, while Rotation3D<float> uses float precision. Notice that it is only possible to specify the templated types in C++. The equivalent types in the script interfaces (Python, Java and LUA) is Rotation3Dd and Rotation3Df for double and float precision respectively.
The constructor for Rotation3D takes the elements of the rotation matrix \(\mathbf{R} = \left[\begin{array}{ccc} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array} \right]\)
in the following order: Rotation3D(\(r_{11},r_{12},r_{13},r_{21},r_{22},r_{23},r_{31},r_{32},r_{33}\)).
Code examples are shown below for different languages.
Warning
There are two functions for finding the inverse rotation matrix, namely inverse(rot) and rot.inverse(). The inverse(rot) returns a new rotation without altering the original rotation, while rot.inverse() calculates the inverse by changing the original rotation matrix.
C++
1#include <rw/math/Rotation3D.hpp>
2
3using rw::math::Rotation3D;
4
5int main(int argc, char** argv) {
6 Rotation3D<> rotd = Rotation3D<>(1,0,0,0,0,-1,0,1,0);
7 Rotation3D<float> rotf = Rotation3D<float>(1,0,0,0,0,-1,0,1,0);
8
9 std::cout << "Rotation double:" << std::endl << rotd << std::endl;
10 std::cout << "Rotation float:" << std::endl << rotf << std::endl;
11 std::cout << "Rotation inverse:" << std::endl << inverse(rotd) << std::endl;
12 std::cout << "Identity:" << std::endl << rotd*inverse(rotd) << std::endl;
13
14 return 0;
15}
API Reference: rw::math::Rotation3D
See C++ Interface for more information about compilation and execution.
Python
1from sdurw import *
2
3if __name__ == '__main__':
4 rotd = Rotation3D(1,0,0,0,0,-1,0,1,0)
5 rotf = Rotation3Df(1,0,0,0,0,-1,0,1,0)
6
7 print("Rotation double:")
8 print(str(rotd))
9 print("Rotation float:")
10 print(str(rotf))
11 print("Rotation inverse:")
12 print(str(inverse(rotd)))
13 print("Identity:")
14 print(str(rotd*inverse(rotd)))
15
16 rotd[2,2] = 25
17 print("Corner = 25")
18 print(str(rotd))
19
20 if rotd[2,2] == 25:
21 exit(0)
22 else:
23 exit(1)
API Reference:
rw.Rotation3Dd
(double precision)rw.Rotation3Df
(float precision)
See Python interface for more information about execution.
Java
1import org.robwork.LoaderRW;
2import org.robwork.sdurw.*;
3import static org.robwork.sdurw.sdurw.*;
4
5public class ExRotation3D {
6 public static void main(String[] args) throws Exception {
7 LoaderRW.load("sdurw");
8
9 Rotation3Dd rotd = new Rotation3Dd(1,0,0,0,0,-1,0,1,0);
10 Rotation3Df rotf = new Rotation3Df(1,0,0,0,0,-1,0,1,0);
11
12 System.out.println("Rotation double:");
13 System.out.println(rotd);
14 System.out.println("Rotation float:");
15 System.out.println(rotf);
16 System.out.println("Rotation inverse:");
17 System.out.println(rotd.inverse());
18 System.out.println("Identity:");
19 System.out.println(rotd.multiply(inverse(rotd)));
20 }
21}
API Reference:
See Java Interface for more information about compilation and execution.
LUA
1require("sdurw_core")
2require("sdurw")
3using("sdurw")
4require("sdurw_math")
5using("sdurw_math")
6
7rotd = Rotation3D(1,0,0,0,0,-1,0,1,0);
8rotf = Rotation3Df(1,0,0,0,0,-1,0,1,0);
9
10print("Rotation double:");
11print(tostring(rotd));
12print("Rotation float:");
13print(tostring(rotf));
14print("Rotation inverse:");
15print(tostring(inverse(rotd)));
16print("Identity:");
17print(tostring(rotd*inverse(rotd)));
See Lua Interface for more information about execution of the script.
Output
Rotation double:
Rotation3D(1, 0, 0, 0, 0, -1, 0, 1, 0)
Rotation float:
Rotation3D(1, 0, 0, 0, 0, -1, 0, 1, 0)
Rotation inverse:
Rotation3D(1, 0, 0, 0, 0, 1, 0, -1, 0)
Identity:
Rotation3D(1, 0, 0, 0, 1, 0, 0, 0, 1)
Axis-Angle (EAA)
Equivalent Angle-Axis (EAA) is the RobWork type for rotations defined by the product of a unit vector and an angle of rotation. The direction of the vector gives the axis of rotation, and the length of the vector gives the amount of rotation in radians.
C++
1#include <rw/math/Constants.hpp>
2#include <rw/math/EAA.hpp>
3#include <rw/math/Rotation3D.hpp>
4
5using namespace rw::math;
6
7int main(int argc, char** argv) {
8 const EAA<> eaa = EAA<>(sqrt(2)/2*Pi,sqrt(2)/2*Pi,0);
9 std::cout << "EAA: " << eaa << std::endl;
10 std::cout << " angle: " << eaa.angle() << std::endl;
11 std::cout << " axis: " << eaa.axis() << std::endl;
12 const Rotation3D<> rotationFromEAA = eaa.toRotation3D();
13 std::cout << "Rotation from EAA: " << rotationFromEAA << std::endl;
14
15 const Rotation3D<> rot = Rotation3D<>(-1,0,0,0,0,1,0,1,0);
16 std::cout << "Rotation: " << rot << std::endl;
17 const EAA<> eaaFromRotation = EAA<>(rot);
18 std::cout << "EAA from Rotation: " << eaaFromRotation << std::endl;
19 std::cout << " angle: " << eaaFromRotation.angle() << std::endl;
20 std::cout << " axis: " << eaaFromRotation.axis() << std::endl;
21
22 return 0;
23}
API Reference: rw::math::EAA
See C++ Interface for more information about compilation and execution.
Python
1from sdurw_math import *
2import math
3
4if __name__ == '__main__':
5 eaa = EAA(math.sqrt(2)/2*math.pi,math.sqrt(2)/2*math.pi,0)
6 print("EAA: " + str(eaa))
7 print(" angle: " + str(eaa.angle()))
8 print(" axis: " + str(eaa.axis()))
9 rotationFromEAA = eaa.toRotation3D()
10 print("Rotation from EAA: " + str(rotationFromEAA))
11
12 rot = Rotation3D(-1,0,0,0,0,1,0,1,0)
13 print("Rotation: " + str(rot))
14 eaaFromRotation = EAA(rot)
15 print("EAA from Rotation: " + str(eaaFromRotation))
16 print(" angle: " + str(eaaFromRotation.angle()))
17 print(" axis: " + str(eaaFromRotation.axis()))
API Reference:
rw.EAAd
(double precision)rw.EAAf
(float precision)
See Python interface for more information about execution.
Java
1import org.robwork.LoaderRW;
2import org.robwork.sdurw_math.*;
3import static org.robwork.sdurw.sdurwConstants.*;
4import java.lang.Math;
5
6public class ExEAA {
7 public static void main(String[] args) throws Exception {
8 LoaderRW.load("sdurw_math");
9
10 EAAd eaa = new EAAd(Math.sqrt(2)/2*sdurw_mathConstants.Pi,Math.sqrt(2)/2*sdurw_mathConstants.Pi,0);
11 System.out.println("EAA: " + eaa);
12 System.out.println(" angle: " + eaa.angle());
13 System.out.println(" axis: " + eaa.axis());
14 Rotation3Dd rotationFromEAA = eaa.toRotation3D();
15 System.out.println("Rotation from RPY: " + rotationFromEAA);
16
17 Rotation3Dd rot = new Rotation3Dd(-1,0,0,0,0,1,0,1,0);
18 System.out.println("Rotation: " + rot);
19 EAAd eaaFromRotation = new EAAd(rot);
20 System.out.println("EAA from Rotation: " + eaaFromRotation);
21 System.out.println(" angle: " + eaaFromRotation.angle());
22 System.out.println(" axis: " + eaaFromRotation.axis());
23 }
24}
API Reference:
See Java Interface for more information about compilation and execution.
LUA
1require("sdurw_core")
2require("sdurw")
3using("sdurw")
4require("sdurw_math")
5using("sdurw_math")
6
7eaa = EAA(math.sqrt(2)/2*Pi,math.sqrt(2)/2*Pi,0);
8print("EAA: " .. tostring(eaa))
9print(" angle: " .. eaa:angle())
10print(" axis: " .. tostring(eaa:axis()));
11rotationFromEAA = eaa:toRotation3D();
12print("Rotation from EAA: " .. tostring(rotationFromEAA));
13
14rot = Rotation3D(-1,0,0,0,0,1,0,1,0);
15print("Rotation: " .. tostring(rot));
16eaaFromRotation = EAA(rot);
17print("EAA from Rotation: " .. tostring(eaaFromRotation))
18print(" angle: " .. eaaFromRotation:angle())
19print(" axis: " .. tostring(eaaFromRotation:axis()));
See Lua Interface for more information about execution of the script.
Output
EAA: EAA { 2.22144, 2.22144, 0}
angle: 3.14159
axis: Vector3D(0.707107, 0.707107, 0)
Rotation from EAA: Rotation3D(2.22045e-16, 1, 8.65956e-17, 1, 2.22045e-16, -8.65956e-17, -8.65956e-17, 8.65956e-17, -1)
Rotation: Rotation3D(-1, 0, 0, 0, 0, 1, 0, 1, 0)
EAA from Rotation: EAA { 0, 2.22144, 2.22144}
angle: 3.14159
axis: Vector3D(0, 0.707107, 0.707107)
Roll Pitch Yaw Angle (RPY)
Roll Pitch Yaw angles is one form of Euler angles where the rotation is defined by three consecutive rotations around the axes of a coordinate system. In RobWork, RPY is the rotation around the z, y and x axes (in that order). Notice that the rotation around the y axis is the y axis after doing the rotation around z. The rotation around the x axis is the x axis after doing the rotations around both z and y.
C++
1#include <rw/math/Constants.hpp>
2#include <rw/math/Rotation3D.hpp>
3#include <rw/math/RPY.hpp>
4
5using namespace rw::math;
6
7int main(int argc, char** argv) {
8 const RPY<> rpy = RPY<>(Pi,Pi/2,0);
9 std::cout << "RPY: " << rpy << std::endl;
10 const Rotation3D<> rotationFromRPY = rpy.toRotation3D();
11 std::cout << "Rotation from RPY: " << rotationFromRPY << std::endl;
12
13 const Rotation3D<> rot = Rotation3D<>(-1,0,0,0,0,1,0,1,0);
14 std::cout << "Rotation: " << rot << std::endl;
15 const RPY<> rpyFromRotation = RPY<>(rot);
16 std::cout << "RPY from Rotation: " << rpyFromRotation << std::endl;
17
18 return 0;
19}
API Reference: rw::math::RPY
See C++ Interface for more information about compilation and execution.
Python
1from sdurw import *
2
3if __name__ == '__main__':
4 rpy = RPY(Pi,Pi/2,0)
5 print("RPY: " + str(rpy))
6 rotationFromRPY = rpy.toRotation3D()
7 print("Rotation from RPY: " + str(rotationFromRPY))
8
9 rot = Rotation3D(-1,0,0,0,0,1,0,1,0)
10 print("Rotation: " + str(rot))
11 rpyFromRotation = RPY(rot)
12 print("RPY from Rotation: " + str(rpyFromRotation))
13 rot = Rotation3D(1,0,0,0,1,0,0,0,1)
API Reference:
rw.RPYd
(double precision)rw.RPYf
(float precision)
See Python interface for more information about execution.
Java
1import org.robwork.LoaderRW;
2import org.robwork.sdurw.*;
3import static org.robwork.sdurw.sdurwConstants.*;
4
5public class ExRPY {
6 public static void main(String[] args) throws Exception {
7 LoaderRW.load("sdurw");
8
9 RPYd rpy = new RPYd(Pi,Pi/2,0);
10 System.out.println("RPY: " + rpy);
11 Rotation3Dd rotationFromRPY = rpy.toRotation3D();
12 System.out.println("Rotation from RPY: " + rotationFromRPY);
13
14 Rotation3Dd rot = new Rotation3Dd(-1,0,0,0,0,1,0,1,0);
15 System.out.println("Rotation: " + rot);
16 RPYd rpyFromRotation = new RPYd(rot);
17 System.out.println("RPY from Rotation: " + rpyFromRotation);
18 }
19}
API Reference:
See Java Interface for more information about compilation and execution.
LUA
1require("sdurw_core")
2require("sdurw")
3using("sdurw")
4require("sdurw_math")
5using("sdurw_math")
6
7rpy = RPY(Pi,Pi/2,0);
8print("RPY: " .. tostring(rpy));
9rotationFromRPY = rpy:toRotation3D();
10print("Rotation from RPY: " .. tostring(rotationFromRPY));
11
12rot = Rotation3D(-1,0,0,0,0,1,0,1,0);
13print("Rotation: " .. tostring(rot));
14rpyFromRotation = RPY(rot);
15print("RPY from Rotation: " .. tostring(rpyFromRotation));
See Lua Interface for more information about execution of the script.
Output
RPY: RPY {3.14159, 1.5708, 0}
Rotation from RPY: Rotation3D(-6.12323e-17, -1.22465e-16, -1, 7.4988e-33, -1, 1.22465e-16, -1, 0, 6.12323e-17)
Rotation: Rotation3D(-1, 0, 0, 0, 0, 1, 0, 1, 0)
RPY from Rotation: RPY {3.14159, -0, 1.5708}
Quaternion
Quaternions are complex numbers, storing the rotation as 4 values.
C++
1#include <rw/math/Quaternion.hpp>
2#include <rw/math/Rotation3D.hpp>
3
4using namespace rw::math;
5
6int main (int argc, char** argv)
7{
8 const Quaternion<> quat = Quaternion<> (sqrt (2) / 2, sqrt (2) / 2, 0, 0);
9 std::cout << "Quaternion: " << quat << std::endl;
10 const Rotation3D<> rotationFromQuat = quat.toRotation3D ();
11 std::cout << "Rotation from Quaternion: " << rotationFromQuat << std::endl;
12
13 const Rotation3D<> rot = Rotation3D<> (-1, 0, 0, 0, 0, 1, 0, 1, 0);
14 std::cout << "Rotation: " << rot << std::endl;
15 const Quaternion<> quatFromRotation = Quaternion<> (rot);
16 std::cout << "Quaternion from Rotation: " << quatFromRotation << std::endl;
17
18 return 0;
19}
API Reference: rw::math::Quaternion
See C++ Interface for more information about compilation and execution.
Python
1from sdurw import *
2import math
3
4if __name__ == '__main__':
5 quat = Quaternion(math.sqrt(2)/2,math.sqrt(2)/2,0,0)
6 print("Quaternion: " + str(quat))
7 rotationFromQuat = quat.toRotation3D()
8 print("Rotation from Quaternion: " + str(rotationFromQuat));
9
10 rot = Rotation3D(-1,0,0,0,0,1,0,1,0)
11 print("Rotation: " + str(rot))
12 quatFromRotation = Quaternion(rot)
13 print("Quaternion from Rotation: " + str(quatFromRotation));
API Reference:
rw.Quaterniond
(double precision)rw.Quaternionf
(float precision)
See Python interface for more information about execution.
Java
1import org.robwork.LoaderRW;
2import org.robwork.sdurw.*;
3import java.lang.Math;
4
5public class ExQuaternion {
6 public static void main(String[] args) throws Exception {
7 LoaderRW.load("sdurw");
8
9 Quaterniond quat = new Quaterniond(Math.sqrt(2)/2,Math.sqrt(2)/2,0,0);
10 System.out.println("Quaternion: " + quat);
11 Rotation3Dd rotationFromQuat = quat.toRotation3D();
12 System.out.println("Rotation from Quaternion: " + rotationFromQuat);
13
14 Rotation3Dd rot = new Rotation3Dd(-1,0,0,0,0,1,0,1,0);
15 System.out.println("Rotation: " + rot);
16 Quaterniond quatFromRotation = new Quaterniond(rot);
17 System.out.println("Quaternion from Rotation: " + quatFromRotation);
18 }
19}
API Reference:
See Java Interface for more information about compilation and execution.
LUA
1require("sdurw_core")
2require("sdurw")
3using("sdurw_core")
4require("sdurw_math")
5using("sdurw_math")
6
7quat = Quaternion(math.sqrt(2)/2,math.sqrt(2)/2,0,0);
8print("Quaternion: " .. tostring(quat))
9rotationFromQuat = quat:toRotation3D();
10print("Rotation from Quaternion: " .. tostring(rotationFromQuat));
11
12rot = Rotation3D(-1,0,0,0,0,1,0,1,0);
13print("Rotation: " .. tostring(rot));
14quatFromRotation = Quaternion(rot);
15print("Quaternion from Rotation: " .. tostring(quatFromRotation))
See Lua Interface for more information about execution of the script.
Output
Quaternion: Quaternion {0.707107, 0.707107, 0, 0}
Rotation from Quaternion: Rotation3D(-2.22045e-16, 1, 0, 1, -2.22045e-16, 0, 0, 0, -1)
Rotation: Rotation3D(-1, 0, 0, 0, 0, 1, 0, 1, 0)
Quaternion from Rotation: Quaternion {0, 0.707107, 0.707107, 0}
Converting Rotations
To convert between EAA, RPY and Quaternions, it is in general possible to convert the types via the full Rotation3D representation. First convert to Rotation3D using the toRotation3D() function and then construct the new target type from this rotation matrix.
The RobWork Math class provides special functions for converting between EAA and Quaternion. Please see the quaternionToEAA and eaaToQuaternion functions.
API Reference:
C++ : rw::math::Math
Python:
rw.Math
Javadoc: org.robwork.rw.Math
Transformations
The Transform3D type is a full 4x4 homogeneous transformation matrix. A transformation matrix combines a rotation and a translation. It can be constructed from a Vector3D and any of the rotation types above.
Alternatively, the Pose6D stores a pose using six values. Three values for the position and three values for the EAA orientation.
API Reference:
C++ : rw::math::Transform3D
Python:
rw.Transform3Dd
Python:
rw.Transform3Df
Python:
rw.Pose6d
Python:
rw.Pose6f
Javadoc: org.robwork.rw.Transform3Dd
Javadoc: org.robwork.rw.Transform3Df