Mathematical Model for 3D Point-footed, Midleg-Mass, Hipped Walker with Yaw

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

\(<< \ H:\Screws.m\)
\(<< \ H:\RobotLinks.m\)

Variables

x4 -- >  stance yaw angle
x5 -- >  stance roll angle
x6 -- >  stance pitch angle
x7 -- >  swing pitch angle

Constants

w -- >  hip width
l -- >  leg length
Mp -- >  pelvis/hip mass
M -- >  midleg mass
gamma -- >  slope angle

3D Walker Open Chain Kinematics

Foot/Leg links

q  =  {{x1[t]}, {x2[t]}, {x3[t]}, {x4[t]}, {x5[t]}, {x6[t]}, {x7[
              t]}};
qdot  =  D[q, t];

ax1  =  {1, 0, 0, 0, 0, 0};
ax2  =  {0, 1, 0, 0, 0, 0};
ax3  =  {0, 0, 1, 0, 0, 0};
ax4  =  {0, 0, 0, 0, 0, 1};
ax5  =  {0, 0, 0, 0, 1, 0};
ax6  =  {0, 0, 0, 1, 0, 0};
ax7  =  
        Flatten[Append[
            Cross[{0, 0, l}, {1, 0, 0}], {1, 0, 0}]];

gst01  =  {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, l/2}, {0, 0, 0, 
            1}};
gst02  =  {{1, 0, 0, w/2}, {0, 1, 0, 0}, {0, 0, 1, l}, {0, 0, 0, 
            1}};
gst03  =  {{1, 0, 0, w}, {0, 1, 0, 0}, {0, 0, 1, l/2}, {0, 0, 0, 
            1}};

J1  =  
        BodyJacobian[{ax1, x1[t]}, {ax2, x2[t]}, {ax3, x3[t]}, {ax4, 
              x4[t]}, {ax5, x5[t]}, {ax6, x6[t]}, {{0, 0, 0, 0, 0, 0}, 
              x7[t]},  gst01] // FullSimplify;
J2  =  
        BodyJacobian[{ax1, x1[t]}, {ax2, x2[t]}, {ax3, x3[t]}, {ax4, 
              x4[t]}, {ax5, x5[t]}, {ax6, x6[t]}, {{0, 0, 0, 0, 0, 0}, 
              x7[t]},  gst02] // FullSimplify;
J3  =  
        BodyJacobian[{ax1, x1[t]}, {ax2, x2[t]}, {ax3, x3[t]}, {ax4, 
              x4[t]}, {ax5, x5[t]}, {ax6, x6[t]}, {ax7, x7[t]},  gst03] // 
          FullSimplify;
MatrixForm[J1]
MatrixForm[J2]
MatrixForm[J3]

genM1  =  
        M*{{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, {0, 0, 
              0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 
              0}};
genM2  =  
        Mp*{{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, {0, 
              0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 
              0}};
genM3  =  
        M*{{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, {0, 0, 
              0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 
              0}};

MatrixForm[
      Mmatrix  =  
        Transpose[J1] . genM1 . J1  +  Transpose[J2] . genM2 . J2  +  
            Transpose[J3] . genM3 . J3  // FullSimplify]

MatrixForm[Mmatrix  /. w -> 0  // FullSimplify]

KE  =  
        First[First[1/2 Transpose[qdot] . Mmatrix . qdot]] // 
          Simplify;

KE /. {x1[t] -> 0, x2[t] -> 0, 
          x3[t] -> 0, x1'[t] -> 0, x2'[t] -> 
            0, x3'[t] -> 0}  // FullSimplify

Potential Term

MatrixForm[e1  =  TwistExp[ax1, x1[t]]]
MatrixForm[e2  =  TwistExp[ax2, x2[t]]]
MatrixForm[e3  =  TwistExp[ax3, x3[t]]]
MatrixForm[e4  =  TwistExp[ax4, x4[t]]]
MatrixForm[e5  =  TwistExp[ax5, x5[t]]]
MatrixForm[e6  =  TwistExp[ax6, x6[t]]]
MatrixForm[e7  =  TwistExp[ax7, x7[t]]]

pS  =  {{0}, {0}, {l/2}, {1}};
pH  =  {{w/2}, {0}, {l}, {1}};
pNS  = {{w}, {0}, {l/2}, {1}};

MatrixForm[
      pSp  =  e1 . e2 . e3 . e4 . e5 . e6 . pS  // 
          FullSimplify]
MatrixForm[
      pHp  =  e1 . e2 . e3 . e4 . e5 . e6 . pH // 
          FullSimplify]
MatrixForm[
      pNSp  =  
        e1 . e2 . e3 . e4 . e5 . e6 . e7 . pNS  // FullSimplify]

PE  =  
      First[M*g*pSp[[3]] + Mp*g*pHp[[3]] + M*g*pNSp[[3]]] // 
        FullSimplify

% /. w -> 0

3D Walker Dynamics

Lagrangian  =  
        KE  -  PE  /. {x1[t] -> 0, x2[t] -> 0, 
            x3[t] -> 0, x1'[t] -> 0, x2'[t] -> 
              0, x3'[t] -> 0} ;

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

Solve[{eq1  == 0 ,  eq2 == 0, eq3 == 0, 
          eq4 == 0}, {x4''[t], x5''[t], x6''[t], x7''[
            t]}];

Without a hip:

Solve[{eq1  == 0  /. w -> 0,  
          eq2 == 0  /. w -> 0, eq3 == 0  /. w -> 0, 
          eq4 == 0  /. w -> 0}, {x4''[t], x5''[
            t], x6''[t], x7''[t]}]  // FullSimplify

Impact Equations

pNSFoot  = {{w}, {0}, {0}, {1}};
MatrixForm[
      pNSFootp  =  
        e1 . e2 . e3 . e4 . e5 . e6 . e7 . pNSFoot  // 
          FullSimplify]

Pos1  =  First[pNSFootp[[1]]];
Pos2  =  First[pNSFootp[[2]]];
Pos3  =  First[pNSFootp[[3]]];
MatrixForm[
      F  =  FullSimplify[{{D[Pos1, x1[t]], D[Pos1, x2[t]], D[Pos1, x3[t]], 
              D[Pos1, x4[t]], D[Pos1, x5[t]], D[Pos1, x6[t]], 
              D[Pos1, x7[t]]}, 
{D[Pos2, x1[t]], 
              D[Pos2, x2[t]], D[Pos2, x3[t]], D[Pos2, x4[t]], D[Pos2, x5[t]], 
              D[Pos2, x6[t]], 
              D[Pos2, x7[t]]}, 
{D[Pos3, x1[t]], 
              D[Pos3, x2[t]], D[Pos3, x3[t]], D[Pos3, x4[t]], D[Pos3, x5[t]], 
              D[Pos3, x6[t]], D[Pos3, x7[t]]}}]]

MatrixForm[
      nFT  =  Join[-Transpose[F], {{0, 0, 0}, {0, 0, 0}, {0, 0, 
              0}}]]

MatrixForm[
      FullMatrix  =  
        Transpose[
          Join[Transpose[Join[Mmatrix, F]], Transpose[nFT]]]]

MatrixForm[FullMatrix /. w -> 0 // FullSimplify];

MatrixForm[
      DMatrix  =  
        Join[Mmatrix . {0, 0, 
              0, x4'[t], x5'[t], x6'[t], x7'[t]}, {0, 0, 
            0}]]

First, we have the angular positions for 4,5,6,7:

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

We are interested in the angular velocities for 4,5,6,7 (as of now these are incomputable for the hipped walker):

K  =  Inverse[FullMatrix] . DMatrix;

x4dotimpact  =  FullSimplify[K[[4]]]
x5dotimpact  =  FullSimplify[K[[5]]]
x6dotimpact  =  FullSimplify[K[[6]] + K[[7]]]
x7dotimpact  =  -FullSimplify[K[[7]]]

Without Hip

FullMatrix  =  
        FullMatrix  /.  w -> 0  // 
          FullSimplify;
DMatrix  =  
        DMatrix  /.  w -> 0  // 
          FullSimplify;
K  =  Inverse[FullMatrix] . DMatrix;

x4dotimpact  =  FullSimplify[K[[4]]]
x5dotimpact  =  FullSimplify[K[[5]]]
x6dotimpact  =  FullSimplify[K[[6]] + K[[7]]]
x7dotimpact  =  -FullSimplify[K[[7]]]

Collision Guard

Note: gamma is the slope angle

height  =  
      First[pNSFootp[[3]]  +  
            Tan[gamma]*pNSFootp[[2]]  /.  {x2[t] -> 0, 
            x3[t] -> 0}]

Avect  =  {{D[height, x4[t]], D[height, x5[t]], D[height, x6[t]], 
            D[height, x7[t]]}};
holonomicTraj  =  
      First[First[
            Avect . {{x4'[t]}, {x5'[t]}, {x6'[t]}, {x7'[
                    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