def __init__(self, lean, steer, bicycleParameters = None):
        """Given lean and steer angles for a steady-turning configuration.

        Parameter
        ---------
        lean, steer: float
            Angles for defining a steady turning configuration.
        bicycleParameters: a dictionary
            The parameters is the dictionary is expressed in Moore set, not 
            benchmark set.
            Default parameters are benchmark parameters.

        """

        # Bicycle model: biModel
        # forceFull matrix
        # conForceNoncontri
        # contact points relative position in a list, but from ways to obtain it
        # individual centers of mass of four rigid bodies
        biModel = mo.BicycleModel()
        self._biModel = biModel

        self._mmFull = biModel.mass_matrix_full()
        self._forcingLinA = biModel.linearized_a()
        self._forceFull = biModel.forcing_full()
        self._contactforcesOrig = biModel.contact_forces()
        self._contactPosition = (biModel._contact_posi_pq + 
                                    biModel._contact_posi_dfn)
        self._turningRadiusSym = biModel._turningRadiusSym
        self._bodies_dn_A = biModel._bodies_dn_A
        self._massSym = biModel._massSym
        self._contactforcesSym = biModel._auxiliaryForces

        # States assignment
        # Parameters
        u1, u3, u6 = biModel._speedsDe
        u2, u4, u5 = biModel._speedsInde
        T4 = biModel._inputForces[0]

        if bicycleParameters is None:
            bp = bi.benchmark_parameters()
            mp = bi.benchmark_to_moore(bp)
        else:
            mp = bicycleParameters
        self._parameters = mo.strings2symbols(mp, go2type="orsymbols")

        # Steady turning configuration:
        # Configuration: e.g. lean = pi/8;  steer = pi/4
        self._ud0s = mo.zeros_dict(biModel._speedsDerivative)
        self._u0s = mo.zeros_dict([u2, u3, u4])
        self._equilibriumSym = [u1, u5, u6, T4]

        q_dict, q_dict_d = st.configuration(lean, steer, mp)
        self._configuration = q_dict
        self._configurationDegree = q_dict_d

        # Total center of mass(including relative position of fn to dn)
        # Turning radius
        self.total_com()
        self.turning_radius()

        # Dynamic equations
        # Nonholonomic equations
        dynamic_equ = st.forcing_dynamic_equations(self._forceFull, 
                                            self._parameters, q_dict, self._u0s)
        self._dynamic = dynamic_equ

        inde_expre, inde_expre_subs = st.de_by_inde(biModel._nonholonomic, 
                                            q_dict, self._parameters, self._u0s)
        self._nonho = inde_expre
        self._nonhoSubs = inde_expre_subs

        # Combination (Substitution)
        dynamic_nonho_equ = st.dynamic_nonholonomic_equations(inde_expre_subs, 
                                                                dynamic_equ)
        self._dynamicnonho = dynamic_nonho_equ

        # Equilibrium values
        self.equi_cal()
Пример #2
0
biModel = BicycleModel()

forceFull = biModel.forcing_full()

# Parameters and states values:
# Parameters
bp = benchmark_parameters()
mp = benchmark_to_moore(bp)
para_dict = strings2symbols(mp, go2type="orsymbols")

# Test reference configuration
# u2-leanrate, u3-pitchrate, u4-steerrate
u_dict = strings2symbols(biModel._speeds[1:4], go2type="dysymbols")

lean = 0.0; steer = 0.0

# Steady turning:
# Configuration
# Dynamic equations: dynamic_equ
# Nonholonomic equations: inde_expression, inde_expression_subs
# Substitution: dynamic_nonho_equ
q_dict, q_dict_d = configuration(lean, steer, mp)

dynamic_equ = forcing_dynamic_equations(forceFull, 
                                                para_dict, q_dict, u_dict)

inde_expression, inde_expression_subs = de_by_inde(biModel._nonholonomic, 
                                                q_dict, para_dict, u_dict)

dynamic_nonho_equ = dynamic_nonholonomic_equations(inde_expression_subs, dynamic_equ)