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"))