def __init__(self, urdf_path, meshes_path):
self.model, self.collision_model, self.visual_model = pin.buildModelsFromUrdf(
urdf_path, meshes_path)
self.viz = MeshcatVisualizer(self.model, self.collision_model,
self.visual_model)
try:
self.viz.initViewer(open=False)
except ImportError as err:
print(err)
exit(0)
self.viz.loadViewerModel()
from pinocchio.visualize import MeshcatVisualizer
# Load the URDF model.
# Conversion with str seems to be necessary when executing this file with ipython
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
model_path = join(pinocchio_model_dir, "example-robot-data/robots")
mesh_dir = pinocchio_model_dir
urdf_filename = "talos_reduced.urdf"
urdf_model_path = join(join(model_path, "talos_data/robots"), urdf_filename)
model, collision_model, visual_model = pin.buildModelsFromUrdf(
urdf_model_path, mesh_dir, pin.JointModelFreeFlyer())
viz = MeshcatVisualizer(model, collision_model, visual_model)
# 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.
#
# Option open=True pens the visualizer.
# Note: the visualizer can also be opened seperately by visiting the provided URL.
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)
def __init__(self,
jiminy_model,
use_theoretical_model=False,
urdf_rgba=None,
robot_index=0,
backend=None,
window_name='python-pinocchio',
scene_name='world'):
# Backup some user arguments
self.urdf_path = jiminy_model.urdf_path
self.scene_name = scene_name
self.window_name = window_name
self.use_theoretical_model = use_theoretical_model
# Extract the right Pinocchio model
if self.use_theoretical_model:
self.pinocchio_model = jiminy_model.pinocchio_model_th
self.pinocchio_data = jiminy_model.pinocchio_data_th
else:
self.pinocchio_model = jiminy_model.pinocchio_model
self.pinocchio_data = jiminy_model.pinocchio_data
# Select the desired backend
if backend is None:
if Viewer.backend is None:
backend = 'meshcat' if Viewer._is_notebook() else 'gepetto-gui'
else:
backend = Viewer.backend
# Update the backend currently running, if any
if (Viewer.backend != backend) and \
(Viewer._backend_obj is not None or \
Viewer._backend_proc is not None):
Viewer.close()
print("Different backend already running. Closing it...")
Viewer.backend = backend
# Check if the backend is still available, if any
if Viewer._backend_obj is not None and Viewer._backend_proc is not None:
if Viewer._backend_proc.poll() is not None:
Viewer._backend_obj = None
Viewer._backend_proc = None
# Access the current backend or create one if none is available
try:
if (Viewer.backend == 'gepetto-gui'):
if Viewer._backend_obj is None:
Viewer._backend_obj, Viewer._backend_proc = \
Viewer._get_gepetto_client(True)
if Viewer._backend_obj is not None:
self._client = Viewer._backend_obj.gui
else:
raise RuntimeError("Impossible to open Gepetto-viewer.")
else:
from pinocchio.visualize import MeshcatVisualizer
from pinocchio.shortcuts import createDatas
if Viewer._backend_obj is None:
Viewer._create_meshcat_backend()
if Viewer._is_notebook():
Viewer.display_jupyter_cell()
else:
Viewer._backend_obj.open()
self._client = MeshcatVisualizer(self.pinocchio_model, None,
None)
self._client.viewer = Viewer._backend_obj
except:
raise RuntimeError("Impossible to load backend.")
# Create a RobotWrapper
robot_name = "robot_" + str(robot_index)
if (Viewer.backend == 'gepetto-gui'):
Viewer._delete_gepetto_nodes_viewer(scene_name + '/' + robot_name)
if (urdf_rgba is not None):
alpha = urdf_rgba[3]
self.urdf_path = Viewer._get_colorized_urdf(
self.urdf_path, urdf_rgba[:3])
else:
alpha = 1.0
collision_model = pin.buildGeomFromUrdf(
self.pinocchio_model, self.urdf_path,
os.environ.get('JIMINY_MESH_PATH', []), pin.GeometryType.COLLISION)
visual_model = pin.buildGeomFromUrdf(
self.pinocchio_model, self.urdf_path,
os.environ.get('JIMINY_MESH_PATH', []), pin.GeometryType.VISUAL)
self._rb = RobotWrapper(model=self.pinocchio_model,
collision_model=collision_model,
visual_model=visual_model)
if not self.use_theoretical_model:
self._rb.data = jiminy_model.pinocchio_data
self.pinocchio_data = self._rb.data
# Load robot in the backend viewer
if (Viewer.backend == 'gepetto-gui'):
if not scene_name in self._client.getSceneList():
self._client.createSceneWithFloor(scene_name)
if not window_name in self._client.getWindowList():
self._window_id = self._client.createWindow(window_name)
self._client.addSceneToWindow(scene_name, self._window_id)
self._client.createGroup(scene_name + '/' + scene_name)
self._client.addLandmark(scene_name + '/' + scene_name, 0.1)
else:
self._window_id = int(
np.where([
name == window_name
for name in self._client.getWindowList()
])[0][0])
self._rb.initViewer(windowName=window_name,
sceneName=scene_name,
loadModel=False)
self._rb.loadViewerModel(robot_name)
self._client.setFloatProperty(scene_name + '/' + robot_name,
'Transparency', 1 - alpha)
else:
self._client.collision_model = collision_model
self._client.visual_model = visual_model
self._client.data, self._client.collision_data, self._client.visual_data = \
createDatas(self.pinocchio_model, collision_model, visual_model)
self._client.loadViewerModel(rootNodeName=robot_name,
color=urdf_rgba)
self._rb.viz = self._client
from pinocchio.visualize import MeshcatVisualizer
# Load the URDF model.
# Conversion with str seems to be necessary when executing this file with ipython
current_path = str(os.path.dirname(os.path.abspath(__file__)))
model_path = str(
os.path.abspath(os.path.join(current_path, '../../models/romeo')))
mesh_dir = model_path
urdf_model_path = str(
os.path.abspath(
os.path.join(model_path, 'romeo_description/urdf/romeo_small.urdf')))
model, collision_model, visual_model = pin.buildModelsFromUrdf(
urdf_model_path, mesh_dir, pin.JointModelFreeFlyer())
display = MeshcatVisualizer(model, collision_model, visual_model)
# pin.setGeometryMeshScales(visual_model,0.01)
# 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.
#
# Option open=True pens the visualizer.
# Note: the visualizer can also be opened seperately by visiting the provided URL.
display.initViewer(open=True)
input("Press enter to continue")
# Load the robot in the viewer.
# 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).
def __init__(self,
robot,
use_theoretical_model=False,
mesh_root_path=None,
urdf_rgba=None,
lock=None,
backend=None,
robot_name="robot",
window_name='jiminy',
scene_name='world'):
"""
@brief Constructor.
@param robot The jiminy.Robot to display.
@param use_theoretical_model Whether to use the theoretical (rigid) model or the flexible model for this robot.
@param mesh_root_path Optional, path to the folder containing the URDF meshes.
@param urdf_rgba Color to use to display this robot (rgba).
- @param lock Custom threading.Lock
- Optional: Only required for parallel rendering using multiprocessing.
It is required since some backends does not support multiple
simultaneous connections (e.g. corbasever).
@param backend The name of the desired backend to use for rendering.
Optional, either 'gepetto-gui' or 'meshcat' ('panda3d' available soon).
@param robot_name Unique robot name, to identify each robot in the viewer.
@param scene_name Scene name, used only when gepetto-gui is used as backend.
@param window_name Window name, used only when gepetto-gui is used as backend.
Note that it is not allowed to be equal to the window name.
"""
# Backup some user arguments
self.urdf_path = robot.urdf_path
self.robot_name = robot_name
self.scene_name = scene_name
self.window_name = window_name
self.use_theoretical_model = use_theoretical_model
self._lock = lock if lock is not None else Viewer._lock
if self.scene_name == self.window_name:
raise ValueError(
"Please, choose a different name for the scene and the window."
)
# Extract the right Pinocchio model
if self.use_theoretical_model:
self.pinocchio_model = robot.pinocchio_model_th
self.pinocchio_data = robot.pinocchio_data_th
else:
self.pinocchio_model = robot.pinocchio_model
self.pinocchio_data = robot.pinocchio_data
# Select the desired backend
if backend is None:
if Viewer.backend is None:
if Viewer._is_notebook() or (not 'gepetto-gui'
in backends_available):
backend = 'meshcat'
else:
backend = 'gepetto-gui'
else:
backend = Viewer.backend
else:
if not backend in backends_available:
raise ValueError("%s backend not available." % backend)
# Update the backend currently running, if any
if (Viewer.backend != backend) and \
(Viewer._backend_obj is not None or \
Viewer._backend_proc is not None):
Viewer.close()
print("Different backend already running. Closing it...")
Viewer.backend = backend
# Check if the backend is still available, if any
if Viewer._backend_obj is not None and Viewer._backend_proc is not None:
is_backend_running = True
if Viewer._backend_proc.poll() is not None:
is_backend_running = False
if (Viewer.backend == 'gepetto-gui'):
from omniORB.CORBA import TRANSIENT as gepetto_server_error
try:
Viewer._backend_obj.gui.refresh()
except gepetto_server_error:
is_backend_running = False
if not is_backend_running:
Viewer._backend_obj = None
Viewer._backend_proc = None
Viewer._backend_exception = None
else:
is_backend_running = False
# Access the current backend or create one if none is available
try:
if (Viewer.backend == 'gepetto-gui'):
import omniORB
Viewer._backend_exception = omniORB.CORBA.COMM_FAILURE
if Viewer._backend_obj is None:
Viewer._backend_obj, Viewer._backend_proc = \
Viewer._get_gepetto_client(True)
is_backend_running = Viewer._backend_proc is None
if Viewer._backend_obj is not None:
self._client = Viewer._backend_obj.gui
else:
raise RuntimeError("Impossible to open Gepetto-viewer.")
if not scene_name in self._client.getSceneList():
self._client.createSceneWithFloor(scene_name)
if not window_name in self._client.getWindowList():
self._window_id = self._client.createWindow(window_name)
self._client.addSceneToWindow(scene_name, self._window_id)
self._client.createGroup(scene_name + '/' + scene_name)
self._client.addLandmark(scene_name + '/' + scene_name,
0.1)
else:
self._window_id = int(
np.where([
name == window_name
for name in self._client.getWindowList()
])[0][0])
# Set the default camera pose if the viewer is not running before
if not is_backend_running:
self.setCameraTransform(
translation=DEFAULT_CAMERA_XYZRPY_OFFSET_GEPETTO[:3],
rotation=DEFAULT_CAMERA_XYZRPY_OFFSET_GEPETTO[3:])
else:
from pinocchio.visualize import MeshcatVisualizer
from pinocchio.shortcuts import createDatas
if Viewer._backend_obj is None:
Viewer._create_meshcat_backend()
if Viewer._is_notebook():
Viewer.display_jupyter_cell()
else:
Viewer._backend_obj.open()
self._client = MeshcatVisualizer(self.pinocchio_model, None,
None)
self._client.viewer = Viewer._backend_obj
except:
raise RuntimeError("Impossible to load backend.")
# Create a RobotWrapper
root_path = mesh_root_path if mesh_root_path is not None else os.environ.get(
'JIMINY_MESH_PATH', [])
if (Viewer.backend == 'gepetto-gui'):
Viewer._delete_gepetto_nodes_viewer(scene_name + '/' +
self.robot_name)
if (urdf_rgba is not None):
alpha = urdf_rgba[3]
self.urdf_path = Viewer._get_colorized_urdf(
self.urdf_path, urdf_rgba[:3], root_path)
else:
alpha = 1.0
collision_model = pin.buildGeomFromUrdf(self.pinocchio_model,
self.urdf_path, root_path,
pin.GeometryType.COLLISION)
visual_model = pin.buildGeomFromUrdf(self.pinocchio_model,
self.urdf_path, root_path,
pin.GeometryType.VISUAL)
self._rb = RobotWrapper(model=self.pinocchio_model,
collision_model=collision_model,
visual_model=visual_model)
if not self.use_theoretical_model:
self._rb.data = robot.pinocchio_data
self.pinocchio_data = self._rb.data
# Load robot in the backend viewer
if (Viewer.backend == 'gepetto-gui'):
self._rb.initViewer(windowName=window_name,
sceneName=scene_name,
loadModel=False)
self._rb.loadViewerModel(self.robot_name)
self._client.setFloatProperty(scene_name + '/' + self.robot_name,
'Transparency', 1 - alpha)
else:
self._client.collision_model = collision_model
self._client.visual_model = visual_model
self._client.data, self._client.collision_data, self._client.visual_data = \
createDatas(self.pinocchio_model, collision_model, visual_model)
self._client.loadViewerModel(rootNodeName=self.robot_name,
color=urdf_rgba)
self._rb.viz = self._client
model_path = join(pinocchio_model_dir, "others/robots")
mesh_dir = model_path
urdf_filename = "romeo_small.urdf"
urdf_model_path = join(join(model_path, "romeo_description/urdf"),
urdf_filename)
model, collision_model, visual_model = pin.buildModelsFromUrdf(
urdf_model_path, mesh_dir, pin.JointModelFreeFlyer())
# Currently, MeshCat is not able to retrieve the scaling from DAE files. Set it manually.
for geom in visual_model.geometryObjects:
s = geom.meshScale
s *= 0.01
geom.meshScale = s
viz = MeshcatVisualizer(model, collision_model, visual_model)
# 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.
#
# Option open=True pens the visualizer.
# Note: the visualizer can also be opened seperately by visiting the provided URL.
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)
context = zmq.Context()
pnc_socket = context.socket(zmq.SUB)
pnc_socket.connect(addr)
pnc_socket.setsockopt_string(zmq.SUBSCRIBE, "")
# pj_context = zmq.Context()
# pj_socket = pj_context.socket(zmq.PUB)
# pj_socket.bind("tcp://*:9872")
data_saver = DataSaver()
if args.b_visualize:
model, collision_model, visual_model = pin.buildModelsFromUrdf(
"robot_model/draco/draco.urdf", "robot_model/draco",
pin.JointModelFreeFlyer())
viz = MeshcatVisualizer(model, collision_model, visual_model)
try:
viz.initViewer(open=True)
except ImportError as err:
print(
"Error while initializing the viewer. It seems you should install python meshcat"
)
print(err)
exit()
viz.loadViewerModel()
vis_q = pin.neutral(model)
icp_model, icp_collision_model, icp_visual_model = pin.buildModelsFromUrdf(
"robot_model/ground/sphere.urdf", "robot_model/ground",
pin.JointModelFreeFlyer())
icp_viz = MeshcatVisualizer(icp_model, icp_collision_model,