def eval_mass_eq(self):
config = self.config
t = self.t
m0 = G(self.P_ground)
m1 = G(self.P_rbs_body)
self.mass_eq_blocks = (self.M_ground,
(4) * multi_dot([m0.T,self.Jbar_ground,m0]),
self.M_rbs_body,
(4) * multi_dot([m1.T,config.Jbar_rbs_body,m1]),)
def eval_mass_eq(self):
config = self.config
t = self.t
m0 = I3
m1 = G(self.P_ground)
m2 = G(self.P_rbs_body)
self.mass_eq_blocks = (
self.m_ground * m0,
(4) * multi_dot([m1.T, self.Jbar_ground, m1]),
config.m_rbs_body * m0,
(4) * multi_dot([m2.T, config.Jbar_rbs_body, m2]),
)
def eval_frc_eq(self):
config = self.config
t = self.t
f0 = G(self.Pd_rbs_input_shaft)
f1 = G(self.Pd_rbs_connect_shaft)
f2 = G(self.Pd_rbs_output_shaft)
self.frc_eq_blocks = (self.F_rbs_input_shaft_gravity,
8*multi_dot([f0.T,config.Jbar_rbs_input_shaft,f0,self.P_rbs_input_shaft]),
self.F_rbs_connect_shaft_gravity,
8*multi_dot([f1.T,config.Jbar_rbs_connect_shaft,f1,self.P_rbs_connect_shaft]),
self.F_rbs_output_shaft_gravity,
8*multi_dot([f2.T,config.Jbar_rbs_output_shaft,f2,self.P_rbs_output_shaft]),)
def eval_mass_eq(self):
config = self.config
t = self.t
m0 = np.eye(3, dtype=np.float64)
m1 = G(self.P_rbs_input_shaft)
m2 = G(self.P_rbs_connect_shaft)
m3 = G(self.P_rbs_output_shaft)
self.mass_eq_blocks = (config.m_rbs_input_shaft*m0,
4*multi_dot([m1.T,config.Jbar_rbs_input_shaft,m1]),
config.m_rbs_connect_shaft*m0,
4*multi_dot([m2.T,config.Jbar_rbs_connect_shaft,m2]),
config.m_rbs_output_shaft*m0,
4*multi_dot([m3.T,config.Jbar_rbs_output_shaft,m3]),)
def eval_mass_eq(self):
config = self.config
t = self.t
m0 = I3
m1 = G(self.P_rbs_coupler)
m2 = G(self.P_rbr_rocker)
m3 = G(self.P_rbl_rocker)
self.mass_eq_blocks = (
config.m_rbs_coupler * m0,
(4) * multi_dot([m1.T, config.Jbar_rbs_coupler, m1]),
config.m_rbr_rocker * m0,
(4) * multi_dot([m2.T, config.Jbar_rbr_rocker, m2]),
config.m_rbl_rocker * m0,
(4) * multi_dot([m3.T, config.Jbar_rbl_rocker, m3]),
)
def eval_frc_eq(self):
config = self.config
t = self.t
f0 = G(self.Pd_rbs_body_1)
f1 = G(self.Pd_rbs_body_2)
self.frc_eq_blocks = (
Z3x1,
Z4x1,
self.F_rbs_body_1_gravity,
(8) *
multi_dot([f0.T, config.Jbar_rbs_body_1, f0, self.P_rbs_body_1]),
self.F_rbs_body_2_gravity,
(8) *
multi_dot([f1.T, config.Jbar_rbs_body_2, f1, self.P_rbs_body_2]),
)
def eval_mass_eq(self):
config = self.config
t = self.t
m0 = I3
m1 = G(self.P_rbr_inner_shaft)
m2 = G(self.P_rbl_inner_shaft)
m3 = G(self.P_rbr_coupling)
m4 = G(self.P_rbl_coupling)
self.mass_eq_blocks = (config.m_rbr_inner_shaft * m0,
(4) * multi_dot([m1.T,config.Jbar_rbr_inner_shaft,m1]),
config.m_rbl_inner_shaft * m0,
(4) * multi_dot([m2.T,config.Jbar_rbl_inner_shaft,m2]),
config.m_rbr_coupling * m0,
(4) * multi_dot([m3.T,config.Jbar_rbr_coupling,m3]),
config.m_rbl_coupling * m0,
(4) * multi_dot([m4.T,config.Jbar_rbl_coupling,m4]),)
def eval_frc_eq(self):
config = self.config
t = self.t
f0 = G(self.Pd_rbs_l1)
f1 = G(self.Pd_rbs_l2)
f2 = G(self.Pd_rbs_l3)
self.frc_eq_blocks = (
Z3x1,
Z4x1,
self.F_rbs_l1_gravity,
(8) * multi_dot([f0.T, config.Jbar_rbs_l1, f0, self.P_rbs_l1]),
self.F_rbs_l2_gravity,
(8) * multi_dot([f1.T, config.Jbar_rbs_l2, f1, self.P_rbs_l2]),
self.F_rbs_l3_gravity,
(8) * multi_dot([f2.T, config.Jbar_rbs_l3, f2, self.P_rbs_l3]),
)
def eval_frc_eq(self):
config = self.config
t = self.t
f0 = G(self.Pd_rbs_coupler)
f1 = G(self.Pd_rbr_rocker)
f2 = G(self.Pd_rbl_rocker)
self.frc_eq_blocks = (
self.F_rbs_coupler_gravity,
(8) *
multi_dot([f0.T, config.Jbar_rbs_coupler, f0, self.P_rbs_coupler]),
self.F_rbr_rocker_gravity,
(8) *
multi_dot([f1.T, config.Jbar_rbr_rocker, f1, self.P_rbr_rocker]),
self.F_rbl_rocker_gravity,
(8) *
multi_dot([f2.T, config.Jbar_rbl_rocker, f2, self.P_rbl_rocker]),
)
def eval_frc_eq(self):
config = self.config
t = self.t
f0 = G(self.Pd_rbs_rack)
self.frc_eq_blocks = (
self.F_rbs_rack_gravity,
(8) * multi_dot([f0.T, config.Jbar_rbs_rack, f0, self.P_rbs_rack]),
)
def eval_mass_eq(self):
config = self.config
t = self.t
m0 = G(self.P_rbs_rack)
self.mass_eq_blocks = (
config.m_rbs_rack * I3,
(4) * multi_dot([m0.T, config.Jbar_rbs_rack, m0]),
)
def eval_frc_eq(self):
config = self.config
t = self.t
f0 = G(self.Pd_rbs_body)
self.frc_eq_blocks = (Z3x1,
Z4x1,
self.F_rbs_body_gravity,
(8) * multi_dot([f0.T,config.Jbar_rbs_body,f0,self.P_rbs_body]),)
def eval_mass_eq(self):
config = self.config
t = self.t
m0 = I3
m1 = G(self.P_ground)
m2 = G(self.P_rbs_l1)
m3 = G(self.P_rbs_l2)
m4 = G(self.P_rbs_l3)
self.mass_eq_blocks = (
self.m_ground * m0,
(4) * multi_dot([m1.T, self.Jbar_ground, m1]),
config.m_rbs_l1 * m0,
(4) * multi_dot([m2.T, config.Jbar_rbs_l1, m2]),
config.m_rbs_l2 * m0,
(4) * multi_dot([m3.T, config.Jbar_rbs_l2, m3]),
config.m_rbs_l3 * m0,
(4) * multi_dot([m4.T, config.Jbar_rbs_l3, m4]),
)
def eval_mass_eq(self):
config = self.config
t = self.t
m0 = I3
m1 = G(self.P_ground)
m2 = G(self.P_rbs_l1)
m3 = G(self.P_rbs_l2)
m4 = G(self.P_rbs_l3)
m5 = G(self.P_rbs_l4)
m6 = G(self.P_rbs_l5)
m7 = G(self.P_rbs_l6)
m8 = G(self.P_rbs_l7)
m9 = G(self.P_rbs_l8)
m10 = G(self.P_rbs_l9)
self.mass_eq_blocks = (
self.m_ground * m0,
(4) * multi_dot([m1.T, self.Jbar_ground, m1]),
config.m_rbs_l1 * m0,
(4) * multi_dot([m2.T, config.Jbar_rbs_l1, m2]),
config.m_rbs_l2 * m0,
(4) * multi_dot([m3.T, config.Jbar_rbs_l2, m3]),
config.m_rbs_l3 * m0,
(4) * multi_dot([m4.T, config.Jbar_rbs_l3, m4]),
config.m_rbs_l4 * m0,
(4) * multi_dot([m5.T, config.Jbar_rbs_l4, m5]),
config.m_rbs_l5 * m0,
(4) * multi_dot([m6.T, config.Jbar_rbs_l5, m6]),
config.m_rbs_l6 * m0,
(4) * multi_dot([m7.T, config.Jbar_rbs_l6, m7]),
config.m_rbs_l7 * m0,
(4) * multi_dot([m8.T, config.Jbar_rbs_l7, m8]),
config.m_rbs_l8 * m0,
(4) * multi_dot([m9.T, config.Jbar_rbs_l8, m9]),
config.m_rbs_l9 * m0,
(4) * multi_dot([m10.T, config.Jbar_rbs_l9, m10]),
)
def eval_frc_eq(self):
config = self.config
t = self.t
f0 = G(self.Pd_rbs_l1)
f1 = G(self.Pd_rbs_l2)
f2 = G(self.Pd_rbs_l3)
f3 = G(self.Pd_rbs_l4)
f4 = G(self.Pd_rbs_l5)
f5 = G(self.Pd_rbs_l6)
f6 = G(self.Pd_rbs_l7)
f7 = G(self.Pd_rbs_l8)
f8 = G(self.Pd_rbs_l9)
self.frc_eq_blocks = (
Z3x1,
Z4x1,
self.F_rbs_l1_gravity,
(8) * multi_dot([f0.T, config.Jbar_rbs_l1, f0, self.P_rbs_l1]),
self.F_rbs_l2_gravity,
(8) * multi_dot([f1.T, config.Jbar_rbs_l2, f1, self.P_rbs_l2]),
self.F_rbs_l3_gravity,
(8) * multi_dot([f2.T, config.Jbar_rbs_l3, f2, self.P_rbs_l3]),
self.F_rbs_l4_gravity,
(8) * multi_dot([f3.T, config.Jbar_rbs_l4, f3, self.P_rbs_l4]),
self.F_rbs_l5_gravity,
(8) * multi_dot([f4.T, config.Jbar_rbs_l5, f4, self.P_rbs_l5]),
self.F_rbs_l6_gravity,
(8) * multi_dot([f5.T, config.Jbar_rbs_l6, f5, self.P_rbs_l6]),
self.F_rbs_l7_gravity,
(8) * multi_dot([f6.T, config.Jbar_rbs_l7, f6, self.P_rbs_l7]),
self.F_rbs_l8_gravity,
(8) * multi_dot([f7.T, config.Jbar_rbs_l8, f7, self.P_rbs_l8]),
self.F_rbs_l9_gravity,
(8) * multi_dot([f8.T, config.Jbar_rbs_l9, f8, self.P_rbs_l9]),
)
def eval_frc_eq(self):
config = self.config
t = self.t
f0 = self.R_rbs_body
f1 = self.R_ground
f2 = self.ubar_rbs_body_fas_TSDA
f3 = self.P_rbs_body
f4 = A(f3)
f5 = self.ubar_ground_fas_TSDA
f6 = self.P_ground
f7 = A(f6)
f8 = (f0.T + (-1) * f1.T + multi_dot([f2.T, f4.T]) +
(-1) * multi_dot([f5.T, f7.T]))
f9 = multi_dot([f4, f2])
f10 = multi_dot([f7, f5])
f11 = (f0 + (-1) * f1 + f9 + (-1) * f10)
f12 = ((multi_dot([f8, f11]))**(1.0 / 2.0))[0]
f13 = 1.0 / f12
f14 = config.UF_fas_TSDA_Fs((config.fas_TSDA_FL + (-1 * f12)))
f15 = self.Pd_rbs_body
f16 = config.UF_fas_TSDA_Fd((-1 * 1.0 / f12) * multi_dot([
f8,
(self.Rd_rbs_body +
(-1) * self.Rd_ground + multi_dot([B(f3, f2), f15]) +
(-1) * multi_dot([B(f6, f5), self.Pd_ground]))
]))
f17 = (f13 * (f14 + f16)) * f11
f18 = (2 * f14)
f19 = (2 * f16)
f20 = G(f15)
self.frc_eq_blocks = (
(Z3x1 + (-1) * f17),
(Z4x1 +
(f13 *
(f18 + f19)) * multi_dot([E(f6).T, skew(f10).T, f11])),
(self.F_rbs_body_gravity + f17),
((8) * multi_dot([f20.T, config.Jbar_rbs_body, f20, f3]) +
(f13 *
((-1 * f18) +
(-1 * f19))) * multi_dot([E(f3).T, skew(f9).T, f11])),
)
def eval_frc_eq(self):
config = self.config
t = self.t
f0 = t
f1 = config.UF_fas_aero_drag_F(f0)
f2 = G(self.Pd_rbs_chassis)
f3 = self.P_rbs_chassis
self.frc_eq_blocks = (
(self.F_rbs_chassis_gravity + f1),
((8) * multi_dot([f2.T, config.Jbar_rbs_chassis, f2, f3]) +
(2) * multi_dot([
E(f3).T,
(config.UF_fas_aero_drag_T(f0) + multi_dot([
skew(
multi_dot(
[A(f3), self.ubar_rbs_chassis_fas_aero_drag])), f1
]))
])),
)
def eval_frc_eq(self):
config = self.config
t = self.t
f0 = Z3x1
f1 = self.P_rbr_inner_shaft
f2 = G(self.Pd_rbr_inner_shaft)
f3 = self.P_rbl_inner_shaft
f4 = G(self.Pd_rbl_inner_shaft)
f5 = G(self.Pd_rbr_coupling)
f6 = G(self.Pd_rbl_coupling)
self.frc_eq_blocks = ((self.F_rbr_inner_shaft_gravity + f0),
((2 * config.UF_far_drive(t)) * multi_dot([G(f1).T,self.vbar_rbr_inner_shaft_far_drive]) + (8) * multi_dot([f2.T,config.Jbar_rbr_inner_shaft,f2,f1])),
(self.F_rbl_inner_shaft_gravity + f0),
((2 * config.UF_fal_drive(t)) * multi_dot([G(f3).T,self.vbar_rbl_inner_shaft_fal_drive]) + (8) * multi_dot([f4.T,config.Jbar_rbl_inner_shaft,f4,f3])),
self.F_rbr_coupling_gravity,
(8) * multi_dot([f5.T,config.Jbar_rbr_coupling,f5,self.P_rbr_coupling]),
self.F_rbl_coupling_gravity,
(8) * multi_dot([f6.T,config.Jbar_rbl_coupling,f6,self.P_rbl_coupling]),)
def eval_frc_eq(self):
config = self.config
t = self.t
f0 = Z3x1
f1 = self.R_rbs_body_1
f2 = f1.T
f3 = self.R_ground
f4 = self.ubar_rbs_body_1_fas_TSDA_1
f5 = self.P_rbs_body_1
f6 = A(f5)
f7 = f6.T
f8 = self.ubar_ground_fas_TSDA_1
f9 = self.P_ground
f10 = A(f9)
f11 = (f2 + (-1) * f3.T + multi_dot([f4.T, f7]) +
(-1) * multi_dot([f8.T, f10.T]))
f12 = multi_dot([f6, f4])
f13 = multi_dot([f10, f8])
f14 = (f1 + (-1) * f3 + f12 + (-1) * f13)
f15 = ((multi_dot([f11, f14]))**(1.0 / 2.0))[0]
f16 = 1.0 / f15
f17 = config.UF_fas_TSDA_1_Fs((config.fas_TSDA_1_FL + (-1 * f15)))
f18 = self.Rd_rbs_body_1
f19 = self.Pd_rbs_body_1
f20 = config.UF_fas_TSDA_1_Fd((-1 * 1.0 / f15) * multi_dot([
f11,
(f18 + (-1) * self.Rd_ground + multi_dot([B(f5, f4), f19]) +
(-1) * multi_dot([B(f9, f8), self.Pd_ground]))
]))
f21 = (f16 * (f17 + f20)) * f14
f22 = Z4x1
f23 = (2 * f17)
f24 = (2 * f20)
f25 = self.R_rbs_body_2
f26 = self.ubar_rbs_body_2_fas_TSDA_2
f27 = self.P_rbs_body_2
f28 = A(f27)
f29 = self.ubar_rbs_body_1_fas_TSDA_2
f30 = (f25.T + (-1) * f2 + multi_dot([f26.T, f28.T]) +
(-1) * multi_dot([f29.T, f7]))
f31 = multi_dot([f28, f26])
f32 = multi_dot([f6, f29])
f33 = (f25 + (-1) * f1 + f31 + (-1) * f32)
f34 = ((multi_dot([f30, f33]))**(1.0 / 2.0))[0]
f35 = 1.0 / f34
f36 = config.UF_fas_TSDA_2_Fs((config.fas_TSDA_2_FL + (-1 * f34)))
f37 = self.Pd_rbs_body_2
f38 = config.UF_fas_TSDA_2_Fd((-1 * 1.0 / f34) * multi_dot([
f30,
(self.Rd_rbs_body_2 + (-1) * f18 + multi_dot([B(f27, f26), f37]) +
(-1) * multi_dot([B(f5, f29), f19]))
]))
f39 = (f35 * (f36 + f38)) * f33
f40 = G(f19)
f41 = E(f5).T
f42 = (2 * f36)
f43 = (2 * f38)
f44 = G(f37)
self.frc_eq_blocks = (
(f0 + (-1) * f21),
(f22 +
(f16 *
(f23 + f24)) * multi_dot([E(f9).T, skew(f13).T, f14])),
(self.F_rbs_body_1_gravity + f0 + f21 + (-1) * f39),
(f22 + (8) * multi_dot([f40.T, config.Jbar_rbs_body_1, f40, f5]) +
(f16 * ((-1 * f23) +
(-1 * f24))) * multi_dot([f41, skew(f12).T, f14]) +
(f35 * (f42 + f43)) * multi_dot([f41, skew(f32).T, f33])),
(self.F_rbs_body_2_gravity + f39),
((8) * multi_dot([f44.T, config.Jbar_rbs_body_2, f44, f27]) +
(f35 *
((-1 * f42) +
(-1 * f43))) * multi_dot([E(f27).T, skew(f31).T, f33])),
)