Spaces:
Running
Running
| from sympy import (zeros, Matrix, symbols, lambdify, sqrt, pi, | |
| simplify) | |
| from sympy.physics.mechanics import (dynamicsymbols, cross, inertia, RigidBody, | |
| ReferenceFrame, KanesMethod) | |
| def _create_rolling_disc(): | |
| # Define symbols and coordinates | |
| t = dynamicsymbols._t | |
| q1, q2, q3, q4, q5, u1, u2, u3, u4, u5 = dynamicsymbols('q1:6 u1:6') | |
| g, r, m = symbols('g r m') | |
| # Define bodies and frames | |
| ground = RigidBody('ground') | |
| disc = RigidBody('disk', mass=m) | |
| disc.inertia = (m * r ** 2 / 4 * inertia(disc.frame, 1, 2, 1), | |
| disc.masscenter) | |
| ground.masscenter.set_vel(ground.frame, 0) | |
| disc.masscenter.set_vel(disc.frame, 0) | |
| int_frame = ReferenceFrame('int_frame') | |
| # Orient frames | |
| int_frame.orient_body_fixed(ground.frame, (q1, q2, 0), 'zxy') | |
| disc.frame.orient_axis(int_frame, int_frame.y, q3) | |
| g_w_d = disc.frame.ang_vel_in(ground.frame) | |
| disc.frame.set_ang_vel(ground.frame, | |
| u1 * disc.x + u2 * disc.y + u3 * disc.z) | |
| # Define points | |
| cp = ground.masscenter.locatenew('contact_point', | |
| q4 * ground.x + q5 * ground.y) | |
| cp.set_vel(ground.frame, u4 * ground.x + u5 * ground.y) | |
| disc.masscenter.set_pos(cp, r * int_frame.z) | |
| disc.masscenter.set_vel(ground.frame, cross( | |
| disc.frame.ang_vel_in(ground.frame), disc.masscenter.pos_from(cp))) | |
| # Define kinematic differential equations | |
| kdes = [g_w_d.dot(disc.x) - u1, g_w_d.dot(disc.y) - u2, | |
| g_w_d.dot(disc.z) - u3, q4.diff(t) - u4, q5.diff(t) - u5] | |
| # Define nonholonomic constraints | |
| v0 = cp.vel(ground.frame) + cross( | |
| disc.frame.ang_vel_in(int_frame), cp.pos_from(disc.masscenter)) | |
| fnh = [v0.dot(ground.x), v0.dot(ground.y)] | |
| # Define loads | |
| loads = [(disc.masscenter, -disc.mass * g * ground.z)] | |
| bodies = [disc] | |
| return { | |
| 'frame': ground.frame, | |
| 'q_ind': [q1, q2, q3, q4, q5], | |
| 'u_ind': [u1, u2, u3], | |
| 'u_dep': [u4, u5], | |
| 'kdes': kdes, | |
| 'fnh': fnh, | |
| 'bodies': bodies, | |
| 'loads': loads | |
| } | |
| def _verify_rolling_disc_numerically(kane, all_zero=False): | |
| q, u, p = dynamicsymbols('q1:6'), dynamicsymbols('u1:6'), symbols('g r m') | |
| eval_sys = lambdify((q, u, p), (kane.mass_matrix_full, kane.forcing_full), | |
| cse=True) | |
| solve_sys = lambda q, u, p: Matrix.LUsolve( | |
| *(Matrix(mat) for mat in eval_sys(q, u, p))) | |
| solve_u_dep = lambdify((q, u[:3], p), kane._Ars * Matrix(u[:3]), cse=True) | |
| eps = 1e-10 | |
| p_vals = (9.81, 0.26, 3.43) | |
| # First numeric test | |
| q_vals = (0.3, 0.1, 1.97, -0.35, 2.27) | |
| u_vals = [-0.2, 1.3, 0.15] | |
| u_vals.extend(solve_u_dep(q_vals, u_vals, p_vals)[:2, 0]) | |
| expected = Matrix([ | |
| 0.126603940595934, 0.215942571601660, 1.28736069604936, | |
| 0.319764288376543, 0.0989146857254898, -0.925848952664489, | |
| -0.0181350656532944, 2.91695398184589, -0.00992793421754526, | |
| 0.0412861634829171]) | |
| assert all(abs(x) < eps for x in | |
| (solve_sys(q_vals, u_vals, p_vals) - expected)) | |
| # Second numeric test | |
| q_vals = (3.97, -0.28, 8.2, -0.35, 2.27) | |
| u_vals = [-0.25, -2.2, 0.62] | |
| u_vals.extend(solve_u_dep(q_vals, u_vals, p_vals)[:2, 0]) | |
| expected = Matrix([ | |
| 0.0259159090798597, 0.668041660387416, -2.19283799213811, | |
| 0.385441810852219, 0.420109283790573, 1.45030568179066, | |
| -0.0110924422400793, -8.35617840186040, -0.154098542632173, | |
| -0.146102664410010]) | |
| assert all(abs(x) < eps for x in | |
| (solve_sys(q_vals, u_vals, p_vals) - expected)) | |
| if all_zero: | |
| q_vals = (0, 0, 0, 0, 0) | |
| u_vals = (0, 0, 0, 0, 0) | |
| assert solve_sys(q_vals, u_vals, p_vals) == zeros(10, 1) | |
| def test_kane_rolling_disc_lu(): | |
| props = _create_rolling_disc() | |
| kane = KanesMethod(props['frame'], props['q_ind'], props['u_ind'], | |
| props['kdes'], u_dependent=props['u_dep'], | |
| velocity_constraints=props['fnh'], | |
| bodies=props['bodies'], forcelist=props['loads'], | |
| explicit_kinematics=False, constraint_solver='LU') | |
| kane.kanes_equations() | |
| _verify_rolling_disc_numerically(kane) | |
| def test_kane_rolling_disc_kdes_callable(): | |
| props = _create_rolling_disc() | |
| kane = KanesMethod( | |
| props['frame'], props['q_ind'], props['u_ind'], props['kdes'], | |
| u_dependent=props['u_dep'], velocity_constraints=props['fnh'], | |
| bodies=props['bodies'], forcelist=props['loads'], | |
| explicit_kinematics=False, | |
| kd_eqs_solver=lambda A, b: simplify(A.LUsolve(b))) | |
| q, u, p = dynamicsymbols('q1:6'), dynamicsymbols('u1:6'), symbols('g r m') | |
| qd = dynamicsymbols('q1:6', 1) | |
| eval_kdes = lambdify((q, qd, u, p), tuple(kane.kindiffdict().items())) | |
| eps = 1e-10 | |
| # Test with only zeros. If 'LU' would be used this would result in nan. | |
| p_vals = (9.81, 0.25, 3.5) | |
| zero_vals = (0, 0, 0, 0, 0) | |
| assert all(abs(qdi - fui) < eps for qdi, fui in | |
| eval_kdes(zero_vals, zero_vals, zero_vals, p_vals)) | |
| # Test with some arbitrary values | |
| q_vals = tuple(map(float, (pi / 6, pi / 3, pi / 2, 0.42, 0.62))) | |
| qd_vals = tuple(map(float, (4, 1 / 3, 4 - 2 * sqrt(3), | |
| 0.25 * (2 * sqrt(3) - 3), | |
| 0.25 * (2 - sqrt(3))))) | |
| u_vals = tuple(map(float, (-2, 4, 1 / 3, 0.25 * (-3 + 2 * sqrt(3)), | |
| 0.25 * (-sqrt(3) + 2)))) | |
| assert all(abs(qdi - fui) < eps for qdi, fui in | |
| eval_kdes(q_vals, qd_vals, u_vals, p_vals)) | |