Given a fixed mode sequence

For the case when the mode sequence is fixed, then hybrid trajectory optimization can be as simple as stitching together multiple individual mathematical programs into a single mathematical program, with the boundary conditions constrained to enforce the guard/reset constraints. Using the shorthand $\bx_k$ for the state in the $k$th segment of the sequence, and $m_k$ for the mode in segment $k$, we can write: \begin{align*} \find_{\bx_k[\cdot],h_k} \quad \subjto \quad & \bx_0[0] = \bx_0, \\ \forall k, \forall n_k \in [0, N_k-1], \quad & \text{collocation constraints}_{m_k}(\bx_k[n_k], \bx_k[n_k+1], h_k[n_k]), \\ & h_{min} \le h_k[n_k] \le h_{max}, \\ \forall k\in[0,K-1] \quad & \phi_{m_k,m_{k+1}}(\bx_k[N_k]) = 0, \\ & \bx_{k+1}[0] = {\bf \Delta}_{m_k,m_{k+1}}(\bx_k[N_k]), \\ \forall k, n_k, m \in G \quad & \phi_{m_k,m}(\bx_k[n_k]) > 0. \end{align*} The constraints in the last line ensure that no trajectories intersect an unscheduled guard, and $G$ is taken to be the tuples of $k,n_k, m$ which list all guards and times except the scheduled ones. It is then natural to add control inputs (as additional decision variables), and to add an objective and any more constraints.

A basketball trick shot.

As a simple example of this hybrid trajectory optimization, I thought it would be fun to see if we can formulate the search for initial conditions that optimizes a basketball "trick shot". A quick search turned up this video for inspiration.

Let's start simpler -- with just a "bounce pass". We can capture the dynamics of a bouncing ball (in the plane, ignoring spin) with some very simple dynamics: $$\bq = \begin{bmatrix}x \\ z\end{bmatrix}, \qquad \ddot{\bq} = \begin{bmatrix} 0 \\ -g \end{bmatrix}.$$ During any time interval without contact of duration $h$, we can actually integrate these dynamics perfectly: $$\bx(t+h) = \begin{bmatrix} x(t) + h\dot{x}(t) \\ z(t) + h \dot{z}(t) - \frac{1}{2}gh^2 \\ \dot{x}(t) \\ \dot{z}(t) - hg \end{bmatrix}.$$ With the bounce pass, we just consider collisions with the ground, so we have a guard, ${\bf \phi}(\bx) = z,$ which triggers when $z=0$, and a reset map which assumes an elastic collision with coefficient of restitution $e$: $$\bx^+ = {\bf \Delta(\bx^-)} = \begin{bmatrix} x^- & z^- & \dot{x}^- & - e \dot{z}^- \end{bmatrix}^T.$$

We'll formulate the problem as this: given an initial ball position $(x = 0, z = 1)$, a final ball position 4m away $(x=4, z=1)$, find the initial velocity to achieve that goal in 5 seconds. Clearly, this implies that $\dot{x}(0) = 4/5.$ The interesting question is -- what should we do with $\dot{z}(0)$? There are multiple solutions -- which involve bouncing a different number of times. We can find them all with a simple hybrid trajectory optimization, using the observation that there are two possible solutions for each number of bounces -- one that starts with a positive $\dot{z}(0)$ and one with a negative $\dot{z}(0)$.

Now let's try our trick shot. I'll move our goal to $x_f = -1m, z_f = 3m,$ and introduce a vertical wall at $x=0$, and move our initial conditions back to $x_0=-.25m.$ The collision dynamics, which now must take into account the spin of the ball, are in the appendix. The first bounce is against the wall, the second is against the floor. I'll also constrain the final velocity to be down (have to approach the hoop from above). Try it out.

In this example, we could integrate the dynamics in each segment analytically. That is the exception, not the rule. But you can take the same steps with a little more code to use, e.g. direct transcription or collocation with multiple break points in each segment.

Direct shooting

It is also possible to optimize the trajectory in a single shot by taking gradients through the guard/reset map. This method does not require an explicit mode sequence, but is prone to having local minima (since there is no gradient to "pull" the trajectory towards a guard that is not visited along the nominal trajectory).

In order to achieve this, we need to take the gradient of the cost with respect to the trajectory parameters. The methods we've developed previously for calculating gradients using the adjoint method (e.g. "backpropagation through time") in discrete and continuous time can be combined to handle the hybrid case. The derivation is almost as simple as using the continuous adjoint equation to solve (from the final time forward) for the continuous modes, and the discrete adjoint equation to handle the jump discontinuity. The only detail is that we also have to consider variations relative to the contact time; this gradient is often referred to as the "Saltation matrix".

Consider the case of two modes and one transition in our canonical hybrid system picture above. Then one could write the total cost as \begin{gather*} J_\balpha = \int_0^{t_c^-} \ell_1(\bx_1, \bu_\balpha) dt + \int_{t_c^+}^{t_f} \ell_2(\bx_2, \bu_\balpha) dt,\\ \dot{\bx}_1 = {\bf f}_1(\bx_1, \bu_\balpha), \quad \dot{\bx}_2 = {\bf f}_2(\bx_2, \bu_\balpha), \quad \bx_2(t_c^+) = {\bf \Delta}_{1,2}(\bx_1(t_c^-)). \end{gather*} We wish to take the gradient of $J$ with respect to the trajectory parameters, $\alpha$. The gradient of the first and second integrals is exactly the familiar continuous-time case, but the second integral must be initialized with $\pd{\bx_2(t_c^+)}{\alpha_i},$ which should be related to $\pd{\bx_1(t_c^-)}{\alpha_i}$ via the discrete transition. The important point is to capture the dependence of this transition not only on $\bx$ but also on $t_c$, which both depend on $\balpha.$ Writing it out we have...

Deriving hybrid models: minimal vs floating-base coordinates

There is some work to do in order to derive the equations of motion in this form. Do you remember how we did it for the rimless wheel and compass gait examples? In both cases we assumed that exactly one foot was attached to the ground and that it would not slip, this allowed us to write the Lagrangian as if there was a pin joint attaching the foot to the ground to obtain the equations of motion. For the SLIP model, we derived the flight phase and stance phase using separate Lagrangian equations each with different state representations. I would describe this as the minimal coordinates modeling approach -- it is elegant and has some important computational advantages that we will come to appreciate in the algorithms below. But it's a lot of work! For instance, if we also wanted to consider friction in the foot contact of the rimless wheel, we would have to derive yet another set of equations to describe the sliding mode (adding, for instance, a prismatic joint that moved the foot along the ramp), plus the guards which compute the contact force for a given state and the distance from the boundary of the friction cone, and on and on.

Fortunately, there is an alternative modeling approach for deriving the modes, guards, and resets for contact that is more general (though it makes the dynamics of each mode a bit more complex). We can instead model the robot in the floating-base coordinates -- we add a fictitious six degree-of-freedom "floating-base" joint connecting some part of the robot to the world (in planar models, we use just three degrees-of-freedom, e.g. $(x,z,\theta)$). We can derive the equations of motion for the floating-base robot once, without considering contact, then add the additional constraints that come from being in contact as contact forces which get applied to the bodies. The resulting manipulator equations take the form \begin{equation}\bM({\bq})\ddot{\bq} + \bC(\bq,\dot{\bq})\dot\bq = \btau_g(\bq) + \bB\bu + \sum_i \bJ_i^T(\bq) \blambda_i,\end{equation} where $\blambda_i$ are the constraint forces and $\bJ_i$ are the constraint Jacobians. Conveniently, if the guard function in our contact equations is the signed distance from contact, $\phi_i(\bq)$, then this Jacobian is simply $\bJ_i(\bq) = \pd{\phi_i}{\bq}$. I've written the basic derivations for the common cases (position constraints, velocity constraints, impact equations, etc) in the appendix. What is important to understand here is that this is an alternative formulation for the equations governing the modes, guards, and resets, but that is it no longer a minimal coordinate system -- the equations of motion are written in $2N$ state variables but the system might actually be constrained to evolve only along a lower dimensional manifold (if we write the rimless wheel equations with three configuration variables for the floating base, it still only rotates around the toe when it is in stance and is inside the friction cone). This will have implications for our algorithms.

Note that there are some subtleties about using direct collocation methods with systems parameterized in the floating-base coordinates with additional contact constraints -- the naive implementation of direct collocation may not have enough degrees of freedom in the spline to satisfy the dynamics constraints exactly at the knot points and the collocation points. Posa15 shows the natural extension of direct collocation to constrained systems; and describes more completely the advantages of parameterizing contact forces as decision variables.

For completeness, you might also hear the term maximal coordinates. This is the extreme case where every rigid body in the robot is parameterized with its own floating-base, and even the robot joints are implemented as constraints holding the links together. This has been used by some notable simulation engines (e.g. the Open Dynamics Engine), and may have advantages for some computational methods. But for our purposes, using the dynamics in floating-base coordinates is typically much more effective.