prm_display.py

import time
 
from pinocchio.utils import se3ToXYZQUATtuple
 
 
def display_prm(robot, graph):
    """
    Take a graph object containing a list of configurations q and a dictionnary of graph
    relations edge. Display the configurations by the correspond placement of the robot
    end effector. Display the graph relation by vertices connecting the robot end
    effector positions.
    """
 
    gui = robot.viewer.gui
 
    try:
        gui.deleteNode("world/prm", True)
    except:  # noqa: E722
        pass
    gui.createRoadmap(
        "world/prm", [1.0, 0.2, 0.2, 0.8], 1e-2, 1e-2, [1.0, 0.2, 0.2, 0.8]
    )
 
    for q in graph.q:
        gui.addNodeToRoadmap("world/prm", se3ToXYZQUATtuple(robot.position(q, 6)))
 
        for parent, children in graph.children.items():
            for child in children:
                if child > parent:
                    q1, q2 = graph.q[parent], graph.q[child]
                    p1 = robot.position(q1, 6).translation.ravel().tolist()[0]
                    p2 = robot.position(q2, 6).translation.ravel().tolist()[0]
                    gui.addEdgeToRoadmap("world/prm", p1, p2)
 
                    gui.refresh()
 
 
def display_path(robot, path, sleeptime=1e-2):
    """
    Display a path, i.e. a sequence of robot configuration, by moving the robot
    to each list element.
    """
    for q in path:
        robot.display(q)
        time.sleep(sleeptime)