SYSTEMS AND METHODS FOR ONLINE ITERATIVE RE-PLANNING

Systems and methods for online iterative re-planning are provided herein. In one embodiment, a method includes receiving, at a first time step, a first grasp and an initial object pose of an agent. The method also includes generating a first set of candidate object trajectories based on the first grasp and the initial object pose. Candidate object trajectories of the first set of candidate object trajectories provide a number object poses from the initial object pose to a goal for a number of future time steps after the first time step. The method further includes calculating contact points for grasps associated with each candidate object trajectory of the first set of candidate object trajectories. The method further includes selecting a first candidate object trajectory from the first set of candidate object trajectories. The method includes causing the agent to execute the first candidate object trajectory at a second time step.