Hybrid iLQR Model Predictive Control for Contact Implicit Stabilization on Legged Robots

We begin with a 1D bouncing ball under elastic impact [36], where the state $x=[z,\dot{z}]^{T}$ is the vertical position $z$ and velocity $\dot{z}$. The input $u$ is a force applied directly to the ball. The two hybrid modes, $1$ and $2$, are defined when the ball has negative velocity $\dot{z}<0$ and when the ball has non-negative velocity $\dot{z}\geq 0$, respectively. The dynamics on each mode are ballistic dynamics plus the input

Hybrid mode $1$ transitions to $2$ when the ball hits the ground, $g_{(1,2)}(x):=z$, and mode $2$ transitions to $1$ at apex $g_{(2,1)}(x):=\dot{z}$. When mode $1$ transitions to $2$, an elastic impact is applied, $R_{(1,2)}(x)=[z,-e\dot{z}]^{T}$ where $e$ is the coefficient of restitution. The reset map from $2$ to $1$ is identity.

The Jacobian of the reset map and saltation matrix are,

When transitioning from $2$ to $1$, both Jacobian of the reset map and saltation matrix are identity.

The problem data is to have the ball fall from an initial height with no velocity, $x_{0}=[4,0]^{T}$, and end up at a final height $x_{des}$ with no velocity with penalties $R=5\times 10^{-7}/\Delta$, $Q_{N}=100I_{2\times 2}$ and the problems were seeded with a constant input force to obtain different number of bounces. A suite of bouncing conditions are considered and are summarized in Table LABEL:table::experiment_table. In the case where $0$ bounces are optimal $x_{des}=[3,0]^{T}$ while where $1$ or $3$ bounces are optimal $x_{des}=[1,0]^{T}$. For $3$ bounces the timestep is set to $\Delta=0.004$. To evaluate the effectiveness of the hybrid extensions, Sec. III-D, an additional comparison using our hybrid iLQR algorithm where we do not apply any hybrid extensions is made. For all cases, a convergence cutoff for this problem is set to be if $|\delta J|\leq 0.05$.

The outcomes of the experiment comparing $D_{x}R$-iLQR to $\Xi$-iLQR are shown in Table LABEL:table::experiment_table. An example run is shown in Fig. 4. $D_{x}R$-iLQR did not converge ($|\delta J|>0.05$) on any example if a hybrid transition was maintained, while $\Xi$-iLQR converged on every example. The only cases where $D_{x}R$-iLQR converged were when the algorithm removed all of the bounces – which becomes equivalent to smooth iLQR. $\Xi$-iLQR has lower cost compared to $D_{x}R$-iLQR for every example except for when the problem is seeded with no bounces (they obtain the same smooth solution) and when no bounces was the optimal solution but the problem was seeded with a single bounce – which is an antagonistic seed for the problem. In this case, $\Xi$-iLQR did converge to a different local minima¹¹1 This solution was confirmed as a local minima under a single bounce by comparing it against a trajectory produced using direct collocation [7] constrained to a single bounce, as shown in Table. LABEL:table::experiment_table. , which is not surprising as it is not a global optimization.

The value of the hybrid extension was tested on the three bounce optimal three bounce seeded case. Without the hybrid extension, the optimizer did not converge and did significantly worse than $D_{x}R$-iLQR. This highlights the importance of the hybrid trajectory extensions: even though the backwards pass is correct, having mode mismatches will lead to unfavorable convergence and trajectory quality.

Overall, $\Xi$-iLQR produced locally optimal solutions for each variation and was able to remove unnecessary bounces in some cases, though it never added any. This result is expected because there is no gradient information on the backwards pass being provided to give knowledge about adding additional bounces. Furthermore, as discussed above, there may not be an appropriate controller available when a novel hybrid mode is encountered.

Hard contacts are sometimes relaxed using springs and dampers, so we consider the 1D bouncing ball case, but instead of having a discontinuous event at impact, the impact event is extended by assuming the ground is a spring damper (i.e., a force law $f_{sd}(z,\dot{z}):=kz+d\dot{z})$ when being penetrated and a spring when releasing. The system now has an identity reset, but since the saltation matrix is not identity, the hybrid transition still produces a jump in the linearization.

The hybrid modes are defined as: the aerial phase $1$, the spring-damper phase $2$ and the spring phase $3$. The spring and dampening coefficients are chosen to be $k=100$ and $d=5$. The guards are when the ball hits the ground $g_{(1,2)}=z$, when the ball no longer has any penetrating velocity $g_{(2,3)}=\dot{z}$, and when the ball is released from the ground $g_{(3,1)}=z$. For all of these transitions, the reset map is an identity transformation and the states do not change.

The example is setup to have the ball fall an initial height with an initial downwards velocity $x_{0}=[3,-2]$, end up at a height with no velocity $x_{des}=[1,0]$, with penalties $R=0.0001$, $Q_{N}=100I_{2\times 2}$ and no input for the seed.

For this experiment, $\Xi$-iLQR and $D_{x}R$-iLQR came up with similar solutions where the cost of $\Xi$-iLQR $J=13.21$ is slightly lower than $D_{x}R$-iLQR $J=13.29$. This difference is highlighted in Fig. 5 where $D_{x}R$-iLQR was not able to smooth out the spikes near mode changes. This is also reflected in $D_{x}R$-iLQR having a higher expected cost reduction as well $\delta J=0.001$ where $\Xi$-iLQR is a magnitude lower $\delta J=0.00017$. This difference in convergence can most likely be attributed to $D_{x}R$ providing gradient information that does not adjust the input pre-impact accordingly to allow for adjustments on the spikes post-impact without destructively changing the resulting end state.

To test our algorithm with a nonlinear constraint surface, we designed a system where an actuated ball in 2D space is dropped inside a hollow tube and is tasked to end in a goal location on the tube surface.

The configuration states of the system are the horizontal and vertical positions $q=[y,z]^{T}$. This system consists of two different hybrid modes: the unconstrained mode $1$ and in the constrained mode $2$. The constrained surface is defined to be a circle with radius $2$, $a(q)=4-y^{2}-z^{2}$. The dynamics of the system, (40), are ballistic dynamics with direct inputs on configurations, $M(q)=mI_{2\times 2}$, $N(q,\dot{q})=[0,-mg]^{T}$, $C(q,\dot{q})=0_{2\times 2}$, and $\Upsilon=[u_{y},u_{z}]^{T}$. The impact guard from (1,2) is defined by the circle’s constrained surface and the liftoff guard from (2,1) is the constraint force $\lambda$.

The example is setup to have the ball fall from an initial height with velocity pointing down and to the right $x_{0}=[1,0]$, end up at a specific location on circle with no velocity $x_{des}=[-\sqrt{3},-1,0,0]$, with penalties $R=0.0001$, $Q_{N}=100I_{4\times 4}$ and no input for the initial seed except for a vertical force $2mg$ applied for a small duration to cause the ball to momentarily leave the constraint.

The trajectory produced by $\Xi$-iLQR has a cost of $J=10.7$ and $D_{x}R$-iLQR a cost of $J=50.5$. The generated position trajectories along with the initial seeded trajectory are shown in Fig. 6 where both methods ended up at the goal state but $D_{x}R$-iLQR converged significantly less than $\Xi$-iLQR.

In this example, we purposely seeded a sub-optimal trajectory which releases the contact for a small duration and returns back to the constraint to evaluate if the algorithms would modify the contact sequence. $\Xi$-iLQR ended up removing this erroneous contact change and whereas $D_{x}R$-iLQR ended up not going back to the constraint surface and ended in the unconstrained mode. We speculate that because $D_{x}R$ has the wrong gradient information about contacts, it ended up staying in the unconstrained mode for a longer duration and ultimately could not converge.

We introduce a quadcopter perching example inspired by [52], where we consider a planar quadcopter which can make contact with sliding friction on a surface. When both edges of the quadcopter are touching the constraint, we assume some latching mechanism engages and fully constrains the quadcopter in place with no way to release. This problem explores planning with an underactuated system, friction, constraint surfaces, nonlinear dynamics, nonlinear guards, and nonlinear resets.

The configurations of the system are the vertical, horizontal, and angular position $q=[y,z,\theta]^{T}$ and the inputs are the left and right thrusters, $u_{1}$ and $u_{2}$. The dynamics are defined by (40) with the following

where $w=0.25$ is the width and $I=1$ is the inertia of the quadcopter.

To add more complex geometry, the constrained surface is a circle centered about the origin with radius 5. Since the edges of the quadcopter make contact with the surface, the left and right edges of the quadcopter are located at,

The constraints are then $a_{1}=25-y_{L}^{2}-z_{L}^{2}$ and $a_{2}=25-y_{R}^{2}-z_{R}^{2}$. Frictional force $\lambda_{t}$ is defined to be tangential to the constraint with magnitude proportional to the constraint force $\lambda_{n}$, $\lambda_{t}=\mu\lambda_{n}$, where $\mu$ is the coefficient of friction.

The example is setup to have the quadcopter start some distance away from the constraint with a horizontal velocity, $x_{0}=[2,2.5,-\pi/8,4,0,0]^{T}$, end up oriented with the constraint with no velocity $x_{des}=[5\cos(-\pi/12),\allowbreak 5\cos(-\pi/12),\allowbreak-7/12\pi,\allowbreak 0,0,0]^{T}$, timesteps $\Delta=0.002$, with penalties $R=0.01_{2\times 2}$, and $Q_{N}=[1000I_{3\times 3},0_{3\times 3};\allowbreak 0_{3\times 3},0.1I_{3\times 3}]$. The position portion is weighted more heavily than velocity because the goal is to get close enough to the desired location to perch. For the seed, a combined thrust of equal to $1.5mg$ was applied constantly and if both edges made contact with the constraint, the thrust force was dropped to $0.1mg$. This initial input resulted in a trajectory which makes contact with the right edge and then shortly after makes double contact with the constraint as shown in Fig. 7.

In this example, the final position trajectories are shown in Fig. 7 where $\Xi$-iLQR converged $\delta J=0.170$ with a cost of $J=4.76$ whereas $D_{x}R$-iLQR did not converge $\delta J=3\times 10^{5}$ and produced an erratic solution with very high cost of $J=2.66\times 10^{3}$.

$\Xi$-iLQR seemed to make the natural extension of the seed and followed the constraint until the target position was achieved, but removed the double constrained mode at the end. We postulate that the fully constrained mode was removed in order to better fine tune the final position because position error is weighted significantly more than velocity. However, the true optimal solution should include the fully constrained mode to eliminate any velocity for free.

In this experiment, the same 1D bouncing ball hybrid system from Sec. V-A is used. To validate that updating the cost on mode mismatches improves convergence for HiLQR MPC, we first generate a reference trajectory using Hybrid iLQR to create an optimal single bounce trajectory. HiLQR MPC is used to stabilize an initial large perturbation and is run with and without the hybrid cost update for event-driven simulations. For both cases, HiLQR MPC is applied at every timestep. At each timestep, convergence is recorded where convergence is determined by the expected reduction (21). For this test, the convergence cut-off is set to be $\delta J<1e^{-4}$. It is expected that, by utilizing the mode extensions, convergence will improve because the algorithm will not spend unnecessary computation and effort in flipping the velocity of the ball if there is a mismatch in impact timing, rather it will wait for when the impact applies the flip.

The task for the bouncing ball experiment is to track a predefined trajectory using HiLQR MPC for a fully actuated bouncing ball. The target trajectory is 1 second long, where the ball starts at 4 meters above the ground with no velocity and ends at 2.5 meters above the ground with no velocity. We compare using the event-driven hybrid cost update (Sec. IV-A) to not using this update, and the results of this experiment are shown in Fig. 8.

As expected, both methods converge and track well before the impact event is within the horizon of the HiLQR MPC. The approaches differ once the hybrid event is within the horizon, as can be seen by the high control effort and unnatural kink in state space that is produced when not using the cost update. Furthermore, of the $1001$ time steps, $8$ did not converge when the cost update was not used. Although the number of unconverged timesteps is small, the quality of the trajectory suffered greatly, as shown in Fig. 8, top row. This is because without updating the cost to account for hybrid mode mismatches, the gradient information biases the solution towards flipping the velocity before impact.

Using the cost update for hybrid mode mismatches, HiLQR MPC can correctly utilize the impact to reduce tracking error, as shown in Fig. 8, bottom row. The cost update allows HiLQR MPC to create plans that are closer to the target trajectory by shifting contact times rather than making large modifications to the input to match the contact schedule, which results in significantly better convergence. In addition to having better tracking performance, when using trajectory optimization for MPC, it is desirable to always converge and to not make drastic changes from the planned trajectory unless necessary.

To demonstrate the robustness of cohesively planning whole body motions and allowing contact schedules to change, we compare HiLQR MPC to Convex MPC and Instant QP by applying perturbations to A1 while implementing a walking gait in simulation. To make the comparison fair, the walking gait that HiLQR MPC is tracking is the same one generated from Convex MPC in the absence of perturbations. Similar gait parameters are chosen for Instant QP to produce a similar gait. All controllers are run at each timestep and use the first control input of the new trajectory as the control input for that timestep.

The walking gait starts from a standing pose and then attempts to reach a desired forward velocity of $0.2\frac{m}{s}$. Lateral velocity perturbations are applied to the robot’s body at two different magnitudes and eight different times along the gait cycle: four when each foot is in swing when getting up to speed and the other four when the gait is in steady state. The number of times the robot falls and the maximum perturbed lateral position are recorded for each push.

It is expected that HiLQR MPC should be able to recover from a wider variety of perturbations and have less deviation when the perturbations are large when compared against the centroidal methods because it can utilize the nonlinear contact dynamics of the swing and stance legs cohesively.

The robustness of HiLQR MPC is compared with Convex MPC and Instant QP for a walking trajectory at eight different perturbations in simulation. The results are summarized in Table LABEL:table::lateral_pert, farthest perturbed position is visualized for each experiment in Figs. 9 and 10, change in contact sequence in Figs. 11 and 12, and the resuling behavior shown in Fig 13.

As expected, deviations from the smaller perturbation lead to similar results and high success for all controllers. This is most likely because the perturbations do not require the controller to heavily modify the trajectory while stabilizing less stable robot states, such as in the case of the larger perturbations. In the medium and large perturbation experiments, HiLQR MPC had a higher success rate of 100% and 88% compared to the centroidal methods – Convex MPC 88% and 50% and Instant QP 50% and 25%. Failure for the controllers tended to occur when a right leg was in swing (both front right and back). This failure mode is most likely due to the lateral perturbation being applied in the left direction causing the stabilizing maneuvers to be more complicated and less stable. Because HiLQR MPC is able to plan the body and swing legs more cohesively, it can handle these complex maneuvers better than the centroidal methods, where the stance and swing legs are planned separately. This difference is mostly highlighted when the perturbations are larger. Since Instant QP performed worse than Convex MPC, further comparisons are made only between HiLQR MPC and Convex MPC.

In the large perturbation experiments, HiLQR MPC deviated half as much as Convex MPC when comparing max lateral deviations in body position, as shown in Table LABEL:table::lateral_pert. An example trial (large perturbation during the first step) is shown in Fig. 13, where HiLQR MPC used less steps to stabilize the perturbation, which ultimately led to the body deviating less than half of the deviation from Convex MPC. The contact sequence for the reference and the initial solution after applying the perturbation are shown in Figures 11 and 12. Note that HiLQR MPC is solving for new trajectories that modify the contact sequence in order to better stabilize the behavior rather than adhering to the original plan’s contact sequence. This is crucial because HiLQR MPC can add or remove contacts to help catch itself, as well as optimize the new contact locations.

Overall, HiLQR MPC performed similarly to or better than Convex MPC when stabilizing perturbations along a walking trajectory. When the perturbations are large, HiLQR MPC outperforms Convex MPC because it is able to replan a new contact sequence to stabilize about and it can fully utilize the nonlinear dynamics for the more aggressive maneuvers.

The bulk of the analysis for comparing the controllers is done in simulation because the perturbations can be consistently applied in both cases with a variety of different perturbations. To reliably apply the same perturbation on hardware, we opt for a consistent motor command block for a short period of time while the robot is walking. Once the motor commands are unblocked, the controller must react to the robot falling over, catch itself, and then continue walking.

In this experiment, we compare HiLQR MPC against Instant QP. Instant QP utilizes similar assumptions to Convex MPC (floating base convex optimization), but differs in being able to update the contact mode on every timestep while Convex MPC follows the contact sequence of the planned gait. To have a more fair comparison we utilize Instant QP to remove the clear absence of handling a new contact mode that is bound to happen with this experiment. Both Hybrid iLQR MPC and Instant QP are able to handle the perturbation in simulation but come up with different solutions. HiLQR MPC tends to replan a stand trajectory after it realizes that it is falling to catch itself, while Instant QP tries to continue the walking gait and recirculates the legs in order to catch itself. The perturbation is applied shortly after walking has started, and the torque commands are blocked for $0.15$ seconds. The experiment is run 5 times for each controller and failure is determined by if the robot’s body hits the floor and if the controller is able to continue walking after the perturbation. For state estimation, we use the Kalman filter from [53]. Because HiLQR MPC creates a new plan to track in order to handle the perturbation, it is expected to outperform Instant QP which is trying its best to continue walking.

The results of the motor-blocking physical robot experiment are shown in Fig. 14 and Table LABEL:table::motor_block. Over five trials, HiLQR MPC was able to stabilize successfully after the motor block was released every time, while Instant QP was completely unstable $60$% of the time and $40$% of the time was able to stand up and walk after the body hit the ground. Two unintended additional perturbations occurred in this experiment – there is a consistent $10$ millisecond input delay on A1 and another perturbation caused by the state estimator. The estimator relies on the kinematic information from the legs that are in contact to get a better estimate of the robot body. However, when the motor commands were blocked, all the contact forces went close to zero, which resulted in a degraded estimate of the robot body until the legs made sufficient contact with the ground again.

HiLQR MPC was able to replan a stand trajectory in order to catch itself rather than sticking to the original plan of walking as Instant QP. In the times that Instant QP successfully rejected the perturbation, the robot body actually hits the ground and the legs that are planned to be in stance apply enough standing force to get back up while the back right leg recirculates in order to counteract the backward velocity induced by getting back up. Since this relies on the body hitting the ground correctly and the back leg perfectly stabilizing the motion, it is a lot less reliable but is able to catch itself occasionally.

Similarly to the simulated experiments, HiLQR MPC outperforms the centroidal method (Instant QP) because HiLQR MPC does not have to adhere to a rigid gait schedule and can fluidly replan a new contact sequence to help stabilize the perturbation. Although Instant QP utilizes the current contact information to inform which legs are in contact, the controller is trying its best to follow the scheduled gait sequence. In this case, modifying the gait sequence from a walk to a stand is much more reliable. HiLQR MPC is able to track the walking gait when appropriate but modify it to a stand if needed to catch the robot and seamlessly return to walking once the perturbation has been stabilized. Having the ability to automatically modify the gait schedule to generate these stabilizing behaviors is important for a controller because the initial plan might not always be the best in the presence of disturbances.