2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, 3 Atlanta, Georgia 30332-0415 6 See LICENSE for the license information 9 Author: Frank Dellaert & Duy Nguyen Ta (Python) 11 from __future__
import print_function
33 W = np.array([0, -w, 0])
34 V = np.array([v, 0, 0])
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))
45 T30 = scenario.pose(T)
46 xyz = T30.rotation().xyz()
49 np.testing.assert_almost_equal(
50 np.array([math.pi, 0, math.pi]), xyz)
52 0, 0, 2.0 * R), T30.translation(), 1e-9)
55 if __name__ ==
'__main__':