catl_planning package

Submodules

catl_planning.history_builder module

main()

main_2()

padHistory(hist)

regCapHist(hist, regions, caps, agents)

catl_planning.kml_gen module

gen_kml_file(m, plan_to_draw)

point_transform(point, origin, scale_to_m_x, scale_to_m_y, aprox_m_per_deg=111134.75)

txt_output(m, the_plan)

catl_planning.pure_pursuit module

Path tracking simulation with pure pursuit steering control and PID speed control.

author: Atsushi Sakai (@Atsushi_twi) Modified: Zachary Serlin

PIDControl(target, current)

class State(x=0.0, y=0.0, yaw=0.0, v=0.0)

Bases: object

calc_distance(state, point_x, point_y)

calc_target_index_time(state, cx, cy)

plot_arrow(x, y, yaw, length=1.0, width=0.5, fc='r', ec='k')

Plot arrow

pure_pursuit_control(state, cx, cy, pind)

sim_dynamics(m, the_plan)

update(state, a, delta)

catl_planning.route_planning module

add_proposition_constraints_pulp(mpulp, stl_milp, ts, ast, capabilities, agent_classes, bound, vtype='Integer', num_agents=1000)

Adds the proposition constraints. First, the proposition-state variables are defined such that capabilities are not double booked. Second, contraints are added such that proposition are satisfied as best as possible. The variables in the MILP encoding of the STL formula are used for the encoding as the minimizers of over proposition-state variables.

This function uses the PuLP modeling language rather than Gurobi directly.

• The PuLP model variable.

• The MILP encoding of the STL formula obtained from the CaTL specification.

• The transition system specifying the environment.

• The AST of the CaTL specification formula.

• Dictionary of capability encoding that maps capabilities to binary words

represented as integers. - The agent classes given as a dictionary from frozen sets of capabilities to bitmaps (integers). - Time bound. - Variable type (default: integer).’F[0, 10] T(2, orange, {(UV, 1), (Mo, 1)})’

add_system_constraints_pulp(mpulp, ts, agent_classes, capability_distribution, bound)

Computes the constraints that capture the system dynamics.

• The PuLP model object.

• The transition system specifying the environment.

• The agent classes given as a dictionary from frozen sets of capabilities

to bitmaps (integers). - The initial distribution of capabilities at each state. - Time bound.

Note

The initial time constraints

z_{state}_g_0 = eta_{state}_g

is equivalent to

sum_{e=(u, v) in T} z_e_g_W(e) = eta_{state}_g

because of the definition of the team state at TS states, where eta_{state}_g is the number of agents of class g at state {state} at time 0.

computeRobustnessUpperBound(ts, agents, formula)

Computes a loose upper bound on the robustness value.

The robustness value measures whether there are enough agents to satisfy the temporal logic specification in the given transition system. A positive robustness value indicates there are more than enough required agents to satisfy the formula. A negative robustness value indicates that there are not enough required agents to satisfy the formula.

This function computes an upper bound on the robustness value for a given transition system (TS), set of agents, and CaTL formula. This upper bound can be used as a “sanity check” to test if the formula is infeasible for the given TS and set of agents. If the upper bound is negative, then the problem is infeasible. If the upper bound is positive, then it is possible that the problem is feasible.

If the upper bound is negative, there is no point in solving the associated MILP. The problem is infeasible.

Note: This function does not take into account the travel time between regions in the TS.

Parameters

• ts – The transition system

• agents (list) – List of agents in the system. Each agent has the form (initial_state, {capability1, capability2,...}).

• formula – Either a string or a catl.CATLFormula object containing the formula. If a string, it is converted into a catl.CATLFormula object by calling formula = CATLFormula.from_formula(formula).

Returns

Upper bound on the robustness value.

Return type

(float)

compute_agent_classes(agents, capabilities)

Computes the set of agent types w.r.t. capabilities.

Parameters

• agents (list) – List of agents, where agents are tuples (q, cap), q is the initial state of the agent, and cap is the set of capabilities. Agents’ identifiers are their indices in the list.

• capabilities (dict) – Dictionary of capability class encoding that maps capability classes to binary words represented as integers.

Returns

Dictionary of agent capability classes that maps frozen (immutable) sets of capabilities to the binary words encoding the corresponding capability classes.

Return type

(dict)

compute_capability_bitmap(agents)

Computes a bitmap encoding of agents’ capabilities. Each capability is associated with a bit in a binary word of length equal to the number of capabilities.

Parameters

agents (list) –

List containing agent information. The iith entry is a tuple (q, cap) corresponding to the iith agent, where q is the initial state of the agent and cap is the set of all capabilities that agent has.

An example of a valid agents list:

agents = [('q1', {'VIS'}), ('q3', {'LID'}), ('q7', {'LID','IR'})]

Returns

Dictionary mapping capabilities to integers representing the binary words for the capabilities.

Return type

(dict)

compute_initial_capability_distribution(ts, agents, capabilities)

Computes the initial number of agents of each class at each state. Input —– - The transition system specifying the environment. - List of agents, where agents are tuples (q, cap), q is the initial state of the agent, and cap is the set of capabilities. Agents’ identifiers are their indices in the list. - Dictionary of capability encoding that maps capabilities to binary words represented as integers. Output —— Dictionary from states to distribution of agents from each class. The distribution is a list of length equal to the number of capabilities, and each element is the number of agents of having those capabilities (a class).

constrain_grave_state_pulp(mpulp, ast, agent_classes, grave_region)

create_system_variables_pulp(mpulp, ts, agent_classes, bound, vtype='Integer', num_agents=1000, regularize=False, alpha=0.5)

Creates the state and transition variables associated with the given transition system using the PuLP framework.

The state variables are z_{state}_{cap}_k, where {state} is a node in the TS graph, {cap} is a capability class encoded as an integer, and k is the time step.

The transition variables are z_{state1}_{state2}_{cap}_k, where {state1} and {state2} define the transition, {cap} is a capability class encoded as an integer, and k is the time step.

• The PuLP model object.

• The transition system specifying the environment.

• The agent classes given as a dictionary from frozen sets of capabilities

to bitmaps (integers). - Time bound. - Variable type (default: integer).

Note

Data structure holding the variables is a list of list of variables, e.g.,

d[‘vars’][k][g] is the z_{q/e}_bitmap(g)_k

where d is the dictionary of attributes for a node q or an edge e in the TS, g is an agent class (frozen set of capabilities), bitmap(g) is the binary encoding of g as an integer, and k is the time step. Also, d[‘vars’] is a list of length `bound+1’, d[‘vars’][k] is a dictionary from frozen sets to gurobi variables.

extract_propositions(ts, ast)

Returns the set of propositions in the formula, and checks that it is included in the transitions system.

• The transition system specifying the environment.

• The AST of the CaTL specification formula.

Set of propositions in the specification formula.

generate_MILP_problems(ts, agents, formula, bound=None, file_name=None, robust=False, regularize=False, alpha=0.5, upperBound=False, replan_grave=None, verbose=True, compress_files=True)

Similar to route_planning(), but simply generates the MILP .lp / .mps problem files without actually solving them.

Parameters

• inputs –

• ts –

• agents –

• formula (str) –

• bound –

• file_name (str) –

• replan_req (bool) –

• robust (bool) –

• regularize (bool) –

• alpha (float) –

• upperBound (bool) –

• load_previous –

• solver_name (str) –

• compute_IIS (bool) –

• verbose –

• compress_files (bool) –

• solver_time_limit (float) – Time limit on the MILP solution process. The MILP will stop after this amount of time. If no time limit is specified, the MILP will run to completion.

Returns

Tuple containing the following elements:

• mpulp: PuLP LpProblem object containing the (solved) MILP corresponding to the planning problem

• replan_data: Object containing the following fields:

  • TODO: [INSERT HERE]

Return type

(tuple)

• The transition system specifying the environment.

• List of agents, where agents are tuples (q, cap), q is the initial state

of the agent, and cap is the set of capabilities. Agents’ identifiers are their indices in the list. - The CaTL specification formula. - The time bound used in the encoding (default: computed from CaTL formula).

TODO: TBD

get_ub(formula, ts, capDict)

Uses recursive relationships to compute the excess capacity of the formula with respect to the team

route_planning(inputs, ts, agents, formula, bound=None, file_name=None, replan_req=False, robust=False, regularize=False, alpha=0.5, upperBound=False, load_previous=False, solver=None, compute_IIS=True, verbose=True, compress_files=True, solver_time_limit=None, solver_threads=None)

Performs route planning for agents agents moving in a transition system ts such that the CaTL specification formula is satisfied.

Parameters

• inputs –

• ts –

• agents –

• formula (str) –

• bound –

• file_name (str) –

• replan_req (bool) –

• robust (bool) –

• regularize (bool) –

• alpha (float) –

• upperBound (bool) –

• load_previous –

• solver_name (str) –

• compute_IIS (bool) –

• verbose –

• compress_files (bool) –

• solver_time_limit (float) – Time limit on the MILP solution process. The MILP will stop after this amount of time. If no time limit is specified, the MILP will run to completion.

Returns

Tuple containing the following elements:

• mpulp: PuLP LpProblem object containing the (solved) MILP corresponding to the planning problem

• replan_data: Object containing the following fields:

  • TODO: [INSERT HERE]

Return type

(tuple)

• The transition system specifying the environment.

• List of agents, where agents are tuples (q, cap), q is the initial state

of the agent, and cap is the set of capabilities. Agents’ identifiers are their indices in the list. - The CaTL specification formula. - The time bound used in the encoding (default: computed from CaTL formula).

TODO: TBD

suppress_output(func)

Prevents a function from printing output to terminal.

Redirects the output to /dev/null. Used to suppress the output of the optimization solvers.

catl_planning.rrt_planning module

Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT)

Author: Zachary Serlin (zserlin@bu.edu)

DrawGraph(bounds, past_paths, num_agents, start, goal, agent_radius, obstacleList, t)

GetPathLength(path)

GetTargetPoint(path, targetL)

LineCollisionCheck(first, second, obstacleList)

class Node(x, y, t=0)

Bases: object

RRT Node

PathSmoothing(path, maxIter, obstacleList)

PlotCircle(x, y, size, bounds)

class RRT(start, goal, obstacleList, randArea, expandDis=1.0, goalSampleRate=5, maxIter=500, num_agents=1, past_paths=None, agentnum=0, escape_time=2, box_bounds_obstacleList=None)

Bases: object

Class for RRT Planning

DrawGraph(bounds, rnd=None)

GetNearestListIndex(nodeList, rnd)

Planning(safe_size, agent_size, animation=True)

Pathplanning animation: flag for animation on or off

PlotCircle(x, y, size)

RRT_Get_Path(regions_to_avoid=None, start=None, goal=None, past_paths=None, start_region=None, goal_region=None, agentnum=None, agent_radius=None, expandDis=1, bounds=[0, 100, 0, 100], max_rrt_time=2, box_bounds_obs_regions=None)

obstacle_collision_check(point, obstacleList)

catl_planning.traj_planner module

agent_positions_to_MOOS_strings(agent_positions, num_agents, agent_index_dict, agent_tasks=None)

Parses agent positions and tasks into string form to send to MOOS.

The final format of the output strings is “state1-state1:start_time-end_time:task1; state1-state2:start_time:end_time:None; …”

The task “None” specifies that the agent does not have a task. Note that all agents have the task “None” when traversing edges between states.

Parameters

• agent_positions (list) – A list of lists containing the positions of each agent at each time step. The first index corresponds to time step, and the second index corresponds to agent number. For example, agent_positions[ii][jj] is the position of the jjth agent at time step ii. If agent jj is in a state at time ii, then agent_positions[ii][jj] is an integer corresponding to the state (e.g. 7 for state Q7). If agent jj is traversing an edge between states at time ii, then agent_positions[ii][jj] is a list with entries [state1, state2, time_duration] meaning it has time_duration steps to transfer from state1 to state2.

• num_agents (int) – Number of agents in the network. TODO: This is redundant; we can compute this from agent_positions itself.

• agent_index_dict (dict) – A dictionary mapping agent number to column index (second dimension index) in agent_positions. The ordering of agent numbers does not correspond to the column order; i.e. the iith agent does not correspond to the iith column in agent_positions. Instead, it corresponds to the agent_index_dict[ii] index in agent_positions.

• agent_tasks (list) – A list of lists similar to agent_positions, but containing the task that each agent is doing at each time step. Work in progress; final format TBA.

Returns

A list of strings with the format specified previously.

Return type

(list)

assign_caps_to_trajs(states, edges, preds, sim_time, replan_agent=None, grave_state=None, start_time=0, replan_agent_idx=None, replanning_time=None)

collision_check(point, current_time, radius, past_paths=None)

create_random_tasks(agent_positions, agent_index_dict)

Creates random tasks for agents. Placeholder function.

Task string options are:

• NULL

• LOITER

• RASTER

• DEFENSE

• ESCORT

• KILLCHAIN

• BLOCKADE

All agents have have the task “NONE” when they are traversing an edge

Parameters

• agent_positions (list) – A list of lists containing the positions of each agent at each time step. The first index corresponds to time step, and the second index corresponds to agent number. For example, agent_positions[ii][jj] is the position of the jjth agent at time step ii. If agent jj is in a state at time ii, then agent_positions[ii][jj] is an integer corresponding to the state (e.g. 7 for state Q7). If agent jj is traversing an edge between states at time ii, then agent_positions[ii][jj] is a list with entries [state1, state2, time_duration] meaning it has time_duration steps to transfer from state1 to state2.

• agent_index_dict (dict) – A dictionary mapping agent number to column index in the agent_positions matrix. The column in agent_positions corresponding to agent ii is given by agent_index_dict[ii].

Returns

List of lists agent_tasks. The first dimension indexes time step, the second dimension indexes agent number. For example, agent_tasks[ii][jj] contains the task string for the jjth agent at time step ii (e.g. “BLOCKADE”).

Return type

(list)

define_region_bounds(ts, state)

expand_agent_positions(ts, agent_positions)

Edits agent_positions in-place to reinsert removed states and edges. This is meant for use with reduce_ts in decomposition_functions.py, which removes unnecessary states and combines the edge weights.

This function takes agent paths described by agent positions and replaces edges with the expanded path by reinserting removed states and edges.

Example

q0 -weight:2-> q1 -weight:4-> q2

q1 was removed by reduce_ts

solution has q0-weight:6->q2

this function edits the solution to go through q1 again

Parameters

• ts – the reduced TS

• agent_positions – a numpy array where each row is a time and each column is an agent elements can be a state or an edge

get_agent_capabilities(states)

Returns a list of agent capabilities.

The order of capabilities in the list corresponds to the columns in agent_positions, but does not correspond to the agent order in the casefile agent list.

get_agent_map(agents, agent_positions, agent_caps, classes)

randint(low, high=None, size=None, dtype=int)

Return random integers from low (inclusive) to high (exclusive).

Return random integers from the “discrete uniform” distribution of the specified dtype in the “half-open” interval [low, high). If high is None (the default), then results are from [0, low).

Note

New code should use the integers method of a default_rng() instance instead; please see the random-quick-start.

Parameters

• low (int or array-like of ints) – Lowest (signed) integers to be drawn from the distribution (unless high=None, in which case this parameter is one above the highest such integer).

• high (int or array-like of ints, optional) – If provided, one above the largest (signed) integer to be drawn from the distribution (see above for behavior if high=None). If array-like, must contain integer values

• size (int or tuple of ints, optional) – Output shape. If the given shape is, e.g., (m, n, k), then m * n * k samples are drawn. Default is None, in which case a single value is returned.

• dtype (dtype, optional) –

  Desired dtype of the result. Byteorder must be native. The default value is int.

  New in version 1.11.0.

Returns

out – size-shaped array of random integers from the appropriate distribution, or a single such random int if size not provided.

Return type

int or ndarray of ints

See also

random_integers

similar to randint, only for the closed interval [low, high], and 1 is the lowest value if high is omitted.

Generator.integers

which should be used for new code.

Examples

>>> np.random.randint(2, size=10)
array([1, 0, 0, 0, 1, 1, 0, 0, 1, 0]) # random
>>> np.random.randint(1, size=10)
array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0])

Generate a 2 x 4 array of ints between 0 and 4, inclusive:

>>> np.random.randint(5, size=(2, 4))
array([[4, 0, 2, 1], # random
       [3, 2, 2, 0]])

Generate a 1 x 3 array with 3 different upper bounds

>>> np.random.randint(1, [3, 5, 10])
array([2, 2, 9]) # random

Generate a 1 by 3 array with 3 different lower bounds

>>> np.random.randint([1, 5, 7], 10)
array([9, 8, 7]) # random

Generate a 2 by 4 array using broadcasting with dtype of uint8

>>> np.random.randint([1, 3, 5, 7], [[10], [20]], dtype=np.uint8)
array([[ 8,  6,  9,  7], # random
       [ 1, 16,  9, 12]], dtype=uint8)

random_point_in_region(region, radius=1, past_paths=None, current_time=None, other_obstacles=[], over_inc=None, plot_region_bounds=None)

read_sol_data(data, ts)

run_planner(m, ts, data, other_past_paths=[], show_sol=None)

run_planner_cpp(m, ts, data)

single_region_bounds_for_rrt(region, bounds, overlaps)

single_region_box_bounds_for_rrt(region, box_bounds, overlaps)

split_bounds_for_rrt(region1, region2, bounds, overlaps)

split_box_bounds_for_rrt(region1, region2, box_bounds, overlaps)

which_agent_down(the_plan, ts, agent_caps, where, quant, cap, time, step_time, world_max)

catl_planning.visualization module

drawGraph(viewport, g, node_color='blue', edge_color='black', zorder=2)

Plots the given graph in the viewport.

drawPoint(viewport, pointx, pointy, color, style=None)

Draws a point in the planar environment.

drawPolicy(viewport, solution, color='black', alpha_min=1.0, zorder=2)

Draws the solution path with a fading effect.

drawRegion(viewport, shape, style=None, text=None, textStyle=None)

Draws a polygonal region in a planar environment.

show_environment(ts, save=None, figsize=None)

Draws the environment and optionally save it to a file.

show_environment_agents(ts, agents, fig, viewport, save=None, figsize=None)

Draws agents in the environment.

show_transition_agents(ts, agents, plt, save=None, figsize=None)

Draws edges in the environment.

show_world(ts, fig)

Draws a polygonal regions in a planar environment with labels.

catl_planning.write_sol module

PuLP SOL File Generator

Generates .sol files when using PuLP as an optimization interface. Using Gurobi is not required.

Note: .sol files are specific to the Gurobi solver. The specification can be found at: https://www.gurobi.com/documentation/9.1/refman/sol_format.html

write_sol(pulp_problem, file_name=None)

Generates a .sol file from a PuLP LpProblem object.

Parameters

• pulp_problem – PuLP LpProblem object to create .sol file from. The LpProblem object must be solved before passing it into this function.

• file_name (str) – (Optional). Name (and path) of the output file. By default the output file is saved to ./pulp.sol.

Module contents