Note
You can download this example as a Python script: :jupyter-download:script:`nonholonomic-eom` or Jupyter Notebook: :jupyter-download📓`nonholonomic-eom`.
.. jupyter-execute:: from IPython.display import HTML from matplotlib.animation import FuncAnimation from scipy.integrate import solve_ivp import matplotlib.pyplot as plt import numpy as np import sympy as sm import sympy.physics.mechanics as me me.init_vprinting(use_latex='mathjax')
In chapters, :ref:`Holonomic Constraints` and :ref:`Nonholonomic Constraints`, I introduced two types of constraints: holonomic (configuration) constraints and nonholonomic (motion) constraints. Holonomic constraints are nonlinear constraints in the coordinates [*]. Nonholonomic constraints are linear in the generalized speeds, by definition. We will address the nonholonomic equations of motion first, as they are slightly easier to deal with.
| [*] | They can be linear in the coordinates, but then there is little reason not to solve for the depedendent coordinates and eliminate them. |
Nonholonomic constraint equations are linear in both the independent and dependent generalized speeds (see Sec. :ref:`Snakeboard`). We have shown that you can explicitly solve for the dependent generalized speeds \bar{u}_r as a function of the independent generalized speeds \bar{u}_s. This means that number of dynamical differential equations can be reduced to p from n with m nonholonomic constraints. Recall that the nonholonomic constraints take this form:
\bar{f}_n(\bar{u}_s, \bar{u}_r, \bar{q}, t) = \mathbf{M}_n\bar{u}_r + \bar{g}_n = 0 \in \mathbb{R}^m
and u_r can be solved for as so:
\bar{u}_r = -\mathbf{M}_n(\bar{q}, t)^{-1}\bar{g}_n(\bar{u}_s, \bar{q}, t) \\
Using Eq. :math:numref:`eq-dep-speeds-solve` equation we can now write our equations of motion as n kinematical differential equations and p dynamical differential equations.
\bar{f}_k(\bar{u}_s, \dot{\bar{q}}, \bar{q}, t) = \mathbf{M}_k\dot{\bar{q}} + \bar{g}_k = 0 \in \mathbb{R}^n \\
\bar{f}_d(\dot{\bar{u}}_s, \bar{u}_s, \bar{q}, t) = \mathbf{M}_d\dot{\bar{u}}_s + \bar{g}_d = 0 \in \mathbb{R}^p
and these can be written in explicit form:
\dot{\bar{q}} = -\mathbf{M}_k(\bar{q}, t)^{-1} \bar{g}_k(\bar{u}_s, \bar{q}, t) \\
\dot{\bar{u}}_s = -\mathbf{M}_d(\bar{q}, t)^{-1} \bar{g}_d(\bar{u}_s, \bar{q}, t) \\
Let's revisit the snakeboard example (see Sec. :ref:`Snakeboard`) and develop the equations of motion for that nonholonomic system. This system only has nonholonomic constraints and we selected u_1 and u_2 as the dependent speeds. For simplicity, we will assume that the mass and moments of inertia of the three bodies are the same.
Configuration diagram of a planar Snakeboard model.
First introduce the necessary variables; adding I for the central moment of inertia of each body and m as the mass of each body. Then create column matrices for the various sets of variables.
.. jupyter-execute::
q1, q2, q3, q4, q5 = me.dynamicsymbols('q1, q2, q3, q4, q5')
u1, u2, u3, u4, u5 = me.dynamicsymbols('u1, u2, u3, u4, u5')
l, I, m = sm.symbols('l, I, m')
t = me.dynamicsymbols._t
p = sm.Matrix([l, I, m])
q = sm.Matrix([q1, q2, q3, q4, q5])
us = sm.Matrix([u3, u4, u5])
ur = sm.Matrix([u1, u2])
u = ur.col_join(us)
q, ur, us, u, p
We will also need column matrices for the time derivatives of each set of variables and some dictionaries to zero out any of these variables in various expressions we create.
.. jupyter-execute:: qd = q.diff() urd = ur.diff(t) usd = us.diff(t) ud = u.diff(t) qd, urd, usd, ud
.. jupyter-execute::
qd_zero = {qdi: 0 for qdi in qd}
ur_zero = {ui: 0 for ui in ur}
us_zero = {ui: 0 for ui in us}
urd_zero = {udi: 0 for udi in urd}
usd_zero = {udi: 0 for udi in usd}
qd_zero, ur_zero, us_zero, urd_zero, usd_zero
The following code sets up the orientations, positions, and velocities exactly as done in the original example. All of the velocities are in terms of \bar{q} and \dot{\bar{q}}.
.. jupyter-execute::
N = me.ReferenceFrame('N')
A = me.ReferenceFrame('A')
B = me.ReferenceFrame('B')
C = me.ReferenceFrame('C')
A.orient_axis(N, q3, N.z)
B.orient_axis(A, q4, A.z)
C.orient_axis(A, q5, A.z)
A.ang_vel_in(N)
B.ang_vel_in(N)
C.ang_vel_in(N)
O = me.Point('O')
Ao = me.Point('A_o')
Bo = me.Point('B_o')
Co = me.Point('C_o')
Ao.set_pos(O, q1*N.x + q2*N.y)
Bo.set_pos(Ao, l/2*A.x)
Co.set_pos(Ao, -l/2*A.x)
O.set_vel(N, 0)
Bo.v2pt_theory(Ao, N, A)
Co.v2pt_theory(Ao, N, A);
Now create the n=5 kinematical differential equations \bar{f}_k:
.. jupyter-execute::
fk = sm.Matrix([
u1 - q1.diff(t),
u2 - q2.diff(t),
u3 - l*q3.diff(t)/2,
u4 - q4.diff(t),
u5 - q5.diff(t),
])
It is a good idea to use :external:py:func:`~sympy.physics.mechanics.find_dynamicsymbols` to check which functions of time are present in the various equations. This function is invaluable when the equations begin to become very large.
.. jupyter-execute:: me.find_dynamicsymbols(fk)
Symbolically solve these equations for \dot{\bar{q}} and setup a dictionary we can use for substitutions:
.. jupyter-execute:: Mk = fk.jacobian(qd) gk = fk.xreplace(qd_zero) qd_sol = -Mk.LUsolve(gk) qd_repl = dict(zip(qd, qd_sol)) qd_repl
Create the m=2 nonholonomic constraints:
.. jupyter-execute:: fn = sm.Matrix([Bo.vel(N).dot(B.y), Co.vel(N).dot(C.y)]) fn
and rewrite them in terms of the generalized speeds:
.. jupyter-execute:: fn = fn.xreplace(qd_repl) fn
.. jupyter-execute:: me.find_dynamicsymbols(fn)
With the nonholonomic constraint equations we choose \bar{u}_r=[u_1 \ u_2]^T and symbolically for these dependent speeds.
.. jupyter-execute:: Mn = fn.jacobian(ur) gn = fn.xreplace(ur_zero) ur_sol = Mn.LUsolve(-gn) ur_repl = dict(zip(ur, ur_sol))
In our case, the dependent generalized speeds are only a function of one independent generalized speed, u_3.
.. jupyter-execute:: me.find_dynamicsymbols(ur_sol)
Exercise
Why does u_1 and u_2 not depend on q_1,q_2,u_4 and u_5?
Our kinematical differential equations can now be rewritten in terms of the independent generalized speeds. We only need to rewrite \bar{g}_k for later use in our numerical functions.
.. jupyter-execute:: gk = gk.xreplace(ur_repl) me.find_dynamicsymbols(gk)
The snakeboard model, as described, has no generalized active forces because there are no contributing external forces acting on the system, so we only need to generate the nonholonomic generalized inertia forces \tilde{F}_r^*. We now then calculate the velocities we will need to form \tilde{F}_r^* and make sure they are written only in terms of the independent generalized speeds.
.. jupyter-execute::
N_w_A = A.ang_vel_in(N).xreplace(qd_repl).xreplace(ur_repl)
N_w_B = B.ang_vel_in(N).xreplace(qd_repl).xreplace(ur_repl)
N_w_C = C.ang_vel_in(N).xreplace(qd_repl).xreplace(ur_repl)
N_v_Ao = Ao.vel(N).xreplace(qd_repl).xreplace(ur_repl)
N_v_Bo = Bo.vel(N).xreplace(qd_repl).xreplace(ur_repl)
N_v_Co = Co.vel(N).xreplace(qd_repl).xreplace(ur_repl)
vels = (N_w_A, N_w_B, N_w_C, N_v_Ao, N_v_Bo, N_v_Co)
for vel in vels:
print(me.find_dynamicsymbols(vel, reference_frame=N))
With the velocities only in terms of the independent generalized speeds, we can calculate the p nonholonomic partial velocities:
.. jupyter-execute:: w_A, w_B, w_C, v_Ao, v_Bo, v_Co = me.partial_velocity(vels, us, N)
We can also write the accelerations in terms of only the independent generalized speeds, their time derivatives, and the generalized coordinates. To do so, we need to differentiate the nonholonomic constraints so that we can eliminate the dependent generalized accelerations, \dot{\bar{u}}_r. Differentiating the constraints with respect to time and then substituting for the dependent generalized speeds gives us equations for the dependent generalized accelerations.
\dot{\bar{f}}_n(\dot{\bar{u}}_r, \dot{\bar{u}}_s, \bar{u}_s, \bar{u}_r, \bar{q}, t) =
\mathbf{M}_{nd}\dot{\bar{u}}_r + \bar{g}_{nd}= 0 \in \mathbb{R}^m\\
\dot{\bar{u}}_r = -\mathbf{M}_{nd}(\bar{q}, t)^{-1}
\bar{g}_{nd}(\dot{\bar{u}}_s, \bar{u}_s, \bar{q}, t)
First, time differentiate the nonholonomic constraints and eliminate the time derivatives of the generalized coordinates.
.. jupyter-execute:: fnd = fn.diff(t).xreplace(qd_repl) me.find_dynamicsymbols(fnd)
Now solve for the dependent generalized accelerations. Note that I replace the
dependent generalized speeds in \bar{g}_{nd} instead of
\dot{\bar{f}}_n earlier. This is to avoid replacing the u_1 and
u_2 terms in the Derivative(u1, t) and Derivative(u2, t) terms.
.. jupyter-execute:: Mnd = fnd.jacobian(urd) gnd = fnd.xreplace(urd_zero).xreplace(ur_repl) urd_sol = Mnd.LUsolve(-gnd) urd_repl = dict(zip(urd, urd_sol)) me.find_dynamicsymbols(urd_sol)
Now we can form the inertia forces and inertia torques. First check what derivatives appear in the accelerations.
.. jupyter-execute::
Rs_Ao = -m*Ao.acc(N)
Rs_Bo = -m*Bo.acc(N)
Rs_Co = -m*Co.acc(N)
(me.find_dynamicsymbols(Rs_Ao, reference_frame=N) |
me.find_dynamicsymbols(Rs_Bo, reference_frame=N) |
me.find_dynamicsymbols(Rs_Co, reference_frame=N))
.. todo:: Open and issue about find_dynamicsymbols not supporting an iterable of inputs.
We'll need to replace the \ddot{\bar{q}} first and then the \dot{\bar{q}}. Create the first replacement by differentiating the expressions for \dot{\bar{q}}.
Warning
If you use chained replacements, e.g. .xreplace().xreplace().xreplace()
you have to be careful about the order of replacements so that you don't
substitute symbols inside a derivative, e.g. Derivative(u, t). If you
have expr = Derivative(u, t) + u then you need to replace the entire
derivative first: expr.xreplace({u.diff(): 1}).xreplace({u: 2}).
.. jupyter-execute::
qdd_repl = {k.diff(t): v.diff(t).xreplace(urd_repl) for k, v in qd_repl.items()}
.. jupyter-execute::
Rs_Ao = -m*Ao.acc(N).xreplace(qdd_repl).xreplace(qd_repl)
Rs_Bo = -m*Bo.acc(N).xreplace(qdd_repl).xreplace(qd_repl)
Rs_Co = -m*Co.acc(N).xreplace(qdd_repl).xreplace(qd_repl)
(me.find_dynamicsymbols(Rs_Ao, reference_frame=N) |
me.find_dynamicsymbols(Rs_Bo, reference_frame=N) |
me.find_dynamicsymbols(Rs_Co, reference_frame=N))
The motion is planar so the generalized inertia torques are simply angular accelerations dotted with the central inertia dyadics.
.. jupyter-execute:: I_A_Ao = I*me.outer(A.z, A.z) I_B_Bo = I*me.outer(B.z, B.z) I_C_Co = I*me.outer(C.z, C.z)
Now have a look at which functions are present in the inertia torques:
.. jupyter-execute::
Ts_A = -A.ang_acc_in(N).dot(I_A_Ao)
Ts_B = -B.ang_acc_in(N).dot(I_B_Bo)
Ts_C = -C.ang_acc_in(N).dot(I_C_Co)
(me.find_dynamicsymbols(Ts_A, reference_frame=N) |
me.find_dynamicsymbols(Ts_B, reference_frame=N) |
me.find_dynamicsymbols(Ts_C, reference_frame=N))
and eliminate the dependent generalized accelerations:
.. jupyter-execute::
Ts_A = -A.ang_acc_in(N).dot(I_A_Ao).xreplace(qdd_repl)
Ts_B = -B.ang_acc_in(N).dot(I_B_Bo).xreplace(qdd_repl)
Ts_C = -C.ang_acc_in(N).dot(I_C_Co).xreplace(qdd_repl)
(me.find_dynamicsymbols(Ts_A, reference_frame=N) |
me.find_dynamicsymbols(Ts_B, reference_frame=N) |
me.find_dynamicsymbols(Ts_C, reference_frame=N))
All of the components are present to formulate the nonholonomic generalized inertia forces. After we form them, make sure they are only a function of the independent generalized speeds, their time derivatives, and the generalized coordinates.
.. jupyter-execute::
Frs = []
for i in range(len(us)):
Frs.append(v_Ao[i].dot(Rs_Ao) + v_Bo[i].dot(Rs_Bo) + v_Co[i].dot(Rs_Co) +
w_A[i].dot(Ts_A) + w_B[i].dot(Ts_B) + w_C[i].dot(Ts_C))
Frs = sm.Matrix(Frs)
me.find_dynamicsymbols(Frs)
At this point you may have noticed that q_1 and q_2 have not appeared in any equations. This means that the dynamics do not depend on the planar location of the snakeboard. q_1 and q_2 are called ignorable coordinates if they do not appear in the equations of motion. It is only coincidence that the time derivatives of these ignorable coordinates are equal to the to dependent generalized speeds.
Lastly, extract the linear coefficients and the remainder for the dynamical differential equations.
.. jupyter-execute:: Md = Frs.jacobian(usd) gd = Frs.xreplace(usd_zero)
And one last time, check that \mathbf{M}_d and \mathbf{g}_d are only functions of the independent generalized speeds and the generalized coordinates.
.. jupyter-execute:: me.find_dynamicsymbols(Md)
.. jupyter-execute:: me.find_dynamicsymbols(gd)
We now have \mathbf{M}_k, \bar{g}_k, \mathbf{M}_d and \bar{g}_d and can proceed to numerical evaluation.
We now move to numerical evaluation for the simulation. First, create a function that evaluates the matrices of the equations of motion.
.. todo:: lambdify(cse=True) fail for this. Open an issue on SymPy.
.. todo:: sm.Matrix.count_ops() doesn't seem like it exists. Open an issue.
.. jupyter-execute:: eval_kd = sm.lambdify((q, us, p), (Mk, gk, Md, gd))
Now create a function that evaluates the right hand side of the explicit
ordinary differential equations for use with solve_ivp().
.. jupyter-execute::
def eval_rhs(t, x, p):
"""Returns the time derivative of the states.
Parameters
==========
t : float
x : array_like, shape(8,)
x = [q1, q2, q3, q4, q5, u3, u4, u5]
p : array_like, shape(3,)
p = [l, I, m]
Returns
=======
xd : ndarray, shape(8,)
xd = [q1d, q2d, q3d, q4d, q5d, u3d, u4d, u5d]
"""
q, us = x[:5], x[5:]
Mk, gk, Md, gd = eval_kd(q, us, p)
qd = -np.linalg.solve(Mk, gk.squeeze())
usd = -np.linalg.solve(Md, gd.squeeze())
return np.hstack((qd, usd))
Now introduce some numeric values for the constant parameters and the initial condition of the state. I've selected some values here that will put the snakeboard in an initial state of motion.
.. jupyter-execute::
p_vals = np.array([
0.7, # l [m]
0.1, # I [kg*m^2]
1.0, # m [kg]
])
q0 = np.array([
0.0, # q1 [m]
0.0, # q2 [m]
0.0, # q3 [rad]
np.deg2rad(5.0), # q4 [rad]
-np.deg2rad(5.0), # q5 [rad]
])
us0 = np.array([
0.1, # u3 [m/s]
0.01, # u4 [rad/s]
-0.01, # u5 [rad/s]
])
x0 = np.hstack((q0, us0))
p_vals, x0
Check whether eval_rhs() works with these arrays:
.. jupyter-execute:: eval_rhs(1.0, x0, p_vals)
We can now integrate the equations of motion to find the state trajectories. I setup the time array for the solution to correspond to 30 frames per second for later use in the animation of the motion.
.. jupyter-execute:: t0, tf = 0.0, 10.0 fps = 30 ts = np.linspace(t0, tf, num=int(fps*(tf - t0))) sol = solve_ivp(eval_rhs, (t0, tf), x0, args=(p_vals,), t_eval=ts) xs = np.transpose(sol.y)
Now we can plot the state trajectories to see if there is realistic motion.
.. jupyter-execute::
fig, axes = plt.subplots(2, 1, sharex=True)
fig.set_figwidth(10.0)
axes[0].plot(ts, xs[:, :2])
axes[0].legend(('$q_1$', '$q_2$'))
axes[0].set_ylabel('Distance [m]')
axes[1].plot(ts, np.rad2deg(xs[:, 2:5]))
axes[1].legend(('$q_3$', '$q_4$', '$q_5$'))
axes[1].set_ylabel('Angle [deg]')
axes[1].set_xlabel('Time [s]');
We see that the x and y positions vary over several meters and that there is a sharp transition around about 7 seconds. q_3(t) shows that the primary angle of the snakeboard grows with time and does almost a full rotation. Plotting the path on the ground plane of A_o gives a bit more insight to the motion.
.. jupyter-execute::
fig, ax = plt.subplots()
fig.set_figwidth(10.0)
ax.plot(xs[:, 0], xs[:, 1])
ax.set_aspect('equal')
ax.set_xlabel('$q_1$ [m]')
ax.set_ylabel('$q_2$ [m]');
We see that the snakeboard curves to the left but eventually makes a very sharp trajectory change. An animation will provide an even more clear idea of the motion of this nonholonomic system.
We will animate the snakeboard as a collection of lines and points and animate the 2D motion with matplotlib. First, create some new points that represent the location of the left and right wheels on bodies B and C.
.. jupyter-execute::
Bl = me.Point('B_l')
Br = me.Point('B_r')
Cr = me.Point('C_r')
Cl = me.Point('C_l')
Bl.set_pos(Bo, -l/4*B.y)
Br.set_pos(Bo, l/4*B.y)
Cl.set_pos(Co, -l/4*C.y)
Cr.set_pos(Co, l/4*C.y)
Create a function that numerically evaluates the Cartesian coordinates of all the points we want to plot given the generalized coordinates.
.. jupyter-execute::
coordinates = Cl.pos_from(O).to_matrix(N)
for point in [Co, Cr, Co, Ao, Bo, Bl, Br]:
coordinates = coordinates.row_join(point.pos_from(O).to_matrix(N))
eval_point_coords = sm.lambdify((q, p), coordinates)
eval_point_coords(q0, p_vals)
Now create a plot of the initial configuration:
.. jupyter-execute::
x, y, z = eval_point_coords(q0, p_vals)
fig, ax = plt.subplots()
fig.set_size_inches((10.0, 10.0))
ax.set_aspect('equal')
lines, = ax.plot(x, y, color='black',
marker='o', markerfacecolor='blue', markersize=10)
# some empty lines to use for the wheel paths
bl_path, = ax.plot([], [])
br_path, = ax.plot([], [])
cl_path, = ax.plot([], [])
cr_path, = ax.plot([], [])
title_template = 'Time = {:1.2f} s'
title_text = ax.set_title(title_template.format(t0))
ax.set_xlim((np.min(xs[:, 0]) - 0.5, np.max(xs[:, 0]) + 0.5))
ax.set_ylim((np.min(xs[:, 1]) - 0.5, np.max(xs[:, 1]) + 0.5))
ax.set_xlabel('$x$ [m]')
ax.set_ylabel('$y$ [m]');
And, finally, animate the motion:
.. jupyter-execute::
coords = []
for xi in xs:
coords.append(eval_point_coords(xi[:5], p_vals))
coords = np.array(coords) # shape(600, 3, 8)
def animate(i):
title_text.set_text(title_template.format(sol.t[i]))
lines.set_data(coords[i, 0, :], coords[i, 1, :])
cl_path.set_data(coords[:i, 0, 0], coords[:i, 1, 0])
cr_path.set_data(coords[:i, 0, 2], coords[:i, 1, 2])
bl_path.set_data(coords[:i, 0, 6], coords[:i, 1, 6])
br_path.set_data(coords[:i, 0, 7], coords[:i, 1, 7])
ani = FuncAnimation(fig, animate, len(sol.t))
HTML(ani.to_jshtml(fps=fps))
Since we have eliminated the dependent generalized speeds (u_1 and
u_2) from the equations of motion, these are not computed from
solve_ivp(). If these are needed, it is possible to calculate them using
the constraint equations. Here I loop through time to calculate
\bar{u}_r at each time step and then plot the results.
.. jupyter-execute::
x = sm.Matrix([q1, q2, q3, q4, q5, u3, u4, u5])
eval_ur = sm.lambdify((x, p), ur_sol)
ur_vals = []
for xi in xs:
ur_vals.append(eval_ur(xi, p_vals))
ur_vals = np.array(ur_vals).squeeze()
fig, ax = plt.subplots()
fig.set_figwidth(10.0)
ax.plot(ts, ur_vals)
ax.set_ylabel('Speed [m/s]')
ax.set_xlabel('Time [s]')
ax.legend(['$u_1$', '$u_2$']);