Module pyastrobee.trajectories.planner
The main trajectory planning structure for Astrobee
We use a global/local hierarchy where:
- The global planner computes a reference trajectory which enforces all of the required constraints, such as Astrobee's speed/acceleration limits, as well as collision avoidance
- The local planner handles very fast trajectory generation between states with arbitrary boundary conditions on dynamics, but does not enforce some of the more computationally intensive optimizations and constraints
This also assumes decoupled dynamics between the position and orientation components
Note: the orientation component of the trajectories technically isn't "optimal", but it works
View Source
"""The main trajectory planning structure for Astrobee
We use a global/local hierarchy where:
- The global planner computes a reference trajectory which enforces all of the required constraints, such as Astrobee's
speed/acceleration limits, as well as collision avoidance
- The local planner handles very fast trajectory generation between states with arbitrary boundary conditions on
dynamics, but does not enforce some of the more computationally intensive optimizations and constraints
This also assumes decoupled dynamics between the position and orientation components
Note: the orientation component of the trajectories technically isn't "optimal", but it works
"""
# TODO add the max-angular-velocity-constrained rotation planner into the global planner (would only really be needed if
# we have a very short time horizon on the position component, i.e. if we're not moving much but rotating a lot)
from typing import Optional
import numpy as np
import numpy.typing as npt
from pyastrobee.trajectories.trajectory import Trajectory
from pyastrobee.trajectories.bezier import bezier_trajectory
from pyastrobee.trajectories.quaternion_interpolation import (
quaternion_interpolation_with_bcs,
)
from pyastrobee.utils.quaternions import quats_to_angular_velocities
from pyastrobee.trajectories.timing import (
spline_duration_heuristic,
rotation_duration_heuristic,
)
from pyastrobee.config.astrobee_motion import LINEAR_SPEED_LIMIT, LINEAR_ACCEL_LIMIT
from pyastrobee.config.iss_safe_boxes import ROBOT_SAFE_SET
from pyastrobee.config.iss_paths import GRAPH, PATHS
from pyastrobee.utils.boxes import Box, find_containing_box_name, compute_graph
from pyastrobee.utils.algos import dfs
from pyastrobee.trajectories.curve_utils import traj_from_curve
from pyastrobee.trajectories.variable_time_curves import (
free_final_time_bezier,
free_final_time_spline,
)
def local_planner(
p0: npt.ArrayLike,
q0: npt.ArrayLike,
v0: Optional[npt.ArrayLike],
w0: npt.ArrayLike,
a0: Optional[npt.ArrayLike],
dw0: npt.ArrayLike,
pf: npt.ArrayLike,
qf: npt.ArrayLike,
vf: Optional[npt.ArrayLike],
wf: npt.ArrayLike,
af: Optional[npt.ArrayLike],
dwf: npt.ArrayLike,
duration: float,
dt: float,
) -> Trajectory:
"""Generate an optimal Bezier-curve-based position trajectory with a polynomial orientation component
Note that this planner prioritizes solve time over constraint enforcement. This will plan with boundary conditions
in mind, but other than that, it does not enforce things like speed/acceleration limits or collision avoidance. This
allows us to use this in very fast online applications.
In general, this should be paired with the global planner, so that we can precompute a reference trajectory which
does enforce all constraints (and takes a bit longer to solve), and use this local planner to evaluate different
rollouts between states along this reference trajectory
Args:
p0 (npt.ArrayLike): Initial position, shape (3,)
q0 (npt.ArrayLike): Initial XYZW quaternion, shape (4,)
v0 (npt.ArrayLike): Initial linear velocity, shape (3,)
w0 (npt.ArrayLike): Initial angular velocity, shape (3,)
a0 (npt.ArrayLike): Initial linear acceleration, shape (3,)
dw0 (npt.ArrayLike): Initial angular acceleration, shape (3,)
pf (npt.ArrayLike): Final position, shape (3,)
qf (npt.ArrayLike): Final XYZW quaternion, shape (4,)
vf (npt.ArrayLike): Final linear velocity, shape (3,)
wf (npt.ArrayLike): Final angular velocity, shape (3,)
af (npt.ArrayLike): Final linear acceleration, shape (3,)
dwf (npt.ArrayLike): Final angular acceleration, shape (3,)
duration (float): Trajectory duration (seconds)
dt (float): Sampling period (seconds)
Returns:
Trajectory: The optimal local trajectory plan
"""
# Min-jerk position traj
# Don't need a ton of control points because we're not enforcing constraints
n_control_pts = 8
curve, _ = bezier_trajectory(
p0, pf, 0, duration, n_control_pts, v0, vf, a0, af, time_weight=0
)
pos_traj = traj_from_curve(curve, dt)
n_timesteps = len(pos_traj.times)
quats = quaternion_interpolation_with_bcs(
q0, qf, w0, wf, dw0, dwf, duration, n_timesteps
)
omega = quats_to_angular_velocities(quats, dt)
alpha = np.gradient(omega, dt, axis=0)
return Trajectory(
positions=pos_traj.positions,
quats=quats,
lin_vels=pos_traj.linear_velocities,
ang_vels=omega,
lin_accels=pos_traj.linear_accels,
ang_accels=alpha,
times=pos_traj.times,
)
def global_planner(
p0: npt.ArrayLike,
q0: npt.ArrayLike,
pf: npt.ArrayLike,
qf: npt.ArrayLike,
dt: float,
safe_set: dict[str, Box] = ROBOT_SAFE_SET,
graph: Optional[dict[str, list[str]]] = GRAPH,
) -> Trajectory:
"""Generate an optimal spline-based position trajectory with a polynomial orientation component
This will enforce all of the Astrobee's constraints (such as maximum velocity/acceleration and collision avoidance).
It will also be time-optimal in the sense that 1) The total duration of the curve is minimized, and 2) each Bezier
curve in the spline will have its relative duration (w.r.t. the total duration) optimized to minimize jerk
Note that this can take on the order of 10 seconds to compute, so it is not something that should be called online
Args:
p0 (npt.ArrayLike): Initial position, shape (3,)
q0 (npt.ArrayLike): Initial XYZW quaternion, shape (4,)
pf (npt.ArrayLike): Final position, shape (3,)
qf (npt.ArrayLike): Final XYZW quaternion, shape (4,)
dt (float): Sampling period (seconds)
safe_set (dict[str, Box], optional): Description of the safe set of the environment. Defaults to ROBOT_SAFE_SET
(the precomputed safe-set decomposition for the ISS, accounting for the robot's collision radius)
graph (Optional[dict[str, list[str]]], optional): The graph defining the connections between the safe boxes in
the environment. Defaults to GRAPH (the precomputed safe-set graph for the ISS). Note: if set to None, this
graph will be recomputed from the ROBOT_SAFE_SET parameter
Returns:
Trajectory: The optimal global trajectory plan
"""
# Dynamics parameters: Assume start and end from rest, satisfy operating limits
t0 = 0
v0 = np.zeros(3)
vf = np.zeros(3)
a0 = np.zeros(3)
af = np.zeros(3)
w0 = np.zeros(3)
wf = np.zeros(3)
dw0 = np.zeros(3)
dwf = np.zeros(3)
v_max = LINEAR_SPEED_LIMIT
a_max = LINEAR_ACCEL_LIMIT
# Parameters associated with the retiming optimization or the free-final-time optimization
# will be left at the default values for now
# Determine the path through the safe-space graph
start = find_containing_box_name(p0, safe_set)
end = find_containing_box_name(pf, safe_set)
if graph is None:
graph = compute_graph(safe_set)
path = dfs(graph, start, end)
box_path = [safe_set[p] for p in path]
init_angular_duration = rotation_duration_heuristic(q0, qf)
init_pos_duration, init_timing_fractions = spline_duration_heuristic(
p0, pf, box_path
)
# TODO enforce a minimum final time in the free-final-time optimization
# to make sure that the rotation plan still has enough time to execute
duration = max(init_pos_duration, init_angular_duration)
init_curve_durations = duration * init_timing_fractions
if len(box_path) == 1:
# Crank up the control points for a single Bezier curve so that we can be nice and tight
# against the velocity/accel constraints
n_control_pts = 20
# If our start and end positions are contained within the same free box,
# we can construct the trajectory from a single Bezier curve
curve = free_final_time_bezier(
p0,
pf,
t0,
duration,
n_control_pts,
v0,
vf,
a0,
af,
box_path[0],
v_max,
a_max,
)
else:
# Our start/end positions are not in the same box, so use a spline between boxes
# Use less control points for the spline for optimization stability
n_control_pts = 10
curve = free_final_time_spline(
p0,
pf,
t0,
duration,
n_control_pts,
box_path,
init_curve_durations,
v0,
vf,
a0,
af,
v_max,
a_max,
)
pos_traj = traj_from_curve(curve, dt)
n_timesteps = pos_traj.num_timesteps
quats = quaternion_interpolation_with_bcs(
q0, qf, w0, wf, dw0, dwf, duration, n_timesteps
)
omega = quats_to_angular_velocities(quats, dt)
alpha = np.gradient(omega, dt, axis=0)
return Trajectory(
pos_traj.positions,
quats,
pos_traj.linear_velocities,
omega,
pos_traj.linear_accels,
alpha,
pos_traj.times,
)
Variables
GRAPH
LINEAR_ACCEL_LIMIT
LINEAR_SPEED_LIMIT
PATHS
ROBOT_SAFE_SET
Functions
global_planner
def global_planner(
p0: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
q0: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
pf: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
qf: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
dt: float,
safe_set: dict[str, pyastrobee.utils.boxes.Box] = {'jpm': Box(lower=[3.47650494853664, -0.4984950514633599, -0.7234950514633598], upper=[10.22349505146336, 0.6354950514633599, 0.7234950514633598]), 'node_2': Box(lower=[-0.7234950514633598, -2.18149505146336, -0.8234950514633599], upper=[0.8234950514633599, 0.8144950514633598, 0.64749505146336]), 'eu_lab': Box(lower=[-7.42649505146336, -0.5894950514633599, -0.7234950514633598], upper=[-3.67650494853664, 0.6484950514633598, 0.7234950514633598]), 'us_lab': Box(lower=[-0.7234950514633598, -10.98849505146336, -0.7234950514633598], upper=[0.5974950514633599, -6.0825049485366405, 0.7234950514633598]), 'node_1': Box(lower=[-0.6754950514633598, -16.88849505146336, -0.7234950514633598], upper=[0.7234950514633598, -13.72550494853664, 0.7514950514633598]), 'node_3_a': Box(lower=[3.17650494853664, -15.31249505146336, -0.7234950514633598], upper=[6.04549505146336, -14.97650494853664, 0.3114950514633598]), 'node_3_b': Box(lower=[5.234504948536641, -15.253495051463359, -0.7234950514633598], upper=[7.3694950514633595, -14.38350494853664, 0.6624950514633599]), 'cupola': Box(lower=[6.176504948536641, -14.90349505146336, 1.6435049485366402], upper=[6.3854950514633595, -14.56150494853664, 2.88149505146336]), 'jpm_n2_corridor': Box(lower=[0.3765049485366402, -0.19849505146335983, -0.32349505146335983], upper=[4.523495051463359, 0.19849505146335983, 0.32349505146335983]), 'n2_eu_corridor': Box(lower=[-4.523495051463359, -0.19849505146335983, -0.32349505146335983], upper=[-0.3765049485366402, 0.19849505146335983, 0.32349505146335983]), 'n2_us_corridor': Box(lower=[-0.32349505146335983, -6.62549505146336, -0.32349505146335983], upper=[0.32349505146335983, -1.6245049485366403, 0.32349505146335983]), 'us_n1_corridor': Box(lower=[-0.32349505146335983, -14.07349505146336, -0.32349505146335983], upper=[0.32349505146335983, -10.47650494853664, 0.32349505146335983]), 'n1_n3_corridor': Box(lower=[0.17650494853664014, -15.12349505146336, -0.32349505146335983], upper=[3.9034950514633597, -14.67650494853664, 0.32349505146335983]), 'n3_cupola_corridor': Box(lower=[6.07650494853664, -15.144495051463359, 0.39550494853664014], upper=[6.72349505146336, -14.37650494853664, 1.9024950514633596])},
graph: Optional[dict[str, list[str]]] = {'cupola': ['n3_cupola_corridor'], 'eu_lab': ['n2_eu_corridor'], 'jpm': ['jpm_n2_corridor'], 'jpm_n2_corridor': ['jpm', 'node_2'], 'n1_n3_corridor': ['node_1', 'node_3_a'], 'n2_eu_corridor': ['node_2', 'eu_lab'], 'n2_us_corridor': ['node_2', 'us_lab'], 'n3_cupola_corridor': ['node_3_b', 'cupola'], 'node_1': ['us_n1_corridor', 'n1_n3_corridor'], 'node_2': ['jpm_n2_corridor', 'n2_eu_corridor', 'n2_us_corridor'], 'node_3_a': ['node_3_b', 'n1_n3_corridor'], 'node_3_b': ['node_3_a', 'n3_cupola_corridor'], 'us_lab': ['n2_us_corridor', 'us_n1_corridor'], 'us_n1_corridor': ['us_lab', 'node_1']}
) -> pyastrobee.trajectories.trajectory.Trajectory
Generate an optimal spline-based position trajectory with a polynomial orientation component
This will enforce all of the Astrobee's constraints (such as maximum velocity/acceleration and collision avoidance). It will also be time-optimal in the sense that 1) The total duration of the curve is minimized, and 2) each Bezier curve in the spline will have its relative duration (w.r.t. the total duration) optimized to minimize jerk
Note that this can take on the order of 10 seconds to compute, so it is not something that should be called online
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
| p0 | npt.ArrayLike | Initial position, shape (3,) | None |
| q0 | npt.ArrayLike | Initial XYZW quaternion, shape (4,) | None |
| pf | npt.ArrayLike | Final position, shape (3,) | None |
| qf | npt.ArrayLike | Final XYZW quaternion, shape (4,) | None |
| dt | float | Sampling period (seconds) | None |
| safe_set | dict[str, Box] | Description of the safe set of the environment. Defaults to ROBOT_SAFE_SET (the precomputed safe-set decomposition for the ISS, accounting for the robot's collision radius) |
None |
| graph | Optional[dict[str, list[str]]] | The graph defining the connections between the safe boxes in the environment. Defaults to GRAPH (the precomputed safe-set graph for the ISS). Note: if set to None, this graph will be recomputed from the ROBOT_SAFE_SET parameter |
None |
Returns:
| Type | Description |
|---|---|
| Trajectory | The optimal global trajectory plan |
View Source
def global_planner(
p0: npt.ArrayLike,
q0: npt.ArrayLike,
pf: npt.ArrayLike,
qf: npt.ArrayLike,
dt: float,
safe_set: dict[str, Box] = ROBOT_SAFE_SET,
graph: Optional[dict[str, list[str]]] = GRAPH,
) -> Trajectory:
"""Generate an optimal spline-based position trajectory with a polynomial orientation component
This will enforce all of the Astrobee's constraints (such as maximum velocity/acceleration and collision avoidance).
It will also be time-optimal in the sense that 1) The total duration of the curve is minimized, and 2) each Bezier
curve in the spline will have its relative duration (w.r.t. the total duration) optimized to minimize jerk
Note that this can take on the order of 10 seconds to compute, so it is not something that should be called online
Args:
p0 (npt.ArrayLike): Initial position, shape (3,)
q0 (npt.ArrayLike): Initial XYZW quaternion, shape (4,)
pf (npt.ArrayLike): Final position, shape (3,)
qf (npt.ArrayLike): Final XYZW quaternion, shape (4,)
dt (float): Sampling period (seconds)
safe_set (dict[str, Box], optional): Description of the safe set of the environment. Defaults to ROBOT_SAFE_SET
(the precomputed safe-set decomposition for the ISS, accounting for the robot's collision radius)
graph (Optional[dict[str, list[str]]], optional): The graph defining the connections between the safe boxes in
the environment. Defaults to GRAPH (the precomputed safe-set graph for the ISS). Note: if set to None, this
graph will be recomputed from the ROBOT_SAFE_SET parameter
Returns:
Trajectory: The optimal global trajectory plan
"""
# Dynamics parameters: Assume start and end from rest, satisfy operating limits
t0 = 0
v0 = np.zeros(3)
vf = np.zeros(3)
a0 = np.zeros(3)
af = np.zeros(3)
w0 = np.zeros(3)
wf = np.zeros(3)
dw0 = np.zeros(3)
dwf = np.zeros(3)
v_max = LINEAR_SPEED_LIMIT
a_max = LINEAR_ACCEL_LIMIT
# Parameters associated with the retiming optimization or the free-final-time optimization
# will be left at the default values for now
# Determine the path through the safe-space graph
start = find_containing_box_name(p0, safe_set)
end = find_containing_box_name(pf, safe_set)
if graph is None:
graph = compute_graph(safe_set)
path = dfs(graph, start, end)
box_path = [safe_set[p] for p in path]
init_angular_duration = rotation_duration_heuristic(q0, qf)
init_pos_duration, init_timing_fractions = spline_duration_heuristic(
p0, pf, box_path
)
# TODO enforce a minimum final time in the free-final-time optimization
# to make sure that the rotation plan still has enough time to execute
duration = max(init_pos_duration, init_angular_duration)
init_curve_durations = duration * init_timing_fractions
if len(box_path) == 1:
# Crank up the control points for a single Bezier curve so that we can be nice and tight
# against the velocity/accel constraints
n_control_pts = 20
# If our start and end positions are contained within the same free box,
# we can construct the trajectory from a single Bezier curve
curve = free_final_time_bezier(
p0,
pf,
t0,
duration,
n_control_pts,
v0,
vf,
a0,
af,
box_path[0],
v_max,
a_max,
)
else:
# Our start/end positions are not in the same box, so use a spline between boxes
# Use less control points for the spline for optimization stability
n_control_pts = 10
curve = free_final_time_spline(
p0,
pf,
t0,
duration,
n_control_pts,
box_path,
init_curve_durations,
v0,
vf,
a0,
af,
v_max,
a_max,
)
pos_traj = traj_from_curve(curve, dt)
n_timesteps = pos_traj.num_timesteps
quats = quaternion_interpolation_with_bcs(
q0, qf, w0, wf, dw0, dwf, duration, n_timesteps
)
omega = quats_to_angular_velocities(quats, dt)
alpha = np.gradient(omega, dt, axis=0)
return Trajectory(
pos_traj.positions,
quats,
pos_traj.linear_velocities,
omega,
pos_traj.linear_accels,
alpha,
pos_traj.times,
)
local_planner
def local_planner(
p0: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
q0: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
v0: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]], NoneType],
w0: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
a0: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]], NoneType],
dw0: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
pf: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
qf: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
vf: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]], NoneType],
wf: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
af: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]], NoneType],
dwf: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
duration: float,
dt: float
) -> pyastrobee.trajectories.trajectory.Trajectory
Generate an optimal Bezier-curve-based position trajectory with a polynomial orientation component
Note that this planner prioritizes solve time over constraint enforcement. This will plan with boundary conditions in mind, but other than that, it does not enforce things like speed/acceleration limits or collision avoidance. This allows us to use this in very fast online applications.
In general, this should be paired with the global planner, so that we can precompute a reference trajectory which does enforce all constraints (and takes a bit longer to solve), and use this local planner to evaluate different rollouts between states along this reference trajectory
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
| p0 | npt.ArrayLike | Initial position, shape (3,) | None |
| q0 | npt.ArrayLike | Initial XYZW quaternion, shape (4,) | None |
| v0 | npt.ArrayLike | Initial linear velocity, shape (3,) | None |
| w0 | npt.ArrayLike | Initial angular velocity, shape (3,) | None |
| a0 | npt.ArrayLike | Initial linear acceleration, shape (3,) | None |
| dw0 | npt.ArrayLike | Initial angular acceleration, shape (3,) | None |
| pf | npt.ArrayLike | Final position, shape (3,) | None |
| qf | npt.ArrayLike | Final XYZW quaternion, shape (4,) | None |
| vf | npt.ArrayLike | Final linear velocity, shape (3,) | None |
| wf | npt.ArrayLike | Final angular velocity, shape (3,) | None |
| af | npt.ArrayLike | Final linear acceleration, shape (3,) | None |
| dwf | npt.ArrayLike | Final angular acceleration, shape (3,) | None |
| duration | float | Trajectory duration (seconds) | None |
| dt | float | Sampling period (seconds) | None |
Returns:
| Type | Description |
|---|---|
| Trajectory | The optimal local trajectory plan |
View Source
def local_planner(
p0: npt.ArrayLike,
q0: npt.ArrayLike,
v0: Optional[npt.ArrayLike],
w0: npt.ArrayLike,
a0: Optional[npt.ArrayLike],
dw0: npt.ArrayLike,
pf: npt.ArrayLike,
qf: npt.ArrayLike,
vf: Optional[npt.ArrayLike],
wf: npt.ArrayLike,
af: Optional[npt.ArrayLike],
dwf: npt.ArrayLike,
duration: float,
dt: float,
) -> Trajectory:
"""Generate an optimal Bezier-curve-based position trajectory with a polynomial orientation component
Note that this planner prioritizes solve time over constraint enforcement. This will plan with boundary conditions
in mind, but other than that, it does not enforce things like speed/acceleration limits or collision avoidance. This
allows us to use this in very fast online applications.
In general, this should be paired with the global planner, so that we can precompute a reference trajectory which
does enforce all constraints (and takes a bit longer to solve), and use this local planner to evaluate different
rollouts between states along this reference trajectory
Args:
p0 (npt.ArrayLike): Initial position, shape (3,)
q0 (npt.ArrayLike): Initial XYZW quaternion, shape (4,)
v0 (npt.ArrayLike): Initial linear velocity, shape (3,)
w0 (npt.ArrayLike): Initial angular velocity, shape (3,)
a0 (npt.ArrayLike): Initial linear acceleration, shape (3,)
dw0 (npt.ArrayLike): Initial angular acceleration, shape (3,)
pf (npt.ArrayLike): Final position, shape (3,)
qf (npt.ArrayLike): Final XYZW quaternion, shape (4,)
vf (npt.ArrayLike): Final linear velocity, shape (3,)
wf (npt.ArrayLike): Final angular velocity, shape (3,)
af (npt.ArrayLike): Final linear acceleration, shape (3,)
dwf (npt.ArrayLike): Final angular acceleration, shape (3,)
duration (float): Trajectory duration (seconds)
dt (float): Sampling period (seconds)
Returns:
Trajectory: The optimal local trajectory plan
"""
# Min-jerk position traj
# Don't need a ton of control points because we're not enforcing constraints
n_control_pts = 8
curve, _ = bezier_trajectory(
p0, pf, 0, duration, n_control_pts, v0, vf, a0, af, time_weight=0
)
pos_traj = traj_from_curve(curve, dt)
n_timesteps = len(pos_traj.times)
quats = quaternion_interpolation_with_bcs(
q0, qf, w0, wf, dw0, dwf, duration, n_timesteps
)
omega = quats_to_angular_velocities(quats, dt)
alpha = np.gradient(omega, dt, axis=0)
return Trajectory(
positions=pos_traj.positions,
quats=quats,
lin_vels=pos_traj.linear_velocities,
ang_vels=omega,
lin_accels=pos_traj.linear_accels,
ang_accels=alpha,
times=pos_traj.times,
)