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]]))