def eval_acc_eq(self):
config = self.config
t = self.t
a0 = I1
a1 = self.Mbar_vbr_wheel_upright_jcr_hub_bearing[:,0:1]
a2 = self.P_vbr_wheel_upright
a3 = self.Pd_vbr_wheel_hub
a4 = self.Mbar_vbr_wheel_hub_jcr_hub_bearing[:,1:2]
a5 = self.Mbar_vbr_wheel_hub_jcr_hub_bearing[:,0:1]
a6 = self.P_vbr_wheel_hub
a7 = A(a6).T
a8 = self.Pd_vbr_wheel_upright
a9 = a3.T
a10 = self.Mbar_vbl_wheel_upright_jcl_hub_bearing[:,0:1]
a11 = self.P_vbl_wheel_upright
a12 = self.Pd_vbl_wheel_hub
a13 = self.Mbar_vbl_wheel_hub_jcl_hub_bearing[:,1:2]
a14 = self.Mbar_vbl_wheel_hub_jcl_hub_bearing[:,0:1]
a15 = self.P_vbl_wheel_hub
a16 = A(a15).T
a17 = self.Pd_vbl_wheel_upright
a18 = a12.T
self.acc_eq_blocks = (((-1 * derivative(config.UF_mcr_wheel_lock, t, 0.1, 2)) * a0 + multi_dot([a1.T,A(a2).T,(cos(config.UF_mcr_wheel_lock(t)) * B(a3,a4) + (-1 * sin(config.UF_mcr_wheel_lock(t))) * B(a3,a5)),a3]) + multi_dot([(cos(config.UF_mcr_wheel_lock(t)) * multi_dot([a4.T,a7]) + (-1 * sin(config.UF_mcr_wheel_lock(t))) * multi_dot([a5.T,a7])),B(a8,a1),a8]) + (2) * multi_dot([(cos(config.UF_mcr_wheel_lock(t)) * multi_dot([a9,B(a6,a4).T]) + (-1 * sin(config.UF_mcr_wheel_lock(t))) * multi_dot([a9,B(a6,a5).T])),B(a2,a1),a8])),
((-1 * derivative(config.UF_mcl_wheel_lock, t, 0.1, 2)) * a0 + multi_dot([a10.T,A(a11).T,(cos(config.UF_mcl_wheel_lock(t)) * B(a12,a13) + (-1 * sin(config.UF_mcl_wheel_lock(t))) * B(a12,a14)),a12]) + multi_dot([(cos(config.UF_mcl_wheel_lock(t)) * multi_dot([a13.T,a16]) + (-1 * sin(config.UF_mcl_wheel_lock(t))) * multi_dot([a14.T,a16])),B(a17,a10),a17]) + (2) * multi_dot([(cos(config.UF_mcl_wheel_lock(t)) * multi_dot([a18,B(a15,a13).T]) + (-1 * sin(config.UF_mcl_wheel_lock(t))) * multi_dot([a18,B(a15,a14).T])),B(a11,a10),a17])),)
def eval_pos_eq(self):
config = self.config
t = self.t
x0 = self.Mbar_rbs_body_jcs_trans[:, 0:1].T
x1 = self.P_rbs_body
x2 = A(x1)
x3 = x2.T
x4 = self.P_ground
x5 = A(x4)
x6 = self.Mbar_ground_jcs_trans[:, 2:3]
x7 = self.Mbar_rbs_body_jcs_trans[:, 1:2].T
x8 = self.R_ground
x9 = (self.R_rbs_body + (-1) * x8 +
multi_dot([x2, self.ubar_rbs_body_jcs_trans]) +
(-1) * multi_dot([x5, self.ubar_ground_jcs_trans]))
self.pos_eq_blocks = (
multi_dot([x0, x3, x5, x6]),
multi_dot([x7, x3, x5, x6]),
multi_dot([x0, x3, x9]),
multi_dot([x7, x3, x9]),
multi_dot([x0, x3, x5, self.Mbar_ground_jcs_trans[:, 1:2]]),
x8,
(x4 + (-1) * self.Pg_ground),
((-1) * I1 + multi_dot([x1.T, x1])),
)
def eval_pos_eq(self):
config = self.config
t = self.t
x0 = self.R_ground
x1 = self.R_rbs_body_1
x2 = self.P_ground
x3 = A(x2)
x4 = self.P_rbs_body_1
x5 = A(x4)
x6 = x3.T
x7 = self.Mbar_rbs_body_1_jcs_a[:, 2:3]
x8 = self.P_rbs_body_2
x9 = A(x8)
x10 = x5.T
x11 = self.Mbar_rbs_body_2_jcs_b[:, 2:3]
x12 = (-1) * I1
self.pos_eq_blocks = (
(x0 + (-1) * x1 + multi_dot([x3, self.ubar_ground_jcs_a]) +
(-1) * multi_dot([x5, self.ubar_rbs_body_1_jcs_a])),
multi_dot([self.Mbar_ground_jcs_a[:, 0:1].T, x6, x5, x7]),
multi_dot([self.Mbar_ground_jcs_a[:, 1:2].T, x6, x5, x7]),
(x1 + (-1) * self.R_rbs_body_2 +
multi_dot([x5, self.ubar_rbs_body_1_jcs_b]) +
(-1) * multi_dot([x9, self.ubar_rbs_body_2_jcs_b])),
multi_dot([self.Mbar_rbs_body_1_jcs_b[:, 0:1].T, x10, x9, x11]),
multi_dot([self.Mbar_rbs_body_1_jcs_b[:, 1:2].T, x10, x9, x11]),
x0,
(x2 + (-1) * self.Pg_ground),
(x12 + multi_dot([x4.T, x4])),
(x12 + multi_dot([x8.T, x8])),
)
def eval_constants(self):
config = self.config
self.R_ground = np.array([[0], [0], [0]], dtype=np.float64)
self.P_ground = np.array([[1], [0], [0], [0]], dtype=np.float64)
self.Pg_ground = np.array([[1], [0], [0], [0]], dtype=np.float64)
self.m_ground = 1.0
self.Jbar_ground = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]],
dtype=np.float64)
self.F_rbs_body_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_body]], dtype=np.float64)
self.T_rbs_body_fas_TSDA = Z3x1
self.T_ground_fas_TSDA = Z3x1
self.Mbar_rbs_body_jcs_trans = multi_dot(
[A(config.P_rbs_body).T,
triad(config.ax1_jcs_trans)])
self.Mbar_ground_jcs_trans = multi_dot(
[A(self.P_ground).T,
triad(config.ax1_jcs_trans)])
self.ubar_rbs_body_jcs_trans = (
multi_dot([A(config.P_rbs_body).T, config.pt1_jcs_trans]) +
(-1) * multi_dot([A(config.P_rbs_body).T, config.R_rbs_body]))
self.ubar_ground_jcs_trans = (
multi_dot([A(self.P_ground).T, config.pt1_jcs_trans]) +
(-1) * multi_dot([A(self.P_ground).T, self.R_ground]))
self.ubar_rbs_body_fas_TSDA = (
multi_dot([A(config.P_rbs_body).T, config.pt1_fas_TSDA]) +
(-1) * multi_dot([A(config.P_rbs_body).T, config.R_rbs_body]))
self.ubar_ground_fas_TSDA = (
multi_dot([A(self.P_ground).T, config.pt2_fas_TSDA]) +
(-1) * multi_dot([A(self.P_ground).T, self.R_ground]))
def eval_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_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_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_pos_eq(self):
config = self.config
t = self.t
x0 = self.Mbar_rbs_rack_jcs_rack[:, 0:1].T
x1 = self.P_rbs_rack
x2 = A(x1)
x3 = x2.T
x4 = A(self.P_vbs_chassis)
x5 = self.Mbar_vbs_chassis_jcs_rack[:, 2:3]
x6 = self.Mbar_rbs_rack_jcs_rack[:, 1:2].T
x7 = (self.R_rbs_rack + (-1) * self.R_vbs_chassis +
multi_dot([x2, self.ubar_rbs_rack_jcs_rack]) +
(-1) * multi_dot([x4, self.ubar_vbs_chassis_jcs_rack]))
x8 = I1
self.pos_eq_blocks = (
multi_dot([x0, x3, x4, x5]),
multi_dot([x6, x3, x4, x5]),
multi_dot([x0, x3, x7]),
multi_dot([x6, x3, x7]),
multi_dot([x0, x3, x4, self.Mbar_vbs_chassis_jcs_rack[:, 1:2]]),
((-1 * config.UF_mcs_rack_act(t)) * x8 +
multi_dot([self.Mbar_rbs_rack_jcs_rack[:, 2:3].T, x3, x7])),
((-1) * x8 + multi_dot([x1.T, x1])),
)
def eval_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_pos_eq(self):
config = self.config
t = self.t
x0 = self.R_rbr_rocker
x1 = (-1) * self.R_vbs_chassis
x2 = self.P_rbr_rocker
x3 = A(x2)
x4 = A(self.P_vbs_chassis)
x5 = x3.T
x6 = self.Mbar_vbs_chassis_jcr_rocker_chassis[:, 2:3]
x7 = (-1) * self.R_rbs_coupler
x8 = self.P_rbs_coupler
x9 = A(x8)
x10 = self.R_rbl_rocker
x11 = self.P_rbl_rocker
x12 = A(x11)
x13 = x12.T
x14 = self.Mbar_vbs_chassis_jcl_rocker_chassis[:, 2:3]
x15 = self.Mbar_vbs_chassis_jcl_rocker_chassis[:, 0:1]
x16 = (-1) * I1
self.pos_eq_blocks = (
(x0 + x1 +
multi_dot([x3, self.ubar_rbr_rocker_jcr_rocker_chassis]) +
(-1) * multi_dot([x4, self.ubar_vbs_chassis_jcr_rocker_chassis])),
multi_dot([
self.Mbar_rbr_rocker_jcr_rocker_chassis[:, 0:1].T, x5, x4, x6
]),
multi_dot([
self.Mbar_rbr_rocker_jcr_rocker_chassis[:, 1:2].T, x5, x4, x6
]),
(x0 + x7 + multi_dot([x3, self.ubar_rbr_rocker_jcs_rocker_uni]) +
(-1) * multi_dot([x9, self.ubar_rbs_coupler_jcs_rocker_uni])),
multi_dot([
self.Mbar_rbr_rocker_jcs_rocker_uni[:, 0:1].T, x5, x9,
self.Mbar_rbs_coupler_jcs_rocker_uni[:, 0:1]
]),
(x10 + x1 +
multi_dot([x12, self.ubar_rbl_rocker_jcl_rocker_chassis]) +
(-1) * multi_dot([x4, self.ubar_vbs_chassis_jcl_rocker_chassis])),
multi_dot([
self.Mbar_rbl_rocker_jcl_rocker_chassis[:, 0:1].T, x13, x4, x14
]),
multi_dot([
self.Mbar_rbl_rocker_jcl_rocker_chassis[:, 1:2].T, x13, x4, x14
]),
(cos(config.UF_mcs_steer_act(t)) * multi_dot([
self.Mbar_rbl_rocker_jcl_rocker_chassis[:, 1:2].T, x13, x4, x15
]) + (-1 * sin(config.UF_mcs_steer_act(t))) * multi_dot([
self.Mbar_rbl_rocker_jcl_rocker_chassis[:, 0:1].T, x13, x4, x15
])),
(x10 + x7 + multi_dot([x12, self.ubar_rbl_rocker_jcs_rocker_sph]) +
(-1) * multi_dot([x9, self.ubar_rbs_coupler_jcs_rocker_sph])),
(x16 + multi_dot([x8.T, x8])),
(x16 + multi_dot([x2.T, x2])),
(x16 + multi_dot([x11.T, x11])),
)
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_constants(self):
config = self.config
self.Mbar_vbr_wheel_hub_jcr_hub_bearing = multi_dot([A(config.P_vbr_wheel_hub).T,triad(config.ax1_jcr_hub_bearing)])
self.Mbar_vbr_wheel_upright_jcr_hub_bearing = multi_dot([A(config.P_vbr_wheel_upright).T,triad(config.ax1_jcr_hub_bearing)])
self.Mbar_vbl_wheel_hub_jcl_hub_bearing = multi_dot([A(config.P_vbl_wheel_hub).T,triad(config.ax1_jcl_hub_bearing)])
self.Mbar_vbl_wheel_upright_jcl_hub_bearing = multi_dot([A(config.P_vbl_wheel_upright).T,triad(config.ax1_jcl_hub_bearing)])
def eval_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_pos_eq(self):
config = self.config
t = self.t
x0 = self.R_ground
x1 = self.R_rbs_l1
x2 = self.P_ground
x3 = A(x2)
x4 = self.P_rbs_l1
x5 = A(x4)
x6 = x3.T
x7 = self.Mbar_rbs_l1_jcs_a[:, 2:3]
x8 = self.Mbar_rbs_l1_jcs_a[:, 0:1]
x9 = self.R_rbs_l2
x10 = self.P_rbs_l2
x11 = A(x10)
x12 = self.R_rbs_l3
x13 = self.P_rbs_l3
x14 = A(x13)
x15 = self.Mbar_rbs_l3_jcs_d[:, 0:1].T
x16 = x14.T
x17 = self.Mbar_ground_jcs_d[:, 2:3]
x18 = self.Mbar_rbs_l3_jcs_d[:, 1:2].T
x19 = (x12 + (-1) * x0 + multi_dot([x14, self.ubar_rbs_l3_jcs_d]) +
(-1) * multi_dot([x3, self.ubar_ground_jcs_d]))
x20 = (-1) * I1
self.pos_eq_blocks = (
(x0 + (-1) * x1 + multi_dot([x3, self.ubar_ground_jcs_a]) +
(-1) * multi_dot([x5, self.ubar_rbs_l1_jcs_a])),
multi_dot([self.Mbar_ground_jcs_a[:, 0:1].T, x6, x5, x7]),
multi_dot([self.Mbar_ground_jcs_a[:, 1:2].T, x6, x5, x7]),
(cos(config.UF_mcs_act(t)) *
multi_dot([self.Mbar_ground_jcs_a[:, 1:2].T, x6, x5, x8]) +
(-1 * sin(config.UF_mcs_act(t))) *
multi_dot([self.Mbar_ground_jcs_a[:, 0:1].T, x6, x5, x8])),
(x1 + (-1) * x9 + multi_dot([x5, self.ubar_rbs_l1_jcs_b]) +
(-1) * multi_dot([x11, self.ubar_rbs_l2_jcs_b])),
(x9 + (-1) * x12 + multi_dot([x11, self.ubar_rbs_l2_jcs_c]) +
(-1) * multi_dot([x14, self.ubar_rbs_l3_jcs_c])),
multi_dot([
self.Mbar_rbs_l2_jcs_c[:, 0:1].T, x11.T, x14,
self.Mbar_rbs_l3_jcs_c[:, 0:1]
]),
multi_dot([x15, x16, x3, x17]),
multi_dot([x18, x16, x3, x17]),
multi_dot([x15, x16, x19]),
multi_dot([x18, x16, x19]),
multi_dot([x15, x16, x3, self.Mbar_ground_jcs_d[:, 1:2]]),
x0,
(x2 + (-1) * self.Pg_ground),
(x20 + multi_dot([x4.T, x4])),
(x20 + multi_dot([x10.T, x10])),
(x20 + multi_dot([x13.T, x13])),
)
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_constants(self):
config = self.config
self.F_rbs_chassis_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_chassis]], dtype=np.float64)
self.ubar_rbs_chassis_fas_aero_drag = (
multi_dot([A(config.P_rbs_chassis).T, config.pt1_fas_aero_drag]) +
(-1) *
multi_dot([A(config.P_rbs_chassis).T, config.R_rbs_chassis]))
self.ubar_vbs_ground_fas_aero_drag = (
multi_dot([A(config.P_vbs_ground).T, config.pt1_fas_aero_drag]) +
(-1) * multi_dot([A(config.P_vbs_ground).T, config.R_vbs_ground]))
def eval_pos_eq(self):
config = self.config
t = self.t
x0 = A(self.P_vbr_wheel_hub).T
x1 = A(self.P_vbr_wheel_upright)
x2 = self.Mbar_vbr_wheel_upright_jcr_hub_bearing[:,0:1]
x3 = A(self.P_vbl_wheel_hub).T
x4 = A(self.P_vbl_wheel_upright)
x5 = self.Mbar_vbl_wheel_upright_jcl_hub_bearing[:,0:1]
self.pos_eq_blocks = ((cos(config.UF_mcr_wheel_lock(t)) * multi_dot([self.Mbar_vbr_wheel_hub_jcr_hub_bearing[:,1:2].T,x0,x1,x2]) + (-1 * sin(config.UF_mcr_wheel_lock(t))) * multi_dot([self.Mbar_vbr_wheel_hub_jcr_hub_bearing[:,0:1].T,x0,x1,x2])),
(cos(config.UF_mcl_wheel_lock(t)) * multi_dot([self.Mbar_vbl_wheel_hub_jcl_hub_bearing[:,1:2].T,x3,x4,x5]) + (-1 * sin(config.UF_mcl_wheel_lock(t))) * multi_dot([self.Mbar_vbl_wheel_hub_jcl_hub_bearing[:,0:1].T,x3,x4,x5])),)
def eval_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 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_constants(self):
config = self.config
self.R_ground = np.array([[0], [0], [0]], dtype=np.float64)
self.P_ground = np.array([[1], [0], [0], [0]], dtype=np.float64)
self.Pg_ground = np.array([[1], [0], [0], [0]], dtype=np.float64)
self.m_ground = 1.0
self.Jbar_ground = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]], dtype=np.float64)
self.F_rbs_body_gravity = np.array([[0], [0], [-9810.0*config.m_rbs_body]], dtype=np.float64)
self.M_ground = self.m_ground * I3
self.M_rbs_body = config.m_rbs_body * I3
self.Mbar_ground_jcs_a = multi_dot([A(self.P_ground).T,triad(config.ax1_jcs_a)])
self.Mbar_rbs_body_jcs_a = multi_dot([A(config.P_rbs_body).T,triad(config.ax1_jcs_a)])
self.ubar_ground_jcs_a = (multi_dot([A(self.P_ground).T,config.pt1_jcs_a]) + (-1) * multi_dot([A(self.P_ground).T,self.R_ground]))
self.ubar_rbs_body_jcs_a = (multi_dot([A(config.P_rbs_body).T,config.pt1_jcs_a]) + (-1) * multi_dot([A(config.P_rbs_body).T,config.R_rbs_body]))
def eval_pos_eq(self):
config = self.config
t = self.t
x0 = self.R_ground
x1 = self.P_ground
x2 = A(x1)
x3 = self.P_rbs_body
x4 = A(x3)
x5 = x2.T
x6 = self.Mbar_rbs_body_jcs_a[:,2:3]
self.pos_eq_blocks = ((x0 + (-1) * self.R_rbs_body + multi_dot([x2,self.ubar_ground_jcs_a]) + (-1) * multi_dot([x4,self.ubar_rbs_body_jcs_a])),
multi_dot([self.Mbar_ground_jcs_a[:,0:1].T,x5,x4,x6]),
multi_dot([self.Mbar_ground_jcs_a[:,1:2].T,x5,x4,x6]),
x0,
(x1 + (-1) * self.Pg_ground),
((-1) * I1 + multi_dot([x3.T,x3])),)
def eval_pos_eq(self):
config = self.config
t = self.t
x0 = self.Mbar_rbs_body_1_jcs_trans_1[:, 0:1].T
x1 = self.P_rbs_body_1
x2 = A(x1)
x3 = x2.T
x4 = self.P_ground
x5 = A(x4)
x6 = self.Mbar_ground_jcs_trans_1[:, 2:3]
x7 = self.Mbar_rbs_body_1_jcs_trans_1[:, 1:2].T
x8 = self.R_ground
x9 = (-1) * x8
x10 = (self.R_rbs_body_1 + x9 +
multi_dot([x2, self.ubar_rbs_body_1_jcs_trans_1]) +
(-1) * multi_dot([x5, self.ubar_ground_jcs_trans_1]))
x11 = self.Mbar_rbs_body_2_jcs_trans_2[:, 0:1].T
x12 = self.P_rbs_body_2
x13 = A(x12)
x14 = x13.T
x15 = self.Mbar_ground_jcs_trans_2[:, 2:3]
x16 = self.Mbar_rbs_body_2_jcs_trans_2[:, 1:2].T
x17 = (self.R_rbs_body_2 + x9 +
multi_dot([x13, self.ubar_rbs_body_2_jcs_trans_2]) +
(-1) * multi_dot([x5, self.ubar_ground_jcs_trans_2]))
x18 = (-1) * I1
self.pos_eq_blocks = (
multi_dot([x0, x3, x5, x6]),
multi_dot([x7, x3, x5, x6]),
multi_dot([x0, x3, x10]),
multi_dot([x7, x3, x10]),
multi_dot([x0, x3, x5, self.Mbar_ground_jcs_trans_1[:, 1:2]]),
multi_dot([x11, x14, x5, x15]),
multi_dot([x16, x14, x5, x15]),
multi_dot([x11, x14, x17]),
multi_dot([x16, x14, x17]),
multi_dot([x11, x14, x5, self.Mbar_ground_jcs_trans_2[:, 1:2]]),
x8,
(x4 + (-1) * self.Pg_ground),
(x18 + multi_dot([x1.T, x1])),
(x18 + multi_dot([x12.T, x12])),
)
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_pos_eq(self):
config = self.config
t = self.t
x0 = self.Mbar_rbs_input_shaft_jcs_input_bearing[:,0:1].T
x1 = self.P_rbs_input_shaft
x2 = A(x1)
x3 = x2.T
x4 = A(self.P_vbs_chassis)
x5 = self.Mbar_vbs_chassis_jcs_input_bearing[:,2:3]
x6 = self.Mbar_rbs_input_shaft_jcs_input_bearing[:,1:2].T
x7 = self.R_rbs_input_shaft
x8 = -1*self.R_vbs_chassis
x9 = (x7 + x8 + multi_dot([x2,self.ubar_rbs_input_shaft_jcs_input_bearing]) + -1*multi_dot([x4,self.ubar_vbs_chassis_jcs_input_bearing]))
x10 = self.Mbar_vbs_chassis_jcs_input_bearing[:,0:1]
x11 = -1*self.R_rbs_connect_shaft
x12 = self.P_rbs_connect_shaft
x13 = A(x12)
x14 = self.R_rbs_output_shaft
x15 = self.P_rbs_output_shaft
x16 = A(x15)
x17 = x16.T
x18 = self.Mbar_vbs_chassis_jcs_output_bearing[:,2:3]
x19 = -1*np.eye(1, dtype=np.float64)
self.pos_eq_blocks = (multi_dot([x0,x3,x4,x5]),
multi_dot([x6,x3,x4,x5]),
multi_dot([x0,x3,x9]),
multi_dot([x6,x3,x9]),
(cos(config.UF_mcs_hand_wheel(t))*multi_dot([self.Mbar_rbs_input_shaft_jcs_input_bearing[:,1:2].T,x3,x4,x10]) + -1*sin(config.UF_mcs_hand_wheel(t))*multi_dot([self.Mbar_rbs_input_shaft_jcs_input_bearing[:,0:1].T,x3,x4,x10])),
(x7 + x11 + multi_dot([x2,self.ubar_rbs_input_shaft_jcs_input_connect]) + -1*multi_dot([x13,self.ubar_rbs_connect_shaft_jcs_input_connect])),
multi_dot([self.Mbar_rbs_input_shaft_jcs_input_connect[:,0:1].T,x3,x13,self.Mbar_rbs_connect_shaft_jcs_input_connect[:,0:1]]),
(x14 + x11 + multi_dot([x16,self.ubar_rbs_output_shaft_jcs_output_connect]) + -1*multi_dot([x13,self.ubar_rbs_connect_shaft_jcs_output_connect])),
multi_dot([self.Mbar_rbs_output_shaft_jcs_output_connect[:,0:1].T,x17,x13,self.Mbar_rbs_connect_shaft_jcs_output_connect[:,0:1]]),
(x14 + x8 + multi_dot([x16,self.ubar_rbs_output_shaft_jcs_output_bearing]) + -1*multi_dot([x4,self.ubar_vbs_chassis_jcs_output_bearing])),
multi_dot([self.Mbar_rbs_output_shaft_jcs_output_bearing[:,0:1].T,x17,x4,x18]),
multi_dot([self.Mbar_rbs_output_shaft_jcs_output_bearing[:,1:2].T,x17,x4,x18]),
(x19 + multi_dot([x1.T,x1])),
(x19 + multi_dot([x12.T,x12])),
(x19 + multi_dot([x15.T,x15])),)
def eval_pos_eq(self):
config = self.config
t = self.t
x0 = self.R_rbr_inner_shaft
x1 = (-1) * self.R_vbs_differential
x2 = self.P_rbr_inner_shaft
x3 = A(x2)
x4 = A(self.P_vbs_differential)
x5 = x3.T
x6 = self.Mbar_vbs_differential_jcr_diff_joint[:,2:3]
x7 = self.R_rbr_coupling
x8 = self.P_rbr_coupling
x9 = A(x8)
x10 = self.R_rbl_inner_shaft
x11 = self.P_rbl_inner_shaft
x12 = A(x11)
x13 = x12.T
x14 = self.Mbar_vbs_differential_jcl_diff_joint[:,2:3]
x15 = self.R_rbl_coupling
x16 = self.P_rbl_coupling
x17 = A(x16)
x18 = self.Mbar_rbr_coupling_jcr_outer_cv[:,0:1].T
x19 = x9.T
x20 = A(self.P_vbr_wheel_hub)
x21 = (x7 + (-1) * self.R_vbr_wheel_hub + multi_dot([x9,self.ubar_rbr_coupling_jcr_outer_cv]) + (-1) * multi_dot([x20,self.ubar_vbr_wheel_hub_jcr_outer_cv]))
x22 = self.Mbar_rbl_coupling_jcl_outer_cv[:,0:1].T
x23 = x17.T
x24 = A(self.P_vbl_wheel_hub)
x25 = (x15 + (-1) * self.R_vbl_wheel_hub + multi_dot([x17,self.ubar_rbl_coupling_jcl_outer_cv]) + (-1) * multi_dot([x24,self.ubar_vbl_wheel_hub_jcl_outer_cv]))
x26 = (-1) * I1
self.pos_eq_blocks = ((x0 + x1 + multi_dot([x3,self.ubar_rbr_inner_shaft_jcr_diff_joint]) + (-1) * multi_dot([x4,self.ubar_vbs_differential_jcr_diff_joint])),
multi_dot([self.Mbar_rbr_inner_shaft_jcr_diff_joint[:,0:1].T,x5,x4,x6]),
multi_dot([self.Mbar_rbr_inner_shaft_jcr_diff_joint[:,1:2].T,x5,x4,x6]),
(x0 + (-1) * x7 + multi_dot([x3,self.ubar_rbr_inner_shaft_jcr_inner_cv]) + (-1) * multi_dot([x9,self.ubar_rbr_coupling_jcr_inner_cv])),
multi_dot([self.Mbar_rbr_inner_shaft_jcr_inner_cv[:,0:1].T,x5,x9,self.Mbar_rbr_coupling_jcr_inner_cv[:,0:1]]),
(x10 + x1 + multi_dot([x12,self.ubar_rbl_inner_shaft_jcl_diff_joint]) + (-1) * multi_dot([x4,self.ubar_vbs_differential_jcl_diff_joint])),
multi_dot([self.Mbar_rbl_inner_shaft_jcl_diff_joint[:,0:1].T,x13,x4,x14]),
multi_dot([self.Mbar_rbl_inner_shaft_jcl_diff_joint[:,1:2].T,x13,x4,x14]),
(x10 + (-1) * x15 + multi_dot([x12,self.ubar_rbl_inner_shaft_jcl_inner_cv]) + (-1) * multi_dot([x17,self.ubar_rbl_coupling_jcl_inner_cv])),
multi_dot([self.Mbar_rbl_inner_shaft_jcl_inner_cv[:,0:1].T,x13,x17,self.Mbar_rbl_coupling_jcl_inner_cv[:,0:1]]),
multi_dot([x18,x19,x20,self.Mbar_vbr_wheel_hub_jcr_outer_cv[:,0:1]]),
multi_dot([x18,x19,x21]),
multi_dot([self.Mbar_rbr_coupling_jcr_outer_cv[:,1:2].T,x19,x21]),
multi_dot([x22,x23,x24,self.Mbar_vbl_wheel_hub_jcl_outer_cv[:,0:1]]),
multi_dot([x22,x23,x25]),
multi_dot([self.Mbar_rbl_coupling_jcl_outer_cv[:,1:2].T,x23,x25]),
(x26 + multi_dot([x2.T,x2])),
(x26 + multi_dot([x11.T,x11])),
(x26 + multi_dot([x8.T,x8])),
(x26 + multi_dot([x16.T,x16])),)
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 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 get_heading_error(self, P_ch):
idx = self._idx
try:
path_heading = self._path_headings[idx]
except (IndexError):
path_heading = self._path_headings[idx - 1]
# getting the chassis heading as a 2d vector of two components [x, y]
chassis_heading = (A(P_ch) @ np.array([[-1], [0], [0]]))[0:2]
# normalizing the vector to get non-scaled angle
chassis_heading = normalize(chassis_heading)
# The cross funtion returns a scaler for 2d vectors that represent the
# z-axis value of the resultant normal vector. This captures the
# direction of rotation needed too.
angle = np.cross(path_heading.flat[:], chassis_heading.flat[:])
return angle