def test_rrtstar():
general_obstacles_list = [
[
(1, 1),
(2, 1),
(2, 2),
(1, 2),
],
[(3, 4), (4, 4), (4, 5), (3, 5)],
]
poly = PolygonEnv(buffer_dist=0.1)
poly.update(general_obstacles_list)
sampler = UniformRectSampler(0, 6, 0, 6)
my_tree = RRTstar(
sampler=sampler,
expand_dis=0.1,
neighbourhood_radius=0.5,
goal_distance=0.5,
max_iter=2000,
)
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(5, 5))
path, _ = my_tree.plan(start, goal, poly)
# from gennav.utils.visualisation import visualize_node
# node_list = _['node_list']
# visualize_node(node_list, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
if len(path.path) != 1:
assert poly.get_traj_status(path) is True
def test_prm_plan():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
sampler = UniformRectSampler(-5, 15, -5, 15)
poly = PolygonEnv(buffer_dist=0.1)
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(12, 10))
my_tree = PRM(sampler=sampler, r=5, n=100)
for obstacles in general_obstacles_list:
poly.update(obstacles)
path, _ = my_tree.plan(start, goal, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
if len(path.path) != 1:
assert poly.get_traj_status(path) is True
def prmstar_plan():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
sampler = UniformRectSampler(-5, 15, -5, 15)
polygon = PolygonEnv()
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(12, 10))
my_prmstar = PRMStar(sampler=sampler, c=30, n=100)
# Plan a path using the PRMStar.plan method for each obstacle:
for obstacles in general_obstacles_list:
polygon.update(obstacles) # updates the environment with the obstacle
path = my_prmstar.plan(start, goal, polygon)
visualize_path(path, polygon)
if len(path.path) != 1: # check if the path has only the start state
assert polygon.get_traj_status(path) is True
def test_rrt():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
sampler = UniformRectSampler(-5, 15, -5, 15)
poly = PolygonEnv(buffer_dist=0.5)
my_tree = RRT(sampler=sampler, expand_dis=0.1)
start = RobotState(position=Point(1, 1))
goal = RobotState(position=Point(10, 10))
for obstacles in general_obstacles_list:
poly.update(obstacles)
path, _ = my_tree.plan(start, goal, poly)
# from gennav.utils.visualisation import visualize_node
# node_list = _['node_list']
# visualize_node(node_list, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
assert poly.get_traj_status(path) is True
def test_rrt():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
poly = PolygonEnv()
# poly.update(general_obstacles_list)
for obstacles in general_obstacles_list:
poly.update(obstacles)
# Instatiate rrt planner object
my_tree = RRT(sample_area=(-5, 15), sampler=sampler, expand_dis=0.1)
start = RobotState(position=Point(1, 1))
goal = RobotState(position=Point(10, 10))
path = my_tree.plan(start, goal, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
assert poly.get_traj_status(path) is True
def test_dstar():
obstacles = [[(8, 5), (7, 8), (2, 9), (3, 5)],
[(3, 3), (3, 5), (5, 5), (5, 3)]]
sampler = UniformRectSampler(-5, 15, -5, 15)
poly = PolygonEnv()
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(12, 10))
my_tree = DStar(sampler=sampler, r=3, n=75)
poly.update(obstacles)
path = my_tree.plan(start, goal, poly)
from gennav.envs.common import visualize_path
visualize_path(path, poly)
obstacles = [
[(8, 5), (7, 8), (2, 9), (3, 5)],
[(3, 3), (3, 5), (5, 5), (5, 3)],
[(10, 8), (12, 8), (11, 6)],
]
# obstacles = [[(10, 7), (9, 10), (4, 11), (5, 7)], [(5, 5), (5, 7), (7, 7), (7, 5)],[(9,8),(13,8),(11,5)]]
poly.update(obstacles)
path_new = my_tree.replan(start, goal, poly)
from gennav.envs.common import visualize_path
visualize_path(path_new, poly)
def test_potential_field():
obstacle_list = [[(1, 1), (1.5, 1.5), (1.5, 1)]]
env = PolygonEnv(buffer_dist=0.1)
env.update(obstacle_list)
my_planner = PotentialField(KP=5, ETA=100, THRESH=15, STEP_SIZE=0.1, error=0.2)
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(7, 7))
path, _ = my_planner.plan(start, goal, env)
assert env.get_traj_status(path) is True
def plan(self, start, goal, env):
"""Constructs a graph avoiding obstacles and then plans path from start to goal within the graph.
Args:
start (gennav.utils.RobotState): tuple with start point coordinates.
goal (gennav.utils.RobotState): tuple with end point coordinates.
env (gennav.envs.Environment): Base class for an envrionment.
Returns:
gennav.utils.Trajectory: The planned path as trajectory
"""
# construct graph
graph = self.construct(env)
# find collision free point in graph closest to start_point
min_dist = float("inf")
for node in graph.keys():
dist = math.sqrt((node.x - start.position.x)**2 +
(node.y - start.position.y)**2)
traj = Trajectory([
RobotState(position=node),
RobotState(position=start.position)
])
if dist < min_dist and (env.get_traj_status(traj)):
min_dist = dist
s = node
# find collision free point in graph closest to end_point
min_dist = float("inf")
for node in graph.keys():
dist = math.sqrt((node.x - goal.position.x)**2 +
(node.y - goal.position.y)**2)
traj = Trajectory([
RobotState(position=node),
RobotState(position=goal.position)
])
if dist < min_dist and (env.get_traj_status(traj)):
min_dist = dist
e = node
# add start_point to path
path = [start]
traj = Trajectory(path)
# perform astar search
p = astar(graph, s, e)
if len(p.path) == 1:
return traj
else:
traj.path.extend(p.path)
# add end_point to path
traj.path.append(goal)
return traj
def construct(self, env):
"""Constructs PRM-Star graph.
Args:
env (gennav.envs.Environment): Base class for an envrionment.
Returns:
graph (dict): A dict where the keys correspond to nodes and
the values for each key is a list of the neighbour nodes
"""
nodes = []
graph = {}
i = 0
# samples points from the sample space until n points
# outside obstacles are obtained
while i < self.n:
sample = self.sampler(self.sample_area)
if not env.get_status(RobotState(position=sample)):
continue
else:
i += 1
nodes.append(sample)
# finds neighbours for each node in a dynamic radius
for node1 in nodes:
for node2 in nodes:
if node1 != node2:
r = self.c * math.sqrt(math.log(self.n) / self.n)
dist = math.sqrt((node1.x - node2.x)**2 +
(node1.y - node2.y)**2)
if dist < r:
traj = Trajectory([
RobotState(position=node1),
RobotState(position=node2)
])
if env.get_traj_status(traj):
if node1 not in graph:
graph[node1] = [node2]
elif node2 not in graph[node1]:
graph[node1].append(node2)
if node2 not in graph:
graph[node2] = [node1]
elif node1 not in graph[node2]:
graph[node2].append(node1)
return graph
def grad_repulsive(self):
"""Calculates the gradient due to the repulsive component of the potential field, i.e, the potential field created by the obstacles
Args:
None
Returns:
total_grad (float) : Total gradient due to potential field of all obstacles at the state of the robot
"""
total_grad = [0.0, 0.0]
min_dist = self.env.minimum_distances(self.current)
for i, _ in enumerate(self.env.obstacle_list):
if min_dist[i] < self.THRESH:
dummy_state_x = RobotState(
position=Point(
self.current.position.x + 0.01,
self.current.position.y,
self.current.position.z,
)
)
x_grad = self.env.minimum_distances(dummy_state_x)[i]
dummy_state_y = RobotState(
position=Point(
self.current.position.x,
self.current.position.y + 0.01,
self.current.position.z,
)
)
y_grad = self.env.minimum_distances(dummy_state_y)[i]
grad = [
(x_grad - min_dist[i]) / 0.01,
(y_grad - min_dist[i]) / 0.01,
]
factor = (
self.ETA
* ((1 / self.THRESH) - 1 / min_dist[i])
* 1
/ (min_dist[i] ** 2 + 1)
)
grad = [x * factor for x in grad]
total_grad[0] = total_grad[0] + grad[0]
total_grad[1] = total_grad[1] + grad[1]
return total_grad
def test_astar_heuristic():
graph = Graph()
node1 = RobotState(position=Point(0, 0))
node2 = RobotState(position=Point(1, 1))
node3 = RobotState(position=Point(3, 1))
node4 = RobotState(position=Point(2, 2))
node5 = RobotState(position=Point(3, 2))
node6 = RobotState(position=Point(2, 3))
graph.add_node(node1)
graph.add_node(node2)
graph.add_node(node3)
graph.add_node(node4)
graph.add_node(node5)
graph.add_node(node6)
graph.add_edge(node1, node2)
graph.add_edge(node2, node3)
graph.add_edge(node2, node4)
graph.add_edge(node4, node5)
graph.add_edge(node4, node6)
graph.add_edge(node5, node6)
heuristic = {
node1: 5,
node2: 3,
node3: 1,
node4: 1,
node5: 0,
node6: 2,
}
start = node1
end = node5
_ = astar(graph, start, end, heuristic)
print(_.path)
def test_astar():
graph = Graph()
node1 = RobotState(position=Point(0, 0))
node2 = RobotState(position=Point(1, 1))
node3 = RobotState(position=Point(3, 1))
node4 = RobotState(position=Point(2, 2))
node5 = RobotState(position=Point(3, 2))
node6 = RobotState(position=Point(2, 3))
graph.add_node(node1)
graph.add_node(node2)
graph.add_node(node3)
graph.add_node(node4)
graph.add_node(node5)
graph.add_node(node6)
graph.add_edge(node1, node2)
graph.add_edge(node2, node3)
graph.add_edge(node2, node4)
graph.add_edge(node4, node5)
graph.add_edge(node4, node6)
graph.add_edge(node5, node6)
start = node1
end = node5
_ = astar(graph, start, end)
print(_.path)
def test_prm_plan():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
poly = PolygonEnv()
for obstacles in general_obstacles_list:
poly.update(obstacles)
# Instatiate prm constructer object
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(12, 10))
my_tree = PRM(sample_area=(-5, 15), sampler=sampler, r=5, n=100)
path = my_tree.plan(start, goal, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
assert poly.get_traj_status(path) is True
def msg_to_traj(msg):
"""Converts the trajectory containing ROS message to gennav.utils.Trajectory data type
Args:
msg (geometry_msgs/MultiDOFJointTrajectory.msg): The ROS message
Returns:
gennav.utils.Trajectory : Object containing the trajectory data
"""
path = []
timestamps = []
for point in msg.points:
timestamps.append(point.time_from_start)
position = utils.common.Point(
x=point.transforms[0].translation.x,
y=point.transforms[0].translation.y,
z=point.transforms[0].translation.z,
)
rotation = tf.transformations.euler_from_quaternion([
point.transforms[0].rotation.x,
point.transforms[0].rotation.y,
point.transforms[0].rotation.z,
point.transforms[0].rotation.w,
])
rotation = utils.geometry.OrientationRPY(roll=rotation[0],
pitch=rotation[1],
yaw=rotation[2])
linear_vel = utils.geometry.Vector3D(
x=point.velocities[0].linear.x,
y=point.velocities[0].linear.y,
z=point.velocities[0].linear.z,
)
angular_vel = utils.geometry.Vector3D(
x=point.velocities[0].angular.x,
y=point.velocities[0].angular.y,
z=point.velocities[0].linear.z,
)
velocity = Velocity(linear=linear_vel, angular=angular_vel)
state = RobotState(position=position,
orientation=rotation,
velocity=velocity)
path.append(state)
traj = Trajectory(path=path, timestamps=timestamps)
return traj
def Odom_to_RobotState(msg):
"""Converts a ROS message of type nav_msgs/Odometry.msg to an object of type gennav.utils.RobotState
Args:
msg (nav_msgs/Odometry.msg): ROS message containing the pose and velocity of the robot
Returns:
gennav.utils.RobotState: A RobotState() object which can be passed to the controller
"""
position = utils.common.Point(
x=msg.pose.pose.position.x,
y=msg.pose.pose.position.y,
z=msg.pose.pose.position.z,
)
orientation = tf.transformations.euler_from_quaternion([
msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w,
])
orientation = utils.geometry.OrientationRPY(roll=orientation[0],
pitch=orientation[1],
yaw=orientation[2])
linear_vel = utils.geometry.Vector3D(
x=msg.twist.twist.linear.x,
y=msg.twist.twist.linear.y,
z=msg.twist.twist.linear.z,
)
angular_vel = utils.geometry.Vector3D(
x=msg.twist.twist.angular.x,
y=msg.twist.twist.angular.y,
z=msg.twist.twist.angular.z,
)
velocity = Velocity(linear=linear_vel, angular=angular_vel)
state = RobotState(position=position,
orientation=orientation,
velocity=velocity)
return state
def plan(self, start, goal, env):
"""Plans path from start to goal avoiding obstacles.
Args:
start_point(gennav.utils.RobotState): with start point coordinates.
end_point (gennav.utils.RobotState): with end point coordinates.
env: (gennav.envs.Environment) Base class for an envrionment.
Returns:
gennav.utils.Trajectory: The planned path as trajectory
"""
# Initialize start and goal nodes
start_node = Node(state=start)
goal_node = Node(state=goal)
# Initialize node_list with start
node_list = [start_node]
# Loop to keep expanding the tree towards goal if there is no direct connection
traj = Trajectory(path=[start, goal])
if not env.get_traj_status(traj):
while True:
# Sample random point in specified area
rnd_point = self.sampler(self.sample_area, goal.position,
self.goal_sample_rate)
# Find nearest node to the sampled point
distance_list = [(node.state.position.x - rnd_point.x)**2 +
(node.state.position.y - rnd_point.y)**2 +
(node.state.position.z - rnd_point.z)**2
for node in node_list]
try:
# for python2
nearest_node_index = min(xrange(len(distance_list)),
key=distance_list.__getitem__)
except NameError:
# for python 3
nearest_node_index = distance_list.index(
min(distance_list))
nearest_node = node_list[nearest_node_index]
# Create new point in the direction of sampled point
theta = math.atan2(
rnd_point.y - nearest_node.state.position.y,
rnd_point.x - nearest_node.state.position.x,
)
new_point = Point()
new_point.x = (nearest_node.state.position.x +
self.expand_dis * math.cos(theta))
new_point.y = (nearest_node.state.position.y +
self.expand_dis * math.sin(theta))
# Check whether new point is inside an obstacles
if not env.get_status(RobotState(position=new_point)):
continue
else:
new_node = Node.from_coordinates(new_point)
new_node.parent = nearest_node
node_list.append(new_node)
# Check if goal has been reached or if there is direct connection to goal
del_x, del_y, del_z = (
new_node.state.position.x - goal_node.state.position.x,
new_node.state.position.y - goal_node.state.position.y,
new_node.state.position.z - goal_node.state.position.z,
)
distance_to_goal = math.sqrt(del_x**2 + del_y**2 + del_z**2)
traj = Trajectory(path=[RobotState(position=new_point), goal])
if distance_to_goal < self.expand_dis or env.get_traj_status(
traj):
goal_node.parent = node_list[-1]
node_list.append(goal_node)
print("Goal reached!")
break
else:
goal_node.parent = start_node
node_list = [start_node, goal_node]
# Construct path by traversing backwards through the tree
path = []
last_node = node_list[-1]
while node_list[node_list.index(last_node)].parent is not None:
node = node_list[node_list.index(last_node)]
path.append(RobotState(position=node.state.position))
last_node = node.parent
path.append(start)
path = Trajectory(list(reversed(path)))
return path
def plan(self, start, goal, env):
"""Plans path from start to goal avoiding obstacles.
Args:
start_point(gennav.utils.RobotState): with start point coordinates.
end_point (gennav.utils.RobotState): with end point coordinates.
env: (gennav.envs.Environment) Base class for an envrionment.
Returns:
gennav.utils.Trajectory: The planned path as trajectory
dict: Dictionary containing additional information like the node_list
"""
# Check if start and goal states are obstacle free
if not env.get_status(start):
raise InvalidStartState(start,
message="Start state is in obstacle.")
if not env.get_status(goal):
raise InvalidGoalState(goal, message="Goal state is in obstacle.")
# Initialize start and goal nodes
start_node = Node(state=start)
goal_node = Node(state=goal)
# Initialize node_list with start
node_list = [start_node]
# Loop to keep expanding the tree towards goal if there is no direct connection
traj = Trajectory(path=[start, goal])
if not env.get_traj_status(traj):
while True:
# Sample random state in specified area
rnd_state = self.sampler()
# Find nearest node to the sampled point
distance_list = [
compute_distance(node.state.position, rnd_state.position)
for node in node_list
]
try:
# for python2
nearest_node_index = min(xrange(len(distance_list)),
key=distance_list.__getitem__)
except NameError:
# for python 3
nearest_node_index = distance_list.index(
min(distance_list))
nearest_node = node_list[nearest_node_index]
# Create new point in the direction of sampled point
theta = math.atan2(
rnd_state.position.y - nearest_node.state.position.y,
rnd_state.position.x - nearest_node.state.position.x,
)
new_point = Point()
new_point.x = (nearest_node.state.position.x +
self.expand_dis * math.cos(theta))
new_point.y = (nearest_node.state.position.y +
self.expand_dis * math.sin(theta))
# Check whether new point is inside an obstacles
if not env.get_status(RobotState(position=new_point)):
continue
else:
new_node = Node.from_coordinates(new_point)
new_node.parent = nearest_node
node_list.append(new_node)
# Check if goal has been reached or if there is direct connection to goal
distance_to_goal = compute_distance(goal_node.state.position,
new_node.state.position)
traj = Trajectory(path=[RobotState(position=new_point), goal])
if distance_to_goal < self.expand_dis or env.get_traj_status(
traj):
goal_node.parent = node_list[-1]
node_list.append(goal_node)
print("Goal reached!")
break
else:
goal_node.parent = start_node
node_list = [start_node, goal_node]
# Construct path by traversing backwards through the tree
path = []
last_node = node_list[-1]
while last_node.parent is not None:
path.append(last_node.state)
last_node = last_node.parent
path.append(start)
info_dict = {}
info_dict["node_list"] = node_list
path = Trajectory(path[::-1])
if len(path.path) == 1:
raise PathNotFound(path, message="Path contains only one state")
return (path, info_dict)
def plan(self, start, goal, env):
"""Path planning method
Args:
start (gennav.utils.common.RobotState): Starting state.
goal (gennav.utils.common.RobotState): Destination state
env (gennav.envs.Environment): Environment object
Returns:
gennav.utils.Trajectory: The path as a Trajectory
dict: Dictionary containing additional information like the node_list
"""
# Check if start and goal states are obstacle free
if not env.get_status(start):
raise InvalidStartState(start, message="Start state is in obstacle.")
if not env.get_status(goal):
raise InvalidGoalState(goal, message="Goal state is in obstacle.")
# Initialize start and goal nodes.
x_start = Node(state=start)
x_goal = Node(state=goal)
# Starting the tree:
node_list = [x_start]
# X_soln contains all the nodes near the goal
X_soln = []
for i in range(self.max_iter):
# defining the best cost as cbest
if len(X_soln) == 0:
cbest = float("inf")
else:
cbest = X_soln[0].cost + compute_distance(
X_soln[0].state.position, x_goal.state.position
)
for node in X_soln:
if (
node.cost
+ compute_distance(node.state.position, x_goal.state.position)
< cbest
):
cbest = node.cost + compute_distance(
node.state.position, x_goal.state.position
)
# sample the new random point using the sample function
x_rand = self.rectangular_sampler()
x_rand_node = Node(state=x_rand)
# Finding the nearest node to the random point
distance_list = [
compute_distance(node.state.position, x_rand_node.state.position)
for node in node_list
]
min_dist = distance_list[0]
min_index = 0
for index, distance in enumerate(distance_list):
if distance < min_dist:
min_dist = distance
min_index = index
x_nearest = node_list[min_index]
# Steering at a distance of self.expand_dis
theta = compute_angle(x_nearest.state.position, x_rand_node.state.position)
x = x_nearest.state.position.x + self.expand_dis * math.cos(theta)
y = x_nearest.state.position.y + self.expand_dis * math.sin(theta)
t = RobotState(position=Point(x, y))
x_new = Node(state=t)
# Defining a temporary trajectory between the new node and its nearest node
traj = Trajectory(path=[x_nearest.state, x_new.state])
# Checking whether new node and the nearest node can be connected
if env.get_traj_status(traj):
# Adding new node to the node list
node_list.append(x_new)
# Defining the neighbours of x_new
X_near = []
for node in node_list:
if (compute_distance(node.state.position, x_new.state.position)) < (
self.neighbourhood_radius
) and ( # If it is inside the neighbourhood radius
compute_distance(node.state.position, x_new.state.position) != 0
):
X_near.append(node)
x_min = x_nearest
c_min = x_min.cost + self.expand_dis
for x_near in X_near:
c_new = x_near.cost + compute_distance(
x_near.state.position, x_new.state.position
)
if c_new < c_min:
traj = Trajectory(path=[x_near.state, x_new.state])
if env.get_traj_status(traj):
x_min = x_near
c_min = c_new
# Defining the parent node and cost of x_new
x_new.parent = x_min
x_new.cost = c_min
# Rewiring
for x_near in X_near:
c_near = x_near.cost
c_new = x_new.cost + compute_distance(
x_near.state.position, x_new.state.position
)
if c_new < c_near:
traj = Trajectory(path=[x_new.state, x_near.state])
if env.get_traj_status(traj):
x_near.parent = x_new
x_near.cost = c_new
goal_traj = Trajectory(path=[x_new.state, x_goal.state])
if (
compute_distance(x_goal.state.position, x_new.state.position)
) < self.goal_distance and env.get_traj_status(goal_traj):
X_soln.append(x_new)
# X_soln contains all the solutions
if len(X_soln) == 0:
print("No solution found!")
path = []
path.append(x_start.state)
traj = Trajectory(path=path)
info_dict = {}
info_dict["node_list"] = node_list
if len(traj.path) == 1:
raise PathNotFound(path, message="Path contains only one state")
return (traj, info_dict)
else:
print("Goal reached!")
Cbest = X_soln[0].cost
Xbest = X_soln[0]
node_list.append(x_goal)
for node in X_soln:
if node.cost < Cbest:
Cbest = node.cost
Xbest = node
x_goal.parent = Xbest
# Xbest is the optimum solution to the problem.
# Back tracing the path:
path = []
path.append(x_goal.state)
path.append(Xbest.state)
p = Xbest.parent
while p is not None:
path.append(p.state)
p = p.parent
path.append(x_start.state)
path.reverse()
# print("No. of solutions " + str(len(X_soln)))
trajectory = Trajectory(path=path)
info_dict = {}
info_dict["node_list"] = node_list
return (trajectory, info_dict)
def plan(self, start, end, env):
"""Plans path from start to goal avoiding obstacles.
Args:
start (gennav.utils.RobotState): Starting state
end (gennav.utils.RobotState): Goal state
env: (gennav.envs.Environment) Base class for an envrionment.
Returns:
gennav.utils.Trajectory: The planned path as trajectory
dict: Dictionary containing additional information
"""
# Check if start and end states are obstacle free
if not env.get_status(start):
raise InvalidStartState(start,
message="Start state is in obstacle.")
if not env.get_status(end):
raise InvalidGoalState(end, message="Goal state is in obstacle.")
# Initialize graph
graph = Graph()
graph.add_node(start)
# Initialize node list with Starting Position
node_list = [start]
# Loop for maximum iterations to get the best possible path
for iter in range(self.max_iter):
rnd_node = self.sampler()
# Find nearest node to the sampled point
distance_list = [
compute_distance(node.position, rnd_node.position)
for node in node_list
]
try:
# for python2
nearest_node_index = min(xrange(len(distance_list)),
key=distance_list.__getitem__)
except NameError:
# for python 3
nearest_node_index = distance_list.index(min(distance_list))
nearest_node = node_list[nearest_node_index]
# Create new point in the direction of sampled point
theta = math.atan2(
rnd_node.position.y - nearest_node.position.y,
rnd_node.position.x - nearest_node.position.x,
)
new_node = RobotState(position=Point(
nearest_node.position.x + self.exp_dis * math.cos(theta),
nearest_node.position.y + self.exp_dis * math.sin(theta),
))
# Check whether new point is inside an obstacles
trj = Trajectory([nearest_node, new_node])
if not env.get_status(new_node) or not env.get_traj_status(trj):
continue
else:
node_list.append(new_node)
# FINDING NEAREST INDICES
nnode = len(node_list) + 1
# The circle in which to check parent node and rewiring
r = self.circle * math.sqrt((math.log(nnode) / nnode))
dist_list = [
compute_distance(node.position, new_node.position)
for node in node_list
]
# Getting all the indexes within r units of new_node
near_inds = [dist_list.index(i) for i in dist_list if i <= r**2]
graph.add_node(new_node)
graph.add_edge(nearest_node, new_node)
# Add all the collision free edges to the graph.
for ind in near_inds:
node_check = node_list[ind]
trj = Trajectory([new_node, node_check])
# If the above condition is met append the edge.
if env.get_traj_status(trj):
graph.add_edge(new_node, node_check)
if end not in graph.nodes:
min_cost = 2.0
closest_node = RobotState()
for node in graph.nodes:
temp = compute_distance(node.position, end.position)
if env.get_traj_status(Trajectory([node, end
])) and temp < min_cost:
min_cost = temp
closest_node = node
graph.add_edge(closest_node, end)
graph.add_node(end)
if start in graph.edges[end]:
graph.del_edge(start, end)
path = []
p = astar(graph, start, end)
path.extend(p.path)
if len(path) != 1:
print("Goal Reached!")
path = Trajectory(path)
if len(path.path) == 1:
raise PathNotFound(path, message="Path contains only one state")
return path, {}
raise NotImplementedError("Specified controller ", env_name,
" is not implemented")
# Get message type object
msg_dtype_name = param_dict["msg_dtype_name"]
if msg_dtype_name in msg_dtype_registry.keys():
msg_dtype = msg_dtype_registry[msg_dtype_name]
else:
raise NotImplementedError("Specified message type ", planner_name,
" is not compatible")
# Construct controller
controller_node = gennav_ros.Controller(controller)
# Construct commander
replan_interval = rospy.Duration(param_dict["replan_interval"])
commander_node = gennav_ros.Commander(
planner=planner,
env=env,
replan_interval=replan_interval,
msg_dtype=msg_dtype,
)
# Execute command
goal_state = RobotState(position=Point(*goal))
start_state = RobotState(position=Point(*start))
commander_node.goto(goal_state, start_state)
# ROS Magic (spin)
rospy.spin()
def plan(self, start, goal, env):
"""Path planning method
Args:
start (gennav.utils.common.RobotState): Starting state.
goal (gennav.utils.common.RobotState): Destination state
env (gennav.envs.Environment): Environment object
Returns:
gennav.utils.Trajectory: The path as a Trajectory
dict: Dictionary containing additional information like the node_list
"""
# Check if start and goal states are obstacle free
if not env.get_status(start):
raise InvalidStartState(start,
message="Start state is in obstacle.")
if not env.get_status(goal):
raise InvalidGoalState(goal, message="Goal state is in obstacle.")
# Initialize start and goal nodes.
x_start = Node(state=start)
x_goal = Node(state=goal)
# Starting the tree:
node_list = [x_start]
# X_soln contains all the nodes near the goal
X_soln = []
for i in range(self.max_iter):
# defining the best cost as cbest
if len(X_soln) == 0:
cbest = float("inf")
else:
cbest = X_soln[0].cost + compute_distance(
X_soln[0].state.position, x_goal.state.position)
for node in X_soln:
if (node.cost + compute_distance(node.state.position,
x_goal.state.position) <
cbest):
cbest = node.cost + compute_distance(
node.state.position, x_goal.state.position)
# sample the new random point using the sample function
if cbest < float("inf"):
# Converting x_start and x_goal into arrays so that matrix calculations can be handled
x_start_array = np.array(
[x_start.state.position.x, x_start.state.position.y],
dtype=np.float32,
).reshape(2, 1)
x_goal_array = np.array(
[x_goal.state.position.x, x_goal.state.position.y],
dtype=np.float32).reshape(2, 1)
cmin = np.linalg.norm((x_goal_array - x_start_array))
x_center = (x_start_array + x_goal_array) / 2
a1 = (x_goal_array - x_start_array) / cmin
I1 = np.eye(len(x_start_array))[0].reshape(
len(x_start_array), 1)
M = np.dot(a1, I1.T)
[u, s, v] = np.linalg.svd(M)
# numpy returns the transposed value for v, so to convert it back
v = v.T
# Calculating the rotation to world frame matrix given by C = U*diag{1,....1.det(U)det(V)}*V.T where * denotes matrix multiplication
diag = np.eye(2)
diag[1, 1] = np.linalg.det(u) * np.linalg.det(v)
C = np.dot(np.dot(u, diag), v.T)
L = np.eye(2)
L[0, 0] = cbest / 2
L[1, 1] = np.sqrt(cbest**2 - cmin**2) / 2
rand_state = self.circular_sampler()
x_ball = np.array(
[rand_state.position.x, rand_state.position.y],
dtype=np.float32).reshape(2, 1)
x_rand_array = np.dot(np.dot(C, L), x_ball) + x_center
x_rand = RobotState()
x_rand.position.x = x_rand_array[0, 0]
x_rand.position.y = x_rand_array[1, 0]
x_rand_node = Node(state=x_rand)
else:
x_rand = self.rectangular_sampler()
x_rand_node = Node(state=x_rand)
# Finding the nearest node to the random point
distance_list = [
compute_distance(node.state.position,
x_rand_node.state.position)
for node in node_list
]
min_dist = distance_list[0]
min_index = 0
for index, distance in enumerate(distance_list):
if distance < min_dist:
min_dist = distance
min_index = index
x_nearest = node_list[min_index]
# Steering at a distance of self.expand_dis
theta = compute_angle(x_nearest.state.position,
x_rand_node.state.position)
x = x_nearest.state.position.x + self.expand_dis * math.cos(theta)
y = x_nearest.state.position.y + self.expand_dis * math.sin(theta)
t = RobotState(position=Point(x, y))
x_new = Node(state=t)
# Defining a temporary trajectory between the new node and its nearest node
traj = Trajectory(path=[x_nearest.state, x_new.state])
# Checking whether new node and the nearest node can be connected
if env.get_traj_status(traj):
# Adding new node to the node list
node_list.append(x_new)
# Defining the neighbours of x_new
X_near = []
for node in node_list:
if (compute_distance(
node.state.position, x_new.state.position)) < (
self.neighbourhood_radius
) and ( # If it is inside the neighbourhood radius
compute_distance(node.state.position,
x_new.state.position) != 0):
X_near.append(node)
x_min = x_nearest
c_min = x_min.cost + self.expand_dis
for x_near in X_near:
c_new = x_near.cost + compute_distance(
x_near.state.position, x_new.state.position)
if c_new < c_min:
traj = Trajectory(path=[x_near.state, x_new.state])
if env.get_traj_status(traj):
x_min = x_near
c_min = c_new
# Defining the parent node and cost of x_new
x_new.parent = x_min
x_new.cost = c_min
# Rewiring
for x_near in X_near:
c_near = x_near.cost
c_new = x_new.cost + compute_distance(
x_near.state.position, x_new.state.position)
if c_new < c_near:
traj = Trajectory(path=[x_new.state, x_near.state])
if env.get_traj_status(traj):
x_near.parent = x_new
x_near.cost = c_new
goal_traj = Trajectory(path=[x_new.state, x_goal.state])
if (compute_distance(x_goal.state.position,
x_new.state.position)
) < self.goal_distance and env.get_traj_status(goal_traj):
X_soln.append(x_new)
# X_soln contains all the solutions
if len(X_soln) == 0:
print("No solution found!")
path = []
path.append(x_start.state)
traj = Trajectory(path=path)
info_dict = {}
info_dict["node_list"] = node_list
if len(traj.path) == 1:
raise PathNotFound(path,
message="Path contains only one state")
return (traj, info_dict)
else:
print("Goal reached!")
Cbest = X_soln[0].cost
Xbest = X_soln[0]
node_list.append(x_goal)
for node in X_soln:
if node.cost < Cbest:
Cbest = node.cost
Xbest = node
x_goal.parent = Xbest
# Xbest is the optimum solution to the problem.
# Back tracing the path:
path = []
path.append(x_goal.state)
path.append(Xbest.state)
p = Xbest.parent
while p is not None:
path.append(p.state)
p = p.parent
path.append(x_start.state)
path.reverse()
# print("No. of solutions " + str(len(X_soln)))
trajectory = Trajectory(path=path)
info_dict = {}
info_dict["node_list"] = node_list
return (trajectory, info_dict)
UniformRectSampler, )
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
# A list of obstacles for the planner.
sampler = UniformRectSampler(-5, 15, -5, 15) # for uniformly sampling points
poly = PolygonEnv(
buffer_dist=1.0
) # intializing the environment object needed for updating the obstacles of the environment
my_tree = RRG(sampler=sampler, expand_dis=1.0,
max_iter=500) # initializing RRG planner object
start = RobotState(position=Point(0, 0)) # setting start point to (0,0)
goal = RobotState(position=Point(10, 10)) # setting end point to (0,0)
for obstacles in general_obstacles_list: # updating the environment with all obstacles
poly.update(obstacles)
path, _ = my_tree.plan(start, goal, poly) # planning the path
visualize_path(path, poly) # visualizing path and obstacles
