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_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_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_jac_eq(self):
config = self.config
t = self.t
j0 = Z1x3
j1 = self.Mbar_vbs_chassis_jcs_rack[:, 2:3]
j2 = j1.T
j3 = self.P_vbs_chassis
j4 = A(j3).T
j5 = self.P_rbs_rack
j6 = self.Mbar_rbs_rack_jcs_rack[:, 0:1]
j7 = B(j5, j6)
j8 = self.Mbar_rbs_rack_jcs_rack[:, 1:2]
j9 = B(j5, j8)
j10 = j6.T
j11 = A(j5).T
j12 = multi_dot([j10, j11])
j13 = self.ubar_rbs_rack_jcs_rack
j14 = B(j5, j13)
j15 = self.ubar_vbs_chassis_jcs_rack
j16 = (self.R_rbs_rack.T + (-1) * self.R_vbs_chassis.T +
multi_dot([j13.T, j11]) + (-1) * multi_dot([j15.T, j4]))
j17 = j8.T
j18 = multi_dot([j17, j11])
j19 = self.Mbar_vbs_chassis_jcs_rack[:, 1:2]
j20 = B(j3, j1)
j21 = B(j3, j15)
j22 = self.Mbar_rbs_rack_jcs_rack[:, 2:3]
j23 = j22.T
j24 = multi_dot([j23, j11])
self.jac_eq_blocks = (
j0,
multi_dot([j2, j4, j7]),
j0,
multi_dot([j10, j11, j20]),
j0,
multi_dot([j2, j4, j9]),
j0,
multi_dot([j17, j11, j20]),
j12,
(multi_dot([j10, j11, j14]) + multi_dot([j16, j7])),
(-1) * j12,
(-1) * multi_dot([j10, j11, j21]),
j18,
(multi_dot([j17, j11, j14]) + multi_dot([j16, j9])),
(-1) * j18,
(-1) * multi_dot([j17, j11, j21]),
j0,
multi_dot([j19.T, j4, j7]),
j0,
multi_dot([j10, j11, B(j3, j19)]),
j24,
(multi_dot([j23, j11, j14]) + multi_dot([j16, B(j5, j22)])),
(-1) * j24,
(-1) * multi_dot([j23, j11, j21]),
j0,
(2) * j5.T,
)
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_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_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_jac_eq(self):
config = self.config
t = self.t
j0 = Z1x3
j1 = self.Mbar_ground_jcs_trans[:, 2:3]
j2 = j1.T
j3 = self.P_ground
j4 = A(j3).T
j5 = self.P_rbs_body
j6 = self.Mbar_rbs_body_jcs_trans[:, 0:1]
j7 = B(j5, j6)
j8 = self.Mbar_rbs_body_jcs_trans[:, 1:2]
j9 = B(j5, j8)
j10 = j6.T
j11 = A(j5).T
j12 = multi_dot([j10, j11])
j13 = self.ubar_rbs_body_jcs_trans
j14 = B(j5, j13)
j15 = self.ubar_ground_jcs_trans
j16 = (self.R_rbs_body.T + (-1) * self.R_ground.T +
multi_dot([j13.T, j11]) + (-1) * multi_dot([j15.T, j4]))
j17 = j8.T
j18 = multi_dot([j17, j11])
j19 = self.Mbar_ground_jcs_trans[:, 1:2]
j20 = B(j3, j1)
j21 = B(j3, j15)
self.jac_eq_blocks = (
j0,
multi_dot([j10, j11, j20]),
j0,
multi_dot([j2, j4, j7]),
j0,
multi_dot([j17, j11, j20]),
j0,
multi_dot([j2, j4, j9]),
(-1) * j12,
(-1) * multi_dot([j10, j11, j21]),
j12,
(multi_dot([j10, j11, j14]) + multi_dot([j16, j7])),
(-1) * j18,
(-1) * multi_dot([j17, j11, j21]),
j18,
(multi_dot([j17, j11, j14]) + multi_dot([j16, j9])),
j0,
multi_dot([j10, j11, B(j3, j19)]),
j0,
multi_dot([j19.T, j4, j7]),
I3,
Z3x4,
Z4x3,
I4,
j0,
(2) * j5.T,
)
def eval_acc_eq(self):
config = self.config
t = self.t
a0 = self.Pd_ground
a1 = self.Pd_rbs_body
a2 = self.Mbar_rbs_body_jcs_a[:,2:3]
a3 = a2.T
a4 = self.P_rbs_body
a5 = A(a4).T
a6 = self.Mbar_ground_jcs_a[:,0:1]
a7 = self.P_ground
a8 = A(a7).T
a9 = B(a1,a2)
a10 = a0.T
a11 = B(a4,a2)
a12 = self.Mbar_ground_jcs_a[:,1:2]
self.acc_eq_blocks = ((multi_dot([B(a0,self.ubar_ground_jcs_a),a0]) + (-1) * multi_dot([B(a1,self.ubar_rbs_body_jcs_a),a1])),
(multi_dot([a3,a5,B(a0,a6),a0]) + multi_dot([a6.T,a8,a9,a1]) + (2) * multi_dot([a10,B(a7,a6).T,a11,a1])),
(multi_dot([a3,a5,B(a0,a12),a0]) + multi_dot([a12.T,a8,a9,a1]) + (2) * multi_dot([a10,B(a7,a12).T,a11,a1])),
Z3x1,
Z4x1,
(2) * multi_dot([a1.T,a1]),)
def steering_function(t):
R_ch = num_model.Subsystems.CH.R_rbs_chassis
P_ch = num_model.Subsystems.CH.P_rbs_chassis
Rd_ch = num_model.Subsystems.CH.Rd_rbs_chassis
Pd_ch = num_model.Subsystems.CH.Pd_rbs_chassis
rbar_ax1 = np.array([[-800], [0], [0]], dtype=np.float64)
r_ax1 = R_ch + A(P_ch) @ rbar_ax1
vel = (A(P_ch).T @ (Rd_ch + B(P_ch, rbar_ax1) @ Pd_ch))[0, 0]
delta = lateral_controller.get_steer_factor(r_ax1, P_ch, Pd_ch, vel)
travel = delta * 18
#print('Travel = %s'%travel)
return travel
def eval_acc_eq(self):
config = self.config
t = self.t
a0 = self.Mbar_ground_jcs_trans_1[:, 2:3]
a1 = a0.T
a2 = self.P_ground
a3 = A(a2).T
a4 = self.Pd_rbs_body_1
a5 = self.Mbar_rbs_body_1_jcs_trans_1[:, 0:1]
a6 = B(a4, a5)
a7 = a5.T
a8 = self.P_rbs_body_1
a9 = A(a8).T
a10 = self.Pd_ground
a11 = B(a10, a0)
a12 = a4.T
a13 = B(a8, a5).T
a14 = B(a2, a0)
a15 = self.Mbar_rbs_body_1_jcs_trans_1[:, 1:2]
a16 = B(a4, a15)
a17 = a15.T
a18 = B(a8, a15).T
a19 = self.ubar_rbs_body_1_jcs_trans_1
a20 = self.ubar_ground_jcs_trans_1
a21 = (multi_dot([B(a4, a19), a4]) +
(-1) * multi_dot([B(a10, a20), a10]))
a22 = (-1) * self.Rd_ground
a23 = (self.Rd_rbs_body_1 + a22 + multi_dot([B(a8, a19), a4]) +
(-1) * multi_dot([B(a2, a20), a10]))
a24 = (-1) * self.R_ground.T
a25 = (self.R_rbs_body_1.T + a24 + multi_dot([a19.T, a9]) +
(-1) * multi_dot([a20.T, a3]))
a26 = self.Mbar_ground_jcs_trans_1[:, 1:2]
a27 = self.Mbar_rbs_body_2_jcs_trans_2[:, 0:1]
a28 = a27.T
a29 = self.P_rbs_body_2
a30 = A(a29).T
a31 = self.Mbar_ground_jcs_trans_2[:, 2:3]
a32 = B(a10, a31)
a33 = a31.T
a34 = self.Pd_rbs_body_2
a35 = B(a34, a27)
a36 = a34.T
a37 = B(a29, a27).T
a38 = B(a2, a31)
a39 = self.Mbar_rbs_body_2_jcs_trans_2[:, 1:2]
a40 = a39.T
a41 = B(a34, a39)
a42 = B(a29, a39).T
a43 = self.ubar_rbs_body_2_jcs_trans_2
a44 = self.ubar_ground_jcs_trans_2
a45 = (multi_dot([B(a34, a43), a34]) +
(-1) * multi_dot([B(a10, a44), a10]))
a46 = (self.Rd_rbs_body_2 + a22 + multi_dot([B(a29, a43), a34]) +
(-1) * multi_dot([B(a2, a44), a10]))
a47 = (self.R_rbs_body_2.T + a24 + multi_dot([a43.T, a30]) +
(-1) * multi_dot([a44.T, a3]))
a48 = self.Mbar_ground_jcs_trans_2[:, 1:2]
self.acc_eq_blocks = (
(multi_dot([a1, a3, a6, a4]) + multi_dot([a7, a9, a11, a10]) +
(2) * multi_dot([a12, a13, a14, a10])),
(multi_dot([a1, a3, a16, a4]) + multi_dot([a17, a9, a11, a10]) +
(2) * multi_dot([a12, a18, a14, a10])),
(multi_dot([a7, a9, a21]) + (2) * multi_dot([a12, a13, a23]) +
multi_dot([a25, a6, a4])),
(multi_dot([a17, a9, a21]) + (2) * multi_dot([a12, a18, a23]) +
multi_dot([a25, a16, a4])),
(multi_dot([a26.T, a3, a6, a4]) +
multi_dot([a7, a9, B(a10, a26), a10]) +
(2) * multi_dot([a12, a13, B(a2, a26), a10])),
(multi_dot([a28, a30, a32, a10]) + multi_dot([a33, a3, a35, a34]) +
(2) * multi_dot([a36, a37, a38, a10])),
(multi_dot([a40, a30, a32, a10]) + multi_dot([a33, a3, a41, a34]) +
(2) * multi_dot([a36, a42, a38, a10])),
(multi_dot([a28, a30, a45]) + (2) * multi_dot([a36, a37, a46]) +
multi_dot([a47, a35, a34])),
(multi_dot([a40, a30, a45]) + (2) * multi_dot([a36, a42, a46]) +
multi_dot([a47, a41, a34])),
(multi_dot([a28, a30, B(a10, a48), a10]) +
multi_dot([a48.T, a3, a35, a34]) +
(2) * multi_dot([a36, a37, B(a2, a48), a10])),
Z3x1,
Z4x1,
(2) * multi_dot([a12, a4]),
(2) * multi_dot([a36, a34]),
)
def eval_jac_eq(self):
config = self.config
t = self.t
j0 = Z1x3
j1 = self.Mbar_ground_jcs_trans_1[:, 2:3]
j2 = j1.T
j3 = self.P_ground
j4 = A(j3).T
j5 = self.P_rbs_body_1
j6 = self.Mbar_rbs_body_1_jcs_trans_1[:, 0:1]
j7 = B(j5, j6)
j8 = self.Mbar_rbs_body_1_jcs_trans_1[:, 1:2]
j9 = B(j5, j8)
j10 = j6.T
j11 = A(j5).T
j12 = multi_dot([j10, j11])
j13 = self.ubar_rbs_body_1_jcs_trans_1
j14 = B(j5, j13)
j15 = (-1) * self.R_ground.T
j16 = self.ubar_ground_jcs_trans_1
j17 = (self.R_rbs_body_1.T + j15 + multi_dot([j13.T, j11]) +
(-1) * multi_dot([j16.T, j4]))
j18 = j8.T
j19 = multi_dot([j18, j11])
j20 = self.Mbar_ground_jcs_trans_1[:, 1:2]
j21 = B(j3, j1)
j22 = B(j3, j16)
j23 = self.Mbar_ground_jcs_trans_2[:, 2:3]
j24 = j23.T
j25 = self.P_rbs_body_2
j26 = self.Mbar_rbs_body_2_jcs_trans_2[:, 0:1]
j27 = B(j25, j26)
j28 = self.Mbar_rbs_body_2_jcs_trans_2[:, 1:2]
j29 = B(j25, j28)
j30 = j26.T
j31 = A(j25).T
j32 = multi_dot([j30, j31])
j33 = self.ubar_rbs_body_2_jcs_trans_2
j34 = B(j25, j33)
j35 = self.ubar_ground_jcs_trans_2
j36 = (self.R_rbs_body_2.T + j15 + multi_dot([j33.T, j31]) +
(-1) * multi_dot([j35.T, j4]))
j37 = j28.T
j38 = multi_dot([j37, j31])
j39 = self.Mbar_ground_jcs_trans_2[:, 1:2]
j40 = B(j3, j23)
j41 = B(j3, j35)
self.jac_eq_blocks = (
j0,
multi_dot([j10, j11, j21]),
j0,
multi_dot([j2, j4, j7]),
j0,
multi_dot([j18, j11, j21]),
j0,
multi_dot([j2, j4, j9]),
(-1) * j12,
(-1) * multi_dot([j10, j11, j22]),
j12,
(multi_dot([j10, j11, j14]) + multi_dot([j17, j7])),
(-1) * j19,
(-1) * multi_dot([j18, j11, j22]),
j19,
(multi_dot([j18, j11, j14]) + multi_dot([j17, j9])),
j0,
multi_dot([j10, j11, B(j3, j20)]),
j0,
multi_dot([j20.T, j4, j7]),
j0,
multi_dot([j30, j31, j40]),
j0,
multi_dot([j24, j4, j27]),
j0,
multi_dot([j37, j31, j40]),
j0,
multi_dot([j24, j4, j29]),
(-1) * j32,
(-1) * multi_dot([j30, j31, j41]),
j32,
(multi_dot([j30, j31, j34]) + multi_dot([j36, j27])),
(-1) * j38,
(-1) * multi_dot([j37, j31, j41]),
j38,
(multi_dot([j37, j31, j34]) + multi_dot([j36, j29])),
j0,
multi_dot([j30, j31, B(j3, j39)]),
j0,
multi_dot([j39.T, j4, j27]),
I3,
Z3x4,
Z4x3,
I4,
j0,
(2) * j5.T,
j0,
(2) * j25.T,
)
def eval_jac_eq(self):
config = self.config
t = self.t
j0 = I3
j1 = self.P_rbr_rocker
j2 = Z1x3
j3 = self.Mbar_vbs_chassis_jcr_rocker_chassis[:, 2:3]
j4 = j3.T
j5 = self.P_vbs_chassis
j6 = A(j5).T
j7 = self.Mbar_rbr_rocker_jcr_rocker_chassis[:, 0:1]
j8 = self.Mbar_rbr_rocker_jcr_rocker_chassis[:, 1:2]
j9 = (-1) * j0
j10 = A(j1).T
j11 = B(j5, j3)
j12 = self.Mbar_rbs_coupler_jcs_rocker_uni[:, 0:1]
j13 = self.P_rbs_coupler
j14 = self.Mbar_rbr_rocker_jcs_rocker_uni[:, 0:1]
j15 = self.P_rbl_rocker
j16 = self.Mbar_vbs_chassis_jcl_rocker_chassis[:, 2:3]
j17 = j16.T
j18 = self.Mbar_rbl_rocker_jcl_rocker_chassis[:, 0:1]
j19 = self.Mbar_rbl_rocker_jcl_rocker_chassis[:, 1:2]
j20 = A(j15).T
j21 = B(j5, j16)
j22 = self.Mbar_vbs_chassis_jcl_rocker_chassis[:, 0:1]
j23 = self.Mbar_rbl_rocker_jcl_rocker_chassis[:, 1:2]
j24 = self.Mbar_rbl_rocker_jcl_rocker_chassis[:, 0:1]
self.jac_eq_blocks = (
j0,
B(j1, self.ubar_rbr_rocker_jcr_rocker_chassis),
j9,
(-1) * B(j5, self.ubar_vbs_chassis_jcr_rocker_chassis),
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]),
j9,
(-1) * B(j13, self.ubar_rbs_coupler_jcs_rocker_uni),
j0,
B(j1, self.ubar_rbr_rocker_jcs_rocker_uni),
j2,
multi_dot([j14.T, j10, B(j13, j12)]),
j2,
multi_dot([j12.T, A(j13).T, B(j1, j14)]),
j0,
B(j15, self.ubar_rbl_rocker_jcl_rocker_chassis),
j9,
(-1) * B(j5, self.ubar_vbs_chassis_jcl_rocker_chassis),
j2,
multi_dot([j17, j6, B(j15, j18)]),
j2,
multi_dot([j18.T, j20, j21]),
j2,
multi_dot([j17, j6, B(j15, j19)]),
j2,
multi_dot([j19.T, j20, j21]),
j2,
multi_dot([
j22.T, j6,
(cos(config.UF_mcs_steer_act(t)) * B(j15, j23) +
(-1 * sin(config.UF_mcs_steer_act(t))) * B(j15, j24))
]),
j2,
multi_dot([
(cos(config.UF_mcs_steer_act(t)) * multi_dot([j23.T, j20]) +
(-1 * sin(config.UF_mcs_steer_act(t))) *
multi_dot([j24.T, j20])),
B(j5, j22)
]),
j9,
(-1) * B(j13, self.ubar_rbs_coupler_jcs_rocker_sph),
j0,
B(j15, self.ubar_rbl_rocker_jcs_rocker_sph),
j2,
(2) * j13.T,
j2,
(2) * j1.T,
j2,
(2) * j15.T,
)
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_jac_eq(self):
config = self.config
t = self.t
j0 = I3
j1 = self.P_ground
j2 = Z1x3
j3 = self.Mbar_rbs_l1_jcs_a[:, 2:3]
j4 = j3.T
j5 = self.P_rbs_l1
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_l1_jcs_a[:, 0:1]
j13 = self.Mbar_ground_jcs_a[:, 1:2]
j14 = self.Mbar_ground_jcs_a[:, 0:1]
j15 = self.P_rbs_l2
j16 = self.Mbar_rbs_l3_jcs_c[:, 0:1]
j17 = self.P_rbs_l3
j18 = A(j17).T
j19 = self.Mbar_rbs_l2_jcs_c[:, 0:1]
j20 = self.Mbar_ground_jcs_d[:, 2:3]
j21 = j20.T
j22 = self.Mbar_rbs_l3_jcs_d[:, 0:1]
j23 = B(j17, j22)
j24 = self.Mbar_rbs_l3_jcs_d[:, 1:2]
j25 = B(j17, j24)
j26 = j22.T
j27 = multi_dot([j26, j18])
j28 = self.ubar_rbs_l3_jcs_d
j29 = B(j17, j28)
j30 = self.ubar_ground_jcs_d
j31 = (self.R_rbs_l3.T + (-1) * self.R_ground.T +
multi_dot([j28.T, j18]) + (-1) * multi_dot([j30.T, j10]))
j32 = j24.T
j33 = multi_dot([j32, j18])
j34 = self.Mbar_ground_jcs_d[:, 1:2]
j35 = B(j1, j20)
j36 = B(j1, j30)
self.jac_eq_blocks = (
j0,
B(j1, self.ubar_ground_jcs_a),
j9,
(-1) * B(j5, self.ubar_rbs_l1_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]),
j2,
multi_dot([
j12.T, j6,
(cos(config.UF_mcs_act(t)) * B(j1, j13) +
(-1 * sin(config.UF_mcs_act(t))) * B(j1, j14))
]),
j2,
multi_dot([
(cos(config.UF_mcs_act(t)) * multi_dot([j13.T, j10]) +
(-1 * sin(config.UF_mcs_act(t))) * multi_dot([j14.T, j10])),
B(j5, j12)
]),
j0,
B(j5, self.ubar_rbs_l1_jcs_b),
j9,
(-1) * B(j15, self.ubar_rbs_l2_jcs_b),
j0,
B(j15, self.ubar_rbs_l2_jcs_c),
j9,
(-1) * B(j17, self.ubar_rbs_l3_jcs_c),
j2,
multi_dot([j16.T, j18, B(j15, j19)]),
j2,
multi_dot([j19.T, A(j15).T, B(j17, j16)]),
j2,
multi_dot([j26, j18, j35]),
j2,
multi_dot([j21, j10, j23]),
j2,
multi_dot([j32, j18, j35]),
j2,
multi_dot([j21, j10, j25]),
(-1) * j27,
(-1) * multi_dot([j26, j18, j36]),
j27,
(multi_dot([j26, j18, j29]) + multi_dot([j31, j23])),
(-1) * j33,
(-1) * multi_dot([j32, j18, j36]),
j33,
(multi_dot([j32, j18, j29]) + multi_dot([j31, j25])),
j2,
multi_dot([j26, j18, B(j1, j34)]),
j2,
multi_dot([j34.T, j10, j23]),
j0,
Z3x4,
Z4x3,
I4,
j2,
(2) * j5.T,
j2,
(2) * j15.T,
j2,
(2) * j17.T,
)
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])),
)
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_acc_eq(self):
config = self.config
t = self.t
a0 = self.Mbar_rbs_body_jcs_trans[:, 0:1]
a1 = a0.T
a2 = self.P_rbs_body
a3 = A(a2).T
a4 = self.Pd_ground
a5 = self.Mbar_ground_jcs_trans[:, 2:3]
a6 = B(a4, a5)
a7 = a5.T
a8 = self.P_ground
a9 = A(a8).T
a10 = self.Pd_rbs_body
a11 = B(a10, a0)
a12 = a10.T
a13 = B(a2, a0).T
a14 = B(a8, a5)
a15 = self.Mbar_rbs_body_jcs_trans[:, 1:2]
a16 = a15.T
a17 = B(a10, a15)
a18 = B(a2, a15).T
a19 = self.ubar_rbs_body_jcs_trans
a20 = self.ubar_ground_jcs_trans
a21 = (multi_dot([B(a10, a19), a10]) +
(-1) * multi_dot([B(a4, a20), a4]))
a22 = (self.Rd_rbs_body + (-1) * self.Rd_ground +
multi_dot([B(a2, a19), a10]) +
(-1) * multi_dot([B(a8, a20), a4]))
a23 = (self.R_rbs_body.T + (-1) * self.R_ground.T +
multi_dot([a19.T, a3]) + (-1) * multi_dot([a20.T, a9]))
a24 = self.Mbar_ground_jcs_trans[:, 1:2]
self.acc_eq_blocks = (
(multi_dot([a1, a3, a6, a4]) + multi_dot([a7, a9, a11, a10]) +
(2) * multi_dot([a12, a13, a14, a4])),
(multi_dot([a16, a3, a6, a4]) + multi_dot([a7, a9, a17, a10]) +
(2) * multi_dot([a12, a18, a14, a4])),
(multi_dot([a1, a3, a21]) + (2) * multi_dot([a12, a13, a22]) +
multi_dot([a23, a11, a10])),
(multi_dot([a16, a3, a21]) + (2) * multi_dot([a12, a18, a22]) +
multi_dot([a23, a17, a10])),
(multi_dot([a1, a3, B(a4, a24), a4]) +
multi_dot([a24.T, a9, a11, a10]) +
(2) * multi_dot([a12, a13, B(a8, a24), a4])),
Z3x1,
Z4x1,
(2) * multi_dot([a12, a10]),
)
def eval_acc_eq(self):
config = self.config
t = self.t
a0 = self.Pd_ground
a1 = self.Pd_rbs_body_1
a2 = self.Mbar_ground_jcs_a[:, 0:1]
a3 = self.P_ground
a4 = A(a3).T
a5 = self.Mbar_rbs_body_1_jcs_a[:, 2:3]
a6 = B(a1, a5)
a7 = a5.T
a8 = self.P_rbs_body_1
a9 = A(a8).T
a10 = a0.T
a11 = B(a8, a5)
a12 = self.Mbar_ground_jcs_a[:, 1:2]
a13 = self.Pd_rbs_body_2
a14 = self.Mbar_rbs_body_1_jcs_b[:, 0:1]
a15 = self.Mbar_rbs_body_2_jcs_b[:, 2:3]
a16 = B(a13, a15)
a17 = a15.T
a18 = self.P_rbs_body_2
a19 = A(a18).T
a20 = a1.T
a21 = B(a18, a15)
a22 = self.Mbar_rbs_body_1_jcs_b[:, 1:2]
self.acc_eq_blocks = (
(multi_dot([B(a0, self.ubar_ground_jcs_a), a0]) +
(-1) * multi_dot([B(a1, self.ubar_rbs_body_1_jcs_a), a1])),
(multi_dot([a2.T, a4, a6, a1]) +
multi_dot([a7, a9, B(a0, a2), a0]) +
(2) * multi_dot([a10, B(a3, a2).T, a11, a1])),
(multi_dot([a12.T, a4, a6, a1]) +
multi_dot([a7, a9, B(a0, a12), a0]) +
(2) * multi_dot([a10, B(a3, a12).T, a11, a1])),
(multi_dot([B(a1, self.ubar_rbs_body_1_jcs_b), a1]) +
(-1) * multi_dot([B(a13, self.ubar_rbs_body_2_jcs_b), a13])),
(multi_dot([a14.T, a9, a16, a13]) +
multi_dot([a17, a19, B(a1, a14), a1]) +
(2) * multi_dot([a20, B(a8, a14).T, a21, a13])),
(multi_dot([a22.T, a9, a16, a13]) +
multi_dot([a17, a19, B(a1, a22), a1]) +
(2) * multi_dot([a20, B(a8, a22).T, a21, a13])),
Z3x1,
Z4x1,
(2) * multi_dot([a20, a1]),
(2) * multi_dot([a13.T, a13]),
)
def eval_acc_eq(self):
config = self.config
t = self.t
a0 = self.Mbar_rbs_rack_jcs_rack[:, 0:1]
a1 = a0.T
a2 = self.P_rbs_rack
a3 = A(a2).T
a4 = self.Pd_vbs_chassis
a5 = self.Mbar_vbs_chassis_jcs_rack[:, 2:3]
a6 = B(a4, a5)
a7 = a5.T
a8 = self.P_vbs_chassis
a9 = A(a8).T
a10 = self.Pd_rbs_rack
a11 = B(a10, a0)
a12 = a10.T
a13 = B(a2, a0).T
a14 = B(a8, a5)
a15 = self.Mbar_rbs_rack_jcs_rack[:, 1:2]
a16 = a15.T
a17 = B(a10, a15)
a18 = B(a2, a15).T
a19 = self.ubar_rbs_rack_jcs_rack
a20 = self.ubar_vbs_chassis_jcs_rack
a21 = (multi_dot([B(a10, a19), a10]) +
(-1) * multi_dot([B(a4, a20), a4]))
a22 = (self.Rd_rbs_rack + (-1) * self.Rd_vbs_chassis +
multi_dot([B(a2, a19), a10]) +
(-1) * multi_dot([B(a8, a20), a4]))
a23 = (self.R_rbs_rack.T + (-1) * self.R_vbs_chassis.T +
multi_dot([a19.T, a3]) + (-1) * multi_dot([a20.T, a9]))
a24 = self.Mbar_vbs_chassis_jcs_rack[:, 1:2]
a25 = self.Mbar_rbs_rack_jcs_rack[:, 2:3]
self.acc_eq_blocks = (
(multi_dot([a1, a3, a6, a4]) + multi_dot([a7, a9, a11, a10]) +
(2) * multi_dot([a12, a13, a14, a4])),
(multi_dot([a16, a3, a6, a4]) + multi_dot([a7, a9, a17, a10]) +
(2) * multi_dot([a12, a18, a14, a4])),
(multi_dot([a1, a3, a21]) + (2) * multi_dot([a12, a13, a22]) +
multi_dot([a23, a11, a10])),
(multi_dot([a16, a3, a21]) + (2) * multi_dot([a12, a18, a22]) +
multi_dot([a23, a17, a10])),
(multi_dot([a1, a3, B(a4, a24), a4]) +
multi_dot([a24.T, a9, a11, a10]) +
(2) * multi_dot([a12, a13, B(a8, a24), a4])),
((-1 * derivative(config.UF_mcs_rack_act, t, 0.1, 2)) * I1 +
multi_dot([a25.T, a3, a21]) +
(2) * multi_dot([a12, B(a2, a25).T, a22]) +
multi_dot([a23, B(a10, a25), a10])),
(2) * multi_dot([a12, a10]),
)
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_acc_eq(self):
config = self.config
t = self.t
a0 = self.Pd_ground
a1 = self.Pd_rbs_l1
a2 = self.Mbar_ground_jcs_j1[:, 0:1]
a3 = self.P_ground
a4 = A(a3).T
a5 = self.Mbar_rbs_l1_jcs_j1[:, 2:3]
a6 = B(a1, a5)
a7 = a5.T
a8 = self.P_rbs_l1
a9 = A(a8).T
a10 = a0.T
a11 = B(a8, a5)
a12 = self.Mbar_ground_jcs_j1[:, 1:2]
a13 = self.Pd_rbs_l2
a14 = self.Pd_rbs_l3
a15 = self.Mbar_ground_jcs_j4[:, 2:3]
a16 = a15.T
a17 = self.Mbar_rbs_l3_jcs_j4[:, 0:1]
a18 = self.P_rbs_l3
a19 = A(a18).T
a20 = B(a0, a15)
a21 = a14.T
a22 = B(a3, a15)
a23 = self.Mbar_rbs_l3_jcs_j4[:, 1:2]
a24 = self.Pd_rbs_l4
a25 = self.Pd_rbs_l5
a26 = self.Mbar_ground_jcs_j7[:, 2:3]
a27 = a26.T
a28 = self.Mbar_rbs_l5_jcs_j7[:, 0:1]
a29 = self.P_rbs_l5
a30 = A(a29).T
a31 = B(a0, a26)
a32 = a25.T
a33 = B(a3, a26)
a34 = self.Mbar_rbs_l5_jcs_j7[:, 1:2]
a35 = self.Pd_rbs_l6
a36 = self.Pd_rbs_l7
a37 = self.Mbar_ground_jcs_j10[:, 2:3]
a38 = a37.T
a39 = self.Mbar_rbs_l7_jcs_j10[:, 0:1]
a40 = self.P_rbs_l7
a41 = A(a40).T
a42 = B(a0, a37)
a43 = a36.T
a44 = B(a3, a37)
a45 = self.Mbar_rbs_l7_jcs_j10[:, 1:2]
a46 = self.Pd_rbs_l8
a47 = self.Pd_rbs_l9
a48 = self.Mbar_ground_jcs_j13[:, 2:3]
a49 = a48.T
a50 = self.Mbar_rbs_l9_jcs_j13[:, 0:1]
a51 = self.P_rbs_l9
a52 = A(a51).T
a53 = B(a0, a48)
a54 = a47.T
a55 = B(a3, a48)
a56 = self.Mbar_rbs_l9_jcs_j13[:, 1:2]
self.acc_eq_blocks = (
(multi_dot([B(a0, self.ubar_ground_jcs_j1), a0]) +
(-1) * multi_dot([B(a1, self.ubar_rbs_l1_jcs_j1), a1])),
(multi_dot([a2.T, a4, a6, a1]) +
multi_dot([a7, a9, B(a0, a2), a0]) +
(2) * multi_dot([a10, B(a3, a2).T, a11, a1])),
(multi_dot([a12.T, a4, a6, a1]) +
multi_dot([a7, a9, B(a0, a12), a0]) +
(2) * multi_dot([a10, B(a3, a12).T, a11, a1])),
(multi_dot([B(a1, self.ubar_rbs_l1_jcs_j2), a1]) +
(-1) * multi_dot([B(a13, self.ubar_rbs_l2_jcs_j2), a13])),
(multi_dot([B(a13, self.ubar_rbs_l2_jcs_j3), a13]) +
(-1) * multi_dot([B(a14, self.ubar_rbs_l3_jcs_j3), a14])),
(multi_dot([B(a14, self.ubar_rbs_l3_jcs_j4), a14]) +
(-1) * multi_dot([B(a0, self.ubar_ground_jcs_j4), a0])),
(multi_dot([a16, a4, B(a14, a17), a14]) +
multi_dot([a17.T, a19, a20, a0]) +
(2) * multi_dot([a21, B(a18, a17).T, a22, a0])),
(multi_dot([a16, a4, B(a14, a23), a14]) +
multi_dot([a23.T, a19, a20, a0]) +
(2) * multi_dot([a21, B(a18, a23).T, a22, a0])),
(multi_dot([B(a14, self.ubar_rbs_l3_jcs_j5), a14]) +
(-1) * multi_dot([B(a24, self.ubar_rbs_l4_jcs_j5), a24])),
(multi_dot([B(a24, self.ubar_rbs_l4_jcs_j6), a24]) +
(-1) * multi_dot([B(a25, self.ubar_rbs_l5_jcs_j6), a25])),
(multi_dot([B(a25, self.ubar_rbs_l5_jcs_j7), a25]) +
(-1) * multi_dot([B(a0, self.ubar_ground_jcs_j7), a0])),
(multi_dot([a27, a4, B(a25, a28), a25]) +
multi_dot([a28.T, a30, a31, a0]) +
(2) * multi_dot([a32, B(a29, a28).T, a33, a0])),
(multi_dot([a27, a4, B(a25, a34), a25]) +
multi_dot([a34.T, a30, a31, a0]) +
(2) * multi_dot([a32, B(a29, a34).T, a33, a0])),
(multi_dot([B(a25, self.ubar_rbs_l5_jcs_j8), a25]) +
(-1) * multi_dot([B(a35, self.ubar_rbs_l6_jcs_j8), a35])),
(multi_dot([B(a35, self.ubar_rbs_l6_jcs_j9), a35]) +
(-1) * multi_dot([B(a36, self.ubar_rbs_l7_jcs_j9), a36])),
(multi_dot([B(a36, self.ubar_rbs_l7_jcs_j10), a36]) +
(-1) * multi_dot([B(a0, self.ubar_ground_jcs_j10), a0])),
(multi_dot([a38, a4, B(a36, a39), a36]) +
multi_dot([a39.T, a41, a42, a0]) +
(2) * multi_dot([a43, B(a40, a39).T, a44, a0])),
(multi_dot([a38, a4, B(a36, a45), a36]) +
multi_dot([a45.T, a41, a42, a0]) +
(2) * multi_dot([a43, B(a40, a45).T, a44, a0])),
(multi_dot([B(a36, self.ubar_rbs_l7_jcs_j11), a36]) +
(-1) * multi_dot([B(a46, self.ubar_rbs_l8_jcs_j11), a46])),
(multi_dot([B(a46, self.ubar_rbs_l8_jcs_j12), a46]) +
(-1) * multi_dot([B(a47, self.ubar_rbs_l9_jcs_j12), a47])),
(multi_dot([B(a47, self.ubar_rbs_l9_jcs_j13), a47]) +
(-1) * multi_dot([B(a0, self.ubar_ground_jcs_j13), a0])),
(multi_dot([a49, a4, B(a47, a50), a47]) +
multi_dot([a50.T, a52, a53, a0]) +
(2) * multi_dot([a54, B(a51, a50).T, a55, a0])),
(multi_dot([a49, a4, B(a47, a56), a47]) +
multi_dot([a56.T, a52, a53, a0]) +
(2) * multi_dot([a54, B(a51, a56).T, a55, a0])),
Z3x1,
Z4x1,
(2) * multi_dot([a1.T, a1]),
(2) * multi_dot([a13.T, a13]),
(2) * multi_dot([a21, a14]),
(2) * multi_dot([a24.T, a24]),
(2) * multi_dot([a32, a25]),
(2) * multi_dot([a35.T, a35]),
(2) * multi_dot([a43, a36]),
(2) * multi_dot([a46.T, a46]),
(2) * multi_dot([a54, a47]),
)
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_jac_eq(self):
config = self.config
t = self.t
j0 = I3
j1 = self.P_ground
j2 = Z1x3
j3 = self.Mbar_rbs_l1_jcs_j1[:, 2:3]
j4 = j3.T
j5 = self.P_rbs_l1
j6 = A(j5).T
j7 = self.Mbar_ground_jcs_j1[:, 0:1]
j8 = self.Mbar_ground_jcs_j1[:, 1:2]
j9 = (-1) * j0
j10 = A(j1).T
j11 = B(j5, j3)
j12 = self.P_rbs_l2
j13 = self.P_rbs_l3
j14 = self.Mbar_ground_jcs_j4[:, 2:3]
j15 = j14.T
j16 = self.Mbar_rbs_l3_jcs_j4[:, 0:1]
j17 = self.Mbar_rbs_l3_jcs_j4[:, 1:2]
j18 = A(j13).T
j19 = B(j1, j14)
j20 = self.P_rbs_l4
j21 = self.P_rbs_l5
j22 = self.Mbar_ground_jcs_j7[:, 2:3]
j23 = j22.T
j24 = self.Mbar_rbs_l5_jcs_j7[:, 0:1]
j25 = self.Mbar_rbs_l5_jcs_j7[:, 1:2]
j26 = A(j21).T
j27 = B(j1, j22)
j28 = self.P_rbs_l6
j29 = self.P_rbs_l7
j30 = self.Mbar_ground_jcs_j10[:, 2:3]
j31 = j30.T
j32 = self.Mbar_rbs_l7_jcs_j10[:, 0:1]
j33 = self.Mbar_rbs_l7_jcs_j10[:, 1:2]
j34 = A(j29).T
j35 = B(j1, j30)
j36 = self.P_rbs_l8
j37 = self.P_rbs_l9
j38 = self.Mbar_ground_jcs_j13[:, 2:3]
j39 = j38.T
j40 = self.Mbar_rbs_l9_jcs_j13[:, 0:1]
j41 = self.Mbar_rbs_l9_jcs_j13[:, 1:2]
j42 = A(j37).T
j43 = B(j1, j38)
self.jac_eq_blocks = (
j0,
B(j1, self.ubar_ground_jcs_j1),
j9,
(-1) * B(j5, self.ubar_rbs_l1_jcs_j1),
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_l1_jcs_j2),
j9,
(-1) * B(j12, self.ubar_rbs_l2_jcs_j2),
j0,
B(j12, self.ubar_rbs_l2_jcs_j3),
j9,
(-1) * B(j13, self.ubar_rbs_l3_jcs_j3),
j9,
(-1) * B(j1, self.ubar_ground_jcs_j4),
j0,
B(j13, self.ubar_rbs_l3_jcs_j4),
j2,
multi_dot([j16.T, j18, j19]),
j2,
multi_dot([j15, j10, B(j13, j16)]),
j2,
multi_dot([j17.T, j18, j19]),
j2,
multi_dot([j15, j10, B(j13, j17)]),
j0,
B(j13, self.ubar_rbs_l3_jcs_j5),
j9,
(-1) * B(j20, self.ubar_rbs_l4_jcs_j5),
j0,
B(j20, self.ubar_rbs_l4_jcs_j6),
j9,
(-1) * B(j21, self.ubar_rbs_l5_jcs_j6),
j9,
(-1) * B(j1, self.ubar_ground_jcs_j7),
j0,
B(j21, self.ubar_rbs_l5_jcs_j7),
j2,
multi_dot([j24.T, j26, j27]),
j2,
multi_dot([j23, j10, B(j21, j24)]),
j2,
multi_dot([j25.T, j26, j27]),
j2,
multi_dot([j23, j10, B(j21, j25)]),
j0,
B(j21, self.ubar_rbs_l5_jcs_j8),
j9,
(-1) * B(j28, self.ubar_rbs_l6_jcs_j8),
j0,
B(j28, self.ubar_rbs_l6_jcs_j9),
j9,
(-1) * B(j29, self.ubar_rbs_l7_jcs_j9),
j9,
(-1) * B(j1, self.ubar_ground_jcs_j10),
j0,
B(j29, self.ubar_rbs_l7_jcs_j10),
j2,
multi_dot([j32.T, j34, j35]),
j2,
multi_dot([j31, j10, B(j29, j32)]),
j2,
multi_dot([j33.T, j34, j35]),
j2,
multi_dot([j31, j10, B(j29, j33)]),
j0,
B(j29, self.ubar_rbs_l7_jcs_j11),
j9,
(-1) * B(j36, self.ubar_rbs_l8_jcs_j11),
j0,
B(j36, self.ubar_rbs_l8_jcs_j12),
j9,
(-1) * B(j37, self.ubar_rbs_l9_jcs_j12),
j9,
(-1) * B(j1, self.ubar_ground_jcs_j13),
j0,
B(j37, self.ubar_rbs_l9_jcs_j13),
j2,
multi_dot([j40.T, j42, j43]),
j2,
multi_dot([j39, j10, B(j37, j40)]),
j2,
multi_dot([j41.T, j42, j43]),
j2,
multi_dot([j39, j10, B(j37, j41)]),
j0,
Z3x4,
Z4x3,
I4,
j2,
(2) * j5.T,
j2,
(2) * j12.T,
j2,
(2) * j13.T,
j2,
(2) * j20.T,
j2,
(2) * j21.T,
j2,
(2) * j28.T,
j2,
(2) * j29.T,
j2,
(2) * j36.T,
j2,
(2) * j37.T,
)
def eval_acc_eq(self):
config = self.config
t = self.t
a0 = self.Pd_ground
a1 = self.Pd_rbs_l1
a2 = self.Mbar_ground_jcs_a[:, 0:1]
a3 = self.P_ground
a4 = A(a3).T
a5 = self.Mbar_rbs_l1_jcs_a[:, 2:3]
a6 = B(a1, a5)
a7 = a5.T
a8 = self.P_rbs_l1
a9 = A(a8).T
a10 = a0.T
a11 = B(a8, a5)
a12 = self.Mbar_ground_jcs_a[:, 1:2]
a13 = self.Mbar_rbs_l1_jcs_a[:, 0:1]
a14 = self.Mbar_ground_jcs_a[:, 1:2]
a15 = self.Mbar_ground_jcs_a[:, 0:1]
a16 = self.Pd_rbs_l2
a17 = self.Mbar_ground_jcs_b[:, 0:1]
a18 = self.Mbar_rbs_l2_jcs_b[:, 2:3]
a19 = B(a16, a18)
a20 = a18.T
a21 = self.P_rbs_l2
a22 = A(a21).T
a23 = B(a21, a18)
a24 = self.Mbar_ground_jcs_b[:, 1:2]
a25 = self.Mbar_rbs_l1_jcs_c[:, 0:1]
a26 = a25.T
a27 = self.Pd_rbs_l3
a28 = self.Mbar_rbs_l3_jcs_c[:, 2:3]
a29 = B(a27, a28)
a30 = a28.T
a31 = self.P_rbs_l3
a32 = A(a31).T
a33 = B(a1, a25)
a34 = a1.T
a35 = B(a8, a25).T
a36 = B(a31, a28)
a37 = self.Mbar_rbs_l1_jcs_c[:, 1:2]
a38 = a37.T
a39 = B(a1, a37)
a40 = B(a8, a37).T
a41 = self.ubar_rbs_l1_jcs_c
a42 = self.ubar_rbs_l3_jcs_c
a43 = (multi_dot([B(a1, a41), a1]) +
(-1) * multi_dot([B(a27, a42), a27]))
a44 = self.Rd_rbs_l3
a45 = (self.Rd_rbs_l1 + (-1) * a44 + multi_dot([B(a8, a41), a1]) +
(-1) * multi_dot([B(a31, a42), a27]))
a46 = self.R_rbs_l3.T
a47 = (self.R_rbs_l1.T + (-1) * a46 + multi_dot([a41.T, a9]) +
(-1) * multi_dot([a42.T, a32]))
a48 = self.Mbar_rbs_l4_jcs_e[:, 2:3]
a49 = a48.T
a50 = self.P_rbs_l4
a51 = A(a50).T
a52 = self.Mbar_rbs_l3_jcs_e[:, 0:1]
a53 = B(a27, a52)
a54 = a52.T
a55 = self.Pd_rbs_l4
a56 = B(a55, a48)
a57 = a27.T
a58 = B(a31, a52).T
a59 = B(a50, a48)
a60 = self.Mbar_rbs_l3_jcs_e[:, 1:2]
a61 = B(a27, a60)
a62 = a60.T
a63 = B(a31, a60).T
a64 = self.ubar_rbs_l3_jcs_e
a65 = self.ubar_rbs_l4_jcs_e
a66 = (multi_dot([B(a27, a64), a27]) +
(-1) * multi_dot([B(a55, a65), a55]))
a67 = (a44 + (-1) * self.Rd_rbs_l4 + multi_dot([B(a31, a64), a27]) +
(-1) * multi_dot([B(a50, a65), a55]))
a68 = (a46 + (-1) * self.R_rbs_l4.T + multi_dot([a64.T, a32]) +
(-1) * multi_dot([a65.T, a51]))
a69 = self.Pd_rbs_l5
a70 = self.Mbar_rbs_l2_jcs_h[:, 2:3]
a71 = a70.T
a72 = self.Mbar_rbs_l5_jcs_h[:, 0:1]
a73 = B(a69, a72)
a74 = a72.T
a75 = self.P_rbs_l5
a76 = A(a75).T
a77 = B(a16, a70)
a78 = a69.T
a79 = B(a75, a72).T
a80 = B(a21, a70)
a81 = self.Mbar_rbs_l5_jcs_h[:, 1:2]
a82 = B(a69, a81)
a83 = a81.T
a84 = B(a75, a81).T
a85 = self.ubar_rbs_l5_jcs_h
a86 = self.ubar_rbs_l2_jcs_h
a87 = (multi_dot([B(a69, a85), a69]) +
(-1) * multi_dot([B(a16, a86), a16]))
a88 = (self.Rd_rbs_l5 + (-1) * self.Rd_rbs_l2 +
multi_dot([B(a75, a85), a69]) +
(-1) * multi_dot([B(a21, a86), a16]))
a89 = (self.R_rbs_l5.T + (-1) * self.R_rbs_l2.T +
multi_dot([a85.T, a76]) + (-1) * multi_dot([a86.T, a22]))
self.acc_eq_blocks = (
(multi_dot([B(a0, self.ubar_ground_jcs_a), a0]) +
(-1) * multi_dot([B(a1, self.ubar_rbs_l1_jcs_a), a1])),
(multi_dot([a2.T, a4, a6, a1]) +
multi_dot([a7, a9, B(a0, a2), a0]) +
(2) * multi_dot([a10, B(a3, a2).T, a11, a1])),
(multi_dot([a12.T, a4, a6, a1]) +
multi_dot([a7, a9, B(a0, a12), a0]) +
(2) * multi_dot([a10, B(a3, a12).T, a11, a1])),
((-1 * derivative(config.UF_mcs_act, t, 0.1, 2)) * I1 + multi_dot([
a13.T, a9,
(cos(config.UF_mcs_act(t)) * B(a0, a14) +
(-1 * sin(config.UF_mcs_act(t))) * B(a0, a15)), a0
]) + multi_dot([
(cos(config.UF_mcs_act(t)) * multi_dot([a14.T, a4]) +
(-1 * sin(config.UF_mcs_act(t))) * multi_dot([a15.T, a4])),
B(a1, a13), a1
]) + (2) * multi_dot([
(cos(config.UF_mcs_act(t)) * multi_dot([a10, B(a3, a14).T]) +
(-1 * sin(config.UF_mcs_act(t))) *
multi_dot([a10, B(a3, a15).T])),
B(a8, a13), a1
])),
(multi_dot([B(a0, self.ubar_ground_jcs_b), a0]) +
(-1) * multi_dot([B(a16, self.ubar_rbs_l2_jcs_b), a16])),
(multi_dot([a17.T, a4, a19, a16]) +
multi_dot([a20, a22, B(a0, a17), a0]) +
(2) * multi_dot([a10, B(a3, a17).T, a23, a16])),
(multi_dot([a24.T, a4, a19, a16]) +
multi_dot([a20, a22, B(a0, a24), a0]) +
(2) * multi_dot([a10, B(a3, a24).T, a23, a16])),
(multi_dot([a26, a9, a29, a27]) + multi_dot([a30, a32, a33, a1]) +
(2) * multi_dot([a34, a35, a36, a27])),
(multi_dot([a38, a9, a29, a27]) + multi_dot([a30, a32, a39, a1]) +
(2) * multi_dot([a34, a40, a36, a27])),
(multi_dot([a26, a9, a43]) + (2) * multi_dot([a34, a35, a45]) +
multi_dot([a47, a33, a1])),
(multi_dot([a38, a9, a43]) + (2) * multi_dot([a34, a40, a45]) +
multi_dot([a47, a39, a1])),
(multi_dot([B(a16, self.ubar_rbs_l2_jcs_d), a16]) +
(-1) * multi_dot([B(a27, self.ubar_rbs_l3_jcs_d), a27])),
(multi_dot([a49, a51, a53, a27]) +
multi_dot([a54, a32, a56, a55]) +
(2) * multi_dot([a57, a58, a59, a55])),
(multi_dot([a49, a51, a61, a27]) +
multi_dot([a62, a32, a56, a55]) +
(2) * multi_dot([a57, a63, a59, a55])),
(multi_dot([a54, a32, a66]) + (2) * multi_dot([a57, a58, a67]) +
multi_dot([a68, a53, a27])),
(multi_dot([a62, a32, a66]) + (2) * multi_dot([a57, a63, a67]) +
multi_dot([a68, a61, a27])),
(multi_dot([B(a55, self.ubar_rbs_l4_jcs_f), a55]) +
(-1) * multi_dot([B(a69, self.ubar_rbs_l5_jcs_f), a69])),
(multi_dot([a71, a22, a73, a69]) +
multi_dot([a74, a76, a77, a16]) +
(2) * multi_dot([a78, a79, a80, a16])),
(multi_dot([a71, a22, a82, a69]) +
multi_dot([a83, a76, a77, a16]) +
(2) * multi_dot([a78, a84, a80, a16])),
(multi_dot([a74, a76, a87]) + (2) * multi_dot([a78, a79, a88]) +
multi_dot([a89, a73, a69])),
(multi_dot([a83, a76, a87]) + (2) * multi_dot([a78, a84, a88]) +
multi_dot([a89, a82, a69])),
Z3x1,
Z4x1,
(2) * multi_dot([a34, a1]),
(2) * multi_dot([a16.T, a16]),
(2) * multi_dot([a57, a27]),
(2) * multi_dot([a55.T, a55]),
(2) * multi_dot([a78, a69]),
)
def eval_acc_eq(self):
config = self.config
t = self.t
a0 = self.Pd_ground
a1 = self.Pd_rbs_l1
a2 = self.Mbar_rbs_l1_jcs_a[:, 2:3]
a3 = a2.T
a4 = self.P_rbs_l1
a5 = A(a4).T
a6 = self.Mbar_ground_jcs_a[:, 0:1]
a7 = self.P_ground
a8 = A(a7).T
a9 = B(a1, a2)
a10 = a0.T
a11 = B(a4, a2)
a12 = self.Mbar_ground_jcs_a[:, 1:2]
a13 = self.Mbar_rbs_l1_jcs_a[:, 0:1]
a14 = self.Mbar_ground_jcs_a[:, 1:2]
a15 = self.Mbar_ground_jcs_a[:, 0:1]
a16 = self.Pd_rbs_l2
a17 = self.Pd_rbs_l3
a18 = self.Mbar_rbs_l2_jcs_c[:, 0:1]
a19 = self.P_rbs_l2
a20 = self.Mbar_rbs_l3_jcs_c[:, 0:1]
a21 = self.P_rbs_l3
a22 = A(a21).T
a23 = a16.T
a24 = self.Mbar_ground_jcs_d[:, 2:3]
a25 = a24.T
a26 = self.Mbar_rbs_l3_jcs_d[:, 0:1]
a27 = B(a17, a26)
a28 = a26.T
a29 = B(a0, a24)
a30 = a17.T
a31 = B(a21, a26).T
a32 = B(a7, a24)
a33 = self.Mbar_rbs_l3_jcs_d[:, 1:2]
a34 = B(a17, a33)
a35 = a33.T
a36 = B(a21, a33).T
a37 = self.ubar_rbs_l3_jcs_d
a38 = self.ubar_ground_jcs_d
a39 = (multi_dot([B(a17, a37), a17]) +
(-1) * multi_dot([B(a0, a38), a0]))
a40 = (self.Rd_rbs_l3 + (-1) * self.Rd_ground +
multi_dot([B(a21, a37), a17]) +
(-1) * multi_dot([B(a7, a38), a0]))
a41 = (self.R_rbs_l3.T + (-1) * self.R_ground.T +
multi_dot([a37.T, a22]) + (-1) * multi_dot([a38.T, a8]))
a42 = self.Mbar_ground_jcs_d[:, 1:2]
self.acc_eq_blocks = (
(multi_dot([B(a0, self.ubar_ground_jcs_a), a0]) +
(-1) * multi_dot([B(a1, self.ubar_rbs_l1_jcs_a), a1])),
(multi_dot([a3, a5, B(a0, a6), a0]) +
multi_dot([a6.T, a8, a9, a1]) +
(2) * multi_dot([a10, B(a7, a6).T, a11, a1])),
(multi_dot([a3, a5, B(a0, a12), a0]) +
multi_dot([a12.T, a8, a9, a1]) +
(2) * multi_dot([a10, B(a7, a12).T, a11, a1])),
((-1 * derivative(config.UF_mcs_act, t, 0.1, 2)) * I1 + multi_dot([
a13.T, a5,
(cos(config.UF_mcs_act(t)) * B(a0, a14) +
(-1 * sin(config.UF_mcs_act(t))) * B(a0, a15)), a0
]) + multi_dot([
(cos(config.UF_mcs_act(t)) * multi_dot([a14.T, a8]) +
(-1 * sin(config.UF_mcs_act(t))) * multi_dot([a15.T, a8])),
B(a1, a13), a1
]) + (2) * multi_dot([
(cos(config.UF_mcs_act(t)) * multi_dot([a10, B(a7, a14).T]) +
(-1 * sin(config.UF_mcs_act(t))) *
multi_dot([a10, B(a7, a15).T])),
B(a4, a13), a1
])),
(multi_dot([B(a1, self.ubar_rbs_l1_jcs_b), a1]) +
(-1) * multi_dot([B(a16, self.ubar_rbs_l2_jcs_b), a16])),
(multi_dot([B(a16, self.ubar_rbs_l2_jcs_c), a16]) +
(-1) * multi_dot([B(a17, self.ubar_rbs_l3_jcs_c), a17])),
(multi_dot([a18.T, A(a19).T, B(a17, a20), a17]) +
multi_dot([a20.T, a22, B(a16, a18), a16]) + (2) * multi_dot(
[a23, B(a19, a18).T, B(a21, a20), a17])),
(multi_dot([a25, a8, a27, a17]) + multi_dot([a28, a22, a29, a0]) +
(2) * multi_dot([a30, a31, a32, a0])),
(multi_dot([a25, a8, a34, a17]) + multi_dot([a35, a22, a29, a0]) +
(2) * multi_dot([a30, a36, a32, a0])),
(multi_dot([a28, a22, a39]) + (2) * multi_dot([a30, a31, a40]) +
multi_dot([a41, a27, a17])),
(multi_dot([a35, a22, a39]) + (2) * multi_dot([a30, a36, a40]) +
multi_dot([a41, a34, a17])),
(multi_dot([a42.T, a8, a27, a17]) +
multi_dot([a28, a22, B(a0, a42), a0]) +
(2) * multi_dot([a30, a31, B(a7, a42), a0])),
Z3x1,
Z4x1,
(2) * multi_dot([a1.T, a1]),
(2) * multi_dot([a23, a16]),
(2) * multi_dot([a30, a17]),
)
def eval_jac_eq(self):
config = self.config
t = self.t
j0 = I3
j1 = self.P_ground
j2 = Z1x3
j3 = self.Mbar_rbs_l1_jcs_a[:, 2:3]
j4 = j3.T
j5 = self.P_rbs_l1
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_l1_jcs_a[:, 0:1]
j13 = self.Mbar_ground_jcs_a[:, 1:2]
j14 = self.Mbar_ground_jcs_a[:, 0:1]
j15 = self.Mbar_rbs_l2_jcs_b[:, 2:3]
j16 = j15.T
j17 = self.P_rbs_l2
j18 = A(j17).T
j19 = self.Mbar_ground_jcs_b[:, 0:1]
j20 = self.Mbar_ground_jcs_b[:, 1:2]
j21 = B(j17, j15)
j22 = self.Mbar_rbs_l3_jcs_c[:, 2:3]
j23 = j22.T
j24 = self.P_rbs_l3
j25 = A(j24).T
j26 = self.Mbar_rbs_l1_jcs_c[:, 0:1]
j27 = B(j5, j26)
j28 = self.Mbar_rbs_l1_jcs_c[:, 1:2]
j29 = B(j5, j28)
j30 = j26.T
j31 = multi_dot([j30, j6])
j32 = self.ubar_rbs_l1_jcs_c
j33 = B(j5, j32)
j34 = self.R_rbs_l3.T
j35 = self.ubar_rbs_l3_jcs_c
j36 = (self.R_rbs_l1.T + (-1) * j34 + multi_dot([j32.T, j6]) +
(-1) * multi_dot([j35.T, j25]))
j37 = j28.T
j38 = multi_dot([j37, j6])
j39 = B(j24, j22)
j40 = B(j24, j35)
j41 = self.Mbar_rbs_l4_jcs_e[:, 2:3]
j42 = j41.T
j43 = self.P_rbs_l4
j44 = A(j43).T
j45 = self.Mbar_rbs_l3_jcs_e[:, 0:1]
j46 = B(j24, j45)
j47 = self.Mbar_rbs_l3_jcs_e[:, 1:2]
j48 = B(j24, j47)
j49 = j45.T
j50 = multi_dot([j49, j25])
j51 = self.ubar_rbs_l3_jcs_e
j52 = B(j24, j51)
j53 = self.ubar_rbs_l4_jcs_e
j54 = (j34 + (-1) * self.R_rbs_l4.T + multi_dot([j51.T, j25]) +
(-1) * multi_dot([j53.T, j44]))
j55 = j47.T
j56 = multi_dot([j55, j25])
j57 = B(j43, j41)
j58 = B(j43, j53)
j59 = self.P_rbs_l5
j60 = self.Mbar_rbs_l2_jcs_h[:, 2:3]
j61 = j60.T
j62 = self.Mbar_rbs_l5_jcs_h[:, 0:1]
j63 = B(j59, j62)
j64 = self.Mbar_rbs_l5_jcs_h[:, 1:2]
j65 = B(j59, j64)
j66 = j62.T
j67 = A(j59).T
j68 = multi_dot([j66, j67])
j69 = self.ubar_rbs_l5_jcs_h
j70 = B(j59, j69)
j71 = self.ubar_rbs_l2_jcs_h
j72 = (self.R_rbs_l5.T + (-1) * self.R_rbs_l2.T +
multi_dot([j69.T, j67]) + (-1) * multi_dot([j71.T, j18]))
j73 = j64.T
j74 = multi_dot([j73, j67])
j75 = B(j17, j60)
j76 = B(j17, j71)
self.jac_eq_blocks = (
j0,
B(j1, self.ubar_ground_jcs_a),
j9,
(-1) * B(j5, self.ubar_rbs_l1_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]),
j2,
multi_dot([
j12.T, j6,
(cos(config.UF_mcs_act(t)) * B(j1, j13) +
(-1 * sin(config.UF_mcs_act(t))) * B(j1, j14))
]),
j2,
multi_dot([
(cos(config.UF_mcs_act(t)) * multi_dot([j13.T, j10]) +
(-1 * sin(config.UF_mcs_act(t))) * multi_dot([j14.T, j10])),
B(j5, j12)
]),
j0,
B(j1, self.ubar_ground_jcs_b),
j9,
(-1) * B(j17, self.ubar_rbs_l2_jcs_b),
j2,
multi_dot([j16, j18, B(j1, j19)]),
j2,
multi_dot([j19.T, j10, j21]),
j2,
multi_dot([j16, j18, B(j1, j20)]),
j2,
multi_dot([j20.T, j10, j21]),
j2,
multi_dot([j23, j25, j27]),
j2,
multi_dot([j30, j6, j39]),
j2,
multi_dot([j23, j25, j29]),
j2,
multi_dot([j37, j6, j39]),
j31,
(multi_dot([j30, j6, j33]) + multi_dot([j36, j27])),
(-1) * j31,
(-1) * multi_dot([j30, j6, j40]),
j38,
(multi_dot([j37, j6, j33]) + multi_dot([j36, j29])),
(-1) * j38,
(-1) * multi_dot([j37, j6, j40]),
j0,
B(j17, self.ubar_rbs_l2_jcs_d),
j9,
(-1) * B(j24, self.ubar_rbs_l3_jcs_d),
j2,
multi_dot([j42, j44, j46]),
j2,
multi_dot([j49, j25, j57]),
j2,
multi_dot([j42, j44, j48]),
j2,
multi_dot([j55, j25, j57]),
j50,
(multi_dot([j49, j25, j52]) + multi_dot([j54, j46])),
(-1) * j50,
(-1) * multi_dot([j49, j25, j58]),
j56,
(multi_dot([j55, j25, j52]) + multi_dot([j54, j48])),
(-1) * j56,
(-1) * multi_dot([j55, j25, j58]),
j0,
B(j43, self.ubar_rbs_l4_jcs_f),
j9,
(-1) * B(j59, self.ubar_rbs_l5_jcs_f),
j2,
multi_dot([j66, j67, j75]),
j2,
multi_dot([j61, j18, j63]),
j2,
multi_dot([j73, j67, j75]),
j2,
multi_dot([j61, j18, j65]),
(-1) * j68,
(-1) * multi_dot([j66, j67, j76]),
j68,
(multi_dot([j66, j67, j70]) + multi_dot([j72, j63])),
(-1) * j74,
(-1) * multi_dot([j73, j67, j76]),
j74,
(multi_dot([j73, j67, j70]) + multi_dot([j72, j65])),
j0,
Z3x4,
Z4x3,
I4,
j2,
(2) * j5.T,
j2,
(2) * j17.T,
j2,
(2) * j24.T,
j2,
(2) * j43.T,
j2,
(2) * j59.T,
)
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_acc_eq(self):
config = self.config
t = self.t
a0 = self.Pd_rbr_rocker
a1 = self.Pd_vbs_chassis
a2 = self.Mbar_rbr_rocker_jcr_rocker_chassis[:, 0:1]
a3 = self.P_rbr_rocker
a4 = A(a3).T
a5 = self.Mbar_vbs_chassis_jcr_rocker_chassis[:, 2:3]
a6 = B(a1, a5)
a7 = a5.T
a8 = self.P_vbs_chassis
a9 = A(a8).T
a10 = a0.T
a11 = B(a8, a5)
a12 = self.Mbar_rbr_rocker_jcr_rocker_chassis[:, 1:2]
a13 = self.Pd_rbs_coupler
a14 = self.Mbar_rbs_coupler_jcs_rocker_uni[:, 0:1]
a15 = self.P_rbs_coupler
a16 = self.Mbar_rbr_rocker_jcs_rocker_uni[:, 0:1]
a17 = self.Pd_rbl_rocker
a18 = self.Mbar_rbl_rocker_jcl_rocker_chassis[:, 0:1]
a19 = self.P_rbl_rocker
a20 = A(a19).T
a21 = self.Mbar_vbs_chassis_jcl_rocker_chassis[:, 2:3]
a22 = B(a1, a21)
a23 = a21.T
a24 = a17.T
a25 = B(a8, a21)
a26 = self.Mbar_rbl_rocker_jcl_rocker_chassis[:, 1:2]
a27 = self.Mbar_vbs_chassis_jcl_rocker_chassis[:, 0:1]
a28 = self.Mbar_rbl_rocker_jcl_rocker_chassis[:, 1:2]
a29 = self.Mbar_rbl_rocker_jcl_rocker_chassis[:, 0:1]
self.acc_eq_blocks = (
(multi_dot([B(a0, self.ubar_rbr_rocker_jcr_rocker_chassis), a0]) +
(-1) *
multi_dot([B(a1, self.ubar_vbs_chassis_jcr_rocker_chassis), a1])),
(multi_dot([a2.T, a4, a6, a1]) +
multi_dot([a7, a9, B(a0, a2), a0]) +
(2) * multi_dot([a10, B(a3, a2).T, a11, a1])),
(multi_dot([a12.T, a4, a6, a1]) +
multi_dot([a7, a9, B(a0, a12), a0]) +
(2) * multi_dot([a10, B(a3, a12).T, a11, a1])),
(multi_dot([B(a0, self.ubar_rbr_rocker_jcs_rocker_uni), a0]) +
(-1) *
multi_dot([B(a13, self.ubar_rbs_coupler_jcs_rocker_uni), a13])),
(multi_dot([a14.T, A(a15).T, B(a0, a16), a0]) +
multi_dot([a16.T, a4, B(a13, a14), a13]) + (2) *
multi_dot([a10, B(a3, a16).T, B(a15, a14), a13])),
(multi_dot([B(a17, self.ubar_rbl_rocker_jcl_rocker_chassis), a17
]) + (-1) *
multi_dot([B(a1, self.ubar_vbs_chassis_jcl_rocker_chassis), a1])),
(multi_dot([a18.T, a20, a22, a1]) +
multi_dot([a23, a9, B(a17, a18), a17]) +
(2) * multi_dot([a24, B(a19, a18).T, a25, a1])),
(multi_dot([a26.T, a20, a22, a1]) +
multi_dot([a23, a9, B(a17, a26), a17]) +
(2) * multi_dot([a24, B(a19, a26).T, a25, a1])),
((-1 * derivative(config.UF_mcs_steer_act, t, 0.1, 2)) * I1 +
multi_dot([
a27.T, a9,
(cos(config.UF_mcs_steer_act(t)) * B(a17, a28) +
(-1 * sin(config.UF_mcs_steer_act(t))) * B(a17, a29)), a17
]) + multi_dot([
(cos(config.UF_mcs_steer_act(t)) * multi_dot([a28.T, a20]) +
(-1 * sin(config.UF_mcs_steer_act(t))) *
multi_dot([a29.T, a20])),
B(a1, a27), a1
]) + (2) * multi_dot([(cos(config.UF_mcs_steer_act(t)) *
multi_dot([a24, B(a19, a28).T]) +
(-1 * sin(config.UF_mcs_steer_act(t))) *
multi_dot([a24, B(a19, a29).T])),
B(a8, a27), a1])),
(multi_dot([B(a17, self.ubar_rbl_rocker_jcs_rocker_sph), a17]) +
(-1) *
multi_dot([B(a13, self.ubar_rbs_coupler_jcs_rocker_sph), a13])),
(2) * multi_dot([a13.T, a13]),
(2) * multi_dot([a10, a0]),
(2) * multi_dot([a24, a17]),
)