Sampling-Based Motion Planning with Scene Graphs Under Perception Constraints

We develop STTO, a novel trajectory optimization method that minimizes a cost function jointly over the robot state and time. STTO explicitly enforces state and temporal constraints (e.g., for time-critical tasks such as delivering a cup of water within 8 seconds) in a nonlinear least squares formulation, that can be solved for a smooth trajectory in just tens of milliseconds.