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