def eval_reactions_eq(self):
config = self.config
t = self.t
Q_rbs_input_shaft_jcs_input_bearing = -1*multi_dot([np.bmat([[np.zeros((1,3),dtype=np.float64).T,np.zeros((1,3),dtype=np.float64).T,multi_dot([A(self.P_rbs_input_shaft),self.Mbar_rbs_input_shaft_jcs_input_bearing[:,0:1]]),multi_dot([A(self.P_rbs_input_shaft),self.Mbar_rbs_input_shaft_jcs_input_bearing[:,1:2]])],[multi_dot([B(self.P_rbs_input_shaft,self.Mbar_rbs_input_shaft_jcs_input_bearing[:,0:1]).T,A(self.P_vbs_chassis),self.Mbar_vbs_chassis_jcs_input_bearing[:,2:3]]),multi_dot([B(self.P_rbs_input_shaft,self.Mbar_rbs_input_shaft_jcs_input_bearing[:,1:2]).T,A(self.P_vbs_chassis),self.Mbar_vbs_chassis_jcs_input_bearing[:,2:3]]),(multi_dot([B(self.P_rbs_input_shaft,self.Mbar_rbs_input_shaft_jcs_input_bearing[:,0:1]).T,(-1*self.R_vbs_chassis + multi_dot([A(self.P_rbs_input_shaft),self.ubar_rbs_input_shaft_jcs_input_bearing]) + -1*multi_dot([A(self.P_vbs_chassis),self.ubar_vbs_chassis_jcs_input_bearing]) + self.R_rbs_input_shaft)]) + multi_dot([B(self.P_rbs_input_shaft,self.ubar_rbs_input_shaft_jcs_input_bearing).T,A(self.P_rbs_input_shaft),self.Mbar_rbs_input_shaft_jcs_input_bearing[:,0:1]])),(multi_dot([B(self.P_rbs_input_shaft,self.Mbar_rbs_input_shaft_jcs_input_bearing[:,1:2]).T,(-1*self.R_vbs_chassis + multi_dot([A(self.P_rbs_input_shaft),self.ubar_rbs_input_shaft_jcs_input_bearing]) + -1*multi_dot([A(self.P_vbs_chassis),self.ubar_vbs_chassis_jcs_input_bearing]) + self.R_rbs_input_shaft)]) + multi_dot([B(self.P_rbs_input_shaft,self.ubar_rbs_input_shaft_jcs_input_bearing).T,A(self.P_rbs_input_shaft),self.Mbar_rbs_input_shaft_jcs_input_bearing[:,1:2]]))]]),self.L_jcs_input_bearing])
self.F_rbs_input_shaft_jcs_input_bearing = Q_rbs_input_shaft_jcs_input_bearing[0:3,0:1]
Te_rbs_input_shaft_jcs_input_bearing = Q_rbs_input_shaft_jcs_input_bearing[3:7,0:1]
self.T_rbs_input_shaft_jcs_input_bearing = (-1*multi_dot([skew(multi_dot([A(self.P_rbs_input_shaft),self.ubar_rbs_input_shaft_jcs_input_bearing])),self.F_rbs_input_shaft_jcs_input_bearing]) + 0.5*multi_dot([E(self.P_rbs_input_shaft),Te_rbs_input_shaft_jcs_input_bearing]))
Q_rbs_input_shaft_mcs_hand_wheel = -1*multi_dot([np.bmat([[np.zeros((1,3),dtype=np.float64).T],[multi_dot([(-1*sin(config.UF_mcs_hand_wheel(t))*B(self.P_rbs_input_shaft,self.Mbar_rbs_input_shaft_jcs_input_bearing[:,0:1]).T + cos(config.UF_mcs_hand_wheel(t))*B(self.P_rbs_input_shaft,self.Mbar_rbs_input_shaft_jcs_input_bearing[:,1:2]).T),A(self.P_vbs_chassis),self.Mbar_vbs_chassis_jcs_input_bearing[:,0:1]])]]),self.L_mcs_hand_wheel])
self.F_rbs_input_shaft_mcs_hand_wheel = Q_rbs_input_shaft_mcs_hand_wheel[0:3,0:1]
Te_rbs_input_shaft_mcs_hand_wheel = Q_rbs_input_shaft_mcs_hand_wheel[3:7,0:1]
self.T_rbs_input_shaft_mcs_hand_wheel = 0.5*multi_dot([E(self.P_rbs_input_shaft),Te_rbs_input_shaft_mcs_hand_wheel])
Q_rbs_input_shaft_jcs_input_connect = -1*multi_dot([np.bmat([[np.eye(3, dtype=np.float64),np.zeros((1,3),dtype=np.float64).T],[B(self.P_rbs_input_shaft,self.ubar_rbs_input_shaft_jcs_input_connect).T,multi_dot([B(self.P_rbs_input_shaft,self.Mbar_rbs_input_shaft_jcs_input_connect[:,0:1]).T,A(self.P_rbs_connect_shaft),self.Mbar_rbs_connect_shaft_jcs_input_connect[:,0:1]])]]),self.L_jcs_input_connect])
self.F_rbs_input_shaft_jcs_input_connect = Q_rbs_input_shaft_jcs_input_connect[0:3,0:1]
Te_rbs_input_shaft_jcs_input_connect = Q_rbs_input_shaft_jcs_input_connect[3:7,0:1]
self.T_rbs_input_shaft_jcs_input_connect = (-1*multi_dot([skew(multi_dot([A(self.P_rbs_input_shaft),self.ubar_rbs_input_shaft_jcs_input_connect])),self.F_rbs_input_shaft_jcs_input_connect]) + 0.5*multi_dot([E(self.P_rbs_input_shaft),Te_rbs_input_shaft_jcs_input_connect]))
Q_rbs_output_shaft_jcs_output_connect = -1*multi_dot([np.bmat([[np.eye(3, dtype=np.float64),np.zeros((1,3),dtype=np.float64).T],[B(self.P_rbs_output_shaft,self.ubar_rbs_output_shaft_jcs_output_connect).T,multi_dot([B(self.P_rbs_output_shaft,self.Mbar_rbs_output_shaft_jcs_output_connect[:,0:1]).T,A(self.P_rbs_connect_shaft),self.Mbar_rbs_connect_shaft_jcs_output_connect[:,0:1]])]]),self.L_jcs_output_connect])
self.F_rbs_output_shaft_jcs_output_connect = Q_rbs_output_shaft_jcs_output_connect[0:3,0:1]
Te_rbs_output_shaft_jcs_output_connect = Q_rbs_output_shaft_jcs_output_connect[3:7,0:1]
self.T_rbs_output_shaft_jcs_output_connect = (-1*multi_dot([skew(multi_dot([A(self.P_rbs_output_shaft),self.ubar_rbs_output_shaft_jcs_output_connect])),self.F_rbs_output_shaft_jcs_output_connect]) + 0.5*multi_dot([E(self.P_rbs_output_shaft),Te_rbs_output_shaft_jcs_output_connect]))
Q_rbs_output_shaft_jcs_output_bearing = -1*multi_dot([np.bmat([[np.eye(3, dtype=np.float64),np.zeros((1,3),dtype=np.float64).T,np.zeros((1,3),dtype=np.float64).T],[B(self.P_rbs_output_shaft,self.ubar_rbs_output_shaft_jcs_output_bearing).T,multi_dot([B(self.P_rbs_output_shaft,self.Mbar_rbs_output_shaft_jcs_output_bearing[:,0:1]).T,A(self.P_vbs_chassis),self.Mbar_vbs_chassis_jcs_output_bearing[:,2:3]]),multi_dot([B(self.P_rbs_output_shaft,self.Mbar_rbs_output_shaft_jcs_output_bearing[:,1:2]).T,A(self.P_vbs_chassis),self.Mbar_vbs_chassis_jcs_output_bearing[:,2:3]])]]),self.L_jcs_output_bearing])
self.F_rbs_output_shaft_jcs_output_bearing = Q_rbs_output_shaft_jcs_output_bearing[0:3,0:1]
Te_rbs_output_shaft_jcs_output_bearing = Q_rbs_output_shaft_jcs_output_bearing[3:7,0:1]
self.T_rbs_output_shaft_jcs_output_bearing = (-1*multi_dot([skew(multi_dot([A(self.P_rbs_output_shaft),self.ubar_rbs_output_shaft_jcs_output_bearing])),self.F_rbs_output_shaft_jcs_output_bearing]) + 0.5*multi_dot([E(self.P_rbs_output_shaft),Te_rbs_output_shaft_jcs_output_bearing]))
self.reactions = {'F_rbs_input_shaft_jcs_input_bearing' : self.F_rbs_input_shaft_jcs_input_bearing,
'T_rbs_input_shaft_jcs_input_bearing' : self.T_rbs_input_shaft_jcs_input_bearing,
'F_rbs_input_shaft_mcs_hand_wheel' : self.F_rbs_input_shaft_mcs_hand_wheel,
'T_rbs_input_shaft_mcs_hand_wheel' : self.T_rbs_input_shaft_mcs_hand_wheel,
'F_rbs_input_shaft_jcs_input_connect' : self.F_rbs_input_shaft_jcs_input_connect,
'T_rbs_input_shaft_jcs_input_connect' : self.T_rbs_input_shaft_jcs_input_connect,
'F_rbs_output_shaft_jcs_output_connect' : self.F_rbs_output_shaft_jcs_output_connect,
'T_rbs_output_shaft_jcs_output_connect' : self.T_rbs_output_shaft_jcs_output_connect,
'F_rbs_output_shaft_jcs_output_bearing' : self.F_rbs_output_shaft_jcs_output_bearing,
'T_rbs_output_shaft_jcs_output_bearing' : self.T_rbs_output_shaft_jcs_output_bearing}
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_1), self.Mbar_rbs_body_1_jcs_a[:,
2:3]
]),
multi_dot([
B(self.P_ground, self.Mbar_ground_jcs_a[:, 1:2]).T,
A(self.P_rbs_body_1), self.Mbar_rbs_body_1_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]))
Q_rbs_body_1_jcs_b = (-1) * multi_dot([
np.bmat([[I3, Z1x3.T, Z1x3.T],
[
B(self.P_rbs_body_1, self.ubar_rbs_body_1_jcs_b).T,
multi_dot([
B(self.P_rbs_body_1,
self.Mbar_rbs_body_1_jcs_b[:, 0:1]).T,
A(self.P_rbs_body_2),
self.Mbar_rbs_body_2_jcs_b[:, 2:3]
]),
multi_dot([
B(self.P_rbs_body_1,
self.Mbar_rbs_body_1_jcs_b[:, 1:2]).T,
A(self.P_rbs_body_2),
self.Mbar_rbs_body_2_jcs_b[:, 2:3]
])
]]), self.L_jcs_b
])
self.F_rbs_body_1_jcs_b = Q_rbs_body_1_jcs_b[0:3]
Te_rbs_body_1_jcs_b = Q_rbs_body_1_jcs_b[3:7]
self.T_rbs_body_1_jcs_b = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_body_1), self.ubar_rbs_body_1_jcs_b
])), self.F_rbs_body_1_jcs_b
]) + (0.5) * multi_dot([E(self.P_rbs_body_1), Te_rbs_body_1_jcs_b]))
self.reactions = {
'F_ground_jcs_a': self.F_ground_jcs_a,
'T_ground_jcs_a': self.T_ground_jcs_a,
'F_rbs_body_1_jcs_b': self.F_rbs_body_1_jcs_b,
'T_rbs_body_1_jcs_b': self.T_rbs_body_1_jcs_b
}
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_reactions_eq(self):
config = self.config
t = self.t
Q_rbr_inner_shaft_jcr_diff_joint = (-1) * multi_dot([np.bmat([[I3,Z1x3.T,Z1x3.T],[B(self.P_rbr_inner_shaft,self.ubar_rbr_inner_shaft_jcr_diff_joint).T,multi_dot([B(self.P_rbr_inner_shaft,self.Mbar_rbr_inner_shaft_jcr_diff_joint[:,0:1]).T,A(self.P_vbs_differential),self.Mbar_vbs_differential_jcr_diff_joint[:,2:3]]),multi_dot([B(self.P_rbr_inner_shaft,self.Mbar_rbr_inner_shaft_jcr_diff_joint[:,1:2]).T,A(self.P_vbs_differential),self.Mbar_vbs_differential_jcr_diff_joint[:,2:3]])]]),self.L_jcr_diff_joint])
self.F_rbr_inner_shaft_jcr_diff_joint = Q_rbr_inner_shaft_jcr_diff_joint[0:3]
Te_rbr_inner_shaft_jcr_diff_joint = Q_rbr_inner_shaft_jcr_diff_joint[3:7]
self.T_rbr_inner_shaft_jcr_diff_joint = ((-1) * multi_dot([skew(multi_dot([A(self.P_rbr_inner_shaft),self.ubar_rbr_inner_shaft_jcr_diff_joint])),self.F_rbr_inner_shaft_jcr_diff_joint]) + (0.5) * multi_dot([E(self.P_rbr_inner_shaft),Te_rbr_inner_shaft_jcr_diff_joint]))
Q_rbr_inner_shaft_jcr_inner_cv = (-1) * multi_dot([np.bmat([[I3,Z1x3.T],[B(self.P_rbr_inner_shaft,self.ubar_rbr_inner_shaft_jcr_inner_cv).T,multi_dot([B(self.P_rbr_inner_shaft,self.Mbar_rbr_inner_shaft_jcr_inner_cv[:,0:1]).T,A(self.P_rbr_coupling),self.Mbar_rbr_coupling_jcr_inner_cv[:,0:1]])]]),self.L_jcr_inner_cv])
self.F_rbr_inner_shaft_jcr_inner_cv = Q_rbr_inner_shaft_jcr_inner_cv[0:3]
Te_rbr_inner_shaft_jcr_inner_cv = Q_rbr_inner_shaft_jcr_inner_cv[3:7]
self.T_rbr_inner_shaft_jcr_inner_cv = ((-1) * multi_dot([skew(multi_dot([A(self.P_rbr_inner_shaft),self.ubar_rbr_inner_shaft_jcr_inner_cv])),self.F_rbr_inner_shaft_jcr_inner_cv]) + (0.5) * multi_dot([E(self.P_rbr_inner_shaft),Te_rbr_inner_shaft_jcr_inner_cv]))
Q_rbl_inner_shaft_jcl_diff_joint = (-1) * multi_dot([np.bmat([[I3,Z1x3.T,Z1x3.T],[B(self.P_rbl_inner_shaft,self.ubar_rbl_inner_shaft_jcl_diff_joint).T,multi_dot([B(self.P_rbl_inner_shaft,self.Mbar_rbl_inner_shaft_jcl_diff_joint[:,0:1]).T,A(self.P_vbs_differential),self.Mbar_vbs_differential_jcl_diff_joint[:,2:3]]),multi_dot([B(self.P_rbl_inner_shaft,self.Mbar_rbl_inner_shaft_jcl_diff_joint[:,1:2]).T,A(self.P_vbs_differential),self.Mbar_vbs_differential_jcl_diff_joint[:,2:3]])]]),self.L_jcl_diff_joint])
self.F_rbl_inner_shaft_jcl_diff_joint = Q_rbl_inner_shaft_jcl_diff_joint[0:3]
Te_rbl_inner_shaft_jcl_diff_joint = Q_rbl_inner_shaft_jcl_diff_joint[3:7]
self.T_rbl_inner_shaft_jcl_diff_joint = ((-1) * multi_dot([skew(multi_dot([A(self.P_rbl_inner_shaft),self.ubar_rbl_inner_shaft_jcl_diff_joint])),self.F_rbl_inner_shaft_jcl_diff_joint]) + (0.5) * multi_dot([E(self.P_rbl_inner_shaft),Te_rbl_inner_shaft_jcl_diff_joint]))
Q_rbl_inner_shaft_jcl_inner_cv = (-1) * multi_dot([np.bmat([[I3,Z1x3.T],[B(self.P_rbl_inner_shaft,self.ubar_rbl_inner_shaft_jcl_inner_cv).T,multi_dot([B(self.P_rbl_inner_shaft,self.Mbar_rbl_inner_shaft_jcl_inner_cv[:,0:1]).T,A(self.P_rbl_coupling),self.Mbar_rbl_coupling_jcl_inner_cv[:,0:1]])]]),self.L_jcl_inner_cv])
self.F_rbl_inner_shaft_jcl_inner_cv = Q_rbl_inner_shaft_jcl_inner_cv[0:3]
Te_rbl_inner_shaft_jcl_inner_cv = Q_rbl_inner_shaft_jcl_inner_cv[3:7]
self.T_rbl_inner_shaft_jcl_inner_cv = ((-1) * multi_dot([skew(multi_dot([A(self.P_rbl_inner_shaft),self.ubar_rbl_inner_shaft_jcl_inner_cv])),self.F_rbl_inner_shaft_jcl_inner_cv]) + (0.5) * multi_dot([E(self.P_rbl_inner_shaft),Te_rbl_inner_shaft_jcl_inner_cv]))
Q_rbr_coupling_jcr_outer_cv = (-1) * multi_dot([np.bmat([[Z1x3.T,multi_dot([A(self.P_rbr_coupling),self.Mbar_rbr_coupling_jcr_outer_cv[:,0:1]]),multi_dot([A(self.P_rbr_coupling),self.Mbar_rbr_coupling_jcr_outer_cv[:,1:2]])],[multi_dot([B(self.P_rbr_coupling,self.Mbar_rbr_coupling_jcr_outer_cv[:,0:1]).T,A(self.P_vbr_wheel_hub),self.Mbar_vbr_wheel_hub_jcr_outer_cv[:,0:1]]),(multi_dot([B(self.P_rbr_coupling,self.Mbar_rbr_coupling_jcr_outer_cv[:,0:1]).T,((-1) * self.R_vbr_wheel_hub + multi_dot([A(self.P_rbr_coupling),self.ubar_rbr_coupling_jcr_outer_cv]) + (-1) * multi_dot([A(self.P_vbr_wheel_hub),self.ubar_vbr_wheel_hub_jcr_outer_cv]) + self.R_rbr_coupling)]) + multi_dot([B(self.P_rbr_coupling,self.ubar_rbr_coupling_jcr_outer_cv).T,A(self.P_rbr_coupling),self.Mbar_rbr_coupling_jcr_outer_cv[:,0:1]])),(multi_dot([B(self.P_rbr_coupling,self.Mbar_rbr_coupling_jcr_outer_cv[:,1:2]).T,((-1) * self.R_vbr_wheel_hub + multi_dot([A(self.P_rbr_coupling),self.ubar_rbr_coupling_jcr_outer_cv]) + (-1) * multi_dot([A(self.P_vbr_wheel_hub),self.ubar_vbr_wheel_hub_jcr_outer_cv]) + self.R_rbr_coupling)]) + multi_dot([B(self.P_rbr_coupling,self.ubar_rbr_coupling_jcr_outer_cv).T,A(self.P_rbr_coupling),self.Mbar_rbr_coupling_jcr_outer_cv[:,1:2]]))]]),self.L_jcr_outer_cv])
self.F_rbr_coupling_jcr_outer_cv = Q_rbr_coupling_jcr_outer_cv[0:3]
Te_rbr_coupling_jcr_outer_cv = Q_rbr_coupling_jcr_outer_cv[3:7]
self.T_rbr_coupling_jcr_outer_cv = ((-1) * multi_dot([skew(multi_dot([A(self.P_rbr_coupling),self.ubar_rbr_coupling_jcr_outer_cv])),self.F_rbr_coupling_jcr_outer_cv]) + (0.5) * multi_dot([E(self.P_rbr_coupling),Te_rbr_coupling_jcr_outer_cv]))
Q_rbl_coupling_jcl_outer_cv = (-1) * multi_dot([np.bmat([[Z1x3.T,multi_dot([A(self.P_rbl_coupling),self.Mbar_rbl_coupling_jcl_outer_cv[:,0:1]]),multi_dot([A(self.P_rbl_coupling),self.Mbar_rbl_coupling_jcl_outer_cv[:,1:2]])],[multi_dot([B(self.P_rbl_coupling,self.Mbar_rbl_coupling_jcl_outer_cv[:,0:1]).T,A(self.P_vbl_wheel_hub),self.Mbar_vbl_wheel_hub_jcl_outer_cv[:,0:1]]),(multi_dot([B(self.P_rbl_coupling,self.Mbar_rbl_coupling_jcl_outer_cv[:,0:1]).T,((-1) * self.R_vbl_wheel_hub + multi_dot([A(self.P_rbl_coupling),self.ubar_rbl_coupling_jcl_outer_cv]) + (-1) * multi_dot([A(self.P_vbl_wheel_hub),self.ubar_vbl_wheel_hub_jcl_outer_cv]) + self.R_rbl_coupling)]) + multi_dot([B(self.P_rbl_coupling,self.ubar_rbl_coupling_jcl_outer_cv).T,A(self.P_rbl_coupling),self.Mbar_rbl_coupling_jcl_outer_cv[:,0:1]])),(multi_dot([B(self.P_rbl_coupling,self.Mbar_rbl_coupling_jcl_outer_cv[:,1:2]).T,((-1) * self.R_vbl_wheel_hub + multi_dot([A(self.P_rbl_coupling),self.ubar_rbl_coupling_jcl_outer_cv]) + (-1) * multi_dot([A(self.P_vbl_wheel_hub),self.ubar_vbl_wheel_hub_jcl_outer_cv]) + self.R_rbl_coupling)]) + multi_dot([B(self.P_rbl_coupling,self.ubar_rbl_coupling_jcl_outer_cv).T,A(self.P_rbl_coupling),self.Mbar_rbl_coupling_jcl_outer_cv[:,1:2]]))]]),self.L_jcl_outer_cv])
self.F_rbl_coupling_jcl_outer_cv = Q_rbl_coupling_jcl_outer_cv[0:3]
Te_rbl_coupling_jcl_outer_cv = Q_rbl_coupling_jcl_outer_cv[3:7]
self.T_rbl_coupling_jcl_outer_cv = ((-1) * multi_dot([skew(multi_dot([A(self.P_rbl_coupling),self.ubar_rbl_coupling_jcl_outer_cv])),self.F_rbl_coupling_jcl_outer_cv]) + (0.5) * multi_dot([E(self.P_rbl_coupling),Te_rbl_coupling_jcl_outer_cv]))
self.reactions = {'F_rbr_inner_shaft_jcr_diff_joint' : self.F_rbr_inner_shaft_jcr_diff_joint,
'T_rbr_inner_shaft_jcr_diff_joint' : self.T_rbr_inner_shaft_jcr_diff_joint,
'F_rbr_inner_shaft_jcr_inner_cv' : self.F_rbr_inner_shaft_jcr_inner_cv,
'T_rbr_inner_shaft_jcr_inner_cv' : self.T_rbr_inner_shaft_jcr_inner_cv,
'F_rbl_inner_shaft_jcl_diff_joint' : self.F_rbl_inner_shaft_jcl_diff_joint,
'T_rbl_inner_shaft_jcl_diff_joint' : self.T_rbl_inner_shaft_jcl_diff_joint,
'F_rbl_inner_shaft_jcl_inner_cv' : self.F_rbl_inner_shaft_jcl_inner_cv,
'T_rbl_inner_shaft_jcl_inner_cv' : self.T_rbl_inner_shaft_jcl_inner_cv,
'F_rbr_coupling_jcr_outer_cv' : self.F_rbr_coupling_jcr_outer_cv,
'T_rbr_coupling_jcr_outer_cv' : self.T_rbr_coupling_jcr_outer_cv,
'F_rbl_coupling_jcl_outer_cv' : self.F_rbl_coupling_jcl_outer_cv,
'T_rbl_coupling_jcl_outer_cv' : self.T_rbl_coupling_jcl_outer_cv}
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_l1), self.Mbar_rbs_l1_jcs_a[:, 2:3]
]),
multi_dot([
B(self.P_ground, self.Mbar_ground_jcs_a[:, 1:2]).T,
A(self.P_rbs_l1), self.Mbar_rbs_l1_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]))
Q_ground_mcs_act = (-1) * multi_dot([
np.bmat(
[[Z1x3.T],
[
multi_dot([
((-1 * sin(config.UF_mcs_act(t))) *
B(self.P_ground, self.Mbar_ground_jcs_a[:, 0:1]).T +
cos(config.UF_mcs_act(t)) *
B(self.P_ground, self.Mbar_ground_jcs_a[:, 1:2]).T),
A(self.P_rbs_l1), self.Mbar_rbs_l1_jcs_a[:, 0:1]
])
]]), self.L_mcs_act
])
self.F_ground_mcs_act = Q_ground_mcs_act[0:3]
Te_ground_mcs_act = Q_ground_mcs_act[3:7]
self.T_ground_mcs_act = (0.5) * multi_dot(
[E(self.P_ground), Te_ground_mcs_act])
Q_rbs_l1_jcs_b = (-1) * multi_dot([
np.bmat([[I3], [B(self.P_rbs_l1, self.ubar_rbs_l1_jcs_b).T]]),
self.L_jcs_b
])
self.F_rbs_l1_jcs_b = Q_rbs_l1_jcs_b[0:3]
Te_rbs_l1_jcs_b = Q_rbs_l1_jcs_b[3:7]
self.T_rbs_l1_jcs_b = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_l1), self.ubar_rbs_l1_jcs_b])),
self.F_rbs_l1_jcs_b
]) + (0.5) * multi_dot([E(self.P_rbs_l1), Te_rbs_l1_jcs_b]))
Q_rbs_l2_jcs_c = (-1) * multi_dot([
np.bmat([[I3, Z1x3.T],
[
B(self.P_rbs_l2, self.ubar_rbs_l2_jcs_c).T,
multi_dot([
B(self.P_rbs_l2, self.Mbar_rbs_l2_jcs_c[:,
0:1]).T,
A(self.P_rbs_l3), self.Mbar_rbs_l3_jcs_c[:, 0:1]
])
]]), self.L_jcs_c
])
self.F_rbs_l2_jcs_c = Q_rbs_l2_jcs_c[0:3]
Te_rbs_l2_jcs_c = Q_rbs_l2_jcs_c[3:7]
self.T_rbs_l2_jcs_c = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_l2), self.ubar_rbs_l2_jcs_c])),
self.F_rbs_l2_jcs_c
]) + (0.5) * multi_dot([E(self.P_rbs_l2), Te_rbs_l2_jcs_c]))
Q_rbs_l3_jcs_d = (-1) * multi_dot([
np.bmat([[
Z1x3.T, Z1x3.T,
multi_dot([A(self.P_rbs_l3), self.Mbar_rbs_l3_jcs_d[:, 0:1]]),
multi_dot([A(self.P_rbs_l3), self.Mbar_rbs_l3_jcs_d[:, 1:2]]),
Z1x3.T
],
[
multi_dot([
B(self.P_rbs_l3, self.Mbar_rbs_l3_jcs_d[:,
0:1]).T,
A(self.P_ground), self.Mbar_ground_jcs_d[:, 2:3]
]),
multi_dot([
B(self.P_rbs_l3, self.Mbar_rbs_l3_jcs_d[:,
1:2]).T,
A(self.P_ground), self.Mbar_ground_jcs_d[:, 2:3]
]),
(multi_dot([
B(self.P_rbs_l3, self.Mbar_rbs_l3_jcs_d[:,
0:1]).T,
((-1) * self.R_ground + multi_dot(
[A(self.P_rbs_l3), self.ubar_rbs_l3_jcs_d]) +
(-1) * multi_dot(
[A(self.P_ground), self.ubar_ground_jcs_d]) +
self.R_rbs_l3)
]) + multi_dot([
B(self.P_rbs_l3, self.ubar_rbs_l3_jcs_d).T,
A(self.P_rbs_l3), self.Mbar_rbs_l3_jcs_d[:, 0:1]
])),
(multi_dot([
B(self.P_rbs_l3, self.Mbar_rbs_l3_jcs_d[:,
1:2]).T,
((-1) * self.R_ground + multi_dot(
[A(self.P_rbs_l3), self.ubar_rbs_l3_jcs_d]) +
(-1) * multi_dot(
[A(self.P_ground), self.ubar_ground_jcs_d]) +
self.R_rbs_l3)
]) + multi_dot([
B(self.P_rbs_l3, self.ubar_rbs_l3_jcs_d).T,
A(self.P_rbs_l3), self.Mbar_rbs_l3_jcs_d[:, 1:2]
])),
multi_dot([
B(self.P_rbs_l3, self.Mbar_rbs_l3_jcs_d[:,
0:1]).T,
A(self.P_ground), self.Mbar_ground_jcs_d[:, 1:2]
])
]]), self.L_jcs_d
])
self.F_rbs_l3_jcs_d = Q_rbs_l3_jcs_d[0:3]
Te_rbs_l3_jcs_d = Q_rbs_l3_jcs_d[3:7]
self.T_rbs_l3_jcs_d = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_l3), self.ubar_rbs_l3_jcs_d])),
self.F_rbs_l3_jcs_d
]) + (0.5) * multi_dot([E(self.P_rbs_l3), Te_rbs_l3_jcs_d]))
self.reactions = {
'F_ground_jcs_a': self.F_ground_jcs_a,
'T_ground_jcs_a': self.T_ground_jcs_a,
'F_ground_mcs_act': self.F_ground_mcs_act,
'T_ground_mcs_act': self.T_ground_mcs_act,
'F_rbs_l1_jcs_b': self.F_rbs_l1_jcs_b,
'T_rbs_l1_jcs_b': self.T_rbs_l1_jcs_b,
'F_rbs_l2_jcs_c': self.F_rbs_l2_jcs_c,
'T_rbs_l2_jcs_c': self.T_rbs_l2_jcs_c,
'F_rbs_l3_jcs_d': self.F_rbs_l3_jcs_d,
'T_rbs_l3_jcs_d': self.T_rbs_l3_jcs_d
}
def eval_reactions_eq(self):
config = self.config
t = self.t
Q_ground_jcs_j1 = (-1) * multi_dot([
np.bmat(
[[I3, Z1x3.T, Z1x3.T],
[
B(self.P_ground, self.ubar_ground_jcs_j1).T,
multi_dot([
B(self.P_ground, self.Mbar_ground_jcs_j1[:, 0:1]).T,
A(self.P_rbs_l1), self.Mbar_rbs_l1_jcs_j1[:, 2:3]
]),
multi_dot([
B(self.P_ground, self.Mbar_ground_jcs_j1[:, 1:2]).T,
A(self.P_rbs_l1), self.Mbar_rbs_l1_jcs_j1[:, 2:3]
])
]]), self.L_jcs_j1
])
self.F_ground_jcs_j1 = Q_ground_jcs_j1[0:3]
Te_ground_jcs_j1 = Q_ground_jcs_j1[3:7]
self.T_ground_jcs_j1 = ((-1) * multi_dot([
skew(multi_dot([A(self.P_ground), self.ubar_ground_jcs_j1])),
self.F_ground_jcs_j1
]) + (0.5) * multi_dot([E(self.P_ground), Te_ground_jcs_j1]))
Q_rbs_l1_jcs_j2 = (-1) * multi_dot([
np.bmat([[I3], [B(self.P_rbs_l1, self.ubar_rbs_l1_jcs_j2).T]]),
self.L_jcs_j2
])
self.F_rbs_l1_jcs_j2 = Q_rbs_l1_jcs_j2[0:3]
Te_rbs_l1_jcs_j2 = Q_rbs_l1_jcs_j2[3:7]
self.T_rbs_l1_jcs_j2 = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_l1), self.ubar_rbs_l1_jcs_j2])),
self.F_rbs_l1_jcs_j2
]) + (0.5) * multi_dot([E(self.P_rbs_l1), Te_rbs_l1_jcs_j2]))
Q_rbs_l2_jcs_j3 = (-1) * multi_dot([
np.bmat([[I3], [B(self.P_rbs_l2, self.ubar_rbs_l2_jcs_j3).T]]),
self.L_jcs_j3
])
self.F_rbs_l2_jcs_j3 = Q_rbs_l2_jcs_j3[0:3]
Te_rbs_l2_jcs_j3 = Q_rbs_l2_jcs_j3[3:7]
self.T_rbs_l2_jcs_j3 = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_l2), self.ubar_rbs_l2_jcs_j3])),
self.F_rbs_l2_jcs_j3
]) + (0.5) * multi_dot([E(self.P_rbs_l2), Te_rbs_l2_jcs_j3]))
Q_rbs_l3_jcs_j4 = (-1) * multi_dot([
np.bmat(
[[I3, Z1x3.T, Z1x3.T],
[
B(self.P_rbs_l3, self.ubar_rbs_l3_jcs_j4).T,
multi_dot([
B(self.P_rbs_l3, self.Mbar_rbs_l3_jcs_j4[:, 0:1]).T,
A(self.P_ground), self.Mbar_ground_jcs_j4[:, 2:3]
]),
multi_dot([
B(self.P_rbs_l3, self.Mbar_rbs_l3_jcs_j4[:, 1:2]).T,
A(self.P_ground), self.Mbar_ground_jcs_j4[:, 2:3]
])
]]), self.L_jcs_j4
])
self.F_rbs_l3_jcs_j4 = Q_rbs_l3_jcs_j4[0:3]
Te_rbs_l3_jcs_j4 = Q_rbs_l3_jcs_j4[3:7]
self.T_rbs_l3_jcs_j4 = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_l3), self.ubar_rbs_l3_jcs_j4])),
self.F_rbs_l3_jcs_j4
]) + (0.5) * multi_dot([E(self.P_rbs_l3), Te_rbs_l3_jcs_j4]))
Q_rbs_l3_jcs_j5 = (-1) * multi_dot([
np.bmat([[I3], [B(self.P_rbs_l3, self.ubar_rbs_l3_jcs_j5).T]]),
self.L_jcs_j5
])
self.F_rbs_l3_jcs_j5 = Q_rbs_l3_jcs_j5[0:3]
Te_rbs_l3_jcs_j5 = Q_rbs_l3_jcs_j5[3:7]
self.T_rbs_l3_jcs_j5 = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_l3), self.ubar_rbs_l3_jcs_j5])),
self.F_rbs_l3_jcs_j5
]) + (0.5) * multi_dot([E(self.P_rbs_l3), Te_rbs_l3_jcs_j5]))
Q_rbs_l4_jcs_j6 = (-1) * multi_dot([
np.bmat([[I3], [B(self.P_rbs_l4, self.ubar_rbs_l4_jcs_j6).T]]),
self.L_jcs_j6
])
self.F_rbs_l4_jcs_j6 = Q_rbs_l4_jcs_j6[0:3]
Te_rbs_l4_jcs_j6 = Q_rbs_l4_jcs_j6[3:7]
self.T_rbs_l4_jcs_j6 = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_l4), self.ubar_rbs_l4_jcs_j6])),
self.F_rbs_l4_jcs_j6
]) + (0.5) * multi_dot([E(self.P_rbs_l4), Te_rbs_l4_jcs_j6]))
Q_rbs_l5_jcs_j7 = (-1) * multi_dot([
np.bmat(
[[I3, Z1x3.T, Z1x3.T],
[
B(self.P_rbs_l5, self.ubar_rbs_l5_jcs_j7).T,
multi_dot([
B(self.P_rbs_l5, self.Mbar_rbs_l5_jcs_j7[:, 0:1]).T,
A(self.P_ground), self.Mbar_ground_jcs_j7[:, 2:3]
]),
multi_dot([
B(self.P_rbs_l5, self.Mbar_rbs_l5_jcs_j7[:, 1:2]).T,
A(self.P_ground), self.Mbar_ground_jcs_j7[:, 2:3]
])
]]), self.L_jcs_j7
])
self.F_rbs_l5_jcs_j7 = Q_rbs_l5_jcs_j7[0:3]
Te_rbs_l5_jcs_j7 = Q_rbs_l5_jcs_j7[3:7]
self.T_rbs_l5_jcs_j7 = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_l5), self.ubar_rbs_l5_jcs_j7])),
self.F_rbs_l5_jcs_j7
]) + (0.5) * multi_dot([E(self.P_rbs_l5), Te_rbs_l5_jcs_j7]))
Q_rbs_l5_jcs_j8 = (-1) * multi_dot([
np.bmat([[I3], [B(self.P_rbs_l5, self.ubar_rbs_l5_jcs_j8).T]]),
self.L_jcs_j8
])
self.F_rbs_l5_jcs_j8 = Q_rbs_l5_jcs_j8[0:3]
Te_rbs_l5_jcs_j8 = Q_rbs_l5_jcs_j8[3:7]
self.T_rbs_l5_jcs_j8 = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_l5), self.ubar_rbs_l5_jcs_j8])),
self.F_rbs_l5_jcs_j8
]) + (0.5) * multi_dot([E(self.P_rbs_l5), Te_rbs_l5_jcs_j8]))
Q_rbs_l6_jcs_j9 = (-1) * multi_dot([
np.bmat([[I3], [B(self.P_rbs_l6, self.ubar_rbs_l6_jcs_j9).T]]),
self.L_jcs_j9
])
self.F_rbs_l6_jcs_j9 = Q_rbs_l6_jcs_j9[0:3]
Te_rbs_l6_jcs_j9 = Q_rbs_l6_jcs_j9[3:7]
self.T_rbs_l6_jcs_j9 = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_l6), self.ubar_rbs_l6_jcs_j9])),
self.F_rbs_l6_jcs_j9
]) + (0.5) * multi_dot([E(self.P_rbs_l6), Te_rbs_l6_jcs_j9]))
Q_rbs_l7_jcs_j10 = (-1) * multi_dot([
np.bmat(
[[I3, Z1x3.T, Z1x3.T],
[
B(self.P_rbs_l7, self.ubar_rbs_l7_jcs_j10).T,
multi_dot([
B(self.P_rbs_l7, self.Mbar_rbs_l7_jcs_j10[:, 0:1]).T,
A(self.P_ground), self.Mbar_ground_jcs_j10[:, 2:3]
]),
multi_dot([
B(self.P_rbs_l7, self.Mbar_rbs_l7_jcs_j10[:, 1:2]).T,
A(self.P_ground), self.Mbar_ground_jcs_j10[:, 2:3]
])
]]), self.L_jcs_j10
])
self.F_rbs_l7_jcs_j10 = Q_rbs_l7_jcs_j10[0:3]
Te_rbs_l7_jcs_j10 = Q_rbs_l7_jcs_j10[3:7]
self.T_rbs_l7_jcs_j10 = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_l7), self.ubar_rbs_l7_jcs_j10])),
self.F_rbs_l7_jcs_j10
]) + (0.5) * multi_dot([E(self.P_rbs_l7), Te_rbs_l7_jcs_j10]))
Q_rbs_l7_jcs_j11 = (-1) * multi_dot([
np.bmat([[I3], [B(self.P_rbs_l7, self.ubar_rbs_l7_jcs_j11).T]]),
self.L_jcs_j11
])
self.F_rbs_l7_jcs_j11 = Q_rbs_l7_jcs_j11[0:3]
Te_rbs_l7_jcs_j11 = Q_rbs_l7_jcs_j11[3:7]
self.T_rbs_l7_jcs_j11 = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_l7), self.ubar_rbs_l7_jcs_j11])),
self.F_rbs_l7_jcs_j11
]) + (0.5) * multi_dot([E(self.P_rbs_l7), Te_rbs_l7_jcs_j11]))
Q_rbs_l8_jcs_j12 = (-1) * multi_dot([
np.bmat([[I3], [B(self.P_rbs_l8, self.ubar_rbs_l8_jcs_j12).T]]),
self.L_jcs_j12
])
self.F_rbs_l8_jcs_j12 = Q_rbs_l8_jcs_j12[0:3]
Te_rbs_l8_jcs_j12 = Q_rbs_l8_jcs_j12[3:7]
self.T_rbs_l8_jcs_j12 = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_l8), self.ubar_rbs_l8_jcs_j12])),
self.F_rbs_l8_jcs_j12
]) + (0.5) * multi_dot([E(self.P_rbs_l8), Te_rbs_l8_jcs_j12]))
Q_rbs_l9_jcs_j13 = (-1) * multi_dot([
np.bmat(
[[I3, Z1x3.T, Z1x3.T],
[
B(self.P_rbs_l9, self.ubar_rbs_l9_jcs_j13).T,
multi_dot([
B(self.P_rbs_l9, self.Mbar_rbs_l9_jcs_j13[:, 0:1]).T,
A(self.P_ground), self.Mbar_ground_jcs_j13[:, 2:3]
]),
multi_dot([
B(self.P_rbs_l9, self.Mbar_rbs_l9_jcs_j13[:, 1:2]).T,
A(self.P_ground), self.Mbar_ground_jcs_j13[:, 2:3]
])
]]), self.L_jcs_j13
])
self.F_rbs_l9_jcs_j13 = Q_rbs_l9_jcs_j13[0:3]
Te_rbs_l9_jcs_j13 = Q_rbs_l9_jcs_j13[3:7]
self.T_rbs_l9_jcs_j13 = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_l9), self.ubar_rbs_l9_jcs_j13])),
self.F_rbs_l9_jcs_j13
]) + (0.5) * multi_dot([E(self.P_rbs_l9), Te_rbs_l9_jcs_j13]))
self.reactions = {
'F_ground_jcs_j1': self.F_ground_jcs_j1,
'T_ground_jcs_j1': self.T_ground_jcs_j1,
'F_rbs_l1_jcs_j2': self.F_rbs_l1_jcs_j2,
'T_rbs_l1_jcs_j2': self.T_rbs_l1_jcs_j2,
'F_rbs_l2_jcs_j3': self.F_rbs_l2_jcs_j3,
'T_rbs_l2_jcs_j3': self.T_rbs_l2_jcs_j3,
'F_rbs_l3_jcs_j4': self.F_rbs_l3_jcs_j4,
'T_rbs_l3_jcs_j4': self.T_rbs_l3_jcs_j4,
'F_rbs_l3_jcs_j5': self.F_rbs_l3_jcs_j5,
'T_rbs_l3_jcs_j5': self.T_rbs_l3_jcs_j5,
'F_rbs_l4_jcs_j6': self.F_rbs_l4_jcs_j6,
'T_rbs_l4_jcs_j6': self.T_rbs_l4_jcs_j6,
'F_rbs_l5_jcs_j7': self.F_rbs_l5_jcs_j7,
'T_rbs_l5_jcs_j7': self.T_rbs_l5_jcs_j7,
'F_rbs_l5_jcs_j8': self.F_rbs_l5_jcs_j8,
'T_rbs_l5_jcs_j8': self.T_rbs_l5_jcs_j8,
'F_rbs_l6_jcs_j9': self.F_rbs_l6_jcs_j9,
'T_rbs_l6_jcs_j9': self.T_rbs_l6_jcs_j9,
'F_rbs_l7_jcs_j10': self.F_rbs_l7_jcs_j10,
'T_rbs_l7_jcs_j10': self.T_rbs_l7_jcs_j10,
'F_rbs_l7_jcs_j11': self.F_rbs_l7_jcs_j11,
'T_rbs_l7_jcs_j11': self.T_rbs_l7_jcs_j11,
'F_rbs_l8_jcs_j12': self.F_rbs_l8_jcs_j12,
'T_rbs_l8_jcs_j12': self.T_rbs_l8_jcs_j12,
'F_rbs_l9_jcs_j13': self.F_rbs_l9_jcs_j13,
'T_rbs_l9_jcs_j13': self.T_rbs_l9_jcs_j13
}
def eval_reactions_eq(self):
config = self.config
t = self.t
Q_rbs_body_jcs_trans = (-1) * multi_dot([
np.bmat([[
Z1x3.T, Z1x3.T,
multi_dot([
A(self.P_rbs_body), self.Mbar_rbs_body_jcs_trans[:, 0:1]
]),
multi_dot([
A(self.P_rbs_body), self.Mbar_rbs_body_jcs_trans[:, 1:2]
]), Z1x3.T
],
[
multi_dot([
B(self.P_rbs_body,
self.Mbar_rbs_body_jcs_trans[:, 0:1]).T,
A(self.P_ground), self.Mbar_ground_jcs_trans[:,
2:3]
]),
multi_dot([
B(self.P_rbs_body,
self.Mbar_rbs_body_jcs_trans[:, 1:2]).T,
A(self.P_ground), self.Mbar_ground_jcs_trans[:,
2:3]
]),
(multi_dot([
B(self.P_rbs_body,
self.Mbar_rbs_body_jcs_trans[:, 0:1]).T,
((-1) * self.R_ground + multi_dot([
A(self.P_rbs_body),
self.ubar_rbs_body_jcs_trans
]) + (-1) * multi_dot([
A(self.P_ground), self.ubar_ground_jcs_trans
]) + self.R_rbs_body)
]) + multi_dot([
B(self.P_rbs_body,
self.ubar_rbs_body_jcs_trans).T,
A(self.P_rbs_body),
self.Mbar_rbs_body_jcs_trans[:, 0:1]
])),
(multi_dot([
B(self.P_rbs_body,
self.Mbar_rbs_body_jcs_trans[:, 1:2]).T,
((-1) * self.R_ground + multi_dot([
A(self.P_rbs_body),
self.ubar_rbs_body_jcs_trans
]) + (-1) * multi_dot([
A(self.P_ground), self.ubar_ground_jcs_trans
]) + self.R_rbs_body)
]) + multi_dot([
B(self.P_rbs_body,
self.ubar_rbs_body_jcs_trans).T,
A(self.P_rbs_body),
self.Mbar_rbs_body_jcs_trans[:, 1:2]
])),
multi_dot([
B(self.P_rbs_body,
self.Mbar_rbs_body_jcs_trans[:, 0:1]).T,
A(self.P_ground), self.Mbar_ground_jcs_trans[:,
1:2]
])
]]), self.L_jcs_trans
])
self.F_rbs_body_jcs_trans = Q_rbs_body_jcs_trans[0:3]
Te_rbs_body_jcs_trans = Q_rbs_body_jcs_trans[3:7]
self.T_rbs_body_jcs_trans = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_body), self.ubar_rbs_body_jcs_trans
])), self.F_rbs_body_jcs_trans
]) + (0.5) * multi_dot([E(self.P_rbs_body), Te_rbs_body_jcs_trans]))
self.F_rbs_body_fas_TSDA = (1.0 / ((multi_dot([
((-1) * self.R_ground.T +
multi_dot([self.ubar_rbs_body_fas_TSDA.T,
A(self.P_rbs_body).T]) + (-1) *
multi_dot([self.ubar_ground_fas_TSDA.T,
A(self.P_ground).T]) + self.R_rbs_body.T),
((-1) * self.R_ground +
multi_dot([A(self.P_rbs_body), self.ubar_rbs_body_fas_TSDA]) +
(-1) * multi_dot([A(self.P_ground), self.ubar_ground_fas_TSDA]) +
self.R_rbs_body)
]))**(1.0 / 2.0))[0] * (config.UF_fas_TSDA_Fd((-1 * 1.0 / ((multi_dot([
((-1) * self.R_ground.T +
multi_dot([self.ubar_rbs_body_fas_TSDA.T,
A(self.P_rbs_body).T]) + (-1) *
multi_dot([self.ubar_ground_fas_TSDA.T,
A(self.P_ground).T]) + self.R_rbs_body.T),
((-1) * self.R_ground +
multi_dot([A(self.P_rbs_body), self.ubar_rbs_body_fas_TSDA]) +
(-1) * multi_dot([A(self.P_ground), self.ubar_ground_fas_TSDA]) +
self.R_rbs_body)
]))**(1.0 / 2.0))[0]) * multi_dot([
((-1) * self.R_ground.T +
multi_dot([self.ubar_rbs_body_fas_TSDA.T,
A(self.P_rbs_body).T]) + (-1) *
multi_dot([self.ubar_ground_fas_TSDA.T,
A(self.P_ground).T]) + self.R_rbs_body.T),
((-1) * self.Rd_ground + multi_dot([
B(self.P_rbs_body, self.ubar_rbs_body_fas_TSDA),
self.Pd_rbs_body
]) + (-1) * multi_dot(
[B(self.P_ground, self.ubar_ground_fas_TSDA), self.Pd_ground])
+ self.Rd_rbs_body)
])) + config.UF_fas_TSDA_Fs((config.fas_TSDA_FL + (-1 * ((multi_dot([
((-1) * self.R_ground.T +
multi_dot([self.ubar_rbs_body_fas_TSDA.T,
A(self.P_rbs_body).T]) + (-1) *
multi_dot([self.ubar_ground_fas_TSDA.T,
A(self.P_ground).T]) + self.R_rbs_body.T),
((-1) * self.R_ground +
multi_dot([A(self.P_rbs_body), self.ubar_rbs_body_fas_TSDA]) +
(-1) * multi_dot([A(self.P_ground), self.ubar_ground_fas_TSDA]) +
self.R_rbs_body)
]))**(1.0 / 2.0))[0]))))) * (
(-1) * self.R_ground +
multi_dot([A(self.P_rbs_body), self.ubar_rbs_body_fas_TSDA]) +
(-1) * multi_dot([A(self.P_ground), self.ubar_ground_fas_TSDA]) +
self.R_rbs_body)
self.T_rbs_body_fas_TSDA = Z3x1
self.reactions = {
'F_rbs_body_jcs_trans': self.F_rbs_body_jcs_trans,
'T_rbs_body_jcs_trans': self.T_rbs_body_jcs_trans,
'F_rbs_body_fas_TSDA': self.F_rbs_body_fas_TSDA,
'T_rbs_body_fas_TSDA': self.T_rbs_body_fas_TSDA
}
def eval_reactions_eq(self):
config = self.config
t = self.t
Q_rbs_rack_jcs_rack = (-1) * multi_dot([
np.bmat([[
Z1x3.T, Z1x3.T,
multi_dot(
[A(self.P_rbs_rack), self.Mbar_rbs_rack_jcs_rack[:, 0:1]]),
multi_dot([
A(self.P_rbs_rack), self.Mbar_rbs_rack_jcs_rack[:, 1:2]
]), Z1x3.T
],
[
multi_dot([
B(self.P_rbs_rack,
self.Mbar_rbs_rack_jcs_rack[:, 0:1]).T,
A(self.P_vbs_chassis),
self.Mbar_vbs_chassis_jcs_rack[:, 2:3]
]),
multi_dot([
B(self.P_rbs_rack,
self.Mbar_rbs_rack_jcs_rack[:, 1:2]).T,
A(self.P_vbs_chassis),
self.Mbar_vbs_chassis_jcs_rack[:, 2:3]
]),
(multi_dot([
B(self.P_rbs_rack,
self.Mbar_rbs_rack_jcs_rack[:, 0:1]).T,
((-1) * self.R_vbs_chassis + multi_dot([
A(self.P_rbs_rack),
self.ubar_rbs_rack_jcs_rack
]) + (-1) * multi_dot([
A(self.P_vbs_chassis),
self.ubar_vbs_chassis_jcs_rack
]) + self.R_rbs_rack)
]) + multi_dot([
B(self.P_rbs_rack, self.ubar_rbs_rack_jcs_rack).T,
A(self.P_rbs_rack),
self.Mbar_rbs_rack_jcs_rack[:, 0:1]
])),
(multi_dot([
B(self.P_rbs_rack,
self.Mbar_rbs_rack_jcs_rack[:, 1:2]).T,
((-1) * self.R_vbs_chassis + multi_dot([
A(self.P_rbs_rack),
self.ubar_rbs_rack_jcs_rack
]) + (-1) * multi_dot([
A(self.P_vbs_chassis),
self.ubar_vbs_chassis_jcs_rack
]) + self.R_rbs_rack)
]) + multi_dot([
B(self.P_rbs_rack, self.ubar_rbs_rack_jcs_rack).T,
A(self.P_rbs_rack),
self.Mbar_rbs_rack_jcs_rack[:, 1:2]
])),
multi_dot([
B(self.P_rbs_rack,
self.Mbar_rbs_rack_jcs_rack[:, 0:1]).T,
A(self.P_vbs_chassis),
self.Mbar_vbs_chassis_jcs_rack[:, 1:2]
])
]]), self.L_jcs_rack
])
self.F_rbs_rack_jcs_rack = Q_rbs_rack_jcs_rack[0:3]
Te_rbs_rack_jcs_rack = Q_rbs_rack_jcs_rack[3:7]
self.T_rbs_rack_jcs_rack = ((-1) * multi_dot([
skew(multi_dot([A(self.P_rbs_rack), self.ubar_rbs_rack_jcs_rack])),
self.F_rbs_rack_jcs_rack
]) + (0.5) * multi_dot([E(self.P_rbs_rack), Te_rbs_rack_jcs_rack]))
Q_rbs_rack_mcs_rack_act = (-1) * multi_dot([
np.bmat(
[[
multi_dot([
A(self.P_rbs_rack), self.Mbar_rbs_rack_jcs_rack[:, 2:3]
])
],
[(multi_dot([
B(self.P_rbs_rack, self.Mbar_rbs_rack_jcs_rack[:, 2:3]).T,
((-1) * self.R_vbs_chassis + multi_dot(
[A(self.P_rbs_rack), self.ubar_rbs_rack_jcs_rack]) +
(-1) * multi_dot([
A(self.P_vbs_chassis), self.ubar_vbs_chassis_jcs_rack
]) + self.R_rbs_rack)
]) + multi_dot([
B(self.P_rbs_rack, self.ubar_rbs_rack_jcs_rack).T,
A(self.P_rbs_rack), self.Mbar_rbs_rack_jcs_rack[:, 2:3]
]))]]), self.L_mcs_rack_act
])
self.F_rbs_rack_mcs_rack_act = Q_rbs_rack_mcs_rack_act[0:3]
Te_rbs_rack_mcs_rack_act = Q_rbs_rack_mcs_rack_act[3:7]
self.T_rbs_rack_mcs_rack_act = (0.5) * multi_dot(
[E(self.P_rbs_rack), Te_rbs_rack_mcs_rack_act])
self.reactions = {
'F_rbs_rack_jcs_rack': self.F_rbs_rack_jcs_rack,
'T_rbs_rack_jcs_rack': self.T_rbs_rack_jcs_rack,
'F_rbs_rack_mcs_rack_act': self.F_rbs_rack_mcs_rack_act,
'T_rbs_rack_mcs_rack_act': self.T_rbs_rack_mcs_rack_act
}
def eval_reactions_eq(self):
config = self.config
t = self.t
Q_rbr_rocker_jcr_rocker_chassis = (-1) * multi_dot([
np.bmat(
[[I3, Z1x3.T, Z1x3.T],
[
B(self.P_rbr_rocker,
self.ubar_rbr_rocker_jcr_rocker_chassis).T,
multi_dot([
B(self.P_rbr_rocker,
self.Mbar_rbr_rocker_jcr_rocker_chassis[:, 0:1]).T,
A(self.P_vbs_chassis),
self.Mbar_vbs_chassis_jcr_rocker_chassis[:, 2:3]
]),
multi_dot([
B(self.P_rbr_rocker,
self.Mbar_rbr_rocker_jcr_rocker_chassis[:, 1:2]).T,
A(self.P_vbs_chassis),
self.Mbar_vbs_chassis_jcr_rocker_chassis[:, 2:3]
])
]]), self.L_jcr_rocker_chassis
])
self.F_rbr_rocker_jcr_rocker_chassis = Q_rbr_rocker_jcr_rocker_chassis[
0:3]
Te_rbr_rocker_jcr_rocker_chassis = Q_rbr_rocker_jcr_rocker_chassis[3:7]
self.T_rbr_rocker_jcr_rocker_chassis = ((-1) * multi_dot([
skew(
multi_dot([
A(self.P_rbr_rocker),
self.ubar_rbr_rocker_jcr_rocker_chassis
])), self.F_rbr_rocker_jcr_rocker_chassis
]) + (0.5) * multi_dot(
[E(self.P_rbr_rocker), Te_rbr_rocker_jcr_rocker_chassis]))
Q_rbr_rocker_jcs_rocker_uni = (-1) * multi_dot([
np.bmat([[I3, Z1x3.T],
[
B(self.P_rbr_rocker,
self.ubar_rbr_rocker_jcs_rocker_uni).T,
multi_dot([
B(self.P_rbr_rocker,
self.Mbar_rbr_rocker_jcs_rocker_uni[:, 0:1]).T,
A(self.P_rbs_coupler),
self.Mbar_rbs_coupler_jcs_rocker_uni[:, 0:1]
])
]]), self.L_jcs_rocker_uni
])
self.F_rbr_rocker_jcs_rocker_uni = Q_rbr_rocker_jcs_rocker_uni[0:3]
Te_rbr_rocker_jcs_rocker_uni = Q_rbr_rocker_jcs_rocker_uni[3:7]
self.T_rbr_rocker_jcs_rocker_uni = ((-1) * multi_dot([
skew(
multi_dot([
A(self.P_rbr_rocker), self.ubar_rbr_rocker_jcs_rocker_uni
])), self.F_rbr_rocker_jcs_rocker_uni
]) + (0.5) * multi_dot(
[E(self.P_rbr_rocker), Te_rbr_rocker_jcs_rocker_uni]))
Q_rbl_rocker_jcl_rocker_chassis = (-1) * multi_dot([
np.bmat(
[[I3, Z1x3.T, Z1x3.T],
[
B(self.P_rbl_rocker,
self.ubar_rbl_rocker_jcl_rocker_chassis).T,
multi_dot([
B(self.P_rbl_rocker,
self.Mbar_rbl_rocker_jcl_rocker_chassis[:, 0:1]).T,
A(self.P_vbs_chassis),
self.Mbar_vbs_chassis_jcl_rocker_chassis[:, 2:3]
]),
multi_dot([
B(self.P_rbl_rocker,
self.Mbar_rbl_rocker_jcl_rocker_chassis[:, 1:2]).T,
A(self.P_vbs_chassis),
self.Mbar_vbs_chassis_jcl_rocker_chassis[:, 2:3]
])
]]), self.L_jcl_rocker_chassis
])
self.F_rbl_rocker_jcl_rocker_chassis = Q_rbl_rocker_jcl_rocker_chassis[
0:3]
Te_rbl_rocker_jcl_rocker_chassis = Q_rbl_rocker_jcl_rocker_chassis[3:7]
self.T_rbl_rocker_jcl_rocker_chassis = ((-1) * multi_dot([
skew(
multi_dot([
A(self.P_rbl_rocker),
self.ubar_rbl_rocker_jcl_rocker_chassis
])), self.F_rbl_rocker_jcl_rocker_chassis
]) + (0.5) * multi_dot(
[E(self.P_rbl_rocker), Te_rbl_rocker_jcl_rocker_chassis]))
Q_rbl_rocker_mcs_steer_act = (-1) * multi_dot([
np.bmat([
[Z1x3.T],
[
multi_dot([
((-1 * sin(config.UF_mcs_steer_act(t))) *
B(self.P_rbl_rocker,
self.Mbar_rbl_rocker_jcl_rocker_chassis[:, 0:1]).T +
cos(config.UF_mcs_steer_act(t)) *
B(self.P_rbl_rocker,
self.Mbar_rbl_rocker_jcl_rocker_chassis[:, 1:2]).T),
A(self.P_vbs_chassis),
self.Mbar_vbs_chassis_jcl_rocker_chassis[:, 0:1]
])
]
]), self.L_mcs_steer_act
])
self.F_rbl_rocker_mcs_steer_act = Q_rbl_rocker_mcs_steer_act[0:3]
Te_rbl_rocker_mcs_steer_act = Q_rbl_rocker_mcs_steer_act[3:7]
self.T_rbl_rocker_mcs_steer_act = (0.5) * multi_dot(
[E(self.P_rbl_rocker), Te_rbl_rocker_mcs_steer_act])
Q_rbl_rocker_jcs_rocker_sph = (-1) * multi_dot([
np.bmat([
[I3],
[B(self.P_rbl_rocker, self.ubar_rbl_rocker_jcs_rocker_sph).T]
]), self.L_jcs_rocker_sph
])
self.F_rbl_rocker_jcs_rocker_sph = Q_rbl_rocker_jcs_rocker_sph[0:3]
Te_rbl_rocker_jcs_rocker_sph = Q_rbl_rocker_jcs_rocker_sph[3:7]
self.T_rbl_rocker_jcs_rocker_sph = ((-1) * multi_dot([
skew(
multi_dot([
A(self.P_rbl_rocker), self.ubar_rbl_rocker_jcs_rocker_sph
])), self.F_rbl_rocker_jcs_rocker_sph
]) + (0.5) * multi_dot(
[E(self.P_rbl_rocker), Te_rbl_rocker_jcs_rocker_sph]))
self.reactions = {
'F_rbr_rocker_jcr_rocker_chassis':
self.F_rbr_rocker_jcr_rocker_chassis,
'T_rbr_rocker_jcr_rocker_chassis':
self.T_rbr_rocker_jcr_rocker_chassis,
'F_rbr_rocker_jcs_rocker_uni': self.F_rbr_rocker_jcs_rocker_uni,
'T_rbr_rocker_jcs_rocker_uni': self.T_rbr_rocker_jcs_rocker_uni,
'F_rbl_rocker_jcl_rocker_chassis':
self.F_rbl_rocker_jcl_rocker_chassis,
'T_rbl_rocker_jcl_rocker_chassis':
self.T_rbl_rocker_jcl_rocker_chassis,
'F_rbl_rocker_mcs_steer_act': self.F_rbl_rocker_mcs_steer_act,
'T_rbl_rocker_mcs_steer_act': self.T_rbl_rocker_mcs_steer_act,
'F_rbl_rocker_jcs_rocker_sph': self.F_rbl_rocker_jcs_rocker_sph,
'T_rbl_rocker_jcs_rocker_sph': self.T_rbl_rocker_jcs_rocker_sph
}
def eval_reactions_eq(self):
config = self.config
t = self.t
Q_rbs_body_1_jcs_trans_1 = (-1) * multi_dot([
np.bmat([[
Z1x3.T, Z1x3.T,
multi_dot([
A(self.P_rbs_body_1), self.Mbar_rbs_body_1_jcs_trans_1[:,
0:1]
]),
multi_dot([
A(self.P_rbs_body_1), self.Mbar_rbs_body_1_jcs_trans_1[:,
1:2]
]), Z1x3.T
],
[
multi_dot([
B(self.P_rbs_body_1,
self.Mbar_rbs_body_1_jcs_trans_1[:, 0:1]).T,
A(self.P_ground),
self.Mbar_ground_jcs_trans_1[:, 2:3]
]),
multi_dot([
B(self.P_rbs_body_1,
self.Mbar_rbs_body_1_jcs_trans_1[:, 1:2]).T,
A(self.P_ground),
self.Mbar_ground_jcs_trans_1[:, 2:3]
]),
(multi_dot([
B(self.P_rbs_body_1,
self.Mbar_rbs_body_1_jcs_trans_1[:, 0:1]).T,
((-1) * self.R_ground + multi_dot([
A(self.P_rbs_body_1),
self.ubar_rbs_body_1_jcs_trans_1
]) + (-1) * multi_dot([
A(self.P_ground), self.ubar_ground_jcs_trans_1
]) + self.R_rbs_body_1)
]) + multi_dot([
B(self.P_rbs_body_1,
self.ubar_rbs_body_1_jcs_trans_1).T,
A(self.P_rbs_body_1),
self.Mbar_rbs_body_1_jcs_trans_1[:, 0:1]
])),
(multi_dot([
B(self.P_rbs_body_1,
self.Mbar_rbs_body_1_jcs_trans_1[:, 1:2]).T,
((-1) * self.R_ground + multi_dot([
A(self.P_rbs_body_1),
self.ubar_rbs_body_1_jcs_trans_1
]) + (-1) * multi_dot([
A(self.P_ground), self.ubar_ground_jcs_trans_1
]) + self.R_rbs_body_1)
]) + multi_dot([
B(self.P_rbs_body_1,
self.ubar_rbs_body_1_jcs_trans_1).T,
A(self.P_rbs_body_1),
self.Mbar_rbs_body_1_jcs_trans_1[:, 1:2]
])),
multi_dot([
B(self.P_rbs_body_1,
self.Mbar_rbs_body_1_jcs_trans_1[:, 0:1]).T,
A(self.P_ground),
self.Mbar_ground_jcs_trans_1[:, 1:2]
])
]]), self.L_jcs_trans_1
])
self.F_rbs_body_1_jcs_trans_1 = Q_rbs_body_1_jcs_trans_1[0:3]
Te_rbs_body_1_jcs_trans_1 = Q_rbs_body_1_jcs_trans_1[3:7]
self.T_rbs_body_1_jcs_trans_1 = (
(-1) * multi_dot([
skew(
multi_dot([
A(self.P_rbs_body_1), self.ubar_rbs_body_1_jcs_trans_1
])), self.F_rbs_body_1_jcs_trans_1
]) + (0.5) *
multi_dot([E(self.P_rbs_body_1), Te_rbs_body_1_jcs_trans_1]))
self.F_rbs_body_1_fas_TSDA_1 = (1.0 / ((multi_dot([
((-1) * self.R_ground.T + multi_dot(
[self.ubar_rbs_body_1_fas_TSDA_1.T,
A(self.P_rbs_body_1).T]) + (-1) *
multi_dot([self.ubar_ground_fas_TSDA_1.T,
A(self.P_ground).T]) + self.R_rbs_body_1.T),
((-1) * self.R_ground + multi_dot(
[A(self.P_rbs_body_1), self.ubar_rbs_body_1_fas_TSDA_1]) +
(-1) * multi_dot([A(self.P_ground), self.ubar_ground_fas_TSDA_1])
+ self.R_rbs_body_1)
]))**(1.0 / 2.0))[0] * (config.UF_fas_TSDA_1_Fd(
(-1 * 1.0 / ((multi_dot([
((-1) * self.R_ground.T + multi_dot([
self.ubar_rbs_body_1_fas_TSDA_1.T,
A(self.P_rbs_body_1).T
]) + (-1) *
multi_dot([self.ubar_ground_fas_TSDA_1.T,
A(self.P_ground).T]) + self.R_rbs_body_1.T),
((-1) * self.R_ground + multi_dot(
[A(self.P_rbs_body_1), self.ubar_rbs_body_1_fas_TSDA_1]) +
(-1) *
multi_dot([A(self.P_ground), self.ubar_ground_fas_TSDA_1]) +
self.R_rbs_body_1)
]))**(1.0 / 2.0))[0]) *
multi_dot([((-1) * self.R_ground.T + multi_dot(
[self.ubar_rbs_body_1_fas_TSDA_1.T,
A(self.P_rbs_body_1).T]) + (-1) * multi_dot(
[self.ubar_ground_fas_TSDA_1.T,
A(self.P_ground).T]) + self.R_rbs_body_1.T),
((-1) * self.Rd_ground + multi_dot([
B(self.P_rbs_body_1, self.ubar_rbs_body_1_fas_TSDA_1
), self.Pd_rbs_body_1
]) + (-1) * multi_dot([
B(self.P_ground, self.ubar_ground_fas_TSDA_1),
self.Pd_ground
]) + self.Rd_rbs_body_1)])
) + config.UF_fas_TSDA_1_Fs((config.fas_TSDA_1_FL + (-1 * ((multi_dot([
((-1) * self.R_ground.T + multi_dot(
[self.ubar_rbs_body_1_fas_TSDA_1.T,
A(self.P_rbs_body_1).T]) + (-1) *
multi_dot([self.ubar_ground_fas_TSDA_1.T,
A(self.P_ground).T]) + self.R_rbs_body_1.T),
((-1) * self.R_ground + multi_dot(
[A(self.P_rbs_body_1), self.ubar_rbs_body_1_fas_TSDA_1]) +
(-1) * multi_dot([A(self.P_ground), self.ubar_ground_fas_TSDA_1])
+ self.R_rbs_body_1)
]))**(1.0 / 2.0))[0]))))) * (
(-1) * self.R_ground + multi_dot(
[A(self.P_rbs_body_1), self.ubar_rbs_body_1_fas_TSDA_1]) +
(-1) * multi_dot([A(self.P_ground), self.ubar_ground_fas_TSDA_1]) +
self.R_rbs_body_1)
self.T_rbs_body_1_fas_TSDA_1 = Z3x1
Q_rbs_body_2_jcs_trans_2 = (-1) * multi_dot([
np.bmat([[
Z1x3.T, Z1x3.T,
multi_dot([
A(self.P_rbs_body_2), self.Mbar_rbs_body_2_jcs_trans_2[:,
0:1]
]),
multi_dot([
A(self.P_rbs_body_2), self.Mbar_rbs_body_2_jcs_trans_2[:,
1:2]
]), Z1x3.T
],
[
multi_dot([
B(self.P_rbs_body_2,
self.Mbar_rbs_body_2_jcs_trans_2[:, 0:1]).T,
A(self.P_ground),
self.Mbar_ground_jcs_trans_2[:, 2:3]
]),
multi_dot([
B(self.P_rbs_body_2,
self.Mbar_rbs_body_2_jcs_trans_2[:, 1:2]).T,
A(self.P_ground),
self.Mbar_ground_jcs_trans_2[:, 2:3]
]),
(multi_dot([
B(self.P_rbs_body_2,
self.Mbar_rbs_body_2_jcs_trans_2[:, 0:1]).T,
((-1) * self.R_ground + multi_dot([
A(self.P_rbs_body_2),
self.ubar_rbs_body_2_jcs_trans_2
]) + (-1) * multi_dot([
A(self.P_ground), self.ubar_ground_jcs_trans_2
]) + self.R_rbs_body_2)
]) + multi_dot([
B(self.P_rbs_body_2,
self.ubar_rbs_body_2_jcs_trans_2).T,
A(self.P_rbs_body_2),
self.Mbar_rbs_body_2_jcs_trans_2[:, 0:1]
])),
(multi_dot([
B(self.P_rbs_body_2,
self.Mbar_rbs_body_2_jcs_trans_2[:, 1:2]).T,
((-1) * self.R_ground + multi_dot([
A(self.P_rbs_body_2),
self.ubar_rbs_body_2_jcs_trans_2
]) + (-1) * multi_dot([
A(self.P_ground), self.ubar_ground_jcs_trans_2
]) + self.R_rbs_body_2)
]) + multi_dot([
B(self.P_rbs_body_2,
self.ubar_rbs_body_2_jcs_trans_2).T,
A(self.P_rbs_body_2),
self.Mbar_rbs_body_2_jcs_trans_2[:, 1:2]
])),
multi_dot([
B(self.P_rbs_body_2,
self.Mbar_rbs_body_2_jcs_trans_2[:, 0:1]).T,
A(self.P_ground),
self.Mbar_ground_jcs_trans_2[:, 1:2]
])
]]), self.L_jcs_trans_2
])
self.F_rbs_body_2_jcs_trans_2 = Q_rbs_body_2_jcs_trans_2[0:3]
Te_rbs_body_2_jcs_trans_2 = Q_rbs_body_2_jcs_trans_2[3:7]
self.T_rbs_body_2_jcs_trans_2 = (
(-1) * multi_dot([
skew(
multi_dot([
A(self.P_rbs_body_2), self.ubar_rbs_body_2_jcs_trans_2
])), self.F_rbs_body_2_jcs_trans_2
]) + (0.5) *
multi_dot([E(self.P_rbs_body_2), Te_rbs_body_2_jcs_trans_2]))
self.F_rbs_body_2_fas_TSDA_2 = (1.0 / ((multi_dot([
((-1) * self.R_rbs_body_1.T + multi_dot(
[self.ubar_rbs_body_2_fas_TSDA_2.T,
A(self.P_rbs_body_2).T]) +
(-1) * multi_dot(
[self.ubar_rbs_body_1_fas_TSDA_2.T,
A(self.P_rbs_body_1).T]) + self.R_rbs_body_2.T),
((-1) * self.R_rbs_body_1 + multi_dot(
[A(self.P_rbs_body_2), self.ubar_rbs_body_2_fas_TSDA_2]) +
(-1) *
multi_dot([A(self.P_rbs_body_1), self.ubar_rbs_body_1_fas_TSDA_2])
+ self.R_rbs_body_2)
]))**(1.0 / 2.0))[0] * (config.UF_fas_TSDA_2_Fd(
(-1 * 1.0 / ((multi_dot([
((-1) * self.R_rbs_body_1.T + multi_dot([
self.ubar_rbs_body_2_fas_TSDA_2.T,
A(self.P_rbs_body_2).T
]) + (-1) * multi_dot([
self.ubar_rbs_body_1_fas_TSDA_2.T,
A(self.P_rbs_body_1).T
]) + self.R_rbs_body_2.T),
((-1) * self.R_rbs_body_1 + multi_dot(
[A(self.P_rbs_body_2), self.ubar_rbs_body_2_fas_TSDA_2]) +
(-1) * multi_dot(
[A(self.P_rbs_body_1), self.ubar_rbs_body_1_fas_TSDA_2]) +
self.R_rbs_body_2)
]))**(1.0 / 2.0))[0]) *
multi_dot([((-1) * self.R_rbs_body_1.T + multi_dot(
[self.ubar_rbs_body_2_fas_TSDA_2.T,
A(self.P_rbs_body_2).T]) + (-1) * multi_dot([
self.ubar_rbs_body_1_fas_TSDA_2.T,
A(self.P_rbs_body_1).T
]) + self.R_rbs_body_2.T),
((-1) * self.Rd_rbs_body_1 + multi_dot([
B(self.P_rbs_body_2, self.ubar_rbs_body_2_fas_TSDA_2
), self.Pd_rbs_body_2
]) + (-1) * multi_dot([
B(self.P_rbs_body_1, self.ubar_rbs_body_1_fas_TSDA_2
), self.Pd_rbs_body_1
]) + self.Rd_rbs_body_2)])
) + config.UF_fas_TSDA_2_Fs((config.fas_TSDA_2_FL + (-1 * ((multi_dot([
((-1) * self.R_rbs_body_1.T + multi_dot(
[self.ubar_rbs_body_2_fas_TSDA_2.T,
A(self.P_rbs_body_2).T]) +
(-1) * multi_dot(
[self.ubar_rbs_body_1_fas_TSDA_2.T,
A(self.P_rbs_body_1).T]) + self.R_rbs_body_2.T),
((-1) * self.R_rbs_body_1 + multi_dot(
[A(self.P_rbs_body_2), self.ubar_rbs_body_2_fas_TSDA_2]) +
(-1) *
multi_dot([A(self.P_rbs_body_1), self.ubar_rbs_body_1_fas_TSDA_2])
+ self.R_rbs_body_2)
]))**(1.0 / 2.0))[0]))))) * (
(-1) * self.R_rbs_body_1 + multi_dot(
[A(self.P_rbs_body_2), self.ubar_rbs_body_2_fas_TSDA_2]) +
(-1) *
multi_dot([A(self.P_rbs_body_1), self.ubar_rbs_body_1_fas_TSDA_2])
+ self.R_rbs_body_2)
self.T_rbs_body_2_fas_TSDA_2 = Z3x1
self.reactions = {
'F_rbs_body_1_jcs_trans_1': self.F_rbs_body_1_jcs_trans_1,
'T_rbs_body_1_jcs_trans_1': self.T_rbs_body_1_jcs_trans_1,
'F_rbs_body_1_fas_TSDA_1': self.F_rbs_body_1_fas_TSDA_1,
'T_rbs_body_1_fas_TSDA_1': self.T_rbs_body_1_fas_TSDA_1,
'F_rbs_body_2_jcs_trans_2': self.F_rbs_body_2_jcs_trans_2,
'T_rbs_body_2_jcs_trans_2': self.T_rbs_body_2_jcs_trans_2,
'F_rbs_body_2_fas_TSDA_2': self.F_rbs_body_2_fas_TSDA_2,
'T_rbs_body_2_fas_TSDA_2': self.T_rbs_body_2_fas_TSDA_2
}
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])),
)