def circle_bot():
world = tf.Frame(3, name="world")
ellbow = tf.Frame(3, name="ellbow")
tool = tf.Frame(3, name="tool")
rotate = tf.RotationalJoint((0, 0, 1), angle=0)
reach = tf.PrismaticJoint((-1, 0, 0), upper_limit=10, lower_limit=-10)
rotate(world, ellbow)
reach(ellbow, tool)
return world, [rotate, reach]
def simple_graph():
frames = [tf.Frame(1, name=f"frame{str(idx)}") for idx in range(10)]
def directional(idxA, idxB):
# "degenerate" links that track path cost
tf.CustomLink(1, 1, lambda x: x + 1)(frames[idxA],
frames[idxB],
add_inverse=False)
def undirectional(idxA, idxB):
tf.CustomLink(1, 1, lambda x: x + 1)(frames[idxA], frames[idxB])
tf.CustomLink(1, 1, lambda x: x + 1)(frames[idxB], frames[idxA])
undirectional(0, 2)
undirectional(3, 2)
undirectional(4, 2)
undirectional(6, 2)
undirectional(4, 5)
undirectional(5, 9)
undirectional(5, 6)
undirectional(6, 8)
directional(6, 7)
directional(2, 1)
directional(5, 8)
return frames
def test_inverse_transform():
frame1 = tf.Frame(3)
frame2 = tf.affine.Translation((0, 1, 0))(frame1)
result = frame1.transform((1, 0, 0), to_frame=frame2)
assert np.allclose(result, (1, 1, 0))
result = frame2.transform(result, to_frame=frame1)
assert np.allclose(result, (1, 0, 0))
def test_2d_rotation_target():
frameA = tf.Frame(2, name="world")
frameB = tf.AngleJoint()(frameA)
target = ik.RotationTarget(
tf.AngleJoint(angle=90, degrees=True),
frameA,
frameB,
)
assert np.isclose(target.score(), np.pi / 2)
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_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_translation_amount():
link = tf.affine.Translation((1, 0))
assert link.amount == 1
frameA = tf.Frame(2)
frameB = link(frameA)
point = np.array((0, 1), dtype=np.float_)
point_in_B = np.array((1, 1), dtype=np.float_)
assert np.allclose(frameA.transform(point, frameB), point_in_B)
link.amount = 0
assert np.allclose(frameA.transform(point, frameB), point)
def test_1d_robot():
arm = tf.affine.Translation((1, 0))
joint = tf.affine.Rotation((1, 0), (0, 1))
assert np.allclose(arm.direction, (1, 0))
tool_frame = tf.Frame(2)
ellbow_frame = arm(tool_frame)
world_frame = joint(ellbow_frame)
joint.angle = 0
tool_pos = tool_frame.transform((0, 0), to_frame=world_frame)
assert np.allclose(tool_pos, (1, 0))
joint.angle = np.pi / 2
tool_pos = tool_frame.transform((0, 0), to_frame=world_frame)
assert np.allclose(tool_pos, (0, -1))
def test_target():
frameA = tf.Frame(2, name="root")
joint = tf.PrismaticJoint((1, 0), upper_limit=10, lower_limit=-10)
frameB = joint(frameA)
target = ik.Target(frameB, frameA)
assert target.uses(joint)
assert target.usage_count(joint) == 1
missing_joint = tf.PrismaticJoint((1, 0), upper_limit=2, lower_limit=-2)
assert target.uses(missing_joint) == False
assert target.usage_count(missing_joint) == 0
target = ik.Target(frameA, frameB)
assert target.uses(joint)
assert target.usage_count(joint) == 1
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_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_matrix_identity():
link = tf.affine.Rotation((0, 1, 0), (0, 0, 1))
world_frame = tf.Frame(3)
camera_frame = link(world_frame)
link_mat = world_frame.get_affine_matrix(camera_frame)
link_inv_mat = camera_frame.get_affine_matrix(world_frame)
assert np.allclose(link_mat @ link_inv_mat, np.eye(4))
assert np.allclose(link_inv_mat @ link_mat, np.eye(4))
link.angle = np.pi / 2
assert link.angle == np.pi / 2
link_mat = world_frame.get_affine_matrix(camera_frame)
link_inv_mat = camera_frame.get_affine_matrix(world_frame)
assert np.allclose(link_mat @ link_inv_mat, np.eye(4))
assert np.allclose(link_inv_mat @ link_mat, np.eye(4))
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_affine():
cartesian = tf.Frame(3)
affine = tf.AffineSpace(3)(cartesian)
input = np.array([[1, 0, 0], [1, 1, 0], [1, 1, 1]])
expected = np.array([[1, 0, 0, 1], [1, 1, 0, 1], [1, 1, 1, 1]])
as_affine = cartesian.transform(input, affine)
assert np.allclose(as_affine, expected)
as_cartesian = affine.transform(as_affine, cartesian)
assert np.allclose(as_cartesian, input)
input = np.array([
[1, 1, 1, 1],
[0.5, 0.5, 0.5, 0.5],
[3, 3, 3, 3],
[0.1, 0.1, 0.1, 0.1],
])
expected = np.ones((4, 3))
result = affine.transform(input, cartesian)
assert np.allclose(result, expected)