diff --git a/python/ur_simple_control/path_generation/starworlds b/python/ur_simple_control/path_generation/starworlds
deleted file mode 160000
index 2990b9ecdeb8d7dd421fc518b5a97b2a31d5e67e..0000000000000000000000000000000000000000
--- a/python/ur_simple_control/path_generation/starworlds
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 2990b9ecdeb8d7dd421fc518b5a97b2a31d5e67e
diff --git a/python/ur_simple_control/path_generation/starworlds/LICENSE b/python/ur_simple_control/path_generation/starworlds/LICENSE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/ur_simple_control/path_generation/starworlds/README.md b/python/ur_simple_control/path_generation/starworlds/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/ur_simple_control/path_generation/starworlds/obstacles/__init__.py b/python/ur_simple_control/path_generation/starworlds/obstacles/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..52cb0934227437bf233de1e81164eac4f85fa2fe
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/obstacles/__init__.py
@@ -0,0 +1,6 @@
+from .obstacle import Obstacle, Frame
+from .starshaped_obstacle import StarshapedObstacle
+from .ellipse import Ellipse
+from .polygon import Polygon
+from .starshaped_polygon import StarshapedPolygon
+from .starshaped_primitive_combination import StarshapedPrimitiveCombination
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworlds/obstacles/ellipse.py b/python/ur_simple_control/path_generation/starworlds/obstacles/ellipse.py
new file mode 100644
index 0000000000000000000000000000000000000000..17f216ea08bfd56748b3be73dbb0ffe8d9155481
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/obstacles/ellipse.py
@@ -0,0 +1,217 @@
+from obstacles import StarshapedObstacle, Frame
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+from utils import is_ccw, tic, toc
+import shapely
+
+
+class Ellipse(StarshapedObstacle):
+ def __init__(self, a, xr=None, n_pol=20, **kwargs):
+ self._a = np.array(a, float)
+ self._a2 = np.square(self._a)
+ self._n_pol = n_pol
+ # self._area = np.pi * self._a[0] * self._a[1]
+ if xr is None:
+ xr = np.zeros(2)
+ super().__init__(xr=xr, **kwargs)
+ self.enclosing_ball_diameter = max(2*self._a[0], 2*self._a[1])
+
+ def copy(self, id, name):
+ if (id == 'duplicate' or id == 'd'):
+ id = self.id()
+ return Ellipse(id=id, name=name, a=self._a, xr=self._xr, n_pol=self._n_pol, motion_model=self._motion_model)
+
+ def dilated_obstacle(self, padding, id="new", name=None):
+ cp = self.copy(id, name)
+ cp._a += padding
+ cp._a2 = np.square(cp._a)
+ cp.enclosing_ball_diameter = max(2 * cp._a[0], 2 * cp._a[1])
+ if self._polygon is not None:
+ cp._polygon = self._polygon.buffer(padding, cap_style=1, join_style=1)
+ cp._polygon_global_pose = None
+ cp._polygon_global = None
+ cp._kernel = cp._polygon
+ return cp
+
+ def init_plot(self, ax=None, show_reference=True, show_name=False, **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ # Default facecolor
+ if "fc" not in kwargs and "facecolor" not in kwargs:
+ kwargs["fc"] = 'lightgrey'
+ line_handles = []
+ # Boundary
+ line_handles += [patches.Ellipse(xy=[0, 0], width=2*self._a[0], height=2*self._a[1], angle=0, **kwargs)]
+ ax.add_patch(line_handles[-1])
+ # Reference point
+ line_handles += ax.plot(*self._xr, '+', color='k') if show_reference else [None]
+ # Name
+ line_handles += [ax.text(*self._xr, self._name)] if show_name else [None]
+ return line_handles, ax
+
+ def update_plot(self, line_handles, frame=Frame.GLOBAL):
+ pos, rot = self.pos(frame), self.rot(frame)
+ line_handles[0].center = pos
+ line_handles[0].angle = np.rad2deg(rot)
+ if line_handles[1] is not None:
+ line_handles[1].set_data(*self.xr(frame))
+ if line_handles[2] is not None:
+ line_handles[2].set_position(self.xr(frame))
+
+ def draw(self, frame=Frame.GLOBAL, **kwargs):
+ line_handles, ax = self.init_plot(**kwargs)
+ self.update_plot(line_handles, frame)
+ return line_handles, ax
+
+ def boundary_mapping(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+
+ intersect_obstacle = self.line_intersection([self._xr, self._xr+1.1*self.enclosing_ball_diameter*self.reference_direction(x_obstacle, Frame.OBSTACLE, Frame.OBSTACLE)],
+ input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
+ if not intersect_obstacle:
+ return None
+ if len(intersect_obstacle) == 1:
+ return self.transform(intersect_obstacle[0], Frame.OBSTACLE, output_frame)
+ else:
+ if np.linalg.norm(intersect_obstacle[0]-x_obstacle) < np.linalg.norm(intersect_obstacle[1]-x_obstacle):
+ return self.transform(intersect_obstacle[0], Frame.OBSTACLE, output_frame)
+ else:
+ return self.transform(intersect_obstacle[1], Frame.OBSTACLE, output_frame)
+
+ def normal(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL, x_is_boundary=False):
+ b_obstacle = self.transform(x, input_frame, Frame.OBSTACLE) if x_is_boundary else self.boundary_mapping(x, input_frame, Frame.OBSTACLE)
+ if b_obstacle is None:
+ return None
+ n_obstacle = np.array([self._a2[1] * b_obstacle[0], self._a2[0] * b_obstacle[1]])
+ n_obstacle = n_obstacle / np.linalg.norm(n_obstacle)
+ return self.rotate(n_obstacle, Frame.OBSTACLE, output_frame)
+
+ def point_location(self, x, input_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ loc = (x_obstacle[0] / self._a[0]) ** 2 + (x_obstacle[1] / self._a[1]) ** 2 - 1
+ return loc
+
+ def line_intersection(self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ # Transform line points to left/right points in obstacle ellipse coordinates
+ l0_obstacle = self.transform(line[0], input_frame, Frame.OBSTACLE)
+ l1_obstacle = self.transform(line[1], input_frame, Frame.OBSTACLE)
+ l_left_obstacle = l0_obstacle if l0_obstacle[0] < l1_obstacle[0] else l1_obstacle
+ l_right_obstacle = l1_obstacle if l0_obstacle[0] < l1_obstacle[0] else l0_obstacle
+
+
+ vertical_line = abs(l_right_obstacle[0]-l_left_obstacle[0]) < 1e-4
+ if not vertical_line:
+ # Line parameters
+ m = (l_right_obstacle[1] - l_left_obstacle[1]) / (l_right_obstacle[0] - l_left_obstacle[0])
+ c = l_left_obstacle[1] - m * l_left_obstacle[0]
+ vertical_line = abs(m) > 100
+
+ # Special case with vertical line
+ if vertical_line:
+ if l_right_obstacle[0] < -self._a[0] or l_right_obstacle[0] > self._a[0]:
+ return []
+
+ l_top_obstacle = l_right_obstacle if l_right_obstacle[1] > l_left_obstacle[1] else l_left_obstacle
+ l_bottom_obstacle = l_left_obstacle if l_right_obstacle[1] > l_left_obstacle[1] else l_right_obstacle
+ x_intersect_top_obstacle, x_intersect_bottom_obstacle = np.array([0, self._a[1]]), np.array([0, -self._a[1]])
+ x_intersect_top = self.transform(x_intersect_top_obstacle, Frame.OBSTACLE, output_frame)
+ x_intersect_bottom = self.transform(x_intersect_bottom_obstacle, Frame.OBSTACLE, output_frame)
+ if l_top_obstacle[1] >= self._a[1] and l_bottom_obstacle[1] <= -self._a[1]:
+ return [x_intersect_top, x_intersect_bottom]
+ elif l_top_obstacle[1] >= self._a[1] and l_bottom_obstacle[1] <= self._a[1]:
+ return [x_intersect_top]
+ elif l_top_obstacle[1] >= -self._a[1] and l_bottom_obstacle[1] <= -self._a[1]:
+ return [x_intersect_bottom]
+ else:
+ return []
+
+ # obstacle ellipse coefficients at intersection with line m*x+c
+ kx2 = self._a2[0] * m**2 + self._a2[1]
+ kx = 2 * self._a2[0] * m * c
+ k1 = self._a2[0] * (c**2 - self._a2[1])
+
+ # TODO: Stable fix for finding intersection
+ discriminant = self._a2[0] * m**2 + self._a2[1] - c**2
+ if discriminant < 0:
+ return []
+ elif discriminant == 0:
+ tmp_x = -kx / (2 * kx2)
+ x_intersect_obstacle = np.array([tmp_x, m * tmp_x + c])
+ return [self.transform(x_intersect_obstacle, Frame.OBSTACLE, output_frame)]
+ else:
+ in_sqrt = kx ** 2 / (4 * kx2 ** 2) - k1 / kx2
+ if np.isclose(in_sqrt, 0):
+ in_sqrt = 0
+ tmp_x = -kx / (2 * kx2) - np.sqrt(in_sqrt)
+ x_intersect_left_obstacle = np.array([tmp_x, m * tmp_x + c])
+ tmp_x = -kx / (2 * kx2) + np.sqrt(in_sqrt)
+ x_intersect_right_obstacle = np.array([tmp_x, m * tmp_x + c])
+ x_intersect_left = self.transform(x_intersect_left_obstacle, Frame.OBSTACLE, output_frame)
+ x_intersect_right = self.transform(x_intersect_right_obstacle, Frame.OBSTACLE, output_frame)
+
+ if l_right_obstacle[0] < x_intersect_left_obstacle[0] or x_intersect_right_obstacle[0] < l_left_obstacle[0] or \
+ x_intersect_left_obstacle[0] < l_left_obstacle[0] < l_right_obstacle[0] < x_intersect_right_obstacle[0]:
+ return []
+ elif x_intersect_left_obstacle[0] < l_left_obstacle[0]:
+ return [x_intersect_right]
+ elif l_right_obstacle[0] < x_intersect_right_obstacle[0]:
+ return [x_intersect_left]
+ else:
+ return [x_intersect_left, x_intersect_right]
+
+ def tangent_points(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ if not self.exterior_point(x_obstacle, Frame.OBSTACLE):
+ return []
+ px, py = x_obstacle
+
+ # Special case with vertical tangent
+ a2 = self._a**2
+ vertical_tangent = abs(a2[0] - px**2) < 1e-5
+ if vertical_tangent:
+ m2 = (py**2-a2[1]) / (2*px*py)
+ x2 = (px * m2 ** 2 - py * m2) / (a2[1] / a2[0] + m2 ** 2)
+ y2 = m2 * (x2 - px) + py
+ tp1_obstacle = np.array([px, 0])
+ tp2_obstacle = np.array([x2, y2])
+ else:
+ tmp = px**2 - a2[0]
+ c1 = (px * py) / tmp
+ c2 = (a2[1] - py**2) / tmp
+ tmp = np.sqrt(c1**2+c2)
+ m1, m2 = c1 + tmp, c1 - tmp
+ tmp = a2[1] / a2[0]
+ m1_sqr = m1**2
+ m2_sqr = m2**2
+ x1 = (px * m1_sqr - py * m1) / (tmp + m1_sqr)
+ x2 = (px * m2_sqr - py * m2) / (tmp + m2_sqr)
+ y1 = m1 * (x1 - px) + py
+ y2 = m2 * (x2 - px) + py
+ tp1_obstacle = np.array([x1, y1])
+ tp2_obstacle = np.array([x2, y2])
+
+ tp1 = self.transform(tp1_obstacle, Frame.OBSTACLE, output_frame)
+ tp2 = self.transform(tp2_obstacle, Frame.OBSTACLE, output_frame)
+
+ if is_ccw(x, tp1, tp2):
+ tp1, tp2 = tp2, tp1
+
+ return [tp1, tp2]
+
+ # def area(self):
+ # return self._area
+
+ # ------------ Private methods ------------ #
+ def _check_convexity(self):
+ self._is_convex = True
+
+ def _compute_kernel(self):
+ self._kernel = self._polygon
+
+ def _compute_polygon_representation(self):
+ # logprint(str(self) + ": " + str(self._polygon), 0)
+ t = np.linspace(0, 2 * np.pi, self._n_pol, endpoint=False)
+ a = self._a + 1e-3 # Add offset to adjust for polygon approximation
+ polygon = np.vstack((a[0] * np.cos(t), a[1] * np.sin(t))).T
+ self._polygon = shapely.geometry.Polygon(polygon)
diff --git a/python/ur_simple_control/path_generation/starworlds/obstacles/motion_model.py b/python/ur_simple_control/path_generation/starworlds/obstacles/motion_model.py
new file mode 100644
index 0000000000000000000000000000000000000000..64e4746494cc9c7b3f8d43459f8dfdff44807bf3
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/obstacles/motion_model.py
@@ -0,0 +1,119 @@
+import numpy as np
+from abc import ABC, abstractmethod
+
+
+class MotionModel(ABC):
+
+ def __init__(self, pos=None, rot=0):
+ self._pos = np.array([0., 0.]) if pos is None else np.array(pos, dtype='float64')
+ self._rot = rot
+ self._t = 0.
+
+ def move(self, obs_self, dt):
+ xy_vel = np.array(self.lin_vel())
+ rot_vel = self.rot_vel()
+ prev_pos, prev_rot = self._pos.copy(), self._rot
+ self._pos += xy_vel * dt
+ self._rot += rot_vel * dt
+ self._t += dt
+
+ def set_pos(self, pos):
+ self._pos = pos
+
+ def set_rot(self, rot):
+ self._rot = rot
+
+ def pos(self):
+ return self._pos
+
+ def rot(self):
+ return self._rot
+
+ @abstractmethod
+ def lin_vel(self):
+ pass
+
+ @abstractmethod
+ def rot_vel(self):
+ pass
+
+
+class Static(MotionModel):
+
+ def lin_vel(self):
+ return np.zeros(2)
+
+ def rot_vel(self):
+ return 0.
+
+
+class SinusVelocity(MotionModel):
+
+ # If cartesian_coords: (x1,x2) vel are Cartesian (x,y) vel.
+ # Else: (x1,x2) vel are linear and rotational (lin,ang) vel.
+ def __init__(self, pos=None, rot=0, cartesian_coords=True, x1_mag=0., x1_period=0, x2_mag=0., x2_period=0):
+ super().__init__(pos, rot)
+ self.cartesian_coords = cartesian_coords
+ self.x1_vel_mag = x1_mag
+ self.x1_vel_freq = 2 * np.pi / x1_period if x1_period else 0
+ self.x2_vel_mag = x2_mag
+ self.x2_vel_freq = 2 * np.pi / x2_period if x2_period else 0
+
+ def lin_vel(self):
+ if self.cartesian_coords:
+ return np.array([self.x1_vel_mag * np.cos(self.x1_vel_freq * self._t),
+ self.x2_vel_mag * np.cos(self.x2_vel_freq * self._t)])
+ else:
+ rot_mat = np.array([[np.cos(self._rot), -np.sin(self._rot)], [np.sin(self._rot), np.cos(self._rot)]])
+ return rot_mat.dot([self.x1_vel_mag * np.cos(self.x1_vel_freq * self._t), 0])
+
+ def rot_vel(self):
+ if self.cartesian_coords:
+ return 0.
+ else:
+ return np.array(self.x2_vel_mag * np.cos(self.x2_vel_freq * self._t))
+
+
+class Interval(MotionModel):
+
+ def __init__(self, init_pos, time_pos):
+ self.pos_point = np.array([init_pos] + [p for _, p in time_pos])
+ self.time_point = np.cumsum([0] + [t for t, _ in time_pos])
+ super().__init__(init_pos, 0)
+
+ def lin_vel(self):
+ if self._t > self.time_point[-1]:
+ vel_norm = np.linalg.norm((self.pos_point[-1] - self.pos_point[-2]) / (self.time_point[-1] - self.time_point[-2]))
+ dir = self.pos_point[-1] - self.pos()
+ if np.linalg.norm(dir) > vel_norm:
+ dir /= np.linalg.norm(dir)
+ return vel_norm * dir
+ idx = np.argmax(self._t < self.time_point)
+ return (self.pos_point[idx] - self.pos_point[idx-1]) / (self.time_point[idx] - self.time_point[idx-1])
+
+ def rot_vel(self):
+ return 0.
+
+
+class Waypoints(MotionModel):
+
+ def __init__(self, init_pos, waypoints, vel, wp_thresh=0.2):
+ self.waypoints = np.array([init_pos] + waypoints)
+ self.vel = vel
+ self._wp_idx = 0
+ self.wp_thresh = wp_thresh
+ super().__init__(init_pos, 0)
+
+ def lin_vel(self):
+ if not self._wp_idx < len(self.waypoints):
+ return np.zeros(2)
+ dir = self.waypoints[self._wp_idx] - self.pos()
+ wp_dist = np.linalg.norm(dir)
+ if wp_dist < self.wp_thresh:
+ self._wp_idx += 1
+ return self.lin_vel()
+ dir /= wp_dist
+ return self.vel * dir
+
+ def rot_vel(self):
+ return 0.
diff --git a/python/ur_simple_control/path_generation/starworlds/obstacles/obstacle.py b/python/ur_simple_control/path_generation/starworlds/obstacles/obstacle.py
new file mode 100644
index 0000000000000000000000000000000000000000..7789611537fe4a9621377a6521a8c1d6ae239f7d
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/obstacles/obstacle.py
@@ -0,0 +1,201 @@
+from abc import ABC, abstractmethod
+import numpy as np
+import shapely
+from shapely import affinity as sh_affinity
+from utils import affine_transform
+from copy import deepcopy
+from enum import Enum
+
+
+class Frame(Enum):
+ GLOBAL = 1
+ OBSTACLE = 2
+
+ class InvalidFrameError(Exception):
+ pass
+
+
+class Obstacle(ABC):
+ """ Abstract base class of obstacles
+ """
+ id_counter = 0
+
+ # obs_id <0: temp object, obs_id=0: new object, obs_id>0: existing object with id #obs_id
+ def __init__(self, motion_model=None, is_convex=None, is_starshaped=None, id='new', name=None, compute_polygon=False):
+ self._id = None
+ self._is_convex = is_convex
+ self._is_starshaped = is_starshaped
+ # Pose of local frame in global frame
+ self._motion_model = motion_model # if motion_model is not None else mm.Static([0., 0.], 0.)
+ # Initialize id and name
+ self._set_id_name(id, name)
+ self._polygon = None # Polygon in obstacle frame
+ self._polygon_global = None # Polygon in global frame if static obstacle
+ self._polygon_global_pose = None # Obstacle pose corresponding to global polygon
+ if compute_polygon:
+ self._compute_global_polygon_representation()
+
+ def __str__(self): return self._name
+
+ def copy(self, id='temporary', name=None):
+ ob = deepcopy(self)
+ if not (id == 'duplicate' or id == 'd'):
+ ob._set_id_name(id, name)
+ return ob
+
+ def pos(self, output_frame=Frame.GLOBAL):
+ if output_frame == Frame.OBSTACLE or self._motion_model is None:
+ return np.zeros(2)
+ if output_frame == Frame.GLOBAL:
+ return self._motion_model.pos()
+
+ def rot(self, output_frame=Frame.GLOBAL):
+ if output_frame == Frame.OBSTACLE or self._motion_model is None:
+ return 0.
+ if output_frame == Frame.GLOBAL:
+ return self._motion_model.rot()
+
+ def interior_point(self, x, input_frame=Frame.GLOBAL):
+ return True if self.point_location(x, input_frame) < 0 else False
+
+ def exterior_point(self, x, input_frame=Frame.GLOBAL):
+ return True if self.point_location(x, input_frame) > 0 else False
+
+ def boundary_point(self, x, input_frame=Frame.GLOBAL):
+ return np.isclose(self.point_location(x, input_frame), 0.)
+
+ def move(self, dt):
+ if self._motion_model is not None:
+ self._motion_model.move(self, dt)
+
+ def id(self): return self._id
+
+ def polygon(self, output_frame=Frame.GLOBAL):
+ if self._polygon is None:
+ self._compute_polygon_representation()
+ # self._compute_global_polygon_representation()
+ if output_frame == Frame.OBSTACLE or self._motion_model is None:
+ return self._polygon
+ elif output_frame == Frame.GLOBAL:
+ current_pose = [*self._motion_model.pos(), self._motion_model.rot()]
+ if not current_pose == self._polygon_global_pose:
+ self._polygon_global_pose = current_pose
+ c, s = np.cos(current_pose[2]), np.sin(current_pose[2])
+ trans_matrix = np.array([[c, -s, current_pose[0]], [s, c, current_pose[1]], [0, 0, 1]])
+ affinity_matrix = [trans_matrix[0, 0], trans_matrix[0, 1], trans_matrix[1, 0], trans_matrix[1, 1], trans_matrix[0, 2], trans_matrix[1, 2]]
+ self._polygon_global = sh_affinity.affine_transform(self._polygon, affinity_matrix)
+ return self._polygon_global
+ else:
+ raise Frame.InvalidFrameError
+
+ def intersects(self, other):
+ return self.polygon().intersects(other.polygon())
+
+ def transform(self, x, input_frame, output_frame):
+ if input_frame == output_frame or self._motion_model is None:
+ return x
+ elif input_frame == Frame.OBSTACLE and output_frame == Frame.GLOBAL:
+ return self._transform_obstacle2global(x)
+ elif input_frame == Frame.GLOBAL and output_frame == Frame.OBSTACLE:
+ return self._transform_global2obstacle(x)
+ else:
+ raise Frame.InvalidFrameError
+
+ def rotate(self, x, input_frame, output_frame):
+ if input_frame == output_frame or self._motion_model is None:
+ return x
+ elif input_frame == Frame.OBSTACLE and output_frame == Frame.GLOBAL:
+ return self._rotate_obstacle2global(x)
+ elif input_frame == Frame.GLOBAL and output_frame == Frame.OBSTACLE:
+ return self._rotate_global2obstacle(x)
+ else:
+ raise Frame.InvalidFrameError
+
+ def is_convex(self):
+ # Check if convexity already has been computed
+ if self._is_convex is None:
+ self._check_convexity()
+ return self._is_convex
+
+ def is_starshaped(self):
+ if self._is_starshaped is None:
+ if self.is_convex():
+ self._is_starshaped = True
+ else:
+ # TODO: Add check for starshapedness. Currently default to not starshaped
+ self._is_starshaped = False
+ return self._is_starshaped
+
+ def set_motion_model(self, motion_model):
+ self._motion_model = motion_model
+
+ # ------------ Private methods ------------ #
+ def _set_id_name(self, id, name=None):
+ if id == 'new' or id == 'n':
+ Obstacle.id_counter += 1
+ self._id = Obstacle.id_counter
+ elif id == 'temporary' or id == 'temp' or id == 't':
+ self._id = None
+ elif isinstance(id, int) and 0 < id <= Obstacle.id_counter:
+ self._id = id
+ else:
+ print("Invalid id '" + str(id) + "' in set_id. Create temporary obstacle.")
+ self._id = None
+ self._name = name if name else str(self._id)
+
+ def _rotate_obstacle2global(self, x_obstacle):
+ rot = self._motion_model.rot()
+ return affine_transform(x_obstacle, rotation=rot, translation=[0, 0])
+
+ def _rotate_global2obstacle(self, x_global):
+ rot = self._motion_model.rot()
+ return affine_transform(x_global, rotation=rot, translation=[0, 0], inverse=True)
+
+ def _transform_obstacle2global(self, x_obstacle):
+ pos, rot = (self._motion_model.pos(), self._motion_model.rot())
+ return affine_transform(x_obstacle, rotation=rot, translation=pos)
+
+ def _transform_global2obstacle(self, x_global):
+ pos, rot = (self._motion_model.pos(), self._motion_model.rot())
+ return affine_transform(x_global, rotation=rot, translation=pos, inverse=True)
+
+ def _compute_global_polygon_representation(self):
+ self._compute_polygon_representation()
+ if self._motion_model is None:
+ self._polygon_global = self._polygon
+ if self._motion_model.__class__.__name__ == 'Static':
+ pos, rot = (self._motion_model.pos(), self._motion_model.rot())
+ c, s = np.cos(rot), np.sin(rot)
+ trans_matrix = np.array([[c, -s, pos[0]], [s, c, pos[1]], [0, 0, 1]])
+ affinity_matrix = [trans_matrix[0, 0], trans_matrix[0, 1], trans_matrix[1, 0], trans_matrix[1, 1],
+ trans_matrix[0, 2], trans_matrix[1, 2]]
+ self._polygon_global = shapely.affinity.affine_transform(self._polygon, affinity_matrix)
+
+ # ------------ Abstract methods ------------ #
+ @abstractmethod
+ def draw(self):
+ pass
+
+ @abstractmethod
+ def point_location(self, x, input_frame=Frame.GLOBAL):
+ pass
+
+ @abstractmethod
+ def dilated_obstacle(self, padding, id="new", name=None):
+ pass
+
+ @abstractmethod
+ def line_intersection(self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ pass
+
+ @abstractmethod
+ def tangent_points(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ pass
+
+ @abstractmethod
+ def _check_convexity(self):
+ pass
+
+ @abstractmethod
+ def _compute_polygon_representation(self):
+ pass
diff --git a/python/ur_simple_control/path_generation/starworlds/obstacles/polygon.py b/python/ur_simple_control/path_generation/starworlds/obstacles/polygon.py
new file mode 100644
index 0000000000000000000000000000000000000000..d8ec63a62b891136497967a50f588d08dd1b8343
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/obstacles/polygon.py
@@ -0,0 +1,158 @@
+from obstacles import Obstacle, Frame
+from utils import is_cw, is_ccw, is_collinear, tic, toc
+import shapely
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+
+
+class Polygon(Obstacle):
+
+ def __init__(self, polygon, **kwargs):
+ super().__init__(**kwargs)
+ self._polygon = shapely.geometry.Polygon(polygon)
+ self._polygon = shapely.ops.orient(self._polygon)
+ self._pol_bounds = self._polygon.bounds
+ self._compute_global_polygon_representation()
+ self.vertices = np.array(self._polygon.exterior.coords[:-1])
+ self.circular_vertices = np.array(self._polygon.exterior.coords)
+
+ def init_plot(self, ax=None, show_name=False, **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ if "fc" not in kwargs and "facecolor" not in kwargs:
+ kwargs["fc"] = 'lightgrey'
+ if 'show_reference' in kwargs:
+ del kwargs['show_reference']
+ line_handles = []
+ # Boundary
+ line_handles += [patches.Polygon(np.random.rand(3, 2), **kwargs)]
+ ax.add_patch(line_handles[-1])
+ # Name
+ line_handles += [ax.text(0, 0, self._name)] if show_name else [None]
+ return line_handles, ax
+
+ def extreme_points(self, frame=Frame.GLOBAL):
+ vertices = np.asarray(self.polygon(frame).exterior.coords)[:-1, :]
+ return [vertices[i] for i in range(vertices.shape[0])]
+
+ def update_plot(self, line_handles, frame=Frame.GLOBAL):
+ polygon = self.polygon(frame)
+ boundary = np.vstack((polygon.exterior.xy[0], polygon.exterior.xy[1])).T
+ line_handles[0].set_xy(boundary)
+ if line_handles[1] is not None:
+ line_handles[1].set_position(self.pos(frame))
+
+ def draw(self, frame=Frame.GLOBAL, **kwargs):
+ line_handles, ax = self.init_plot(**kwargs)
+ self.update_plot(line_handles, frame)
+ return line_handles, ax
+
+ def dilated_obstacle(self, padding, id="new", name=None):
+ cp = self.copy(id, name)
+ cp._polygon = cp._polygon.buffer(padding, cap_style=1, join_style=1)
+ cp._pol_bounds = cp._polygon.bounds
+ cp.vertices = np.array(cp._polygon.exterior.coords[:-1])
+ cp.circular_vertices = np.array(cp._polygon.exterior.coords)
+ cp._polygon_global_pose = None
+ cp._polygon_global = None
+ return cp
+
+ def point_location(self, x, input_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ xmin, ymin, xmax, ymax = self._pol_bounds
+ if not (xmin < x_obstacle[0] < xmax and ymin < x_obstacle[1] < ymax):
+ return 1
+ x_sh = shapely.geometry.Point(x_obstacle)
+ if self._polygon.contains(x_sh):
+ return -1
+ if self._polygon.exterior.contains(x_sh):
+ return 0
+ return 1
+
+ def line_intersection(self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ l0_obstacle = self.transform(line[0], input_frame, Frame.OBSTACLE)
+ l1_obstacle = self.transform(line[1], input_frame, Frame.OBSTACLE)
+ intersection_points_shapely = shapely.geometry.LineString([l0_obstacle, l1_obstacle]).intersection(self._polygon.exterior)
+ if intersection_points_shapely.is_empty:
+ return []
+ if intersection_points_shapely.geom_type == 'Point':
+ intersection_points_obstacle = [np.array([intersection_points_shapely.x, intersection_points_shapely.y])]
+ elif intersection_points_shapely.geom_type == 'MultiPoint':
+ intersection_points_obstacle = [np.array([p.x, p.y]) for p in intersection_points_shapely.geoms]
+ elif intersection_points_shapely.geom_type == 'LineString':
+ intersection_points_obstacle = [np.array([ip[0], ip[1]]) for ip in intersection_points_shapely.coords]
+ elif intersection_points_shapely.geom_type == 'MultiLineString':
+ intersection_points_obstacle = [np.array([ip[0], ip[1]]) for line in intersection_points_shapely.geoms for ip in line.coords]
+ else:
+ print(intersection_points_shapely)
+ return [self.transform(ip, Frame.OBSTACLE, output_frame) for ip in intersection_points_obstacle]
+
+ def tangent_points(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ t0 = tic()
+ phi = np.arctan2(self.circular_vertices[:, 1] - x_obstacle[1], self.circular_vertices[:, 0] - x_obstacle[0])
+ phi[phi < 0] += 2 * np.pi
+ t1 = toc(t0)
+ t0 = tic()
+ phi_diff = np.diff(phi)
+ t2 = toc(t0)
+ t0 = tic()
+ phi_decrease_idcs = phi_diff > np.pi
+ phi_increase_idcs = phi_diff < -np.pi
+ t3 = toc(t0)
+ t0 = tic()
+ phi_decrease_idcs = np.flatnonzero(phi_decrease_idcs)
+ phi_increase_idcs = np.flatnonzero(phi_increase_idcs)
+ for i in phi_decrease_idcs:
+ phi[i+1:] -= 2*np.pi
+ for i in phi_increase_idcs:
+ phi[i+1:] += 2*np.pi
+ t4 = toc(t0)
+
+ t0 = tic()
+
+ i_min, i_max = np.argmin(phi), np.argmax(phi)
+
+ if abs(phi[0] - phi[-1]) > 0.00001:
+ # Interior point
+ return []
+ if (phi[i_max] - phi[i_min]) >= 2*np.pi:
+ # Blocked exterior point
+ return []
+ t5 = toc(t0)
+
+ t0 = tic()
+ tp1_obstacle = self.circular_vertices[i_max]
+ tp2_obstacle = self.circular_vertices[i_min]
+
+ tp1 = self.transform(tp1_obstacle, Frame.OBSTACLE, output_frame)
+ tp2 = self.transform(tp2_obstacle, Frame.OBSTACLE, output_frame)
+
+ tend = toc(t0)
+ # print(sum([t1*100,t2*100,t3*100,t4*100,t5*100,tend*100*0]))
+ return [tp1, tp2]
+
+ def area(self):
+ return self._polygon.area
+
+ # ------------ Private methods ------------ #
+ def _check_convexity(self):
+ v = np.asarray(self._polygon.exterior.coords)[:-1, :]
+ i = 0
+ N = v.shape[0]
+ # Make sure first vertice is not collinear
+ while is_collinear(v[i-1, :], v[i, :], v[(i+1) % N, :]):
+ i += 1
+ if i > N:
+ raise RuntimeError("Bad polygon shape. All vertices collinear")
+ # All vertices must be either cw or ccw when iterating through for convexity
+ if is_cw(v[i-1, :], v[i, :], v[i+1, :]):
+ self._is_convex = not any([is_ccw(v[j-1, :], v[j, :], v[(j+1) % N, :]) for j in range(v.shape[0])])
+ else:
+ self._is_convex = not any([is_cw(v[j-1, :], v[j, :], v[(j+1) % N, :]) for j in range(v.shape[0])])
+
+ # Not needed
+ def _compute_polygon_representation(self):
+ pass
diff --git a/python/ur_simple_control/path_generation/starworlds/obstacles/starshaped_obstacle.py b/python/ur_simple_control/path_generation/starworlds/obstacles/starshaped_obstacle.py
new file mode 100644
index 0000000000000000000000000000000000000000..122ab8f02c24f2b1c0d57754353c639f4d5a3fae
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/obstacles/starshaped_obstacle.py
@@ -0,0 +1,85 @@
+from abc import abstractmethod
+import numpy as np
+from obstacles import Obstacle, Frame
+from shapely import affinity as sh_affinity
+
+
+class StarshapedObstacle(Obstacle):
+
+ def __init__(self, xr, **kwargs):
+ super().__init__(is_starshaped=True, **kwargs)
+ self._xr = np.array(xr) # Reference point in obstacle frame
+ self._kernel = None
+ self._kernel_global = None # Kernel in global frame
+ self._kernel_global_pose = None # Obstacle pose corresponding to global kernel
+
+ def xr(self, output_frame=Frame.GLOBAL):
+ return self.transform(self._xr, Frame.OBSTACLE, output_frame)
+
+ def set_xr(self, xr, input_frame=Frame.OBSTACLE, safe_set=False):
+ new_xr = self.transform(xr, input_frame, Frame.OBSTACLE)
+ if safe_set:
+ k = self.kernel()
+ if not k.exterior_point(new_xr, Frame.OBSTACLE):
+ self._xr = new_xr
+ else:
+ self._xr = new_xr
+
+ def reference_direction(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ dir = self.transform(x, input_frame, output_frame) - self.transform(self._xr, Frame.OBSTACLE, output_frame)
+ if not np.any(dir):
+ print("reference_direction for xr is not defined")
+ return dir / np.linalg.norm(dir, axis=x.ndim - 1)
+
+ # interior point: <1. exterior point: >1. boundary point: 1.
+ def distance_function(self, x, input_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ dist_func = (np.linalg.norm(x_obstacle - self._xr, axis=x.ndim - 1) / (
+ np.linalg.norm(self.boundary_mapping(x_obstacle, input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
+ - self._xr, axis=x.ndim - 1))) ** 2
+ return dist_func
+
+ # interior point: <0. exterior point: >0. boundary point: 0.
+ def point_location(self, x, input_frame=Frame.GLOBAL):
+ return np.sign(self.distance_function(x, input_frame)-1.)
+
+ def kernel(self, output_frame=Frame.GLOBAL):
+ if self._kernel is None:
+ self._compute_kernel()
+ if output_frame == Frame.OBSTACLE or self._motion_model is None:
+ return self._kernel
+ elif output_frame == Frame.GLOBAL:
+ current_pose = [*self._motion_model.pos(), self._motion_model.rot()]
+ if not current_pose == self._kernel_global_pose:
+ self._kernel_global_pose = current_pose
+ c, s = np.cos(current_pose[2]), np.sin(current_pose[2])
+ trans_matrix = np.array([[c, -s, current_pose[0]], [s, c, current_pose[1]], [0, 0, 1]])
+ affinity_matrix = [trans_matrix[0, 0], trans_matrix[0, 1], trans_matrix[1, 0], trans_matrix[1, 1], trans_matrix[0, 2], trans_matrix[1, 2]]
+ self._kernel_global = sh_affinity.affine_transform(self._kernel, affinity_matrix)
+ return self._kernel_global
+ else:
+ raise Frame.InvalidFrameError
+
+ def vel_intertial_frame(self, x):
+ if self._motion_model is None:
+ return np.zeros(2)
+ omega = self._motion_model.rot_vel()
+ lin_vel_omega = np.cross(np.hstack(([0, 0], omega)), np.hstack((self.reference_direction(x), 0)))[:2]
+ return self._motion_model.lin_vel() + lin_vel_omega
+
+ # ------------ Abstract methods ------------ #
+ @abstractmethod
+ def normal(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL, x_is_boundary=False):
+ pass
+
+ @abstractmethod
+ def boundary_mapping(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ pass
+
+ # @abstractmethod
+ # def signed_boundary_distance(self, x, input_frame=Frame.GLOBAL):
+ # pass
+
+ @abstractmethod
+ def _compute_kernel(self):
+ pass
diff --git a/python/ur_simple_control/path_generation/starworlds/obstacles/starshaped_polygon.py b/python/ur_simple_control/path_generation/starworlds/obstacles/starshaped_polygon.py
new file mode 100644
index 0000000000000000000000000000000000000000..85eba3ce218671de85d625c612a6fa9494377efe
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/obstacles/starshaped_polygon.py
@@ -0,0 +1,866 @@
+from obstacles import Frame, StarshapedObstacle, Polygon
+import matplotlib.pyplot as plt
+import numpy as np
+import shapely
+from utils import is_cw, is_ccw, is_collinear, orientation_val, get_intersection, is_between, tic, toc
+
+
+class StarshapedPolygon(Polygon, StarshapedObstacle):
+
+ def __init__(self, polygon, xr=None, **kwargs):
+ super().__init__(polygon, xr=xr, **kwargs)
+ if xr is None:
+ self._compute_kernel()
+ if self._kernel.contains(self._kernel.centroid):
+ self._xr = np.array(self._kernel.centroid.coords[0])
+ else:
+ self._xr = np.array(self._kernel.representative_point().coords[0])
+ else:
+ self._xr = np.array(xr)
+ self.vertex_angles = None
+ self._update_vertex_angles()
+ self.enclosing_ball_diameter = self._polygon.bounds[2]-self._polygon.bounds[0] + self._polygon.bounds[3]-self._polygon.bounds[1]
+
+ def copy(self, id, name):
+ if (id == 'duplicate' or id == 'd'):
+ id = self.id()
+ return StarshapedPolygon(id=id, name=name, polygon=self._polygon, xr=self._xr, motion_model=self._motion_model)
+
+ # Note: Does not recompute the kernel
+ def dilated_obstacle(self, padding, id="new", name=None):
+ cp = self.copy(id, name)
+ cp._polygon = cp._polygon.buffer(padding, cap_style=1, join_style=1)
+ cp._pol_bounds = cp._polygon.bounds
+ cp.enclosing_ball_diameter = max(cp._polygon.bounds[2]-cp._polygon.bounds[0], cp._polygon.bounds[3]-cp._polygon.bounds[1])
+ cp.vertices = np.array(cp._polygon.exterior.coords[:-1])
+ cp.circular_vertices = np.array(cp._polygon.exterior.coords)
+ cp._polygon_global_pose = None
+ cp._polygon_global = None
+ cp._update_vertex_angles()
+ return cp
+
+ def distance_function(self, x, input_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ dist_center = np.linalg.norm(x_obstacle - self._xr, axis=x.ndim - 1)
+ local_radius = np.linalg.norm(self.boundary_mapping(x_obstacle, input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
+ - self._xr, axis=x.ndim - 1)
+ if dist_center < local_radius:
+ # Return proportional inside to have -> [0, 1]
+ return (dist_center / local_radius) ** 2
+ else:
+ return ((dist_center - local_radius) + 1) ** 2
+
+
+ def boundary_mapping(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ intersect_obstacle = self.line_intersection([self._xr, self._xr+1.1*self.enclosing_ball_diameter*self.reference_direction(x_obstacle, Frame.OBSTACLE, Frame.OBSTACLE)],
+ input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
+ if not intersect_obstacle:
+ return None
+ if len(intersect_obstacle) == 1:
+ return self.transform(intersect_obstacle[0], Frame.OBSTACLE, output_frame)
+ else:
+ if np.linalg.norm(intersect_obstacle[0]-x_obstacle) < np.linalg.norm(intersect_obstacle[1]-x_obstacle):
+ return self.transform(intersect_obstacle[0], Frame.OBSTACLE, output_frame)
+ else:
+ return self.transform(intersect_obstacle[1], Frame.OBSTACLE, output_frame)
+
+ def normal(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL, x_is_boundary=False, type='edge'):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ angle = np.arctan2(x_obstacle[1]-self._xr[1], x_obstacle[0]-self._xr[0])
+ v_idx = np.argmax(self.vertex_angles > angle)
+
+ if v_idx == self.vertices.shape[0]:
+ v_idx = 0
+
+ if type == 'edge':
+ n_obstacle = np.array([self.vertices[v_idx, 1] - self.vertices[v_idx - 1, 1], self.vertices[v_idx - 1, 0] - self.vertices[v_idx, 0]])
+ n_obstacle /= np.linalg.norm(n_obstacle)
+ elif type == 'weighted_edges':
+ edge_neighbors = [(self.vertices[(v_idx - 2 + i) % self.vertices.shape[0]], self.vertices[(v_idx - 1 + i) % self.vertices.shape[0]]) for i in range(3)]
+ edge_neighbors_normal = np.array([[e[1][1] - e[0][1],
+ e[0][0] - e[1][0]] for e in edge_neighbors])
+ edge_closest = [shapely.ops.nearest_points(shapely.geometry.LineString(e),
+ shapely.geometry.Point(x_obstacle))[0].coords[0] for e in edge_neighbors]
+
+ dist = [np.linalg.norm(np.array(e)-x_obstacle) for e in edge_closest]
+ w = np.array([1/(d+1e-6) for d in dist])
+ w /= sum(w)
+ n_obstacle = edge_neighbors_normal.T.dot(w)
+ return self.rotate(n_obstacle, Frame.OBSTACLE, output_frame)
+
+
+ #directional weighted mean (see Appendix A) of normal vectors of the surface tiles ni(ΞΎ), the weights wi,
+ # and with respect to the reference direction r
+ # n_vert = self.vertices.shape[0]
+ # r = self.reference_direction(x, input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
+ # B = np.array((r, (-r[1], r[0])))
+ # ws = np.zeros(n_vert)
+ # kappa = np.zeros(n_vert)
+ # p = 3
+ # vi_min = np.inf
+ #
+ # edge_r = np.zeros((n_vert, 2))
+ # _, ax = self.draw(frame=Frame.OBSTACLE)
+ # ax.plot(*x_obstacle, 'ko')
+ # for i in range(n_vert):
+ # # surface_i = np.array([self.vertices[v_idcs[i] - 1, :], self.vertices[v_idcs[i], :]])
+ # # edge_normal_i = np.array([-(surface_i[0, 1] - surface_i[1, 1]), surface_i[0, 0] - surface_i[1, 0]])
+ # edge_normal_i = np.array([self.vertices[i, 1] - self.vertices[i - 1, 1], self.vertices[i - 1, 0] - self.vertices[i, 0]])
+ # edge_normal_i /= np.linalg.norm(edge_normal_i)
+ # # rotated_edge_normal_i = Rpn.T.dot(edge_normal_i)
+ #
+ # pi = np.array(
+ # shapely.ops.nearest_points(shapely.geometry.LineString([self.vertices[i-1, :], self.vertices[i, :]]),
+ # shapely.geometry.Point(x_obstacle))[0].coords[0])
+ # # ax.quiver(*pi, *edge_normals[i, :], color='k')
+ # edge_r[i, :] = [np.mean([self.vertices[i, 0], self.vertices[i - 1, 0]]),
+ # np.mean([self.vertices[i, 1], self.vertices[i - 1, 1]])]
+ # vi = x_obstacle - pi
+ # ei = (vi - edge_normal_i.dot(vi)*edge_normal_i) * np.sign(vi.dot(x_obstacle-edge_r[i, :]))
+ # phi = np.arccos(ei.dot(vi) / (np.linalg.norm(ei) * np.linalg.norm(vi))) * np.sign(edge_normal_i.dot(vi))
+ #
+ # if phi > 0:
+ # ws[i] = 0
+ # else:
+ # ws[i] = (np.pi / phi) ** p - 1
+ #
+ # # if np.linalg.norm(vi) < vi_min:
+ # # ws = np.zeros(n_vert)
+ # # ws[i] = 1
+ # # vi_min = np.linalg.norm(vi)
+ #
+ #
+ #
+ # # if np.all(np.isclose(pi, x_obstacle)):
+ # # ws[i] = -1
+ # # else:
+ # # ws[i] = 1 / np.linalg.norm(x_obstacle - pi) ** 2
+ # rotated_edge_normal_i = B.T.dot(edge_normal_i)
+ #
+ # kappa[i] = 0 if rotated_edge_normal_i[0] == 1 else np.arccos(rotated_edge_normal_i[0]) * np.sign(
+ # rotated_edge_normal_i[1])
+ #
+ # # v_idx += 1
+ # # if np.any(ws < 0):
+ # # ws[np.nonzero(ws > 0)] = 0
+ # # ws[np.nonzero(ws)] = 1
+ # #
+ # ws = ws / np.sum(ws)
+ # # n_obstacle2 =
+ #
+ # kappa_bar = ws.dot(kappa)
+ # tmp_vec = [np.cos(abs(kappa_bar)), np.sin(abs(kappa_bar)) * np.sign(kappa_bar)]
+ # n_obstacle = B.dot(tmp_vec)
+ #
+ # return self.rotate(n_obstacle, Frame.OBSTACLE, output_frame)
+
+ def set_xr(self, xr, input_frame=Frame.OBSTACLE, safe_set=False):
+ super().set_xr(xr, input_frame, safe_set)
+ self._update_vertex_angles()
+
+ def init_plot(self, ax=None, show_reference=True, show_name=False, **kwargs):
+ line_handles, ax = super().init_plot(ax=ax, show_name=show_name, **kwargs)
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ # Reference point
+ line_handles += ax.plot(0, 0, '+', color='k') if show_reference else [None]
+ return line_handles, ax
+
+ def update_plot(self, line_handles, frame=Frame.GLOBAL):
+ super().update_plot(line_handles, frame)
+ if line_handles[1] is not None:
+ line_handles[1].set_position(self.xr(frame))
+ if line_handles[2] is not None:
+ line_handles[2].set_data(*self.xr(frame))
+
+ def _update_vertex_angles(self):
+ self.vertex_angles = np.arctan2(self.vertices[:, 1] - self._xr[1], self.vertices[:, 0] - self._xr[0])
+ idcs = np.argsort(self.vertex_angles)
+ self.vertex_angles = self.vertex_angles[idcs]
+ self.vertices = self.vertices[idcs, :]
+ self.circular_vertices = np.vstack((self.vertices, self.vertices[0, :]))
+ self.vertex_angles = np.hstack((self.vertex_angles, self.vertex_angles[0] + 2 * np.pi))
+
+ def _compute_kernel(self, verbose=False, debug=False):
+ if self.is_convex():
+ self._kernel = self._polygon
+ return
+
+ # Returns true if vertex v[i, :] is reflex
+ def is_reflex(i):
+ return is_cw(v[i - 1, :], v[i, :], v[(i + 1) % v.shape[0], :])
+
+ # Show polygon
+ def draw(xk, F1_idx, L1_idx, i, xk_bounded):
+ v_ext = np.vstack((v, v[0, :]))
+ xk_ext = np.vstack((xk, xk[0, :])) if xk_bounded else xk
+ plt.plot(v_ext[:, 0], v_ext[:, 1], label='v', marker='.')
+ # plt.plot(v_ext[:, 0], v_ext[:, 1], label='v')
+ # plt.text(v_ext[0, 0], v_ext[0, 1], 'v0')
+ plt.text(xk[0, 0], xk[0, 1], 'xk0')
+ axes = plt.gca()
+ xlim, ylim = axes.get_xlim(), axes.get_ylim()
+ plt.plot(xk_ext[:, 0], xk_ext[:, 1], label='xk')
+ plt.plot(xk[F1_idx, 0], xk[F1_idx, 1], marker='o', c='c', label='F1')
+ end_point = v[i, :] + INF_VAL * (xk[F1_idx, :] - v[i, :]) / np.linalg.norm(xk[F1_idx, :] - v[i, :])
+ plt.plot([v[i, 0], end_point[0]], [v[i, 1], end_point[1]], 'c--')
+ plt.plot(xk[L1_idx, 0], xk[L1_idx, 1], marker='o', c='b', label='L1')
+ end_point = v[i, :] + INF_VAL * (xk[L1_idx, :] - v[i, :]) / np.linalg.norm(xk[L1_idx, :] - v[i, :])
+ plt.plot([v[i, 0], end_point[0]], [v[i, 1], end_point[1]], 'b--')
+ plt.plot(v[i, 0], v[i, 1], marker='x', c='r', ms=12, label='v{}'.format(i))
+ axes.set_xlim(xlim)
+ axes.set_ylim(ylim)
+
+ def point_left_of_line(point, line_head, line_tail):
+ return is_ccw(line_head, line_tail, point)
+
+ def point_right_of_line(point, line_head, line_tail):
+ return is_cw(line_head, line_tail, point)
+
+ pol = shapely.ops.orient(self._polygon)
+ v = np.asarray(pol.exterior.coords)[:-1, :]
+
+ # Remove points in line
+ v_idcs = np.arange(v.shape[0])
+ for i in range(v.shape[0]):
+ if is_collinear(v[v_idcs[i - 2], :], v[v_idcs[i - 1], :], v[v_idcs[i], :]):
+ v_idcs[i - 1] = v_idcs[i - 2]
+ v_idcs = np.unique(v_idcs)
+ v = v[v_idcs, :]
+ if is_collinear(v[-2, :], v[-1, :], v[0, :]):
+ v = v[:-1]
+
+ N = v.shape[0]
+ for i in range(N):
+ # Order v to have v[:, 0] as reflex vertex
+ if is_reflex(i):
+ v = np.vstack((v[i:, :], v[:i, :]))
+ break
+
+ INF_VAL = 300.
+ # Initial step
+ F1 = v[0, :] + INF_VAL * (v[0, :] - v[1, :]) / np.linalg.norm(v[0, :] - v[1, :])
+ L1 = v[0, :] + INF_VAL * (v[0, :] - v[-1, :]) / np.linalg.norm(v[0, :] - v[-1, :])
+ xk = np.vstack((F1, v[0, :], L1))
+ F1_idx = 0 # Index of point F1 in xk
+ L1_idx = 2 # Index of point L1 in xk
+ xk_bounded = False
+
+ if verbose:
+ print("------")
+ print(0)
+ print("F1:", end=" ")
+ print(F1)
+ print("L1:", end=" ")
+ print(L1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, 0, xk_bounded)
+ plt.show()
+
+ for i in range(1, N):
+
+ if verbose:
+ print("------")
+ print(i)
+ print("F1 index: {}".format(F1_idx))
+ print("L1 index: {}".format(L1_idx))
+
+ L1 = xk[L1_idx, :]
+ F1 = xk[F1_idx, :]
+ vi = v[i, :]
+ vi_1 = v[(i + 1) % N, :]
+
+ # (1) vi is reflex
+ if is_reflex(i):
+ # (1.2) F1 lies to the left of line ->(vi->vi+1)->vi+1
+ start_point = vi - INF_VAL * (vi_1 - vi) / np.linalg.norm(vi_1 - vi)
+ if point_left_of_line(F1, start_point, vi_1):
+ case = 2
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*', color='y',
+ label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title("F1 lies to the left of line ->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.show()
+ if verbose:
+ print("(1.2) - F1 ({},{}) lies to the left of line ->(v{}->v{})->v{}".format(F1[0], F1[0], i,
+ i + 1, i + 1))
+ # (1.1) F1 lies on or to the right of line ->(vi->vi+1)->vi+1
+ else:
+ case = 1
+ if verbose:
+ print(
+ "(1.1) - F1 ({},{}) lies on or to the right of line ->(v{}->v{})->v{}".format(F1[0], F1[0],
+ i, i + 1,
+ i + 1))
+ print("Scan xk ccw from F1 until we reach edge intersecting line ->(v{}->v{})->v{}".format(i,
+ i + 1,
+ i + 1))
+ # Scan xk ccw from F1 to L1 until we reach edge intersecting line ->(vi->vi+1)->vi+1
+ w1 = None
+ idx_offsets = range(1, xk.shape[0] + 1)
+ for off in idx_offsets:
+ t = (F1_idx + off) % xk.shape[0]
+ if not xk_bounded and t == 0:
+ break
+ # Get intersection w1 of current edge and line ->(vi->vi+1)->vi+1
+ wt_prev = xk[t - 1, :]
+ wt = xk[t, :]
+ w1 = get_intersection(wt_prev, wt, start_point, vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ col = 'r' if w1 is None else 'g'
+ plt.plot([wt_prev[0], wt[0]], [wt_prev[1], wt[1]], marker='*', color=col, label="edge")
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*', color='y',
+ label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title(
+ "Scan xk ccw from F1 until we reach edge intersecting line ->(v{}->v{})->v{}".format(i,
+ i + 1,
+ i + 1))
+ if w1 is not None:
+ if debug:
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ plt.legend()
+ plt.show()
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+
+ if t == L1_idx:
+ break
+ # If no intersecting line is reached no kernel exists
+ if w1 is None:
+ # if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.title('No kernel found.. Polygon not starshaped.')
+ plt.legend()
+ plt.show()
+ return False
+ if verbose:
+ print("Found intersection ({},{}) at line ->(v{}->v{})->v{}".format(w1[0], w1[1], i, i + 1,
+ i + 1))
+ print("Scan xk cw from F1 until we reach edge intersecting line ->(v{}->v{})->v{}".format(i,
+ i + 1,
+ i + 1))
+ # Scan xk cw from F1 until we reach edge intersecting line ->(vi->vi+1)->vi+1
+ w2 = None
+ idcs_list = np.flip(np.roll(np.arange(xk.shape[0]), -F1_idx - 1)) if xk_bounded else range(F1_idx,
+ 0, -1)
+ # for s in range(F1_idx, 0, -1):
+ for s in idcs_list:
+ ws = xk[s, :]
+ ws_prev = xk[s - 1, :]
+ # Get intersection w2 of edge and line ->(vi->vi+1)->vi+1
+ w2 = get_intersection(ws_prev, ws, start_point, vi_1)
+
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ col = 'r' if w2 is None else 'g'
+ plt.plot([ws_prev[0], ws[0]], [ws_prev[1], ws[1]], marker='*', color=col, label="edge")
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*', color='y',
+ label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title(
+ "Scan xk cw from F1 until we reach edge intersecting line ->(v{}->v{})->v{}".format(i,
+ i + 1,
+ i + 1))
+
+ if w2 is not None:
+ if xk_bounded and s > t:
+ alpha = xk[xk.shape[0]:, :] # Empty array
+ beta = xk[t:s, :]
+ L1_idx = L1_idx - t + 2
+ else:
+ alpha = xk[:s, :]
+ beta = xk[t:, :]
+ L1_idx = L1_idx - t + s + 2
+ if debug:
+ print("xk: ", end=" ")
+ print(xk)
+ print("alpha: ", end=" ")
+ print(alpha)
+ print("beta: ", end=" ")
+ print(beta)
+ print("w1: ", end=" ")
+ print(w1)
+ print("w2: ", end=" ")
+ print(w2)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='k')
+ plt.plot(w2[0], w2[1], 's', label="w2", color='g')
+ plt.plot(alpha[:, 0], alpha[:, 1], 'r--', label="alpha")
+ plt.plot(beta[:, 0], beta[:, 1], 'k--', label="beta")
+ plt.legend()
+ plt.show()
+ xk = np.vstack((alpha, w2, w1, beta))
+ w1_idx = alpha.shape[0] + 1
+ w2_idx = alpha.shape[0]
+ ####### F1 index reassignment should not be necessary for this case
+ F1_idx = w2_idx
+ # L1_idx = L1_idx - t + s + 2
+
+ if verbose:
+ print("Update 1")
+ print("Found intersection ({},{}) at line ->(v{}->v{})->v{}".format(w2[0], w2[1], i,
+ i + 1, i + 1))
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+ # If no intersecting line is reached
+ if w2 is None:
+ # Test if xk+1 is bounded
+ # If slope ->(vi->vi+1)->vi+1 is comprised between the slopes of initial and final half lines of xk,
+ if (orientation_val(xk[-2, :], xk[-1, :], start_point) * orientation_val(vi_1, start_point,
+ xk[0, :])) > 0:
+ beta = xk[t:, :]
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ plt.plot(xk[:2, 0], xk[:2, 1], '--c', label="initial half line")
+ plt.plot(xk[-2:, 0], xk[-2:, 1], '--m', label="final half line")
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*', color='y',
+ label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title(
+ "->(v{}->v{})->v{} between initial and finial half lines of xk".format(
+ i, i + 1, i + 1))
+ plt.plot(beta[:, 0], beta[:, 1], 'k--', label="beta")
+ plt.legend()
+ plt.show()
+ if verbose:
+ print("Update 2 - xk still unbounded")
+ # then xk+1= ->(vi->vi+1)->w1->beta is also unbounded.
+ xk = np.vstack((start_point, w1, beta))
+ w1_idx = 1
+ # xk_bounded = False
+ F1_idx = 0
+ L1_idx -= t - 1
+
+
+ else:
+ # otherwise scan xk cw from xk[-1,:] until we reach edge intersecting line ->(vi->vi+1)->vi+1
+ if verbose:
+ print(
+ "Scan xk cw from end until we reach edge intersecting line ->(v{}->v{})->v{}".format(
+ i, i + 1, i + 1))
+ w2 = None
+ if xk_bounded:
+ r = xk.shape[0]
+ w2 = get_intersection(xk[0, :], xk[r - 1, :], start_point, vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ col = 'r' if w2 is None else 'g'
+ plt.plot([xk[r - 1, 0], xk[0, 0]], [xk[r - 1, 1], xk[0, 1]], marker='*', color=col,
+ label="edge")
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*',
+ color='y',
+ label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title(
+ "Scan xk cw from xk[-1, :] until we reach edge intersecting line ->(v{}->v{})->v{}".format(
+ i, i + 1, i + 1))
+ if w2 is not None:
+ delta = xk[t:r, :]
+ if debug:
+ print(t, r)
+ plt.plot(delta[:, 0], delta[:, 1], 'k--', label="delta")
+ plt.plot(w1[0], w1[1], 's', label="w1", color='k')
+ plt.plot(w2[0], w2[1], 's', label="w2", color='g')
+ plt.legend()
+ plt.show()
+ if debug:
+ plt.legend()
+ plt.show()
+ if w2 is None:
+ for r in range(xk.shape[0] - 1, 0, -1):
+ # Get intersection w2 of edge and line ->(vi->vi+1)->vi+1
+ w2 = get_intersection(xk[r, :], xk[r - 1, :], start_point, vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ col = 'r' if w2 is None else 'g'
+ plt.plot([xk[r - 1, 0], xk[r, 0]], [xk[r - 1, 1], xk[r, 1]], marker='*',
+ color=col, label="edge")
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*',
+ color='y', label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title(
+ "Scan xk cw from xk[-1, :] until we reach edge intersecting line ->(v{}->v{})->v{}".format(
+ i, i + 1, i + 1))
+ if w2 is not None:
+ delta = xk[t:r, :]
+ if debug:
+ print(t, r)
+ plt.plot(delta[:, 0], delta[:, 1], 'k--', label="delta")
+ plt.plot(w1[0], w1[1], 's', label="w1", color='k')
+ plt.plot(w2[0], w2[1], 's', label="w2", color='g')
+ plt.legend()
+ plt.show()
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+ if verbose:
+ print("Update 3")
+ print("Found intersection ({},{}) at line ->(v{}->v{})->v{}".format(w2[0], w2[1], i,
+ i + 1, i + 1))
+ # Set xk as delta-w2-w1
+ xk = np.vstack((delta, w2, w1))
+ w1_idx = delta.shape[0] + 1
+ w2_idx = delta.shape[0]
+ xk_bounded = True
+
+ F1_idx = 0
+ L1_idx = min(L1_idx - t, xk.shape[0] - 1)
+
+ # F1 update
+ if case == 1:
+ # If ->(vi->vi+1)->vi+1 has just one intersection with xk F1 = startpoint
+ if w2 is None:
+ F1_idx = 0
+ # Otherwise F1 = w2
+ else:
+ F1_idx = w2_idx
+ if case == 2:
+ # Scan xk ccw from F1 until find vertex wt s.t. wt+1 lies to the
+ # right of vi+1->(vi+1->wt)->. Let F1 = wt.
+ idx_offsets = range(xk.shape[0])
+ for off in idx_offsets:
+ t = (F1_idx + off) % xk.shape[0]
+ w_next = xk[(t + 1) % xk.shape[0], :]
+ line_end_point = vi_1 + INF_VAL * (xk[t, :] - vi_1)
+ if point_right_of_line(w_next, vi_1, line_end_point):
+ F1_idx = t
+ break
+
+ # Check update of previous L1 index for new xk
+ # if not np.isclose(np.linalg.norm(L1 - xk[L1_idx, :]), 0):
+ # print("BAD L1 update [{},{}] -> [{},{}]".format(xk[L1_idx, 0], xk[L1_idx, 1], L1[0], L1[1]))
+ # plt.figure(2)
+ # draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ # plt.show()
+
+ # L1 update
+ # scan xk ccw from L1 until find vertex wu s.t. wu+1 lies to the
+ # left of vi+1->(vi+1->wu)->. Let L1 = wu.
+ idx_offsets = range(xk.shape[0])
+ for off in idx_offsets:
+ u = (L1_idx + off) % xk.shape[0]
+ w_next = xk[(u + 1) % xk.shape[0], :]
+ line_end_point = vi_1 + INF_VAL * (xk[u, :] - vi_1)
+ if point_left_of_line(w_next, vi_1, line_end_point):
+ L1_idx = u
+ break
+
+ else:
+ if verbose:
+ print("(2)")
+ # Endpoint for line vi->(vi->vi+1)->
+ end_point = vi + INF_VAL * (vi_1 - vi) / np.linalg.norm(vi_1 - vi)
+
+ # (2.2) L1 lies to the left of line vi->(vi->vi+1)->
+ if point_left_of_line(L1, vi, end_point):
+ case = 2
+ if verbose:
+ print("(2.2) - L1 ({},{}) lies to the left of line v{}->(v{}->v{})->".format(L1[0], L1[1], i, i,
+ i + 1))
+ # xk stays the same
+
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot([end_point[0], vi[0]], [end_point[1], vi[1]], marker='*', color='y',
+ label="v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.title("L1 lies to the left of line v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.show()
+
+ # (2.1) L1 lies on or to the right of line vi->(vi->vi+1)->
+ else:
+ case = 1
+ if verbose:
+ print(
+ "(2.1) - L1 ({},{}) lies on or to the right of line v{}->(v{}->v{})->".format(L1[0], L1[1],
+ i, i, i + 1))
+ print(
+ "Scan xk cw from L1 until we reach F1 or an edge intersecting line v{}->(v{}->v{})->".format(
+ i, i, i + 1))
+ # Scan xk cw from L1 until we reach F1 or an edge intersecting line vi->(vi->vi+1)->
+ idx_offsets = range(xk.shape[0])
+ w1 = None
+ for off in idx_offsets:
+ t = L1_idx - off
+ # If circular
+ if t < 0:
+ t += xk.shape[0]
+ if t == F1_idx:
+ break
+ # Get intersection w1 of edge and line vi->(vi->vi+1)->
+ w1 = get_intersection(xk[t, :], xk[t - 1, :], vi, end_point)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ col = 'r' if w1 is None else 'g'
+ plt.plot([xk[t - 1, 0], xk[t, 0]], [xk[t - 1, 1], xk[t, 1]], marker='*', color=col,
+ label="edge")
+ plt.plot([end_point[0], v[i, 0]], [end_point[1], v[i, 1]], marker='*', color='y',
+ label="v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.title(
+ "Scan xk cw from L1 until we reach F1 or an edge intersecting line v{}->(v{}->v{})->".format(
+ i,
+ i,
+ i + 1))
+ if w1 is not None:
+ if debug:
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ plt.legend()
+ plt.show()
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+ # If no intersecting line is reached no kernel exists
+ if w1 is None:
+ # if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.title('No kernel found. Polygon not starshaped.')
+ plt.show()
+ return False
+
+ if verbose:
+ print("Found intersection ({},{}) at line v{}->(v{}->v{})-> for edge ({},{})-({},{})".format(
+ w1[0], w1[1], i, i, i + 1, xk[t - 1, 0], xk[t - 1, 1], xk[t, 0], xk[t, 1]))
+ print("Scan xk ccw from L1 until we reach edge intersecting line v{}->(v{}->v{})->".format(i,
+ i,
+ i + 1))
+ # Scan xk ccw from L1 until we reach edge w2 intersecting line vi->(vi->vi+1)->
+ w2 = None
+ idx_offsets = range(1, xk.shape[0] + 1)
+ for off in idx_offsets:
+ s = (L1_idx + off) % xk.shape[0]
+ if not xk_bounded and s == 0:
+ break
+ # Get intersection w2 of edge and line vi->(vi->vi+1)->
+ w2 = get_intersection(xk[s - 1, :], xk[s, :], vi, end_point)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ col = 'r' if w2 is None else 'g'
+ plt.plot([xk[s - 1, 0], xk[s, 0]], [xk[s - 1, 1], xk[s, 1]], marker='*', color=col,
+ label="edge")
+ plt.plot([end_point[0], v[i, 0]], [end_point[1], v[i, 1]], marker='*', color='y',
+ label="v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.title(
+ "Scan xk ccw from L1 until we reach edge intersecting line v{}->(v{}->v{})->".format(i,
+ i,
+ i + 1))
+ if w2 is not None:
+ if xk_bounded and t > s:
+ alpha = xk[xk.shape[0]:, :] # Empty array
+ beta = xk[s:t, :]
+ else:
+ alpha = xk[:t, :]
+ beta = xk[s:, :]
+ if debug:
+ print("xk: ", end=" ")
+ print(xk)
+ print("alpha: ", end=" ")
+ print(alpha)
+ print("beta: ", end=" ")
+ print(beta)
+ print("w1: ", end=" ")
+ print(w1)
+ print("w2: ", end=" ")
+ print(w2)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='k')
+ plt.plot(w2[0], w2[1], 's', label="w2", color='g')
+ plt.plot(alpha[:, 0], alpha[:, 1], 'r--', label="alpha")
+ plt.plot(beta[:, 0], beta[:, 1], 'k--', label="beta")
+ plt.legend()
+ plt.show()
+ if verbose:
+ print("Update 1")
+ print(
+ "Found intersection ({},{}) at line v{}->(v{}->v{})-> for edge ({},{})-({},{})".format(
+ w2[0], w2[1], i, i, i + 1, xk[s, 0], xk[s, 1], xk[(s + 1) % xk.shape[0], 0],
+ xk[(s + 1) % xk.shape[0], 1]))
+
+ xk = np.vstack((alpha, w1, w2, beta))
+ w1_idx = alpha.shape[0]
+ w2_idx = alpha.shape[0] + 1
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+ # If no intersecting line is reached
+ if w2 is None:
+ # Test if xk+1 is bounded
+ # If slope vi->(vi->vi+1)-> is comprised between the slopes of initial and final half lines of xk,
+ if not xk_bounded and ((orientation_val(xk[-2, :], xk[-1, :], end_point) * orientation_val(vi,
+ end_point,
+ xk[0,
+ :])) > 0):
+ alpha = xk[:t, :]
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ plt.plot(xk[:2, 0], xk[:2, 1], '--c', label="initial half line")
+ plt.plot(xk[-2:, 0], xk[-2:, 1], '--m', label="final half line")
+ plt.plot([end_point[0], v[i, 0]], [end_point[1], v[i, 1]], marker='*', color='y',
+ label="v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.title(
+ "v{}->(v{}->v{})-> between initial and final half lines of xk".format(
+ i, i, i + 1))
+ plt.plot(alpha[:, 0], alpha[:, 1], 'r--', label="alpha")
+ plt.legend()
+ plt.show()
+ if verbose:
+ print("Update 2 - xk still unbounded")
+ # then xk+1= alpha->w1->(vi->vi+1)-> is also unbounded.
+ xk = np.vstack((alpha, w1, end_point))
+ w1_idx = alpha.shape[0]
+ L1_idx = xk.shape[0] - 1
+ F1_idx = 0
+ else:
+ if verbose:
+ print(
+ "Scan xk ccw from start until we reach edge intersecting line v{}->(v{}->v{})->".format(
+ i,
+ i,
+ i + 1))
+ # otherwise scan xk ccw from xk[0,:] until we reach edge intersecting line vi->(vi->vi+1)->
+ w2 = None
+ for r in range(1, xk.shape[0] + 1):
+ # Get intersection w2 of edge and line vi->(vi->vi+1)->
+ w2 = get_intersection(xk[r - 1, :], xk[r % xk.shape[0], :], vi, end_point)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ col = 'r' if w2 is None else 'g'
+ plt.plot([xk[r - 1, 0], xk[r % xk.shape[0], 0]],
+ [xk[r - 1, 1], xk[r % xk.shape[0], 1]], marker='*', color=col,
+ label="edge")
+ plt.plot([end_point[0], v[i, 0]], [end_point[1], v[i, 1]], marker='*', color='y',
+ label="v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.title(
+ "Scan xk ccw from xk[0,:] until we reach edge intersecting line v{}->(v{}->v{})->".format(
+ i,
+ i,
+ i + 1))
+ if w2 is not None:
+ delta = xk[r:t, :]
+ if debug:
+ plt.plot(w2[0], w2[1], 's', label="w2", color='g')
+ plt.legend()
+ plt.show()
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+ if verbose:
+ print("Update 3")
+ print("Found intersection ({},{}) at line v{}->(v{}->v{})->".format(w2[0], w2[1], i, i,
+ i + 1))
+ # Set xk as delta-w1-vi-vi+1-w2
+ xk = np.vstack((delta, w1, w2))
+ w1_idx = delta.shape[0]
+ w2_idx = delta.shape[0] + 1
+ xk_bounded = True
+ F1_idx = 0
+ L1_idx = min(L1_idx - t, xk.shape[0] - 1)
+
+ # F1 update
+
+ # If vi+1 in vi->(vi->vi+1)->w1,
+ if case == 2 or is_between(vi, vi_1, w1):
+ # scan xk ccw from F1 until find vertex wt s.t. wt+1 lies to the
+ # right of vi+1->(vi+1->wt)->. Let F1 = wt.
+ idx_offsets = range(xk.shape[0])
+ for off in idx_offsets:
+ t = (F1_idx + off) % xk.shape[0]
+ w_next = xk[(t + 1) % xk.shape[0], :]
+ line_end_point = vi_1 + INF_VAL * (xk[t, :] - vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot([vi_1[0], line_end_point[0]], [vi_1[1], line_end_point[1]], '--',
+ label='v{}->(v{}->wt)'.format(i + 1, i + 1))
+ c = 'g' if point_right_of_line(w_next, vi_1, line_end_point) else 'r'
+ plt.plot(w_next[0], w_next[1], color=c, marker='s', label='w_t+1')
+ plt.title(
+ "Update F1\n scan xk ccw from F1 until find vertex wt s.t. wt+1 lies to the right of v{}->(v{}->wt)->.".format(
+ i + 1, i + 1))
+ plt.legend()
+ plt.show()
+ if point_right_of_line(w_next, vi_1, line_end_point):
+ F1_idx = t
+ break
+ else:
+ F1_idx = w1_idx
+
+ # L1 update
+ if case == 1:
+ if w2 is not None:
+ # If vi+1 in vi->(vi->vi+1)->w2
+ if is_between(vi, vi_1, w2):
+ L1_idx = w2_idx
+ else:
+ # scan xk ccw from w2 until find vertex wu s.t. wu+1 lies to the
+ # left of vi+1->(vi+1->wu)->. Let L1 = wu.
+ idx_offsets = range(xk.shape[0])
+ for off in idx_offsets:
+ u = (w2_idx + off) % xk.shape[0]
+ w_next = xk[(u + 1) % xk.shape[0], :]
+ line_end_point = vi_1 + INF_VAL * (xk[u, :] - vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot([vi_1[0], line_end_point[0]], [vi_1[1], line_end_point[1]], '--',
+ label='v{}->(v{}->wt)'.format(i + 1, i + 1))
+ c = 'g' if point_left_of_line(w_next, vi_1, line_end_point) else 'r'
+ plt.plot(w_next[0], w_next[1], color=c, marker='s', label='w_t+1')
+ plt.title(
+ "Update L1\n scan xk ccw from w2 until find vertex wu s.t. wu+1 lies to the left of v{}->(v{}->wt)->.".format(
+ i + 1, i + 1))
+ plt.legend()
+ plt.show()
+ if point_left_of_line(w_next, vi_1, line_end_point):
+ L1_idx = u
+ break
+
+ # (2.1.2) line vi->(vi->vi+1)-> intersects xk just in w1
+ else:
+ L1_idx = xk.shape[0] - 1
+ if case == 2:
+ # print("(2.1.2)")
+ if xk_bounded:
+ # scan xk ccw from w2 until find vertex wu s.t. wu+1 lies to the
+ # left of vi+1->(vi+1->wu)->. Let L1 = wu.
+ idcs_list = np.roll(np.arange(xk.shape[0]), -L1_idx) if xk_bounded else range(L1_idx,
+ xk.shape[0] - 1)
+ for u in idcs_list:
+ w_next = xk[(u + 1) % xk.shape[0], :]
+ line_end_point = vi_1 + INF_VAL * (xk[u, :] - vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot([vi_1[0], line_end_point[0]], [vi_1[1], line_end_point[1]], '--',
+ label='v{}->(v{}->wt)'.format(i + 1, i + 1))
+ c = 'g' if point_left_of_line(w_next, vi_1, line_end_point) else 'r'
+ plt.plot(w_next[0], w_next[1], color=c, marker='s', label='w_t+1')
+ plt.title(
+ "Update L1\n scan xk ccw from w2 until find vertex wu s.t. wu+1 lies to the left of v{}->(v{}->wt)->.".format(
+ i + 1, i + 1))
+ plt.legend()
+ plt.show()
+ if point_left_of_line(w_next, vi_1, line_end_point):
+ L1_idx = u
+ break
+
+ if verbose:
+ print("F1:", end=" ")
+ print(F1)
+ print("L1:", end=" ")
+ print(L1)
+ print("xk:", end=" ")
+ print(xk)
+ if debug or verbose:
+ draw(xk, F1_idx, L1_idx, 0, xk_bounded)
+ plt.show()
+ _, idx = np.unique(xk, axis=0, return_index=True)
+ xk = xk[np.sort(idx), :]
+ self._kernel = shapely.geometry.Polygon(xk)
+
+
diff --git a/python/ur_simple_control/path_generation/starworlds/obstacles/starshaped_primitive_combination.py b/python/ur_simple_control/path_generation/starworlds/obstacles/starshaped_primitive_combination.py
new file mode 100644
index 0000000000000000000000000000000000000000..6fdf2cd7ba907711261851bd22c60a6ab5d1b95d
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/obstacles/starshaped_primitive_combination.py
@@ -0,0 +1,226 @@
+import shapely
+import numpy as np
+from obstacles import Frame, StarshapedObstacle, StarshapedPolygon
+from utils import is_ccw, is_cw, draw_shapely_polygon
+import matplotlib.pyplot as plt
+
+# Note: Local == Global frame
+class StarshapedPrimitiveCombination(StarshapedObstacle):
+
+ def __init__(self, obstacle_cluster, hull_cluster, xr, **kwargs):
+ self._obstacle_cluster = obstacle_cluster
+ self._hull_cluster = hull_cluster
+ super().__init__(xr=xr, **kwargs)
+ self.vertices = None
+ self.circular_vertices = None
+ self.vertex_angles = None
+
+ def obstacle_cluster(self):
+ return self._obstacle_cluster
+
+ def hull_cluster(self):
+ return self._hull_cluster
+
+ def dilated_obstacle(self, padding, id="new", name=None):
+ pass
+
+ def point_location(self, x, input_frame=Frame.GLOBAL):
+ locs = [obs.point_location(x, input_frame=Frame.GLOBAL) for obs in self._obstacle_cluster] + \
+ [self._hull_cluster_point_location(x)]
+ if any([l < 0 for l in locs]):
+ # Interior point
+ return -1
+ if any([l == 0 for l in locs]):
+ # Boundary point
+ return 0
+ # Exterior point
+ return 1
+
+ def line_intersection(self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ intersection_points = []
+ for o in self._obstacle_cluster:
+ intersection_points += o.line_intersection(line, Frame.GLOBAL, Frame.GLOBAL)
+ intersection_points += self._hull_cluster_line_intersections(line)
+ return intersection_points
+
+ # TODO: Fix if needed. Currently not considering hull.
+ def tangent_points(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ tp, tp_candidates = [], []
+ for obs in self._obstacle_cluster:
+ tp_candidates += obs.tangent_points(x, Frame.GLOBAL, Frame.GLOBAL)
+ for i in range(len(tp_candidates)):
+ if all([is_ccw(x, tp_candidates[i], tp_candidates[j]) for j in range(len(tp_candidates)) if j is not i]) or \
+ all([is_cw(x, tp_candidates[i], tp_candidates[j]) for j in range(len(tp_candidates)) if j is not i]):
+ tp += [tp_candidates[i]]
+ return tp
+
+ def _compute_kernel(self):
+ self._kernel = StarshapedPolygon(self.polygon(), xr=self.xr(), id="temp").kernel()
+
+ def _check_convexity(self):
+ self._is_convex = StarshapedPolygon(self.polygon(), xr=self.xr(), id="temp").is_convex()
+
+ def boundary_mapping(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ # intersection_points = [p for ps in self.line_intersection([self._xr, self._xr+10*(x-self._xr)]) for p in ps]
+ intersection_points = self.line_intersection([self._xr, self._xr+10*(x-self._xr)])
+ if not intersection_points:
+ return None
+ dist_intersection_points = [np.linalg.norm(ip - self._xr) for ip in intersection_points]
+ return intersection_points[np.argmax(dist_intersection_points)]
+
+ def vel_intertial_frame(self, x):
+ boundary_obs_idx = 0
+ max_dist = -1
+ for i, ps in enumerate(self.line_intersection([self._xr, self._xr+10*(x-self._xr)])):
+ o_intersection_dist = max([np.linalg.norm(p-self._xr) for p in ps] + [-1])
+ if o_intersection_dist > max_dist:
+ boundary_obs_idx = i
+ max_dist = o_intersection_dist
+ if boundary_obs_idx >= len(self._obstacle_cluster):
+ boundary_obs_idx -= len(self._obstacle_cluster)
+ return self._obstacle_cluster[boundary_obs_idx].vel_intertial_frame(x)
+
+ def normal(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL, x_is_boundary=False, type='weigthed_polygon_approx'):
+ if type == 'sub_normal':
+ boundary_obs_idx = 0
+ max_dist = -1
+ line = [self._xr, self._xr+10*(x-self._xr)]
+ for i, o in enumerate(self._obstacle_cluster):
+ intersection_points = o.line_intersection(line, Frame.GLOBAL, Frame.GLOBAL)
+ for p in intersection_points:
+ p_dist = np.linalg.norm(p - self._xr)
+ if p_dist > max_dist:
+ max_dist = p_dist
+ boundary_obs_idx = i
+
+ hull_ip = self._hull_cluster_line_intersections(line)
+ if hull_ip:
+ for p in hull_ip:
+ p_dist = np.linalg.norm(p - self._xr)
+ if p_dist > max_dist:
+ max_dist = p_dist
+ boundary_obs_idx = len(self._obstacle_cluster)
+ if boundary_obs_idx < len(self._obstacle_cluster):
+ return self._obstacle_cluster[boundary_obs_idx].normal(x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL)
+ else:
+ boundary_obs_idx -= len(self._obstacle_cluster)
+ hull_vertices = np.array(self._hull_cluster.exterior.coords[:-1])
+ vertex_angles = np.array([np.arctan2(v[1] - self._xr[1], v[0] - self._xr[0]) for v in hull_vertices]).flatten()
+ idcs = np.argsort(vertex_angles)
+ vertex_angles = vertex_angles[idcs]
+ hull_vertices = hull_vertices[idcs, :]
+ vertex_angles = np.hstack((vertex_angles, vertex_angles[0] + 2 * np.pi))
+ hull_vertices = np.vstack((hull_vertices, hull_vertices[0, :]))
+
+ angle = np.arctan2(x[1] - self._xr[1], x[0] - self._xr[0])
+ v_idx = np.argmax(vertex_angles > angle)
+ # Adjust for circular self.vertices (self.vertices[0] == self.vertices[-1])
+ if v_idx == 0:
+ v_idx = -1
+ n = np.array([hull_vertices[v_idx, 1] - hull_vertices[v_idx - 1, 1],
+ hull_vertices[v_idx - 1, 0] - hull_vertices[v_idx, 0]])
+ n /= np.linalg.norm(n)
+ return n
+ elif type == 'polygon_approx' or type == 'weigthed_polygon_approx':
+ if self.vertices is None:
+ self._update_vertex_angles()
+ angle = np.arctan2(x[1] - self._xr[1], x[0] - self._xr[0])
+ v_idx = np.argmax(self.vertex_angles > angle)
+
+ if v_idx == self.vertices.shape[0]:
+ v_idx = 0
+
+ if type == 'polygon_approx':
+ n = np.array([self.vertices[v_idx, 1] - self.vertices[v_idx - 1, 1],
+ self.vertices[v_idx - 1, 0] - self.vertices[v_idx, 0]])
+ else:
+ edge_neighbors = [(self.vertices[(v_idx - 2 + i) % self.vertices.shape[0]],
+ self.vertices[(v_idx - 1 + i) % self.vertices.shape[0]]) for i in range(3)]
+ edge_neighbors_normal = np.array([[e[1][1] - e[0][1],
+ e[0][0] - e[1][0]] for e in edge_neighbors])
+ edge_closest = [shapely.ops.nearest_points(shapely.geometry.LineString(e),
+ shapely.geometry.Point(x))[0].coords[0] for e in
+ edge_neighbors]
+
+ dist = [np.linalg.norm(np.array(e) - x) for e in edge_closest]
+ w = np.array([1 / (d + 1e-10) for d in dist])
+ w /= sum(w)
+ n = edge_neighbors_normal.T.dot(w)
+
+ n /= np.linalg.norm(n)
+ return n
+
+ def set_xr(self, xr, input_frame=Frame.OBSTACLE, safe_set=False):
+ super().set_xr(xr, input_frame, safe_set)
+ self._update_vertex_angles()
+
+ def init_plot(self, ax=None, show_reference=True, show_name=False, **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ if "fc" not in kwargs and "facecolor" not in kwargs:
+ kwargs["fc"] = 'lightgrey'
+
+ line_handles = []
+
+ lh, _ = draw_shapely_polygon(self.polygon(), ax=ax, **kwargs)
+ line_handles += lh
+
+ # Reference point
+ line_handles += ax.plot(*self.xr(), '+', color='k') if show_reference else [None]
+ # Name
+ line_handles += [ax.text(*self.xr(), self._name)] if show_name else [None]
+ return line_handles, ax
+
+ def update_plot(self, line_handles):
+ pass
+
+ def draw(self, ax=None, show_reference=True, show_name=False, **kwargs):
+ line_handles, ax = self.init_plot(ax, show_reference, show_name, **kwargs)
+ self.update_plot(line_handles)
+ return line_handles, ax
+
+ def _hull_cluster_point_location(self, x):
+ if self._hull_cluster is None:
+ return 1
+ x_sh = shapely.geometry.Point(x)
+ if self._hull_cluster.contains(x_sh):
+ return -1
+ if self._hull_cluster.disjoint(x_sh):
+ return 1
+ return 0
+
+ def _hull_cluster_line_intersections(self, line):
+ if self._hull_cluster is None:
+ return []
+ line_sh = shapely.geometry.LineString(line)
+ intersection_points_shapely = line_sh.intersection(self._hull_cluster.exterior)
+ if intersection_points_shapely.is_empty:
+ return []
+ if intersection_points_shapely.geom_type == 'Point':
+ return [np.array([intersection_points_shapely.x, intersection_points_shapely.y])]
+ elif intersection_points_shapely.geom_type == 'MultiPoint':
+ return [np.array([p.x, p.y]) for p in intersection_points_shapely.geoms]
+ elif intersection_points_shapely.geom_type == 'LineString':
+ return [np.array([ip[0], ip[1]]) for ip in intersection_points_shapely.coords]
+ elif intersection_points_shapely.geom_type == 'MultiLineString':
+ return [np.array([l[0], l[1]]) for line in intersection_points_shapely.geoms for
+ l in line.coords]
+ else:
+ print("[_hull_cluster_line_intersections]: Shapely geom_type not covered!")
+ print(intersection_points_shapely)
+
+ def _update_vertex_angles(self):
+ self.vertices = np.array(self._polygon.exterior.coords[:-1])
+ self.circular_vertices = np.array(self._polygon.exterior.coords)
+ self.vertex_angles = np.arctan2(self.vertices[:, 1] - self._xr[1], self.vertices[:, 0] - self._xr[0])
+ idcs = np.argsort(self.vertex_angles)
+ self.vertex_angles = self.vertex_angles[idcs]
+ self.vertices = self.vertices[idcs, :]
+ self.circular_vertices = np.vstack((self.vertices, self.vertices[0, :]))
+ self.vertex_angles = np.hstack((self.vertex_angles, self.vertex_angles[0] + 2 * np.pi))
+
+ def _compute_polygon_representation(self):
+ obs_pol = [obs.polygon() for obs in self._obstacle_cluster]
+ if self._hull_cluster is not None:
+ obs_pol += [self._hull_cluster]
+ self._polygon = shapely.ops.unary_union(obs_pol)
diff --git a/python/ur_simple_control/path_generation/starworlds/requirements.txt b/python/ur_simple_control/path_generation/starworlds/requirements.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3689a74ef0eb7796d52dc466d5aafaa6c624e28f
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/requirements.txt
@@ -0,0 +1,4 @@
+numpy
+scipy
+matplotlib
+shapely
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworlds/setup.py b/python/ur_simple_control/path_generation/starworlds/setup.py
new file mode 100644
index 0000000000000000000000000000000000000000..67761811b674817cb9f0d2dea2db43c8dba03227
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/setup.py
@@ -0,0 +1,14 @@
+from setuptools import setup, find_packages
+
+setup(name='starworlds',
+ version='1.0',
+ packages=find_packages(),
+ install_requires=[
+ 'pyyaml',
+ 'numpy',
+ 'scipy',
+ 'matplotlib',
+ 'shapely'
+ ]
+
+)
diff --git a/python/ur_simple_control/path_generation/starworlds/starshaped_hull/__init__.py b/python/ur_simple_control/path_generation/starworlds/starshaped_hull/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..45406b14bb497117f7b86c4941c8e4849f3160e8
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/starshaped_hull/__init__.py
@@ -0,0 +1,2 @@
+from .starshaped_hull import admissible_kernel, kernel_starshaped_hull
+from .cluster_and_starify import ObstacleCluster, get_intersection_clusters, cluster_and_starify, draw_clustering, draw_adm_ker, draw_star_hull
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworlds/starshaped_hull/cluster_and_starify.py b/python/ur_simple_control/path_generation/starworlds/starshaped_hull/cluster_and_starify.py
new file mode 100644
index 0000000000000000000000000000000000000000..2e825e86581a3bbecb805d3ea1351a1a6a5c95ed
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/starshaped_hull/cluster_and_starify.py
@@ -0,0 +1,500 @@
+import shapely
+import numpy as np
+from obstacles import Frame, StarshapedPrimitiveCombination, Ellipse, StarshapedPolygon
+from utils import is_ccw, is_collinear, equilateral_triangle, Cone, tic, toc, draw_shapely_polygon
+from scipy.spatial import ConvexHull
+import starshaped_hull as sh
+import matplotlib.pyplot as plt
+
+
+class ObstacleCluster:
+ def __init__(self, obstacles):
+ self.name = '_'.join([str(o.id()) for o in obstacles])
+ self.obstacles = obstacles
+ self.cluster_obstacle = None if len(obstacles) > 1 else obstacles[0]
+ self.kernel_points = None
+ self.admissible_kernel = None
+ self._polygon = None
+ self._polygon_excluding_hull = None
+
+ def __str__(self):
+ return self.name
+
+ def polygon(self):
+ if self._polygon is None:
+ if self.cluster_obstacle is None:
+ print("[OBSTACLE CLUSTER]: WARNING, cluster_obstacle must be defined before accessing polygon.")
+ else:
+ self._polygon = self.cluster_obstacle.polygon()
+ return self._polygon
+
+ def polygon_excluding_hull(self):
+ if self._polygon_excluding_hull is None:
+ self._polygon_excluding_hull = shapely.ops.unary_union([o.polygon() for o in self.obstacles])
+ return self._polygon_excluding_hull
+
+ def draw(self, ax=None):
+ if self.cluster_obstacle is not None:
+ ax, _ = self.cluster_obstacle.draw(ax=ax, fc="green")
+ for obs in self.obstacles:
+ ax, _ = obs.draw(ax=ax)
+ return ax, _
+
+
+def get_intersection_clusters(clusters):
+ No = len(clusters)
+ intersection_idcs = []
+
+ # Use polygon approximations for intersection check
+ cluster_polygons = [cl.polygon() for cl in clusters]
+
+ # Find intersections
+ intersections_exist = False
+ for i in range(No):
+ intersection_idcs += [[i]]
+ for j in range(i + 1, No):
+ if cluster_polygons[i].intersects(cluster_polygons[j]):
+ intersection_idcs[i] += [j]
+ intersections_exist = True
+
+ if not intersections_exist:
+ return clusters, intersections_exist
+
+ # Cluster intersecting obstacles
+ for i in range(No - 1, 0, -1):
+ for j in range(i - 1, -1, -1):
+ found = False
+ for l_j in intersection_idcs[j]:
+ if l_j in intersection_idcs[i]:
+ found = True
+ break
+ if found:
+ intersection_idcs[j] = list(set(intersection_idcs[j] + intersection_idcs[i]))
+ intersection_idcs[i] = []
+ break
+
+ # Create obstacle clusters
+ cluster_obstacles = [cl.obstacles for cl in clusters]
+ new_clusters = []
+ for i in range(No):
+ if intersection_idcs[i]:
+ new_clusters += [ObstacleCluster([o for j in intersection_idcs[i] for o in cluster_obstacles[j]])]
+
+ return new_clusters, intersections_exist
+
+
+def compute_kernel_points(cl, x, xg, epsilon, cl_prev, workspace):
+ triangle_center_prev = np.mean(cl_prev.kernel_points, axis=0) if cl_prev else None
+
+ t0 = tic()
+ ts = {}
+ # Find triangle center selection set (TCSS)
+ tcss = cl.admissible_kernel
+ # If obstacle is in exterior of workspace limit TCSS to workspace exterior
+ if not cl.polygon_excluding_hull().within(workspace.polygon()):
+ tcss_tmp = tcss.difference(workspace.polygon())
+ tcss_tmp = tcss_tmp.intersection(cl.polygon_excluding_hull()) # NOTE: Added for exlcluding undesired extremas in other workspace exterior than close to obstacles
+ if tcss_tmp.area > 1e-6:
+ tcss = tcss_tmp
+ tcss_tmp = tcss
+ ts['ws check'] = toc(t0)
+ # Try to use intersection of all obstacle kernels in cluster if all starshaped
+ if all([o.is_starshaped() for o in cl.obstacles]):
+ for o in cl.obstacles:
+ tcss_tmp = tcss_tmp.intersection(o.kernel())
+ ts['kernel intersection'] = toc(t0)-list(ts.values())[-1]
+ # Else, try to use union of all obstacles in cluster
+ if tcss_tmp.area < 1e-6:
+ tcss_tmp = tcss.intersection(cl.polygon_excluding_hull())
+ ts['cluster intersection'] = toc(t0)-list(ts.values())[-1]
+ if tcss_tmp.area > 1e-6:
+ tcss = tcss_tmp
+
+ # If not tc from previous iteraion, use closest point to TCSS in ad ker as triangle center
+ if triangle_center_prev is None:
+ tc, _ = shapely.ops.nearest_points(cl.admissible_kernel, tcss.centroid)
+ triangle_center = np.array(tc.coords[0])
+ while is_collinear(x, xg, triangle_center):
+ triangle_center += np.random.uniform(-1e-4, 1e-4, 2)
+ # Else, use previous triangle center if still in selection set
+ elif tcss.contains(shapely.geometry.Point(triangle_center_prev))\
+ and not is_collinear(x, xg, triangle_center_prev):
+ triangle_center = triangle_center_prev
+ # Else, try to maintain triangle center on same side of l(x,xg) as previous time step
+ else:
+ x_xg_line = shapely.geometry.LineString([x, xg])
+ splitted_tcss = shapely.ops.split(tcss, x_xg_line).geoms
+ triangle_center_selection_set = splitted_tcss[0]
+ if len(splitted_tcss) > 1:
+ for i in range(1, len(splitted_tcss)):
+ if is_ccw(x, xg, splitted_tcss[i].centroid.coords[0]) == is_ccw(x, xg, triangle_center_prev):
+ triangle_center_selection_set = splitted_tcss[i]
+ break
+ tc, _ = shapely.ops.nearest_points(cl.admissible_kernel, triangle_center_selection_set.centroid)
+ triangle_center = np.array(tc.coords[0])
+
+ ts['tc selection'] = toc(t0)-list(ts.values())[-1]
+
+ # if cl.name == '5_6_7':
+ # hs, _ = draw_shapely_polygon(tcss, plt.gca(), fc='g')
+ # hs += plt.plot(*triangle_center, 'kd')
+ # while not plt.waitforbuttonpress(): pass
+ # [h.remove() for h in hs]
+
+ # Select kernel points as largest equilateral triangle in TCSS (with maximum side length epsilon)
+ if tcss.geom_type == 'Polygon':
+ dist = tcss.exterior.distance(shapely.geometry.Point(triangle_center))
+ else:
+ tc = shapely.geometry.Point(triangle_center)
+ dist = min([p.exterior.distance(tc) for p in tcss.geoms])
+ triangle_length = min(epsilon, 0.9 * dist)
+ kernel_points = equilateral_triangle(triangle_center, triangle_length)
+ ts['triangle generation'] = toc(t0)-list(ts.values())[-1]
+ tot_time = sum(ts.values())
+ for k in ts.keys():
+ ts[k] = int(ts[k] / tot_time * 100)
+ # print(ts)
+ return kernel_points
+
+def extract_cluster(cl, cl_list):
+ if cl_list is None:
+ return None
+ for cl_i in cl_list:
+ if cl.name == cl_i.name:
+ return cl_i
+ return None
+
+# Input: Convex obstacles, excluding points x and xg, kernel width epsilon
+def cluster_and_starify(obstacles, x, xg, epsilon, workspace=None, max_compute_time=np.inf, previous_clusters=None,
+ make_convex=False, exclude_obstacles=False, max_iterations=np.inf, verbose=False,
+ timing_verbose=False, return_history=False):
+ t0 = tic()
+
+ if workspace is None:
+ workspace = Ellipse([1e10, 1e10])
+
+ # Exit flags
+ INCOMPLETE = 0
+ COMPLETE = 1
+ MAX_COMPUTE_TIME_CONVEX_HULL = 2
+
+ # Variable initialization
+ kernel_time, hull_time, cluster_time, convex_time = [0.], [0.], [0.], 0.
+ adm_ker_robot_cones = {obs.id(): None for obs in obstacles}
+ adm_ker_goal_cones = {obs.id(): None for obs in obstacles}
+ adm_ker_obstacles = {obs.id(): {} for obs in obstacles}
+ cluster_iterations = []
+ n_iter = 0
+
+ def default_result():
+ if return_history:
+ cl_history = cluster_iterations if n_iter > 0 else [ObstacleCluster([o]) for o in obstacles]
+ return [ObstacleCluster([o]) for o in obstacles], [0.] * 4, INCOMPLETE, n_iter, cl_history
+ else:
+ return [ObstacleCluster([o]) for o in obstacles], [0.] * 4, INCOMPLETE, n_iter
+
+ # Compute admissible kernel for all obstacles
+ for o in obstacles:
+ if max_compute_time < toc(t0):
+ return default_result()
+ # Find admissible kernel
+ adm_ker_robot_cones[o.id()] = sh.admissible_kernel(o, x)
+ adm_ker_goal_cones[o.id()] = sh.admissible_kernel(o, xg)
+
+ if adm_ker_robot_cones[o.id()] is None:
+ if verbose:
+ print("[Cluster and Starify]: Robot position is not a free exterior point of obstacle " + str(o.id()))
+ return default_result()
+
+ if adm_ker_goal_cones[o.id()] is None:
+ if verbose:
+ print("[Cluster and Starify]: Goal position is not a free exterior point of obstacle " + str(o.id()))
+ return default_result()
+
+ # Admissible kernel when excluding points of other obstacles
+ if exclude_obstacles:
+ for o_ex in obstacles:
+ if o_ex.id() == o.id():
+ continue
+ o_x_exclude = o_ex.extreme_points()
+ if not all([o.exterior_point(x_ex) for x_ex in o_x_exclude]):
+ adm_ker_obstacles[o.id()][o_ex.id()] = None
+ continue
+ adm_ker_obstacles[o.id()][o_ex.id()] = sh.admissible_kernel(o, o_x_exclude[0]).polygon()
+ for v in o_x_exclude[1:]:
+ adm_ker_obstacles[o.id()][o_ex.id()] = adm_ker_obstacles[o.id()][o_ex.id()].intersection(
+ sh.admissible_kernel(o, v).polygon())
+ init_kernel_time = toc(t0)
+
+ # -- First iteration -- #
+
+ ker_sel_time, hull_compute_time, star_obj_time = [0], [0], [0]
+ # Initialize clusters as single obstacles
+ clusters = []
+ for o in obstacles:
+ # Ensure not xr in l(x,xg)
+ # while is_collinear(x, o.xr(), xg):
+ # o.set_xr(o.xr(output_frame=Frame.OBSTACLE) + np.random.normal(0, 0.01, 2))
+ cl = ObstacleCluster([o])
+
+ # New---
+ t1 = tic()
+ cl.admissible_kernel = adm_ker_robot_cones[o.id()].intersection(adm_ker_goal_cones[o.id()])
+ kernel_time[0] += toc(t1)
+
+ t1 = tic()
+ cl_prev = extract_cluster(cl, previous_clusters)
+ # if cl_prev is None:
+ # cl_prev.kernel_points = equilateral_triangle(o.xr(), epsilon)
+ cl.kernel_points = compute_kernel_points(cl, x, xg, epsilon, cl_prev, workspace=workspace)
+ ker_sel_time[0] += toc(t1)
+ t1 = tic()
+
+ # -- Compute starshaped hull of cluster
+ cl_id = "new" if cl_prev is None else cl_prev.cluster_obstacle.id()
+ cluster_hull_extensions = sh.kernel_starshaped_hull(cl.obstacles, cl.kernel_points)
+ hull_compute_time[0] += toc(t1)
+ t1 = tic()
+ k_centroid = np.mean(cl.kernel_points, axis=0)
+ if cluster_hull_extensions is None:
+ cl.cluster_obstacle = o
+ cl.cluster_obstacle.set_xr(k_centroid, input_frame=Frame.GLOBAL)
+ else:
+ # Non-starshaped polygons are included in the cluster hull
+ # cl_obstacles = [o] if o.is_starshaped() else []
+ cl.cluster_obstacle = StarshapedPrimitiveCombination(cl.obstacles, cluster_hull_extensions, xr=k_centroid,
+ id=cl_id)
+ star_obj_time[0] += toc(t1)
+ # cl.polygon()
+ # if o.is_starshaped and o.kernel().contains(shapely.geometry.Point(k_centroid)):
+ # cl.cluster_obstacle
+ # cl.cluster_obstacle = StarshapedPolygon(cl._polygon, xr=k_centroid, id=o.id())
+ # hull_time[0] += toc(t1)
+
+
+ clusters += [cl]
+
+
+ # if not o.is_starshaped():
+ # cl = clusters[-1]
+ # t1 = tic()
+ # cl.admissible_kernel = adm_ker_robot_cones[o.id()].intersection(adm_ker_goal_cones[o.id()])
+ # kernel_time[0] += toc(t1)
+ #
+ # t1 = tic()
+ # cl_prev = None
+ # if previous_clusters:
+ # for p_cl in previous_clusters:
+ # if cl.name == p_cl.name:
+ # cl_prev = p_cl
+ # cl.kernel_points = compute_kernel_points(cl, x, xg, epsilon, cl_prev, workspace=workspace)
+ # # -- Compute starshaped hull of cluster
+ # k_centroid = np.mean(cl.kernel_points, axis=0)
+ # cl._polygon = sh.kernel_starshaped_hull(o, cl.kernel_points)
+ # cl.cluster_obstacle = StarshapedPolygon(cl._polygon, xr=k_centroid, id=o.id())
+ # hull_time[0] += toc(t1)
+ hull_time[0] = ker_sel_time[0] + hull_compute_time[0] + star_obj_time[0]
+
+ # Set cluster history
+ cluster_history = {cl.name: cl for cl in clusters}
+ if return_history:
+ cluster_iterations += [clusters]
+
+ # -- Cluster obstacles such that no intersection exists
+ t1 = tic()
+ clusters, intersection_exists = get_intersection_clusters(clusters)
+ cluster_time[0] = toc(t1)
+
+ n_iter = 1
+ # -- End first iteration -- #
+
+ while intersection_exists:
+ kernel_time += [0.]
+ hull_time += [0.]
+ cluster_time += [0.]
+ ker_sel_time += [0.]
+ hull_compute_time += [0.]
+ star_obj_time += [0.]
+
+ # Check compute time
+ if max_compute_time < toc(t0):
+ if verbose:
+ print("[Cluster and Starify]: Max compute time.")
+ cluster_iterations += [clusters]
+ return default_result()
+
+ # Find starshaped obstacle representation for each cluster
+ for i, cl in enumerate(clusters):
+ # If cluster already calculated keep it
+ if cl.name in cluster_history:
+ clusters[i] = cluster_history[cl.name]
+ continue
+
+ # ----------- Admissible Kernel ----------- #
+ t1 = tic()
+
+ # If cluster is two convex obstacles
+ # if len(cl.obstacles) == 2 and cl.obstacles[0].is_convex() and cl.obstacles[1].is_convex():
+ # cl.admissible_kernel = cl.obstacles[0].polygon().intersection(cl.obstacles[1].polygon()).buffer(0.01)
+ # else:
+ adm_ker_robot = Cone.list_intersection([adm_ker_robot_cones[o.id()] for o in cl.obstacles], same_apex=True)
+ adm_ker_goal = Cone.list_intersection([adm_ker_goal_cones[o.id()] for o in cl.obstacles], same_apex=True)
+ cl.admissible_kernel = adm_ker_robot.intersection(adm_ker_goal)
+
+ if cl.admissible_kernel.is_empty or cl.admissible_kernel.area < 1e-6:
+ if verbose:
+ print("[Cluster and Starify]: Could not find disjoint starshaped obstacles. Admissible kernel empty for the cluster " + cl.name)
+ cluster_iterations += [clusters]
+ return default_result()
+
+ # Exclude other obstacles
+ adm_ker_o_ex = cl.admissible_kernel
+ if exclude_obstacles:
+ for o in cl.obstacles:
+ for o_ex in obstacles:
+ if o_ex.id() == o.id() or adm_ker_obstacles[o.id()][o_ex.id()] is None:
+ continue
+ adm_ker_o_ex = adm_ker_o_ex.intersection(adm_ker_obstacles[o.id()][o_ex.id()])
+ if not (adm_ker_o_ex.is_empty or adm_ker_o_ex.area < 1e-6):
+ cl.admissible_kernel = adm_ker_o_ex
+
+ kernel_time[n_iter] += toc(t1)
+ # ----------- End Admissible Kernel ----------- #
+
+ # ----------- Starshaped Hull ----------- #
+ t1 = tic()
+ # Check if cluster exist in previous cluster
+ cl_prev = None
+ if previous_clusters:
+ for p_cl in previous_clusters:
+ if cl.name == p_cl.name:
+ cl_prev = p_cl
+
+ # -- Kernel points selection
+ cl.kernel_points = compute_kernel_points(cl, x, xg, epsilon, cl_prev, workspace=workspace)
+ ker_sel_time[n_iter] += toc(t1)
+ t1 = tic()
+
+ # -- Compute starshaped hull of cluster
+ cl_id = "new" if cl_prev is None else cl_prev.cluster_obstacle.id()
+ cluster_hull_extensions = sh.kernel_starshaped_hull(cl.obstacles, cl.kernel_points)
+ hull_compute_time[n_iter] += toc(t1)
+
+ t1 = tic()
+ k_centroid = np.mean(cl.kernel_points, axis=0)
+ # Non-starshaped polygons are included in the cluster hull
+ cl_obstacles = [o for o in cl.obstacles if o.is_starshaped()]
+ cl.cluster_obstacle = StarshapedPrimitiveCombination(cl_obstacles, cluster_hull_extensions, xr=k_centroid, id=cl_id)
+ cl.polygon()
+
+ star_obj_time[n_iter] += toc(t1)
+ # ----------- End Starshaped Hull ----------- #
+
+ # -- Add cluster to history
+ cluster_history[cl.name] = cl
+
+ hull_time[n_iter] = ker_sel_time[n_iter] + hull_compute_time[n_iter] + star_obj_time[n_iter]
+
+ if return_history:
+ cluster_iterations += [clusters]
+
+ # ----------- Clustering ----------- #
+ t1 = tic()
+ # -- Cluster star obstacles such that no intersection exists
+ clusters, intersection_exists = get_intersection_clusters(clusters)
+ cluster_time[n_iter] = toc(t1)
+ # ----------- End Clustering ----------- #
+
+ n_iter += 1
+
+ if n_iter >= max_iterations:
+ break
+
+ # ----------- Make Convex ----------- #
+
+ if make_convex:
+ # Make convex if no intersection occurs
+ t1 = tic()
+ for j, cl in enumerate(clusters):
+ if not cl.cluster_obstacle.is_convex():
+ # Check compute time
+ if max_compute_time < toc(t0):
+ if verbose:
+ print("[Cluster and Starify]: Max compute time in convex hull.")
+ return clusters, [sum(cluster_time), init_kernel_time+sum(kernel_time), sum(hull_time), toc(t1)], MAX_COMPUTE_TIME_CONVEX_HULL, n_iter
+
+ v = np.array(cl.polygon().exterior.coords)[:-1, :]
+ hull = ConvexHull(v)
+ hull_polygon = shapely.geometry.Polygon(v[hull.vertices, :])
+ if not any([hull_polygon.contains(shapely.geometry.Point(x_ex)) for x_ex in [x, xg]]) and not any(
+ [hull_polygon.intersects(clusters[k].polygon()) for k in range(len(clusters)) if k != j]):
+ # clusters[j].cluster_obstacle = StarshapedPrimitiveCombination(cl.obstacles, hull_polygon, cl.cluster_obstacle.xr(Frame.GLOBAL))
+ clusters[j].cluster_obstacle = StarshapedPolygon(hull_polygon, xr=cl.cluster_obstacle.xr(Frame.GLOBAL))
+
+ convex_time = toc(t1)
+ # ----------- End Make Convex ----------- #
+
+ timing_vec = [sum(cluster_time), init_kernel_time+sum(kernel_time), sum(hull_time), convex_time]
+ if timing_verbose:
+
+ print("------------\nTotal timing (Cluster/AdmKer/StHull/ConvHull): {:.1f} [{:.1f}, {:.1f}, {:.1f}, {:.1f}]".format(sum(timing_vec), *timing_vec))
+ print("Init kernel timing: {:.1f}".format(init_kernel_time))
+ for i in range(n_iter):
+ print("Iteration {} timing (Cluster/AdmKer/StHull): [{:.1f}, {:.1f}, {:.1f}]".format(i, cluster_time[i], kernel_time[i], hull_time[i]))
+ print("\t Hull timing divide: (KerSel/Hull/Obj) calculation [{:.1f}, {:.1f}, {:.1f}]".format(ker_sel_time[i], hull_compute_time[i], star_obj_time[i]))
+
+ if return_history:
+ return clusters, timing_vec, COMPLETE, n_iter, cluster_iterations
+ else:
+ return clusters, timing_vec, COMPLETE, n_iter
+
+
+def draw_clustering(clusters, p, pg, xlim=None, ylim=None):
+ color = plt.cm.rainbow(np.linspace(0, 1, len(clusters)))
+ fig, ax = plt.subplots()
+ for i, cl in enumerate(clusters):
+ [o.draw(ax=ax, fc=color[i], show_reference=False, ec='k', alpha=0.8) for o in cl.obstacles]
+ ax.plot(*p, 'ko')
+ ax.plot(*pg, 'k*')
+ if xlim is not None:
+ ax.set_xlim(xlim)
+ if ylim is not None:
+ ax.set_ylim(ylim)
+ return fig, ax
+
+
+def draw_star_hull(clusters, p, pg, xlim=None, ylim=None):
+ fig, ax = plt.subplots()
+ for cl in clusters:
+ if cl.cluster_obstacle is not None:
+ cl.cluster_obstacle.draw(ax=ax, fc='g', alpha=0.8)
+ draw_shapely_polygon(cl.polygon_excluding_hull(), ax=ax, fc='lightgrey', ec='k')
+ else:
+ draw_shapely_polygon(cl.polygon_excluding_hull(), ax=ax, fc='r', ec='k')
+ ax.plot(*p, 'ko')
+ ax.plot(*pg, 'k*')
+ if xlim is not None:
+ ax.set_xlim(xlim)
+ if ylim is not None:
+ ax.set_ylim(ylim)
+ return fig, ax
+
+
+def draw_adm_ker(clusters, p, pg, xlim=None, ylim=None):
+ valid_adm_ker_cls = [cl for cl in clusters if not (cl.admissible_kernel is None or cl.admissible_kernel.is_empty)]
+ n_col = int(np.sqrt(len(valid_adm_ker_cls))) + 1
+ fig, axs = plt.subplots(n_col, n_col)
+ for i, cl in enumerate(valid_adm_ker_cls):
+ ax_i = axs[i//n_col, i%n_col]
+ [o.draw(ax=ax_i, show_name=1, show_reference=0, fc='lightgrey', ec='k', alpha=0.8) for o in cl.obstacles]
+ if not cl.admissible_kernel.geom_type == 'Point':
+ draw_shapely_polygon(cl.admissible_kernel, ax=ax_i, fc='y', alpha=0.3)
+ if cl.kernel_points is not None:
+ draw_shapely_polygon(shapely.geometry.Polygon(cl.kernel_points), ax=ax_i, fc='g', alpha=0.6)
+ ax_i.plot(*p, 'ko')
+ ax_i.plot(*pg, 'k*')
+ if xlim is not None:
+ ax_i.set_xlim(xlim)
+ if ylim is not None:
+ ax_i.set_ylim(ylim)
+ return fig, axs
diff --git a/python/ur_simple_control/path_generation/starworlds/starshaped_hull/starshaped_hull.py b/python/ur_simple_control/path_generation/starworlds/starshaped_hull/starshaped_hull.py
new file mode 100644
index 0000000000000000000000000000000000000000..8fde482f9edaf717ffcb82f170b9bacc158c3714
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/starshaped_hull/starshaped_hull.py
@@ -0,0 +1,207 @@
+import shapely
+import numpy as np
+from obstacles import Polygon
+from utils import is_ccw, is_cw, line, ray, Cone, convex_hull
+import matplotlib.pyplot as plt
+
+
+def admissible_kernel(obstacle, x):
+ # Find tangents of obstacle through x
+ tps = obstacle.tangent_points(x)
+ if not tps:
+ # Interior point
+ return None
+ return Cone(x, x-tps[0], x-tps[1])
+
+
+# Computes the starshaped hull of a list of obstacles for specified kernel points
+def kernel_starshaped_hull(obstacles, kernel_points):
+ if not type(obstacles) is list:
+ if obstacles.is_convex():
+ return convex_kernel_starshaped_hull(obstacles, kernel_points)
+ if issubclass(obstacles.__class__, Polygon):
+ return polygon_kernel_starshaped_hull(obstacles.polygon(), kernel_points)
+ else:
+ print("[kernel_starshaped_hull]: Bad obstacle class.")
+ print(obstacles)
+
+ sub_pols = [kernel_starshaped_hull(o, kernel_points) for o in obstacles]
+ hull_polygon = shapely.ops.unary_union(sub_pols)
+ if hull_polygon.is_empty:
+ return None
+ return hull_polygon
+
+
+def convex_kernel_starshaped_hull(convex_obstacle, kernel_points):
+ tps = []
+ for k in kernel_points:
+ tps += convex_obstacle.tangent_points(k)
+ if not tps:
+ return shapely.geometry.Polygon([])
+
+ tps = np.unique(tps,axis=0)
+ ch_points = np.vstack((tps, kernel_points))
+ pol = convex_hull(ch_points)
+ return shapely.geometry.Polygon(pol)
+
+
+# TODO: Improve computational consideration
+def polygon_kernel_starshaped_hull(polygon, kernel_points, debug=0):
+ kernel_points = kernel_points.reshape((kernel_points.size//2, 2))
+
+ if kernel_points.shape[0] > 2:
+ # NOTE: Assumes kernel points convex
+ # convex_kernel_subset = shapely.geometry.Polygon(kernel_points[ConvexHull(kernel_points).vertices, :])
+ convex_kernel_subset = shapely.geometry.Polygon(kernel_points)
+
+ # polygon_sh = polygon.polygon() # Shapely represenation of polygon
+ # vertices = np.asarray(polygon_sh.exterior.coords)[:-1, :] # Vertices of polygon
+ vertices = np.asarray(polygon.exterior.coords)[:-1, :] # Vertices of polygon
+ star_vertices = [] # Vertices of starshaped hull polygon
+ v_bar = kernel_points[0].copy() # Last vertex of starshaped hull polygon
+ e1_idx = 0
+ e2_idx = 0
+ k_centroid = np.mean(kernel_points, axis=0)
+ k_included = [False] * kernel_points.shape[0]
+
+ # Arrange vertices such that v_1 is the one with largest x-value and vendv1v2 is CCW, (assumes no collinear vertices in P)
+ start_idx = np.argmax(vertices[:, 0])
+ vertices = np.roll(vertices, -start_idx, axis=0)
+ if is_cw(vertices[-1], vertices[0], vertices[1]):
+ vertices = np.flip(vertices, axis=0)
+ vertices = np.roll(vertices, 1, axis=0)
+ # print("Initial sort: {:.1f}".format(toc(t0)))
+
+ # Iterate through all vertices
+ for v_idx, v in enumerate(vertices):
+ adjust_e1 = False
+ # Check if no ray r(v,kv) intersects with interior of polygon
+ if all([ray(v, k, v).disjoint(polygon) for k in kernel_points]):
+ # Add current vertex
+ if kernel_points.shape[0] < 3 or not convex_kernel_subset.contains(shapely.geometry.Point(v)):
+ star_vertices += [v]
+ if star_vertices:
+ # Intersections of lines l(k,v) and l(e1,e2)
+ e1, e2 = star_vertices[e1_idx], star_vertices[e2_idx]
+ e1_e2 = line(e1, e2)
+
+ for k in kernel_points:
+ kv_e1e2_intersect = line(k,v).intersection(e1_e2)
+ # Adjust to closest intersection to e2
+ if not kv_e1e2_intersect.is_empty:
+ adjust_e1 = True
+ e1_candidate = np.array([kv_e1e2_intersect.x, kv_e1e2_intersect.y])
+ if np.linalg.norm(e2 - e1_candidate) < np.linalg.norm(e2 - star_vertices[e1_idx]):
+ star_vertices[e1_idx] = e1_candidate
+
+ if not adjust_e1:
+ for k_idx, k in enumerate(kernel_points):
+ kps = [kp for kp in kernel_points if not np.array_equal(kp, k)]
+ kv_P_intersect = line(k, v).intersection(polygon)
+
+ # If l(k,v) intersects interior of P
+ if not kv_P_intersect.is_empty:
+ # Find last intersection of l(k,v) and polygon boundary
+ if kv_P_intersect.geom_type == 'LineString':
+ intersection_points = [np.array([ip[0], ip[1]]) for ip in kv_P_intersect.coords]
+ elif kv_P_intersect.geom_type == 'MultiLineString':
+ intersection_points = [np.array([ip[0], ip[1]]) for l in kv_P_intersect.geoms for ip in
+ l.coords]
+ elif kv_P_intersect.geom_type == 'GeometryCollection':
+ intersection_points = []
+ for g in kv_P_intersect.geoms:
+ if g.geom_type == 'Point':
+ intersection_points += [np.array(g.coords[0])]
+ if kv_P_intersect.geom_type == 'LineString':
+ intersection_points += [np.array([ip[0], ip[1]]) for ip in g.coords]
+ else:
+ intersection_points = []
+
+ u = None
+ u_v = None
+ for u_candidate in intersection_points:
+ u_v = line(u_candidate, v)
+ if u_v.disjoint(polygon):
+ u = u_candidate
+ break
+ if u is None:
+ continue
+
+ # If no ray r(u,k'v) intersect with interior of polygon
+ if not any([ray(u, kp, v).intersects(polygon) for kp in kps]):
+ # Adjust u if l(k',v_bar) intersects l(u,v)
+ for kp in kps:
+ kpvb_uv_intersect = line(kp, v_bar).intersection(u_v)
+ if not kpvb_uv_intersect.is_empty:
+ u = np.array([kpvb_uv_intersect.x, kpvb_uv_intersect.y])
+ # Append u to P*
+ star_vertices += [u]
+ # Update last augmented edge
+ e1_idx, e2_idx = len(star_vertices)-1, len(star_vertices)-2
+ # Swap last vertices if not CCW
+ if is_ccw(u, v, vertices[v_idx-1]):
+ # if is_ccw(v_bar, v, u):
+ # if is_cw(k_centroid, v, u):
+ star_vertices[-2], star_vertices[-1] = star_vertices[-1], star_vertices[-2]
+ e1_idx, e2_idx = e2_idx, e1_idx
+ adjust_e1 = True
+ else:
+ # Check if no ray r(k,k'v) intersect with interior of polygon
+ if (not k_included[k_idx]) and (not any([ray(k, kp, v).intersects(polygon) for kp in kps])):
+ k_included[k_idx] = True
+ # Append k to P*
+ star_vertices += [k]
+ # Update last augmented edge
+ e1_idx, e2_idx = len(star_vertices)-1, len(star_vertices)-2
+ # Swap last vertices if not CCW
+ if is_ccw(k, v, vertices[v_idx-1]):
+ # if is_cw(k_centroid, v, k):
+ star_vertices[-2], star_vertices[-1] = star_vertices[-1], star_vertices[-2]
+ e1_idx, e2_idx = e2_idx, e1_idx
+ adjust_e1 = True
+ # Update v_bar
+ v_bar = star_vertices[-1]
+
+ # Visualize debug information
+ if debug == 1:
+ plt.plot(*k_centroid, 'ko')
+ plt.plot(*polygon.exterior.xy, 'k')
+ plt.plot([p[0] for p in star_vertices], [p[1] for p in star_vertices], 'g-o', linewidth=2)
+ [plt.plot(*k, 'kx') for k in kernel_points]
+ [plt.plot(*line(k,v).xy, 'k--') for k in kernel_points]
+ if adjust_e1:
+ plt.plot(*star_vertices[e1_idx], 'ys')
+ plt.show()
+
+ # Check not added kernel points if they should be included
+ for j in range(len(star_vertices)):
+ v, vp = star_vertices[j - 1], star_vertices[j]
+ for k_idx, k in enumerate(kernel_points):
+ if (not k_included[k_idx]) and is_cw(k, v, vp):
+ k_included[k_idx] = True
+ # Insert k
+ star_vertices = star_vertices[:j] + [k] + star_vertices[j:]
+ # Visualize debug information
+ if debug == 1:
+ plt.plot(*k_centroid, 'ko')
+ plt.plot(*polygon.exterior.xy, 'k')
+ plt.plot([p[0] for p in star_vertices], [p[1] for p in star_vertices], 'g-o', linewidth=2)
+ [plt.plot(*ki, 'kx') for ki in kernel_points]
+ plt.plot(*line(k, v).xy, 'r--*')
+ plt.plot(*line(k, vp).xy, 'r--*')
+ plt.plot(*k, 'go')
+ plt.show()
+ # print("Final kernel check: {:.1f}".format(toc(t0)))
+
+ if debug:
+ ax = plt.gca()
+ ax.plot(*polygon.exterior.xy, 'k')
+ ax.plot([p[0] for p in star_vertices] + [star_vertices[0][0]],
+ [p[1] for p in star_vertices] + [star_vertices[0][1]], 'g-o', linewidth=2)
+ # [ax.plot(star_vertices[i][0], star_vertices[i][1], 'r*') for i in augmented_vertex_idcs]
+ [ax.plot(*zip(k, sv), 'y--') for sv in star_vertices for k in kernel_points]
+ ax.plot(*k_centroid, 'bs')
+ plt.show()
+
+ return shapely.geometry.Polygon(star_vertices)
+
diff --git a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/PKG-INFO b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/PKG-INFO
new file mode 100644
index 0000000000000000000000000000000000000000..4dee13fc6c070e01a3bca7b804984f6d9645b9f5
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/PKG-INFO
@@ -0,0 +1,9 @@
+Metadata-Version: 2.1
+Name: starworlds
+Version: 1.0
+License-File: LICENSE
+Requires-Dist: pyyaml
+Requires-Dist: numpy
+Requires-Dist: scipy
+Requires-Dist: matplotlib
+Requires-Dist: shapely
diff --git a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/SOURCES.txt b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/SOURCES.txt
new file mode 100644
index 0000000000000000000000000000000000000000..853123cb2e00b78f079beab8e29ceb9251e378d7
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/SOURCES.txt
@@ -0,0 +1,29 @@
+.gitignore
+LICENSE
+README.md
+requirements.txt
+setup.py
+obstacles/__init__.py
+obstacles/ellipse.py
+obstacles/motion_model.py
+obstacles/obstacle.py
+obstacles/polygon.py
+obstacles/starshaped_obstacle.py
+obstacles/starshaped_polygon.py
+obstacles/starshaped_primitive_combination.py
+starshaped_hull/__init__.py
+starshaped_hull/cluster_and_starify.py
+starshaped_hull/starshaped_hull.py
+starworlds.egg-info/PKG-INFO
+starworlds.egg-info/SOURCES.txt
+starworlds.egg-info/dependency_links.txt
+starworlds.egg-info/requires.txt
+starworlds.egg-info/top_level.txt
+tests/test_cluster_and_starify.py
+tests/test_obstacles.py
+tests/test_starshaped_hull.py
+tests/test_timing.py
+tests/test_utils.py
+utils/__init__.py
+utils/cg.py
+utils/misc.py
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/dependency_links.txt b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/dependency_links.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8b137891791fe96927ad78e64b0aad7bded08bdc
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/dependency_links.txt
@@ -0,0 +1 @@
+
diff --git a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/requires.txt b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/requires.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f849890421afe2baa719e8822ed041b4c0ea68da
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/requires.txt
@@ -0,0 +1,5 @@
+pyyaml
+numpy
+scipy
+matplotlib
+shapely
diff --git a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/top_level.txt b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/top_level.txt
new file mode 100644
index 0000000000000000000000000000000000000000..35cb7de772c37f5866607798fbfd9e7e835bff72
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/top_level.txt
@@ -0,0 +1,3 @@
+obstacles
+starshaped_hull
+utils
diff --git a/python/ur_simple_control/path_generation/starworlds/tests/test_cluster_and_starify.py b/python/ur_simple_control/path_generation/starworlds/tests/test_cluster_and_starify.py
new file mode 100644
index 0000000000000000000000000000000000000000..0461e20f4aaa1b680a8ce87348f9b1700086117f
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/tests/test_cluster_and_starify.py
@@ -0,0 +1,129 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from obstacles import Ellipse, StarshapedPolygon
+from obstacles import motion_model as mm
+from utils import generate_convex_polygon, draw_shapely_polygon
+from starshaped_hull import cluster_and_starify, draw_clustering, draw_adm_ker, draw_star_hull
+import shapely
+
+
+def test_cluster_and_starify():
+ n_obstacles = 40
+ ellipse_fraction = .5
+ ell_radius_mean, ell_radius_std = 1, 0.2
+ n_vertices, pol_box = 8, [2, 2]
+ target_scene_coverage = 0.3
+ epsilon = 0.2
+
+ np.random.seed(0)
+
+ def random_scene_point(scene_width):
+ return np.random.rand(2) * scene_width
+
+ def select_x_xg(scene_width, obstacles):
+ x = random_scene_point(scene_width)
+ while any([o.interior_point(x) for o in obstacles]):
+ x = random_scene_point(scene_width)
+ xg = random_scene_point(scene_width)
+ while any([o.interior_point(xg) for o in obstacles]):
+ xg = random_scene_point(scene_width)
+ return x, xg
+
+ # Generate obstacles
+ Nell = int(n_obstacles * ellipse_fraction)
+ Npol = n_obstacles - Nell
+ obstacles = [Ellipse(a=np.random.normal(ell_radius_mean, ell_radius_std, 2), n_pol=10) for j in range(Nell)]
+ obstacles += [StarshapedPolygon(generate_convex_polygon(n_vertices, pol_box), xr=[0, 0], is_convex=True) for j in range(Npol)]
+
+ # Compute area data
+ obstacle_area = sum([o.area() for o in obstacles])
+
+ # Setup scene
+ scene_width = np.sqrt(obstacle_area / target_scene_coverage * 0.9)
+
+ for j in range(Nell):
+ # obstacles[j].set_motion_model(ob.Static(random_scene_point(res['scene_width'][i])))
+ obstacles[j].set_motion_model(mm.Static(random_scene_point(scene_width - 2 * ell_radius_mean) + ell_radius_mean))
+ for j in range(Npol):
+ # obstacles[Nell+j].set_motion_model(ob.Static(random_scene_point(res['scene_width'][i])))
+ obstacles[Nell + j].set_motion_model(mm.Static(random_scene_point(scene_width - pol_box[0]) + pol_box[0] / 2))
+ [obs.polygon() for obs in obstacles]
+
+ # Select collision free robot and goal positions
+ x, xg = select_x_xg(scene_width, obstacles)
+ # Cluster and starify
+ clusters, timing, flag, n_iter, cluster_history = cluster_and_starify(obstacles, x, xg, epsilon, make_convex=0,
+ verbose=1, return_history=1, timing_verbose=1)
+
+ # Draw iteration steps
+ for i, clusters_i in enumerate(cluster_history[1:]):
+ _, ax = draw_clustering(clusters_i, x, xg, xlim=[0, scene_width], ylim=[0, scene_width])
+ ax.set_title("Clustering, Iteration {}/{}".format(i+1, len(cluster_history)))
+ fig, axs = draw_adm_ker(clusters_i, x, xg, xlim=[-0.2*scene_width, 1.2*scene_width], ylim=[-0.2*scene_width, 1.2*scene_width])
+ fig.suptitle("Admissible Kernel, Iteration {}/{}".format(i+1, len(cluster_history)))
+ _, ax = draw_star_hull(clusters_i, x, xg, xlim=[0, scene_width], ylim=[0, scene_width])
+ ax.set_title("Starshaped Hull, Iteration {}/{}".format(i+1, len(cluster_history)))
+
+
+def test_moving_cluster():
+ obstacles = [
+ Ellipse([1, 1], motion_model=mm.Static([-1, 0.3])),
+ Ellipse([1, 1], motion_model=mm.Static([0., 0.3])),
+ Ellipse([1, 1], motion_model=mm.Static([1, 0.3]))
+ ]
+ p0 = np.array([0.1, -2.5])
+ pg = np.array([0.1, 2.5])
+ epsilon = 0.2
+ xlim = [-3, 3]
+ ylim = [-3, 3]
+
+ clusters, timing, flag, n_iter = cluster_and_starify(obstacles, p0, pg, epsilon)
+
+ fig, axs = plt.subplots(1, 3)
+ [o.draw(ax=axs[0], show_reference=0, fc='lightgrey', ec='k', alpha=0.8) for o in obstacles]
+ axs[0].plot(*p0, 'ko')
+ axs[0].plot(*pg, 'k*')
+ axs[0].plot(*clusters[0].cluster_obstacle.xr(), 'gd')
+ axs[0].plot(*zip(p0, pg), '--')
+ axs[0].set_xlim(xlim)
+ axs[0].set_ylim(ylim)
+
+ xr_prev = clusters[0].cluster_obstacle.xr()
+ obstacles[0].set_motion_model(mm.Static([-1, -0.3]))
+ obstacles[1].set_motion_model(mm.Static([0., -0.3]))
+ obstacles[2].set_motion_model(mm.Static([1, -0.3]))
+ clusters, timing, flag, n_iter = cluster_and_starify(obstacles, p0, pg, epsilon, previous_clusters=clusters)
+
+ [o.draw(ax=axs[1], show_reference=0, fc='lightgrey', ec='k', alpha=0.8) for o in obstacles]
+ axs[1].plot(*p0, 'ko')
+ axs[1].plot(*pg, 'k*')
+ axs[1].plot(*xr_prev, 'sk')
+ axs[1].plot(*clusters[0].cluster_obstacle.xr(), 'gd')
+ axs[1].plot(*zip(p0, pg), '--')
+ axs[1].set_xlim(xlim)
+ axs[1].set_ylim(ylim)
+
+ xr_prev = clusters[0].cluster_obstacle.xr()
+ obstacles[0].set_motion_model(mm.Static([-1, -1]))
+ obstacles[1].set_motion_model(mm.Static([0., -1]))
+ obstacles[2].set_motion_model(mm.Static([1, -1]))
+ clusters, timing, flag, n_iter = cluster_and_starify(obstacles, p0, pg, epsilon, previous_clusters=clusters)
+
+ x_xg_line = shapely.geometry.LineString([p0, pg])
+ kernel_selection_set = shapely.ops.split(clusters[0].cluster_obstacle.polygon(), x_xg_line).geoms[0]
+
+ [o.draw(ax=axs[2], show_reference=0, fc='lightgrey', ec='k', alpha=0.8) for o in obstacles]
+ draw_shapely_polygon(kernel_selection_set, ax=axs[2], hatch='///', fill=False, linestyle='None')
+ axs[2].plot(*p0, 'ko')
+ axs[2].plot(*pg, 'k*')
+ axs[2].plot(*xr_prev, 'sk')
+ axs[2].plot(*clusters[0].cluster_obstacle.xr(), 'gd')
+ axs[2].plot(*zip(p0, pg), '--')
+ axs[2].set_xlim(xlim)
+ axs[2].set_ylim(ylim)
+
+
+if (__name__) == "__main__":
+ test_cluster_and_starify()
+ # test_moving_cluster()
+ plt.show()
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworlds/tests/test_obstacles.py b/python/ur_simple_control/path_generation/starworlds/tests/test_obstacles.py
new file mode 100644
index 0000000000000000000000000000000000000000..2839f4036d8d58afac21a42b6d0c7daf42e4c8b4
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/tests/test_obstacles.py
@@ -0,0 +1,147 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from obstacles import Ellipse, Polygon, StarshapedPolygon, StarshapedPrimitiveCombination, Frame
+from obstacles import motion_model as mm
+from utils import generate_convex_polygon, draw_shapely_polygon, generate_star_polygon
+import shapely.geometry
+
+
+def test_ellipse():
+ ell_axes = [2, 1]
+ ell_pos = [0, 0.5]
+ xlim = [ell_pos[0] - 2 * ell_axes[0], ell_pos[0] + 2 * ell_axes[0]]
+ ylim = [ell_pos[1] - 2 * ell_axes[1], ell_pos[1] + 2 * ell_axes[1]]
+ ell = Ellipse(ell_axes, xr=[0, .9], motion_model=mm.Static(ell_pos, 1))
+ while True:
+ x = np.array([np.random.uniform(*xlim),np.random.uniform(*ylim)])
+ if ell.exterior_point(x):
+ break
+ b = ell.boundary_mapping(x)
+ n = ell.normal(x)
+ tp = ell.tangent_points(x)
+ dir = ell.reference_direction(x)
+
+ _, ax = ell.draw()
+ ax.plot(*zip(ell.xr(Frame.GLOBAL), x), 'k--o')
+ if b is not None:
+ ax.plot(*b, 'y+')
+ ax.quiver(*b, *n)
+ if tp:
+ ax.plot(*zip(x, tp[0]), 'g:')
+ ax.plot(*zip(x, tp[1]), 'g:')
+ ax.quiver(*ell.xr(Frame.GLOBAL), *dir, color='c', zorder=3)
+ ax.set_xlim(xlim)
+ ax.set_ylim(ylim)
+
+
+def test_nonstar_polygon():
+ pass
+
+
+def test_star_polygon():
+ avg_radius = 1
+ xlim = [-2*avg_radius, 2*avg_radius]
+ ylim = xlim
+ pol = StarshapedPolygon(generate_star_polygon([0, 0], avg_radius, irregularity=0.3, spikiness=0.5, num_vertices=10))
+
+ while True:
+ x = np.array([np.random.uniform(*xlim), np.random.uniform(*ylim)])
+ if pol.exterior_point(x):
+ break
+ b = pol.boundary_mapping(x)
+ n = pol.normal(x)
+ tp = pol.tangent_points(x)
+ dir = pol.reference_direction(x)
+
+ _, ax = pol.draw()
+ ax.plot(*zip(pol.xr(Frame.GLOBAL), x), 'k--o')
+ if b is not None:
+ ax.plot(*b, 'y+')
+ ax.quiver(*b, *n)
+ if tp:
+ ax.plot(*zip(x, tp[0]), 'g:')
+ ax.plot(*zip(x, tp[1]), 'g:')
+ ax.quiver(*pol.xr(Frame.GLOBAL), *dir, color='c', zorder=3)
+
+ for i in np.linspace(0, 2 * np.pi, 100):
+ x = pol.xr() + 100*np.array([np.cos(i), np.sin(i)])
+ b = pol.boundary_mapping(x)
+ n = pol.normal(b)
+ ax.quiver(*b, *n)
+ ax.set_xlim(xlim)
+ ax.set_ylim(ylim)
+ print("es")
+
+def test_star_primitive_combination():
+ n_ellipses = 3
+ n_polygons = 2
+ n_vertices = 6
+ box_width = 2
+
+ ell_axes = [0.7, 0.4]
+ pol_box = [box_width, box_width]
+
+ xlim = [-box_width, box_width]
+ ylim = [-box_width, box_width]
+ ellipses = [Ellipse(ell_axes) for i in range(n_ellipses)]
+ polygons = [StarshapedPolygon(generate_convex_polygon(n_vertices, pol_box), is_convex=True)
+ for i in range(n_polygons)]
+ obstacles = ellipses + polygons
+ while True:
+ # Generate new positions
+ for obs in obstacles:
+ obs.set_motion_model(mm.Static(np.random.uniform(-0.2*box_width, 0.2*box_width, 2), np.random.uniform(0, 2*np.pi)))
+
+ # Identify if all obstacle form a single cluster
+ kernel = obstacles[0].polygon()
+ for obs in obstacles[1:]:
+ kernel = kernel.intersection(obs.polygon())
+
+ if not kernel.is_empty:
+ break
+ else:
+ _, ax = plt.subplots()
+ [obs.draw(ax=ax, fc='r', alpha=0.2, ec='k') for obs in obstacles]
+ ax.set_xlim(xlim)
+ ax.set_ylim(ylim)
+ plt.show()
+
+ xr = np.array(kernel.representative_point().coords[0])
+ star_obs = StarshapedPrimitiveCombination(obstacles, shapely.geometry.Polygon([]), xr)
+
+ while True:
+ x = np.array([np.random.uniform(*xlim), np.random.uniform(*ylim)])
+ if star_obs.exterior_point(x):
+ break
+ b = star_obs.boundary_mapping(x)
+ n = star_obs.normal(x)
+ tp = star_obs.tangent_points(x)
+ dir = star_obs.reference_direction(x)
+
+ _, ax = star_obs.draw()
+ draw_shapely_polygon(kernel, ax=ax, fc='g')
+ ax.plot(*zip(star_obs.xr(Frame.GLOBAL), x), 'k--o')
+ if b is not None:
+ ax.plot(*b, 'y+')
+ ax.quiver(*b, *n)
+ if tp:
+ ax.plot(*zip(x, tp[0]), 'g:')
+ ax.plot(*zip(x, tp[1]), 'g:')
+ ax.quiver(*star_obs.xr(Frame.GLOBAL), *dir, color='c', zorder=3)
+
+ for i in np.linspace(0, 2 * np.pi, 100):
+ x = star_obs.xr() + np.array([np.cos(i), np.sin(i)])
+ b = star_obs.boundary_mapping(x)
+ n = star_obs.normal(b)
+ # ax.quiver(*b, *n)
+
+ ax.set_xlim(xlim)
+ ax.set_ylim(ylim)
+
+
+if (__name__) == "__main__":
+ # test_ellipse()
+ # test_nonstar_polygon()
+ # test_star_polygon()
+ test_star_primitive_combination()
+ plt.show()
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworlds/tests/test_starshaped_hull.py b/python/ur_simple_control/path_generation/starworlds/tests/test_starshaped_hull.py
new file mode 100644
index 0000000000000000000000000000000000000000..670a63616cdea33d59d825bb4067a08765a0b00d
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/tests/test_starshaped_hull.py
@@ -0,0 +1,53 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from starshaped_hull import admissible_kernel
+from utils import draw_shapely_polygon, Cone
+from obstacles import Polygon, Ellipse
+from obstacles import motion_model as mm
+
+def test_admissible_kernel():
+ pol = Polygon([[0, 0], [1, 0], [2, 2], [2, 4], [-0.75, 1], [-1, 4], [-2, 2], [-2, 1], [-1.5, 0], [-2, -1]])
+
+ x_exclude = np.array([0, 2])
+
+ ad_ker = admissible_kernel(pol, x_exclude)
+
+ _, ax = pol.draw()
+ draw_shapely_polygon(ad_ker.polygon(), ax=ax, fc='y', alpha=0.6)
+ ax.plot(x_exclude[0], x_exclude[1], 'ro')
+ ax.set_xlim([-3, 6])
+ ax.set_ylim([-1, 5])
+
+
+def test_adm_ker_ellipse():
+ ell = Ellipse([1.08877265, 1.06673487], motion_model=mm.Static([4.61242385, 1.87941425]))
+ p = np.array([3.641344, 4.87955125])
+ pg = [0.73012219, 8.95180958]
+ adm_ker_p = admissible_kernel(ell, p)# Cone.list_intersection([adm_ker_robot_cones[o.id()] for o in cl.obstacles])
+ adm_ker_pg = admissible_kernel(ell, pg)
+ ad_ker = adm_ker_p.intersection(adm_ker_pg)
+
+ _, ax = ell.draw()
+ draw_shapely_polygon(ad_ker, ax=ax, fc='y', alpha=0.6)
+ draw_shapely_polygon(adm_ker_p.polygon(), ax=ax, fc='None', ec='k')
+ draw_shapely_polygon(adm_ker_pg.polygon(), ax=ax, fc='None', ec='k')
+ ax.plot(*p, 'rx')
+ ax.plot(*pg, 'rx')
+ ax.set_xlim(0, 10)
+ ax.set_ylim(0, 10)
+
+
+def test_kernel_starshaped_hull_single():
+ pass
+
+
+def test_kernel_starshaped_hull_cluster():
+ pass
+
+
+if (__name__) == "__main__":
+ test_admissible_kernel()
+ test_adm_ker_ellipse()
+ test_kernel_starshaped_hull_single()
+ test_kernel_starshaped_hull_cluster()
+ plt.show()
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworlds/tests/test_timing.py b/python/ur_simple_control/path_generation/starworlds/tests/test_timing.py
new file mode 100644
index 0000000000000000000000000000000000000000..068ff3bcb7db0fa8077c7586bf29ec78c75f1d8a
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/tests/test_timing.py
@@ -0,0 +1,173 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from obstacles import Ellipse, StarshapedPolygon
+from obstacles import motion_model as mm
+from utils import generate_convex_polygon
+from starshaped_hull import cluster_and_starify, draw_clustering, draw_adm_ker, draw_star_hull
+import shapely
+
+
+def test_cluster_and_starify_compute():
+ par = {'N_samples': 1000, 'pol_Nvert': 10, 'ell_n_pol': 30, 'ell_fraction': 0.5, 'pol_box': [2, 2],
+ 'ell_radius_mean': 1., 'ell_radius_std': 0.2, 'target_scene_coverage': 0.25,
+ 'No_min': 5, 'No_max': 50, 'rd_seed': 0, 'epsilon': 0.1}
+
+ plot_fail = 0
+ # ----- Data generation ------ #
+ np.random.seed(par['rd_seed'])
+ # Result data
+ res = {'compute_time': np.zeros((par['N_samples'], 4)), 'n_iter': np.zeros(par['N_samples'], dtype=np.int32),
+ 'No': np.zeros(par['N_samples'], dtype=np.int32), 'Ncl': np.zeros(par['N_samples'], dtype=np.int32),
+ 'obstacle_area': np.zeros(par['N_samples']), 'obstacle_coverage': np.zeros(par['N_samples']),
+ 'scene_width': np.zeros(par['N_samples']), 'scene_coverage': np.zeros(par['N_samples'])}
+
+ def random_scene_point(scene_width):
+ return np.random.rand(2) * scene_width
+
+ def select_x_xg(scene_width, obstacles):
+ x = random_scene_point(scene_width)
+ while any([o.interior_point(x) for o in obstacles]):
+ x = random_scene_point(scene_width)
+ xg = random_scene_point(scene_width)
+ while any([o.interior_point(xg) for o in obstacles]):
+ xg = random_scene_point(scene_width)
+ return x, xg
+
+ for i in range(par['N_samples']):
+ # Generate obstacles
+ res['No'][i] = np.random.randint(par['No_min'], par['No_max'] + 1)
+ Nell = int(res['No'][i] * par['ell_fraction'])
+ Npol = res['No'][i] - Nell
+ obstacles = [
+ Ellipse(a=np.random.normal(par['ell_radius_mean'], par['ell_radius_std'], 2), n_pol=par['ell_n_pol'])
+ for j in range(Nell)]
+ obstacles += [StarshapedPolygon(generate_convex_polygon(par['pol_Nvert'], par['pol_box']), xr=[0, 0],
+ is_convex=True) for j in range(Npol)]
+
+ # Compute area data
+ res['obstacle_area'][i] = sum([o.area() for o in obstacles])
+
+ # Setup scene
+ res['scene_width'][i] = np.sqrt(res['obstacle_area'][i] / par['target_scene_coverage'] * 0.85)
+
+ for j in range(Nell):
+ # obstacles[j].set_motion_model(ob.Static(random_scene_point(res['scene_width'][i])))
+ obstacles[j].set_motion_model(mm.Static(
+ random_scene_point(res['scene_width'][i] - 2 * par['ell_radius_mean']) + par['ell_radius_mean']))
+ for j in range(Npol):
+ # obstacles[Nell+j].set_motion_model(ob.Static(random_scene_point(res['scene_width'][i])))
+ obstacles[Nell + j].set_motion_model(
+ mm.Static(random_scene_point(res['scene_width'][i] - par['pol_box'][0]) + par['pol_box'][0] / 2))
+
+ # Compute coverage data
+ res['obstacle_coverage'][i] = shapely.ops.unary_union([o.polygon() for o in obstacles]).area
+ res['scene_coverage'][i] = res['obstacle_coverage'][i] / (res['scene_width'][i] ** 2)
+
+ flag = 0
+ n_failures = -1
+ while flag == 0:
+ n_failures += 1
+ # Select collision free robot and goal positions
+ x, xg = select_x_xg(res['scene_width'][i], obstacles)
+ # Cluster and starify
+ clusters, timing, flag, res['n_iter'][i] = cluster_and_starify(obstacles, x, xg, par['epsilon'],
+ exclude_obstacles=0, make_convex=0,
+ max_iterations=100,
+ verbose=0)
+
+ star_obstacles = [cl.cluster_obstacle for cl in clusters]
+ res['compute_time'][i, :] = timing
+ res['Ncl'][i] = len(clusters)
+
+ if plot_fail and flag == 0:
+ _, ax_i = plt.subplots()
+ [o.draw(ax=ax_i, fc='g', alpha=0.8) for o in star_obstacles]
+ [o.draw(ax=ax_i, show_name=1, show_reference=0, ec='k', linestyle='--') for o in obstacles]
+ ax_i.plot(*x, 'rx', markersize=16)
+ ax_i.plot(*xg, 'rx', markersize=16)
+ ax_i.set_xlim([0, res['scene_width'][i]])
+ ax_i.set_ylim([0, res['scene_width'][i]])
+ if flag == 0:
+ ax_i.set_title(
+ 'Fail\nSample: {}, #O: {}, #Cl: {}, Time: {:.1f}, It: {}, Scene coverage: {:.2f}({:.2f})'.format(
+ i, res['No'][i], res['Ncl'][i], sum(res['compute_time'][i, :]), res['n_iter'][i],
+ res['scene_coverage'][i], par['target_scene_coverage']))
+ else:
+ ax_i.set_title(
+ 'Sample: {}, #O: {}, #Cl: {}, Time: {:.1f}, It: {}, Scene coverage: {:.2f}({:.2f})'.format(
+ i, res['No'][i], res['Ncl'][i], sum(res['compute_time'][i, :]), res['n_iter'][i],
+ res['scene_coverage'][i], par['target_scene_coverage']))
+ plt.show()
+
+ if n_failures == 5:
+ break
+
+ # ----- Postprocessing ------ #
+ total_compute_time = np.sum(res['compute_time'], axis=1)
+ binc_niter = np.bincount(res['n_iter'])[min(res['n_iter']):]
+ print(np.vstack((np.arange(min(res['n_iter']), min(res['n_iter']) + len(binc_niter)), binc_niter)))
+
+ cl = ['r', 'g', 'b', 'y', 'k', 'c']
+ mk = ['o', '+', '^', 'x', 'd', '8']
+ tct_it = [None] * len(binc_niter)
+ No_it = [None] * len(binc_niter)
+ ct_it = [[]] * len(binc_niter)
+ scene_coverage_it = [None] * len(binc_niter)
+ for i in range(len(binc_niter)):
+ scene_coverage_it[i] = np.ma.masked_where(res['n_iter'] != min(res['n_iter']) + i, res['scene_coverage'])
+ tct_it[i] = np.ma.masked_where(res['n_iter'] != min(res['n_iter']) + i, total_compute_time)
+ No_it[i] = np.ma.masked_where(res['n_iter'] != min(res['n_iter']) + i, res['No'])
+ for j in range(3):
+ ma = np.ma.masked_where(res['n_iter'] != min(res['n_iter']) + i, res['compute_time'][:, j])
+ ct_it[i] += [ma.data]
+
+ timename = ['Clustering', 'Adm Ker', 'St Hull']
+ if max(res['No']) - min(res['No']) > 0:
+ _, axs = plt.subplots(3)
+ for j in range(3):
+ for i in range(len(binc_niter)):
+ axs[j].scatter(No_it[i], ct_it[i][j], c=cl[i], marker=mk[i])
+ axs[j].set_xlabel('No'), axs[j].set_ylabel('Time [ms]')
+ axs[j].set_title(timename[j])
+ plt.tight_layout()
+
+ _, axs = plt.subplots(1, 2)
+ # ax.scatter(res['No'], total_compute_time)
+ for i in range(len(binc_niter)):
+ axs[0].scatter(No_it[i], tct_it[i], c=cl[i], marker=mk[i], s=np.square(scene_coverage_it[i] * 20))
+ axs[1].scatter(scene_coverage_it[i], tct_it[i], c=cl[i], marker=mk[i], s=No_it[i] * 2)
+
+ _, ax = plt.subplots()
+ ax.scatter(res['No'], np.divide(res['compute_time'][:, 0], total_compute_time), color='r')
+ ax.scatter(res['No'], np.divide(res['compute_time'][:, 1], total_compute_time), color='g')
+ ax.scatter(res['No'], np.divide(res['compute_time'][:, 2], total_compute_time), color='b')
+ ax.set_title("Clustering fraction of total"), ax.set_xlabel('No'), ax.set_ylabel('Cl time / Tot time')
+
+ _, axs = plt.subplots(2, 2)
+ axs[0, 0].scatter(res['No'], res['obstacle_area'])
+ axs[0, 0].scatter(res['No'], res['obstacle_coverage']), axs[0, 0].set_title('Obstacle area')
+ axs[0, 0].set_xlabel('#Obstacles'), axs[1, 0].set_ylabel('Obstacle area')
+ axs[1, 0].scatter(res['scene_coverage'], res['n_iter']), axs[1, 0].set_title('#Iterations')
+ axs[1, 0].set_xlabel('Scene coverage'), axs[1, 0].set_ylabel('#Iterations')
+ axs[0, 1].scatter(res['No'], np.divide(res['Ncl'], res['No'])), axs[0, 1].set_title('#Clusters')
+ axs[0, 1].set_xlabel('#Obstacles'), axs[0, 1].set_ylabel('#Clusters/#Obstacles')
+ axs[1, 1].scatter(res['scene_coverage'], np.divide(res['Ncl'], res['No'])), axs[1, 1].set_title('#Clusters/#No')
+ axs[1, 1].set_xlabel('Scene coverage'), axs[1, 1].set_ylabel('#Clusters/#Obstacles')
+ plt.tight_layout()
+
+ scene_coverage = res['scene_coverage'] * 100
+ cov_span = max(scene_coverage) - min(scene_coverage)
+ binwidth = 1
+ _, ax = plt.subplots()
+ ax.hist(scene_coverage, bins=int(cov_span / binwidth),
+ color='blue', edgecolor='black')
+ ymin, ymax = ax.get_ylim()
+ ax.plot([par['target_scene_coverage'] * 100, par['target_scene_coverage'] * 100], [ymin, ymax], 'r--')
+ ax.set_title('Histogram with Binwidth = %d' % binwidth, size=30)
+ ax.set_xlabel('Coverage', size=22)
+ ax.set_ylabel('Samples', size=22)
+
+
+if (__name__) == "__main__":
+ test_cluster_and_starify_compute()
+ plt.show()
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworlds/tests/test_utils.py b/python/ur_simple_control/path_generation/starworlds/tests/test_utils.py
new file mode 100644
index 0000000000000000000000000000000000000000..54cac6a5e2d79d03452cf847b41117d1674dd59b
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/tests/test_utils.py
@@ -0,0 +1,106 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from utils import Cone, draw_shapely_polygon
+
+
+def test_affine_transformation():
+ pass
+
+
+def test_point_in_triangle():
+ pass
+
+
+def test_orientation():
+ pass
+
+
+def test_equilateral_triangle():
+ pass
+
+
+def test_convex_hull():
+ pass
+
+
+def test_generate_convex_polygon():
+ pass
+
+
+def test_generate_star_polygon():
+ pass
+
+
+def test_cone():
+ n_cones_sqrt = 4
+ n_cones_same_apex = 4
+ n_points = 8
+ n_several_cone_intersection = 2
+ boxlim = [-1, 1]
+
+ fig_int_sev, axs_int_sev = plt.subplots(n_cones_sqrt, n_cones_sqrt)
+ fig_int, axs_int = plt.subplots(n_cones_sqrt, n_cones_sqrt)
+ fig_p, axs_p = plt.subplots(n_cones_sqrt, n_cones_sqrt)
+ for i in range(n_cones_sqrt ** 2):
+ # Test cone intersection
+ c_params = np.random.uniform(*boxlim, (2, 3))
+ c1 = Cone(c_params[:, 0], c_params[:, 1] - c_params[:, 0], c_params[:, 2] - c_params[:, 0])
+ c2_params = np.random.uniform(*boxlim, (2, 3))
+ c2_apex = c_params[:, 0] if i < n_cones_same_apex else c2_params[:, 0]
+ c2 = Cone(c2_apex, c2_params[:, 1] - c2_apex, c2_params[:, 2] - c2_apex)
+ c_several_intersect = [c1, c2]
+ for j in range(1, n_several_cone_intersection):
+ c3_params = np.random.uniform(*boxlim, (2, 3))
+ c3_apex = c_params[:, 0] if i < n_cones_same_apex else c3_params[:, 0]
+ c_several_intersect += [Cone(c3_apex, c3_params[:, 1] - c3_apex, c3_params[:, 2] - c3_apex)]
+
+ two_cones_intersection = c1.intersection(c2)
+ several_cones_intersection = Cone.list_intersection(c_several_intersect, same_apex=i<n_cones_same_apex)
+
+ # Two cones intersection plot
+ fig_int.suptitle("Intersection of two cones")
+ ax_int_i = axs_int[i//n_cones_sqrt, i%n_cones_sqrt]
+ c1.draw(ax=ax_int_i, color='b', alpha=0.2, ray_color='b')
+ c2.draw(ax=ax_int_i, color='r', alpha=0.2, ray_color='r')
+ if not two_cones_intersection.is_empty and not two_cones_intersection.geom_type == 'Point':
+ draw_shapely_polygon(two_cones_intersection, ax=ax_int_i, color='k', alpha=0.2, hatch='///')
+ ax_int_i.set_xlim(1.2 * np.array(boxlim))
+ ax_int_i.set_ylim(1.2 * np.array(boxlim))
+
+ # Several cones intersection plot
+ fig_int_sev.suptitle("Intersection of several cones")
+ color = plt.cm.rainbow(np.linspace(0, 1, 2*n_several_cone_intersection))
+ ax_int_sev_i = axs_int_sev[i//n_cones_sqrt, i%n_cones_sqrt]
+ for j, c in enumerate(c_several_intersect):
+ c.draw(ax=ax_int_sev_i, color=color[j], alpha=0.2, ray_color=color[j])
+ if not several_cones_intersection.is_empty:
+ draw_shapely_polygon(several_cones_intersection, ax=ax_int_sev_i, color='k', alpha=0.2, hatch='///')
+ ax_int_sev_i.set_xlim(1.2 * np.array(boxlim))
+ ax_int_sev_i.set_ylim(1.2 * np.array(boxlim))
+
+ # Test point in cone
+ fig_p.suptitle("Point in cone")
+ ax_p_i = axs_p[i//n_cones_sqrt, i%n_cones_sqrt]
+ xs = np.linspace(*boxlim, n_points)
+ ys = np.linspace(*boxlim, n_points)
+ XS, YS = np.meshgrid(xs, ys)
+ c1.draw(ax=ax_p_i, color='b', alpha=0.2, ray_color='b')
+ for j in range(n_points):
+ for k in range(n_points):
+ col = 'g' if c1.point_in_cone([XS[j, k], YS[j, k]]) else 'r'
+ ax_p_i.plot(XS[j, k], YS[j, k], marker='.', color=col)
+ ax_p_i.set_xlim(1.2 * np.array(boxlim))
+ ax_p_i.set_ylim(1.2 * np.array(boxlim))
+
+
+if (__name__) == "__main__":
+ test_convex_hull()
+ test_orientation()
+ test_affine_transformation()
+ test_equilateral_triangle()
+ test_orientation()
+ test_generate_convex_polygon()
+ test_generate_star_polygon()
+ test_point_in_triangle()
+ test_cone()
+ plt.show()
diff --git a/python/ur_simple_control/path_generation/starworlds/utils/__init__.py b/python/ur_simple_control/path_generation/starworlds/utils/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..428e2f7fa52d681e0fec32febc1c69117e92bbb4
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/utils/__init__.py
@@ -0,0 +1,2 @@
+from .misc import *
+from .cg import *
diff --git a/python/ur_simple_control/path_generation/starworlds/utils/cg.py b/python/ur_simple_control/path_generation/starworlds/utils/cg.py
new file mode 100644
index 0000000000000000000000000000000000000000..bb7525d45bbaef9404d59fb15fa2c32d9e666f12
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/utils/cg.py
@@ -0,0 +1,593 @@
+import numpy as np
+import shapely.geometry
+import shapely.ops
+import matplotlib.pyplot as plt
+from typing import List, Tuple
+from utils import draw_shapely_polygon
+
+DEFAULT_RAY_INFINITY_LENGTH = 100000.
+COLLINEAR_THRESHOLD = 1e-10
+
+
+def affine_transform(x, rotation, translation, inverse=False):
+ if inverse:
+ x_t = [x[0] - translation[0], x[1] - translation[1]]
+ if rotation == 0:
+ return np.array(x_t)
+ c, s = np.cos(rotation), np.sin(rotation)
+ return np.array([c*x_t[0] + s*x_t[1], -s*x_t[0] + c*x_t[1]])
+ else:
+ if rotation != 0:
+ c, s = np.cos(rotation), np.sin(rotation)
+ return np.array([c*x[0]-s*x[1]+translation[0], s*x[0]+c*x[1]+translation[1]])
+ else:
+ return np.array([x[0]+translation[0], x[1]+translation[1]])
+
+
+# Line segment from a to b, excluding a and b
+def line(a, b):
+ return shapely.geometry.LineString([a + .0001 * (b - a), a + .9999 * (b - a)])
+
+
+# Random point on the triangle with vertices a, b and c
+def point_in_triangle(a, b, c):
+ """
+ """
+ x, y = np.random.rand(2)
+ q = abs(x - y)
+ s, t, u = q, 0.5 * (x + y - q), 1 - 0.5 * (q + x + y)
+ return s * a[0] + t * b[0] + u * c[0], s * a[1] + t * b[1] + u * c[1]
+
+# TODO: Dynamic length of ray
+# Ray emanating from a in direction of b->c
+def ray(a, b, c, ray_inf_length=DEFAULT_RAY_INFINITY_LENGTH):
+ return shapely.geometry.LineString([a + .0001 * (c - b), a + ray_inf_length * (c - b)])
+
+
+def np_orientation_val(a, b, c):
+ return (b[:, 1] - a[:, 1]) * (c[:, 0] - b[:, 0]) - (b[:, 0] - a[:, 0]) * (c[:, 1] - b[:, 1])
+
+
+def orientation_val(a, b, c):
+ return (b[1] - a[1]) * (c[0] - b[0]) - (b[0] - a[0]) * (c[1] - b[1])
+
+
+def is_collinear(a, b, c):
+ return abs(orientation_val(a, b, c)) < COLLINEAR_THRESHOLD
+
+
+def is_cw(a, b, c):
+ return orientation_val(a, b, c) > COLLINEAR_THRESHOLD
+
+
+def is_ccw(a, b, c):
+ return orientation_val(a, b, c) < -COLLINEAR_THRESHOLD
+
+
+# TODO: Use [NOT is_cw and NOT is_ccw] instead?
+# Returns true if point b is between point and b
+def is_between(a, b, c):
+ return np.isclose(np.linalg.norm(a-b) + np.linalg.norm(c-b), np.linalg.norm(a-c), rtol=1e-7)
+
+
+def intersect(a1, a2, b1, b2):
+ return (is_ccw(a1, b1, b2) != is_ccw(a2, b1, b2) and is_ccw(a1, a2, b1) != is_ccw(a1, a2, b2))
+
+
+class Point:
+
+ def __init__(self, x: float, y: float):
+ self.x = x
+ self.y = y
+ self.xy = [x, y]
+
+ def __str__(self):
+ return "Point {:s}".format(str(self.xy))
+
+ def __iter__(self):
+ return iter(self.xy)
+
+ def __getitem__(self, item):
+ return self.xy[item]
+
+ def draw(self, ax=None, marker='o', **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ handles = ax.plot(self.x, self.y, marker=marker, **kwargs)
+ return handles, ax
+
+
+class Line:
+
+ def __init__(self, p1: Point, p2: Point):
+ self.p1 = p1
+ self.p2 = p2
+
+ def __str__(self):
+ return "Line --({:.2f},{:.2f})--({:.2f},{:.2f})--".format(self.p1.x, self.p1.y, self.p2.x, self.p2.y)
+
+ def line_intersection(self, other: 'Line') -> Point:
+ self_dx = self.p1.x - self.p2.x
+ other_dx = other.p1.x - other.p2.x
+ self_dy = self.p1.y - self.p2.y
+ other_dy = other.p1.y - other.p2.y
+ den = self_dx * other_dy - self_dy * other_dx
+ if abs(den) < 1e-10:
+ # Parallel or coincident lines
+ return None
+ tmp1 = self.p1.x * self.p2.y - self.p1.y * self.p2.x
+ tmp2 = other.p1.x * other.p2.y - other.p1.y * other.p2.x
+ ip_x = (tmp1 * other_dx - self_dx * tmp2) / den
+ ip_y = (tmp1 * other_dy - self_dy * tmp2) / den
+ return Point(ip_x, ip_y)
+
+ def intersects(self, other):
+ return (is_ccw(self.p1, other.p1, other.p1) != is_ccw(self.p2, other.p1, other.p1) and is_ccw(self.p1, self.p2, other.p1) != is_ccw(self.p1, self.p2, other.p2))
+
+
+class LineSegment(Line):
+
+ def __str__(self):
+ return "Line segment ({:.2f},{:.2f})--({:.2f},{:.2f})".format(self.p1.x, self.p1.y, self.p2.x, self.p2.y)
+
+ def line_segment_intersection(self, other: 'LineSegment') -> Point:
+ self_dx = self.p1.x - self.p2.x
+ other_dx = other.p1.x - other.p2.x
+ self_dy = self.p1.y - self.p2.y
+ other_dy = other.p1.y - other.p2.y
+ p1_dx = self.p1.x - other.p1.x
+ p1_dy = self.p1.y - other.p1.y
+ den = self_dx * other_dy - self_dy * other_dx
+ if abs(den) < 1e-10:
+ # Parallel or coincident lines
+ return None
+ t = (p1_dx * other_dy - p1_dy * other_dx) / den
+ u = (p1_dx * self_dy - p1_dy * self_dx) / den
+ if t < 0 or t > 1 or u < 0 or u > 1:
+ return None
+ ip_x = self.p1.x - t * self_dx
+ ip_y = self.p1.y - t * self_dy
+ return Point(ip_x, ip_y)
+
+class Ray(Line):
+
+ def __str__(self):
+ return "Ray ({:.2f},{:.2f})--({:.2f},{:.2f})--".format(self.p1.x, self.p1.y, self.p2.x, self.p2.y)
+
+ def ray_intersection(self, other: 'Ray') -> Point:
+ self_dx = self.p1.x - self.p2.x
+ other_dx = other.p1.x - other.p2.x
+ self_dy = self.p1.y - self.p2.y
+ other_dy = other.p1.y - other.p2.y
+ p1_dx = self.p1.x - other.p1.x
+ p1_dy = self.p1.y - other.p1.y
+ den = self_dx * other_dy - self_dy * other_dx
+ if abs(den) < 1e-10:
+ # Parallel or coincident lines
+ return None
+ t = (p1_dx * other_dy - p1_dy * other_dx) / den
+ u = (p1_dx * self_dy - p1_dy * self_dx) / den
+ if t < 0 or u < 0:
+ return None
+ ip_x = self.p1.x - t * self_dx
+ ip_y = self.p1.y - t * self_dy
+ return Point(ip_x, ip_y)
+
+ def draw(self, ax=None, linestyle='--', color='k', markersize=16, **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ handles = ax.plot(*zip(self.p1, self.p2), linestyle=linestyle, color=color, **kwargs)
+ orient = np.rad2deg(np.arctan2(self.p2.y-self.p1.y, self.p2.x-self.p1.x))
+ handles += ax.plot(*self.p2, marker=(3, 0, orient-90), markersize=markersize, linestyle='None', color=color)
+ return handles, ax
+
+
+# Get intersection of line a and line b
+def get_intersection(a1, a2, b1, b2):
+ if not intersect(a1, a2, b1, b2):
+ if is_between(b1, a1, b2):
+ return a1
+ if is_between(b1, a2, b2):
+ return a2
+ if is_between(a1, b1, a2):
+ return b1
+ if is_between(a1, b2, a2):
+ return b2
+ return None
+ da = a2 - a1
+ db = b2 - b1
+ dp = a1 - b1
+ dap = np.array([-da[1], da[0]])
+ denom = np.dot(dap, db)
+ num = np.dot(dap, dp)
+ return (num / denom.astype(float)) * db + b1
+
+
+def equilateral_triangle(centroid, side_length, rot=0):
+ triangle = np.array(centroid) + np.array([[0, 1 / np.sqrt(3) * side_length],
+ [1 / 2 * side_length, -1 / (2*np.sqrt(3)) * side_length],
+ [-1 / 2 * side_length, -1 / (2*np.sqrt(3)) * side_length]])
+ if not rot:
+ return triangle
+ c,s = np.cos(rot), np.sin(rot)
+ return np.array([[c * triangle[0, 0] - s * triangle[0, 1], s * triangle[0, 0] + c * triangle[0, 1]],
+ [c * triangle[1, 0] - s * triangle[1, 1], s * triangle[1, 0] + c * triangle[1, 1]],
+ [c * triangle[2, 0] - s * triangle[2, 1], s * triangle[2, 0] + c * triangle[2, 1]]])
+
+
+def convex_hull(points):
+ n = len(points)
+
+ # Find the leftmost point
+ l = 0
+ for i in range(1, n):
+ if points[i][0] < points[l][0]:
+ l = i
+ elif points[i][0] == points[l][0]:
+ if points[i][1] > points[l][1]:
+ l = i
+
+ hull = []
+ '''
+ Start from leftmost point, keep moving counterclockwise
+ until reach the start point again. This loop runs O(h)
+ times where h is number of points in result or output.
+ '''
+ p = l
+ while (True):
+ # Add current point to result
+ hull.append([points[p][0], points[p][1]])
+
+ '''
+ Search for a point 'q' such that orientation(p, q,
+ x) is counterclockwise for all points 'x'. The idea
+ is to keep track of last visited most counterclock-
+ wise point in q. If any point 'i' is more counterclock-
+ wise than q, then update q.
+ '''
+ q = (p + 1) % n
+
+ for i in range(n):
+
+ # If i is more counterclockwise
+ # than current q, then update q
+ if is_ccw(points[p], points[i], points[q]):
+ q = i
+ '''
+ Now q is the most counterclockwise with respect to p
+ Set p as q for next iteration, so that q is added to
+ result 'hull'
+ '''
+ p = q
+
+ # While we don't come to first point
+ if (p == l):
+ break
+
+ return hull
+
+
+class Cone:
+ bb_width = 1e6
+ bottom_right = Point(bb_width, -bb_width)
+ top_right = Point(bb_width, bb_width)
+ bottom_left = Point(-bb_width, -bb_width)
+ top_left = Point(-bb_width, bb_width)
+ right_line = Line(bottom_right, top_right)
+ top_line = Line(top_left, top_right)
+ left_line = Line(top_left, bottom_left)
+ bottom_line = Line(bottom_left, bottom_right)
+ bb_edges = [bottom_line, right_line, top_line, left_line]
+ bb_corners = [bottom_right.xy, top_right.xy, top_left.xy, bottom_left.xy]
+ bb_corner_angles = np.pi / 4 * np.array([1, 3, 5, 7])
+
+ def __init__(self, apex, dir1, dir2):
+ self.apex = np.array(apex)
+ self.dir1 = np.array(dir1)
+ self.dir2 = np.array(dir2)
+ self.is_convex = is_ccw([0, 0], self.dir1, self.dir2)
+ self.ray1 = Ray(Point(*self.apex), Point(*(self.apex+self.dir1)))
+ self.ray2 = Ray(Point(*self.apex), Point(*(self.apex+self.dir2)))
+
+ def __str__(self):
+ return "Cone: ({:s}, {:s}, {:s})".format(str(self.apex), str(self.dir1), str(self.dir2))
+
+ def polygon(self) -> shapely.geometry.Polygon:
+
+ angle1 = np.arctan2(self.dir1[1], self.dir1[0]) + np.pi
+ angle2 = np.arctan2(self.dir2[1], self.dir2[0]) + np.pi
+
+ if Cone.bb_corner_angles[0] <= angle1 < Cone.bb_corner_angles[1]:
+ ray1_intersection_idx = 0
+ elif Cone.bb_corner_angles[1] <= angle1 < Cone.bb_corner_angles[2]:
+ ray1_intersection_idx = 1
+ elif Cone.bb_corner_angles[2] <= angle1 < Cone.bb_corner_angles[3]:
+ ray1_intersection_idx = 2
+ else:
+ ray1_intersection_idx = 3
+
+ if Cone.bb_corner_angles[0] <= angle2 < Cone.bb_corner_angles[1]:
+ ray2_intersection_idx = 0
+ elif Cone.bb_corner_angles[1] <= angle2 < Cone.bb_corner_angles[2]:
+ ray2_intersection_idx = 1
+ elif Cone.bb_corner_angles[2] <= angle2 < Cone.bb_corner_angles[3]:
+ ray2_intersection_idx = 2
+ else:
+ ray2_intersection_idx = 3
+
+ r1_border = self.ray1.ray_intersection(Cone.bb_edges[ray1_intersection_idx])
+ r2_border = self.ray2.ray_intersection(Cone.bb_edges[ray2_intersection_idx])
+ if r1_border is None:
+ ray1_intersection_idx = (ray1_intersection_idx + 1) % 4
+ r1_border = self.ray1.ray_intersection(Cone.bb_edges[ray1_intersection_idx])
+ if r1_border is None:
+ ray1_intersection_idx = (ray1_intersection_idx + 2) % 4
+ r1_border = self.ray1.ray_intersection(Cone.bb_edges[ray1_intersection_idx])
+ if r1_border is None:
+ print("SOMETHING WRONG!")
+ if r2_border is None:
+ ray2_intersection_idx = (ray2_intersection_idx + 1) % 4
+ r2_border = self.ray2.ray_intersection(Cone.bb_edges[ray2_intersection_idx])
+ if r2_border is None:
+ ray2_intersection_idx = (ray2_intersection_idx + 2) % 4
+ r2_border = self.ray2.ray_intersection(Cone.bb_edges[ray2_intersection_idx])
+ if r2_border is None:
+ print("SOMETHING WRONG!")
+
+ vertices = [self.apex, r1_border.xy]
+ c_idx = ray1_intersection_idx
+ if not (self.is_convex and (ray1_intersection_idx == ray2_intersection_idx)):
+ while True:
+ vertices += [Cone.bb_corners[c_idx]]
+ c_idx = (c_idx + 1) % 4
+ if c_idx == ray2_intersection_idx:
+ break
+ vertices += [r2_border.xy]
+
+ return shapely.geometry.Polygon(vertices)
+
+ def point_in_cone(self, x):
+ if self.is_convex:
+ return is_ccw(self.apex, self.ray1.p2, x) and is_cw(self.apex, self.ray2.p2, x)
+ else:
+ return not (is_ccw(self.apex, self.ray2.p2, x) and is_cw(self.apex, self.ray1.p2, x))
+
+ def intersection(self, other: 'Cone', same_apex: bool=False) -> shapely.geometry.Polygon:
+ if same_apex:
+ intersection_list = self.intersection_same_apex(other)
+ if len(intersection_list) == 1:
+ return intersection_list[0].polygon()
+ else:
+ return shapely.ops.unary_union([i.polygon() for i in intersection_list])
+ else:
+ return self.polygon().intersection(other.polygon())
+
+ def intersection_same_apex(self, other: 'Cone') -> List['Cone']:
+ other_dir1_in_self = self.point_in_cone(other.apex+other.dir1)
+ other_dir2_in_self = self.point_in_cone(other.apex+other.dir2)
+ self_dir1_in_other = other.point_in_cone(self.apex+self.dir1)
+
+ if other_dir1_in_self and other_dir2_in_self:
+ # Check several cones
+ if self_dir1_in_other:
+ return [Cone(self.apex, self.dir1, other.dir2),
+ Cone(self.apex, other.dir1, self.dir2)]
+ else:
+ return [other]
+ elif other_dir1_in_self:
+ return [Cone(self.apex, other.dir1, self.dir2)]
+ elif other_dir2_in_self:
+ return [Cone(self.apex, self.dir1, other.dir2)]
+ elif self_dir1_in_other:
+ return [self]
+ else:
+ return []
+
+ @staticmethod
+ def list_intersection(cones: List['Cone'], same_apex=False) -> shapely.geometry.Polygon:
+ if not same_apex:
+ intersection = cones[0].polygon()
+ for c in cones[1:]:
+ intersection = intersection.intersection(c.polygon())
+ return intersection
+
+ # List of cones
+ cones_intersect = [cones[0]]
+ for c in cones[1:]:
+ # Find intersection of current cones and next in list
+ cones_intersect_new = []
+ for i, ci in enumerate(cones_intersect):
+ cones_intersect_new += ci.intersection_same_apex(c)
+ cones_intersect = cones_intersect_new
+
+ # If empty intersection
+ if not cones_intersect:
+ return shapely.geometry.Polygon([])
+
+ if len(cones_intersect) == 1:
+ ret = cones_intersect[0].polygon()
+ return ret
+ else:
+ return shapely.ops.unary_union([c.polygon() for c in cones_intersect])
+
+ def draw(self, ax=None, ray_color='k', **kwargs):
+ handles, ax = draw_shapely_polygon(self.polygon(), ax=ax, **kwargs)
+ if ray_color is not None:
+ handles += ax.plot(*zip(self.apex, self.apex + Cone.bb_width * self.dir1), linestyle='--', color=ray_color)
+ handles += ax.plot(*zip(self.apex, self.apex + Cone.bb_width * self.dir2), linestyle='-', color=ray_color)
+ return handles, ax
+
+
+def generate_convex_polygon(n, box=None):
+ if box is None:
+ box = [1, 1]
+
+ # Generate two lists of random X and Y coordinates
+ xPool = [box[0]*np.random.uniform() for i in range(n)]
+ yPool = [box[0]*np.random.uniform() for i in range(n)]
+
+ # Sort them
+ xPool = np.sort(xPool)
+ yPool = np.sort(yPool)
+
+ # Isolate the extreme points
+ minX = xPool[0]
+ maxX = xPool[-1]
+ minY = yPool[0]
+ maxY = yPool[-1]
+
+ # Divide the interior points into two chains & Extract the vector components
+ lastTop = minX
+ lastBot = minX
+
+ xVec, yVec = [], []
+ for i in range(1, n-1):
+ if np.random.randint(2):
+ xVec += [xPool[i] - lastTop]
+ lastTop = xPool[i]
+ else:
+ xVec += [lastBot - xPool[i]]
+ lastBot = xPool[i]
+
+ xVec += [maxX - lastTop]
+ xVec += [lastBot - maxX]
+
+ lastLeft = minY
+ lastRight = minY
+
+ for i in range(1, n - 1):
+ if np.random.randint(2):
+ yVec += [yPool[i] - lastLeft]
+ lastLeft = yPool[i]
+ else:
+ yVec += [lastRight - yPool[i]]
+ lastRight = yPool[i]
+
+ yVec += [maxY - lastLeft]
+ yVec += [lastRight - maxY]
+
+ # Randomly pair up the X- and Y-components
+ np.random.shuffle(yVec)
+
+
+ # Combine the paired up components into vectors
+ # vec = [[xVec[i], yVec[i]] for i in range(n)]
+
+ # Sort the vectors by angle
+ angle = [np.arctan2(yVec[i], xVec[i]) for i in range(n)]
+ xVec = [x for _, x in sorted(zip(angle, xVec))]
+ yVec = [y for _, y in sorted(zip(angle, yVec))]
+
+ # Lay them end-to-end
+ x, y = 0, 0
+ minPolygonX, minPolygonY = 0, 0
+ points = []
+
+ for i in range(n):
+ points += [[x, y]]
+ x += xVec[i]
+ y += yVec[i]
+
+ minPolygonX = min(minPolygonX, x)
+ minPolygonY = min(minPolygonY, y)
+
+ xShift = minX - minPolygonX
+ yShift = minY - minPolygonY
+
+
+ # Move the polygon to have center in origin
+ for i in range(n):
+ points[i][0] += xShift - box[0]/2
+ points[i][1] += yShift - box[1]/2
+
+ xr = np.mean(points, axis=0)
+ for i in range(n):
+ points[i][0] -= xr[0]
+ points[i][1] -= xr[1]
+
+ if not shapely.geometry.box(-box[0],-box[1],box[0],box[1],).contains(shapely.geometry.Polygon(points)):
+ _, ax = plt.subplots()
+ draw_shapely_polygon(shapely.geometry.Polygon(points), ax=ax)
+ ax.set_xlim([-2*box[0], 2*box[0]])
+ ax.set_ylim([-2*box[1], 2*box[1]])
+ plt.show()
+
+ return points
+
+def generate_star_polygon(center: Tuple[float, float], avg_radius: float,
+ irregularity: float, spikiness: float,
+ num_vertices: int) -> List[Tuple[float, float]]:
+ """
+ Start with the center of the polygon at center, then creates the
+ polygon by sampling points on a circle around the center.
+ Random noise is added by varying the angular spacing between
+ sequential points, and by varying the radial distance of each
+ point from the centre.
+
+ Args:
+ center (Tuple[float, float]):
+ a pair representing the center of the circumference used
+ to generate the polygon.
+ avg_radius (float):
+ the average radius (distance of each generated vertex to
+ the center of the circumference) used to generate points
+ with a normal distribution.
+ irregularity (float):
+ variance of the spacing of the angles between consecutive
+ vertices.
+ spikiness (float):
+ variance of the distance of each vertex to the center of
+ the circumference.
+ num_vertices (int):
+ the number of vertices of the polygon.
+ Returns:
+ List[Tuple[float, float]]: list of vertices, in CCW order.
+ """
+ # Parameter check
+ if irregularity < 0 or irregularity > 1:
+ raise ValueError("Irregularity must be between 0 and 1.")
+ if spikiness < 0 or spikiness > 1:
+ raise ValueError("Spikiness must be between 0 and 1.")
+
+ irregularity *= 2 * np.pi / num_vertices
+ spikiness *= avg_radius
+ angle_steps = random_angle_steps(num_vertices, irregularity)
+
+ # now generate the points
+ points = []
+ angle = np.random.uniform(0, 2 * np.pi)
+ for i in range(num_vertices):
+ radius = np.clip(np.random.normal(avg_radius, spikiness), 0, 2 * avg_radius)
+ point = (center[0] + radius * np.cos(angle),
+ center[1] + radius * np.sin(angle))
+ points.append(point)
+ angle += angle_steps[i]
+
+ return points
+
+def random_angle_steps(steps: int, irregularity: float) -> List[float]:
+ """Generates the division of a circumference in random angles.
+
+ Args:
+ steps (int):
+ the number of angles to generate.
+ irregularity (float):
+ variance of the spacing of the angles between consecutive vertices.
+ Returns:
+ List[float]: the list of the random angles.
+ """
+ # generate n angle steps
+ angles = []
+ lower = (2 * np.pi / steps) - irregularity
+ upper = (2 * np.pi / steps) + irregularity
+ cumsum = 0
+ for i in range(steps):
+ angle = np.random.uniform(lower, upper)
+ angles.append(angle)
+ cumsum += angle
+
+ # normalize the steps so that point 0 and point n+1 are the same
+ cumsum /= (2 * np.pi)
+ for i in range(steps):
+ angles[i] /= cumsum
+ return angles
diff --git a/python/ur_simple_control/path_generation/starworlds/utils/misc.py b/python/ur_simple_control/path_generation/starworlds/utils/misc.py
new file mode 100644
index 0000000000000000000000000000000000000000..05e09b0376a01818bd92cee24da50dcb64d46f8c
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworlds/utils/misc.py
@@ -0,0 +1,51 @@
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+import time
+import inspect
+import traceback
+import shapely
+import numpy as np
+
+def tic():
+ return time.time()
+
+
+def toc(t0):
+ return (time.time()-t0) * 1000
+
+
+def draw_shapely_polygon(pol, ax=None, xlim=None, ylim=None, **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ pol_list = []
+ handles = []
+ if pol.geom_type == 'Polygon':
+ pol_list += [pol]
+ else:
+ for p in pol.geoms:
+ if p.geom_type == 'Polygon':
+ pol_list += [p]
+ for p in pol_list:
+ if xlim is not None and ylim is not None:
+ pol_plot = p.intersection(shapely.geometry.box(xlim[0] - 1, ylim[0] - 1, xlim[1] + 1, ylim[1] + 1))
+ else:
+ pol_plot = p
+ handles += [patches.Polygon(xy=np.vstack((pol_plot.exterior.xy[0], pol_plot.exterior.xy[1])).T, **kwargs)]
+ ax.add_patch(handles[-1])
+ if xlim is not None:
+ ax.set_xlim(xlim)
+ if ylim is not None:
+ ax.set_ylim(ylim)
+ return handles, ax
+
+
+def logprint(message=None, print_stack=0):
+ callerframerecord = inspect.stack()[1] # 0 represents this line
+ # 1 represents line at caller
+ frame = callerframerecord[0]
+ info = inspect.getframeinfo(frame)
+ if print_stack:
+ traceback.print_stack()
+ print(info.function + ", line: " + str(info.lineno))
+ if message:
+ print(message)