Optimization for Contact-rich Robotic Manipulation with High-fidelity Geometry

Mengchao Zhang and Kris Hauser

Summary

Humans leverage diverse strategies such as in-hand manipulation, whole-body manipulation, and environmental contact to manipulate a variety of objects in our daily lives. In contrast, even though the robotics field has long attempted to imitate these behaviors, the interactions of the majority of robots with their surroundings are primarily restricted to their end-effectors, and the motions are mostly pick and place, which are not as dexterous or energy-efficient as those performed by humans.

In bridging the gap between human-like contact interactions and current robotic capabilities, we make foundational contributions through introducing a novel computational model that employs a semi-infinite/infinite programming approach to effectively address the intricate nature of pervasive contact scenarios. The proposed model demonstrates applicability in various contexts, including object's 6D pose estimation within cluttered environments, contact-rich stable grasp pose optimization, and contact-rich manipulation trajectory optimization involving complex-shaped objects. In each of these scenarios, we address the problem that contact is an infinite phenomena involving continuous regions of interaction, which requires a discrete approximation to solve. Contrary to previous methods which need to discretize contacting surfaces a priori into a finite set of contact points, the proposed method operates directly on the continuous underlying geometry, and it dynamically identifies a finite set of constraints essential for problem resolution. As a result, the subsequent solving process is rendered feasible and tractable.

Additionally, to enable robust execution of planned trajectories, we introduce an innovative technique that combine the strengths of model-based planning and reinforcement learning. The innovative method, Planned-Contact Informed Policy, takes features extracted from the planned trajectory as input for the control policy, enabling it to anticipate changes in contact modes and adjust its actions accordingly. Consequently, control policies trained in this manner is capable of stabilizing the execution of similar trajectories planned for the same task type.

All the contributions presented here constitute fundamental building blocks towards automated robotic manipulation.

		Simultaneous Trajectory Optimization and Contact Selection for Contact-rich Manipulation with High-fidelity Geometry Contact-implicit trajectory optimization (CITO) is an effective method to plan complex trajectories for various contact-rich systems including manipulation and locomotion. CITO formulates contact as complementarity constraints and requires solving a mathematical program with complementarity constraints (MPCC). However, MPCC's solve time increases steeply with the number of variables and complementarity constraints, which limits CITO's applicability to problems with low geometric complexity. Simultaneous trajectory optimization and contact selection (STOCS), as an extension of CITO, innovatively incorporates the identification of salient contact points and contact times inside trajectory optimization. This integration effectively reduces the number of variables and constraints in the resulting MPCC, rendering the computation of manipulation trajectories for objects with complex, non-convex geometries computationally tractable. Notwithstanding, the application of STOCS was initially confined to 2D contexts with relatively simple geometries due to the long solve time. Through re-implementing the software to incorporate auto differentiation support to accelerate MPCCs' solving and introducing a novel contact points and contact times selection method, Time-active Maximum Violation Oracle, this study firstly broadens the applicability of STOCS in 2D and enables its use with more complex objects and environments. Furthermore, we extend STOCS from 2D to 3D and demonstrate contact-rich manipulation trajectory planning with high-fidelity geometric representations for the first time.
		Simultaneous Trajectory Optimization and Contact Selection for Multi-Modal Manipulation Planning Complex dexterous manipulations require switching between prehensile and non-prehensile grasps, and sliding and pivoting the object against the environment. This paper presents a manipulation planner that is able to reason about diverse changes of contacts to discover such plans. It implements a hybrid approach that performs contact-implicit trajectory optimization for pivoting and sliding manipulation primitives and sampling-based planning to change between manipulation primitives and target object poses. The optimization method, simultaneous trajectory optimization and contact selection (STOCS), introduces an infinite programming framework to dynamically select from contact points and support forces between the object and environment during a manipulation primitive. To sequence manipulation primitives, a sampling-based tree-growing planner uses STOCS to construct a manipulation tree. We show that by using a powerful trajectory optimizer, the proposed planner can discover multi-modal manipulation trajectories involving grasping, sliding, and pivoting within a few dozen samples. The resulting trajectories are verified to enable a 6 DoF manipulator to manipulate physical objects successfully. Video
		Non-Penetration Iterative Closest Points for Single-View Multi-Object 6D Pose Estimation This paper presents a novel iterative closest points (ICP) variant, non-penetration iterative closest points (NPICP), which prevents interpenetration in 6DOF pose optimization and/or joint optimization of multiple object poses. This capability is particularly advantageous in cluttered scenarios, where there are many interactions between objects that constrain the space of valid poses. We use a semi-infinite programming approach to handle non-penetration constraints between complex, non-convex 3D geometries. NPICP is applied to a common use case for ICP as a post-processing method to improve the pose estimation accuracy of a rough guess. The results show that NPICP outperforms ICP, assists in outlier detection, and also outperforms the best result on the IC-BIN dataset in the Benchmark for 6D Object Pose Estimation. Video
		Infinite-programming with Complementarity Constraints for Pose Optimization with Pervasive Contac This paper presents a novel computational model to address the problem that contact is an infinite phenomena involving continuous regions of interaction. The problem is cast as an infinite program with complementarity constraints (IPCC). Rather than pre-discretize contacting surfaces into a finite number of contact points, we use infinite programming (IP) techniques that operate on the underlying continuous geometry, but dynamically determine a finite number of constraints that are most relevant to solving the problem. Then we solve the series of problems whose solutions converge toward one that contains a true optimum of the original IPCC. We apply the model to a grasping pose optimization problem for a gripper and a humanoid robot, and our model enables the robots to find a feasible pose to hold (non-)convex objects while ensuring force and torque balance.

NSF NRI Grant #1911087