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_constants(self):
config = self.config
self.F_rbs_rack_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_rack]], dtype=np.float64)
self.Mbar_rbs_rack_jcs_rack = multi_dot(
[A(config.P_rbs_rack).T,
triad(config.ax1_jcs_rack)])
self.Mbar_vbs_chassis_jcs_rack = multi_dot(
[A(config.P_vbs_chassis).T,
triad(config.ax1_jcs_rack)])
self.ubar_rbs_rack_jcs_rack = (
multi_dot([A(config.P_rbs_rack).T, config.pt1_jcs_rack]) +
(-1) * multi_dot([A(config.P_rbs_rack).T, config.R_rbs_rack]))
self.ubar_vbs_chassis_jcs_rack = (
multi_dot([A(config.P_vbs_chassis).T, config.pt1_jcs_rack]) +
(-1) *
multi_dot([A(config.P_vbs_chassis).T, config.R_vbs_chassis]))
self.Mbar_rbs_rack_jcs_rack = multi_dot(
[A(config.P_rbs_rack).T,
triad(config.ax1_jcs_rack)])
self.Mbar_vbs_chassis_jcs_rack = multi_dot(
[A(config.P_vbs_chassis).T,
triad(config.ax1_jcs_rack)])
self.ubar_rbs_rack_jcs_rack = (
multi_dot([A(config.P_rbs_rack).T, config.pt1_jcs_rack]) +
(-1) * multi_dot([A(config.P_rbs_rack).T, config.R_rbs_rack]))
self.ubar_vbs_chassis_jcs_rack = (
multi_dot([A(config.P_vbs_chassis).T, config.pt1_jcs_rack]) +
(-1) *
multi_dot([A(config.P_vbs_chassis).T, config.R_vbs_chassis]))
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_constants(self):
config = self.config
self.F_rbr_inner_shaft_gravity = np.array([[0], [0], [-9810.0*config.m_rbr_inner_shaft]], dtype=np.float64)
self.F_rbr_inner_shaft_far_drive = Z3x1
self.F_rbl_inner_shaft_gravity = np.array([[0], [0], [-9810.0*config.m_rbl_inner_shaft]], dtype=np.float64)
self.F_rbl_inner_shaft_fal_drive = Z3x1
self.F_rbr_coupling_gravity = np.array([[0], [0], [-9810.0*config.m_rbr_coupling]], dtype=np.float64)
self.F_rbl_coupling_gravity = np.array([[0], [0], [-9810.0*config.m_rbl_coupling]], dtype=np.float64)
self.vbar_rbr_inner_shaft_far_drive = multi_dot([A(config.P_rbr_inner_shaft).T,config.ax1_far_drive,(multi_dot([config.ax1_far_drive.T,A(config.P_rbr_inner_shaft),A(config.P_rbr_inner_shaft).T,config.ax1_far_drive]))**(-1.0/2.0)])
self.Mbar_rbr_inner_shaft_far_drive = multi_dot([A(config.P_rbr_inner_shaft).T,triad(config.ax1_far_drive)])
self.Mbar_vbs_ground_far_drive = multi_dot([A(config.P_vbs_ground).T,triad(config.ax1_far_drive)])
self.Mbar_rbr_inner_shaft_jcr_diff_joint = multi_dot([A(config.P_rbr_inner_shaft).T,triad(config.ax1_jcr_diff_joint)])
self.Mbar_vbs_differential_jcr_diff_joint = multi_dot([A(config.P_vbs_differential).T,triad(config.ax1_jcr_diff_joint)])
self.ubar_rbr_inner_shaft_jcr_diff_joint = (multi_dot([A(config.P_rbr_inner_shaft).T,config.pt1_jcr_diff_joint]) + (-1) * multi_dot([A(config.P_rbr_inner_shaft).T,config.R_rbr_inner_shaft]))
self.ubar_vbs_differential_jcr_diff_joint = (multi_dot([A(config.P_vbs_differential).T,config.pt1_jcr_diff_joint]) + (-1) * multi_dot([A(config.P_vbs_differential).T,config.R_vbs_differential]))
self.Mbar_rbr_inner_shaft_jcr_inner_cv = multi_dot([A(config.P_rbr_inner_shaft).T,triad(config.ax1_jcr_inner_cv)])
self.Mbar_rbr_coupling_jcr_inner_cv = multi_dot([A(config.P_rbr_coupling).T,triad(config.ax2_jcr_inner_cv,triad(config.ax1_jcr_inner_cv)[0:3,1:2])])
self.ubar_rbr_inner_shaft_jcr_inner_cv = (multi_dot([A(config.P_rbr_inner_shaft).T,config.pt1_jcr_inner_cv]) + (-1) * multi_dot([A(config.P_rbr_inner_shaft).T,config.R_rbr_inner_shaft]))
self.ubar_rbr_coupling_jcr_inner_cv = (multi_dot([A(config.P_rbr_coupling).T,config.pt1_jcr_inner_cv]) + (-1) * multi_dot([A(config.P_rbr_coupling).T,config.R_rbr_coupling]))
self.vbar_rbl_inner_shaft_fal_drive = multi_dot([A(config.P_rbl_inner_shaft).T,config.ax1_fal_drive,(multi_dot([config.ax1_fal_drive.T,A(config.P_rbl_inner_shaft),A(config.P_rbl_inner_shaft).T,config.ax1_fal_drive]))**(-1.0/2.0)])
self.Mbar_rbl_inner_shaft_fal_drive = multi_dot([A(config.P_rbl_inner_shaft).T,triad(config.ax1_fal_drive)])
self.Mbar_vbs_ground_fal_drive = multi_dot([A(config.P_vbs_ground).T,triad(config.ax1_fal_drive)])
self.Mbar_rbl_inner_shaft_jcl_diff_joint = multi_dot([A(config.P_rbl_inner_shaft).T,triad(config.ax1_jcl_diff_joint)])
self.Mbar_vbs_differential_jcl_diff_joint = multi_dot([A(config.P_vbs_differential).T,triad(config.ax1_jcl_diff_joint)])
self.ubar_rbl_inner_shaft_jcl_diff_joint = (multi_dot([A(config.P_rbl_inner_shaft).T,config.pt1_jcl_diff_joint]) + (-1) * multi_dot([A(config.P_rbl_inner_shaft).T,config.R_rbl_inner_shaft]))
self.ubar_vbs_differential_jcl_diff_joint = (multi_dot([A(config.P_vbs_differential).T,config.pt1_jcl_diff_joint]) + (-1) * multi_dot([A(config.P_vbs_differential).T,config.R_vbs_differential]))
self.Mbar_rbl_inner_shaft_jcl_inner_cv = multi_dot([A(config.P_rbl_inner_shaft).T,triad(config.ax1_jcl_inner_cv)])
self.Mbar_rbl_coupling_jcl_inner_cv = multi_dot([A(config.P_rbl_coupling).T,triad(config.ax2_jcl_inner_cv,triad(config.ax1_jcl_inner_cv)[0:3,1:2])])
self.ubar_rbl_inner_shaft_jcl_inner_cv = (multi_dot([A(config.P_rbl_inner_shaft).T,config.pt1_jcl_inner_cv]) + (-1) * multi_dot([A(config.P_rbl_inner_shaft).T,config.R_rbl_inner_shaft]))
self.ubar_rbl_coupling_jcl_inner_cv = (multi_dot([A(config.P_rbl_coupling).T,config.pt1_jcl_inner_cv]) + (-1) * multi_dot([A(config.P_rbl_coupling).T,config.R_rbl_coupling]))
self.Mbar_rbr_coupling_jcr_outer_cv = multi_dot([A(config.P_rbr_coupling).T,triad(config.ax1_jcr_outer_cv)])
self.Mbar_vbr_wheel_hub_jcr_outer_cv = multi_dot([A(config.P_vbr_wheel_hub).T,triad(config.ax2_jcr_outer_cv,triad(config.ax1_jcr_outer_cv)[0:3,1:2])])
self.ubar_rbr_coupling_jcr_outer_cv = (multi_dot([A(config.P_rbr_coupling).T,config.pt1_jcr_outer_cv]) + (-1) * multi_dot([A(config.P_rbr_coupling).T,config.R_rbr_coupling]))
self.ubar_vbr_wheel_hub_jcr_outer_cv = (multi_dot([A(config.P_vbr_wheel_hub).T,config.pt1_jcr_outer_cv]) + (-1) * multi_dot([A(config.P_vbr_wheel_hub).T,config.R_vbr_wheel_hub]))
self.Mbar_rbl_coupling_jcl_outer_cv = multi_dot([A(config.P_rbl_coupling).T,triad(config.ax1_jcl_outer_cv)])
self.Mbar_vbl_wheel_hub_jcl_outer_cv = multi_dot([A(config.P_vbl_wheel_hub).T,triad(config.ax2_jcl_outer_cv,triad(config.ax1_jcl_outer_cv)[0:3,1:2])])
self.ubar_rbl_coupling_jcl_outer_cv = (multi_dot([A(config.P_rbl_coupling).T,config.pt1_jcl_outer_cv]) + (-1) * multi_dot([A(config.P_rbl_coupling).T,config.R_rbl_coupling]))
self.ubar_vbl_wheel_hub_jcl_outer_cv = (multi_dot([A(config.P_vbl_wheel_hub).T,config.pt1_jcl_outer_cv]) + (-1) * multi_dot([A(config.P_vbl_wheel_hub).T,config.R_vbl_wheel_hub]))
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_l1_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_l1]], dtype=np.float64)
self.F_rbs_l2_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_l2]], dtype=np.float64)
self.F_rbs_l3_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_l3]], dtype=np.float64)
self.Mbar_ground_jcs_a = multi_dot(
[A(self.P_ground).T, triad(config.ax1_jcs_a)])
self.Mbar_rbs_l1_jcs_a = multi_dot(
[A(config.P_rbs_l1).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_l1_jcs_a = (
multi_dot([A(config.P_rbs_l1).T, config.pt1_jcs_a]) +
(-1) * multi_dot([A(config.P_rbs_l1).T, config.R_rbs_l1]))
self.Mbar_ground_jcs_a = multi_dot(
[A(self.P_ground).T, triad(config.ax1_jcs_a)])
self.Mbar_rbs_l1_jcs_a = multi_dot(
[A(config.P_rbs_l1).T,
triad(config.ax1_jcs_a)])
self.Mbar_rbs_l1_jcs_b = multi_dot(
[A(config.P_rbs_l1).T,
triad(config.ax1_jcs_b)])
self.Mbar_rbs_l2_jcs_b = multi_dot(
[A(config.P_rbs_l2).T,
triad(config.ax1_jcs_b)])
self.ubar_rbs_l1_jcs_b = (
multi_dot([A(config.P_rbs_l1).T, config.pt1_jcs_b]) +
(-1) * multi_dot([A(config.P_rbs_l1).T, config.R_rbs_l1]))
self.ubar_rbs_l2_jcs_b = (
multi_dot([A(config.P_rbs_l2).T, config.pt1_jcs_b]) +
(-1) * multi_dot([A(config.P_rbs_l2).T, config.R_rbs_l2]))
self.Mbar_rbs_l2_jcs_c = multi_dot(
[A(config.P_rbs_l2).T,
triad(config.ax1_jcs_c)])
self.Mbar_rbs_l3_jcs_c = multi_dot([
A(config.P_rbs_l3).T,
triad(config.ax2_jcs_c,
triad(config.ax1_jcs_c)[0:3, 1:2])
])
self.ubar_rbs_l2_jcs_c = (
multi_dot([A(config.P_rbs_l2).T, config.pt1_jcs_c]) +
(-1) * multi_dot([A(config.P_rbs_l2).T, config.R_rbs_l2]))
self.ubar_rbs_l3_jcs_c = (
multi_dot([A(config.P_rbs_l3).T, config.pt1_jcs_c]) +
(-1) * multi_dot([A(config.P_rbs_l3).T, config.R_rbs_l3]))
self.Mbar_rbs_l3_jcs_d = multi_dot(
[A(config.P_rbs_l3).T,
triad(config.ax1_jcs_d)])
self.Mbar_ground_jcs_d = multi_dot(
[A(self.P_ground).T, triad(config.ax1_jcs_d)])
self.ubar_rbs_l3_jcs_d = (
multi_dot([A(config.P_rbs_l3).T, config.pt1_jcs_d]) +
(-1) * multi_dot([A(config.P_rbs_l3).T, config.R_rbs_l3]))
self.ubar_ground_jcs_d = (
multi_dot([A(self.P_ground).T, config.pt1_jcs_d]) +
(-1) * multi_dot([A(self.P_ground).T, self.R_ground]))
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_l1_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_l1]], dtype=np.float64)
self.F_rbs_l2_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_l2]], dtype=np.float64)
self.F_rbs_l3_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_l3]], dtype=np.float64)
self.F_rbs_l4_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_l4]], dtype=np.float64)
self.F_rbs_l5_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_l5]], dtype=np.float64)
self.F_rbs_l6_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_l6]], dtype=np.float64)
self.F_rbs_l7_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_l7]], dtype=np.float64)
self.F_rbs_l8_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_l8]], dtype=np.float64)
self.F_rbs_l9_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_l9]], dtype=np.float64)
self.Mbar_ground_jcs_j1 = multi_dot(
[A(self.P_ground).T, triad(config.ax1_jcs_j1)])
self.Mbar_rbs_l1_jcs_j1 = multi_dot(
[A(config.P_rbs_l1).T,
triad(config.ax1_jcs_j1)])
self.ubar_ground_jcs_j1 = (
multi_dot([A(self.P_ground).T, config.pt1_jcs_j1]) +
(-1) * multi_dot([A(self.P_ground).T, self.R_ground]))
self.ubar_rbs_l1_jcs_j1 = (
multi_dot([A(config.P_rbs_l1).T, config.pt1_jcs_j1]) +
(-1) * multi_dot([A(config.P_rbs_l1).T, config.R_rbs_l1]))
self.Mbar_rbs_l1_jcs_j2 = multi_dot(
[A(config.P_rbs_l1).T,
triad(config.ax1_jcs_j2)])
self.Mbar_rbs_l2_jcs_j2 = multi_dot(
[A(config.P_rbs_l2).T,
triad(config.ax1_jcs_j2)])
self.ubar_rbs_l1_jcs_j2 = (
multi_dot([A(config.P_rbs_l1).T, config.pt1_jcs_j2]) +
(-1) * multi_dot([A(config.P_rbs_l1).T, config.R_rbs_l1]))
self.ubar_rbs_l2_jcs_j2 = (
multi_dot([A(config.P_rbs_l2).T, config.pt1_jcs_j2]) +
(-1) * multi_dot([A(config.P_rbs_l2).T, config.R_rbs_l2]))
self.Mbar_rbs_l2_jcs_j3 = multi_dot(
[A(config.P_rbs_l2).T,
triad(config.ax1_jcs_j3)])
self.Mbar_rbs_l3_jcs_j3 = multi_dot(
[A(config.P_rbs_l3).T,
triad(config.ax1_jcs_j3)])
self.ubar_rbs_l2_jcs_j3 = (
multi_dot([A(config.P_rbs_l2).T, config.pt1_jcs_j3]) +
(-1) * multi_dot([A(config.P_rbs_l2).T, config.R_rbs_l2]))
self.ubar_rbs_l3_jcs_j3 = (
multi_dot([A(config.P_rbs_l3).T, config.pt1_jcs_j3]) +
(-1) * multi_dot([A(config.P_rbs_l3).T, config.R_rbs_l3]))
self.Mbar_rbs_l3_jcs_j4 = multi_dot(
[A(config.P_rbs_l3).T,
triad(config.ax1_jcs_j4)])
self.Mbar_ground_jcs_j4 = multi_dot(
[A(self.P_ground).T, triad(config.ax1_jcs_j4)])
self.ubar_rbs_l3_jcs_j4 = (
multi_dot([A(config.P_rbs_l3).T, config.pt1_jcs_j4]) +
(-1) * multi_dot([A(config.P_rbs_l3).T, config.R_rbs_l3]))
self.ubar_ground_jcs_j4 = (
multi_dot([A(self.P_ground).T, config.pt1_jcs_j4]) +
(-1) * multi_dot([A(self.P_ground).T, self.R_ground]))
self.Mbar_rbs_l3_jcs_j5 = multi_dot(
[A(config.P_rbs_l3).T,
triad(config.ax1_jcs_j5)])
self.Mbar_rbs_l4_jcs_j5 = multi_dot(
[A(config.P_rbs_l4).T,
triad(config.ax1_jcs_j5)])
self.ubar_rbs_l3_jcs_j5 = (
multi_dot([A(config.P_rbs_l3).T, config.pt1_jcs_j5]) +
(-1) * multi_dot([A(config.P_rbs_l3).T, config.R_rbs_l3]))
self.ubar_rbs_l4_jcs_j5 = (
multi_dot([A(config.P_rbs_l4).T, config.pt1_jcs_j5]) +
(-1) * multi_dot([A(config.P_rbs_l4).T, config.R_rbs_l4]))
self.Mbar_rbs_l4_jcs_j6 = multi_dot(
[A(config.P_rbs_l4).T,
triad(config.ax1_jcs_j6)])
self.Mbar_rbs_l5_jcs_j6 = multi_dot(
[A(config.P_rbs_l5).T,
triad(config.ax1_jcs_j6)])
self.ubar_rbs_l4_jcs_j6 = (
multi_dot([A(config.P_rbs_l4).T, config.pt1_jcs_j6]) +
(-1) * multi_dot([A(config.P_rbs_l4).T, config.R_rbs_l4]))
self.ubar_rbs_l5_jcs_j6 = (
multi_dot([A(config.P_rbs_l5).T, config.pt1_jcs_j6]) +
(-1) * multi_dot([A(config.P_rbs_l5).T, config.R_rbs_l5]))
self.Mbar_rbs_l5_jcs_j7 = multi_dot(
[A(config.P_rbs_l5).T,
triad(config.ax1_jcs_j7)])
self.Mbar_ground_jcs_j7 = multi_dot(
[A(self.P_ground).T, triad(config.ax1_jcs_j7)])
self.ubar_rbs_l5_jcs_j7 = (
multi_dot([A(config.P_rbs_l5).T, config.pt1_jcs_j7]) +
(-1) * multi_dot([A(config.P_rbs_l5).T, config.R_rbs_l5]))
self.ubar_ground_jcs_j7 = (
multi_dot([A(self.P_ground).T, config.pt1_jcs_j7]) +
(-1) * multi_dot([A(self.P_ground).T, self.R_ground]))
self.Mbar_rbs_l5_jcs_j8 = multi_dot(
[A(config.P_rbs_l5).T,
triad(config.ax1_jcs_j8)])
self.Mbar_rbs_l6_jcs_j8 = multi_dot(
[A(config.P_rbs_l6).T,
triad(config.ax1_jcs_j8)])
self.ubar_rbs_l5_jcs_j8 = (
multi_dot([A(config.P_rbs_l5).T, config.pt1_jcs_j8]) +
(-1) * multi_dot([A(config.P_rbs_l5).T, config.R_rbs_l5]))
self.ubar_rbs_l6_jcs_j8 = (
multi_dot([A(config.P_rbs_l6).T, config.pt1_jcs_j8]) +
(-1) * multi_dot([A(config.P_rbs_l6).T, config.R_rbs_l6]))
self.Mbar_rbs_l6_jcs_j9 = multi_dot(
[A(config.P_rbs_l6).T,
triad(config.ax1_jcs_j9)])
self.Mbar_rbs_l7_jcs_j9 = multi_dot(
[A(config.P_rbs_l7).T,
triad(config.ax1_jcs_j9)])
self.ubar_rbs_l6_jcs_j9 = (
multi_dot([A(config.P_rbs_l6).T, config.pt1_jcs_j9]) +
(-1) * multi_dot([A(config.P_rbs_l6).T, config.R_rbs_l6]))
self.ubar_rbs_l7_jcs_j9 = (
multi_dot([A(config.P_rbs_l7).T, config.pt1_jcs_j9]) +
(-1) * multi_dot([A(config.P_rbs_l7).T, config.R_rbs_l7]))
self.Mbar_rbs_l7_jcs_j10 = multi_dot(
[A(config.P_rbs_l7).T,
triad(config.ax1_jcs_j10)])
self.Mbar_ground_jcs_j10 = multi_dot(
[A(self.P_ground).T, triad(config.ax1_jcs_j10)])
self.ubar_rbs_l7_jcs_j10 = (
multi_dot([A(config.P_rbs_l7).T, config.pt1_jcs_j10]) +
(-1) * multi_dot([A(config.P_rbs_l7).T, config.R_rbs_l7]))
self.ubar_ground_jcs_j10 = (
multi_dot([A(self.P_ground).T, config.pt1_jcs_j10]) +
(-1) * multi_dot([A(self.P_ground).T, self.R_ground]))
self.Mbar_rbs_l7_jcs_j11 = multi_dot(
[A(config.P_rbs_l7).T,
triad(config.ax1_jcs_j11)])
self.Mbar_rbs_l8_jcs_j11 = multi_dot(
[A(config.P_rbs_l8).T,
triad(config.ax1_jcs_j11)])
self.ubar_rbs_l7_jcs_j11 = (
multi_dot([A(config.P_rbs_l7).T, config.pt1_jcs_j11]) +
(-1) * multi_dot([A(config.P_rbs_l7).T, config.R_rbs_l7]))
self.ubar_rbs_l8_jcs_j11 = (
multi_dot([A(config.P_rbs_l8).T, config.pt1_jcs_j11]) +
(-1) * multi_dot([A(config.P_rbs_l8).T, config.R_rbs_l8]))
self.Mbar_rbs_l8_jcs_j12 = multi_dot(
[A(config.P_rbs_l8).T,
triad(config.ax1_jcs_j12)])
self.Mbar_rbs_l9_jcs_j12 = multi_dot(
[A(config.P_rbs_l9).T,
triad(config.ax1_jcs_j12)])
self.ubar_rbs_l8_jcs_j12 = (
multi_dot([A(config.P_rbs_l8).T, config.pt1_jcs_j12]) +
(-1) * multi_dot([A(config.P_rbs_l8).T, config.R_rbs_l8]))
self.ubar_rbs_l9_jcs_j12 = (
multi_dot([A(config.P_rbs_l9).T, config.pt1_jcs_j12]) +
(-1) * multi_dot([A(config.P_rbs_l9).T, config.R_rbs_l9]))
self.Mbar_rbs_l9_jcs_j13 = multi_dot(
[A(config.P_rbs_l9).T,
triad(config.ax1_jcs_j13)])
self.Mbar_ground_jcs_j13 = multi_dot(
[A(self.P_ground).T, triad(config.ax1_jcs_j13)])
self.ubar_rbs_l9_jcs_j13 = (
multi_dot([A(config.P_rbs_l9).T, config.pt1_jcs_j13]) +
(-1) * multi_dot([A(config.P_rbs_l9).T, config.R_rbs_l9]))
self.ubar_ground_jcs_j13 = (
multi_dot([A(self.P_ground).T, config.pt1_jcs_j13]) +
(-1) * multi_dot([A(self.P_ground).T, self.R_ground]))
def eval_constants(self):
config = self.config
self.F_rbs_coupler_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbs_coupler]], dtype=np.float64)
self.F_rbr_rocker_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbr_rocker]], dtype=np.float64)
self.F_rbl_rocker_gravity = np.array(
[[0], [0], [-9810.0 * config.m_rbl_rocker]], dtype=np.float64)
self.Mbar_rbr_rocker_jcr_rocker_chassis = multi_dot(
[A(config.P_rbr_rocker).T,
triad(config.ax1_jcr_rocker_chassis)])
self.Mbar_vbs_chassis_jcr_rocker_chassis = multi_dot(
[A(config.P_vbs_chassis).T,
triad(config.ax1_jcr_rocker_chassis)])
self.ubar_rbr_rocker_jcr_rocker_chassis = (
multi_dot(
[A(config.P_rbr_rocker).T, config.pt1_jcr_rocker_chassis]) +
(-1) * multi_dot([A(config.P_rbr_rocker).T, config.R_rbr_rocker]))
self.ubar_vbs_chassis_jcr_rocker_chassis = (
multi_dot(
[A(config.P_vbs_chassis).T, config.pt1_jcr_rocker_chassis]) +
(-1) *
multi_dot([A(config.P_vbs_chassis).T, config.R_vbs_chassis]))
self.Mbar_rbr_rocker_jcs_rocker_uni = multi_dot(
[A(config.P_rbr_rocker).T,
triad(config.ax1_jcs_rocker_uni)])
self.Mbar_rbs_coupler_jcs_rocker_uni = multi_dot([
A(config.P_rbs_coupler).T,
triad(config.ax2_jcs_rocker_uni,
triad(config.ax1_jcs_rocker_uni)[0:3, 1:2])
])
self.ubar_rbr_rocker_jcs_rocker_uni = (
multi_dot([A(config.P_rbr_rocker).T, config.pt1_jcs_rocker_uni]) +
(-1) * multi_dot([A(config.P_rbr_rocker).T, config.R_rbr_rocker]))
self.ubar_rbs_coupler_jcs_rocker_uni = (
multi_dot([A(config.P_rbs_coupler).T, config.pt1_jcs_rocker_uni]) +
(-1) *
multi_dot([A(config.P_rbs_coupler).T, config.R_rbs_coupler]))
self.Mbar_rbl_rocker_jcl_rocker_chassis = multi_dot(
[A(config.P_rbl_rocker).T,
triad(config.ax1_jcl_rocker_chassis)])
self.Mbar_vbs_chassis_jcl_rocker_chassis = multi_dot(
[A(config.P_vbs_chassis).T,
triad(config.ax1_jcl_rocker_chassis)])
self.ubar_rbl_rocker_jcl_rocker_chassis = (
multi_dot(
[A(config.P_rbl_rocker).T, config.pt1_jcl_rocker_chassis]) +
(-1) * multi_dot([A(config.P_rbl_rocker).T, config.R_rbl_rocker]))
self.ubar_vbs_chassis_jcl_rocker_chassis = (
multi_dot(
[A(config.P_vbs_chassis).T, config.pt1_jcl_rocker_chassis]) +
(-1) *
multi_dot([A(config.P_vbs_chassis).T, config.R_vbs_chassis]))
self.Mbar_rbl_rocker_jcl_rocker_chassis = multi_dot(
[A(config.P_rbl_rocker).T,
triad(config.ax1_jcl_rocker_chassis)])
self.Mbar_vbs_chassis_jcl_rocker_chassis = multi_dot(
[A(config.P_vbs_chassis).T,
triad(config.ax1_jcl_rocker_chassis)])
self.Mbar_rbl_rocker_jcs_rocker_sph = multi_dot(
[A(config.P_rbl_rocker).T,
triad(config.ax1_jcs_rocker_sph)])
self.Mbar_rbs_coupler_jcs_rocker_sph = multi_dot(
[A(config.P_rbs_coupler).T,
triad(config.ax1_jcs_rocker_sph)])
self.ubar_rbl_rocker_jcs_rocker_sph = (
multi_dot([A(config.P_rbl_rocker).T, config.pt1_jcs_rocker_sph]) +
(-1) * multi_dot([A(config.P_rbl_rocker).T, config.R_rbl_rocker]))
self.ubar_rbs_coupler_jcs_rocker_sph = (
multi_dot([A(config.P_rbs_coupler).T, config.pt1_jcs_rocker_sph]) +
(-1) *
multi_dot([A(config.P_rbs_coupler).T, config.R_rbs_coupler]))
def eval_constants(self):
config = self.config
self.F_rbs_input_shaft_gravity = np.array([[0], [0], [-9810.0*config.m_rbs_input_shaft]], dtype=np.float64)
self.F_rbs_connect_shaft_gravity = np.array([[0], [0], [-9810.0*config.m_rbs_connect_shaft]], dtype=np.float64)
self.F_rbs_output_shaft_gravity = np.array([[0], [0], [-9810.0*config.m_rbs_output_shaft]], dtype=np.float64)
self.Mbar_rbs_input_shaft_jcs_input_bearing = multi_dot([A(config.P_rbs_input_shaft).T,triad(config.ax1_jcs_input_bearing)])
self.Mbar_vbs_chassis_jcs_input_bearing = multi_dot([A(config.P_vbs_chassis).T,triad(config.ax1_jcs_input_bearing)])
self.ubar_rbs_input_shaft_jcs_input_bearing = (multi_dot([A(config.P_rbs_input_shaft).T,config.pt1_jcs_input_bearing]) + -1*multi_dot([A(config.P_rbs_input_shaft).T,config.R_rbs_input_shaft]))
self.ubar_vbs_chassis_jcs_input_bearing = (multi_dot([A(config.P_vbs_chassis).T,config.pt1_jcs_input_bearing]) + -1*multi_dot([A(config.P_vbs_chassis).T,config.R_vbs_chassis]))
self.Mbar_rbs_input_shaft_jcs_input_bearing = multi_dot([A(config.P_rbs_input_shaft).T,triad(config.ax1_jcs_input_bearing)])
self.Mbar_vbs_chassis_jcs_input_bearing = multi_dot([A(config.P_vbs_chassis).T,triad(config.ax1_jcs_input_bearing)])
self.Mbar_rbs_input_shaft_jcs_input_connect = multi_dot([A(config.P_rbs_input_shaft).T,triad(config.ax1_jcs_input_connect)])
self.Mbar_rbs_connect_shaft_jcs_input_connect = multi_dot([A(config.P_rbs_connect_shaft).T,triad(config.ax2_jcs_input_connect,triad(config.ax1_jcs_input_connect)[0:3,1:2])])
self.ubar_rbs_input_shaft_jcs_input_connect = (multi_dot([A(config.P_rbs_input_shaft).T,config.pt1_jcs_input_connect]) + -1*multi_dot([A(config.P_rbs_input_shaft).T,config.R_rbs_input_shaft]))
self.ubar_rbs_connect_shaft_jcs_input_connect = (multi_dot([A(config.P_rbs_connect_shaft).T,config.pt1_jcs_input_connect]) + -1*multi_dot([A(config.P_rbs_connect_shaft).T,config.R_rbs_connect_shaft]))
self.Mbar_rbs_output_shaft_jcs_output_connect = multi_dot([A(config.P_rbs_output_shaft).T,triad(config.ax1_jcs_output_connect)])
self.Mbar_rbs_connect_shaft_jcs_output_connect = multi_dot([A(config.P_rbs_connect_shaft).T,triad(config.ax2_jcs_output_connect,triad(config.ax1_jcs_output_connect)[0:3,1:2])])
self.ubar_rbs_output_shaft_jcs_output_connect = (multi_dot([A(config.P_rbs_output_shaft).T,config.pt1_jcs_output_connect]) + -1*multi_dot([A(config.P_rbs_output_shaft).T,config.R_rbs_output_shaft]))
self.ubar_rbs_connect_shaft_jcs_output_connect = (multi_dot([A(config.P_rbs_connect_shaft).T,config.pt1_jcs_output_connect]) + -1*multi_dot([A(config.P_rbs_connect_shaft).T,config.R_rbs_connect_shaft]))
self.Mbar_rbs_output_shaft_jcs_output_bearing = multi_dot([A(config.P_rbs_output_shaft).T,triad(config.ax1_jcs_output_bearing)])
self.Mbar_vbs_chassis_jcs_output_bearing = multi_dot([A(config.P_vbs_chassis).T,triad(config.ax1_jcs_output_bearing)])
self.ubar_rbs_output_shaft_jcs_output_bearing = (multi_dot([A(config.P_rbs_output_shaft).T,config.pt1_jcs_output_bearing]) + -1*multi_dot([A(config.P_rbs_output_shaft).T,config.R_rbs_output_shaft]))
self.ubar_vbs_chassis_jcs_output_bearing = (multi_dot([A(config.P_vbs_chassis).T,config.pt1_jcs_output_bearing]) + -1*multi_dot([A(config.P_vbs_chassis).T,config.R_vbs_chassis]))
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)])