Esempio n. 1
0
    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])),)
Esempio n. 2
0
    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])),
        )
Esempio n. 3
0
    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])),
        )
Esempio n. 4
0
    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]))
Esempio n. 5
0
    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}
Esempio n. 7
0
    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)]),)
Esempio n. 8
0
    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])),
        )
Esempio n. 9
0
    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])),
        )
Esempio n. 11
0
    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,
        )
Esempio n. 12
0
    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)])
Esempio n. 13
0
    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,
        )
Esempio n. 14
0
    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])),
        )
Esempio n. 15
0
    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]),
        )
Esempio n. 16
0
    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]))
Esempio n. 17
0
    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])),)
Esempio n. 18
0
    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]),
        )
Esempio n. 19
0
    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]),
        )
Esempio n. 20
0
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
Esempio n. 21
0
    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]))
Esempio n. 22
0
    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])),)
Esempio n. 23
0
    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])),
        )
Esempio n. 24
0
    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])),
        )
Esempio n. 25
0
    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}
Esempio n. 26
0
    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])),)
Esempio n. 27
0
    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])),)
Esempio n. 28
0
    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]),)
Esempio n. 29
0
    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}
Esempio n. 30
0
    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