106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248 | class HyundaiGenesisDynamicsModel(object):
'''
A vehicle dynamics simulator using a linear tire model.
Modified Code from:
https://github.com/MPC-Berkeley/genesis_path_follower/blob/master/scripts/vehicle_simulator.py
https://github.com/urosolia/RacingLMPC/blob/master/src/fnc/SysModel.py
'''
def __init__(self):
# Hyundai Genesis Parameters from HCE:
self.param = HyundaiGenesisParameters()
self.state = StateSE3()
self.state.pose = PoseSE3()
self.state.pose.position = torch.tensor([[0.0,0.0,0.0]])
self.state.pose.orientation = torch.eye(3).unsqueeze(0)
self.state.twist = TwistSE3()
self.state.twist.linear = torch.tensor([[0.0,0.0,0.0]])
self.state.twist.angular = torch.tensor([[0.0,0.0,0.0]])
self.input = AckermannInput()
self.input.acceleration = torch.tensor([0.0])
self.input.steering_angle = torch.tensor([0.0])
self.dt = 0.01 # vehicle model update period (s) and frequency (Hz)
self.hz = int(1.0/self.dt)
self.desired_acceleration = 0.0 # m/s^2
self.desired_steering_angle = 0.0 # rad
@property
def ctrl_size(self):
return 3
@property
def state_size(self):
return 9
def setInput(self, acc, steer):
self.desired_acceleration = acc
self.desired_steering_angle = steer
def _fg_func(self, X_in):
if X_in.ndim == 1:
X = X_in.unsqueeze(0)
else:
X = X_in
assert X.shape[-1] == self.state_size
state, inp = StateAsArray().deserialize(X)
m, Iz, lf, lr = self.param.mass, self.param.Iz, self.param.lf, self.param.lr
vx, vy, w = (state.twist.linear[:, 0], state.twist.linear[:, 1],
state.twist.angular[:, 2])
# Compute tire slip angle and lateral force at front and rear tire (linear model)
Fyf, Fyr = self.tireLateralForce(state, inp)
fX = torch.zeros_like(X)
gX = X.new_zeros(X.shape[0], X.shape[-1], self.ctrl_size)
fX[:, :2] = dpos = state.pose.orientation.bmm(
state.twist.linear.unsqueeze(-1)).squeeze(-1)[:, :2]
fX[:, 2] = dori = state.twist.angular[:, 2]
#dX[:, 3] = dvx = (a - 1.0/m*Fyf*sins + w*vy)
#dX[:, 4] = dvy = (1.0/m*(Fyf*coss + Fyr) - w*vx)
#dX[:, 5] = dw = 1.0/Iz*(lf*Fyf*coss - lr*Fyr)
gX[:, 3, :], fX[:, 3] = torch.tensor([1, 0, - 1.0/m*Fyf]), w*vy
gX[:, 4, :], fX[:, 4] = torch.tensor([0, 1.0/m*Fyf, 0]), 1.0/m*Fyr - w*vx
gX[:, 5, :], fX[:, 5] = torch.tensor([0, 1.0/Iz*lf*Fyf, 0]), - 1.0/Iz*lr*Fyr
gX[:, 6:9, :] = torch.eye(self.ctrl_size)
if X_in.ndim == 1:
fX = fX.squeeze(0)
gX = gX.squeeze(0)
return fX, gX
def fu_func(self, X, U):
assert U.shape[-1] == self.ctrl_size
_, inp = StateAsArray().deserialize(X)
dU = self.controlDelay(self.dt, inp, U)
inp.inc_control(dU)
Ut = inp.control()
fX, gX = self._fg_func(X)
return fX + gX.bmm(Ut.unsqueeze(-1)).squeeze(-1)
def f_func(self, X):
return self._fg_func(X)[0]
def g_func(self, X):
return self._fg_func(X)[1]
def updateModel(self, disc_steps = 10):
deltaT = self.dt / disc_steps
U = torch.tensor([[
self.desired_acceleration,
math.cos(self.desired_steering_angle),
math.sin(self.desired_steering_angle)]])
#U = torch.tensor([[self.desired_acceleration, self.desired_steering_angle]])
dU = self.controlDelay(self.dt, self.input, U)
self.input.inc_control(dU)
for i in range(disc_steps):
R = self.state.pose.orientation
linear_twist = self.state.twist.linear
X = StateAsArray().serialize(self.state, self.input)
dX = self.fu_func(X, U)
# discretized SE(3) dynamics
dstate, dinp = StateAsArray().deserialize(dX * deltaT)
# self.state.pose.orientation @ self.state.twist.linear
self.state.pose.position += dstate.pose.position
self.state.pose.orientation = self.state.pose.orientation @ dstate.pose.orientation
self.state.twist.linear += dstate.twist.linear
self.state.twist.angular += dstate.twist.angular
def tireLateralForce(self, state, inp):
''' compute tire slip angle and lateral force '''
alpha_f, alpha_r = 0.0, 0.0
vx, vy, w = state.twist.linear[:, 0], state.twist.linear[:, 1], state.twist.angular[:, 2]
if torch.abs(vx) > 1.0: # np.fabs
alpha_f = inp.steering_angle - torch.atan2(vy + self.param.lf*w, vx)
alpha_r = -torch.atan2(vy - self.param.lr*w, vx)
return self.param.C_alpha_f * alpha_f, self.param.C_alpha_r * alpha_r
def controlDelay(self, dt, inp, dctrl):
''' Simulate first order control delay in acceleration/steering '''
# e_<n> = self.<n> - self.<n>_des
# d/dt e_<n> = - kp * e_<n>
# or kp = alpha, low pass filter gain
# kp = alpha = discretization time/(time constant + discretization time)
#ad = self.desired_acceleration
#sd = self.desired_steering_angle
ad = dctrl[:, 0]
sd = torch.atan2(dctrl[:, 2], dctrl[:, 1])
atc = self.param.acceleration_time_constant
stc = self.param.steering_angle_time_constant
ctc = self.param.control_time_constant
da = dt/(dt + atc) * (ad - inp.acceleration)
ds = dt/(dt + stc) * (sd - inp.steering_angle)
#du = dt/(dt + ctc) * (dctrl - inp.control())
inp = AckermannInput()
inp.acceleration = da
inp.steering_angle = ds
return inp.control()
|