def model(self, states, u):
"""
Front wheel drive.
Input: x - state vector (x, y, theta, phi)
u - controls (speed, steering_angle)
Output: system - system states
"""
vel = u[0]
w = u[1]
# set contraints
vel = utils.constrain_value(vel, self.min_vel, self.max_vel)
w = utils.constrain_value(w, self.min_steering_vel,
self.max_steering_vel)
x = states[0, 0]
y = states[1, 0]
theta = states[2, 0]
phi = states[3, 0]
system = np.array([[vel * np.cos(theta) * np.cos(phi)],
[vel * np.sin(theta) * np.cos(phi)],
[vel * np.sin(phi) / self.L], [w]])
return system
def model(self, states, u):
"""
Extended bicycle model.
Input: x - state vector (x, y, theta, velocity, steering angle)
u - controls (acceleration, steering velocity)
params - parameters (distance from front and back axis)
Output: system - system states
"""
a = u[0] # velocity control
w = u[1]
# set contraints
a = utils.constrain_value(a, self.min_acc, self.max_acc)
w = utils.constrain_value(w, self.min_steering_vel,
self.max_steering_vel)
x = states[0, 0]
y = states[1, 0]
theta = states[2, 0]
vel = states[3, 0]
vel = utils.constrain_value(vel, self.min_vel, self.max_vel)
phi = utils.constrain_value(states[4, 0], self.min_steering_angle,
self.max_steering_angle)
system = np.array([[vel * np.cos(theta)], [vel * np.sin(theta)],
[utils.truncate_angle(vel * np.tan(phi) / self.L)],
[a], [w]])
return system
def model(self, states, u):
"""
Simple bicycle/car nonlinear system.
Input: x - state vector (x, y, theta)
u - controls (speed, steering_angle)
Output: system - system states
"""
v = u[0] # velocity control
df = math.atan(u[1] * self.L /
v) # transformation of steering angle to robots heading
# set contraints
vel = utils.constrain_value(v, self.min_vel, self.max_vel)
df = utils.constrain_value(df, self.min_steering_angle,
self.max_steering_angle)
# states
x = states[0, 0]
y = states[1, 0]
theta = states[2, 0]
# update
system = np.array([[vel * np.cos(theta)], [vel * np.sin(theta)],
[utils.truncate_angle(vel * np.tan(df) / self.L)]])
return system
def system_constraints(self, states, controls):
states[2, 0] = utils.truncate_angle(
states[2, 0]) # orientation from -pi to pi
controls[0] = utils.constrain_value(controls[0], self.min_vel,
self.max_vel) # constrain velocity
controls[1] = utils.constrain_value(
controls[1], self.min_steering_angle,
self.max_steering_angle) # constrain steering angle
return states, controls
def system_constraints(self, states, controls):
states[2, 0] = utils.truncate_angle(
states[2, 0]) # orientation from -pi to pi
states[3, 0] = utils.constrain_value(states[3, 0], self.min_vel,
self.max_vel)
states[4, 0] = utils.constrain_value(
states[4, 0], self.min_steering_angle,
self.max_steering_angle) # orientation from -pi to pi
controls[0] = utils.constrain_value(controls[0], self.min_acc,
self.max_acc) # constrain acc
controls[1] = utils.constrain_value(
controls[1], self.min_steering_vel,
self.max_steering_vel) # constrain acc
return states, controls
def model(self, states, u):
"""
Differential Drive model.
Input: x - state vector (x, y, theta)
u - controls (speed, steering_angle)
params - parameters (wheel radius)
Output: system - system states
"""
v = u[0]
w = u[1]
# set contraints
v = utils.constrain_value(v, self.min_vel, self.max_vel)
x = states[0, 0]
y = states[1, 0]
theta = states[2, 0]
phi = states[3, 0]
system = np.array([[self.r * v * np.cos(theta)],
[self.r * v * np.sin(theta)],
[utils.truncate_angle(self.r * phi / self.L)], [w]])
return system