Dynamics of the Planar Double-Pendulum

Description of the multibody system

revolute joints

unit vector parallel to the revolute joints axis

> cz := make_VECTOR(ground,0,0,1): show(cz);

fixed revolute joint

> A := make_POINT(ground,0,0,0): show(A);

revolute joint between bodies

> B := make_POINT(ground,xB(t),yB(t),0): show(B);

body 1

unit vectors fixed to the body 1

> u1 := 1/lAB*make_VECTOR(A,B): show(u1); along X axis
w1 := rotate('Z',Pi/2,u1):
w1 := project(w1,ground): show(w1); along Y axis

frame fixed to the body

> T1 := linalg[transpose] (matrix(4,4, [ [Comps(u1),0] , [Comps(w1),0] , [Comps(cz),0] , [Comps(A),1] ]));

center of mass

> G1 := make_POINT(T1,a1,b1,0):

properties of the rigid body

> body1 := make_BODY(G1,m1,0,0,IZ1): show(body1);
G1 := project(G1,ground): show(G1);

weight force

> W1 := make_FORCE(ground,0,-m1*g,0,G1,body1): show(W1);

body 2

unit vectors fixed to the boby 2

> u2 := make_VECTOR(ground,u2x(t),u2y(t),0): show(u2); along X axis
w2 := rotate('Z',Pi/2,u2):
w2 := project(w2,ground): show(w2); along Y axis

frame fixed to body 2

> T2 := linalg[transpose] (matrix(4,4, [ [Comps(u2),0] , [Comps(w2),0] , [Comps(cz),0] , [Comps(B),1] ]));

center of mass

> b2:=0:
G2 := make_POINT(T2,a2,b2,0):

properties of the rigid body

> body2 := make_BODY(G2,m2,0,0,IZ2): show(body2);
G2 := project(G2,ground): show(G2);

weight force

> W2 := make_FORCE(ground,0,-m2*g,0,G2,body2): show(W2);

equations of motion

variables

> q := [xB(t),yB(t),u2x(t),u2y(t)];
nq := nops(q);

constraints

since there are 4 variables for 2 degrees of freedom only, 2 independent constraint equations must be found

1. distance between points A and B

> AB := make_VECTOR(A,B): show(AB);

> phi1 := dot_prod(AB,AB)-lAB^2;

2. magnitude of vector u2

> phi2 := dot_prod(u2,u2)-1;

assembly constraint equations

> constraints := [phi1,phi2];

constraints - alternative solution

rigid body constraints are implicitely contained into the transformation matrices, where the rotation submatrix is orthogonal

> evalm(inv_frame(T1)*T1-1);
convert(%,set) minus {0};

> evalm(inv_frame(T2)*T2-1);
convert(%,set) minus {0};

Lagrange's equations

kinetic energy

> KE := kinetic_energy(body1) + kinetic_energy(body2);

> quadratic_form(KE,diff(q,t));

generalixed forces

> FF := simplify(generalized_force({W1,W2},q)):
for i from 1 to nq do i=q[i],'FF'=FF[i]; end do;

Lagrange's equations

> LL := KE + sum('lambda||k(t)'*constraints['k'],'k'=1..nops(constraints)): lagrangian

> leqns := [seq(expand(lagrange(LL,q[i],t)) - FF[i] , i=1..nq)]:

> for i from 1 to nq do i,q[i]; collect(leqns[i],diff(q,t,t)); od;

The result is a set on n=4 lagrange equations (2nd order differential equations) plus f=2 constraint equations (algebraic equations).
The unknowns are n=4 coordinates plus f=2 Lagrange's multipliers

Equations solution

a. Baumgarte stabilization method

in order to transfom the equations above into a standard ODE problem, the algebraic constraint equations must be derived twice with respece to the time.
Unfortunately, this operation lead to large drift error during numerical integration.

However, the Baumgarte's stabilization is sufficient for avoiding this inconvenience.

> baumgarte := convert(evalm(omega^2*constraints + 2*zeta*omega*diff(constraints,t) + diff(constraints,t,t)) ,list):
for i from 1 to nops(baumgarte) do i; baumgarte[i]; od;

The Lagrange's multipliers are algebraic unknowns, but they may be easily transformed into a differential form

> SL := [seq(lambda||i(t)= gamma||i(t)+tau*diff(gamma||i(t),t), i=1..nops(constraints))];

assembly variables and equations

> all_vars := q union [seq(gamma||i(t), i=1..nops(constraints))];
all_eqns := subs(SL,leqns) union baumgarte:

> for i from 1 to nops(all_vars) do i,all_vars[i]; all_eqns[i]; od;

ODEs numerical integrations

pendulum characteristics

> pendulum_data:= {lAB=0.3, m1=2.,a1=0.2,b1=0.05, IZ1=0.2, m2=1.5,IZ2=0.3,a2=0.1, g=9.81};
baumgarte_data := {tau=0.1,omega=10,zeta=0.8};

numerical ODEs

> ODE := convert(evalf(subs(pendulum_data,baumgarte_data,all_eqns)),set):
for i from 1 to nops(ODE) do i,ODE[i]; end;

initial conditions

> initA := { xB (0)=0, yB (0)=0.3, u2x (0)=1, u2y (0)=0,
D(xB)(0)=-0.1, D(yB)(0)=+0.01, D(u2x)(0)=0.01, D(u2y)(0)=.1}:
init := initA union {gamma1(0)=0, gamma2(0)=0 };
nops(init);

check that initial conditions are compatible with constraints

> subs(t=0,init,pendulum_data,constraints);

integration parameters

> tend:=3; integration duration
np:=60; number of steps
tstep:=tend/np:

numerical solution

> FF := dsolve( ODE union init, convert(all_vars,set), type=numeric, output=array([seq(1.*i/np*tend , i=0..np)])):

> outvar := evalm(FF[1,1]);
solz := evalm(FF[2,1]):

results

> i0:=1: i1:=8: i2:=10:
outvar[i0]; outvar[i1], outvar[i2];

> plot([ [[solz[n,i0],solz[n,i1]] $n=1..np+1], [[solz[n,i0],solz[n,i2]] $n=1..np+1] ] , color=[red,green,blue]);

results

> i0:=1: i3:=4: i4:=6:
outvar[i0]; outvar[i3], outvar[i4];

> plot([ [[solz[n,i0],solz[n,i3]] $n=1..np+1], [[solz[n,i0],solz[n,i4]] $n=1..np+1] ] , color=[red,green,blue]);

mechanism animation

> with(plots):
with(plottools):

Warning, the name changecoords has been redefined

Warning, the name arrow has been redefined

Warning, the previous bindings of the names project rotate, and translate have been removed and they now have an assigned value

extract the i-th configuration from data

> configuration := (solz,n)-> [ seq(outvar[i||k]=solz[n,i||k],k=1..4) ]:

> configuration(solz,1);

definiton of the mechanism drawing

pin joints

> PA := disk([ 0, 0], 0.03, color=yellow):
PB := disk([xB(t),yB(t)], 0.03, color=yellow):

body 1

> lx1 := max(a1,lAB/2): lx1:=evalf(subs(pendulum_data,lx1)):

> PG1 := disk([Xcomp(G1),Ycomp(G1)], 0.02, color=red):
body1 := rectangle([-lx1,-b1], [lx1, b1], filled=true, color=cyan):

> alpha1:= arctan(yB(t),xB(t)): rotation angle

> body1 := translate(rotate(body1,alpha1),Xcomp(G1),Ycomp(G1)):

> pendulum := subs(pendulum_data,[PA,PB,PG1,body1]):

body 2

> PG2 := disk([Xcomp(G2),Ycomp(G2)], 0.02, color=red):
body2 := rectangle([-a2,-b2], [a2, b2], filled=true, color=green):

> alpha2:= arctan(u2y(t),u2x(t)): rotation angle

> body2 := translate(rotate(body2,alpha2),Xcomp(G2),Ycomp(G2)):

assembly the pendulum

> pendulum := subs(pendulum_data,[PA,PB,PG1,PG2,body1,body2]):

plot of a single configuration

> #nn := 4; display( subs(configuration(solz,nn),pendulum), scaling=constrained);

animation

> display([seq(display( subs(configuration(solz,i),pendulum)),i=1..np)], insequence=true, scaling=constrained);

b. without constraint stabilization

The Baumgarte's constraint stabilization is indispensable for avoiding drift error during integration, as shown in this section.

ignore stabilization

> baumgarte_data := {tau=0.1,omega=0,zeta=00};

numerical ODEs

> ODE2 := convert(evalf(subs(pendulum_data,baumgarte_data,all_eqns)),set):
for i from 1 to nops(ODE2) do i,ODE[i]; end;

numerical solution

> FF := dsolve( ODE2 union init, convert(all_vars,set), type=numeric, output=array([seq(1.*i/np*tend , i=0..np)])):

> outvar := evalm(FF[1,1]);
solz2 := evalm(FF[2,1]):

comparison between Baumgarte and Penalty methods

> outvar[i0], outvar[i1];
plot([ [[solz [n,1],solz [n, i1]] $n=1..np+1], [[solz2 [n,1],solz2 [n,i1]] $n=1..np+1] ] ,
color=[red], linestyle=[SOLID,DASH]);

> outvar[i0], outvar[i2];
plot([ [[solz [n,1],solz [n, i2]] $n=1..np+1], [[solz2 [n,1],solz2 [n,i2]] $n=1..np+1] ] ,
color=[green], linestyle=[SOLID,DASH]);

c. penalty formulation

Lagrange's multipliers represent the constraint actions on the system.
An idea for eliminating constraint equations and the corresponding Lagrange's multipliers is based on tolerating a small constraint violation and calculating the Lagrange's multipliers proportionally to this violation. Unfortunately, this trick generate stiff equations and at least some damping and inertia must be included in the constraints, as follows:

> alpha, constraints stiffness

> omega, zeta; constraints natural frequency and damping ratio

> penalty := [seq( lambda||i(t) = -alpha*(omega**2*constraints[i] + 2*zeta*omega*diff(constraints[i],t) + diff(constraints[i],t,t)), i=1..nops(constraints))];

the new equations are

> penalty_eqns := subs(penalty,leqns):
for i from 1 to nq do i,q[i]; penalty_eqns[i]; od;

ODEs numerical integrations

numerical ODEs

> penalty_data:= {alpha= 1e8, omega=10, zeta=0.7};

> ODEP := convert(evalf(subs(pendulum_data,penalty_data,penalty_eqns)),set):
#for i from 1 to nops(ODEP) do i,ODEP[i]; end;

initial conditions

> initP := initA;
nops(initP);

check that initial conditions are compatible with constraints

> subs(t=0,initP,pendulum_data,constraints);

numerical solution

> FFP := dsolve( ODEP union initP, convert(q,set), type=numeric, stiff=true, output=array([seq(1.*i/np*tend , i=0..np)])):

> outvarP := evalm(FFP[1,1]);
solzP := evalm(FFP[2,1]):

results

> iP1:=6: iP2:=8:
outvarP[iP1], outvarP[iP2];

> plot([ [[solzP[n,1],solzP[n,iP1]] $n=1..np+1], [[solzP[n,1],solzP[n,iP2]] $n=1..np+1] ] , color=[red,green,blue] , linestyle=[3]);

comparison between Baumgarte and Penalty methods

> plot([ [[solz [n,1],solz [n, i1]] $n=1..np+1], [[solz [n,1],solz [n, i2]] $n=1..np+1],
[[solzP[n,1],solzP[n,iP1]] $n=1..np+1], [[solzP[n,1],solzP[n,iP2]] $n=1..np+1] ] ,
color=[red,green,red,green], style=[line,line,point,point]);

>