> restart: libname := libname, "C:/libs/MBSymba.mla": with(MBSymba_r6):
Definition of the 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 * join_points(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 := < <comp_XYZ(u1),0> | <comp_XYZ(w1),0> | <comp_XYZ(cz),0> | <comp_XYZ(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);
> show(ground);
weight force
> W1 := make_FORCE( make_VECTOR(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 := < <comp_XYZ(u2),0> | <comp_XYZ(w2),0> | <comp_XYZ(cz),0> | <comp_XYZ(B),1> >;
center of mass
> G2 := make_POINT(T2,a2,0,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( make_VECTOR(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 := join_points(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
> PDEtools[declare](q,prime=t,quiet);
kinetic energy
> KE := kinetic_energy(body1) + kinetic_energy(body2);
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
Numerical 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 stabilizationis 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
> PDEtools[declare](gamma1(t),gamma2(t),prime=t,quiet);
> 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,b2=0.04, 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):
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([comp_X(G1),comp_Y(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),comp_X(G1),comp_Y(G1)):
> pendulum := subs(pendulum_data,[PA,PB,PG1,body1]):
body 2
> PG2 := disk([comp_X(G2),comp_Y(G2)], 0.02, color=red):
body2 := rectangle([-a2,-b2], [a2, b2], filled=true, color=orange):
> alpha2:= arctan(u2y(t),u2x(t)): ##rotation angle
> body2 := translate(rotate(body2,alpha2),comp_X(G2),comp_Y(G2)):
assembly the pendulum
> pendulum := subs(pendulum_data,[PA,PB,PG1,PG2,body1,body2]):
plot of a single configuration
> nn := 17; 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]):
Warning, cannot evaluate the solution further right of .31778736, maxfun limit exceeded (see ?dsolve,maxfun for details)
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]);
>
>