#Sets up the angular velocities omega1, omega2 = dynamicsymbols('omega1, omega2') #Relates angular velocity values to the angular positions theta1 and theta2 kinematic_differential_equations = [omega1 - theta1.diff(), omega2 - theta2.diff()] #Sets up the rotational axes of the angular velocities leg_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z) leg_frame.ang_vel_in(inertial_frame) body_frame.set_ang_vel(leg_frame, omega2*inertial_frame.z) body_frame.ang_vel_in(inertial_frame) #Sets up the linear velocities of the points on the linkages ankle.set_vel(inertial_frame, 0) waist.v2pt_theory(ankle, inertial_frame, leg_frame) waist.vel(inertial_frame) body.v2pt_theory(waist, inertial_frame, body_frame) body.vel(inertial_frame) #Sets up the masses of the linkages leg_mass, body_mass = symbols('m_L, m_B') #Defines the linkages as particles waistP = Particle('waistP', waist, leg_mass) bodyP = Particle('bodyP', body, body_mass) #Sets up gravity information and assigns gravity to act on mass centers g = symbols('g') leg_grav_force_vector = -1*leg_mass*g*inertial_frame.y leg_grav_force = (waist, leg_grav_force_vector)
#Relates angular velocity values to the angular positions theta1 and theta2 kinematic_differential_equations = [omega_a - theta_a.diff(), omega_b - theta_b.diff(), omega_c - theta_c.diff()] #Sets up the rotational axes of the angular velocities a_frame.set_ang_vel(inertial_frame, omega_a*inertial_frame.z) a_frame.ang_vel_in(inertial_frame) b_frame.set_ang_vel(a_frame, omega_b*inertial_frame.z) b_frame.ang_vel_in(inertial_frame) c_frame.set_ang_vel(b_frame, omega_c*inertial_frame.z) c_frame.ang_vel_in(inertial_frame) #Sets up the linear velocities of the points on the linkages A.set_vel(inertial_frame, 0) B.v2pt_theory(A, inertial_frame, a_frame) B.vel(inertial_frame) C.v2pt_theory(B, inertial_frame, b_frame) C.vel(inertial_frame) D.v2pt_theory(C, inertial_frame, c_frame) D.vel(inertial_frame) #Sets up the masses of the linkages a_mass, b_mass, c_mass = symbols('m_a, m_b, m_c') #Defines the linkages as particles bP = Particle('bP', B, a_mass) cP = Particle('cP', C, b_mass) dP = Particle('dP', D, c_mass) #Sets up gravity information and assigns gravity to act on mass centers
#kinematic differential equations #init var for generalized speeds (aka angular velocities) omega0, omega1, omega2 = dynamicsymbols('omega0, omega1, omega2') kinematical_differential_equations = [ omega0 - theta0.diff(), omega1 - theta1.diff(), omega2 - theta2.diff() ] pretty_print(kinematical_differential_equations) j0_frame.set_ang_vel(inertial_frame, omega0 * j0_frame.y) j1_frame.set_ang_vel(j0_frame, omega1 * j0_frame.z) j2_frame.set_ang_vel(j1_frame, omega2 * j1_frame.x) #set base link fixed to ground plane joint0.set_vel(inertial_frame, 0) #get linear velocities of each point j0_mass_center.v2pt_theory(joint0, inertial_frame, j0_frame) joint1.v2pt_theory(joint0, inertial_frame, j0_frame) j1_mass_center.v2pt_theory(joint1, inertial_frame, j1_frame) joint2.v2pt_theory(joint1, inertial_frame, j1_frame) j2_mass_center.v2pt_theory(joint2, inertial_frame, j2_frame) EE.v2pt_theory(joint2, inertial_frame, j2_frame) print("finished Kinematics") #Inertia--------------------------------------------------------------- j0_mass, j1_mass, j2_mass = symbols('m_j0, m_j1, m_j2') j0_inertia, j1_inertia, j2_inertia = symbols('I_j0, I_j1, I_j2') #inertia values taken from CAD model (same as simscape multibody) [kg*m^2] #<joint>.inertia(frame, ixx, iyy, izz, ixy = _, iyz= _, izx = _) j0_inertia_dyadic = inertia(
S = Point('S') s_length = symbols('l_s') T = Point('T') T.set_pos(S, s_length*s_frame.y) #Sets up the angular velocities omega_s = dynamicsymbols('omega_s') #Relates angular velocity values to the angular positions theta1 and theta2 kinematic_differential_equations = [omega_s - theta_s.diff()] #Sets up the rotational axes of the angular velocities s_frame.set_ang_vel(inertial_frame, omega_s*inertial_frame.z) #Sets up the linear velocities of the points on the linkages S.set_vel(inertial_frame, 0) T.v2pt_theory(S, inertial_frame, s_frame) #Sets up the masses of the linkages s_mass = symbols('m_s') #Defines the linkages as particles tP = Particle('tP', T, s_mass) #Sets up gravity information and assigns gravity to act on mass centers g = symbols('g') t_grav_force_vector = -1*s_mass*g*inertial_frame.y t_grav_force = (T, t_grav_force_vector) #Sets up joint torques s_torque = dynamicsymbols('T_s') s_torque_vector = s_torque*inertial_frame.z
def get_model(model_name, **opts): """ model: string defining model opts: dictionary of options with keys: see _defaultOpts """ for k, v in _defaultOpts.items(): if k not in opts.keys(): opts[k] = v for k, v in opts.items(): if k not in _defaultOpts.keys(): raise Exception( 'Key {} not supported for model options.'.format(k)) #print(opts) # Extract info from model name sFnd = model_name.split('T')[0][1:] if len(sFnd) == 1: nDOF_fnd = int(sFnd[0]) bFndDOFs = [False] * 6 bFndDOFs[0] = nDOF_fnd >= 1 # x bFndDOFs[4] = nDOF_fnd >= 2 # phiy bFndDOFs[2] = nDOF_fnd == 3 or nDOF_fnd == 6 # z bFndDOFs[1] = nDOF_fnd >= 5 # y bFndDOFs[3] = nDOF_fnd >= 5 # phi_x bFndDOFs[5] = nDOF_fnd >= 5 # phi_z else: bFndDOFs = [s == '1' for s in sFnd] nDOF_fnd = sum(bFndDOFs) nDOF_twr = int(model_name.split('T')[1][0]) bFullRNA = model_name.find('RNA') == -1 bBlades = False # Rotor nDOF_nac = 0 nDOF_sft = 0 nDOF_bld = 0 if bFullRNA: nDOF_nac = int(model_name.split('N')[1][0]) nDOF_sft = int(model_name.split('S')[1][0]) bBlades = model_name.find('B') > 0 if bBlades: nDOF_bld = model_name.split('B')[1][0] else: nDOF_bld = 0 #print('fnd',','.join(['1' if b else '0' for b in bFndDOFs]), 'twr',nDOF_twr, 'nac',nDOF_nac, 'sft',nDOF_sft, 'bld',nDOF_bld) # --------------------------------------------------------------------------------} # --- Isolated bodies # --------------------------------------------------------------------------------{ # Reference frame ref = YAMSInertialBody('E') # Foundation, floater, always rigid for now if (not opts['floating']) or opts['mergeFndTwr']: fnd = None # the floater is merged with the twr, or we are not floating else: fnd = YAMSRigidBody('F', rho_G=[0, 0, z_FG], J_diag=True) # Tower if nDOF_twr == 0: # Ridid tower twr = YAMSRigidBody('T', rho_G=[0, 0, z_TG], J_diag=True) twrDOFs = [] twrSpeeds = [] elif nDOF_twr <= 4: # Flexible tower twr = YAMSFlexibleBody('T', nDOF_twr, directions=opts['twrDOFDir'], orderMM=opts['orderMM'], orderH=opts['orderH'], predefined_kind='twr-z') twrDOFs = twr.q twrSpeeds = twr.qd # Nacelle rotor assembly if bFullRNA: # Nacelle nac = YAMSRigidBody('N', rho_G=[x_NG, 0, z_NG], J_cross=True) # Shaft #... # Individual blades or rotor if bBlades: # Individual blades raise NotImplementedError() else: # Rotor rot = YAMSRigidBody('R', rho_G=[0, 0, 0], J_diag=True) rot.inertia = (inertia(rot.frame, Jxx_R, JO_R, JO_R), rot.origin ) # defining inertia at orign else: # Nacelle #nac = YAMSRigidBody('RNA', rho_G = [x_RNAG ,0, z_RNAG], J_diag=True) nac = YAMSRigidBody('RNA', rho_G=[x_RNAG, 0, z_RNAG], J_cross=True) rot = None # --------------------------------------------------------------------------------} # --- Body DOFs # --------------------------------------------------------------------------------{ # Fnd if (not opts['floating']): fndDOFs = [] fndSpeeds = [] else: fndDOFsAll = [x, y, z, phi_x, phi_y, phi_z] fndSpeedsAll = [xd, yd, zd, omega_x_T, omega_y_T, omega_z_T] fndDOFs = [dof for active, dof in zip(bFndDOFs, fndDOFsAll) if active] fndSpeeds = [ dof for active, dof in zip(bFndDOFs, fndSpeedsAll) if active ] # Twr # Nac if nDOF_nac == 2: opts['yaw'] = 'dynamic' opts['tilt'] = 'dynamic' if opts['tiltShaft'] and opts['tilt'] == 'dynamic': raise Exception('Cannot do tiltshaft with tilt dynamic') yawDOF = {'zero': 0, 'fixed': theta_yaw, 'dynamic': q_yaw}[opts['yaw']] tiltDOF = {'zero': 0, 'fixed': theta_tilt, 'dynamic': q_tilt}[opts['tilt']] nacDOFs = [] nacSpeeds = [] nacKDEqSubs = [] if opts['yaw'] == 'dynamic': nacDOFs += [q_yaw] nacSpeeds += [qd_yaw] nacKDEqSubs += [(qd_yaw, diff(q_yaw, time))] if opts['tilt'] == 'dynamic': nacDOFs += [q_tilt] nacSpeeds += [qd_tilt] nacKDEqSubs += [(qd_tilt, diff(q_tilt, time))] nacDOFsAct = (opts['yaw'] == 'dynamic', opts['tilt'] == 'dynamic') if nDOF_nac == 0: if not (nacDOFsAct == (False, False)): raise Exception( 'If nDOF_nac is 0, yaw and tilt needs to be "fixed" or "zero"') elif nDOF_nac == 1: if not (nacDOFsAct == (True, False) or nacDOFsAct == (False, True)): raise Exception( 'If nDOF_nac is 1, yaw or tilt needs to be "dynamic"') else: if not (nacDOFsAct == (True, True)): raise Exception( 'If nDOF_nac is 2, yaw and tilt needs to be "dynamic"') # Shaft sftDOFs = [] sftSpeeds = [] if bFullRNA: if nDOF_sft == 1: sftDOFs = [psi] sftSpeeds = [omega_x_R] elif nDOF_sft == 0: pass else: raise Exception('nDOF shaft should be 0 or 1') # Blade Rotor if bBlades: pass else: pass coordinates = fndDOFs + twrDOFs + nacDOFs + sftDOFs speeds = fndSpeeds + twrSpeeds + nacSpeeds + sftSpeeds # Order determine eq order # --------------------------------------------------------------------------------} # --- Connections between bodies # --------------------------------------------------------------------------------{ z_OT = Symbol('z_OT') if opts['floating']: rel_pos = [0, 0, 0] rel_pos[0] = x if bFndDOFs[0] else 0 rel_pos[1] = y if bFndDOFs[1] else 0 rel_pos[2] = z + z_OT if bFndDOFs[2] else z_OT rots = [0, 0, 0] rots[0] = phi_x if bFndDOFs[3] else 0 rots[1] = phi_y if bFndDOFs[4] else 0 rots[2] = phi_z if bFndDOFs[5] else 0 if nDOF_fnd == 0: #print('Rigid connection ref twr', rel_pos) ref.connectTo(twr, type='Rigid', rel_pos=rel_pos) elif nDOF_fnd == 1: #print('Constraint connection ref twr') ref.connectTo(twr, type='Free', rel_pos=(x, 0, z_OT), rot_amounts=(0, x * symbols('nu'), 0), rot_order='XYZ') else: #print('Free connection ref twr', rel_pos, rots) ref.connectTo( twr, type='Free', rel_pos=rel_pos, rot_amounts=rots, rot_order='XYZ' ) #NOTE: rot order is not "optimal".. phi_x should be last #ref.connectTo(twr, type='Free' , rel_pos=rel_pos, rot_amounts=(rots[2],rots[1],rots[0]), rot_order='ZYX') #NOTE: rot order is not "optimal".. phi_x should be last else: #print('Rigid connection ref twr') ref.connectTo(twr, type='Rigid', rel_pos=(0, 0, 0)) # Rigid connection between twr and fnd if fnd exists if fnd is not None: #print('Rigid connection twr fnd') if nDOF_twr == 0: twr.connectTo(fnd, type='Rigid', rel_pos=(0, 0, 0)) # -L_F else: twr.connectTo(fnd, type='Rigid', rel_pos=(0, 0, 0)) # -L_F if nDOF_twr == 0: # Tower rigid -> Rigid connection to nacelle # TODO TODO L_T or twr.L #if nDOF_nac==0: #print('Rigid connection twr nac') #else: #print('Dynamic connection twr nac') if opts['tiltShaft']: # Shaft will be tilted, not nacelle twr.connectTo(nac, type='Rigid', rel_pos=(0, 0, L_T), rot_amounts=(yawDOF, 0, 0), rot_order='ZYX') else: # Nacelle is tilted twr.connectTo(nac, type='Rigid', rel_pos=(0, 0, L_T), rot_amounts=(yawDOF, tiltDOF, 0), rot_order='ZYX') else: # Flexible tower -> Flexible connection to nacelle #print('Flexible connection twr nac') if opts['tiltShaft']: twr.connectToTip(nac, type='Joint', rel_pos=(0, 0, twr.L), rot_amounts=(yawDOF, 0, 0), rot_order='ZYX', rot_type_elastic=opts['rot_elastic_type'], doSubs=opts['rot_elastic_subs']) else: twr.connectToTip(nac, type='Joint', rel_pos=(0, 0, twr.L), rot_amounts=(yawDOF, tiltDOF, 0), rot_order='ZYX', rot_type_elastic=opts['rot_elastic_type'], doSubs=opts['rot_elastic_subs']) if bFullRNA: if bBlades: raise NotImplementedError() else: if opts['tiltShaft']: if nDOF_sft == 0: nac.connectTo(rot, type='Joint', rel_pos=(x_NR, 0, z_NR), rot_amounts=(0, tiltDOF, 0), rot_order='ZYX') else: nac.connectTo(rot, type='Joint', rel_pos=(x_NR, 0, z_NR), rot_amounts=(0, tiltDOF, psi), rot_order='ZYX') else: if nDOF_sft == 0: nac.connectTo(rot, type='Joint', rel_pos=(x_NR, 0, z_NR), rot_amounts=(0, 0, 0), rot_order='ZYX') else: nac.connectTo(rot, type='Joint', rel_pos=(x_NR, 0, z_NR), rot_amounts=(0, 0, psi), rot_order='ZYX') # --- Defining Body rotational velocities omega_TE = twr.ang_vel_in( ref) # Angular velocity of nacelle in inertial frame omega_NT = nac.ang_vel_in( twr.frame) # Angular velocity of nacelle in inertial frame if rot is not None: omega_RN = rot.ang_vel_in( nac.frame ) # Angular velocity of rotor wrt Nacelle (omega_R-omega_N) # --- Kinetics body_loads = [] bodies = [] g = symbols('g') g_vect = -g * ref.frame.z # Foundation/floater loads if fnd is not None: bodies += [fnd] grav_F = (fnd.masscenter, -fnd.mass * g * ref.frame.z) # Point of application for Buoyancy and mooring P_B = twr.origin.locatenew('P_B', z_TB * fnd.frame.z) # <<<< Measured from T P_M = twr.origin.locatenew('P_M', z_TM * fnd.frame.z) # <<<< Measured from T P_B.v2pt_theory(twr.origin, ref.frame, twr.frame) # PB & T are fixed in e_T P_M.v2pt_theory(twr.origin, ref.frame, twr.frame) # PM & T are fixed in e_T if opts['fnd_loads']: K_Mx, K_My, K_Mz = symbols( 'K_x_M, K_y_M, K_z_M') # Mooring restoring K_Mphix, K_Mphiy, K_Mphiz = symbols( 'K_phi_x_M, K_phi_y_M, K_phi_z_M') # Mooring restoring F_B = dynamicsymbols('F_B') # Buoyancy force #print('>>> Adding fnd loads. NOTE: reference might need to be adapted') # Buoyancy body_loads += [(fnd, (P_B, F_B * ref.frame.z))] ## Restoring mooring and torques fr = 0 fr += -K_Mx * x * ref.frame.x if bFndDOFs[0] else 0 fr += -K_My * y * ref.frame.y if bFndDOFs[1] else 0 fr += -K_Mz * z * ref.frame.z if bFndDOFs[2] else 0 Mr += -K_MPhix * phi_x * ref.frame.x if bFndDOFs[3] else 0 Mr += -K_MPhiy * phi_y * ref.frame.y if bFndDOFs[4] else 0 Mr += -K_MPhiz * phi_z * ref.frame.z if bFndDOFs[5] else 0 body_loads += [(fnd, (P_M, fr))] body_loads += [(fnd, (fnd.frame, Mr))] body_loads += [(fnd, grav_F)] # Tower loads grav_T = (twr.masscenter, -twr.mass * g * ref.frame.z) bodies += [twr] body_loads += [(twr, grav_T)] # Nacelle loads grav_N = (nac.masscenter, -nac.mass * g * ref.frame.z) bodies += [nac] body_loads += [(nac, grav_N)] T_a = dynamicsymbols('T_a') # NOTE NOTE #T_a = Function('T_a')(dynamicsymbols._t, *coordinates, *speeds) # NOTE: to introduce it in the linearization, add coordinates M_ax, M_ay, M_az = dynamicsymbols('M_x_a, M_y_a, M_z_a') # Aero torques if bFullRNA: if bBlades: raise NotImplementedError() else: # Rotor loads grav_R = (rot.masscenter, -M_R * g * ref.frame.z) bodies += [rot] body_loads += [(rot, grav_R)] # NOTE: loads on rot, but expressed in N frame if opts['tiltShaft']: # TODO actually tilt shaft, introduce non rotating shaft body if opts['aero_forces']: thrustR = (rot.origin, T_a * cos(tiltDOF) * nac.frame.x - T_a * sin(tiltDOF) * nac.frame.z) if opts['aero_torques']: M_a_R = (nac.frame, M_ax * nac.frame.x * 0) # TODO TODO #print('>>> WARNING tilt shaft aero moments not implemented') # TODO body_loads += [(nac, M_a_R)] else: thrustR = (rot.origin, T_a * nac.frame.x) #thrustR = (rot.origin, T_a * rot.frame.x ) #M_a_R = (rot.frame, M_ax*rot.frame.x + M_ay*rot.frame.y + M_az*rot.frame.z) # TODO TODO TODO introduce a non rotating shaft if opts['aero_torques']: M_a_R = (nac.frame, M_ax * nac.frame.x + M_ay * nac.frame.y + M_az * nac.frame.z) body_loads += [(nac, M_a_R)] body_loads += [(rot, thrustR)] else: # RNA loads, point load at R R = Point('R') R.set_pos(nac.origin, x_NR * nac.frame.x + z_NR * nac.frame.z) R.set_vel(nac.frame, 0 * nac.frame.x) R.v2pt_theory(nac.origin, ref.frame, nac.frame) #thrustN = (nac.masscenter, T * nac.frame.x) if opts['tiltShaft']: thrustN = (R, T_a * cos(tiltDOF) * nac.frame.x - T_a * sin(tiltDOF) * nac.frame.z) else: thrustN = (R, T_a * nac.frame.x) if opts['aero_forces']: body_loads += [(nac, thrustN)] if opts['aero_torques']: #print('>>> Adding aero torques') if opts['tiltShaft']: # NOTE: for a rigid RNA we keep only M_y and M_z, no shaft torque x_tilted = cos(tiltDOF) * nac.frame.x - sin( tiltDOF) * nac.frame.z z_tilted = cos(tiltDOF) * nac.frame.y + sin( tiltDOF) * nac.frame.x M_a_N = (nac.frame, M_ay * nac.frame.y + M_az * z_tilted) else: M_a_N = (nac.frame, M_ax * nac.frame.x + M_ay * nac.frame.y + M_az * nac.frame.z) body_loads += [(nac, M_a_N)] # --------------------------------------------------------------------------------} # --- Kinematic equations # --------------------------------------------------------------------------------{ kdeqsSubs = [] # --- Fnd if not opts['floating']: pass else: # Kdeqs for fnd: # : (xd, diff(x,time)) and (omega_y_T, diff(phi_y,time)) fndVelAll = [diff(x, time), diff(y, time), diff(z, time)] if opts['linRot']: fndVelAll += [ diff(phi_x, time), diff(phi_y, time), diff(phi_z, time) ] else: #print('>>>>>>>> TODO sort out which frame') #fndVelAll +=[ omega_TE.dot(ref.frame.x).simplify(), omega_TE.dot(ref.frame.y).simplify(), omega_TE.dot(ref.frame.z).simplify()] fndVelAll += [ omega_TE.dot(twr.frame.x).simplify(), omega_TE.dot(twr.frame.y).simplify(), omega_TE.dot(twr.frame.z).simplify() ] kdeqsSubs += [(fndSpeedsAll[i], fndVelAll[i]) for i, dof in enumerate(bFndDOFs) if dof] # --- Twr if nDOF_twr == 0: pass else: kdeqsSubs += [(twr.qd[i], twr.qdot[i]) for i, _ in enumerate(twr.q)] # --- Nac kdeqsSubs += nacKDEqSubs # --- Shaft if bFullRNA: if bBlades: raise NotImplementedError() else: if nDOF_sft == 1: #print('>>>>>>>> TODO sort out which frame') # I believe we should use omega_RE kdeqsSubs += [(omega_x_R, omega_RN.dot(rot.frame.x).simplify()) ] # --- Create a YAMS wrapper model model = YAMSModel(name=model_name) model.opts = opts model.ref = ref model.bodies = bodies model.body_loads = body_loads model.coordinates = coordinates model.speeds = speeds model.kdeqsSubs = kdeqsSubs #print(model) model.fnd = fnd model.twr = twr model.nac = nac model.rot = rot #model.sft=sft #model.bld=bld model.g_vect = g_vect # Small angles model.smallAnglesFnd = [phi_x, phi_y, phi_z] if nDOF_twr > 0: if opts['rot_elastic_smallAngle']: model.smallAnglesTwr = twr.vcList else: model.smallAnglesTwr = [] else: model.smallAnglesTwr = [] model.smallAnglesNac = [] if opts['yaw'] == 'dynamic': model.smallAnglesNac += [q_yaw] if opts['tilt'] == 'dynamic': model.smallAnglesNac += [q_tilt] model.smallAngles = model.smallAnglesFnd + model.smallAnglesTwr + model.smallAnglesNac # Shape normalization if nDOF_twr > 0: model.shapeNormSubs = [(v, 1) for v in twr.ucList] else: model.shapeNormSubs = [] return model
time = symbols('t') kinematical_differential_equations = [ omega1 - theta1.diff(time), omega2 - theta2.diff(time), omega3 - theta3.diff(time) ] # Angular Velocities # ================== lower_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z) upper_leg_frame.set_ang_vel(lower_leg_frame, omega2 * lower_leg_frame.z) torso_frame.set_ang_vel(upper_leg_frame, omega3 * upper_leg_frame.z) # Linear Velocities # ================= ankle.set_vel(inertial_frame, 0) lower_leg_mass_center.v2pt_theory(ankle, inertial_frame, lower_leg_frame) knee.v2pt_theory(ankle, inertial_frame, lower_leg_frame) upper_leg_mass_center.v2pt_theory(knee, inertial_frame, upper_leg_frame) hip.v2pt_theory(knee, inertial_frame, upper_leg_frame) torso_mass_center.v2pt_theory(hip, inertial_frame, torso_frame)
a_length = symbols('l_a') B = Point('B') B.set_pos(A, a_length*a_frame.y) #Sets up the angular velocities omega_base, omega_a = dynamicsymbols('omega_b, omega_a') #Relates angular velocity values to the angular positions theta1 and theta2 kinematic_differential_equations = [omega_a - theta_a.diff()] #Sets up the rotational axes of the angular velocities a_frame.set_ang_vel(inertial_frame, omega_base*inertial_frame.z + omega_a*inertial_frame.z) #Sets up the linear velocities of the points on the linkages base_vel_x, base_vel_y = dynamicsymbols('v_bx, v_by') A.set_vel(inertial_frame, base_vel_x*base_frame.x + base_vel_y*base_frame.y) B.v2pt_theory(A, inertial_frame, a_frame) #Sets up the masses of the linkages a_mass = symbols('m_a') #Defines the linkages as particles bP = Particle('bP', B, a_mass) #Sets up gravity information and assigns gravity to act on mass centers g = symbols('g') b_grav_force_vector = -1*a_mass*g*inertial_frame.y b_grav_force = (B, b_grav_force_vector) #Sets up joint torques a_torque = dynamicsymbols('T_a') a_torque_vector = a_torque*inertial_frame.z
hip_frame.set_ang_vel(hip_frame, omega1 * inertial_frame.z) #hip_frame.ang_vel_in(inertial_frame) upper_leg_right_frame.set_ang_vel(upper_leg_right_frame, omega2 * inertial_frame.z) #upper_leg_right_frame.ang_vel_in(inertial_frame) lower_leg_right_frame.set_ang_vel(lower_leg_right_frame, omega3 * inertial_frame.z) #lower_leg_right_frame.ang_vel_in(inertial_frame) #%% Linear Velocities print("Defining linear velocities") origin.set_vel(inertial_frame, 0) ankle_left.set_vel(inertial_frame, 0) knee_left.v2pt_theory(ankle_left, inertial_frame, lower_leg_left_frame) #knee_left.vel(inertial_frame) hip_left.v2pt_theory(knee_left, inertial_frame, upper_leg_left_frame) #hip_left.vel(inertial_frame) hip_center.v2pt_theory(hip_left, inertial_frame, hip_frame) #hip_center.vel(inertial_frame) hip_mass_center.v2pt_theory(hip_center, inertial_frame, hip_frame) #hip_mass_center.vel(inertial_frame) hip_right.v2pt_theory(hip_center, inertial_frame, hip_frame) #hip_right.vel(inertial_frame) knee_right.v2pt_theory(hip_right, inertial_frame, upper_leg_right_frame)
omega2 - theta2.diff(time)] # Angular Velocities # ================== lower_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z) upper_leg_frame.set_ang_vel(lower_leg_frame, omega2 * lower_leg_frame.z) # Linear Velocities # ================= ankle.set_vel(inertial_frame, 0) lower_leg_mass_center.v2pt_theory(ankle, inertial_frame, lower_leg_frame) knee.v2pt_theory(ankle, inertial_frame, lower_leg_frame) upper_leg_mass_center.v2pt_theory(knee, inertial_frame, upper_leg_frame) # Mass # ==== lower_leg_mass, upper_leg_mass = symbols('m_L, m_U') # Inertia # ======= lower_leg_inertia, upper_leg_inertia, torso_inertia = symbols('I_Lz, I_Uz, I_Tz')
one_frame_dp2.set_ang_vel(inertial_frame_dp2, omega1_dp2 * inertial_frame_dp2.z) one_frame_dp2.ang_vel_in(inertial_frame_dp2) two_frame_dp1.set_ang_vel(one_frame_dp1, omega2_dp1 * one_frame_dp1.z - omega1_dp2 * one_frame_dp1.z) two_frame_dp1.ang_vel_in(inertial_frame_dp1) two_frame_dp2.set_ang_vel(one_frame_dp2, omega2_dp2 * one_frame_dp2.z) two_frame_dp2.ang_vel_in(inertial_frame_dp2) # Linear Velocities # ================= one_dp1.set_vel(inertial_frame_dp1, 0) one_dp2.set_vel(inertial_frame_dp2, two_dp1.vel) one_mass_center_dp1.v2pt_theory(one_dp1, inertial_frame_dp1, one_frame_dp1) one_mass_center_dp1.vel(inertial_frame_dp1) one_mass_center_dp2.v2pt_theory(one_dp2, inertial_frame_dp2, one_frame_dp2) one_mass_center_dp2.vel(inertial_frame_dp2) two_dp1.v2pt_theory(one_dp1, inertial_frame_dp1, one_frame_dp1) two_dp1.vel(inertial_frame_dp1) two_dp2.v2pt_theory(one_dp2, inertial_frame_dp2, one_frame_dp2) two_dp2.vel(inertial_frame_dp2) two_mass_center_dp1.v2pt_theory(two_dp1, inertial_frame_dp1, two_frame_dp1) two_mass_center_dp2.v2pt_theory(two_dp2, inertial_frame_dp2, two_frame_dp2) # Mass # ====
time = symbols('t') kinematical_differential_equations = [omega1 - theta1.diff(time), omega2 - theta2.diff(time), omega3 - theta3.diff(time)] # Angular Velocities # ================== lower_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z) upper_leg_frame.set_ang_vel(lower_leg_frame, omega2 * lower_leg_frame.z) torso_frame.set_ang_vel(upper_leg_frame, omega3 * upper_leg_frame.z) # Linear Velocities # ================= ankle.set_vel(inertial_frame, 0) lower_leg_mass_center.v2pt_theory(ankle, inertial_frame, lower_leg_frame) knee.v2pt_theory(ankle, inertial_frame, lower_leg_frame) upper_leg_mass_center.v2pt_theory(knee, inertial_frame, upper_leg_frame) hip.v2pt_theory(knee, inertial_frame, upper_leg_frame) torso_mass_center.v2pt_theory(hip, inertial_frame, torso_frame)
# Referenciais # Referencial Inercial B0 = ReferenceFrame('B0') # Referencial móvel: theta_1 em relação a B0.z B1 = ReferenceFrame('B1') B1.orient(B0, 'Axis', [THETA_1, B0.z]) # Referencial móvel: theta_2 em relação a B1.z B2 = ReferenceFrame('B2') B2.orient(B1, 'Axis', [THETA_2, B1.z]) # Pontos e Centros de Massa O = Point('O') O.set_vel(B0, 0) A = Point('A') A.set_pos(O, L_1 * B1.x) A.v2pt_theory(O, B0, B1) CM_1 = Point('CM_1') CM_1.set_pos(O, R_1 * B1.x) CM_1.v2pt_theory(O, B0, B1) CM_2 = Point('CM_2') CM_2.set_pos(A, R_2 * B2.x) CM_2.v2pt_theory(O, B0, B2) # Corpos Rígidos I_1 = inertia(B1, 0, 0, I_1_ZZ) # Elo 1 E_1 = RigidBody('Elo_1', CM_1, B1, M_1, (I_1, CM_1)) I_2 = inertia(B1, 0, 0, I_1_ZZ) # Elo 2 E_2 = RigidBody('Elo_2', CM_2, B2, M_2, (I_2, CM_2))
# ================== l_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z) crotch_frame.set_ang_vel(l_leg_frame, omega2 * l_leg_frame.z) body_frame.set_ang_vel(crotch_frame, omega4 * crotch_frame.z) r_leg_frame.set_ang_vel(crotch_frame, omega3 * crotch_frame.z) # Linear Velocities # ================= l_ankle.set_vel(inertial_frame, 0) l_leg_mass_center.v2pt_theory(l_ankle, inertial_frame, l_leg_frame) l_hip.v2pt_theory(l_ankle, inertial_frame, l_leg_frame) crotch_mass_center.v2pt_theory(l_hip, inertial_frame, crotch_frame) waist.v2pt_theory(l_hip, inertial_frame, crotch_frame) body_mass_center.v2pt_theory(waist, inertial_frame, body_frame) r_hip.v2pt_theory(l_hip, inertial_frame, crotch_frame) r_leg_mass_center.v2pt_theory(r_hip, inertial_frame, r_leg_frame) # Mass # ====
omega2 - theta2.diff(time)] # Angular Velocities # ================== one_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z) one_frame.ang_vel_in(inertial_frame) two_frame.set_ang_vel(one_frame, omega2 * one_frame.z) two_frame.ang_vel_in(inertial_frame) # Linear Velocities # ================= one.set_vel(inertial_frame, 0) one_mass_center.v2pt_theory(one, inertial_frame, one_frame) one_mass_center.vel(inertial_frame) two.v2pt_theory(one, inertial_frame, one_frame) two.vel(inertial_frame) two_mass_center.v2pt_theory(two, inertial_frame, two_frame) # Mass # ==== one_mass, two_mass = symbols('m_1, m_2') # Inertia # ======= rotI = lambda I, f: Matrix([j & I & i for i in f for j in f]).reshape(3,3)
RLB_frame.set_ang_vel(Chassis_frame,(wRLBx * RLB_frame.x + wRLBy * RLB_frame.y + wRLBz * RLB_frame.z)) Jbar_frame.set_ang_vel(Chassis_frame,(wJbarx * Jbar_frame.x + wJbary * Jbar_frame.y + wJbarz * Jbar_frame.z)) LFT_frame.set_ang_vel(LSpdl_frame,(wLFTy * LFT_frame.y)) RFT_frame.set_ang_vel(RSpdl_frame,(wRFTy * RFT_frame.y)) LRT_frame.set_ang_vel(Hsg_frame,(wLRTy * LRT_frame.y)) RRT_frame.set_ang_vel(Hsg_frame,(wRRTy * RRT_frame.y)) #LRTeth_frame.set_ang_vel(Hsg_frame, 'Axis',(wLRTewy, Hsg_frame.y)) #RRTeth_frame.set_ang_vel(Hsg_frame, 'Axis',(wRRTewy, Hsg_frame.y)) # ______________________________________________________________________________________________________________________________ # Linear Velocities # Finally, the linear velocities of the mass centers are needed. GlobalCoord0.set_vel(inertial_frame,0) CoM_RP.set_vel(RP_frame,veltransRPy * RP_frame.y) CoM_Chassis.v2pt_theory(GlobalCoord0,inertial_frame,Chassis_frame) CoM_LUCA.v2pt_theory(GlobalCoord0,inertial_frame,LUCA_frame) CoM_RUCA.v2pt_theory(GlobalCoord0,inertial_frame,RUCA_frame) CoM_LLCA.v2pt_theory(GlobalCoord0,inertial_frame,LLCA_frame) CoM_RLCA.v2pt_theory(GlobalCoord0,inertial_frame,RUCA_frame) CoM_LSpdl.v2pt_theory(GlobalCoord0,inertial_frame,LSpdl_frame) CoM_RSpdl.v2pt_theory(GlobalCoord0,inertial_frame,RSpdl_frame) CoM_LTR.v2pt_theory(GlobalCoord0,inertial_frame,LTR_frame) CoM_RTR.v2pt_theory(GlobalCoord0,inertial_frame,RTR_frame) CoM_Hsg.v2pt_theory(GlobalCoord0,inertial_frame,Hsg_frame) CoM_LBC.v2pt_theory(GlobalCoord0,inertial_frame,LBC_frame) CoM_RBC.v2pt_theory(GlobalCoord0,inertial_frame,RBC_frame) CoM_LUB.v2pt_theory(GlobalCoord0,inertial_frame,LUB_frame) CoM_RUB.v2pt_theory(GlobalCoord0,inertial_frame,RUB_frame) CoM_LLB.v2pt_theory(GlobalCoord0,inertial_frame,LLB_frame) CoM_RLB.v2pt_theory(GlobalCoord0,inertial_frame,RLB_frame)
#hip_frame.ang_vel_in(inertial_frame) upper_leg_right_frame.set_ang_vel(upper_leg_right_frame, omega2 * inertial_frame.z) #upper_leg_right_frame.ang_vel_in(inertial_frame) lower_leg_right_frame.set_ang_vel(lower_leg_right_frame, omega3 * inertial_frame.z) #lower_leg_right_frame.ang_vel_in(inertial_frame) #%% Linear Velocities print("Defining linear velocities") origin.set_vel(inertial_frame, 0) ankle_left.set_vel(inertial_frame, 0) knee_left.v2pt_theory(ankle_left, inertial_frame, lower_leg_left_frame) #knee_left.vel(inertial_frame) hip_left.v2pt_theory(knee_left, inertial_frame, upper_leg_left_frame) #hip_left.vel(inertial_frame) hip_center.v2pt_theory(hip_left, inertial_frame, hip_frame) #hip_center.vel(inertial_frame) hip_mass_center.v2pt_theory(hip_center, inertial_frame, hip_frame) #hip_mass_center.vel(inertial_frame) hip_right.v2pt_theory(hip_center, inertial_frame, hip_frame) #hip_right.vel(inertial_frame) knee_right.v2pt_theory(hip_right, inertial_frame, upper_leg_right_frame)
omega_c - theta_c.diff(), omega_go - theta_] # Angular Velocities # ================== a_frame.set_ang_vel(inertial_frame, omega_a*inertial_frame.z) b_frame.set_ang_vel(a_frame, omega_b*inertial_frame.z) c_frame.set_ang_vel(b_frame, omega_c*inertial_frame.z) go_frame.set_ang_vel(inertial_frame, omega_go*inertial_frame.z) bco_frame.set_ang_vel(b_frame, omega_bco*inertial_frame.z) # Linear Velocities # ================= A.set_vel(inertial_frame, 0) B.v2pt_theory(A, inertial_frame, a_frame) C.v2pt_theory(B, inertial_frame, b_frame) D.v2pt_theory(C, inertial_frame, c_frame) com_global.v2pt_theory(A, inertial_frame, go_frame) com_bc.v2pt_theory(B, inertial_frame, bco_frame) # Mass # ==== a_mass, b_mass, c_mass = symbols('m_a, m_b, m_c') #Defines the linkages as particles bP = Particle('bP', B, a_mass) cP = Particle('cP', C, b_mass) dP = Particle('dP', D, c_mass) goP = Particle('goP', com_global, a_mass+b_mass+c_mass)
#Sets up the angular velocities omega1, omega2 = dynamicsymbols('omega1, omega2') #Relates angular velocity values to the angular positions theta1 and theta2 kinematic_differential_equations = [omega1 - theta1.diff(), omega2 - theta2.diff()] #Sets up the rotational axes of the angular velocities one_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z) one_frame.ang_vel_in(inertial_frame) two_frame.set_ang_vel(one_frame, omega2*inertial_frame.z) two_frame.ang_vel_in(inertial_frame) #Sets up the linear velocities of the points on the linkages #one.set_vel(inertial_frame, 0) two.v2pt_theory(one, inertial_frame, one_frame) two.vel(inertial_frame) three.v2pt_theory(two, inertial_frame, two_frame) three.vel(inertial_frame) #Sets up the masses of the linkages one_mass, two_mass = symbols('m_1, m_2') #Defines the linkages as particles twoP = Particle('twoP', two, one_mass) threeP = Particle('threeP', three, two_mass) #Sets up gravity information and assigns gravity to act on mass centers g = symbols('g') two_grav_force_vector = -1*one_mass*g*inertial_frame.y two_grav_force = (two, two_grav_force_vector)
kinematical_differential_equations = [omega1 - theta1.diff(time), omega2 - theta2.diff(time)] # Angular Velocities # ================== l_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z) body_frame.set_ang_vel(l_leg_frame, omega2 * l_leg_frame.z) # Linear Velocities # ================= l_ankle.set_vel(inertial_frame, 0) l_leg_mass_center.v2pt_theory(l_ankle, inertial_frame, l_leg_frame) l_hip.v2pt_theory(l_ankle, inertial_frame, l_leg_frame) r_hip.v2pt_theory(l_hip, inertial_frame, body_frame) body_mass_center.v2pt_theory(l_hip, inertial_frame, body_frame) r_leg_mass_center.v2pt_theory(r_hip, inertial_frame, body_frame) # Mass # ==== l_leg_mass, body_mass, r_leg_mass = symbols("m_L, m_B, m_R") # Inertia
from sympy.physics.mechanics import dot, dynamicsymbols, inertia, msprint m, B11, B22, B33, B12, B23, B31 = symbols('m B11 B22 B33 B12 B23 B31') q1, q2, q3, q4, q5, q6 = dynamicsymbols('q1:7') # reference frames A = ReferenceFrame('A') B = A.orientnew('B', 'body', [q4, q5, q6], 'xyz') omega = B.ang_vel_in(A) # points B*, O pB_star = Point('B*') pO = pB_star.locatenew('O', q1*B.x + q2*B.y + q3*B.z) pO.set_vel(A, 0) pO.set_vel(B, 0) pB_star.v2pt_theory(pO, A, B) # rigidbody B I_B_O = inertia(B, B11, B22, B33, B12, B23, B31) rbB = RigidBody('rbB', pB_star, B, m, (I_B_O, pO)) # kinetic energy K = rbB.kinetic_energy(A) print('K = {0}'.format(msprint(trigsimp(K)))) K_expected = dot(dot(omega, I_B_O), omega)/2 print('K_expected = 1/2*omega*I*omega = {0}'.format( msprint(trigsimp(K_expected)))) assert expand(expand_trig(K - K_expected)) == 0