def eval_jac_eq(self):
config = self.config
t = self.t
j0 = I3
j1 = self.P_ground
j2 = Z1x3
j3 = self.Mbar_rbs_body_jcs_a[:,2:3]
j4 = j3.T
j5 = self.P_rbs_body
j6 = A(j5).T
j7 = self.Mbar_ground_jcs_a[:,0:1]
j8 = self.Mbar_ground_jcs_a[:,1:2]
j9 = A(j1).T
j10 = B(j5,j3)
self.jac_eq_blocks = (j0,
B(j1,self.ubar_ground_jcs_a),
(-1) * j0,
(-1) * B(j5,self.ubar_rbs_body_jcs_a),
j2,
multi_dot([j4,j6,B(j1,j7)]),
j2,
multi_dot([j7.T,j9,j10]),
j2,
multi_dot([j4,j6,B(j1,j8)]),
j2,
multi_dot([j8.T,j9,j10]),
j0,
Z3x4,
Z4x3,
I4,
j2,
(2) * j5.T,)
def eval_constants(self):
config = self.config
self.R_ground = np.array([[0], [0], [0]], dtype=np.float64)
self.P_ground = np.array([[1], [0], [0], [0]], dtype=np.float64)
self.Pg_ground = np.array([[1], [0], [0], [0]], dtype=np.float64)
self.m_ground = 1.0
self.Jbar_ground = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]],
dtype=np.float64)
self.F_rbs_body_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_body]], dtype=np.float64)
self.T_rbs_body_fas_TSDA = Z3x1
self.T_ground_fas_TSDA = Z3x1
self.Mbar_rbs_body_jcs_trans = multi_dot(
[A(config.P_rbs_body).T,
triad(config.ax1_jcs_trans)])
self.Mbar_ground_jcs_trans = multi_dot(
[A(self.P_ground).T,
triad(config.ax1_jcs_trans)])
self.ubar_rbs_body_jcs_trans = (
multi_dot([A(config.P_rbs_body).T, config.pt1_jcs_trans]) +
(-1) * multi_dot([A(config.P_rbs_body).T, config.R_rbs_body]))
self.ubar_ground_jcs_trans = (
multi_dot([A(self.P_ground).T, config.pt1_jcs_trans]) +
(-1) * multi_dot([A(self.P_ground).T, self.R_ground]))
self.ubar_rbs_body_fas_TSDA = (
multi_dot([A(config.P_rbs_body).T, config.pt1_fas_TSDA]) +
(-1) * multi_dot([A(config.P_rbs_body).T, config.R_rbs_body]))
self.ubar_ground_fas_TSDA = (
multi_dot([A(self.P_ground).T, config.pt2_fas_TSDA]) +
(-1) * multi_dot([A(self.P_ground).T, self.R_ground]))
def eval_pos_eq(self):
config = self.config
t = self.t
x0 = self.R_ground
x1 = self.R_rbs_body_1
x2 = self.P_ground
x3 = A(x2)
x4 = self.P_rbs_body_1
x5 = A(x4)
x6 = x3.T
x7 = self.Mbar_rbs_body_1_jcs_a[:, 2:3]
x8 = self.P_rbs_body_2
x9 = A(x8)
x10 = x5.T
x11 = self.Mbar_rbs_body_2_jcs_b[:, 2:3]
x12 = (-1) * I1
self.pos_eq_blocks = (
(x0 + (-1) * x1 + multi_dot([x3, self.ubar_ground_jcs_a]) +
(-1) * multi_dot([x5, self.ubar_rbs_body_1_jcs_a])),
multi_dot([self.Mbar_ground_jcs_a[:, 0:1].T, x6, x5, x7]),
multi_dot([self.Mbar_ground_jcs_a[:, 1:2].T, x6, x5, x7]),
(x1 + (-1) * self.R_rbs_body_2 +
multi_dot([x5, self.ubar_rbs_body_1_jcs_b]) +
(-1) * multi_dot([x9, self.ubar_rbs_body_2_jcs_b])),
multi_dot([self.Mbar_rbs_body_1_jcs_b[:, 0:1].T, x10, x9, x11]),
multi_dot([self.Mbar_rbs_body_1_jcs_b[:, 1:2].T, x10, x9, x11]),
x0,
(x2 + (-1) * self.Pg_ground),
(x12 + multi_dot([x4.T, x4])),
(x12 + multi_dot([x8.T, x8])),
)
def eval_constants(self):
config = self.config
self.Mbar_vbr_wheel_hub_jcr_hub_bearing = multi_dot([A(config.P_vbr_wheel_hub).T,triad(config.ax1_jcr_hub_bearing)])
self.Mbar_vbr_wheel_upright_jcr_hub_bearing = multi_dot([A(config.P_vbr_wheel_upright).T,triad(config.ax1_jcr_hub_bearing)])
self.Mbar_vbl_wheel_hub_jcl_hub_bearing = multi_dot([A(config.P_vbl_wheel_hub).T,triad(config.ax1_jcl_hub_bearing)])
self.Mbar_vbl_wheel_upright_jcl_hub_bearing = multi_dot([A(config.P_vbl_wheel_upright).T,triad(config.ax1_jcl_hub_bearing)])
def eval_pos_eq(self):
config = self.config
t = self.t
x0 = self.Mbar_rbs_rack_jcs_rack[:, 0:1].T
x1 = self.P_rbs_rack
x2 = A(x1)
x3 = x2.T
x4 = A(self.P_vbs_chassis)
x5 = self.Mbar_vbs_chassis_jcs_rack[:, 2:3]
x6 = self.Mbar_rbs_rack_jcs_rack[:, 1:2].T
x7 = (self.R_rbs_rack + (-1) * self.R_vbs_chassis +
multi_dot([x2, self.ubar_rbs_rack_jcs_rack]) +
(-1) * multi_dot([x4, self.ubar_vbs_chassis_jcs_rack]))
x8 = I1
self.pos_eq_blocks = (
multi_dot([x0, x3, x4, x5]),
multi_dot([x6, x3, x4, x5]),
multi_dot([x0, x3, x7]),
multi_dot([x6, x3, x7]),
multi_dot([x0, x3, x4, self.Mbar_vbs_chassis_jcs_rack[:, 1:2]]),
((-1 * config.UF_mcs_rack_act(t)) * x8 +
multi_dot([self.Mbar_rbs_rack_jcs_rack[:, 2:3].T, x3, x7])),
((-1) * x8 + multi_dot([x1.T, x1])),
)
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_reactions_eq(self):
config = self.config
t = self.t
Q_ground_jcs_a = (-1) * multi_dot([np.bmat([[I3,Z1x3.T,Z1x3.T],[B(self.P_ground,self.ubar_ground_jcs_a).T,multi_dot([B(self.P_ground,self.Mbar_ground_jcs_a[:,0:1]).T,A(self.P_rbs_body),self.Mbar_rbs_body_jcs_a[:,2:3]]),multi_dot([B(self.P_ground,self.Mbar_ground_jcs_a[:,1:2]).T,A(self.P_rbs_body),self.Mbar_rbs_body_jcs_a[:,2:3]])]]),self.L_jcs_a])
self.F_ground_jcs_a = Q_ground_jcs_a[0:3]
Te_ground_jcs_a = Q_ground_jcs_a[3:7]
self.T_ground_jcs_a = ((-1) * multi_dot([skew(multi_dot([A(self.P_ground),self.ubar_ground_jcs_a])),self.F_ground_jcs_a]) + (0.5) * multi_dot([E(self.P_ground),Te_ground_jcs_a]))
self.reactions = {'F_ground_jcs_a' : self.F_ground_jcs_a,
'T_ground_jcs_a' : self.T_ground_jcs_a}
def eval_constants(self):
config = self.config
self.F_rbs_chassis_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_chassis]], dtype=np.float64)
self.ubar_rbs_chassis_fas_aero_drag = (
multi_dot([A(config.P_rbs_chassis).T, config.pt1_fas_aero_drag]) +
(-1) *
multi_dot([A(config.P_rbs_chassis).T, config.R_rbs_chassis]))
self.ubar_vbs_ground_fas_aero_drag = (
multi_dot([A(config.P_vbs_ground).T, config.pt1_fas_aero_drag]) +
(-1) * multi_dot([A(config.P_vbs_ground).T, config.R_vbs_ground]))
def eval_pos_eq(self):
config = self.config
t = self.t
x0 = A(self.P_vbr_wheel_hub).T
x1 = A(self.P_vbr_wheel_upright)
x2 = self.Mbar_vbr_wheel_upright_jcr_hub_bearing[:,0:1]
x3 = A(self.P_vbl_wheel_hub).T
x4 = A(self.P_vbl_wheel_upright)
x5 = self.Mbar_vbl_wheel_upright_jcl_hub_bearing[:,0:1]
self.pos_eq_blocks = ((cos(config.UF_mcr_wheel_lock(t)) * multi_dot([self.Mbar_vbr_wheel_hub_jcr_hub_bearing[:,1:2].T,x0,x1,x2]) + (-1 * sin(config.UF_mcr_wheel_lock(t))) * multi_dot([self.Mbar_vbr_wheel_hub_jcr_hub_bearing[:,0:1].T,x0,x1,x2])),
(cos(config.UF_mcl_wheel_lock(t)) * multi_dot([self.Mbar_vbl_wheel_hub_jcl_hub_bearing[:,1:2].T,x3,x4,x5]) + (-1 * sin(config.UF_mcl_wheel_lock(t))) * multi_dot([self.Mbar_vbl_wheel_hub_jcl_hub_bearing[:,0:1].T,x3,x4,x5])),)
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_pos_eq(self):
config = self.config
t = self.t
x0 = self.Mbar_rbs_body_jcs_trans[:, 0:1].T
x1 = self.P_rbs_body
x2 = A(x1)
x3 = x2.T
x4 = self.P_ground
x5 = A(x4)
x6 = self.Mbar_ground_jcs_trans[:, 2:3]
x7 = self.Mbar_rbs_body_jcs_trans[:, 1:2].T
x8 = self.R_ground
x9 = (self.R_rbs_body + (-1) * x8 +
multi_dot([x2, self.ubar_rbs_body_jcs_trans]) +
(-1) * multi_dot([x5, self.ubar_ground_jcs_trans]))
self.pos_eq_blocks = (
multi_dot([x0, x3, x5, x6]),
multi_dot([x7, x3, x5, x6]),
multi_dot([x0, x3, x9]),
multi_dot([x7, x3, x9]),
multi_dot([x0, x3, x5, self.Mbar_ground_jcs_trans[:, 1:2]]),
x8,
(x4 + (-1) * self.Pg_ground),
((-1) * I1 + multi_dot([x1.T, x1])),
)
def eval_jac_eq(self):
config = self.config
t = self.t
j0 = I3
j1 = self.P_ground
j2 = Z1x3
j3 = self.Mbar_rbs_body_1_jcs_a[:, 2:3]
j4 = j3.T
j5 = self.P_rbs_body_1
j6 = A(j5).T
j7 = self.Mbar_ground_jcs_a[:, 0:1]
j8 = self.Mbar_ground_jcs_a[:, 1:2]
j9 = (-1) * j0
j10 = A(j1).T
j11 = B(j5, j3)
j12 = self.Mbar_rbs_body_2_jcs_b[:, 2:3]
j13 = j12.T
j14 = self.P_rbs_body_2
j15 = A(j14).T
j16 = self.Mbar_rbs_body_1_jcs_b[:, 0:1]
j17 = self.Mbar_rbs_body_1_jcs_b[:, 1:2]
j18 = B(j14, j12)
self.jac_eq_blocks = (
j0,
B(j1, self.ubar_ground_jcs_a),
j9,
(-1) * B(j5, self.ubar_rbs_body_1_jcs_a),
j2,
multi_dot([j4, j6, B(j1, j7)]),
j2,
multi_dot([j7.T, j10, j11]),
j2,
multi_dot([j4, j6, B(j1, j8)]),
j2,
multi_dot([j8.T, j10, j11]),
j0,
B(j5, self.ubar_rbs_body_1_jcs_b),
j9,
(-1) * B(j14, self.ubar_rbs_body_2_jcs_b),
j2,
multi_dot([j13, j15, B(j5, j16)]),
j2,
multi_dot([j16.T, j6, j18]),
j2,
multi_dot([j13, j15, B(j5, j17)]),
j2,
multi_dot([j17.T, j6, j18]),
j0,
Z3x4,
Z4x3,
I4,
j2,
(2) * j5.T,
j2,
(2) * j14.T,
)
def eval_acc_eq(self):
config = self.config
t = self.t
a0 = self.Pd_rbs_chassis
self.acc_eq_blocks = ((2) * multi_dot([a0.T, a0]), )
def eval_pos_eq(self):
config = self.config
t = self.t
x0 = self.P_rbs_chassis
self.pos_eq_blocks = (((-1) * I1 + multi_dot([x0.T, x0])), )
def eval_acc_eq(self):
config = self.config
t = self.t
a0 = I1
a1 = self.Mbar_vbr_wheel_upright_jcr_hub_bearing[:,0:1]
a2 = self.P_vbr_wheel_upright
a3 = self.Pd_vbr_wheel_hub
a4 = self.Mbar_vbr_wheel_hub_jcr_hub_bearing[:,1:2]
a5 = self.Mbar_vbr_wheel_hub_jcr_hub_bearing[:,0:1]
a6 = self.P_vbr_wheel_hub
a7 = A(a6).T
a8 = self.Pd_vbr_wheel_upright
a9 = a3.T
a10 = self.Mbar_vbl_wheel_upright_jcl_hub_bearing[:,0:1]
a11 = self.P_vbl_wheel_upright
a12 = self.Pd_vbl_wheel_hub
a13 = self.Mbar_vbl_wheel_hub_jcl_hub_bearing[:,1:2]
a14 = self.Mbar_vbl_wheel_hub_jcl_hub_bearing[:,0:1]
a15 = self.P_vbl_wheel_hub
a16 = A(a15).T
a17 = self.Pd_vbl_wheel_upright
a18 = a12.T
self.acc_eq_blocks = (((-1 * derivative(config.UF_mcr_wheel_lock, t, 0.1, 2)) * a0 + multi_dot([a1.T,A(a2).T,(cos(config.UF_mcr_wheel_lock(t)) * B(a3,a4) + (-1 * sin(config.UF_mcr_wheel_lock(t))) * B(a3,a5)),a3]) + multi_dot([(cos(config.UF_mcr_wheel_lock(t)) * multi_dot([a4.T,a7]) + (-1 * sin(config.UF_mcr_wheel_lock(t))) * multi_dot([a5.T,a7])),B(a8,a1),a8]) + (2) * multi_dot([(cos(config.UF_mcr_wheel_lock(t)) * multi_dot([a9,B(a6,a4).T]) + (-1 * sin(config.UF_mcr_wheel_lock(t))) * multi_dot([a9,B(a6,a5).T])),B(a2,a1),a8])),
((-1 * derivative(config.UF_mcl_wheel_lock, t, 0.1, 2)) * a0 + multi_dot([a10.T,A(a11).T,(cos(config.UF_mcl_wheel_lock(t)) * B(a12,a13) + (-1 * sin(config.UF_mcl_wheel_lock(t))) * B(a12,a14)),a12]) + multi_dot([(cos(config.UF_mcl_wheel_lock(t)) * multi_dot([a13.T,a16]) + (-1 * sin(config.UF_mcl_wheel_lock(t))) * multi_dot([a14.T,a16])),B(a17,a10),a17]) + (2) * multi_dot([(cos(config.UF_mcl_wheel_lock(t)) * multi_dot([a18,B(a15,a13).T]) + (-1 * sin(config.UF_mcl_wheel_lock(t))) * multi_dot([a18,B(a15,a14).T])),B(a11,a10),a17])),)
def eval_constants(self):
config = self.config
self.R_ground = np.array([[0], [0], [0]], dtype=np.float64)
self.P_ground = np.array([[1], [0], [0], [0]], dtype=np.float64)
self.Pg_ground = np.array([[1], [0], [0], [0]], dtype=np.float64)
self.m_ground = 1.0
self.Jbar_ground = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]], dtype=np.float64)
self.F_rbs_body_gravity = np.array([[0], [0], [-9810.0*config.m_rbs_body]], dtype=np.float64)
self.M_ground = self.m_ground * I3
self.M_rbs_body = config.m_rbs_body * I3
self.Mbar_ground_jcs_a = multi_dot([A(self.P_ground).T,triad(config.ax1_jcs_a)])
self.Mbar_rbs_body_jcs_a = multi_dot([A(config.P_rbs_body).T,triad(config.ax1_jcs_a)])
self.ubar_ground_jcs_a = (multi_dot([A(self.P_ground).T,config.pt1_jcs_a]) + (-1) * multi_dot([A(self.P_ground).T,self.R_ground]))
self.ubar_rbs_body_jcs_a = (multi_dot([A(config.P_rbs_body).T,config.pt1_jcs_a]) + (-1) * multi_dot([A(config.P_rbs_body).T,config.R_rbs_body]))
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_pos_eq(self):
config = self.config
t = self.t
x0 = self.R_ground
x1 = self.P_ground
x2 = A(x1)
x3 = self.P_rbs_body
x4 = A(x3)
x5 = x2.T
x6 = self.Mbar_rbs_body_jcs_a[:,2:3]
self.pos_eq_blocks = ((x0 + (-1) * self.R_rbs_body + multi_dot([x2,self.ubar_ground_jcs_a]) + (-1) * multi_dot([x4,self.ubar_rbs_body_jcs_a])),
multi_dot([self.Mbar_ground_jcs_a[:,0:1].T,x5,x4,x6]),
multi_dot([self.Mbar_ground_jcs_a[:,1:2].T,x5,x4,x6]),
x0,
(x1 + (-1) * self.Pg_ground),
((-1) * I1 + multi_dot([x3.T,x3])),)
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_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 = 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_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_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_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_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_jac_eq(self):
config = self.config
t = self.t
j0 = Z1x3
j1 = self.Mbar_vbr_wheel_upright_jcr_hub_bearing[:,0:1]
j2 = self.P_vbr_wheel_upright
j3 = self.P_vbr_wheel_hub
j4 = self.Mbar_vbr_wheel_hub_jcr_hub_bearing[:,1:2]
j5 = self.Mbar_vbr_wheel_hub_jcr_hub_bearing[:,0:1]
j6 = A(j3).T
j7 = self.Mbar_vbl_wheel_upright_jcl_hub_bearing[:,0:1]
j8 = self.P_vbl_wheel_upright
j9 = self.P_vbl_wheel_hub
j10 = self.Mbar_vbl_wheel_hub_jcl_hub_bearing[:,1:2]
j11 = self.Mbar_vbl_wheel_hub_jcl_hub_bearing[:,0:1]
j12 = A(j9).T
self.jac_eq_blocks = (j0,
multi_dot([j1.T,A(j2).T,(cos(config.UF_mcr_wheel_lock(t)) * B(j3,j4) + (-1 * sin(config.UF_mcr_wheel_lock(t))) * B(j3,j5))]),
j0,
multi_dot([(cos(config.UF_mcr_wheel_lock(t)) * multi_dot([j4.T,j6]) + (-1 * sin(config.UF_mcr_wheel_lock(t))) * multi_dot([j5.T,j6])),B(j2,j1)]),
j0,
multi_dot([j7.T,A(j8).T,(cos(config.UF_mcl_wheel_lock(t)) * B(j9,j10) + (-1 * sin(config.UF_mcl_wheel_lock(t))) * B(j9,j11))]),
j0,
multi_dot([(cos(config.UF_mcl_wheel_lock(t)) * multi_dot([j10.T,j12]) + (-1 * sin(config.UF_mcl_wheel_lock(t))) * multi_dot([j11.T,j12])),B(j8,j7)]),)
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]),
)
