class Viewer:
backend = None
port_forwarding = None
_backend_obj = None
_backend_proc = None
## Unique threading.Lock for every simulation.
# It is required for parallel rendering since corbaserver does not support multiple connection simultaneously.
_lock = Lock()
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
@staticmethod
def _create_meshcat_backend():
import meshcat
from contextlib import redirect_stdout
with redirect_stdout(None):
Viewer._backend_obj = meshcat.Visualizer()
Viewer._backend_proc = Viewer._backend_obj.window.server_proc
@staticmethod
def reset_port_forwarding(port_forwarding=None):
Viewer.port_forwarding = port_forwarding
@staticmethod
def display_jupyter_cell(height=600,
width=900,
force_create_backend=False):
if Viewer.backend == 'meshcat' and Viewer._is_notebook():
from IPython.core.display import HTML, display as ipython_display
if Viewer._backend_obj is None:
if force_create_backend:
Viewer._create_meshcat_backend()
else:
raise ValueError(
"No meshcat backend available and 'force_create_backend' is set to False."
)
viewer_url = Viewer._backend_obj.url()
if Viewer.port_forwarding is not None:
url_port_pattern = '(?<=:)[0-9]+(?=/)'
port_localhost = int(
re.search(url_port_pattern, viewer_url).group())
if port_localhost in Viewer.port_forwarding.keys():
viewer_url = re.sub(
url_port_pattern,
str(Viewer.port_forwarding[port_localhost]),
viewer_url)
else:
print(
"Port forwarding defined but no port mapping associated with {port_localhost}."
)
jupyter_html = f'\n<div style="height: {height}px; width: {width}px; overflow-x: auto; overflow-y: hidden; resize: both">\
\n<iframe src="{viewer_url}" style="width: 100%; height: 100%; border: none">\
</iframe>\n</div>\n'
ipython_display(HTML(jupyter_html))
else:
raise ValueError(
"Display in a Jupyter cell is only available using 'meshcat' backend and within a Jupyter notebook."
)
@staticmethod
def close():
if Viewer._backend_proc is not None:
Viewer._backend_proc.terminate()
Viewer._backend_proc = None
Viewer._backend_obj = None
@staticmethod
def _is_notebook():
try:
shell = get_ipython().__class__.__name__
if shell == 'ZMQInteractiveShell':
return True # Jupyter notebook or qtconsole
elif shell == 'TerminalInteractiveShell':
return False # Terminal running IPython
else:
return False # Other type, if any
except NameError:
return False # Probably standard Python interpreter
@staticmethod
def _get_colorized_urdf(urdf_path, rgb, custom_mesh_search_path=None):
"""
@brief Generate a unique colorized URDF.
@remark Multiple identical URDF model of different colors can be
loaded in Gepetto-viewer this way.
@param[in] urdf_path Full path of the URDF file
@param[in] rgb RGB code defining the color of the model. It is the same for each link.
@return Full path of the colorized URDF file.
"""
color_string = "%.3f_%.3f_%.3f_1.0" % rgb
color_tag = "<color rgba=\"%.3f %.3f %.3f 1.0\"" % rgb # don't close tag with '>', in order to handle <color/> and <color></color>
colorized_tmp_path = os.path.join(
"/tmp", "colorized_urdf_rgba_" + color_string)
colorized_urdf_path = os.path.join(colorized_tmp_path,
os.path.basename(urdf_path))
if not os.path.exists(colorized_tmp_path):
os.makedirs(colorized_tmp_path)
with open(urdf_path, 'r') as urdf_file:
colorized_contents = urdf_file.read()
for mesh_fullpath in re.findall('<mesh filename="(.*)"',
colorized_contents):
colorized_mesh_fullpath = os.path.join(colorized_tmp_path,
mesh_fullpath[1:])
colorized_mesh_path = os.path.dirname(colorized_mesh_fullpath)
if not os.access(colorized_mesh_path, os.F_OK):
os.makedirs(colorized_mesh_path)
shutil.copy2(mesh_fullpath, colorized_mesh_fullpath)
colorized_contents = colorized_contents.replace(
'"' + mesh_fullpath + '"', '"' + colorized_mesh_fullpath + '"',
1)
colorized_contents = re.sub("<color rgba=\"[\d. ]*\"", color_tag,
colorized_contents)
with open(colorized_urdf_path, 'w') as colorized_urdf_file:
colorized_urdf_file.write(colorized_contents)
return colorized_urdf_path
@staticmethod
def _get_gepetto_client(open_if_needed=False):
"""
@brief Get a pointer to the running process of Gepetto-Viewer.
@details This method can be used to open a new process if necessary.
.
@param[in] open_if_needed Whether a new process must be opened if
no running process is found.
Optional: False by default
@return A pointer to the running Gepetto-viewer Client and its PID.
"""
try:
from gepetto.corbaserver.client import Client
return Client(), None
except:
try:
return Client(), None
except:
if (open_if_needed):
FNULL = open(os.devnull, 'w')
proc = subprocess.Popen(['gepetto-gui'],
shell=False,
stdout=FNULL,
stderr=FNULL)
time.sleep(1)
try:
return Client(), proc
except:
try:
return Client(), proc
except:
print("Impossible to open Gepetto-viewer")
return None, None
def _delete_gepetto_nodes_viewer(self, *nodes_path):
"""
@brief Delete a 'node' in Gepetto-viewer.
@remark Be careful, one must specify the full path of a node, including
all parent group, but without the window name, ie
'scene_name/robot_name' to delete the robot.
@param[in] nodes_path Full path of the node to delete
"""
if Viewer.backend == 'gepetto-gui':
for node_path in nodes_path:
if node_path in self._client.getNodeList():
self._client.deleteNode(node_path, True)
def _getViewerNodeName(self, geometry_object, geometry_type):
"""
@brief Get the full path of a node associated to a given geometry
object and geometry type.
@remark This is a hidden function that is not automatically imported
using 'from wdc_jiminy_py import *'. It is not intended to
be called manually.
@param[in] geometry_object Geometry object from which to get the node
@param[in] geometry_type Geometry type. It must be either
pin.GeometryType.VISUAL or pin.GeometryType.COLLISION
for display and collision, respectively.
@return Full path of the associated node.
"""
if geometry_type is pin.GeometryType.VISUAL:
return self._rb.viz.viewerVisualGroupName + '/' + geometry_object.name
elif geometry_type is pin.GeometryType.COLLISION:
return self._rb.viz.viewerCollisionGroupName + '/' + geometry_object.name
def _updateGeometryPlacements(self, visual=False):
"""
@brief Update the generalized position of a geometry object.
@remark This is a hidden function that is not automatically imported
using 'from wdc_jiminy_py import *'. It is not intended to
be called manually.
@param[in] visual Wether it is a visual or collision update
"""
if visual:
geom_model = self._rb.visual_model
geom_data = self._rb.visual_data
else:
geom_model = self._rb.collision_model
geom_data = self._rb.collision_data
pin.updateGeometryPlacements(self.pinocchio_model, self.pinocchio_data,
geom_model, geom_data)
def setCameraTransform(self, translation, rotation):
# translation : [Px, Py, Pz],
# rotation : [Roll, Pitch, Yaw]
R_pnc = rpyToMatrix(np.array(rotation))
if Viewer.backend == 'gepetto-gui':
T_pnc = np.array(translation)
T_R = SE3(R_pnc, T_pnc)
self._client.setCameraTransform(self._window_id,
se3ToXYZQUAT(T_R).tolist())
else:
import meshcat.transformations as tf
# Transformation of the camera object
T_meshcat = tf.translation_matrix(translation)
self._client.viewer[
"/Cameras/default/rotated/<object>"].set_transform(T_meshcat)
# Orientation of the camera object
Q_pnc = Quaternion(R_pnc).coeffs()
Q_meshcat = np.roll(Q_pnc, shift=1)
R_meshcat = tf.quaternion_matrix(Q_meshcat)
self._client.viewer["/Cameras/default"].set_transform(R_meshcat)
def captureFrame(self):
if Viewer.backend == 'gepetto-gui':
png_path = next(tempfile._get_candidate_names())
self._client.captureFrame(self._window_id, png_path)
rgb_array = np.array(Image.open(png_path))[:, :, :-1]
os.remove(png_path)
return rgb_array
else:
raise RuntimeError(
"Screen capture through Python only available using 'gepetto-gui' backend."
)
def refresh(self):
"""
@brief Refresh the configuration of Model in the viewer.
"""
if self.use_theoretical_model:
raise RuntimeError(
"'Refresh' method only available if 'use_theoretical_model'=False."
)
if Viewer.backend == 'gepetto-gui':
if self._rb.displayCollisions:
self._client.applyConfigurations(
[self._getViewerNodeName(collision, pin.GeometryType.COLLISION)
for collision in self._rb.collision_model.geometryObjects],
[pin.se3ToXYZQUATtuple(self._rb.collision_data.oMg[\
self._rb.collision_model.getGeometryId(collision.name)])
for collision in self._rb.collision_model.geometryObjects]
)
if self._rb.displayVisuals:
self._updateGeometryPlacements(visual=True)
self._client.applyConfigurations(
[self._getViewerNodeName(visual, pin.GeometryType.VISUAL)
for visual in self._rb.visual_model.geometryObjects],
[pin.se3ToXYZQUATtuple(self._rb.visual_data.oMg[\
self._rb.visual_model.getGeometryId(visual.name)])
for visual in self._rb.visual_model.geometryObjects]
)
self._client.refresh()
else:
self._updateGeometryPlacements(visual=True)
for visual in self._rb.visual_model.geometryObjects:
T = self._rb.visual_data.oMg[\
self._rb.visual_model.getGeometryId(visual.name)].homogeneous.A
self._client.viewer[\
self._getViewerNodeName(visual, pin.GeometryType.VISUAL)].set_transform(T)
def display(self, evolution_robot, speed_ratio, xyz_offset=None):
t = [s.t for s in evolution_robot]
i = 0
init_time = time.time()
while i < len(evolution_robot):
s = evolution_robot[i]
if (xyz_offset is not None):
q = s.q.copy(
) # Make sure to use a copy to avoid altering the original data
q[:3] += xyz_offset
else:
q = s.q
with Viewer._lock: # It is necessary to use Lock since corbaserver does not support multiple connection simultaneously.
self._rb.display(q)
t_simu = (time.time() - init_time) * speed_ratio
i = bisect_right(t, t_simu)
if t_simu < s.t:
time.sleep(s.t - t_simu)
class Viewer:
backend = None
port_forwarding = None
_backend_obj = None
_backend_exception = None
_backend_proc = None
_lock = Lock(
) # Unique threading.Lock for every simulations (in the same thread ONLY!)
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
@staticmethod
def _create_meshcat_backend():
import meshcat
from contextlib import redirect_stdout
with redirect_stdout(None):
Viewer._backend_obj = meshcat.Visualizer()
Viewer._backend_proc = Viewer._backend_obj.window.server_proc
@staticmethod
def reset_port_forwarding(port_forwarding=None):
Viewer.port_forwarding = port_forwarding
@staticmethod
def display_jupyter_cell(height=600,
width=900,
force_create_backend=False):
if Viewer.backend == 'meshcat' and Viewer._is_notebook():
from IPython.core.display import HTML, display as ipython_display
if Viewer._backend_obj is None:
if force_create_backend:
Viewer._create_meshcat_backend()
else:
raise ValueError(
"No meshcat backend available and 'force_create_backend' is set to False."
)
viewer_url = Viewer._backend_obj.url()
if Viewer.port_forwarding is not None:
url_port_pattern = '(?<=:)[0-9]+(?=/)'
port_localhost = int(
re.search(url_port_pattern, viewer_url).group())
if port_localhost in Viewer.port_forwarding.keys():
viewer_url = re.sub(
url_port_pattern,
str(Viewer.port_forwarding[port_localhost]),
viewer_url)
else:
print(
"Port forwarding defined but no port mapping associated with {port_localhost}."
)
jupyter_html = f'\n<div style="height: {height}px; width: {width}px; overflow-x: auto; overflow-y: hidden; resize: both">\
\n<iframe src="{viewer_url}" style="width: 100%; height: 100%; border: none">\
</iframe>\n</div>\n'
ipython_display(HTML(jupyter_html))
else:
raise ValueError(
"Display in a Jupyter cell is only available using 'meshcat' backend and within a Jupyter notebook."
)
@staticmethod
def close():
if Viewer._backend_proc is not None:
if Viewer._backend_proc.poll() is None:
Viewer._backend_proc.terminate()
Viewer._backend_proc = None
Viewer._backend_obj = None
Viewer._backend_exception = None
@staticmethod
def _is_notebook():
try:
shell = get_ipython().__class__.__name__
if shell == 'ZMQInteractiveShell':
return True # Jupyter notebook or qtconsole
elif shell == 'TerminalInteractiveShell':
return False # Terminal running IPython
else:
return False # Other type, if any
except NameError:
return False # Probably standard Python interpreter
@staticmethod
def _get_colorized_urdf(urdf_path, rgb, root_path=None):
"""
@brief Generate a unique colorized URDF.
@remark Multiple identical URDF model of different colors can be
loaded in Gepetto-viewer this way.
@param[in] urdf_path Full path of the URDF file
@param[in] rgb RGB code defining the color of the model. It is the same for each link.
@param[in] root_path Root path of the meshes.
@return Full path of the colorized URDF file.
"""
color_string = "%.3f_%.3f_%.3f_1.0" % rgb
color_tag = "<color rgba=\"%.3f %.3f %.3f 1.0\"" % rgb # don't close tag with '>', in order to handle <color/> and <color></color>
colorized_tmp_path = os.path.join(
tempfile.gettempdir(), "colorized_urdf_rgba_" + color_string)
colorized_urdf_path = os.path.join(colorized_tmp_path,
os.path.basename(urdf_path))
if not os.path.exists(colorized_tmp_path):
os.makedirs(colorized_tmp_path)
with open(urdf_path, 'r') as urdf_file:
colorized_contents = urdf_file.read()
for mesh_fullpath in re.findall('<mesh filename="(.*)"',
colorized_contents):
# Replace package path by root_path.
if mesh_fullpath.startswith('package://'):
mesh_fullpath = root_path + mesh_fullpath[len("package:/"):]
colorized_mesh_fullpath = os.path.join(colorized_tmp_path,
mesh_fullpath[1:])
colorized_mesh_path = os.path.dirname(colorized_mesh_fullpath)
if not os.access(colorized_mesh_path, os.F_OK):
os.makedirs(colorized_mesh_path)
shutil.copy2(mesh_fullpath, colorized_mesh_fullpath)
colorized_contents = colorized_contents.replace(
'"' + mesh_fullpath + '"', '"' + colorized_mesh_fullpath + '"',
1)
colorized_contents = re.sub("<color rgba=\"[\d. ]*\"", color_tag,
colorized_contents)
with open(colorized_urdf_path, 'w') as colorized_urdf_file:
colorized_urdf_file.write(colorized_contents)
return colorized_urdf_path
@staticmethod
def _get_gepetto_client(create_if_needed=False, create_timeout=2000):
"""
@brief Get a pointer to the running process of Gepetto-Viewer.
@details This method can be used to open a new process if necessary.
.
@param[in] create_if_needed Whether a new process must be created if
no running process is found.
Optional: False by default
@param[in] create_timeout Wait some millisecond before considering
creating new viewer as failed.
Optional: 1s by default
@return A pointer to the running Gepetto-viewer Client and its PID.
"""
from gepetto.corbaserver.client import Client as gepetto_client
try:
# Get the existing Gepetto client
client = gepetto_client()
# Try to fetch the list of scenes to make sure that the Gepetto client is responding
client.gui.getSceneList()
return client, None
except:
try:
client = gepetto_client()
client.gui.getSceneList()
return client, None
except:
if (create_if_needed):
FNULL = open(os.devnull, 'w')
proc = subprocess.Popen(
['/opt/openrobots/bin/gepetto-gui'],
shell=False,
stdout=FNULL,
stderr=FNULL)
for _ in range(max(2, int(
create_timeout /
200))): # Must try at least twice for robustness
time.sleep(0.2)
try:
return gepetto_client(), proc
except:
pass
print("Impossible to open Gepetto-viewer")
return None, None
def _delete_gepetto_nodes_viewer(self, *nodes_path):
"""
@brief Delete a 'node' in Gepetto-viewer.
@remark Be careful, one must specify the full path of a node, including
all parent group, but without the window name, ie
'scene_name/robot_name' to delete the robot.
@param[in] nodes_path Full path of the node to delete
"""
if Viewer.backend == 'gepetto-gui':
for node_path in nodes_path:
if node_path in self._client.getNodeList():
self._client.deleteNode(node_path, True)
def _getViewerNodeName(self, geometry_object, geometry_type):
"""
@brief Get the full path of a node associated with a given geometry
object and geometry type.
@remark This is a hidden function not intended to be called manually.
@param[in] geometry_object Geometry object from which to get the node
@param[in] geometry_type Geometry type. It must be either
pin.GeometryType.VISUAL or pin.GeometryType.COLLISION
for display and collision, respectively.
@return Full path of the associated node.
"""
if geometry_type is pin.GeometryType.VISUAL:
return self._rb.viz.viewerVisualGroupName + '/' + geometry_object.name
elif geometry_type is pin.GeometryType.COLLISION:
return self._rb.viz.viewerCollisionGroupName + '/' + geometry_object.name
def _updateGeometryPlacements(self, visual=False):
"""
@brief Update the generalized position of a geometry object.
@remark This is a hidden function not intended to be called manually.
@param[in] visual Wether it is a visual or collision update
"""
if visual:
geom_model = self._rb.visual_model
geom_data = self._rb.visual_data
else:
geom_model = self._rb.collision_model
geom_data = self._rb.collision_data
pin.updateGeometryPlacements(self.pinocchio_model, self.pinocchio_data,
geom_model, geom_data)
def setCameraTransform(self,
translation=np.zeros(3),
rotation=np.zeros(3),
relative=False):
# translation : [Px, Py, Pz],
# rotation : [Roll, Pitch, Yaw]
R_pnc = rpyToMatrix(np.array(rotation))
if Viewer.backend == 'gepetto-gui':
H_abs = SE3(R_pnc, np.array(translation))
if relative:
H_orig = XYZQUATToSe3(
self._client.getCameraTransform(self._window_id))
H_abs = H_abs * H_orig
self._client.setCameraTransform(self._window_id,
se3ToXYZQUAT(H_abs).tolist())
else:
if relative:
raise RuntimeError(
"'relative'=True not available with meshcat.")
import meshcat.transformations as tf
# Transformation of the camera object
T_meshcat = tf.translation_matrix(translation)
self._client.viewer[
"/Cameras/default/rotated/<object>"].set_transform(T_meshcat)
# Orientation of the camera object
Q_pnc = Quaternion(R_pnc).coeffs()
Q_meshcat = np.roll(Q_pnc, shift=1)
R_meshcat = tf.quaternion_matrix(Q_meshcat)
self._client.viewer["/Cameras/default"].set_transform(R_meshcat)
def captureFrame(self):
if Viewer.backend == 'gepetto-gui':
png_path = next(tempfile._get_candidate_names())
self._client.captureFrame(self._window_id, png_path)
rgb_array = np.array(Image.open(png_path))[:, :, :-1]
os.remove(png_path)
return rgb_array
else:
raise RuntimeError(
"Screen capture through Python only available using 'gepetto-gui' backend."
)
def refresh(self):
"""
@brief Refresh the configuration of Robot in the viewer.
"""
if self.use_theoretical_model:
raise RuntimeError(
"'Refresh' method only available if 'use_theoretical_model'=False."
)
if Viewer.backend == 'gepetto-gui':
with self._lock:
if self._rb.displayCollisions:
self._client.applyConfigurations(
[self._getViewerNodeName(collision, pin.GeometryType.COLLISION)
for collision in self._rb.collision_model.geometryObjects],
[pin.se3ToXYZQUATtuple(self._rb.collision_data.oMg[\
self._rb.collision_model.getGeometryId(collision.name)])
for collision in self._rb.collision_model.geometryObjects]
)
if self._rb.displayVisuals:
self._updateGeometryPlacements(visual=True)
self._client.applyConfigurations(
[self._getViewerNodeName(visual, pin.GeometryType.VISUAL)
for visual in self._rb.visual_model.geometryObjects],
[pin.se3ToXYZQUATtuple(self._rb.visual_data.oMg[\
self._rb.visual_model.getGeometryId(visual.name)])
for visual in self._rb.visual_model.geometryObjects]
)
self._client.refresh()
else:
self._updateGeometryPlacements(visual=True)
for visual in self._rb.visual_model.geometryObjects:
T = self._rb.visual_data.oMg[\
self._rb.visual_model.getGeometryId(visual.name)].homogeneous
self._client.viewer[\
self._getViewerNodeName(visual, pin.GeometryType.VISUAL)].set_transform(T)
def display(self, evolution_robot, replay_speed, xyz_offset=None):
t = [s.t for s in evolution_robot]
i = 0
init_time = time.time()
while i < len(evolution_robot):
s = evolution_robot[i]
if (xyz_offset is not None):
q = s.q.copy(
) # Make sure to use a copy to avoid altering the original data
q[:3] += xyz_offset
else:
q = s.q
try:
with self._lock:
self._rb.display(q)
except Viewer._backend_exception:
break
t_simu = (time.time() - init_time) * replay_speed
i = bisect_right(t, t_simu)
sleep(s.t - t_simu)