text: import math
  import unittest
  import numpy as np
  
  from gtsam_py import gtsam
  from util import Point3
  
  
  class TestScenario(unittest.TestCase):
      def setUp(self):
          pass
  
      def test_loop(self):
          # Forward velocity 2m/s
          # Pitch up with angular velocity 6 degree/sec (negative in FLU)
          v = 2
          w = math.radians(6)
          W = np.array([0, -w, 0])
          V = np.array([v, 0, 0])
          scenario = gtsam.ConstantTwistScenario(W, V)
  
          T = 30
          np.testing.assert_almost_equal(W, scenario.omega_b(T))
          np.testing.assert_almost_equal(V, scenario.velocity_b(T))
          np.testing.assert_almost_equal(
              np.cross(W, V), scenario.acceleration_b(T))
  
          # R = v/w, so test if loop crests at 2*R
          R = v / w
          T30 = scenario.pose(T)
          np.testing.assert_almost_equal(
              np.array([math.pi, 0, math.pi]),
              T30.rotation().xyz())
          np.testing.assert_almost_equal(
              Point3(0, 0, 2 * R), T30.translation(), 1e-9)
  
  
  if __name__ == '__main__':
      unittest.main()
  