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_multi_frame_ccd(panda):
base_frame: tf.Frame
joints: List[joint_types]
base_frame, joints = panda
tool_frame = base_frame.find_frame(".../panda_link8")
root_pos = tool_frame.transform((0, 0, 0), base_frame)
tool_origin = tool_frame.transform((0, 0, 0), base_frame)
tool_facing = tool_frame.transform((0, 1, 0), base_frame)
initial_ori = tool_facing - tool_origin
expected = np.zeros(len(joints))
for idx, joint in enumerate(joints):
expected[idx] = joint.param
joint.param += 0.2
targets = [
ik.PositionTarget((0, 0, 0), root_pos, tool_frame, base_frame),
ik.RotationTarget(
tf.EulerRotation("X", 180, degrees=True), tool_frame, base_frame
),
]
ik.gd(targets, joints)
tool_origin = tool_frame.transform((0, 0, 0), base_frame)
tool_facing = tool_frame.transform((0, 0, 1), base_frame)
final_ori = tool_facing - tool_origin
assert np.allclose(final_ori, (0, 0, -1), atol=0.001)
final_pos = tool_frame.transform((0, 0, 0), base_frame)
assert np.allclose(final_pos, root_pos, atol=0.001)
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_EulerRotation(sequence, angles, degrees):
in_vectors = np.eye(3)
angles = np.asarray(angles)
rot = tf.EulerRotation(sequence, angles, degrees=degrees)
scipy_rot = ScipyRotation.from_euler(sequence, angles, degrees)
result = rot.transform(in_vectors)
expected = scipy_rot.apply(in_vectors)
assert np.allclose(result, expected)
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_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_EulerRotation_vectorized(sequence, angles, degrees):
angles = np.asarray(angles)
in_vectors = np.eye(3)
rot = tf.EulerRotation(sequence, angles[None, ...], degrees=degrees)
scipy_rot = ScipyRotation.from_euler(sequence, angles, degrees)
result = rot.transform(in_vectors[:, None, :])
expected = list()
for basis in in_vectors:
partial = scipy_rot.apply(basis)
expected.append(partial)
expected = np.stack(expected, axis=0)
assert np.allclose(result, expected)
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_panda_orientation(panda):
base_frame: tf.Frame
joints: List[joint_types]
base_frame, joints = panda
tool_frame = base_frame.find_frame(".../panda_link8")
expected = np.zeros(len(joints))
for idx, joint in enumerate(joints):
expected[idx] = joint.param
joint.param = joint.param + 0.2
targets = [
ik.RotationTarget(
tf.EulerRotation("Y", 90, degrees=True), tool_frame, base_frame
)
]
ik.gd(targets, joints)
tool_origin = tool_frame.transform((0, 0, 0), base_frame)
tool_facing = tool_frame.transform((1, 0, 0), base_frame)
final_ori = tool_facing - tool_origin
assert np.allclose(final_ori, (0, 0, -1), atol=0.001)
def test_EulerRotation_invalid_sequence(sequence):
with pytest.raises(ValueError):
tf.EulerRotation(sequence, (0, 0, 0))