Systems and methods for online iterative re-planning

A method for online iterative re-planning, comprising;

• receiving, at a first time step, a first grasp and an initial object pose of an agent;
• generating a first set of candidate object trajectories based on the first grasp and the initial object pose, wherein candidate object trajectories of the first set of candidate object trajectories provide a number of object poses from the initial object pose to a goal for a number of future time steps after the first time step;
• calculating contact points for grasps associated with each candidate object trajectory of the first set of candidate object trajectories;
• selecting a first candidate object trajectory from the first set of candidate object trajectories by applying a learned neural network cost model with the contact points to calculate respective energy for each grasp, the learned neural network cost model being trained based on an energy function using offline data associated with the agent;
• and
• causing the agent to execute the first candidate object trajectory at a second time step of the number of future time steps.

A non-transitory computer readable storage medium storing instructions that when executed by a computer having a processor to perform a method for online iterative re-planning, the method comprising:

• receiving, at a first time step, a first grasp and an initial object pose of an agent;
• generating a first set of candidate object trajectories based on the first grasp and the initial object pose, wherein candidate object trajectories of the first set of candidate object trajectories provide a number of object poses from the initial object pose to a goal for a number of future time steps after the first time step;
• calculating contact points for grasps associated with each candidate object trajectory of the first set of candidate object trajectories;
• selecting a first candidate object trajectory from the first set of candidate object trajectories by applying a learned neural network cost model with the contact points to calculate respective energy for each grasp, the learned neural network cost model being trained based on an energy function using offline data associated with the agent;
• and
• causing the agent to execute the first candidate object trajectory at a second time step of the number of future time steps.