meshcat-viewer-dae.py
Go to the documentation of this file.
1 # This examples shows how to load and move a robot in meshcat.
2 # Note: this feature requires Meshcat to be installed, this can be done using
3 # pip install --user meshcat
4 
5 import pinocchio as pin
6 import numpy as np
7 import sys
8 import os
9 from os.path import dirname, join, abspath
10 
11 from pinocchio.visualize import MeshcatVisualizer
12 
13 # Load the URDF model.
14 # Conversion with str seems to be necessary when executing this file with ipython
15 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
16 
17 model_path = join(pinocchio_model_dir, "example-robot-data/robots")
18 mesh_dir = pinocchio_model_dir
19 urdf_filename = "romeo_small.urdf"
20 urdf_model_path = join(join(model_path, "romeo_description/urdf"), urdf_filename)
21 
22 model, collision_model, visual_model = pin.buildModelsFromUrdf(
23  urdf_model_path, mesh_dir, pin.JointModelFreeFlyer()
24 )
25 
26 # Currently, MeshCat is not able to retrieve the scaling from DAE files. Set it manually.
27 for geom in visual_model.geometryObjects:
28  s = geom.meshScale
29  s *= 0.01
30  geom.meshScale = s
31 
32 
33 # Start a new MeshCat server and client.
34 # Note: the server can also be started separately using the "meshcat-server" command in a terminal:
35 # this enables the server to remain active after the current script ends.
36 #
37 # Option open=True pens the visualizer.
38 # Note: the visualizer can also be opened seperately by visiting the provided URL.
39 try:
40  viz = MeshcatVisualizer(model, collision_model, visual_model)
41  viz.initViewer(open=True)
42 except ImportError as err:
43  print(
44  "Error while initializing the viewer. It seems you should install Python meshcat"
45  )
46  print(err)
47  sys.exit(0)
48 
49 # Load the robot in the viewer.
50 # Color is needed here because the Romeo URDF doesn't contain any color, so the default color results in an
51 # invisible robot (alpha value set to 0).
52 viz.loadViewerModel(color=[0.0, 0.0, 0.0, 1.0])
53 
54 # Display a robot configuration.
55 q0 = np.array(
56  [
57  0,
58  0,
59  0.840252,
60  0,
61  0,
62  0,
63  1, # Free flyer
64  0,
65  0,
66  -0.3490658,
67  0.6981317,
68  -0.3490658,
69  0, # left leg
70  0,
71  0,
72  -0.3490658,
73  0.6981317,
74  -0.3490658,
75  0, # right leg
76  0, # chest
77  1.5,
78  0.6,
79  -0.5,
80  -1.05,
81  -0.4,
82  -0.3,
83  -0.2, # left arm
84  0,
85  0,
86  0,
87  0, # head
88  1.5,
89  -0.6,
90  0.5,
91  1.05,
92  -0.4,
93  -0.3,
94  -0.2, # right arm
95  ]
96 ).T
97 viz.display(q0)
98 
99 # Display another robot.
100 red_robot_viz = MeshcatVisualizer(model, collision_model, visual_model)
101 red_robot_viz.initViewer(viz.viewer)
102 red_robot_viz.loadViewerModel(rootNodeName="red_robot", color=[1.0, 0.0, 0.0, 0.5])
103 q = q0.copy()
104 q[1] = 1.0
105 red_robot_viz.display(q)
pinocchio.visualize
Definition: bindings/python/pinocchio/visualize/__init__.py:1
pinocchio.visualize.meshcat_visualizer.MeshcatVisualizer
Definition: meshcat_visualizer.py:532
path


pinocchio
Author(s):
autogenerated on Fri Jun 7 2024 02:40:48