Apparatus and method for planning contact-interaction trajectories

1. A robot configured for performing a task involving moving an object from an initial pose of the object to a target pose of the object in an environment, the robot comprising:

an input interface configured to accept contact interactions between the robot and the environment;

a memory configured to store 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;

a processor configured to determine, 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 reducing the stiffness of the virtual force and reducing 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 processor is configured to:

determine 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;

update 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

update the current value of the stiffness of the virtual force for the optimization in the next iteration; and

an actuator configured to move a robot arm of the robot according to the trajectory and the associated control commands, wherein the memory is further configured to store a pulling controller which uses virtual forces left after computing the current trajectory to attract the geometry on the robot to the corresponding geometry in the environment to facilitate physical contacts.

2. The robot of claim 1 ,

wherein the virtual force corresponding to the contact pair is based on one or more of the stiffness of the virtual force, a curvature associated with the virtual force, and a signed distance between the geometry on the robot and a geometry in the environment associated with the contact pair.

3. The robot of claim 1 ,

wherein the virtual force points, at each instance of time during the interaction, a projection of a contact surface normal onto a center of mass of the object.

4. The robot of claim 1 ,

wherein the optimization corresponds to a multi-objective optimization of a cost function, wherein the processor is further configured to perform the multi-objective optimization of the cost function, and

wherein the cost function is a combination of:

a first cost to determine a positioning error of the final pose of the object moved by the robot with respect to the target pose of the object, and

a second cost to determine a cumulative stiffness of the virtual forces.

5. The robot of claim 1 , wherein for executing the at least one iteration, the processor is further configured to:

perform a trajectory optimization problem using a successive convexification;

assign 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 error and orientation error associated with the trajectory;

determine the current trajectory, the current control commands, and current virtual stiffness values satisfying the pose constraints, and residual stiffness indicating position, timing, and magnitude of physical forces for performing the task; and

execute the pulling controller on the current trajectory to determine the pulling force for pulling contact pairs on the robot associated with non-zero stiffness values towards the corresponding contact pairs in the environment.

6. The robot of claim 5 , wherein the processor is further configured to:

assign a second penalty value, as the updated penalty value, to the stiffness associated with the virtual forces, wherein the assigned penalty value is less than a penalty value assigned in a previous iteration if the pose constraints are not satisfied; and

perform the trajectory optimization problem using the successive convexification.

7. The robot of claim 5 ,

wherein the processor is further configured to execute the pulling controller based on an average of the stiffness, when the average of the stiffness is greater than a stiffness threshold.

8. The robot of claim 5 ,

wherein to determine the pulling force, the processor is further configured to execute the pulling controller based on a prior stiffness, and

wherein the prior stiffness indicates position, timing, and magnitude of physical forces associated with the previous iteration.

9. The robot of claim 5 ,

wherein the memory further stores a hill-climbing search, and

wherein the processor is further configured to execute the hill-climbing search to reduce the non-zero stiffness values to eliminate excessive virtual forces.

10. The robot of claim 1 ,

wherein the task comprises at least one of a non-prehensile operation or a prehensile operation.

11. The robot of claim 1 , wherein the termination condition is met when:

a number of iterations is greater than a first threshold, or

the virtual stiffness values are reduced to zero.

12. 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.

13. The method of claim 12 , further comprising:

assigning a second penalty value, as the updated value, to the stiffness associated with the virtual forces, and wherein the assigned penalty value is less than a penalty value assigned in a previous iteration if the pose constraints are not satisfied; and

performing a trajectory optimization problem using the successive convexification.

14. 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.