Vehicle steering equations
OngoingHello,
I have a vehicle model driving on the highway which accelerates to max speed. I would like to control vehicle steering angle as well but could not finalize equations and objective function? And also I am not sure how to get two outputs at the same time from the objective function?
import cvxpy as cvx
import numpy as np
import matplotlib.pyplot as plt
import math
import time
N = 24 # time steps to look ahead
path = cvx.Variable((N, 2)) # initialize the y pos and y velocity
flap = cvx.Variable(N1, boolean=True) # initialize the inputs, whether or not the bird should flap in each step
acc = cvx.Variable(N1, boolean=True)
angle_list=cvx.Variable(N1, boolean=True)
last_solution = [False, False, False] # seed last solution
last_path = [(0,0),(0,0)] # seed last path
PIPEGAPSIZE = 100 # gap between upper and lower pipe
PIPEWIDTH = 34
BIRDWIDTH = 34
BIRDHEIGHT = 16
BIRDDIAMETER = np.sqrt(BIRDHEIGHT**2 + BIRDWIDTH**2) # the bird rotates in the game, so we use it's maximum extent
SKY = 50 # location of sky
GROUND =120 #(150*0.79)1 # location of ground
PLAYERX = 0#180 # location of bird
theta=0
def getPipeConstraintsDistance(x, y, upperPipes):
constraints = [] # init pipe constraint list
pipe_dist = 0 # init dist from pipe center
for pipe in upperPipes:
dist_from_front = pipe['x']  x  BIRDDIAMETER
dist_from_back = pipe['x']  x + PIPEWIDTH
if (dist_from_front < 0) and (dist_from_back > 0):
constraints += [y <= (pipe['y']  BIRDDIAMETER)] # y above lower pipe
constraints += [y >= (pipe['y']  PIPEGAPSIZE)] # y below upper pipe
pipe_dist += cvx.abs(pipe['y']  (PIPEGAPSIZE//2)  (BIRDDIAMETER//2)  y) # add distance from center
return constraints, pipe_dist
def solve(playerx,playery,playerVel,angle):
playerAcc = 0.1# 0.1 # players accleration
playerFlapAcc = 1 # players speed on flapping
# unpack path variables
y = path[:,0]
vy = path[:,1]
c = [] # init constraint list
#print('ilk cccc',c)
c += [y <= GROUND, y >= SKY] # constraints for sky and ground
#print('2. cccc',c)
c += [y[0] == playery, vy[0] == playerVel] # initial conditions
#print('3....ccccccccccccccccccc',c)
obj = 0
print('playerx',playerx)
x = playerx
xs = [x] # init x list
for t in range(N1): # look ahead
dt = t//15 + 1 # let time get coarser further in the look ahead
playerVel+=playerAcc*acc[t]*dt
#x += playerVel*math.cos(angle)*angle_list[t]*dt # update x position
#print('x',x)
#time.sleep(5)
xs += [x] # add to list
#c += [vy[t + 1] == vy[t] + playerVel*math.sin(angle)*angle_list[t]*dt ]# playerFlapAcc * flap[t] ,+ playerAccY * dt # add y velocity constraint, f=ma
#c += [y[t + 1] == y[t] + vy[t + 1]*dt ] # add y constraint, dy/dt = a
#pipe_c, dist = getPipeConstraintsDistance(x, y[t+1], upperPipes) # add pipe constraints
#c += pipe_c
#c += [playerVel<=100]
#obj += dist
print('playervel',playerVel)
#objective = cvx.Minimize(cvx.sum(flap) + 0.5* cvx.sum(cvx.abs(vy))) # minimize total flaps and y velocity
objective = cvx.Maximize(playerVel)#+cvx.Minimize(cvx.sum(angle_list))
#print(objective)
#objective = cvx.Minimize(cvx.sum(cvx.abs(vy)) + 100* obj)
#print('4..ccccccccccccccccccc',c)
prob = cvx.Problem(objective, c) # init the problem
try:
#prob.solve(verbose = False) # use this line for open source solvers
prob.solve(verbose = False, solver="GUROBI") # use this line if you have access to Gurobi, a faster solver
#print('burdamiyim')
#y.value is output of prob.solve check cvxpy documentation for more
#print('xs',xs)
#print('y.value',y.value)
last_path = list(zip(xs, y.value)) # store the path
#print('last_path',last_path)
last_solution = np.round(acc.value).astype(bool) # store the solution
#print('last solution',acc.value)
#print('last solution',np.round(acc.value))
#print('last solution0000',last_solution[0])
#print('last sol sonrasi')
return last_solution[0], last_path # return the next input and path for plotting
except:
try:
last_solution = last_solution[1:] # if we didn't get a solution this round, use the last solution
last_path = [((x4), y) for (x,y) in last_path[1:]]
return last_solution[0], last_path
except:
return False, [(0,0), (0,0)] # if we fail to solve many times in a row, do nothing

Hi Mustafa,
could you please specify what is the difficulty you are dealing with? What is the model you are trying to implement? Which two outputs are you trying to extract from your objective function?
Best regards
Jonasz0 
The main difficulty, I can set a boolean variable for acceleration
acc = cvx.Variable(N1, boolean=True)
which true accelerates false don't.
But for steering angle how I can model turn left, turn right or do nothing. How should I define it?
The main model works like the below:
if acc: #output from Gurobi( True or False)
moved = True
player_car.move_forward()
# if angle_left:
# player_car.rotate(left=True)
# if angle_right:
# player_car.rotate(right=True)So when writing vehicle equations should I consider both options in the equation?
for t in range(N1): # look ahead
dt = t//15 + 1 # let time get coarser further in the look ahead
playerVel+=playerAcc*acc[t]*dt
x += playerVel*math.cos(angle)*angle_left[t]*dt + playerVel*math.cos(angle)*angle_right[t]*dt # update x position
y += playerVel*math.sin(angle)*angle_left[t]*dt + playerVel*math.sin(angle)*angle_right[t]*dt # update x position0 
Hi Mustafa,
Could you please share a formulation of the mathematical model (LP, MIP, ...) you are trying to solve? Rather than code, a mathematical formulation would be useful.
An angle, in which a vehicle should move, could be modeled, for example, by a continuous or integer variable in the range \([0,180]\). But without understanding the full model I can't give you any further guidance.
Best regards
Jonasz1 
Hello Jonasz,
I added vehicle equations below.
Thanks for the help.
0 
If my understanding of your problem is correct, these equations are used to update the position of a car. I also infer that you want Gurobi to tell you the acceleration and vehicle's angle. What we need to know is
 What is the overall aim of this code, i.e. what is the objective? What should be optimized in each step?
 Which constraints should be taken into account when deciding about the acceleration and angle in each step?
Best regards
Jonasz0 
I have a red car below and would like to reach the finish line.
Objective function= Minimize(x_carX_finish)
For constraints:
 Road boundaries, the top and bottom red lines onroad
 green vehicles.
0 
def solve(playerx,playery,playervel_hiz,angle,computercar_x,computercar_y):
playerAcc = 0.1# 0.1 # players accleration
# unpack path variables
y = path[:,0]
vy = path[:,1]
x=path[:,2]
playerVel=path[:,3]
c = [] # init constraint list
#print('ilk cccc',c)
#c += [y <= RightRoadBorder, y >= LeftRoadBorder] # constraints for highway boundaries
#c += [y <= secondLaneBorder] # constraints for highway boundaries
#c += [y[0] == playery, vy[0] == playerVel] # initial conditions vx[0]==playerVel,x[0]==playerx,
c += [x[0] == playerx, playerVel[0] == playervel_hiz] # initial conditions vx[0]==playerVel,x[0]==playerx,
obj = 0
#y = playery
#ys = [y] # init x list
for t in range(N1): # look ahead
dt = t//15 + 1 # let time get coarser further in the look ahead
c += [playerVel[t+1] == playerVel[t] + playerAcc*acc[t]*dt]
c += [y[t + 1] == y[t] + playerVel[t]*math.sin(angle)*dt ] # add y constraint, dy/dt = a
c += [x[t + 1] == x[t] + playerVel[t]*math.cos(angle)*dt ] # add y constraint, dy/dt = a
#vehicle_c, dist = getVehicleConstraintsDistance(x, y[t+1], computercar_x,computercar_y) # add pipe constraints
#c += vehicle_c
#c += [playerVel<=100]
#obj += dist
objective = cvx.Maximize( cvx.sum(acc))#cvx.Maximize(playerVel)#+cvx.Minimize(cvx.sum(angle_list))
prob = cvx.Problem(objective, c)Now the model is like this, how I should add angle to the objective or how I should get output from it?
0 
Hi Mustafa,
I am afraid I still have too little information to help you. You could try writing down the exact mathematical model to be solved with each iteration, before updating the values for v, x, y. We might be then able to help you.
You could also look for inspiration e.g. here. Maybe their approaches could serve as inspiration.
Also, to query the solution via cvxpy you could first optimize and  assuming a solution is found  iterate over your variables and query their value parameter. You can find more information here.
Best regards
Jonasz0 
Hello Jonasz,
I have shared mathematical equations in the previous post.
I think you are asking x_dot=Ax +bu form of equations but do I need this?
Because currently, my model works for acceleration with the current setup. I am not sure how to get output for the steering angle!
Thanks
0 
Hi Mustafa,
Some more questions and thoughts:
1) Why do you model angles as binary variables?
angle_list=cvx.Variable(N1, boolean=True)
Don't they have to take some other value, for example between 0 and 180 degrees or 0 and 2*pi radians?
2) Gurobi allows for the inclusion of trigonometric functions (for example, sine). To be exact, a piecewise linearization of the function is added, but you can control its granularity. I am no expert in cvxpy, and hence can't tell you how to access this Gurobispecific constructor from cvxpy.
3) Once you include variables for angle and the appropriate constraints (as discussed in points 1) and 2)) you should be able to optimize and then query each variable and its value (assuming the optimization is successful). You can find an example in one of my posts above.
Hope this helps.
Best regards
Jonasz0 
1) Why do you model angles as binary variables?
left_angle=cvx.Variable(N1, boolean=True)
right_angle=cvx.Variable(N1, boolean=True)My main vehicle model changes angle 1 by one so I was thinking to get True or False for angle and if the left angle is true rotate one degree left or if the right angle is true, rotate one degree right.
if Acc:
moved = True
player_car.move_forward()
if left_angle:
player_car.rotate(left=True)
if right_angle:
player_car.rotate(right=True)c += [playerVel[t+1] == playerVel[t] + playerAcc*acc[t]*dt]
c += [angle == angle + 1*left_angle[t]  1*right_angle[t] ]
#angle=math.radians(angle)
c += [y[t + 1] == y[t] + playerVel[t]*math.sin(angle)*dt ] # add y constraint, dy/dt = a
c += [x[t + 1] == x[t] + playerVel[t]]*math.cos(angle)*dt ] # add y constraint, dy/dt = a0
Please sign in to leave a comment.
Comments
11 comments