In [1]:
# This file is prepared by Dr. Heung Wing Joseph LEE
#
# Putsuit on Sphere
#
# Both the pursuer and target are travelling on the surface of unit sphere
# centered at origin
# The Target is travelling with constant angular speed 1, with
# great circle normal fixed direction (beta1, beta2, beta3)
#
# The Pursuer speed is control u(t) in [-1,1], with
# great circle normal direction (alpha1, alpha2, alpha3) which is a
# constant vector, but chosen by gekko (system parameters)
#
# In other words, both pursuer and target are not changing direction
# once t>=0.
#
# The objective is for the pursuer to intercept the target in minimum 
# time.
#
# Note that, if the starting point of the pursuer is not on the same 
# great circle of the target, then, they are on two different great 
# circles. The intercepting point must then be at one of the two 
# antipodal interception points of the two great circles.
#
# if gekko is not installed, uncomment (delete #) in the following line
#! pip install gekko
# only need to do it once, and put it (#) back as comment once gekko is
# installed, e.g for anaconda python, you only need to do it once to
# install gekko
#
# however, if you are using Google CoLab to run python,
# then, you need to install it every time, if so, keep uncomment if you are
# using Google CoLab
In [2]:
from gekko import GEKKO
import numpy as np
import matplotlib.pyplot as plt
In [3]:
m = GEKKO()
nt = 501
m.time = np.linspace(0,1,nt)

initialx=1/np.sqrt(3)
initialy=1/np.sqrt(3)
initialz=1/np.sqrt(3)

TThetaRate=1
beta1=0
beta2=0
beta3=-1
tinitialx=1
tinitialy=0
tinitialz=0

targetx=1
targety=0
targetz=0

# (x1,x2,x3) is the pursuer position
x1 = m.Var(value=initialx)
x2 = m.Var(value=initialy)
x3 = m.Var(value=initialz)

# x4 is time
x4 = m.Var(value=0)



# x5 is theta, and u1 is dot-theta
x5 = m.Var(value=0)

# (x6, x7, x8) is the target position
x6 = m.Var(value=tinitialx)
x7 = m.Var(value=tinitialy)
x8 = m.Var(value=tinitialz)

# tf is the final time, time scale, 
# that means, it is a system parameter
p = np.zeros(nt) # final time = 1 
p[-1] = 1.0
final = m.Param(value=p)
tf = m.FV(value=1,lb=0.1,ub=10.0) 
tf.STATUS = 1

# (alpha1,alpha2,alpha3) is the great circle normal direction
# of the pursuer, that means, three system parameters
alpha1 = m.FV(value=-1/np.sqrt(2),lb=-1,ub=1)
alpha2 = m.FV(value=1/np.sqrt(2),lb=-1,ub=1)
alpha3 = m.FV(value=0,lb=-1,ub=1)
alpha1.STATUS = 1
alpha2.STATUS = 1
alpha3.STATUS = 1

# control is the angular speed of teh pursuer
u1 = m.MV(value = 1, lb = -1, ub = 1, fixed_initial=False) 
u1.STATUS = 1


# RR is the time derivative of the rotation matrix of 
# (x1,x2,x3) in 3D
# note that x5 is theta (time), and time derivative is u1 
RR00=-m.sin(x5)*u1+alpha1**2*m.sin(x5)*u1
RR01=alpha1*alpha2*m.sin(x5)*u1-alpha3*m.cos(x5)*u1
RR02=alpha1*alpha3*m.sin(x5)*u1+alpha2*m.cos(x5)*u1
RR10=alpha1*alpha2*m.sin(x5)*u1+alpha3*m.cos(x5)*u1
RR11=-m.sin(x5)*u1+alpha2**2*m.sin(x5)*u1
RR12=alpha2*alpha3*m.sin(x5)*u1-alpha1*m.cos(x5)*u1
RR20=alpha1*alpha3*m.sin(x5)*u1-alpha2*m.cos(x5)*u1
RR21=alpha2*alpha3*m.sin(x5)*u1+alpha1*m.cos(x5)*u1
RR22=-m.sin(x5)*u1+alpha3**2*m.sin(x5)*u1

m.Equation(x1.dt()==(RR00*initialx+RR01*initialy+RR02*initialz)*tf)  
m.Equation(x2.dt()==(RR10*initialx+RR11*initialy+RR12*initialz)*tf) 
m.Equation(x3.dt()==(RR20*initialx+RR21*initialy+RR22*initialz)*tf) 
m.Equation(x4.dt()==tf)
m.Equation(x5.dt()==u1*tf)

# TRR is the time derivative of the rotation matrix of 
# (x6,x7,x8) in 3D
# note that x5 is theta (time), and time derivative is 1 

TRR00=-m.sin(x5)*TThetaRate+beta1**2*m.sin(x5)*TThetaRate
TRR01=beta1*beta2*m.sin(x5)*TThetaRate-beta3*m.cos(x5)*TThetaRate
TRR02=beta1*beta3*m.sin(x5)*TThetaRate+beta2*m.cos(x5)*TThetaRate
TRR10=beta1*beta2*m.sin(x5)*TThetaRate+beta3*m.cos(x5)*TThetaRate
TRR11=-m.sin(x5)*TThetaRate+beta2**2*m.sin(x5)*TThetaRate
TRR12=beta2*beta3*m.sin(x5)*TThetaRate-beta1*m.cos(x5)*TThetaRate
TRR20=beta1*beta3*m.sin(x5)*TThetaRate-beta2*m.cos(x5)*TThetaRate
TRR21=beta2*beta3*m.sin(x5)*TThetaRate+beta1*m.cos(x5)*TThetaRate
TRR22=-m.sin(x5)*TThetaRate+beta3**2*m.sin(x5)*TThetaRate

m.Equation(x6.dt()==(TRR00*tinitialx+TRR01*tinitialy+TRR02*tinitialz)*tf)  
m.Equation(x7.dt()==(TRR10*tinitialx+TRR11*tinitialy+TRR12*tinitialz)*tf) 
m.Equation(x8.dt()==(TRR20*tinitialx+TRR21*tinitialy+TRR22*tinitialz)*tf) 

# terminal constraint
m.Equation(((x1-x6)**2+(x2-x7)**2+(x3-x8)**2)*final<=0.00001)

# constraint on system parameters, (alpha1,alpha2,alpha3)
m.Equation(alpha1**2+alpha2**2+alpha3**2<=1.00001)

m.Obj(tf)
m.options.IMODE = 6
In [4]:
m.solve(disp=False)
In [5]:
print('Final Time: ' + str(tf.value[0]))
tm = np.linspace(0,tf.value[0],nt)

Final Time: 2.5007391368
In [6]:
print('alpha1=',alpha1[-1],'alpha2=',alpha2[-1],'alpha3=',alpha3[-1])

alpha1= 0.57735026328 alpha2= -0.7813914559 alpha3= 0.20404119262
In [7]:
plt.figure(figsize=(15,8))
plt.plot(tm, x1.value, 'g:', label = '$x_1$')
plt.plot(tm, x2.value, 'r:', label = '$x_2$')
plt.plot(tm, x3.value, 'b:', label = '$x_3$')
plt.plot(tm, u1.value, 'c-', label = '$u_1$')
plt.plot(tm, x6.value, 'g:', label = '$x_6$')
plt.plot(tm, x7.value, 'r:', label = '$x_7$')
plt.plot(tm, x8.value, 'b:', label = '$x_8$')

plt.legend()
plt.show()
In [8]:
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import proj3d
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.patches import Circle, PathPatch
import mpl_toolkits.mplot3d.art3d as art3d
In [9]:
def pathpatch_2d_to_3d(pathpatch, z = 0, normal = 'z'):
 """
 Transforms a 2D Patch to a 3D patch using the given normal vector.
 The patch is projected into they XY plane, rotated about the origin
 and finally translated by z.
 """
In [10]:
from mpl_toolkits.mplot3d import art3d
def rotation_matrix(d):
 """
 Calculates a rotation matrix given a vector d. The direction of d
 corresponds to the rotation axis. The length of d corresponds to
 the sin of the angle of rotation.
 Variant of: http://mail.scipy.org/pipermail/numpy-discussion/2009-March/040806.html
 """
 sin_angle = np.linalg.norm(d)
 if sin_angle == 0:
   return np.identity(3)
 d /= sin_angle
 eye = np.eye(3)
 ddt = np.outer(d, d)
 skew = np.array([[ 0, d[2], -d[1]],
 [-d[2], 0, d[0]],
 [d[1], -d[0], 0]], dtype=np.float64)
 M = ddt + np.sqrt(1 - sin_angle**2) * (eye - ddt) + sin_angle * skew
 return M

def pathpatch_2d_to_3d(pathpatch, z = 0, normal = 'z'):
 """
 Transforms a 2D Patch to a 3D patch using the given normal vector.
 The patch is projected into they XY plane, rotated about the origin
 and finally translated by z.
 """
 if type(normal) is str: #Translate strings to normal vectors
    index = "xyz".index(normal)
    normal = np.roll((1.0,0,0), index)
 normal /= np.linalg.norm(normal) #Make sure the vector is normalised
 path = pathpatch.get_path() #Get the path and the associated transform
 trans = pathpatch.get_patch_transform()
 path = trans.transform_path(path) #Apply the transform
 pathpatch.__class__ = art3d.PathPatch3D #Change the class
 pathpatch._code3d = path.codes #Copy the codes
 pathpatch._facecolor3d = pathpatch.get_facecolor #Get the face color
 verts = path.vertices #Get the vertices in 2D
 d = np.cross(normal, (0, 0, 1)) #Obtain the rotation vector 
 M = rotation_matrix(d) #Get the rotation matrix
 pathpatch._segment3d = np.array([np.dot(M,(x,y,0))+(0,0,z) for x, y in verts])

def pathpatch_translate(pathpatch, delta):
 """
 Translates the 3D pathpatch by the amount delta.
 """
 pathpatch._segment3d += delta
In [11]:
fig = plt.figure(figsize=(10,10))
ax = fig.add_subplot(111, projection='3d')
# draw wireframe sphere
u, v = np.mgrid[0:2*np.pi:30j, 0:np.pi:30j]
xx = np.cos(u)*np.sin(v)
yy = np.sin(u)*np.sin(v)
zz = np.cos(v)
ax.plot_wireframe(xx, yy, zz, color="red", alpha=0.2)
ax.plot(x1, x2, x3, color='b')
ax.plot(x6, x7, x8, color='m')
ax.scatter(x1[0],x2[0],x3[0],color='y',s=100)
ax.scatter(x1[-1],x2[-1],x3[-1],color='y',s=100)
ax.scatter(x6[0],x7[0],x8[0],color='g',s=100)
ax.scatter(x6[-1],x7[-1],x8[-1],color='g',s=100)

ppa=Circle((0,0),1, edgecolor='c', facecolor=None, fill=False, linestyle='--')
ax.add_patch(ppa)
aaaa1=(x1[0],x2[0],x3[0])
aaaa2=(x1[-1],x2[-1],x3[-1])
pathpatch_2d_to_3d(ppa, z = 0, normal = np.cross(aaaa1,aaaa2))
pathpatch_translate(ppa, (0, 0, 0))

ppb=Circle((0,0),1, edgecolor='c', facecolor=None, fill=False, linestyle='--')
ax.add_patch(ppb)
bbbb1=(x6[0],x7[0],x8[0])
bbbb2=(x6[-1],x7[-1],x8[-1])
pathpatch_2d_to_3d(ppb, z = 0, normal = np.cross(bbbb1,bbbb2))
pathpatch_translate(ppb, (0, 0, 0))

ax.view_init(40, 30)
fig.set_dpi(80)
plt.show()
In [ ]: