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)
    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()
B8_num = B8.subs(T4_zero).subs(input_states_dict).subs(para_dict)

B47 = B4_num.col_join(B7_num)
B58 = B5_num.col_join(B8_num)
output_cal = -B47.inv() * B58
"""


# Steady turning
t5 = time()
# Configuration: lean, steer, pitch
# pitch angle calculated here is to import the function of configuration in the
# steadyturning.py, since the equation it uses to calculate the pitch angle is
# the same as the one from f_c configuration constraint.
lean = pi/8; steer = pi/4
q_conf, q_conf_de = configuration(lean, steer, mp)

# Equilibrium values: u5, T4, u1, u6
u_dep_num_st = u_dep.subs(para_dict).subs(q_conf).subs(steady_conditions)
u_dep_dict_num_st = {udei : u_dep_num_sti[0] for udei, u_dep_num_sti 
                  in zip(u[3:], u_dep_num_st.tolist())}

B8_num_st = B8.subs(steady_conditions).subs(q_conf).subs(para_dict)\
          .subs(qd_kd).subs(u_dep_dict_num_st)

u5sqr_value = sym.solve(B8_num_st[0], u5**2)
if u5sqr_value == []:
    raise ValueError("\nOops! Rear wheel rate in configuration {0} cannot be \
solved. Please select valid configuration according to the plot from <General \
steady turning of a benchmark bicycle model> by Luke. Good luck!\n"\
.format(q_conf))