Demo for SMCS.m
Andrew D. Lewis
Department of Mathematics and Statistics
Queen's University
Kingston ON K7L 3N6
Canada

andrew@mast.queensu.ca

This is a set of Mathematica commands for modeling of simple mechanical systems.  This is an initial implementation, and will hopefully be improved upon as time goes by.

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

Note that the packages \package{Tensors.m} and \package{Affine.m} are loaded.  We refer the reader to their documentation for instructions on using commands from these packages.

To get help, type ?command

?OrthogonalChristoffelSymbols

Rigid body modeling

The rolling disk is comprised of a single body.  Let us first define the inertia tensor of the body.

Iten  =  {{Jspin, 0, 0}, {0, Jspin, 0}, {0, 0, Jroll}}

Now we define the forward kinematic map for the body by defining the position of the center of mass from the spatial origin, and by defining the orientation of the body frame relative to the spatial frame.  Thus this step amounts to defining a vector in $\real^3$ and a matrix in $\SO{3}$.  In specific examples, Mathematica can be useful in obtaining these expressions.  For the rolling disk, the derivation of the orientation matrix is not entirely trivial, and we refer the reader to \fref{eg:rolling-diskQ} for details.  First we define the configuration space coordinates and their velocities.

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

vel  =  D[conf, t]

We define the coordinates as ``functions of time'' in Mathematica.  We shall see that having the coordinates as functions is essential to using some of the macros defined in \package{SMCS.m}.

Now for the forward kinematic map.

r  =  {x[t], y[t], rho}

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

It is now possible to compute a multitude of things, since, as we emphasize in the text, the forward kinematic maps are key to much of our modeling.  For example, one can compute body and spatial angular velocities.

Simplify[BodyAngularVelocity[R, conf, t]]

Simplify[SpatialAngularVelocity[R, conf, t]]

Note that we do require the coordinates to be functions of time here, since time is one of the arguments of the angular velocity commands.

Kinetic energy and the kinetic energy metric

Now we compute the kinetic energy, translational and rotational, for the body.  Again, the forward kinematic map is key, and again, we do require the configuration space coordinates to be functions of time.

ketran  =  KEtrans[r, conf, m, t]

kerot  =  Simplify[KErot[R, conf, Iten, t]]

Note that, in the above computations, the argument ``m'' is the mass.  We can now obtain the total kinetic energy.

KE  =  Simplify[ketran + kerot]

Now we can compute the components of the kinetric energy metric.

metric  =  Simplify[KE2Metric[KE, vel]]

We can also compute the Christoffel symbols for the associated Levi-Civita affine connection, although these are trivial in this case.

lcgamma  =  LeviCivita[metric, conf]

Force modeling

Now we consider the modeling of forces using the approach in the text.  This is simple given the forward kinematic map.  For the rolling disk, there are only control forces, and there are two of these.  First we consider the force that spins the disk.  The Newtonian force and torque are first defined.

force1  =  {{0, 0, 0}}

We now do the same for torques.

torque1  =  {{0, 0, 1}}

Note that the Newtonian force and torque are a list of vectors in $\real^3$.  The length of the list is the number of bodies the force and torque act on, with each entry in the list corresponding to the force and torque exerted on a single one of the bodies.  In this case, there is just one body, so the list has length one.  See \fref{sec:snakeboardMma} for an example with multiple bodies.

Next we create the Lagrangian force.

F1  =  Simplify[Force[torque1, force1, {R}, {r}, conf, t]]

Note that the first four arguments are lists whose length is the number of bodies the Newtonian force and torque interact with.  Let us do the same for the other control force that moves the wheels.

force2  =  {{0, 0, 0}}

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

F2  =  Simplify[Force[torque2, force2, {R}, {r}, conf, t]]

Nonholonomic constraint modeling I

Next we turn to the modeling of nonholonomic constraints as described in the text.  There are many ways one can do this.  For example, one can use the constrained connection by computing its Christoffel symbols.  Let us illustrate the steps.  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.  For the rolling disk, it turns out that there is a global orthogonal basis for $\dist{D}$.  This is generally not the case.  For example, it might be the case that the constraint distribution does not have constant rank.  And, if the constraint distribution does have constant rank, there still might not be a global basis.  However, since we are in luck here, we can proceed without misadventure.  First we provide a set of covector fields that annihilate the constraint distribution.

omega1  =  {1, 0, 0, -rho Cos[theta[t]]}

omega2  =  {0, 1, 0, -rho Sin[theta[t]]}

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.

X3  =  RiemannSharp[omega1, metric]

X4t  =  RiemannSharp[omega2, metric]

The next formula is the Gram\textendash{}Schmidt Procedure to get an orthogonal basis.

X4  =  
      Simplify[X4t - (X3 . metric . X4t) 
            X3/(X3 . metric . X3)]

Note that we will not actually do much with X3 and X4, but we produce them anyway, just to show how one does these orthogonal basis computations.

Now we use the two vector fields defined in the text as being a $\metric$-orthogonal basis for the constraint distribution.

X1  =  {rho Cos[theta[t]], rho Sin[theta[t]], 0, 1}

X2  =  {0, 0, 1, 0}

X  =  {X1, X2, X3, X4}

One can check that these vector fields are indeed orthogonal.

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

One can now determine the components of the orthogonal projection onto $\dist{D}^\perp$.  To do this, it is less cumbersome if we compute the orthogonal projection onto $\dist{D}$ first.  Note that this only requires the basis for $\dist{D}$, and that this basis needs to be orthonormal for the macro OrthogonalProjection.

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

Let us at least verify that this is actually the $\metric$-orthogonal projection onto $\dist{D}$.

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

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

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

Pperp  =  Simplify[IdentityMatrix[4] - P]

Let us record the orthogonal basis for $\dist{D}$ for future use.

Ddim  =  2

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

Now we compute the Christoffel symbols for the constrained connection.  The identity matrix in the second argument seems to be out of place here.  The meaning of this second argument, along with an example of how it is used, can be found in \fref{sec:snakeboardMma}.

cgamma  =  
      ConstrainedConnection[lcgamma, IdentityMatrix[4], Pperp, 
        conf]

Nonholonomic constraint modeling II

In this section we illustrate the method for handling nonholonomic constraints that normally works best in practice, namely using the orthogonal Poincar\'e representation.  Here we only compute the minimum number of Christoffel symbols.  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 $2^3$ Christoffel symbols that appear in the orthogonal Poincar\'e representation.

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

It is possible to covariantly differentiate vector fields taking values in the constraint distribution using the orthogonal Christoffel symbols.  In fact, the command for doing this will work even if the vector fields forming the basis for the constraint distribution are not orthogonal.  To execute the command, one needs to represent vector fields with values in the constraint distribution.  This is done by giving their components relative to the basis vector fields.  Let us define two such vector fields in general form.

U  =  
      Table[Ucomp[i][x[t], x[y], theta[t], phi[t]], {i, Ddim}]

V  =  
      Table[Vcomp[i][x[t], x[y], theta[t], phi[t]], {i, Ddim}]

Now we covariantly differentiate V with respect to U.

GeneralizedCovariantDerivative[U, V, Dbasis, ogamma, conf]

It is possible to use the generalized covariant derivative to perform controllability computations.  An example of this is given in \fref{sec:snakeboardMma}.

We also need to model the forces in the framework of pseudo-velocities.  Two things must be done to do this.  First, the vector forces need to be projected onto the constraint distribution.  Then the resulting vector forces need to be represented in terms of the (not necessarily $\metric$-orthogonal) basis for the constraint distribution.  In the case when the basis for $\dist{D}$ is $\metric$-orthogonal, there is a command for this.

Y1o  =  OrthogonalForce[F1, Dbasis, metric]

Y2o  =  Simplify[OrthogonalForce[F2, Dbasis, metric]]

Equations of motion I

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  =  Simplify[ACCSequations[cgamma, Yc, conf, t]]

Equations of motion II

Now we provide another means of producing the equations of motion, using the Poincar\'e representation.  Since this representation, in principle, captures all possibilities, one must allow for both constrained and unconstrained cases.  One of the differences will be that, in the unconstrained case with the natural Christoffel symbols, the dependent variables will be the configuration coordinates, and all equations will be second-order.  For systems with constraints, and using generalized Christoffel symbols, there will be pseudo-velocities, and the equations will be first-order.  Things are further complicated by the fact that, in some examples, some of the pseudo-velocities will be actual velocities.  Thus the resulting equations of motion will be a mixture of first- and second-order equations.  The difficulty is then to determine the correct state, taking into account that some pseudo-velocities are actual velocities.  There is a command for this, whose usage we now illustrate.  First one defines the ``full'' set of pseudo-velocities.  In this case there are two.

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

Then one extracts the state for the equations, properly taking into account that some of the pseudo-velocities are velocities.  The following command does not require a $\metric$-orthogonal basis for $\dist{D}$.

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

Note that, in the rolling disk, all pseudo-velocities are actual velocities, reflected by the fact that no pseudo-velocities appear in the list of states.

If the system were unconstrained and one wished to use the natural representation, then one would proceed as follows.

GetState[IdentityMatrix[4], {pv1[t], pv2[t], pv3[t], pv4[t]}, conf, 
      t]

The first argument being the identity matrix corresponds to the fact that the pssudo-velocities are all real velocities.  Then the state is correctly returned as simply the configuration coordinates.  In such cases one may want to not bother with listing the pseudo-velocities, in which case an empty list will guarantee the correct result.

GetState[IdentityMatrix[4], {}, conf, t]

A second difficulty arises with the treatment of forces.  In unconstrained systems, one simply wants to use the natural representation of the force.  For constrained systems using pseudo-velocities, one must properly represent vector forces as above.  Therefore, the user is required to define a vector force being applied to the system by giving its components in the basis for $\dist{D}$.  In this case, we have already done this.

Yo  =  u1 Y1o + u2 Y2o

In the unconstrained case when using the natural representation, one would simply use the list comprised on the components of the vector force.

Now we can formulate the equations of motion.  Note that one uses all pseudo-velocities.  The program sorts out the state along the lines of the GetState command above.  For an unconstrained system, an empty list of pseudo-velocities will give the desired result.  Note that, for the following command, the basis for $\dist{D}$ need not be $\metric$-orthogonal.

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

Simulation

Once one has the equations of motion, one would like to be able to numerically solve the equations.  In Mathematica this is done using NDSolve, but an interface has been provided that simplifies certain things.  First let us give numerical values for the parameters.

params  =  {Jspin -> 2, Jroll -> 1, m -> 1/2, 
        rho -> 1}

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}

It is assumed that the initial velocity satisfies the constraint.

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.  The initial condition is given as initial configuration, plus a complete list of initial pseudo-velocities.  The program converts this into a state initial condition.

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.

Other useful macros

The primary components of \package{SMCS.m} are illustrated above.  But there are a few other macros that are implemented that might be useful.  Let us indicate what these are and what they do.

There are macros that manage the isomorphism between $\so{3}$ and $\real^3$.

omegahat  =  Hat[{w1, w2, w3}]

omega  =  Unhat[omegahat]

There is also an implementation of the Hessian.  The implementation supposes that the function is being evaluated at a critical point, where the matrix representative of the Hessian is simply the matrix of second partial derivatives.

Hessian[f[x[t], y[t], theta[t], phi[t]], conf]

A generally useful macro is SetEqual, which is used to set the components of two lists equal to one another in the form of an equation.

list1  =  Table[l1[i], {i, 3}]

list2  =  Table[l2[i], {i, 3}]

SetEqual[list1, list2]