pinocchio  3.6.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
prm_display.py
1 import time
2 
3 from pinocchio.utils import se3ToXYZQUATtuple
4 
5 
6 def display_prm(robot, graph):
7  """
8  Take a graph object containing a list of configurations q and a dictionnary of graph
9  relations edge. Display the configurations by the correspond placement of the robot
10  end effector. Display the graph relation by vertices connecting the robot end
11  effector positions.
12  """
13 
14  gui = robot.viewer.gui
15 
16  try:
17  gui.deleteNode("world/prm", True)
18  except: # noqa: E722
19  pass
20  gui.createRoadmap(
21  "world/prm", [1.0, 0.2, 0.2, 0.8], 1e-2, 1e-2, [1.0, 0.2, 0.2, 0.8]
22  )
23 
24  for q in graph.q:
25  gui.addNodeToRoadmap("world/prm", se3ToXYZQUATtuple(robot.position(q, 6)))
26 
27  for parent, children in graph.children.items():
28  for child in children:
29  if child > parent:
30  q1, q2 = graph.q[parent], graph.q[child]
31  p1 = robot.position(q1, 6).translation.ravel().tolist()[0]
32  p2 = robot.position(q2, 6).translation.ravel().tolist()[0]
33  gui.addEdgeToRoadmap("world/prm", p1, p2)
34 
35  gui.refresh()
36 
37 
38 def display_path(robot, path, sleeptime=1e-2):
39  """
40  Display a path, i.e. a sequence of robot configuration, by moving the robot
41  to each list element.
42  """
43  for q in path:
44  robot.display(q)
45  time.sleep(sleeptime)