Python example: test-poses-se2.py
Self-checking test of SE(2) pose composition and TPose2D conversion.
Modules: mrpt.poses, mrpt.math
1#!/usr/bin/env python3
2"""
3Self-checking test of SE(2) pose composition and TPose2D conversion.
4"""
5
6from math import radians
7from mrpt.poses import CPose2D
8from mrpt.math import TPose2D
9import os
10
11print('[Running python test ' + os.path.basename(__file__) + ']')
12
13p1 = CPose2D(1.0, 2.0, radians(90.0))
14p2 = CPose2D(3.0, 0.0, radians(0.0))
15
16p3 = p1 + p2
17p4 = p3 - p1
18
19assert ((p4-p2).norm() < 1e-4)
20assert (abs(p4.phi-p2.phi) < 1e-4)
21
22p4t: TPose2D = p4.asTPose()
23assert (abs(p4t.x - p4.x) < 1e-8)
24assert (abs(p4t.y - p4.y) < 1e-8)
25assert (abs(p4t.phi - p4.phi) < 1e-8)
26
27print('[Successfully run ' + os.path.basename(__file__) + ']')