def make_kane_eom(dynamic, setup, fbd): """Formulate the equations of motion using Kane's method. Paramters --------- The dictionaries that are returned from formulated_nchain_parameters Returns ------- """ # equations of motion using Kane's method kane = me.KanesMethod(frame=setup['frames'][0], q_ind=dynamic['q'], u_ind=dynamic['u'], kd_eqs=fbd['kd'], q_dependent=[], configuration_constraints=[], u_dependent=[], velocity_constraints=[]) (fr, frstar) = kane.kanes_equations(fbd['fl'], fbd['bodies']) kanezero = fr + frstar mass = kane.mass_matrix_full forcing = kane.forcing_full eom = dict(kane=kane, fr=fr, frstar=frstar, mass=mass, forcing=forcing, kanezero=kanezero) return eom
def _generate_eoms(self): self.kane = me.KanesMethod(self.frames['inertial'], list(self.coordinates.values()), list(self.speeds.values()), self.kin_diff_eqs) fr, frstar = self.kane.kanes_equations( list(self.rigid_bodies.values()), list(self.loads.values())) fr_plus_frstar = sy.trigsimp(fr + frstar) sub = { self.speeds['left_hip'].diff(self.time): self.accelerations['left_hip'], self.speeds['left_knee'].diff(self.time): self.accelerations['left_knee'], self.speeds['right_hip'].diff(self.time): self.accelerations['right_hip'], self.speeds['right_knee'].diff(self.time): self.accelerations['right_knee'] } self.fr_plus_frstar = fr_plus_frstar.xreplace(sub) return self.fr_plus_frstar, self.kane
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 generate_equation(self): """ New attributes: rhs : right hand side :return: """ print('symbolic dynamic: generating symbolic equations') kane = me.KanesMethod(self.frames[0], self.q, self.q_dot, self.kde) fr, frstar = kane.kanes_equations(self.loads, self.bodies) mass_matrix = kane.mass_matrix_full forcing_vector = kane.forcing_full right_hand_side = generate_ode_function(forcing_vector, self.q, self.q_dot, [], mass_matrix=mass_matrix, specifieds=self.tau) self.kane = kane self.rhs = right_hand_side
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 _generate_eoms(self, simplify=False): self.kane = me.KanesMethod(self.frames['inertial'], list(self.coordinates.values()), list(self.speeds.values()), self.kin_diff_eqs) fr, frstar = self.kane.kanes_equations(list(self.loads.values()), list(self.rigid_bodies.values())) sub = {self.specified['platform_speed'].diff(self.time): self.specified['platform_acceleration']} if simplify: fr_plus_frstar = sy.trigsimp(fr + frstar) else: fr_plus_frstar = fr + frstar self.fr_plus_frstar = fr_plus_frstar.xreplace(sub) udots = sy.Matrix([s.diff(self.time) for s in self.speeds.values()]) m1 = self.fr_plus_frstar.diff(udots[0]) m2 = self.fr_plus_frstar.diff(udots[1]) # M x' = F self.mass_matrix = -m1.row_join(m2) self.forcing_vector = sy.simplify(self.fr_plus_frstar + self.mass_matrix * udots) M_top_rows = sy.eye(2).row_join(sy.zeros(2)) F_top_rows = sy.Matrix(list(self.speeds.values())) tmp = sy.zeros(2).row_join(self.mass_matrix) self.mass_matrix_full = M_top_rows.col_join(tmp) self.forcing_vector_full = F_top_rows.col_join(self.forcing_vector)
# Storing the forces and respective points in tuple damping_1 = (P1, P1_damp) damping_2 = (P2, P2_damp) # These are the damping forces acting on the rod and on the plate from the rod damping_rod = (Z_G, -c_rod * e_dot * B.y) damping_rod_on_plate = (G, c_rod * e_dot * B.y) # This is the moment that needs to be applied in order for the plate to stay in # static equilibrium moment = (B, Lengths_and_Moments(x, y)[2] * N.z) # Setting up the coordinates speeds and creating the calling KanesMethod coordinates = [x, y, beta, e] speeds = [x_dot, y_dot, beta_dot, e_dot] kane = me.KanesMethod(N, coordinates, speeds, kde) loads = [ spring_1_force_P1, spring_2_force_P2, grav_force_plate, grav_force_rod, spring_force_rod, spring_force_rod_on_plate, damping_rod, damping_rod_on_plate, damping_1, damping_2, moment ] fr, frstar = kane.kanes_equations(loads, [Plate, rod]) Mass = kane.mass_matrix_full f = kane.forcing_full sys = System(kane) sys.constants = { m: mass_of_rod,
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
plate = me.RigidBody('plate', Bo, B, mB, (me.inertia(B, IBxx, IByy, IBzz), Bo)) # forces # # add the gravitional force to each body rodGravity = (Ao, N.z * gravity * mA) plateGravity = (Bo, N.z * gravity * mB) ## equations of motion with Kane's method ## # make a list of the bodies and forces bodyList = [rod, plate] forceList = [rodGravity, plateGravity] # create a Kane object with respect to the Newtonian reference frame kane = me.KanesMethod(N, q_ind=[theta, phi], u_ind=[omega, alpha], kd_eqs=kinDiffs) # calculate Kane's equations fr, frstar = kane.kanes_equations(forceList, bodyList) zero = fr + frstar # solve Kane's equations for the derivatives of the speeds eom = sym.solvers.solve(zero, omegad, alphad) # add the kinematical differential equations to get the equations of motion eom.update(kane.kindiffdict()) # print the results me.mprint(eom)
torque_vector = mx * S.x + my * S.y + mz * S.z force = (O, force_vector) torque = (S, torque_vector) kinematical_differential_equations = [ x0.diff() - velocity_matrix[0], y0.diff() - velocity_matrix[1], z0.diff() - velocity_matrix[2], phi.diff() - phi1d, theta.diff() - theta1d, psi.diff() - psi1d, ] coordinates = [x0, y0, z0, phi, theta, psi] speeds = [u, v, w, phi1d, theta1d, psi1d] kane = me.KanesMethod(N, coordinates, speeds, kinematical_differential_equations) loads = [force, torque] bodies = [body] fr, frstar = kane.kanes_equations(bodies, loads) constants = [I_xx, I_yy, I_zz, mass] specified = [fx, fy, fz, mx, my, mz] # External force/torque right_hand_side = generate_ode_function(kane.forcing_full, coordinates, speeds, constants, mass_matrix=kane.mass_matrix_full,
def get_symbolic_equations_of_motion(self, verbose=False): """ This returns the symbolic equation of motions of the robot (using the URDF). Internally, this used the `sympy.mechanics` module. """ # gravity and time g, t = sympy.symbols('g t') # create the world inertial frame of reference and its origin world_frame = mechanics.ReferenceFrame('Fw') world_origin = mechanics.Point('Pw') world_origin.set_vel(world_frame, mechanics.Vector(0)) # create the base frame (its position, orientation and velocities) + generalized coordinates and speeds base_id = -1 # Check if the robot has a fixed base and create the generalized coordinates and speeds based on that, # as well the base position, orientation and velocities if self.has_fixed_base(): # generalized coordinates q(t) and speeds dq(t) q = mechanics.dynamicsymbols('q:{}'.format(len(self.joints))) dq = mechanics.dynamicsymbols('dq:{}'.format(len(self.joints))) pos, orn = self.get_base_pose() lin_vel, ang_vel = [0, 0, 0], [0, 0, 0] # 0 because fixed base joint_id = 0 else: # generalized coordinates q(t) and speeds dq(t) q = mechanics.dynamicsymbols('q:{}'.format(7 + len(self.joints))) dq = mechanics.dynamicsymbols('dq:{}'.format(6 + len(self.joints))) pos, orn = q[:3], q[3:7] lin_vel, ang_vel = dq[:3], dq[3:6] joint_id = 7 # set the position, orientation and velocities of the base base_frame = world_frame.orientnew('Fb', 'Quaternion', [orn[3], orn[0], orn[1], orn[2]]) base_frame.set_ang_vel( world_frame, ang_vel[0] * world_frame.x + ang_vel[1] * world_frame.y + ang_vel[2] * world_frame.z) base_origin = world_origin.locatenew( 'Pb', pos[0] * world_frame.x + pos[1] * world_frame.y + pos[2] * world_frame.z) base_origin.set_vel( world_frame, lin_vel[0] * world_frame.x + lin_vel[1] * world_frame.y + lin_vel[2] * world_frame.z) # inputs u(t) (applied torques) u = mechanics.dynamicsymbols('u:{}'.format(len(self.joints))) joint_id_u = 0 # kinematics differential equations kd_eqs = [q[i].diff(t) - dq[i] for i in range(len(self.joints))] # define useful lists/dicts for later bodies, loads = [], [] frames = {base_id: (base_frame, base_origin)} # frames = {base_id: (worldFrame, worldOrigin)} # go through each joint/link (each link is associated to a joint) for link_id in range(self.num_links): # get useful information about joint/link kinematics and dynamics from simulator info = self.sim.get_dynamics_info(self.id, link_id) mass, local_inertia_diagonal = info[0], info[2] info = self.sim.get_link_state(self.id, link_id) local_inertial_frame_position, local_inertial_frame_orientation = info[ 2], info[3] # worldLinkFramePosition, worldLinkFrameOrientation = info[4], info[5] info = self.sim.get_joint_info(self.id, link_id) joint_name, joint_type = info[1:3] # jointDamping, jointFriction = info[6:8] link_name, joint_axis_in_local_frame, parent_frame_position, parent_frame_orientation, \ parent_idx = info[-5:] xl, yl, zl = joint_axis_in_local_frame # get previous references parent_frame, parent_point = frames[parent_idx] # create a reference frame with its origin for each joint # set frame orientation if joint_type == self.sim.JOINT_REVOLUTE: R = get_matrix_from_quaternion(parent_frame_orientation) R1 = get_symbolic_matrix_from_axis_angle( joint_axis_in_local_frame, q[joint_id]) R = R1.dot(R) frame = parent_frame.orientnew('F' + str(link_id), 'DCM', sympy.Matrix(R)) else: x, y, z, w = parent_frame_orientation # orientation of the joint in parent CoM inertial frame frame = parent_frame.orientnew('F' + str(link_id), 'Quaternion', [w, x, y, z]) # set frame angular velocity ang_vel = 0 if joint_type == self.sim.JOINT_REVOLUTE: ang_vel = dq[joint_id] * (xl * frame.x + yl * frame.y + zl * frame.z) frame.set_ang_vel(parent_frame, ang_vel) # create origin of the reference frame # set origin position x, y, z = parent_frame_position # position of the joint in parent CoM inertial frame pos = x * parent_frame.x + y * parent_frame.y + z * parent_frame.z if joint_type == self.sim.JOINT_PRISMATIC: pos += q[joint_id] * (xl * frame.x + yl * frame.y + zl * frame.z) origin = parent_point.locatenew('P' + str(link_id), pos) # set origin velocity if joint_type == self.sim.JOINT_PRISMATIC: vel = dq[joint_id] * (xl * frame.x + yl * frame.y + zl * frame.z) origin.set_vel(world_frame, vel.express(world_frame)) else: origin.v2pt_theory(parent_point, world_frame, parent_frame) # define CoM frame and position (and velocities) wrt the local link frame x, y, z, w = local_inertial_frame_orientation com_frame = frame.orientnew('Fc' + str(link_id), 'Quaternion', [w, x, y, z]) com_frame.set_ang_vel(frame, mechanics.Vector(0)) x, y, z = local_inertial_frame_position com = origin.locatenew('C' + str(link_id), x * frame.x + y * frame.y + z * frame.z) com.v2pt_theory(origin, world_frame, frame) # define com particle # com_particle = mechanics.Particle('Pa' + str(linkId), com, mass) # bodies.append(com_particle) # save # frames[linkId] = (frame, origin) # frames[linkId] = (frame, origin, com_frame, com) frames[link_id] = (com_frame, com) # define mass and inertia ixx, iyy, izz = local_inertia_diagonal inertia = mechanics.inertia(com_frame, ixx, iyy, izz, ixy=0, iyz=0, izx=0) inertia = (inertia, com) # define rigid body associated to frame body = mechanics.RigidBody(link_name, com, frame, mass, inertia) bodies.append(body) # define dynamical forces/torques acting on the body # gravity force applied on the CoM force = (com, -mass * g * world_frame.z) loads.append(force) # if prismatic joint, compute force if joint_type == self.sim.JOINT_PRISMATIC: force = (origin, u[joint_id_u] * (xl * frame.x + yl * frame.y + zl * frame.z)) # force = (com, u[jointIdU] * (x * frame.x + y * frame.y + z * frame.z) - mass * g * worldFrame.z) loads.append(force) # if revolute joint, compute torque if joint_type == self.sim.JOINT_REVOLUTE: v = (xl * frame.x + yl * frame.y + zl * frame.z) # torqueOnPrevBody = (parentFrame, - u[jointIdU] * v) torque_on_prev_body = (parent_frame, -u[joint_id_u] * v) torque_on_curr_body = (frame, u[joint_id_u] * v) loads.append(torque_on_prev_body) loads.append(torque_on_curr_body) # if joint is not fixed increment the current joint id if joint_type != self.sim.JOINT_FIXED: joint_id += 1 joint_id_u += 1 if verbose: print("\nLink name with type: {} - {}".format( link_name, self.get_joint_types(joint_ids=link_id))) print("------------------------------------------------------") print("Position of joint frame wrt parent frame: {}".format( origin.pos_from(parent_point))) print("Orientation of joint frame wrt parent frame: {}".format( frame.dcm(parent_frame))) print("Linear velocity of joint frame wrt parent frame: {}". format(origin.vel(world_frame).express(parent_frame))) print("Angular velocity of joint frame wrt parent frame: {}". format(frame.ang_vel_in(parent_frame))) print("------------------------------------------------------") print("Position of joint frame wrt world frame: {}".format( origin.pos_from(world_origin))) print("Orientation of joint frame wrt world frame: {}".format( frame.dcm(world_frame).simplify())) print("Linear velocity of joint frame wrt world frame: {}". format(origin.vel(world_frame))) print("Angular velocity of joint frame wrt parent frame: {}". format(frame.ang_vel_in(world_frame))) print("------------------------------------------------------") # print("Local position of CoM wrt joint frame: {}".format(com.pos_from(origin))) # print("Local linear velocity of CoM wrt joint frame: {}".format(com.vel(worldFrame).express(frame))) # print("Local angular velocity of CoM wrt joint frame: {}".format(com_frame.ang_vel_in(frame))) # print("------------------------------------------------------") if joint_type == self.sim.JOINT_PRISMATIC: print("Input value (force): {}".format(loads[-1])) elif joint_type == self.sim.JOINT_REVOLUTE: print( "Input value (torque on previous and current bodies): {} and {}" .format(loads[-2], loads[-1])) print("") if verbose: print("Summary:") print("Generalized coordinates: {}".format(q)) print("Generalized speeds: {}".format(dq)) print("Inputs: {}".format(u)) print("Kinematic differential equations: {}".format(kd_eqs)) print("Bodies: {}".format(bodies)) print("Loads: {}".format(loads)) print("") # TODO: 1. account for external forces applied on different rigid-bodies (e.g. contact forces) # TODO: 2. account for constraints (e.g. holonomic, non-holonomic, etc.) # Get the Equation of Motion (EoM) using Kane's method kane = mechanics.KanesMethod(world_frame, q_ind=q, u_ind=dq, kd_eqs=kd_eqs) kane.kanes_equations(bodies=bodies, loads=loads) # get mass matrix and force vector (after simplifying) such that :math:`M(x,t) \dot{x} = f(x,t)` M = sympy.trigsimp(kane.mass_matrix_full) f = sympy.trigsimp(kane.forcing_full) # mechanics.find_dynamicsymbols(M) # mechanics.find_dynamicsymbols(f) # save useful info for future use (by other methods) constants = [g] constant_values = [9.81] parameters = (dict(zip(constants, constant_values))) self.symbols = { 'q': q, 'dq': dq, 'kane': kane, 'parameters': parameters } # linearize # parameters = dict(zip(constants, constant_values)) # M_, A_, B_, u_ = kane.linearize() # A_ = A_.subs(parameters) # B_ = B_.subs(parameters) # M_ = kane.mass_matrix_full.subs(parameters) # self.symbols = {'A_': A_, 'B_': B_, 'M_': M_} # return M_, A_, B_, u_ return M, f
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], u_dependent=[u_q1d], kd_eqs=kd_eqs, velocity_constraints=velocity_constraints) fr, frstar = kane.kanes_equations([particle_p], forceList) zero = fr + frstar f_c = point_pn.pos_from(particle_p.point).magnitude() - l config = sm.Matrix([[0]]) config[0] = f_c zero = zero.row_insert(zero.shape[0], sm.Matrix([[0]])) zero[zero.shape[0] - 1] = config[0]
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 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)
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 as sym import sympy.physics.mechanics as me # 定义变量 x, v = me.dynamicsymbols('x v') m, c, k, g, t = sym.symbols('m c k g t') # 定义坐标系 ceiling = me.ReferenceFrame('Ceil') # 定义关键点 O = me.Point('O') P = me.Point('P') # 定义点与坐标系的关系 O.set_vel(ceiling, 0) P.set_pos(O, x * ceiling.x) P.set_vel(ceiling, v * ceiling.x) # 外力的表达式,kane方程中将所有的重力视为外力 damping = -c * P.vel(ceiling) # 与天花板的速度 stiffness = -k * P.pos_from(O) # 与O点的距离 gravity = m * g * ceiling.x # 重力方向为x forces = damping + stiffness + gravity # 添加质量点 mass = me.particle('mass', P, m) # Kane方法求解 kane = me.KanesMethod(ceiling, q_ind=[x], u_ind=[v], kd_eqs=[v - x.diff(t)]) fr, frstar = kane.kanes_equations([mass], [(P, forces)]) M = kane.mass_matrix_full f = kane.forcing_full
u_ind = u[[0, 3, 4]].tolist() q_dep = q[[1, 2]].tolist() u_dep = u[[1, 2]].tolist() q_cons = [q[1] - q[0], q[2] - q[1]] u_cons = [u[1] - u[0], u[2] - u[1]] #q_cons = [0, 0] #u_cons = [0, 0] # equations of motion using Kane's method kane = me.KanesMethod(frame=setup['frames'][0], q_ind=q_ind, u_ind=u_ind, kd_eqs=fbd['kd'], q_dependent=q_dep, configuration_constraints=q_cons, u_dependent=u_dep, velocity_constraints=u_cons) (fr, frstar) = kane.kanes_equations(fbd['fl'], fbd['bodies']) kanezero = fr + frstar mass = kane.mass_matrix_full forcing = kane.forcing_full eom = dict(kane=kane, fr=fr, frstar=frstar, mass=mass, forcing=forcing, kanzero=kanezero)
def derive_sys(n,p): """ Derive the equations of motion using Kane's method Inputs: n - number of discrete elements to use in the model p - packed parameters to create symbolic material and physical equations Outputs: Symbolic, nolinear equations of motion for the discretized catheter model """ #------------------------------------------------- # Step 1: construct the catheter 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)) # Torques applied to each element due to external loads Torque = dynamicsymbols('tau:{0}'.format(n)) # Force applied at the end of the catheter by the user F_in = dynamicsymbols('F:{0}'.format(1)) # Unpack the system values M, L, E, I = p # Structural damping damp = 0.05 # Assuming that the lengths and masses are evenly distributed # (that is, the sytem is homogeneous), let's evenly divide the # Lengths and masses along each discrete member lengths = np.concatenate([np.broadcast_to(L / n,n)]) masses = np.concatenate([np.broadcast_to(M / n,n)]) # time symbol t = symbols('t') # The stiffness of the internal springs simulating material stiffness stiffness = E * I #-------------------------------------------------- # 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 = [] # Create a rotated reference frame for the first rigid link Ar = A.orientnew('A' + str(0), 'axis', [q[0],A.z]) # Create a point at the center of gravity of the first link Gr = P.locatenew('G' + str(0),(lengths[0] / 2) * Ar.x) Gr.v2pt_theory(P,A,Ar) # Create a point at the end of the link Pr = P.locatenew('P' + str(0), lengths[0] * Ar.x) Pr.v2pt_theory(P, A, Ar) # Create the inertia for the first rigid link Inertia_r = inertia(Ar,0,0,masses[0] * lengths[0]**2 / 12) # Create a new particle of mass m[i] at this point Par = mechanics.RigidBody('Pa' + str(0), Gr, Ar, masses[0], (Inertia_r,Gr)) particles.append(Par) # Add an internal spring based on Euler-Bernoulli Beam theory forces.append((Ar, -stiffness * (q[0]) / (lengths[0]) * Ar.z)) # Add a damping term forces.append((Ar, (-damp * u[0]) * Ar.z)) # Add a new ODE term kinetic_odes.append(q[0].diff(t) - u[0]) P = Pr for i in range(1,n): # Create a reference frame following the i^th link Ai = A.orientnew('A' + str(i), 'Axis', [q[i],Ar.z]) Ai.set_ang_vel(A, u[i] * Ai.z) # Set the center of gravity for this link Gi = P.locatenew('G' + str(i),lengths[i] / 2 * Ai.x) Gi.v2pt_theory(P,A,Ai) # Create a point in this reference frame Pi = P.locatenew('P' + str(i), lengths[i] * Ai.x) Pi.v2pt_theory(P, A, Ai) # Set the inertia for this link Inertia_i = inertia(Ai,0,0,masses[i] * lengths[i]**2 / 12) # Create a new particle of mass m[i] at this point Pai = mechanics.RigidBody('Pa' + str(i), Gi, Ai, masses[i], (Inertia_i,Gi)) particles.append(Pai) # The external torques influence neighboring links if i + 1 < n: next_torque = 0 for j in range(i,n): next_torque += Torque[j] else: next_torque = 0. forces.append((Ai,(Torque[i] + next_torque) * Ai.z)) # Add another internal spring forces.append((Ai, (-stiffness * (q[i] - q[i-1]) / (2 * lengths[i])) * Ai.z)) # Add the damping term forces.append((Ai, (-damp * u[i]) * Ai.z)) kinetic_odes.append(q[i].diff(t) - u[i]) P = Pi # Add the user-defined input at the tip of the catheter, pointing normal to the # last element forces.append((P, F_in[0] * Ai.y)) # 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) return KM, fr, fr_star, q, u, Torque, F_in, lengths, masses
body4 = me.RigidBody('b_4', P4, N3, m4, In4) body5 = me.RigidBody('b_5', P5, N5, m5, In5) body6 = me.RigidBody('b_6', P6, N6, m6, In6) kdes = [ q[0].diff() - u[0], q[1].diff() - u[1], q[2].diff() - u[2], q[3].diff() - u[3], q[4].diff() - u[4], q[5].diff() - u[5], q[6].diff() - u[6], q[7].diff() - u[7], q[8].diff() - u[8], q[9].diff() - u[9], q[10].diff() - u[10], q[11].diff() - u[11], q[12].diff() - u[12], q[13].diff() - u[13], q[14].diff() - u[14] ] KM = me.KanesMethod(N0, q[0:13], u[0:13], kdes, configuration_constraints=cfgconstr, q_dependent=[q[13], q[14]], u_auxiliary=[u[13], u[14]]) fr, frstar = KM.kanes_equations([body1, body2, body3, body4, body5, body6], loads) consts = { I1[0]: 1.0, I1[1]: 1.0, I1[2]: 1.0, I2[0]: 1.0, I2[1]: 1.0, I2[2]: 1.0, I3[0]: 1.0, I3[1]: 1.0,
# AB[_i].set_vel(N, AB[_i].vel(N).subs(qdots).simplify()) # BA[_i].set_vel(N, BA[_i].vel(N).subs(qdots).simplify()) rhs = [ eqs[0].rhs, eqs[1].rhs, eqs[2].rhs, eqs[3].rhs, eqs[4].rhs, eqs[5].rhs, eqs[6].rhs, eqs[7].rhs, eqs[8].rhs, eqs[9].rhs, eqs[10].rhs, eqs[11].rhs ] kdes = [ q[0].diff() - u[0], q[1].diff() - u[1], q[2].diff() - u[2], q[3].diff() - u[3], q[4].diff() - u[4], q[5].diff() - u[5], q[6].diff() - u[6], q[7].diff() - u[7], q[8].diff() - u[8], q[9].diff() - u[9], q[10].diff() - u[10], q[11].diff() - u[11] ] kane = me.KanesMethod(N, q, u, kd_eqs=kdes) Fr, Frst = kane.kanes_equations(bodies, loads=loads) sys = System(kane) sys.constants = { I[0]: 1.0, I[1]: 1.0, I[2]: 1.0, I[3]: 1.0, I[4]: 1.0, I[5]: 1.0, L[0]: 0.3, L[1]: 0.0, L[2]: 0.5,
Td = (D, T6 * C['2']) Te = (E, T7 * C['3']) forces = [Fco, Fdo, Feo, Ffo, Tc, Td, Te] ############### # Kane's Method ############### print("Generating Kane's equations.") kane = mec.KanesMethod( N, [q3, q4, q7], # yaw, roll, steer [u4, u6, u7], # roll rate, rear wheel rate, steer rate kd_eqs=kinematical, q_dependent=[q5], # pitch angle configuration_constraints=[holonomic], u_dependent=[u3, u5, u8], # yaw rate, pitch rate, front wheel rate velocity_constraints=nonholonomic) fr, frstar = kane.kanes_equations(forces, bodies) # Validation of non-linear equations print('Loading numerical input parameters.') from dtk import bicycle bp = bicycle.benchmark_parameters() mp = bicycle.benchmark_to_moore(bp)
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
frame_a = _me.ReferenceFrame('a') frame_b = _me.ReferenceFrame('b') frame_a.orient(frame_n, 'Axis', [q1, frame_n.z]) frame_b.orient(frame_n, 'Axis', [q2, frame_n.z]) frame_a.set_ang_vel(frame_n, u1*frame_n.z) frame_b.set_ang_vel(frame_n, u2*frame_n.z) point_o = _me.Point('o') particle_p = _me.Particle('p', _me.Point('p_pt'), _sm.Symbol('m')) particle_r = _me.Particle('r', _me.Point('r_pt'), _sm.Symbol('m')) particle_p.point.set_pos(point_o, l*frame_a.x) particle_r.point.set_pos(particle_p.point, l*frame_b.x) point_o.set_vel(frame_n, 0) particle_p.point.v2pt_theory(point_o,frame_n,frame_a) particle_r.point.v2pt_theory(particle_p.point,frame_n,frame_b) particle_p.mass = m particle_r.mass = m force_p = particle_p.mass*(g*frame_n.x) force_r = particle_r.mass*(g*frame_n.x) kd_eqs = [q1_d - u1, q2_d - u2] forceList = [(particle_p.point,particle_p.mass*(g*frame_n.x)), (particle_r.point,particle_r.mass*(g*frame_n.x))] kane = _me.KanesMethod(frame_n, q_ind=[q1,q2], u_ind=[u1, u2], kd_eqs = kd_eqs) fr, frstar = kane.kanes_equations([particle_p, particle_r], forceList) zero = fr+frstar from pydy.system import System sys = System(kane, constants = {l:1, m:1, g:9.81}, specifieds={}, initial_conditions={q1:.1, q2:.2, u1:0, u2:0}, times = _np.linspace(0.0, 10, 10/.01)) y=sys.integrate()
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 mb = sm.symbols('mb') body_b.mass = mb iaxx = 1/12*ma*(2*la)**2 iayy = iaxx iazz = 0 ibxx = 1/12*mb*h**2 ibyy = 1/12*mb*(w**2+h**2) ibzz = 1/12*mb*w**2 body_a.inertia = (me.inertia(body_a_f, iaxx, iayy, iazz, 0, 0, 0), body_a_cm) body_b.inertia = (me.inertia(body_b_f, ibxx, ibyy, ibzz, 0, 0, 0), body_b_cm) force_a = body_a.mass*(g*frame_n.z) force_b = body_b.mass*(g*frame_n.z) kd_eqs = [thetad - omega, phid - alpha] forceList = [(body_a.masscenter,body_a.mass*(g*frame_n.z)), (body_b.masscenter,body_b.mass*(g*frame_n.z))] kane = me.KanesMethod(frame_n, q_ind=[theta,phi], u_ind=[omega, alpha], kd_eqs = kd_eqs) fr, frstar = kane.kanes_equations([body_a, body_b], forceList) zero = fr+frstar from pydy.system import System sys = System(kane, constants = {g:9.81, lb:0.2, w:0.2, h:0.1, ma:0.01, mb:0.1}, specifieds={}, initial_conditions={theta:np.deg2rad(90), phi:np.deg2rad(0.5), omega:0, alpha:0}, times = np.linspace(0.0, 10, 10/0.02)) y=sys.integrate()
def example_pydy(ax=None, **options): """ Example from the documentation of :epkg:`pydy`. @param ax matplotlib axis @parm options extra options @return ax """ # part 1 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) # part 2 from numpy import linspace, sin from pydy.system import System sys = System(kane, constants={ mass: 1.0, stiffness: 1.0, damping: 0.2, gravity: 9.8 }, specifieds={ force: lambda x, t: sin(t) }, initial_conditions={ position: 0.1, speed: -1.0 }, times=linspace(0.0, 10.0, 1000)) y = sys.integrate() # part 3 import matplotlib.pyplot as plt if ax is None: _, ax = plt.subplots(nrows=1, ncols=1, figsize=options.get('figsize', (5, 5))) ax.plot(sys.times, y) ax.legend((str(position), str(speed))) return ax
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, ))
plate = me.RigidBody('plate', Bo, B, mB, IB) # forces # # add the gravitional force to each body rod_gravity = (Ao, mA * g * N.z) plate_gravity = (Bo, mB * g * N.z) # equations of motion with Kane's method # make a tuple of the bodies and forces bodies = (rod, plate) loads = (rod_gravity, plate_gravity) # create a Kane object with respect to the Newtonian reference frame kane = me.KanesMethod(N, q_ind=(theta, phi), u_ind=(omega, alpha), kd_eqs=kinDiffs) # calculate Kane's equations fr, frstar = kane.kanes_equations(loads, bodies) sys = System(kane) sys.constants = { lB: 0.2, # m h: 0.1, # m w: 0.2, # m mA: 0.01, # kg mB: 0.1, # kg g: 9.81, # m/s**2 }