testScenarioRunner.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 ScenarioRunner unit tests.
9 Author: Frank Dellaert & Duy Nguyen Ta (Python)
10 """
11 import math
12 import unittest
13 
14 import numpy as np
15 
16 import gtsam
17 from gtsam.utils.test_case import GtsamTestCase
18 
19 
21  def setUp(self):
22  self.g = 10 # simple gravity constant
23 
24  def test_loop_runner(self):
25  # Forward velocity 2m/s
26  # Pitch up with angular velocity 6 degree/sec (negative in FLU)
27  v = 2
28  w = math.radians(6)
29  W = np.array([0, -w, 0])
30  V = np.array([v, 0, 0])
31  scenario = gtsam.ConstantTwistScenario(W, V)
32 
33  dt = 0.1
36  runner = gtsam.ScenarioRunner(
37  scenario, params, dt, bias)
38 
39  # Test specific force at time 0: a is pointing up
40  t = 0.0
41  a = w * v
42  np.testing.assert_almost_equal(
43  np.array([0, 0, a + self.g]), runner.actualSpecificForce(t))
44 
45 
46 if __name__ == '__main__':
47  unittest.main()
static std::shared_ptr< PreintegrationParams > MakeSharedU(double g=9.81)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:18