Nonminimal Coordinates Pendulum#
In this example we demonstrate the use of the functionality provided in
sympy.physics.mechanics
for deriving the equations of motion (EOM) for a pendulum
with a nonminimal set of coordinates. As the pendulum is a one degree of
freedom system, it can be described using one coordinate and one speed (the
pendulum angle, and the angular velocity respectively). Choosing instead to
describe the system using the and
coordinates of the mass results in
a need for constraints. The system is shown below:
The system will be modeled using both Kane’s and Lagrange’s methods, and the resulting EOM linearized. While this is a simple problem, it should illustrate the use of the linearization methods in the presence of constraints.
Kane’s Method#
First we need to create the dynamicsymbols
needed to describe the system as
shown in the above diagram. In this case, the generalized coordinates and
represent the mass
and
coordinates in the inertial
frame.
Likewise, the generalized speeds
and
represent the velocities in
these directions. We also create some
symbols
to represent the length and
mass of the pendulum, as well as gravity and time.
>>> from sympy.physics.mechanics import *
>>> from sympy import symbols, atan, Matrix, solve
>>> # Create generalized coordinates and speeds for this non-minimal realization
>>> # q1, q2 = N.x and N.y coordinates of pendulum
>>> # u1, u2 = N.x and N.y velocities of pendulum
>>> q1, q2 = dynamicsymbols('q1:3')
>>> q1d, q2d = dynamicsymbols('q1:3', level=1)
>>> u1, u2 = dynamicsymbols('u1:3')
>>> u1d, u2d = dynamicsymbols('u1:3', level=1)
>>> L, m, g, t = symbols('L, m, g, t')
Next, we create a world coordinate frame , and its origin point
. The
velocity of the origin is set to 0. A second coordinate frame
is oriented
such that its x-axis is along the pendulum (as shown in the diagram above).
>>> # Compose world frame
>>> N = ReferenceFrame('N')
>>> pN = Point('N*')
>>> pN.set_vel(N, 0)
>>> # A.x is along the pendulum
>>> theta1 = atan(q2/q1)
>>> A = N.orientnew('A', 'axis', [theta1, N.z])
Locating the pendulum mass is then as easy as specifying its location with in
terms of its x and y coordinates in the world frame. A Particle
object is
then created to represent the mass at this location.
>>> # Locate the pendulum mass
>>> P = pN.locatenew('P1', q1*N.x + q2*N.y)
>>> pP = Particle('pP', P, m)
The kinematic differential equations (KDEs) relate the derivatives of the
generalized coordinates to the generalized speeds. In this case the speeds are
the derivatives, so these are simple. A dictionary is also created to map
to
:
>>> # Calculate the kinematic differential equations
>>> kde = Matrix([q1d - u1,
... q2d - u2])
>>> dq_dict = solve(kde, [q1d, q2d])
The velocity of the mass is then the time derivative of the position from the
origin :
>>> # Set velocity of point P
>>> P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict))
As this system has more coordinates than degrees of freedom, constraints are
needed. The configuration constraints relate the coordinates to each other. In
this case the constraint is that the distance from the origin to the mass is
always the length (the pendulum doesn’t get longer). Likewise, the velocity
constraint is that the mass velocity in the
A.x
direction is always 0 (no
radial velocity).
>>> f_c = Matrix([P.pos_from(pN).magnitude() - L])
>>> f_v = Matrix([P.vel(N).express(A).dot(A.x)])
>>> f_v.simplify()
The force on the system is just gravity, at point P
.
>>> # Input the force resultant at P
>>> R = m*g*N.x
With the problem setup, the equations of motion can be generated using the
KanesMethod
class. As there are constraints, dependent and independent
coordinates need to be provided to the class. In this case we’ll use and
as the independent coordinates and speeds:
>>> # Derive the equations of motion using the KanesMethod class.
>>> KM = KanesMethod(N, q_ind=[q2], u_ind=[u2], q_dependent=[q1],
... u_dependent=[u1], configuration_constraints=f_c,
... velocity_constraints=f_v, kd_eqs=kde)
>>> (fr, frstar) = KM.kanes_equations([pP],[(P, R)])
For linearization, operating points can be specified on the call, or be
substituted in afterwards. In this case we’ll provide them in the call,
supplied in a list. The A_and_B=True
kwarg indicates to solve invert the
matrix and solve for just the explicit linearized
and
matrices. The
simplify=True
kwarg indicates to simplify inside the linearize call, and
return the presimplified matrices. The cost of doing this is small for simple
systems, but for larger systems this can be a costly operation, and should be
avoided.
>>> # Set the operating point to be straight down, and non-moving
>>> q_op = {q1: L, q2: 0}
>>> u_op = {u1: 0, u2: 0}
>>> ud_op = {u1d: 0, u2d: 0}
>>> # Perform the linearization
>>> A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True,
... new_method=True, simplify=True)
>>> A
Matrix([
[ 0, 1],
[-g/L, 0]])
>>> B
Matrix(0, 0, [])
The resulting matrix has dimensions 2 x 2, while the number of total states
is
len(q) + len(u) = 2 + 2 = 4
. This is because for constrained systems the
resulting A_and_B
form has a partitioned state vector only containing
the independent coordinates and speeds. Written out mathematically, the system
linearized about this point would be written as:
Lagrange’s Method#
The derivation using Lagrange’s method is very similar to the approach using
Kane’s method described above. As before, we first create the
dynamicsymbols
needed to describe the system. In this case, the generalized
coordinates and
represent the mass
and
coordinates in the
inertial
frame. This results in the time derivatives
and
representing the velocities in these directions. We also create some
symbols
to represent the length and mass of the pendulum, as well as
gravity and time.
>>> from sympy.physics.mechanics import *
>>> from sympy import symbols, atan, Matrix
>>> q1, q2 = dynamicsymbols('q1:3')
>>> q1d, q2d = dynamicsymbols('q1:3', level=1)
>>> L, m, g, t = symbols('L, m, g, t')
Next, we create a world coordinate frame , and its origin point
. The
velocity of the origin is set to 0. A second coordinate frame
is oriented
such that its x-axis is along the pendulum (as shown in the diagram above).
>>> # Compose World Frame
>>> N = ReferenceFrame('N')
>>> pN = Point('N*')
>>> pN.set_vel(N, 0)
>>> # A.x is along the pendulum
>>> theta1 = atan(q2/q1)
>>> A = N.orientnew('A', 'axis', [theta1, N.z])
Locating the pendulum mass is then as easy as specifying its location with in
terms of its x and y coordinates in the world frame. A Particle
object is
then created to represent the mass at this location.
>>> # Create point P, the pendulum mass
>>> P = pN.locatenew('P1', q1*N.x + q2*N.y)
>>> P.set_vel(N, P.pos_from(pN).dt(N))
>>> pP = Particle('pP', P, m)
As this system has more coordinates than degrees of freedom, constraints are
needed. In this case only a single holonomic constraints is needed: the
distance from the origin to the mass is always the length (the pendulum
doesn’t get longer).
>>> # Holonomic Constraint Equations
>>> f_c = Matrix([q1**2 + q2**2 - L**2])
The force on the system is just gravity, at point P
.
>>> # Input the force resultant at P
>>> R = m*g*N.x
With the problem setup, the Lagrangian can be calculated, and the equations of
motion formed. Note that the call to LagrangesMethod
includes the
Lagrangian, the generalized coordinates, the constraints (specified by
hol_coneqs
or nonhol_coneqs
), the list of (body, force) pairs, and the
inertial frame. In contrast to the KanesMethod
initializer, independent and
dependent coordinates are not partitioned inside the LagrangesMethod
object. Such a partition is supplied later.
>>> # Calculate the lagrangian, and form the equations of motion
>>> Lag = Lagrangian(N, pP)
>>> LM = LagrangesMethod(Lag, [q1, q2], hol_coneqs=f_c, forcelist=[(P, R)], frame=N)
>>> lag_eqs = LM.form_lagranges_equations()
Next, we compose the operating point dictionary, set in the hanging at rest position:
>>> # Compose operating point
>>> op_point = {q1: L, q2: 0, q1d: 0, q2d: 0, q1d.diff(t): 0, q2d.diff(t): 0}
As there are constraints in the formulation, there will be corresponding
Lagrange Multipliers. These may appear inside the linearized form as well, and
thus should also be included inside the operating point dictionary.
Fortunately, the LagrangesMethod
class provides an easy way of solving
for the multipliers at a given operating point using the solve_multipliers
method.
>>> # Solve for multiplier operating point
>>> lam_op = LM.solve_multipliers(op_point=op_point)
With this solution, linearization can be completed. Note that in contrast to
the KanesMethod
approach, the LagrangesMethod.linearize
method also
requires the partitioning of the generalized coordinates and their time
derivatives into independent and dependent vectors. This is the same as what
was passed into the KanesMethod
constructor above:
>>> op_point.update(lam_op)
>>> # Perform the Linearization
>>> A, B, inp_vec = LM.linearize([q2], [q2d], [q1], [q1d],
... op_point=op_point, A_and_B=True)
>>> A
Matrix([
[ 0, 1],
[-g/L, 0]])
>>> B
Matrix(0, 0, [])
The resulting matrix has dimensions 2 x 2, while the number of total states
is
2*len(q) = 4
. This is because for constrained systems the resulting
A_and_B
form has a partitioned state vector only containing the independent
coordinates and their derivatives. Written out mathematically, the system
linearized about this point would be written as: