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(urdf_model_path, mesh_dir, pin.JointModelFreeFlyer())
23 
24 # Currently, MeshCat is not able to retrieve the scaling from DAE files. Set it manually.
25 for geom in visual_model.geometryObjects:
26  s = geom.meshScale
27  s *= 0.01
28  geom.meshScale = s
29 
30 viz = MeshcatVisualizer(model, collision_model, visual_model)
31 
32 # Start a new MeshCat server and client.
33 # Note: the server can also be started separately using the "meshcat-server" command in a terminal:
34 # this enables the server to remain active after the current script ends.
35 #
36 # Option open=True pens the visualizer.
37 # Note: the visualizer can also be opened seperately by visiting the provided URL.
38 try:
39  viz.initViewer(open=True)
40 except ImportError as err:
41  print("Error while initializing the viewer. It seems you should install Python meshcat")
42  print(err)
43  sys.exit(0)
44 
45 # Load the robot in the viewer.
46 # Color is needed here because the Romeo URDF doesn't contain any color, so the default color results in an
47 # invisible robot (alpha value set to 0).
48 viz.loadViewerModel(color = [0.0, 0.0, 0.0, 1.0])
49 
50 # Display a robot configuration.
51 q0 = np.array([
52  0, 0, 0.840252, 0, 0, 0, 1, # Free flyer
53  0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # left leg
54  0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # right leg
55  0, # chest
56  1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, # left arm
57  0, 0, 0, 0, # head
58  1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2, # right arm
59 ]).T
60 viz.display(q0)
61 
62 # Display another robot.
63 red_robot_viz = MeshcatVisualizer(model, collision_model, visual_model)
64 red_robot_viz.initViewer(viz.viewer)
65 red_robot_viz.loadViewerModel(rootNodeName = "red_robot", color = [1.0, 0.0, 0.0, 0.5])
66 q = q0.copy()
67 q[1] = 1.0
68 red_robot_viz.display(q)
69 


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:32