Summary of Equations for Non-Generalized 3D Point-footed, Midleg-Mass, Hipped Walker without Yaw

January 22, 2006

Modeling by Robert D Gregg
rdgregg@eecs.berkeley.edu
Center for Hybrid and Embedded Software Systems
Electrical Engineering and Computer Sciences
University of California, Berkeley

Variables

x5-- >  relative stance roll  angle
x6 -- >  relative stance pitch angle
x7 -- >  
      relative swing pitch angle

ϕ -- >  
      static stance roll angle
θs -- >  
      static stance pitch angle
θns -- >  static swing pitch angle

Constants

w -- >  hip width
l -- >  leg length
Mp -- >  pelvis/hip mass
M -- >  midleg mass
gamma -- >  slope angle
\(\(Beta\ --\) > \ 
      potential\ shaping\ angle\ \((difference\ between\ passive\ slope\ angle\ and\ actual\ slope\ gamma)\)\)

Relative Coordinate System (from Derivation) for Passive Walking

Mass/Inertia Matrix

q  =  {{x5[t]}, {x6[t]}, {x7[t]}};
qdot  =  D[q, t];

MatrixForm[
      Mmatrix3x3  =  {{(1/8) ((l^2) (6 M + 4 Mp) + 
                  2 (4 M + Mp) (w^2) + 
                  (l^2) ((5 M + 4 Mp) Cos[2 x6[t]] + 
                        M (-4 Cos[x7[t]] + 
                              Cos[2 (x6[t] + x7[t])] - 
                              4 Cos[2 x6[t] + x7[t]]))), 
            (1/2) l w ((2 M + Mp) Sin[x6[t]] - 
                  M Sin[x6[t] + x7[t]]), -(1/2) l M w Sin[
                x6[t] + 
                  x7[t]]}, {(1/2) l w ((2 M + Mp) Sin[x6[t]] - 
                  M Sin[x6[t] + x7[t]]), 
            (1/2) (l^2) (3 M + 2 Mp - 2 M Cos[x7[t]]), 
            (1/4) l M (l - 
                  2 l Cos[x7[t]])}, {-(1/2) l M w Sin[
                x6[t] + x7[t]], 
            (1/4) l M (l - 
                  2 l Cos[x7[t]]), (((l^2) M)/4)}}]

Hipless

MatrixForm[Mmatrix3x3  /. w -> 0]

Lagrangian

KE  =  
      First[First[1/2 Transpose[qdot] . Mmatrix3x3 . qdot]] // 
        FullSimplify

PE  =  
        (1/2) g (l Cos[
                  x5[t]] ((3 M + 2 Mp) Cos[x6[t]] - 
                    M Cos[x6[t] + x7[t]]) - (2 M + Mp) w Sin[
                  x5[t]]);

Lagrangian  =  KE - PE

Hipless

Lagrangian  /. w -> 0  // FullSimplify

Equations of Motion

eq1  =  
      D[D[Lagrangian, x5'[t]], t]  -  D[Lagrangian, x5[t]]  // 
        FullSimplify
eq2  =  
      D[D[Lagrangian, x6'[t]], t]  -  D[Lagrangian, x6[t]]  // 
        FullSimplify
eq3  =  
      D[D[Lagrangian, x7'[t]], t]  -  D[Lagrangian, x7[t]]  // 
        FullSimplify

Collision Guard

height  =  -w Sin[x5[t]] + 
        2 l Cos[x5[t]] Sin[x6[t] + x7[t]/2] Sin[x7[t]/2] + 
        l (-Sin[x6[t]] + Sin[x6[t] + x7[t]]) Tan[gamma]

Avect  =  {{D[height, x5[t]], D[height, x6[t]], 
            D[height, x7[t]]}};
holonomicTraj  =  
      First[First[Avect . qdot]]  // FullSimplify

The guard is the zero-level set of the height function:

height == 0

and the negative region of the holonomic constraint's trajectory:

holonomicTraj  <  0

Impact Equations

{x5impact  =  x5[t],  x6impact  =  x6[t] + x7[t],  
        x7impact  =  -x7[t]};

{x5dotimpact = -((Cos[
                              2 x6[t]] (((l^2) M (M + 4 Mp) + 
                                      4 (((M + Mp))^2) (w^2)) Cos[
                                    2 x7[t]] - 
                                M (-2 (l^2) (M + 2 Mp) + 
                                      4 (M + Mp) (w^2) + 
                                      (l^2) M Cos[4 x7[t]])) + 
                          2 ((l^2) M (3 M + 4 Mp) - (2 (M^2) + 
                                      M Mp - 2 (Mp^2)) (w^2) + 
                                2 (M^2) (-(l^2) + (w^2)) Cos[
                                    2 x7[t]] + 
                                4 (l^2) (M + Mp) Cos[x6[t]]^2 Cos[
                                    x7[t]] (-3 M - 4 Mp + 
                                      2 M Cos[
                                        2 x7[
                                        t]]) + (-2 ((l^2) M (M + 2 Mp) + 2 (((M + Mp))^2) (w^2)) Cos[x7[t]] + 
                                      (l^2) (-2 (M + 
                                        Mp) (-3 M - 4 Mp + 
                                        2 M Cos[2 x7[t]]) + 
                                        (M^2) Cos[3 x7[t]])) Sin[
                                    2 x6[t]] Sin[x7[t]])) x5'[t]+4 l w ((2 Mp (5 M + 
                                      4 Mp + (6 M + 4 Mp) Cos[
                                        x7[t]]) Sin[
                                    x6[t]] Sin[x7[t]/2]^2 - (2 M + 
                                      Mp) Cos[
                                    x6[t]] (-M + 
                                      2 (M + 2 Mp) Cos[x7[t]]) Sin[
                                    x7[t]]) x6'[t]+M (2 M Cos[x6[t]] Sin[x7[t]] + 
                                Mp Sin[x6[t] + x7[t]]) x7'[t]))/(Cos[
                      2 x6[t]] (((l^2) (13 (M^2) + 32 M Mp + 
                                    16 (Mp^2)) + 
                              4 Mp (2 M + Mp) (w^2)) Cos[
                            2 x7[t]] + 
                        M (-2 (l^2) M + 4 (M + 2 Mp) (w^2) - 
                              (l^2) (5 M + 4 Mp) Cos[4 x7[t]])) + 
                  2 ((l^2) (3 M + 2 Mp) (3 M + 
                              4 Mp) + (2 (M^2) + 7 M Mp + 
                              2 (Mp^2)) (w^2) + 
                        2 M (-(l^2) (3 M + 2 Mp) + 
                              Mp (w^2)) Cos[2 x7[t]] + 
                        4 (l^2) M Cos[x6[t]]^2 (-2 (M + 
                                    2 Mp) Cos[x7[t]] + 
                              M Cos[
                                  3 x7[
                                      t]]) + (-2 ((l^2) (M + 
                                        2 Mp) (5 M + 4 Mp) + 
                                    2 Mp (2 M + Mp) (w^2)) Cos[
                                  x7[t]] + 
                              (l^2) M (6 M + 8 Mp - 
                                    4 M Cos[
                                        2 x7[t]] + (5 M + 4 Mp) Cos[
                                        3 x7[t]])) Sin[2 x6[t]] Sin[
                            x7[t]])),
x6dotimpact = -(2 w (8 (l^2) (M + Mp) Cos[x6[t]]^2 (M - 
                                Mp Cos[2 x7[t]]) Sin[x6[t]] + 
                          Cos[x7[
                                t]] ((-2 (l^2) (2 (M^2) + 
                                        2 M Mp + (Mp^2)) - 
                                      2 M (M + 2 Mp) (w^2) + 
                                      (l^2) (M^2) Cos[2 x7[t]]) Sin[
                                    x6[t]] + ((l^2) ((M^2) + 4 M Mp + 
                                        2 (Mp^2)) - 
                                      2 M (M + Mp) (w^2) - 
                                      4 (l^2) (((M + Mp))^2) Cos[
                                        2 x7[t]]) Sin[3 x6[t]]) + 
                          Cos[x6[
                                t]] (M (3 (l^2) M - 
                                      2 (2 M + Mp) (w^2)) - 
                                4 (M + 
                                      Mp) (((l^2) (M + Mp) + 
                                        M (w^2)) Cos[2 x6[t]] + 
                                      2 (l^2) (M + 
                                        Mp Cos[2 x6[t]]) Cos[x7[t]]) - 
                                (l^2) (-5 (M^2) - 8 M Mp - 4 (Mp^2) + 
                                      8 (((M + Mp))^2) Cos[
                                        2 x6[t]]) Cos[2 x7[t]]) Sin[
                              x7[t]]) x5'[t]+l ((Cos[
                                    2 x6[
                                        t]] (M ((l^2) (5 M + 
                                        8 Mp) + 
                                        4 M (w^2)) - ((l^2) (11 (M^2) + 18 M Mp + 8 (Mp^2)) + 4 (2 (M^2) + 4 M Mp + (Mp^2)) (w^2)) Cos[
                                        x7[t]] + 
                                      (l^2) (3 M (3 M + 4 Mp) Cos[
                                        2 x7[t]] - (M + 
                                        2 Mp) (5 M + 4 Mp) Cos[
                                        3 x7[t]])) + 
                                2 ((l^2) M (5 M + 6 Mp) + 
                                      M (2 M + Mp) (w^2) - 
                                      2 (4 (l^2) (((M + Mp))^2) + (2 (M^2) + 5 M Mp + (Mp^2)) (w^2)) Cos[x7[t]] + 
                                      2 (l^2) M (M + 2 Mp) Cos[
                                        2 x7[
                                        t]] + ((l^2) (7 (M^2) + 
                                        14 M Mp + 8 (Mp^2)) + 
                                        2 Mp (2 M + Mp) (w^2) + 
                                        (l^2) (-3 M (3 M + 
                                        4 Mp) Cos[
                                        x7[t]] + (M + 2 Mp) (5 M + 
                                        4 Mp) Cos[2 x7[t]])) Sin[
                                        2 x6[t]] Sin[x7[t]])) x6'[t]+M ((l^2) (6 M + 4 Mp) + 
                                2 (2 M + Mp) (w^2) + 
                                M ((l^2) + 4 (w^2)) Cos[2 x6[t]] + 
                                (l^2) (-4 M Cos[
                                        x7[t]] + (5 M + 4 Mp) Cos[
                                        2 (x6[t] + x7[t])] - 
                                      4 M Cos[2 x6[t] + x7[t]])) x7'[t]))/(l (Cos[
                          2 x6[t]] (((l^2) (13 (M^2) + 32 M Mp + 
                                        16 (Mp^2)) + 
                                  4 Mp (2 M + Mp) (w^2)) Cos[
                                2 x7[t]] + 
                            M (-2 (l^2) M + 
                                  4 (M + 2 Mp) (w^2) - 
                                  (l^2) (5 M + 4 Mp) Cos[
                                      4 x7[t]])) + 
                      2 ((l^2) (3 M + 2 Mp) (3 M + 
                                  4 Mp) + (2 (M^2) + 7 M Mp + 
                                  2 (Mp^2)) (w^2) + 
                            2 M (-(l^2) (3 M + 2 Mp) + 
                                  Mp (w^2)) Cos[2 x7[t]] + 
                            4 (l^2) M Cos[x6[t]]^2 (-2 (M + 
                                        2 Mp) Cos[x7[t]] + 
                                  M Cos[
                                      3 x7[
                                        t]]) + (-2 ((l^2) (M + 
                                        2 Mp) (5 M + 4 Mp) + 
                                        2 Mp (2 M + Mp) (w^2)) Cos[
                                      x7[t]] + 
                                  (l^2) M (6 M + 8 Mp - 
                                        4 M Cos[
                                        2 x7[t]] + (5 M + 4 Mp) Cos[
                                        3 x7[t]])) Sin[2 x6[t]] Sin[
                                x7[t]]))),
x7dotimpact = (w (((l^2) (21 (M^2) + 24 M Mp + 
                                    4 (Mp^2)) + 
                              8 Mp (M + Mp) (w^2)) Sin[x6[t]] + 
                        8 (l^2) M (M + Mp) Sin[3 x6[t]] + 
                        5 (l^2) (M^2) Sin[x6[t] - 2 x7[t]] + 
                        4 (l^2) M Mp Sin[x6[t] - 2 x7[t]] - 
                        24 (l^2) (M^2) Sin[x6[t] - x7[t]] - 
                        36 (l^2) M Mp Sin[x6[t] - x7[t]] - 
                        16 (l^2) (Mp^2) Sin[x6[t] - x7[t]] + 
                        4 (M^2) (w^2) Sin[x6[t] - x7[t]] - 
                        (l^2) (M^2) Sin[3 x6[t] - x7[t]] + 
                        (l^2) (M^2) Sin[x6[t] + x7[t]] - 
                        8 (M^2) (w^2) Sin[x6[t] + x7[t]] - 
                        8 M Mp (w^2) Sin[x6[t] + x7[t]] - 
                        17 (l^2) (M^2) Sin[3 x6[t] + x7[t]] - 
                        28 (l^2) M Mp Sin[3 x6[t] + x7[t]] - 
                        12 (l^2) (Mp^2) Sin[3 x6[t] + x7[t]] - 
                        4 (M^2) (w^2) Sin[3 x6[t] + x7[t]] - 
                        4 M Mp (w^2) Sin[3 x6[t] + x7[t]] - 
                        8 (l^2) (M^2) Sin[x6[t] + 2 x7[t]] - 
                        4 (l^2) M Mp Sin[x6[t] + 2 x7[t]] + 
                        8 (M^2) (w^2) Sin[x6[t] + 2 x7[t]] - 
                        4 (Mp^2) (w^2) Sin[x6[t] + 2 x7[t]] + 
                        7 (l^2) (M^2) Sin[3 x6[t] + 2 x7[t]] + 
                        8 (l^2) M Mp Sin[3 x6[t] + 2 x7[t]] + 
                        8 (M^2) (w^2) Sin[3 x6[t] + 2 x7[t]] + 
                        12 M Mp (w^2) Sin[3 x6[t] + 2 x7[t]] + 
                        4 (Mp^2) (w^2) Sin[3 x6[t] + 2 x7[t]] + 
                        5 (l^2) (M^2) Sin[x6[t] + 3 x7[t]] + 
                        8 (l^2) M Mp Sin[x6[t] + 3 x7[t]] + 
                        4 (l^2) (Mp^2) Sin[x6[t] + 3 x7[t]] + 
                        (l^2) (M + 2 Mp) (3 M + 2 Mp) Sin[
                            3 x6[t] + 4 x7[t]]) x5'[t]+l (4 (2 (-(M + 
                                        Mp) (2 (l^2) (M + 
                                        2 Mp) - 
                                        3 Mp (w^2) + (8 (l^2) (M + 
                                        Mp) + 
                                        2 (2 M - Mp) (w^2)) Cos[
                                        x7[t]] - 
                                        4 (l^2) M Cos[2 x7[t]]) - 
                                    Cos[2 x6[
                                        t]] (-3 (l^2) M (M + 
                                        Mp) + 
                                        Mp (5 M + 
                                        3 Mp) (w^2) + ((l^2) (M + 
                                        Mp) (3 M + 4 Mp) + 
                                        2 (2 M + Mp) (M + 
                                        2 Mp) (w^2)) Cos[x7[t]] + 
                                        (l^2) (M + 
                                        Mp) ((M + 4 Mp) Cos[
                                        2 x7[t]] + (5 M + 4 Mp) Cos[
                                        3 x7[
                                        t]]))) Sin[x7[t]/2]^2 + (-((l^2) (M + Mp) (9 M + 4 Mp) + 
                                        2 (2 M + Mp) (M + 
                                        2 Mp) (w^2)) Cos[
                                        x7[t]] + (M + 
                                        Mp) ((l^2) (5 M + 
                                        4 Mp) + (2 M + Mp) (w^2) + 
                                        (l^2) ((9 M + 4 Mp) Cos[
                                        2 x7[t]] - (5 M + 4 Mp) Cos[
                                        3 x7[t]]))) Sin[
                                  2 x6[t]] Sin[x7[t]]) x6'[t]+M (2 (l^2) (5 M + 2 Mp) + 
                              2 (2 M + Mp) (w^2) + 
                              M (5 (l^2) + 4 (w^2)) Cos[2 x6[t]] - 
                              (l^2) M Cos[2 x6[t] - x7[t]] - 
                              16 (l^2) M Cos[x7[t]] - 
                              8 (l^2) Mp Cos[x7[t]] - 
                              8 M (w^2) Cos[x7[t]] + 
                              4 (l^2) M Cos[2 x7[t]] + 
                              9 (l^2) M Cos[2 (x6[t] + x7[t])] + 
                              4 (l^2) Mp Cos[2 (x6[t] + x7[t])] - 
                              10 (l^2) M Cos[2 x6[t] + x7[t]] - 
                              4 (l^2) Mp Cos[2 x6[t] + x7[t]] - 
                              8 M (w^2) Cos[2 x6[t] + x7[t]] - 
                              4 Mp (w^2) Cos[2 x6[t] + x7[t]] - 
                              (l^2) (5 M + 4 Mp) Cos[
                                  2 x6[t] + 3 x7[t]]) x7'[t]))/(l (Cos[
                          2 x6[t]] (((l^2) (13 (M^2) + 32 M Mp + 
                                        16 (Mp^2)) + 
                                  4 Mp (2 M + Mp) (w^2)) Cos[
                                2 x7[t]] + 
                            M (-2 (l^2) M + 
                                  4 (M + 2 Mp) (w^2) - 
                                  (l^2) (5 M + 4 Mp) Cos[
                                      4 x7[t]])) + 
                      2 ((l^2) (3 M + 2 Mp) (3 M + 
                                  4 Mp) + (2 (M^2) + 7 M Mp + 
                                  2 (Mp^2)) (w^2) + 
                            2 M (-(l^2) (3 M + 2 Mp) + 
                                  Mp (w^2)) Cos[2 x7[t]] + 
                            4 (l^2) M Cos[x6[t]]^2 (-2 (M + 
                                        2 Mp) Cos[x7[t]] + 
                                  M Cos[
                                      3 x7[
                                        t]]) + (-2 ((l^2) (M + 
                                        2 Mp) (5 M + 4 Mp) + 
                                        2 Mp (2 M + Mp) (w^2)) Cos[
                                      x7[t]] + 
                                  (l^2) M (6 M + 8 Mp - 
                                        4 M Cos[
                                        2 x7[t]] + (5 M + 4 Mp) Cos[
                                        3 x7[t]])) Sin[2 x6[t]] Sin[
                                x7[t]])))};

Converting to Static Coordinate System

Conversion from relative to static coordinates:

{x6[t] -> -θs[t], x6'[
          t] -> -θs'[t], 
      x7[t] -> (-θns[t] + θs[t]), x7'[
          t] -> (-θns'[t] + θs'[t]), 
      x5[t] -> ϕ[t], x5'[t] -> ϕ'[
          t]}

Variables

ϕ -- >  stance roll  angle
θns -- >  swing pitch angle
θs -- >  stance pitch angle

Lagrangian

q  =  {{ϕ[t]}, {θns[t]}, {θs[
              t]}};
qdot  =  D[q, t];

KE  =  
      KE  /. {x6[t] -> -θs[t], x6'[
                t] -> -θs'[t], 
            x7[t] -> (-θns[t] + θs[t]), x7'[
                t] -> (-θns'[t] + θs'[t]), 
            x5[t] -> ϕ[t], x5'[t] -> ϕ'[t]} // 
        FullSimplify

PE  =  
      PE  /.  {x6[t] -> -θs[t], x6'[
                t] -> -θs'[t], 
            x7[t] -> (-θns[t] + θs[t]), x7'[
                t] -> (-θns'[t] + θs'[t]), 
            x5[t] -> ϕ[t], x5'[t] -> ϕ'[t]}  // 
        FullSimplify

Lagrangian  =  KE  -  PE  /. w -> 0

Lagrangian  /.  w -> 0

Mass/Inertia Matrix

MatrixForm[
      MmatrixStatic  =  {{(1/8) ((l^2) (6 M + 4 Mp) + 
                  2 (4 M + Mp) (w^2) + 
                  (l^2) (M Cos[2 θns[t]] - 
                        8 M Cos[θns[t]] Cos[θs[
                              t]] + (5 M + 4 Mp) Cos[
                            2 θs[t]])),   1/
                2 l w (-M Sin[θns[t]]) ,  
            1/2 l w ((2 M + Mp) Sin[θs[
                      t]]) }, 
{ 
            1/2 l w (-M Sin[θns[t]]), 
            l^2 M/4,  -1/
                2 l^2 M Cos[θs[t] - θns[
                    t]]}, 
{ 
            1/2 l w ((2 M + Mp) Sin[θs[t]]) ,  -1/
                2 l^2 M Cos[θs[t] - θns[t]],  
            l^2 M/4  +  l^2 (M + Mp)}}]

Hipless

MatrixForm[MmatrixStatic  /. w -> 0]

KE  ==  
        First[First[
            1/2 Transpose[{{ϕ'[t]}, {θns'[
                        t]}, {θs'[t]}}] . 
                MmatrixStatic . {{ϕ'[t]}, {θns'[
                      t]}, {θs'[t]}}]]  // 
      FullSimplify

Equations of Motion

eq1  =  
      D[D[Lagrangian, ϕ'[t]], t]  -  D[Lagrangian, ϕ[t]]  // 
        FullSimplify
eq2  =  
      D[D[Lagrangian, θns'[t]], t]  -  
          D[Lagrangian, θns[t]]  // 
        FullSimplify
eq3  =  
      D[D[Lagrangian, θs'[t]], t]  -  
          D[Lagrangian, θs[t]]  // FullSimplify

odes  =  
      Solve[{eq1  ==  0,  eq2 == 0,  
            eq3 == 0},  {ϕ''[t],  θns''[
              t],  θs''[t]}]  // FullSimplify

Hipless

odes  /. w -> 0  // FullSimplify

Collision Guard

height  =  
      l (-Cos[θns[t]] + Cos[θs[t]]) Cos[ϕ[t]] - 
        w Sin[ϕ[t]] + 
        l (-Sin[θns[t]] + Sin[θs[t]]) Tan[
            gamma]

Avect  =  {{D[height, ϕ[t]], D[height, θns[t]], 
            D[height, θs[t]]}};
holonomicTraj  =  
      First[First[
            Avect . {{ϕ'[t]}, {θns'[t]}, {θs'[
                    t]}}]]  // FullSimplify

The guard is the zero-level set of the height function:

height == 0

and the negative region of the holonomic constraint's trajectory:

holonomicTraj  <  0

Hipless

height  ==  0  /. w -> 0
holonomicTraj  < 0  /. w -> 0

Impact Equations

{phiimpact  =  ϕ[t],  thetaNSimpact  =  θs[t],  
        thetaSimpact  =  θns[t]};

phidotimpact  =  
      x5dotimpact  /. {x6[t] -> -θs[t], x6'[
                t] -> -θs'[t], 
            x7[t] -> (-θns[t] + θs[t]), x7'[
                t] -> (-θns'[t] + θs'[t]), 
            x5[t] -> ϕ[t], x5'[t] -> ϕ'[t]}  // 
        FullSimplify
thetaNSdotimpact  =  -x7dotimpact - 
            x6dotimpact  /. {x6[t] -> -θs[t], x6'[
                t] -> -θs'[t], 
            x7[t] -> (-θns[t] + θs[t]), x7'[
                t] -> (-θns'[t] + θs'[t]), 
            x5[t] -> ϕ[t], x5'[t] -> ϕ'[t]}  // 
        FullSimplify
thetaSdotimpact  =  -x6dotimpact  /. {x6[
                t] -> -θs[t], x6'[
                t] -> -θs'[t], 
            x7[t] -> (-θns[t] + θs[t]), x7'[
                t] -> (-θns'[t] + θs'[t]), 
            x5[t] -> ϕ[t], x5'[t] -> ϕ'[t]}  // 
        FullSimplify

Hipless

{phidotimpact,  thetaNSdotimpact,  thetaSdotimpact}  /. 
        w -> 0   // FullSimplify

The last two equations match the impact equations for the 2D-equivalent walker as derived by Bullo/Grizzle's method.

Controlled Hipless Walking on Flat Ground (Static Coordinates)

Controlled Lagrangian

PE

Two-dimensional potential shaping for flat-ground walking:

PE2Dgamma  =  
      PE  /. {ϕ[t] -> 
            0,  θns[t] -> θns[t] - Beta,  θs[
              t] -> θs[t] - Beta}

Three-dimensional potential shaping for flat-ground walking and lean compensation:

PE3Dmu  =  
      PE2Dgamma  -  
          1/2 (-c ϕ[
                      t])^2/((1/8) ((l^2) (6 M + 4 Mp) + 
                      (l^2) (M Cos[2 θns[t]] - 
                            8 M Cos[θns[t]] Cos[θs[
                                  t]] + (5 M + 4 Mp) Cos[
                                2 θs[t]])))  // 
        FullSimplify

L3Dmu  =  KE - PE3Dmu  /. w -> 0

Controlled Equations of Motion

eq1  =  
      D[D[L3Dmu, ϕ'[t]], t]  -  D[L3Dmu, ϕ[t]]  // 
        FullSimplify
eq2  =  
      D[D[L3Dmu, θns'[t]], t]  -  D[L3Dmu, θns[t]]  // 
        FullSimplify
eq3  =  
      D[D[L3Dmu, θs'[t]], t]  -  D[L3Dmu, θs[t]]  // 
        FullSimplify

odes  =  
      Solve[{eq1  ==  0,  eq2 == 0,  
            eq3 == 0},  {ϕ''[t],  θns''[
              t],  θs''[t]}]  // FullSimplify

Conserved Quantity Relationship (Note: Initial Condition for ϕdot)

Conserved quantity μ is parameterized in terms of ϕ in order to implement lean compensation:

μ  =  -c ϕ[t]

ϕ'[
        t]  =  μ/
          m3D[θ]  =   -c ϕ[
              t]/((1/8) ((l^2) (6 M + 4 Mp) + 
                    (l^2) (M Cos[2 θns[t]] - 
                          8 M Cos[θns[t]] Cos[θs[
                                t]] + (5 M + 4 Mp) Cos[
                              2 θs[t]])))

Collision Guard (Invariant of Control)

Assuming ϕ ∈ (-π/2, π/2), so as to disregard scaling factor Cos[ϕ] that will never bring height to zero, flat ground, and hipless:

height  ==  0  /. {ϕ[t] -> 0, w -> 0, 
        gamma -> 0}
holonomicTraj  < 0  /. {ϕ[t] -> 0, w -> 0, 
        gamma -> 0}

Impact Equations (Invariant of Control)

{phidotimpact,  thetaNSdotimpact,  thetaSdotimpact}  /. 
        w -> 0   // FullSimplify