Created
April 21, 2026 06:04
-
-
Save moorepants/799a06ad087f1fcf33d1a74cc6533bc0 to your computer and use it in GitHub Desktop.
PyDy Simulation of a Rolling Disc
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| from pydy.system import System | |
| from symmeplot.matplotlib import Scene3D | |
| import matplotlib.pyplot as plt | |
| import numpy as np | |
| import sympy as sm | |
| import sympy.physics.mechanics as me | |
| r, g, m = sm.symbols('r, g, m', real=True) | |
| x, y = me.dynamicsymbols('q1, q2', real=True) | |
| yaw, roll, pitch = me.dynamicsymbols('q3, q4, q5', real=True) | |
| u1, u2, u3, u4, u5 = me.dynamicsymbols('u1, u2, u3, u4, u5', real=True) | |
| N, A, B, C = sm.symbols('N, A, B, C', cls=me.ReferenceFrame) | |
| O, P, Q = sm.symbols('O, P, Q', cls=me.Point) | |
| A.orient_axis(N, yaw, N.z) | |
| B.orient_axis(A, roll, A.x) | |
| C.orient_axis(B, pitch, B.y) | |
| N_w_C = C.ang_vel_in(N) | |
| kd_eqs=( | |
| x.diff() - u1, | |
| y.diff() - u2, | |
| N_w_C.dot(C.z) - u3, | |
| N_w_C.dot(C.x) - u4, | |
| N_w_C.dot(C.y) - u5, | |
| ) | |
| A.set_ang_vel(N, u3*N.z) | |
| B.set_ang_vel(A, u4*A.x) | |
| C.set_ang_vel(B, u5*B.y) | |
| P.set_pos(O, x*N.x + y*N.y) | |
| Q.set_pos(P, r*B.z) | |
| O.set_vel(N, 0) | |
| P.set_vel(N, u1*N.x + u2*N.y) | |
| Q.v2pt_theory(P, N, B) | |
| v = Q.vel(N) + C.ang_vel_in(N).cross(-r*B.z) | |
| nonholonomic = (v.dot(A.x), v.dot(A.y)) | |
| inertia = me.Inertia.from_inertia_scalars(Q, C, m*r**2/4, m*r**2/2, m*r**2/4) | |
| disc = me.RigidBody('disc', Q, C, m, inertia) | |
| gravity = me.Force(Q, -m*g*N.z) | |
| kane = me.KanesMethod( | |
| N, | |
| (x, y, yaw, roll, pitch), | |
| (u3, u4, u5), | |
| kd_eqs=kd_eqs, | |
| u_dependent=(u1, u2), | |
| velocity_constraints=nonholonomic, | |
| bodies=(disc,), | |
| forcelist=(gravity,), | |
| kd_eqs_solver='CRAMER', | |
| constraint_solver='CRAMER', | |
| ) | |
| fr, frstar = kane.kanes_equations() | |
| sys = System(kane) | |
| sys.constants = { | |
| r: 0.3, | |
| g: 9.81, | |
| m: 1.25, | |
| } | |
| speed = 5.0 | |
| yaw0 = 0.0 | |
| sys.initial_conditions = { | |
| x: 0.0, | |
| y: 0.0, | |
| yaw: yaw0, | |
| roll: 0.0, | |
| pitch: 0.0, | |
| u1: speed*np.cos(yaw0), | |
| u2: speed*np.sin(yaw0), | |
| u3: 0.0, | |
| u4: np.deg2rad(40.0), | |
| u5: speed/sys.constants[r], | |
| } | |
| fps = 30 # frames per second | |
| duration = 10.0 # seconds | |
| sys.times = np.linspace(0.0, duration, num=int(duration*fps)) | |
| trajectories = sys.integrate() | |
| fig, axes = plt.subplots(len(sys.states), 1, sharex=True, layout='constrained') | |
| for ax, traj, s in zip(axes, trajectories.T, sys.states): | |
| ax.plot(sys.times, traj) | |
| ax.set_ylabel(s) | |
| fig, ax = plt.subplots() | |
| ax.plot(trajectories[:, 0], trajectories[:, 1]) | |
| ax.set_aspect('equal') | |
| fig, ax = plt.subplots(subplot_kw={'projection': '3d'}) | |
| scene = Scene3D(N, O, ax=ax, scale=1.0) | |
| disc_plot = scene.add_body(disc, plot_frame_properties={"scale": 0.3}) | |
| disc_plot.attach_circle(Q, r, C.y, facecolor="C0", alpha=0.4, | |
| edgecolor="black") | |
| con_syms = list(sys.constants_symbols) | |
| scene.lambdify_system(list(sys.states) + con_syms) | |
| con_vals = [sys.constants[s] for s in con_syms] | |
| scene.evaluate_system(*np.hstack((trajectories[0], con_vals))) | |
| scene.plot() | |
| ax.set_xlim((0.0, 30.0)) | |
| ax.set_ylim((0.0, 30.0)) | |
| ax.set_zlim((-15.0, 15.0)) | |
| ani = scene.animate(lambda i: np.hstack((trajectories[i], con_vals)), | |
| frames=len(sys.times), interval=fps*1000) | |
| ani.save("animation.gif", fps=fps) | |
| plt.show() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment