Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix RRT options instantiation and move them to planner constructor #24

Merged
merged 1 commit into from
Apr 21, 2024
Merged

Fix RRT options instantiation and move them to planner constructor #24

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
25 changes: 13 additions & 12 deletions examples/example_rrt.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,18 +32,19 @@
time.sleep(1.0)

# Search for a path
options = RRTPlannerOptions()
options.max_angle_step = 0.05
options.max_connection_dist = 0.25
options.goal_biasing_probability = 0.15
options.max_planning_time = 10.0
options.rrt_connect = False
options.bidirectional_rrt = False
options.rrt_star = False
options.max_rewire_dist = 3.0

planner = RRTPlanner(model, collision_model)
path = planner.plan(q_start, q_end, options=options)
options = RRTPlannerOptions(
max_angle_step=0.05,
max_connection_dist=0.25,
goal_biasing_probability=0.15,
max_planning_time=10.0,
rrt_connect=False,
bidirectional_rrt=False,
rrt_star=False,
max_rewire_dist=3.0,
)

planner = RRTPlanner(model, collision_model, options=options)
path = planner.plan(q_start, q_end)
planner.visualize(viz, "panda_hand", show_tree=True)

# Animate the path
Expand Down
136 changes: 82 additions & 54 deletions src/pyroboplan/planning/rrt.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,38 +19,51 @@
class RRTPlannerOptions:
"""Options for Rapidly-expanding Random Tree (RRT) planning."""

max_angle_step = 0.05
""" Maximum angle step, in radians, for collision checking along path segments. """

max_connection_dist = 0.2
""" Maximum angular distance, in radians, for connecting nodes. """

rrt_connect = False
""" If true, enables the RRTConnect algorithm. """

bidirectional_rrt = False
"""
If true, uses bidirectional RRTs from both start and goal nodes.
Otherwise, only grows a tree from the start node.
"""

rrt_star = False
"""
If true, enables the RRT* algorithm.
This in turn will use the `max_rewire_dist` parameter.
"""

max_rewire_dist = np.inf
"""
Maximum angular distance, in radians, to consider rewiring nodes for RRT*.
If set to `np.inf`, all nodes in the trees will be considering for rewiring.
"""

max_planning_time = 10.0
""" Maximum planning time, in seconds. """
def __init__(
self,
max_angle_step=0.05,
max_connection_dist=0.2,
rrt_connect=False,
bidirectional_rrt=False,
rrt_star=False,
max_rewire_dist=np.inf,
max_planning_time=10.0,
goal_biasing_probability=0.0,
):
"""
Initializes a set of RRT planner options.

goal_biasing_probability = 0.0
""" Probability of sampling the goal configuration itself, which can help planning converge. """
Parameters
----------
max_angle_step : float
Maximum angle step, in radians, for collision checking along path segments.
max_connection_dist : float
Maximum angular distance, in radians, for connecting nodes.
rrt_connect : bool
If true, uses bidirectional RRTs from both start and goal nodes.
Otherwise, only grows a tree from the start node.
bidirectional_rrt : bool
If true, enables the RRTConnect algorithm, which continues extending
nodes towards a random node until an invalid state is reached.
rrt_star : bool
If true, enables the RRT* algorithm to shortcut node connections during planning.
This in turn will use the `max_rewire_dist` parameter.
max_rewire_dist : float
Maximum angular distance, in radians, to consider rewiring nodes for RRT*.
If set to `np.inf`, all nodes in the trees will be considered for rewiring.
max_planning_time : float
Maximum planning time, in seconds.
goal_biasing_probability : float
Probability of sampling the goal configuration itself, which can help planning converge.
"""
self.max_angle_step = max_angle_step
self.max_connection_dist = max_connection_dist
self.rrt_connect = rrt_connect
self.bidirectional_rrt = bidirectional_rrt
self.rrt_star = rrt_star
self.max_rewire_dist = max_rewire_dist
self.max_planning_time = max_planning_time
self.goal_biasing_probability = goal_biasing_probability


class RRTPlanner:
Expand All @@ -64,7 +77,7 @@ class RRTPlanner:
* RRT* and PRM* paper: https://arxiv.org/abs/1105.1186
"""

def __init__(self, model, collision_model):
def __init__(self, model, collision_model, options=RRTPlannerOptions()):
"""
Creates an instance of an RRT planner.

Expand All @@ -74,9 +87,12 @@ def __init__(self, model, collision_model):
The model to use for this solver.
collision_model : `pinocchio.Model`
The model to use for collision checking.
options : `RRTPlannerOptions`, optional
The options to use for planning. If not specified, default options are used.
"""
self.model = model
self.collision_model = collision_model
self.options = options
self.reset()

def reset(self):
Expand All @@ -85,7 +101,7 @@ def reset(self):
self.start_tree = Graph()
self.goal_tree = Graph()

def plan(self, q_start, q_goal, options=RRTPlannerOptions()):
def plan(self, q_start, q_goal):
"""
Plans a path from a start to a goal configuration.

Expand All @@ -95,12 +111,9 @@ def plan(self, q_start, q_goal, options=RRTPlannerOptions()):
The starting robot configuration.
q_start : array-like
The goal robot configuration.
options : `RRTPlannerOptions`, optional
The options to use for planning. If not specified, default options are used.
"""
self.reset()
t_start = time.time()
self.options = options

start_node = Node(q_start, parent=None, cost=0.0)
self.start_tree.add_node(start_node)
Expand Down Expand Up @@ -132,8 +145,10 @@ def plan(self, q_start, q_goal, options=RRTPlannerOptions()):
start_tree_phase = True
while not goal_found:
# Check for timeouts.
if time.time() - t_start > options.max_planning_time:
print(f"Planning timed out after {options.max_planning_time} seconds.")
if time.time() - t_start > self.options.max_planning_time:
print(
f"Planning timed out after {self.options.max_planning_time} seconds."
)
break

# Choose variables based on whether we're growing the start or goal tree.
Expand All @@ -148,11 +163,11 @@ def plan(self, q_start, q_goal, options=RRTPlannerOptions()):

# Run the extend or connect operation to connect the tree to the new node.
nearest_node = tree.get_nearest_node(q_sample)
q_new = self.extend_or_connect(nearest_node, q_sample, options)
q_new = self.extend_or_connect(nearest_node, q_sample)

# Only if extend/connect succeeded, add the new node to the tree.
if q_new is not None:
new_node = self.add_node_to_tree(tree, q_new, nearest_node, options)
new_node = self.add_node_to_tree(tree, q_new, nearest_node)
if start_tree_phase:
latest_start_tree_node = new_node
else:
Expand All @@ -170,7 +185,7 @@ def plan(self, q_start, q_goal, options=RRTPlannerOptions()):
self.model, self.collision_model, path_to_other_tree
):
new_node = self.add_node_to_tree(
tree, nearest_node_in_other_tree.q, new_node, options
tree, nearest_node_in_other_tree.q, new_node
)
if start_tree_phase:
latest_start_tree_node = new_node
Expand All @@ -181,7 +196,7 @@ def plan(self, q_start, q_goal, options=RRTPlannerOptions()):
goal_found = True

# Switch to the other tree next iteration, if bidirectional mode is enabled.
if options.bidirectional_rrt:
if self.options.bidirectional_rrt:
start_tree_phase = not start_tree_phase

# Back out the path by traversing the parents from the goal.
Expand All @@ -192,7 +207,7 @@ def plan(self, q_start, q_goal, options=RRTPlannerOptions()):
)
return self.latest_path

def extend_or_connect(self, parent_node, q_sample, options=RRTPlannerOptions()):
def extend_or_connect(self, parent_node, q_sample):
"""
Extends a tree towards a sampled node with steps no larger than the maximum connection distance.

Expand All @@ -202,21 +217,20 @@ def extend_or_connect(self, parent_node, q_sample, options=RRTPlannerOptions()):
The node from which to start extending or connecting towards the sample.
q_sample : array-like
The robot configuration sample to extend or connect towards.
options : `pyroboplan.planning.rrt.RRTPlannerOptions`, optional
The options to use for this operation.
These include whether to extend once or keep connecting (`options.rrt_connect`),
as well as the maximum angular connection distance (`options.max_connection_dist`).
"""
q_diff = q_sample - parent_node.q
q_increment = options.max_connection_dist * q_diff / np.linalg.norm(q_diff)
q_increment = self.options.max_connection_dist * q_diff / np.linalg.norm(q_diff)

terminated = False
q_out = None
q_cur = parent_node.q
while not terminated:
# Clip the distance between nearest and sampled nodes to max connection distance.
# If we have reached the sampled node, this is the final iteration.
if configuration_distance(q_cur, q_sample) > options.max_connection_dist:
if (
configuration_distance(q_cur, q_sample)
> self.options.max_connection_dist
):
q_extend = q_cur + q_increment
else:
q_extend = q_sample
Expand All @@ -238,7 +252,7 @@ def extend_or_connect(self, parent_node, q_sample, options=RRTPlannerOptions()):
terminated |= True

# If RRTConnect is disabled, only one iteration is needed.
if not options.rrt_connect:
if not self.options.rrt_connect:
terminated |= True

return q_out
Expand Down Expand Up @@ -282,9 +296,23 @@ def extract_path_from_trees(self, start_tree_final_node, goal_tree_final_node):

return path

def add_node_to_tree(self, tree, q_new, parent_node, options=RRTPlannerOptions()):
def add_node_to_tree(self, tree, q_new, parent_node):
"""
Add a new node to the tree. If the RRT* algorithm is enabled, will also rewire.

Parameters
----------
tree : `pyroboplan.planning.graph.Graph`
The tree to which to add the new node.
q_new : array-like
The robot configuration from which to create a new tree node.
parent_node : `pyroboplan.planning.graph.Node`
The parent node to connect the new node to.

Returns
-------
`pyroboplan.planning.graph.Node`
The new node that was added to the tree.
"""
# Add the new node to the tree
new_node = Node(q_new, parent=parent_node)
Expand All @@ -293,21 +321,21 @@ def add_node_to_tree(self, tree, q_new, parent_node, options=RRTPlannerOptions()
new_node.cost = parent_node.cost + edge.cost

# If RRT* is enable it, rewire that node in the tree.
if options.rrt_star:
if self.options.rrt_star:
min_cost = new_node.cost
for other_node in tree.nodes:
# Do not consider trivial nodes.
if other_node == new_node or other_node == parent_node:
continue
# Do not consider nodes farther than the configured rewire distance,
new_distance = configuration_distance(other_node.q, q_new)
if new_distance > options.max_rewire_dist:
if new_distance > self.options.max_rewire_dist:
continue
# Rewire if this new connections would be of lower cost and is collision free.
new_cost = other_node.cost + new_distance
if new_cost < min_cost:
new_path = discretize_joint_space_path(
q_new, other_node.q, options.max_angle_step
q_new, other_node.q, self.options.max_angle_step
)
if not check_collisions_along_path(
self.model, self.collision_model, new_path
Expand Down
19 changes: 10 additions & 9 deletions test/planning/test_rrt.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,12 +64,12 @@ def test_plan_rrt_connect():
q_start = np.array([0.0, 1.57, 0.0, 0.0, 1.57, 1.57, 0.0, 0.0, 0.0])
q_goal = np.array([1.57, 1.57, 0.0, 0.0, 1.57, 1.57, 0.0, 0.0, 0.0])

options = RRTPlannerOptions()
options.rrt_connect = True
options.bidirectional_rrt = True

planner = RRTPlanner(model, collision_model)
path = planner.plan(q_start, q_goal, options=options)
options = RRTPlannerOptions(
rrt_connect=True,
bidirectional_rrt=True,
)
planner = RRTPlanner(model, collision_model, options=options)
path = planner.plan(q_start, q_goal)

# The path must exist and have more than the start and goal nodes.
assert path is not None
Expand All @@ -96,13 +96,14 @@ def test_plan_rrt_star():
options = RRTPlannerOptions()
options.rrt_star = False
options.bidirectional_rrt = True
planner = RRTPlanner(model, collision_model)
path_original = planner.plan(q_start, q_goal, options=options)
planner = RRTPlanner(model, collision_model, options=options)
path_original = planner.plan(q_start, q_goal)

# Then, plan with RRT* enabled (use maximum rewire distance for effect).
options.rrt_star = True
options.max_rewire_dist = np.inf
path_star = planner.plan(q_start, q_goal, options=options)
planner = RRTPlanner(model, collision_model, options=options)
path_star = planner.plan(q_start, q_goal)

# The RRT* path must be shorter or of equal length, though it also took longer to plan.
assert path_original is not None
Expand Down
Loading