System and method suitable for controlling a robot using a hierarchical optimization framework

Abstract

The present disclosure provides a system and a method for controlling a robot to execute a manipulation task of manipulating an object. The method includes determining, based on an initial pose and a target pose of the object, a kinematically feasible trajectory of pose of the object and extrinsic contact points between the object and an environment for different poses of the object while satisfying a collision constraint. The method further includes refining the kinematically feasible trajectory by introducing contact forces the robot applies to surfaces of the object and using extrinsic contact points between the object and the environment. The method further includes determining an optimal trajectory by further refining the refined trajectory provided by incorporating nonlinear dynamic constraints and sticking-sliding complementarity constraints, controlling the robot based on the optimal trajectory to execute the manipulation task.