We present a contact-implicit trajectory optimization framework that can plan contact-interaction trajectories for different robot architectures and tasks using a trivial initial guess and without requiring any parameter tuning. This is achieved by using a relaxed contact model along with an automatic penalty adjustment loop for suppressing the relaxation. Moreover, the structure of the problem enables us to exploit the contact information implied by the use of relaxation in the previous iteration, such that the solution is explicitly improved with little computational overhead. We test the proposed approach in simulation experiments for non-prehensile manipulation using a 7-DOF arm and a mobile robot and for planar locomotion using a humanoid-like robot in zero gravity. The results demonstrate that our method provides an out-of-the-box solution with good performance for a wide range of applications.

Avoiding obstacles in the perceived world has been the classical approach to autonomous mobile robot navigation. However, this usually leads to unnatural and inefficient motions that significantly differ from the way humans move in tight and dynamic spaces, as we do not refrain interacting with the environment around us when necessary. Inspired by this observation, we propose a framework for autonomous robot navigation among movable obstacles (NAMO) that is based on the theory of affordances and contact-implicit motion planning. We consider a realistic scenario in which a mobile service robot negotiates unknown obstacles in the environment while navigating to a goal state. An affordance extraction procedure is performed for novel obstacles to detect their movability, and a contact-implicit trajectory optimization method is used to enable the robot to interact with movable obstacles to improve the task performance or to complete an otherwise infeasible task. We demonstrate the performance of the proposed framework by hardware experiments with Toyota's Human Support Robot.

This paper presents a new method of maximizing the free space for a robot operating in a constrained environment under operator supervision. The objective is to make the resulting trajectories more robust to operator commands and/or changes in the environment. To represent the volume of free space, the constrained manipulability polytopes are used. These polytopes embed the distance to obstacles, the distance to joint limits and the distance to singular configurations. The volume of the resulting Cartesian polyhedron is used in an optimization-based motion planner to create the trajectories. Additionally, we show how fast collision-free inverse kinematic solutions can be obtained by exploiting the pre-computed inequality constraints. The proposed algorithm is validated in simulation and experimentally.

In this paper, we propose a contact-implicit trajectory optimization (CITO) method based on a variable smooth contact model (VSCM) and successive convexification (SCvx). The VSCM facilitates the convergence of gradient-based optimization without compromising physical fidelity. On the other hand, the proposed SCvx-based approach combines the advantages of direct and shooting methods for CITO. For evaluations, we consider non-prehensile manipulation tasks. The proposed method is compared to a version based on iterative linear quadratic regulator (iLQR) on a planar example. The results demonstrate that both methods can find physically-consistent motions that complete the tasks without a meaningful initial guess owing to the VSCM. The proposed SCvx-based method outperforms the iLQR-based method in terms of convergence, computation time, and the quality of motions found. Finally, the proposed SCvx-based method is tested on a standard robot platform and shown to perform efficiently for a real-world application.

In this paper, we analyze the effects of contact models on contact-implicit trajectory optimization for manipulation. We consider three different approaches: (1)a contact model that is based on complementarity constraints, (2)a smooth contact model, and our proposed method (3) a variable smooth contact model. We compare these models in simulation in terms of physical accuracy, quality of motions, and computation time. In each case, the optimization process is initialized by setting all torque variables to zero, namely, without a meaningful initial guess. For simulations, we consider a pushing task with varying complexity for a 7 degrees-of-freedom robot arm. Our results demonstrate that the optimization based on the proposed variable smooth contact model provides a good trade-off between the physical fidelity and quality of motions at the cost of increased computation time.