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()
# we need to check the forcing?
ua = biModel._auxiliarySpeeds
uad = biModel._auxiliarySpeedsDerivative
ua_zero = {uai:0 for uai in ua}
uad_zero = {uadi:0 for uadi in uad}
q = biModel._coordinatesInde + biModel._coordinatesDe
u = biModel._speedsInde + biModel._speedsDe
T4 = biModel._inputForces[0]
dummy_symbols = [Dummy() for i in q+u]
symbols_dict = dict(zip(q+u, dummy_symbols))
symbols_dict.update({T4: 0.})
v = symbols('v')
# Parameters
bp = benchmark_parameters()
mp = benchmark_to_moore(bp)
para_dict = {}
for key, value in mp.items():
para_dict.update(dict(zip([symbols(key)], [value])))
# Substitution
MM = left.subs(symbols_dict).subs(para_dict).subs(uad_zero).subs(ua_zero)
For = right.subs(symbols_dict).subs(para_dict).subs(uad_zero).subs(ua_zero)
# Built quick functions for mm and forcing
# Built a function of derivs for q_dot and u_dot
# * here is to convert variables of array (maybe or list) to a tuple for input
# into the lambdify
mm = lambdify(dummy_symbols, MM)
fo = lambdify(dummy_symbols, For)
def derivs(y, t):
Jason's DynamicistToolKit package.
"""
from bicycle import (benchmark_parameters, benchmark_to_moore,
lambda_from_132, pitch_from_roll_and_steer,
basu_table_one_input, basu_to_stefen_input,
basu_table_one_output, basu_to_stefen_output
)
from numpy import pi
#from dtk import bicycle as bicycle
# Parameters test
bp = benchmark_parameters() #assertation bicycle.benchmark_parameters
mp = benchmark_to_moore(bp) #assertation bicycle.benchmark_to_moore
# Pitch test
# Assertation bicycle.lambda_from_abc
# Assertation bicycle.pitch_from_roll_and_steer(lean, steer, mp['rf'], mp['rr'],
# mp['d1'], mp['d2'], mp['d3'])
lam = lambda_from_132(mp['rf'], mp['rr'], mp['d1'], mp['d3'], mp['d2'])
lean = 0.0; steer = 0.0
pitch = pitch_from_roll_and_steer(lean, steer, mp['rf'], mp['rr'],
mp['d1'], mp['d2'], mp['d3'])
# Basu test
# Assertation bicycle.basu_to_moore_input
basu_input = basu_table_one_input()