Exemple #1
0
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)
Exemple #2
0
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)