publish_trajectory.py
Go to the documentation of this file.
1 from __future__ import print_function, division
2 from time import sleep
3 import matplotlib.pyplot as plt
4 import signal
5 
6 __all__ = ["sig_int_handler", "publish_pose", "publish_trajectory",
7  "publish_time_indexed_trajectory", "plot"]
8 
9 
10 def sig_int_handler(signal, frame):
11  raise KeyboardInterrupt
12 
13 
14 def publish_pose(q, problem, t=0.0):
15  problem.get_scene().update(q, t)
16  problem.get_scene().get_kinematic_tree().publish_frames()
17 
18 
19 def publish_trajectory(traj, T, problem, once=False):
20  if len(traj) == 0:
21  raise ValueError("Trajectory has zero elements")
22  signal.signal(signal.SIGINT, sig_int_handler)
23  print('Playing back trajectory ' + str(T) + 's')
24  dt = float(T) / float(len(traj))
25  t = 0
26  while True:
27  try:
28  publish_pose(traj[t], problem, float(t) * dt)
29  sleep(dt)
30  if t >= len(traj) - 1 and once:
31  return
32  t = (t + 1) % len(traj)
33  except KeyboardInterrupt:
34  return False
35 
36 
37 def publish_time_indexed_trajectory(traj, Ts, problem, once=False):
38  if len(traj) == 0:
39  raise ValueError("Trajectory has zero elements")
40  signal.signal(signal.SIGINT, sig_int_handler)
41  print('Playing back trajectory ' + str(len(Ts)) +
42  ' states in ' + str(Ts[len(Ts) - 1]))
43 
44  while True:
45  try:
46  for i in range(1, len(Ts) - 1):
47  publish_pose(traj[i], problem, Ts[i])
48  sleep(Ts[i] - Ts[i-1])
49  if once:
50  break
51  except KeyboardInterrupt:
52  return False
53  return True
54 
55 
56 def plot(solution, labels=None, yscale=None):
57  print('Plotting the solution')
58  plt.plot(solution, '.-')
59  if labels is not None:
60  plt.legend(labels)
61  if yscale is not None:
62  plt.yscale(yscale)
63  plt.show()
def publish_pose(q, problem, t=0.0)
def publish_trajectory(traj, T, problem, once=False)
def plot(solution, labels=None, yscale=None)
def publish_time_indexed_trajectory(traj, Ts, problem, once=False)
def sig_int_handler(signal, frame)


exotica_python
Author(s):
autogenerated on Mon Feb 22 2021 03:33:27