Python example: se3-poses-example.py

Example usage of SE(3) poses, for transformations in the space (x,y,z,yaw,pitch,roll):

 1#!/usr/bin/env python3
 2
 3# ---------------------------------------------------------------------
 4# Install python3-pymrpt, ros-$ROS_DISTRO-mrpt2, or test with a local build with:
 5# export PYTHONPATH=$HOME/code/mrpt/build-Release/:$PYTHONPATH
 6# ---------------------------------------------------------------------
 7
 8from mrpt.pymrpt import mrpt
 9from math import radians
10
11p1 = mrpt.poses.CPose3D.FromXYZYawPitchRoll(
12    1.0, 2.0, 0, radians(90), radians(0), radians(0))
13p2 = mrpt.poses.CPose3D.FromXYZYawPitchRoll(
14    3.0, 0.0, 0, radians(0.0), radians(0), radians(0))
15
16p3 = p1 + p2
17p4 = p3 - p1
18
19print('p1             : ' + str(p1))
20print('p2             : ' + str(p2))
21print('p1(+)p2        : ' + str(p3))
22print('(p1(+)p2)(-)p1 : ' + str(p4))

Output:

1p1             : (x,y,z,yaw,pitch,roll)=(1.0000,2.0000,0.0000,90.00deg,0.00deg,0.00deg)
2p2             : (x,y,z,yaw,pitch,roll)=(3.0000,0.0000,0.0000,0.00deg,0.00deg,0.00deg)
3p1(+)p2        : (x,y,z,yaw,pitch,roll)=(1.0000,5.0000,0.0000,90.00deg,-0.00deg,0.00deg)
4(p1(+)p2)(-)p1 : (x,y,z,yaw,pitch,roll)=(3.0000,-0.0000,0.0000,0.00deg,-0.00deg,0.00deg)