A Quadratic Programming Approach to Quasi-Static Whole-Body Manipulation

This paper introduces a local motion planning method for robotic systems with manipulating limbs, moving bases (legged or wheeled), and stance stability constraints arising from the presence of gravity. We formulate the problem of selecting local motions as a linearly constrained quadratic program (QP), that can be solved efficiently. The solution to this QP is a tuple of locally optimal joint velocities. By using these velocities to step towards a goal, both a path and an inverse-kinematic solution to the goal are obtained. This formulation can be used directly for real-time control, or as a local motion planner to connect waypoints. This method is particularly useful for high-degree-of-freedom mobile robotic systems, as the QP solution scales well with the number of joints. We also show how a number of practically important geometric constraints (collision avoidance, mechanism self-collision avoidance, gaze direction, etc.) can be readily incorporated into either the constraint or objective parts of the formulation. Additionally, motion of the base, a particular joint, or a particular link can be encouraged/discouraged as desired. We summarize the important kinematic variables of the formulation, including the stance Jacobian, the reach Jacobian, and a center of mass Jacobian. The method is easily extended to provide sparse solutions, where the fewest number of joints are moved, by iteration using Tibshirani’s method to accommodate an l_1 regularizer. The approach is validated and demonstrated on SURROGATE, a mobile robot with a TALON base, a 7 DOF serial-revolute torso, and two 7 DOF modular arms developed at JPL/Caltech

Contact-Implicit Trajectory Optimization Based on a Variable Smooth Contact Model and Successive Convexification

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.Comment: Accepted for publication in ICRA 201

Trajectory Optimization Through Contacts and Automatic Gait Discovery for Quadrupeds

In this work we present a trajectory Optimization framework for whole-body motion planning through contacts. We demonstrate how the proposed approach can be applied to automatically discover different gaits and dynamic motions on a quadruped robot. In contrast to most previous methods, we do not pre-specify contact switches, timings, points or gait patterns, but they are a direct outcome of the optimization. Furthermore, we optimize over the entire dynamics of the robot, which enables the optimizer to fully leverage the capabilities of the robot. To illustrate the spectrum of achievable motions, here we show eight different tasks, which would require very different control structures when solved with state-of-the-art methods. Using our trajectory Optimization approach, we are solving each task with a simple, high level cost function and without any changes in the control structure. Furthermore, we fully integrated our approach with the robot's control and estimation framework such that optimization can be run online. By demonstrating a rough manipulation task with multiple dynamic contact switches, we exemplarily show how optimized trajectories and control inputs can be directly applied to hardware.Comment: Video: https://youtu.be/sILuqJBsyK

Kinematics for Combined Quasi-Static Force and Motion Control in Multi-Limbed Robots

This paper considers how a multi-limbed robot can carry out manipulation tasks involving simultaneous and compatible end-effector velocity and force goals, while also maintaining quasi-static stance stability. The formulation marries a local optimization process with an assumption of a compliant model of the environment. For purposes of illustration, we first develop the formulation for a single fixed based manipulator arm. Some of the basic kinematic variables we previously introduced for multi-limbed robot mechanism analysis in [1] are extended to accommodate this new formulation. Using these extensions, we provide a novel definition for static equilibrium of multi-limbed robot with actuator limits, and provide general conditions that guarantee the ability to apply arbitrary end-effector forces. Using these extended definitions, we present the local optimization problem and its solution for combined manipulation and stance. We also develop, using the theory of strong alternatives, a new definition and a computable test for quasi-static stance feasibility in the presence of manipulation forces. Simulations illustrate the concepts and method

Multicontact Motion Retargeting Using Whole-Body Optimization of Full Kinematics and Sequential Force Equilibrium

This article presents a multicontact motion adaptation framework that enables teleoperation of high degree-of-freedom robots, such as quadrupeds and humanoids, for loco-manipulation tasks in multicontact settings. Our proposed algorithms optimize whole-body configurations and formulate the retargeting of multicontact motions as sequential quadratic programming, which is robust and stable near the edges of feasibility constraints. Our framework allows real-time operation of the robot and reduces cognitive load for the operator because infeasible commands are automatically adapted into physically stable and viable motions on the robot. The results in simulations with full dynamics demonstrated the effectiveness of teleoperating different legged robots interactively and generating rich multicontact movements. We evaluated the computational efficiency of the proposed algorithms, and further validated and analyzed multicontact loco-manipulation tasks on humanoid and quadruped robots by reaching, active pushing, and various traversal on uneven terrains

A simple and efficient BEM implementation of quasistatic linear visco-elasticity

A simple, yet efficient procedure to solve quasistatic problems of special linear visco-elastic solids at small strains with equal rheological response in all tensorial components, utilizing boundary element method (BEM), is introduced. This procedure is based on the implicit discretisation in time (the so-called Rothe method) combined with a simple "algebraic" transformation of variables, leading to a numerically stable procedure (proved explicitly by discrete energy estimates), which can be easily implemented in a BEM code to solve initial-boundary value visco-elastic problems by using the Kelvin elastostatic fundamental solution only. It is worth mentioning that no inverse Laplace transform is required here. The formulation is straightforward for both 2D and 3D problems involving unilateral frictionless contact. Although the focus is to the simplest Kelvin-Voigt rheology, a generalization to Maxwell, Boltzmann, Jeffreys, and Burgers rheologies is proposed, discussed, and implemented in the BEM code too. A few 2D and 3D initial-boundary value problems, one of them with unilateral frictionless contact, are solved numerically

ConSUS: A light-weight program conditioner

Program conditioning consists of identifying and removing a set of statements which cannot be executed when a condition of interest holds at some point in a program. It has been applied to problems in maintenance, testing, re-use and re-engineering. All current approaches to program conditioning rely upon both symbolic execution and reasoning about symbolic predicates. The reasoning can be performed by a ‘heavy duty’ theorem prover but this may impose unrealistic performance constraints. This paper reports on a lightweight approach to theorem proving using the FermaT Simplify decision procedure. This is used as a component to ConSUS, a program conditioning system for the Wide Spectrum Language WSL. The paper describes the symbolic execution algorithm used by ConSUS, which prunes as it conditions. The paper also provides empirical evidence that conditioning produces a significant reduction in program size and, although exponential in the worst case, the conditioning system has low degree polynomial behaviour in many cases, thereby making it scalable to unit level applications of program conditioning

Dynamic whole-body motion generation under rigid contacts and other unilateral constraints

The most widely used technique for generating wholebody motions on a humanoid robot accounting for various tasks and constraints is inverse kinematics. Based on the task-function approach, this class of methods enables the coordination of robot movements to execute several tasks in parallel and account for the sensor feedback in real time, thanks to the low computation cost. To some extent, it also enables us to deal with some of the robot constraints (e.g., joint limits or visibility) and manage the quasi-static balance of the robot. In order to fully use the whole range of possible motions, this paper proposes extending the task-function approach to handle the full dynamics of the robot multibody along with any constraint written as equality or inequality of the state and control variables. The definition of multiple objectives is made possible by ordering them inside a strict hierarchy. Several models of contact with the environment can be implemented in the framework. We propose a reduced formulation of the multiple rigid planar contact that keeps a low computation cost. The efficiency of this approach is illustrated by presenting several multicontact dynamic motions in simulation and on the real HRP-2 robot