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_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_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_plot_mesh_smoke_with_package_dir(): if not matplotlib_available: raise SkipTest("matplotlib is required for this test") urdf = """ <?xml version="1.0"?> <robot name="simple_mechanism"> <link name="cone"> <visual name="cone"> <origin xyz="0 0 0" rpy="0 1.5708 0"/> <geometry> <mesh filename="package://test/test_data/cone.stl"/> </geometry> </visual> </link> </robot> """ tm = UrdfTransformManager() tm.load_urdf(urdf, package_dir="./") tm.plot_visuals("cone")
""" print(__doc__) import os import numpy as np import matplotlib.pyplot as plt from pytransform3d.urdf import UrdfTransformManager BASE_DIR = "test/test_data/" data_dir = BASE_DIR search_path = "." while (not os.path.exists(data_dir) and os.path.dirname(search_path) != "pytransform3d"): search_path = os.path.join(search_path, "..") data_dir = os.path.join(search_path, BASE_DIR) tm = UrdfTransformManager() filename = os.path.join(data_dir, "robot_with_visuals.urdf") with open(filename, "r") as f: robot_urdf = f.read() tm.load_urdf(robot_urdf, mesh_path=data_dir) tm.set_joint("joint2", 0.2 * np.pi) tm.set_joint("joint3", 0.2 * np.pi) tm.set_joint("joint5", 0.1 * np.pi) tm.set_joint("joint6", 0.5 * np.pi) tm.plot_visuals("robot_arm", ax_s=0.6, alpha=0.7) plt.show()
This example shows how to load a URDF with STL meshes. This example must be run from within the examples folder or the main folder because it uses a hard-coded path to the URDF file and the meshes. """ import os import matplotlib.pyplot as plt from pytransform3d.urdf import UrdfTransformManager BASE_DIR = "test/test_data/" data_dir = BASE_DIR search_path = "." while (not os.path.exists(data_dir) and os.path.dirname(search_path) != "pytransform3d"): search_path = os.path.join(search_path, "..") data_dir = os.path.join(search_path, BASE_DIR) tm = UrdfTransformManager() with open(os.path.join(data_dir, "simple_mechanism.urdf"), "r") as f: tm.load_urdf(f.read(), mesh_path=data_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) ax.set_xlim((-0.1, 0.15)) ax.set_ylim((-0.1, 0.15)) ax.set_zlim((0.0, 0.25)) plt.show()