From 34b20c7d99d2b07a14fc4628e65385a6e8d220f9 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Wed, 2 Oct 2024 10:42:13 +0200 Subject: [PATCH 01/15] Started work on a new model that implements full state feedback on the Carvallo-Whipple model. --- bicycleparameters/models.py | 109 ++++++++++++++++++++++++++++++++++++ docs/examples.rst | 58 +++++++++++++++++++ 2 files changed, 167 insertions(+) diff --git a/bicycleparameters/models.py b/bicycleparameters/models.py index 7848f01..5ee62c7 100644 --- a/bicycleparameters/models.py +++ b/bicycleparameters/models.py @@ -944,3 +944,112 @@ def plot_mode_simulations(self, times, **parameter_overrides): axes[3, 1].set_xlabel('Time [s]') return axes + + +class Meijaard2007WithFeedbackModel(Meijaard2007Model): + """ + + The states are:: + + x = |roll angle | = |phi | + |steer angle| |del | + |roll rate | |phid| + |steer rate | |deld| + + The inputs are:: + + u = |roll torque | = |Tphi| + |steer torque| |Tdelta| + + Applying full state feedback gives this controller:: + + u = -K*x = -|kphi_phi, kphi_del, kphi_phid, kphi_deld|*|phi | + |kdel_phi, kdel_del, kdel_phid, kdel_deld| |delta| + |phid | + |deld | + + """ + + def form_state_space_matrices(self, **parameter_overrides): + """Returns the A and B matrices for the Whipple-Carvallo model + linearized about the upright constant velocity configuration with a + full state feedback steer controller. + + Returns + ======= + A : ndarray, shape(4,4) or shape(n,4,4) + The state matrix. + B : ndarray, shape(4,2) or shape(n,4,2) + The input matrix. + + Notes + ===== + A, B, and K describe the model in state space form: + + x' = (A - B*K)*x + B*u + + where:: + + x = |phi | = |roll angle | + |delta | |steer angle| + |phidot | |roll rate | + |deltadot| |steer rate | + + K = |0 0 0 0 | + |kphi kdelta kphidot kdeltadot| + + u = |Tphi | = |roll torque | + |Tdelta| |steer torque| + + """ + gain_names = ['kphi_phi', 'kphi_del', 'kphi_phid', 'kphi_deld', + 'kdel_phi', 'kdel_del', 'kdel_phid', 'kdel_deld'] + + par, arr_keys, arr_len = self._parse_parameter_overrides( + **parameter_overrides) + + # g, v, and the contoller gains are not used in the computation of M, + # C1, K0, K2. + + M, C1, K0, K2 = self.form_reduced_canonical_matrices( + **parameter_overrides) + + # steer controller gains, 2x4, no roll control + if any(k in gain_names for k in arr_keys): + # if one of the gains is an array, create a set of gain matrices + # where that single gain varies across the set + K = np.array([ + [par[p][0] if p in arr_keys else par[p] for p in gain_names[:4]], + [par[p][0] if p in arr_keys else par[p] for p in gain_names[4:]] + ]) + # K is now shape(n, 2, 4) + K = np.tile(K, (arr_len, 1, 1)) + for k in arr_keys: + if k in gain_names[:4]: + K[:, 0, gain_names[:4].index(k)] = par[k] + if k in gain_names[4:]: + K[:, 1, gain_names[4:].index(k)] = par[k] + else: # gains are not an array + K = np.array([[par[p] for p in gain_names[:4]], + [par[p] for p in gain_names[4:]]]) + + if arr_keys: + A = np.zeros((arr_len, 4, 4)) + B = np.zeros((arr_len, 4, 2)) + for i in range(arr_len): + Mi = M[i] if M.ndim == 3 else M + C1i = C1[i] if C1.ndim == 3 else C1 + K0i = K0[i] if K0.ndim == 3 else K0 + K2i = K2[i] if K2.ndim == 3 else K2 + vi = par['v'] if np.isscalar(par['v']) else par['v'][i] + gi = par['g'] if np.isscalar(par['g']) else par['g'][i] + Ki = K[i] if K.ndim == 3 else K + Ai, Bi = ab_matrix(Mi, C1i, K0i, K2i, vi, gi) + A[i] = Ai - Bi@Ki + B[i] = Bi + else: # scalar parameters + A, B = ab_matrix(M, C1, K0, K2, par['v'], par['g']) + A = A - B@K + B = B + + return A, B diff --git a/docs/examples.rst b/docs/examples.rst index e54fc1a..9f7fb3b 100644 --- a/docs/examples.rst +++ b/docs/examples.rst @@ -683,3 +683,61 @@ derivative controller on roll: model.plot_simulation(times, x0, input_func=lambda t, x: np.array([0.0, 50.0*x[2]]), v=2.0) + +.. plot:: + :include-source: True + :context: close-figs + + from scipy.linalg import solve_continuous_are + + speeds = np.linspace(0.1, 10.1, num=1001) + + As, Bs = model.form_state_space_matrices(v=speeds) + Ks = np.empty((len(speeds), 2, 4)) + Q = np.eye(4) + R = np.eye(1) + + for i, (Ai, Bi) in enumerate(zip(As, Bs)): + S = solve_continuous_are(Ai, Bi[:, 1:2], Q, R) # steer torque control + Ks[i] = (np.linalg.inv(R) @ Bi[:, 1:2].T @ S).squeeze() + + from bicycleparameters.models import Meijaard2007WithFeedbackModel + + gain_names = ['kphi_phi', 'kphi_del', 'kphi_phid', 'kphi_deld', + 'kdel_phi', 'kdel_del', 'kdel_phid', 'kdel_deld'] + for name in gain_names: + par[name] = 0.0 + + model = Meijaard2007WithFeedbackModel(par_set) + + ax = model.plot_eigenvalue_parts(v=speeds, + kphi_phi=Ks[:, 0, 0], + kphi_del=Ks[:, 0, 1], + kphi_phid=Ks[:, 0, 2], + kphi_deld=Ks[:, 0, 3], + kdel_phi=Ks[:, 1, 0], + kdel_del=Ks[:, 1, 1], + kdel_phid=Ks[:, 1, 2], + kdel_deld=Ks[:, 1, 3], + colors=['C0', 'C0', 'C1', 'C2'], + show_stable_regions=False, + hide_zeros=True) + ax.set_ylim((-10.0, 10.0)) + +.. plot:: + :include-source: True + :context: close-figs + + x0 = np.deg2rad([5.0, -3.0, 0.0, 0.0]) + + ax = model.plot_simulation(times, x0, + v=speeds[90], + kphi_phi=Ks[90, 0, 0], + kphi_del=Ks[90, 0, 1], + kphi_phid=Ks[90, 0, 2], + kphi_deld=Ks[90, 0, 3], + kdel_phi=Ks[90, 1, 0], + kdel_del=Ks[90, 1, 1], + kdel_phid=Ks[90, 1, 2], + kdel_deld=Ks[90, 1, 3]) + ax[0].set_title('$v$ = {} m/s'.format(speeds[90])) From c7652e63d73167380d7010da16ea1ef785bc26cd Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Wed, 2 Oct 2024 10:55:24 +0200 Subject: [PATCH 02/15] Add a roll torque pulse to the simulatino. --- docs/examples.rst | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/docs/examples.rst b/docs/examples.rst index 9f7fb3b..92f2ed9 100644 --- a/docs/examples.rst +++ b/docs/examples.rst @@ -720,7 +720,6 @@ derivative controller on roll: kdel_phid=Ks[:, 1, 2], kdel_deld=Ks[:, 1, 3], colors=['C0', 'C0', 'C1', 'C2'], - show_stable_regions=False, hide_zeros=True) ax.set_ylim((-10.0, 10.0)) @@ -739,5 +738,8 @@ derivative controller on roll: kdel_phi=Ks[90, 1, 0], kdel_del=Ks[90, 1, 1], kdel_phid=Ks[90, 1, 2], - kdel_deld=Ks[90, 1, 3]) + kdel_deld=Ks[90, 1, 3], + input_func=lambda t, x: np.array([100.0, 0.0]) + if (t > 2.5 and t < 2.6) + else np.zeros(2)) ax[0].set_title('$v$ = {} m/s'.format(speeds[90])) From 3118c4beb4a1c61647b188e2226bd80976f39fb1 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Wed, 2 Oct 2024 15:40:01 +0200 Subject: [PATCH 03/15] Plot gains versus speed. --- docs/examples.rst | 27 ++++++++++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/docs/examples.rst b/docs/examples.rst index 92f2ed9..d0e8c93 100644 --- a/docs/examples.rst +++ b/docs/examples.rst @@ -684,23 +684,48 @@ derivative controller on roll: input_func=lambda t, x: np.array([0.0, 50.0*x[2]]), v=2.0) +Feedback Control +---------------- + +We have a :py:class:`~bicycleparameters.models.Meijaard2007WithFeedbackModel` +that applies negative full state feedback to the +:py:class:`~bicycleparameters.models.Meijaard2007Model` using eight feedback +gains. These feedback gains can be chosen with a variety of methods. One +method is to create gain scheduling with respect to speed using LQR optimal +control. The 4 x 2 gain matrix can be found by solving the continous Ricatti +equation. If the system is controllable, this guarantees a stable closed loop +system. + .. plot:: :include-source: True :context: close-figs from scipy.linalg import solve_continuous_are - speeds = np.linspace(0.1, 10.1, num=1001) + speeds = np.linspace(1.0, 10.0, num=1001) As, Bs = model.form_state_space_matrices(v=speeds) Ks = np.empty((len(speeds), 2, 4)) Q = np.eye(4) + Q = np.diag([10.0, 1.0, 10.0, 1.0]) R = np.eye(1) for i, (Ai, Bi) in enumerate(zip(As, Bs)): S = solve_continuous_are(Ai, Bi[:, 1:2], Q, R) # steer torque control Ks[i] = (np.linalg.inv(R) @ Bi[:, 1:2].T @ S).squeeze() + fig, axes = plt.subplots(2, 4, sharex=True, layout='constrained') + for i, row in enumerate(axes): + for j, col in enumerate(row): + col.plot(speeds, Ks[:, i, j]) + +Now create the feedback model and use the computed gains to check for closed +loop stability: + +.. plot:: + :include-source: True + :context: close-figs + from bicycleparameters.models import Meijaard2007WithFeedbackModel gain_names = ['kphi_phi', 'kphi_del', 'kphi_phid', 'kphi_deld', From 492dcada56aa6319543473eb6b3bdd0829c20e3e Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Wed, 23 Oct 2024 10:20:25 +0200 Subject: [PATCH 04/15] Added parameterset for model with feedback and corrected gain calcs and plots. --- bicycleparameters/models.py | 17 +++-- bicycleparameters/parameter_sets.py | 114 ++++++++++++++++++++++++++++ docs/examples.rst | 57 ++++++++------ 3 files changed, 156 insertions(+), 32 deletions(-) diff --git a/bicycleparameters/models.py b/bicycleparameters/models.py index 5ee62c7..3e74d3e 100644 --- a/bicycleparameters/models.py +++ b/bicycleparameters/models.py @@ -969,6 +969,9 @@ class Meijaard2007WithFeedbackModel(Meijaard2007Model): |deld | """ + def __init__(self, parameter_set): + self.parameter_set = parameter_set.to_parameterization( + 'Meijaard2007WithFeedback') def form_state_space_matrices(self, **parameter_overrides): """Returns the A and B matrices for the Whipple-Carvallo model @@ -990,20 +993,20 @@ def form_state_space_matrices(self, **parameter_overrides): where:: - x = |phi | = |roll angle | - |delta | |steer angle| - |phidot | |roll rate | - |deltadot| |steer rate | + x = |phi | = |roll angle | + |delta | |steer angle| + |phid | |roll rate | + |deld | |steer rate | K = |0 0 0 0 | - |kphi kdelta kphidot kdeltadot| + |kTdel_phi kTdel_del kphidot kdeltadot| u = |Tphi | = |roll torque | |Tdelta| |steer torque| """ - gain_names = ['kphi_phi', 'kphi_del', 'kphi_phid', 'kphi_deld', - 'kdel_phi', 'kdel_del', 'kdel_phid', 'kdel_deld'] + gain_names = ['kTphi_phi', 'kTphi_del', 'kTphi_phid', 'kTphi_deld', + 'kTdel_phi', 'kTdel_del', 'kTdel_phid', 'kTdel_deld'] par, arr_keys, arr_len = self._parse_parameter_overrides( **parameter_overrides) diff --git a/bicycleparameters/parameter_sets.py b/bicycleparameters/parameter_sets.py index 500c8b2..adb40cb 100644 --- a/bicycleparameters/parameter_sets.py +++ b/bicycleparameters/parameter_sets.py @@ -806,6 +806,120 @@ def plot_all(self, ax=None): return ax +class Meijaard2007WithFeedbackParameterSet(Meijaard2007ParameterSet): + """Represents the parameters of the benchmark bicycle presented in + [Meijaard2007]_ and with full state feedback control gains. + + The four bodies are: + + - B: rear frame + rigid rider + - F: front wheel + - H: front frame (fork & handlebars) + - R: rear wheel + + Parameters + ========== + parameters : dictionary + A dictionary mapping variable names to values that contains the + following keys: + + - ``IBxx`` : x moment of inertia of the frame/rider [kg*m**2] + - ``IBxz`` : xz product of inertia of the frame/rider [kg*m**2] + - ``IBzz`` : z moment of inertia of the frame/rider [kg*m**2] + - ``IFxx`` : x moment of inertia of the front wheel [kg*m**2] + - ``IFyy`` : y moment of inertia of the front wheel [kg*m**2] + - ``IHxx`` : x moment of inertia of the handlebar/fork [kg*m**2] + - ``IHxz`` : xz product of inertia of the handlebar/fork [kg*m**2] + - ``IHzz`` : z moment of inertia of the handlebar/fork [kg*m**2] + - ``IRxx`` : x moment of inertia of the rear wheel [kg*m**2] + - ``IRyy`` : y moment of inertia of the rear wheel [kg*m**2] + - ``c`` : trail [m] + - ``g`` : acceleration due to gravity [m/s**2] + - ``kTdel_del`` : steer torque feedback gain from steer angle [N*m/rad] + - ``kTdel_deld`` : steer torque feedback gain from steer rate [N*m*s/rad] + - ``kTdel_phi`` : steer torque feedback gain from roll angle [N*m/rad] + - ``kTdel_phid`` : steer torque feedback gain from roll rate [N*m*s/rad] + - ``kTphi_del`` : roll torque feedback gain from steer angle [N*m/rad] + - ``kTphi_deld`` : roll torque feedback gain from steer rate [N*m*s/rad] + - ``kTphi_phi`` : roll torque feedback gain from roll angle [N*m/rad] + - ``kTphi_phid`` : roll torque feedback gain from roll rate [N*m*s/rad] + - ``lam`` : steer axis tilt [rad] + - ``mB`` : frame/rider mass [kg] + - ``mF`` : front wheel mass [kg] + - ``mH`` : handlebar/fork assembly mass [kg] + - ``mR`` : rear wheel mass [kg] + - ``rF`` : front wheel radius [m] + - ``rR`` : rear wheel radius [m] + - ``w`` : wheelbase [m] + - ``xB`` : x distance to the frame/rider center of mass [m] + - ``xH`` : x distance to the frame/rider center of mass [m] + - ``zB`` : z distance to the frame/rider center of mass [m] + - ``zH`` : z distance to the frame/rider center of mass [m] + + includes_rider : boolean + True if body B is the combined rear frame and rider in terms of + mass and inertia values. + + Attributes + ========== + par_strings : dictionary + Maps ASCII strings to their LaTeX string. + body_labels : list of strings + Single capital letters that correspond to the four rigid bodies in the + model. + + References + ========== + + .. [Meijaard2007] Meijaard J.P, Papadopoulos Jim M, Ruina Andy and Schwab + A.L, 2007, Linearized dynamics equations for the balance and steer of a + bicycle: a benchmark and review, Proc. R. Soc. A., 463:1955–1982 + http://doi.org/10.1098/rspa.2007.1857 + + """ + + # maps "Python" string to LaTeX version + par_strings = { + 'IBxx': r'I_{Bxx}', + 'IBxz': r'I_{Bxz}', + 'IByy': r'I_{Byy}', + 'IBzz': r'I_{Bzz}', + 'IFxx': r'I_{Fxx}', + 'IFyy': r'I_{Fyy}', + 'IHxx': r'I_{Hxx}', + 'IHxz': r'I_{Hxz}', + 'IHyy': r'I_{Hyy}', + 'IHzz': r'I_{Hzz}', + 'IRxx': r'I_{Rxx}', + 'IRyy': r'I_{Ryy}', + 'c': r'c', + 'g': r'g', + 'kTdel_del': r'k_{T_{\delta}\delta}', + 'kTdel_deld': r'k_{T_{\delta}\dot{\delta}}', + 'kTdel_phi': r'k_{T_{\delta}\phi}', + 'kTdel_phid': r'k_{T_{\delta}\dot{\phi}}', + 'kTphi_del': r'k_{T_{\phi}\delta}', + 'kTphi_deld': r'k_{T_{\phi}\dot{\delta}}', + 'kTphi_phi': r'k_{T_{\phi}\phi}', + 'kTphi_phid': r'k_{T_{\phi}\dot{\phi}}', + 'lam': r'\lambda', + 'mB': r'm_B', + 'mF': r'm_F', + 'mH': r'm_H', + 'mR': r'm_R', + 'rF': r'r_F', + 'rR': r'r_R', + 'v': r'v', + 'w': r'w', + 'xB': r'x_B', + 'xH': r'x_H', + 'zB': r'z_B', + 'zH': r'z_H', + } + + body_labels = ['B', 'F', 'H', 'R'] + + class Moore2019ParameterSet(ParameterSet): """Represents the parameters of the bicycle parameterization presented in [1]_. diff --git a/docs/examples.rst b/docs/examples.rst index d0e8c93..8e1b315 100644 --- a/docs/examples.rst +++ b/docs/examples.rst @@ -700,24 +700,36 @@ system. :include-source: True :context: close-figs + from bicycleparameters.parameter_sets import Meijaard2007WithFeedbackParameterSet from scipy.linalg import solve_continuous_are + gain_names = [['kTphi_phi', 'kTphi_del', 'kTphi_phid', 'kTphi_deld'], + ['kTdel_phi', 'kTdel_del', 'kTdel_phid', 'kTdel_deld']] + for row in gain_names: + for name in row: + par[name] = 0.0 + + par_set = Meijaard2007WithFeedbackParameterSet(par, True) + speeds = np.linspace(1.0, 10.0, num=1001) As, Bs = model.form_state_space_matrices(v=speeds) - Ks = np.empty((len(speeds), 2, 4)) + Ks = np.zeros((len(speeds), 2, 4)) Q = np.eye(4) Q = np.diag([10.0, 1.0, 10.0, 1.0]) R = np.eye(1) for i, (Ai, Bi) in enumerate(zip(As, Bs)): S = solve_continuous_are(Ai, Bi[:, 1:2], Q, R) # steer torque control - Ks[i] = (np.linalg.inv(R) @ Bi[:, 1:2].T @ S).squeeze() + Ks[i, 1, :] = (np.linalg.inv(R) @ Bi[:, 1:2].T @ S).squeeze() fig, axes = plt.subplots(2, 4, sharex=True, layout='constrained') - for i, row in enumerate(axes): - for j, col in enumerate(row): + for i, (row, name_row) in enumerate(zip(axes, gain_names)): + for j, (col, name) in enumerate(zip(row, name_row)): col.plot(speeds, Ks[:, i, j]) + col.set_ylabel(r'${}$'.format(par_set.par_strings[name])) + for ax in row: + ax.set_label('v [m/s]') Now create the feedback model and use the computed gains to check for closed loop stability: @@ -728,22 +740,17 @@ loop stability: from bicycleparameters.models import Meijaard2007WithFeedbackModel - gain_names = ['kphi_phi', 'kphi_del', 'kphi_phid', 'kphi_deld', - 'kdel_phi', 'kdel_del', 'kdel_phid', 'kdel_deld'] - for name in gain_names: - par[name] = 0.0 - model = Meijaard2007WithFeedbackModel(par_set) ax = model.plot_eigenvalue_parts(v=speeds, - kphi_phi=Ks[:, 0, 0], - kphi_del=Ks[:, 0, 1], - kphi_phid=Ks[:, 0, 2], - kphi_deld=Ks[:, 0, 3], - kdel_phi=Ks[:, 1, 0], - kdel_del=Ks[:, 1, 1], - kdel_phid=Ks[:, 1, 2], - kdel_deld=Ks[:, 1, 3], + kTphi_phi=Ks[:, 0, 0], + kTphi_del=Ks[:, 0, 1], + kTphi_phid=Ks[:, 0, 2], + kTphi_deld=Ks[:, 0, 3], + kTdel_phi=Ks[:, 1, 0], + kTdel_del=Ks[:, 1, 1], + kTdel_phid=Ks[:, 1, 2], + kTdel_deld=Ks[:, 1, 3], colors=['C0', 'C0', 'C1', 'C2'], hide_zeros=True) ax.set_ylim((-10.0, 10.0)) @@ -756,14 +763,14 @@ loop stability: ax = model.plot_simulation(times, x0, v=speeds[90], - kphi_phi=Ks[90, 0, 0], - kphi_del=Ks[90, 0, 1], - kphi_phid=Ks[90, 0, 2], - kphi_deld=Ks[90, 0, 3], - kdel_phi=Ks[90, 1, 0], - kdel_del=Ks[90, 1, 1], - kdel_phid=Ks[90, 1, 2], - kdel_deld=Ks[90, 1, 3], + kTphi_phi=Ks[90, 0, 0], + kTphi_del=Ks[90, 0, 1], + kTphi_phid=Ks[90, 0, 2], + kTphi_deld=Ks[90, 0, 3], + kTdel_phi=Ks[90, 1, 0], + kTdel_del=Ks[90, 1, 1], + kTdel_phid=Ks[90, 1, 2], + kTdel_deld=Ks[90, 1, 3], input_func=lambda t, x: np.array([100.0, 0.0]) if (t > 2.5 and t < 2.6) else np.zeros(2)) From 4a30f72835be987cb39dd612852311e3936898b4 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Wed, 23 Oct 2024 10:29:33 +0200 Subject: [PATCH 05/15] Show simple control example. --- docs/examples.rst | 37 ++++++++++++++++++++++++++++++++++--- 1 file changed, 34 insertions(+), 3 deletions(-) diff --git a/docs/examples.rst b/docs/examples.rst index 8e1b315..891e967 100644 --- a/docs/examples.rst +++ b/docs/examples.rst @@ -711,7 +711,37 @@ system. par_set = Meijaard2007WithFeedbackParameterSet(par, True) - speeds = np.linspace(1.0, 10.0, num=1001) +It is well known that a simple proportional positive feedback of roll angular +rate can stablize a bicycle at lower speeds. + +.. plot:: + :include-source: True + :context: close-figs + + from bicycleparameters.models import Meijaard2007WithFeedbackModel + + model = Meijaard2007WithFeedbackModel(par_set) + + ax = model.plot_eigenvalue_parts(v=speeds, + colors=['C0', 'C0', 'C1', 'C2'], + hide_zeros=True) + ax.set_ylim((-10.0, 10.0)) + +.. plot:: + :include-source: True + :context: close-figs + + ax = model.plot_eigenvalue_parts(v=speeds, + kTdel_phid=-50.0, + colors=['C0', 'C0', 'C1', 'C2'], + hide_zeros=True) + ax.set_ylim((-10.0, 10.0)) + +.. plot:: + :include-source: True + :context: close-figs + + speeds = np.linspace(0.0, 10.0, num=1001) As, Bs = model.form_state_space_matrices(v=speeds) Ks = np.zeros((len(speeds), 2, 4)) @@ -720,8 +750,9 @@ system. R = np.eye(1) for i, (Ai, Bi) in enumerate(zip(As, Bs)): - S = solve_continuous_are(Ai, Bi[:, 1:2], Q, R) # steer torque control - Ks[i, 1, :] = (np.linalg.inv(R) @ Bi[:, 1:2].T @ S).squeeze() + if i > 100: + S = solve_continuous_are(Ai, Bi[:, 1:2], Q, R) # steer torque control + Ks[i, 1, :] = (np.linalg.inv(R) @ Bi[:, 1:2].T @ S).squeeze() fig, axes = plt.subplots(2, 4, sharex=True, layout='constrained') for i, (row, name_row) in enumerate(zip(axes, gain_names)): From 22a8a7826801a7d99821d749c82b02811265574f Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Wed, 23 Oct 2024 16:01:49 +0200 Subject: [PATCH 06/15] Improved docstrings of new model and made simulation code more robust to shape of arrays. --- bicycleparameters/models.py | 63 +++++++++++++++++++++++-------------- 1 file changed, 39 insertions(+), 24 deletions(-) diff --git a/bicycleparameters/models.py b/bicycleparameters/models.py index 3e74d3e..51e09fb 100644 --- a/bicycleparameters/models.py +++ b/bicycleparameters/models.py @@ -771,15 +771,19 @@ def simulate(self, times, initial_conditions, input_func=None, if input_func is None: def eval_rhs(t, x): - return A@x + x = x.reshape((4, 1)) + return (A@x).squeeze() else: def eval_rhs(t, x): - return A@x + B@input_func(t, x) + u = input_func(t, x).reshape((2, 1)) + x = x.reshape((4, 1)) + return (A@x + B@u).squeeze() res = spi.solve_ivp(eval_rhs, (times[0], times[-1]), initial_conditions, - t_eval=times) + t_eval=times, + method="LSODA") if input_func is None: inputs = np.zeros((len(times), 2)) @@ -947,26 +951,37 @@ def plot_mode_simulations(self, times, **parameter_overrides): class Meijaard2007WithFeedbackModel(Meijaard2007Model): - """ + """Linear Carvallo-Whipple bicycle model that includes full state feedback + to drive all states to zero. With two inputs (roll torque and steer torque) + and four states (roll angle, steer angle, roll rate, steer rate) there are + eight control gain parameters in addition to the parameters defined in + [Meijaard2007]_. The states are:: - x = |roll angle | = |phi | - |steer angle| |del | - |roll rate | |phid| - |steer rate | |deld| + x = |roll angle | = |phi | + |steer angle | |delta | + |roll angular rate | |phidot | + |steer angular rate | |deltadot| The inputs are:: - u = |roll torque | = |Tphi| + u = |roll torque | = |Tphi | |steer torque| |Tdelta| Applying full state feedback gives this controller:: - u = -K*x = -|kphi_phi, kphi_del, kphi_phid, kphi_deld|*|phi | - |kdel_phi, kdel_del, kdel_phid, kdel_deld| |delta| - |phid | - |deld | + u = -K*x = -|kTphi_phi, kTphi_del, kTphi_phid, kTphi_deld|*|phi | + |kTdel_phi, kTdel_del, kTdel_phid, kTdel_deld| |delta | + |phidot | + |deltadot| + + This represents the new model:: + + x' = (A - B*K)*x + B*u + + so steer and roll torque can be applied in parallel to the feedback + control. """ def __init__(self, parameter_set): @@ -974,9 +989,9 @@ def __init__(self, parameter_set): 'Meijaard2007WithFeedback') def form_state_space_matrices(self, **parameter_overrides): - """Returns the A and B matrices for the Whipple-Carvallo model + """Returns the A and B matrices for the Carvallo-Whipple model linearized about the upright constant velocity configuration with a - full state feedback steer controller. + full state feedback steer controller to drive the states to zero. Returns ======= @@ -987,19 +1002,20 @@ def form_state_space_matrices(self, **parameter_overrides): Notes ===== - A, B, and K describe the model in state space form: - x' = (A - B*K)*x + B*u + A, B, and K describe the model in state space form:: + + x' = (A - B*K)*x + B*u where:: - x = |phi | = |roll angle | - |delta | |steer angle| - |phid | |roll rate | - |deld | |steer rate | + x = |phi | = |roll angle | + |delta | |steer angle | + |phidot | |roll angular rate | + |deldot | |steer angular rate | - K = |0 0 0 0 | - |kTdel_phi kTdel_del kphidot kdeltadot| + K = | kTphi_phi kTphi_del kTphi_phid kTphi_deld | + | kTdel_phi kTdel_del kTdel_phid kTdel_deld | u = |Tphi | = |roll torque | |Tdelta| |steer torque| @@ -1053,6 +1069,5 @@ def form_state_space_matrices(self, **parameter_overrides): else: # scalar parameters A, B = ab_matrix(M, C1, K0, K2, par['v'], par['g']) A = A - B@K - B = B return A, B From 3a9af67b3a4131b14f95a5bfded5dc2c98d0bc28 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Wed, 23 Oct 2024 16:02:12 +0200 Subject: [PATCH 07/15] Add springy kickstand and other improvements. --- docs/examples.rst | 110 ++++++++++++++++++++++++++++++++-------------- 1 file changed, 77 insertions(+), 33 deletions(-) diff --git a/docs/examples.rst b/docs/examples.rst index 891e967..42586bf 100644 --- a/docs/examples.rst +++ b/docs/examples.rst @@ -688,20 +688,19 @@ Feedback Control ---------------- We have a :py:class:`~bicycleparameters.models.Meijaard2007WithFeedbackModel` -that applies negative full state feedback to the +that applies full state feedback to the :py:class:`~bicycleparameters.models.Meijaard2007Model` using eight feedback -gains. These feedback gains can be chosen with a variety of methods. One -method is to create gain scheduling with respect to speed using LQR optimal -control. The 4 x 2 gain matrix can be found by solving the continous Ricatti -equation. If the system is controllable, this guarantees a stable closed loop -system. +gains. These feedback gains can be chosen with a variety of methods to +stabilize the system. + +Start by creating a new parameter set to use with the model that includes the +eight gain parameters. Initialize the gains to zero. .. plot:: :include-source: True :context: close-figs from bicycleparameters.parameter_sets import Meijaard2007WithFeedbackParameterSet - from scipy.linalg import solve_continuous_are gain_names = [['kTphi_phi', 'kTphi_del', 'kTphi_phid', 'kTphi_deld'], ['kTdel_phi', 'kTdel_del', 'kTdel_phid', 'kTdel_deld']] @@ -711,8 +710,9 @@ system. par_set = Meijaard2007WithFeedbackParameterSet(par, True) -It is well known that a simple proportional positive feedback of roll angular -rate can stablize a bicycle at lower speeds. +Now create the model and plote the eigenvalues versus speed. This should be +identical to the model without feedback given that the gains are all set to +zero. .. plot:: :include-source: True @@ -722,11 +722,17 @@ rate can stablize a bicycle at lower speeds. model = Meijaard2007WithFeedbackModel(par_set) + speeds = np.linspace(0.0, 10.0, num=1001) + ax = model.plot_eigenvalue_parts(v=speeds, colors=['C0', 'C0', 'C1', 'C2'], hide_zeros=True) ax.set_ylim((-10.0, 10.0)) +It is well known that a simple proportional positive feedback of roll angular +rate to control steer torque can stablize a bicycle at lower speeds. So set the +:math:`k_{T_{\delta}\dot{\phi}}` to a larger negative value. + .. plot:: :include-source: True :context: close-figs @@ -737,21 +743,52 @@ rate can stablize a bicycle at lower speeds. hide_zeros=True) ax.set_ylim((-10.0, 10.0)) +The stable speed range is significantly increased, but the weave mode +eigenfrequency is increased as a consequence. + +This can also be used to model adding springy training wheels by including a +negative feedback of roll angle to roll torque. + .. plot:: :include-source: True :context: close-figs - speeds = np.linspace(0.0, 10.0, num=1001) + ax = model.plot_eigenvalue_parts(v=speeds, + kTphi_phi=3000.0, + kTphi_phid=30.0, + colors=['C0', 'C0', 'C1', 'C2'], + hide_zeros=True) + ax.set_ylim((-10.0, 10.0)) + +.. plot:: + :include-source: True + :context: close-figs + + times = np.linspace(0.0, 1.0, num=1001) + + model.plot_mode_simulations(times, v=0.1) + +A more general method to control the bicycle is to create gain scheduling with +respect to speed using LQR optimal control. Assuming we only control steer +torque via feedback of all four states, the 4 gains can be found by solving the +continous Ricatti equation. If the system is controllable, this guarantees a +stable closed loop system. There is an uncontrollable speed just below 0.8 m/s, +so we will only apply control at speeds greater than 0.8 m/s. + +.. plot:: + :include-source: True + :context: close-figs + + from scipy.linalg import solve_continuous_are As, Bs = model.form_state_space_matrices(v=speeds) Ks = np.zeros((len(speeds), 2, 4)) Q = np.eye(4) - Q = np.diag([10.0, 1.0, 10.0, 1.0]) R = np.eye(1) - for i, (Ai, Bi) in enumerate(zip(As, Bs)): - if i > 100: - S = solve_continuous_are(Ai, Bi[:, 1:2], Q, R) # steer torque control + for i, (vi, Ai, Bi) in enumerate(zip(speeds, As, Bs)): + if vi >= 0.8: + S = solve_continuous_are(Ai, Bi[:, 1:2], Q, R) Ks[i, 1, :] = (np.linalg.inv(R) @ Bi[:, 1:2].T @ S).squeeze() fig, axes = plt.subplots(2, 4, sharex=True, layout='constrained') @@ -760,19 +797,14 @@ rate can stablize a bicycle at lower speeds. col.plot(speeds, Ks[:, i, j]) col.set_ylabel(r'${}$'.format(par_set.par_strings[name])) for ax in row: - ax.set_label('v [m/s]') + ax.set_xlabel('v [m/s]') -Now create the feedback model and use the computed gains to check for closed -loop stability: +Now use the computed gains to check for closed loop stability: .. plot:: :include-source: True :context: close-figs - from bicycleparameters.models import Meijaard2007WithFeedbackModel - - model = Meijaard2007WithFeedbackModel(par_set) - ax = model.plot_eigenvalue_parts(v=speeds, kTphi_phi=Ks[:, 0, 0], kTphi_del=Ks[:, 0, 1], @@ -786,23 +818,35 @@ loop stability: hide_zeros=True) ax.set_ylim((-10.0, 10.0)) +This is stable over a wide speed range and retains the weave eigenfrequency of +the uncontrolled system. + .. plot:: :include-source: True :context: close-figs x0 = np.deg2rad([5.0, -3.0, 0.0, 0.0]) + def input_func(t, x): + print('in here') + if (t > 2.5 and t < 2.6): + return np.array([200.0, 0.0]) + else: + return np.zeros(2) + + times = np.linspace(0.0, 5.0, num=1001) + + idx = 90 ax = model.plot_simulation(times, x0, - v=speeds[90], - kTphi_phi=Ks[90, 0, 0], - kTphi_del=Ks[90, 0, 1], - kTphi_phid=Ks[90, 0, 2], - kTphi_deld=Ks[90, 0, 3], - kTdel_phi=Ks[90, 1, 0], - kTdel_del=Ks[90, 1, 1], - kTdel_phid=Ks[90, 1, 2], - kTdel_deld=Ks[90, 1, 3], - input_func=lambda t, x: np.array([100.0, 0.0]) - if (t > 2.5 and t < 2.6) - else np.zeros(2)) + input_func=input_func, + v=speeds[idx], + kTphi_phi=Ks[idx, 0, 0], + kTphi_del=Ks[idx, 0, 1], + kTphi_phid=Ks[idx, 0, 2], + kTphi_deld=Ks[idx, 0, 3], + kTdel_phi=Ks[idx, 1, 0], + kTdel_del=Ks[idx, 1, 1], + kTdel_phid=Ks[idx, 1, 2], + kTdel_deld=Ks[idx, 1, 3], + ) ax[0].set_title('$v$ = {} m/s'.format(speeds[90])) From 1110f280481881a03002435db401ba10ca8c7c62 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Wed, 23 Oct 2024 16:07:59 +0200 Subject: [PATCH 08/15] Add damping to kickstand. --- docs/examples.rst | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/docs/examples.rst b/docs/examples.rst index 42586bf..249b26b 100644 --- a/docs/examples.rst +++ b/docs/examples.rst @@ -747,7 +747,7 @@ The stable speed range is significantly increased, but the weave mode eigenfrequency is increased as a consequence. This can also be used to model adding springy training wheels by including a -negative feedback of roll angle to roll torque. +negative feedback of roll angle to roll torque with damping. .. plot:: :include-source: True @@ -755,7 +755,7 @@ negative feedback of roll angle to roll torque. ax = model.plot_eigenvalue_parts(v=speeds, kTphi_phi=3000.0, - kTphi_phid=30.0, + kTphi_phid=600.0, colors=['C0', 'C0', 'C1', 'C2'], hide_zeros=True) ax.set_ylim((-10.0, 10.0)) @@ -764,9 +764,10 @@ negative feedback of roll angle to roll torque. :include-source: True :context: close-figs - times = np.linspace(0.0, 1.0, num=1001) + times = np.linspace(0.0, 10.0, num=1001) - model.plot_mode_simulations(times, v=0.1) + model.plot_mode_simulations(times, v=2.0, kTphi_phi=3000.0, + kTphi_phid=600.0) A more general method to control the bicycle is to create gain scheduling with respect to speed using LQR optimal control. Assuming we only control steer From b432875926d04ca3d749959c9a50812e0c300ed6 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Wed, 23 Oct 2024 16:10:49 +0200 Subject: [PATCH 09/15] Shorten simulation. --- docs/examples.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/examples.rst b/docs/examples.rst index 249b26b..33f4d82 100644 --- a/docs/examples.rst +++ b/docs/examples.rst @@ -764,7 +764,7 @@ negative feedback of roll angle to roll torque with damping. :include-source: True :context: close-figs - times = np.linspace(0.0, 10.0, num=1001) + times = np.linspace(0.0, 5.0, num=1001) model.plot_mode_simulations(times, v=2.0, kTphi_phi=3000.0, kTphi_phid=600.0) From 61da6900ffa43efb92af049178abd3818761857f Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Wed, 23 Oct 2024 20:56:42 +0200 Subject: [PATCH 10/15] Back to simpler simulation code. --- bicycleparameters/models.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/bicycleparameters/models.py b/bicycleparameters/models.py index 51e09fb..61b294f 100644 --- a/bicycleparameters/models.py +++ b/bicycleparameters/models.py @@ -771,13 +771,10 @@ def simulate(self, times, initial_conditions, input_func=None, if input_func is None: def eval_rhs(t, x): - x = x.reshape((4, 1)) - return (A@x).squeeze() + return A@x else: def eval_rhs(t, x): - u = input_func(t, x).reshape((2, 1)) - x = x.reshape((4, 1)) - return (A@x + B@u).squeeze() + return A@x + B@input_func(t, x) res = spi.solve_ivp(eval_rhs, (times[0], times[-1]), @@ -840,14 +837,17 @@ def plot_simulation(self, times, initial_conditions, input_func=None, input_func=input_func, **parameter_overrides) - fig, axes = plt.subplots(3, sharex=True) + fig, axes = plt.subplots(3, sharex=True, layout='constrained') axes[0].plot(times, inputs) axes[0].legend([r'$T_\phi$', r'$T_\delta$']) + axes[0].set_ylabel('Torque\n[Nm]') axes[1].plot(times, np.rad2deg(res[:, :2])) axes[1].legend(['$' + lab + '$' for lab in self.state_vars_latex[:2]]) + axes[1].set_ylabel('Angle\n[deg]') axes[2].plot(times, np.rad2deg(res[:, 2:])) axes[2].legend(['$' + lab + '$' for lab in self.state_vars_latex[2:]]) + axes[2].set_ylabel('Angluar Rate\n[deg/s]') axes[2].set_xlabel('Time [s]') From 39318bb13340e6542f84cc008604f4ddb658105d Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Wed, 23 Oct 2024 20:57:10 +0200 Subject: [PATCH 11/15] Tidy the example plots. --- docs/examples.rst | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/docs/examples.rst b/docs/examples.rst index 33f4d82..590d8eb 100644 --- a/docs/examples.rst +++ b/docs/examples.rst @@ -796,7 +796,7 @@ so we will only apply control at speeds greater than 0.8 m/s. for i, (row, name_row) in enumerate(zip(axes, gain_names)): for j, (col, name) in enumerate(zip(row, name_row)): col.plot(speeds, Ks[:, i, j]) - col.set_ylabel(r'${}$'.format(par_set.par_strings[name])) + col.set_title(r'${}$'.format(par_set.par_strings[name])) for ax in row: ax.set_xlabel('v [m/s]') @@ -814,9 +814,7 @@ Now use the computed gains to check for closed loop stability: kTdel_phi=Ks[:, 1, 0], kTdel_del=Ks[:, 1, 1], kTdel_phid=Ks[:, 1, 2], - kTdel_deld=Ks[:, 1, 3], - colors=['C0', 'C0', 'C1', 'C2'], - hide_zeros=True) + kTdel_deld=Ks[:, 1, 3]) ax.set_ylim((-10.0, 10.0)) This is stable over a wide speed range and retains the weave eigenfrequency of @@ -829,7 +827,6 @@ the uncontrolled system. x0 = np.deg2rad([5.0, -3.0, 0.0, 0.0]) def input_func(t, x): - print('in here') if (t > 2.5 and t < 2.6): return np.array([200.0, 0.0]) else: @@ -850,4 +847,4 @@ the uncontrolled system. kTdel_phid=Ks[idx, 1, 2], kTdel_deld=Ks[idx, 1, 3], ) - ax[0].set_title('$v$ = {} m/s'.format(speeds[90])) + ax[0].set_title('$v$ = {} m/s'.format(speeds[idx])) From 7853609474210e0bc942d8ddea5c31182986e98b Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Thu, 24 Oct 2024 07:49:53 +0200 Subject: [PATCH 12/15] Support to_parameterization for the feedback parameter set, move gain plotting into the model, increase roll torque pulse width. --- bicycleparameters/models.py | 55 +++++++++++++++++++++++++++++ bicycleparameters/parameter_sets.py | 44 ++++++++++++++++++++++- docs/examples.rst | 48 +++++++++---------------- 3 files changed, 114 insertions(+), 33 deletions(-) diff --git a/bicycleparameters/models.py b/bicycleparameters/models.py index 61b294f..fd6b8c5 100644 --- a/bicycleparameters/models.py +++ b/bicycleparameters/models.py @@ -1071,3 +1071,58 @@ def form_state_space_matrices(self, **parameter_overrides): A = A - B@K return A, B + + def plot_gains(self, axes=None, **parameter_overrides): + """Plots the gains versus a single varying parameter. The + ``parameter_overrides`` should contain one parameter that is an array, + other than the eight gains. That parameter will be used for the x axis. + The gains can be either arrays of the same length or scalars. + + Parameters + ========== + axes : array_like, shape(2, 4) + Matplotlib axes set to plot to. + parameter_overrides : dictionary + Parameter keys that map to floats or array_like of floats + shape(n,). All keys that map to array_like must be of the same + length. + + Returns + ======= + axes : ndarray, shape(2, 4) + Array of matplotlib axes. + + """ + gain_names = ['kTphi_phi', 'kTphi_del', 'kTphi_phid', 'kTphi_deld', + 'kTdel_phi', 'kTdel_del', 'kTdel_phid', 'kTdel_deld'] + + if axes is None: + fig, axes = plt.subplots(2, 4, sharex=True, layout='constrained') + + par, array_keys, array_len = self._parse_parameter_overrides( + **parameter_overrides) + + non_gain_array_keys = [k for k in array_keys if k not in gain_names] + if len(non_gain_array_keys) == 0: + raise ValueError('No x axis key. Set one parameter other than the ' + 'gains as an array.') + else: + x_axis_key = non_gain_array_keys[0] + + if len(non_gain_array_keys) > 1: + msg = 'More than one array for x axis, choosing {}.' + print(msg.format(x_axis_key)) + + for ax, name in zip(axes.flatten(), gain_names): + if name in array_keys: + ax.plot(par[x_axis_key], par[name]) + else: + ax.plot(par[x_axis_key], + par[name]*np.ones_like(par[x_axis_key])) + msg = r'${}$'.format(self.parameter_set.par_strings[name]) + ax.set_title(msg) + + for ax in axes[1, :]: + ax.set_xlabel(x_axis_key) + + return axes diff --git a/bicycleparameters/parameter_sets.py b/bicycleparameters/parameter_sets.py index adb40cb..0504d53 100644 --- a/bicycleparameters/parameter_sets.py +++ b/bicycleparameters/parameter_sets.py @@ -301,6 +301,40 @@ def __init__(self, parameters, includes_rider): self.includes_rider = includes_rider self._generate_body_colors() + def to_parameterization(self, name): + """Returns a specific parameter set based on the provided + parameterization name. + + Parameters + ========== + name : string + The name of the parameterization. These should correspond to a + subclass of a ``ParameterSet`` and the name will be the string that + precedes "ParameterSet". For example, the parameterization name of + ``Meijaard2007ParameterSet`` is ``Meijaard2007``. + + Returns + ======= + ParmeterSet + If a different parameterization is requested and this class can + convert itself, it will return a new parameter set of the correct + parameterization. + + """ + if name == self.parameterization: + return self + elif name == 'Meijaard2007WithFeedback': + par = self.parameters.copy() + gain_names = ['kTphi_phi', 'kTphi_del', 'kTphi_phid', 'kTphi_deld', + 'kTdel_phi', 'kTdel_del', 'kTdel_phid', 'kTdel_deld'] + for gain in gain_names: + par[gain] = 0.0 + return Meijaard2007WithFeedbackParameterSet(par, + self.includes_rider) + else: + msg = '{} is not an avaialble parameter set' + raise ValueError(msg.format(name)) + @classmethod def _from_yaml(cls, path_to_file): @@ -919,6 +953,9 @@ class Meijaard2007WithFeedbackParameterSet(Meijaard2007ParameterSet): body_labels = ['B', 'F', 'H', 'R'] + def __init__(self, parameters, includes_rider): + super().__init__(parameters, includes_rider) + class Moore2019ParameterSet(ParameterSet): """Represents the parameters of the bicycle parameterization presented in @@ -1075,8 +1112,13 @@ def to_parameterization(self, name): b = _convert_principal_to_benchmark(self.parameters) # Moore2019 always includes the rider return Meijaard2007ParameterSet(b, True) + elif name == 'Meijaard2007WithFeedback': + b = _convert_principal_to_benchmark(self.parameters) + # Moore2019 always includes the rider + par_set = Meijaard2007ParameterSet(b, True) + return par_set.to_parameterization(name) else: - msg = '{} is not an avaialble parameter set' + msg = '{} is not an available parameter set' raise ValueError(msg.format(name)) def plot_geometry(self, show_steer_axis=True, ax=None): diff --git a/docs/examples.rst b/docs/examples.rst index 590d8eb..6704b79 100644 --- a/docs/examples.rst +++ b/docs/examples.rst @@ -693,24 +693,7 @@ that applies full state feedback to the gains. These feedback gains can be chosen with a variety of methods to stabilize the system. -Start by creating a new parameter set to use with the model that includes the -eight gain parameters. Initialize the gains to zero. - -.. plot:: - :include-source: True - :context: close-figs - - from bicycleparameters.parameter_sets import Meijaard2007WithFeedbackParameterSet - - gain_names = [['kTphi_phi', 'kTphi_del', 'kTphi_phid', 'kTphi_deld'], - ['kTdel_phi', 'kTdel_del', 'kTdel_phid', 'kTdel_deld']] - for row in gain_names: - for name in row: - par[name] = 0.0 - - par_set = Meijaard2007WithFeedbackParameterSet(par, True) - -Now create the model and plote the eigenvalues versus speed. This should be +Create the model and plot the eigenvalues versus speed. This should be identical to the model without feedback given that the gains are all set to zero. @@ -730,8 +713,8 @@ zero. ax.set_ylim((-10.0, 10.0)) It is well known that a simple proportional positive feedback of roll angular -rate to control steer torque can stablize a bicycle at lower speeds. So set the -:math:`k_{T_{\delta}\dot{\phi}}` to a larger negative value. +rate to control steer torque can stabilize a bicycle at lower speeds. So set +the :math:`k_{T_{\delta}\dot{\phi}}` to a larger negative value. .. plot:: :include-source: True @@ -739,7 +722,7 @@ rate to control steer torque can stablize a bicycle at lower speeds. So set the ax = model.plot_eigenvalue_parts(v=speeds, kTdel_phid=-50.0, - colors=['C0', 'C0', 'C1', 'C2'], + colors=['C0', 'C0', 'C1', 'C1'], hide_zeros=True) ax.set_ylim((-10.0, 10.0)) @@ -756,7 +739,6 @@ negative feedback of roll angle to roll torque with damping. ax = model.plot_eigenvalue_parts(v=speeds, kTphi_phi=3000.0, kTphi_phid=600.0, - colors=['C0', 'C0', 'C1', 'C2'], hide_zeros=True) ax.set_ylim((-10.0, 10.0)) @@ -772,7 +754,7 @@ negative feedback of roll angle to roll torque with damping. A more general method to control the bicycle is to create gain scheduling with respect to speed using LQR optimal control. Assuming we only control steer torque via feedback of all four states, the 4 gains can be found by solving the -continous Ricatti equation. If the system is controllable, this guarantees a +continuous Ricatti equation. If the system is controllable, this guarantees a stable closed loop system. There is an uncontrollable speed just below 0.8 m/s, so we will only apply control at speeds greater than 0.8 m/s. @@ -792,13 +774,15 @@ so we will only apply control at speeds greater than 0.8 m/s. S = solve_continuous_are(Ai, Bi[:, 1:2], Q, R) Ks[i, 1, :] = (np.linalg.inv(R) @ Bi[:, 1:2].T @ S).squeeze() - fig, axes = plt.subplots(2, 4, sharex=True, layout='constrained') - for i, (row, name_row) in enumerate(zip(axes, gain_names)): - for j, (col, name) in enumerate(zip(row, name_row)): - col.plot(speeds, Ks[:, i, j]) - col.set_title(r'${}$'.format(par_set.par_strings[name])) - for ax in row: - ax.set_xlabel('v [m/s]') + ax = model.plot_gains(v=speeds, + kTphi_phi=Ks[:, 0, 0], + kTphi_del=Ks[:, 0, 1], + kTphi_phid=Ks[:, 0, 2], + kTphi_deld=Ks[:, 0, 3], + kTdel_phi=Ks[:, 1, 0], + kTdel_del=Ks[:, 1, 1], + kTdel_phid=Ks[:, 1, 2], + kTdel_deld=Ks[:, 1, 3]) Now use the computed gains to check for closed loop stability: @@ -827,8 +811,8 @@ the uncontrolled system. x0 = np.deg2rad([5.0, -3.0, 0.0, 0.0]) def input_func(t, x): - if (t > 2.5 and t < 2.6): - return np.array([200.0, 0.0]) + if (t > 2.5 and t < 2.8): + return np.array([50.0, 0.0]) else: return np.zeros(2) From 96842370ad40e762381f9519418557f52ed45fb8 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Thu, 24 Oct 2024 08:10:29 +0200 Subject: [PATCH 13/15] Added unit tests for the new classes. --- bicycleparameters/tests/test_models.py | 51 ++++++++++++++++++- .../tests/test_parameter_sets.py | 36 ++++++++++++- 2 files changed, 84 insertions(+), 3 deletions(-) diff --git a/bicycleparameters/tests/test_models.py b/bicycleparameters/tests/test_models.py index 668b906..db31335 100644 --- a/bicycleparameters/tests/test_models.py +++ b/bicycleparameters/tests/test_models.py @@ -1,8 +1,9 @@ import numpy as np from pytest import raises -from ..parameter_sets import Meijaard2007ParameterSet -from ..models import Meijaard2007Model +from ..parameter_sets import (Meijaard2007ParameterSet, + Meijaard2007WithFeedbackParameterSet) +from ..models import Meijaard2007Model, Meijaard2007WithFeedbackModel meijaard2007_parameters = { # dictionary of the parameters in Meijaard 2007 'IBxx': 9.2, @@ -144,3 +145,49 @@ def test_Meijaard2007Model(show=False): if show: import matplotlib.pyplot as plt plt.show() + + +def test_Meijaard2007WithFeedbackModel(show=False): + + parameter_set = Meijaard2007ParameterSet(meijaard2007_parameters, True) + + model = Meijaard2007WithFeedbackModel(parameter_set) + assert type(model.parameter_set) == Meijaard2007WithFeedbackParameterSet + + M, C1, K0, K2 = model.form_reduced_canonical_matrices() + assert M.shape == (2, 2) + assert C1.shape == (2, 2) + assert K0.shape == (2, 2) + assert K2.shape == (2, 2) + + A, B = model.form_state_space_matrices() + assert A.shape == (4, 4) + assert B.shape == (4, 2) + A, B = model.form_state_space_matrices(w=np.linspace(0.5, 1.5, num=5)) + assert A.shape == (5, 4, 4) + assert B.shape == (5, 4, 2) + A, B = model.form_state_space_matrices(v=np.linspace(0, 10, num=10)) + assert A.shape == (10, 4, 4) + assert B.shape == (10, 4, 2) + + A, B = model.form_state_space_matrices(v=np.linspace(0, 10, num=10), + g=np.linspace(0, 10, num=10)) + assert A.shape == (10, 4, 4) + assert B.shape == (10, 4, 2) + + with raises(ValueError): + # one parameter must be an array + model.plot_gains() + + model.plot_gains(g=np.linspace(0, 10, num=10)) + + model.plot_gains(g=np.linspace(0, 10, num=10), + kTdel_phi=np.linspace(0, 10, num=10)) + + model.plot_gains(g=np.linspace(0, 10, num=10), + v=np.linspace(0, 10, num=10), + kTdel_phi=np.linspace(0, 10, num=10)) + + if show: + import matplotlib.pyplot as plt + plt.show() diff --git a/bicycleparameters/tests/test_parameter_sets.py b/bicycleparameters/tests/test_parameter_sets.py index c83f490..64a64ca 100644 --- a/bicycleparameters/tests/test_parameter_sets.py +++ b/bicycleparameters/tests/test_parameter_sets.py @@ -4,7 +4,8 @@ import numpy as np import matplotlib.pyplot as plt -from ..parameter_sets import Meijaard2007ParameterSet, Moore2019ParameterSet +from ..parameter_sets import (Meijaard2007ParameterSet, Moore2019ParameterSet, + Meijaard2007WithFeedbackParameterSet) PARDIR_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'parameter_sets') @@ -99,3 +100,36 @@ def test_conversion(plot=False): bench_pset.plot_all() if plot: plt.show() + + +def test_Meijaard2007WithFeedbackParameterSet(): + + filepath = os.path.join(PARDIR_PATH, 'benchmark-benchmark.yml') + with open(filepath, 'r') as f: + benchmark_par = yaml.load(f, Loader=yaml.FullLoader)['values'] + + pset = Meijaard2007ParameterSet( + benchmark_par, True).to_parameterization('Meijaard2007WithFeedback') + + pset2 = Meijaard2007WithFeedbackParameterSet(pset.parameters, True) + + assert type(pset) == Meijaard2007WithFeedbackParameterSet + assert type(pset2) == Meijaard2007WithFeedbackParameterSet + + gain_names = ['kTphi_phi', 'kTphi_del', 'kTphi_phid', 'kTphi_deld', + 'kTdel_phi', 'kTdel_del', 'kTdel_phid', 'kTdel_deld'] + + for name in gain_names: + assert pset.parameters[name] == 0.0 + assert pset2.parameters[name] == 0.0 + + assert pset.par_strings['kTphi_phi'] == r'k_{T_{\phi}\phi}' + assert pset2.par_strings['kTphi_phi'] == r'k_{T_{\phi}\phi}' + + expected_IH = np.array([[0.05892, 0.0, -0.00756], + [0.0, 0.06, 0.0], + [-0.00756, 0.0, 0.00708]]) + np.testing.assert_allclose(pset.form_inertia_tensor('H'), + expected_IH) + np.testing.assert_allclose(pset2.form_inertia_tensor('H'), + expected_IH) From 4b645127ef19bfa24d3f2c5d6426ba03a4ec12f3 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Thu, 24 Oct 2024 08:19:49 +0200 Subject: [PATCH 14/15] Add example gain plot. --- bicycleparameters/models.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/bicycleparameters/models.py b/bicycleparameters/models.py index fd6b8c5..8492400 100644 --- a/bicycleparameters/models.py +++ b/bicycleparameters/models.py @@ -1092,6 +1092,22 @@ def plot_gains(self, axes=None, **parameter_overrides): axes : ndarray, shape(2, 4) Array of matplotlib axes. + Examples + ======== + + .. plot:: + :include-source: True + :context: reset + + import numpy as np + from bicycleparameters.parameter_dicts import meijaard2007_browser_jason + from bicycleparameters.parameter_sets import Meijaard2007ParameterSet + from bicycleparameters.models import Meijaard2007WithFeedbackModel + p = Meijaard2007ParameterSet(meijaard2007_browser_jason, True) + m = Meijaard2007WithFeedbackModel(p) + m.plot_gains(v=np.linspace(0.0, 10.0, num=101), + kTdel_phid=-10.0*np.linspace(0.0, 5.0, num=101)) + """ gain_names = ['kTphi_phi', 'kTphi_del', 'kTphi_phid', 'kTphi_deld', 'kTdel_phi', 'kTdel_del', 'kTdel_phid', 'kTdel_deld'] From 7bbe64f68a40e1a5ccc413f0e2f15710d4130790 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Thu, 24 Oct 2024 08:26:25 +0200 Subject: [PATCH 15/15] Spelling. --- docs/examples.rst | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/examples.rst b/docs/examples.rst index 6704b79..73f95e4 100644 --- a/docs/examples.rst +++ b/docs/examples.rst @@ -122,7 +122,7 @@ looks to see if there are any parameter sets in ``./bicycles/Stratos/Parameters/``. If so, it loads the data, if not it looks for ``./bicycles/Stratos/RawData/StratosMeasurments.txt`` so that it can generate the parameter set. The raw measurement file may or may not contain the -oscillation period data for the bicycle moment of inertia caluclations. If it +oscillation period data for the bicycle moment of inertia calculations. If it doesn't then the program will look for the series of ``.mat`` files need to calculate the periods. If no data is there, then you get an error. @@ -133,9 +133,9 @@ There are other loading options:: The ``pathToData`` option allows you specify a directory other than the current directory as your data directory. The ``forceRawCalc`` forces the program to load ``./bicycles/Stratos/RawData/StratosMeasurments.txt`` and recalculate the -parameters regarless if there are any parameter files available in +parameters regardless if there are any parameter files available in ``./bicycles/Stratos/Parameters/``. The ``forcePeriodCalc`` option forces the period -calcluation from the ``.mat`` files regardless if they already exist in the raw +calculation from the ``.mat`` files regardless if they already exist in the raw measurement file. Exploring bicycle parameter data @@ -490,7 +490,7 @@ displays a simplified depiction:: .. image:: bicycleRiderGeometry.png -The eigenvalue plot also relfects the changes:: +The eigenvalue plot also reflects the changes:: >>> bicycle.plot_eigenvalues_vs_speed(speeds, show=True)