Pre-trained LLMs that are further trained with image data perform well on vision-language tasks. While adding images during a second training phase effectively unlocks this capability, it is unclear how much of a gain or loss this two-step pipeline gives over VLMs which integrate images earlier into the training process. To investigate this, we train models spanning various datasets, scales, image-text ratios, and amount of pre-training done before introducing vision tokens. We then fine-tune these models and evaluate their downstream performance on a suite of vision-language and text-only tasks. We find that pre-training with a mixture of image and text data allows models to perform better on vision-language tasks while maintaining strong performance on text-only evaluations. On an average of 6 diverse tasks, we find that for a 1B model, introducing visual tokens 80\% of the way through pre-training results in a 2\% average improvement over introducing visual tokens to a fully pre-trained model.

Classifier-free guidance (CFG) is a key technique for improving conditional generation in diffusion models, enabling more accurate control while enhancing sample quality. It is natural to extend this technique to video diffusion, which generates video conditioned on a variable number of context frames, collectively referred to as history. However, we find two key challenges to guiding with variable-length history: architectures that only support fixed-size conditioning, and the empirical observation that CFG-style history dropout performs poorly. To address this, we propose the Diffusion Forcing Transformer (DFoT), a video diffusion architecture and theoretically grounded training objective that jointly enable conditioning on a flexible number of history frames. We then introduce History Guidance, a family of guidance methods uniquely enabled by DFoT. We show that its simplest form, vanilla history guidance, already significantly improves video generation quality and temporal consistency. A more advanced method, history guidance across time and frequency further enhances motion dynamics, enables compositional generalization to out-of-distribution history, and can stably roll out extremely long videos. Project website: https://boyuan.space/history-guidance

We introduce a semidefinite relaxation for optimal control of linear systems with time scaling. These problems are inherently nonconvex, since the system dynamics involves bilinear products between the discretization time step and the system state and controls. The proposed relaxation is closely related to the standard second-order semidefinite relaxation for quadratic constraints, but we carefully select a subset of the possible bilinear terms and apply a change of variables to achieve empirically tight relaxations while keeping the computational load light. We further extend our method to handle piecewise-affine (PWA) systems by formulating the PWA optimal-control problem as a shortest-path problem in a graph of convex sets (GCS). In this GCS, different paths represent different mode sequences for the PWA system, and the convex sets model the relaxed dynamics within each mode. By combining a tight convex relaxation of the GCS problem with our semidefinite relaxation with time scaling, we can solve PWA optimal-control problems through a single semidefinite program.

Visuotactile sensors are a popular tactile sensing strategy due to high-fidelity estimates of local object geometry. However, existing algorithms for processing raw sensor inputs to useful intermediate signals such as contact patches struggle in high-deformation regimes. This is due to physical constraints imposed by sensor hardware and small-deformation assumptions used by mechanics-based models. In this work, we propose a fusion algorithm for proximity and visuotactile point clouds for contact patch segmentation, entirely independent from membrane mechanics. This algorithm exploits the synchronous, high spatial resolution proximity and visuotactile modalities enabled by an extremely deformable, selectively transmissive soft membrane, which uses visible light for visuotactile sensing and infrared light for proximity depth. We evaluate our contact patch algorithm in low ($\mathbf{1 0 \%}$), medium ($\mathbf{6 0 \%}$), and high$(100 \%+)$strain states. We compare our method against three baselines: proximity-only, tactile-only, and a first principles mechanics model. Our approach outperforms all baselines with an average RMSE under 2.8 mm of the contact patch geometry across all strain ranges. We demonstrate our contact patch algorithm in four applications: varied stiffness membranes, torque and shear-induced wrinkling, closed loop control, and pose estimation.

When planning motions in a configuration space that has underlying symmetries (e.g. when manipulating one or multiple symmetric objects), the ideal planning algorithm should take advantage of those symmetries to produce shorter trajectories. However, finite symmetries lead to complicated changes to the underlying topology of configuration space, preventing the use of standard algorithms. We demonstrate how the key primitives used for sampling-based planning can be efficiently implemented in spaces with finite symmetries. A rigorous theoretical analysis, building upon a study of the geometry of the configuration space, shows improvements in the sample complexity of several standard algorithms. Furthermore, a comprehensive slate of experiments demonstrates the practical improvements in both path length and runtime.

Simulating object dynamics from real-world perception shows great promise for digital twins and robotic manipulation but often demands labor-intensive measurements and expertise. We present a fully automated Real2Sim pipeline that generates simulation-ready assets for real-world objects through robotic interaction. Using only a robot’s joint torque sensors and an external camera, the pipeline identifies visual geometry, collision geometry, and physical properties such as inertial parameters. Our approach introduces a general method for extracting high-quality, object-centric meshes from photometric reconstruction techniques (e.g., NeRF, Gaussian Splatting) by employing alpha-transparent training while explicitly distinguishing foreground occlusions from background subtraction. We validate the full pipeline through extensive experiments, demonstrating its effectiveness across diverse objects. By eliminating the need for manual intervention or environment modifications, our pipeline can be integrated directly into existing pick-and-place setups, enabling scalable and efficient dataset creation. Project page (with code and data): https://scalable-real2sim.github.io/.

Cotraining with demonstration data generated both in simulation and on real hardware has emerged as a promising recipe for scaling imitation learning in robotics. This work seeks to elucidate basic principles of this simand-real cotraining to inform simulation design, sim-and-real dataset creation, and policy training. Our experiments confirm that cotraining with simulated data can dramatically improve performance, especially when real data is limited. We show that these performance gains scale with additional simulated data up to a plateau; adding more real-world data increases this performance ceiling. The results also suggest that reducing physical domain gaps may be more impactful than visual fidelity for non-prehensile or contact-rich tasks. Perhaps surprisingly, we find that some visual gap can help cotraining – binary probes reveal that high-performing policies must learn to distinguish simulated domains from real. We conclude by investigating this nuance and mechanisms that facilitate positive transfer between sim-and-real. Focusing narrowly on the canonical task of planar pushing from pixels allows us to be thorough in our study. In total, our experiments span 50+ real-world policies (evaluated on 1000+ trials) and 250 simulated policies (evaluated on 50,000+ trials). Videos and code can be found at https://sim-and-real-cotraining.github.io/.

Fleets of robots ingest massive amounts of heterogeneous streaming data silos generated by interacting with their environments, far more than what can be stored or transmitted with ease. At the same time, teams of robots should co-acquire diverse skills through their heterogeneous experiences in varied settings. How can we enable such fleet-level learning without having to transmit or centralize fleet-scale data? In this paper, we investigate policy merging (PoMe) from such distributed heterogeneous datasets as a potential solution. To efficiently merge policies in the fleet setting, we propose FLEET-MERGE, an instantiation of distributed learning that accounts for the permutation invariance that arises when parameterizing the control policies with recurrent neural networks. We show that FLEET-MERGE consolidates the behavior of policies trained on 50 tasks in the Meta-World environment, with good performance on nearly all training tasks at test time. Moreover, we introduce a novel robotic tool-use benchmark, FLEET-TOOLS, for fleet policy learning in compositional and contact-rich robot manipulation tasks, to validate the efficacy of FLEET-MERGE on the benchmark.

Learning-based neural-network (NN) control policies have shown impressive empirical performance in a wide range of tasks in robotics and control. However, formal (Lyapunov) stability guarantees over the region-of-attraction (ROA) for NN controllers with nonlinear dynamical systems are challenging to obtain, and most existing approaches rely on expensive solvers for sums-of-squares (SOS), mixed-integer programming (MIP), or satisfiability modulo theories (SMT). In this paper, we demonstrate a new framework for learning NN controllers together with Lyapunov certificates using fast empirical falsification and strategic regularizations. We propose a novel formulation that defines a larger verifiable region-of-attraction (ROA) than shown in the literature, and refines the conventional restrictive constraints on Lyapunov derivatives to focus only on certifiable ROAs. The Lyapunov condition is rigorously verified post-hoc using branch-and-bound with scalable linear bound propagation-based NN verification techniques. The approach is efficient and flexible, and the full training and verification procedure is accelerated on GPUs without relying on expensive solvers for SOS, MIP, nor SMT. The flexibility and efficiency of our framework allow us to demonstrate Lyapunov-stable output feedback control with synthesized NN-based controllers and NN-based observers with formal stability guarantees, for the first time in literature.

We present an efficient method for certifying non-collision for piecewise-polynomial motion plans in algebraic reparametrizations of configuration space. Such motion plans include those generated by popular randomized methods including RRTs and PRMs, as well as those generated by many methods in trajectory optimization. Based on Sums-of-Squares optimization, our method provides exact, rigorous certificates of non-collision; it can never falsely claim that a motion plan containing collisions is collision-free. We demonstrate that our formulation is practical for real world deployment, certifying the safety of a twelve degree of freedom motion plan in just over a second. Moreover, the method is capable of discriminating the safety or lack thereof of two motion plans which differ by only millimeters.

In order for a bimanual robot to manipulate an object that is held by both hands, it must construct motion plans such that the transformation between its end effectors remains fixed. This amounts to complicated nonlinear equality constraints in the configuration space, which are difficult for trajectory optimizers. In addition, the set of feasible configurations becomes a measure zero set, which presents a challenge to sampling-based motion planners. We leverage an analytic solution to the inverse kinematics problem to parametrize the configuration space, resulting in a lower-dimensional representation where the set of valid configurations has positive measure. We describe how to use this parametrization with existing motion planning algorithms, including sampling-based approaches, trajectory optimizers, and techniques that plan through convex inner-approximations of collision-free space.

Many computations in robotics can be dramatically accelerated if the robot configuration space is described as a collection of simple sets. For example, recently developed motion planners rely on a convex decomposition of the free space to design collision-free trajectories using fast convex optimization. In this work, we present an efficient method for approximately covering complex configuration spaces with a small number of polytopes. The approach constructs a visibility graph using sampling and generates a clique cover of this graph to find clusters of samples that have mutual line of sight. These clusters are then inflated into large, full-dimensional, polytopes. We evaluate our method on a variety of robotic systems and show that it consistently covers larger portions of free configuration space, with fewer polytopes, and in a fraction of the time compared to previous methods.

Collision-free motion planning with trajectory optimization is inherently nonconvex. Some of this nonconvexity is fundamental: the robot might need to make a discrete decision to go left around an obstacle or right around an obstacle. Some of the nonconvexity is potentially more benign: we might want to penalize high-order derivatives of our continuous trajectories in order to encourage smoothness. Recently, Graphs of Convex Sets (GCS) have been applied to trajectory optimization, addressing the fundamental nonconvexity with efficient online optimization over a "roadmap" represented by an approximate convex decomposition of the configuration space. In this paper, we explore some of the most useful nonconvex costs and constraints and the suitability of combining convex "global" optimization using GCS with nonconvex trajectory optimization for rounding the local solutions. We find that for many applications, this combination can lead to a small number of nonconvex optimizations finding extremely good solutions to the nonconvex trajectory optimization problem.

This paper presents Diffusion Forcing, a new training paradigm where a diffusion model is trained to denoise a set of tokens with independent per-token noise levels. We apply Diffusion Forcing to sequence generative modeling by training a causal next-token prediction model to generate one or several future tokens without fully diffusing past ones. Our approach is shown to combine the strengths of next-token prediction models, such as variable-length generation, with the strengths of full-sequence diffusion models, such as the ability to guide sampling to desirable trajectories. Our method offers a range of additional capabilities, such as (1) rolling-out sequences of continuous tokens, such as video, with lengths past the training horizon, where baselines diverge and (2) new sampling and guiding schemes that uniquely profit from Diffusion Forcing's variable-horizon and causal architecture, and which lead to marked performance gains in decision-making and planning tasks. In addition to its empirical success, our method is proven to optimize a variational lower bound on the likelihoods of all subsequences of tokens drawn from the true joint distribution. Project website: https://boyuan.space/diffusion-forcing/

We present a fast algorithm for the design of smooth paths (or trajectories) that are constrained to lie in a collection of axis-aligned boxes. We consider the case where the number of these safe boxes is large, and basic preprocessing of them (such as finding their intersections) can be done offline. At runtime, we quickly generate a smooth path between given initial and terminal positions. Our algorithm designs trajectories that are guaranteed to be safe at all times, and detects infeasibility whenever such a trajectory does not exist. Our algorithm is based on two subproblems that we can solve very efficiently: finding a shortest path in a weighted graph, and solving (multiple) convex optimal-control problems. We demonstrate the proposed path planner on large-scale numerical examples, and we provide an efficient open-source software implementation,fastpathplanning.

We propose a theoretical framework for studying behavior cloning of complex expert demonstrations using generative modeling. Our framework invokes low-level controllers - either learned or implicit in position-command control - to stabilize imitation around expert demonstrations. We show that with (a) a suitable low-level stability guarantee and (b) a powerful enough generative model as our imitation learner, pure supervised behavior cloning can generate trajectories matching the per-time step distribution of essentially arbitrary expert trajectories in an optimal transport cost. Our analysis relies on a stochastic continuity property of the learned policy we call "total variation continuity" (TVC). We then show that TVC can be ensured with minimal degradation of accuracy by combining a popular data-augmentation regimen with a novel algorithmic trick: adding augmentation noise at execution time. We instantiate our guarantees for policies parameterized by diffusion models and prove that if the learner accurately estimates the score of the (noise-augmented) expert policy, then the distribution of imitator trajectories is close to the demonstrator distribution in a natural optimal transport distance. Our analysis constructs intricate couplings between noise-augmented trajectories, a technique that may be of independent interest. We conclude by empirically validating our algorithmic recommendations, and discussing implications for future research directions for better behavior cloning with generative modeling.

The problem of piecewise affine (PWA) regression and planning is of foundational importance to the study of online learning, control, and robotics, where it provides a theoretically and empirically tractable setting to study systems undergoing sharp changes in the dynamics. Unfortunately, due to the discontinuities that arise when crossing into different ``pieces,'' learning in general sequential settings is impossible and practical algorithms are forced to resort to heuristic approaches. This paper builds on the recently developed smoothed online learning framework and provides the first algorithms for prediction and simulation in PWA systems whose regret is polynomial in all relevant problem parameters under a weak smoothness assumption; moreover, our algorithms are efficient in the number of calls to an optimization oracle. We further apply our results to the problems of one-step prediction and multi-step simulation regret in piecewise affine dynamical systems, where the learner is tasked with simulating trajectories and regret is measured in terms of the Wasserstein distance between simulated and true data. Along the way, we develop several technical tools of more general interest.

The empirical success of reinforcement learning (RL) in contact-rich manipulation leaves much to be understood from a model-based perspective, where the key difficulties are often attributed to 1) the explosion of contact modes, 2) stiff, nonsmooth contact dynamics and the resulting exploding/discontinuous gradients, and 3) the nonconvexity of the planning problem. The stochastic nature of RL addresses 1) and 2) by effectively sampling and averaging the contact modes. On the other hand, model-based methods have tackled the same challenges by smoothing contact dynamics analytically. Our first contribution is to establish the theoretical equivalence of the two smoothing schemes for simple systems, and provide qualitative and empirical equivalence on several complex examples. In order to further alleviate 2), our second contribution is a convex, differentiable, and quasi-dynamic formulation of contact dynamics, which is amenable to both smoothing schemes, and has proven to be highly effective for contact-rich planning. Our final contribution resolves 3), where we show that classical sampling-based motion planning algorithms can be effective in global planning when contact modes are abstracted via smoothing. Applying our method on several challenging contact-rich manipulation tasks, we demonstrate that efficient model-based motion planning can achieve results comparable to RL, but with dramatically less computation.

Autonomous robots typically incorporate complex sensors in their decision-making and control loops. These sensors, such as cameras and lidars, have imperfections in their sensing and are influenced by environmental conditions. In this paper, we present a method for probabilistic verification of linearizable systems with Gaussian and Gaussian mixture noise models (e.g. from perception modules, machine learning components). We compute the probabilities of task satisfaction under Signal Temporal Logic (STL) specifications, using its robustness semantics, with a Markov Chain Monte-Carlo slice sampler. As opposed to other techniques, our method avoids over-approximations and double-counting of failure events. Central to our approach is a method for efficient and rejection-free sampling of signals from a Gaussian distribution that satisfy or violate a given STL formula. We show illustrative examples from applications in robot motion planning.

Differentiable simulators promise faster computation time for reinforcement learning by replacing zeroth-order gradient estimates of a stochastic objective with an estimate based on first-order gradients. However, it is yet unclear what factors decide the performance of the two estimators on complex landscapes that involve long-horizon planning and control on physical systems, despite the crucial relevance of this question for the utility of differentiable simulators. We show that characteristics of certain physical systems, such as stiffness or discontinuities, may compromise the efficacy of the first-order estimator, and analyze this phenomenon through the lens of bias and variance. We additionally propose an $\alpha$-order gradient estimator, with $\alpha \in [0,1]$, which correctly utilizes exact gradients to combine the efficiency of first-order estimates with the robustness of zero-order methods. We demonstrate the pitfalls of traditional estimators and the advantages of the $\alpha$-order estimator on some numerical examples.

We believe that the future of robot motion planning will look very different than how it looks today: instead of complex collision avoidance trajectories with a brittle dependence on sensing and estimation of the environment, motion plans should consist of smooth, simple trajectories and be executed by robots that are not afraid of making contact. Here we present a “contact-aware” controller which continues to execute a given trajectory despite unexpected collisions while keeping the contact force stable and small. We introduce a quadratic programming (QP) formulation, which minimizes a trajectory-tracking error subject to quasistatic dynamics and contact-force constraints. Compared with the classical null-space projection technique, the inequality constraint on contact forces in the proposed QP controller allows for more gentle release when the robot comes out of contact. In the quasistatic dynamics model, control actions consist only of commanded joint positions, allowing the QP controller to run on stiffness-controlled robots which do not have a straightforward torque-control interface nor accurate dynamic models. The effectiveness of the proposed QP controller is demonstrated on a KUKA iiwa arm. Project video: https://youtu.be/M-7JMQRkiPk.

We propose the framework of Series Elastic End Effectors in 6D (SEED), which combines a spatially compliant element with visuotactile sensing to grasp and manipulate tools in the wild. Our framework generalizes the benefits of series elasticity to 6-dof, while providing an abstraction of control using visuotactile sensing. We propose an algorithm for relative pose estimation from visuotactile sensing, and a spatial hybrid force-position controller capable of achieving stable force interaction with the environment. We demonstrate the effectiveness of our framework on tools that require regulation of spatial forces. Video link: https://youtu.be/2-YuIfspDrk.

We introduce the first direct policy search algorithm which provably converges to the globally optimal dynamic filter for the classical problem of predicting the outputs of a linear dynamical system, given noisy, partial observations. Despite the ubiquity of partial observability in practice, theoretical guarantees for direct policy search algorithms, one of the backbones of modern reinforcement learning, have proven difficult to achieve. This is primarily due to the degeneracies which arise when optimizing over filters that maintain an internal state. In this paper, we provide a new perspective on this challenging problem based on the notion of informativity, which intuitively requires that all components of a filter’s internal state are representative of the true state of the underlying dynamical system. We show that informativity overcomes the aforementioned degeneracy. Specifically, we propose a regularizer which explicitly enforces informativity, and establish that gradient descent on this regularized objective - combined with a “reconditioning step” – converges to the globally optimal cost at a $O(1/T)$ rate.

Long-horizon manipulation tasks require joint reasoning over a sequence of discrete actions and their associated continuous control parameters. While Task and Motion Planning (TAMP) approaches are capable of generating motion plans that account for this joint reasoning, they usually assume full knowledge about the environment (e.g. in terms of shapes, poses of objects) and often require computation times not suitable for real-time control.To overcome this, we propose a learning framework where a high-level reasoning network predicts, based on an image of the scene, a sequence of discrete actions and the parameter values of their associated low-level controllers. These controllers are parameterized in terms of a learned energy function, leading to time-invariant controllers for each phase. We train the whole framework end-to-end using a dataset of TAMP solutions computed using Logic Geometric Programming. A key feature is that the reasoning network determines the parameters of the controllers jointly, such that the overall task can be solved. Despite having no explicit representation of the geometry nor pose of the objects in the scene, our network is still able to accomplish geometrically precise manipulation tasks, including handovers and an accurate pointing task where the parameters of early actions are tightly coupled with those of later actions. Video: https://youtu.be/AcPWRTkr3_g

While traditional approaches to manipulation planning assume known object templates, recent approaches to "category-level manipulation" aim to manipulate a category of objects with potentially unknown instances and large intra-category shape variation. In this paper we explore an object representation to enable precise category-level manipulation, capturing a notion of the object configuration and extent, while being generalizable to novel instances. Building on our previous work, kPAM1, we combine semantic keypoints with dense geometry (a point cloud or mesh) as the interface between the perception module and motion planner. Leveraging advances in learning-based keypoint detection and shape completion, both dense geometry and keypoints can be perceived from raw sensor input. Using the proposed hybrid object representation, we formulate the manipulation task as a motion planning problem which encodes both the object target configuration and physical feasibility for a category of objects. In this way, many existing manipulation planners can be generalized to categories of objects, and the resulting perception-to-action manipulation pipeline is robust to large intra-category shape variation. Extensive hardware experiments demonstrate our pipeline can produce robot trajectories that accomplish tasks with never-before-seen objects. The video demo is available on this link: https://sites.google.com/view/generalizable-manipulation.

Motion planning for robotic manipulation makes heavy use of quasistatic models, but these same models have not yet proven useful for simulation. This is because in many multi-contact situations, the quasistatic models do not describe a unique next state for the system. A planner is able to use these models optimistically (checking only for feasibility of a motion), but simulation requires more.In this work, we enable quasistatic models to uniquely determine contact forces by modeling actuated robots as impedances instead of prescribed motions. Using this model with a well-known convex relaxation for Coulomb friction, time-stepping of quasistatic models can be formulated as a convex Quadratic Program (QP). This convex relaxation does admit mild non-physical behavior between relatively-sliding objects, but through simulations of various complexity, we show that the proposed quasistatic time-stepping scheme generates mostly physically-realistic behaviors, and scales well with the complexity of the simulated systems.

The ability to detect and estimate external contacts is essential for robot arms to operate in unstructured environments occupied by humans. However, most robot arms are not equipped with adequate sensors to detect contacts on their entire body. What many robot arms do have is torque sensors for individual joints. Through a quantitative analysis, we argue that it is fairly likely for two distinct contacts on the robot’s surface to generate almost identical joint torque measurements. When this happens, the best contact estimate achievable is the set of possible contact positions, all of which would reproduce the measured joint torque. Searching for elements of this set is equivalent to solving to global optimality a nonlinear program.By combining rejection sampling with gradient descent, we propose a contact estimation method which in practice finds all local optima of the nonlinear program at real-time rates. In addition, we propose an active contact exploration method which falsifies spurious contact estimates in the set of local optima by making small motions around the robot’s current configuration. The proposed methods highlight the caveats of contact estimation from only joint torque, which, coupled with known limitations of such estimators, suggest that a more capable sensor is probably needed for robust whole-body contact estimation.

We propose a method to control linear time-varying (LTV) discrete-time systems subject to bounded process disturbances and measurable outputs with bounded noise, and polyhedral constraints over system inputs and states. We search over control policies that map the history of measurable outputs to the current control input. We solve the problem in two stages. First, using the original system, we build a linear system that predicts future observations using the past observations. The bounded errors are characterized using zonotopes. Next, we propose control laws based on affine maps of such output prediction errors, and show that controllers can be synthesized using convex linear/quadratic programs. Furthermore, we can add constraints on trajectories and guarantee their satisfaction for all allowable sequences of observation noise and process disturbances. Our method does not require any assumptions about system controllability and observability. The controller design does not directly take into account the state-space dynamics, and its implementation does not require an observer. Instead, partial observability is often sufficient to design a correct controller. We provide the polytopic representation of observability errors and reachable sets in the form of zonotopes. Illustrative examples are included.

Balancing performance and safety is crucial to deploying autonomous vehicles in multi-agent environments. In particular, autonomous racing is a domain that penalizes safe but conservative policies, highlighting the need for robust, adaptive strategies. Current approaches either make simplifying assumptions about other agents or lack robust mechanisms for online adaptation. This work makes algorithmic contributions to both challenges. First, to generate a realistic, diverse set of opponents, we develop a novel method for self-play based on replica-exchange Markov chain Monte Carlo. Second, we propose a distributionally robust bandit optimization procedure that adaptively adjusts risk aversion relative to uncertainty in beliefs about opponents’ behaviors. We rigorously quantify the tradeoffs in performance and robustness when approximating these computations in real-time motion-planning, and we demonstrate our methods experimentally on autonomous vehicles that achieve scaled speeds comparable to Formula One racecars.

We propose a model-based approach to design feedback policies for dexterous robotic manipulation. The manipulation problem is formulated as reaching the target region from an initial state for some non-smooth nonlinear system. First, we use trajectory optimization to find a feasible trajectory. Next, we characterize the local multi-contact dynamics around the trajectory as a piecewise affine system, and build a funnel around the linearization of the nominal trajectory using polytopes. We prove that the feedback controller at the vicinity of the linearization is guaranteed to drive the nonlinear system to the target region. During online execution, we solve linear programs to track the system trajectory. We validate the algorithm on hardware, showing that even under large external disturbances, the controller is able to accomplish the task.

In order to understand how a robot will perform in the open world, we aim to establish a quantitative understanding of the distribution of environments that a robot will face when when it is deployed. However, even restricting attention only to the distribution of objects in a scene, these distributions over environments are nontrivial: they describe mixtures of discrete and continuous variables related to the number, type, poses, and attributes of objects in the scene. We describe a probabilistic generative model that uses scene trees to capture hierarchical relationships between collections of objects, as well as a variational inference algorithm for tuning that model to best match a set of observed environments without any need for tediously labeled parse trees. We demonstrate that this model can accurately capture the distribution of a pair of nontrivial manipulation-relevant datasets and be deployed as a density estimator and outlier detector for novel environments.

We introduce R3T, a reachability-based variant of the rapidly-exploring random tree (RRT) algorithm that is suitable for (optimal) kinodynamic planning in nonlinear and hybrid systems. We developed tools to approximate reachable sets using polytopes and perform sampling-based planning with them. This method has a unique advantage in hybrid systems: different dynamic modes in the reachable set can be explicitly represented using multiple polytopes. We prove that under mild assumptions, R3T is probabilistically complete in kinodynamic systems, and asymptotically optimal through rewiring. Moreover, R3T provides a formal verification method for reachability analysis of nonlinear systems. The advantages of R3T are demonstrated with case studies on nonlinear, hybrid, and contact-rich robotic systems.

Manipulation in cluttered environments like homes requires stable grasps, precise placement and robustness against external contact. Towards addressing these challenges, we present the Soft-bubble gripper system that combines highly compliant gripping surfaces with dense-geometry visuotactile sensing and facilitates multiple kinds of tactile perception. We first present several mechanical design advances on the Soft-bubble sensors including a fabrication technique to deposit custom patterns to the internal surface of the sensor membrane that enables tracking of shear-induced displacement of the grasped object. The depth maps output by the internal imaging sensor are used in an in-hand proximity pose estimation framework - the method better captures distances to corners or edges on the object geometry. We also extend our previous work on tactile classification and integrate the system within a robust manipulation pipeline for cluttered home environments. The capabilities of the proposed system are demonstrated through robust execution of multiple real-world manipulation tasks.

Learning-based methodologies increasingly find applications in safety-critical domains like autonomous driving and medical robotics. Due to the rare nature of dangerous events, real-world testing is prohibitively expensive and unscalable. In this work, we employ a probabilistic approach to safety evaluation in simulation, where we are concerned with computing the probability of dangerous events. We develop a novel rare-event simulation method that combines exploration, exploitation, and optimization techniques to find failure modes and estimate their rate of occurrence. We provide rigorous guarantees for the performance of our method in terms of both statistical and computational efficiency. Finally, we demonstrate the efficacy of our approach on a variety of scenarios, illustrating its usefulness as a tool for rapid sensitivity analysis and model comparison that are essential to developing and testing safety-critical autonomous systems.

Probabilistic point-set registration methods have been gaining more attention for their robustness to noise, outliers and occlusions. However, these methods tend to be much slower than the popular iterative closest point (ICP) algorithms, which severely limits their usability. In this paper, we contribute a novel probabilistic registration method that achieves state-of-the-art robustness as well as substantially faster computational performance than modern ICP implementations. This is achieved using a rigorous yet computationally-efficient probabilistic formulation. Point-set registration is cast as a maximum likelihood estimation and solved using the EM algorithm. We show that with a simple augmentation, the E step can be formulated as a filtering problem, allowing us to leverage advances in efficient Gaussian filtering methods. We also propose a customized permutohedral filter to improve its performance while retaining sufficient accuracy for our task. Additionally, we present a simple and efficient twist parameterization that generalizes our method to the registration of articulated and deformable objects. For articulated objects, the complexity of our method is almost independent of the Degrees Of Freedom (DOFs), which makes it highly efficient even for high DOF systems. The results demonstrate the proposed method consistently outperforms many competitive baselines on a variety of registration tasks.

Humans perceive the world using multi-modal sensory inputs such as vision, audition, and touch. In this work, we investigate the cross-modal connection between vision and touch. The main challenge in this cross-domain modeling task lies in the significant scale discrepancy between the two: while our eyes perceive an entire visual scene at once, humans can only feel a small region of an object at any given moment. To connect vision and touch, we introduce new tasks of synthesizing plausible tactile signals from visual inputs as well as imagining how we interact with objects given tactile data as input. To accomplish our goals, we first equip robots with both visual and tactile sensors and collect a large-scale dataset of corresponding vision and tactile image sequences. To close the scale gap, we present a new conditional adversarial model that incorporates the scale and location information of the touch. Human perceptual studies demonstrate that our model can produce realistic visual images from tactile data and vice versa. Finally, we present both qualitative and quantitative experimental results regarding different system designs, as well as visualizing the learned representations of our model.

In this paper we study how to formulate the optimal control problem for a piecewise-affine dynamical system as a mixed-integer program. Problems of this form arise typically in hybrid Model Predictive Control (MPC), where at every time step an open-loop optimal control sequence is computed via numerical optimization and applied to the system in a moving horizon fashion. Not surprisingly, the efficiency in the formulation of the underlying mathematical program has a crucial influence on computation times, and hence on the applicability of hybrid MPC to high-dimensional systems.

Guided policy search is a popular approach for training controllers for high-dimensional systems, but it has a number of pitfalls. Non-convex trajectory optimization has local minima, and non-uniqueness in the optimal policy itself can mean that independently-optimized samples do not describe a coherent policy from which to train. We introduce LVIS, which circumvents the issue of local minima through global mixed-integer optimization and the issue of non-uniqueness through learning the optimal value function rather than the optimal policy. To avoid the expense of solving the mixed-integer programs to full global optimality, we instead solve them only partially, extracting intervals containing the true cost-to-go from early termination of the branch-and-bound algorithm. These interval samples are used to weakly supervise the training of a neural net which approximates the true cost-to-go. Online, we use that learned cost-to-go as the terminal cost of a one-step model-predictive controller, which we solve via a small mixed-integer optimization. We demonstrate LVIS on piecewise affine models of a cart-pole system with walls and a planar humanoid robot and show that it can be applied to a fundamentally hard problem in feedback control-control through contact.

We consider the feedback design for stabilizing a rigid body system by making and breaking multiple contacts with the environment without prespecifying the timing or the number of occurrence of the contacts. We model such a system as a discrete-time hybrid polynomial system, where the state-input space is partitioned into several polytopic regions with each region associated with a different polynomial dynamics equation. Based on the notion of occupation measures, we present a novel controller synthesis approach that solves finite-dimensional semidefinite programs as approximations to an infinite-dimensional linear program to stabilize the system. The optimization formulation is simple and convex, and for any fixed degree of approximations the computational complexity is polynomial in the state and control input dimensions. We illustrate our approach on some robotics examples.

There has been an increasing interest in learning dynamics simulators for model-based control. Compared with off-the-shelf physics engines, a learnable simulator can quickly adapt to unseen objects, scenes, and tasks. However, existing models like interaction networks only work for fully observable systems; they also only consider pairwise interactions within a single time step, both restricting their use in practical systems. We introduce Propagation Networks (PropNet), a differentiable, learnable dynamics model that handles partially observable scenarios and enables instantaneous propagation of signals beyond pairwise interactions. With these innovations, our propagation networks not only outperform current learnable physics engines in forward simulation, but also achieves superior performance on various control tasks. Compared with existing deep reinforcement learning algorithms, model-based control with propagation networks is more accurate, efficient, and generalizable to novel, partially observable scenes and tasks.

Piecewise affine (PWA) systems are widely used to model highly nonlinear behaviors such as contact dynamics in robot locomotion and manipulation. Existing control techniques for PWA systems have computational drawbacks, both in offline design and online implementation. In this paper, we introduce a method to obtain feedback control policies and a corresponding set of admissible initial conditions for discrete-time PWA systems such that all the closed-loop trajectories reach a goal polytope, while a cost function is optimized. The idea is conceptually similar to LQR-trees [1], which consists of 3 steps: (1) open-loop trajectory optimization, (2) feedback control for computation of “funnels” of states around trajectories, and (3) repeating (1) and (2) in a way that the funnels are grown backward from the goal in a tree fashion and fill the state-space as much as possible. We show PWA dynamics can be exploited to combine step (1) and (2) into a single step that is tackled using mixed-integer convex programming, which makes the method suitable for dealing with hard constraints. Illustrative examples on contact-based dynamics are presented.

Modern robotic systems are very complex and need to be tested in simulations with detailed sensor noise models to effectively verify robotic behavior. Depth imagery in particular comes with significant noise in the form of scene-dependent pixel-wise dropouts and distortions. Unfortunately, many depth camera simulations contain limited noise models, or can only support generating realistic depth images of simple scenes, which limits their usefulness in effectively testing perception algorithms. We propose a data driven approach to generate more realistic noise for complex simulated environments by using a convolutional neural network (CNN) to predict which pixels of a simulated noise-free depth image will not have returns (no-depth-return pixels, or NDP). We choose to focus on NDP here, as these dropouts are the most common and dramatic form of depth image noise. To train this network, we use reconstructed real-world scenes from the Label Fusion dataset to provide ground truth depth for each noisy depth image used to scan the scene. We use the resulting noise-free and noisy depth image pairs as labeled examples and train the network to predict which pixels of the noise-free image will be NDP. When used to post-process a simulation of a depth sensor, this system produces realistic depth images, even in cluttered scenes. To demonstrate that our approach successfully closes the reality gap for depth imagery, we show that the popular ICP algorithm for object pose estimation fails more realistically on our CNN-corrupted simulated depth images than on uncorrupted depth images and unsupervised domain adaptation baselines.

We would like robots to be able to safely navigate at high speed, efficiently use local 3D information, and robustly plan motions that consider pose uncertainty of measurements in a local map structure. This is hard to do with previously existing mapping approaches, like occupancy grids, that are focused on incrementally fusing 3D data into a common world frame. In particular, both their fragile sensitivity to state estimation errors and computational cost can be limiting. We develop an alternative framework, NanoMap, which alleviates the need for global map fusion and enables a motion planner to efficiently query pose-uncertainty-aware local 3D geometric information. The key idea of NanoMap is to store a history of noisy relative pose transforms and search over a corresponding set of depth sensor measurements for the minimum-uncertainty view of a queried point in space. This approach affords a variety of capabilities not offered by traditional mapping techniques: (a) the pose uncertainty associated with 3D data can be incorporated in motion planning, (b) poses can be updated (i.e., from loop closures) with minimal computational effort, and (c) 3D data can be fused lazily for the purpose of planning. We provide an open-source implementation of NanoMap, and analyze its capabilities and computational efficiency in simulation experiments. Finally, we demonstrate in hardware its effectiveness for fast 3D obstacle avoidance onboard a quadrotor flying up to 10 m/s.

Deep neural network (DNN) architectures have been shown to outperform traditional pipelines for object segmentation and pose estimation using RGBD data, but the performance of these DNN pipelines is directly tied to how representative the training data is of the true data. Hence a key requirement for employing these methods in practice is to have a large set of labeled data for your specific robotic manipulation task, a requirement that is not generally satisfied by existing datasets. In this paper we develop a pipeline to rapidly generate high quality RGBD data with pixelwise labels and object poses. We use an RGBD camera to collect video of a scene from multiple viewpoints and leverage existing reconstruction techniques to produce a 3D dense reconstruction. We label the 3D reconstruction using a human assisted ICP-fitting of object meshes. By reprojecting the results of labeling the 3D scene we can produce labels for each RGBD image of the scene. This pipeline enabled us to collect over 1,000,000 labeled object instances in just a few days. We use this dataset to answer questions related to how much training data is required, and of what quality the data must be, to achieve high performance from a DNN architecture. Our dataset and annotation pipeline are available at labelfusion.csail.mit.edu.

In this paper, we design nonlinear state feedback controllers for discrete-time polynomial dynamical systems via the occupation measure approach. We propose the discrete-time controlled Liouville equation, and use it to formulate the controller synthesis problem as an infinite-dimensional linear programming problem on measures, which is then relaxed as finite-dimensional semidefinite programming problems on moments of measures and their duals on sums-of-squares polynomials. Nonlinear controllers can be extracted from the solutions to the relaxed problems. The advantage of the occupation measure approach is that we solve convex problems instead of generally non-convex problems, and the computational complexity is polynomial in the state and input dimensions, and hence the approach is more scalable. In addition, we show that the approach can be applied to over-approximating the backward reachable set of discrete-time autonomous polynomial systems and the controllable set of discrete-time polynomial systems under known state feedback control laws. We illustrate our approach on several dynamical systems.

An effective scheme to simulate low-speed, contact-rich manipulation tasks is to assume quasistatic physics and advance system states by solving linear complementarity problems (LCPs). However, the existing LCP-based quasistatic time-stepping scheme fails to simulate grasping-an essential motion primitive in manipulation-due to two drawbacks specific to quasistatic systems. Firstly, inputs to quasistatic systems are velocity commands instead of torques. This can lead to penetration, and thus an infeasible LCP, when two rigid bodies in contact are commanded to push against each other. Secondly, as multiple force solutions exist for a given velocity command, a grasping velocity command is not guaranteed to generate sufficient grasping forces. In this paper, we reformulate the quasistatic time-stepping scheme as an optimization problem with complementarity constraints and a quadratic objective. By minimizing the difference between actual and commanded velocities, linearized non-penetration constraints can always be satisfied. Moreover, undesirable solutions with insufficient normal forces can be removed by considering elasticity, which is modeled by comparing actual and commanded velocities. The resulting optimization problem is a mixed-integer quadratic program, which can be solved reasonably quickly for small-to-medium-sized systems. The effectiveness of the proposed reformulation is validated by simulation results of systems with different levels of complexity.

While recent developments in autonomous vehicle (AV) technology highlight substantial progress, we lack tools for rigorous and scalable testing. Real-world testing, the de facto evaluation environment, places the public in danger, and, due to the rare nature of accidents, will require billions of miles in order to statistically validate performance claims. We implement a simulation framework that can test an entire modern autonomous driving system, including, in particular, systems that employ deep-learning perception and control algorithms. Using adaptive importance-sampling methods to accelerate rare-event probability evaluation, we estimate the probability of an accident under a base distribution governing standard traffic behavior. We demonstrate our framework on a highway scenario, accelerating system evaluation by 2-20 times over naive Monte Carlo sampling methods and 10-300P times (where P is the number of processors) over real-world testing.

Humanoid robots walking across intermittent terrain, robotic arms grasping multifaceted objects, UAVs darting left or right around a tree, or autonomous vehicles making discrete navigation decisions in traffic, many of the dynamics and control problems we face today have both rich nonlinear dynamics and an inherently combinatorial structure. In this talk, I'll review some recent work on optimization-based planning and control methods which address these two challenges simultaneously. All of these can be modeled as hybrid systems, but in some cases more efficient optimizations are possible by using alternative formulations, such as (measure-) differential inclusions. I'll present our explorations with mixed-integer convex-, semidefinite-programming-relaxations, and satisfiability-modulo- theory(SMT)-based methods applied to hard problems in legged locomotion over rough terrain, grasp optimization, and UAVs flying through highly cluttered environments.

We present an object-tracking framework that fuses point cloud information from an RGB-D camera with tactile information from a GelSight contact sensor. GelSight can be treated as a source of dense local geometric information, which we incorporate directly into a conventional point-cloud-based articulated object tracker based on signed-distance functions. Our implementation runs at 12 Hz using an online depth reconstruction algorithm for GelSight and a modified second-order update for the tracking algorithm. We present data from hardware experiments demonstrating that the addition of contact-based geometric information significantly improves the pose accuracy during contact, and provides robustness to occlusions of small objects by the robot's end effector.

We present parametric trajectory optimization, a method for simultaneously computing physical parameters, actuation requirements, and robot motions for more efficient robot designs. In this scheme, robot dimensions, masses, and other physical parameters are solved for concurrently with traditional motion planning variables, including dynamically consistent robot states, actuation inputs, and contact forces. Our method requires minimal user domain knowledge, requiring only a coarse guess of the target robot configuration sequence and a parameterized robot topology as input. We demonstrate our results on four simulated robots, one of which we physically fabricated in order to demonstrate physical consistency. We demonstrate that by optimizing robot body parameters alongside robot trajectories, motion planning problems which would otherwise be infeasible can be made feasible, and actuation requirements can be significantly reduced.

We present the development of a full-scale “parallel autonomy” research platform including software and hardware. In the parallel autonomy paradigm, the control of the vehicle is shared; the human is still in control of the vehicle, but the autonomy system is always running in the background to prevent accidents. Our holistic approach includes: (1) a drive-by-wire conversion method only based on reverse engineering mounting of relatively inexpensive sensors onto the vehicle implementation of a localization and mapping system, (4) obstacle detection and (5) a shared controller as well as (6) integration with an advanced autonomy simulation system (Drake) for rapid development and testing. The system can operate in three modes: (a) manual driving, (b) full autonomy, where the system is in complete control of the vehicle and (c) parallel autonomy, where the shared controller is implemented. We present results from extensive testing of a full-scale vehicle on closed tracks that demonstrate these capabilities.

Quadrotor flight has typically been limited to sparse environments due to numerical complications that arise when dealing with large numbers of obstacles. We hypothesized that it would be possible to plan and robustly execute trajectories in obstacle-dense environments using the novel Iterative Regional Inflation by Semidefinite programming algorithm (IRIS), mixed-integer semidefinite programs (MISDP), and model-based control. Unlike sampling-based approaches, the planning algorithm first introduced by Deits theoretically guarantees non-penetration of the trajectories even with small obstacles such as strings. We present experimental validation of this claim by aggressively flying a small quadrotor (34g, 92mm rotor to rotor) in a series of indoor environments including a cubic meter volume containing 20 interwoven strings, and present the control architecture we developed to do so.

Contact constraints, such as those between a foot and the ground or a hand and an object, are inherent in many robotic tasks. These constraints define a manifold of feasible states; while well understood mathematically, they pose numerical challenges to many algorithms for planning and controlling whole-body dynamic motions. In this paper, we present an approach to the synthesis and stabilization of complex trajectories for both fully-actuated and underactuated robots subject to contact constraints. We introduce a trajectory optimization algorithm (DIRCON) that extends the direct collocation method, naturally incorporating manifold constraints to produce a nominal trajectory with third-order integration accuracy-a critical feature for achieving reliable tracking control. We adapt the classical time-varying linear quadratic regulator to produce a local cost-to-go in the manifold tangent plane. Finally, we descend the cost-to-go using a quadratic program that incorporates unilateral friction and torque constraints. This approach is demonstrated on three complex walking and climbing locomotion examples in simulation.

In order for robots to interact safely and intelligently with their environment they must be able to reliably estimate and localize external contacts. This paper introduces CPF, the Contact Particle Filter, which is a general algorithm for detecting and localizing external contacts on rigid body robots without the need for external sensing. CPF finds external contact points that best explain the observed external joint torque, and returns sensible estimates even when the external torque measurement is corrupted with noise. We demonstrate the capability of the CPF to track multiple external contacts on a simulated Atlas robot, and compare our work to existing approaches.

We present a novel stereo vision algorithm that is capable of obstacle detection on a mobile ARM processor at 120 frames per second. Our system performs a subset of standard block-matching stereo processing, searching only for obstacles at a single depth. By using an onboard IMU and state-estimator, we can recover the position of obstacles at all other depths, building and updating a local depth-map at framerate. Here, we describe both the algorithm and our implementation on a high-speed, small UAV, flying at over 20 MPH (9 m/s) close to obstacles. The system requires no external sensing or computation and is, to the best of our knowledge, the first high-framerate stereo detection system running onboard a small UAV.

We present a new approach to the design of smooth trajectories for quadrotor unmanned aerial vehicles (UAVs), which are free of collisions with obstacles along their entire length. To avoid the non-convex constraints normally required for obstacle-avoidance, we perform a mixed-integer optimization in which polynomial trajectories are assigned to convex regions which are known to be obstacle-free. Prior approaches have used the faces of the obstacles themselves to define these convex regions. We instead use IRIS, a recently developed technique for greedy convex segmentation [1], to pre-compute convex regions of safe space. This results in a substantially reduced number of integer variables, which improves the speed with which the optimization can be solved to its global optimum, even for tens or hundreds of obstacle faces. In addition, prior approaches have typically enforced obstacle avoidance at a finite set of sample or knot points. We introduce a technique based on sums-of-squares (SOS) programming that allows us to ensure that the entire piecewise polynomial trajectory is free of collisions using convex constraints. We demonstrate this technique in 2D and in 3D using a dynamical model in the Drake toolbox for Matlab [2].

The goal of this work is to develop a soft robotic manipulation system that is capable of autonomous, dynamic, and safe interactions with humans and its environment. First, we develop a dynamic model for a multi-body fluidic elastomer manipulator that is composed entirely from soft rubber and subject to the self-loading effects of gravity. Then, we present a strategy for independently identifying all unknown components of the system: the soft manipulator, its distributed fluidic elastomer actuators, as well as drive cylinders that supply fluid energy. Next, using this model and trajectory optimization techniques we find locally optimal open-loop policies that allow the system to perform dynamic maneuvers we call grabs. In 37 experimental trials with a physical prototype, we successfully perform a grab 92% of the time. By studying such an extreme example of a soft robot, we can begin to solve hard problems inhibiting the mainstream use of soft machines.

Avian flight far exceeds our best aircraft control systems. We have conducted a series of experiments at the Concord Field Station demonstrating the extraordinary maneuverability of the common pigeon, showing it darting through tight spaces and recovering from large disturbances with ease. Our goal is to understand how to make small fixed-wing aircraft achieve similar feats in equally challenging environments.

We describe a whole-body dynamic walking controller implemented as a convex quadratic program. The controller solves an optimal control problem using an approximate value function derived from a simple walking model while respecting the dynamic, input, and contact constraints of the full robot dynamics. By exploiting sparsity and temporal structure in the optimization with a custom active-set algorithm, we surpass the performance of the best available off-the-shelf solvers and achieve 1kHz control rates for a 34-DOF humanoid. We describe applications to balancing and walking tasks using the simulated Atlas robot in the DARPA Virtual Robotics Challenge.

The paper describes the system developed by researchers from MIT for the Defense Advanced Research Projects Agency's (DARPA) Virtual Robotics Challenge (VRC), held in June 2013. The VRC was the first competition in the DARPA Robotics Challenge (DRC), a program that aims to “develop ground robotic capabilities to execute complex tasks in dangerous, degraded, human-engineered environments”. The VRC required teams to guide a model of Boston Dynamics' humanoid robot, Atlas, through driving, walking, and manipulation tasks in simulation. Team MIT's user interface, the Viewer, provided the operator with a unified representation of all available information. A 3D rendering of the robot depicted its most recently estimated body state with respect to the surrounding environment, represented by point clouds and texture-mapped meshes as sensed by on-board LIDAR and fused over time.

Many critical tasks in robotics, such as locomotion or manipulation, involve collisions between a rigid body and the environment or between multiple bodies. Sums-of-squares (SOS) based methods for numerical computation of Lyapunov certificates are a powerful tool for analyzing the stability of continuous nonlinear systems, which can play a powerful role in motion planning and control design. Here, we present a method for applying sums-of-squares verification to rigid bodies with Coulomb friction undergoing discontinuous, inelastic impact events. The proposed algorithm explicitly generates Lyapunov certificates for stability, positive invariance, and reachability over admissible (non-penetrating) states and contact forces. We leverage the complementarity formulation of contact, which naturally generates the semialgebraic constraints that define this admissible region. The approach is demonstrated on multiple robotics examples, including simple models of a walking robot and a perching aircraft.

In this paper we seek to quantify and explicitly optimize the robustness of a control system for a robot walking on terrain with uncertain geometry. Geometric perturbations to the terrain enter the equations of motion through a relocation of the hybrid event “guards” which trigger an impact event; these perturbations can have a large effect on the stability of the robot and do not fit into the traditional robust control analysis and design methodologies without additional machinery. We attempt to provide that machinery here. In particular, we quantify the robustness of the system to terrain perturbations by defining an L2gain from terrain perturbations to deviations from the nominal limit cycle. We show that the solution to a periodic dissipation inequality provides a sufficient upper bound on this gain for a linear approximation of the dynamics around the limit cycle, and we formulate a semidefinite programming problem to compute the L2gain for the system with a fixed linear controller. We then use either binary search or an iterative optimization method to construct a linear robust controller and to minimize the L2gain. The simulation results on canonical robots suggest that the L2gain is closely correlated to the actual number of steps traversed on the rough terrain, and our controller can improve the robot's robustness to terrain disturbances.

Motivated by the need for formal guarantees on the stability and safety of controllers for challenging robot control tasks, we present a control design procedure that explicitly seeks to maximize the size of an invariant “funnel” that leads to a predefined goal set. Our certificates of invariance are given in terms of sums of squares proofs of a set of appropriately defined Lyapunov inequalities. These certificates, together with our proposed polynomial controllers, can be efficiently obtained via semidefinite optimization. Our approach can handle time-varying dynamics resulting from tracking a given trajectory, input saturations (e.g. torque limits), and can be extended to deal with uncertainty in the dynamics and state. The resulting controllers can be used by space-filling feedback motion planning algorithms to fill up the space with significantly fewer trajectories. We demonstrate our approach on a severely torque limited underactuated double pendulum (Acrobot) and provide extensive simulation and hardware validation.

Unmanned aerial vehicles (UAVs) have a so-far untapped potential to operate at high speeds through cluttered environments. Many of these systems are limited by their adhoc reactive controllers using simple visual cues like optical flow. Here we consider the problem of formally verifying an output-feedback controller for an aircraft operating in an unknown environment. Using recent advances in sums-of-squares programming that allow for efficient computation of barrier functions, we search for global certificates of safety for the closed-loop system in a given environment. In contrast to previous work, we use rational functions to globally approximate non-smooth dynamics and use multiple barrier functions to guard against more than one obstacle. We expect that these formal verification techniques will allow for the comparison, and ultimately optimization, of reactive controllers for robustness to varying initial conditions and environments.

Dynamic perching maneuvers for fixed-wing aircraft are becoming increasingly plausible due to recent progress in perching using `micro-spines' mounted on tuned suspensions and, separately, on feedback motion planning techniques for post-stall maneuvering. In this paper, we bring these complementary techniques together by efficiently estimating the mechanical stability of the plane when it makes contact with a vertical surface; the resulting landing funnel can then be used in a feedback motion planning algorithm for the flight controller. We consider a simplified model of the perching dynamics and report an extension of the region of attraction techniques, using sums-of-squares optimization, which combines polynomial approximations of barrier constraints with the traditional Lyapunov methods to achieve tight estimation of the true region of attraction for the model. We demonstrate the new method on a variety of design parameters for the perching system, suggesting a potential use as a mechanical system or controller design tool.

We consider the partially observable control problem where it is potentially necessary to perform complex information-gathering operations in order to localize state. One approach to solving these problems is to create plans in belief-space, the space of probability distributions over the underlying state of the system. The belief-space plan encodes a strategy for performing a task while gaining information as necessary. Unlike most approaches in the literature which rely upon representing belief state as a Gaussian distribution, we have recently proposed an approach to non-Gaussian belief space planning based on solving a non-linear optimization problem defined in terms of a set of state samples [1]. In this paper, we show that even though our approach makes optimistic assumptions about the content of future observations for planning purposes, all low-cost plans are guaranteed to gain information in a specific way under certain conditions. We show that eventually, the algorithm is guaranteed to localize the true state of the system and to reach a goal region with high probability. Although the computational complexity of the algorithm is dominated by the number of samples used to define the optimization problem, our convergence guarantee holds with as few as two samples. Moreover, we show empirically that it is unnecessary to use large numbers of samples in order to obtain good performance.

Reinforcement Learning offers a very general framework for learning controllers, but its effectiveness is closely tied to the controller parameterization used. Especially when learning feedback controllers for weakly stable systems, ineffective parameterizations can result in unstable controllers and poor performance both in terms of learning convergence and in the cost of the resulting policy. In this paper we explore four linear controller parameterizations in the context of REINFORCE, applying them to the control of a reaching task with a linearized flexible manipulator. We find that some natural but naive parameterizations perform very poorly, while the Youla Parameterization (a popular parameterization from the controls literature) offers a number of robustness and performance advantages.

Perching on powerlines to recharge provides a unique opportunity to extend the mission duration capabilities of small-scale UAVs (Unmanned Aerial Vehicles). In this paper, we investigate the feasibility of localizing an aircraft using the magnetic field generated by a current carrying wire through state estimation and hardware development. By using an Extended Kalman Filter to track the real and imaginary components of the magnetic field signal, we overcome the problems posed by the field's phase-amplitude ambiguity and demonstrate the ability to track an aircraft flying at speeds up to 8 m/s at a distance of 4 meters from the wire. We conclude that the achieved performance is adequate for controlling a bird-scale UAV in a dynamic perching maneuver and that our system would generalize to real world scenarios.

Kinodynamic planning algorithms like Rapidly-Exploring Randomized Trees (RRTs) hold the promise of finding feasible trajectories for rich dynamical systems with complex, nonconvex constraints. In practice, these algorithms perform very well on configuration space planning, but struggle to grow efficiently in systems with dynamics or differential constraints. This is due in part to the fact that the conventional distance metric, Euclidean distance, does not take into account system dynamics and constraints when identifying which node in the existing tree is capable of producing children closest to a given point in state space. We show that an affine quadratic regulator (AQR) design can be used to approximate the exact minimum-time distance pseudometric at a reasonable computational cost. We demonstrate improved exploration of the state spaces of the double integrator and simple pendulum when using this pseudometric within the RRT framework, but this improvement drops off as systems' nonlinearity and complexity increase. Future work includes exploring methods for approximating the exact minimum-time distance pseudometric that can reason about dynamics with higher-order terms.

We present an algorithm that probabilistically covers a bounded region of the state space of a nonlinear system with a sparse tree of feedback stabilized trajectories leading to a goal state. The generated tree serves as a lookup table control policy to get any reachable initial condition within that region to the goal. The approach combines motion planning with reasoning about the set of states around a trajectory for which the feedback policy of the trajectory is able to stabilize the system. The key idea is to use a random sample from the bounded region for both motion planning and approximation of the stabilizable sets by falsification; this keeps the number of samples and simulations needed to generate covering policies reasonably low. We simulate the nonlinear system to falsify the stabilizable sets, which allows enforcing input and state constraints. Compared to the algebraic verification using sums of squares optimization in our previous work, the simulation-based approximation of the stabilizable set is less exact, but considerably easier to implement and can be applied to a broader range of nonlinear systems. We show simulation results obtained with model systems and study the performance and robustness of the generated policies.

In this video, we demonstrate the effectiveness of a kinodynamic planning strategy that allows a high-impedance quadruped to operate across a variety of rough terrain. At one extreme, the robot can achieve precise foothold selection on intermittent terrain. More surprisingly, the same inherently-stiff robot can also execute highly dynamic and underactuated motions with high repeatability. This range of dynamic motion is possible through careful reasoning about the coupled dynamics during underactuated phases of motion. Our results demonstrate visceral progress toward realization of one of the central theoretically claims giving legged locomotion a “leg-up” over wheeled robotics: that appropriate design of control can produce a set of capabilities which span a dynamic range from deliberate foothold selection through acrobatic-style motion on a single, particular robot.

Although there has been an increasing interest in dynamic bipedal locomotion for significant improvement of energy efficiency and dexterity of mobile robots in the real world, their locomotion capabilities are still mostly restricted on flat surfaces. The difficulty of dynamic locomotion in rough terrain is mainly originated in the stability and controllability of gait patterns while exploiting the natural mechanical dynamics of the robots. For a systematic investigation of the challenging problem, this paper presents the simplest control architecture for the compass gait model which can be used for locomotion in rough terrain. Locomotion of the model is mainly achieved by an open-loop oscillator which induces self-stabilizing gait patterns, and we test the proposed control architecture in a real-world robotic platform. In addition, we also found that this controller is capable of varying stride length with a minimum change of control parameters, which enables locomotion in rough terrains. By using these basic principles of self-stability and gait variability, we extended the proposed controller with a simple sensory feedback about the location in the environment, which makes the robot possible to control gait patterns autonomously for traversing a rough terrain. We describe a set of experimental results and discuss how the proposed minimalistic control architecture can be enhanced for dynamic locomotion control in more complex environment.

The reduction of the kinematics and/or dynamics of a high-DOF robotic manipulator to a low-dimension ldquotask spacerdquo has proven to be an invaluable tool for designing feedback controllers. When obstacles or other kinodynamic constraints complicate the feedback design process, motion planning techniques can often still find feasible paths, but these techniques are typically implemented in the high-dimensional configuration (or state) space. Here we argue that providing a Voronoi bias in the task space can dramatically improve the performance of randomized motion planners, while still avoiding non-trivial constraints in the configuration (or state) space. We demonstrate the potential of task-space search by planning collision-free trajectories for a 1500 link arm through obstacles to reach a desired end-effector position.

Rapidly-exploring random trees (RRTs) are widely used to solve large planning problems where the scope prohibits the feasibility of deterministic solvers, but the efficiency of these algorithms can be severely compromised in the presence of certain kinodynamics constraints. Obstacle fields with tunnels, or tubes are notoriously difficult, as are systems with differential constraints, because the tree grows inefficiently at the boundaries. Here we present a new sampling strategy for the RRT algorithm, based on an estimated feasibility set, which affords a dramatic improvement in performance in these severely constrained systems. We demonstrate the algorithm with a detailed look at the expansion of an RRT in a swing up task, and on path planning for a nonholonomic car.

In this paper, we explore the capabilities of actuated models of the compass gait walker on rough terrain. We solve for the optimal high-level feedback policy to negotiate a perfectly known but qualitatively complex terrain, using a fixed low-level controller which selects a high-level action once- per-step. We also demonstrate that a one-step time horizon control strategy using the same low-level controller can provide performance which is surprisingly comparable to that of the infinite time horizon optimal policy. The model presented here uses a torque at the hip and an axially-directed impulsive toe-off applied just before each ground collision. Our results provide compelling evidence that actuated robots based on passive dynamic principles (e.g. no ankle torque) should inherently be capable of walking on significantly rough terrain.

Kinodynamic planning algorithms have the potential to find feasible control trajectories which accomplish a task even in very nonlinear or constrained dynamical systems. Underactuation represents a particular form of a dynamic constraint, inherently present in many machines of interest (e.g., walking robots), and necessitates planning for long-term control solutions. A major limitation in motion planning techniques, especially for real-time implementation, is that they are only practical for relatively low degree-of-freedom problems. Here we present a model-based dimensionality reduction technique based on an extension of partial feedback linearization control into a task-space framework. This allows one to plan motions for a complex underactuated robot directly in a low-dimensional task-space, and to resolve redundancy with lower-priority tasks. We illustrate the potential of this approach with an extremely simple motion planning system which solves the swing-up problem for multi-link underactuated pendula, and discuss extensions to the control of walking.

Policy gradient (PG) reinforcement learning algorithms have strong (local) convergence guarantees, but their learning performance is typically limited by a large variance in the estimate of the gradient. In this paper, we formulate the variance reduction problem by describing a signal-to-noise ratio (SNR) for policy gradient algorithms, and evaluate this SNR carefully for the popular Weight Perturbation (WP) algorithm. We confirm that SNR is a good predictor of long-term learning performance, and that in our episodic formulation, the cost-to-go function is indeed the optimal baseline. We then propose two modifications to traditional model-free policy gradient algorithms in order to optimize the SNR. First, we examine WP using anisotropic sampling distributions, which introduces a bias into the update but increases the SNR; this bias can be interpretted as following the natural gradient of the cost function. Second, we show that non-Gaussian distributions can also increase the SNR, and argue that the optimal isotropic distribution is a ‘shell’ distribution with a constant magnitude and uniform distribution in direction. We demonstrate that both modifications produce substantial improvements in learning performance in challenging policy gradient experiments.

In this work we examine the control of center of mass and swing leg trajectories in LittleDog, a point-foot quadruped robot. It is not clear how to formulate a function to compute forward kinematics of the center of mass of the robot as a function of actuated joint angles because point-foot walkers have no direct actuation between the feet and the ground. Nevertheless, we show that a whole-body Jacobian exists and is well defined when at least three of the feet are on the ground. Also, the typical approach of work-space centering for redundancy resolution causes destabilizing motions when executing fast motions. An alternative redundancy resolution optimization is proposed which projects single-leg inverse kinematic solutions into the nullspace. This hybrid approach seems to minimize 1) unnecessary rotation of the body, 2) twisting of the stance legs, and 3) whole-body involvement in achieving a step leg trajectory. In simulation, this control allows the robot to perform significantly more dynamic behaviors while maintaining stability.

We propose a fast approach for detecting collision- free swing-foot trajectories for legged locomotion over extreme terrains. Instead of simulating the swing trajectories and checking for collisions along them, our approach uses machine learning techniques to predict whether a swing trajectory is collision-free. Using a set of local terrain features, we apply supervised learning to train a classifier to predict collisions. Both in simulation and on a real quadruped platform, our results show that our classifiers can improve the accuracy of collision detection compared to a real-time geometric approach without significantly increasing the computation time.

While underactuated robotic systems are capable of energy efficient and rapid dynamic behavior, we still do not fully understand how body dynamics can be actively used for adaptive behavior in complex unstructured environment. In particular, we can expect that the robotic systems could achieve high maneuverability by flexibly storing and releasing energy through the motor control of the physical interaction between the body and the environment. This paper presents a minimalistic optimization strategy of motor control policy for underactuated legged robotic systems. Based on a reinforcement learning algorithm, we propose an optimization scheme, with which the robot can exploit passive elasticity for hopping forward while maintaining the stability of locomotion process in the environment with a series of large changes of ground surface. We show a case study of a simple one-legged robot which consists of a servomotor and a passive elastic joint. The dynamics and learning performance of the robot model are tested in simulation, and then transferred the results to the real-world robot.

The passive dynamic walker described in this paper is a robot with a minimal number of degrees of freedom which is still capable of stable 3D dynamic walking. First, we present the reduced-order dynamic models used to tune the characteristics of the robot's passive gait. Our sagittal plane model is closely related to the compass gait model, but the steady state trajectory passively converges from a much larger range of initial conditions. We then experimentally quantify the stability of the mechanical device. Finally, we present an actuated version of the robot and some preliminary active control strategies. The control problem for the actuated version of the robot is interesting because although it is theoretically challenging (4 degrees of under-actuation), the mechanical design of the robot made it relatively easy to create controllers which allowed the robot to walk stably on flat terrain and even up a small slope.

We present a learning system which is able to quickly and reliably acquire a robust feedback control policy for 3D dynamic walking from a blank-slate using only trials implemented on our physical robot. The robot begins walking within a minute and learning converges in approximately 20 minutes. This success can be attributed to the mechanics of our robot, which are modeled after a passive dynamic walker, and to a dramatic reduction in the dimensionality of the learning problem. We reduce the dimensionality by designing a robot with only 6 internal degrees of freedom and 4 actuators, by decomposing the control system in the frontal and sagittal planes, and by formulating the learning problem on the discrete return map dynamics. We apply a stochastic policy gradient algorithm to this reduced problem and decrease the variance of the update using a state-based estimate of the expected cost. This optimized learning system works quickly enough that the robot is able to continually adapt to the terrain as it walks.