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))