test_Scenario.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 
6 See LICENSE for the license information
7 
8 Scenario unit tests.
9 Author: Frank Dellaert & Duy Nguyen Ta (Python)
10 """
11 from __future__ import print_function
12 
13 import math
14 import unittest
15 
16 import numpy as np
17 
18 import gtsam
19 from gtsam.utils.test_case import GtsamTestCase
20 
21 # pylint: disable=invalid-name, E1101
22 
23 
25  def setUp(self):
26  pass
27 
28  def test_loop(self):
29  # Forward velocity 2m/s
30  # Pitch up with angular velocity 6 degree/sec (negative in FLU)
31  v = 2
32  w = math.radians(6)
33  W = np.array([0, -w, 0])
34  V = np.array([v, 0, 0])
35  scenario = gtsam.ConstantTwistScenario(W, V)
36 
37  T = 30
38  np.testing.assert_almost_equal(W, scenario.omega_b(T))
39  np.testing.assert_almost_equal(V, scenario.velocity_b(T))
40  np.testing.assert_almost_equal(
41  np.cross(W, V), scenario.acceleration_b(T))
42 
43  # R = v/w, so test if loop crests at 2*R
44  R = v / w
45  T30 = scenario.pose(T)
46  xyz = T30.rotation().xyz()
47  if xyz[0] < 0:
48  xyz = -xyz
49  np.testing.assert_almost_equal(
50  np.array([math.pi, 0, math.pi]), xyz)
51  self.gtsamAssertEquals(gtsam.Point3(
52  0, 0, 2.0 * R), T30.translation(), 1e-9)
53 
54 
55 if __name__ == '__main__':
56  unittest.main()
Vector3 Point3
Definition: Point3.h:38


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:37:46