> restart: mylib :="C:/LIBS": libname := mylib,libname: with(MBSymba_r5.1):

MBSymba release 5.1 - Copyright (C) 2009 - by Roberto.Lot@unipd.it & Matteo.Massaro@unipd.it

Warning, Non-Linear Modeling option has been choosen

NB: the kinematic model has to be rebuilt by intorducing time dependency

> PDEtools[declare](xB(t),yB(t),yC(t), prime=t, quiet):

points definition

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

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

> C := make_POINT(ground,xC ,yC(t),0): show(C); slider

Since the single-degree-of-freedom mechanism is described by means of 5 variables,
4 constraint equations must be written. TAll they correspond to the lenght of constant segments

> AB := make_VECTOR(A,B): phi[1] := dot_prod(AB,AB)-lAB^2;

> BC := make_VECTOR(B,C): phi[2] := dot_prod(BC,BC)-lBC^2;

> constraints:=convert(phi,list):

command lever

unit vectors fixed to the lever

> u1 := 1/lAB*AB:

> w1 := rotate('Z',Pi/2,u1):
w1 := project(w1,ground):

> show(u1), show(w1);

frame, center of mass and body

> T1 := < <Comps(u1),0> | <Comps(w1),0> | <0,0,1,0> | <0,0,0,1> >;
G1 := make_POINT(T1,a1,b1,0):

> lever := make_BODY(G1,m1,0,0,IZ1): show(lever);

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

blade

the blade (slider) has a pure translational motion

> T2 := translate(xC ,yC(t),0):
G2 := origin(T2):
blade := make_BODY(G2,m2): show(blade);

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

connecting rod

it has been considered not inert

active force

application point:
in order to reduce the number of variables, P is defined as a linear combination of the lever coordinates

> P := project(make_POINT(T1,aP,bP,0),ground): show(P);

compute aP and bP as a function of segment lenghts

> AP := make_VECTOR(A,P): simplify(dot_prod(AP,AP)-lAP^2): c1:=simplify(subs(xB(t)^2=lAB^2-yB(t)^2,%));

> BP := make_VECTOR(B,P): simplify(dot_prod(BP,BP)-lBP^2): c2:=simplify(subs(xB(t)^2=lAB^2-yB(t)^2,%));

> findP := solve({c1,c2},{aP,bP});

force definition

> AF := make_FORCE(ground,0,-FP,0, P, lever): show(AF);

reactive force

force definition

> RF := make_FORCE(ground,0,FC,0, C, blade): show(RF);

Dynamic model - equations of motion

variables

> q := { xB(t),yB(t),yC(t) }; nq := nops(q);

kinetic energy

> KE := kinetic_energy(lever) + kinetic_energy(blade);

generalixed forces

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

simplification: coordinates xB(t) and yB(t) may be eliminated using constraint equations

> FF := simplify(subs(xB(t)^2=lAB^2-yB(t)^2,FF)):
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]; leqns[i]; 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 integration and results displaying

Baumgarte's method

The Differential-Algebraic set of equations is being transformed into a pure Differential set of equations

> 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

geometric characteristics

> geom := {lAB=0.230, lBP=0.450, lBC=0.250, lAP=0.650, xC=0.27};

position of P

> allvalues(subs(geom, findP ));
choose negative solution

> geom := geom union %[2]:

inertias

> inertias := {m1=2, a1=0.2, b1=-0.3, IZ1=0.2, m2=0.4};

forces

> forces := {FP=80, FC=max(-5000*(yC(t)-0.03)-200*diff(yC(t),t),0), g=9.81};

baumgarte

> baumgarte_data := {tau=0.1,omega=10,zeta=0.8};

numerical ODEs

> data := geom union inertias union forces union baumgarte_data:

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

initial conditions

> init := { xB (0)=+0.062, yB (0)=+0.222, yC (0)=+0.083,
D(xB)(0)= 0.0, D(yB)(0)= 0.0, D(yC)(0)= 0.0,
gamma1(0)=0, gamma2(0)=0 };
nops(init);

check that initial conditions are compatible with constraints

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

integration parameters

> tend:=0.5; integration duration
np:=10; 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

> i1:=4: i2:=6: i3:=8:
outvar[i1], outvar[i2], outvar[i3];

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

extract the i-th configuration from data

> configuration := (solz,n)-> [ xB=solz[n,i1], yB=solz[n,i2], xC=subs(geom,xC), yC=solz[n,i3],
xP=subs(geom,(solz[n,i1]*aP-solz[n,i1]*bP)/lAB),
yP=subs(geom,(solz[n,i2]*aP+solz[n,i1]*bP)/lAB),
FC = evalf(subs(forces,diff(yC(t),t)=solz[n,i3+1],yC(t)=solz[n,i3],FC)) ]:

> q := configuration(solz,np);

mechanism drawing

> with(plots): with(plottools):

> draw_mechanism := proc(q)
local revj_A,revj_B,revj_C, mech,lever,rod,blade,guide, FM,FR:
revj_A := disk([ 0, 0], 0.02, color=yellow);
revj_B := disk(subs(q,[xB,yB]), 0.02, color=yellow);
revj_C := disk(subs(q,[xC,yC]), 0.02, color=yellow);
lever := polygon(subs(q,[[0,0],[xB,yB],[xP,yP]]), color=magenta, thickness=1);
rod := line( subs(q,[xB,yB]),subs(q,[xC,yC]), color=black, thickness=5);
blade := polygon(subs(q,[[xC+0.03,yC+0.03],[xC-0.03,yC+0.03],[xC-0.03,yC-0.05],[xC+0.03,yC-0.13]]), color=blue, thickness=1);
guide := rectangle( subs(q,[xC+0.03, +0.13]), subs(q,[xC+0.13, -0.2]), color=grey ):

FM := arrow(subs(q,forces,[xP,yP+FP/500]), subs(q,[xP,yP]), .03, .06, .3, color=green);
FR := arrow(subs(q,[xC+0.03,yC-0.13001-FC/500]), subs(q,[xC+0.03,yC-0.13]), .03, .06, .3, color=red);

display(FM,FR,rod,guide,revj_A,revj_B,revj_C,blade,lever, scaling=constrained);
end:

single configuration

> #nn:=1; draw_mechanism(configuration(solz,nn)); nn:=nn+1;

animation

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

>