def test_dimension_error(self):
obstacles = [np.array([[2., 0], [2, 2], [3, 2], [3, 0]]).T]
bounds = irispy.Polyhedron.fromBounds([-1, -1], [3, 3])
seed_point = np.array([1.0, 2.0])
# this should pass
region = irispy.inflate_region(obstacles, seed_point, bounds)
# Now the obstacle is the wrong shape, so we should get a runtime error (but not a crash)
obstacles = [np.array([[2., 0], [2, 2], [3, 2], [3, 0]])]
try:
region = irispy.inflate_region(obstacles, seed_point, bounds)
self.assertTrue(False) # This should not have succeeded
except RuntimeError as e:
self.assertTrue(str(e) == "The matrix of obstacle vertices must have the same number of row as the dimension of the problem")
def test_dimension_error(self):
obstacles = [np.array([[2., 0], [2, 2], [3, 2], [3, 0]]).T]
bounds = irispy.Polyhedron.fromBounds([-1, -1], [3, 3])
seed_point = np.array([1.0, 2.0])
# this should pass
region = irispy.inflate_region(obstacles, seed_point, bounds)
# Now the obstacle is the wrong shape, so we should get a runtime error (but not a crash)
obstacles = [np.array([[2., 0], [2, 2], [3, 2], [3, 0]])]
try:
region = irispy.inflate_region(obstacles, seed_point, bounds)
self.assertTrue(False) # This should not have succeeded
except RuntimeError as e:
self.assertTrue(
str(e) ==
"The matrix of obstacle vertices must have the same number of row as the dimension of the problem"
)
def test_debug_data():
import matplotlib.pyplot as plt
obstacles = [np.array([[0.3, 0.5, 1.0, 1.0],
[0.1, 1.0, 1.0, 0.0]])]
bounds = irispy.Polyhedron()
bounds.setA(np.vstack((np.eye(2), -np.eye(2))))
bounds.setB(np.array([2.0, 2, 2, 2]))
start = [0.1, -0.05]
# print "running with debug"
region, debug = irispy.inflate_region(obstacles, start, bounds=bounds, return_debug_data=True)
# print "done"
debug.animate(pause=0.5, show=False)
def test_random_obstacles_2d(show=False):
bounds = irispy.Polyhedron.from_bounds([0, 0], [1, 1])
obstacles = []
for i in range(10):
center = np.random.random((2,))
scale = np.random.random() * 0.3
pts = np.random.random((2,4))
pts = pts - np.mean(pts, axis=1)[:,np.newaxis]
pts = scale * pts + center[:,np.newaxis]
obstacles.append(pts)
start = np.array([0.5, 0.5])
region, debug = irispy.inflate_region(obstacles, start, bounds=bounds, return_debug_data=True)
debug.animate(pause=0.5, show=show)
def test_required_containment(self):
obstacles = [np.array([[0., 1],
[0, 0]]),
np.array([[1., 1],
[0, 1]]),
np.array([[1., 0],
[1, 1]]),
np.array([[0., 0],
[1, 0]])]
required_containment_pts = [np.array([1.5, 1.5])]
start = np.array([0.1, 0.1])
region = irispy.inflate_region(obstacles, start,
require_containment=True,
required_containment_points=required_containment_pts)
self.assertTrue(region.polyhedron.getNumberOfConstraints() == 0, "polyhedron should be empty")
def test_required_containment(self):
obstacles = [
np.array([[0., 1], [0, 0]]),
np.array([[1., 1], [0, 1]]),
np.array([[1., 0], [1, 1]]),
np.array([[0., 0], [1, 0]])
]
required_containment_pts = [np.array([1.5, 1.5])]
start = np.array([0.1, 0.1])
region = irispy.inflate_region(
obstacles,
start,
require_containment=True,
required_containment_points=required_containment_pts)
self.assertTrue(region.polyhedron.getNumberOfConstraints() == 0,
"polyhedron should be empty")
def test_debug_data():
import matplotlib.pyplot as plt
obstacles = [np.array([[0.3, 0.5, 1.0, 1.0], [0.1, 1.0, 1.0, 0.0]])]
bounds = irispy.Polyhedron()
bounds.setA(np.vstack((np.eye(2), -np.eye(2))))
bounds.setB(np.array([2.0, 2, 2, 2]))
start = np.array([0.1, -0.05])
# print "running with debug"
region, debug = irispy.inflate_region(obstacles,
start,
bounds=bounds,
return_debug_data=True)
# print "done"
debug.animate(pause=0.5, show=False)
def test_box(self):
obstacles = [
np.array([[0.0, 1.0], [0.0, 0.0]]),
np.array([[1.0, 1.0], [0.0, 1.0]]),
np.array([[1.0, 0.0], [1.0, 1.0]]),
np.array([[0.0, 0.0], [1.0, 0.0]])
]
start = irispy.Ellipsoid.fromNSphere(np.array([0.1, 0.1]))
region = irispy.inflate_region(obstacles, start)
C = region.getEllipsoid().getC()
self.assertAlmostEqual(C[0, 0], 0.5, 3)
self.assertAlmostEqual(C[0, 1], 0.0, 3)
d = region.getEllipsoid().getD()
self.assertAlmostEqual(d[0], 0.5, 3)
self.assertAlmostEqual(d[1], 0.5, 3)
def test_point_start(self):
obstacles = [
np.array([[0.0, 1.0], [0.0, 0.0]]),
np.array([[1.0, 1.0], [0.0, 1.0]]),
np.array([[1.0, 0.0], [1.0, 1.0]]),
np.array([[0.0, 0.0], [1.0, 0.0]])
]
start = [0.1, 0.1]
region = irispy.inflate_region(obstacles, start)
C = region.getEllipsoid().getC()
self.assertAlmostEqual(C[0, 0], 0.5, 3)
self.assertAlmostEqual(C[0, 1], 0.0, 3)
d = region.getEllipsoid().getD()
self.assertAlmostEqual(d[0], 0.5, 3)
self.assertAlmostEqual(d[1], 0.5, 3)
def test_box(self):
obstacles = [np.array([[0.0, 1.0],
[0.0, 0.0]]),
np.array([[1.0, 1.0],
[0.0, 1.0]]),
np.array([[1.0, 0.0],
[1.0, 1.0]]),
np.array([[0.0, 0.0],
[1.0, 0.0]])
]
start = irispy.Ellipsoid.fromNSphere(np.array([0.1, 0.1]))
region = irispy.inflate_region(obstacles, start)
C = region.getEllipsoid().getC()
self.assertAlmostEqual(C[0,0], 0.5, 3)
self.assertAlmostEqual(C[0,1], 0.0, 3)
d = region.getEllipsoid().getD()
self.assertAlmostEqual(d[0], 0.5, 3)
self.assertAlmostEqual(d[1], 0.5, 3)
def test_point_start(self):
obstacles = [np.array([[0.0, 1.0],
[0.0, 0.0]]),
np.array([[1.0, 1.0],
[0.0, 1.0]]),
np.array([[1.0, 0.0],
[1.0, 1.0]]),
np.array([[0.0, 0.0],
[1.0, 0.0]])
]
start = [0.1, 0.1]
region = irispy.inflate_region(obstacles, start)
C = region.getEllipsoid().getC()
self.assertAlmostEqual(C[0,0], 0.5, 3)
self.assertAlmostEqual(C[0,1], 0.0, 3)
d = region.getEllipsoid().getD()
self.assertAlmostEqual(d[0], 0.5, 3)
self.assertAlmostEqual(d[1], 0.5, 3)
],
[
trans_x - (sz_x + RADIUS), trans_y - (sz_y + RADIUS),
trans_z - (sz_z + RADIUS)
],
[
trans_x - (sz_x + RADIUS), trans_y - (sz_y + RADIUS),
trans_z + (sz_z + RADIUS)
],
[
trans_x - (sz_x + RADIUS), trans_y + (sz_y + RADIUS),
trans_z + (sz_z + RADIUS)
],
]).T)
REGIONS = []
REGIONS.append(irispy.inflate_region(obstacles, np.array([3, 0, 0]),
bounds)) # in front
REGIONS.append(irispy.inflate_region(obstacles, np.array([-3, 0, 0]),
bounds)) # in back
REGIONS.append(irispy.inflate_region(obstacles, np.array([0, 3, 0]),
bounds)) # right
REGIONS.append(irispy.inflate_region(obstacles, np.array([0, -3, 0]),
bounds)) # left
REGIONS = [(r.polyhedron.getA(), r.polyhedron.getB()) for r in REGIONS]
START_STATE = np.zeros(12) #np.random.randn(12,)
START_STATE[0] = 4.0 # x = 4.0
GOAL_STATE = np.zeros(12)
GOAL_STATE[0] = -4.0 # x = -4.0
DURATION = 4
assert type(DURATION) is int
def refine_free_space(self,point_cloud,point_type,idx):
# when observing lidar points violating the free space, refine the free space
added = False
pts = [pt for pt,type in zip(point_cloud,point_type) if type==0]
violated_pts = []
# bloat the polytope back
bloated_poly = minkowski_shrink_poly(self.free_space[idx],-1.1*self.radius)
max_violation = 0
for pt in pts:
# pdb.set_trace()
iscontain, dis = bloated_poly.contain(pt)
if iscontain:
violated_pts.append(pt)
max_violation = max(max_violation,dis)
if max_violation>0.1*self.radius:
obstacles = grouping_point_cloud(violated_pts)
facets = bloated_poly.get_facet_vertices()
for facet in facets:
if facet:
# generate obstacles based on the facets of the previous polytope
m = self.dim+1-len(facet)
m = max(1,m)
new_obs = np.concatenate((np.array(facet),np.array(facet[0]+1e-4*np.random.random((m,self.dim)))))
obstacles.append(new_obs.transpose())
bounds = irispy.Polyhedron.from_bounds(self.env.lb, self.env.ub)
region = irispy.inflate_region(obstacles, self.pos, bounds=bounds, return_debug_data=False)
deleted_fs = self.free_space[idx]
self.delete_free_space(idx)
new_poly = polyhedron([region.polyhedron.getA(),region.polyhedron.getB()],'H')
new_poly_shrinked = minkowski_shrink_poly(new_poly,1.1*self.radius)
if new_poly_shrinked.R.shape[0]>0:
# returns unbounded polytopes, need to debug
pdb.set_trace()
if not self.free_space:
self.add_new_free_set(new_poly_shrinked)
else:
set_to_add = [new_poly_shrinked]
for fs in self.free_space.values():
for new_set in set_to_add:
H_set,sub_poly1,sub_poly2 = find_intersecting_hyperplane(fs,new_set)
if sub_poly2:
set_to_add.remove(new_set)
set_to_add = set_to_add + sub_poly2
for new_set in set_to_add:
if new_set.R.shape[0]>0:
pdb.set_trace()
if Hausdorff_distance(new_set,self.free_space)[0]>2*self.radius or new_set.contain(self.pos)[0]:
if not added:
added = True
self.add_new_free_set(new_set)
self.update_pot_wp(1.2*self.radius)
if added:
self.select_wp()
self.update_reachable_ap()
if not self.wp:
# no waypoint is returned, need debug
pdb.set_trace()
else:
# if no set is added, the refinement fails, then add back the original polytope and try next time step
self.add_new_free_set(deleted_fs)
self.update_pot_wp(1.2*self.radius)
return added
else:
# no violation, return []
return []
def extend_free_space(self,point_cloud,point_type,ap_points,ap_idx):
# use lidar scan to add new free spaces
obstacles = grouping_point_cloud(point_cloud)
potential_set = []
if self.free_space:
idx = self.determine_discrete_state()
dis_set = []
for pt in point_cloud: # choose the point in the point cloud that is far away from existing free space
dis_set.append(dis2bdry(self.free_space,pt)[0]+dis2bdry(self.free_space[idx],pt)[0])
dis_set = np.array(dis_set)
selected_pt = point_cloud[dis_set.argmin()]
print(selected_pt)
else:
selected_pt = self.pos
bounds = irispy.Polyhedron.from_bounds(self.env.lb, self.env.ub)
region = irispy.inflate_region(obstacles, 0.5*(self.pos+selected_pt), bounds=bounds, return_debug_data=False)
# call IRIS to compute the free space, with center chosen as the mid point between current position and the point selected from the point cloud
new_poly = polyhedron([region.polyhedron.getA(),region.polyhedron.getB()],'H')
# shrink the polytope with radius of the robot
new_poly_shrinked = minkowski_shrink_poly(new_poly,1.1*self.radius)
if new_poly_shrinked.contain(self.pos)[0]:
potential_set.append(new_poly_shrinked)
# repeat the same procedure with the center being the current position
region = irispy.inflate_region(obstacles, self.pos, bounds=bounds, return_debug_data=False)
new_poly = polyhedron([region.polyhedron.getA(),region.polyhedron.getB()],'H')
new_poly_shrinked = minkowski_shrink_poly(new_poly,1.1*self.radius)
potential_set.append(new_poly_shrinked)
# if any ap currently not reachable can be seen by the lidar, extend free space towards that direction
checked_ap = list(self.reachable_ap)
for i in range(0,len(ap_points)):
if ap_idx[i] not in checked_ap:
region = irispy.inflate_region(obstacles, ap_points[i], bounds=bounds, return_debug_data=False)
new_poly = polyhedron([region.polyhedron.getA(),region.polyhedron.getB()],'H')
new_poly_shrinked = minkowski_shrink_poly(new_poly,1.1*self.radius)
checked_ap.append(ap_idx[i])
if new_poly_shrinked.contain(self.pos)[0]:
potential_set.append(new_poly_shrinked)
# for all potential free spaces, remove the unnecessary intersections with existing free spaces and add some of them
for set in potential_set:
if not self.free_space:
self.add_new_free_set(set)
else:
set_to_add = [set]
for fs in self.free_space.values():
for new_set in set_to_add:
H_set,sub_poly1,sub_poly2 = find_intersecting_hyperplane(fs,new_set)
if sub_poly2:
set_to_add.remove(new_set)
set_to_add = set_to_add + sub_poly2
for new_set in set_to_add:
added = False
for prop in self.ap.keys():
if prop not in self.reachable_ap:
poly_sect = poly_intersect(self.ap[prop],new_set)
if not poly_sect.isempty: # if the new set intersects with an ap set that was not reachable, add it
if not added:
# seperate out the intersection, and add the original new set
self.add_new_free_set(new_set)
self.add_new_free_set(poly_sect)
idx = list(self.free_space.keys())[list(self.free_space.values()).index(poly_sect)]
self.label[idx].append(prop)
self.update_pot_wp(1.2*self.radius)
if Hausdorff_distance(new_set,self.free_space)[0]>3*self.radius:
# if there is substantial progress measured in Hausdorff distance, add it
if not added:
self.add_new_free_set(new_set)
self.update_pot_wp(1.2*self.radius)
for pt in point_cloud:
# add new potential waypoints
dis,free_space_idx = dis2bdry(self.free_space,pt)
if dis<=-self.radius:
# the potential way points are the projections of point cloud onto the free space
wp_projected = poly_projection(self.free_space[free_space_idx],pt,self.radius)[0]
self.pot_wp.append(way_point(wp_projected,dis,free_space_idx))
self.update_reachable_ap()
],
[
trans_x - (sz_x + RADIUS), trans_y - (sz_y + RADIUS),
trans_z - (sz_z + RADIUS)
],
[
trans_x - (sz_x + RADIUS), trans_y - (sz_y + RADIUS),
trans_z + (sz_z + RADIUS)
],
[
trans_x - (sz_x + RADIUS), trans_y + (sz_y + RADIUS),
trans_z + (sz_z + RADIUS)
],
]).T)
REGIONS = []
REGIONS.append(irispy.inflate_region(obstacles, np.array([0, 0, 0]),
bounds)) # in front
# REGIONS.append(irispy.inflate_region(obstacles, np.array([-3,0,0]), bounds)) # in back
# REGIONS.append(irispy.inflate_region(obstacles, np.array([0,3,0]), bounds)) # right
# REGIONS.append(irispy.inflate_region(obstacles, np.array([0,-3,0]), bounds)) # left
REGIONS = [(r.polyhedron.getA(), r.polyhedron.getB()) for r in REGIONS]
START_STATE = np.zeros(12) #np.random.randn(12,)
START_STATE[0] = 4.0 # x = 4.0
GOAL_STATE = np.zeros(12)
GOAL_STATE[0] = -4.0 # x = -4.0
DURATION = 4.0
dt = .1
n_segments = 1
degree = 3
