def _create_points(self): self.points = OrderedDict() self.points['origin'] = me.Point('O') self.points['ankle'] = me.Point('A') self.points['hip'] = me.Point('H') self.points['leg_mass_center'] = me.Point('L_o') self.points['torso_mass_center'] = me.Point('T_o')
def _create_points(self): self.points = OrderedDict() self.points['origin'] = me.Point('O') self.points['hip'] = me.Point('H') self.points['lknee'] = me.Point('LK') self.points['lankle'] = me.Point('LA') self.points['rknee'] = me.Point('RK') self.points['rankle'] = me.Point('RA') self.points['lthigh_mass_center'] = me.Point('LT_o') self.points['lshank_mass_center'] = me.Point('LS_o') self.points['rthigh_mass_center'] = me.Point('RT_o') self.points['rshank_mass_center'] = me.Point('RS_o')
def test_create_static_html(): # derive simple system mass, stiffness, damping, gravity = symbols('m, k, c, g') position, speed = me.dynamicsymbols('x v') positiond = me.dynamicsymbols('x', 1) ceiling = me.ReferenceFrame('N') origin = me.Point('origin') origin.set_vel(ceiling, 0) center = origin.locatenew('center', position * ceiling.x) center.set_vel(ceiling, speed * ceiling.x) block = me.Particle('block', center, mass) kinematic_equations = [speed - positiond] total_force = mass * gravity - stiffness * position - damping * speed forces = [(center, total_force * ceiling.x)] particles = [block] kane = me.KanesMethod(ceiling, q_ind=[position], u_ind=[speed], kd_eqs=kinematic_equations) kane.kanes_equations(forces, particles) sys = System(kane, initial_conditions={ position: 0.1, speed: -1.0 }, constants={ mass: 1.0, stiffness: 1.0, damping: 0.2, gravity: 9.8 }) # integrate eoms t = linspace(0.0, 10.0, 100) sys.times = t y = sys.integrate() # create visualization sphere = Sphere() viz_frame = VisualizationFrame(ceiling, block, sphere) scene = Scene(ceiling, origin, viz_frame) scene.generate_visualization_json_system(sys, outfile_prefix="test") # test static dir creation scene.create_static_html(overwrite=True) assert os.path.exists('static') assert os.path.exists('static/index.html') assert os.path.exists('static/test_scene_desc.json') assert os.path.exists('static/test_simulation_data.json') # test static dir deletion scene.remove_static_html(force=True) assert not os.path.exists('static')
def test_specifying_coordinate_issue_339(): """This test ensures that you can use derivatives as specified values.""" # beta will be a specified angle beta = me.dynamicsymbols('beta') q1, q2, q3, q4 = me.dynamicsymbols('q1, q2, q3, q4') u1, u2, u3, u4 = me.dynamicsymbols('u1, u2, u3, u4') N = me.ReferenceFrame('N') A = N.orientnew('A', 'Axis', (q1, N.x)) B = A.orientnew('B', 'Axis', (beta, A.y)) No = me.Point('No') Ao = No.locatenew('Ao', q2 * N.x + q3 * N.y + q4 * N.z) Bo = Ao.locatenew('Bo', 10 * A.x + 10 * A.y + 10 * A.z) A.set_ang_vel(N, u1 * N.x) B.ang_vel_in(N) # compute it automatically No.set_vel(N, 0) Ao.set_vel(N, u2 * N.x + u3 * N.y + u4 * N.z) Bo.v2pt_theory(Ao, N, B) body_A = me.RigidBody('A', Ao, A, 1.0, (me.inertia(A, 1, 2, 3), Ao)) body_B = me.RigidBody('B', Bo, B, 1.0, (me.inertia(A, 3, 2, 1), Bo)) bodies = [body_A, body_B] # TODO : This should be able to be simple an empty iterable. loads = [(No, 0 * N.x)] kdes = [u1 - q1.diff(), u2 - q2.diff(), u3 - q3.diff(), u4 - q4.diff()] kane = me.KanesMethod(N, q_ind=[q1, q2, q3, q4], u_ind=[u1, u2, u3, u4], kd_eqs=kdes) if sympy_newer_than('1.0'): fr, frstar = kane.kanes_equations(bodies, loads) else: fr, frstar = kane.kanes_equations(loads, bodies) sys = System(kane) sys.specifieds = { (beta, beta.diff(), beta.diff().diff()): lambda x, t: np.array([1.0, 1.0, 1.0]) } sys.times = np.linspace(0, 10, 20) sys.integrate()
def setup(self): """Setups a simple 1 DoF mass spring damper system visualization.""" mass, stiffness, damping, gravity = symbols('m, k, c, g') position, speed = me.dynamicsymbols('x v') positiond = me.dynamicsymbols('x', 1) kinematic_equations = [speed - positiond] ceiling = me.ReferenceFrame('N') origin = me.Point('origin') origin.set_vel(ceiling, 0) center = origin.locatenew('center', position * ceiling.x) center.set_vel(ceiling, speed * ceiling.x) block = me.Particle('block', center, mass) particles = [block] total_force = mass * gravity - stiffness * position - damping * speed forces = [(center, total_force * ceiling.x)] kane = me.KanesMethod(ceiling, q_ind=[position], u_ind=[speed], kd_eqs=kinematic_equations) if sympy_newer_than('1.0'): kane.kanes_equations(particles, forces) else: kane.kanes_equations(forces, particles) self.sys = System(kane) self.sys.initial_conditions = {position: 0.1, speed: -1.0} self.sys.constants = { mass: 1.0, stiffness: 2.0, damping: 3.0, gravity: 9.8 } self.sys.times = np.linspace(0.0, 0.01, 2) sphere = Sphere() self.ref_frame = ceiling self.origin = origin self.viz_frame = VisualizationFrame(ceiling, block, sphere) self.viz_frame_sym_shape = VisualizationFrame( ceiling, block, Sphere(radius=mass / 10.0))
def test_create_static_html(): # derive simple system mass, stiffness, damping, gravity = symbols('m, k, c, g') position, speed = me.dynamicsymbols('x v') positiond = me.dynamicsymbols('x', 1) ceiling = me.ReferenceFrame('N') origin = me.Point('origin') origin.set_vel(ceiling, 0) center = origin.locatenew('center', position * ceiling.x) center.set_vel(ceiling, speed * ceiling.x) block = me.Particle('block', center, mass) kinematic_equations = [speed - positiond] total_force = mass * gravity - stiffness * position - damping * speed forces = [(center, total_force * ceiling.x)] particles = [block] kane = me.KanesMethod(ceiling, q_ind=[position], u_ind=[speed], kd_eqs=kinematic_equations) kane.kanes_equations(forces, particles) mass_matrix = kane.mass_matrix_full forcing_vector = kane.forcing_full constants = (mass, stiffness, damping, gravity) coordinates = (position, ) speeds = (speed, ) system = (mass_matrix, forcing_vector, constants, coordinates, speeds) # integrate eoms evaluate_ode = generate_ode_function(*system) x0 = array([0.1, -1.0]) args = {'constants': array([1.0, 1.0, 0.2, 9.8])} t = linspace(0.0, 10.0, 100) y = odeint(evaluate_ode, x0, t, args=(args, )) # create visualization sphere = Sphere() viz_frame = VisualizationFrame(ceiling, block, sphere) scene = Scene(ceiling, origin, viz_frame) scene.generate_visualization_json([position, speed], list(constants), y, args['constants']) # test static dir creation scene.create_static_html(overwrite=True) assert os.path.exists('static') assert os.path.exists('static/index.html') assert os.path.exists('static/data.json') # test static dir deletion scene.remove_static_html(force=True) assert not os.path.exists('static')
def biped_model_left(): # 定义状态变量 alpha, beta, gamma = me.dynamicsymbols('alp beta gama') # 与地面接触点状态 dalpha, dbeta, dgamma = me.dynamicsymbols('alp beta gama', 1) lj1, lj2, lj3 = me.dynamicsymbols('lj1 lj2 lj3') # 左腿状态变量 dlj1, dlj2, dlj3 = me.dynamicsymbols('lj1 lj2 lj3', 1) rj1, rj2, rj3 = me.dynamicsymbols('rj1, rj2 rj3') # 右腿状态变量 drj1, drj2, drj3 = me.dynamicsymbols('rj1, rj2 rj3', 1) # 定义常量 # 定义坐标系 ground = me.ReferenceFrame('ground') axis_1 = ground.orientnew('Axis1', 'Body', [alpha, beta, gamma], '123') axis_2 = axis_1.orientnew('Axis2', 'Axis', [lj2, axis_1.y]) # 定义点 ori = me.Point('ori')
def setup(self): # This is taken from the example in KanesMethod docstring # System state variables q, u = me.dynamicsymbols('q u') qd, ud = me.dynamicsymbols('q u', 1) # Other system variables m, c, k = symbols('m c k') # Set up the reference frame N = me.ReferenceFrame('N') # Set up the point and particle P = me.Point('P') P.set_vel(N, u * N.x) pa = me.Particle('pa', P, m) # Create the list of kinematic differential equations, force list and # list of bodies/particles kd = [qd - u] force_list = [(P, (-k * q - c * u) * N.x)] body_list = [pa] # Create an instance of KanesMethod self.KM = me.KanesMethod(N, q_ind=[q], u_ind=[u], kd_eqs=kd) # Account for the new method of input to kanes_equations, i.e. the # order of the args in the old API is forces, bodies and the new API # is bodies, forces. try: self.KM.kanes_equations(body_list, force_list) self.first_input = body_list self.second_input = force_list except TypeError: self.first_input = force_list self.second_input = body_list
def __init__(self, n, lengths=None, masses=1): #------------------------------------------------- # Step 1: construct the pendulum model # Generalized coordinates and velocities # (in this case, angular positions & velocities of each mass) q = mechanics.dynamicsymbols('q:{0}'.format(n)) u = mechanics.dynamicsymbols('u:{0}'.format(n)) f = mechanics.dynamicsymbols('f:{0}'.format(n)) # mass and length m = symbols('m:{0}'.format(n)) l = symbols('l:{0}'.format(n)) # gravity and time symbols g, t = symbols('g,t') #-------------------------------------------------- # Step 2: build the model using Kane's Method # Create pivot point reference frame A = mechanics.ReferenceFrame('A') P = mechanics.Point('P') P.set_vel(A, 0) # lists to hold particles, forces, and kinetic ODEs # for each pendulum in the chain particles = [] forces = [] kinetic_odes = [] for i in range(n): # Create a reference frame following the i^th mass Ai = A.orientnew('A' + str(i), 'Axis', [q[i], A.z]) Ai.set_ang_vel(A, u[i] * A.z) # Create a point in this reference frame Pi = P.locatenew('P' + str(i), l[i] * Ai.x) Pi.v2pt_theory(P, A, Ai) # Create a new particle of mass m[i] at this point Pai = mechanics.Particle('Pa' + str(i), Pi, m[i]) particles.append(Pai) # Set forces & compute kinematic ODE forces.append((Pi, m[i] * g * A.x)) # Add external torque: forces.append((Ai, f[i] * A.z)) kinetic_odes.append(q[i].diff(t) - u[i]) P = Pi # Generate equations of motion KM = mechanics.KanesMethod(A, q_ind=q, u_ind=u, kd_eqs=kinetic_odes) fr, fr_star = KM.kanes_equations(particles, forces) #----------------------------------------------------- # Step 3: numerically evaluate equations # lengths and masses if lengths is None: lengths = np.ones(n) / n lengths = np.broadcast_to(lengths, n) masses = np.broadcast_to(masses, n) # Fixed parameters: gravitational constant, lengths, and masses parameters = [g] + list(l) + list(m) parameter_vals = [9.81] + list(lengths) + list(masses) # define symbols for unknown parameters unknowns = [Dummy() for i in q + u + f] unknown_dict = dict(zip(q + u + f, unknowns)) kds = KM.kindiffdict() # substitute unknown symbols for qdot terms mm_sym = KM.mass_matrix_full.subs(kds).subs(unknown_dict) fo_sym = KM.forcing_full.subs(kds).subs(unknown_dict) # create functions for numerical calculation self.mm_func = lambdify(unknowns + parameters, mm_sym) self.fo_func = lambdify(unknowns + parameters, fo_sym) self.args = parameter_vals A, B, _ = KM.linearize(A_and_B=True) parameter_dict = dict(zip(parameters, parameter_vals)) self.A = A.subs(parameter_dict) self.B = B.subs(parameter_dict) self.state = q + u
from pydy.codegen.ode_function_generators import generate_ode_function from scipy.integrate import odeint x0, y0, z0 = me.dynamicsymbols('x0 y0 z0') x01d, y01d, z01d = me.dynamicsymbols('x01d y01d z01d') u, v, w = me.dynamicsymbols('u v w') phi, theta, psi = me.dynamicsymbols('phi theta psi') phi1d, theta1d, psi1d = me.dynamicsymbols('phi1d theta1d psi1d') N = me.ReferenceFrame('N') S = N.orientnew('S', 'Body', [psi, theta, phi], 'ZYX') M = me.Point('M') O = M.locatenew('P', 0) M.set_vel(N, 0) O.set_vel(S, u * S.x + v * S.y + w * S.z) O.v1pt_theory(M, N, S) velocity_matrix = O.vel(N).to_matrix(N) mass = sp.symbols('m') I_xx, I_yy, I_zz = sp.symbols('I_xx, I_yy, I_zz') body_inertia_dyadic = me.inertia(S, ixx=I_xx, iyy=I_yy, izz=I_zz) body_central_inertia = (body_inertia_dyadic, O) body = me.RigidBody('Rigid body',
import sympy.physics.mechanics as me import sympy as sm import math as m import numpy as np frame_a = me.ReferenceFrame('a') c1, c2, c3 = sm.symbols('c1 c2 c3', real=True) a = me.inertia(frame_a, 1, 1, 1) particle_p1 = me.Particle('p1', me.Point('p1_pt'), sm.Symbol('m')) particle_p2 = me.Particle('p2', me.Point('p2_pt'), sm.Symbol('m')) body_r_cm = me.Point('r_cm') body_r_f = me.ReferenceFrame('r_f') body_r = me.RigidBody('r', body_r_cm, body_r_f, sm.symbols('m'), (me.outer(body_r_f.x, body_r_f.x), body_r_cm)) frame_a.orient(body_r_f, 'DCM', sm.Matrix([1, 1, 1, 1, 1, 0, 0, 0, 1]).reshape(3, 3)) point_o = me.Point('o') m1 = sm.symbols('m1') particle_p1.mass = m1 m2 = sm.symbols('m2') particle_p2.mass = m2 mr = sm.symbols('mr') body_r.mass = mr i1 = sm.symbols('i1') i2 = sm.symbols('i2') i3 = sm.symbols('i3') body_r.inertia = (me.inertia(body_r_f, i1, i2, i3, 0, 0, 0), body_r_cm) point_o.set_pos(particle_p1.point, c1 * frame_a.x) point_o.set_pos(particle_p2.point, c2 * frame_a.y) point_o.set_pos(body_r_cm, c3 * frame_a.z) a = me.inertia_of_point_mass(particle_p1.mass,
x, z, e, theta, phi = sympy.symbols('x z e theta phi') # Orient the Beta frame B = A.orientnew('B', 'Axis', [theta, A.y]) C = A.orientnew('C', 'Axis', [phi, A.y]) B.set_ang_vel(A, theta_dot * A.y) C.set_ang_vel(A, phi_dot * A.y) else: x, z, e, theta, phi = me.dynamicsymbols('x z e theta phi') # Orient the Beta frame B = A.orientnew('B', 'Axis', [theta, A.y]) C = A.orientnew('C', 'Axis', [phi, A.y]) B.set_ang_vel(A, theta_dot * A.y) C.set_ang_vel(A, phi_dot * A.y) # Create the origin points A1 = me.Point('A1') A2 = me.Point('A2') # Set the origin points positions A1.set_pos(A1, 0) A2.set_pos(A1, H * A.x) # Create the plate and rod center G = me.Point('G') Gr = me.Point('Gr') # Set both centers position G.set_pos(A1, x * A.x + z * A.z) Gr.set_pos(G, e * C.z) # Create the attachment points
def pendulum_energy(n=1, lengths=1, masses=1, include_gpe=True, include_ke=True): # Generalized coordinates and velocities # (in this case, angular positions & velocities of each mass) q = mechanics.dynamicsymbols('q:{0}'.format(n)) u = mechanics.dynamicsymbols('u:{0}'.format(n)) # mass and length m = symbols('m:{0}'.format(n)) l = symbols('l:{0}'.format(n)) # gravity and time symbols g, t = symbols('g,t') #-------------------------------------------------- # Step 2: build the model using Kane's Method # Create pivot point reference frame A = mechanics.ReferenceFrame('A') P = mechanics.Point('P') Origin = P P.set_vel(A, 0) gravity_direction = -A.x # lists to hold particles, forces, and kinetic ODEs # for each pendulum in the chain particles = [] forces = [] gpe = [] ke = [] cartVel = 0.0 cartPos = 0.0 for i in range(n): # Create a reference frame following the i^th mass Ai = A.orientnew('A' + str(i), 'Axis', [q[i], A.z]) Ai.set_ang_vel(A, u[i] * A.z) # Create a point in this reference frame Pi = P.locatenew('P' + str(i), l[i] * Ai.x) Pi.v2pt_theory(P, A, Ai) # Create a new particle of mass m[i] at this point Pai = mechanics.Particle('Pa' + str(i), Pi, m[i]) particles.append(Pai) # Calculate the cartesian position and velocity: # cartPos += l[i] * q[i] pos = Pi.pos_from(Origin) ke.append(1/n * Pai.kinetic_energy(A)) gpe.append(m[i] * g * (Pi.pos_from(Origin) & gravity_direction)) P = Pi # lengths and masses if lengths is None: lengths = np.ones(n) / n lengths = np.broadcast_to(lengths, n) masses = np.broadcast_to(masses, n) # Fixed parameters: gravitational constant, lengths, and masses parameters = [g] + list(l) + list(m) parameter_vals = [9.81] + list(lengths) + list(masses) # define symbols for unknown parameters unknowns = [Dummy() for i in q + u] unknown_dict = dict(zip(q + u, unknowns)) # create functions for numerical calculation total_energy = 0 if include_gpe: total_energy += (sum(gpe)).subs(zip(parameters, parameter_vals)) if include_ke: total_energy += (sum( ke)).subs(zip(parameters, parameter_vals)) total_energy_func = sympy2torch.sympy2torch(total_energy) minimum_energy = total_energy_func(**fixvalue(n, torch.tensor([[0.]*2*n]))).detach() return lambda inp: (total_energy_func(**fixvalue(n, inp)) - minimum_energy.to(inp)).unsqueeze(1)
# Blade angle phiDot = mech.dynamicsymbols('thetaDot') # Blade angluar velocity # Create symboloic variables we'll need m, L, kT, Omega, epsilon, Izz = sympy.symbols('m L kT Omega epsilon Izz') # Create inertial frame inertialFrame = mech.ReferenceFrame('inertialFrame') # Create reference frame at A that rotates with Omega N = mech.ReferenceFrame('N') N.set_ang_vel(inertialFrame, -Omega * inertialFrame.z) # Set point A A = mech.Point('A') A.set_vel(N, 0) # Set point B B = mech.Point('B') B.set_vel(N, 0) # B has no velocity in the rotating frame N B.set_pos(A, epsilon * N.x) # phi measured from x-axis along AB # Create frame for rod BC rodBCFrame = mech.ReferenceFrame('rodBCFrame') rodBCFrame.set_ang_vel(N, -phiDot * N.z) # Create point for the center of mass of BC comBC_x = (L / 2) * sympy.cos(phi)
import sympy.physics.mechanics as me import sympy as sm import math as m import numpy as np q1, q2 = me.dynamicsymbols('q1 q2') q1d, q2d = me.dynamicsymbols('q1 q2', 1) q1d2, q2d2 = me.dynamicsymbols('q1 q2', 2) l, m, g = sm.symbols('l m g', real=True) frame_n = me.ReferenceFrame('n') point_pn = me.Point('pn') point_pn.set_vel(frame_n, 0) theta1 = sm.atan(q2 / q1) frame_a = me.ReferenceFrame('a') frame_a.orient(frame_n, 'Axis', [theta1, frame_n.z]) particle_p = me.Particle('p', me.Point('p_pt'), sm.Symbol('m')) particle_p.point.set_pos(point_pn, q1 * frame_n.x + q2 * frame_n.y) particle_p.mass = m particle_p.point.set_vel(frame_n, (point_pn.pos_from(particle_p.point)).dt(frame_n)) f_v = me.dot((particle_p.point.vel(frame_n)).express(frame_a), frame_a.x) dependent = sm.Matrix([[0]]) dependent[0] = f_v velocity_constraints = [i for i in dependent] u_q1d = me.dynamicsymbols('u_q1d') u_q2d = me.dynamicsymbols('u_q2d') kd_eqs = [q1d - u_q1d, q2d - u_q2d] forceList = [(particle_p.point, particle_p.mass * (g * frame_n.x))] kane = me.KanesMethod(frame_n, q_ind=[q1, q2], u_ind=[u_q2d],
k_beta = rotational k D = rod length Izz = moment of Inertia about the end of a rod ''' # Create the world frame N = me.ReferenceFrame('N') # Create the rod frame B = N.orientnew('B', 'axis', [beta, N.z]) # Set the rotation of the rod frame B.set_ang_vel(N, -beta_dot * N.z) # Create the Origin O1 = me.Point('O_1') # Set origin velocity to zero O1.set_vel(N, 0 * N.x) # Create the second attachment point # O2 = O1.locatenew('O_2', H * N.x) O2 = me.Point('O_2') O2.set_pos(O1, H * N.x) O2.set_vel(N, 0) # Locate the point in the N frame # P = me.Point('pen') # P = O1.locatenew('P', x * N.x + y * N.y) P = me.Point('P') P.set_pos(O1, x * N.x + y * N.y)
import sympy.physics.mechanics as me import sympy as sm import math as m import numpy as np frame_n = me.ReferenceFrame("n") frame_a = me.ReferenceFrame("a") a = 0 d = me.inertia(frame_a, 1, 1, 1) point_po1 = me.Point("po1") point_po2 = me.Point("po2") particle_p1 = me.Particle("p1", me.Point("p1_pt"), sm.Symbol("m")) particle_p2 = me.Particle("p2", me.Point("p2_pt"), sm.Symbol("m")) c1, c2, c3 = me.dynamicsymbols("c1 c2 c3") c1d, c2d, c3d = me.dynamicsymbols("c1 c2 c3", 1) body_r_cm = me.Point("r_cm") body_r_cm.set_vel(frame_n, 0) body_r_f = me.ReferenceFrame("r_f") body_r = me.RigidBody( "r", body_r_cm, body_r_f, sm.symbols("m"), (me.outer(body_r_f.x, body_r_f.x), body_r_cm), ) point_po2.set_pos(particle_p1.point, c1 * frame_a.x) v = 2 * point_po2.pos_from(particle_p1.point) + c2 * frame_a.y frame_a.set_ang_vel(frame_n, c3 * frame_a.z) v = 2 * frame_a.ang_vel_in(frame_n) + c2 * frame_a.y body_r_f.set_ang_vel(frame_n, c3 * frame_a.z) v = 2 * body_r_f.ang_vel_in(frame_n) + c2 * frame_a.y
# create a Newtonian reference frame N = me.ReferenceFrame('N') # create a reference for the rod, A, and the plate, B A = me.ReferenceFrame('A') B = me.ReferenceFrame('B') # orientations # # the rod rotates with respect to the Newtonian reference frame about the 2 # axis A.orient(N, 'Axis', [theta, N.y]) # the plate rotates about the rod's primay axis B.orient(A, 'Axis', [phi, A.z]) # positions # # origin of the Newtonian reference frame No = me.Point('No') # create a point for the mass centers of the two bodies Ao = me.Point('Ao') Bo = me.Point('Bo') # define the positions of the mass centers relative to the Newtonian origin Ao.set_pos(No, lA * A.z) Bo.set_pos(No, lB * A.z) # angular velocities and accelerations # A.set_ang_vel(N, omega * N.y) B.set_ang_vel(A, alpha * A.z) # take the derivative of the angular velocities to get angular accelerations A.set_ang_acc(N, A.ang_vel_in(N).dt(N)) B.set_ang_acc(N, B.ang_vel_in(N).dt(N))
def integrate_pendulum(n, times, initial_positions=135, initial_velocities=0, lengths=None, masses=1): """Integrate a multi-pendulum with `n` sections""" # ------------------------------------------------- # Step 1: construct the pendulum model # Generalized coordinates and velocities # (in this case, angular positions & velocities of each mass) q = mechanics.dynamicsymbols('q:{0}'.format(n)) u = mechanics.dynamicsymbols('u:{0}'.format(n)) # mass and length m = symbols('m:{0}'.format(n)) l = symbols('l:{0}'.format(n)) # gravity and time symbols g, t = symbols('g,t') # -------------------------------------------------- # Step 2: build the model using Kane's Method # Create pivot point reference frame A = mechanics.ReferenceFrame('A') P = mechanics.Point('P') P.set_vel(A, 0) # lists to hold particles, forces, and kinetic ODEs # for each pendulum in the chain particles = [] forces = [] kinetic_odes = [] for i in range(n): # Create a reference frame following the i^th mass Ai = A.orientnew('A' + str(i), 'Axis', [q[i], A.z]) Ai.set_ang_vel(A, u[i] * A.z) # Create a point in this reference frame Pi = P.locatenew('P' + str(i), l[i] * Ai.x) Pi.v2pt_theory(P, A, Ai) # Create a new particle of mass m[i] at this point Pai = mechanics.Particle('Pa' + str(i), Pi, m[i]) particles.append(Pai) # Set forces & compute kinematic ODE forces.append((Pi, m[i] * g * A.x)) kinetic_odes.append(q[i].diff(t) - u[i]) P = Pi # Generate equations of motion KM = mechanics.KanesMethod(A, q_ind=q, u_ind=u, kd_eqs=kinetic_odes) fr, fr_star = KM.kanes_equations(forces, particles) # ----------------------------------------------------- # Step 3: numerically evaluate equations and integrate # initial positions and velocities – assumed to be given in degrees y0 = np.deg2rad( np.concatenate([ np.broadcast_to(initial_positions, n), np.broadcast_to(initial_velocities, n) ])) # lengths and masses if lengths is None: lengths = np.ones(n) / n lengths = np.broadcast_to(lengths, n) masses = np.broadcast_to(masses, n) # Fixed parameters: gravitational constant, lengths, and masses parameters = [g] + list(l) + list(m) parameter_vals = [9.81] + list(lengths) + list(masses) # define symbols for unknown parameters unknowns = [Dummy() for i in q + u] unknown_dict = dict(zip(q + u, unknowns)) kds = KM.kindiffdict() # substitute unknown symbols for qdot terms mm_sym = KM.mass_matrix_full.subs(kds).subs(unknown_dict) fo_sym = KM.forcing_full.subs(kds).subs(unknown_dict) # create functions for numerical calculation mm_func = lambdify(unknowns + parameters, mm_sym) fo_func = lambdify(unknowns + parameters, fo_sym) # function which computes the derivatives of parameters def gradient(y, t, args): vals = np.concatenate((y, args)) sol = np.linalg.solve(mm_func(*vals), fo_func(*vals)) return np.array(sol).T[0] # ODE integration return odeint(gradient, y0, times, args=(parameter_vals, ))
m = sm.Matrix([a, b, c, 0]).reshape(2, 2) m2 = sm.Matrix([i.subs({ a: 1, b: 2, c: 3 }) for i in m]).reshape((m).shape[0], (m).shape[1]) eigvalue = sm.Matrix([i.evalf() for i in (m2).eigenvals().keys()]) eigvec = sm.Matrix([i[2][0].evalf() for i in (m2).eigenvects() ]).reshape(m2.shape[0], m2.shape[1]) frame_n = me.ReferenceFrame('n') frame_a = me.ReferenceFrame('a') frame_a.orient(frame_n, 'Axis', [x, frame_n.x]) frame_a.orient(frame_n, 'Axis', [sm.pi / 2, frame_n.x]) c1, c2, c3 = sm.symbols('c1 c2 c3', real=True) v = c1 * frame_a.x + c2 * frame_a.y + c3 * frame_a.z point_o = me.Point('o') point_p = me.Point('p') point_o.set_pos(point_p, c1 * frame_a.x) v = (v).express(frame_n) point_o.set_pos(point_p, (point_o.pos_from(point_p)).express(frame_n)) frame_a.set_ang_vel(frame_n, c3 * frame_a.z) print(frame_n.ang_vel_in(frame_a)) point_p.v2pt_theory(point_o, frame_n, frame_a) particle_p1 = me.Particle('p1', me.Point('p1_pt'), sm.Symbol('m')) particle_p2 = me.Particle('p2', me.Point('p2_pt'), sm.Symbol('m')) particle_p2.point.v2pt_theory(particle_p1.point, frame_n, frame_a) point_p.a2pt_theory(particle_p1.point, frame_n, frame_a) body_b1_cm = me.Point('b1_cm') body_b1_cm.set_vel(frame_n, 0) body_b1_f = me.ReferenceFrame('b1_f') body_b1 = me.RigidBody('b1', body_b1_cm, body_b1_f, sm.symbols('m'),
N = me.ReferenceFrame('N') B = me.ReferenceFrame('B') # Create the symbols x, y, beta, e, F, L1, L2 = me.dynamicsymbols('x y beta e F L1 L2') x_dot, y_dot, beta_dot, e_dot = me.dynamicsymbols('x_dot y_dot beta_dot e_dot') H, a, b, m, g, k, t = sympy.symbols('H a b m g k t') c, c_rod, D, M, k_rod, mom = sympy.symbols('c c_rod D M k_rod mom') Izz, Izz_rod = sympy.symbols('Izz Izz_rod') # Orient the Beta frame B.orient(N, 'Axis', (beta, N.z)) B.set_ang_vel(N, beta_dot * N.z) # Create the first point O1 = me.Point('O1') O2 = me.Point('O2') O1.set_pos(O1, 0) O2.set_pos(O1, H * N.x) G = me.Point('G') G.set_pos(O1, x * N.x - y * N.y) P1 = me.Point('P1') P1.set_pos(G, -a / 2 * B.x + b / 2 * B.y) P2 = me.Point('P2') P2.set_pos(G, a / 2 * B.x + b / 2 * B.y) O1.set_vel(N, 0) O2.set_vel(N, 0) G.set_vel(B, 0)
import sympy.physics.mechanics as me import sympy as sm import math as m import numpy as np g, lb, w, h = sm.symbols('g lb w h', real=True) theta, phi, omega, alpha = me.dynamicsymbols('theta phi omega alpha') thetad, phid, omegad, alphad = me.dynamicsymbols('theta phi omega alpha', 1) thetad2, phid2 = me.dynamicsymbols('theta phi', 2) frame_n = me.ReferenceFrame('n') body_a_cm = me.Point('a_cm') body_a_cm.set_vel(frame_n, 0) body_a_f = me.ReferenceFrame('a_f') body_a = me.RigidBody('a', body_a_cm, body_a_f, sm.symbols('m'), (me.outer(body_a_f.x,body_a_f.x),body_a_cm)) body_b_cm = me.Point('b_cm') body_b_cm.set_vel(frame_n, 0) body_b_f = me.ReferenceFrame('b_f') body_b = me.RigidBody('b', body_b_cm, body_b_f, sm.symbols('m'), (me.outer(body_b_f.x,body_b_f.x),body_b_cm)) body_a_f.orient(frame_n, 'Axis', [theta, frame_n.y]) body_b_f.orient(body_a_f, 'Axis', [phi, body_a_f.z]) point_o = me.Point('o') la = (lb-h/2)/2 body_a_cm.set_pos(point_o, la*body_a_f.z) body_b_cm.set_pos(point_o, lb*body_a_f.z) body_a_f.set_ang_vel(frame_n, omega*frame_n.y) body_b_f.set_ang_vel(body_a_f, alpha*body_a_f.z) point_o.set_vel(frame_n, 0) body_a_cm.v2pt_theory(point_o,frame_n,body_a_f) body_b_cm.v2pt_theory(point_o,frame_n,body_a_f) ma = sm.symbols('ma') body_a.mass = ma
from sympy import symbols import sympy.physics.mechanics as me mass, stiffness, damping, gravity = symbols('m, k, c, g') position, speed = me.dynamicsymbols('x v') positiond = me.dynamicsymbols('x', 1) force = me.dynamicsymbols('F') ceiling = me.ReferenceFrame('N') origin = me.Point('origin') origin.set_vel(ceiling, 0) center = origin.locatenew('center', position * ceiling.x) center.set_vel(ceiling, speed * ceiling.x) block = me.Particle('block', center, mass) kinematic_equations = [speed - positiond] force_magnitude = mass * gravity - stiffness * position - damping * speed + force forces = [(center, force_magnitude * ceiling.x)] particles = [block] kane = me.KanesMethod(ceiling, q_ind=[position], u_ind=[speed], kd_eqs=kinematic_equations) kane.kanes_equations(forces, particles)
def multi_mass_spring_damper(n=1, apply_gravity=False, apply_external_forces=False): """Returns a system containing the symbolic equations of motion and associated variables for a simple mutli-degree of freedom point mass, spring, damper system with optional gravitational and external specified forces. For example, a two mass system under the influence of gravity and external forces looks like: :: ---------------- | | | | g \ | | | V k0 / --- c0 | | | | x0, v0 --------- V | m0 | ----- --------- | | | | | \ v | | | k1 / f0 --- c1 | | | | x1, v1 --------- V | m1 | ----- --------- | f1 V Parameters ---------- n : integer The number of masses in the serial chain. apply_gravity : boolean If true, gravity will be applied to each mass. apply_external_forces : boolean If true, a time varying external force will be applied to each mass. Returns ------- kane : sympy.physics.mechanics.kane.KanesMethod A KanesMethod object. """ mass = sm.symbols('m:{}'.format(n)) stiffness = sm.symbols('k:{}'.format(n)) damping = sm.symbols('c:{}'.format(n)) acceleration_due_to_gravity = sm.symbols('g') coordinates = me.dynamicsymbols('x:{}'.format(n)) speeds = me.dynamicsymbols('v:{}'.format(n)) specifieds = me.dynamicsymbols('f:{}'.format(n)) ceiling = me.ReferenceFrame('N') origin = me.Point('origin') origin.set_vel(ceiling, 0) points = [origin] kinematic_equations = [] particles = [] forces = [] for i in range(n): center = points[-1].locatenew('center{}'.format(i), coordinates[i] * ceiling.x) center.set_vel(ceiling, points[-1].vel(ceiling) + speeds[i] * ceiling.x) points.append(center) block = me.Particle('block{}'.format(i), center, mass[i]) kinematic_equations.append(speeds[i] - coordinates[i].diff()) total_force = (-stiffness[i] * coordinates[i] - damping[i] * speeds[i]) try: total_force += (stiffness[i + 1] * coordinates[i + 1] + damping[i + 1] * speeds[i + 1]) except IndexError: # no force from below on last mass pass if apply_gravity: total_force += mass[i] * acceleration_due_to_gravity if apply_external_forces: total_force += specifieds[i] forces.append((center, total_force * ceiling.x)) particles.append(block) kane = me.KanesMethod(ceiling, q_ind=coordinates, u_ind=speeds, kd_eqs=kinematic_equations) kane.kanes_equations(particles, forces) return kane
def n_link_pendulum_on_cart(n=1, cart_force=True, joint_torques=False): """Returns the system containing the symbolic first order equations of motion for a 2D n-link pendulum on a sliding cart under the influence of gravity. :: | o y v \ 0 ^ g \ | --\-|---- | \| | F-> | o --|---> x | | --------- o o Parameters ---------- n : integer The number of links in the pendulum. cart_force : boolean, default=True If true an external specified lateral force is applied to the cart. joint_torques : boolean, default=False If true joint torques will be added as specified inputs at each joint. Returns ------- kane : sympy.physics.mechanics.kane.KanesMethod A KanesMethod object. Notes ----- The degrees of freedom of the system are n + 1, i.e. one for each pendulum link and one for the lateral motion of the cart. M x' = F, where x = [u0, ..., un+1, q0, ..., qn+1] The joint angles are all defined relative to the ground where the x axis defines the ground line and the y axis points up. The joint torques are applied between each adjacent link and the between the cart and the lower link where a positive torque corresponds to positive angle. """ if n <= 0: raise ValueError('The number of links must be a positive integer.') q = me.dynamicsymbols('q:{}'.format(n + 1)) u = me.dynamicsymbols('u:{}'.format(n + 1)) if joint_torques is True: T = me.dynamicsymbols('T1:{}'.format(n + 1)) m = sm.symbols('m:{}'.format(n + 1)) l = sm.symbols('l:{}'.format(n)) g, t = sm.symbols('g t') I = me.ReferenceFrame('I') O = me.Point('O') O.set_vel(I, 0) P0 = me.Point('P0') P0.set_pos(O, q[0] * I.x) P0.set_vel(I, u[0] * I.x) Pa0 = me.Particle('Pa0', P0, m[0]) frames = [I] points = [P0] particles = [Pa0] forces = [(P0, -m[0] * g * I.y)] kindiffs = [q[0].diff(t) - u[0]] if cart_force is True or joint_torques is True: specified = [] else: specified = None for i in range(n): Bi = I.orientnew('B{}'.format(i), 'Axis', [q[i + 1], I.z]) Bi.set_ang_vel(I, u[i + 1] * I.z) frames.append(Bi) Pi = points[-1].locatenew('P{}'.format(i + 1), l[i] * Bi.y) Pi.v2pt_theory(points[-1], I, Bi) points.append(Pi) Pai = me.Particle('Pa' + str(i + 1), Pi, m[i + 1]) particles.append(Pai) forces.append((Pi, -m[i + 1] * g * I.y)) if joint_torques is True: specified.append(T[i]) if i == 0: forces.append((I, -T[i] * I.z)) if i == n - 1: forces.append((Bi, T[i] * I.z)) else: forces.append((Bi, T[i] * I.z - T[i + 1] * I.z)) kindiffs.append(q[i + 1].diff(t) - u[i + 1]) if cart_force is True: F = me.dynamicsymbols('F') forces.append((P0, F * I.x)) specified.append(F) kane = me.KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kindiffs) kane.kanes_equations(particles, forces) return kane
import sympy.physics.mechanics as _me import sympy as _sm import math as m import numpy as _np m, k, b, g = _sm.symbols('m k b g', real=True) position, speed = _me.dynamicsymbols('position speed') position_d, speed_d = _me.dynamicsymbols('position_ speed_', 1) o = _me.dynamicsymbols('o') force = o*_sm.sin(_me.dynamicsymbols._t) frame_ceiling = _me.ReferenceFrame('ceiling') point_origin = _me.Point('origin') point_origin.set_vel(frame_ceiling, 0) particle_block = _me.Particle('block', _me.Point('block_pt'), _sm.Symbol('m')) particle_block.point.set_pos(point_origin, position*frame_ceiling.x) particle_block.mass = m particle_block.point.set_vel(frame_ceiling, speed*frame_ceiling.x) force_magnitude = m*g-k*position-b*speed+force force_block = (force_magnitude*frame_ceiling.x).subs({position_d:speed}) kd_eqs = [position_d - speed] forceList = [(particle_block.point,(force_magnitude*frame_ceiling.x).subs({position_d:speed}))] kane = _me.KanesMethod(frame_ceiling, q_ind=[position], u_ind=[speed], kd_eqs = kd_eqs) fr, frstar = kane.kanes_equations([particle_block], forceList) zero = fr+frstar from pydy.system import System sys = System(kane, constants = {m:1.0, k:1.0, b:0.2, g:9.8}, specifieds={_me.dynamicsymbols('t'):lambda x, t: t, o:2}, initial_conditions={position:0.1, speed:-1*1.0}, times = _np.linspace(0.0, 10.0, 10.0/0.01)) y=sys.integrate()
import sympy.physics.mechanics as _me import sympy as _sm import math as m import numpy as _np frame_a = _me.ReferenceFrame('a') c1, c2, c3 = _sm.symbols('c1 c2 c3', real=True) a = _me.inertia(frame_a, 1, 1, 1) particle_p1 = _me.Particle('p1', _me.Point('p1_pt'), _sm.Symbol('m')) particle_p2 = _me.Particle('p2', _me.Point('p2_pt'), _sm.Symbol('m')) body_r_cm = _me.Point('r_cm') body_r_f = _me.ReferenceFrame('r_f') body_r = _me.RigidBody('r', body_r_cm, body_r_f, _sm.symbols('m'), (_me.outer(body_r_f.x,body_r_f.x),body_r_cm)) frame_a.orient(body_r_f, 'DCM', _sm.Matrix([1,1,1,1,1,0,0,0,1]).reshape(3, 3)) point_o = _me.Point('o') m1 = _sm.symbols('m1') particle_p1.mass = m1 m2 = _sm.symbols('m2') particle_p2.mass = m2 mr = _sm.symbols('mr') body_r.mass = mr i1 = _sm.symbols('i1') i2 = _sm.symbols('i2') i3 = _sm.symbols('i3') body_r.inertia = (_me.inertia(body_r_f, i1, i2, i3, 0, 0, 0), body_r_cm) point_o.set_pos(particle_p1.point, c1*frame_a.x) point_o.set_pos(particle_p2.point, c2*frame_a.y) point_o.set_pos(body_r_cm, c3*frame_a.z) a = _me.inertia_of_point_mass(particle_p1.mass, particle_p1.point.pos_from(point_o), frame_a) a = _me.inertia_of_point_mass(particle_p2.mass, particle_p2.point.pos_from(point_o), frame_a) a = body_r.inertia[0] + _me.inertia_of_point_mass(body_r.mass, body_r.masscenter.pos_from(point_o), frame_a)
#-----------------------------# # newtonian frame N = me.ReferenceFrame('N', indices=('1', '2', '3')) # bicycle frame B = me.ReferenceFrame('B', indices=('1', '2', '3')) # handlebar frame G = me.ReferenceFrame('G', indices=('1', '2', '3')) G.orient(B, 'Axis', (delta, B['3'])) #------------------------------------# # define the locations of the points # #------------------------------------# # vectornav center v = me.Point('v') # point on the steer axis s = me.Point('s') s.set_pos(v, ds1 * B['1'] + ds3 * B['3']) # handlebar center of mass go = me.Point('go') go.set_pos(s, d * G['1']) #---------------------------------------------------# # set the known angular velocites and accelerations # #---------------------------------------------------# # set the angular velocities of B and G B.set_ang_vel(N, wb1 * B['1'] + wb2 * B['2'] + wb3 * B['3'])
import pydy q = me.dynamicsymbols('q:12') u = me.dynamicsymbols('u0:12') qd = me.dynamicsymbols('q:12', 1) ud = me.dynamicsymbols('u0:12', 1) L = sm.symbols('LA:D0:4x:z') I = sm.symbols('Ia:bx:z') g = sm.symbols('g') ma, mb = sm.symbols('ma:b') k = sm.symbols('k0:2x:z') Lgr, Lsp = sm.symbols('Lgr Lsp') N = me.ReferenceFrame('N') Orig = me.Point('O') Orig.set_vel(N, 0 * N.x + 0 * N.y + 0 * N.z) OA = [ Orig.locatenew('OA_0', L[0] * N.x + L[1] * N.y + L[2] * N.z), Orig.locatenew('OA_1', -L[3] * N.x + L[4] * N.y + L[5] * N.z), Orig.locatenew('OA_2', -L[6] * N.x + L[7] * N.y - L[8] * N.z), Orig.locatenew('OA_3', L[9] * N.x + L[10] * N.y - L[11] * N.z) ] A = N.orientnew('A', 'Body', (q[0], q[1], q[2]), '123') A.set_ang_vel(N, u[0] * N.x + u[1] * N.y + u[2] * N.z) CMa = Orig.locatenew('CM_s', q[3] * N.x + q[4] * N.y + q[5] * N.z) CMa.set_vel(N, u[3] * N.x + u[4] * N.y + u[5] * N.z) AO = [ CMa.locatenew('AO_0', L[12] * A.x - L[13] * A.y + L[14] * A.z), CMa.locatenew('AO_1', -L[15] * A.x - L[16] * A.y + L[17] * A.z),
def setup(self): # System state variables q = me.dynamicsymbols('q') qd = me.dynamicsymbols('q', 1) # Other system variables m, k, b = symbols('m k b') # Set up the reference frame N = me.ReferenceFrame('N') # Set up the points and particles P = me.Point('P') P.set_vel(N, qd * N.x) Pa = me.Particle('Pa', P, m) # Define the potential energy and create the Lagrangian Pa.potential_energy = k * q**2 / 2.0 L = me.Lagrangian(N, Pa) # Create the list of forces acting on the system fl = [(P, -b * qd * N.x)] # Create an instance of Lagranges Method self.l = me.LagrangesMethod(L, [q], forcelist=fl, frame=N)