#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
