Python example: se2-poses-example.py

Example usage of SE(2) poses, for transformations in the plane (x,y,phi):

 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 math import radians
 9from mrpt.pymrpt.mrpt.poses import CPose2D
10
11p1 = CPose2D(1.0, 2.0, radians(90.0))
12p2 = CPose2D(3.0, 0.0, radians(0.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:

1p1             : (1.000,2.000,90.00deg)
2p2             : (3.000,0.000,0.00deg)
3p1(+)p2        : (1.000,5.000,90.00deg)
4(p1(+)p2)(-)p1 : (3.000,-0.000,0.00deg)