def create_map_from_world(self):
"""
Creates the occupancy grid (self.map) as a boolean numpy array. True is occupied, False is unoccupied.
This function is called during initialization of the object.
"""
bounds = self.world.world['bounds']['extents']
voxel_dimensions_metric = []
voxel_dimensions_indices = []
for i in range(3):
voxel_dimensions_metric.append(abs(bounds[1+i*2]-bounds[i*2]))
voxel_dimensions_indices.append(int(np.ceil(voxel_dimensions_metric[i]/self.resolution[i])))
# initialize the map with the correct dimensions as unoccupied
self.map = np.zeros(voxel_dimensions_indices, dtype=bool)
self.origin = np.array([bounds[0], bounds[2], bounds[4]])
# Create Rectangle objects from the obstacles to use for computing the configuration space
obstacle_rects = []
if 'blocks' in self.world.world:
for block in self.world.world['blocks']:
extent = block['extents']
obstacle_rects.append(Rectangle([extent[1], extent[3], extent[5]], [extent[0], extent[2], extent[4]]))
it = np.nditer(self.map, flags=['multi_index'])
# Iterate through every voxel in the map and check if it is too close to an obstacle. If so, mark occupied
while not it.finished:
metric_loc = self.index_to_metric_negative_corner(it.multi_index)
voxel_rectangle = Rectangle(metric_loc+self.resolution, metric_loc)
for obstacle in obstacle_rects:
rect_distance = voxel_rectangle.min_distance_rectangle(obstacle)
if rect_distance <= self.margin:
self.map[it.multi_index] = True
it.iternext()
def det_valid(pts,obsts,movObsts,spd):
lastPt = pts[0]
MO = []
for o, spdX, spdY in movObsts:
obst = Rectangle(maxes=o.maxes, mins=o.mins)
MO.append([obst, spdX, spdY])
for pt in pts:
for obst in obsts:
dist = obst.min_distance_point(pt)
if dist <= 0.2:
midPT = [(obst.maxes[0]+obst.mins[0])/2, (obst.maxes[1]+obst.mins[1])/2]
dist = distance.euclidean(midPT, pt)
return -(dist*4)+999
time = distance.euclidean(lastPt, pt)/spd
lastPt = pt
for obst, spdX, spdY in MO:
obst.maxes[0] += spdX*time
obst.mins[0] += spdX*time
obst.maxes[1] += spdY*time
obst.mins[1] += spdY*time
dist = obst.min_distance_point(pt)
if dist <= 0.5:
# print(time.__str__())
midPT = [(obst.maxes[0]+obst.mins[0])/2, (obst.maxes[1]+obst.mins[1])/2]
dist = distance.euclidean(midPT, pt)
return -(dist*4)+999
return 0
def test_contract_manager() -> None:
aut = AirspaceManager()
stop_ev = Event()
conn, manager_conn = Pipe()
p = Process(target=run_as_process, kwargs={"aut": aut,
"conn": manager_conn,
"stop_ev": stop_ev})
p.start()
try:
uid = 0
for i in range(10):
if conn.poll(1.0):
act = conn.recv()
print(act)
elif i % 3 != 2:
uid = i % 5
target = Contract.from_stamped_rectangles([
(0.0, Rectangle(mins=[0, 0, 0], maxes=[1, 1, 0.5])),
(0.5, Rectangle(mins=[0, 0.5, 0], maxes=[2, 3, 0.5])),
(1.0, Rectangle(mins=[0.5, 0.5, 1.0], maxes=[1.5, 1.5, 1.5]))
])
conn.send(("request", {"uid": uid, "target": target}))
else:
releasable = Contract.from_stamped_rectangles([
(0.0, Rectangle(mins=[0, 0, 0], maxes=[1, 1, 0.5])),
(0.5, Rectangle(mins=[0, 0.5, 0], maxes=[2, 2, 0.5]))
])
print("Agent " + str(uid) + " release > " + str(releasable))
conn.send(("release", {"uid": uid, "releasable": releasable}))
finally:
stop_ev.set() # Stop all automatons
p.join()
conn.close()
manager_conn.close()
def _init_map_from_world(self):
"""
Creates the occupancy grid (self.map) as a boolean numpy array. True is
occupied, False is unoccupied. This function is called during
initialization of the object.
"""
# Initialize the occupancy map, marking all free.
bounds = self.world.world['bounds']['extents']
voxel_dimensions_metric = []
voxel_dimensions_indices = []
for i in range(3):
voxel_dimensions_metric.append(
abs(bounds[1 + i * 2] - bounds[i * 2]))
voxel_dimensions_indices.append(
int(np.ceil(voxel_dimensions_metric[i] / self.resolution[i])))
self.map = np.zeros(voxel_dimensions_indices, dtype=bool)
self.origin = np.array(bounds[0::2])
# Iterate through each block obstacle.
for block in self.world.world.get('blocks', []):
extent = block['extents']
block_rect = Rectangle([extent[1], extent[3], extent[5]],
[extent[0], extent[2], extent[4]])
# Get index range that is definitely occupied by this block.
(inner_min_index,
inner_max_index) = self._metric_block_to_index_range(
extent, outer_bound=False)
a, b = inner_min_index, inner_max_index
self.map[a[0]:(b[0] + 1), a[1]:(b[1] + 1), a[2]:(b[2] + 1)] = True
# Get index range that is definitely not occupied by this block.
outer_extent = extent + self.margin * np.array(
[-1, 1, -1, 1, -1, 1])
(outer_min_index,
outer_max_index) = self._metric_block_to_index_range(
outer_extent, outer_bound=True)
# Iterate over uncertain voxels with rect-rect distance check.
for i in range(outer_min_index[0], outer_max_index[0] + 1):
for j in range(outer_min_index[1], outer_max_index[1] + 1):
for k in range(outer_min_index[2], outer_max_index[2] + 1):
# If map is not already occupied, check for collision.
if not self.map[i, j, k]:
metric_loc = self.index_to_metric_negative_corner(
(i, j, k))
voxel_rect = Rectangle(
metric_loc + self.resolution, metric_loc)
rect_distance = voxel_rect.min_distance_rectangle(
block_rect)
self.map[i, j, k] = rect_distance <= self.margin
def detectCol():
stt = fileHandler.loadJsonFromFile('state.json')
sps = fileHandler.loadJsonFromFile('ships.json')
mX = float(stt["lng"])
mY = float(stt["lat"])
mvX = float(stt["vx"])
mvY = float(stt["vy"])
colShips = []
for ship in sps["ships"]:
x = float(ship["lon"])
y = float(ship["lat"])
hed = float(ship["hed"]) * math.pi / 180
V = float(ship["sog"])
vx = V * math.cos(hed)
vy = V * math.sin(hed)
rx = x - mX
ry = y - mY
rvx = vx - mvX
rvy = vy - mvY
a = rvy / rvx
b = ry - a * rx
d = abs(b) / (a**2 + 1)**0.5
if d <= dMin:
obst = Rectangle(maxes=[x + dX, y + dY], mins=[x - dX, y - dY])
colShips.append([obst, vx, vy])
return colShips
def _extend_contract_from_reachtube(cls,
plan: List[StampedRect],
key: str,
t_start: float = 0.0) -> float:
"""
Extend the given plan with rectangles from the reachtube under the given key.
Parameters
----------
plan
key
t_start
Returns
-------
float
the timestamp where the last rectangle should still hold.
"""
SUBSAMPLE_STEP = 40
t_ned_arr = cls.CONTRACT_DICT[key][:, :, 0:4]
assert len(t_ned_arr) > 0
for t_ned in t_ned_arr[::SUBSAMPLE_STEP]:
t_min, t_max = float(t_ned[0][0]), float(t_ned[1][0])
ned_min, ned_max = t_ned[0][1:4].astype(
float), t_ned[1][1:4].astype(float)
xyz_min, xyz_max = cls._position_ned_to_xyz(
ned_min), cls._position_ned_to_xyz(ned_max)
rect = Rectangle(mins=xyz_min, maxes=xyz_max)
plan.append(
StampedRect(stamp=t_start + t_min,
rect=rect,
reaching_wp=False))
return t_start + t_max
class test_rectangle:
def setUp(self):
self.rect = Rectangle([0,0],[1,1])
def test_min_inside(self):
assert_almost_equal(self.rect.min_distance_point([0.5,0.5]),0)
def test_min_one_side(self):
assert_almost_equal(self.rect.min_distance_point([0.5,1.5]),0.5)
def test_min_two_sides(self):
assert_almost_equal(self.rect.min_distance_point([2,2]),np.sqrt(2))
def test_max_inside(self):
assert_almost_equal(self.rect.max_distance_point([0.5,0.5]),1/np.sqrt(2))
def test_max_one_side(self):
assert_almost_equal(self.rect.max_distance_point([0.5,1.5]),np.hypot(0.5,1.5))
def test_max_two_sides(self):
assert_almost_equal(self.rect.max_distance_point([2,2]),2*np.sqrt(2))
def test_split(self):
less, greater = self.rect.split(0,0.1)
assert_array_equal(less.maxes,[0.1,1])
assert_array_equal(less.mins,[0,0])
assert_array_equal(greater.maxes,[1,1])
assert_array_equal(greater.mins,[0.1,0])
class Test_rectangle:
def setup_method(self):
self.rect = Rectangle([0, 0], [1, 1])
def test_min_inside(self):
assert_almost_equal(self.rect.min_distance_point([0.5, 0.5]), 0)
def test_min_one_side(self):
assert_almost_equal(self.rect.min_distance_point([0.5, 1.5]), 0.5)
def test_min_two_sides(self):
assert_almost_equal(self.rect.min_distance_point([2, 2]), np.sqrt(2))
def test_max_inside(self):
assert_almost_equal(self.rect.max_distance_point([0.5, 0.5]),
1 / np.sqrt(2))
def test_max_one_side(self):
assert_almost_equal(self.rect.max_distance_point([0.5, 1.5]),
np.hypot(0.5, 1.5))
def test_max_two_sides(self):
assert_almost_equal(self.rect.max_distance_point([2, 2]),
2 * np.sqrt(2))
def test_split(self):
less, greater = self.rect.split(0, 0.1)
assert_array_equal(less.maxes, [0.1, 1])
assert_array_equal(less.mins, [0, 0])
assert_array_equal(greater.maxes, [1, 1])
assert_array_equal(greater.mins, [0.1, 0])
def ADM2_solve(method,problem, rectangles,evals=2000,verbose=1,max_iter=10,**kwargs):
logger.info("AMD2 Solving: %s:%s"%(str((method.__name__,problem)),str(len(rectangles))))
iter=0
objs=[]
refs=[ADM2_reference(rectangles[-1],**kwargs)]
nf = len(rectangles[-1][-1].mins)
while iter<max_iter:
new_rectangles=copy.deepcopy(rectangles[-1])
logger.debug("Running method %s"%method.__name__)
objs.append(method(problem,refs[-1],evals=evals))
try:
if np.linalg.norm(refs[-1]-refs[-2]) < 0.000001:
break
except IndexError:
pass
srnd=lambda v: list([round(x,6) for x in v])
ref_str="%s"%srnd(refs[-1])
logger.debug(format("ref point %s -> %s"%(ref_str.ljust(10*nf),srnd(objs[-1]))))
replace_rec=None
distances=[]
outside=True
for rec in new_rectangles:
distances.append(rec.min_distance_point(objs[-1]))
if distances[-1]<=0.0001:
replace_rec=rec
outside=False
if outside:
replace_rec=new_rectangles[np.argmin(distances)]
new_rectangles.remove(replace_rec)
rectangles.append(new_rectangles)
for i,obj in enumerate(objs[-1][:]):
low=list(replace_rec.mins)
if outside:
if obj>replace_rec.mins[i]:
low[i]=replace_rec.mins[i]
else:
low[i]=obj
up=objs[-1][:]
up[i]=list(replace_rec.maxes)[i]
rectangles[-1].append(Rectangle(low,up))
ref=ADM2_reference(rectangles[-1])
if np.array_equal(ref,refs[-1]):
logger.warning("Could not generate new refpoint from PO point %s"%objs[-1])
break
refs.append(ref)
iter +=1
logger.info("AMD2 %s:%s Done"%(str((method.__name__,problem)),str(len(rectangles))))
return objs[-1],rectangles,(method.__name__,problem),(objs,refs)
def get_res(nf=2,
c=None,
r=.5,
problem='DTLZ2',
uf_n=None,
method=pyMOEA.ACH_solution,
**kwargs):
problem_instance = pyMOEA.problem(problem, nf)
try:
ideal = problem_instance.ideal
nadir = problem_instance.nadir
except AttributeError:
ideal = [0.0] * nf
nadir = [1.0] * nf
init_pref = [[Rectangle(ideal, nadir)]]
return pyMOEA.ADM2_solve(method, problem, init_pref,
**kwargs) #,c=-np.inf)
def ADM2_run(nf=3,jobs=8,evals=20000,runs=4,single_pref=True,
methods=[pyMOEA.ACH_solution,pyMOEA.rNSGAII_solution],
problems=['DTLZ1','DTLZ2','DTLZ3','DTLZ4']):
"""
Returns dict with keys (nf,method_name,problem_name)
and values of final_solution,(aspir,weigth),(objectives,references)
"""
ideal=[0.0]*nf
nadir=[1.0]*nf
bounds=(ideal,nadir)
init_pref=[[Rectangle(ideal,nadir)]]
results=defaultdict(list)
for r in range(runs):
res=Parallel(n_jobs=jobs)(
delayed(pyMOEA.agent_paraller)(target,init_pref,evals=evals)
for target in itertools.product(methods,problems)
)
for r in res:
results[(nf,r[2][0].__name__,r[2][1])].append((r[0],r[1],r[3]))
return results
def setUp(self):
self.rect = Rectangle([0,0],[1,1])
def setUp(self):
self.rect = Rectangle([0, 0], [1, 1])
def setup_method(self):
self.rect = Rectangle([0,0],[1,1])
def _bloat_point(cls, p: Tuple[float, ...]) -> Rectangle:
p_arr = np.array(p)
return Rectangle(mins=p_arr - cls.BLOAT_WIDTH,
maxes=p_arr + cls.BLOAT_WIDTH)
def _bloat_segment(bloat_a: Rectangle, bloat_b: Rectangle) -> Rectangle:
new_maxes = np.maximum(bloat_a.maxes, bloat_b.maxes)
new_mins = np.minimum(bloat_a.mins, bloat_b.mins)
return Rectangle(maxes=new_maxes, mins=new_mins)
def setup_method(self):
self.rect = Rectangle([0, 0], [1, 1])
# import math from scipy.spatial import Rectangle # from matplotlib.patches import Rectangle as drawRect # import matplotlib.pyplot as plt # import spline # import detobst import evoalg import time starttime = time.time() interpol = 100 start = [0, 5] stop = [10, 5] points = [[0, 5], [10, 5]] obst = Rectangle(maxes=[5,8], mins=[4,-300]) obstMov = Rectangle(maxes=[6.5, 1.5], mins=[3.5, -1.5]) obstMov2 = Rectangle(maxes=[9.5, 11.5], mins=[6.5, 8.5]) obst2 = Rectangle(maxes=[2,6], mins=[-300,4]) obsts = [obst, obst2] movObsts = [[obstMov, 0, 1], [obstMov2, 0, -0.5]] obsts = [] # movObsts = [] evoalg.start = start evoalg.stop = stop evoalg.obsts = obsts evoalg.movObsts = movObsts x,y = evoalg.evolutionAlgorithm(obsts.__len__() + movObsts.__len__()) print(time.time()-starttime) # drawObst = drawRect([4,-300], 1, 308)
