Skip to content

Commit

Permalink
Use robot_descriptions for robot URDF example, fix modal edge case
Browse files Browse the repository at this point in the history
  • Loading branch information
brentyi committed May 29, 2024
1 parent d1d1253 commit 7a0e687
Show file tree
Hide file tree
Showing 6 changed files with 254 additions and 79 deletions.
4 changes: 1 addition & 3 deletions docs/source/examples/03_gui_callbacks.rst
Original file line number Diff line number Diff line change
Expand Up @@ -93,9 +93,7 @@ we get updates.
# Here, we update the point clouds + frames whenever any of the GUI items are updated.
gui_show_frame.on_update(lambda _: draw_frame())
gui_show_everything.on_update(
lambda _: server.scene.set_global_scene_node_visibility(
gui_show_everything.value
)
lambda _: server.scene.set_global_visibility(gui_show_everything.value)
)
gui_axis.on_update(lambda _: draw_frame())
gui_location.on_update(lambda _: draw_frame())
Expand Down
142 changes: 112 additions & 30 deletions docs/source/examples/09_urdf_visualizer.rst
Original file line number Diff line number Diff line change
@@ -1,17 +1,18 @@
.. Comment: this file is automatically generated by `update_example_docs.py`.
It should not be modified manually.
URDF visualizer
Robot URDF visualizer
==========================================


Requires yourdfpy and URDF. Any URDF supported by yourdfpy should work.
Requires yourdfpy and robot_descriptions. Any URDF supported by yourdfpy should work.

Examples:

* https://github.com/robot-descriptions/robot_descriptions.py
* https://github.com/clemense/yourdfpy

* https://github.com/OrebroUniversity/yumi/blob/master/yumi_description/urdf/yumi.urdf
* https://github.com/ankurhanda/robot-assets
The :class:`viser.extras.ViserUrdf` is a lightweight interface between yourdfpy
and viser. It can also take a path to a local URDF file as input.



Expand All @@ -27,37 +28,62 @@ Examples:
import numpy as onp
import tyro
import viser
from robot_descriptions.loaders.yourdfpy import load_robot_description
from viser.extras import ViserUrdf
def main(urdf_path: Path) -> None:
def main() -> None:
# Start viser server.
server = viser.ViserServer()
# Create a helper for adding URDFs to Viser. This just adds meshes to the scene,
# helps us set the joint angles, etc.
urdf = ViserUrdf(server, urdf_path)
# Create joint angle sliders.
# Logic for updating the visualized robot.
gui_joints: list[viser.GuiInputHandle[float]] = []
initial_angles: list[float] = []
for joint_name, (lower, upper) in urdf.get_actuated_joint_limits().items():
lower = lower if lower is not None else -onp.pi
upper = upper if upper is not None else onp.pi
initial_angle = 0.0 if lower < 0 and upper > 0 else (lower + upper) / 2.0
slider = server.gui.add_slider(
label=joint_name,
min=lower,
max=upper,
step=1e-3,
initial_value=initial_angle,
)
slider.on_update( # When sliders move, we update the URDF configuration.
lambda _: urdf.update_cfg(onp.array([gui.value for gui in gui_joints]))
def update_robot_model(robot_name: str) -> None:
server.scene.reset()
loading_modal = server.gui.add_modal("Loading URDF...")
with loading_modal:
server.gui.add_markdown("See terminal for progress!")
# Create a helper for adding URDFs to Viser. This just adds meshes to the scene,
# helps us set the joint angles, etc.
urdf = ViserUrdf(
server,
# This can also be set to a path to a local URDF file.
urdf_or_path=load_robot_description(robot_name),
)
loading_modal.close()
for gui_joint in gui_joints:
gui_joint.remove()
gui_joints.clear()
gui_joints.append(slider)
initial_angles.append(initial_angle)
for joint_name, (lower, upper) in urdf.get_actuated_joint_limits().items():
lower = lower if lower is not None else -onp.pi
upper = upper if upper is not None else onp.pi
initial_angle = 0.0 if lower < 0 and upper > 0 else (lower + upper) / 2.0
slider = server.gui.add_slider(
label=joint_name,
min=lower,
max=upper,
step=1e-3,
initial_value=initial_angle,
)
slider.on_update( # When sliders move, we update the URDF configuration.
lambda _: urdf.update_cfg(onp.array([gui.value for gui in gui_joints]))
)
gui_joints.append(slider)
initial_angles.append(initial_angle)
# Apply initial joint angles.
urdf.update_cfg(onp.array([gui.value for gui in gui_joints]))
robot_model_name = server.gui.add_dropdown("Robot model", ROBOT_MODEL_LIST)
robot_model_name.on_update(lambda _: update_robot_model(robot_model_name.value))
# Create joint reset button.
reset_button = server.gui.add_button("Reset")
Expand All @@ -67,12 +93,68 @@ Examples:
for g, initial_angle in zip(gui_joints, initial_angles):
g.value = initial_angle
# Apply initial joint angles.
urdf.update_cfg(onp.array([gui.value for gui in gui_joints]))
while True:
time.sleep(10.0)
ROBOT_MODEL_LIST = (
"edo_description",
"fanuc_m710ic_description",
"gen2_description",
"gen3_description",
"iiwa14_description",
"iiwa7_description",
"panda_description",
"poppy_ergo_jr_description",
"ur10_description",
"ur3_description",
"ur5_description",
"z1_description",
"bolt_description",
"cassie_description",
"rhea_description",
"spryped_description",
"upkie_description",
"baxter_description",
"nextage_description",
"poppy_torso_description",
"yumi_description",
"cf2_description",
"skydio_x2_description",
"double_pendulum_description",
"finger_edu_description",
"simple_humanoid_description",
"trifinger_edu_description",
"allegro_hand_description",
"barrett_hand_description",
"robotiq_2f85_description",
"atlas_drc_description",
"atlas_v4_description",
"draco3_description",
"ergocub_description",
"g1_description",
"h1_description",
"icub_description",
"jaxon_description",
"jvrc_description",
"r2_description",
"romeo_description",
"sigmaban_description",
"talos_description",
"valkyrie_description",
"a1_description",
"aliengo_description",
"anymal_b_description",
"anymal_c_description",
"b1_description",
"go1_description",
"go2_description",
"hyq_description",
"laikago_description",
"mini_cheetah_description",
"minitaur_description",
"solo_description",
)
if __name__ == "__main__":
tyro.cli(main)
142 changes: 112 additions & 30 deletions examples/09_urdf_visualizer.py
Original file line number Diff line number Diff line change
@@ -1,10 +1,11 @@
"""URDF visualizer
"""Robot URDF visualizer
Requires yourdfpy and URDF. Any URDF supported by yourdfpy should work.
Requires yourdfpy and robot_descriptions. Any URDF supported by yourdfpy should work.
- https://github.com/robot-descriptions/robot_descriptions.py
- https://github.com/clemense/yourdfpy
Examples:
- https://github.com/OrebroUniversity/yumi/blob/master/yumi_description/urdf/yumi.urdf
- https://github.com/ankurhanda/robot-assets
The :class:`viser.extras.ViserUrdf` is a lightweight interface between yourdfpy
and viser. It can also take a path to a local URDF file as input.
"""

from __future__ import annotations
Expand All @@ -15,37 +16,62 @@
import numpy as onp
import tyro
import viser
from robot_descriptions.loaders.yourdfpy import load_robot_description
from viser.extras import ViserUrdf


def main(urdf_path: Path) -> None:
def main() -> None:
# Start viser server.
server = viser.ViserServer()

# Create a helper for adding URDFs to Viser. This just adds meshes to the scene,
# helps us set the joint angles, etc.
urdf = ViserUrdf(server, urdf_path)

# Create joint angle sliders.
# Logic for updating the visualized robot.
gui_joints: list[viser.GuiInputHandle[float]] = []
initial_angles: list[float] = []
for joint_name, (lower, upper) in urdf.get_actuated_joint_limits().items():
lower = lower if lower is not None else -onp.pi
upper = upper if upper is not None else onp.pi

initial_angle = 0.0 if lower < 0 and upper > 0 else (lower + upper) / 2.0
slider = server.gui.add_slider(
label=joint_name,
min=lower,
max=upper,
step=1e-3,
initial_value=initial_angle,
)
slider.on_update( # When sliders move, we update the URDF configuration.
lambda _: urdf.update_cfg(onp.array([gui.value for gui in gui_joints]))

def update_robot_model(robot_name: str) -> None:
server.scene.reset()

loading_modal = server.gui.add_modal("Loading URDF...")
with loading_modal:
server.gui.add_markdown("See terminal for progress!")

# Create a helper for adding URDFs to Viser. This just adds meshes to the scene,
# helps us set the joint angles, etc.
urdf = ViserUrdf(
server,
# This can also be set to a path to a local URDF file.
urdf_or_path=load_robot_description(robot_name),
)
loading_modal.close()

for gui_joint in gui_joints:
gui_joint.remove()
gui_joints.clear()

gui_joints.append(slider)
initial_angles.append(initial_angle)
for joint_name, (lower, upper) in urdf.get_actuated_joint_limits().items():
lower = lower if lower is not None else -onp.pi
upper = upper if upper is not None else onp.pi

initial_angle = 0.0 if lower < 0 and upper > 0 else (lower + upper) / 2.0
slider = server.gui.add_slider(
label=joint_name,
min=lower,
max=upper,
step=1e-3,
initial_value=initial_angle,
)
slider.on_update( # When sliders move, we update the URDF configuration.
lambda _: urdf.update_cfg(onp.array([gui.value for gui in gui_joints]))
)

gui_joints.append(slider)
initial_angles.append(initial_angle)

# Apply initial joint angles.
urdf.update_cfg(onp.array([gui.value for gui in gui_joints]))

robot_model_name = server.gui.add_dropdown("Robot model", ROBOT_MODEL_LIST)
robot_model_name.on_update(lambda _: update_robot_model(robot_model_name.value))

# Create joint reset button.
reset_button = server.gui.add_button("Reset")
Expand All @@ -55,12 +81,68 @@ def _(_):
for g, initial_angle in zip(gui_joints, initial_angles):
g.value = initial_angle

# Apply initial joint angles.
urdf.update_cfg(onp.array([gui.value for gui in gui_joints]))

while True:
time.sleep(10.0)


ROBOT_MODEL_LIST = (
"edo_description",
"fanuc_m710ic_description",
"gen2_description",
"gen3_description",
"iiwa14_description",
"iiwa7_description",
"panda_description",
"poppy_ergo_jr_description",
"ur10_description",
"ur3_description",
"ur5_description",
"z1_description",
"bolt_description",
"cassie_description",
"rhea_description",
"spryped_description",
"upkie_description",
"baxter_description",
"nextage_description",
"poppy_torso_description",
"yumi_description",
"cf2_description",
"skydio_x2_description",
"double_pendulum_description",
"finger_edu_description",
"simple_humanoid_description",
"trifinger_edu_description",
"allegro_hand_description",
"barrett_hand_description",
"robotiq_2f85_description",
"atlas_drc_description",
"atlas_v4_description",
"draco3_description",
"ergocub_description",
"g1_description",
"h1_description",
"icub_description",
"jaxon_description",
"jvrc_description",
"r2_description",
"romeo_description",
"sigmaban_description",
"talos_description",
"valkyrie_description",
"a1_description",
"aliengo_description",
"anymal_b_description",
"anymal_c_description",
"b1_description",
"go1_description",
"go2_description",
"hyq_description",
"laikago_description",
"mini_cheetah_description",
"minitaur_description",
"solo_description",
)

if __name__ == "__main__":
tyro.cli(main)
1 change: 1 addition & 0 deletions pyproject.toml
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ examples = [
"torch>=1.13.1",
"matplotlib>=3.7.1",
"plotly>=5.21.0",
"robot_descriptions>=1.10.0",
]

[project.urls]
Expand Down
2 changes: 1 addition & 1 deletion src/viser/_gui_handles.py
Original file line number Diff line number Diff line change
Expand Up @@ -464,9 +464,9 @@ def close(self) -> None:
self._gui_api._websock_interface.queue_message(
GuiCloseModalMessage(self._id),
)
self._gui_api._container_handle_from_id.pop(self._id)
for child in tuple(self._children.values()):
child.remove()
self._gui_api._container_handle_from_id.pop(self._id)


@dataclasses.dataclass
Expand Down
Loading

0 comments on commit 7a0e687

Please sign in to comment.