Apparatus and method for planning contact-interaction trajectories

A method for performing, by a robot, a task involving moving an object from an initial pose of the object to a target pose of the object, wherein the method uses a processor coupled with instructions implementing the method, wherein the instructions are stored in a memory,

• wherein the memory storing a dynamic model representing one or more of geometric, dynamic, and frictional properties of the robot and the environment, and a relaxed contact model to represent dynamic interactions between the robot and the object via virtual forces generated by one or more contact pairs associated with a geometry on the robot and a geometry on the object, where the virtual force acting on the object at a distance in each contact pair is in proportion to a stiffness of the virtual force, and
• wherein the instructions, when executed by the processor carry out steps of the method, comprising:
• obtaining a current state of interaction between the robot and the object;
• and
• determining, iteratively until a termination condition is met, a trajectory, associated control commands for controlling the robot, and virtual stiffness values to move the object according to the trajectory by performing optimization minimizing the stiffness of the virtual force and minimizing a difference between the target pose of the object and a final pose of the object moved from the initial pose by the robot controlled according to the control commands via the virtual force generated according to the relaxed contact model, wherein for executing at least one iteration, the method further comprising:
• determining a current trajectory, current control commands, and current virtual stiffness values for a current penalty value on the stiffness of the virtual force by solving an optimization problem initialized with a previous trajectory and previous control commands determined during a previous iteration with a previous penalty value on the stiffness of the virtual force;
• updating the current trajectory and current control commands to reduce the distance in each contact pair to produce an updated trajectory and updated control commands to initialize the optimization problem in a next iteration;
• and
• updating the current value of the stiffness of the virtual force for the optimization in the next iteration;
• performing a trajectory optimization problem using successive convexification;
• assigning a first penalty value, as an updated penalty value, to the stiffness associated with the virtual forces, wherein the assigned penalty value is greater than a penalty value assigned in a previous iteration if pose constraints are satisfied, and wherein the pose constraints comprise information about position and orientation errors associated with the trajectory;
• determining the current trajectory and the control commands satisfying the pose constraints, and residual stiffness indicating position, timing, and magnitude of physical forces for performing the task;
• executing a pulling controller on the current trajectory to determine the pulling force for pulling contact pairs on the robot associated with non-zero stiffness towards the corresponding contact pairs in the environment and
• moving a robot arm of the robot according to the trajectory and the associated control commands.

A non-transitory computer readable storage medium, embodied thereon a program executable by a processor for performing a method moving an object from an initial pose of the object to a target pose of the object, wherein the medium storing a dynamic model representing one or more of geometric, dynamic, and frictional properties of the robot and the environment, and a relaxed contact model to represent dynamic interactions between the robot and the object via virtual forces generated by one or more contact pairs associated with a geometry on the robot and a geometry on the object, where the virtual force acting on the object at a distance in each contact pair is in proportion to a stiffness of the virtual force, the method comprising:

• obtaining a current state of interaction between the robot and the object;
• and
• determining, iteratively until a termination condition is met, a trajectory, associated control commands for controlling the robot, and virtual stiffness values to move the object according to the trajectory by performing optimization minimizing the stiffness of the virtual force and minimizing a difference between the target pose of the object and a final pose of the object moved from the initial pose by the robot controlled according to the control commands via the virtual force generated according to the relaxed contact model, wherein for executing at least one iteration, the method further comprising:
• determining a current trajectory, current control commands, and current virtual stiffness values for a current penalty value on the stiffness of the virtual force by solving an optimization problem initialized with a previous trajectory and previous control commands determined during a previous iteration with a previous penalty value on the stiffness of the virtual force;
• updating the current trajectory and current control commands to reduce the distance in each contact pair to produce an updated trajectory and updated control commands to initialize the optimization problem in a next iteration;
• and
• updating the current value of the stiffness of the virtual force for the optimization in the next iteration;
• performing a trajectory optimization problem using successive convexification;
• assigning a first penalty value, as an updated penalty value, to the stiffness associated with the virtual forces, wherein the assigned penalty value is greater than a penalty value assigned in a previous iteration if pose constraints are satisfied, and wherein the pose constraints comprise information about position and orientation errors associated with the trajectory;
• determining the current trajectory and the control commands satisfying the pose constraints, and residual stiffness indicating position, timing, and magnitude of physical forces for performing the task;
• executing a pulling controller on the current trajectory to determine the pulling force for pulling contact pairs on the robot associated with non-zero stiffness towards the corresponding contact pairs in the environment and
• moving a robot arm of the robot according to the trajectory and the associated control commands.