def test_unique_frame_names():
urdf = """
<robot name="robot_name">
<link name="link0"/>
<link name="link1">
<collision name="link1_geometry">
<origin xyz="0 0 1"/>
<geometry/>
</collision>
<visual name="link1_geometry">
<origin xyz="0 0 1"/>
<geometry/>
</visual>
</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)
assert_in("robot_name", tm.nodes)
assert_in("link0", tm.nodes)
assert_in("link1", tm.nodes)
assert_in("visual:link1/link1_geometry", tm.nodes)
assert_in("collision:link1/link1_geometry", tm.nodes)
assert_equal(len(tm.nodes), 5)
def test_multiple_visuals():
urdf = """
<robot name="robot_name">
<link name="link0">
<visual>
<origin xyz="0 0 1"/>
<geometry>
<sphere radius="0.123"/>
</geometry>
</visual>
</link>
<link name="link1">
<visual>
<origin xyz="0 0 1"/>
<geometry>
<sphere radius="0.234"/>
</geometry>
</visual>
</link>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
assert_equal(len(tm.visuals), 2)
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_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_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_plot_mesh_smoke_with_scale():
if not matplotlib_available:
raise SkipTest("matplotlib is required for this test")
BASE_DIR = "test/test_data/"
tm = UrdfTransformManager()
with open(BASE_DIR + "simple_mechanism.urdf", "r") as f:
tm.load_urdf(f.read(), mesh_path=BASE_DIR)
tm.set_joint("joint", -1.1)
ax = tm.plot_frames_in(
"lower_cone", s=0.1, whitelist=["upper_cone", "lower_cone"], show_name=True)
ax = tm.plot_connections_in("lower_cone", ax=ax)
tm.plot_visuals("lower_cone", ax=ax)
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_mesh_missing_filename():
urdf = """
<?xml version="1.0"?>
<robot name="simple_mechanism">
<link name="upper_cone">
<visual name="upper_cone">
<origin xyz="0 0 0" rpy="0 1.5708 0"/>
<geometry>
<mesh scale="1 1 0.5"/>
</geometry>
</visual>
</link>
<link name="lower_cone">
<visual name="lower_cone">
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh scale="1 1 0.5"/>
</geometry>
</visual>
</link>
<joint name="joint" type="revolute">
<origin xyz="0 0 0.2" rpy="0 0 0"/>
<parent link="lower_cone"/>
<child link="upper_cone"/>
<axis xyz="1 0 0"/>
<limit lower="-2.79253" upper="2.79253" effort="0" velocity="0"/>
</joint>
</robot>
"""
tm = UrdfTransformManager()
assert_raises(UrdfException, tm.load_urdf, urdf, mesh_path="")
def test_plot_without_mesh():
if not matplotlib_available:
raise SkipTest("matplotlib is required for this test")
BASE_DIR = "test/test_data/"
tm = UrdfTransformManager()
with open(BASE_DIR + "simple_mechanism.urdf", "r") as f:
tm.load_urdf(f.read(), mesh_path=None)
tm.set_joint("joint", -1.1)
ax = tm.plot_frames_in(
"lower_cone", s=0.1, whitelist=["upper_cone", "lower_cone"], show_name=True)
ax = tm.plot_connections_in("lower_cone", ax=ax)
with warnings.catch_warnings(record=True) as w:
tm.plot_visuals("lower_cone", ax=ax)
assert_equal(len(w), 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_plot_mesh_smoke_without_scale():
if not matplotlib_available:
raise SkipTest("matplotlib is required for this test")
urdf = """
<?xml version="1.0"?>
<robot name="simple_mechanism">
<link name="upper_cone">
<visual name="upper_cone">
<origin xyz="0 0 0" rpy="0 1.5708 0"/>
<geometry>
<mesh filename="cone.stl"/>
</geometry>
</visual>
</link>
<link name="lower_cone">
<visual name="lower_cone">
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="cone.stl"/>
</geometry>
</visual>
</link>
<joint name="joint" type="revolute">
<origin xyz="0 0 0.2" rpy="0 0 0"/>
<parent link="lower_cone"/>
<child link="upper_cone"/>
<axis xyz="1 0 0"/>
<limit lower="-2.79253" upper="2.79253" effort="0" velocity="0"/>
</joint>
</robot>
"""
BASE_DIR = "test/test_data/"
tm = UrdfTransformManager()
tm.load_urdf(urdf, mesh_path=BASE_DIR)
tm.set_joint("joint", -1.1)
ax = tm.plot_frames_in("lower_cone",
s=0.1,
whitelist=["upper_cone", "lower_cone"],
show_name=True)
ax = tm.plot_connections_in("lower_cone", ax=ax)
tm.plot_visuals("lower_cone", ax=ax)
def test_prismatic_joint():
urdf = """
<?xml version="1.0"?>
<robot name="mmm">
<link name="parent"/>
<link name="child"/>
<joint name="joint" type="prismatic">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="parent"/>
<child link="child"/>
<axis xyz="1 0 0"/>
</joint>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
tm.set_joint("joint", 5.33)
c2p = tm.get_transform("child", "parent")
assert_array_almost_equal(
c2p,
np.array([[1, 0, 0, 5.33],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
)
def test_collision_box_without_size():
urdf = """
<robot name="robot_name">
<link name="link0">
<collision>
<origin xyz="0 0 1"/>
<geometry>
<box/>
</geometry>
</collision>
</link>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
assert_equal(len(tm.collision_objects), 1)
assert_array_almost_equal(tm.collision_objects[0].size, np.zeros(3))
def test_collision_sphere():
urdf = """
<robot name="robot_name">
<link name="link0">
<collision>
<origin xyz="0 0 1"/>
<geometry>
<sphere radius="0.123"/>
</geometry>
</collision>
</link>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
assert_equal(len(tm.collision_objects), 1)
assert_equal(tm.collision_objects[0].radius, 0.123)
def test_parse_material_without_color():
urdf = """
<?xml version="1.0"?>
<robot name="mmm">
<material name="Black"/>
<link name="root">
<visual>
<geometry>
<box size="0.758292 1.175997 0.8875"/>
<material name="Black"/>
</geometry>
</visual>
</link>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
assert_true(tm.visuals[0].color is None)
def __init__(self, urdf_path, theta_stds=None):
""" Constructor:
Params:
urdf_path: string: path to the URDF file containing the model of the manipulator
including all relevant parameters
theta_stds: 15x1-vector: standard deviations of the joint accuracies of the manipulator (rad)
"""
with open(urdf_path) as f:
urdf_string = f.read()
self.tm = UrdfTransformManager()
self.tm.load_urdf(urdf_string)
self.n_DOFS = 15
self.has_jacobian = False
self.has_IK = False
self.has_singularity_condition = False
#link names of the investigated subchain of Atlas from base to end effector (15-DOF)
self.sorted_link_names = ['l_foot', 'l_talus', 'l_lleg', 'l_uleg', 'l_lglut', 'l_uglut', 'pelvis', 'ltorso', 'mtorso',\
'utorso', 'l_clav', 'l_scap', 'l_uarm', 'l_larm', 'l_farm', 'l_hand', 'tcp']
self.joint_info = self._extract_relevant_joints()
#joint names of the investigated subchain of Atlas from base to end effector (15-DOF)
self.sorted_joint_names = self._get_sorted_joint_names()
if theta_stds is None:
#default is an uncertainty of a standard deviation of 1 degree per joint
self.theta_stds = np.zeros(self.n_DOFS) + (1 * math.pi / 180)
else:
self.theta_stds = theta_stds
self.joint_transformations_notheta = self._construct_transformations_notheta(
)
self.summands2, self.summands3 = self._prepare_rodrigues_summands()
#trajectory that is tested in one of the experiments
#numbers are: [start_x, end_x, start_y, end_y, start_z, end_z]
self.trajectory = [-0.5, -0.1, -0.3, 0.3, 1.3, 1.8]
#arbitrarily chosen subspace of the maximum workspace (for no joint limits) where we train and test
#it is secured via numerical tests that this set really lies in the workspace
self.subspace = np.array([[0.7, 1.2], [-0.4, 0.1], [1,
1.5], [0.6, 0.6],
[0.1, 0.1], [0.1, 0.1], [0, 0.5]])
def __init__(self, urdf_path, theta_stds=None):
""" Constructor:
Params:
urdf_path: path to the URDF file containing the model of the manipulator
including all relevant parameters
theta_stds: 6x1-vector: standard deviations of the joint accuracies of the manipulator (rad)
"""
with open(urdf_path) as f:
urdf_string = f.read()
self.tm = UrdfTransformManager()
self.tm.load_urdf(urdf_string)
self.n_DOFS = 6
self.has_jacobian = True
self.has_IK = True
self.has_singularity_condition = True
#sorted link names of the serial chain
self.sorted_link_names = ['base_link', 'link1', 'link2', 'link3', 'link4', 'link5', 'link6', 'tcp']
#sorted joint names of the serial chain
self.sorted_joint_names = self._get_sorted_joint_names()
self.joint_info = self.tm._joints.copy()
if theta_stds is None:
#default is an uncertainty of a standard deviation of 1 degree per joint
self.theta_stds = np.zeros(self.n_DOFS) + (1*math.pi/180)
else:
self.theta_stds = theta_stds
self.joint_transformations_notheta = self._construct_transformations_notheta()
#trajectory that is tested in one of the experiments
#numbers are: [start_x, end_x, start_y, end_y, start_z, end_z]
self.trajectory = [-0.5, -0.1, -0.3, 0.3, 0.4, 0.4]
#arbitrarily chosen subspace of the maximum workspace (for no joint limits) where we train and test
#it is secured via numerical tests that this set really lies in the workspace
self.subspace = np.array([[0, 0.5],
[0, 0.3],
[0.3, 0.6],
[0.8, 0.8],
[0.1, 0.1],
[0.1, 0.1],
[0, 0.3]])
def _load_urdf(self):
import os
urdf_path = os.path.join(
os.getenv("HELLO_FLEET_PATH"),
os.getenv("HELLO_FLEET_ID"),
"exported_urdf",
"stretch.urdf",
)
from pytransform3d.urdf import UrdfTransformManager
import pytransform3d.transformations as pt
import pytransform3d.visualizer as pv
import numpy as np
self.tm = UrdfTransformManager()
with open(urdf_path, "r") as f:
urdf = f.read()
self.tm.load_urdf(urdf)
def test_parse_material_local():
urdf = """
<?xml version="1.0"?>
<robot name="mmm">
<link name="root">
<visual>
<geometry>
<box size="0.758292 1.175997 0.8875"/>
<material name="Black">
<color rgba="1.0 0.0 0.0 1.0"/>
</material>
</geometry>
</visual>
</link>
</robot>
"""
tm = UrdfTransformManager()
tm.load_urdf(urdf)
assert_array_almost_equal(tm.visuals[0].color, np.array([1, 0, 0, 1]))
def test_reference_to_unknown_parent():
urdf = """
<robot name="robot_name">
<link name="link1"/>
<joint name="joint0" type="fixed">
<parent link="link0"/>
<child link="link1"/>
</joint>
</robot>
"""
assert_raises(UrdfException, UrdfTransformManager().load_urdf, urdf)
def test_missing_child_link_name():
urdf = """
<robot name="robot_name">
<link name="link0"/>
<link name="link1"/>
<joint name="joint0" type="fixed">
<parent link="link0"/>
<child/>
</joint>
</robot>
"""
assert_raises(UrdfException, UrdfTransformManager().load_urdf, urdf)
def test_unknown_joint_type():
urdf = """
<robot name="robot_name">
<link name="link0"/>
<link name="link1"/>
<joint name="joint0" type="does_not_exist">
<parent link="link0"/>
<child link="link1"/>
</joint>
</robot>
"""
assert_raises(UrdfException, UrdfTransformManager().load_urdf, urdf)
def test_unsupported_joint_type():
urdf = """
<robot name="robot_name">
<link name="link0"/>
<link name="link1"/>
<joint name="joint0" type="prismatic">
<parent link="link0"/>
<child link="link1"/>
</joint>
</robot>
"""
assert_raises(UrdfException, UrdfTransformManager().load_urdf, urdf)
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("collision:link1/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_joint_limits():
tm = UrdfTransformManager()
tm.load_urdf(COMPI_URDF)
assert_raises(KeyError, tm.get_joint_limits, "unknown_joint")
assert_array_almost_equal(tm.get_joint_limits("joint1"), (-1, 1))
assert_array_almost_equal(tm.get_joint_limits("joint2"), (-1, np.inf))
assert_array_almost_equal(tm.get_joint_limits("joint3"), (-np.inf, 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_collision_cylinder_without_length():
urdf = """
<robot name="robot_name">
<link name="link0">
<collision>
<origin xyz="0 0 1"/>
<geometry>
<cylinder radius="0.123"/>
</geometry>
</collision>
</link>
</robot>
"""
assert_raises(UrdfException, UrdfTransformManager().load_urdf, urdf)
def test_parse_material_without_name():
urdf = """
<?xml version="1.0"?>
<robot name="mmm">
<material/>
<link name="root">
<visual>
<geometry>
<box size="0.758292 1.175997 0.8875"/>
<material name="Black"/>
</geometry>
</visual>
</link>
</robot>
"""
tm = UrdfTransformManager()
assert_raises(UrdfException, tm.load_urdf, urdf)
def test_visual_without_geometry():
urdf = """
<robot name="robot_name">
<link name="link0"/>
<link name="link1">
<visual name="link1_visual">
<origin xyz="0 0 1"/>
</visual>
</link>
<joint name="joint0" type="fixed">
<parent link="link0"/>
<child link="link1"/>
<origin xyz="0 1 0"/>
</joint>
</robot>
"""
assert_raises(UrdfException, UrdfTransformManager().load_urdf, urdf)
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]]))
