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


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:32