We revisit the concept of constraint embedding as a means for dealing with kinematic loop constraints during dynamics computations for rigid-body systems. Specifically, we consider the local loop constraints emerging from common actuation sub-mechanisms in modern robotics systems (e.g., geared motors, differential drives, and four-bar mechanisms). As a complementary perspective to prior work on constraint embedding, we present an analysis that generalizes the traditional concepts of joint models and motion/force subspaces between individual rigid bodies to generalized joint models and motion/force subspaces between groups of rigid bodies subject to loop constraints. We then use these generalized concepts to derive the constraint-embedded recursive forward dynamics algorithm using multi-handle articulated bodies. We demonstrate the broad applicability of the generalized joint concepts by showing how they also lead to the constraint-embedding-based recursive algorithm for inverse dynamics. Lastly, we benchmark our open-source implementation in C++ for the forward dynamics algorithm against state-of-the-art, sparsity-exploiting algorithms. Our alternative derivation is intended to make the constraint embedding methodology more accessible to the broader robotics community, while the benchmarking study clarifies the relative strengths and limitations of constraint embedding versus sparsity-exploiting methods. Indeed, our benchmarking validates that constraint embedding outperforms the non-recursive alternative in cases involving local kinematic loops.

We present a homotopic approach to solving challenging, optimization-based motion planning problems. The approach uses Homotopy Optimization, which, unlike standard continuation methods for solving homotopy problems, solves a sequence of constrained optimization problems rather than a sequence of nonlinear systems of equations. The insight behind our proposed algorithm is formulating the discovery of this sequence of optimization problems as a search problem in a multidimensional homotopy parameter space. Our proposed algorithm, the Probabilistic Homotopy Optimization algorithm, switches between solve and sample phases, using solutions to easy problems as initial guesses to more challenging problems. We analyze how our algorithm performs in the presence of common challenges to homotopy methods, such as bifurcation, folding, and disconnectedness of the homotopy solution manifold. Finally, we demonstrate its utility via a case study on two dynamic motion planning problems. the cart-pole and the MIT Humanoid.

Dynamic jumping with legged robots poses a challenging problem in planning and control. Formulating the jump optimization to allow fast online execution is difficult; efficiently using this capability to generate long-horizon motion plans further complicates the problem. In this work, we present a hierarchical planning framework to address this problem. We first formulate a real-time tractable trajectory optimization for performing omnidirectional jumping. We then embed the results of this optimization into a low dimensional jump feasibility classifier. This classifier is leveraged to produce geometric motion plans that select dynamically feasible jumps while mitigating the effects of the process noise. We deploy our framework on the Mini Cheetah Vision quadruped, demonstrating the robot's ability to generate and execute reliable, goal-oriented plans that involve forward, lateral, and rotational jumps onto surfaces as tall as the robot's nominal hip height. The ability to plan through omnidirectional jumping greatly expands the robot's mobility relative to planners that restrict jumping to the sagittal or frontal planes.

This work presents a two part framework for online planning and execution of dynamic aerial motions on a quadruped robot. Motions are planned via a centroidal momentum-based nonlinear optimization that is general enough to produce rich sets of novel dynamic motions based solely on the user-specified contact schedule and desired launch velocity of the robot. Since this nonlinear optimization is not tractable for real-time receding horizon control, motions are planned once via nonlinear optimization in preparation of an aerial motion and then tracked continuously using a variational-based optimal controller that offers robustness to the uncertainties that exist in the real hardware such as modeling error or disturbances. Motion planning typically takes between 0.05-0.15 s, while the optimal controller finds stabilizing feedback inputs at 500 Hz. Experimental results on the MIT Mini Cheetah demonstrate that the framework can reliably produce successful aerial motions such as jumps onto and off of platforms, spins, flips, barrel rolls, and running jumps over obstacles.

Animal-level agility and robustness in robots cannot be accomplished by solely relying on blind locomotion controllers. A significant portion of a robot's ability to traverse terrain comes from reacting to the external world through visual sensing. However, embedding the sensors and compute that provide sufficient accuracy at high speeds is challenging, especially if the robot has significant space limitations. In this paper, we propose a system integration of a small-scale quadruped robot, the MIT Mini-Cheetah Vision, that exteroceptively senses the terrain and dynamically explores the world around it at high velocities. Through extensive hardware and software development, we demonstrate a fully untethered robot with all hardware onboard running a locomotion controller that combines state-of-the-art Regularized Predictive Control (RPC) with Whole-Body Impulse Control (WBIC). We devise a hierarchical state estimator that integrates kinematic, IMU, and localization sensor data to provide state estimates specific to path planning and locomotion tasks. Our integrated system has demonstrated robust autonomous waypoint tracking in dynamic real-world environments at speeds of over 1 m/s with high rates of success.