tools.py
Go to the documentation of this file.
1 from __future__ import print_function, division
2 import numpy as np
3 from time import time
4 import matplotlib.pyplot as plt
5 from collections import OrderedDict
6 
7 __all__ = ["check_trajectory_continuous_time",
8  "check_whether_trajectory_is_collision_free_by_subsampling", "get_colliding_links"]
9 
10 def check_trajectory_continuous_time(scene, trajectory):
11  start_time = time()
12  all_good = True
13  robot_links = scene.get_collision_robot_links()
14  world_links = scene.get_collision_world_links()
15  for t in range(1, trajectory.shape[0]):
16  q_t_1 = trajectory[t-1,:]
17  q_t_2 = trajectory[t,:]
18  for r_l in robot_links:
19  for w_l in world_links:
20  scene.update(q_t_1)
21  T_r_l_1 = scene.fk(r_l)
22  T_w_l_1 = scene.fk(w_l)
23  scene.update(q_t_2)
24  T_r_l_2 = scene.fk(r_l)
25  T_w_l_2 = scene.fk(w_l)
26  p = scene.get_collision_scene().continuous_collision_check(r_l, T_r_l_1, T_r_l_2, w_l, T_w_l_1, T_w_l_2)
27  if p.in_collision:
28  print(t, p)
29  all_good = False
30  end_time = time()
31  print("Continuous-time collision verification took", end_time - start_time)
32  return all_good
33 
34 
35 def check_whether_trajectory_is_collision_free_by_subsampling(scene, trajectory, num_subsamples=10, debug=False):
36  '''
37  num_subsamples specifies how many steps are checked between two configurations.
38  Returns True if trajectory is collision-free, and False otherwise.
39 
40  TODO: Support setting time for Scene update.
41  '''
42  start_time = time()
43  trajectory_length = trajectory.shape[0]
44  for t in range(1, trajectory_length):
45  q_t_1 = trajectory[t-1, :]
46  q_t_2 = trajectory[t, :]
47  q_t_interpolation = np.linspace(q_t_1, q_t_2, num_subsamples)
48  for i in range(num_subsamples):
49  scene.update(q_t_interpolation[i, :])
50  if not scene.is_state_valid(True):
51  return False
52  end_time = time()
53  if debug:
54  print("Trajectory transition collision check took", end_time - start_time)
55  return True
56 
57 
58 def get_colliding_links(scene, margin=0.0, safe_distance=0.0, check_self_collision=True, debug=False):
59  robotLinks = scene.get_collision_robot_links()
60  world_links = scene.get_collision_world_links()
61  collisions = []
62  for r_l in robotLinks:
63  for w_l in world_links:
64  if scene.is_allowed_to_collide(r_l, w_l, True):
65  if not scene.is_collision_free(r_l, w_l, margin):
66  d=scene.get_collision_distance(r_l, w_l)
67  if abs(d[0].distance) > safe_distance:
68  collisions.append((r_l, w_l, d[0].distance))
69  if debug:
70  print(r_l,"-",w_l,"d=",d[0].distance)
71  if check_self_collision:
72  for w_l in robotLinks:
73  if w_l != r_l:
74  if scene.is_allowed_to_collide(r_l, w_l, True):
75  if not scene.is_collision_free(r_l, w_l, margin):
76  d=scene.get_collision_distance(r_l,w_l)
77  if abs(d[0].distance) > safe_distance:
78  collisions.append((r_l, w_l, d[0].distance))
79  if debug:
80  print(r_l,"-",w_l,d[0].distance)
81  return collisions
82 
83 
85  '''
86  Plots the task cost (task maps) over time given a problem.
87  '''
88  costs = OrderedDict()
89  for task_name in problem.cost.task_maps:
90  costs[task_name] = np.zeros((problem.T,))
91  for t in range(problem.T):
92  ydiff = problem.cost.get_task_error(task_name, t)
93  S = problem.cost.get_S(task_name, t)
94  cost = np.dot(np.dot(ydiff, S), ydiff.T)
95  costs[task_name][t] = cost
96  fig = plt.figure()
97  for task_name in costs:
98  plt.plot(costs[task_name], label=task_name)
99  plt.title('Task cost over time')
100  plt.legend()
101  plt.tight_layout()
102  plt.show()
def plot_task_cost_over_time(problem)
Definition: tools.py:84
def check_whether_trajectory_is_collision_free_by_subsampling(scene, trajectory, num_subsamples=10, debug=False)
Definition: tools.py:35
def check_trajectory_continuous_time(scene, trajectory)
Definition: tools.py:10
def get_colliding_links(scene, margin=0.0, safe_distance=0.0, check_self_collision=True, debug=False)
Definition: tools.py:58


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