def test_request_added_transform(): """Request an added transform from the transform manager.""" random_state = np.random.RandomState(0) A2B = random_transform(random_state) tm = TransformManager() tm.add_transform("A", "B", A2B) A2B_2 = tm.get_transform("A", "B") assert_array_almost_equal(A2B, A2B_2)
def test_transform_not_added(): """Test request for transforms that have not been added.""" random_state = np.random.RandomState(0) A2B = random_transform(random_state) C2D = random_transform(random_state) tm = TransformManager() tm.add_transform("A", "B", A2B) tm.add_transform("C", "D", C2D) assert_raises_regexp(KeyError, "Unknown frame", tm.get_transform, "A", "G") assert_raises_regexp(KeyError, "Unknown frame", tm.get_transform, "G", "D") assert_raises_regexp(KeyError, "Cannot compute path", tm.get_transform, "A", "D")
def test_update_transform(): """Update an existing transform.""" random_state = np.random.RandomState(0) A2B1 = random_transform(random_state) A2B2 = random_transform(random_state) tm = TransformManager() tm.add_transform("A", "B", A2B1) tm.add_transform("A", "B", A2B2) A2B = tm.get_transform("A", "B") # Hack: test depends on internal member assert_array_almost_equal(A2B, A2B2) assert_equal(len(tm.i), 1) assert_equal(len(tm.j), 1)
def test_pickle(): """Test if a transform manager can be pickled.""" random_state = np.random.RandomState(1) A2B = random_transform(random_state) tm = TransformManager() tm.add_transform("A", "B", A2B) _, filename = tempfile.mkstemp(".pickle") try: pickle.dump(tm, open(filename, "wb")) tm2 = pickle.load(open(filename, "rb")) finally: if os.path.exists(filename): try: os.remove(filename) except WindowsError: pass # workaround for permission problem on Windows A2B2 = tm2.get_transform("A", "B") assert_array_almost_equal(A2B, A2B2)
def test_whitelist(): """Test correct handling of whitelists for plotting.""" random_state = np.random.RandomState(2) A2B = random_transform(random_state) tm = TransformManager() tm.add_transform("A", "B", A2B) nodes = tm._whitelisted_nodes(None) assert_equal(set(["A", "B"]), nodes) nodes = tm._whitelisted_nodes("A") assert_equal(set(["A"]), nodes) assert_raises_regexp(KeyError, "unknown nodes", tm._whitelisted_nodes, "C")
automatically. """ print(__doc__) import numpy as np import matplotlib.pyplot as plt from pytransform.rotations import (random_quaternion, matrix_from_euler_xyz, q_id) from pytransform.transformations import transform_from_pq, transform_from from pytransform.transform_manager import TransformManager random_state = np.random.RandomState(0) ee2robot = transform_from_pq( np.hstack((np.array([0.4, -0.3, 0.5]), random_quaternion(random_state)))) cam2robot = transform_from_pq(np.hstack((np.array([0.0, 0.0, 0.8]), q_id))) object2cam = transform_from(matrix_from_euler_xyz(np.array([0.0, 0.0, 0.5])), np.array([0.5, 0.1, 0.1])) tm = TransformManager() tm.add_transform("end-effector", "robot", ee2robot) tm.add_transform("camera", "robot", cam2robot) tm.add_transform("object", "camera", object2cam) ee2object = tm.get_transform("end-effector", "object") ax = tm.plot_frames_in("robot", s=0.1) ax.set_xlim((-0.25, 0.75)) ax.set_ylim((-0.5, 0.5)) ax.set_zlim((0.0, 1.0)) plt.show()
def test_request_concatenated_transform(): """Request a concatenated transform from the transform manager.""" random_state = np.random.RandomState(0) A2B = random_transform(random_state) B2C = random_transform(random_state) F2A = random_transform(random_state) tm = TransformManager() tm.add_transform("A", "B", A2B) tm.add_transform("B", "C", B2C) tm.add_transform("D", "E", np.eye(4)) tm.add_transform("F", "A", F2A) A2C = tm.get_transform("A", "C") assert_array_almost_equal(A2C, concat(A2B, B2C)) C2A = tm.get_transform("C", "A") assert_array_almost_equal(C2A, concat(invert_transform(B2C), invert_transform(A2B))) F2B = tm.get_transform("F", "B") assert_array_almost_equal(F2B, concat(F2A, A2B))
def test_check_consistency(): """Test correct detection of inconsistent graphs.""" random_state = np.random.RandomState(2) tm = TransformManager() A2B = random_transform(random_state) tm.add_transform("A", "B", A2B) B2A = random_transform(random_state) tm.add_transform("B", "A", B2A) assert_false(tm.check_consistency()) tm = TransformManager() A2B = random_transform(random_state) tm.add_transform("A", "B", A2B) assert_true(tm.check_consistency()) C2D = random_transform(random_state) tm.add_transform("C", "D", C2D) assert_true(tm.check_consistency()) B2C = random_transform(random_state) tm.add_transform("B", "C", B2C) assert_true(tm.check_consistency()) A2D_over_path = tm.get_transform("A", "D") A2D = random_transform(random_state) tm.add_transform("A", "D", A2D) assert_false(tm.check_consistency()) tm.add_transform("A", "D", A2D_over_path) assert_true(tm.check_consistency())
""" print(__doc__) import numpy as np import matplotlib.pyplot as plt from pytransform.plot_utils import make_3d_axis from pytransform.transformations import random_transform from pytransform.transform_manager import TransformManager random_state = np.random.RandomState(0) A2world = random_transform(random_state) B2world = random_transform(random_state) A2C = random_transform(random_state) D2B = random_transform(random_state) tm = TransformManager() tm.add_transform("A", "world", A2world) tm.add_transform("B", "world", B2world) tm.add_transform("A", "C", A2C) tm.add_transform("D", "B", D2B) plt.figure(figsize=(10, 5)) ax = make_3d_axis(3, 121) ax = tm.plot_frames_in("world", ax=ax, alpha=0.6) ax.view_init(30, 20) ax = make_3d_axis(3, 122) ax = tm.plot_frames_in("A", ax=ax, alpha=0.6) ax.view_init(30, 20)
""" ===================== Transformation Editor ===================== The transformation editor can be used to manipulate transformations. """ from pytransform.transform_manager import TransformManager from pytransform.editor import TransformEditor from pytransform.transformations import transform_from from pytransform.rotations import matrix_from_euler_xyz tm = TransformManager() tm.add_transform( "tree", "world", transform_from(matrix_from_euler_xyz([0, 0.5, 0]), [0, 0, 0.5])) tm.add_transform( "car", "world", transform_from(matrix_from_euler_xyz([0.5, 0, 0]), [0.5, 0, 0])) te = TransformEditor(tm, "world", s=0.3) te.show() print("tree to world:") print(te.transform_manager.get_transform("tree", "world"))