Skip to content

Commit b65e71b

Browse files
realJoeyrealJoey
committed
added examples from 2nd edition
1 parent 7d5019e commit b65e71b

54 files changed

Lines changed: 4097 additions & 0 deletions

File tree

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎examples/bicycle.py‎

Lines changed: 169 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,169 @@
1+
# bicycle.py - bicycle dynamics
2+
# RMM, 24 Nov 2024 (from KJA)
3+
#
4+
# Bicyle dynamics
5+
#
6+
# This model describes the dynamics of a bicycle with the feature that one
7+
# of its key properties is due to a feedback mechanism that is created by
8+
# the design of the front fork. This model is described in more detail in
9+
# Section 4.2 of FBS2e.
10+
#
11+
# MATLAB header (bicycle_stabplot.m)
12+
# % Linearized 4th order model and analysis of eigenvalues
13+
# % Equations based on Schwab et al 2004
14+
# % Run bicycleparameters first
15+
# % kja 040611
16+
# % Parameters of a bicycle model
17+
# % kja 040613
18+
# % Basic data is given by 26 parameters
19+
20+
import numpy as np
21+
import control as ct
22+
from math import pi
23+
24+
import numpy as np
25+
26+
# Acceleration of gravity [m/s^2]
27+
g = 9.81
28+
29+
# Wheel base [m]
30+
b = 1.00
31+
32+
# Trail [m]
33+
c = 0.08
34+
35+
# Wheel radii
36+
Rrw = 0.35
37+
Rfw = 0.35
38+
39+
# Head angle [radians]
40+
lambda_angle = np.pi * 70 / 180
41+
42+
# Rear frame mass [kg], center of mass [m], and inertia tensor [kgm^2]
43+
mrf = 87
44+
xrf = 0.491586
45+
zrf = 1.028138
46+
Jxxrf = 3.283666
47+
Jxzrf = 0.602765
48+
Jyyrf = 3.8795952
49+
Jzzrf = 0.565929
50+
51+
# Front frame mass [kg], center of mass [m], and inertia tensor [kgm^2]
52+
mff = 2
53+
xff = 0.866
54+
zff = 0.676
55+
Jxxff = 0.08
56+
Jxzff = -0.02
57+
Jyyff = 0.07
58+
Jzzff = 0.02
59+
60+
# Rear wheel mass [kg], center of mass [m], and inertia tensor [kgm^2]
61+
mrw = 1.5
62+
Jxxrw = 0.07
63+
Jyyrw = 0.14
64+
65+
# Front wheel mass [kg], center of mass [m], and inertia tensor [kgm^2]
66+
mfw = 1.5
67+
Jxxfw = 0.07
68+
Jyyfw = 0.14
69+
70+
# Auxiliary variables
71+
xrw = 0
72+
zrw = Rrw
73+
xfw = b
74+
zfw = Rfw
75+
Jzzrw = Jxxrw
76+
Jzzfw = Jxxfw
77+
78+
# Total mass
79+
mt = mrf + mrw + mff + mfw
80+
81+
# Center of mass
82+
xt = (mrf * xrf + mrw * xrw + mff * xff + mfw * xfw) / mt
83+
zt = (mrf * zrf + mrw * zrw + mff * zff + mfw * zfw) / mt
84+
85+
# Inertia tensor components
86+
Jxxt = (
87+
Jxxrf + mrf * zrf**2 +
88+
Jxxrw + mrw * zrw**2 +
89+
Jxxff + mff * zff**2 +
90+
Jxxfw + mfw * zfw**2
91+
)
92+
Jxzt = (
93+
Jxzrf + mrf * xrf * zrf +
94+
mrw * xrw * zrw +
95+
Jxzff + mff * xff * zff +
96+
mfw * xfw * zfw
97+
)
98+
Jzzt = (
99+
Jzzrf + mrf * xrf**2 +
100+
Jzzrw + mrw * xrw**2 +
101+
Jzzff + mff * xff**2 +
102+
Jzzfw + mfw * xfw**2
103+
)
104+
105+
# Front frame parameters
106+
mf = mff + mfw
107+
xf = (mff * xff + mfw * xfw) / mf
108+
zf = (mff * zff + mfw * zfw) / mf
109+
110+
Jxxf = (
111+
Jxxff + mff * (zff - zf)**2 +
112+
Jxxfw + mfw * (zfw - zf)**2
113+
)
114+
Jxzf = (
115+
Jxzff + mff * (xff - xf) * (zff - zf) +
116+
mfw * (xfw - xf) * (zfw - zf)
117+
)
118+
Jzzf = (
119+
Jzzff + mff * (xff - xf)**2 +
120+
Jzzfw + mfw * (xfw - xf)**2
121+
)
122+
123+
# Auxiliary variables
124+
d = (xf - b - c) * np.sin(lambda_angle) + zf * np.cos(lambda_angle)
125+
Fll = (
126+
mf * d**2 +
127+
Jxxf * np.cos(lambda_angle)**2 +
128+
2 * Jxzf * np.sin(lambda_angle) * np.cos(lambda_angle) +
129+
Jzzf * np.sin(lambda_angle)**2
130+
)
131+
Flx = mf * d * zf + Jxxf * np.cos(lambda_angle) + Jxzf * np.sin(lambda_angle)
132+
Flz = mf * d * xf + Jxzf * np.cos(lambda_angle) + Jzzf * np.sin(lambda_angle)
133+
gamma = c * np.sin(lambda_angle) / b
134+
Sr = Jyyrw / Rrw
135+
Sf = Jyyfw / Rfw
136+
St = Sr + Sf
137+
Su = mf * d + gamma * mt * xt
138+
139+
# Matrices for the linearized fourth-order model
140+
M = np.array([
141+
[Jxxt, -Flx - gamma * Jxzt],
142+
[-Flx - gamma * Jxzt, Fll + 2 * gamma * Flz + gamma**2 * Jzzt]
143+
])
144+
145+
K0 = np.array([
146+
[-mt * g * zt, g * Su],
147+
[g * Su, -g * Su * np.cos(lambda_angle)]
148+
])
149+
150+
K2 = np.array([
151+
[0, -(St + mt * zt) * np.sin(lambda_angle) / b],
152+
[0, (Su + Sf * np.cos(lambda_angle)) * np.sin(lambda_angle) / b]
153+
])
154+
155+
c12 = gamma * St + Sf * np.sin(lambda_angle) \
156+
+ Jxzt * np.sin(lambda_angle) / b + gamma * mt * zt
157+
c22 = Flz * np.sin(lambda_angle) / b \
158+
+ gamma * (Su + Jzzt * np.sin(lambda_angle) / b)
159+
160+
C = np.array([
161+
[0, -c12],
162+
[gamma * St + Sf * np.sin(lambda_angle), c22]
163+
])
164+
165+
def whipple_A(v0):
166+
return np.block([
167+
[np.zeros((2, 2)), np.eye(2)],
168+
[-np.linalg.inv(M) @ (K0 + K2 * v0**2), -np.linalg.inv(M) @ C * v0]
169+
])

‎examples/congctrl.py‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
# congctrl.py - Congestion control dynamics and control
2+
# RMM, 11 Jul 2006 (from MATLAB)
3+
#
4+
# Congestion control dynamics
5+
#
6+
# This model implements the congestion control dynamics described in
7+
# the text. We assume that we have N identical sources and 1 router.
8+
#
9+
# To allow the model to be used in a variety of ways, we
10+
# allow the number of states and the number of sources to be set
11+
# independently. The length of the state vector, M+1, is used to determine
12+
# the number of simulated sources, with each source representing N/M
13+
# sources.
14+
15+
import numpy as np
16+
import control as ct
17+
18+
# Congestion control dynamics
19+
def _congctrl_update(t, x, u, params):
20+
# Number of sources per state of the simulation
21+
M = x.size - 1
22+
23+
# Remaining parameters
24+
N = params.get('N', M) # number of sources
25+
rho = params.get('rho', 2e-4) # RED parameter = pbar / (bupper-blower)
26+
c = params.get('c', 10) # link capacity (Mp/ms)
27+
28+
# Compute the derivative (last state = bdot)
29+
return np.append(
30+
c / x[M] - (rho * c) * (1 + (x[:-1]**2) / 2),
31+
N/M * np.sum(x[:-1]) * c / x[M] - c)
32+
33+
34+
# Function to define an I/O system
35+
def create_iosystem(M, N=60, rho=2e-4, c=10):
36+
#! TODO: Check to make sure M and N are compatible
37+
return ct.nlsys(
38+
_congctrl_update, None, states=M+1,
39+
params={'N': N, 'rho': rho, 'c': c})

‎examples/cruise.py‎

Lines changed: 124 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,124 @@
1+
# cruise.py - Cruise control dynamics and control
2+
# RMM, 20 Jun 2021
3+
4+
import numpy as np
5+
import matplotlib.pyplot as plt
6+
from math import pi
7+
import control as ct
8+
9+
#
10+
# Vehicle model
11+
#
12+
# The dynamics for this system are implemented using the I/O systems module
13+
# in python-control. This model is described in detail in Section 4.1 of
14+
# FBS2e.
15+
#
16+
17+
# Engine model
18+
def motor_torque(omega, params={}):
19+
# Set up the system parameters
20+
Tm = params.get('Tm', 190.) # engine torque constant
21+
omega_m = params.get('omega_m', 420.) # peak engine angular speed
22+
beta = params.get('beta', 0.4) # peak engine rolloff
23+
24+
return np.clip(Tm * (1 - beta * (omega/omega_m - 1)**2), 0, None)
25+
26+
27+
# Vehicle dynamics
28+
def vehicle_update(t, x, u, params={}):
29+
"""Vehicle dynamics for cruise control system.
30+
31+
Parameters
32+
----------
33+
x : array
34+
System state: car velocity in m/s
35+
u : array
36+
System input: [throttle, gear, road_slope], where throttle is
37+
a float between 0 and 1, gear is an integer between 1 and 5,
38+
and road_slope is in rad.
39+
40+
Returns
41+
-------
42+
float
43+
Vehicle acceleration
44+
45+
"""
46+
from math import copysign, sin
47+
sign = lambda x: copysign(1, x) # define the sign() function
48+
49+
# Set up the system parameters
50+
m = params.get('m', 1600.) # vehicle mass, kg
51+
g = params.get('g', 9.8) # gravitational constant, m/s^2
52+
Cr = params.get('Cr', 0.01) # coefficient of rolling friction
53+
Cd = params.get('Cd', 0.32) # drag coefficient
54+
rho = params.get('rho', 1.3) # density of air, kg/m^3
55+
A = params.get('A', 2.4) # car area, m^2
56+
alpha = params.get(
57+
'alpha', [40, 25, 16, 12, 10]) # gear ratio / wheel radius
58+
59+
# Define variables for vehicle state and inputs
60+
v = x[0] # vehicle velocity
61+
throttle = np.clip(u[0], 0, 1) # vehicle throttle
62+
gear = u[1] # vehicle gear
63+
theta = u[2] # road slope
64+
65+
# Force generated by the engine
66+
67+
omega = alpha[int(gear)-1] * v # engine angular speed
68+
F = alpha[int(gear)-1] * motor_torque(omega, params) * throttle
69+
70+
# Disturbance forces
71+
#
72+
# The disturbance force Fd has three major components: Fg, the forces due
73+
# to gravity; Fr, the forces due to rolling friction; and Fa, the
74+
# aerodynamic drag.
75+
76+
# Letting the slope of the road be \theta (theta), gravity gives the
77+
# force Fg = m g sin \theta.
78+
79+
Fg = m * g * sin(theta)
80+
81+
# A simple model of rolling friction is Fr = m g Cr sgn(v), where Cr is
82+
# the coefficient of rolling friction and sgn(v) is the sign of v (±1) or
83+
# zero if v = 0.
84+
85+
Fr = m * g * Cr * sign(v)
86+
87+
# The aerodynamic drag is proportional to the square of the speed: Fa =
88+
# 1/2 \rho Cd A |v| v, where \rho is the density of air, Cd is the
89+
# shape-dependent aerodynamic drag coefficient, and A is the frontal area
90+
# of the car.
91+
92+
Fa = 1/2 * rho * Cd * A * abs(v) * v
93+
94+
# Final acceleration on the car
95+
Fd = Fg + Fr + Fa
96+
dv = (F - Fd) / m
97+
98+
return dv
99+
100+
# Vehicle input/output model
101+
vehicle_dynamics = ct.nlsys(
102+
vehicle_update, None, name='vehicle',
103+
inputs = ['u', 'gear', 'theta'], outputs = ['v'], states=['v'])
104+
105+
#
106+
# PI controller
107+
#
108+
# The controller for the system is designed in Example XX, but is included
109+
# here since it is needed for some earlier examples.
110+
#
111+
112+
# Nominal controller design for remaining analyses
113+
# Construct a PI controller with rolloff, as a transfer function
114+
Kp = 0.5 # proportional gain
115+
Ki = 0.1 # integral gain
116+
PI_control = ct.tf(
117+
[Kp, Ki], [1, 0.01*Ki/Kp], name='control', inputs='u', outputs='y')
118+
119+
cruise_PI = ct.interconnect(
120+
(vehicle_dynamics, PI_control), name='cruise',
121+
connections = [['control.u', '-vehicle.v'], ['vehicle.u', 'control.y']],
122+
inplist = ['control.u', 'vehicle.gear', 'vehicle.theta'],
123+
inputs = ['vref', 'gear', 'theta'],
124+
outlist = ['vehicle.v', 'vehicle.u'], outputs = ['v', 'u'])

0 commit comments

Comments
 (0)