Python example: se3-poses-example.py

SE(3) pose composition and inverse composition with mrpt.poses.CPose3D.

Modules: mrpt.poses

 1#!/usr/bin/env python3
 2"""
 3SE(3) pose composition and inverse composition with mrpt.poses.CPose3D.
 4"""
 5
 6from mrpt.poses import CPose3D
 7from math import radians
 8
 9p1 = CPose3D.FromXYZYawPitchRoll(
10    1.0, 2.0, 0, radians(90), radians(0), radians(0))
11p2 = CPose3D.FromXYZYawPitchRoll(
12    3.0, 0.0, 0, radians(0.0), radians(0), radians(0))
13
14p3 = p1 + p2
15p4 = p3 - p1
16
17print('p1             : ' + str(p1))
18print('p2             : ' + str(p2))
19print('p1(+)p2        : ' + str(p3))
20print('(p1(+)p2)(-)p1 : ' + str(p4))

Output:

p1             : [1.000000 2.000000 0.000000 90.000000 0.000000 0.000000]
p2             : [3.000000 0.000000 0.000000 0.000000 0.000000 0.000000]
p1(+)p2        : [1.000000 5.000000 0.000000 90.000000 -0.000000 0.000000]
(p1(+)p2)(-)p1 : [3.000000 -0.000000 0.000000 0.000000 -0.000000 0.000000]