Discrete path tracking with python gekko

2
votes

I have some discrete data points representing a path and I want to minimize the distance between a trajectory of an object to these path points along with some other constraints. I'm trying out gekko as a tool to solve this problem and for that I made a simple problem by making data points from a parabola and a constraint to the path. My attempt to solve it is

from gekko import GEKKO
import numpy as np
import matplotlib.pyplot as plt
import time

#path data points
x_ref = np.linspace(0, 4, num=21)
y_ref = - np.square(x_ref) + 16

#constraint for visualization purposes
x_bound = np.linspace(0, 4, num=10)
y_bound = 1.5*x_bound + 4

def distfunc(x,y,xref,yref,p):
    '''
    Shortest distance from (x,y) to (xref, yref)
    '''

    dtemp = []
    for i in range(len(xref)):
        d = (x-xref[i])**2+(y-yref[i])**2
        dtemp.append(dtemp)
    min_id = dtemp.index(min(dtemp))
    if min_id == 0:
        next_id = min_id+1
    elif min_id == len(x_ref):
        next_id = min_id-1
    else:
        d2 = (x-xref[min_id-1])**2+(y-yref[min_id-1])**2
        d1 = (x-xref[min_id+1])**2+(y-yref[mid_id+1])**2
        d_next = [d2, d1]
        next_id = min_id + 2*d_next.index(min(d_next)) - 1
    n1 = xref[next_id] - xref[min_id]
    n2 = yref[next_id] - yref[min_id]
    nnorm = p.sqrt(n1**2+n2**2)
    n1 = n1 / nnorm
    n2 = n2 / nnorm
    difx = x-xref[min_id]
    dify = y-yref[min_id]
    dot = difx*n1 + dify*n2
    deltax = difx - dot*n1
    deltay = dify - dot*n2
    return deltax**2+deltay**2

v_ref = 3
now = time.time()

p = GEKKO(remote=False)
p.time = np.linspace(0,10,21)
x = p.Var(value=0)
y = p.Var(value=16)
vx = p.Var(value=1)
vy = p.Var(value=0)
ax = p.Var(value=0)
ay = p.Var(value=0)
p.options.IMODE = 6
p.options.SOLVER = 3
p.options.WEB = 0

x_refg = p.Param(value=x_ref)
y_refg = p.Param(value=y_ref)

x_refg = p.Param(value=x_ref)
y_refg = p.Param(value=y_ref)
v_ref = p.Const(value=v_ref)


p.Obj(distfunc(x,y,x_refg,y_refg,p))
p.Obj( (p.sqrt(vx**2+vy**2) - v_ref)**2 + ax**2 + ay**2)

p.Equation(x.dt()==vx)
p.Equation(y.dt()==vy)
p.Equation(vx.dt()==ax)
p.Equation(vy.dt()==ay)
p.Equation(y>=1.5*x+4)
p.solve(disp=False, debug=True)
print(f'run time: {time.time()-now}')

plt.plot(x_ref, y_ref)
plt.plot(x_bound, y_bound)
plt.plot(x1.value,x2.value)
plt.show()

This is the result that I get. As you can see, its not exactly the solution that one should expect. For reference to a solution that you may expect, here is what I get using the cost function below

p.Obj((x-x_refg)**2 + (y-y_refg)**2 + ax**2 + ay**2)

However since what I actually wanted is the shortest distance to a path described by these points I expect the distfunc to be closer to what I want since the shortest distance is most likely to some interpolated point. So my question is twofold:

  1. Is this the correct gekko expression/formulation for the objective function?
  2. My other goal is solution speed so is there a more efficient way of expressing this problem for gekko?
1
gekko

1 Answers

1
votes

You can't define an objective function that changes based on conditions unless you insert logical conditions that are continuously differentiable such as with the if2 or if3 function. Gekko evaluates the symbolic model once and then passes that off to an executable for solution. It only calls the Python model build once because it is compiling the model to efficient byte-code for execution. You can see the model that you created with p.open_folder(). The model file ends in the apm extension: gk_model0.apm.

Model
Constants
    i0 = 3
End Constants
Parameters
    p1
    p2
    p3
    p4
End Parameters
Variables
    v1 = 0
    v2 = 16
    v3 = 1
    v4 = 0
    v5 = 0
    v6 = 0
End Variables
Equations
    v3=$v1
    v4=$v2
    v5=$v3
    v6=$v4
    v2>=(((1.5)*(v1))+4)
    minimize (((((v1-0.0)-((((((v1-0.0))*((0.2/sqrt(0.04159999999999994))))+(((v2-16.0))&
             *((-0.03999999999999915/sqrt(0.04159999999999994))))))*&
             ((0.2/sqrt(0.04159999999999994))))))^(2))+((((v2-16.0)&
             -((((((v1-0.0))*((0.2/sqrt(0.04159999999999994))))+(((v2-16.0))&
             *((-0.03999999999999915/sqrt(0.04159999999999994))))))&
             *((-0.03999999999999915/sqrt(0.04159999999999994))))))^(2)))
    minimize (((((sqrt((((v3)^(2))+((v4)^(2))))-i0))^(2))+((v5)^(2)))+((v6)^(2)))
End Equations

End Model

One strategy is to split your problem into multiple optimization problems that are all minimal time problems where you navigate to the first way-point and then re-initialize the problem to navigate to the second way-point, and so on. If you want to preserve momentum and anticipate the turning then you'll need to use more advanced methods such as shown in the Pigeon / Eagle tracking problem (see source files) or similar to a trajectory optimization with UAVs or HALE UAVs (see references below).