Coverage for src/pyroboplan/planning/utils.py: 100%
47 statements
« prev ^ index » next coverage.py v7.6.12, created at 2025-03-02 22:03 -0500
1"""Utilities for motion planning."""
3import numpy as np
5from ..core.utils import (
6 check_collisions_at_state,
7 check_collisions_along_path,
8 configuration_distance,
9 get_random_state,
10)
12from itertools import product
15def extend_robot_state(q_parent, q_sample, max_connection_distance):
16 """
17 Determines an incremental robot configuration between the parent and sample states, if one exists.
19 Parameters
20 ----------
21 q_parent : array-like
22 The starting robot configuration.
23 q_sample : array-like
24 The candidate sample configuration to extend towards.
25 max_connection_distance : float
26 Maximum angular distance, in radians, for connecting nodes.
28 Returns
29 -------
30 array-like
31 The resulting robot configuration, or None if it is not feasible.
32 """
33 q_diff = q_sample - q_parent
34 distance = np.linalg.norm(q_diff)
35 if distance == 0.0:
36 return q_sample
37 q_increment = max_connection_distance * q_diff / distance
39 q_cur = q_parent
40 # Clip the distance between nearest and sampled nodes to max connection distance.
41 # If we have reached the sampled node, then we just check that.
42 if configuration_distance(q_cur, q_sample) > max_connection_distance:
43 q_extend = q_cur + q_increment
44 else:
45 q_extend = q_sample
47 # Then there are no collisions so the extension is valid
48 return q_extend
51def has_collision_free_path(
52 q1,
53 q2,
54 max_step_size,
55 model,
56 collision_model,
57 data=None,
58 collision_data=None,
59 distance_padding=0.0,
60):
61 """
62 Determines if there is a collision free path between the provided nodes and models.
64 Parameters
65 ----------
66 q1 : array-like
67 The starting robot configuration.
68 q2 : array-like
69 The destination robot configuration.
70 max_step_size : float
71 Maximum joint configuration step size for collision checking along path segments.
72 model : `pinocchio.Model`
73 The model for the robot configuration.
74 collision_model : `pinocchio.Model`
75 The model to use for collision checking.
76 data : `pinocchio.Data`, optional
77 The model data to use for this solver. If None, data is created automatically.
78 collision_data : `pinocchio.GeometryData`, optional
79 The collision_model data to use for this solver. If None, data is created automatically.
80 distance_padding : float, optional
81 The padding, in meters, to use for distance to nearest collision.
83 Returns
84 -------
85 bool
86 True if the configurations can be connected, False otherwise.
87 """
88 # Ensure the destination is collision free.
89 if check_collisions_at_state(
90 model,
91 collision_model,
92 q2,
93 data,
94 collision_data,
95 distance_padding=distance_padding,
96 ):
97 return False
99 # Ensure the discretized path is collision free.
100 path_to_q_extend = discretize_joint_space_path([q1, q2], max_step_size)
101 if check_collisions_along_path(
102 model,
103 collision_model,
104 path_to_q_extend,
105 data,
106 collision_data,
107 distance_padding=distance_padding,
108 ):
109 return False
111 return True
114def discretize_joint_space_path(q_path, max_angle_distance):
115 """
116 Discretizes a joint space path given a maximum angle distance between samples.
118 This is used primarily for producing paths for collision checking.
120 Parameters
121 ----------
122 q_path : list[array-like]
123 A list of the joint configurations describing a path.
124 max_angle_distance : float
125 The maximum angular displacement, in radians, between samples.
127 Returns
128 -------
129 list[array-like]
130 A list of joint configuration arrays between the start and end points, inclusive.
131 """
132 q_discretized = []
133 for idx in range(1, len(q_path)):
134 q_start = q_path[idx - 1]
135 q_end = q_path[idx]
136 q_diff = q_end - q_start
137 num_steps = int(np.ceil(np.linalg.norm(q_diff) / max_angle_distance)) + 1
138 step_vec = np.linspace(0.0, 1.0, num_steps)
139 q_discretized.extend([q_start + step * q_diff for step in step_vec])
140 return q_discretized
143def retrace_path(goal_node):
144 """
145 Retraces a path to the specified `goal_node` from a root node (a node with no parent).
147 The resulting path will be returned in order form the start at index `0` to the `goal_node`
148 at the index `-1`.
150 Parameters
151 ----------
152 goal_node : `pyroboplan.planning.graph.Node`
153 The starting joint configuration.
155 Returns
156 -------
157 list[`pyroboplan.planning.graph.Node`]
158 A list a nodes from the root to the specified `goal_node`.
160 """
161 path = []
162 current = goal_node
163 while current:
164 path.append(current)
165 current = current.parent
166 path.reverse()
167 return path
170def discretized_joint_space_generator(model, step_size, generate_random=True):
171 """
172 Discretizes the entire joint space of the model at step_size increments.
173 Once the entire space has been returned, the generator can optionally continue
174 returning random samples from the configuration space - in which case this
175 generator will never terminate.
177 This is an extraordinarily expensive operation for high DOF manipulators
178 and small step sizes!
180 Parameters
181 ----------
182 model : `pinocchio.Model`
183 The robot model containing lower and upper position limits.
184 step_size : float
185 The step size for sampling.
186 generate_random : bool
187 If True, continue randomly sampling the configuration space.
188 Otherwise this generator will terminate.
190 Yields
191 ------
192 np.ndarray
193 The next point in the configuration space.
194 """
195 lower = model.lowerPositionLimit
196 upper = model.upperPositionLimit
198 # Ensure the range is inclusive of endpoints
199 ranges = [np.arange(l, u + step_size, step_size) for l, u in zip(lower, upper)]
200 for point in product(*ranges):
201 yield np.array(point)
203 # Once we have iterated through all available points we return random samples.
204 while generate_random:
205 yield get_random_state(model)