def test_two_dof(): # This is for a 2 d.o.f., 2 particle spring-mass-damper. # The first coordinate is the displacement of the first particle, and the # second is the relative displacement between the first and second # particles. Speeds are defined as the time derivatives of the particles. q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2') q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1) m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2') N = ReferenceFrame('N') P1 = Point('P1') P2 = Point('P2') P1.set_vel(N, u1 * N.x) P2.set_vel(N, (u1 + u2) * N.x) kd = [q1d - u1, q2d - u2] # Now we create the list of forces, then assign properties to each # particle, then create a list of all particles. FL = [(P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 * q2 - c2 * u2) * N.x)] pa1 = Particle('pa1', P1, m) pa2 = Particle('pa2', P2, m) BL = [pa1, pa2] # Finally we create the KanesMethod object, specify the inertial frame, # pass relevant information, and form Fr & Fr*. Then we calculate the mass # matrix and forcing terms, and finally solve for the udots. KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd) KM.kanes_equations(FL, BL) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing assert expand(rhs[0]) == expand((-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2)/m) assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 * c2 * u2) / m)
def test_one_dof(): # This is for a 1 dof spring-mass-damper case. # It is described in more detail in the KanesMethod docstring. q, u = dynamicsymbols('q u') qd, ud = dynamicsymbols('q u', 1) m, c, k = symbols('m c k') N = ReferenceFrame('N') P = Point('P') P.set_vel(N, u * N.x) kd = [qd - u] FL = [(P, (-k * q - c * u) * N.x)] pa = Particle('pa', P, m) BL = [pa] KM = KanesMethod(N, [q], [u], kd) # The old input format raises a deprecation warning, so catch it here so # it doesn't cause py.test to fail. with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) KM.kanes_equations(FL, BL) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing assert expand(rhs[0]) == expand(-(q * k + u * c) / m) assert simplify(KM.rhs() - KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(2, 1) assert (KM.linearize(A_and_B=True, )[0] == Matrix([[0, 1], [-k/m, -c/m]]))
def test_pend(): q, u = dynamicsymbols('q u') qd, ud = dynamicsymbols('q u', 1) m, l, g = symbols('m l g') N = ReferenceFrame('N') P = Point('P') P.set_vel(N, -l * u * sin(q) * N.x + l * u * cos(q) * N.y) kd = [qd - u] FL = [(P, m * g * N.x)] pa = Particle('pa', P, m) BL = [pa] KM = KanesMethod(N, [q], [u], kd) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) KM.kanes_equations(FL, BL) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing rhs.simplify() assert expand(rhs[0]) == expand(-g / l * sin(q)) assert simplify(KM.rhs() - KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros( 2, 1)
def test_two_dof(): # This is for a 2 d.o.f., 2 particle spring-mass-damper. # The first coordinate is the displacement of the first particle, and the # second is the relative displacement between the first and second # particles. Speeds are defined as the time derivatives of the particles. q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2') q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1) m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2') N = ReferenceFrame('N') P1 = Point('P1') P2 = Point('P2') P1.set_vel(N, u1 * N.x) P2.set_vel(N, (u1 + u2) * N.x) kd = [q1d - u1, q2d - u2] # Now we create the list of forces, then assign properties to each # particle, then create a list of all particles. FL = [(P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 * q2 - c2 * u2) * N.x)] pa1 = Particle('pa1', P1, m) pa2 = Particle('pa2', P2, m) BL = [pa1, pa2] # Finally we create the KanesMethod object, specify the inertial frame, # pass relevant information, and form Fr & Fr*. Then we calculate the mass # matrix and forcing terms, and finally solve for the udots. KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd) KM.kanes_equations(FL, BL) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing assert expand(rhs[0]) == expand( (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) / m) assert expand(rhs[1]) == expand( (k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 * c2 * u2) / m)
def test_parallel_axis(): # This is for a 2 dof inverted pendulum on a cart. # This tests the parallel axis code in KanesMethod. The inertia of the # pendulum is defined about the hinge, not about the center of mass. # Defining the constants and knowns of the system gravity = symbols("g") k, ls = symbols("k ls") a, mA, mC = symbols("a mA mC") F = dynamicsymbols("F") Ix, Iy, Iz = symbols("Ix Iy Iz") # Declaring the Generalized coordinates and speeds q1, q2 = dynamicsymbols("q1 q2") q1d, q2d = dynamicsymbols("q1 q2", 1) u1, u2 = dynamicsymbols("u1 u2") u1d, u2d = dynamicsymbols("u1 u2", 1) # Creating reference frames N = ReferenceFrame("N") A = ReferenceFrame("A") A.orient(N, "Axis", [-q2, N.z]) A.set_ang_vel(N, -u2 * N.z) # Origin of Newtonian reference frame O = Point("O") # Creating and Locating the positions of the cart, C, and the # center of mass of the pendulum, A C = O.locatenew("C", q1 * N.x) Ao = C.locatenew("Ao", a * A.y) # Defining velocities of the points O.set_vel(N, 0) C.set_vel(N, u1 * N.x) Ao.v2pt_theory(C, N, A) Cart = Particle("Cart", C, mC) Pendulum = RigidBody("Pendulum", Ao, A, mA, (inertia(A, Ix, Iy, Iz), C)) # kinematical differential equations kindiffs = [q1d - u1, q2d - u2] bodyList = [Cart, Pendulum] forceList = [ (Ao, -N.y * gravity * mA), (C, -N.y * gravity * mC), (C, -N.x * k * (q1 - ls)), (C, N.x * F), ] km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs) with warns_deprecated_sympy(): (fr, frstar) = km.kanes_equations(forceList, bodyList) mm = km.mass_matrix_full assert mm[3, 3] == Iz
def test_parallel_axis(): # This is for a 2 dof inverted pendulum on a cart. # This tests the parallel axis code in KanesMethod. The inertia of the # pendulum is defined about the hinge, not about the center of mass. # Defining the constants and knowns of the system gravity = symbols('g') k, ls = symbols('k ls') a, mA, mC = symbols('a mA mC') F = dynamicsymbols('F') Ix, Iy, Iz = symbols('Ix Iy Iz') # Declaring the Generalized coordinates and speeds q1, q2 = dynamicsymbols('q1 q2') q1d, q2d = dynamicsymbols('q1 q2', 1) u1, u2 = dynamicsymbols('u1 u2') u1d, u2d = dynamicsymbols('u1 u2', 1) # Creating reference frames N = ReferenceFrame('N') A = ReferenceFrame('A') A.orient(N, 'Axis', [-q2, N.z]) A.set_ang_vel(N, -u2 * N.z) # Origin of Newtonian reference frame O = Point('O') # Creating and Locating the positions of the cart, C, and the # center of mass of the pendulum, A C = O.locatenew('C', q1 * N.x) Ao = C.locatenew('Ao', a * A.y) # Defining velocities of the points O.set_vel(N, 0) C.set_vel(N, u1 * N.x) Ao.v2pt_theory(C, N, A) Cart = Particle('Cart', C, mC) Pendulum = RigidBody('Pendulum', Ao, A, mA, (inertia(A, Ix, Iy, Iz), C)) # kinematical differential equations kindiffs = [q1d - u1, q2d - u2] bodyList = [Cart, Pendulum] forceList = [(Ao, -N.y * gravity * mA), (C, -N.y * gravity * mC), (C, -N.x * k * (q1 - ls)), (C, N.x * F)] km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) (fr, frstar) = km.kanes_equations(forceList, bodyList) mm = km.mass_matrix_full assert mm[3, 3] == Iz
def test_aux(): # Same as above, except we have 2 auxiliary speeds for the ground contact # point, which is known to be zero. In one case, we go through then # substitute the aux. speeds in at the end (they are zero, as well as their # derivative), in the other case, we use the built-in auxiliary speed part # of KanesMethod. The equations from each should be the same. q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3') q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1) u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2') u4d, u5d = dynamicsymbols('u4, u5', 1) r, m, g = symbols('r m g') N = ReferenceFrame('N') Y = N.orientnew('Y', 'Axis', [q1, N.z]) L = Y.orientnew('L', 'Axis', [q2, Y.x]) R = L.orientnew('R', 'Axis', [q3, L.y]) w_R_N_qd = R.ang_vel_in(N) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) C = Point('C') C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x)) Dmc = C.locatenew('Dmc', r * L.z) Dmc.v2pt_theory(C, N, R) Dmc.a2pt_theory(C, N, R) I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2) kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L] ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))] BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc)) BodyList = [BodyD] KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3, u4, u5], kd_eqs=kd) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) (fr, frstar) = KM.kanes_equations(ForceList, BodyList) fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0}) frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0}) KM2 = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd, u_auxiliary=[u4, u5]) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) (fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList) fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0}) frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0}) frstar.simplify() frstar2.simplify() assert (fr - fr2).expand() == Matrix([0, 0, 0, 0, 0]) assert (frstar - frstar2).expand() == Matrix([0, 0, 0, 0, 0])
def mass_matrix(self): #if not (self._kanes_method and self.up_to_date): # self._init_kanes_method() # TODO move the creation of Kane's Method somewhere else. self._kanes_method = KanesMethod( self.root.frame, q_ind=self.independent_coordinates(), u_ind=self.independent_speeds(), kd_eqs=self.kinematic_differential_equations()) # TODO must make this call to get the mass matrix, etc.? self._kanes_method.kanes_equations(self.force_list(), self.body_list()) return self._kanes_method.mass_matrix
def get_equations(m_val, g_val, l_val): # This function body is copyied from: # http://www.pydy.org/examples/double_pendulum.html # Retrieved 2015-09-29 from sympy import symbols from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame, Point, Particle, KanesMethod) q1, q2 = dynamicsymbols('q1 q2') q1d, q2d = dynamicsymbols('q1 q2', 1) u1, u2 = dynamicsymbols('u1 u2') u1d, u2d = dynamicsymbols('u1 u2', 1) l, m, g = symbols('l m g') N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q1, N.z]) B = N.orientnew('B', 'Axis', [q2, N.z]) A.set_ang_vel(N, u1 * N.z) B.set_ang_vel(N, u2 * N.z) O = Point('O') P = O.locatenew('P', l * A.x) R = P.locatenew('R', l * B.x) O.set_vel(N, 0) P.v2pt_theory(O, N, A) R.v2pt_theory(P, N, B) ParP = Particle('ParP', P, m) ParR = Particle('ParR', R, m) kd = [q1d - u1, q2d - u2] FL = [(P, m * g * N.x), (R, m * g * N.x)] BL = [ParP, ParR] KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd) try: (fr, frstar) = KM.kanes_equations(bodies=BL, loads=FL) except TypeError: (fr, frstar) = KM.kanes_equations(FL, BL) kdd = KM.kindiffdict() mm = KM.mass_matrix_full fo = KM.forcing_full qudots = mm.inv() * fo qudots = qudots.subs(kdd) qudots.simplify() # Edit: depv = [q1, q2, u1, u2] subs = list(zip([m, g, l], [m_val, g_val, l_val])) return zip(depv, [expr.subs(subs) for expr in qudots])
def get_equations(m_val, g_val, l_val): # This function body is copyied from: # http://www.pydy.org/examples/double_pendulum.html # Retrieved 2015-09-29 from sympy import symbols from sympy.physics.mechanics import ( dynamicsymbols, ReferenceFrame, Point, Particle, KanesMethod ) q1, q2 = dynamicsymbols('q1 q2') q1d, q2d = dynamicsymbols('q1 q2', 1) u1, u2 = dynamicsymbols('u1 u2') u1d, u2d = dynamicsymbols('u1 u2', 1) l, m, g = symbols('l m g') N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q1, N.z]) B = N.orientnew('B', 'Axis', [q2, N.z]) A.set_ang_vel(N, u1 * N.z) B.set_ang_vel(N, u2 * N.z) O = Point('O') P = O.locatenew('P', l * A.x) R = P.locatenew('R', l * B.x) O.set_vel(N, 0) P.v2pt_theory(O, N, A) R.v2pt_theory(P, N, B) ParP = Particle('ParP', P, m) ParR = Particle('ParR', R, m) kd = [q1d - u1, q2d - u2] FL = [(P, m * g * N.x), (R, m * g * N.x)] BL = [ParP, ParR] KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd) (fr, frstar) = KM.kanes_equations(FL, BL) kdd = KM.kindiffdict() mm = KM.mass_matrix_full fo = KM.forcing_full qudots = mm.inv() * fo qudots = qudots.subs(kdd) qudots.simplify() # Edit: depv = [q1, q2, u1, u2] subs = list(zip([m, g, l], [m_val, g_val, l_val])) return zip(depv, [expr.subs(subs) for expr in qudots])
def second_order_system(): # from sympy.printing.pycode import NumPyPrinter, pycode coordinates = dynamicsymbols('q:1') # Generalized coordinates speeds = dynamicsymbols('u:1') # Generalized speeds # Force applied to the cart cart_thrust = dynamicsymbols('thrust') m = sp.symbols('m:1') # Mass of each bob g, t = sp.symbols('g t') # Gravity and time ref_frame = ReferenceFrame('I') # Inertial reference frame origin = Point('O') # Origin point origin.set_vel(ref_frame, 0) # Origin's velocity is zero P0 = Point('P0') # Hinge point of top link # Set the position of P0 P0.set_pos(origin, coordinates[0] * ref_frame.x) P0.set_vel(ref_frame, speeds[0] * ref_frame.x) # Set the velocity of P0 Pa0 = Particle('Pa0', P0, m[0]) # Define a particle at P0 # List to hold the n + 1 frames frames = [ref_frame] points = [P0] # List to hold the n + 1 points # List to hold the n + 1 particles particles = [Pa0] # List to hold the n + 1 applied forces, including the input force, f applied_forces = [(P0, cart_thrust * ref_frame.x - m[0] * g * ref_frame.y)] # List to hold kinematic ODE's kindiffs = [coordinates[0].diff(t) - speeds[0]] # Initialize the object kane = KanesMethod(ref_frame, q_ind=coordinates, u_ind=speeds, kd_eqs=kindiffs) # Generate EoM's fr + frstar = 0 fr, frstar = kane.kanes_equations(particles, applied_forces) state = coordinates + speeds gain = [cart_thrust] kindiff_dict = kane.kindiffdict() M = kane.mass_matrix_full.subs(kindiff_dict) F = kane.forcing_full.subs(kindiff_dict) static_parameters = [g, m[0]] transfer = M.inv() * F return DynamicSystem(state, gain, static_parameters, transfer)
def test_input_format(): # 1 dof problem from test_one_dof q, u = dynamicsymbols('q u') qd, ud = dynamicsymbols('q u', 1) m, c, k = symbols('m c k') N = ReferenceFrame('N') P = Point('P') P.set_vel(N, u * N.x) kd = [qd - u] FL = [(P, (-k * q - c * u) * N.x)] pa = Particle('pa', P, m) BL = [pa] KM = KanesMethod(N, [q], [u], kd) # test for input format kane.kanes_equations((body1, body2, particle1)) assert KM.kanes_equations(BL)[0] == Matrix([0]) # test for input format kane.kanes_equations(bodies=(body1, body 2), loads=(load1,load2)) assert KM.kanes_equations(bodies=BL, loads=None)[0] == Matrix([0]) # test for input format kane.kanes_equations(bodies=(body1, body 2), loads=None) assert KM.kanes_equations(BL, loads=None)[0] == Matrix([0]) # test for input format kane.kanes_equations(bodies=(body1, body 2)) assert KM.kanes_equations(BL)[0] == Matrix([0]) # test for error raised when a wrong force list (in this case a string) is provided from sympy.utilities.pytest import raises raises(ValueError, lambda: KM._form_fr('bad input')) # 2 dof problem from test_two_dof q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2') q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1) m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2') N = ReferenceFrame('N') P1 = Point('P1') P2 = Point('P2') P1.set_vel(N, u1 * N.x) P2.set_vel(N, (u1 + u2) * N.x) kd = [q1d - u1, q2d - u2] FL = ((P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 * q2 - c2 * u2) * N.x)) pa1 = Particle('pa1', P1, m) pa2 = Particle('pa2', P2, m) BL = (pa1, pa2) KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd) # test for input format # kane.kanes_equations((body1, body2), (load1, load2)) KM.kanes_equations(BL, FL) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing assert expand(rhs[0]) == expand((-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2)/m) assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 * c2 * u2) / m)
def test_two_dof(): # This is for a 2 d.o.f., 2 particle spring-mass-damper. # The first coordinate is the displacement of the first particle, and the # second is the relative displacement between the first and second # particles. Speeds are defined as the time derivatives of the particles. q1, q2, u1, u2 = dynamicsymbols("q1 q2 u1 u2") q1d, q2d, u1d, u2d = dynamicsymbols("q1 q2 u1 u2", 1) m, c1, c2, k1, k2 = symbols("m c1 c2 k1 k2") N = ReferenceFrame("N") P1 = Point("P1") P2 = Point("P2") P1.set_vel(N, u1 * N.x) P2.set_vel(N, (u1 + u2) * N.x) kd = [q1d - u1, q2d - u2] # Now we create the list of forces, then assign properties to each # particle, then create a list of all particles. FL = [ (P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 * q2 - c2 * u2) * N.x), ] pa1 = Particle("pa1", P1, m) pa2 = Particle("pa2", P2, m) BL = [pa1, pa2] # Finally we create the KanesMethod object, specify the inertial frame, # pass relevant information, and form Fr & Fr*. Then we calculate the mass # matrix and forcing terms, and finally solve for the udots. KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd) # The old input format raises a deprecation warning, so catch it here so # it doesn't cause py.test to fail. with warns_deprecated_sympy(): KM.kanes_equations(FL, BL) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing assert expand(rhs[0]) == expand( (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) / m) assert expand(rhs[1]) == expand( (k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 * c2 * u2) / m) assert simplify(KM.rhs() - KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros( 4, 1)
def test_pend(): q, u = dynamicsymbols('q u') qd, ud = dynamicsymbols('q u', 1) m, l, g = symbols('m l g') N = ReferenceFrame('N') P = Point('P') P.set_vel(N, -l * u * sin(q) * N.x + l * u * cos(q) * N.y) kd = [qd - u] FL = [(P, m * g * N.x)] pa = Particle('pa', P, m) BL = [pa] KM = KanesMethod(N, [q], [u], kd) KM.kanes_equations(FL, BL) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing rhs.simplify() assert expand(rhs[0]) == expand(-g / l * sin(q))
def mass_matrix(self): #if not (self._kanes_method and self.up_to_date): # self._init_kanes_method() # TODO move the creation of Kane's Method somewhere else. self._kanes_method = KanesMethod(self.root.frame, q_ind=self.independent_coordinates(), u_ind=self.independent_speeds(), kd_eqs=self.kinematic_differential_equations() ) # TODO must make this call to get the mass matrix, etc.? self._kanes_method.kanes_equations(self.force_list(), self.body_list()); return self._kanes_method.mass_matrix
def test_one_dof(): # This is for a 1 dof spring-mass-damper case. # It is described in more detail in the KanesMethod docstring. q, u = dynamicsymbols('q u') qd, ud = dynamicsymbols('q u', 1) m, c, k = symbols('m c k') N = ReferenceFrame('N') P = Point('P') P.set_vel(N, u * N.x) kd = [qd - u] FL = [(P, (-k * q - c * u) * N.x)] pa = Particle('pa', P, m) BL = [pa] KM = KanesMethod(N, [q], [u], kd) # The old input format raises a deprecation warning, so catch it here so # it doesn't cause py.test to fail. with warns_deprecated_sympy(): KM.kanes_equations(FL, BL) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing assert expand(rhs[0]) == expand(-(q * k + u * c) / m) assert simplify(KM.rhs() - KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros( 2, 1) assert (KM.linearize(A_and_B=True, )[0] == Matrix([[0, 1], [-k / m, -c / m]]))
def test_one_dof(): # This is for a 1 dof spring-mass-damper case. # It is described in more detail in the KanesMethod docstring. q, u = dynamicsymbols('q u') qd, ud = dynamicsymbols('q u', 1) m, c, k = symbols('m c k') N = ReferenceFrame('N') P = Point('P') P.set_vel(N, u * N.x) kd = [qd - u] FL = [(P, (-k * q - c * u) * N.x)] pa = Particle('pa', P, m) BL = [pa] KM = KanesMethod(N, [q], [u], kd) KM.kanes_equations(BL, FL) assert KM.bodies == BL MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing assert expand(rhs[0]) == expand(-(q * k + u * c) / m) assert simplify(KM.rhs() - KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros( 2, 1) assert (KM.linearize(A_and_B=True, )[0] == Matrix([[0, 1], [-k / m, -c / m]]))
def test_one_dof(): # This is for a 1 dof spring-mass-damper case. # It is described in more detail in the KanesMethod docstring. q, u = dynamicsymbols('q u') qd, ud = dynamicsymbols('q u', 1) m, c, k = symbols('m c k') N = ReferenceFrame('N') P = Point('P') P.set_vel(N, u * N.x) kd = [qd - u] FL = [(P, (-k * q - c * u) * N.x)] pa = Particle('pa', P, m) BL = [pa] KM = KanesMethod(N, [q], [u], kd) KM.kanes_equations(FL, BL) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing assert expand(rhs[0]) == expand(-(q * k + u * c) / m) assert KM.linearize() == (Matrix([[0, 1], [-k, -c]]), Matrix([]), Matrix([]))
def test_aux(): # Same as above, except we have 2 auxiliary speeds for the ground contact # point, which is known to be zero. In one case, we go through then # substitute the aux. speeds in at the end (they are zero, as well as their # derivative), in the other case, we use the built-in auxiliary speed part # of KanesMethod. The equations from each should be the same. q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3') q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1) u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2') u4d, u5d = dynamicsymbols('u4, u5', 1) r, m, g = symbols('r m g') N = ReferenceFrame('N') Y = N.orientnew('Y', 'Axis', [q1, N.z]) L = Y.orientnew('L', 'Axis', [q2, Y.x]) R = L.orientnew('R', 'Axis', [q3, L.y]) w_R_N_qd = R.ang_vel_in(N) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) C = Point('C') C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x)) Dmc = C.locatenew('Dmc', r * L.z) Dmc.v2pt_theory(C, N, R) Dmc.a2pt_theory(C, N, R) I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2) kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L] ForceList = [(Dmc, -m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))] BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc)) BodyList = [BodyD] KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3, u4, u5], kd_eqs=kd) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) (fr, frstar) = KM.kanes_equations(ForceList, BodyList) fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0}) frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0}) KM2 = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd, u_auxiliary=[u4, u5]) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) (fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList) fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0}) frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0}) frstar.simplify() frstar2.simplify() assert (fr - fr2).expand() == Matrix([0, 0, 0, 0, 0]) assert (frstar - frstar2).expand() == Matrix([0, 0, 0, 0, 0])
def test_two_dof(): # This is for a 2 d.o.f., 2 particle spring-mass-damper. # The first coordinate is the displacement of the first particle, and the # second is the relative displacement between the first and second # particles. Speeds are defined as the time derivatives of the particles. q1, q2, u1, u2 = dynamicsymbols("q1 q2 u1 u2") q1d, q2d, u1d, u2d = dynamicsymbols("q1 q2 u1 u2", 1) m, c1, c2, k1, k2 = symbols("m c1 c2 k1 k2") N = ReferenceFrame("N") P1 = Point("P1") P2 = Point("P2") P1.set_vel(N, u1 * N.x) P2.set_vel(N, (u1 + u2) * N.x) kd = [q1d - u1, q2d - u2] # Now we create the list of forces, then assign properties to each # particle, then create a list of all particles. FL = [(P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 * q2 - c2 * u2) * N.x)] pa1 = Particle("pa1", P1, m) pa2 = Particle("pa2", P2, m) BL = [pa1, pa2] # Finally we create the KanesMethod object, specify the inertial frame, # pass relevant information, and form Fr & Fr*. Then we calculate the mass # matrix and forcing terms, and finally solve for the udots. KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd) # The old input format raises a deprecation warning, so catch it here so # it doesn't cause py.test to fail. with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) KM.kanes_equations(FL, BL) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing assert expand(rhs[0]) == expand((-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) / m) assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 * c2 * u2) / m) assert simplify(KM.rhs() - KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(4, 1)
def test_linearize_pendulum_kane_minimal(): q1 = dynamicsymbols('q1') # angle of pendulum u1 = dynamicsymbols('u1') # Angular velocity q1d = dynamicsymbols('q1', 1) # Angular velocity L, m, t = symbols('L, m, t') g = 9.8 # Compose world frame N = ReferenceFrame('N') pN = Point('N*') pN.set_vel(N, 0) # A.x is along the pendulum A = N.orientnew('A', 'axis', [q1, N.z]) A.set_ang_vel(N, u1 * N.z) # Locate point P relative to the origin N* P = pN.locatenew('P', L * A.x) P.v2pt_theory(pN, N, A) pP = Particle('pP', P, m) # Create Kinematic Differential Equations kde = Matrix([q1d - u1]) # Input the force resultant at P R = m * g * N.x # Solve for eom with kanes method KM = KanesMethod(N, q_ind=[q1], u_ind=[u1], kd_eqs=kde) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) (fr, frstar) = KM.kanes_equations([(P, R)], [pP]) # Linearize A, B, inp_vec = KM.linearize(A_and_B=True, new_method=True, simplify=True) assert A == Matrix([[0, 1], [-9.8 * cos(q1) / L, 0]]) assert B == Matrix([])
def test_linearize_pendulum_kane_minimal(): q1 = dynamicsymbols('q1') # angle of pendulum u1 = dynamicsymbols('u1') # Angular velocity q1d = dynamicsymbols('q1', 1) # Angular velocity L, m, t = symbols('L, m, t') g = 9.8 # Compose world frame N = ReferenceFrame('N') pN = Point('N*') pN.set_vel(N, 0) # A.x is along the pendulum A = N.orientnew('A', 'axis', [q1, N.z]) A.set_ang_vel(N, u1*N.z) # Locate point P relative to the origin N* P = pN.locatenew('P', L*A.x) P.v2pt_theory(pN, N, A) pP = Particle('pP', P, m) # Create Kinematic Differential Equations kde = Matrix([q1d - u1]) # Input the force resultant at P R = m*g*N.x # Solve for eom with kanes method KM = KanesMethod(N, q_ind=[q1], u_ind=[u1], kd_eqs=kde) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) (fr, frstar) = KM.kanes_equations([(P, R)], [pP]) # Linearize A, B, inp_vec = KM.linearize(A_and_B=True, new_method=True, simplify=True) assert A == Matrix([[0, 1], [-9.8*cos(q1)/L, 0]]) assert B == Matrix([])
def test_pend(): q, u = dynamicsymbols("q u") qd, ud = dynamicsymbols("q u", 1) m, l, g = symbols("m l g") N = ReferenceFrame("N") P = Point("P") P.set_vel(N, -l * u * sin(q) * N.x + l * u * cos(q) * N.y) kd = [qd - u] FL = [(P, m * g * N.x)] pa = Particle("pa", P, m) BL = [pa] KM = KanesMethod(N, [q], [u], kd) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) KM.kanes_equations(FL, BL) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing rhs.simplify() assert expand(rhs[0]) == expand(-g / l * sin(q)) assert simplify(KM.rhs() - KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(2, 1)
def test_linearize_pendulum_kane_minimal(): q1 = dynamicsymbols("q1") # angle of pendulum u1 = dynamicsymbols("u1") # Angular velocity q1d = dynamicsymbols("q1", 1) # Angular velocity L, m, t = symbols("L, m, t") g = 9.8 # Compose world frame N = ReferenceFrame("N") pN = Point("N*") pN.set_vel(N, 0) # A.x is along the pendulum A = N.orientnew("A", "axis", [q1, N.z]) A.set_ang_vel(N, u1 * N.z) # Locate point P relative to the origin N* P = pN.locatenew("P", L * A.x) P.v2pt_theory(pN, N, A) pP = Particle("pP", P, m) # Create Kinematic Differential Equations kde = Matrix([q1d - u1]) # Input the force resultant at P R = m * g * N.x # Solve for eom with kanes method KM = KanesMethod(N, q_ind=[q1], u_ind=[u1], kd_eqs=kde) with warns_deprecated_sympy(): (fr, frstar) = KM.kanes_equations([(P, R)], [pP]) # Linearize A, B, inp_vec = KM.linearize(A_and_B=True, simplify=True) assert A == Matrix([[0, 1], [-9.8 * cos(q1) / L, 0]]) assert B == Matrix([])
def test_one_dof(): # This is for a 1 dof spring-mass-damper case. # It is described in more detail in the KanesMethod docstring. q, u = dynamicsymbols('q u') qd, ud = dynamicsymbols('q u', 1) m, c, k = symbols('m c k') N = ReferenceFrame('N') P = Point('P') P.set_vel(N, u * N.x) kd = [qd - u] FL = [(P, (-k * q - c * u) * N.x)] pa = Particle('pa', P, m) BL = [pa] KM = KanesMethod(N, [q], [u], kd) # The old input format raises a deprecation warning, so catch it here so # it doesn't cause py.test to fail. with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) KM.kanes_equations(FL, BL) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing assert expand(rhs[0]) == expand(-(q * k + u * c) / m) assert simplify(KM.rhs() - KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros( 2, 1) assert (KM.linearize(A_and_B=True, new_method=True)[0] == Matrix([[0, 1], [-k / m, -c / m]])) # Ensure that the old linearizer still works and that the new linearizer # gives the same results. The old linearizer is deprecated and should be # removed in >= 1.0. M_old = KM.mass_matrix_full # The old linearizer raises a deprecation warning, so catch it here so # it doesn't cause py.test to fail. with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) F_A_old, F_B_old, r_old = KM.linearize() M_new, F_A_new, F_B_new, r_new = KM.linearize(new_method=True) assert simplify(M_new.inv() * F_A_new - M_old.inv() * F_A_old) == zeros(2)
def test_linearize_pendulum_kane_minimal(): q1 = dynamicsymbols("q1") # angle of pendulum u1 = dynamicsymbols("u1") # Angular velocity q1d = dynamicsymbols("q1", 1) # Angular velocity L, m, t = symbols("L, m, t") g = 9.8 # Compose world frame N = ReferenceFrame("N") pN = Point("N*") pN.set_vel(N, 0) # A.x is along the pendulum A = N.orientnew("A", "axis", [q1, N.z]) A.set_ang_vel(N, u1 * N.z) # Locate point P relative to the origin N* P = pN.locatenew("P", L * A.x) P.v2pt_theory(pN, N, A) pP = Particle("pP", P, m) # Create Kinematic Differential Equations kde = Matrix([q1d - u1]) # Input the force resultant at P R = m * g * N.x # Solve for eom with kanes method KM = KanesMethod(N, q_ind=[q1], u_ind=[u1], kd_eqs=kde) (fr, frstar) = KM.kanes_equations([(P, R)], [pP]) # Linearize A, B, inp_vec = KM.linearize(A_and_B=True, new_method=True, simplify=True) assert A == Matrix([[0, 1], [-9.8 * cos(q1) / L, 0]]) assert B == Matrix([])
def test_pend(): q, u = dynamicsymbols("q u") qd, ud = dynamicsymbols("q u", 1) m, l, g = symbols("m l g") N = ReferenceFrame("N") P = Point("P") P.set_vel(N, -l * u * sin(q) * N.x + l * u * cos(q) * N.y) kd = [qd - u] FL = [(P, m * g * N.x)] pa = Particle("pa", P, m) BL = [pa] KM = KanesMethod(N, [q], [u], kd) with warns_deprecated_sympy(): KM.kanes_equations(FL, BL) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing rhs.simplify() assert expand(rhs[0]) == expand(-g / l * sin(q)) assert simplify(KM.rhs() - KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros( 2, 1)
def test_mat_inv_mul(): # Just a quick test to check that KanesMethod._mat_inv_mul works as # intended. Uses SymPy generated primes as matrix entries, so each entry in # each matrix should be symbolic and unique, allowing proper comparison. # Checks _mat_inv_mul against Matrix.inv / Matrix.__mul__. from sympy import Matrix, prime from sympy.physics.mechanics import ReferenceFrame, KanesMethod # Just need to create an instance of KanesMethod to get to _mat_inv_mul mat_inv_mul = KanesMethod(ReferenceFrame('N'), [1], [1])._mat_inv_mul # going to form 3 matrices # 1 n x n # different n x n # 1 n x 2n n = 3 m1 = Matrix(n, n, lambda i, j: prime(i * n + j + 2)) m2 = Matrix(n, n, lambda i, j: prime(i * n + j + 5)) m3 = Matrix(n, n, lambda i, j: prime(i + j * n + 2)) assert mat_inv_mul(m1, m2) == m1.inv() * m2 assert mat_inv_mul(m1, m3) == m1.inv() * m3
def test_bicycle(): if ON_TRAVIS: skip("Too slow for travis.") # Code to get equations of motion for a bicycle modeled as in: # J.P Meijaard, Jim M Papadopoulos, Andy Ruina and A.L Schwab. Linearized # dynamics equations for the balance and steer of a bicycle: a benchmark # and review. Proceedings of The Royal Society (2007) 463, 1955-1982 # doi: 10.1098/rspa.2007.1857 # Note that this code has been crudely ported from Autolev, which is the # reason for some of the unusual naming conventions. It was purposefully as # similar as possible in order to aide debugging. # Declare Coordinates & Speeds # Simple definitions for qdots - qd = u # Speeds are: yaw frame ang. rate, roll frame ang. rate, rear wheel frame # ang. rate (spinning motion), frame ang. rate (pitching motion), steering # frame ang. rate, and front wheel ang. rate (spinning motion). # Wheel positions are ignorable coordinates, so they are not introduced. q1, q2, q4, q5 = dynamicsymbols('q1 q2 q4 q5') q1d, q2d, q4d, q5d = dynamicsymbols('q1 q2 q4 q5', 1) u1, u2, u3, u4, u5, u6 = dynamicsymbols('u1 u2 u3 u4 u5 u6') u1d, u2d, u3d, u4d, u5d, u6d = dynamicsymbols('u1 u2 u3 u4 u5 u6', 1) # Declare System's Parameters WFrad, WRrad, htangle, forkoffset = symbols( 'WFrad WRrad htangle forkoffset') forklength, framelength, forkcg1 = symbols( 'forklength framelength forkcg1') forkcg3, framecg1, framecg3, Iwr11 = symbols( 'forkcg3 framecg1 framecg3 Iwr11') Iwr22, Iwf11, Iwf22, Iframe11 = symbols('Iwr22 Iwf11 Iwf22 Iframe11') Iframe22, Iframe33, Iframe31, Ifork11 = symbols( 'Iframe22 Iframe33 Iframe31 Ifork11') Ifork22, Ifork33, Ifork31, g = symbols('Ifork22 Ifork33 Ifork31 g') mframe, mfork, mwf, mwr = symbols('mframe mfork mwf mwr') # Set up reference frames for the system # N - inertial # Y - yaw # R - roll # WR - rear wheel, rotation angle is ignorable coordinate so not oriented # Frame - bicycle frame # TempFrame - statically rotated frame for easier reference inertia definition # Fork - bicycle fork # TempFork - statically rotated frame for easier reference inertia definition # WF - front wheel, again posses a ignorable coordinate N = ReferenceFrame('N') Y = N.orientnew('Y', 'Axis', [q1, N.z]) R = Y.orientnew('R', 'Axis', [q2, Y.x]) Frame = R.orientnew('Frame', 'Axis', [q4 + htangle, R.y]) WR = ReferenceFrame('WR') TempFrame = Frame.orientnew('TempFrame', 'Axis', [-htangle, Frame.y]) Fork = Frame.orientnew('Fork', 'Axis', [q5, Frame.x]) TempFork = Fork.orientnew('TempFork', 'Axis', [-htangle, Fork.y]) WF = ReferenceFrame('WF') # Kinematics of the Bicycle First block of code is forming the positions of # the relevant points # rear wheel contact -> rear wheel mass center -> frame mass center + # frame/fork connection -> fork mass center + front wheel mass center -> # front wheel contact point WR_cont = Point('WR_cont') WR_mc = WR_cont.locatenew('WR_mc', WRrad * R.z) Steer = WR_mc.locatenew('Steer', framelength * Frame.z) Frame_mc = WR_mc.locatenew('Frame_mc', -framecg1 * Frame.x + framecg3 * Frame.z) Fork_mc = Steer.locatenew('Fork_mc', -forkcg1 * Fork.x + forkcg3 * Fork.z) WF_mc = Steer.locatenew('WF_mc', forklength * Fork.x + forkoffset * Fork.z) WF_cont = WF_mc.locatenew( 'WF_cont', WFrad * (dot(Fork.y, Y.z) * Fork.y - Y.z).normalize()) # Set the angular velocity of each frame. # Angular accelerations end up being calculated automatically by # differentiating the angular velocities when first needed. # u1 is yaw rate # u2 is roll rate # u3 is rear wheel rate # u4 is frame pitch rate # u5 is fork steer rate # u6 is front wheel rate Y.set_ang_vel(N, u1 * Y.z) R.set_ang_vel(Y, u2 * R.x) WR.set_ang_vel(Frame, u3 * Frame.y) Frame.set_ang_vel(R, u4 * Frame.y) Fork.set_ang_vel(Frame, u5 * Fork.x) WF.set_ang_vel(Fork, u6 * Fork.y) # Form the velocities of the previously defined points, using the 2 - point # theorem (written out by hand here). Accelerations again are calculated # automatically when first needed. WR_cont.set_vel(N, 0) WR_mc.v2pt_theory(WR_cont, N, WR) Steer.v2pt_theory(WR_mc, N, Frame) Frame_mc.v2pt_theory(WR_mc, N, Frame) Fork_mc.v2pt_theory(Steer, N, Fork) WF_mc.v2pt_theory(Steer, N, Fork) WF_cont.v2pt_theory(WF_mc, N, WF) # Sets the inertias of each body. Uses the inertia frame to construct the # inertia dyadics. Wheel inertias are only defined by principle moments of # inertia, and are in fact constant in the frame and fork reference frames; # it is for this reason that the orientations of the wheels does not need # to be defined. The frame and fork inertias are defined in the 'Temp' # frames which are fixed to the appropriate body frames; this is to allow # easier input of the reference values of the benchmark paper. Note that # due to slightly different orientations, the products of inertia need to # have their signs flipped; this is done later when entering the numerical # value. Frame_I = (inertia(TempFrame, Iframe11, Iframe22, Iframe33, 0, 0, Iframe31), Frame_mc) Fork_I = (inertia(TempFork, Ifork11, Ifork22, Ifork33, 0, 0, Ifork31), Fork_mc) WR_I = (inertia(Frame, Iwr11, Iwr22, Iwr11), WR_mc) WF_I = (inertia(Fork, Iwf11, Iwf22, Iwf11), WF_mc) # Declaration of the RigidBody containers. :: BodyFrame = RigidBody('BodyFrame', Frame_mc, Frame, mframe, Frame_I) BodyFork = RigidBody('BodyFork', Fork_mc, Fork, mfork, Fork_I) BodyWR = RigidBody('BodyWR', WR_mc, WR, mwr, WR_I) BodyWF = RigidBody('BodyWF', WF_mc, WF, mwf, WF_I) # The kinematic differential equations; they are defined quite simply. Each # entry in this list is equal to zero. kd = [q1d - u1, q2d - u2, q4d - u4, q5d - u5] # The nonholonomic constraints are the velocity of the front wheel contact # point dotted into the X, Y, and Z directions; the yaw frame is used as it # is "closer" to the front wheel (1 less DCM connecting them). These # constraints force the velocity of the front wheel contact point to be 0 # in the inertial frame; the X and Y direction constraints enforce a # "no-slip" condition, and the Z direction constraint forces the front # wheel contact point to not move away from the ground frame, essentially # replicating the holonomic constraint which does not allow the frame pitch # to change in an invalid fashion. conlist_speed = [ WF_cont.vel(N) & Y.x, WF_cont.vel(N) & Y.y, WF_cont.vel(N) & Y.z ] # The holonomic constraint is that the position from the rear wheel contact # point to the front wheel contact point when dotted into the # normal-to-ground plane direction must be zero; effectively that the front # and rear wheel contact points are always touching the ground plane. This # is actually not part of the dynamic equations, but instead is necessary # for the lineraization process. conlist_coord = [WF_cont.pos_from(WR_cont) & Y.z] # The force list; each body has the appropriate gravitational force applied # at its mass center. FL = [(Frame_mc, -mframe * g * Y.z), (Fork_mc, -mfork * g * Y.z), (WF_mc, -mwf * g * Y.z), (WR_mc, -mwr * g * Y.z)] BL = [BodyFrame, BodyFork, BodyWR, BodyWF] # The N frame is the inertial frame, coordinates are supplied in the order # of independent, dependent coordinates, as are the speeds. The kinematic # differential equation are also entered here. Here the dependent speeds # are specified, in the same order they were provided in earlier, along # with the non-holonomic constraints. The dependent coordinate is also # provided, with the holonomic constraint. Again, this is only provided # for the linearization process. KM = KanesMethod(N, q_ind=[q1, q2, q5], q_dependent=[q4], configuration_constraints=conlist_coord, u_ind=[u2, u3, u5], u_dependent=[u1, u4, u6], velocity_constraints=conlist_speed, kd_eqs=kd) (fr, frstar) = KM.kanes_equations(FL, BL) # This is the start of entering in the numerical values from the benchmark # paper to validate the eigen values of the linearized equations from this # model to the reference eigen values. Look at the aforementioned paper for # more information. Some of these are intermediate values, used to # transform values from the paper into the coordinate systems used in this # model. PaperRadRear = 0.3 PaperRadFront = 0.35 HTA = evalf.N(pi / 2 - pi / 10) TrailPaper = 0.08 rake = evalf.N(-(TrailPaper * sin(HTA) - (PaperRadFront * cos(HTA)))) PaperWb = 1.02 PaperFrameCgX = 0.3 PaperFrameCgZ = 0.9 PaperForkCgX = 0.9 PaperForkCgZ = 0.7 FrameLength = evalf.N(PaperWb * sin(HTA) - (rake - (PaperRadFront - PaperRadRear) * cos(HTA))) FrameCGNorm = evalf.N((PaperFrameCgZ - PaperRadRear - (PaperFrameCgX / sin(HTA)) * cos(HTA)) * sin(HTA)) FrameCGPar = evalf.N( (PaperFrameCgX / sin(HTA) + (PaperFrameCgZ - PaperRadRear - PaperFrameCgX / sin(HTA) * cos(HTA)) * cos(HTA))) tempa = evalf.N((PaperForkCgZ - PaperRadFront)) tempb = evalf.N((PaperWb - PaperForkCgX)) tempc = evalf.N(sqrt(tempa**2 + tempb**2)) PaperForkL = evalf.N( (PaperWb * cos(HTA) - (PaperRadFront - PaperRadRear) * sin(HTA))) ForkCGNorm = evalf.N(rake + (tempc * sin(pi / 2 - HTA - acos(tempa / tempc)))) ForkCGPar = evalf.N(tempc * cos((pi / 2 - HTA) - acos(tempa / tempc)) - PaperForkL) # Here is the final assembly of the numerical values. The symbol 'v' is the # forward speed of the bicycle (a concept which only makes sense in the # upright, static equilibrium case?). These are in a dictionary which will # later be substituted in. Again the sign on the *product* of inertia # values is flipped here, due to different orientations of coordinate # systems. v = symbols('v') val_dict = { WFrad: PaperRadFront, WRrad: PaperRadRear, htangle: HTA, forkoffset: rake, forklength: PaperForkL, framelength: FrameLength, forkcg1: ForkCGPar, forkcg3: ForkCGNorm, framecg1: FrameCGNorm, framecg3: FrameCGPar, Iwr11: 0.0603, Iwr22: 0.12, Iwf11: 0.1405, Iwf22: 0.28, Ifork11: 0.05892, Ifork22: 0.06, Ifork33: 0.00708, Ifork31: 0.00756, Iframe11: 9.2, Iframe22: 11, Iframe33: 2.8, Iframe31: -2.4, mfork: 4, mframe: 85, mwf: 3, mwr: 2, g: 9.81, q1: 0, q2: 0, q4: 0, q5: 0, u1: 0, u2: 0, u3: v / PaperRadRear, u4: 0, u5: 0, u6: v / PaperRadFront } # Linearizes the forcing vector; the equations are set up as MM udot = # forcing, where MM is the mass matrix, udot is the vector representing the # time derivatives of the generalized speeds, and forcing is a vector which # contains both external forcing terms and internal forcing terms, such as # centripital or coriolis forces. This actually returns a matrix with as # many rows as *total* coordinates and speeds, but only as many columns as # independent coordinates and speeds. with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) forcing_lin = KM.linearize()[0] # As mentioned above, the size of the linearized forcing terms is expanded # to include both q's and u's, so the mass matrix must have this done as # well. This will likely be changed to be part of the linearized process, # for future reference. MM_full = KM.mass_matrix_full MM_full_s = MM_full.subs(val_dict) forcing_lin_s = forcing_lin.subs(KM.kindiffdict()).subs(val_dict) MM_full_s = MM_full_s.evalf() forcing_lin_s = forcing_lin_s.evalf() # Finally, we construct an "A" matrix for the form xdot = A x (x being the # state vector, although in this case, the sizes are a little off). The # following line extracts only the minimum entries required for eigenvalue # analysis, which correspond to rows and columns for lean, steer, lean # rate, and steer rate. Amat = MM_full_s.inv() * forcing_lin_s A = Amat.extract([1, 2, 4, 6], [1, 2, 3, 5]) # Precomputed for comparison Res = Matrix([[0, 0, 1.0, 0], [0, 0, 0, 1.0], [ 9.48977444677355, -0.891197738059089 * v**2 - 0.571523173729245, -0.105522449805691 * v, -0.330515398992311 * v ], [ 11.7194768719633, -1.97171508499972 * v**2 + 30.9087533932407, 3.67680523332152 * v, -3.08486552743311 * v ]]) # Actual eigenvalue comparison eps = 1.e-12 for i in xrange(6): error = Res.subs(v, i) - A.subs(v, i) assert all(abs(x) < eps for x in error)
def test_linearize_rolling_disc_kane(): # Symbols for time and constant parameters t, r, m, g, v = symbols('t r m g v') # Configuration variables and their time derivatives q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7') q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q] # Generalized speeds and their time derivatives u = dynamicsymbols('u:6') u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7') u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u] # Reference frames N = ReferenceFrame('N') # Inertial frame NO = Point('NO') # Inertial origin A = N.orientnew('A', 'Axis', [q1, N.z]) # Yaw intermediate frame B = A.orientnew('B', 'Axis', [q2, A.x]) # Lean intermediate frame C = B.orientnew('C', 'Axis', [q3, B.y]) # Disc fixed frame CO = NO.locatenew('CO', q4*N.x + q5*N.y + q6*N.z) # Disc center # Disc angular velocity in N expressed using time derivatives of coordinates w_c_n_qd = C.ang_vel_in(N) w_b_n_qd = B.ang_vel_in(N) # Inertial angular velocity and angular acceleration of disc fixed frame C.set_ang_vel(N, u1*B.x + u2*B.y + u3*B.z) # Disc center velocity in N expressed using time derivatives of coordinates v_co_n_qd = CO.pos_from(NO).dt(N) # Disc center velocity in N expressed using generalized speeds CO.set_vel(N, u4*C.x + u5*C.y + u6*C.z) # Disc Ground Contact Point P = CO.locatenew('P', r*B.z) P.v2pt_theory(CO, N, C) # Configuration constraint f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)]) # Velocity level constraints f_v = Matrix([dot(P.vel(N), uv) for uv in C]) # Kinematic differential equations kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] + [dot(v_co_n_qd - CO.vel(N), uv) for uv in N]) qdots = solve(kindiffs, qd) # Set angular velocity of remaining frames B.set_ang_vel(N, w_b_n_qd.subs(qdots)) C.set_ang_acc(N, C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N))) # Active forces F_CO = m*g*A.z # Create inertia dyadic of disc C about point CO I = (m * r**2) / 4 J = (m * r**2) / 2 I_C_CO = inertia(C, I, J, I) Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO)) BL = [Disc] FL = [(CO, F_CO)] KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3], kd_eqs=kindiffs, q_dependent=[q6], configuration_constraints=f_c, u_dependent=[u4, u5, u6], velocity_constraints=f_v) with warns_deprecated_sympy(): (fr, fr_star) = KM.kanes_equations(FL, BL) # Test generalized form equations linearizer = KM.to_linearizer() assert linearizer.f_c == f_c assert linearizer.f_v == f_v assert linearizer.f_a == f_v.diff(t) sol = solve(linearizer.f_0 + linearizer.f_1, qd) for qi in qd: assert sol[qi] == qdots[qi] assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix([0, 0, 0]) # Perform the linearization # Precomputed operating point q_op = {q6: -r*cos(q2)} u_op = {u1: 0, u2: sin(q2)*q1d + q3d, u3: cos(q2)*q1d, u4: -r*(sin(q2)*q1d + q3d)*cos(q3), u5: 0, u6: -r*(sin(q2)*q1d + q3d)*sin(q3)} qd_op = {q2d: 0, q4d: -r*(sin(q2)*q1d + q3d)*cos(q1), q5d: -r*(sin(q2)*q1d + q3d)*sin(q1), q6d: 0} ud_op = {u1d: 4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5, u2d: 0, u3d: 0, u4d: r*(sin(q2)*sin(q3)*q1d*q3d + sin(q3)*q3d**2), u5d: r*(4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5), u6d: -r*(sin(q2)*cos(q3)*q1d*q3d + cos(q3)*q3d**2)} A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op], A_and_B=True, simplify=True) upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1} # Precomputed solution A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0], [sin(q1)*q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0], [-cos(q1)*q3d, 0, 0, 0, 0, cos(q1), -sin(q1), 0], [0, Rational(4, 5), 0, 0, 0, 0, 0, 6*q3d/5], [0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, -2*q3d, 0, 0]]) B_sol = Matrix([]) # Check that linearization is correct assert A.subs(upright_nominal) == A_sol assert B.subs(upright_nominal) == B_sol # Check eigenvalues at critical speed are all zero: assert A.subs(upright_nominal).subs(q3d, 1/sqrt(3)).eigenvals() == {0: 8}
def test_linearize_pendulum_kane_nonminimal(): # Create generalized coordinates and speeds for this non-minimal realization # q1, q2 = N.x and N.y coordinates of pendulum # u1, u2 = N.x and N.y velocities of pendulum q1, q2 = dynamicsymbols('q1:3') q1d, q2d = dynamicsymbols('q1:3', level=1) u1, u2 = dynamicsymbols('u1:3') u1d, u2d = dynamicsymbols('u1:3', level=1) L, m, t = symbols('L, m, t') g = 9.8 # Compose world frame N = ReferenceFrame('N') pN = Point('N*') pN.set_vel(N, 0) # A.x is along the pendulum theta1 = atan(q2/q1) A = N.orientnew('A', 'axis', [theta1, N.z]) # Locate the pendulum mass P = pN.locatenew('P1', q1*N.x + q2*N.y) pP = Particle('pP', P, m) # Calculate the kinematic differential equations kde = Matrix([q1d - u1, q2d - u2]) dq_dict = solve(kde, [q1d, q2d]) # Set velocity of point P P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict)) # Configuration constraint is length of pendulum f_c = Matrix([P.pos_from(pN).magnitude() - L]) # Velocity constraint is that the velocity in the A.x direction is # always zero (the pendulum is never getting longer). f_v = Matrix([P.vel(N).express(A).dot(A.x)]) f_v.simplify() # Acceleration constraints is the time derivative of the velocity constraint f_a = f_v.diff(t) f_a.simplify() # Input the force resultant at P R = m*g*N.x # Derive the equations of motion using the KanesMethod class. KM = KanesMethod(N, q_ind=[q2], u_ind=[u2], q_dependent=[q1], u_dependent=[u1], configuration_constraints=f_c, velocity_constraints=f_v, acceleration_constraints=f_a, kd_eqs=kde) with warns_deprecated_sympy(): (fr, frstar) = KM.kanes_equations([(P, R)], [pP]) # Set the operating point to be straight down, and non-moving q_op = {q1: L, q2: 0} u_op = {u1: 0, u2: 0} ud_op = {u1d: 0, u2d: 0} A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True, simplify=True) assert A.expand() == Matrix([[0, 1], [-9.8/L, 0]]) assert B == Matrix([])
def test_non_central_inertia(): # This tests that the calculation of Fr* does not depend the point # about which the inertia of a rigid body is defined. This test solves # exercises 8.12, 8.17 from Kane 1985. # Declare symbols q1, q2, q3 = dynamicsymbols('q1:4') q1d, q2d, q3d = dynamicsymbols('q1:4', level=1) u1, u2, u3, u4, u5 = dynamicsymbols('u1:6') u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta') a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t') Q1, Q2, Q3 = symbols('Q1, Q2 Q3') IA22, IA23, IA33 = symbols('IA22 IA23 IA33') # Reference Frames F = ReferenceFrame('F') P = F.orientnew('P', 'axis', [-theta, F.y]) A = P.orientnew('A', 'axis', [q1, P.x]) A.set_ang_vel(F, u1 * A.x + u3 * A.z) # define frames for wheels B = A.orientnew('B', 'axis', [q2, A.z]) C = A.orientnew('C', 'axis', [q3, A.z]) B.set_ang_vel(A, u4 * A.z) C.set_ang_vel(A, u5 * A.z) # define points D, S*, Q on frame A and their velocities pD = Point('D') pD.set_vel(A, 0) # u3 will not change v_D_F since wheels are still assumed to roll without slip. pD.set_vel(F, u2 * A.y) pS_star = pD.locatenew('S*', e * A.y) pQ = pD.locatenew('Q', f * A.y - R * A.x) for p in [pS_star, pQ]: p.v2pt_theory(pD, F, A) # masscenters of bodies A, B, C pA_star = pD.locatenew('A*', a * A.y) pB_star = pD.locatenew('B*', b * A.z) pC_star = pD.locatenew('C*', -b * A.z) for p in [pA_star, pB_star, pC_star]: p.v2pt_theory(pD, F, A) # points of B, C touching the plane P pB_hat = pB_star.locatenew('B^', -R * A.x) pC_hat = pC_star.locatenew('C^', -R * A.x) pB_hat.v2pt_theory(pB_star, F, B) pC_hat.v2pt_theory(pC_star, F, C) # the velocities of B^, C^ are zero since B, C are assumed to roll without slip kde = [q1d - u1, q2d - u4, q3d - u5] vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] # inertias of bodies A, B, C # IA22, IA23, IA33 are not specified in the problem statement, but are # necessary to define an inertia object. Although the values of # IA22, IA23, IA33 are not known in terms of the variables given in the # problem statement, they do not appear in the general inertia terms. inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0) inertia_B = inertia(B, K, K, J) inertia_C = inertia(C, K, K, J) # define the rigid bodies A, B, C rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star)) rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star)) rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star)) km = KanesMethod(F, q_ind=[q1, q2, q3], u_ind=[u1, u2], kd_eqs=kde, u_dependent=[u4, u5], velocity_constraints=vc, u_auxiliary=[u3]) forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)] bodies = [rbA, rbB, rbC] with warns_deprecated_sympy(): fr, fr_star = km.kanes_equations(forces, bodies) vc_map = solve(vc, [u4, u5]) # KanesMethod returns the negative of Fr, Fr* as defined in Kane1985. fr_star_expected = Matrix([ -(IA + 2 * J * b**2 / R**2 + 2 * K + mA * a**2 + 2 * mB * b**2) * u1.diff(t) - mA * a * u1 * u2, -(mA + 2 * mB + 2 * J / R**2) * u2.diff(t) + mA * a * u1**2, 0 ]) t = trigsimp(fr_star.subs(vc_map).subs({u3: 0})).doit().expand() assert ((fr_star_expected - t).expand() == zeros(3, 1)) # define inertias of rigid bodies A, B, C about point D # I_S/O = I_S/S* + I_S*/O bodies2 = [] for rb, I_star in zip([rbA, rbB, rbC], [inertia_A, inertia_B, inertia_C]): I = I_star + inertia_of_point_mass(rb.mass, rb.masscenter.pos_from(pD), rb.frame) bodies2.append(RigidBody('', rb.masscenter, rb.frame, rb.mass, (I, pD))) with warns_deprecated_sympy(): fr2, fr_star2 = km.kanes_equations(forces, bodies2) t = trigsimp(fr_star2.subs(vc_map).subs({u3: 0})).doit() assert (fr_star_expected - t).expand() == zeros(3, 1)
def test_non_central_inertia(): # This tests that the calculation of Fr* does not depend the point # about which the inertia of a rigid body is defined. This test solves # exercises 8.12, 8.17 from Kane 1985. # Declare symbols q1, q2, q3 = dynamicsymbols('q1:4') q1d, q2d, q3d = dynamicsymbols('q1:4', level=1) u1, u2, u3, u4, u5 = dynamicsymbols('u1:6') u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta') a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t') Q1, Q2, Q3 = symbols('Q1, Q2 Q3') IA22, IA23, IA33 = symbols('IA22 IA23 IA33') # Reference Frames F = ReferenceFrame('F') P = F.orientnew('P', 'axis', [-theta, F.y]) A = P.orientnew('A', 'axis', [q1, P.x]) A.set_ang_vel(F, u1*A.x + u3*A.z) # define frames for wheels B = A.orientnew('B', 'axis', [q2, A.z]) C = A.orientnew('C', 'axis', [q3, A.z]) B.set_ang_vel(A, u4 * A.z) C.set_ang_vel(A, u5 * A.z) # define points D, S*, Q on frame A and their velocities pD = Point('D') pD.set_vel(A, 0) # u3 will not change v_D_F since wheels are still assumed to roll without slip. pD.set_vel(F, u2 * A.y) pS_star = pD.locatenew('S*', e*A.y) pQ = pD.locatenew('Q', f*A.y - R*A.x) for p in [pS_star, pQ]: p.v2pt_theory(pD, F, A) # masscenters of bodies A, B, C pA_star = pD.locatenew('A*', a*A.y) pB_star = pD.locatenew('B*', b*A.z) pC_star = pD.locatenew('C*', -b*A.z) for p in [pA_star, pB_star, pC_star]: p.v2pt_theory(pD, F, A) # points of B, C touching the plane P pB_hat = pB_star.locatenew('B^', -R*A.x) pC_hat = pC_star.locatenew('C^', -R*A.x) pB_hat.v2pt_theory(pB_star, F, B) pC_hat.v2pt_theory(pC_star, F, C) # the velocities of B^, C^ are zero since B, C are assumed to roll without slip kde = [q1d - u1, q2d - u4, q3d - u5] vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] # inertias of bodies A, B, C # IA22, IA23, IA33 are not specified in the problem statement, but are # necessary to define an inertia object. Although the values of # IA22, IA23, IA33 are not known in terms of the variables given in the # problem statement, they do not appear in the general inertia terms. inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0) inertia_B = inertia(B, K, K, J) inertia_C = inertia(C, K, K, J) # define the rigid bodies A, B, C rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star)) rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star)) rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star)) km = KanesMethod(F, q_ind=[q1, q2, q3], u_ind=[u1, u2], kd_eqs=kde, u_dependent=[u4, u5], velocity_constraints=vc, u_auxiliary=[u3]) forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)] bodies = [rbA, rbB, rbC] fr, fr_star = km.kanes_equations(forces, bodies) vc_map = solve(vc, [u4, u5]) # KanesMethod returns the negative of Fr, Fr* as defined in Kane1985. fr_star_expected = Matrix([ -(IA + 2*J*b**2/R**2 + 2*K + mA*a**2 + 2*mB*b**2) * u1.diff(t) - mA*a*u1*u2, -(mA + 2*mB +2*J/R**2) * u2.diff(t) + mA*a*u1**2, 0]) assert (trigsimp(fr_star.subs(vc_map).subs(u3, 0)).doit().expand() == fr_star_expected.expand()) # define inertias of rigid bodies A, B, C about point D # I_S/O = I_S/S* + I_S*/O bodies2 = [] for rb, I_star in zip([rbA, rbB, rbC], [inertia_A, inertia_B, inertia_C]): I = I_star + inertia_of_point_mass(rb.mass, rb.masscenter.pos_from(pD), rb.frame) bodies2.append(RigidBody('', rb.masscenter, rb.frame, rb.mass, (I, pD))) fr2, fr_star2 = km.kanes_equations(forces, bodies2) assert (trigsimp(fr_star2.subs(vc_map).subs(u3, 0)).doit().expand() == fr_star_expected.expand())
def test_rolling_disc(): # Rolling Disc Example # Here the rolling disc is formed from the contact point up, removing the # need to introduce generalized speeds. Only 3 configuration and three # speed variables are need to describe this system, along with the disc's # mass and radius, and the local gravity (note that mass will drop out). q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3') q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1) r, m, g = symbols('r m g') # The kinematics are formed by a series of simple rotations. Each simple # rotation creates a new frame, and the next rotation is defined by the new # frame's basis vectors. This example uses a 3-1-2 series of rotations, or # Z, X, Y series of rotations. Angular velocity for this is defined using # the second frame's basis (the lean frame). N = ReferenceFrame('N') Y = N.orientnew('Y', 'Axis', [q1, N.z]) L = Y.orientnew('L', 'Axis', [q2, Y.x]) R = L.orientnew('R', 'Axis', [q3, L.y]) w_R_N_qd = R.ang_vel_in(N) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) # This is the translational kinematics. We create a point with no velocity # in N; this is the contact point between the disc and ground. Next we form # the position vector from the contact point to the disc's center of mass. # Finally we form the velocity and acceleration of the disc. C = Point('C') C.set_vel(N, 0) Dmc = C.locatenew('Dmc', r * L.z) Dmc.v2pt_theory(C, N, R) # This is a simple way to form the inertia dyadic. I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2) # Kinematic differential equations; how the generalized coordinate time # derivatives relate to generalized speeds. kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L] # Creation of the force list; it is the gravitational force at the mass # center of the disc. Then we create the disc by assigning a Point to the # center of mass attribute, a ReferenceFrame to the frame attribute, and mass # and inertia. Then we form the body list. ForceList = [(Dmc, - m * g * Y.z)] BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc)) BodyList = [BodyD] # Finally we form the equations of motion, using the same steps we did # before. Specify inertial frame, supply generalized speeds, supply # kinematic differential equation dictionary, compute Fr from the force # list and Fr* from the body list, compute the mass matrix and forcing # terms, then solve for the u dots (time derivatives of the generalized # speeds). KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd) KM.kanes_equations(ForceList, BodyList) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing kdd = KM.kindiffdict() rhs = rhs.subs(kdd) rhs.simplify() assert rhs.expand() == Matrix([(6*u2*u3*r - u3**2*r*tan(q2) + 4*g*sin(q2))/(5*r), -2*u1*u3/3, u1*(-2*u2 + u3*tan(q2))]).expand() # This code tests our output vs. benchmark values. When r=g=m=1, the # critical speed (where all eigenvalues of the linearized equations are 0) # is 1 / sqrt(3) for the upright case. A = KM.linearize(A_and_B=True, new_method=True)[0] A_upright = A.subs({r: 1, g: 1, m: 1}).subs({q1: 0, q2: 0, q3: 0, u1: 0, u3: 0}) assert A_upright.subs(u2, 1 / sqrt(3)).eigenvals() == {S(0): 6}
# ============= l_ankle_torque, l_hip_torque, r_hip_torque = dynamicsymbols("T_a, T_k, T_h") l_leg_torque = (l_leg_frame, l_ankle_torque * inertial_frame.z - l_hip_torque * inertial_frame.z) body_torque = (body_frame, l_hip_torque * inertial_frame.z) # Equations of Motion # =================== coordinates = [theta1, theta2] speeds = [omega1, omega2] kane = KanesMethod(inertial_frame, coordinates, speeds, kinematical_differential_equations) loads = [l_leg_grav_force, body_grav_force, r_leg_grav_force, l_leg_torque, body_torque] bodies = [l_leg, body, r_leg] fr, frstar = kane.kanes_equations(loads, bodies) mass_matrix = kane.mass_matrix_full forcing_vector = kane.forcing_full # List the symbolic arguments # =========================== # Constants # ---------
class Linkage(MultiBodySystem): """TODO """ def __init__(self, name): self._name = name self._root = RootLink(self) # TODO maybe don't need backpointer self._constants = dict() self._constant_descs = dict() self._forces = dict() self._gravity_vector = None self._gravity_vector_updated = Event( 'Update gravity forces when gravity vector is changed.') self._gravity_vector_updated.subscriber_new(self._manage_gravity_forces) def _get_name(self): return self._name def _set_name(self, name): self._name = name name = property(_get_name, _set_name) def _get_root(self): return self._root root = property(_get_root) def constant_new(self, name, description): self._constants[name] = symbols(name) self._constant_descs[name] = description return self._constants[name] def force_new(self, name, point_of_application, vec): """TODO reserved names: '_gravity' """ # TODO #if name in self._forces: # # TODO # raise Exception("Force with name '{}' already exists.".format(name)) self._forces[name] = (point_of_application, vec) def force_del(self, name): self._forces.pop(name) def _manage_gravity_forces(self): # TODO must modify to account for gravity_force being modified more # than once. for body in self.body_list(): self.force_new('%s_gravity', body.masscenter, body.mass * self.gravity_vector) def _get_gravity_vector(self): return self._gravity_vector def _set_gravity_vector(self, vec): _check_vector(vec) self._gravity_vector = vec self._gravity_vector_updated.fire() gravity_vector = property(_get_gravity_vector, _set_gravity_vector) def independent_coordinates(self): return self.root.independent_coordinates_in_subtree() def independent_speeds(self): return self.root.independent_speeds_in_subtree() def kinematic_differential_equations(self): return self.root.kinematic_differential_equations_in_subtree() def body_list(self): return self.root.body_list_in_subtree() def force_list(self): return self._forces.values() #TODO def _init_kanes_method(self): #TODO # TODO move the creation of Kane's Method somewhere else. #TODO self._kanes_method = KanesMethod(self.independent_coordinates, #TODO self.independent_speeds, self.kinematic_diffeqs) #TODO # TODO must make this call to get the mass matrix, etc.? #TODO self._kanes_method.kanes_equations(self.force_list, self.body_list) def mass_matrix(self): #if not (self._kanes_method and self.up_to_date): # self._init_kanes_method() # TODO move the creation of Kane's Method somewhere else. self._kanes_method = KanesMethod(self.root.frame, q_ind=self.independent_coordinates(), u_ind=self.independent_speeds(), kd_eqs=self.kinematic_differential_equations() ) # TODO must make this call to get the mass matrix, etc.? self._kanes_method.kanes_equations(self.force_list(), self.body_list()); return self._kanes_method.mass_matrix def state_derivatives(self): # TODO find a way to use a cached mass matrix. kin_diff_eqns = self._kanes_method.kindiffdict() state_derivatives = self.mass_matrix.inv() * self._kanes_method.forcing state_derivatives = state_derivatives.subs(kin_diff_eqns) state_derivatives.simplify() return state_derivatives def _check_link_name(self): # TODO pass
#!/usr/bin/env python from sympy.physics.mechanics import KanesMethod from .kinetics import * # Equations of Motion # =================== coordinates = [theta1, theta2, theta3] speeds = [omega1, omega2, omega3] kane = KanesMethod(inertial_frame, coordinates, speeds, kinematical_differential_equations) loads = [lower_leg_grav_force, upper_leg_grav_force, torso_grav_force, lower_leg_torque, upper_leg_torque, torso_torque] bodies = [lower_leg, upper_leg, torso] fr, frstar = kane.kanes_equations(bodies, loads) mass_matrix = kane.mass_matrix_full forcing_vector = kane.forcing_full
two_link_torque_dp1 = (two_frame_dp1, two_torque_dp1 * inertial_frame_dp1.z + one_torque_dp2 * inertial_frame_dp1.z) one_link_torque_dp2 = (one_frame_dp2, one_torque_dp2 * inertial_frame_dp2.z - two_torque_dp2 * inertial_frame_dp2.z) two_link_torque_dp2 = (two_frame_dp2, two_torque_dp2 * inertial_frame_dp2.z) # Equations of Motion # =================== coordinates_dp1 = [theta1_dp1, theta2_dp1] coordinates_dp2 = [theta1_dp2, theta2_dp2] speeds_dp1 = [omega1_dp1, omega2_dp1] speeds_dp2 = [omega1_dp2, omega2_dp2] kane_dp1 = KanesMethod(inertial_frame_dp1, coordinates_dp1, speeds_dp1, kinematical_differential_equations_dp1) kane_dp2 = KanesMethod(inertial_frame_dp2, coordinates_dp2, speeds_dp2, kinematic_differential_equations_dp2) loads_dp1 = [one_grav_force_dp1, two_grav_force_dp1, one_link_torque_dp1, two_link_torque_dp1] loads_dp2 = [one_grav_force_dp2, two_grav_force_dp2, two_link_torque_dp2, two_link_torque_dp2] bodies_dp1 = [oneB_dp1, twoB_dp1] particles_dp2 = [oneP_dp2, twoP_dp2] fr_dp1, frstar_dp1 = kane_dp1.kanes_equations(loads, bodies) mass_matrix_dp1 = kane_dp1.mass_matrix forcing_vector_dp1 = kane_dp1.forcing mass_matrix_dp2 = kane_dp2.mass_matrix forcing_vector_dp2 = kane_dp2.forcing
class Linkage(MultiBodySystem): """TODO """ def __init__(self, name): self._name = name self._root = RootLink(self) # TODO maybe don't need backpointer self._constants = dict() self._constant_descs = dict() self._forces = dict() self._gravity_vector = None self._gravity_vector_updated = Event( 'Update gravity forces when gravity vector is changed.') self._gravity_vector_updated.subscriber_new( self._manage_gravity_forces) def _get_name(self): return self._name def _set_name(self, name): self._name = name name = property(_get_name, _set_name) def _get_root(self): return self._root root = property(_get_root) def constant_new(self, name, description): self._constants[name] = symbols(name) self._constant_descs[name] = description return self._constants[name] def force_new(self, name, point_of_application, vec): """TODO reserved names: '_gravity' """ # TODO #if name in self._forces: # # TODO # raise Exception("Force with name '{}' already exists.".format(name)) self._forces[name] = (point_of_application, vec) def force_del(self, name): self._forces.pop(name) def _manage_gravity_forces(self): # TODO must modify to account for gravity_force being modified more # than once. for body in self.body_list(): self.force_new('%s_gravity', body.masscenter, body.mass * self.gravity_vector) def _get_gravity_vector(self): return self._gravity_vector def _set_gravity_vector(self, vec): _check_vector(vec) self._gravity_vector = vec self._gravity_vector_updated.fire() gravity_vector = property(_get_gravity_vector, _set_gravity_vector) def independent_coordinates(self): return self.root.independent_coordinates_in_subtree() def independent_speeds(self): return self.root.independent_speeds_in_subtree() def kinematic_differential_equations(self): return self.root.kinematic_differential_equations_in_subtree() def body_list(self): return self.root.body_list_in_subtree() def force_list(self): return self._forces.values() #TODO def _init_kanes_method(self): #TODO # TODO move the creation of Kane's Method somewhere else. #TODO self._kanes_method = KanesMethod(self.independent_coordinates, #TODO self.independent_speeds, self.kinematic_diffeqs) #TODO # TODO must make this call to get the mass matrix, etc.? #TODO self._kanes_method.kanes_equations(self.force_list, self.body_list) def mass_matrix(self): #if not (self._kanes_method and self.up_to_date): # self._init_kanes_method() # TODO move the creation of Kane's Method somewhere else. self._kanes_method = KanesMethod( self.root.frame, q_ind=self.independent_coordinates(), u_ind=self.independent_speeds(), kd_eqs=self.kinematic_differential_equations()) # TODO must make this call to get the mass matrix, etc.? self._kanes_method.kanes_equations(self.force_list(), self.body_list()) return self._kanes_method.mass_matrix def state_derivatives(self): # TODO find a way to use a cached mass matrix. kin_diff_eqns = self._kanes_method.kindiffdict() state_derivatives = self.mass_matrix.inv() * self._kanes_method.forcing state_derivatives = state_derivatives.subs(kin_diff_eqns) state_derivatives.simplify() return state_derivatives def _check_link_name(self): # TODO pass
def test_sub_qdot2(): # This test solves exercises 8.3 from Kane 1985 and defines # all velocities in terms of q, qdot. We check that the generalized active # forces are correctly computed if u terms are only defined in the # kinematic differential equations. # # This functionality was added in PR 8948. Without qdot/u substitution, the # KanesMethod constructor will fail during the constraint initialization as # the B matrix will be poorly formed and inversion of the dependent part # will fail. g, m, Px, Py, Pz, R, t = symbols('g m Px Py Pz R t') q = dynamicsymbols('q:5') qd = dynamicsymbols('q:5', level=1) u = dynamicsymbols('u:5') ## Define inertial, intermediate, and rigid body reference frames A = ReferenceFrame('A') B_prime = A.orientnew('B_prime', 'Axis', [q[0], A.z]) B = B_prime.orientnew('B', 'Axis', [pi/2 - q[1], B_prime.x]) C = B.orientnew('C', 'Axis', [q[2], B.z]) ## Define points of interest and their velocities pO = Point('O') pO.set_vel(A, 0) # R is the point in plane H that comes into contact with disk C. pR = pO.locatenew('R', q[3]*A.x + q[4]*A.y) pR.set_vel(A, pR.pos_from(pO).diff(t, A)) pR.set_vel(B, 0) # C^ is the point in disk C that comes into contact with plane H. pC_hat = pR.locatenew('C^', 0) pC_hat.set_vel(C, 0) # C* is the point at the center of disk C. pCs = pC_hat.locatenew('C*', R*B.y) pCs.set_vel(C, 0) pCs.set_vel(B, 0) # calculate velocites of points C* and C^ in frame A pCs.v2pt_theory(pR, A, B) # points C* and R are fixed in frame B pC_hat.v2pt_theory(pCs, A, C) # points C* and C^ are fixed in frame C ## Define forces on each point of the system R_C_hat = Px*A.x + Py*A.y + Pz*A.z R_Cs = -m*g*A.z forces = [(pC_hat, R_C_hat), (pCs, R_Cs)] ## Define kinematic differential equations # let ui = omega_C_A & bi (i = 1, 2, 3) # u4 = qd4, u5 = qd5 u_expr = [C.ang_vel_in(A) & uv for uv in B] u_expr += qd[3:] kde = [ui - e for ui, e in zip(u, u_expr)] km1 = KanesMethod(A, q, u, kde) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) fr1, _ = km1.kanes_equations(forces, []) ## Calculate generalized active forces if we impose the condition that the # disk C is rolling without slipping u_indep = u[:3] u_dep = list(set(u) - set(u_indep)) vc = [pC_hat.vel(A) & uv for uv in [A.x, A.y]] km2 = KanesMethod(A, q, u_indep, kde, u_dependent=u_dep, velocity_constraints=vc) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) fr2, _ = km2.kanes_equations(forces, []) fr1_expected = Matrix([ -R*g*m*sin(q[1]), -R*(Px*cos(q[0]) + Py*sin(q[0]))*tan(q[1]), R*(Px*cos(q[0]) + Py*sin(q[0])), Px, Py]) fr2_expected = Matrix([ -R*g*m*sin(q[1]), 0, 0]) assert (trigsimp(fr1.expand()) == trigsimp(fr1_expected.expand())) assert (trigsimp(fr2.expand()) == trigsimp(fr2_expected.expand()))
def test_sub_qdot(): # This test solves exercises 8.12, 8.17 from Kane 1985 and defines # some velocities in terms of q, qdot. ## --- Declare symbols --- q1, q2, q3 = dynamicsymbols('q1:4') q1d, q2d, q3d = dynamicsymbols('q1:4', level=1) u1, u2, u3 = dynamicsymbols('u1:4') u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta') a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t') IA22, IA23, IA33 = symbols('IA22 IA23 IA33') Q1, Q2, Q3 = symbols('Q1 Q2 Q3') # --- Reference Frames --- F = ReferenceFrame('F') P = F.orientnew('P', 'axis', [-theta, F.y]) A = P.orientnew('A', 'axis', [q1, P.x]) A.set_ang_vel(F, u1*A.x + u3*A.z) # define frames for wheels B = A.orientnew('B', 'axis', [q2, A.z]) C = A.orientnew('C', 'axis', [q3, A.z]) ## --- define points D, S*, Q on frame A and their velocities --- pD = Point('D') pD.set_vel(A, 0) # u3 will not change v_D_F since wheels are still assumed to roll w/o slip pD.set_vel(F, u2 * A.y) pS_star = pD.locatenew('S*', e*A.y) pQ = pD.locatenew('Q', f*A.y - R*A.x) # masscenters of bodies A, B, C pA_star = pD.locatenew('A*', a*A.y) pB_star = pD.locatenew('B*', b*A.z) pC_star = pD.locatenew('C*', -b*A.z) for p in [pS_star, pQ, pA_star, pB_star, pC_star]: p.v2pt_theory(pD, F, A) # points of B, C touching the plane P pB_hat = pB_star.locatenew('B^', -R*A.x) pC_hat = pC_star.locatenew('C^', -R*A.x) pB_hat.v2pt_theory(pB_star, F, B) pC_hat.v2pt_theory(pC_star, F, C) # --- relate qdot, u --- # the velocities of B^, C^ are zero since B, C are assumed to roll w/o slip kde = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] kde += [u1 - q1d] kde_map = solve(kde, [q1d, q2d, q3d]) for k, v in list(kde_map.items()): kde_map[k.diff(t)] = v.diff(t) # inertias of bodies A, B, C # IA22, IA23, IA33 are not specified in the problem statement, but are # necessary to define an inertia object. Although the values of # IA22, IA23, IA33 are not known in terms of the variables given in the # problem statement, they do not appear in the general inertia terms. inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0) inertia_B = inertia(B, K, K, J) inertia_C = inertia(C, K, K, J) # define the rigid bodies A, B, C rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star)) rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star)) rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star)) ## --- use kanes method --- km = KanesMethod(F, [q1, q2, q3], [u1, u2], kd_eqs=kde, u_auxiliary=[u3]) forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)] bodies = [rbA, rbB, rbC] # Q2 = -u_prime * u2 * Q1 / sqrt(u2**2 + f**2 * u1**2) # -u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2) = R / Q1 * Q2 fr_expected = Matrix([ f*Q3 + M*g*e*sin(theta)*cos(q1), Q2 + M*g*sin(theta)*sin(q1), e*M*g*cos(theta) - Q1*f - Q2*R]) #Q1 * (f - u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2)))]) fr_star_expected = Matrix([ -(IA + 2*J*b**2/R**2 + 2*K + mA*a**2 + 2*mB*b**2) * u1.diff(t) - mA*a*u1*u2, -(mA + 2*mB +2*J/R**2) * u2.diff(t) + mA*a*u1**2, 0]) fr, fr_star = km.kanes_equations(forces, bodies) assert (fr.expand() == fr_expected.expand()) assert (trigsimp(fr_star).expand() == fr_star_expected.expand())
def test_aux_dep(): # This test is about rolling disc dynamics, comparing the results found # with KanesMethod to those found when deriving the equations "manually" # with SymPy. # The terms Fr, Fr*, and Fr*_steady are all compared between the two # methods. Here, Fr*_steady refers to the generalized inertia forces for an # equilibrium configuration. # Note: comparing to the test of test_rolling_disc() in test_kane.py, this # test also tests auxiliary speeds and configuration and motion constraints #, seen in the generalized dependent coordinates q[3], and depend speeds # u[3], u[4] and u[5]. # First, mannual derivation of Fr, Fr_star, Fr_star_steady. # Symbols for time and constant parameters. # Symbols for contact forces: Fx, Fy, Fz. t, r, m, g, I, J = symbols('t r m g I J') Fx, Fy, Fz = symbols('Fx Fy Fz') # Configuration variables and their time derivatives: # q[0] -- yaw # q[1] -- lean # q[2] -- spin # q[3] -- dot(-r*B.z, A.z) -- distance from ground plane to disc center in # A.z direction # Generalized speeds and their time derivatives: # u[0] -- disc angular velocity component, disc fixed x direction # u[1] -- disc angular velocity component, disc fixed y direction # u[2] -- disc angular velocity component, disc fixed z direction # u[3] -- disc velocity component, A.x direction # u[4] -- disc velocity component, A.y direction # u[5] -- disc velocity component, A.z direction # Auxiliary generalized speeds: # ua[0] -- contact point auxiliary generalized speed, A.x direction # ua[1] -- contact point auxiliary generalized speed, A.y direction # ua[2] -- contact point auxiliary generalized speed, A.z direction q = dynamicsymbols('q:4') qd = [qi.diff(t) for qi in q] u = dynamicsymbols('u:6') ud = [ui.diff(t) for ui in u] #ud_zero = {udi : 0 for udi in ud} ud_zero = dict(zip(ud, [0.]*len(ud))) ua = dynamicsymbols('ua:3') #ua_zero = {uai : 0 for uai in ua} ua_zero = dict(zip(ua, [0.]*len(ua))) # Reference frames: # Yaw intermediate frame: A. # Lean intermediate frame: B. # Disc fixed frame: C. N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q[0], N.z]) B = A.orientnew('B', 'Axis', [q[1], A.x]) C = B.orientnew('C', 'Axis', [q[2], B.y]) # Angular velocity and angular acceleration of disc fixed frame # u[0], u[1] and u[2] are generalized independent speeds. C.set_ang_vel(N, u[0]*B.x + u[1]*B.y + u[2]*B.z) C.set_ang_acc(N, C.ang_vel_in(N).diff(t, B) + cross(B.ang_vel_in(N), C.ang_vel_in(N))) # Velocity and acceleration of points: # Disc-ground contact point: P. # Center of disc: O, defined from point P with depend coordinate: q[3] # u[3], u[4] and u[5] are generalized dependent speeds. P = Point('P') P.set_vel(N, ua[0]*A.x + ua[1]*A.y + ua[2]*A.z) O = P.locatenew('O', q[3]*A.z + r*sin(q[1])*A.y) O.set_vel(N, u[3]*A.x + u[4]*A.y + u[5]*A.z) O.set_acc(N, O.vel(N).diff(t, A) + cross(A.ang_vel_in(N), O.vel(N))) # Kinematic differential equations: # Two equalities: one is w_c_n_qd = C.ang_vel_in(N) in three coordinates # directions of B, for qd0, qd1 and qd2. # the other is v_o_n_qd = O.vel(N) in A.z direction for qd3. # Then, solve for dq/dt's in terms of u's: qd_kd. w_c_n_qd = qd[0]*A.z + qd[1]*B.x + qd[2]*B.y v_o_n_qd = O.pos_from(P).diff(t, A) + cross(A.ang_vel_in(N), O.pos_from(P)) kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] + [dot(v_o_n_qd - O.vel(N), A.z)]) qd_kd = solve(kindiffs, qd) # Values of generalized speeds during a steady turn for later substitution # into the Fr_star_steady. steady_conditions = solve(kindiffs.subs({qd[1] : 0, qd[3] : 0}), u) steady_conditions.update({qd[1] : 0, qd[3] : 0}) # Partial angular velocities and velocities. partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u + ua] partial_v_O = [O.vel(N).diff(ui, N) for ui in u + ua] partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua] # Configuration constraint: f_c, the projection of radius r in A.z direction # is q[3]. # Velocity constraints: f_v, for u3, u4 and u5. # Acceleration constraints: f_a. f_c = Matrix([dot(-r*B.z, A.z) - q[3]]) f_v = Matrix([dot(O.vel(N) - (P.vel(N) + cross(C.ang_vel_in(N), O.pos_from(P))), ai).expand() for ai in A]) v_o_n = cross(C.ang_vel_in(N), O.pos_from(P)) a_o_n = v_o_n.diff(t, A) + cross(A.ang_vel_in(N), v_o_n) f_a = Matrix([dot(O.acc(N) - a_o_n, ai) for ai in A]) # Solve for constraint equations in the form of # u_dependent = A_rs * [u_i; u_aux]. # First, obtain constraint coefficient matrix: M_v * [u; ua] = 0; # Second, taking u[0], u[1], u[2] as independent, # taking u[3], u[4], u[5] as dependent, # rearranging the matrix of M_v to be A_rs for u_dependent. # Third, u_aux ==0 for u_dep, and resulting dictionary of u_dep_dict. M_v = zeros(3, 9) for i in range(3): for j, ui in enumerate(u + ua): M_v[i, j] = f_v[i].diff(ui) M_v_i = M_v[:, :3] M_v_d = M_v[:, 3:6] M_v_aux = M_v[:, 6:] M_v_i_aux = M_v_i.row_join(M_v_aux) A_rs = - M_v_d.inv() * M_v_i_aux u_dep = A_rs[:, :3] * Matrix(u[:3]) u_dep_dict = dict(zip(u[3:], u_dep)) #u_dep_dict = {udi : u_depi[0] for udi, u_depi in zip(u[3:], u_dep.tolist())} # Active forces: F_O acting on point O; F_P acting on point P. # Generalized active forces (unconstrained): Fr_u = F_point * pv_point. F_O = m*g*A.z F_P = Fx * A.x + Fy * A.y + Fz * A.z Fr_u = Matrix([dot(F_O, pv_o) + dot(F_P, pv_p) for pv_o, pv_p in zip(partial_v_O, partial_v_P)]) # Inertia force: R_star_O. # Inertia of disc: I_C_O, where J is a inertia component about principal axis. # Inertia torque: T_star_C. # Generalized inertia forces (unconstrained): Fr_star_u. R_star_O = -m*O.acc(N) I_C_O = inertia(B, I, J, I) T_star_C = -(dot(I_C_O, C.ang_acc_in(N)) \ + cross(C.ang_vel_in(N), dot(I_C_O, C.ang_vel_in(N)))) Fr_star_u = Matrix([dot(R_star_O, pv) + dot(T_star_C, pav) for pv, pav in zip(partial_v_O, partial_w_C)]) # Form nonholonomic Fr: Fr_c, and nonholonomic Fr_star: Fr_star_c. # Also, nonholonomic Fr_star in steady turning condition: Fr_star_steady. Fr_c = Fr_u[:3, :].col_join(Fr_u[6:, :]) + A_rs.T * Fr_u[3:6, :] Fr_star_c = Fr_star_u[:3, :].col_join(Fr_star_u[6:, :])\ + A_rs.T * Fr_star_u[3:6, :] Fr_star_steady = Fr_star_c.subs(ud_zero).subs(u_dep_dict)\ .subs(steady_conditions).subs({q[3]: -r*cos(q[1])}).expand() # Second, using KaneMethod in mechanics for fr, frstar and frstar_steady. # Rigid Bodies: disc, with inertia I_C_O. iner_tuple = (I_C_O, O) disc = RigidBody('disc', O, C, m, iner_tuple) bodyList = [disc] # Generalized forces: Gravity: F_o; Auxiliary forces: F_p. F_o = (O, F_O) F_p = (P, F_P) forceList = [F_o, F_p] # KanesMethod. kane = KanesMethod( N, q_ind= q[:3], u_ind= u[:3], kd_eqs=kindiffs, q_dependent=q[3:], configuration_constraints = f_c, u_dependent=u[3:], velocity_constraints= f_v, u_auxiliary=ua ) # fr, frstar, frstar_steady and kdd(kinematic differential equations). (fr, frstar)= kane.kanes_equations(forceList, bodyList) frstar_steady = frstar.subs(ud_zero).subs(u_dep_dict).subs(steady_conditions)\ .subs({q[3]: -r*cos(q[1])}).expand() kdd = kane.kindiffdict() # Test # First try Fr_c == fr; # Second try Fr_star_c == frstar; # Third try Fr_star_steady == frstar_steady. # Both signs are checked in case the equations were found with an inverse # sign. assert ((Matrix(Fr_c).expand() == fr.expand()) or (Matrix(Fr_c).expand() == (-fr).expand())) assert ((Matrix(Fr_star_c).expand() == frstar.expand()) or (Matrix(Fr_star_c).expand() == (-frstar).expand())) assert ((Matrix(Fr_star_steady).expand() == frstar_steady.expand()) or (Matrix(Fr_star_steady).expand() == (-frstar_steady).expand()))
def __init__(self): self.num_links = 5 # Number of links total_link_length = 1. total_link_mass = 1. self.ind_link_length = total_link_length / self.num_links ind_link_com_length = self.ind_link_length / 2. ind_link_mass = total_link_mass / self.num_links ind_link_inertia = ind_link_mass * (ind_link_com_length**2) # =======================# # Parameters for step() # # =======================# # Maximum number of steps before episode termination self.max_steps = 200 # For ODE integration self.dt = .0001 # Simultaion time step = 1ms self.sim_steps = 51 # Number of simulation steps in 1 learning step self.dt_step = np.linspace( 0., self.dt * self.sim_steps, num=self.sim_steps) # Learning time step = 50ms # Termination conditions for simulation self.num_steps = 0 # Step counter self.done = False # For visualisation self.viewer = None self.ax = False # Constraints for observation min_angle = -np.pi # Angle max_angle = np.pi min_omega = -10. # Angular velocity max_omega = 10. min_torque = -10. # Torque max_torque = 10. low_state_angle = np.full(self.num_links, min_angle) # Min angle low_state_omega = np.full(self.num_links, min_omega) # Min angular velocity low_state = np.append(low_state_angle, low_state_omega) high_state_angle = np.full(self.num_links, max_angle) # Max angle high_state_omega = np.full(self.num_links, max_omega) # Max angular velocity high_state = np.append(high_state_angle, high_state_omega) low_action = np.full(self.num_links, min_torque) # Min torque high_action = np.full(self.num_links, max_torque) # Max torque self.action_space = spaces.Box(low=low_action, high=high_action) self.observation_space = spaces.Box(low=low_state, high=high_state) # Minimum reward self.min_reward = -(max_angle**2 + .1 * max_omega**2 + .001 * max_torque**2) * self.num_links # Seeding self.seed() # ==============# # Orientations # # ==============# self.inertial_frame = ReferenceFrame('I') self.link_frame = [] self.theta = [] for i in range(self.num_links): temp_angle_name = "theta{}".format(i + 1) temp_link_name = "L{}".format(i + 1) self.theta.append(dynamicsymbols(temp_angle_name)) self.link_frame.append(ReferenceFrame(temp_link_name)) if i == 0: # First link self.link_frame[i].orient( self.inertial_frame, 'Axis', (self.theta[i], self.inertial_frame.z)) else: # Second link, third link... self.link_frame[i].orient( self.link_frame[i - 1], 'Axis', (self.theta[i], self.link_frame[i - 1].z)) # =================# # Point Locations # # =================# # --------# # Joints # # --------# self.link_length = [] self.link_joint = [] for i in range(self.num_links): temp_link_length_name = "l_L{}".format(i + 1) temp_link_joint_name = "A{}".format(i) self.link_length.append(symbols(temp_link_length_name)) self.link_joint.append(Point(temp_link_joint_name)) if i > 0: # Set position started from link2, then link3, link4... self.link_joint[i].set_pos( self.link_joint[i - 1], self.link_length[i - 1] * self.link_frame[i - 1].y) # --------------------------# # Centre of mass locations # # --------------------------# self.link_com_length = [] self.link_mass_centre = [] for i in range(self.num_links): temp_link_com_length_name = "d_L{}".format(i + 1) temp_link_mass_centre_name = "L{}_o".format(i + 1) self.link_com_length.append(symbols(temp_link_com_length_name)) self.link_mass_centre.append(Point(temp_link_mass_centre_name)) self.link_mass_centre[i].set_pos( self.link_joint[i], self.link_com_length[i] * self.link_frame[i].y) # ===========================================# # Define kinematical differential equations # # ===========================================# self.omega = [] self.kinematical_differential_equations = [] self.time = symbols('t') for i in range(self.num_links): temp_omega_name = "omega{}".format(i + 1) self.omega.append(dynamicsymbols(temp_omega_name)) self.kinematical_differential_equations.append( self.omega[i] - self.theta[i].diff(self.time)) # ====================# # Angular Velocities # # ====================# for i in range(self.num_links): if i == 0: # First link self.link_frame[i].set_ang_vel( self.inertial_frame, self.omega[i] * self.inertial_frame.z) else: # Second link, third link... self.link_frame[i].set_ang_vel( self.link_frame[i - 1], self.omega[i] * self.link_frame[i - 1].z) # ===================# # Linear Velocities # # ===================# for i in range(self.num_links): if i == 0: # First link self.link_joint[i].set_vel(self.inertial_frame, 0) else: # Second link, third link... self.link_joint[i].v2pt_theory(self.link_joint[i - 1], self.inertial_frame, self.link_frame[i - 1]) self.link_mass_centre[i].v2pt_theory(self.link_joint[i], self.inertial_frame, self.link_frame[i]) # ======# # Mass # # ======# self.link_mass = [] for i in range(self.num_links): temp_link_mass_name = "m_L{}".format(i + 1) self.link_mass.append(symbols(temp_link_mass_name)) # =========# # Inertia # # =========# self.link_inertia = [] self.link_inertia_dyadic = [] self.link_central_inertia = [] for i in range(self.num_links): temp_link_inertia_name = "I_L{}z".format(i + 1) self.link_inertia.append(symbols(temp_link_inertia_name)) self.link_inertia_dyadic.append( inertia(self.link_frame[i], 0, 0, self.link_inertia[i])) self.link_central_inertia.append( (self.link_inertia_dyadic[i], self.link_mass_centre[i])) # ==============# # Rigid Bodies # # ==============# self.link = [] for i in range(self.num_links): temp_link_name = "link{}".format(i + 1) self.link.append( RigidBody(temp_link_name, self.link_mass_centre[i], self.link_frame[i], self.link_mass[i], self.link_central_inertia[i])) # =========# # Gravity # # =========# self.g = symbols('g') self.link_grav_force = [] for i in range(self.num_links): self.link_grav_force.append( (self.link_mass_centre[i], -self.link_mass[i] * self.g * self.inertial_frame.y)) # ===============# # Joint Torques # # ===============# self.link_joint_torque = [] self.link_torque = [] for i in range(self.num_links): temp_link_joint_torque_name = "T_a{}".format(i + 1) self.link_joint_torque.append( dynamicsymbols(temp_link_joint_torque_name)) for i in range(self.num_links): if (i + 1) == self.num_links: # Last link self.link_torque.append( (self.link_frame[i], self.link_joint_torque[i] * self.inertial_frame.z)) else: # Other links self.link_torque.append( (self.link_frame[i], self.link_joint_torque[i] * self.inertial_frame.z - self.link_joint_torque[i + 1] * self.inertial_frame.z)) # =====================# # Equations of Motion # # =====================# self.coordinates = [] self.speeds = [] self.loads = [] self.bodies = [] for i in range(self.num_links): self.coordinates.append(self.theta[i]) self.speeds.append(self.omega[i]) self.loads.append(self.link_grav_force[i]) self.loads.append(self.link_torque[i]) self.bodies.append(self.link[i]) self.kane = KanesMethod(self.inertial_frame, self.coordinates, self.speeds, self.kinematical_differential_equations) self.fr, self.frstar = self.kane.kanes_equations( self.bodies, self.loads) self.mass_matrix = self.kane.mass_matrix_full self.forcing_vector = self.kane.forcing_full # =============================# # List the symbolic arguments # # =============================# # -----------# # Constants # # -----------# self.constants = [] for i in range(self.num_links): if (i + 1) != self.num_links: self.constants.append(self.link_length[i]) self.constants.append(self.link_com_length[i]) self.constants.append(self.link_mass[i]) self.constants.append(self.link_inertia[i]) self.constants.append(self.g) # --------------# # Time Varying # # --------------# self.coordinates = [] self.speeds = [] self.specified = [] for i in range(self.num_links): self.coordinates.append(self.theta[i]) self.speeds.append(self.omega[i]) self.specified.append(self.link_joint_torque[i]) # =======================# # Generate RHS Function # # =======================# self.right_hand_side = generate_ode_function( self.forcing_vector, self.coordinates, self.speeds, self.constants, mass_matrix=self.mass_matrix, specifieds=self.specified) # ==============================# # Specify Numerical Quantities # # ==============================# self.x = np.zeros(self.num_links * 2) self.x[:self.num_links] = deg2rad(2.0) self.numerical_constants = [] for i in range(self.num_links): if (i + 1) != self.num_links: self.numerical_constants.append(self.ind_link_length) self.numerical_constants.append(ind_link_com_length) self.numerical_constants.append(ind_link_mass) self.numerical_constants.append(ind_link_inertia) self.numerical_constants.append(9.81)
def test_rolling_disc(): # Rolling Disc Example # Here the rolling disc is formed from the contact point up, removing the # need to introduce generalized speeds. Only 3 configuration and three # speed variables are need to describe this system, along with the disc's # mass and radius, and the local gravity (note that mass will drop out). q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3') q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1) r, m, g = symbols('r m g') # The kinematics are formed by a series of simple rotations. Each simple # rotation creates a new frame, and the next rotation is defined by the new # frame's basis vectors. This example uses a 3-1-2 series of rotations, or # Z, X, Y series of rotations. Angular velocity for this is defined using # the second frame's basis (the lean frame). N = ReferenceFrame('N') Y = N.orientnew('Y', 'Axis', [q1, N.z]) L = Y.orientnew('L', 'Axis', [q2, Y.x]) R = L.orientnew('R', 'Axis', [q3, L.y]) w_R_N_qd = R.ang_vel_in(N) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) # This is the translational kinematics. We create a point with no velocity # in N; this is the contact point between the disc and ground. Next we form # the position vector from the contact point to the disc's center of mass. # Finally we form the velocity and acceleration of the disc. C = Point('C') C.set_vel(N, 0) Dmc = C.locatenew('Dmc', r * L.z) Dmc.v2pt_theory(C, N, R) # This is a simple way to form the inertia dyadic. I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2) # Kinematic differential equations; how the generalized coordinate time # derivatives relate to generalized speeds. kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L] # Creation of the force list; it is the gravitational force at the mass # center of the disc. Then we create the disc by assigning a Point to the # center of mass attribute, a ReferenceFrame to the frame attribute, and mass # and inertia. Then we form the body list. ForceList = [(Dmc, -m * g * Y.z)] BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc)) BodyList = [BodyD] # Finally we form the equations of motion, using the same steps we did # before. Specify inertial frame, supply generalized speeds, supply # kinematic differential equation dictionary, compute Fr from the force # list and Fr* from the body list, compute the mass matrix and forcing # terms, then solve for the u dots (time derivatives of the generalized # speeds). KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd) with warns_deprecated_sympy(): KM.kanes_equations(ForceList, BodyList) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing kdd = KM.kindiffdict() rhs = rhs.subs(kdd) rhs.simplify() assert rhs.expand() == Matrix([ (6 * u2 * u3 * r - u3**2 * r * tan(q2) + 4 * g * sin(q2)) / (5 * r), -2 * u1 * u3 / 3, u1 * (-2 * u2 + u3 * tan(q2)) ]).expand() assert simplify(KM.rhs() - KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros( 6, 1) # This code tests our output vs. benchmark values. When r=g=m=1, the # critical speed (where all eigenvalues of the linearized equations are 0) # is 1 / sqrt(3) for the upright case. A = KM.linearize(A_and_B=True)[0] A_upright = A.subs({ r: 1, g: 1, m: 1 }).subs({ q1: 0, q2: 0, q3: 0, u1: 0, u3: 0 }) import sympy assert sympy.sympify(A_upright.subs({u2: 1 / sqrt(3)})).eigenvals() == { S.Zero: 6 }
def test_sub_qdot2(): # This test solves exercises 8.3 from Kane 1985 and defines # all velocities in terms of q, qdot. We check that the generalized active # forces are correctly computed if u terms are only defined in the # kinematic differential equations. # # This functionality was added in PR 8948. Without qdot/u substitution, the # KanesMethod constructor will fail during the constraint initialization as # the B matrix will be poorly formed and inversion of the dependent part # will fail. g, m, Px, Py, Pz, R, t = symbols('g m Px Py Pz R t') q = dynamicsymbols('q:5') qd = dynamicsymbols('q:5', level=1) u = dynamicsymbols('u:5') ## Define inertial, intermediate, and rigid body reference frames A = ReferenceFrame('A') B_prime = A.orientnew('B_prime', 'Axis', [q[0], A.z]) B = B_prime.orientnew('B', 'Axis', [pi / 2 - q[1], B_prime.x]) C = B.orientnew('C', 'Axis', [q[2], B.z]) ## Define points of interest and their velocities pO = Point('O') pO.set_vel(A, 0) # R is the point in plane H that comes into contact with disk C. pR = pO.locatenew('R', q[3] * A.x + q[4] * A.y) pR.set_vel(A, pR.pos_from(pO).diff(t, A)) pR.set_vel(B, 0) # C^ is the point in disk C that comes into contact with plane H. pC_hat = pR.locatenew('C^', 0) pC_hat.set_vel(C, 0) # C* is the point at the center of disk C. pCs = pC_hat.locatenew('C*', R * B.y) pCs.set_vel(C, 0) pCs.set_vel(B, 0) # calculate velocites of points C* and C^ in frame A pCs.v2pt_theory(pR, A, B) # points C* and R are fixed in frame B pC_hat.v2pt_theory(pCs, A, C) # points C* and C^ are fixed in frame C ## Define forces on each point of the system R_C_hat = Px * A.x + Py * A.y + Pz * A.z R_Cs = -m * g * A.z forces = [(pC_hat, R_C_hat), (pCs, R_Cs)] ## Define kinematic differential equations # let ui = omega_C_A & bi (i = 1, 2, 3) # u4 = qd4, u5 = qd5 u_expr = [C.ang_vel_in(A) & uv for uv in B] u_expr += qd[3:] kde = [ui - e for ui, e in zip(u, u_expr)] km1 = KanesMethod(A, q, u, kde) with warns_deprecated_sympy(): fr1, _ = km1.kanes_equations(forces, []) ## Calculate generalized active forces if we impose the condition that the # disk C is rolling without slipping u_indep = u[:3] u_dep = list(set(u) - set(u_indep)) vc = [pC_hat.vel(A) & uv for uv in [A.x, A.y]] km2 = KanesMethod(A, q, u_indep, kde, u_dependent=u_dep, velocity_constraints=vc) with warns_deprecated_sympy(): fr2, _ = km2.kanes_equations(forces, []) fr1_expected = Matrix([ -R * g * m * sin(q[1]), -R * (Px * cos(q[0]) + Py * sin(q[0])) * tan(q[1]), R * (Px * cos(q[0]) + Py * sin(q[0])), Px, Py ]) fr2_expected = Matrix([-R * g * m * sin(q[1]), 0, 0]) assert (trigsimp(fr1.expand()) == trigsimp(fr1_expected.expand())) assert (trigsimp(fr2.expand()) == trigsimp(fr2_expected.expand()))
def test_bicycle(): if ON_TRAVIS: skip("Too slow for travis.") # Code to get equations of motion for a bicycle modeled as in: # J.P Meijaard, Jim M Papadopoulos, Andy Ruina and A.L Schwab. Linearized # dynamics equations for the balance and steer of a bicycle: a benchmark # and review. Proceedings of The Royal Society (2007) 463, 1955-1982 # doi: 10.1098/rspa.2007.1857 # Note that this code has been crudely ported from Autolev, which is the # reason for some of the unusual naming conventions. It was purposefully as # similar as possible in order to aide debugging. # Declare Coordinates & Speeds # Simple definitions for qdots - qd = u # Speeds are: yaw frame ang. rate, roll frame ang. rate, rear wheel frame # ang. rate (spinning motion), frame ang. rate (pitching motion), steering # frame ang. rate, and front wheel ang. rate (spinning motion). # Wheel positions are ignorable coordinates, so they are not introduced. q1, q2, q4, q5 = dynamicsymbols('q1 q2 q4 q5') q1d, q2d, q4d, q5d = dynamicsymbols('q1 q2 q4 q5', 1) u1, u2, u3, u4, u5, u6 = dynamicsymbols('u1 u2 u3 u4 u5 u6') u1d, u2d, u3d, u4d, u5d, u6d = dynamicsymbols('u1 u2 u3 u4 u5 u6', 1) # Declare System's Parameters WFrad, WRrad, htangle, forkoffset = symbols('WFrad WRrad htangle forkoffset') forklength, framelength, forkcg1 = symbols('forklength framelength forkcg1') forkcg3, framecg1, framecg3, Iwr11 = symbols('forkcg3 framecg1 framecg3 Iwr11') Iwr22, Iwf11, Iwf22, Iframe11 = symbols('Iwr22 Iwf11 Iwf22 Iframe11') Iframe22, Iframe33, Iframe31, Ifork11 = symbols('Iframe22 Iframe33 Iframe31 Ifork11') Ifork22, Ifork33, Ifork31, g = symbols('Ifork22 Ifork33 Ifork31 g') mframe, mfork, mwf, mwr = symbols('mframe mfork mwf mwr') # Set up reference frames for the system # N - inertial # Y - yaw # R - roll # WR - rear wheel, rotation angle is ignorable coordinate so not oriented # Frame - bicycle frame # TempFrame - statically rotated frame for easier reference inertia definition # Fork - bicycle fork # TempFork - statically rotated frame for easier reference inertia definition # WF - front wheel, again posses a ignorable coordinate N = ReferenceFrame('N') Y = N.orientnew('Y', 'Axis', [q1, N.z]) R = Y.orientnew('R', 'Axis', [q2, Y.x]) Frame = R.orientnew('Frame', 'Axis', [q4 + htangle, R.y]) WR = ReferenceFrame('WR') TempFrame = Frame.orientnew('TempFrame', 'Axis', [-htangle, Frame.y]) Fork = Frame.orientnew('Fork', 'Axis', [q5, Frame.x]) TempFork = Fork.orientnew('TempFork', 'Axis', [-htangle, Fork.y]) WF = ReferenceFrame('WF') # Kinematics of the Bicycle First block of code is forming the positions of # the relevant points # rear wheel contact -> rear wheel mass center -> frame mass center + # frame/fork connection -> fork mass center + front wheel mass center -> # front wheel contact point WR_cont = Point('WR_cont') WR_mc = WR_cont.locatenew('WR_mc', WRrad * R.z) Steer = WR_mc.locatenew('Steer', framelength * Frame.z) Frame_mc = WR_mc.locatenew('Frame_mc', - framecg1 * Frame.x + framecg3 * Frame.z) Fork_mc = Steer.locatenew('Fork_mc', - forkcg1 * Fork.x + forkcg3 * Fork.z) WF_mc = Steer.locatenew('WF_mc', forklength * Fork.x + forkoffset * Fork.z) WF_cont = WF_mc.locatenew('WF_cont', WFrad * (dot(Fork.y, Y.z) * Fork.y - Y.z).normalize()) # Set the angular velocity of each frame. # Angular accelerations end up being calculated automatically by # differentiating the angular velocities when first needed. # u1 is yaw rate # u2 is roll rate # u3 is rear wheel rate # u4 is frame pitch rate # u5 is fork steer rate # u6 is front wheel rate Y.set_ang_vel(N, u1 * Y.z) R.set_ang_vel(Y, u2 * R.x) WR.set_ang_vel(Frame, u3 * Frame.y) Frame.set_ang_vel(R, u4 * Frame.y) Fork.set_ang_vel(Frame, u5 * Fork.x) WF.set_ang_vel(Fork, u6 * Fork.y) # Form the velocities of the previously defined points, using the 2 - point # theorem (written out by hand here). Accelerations again are calculated # automatically when first needed. WR_cont.set_vel(N, 0) WR_mc.v2pt_theory(WR_cont, N, WR) Steer.v2pt_theory(WR_mc, N, Frame) Frame_mc.v2pt_theory(WR_mc, N, Frame) Fork_mc.v2pt_theory(Steer, N, Fork) WF_mc.v2pt_theory(Steer, N, Fork) WF_cont.v2pt_theory(WF_mc, N, WF) # Sets the inertias of each body. Uses the inertia frame to construct the # inertia dyadics. Wheel inertias are only defined by principle moments of # inertia, and are in fact constant in the frame and fork reference frames; # it is for this reason that the orientations of the wheels does not need # to be defined. The frame and fork inertias are defined in the 'Temp' # frames which are fixed to the appropriate body frames; this is to allow # easier input of the reference values of the benchmark paper. Note that # due to slightly different orientations, the products of inertia need to # have their signs flipped; this is done later when entering the numerical # value. Frame_I = (inertia(TempFrame, Iframe11, Iframe22, Iframe33, 0, 0, Iframe31), Frame_mc) Fork_I = (inertia(TempFork, Ifork11, Ifork22, Ifork33, 0, 0, Ifork31), Fork_mc) WR_I = (inertia(Frame, Iwr11, Iwr22, Iwr11), WR_mc) WF_I = (inertia(Fork, Iwf11, Iwf22, Iwf11), WF_mc) # Declaration of the RigidBody containers. :: BodyFrame = RigidBody('BodyFrame', Frame_mc, Frame, mframe, Frame_I) BodyFork = RigidBody('BodyFork', Fork_mc, Fork, mfork, Fork_I) BodyWR = RigidBody('BodyWR', WR_mc, WR, mwr, WR_I) BodyWF = RigidBody('BodyWF', WF_mc, WF, mwf, WF_I) # The kinematic differential equations; they are defined quite simply. Each # entry in this list is equal to zero. kd = [q1d - u1, q2d - u2, q4d - u4, q5d - u5] # The nonholonomic constraints are the velocity of the front wheel contact # point dotted into the X, Y, and Z directions; the yaw frame is used as it # is "closer" to the front wheel (1 less DCM connecting them). These # constraints force the velocity of the front wheel contact point to be 0 # in the inertial frame; the X and Y direction constraints enforce a # "no-slip" condition, and the Z direction constraint forces the front # wheel contact point to not move away from the ground frame, essentially # replicating the holonomic constraint which does not allow the frame pitch # to change in an invalid fashion. conlist_speed = [WF_cont.vel(N) & Y.x, WF_cont.vel(N) & Y.y, WF_cont.vel(N) & Y.z] # The holonomic constraint is that the position from the rear wheel contact # point to the front wheel contact point when dotted into the # normal-to-ground plane direction must be zero; effectively that the front # and rear wheel contact points are always touching the ground plane. This # is actually not part of the dynamic equations, but instead is necessary # for the lineraization process. conlist_coord = [WF_cont.pos_from(WR_cont) & Y.z] # The force list; each body has the appropriate gravitational force applied # at its mass center. FL = [(Frame_mc, -mframe * g * Y.z), (Fork_mc, -mfork * g * Y.z), (WF_mc, -mwf * g * Y.z), (WR_mc, -mwr * g * Y.z)] BL = [BodyFrame, BodyFork, BodyWR, BodyWF] # The N frame is the inertial frame, coordinates are supplied in the order # of independent, dependent coordinates, as are the speeds. The kinematic # differential equation are also entered here. Here the dependent speeds # are specified, in the same order they were provided in earlier, along # with the non-holonomic constraints. The dependent coordinate is also # provided, with the holonomic constraint. Again, this is only provided # for the linearization process. KM = KanesMethod(N, q_ind=[q1, q2, q5], q_dependent=[q4], configuration_constraints=conlist_coord, u_ind=[u2, u3, u5], u_dependent=[u1, u4, u6], velocity_constraints=conlist_speed, kd_eqs=kd) (fr, frstar) = KM.kanes_equations(FL, BL) # This is the start of entering in the numerical values from the benchmark # paper to validate the eigen values of the linearized equations from this # model to the reference eigen values. Look at the aforementioned paper for # more information. Some of these are intermediate values, used to # transform values from the paper into the coordinate systems used in this # model. PaperRadRear = 0.3 PaperRadFront = 0.35 HTA = evalf.N(pi / 2 - pi / 10) TrailPaper = 0.08 rake = evalf.N(-(TrailPaper*sin(HTA)-(PaperRadFront*cos(HTA)))) PaperWb = 1.02 PaperFrameCgX = 0.3 PaperFrameCgZ = 0.9 PaperForkCgX = 0.9 PaperForkCgZ = 0.7 FrameLength = evalf.N(PaperWb*sin(HTA)-(rake-(PaperRadFront-PaperRadRear)*cos(HTA))) FrameCGNorm = evalf.N((PaperFrameCgZ - PaperRadRear-(PaperFrameCgX/sin(HTA))*cos(HTA))*sin(HTA)) FrameCGPar = evalf.N((PaperFrameCgX / sin(HTA) + (PaperFrameCgZ - PaperRadRear - PaperFrameCgX / sin(HTA) * cos(HTA)) * cos(HTA))) tempa = evalf.N((PaperForkCgZ - PaperRadFront)) tempb = evalf.N((PaperWb-PaperForkCgX)) tempc = evalf.N(sqrt(tempa**2+tempb**2)) PaperForkL = evalf.N((PaperWb*cos(HTA)-(PaperRadFront-PaperRadRear)*sin(HTA))) ForkCGNorm = evalf.N(rake+(tempc * sin(pi/2-HTA-acos(tempa/tempc)))) ForkCGPar = evalf.N(tempc * cos((pi/2-HTA)-acos(tempa/tempc))-PaperForkL) # Here is the final assembly of the numerical values. The symbol 'v' is the # forward speed of the bicycle (a concept which only makes sense in the # upright, static equilibrium case?). These are in a dictionary which will # later be substituted in. Again the sign on the *product* of inertia # values is flipped here, due to different orientations of coordinate # systems. v = symbols('v') val_dict = {WFrad: PaperRadFront, WRrad: PaperRadRear, htangle: HTA, forkoffset: rake, forklength: PaperForkL, framelength: FrameLength, forkcg1: ForkCGPar, forkcg3: ForkCGNorm, framecg1: FrameCGNorm, framecg3: FrameCGPar, Iwr11: 0.0603, Iwr22: 0.12, Iwf11: 0.1405, Iwf22: 0.28, Ifork11: 0.05892, Ifork22: 0.06, Ifork33: 0.00708, Ifork31: 0.00756, Iframe11: 9.2, Iframe22: 11, Iframe33: 2.8, Iframe31: -2.4, mfork: 4, mframe: 85, mwf: 3, mwr: 2, g: 9.81, q1: 0, q2: 0, q4: 0, q5: 0, u1: 0, u2: 0, u3: v / PaperRadRear, u4: 0, u5: 0, u6: v / PaperRadFront} # Linearizes the forcing vector; the equations are set up as MM udot = # forcing, where MM is the mass matrix, udot is the vector representing the # time derivatives of the generalized speeds, and forcing is a vector which # contains both external forcing terms and internal forcing terms, such as # centripital or coriolis forces. This actually returns a matrix with as # many rows as *total* coordinates and speeds, but only as many columns as # independent coordinates and speeds. forcing_lin = KM.linearize()[0] # As mentioned above, the size of the linearized forcing terms is expanded # to include both q's and u's, so the mass matrix must have this done as # well. This will likely be changed to be part of the linearized process, # for future reference. MM_full = KM.mass_matrix_full MM_full_s = MM_full.subs(val_dict) forcing_lin_s = forcing_lin.subs(KM.kindiffdict()).subs(val_dict) MM_full_s = MM_full_s.evalf() forcing_lin_s = forcing_lin_s.evalf() # Finally, we construct an "A" matrix for the form xdot = A x (x being the # state vector, although in this case, the sizes are a little off). The # following line extracts only the minimum entries required for eigenvalue # analysis, which correspond to rows and columns for lean, steer, lean # rate, and steer rate. Amat = MM_full_s.inv() * forcing_lin_s A = Amat.extract([1, 2, 4, 6], [1, 2, 3, 5]) # Precomputed for comparison Res = Matrix([[ 0, 0, 1.0, 0], [ 0, 0, 0, 1.0], [9.48977444677355, -0.891197738059089*v**2 - 0.571523173729245, -0.105522449805691*v, -0.330515398992311*v], [11.7194768719633, -1.97171508499972*v**2 + 30.9087533932407, 3.67680523332152*v, -3.08486552743311*v]]) # Actual eigenvalue comparison eps = 1.e-12 for i in range(6): error = Res.subs(v, i) - A.subs(v, i) assert all(abs(x) < eps for x in error)
O.set_acc(A, 0) # velocity and acceleration # Define rigid body objects Enclosure = RigidBody('Enclosure', O, C, 0, (I_enclosure, O)) Flywheel = RigidBody('Flywheel', O, D, 0, (I_flywheel, O)) body_list = [Enclosure, Flywheel] # List of forces and torques tau = symbols('tau') # Gimbal torque scalar torque_force_list = [(O, m*g*A.z), # Gravitational force (B, -tau*B.y), # Gimbal reaction torque (C, tau*B.y)] # Gimbal torque # Form equations of motion KM = KanesMethod(A, q, u, kindiffs) fr, frstar = KM.kanes_equations(torque_force_list, body_list) # Simplifying assumptions simplifications = {IFxx: symbols('I'), # Flywheel transverse inertias IFyy: symbols('I'), # assumed to be equal IFzz: symbols('J'), Ixx: 0, Iyy: 0, Izz: 0} frstar = frstar.subs(simplifications) MM = -frstar.jacobian(ud) MM.simplify() forcing = fr + frstar.subs({ud[0]: 0, ud[1]:0, ud[2]:0}).expand() forcing.simplify()
class MultipendulumEnv(gym.Env): metadata = {'render.modes': ['human']} def __init__(self): self.num_links = 5 # Number of links total_link_length = 1. total_link_mass = 1. self.ind_link_length = total_link_length / self.num_links ind_link_com_length = self.ind_link_length / 2. ind_link_mass = total_link_mass / self.num_links ind_link_inertia = ind_link_mass * (ind_link_com_length**2) # =======================# # Parameters for step() # # =======================# # Maximum number of steps before episode termination self.max_steps = 200 # For ODE integration self.dt = .0001 # Simultaion time step = 1ms self.sim_steps = 51 # Number of simulation steps in 1 learning step self.dt_step = np.linspace( 0., self.dt * self.sim_steps, num=self.sim_steps) # Learning time step = 50ms # Termination conditions for simulation self.num_steps = 0 # Step counter self.done = False # For visualisation self.viewer = None self.ax = False # Constraints for observation min_angle = -np.pi # Angle max_angle = np.pi min_omega = -10. # Angular velocity max_omega = 10. min_torque = -10. # Torque max_torque = 10. low_state_angle = np.full(self.num_links, min_angle) # Min angle low_state_omega = np.full(self.num_links, min_omega) # Min angular velocity low_state = np.append(low_state_angle, low_state_omega) high_state_angle = np.full(self.num_links, max_angle) # Max angle high_state_omega = np.full(self.num_links, max_omega) # Max angular velocity high_state = np.append(high_state_angle, high_state_omega) low_action = np.full(self.num_links, min_torque) # Min torque high_action = np.full(self.num_links, max_torque) # Max torque self.action_space = spaces.Box(low=low_action, high=high_action) self.observation_space = spaces.Box(low=low_state, high=high_state) # Minimum reward self.min_reward = -(max_angle**2 + .1 * max_omega**2 + .001 * max_torque**2) * self.num_links # Seeding self.seed() # ==============# # Orientations # # ==============# self.inertial_frame = ReferenceFrame('I') self.link_frame = [] self.theta = [] for i in range(self.num_links): temp_angle_name = "theta{}".format(i + 1) temp_link_name = "L{}".format(i + 1) self.theta.append(dynamicsymbols(temp_angle_name)) self.link_frame.append(ReferenceFrame(temp_link_name)) if i == 0: # First link self.link_frame[i].orient( self.inertial_frame, 'Axis', (self.theta[i], self.inertial_frame.z)) else: # Second link, third link... self.link_frame[i].orient( self.link_frame[i - 1], 'Axis', (self.theta[i], self.link_frame[i - 1].z)) # =================# # Point Locations # # =================# # --------# # Joints # # --------# self.link_length = [] self.link_joint = [] for i in range(self.num_links): temp_link_length_name = "l_L{}".format(i + 1) temp_link_joint_name = "A{}".format(i) self.link_length.append(symbols(temp_link_length_name)) self.link_joint.append(Point(temp_link_joint_name)) if i > 0: # Set position started from link2, then link3, link4... self.link_joint[i].set_pos( self.link_joint[i - 1], self.link_length[i - 1] * self.link_frame[i - 1].y) # --------------------------# # Centre of mass locations # # --------------------------# self.link_com_length = [] self.link_mass_centre = [] for i in range(self.num_links): temp_link_com_length_name = "d_L{}".format(i + 1) temp_link_mass_centre_name = "L{}_o".format(i + 1) self.link_com_length.append(symbols(temp_link_com_length_name)) self.link_mass_centre.append(Point(temp_link_mass_centre_name)) self.link_mass_centre[i].set_pos( self.link_joint[i], self.link_com_length[i] * self.link_frame[i].y) # ===========================================# # Define kinematical differential equations # # ===========================================# self.omega = [] self.kinematical_differential_equations = [] self.time = symbols('t') for i in range(self.num_links): temp_omega_name = "omega{}".format(i + 1) self.omega.append(dynamicsymbols(temp_omega_name)) self.kinematical_differential_equations.append( self.omega[i] - self.theta[i].diff(self.time)) # ====================# # Angular Velocities # # ====================# for i in range(self.num_links): if i == 0: # First link self.link_frame[i].set_ang_vel( self.inertial_frame, self.omega[i] * self.inertial_frame.z) else: # Second link, third link... self.link_frame[i].set_ang_vel( self.link_frame[i - 1], self.omega[i] * self.link_frame[i - 1].z) # ===================# # Linear Velocities # # ===================# for i in range(self.num_links): if i == 0: # First link self.link_joint[i].set_vel(self.inertial_frame, 0) else: # Second link, third link... self.link_joint[i].v2pt_theory(self.link_joint[i - 1], self.inertial_frame, self.link_frame[i - 1]) self.link_mass_centre[i].v2pt_theory(self.link_joint[i], self.inertial_frame, self.link_frame[i]) # ======# # Mass # # ======# self.link_mass = [] for i in range(self.num_links): temp_link_mass_name = "m_L{}".format(i + 1) self.link_mass.append(symbols(temp_link_mass_name)) # =========# # Inertia # # =========# self.link_inertia = [] self.link_inertia_dyadic = [] self.link_central_inertia = [] for i in range(self.num_links): temp_link_inertia_name = "I_L{}z".format(i + 1) self.link_inertia.append(symbols(temp_link_inertia_name)) self.link_inertia_dyadic.append( inertia(self.link_frame[i], 0, 0, self.link_inertia[i])) self.link_central_inertia.append( (self.link_inertia_dyadic[i], self.link_mass_centre[i])) # ==============# # Rigid Bodies # # ==============# self.link = [] for i in range(self.num_links): temp_link_name = "link{}".format(i + 1) self.link.append( RigidBody(temp_link_name, self.link_mass_centre[i], self.link_frame[i], self.link_mass[i], self.link_central_inertia[i])) # =========# # Gravity # # =========# self.g = symbols('g') self.link_grav_force = [] for i in range(self.num_links): self.link_grav_force.append( (self.link_mass_centre[i], -self.link_mass[i] * self.g * self.inertial_frame.y)) # ===============# # Joint Torques # # ===============# self.link_joint_torque = [] self.link_torque = [] for i in range(self.num_links): temp_link_joint_torque_name = "T_a{}".format(i + 1) self.link_joint_torque.append( dynamicsymbols(temp_link_joint_torque_name)) for i in range(self.num_links): if (i + 1) == self.num_links: # Last link self.link_torque.append( (self.link_frame[i], self.link_joint_torque[i] * self.inertial_frame.z)) else: # Other links self.link_torque.append( (self.link_frame[i], self.link_joint_torque[i] * self.inertial_frame.z - self.link_joint_torque[i + 1] * self.inertial_frame.z)) # =====================# # Equations of Motion # # =====================# self.coordinates = [] self.speeds = [] self.loads = [] self.bodies = [] for i in range(self.num_links): self.coordinates.append(self.theta[i]) self.speeds.append(self.omega[i]) self.loads.append(self.link_grav_force[i]) self.loads.append(self.link_torque[i]) self.bodies.append(self.link[i]) self.kane = KanesMethod(self.inertial_frame, self.coordinates, self.speeds, self.kinematical_differential_equations) self.fr, self.frstar = self.kane.kanes_equations( self.bodies, self.loads) self.mass_matrix = self.kane.mass_matrix_full self.forcing_vector = self.kane.forcing_full # =============================# # List the symbolic arguments # # =============================# # -----------# # Constants # # -----------# self.constants = [] for i in range(self.num_links): if (i + 1) != self.num_links: self.constants.append(self.link_length[i]) self.constants.append(self.link_com_length[i]) self.constants.append(self.link_mass[i]) self.constants.append(self.link_inertia[i]) self.constants.append(self.g) # --------------# # Time Varying # # --------------# self.coordinates = [] self.speeds = [] self.specified = [] for i in range(self.num_links): self.coordinates.append(self.theta[i]) self.speeds.append(self.omega[i]) self.specified.append(self.link_joint_torque[i]) # =======================# # Generate RHS Function # # =======================# self.right_hand_side = generate_ode_function( self.forcing_vector, self.coordinates, self.speeds, self.constants, mass_matrix=self.mass_matrix, specifieds=self.specified) # ==============================# # Specify Numerical Quantities # # ==============================# self.x = np.zeros(self.num_links * 2) self.x[:self.num_links] = deg2rad(2.0) self.numerical_constants = [] for i in range(self.num_links): if (i + 1) != self.num_links: self.numerical_constants.append(self.ind_link_length) self.numerical_constants.append(ind_link_com_length) self.numerical_constants.append(ind_link_mass) self.numerical_constants.append(ind_link_inertia) self.numerical_constants.append(9.81) def seed(self, seed=None): self.np_random, seed = seeding.np_random(seed) return [seed] def reset(self): self.num_steps = 0 # Step counter self.done = False # Done flag self.x = np.random.randn(self.num_links * 2) # State return self._get_obs() def _get_obs(self): return self.x def sample_action(self): return np.random.randn(self.num_links) def step(self, action): if self.done == True or self.num_steps > self.max_steps: self.done = True # Normalised reward reward = 0. return self.x, reward, self.done, {} else: # Increment the step counter self.num_steps += 1 # Simulation self.x = odeint(self.right_hand_side, self.x, self.dt_step, args=(action, self.numerical_constants))[-1] # Normalise joint angles to -pi ~ pi self.x[:self.num_links] = self.angle_normalise( self.x[:self.num_links]) # n-link case reward = 0. # Cost due to angle and torque for i in range(self.num_links): reward -= (self.x[i]**2 + .001 * action[i]**2) # Cost due to angular velocity for i in range(self.num_links, self.num_links * 2): reward -= (.1 * self.x[i]**2) # Normalised reward reward = (reward - self.min_reward) / (-self.min_reward) return self.x, reward, self.done, {} def angle_normalise(self, angle_input): return (((angle_input + np.pi) % (2 * np.pi)) - np.pi) def render(self, mode='human'): if not self.ax: fig, ax = plt.subplots() ax.set_xlim([-1.5, 1.5]) ax.set_ylim([-1.5, 1.5]) ax.set_aspect('equal') self.ax = ax else: self.ax.clear() self.ax.set_xlim([-1.5, 1.5]) self.ax.set_ylim([-1.5, 1.5]) self.ax.set_aspect('equal') x = [0.] y = [0.] for i in range(self.num_links): x.append(x[i] + self.ind_link_length * np.cos(self.x[i] + np.pi / 2.)) y.append(y[i] + self.ind_link_length * np.sin(self.x[i] + np.pi / 2.)) plt.plot(x, y) plt.pause(0.01)
def test_input_format(): # 1 dof problem from test_one_dof q, u = dynamicsymbols('q u') qd, ud = dynamicsymbols('q u', 1) m, c, k = symbols('m c k') N = ReferenceFrame('N') P = Point('P') P.set_vel(N, u * N.x) kd = [qd - u] FL = [(P, (-k * q - c * u) * N.x)] pa = Particle('pa', P, m) BL = [pa] KM = KanesMethod(N, [q], [u], kd) # test for input format kane.kanes_equations((body1, body2, particle1)) assert KM.kanes_equations(BL)[0] == Matrix([0]) # test for input format kane.kanes_equations(bodies=(body1, body 2), loads=(load1,load2)) assert KM.kanes_equations(bodies=BL, loads=None)[0] == Matrix([0]) # test for input format kane.kanes_equations(bodies=(body1, body 2), loads=None) assert KM.kanes_equations(BL, loads=None)[0] == Matrix([0]) # test for input format kane.kanes_equations(bodies=(body1, body 2)) assert KM.kanes_equations(BL)[0] == Matrix([0]) # test for error raised when a wrong force list (in this case a string) is provided from sympy.testing.pytest import raises raises(ValueError, lambda: KM._form_fr('bad input')) # 2 dof problem from test_two_dof q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2') q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1) m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2') N = ReferenceFrame('N') P1 = Point('P1') P2 = Point('P2') P1.set_vel(N, u1 * N.x) P2.set_vel(N, (u1 + u2) * N.x) kd = [q1d - u1, q2d - u2] FL = ((P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 * q2 - c2 * u2) * N.x)) pa1 = Particle('pa1', P1, m) pa2 = Particle('pa2', P2, m) BL = (pa1, pa2) KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd) # test for input format # kane.kanes_equations((body1, body2), (load1, load2)) KM.kanes_equations(BL, FL) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing assert expand(rhs[0]) == expand( (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) / m) assert expand(rhs[1]) == expand( (k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 * c2 * u2) / m)
def test_linearize_rolling_disc_kane(): # Symbols for time and constant parameters t, r, m, g, v = symbols('t r m g v') # Configuration variables and their time derivatives q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7') q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q] # Generalized speeds and their time derivatives u = dynamicsymbols('u:6') u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7') u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u] # Reference frames N = ReferenceFrame('N') # Inertial frame NO = Point('NO') # Inertial origin A = N.orientnew('A', 'Axis', [q1, N.z]) # Yaw intermediate frame B = A.orientnew('B', 'Axis', [q2, A.x]) # Lean intermediate frame C = B.orientnew('C', 'Axis', [q3, B.y]) # Disc fixed frame CO = NO.locatenew('CO', q4*N.x + q5*N.y + q6*N.z) # Disc center # Disc angular velocity in N expressed using time derivatives of coordinates w_c_n_qd = C.ang_vel_in(N) w_b_n_qd = B.ang_vel_in(N) # Inertial angular velocity and angular acceleration of disc fixed frame C.set_ang_vel(N, u1*B.x + u2*B.y + u3*B.z) # Disc center velocity in N expressed using time derivatives of coordinates v_co_n_qd = CO.pos_from(NO).dt(N) # Disc center velocity in N expressed using generalized speeds CO.set_vel(N, u4*C.x + u5*C.y + u6*C.z) # Disc Ground Contact Point P = CO.locatenew('P', r*B.z) P.v2pt_theory(CO, N, C) # Configuration constraint f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)]) # Velocity level constraints f_v = Matrix([dot(P.vel(N), uv) for uv in C]) # Kinematic differential equations kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] + [dot(v_co_n_qd - CO.vel(N), uv) for uv in N]) qdots = solve(kindiffs, qd) # Set angular velocity of remaining frames B.set_ang_vel(N, w_b_n_qd.subs(qdots)) C.set_ang_acc(N, C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N))) # Active forces F_CO = m*g*A.z # Create inertia dyadic of disc C about point CO I = (m * r**2) / 4 J = (m * r**2) / 2 I_C_CO = inertia(C, I, J, I) Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO)) BL = [Disc] FL = [(CO, F_CO)] KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3], kd_eqs=kindiffs, q_dependent=[q6], configuration_constraints=f_c, u_dependent=[u4, u5, u6], velocity_constraints=f_v) (fr, fr_star) = KM.kanes_equations(FL, BL) # Test generalized form equations linearizer = KM.to_linearizer() assert linearizer.f_c == f_c assert linearizer.f_v == f_v assert linearizer.f_a == f_v.diff(t) sol = solve(linearizer.f_0 + linearizer.f_1, qd) for qi in qd: assert sol[qi] == qdots[qi] assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix([0, 0, 0]) # Perform the linearization # Precomputed operating point q_op = {q6: -r*cos(q2)} u_op = {u1: 0, u2: sin(q2)*q1d + q3d, u3: cos(q2)*q1d, u4: -r*(sin(q2)*q1d + q3d)*cos(q3), u5: 0, u6: -r*(sin(q2)*q1d + q3d)*sin(q3)} qd_op = {q2d: 0, q4d: -r*(sin(q2)*q1d + q3d)*cos(q1), q5d: -r*(sin(q2)*q1d + q3d)*sin(q1), q6d: 0} ud_op = {u1d: 4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5, u2d: 0, u3d: 0, u4d: r*(sin(q2)*sin(q3)*q1d*q3d + sin(q3)*q3d**2), u5d: r*(4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5), u6d: -r*(sin(q2)*cos(q3)*q1d*q3d + cos(q3)*q3d**2)} A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op], A_and_B=True, simplify=True) upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1} # Precomputed solution A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0], [sin(q1)*q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0], [-cos(q1)*q3d, 0, 0, 0, 0, cos(q1), -sin(q1), 0], [0, 4/5, 0, 0, 0, 0, 0, 6*q3d/5], [0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, -2*q3d, 0, 0]]) B_sol = Matrix([]) # Check that linearization is correct assert A.subs(upright_nominal) == A_sol assert B.subs(upright_nominal) == B_sol # Check eigenvalues at critical speed are all zero: assert A.subs(upright_nominal).subs(q3d, 1/sqrt(3)).eigenvals() == {0: 8}
def test_linearize_pendulum_kane_nonminimal(): # Create generalized coordinates and speeds for this non-minimal realization # q1, q2 = N.x and N.y coordinates of pendulum # u1, u2 = N.x and N.y velocities of pendulum q1, q2 = dynamicsymbols('q1:3') q1d, q2d = dynamicsymbols('q1:3', level=1) u1, u2 = dynamicsymbols('u1:3') u1d, u2d = dynamicsymbols('u1:3', level=1) L, m, t = symbols('L, m, t') g = 9.8 # Compose world frame N = ReferenceFrame('N') pN = Point('N*') pN.set_vel(N, 0) # A.x is along the pendulum theta1 = atan(q2/q1) A = N.orientnew('A', 'axis', [theta1, N.z]) # Locate the pendulum mass P = pN.locatenew('P1', q1*N.x + q2*N.y) pP = Particle('pP', P, m) # Calculate the kinematic differential equations kde = Matrix([q1d - u1, q2d - u2]) dq_dict = solve(kde, [q1d, q2d]) # Set velocity of point P P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict)) # Configuration constraint is length of pendulum f_c = Matrix([P.pos_from(pN).magnitude() - L]) # Velocity constraint is that the velocity in the A.x direction is # always zero (the pendulum is never getting longer). f_v = Matrix([P.vel(N).express(A).dot(A.x)]) f_v.simplify() # Acceleration constraints is the time derivative of the velocity constraint f_a = f_v.diff(t) f_a.simplify() # Input the force resultant at P R = m*g*N.x # Derive the equations of motion using the KanesMethod class. KM = KanesMethod(N, q_ind=[q2], u_ind=[u2], q_dependent=[q1], u_dependent=[u1], configuration_constraints=f_c, velocity_constraints=f_v, acceleration_constraints=f_a, kd_eqs=kde) (fr, frstar) = KM.kanes_equations([(P, R)], [pP]) # Set the operating point to be straight down, and non-moving q_op = {q1: L, q2: 0} u_op = {u1: 0, u2: 0} ud_op = {u1d: 0, u2d: 0} A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True, new_method=True, simplify=True) assert A == Matrix([[0, 1], [-9.8/L, 0]]) assert B == Matrix([])
def test_sub_qdot(): # This test solves exercises 8.12, 8.17 from Kane 1985 and defines # some velocities in terms of q, qdot. ## --- Declare symbols --- q1, q2, q3 = dynamicsymbols('q1:4') q1d, q2d, q3d = dynamicsymbols('q1:4', level=1) u1, u2, u3 = dynamicsymbols('u1:4') u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta') a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t') IA22, IA23, IA33 = symbols('IA22 IA23 IA33') Q1, Q2, Q3 = symbols('Q1 Q2 Q3') # --- Reference Frames --- F = ReferenceFrame('F') P = F.orientnew('P', 'axis', [-theta, F.y]) A = P.orientnew('A', 'axis', [q1, P.x]) A.set_ang_vel(F, u1 * A.x + u3 * A.z) # define frames for wheels B = A.orientnew('B', 'axis', [q2, A.z]) C = A.orientnew('C', 'axis', [q3, A.z]) ## --- define points D, S*, Q on frame A and their velocities --- pD = Point('D') pD.set_vel(A, 0) # u3 will not change v_D_F since wheels are still assumed to roll w/o slip pD.set_vel(F, u2 * A.y) pS_star = pD.locatenew('S*', e * A.y) pQ = pD.locatenew('Q', f * A.y - R * A.x) # masscenters of bodies A, B, C pA_star = pD.locatenew('A*', a * A.y) pB_star = pD.locatenew('B*', b * A.z) pC_star = pD.locatenew('C*', -b * A.z) for p in [pS_star, pQ, pA_star, pB_star, pC_star]: p.v2pt_theory(pD, F, A) # points of B, C touching the plane P pB_hat = pB_star.locatenew('B^', -R * A.x) pC_hat = pC_star.locatenew('C^', -R * A.x) pB_hat.v2pt_theory(pB_star, F, B) pC_hat.v2pt_theory(pC_star, F, C) # --- relate qdot, u --- # the velocities of B^, C^ are zero since B, C are assumed to roll w/o slip kde = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] kde += [u1 - q1d] kde_map = solve(kde, [q1d, q2d, q3d]) for k, v in list(kde_map.items()): kde_map[k.diff(t)] = v.diff(t) # inertias of bodies A, B, C # IA22, IA23, IA33 are not specified in the problem statement, but are # necessary to define an inertia object. Although the values of # IA22, IA23, IA33 are not known in terms of the variables given in the # problem statement, they do not appear in the general inertia terms. inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0) inertia_B = inertia(B, K, K, J) inertia_C = inertia(C, K, K, J) # define the rigid bodies A, B, C rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star)) rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star)) rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star)) ## --- use kanes method --- km = KanesMethod(F, [q1, q2, q3], [u1, u2], kd_eqs=kde, u_auxiliary=[u3]) forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)] bodies = [rbA, rbB, rbC] # Q2 = -u_prime * u2 * Q1 / sqrt(u2**2 + f**2 * u1**2) # -u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2) = R / Q1 * Q2 fr_expected = Matrix([ f * Q3 + M * g * e * sin(theta) * cos(q1), Q2 + M * g * sin(theta) * sin(q1), e * M * g * cos(theta) - Q1 * f - Q2 * R ]) #Q1 * (f - u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2)))]) fr_star_expected = Matrix([ -(IA + 2 * J * b**2 / R**2 + 2 * K + mA * a**2 + 2 * mB * b**2) * u1.diff(t) - mA * a * u1 * u2, -(mA + 2 * mB + 2 * J / R**2) * u2.diff(t) + mA * a * u1**2, 0 ]) with warns_deprecated_sympy(): fr, fr_star = km.kanes_equations(forces, bodies) assert (fr.expand() == fr_expected.expand()) assert ((fr_star_expected - trigsimp(fr_star)).expand() == zeros(3, 1))
def test_one_dof(): # This is for a 1 dof spring-mass-damper case. # It is described in more detail in the KanesMethod docstring. q, u = dynamicsymbols('q u') qd, ud = dynamicsymbols('q u', 1) m, c, k = symbols('m c k') N = ReferenceFrame('N') P = Point('P') P.set_vel(N, u * N.x) kd = [qd - u] FL = [(P, (-k * q - c * u) * N.x)] pa = Particle('pa', P, m) BL = [pa] KM = KanesMethod(N, [q], [u], kd) KM.kanes_equations(FL, BL) MM = KM.mass_matrix forcing = KM.forcing rhs = MM.inv() * forcing assert expand(rhs[0]) == expand(-(q * k + u * c) / m) assert (KM.linearize(A_and_B=True, new_method=True)[0] == Matrix([[0, 1], [-k/m, -c/m]])) # Ensure that the old linearizer still works and that the new linearizer # gives the same results. The old linearizer is deprecated and should be # removed in >= 0.7.7. M_old = KM.mass_matrix_full # The old linearizer raises a deprecation warning, so catch it here so # it doesn't cause py.test to fail. with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) F_A_old, F_B_old, r_old = KM.linearize() M_new, F_A_new, F_B_new, r_new = KM.linearize(new_method=True) assert simplify(M_new.inv() * F_A_new - M_old.inv() * F_A_old) == zeros(2)