|
| 1 | +#!/usr/bin/env python3 |
| 2 | +# -*- coding: utf-8 -*- |
| 3 | +# |
| 4 | +# SPDX-License-Identifier: Apache-2.0 |
| 5 | +# Copyright 2025 Inria |
| 6 | + |
| 7 | +""" |
| 8 | +Show all humanoid robot descriptions using Pinocchio and MeshCat. |
| 9 | +""" |
| 10 | + |
| 11 | +try: |
| 12 | + import pinocchio as pin |
| 13 | + from pinocchio.visualize import MeshcatVisualizer |
| 14 | +except ImportError as e: |
| 15 | + raise ImportError("Pinocchio not found, try `pip install pin`") from e |
| 16 | + |
| 17 | +from robot_descriptions import DESCRIPTIONS |
| 18 | +from robot_descriptions.loaders.pinocchio import load_robot_description |
| 19 | + |
| 20 | +GRID_SPACING = 2.0 # [m] |
| 21 | +GRID_SIZE = 5 |
| 22 | + |
| 23 | +if __name__ == "__main__": |
| 24 | + nb_humanoids = 0 |
| 25 | + viewer = None |
| 26 | + visualizer = MeshcatVisualizer() |
| 27 | + |
| 28 | + for name, description in DESCRIPTIONS.items(): |
| 29 | + if "humanoid" not in description.tags: |
| 30 | + continue |
| 31 | + if name == "talos_mj_description": |
| 32 | + # See https://github.com/stack-of-tasks/pinocchio/issues/2612 |
| 33 | + continue |
| 34 | + if name == "op3_mj_description": |
| 35 | + # See https://github.com/stack-of-tasks/pinocchio/issues/2614 |
| 36 | + continue |
| 37 | + robot = load_robot_description( |
| 38 | + name, |
| 39 | + root_joint=pin.JointModelFreeFlyer(), |
| 40 | + ) |
| 41 | + robot.setVisualizer(visualizer) |
| 42 | + if viewer is None: |
| 43 | + robot.initViewer(open=True) |
| 44 | + viewer = robot.viewer |
| 45 | + robot.loadViewerModel() |
| 46 | + row = nb_humanoids // GRID_SIZE |
| 47 | + column = nb_humanoids % GRID_SIZE |
| 48 | + nb_humanoids += 1 |
| 49 | + q = robot.q0.copy() |
| 50 | + q[0] = row * GRID_SPACING # [m] |
| 51 | + q[1] = column * GRID_SPACING # [m] |
| 52 | + robot.display(q) |
| 53 | + print(f"Displaying {name=}") |
| 54 | + input("Press Enter to close MeshCat and terminate... ") |
| 55 | + |
| 56 | + print(f"Displaying {nb_humanoids} humanoid robot descriptions") |
| 57 | + input("Press Enter to close MeshCat and terminate... ") |
0 commit comments