sympy.physics.mechanics / nonlinear motion constraints

[Peter230655]

I have been reading Dynamics by Carlos Roithmayr and Dewey Hodges, the updated version of Kane's original book.

I came across the topic motion constraints with nonlinear equations, something I had never come across before.
So, I tried to calculate the simple example given there, where $P_1, P_2$ move in the X/Y plane, such that ${}^N \bar v^{P_1} \circ {}^N \bar v^{P_2} = 0$, N being the inertial frame.

So I set
speed_constr = P1.vel(N).dot(P2.vel(N))

This did not work, there were accelerations in the force vector.
In the error message I noticed the keyword acceleration_constraints in KanesMethod, so I set
speed_constr_dt = speed_constr.diff(t)
and 'gave' this to acceleration_constraints. (I kept the velocity_constraints, too)

Now all seemed to work fine, the results looked 'reasonable', and speed_constr was close to 0 - as it should be.

My questions:
1.
Is my approach correct for nonlinear motion constraints?
2.
In the documentation, I did not find anything about acceleration constraints.
Did I overlook anything?

Any help is greatly appreciated!

[moorepants]

KanesMethod should automatically calculate the "acceleration constraints" from your velocity constraints, but if you want to calculate them yourself you can pass it to KanesMethod. This allows you to ensure all qdots and qddots are gone from the expressions, for example (although we may already do that).

[moorepants]

Your speed constraint is nonlinear in the u's, so this violates the definition of motion constraints we support. u1(t)*u3(t) + u2(t)*u4(t). There is nothing inside KanesMethod that can deal with this equation. KanesMethod assumes the constraints are linear in the u's.

[Peter230655]

Yes, they are.
I thought, this is how I started this 'issue', but now I can't get the threads together. Me and Github!!
But if I give the acceleration constraints this should be o.k.?

Seems to me the documentation does not explicitly say the velocity constraints must be linear - but I might have missed this?

This book Dynamics (updated version of Kane's original book) calls them complex motion constraints as opposed to simple motion constraints, which are linear.

[moorepants]

But if I give the acceleration constraints this should be o.k.?

I don't know. I haven't ever tried nonlinear motion constraints. It is possible it somehow works, but I am skeptical it does. Forming the constrained Kane's equations relies on applying the Jacobian of the constraints to the EoMs as constraint forces. But I don't know if that operation loses information if you are essentially linearizing the nonlinear constraints.

[Peter230655]

Thanks!!

What I have done often, and also in your lecture is to take configuration constraints, and convert them into speed constraints by differentiating them with respect to t.
Would this be linearizing the configuration constraints? (It seems to me that by differentiating one does not 'throw away' information, which one does by linearizing (?) )
2.
Unless I missed something, in the documentation it does not explicitly say that the velocity constraints must be linear.
It does say, that configuration constraints are ignored except for linearization, maybe something similar about velocity constraints?
3.
What could be the meaning of the keyword (in me.KanesMethod) of acceleration_constraints? I did not find it in the documentation.
It seems to have some meaning because:
If I do not put speed_constr.diff(t) there it does not work.
If I do put it there, it gives 'some' results, and the speed constraints are fulfilled within numerical accuracy.
4.
Below I added my program. It is the same as the earlier one, just cleaned up a bit.

Thanks again!

'''
This program tries to get EOMs for the following problem:
two points, P1, P2 move in the X/Y plane (gravity points in the -Y direction) such that
P1.vel(N).dot(P2.vel(N)) - f(t) = 0. In the book "Dynamics. Theory and Application of Kane's Method" this is called a nonlinear
motion constraint. 
the example is straight from this book
'''
import sympy as sm
import sympy.physics.mechanics as me
import numpy as np

import time
from scipy.integrate import solve_ivp
from scipy.optimize import fsolve

import matplotlib.pyplot as plt


N = me.ReferenceFrame('N')
O, P1, P2 = sm.symbols('O, P1, P2', cls=me.Point)

O.set_vel(N, 0)
t = me.dynamicsymbols._t
#t = sm.symbols('t')

q1, q2, q3, q4 = me.dynamicsymbols('q1, q2, q3, q4')
u1, u2, u3, u4 = me.dynamicsymbols('u1, u2, u3, u4')

m, g = sm.symbols('m, g')

P1.set_pos(O, q1*N.x + q2*N.y)
P1.set_vel(N, u1*N.x + u2*N.y)

P2.set_pos(O, q3*N.x + q4*N.y)
P2.set_vel(N, u3*N.x + u4*N.y)

# nonlinear motion constraint
speed_constr = P1.vel(N).dot(P2.vel(N)) - sm.sin(t)**2 
speed_constr_dt = [speed_constr.diff(t)]
speed_constr = [speed_constr]
print('nonlinear speed constraint:', speed_constr)

body1 = me.Particle('P1a', P1, m)
body2 = me.Particle('P2a', P2, m )
BODY = [body1, body2]

FL = [(P1, -m*g*N.y), (P2, -m*g*N.y)]

q_ind = [q1, q2, q3, q4]
u_ind = [u1, u2, u3]
u_dep = [u4]

kd = [i - j.diff(t) for i, j in zip(u_ind + u_dep, q_ind)]
KM = me.KanesMethod(N, q_ind=q_ind, 
                    u_ind=u_ind, 
                    u_dependent=u_dep, 
                    kd_eqs=kd, 
                    velocity_constraints=speed_constr, 
                    acceleration_constraints=speed_constr_dt,
                   )
(fr, frstar) = KM.kanes_equations(BODY, FL)

MM = KM.mass_matrix_full
force = KM.forcing_full

qL = q_ind + u_ind + u_dep
pL = [m, g, t]

MM_lam = sm.lambdify(qL + pL, MM, cse= True)
force_lam = sm.lambdify(qL + pL, force, cse=True)
speed_constr_lam = sm.lambdify(qL + pL, speed_constr, cse=True)
speed_constr_lam1 = sm.lambdify([u4] + [q1, q2, q3, q4] + [u1, u2, u3] + [t], speed_constr, cse=True)

#*++++*++++++++++++++++++++++++++++++++++
start2 = time.time()

m1 = 1
g1 = 0

q11 = 0
q21 = 5
q31 = 5
q41 = 0

# note: u1*u3 + u2*u4 = 0!
u11 = 1 
u21 = 1
u31 = 1

#determine u41, so it fulfills the initial nonlinear motion constraint.
def func(x0, args):
    return speed_constr_lam1(*x0, *args)[0]
x0 = 0   # initial guess for u41
args = [q11, q21, q31, q41, u11, u21, u31, 0]
u41 = fsolve(func, x0, args)[0]

intervall = 10
times = np.linspace(0, intervall, int(100*intervall))

pL_vals = [m1, g1]
y0 = [q11, q21, q31, q41, u11, u21, u31, u41]


t_span = (0., intervall)
def gradient(t, y, args):
    sol = np.linalg.solve(MM_lam(*y, *args, t), force_lam(*y, *args, t))
    return np.array(sol).T[0]
        
resultat1 = solve_ivp(gradient, t_span, y0, t_eval = times, args=(pL_vals,), atol=1.e-10, rtol=1.e-10)
resultat = resultat1.y.T
print('resultat shape', resultat.shape, '\n')
event_dict = {-1: 'Integration failed', 0: 'Integration finished successfully', 1: 'some termination event'}
print(event_dict[resultat1.status], ', message is:', resultat1.message, '\n')

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


speed = np.empty(len(times))
for i in range(len(times)):
    speed[i] = speed_constr_lam(*[resultat[i, j]for j in range(8)],  *pL_vals, times[i])[0]
fig, (ax1, ax2, ax3) = plt.subplots(3, 1, figsize=(10, 15))
#speed = np.array(speed_constr_lam(*[resultat[:, j]for j ). reshape(len(times))

ax1.plot(times, speed)
ax1.set_title('errors in speed constraints')

for i in range(4):
    ax2.plot(times, resultat[:, i], label=str(qL[i]))
    ax2.set_title('generalized coordinates')
ax2.legend()
for i in range(4, 8):
    ax3.plot(times, resultat[:, i], label=str(qL[i]))
    ax3.set_title('generalized speeds')
ax3.legend();
print('symbolic mass matrix and symbolic force vector, both full:')
MM, '......................................', force

[moorepants]

Would this be linearizing the configuration constraints? (It seems to me that by differentiating one does not 'throw away' information, which one does by linearizing (?) )

No, linearizing is take the first two terms of the Taylor series expansion.

Unless I missed something, in the documentation it does not explicitly say that the velocity constraints must be linear.

That may be true, but we also never say we support nonlinear motion constraints. I'm not sure, but I think we only present the linear form of these equations when we do show them in the docs.

It does say, that configuration constraints are ignored except for linearization, maybe something similar about velocity constraints?

No that is a separate note. This is just implying that KanesMethod does not output the DAEs, but only the ODEs minus the holonomic constraints. KanesMethod does differentiate the holonomic constraints internally twice and builds those into the equations (I think).

What could be the meaning of the keyword (in me.KanesMethod) of acceleration_constraints? I did not find it in the documentation.

I recommend looking at the source code if the docs are sufficient. See here:

https://github.com/sympy/sympy/blob/master/sympy/physics/mechanics/kane.py#L315

I subs in the qdot equations and then extracts the linear coefficients to the udots. Those coefficients augment the EoMs for the constraints. If you don't supply it, it differentiates the nonholonomic constraints:

https://github.com/sympy/sympy/blob/master/sympy/physics/mechanics/kane.py#L307

The reason it is there is because at one point we didn't automatically form the acceleration constraints in the ideal form. But now it should. But it is also possible you've found a bug. To determine if it is a bug we have to first understand what happens when you give nonlinear motion constraints, as KanesMethod was never designed to deal with them.

[Peter230655]

Thanks!
I will look at the source code when I will be back home, with my large screen (I am in India at the moment). What I could see on my little screen looks challenging for me.
I hope my python (and my knowledge of Kane) are good enough to understand some of it it.
Of course I hope I did not find a bug, but found something which works contrary to expectation.