import sympy
import numpy
from matplotlib import pyplot
from sympy import symbols
from IPython.display import display, Latex
sympy.init_printing(use_latex=True)
from sympy.utilities.lambdify import lambdify
numpy.set_printoptions(linewidth=300)
from numpy import dot
class ANCF(object):
def __init__(self, var = symbols('x'), dimension = 2, l = 1.0, rho = 1000.0, A = 0.4, I = 0.03, E = 1000000000.0):
self.var = var
self.dimension = dimension
self.g = -9.81
self.l = l# Length of beam element
# The following are arbitrary values
self.rho = rho # Density (mass per volume)
self.A = A # Area of cross section
self.I = I # Inertia of cross section
self.E = E # Youngs modulus
self.e = sympy.Matrix(sympy.symbols(['r[%d]'%i for i in range(8)]))
self.S = self.phi_matrix()
self.r = self.S * self.e
var, e = self.var, self.e
self.r_fnc = lambdify([e[0], e[1], e[2], e[3], e[4], e[5], e[6], e[7], var], self.r)
self.rx = self.r.diff(self.var)
self.rxx = self.r.diff(self.var, 2)
def basis_functions(self):
var = self.var
return [1-3*var**2+2*var**3, var-2*var**2+var**3, 3*var**2-2*var**3, -var**2+var**3]
def phi_matrix(self):
phi, l, eye = self.basis_functions(), self.l, sympy.eye(self.dimension)
return sympy.Matrix([phi[0] * eye, l * phi[1] * eye, phi[2] * eye, l * phi[3] * eye]).T
def gravitational_force(self):
# Taken from Equation 18 in "Application of the Absolute Nodal Coordinate Formulation to multibody system dynamics"
# by "J.L. Escalona, H.A. Hussein and A.A. Shabana, May 1997
rho, A, x, S = self.rho, self.A, self.var, self.phi_matrix()
return rho * A * sympy.integrate(self.g * S[1,:], (x, 0, self.l))
def mass_matrix(self):
# Taken from Equation 8 in "Application of the Absolute Nodal Coordinate Formulation to multibody system dynamics"
# by "J.L. Escalona, H.A. Hussein and A.A. Shabana, May 1997
rho, x, S = self.rho, self.var, self.phi_matrix()
return sympy.integrate(self.rho * S.T * S, (x, 0, self.l))
def strain_energy_longitudinal_deformation(self):
# Strain energy based on the continuum mechanics formulation in
# Development of simple models for the elastic forces in the absolute nodal co-ordinate formulation"
# by "Berzeri, M and Shabana, Ahmed A", 2000
# Strain energy due to longitudinal deformation
rxTrx = (self.rx.T * self.rx)[0]
return 0.5 * self.E * self.A * sympy.integrate((rxTrx - 1)**2, (self.var, 0, self.l))
def strain_energy_curvature_function(self):
# Strain energy based on the continuum mechanics formulation in
# Development of simple models for the elastic forces in the absolute nodal co-ordinate formulation"
# by "Berzeri, M and Shabana, Ahmed A", 2000
# Strain energy due to curvature
rx, rxx = self.rx, self.rxx
# Values for the Gauss-Legendre quadrature
# https://en.wikipedia.org/wiki/Gaussian_quadrature
x_vals = list(map(lambda val: self.l*(val+1.0)/2.0, [0.0,0.4058451513773972,-0.4058451513773972,-0.7415311855993945,0.7415311855993945,-0.9491079123427585,0.9491079123427585]))
weights = [0.4179591836734694, 0.3818300505051189, 0.3818300505051189, 0.2797053914892766, 0.2797053914892766, 0.1294849661688697, 0.1294849661688697]
def cross_prod_2d(a, b):
return sympy.Matrix([a[0] * b[1] - a[1] * b[0]])
num = (cross_prod_2d(rx, rxx).T * cross_prod_2d(rx, rxx))[0,0]
den = (rx.T*rx * rx.T*rx * rx.T*rx)[0,0]
expr = self.E * self.I * num / den
def integrate_expr(nodal_coordinates):
nc_subs = list(zip(e, nodal_coordinates))
expr_ = expr.subs(nc_subs)
res = 0.0
for x_val, weight in zip(x_vals, weights):
res += weight * expr_.subs({self.var : x_val})
return res
return integrate_expr
def strain_force_longitudinal_deformation(self):
# Strain energy based on the continuum mechanics formulation in
# Development of simple models for the elastic forces in the absolute nodal co-ordinate formulation"
# by "Berzeri, M and Shabana, Ahmed A", 2000
# Strain energy due to longitudinal deformation
seld = self.strain_energy_longitudinal_deformation()
return sympy.Matrix([seld.diff(ee) for ee in self.e])
def strain_force_curvature_function(self):
# Strain energy based on the continuum mechanics formulation in
# Development of simple models for the elastic forces in the absolute nodal co-ordinate formulation"
# by "Berzeri, M and Shabana, Ahmed A", 2000
# The force of strain due to curvature
rx, rxx = self.rx, self.rxx
# Values for the Gauss-Legendre quadrature
# https://en.wikipedia.org/wiki/Gaussian_quadrature
x_vals = list(map(lambda val: self.l*(val+1.0)/2.0, [0.0,0.4058451513773972,-0.4058451513773972,-0.7415311855993945,0.7415311855993945,-0.9491079123427585,0.9491079123427585]))
weights = [0.4179591836734694, 0.3818300505051189, 0.3818300505051189, 0.2797053914892766, 0.2797053914892766, 0.1294849661688697, 0.1294849661688697]
def cross_prod_2d(a, b):
return sympy.Matrix([a[0] * b[1] - a[1] * b[0]])
num = (cross_prod_2d(rx, rxx).T * cross_prod_2d(rx, rxx))[0,0]
den = (rx.T*rx * rx.T*rx * rx.T*rx)[0,0]
expr = self.E * self.I * num / den
dexpr_de = sympy.Matrix([expr.diff(ee) for ee in self.e])
def integrate_expr(nodal_coordinates):
nc_subs = list(zip(self.e, nodal_coordinates))
dexpr_de_ = dexpr_de.subs(nc_subs)
res = sympy.zeros(8,1)
for x_val, weight in zip(x_vals, weights):
res += weight * dexpr_de_.subs({self.var : x_val})
return res
return integrate_expr
def force_all_function(self):
rx, rxx = self.rx, self.rxx
# Values for the Gauss-Legendre quadrature
# https://en.wikipedia.org/wiki/Gaussian_quadrature
x_vals = list(map(lambda val: self.l*(val+1.0)/2.0, [0.0,0.4058451513773972,-0.4058451513773972,-0.7415311855993945,0.7415311855993945,-0.9491079123427585,0.9491079123427585]))
weights = [0.4179591836734694, 0.3818300505051189, 0.3818300505051189, 0.2797053914892766, 0.2797053914892766, 0.1294849661688697, 0.1294849661688697]
rxTrx = (rx.T * rx)[0]
drxTrx_de = sympy.Matrix([rxTrx.diff(ee) for ee in self.e])
force1 = 0.5 * self.E * self.A * (2 * rxTrx - 2) * drxTrx_de
def cross_prod_2d(a, b):
return sympy.Matrix([a[0] * b[1] - a[1] * b[0]])
num = (cross_prod_2d(rx, rxx).T * cross_prod_2d(rx, rxx))[0,0]
den = rxTrx * rxTrx * rxTrx
expr = self.E * self.I * num / den
dexpr_de = sympy.Matrix([expr.diff(ee) for ee in self.e])
def integrate_expr(nodal_coordinates):
nc_subs = list(zip(self.e, nodal_coordinates))
dexpr_de_ = dexpr_de.subs(nc_subs)
force1_ = force1.subs(nc_subs)
res = numpy.array(self.gravitational_force()).flatten()
for x_val, weight in zip(x_vals, weights):
res += numpy.array(weight * dexpr_de_.subs({self.var : x_val})).flatten()
res += numpy.array(weight * force1_.subs({self.var : x_val})).flatten()
return res
return integrate_expr
class Beam(object):
def __init__(self, num_elements = 2, beam_length = 2.0, dimension = 2, rho = 1000.0, A = 0.4, I = 0.03, E = 1000000000.0):
element_length = beam_length / num_elements
self.ancf = ANCF(dimension = dimension, l = element_length, rho = rho, A = A, I = I, E = E)
self.num_elements = num_elements
self.num_nodes = num_elements + 1
self.mass_matrix = numpy.zeros((4 * self.num_nodes, 4 * self.num_nodes))
mm = numpy.array(self.ancf.mass_matrix()).astype(numpy.float)
for i in range(self.num_elements):
self.mass_matrix[4*i:4*(i+2),4*i:4*(i+2)] += mm
self.mass_matrix_inverse = numpy.linalg.inv(self.mass_matrix)
def get_state(self):
return self._state
def set_state(self, state):
self._state = state
state = property(get_state, set_state)
def get_constraint_matrix(self):
return self._constraint_matrix
def set_constraint_matrix(self, constraint_matrix):
self._constraint_matrix = constraint_matrix.copy()
Minv, C = self.mass_matrix_inverse, self._constraint_matrix
lambda_ = dot(C.T,dot(numpy.linalg.inv(dot(dot(C,Minv),C.T)),dot(C,Minv)))
self.mm = -dot(Minv,(numpy.eye(*lambda_.shape) - lambda_))
constraint_matrix = property(get_constraint_matrix, set_constraint_matrix)
def eval_kinematics(self, material_coordinate_array):
res = []
for xi in material_coordinate_array:
idx = int(xi / self.ancf.l)
vals = self.state[4 * idx : 4 * (idx+2)]
vals.append(xi - idx * self.ancf.l)
res.append(self.ancf.r_fnc(*vals).T.tolist()[0])
return res
def eval_force(self):
elements = [self.state[4*idx:4*(idx+2)] for idx in range(self.num_elements)]
force = numpy.zeros(4 * self.num_nodes)
lam_f = lambdify(self.ancf.e, self.ancf.strain_force_longitudinal_deformation())
def element_strain_force_longitudinal_deformation(nc):
return lam_f(nc[0], nc[1], nc[2], nc[3], nc[4], nc[5], nc[6], nc[7])
fnc1 = self.ancf.strain_force_curvature_function()
fnc2 = element_strain_force_longitudinal_deformation
gf = self.ancf.gravitational_force()
for idx in range(self.num_elements):
force[4*idx:4*(idx+2)] += numpy.array(fnc1(elements[idx])).flatten()
force[4*idx:4*(idx+2)] += numpy.array(fnc2(elements[idx])).flatten()
force[4*idx:4*(idx+2)] += numpy.array(gf).flatten()
return force
def eval_force2(self):
elements = [self.state[4*idx:4*(idx+2)] for idx in range(self.num_elements)]
force = numpy.zeros(4 * self.num_nodes)
faf = self.ancf.force_all_function()
for idx in range(self.num_elements):
element_force = numpy.array(faf(elements[idx])).astype(numpy.float).flatten()
force[4*idx:4*(idx+2)] += element_force
return force
def eval_acceleration(self):
return numpy.dot(self.mm, self.eval_force2())
beam = Beam(num_elements = 2, beam_length = 2.0)
beam.state = [0.0, 0.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 1.0, 0.0]
x,y = zip(*beam.eval_kinematics(numpy.linspace(0,1.9999,100)))
%matplotlib inline
pyplot.figure(figsize=(17,6))
pyplot.subplot(2,1,1)
pyplot.hold(True) # Deprecated in modern Pythons, gives a warning
pyplot.plot(x, y)
pyplot.plot(beam.state[0::4], beam.state[1::4], 'ro')
pyplot.ylim([-0.05, 1.02])
pyplot.xlim([-0.05, 2.05])
pyplot.subplot(2,1,2)
beam.state = [0.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 2.0, 0.0, 1.0, 0.0]
x,y = zip(*beam.eval_kinematics(numpy.linspace(0,1.9999,100)))
pyplot.hold(True) # Deprecated in modern Pythons, gives a warning
pyplot.plot(x, y)
pyplot.plot(beam.state[0::4], beam.state[1::4], 'ro')
pyplot.xlim([-0.05, 2.05])
pyplot.show()
beam.eval_force2()
array([ 0., -1962., 0., -327., 0., -3924., 0., 0., 0., -1962., 0., 327.])
numpy.dot(beam.mass_matrix_inverse, beam.eval_force2())
array([ 0.00000000e+00, -3.92400000e+00, 0.00000000e+00, 6.82121026e-13, 0.00000000e+00, -3.92400000e+00, 0.00000000e+00, 3.33948563e-13, 0.00000000e+00, -3.92400000e+00, 0.00000000e+00, 6.53699317e-13])
# Pin one end of the beam
C = numpy.zeros((4, 4 * beam.num_nodes))
C[0, 0] = C[1, 1] = C[2, 2] = C[3, 3] = 1.0
beam.constraint_matrix = C
numpy.dot(beam.mm, beam.eval_force2())
array([ 0.00000000e+00, 2.07940723e-14, 0.00000000e+00, -1.42631341e-13, 0.00000000e+00, 4.36059742e+00, 0.00000000e+00, -8.51687586e+00, 0.00000000e+00, 3.29061113e+00, 0.00000000e+00, -8.45235407e+00])
# This simulation is a bit slow. Have some patience.
simulation_results = [[0.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 2.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]
def compute(state):
beam.state = numpy.array(state)
yn = numpy.zeros(4 * beam.num_nodes * 2)
yn[0:4*beam.num_nodes] = state[4*beam.num_nodes:]
yn[4*beam.num_nodes:] = beam.eval_acceleration() #numpy.dot(mm, beam.eval_force2())
return yn
h = 0.0001
for i in range(60):
if i%10 == 0:
print(i)
yn = numpy.array(simulation_results[-1])
k1 = compute(yn)
k2 = compute(yn + h/2 * k1)
k3 = compute(yn + h/2 * k2)
k4 = compute(yn + h * k3)
tmp = yn + h/6 * (k1 + 2*k2 + 2*k3 + k4)
simulation_results.append(tmp.tolist())
0 10 20 30 40 50
for idx in range(len(simulation_results)):
print(idx)
beam.state = simulation_results[idx]
x,y = zip(*beam.eval_kinematics(numpy.linspace(0,1.9999,100)))
pyplot.figure(figsize=(17,6))
pyplot.hold(True)
pyplot.plot(x, y, linewidth=2)
pyplot.plot(beam.state[0:4*beam.num_nodes:4], beam.state[1:4*beam.num_nodes:4], 'ro')
pyplot.ylim([-0.0001, 0.0002])
pyplot.xlim([-0.01, 2.05])
pyplot.savefig('/tmp/movie/frame%06d.png'%idx)
pyplot.close()
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60