meshcat-viewer.py 2.56 KB
Newer Older
1
2
3
4
# This examples shows how to load and move a robot in meshcat.
# Note: this feature requires Meshcat to be installed, this can be done using
# pip install --user meshcat

Gabriele Buondonno's avatar
Gabriele Buondonno committed
5
import pinocchio as pin
6
pin.switchToNumpyMatrix()
7
import numpy as np
8
import sys
9
import os
10
11
12
13
14
15

try:
    # Python 2
    input = raw_input
except NameError:
    pass
16

17
from pinocchio.visualize import MeshcatVisualizer
18
19

# Load the URDF model.
Gabriele Buondonno's avatar
Gabriele Buondonno committed
20
# Conversion with str seems to be necessary when executing this file with ipython
21
current_path =  str(os.path.dirname(os.path.abspath(__file__)))
Justin Carpentier's avatar
Justin Carpentier committed
22
model_path = str(os.path.abspath(os.path.join(current_path, '../models/others/robots')))
23
24
mesh_dir = model_path
urdf_model_path = str(os.path.abspath(os.path.join(model_path, 'romeo_description/urdf/romeo_small.urdf')))
Gabriele Buondonno's avatar
Gabriele Buondonno committed
25

26
model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_model_path, mesh_dir, pin.JointModelFreeFlyer())
27
viz = MeshcatVisualizer(model, collision_model, visual_model)
28
# pin.setGeometryMeshScales(visual_model,0.01)
29

30
31
32
# Start a new MeshCat server and client.
# Note: the server can also be started separately using the "meshcat-server" command in a terminal:
# this enables the server to remain active after the current script ends.
33
34
#
# Option open=True pens the visualizer.
35
# Note: the visualizer can also be opened seperately by visiting the provided URL.
36
37
38
39
40
41
try:
    viz.initViewer(open=True)
except ImportError as err:
    print("Error while initializing the viewer. It seems you should install Python meshcat")
    print(err)
    sys.exit(0)
42

Gabriele Buondonno's avatar
Gabriele Buondonno committed
43
input("Press enter to continue")
44
45

# Load the robot in the viewer.
46
47
# Color is needed here because the Romeo URDF doesn't contain any color, so the default color results in an
# invisible robot (alpha value set to 0).
48
viz.loadViewerModel(color = [0.0, 0.0, 0.0, 1.0])
Gabriele Buondonno's avatar
Gabriele Buondonno committed
49
50
51
52
53
54
55
56
57
58
59

# Display a robot configuration.
q0 = np.matrix([
    0, 0, 0.840252, 0, 0, 0, 1,  # Free flyer
    0, 0, -0.3490658, 0.6981317, -0.3490658, 0,  # left leg
    0, 0, -0.3490658, 0.6981317, -0.3490658, 0,  # right leg
    0,  # chest
    1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2,  # left arm
    0, 0, 0, 0,  # head
    1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2,  # right arm
]).T
60
viz.display(q0)
Gabriele Buondonno's avatar
Gabriele Buondonno committed
61
62
63
64

input("Displaying a single robot configuration. Press enter to continue")

# Display another robot.
65
66
67
red_robot_viz = MeshcatVisualizer(model, collision_model, visual_model)
red_robot_viz.initViewer(viz.viewer)
red_robot_viz.loadViewerModel(rootNodeName = "red_robot", color = [1.0, 0.0, 0.0, 0.5])
Gabriele Buondonno's avatar
Gabriele Buondonno committed
68
q = q0.copy()
69
q[1] = 1.0
70
red_robot_viz.display(q)
71
input("Displaying a second robot with color red, semi-transparent. Press enter to exit")