Robot URDF visualizer#
Requires yourdfpy and robot_descriptions. Any URDF supported by yourdfpy should work.
The viser.extras.ViserUrdf
is a lightweight interface between yourdfpy
and viser. It can also take a path to a local URDF file as input.
1from __future__ import annotations
2
3import time
4
5import numpy as onp
6import tyro
7import viser
8from robot_descriptions.loaders.yourdfpy import load_robot_description
9from viser.extras import ViserUrdf
10
11# A subset of robots available in the robot_descriptions package.
12ROBOT_MODEL_LIST = (
13 "panda_description",
14 "ur10_description",
15 "ur3_description",
16 "ur5_description",
17 "cassie_description",
18 "skydio_x2_description",
19 "allegro_hand_description",
20 "barrett_hand_description",
21 "robotiq_2f85_description",
22 "atlas_drc_description",
23 "atlas_v4_description",
24 "draco3_description",
25 "g1_description",
26 "h1_description",
27 "anymal_c_description",
28 "go2_description",
29 "mini_cheetah_description",
30)
31
32
33def main() -> None:
34 # Start viser server.
35 server = viser.ViserServer()
36
37 # Logic for updating the visualized robot.
38 gui_joints: list[viser.GuiInputHandle[float]] = []
39 initial_angles: list[float] = []
40
41 def update_robot_model(robot_name: str) -> None:
42 server.scene.reset()
43
44 loading_modal = server.gui.add_modal("Loading URDF...")
45 with loading_modal:
46 server.gui.add_markdown("See terminal for progress!")
47
48 # Create a helper for adding URDFs to Viser. This just adds meshes to the scene,
49 # helps us set the joint angles, etc.
50 urdf = ViserUrdf(
51 server,
52 # This can also be set to a path to a local URDF file.
53 urdf_or_path=load_robot_description(robot_name),
54 )
55 loading_modal.close()
56
57 for gui_joint in gui_joints:
58 gui_joint.remove()
59 gui_joints.clear()
60
61 for joint_name, (lower, upper) in urdf.get_actuated_joint_limits().items():
62 lower = lower if lower is not None else -onp.pi
63 upper = upper if upper is not None else onp.pi
64
65 initial_angle = 0.0 if lower < 0 and upper > 0 else (lower + upper) / 2.0
66 slider = server.gui.add_slider(
67 label=joint_name,
68 min=lower,
69 max=upper,
70 step=1e-3,
71 initial_value=initial_angle,
72 )
73 slider.on_update( # When sliders move, we update the URDF configuration.
74 lambda _: urdf.update_cfg(onp.array([gui.value for gui in gui_joints]))
75 )
76
77 gui_joints.append(slider)
78 initial_angles.append(initial_angle)
79
80 # Apply initial joint angles.
81 urdf.update_cfg(onp.array([gui.value for gui in gui_joints]))
82
83 robot_model_name = server.gui.add_dropdown("Robot model", ROBOT_MODEL_LIST)
84 robot_model_name.on_update(lambda _: update_robot_model(robot_model_name.value))
85
86 # Create joint reset button.
87 reset_button = server.gui.add_button("Reset")
88
89 @reset_button.on_click
90 def _(_):
91 for g, initial_angle in zip(gui_joints, initial_angles):
92 g.value = initial_angle
93
94 while True:
95 time.sleep(10.0)
96
97
98if __name__ == "__main__":
99 tyro.cli(main)