Snakeboard modeling
Andrew D. Lewis
Department of Mathematics and Statistics
Queen's University
Kingston ON K7L 3N6
Canada

andrew@mast.queensu.ca

This is a Mathematica implementation of the modeling for the snakeboard.

Reference: Geometric Control of Mechanical Systems.  Modeling, Analysis, and Design for Simple Mechanical Control Systems
                      Francesco Bullo and Andrew D. Lewis
                       http://penelope.mast.queensu.ca/smcs/
       August 2004

<< SMCS.m

Inertia tensors

The snakeboard is comprised of four components, the coupler, the rotor, the ``back'' wheels, and the ``front'' wheels.  These will be denoted with the suffixes ``c,'' ``r,'' ``f,'' and ``b,'' respectively.  In all cases, the frames we consider will be those described in the text.  Let us first define the inertia tensors for the four bodies.

Ic  =  {{a11, a12, 0}, {a12, a22, 0}, {0, 0, Jc}}

Ir  =  {{a11, a12, 0}, {a12, a22, 0}, {0, 0, Jr}}

Ib  =  If1  =  {{a11, a12, 0}, {a12, a22, 0}, {0, 0, 
            Jw}}

Note that we are forced to use ``If1'' since ``If'' is an internal Mathematica symbol.  Also note that the parameters a11, a12, and a22 are dummies, and should not show up in the final expressions by virtue of the way in which the frames are defined.

Forward kinematic maps

Now we define the forward kinematic maps for the four bodies by defining the positions of the centers of mass from the spatial origin, and by defining the orientation of the body frames relative to the spatial frame.  Thus this step amounts to defining four vectors in $\real^3$ and four matrices in $\SO{3}$.  First we need coordinates and their velocities.  We use the ones from the text, of course.

conf  =  {x[t], y[t], theta[t], psi[t], phi[t]}

vel  =  D[conf, t]

Now for the forward kinematic maps.  The lowercase r is for the center of mass position, and the uppercase R is for the orientation matrices.

rc  =  {x[t], y[t], 0}

rr  =  {x[t], y[t], 0}

rb  =  {x[t] - l Cos[theta[t]], y[t] - l Sin[theta[t]], 
        0}

rf  =  {x[t] + l Cos[theta[t]], y[t] + l Sin[theta[t]], 
        0}

Rc  =  {{Cos[theta[t]], -Sin[theta[t]], 0}, {Sin[theta[t]], 
          Cos[theta[t]], 0}, {0, 0, 1}}

Rr  =  {{Cos[theta[t] + psi[t]], -Sin[theta[t] + psi[t]], 
          0}, {Sin[theta[t] + psi[t]], Cos[theta[t] + psi[t]], 0}, {0, 0, 
          1}}

Rb  =  {{Cos[theta[t] + phi[t]], -Sin[theta[t] + phi[t]], 
          0}, {Sin[theta[t] + phi[t]], Cos[theta[t] + phi[t]], 0}, {0, 0, 
          1}}

Rf  =  {{Cos[theta[t] - phi[t]], -Sin[theta[t] - phi[t]], 
          0}, {Sin[theta[t] - phi[t]], Cos[theta[t] - phi[t]], 0}, {0, 0, 
          1}}

Kinetic energy and the kinetic energy metric

Now we compute the kinetic energies, translational and rotational, for all of the bodies.

ketranc  =  KEtrans[rc, conf, mc, t]

kerotc  =  Simplify[KErot[Rc, conf, Ic, t]]

ketranr  =  KEtrans[rr, conf, mr, t]

kerotr  =  Simplify[KErot[Rr, conf, Ir, t]]

ketranb  =  KEtrans[rb, conf, mw, t]

kerotb  =  Simplify[KErot[Rb, conf, Ib, t]]

ketranf  =  KEtrans[rf, conf, mw, t]

kerotf  =  Simplify[KErot[Rf, conf, If1, t]]

Note that, in the above computations, the arguments ``mc,'' ``mr,'' and ``mw'' are masses.  We can now obtain the total kinetic energy.

KE  =  
      Simplify[ketranc + kerotc + ketranr + kerotr + ketranb + kerotb + 
          ketranf + kerotf]

We can compute the components to the kinetric energy metric.

metric  =  Simplify[KE2Metric[KE, vel]]

We can also compute the Christoffel symbols for the associated Levi-Civita affine connection.

lcgamma  =  LeviCivita[metric, conf]

Forces

For the snakeboard there are only control forces, and there are two of these.  First we consider the force that spins the rotor.  The corresponding Newtonian force is applied to the rotor and the coupler, and so the Lagrangian force will be a sum of these two Newtonian contributions.  First we make a list of the Newtonian forces.  In this case, there are two of these (one each for the coupler and the rotor), and they are both zero.

force1  =  {{0, 0, 0}, {0, 0, 0}}

We now do the same for torques.

torque1  =  {{0, 0, -1}, {0, 0, 1}}

Next we create the Lagrangian force.

F1  =  
      Simplify[Force[torque1, force1, {Rc, Rr}, {rc, rr}, conf, 
          t]]

Let us do the same for the other control force that moves the wheels.

force2  =  {{0, 0, 0}, {0, 0, 0}}

torque2  =  {{0, 0, 1/2}, {0, 0, -1/2}}

F2  =  
      Simplify[Force[torque2, force2, {Rb, Rf}, {rb, rf}, conf, 
          t]]

The constrained connection

The explicit expressions here are a little outrageous, so we suppress much of the output.  The first step is to compute an orthogonal basis of vector fields for which the first vector fields in the list are a basis for the constraint distribution.  This is generally not possible.  For example, for the snakeboard, the constraint distribution does not have constant rank.  Even when the constraint distribution does have constant rank, it is not always possible to find a global basis.  We refer to the text for details surround such discussions.  Here we merely note that, provided one omits consideration of the configurations where the constraint distribution gains rank, it is possible to find a global basis of vector fields for the snakeboard constraint distribution.  First we provide a set of covector fields that annihilate the constraint distribution.

omega1  =  {-Sin[phi[t] + theta[t]], 
        Cos[phi[t] + theta[t]], -l Cos[phi[t]], 0, 0}

omega2  =  {Sin[phi[t] - theta[t]], Cos[phi[t] - theta[t]], 
        l Cos[phi[t]], 0, 0}

By ``sharping'' these relative to the kinetic energy metric, we get two vector fields that are $\metric$-orthogonal to the constraint distribution.  Then we need to ensure that these are $\metric$-orthogonal.

X4  =  RiemannSharp[omega1, metric];

X5t  =  RiemannSharp[omega2, metric];

The next equation is just the Gram\textendash{}Schmidt Procedure.

X5  =  
        X5t - (X4 . metric . X5t) X4/(X4 . metric . X4);

Now we define the three vector fields used in the text as a $\metric$-orthogonal basis for the constraint distribution.

V1  =  {Cos[theta[t]], Sin[theta[t]], 0, 0, 0}

X1  =  l Cos[phi[t]] V1 - Sin[phi[t]] {0, 0, 1, 0, 0}

X2t  =  {0, 0, 0, 1, 0}

X2  =  
        Simplify[((X1 . metric . X1) X2t - (X1 . metric . X2t) 
                  X1)/((mc + mr + 2  mw) 
                  l^2 Cos[phi[t]]^2 + (Jc + Jr + 2 (Jw + mw l^2)) 
                  Sin[phi[t]]^2)];

X3  =  {0, 0, 0, 0, 1}

X  =  {X1, X2, X3, X4, X5};

Let us save the basis for $\dist{D}$ for later use.

Ddim  =  3;

Dbasis  =  Table[X[[i]], {i, Ddim}];

One can check that these vector fields are indeed $\metric$-orthogonal.

Simplify[
      Table[(X[[i]] . metric . X[[j]])/(X[[i]] . metric . 
              X[[i]]), {i, 5}, {j, 5}]]

One can now determine the components of the orthogonal projection onto $\dist{D}^\perp$.  To do this, we first compute the orthogonal projection onto $\dist{D}$.

P  =  
        OrthogonalProjection[{X1/Sqrt[X1 . metric . X1], 
            X2/Sqrt[X2 . metric . X2], X3/Sqrt[X3 . metric . X3]}, 
          metric];

Since we have suppressed the somewhat lengthy explicit expression, let us at least verify that it is what it is supposed to be.

Table[Simplify[P . X[[i]] - X[[i]]], {i, 3}]

Table[Simplify[P . X[[i]]], {i, 4, 5}]

Now we define the projection onto $\dist{D}^\perp$.

Pperp  =  IdentityMatrix[5] - P;

Were one to examine the components of $P_{\dist{D}}$, one would find that each of them has as a denominator the following expression:

den  =  (Jc + Jr + 2  Jw + l^2  mc + l^2  mr + 
              4  l^2  mw - (Jc + Jr + 2  Jw - l^2  mc - l^2  mr) 
                Cos[2  phi[t]])^2  (Jc + 2  Jw + l^2  mc + 
            l^2  mr + 
            4  l^2  mw + (-Jc - 2  Jw + l^2 (mc + mr)) 
              Cos[2  phi[t]])

Since we will be covariantly differentiating $P_{\dist{D}}^\perp$, the computations would simplify if we could get rid of this denominator (no messy quotient rule computations).  It is not obvious how this can be done.  The key is the following fact.  If $A$ is an arbitrary invertible $(1,1)$-tensor field, then we may define the affine connection $\Anabla$ on $\man{Q}$ by
\begin{equation*}
\Anabla_XY=\lcconnection{\metric}_XY+A^{-1}(\lcconnection{\metric}_X(AP_{\dist{D}}^\perp))(Y).
\end{equation*}
It is easy to verify that this affine connection restricts to $\dist{D}$, and that its restriction to $\dist{D}$ agrees with $\cconnection{\dist{D}}$ (see \cite{ADL:97a}).  We apply this formula with $A$ being the identity tensor multiplied by the denominator.  Since one covariantly differentiates $A\scirc P_{\dist{D}}^\perp$ when computing $\Anabla$, the denominator disappears, and the differentiations simplify.

cgamma  =  
        ConstrainedConnection[lcgamma, den IdentityMatrix[5], Pperp, 
          conf];

Now, having computed the Christoffel symbols for the constrained connection, we will do nothing with them symbolically.  They are just too unwieldy.

The data for the orthogonal Poincar\'e representation

In this section we illustrate the use of the orthogonal Poincar\'e representation.  Fortunately, we have already done much of the work, namely the computation of a $\metric$-orthogonal basis for the constraint distribution.  Therefore, we can directly compute the $3^3$ Christoffel symbols that appear in the orthogonal Poincar\'e representation.

ogamma  =  
        Simplify[
          OrthogonalChristoffelSymbols[Dbasis, metric, lcgamma, 
            conf]];

While we suppress the output of Mathematica here, the explicit expressions are not that bad, and indeed are produced in the text.

It is possible to use the generalized covariant derivative to perform controllability computations.  Let us, for example, check that the snakeboard is STLC from points with zero initial velocity.  In the text it is shown that the system is KC, and therefore STLCC.  To show that the system is STLC, we need only show that $\symdist{1}{\dist{Y}}=\dist{D}$ (cf. \fref{the:stlccsuff}).  First we need the components of the vector forces relative to the $\metric$-orthogonal basis.

Y1o  =  OrthogonalForce[F1, Dbasis, metric];

Y2o  =  OrthogonalForce[F2, Dbasis, metric]

Now we compute the symmetric products.

Y1symY1o  =  
      2  Simplify[
          GeneralizedCovariantDerivative[Y1o, Y1o, Dbasis, ogamma, 
            conf]]

Y2symY2o  =  
      2  Simplify[
          GeneralizedCovariantDerivative[Y2o, Y2o, Dbasis, ogamma, 
            conf]]

Y1symY2o  =  
        GeneralizedCovariantDerivative[Y1o, Y2o, Dbasis, ogamma, conf] + 
          GeneralizedCovariantDerivative[Y2o, Y1o, Dbasis, ogamma, 
            conf];

The vanishing of $\symprod{Y_1}{Y_1}$ and $\symprod{Y_2}{Y_2}$ gives the kinematic controllability of the system, as described in the text in \fref{subsec:snakemodeling}.

We now see that $\{Y_1,Y_2,\symprod{Y_1}{Y_2}\}$ generate $\dist{D}$ at all points where $\{X_1,X_2,X_3\}$ generate the constraint distribution (i.e., at points where $\cos\phi\not=0$).

Simplify[Det[{Y1o, Y2o, Y1symY2o}]]

Affine connection control system equations

We will compute equations of motion in two different ways.  First we use the Christoffel symbols for the constrained connection as above, and just produce the full geodesic equations.  First we need to give the input vector fields, properly projected onto the constraint distribution.

Y1c  =  P . RiemannSharp[F1, metric];

Y2c  =  P . RiemannSharp[F2, metric]

The total input is a linear combination of the two inputs, with the coefficients being the controls.  Let us leave the controls as general for the moment.

Yc  =  u1 Y1c + u2 Y2c;

Now we can produce the equations of motion.

eqmot1  =  ACCSequations[cgamma, Yc, conf, t];

Poincar\'e equations

First one defines the ``full'' set of pseudo-velocities.  In this case there are three.

pv  =  {pv1[t], pv2[t], pv3[t]}

Then one extracts the state for the equations, properly taking into account that some of the pseudo-velocities are velocities.

state  =  GetState[Dbasis, pv, conf, t]

Note that the second and third pseudo-velocities are actual velocities, and so do not appear in the list of states.

Next we give the vector force.

Yo  =  u1 Y1o + u2 Y2o;

Now we can formulate the equations of motion.

eqmot2  =  
        SMCSequations[ogamma, Yo, Dbasis, pv, conf, t];

Simulation

First let us give numerical values to the parameters.

params  =  {Jc -> 1/2, Jw -> 1/8, Jr -> 3/4, 
        mc -> 1/2, mr -> 3/4, mw -> 1/4, 
        l -> 1/2}

Now define specific controls.

u1  =  2  Sin[3  t]

u2  =  2  Sin[2  t]

Next define the initial and final times for the simulation.

Ti  =  0

Tf  =  3  Pi

Now the initial conditions.

qinit  =  vinit  =  {0, 0, 0, 0, 0}

Now simulate.

sol1  =  
      ACCSsimulate[(eqmot1 /. params), conf, qinit, vinit, t, Ti, 
        Tf]

Plot[x[t] /. sol1, {t, Ti, Tf}]

Now we simulate the system as a Poincar\'e representation.  First we need to compute the initial pseudo-velocities.

pvinit  =  
      Table[vinit . metric . 
            X[[i]]/(X[[i]] . metric . X[[i]]), {i, 
          Ddim}]

Now simulate.

sol2  =  
      SMCSsimulate[(eqmot2 /. params), Dbasis, conf, pv, qinit, pvinit, 
        t, Ti, Tf]

Plot[x[t] /. sol2, {t, Ti, Tf}]

The two solution methods give the same solutions to the differential equation, as expected.