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 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
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
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
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 lqr_vel_steer_control(self, robot):
# robot.lookahead_idx = 25
self.Q = np.eye(5)
self.R = np.eye(2)
robot.goal = self.look_ahead(robot)
######## Linearization ########
A, B = robot.jacobi()
######## DARE ########
X = self.solve_DARE(A, B)
######## LQR solution ########
lqr_k = self.solve_lqr(X, A, B)
error = robot.error()
ustar = -lqr_k @ error
# calc steering input
delta = utils.truncate_angle(ustar[1])
delta = (delta - robot.states[4, 0]) / robot.dT
return np.array([ustar[0], delta])
def lqr_steer_control(self, robot):
self.Q = np.eye(robot.states.shape[0])
self.R = np.eye(2)
robot.goal = self.look_ahead(robot)
######## Linearization ########
vel = utils.euclidean_distance(robot.states, robot.goal)
A, B = robot.jacobi()
######## DARE ########
X = self.solve_DARE(A, B)
######## LQR solution ########
lqr_k = self.solve_lqr(X, A, B)
error = robot.error()
ustar = -lqr_k @ error
# calc steering input
delta = utils.truncate_angle(ustar[1])
return np.array([vel, delta])