def __init__(self,
world=None,
NGEN=1000,
n_ind=100,
n_elite=10,
fitness_thresh=0.1,
margin_on=False,
verbose=True):
self.trajectory = [world.start]
self.history = []
self.pop = []
self.best_ind = []
self.gtot = 0
self.n_ind = n_ind # The number of individuals in a population.
self.n_elite = n_elite
self.NGEN = NGEN # The number of generation loop.
self.fitness_thresh = fitness_thresh
self.verbose = verbose
# --- Generate Collision Checker
self.world = world
if margin_on:
self.world_margin = world.mcopy(rate=1.05)
else:
self.world_margin = world
self.cc = CollisionChecker(world)
self.ccm = CollisionChecker(self.world_margin)
def __init__(self):
self.world_size = GlobalVars.get_world_size()
self.bird_state = BirdState()
self.background_state = BackgroundState()
self.collision_checker = CollisionChecker(self.background_state, self.bird_state)
self.is_alive = True
self.score = 0
class TrajectoryGenerator(object):
"""
Makes calls to the motion planner and checks the resulting
trajectory for collisions.
"""
def __init__(self, env):
self.env = env
self.robot = self.env.GetRobots()[0]
self.manip = self.robot.GetActiveManipulator()
self.motion_planner = TrajoptPlanner(self.env)
self.collision_checker = CollisionChecker(self.env)
def traj_from_pose(self, pos, rot,
collisionfree=True,
joint_targets=None,
n_steps=None):
traj = self.motion_planner.plan_with_pose(pos, rot, collisionfree,
joint_targets, n_steps)
collisions = self.collision_checker.get_collisions(traj)
if collisionfree and collisions:
return None, collisions
else:
return traj, collisions
def traj_from_joints(self, joint_targets,
collisionfree=True,
n_steps=None):
traj = self.motion_planner.plan_with_joints(joint_targets, collisionfree,
n_steps)
collisions = self.collision_checker.get_collisions(traj)
if collisionfree and collisions:
return None, collisions
else:
return traj, collisions
class Arena(object):
def __init__(self, width, height):
self.width = width
self.height = height
self.collision_checker = CollisionChecker(self)
self.stuff = {}
def agents(self):
result = list(self.stuff.keys())
result.append(self.collision_checker)
return result
def place(self, something, x, y):
"""place something on the arena"""
x %= self.width
y %= self.height
if x > self.width:
raise ArenaError("%s is too high for x (0 - %s)" % (x, self.width))
if y > self.height:
raise ArenaError("%s is too high for y (0 - %s)" % (y, self.height))
self.stuff[something] = {'coordinates': (x, y)}
def remove(self, something):
try:
del self.stuff[something]
self.collision_checker.forget(something)
except KeyError:
pass
def where_is(self, something):
return self.stuff[something]['coordinates']
def neighbours_of(self, centre, distance, _type):
x, y = self.stuff[centre]['coordinates']
distance_2 = distance**2
for thing, data in self.stuff.iteritems():
if not isinstance(thing, _type):
continue
dx = x - data['coordinates'][0]
dy = y - data['coordinates'][1]
if dx**2 + dy**2 > distance_2:
continue
yield thing
def move(self, thing, dx, dy):
x, y = self.stuff[thing]['coordinates']
x, y = x + dx, y + dy
x = x % self.width
y = y % self.height
self.stuff[thing]['coordinates'] = (x, y)
def __repr__(self):
return "Arena"
def __init__(self, world,
branch_length=0.1, resampling_length=0.05, verbose=False):
# set RRT parameter
self.verbose = verbose
self.rrt_length = branch_length
self.resampling_length = resampling_length
self.rrt_size = 5000
self.world = world
self.path = []
self.smoothed_path = []
self.resampled_path = []
self.tree = []
self.cc = CollisionChecker(world)
def shortcut(path_list, world):
cc = CollisionChecker(world)
q0 = np.array(path_list[0])
res_path_list = [q0]
i = 1
while i < len(path_list):
q1 = np.array(path_list[i])
if cc.line_validation(q0, q1):
i += 1
else:
q0 = np.array(path_list[i - 1])
res_path_list.append(q0)
i += 1
res_path_list.append(path_list[-1])
return np.array(res_path_list)
def __init__(self, width, height, n_asteroids, min_mass=800, max_mass=1000):
self.min_mass = min_mass
self.max_mass = max_mass
self.schedule = Schedule('move', 'random', 'collision_check', 'collision_response')
Asteroid.activate(self.schedule)
Ship.activate(self.schedule)
Bullet.activate(self.schedule)
CollisionChecker.activate(self.schedule)
self._tick = 0
self.logger = logging.getLogger("Model")
self.arena = Arena(width, height)
for i in range(n_asteroids):
self.add_asteroid()
centre = [self.arena.width / 2, self.arena.height / 2]
self.ship = Ship(centre, 1500, 150, 1500, self.arena)
def __init__(self, world, n, k, delta=0.01):
# --- Collision Checker
self.cc = CollisionChecker(world, delta)
# --- World
self.world = world
# --- parameter
self.n = n
self.d = 2
self.k = k
self.V = np.array([])
self.E = []
self.roadmap = []
self.resampling_length = 0.1
self.path = None
self.smoothed_path = None
self.resampled_path = None
self.path_list = []
def __init__(self, num_paths, path_offset, circle_offsets, circle_radii,
path_select_weight, time_gap, a_max, coasting_speed,
stop_line_buffer):
self.num_paths = num_paths
self.path_offset = path_offset
self.path_optimizer = PathOptimizer()
self.collision_checker = CollisionChecker(circle_offsets, circle_radii,
path_select_weight)
self.velocity_planner = VelocityPlanner(time_gap, a_max,
coasting_speed,
stop_line_buffer)
def __init__(
self,
road: Road,
actions: Actions,
constraints: Constraints,
termination_conditions: TerminationConditions,
cost_calculator: CostCalculator,
start_state: State,
ego,
subject,
):
self.road = road
self.actions = actions
self.start_state = start_state
self.constraints = constraints
self.termination_conditions = termination_conditions
self.cost_calculator = cost_calculator
self.collision_checker = CollisionChecker(1.75, 1)
self.ego_vehicle = ego
self.subject_vehicle = subject
def locate(self, scale_minmax):
frame = np.array(self.world.frame)
xmax = np.max(frame[:, 0])
xmin = np.min(frame[:, 0])
ymax = np.max(frame[:, 1])
ymin = np.min(frame[:, 1])
def collision_check(target):
res = [
cc.path_validation_new(target[i], target[i + 1])
for i in range(len(target) - 1)
]
res_start = [
cc.crossing_number_algorithm(self.world.start, target)
]
res_goal = [cc.crossing_number_algorithm(self.world.goal, target)]
return all(res + res_start + res_goal)
new_objects = []
for obj in self.world.objects:
self.world.objects = new_objects
cc = CollisionChecker(self.world, 0.05)
for iter in range(10):
# set random value
# x, y: translate distance, theta: rotate angle, scale: scale
x = np.random.rand() * (xmax - xmin) + xmin
y = np.random.rand() * (ymax - ymin) + ymin
theta = np.random.rand() * 2.0 * pi
scale = np.random.rand() * \
(scale_minmax[1] - scale_minmax[0]) + scale_minmax[0]
# Rotate, scale, translate
rot_mat = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta),
np.cos(theta)]])
rslt = np.dot(rot_mat, obj.T) * scale + \
np.array([x, y]).reshape(-1, 1)
# Check collisions with the other objects.
if collision_check(rslt.T):
new_objects.append(rslt.T)
break
self.world.objects = new_objects.copy()
class GameState:
def __init__(self):
self.world_size = GlobalVars.get_world_size()
self.bird_state = BirdState()
self.background_state = BackgroundState()
self.collision_checker = CollisionChecker(self.background_state, self.bird_state)
self.is_alive = True
self.score = 0
def update(self, move_command):
self.bird_state.update(move_command)
self.background_state.update()
if self.collision_checker.check_collision():
self.is_alive = False
def get_bird_position(self):
return self.bird_state.bird_position
class PRM():
def __init__(self, world, n, k, delta=0.01):
# --- Collision Checker
self.cc = CollisionChecker(world, delta)
# --- World
self.world = world
# --- parameter
self.n = n
self.d = 2
self.k = k
self.V = np.array([])
self.E = []
self.roadmap = []
self.resampling_length = 0.1
self.path = None
self.smoothed_path = None
self.resampled_path = None
self.path_list = []
def create_k_edges(self, nodes):
self.E = []
for i, target_node in enumerate(nodes):
dist = [np.linalg.norm(target_node - node) for node in nodes]
indices = np.argsort(dist)
count_neighbors = 0
for j in indices[1:]:
# Collision check
if self.cc.line_validation(nodes[i], nodes[j]):
pair = tuple(sorted([i, j]))
# Duplication check
if not (pair in [e[0] for e in self.E]):
self.E.append([pair, dist[j]])
# self.E.append([pair, 1.0])
# If the number of edges equal to k, then break
count_neighbors += 1
if count_neighbors == self.k:
break
return self.E
def find_nearest_node(self, nodes, p):
mindist = np.inf
nearest_node_index = None
for i, target in enumerate(nodes):
dist = np.linalg.norm(p - target)
is_valid = self.cc.line_validation(p, target)
if mindist > dist and is_valid:
mindist = dist
nearest_node_index = i
return nearest_node_index
def create_node(self):
min = np.array([np.min(self.world.frame[:, i]) for i in range(self.d)])
max = np.array([np.max(self.world.frame[:, i]) for i in range(self.d)])
length = max - min
self.V = []
while len(self.V) < self.n:
q = np.random.rand(self.d) * length + min
if self.cc.point_validation(q):
self.V.append(q)
return np.array(self.V)
def build_roadmap(self):
# create nodes
self.V = self.create_node()
# Create edges
self.E = self.create_k_edges(self.V)
# Roadmap
self.roadmap = [
np.vstack((self.V[e[0][0]], self.V[e[0][1]])) for e in self.E
]
def query(self, start, goal):
# Find nearest node to start/goal
start_prm = self.find_nearest_node(self.V, start)
goal_prm = self.find_nearest_node(self.V, goal)
# If nearest node cannot be found, self.path = None
if start_prm is None or goal_prm is None:
self.path = None
# else, find shortest path (Dijkstra's algorithm)
else:
djk = Dijkstra(self.V, self.E)
djk.build(start_prm)
djk.query(goal_prm)
if djk.path is not None:
self.path = np.vstack(
(self.world.start, djk.path, self.world.goal))
else:
self.path = None
def smoothing(self, path_list):
q0 = np.array(path_list[0])
res_path_list = [q0]
i = 1
while i < len(path_list):
q1 = np.array(path_list[i])
if self.cc.line_validation(q0, q1):
i += 1
else:
q0 = np.array(path_list[i - 1])
res_path_list.append(q0)
i += 1
res_path_list.append(path_list[-1])
return np.array(res_path_list)
def resampling(self, path_list):
q0 = np.array(path_list[0])
res_position_list = [q0]
for node in path_list[1:]:
q1 = np.array(node)
vector = q1 - q0
dist = np.linalg.norm(vector)
if dist > self.resampling_length:
norm_vector = vector / dist
num_sample = int(dist / self.resampling_length)
dx = dist / num_sample
else:
norm_vector = vector
num_sample = 1
dx = 1.0
for i in range(1, num_sample + 1):
q_sample = q0 + norm_vector * dx * i
res_position_list.append(q_sample)
q0 = q1
return np.array(res_position_list)
def single_query(self, verbose=False):
iter = 0
while self.path is None:
if verbose:
t0 = time.time()
print('Trial{:>2}: Start'.format(iter))
self.build_roadmap()
if verbose:
t1 = time.time()
print('build_roadmap:{:.3f}'.format(t1 - t0))
self.query(self.world.start, self.world.goal)
if verbose:
t2 = time.time()
print('query:{:.3f}\n'.format(t2 - t1))
if iter == 10:
break
iter += 1
if self.path is not None:
self.smoothed_path = self.smoothing(self.path)
# self.resampled_path = self.resampling(self.smoothed_path)
self.path_list = [self.smoothed_path]
else:
print('Path cannot be found.')
self.path_list = []
def multi_query(self, n, verbose=False):
if verbose:
print('Start multi_query')
t0 = time.time()
# Find nearest node to start/goal
start_prm = self.find_nearest_node(self.V, self.world.start)
goal_prm = self.find_nearest_node(self.V, self.world.goal)
# Initialize Dijkstra module
djk1 = Dijkstra(self.V, self.E)
djk2 = Dijkstra(self.V, self.E)
# Build a distance map
djk1.build(start_prm)
djk2.build(goal_prm)
if verbose:
t1 = time.time()
print('Build a distance map: {}'.format(t1 - t0))
# Generate multiple paths
self.path_list = []
while len(self.path_list) < n:
mid_point = np.random.randint(len(self.V))
djk1.query(mid_point)
djk2.query(mid_point)
if djk1.path is not None and djk2.path is not None:
djk1.path = np.vstack((self.world.start, djk1.path))
djk2.path = np.vstack((djk2.path[-2::-1], self.world.goal))
'''
smoothed_path1 = self.smoothing(djk1.path)
smoothed_path2 = self.smoothing(djk2.path)
self.smoothed_path = np.vstack((
smoothed_path1,
smoothed_path2))
'''
self.path = np.vstack((djk1.path, djk2.path))
self.smoothed_path = self.smoothing(self.path)
# '''
self.path_list.append(self.smoothed_path)
if verbose:
t2 = time.time()
print('Generate multiple paths: {}\n'.format(t2 - t1))
def __init__(self, width, height):
self.width = width
self.height = height
self.collision_checker = CollisionChecker(self)
self.stuff = {}
world = World()
# --- Set frame
world.frame = np.array([[-pi, -pi], [-pi, pi], [pi, pi], [pi, -pi], [-pi,
-pi]])
# --- Set start/goal point
world.start = np.array([-pi / 2, -pi / 2]) * 1.9
world.goal = np.array([pi / 2, pi / 2]) * 1.9
# --- Generate objects
og = ObjectGenerator(world)
world.objects = og.example()
# --- CollisionChecker
cc = CollisionChecker(world)
# In[]:
m = 6
n = 6
w = 1.0
h = 1.0
x0 = -3.0
y0 = -3.0
V = [[x0 + i * w, y0 + j * h] for j in range(n) for i in range(m)]
E = []
for i in range(m * n - 1):
if i % m != (m - 1):
E.append([(i, i + 1), 1.0])
class LatticeGenerator:
def __init__(
self,
road: Road,
actions: Actions,
constraints: Constraints,
termination_conditions: TerminationConditions,
cost_calculator: CostCalculator,
start_state: State,
ego,
subject,
):
self.road = road
self.actions = actions
self.start_state = start_state
self.constraints = constraints
self.termination_conditions = termination_conditions
self.cost_calculator = cost_calculator
self.collision_checker = CollisionChecker(1.75, 1)
self.ego_vehicle = ego
self.subject_vehicle = subject
def sample_random_trajectory(self):
tmp_state = copy.deepcopy(self.start_state)
trajectory = [self.start_state]
ALLOW_LANE_CHANGE = True
while True:
# 1. Randomly select an action
is_next_action_lane_change, next_action_params = self.actions.random_action(
ALLOW_LANE_CHANGE)
if is_next_action_lane_change:
ALLOW_LANE_CHANGE = False
# 2. Apply action + constraints
# 2.a T
tmp_state.time += next_action_params["deltaT"]
# 2.b D
tmp_state.position[0] += (tmp_state.speed +
next_action_params["deltaDDiff"][0])
tmp_state.position[1] += next_action_params["deltaDDiff"][1]
tmp_state.apply_constraints(self.constraints)
# 2.c V
tmp_state.speed += next_action_params["deltaV"]
tmp_state.apply_constraints(self.constraints)
# 3. Append to trajectory
trajectory.append(copy.deepcopy(tmp_state))
# 4. Check if terminal
if tmp_state.check_termination(self.termination_conditions):
break
return trajectory
def sample_and_plot_random_trajectory(self):
random_trajectory = self.sample_random_trajectory()
# Plot the road
self.road.plot()
# Plot the states of the random trajectory
x_vals = []
y_vals = []
for state in random_trajectory:
plt.plot(state.position[0], state.position[1], "o")
x_vals.append(state.position[0])
y_vals.append(state.position[1])
plt.plot(x_vals, y_vals)
return random_trajectory
def check_dense_collision_for_lane_change(
self,
start_state,
end_state,
subject_path,
action_params,
ego_size,
subject_size,
num_points=10,
):
start_x = start_state.position[0]
start_y = start_state.position[1]
end_x = end_state.position[0]
end_y = end_state.position[1]
x_points = np.linspace(start_x, end_x, num_points)
y_points = np.linspace(start_y, end_y, num_points)
time_points = np.linspace(start_state.time, end_state.time, num_points)
for i in range(len(time_points)):
intermediate_state = State(
position=[x_points[i], y_points[i]],
speed=start_state.speed,
time=time_points[i],
)
if self.collision_checker.predict_collision(
ego_size, subject_size, intermediate_state, subject_path,
True):
return True
return False
def generate_full_lattice(self, start_state, subject_path,
lane_change_init_flag):
lattice = (
{}
) # (x,y,v,t,lane_change_status) -> {"action" -> next (x,y,v,t,lane_change_status)}
state_2_cost = {}
goal_cost = 2**31
queue = PriorityQueue()
queue.put((0, lane_change_init_flag,
start_state)) # (state, has lane change happened, cost)
goal_state = None
while not queue.empty():
cost, has_lane_change_happend, curr_state = queue.get()
curr_state_tuple = (
curr_state.position[0],
curr_state.position[1],
curr_state.speed,
curr_state.time,
has_lane_change_happend,
)
if (curr_state_tuple in lattice.keys()
and state_2_cost[curr_state_tuple] <= cost):
continue
lattice[curr_state_tuple] = {}
state_2_cost[curr_state_tuple] = cost
# Find best goal state
if (has_lane_change_happend and curr_state.position[0] > 100
# and cost < goal_cost
):
goal_state = curr_state_tuple
goal_cost = cost
break
for i, next_action_params in enumerate(
self.actions.action_params_list):
if i == 3 and has_lane_change_happend:
continue
tmp_state = copy.deepcopy(curr_state)
if i == 2 and tmp_state.speed < 8:
continue
# Apply action + constraints
# a T
tmp_state.time += next_action_params["deltaT"]
# b D
if i != 3:
tmp_state.position[0] += (
tmp_state.speed + next_action_params["deltaDDiff"][0])
else:
tmp_state.position[0] += tmp_state.speed * 3.6
tmp_state.position[1] += next_action_params["deltaDDiff"][1]
tmp_state.apply_constraints(self.constraints)
# c V
tmp_state.speed += next_action_params["deltaV"]
tmp_state.apply_constraints(self.constraints)
# Check collision
ego_size = [
self.ego_vehicle.bounding_box.extent.x,
self.ego_vehicle.bounding_box.extent.y,
]
subject_size = [
self.subject_vehicle.bounding_box.extent.x,
self.subject_vehicle.bounding_box.extent.y,
]
if i == 3:
if self.check_dense_collision_for_lane_change(
curr_state,
tmp_state,
subject_path,
next_action_params,
ego_size,
subject_size,
):
print("Dense Collision detected....................")
continue
else:
if self.collision_checker.predict_collision(
ego_size, subject_size, tmp_state, subject_path,
i == 3):
print("Lane Collision detected....................")
continue
if i == 3 or has_lane_change_happend:
tmp_state_tuple = (
tmp_state.position[0],
tmp_state.position[1],
tmp_state.speed,
tmp_state.time,
1,
)
else:
tmp_state_tuple = (
tmp_state.position[0],
tmp_state.position[1],
tmp_state.speed,
tmp_state.time,
0,
)
lattice[curr_state_tuple][i] = tmp_state_tuple
cost_of_transition = self.cost_calculator.compute_transition_cost(
next_action_params, curr_state, tmp_state, subject_path)
# Check if terminal
if tmp_state.check_termination(
self.termination_conditions) and i != 3:
continue
# Add to queue
if i == 3 or has_lane_change_happend:
queue.put((cost + cost_of_transition, 1, tmp_state))
else:
queue.put((cost + cost_of_transition, 0, tmp_state))
## Reverse Direction Lattice (x,y,v,t,lane_change_status) -> {"action" -> parent (x,y,v,t,lane_change_status)}
reverse_lattice = {}
for parent_state in lattice.keys():
for action in lattice[parent_state].keys():
next_state = lattice[parent_state][action]
if next_state in reverse_lattice.keys():
reverse_lattice[next_state][action] = parent_state
else:
reverse_lattice[next_state] = {action: parent_state}
# import ipdb
# ipdb.set_trace()
return lattice, state_2_cost, reverse_lattice, goal_state
def backtrack_from_state(self, reverse_lattice, state_2_cost,
end_state_tuple, start_state_tuple):
curr_state_tuple = copy.deepcopy(end_state_tuple)
path = [curr_state_tuple]
action_sequence = []
while curr_state_tuple != start_state_tuple:
minCostAction = -1
minCost = pow(10, 9) # Some large number
for possible_action in reverse_lattice[curr_state_tuple].keys():
corresponding_parent_state_tuple = reverse_lattice[
curr_state_tuple][possible_action]
if state_2_cost[corresponding_parent_state_tuple] < minCost:
minCost = state_2_cost[corresponding_parent_state_tuple]
minCostAction = possible_action
curr_state_tuple = reverse_lattice[curr_state_tuple][minCostAction]
path.append(curr_state_tuple)
action_sequence.append(minCostAction)
path.reverse()
action_sequence.reverse()
return path, action_sequence
def plot_some_trajectories(self, lattice, start_state_tuple, k=10):
self.road.plot()
for i in range(k):
curr_state_tuple = copy.deepcopy(start_state_tuple)
traj = [curr_state_tuple]
while curr_state_tuple in lattice.keys():
next_action = random.choice(
[k for k in range(len(lattice[curr_state_tuple]))])
next_state_tuple = lattice[curr_state_tuple][next_action]
traj.append(next_state_tuple)
curr_state_tuple = copy.deepcopy(next_state_tuple)
x_vals = [p[0] for p in traj]
y_vals = [p[1] for p in traj]
plt.plot(x_vals, y_vals)
for i in range(len(x_vals)):
plt.plot(x_vals[i], y_vals[i], "o")
return 0
global sensor_data
sensor_data = data
if __name__ == '__main__':
os.environ["WANDB_SILENT"] = "true" # Avoid small wandb bug
# Subscribe to essential pathfinding info
sailbot = sbot.Sailbot(nodeName='log_stats')
rospy.Subscriber("sensors", msg.Sensors, sensorsCallback)
sailbot.waitForFirstSensorDataAndGlobalPath()
# Setup subscribers
log_closest_obstacle = LogClosestObstacle(create_ros_node=False)
path_storer = PathStorer(create_ros_node=False)
collision_checker = CollisionChecker(create_ros_node=False)
sailbot_gps_data = SailbotGPSData(create_ros_node=False)
rate = rospy.Rate(UPDATE_TIME_SECONDS)
# Set wandb run name and directory path
start_datetime = datetime.now().strftime('%b_%d-%H_%M_%S')
run_name = 'stats-' + start_datetime
dir_dir = os.path.expanduser('~/.ros/stats/')
dir_path = dir_dir + start_datetime
os.makedirs(dir_dir + start_datetime)
os.symlink(dir_path, dir_dir + 'tmp')
os.rename(dir_dir + 'tmp',
dir_dir + 'latest-dir') # use replace() in Python 3
wandb.init(entity='sailbot', project='stats', dir=dir_path, name=run_name)
class GeneticAlgorithm():
def __init__(self,
world=None,
NGEN=1000,
n_ind=100,
n_elite=10,
fitness_thresh=0.1,
margin_on=False,
verbose=True):
self.trajectory = [world.start]
self.history = []
self.pop = []
self.best_ind = []
self.gtot = 0
self.n_ind = n_ind # The number of individuals in a population.
self.n_elite = n_elite
self.NGEN = NGEN # The number of generation loop.
self.fitness_thresh = fitness_thresh
self.verbose = verbose
# --- Generate Collision Checker
self.world = world
if margin_on:
self.world_margin = world.mcopy(rate=1.05)
else:
self.world_margin = world
self.cc = CollisionChecker(world)
self.ccm = CollisionChecker(self.world_margin)
def create_pop_prm(self, m, n):
prm_list = [PRM(self.world_margin, 30, 3) for i in range(m)]
path_list = []
for prm in prm_list:
prm.single_query()
if prm.path_list != []:
prm.multi_query(n)
path_list += prm.path_list
return [Individual(path) for path in path_list]
def create_pop_cd(self):
cd = CellDecomposition(self.world_margin)
path_list = cd.main(self.n_ind, shortcut=True)
self.pop = [Individual(path) for path in path_list]
return self.pop
def set_fitness(self, eval_func):
"""Set fitnesses of each individual in a population."""
for i, fit in enumerate(map(eval_func, self.pop)):
self.pop[i].fitness = fit
def evalOneMax(self, gene):
"""Objective function."""
delta = [0.1]
score_list = []
for d in delta:
pts = self.smoothing(gene, d)
score = 0.0
for i in range(len(pts) - 1):
dist = np.linalg.norm(pts[i + 1] - pts[i])
score = score + dist
# if not cc.path_validation(pts[i], pts[i+1]):
# score = score + 10
if not self.ccm.path_validation(pts):
score = score + 10
score_list.append(score)
return min(score_list)
def eval_for_cx(self, gene):
"""Objective function."""
pts = gene.copy()
score = 0.0
for i in range(len(pts) - 1):
dist = np.linalg.norm(pts[i + 1] - pts[i])
score = score + dist
return score
def selTournament(self, tournsize):
"""Selection function."""
chosen = []
for i in range(self.n_ind - self.n_elite):
aspirants = [random.choice(self.pop) for j in range(tournsize)]
chosen.append(min(aspirants, key=attrgetter("fitness")))
return chosen
def selElite(self):
pop_sort = sorted(self.pop, key=attrgetter("fitness"))
elites = pop_sort[:self.n_elite]
return elites
def calc_intersection_point(self, A, B, C, D):
denominator = (B[0] - A[0]) * (C[1] - D[1]) - \
(B[1] - A[1]) * (C[0] - D[0])
# If two lines are parallel,
if abs(denominator) < 1e-6:
return None, None, None
AC = A - C
r = ((D[1] - C[1]) * AC[0] - (D[0] - C[0]) * AC[1]) / denominator
s = ((B[1] - A[1]) * AC[0] - (B[0] - A[0]) * AC[1]) / denominator
# If the intersection is out of the edges
if r < -1e-6 or r > 1.00001 or s < -1e-6 or s > 1.00001:
return None, r, s
# Endpoint and startpoint make the intersection.
if ((np.linalg.norm(r - 1.0) < 1e-6 and np.linalg.norm(s) < 1e-6) or
(np.linalg.norm(s - 1.0) < 1e-6 and np.linalg.norm(r) < 1e-6)):
return None, r, s
point_intersection = A + r * (B - A)
return point_intersection, r, s
def subcalc(self, queue, seed, offspring, elites, n):
np.random.seed(seed)
crossover = []
for i in range(10):
randint1 = np.random.randint(len(offspring))
randint2 = np.random.randint(len(elites))
child = self.cx(offspring[randint1], elites[randint2])
crossover.append(child)
queue.put(crossover)
def cx(self, ind1, ind2):
"""Crossover function for path planning."""
# --- If the ind1 and the ind2 is the same path, return ind1 and exit.
if len(ind1) == len(ind2) and all((ind1 == ind2).flatten()):
return ind1
# --- Initialize
best = []
id1 = [0]
id2 = [0]
tmp1 = ind1.copy()
tmp2 = ind2.copy()
j = 0
# --- Search for the intersection
for i1 in range(len(ind1) - 1):
for i2 in range(len(ind2) - 1):
# Calculate an intersection between line AB and line CD.
pt, r, s = self.calc_intersection_point(
ind1[i1], ind1[i1 + 1], ind2[i2], ind2[i2 + 1])
# If intersection is found,
if pt is not None:
if np.linalg.norm(r - 1.0) > 1e-6 and \
np.linalg.norm(r) > 1e-6:
# Add the intersection to the point lists.
tmp1 = np.insert(tmp1, i1 + j + 1, pt, axis=0)
tmp2 = np.insert(tmp2, i2 + j + 1, pt, axis=0)
# Revise the intersection lists.
id1.append(i1 + j + 1)
id2.append(i2 + j + 1)
# j: Num. of the intersection points.
j = j + 1
# Add the last point of the path to the intersection lists.
id1 = id1 + [len(ind1) + j + 1]
id2 = id2 + [len(ind2) + j + 1]
# --- Select the best path based on the path length.
for i in range(len(id1) - 1):
if (self.eval_for_cx(tmp1[id1[i]:id1[i + 1] + 1]) <
self.eval_for_cx(tmp2[id2[i]:id2[i + 1] + 1])):
best = best + list(tmp1[id1[i]:id1[i + 1]])
else:
best = best + list(tmp2[id2[i]:id2[i + 1]])
return Individual(best)
def node_reduction(self, path):
path = np.array(path)
new_path = [path[0]]
for i in range(1, len(path) - 1):
u = path[i + 1] - path[i]
umag = np.linalg.norm(u)
if umag > 0.1:
new_path.append(path[i])
elif not self.ccm.line_validation(new_path[-1], path[i + 1]):
new_path.append(path[i])
new_path.append(path[-1])
path = new_path.copy()
new_path = [path[0]]
for i in range(1, len(path) - 1):
u = path[i + 1] - path[i]
v = path[i - 1] - path[i]
umag = np.linalg.norm(u)
vmag = np.linalg.norm(v)
if umag != 0 and vmag != 0:
cos = np.dot(u, v) / (umag * vmag)
if abs(cos) < 0.99:
new_path.append(path[i])
elif not self.ccm.line_validation(new_path[-1], path[i + 1]):
new_path.append(path[i])
new_path.append(path[-1])
new_path = np.array(new_path)
return new_path
def mut_normal(self, ind, indpb, maxiter):
"""Mutation function."""
mut = ind.copy()
for i in range(1, len(ind) - 1):
if random.random() < indpb:
var = 0.5
for j in range(maxiter):
mut[i] = ind[i] + np.random.normal(0.0, var, 2)
var = var * 0.5
if self.ccm.path_validation(
[mut[i - 1], mut[i], mut[i + 1]]):
break
else:
mut[i] = ind[i]
return Individual(mut)
def smoothing(self, pts, delta=0.1):
# resampled_path = post_process.resampling(pts, delta)
# bezier_path = post_process.bezier(resampled_path, 50)
# return bezier_path
return pts
def main(self, duration):
'''
Main Routine for Genetic Algorithm
'''
ini_time = time.time()
# --- Evaluate the initial population
self.set_fitness(self.evalOneMax)
self.best_ind = min(self.pop, key=attrgetter("fitness"))
self.history.append([self.gtot, self.best_ind.fitness])
self.trajectory.append([self.world.start[0], self.world.start[1]])
np.vstack((self.trajectory, self.world.start))
if self.verbose:
rp.plot(self.pop, self.best_ind, self.trajectory, self.history)
# --- Generation loop starts.
print('\n[Genetic Algorithm]')
print("Generation loop start.")
print("Generation: 0. Best fitness: {}\n".format(
str(self.best_ind.fitness)))
for g in range(self.NGEN):
'''
STEP1 : Selection.
'''
t0 = time.time()
# Elite selection
elites = self.selElite()
# Tournament Selection
offspring = self.selTournament(tournsize=3)
'''
STEP2 : Mutation.
'''
t1 = time.time()
mutant = []
for ind in offspring:
if np.random.rand() < 0.7:
tmp = self.mut_normal(ind, indpb=0.7, maxiter=3)
mutant.append(tmp)
else:
mutant.append(ind)
offspring = mutant.copy()
# mutant = self.create_pop_cd(10)
mutant = self.create_pop_prm(2, 5)
'''
Step3 : Crossover.
'''
t2 = time.time()
proc = 8
n_cross = 10
queue = mp.Queue()
ps = [
mp.Process(target=self.subcalc,
args=(queue, i, offspring, elites, n_cross))
for i in np.random.randint(100, size=proc)
]
for p in ps:
p.start()
crossover = []
for i in range(proc):
crossover += queue.get()
'''
STEP4: Update next generation.
'''
t3 = time.time()
n_offspring = self.n_ind - \
(len(elites) + len(crossover) + len(mutant))
self.pop = (list(elites) + list(crossover) +
list(offspring[0:n_offspring])) + list(mutant)
# --- Delete the redundant points on the path.
self.pop = [
Individual(self.node_reduction(path)) for path in self.pop
]
self.set_fitness(self.evalOneMax)
'''
Output
'''
t4 = time.time()
# --- Print best fitness in the population.
self.best_ind = min(self.pop, key=attrgetter("fitness"))
print("Generation: {: > 2}".format(g))
print("Best fitness: {: .3f}".format(self.best_ind.fitness))
print(
"Time: {0:.3f}, {1:.3f}, {2:.3f}, {3:.3f}, Total: {4:.3f} \n".
format(t1 - t0, t2 - t1, t3 - t2, t4 - t3, t4 - t0))
# Fitness transition
self.history.append([self.gtot, self.best_ind.fitness])
'''
# STEP5: Termination
'''
# --- Visualization
if self.verbose:
rp.plot(self.pop, self.best_ind, self.trajectory, self.history)
if time.time() - ini_time > duration:
# --- Visualization
if self.verbose:
rp.plot(self.pop, self.best_ind, self.trajectory,
self.history)
break
if self.best_ind.fitness <= self.fitness_thresh:
print('The best fitness reaches the threshold value.')
break
if g >= 5:
diff = np.abs(self.history[-5][1] - self.history[-1][1])
if diff < 1e-2:
print('The best fitness does not change for 10 steps.')
break
self.gtot += 1
'''
World setting
'''
# --- world class
world = World()
# --- Set frame
world.frame = np.array(
[[-pi, -pi],
[-pi, pi],
[pi, pi],
[pi, -pi],
[-pi, -pi]])
# --- Set start/goal point
world.start = np.array([-pi / 2, -pi / 2]) * 1.9
world.goal = np.array([pi / 2, pi / 2]) * 1.9
# --- Generate objects
og = ObjectGenerator(world)
world.objects = og.example()
world_margin = world.mcopy(rate=1.1)
cc = CollisionChecker(world_margin)
# In[]:
# --- Visualize
gd = GraphDrawer(world_margin)
plt.show()
def __init__(self, env): self.env = env self.robot = self.env.GetRobots()[0] self.manip = self.robot.GetActiveManipulator() self.motion_planner = TrajoptPlanner(self.env) self.collision_checker = CollisionChecker(self.env)
[pi, -pi],
[-pi, -pi]])
# --- Set start/goal point
world.start = np.array([-pi / 2, -pi / 2]) * 1.9
world.goal = np.array([pi / 2, pi / 2]) * 1.9
# --- Generate objects
og = ObjectGenerator(world)
world.objects = og.example()
# og.generate(10, 5)
# og.locate([2.0, 3.0])
# world.objects = og.world.objects
# --- Generte Collision Checker
cc = CollisionChecker(world, 0.1)
'''
Functions
'''
class Individual(np.ndarray):
"""Container of a individual."""
fitness = None
def __new__(cls, a):
return np.asarray(a).view(cls)
def create_pop_prm(m, n):
class RRT():
def __init__(self, world,
branch_length=0.1, resampling_length=0.05, verbose=False):
# set RRT parameter
self.verbose = verbose
self.rrt_length = branch_length
self.resampling_length = resampling_length
self.rrt_size = 5000
self.world = world
self.path = []
self.smoothed_path = []
self.resampled_path = []
self.tree = []
self.cc = CollisionChecker(world)
def collision_check(self, nodelist):
i = 0
flag = True
q0 = np.array(nodelist[0].position)
for node in nodelist[1:]:
q1 = np.array(node.position)
if not self.cc.line_validation(q0, q1):
print('error', i)
print(q0)
print(q1)
flag = False
q0 = q1
i = i + 1
if flag:
print('No Collision')
def create_rrt_map(self):
def generate_random_position(size):
rand_array = []
for i in range(size):
min = np.min(self.world.frame[:, i])
max = np.max(self.world.frame[:, i])
rand_array.append(np.random.rand() * (max - min) + min)
return np.array(rand_array)
def find_nearest_node(nodelist, p):
mindist = float('inf')
for target in nodelist:
dist = np.linalg.norm(p - target.position)
if mindist > dist:
mindist = dist
nearest_node = target
return nearest_node
def create_new_node(index, q_rand, parent_node):
q_parent = parent_node.position
q_vector = q_rand - q_parent
dist = np.linalg.norm(q_vector)
norm_vector = q_vector / dist
q_new = q_parent + norm_vector * self.rrt_length
new_node = Node(index + 1, q_new, parent_node.index)
return new_node
def add_rrt(rrt, i):
# self.world.points = rrt
while True:
q_rand = generate_random_position(len(self.world.goal))
# world.shift(q_rand)
nearest_node = find_nearest_node(rrt, q_rand)
new_node = create_new_node(i, q_rand, nearest_node)
if self.cc.line_validation(nearest_node.position,
new_node.position):
rrt.append(new_node)
break
return rrt
def connection_check(new_node, rrt):
connect_node_candidates = []
for find_connect_node in rrt:
if self.cc.line_validation(new_node.position,
find_connect_node.position):
connect_node_candidates.append(find_connect_node)
if connect_node_candidates == []:
return None, None
else:
connect_node_1 = new_node
connect_node_2 = find_nearest_node(connect_node_candidates,
new_node.position)
return connect_node_1, connect_node_2
def connection_check2(new_node, rrt):
nearest_node = find_nearest_node(rrt, new_node.position)
if np.linalg.norm(np.array(new_node.position) -
np.array(nearest_node.position)) \
< self.rrt_length:
return new_node, nearest_node
else:
return None, None
rrt_start = [Node(0, self.world.start, 0)]
rrt_goal = [Node(0, self.world.goal, 0)]
for i in range(self.rrt_size):
# if connect node pair is found, break.
connect_node_start, connect_node_goal = \
connection_check2(rrt_start[-1], rrt_goal)
if connect_node_start is not None:
break
connect_node_goal, connect_node_start = \
connection_check2(rrt_goal[-1], rrt_start)
if connect_node_start is not None:
break
# add a new node to RRT_start
rrt_start = add_rrt(rrt_start, i)
rrt_goal = add_rrt(rrt_goal, i)
else:
print('Path cannot be found.')
return rrt_start, rrt_goal, \
connect_node_start.index, connect_node_goal.index
def path_generation(self, rrt_list, connect_index):
path_list = [rrt_list[connect_index]]
next_node = rrt_list[connect_index].parent
for node in reversed(rrt_list):
if node.index == next_node:
path_list.append(node)
next_node = node.parent
return path_list
def path_smoothing(self, path_list):
q0 = np.array(path_list[0].position)
res_path_list = [path_list[0]]
i = 1
while i < len(path_list):
q1 = np.array(path_list[i].position)
if self.cc.line_validation(q0, q1):
i += 1
else:
res_path_list.append(path_list[i - 1])
q0 = np.array(path_list[i - 1].position)
i += 1
res_path_list.append(path_list[-1])
return res_path_list
def resampling(self, path_list):
q0 = np.array(path_list[0].position)
res_position_list = [q0]
for node in path_list[1:]:
q1 = np.array(node.position)
vector = q1 - q0
dist = np.linalg.norm(vector)
if dist > self.resampling_length:
norm_vector = vector / dist
num_sample = int(dist / self.resampling_length)
dx = dist / num_sample
else:
norm_vector = vector
num_sample = 1
dx = 1.0
for i in range(1, num_sample + 1):
q_sample = q0 + norm_vector * dx * i
res_position_list.append(q_sample)
q0 = q1
return res_position_list
def rrt_visualize(self, rrt):
parent_list = [node.parent for node in rrt]
edges = [i for i in range(len(rrt)) if i not in parent_list]
path_list = []
for edge in edges:
nodeset = self.path_generation(rrt, edge)
path = np.array([node.position for node in nodeset])
path_list.append(path)
return path_list
def make_path(self):
time_0 = time.time()
print('') if self.verbose else None
print(' >>> Motion Planning Start') if self.verbose else None
# --- Get Start and Goal Point
if not self.cc.point_validation(self.world.start):
print(' error: current joint state is invalid')
return
if not self.cc.point_validation(self.world.goal):
print(' error: goal position is invalid')
return
# Rapidly Random Tree method
time_1 = time.time()
print(' >>> RRT start') if self.verbose else None
rrt_start, rrt_goal, start_connect_index, goal_connect_index = \
self.create_rrt_map()
# Path Generation
time_2 = time.time()
print(' >>> Path Generation Start ') if self.verbose else None
# --- from mid-point to start-point)
path_node_set_start = self.path_generation(
rrt_start, start_connect_index)
# --- from mid-point to goal-point)
path_node_set_goal = self.path_generation(
rrt_goal, goal_connect_index)
# --- combined path
path_node_set_start.reverse()
path_node_set = path_node_set_start + path_node_set_goal
# Smoothed Path
time_3 = time.time()
print(' >>> Smoothing Start') if self.verbose else None
smooth_path_node_set = self.path_smoothing(path_node_set)
# Re-sampling
time_4 = time.time()
print(' >>> Re-sampling Start') if self.verbose else None
position_list = self.resampling(smooth_path_node_set)
# Publishing
time_5 = time.time()
self.path = np.array([node.position for node in path_node_set])
self.smoothed_path = np.array(
[node.position for node in smooth_path_node_set])
self.resampled_path = position_list
self.tree = self.rrt_visualize(rrt_start) + \
self.rrt_visualize(rrt_goal)
print(' >>> Completed') if self.verbose else None
# Report
if self.verbose:
print('')
print(' ========== RESULTS SUMMARY ==========')
print('')
print('[start/goal]')
print('start', np.round(self.world.start, 3))
print('goal', np.round(self.world.goal, 3))
print('')
print('[RRT]')
print('#RRT node: ', len(rrt_start), '/', len(rrt_goal))
print('')
print('[trajectory points]')
for node in smooth_path_node_set:
print(np.round(node.position, 3))
self.collision_check(smooth_path_node_set)
print('')
print('[Re-Sampling Segment]')
count = 1
seg = 1
length0 = np.linalg.norm(position_list[1] - position_list[0])
for i in range(len(position_list) - 1):
length1 = np.linalg.norm(
position_list[i + 1] - position_list[i])
if np.round(length1, 8) == np.round(length0, 8) \
and i != len(position_list) - 2:
count += 1
else:
print('seg' + str(seg), length0, count)
seg += 1
count = 1
length0 = length1
print('')
print('[time]')
print('Set Goal : ', time_1 - time_0)
print('RRT : ', time_2 - time_1)
print('Path : ', time_3 - time_2)
print('Smoothing: ', time_4 - time_3)
print('Re-sample: ', time_5 - time_4)
print('---------------------------')
print('TOTAL : ', time_5 - time_0)
