def test_keep_joints():
points = np.arange(200 * 3).reshape(200, 3)
joint1 = tf.RotationalJoint((0, 1, 0))
joint2 = tf.PrismaticJoint((1, 0, 0))
links: List[tf.Link] = [
tf.Rotation((1, 0, 0), (0, 1, 0)),
tf.Translation((1, 0, 0)),
joint1,
tf.Translation((1, 1, 0)),
tf.Translation((1, 1, 1)),
tf.InvertLink(joint2),
]
simplified_links = tf.simplify_links(links, keep_joints=True)
expected = points
for link in links:
expected = link.transform(expected)
result = points
for link in simplified_links:
result = link.transform(result)
assert np.allclose(result, expected)
assert joint1 in simplified_links
assert joint2 is simplified_links[-1]._forward_link
def test_translation_dims():
# this is a regression test (no GH issue filed)
direction = np.array([[1, 2, 3]], dtype=np.float_)
link = tf.Translation(direction)
assert link.parent_dim == 3
assert link.child_dim == 3
def test_inverse_links():
points = np.arange(200 * 3).reshape(200, 3)
# --- test double_inverse ---
link = tf.InvertLink(tf.InvertLink(tf.Translation((1, 0, 0))))
simplified = tf.simplify_links([link])[0]
assert isinstance(simplified, tf.Translation)
expected = link.transform(points)
result = simplified.transform(points)
assert np.allclose(result, expected)
# --- test translation unwrapping ---
link = tf.InvertLink(tf.Translation((1, 0, 0)))
simplified = tf.simplify_links([link])[0]
assert isinstance(simplified, tf.Translation)
expected = link.transform(points)
result = simplified.transform(points)
assert np.allclose(result, expected)
# --- test rotation unwrapping ---
link = tf.InvertLink(tf.EulerRotation("X", np.pi / 2))
simplified = tf.simplify_links([link])[0]
assert isinstance(simplified, tf.Rotation)
expected = link.transform(points)
result = simplified.transform(points)
assert np.allclose(result, expected)
# --- test compound unpacking ---
link = tf.InvertLink(
tf.CompundLink([tf.Translation((1, 0, 0)),
tf.Translation((0, 1, 0))]))
simplified_links = tf.simplify_links([link])
expected = link.transform(points)
result = points
for link in simplified_links:
result = link.transform(result)
assert np.allclose(result, expected)
for link in simplified_links:
assert isinstance(link, tf.Translation)
def test_compound_unwrapping():
points = np.arange(200 * 3).reshape(200, 3)
link = tf.CompundLink([
tf.Translation((1, 0, 0)),
tf.CompundLink([
tf.Translation((0, 1, 0)),
]),
tf.EulerRotation("Y", -90, degrees=True),
])
simplified_links = tf.simplify_links([link])
expected = link.transform(points)
result = points
for link in simplified_links:
assert not isinstance(link, tf.CompundLink)
result = link.transform(result)
assert np.allclose(result, expected)
def test_joints_between():
joint1 = tf.RotationalJoint((0, 1, 0))
joint2 = tf.PrismaticJoint((1, 0, 0))
start = tf.Frame(3)
x = tf.Rotation((1, 0, 0), (0, 1, 0))(start)
x = tf.Translation((1, 0, 0))(x)
x = joint1(x)
x = tf.CompundLink(
[joint2,
tf.EulerRotation("xy", (90, -90), degrees=True), joint1])(x)
x = tf.Translation((1, 1, 0))(x)
x = tf.Translation((1, 1, 1))(x)
end = tf.InvertLink(joint2)(x)
joints = start.joints_between(end)
assert joints == [joint1, joint2, joint1, joint2]
def test_keep_link():
points = np.arange(200 * 3).reshape(200, 3)
keep_link = tf.Translation((0, 1, 0))
link = tf.CompundLink([
tf.Translation((1, 0, 0)),
tf.CompundLink([keep_link]),
tf.EulerRotation("Y", -90, degrees=True),
])
simplified_links = tf.simplify_links([link], keep_links=[keep_link])
assert keep_link in simplified_links
expected = link.transform(points)
result = points
for link in simplified_links:
result = link.transform(result)
assert np.allclose(result, expected)
keep_link = tf.Translation((0, 1, 0))
original_link = tf.CompundLink([
tf.Translation((1, 0, 0)),
tf.CompundLink([tf.InvertLink(keep_link)]),
tf.EulerRotation("Y", -90, degrees=True),
])
simplified_links = tf.simplify_links([original_link],
keep_links=[keep_link])
for link in simplified_links:
if isinstance(link, tf.InvertLink):
if link._forward_link is keep_link:
break
else:
raise AssertionError("`keep_link` no longer in link chain.")
expected = original_link.transform(points)
result = points
for link in simplified_links:
result = link.transform(result)
assert np.allclose(result, expected)
keep_link = tf.Translation((0, 1, 0))
original_link = tf.CompundLink([
tf.Translation((1, 0, 0)),
tf.Translation((1, 1, 0)),
keep_link,
tf.EulerRotation("Y", -90, degrees=True),
])
simplified_links = tf.simplify_links([original_link],
keep_links=[keep_link])
assert keep_link in simplified_links
expected = original_link.transform(points)
result = points
for link in simplified_links:
result = link.transform(result)
assert np.allclose(result, expected)
def test_link_ordering():
points = np.arange(200 * 3).reshape(200, 3)
links: List[tf.Link] = [
tf.Rotation((1, 0, 0), (0, 1, 0)),
tf.Translation((1, 0, 0)),
tf.Translation((1, 1, 0)),
tf.Translation((1, 1, 1)),
]
simplified_links = tf.simplify_links(links)
expected = points
for link in links:
expected = link.transform(expected)
result = points
for link in simplified_links:
result = link.transform(result)
assert np.allclose(result, expected)
assert isinstance(simplified_links[-1], tf.Rotation)
def test_named_links_between():
link = tf.Translation((1, 0))
a = tf.Frame(2, name="foo")
x = link(a)
y = link(x)
z = link(y)
elements = z.links_between("foo")
assert len(elements) == 3
def test_compound_frame(vec_child, vec_parent):
translation = tf.Translation((3, 1, 3))
rotation = tf.EulerRotation("Z", 90, degrees=True)
pose = tf.CompundLink([rotation, translation])
out = pose.transform(vec_child)
assert np.allclose(out, vec_parent)
out = pose.__inverse_transform__(vec_parent)
assert np.allclose(out, vec_child)
def test_inverse_inverse():
link = tf.Translation((1, 0), amount=0.5)
inv_link = tf.InvertLink(link)
inverse_inverse = tf.InvertLink(inv_link)
point = (3, 5)
result = inverse_inverse.transform(point)
expected = link.transform(point)
assert np.allclose(result, expected)
def test_named_links_between():
link = tf.Translation((1, 0))
a = tf.Frame(2, name="foo")
x = link(a)
y = link(x)
z = link(y)
with pytest.deprecated_call():
elements = z.transform_chain("foo")
assert len(elements) == 3
def test_identity_drops():
link = tf.Rotation((1, 0, 0), (0, 1, 0))
link.angle = 0
simplified_links = tf.simplify_links([link])
assert len(simplified_links) == 0
link = tf.EulerRotation("XYZ", (0, 0, 0))
simplified_links = tf.simplify_links([link])
assert len(simplified_links) == 0
link = tf.Translation((1, 2, 3), amount=0)
simplified_links = tf.simplify_links([link])
assert len(simplified_links) == 0
def test_affine_matrix():
child = tf.Frame(3)
rotated = tf.RotvecRotation((1, 0, 0), angle=0)(child)
world = tf.Translation((0, 1, 0))(rotated)
origin_in_world = child.transform((0, 0, 0), world)
assert np.allclose(origin_in_world, (0, 1, 0))
affine_matrix = child.get_affine_matrix(world)
expected_tf = np.array([
[1, 0, 0, 0],
[0, 1, 0, 1],
[0, 0, 1, 0],
[0, 0, 0, 1],
])
assert np.allclose(affine_matrix, expected_tf)
def test_frames_between():
link = tf.Translation((1, 0))
a = tf.Frame(2, name="foo")
x = link(a)
y = link(x)
z = link(y)
elements = z.frames_between("foo")
assert len(elements) == 4
assert elements[0] == z
assert elements[1] == y
assert elements[2] == x
assert elements[3] == a
elements = z.frames_between("foo",
include_self=False,
include_to_frame=False)
assert len(elements) == 2
assert elements[0] == y
assert elements[1] == x
def test_combine_translation():
points = np.arange(200 * 3).reshape(200, 3)
link = tf.CompundLink([
tf.Translation((1, 0, 0)),
tf.CompundLink([tf.Translation((0, 1, 0))]),
])
simplified_links = tf.simplify_links([link])
assert len(simplified_links) == 1
simple_link = simplified_links[0]
assert isinstance(simple_link, tf.Translation)
assert simple_link._amount == 1
assert np.allclose(simple_link._direction, (1, 1, 0))
expected = link.transform(points)
result = simple_link.transform(points)
assert np.allclose(result, expected)
link = tf.CompundLink([
tf.Translation((1, 0, 0)),
tf.CompundLink(
[tf.Translation((0, 1, 0)),
tf.Translation((0, 0, 1), amount=0.3)]),
tf.Translation((0.2, 0, 0), amount=0.5),
])
simplified_links = tf.simplify_links([link])
assert len(simplified_links) == 1
simple_link = simplified_links[0]
assert isinstance(simple_link, tf.Translation)
assert simple_link._amount == 1
assert np.allclose(simple_link._direction, (1.1, 1, 0.3))
expected = link.transform(points)
result = simple_link.transform(points)
assert np.allclose(result, expected)
def test_inverse_attributes():
link = tf.Translation((1, 0), amount=0.5)
inv_link = tf.InvertLink(link)
hasattr(inv_link, "amount")
assert inv_link.amount == 0.5
