Summary

This project incorporates the density formulation from the following paper submitted to Robotics Automations and Letters (RA-L), titled “Safe Navigation Using Density Functions” onto quadrupeds. It incorporates the high level plan of the following dynamics $\dot x = u$ and maps to a centroidal reference trajectory of the following dynamic form:

where $\mathbf{M}$ is the inertial matrix, $\mathbf{C}$ contains the centripetal and coriolis terms, $\mathbf{G}$ is the gravitational field, $\mathbf{B}$ is the control matrix, $\mathbf{\tau}$ is the joint torques, and $\mathbf{q} \in \mathbb{R}^{24}$ are the states for the dynamical system. This is then incorporated into a nonlinear model predictive controller (NMPC), where the optimized control trajectory is tracked with a torque-level control. The schematic diagram describes the process.

The main contribution of this work integrates the novel formulation of density for safe navigation using the modular legged planner framework. As a result, we are able to incorporate safe navigation using density for quadrupeds. For more detailed information, see the following paper.

Results

We show results in both simulations and hardware. In simulation, we show the quadruped navigating around multiple obstacles.

We then show on hardware, where the density motion plan navigates around the chair (obstacle) and goes towards the goal, which is tracked using a NMPC.