search

sympy / sympy

A computer algebra system written in pure Python
https://sympy.org/
Other
12.77k stars 4.38k forks source link

sympy.physics.mechanics // velocity constraints #24011

Closed Peter230655 closed 10 months ago

Peter230655 commented 2 years ago

I have been playing around with a simple n+1 particle pendulum. The first particle may only move in X direction. The particles are connected by (unbendable) springs and dampers. Using velocity constraints, the particles may be forced to only move in X or in Y direction.

Now, if I force the LAST particle to move only in a prescribed direction, all seems to work fine. Total energy (without friction) remains constant, the animation looks 'reasonable'.

However, if I try to constrain ANY OTHER particle the numerical integration fails badly.

On mechanical grounds, I see no reason, why the last particle should be special. I wonder what is going on here.

Any help / hints will be greatly appreciated.

The program is attached below.

Thanks! Peter

import sympy.physics.mechanics as me
import sympy as sm
from scipy.integrate import odeint, solve_ivp
import numpy as np
from scipy.optimize import fsolve
import matplotlib.pyplot as plt
from matplotlib import animation
from IPython.display import HTML
import matplotlib
matplotlib.rcParams['animation.embed_limit'] = 2**128

import time

'''
n+1 body 2D pendulum. Bodies in n_rest are restricted non - holonomically to move either only 
vertically or only horizontally. P0 may only move horizontally.
For some reason, only the last body can be restricted, else numerical issues appear.
''' 
# Input
#================================================================================================
n = 6            # number of bodies, called P0.....Pn. P0 is fixed
cse = True       # cse=True is about 10 times faster than cse=False 

# n_rest holds the 'particles' which are to be cnstraint by speed constraints.
n_rest = [6]      # Only if the last particle is restricted it will work

# wie contains whether the particle can move horizontally only or vertically only
wie = ['v']
#================================================================================================
start = time.time()

# Defining the variables needed
m, g, l, l0, reibung, feder = sm.symbols('m g l l0 reibung feder')
t = sm.symbols('t')

auxy, fy = me.dynamicsymbols(' auxy fy')   # reaction forces for first point

q = []            # angle of rotation for frame_i wrt frame N
u = []

x = []            # location of point P_i wrt frame_i
ux = []

A = []
P = []

aux = []          # for the reaction forces
KRAFT = []

for i in range(0, n):
    q.append(me.dynamicsymbols('q' + str(i)))
    u.append(me.dynamicsymbols('u' + str(i)))

    x.append(me.dynamicsymbols('x' + str(i)))
    ux.append(me.dynamicsymbols('ux' + str(i)))

for i in range(n+1):
    A.append(me.ReferenceFrame('A' + str(i)))
    P.append(me.Point('P' + str(i)))

    aux.append(me.dynamicsymbols('aux' + str(i) + 'xy'))
    KRAFT.append(me.dynamicsymbols('F' + str(i) + 'xy'))

auxy, fy = me.dynamicsymbols(' auxy fy')    # for the reaction forces on P[0]
x00, ux00 = me.dynamicsymbols('x00, ux00')  # P[0] may move in horizontal direction

#represent the rhs, needed for reaction forces, calculated numerically later. KM.rhs() is way too large!
rhs = [sm.symbols('rhs' + str(i)) for i in range(4*n + 2)] 

#setting up the geometry
BODY = []
speed_store = []

# Inertial frame
N = me.ReferenceFrame('N')
P0 = me.Point('P0')
P0.set_vel(N, 0)

A[0] = N

P[0].set_pos(P0, x00 * N.x)
P[0].set_vel(N, ux00*N.x + auxy*N.y)      #P[0] is fixed, these are virtual velocities

BODY.append(me.Particle('P0a', P[0], m))
speed_store.append(P[0].vel(N))

for i in range(1, n+1):
    A[i].orient_axis(N, q[i-1], N.z)
    A[i].set_ang_vel(A[i-1], u[i-1] * A[i].z)
    P[i].set_pos(P[i-1], x[i-1]*A[i].x)
    P[i].set_vel(A[i], ux[i-1]*A[i].x)
    P[i].v1pt_theory(P[i-1], N, A[i])
    BODY.append(me.Particle('P' + str(i) + 'a', P[i], m))
    speed_store.append(P[i].vel(N))

#kimenatic equations
kd = [*[u[i] - q[i].diff(t) for i in range(n)]] + [*[ux[i] - x[i].diff(t)
                    for i in range(n)]] + [ux00 - x00.diff(t)]

#speed constraints, reaction forces
speed_constr = []
Kraft1 = [(P[0], fy*N.y)]
auxecht = [auxy]

for i in n_rest:
    k = n_rest.index(i)   #find out whether horizontal or vertical restriction
    richtung = wie[k]

    if richtung == 'h':   # P[i] can only move horizontally
        speed_constr.append(me.dot(P[i].vel(N), N.y))
        P[i].set_vel(N, speed_store[i] + aux[i] * N.y)
        Kraft1.append((P[i], KRAFT[i] * N.y))
        auxecht.append(aux[i])
    elif richtung == 'v':
        speed_constr. append(me.dot(P[i].vel(N), N.x))
        P[i].set_vel(N, speed_store[i] + aux[i] * N.x)
        Kraft1.append((P[i], KRAFT[i] * N.x))
        auxecht.append(aux[i])
    else:
        raise Exception('wrong entry in wie')

# forces, torques
Moment = []
Reibung = []
for i in range(n+1):
    Moment.append((A[i], -reibung*u[i-1]*A[i].z))
    Reibung.append((P[i], -reibung*ux[i-1]*A[i].x))

# forces due to the springs. P0, P[n] must be treated separately
Kraft2 = []  
Kraft2.append((P[0], -m*g*N.y + feder*(x[0] - l0)*A[1].x ))

for i in range(1, n):
    Kraft2.append((P[i], -m*g*N.y - feder*(x[i-1] - l0)*A[i].x + feder*(x[i] - l0)*A[i+1].x))

Kraft2.append((P[n], -m*g*N.y - feder*(x[n-1] - l0) * A[n].x))

FL = Kraft1 + Kraft2 + Reibung + Moment

q_ind = [x00] + x + q
u_ind = [ux00] + ux + [u[k-1] for k in range(1, n+1) if k not in n_rest]
u_dep = [u[k-1] for k in n_rest]

KM = me.KanesMethod(N, q_ind=q_ind, u_ind=u_ind, u_dependent=u_dep, kd_eqs=kd, u_auxiliary=auxecht,
                    velocity_constraints=speed_constr)
(fr, frstar) = KM.kanes_equations(BODY, FL)

MM = KM.mass_matrix_full
print('MM shape:', MM.shape)
print('MM free symbols', MM.free_symbols)
print('MM DS', me.find_dynamicsymbols(MM))
print('MM contains {} operations'.format(sum([MM[i, j].count_ops(visual=False) 
                    for i in range(MM.shape[0]) for j in range(MM.shape[1])])), '\n')

force_dict = {KRAFT[i]: sm.S(0) for i in range(len(KRAFT))} # as per J. Moore this is correct
aux_dict = {i: sm.S(0) for i in auxecht}
Daux_dict  = {i.diff(t): sm.S(0) for i in auxecht}
force = KM.forcing_full.subs(force_dict).subs(Daux_dict).subs(aux_dict)
print('force shape:', force.shape)
print('force free symbols', force.free_symbols)
print('force DS', me.find_dynamicsymbols(force))
print('force contains {} operations'.format(sum([force[i].count_ops(visual=False) 
                            for i in range(force.shape[0])])), '\n')

rhs_dict = {sm.Derivative(i, t): rhs[j] for j, i in enumerate([x00] + x + q + [ux00] + ux + u)} #rhs to be calculated numerically later

eingepraegt = KM.auxiliary_eqs.subs(rhs_dict)
print('eingepraegt shape:', eingepraegt.shape)
print('eingepraegt free symbold', eingepraegt.free_symbols)
print('eingepraegt DS', me.find_dynamicsymbols(eingepraegt))
print('eingepraegt contains {} operations'.format(sum([eingepraegt[i].count_ops(visual=False) 
                            for i in range(eingepraegt.shape[0])])), '\n')

# Carthesian coordinates of the points
orte = [[me.dot(P[i].pos_from(P0), uv) for uv in (N.x, N.y)] for i in range(n+1)]

# absent any friction, the total energy must be constant, a necessary condition for the equations to
# be correct
kin_dict1 = {q[i].diff(t): u[i] for i in range(n)}
kin_dict2 = {i: 0. for i in auxecht}

pot_energie = m * g * sum([me.dot(P[k].pos_from(P0), N.y) for k in range(n+1)])
kin_energie = sum(([BODY[k].kinetic_energy(N) for k in range(n+1)])).subs(kin_dict1).subs(kin_dict2)
feder_energie = 0.5 * feder * sum([(x[i] - l0)**2 for i in range(n)])

#Lambdification

qL = [x00] + x + q + [ux00] + ux + u
pL = [m, g, l, l0, reibung, feder]
F1 = [fy] + [KRAFT[i] for i in n_rest]

MM_lam = sm.lambdify(qL + pL, MM, cse=cse)
force_lam = sm.lambdify(qL + pL, force, cse=cse)

eingepraegt_lam = sm.lambdify(F1 + qL + pL + rhs, eingepraegt, cse=cse)
orte_lam = sm.lambdify(qL + pL, orte)

pot_lam = sm.lambdify(qL + pL, pot_energie)
kin_lam = sm.lambdify(qL + pL, kin_energie)
feder_lam = sm.lambdify(qL + pL, feder_energie)

print('it took {:.3f} sec to establish Kanes equations'.format(time.time() - start))

# Numerical Integration

start = time.time()

#qL = x + q + uvar + u_ind + u_dep
#pL = [m, g, l, l0, reibung, feder]

# Input variables
#============================================================================
m1 = 1.                           # mass of particle
l1 = 1.                           # initial distance of particles from eeach other
l01 = 1.                          # natural length of spring
reibung1 = 0.                     # friction
feder1 = 250.                     # spring constant

intervall = 15
schritte = 300
pL_vals = [1., 9.8, 1., 1., 0., 100.]

x1 = [ l1 for i in range(n+1)]
ux1 = [0. for i in range(n+1)]
q1 = [np.pi/4. for i in range(n)]
u1 = [0. for i in range(n)]

#=====================================================================================

y0 = x1 + q1 + ux1 + u1
pL_vals = [m1, 9.8, l1, l01, reibung1, feder1 ]

#
height_start = []
#height_start = [(orte_lam(*y0, *pL_vals)[0][1], 1)]  #Start Y - coordinate on attachment point P0
for i in n_rest:
    k = n_rest.index(i)
    if wie[k] == 'h':
        zeiger = 1
    else:
        zeiger =0
    height_start.append((orte_lam(*y0, *pL_vals)[i][zeiger], zeiger))

times = np.linspace(0, intervall, schritte)
t_span = (0., intervall)

def gradient(t, y, args):
    sol = np.linalg.solve(MM_lam(*y, *args), force_lam(*y, *args))
    return np.array(sol).T[0]

# method = 'radau' give the best results in that total energy = constant, if reibung = 0.
resultat1 = solve_ivp(gradient, t_span, y0, t_eval = times, args=(pL_vals,), method='Radau')

resultat = resultat1.y.T
event_dict = {-1: 'Integration failed', 0: 'Integration finished successfully', 1: 'some termination event'}
print(event_dict[resultat1.status])

print("To numerically integrate an intervall of {} sec with cse = {}, the routine cycled {} times and it took {:.3f} sec "
      .format(intervall, cse, resultat1.nfev, time.time() - start), '\n')
print('resultat shape', resultat.shape)

# A holonomic constraint (X = const) has to be approximated by a non-holonomic constraint(d/dt(X) = 0). 
# This give the maximum error due to this.
max_deviation = []
for k, ll in enumerate(n_rest):
    max_deviation.append(max([np.abs(orte_lam(*[resultat[i,j] 
        for j in range(resultat.shape[1])] , *pL_vals)[ll][height_start[k][1]] - height_start[k][0]) 
        for i in range(resultat.shape[0])]))

for i, ll in enumerate(n_rest):
    print('largest deviation of point {} was {:.3f} distance units'.format(ll, np.abs(max_deviation[i])))

#Print coordinate

fig, ax = plt.subplots(figsize=(15, 10))
for i in range(0, resultat.shape[1]):
    ax.plot(times[: resultat.shape[0]], resultat[:, i], label = str(i))
ax.legend();

#energies
pot_np = np.empty(resultat.shape[0])
kin_np = np.empty(resultat.shape[0])
feder_np = np.empty(resultat.shape[0])
total_np = np.empty(resultat.shape[0])

for i in range(resultat.shape[0]):
    pot_np[i] = pot_lam(*[resultat[i, j] for j in range(resultat.shape[1])], *pL_vals)
    kin_np[i] = kin_lam(*[resultat[i, j] for j in range(resultat.shape[1])], *pL_vals)
    feder_np[i] = feder_lam(*[resultat[i, j] for j in range(resultat.shape[1])], *pL_vals)
    total_np[i] = pot_np[i] + kin_np[i] + feder_np[i]

fig, ax = plt.subplots(figsize=(15, 10))
ax.plot(times[: resultat.shape[0]], pot_np, label='potential energy')
ax.plot(times[: resultat.shape[0]], kin_np, label='kinetic energy')
ax.plot(times[: resultat.shape[0]], feder_np, label='spring energy')
ax.plot(times[: resultat.shape[0]], total_np, label='total energy')
ax.set_title('Energies of {} bodies, friction = {} units'.format(n+1, reibung1))
ax.legend();

# Print forces
# rhs calculated numerically, needed for reaction forces ( = eingepraegt)

start = time.time()

RHS = np.zeros((resultat.shape[0], resultat.shape[1]))
for i in range(resultat.shape[0]):
    zeit = times[i]
    RHS[i, :] = np.linalg.solve(MM_lam(*[resultat[i, j]for j in range(resultat.shape[1])], *pL_vals), 
                          force_lam(*[resultat[i, j] for j in range(resultat.shape[1])], *pL_vals)).reshape(resultat.shape[1])
print('RHS shape', RHS.shape)

#calculate reaction forces numerically

def func (x, *args):
    kraft11 = eingepraegt_lam(*x, *args)
    return kraft11.reshape(len(F1))

KRAFT1 = np.empty((resultat.shape[0], len(auxecht)))    
x0 = tuple(([100. for i in range(len(auxecht))]))   #erster Startwert

for i in range(resultat.shape[0]):
    y0 = [resultat[i, j] for j in range(resultat.shape[1])]
    rhs = [RHS[i, j] for j in range(resultat.shape[1])]
    args = tuple(y0 + pL_vals + rhs)
    A = fsolve(func, x0, args=args)#.reshape(len(auxecht))
    x0 = tuple(A)      #neuer Startwert = vorheriges Resultat, sollte Konvergenz beschleunigen
    KRAFT1[i] = A

fig, ax = plt.subplots(figsize=(12,6))
for i, j in enumerate(('P[0] in Y - direction', *[n_rest[k] for k in range(len(n_rest))])):
    plt.plot(times[: resultat.shape[0]], KRAFT1[:, i], label='force on body ' + str(j))
plt.legend();
print('it took {:.3f} sec to calulate and plot the reaction forces'.format(time.time() - start))

#Animation
start = time.time()

# get Carthesian coordinates
x_coords = np.zeros((resultat.shape[0], n+1))
y_coords = np.zeros((resultat.shape[0], n+1))

for j in range(resultat.shape[0]):
    x_coords[j] = [orte_lam(*[resultat[j, k] for k in range(resultat.shape[1])], *pL_vals)[l][0] for l in range(n+1)]
    y_coords[j] = [orte_lam(*[resultat[j, k] for k in range(resultat.shape[1])], *pL_vals)[l][1] for l in range(n+1)]

max_x = max([abs(x_coords[i, j])for i in range(resultat.shape[0]) for j in range(n+1)])
max_y = max([abs(y_coords[i, j])for i in range(resultat.shape[0]) for j in range(n+1)])
max_xy = max(max_x, max_y) + 0.5

# Ab hier das eigentliche Animationsprogramm

fig, ax = plt.subplots(figsize=(12, 12))
ax.axis('on')
ax.set(xlim=(-max_xy, max_xy), ylim=(-max_xy, max_xy))

plt.hlines(y_coords[0, 0], -max_xy, max_xy, linestyles='dashed') #für den Aufhängepunkt
for i in range(len(n_rest)):
    if wie[i] == 'h':
        plt.hlines(y_coords[0, n_rest[i]], -max_xy, max_xy, linestyles='dashed')
    else:
        plt.vlines(x_coords[0, n_rest[i]], -max_xy, max_xy, linestyles='dashed')

Lines = []
for i in range(n+1):     #markiert die Punkte, die roten Punkte sind restriktiert
    if i in n_rest or i == 0:
        color = 'red'
    else:
        color = 'blue'
    line, = ax.plot([], [], 'o-', lw=0.5, color=color, markersize=10)
    Lines.append(line)

line, = ax.plot([], [], 'o-', lw=0.5, color='blue', markersize=0) #connects the points
Lines.append(line)

def animate_pendulum(times, x, y):    
    def animate(i):
        for j in range(n+1):
            Lines[j].set_data(x[i, j], y[i, j])
        Lines[-1].set_data(x[i], y[i])
        ax.set_title('running time {:.2f} sec'.format(i/schritte*intervall), fontsize=20)
        return Lines

    anim = animation.FuncAnimation(fig, animate, frames=resultat.shape[0],
                                   interval=1000*times[: resultat.shape[0]].max() / resultat.shape[0],
                                   blit=True) #, init_func=init)
    plt.close(fig)
    return anim

anim = animate_pendulum(times[: resultat.shape[0]], x_coords, y_coords)
HTML(anim.to_jshtml())
Peter230655 commented 10 months ago

I do not think, an important issue, I will close it.