def test_collision():
urdf = """
<robot name="robot_name">
<link name="link0"/>
<link name="link1">
<collision>
<origin xyz="0 0 1"/>
<geometry/>
</collision>
</link>
<joint name="joint0" type="fixed">
<parent link="link0"/>
<child link="link1"/>
<origin xyz="0 0 1"/>
</joint>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
link1_to_link0 = tm.get_transform("link1", "link0")
expected_link1_to_link0 = transform_from(np.eye(3), np.array([0, 0, 1]))
assert_array_almost_equal(link1_to_link0, expected_link1_to_link0)
link1_to_link0 = tm.get_transform("link1/collision_0", "link0")
expected_link1_to_link0 = transform_from(np.eye(3), np.array([0, 0, 2]))
assert_array_almost_equal(link1_to_link0, expected_link1_to_link0)
def test_multiple_parents():
urdf = """
<?xml version="1.0"?>
<robot name="mmm">
<link name="parent0"/>
<link name="parent1"/>
<link name="child"/>
<joint name="joint0" type="revolute">
<origin xyz="1 0 0" rpy="0 0 0"/>
<parent link="parent0"/>
<child link="child"/>
<axis xyz="1 0 0"/>
</joint>
<joint name="joint1" type="revolute">
<origin xyz="0 1 0" rpy="0 0 0"/>
<parent link="parent1"/>
<child link="child"/>
<axis xyz="1 0 0"/>
</joint>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
p0c = tm.get_transform("parent0", "child")
p1c = tm.get_transform("parent1", "child")
assert_equal(p0c[0, 3], p1c[1, 3])
def test_fixed_joint():
tm = UrdfTransformManager()
tm.load_urdf(COMPI_URDF)
tcp_to_link0 = tm.get_transform("tcp", "linkmount")
assert_array_almost_equal(
tcp_to_link0,
np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1.174], [0, 0, 0, 1]]))
def test_ee_frame():
tm = UrdfTransformManager()
tm.load_urdf(COMPI_URDF)
link7_to_linkmount = tm.get_transform("link6", "linkmount")
assert_array_almost_equal(
link7_to_linkmount,
np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1.124], [0, 0, 0, 1]]))
def test_joint_limit_clipping():
tm = UrdfTransformManager()
tm.load_urdf(COMPI_URDF)
tm.set_joint("joint1", 2.0)
link7_to_linkmount = tm.get_transform("link6", "linkmount")
assert_array_almost_equal(
link7_to_linkmount,
np.array([[0.5403023, -0.8414710, 0, 0], [0.8414710, 0.5403023, 0, 0],
[0, 0, 1, 1.124], [0, 0, 0, 1]]))
tm.set_joint("joint1", -2.0)
link7_to_linkmount = tm.get_transform("link6", "linkmount")
assert_array_almost_equal(
link7_to_linkmount,
np.array([[0.5403023, 0.8414710, 0, 0], [-0.8414710, 0.5403023, 0, 0],
[0, 0, 1, 1.124], [0, 0, 0, 1]]))
def test_joint_angles():
tm = UrdfTransformManager()
tm.load_urdf(COMPI_URDF)
for i in range(1, 7):
tm.set_joint("joint%d" % i, 0.1 * i)
link7_to_linkmount = tm.get_transform("link6", "linkmount")
assert_array_almost_equal(
link7_to_linkmount,
np.array([[0.121698, -0.606672, 0.785582, 0.489351],
[0.818364, 0.509198, 0.266455, 0.114021],
[-0.561668, 0.610465, 0.558446, 0.924019], [0., 0., 0.,
1.]]))
def test_without_origin():
urdf = """
<robot name="robot_name">
<link name="link0"/>
<link name="link1"/>
<joint name="joint0" type="fixed">
<parent link="link0"/>
<child link="link1"/>
</joint>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
link1_to_link0 = tm.get_transform("link1", "link0")
assert_array_almost_equal(link1_to_link0, np.eye(4))
def test_with_empty_axis():
urdf = """
<robot name="robot_name">
<link name="link0"/>
<link name="link1"/>
<joint name="joint0" type="revolute">
<parent link="link0"/>
<child link="link1"/>
<origin/>
<axis/>
</joint>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
tm.set_joint("joint0", np.pi)
link1_to_link0 = tm.get_transform("link1", "link0")
assert_array_almost_equal(
link1_to_link0,
np.array([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]))
from pytransform.urdf import UrdfTransformManager
# A script that helps to determine the optimum ratio
# between step-sizes in Cartesian and joint space.
# We sample joint angles with variance 1 and measure
# the resulting standard deviation in Cartesian space.
tm = UrdfTransformManager()
tm.load_urdf(open("../data/kuka_lbr.urdf", "r"))
random_state = np.random.RandomState(42)
n_samples = 1000
joint_limits = np.array(
[tm.get_joint_limits("kuka_lbr_l_joint_%d" % (j + 1))
for j in range(7)]
)
#joint_angles = (random_state.rand(n_samples, 7) *
# (joint_limits[:, 1] - joint_limits[:, 0]) +
# joint_limits[:, 0])
joint_angles = random_state.randn(n_samples, 7)
positions = np.empty((n_samples, 3))
for n in range(n_samples):
for j in range(7):
tm.set_joint("kuka_lbr_l_joint_%d" % (j + 1), joint_angles[n, j])
positions[n] = tm.get_transform("kuka_lbr_l_link_7", "kuka_lbr_l_link_0")[:3, 3]
print(np.std(joint_angles, axis=0))
print(np.mean(positions, axis=0))
print(np.std(positions, axis=0))