У меня проблемы с функцией интерполяции. Я не могу разрешить
TypeError: '>=' not supported between instances of 'list' and 'int'
Мой код не будет работать для всей длины функций. Комментарии с TODO были частями, которые мне нужно было добавить для запуска кода.
Код SplinedPath:
import math
import numpy as np
def tridiag( x, y ):
n = len( x )
f = np.zeros( n - 2 )
g = np.zeros( n - 3 )
r = np.zeros( n - 2 )
e = np.zeros( n - 3 )
f[ 0 ] = 2 * (x[ 2 ] - x[ 0 ])
g[ 0 ] = (x[ 2 ] - x[ 1 ])
r[ 0 ] = (6 / (x[ 2 ] - x[ 1 ])) * (y[ 2 ] - y[ 1 ])
r[ 0 ] = r[ 0 ] + (6 / (x[ 1 ] - x[ 0 ])) * (y[ 0 ] - y[ 1 ])
for i in range( 1, n - 2 ):
e[ i - 1 ] = x[ i + 1 ] - x[ i ]
f[ i ] = 2 * (x[ i + 2 ] - x[ i ])
g[ i - 1 ] = (x[ i + 1 ] - x[ i ])
r[ i ] = (6 / (x[ i + 2 ] - x[ i + 1 ])) * (y[ i + 2 ] - y[ i + 1 ])
r[ i ] = r[ i ] + (6 / (x[ i + 1 ] - x[ i ])) * (y[ i ] - y[ i + 1 ])
return e, f, g, r
return e, f, g, r
def interpolate( x, y, d2x, xu ):
n = len( x )
for i in range( 1, n ):
if xu >= x[ i - 1 ] and xu < x[ i ]:
d2xp = d2x[ i - 1 ]
d2xn = d2x[ i ] if i < n - 1 else 0
dx = x[ i ] - x[ i - 1 ]
c1 = d2xp / 6 / dx
c2 = d2xn / 6 / dx
c3 = y[ i - 1 ] / dx - d2xp * dx / 6
c4 = y[ i ] / dx - d2xn * dx / 6
t1 = c1 * (x[ i ] - xu) ** 3
t2 = c2 * (xu - x[ i - 1 ]) ** 3
t3 = c3 * (x[ i ] - xu)
t4 = c4 * (xu - x[ i - 1 ])
yu = t1 + t2 + t3 + t4
t1 = -3 * c1 * (x[ i ] - xu) ** 2
t2 = 3 * c2 * (xu - x[ i - 1 ]) ** 2
t3 = -c3
t4 = c4
dy = t1 + t2 + t3 + t4
t1 = 6 * c1 * (x[ i ] - xu)
t2 = 6 * c2 * (xu - x[ i - 1 ])
d2y = t1 + t2
return (yu, dy, d2y)
return (0, 0, 0)
def cubic_spline( x, y ):
n = len( x )
e, f, g, r = tridiag( x, y )
A = [ [ 0 for i in range( n - 2 ) ] for i in range( n - 2 ) ]
for i in range( n - 2 ):
A[ i ][ i ] = f[ i ]
if i < n - 3:
A[ i + 1 ][ i ] = e[ i ]
A[ i ][ i + 1 ] = g[ i ]
d2x = np.linalg.solve( A, r )
d2x = [ 0 ] + d2x.tolist() + [ 0 ]
return d2x
def unwrap(dq):
return ((dq+math.pi)%(2*math.pi))-math.pi
class SplinedPath:
def __init__(self, x, y):
self.currentseg = 1;
self.x = x;
self.y = y;
self.s = [0]; # Not really path length but something related to it.
for ii in range(1,len(x)):
dx = x[ii] - x[ii-1]
dy = y[ii] - y[ii-1]
self.s.append(self.s[-1] + math.sqrt(dx*dx+dy*dy))
self.d2sx = cubic_spline(self.s, self.x) #TODO Get the second derivative of the cubic spline x(s) at each knot (remember first and last should be zero)
self.d2sy = cubic_spline(self.s, self.y) #TODO Get the second derivative of the cubic spline y(s) at each knot (remember first and last should be zero)
def nearest(self,botx,boty,botyaw):
en = 0 # Normal error (perpendicular distance from botx,boty to the nearest point on the cubic spline path)
eh = 0 # Heading error (angular distance from the robots heading (botyaw) to the path heading)
kff = 0 # Curvature of the cubic splined path (we'll also call it the feedforward curvature kff)
length_remaining = 0
Ltot = self.s[-1] # Total path length (roughly)
for jj in range(self.currentseg,len(self.x)-1):
# Vector from jjth knot and (jj+1)th knot
segv = [self.x[jj+1] - self.x[jj], self.y[jj+1] - self.y[jj]]
# Vector from jjth knot to the robot
botv = [botx - self.x[jj], boty - self.y[jj]]
# What percent of the way between the jjth and (jj+1)th knot is the robot?
prcnt = (segv[0]*botv[0]+botv[1]*segv[1])/(segv[0]*segv[0]+segv[1]*segv[1]);
if prcnt < 1: # If it is less than 1 then we assume we are on this segment
self.currentseg = jj # Set the current segment to jj so we never look again at segments we've already passed
# Finally, we can get the value of the independent parameter "s" that represents the nearest point to the robot
ss = self.s[jj] + prcnt*(self.s[jj+1]-self.s[jj])
# How much "length" is left on the path?
length_remaining = Ltot - ss;
if prcnt > 0: #TODO make sure ss is in range if it is then
x = self.s #TODO Get x(ss), dxds(ss), d2xds2(ss)
xs, dxds, d2xds2 = interpolate(self.s, self.x, self.d2sx, ss)
y = self.x #TODO, get the path heading HINT: all you need are dxds and dyds
ys, dyds, d2yds2 = interpolate(self.s, self.y, self.d2sy, y)
#TODO Get y(ss), dyds(ss), d2yds2(ss)
q = math.atan2(dyds,dxds)
qp = q-math.pi/2 # This is going to point perpendicularly to the right of the direction of travel along the path
en = (botx - x)*math.cos(qp) + (boty - y)*math.sin(qp) # TODO Normal error ==> (botx-x(ss))*math.cos(qp) + (boty-y(ss))*math.sin(qp)
eh = unwrap(q - botyaw) # TODO unwrap(q-botyaw) <== unwrap to between -pi and pi
kff = (dxds*d2yds2-dyds*d2xds2)/(((dxds**2)+(dyds**2))**(3/2)) # Curvature of the path. You will find the formula for this under the
# sub-topic "In terms of a general parameterization" at https://en.wikipedia.org/wiki/Curvature
# You will need dxds(ss), d2xds2(ss), dyds(ss), and d2yds2(ss)
break
return (en,eh,kff,length_remaining)
Драйвер робота используется для запуска кода:
import math def unwrap(dq): return ((dq+math.pi)%(2*math.pi))-math.pi
def rk4(x,y,h,derivs,params): n = len(y) k1 = derivs(x, y, params) ym = [y[ii] + k1[ii]*h/2 for ii in range(n)] k2 = derivs(x+h/2, ym, params) ym = [y[ii] + k2[ii]*h/2 for ii in range(n)] k3 = derivs(x+h/2, ym, params) ye = [y[ii] + k3[ii]*h for ii in range(n)] k4 = derivs(x+h, ye, params) return [y[ii] + ((k1[ii] + 2*(k2[ii]+k3[ii]) + k4[ii])/6)*h for ii in range(n)]
def getstate(bot): s = bot["state"] return [s["x"], s["y"], s["yaw"], s["vx"], s["vy"]]
def setstate(bot,s): bot["state"]["x"] = s[0] bot["state"]["y"] = s[1] bot["state"]["yaw"] = s[2] bot["state"]["vx"] = s[3] bot["state"]["vy"] = s[4]
def pathcontroller(bot): (en,eh,kff,dl) = bot['path'].nearest( \
bot['state']['x'], bot['state']['y'], bot['state']['yaw']) kd = 0.2*en + 1*eh + kff; sa = math.atan(kd*bot['wheel_base']); bot['steer_angle'] = max(-0.55,min(0.55,sa)); cq = math.cos(bot['state']['yaw']); sq = math.sin(bot['state']['yaw']); body_frame_velocity = bot['state']['vx']*cq+bot['state']['vy']*sq; if dl > 1:
bot['wheel_torque'] = max(-2, min(2, dl - body_frame_velocity)); else:
bot['wheel_torque'] = -0.5*body_frame_velocity;
def controller(bot,goalx,goaly): dx = goalx - bot["state"]["x"] dy
= goaly - bot["state"]["y"]; dq = unwrap(math.atan2(dy,dx) - bot["state"]["yaw"]); dl = math.sqrt(dx*dx + dy*dy); cq = math.cos(bot["state"]["yaw"]); sq = math.sin(bot["state"]["yaw"]); body_frame_velocity = bot["state"]["vx"]*cq + bot["state"]["vy"]*sq; if dl > 1:
bot["wheel_torque"] = max(-2,min(2,dl - body_frame_velocity));
bot["steer_angle"] = max(-0.55,min(0.55,dq)); else:
bot["wheel_torque"] = -body_frame_velocity;
bot["steer_angle"] = 0;
def plant(t,z,bot): setstate(bot,z) speed = math.sqrt(bot["state"]["vx"]*bot["state"]["vx"] + bot["state"]["vy"]*bot["state"]["vy"]) cq = math.cos(bot["state"]["yaw"]) sq = math.sin(bot["state"]["yaw"]) body_frame_velocity = cq*bot["state"]["vx"] + sq*bot["state"]["vy"] yaw_rate = body_frame_velocity*math.tan(bot["steer_angle"])/bot["wheel_base"]; Faero = -bot["drag_coefficient"]*speed*body_frame_velocity; Fprop = bot["wheel_torque"]/bot["wheel_radius"]; acceleration = (Fprop + Faero)/bot["mass"]; ax = acceleration*cq - sq*body_frame_velocity*yaw_rate; ay = acceleration*sq + cq*body_frame_velocity*yaw_rate; return [bot["state"]["vx"], bot["state"]["vy"], yaw_rate, ax, ay];
def simulate(tstart, tend, h, bot, goal): y = getstate(bot); n = int((tend-tstart)/h)+1; with open('out.txt','w') as w:
for ii in range(n):
t = tstart + ii*h
y = rk4(t,y,h,plant,bot);
setstate(bot,y)
controller(bot, goal["x"], goal["y"])
pathcontroller(bot)
v = math.sqrt(bot['state']['vx']*bot['state']['vx'] + bot['state']['vy']*bot['state']['vy'])
w.write("{} {} {} {} {} {}\n".format(t,bot['state']['x'],bot['state']['y'],v,bot['wheel_torque'],bot['steer_angle']))
print(bot["state"]["x"],bot["state"]["y"])
if __name__ == '__main__': from SplinedPath import SplinedPath n = 20 s = [5*ii/(n-1) for ii in range(n)] y = [10*math.cos(si) - 10 for si in s] x = [10*si + 4*math.sin(si) for si in s] path = SplinedPath(x,y)
bot = {"state":{"x":0,"y":0,"yaw":0,"vx":0,"vy":0},
"mass":1, "wheel_base":1, "drag_coefficient":1, "wheel_radius":0.1,
"wheel_torque":1, "steer_angle":0.1, "path":path} goal = {"x":-40, "y":10} simulate(0, 20, 0.1, bot, goal)