Wolfram Cloud Document

Abstract

We compare three numerical solutions for free motion of an inverted spherical pendulum. Our contribution is to find that only the discrete variational solution exhibits long-term, secular energy conservation at acceptably large time steps. By contrast, numerical integration of the continuous equations of motion exhibits secular energy growth, even at unacceptably small time steps.

The discrete variational approach begins by discretizing dynamical variables and the Lagrangian, replacing continuous functions of time with sequences of discrete quantities. We call this early discretization. Appropriate derivatives of the discrete Lagrangian produce algebraic recurrences that we solve by root-finding. The standard approach delays discretizing continuous functions as late in the process as possible, typically inside opaque solvers for differential equations. We call this late discretization. We find two late-discretized solutions with undesirable secular energy growth. We speculate that all late-discretized solutions will have incorrect secular trends of conserved quantities.

Details

When dropped from rest at co-latitude, say, 5 degrees, the expected behavior is for the pendulum to swing through the nadir, rise to 355 degrees in the back hemisphere, then fall back around again and repeat. This large-swing motion is convenient for visually checking energy conservation. If the pendulum swings up too far in the back, the solution is gaining energy through secular numerical noise.

Runge-Kutta integration of quaternions at a 1-ms time step immediately gains enough energy to swing all the way through zenith on the first drop. Only by going to 100-μs time step is there even short-term energy conservation. At 100-μs time step, the solution is far slower than real-time, unacceptable for animation.

In pitch/cone-roll coordinates, and with Mathematica’s automatic (and opaque) choices for time step, secular energy conservation is much better, plus we get animation speed. However, there is still secular energy growth. The solution will eventually require rebooting.

By contrast, the early-discretized solution—the discrete variational solution—has no discernable secular energy growth, even at 1-ms time step. There is periodic energy noise in the fourth significant figure, but this is not visible in plots or animations. The periodic noise grows in amplitude, so the solution will eventually require rebooting, but for a different reason. Solution time is much longer than we would like. However, the attractiveness of secular energy conservation warrants further optimization and study of this method.

This notebook is part of a series on discrete variational dynamics. Previous notebooks illustrated harmonic oscillators and the spinning symmetrical top.

Introduction

Our spherical pendulum is a massive baton of uniform density resting on a massless, sliding, point-like base. The base slides on a ghostly floor: the baton falls through the floor in either direction. When dropped from near upright, the baton falls under its own weight, sliding the base back and then forth. The baton then swings through nadir and back up again via its inertia, sliding the base forth and then back.

In this notebook, we consider only free motion. Later notebooks will address controlling the baton by applying forces to the base.

The spherical pendulum is surprisingly difficult given that the circular pendulum, with one angle and one position coordinate, is an undergraduate exercise with easily solvable optimal control

[15]

The spherical pendulum is the subject of at least one doctoral thesis

[7]

An internet search for “control inverted spherical pendulum” produces many references.

For the spherical problem:

◼

Standard spherical coordinates are ruled out because they are singular at the North pole, exactly where we want to balance and control the baton. Control models diverge in these coordinates.

◼

Quaternions have no singularity, but with fourth-order Runge-Kutta (RK4) or Runge-Kutta-Fehlberg (RK45), solutions fail spectacularly, even with 1-ms time step. In this notebook, the integrators are conclusively shown at fault.

◼

Pitch/cone-roll coordinates, developed in this notebook, avoid singularity at the North pole (the control set-point). Continuous equations in these coordinates behave better under late discretization, i.e., standard numerical methods. Numerical solutions in these coordinates have adequate but imperfect secular energy conservation. While small, secular energy growth is a signal that late discretization is fundamentally flawed. Also, while Mathematica’s NDSolve works well, it does not export out of Mathematica, so we can’t use it for deployed solutions.

◼

Finally, we consider a discrete Lagrangian integrator in pitch/cone-roll coordinates. This has great long-term energy conservation. Its periodic energy noise—in the fourth decimal place—is much larger than the secular-growth noise of the continuous case—in the sixth or seventh decimal place—but still not noticeable visually.

Outline

late discretization: Integrate continuous Euler-Lagrange equations of motion in quaternions via RK4 and RK45.

Demonstrate fast energy divergence.

late discretization: Integrate continuous Euler-Lagrange equations in pitch/cone-roll coordinates.

Demonstrate slow energy divergence.

early discretization: Develop discrete Euler-Lagrange equations in pitch/cone-roll coordinates. Solve resulting recurrences via root-finding.

Demonstrate secular energy conservation despite periodic noise.

Quaternions, RK4, RK45

Quaternions do not have the gimbal lock inherent to Euler angles, which are singular at the North pole. With fourth-order Runge-Kutta (RK4) or Runge-Kutta-Fehlberg integration (RK45), we get plausible results on a unit test of rigid-body free rotation, spontaneously exhibiting the intermediate-axis (Dzhanibekov) effect

[8]

Thus, we trust the integration scheme.

However, RK4 and RK45 fail dramatically on the spherical pendulum, even with a 1-ms time step. They require 100-μs granularity to conserve energy and momentum, even in the short run. Such failure is not unexpected, but the drama is surprising—despite doing a good job on rigid-body rotation, Runge-Kutta integration is not even close for the spherical pendulum.

Tiny Quaternionic Dynamics Library (TQDL)

See Reference [1].

Primitives

Most items have two names: one long and one short. The long names bulk up code, so we use the short ones most of the time. The long names explain and remind us of the short ones.

In[]:=

<<Quaternions`ClearAll[rq,rq0,ranv,ranθ,ranrq,rqw,vq,wrq,θrq,vrq,θvrq,qv,rv,rf,versor,random3Vector,randomAngleRad,randomVersor,versorFromTwistVector,realPart,twistVectorFromVersor,twistAngleFromVersor,twistAngleAndRealPartFromVersor,pureQuaternion,rotatedVector,vectorInRotatedFrame,normalize];

A versor is a rotation quaternion, representing an angular twist around a unit 3-vector axis of rotation. rq is short for versor.

In[]:=

versor:=

;

rq can take a zero vector as input. Normalize does not fail on zero input. Results are not valid versors, though this library improperly allows them. It does not seem to be a problem in practice.

Ignore the error caret in the following definition because Sequence expands correctly to three arguments.

In[]:=

[θ_?NumberQ,v_List]:=Quaternion[Cos[θ/2],Sequence@@(Sin[θ/2]Normalize[v])];

Overload of rq for four numerical arguments:

In[]:=

[θ_?NumberQ,x_?NumberQ,y_?NumberQ,z_?NumberQ]:=rq[θ,{x,y,z}];

It's best to have a canonical 0-rotation quaternion about a non-zero, but arbitrary, axis.

In[]:=

rq0

=rq[0,{1,0,0}];

In[]:=

random3Vector:=ranv;

ranv

[]:=RandomReal[{-1,1},3];

In[]:=

randomAngleRad:=ranθ;

ranθ

[]:=RandomReal[{0,2π}];

In[]:=

randomVersor:=ranrq;

ranrq

[]:=rq[RandomReal[2π],ranv[]];

In[]:=

versorFromTwistVector:=rqw;

rqw

[w_List]:=rq[Norm[w],w];

Every quaternion, not just a versor, has a real part, a 3-vector:

In[]:=

realPart:=vq;

vrq

:=vq;

[q_Quaternion]:=List@@q[[2;;4]];

In[]:=

twistVectorFromVersor:=wrq;

wrq

[rq_Quaternion]:=2ArcCos[rq[[1]]]Normalize@vq@rq;

In[]:=

twistAngleFromVersor:=θrq;

θrq

[rq_Quaternion]:=2ArcCos[rq[[1]]];

In[]:=

twistAngleAndRealPartFromVersor:=θvrq;

θvrq

[rq_Quaternion]:={θrq[rq],vrq[rq]};

A pure quaternion has no imaginary part. It represents an arbitrary 3-vector in the space of quaternions.

In[]:=

pureQuaternion:=qv;

... another incorrect error caret because Sequence expands to three arguments ...

In[]:=

[v_List]:=Quaternion[0,Sequence@@v];

Rotate a vector-as-pure-quaternion by acting on it with versors.

In[]:=

rotatedVector:=rv;

[rq_Quaternion,v_List]:=vq[rq**qv[v]**rq];

The quaternion-conjugate action rotates the frame of a 3-vector rather than the 3-vector itself.

In[]:=

vectorInRotatedFrame:=rf;

[rq_Quaternion,v_List]:=vq[rq**qv[v]**rq];

In[]:=

normalize

[q_Quaternion]:=With[{n=Chop[Abs[q]]},If[n0.0,Quaternion[0,0,0,0],(*?shouldbe"rq0"?*)q/Abs[q]]]

Time Derivatives

In[]:=

ClearAll[drqb2sdt,dωbdt,rk4,ωbn,dVersorDt,dBodyAngVelDt,stepVersorFromBodyAngVel,stepAngVelBodyFrame];

The usual definition of

q/t

in the inertial space frame is

. My angular velocity,

, is in the body frame. But

is in the inertial space frame, so

q/t=

because

q=1

for a versor, by definition.

In[]:=

dVersorDt:=drqb2sdt;(*ignoret*)

drqb2sdt

[t_,qb2s_Quaternion,ωb_List]:=(qb2s/2)**qv[ωb];

In[]:=

dBodyAngVelDt:=dωbdt;(*ignoret*)

dωbdt

[t_,ωb_List,mi_List,mii_List,τs_,rq_]:=mii.(rf[rq,τs]-ωb(mi.ωb));

Quaternion From Euler Angles ψ, θ, ϕ

Mnemonics: θ looks like pitch, ϕ looks like roll, ψ looks like ‘y’ in ‘yaw.’

Via page 206 of Kuipers

[1]

In[]:=

yawQ[ψ_]:=Withα=

,Quaternion[Cos[α],0,0,Sin[α]];pitchQ[θ_]:=Withβ=

,Quaternion[Cos[β],0,Sin[β],0];rollQ[ϕ_]:=Withγ=

,Quaternion[Cos[γ],Sin[γ],0,0];

For OBJECT ROTATIONS, compose these in the forward order (right-to-left). Reverse the order, left-to-right, for FRAME ROTATIONS:

In[]:=

Q[ψ_,θ_,ϕ_]:=rollQ[ϕ]**pitchQ[θ]**yawQ[ψ];

Yaw and roll should be within the ranges

-π

. Pitch should in the range

-π/2

π/2

. Numerical roblems occur near the endpoints of those ranges, especially for pitch.

Euler Angles ψ, θ, ϕ from Quaternions

Via Reference 29, tailored (hacked) to our use-cases, with unit test.

In[]:=

ClearAll[eulerAnglesFromQ,ψθϕFromQ];eulerAnglesFromQ[Quaternion[w_,x_,y_,z_],{i_,j_,k_}/;i!=j&&j!=k]:=Module{a,b,c,d,ε,notProper=True,θ1,θ2,θ3,θPlus,θMinus,ψ,θ,ϕ},If[i==k,notProper=False;k=6-i-j];ε=(i-j)(j-k)(k-i)2;If[notProper,a=w-y;b=x+zε;c=y+w;d=zε-x,(*else*)a=w;b=x;c=y;d=zε];θ2=ArcCos2

-1;θPlus=ArcTan[b,a];θMinus=ArcTan[d,c];If[θ2==0,θ1=0,θ3=2θMinus-θ1;(*else*)If[θ2==N[π]/2,θ1=0;θ3=2θMinus+θ1,(*else*)θ1=θPlus-θMinus;θ3=θPlus+θMinus]];ψ=Mod[-If[notProper,εθ3,θ3],2π]-π;θ=If[notProper,θ2-π/2,θ2];ϕ=Mod[-θ1-π,2π]-π;{ψ,θ,ϕ};

ψθϕFromQ

[q:Quaternion[w_,x_,y_,z_]]:=eulerAnglesFromQ[q,{3,2,1}];With[{randomizeAngles={RandomReal[{

-π,π

}],RandomReal[{

-π/2,π/2

}],RandomReal[{

-π,π

}]}&},DynamicModule[{ψ,θ,ϕ},{ψ,θ,ϕ}=randomizeAngles[];Manipulate[Grid[{{{ψ,θ,ϕ},ψθϕFromQ[Q[ψ,θ,ϕ]]},{ListLinePlot[Table[ψθϕFromQ[Q[ψm,θ,ϕ]][[1]],{ψm,-π,π,0.02π}]]},{ListLinePlot[Table[ψθϕFromQ[Q[ψ,θm,ϕ]][[2]],{θm,-π/2,π/2,0.02π}]]},{ListLinePlot[Table[ψθϕFromQ[Q[ψ,θ,ϕm]][[3]],{ϕm,-π,π,0.02π}]]}}],Control[Button["REFRESH RANDOMS! ",({ψ,θ,ϕ}=randomizeAngles[])]]],SaveDefinitions->True]]

Free Rotation Benchmark for TQDL & RK4

This section contains unit tests for TQDL & RK4.

Mathematica MomentOfInertia Is Not Moment of Inertia

Free Motion Demo (TQDL & RK4)

With a time step of 10 ms, this slowly leaks energy and angular momentum.

See the magenta arrow wiggle a little when the apparatus flips. This is a bad sign. It means that the computation of angular momentum in the inertial frame has numerical trouble. It spontaneously corrects, but this trouble needs deeper investigation.

The arguments of runSimForcedRotationalMotion are the apparatus to display, the initial angular velocity in the body frame, and the initial orientation quaternion.

Notice that the values of kinetic energy and magnitude of angular momentum do not depend on initial orientation.

Accumulating Energy and Angular Momentum

This rapidly accumulates kinetic energy and average angular momentum. In fact, the dropped pendulum immediately goes all the way around with a time step of 10 ms, an unphysical result. The integration also diverges with a time step of 1 ms, as you may check by changing dt below. It is unpleasantly slow to watch, but we invite you to try it. The time step must be reduced by a factor of 100 to 100 μs to prevent swing-around. At that time step, the animation is intolerably slow, taking 10 minutes or so to swing up. In any event, a real pendulum with a frictionless mount would not accumulate energy or angular momentum. We shall see that the discrete integrator has no such energy accumulation.

Addressing the Mystery

First, the gravitational torque is qualitatively wrong. It should be zero when pitch is 0, with the baton upright or dangling. This is the first sign of trouble.

The plots show steadily increasing angular velocity and kinetic energy, plus a mismatch between kinetic energy and the integral of its derivative, which should always be equal.

The plots for yaw and roll coincide. All angles exhibit discontinuities due merely to our choices for branch cuts in conversion from quaternions to Euler angles. The quaternions vary smoothly, as shown by the smoothness of the torque-versus-pitch graph and by the animation.

The “whiplash” in torque and pitch below are artificial, due to discontinuities in converting quaternions to Euler angles. We’ll see the same phenomenon in the discrete solution. We could change the conversions, but it’s just as easy to interpret the angles properly. One should imagine the concave portions of the pitch and torque plots flipped upside down, resulting in a smooth, sinusoidal signal.

With timestamp reduced to 100 μs, it takes several minutes to generate the plot, so I save only a screenshot.

Conclusion:

The integrator is guilty.

Because decreasing the time increment results in physically plausible behavior and in better energy conservation, we deem the dynamics correct.

Discrete Dynamical Integrator

What are good generalized coordinates and velocities? Spherical coordinates have a singularity at the North pole, right where we want to control the baton. A better choice are pitch and cone roll (not axis roll!): co-latitude δ and angular displacement η at a clockwise right angle to the great-circle arc of co-latitude. For any non-zero δ, spinning η makes a non-great circle on the surface around a singularity on the Equator. The demonstration below makes this visually clear. Both δ and η are non-singular at the North pole. η has two singularities on the Equator, when δ is 90° or 270°. But that situation is better than a singularity at the North pole because the baton will fly through these singularities, not linger at them.

The actual physical system is the center of mass of the baton, swinging around the origin. The sliding cart in the animations is a visual fiction. The center of mass of the baton goes up and down.

Demonstration of the Coordinate System

Late Discretization: Numerical Solution of Continuous Equations

Plot the Solutions

The angles and their derivatives exhibit discontinuities as we saw with quaternions, due merely to choice of branch cuts. The animation below shows smooth motion, plus the pitch angle, here, is visually identical to the pitch-angle plot for the 100-μs quaternion solution.

Phase Portrait

Plot the angles versus their velocities, proportional to their Hamiltonian conjugate momenta. If the solution were conservative, these phase portraits would be thin lines. When the time constant is long (400 seconds, here), however, they thicken up, showing that the solution is slightly non-conservative, but it’s much better than Runge-Kutta quaternions. This is going to be a good ground-truth.

Ground-Truth Animation

This is pretty convincing. Energy is well conserved. We’d only have to reboot it after a long time. If we were satisfied with an integrator that works only inside Mathematica, we’d be just about done. But we want an integrator we can code up in any programming language.

Early Discretization

Discretize the dynamical variables and the Lagrangian, itself, via Equation 7 of Reference [10]. First, a reminder:

Discrete Euler-Lagrange Equations

The discrete Euler-Lagrange equations, without generalized force terms, are

All we need is FindRoot, which we could replace with an explicit Newton-Raphson solver in C, C++, Fortran, or Python, for instance. We bootstrap the integrator by taking the first two positions from the late-discretized solution

Notice that the angles are qualitatively the same as for the late-discretized solution.

Energies of the Late Discretization

From the late-discretized solution, we calculate an energy of 11.5490. It slowly drifts upward in the fifth decimal place:

Conclusion

Eventually, the late-discretized solution would accumulate energy, just like the disastrous quaternion-and-Runge-Kutta solution, and would have to be rebooted. We see, again, that early discretization conserves energy much better than late discretization, though with a much larger periodic error. That periodic error is obviously growing in amplitude in the plot, and would eventually require a reboot, too, but for a very different reason.

References

This is a list of general references on discrete variational dynamics and the background mathematics. Not all these references are cited directly in this notebook.

Jack B Kuipers, Quaternions and Rotation Sequences, Princeton University Press, 1999.

Jeongseok Lee, Karen Liu, Frank C. Park, Siddartha S. Srinivasa, A Linear-Time Variational Integrator for Multibody Systems, 2018, https://arxiv.org/abs/1609.02898

Ari Stern, Discrete Geometric Mechanics and Variational Integrators, 2006, http://ddg.cs.columbia.edu/SIGGRAPH06/stern-siggraph-talk.pdf.

Ethan Eade, Lie Groups for 2D and 3D Transformations, 2017, http://ethaneade.com/lie.pdf.

NIST: Digital Library of Mathematical Functions, https://dlmf.nist.gov.

Brian C. Hall, Lie Groups, Lie Algebras, and Representations, Second Edition, 2015, Springer.

Guangyu Liu, Modeling, stabilizing control and trajectory tracking of a spherical inverted pendulum, PhD thesis, 2007, University of Melbourne,
https://minerva-access.unimelb.edu.au/handle/11343/37225.

Wikipedia, Tennis-Racket Theorem, https://en.wikipedia.org/wiki/Tennis_racket_theorem.

Marin Kobilarov, Keenan Crane, Mathieu Desbrun, Lie Group Integrators for Animation and Control of Vehicles, 2009 (?).

Ari Stern, Mathieu Desbrun, Discrete Geometric Mechanics for Variational Time Integrators, 2006 (http://www.geometry.caltech.edu/pubs/SD06.pdf).

Kenth Engø, On The BCH Formula in (3), https://www.researchgate.net/profile/Kenth_Engo-Monsen2/publication/233591614_On _the _BCH-formula_in _so3/links/004635199177f69467000000/On-the-BCH-formula-in-so3.pdf.

Alexander Van-Brunt, Max Visser, Explicit Baker-Campbell-Hausdorff formulae for some specific Lie algebras, https://arxiv.org/pdf/1505.04505.pdf.

Wikipedia, Baker-Campbell-Hausdorff Formula, https://en.wikipedia.org/wiki/Baker%E2%80%93Campbell%E2%80%93Hausdorff_formula

Jerrold E. Marsden, Tudor S. Ratiu, Introduction to Mechanics and Symmetry, Second Edition, 2002

Moylan, Andrew, Stabilized Inverted Pendulum, https://blog.wolfram.com/2011/01/19/stabilized-inverted-pendulum/, and Stabilized n-Link Pendulum, https://blog.wolfram.com/2011/03/01/stabilized-n-link-pendulum/

Paul R. Halmos, Naive Set Theory, 2015.

David Lovelock and Hanno Rund, Tensors, Differential Forms, and Variational Principles, 1975.

Ralph Abraham and Jerrold E. Marsden, Foundations of Mechanics: 2nd Edition, 1980.

Taeyoung Lee, Melvin Leok, N. Harris McClamroch, Discrete Control Systems, https://arxiv.org/abs/0705.3868.

Taeyoung Lee, Melvin Leok, N. Harris McClamroch, Global Formulations of Lagrangian and Hamiltonian Mechanics on Manifolds, Springer, https://a.co/d/0lTbL9t.

Tristan Needham, Visual Differential Geometry and Forms, https://a.co/d/9hoKekz.

xAct, Efficient tensor computer algebra for the Wolfram Language, http://xact.es/index.html.

Blanco, Jose-Luis, A tutorial on SE(3) transformation parameterizations and on-manifold optimization, Technical Report #012010, Universidad de Malaga (https://jinyongjeong.github.io/Download/SE3/jlblanco2010geometry3d_techrep.pdf).

Marsden and West has exhaustive treatment of Noether’s Theorem (http://www.cds.caltech.edu/~marsden/bib/2001/09-MaWe2001/MaWe2001.pdf).

Maxime Tournier, Notes on Lie Groups, https://maxime-tournier.github.io/notes/lie-groups.html.

John Baez, Javier P. Muniain, Gauge Fields, Knots and Gravity, (https://a.co/d/jdRnOU0).

F. C. Park, J. E. Bobrow, S. R. Ploen, A Lie Group Formulation of Robot Dynamics.

George W. Hart, Multidimensional Analysis, https://a.co/d/8c5RqOX

Evandro Bernardes, Stephane Viollet, Quaternion to Euler angles conversion: A direct, general and computationally efficient method, PLoS ONE 17(11) e0276302

CITE THIS NOTEBOOK

Discrete variational spherical pendulum: free motion
by Brian Beckman
Wolfram Community, STAFF PICKS, January 19, 2025
https://community.wolfram.com/groups/-/m/t/3359568