EDIT2: Alright, I'm an idiot. My math on lateral error was questionable. I've rewritten that equation to be something simpler and it's working now. I'll leave here in case it's useful for anyone. But if possible I would still love for someone to make sure I'm not doing anything egregious, for a sanity check.
EDIT:I fixed the issue i was having with heading_error by making it a GEKKO.Var instead of GEKKO.Intermediate. Now that that's working I've tried to incorporate the lateral error part of the equation. ANd now the solution fails. I think it might having something to do with the way I'm using variables inside the intermediate equation, but I'm not totally sure. I've replaced the BikeSolver.py with my current version below.
Before I state my problem let me explain what I am trying to do here with GEKKO and maybe some of my equation modeling methods could be improved to fix whatever is going wrong.
I have an autonomous vehicles student team working on the IGVC competition. Before we start doing anything on the car directly I want to have a robust simulation system that can provide us a general understand of how different control algorithms will perform. In this case I am starting by building a foundation for the students to start with, then they can go ahead and test different control algorithms etc.. Eventually the plan will be to use GEKKO in realtime generating an MPC control algorithm. But for now I want the students to get a good grasp on other feedback control methods before we jump into model based optimization.
Okay so here is the general idea. We are using the simplified kinematic bike model to model the dynamics of the entire car. The gekko solver receives a trajectory setup with a few discrete states (positions and velocities, we are controlling on the xy plane), then this trajectory is interpolated using the BPoly class from scipy to give us the trajectory as a bunch of different GEKKO.parameters (such as x_interp, y_interp, vx_interp, vy_interp, yaw_interp, vmag_interp). This all works great and I was luckily able to figure out how to have this method work for varying solver.time vectors.
Finally I would like to use some vector math to generate steering input controls. For example using a "heading error" to generate a steering input as well as a "lateral error". These two methods require some cross-products and what not to get useful values to turn into a steering command.
So I'm having an issue right now where I'm calculating a simple heading error as heading_error = yaw_interp - yaw, where yaw_interp is the yaw interpolated from the desired trajectory, and yaw is the states orientation variable. For some reason when I set my steering variable (delta) equivalent to the heading_error, the solution fails if I don't multiply heading_error by 2. Super weird. This is line 118 in BikeSolver.py.
So just curious what anyone thinks the mistake I've made it, and if there is a more elegant/robust way to use GEKKO for simulations like this.
Note, both of these files should be in a folder called reference_code. Line 9-12 on BikeSolver.py imports Physics.py by appending the reference_code folder to the system path. This was the quickest solution to have this work on all the students computers.
Best, Michael Giglia
BikeSolver.py-------------------------
from gekko import GEKKO
import numpy as np
import matplotlib.pyplot as plt
import sys
import os
import linecache
# Deal with python relative importing stuff
PACKAGE_PARENT = '..'
SCRIPT_DIR = os.path.dirname(os.path.realpath(os.path.join(os.getcwd(), os.path.expanduser(__file__))))
sys.path.append(os.path.normpath(os.path.join(SCRIPT_DIR, PACKAGE_PARENT)))
from reference_code.Physics import *
def PrintException():
exc_type, exc_obj, tb = sys.exc_info()
f = tb.tb_frame
lineno = tb.tb_lineno
filename = f.f_code.co_filename
linecache.checkcache(filename)
line = linecache.getline(filename, lineno, f.f_globals)
print('EXCEPTION IN ({}, LINE {} "{}"): {}'.format(filename, lineno, line.strip(), exc_obj))
class BikeSolver():
def __init__(self):
self.solver = GEKKO(remote=False)
self.ntd = 31
self.solver.time = np.linspace(0,100, self.ntd)
# Add time variable that way we can pass into the poly() function and return the interpolated positions (instead i've used the solver.time numpy array)
#self.t = self.solver.Var(value=0); self.solver.Equation(self.t.dt() == 1)
self.solver.options.NODES = 3
self.solver.options.SOLVER = 3 #Solver Type IPOPT
self.solver.options.IMODE = 6 #MPC solution mode, will manipulate MV's to minimize Objective functions
# self.solver.options.MAX_ITER = 500
# final time for optimizer (currently unused)
self.tf = self.solver.FV(value=1.0,lb=0.1,ub=100.0)
# allow gekko to change the tf value
self.tf.STATUS = 0
def solve_open_loop(self, v0, d0, L):
# Initialize all state variables
self.x = self.solver.Var(value = 0)
self.y = self.solver.Var(value = 0)
self.yaw = self.solver.Var(value = 0) # state orientation variable
self.vx = self.solver.Var(value = v0*np.cos(0))
self.vy = self.solver.Var(value = v0*np.sin(0))
self.omega_yaw = self.solver.Var(value = (np.tan(d0)/L))
self.v_mag = self.solver.Param(value = v0)
self.delta = self.solver.Var(value = d0, ub=30, lb=-30) #steering angle
self.L = self.solver.Param(value = L)
# Add manipulatable variables
self.delta_control = self.solver.MV(value = self.delta)
self.delta_control.STATUS = 0 # Allow GEKKO to change this value
self.solver.Equation(self.vx == self.v_mag * self.solver.cos(self.yaw))
self.solver.Equation(self.vy == self.v_mag * self.solver.sin(self.yaw))
self.solver.Equation(self.omega_yaw == (self.solver.tan(self.delta_control) / self.L))
self.solver.Equation(self.x.dt() == self.vx)
self.solver.Equation(self.y.dt() == self.vy)
self.solver.Equation(self.yaw.dt() == self.omega_yaw)
# self.solver.Equation(self.delta == "pure pursuit equation")
self.solver.Obj((self.delta - self.delta_control)**2)
self.solver.solve(disp=True) # Solve the system and display results to terminal
def solve_cubic_spline_traj(self, q: Trajectory, s0: State, c0: ControlInput, L):
# Initialize all state variables
self.x = self.solver.Var(value = s0.position.x) # X position
self.y = self.solver.Var(value = s0.position.y) # Y positoin
self.yaw = self.solver.Var(value = s0.orientation.z) # state orientation variable
self.vx = self.solver.Var(value = s0.velocity.x) # x velocity
self.vy = self.solver.Var(value = s0.velocity.y) # y velocity
self.omega_yaw = self.solver.Var(fixed_initial=False) # angular velocity
self.v_mag = self.solver.Var(value = s0.velocity.magnitude(), lb=0.0) # velocity magnitude
self.delta = self.solver.Var(fixed_initial=False) #steering angle (without bounds)
self.L = self.solver.Param(value = L) # Wheel base parameter
# Add manipulatable variables
self.delta_control = self.solver.MV(ub=np.pi/4, lb=-np.pi/4) # Steering angle with bounds
self.delta_control.DCOST = 0.0 # Penalize changing steering angle
self.delta_control.STATUS = 1 # Allow GEKKO to change this value
self.solver.free_initial(self.delta_control)
self.a = self.solver.MV(value = 0.0, lb=-2, ub=2) # Linear acceleration to adjust v_mag
self.a.DCOST = 1e-8 # Small Penilzation on changing acceleration value
self.a.STATUS = 1
self.solver.free_initial(self.a)
# Get poly spline form of trajectory
self.x_poly, self.y_poly = TrajectoryInterpolator.interpolate(q) # Interpolate discrete trajectory
self.x_interp = self.solver.Param(value = self.get_interp(self.x_poly, self.solver.time)) # X position of trajectory vs time
self.y_interp = self.solver.Param(value = self.get_interp(self.y_poly, self.solver.time)) # Y position of trajectory
self.vx_interp = self.solver.Param(value = self.get_interp(self.x_poly.derivative(), self.solver.time)) # X velocity of trajectory
self.vy_interp = self.solver.Param(value = self.get_interp(self.y_poly.derivative(), self.solver.time)) # Y velocity of trajectory
self.vmag_interp = self.solver.Param(value = self.get_vmag(self.vx_interp, self.vy_interp)) # Velocity magnitude of trajectory
self.vx_unit_interp = self.solver.Param(value = self.vx_interp.value/self.vmag_interp.value) # X velocity unit vector for geometric control equations
self.vy_unit_interp = self.solver.Param(value = self.vy_interp.value/self.vmag_interp.value) # Y velocity unit vector for geometric control equations
self.yaw_interp = self.solver.Param(value = self.get_yaw(self.vx_unit_interp, self.vy_unit_interp)) # Orientation of trajectroy for geometric control equations
# Lateral error variables
self.x_traj_error = self.solver.Intermediate(equation = (self.x_interp - self.x))
self.y_traj_error = self.solver.Intermediate(equation = (self.y_interp - self.y))
self.lat_error = self.solver.Var(fixed_initial = False) #equation = ((self.x_interp - self.x)*self.vy/self.v_mag) - ((self.y_interp - self.y)*self.vx/self.v_mag))
# Heading error variables
self.heading_error = self.solver.Var(fixed_initial=False)
# Steering input equation
self.solver.Equation(self.heading_error == self.yaw - self.yaw_interp)
self.solver.Equation(self.lat_error**2 == ((self.x_interp - self.x)**2) + ((self.y_interp - self.y)**2))
self.solver.Equation(self.delta == self.heading_error + 0.1*self.lat_error)
# Define system model equations
self.solver.Equation(self.v_mag.dt() == self.a)
self.solver.Equation(self.vx == self.v_mag * self.solver.cos(self.yaw))
self.solver.Equation(self.vy == self.v_mag * self.solver.sin(self.yaw))
self.solver.Equation(self.omega_yaw == self.v_mag * (self.solver.tan(self.delta_control) / self.L))
self.solver.Equation(self.yaw.dt() == self.omega_yaw)
self.solver.Equation(self.x.dt() == self.vx)
self.solver.Equation(self.y.dt() == self.vy)
self.solver.Obj((self.delta - self.delta_control)**2) # Force solver to set steering angle to delta variable
# self.solver.Obj((self.x_interp - self.x)**2 + (self.y_interp - self.y)**2) # Let solver find best acceleration and steering inputs
self.solver.Obj((self.vmag_interp - self.v_mag)**2) # Follow velocity described by trajectory
# self.solver.open_folder()
self.solver.solve(disp=True) # Solve the system and display results to terminal
# self.solver.solve()
def get_interp(self, poly, t):
return poly(t)
def get_vmag(self, vx, vy):
return np.sqrt(vx.value**2 + vy.value**2)
def get_yaw(self, vx, vy):
return np.arcsin(vy.value)
def plot_data(self):
"""Will plot
"""
num_subplots = 7
fig = plt.figure(2)
plt.subplot(num_subplots, 1, 1)
plt.plot(self.solver.time, self.x.value, 'r-')
plt.plot(self.solver.time, self.x_interp.value, 'r.')
plt.ylabel('x pos')
plt.subplot(num_subplots, 1, 2)
plt.plot(self.solver.time, self.y.value, 'b-')
plt.plot(self.solver.time, self.y_interp.value, 'b.')
plt.ylabel('y pos')
# plt.ylim(-280, 280)
plt.subplot(num_subplots, 1, 3)
plt.plot(self.solver.time, self.yaw.value, 'g-')
plt.plot(self.solver.time, self.yaw_interp.value, 'g.')
plt.ylabel('yaw val')
plt.subplot(num_subplots, 1, 4)
plt.plot(self.solver.time, self.delta_control.value, 'g-')
plt.ylabel('steering val')
# plt.ylim(-280, 280)
plt.subplot(num_subplots, 1, 5)
plt.plot(self.x, self.y, 'g-')
plt.plot(self.x_interp, self.y_interp, 'r.')
plt.ylabel('xy pos')
plt.subplot(num_subplots, 1, 6)
plt.plot(self.solver.time, self.a, 'y-')
plt.ylabel('acceleration')
plt.subplot(num_subplots, 1, 7)
plt.plot(self.solver.time, self.v_mag, 'c-')
plt.plot(self.solver.time,self.vmag_interp, 'c.')
plt.ylabel('v_mag')
plt.draw()
plt.ion()
plt.show()
plt.pause(0.001)
class Solver():
def __init__(self):
self.solver = GEKKO(remote=False)
ntd = 500
self.solver.time = np.linspace(0,50, ntd)
self.solver.options.NODES = 2
self.solver.options.SOLVER = 3 #Solver Type IPOPT
self.solver.options.IMODE = 6 #MPC solution mode, will manipulate MV's to minimize Objective functions
def solve_pid(self, x0, v0, kp, ki, kd, xd):
"""Look into slack variables to make this probelm solving quicker.
Args:
x0 ([type]): [description]
v0 ([type]): [description]
kp ([type]): [description]
ki ([type]): [description]
kd ([type]): [description]
xd ([type]): [description]
"""
self.x = self.solver.Var(value = x0) # Position of system
self.v = self.solver.Var(value = v0) # Velocity of system
self.e = self.solver.Var(value = (xd-x0)) # Positional error of system
self.pid_output = self.solver.Var() # Output of PID algorithm
self.a = self.solver.MV(value = self.pid_output, lb=-10000, ub=10000) # Acceleration input to system
self.a.STATUS = 1 # Allow GEKKO to change this value
self.solver.Equation(self.e == xd - self.x) # Define error function
self.solver.Equation(self.x.dt() == self.v) # Define derivative of position to equal velocity
self.solver.Equation(self.v.dt() == self.a) # Define derivative of velocity to equal acceleration
self.solver.Equation(self.pid_output == (self.e*kp) + (self.e.dt()*kd)) # Define pid_output from error multipled by gains (passed into this function)
self.solver.Obj((self.pid_output - self.a)**2) # Objective function to force acceleration to equal pid_output
# I had to do this so that I could bound acceleration, for some reason was getting erros when useing IMODE=4 (simulation mode)
# And bounding variables, so I made acceleration an MV with bounds, and just penalized the solver for deviating from
# making acceleration != pid_output, squared error which is typical to remove sign
self.solver.solve(disp=True) # Solve the system and display results to terminal
def plot_data(self):
"""Will plot
"""
fig = plt.figure(1)
plt.subplot(3, 1, 2)
plt.plot(self.solver.time, self.v.value, 'r-')
plt.ylabel('velocity')
plt.subplot(3, 1, 1)
plt.plot(self.solver.time, self.x.value, 'r-')
plt.ylabel('position')
plt.subplot(3, 1, 3)
plt.plot(self.solver.time, self.a.value, 'r-')
plt.ylabel('acceleration')
plt.draw()
plt.show(block=True)
class RobotSolver():
def __init__(self):
self.solver = GEKKO(remote=False)
ntd = 100
self.solver.time = np.linspace(0, 3, ntd)
self.solver.options.NODES = 2
self.solver.options.SOLVER = 3 #Solver Type IPOPT
self.solver.options.IMODE = 6 #MPC solution mode, will manipulate MV's to minimize Objective functions
def solve_pid(self, w0, wd, v_base, a1_bias, kp, kd):
"""[summary]
Args:
w0 ([type]): [description]
wd ([type]): [description]
v_base ([type]): [description]
a1_bias ([type]): [description]
kp ([type]): [description]
kd ([type]): [description]
"""
self.w = self.solver.Var(value = w0) # Angular Velocity of system
self.a1 = self.solver.MV(value = v_base, lb=-255, ub=255) # Power of Wheel 1
self.a1.STATUS = 1
self.a2 = self.solver.MV(value = v_base, lb=-255, ub=255) # Power of Wheel 2
self.a2.STATUS = 1
self.a1_unbound = self.solver.Var(value = v_base) # Unbounded velocities
self.a2_unbound = self.solver.Var(value = v_base)
self.e = self.solver.Var(value = wd - w0) # Error of angular velocity (wd - w) wd is desired angular velocity
self.pid_output = self.solver.Var(self.e*kp) # Output of PID algorithm
self.r = self.solver.Param(value = 2) # Distance from rotation center to wheel
self.v_base = self.solver.Param(value = v_base) # Base velocity of both wheels
self.solver.Equation(self.e == wd - self.w) # Error equation
self.solver.Equation(self.pid_output == self.e*kp + self.e.dt()*kd) # PID algorithm equation
self.solver.Equation(self.a1_unbound == self.v_base + self.pid_output) # Power of wheel 1 based off PID
self.solver.Equation(self.a2_unbound == self.v_base - self.pid_output) # VelocPowerity of wheel 2 based off PID
self.solver.Equation(self.w.dt() == (self.a2-self.a1) * self.r) # Derivative of angular velocity based on wheel velocities
self.solver.Obj((self.a2 - self.a2_unbound)**2) # Objective functions to allow bounding of variables
self.solver.Obj((self.a1 - self.a1_unbound)**2)
# self.solver.open_folder()
self.solver.solve(disp=True) # Solve the system of equations and objective functions
def plot_data(self):
"""Will plot
"""
fig = plt.figure(2)
plt.subplot(3, 1, 1)
plt.plot(self.solver.time, self.w.value, 'r-')
plt.ylabel('angular velocity')
plt.subplot(3, 1, 2)
plt.plot(self.solver.time, self.a1.value, 'b-')
plt.ylabel('a1')
# plt.ylim(-280, 280)
plt.subplot(3, 1, 3)
plt.plot(self.solver.time, self.a2.value, 'g-')
plt.ylabel('a2')
# plt.ylim(-280, 280)
plt.draw()
plt.show(block=True)
if __name__ == "__main__":
# s = Solver()
# s.solve_pid(x0=0, v0=0, kp=10, ki=0, kd=0, xd=10)
# print(s.x.value)
# s.plot_data()
# r = RobotSolver()
# r.solve_pid(w0 = 2, wd = 0, v_base = 100, a1_bias = 10, kp = 10, kd = 1)
# r.plot_data()
# b = BikeSolver()
# b.solve_open_loop(1, 1, 1)
# b.plot_data()
# print(b.x.value, b.y.value, b.yaw.value)
# Define a trajectory
t0 = 0
p0 = Vec3(0,0,0)
v0 = Vec3(0,5,0)
s0 = StateBuilder.build_from_pos_and_vel(t0, p0, v0)
t1 = 50
p1 = Vec3(50,0,0)
v1 = Vec3(5**0.5,5**0.5,0)
s1 = StateBuilder.build_from_pos_and_vel(t1, p1, v1)
t2 = 100
p2 = Vec3(100,0,0)
v2 = Vec3(0,1,0)
s2 = StateBuilder.build_from_pos_and_vel(t2, p2, v2)
q = TrajectoryBuilder.build_from_3_states(s0, s1, s2)
# Define controller initial input
c0 = ControlInput(0)
b = BikeSolver()
# b.solve_open_loop(1, 1, 1)
try:
b.solve_cubic_spline_traj(q, s0, c0, 1)
except:
print('did not work')
PrintException()
b.plot_data()
print(b)
Physics.py---------------------
import numpy as np
import scipy
from scipy.interpolate import BPoly
import matplotlib.pyplot as plt
class Vec3:
def __init__(self, x, y, z):
self.x = x
self.y = y
self.z = z
self.asnumpy = np.array([x, y, z])
def normal(self):
mag = self.magnitude()
return np.array([self.x/mag, self.y/mag, self.z/mag], dtype=np.float)
def magnitude(self):
return np.sqrt(self.x**2 + self.y**2 + self.z**2)
class ControlInput:
def __init__(self, delta: float):
self.delta = delta
class State:
time: float
position: Vec3
velocity: Vec3
orientation: Vec3
ang_vel: Vec3
def __init__(self, t, p, v, o, a):
self.time = t
self.position = p
self.velocity = v
self.orientation = o
self.ang_vel = a
class StateBuilder:
def __init__(self):
pass
@staticmethod
def build_from_pos_and_vel(t, pos: Vec3, vel: Vec3):
# We know based off the bike kinematics that the car's orientation is always aligned with the car's velocity.
# Knowing this we can calculate the orientation from velocity directly.
# Calculate orientation from velocity crossed with unit x vector
ux = Vec3(1,0,0).asnumpy
# Get normalized velocity to cross with ux
v_norm = vel.normal()
# Cross v_norm x ux then arcsin to get yaw val in radians
yaw = np.arcsin(np.cross(ux, v_norm))
# Setup orientation vector as roll,pitch,yaw vector
orientation = Vec3(0,0,yaw[2])
return State(t, pos, vel, orientation, a=Vec3(0,0,0))
class Trajectory:
states: list
def __init__(self, s:list):
self.states = s
class TrajectoryInterpolator:
def __init__(self):
pass
@staticmethod
def interpolate(q: Trajectory):
# Generate BPoly class from trajectory states
# Setup time vector, position vectors, first derivative vectors
t = []
x = []
x_dot = []
y = []
y_dot = []
for s in q.states:
t.append(s.time)
x.append(s.position.x)
x_dot.append(s.velocity.x)
y.append(s.position.y)
y_dot.append(s.velocity.y)
# Convert to numpy arrays
x = np.array(x, ndmin=2)
x_dot = np.array(x_dot, ndmin=2)
y = np.array(y, ndmin=2)
y_dot = np.array(y_dot, ndmin=2)
# Generate poly spline, np.hstack() sets up the arrays in the proper orientation for the method
x_poly = BPoly.from_derivatives(t, np.hstack([x.T, x_dot.T]), orders=3)
y_poly = BPoly.from_derivatives(t, np.hstack([y.T, y_dot.T]), orders=3)
return x_poly, y_poly
# Return BPoly class as tuple (x, y)
class TrajectoryBuilder:
def __init__(self):
pass
@staticmethod
def build_from_3_states(s0: State, s1: State, s2: State):
states = [s0, s1, s2]
return Trajectory(states)
if __name__ == "__main__":
t0 = 0
p0 = Vec3(-2,1,0)
v0 = Vec3(0,8**0.5,0)
s0 = StateBuilder.build_from_pos_and_vel(t0, p0, v0)
t1 = 1
p1 = Vec3(4,4,0)
v1 = Vec3(2,-5,0)
s1 = StateBuilder.build_from_pos_and_vel(t1, p1, v1)
t2 = 2
p2 = Vec3(6,5,0)
v2 = Vec3(8**0.5,0,0)
s2 = StateBuilder.build_from_pos_and_vel(t2, p2, v2)
q = TrajectoryBuilder.build_from_3_states(s0, s1, s2)
print(q)
xp, yp = TrajectoryInterpolator.interpolate(q)
t_interp = np.linspace(0,t2,101)
plt.figure(1)
x_i = []
y_i = []
x_dot_i = []
y_dot_i = []
for i in t_interp:
x_i.append(xp(i))
y_i.append(yp(i))
x_dot_i.append(xp.derivative()(i))
y_dot_i.append(yp.derivative()(i))
plt.plot(x_i, y_i)
plt.figure(2)
plt.plot(x_dot_i, y_dot_i, 'ro')
plt.show()
Michael, impressive application! I'm glad that you were able to resolve the problem. You asked for a review to make sure I'm not doing anything egregious, for a sanity check. Everything looks good in the current form. I did see a couple things that you may be planning to do:
tf
.Use the new
m.Minimize()
orm.Maximize()
functions instead ofm.Obj()
to make the model more readable.There is a cubic spline function in Gekko if you need to create an interpolated function as part of the solve. It appears that you are doing this after the solve currently. You can also get interpolated collocation points for improved solution visibility if you use
m.options.NODES=3
or higher andm.options.DIAGLEVEL=2
or higher to produceresults_all.json
in the local run directory withGEKKO(remote=False)
.