def _create_rigid_bodies(self): self.rigid_bodies = OrderedDict() self.rigid_bodies['lthigh'] = \ me.RigidBody('lthigh', self.points['lthigh_mass_center'], self.frames['left_thigh'], self.parameters['thigh_mass'], self.central_inertias['lthigh']) self.rigid_bodies['lshank'] = \ me.RigidBody('lshank', self.points['lshank_mass_center'], self.frames['left_shank'], self.parameters['shank_mass'], self.central_inertias['lshank']) self.rigid_bodies['rthigh'] = \ me.RigidBody('rthigh', self.points['rthigh_mass_center'], self.frames['right_thigh'], self.parameters['thigh_mass'], self.central_inertias['rthigh']) self.rigid_bodies['rshank'] = \ me.RigidBody('rshank', self.points['rshank_mass_center'], self.frames['right_shank'], self.parameters['shank_mass'], self.central_inertias['rshank'])
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 _create_rigid_bodies(self): self.rigid_bodies = OrderedDict() self.rigid_bodies['leg'] = \ me.RigidBody('Leg', self.points['leg_mass_center'], self.frames['leg'], self.parameters['leg_mass'], self.central_inertias['leg']) self.rigid_bodies['torso'] = \ me.RigidBody('Torso', self.points['torso_mass_center'], self.frames['torso'], self.parameters['torso_mass'], self.central_inertias['torso'])
# linear velocities and accelerations # No.set_vel(N, 0) # the newtonian origin is fixed Ao.set_vel(N, omega * lA * A.x) Ao.a2pt_theory(No, N, A) Bo.set_vel(N, omega * lB * A.x) Bo.a2pt_theory(No, N, A) # kinematical differential equations # kinDiffs = [omega - thetad, alpha - phid] ## kinetics ## # rigid bodies # # create the empty rod object rod = me.RigidBody('rod', Ao, A, mA, (me.inertia(A, IAxx, IAxx, 0.0), Ao)) # create the empty plate object 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
# central inertia IAxx = sym.S(1) / 12 * mA * (2 * lA)**2 IAyy = IAxx IAzz = 0 IA = (me.inertia(A, IAxx, IAyy, IAzz), Ao) IBxx = sym.S(1) / 12 * mB * h**2 IByy = sym.S(1) / 12 * mB * (w**2 + h**2) IBzz = sym.S(1) / 12 * mB * w**2 IB = (me.inertia(B, IBxx, IByy, IBzz), Bo) # rigid bodies rod = me.RigidBody('rod', Ao, A, mA, IA) 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,
# 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) comBC_y = (L / 2) * sympy.sin(phi) comBC_vx = phiDot * comBC_x comBC_vy = phiDot * comBC_y comBC = mech.Point('comBC') comBC.set_vel(N, comBC_vx * N.x + comBC_vy * N.y) comBC.set_pos(B, comBC_x * N.x + comBC_y * N.y) # Create rigid body for blade BC I_BC = mech.inertia(N, 0, 0, Izz) rodBC = mech.RigidBody('rodBC', comBC, rodBCFrame, m, (I_BC, comBC)) # Get the kinetic energy of each component to check results mech.kinetic_energy(N, rodBC) # Set potential energies rodBC.potential_energy = (1 / 2) * kT * phi**2 # Get (unforced) Lagrangian of the system L = mech.Lagrangian(N, rodBC) # Get equation of motion LM = mech.LagrangesMethod(L, [rodBC], frame=N) LM.form_lagranges_equations()
# Now the origin and the center of mass of the ladder are defined. O = me.Point('O') A = me.Point('A') # And we use the length and angle to locate the center of mass relative to # the origin. A.set_pos(O, -l / 2 * sm.cos(q) * N.x + l / 2 * sm.sin(q) * N.y) # Take the derivative of the position of the center of mass to get the # corresponding velocity. O.set_vel(N, 0) A.set_vel(N, l / 2 * u * sm.sin(q) * N.x + l / 2 * u * sm.cos(q) * N.y) # The ladder can now be defined as a rigid body. ladder = me.RigidBody('ladder', A, L, m, (me.inertia(L, 0, 0, Izz), A)) # Set up all the inputs to Kane's method. kd = [u - qd] body_list = [ladder] force_list = [(A, -m * g * N.y)] # Finally we solve the dynamics kane = me.KanesMethod(N, q_ind=[q], u_ind=[u], kd_eqs=kd) kane.kanes_equations(force_list, body_list) # The mass matrix and the forcing function can be taken out of the # KanesMethod object. MM = kane.mass_matrix forcing = kane.forcing
def formulate_holonomic_snake(n): """Formulate the n-chain system, returning a bunch of sympy objects to make the equations of motion. Parameters ---------- n : int Number of links Returns ------- TODO: fixme These are acutally 4 dicts filled with lists q : list Generalized coordinates u : list Generalized speeds T : list Torques m : list Masses of each segment l : list Lengths of each segment Izz : list Moments of inertia about center of mass """ # dynamic variables q = me.dynamicsymbols('q:' + str(n + 2)) # (x, y, theta, phi_0, phi_1, ...) u = me.dynamicsymbols('u:' + str(n + 2)) c = me.dynamicsymbols('c:' + str(n - 1), 1) # derivative of constraint # variables for bodies g, t = sym.symbols('g t') m = sym.symbols('m:' + str(n)) l = sym.symbols('l:' + str(n)) Izz = sym.symbols('I_zz_:' + str(n)) # frames frames = [me.ReferenceFrame('fr_N')] for i in range(2, n + 2): name = 'fr_' + str(i) frame = frames[i].orientnew(name, 'Axis', [q[i], frames[i].z]) frame.set_ang_vel(frames[i], u[i] * frames[i].z) frames.append(frame) # origin of inertial frame joints = [me.Point('N_or')] joints[0].set_vel(frames[0], 0) # joints for i in range(n + 1): name = 'jt_' + str(i) if i == 0: joint = joints[i].locatenew( name, q[-2] * frames[0].x + q[-1] * frames[0].y) joint.set_vel(frames[0], u[-2] * frames[0].x + u[-1] * frames[0].y) else: joint = joints[i].locatenew(name, l[i - 1] * frames[i].x) joint.v2pt_theory(joints[i], frames[0], frames[i]) joints.append(joint) # mass centers mc = [] for i in range(n): name = 'mc_' + str(i) mcent = joints[i + 1].locatenew(name, l[i] / 2 * frames[i + 1].x) mcent.v2pt_theory(joints[i + 1], frames[0], frames[i + 1]) mc.append(mcent) # inertia dyads dyads = [] for i in range(n): dyad = (me.inertia(frames[i + 1], 0, 0, Izz[i]), mc[i]) dyads.append(dyad) # bodies bodies = [] for i in range(n): name = 'bd_' + str(i) body = me.RigidBody(name, mc[i], frames[i + 1], m[i], dyads[i]) bodies.append(body) # kinematic differential equations kd = [] for i in range(n + 2): kd.append(q[i].diff(t) - u[i]) forces = [] for i in range(n): forces.append((mc[i], -m[i] * g * frames[0].z)) torques = [] for i in range(n): torques.append((frames[i + 1], (T[i] - T[i + 1]) * frames[0].z)) fl = forces #+ torques # setup dictionaries of values to return dynamic = dict(q=q, u=u, T=T) symbol = dict(m=m, l=l, Izz=Izz, g=g, t=t) setup = dict(frames=frames, joints=joints, mc=mc, dyads=dyads) fbd = dict(kd=kd, fl=fl, bodies=bodies) return dynamic, symbol, setup, fbd
# (y-direction) # Create reference frame for analysis N = mech.ReferenceFrame('N') # Set origin O = mech.Point('O') O.set_vel(N, 0 * N.x + 0 * N.y + 0 * N.z) # Set point B B = mech.Point('B') B.set_vel(N, vB * N.y) # Create vertical rod (this one isn't moving) I1 = mech.inertia(N, 0, 0, 0) rod1 = mech.RigidBody('rod1', B, N, m, (I1, B)) # Create rod AB Izz = m * L**2 / 12 I2 = mech.inertia(N, 0, 0, Izz) rod2Frame = mech.ReferenceFrame('rod2Frame') rod2Frame.set_ang_vel(N, -thetaDot * N.z) rod2 = mech.RigidBody('rod2', B, rod2Frame, m, (I2, B)) # Get the kinetic energy of each component tp check results mech.kinetic_energy(N, rod1) mech.kinetic_energy(N, rod2) # Set potential energies deltaz = (L / 2) * (sympy.sin(theta) - 1 / 2) V1 = m * g * deltaz
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
Z_G.set_vel(B, e_dot * B.y) Z_G.v1pt_theory(G, N, B) Z_G.a1pt_theory(G, N, B) kde = [ x_dot - x.diff(t), y_dot - y.diff(t), beta_dot - beta.diff(t), e_dot - e.diff(t) ] I_plate = me.inertia(N, 0, 0, Izz) inertia_plate = (I_plate, G) I_rod = me.inertia(N, 0, 0, Izz_rod) inertia_rod = (I_rod, Z_G) Plate = me.RigidBody('Plate', G, B, M, inertia_plate) rod = me.RigidBody('rod', Z_G, B, m, inertia_rod) def Lengths_and_Moments(x, y): ''' This function determines the initial length of the springs and the moment needed in order to keep the plate in position. ''' xxx = x yyy = y a, b, x, y, H, k1, k2, Length1, Length2 = sympy.symbols( 'a b x y H k1 k2 Length1 Length2') m, Fsp1, Fsp2, Ma, t1, t2 = sympy.symbols('m Fsp1 Fsp2 Ma t1 t2') L1 = sympy.sqrt((x - (a / 2))**2 + (y - (b / 2))**2) L2 = sympy.sqrt((-(H - x) + (a / 2))**2 + (y - (b / 2))**2)
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 frame_a.set_ang_acc(frame_n, (frame_a.ang_vel_in(frame_n)).dt(frame_a)) v = 2 * frame_a.ang_acc_in(frame_n) + c2 * frame_a.y particle_p1.point.set_vel(frame_a, c1 * frame_a.x + c3 * frame_a.y) body_r_cm.set_acc(frame_n, c2 * frame_a.y) v_a = me.cross(body_r_cm.acc(frame_n), particle_p1.point.vel(frame_a)) x_b_c = v_a
point_c = me.Point("c") point_d = me.Point("d") point_po1 = me.Point("po1") point_po2 = me.Point("po2") point_po3 = me.Point("po3") particle_l = me.Particle("l", me.Point("l_pt"), sm.Symbol("m")) 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_p3 = me.Particle("p3", me.Point("p3_pt"), sm.Symbol("m")) body_s_cm = me.Point("s_cm") body_s_cm.set_vel(frame_n, 0) body_s_f = me.ReferenceFrame("s_f") body_s = me.RigidBody( "s", body_s_cm, body_s_f, sm.symbols("m"), (me.outer(body_s_f.x, body_s_f.x), body_s_cm), ) body_r1_cm = me.Point("r1_cm") body_r1_cm.set_vel(frame_n, 0) body_r1_f = me.ReferenceFrame("r1_f") body_r1 = me.RigidBody( "r1", body_r1_cm, body_r1_f, sm.symbols("m"), (me.outer(body_r1_f.x, body_r1_f.x), body_r1_cm), ) body_r2_cm = me.Point("r2_cm") body_r2_cm.set_vel(frame_n, 0)
v1 = x1 * frame_a.x + x2 * frame_a.y + x3 * frame_a.z v2 = x1 * frame_b.x + x2 * frame_b.y + x3 * frame_b.z v3 = x1 * frame_n.x + x2 * frame_n.y + x3 * frame_n.z v = v1 + v2 + v3 point_c = me.Point('c') point_d = me.Point('d') point_po1 = me.Point('po1') point_po2 = me.Point('po2') point_po3 = me.Point('po3') particle_l = me.Particle('l', me.Point('l_pt'), sm.Symbol('m')) 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_p3 = me.Particle('p3', me.Point('p3_pt'), sm.Symbol('m')) body_s_cm = me.Point('s_cm') body_s_cm.set_vel(frame_n, 0) body_s_f = me.ReferenceFrame('s_f') body_s = me.RigidBody('s', body_s_cm, body_s_f, sm.symbols('m'), (me.outer(body_s_f.x, body_s_f.x), body_s_cm)) body_r1_cm = me.Point('r1_cm') body_r1_cm.set_vel(frame_n, 0) body_r1_f = me.ReferenceFrame('r1_f') body_r1 = me.RigidBody('r1', body_r1_cm, body_r1_f, sm.symbols('m'), (me.outer(body_r1_f.x, body_r1_f.x), body_r1_cm)) body_r2_cm = me.Point('r2_cm') body_r2_cm.set_vel(frame_n, 0) body_r2_f = me.ReferenceFrame('r2_f') body_r2 = me.RigidBody('r2', body_r2_cm, body_r2_f, sm.symbols('m'), (me.outer(body_r2_f.x, body_r2_f.x), body_r2_cm)) v4 = x1 * body_s_f.x + x2 * body_s_f.y + x3 * body_s_f.z body_s_cm.set_pos(point_c, l * frame_n.x)
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
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"), (me.outer(body_b1_f.x, body_b1_f.x), body_b1_cm), ) body_b2_cm = me.Point("b2_cm") body_b2_cm.set_vel(frame_n, 0) body_b2_f = me.ReferenceFrame("b2_f") body_b2 = me.RigidBody( "b2", body_b2_cm, body_b2_f, sm.symbols("m"), (me.outer(body_b2_f.x, body_b2_f.x), body_b2_cm), ) g = sm.symbols("g", real=True) force_p1 = particle_p1.mass * (g * frame_n.x)
# Set up the kinematic differential equations kde = [ x_dot - x.diff(t), z_dot - z.diff(t), e_dot - e.diff(t), theta_dot - theta.diff(t), phi_dot - phi.diff(t) ] # Create the plate inertial tensor I_plate = me.inertia(A, 0, Ip, 0) inertia_plate = (I_plate, G) # Create the rod inertial tensor I_rod = me.inertia(A, 0, Ir, 0) inertia_rod = (I_rod, Gr) # Create the Rigid Bodies Plate = me.RigidBody('Plate', G, B, M, inertia_plate) Rod = me.RigidBody('Rod', Gr, C, m, inertia_rod) # In[5]: # These functions do not allow the springs or dampers to push K1 = lambda LamLenK1: k * (LamLenK1 >= L1) K2 = lambda LamLenK2: k * (LamLenK2 >= L2) C1 = lambda LamLenC1: c * (LamLenC1 >= L1) C2 = lambda LamLenC2: c * (LamLenC2 >= L2) # In[6]: # Creating the forces acting on the body grav_force_plate = (G, M * g * A.z) grav_force_rod = (Gr, m * g * A.z)
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', masscenter=O, frame=S, mass=mass, inertia=body_central_inertia) fx, fy, fz, mx, my, mz = sp.symbols('f_x f_y f_z m_x m_y m_z') force_vector = fx * S.x + fy * S.y + fz * S.z 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,
B = A.orientnew('B', 'Body', (q[6], q[7], q[8]), '123') B.set_ang_vel(A, u[6] * A.x + u[7] * A.y + u[8] * A.z) CMb = CMa.locatenew('CM_b', q[9] * A.x + q[10] * A.y + q[11] * A.z) CMb.set_vel(A, u[9] * A.x + u[10] * A.y + u[11] * A.z) CMb.set_vel(N, CMb.vel(A).express(N)) BA = [ CMb.locatenew('BA_0', L[36] * B.x - L[37] * B.y + L[38] * B.z), CMb.locatenew('BA_1', -L[39] * B.x - L[40] * B.y + L[41] * B.z), CMb.locatenew('BA_2', -L[42] * B.x - L[43] * B.y - L[44] * B.z), CMb.locatenew('BA_3', L[45] * B.x - L[46] * B.y - L[47] * B.z) ] In = [me.inertia(A, I[0], I[1], I[2]), me.inertia(B, I[3], I[4], I[5])] bodies = [ me.RigidBody('Shell', CMa, A, ma, (In[0], CMa)), me.RigidBody('Block', CMb, B, mb, (In[1], CMb)) ] loads = [(CMa, -ma * g * N.y), (CMb, -mb * g * N.y)] # CMa.set_vel(N, CMa.pos_from(Orig).dt(N)) # CMb.set_vel(N, CMb.pos_from(Orig).dt(N)) # CMb.set_vel(A, CMb.pos_from(CMa).dt(A)) for _i in range(4): OA[_i].v2pt_theory(Orig, N, N) AO[_i].v2pt_theory(CMa, N, A) AB[_i].v2pt_theory(CMa, N, A) BA[_i].v2pt_theory(CMb, N, B) BA[_i].v2pt_theory(CMb, A, B)
cg[1] * P10[i].vel(N0).dot(N0.y) * N0.y + cg[2] * P10[i].vel(N0).dot(N0.z) * N0.z))) loads.append((P12[i], -(cs[0] * P12[i].vel(N0).dot(N0.x) * N0.x + cs[1] * P12[i].vel(N0).dot(N0.y) * N0.y + cs[2] * P12[i].vel(N0).dot(N0.z) * N0.z))) loads.append((P21[i], -(cs[0] * P21[i].vel(N0).dot(N0.x) * N0.x + cs[1] * P21[i].vel(N0).dot(N0.y) * N0.y + cs[2] * P21[i].vel(N0).dot(N0.z) * N0.z))) np.vstack = (me.inertia(N2, I2[0], I2[1], I2[2]), P2) In3 = (me.inertia(N3, I3[0], I3[1], I3[2]), P3) In4 = (me.inertia(N3, I4[0], I4[1], I4[2]), P4) In5 = (me.inertia(N5, I5[0], I5[1], I5[2]), P5) In6 = (me.inertia(N6, I6[0], I6[1], I6[2]), P6) body1 = me.RigidBody('b_1', P1, N1, m1, In1) body2 = me.RigidBody('b_2', P2, N2, m2, In2) body3 = me.RigidBody('b_3', P3, N3, m3, In3) 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,
# The linear velocities can be specified the same way as the bob points. P_link1.v2pt_theory(O, I, A) P_link2.v2pt_theory(P1, I, B) P_link3.v2pt_theory(P2, I, C) points_rigid_body = [P_link1, P_link2, P_link3] # The inertia tensors for the links are defined with respect to the mass # center of the link and the link's reference frame. inertia_link1 = (me.inertia(A, Ixx[0], Iyy[0], Izz[0]), P_link1) inertia_link2 = (me.inertia(B, Ixx[1], Iyy[1], Izz[1]), P_link2) inertia_link3 = (me.inertia(C, Ixx[2], Iyy[2], Izz[2]), P_link3) # Now rigid bodies can be created for each link. link1 = me.RigidBody('link1', P_link1, A, m_link[0], inertia_link1) link2 = me.RigidBody('link2', P_link2, B, m_link[1], inertia_link2) link3 = me.RigidBody('link3', P_link3, C, m_link[2], inertia_link3) links = [link1, link2, link3] # The only contributing forces to the system is the force due to gravity # acting on each particle and body. forces = [] for particle in particles: mass = particle.get_mass() point = particle.get_point() forces.append((point, -mass * g * I.y)) for link in links: mass = link.get_mass()
P.set_pos(O2, -(H - x) * N.x + y * N.y) # P.set_pos(O1, x * N.x + y * N.y) # Set the point's velocity P.set_vel(N, x_dot * N.x + y_dot * N.y) # Create the rod center of mass G = P.locatenew('G', D / 2 * B.y) # Set the velocity of G G.v2pt_theory(P, N, B) # Create the rod I_rod = me.inertia(B, 0, 0, Izz) rod = me.RigidBody('rod', G, B, m, (I_rod, G)) # Create the distance from the point to each attachment point L1 = O1.pos_from(P).magnitude L2 = O2.pos_from(P).magnitude L1_vector = O1.pos_from(P).normalize L2_vector = O2.pos_from(P).normalize # Create the height from the center of gravity to the datum h = G.pos_from(O1) & N.y # The forces at the connection point forceP = c * (x_dot + y_dot) * L1_vector() + c * (-x_dot + y_dot) * L2_vector() # The forces on the beta frame forceB = c_beta * beta_dot * N.z
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
# Setting up the total x equation Fy = Fy1 + Fy2 - 9.81*m # try 2 * Fy to make Fy's equal Totalx = Fx1 - Fx2 Totaly = Fy1 + Fy2 - 9.81*m Totalx = (Fx.subs({x:xxx, y:yyy, a:4.0,b:2.0, H:20.0, k1:100.0, k2:100.0})).evalf() Totaly = (Fy.subs({x:xxx, y:yyy, a:4.0,b:2.0, H:20.0, k1:100.0, k2:100.0, m:12.0})).evalf() abv = sympy.solve([sympy.Eq(Totalx, 0.0), sympy.Eq(Totaly, 0.0)], [Fsp1,Fsp2]) l1 = abv[Fsp1] l2 = abv[Fsp2] return l1,l2 I_plate = me.inertia(N, 0, 0, Izz) inertia_plate = (I_plate, G) Plate = me.RigidBody('Plate', G, B, M, inertia_plate) grav_force_plate = (G, -M * g * N.y) Length1 = P1.pos_from(O1).magnitude() Length2 = P2.pos_from(O2).magnitude() P1_vector = P1.pos_from(O1).normalize() P2_vector = P2.pos_from(O2).normalize() left_force = (P1, -(F1*P1_vector)) right_force = (P2, -(F2*P2_vector)) coordinates = [x, y, beta] speeds= [x_dot, y_dot, beta_dot]
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])
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)
holonomic.diff(mec.dynamicsymbols._t).subs({q4d:u4, q5d:u5, q7d:u7})] ######### # Inertia ######### Ic = mec.inertia(C, ic11, ic22, ic33, 0.0, 0.0, ic31) Id = mec.inertia(D, id11, id22, id11, 0.0, 0.0, 0.0) Ie = mec.inertia(E, ie11, ie22, ie33, 0.0, 0.0, ie31) If = mec.inertia(F, if11, if22, if11, 0.0, 0.0, 0.0) ############## # Rigid Bodies ############## rearFrame = mec.RigidBody('Rear Frame', co, C, mc, (Ic, co)) rearWheel = mec.RigidBody('Rear Wheel', do, D, md, (Id, do)) frontFrame = mec.RigidBody('Front Frame', eo, E, me, (Ie, eo)) frontWheel = mec.RigidBody('Front Wheel', fo, F, mf, (If, fo)) bodyList = [rearFrame, rearWheel, frontFrame, frontWheel] ########################### # Generalized Active Forces ########################### # gravity Fco = (co, mc * g * A['3']) Fdo = (do, md * g * A['3']) Feo = (eo, me * g * A['3']) Ffo = (fo, mf * g * A['3'])
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'), (me.outer(body_b1_f.x, body_b1_f.x), body_b1_cm)) body_b2_cm = me.Point('b2_cm') body_b2_cm.set_vel(frame_n, 0) body_b2_f = me.ReferenceFrame('b2_f') body_b2 = me.RigidBody('b2', body_b2_cm, body_b2_f, sm.symbols('m'), (me.outer(body_b2_f.x, body_b2_f.x), body_b2_cm)) g = sm.symbols('g', real=True) force_p1 = particle_p1.mass * (g * frame_n.x) force_p2 = particle_p2.mass * (g * frame_n.x) force_b1 = body_b1.mass * (g * frame_n.x) force_b2 = body_b2.mass * (g * frame_n.x) z = me.dynamicsymbols('z') v = x * frame_a.x + y * frame_a.z point_o.set_pos(point_p, x * frame_a.x + y * frame_a.y) v = (v).subs({x: 2 * z, y: z}) point_o.set_pos(point_p, (point_o.pos_from(point_p)).subs({x: 2 * z, y: z}))
def system_model_generator(ct, param_deviation=False): ''' Modeling inverted pendulum using Kane's Method ''' # Defining symbolic Variables n = ct.number_of_pendulums q = me.dynamicsymbols('q:{}'.format(n + 1)) # generalized coordinates qdot = me.dynamicsymbols('qdot:{}'.format(n + 1)) #generalized speeds qdd = me.dynamicsymbols('qddot:{}'.format(n + 1)) f = me.dynamicsymbols('f') u = sm.symbols('u') m = sm.symbols('m:{}'.format(n + 1)) J = sm.symbols('J:{}'.format(n + 1)) l = sm.symbols('l1:{}'.format(n + 1)) # lenght of each pendlum a = sm.symbols('a1:{}'.format(n + 1)) #location of Mass-centers d = sm.symbols('d1:{}'.format(n + 1)) #viscous damping coef. g, t = sm.symbols('g t') dynamic_symbs = q + qdot + [u] # intertial reference frame In_frame = me.ReferenceFrame('In_frame') # Origninal Point O in Reference Frame : O = me.Point('O') O.set_vel(In_frame, 0) # The Center of Mass Point on cart : C0 = me.Point('C0') C0.set_pos(O, q[0] * In_frame.x) C0.set_vel(In_frame, qdot[0] * In_frame.x) cart_inertia_dyadic = me.inertia(In_frame, 0, 0, J[0]) cart_central_inertia = (cart_inertia_dyadic, C0) cart = me.RigidBody('Cart', C0, In_frame, m[0], cart_central_inertia) kindiffs = [q[0].diff(t) - qdot[0]] # entforcing qdot=Omega frames = [In_frame] mass_centers = [C0] joint_centers = [C0] central_inertias = [cart_central_inertia] forces = [(C0, f * In_frame.x - m[0] * g * In_frame.y)] # torques = [] # cart_potential = 1 / 3 * d[0] * qdot[1]**3 # potentials = [cart_potential] # cart.potential_energy= cart_potential rigid_bodies = [cart] # Lagrangian0 = me.Lagrangian(In_frame, rigid_bodies[0]) # Lagrangians=[Lagrangian0] for i in range(n): #Creating new reference frame Li = In_frame.orientnew('L' + str(i), 'Axis', [q[i + 1], In_frame.z]) Li.set_ang_vel(In_frame, qdot[i + 1] * In_frame.z) frames.append(Li) # Creating new Points representing mass_centers Pi = joint_centers[-1].locatenew('a' + str(i + 1), a[i] * Li.y) Pi.v2pt_theory(joint_centers[-1], In_frame, Li) mass_centers.append(Pi) #Creating new Points representing Joints Jointi = joint_centers[-1].locatenew('jont' + str(i + 1), l[i] * Li.y) Jointi.v2pt_theory(joint_centers[-1], In_frame, Li) joint_centers.append(Jointi) #adding forces forces.append((Pi, -m[i + 1] * g * In_frame.y)) #adding cential inertias IDi = me.inertia(frames[i + 1], 0, 0, J[i + 1]) ICi = (IDi, mass_centers[i + 1]) central_inertias.append(ICi) LBodyi = me.RigidBody('L' + str(i + 1) + '_Body', mass_centers[i + 1], frames[i + 1], m[i + 1], central_inertias[i + 1]) rigid_bodies.append(LBodyi) kindiffs.append(q[i + 1].diff(t) - qdot[i + 1]) # add torques : if n == 1: torqueVector1 = -(d[0] * qdot[1]) * In_frame.z torques = [(frames[1], torqueVector1)] elif n == 2: torqueVector1 = -(d[0] * qdot[1] - d[1] * (qdot[1] - qdot[2])) * In_frame.z torqueVector2 = -(d[1] * (qdot[2] - qdot[1])) * In_frame.z torques = [(frames[1], torqueVector1), (frames[2], torqueVector2)] elif n == 3: torqueVector1 = (-d[0] * qdot[1] - d[1] * (qdot[1] - qdot[2])) * In_frame.z torqueVector2 = (-d[1] * (qdot[2] - qdot[1]) - d[2] * (qdot[2] - qdot[3])) * In_frame.z torqueVector3 = -d[2] * (qdot[3] - qdot[2]) * In_frame.z torques = [(frames[1], torqueVector1), (frames[2], torqueVector2), (frames[3], torqueVector3)] #generalized force loads = torques + forces #Kane's Method --> Equation of motion Kane = me.KanesMethod(In_frame, q, qdot, kd_eqs=kindiffs) fr, frstar = Kane.kanes_equations(rigid_bodies, loads) mass_matrix = sm.trigsimp(Kane.mass_matrix_full) forcing_vector = sm.trigsimp(Kane.forcing_full) #xdot_expr=(mass_matrix.inv()*forcing_vector) # defining parameter values according to number of pendulum n : if param_deviation == True: param_dict_with_deviation = ct.model.param_dict_with_deviation param_values = [param_dict_with_deviation[str(li)] for li in l] + [ param_dict_with_deviation[str(ai)] for ai in a ] + [param_dict_with_deviation[str(mi)] for mi in m] + [ param_dict_with_deviation[str(Ji)] for Ji in J ] + [param_dict_with_deviation[str(di)] for di in d] + [param_dict_with_deviation['g'] ] + [param_dict_with_deviation['f']] else: param_values = ct.parameter_values param_symb = list(l + a + m + J + d + (g, f)) param_list = zip(param_symb, param_values) param_dict = dict(param_list) # substituting parameters to mass and forcing_vector mass_matrix_simplified = mass_matrix.subs(param_dict).simplify() forcing_vector_simplified = forcing_vector.subs(param_dict).simplify() # finding fx and gx wiht qdd0 as input mass_matrix44 = mass_matrix_simplified[len(q):, len(q):] forcing_vector44 = forcing_vector_simplified[len(q):, 0] fx, gx = generate_state_equ(mass_matrix44, forcing_vector44, dynamic_symbs) print('system model succesfully finished !') # returning the model in a container : ct.model.t = t ct.model.q = q ct.model.qdot = qdot ct.model.qdd = qdd ct.model.dynamic_symbs = dynamic_symbs ct.model.f = f ct.model.u = u ct.model.m = m ct.model.J = J ct.model.l = l ct.model.a = a ct.model.d = d ct.model.g = g ct.model.param_dict = param_dict ct.model.frames = frames ct.model.mass_centers = mass_centers ct.model.origin_point = O ct.model.joint_centers = joint_centers ct.model.central_inertias = central_inertias ct.model.rigid_bodies = rigid_bodies ct.model.forces = forces ct.model.torques = torques ct.model.loads = loads ct.model.kindiffs = kindiffs ct.model.fr = fr ct.model.fstar = frstar ct.model.mass_matrix = mass_matrix ct.model.mass_matrix_simplified = mass_matrix_simplified ct.model.forcing_vector = forcing_vector ct.model.forcing_vector_simplified = forcing_vector_simplified ct.model.fx = fx ct.model.gx = gx # thats just a trick to be able to dump the results (there is a problem with dumping classes outside of the main!) for qi in q: qi.__class__.__module__ = '__main__' for qdoti in qdot: qdoti.__class__.__module__ = '__main__' for qddi in qdd: qddi.__class__.__module__ = '__main__' f.__class__.__module__ = '__main__' label = ct.label # storing system model as binary file to be used later param_tol_dict = ct.model.param_tol_dict if param_deviation else None default_tol = ct.model.default_tol if param_deviation else '0.0' sys_model = { 'fx': fx, 'gx': gx, 'q': q, 'qdot': qdot, 'qdd': qdd, 'dynamic_symbs': dynamic_symbs, 'param_dict': param_dict, 'param_tol_dict': param_tol_dict, 'default_tol': default_tol } if param_deviation == True: model_file_name = ct.model.model_file_name else: model_file_name = 'sys_model_' + 'without_param_deviation_' + label + '.pkl' dump_model(model_file_name, sys_model)