lqrrt - Path Planning

Constraints

Methods

• defset_buffers
• defset_feasibility_function

class lqrrt.Constraints(nstates: int, ncontrols: int, goal_buffer: ndarray, is_feasible: Callable)

The constraints of the lqRRT system.

Parameters

• nstates – int - The dimensionality of the state space.

• ncontrols – int - The dimensionality of the effort space.

• goal_buffer – np.ndarray - Half-edge lengths of box defining goal region.

• is_feasible – np.ndarray - Function that takes a state and effort and returns a bool.

set_buffers(goal_buffer: Optional[ndarray] = None) → None

Sets the goal buffer constraints on the lqRRT system.

Parameters

goal_buffer – np.ndarray - Half-edge lengths of a box defining the goal region.

Raises

ValueError - The goal buffer does not have the same dimensionality – as the state, or its length is not equal to the number of states.

set_feasibility_function(is_feasible: Callable) → None

Sets the feasibility function for the lqRRT system.

Parameters

is_feasible – Callable - The method that determines the feasibility of movement.

Raises

ValueError - is_feasible is not a method. –

Planner

Methods

• defkill_update
• defset_goal
• defset_resolution
• defset_runtime
• defset_system
• defunkill
• defupdate_plan
• defvisualize

class lqrrt.Planner(dynamics: ~typing.Callable, lqr: ~typing.Callable, constraints: ~lqrrt.constraints.Constraints, horizon: ~typing.Union[float, ~typing.Tuple], dt: float = 0.05, FPR: float = 0, error_tol: float = 0.05, erf: ~typing.Callable = <ufunc 'subtract'>, min_time: float = 0.5, max_time: int = 1, max_nodes: int = 100000, goal0: ~typing.Optional[~typing.List] = None, sys_time: ~typing.Callable = <built-in function time>, printing: bool = True)

Initializes an lqRRT planner class.

Parameters

• dynamics – Callable - Function that returns the next state given the current state x and the current effort u, and timestep dt. That is, xnext = dynamics(x, u, dt).

• lqr – Callable - Function that returns the local LQR cost-to-go matrix S and policy matrix K as a tuple of arrays (S, K) where S solves the local Riccati equation and K is the associated feedback gain matrix. That is, (S, K) = lqr(x, u).

• constraints – Constraints - Instance of the Constraints class that defines feasibility of states & efforts, goal region, etc…

• horizon – Union[float, Tuple] - The simulation duration in seconds used to extend the tree. If you give a tuple (min, max), an adaptive horizon heuristic will be used.

• dt – float - The simulation timestep in seconds used to extend the tree.

• FPR – float - Failed Path Retention factor. When a path is grown and found to be infeasible, this is the fraction of the path that is retained up to that infeasible point.

• error_tol – float - The state error array or scalar defining controller convergence.

• erf – Callable - Function that takes two states xgoal and x and returns the state error between them. Defaults to simple subtraction xgoal - x. This is useful if your state includes a quaternion or heading.

• min_time – float - The least number of seconds that the tree will grow for. That is, even if a feasible plan is found before min_time, it will keep growing until min_time is reached and then give the best of the plans.

• max_time – int - The max number of seconds that the tree will grow for. That is, if there are still no feasible plans after this amount of time, the plan_reached_goal flag will remain False and the plan that gets closest to the goal is given.

• max_nodes – int - If the tree reaches this number of nodes but no path is found, the plan_reached_goal flag will remain False and the plan that gets closest to the goal is given.

• goal0 – Optional[List] - The initial goal state. If left as None, update_plan cannot be run. Use set_goal to set the goal at any time. Be sure to update the plan after setting a new goal.

• sys_time – Callable - Function that returns the real-world system time. Defaults to the Python time library’s time().

• printing – bool - Bool that specifies if internal stuff should be printed.

kill_update() → None

Raises a flag that will cause an abrupt termination of the update_plan routine.

set_goal(goal: Optional[List]) → None

Modifies the goal state and region. Be sure to update the plan after modifying the goal.

Parameters

goal – Optional[List]] - The list to update the goal with, if any. If None, then updates the goal to None.

Raises

ValueError - Goal state does not have the same dimensionality a – state space.

set_resolution(horizon: Optional[Union[float, Tuple]] = None, dt: Optional[float] = None, FPR: Optional[float] = None, error_tol: Optional[Union[float, List]] = None) → None

Sets resolution settings of the lqRRT system.

Parameters

• horizon – Optional[Union[float, Tuple]] - The simulation duration in seconds used to extend the tree. If you give a tuple (min, max), an adaptive horizon heuristic will be used.

• dt – Optional[float] - The simulation timestep in seconds used to extend the tree.

• FPR – Optional[float] - Failed Path Retention factor. When a path is grown and found to be infeasible, this is the fraction of the path that is retained up to that infeasible point.

• error_tol – Optional[Union[float, List]] - The state error array or scalar defining controller convergence.

Raises

ValueError - Could be due to several conditions – # error_tol is not a scalar or is a list with a shape that does not match the number of states. # horizon is not either a scalar or a tuple of (min, max) # horizon (or the min of the tuple) is less than dt # in the horizon tuple, min > max

set_runtime(min_time: Optional[float] = None, max_time: Optional[float] = None, max_nodes: Optional[int] = None, sys_time: Optional[float] = None) → None

Updates the runtime settings of the lqRRT system.

Parameters

• min_time – Optional[float] - The least number of seconds that the tree will grow for. That is, even if a feasible plan is found before min_time, it will keep growing until min_time is reached and then give the best of the plans.

• max_time – Optional[float] - The max number of seconds that the tree will grow for. That is, if there are still no feasible plans after this amount of time, the plan_reached_goal flag will remain False and the plan that gets closest to the goal is given.

• max_nodes – Optional[int] - If the tree reaches this number of nodes but no path is found, the plan_reached_goal flag will remain False and the plan that gets closest to the goal is given.

• sys_time – Callable - Function that returns the real-world system time. Defaults to the Python time library’s time().

Raises

ValueError - The maximum amount of time is less than the minimum – amount of time allotted to the tree.

set_system(dynamics: Optional[Callable] = None, lqr: Optional[Callable] = None, constraints: Optional[Constraints] = None, erf: Optional[Callable] = None) → None

If dynamics gets modified, so must lqr (and vis versa).

Parameters

• dynamics – Callable - Function that returns the next state given the current state x and the current effort u, and timestep dt. That is, xnext = dynamics(x, u, dt).

• lqr – Callable - Function that returns the local LQR cost-to-go matrix S and policy matrix K as a tuple of arrays (S, K) where S solves the local Riccati equation and K is the associated feedback gain matrix. That is, (S, K) = lqr(x, u).

• constraints – Constraints - Instance of the Constraints class that defines feasibility of states & efforts, goal region, etc…

• erf – Callable - Function that takes two states xgoal and x and returns the state error between them. Defaults to simple subtraction xgoal - x. This is useful if your state includes a quaternion or heading.

Raises

ValueError - One of the parameters provided is not of the required – type.

unkill() → None

Lowers the kill_update flag. Do this if you made a mistake.

update_plan(x0: List, sample_space: List[Tuple[int, int]], goal_bias: float = 0, guide: Optional[List] = None, xrand_gen: Optional[Union[Callable, int]] = None, pruning: bool = True, finish_on_goal: bool = False, specific_time: Optional[float] = None) → bool

A new tree is grown from the seed x0 in an attempt to plan a path to the goal. The returned path can be accessed with the interpolator functions get_state(t) and get_effort(t).

The tree is motivated by uniform random samples in the over the given sample_space. The sample_space is a list of n tuples where n is the number of states; [(min1, max1), (min2, max2)…].

The goal_bias is the fraction of the time the goal is sampled. It can be a scalar from 0 (none of the time) to 1 (all the time) or a list of scalars corresponding to each state dimension.

Alternatively, you can give a function xrand_gen which takes the current planner instance (self) and outputs the random sample state. Doing this will override both sample_space and goal_bias, which you can set to arbitrary values only if you provide an xrand_gen function.

Or, instead of a function, you can set xrand_gen to a single integer 1 or greater, which will act as the number of tries allowed for finding a feasible random sample in the default random sampling routine. (Leaving the default None will set the limit to 10 tries).

After min_time seconds, the fastest available path from x0 to the current goal is returned and the functions get_state(t) and get_effort(t) are modified to interpolate this new path.

If no path was found yet, the search continues until max_time or until the node limit is breached. After the limit, a warning is printed and the path that gets nearest to the guide is used instead. If guide is left None, it defaults to goal.

If pruning is True, then nodes can be marked as “ignore” during growth. Right now, only nodes on a completed path are ignored.

If finish_on_goal is set to True, once the plan makes it to the goal region (goal plus buffer), it will attempt to steer one more path directly into the exact goal. Can fail for nonholonomic systems.

If you want this update_plan to plan for some specific amount of time (instead of using the global min_time and max_time), pass it in as specific_time in seconds.

This function returns True if it finished fully, or False if it was haulted. It can hault if it is killed or if the tree exceeds max_nodes, or if no goal has been set yet.

visualize(dx: int, dy: int) → None

Plots the (dx,dy)-cross-section of the current tree, and highlights the current plan’s trajectory. For example, dx=0, dy=1 plots the states #0 and #1.

Parameters

• dx – int - The index of the first state to plot.

• dy – int - The index of the second state to plot.

Tree

Methods

• defadd_node
• defclimb
• deftrajectory
• defvisualize

class lqrrt.Tree(seed_state, seed_lqr)

To initialize, provide…

seed_state: An array of the state of the seed node.

seed_lqr: The local LQR-optimal cost-to-go array S

and policy array K as a tuple (S, K). That is, S solves the local Riccati equation and K = (R^-1)*(B^T)*(S) for effort jacobian B.

add_node(pID, state, lqr, x_seq, u_seq)

Adds a node to the tree with the given features.

climb(ID)

Returns a list of node IDs that connect the seed to the node with the given ID. The first element in the list is always 0 (seed) and the last is always ID (the given climb destination).

trajectory(IDs)

Given a list of node IDs, the full sequence of states and efforts to go from IDs[0] to IDs[-1] are returned as a tuple (x_seq_full, u_seq_full).

visualize(dx, dy, node_seq=None)

Plots the (dx,dy)-cross-section of the current tree, and highlights the path given by the list, node_seq. For example, dx=0, dy=1 plots the states #0 and #1.

Node

Attributes

• done
• move_type
• path_pub
• ref_pub
• revisit_period
• tree_pub
• unreachable

Methods

• defaction_check
• defangle_diff
• defboundary_analysis
• deferf
• defis_feasible
• defmove_cb
• defodom_cb
• defogrid_cb
• defpack_odom
• defpack_pointstamped
• defpack_pose
• defpack_posestamped
• defpack_wrenchstamped
• defpublish_expl
• defpublish_path
• defpublish_ref
• defpublish_tree
• defreevaluate_plan
• defreset
• defrotation_move
• defselect_behavior
• defselect_exploration
• defset_goal
• defspiral_move
• deftree_chain
• defunpack_odom
• defunpack_pose

class navigator_path_planner.nodes.path_planner.LQRRT_Node(odom_topic: str, ref_topic: str, move_topic: str, path_topic: str, tree_topic: str, goal_topic: str, focus_topic: str, effort_topic: str, ogrid_topic: str, ogrid_threshold: str)

Example of a ROS node that uses lqRRT for a big boat.

This node subscribes to the current boat state (Odometry message), and the world-frame ogrid (OccupancyGrid message). It publishes the REFerence trajectory that moves to the goal, as an Odometry message. It provides an action for moving said reference to that goal (Move.action).

revisit_period

When to refresh the core of the planner.

Type

float

ref_pub

The publisher serving the lqRRT reference topic name.

Type

rospy.Publisher

path_pub

Publisher responsible for publishing a PoseArray to the provided path topic.

Type

rospy.Publisher

tree_pub

Publisher responsible for publishing a PoseArray to the provided tree topic.

Type

rospy.Publisher

unreachable

Whether the planner sees a goal as being unreachable. Defaults to false.

Type

bool

done

Whether the planner has finished its movement. Defaults to false.

Type

bool

move_type

How the boat is planning to move towards its goal.

Type

MoveAction

Initialize with topic names and ogrid threshold as applicable. Defaults are generated at the ROS params level.

Parameters

• odom_topic (str) – The name of the topic supplying Odometry messages.

• ref_topic (str) – The lqRRT reference topic name.

• move_topic (str) – The name of the topic hosting the SimpleActionServer and topic of where to move to.

• path_topic (str) – The name of the lqRRT path topic name.

• tree_topic (str) – The name of the topic holding the lqRRT tree.

• goal_topic (str) – The name of the topic holding the lqRRT goal.

• focus_topic (str) – The name of the topic holding the lqRRT focus.

• effort_topic (str) – The name of the topic hosting the lqRRT effort wrenches.

• ogrid_topic (str) – The name of the topic holding the occupancy grid.

• ogrid_threshold (str) – An occupancy grid cell which exceeds this value will be marked as filled. The value will be converted to a float in the class.

action_check(_: TimerEvent) → None

Manages action preempting. Serves as the callback to a Timer operating

operating every self.revisit_period seconds.

angle_diff(angle_goal: float, angle: float) → float

Takes an angle difference properly on SO2.

Parameters

• angle_goal (float) – The goal angle.

• angle (float) – The angle to grab the difference from.

Returns

The difference between the desired and goal angle.

Return type

float

boundary_analysis(img, seed, goal)

Returns a list of the two boundary points of the contour dividing seed from goal in the occupancy image img. If the seed and goal are connected, returns ‘connected’ or if they are terminally isolated, returns ‘isolated’. Make sure seed and goal are intups and in the same pixel coordinates as img, and that occupied pixels have value 255 in img.

erf(goal_state: List[float], state: List[float]) → ndarray

Returns error given two states goal_state and state. Angle differences are taken properly on SO2.

Parameters

• goal_state (List[float]) – The goal state

• state (List[float]) – The state to derive the error from

Returns

The error between the goal and passed state.

Return type

np.ndarray

is_feasible(x: ndarray, u: ndarray) → bool

Given a state x and effort u, returns whether (x, u) is feasible.

Parameters

• x (np.ndarray) – The state to verify the feasibility of

• u (np.ndarray) – The corresponding effort to verify the feasibility of

Returns

Whether the state is feasible.

Return type

bool

move_cb(msg: MoveAction) → Optional[bool]

Callback for the move action.

Parameters

msg (MoveAction) – The action passed by the action client.

Returns

???

Return type

Optional[bool]

odom_cb(msg: Odometry) → None

Callback to the odometry topic subscriber. Stores the current state of the tracking vehicle and its reference frame. Sets the class tracking variable to True or False based on if the vehicle is successfully tracking.

Parameters

msg (Odometry) – The Odometry message received by the callback.

ogrid_cb(msg: OccupancyGrid) → None

Acts as the callback function to a subscriber to the odom topic. Expects an OccupancyGrid message. Stores the ogrid array and origin vector, and then reevaluates the path plan.

Parameters

msg (OccupancyGrid) – The message passed by the callback.

pack_odom(state: List[float], stamp: Time) → Odometry

Converts a state vector into an Odometry message with given header timestamp.

Parameters

• state (List[float]) – A list containing the following values: state[0] - The x-position of the bot state[1] - The y-position of the bot state[2] - The orientation (yaw) of the bot state[3] - The linear speed in the x-direction state[4] - The linear speed in the y-direction state[5] - The angular speed in the z-direction

• stamp (rospy.Time) – The timestamp of the message.

Returns

The packed Odometry message.

Return type

Odometry

pack_pointstamped(point: List[float], stamp: Time) → PointStamped

Converts a point vector into a PointStamped message with a given header timestamp.

Parameters

• point (List[float]) – The point vector. point[0] represents the x-component of the point and point[1] represents the y-component of the point.

• stamp (rospy.Time) – The timestamp to attach to the message.

Returns

The message to pack the point as.

Return type

PointStamped

pack_pose(state: List[float]) → Pose

Converts the positional part of a state vector into a Pose message.

For a timestamped message, use pack_posestamped.

Parameters

state (List[float]) – The current state of the robot. state[0] and state[1] represent x + y orientations. state[2] represents the orientation of the bot.

Returns

The Pose message.

Return type

Pose

pack_posestamped(state: List[float], stamp: Time) → PoseStamped

Converts the positional part of a state vector into a PoseStamped message with a given header timestamp. For a non-stamped message, use pack_pose.

Parameters

• state (List[float]) – The current state of the robot. state[0] and state[1] represent x + y orientations. state[2] represents the orientation of the bot.

• stamp (rospy.Time) – The time that the bot’s state existed as so.

Returns

The PoseStamped message.

Return type

PoseStamped

pack_wrenchstamped(effort: List[float], stamp: Time) → WrenchStamped

Converts an effort vector into a WrenchStamped message with a given header timestamp.

Parameters

• effort (List[float]) – The effort vector. effort[0] represents the x-component of force, effort[1] represents the y-component of force, and effort[2] represents the z-component of torque.

• stamp (rospy.Time) – The timestamp to attach to the message.

Returns

The message to pack the effort as.

Return type

WrenchStamped

publish_expl() → None

Publishes sample space as a PolygonStamped to the sample space publisher and publishes the guide point as a PointStamped to the guide publisher.

publish_path() → None

Publishes all tree-node poses along the current path as a PoseArray. Publishes to the class’ path publisher.

publish_ref(*args) → None

Publishes the reference trajectory as an Odometry message to the ref publisher. Also publishes the reference effort as a WrenchStamped message to the effort publisher.

publish_tree() → None

Publishes all tree-node poses as a PoseArray. Publishes to the class’ tree publisher.

reevaluate_plan() → None

Iterates through the current plan re-checking for feasibility using

the newest ogrid data, looking for a clear path if we are escaping, and checking that the goal is still feasible.

reset() → None

Resets variables that should definitely be cleared before a new action begins.

rotation_move(x: List[float], h: float, tol: float)

Returns a state sequence, total time, success bool and effort sequence for a simple rotate in place move. Success is False if the move becomes infeasible before the state heading x[2] is within the goal heading h+-tol. Give x (start state), and h (goal heading).

Parameters

• x (List[float]) – The current state

• h (float) – The desired heading (similar to x[2])

• tol (float) – The tolerance of the heading

Returns

A tuple containing

the new state array, the amount of time the movement will take, whether the operation is feasible, and the new effort array.

Return type

Tuple[np.ndarray, float, bool, np.ndarray]

select_behavior() → module

Chooses the behavior for the current move, and returns the associated module containing the implementation of that behavior.

Returns

A module with the behavior implementation

Return type

ModuleType

Raises

ValueError – No relevant implementation could be found

select_exploration() → List

Chooses the goal bias, sample space, guide point, and pruning choice for the current move and behavior.

set_goal(x: ndarray) → None

Publishes the goal state to the goal publisher.

spiral_move(x: List[float], c: List[float], N: int, direc: int, mpr: float)

Generates a movement pattern for a spiral movement.

Parameters

• x (List[float]) – Initial state vector.

• c (List[float]) – Coordinates of initial point.

• N (int) – The number of revolutions to make.

• direc (int) – Whether to move + or - in regards to the z-axis

• mpr (float) – The meters expansion of the spiral per radian

Returns

The state sequence of the spiral

movement, the amount of time (in seconds) required to make the movement, and whether the movement is feasible.

Return type

Tuple[np.ndarray, float, bool]

tree_chain()

Plans an lqRRT and sets things up to chain along another lqRRT when called again.

unpack_odom(msg: Odometry) → ndarray

Converts an Odometry message into a state vector.

Parameters

msg (Odometry) – The Odometry message to unpack.

Returns

The new state vector, containing 6

float values.

Return type

np.ndarray

unpack_pose(msg: Pose) → ndarray

Converts a Pose message into a state vector with zero velocity.

Parameters

msg (Pose) – The message to unpack.

Returns

A state array containing the x-position, y-position,

and yaw. The velocities are all zero.

Return type

np.ndarray