rtd.planner.trajopt.RtdTrajOpt

class rtd.planner.trajopt.RtdTrajOpt(trajOptProps: ~rtd.planner.trajopt.TrajOptProps.TrajOptProps, reachableSets: dict[str, rtd.planner.reachsets.ReachSetGenerator.ReachSetGenerator], objective: ~rtd.planner.trajopt.Objective.Objective, optimizationEngine: ~rtd.planner.trajopt.OptimizationEngine.OptimizationEngine, trajectoryFactory: <module 'rtd.planner.trajectory.TrajectoryFactory' from '/rtd/rtd/planner/trajectory/TrajectoryFactory.py'>, **options)

Bases: object

Core trajectory optimization routine for RTD

This object handles the necessary calls to perform the actual trajectory optimization when requested. It calls the generators for the reachble sets and combines all the resulting nonlinear constraints in the end

__init__(trajOptProps: ~rtd.planner.trajopt.TrajOptProps.TrajOptProps, reachableSets: dict[str, rtd.planner.reachsets.ReachSetGenerator.ReachSetGenerator], objective: ~rtd.planner.trajopt.Objective.Objective, optimizationEngine: ~rtd.planner.trajopt.OptimizationEngine.OptimizationEngine, trajectoryFactory: <module 'rtd.planner.trajectory.TrajectoryFactory' from '/rtd/rtd/planner/trajectory/TrajectoryFactory.py'>, **options)

Construct the RtdTrajOpt object.

Store all the handles to objects that we want to use. This should encapsulate the main trajectory optimization aspection of RTD & ideally need very little specialization between versions

Parameters:

• trajOptProps – TrajOptProps

• reachableSets – dict[str, ReachSetGenerator]

• objective – Objective

• optimizationEngine – OptimizationEngine

• trajectoryFactory – TrajectoryFactory

Methods

__init__(trajOptProps, reachableSets, ...)	Construct the RtdTrajOpt object.
solveTrajOpt(robotState, worldState, waypoint)	Execute the solver for trajectory optimization

class merge_constraints(nlconCallbacks: dict[str, Callable], buffer_size=16)

Bases: object

A functor for computing the constraints for a given input, with lookup for speed optimizations

findBuffer(k) → tuple | None

Return output if result for given input exists in the buffer, otherwise return None

updateBuffer(k, res)

Ensure buffer size < buffer_size and add input-output pair into buffer

solveTrajOpt(robotState: ~rtd.entity.states.EntityState.EntityState, worldState: ~rtd.sim.world.WorldState.WorldState, waypoint, initialGuess: <module 'rtd.planner.trajectory.Trajectory' from '/rtd/rtd/planner/trajectory/Trajectory.py'> = None, **rsAdditionalArgs: dict[dict]) → tuple[<module 'rtd.planner.trajectory.Trajectory' from '/rtd/rtd/planner/trajectory/Trajectory.py'>, float, dict]

Execute the solver for trajectory optimization

Note

The returned info dict has the following entries: worldState, robotState, rsInstances, nlconCallbacks, objectiveCallback, waypoint, bounds, num_parameters, guess, trajectory, cost

Parameters:

• robotState – EntityState: State of the robot.

• worldState – WorldState: Observed state of the world for the reachable sets

• waypoint – Waypoint we want to optimize to

• initialGuess – Trajectory: Past trajectory to use as an initial guess

• rsAdditionalArgs – additional arguments to pass to the reachable sets, by set name

Returns:

Trajectory, cost: float, info: dict): trajectory is an instance of the trajectory object that corresponds to the solved trajectory optimization. If it wasn’t successful, this will either be an invalid trajectory or empty. cost is the final cost of the objective used. info is a dict of optimization data.

Return type:

(trajectory