%matplotlib inline
Try running with this variable set to true and to false and see the difference in the resulting equations of motion
use_constraints = False
Import all the necessary modules
# -*- coding: utf-8 -*-
"""
Written by Daniel M. Aukes
Email: danaukes<at>gmail.com
Please see LICENSE for full license.
"""
import pynamics
from pynamics.frame import Frame
from pynamics.variable_types import Differentiable,Constant
from pynamics.system import System
from pynamics.body import Body
from pynamics.dyadic import Dyadic
from pynamics.output import Output,PointsOutput
from pynamics.particle import Particle
from pynamics.constraint import AccelerationConstraint
import pynamics.integration
import numpy
import sympy
import matplotlib.pyplot as plt
plt.ion()
from math import pi
The next two lines create a new system object and set that system as the global system within the module so that other variables can use and find it.
system = System()
pynamics.set_system(__name__,system)
Parameterization
Constants
Declare constants and seed them with their default value. This can be changed at integration time but is often a nice shortcut when you don’t want the value to change but you want it to be represented symbolically in calculations
lA = Constant(1,'lA',system)
lB = Constant(1,'lB',system)
lC = Constant(1,'lC',system)
mA = Constant(1,'mA',system)
mB = Constant(1,'mB',system)
mC = Constant(1,'mC',system)
g = Constant(9.81,'g',system)
b = Constant(1e1,'b',system)
k = Constant(1e1,'k',system)
preload1 = Constant(0*pi/180,'preload1',system)
preload2 = Constant(0*pi/180,'preload2',system)
preload3 = Constant(0*pi/180,'preload3',system)
Ixx_A = Constant(1,'Ixx_A',system)
Iyy_A = Constant(1,'Iyy_A',system)
Izz_A = Constant(1,'Izz_A',system)
Ixx_B = Constant(1,'Ixx_B',system)
Iyy_B = Constant(1,'Iyy_B',system)
Izz_B = Constant(1,'Izz_B',system)
Ixx_C = Constant(1,'Ixx_C',system)
Iyy_C = Constant(1,'Iyy_C',system)
Izz_C = Constant(1,'Izz_C',system)
torque = Constant(0,'torque',system)
freq = Constant(3e0,'freq',system)
Differentiable State Variables
Define your differentiable state variables that you will use to model the state of the system. In this case $qA$, $qB$, and $qC$ are the rotation angles of a three-link mechanism
qA,qA_d,qA_dd = Differentiable('qA',system)
qB,qB_d,qB_dd = Differentiable('qB',system)
qC,qC_d,qC_dd = Differentiable('qC',system)
Initial Values
Define a set of initial values for the position and velocity of each of your state variables. It is necessary to define a known. This code create a dictionary of initial values.
initialvalues = {}
initialvalues[qA]=0*pi/180
initialvalues[qA_d]=0*pi/180
initialvalues[qB]=0*pi/180
initialvalues[qB_d]=0*pi/180
initialvalues[qC]=0*pi/180
initialvalues[qC_d]=0*pi/180
These two lines of code order the initial values in a list in such a way that the integrator can use it in the same order that it expects the variables to be supplied
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
Kinematics
Frames
Define the reference frames of the system
N = Frame('N',system)
A = Frame('A',system)
B = Frame('B',system)
C = Frame('C',system)
Newtonian Frame
It is important to define the Newtonian reference frame as a reference frame that is not accelerating, otherwise the dynamic equations will not be correct
system.set_newtonian(N)
Rotate each successive frame by amount q
A.rotate_fixed_axis(N,[0,0,1],qA,system)
B.rotate_fixed_axis(A,[0,0,1],qB,system)
C.rotate_fixed_axis(B,[0,0,1],qC,system)
Vectors
Define the vectors that describe the kinematics of a series of connected lengths
- pNA - This is a vector with position at the origin.
- pAB - This vector is length $l_A$ away from the origin along the A.x unit vector
- pBC - This vector is length $l_B$ away from the pAB along the B.x unit vector
- pCtip - This vector is length $l_C$ away from the pBC along the C.x unit vector
Define my rigid body kinematics
pNA=0*N.x
pAB=pNA+lA*A.x
pBC = pAB + lB*B.x
pCtip = pBC + lC*C.x
Centers of Mass
It is important to define the centers of mass of each link. In this case, the center of mass of link A, B, and C is halfway along the length of each
pAcm=pNA+lA/2*A.x
pBcm=pAB+lB/2*B.x
pCcm=pBC+lC/2*C.x
Calculating Velocity
The angular velocity between frames, and the time derivatives of vectors are extremely useful in calculating the equations of motion and for determining many of the forces that need to be applied to your system (damping, drag, etc). Thus, it is useful, once kinematics have been defined, to take or find the derivatives of some of those vectors for calculating linear or angular velocity vectors
Angular Velocity
The following three lines of code computes and returns the angular velocity between frames N and A (${}^N\omega^A$), A and B (${}^A\omega^B$), and B and C (${}^B\omega^C$). In other cases, if the derivative expression is complex or long, you can supply pynamics with a given angular velocity between frames to speed up computation time.
wNA = N.get_w_to(A)
wAB = A.get_w_to(B)
wBC = B.get_w_to(C)
Vector derivatives
The time derivatives of vectors may also be
vCtip = pCtip.time_derivative(N,system)
Define Inertias and Bodies
The next several lines compute the inertia dyadics of each body and define a rigid body on each frame. In the case of frame C, we represent the mass as a particle located at point pCcm.
IA = Dyadic.build(A,Ixx_A,Iyy_A,Izz_A)
IB = Dyadic.build(B,Ixx_B,Iyy_B,Izz_B)
IC = Dyadic.build(C,Ixx_C,Iyy_C,Izz_C)
BodyA = Body('BodyA',A,pAcm,mA,IA,system)
BodyB = Body('BodyB',B,pBcm,mB,IB,system)
BodyC = Body('BodyC',C,pCcm,mC,IC,system)
#BodyC = Particle(pCcm,mC,'ParticleC',system)
Forces and Torques
Forces and torques are added to the system with the generic addforce
method. The first parameter supplied is a vector describing the force applied at a point or the torque applied along a given rotational axis. The second parameter is the vector describing the linear speed (for an applied force) or the angular velocity(for an applied torque)
system.addforce(torque*sympy.sin(freq*2*sympy.pi*system.t)*A.z,wNA)
<pynamics.force.Force at 0x7fb36eb9f5e0>
Damper
system.addforce(-b*wNA,wNA)
system.addforce(-b*wAB,wAB)
system.addforce(-b*wBC,wBC)
<pynamics.force.Force at 0x7fb36eab5430>
Spring Forces
Spring forces are a special case because the energy stored in springs is conservative and should be considered when calculating the system’s potential energy. To do this, use the add_spring_force
command. In this method, the first value is the linear spring constant. The second value is the “stretch” vector, indicating the amount of deflection from the neutral point of the spring. The final parameter is, as above, the linear or angluar velocity vector (depending on whether your spring is a linear or torsional spring)
In this case, the torques applied to each joint are dependent upon whether qA, qB, and qC are absolute or relative rotations, as defined above.
system.add_spring_force1(k,(qA-preload1)*N.z,wNA)
system.add_spring_force1(k,(qB-preload2)*A.z,wAB)
system.add_spring_force1(k,(qC-preload3)*B.z,wBC)
(<pynamics.force.Force at 0x7fb36eabafd0>,
<pynamics.spring.Spring at 0x7fb36eabad90>)
Gravity
Again, like springs, the force of gravity is conservative and should be applied to all bodies. To globally apply the force of gravity to all particles and bodies, you can use the special addforcegravity
method, by supplying the acceleration due to gravity as a vector. This will get applied to all bodies defined in your system.
system.addforcegravity(-g*N.y)
Constraints
Constraints may be defined that prevent the motion of certain elements. Try turning on the constraints flag at the top of the script to see what happens.
if use_constraints:
eq = []
eq.append(pCtip)
eq_d=[item.time_derivative() for item in eq]
eq_dd=[item.time_derivative() for item in eq_d]
eq_dd_scalar = []
#eq_dd_scalar.append(eq_dd[0].dot(N.x))
eq_dd_scalar.append(eq_dd[0].dot(N.x))
constraint = AccelerationConstraint(eq_dd_scalar)
system.add_constraint(constraint)
F=ma
This is where the symbolic expressions for F and ma are calculated. This must be done after all parts of the system have been defined. The getdynamics
function uses Kane’s method to derive the equations of motion.
f,ma = system.getdynamics()
2022-03-02 15:19:02,917 - pynamics.system - INFO - getting dynamic equations
f
[-b*qA_d - g*lA*mA*cos(qA)/2 - g*lA*mB*cos(qA) - g*lA*mC*cos(qA) + g*lB*mB*sin(qA)*sin(qB)/2 - g*lB*mB*cos(qA)*cos(qB)/2 + g*lB*mC*sin(qA)*sin(qB) - g*lB*mC*cos(qA)*cos(qB) - g*lC*mC*(-sin(qB)*sin(qC) + cos(qB)*cos(qC))*cos(qA)/2 - g*lC*mC*(-sin(qB)*cos(qC) - sin(qC)*cos(qB))*sin(qA)/2 - k*(-preload1 + qA) + torque*sin(2*freq*pi*t), -b*qB_d + g*lB*mB*sin(qA)*sin(qB)/2 - g*lB*mB*cos(qA)*cos(qB)/2 + g*lB*mC*sin(qA)*sin(qB) - g*lB*mC*cos(qA)*cos(qB) - g*lC*mC*(-sin(qB)*sin(qC) + cos(qB)*cos(qC))*cos(qA)/2 - g*lC*mC*(-sin(qB)*cos(qC) - sin(qC)*cos(qB))*sin(qA)/2 - k*(-preload2 + qB), -b*qC_d - g*lC*mC*(-sin(qB)*sin(qC) + cos(qB)*cos(qC))*cos(qA)/2 - g*lC*mC*(-sin(qB)*cos(qC) - sin(qC)*cos(qB))*sin(qA)/2 - k*(-preload3 + qC)]
ma
[Izz_A*qA_dd + Izz_B*(qA_dd + qB_dd) + Izz_C*(qA_dd + qB_dd + qC_dd) + lA**2*mA*qA_dd/4 + lA**2*mB*qA_dd + lA**2*mC*qA_dd + lA*lB*mB*qA_d**2*sin(qB)/2 + lA*lB*mB*qA_dd*cos(qB)/2 - lA*lB*mB*(qA_d + qB_d)**2*sin(qB)/2 + lA*lB*mC*qA_d**2*sin(qB) + lA*lB*mC*qA_dd*cos(qB) - lA*lB*mC*(qA_d + qB_d)**2*sin(qB) + lA*mB*(lB*qA_dd/2 + lB*qB_dd/2)*cos(qB) + lA*mC*(lB*qA_dd + lB*qB_dd)*cos(qB) + lA*(-lC*mC*(qA_d + qB_d + qC_d)**2*sin(qC)/2 + mC*(lC*qA_dd/2 + lC*qB_dd/2 + lC*qC_dd/2)*cos(qC))*cos(qB) + lA*(-lC*mC*(qA_d + qB_d + qC_d)**2*cos(qC)/2 - mC*(lC*qA_dd/2 + lC*qB_dd/2 + lC*qC_dd/2)*sin(qC))*sin(qB) + lB*lC*mC*(qA_d + qB_d)**2*sin(qC)/2 - lB*lC*mC*(qA_d + qB_d + qC_d)**2*sin(qC)/2 + lB*mB*(lB*qA_dd/2 + lB*qB_dd/2)/2 + lB*mC*(lB*qA_dd + lB*qB_dd) + lB*mC*(lC*qA_dd/2 + lC*qB_dd/2 + lC*qC_dd/2)*cos(qC) + lC*mC*(lB*qA_dd + lB*qB_dd)*cos(qC)/2 + lC*mC*(lC*qA_dd/2 + lC*qB_dd/2 + lC*qC_dd/2)/2 + lC*(lA*mC*qA_d**2*sin(qB) + lA*mC*qA_dd*cos(qB))*cos(qC)/2 - lC*(-lA*mC*qA_d**2*cos(qB) + lA*mC*qA_dd*sin(qB))*sin(qC)/2, Izz_B*(qA_dd + qB_dd) + Izz_C*(qA_dd + qB_dd + qC_dd) + lA*lB*mB*qA_d**2*sin(qB)/2 + lA*lB*mB*qA_dd*cos(qB)/2 + lA*lB*mC*qA_d**2*sin(qB) + lA*lB*mC*qA_dd*cos(qB) + lB*lC*mC*(qA_d + qB_d)**2*sin(qC)/2 - lB*lC*mC*(qA_d + qB_d + qC_d)**2*sin(qC)/2 + lB*mB*(lB*qA_dd/2 + lB*qB_dd/2)/2 + lB*mC*(lB*qA_dd + lB*qB_dd) + lB*mC*(lC*qA_dd/2 + lC*qB_dd/2 + lC*qC_dd/2)*cos(qC) + lC*mC*(lB*qA_dd + lB*qB_dd)*cos(qC)/2 + lC*mC*(lC*qA_dd/2 + lC*qB_dd/2 + lC*qC_dd/2)/2 + lC*(lA*mC*qA_d**2*sin(qB) + lA*mC*qA_dd*cos(qB))*cos(qC)/2 - lC*(-lA*mC*qA_d**2*cos(qB) + lA*mC*qA_dd*sin(qB))*sin(qC)/2, Izz_C*(qA_dd + qB_dd + qC_dd) + lB*lC*mC*(qA_d + qB_d)**2*sin(qC)/2 + lC*mC*(lB*qA_dd + lB*qB_dd)*cos(qC)/2 + lC*mC*(lC*qA_dd/2 + lC*qB_dd/2 + lC*qC_dd/2)/2 + lC*(lA*mC*qA_d**2*sin(qB) + lA*mC*qA_dd*cos(qB))*cos(qC)/2 - lC*(-lA*mC*qA_d**2*cos(qB) + lA*mC*qA_dd*sin(qB))*sin(qC)/2]
Solve for Acceleration
The next line of code solves the system of equations F=ma plus any constraint equations that have been added above. It returns one or two variables. func1 is the function that computes the velocity and acceleration given a certain state, and lambda1(optional) supplies the function that computes the constraint forces as a function of the resulting states
There are a few ways of solveing for a. The below function inverts the mass matrix numerically every time step. This can be slower because the matrix solution has to be solved for, but is sometimes more tractable than solving the highly nonlinear symbolic expressions that can be generated from the previous step. The other options would be to use state_space_pre_invert
, which pre-inverts the equations symbolically before generating a numerical function, or state_space_post_invert2
, which adds Baumgarte’s method for intermittent constraints.
func1,lambda1 = system.state_space_post_invert(f,ma,return_lambda = True)
2022-03-02 15:19:03,319 - pynamics.system - INFO - solving a = f/m and creating function
2022-03-02 15:19:03,426 - pynamics.system - INFO - substituting constrained in Ma-f.
2022-03-02 15:19:03,497 - pynamics.system - INFO - done solving a = f/m and creating function
2022-03-02 15:19:03,497 - pynamics.system - INFO - calculating function for lambdas
Integration Tolerance
Specify the precision of the integration
tol = 1e-7
Time
Define variables for time that can be used throughout the script. These get used to create the t array, a list of every time value that is solved for during integration
tinitial = 0
tfinal = 10
fps = 30
tstep = 1/fps
t = numpy.r_[tinitial:tfinal:tstep]
Integrate
The next line of code integrates the function calculated
states=pynamics.integration.integrate(func1,ini,t,rtol=tol,atol=tol, args=({'constants':system.constant_values},))
2022-03-02 15:19:03,563 - pynamics.integration - INFO - beginning integration
2022-03-02 15:19:03,565 - pynamics.system - INFO - integration at time 0000.00
2022-03-02 15:19:03,772 - pynamics.integration - INFO - finished integration
Outputs
The next section simply calculates and plots a variety of data from the previous simulation
States
plt.figure()
artists = plt.plot(t,states[:,:3])
plt.legend(artists,['qA','qB','qC'])
<matplotlib.legend.Legend at 0x7fb36eaa6430>
Energy
KE = system.get_KE()
PE = system.getPEGravity(pNA) - system.getPESprings()
energy_output = Output([KE-PE],system)
energy_output.calc(states,t)
energy_output.plot_time()
2022-03-02 15:19:04,061 - pynamics.output - INFO - calculating outputs
2022-03-02 15:19:04,072 - pynamics.output - INFO - done calculating outputs
Constraint Forces
This line of code computes the constraint forces once the system’s states have been solved for.
if use_constraints:
lambda2 = numpy.array([lambda1(item1,item2,system.constant_values) for item1,item2 in zip(t,states)])
plt.figure()
plt.plot(t, lambda2)
Motion
points = [pNA,pAB,pBC,pCtip]
points_output = PointsOutput(points,system)
y = points_output.calc(states,t)
points_output.plot_time(20)
2022-03-02 15:19:04,217 - pynamics.output - INFO - calculating outputs
2022-03-02 15:19:04,232 - pynamics.output - INFO - done calculating outputs
<AxesSubplot:>
Motion Animation
in normal Python the next lines of code produce an animation using matplotlib
points_output.animate(fps = fps,movie_name = 'triple_pendulum.mp4',lw=2,marker='o',color=(1,0,0,1),linestyle='-')
To plot the animation in jupyter you need a couple extra lines of code…
from matplotlib import animation, rc
from IPython.display import HTML
HTML(points_output.anim.to_html5_video())