Dynamic Equations for the Rotary Double Inverted Pendulum (DBPEN-ROT)

©  2012 Quanser Consulting Inc.

URL: http://www.quanser.com

NOTE: This worksheet presents the general dynamic equations modelling a Double Inverted Pendulum mounted on a rotary arm.

Description

The Quanser_Tools  Package

References

Worksheet Initialization

>    restart: interface( imaginaryunit = j ):

>    with( LinearAlgebra ):

>    libname := "C:/Program Files/Quanser/Maple Repository", ".", libname:

>    with( Quanser_Tools );

[HTM, deriveA, deriveB, deriveF, kinetic_energy, lagrange_equations, moment_of_inertia, n_norm, potential_energy, write_ABCD_to_Mfile]
[HTM, deriveA, deriveB, deriveF, kinetic_energy, lagrange_equations, moment_of_inertia, n_norm, potential_energy, write_ABCD_to_Mfile]

environment variable representing the order of series calculations

>    Order := 2:

Notations

Generalized Coordinates: q[i] 's

The generalized coordinates are also called Lagrangian coordinates.

theta(t)  =  rotary arm angular position

alpha(t)  = position angle of the first joint of the double pendulum about the vertical: the zero angle is defined when the bottom link is in the perfect upright position.

gamma(t)  = position angle of the second joint of the double pendulum about the vertical: the zero angle is defined when the top pendulum is in-line with the bottom link of the double pendulum, i.e. link 1 and 2 are aligned with each other

Np = number of links in pendulum

>    Np := 2:

>    q := [ theta(t), alpha(t), phi(t) ];

q := [theta(t), alpha(t), phi(t)]

Nq = number of Lagrangian coordinates (e.g. here, it is Nq = 3)

Nq is also the number of position states.

>    Nq := nops( q ):

qd = first-order time derivative of the generalized coordinates, e.g. position and angular velocities

>    qd := map( diff, q, t );

qd := [diff(theta(t),t), diff(alpha(t),t), diff(phi(t),t)]

Transformation Matrices

From base coordinate to tip of arm: T_0_1 = Rot(Z0, theta)*Trans(X1,r)

>    T_0_1 := matrix(4,4,[
cos(q[1]),-sin(q[1]),0,L[r]*cos(q[1]),
sin(q[1]),cos(q[1]),0,L[r]*sin(q[1]),
0,0,1,0,
0,0,0,1]);

T_0_1 := matrix([[cos(theta(t)), -sin(theta(t)), 0, L[r]*cos(theta(t))], [sin(theta(t)), cos(theta(t)), 0, L[r]*sin(theta(t))], [0, 0, 1, 0], [0, 0, 0, 1]])

From tip of arm to CM of link 1: T_1_2 =  Rot(X1, alpha)*Trans(Z2, l1)

>    T_1_2 := matrix(4,4,[
1,0,0,0,
0,cos(q[2]),-sin(q[2]),-l[p1]*sin(q[2]),
0,sin(q[2]),cos(q[2]),l[p1]*cos(q[2]),
0,0,0,1]);

T_1_2 := matrix([[1, 0, 0, 0], [0, cos(alpha(t)), -sin(alpha(t)), -l[p1]*sin(alpha(t))], [0, sin(alpha(t)), cos(alpha(t)), l[p1]*cos(alpha(t))], [0, 0, 0, 1]])

From tip of arm to hinge: T_1_h = Rot(X1,alpha)*Trans(Z2,L1)

>    T_1_h := matrix(4,4,[
1,0,0,0,
0,cos(q[2]),-sin(q[2]),-L[p1]*sin(q[2]),
0,sin(q[2]),cos(q[2]),L[p1]*cos(q[2]),
0,0,0,1]);

T_1_h := matrix([[1, 0, 0, 0], [0, cos(alpha(t)), -sin(alpha(t)), -L[p1]*sin(alpha(t))], [0, sin(alpha(t)), cos(alpha(t)), L[p1]*cos(alpha(t))], [0, 0, 0, 1]])

From hinge to CM of link 2: T_h_3 = Rot(X2,gamma)*Trans(Y3,l2)

>    T_h_3 := matrix(4,4,[
1,0,0,0,
0,cos(q[3]),-sin(q[3]),-l[p2]*sin(q[3]),
0,sin(q[3]),cos(q[3]),l[p2]*cos(q[3]),
0,0,0,1]);

T_h_3 := matrix([[1, 0, 0, 0], [0, cos(phi(t)), -sin(phi(t)), -l[p2]*sin(phi(t))], [0, sin(phi(t)), cos(phi(t)), l[p2]*cos(phi(t))], [0, 0, 0, 1]])

CM of link 1 relative to base coordinate: T_0_2 = T_0_1 * T_1_2

>    T_0_2 := evalm( T_0_1 &* T_1_2 );

T_0_2 := matrix([[cos(theta(t)), -sin(theta(t))*cos(alpha(t)), sin(theta(t))*sin(alpha(t)), sin(theta(t))*l[p1]*sin(alpha(t))+L[r]*cos(theta(t))], [sin(theta(t)), cos(theta(t))*cos(alpha(t)), -cos(thet...

Hinge relative to base coordiante: T_0_h = T_0_1 * T_1_h

>    T_0_h := evalm( T_0_1 &* T_1_h );

T_0_h := matrix([[cos(theta(t)), -sin(theta(t))*cos(alpha(t)), sin(theta(t))*sin(alpha(t)), sin(theta(t))*L[p1]*sin(alpha(t))+L[r]*cos(theta(t))], [sin(theta(t)), cos(theta(t))*cos(alpha(t)), -cos(thet...

CM of link 2 relative to base coordinate: T_0_3 = T_0_h * T_h_3

>    T_0_3 := evalm( T_0_h &* T_h_3 );

T_0_3 := matrix([[cos(theta(t)), -sin(theta(t))*cos(alpha(t))*cos(phi(t))+sin(theta(t))*sin(alpha(t))*sin(phi(t)), sin(theta(t))*cos(alpha(t))*sin(phi(t))+sin(theta(t))*sin(alpha(t))*cos(phi(t)), sin(t...
T_0_3 := matrix([[cos(theta(t)), -sin(theta(t))*cos(alpha(t))*cos(phi(t))+sin(theta(t))*sin(alpha(t))*sin(phi(t)), sin(theta(t))*cos(alpha(t))*sin(phi(t))+sin(theta(t))*sin(alpha(t))*cos(phi(t)), sin(t...
T_0_3 := matrix([[cos(theta(t)), -sin(theta(t))*cos(alpha(t))*cos(phi(t))+sin(theta(t))*sin(alpha(t))*sin(phi(t)), sin(theta(t))*cos(alpha(t))*sin(phi(t))+sin(theta(t))*sin(alpha(t))*cos(phi(t)), sin(t...
T_0_3 := matrix([[cos(theta(t)), -sin(theta(t))*cos(alpha(t))*cos(phi(t))+sin(theta(t))*sin(alpha(t))*sin(phi(t)), sin(theta(t))*cos(alpha(t))*sin(phi(t))+sin(theta(t))*sin(alpha(t))*cos(phi(t)), sin(t...
T_0_3 := matrix([[cos(theta(t)), -sin(theta(t))*cos(alpha(t))*cos(phi(t))+sin(theta(t))*sin(alpha(t))*sin(phi(t)), sin(theta(t))*cos(alpha(t))*sin(phi(t))+sin(theta(t))*sin(alpha(t))*cos(phi(t)), sin(t...
T_0_3 := matrix([[cos(theta(t)), -sin(theta(t))*cos(alpha(t))*cos(phi(t))+sin(theta(t))*sin(alpha(t))*sin(phi(t)), sin(theta(t))*cos(alpha(t))*sin(phi(t))+sin(theta(t))*sin(alpha(t))*cos(phi(t)), sin(t...
T_0_3 := matrix([[cos(theta(t)), -sin(theta(t))*cos(alpha(t))*cos(phi(t))+sin(theta(t))*sin(alpha(t))*sin(phi(t)), sin(theta(t))*cos(alpha(t))*sin(phi(t))+sin(theta(t))*sin(alpha(t))*cos(phi(t)), sin(t...
T_0_3 := matrix([[cos(theta(t)), -sin(theta(t))*cos(alpha(t))*cos(phi(t))+sin(theta(t))*sin(alpha(t))*sin(phi(t)), sin(theta(t))*cos(alpha(t))*sin(phi(t))+sin(theta(t))*sin(alpha(t))*cos(phi(t)), sin(t...
T_0_3 := matrix([[cos(theta(t)), -sin(theta(t))*cos(alpha(t))*cos(phi(t))+sin(theta(t))*sin(alpha(t))*sin(phi(t)), sin(theta(t))*cos(alpha(t))*sin(phi(t))+sin(theta(t))*sin(alpha(t))*cos(phi(t)), sin(t...
T_0_3 := matrix([[cos(theta(t)), -sin(theta(t))*cos(alpha(t))*cos(phi(t))+sin(theta(t))*sin(alpha(t))*sin(phi(t)), sin(theta(t))*cos(alpha(t))*sin(phi(t))+sin(theta(t))*sin(alpha(t))*cos(phi(t)), sin(t...

Cartesian Coordinates of the Pendulum's Centre Of Gravity

conventions:

1) alpha = 0  and gamma = 0  correspond to the double pendulum being perfectly vertical, pointing upwards

2) positive rotation of arm, 0 < diff(theta(t),t) , is CCW

3) positive rotation of pendulum, 0 < diff(alpha(t),t)  and 0 < diff(gamma(t),t) , is also CCW.

Absolute 3-dimensional Cartesian position: tip of arm.

>    x[1] := T_0_1[1,4];
y[1] := T_0_1[2,4];
z[1] := T_0_1[3,4];

x[1] := L[r]*cos(theta(t))

y[1] := L[r]*sin(theta(t))

z[1] := 0

Absolute 3-dimensional Cartesian position: CM of link 1.

>    x[2] := T_0_2[1,4];
y[2] := T_0_2[2,4];
z[2] := T_0_2[3,4];

x[2] := sin(theta(t))*l[p1]*sin(alpha(t))+L[r]*cos(theta(t))

y[2] := -cos(theta(t))*l[p1]*sin(alpha(t))+L[r]*sin(theta(t))

z[2] := l[p1]*cos(alpha(t))

Absolute 3-dimensional Cartesian position: CM of hinge.

>    x[h] :=  T_0_h[1,4];
y[h] :=  T_0_h[2,4];
z[h] :=  T_0_h[3,4];

x[h] := sin(theta(t))*L[p1]*sin(alpha(t))+L[r]*cos(theta(t))

y[h] := -cos(theta(t))*L[p1]*sin(alpha(t))+L[r]*sin(theta(t))

z[h] := L[p1]*cos(alpha(t))

Absolute 3-dimensional Cartesian position: CM of link 2.

>    x[3] :=  T_0_3[1,4];
y[3] :=  T_0_3[2,4];
z[3] :=  T_0_3[3,4];

x[3] := sin(theta(t))*cos(alpha(t))*l[p2]*sin(phi(t))+sin(theta(t))*sin(alpha(t))*l[p2]*cos(phi(t))+sin(theta(t))*L[p1]*sin(alpha(t))+L[r]*cos(theta(t))

y[3] := -cos(theta(t))*cos(alpha(t))*l[p2]*sin(phi(t))-cos(theta(t))*sin(alpha(t))*l[p2]*cos(phi(t))-cos(theta(t))*L[p1]*sin(alpha(t))+L[r]*sin(theta(t))

z[3] := -sin(alpha(t))*l[p2]*sin(phi(t))+cos(alpha(t))*l[p2]*cos(phi(t))+L[p1]*cos(alpha(t))

Absolute 3-dimensional Cartesian velocities.

>    xd[1] := diff( x[1], t );
yd[1] := diff( y[1], t );
zd[1] := diff( z[1], t );
xd[2] := diff( x[2], t );
yd[2] := diff( y[2], t );
zd[2] := diff( z[2], t );
xd[h] := diff( x[h], t );
yd[h] := diff( y[h], t );
zd[h] := diff( z[h], t );
xd[3] := diff( x[3], t );
yd[3] := diff( y[3], t );
zd[3] := diff( z[3], t );

xd[1] := -L[r]*sin(theta(t))*diff(theta(t),t)

yd[1] := L[r]*cos(theta(t))*diff(theta(t),t)

zd[1] := 0

xd[2] := cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t)

yd[2] := sin(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))-cos(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)+L[r]*cos(theta(t))*diff(theta(t),t)

zd[2] := -l[p1]*sin(alpha(t))*diff(alpha(t),t)

xd[h] := cos(theta(t))*diff(theta(t),t)*L[p1]*sin(alpha(t))+sin(theta(t))*L[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t)

yd[h] := sin(theta(t))*diff(theta(t),t)*L[p1]*sin(alpha(t))-cos(theta(t))*L[p1]*cos(alpha(t))*diff(alpha(t),t)+L[r]*cos(theta(t))*diff(theta(t),t)

zd[h] := -L[p1]*sin(alpha(t))*diff(alpha(t),t)

xd[3] := cos(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))-sin(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))+sin(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi(t),t)+cos(t...
xd[3] := cos(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))-sin(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))+sin(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi(t),t)+cos(t...
xd[3] := cos(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))-sin(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))+sin(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi(t),t)+cos(t...
xd[3] := cos(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))-sin(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))+sin(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi(t),t)+cos(t...

yd[3] := sin(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))+cos(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))-cos(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi(t),t)+sin(t...
yd[3] := sin(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))+cos(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))-cos(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi(t),t)+sin(t...
yd[3] := sin(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))+cos(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))-cos(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi(t),t)+sin(t...
yd[3] := sin(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))+cos(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))-cos(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi(t),t)+sin(t...

zd[3] := -cos(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))-sin(alpha(t))*l[p2]*cos(phi(t))*diff(phi(t),t)-sin(alpha(t))*diff(alpha(t),t)*l[p2]*cos(phi(t))-cos(alpha(t))*l[p2]*sin(phi(t))*diff(phi(t),t)...
zd[3] := -cos(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))-sin(alpha(t))*l[p2]*cos(phi(t))*diff(phi(t),t)-sin(alpha(t))*diff(alpha(t),t)*l[p2]*cos(phi(t))-cos(alpha(t))*l[p2]*sin(phi(t))*diff(phi(t),t)...

State-Space Variables

  • The chosen states should at least include the generalized coordinates and their first-time derivatives.
  • X is the state vector.
  • In the state vector X: Lagrangian coordinates are first, followed by their first-time derivatives, and finally any other states, as required.

Substitution sets for the states (to obtain time-independent state equations).

>    subs_Xq := { seq( q[i] = X[i], i=1..Nq ) };
subs_Xqd := { seq( qd[i] = X[i+Nq], i=1..Nq ) };

subs_Xq := {phi(t) = X[3], alpha(t) = X[2], theta(t) = X[1]}

subs_Xqd := {diff(phi(t),t) = X[6], diff(alpha(t),t) = X[5], diff(theta(t),t) = X[4]}

Substitution set for the input(s).

set the input to be the motor voltage:

>    #subs_U := { V[m] = U[1] }:

set the input to be the torque applied by the motor shaft, tau[output]  (if not expressed as a function of the motor voltage):

>    subs_U := { tau[m] = U[1] }:

Nu = number of inputs; U = input (row) vector (e.g. U = [ V[m] ] )

>    Nu := nops( subs_U ):

substitution set for the position states' second time derivatives

>    subs_Xqdd := { seq( diff( q[i], t$2 ) =  Xd[i+Nq], i=1..Nq ) };

subs_Xqdd := {diff(phi(t),`$`(t,2)) = Xd[6], diff(alpha(t),`$`(t,2)) = Xd[5], diff(theta(t),`$`(t,2)) = Xd[4]}

second time derivatives of the position states (written as time-independent variables).

The set of unknowns is obtained from this list to solve the Lagrange's equations of motion.

>    Xqdd := [ seq( Xd[i+Nq], i=1..Nq ) ];

Xqdd := [Xd[4], Xd[5], Xd[6]]

substitution set to linearize the state-space matrices (i.e. A and B)

about the quiescent null state vector (small-displacement theory)

>    subs_XUzero := { seq( X[i] = 0, i=1..2*Nq ), seq( U[i] = 0, i=1..Nu ) }:

Nx = dim( X ) = total number of states (should be greater than or equal to: 2 * Nq)

Ny = chosen number of outputs

>    Nx := 2 * Nq:
Ny := Nq:

Total Potential and Kinetic Energies of the System

The total potential and kinetic energies are needed to calculate the Lagrangian of the system.

Total Potential Energy: V[T]  

The total potential energy can be expressed in terms of the generalized coordinates alone.

V[e] = Total Elastic Potential Energy of the system

>    V[e] := 0:

V[g1]  = potential energy of link 1 CM.

>    V[g1] := potential_energy( 'gravity', m[p1], g, z[2] );

V[g1] := m[p1]*g*l[p1]*cos(alpha(t))

V[gh]  = potential energy of hinge.

>    V[gh] := potential_energy( 'gravity', m[h], g, z[h] );

V[gh] := m[h]*g*L[p1]*cos(alpha(t))

V[g2]  = potential energy of link 2 CM.

>    V[g2] := potential_energy( 'gravity', m[p2], g, z[3] );

V[g2] := m[p2]*g*(-sin(alpha(t))*l[p2]*sin(phi(t))+cos(alpha(t))*l[p2]*cos(phi(t))+L[p1]*cos(alpha(t)))

V[g] = Total Gravitational Potential Energy of the system

>    V[g] := V[g1] + V[gh] + V[g2]:

V[T] = Total Potential Energy of the system

>    V[T] := V[g] + V[e];

V[T] := m[p1]*g*l[p1]*cos(alpha(t))+m[h]*g*L[p1]*cos(alpha(t))+m[p2]*g*(-sin(alpha(t))*l[p2]*sin(phi(t))+cos(alpha(t))*l[p2]*cos(phi(t))+L[p1]*cos(alpha(t)))
V[T] := m[p1]*g*l[p1]*cos(alpha(t))+m[h]*g*L[p1]*cos(alpha(t))+m[p2]*g*(-sin(alpha(t))*l[p2]*sin(phi(t))+cos(alpha(t))*l[p2]*cos(phi(t))+L[p1]*cos(alpha(t)))

Total Kinetic Energy: T[T]

The total kinetic energy can be expressed in terms of the generalized coordinates and their first-time derivatives.

T[r1]  = kinetic energy due to rotation of arm

J[m]  = moment of inertia of motor and rotary arm.

>    Tr[1] := kinetic_energy( 'rotation', J[r], qd[1] );

Tr[1] := 1/2*J[r]*diff(theta(t),t)^2

The DBPEN motion is composed of two rotations and two translations. The rotation and translation directions of each link composing the double pendulum assembly are orthogonal.

Diff(alpha(t),t)  = pendulum 1 absolute angular velocity

Tr[2] = pendulum 1 rotational kinetic kinergy: set to zero since joint does not move around a lot.

>    #Tr[2] := kinetic_energy( 'rotation', J[p1], qd[2] );
Tr[2] := 0;

Tr[2] := 0

v[2] = pendulum 1 cartesian velocity (magnitude)

Tt[2] = pendulum 1 translational kinetic kinergy

>    v[2] := n_norm( [ xd[2], yd[2], zd[2] ], 2 ):
Tt[2] := kinetic_energy( 'translation', m[p1], v[2] );

Tt[2] := 1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(theta(t))*diff(theta(t),t)*l[p1]*s...
Tt[2] := 1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(theta(t))*diff(theta(t),t)*l[p1]*s...
Tt[2] := 1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(theta(t))*diff(theta(t),t)*l[p1]*s...

Diff(alpha(t),t)  = DBPEN hinge absolute angular velocity

v[3] = DBPEN hinge cartesian velocity (magnitude)

Tt[3] = DBPEN hinge translational kinetic kinergy

>    v[h] := n_norm( [ xd[h], yd[h], zd[h] ], 2 ):
Tt[h] := kinetic_energy( 'translation', m[h], v[h]);

Tt[h] := 1/2*m[h]*((cos(theta(t))*diff(theta(t),t)*L[p1]*sin(alpha(t))+sin(theta(t))*L[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(theta(t))*diff(theta(t),t)*L[p1]*si...
Tt[h] := 1/2*m[h]*((cos(theta(t))*diff(theta(t),t)*L[p1]*sin(alpha(t))+sin(theta(t))*L[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(theta(t))*diff(theta(t),t)*L[p1]*si...
Tt[h] := 1/2*m[h]*((cos(theta(t))*diff(theta(t),t)*L[p1]*sin(alpha(t))+sin(theta(t))*L[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(theta(t))*diff(theta(t),t)*L[p1]*si...

Diff(gamma(t),t)  = pendulum 2 absolute angular velocity

Tr[3] = pendulum 2 rotational kinetic energy: set to zero since joint does not move around a lot.

>    #Tr[3] := kinetic_energy( 'rotation', J[p2], qd[3] );
Tr[3] := 0;

Tr[3] := 0

v[3] = pendulum 2 cartesian velocity (magnitude)

Tt[3] = pendulum 2 translational kinetic kinergy

>    v[3] := n_norm( [ xd[3], yd[3], zd[3] ], 2 ):
Tt[3] := kinetic_energy( 'translation', m[p2], v[3]);

Tt[3] := 1/2*m[p2]*((cos(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))-sin(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))+sin(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi...
Tt[3] := 1/2*m[p2]*((cos(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))-sin(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))+sin(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi...
Tt[3] := 1/2*m[p2]*((cos(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))-sin(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))+sin(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi...
Tt[3] := 1/2*m[p2]*((cos(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))-sin(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))+sin(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi...
Tt[3] := 1/2*m[p2]*((cos(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))-sin(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))+sin(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi...
Tt[3] := 1/2*m[p2]*((cos(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))-sin(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))+sin(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi...
Tt[3] := 1/2*m[p2]*((cos(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))-sin(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))+sin(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi...
Tt[3] := 1/2*m[p2]*((cos(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))-sin(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))+sin(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi...
Tt[3] := 1/2*m[p2]*((cos(theta(t))*diff(theta(t),t)*cos(alpha(t))*l[p2]*sin(phi(t))-sin(theta(t))*sin(alpha(t))*diff(alpha(t),t)*l[p2]*sin(phi(t))+sin(theta(t))*cos(alpha(t))*l[p2]*cos(phi(t))*diff(phi...

Total kinetic energy of DBPEN

>    T[T] := Tr[1] + Tr[2] + Tr[3] + Tt[1] + Tt[2] + Tt[h] + Tt[3];
T[T] := collect( T[T], diff ):

T[T] := 1/2*J[r]*diff(theta(t),t)^2+Tt[1]+1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(t...
T[T] := 1/2*J[r]*diff(theta(t),t)^2+Tt[1]+1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(t...
T[T] := 1/2*J[r]*diff(theta(t),t)^2+Tt[1]+1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(t...
T[T] := 1/2*J[r]*diff(theta(t),t)^2+Tt[1]+1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(t...
T[T] := 1/2*J[r]*diff(theta(t),t)^2+Tt[1]+1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(t...
T[T] := 1/2*J[r]*diff(theta(t),t)^2+Tt[1]+1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(t...
T[T] := 1/2*J[r]*diff(theta(t),t)^2+Tt[1]+1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(t...
T[T] := 1/2*J[r]*diff(theta(t),t)^2+Tt[1]+1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(t...
T[T] := 1/2*J[r]*diff(theta(t),t)^2+Tt[1]+1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(t...
T[T] := 1/2*J[r]*diff(theta(t),t)^2+Tt[1]+1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(t...
T[T] := 1/2*J[r]*diff(theta(t),t)^2+Tt[1]+1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(t...
T[T] := 1/2*J[r]*diff(theta(t),t)^2+Tt[1]+1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(t...
T[T] := 1/2*J[r]*diff(theta(t),t)^2+Tt[1]+1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(t...
T[T] := 1/2*J[r]*diff(theta(t),t)^2+Tt[1]+1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(t...
T[T] := 1/2*J[r]*diff(theta(t),t)^2+Tt[1]+1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(t...
T[T] := 1/2*J[r]*diff(theta(t),t)^2+Tt[1]+1/2*m[p1]*((cos(theta(t))*diff(theta(t),t)*l[p1]*sin(alpha(t))+sin(theta(t))*l[p1]*cos(alpha(t))*diff(alpha(t),t)-L[r]*sin(theta(t))*diff(theta(t),t))^2+(sin(t...

Generalized Forces: Q[i] 's

The non-conservative forces corresponding to the generalized coordinates are: tau[m]  and the viscous damping forces, where

tau[m]  = torque generated by the motor driving the arm

B[r]  = arm viscous friction torque coefficient (a.k.a. viscous damping)

B[p1]  = pendulum 1 viscous friction torque coefficient (a.k.a. viscous damping)

B[p2]  = pendulum 2 viscous friction torque coefficient (a.k.a. viscous damping)

Set to zero if viscous damping is neglected

>    #B[r] := 0:

>    #B[p1] := 0:
#B[p2] := 0:

Q[i] = generalized force applied on generalized coordinate q[i]

>    Q[1] := tau[m] - B[r] * qd[1];

Q[1] := tau[m]-B[r]*diff(theta(t),t)

>    Q[2] := -B[p1] * qd[2];

Q[2] := -B[p1]*diff(alpha(t),t)

>    Q[3] := -B[p2] * qd[3];

Q[3] := -B[p2]*diff(phi(t),t)

[taum]  = torque produced by the motor at the motor pinion (i.e. after the gearbox): arm driving torque

rm: comment the following line out if u = tau[m] , uncomment it if u = V[m]

note : the actuator dynamics are added in the Matlab script to save on the computation time of the EOM in this Maple worksheet.

>    #tau[m] := eta[g] * K[g] * eta[m] * k[t] * ( V[m] - k[g] * k[m] * qd[1] ) / R[m];

>    Q := [ seq( Q[i], i=1..Nq ) ];

Q := [tau[m]-B[r]*diff(theta(t),t), -B[p1]*diff(alpha(t),t), -B[p2]*diff(phi(t),t)]

Euler-Lagrange's Equations

For a N -DOF system, the Lagrange's equations can be written:

Diff(Diff(L,qdot[i]),t)-Diff(L,q[i]) = Q[i]   for i = 1  through N

where:

  Q[i] 's are special combinations of external forces and called the generalized forces,

  q[1] , ..., q[N] , are N independent coordinates chosen to describe the system and called the generalized coordinates ,

 and L  is the Lagrangian  of the system.

L  is defined by:

L = T-U

where T  is the total kinetic energy of the system and U  the total potential energy of the system.

>    EOM_orig := lagrange_equations( T[T], V[T], q, Q ):

this is to display the EOM's

>    EOM_orig := collect( EOM_orig, { seq( diff( q[i], t$2 ), i=1..Nq ), seq( diff( q[i], t ), i=1..Nq ), seq( q[i], i=1..Nq ) } ):

>   

Express the Euler-Lagrange equations of motion as functions of the states:

1) substitute (i.e. name) the acceleration states first!

2) then substitute the velocity states!

3) and only after, the position states, and the inputs!

>    #EOM_states := subs( subs_Xqd, subs( subs_Xqdd, EOM_orig ) ):
EOM_states_acc := subs( subs_Xqdd, EOM_orig ):

>    EOM_states_vel_acc := subs( subs_Xqd, EOM_states_acc ):

>    EOM_states := subs( subs_Xq, subs_U, EOM_states_vel_acc );

EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...
EOM_states := [(m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[6]^2+((2*m[p2]*L[r]*cos(X[2])*l[p2]*sin(X[3])+2*m[p2]*L[r]*sin(X[2])*l[p2]*cos(X[3]))*X[5]+(2*m[p2]*L[p1]*co...

Linearization  in the EOM's   of the Trigonometric Functions

Linearization of the equations of motion around the quiescent point of operation (in order to solve them).

Here, linearization around the zero angles, i.e. for small-amplitude oscillations.

Linearization around: alpha0 = 0, and alpha_dot0 = 0

>    EOM_ser := EOM_states:

Generalized series expansions of the trigonometric functions is used (for small angles).

>    for i from 1 to Nq do
  for k from 1 to Np do
    EOM_ser[i] := subsop( 1 = convert( series( op( 1, EOM_ser[i] ), X[ k+1 ] ), polynom ), EOM_ser[i] );
  end do:
  EOM_ser[i] := simplify( EOM_ser[i] );
end do:

>    EOM_ser:

Additional Insight: Inertia (or mass) Matrix: Fi

The nonlinear system of equations resulting from the Lagrangian mechanics can be written in the following matrix form:

F( q ) . qdd + G( q, qd ) . qd + H( q ) . q = L( q, qd, u )

F, G, and H are called, respectively, the mass, damping, and stiffness matrices.

They are symmetric in form.

The inertia (a.k.a. mass) matrix, F, gives indications regarding the coupling existing in the system.

>    Fi := Matrix( Nq, Nq ):

>    for i from 1 to Nq do
  for k from 1 to Nq do
    Fi[ i, k ] := simplify( diff( op( 1, EOM_states[i] ), Xd[k+Nq] ) );
    Fi[ i, k ] := collect( combine( Fi[ i, k ], trig ), cos );
  end do;
end do:

>    'F[i]' = Fi:

Linearization of the inertia matrix for small-displacements

>    Fi_lin := Matrix( Nq, Nq ):

>    for i from 1 to Nq do
  for k from 1 to Nq do
    Fi_lin[ i, k ] := Fi[ i, k ];
    Fi_lin[ i, k ] := convert( series( Fi_lin[ i, k ], X[ 2 ] ), polynom );
    Fi_lin[ i, k ] := subs( subs_XUzero, Fi_lin[ i, k ] );
  end do;
end do:

>    'F_lin' = Fi_lin;

F_lin = Matrix(%id = 147699208)
F_lin = Matrix(%id = 147699208)
F_lin = Matrix(%id = 147699208)
F_lin = Matrix(%id = 147699208)
F_lin = Matrix(%id = 147699208)

Solving the Euler-Lagrange's Equations

Reverse State Substitution for Pretty Display of the Solved EOM's

only for pretty print

>    subs_Xq_rev := { seq( X[i] = q[i], i=1..Nq ) }:
subs_Xqd_rev := { seq( X[i+Nq] = qd[i], i=1..Nq ) }:

>    #subs_U_rev := { U[1] = V[m] }:
subs_U_rev := { U[1] = tau[m] }:

>    eom_collect_list := { seq( diff( q[i], t ), i=1..Nq ), seq( q[i], i=1..Nq ), J[r], J[p1], J[p2] };

eom_collect_list := {J[p1], J[p2], J[r], phi(t), alpha(t), diff(phi(t),t), diff(alpha(t),t), diff(theta(t),t), theta(t)}

Solution to the Non-Linear Equations of Motion

Solve the non-linear form of the equations of motion for the states' second time derivatives

>    #Xqdd_solset_nl := solve( convert( EOM_states, set ), convert( Xqdd, set ) ):

>    #assign( Xqdd_solset_nl );

>    #for i from 1 to Nq do
#  Xd_nl[i+Nq] := simplify( Xd[i+Nq] ):
#  unassign( 'Xd[i+Nq]' ):
#end do:

pretty display w.r.t. the named system states

>    #for i from 1 to Nq do
# Xd_nl[i+Nq] := simplify( subs( subs_U_rev, subs_Xq_rev, subs_Xqd_rev, Xd_nl[i+Nq] ) #):
#end do:

>    #for i from 1 to Nq do
#  diff( qd[i], t ) = collect( Xd_nl[i+Nq], eom_collect_list );
#end do:

Solution to the Linearized EOM's

Solve the linear form of the equations of motion for the states' second time derivatives

>    Xqdd_solset_ser := solve( convert( EOM_ser, set ), convert( Xqdd, set ) ):

>    assign( Xqdd_solset_ser );

Moreover, for small angles

>    subs_avsq_list := { X[Nq+2]^2 = 0, X[Nq+3]^2 = 0 }:

>    for i from 1 to Nq do
  Xd[i+Nq] := subs( subs_avsq_list, Xd[i+Nq] );
end:

pretty display w.r.t. the named system states

>    for i from 1 to Nq do
  diff( qd[i], t ) = collect( subs( subs_U_rev, subs_Xq_rev, subs_Xqd_rev, Xd[i+Nq] ), eom_collect_list );
end do:

Determine the System State-Space Matrices: A, B, C, and D

>    A_ss := Matrix( Nx, Nx ):

>    A_ss := deriveA( Xqdd, A_ss, Nq, subs_XUzero ):

>    'A' = A_ss;

A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)
A = Matrix(%id = 148476484)

>    B_ss := Matrix( Nx, Nu ):

>    B_ss := deriveB( Xqdd, B_ss, Nq, subs_XUzero ):

>    'B' = B_ss;

B = Matrix(%id = 148477516)

>    C_ss := IdentityMatrix( Nx, Nx ):
'C' = C_ss;

C = Matrix(%id = 148512308)

>    D_ss := Matrix( Nx, Nu, 0 ):
'D' = D_ss;

D = Matrix(%id = 148512724)

Write A, B, C, and D to a Matlab file

Save the state-space matrices A, B, C and D to a MATLAB file.

>    Matlab_File_Name := "DBIP_ABCD_eqns.m":

unassign variables, when necessary, for those present in the "Matlab_Notations" substitution set

>    if assigned( B[r] ) then
  unassign( 'B[r]' );
end if;

>    if assigned( B[p1] ) then
  unassign( 'B[p1]' );
end if;

>    if assigned( B[p2] ) then
  unassign( 'B[p2]' );
end if;

>    if assigned( J[r] ) then
  unassign( 'J[r]' );
end if;

>    if assigned( J[p1] ) then
  unassign( 'J[p1]' );
end if;

>    if assigned( J[p2] ) then
  unassign( 'J[p2]' );
end if;

substitution set containing a notation consistent with that used in the MATLAB design script(s)

>    Matlab_Notations := {m[r] = Mr, m[p1]=Mp1, m[h] = Mh, m[p2] = Mp2, L[r] = Lr, l[p1] = lp1, l[p2] = lp2, L[p1]=Lp1, L[p2]=Lp2, J[r] = Jr, J[r] = Jr, J[p1]=Jp1, J[p2]=Jp2, B[r] = Dr, B[p1] = Dp1, B[p2] = Dp2, k[t] = kt, k[m] = km, K[g] = Kg, R[m] = Rm, eta[m] = eta_m, eta[g] = eta_g}:

>    Experiment_Name := "SRV02+DBPEN-ROT":

>    write_ABCD_to_Mfile( Matlab_File_Name, Experiment_Name, Matlab_Notations, A_ss, B_ss, C_ss, D_ss );

Procedure Printing

default:

>    interface( verboseproc = 1 ):

>    eval( lagrange_equations ):

>   

>   

>   

>   

>   

>   

>   

>   

>   

>   

>   

>   

>   

Click here to go back to top: Description Section.