This paper describes a collection of optimization algorithms for achieving dynamic planning, control, and state estimation for a bipedal robot designed to operate reliably in complex environments. To make challenging locomotion tasks tractable, we describe several novel applications of convex, mixed-integer, and sparse nonlinear optimization to problems ranging from footstep placement to whole-body planning and control. We also present a state estimator formulation that, when combined with our walking controller, permits highly precise execution of extended walking plans over non-flat terrain. We describe our complete system integration and experiments carried out on Atlas, a full-size hydraulic humanoid robot built by Boston Dynamics, Inc.

Optimization-based locomotion planning, estimation, and control design for the atlas humanoid robot

To plan dynamic, whole-body motions for robots, one conventionally faces the choice between a complex, full-body dynamic model containing every link and actuator of the robot, or a highly simplified model of the robot as a point mass. In this paper we explore a powerful middle ground between these extremes. We exploit the fact that while the full dynamics of humanoid robots are complicated, their centroidal dynamics (the evolution of the angular momentum and the center of mass (COM) position) are much simpler. By treating the dynamics of the robot in centroidal form and directly optimizing the joint trajectories for the actuated degrees of freedom, we arrive at a method that enjoys simpler dynamics, while still having the expressiveness required to handle kinematic constraints such as collision avoidance or reaching to a target. We further require that the robot's COM and angular momentum as computed from the joint trajectories match those given by the centroidal dynamics. This ensures that the dynamics considered by our optimization are equivalent to the full dynamics of the robot, provided that the robot's actuators can supply sufficient torque. We demonstrate that this algorithm is capable of generating highly-dynamic motion plans with examples of a humanoid robot negotiating obstacle course elements and gait optimization for a quadrupedal robot. Additionally, we show that we can plan without pre-specifying the contact sequence by exploiting the complementarity conditions between contact forces and contact distance.

/pdf/whole-body-motion-planning-with-centroidal-dynamics-and-full-u337p82it7.pdf

Whole-body motion planning with centroidal dynamics and full kinematics

We present a new method for planning footstep placements for a robot walking on uneven terrain with obstacles, using a mixed-integer quadratically-constrained quadratic program (MIQCQP). Our approach is unique in that it handles obstacle avoidance, kinematic reachability, and rotation of footstep placements, which typically have required non-convex constraints, in a single mixed-integer optimization that can be efficiently solved to its global optimum. Reachability is enforced through a convex inner approximation of the reachable space for the robot's feet. Rotation of the footsteps is handled by a piecewise linear approximation of sine and cosine, designed to ensure that the approximation never overestimates the robot's reachability. Obstacle avoidance is ensured by decomposing the environment into convex regions of obstacle-free configuration space and assigning each footstep to one such safe region. We demonstrate this technique in simple 2D and 3D environments and with real environments sensed by a humanoid robot. We also discuss computational performance of the algorithm, which is currently capable of planning short sequences of a few steps in under one second or longer sequences of 10–30 footsteps in tens of seconds to minutes on common laptop computer hardware. Our implementation is available within the Drake MATLAB toolbox [1].

/pdf/footstep-planning-on-uneven-terrain-with-mixed-integer-56svn7so7g.pdf

Footstep planning on uneven terrain with mixed-integer convex optimization

The Inverse Kinematics (IK) algorithms implemented in the open-source Orocos Kinematics and Dynamics Library (KDL) are arguably the most widely-used generic IK solvers worldwide. However, KDL's only joint-limit-constrained IK implementation, a pseudoinverse Jacobian IK solver, repeatedly exhibits false-negative failures on various humanoid platforms. In order to find a better IK solver for generic manipulator chains, a variety of open-source, drop-in alternatives have been implemented and evaluated for this paper. This article provides quantitative comparisons, using multiple humanoid platforms, between an improved implementation of the KDL inverse Jacobian algorithm, a set of sequential quadratic programming (SQP) IK algorithms that use a variety of quadratic error metrics, and a combined algorithm that concurrently runs the best performing SQP algorithm and the improved inverse Jacobian implementation. The best alternative IK implementation finds solutions much more often than KDL, is faster on average than KDL for typical manipulation chains, and (when desired) allows tolerances on each Cartesian dimension, further improving speed and convergence when an exact Cartesian pose is not possible and/or necessary.

TRAC-IK: An open-source library for improved solving of generic inverse kinematics

Many geometric estimation problems naturally take the form of synchronization over the special Euclidean group: estimate the values of a set of unknown poses \(x_{1},\ldots ,x_n \in \text {SE}(d)\) given noisy measurements of a subset of their pairwise relative transforms \(x_{i}^{-1}x_{j}\). Examples of this class include the foundational problems of pose-graph simultaneous localization and mapping (SLAM) (in robotics) and camera motion estimation (in computer vision), among others. This problem is typically formulated as a nonconvex maximum-likelihood estimation that is computationally hard to solve in general. Nevertheless, in this paper we present an algorithm that is able to effciently recover certifiably globally optimal solutions of the special Euclidean synchronization problem in a non-adversarial noise regime. The crux of our approach is the development of a semidefinite relaxation of the maximum-likelihood estimation whose minimizer provides the exact MLE so long as the magnitude of the noise corrupting the available measurements falls below a certain critical threshold; furthermore, whenever exactness obtains, it is possible to verify this fact a posteriori, thereby certifying the optimality of the recovered estimate. We develop a specialized optimization scheme for solving large-scale instances of this semidefinite relaxation by exploiting its low-rank, geometric, and graph-theoretic structure to reduce it to an equivalent optimization problem on a low-dimensional Riemannian manifold, and then design a Riemannian truncated-Newton trust-region method to solve this reduction effciently. We combine this fast optimization approach with a simple rounding procedure to produce our algorithm, SE-Sync. Experimental evaluation on a variety of simulated and real-world pose-graph SLAM datasets shows that SE-Sync is capable of recovering globally optimal solutions when the available measurements are corrupted by noise up to an order of magnitude greater than that typically encountered in robotics and computer vision applications, and does so more than an order of magnitude faster than the Gauss-Newton-based approach that forms the basis of current state-of-the-art techniques.

SE-Sync: a certifiably correct algorithm for synchronization over the special Euclidean group

The DARPA Robotics Challenge Trials held in December 2013 provided a landmark demonstration of dexterous mobile robots executing a variety of tasks aided by a remote human operator using only data from the robot's sensor suite transmitted over a constrained, field-realistic communications link. We describe the design considerations, architecture, implementation, and performance of the software that Team MIT developed to command and control an Atlas humanoid robot. Our design emphasized human interaction with an efficient motion planner, where operators expressed desired robot actions in terms of affordances fit using perception and manipulated in a custom user interface. We highlight several important lessons we learned while developing our system on a highly compressed schedule.

/pdf/an-architecture-for-online-affordance-based-perception-and-gc6fyaftsd.pdf

Matt DiCicco

Papers

An Architecture for Online Affordance-based Perception and Whole-body Planning