def __init__(self, start, goal, start_heading, goal_heading,
start_steering, mapsize, freeGrid_num, tolerance, car_length,
car_width, wheelbase, obstacle_type):
self.start = start
self.goal = goal
self.start_heading = start_heading
self.goal_heading = goal_heading
self.start_steering = start_steering
self.freeGrid_num = freeGrid_num
self.mapsize = mapsize
self.tolerance = tolerance
self.car_length = car_length
self.car_width = car_width
self.wheelbase = wheelbase
self.obstacle_type = obstacle_type
# the bezier spline
# using the bezier as the referenece line
bz = Bezier(start, goal, self.start_heading, self.goal_heading,
self.start_steering, self.mapsize, self.car_length,
self.car_width, self.wheelbase, self.mapsize * 3,
"NoFound")
self.bezier_spline = bz.calculation()
#print("self.bezier_spline",self.bezier_spline)
# the map and obstacle
self.freeGrid_num, self.obstacle, self.danger_zone = mapGenerator(
start, self.obstacle_type, self.mapsize)
def __init__(self, position, parent):
pygame.sprite.Sprite.__init__(self)
if BezLinkedLine.init:
BezLinkedLine.bezier = Bezier()
BezLinkedLine.init = False
self.transparency = True
self.position = position
# The BezCurve that this sprite is a child to
self.parent = parent
# Line defined by two control points, one is moveable, the other is fixed
# to the bezier curve that this line is a child to
self.CPGroup = pygame.sprite.Group()
self.CPDict = {}
# Movable control point
sp = CPSprite(self.position, self, label="move")
self.CPGroup.add(sp)
self.CPDict["move"] = sp
# UnMovable control point
sp = CPSprite(self.position + vec2d(50,50), self, label="unmove")
self.CPGroup.add(sp)
self.CPDict["unmove"] = sp
self.update_endpoint("move", self.position)
self.calc_rect()
self.update()
def build_traj_list(cpts, times, ndim, nveh):
"""Builds a trajectory list of Bernstein polynomial objects
:param cpts: Control points of the polynomials where the first m rows are
each dimension of the first trajectory. Following sets of m rows
correspond to each trajectory after the first one. The columns hold
each control point for the trajectories. There should be ndim*nveh
rows and deg+1 columns.
:type cpts: np.ndarray
:param times: Array of initial and final times for each trajectory. Each
row corresponds to each trajectory. The first column is t0 and the
second column is tf.
:type times: np.ndarray
:param ndim: Number of dimensions (e.g. 2D, 3D, etc.). Most likely 2 or 3.
:type ndim: int
:param nveh: Number of trajectories (vehicles)
:type nveh: int
:return: List of Bernstein polynomial objects (Bezier) corresponding to
each trajectory passed in.
:rtype: list(Bezier)
"""
trajs = []
for i in range(nveh):
trajs.append(
Bezier(cpts[i * ndim:(i + 1) * ndim, :],
t0=times[i, 0],
tf=times[i, 1]))
return trajs
def plan_mon(p0, v0, psi0, t0, trgt_cpts, pastCpts, pastTimes, tf, params):
"""
MON CONSTRAINTS:
* Usual (speed, rate, Ds)
* Must be somewhere along the inner monitoring radius
"""
ndim = 2
vf = params.monSpeed
nveh = pastCpts.shape[0] // ndim
trgt = trgt_cpts[:, -1]
trgt_traj = Bezier(trgt_cpts, t0=t0, tf=tf)
dt = tf - t0
assert dt >= 0, f'dt should be >= 0, t0: {t0}, tf: {tf}'
x0 = init_guess_m(p0, trgt, v0, vf, psi0, dt, params.deg, params)
def fn(x):
return cost_m(x, trgt_cpts, p0, v0, psi0, dt, params)
cons = [{
'type':
'ineq',
'fun':
lambda x:
nonlinear_constraints_m(x, p0, v0, vf, psi0, t0, tf, nveh, pastCpts,
pastTimes, trgt, trgt_traj, params)
}]
results = minimize(fn,
x0,
constraints=cons,
method='SLSQP',
options={
'maxiter': 250,
'disp': True,
'iprint': params.iprint
})
y = reshape_m(results.x, p0, v0, psi0, dt, params.deg, trgt, params.innerR)
newTraj = Bezier(y, t0=t0, tf=tf)
if not results.success:
print('---> MON UNSUCCESSFUL!')
return newTraj
def cost_m(x, trgt_cpts, p0, v0, psi0, dt, params):
"""Cost function for the monitoring trajectory
"""
pt = Bezier(trgt_cpts, tf=dt)
y = reshape_m(x, p0, v0, psi0, dt, params.deg, trgt_cpts[:, -1],
params.innerR)
p = Bezier(y, tf=dt).elev(params.degElev)
pdot = p.diff()
# if np.any((pdot*(pt-p)).cpts < 0):
# return 99999
costPts = ((pt.y - p.y) * pdot.x - (pt.x - p.x) * pdot.y).normSquare().cpts
# if np.sign(sum(pdot*(pt-p)))
return sum(costPts.squeeze())
class AABB:
""" Axis aligned bounding box """
def __init__(self):
self.reinit()
def reinit(self):
inf = 1e500
self.xmin = inf
self.xmax = -inf
self.ymin = inf
self.ymax = -inf
def addPoint(self, x, y):
if x < self.xmin:
self.xmin = x
if x > self.xmax:
self.xmax = x
if y < self.ymin:
self.ymin = y
if y > self.ymax:
self.ymax = y
def width(self):
return self.xmax - self.xmin
def height(self):
return self.ymax - self.ymin
def x(self):
return self.xmin
def y(self):
return self.ymin
def cx(self):
return (self.xmin + self.xmax) / 2.0
def cy(self):
return (self.ymin + self.ymax) / 2.0
def applyTransform(self, transform):
matrix = Transform().createMatrix(transform)
(x1, y1) = matrix.applyOnPoint(self.xmin, self.ymin)
(x2, y2) = matrix.applyOnPoint(self.xmax, self.ymax)
self.reinit()
self.addPoint(x1, y1)
self.addPoint(x2, y2)
def addBezier(self, (lastX, lastY), params):
# to acces values with simplepath format
(Cx1, Cy1, Cx2, Cy2, Cx, Cy) = range(-6, 0)
x1, y1 = params[Cx1], params[Cy1]
x2, y2 = params[Cx2], params[Cy2]
x, y = params[Cx], params[Cy]
bezVer = Bezier(
((lastX, lastY), (x1, y1), (x2, y2), (x, y))).splitCurve()
[self.addPoint(x, y) for x, y in bezVer]
def paint(self, paintHidden=False):
Bezier.paint(self, paintHidden)
p1 = None
pm = None
(p1, pm) = self.get_border_points(200, 300)
v = p1 - pm
v = v.normalize()
v = JPoint2d(-v.getY(), v.getX())
p2 = pm + v * 30
p3 = pm - v * 30
glColor3f(0, 1, 0)
glBegin(GL_TRIANGLES)
glVertex2f(p1.getX(), p1.getY())
glVertex2f(p2.getX(), p2.getY())
glVertex2f(p3.getX(), p3.getY())
#glVertex2f(pm.getX(), pm.getY())
glEnd()
def __init__(self):
self._poscurve = Bezier([],[])
self._hcurve = Bezier([],[])
self._scurve = Bezier([],[])
self._vcurve = Bezier([],[])
self._radcurve = Bezier([],[])
self._alphacurve = Bezier([],[])
def __init__(self, start, goal, start_heading, goal_heading,
start_steering, mapsize, freeGrid_num, tolerance, car_length,
car_width, wheelbase, OBS_type):
self.start = start
self.goal = goal
self.start_heading = start_heading
self.goal_heading = goal_heading
self.start_steering = start_steering
self.freeGrid_num = freeGrid_num
self.mapsize = mapsize
self.tolerance = tolerance
self.car_length = car_length
self.car_width = car_width
self.wheelbase = wheelbase
self.OBS_type = OBS_type
bz = Bezier(start, goal, self.start_heading, self.goal_heading,
self.start_steering, self.mapsize, self.car_length,
self.car_width, self.wheelbase, self.mapsize * 3,
"NoFound")
self.bezier_spline = bz.calculation()
self.freeGrid_num, self.obstacle, self.danger_zone = mapGenerator(
start, self.OBS_type, self.mapsize)
def __init__(self, control_points, position):
pygame.sprite.Sprite.__init__(self)
if BezCurve.init:
BezCurve.bezier = Bezier()
BezCurve.init = False
self.transparency = True
self.children = []
self.time = pygame.time.get_ticks()
# Position of this graphic
self.position = position
# Length of the bezier curve
self.length = 0
self.dot_pos = 0
self.speed = 0.1
self.movement = 1
self.eps = ["e0","e1","e2","e3"]
# 4 Control points defining the curve
# Also needs a control point to move the shape
# but probably better to build this into a different item
# e.g. move_control_point, which is always at the center
# of the shape
# CPGroup, sprite group containing all control points for quick access
self.CPGroup = pygame.sprite.Group()
# CPDict, dict containing keyed control points for easy access to modify
self.CPDict = {}
for cp, key in zip(control_points, self.eps):
sp = CPSprite(cp + self.position, self, label=key)
self.CPGroup.add(sp)
self.CPDict[key] = sp
# Calculate the sprite's rect
self.calc_rect()
# Add CP for middle of shape to move it
sp = CPSprite(self.position + vec2d(self.width / 2, self.height / 2),
self, label="move")
self.CPGroup.add(sp)
self.CPDict["move"] = sp
# Must modify control points in two ways:
# Update control_points value
# Update CPDict value
self.update()
def convertInLines(self):
"""
convert C and A to lines.
"""
# if smooth = 100, limit = 1.0, is smooth is smaller, limit is
# bigger and less points are generated by splitCurve
limit = smooth2limit(self.smooth) * 10
# to acces values with simplepath format
(Cx1, Cy1, Cx2, Cy2, Cx, Cy) = range(-6, 0)
(Arx, Ary, Aaxis, Aarc, Asweep, Ax, Ay) = range(-7, 0)
(x, y) = range(-2, 0)
newBlocks = []
for vertex in self.blocks:
# as we add new vertex, we need a new list to store them
tmp = []
(lastX, lastY) = (0, 0)
for element, values in vertex:
if element == 'C':
tmp.extend(
Bezier(((lastX, lastY), (values[Cx1], values[Cy1]),
(values[Cx2], values[Cy2]),
(values[Cx], values[Cy]))).splitCurve(limit))
elif element == 'A':
tmp.extend(
ParametricArc((lastX, lastY), (values[Ax], values[Ay]),
(values[Arx], values[Ary]),
values[Aaxis], values[Aarc],
values[Asweep]).splitArc())
else:
tmp.append((values[x], values[y]))
lastX = values[x]
lastY = values[y]
newBlocks.append(tmp)
self.blocks = newBlocks
def __init__(self, radius, position, intersect_link=None, parent=None):
pygame.sprite.Sprite.__init__(self)
if Circle.init:
Circle.bezier = Bezier()
Circle.intersection = Intersection()
Circle.init = False
self.transparency = True
# The Shape that this Shape is a child to
self.parent = parent
# Position of this graphic
self.position = position
# Circles defined by radius
self.radius = radius
# CPGroup, sprite group containing all control points for quick access
self.CPGroup = pygame.sprite.Group()
# CPDict, dict containing keyed control points for easy access to modify
self.CPDict = {}
sp = CPSprite(self.position + vec2d(self.radius, self.radius), self, label="move")
self.CPGroup.add(sp)
self.CPDict["move"] = sp
# Add CP for middle of shape to move it
sp = CPSprite(self.position + vec2d(self.radius * 2, self.radius), self, label="radius")
self.CPGroup.add(sp)
self.CPDict["radius"] = sp
# Set ilink property to this shape's intersection_link neighbour
self.ilink = intersect_link
# If this shape has an intersect_link, need to update intersection
self.update_intersect()
self.calc_rect()
self.update()
from pygame.locals import QUIT, KEYDOWN, K_ESCAPE, K_0, K_1, K_2, K_c
from bezier import Bezier
from nurbs import Nurbs
os.environ["SDL_VIDEO_CENTERED"] = "1"
pygame.init()
screen = pygame.display.set_mode((0, 0), pygame.RESIZABLE)
pygame.display.set_caption("Modelagem Geometrica TOP")
screen.fill((22, 22, 22))
game_run = True
only_curve = False
erro = 1
bezier_curve = Bezier([150, 0, 0], [200, 200, 0], screen, 4)
nurbs_curve = Nurbs([0, 150, 0], [0, 200, 200], screen, 5, [5, 5, 5, 5, 5])
def get_events_draw():
global game_run
global only_curve
for event in pygame.event.get():
if event.type == QUIT:
game_run = False
elif event.type == KEYDOWN and event.key == K_ESCAPE:
game_run = False
elif event.type == KEYDOWN and event.key == K_0:
nurbs_curve.move(bezier_curve.last_control_point())
elif event.type == KEYDOWN and event.key == K_1:
print("WIP C1")
def setRad(self, rads):
self._radcurve = Bezier(rads,[0]*len(rads))
parser.add_argument('--draw', default='quadratic', help='')
parser.add_argument('--batch', default=1, type=int, help='')
# parser.add_argument('--batches', nargs='*', default=[16], type=int, help='')
parser.add_argument('--passes', default=1, type=int, help='')
args = parser.parse_args()
use_cuda = not args.disable_cuda and torch.cuda.is_available()
# Make all tensors cuda tensors
# torch.set_default_tensor_type(torch.cuda.FloatTensor if use_cuda else torch.FloatTensor)
device = torch.device("cuda" if use_cuda else "cpu")
print('Using device "{}"'.format(device))
net = Bezier(res=args.res,
steps=args.steps,
method=args.method,
device=device,
debug=args.debug)
if args.draw == 'quadratic':
control_points_l = [[[0.1, 0.1], [0.9, 0.9], [0.5, 0.9]]]
elif args.draw == 'cubic':
control_points_l = [[[1.0, 0.0], [0.21, 0.12], [0.72, 0.83], [0.0, 1.0]]]
elif args.draw == 'composite':
control_points_l = [
[[0.1, 0.1], [0.9, 0.9], [0.5, 0.9]],
[[0.5, 0.9], [0.1, 0.9], [0.3, 0.3]],
[[0.3, 0.3], [0.9, 0.9], [0.9, 0.1]],
]
elif args.draw == 'char':
def setRad(self, rads):
self._radcurve = Bezier(rads, [0] * len(rads))
def setPos(self, xs, ys):
self._poscurve = Bezier(xs, ys)
class BezierLine(object):
def __init__(self):
self._poscurve = Bezier([], [])
self._hcurve = Bezier([], [])
self._scurve = Bezier([], [])
self._vcurve = Bezier([], [])
self._radcurve = Bezier([], [])
self._alphacurve = Bezier([], [])
def setPos(self, xs, ys):
self._poscurve = Bezier(xs, ys)
def setHSV(self, hs, ss, vs):
self._hcurve = Bezier(hs, [0] * len(hs))
self._scurve = Bezier(ss, [0] * len(ss))
self._vcurve = Bezier(vs, [0] * len(vs))
def setRad(self, rads):
self._radcurve = Bezier(rads, [0] * len(rads))
def setAlpha(self, alpha):
self._alphacurve = Bezier(alpha, [0] * len(alpha))
def getPos(self, t):
return self._poscurve(t)
def getHSV(self, t):
return self._hcurve(t)[0], self._scurve(t)[0], self._vcurve(t)[0]
def getRGB(self, t):
val = colorsys.hsv_to_rgb(
self._hcurve(t)[0],
self._scurve(t)[0],
self._vcurve(t)[0])
val2 = []
for i in range(len(val)):
val2 += [val[i] * 256]
return tuple(val2)
def getRad(self, t):
return self._radcurve(t)[0]
def getAlpha(self, t):
return self._alphacurve(t)[0]
def draw(self, drw, mirror=True):
time = 0
dt = .005
xold, yold = self.getPos(time)
x, y = self.getPos(time + dt)
ds = ((x - xold)**2 + (y - yold)**2)**.5
rad = self.getRad(0)
time += dt
while time < 1:
x, y = self.getPos(time)
alpha = self.getAlpha(time)
ds = ((x - xold)**2 + (y - yold)**2)**.5
rad = self.getRad(0)
r, g, b = self.getRGB(time)
drw.brush(x, y, r, g, b, rad, ds, alpha)
if mirror:
drw.brush(drw.getWidth() - x, y, r, g, b, rad, ds, alpha)
time += dt
xold, yold = x, y
def copy(self):
bz = BezierLine()
bz._poscurve = self._poscurve.copy()
bz._hcurve = self._hcurve.copy()
bz._scurve = self._scurve.copy()
bz._vcurve = self._vcurve.copy()
bz._radcurve = self._radcurve.copy()
bz._alphacurve = self._alphacurve.copy()
return bz
def randomize(self, positionmax=0, colormax=0, radiusmax=0, alphamax=0):
self._poscurve.randomjostle(positionmax)
self._hcurve.randomjostle(colormax)
self._scurve.randomjostle(colormax)
self._vcurve.randomjostle(colormax)
self._radcurve.randomjostle(radiusmax)
self._alphacurve.randomjostle(alphamax)
def redistributePoints(self, n):
self._poscurve.redistributePoints(n)
self._hcurve.redistributePoints(n)
self._scurve.redistributePoints(n)
self._vcurve.redistributePoints(n)
self._radcurve.redistributePoints(n)
self._alphacurve.redistributePoints(n)
def setHSV(self, hs, ss, vs):
self._hcurve = Bezier(hs,[0]*len(hs))
self._scurve = Bezier(ss,[0]*len(ss))
self._vcurve = Bezier(vs,[0]*len(vs))
def compute_flight_traj(self, tf=None):
""" Plans the flight trajectory
"""
print(f'Agent {self.idx} computing flight')
self._traj_state = 'flight'
# Grab states
p0 = self.state[:2]
v0 = self.state[3]
psi0 = self.state[2]
t0 = self.t
try:
pastCpts = np.vstack(Agent.trajList)
pastTimes = np.vstack(Agent.timeList)
except ValueError:
pastCpts = np.atleast_2d([])
pastTimes = np.atleast_2d([])
if tf is None:
tf = t0 + self.params.tflight
# Predict target position and trajectory and save the prediction
trgt = self.predict_target(tf)
trgt_cpts = self.predict_trgt_traj(tf)
self._last_trgt = trgt.copy()
trgt_traj = Bezier(trgt_cpts, t0=t0, tf=tf)
# trgt_traj.plot(self._ax, showCpts=False, color='k', ls=':')
# Plan the flight trajectory and then share it to the other agents via
# the Agent class variable
flight_traj = plan_flight(p0,
v0,
psi0,
t0,
trgt,
trgt_cpts,
pastCpts,
pastTimes,
self.params,
tf=tf)
self.flight_traj = flight_traj
Agent.trajList.append(flight_traj.cpts)
Agent.timeList.append([flight_traj.t0, flight_traj.tf])
self.flightTrajIdx = len(Agent.trajList) - 1
# Agent.flightTrajDict[self.idx] = flight_traj.cpts.squeeze()
# Agent.flightTimesDict[self.idx] = [flight_traj.t0, flight_traj.tf]
# If we have an axis, plot the new trajectory
if self._ax is not None:
if self._flight_plot is None:
self._flight_plot = self._ax.plot(flight_traj.curve[0, :],
flight_traj.curve[1, :],
color=Agent.colors[self.idx],
linestyle='-')[0]
else:
self._flight_plot.set_xdata(flight_traj.curve[0, :])
self._flight_plot.set_ydata(flight_traj.curve[1, :])
# flight_traj.plot(self._ax, showCpts=False,
# color=Agent.colors[self.idx], linestyle='-')
plt.pause(0.001)
from xdrive import XDriveKinematics, EKSDRIVE, XDriveRenderer
from bezier import Bezier
from math import sqrt, pi
import numpy as np
import matplotlib.pyplot as plt
import cv2
path = Bezier([
np.array([0.0, 0.0]),
np.array([-1.26, 0.96]),
np.array([1.61, 4.21]),
np.array([-1.8, 1.98])
], 50)
path_reversed = Bezier([
np.array([-1.8, 1.98]),
np.array([1.61, 4.21]),
np.array([-1.26, 0.96]),
np.array([0.0, 0.0])
], 50)
# path = Bezier([np.array([0.0, 0.0]), np.array([4.0, 0.0])])
# path_reversed = Bezier([np.array([4.0, 0.0]), np.array([0.0, 0.0])])
canvas_size = np.array([800, 800])
m_to_px = 130
dt = 0.01 # 10ms
drive = EKSDRIVE(1.05, 0.35, False)
drive_renderer = XDriveRenderer(drive, m_to_px, np.array([30, 30]),
canvas_size, dt)
def __init__(self, s1, s2):
Bezier.__init__(self)
self._control_points[0] = s1
self._control_points[3] = s2
s1.add_link_to(self)
def setUp(self):
self.b0 = Bezier([(1, 1), (2, 3), (4, 3), (3, 1)])
self.b1 = Bezier([(-1,1), (3,3), (4,-1), (2,-1)])
import json
# open the guide file and extract the guide path
with open("at.svg") as f:
bytestring = f.read()
# interpret the svg as an XML document and remove the "guide" group
tree = lxml.etree.fromstring(bytestring)
viewBox = tree.attrib['viewBox']
dx, dy, w, h = map(float, viewBox.split())
print dx, dy
g = tree.xpath("//*[@id='guide']")[0]
guide = g[0].attrib['d']
points = Bezier.parse_svg(guide)
curve = Bezier(points)
tree.remove(g)
bytestring = lxml.etree.tostring(tree)
print_width_mm = 72
resolution_dpi = 180
width_pixels = int(72/25.4 * 180)
# set up the printer context
printer = win32print.GetDefaultPrinter()
print printer
phandle = win32print.OpenPrinter(printer)
dc = win32ui.CreateDC()
dc.CreatePrinterDC()
# capture all the specified control points
# they are relative to the plotter's starting point
specified_control_points = [plotter.initial_position]
for point in sys.argv[1].split(' '):
specified_control_points.append(
[plotter.initial_position[0] + float(point.split(',')[0])
, plotter.initial_position[1] + float(point.split(',')[1])]
)
if '--control' in sys.argv:
for point in specified_control_points:
if point == plotter.initial_position:
continue
plotter.move_to(point)
# back to start
plotter.move_to(plotter.initial_position)
b = Bezier(specified_control_points)
path = b.generate_path()
# plot curve points
for p in path:
plotter.move_to(p)
# wraps things up with the sim
plotter.finish()
def calculation(self):
start = self.start
goal = self.goal
theta, theta_future, displacement_rear, displacement_rear_future, steering_step = \
Dynamics(self.wheelbase, search_length=2.5, speed=5, dt=1)
# bz = Bezier(start, goal, self.start_heading, self.goal_heading, self.start_steering,
# self.mapsize, self.car_length, self.car_width, self.wheelbase, self.mapsize * 3, "NoFound")
# bezier_spline = bz.calculation()
x = start[0]
y = start[1]
x_future = x
y_future = y
x_prev = x
y_prev = y
heading_state = self.start_heading
rotate_angle = heading_state
steering_state = self.start_steering
found = 0
cost_list = [[0, [x, y], heading_state, steering_state]]
next_state = cost_list
path_discrete = list([]) # Initialize discrete path
global path
global path_tree
path = [] # Initialize continuous path
tree_leaf = [[x, y]] # Initialize search tree leaf (search failed)
search = 1
step = 1
path_tree = [] # Initialize continuous path for search trees
while found != 1:
if search >= self.freeGrid_num:
break
cost_list.sort(key=lambda x: x[0])
next_state = cost_list.pop(0)
path_discrete.append(np.round(next_state[1]))
path.append(next_state[1])
[x, y] = next_state[1]
[x_future, y_future] = [x, y]
heading_state = next_state[2]
steering_state = next_state[3]
if step > 1:
[x_prev, y_prev] = path[step - 1]
step += 1
rotate_angle = heading_state
if sqrt(
np.dot(np.subtract([x, y], goal), np.subtract(
[x, y], goal))) <= self.tolerance:
found = 1
rotate_matrix = [[np.cos(rotate_angle), -np.sin(rotate_angle)],
[np.sin(rotate_angle),
np.cos(rotate_angle)]]
action = (np.dot(displacement_rear, rotate_matrix)).tolist()
action_future = np.dot(displacement_rear_future, rotate_matrix)
candidates = np.add([x, y], action)
candidates_future = np.add([x_future, y_future], action_future)
candidates_round = np.round(candidates).astype(int)
heading_state = np.add(heading_state, theta)
invalid_ID = [
((candidates_round[i] == path).all(1).any() |
(candidates_round[i] == self.danger_zone).all(1).any() |
(candidates_round[i] == tree_leaf).all(1).any())
for i in range(len(candidates_round))
]
remove_ID = np.unique(
np.where((candidates < 0) | (candidates > self.mapsize))[0])
candidates = np.delete(candidates, remove_ID, axis=0)
candidates_future = np.delete(candidates_future, remove_ID, axis=0)
heading_state = np.delete(heading_state, remove_ID, axis=0)
candidates = np.delete(candidates, np.where(invalid_ID), axis=0)
candidates_future = np.delete(candidates_future,
np.where(invalid_ID),
axis=0)
heading_state = np.delete(heading_state,
np.where(invalid_ID),
axis=0)
if len(candidates) > 0:
cost_list = []
for i in range(len(candidates)):
diff = np.square(candidates[i] - self.bezier_spline)
min_dis = min(np.sqrt(np.sum(diff, axis=1)))
diff_future = np.square(candidates_future[i] -
self.bezier_spline)
min_dis_future = min(np.sqrt(np.sum(diff_future, axis=1)))
total_cost = min_dis + min_dis_future
cost_list.append([
total_cost, candidates[i], heading_state[i],
steering_step[i]
])
else:
search += 1
if (next_state[1] == tree_leaf).all(1).any():
tree_leaf.append(np.round([x_prev, y_prev]))
else:
tree_leaf.append((np.round([x, y])).tolist())
x = start[0]
y = start[1]
x_future = x
y_future = y
x_prev = x
y_prev = y
heading_state = self.start_heading
rotate_angle = heading_state
steering_state = self.start_steering
found = 0
cost_list = [[0, [x, y], heading_state, steering_state]]
next_state = cost_list
path_discrete = list([]) # Initialize discrete path
path_tree.append(path)
path = [] # Initialize continuous path
step = 1
bz_last = Bezier(next_state[1], goal, next_state[2], self.goal_heading,
self.start_steering, self.mapsize, self.car_length,
self.car_width, self.wheelbase, self.tolerance * 3,
"Found")
bz_last = bz_last.calculation()
path.append(bz_last)
return path, path_tree
def calculation(self):
start = self.start
goal = self.goal
# the vehicle state in 1.5 second
theta, theta_forward, displacement_rear, displacement_rear_forward, steering_step = vehicle_model(
self.wheelbase, search_length=2.5, speed=5, dt=1.5)
# the raw initialization
x = start[0]
y = start[1]
x_forward = x
y_forward = y
x_prev = x
y_prev = y
heading_state = self.start_heading
rotate_angle = heading_state # the raw initilization
steering_state = self.start_steering
found = 0
cost_list = [[0, [x, y], heading_state, steering_state]]
#print("cost_list",cost_list)
next_state = cost_list # the raw initilization
path_discrete = list([]) # Initialize discrete path
path = [] # Initialize continuous path
tree_leaf = [[x, y]] # Initialize search tree leaf (search failed)
path_tree = [] # Initialize continuous path for search trees
search = 1 # the A_star search iterators
step = 1 # the time step for MPC
while found != 1:
if search >= self.freeGrid_num: # the map has been searched all
break
cost_list.sort(
key=lambda x: x[0]
) # sort by the x[0] element #([total_cost, candidates[i], heading_state[i], steering_step[i]])
# [0.5702793293265661, array([ 2.49501973, 24.86343515]), 0.10936082940740502, 0.08726646259971649]
next_state = cost_list.pop(
0) # pop the first element/ the min cost
path_discrete.append(np.round(
next_state[1])) # round [x,y] of the candidate
path.append(next_state[1]) # [x,y] of the candidate
[x, y] = next_state[1]
[x_forward, y_forward] = [x, y] # the raw initialization
heading_state = next_state[2]
steering_state = next_state[3]
#############################################################################################################################
# step is the flag used for MPC control, the online rolling optimization
if step > 1:
[x_prev, y_prev] = path[step - 1] # not the very first step
step += 1
rotate_angle = heading_state # the raw initilization
# reach the goal position
if sqrt(
np.dot(np.subtract([x, y], goal), np.subtract(
[x, y], goal))) <= self.tolerance:
found = 1
rotate_matrix = [[np.cos(rotate_angle), -np.sin(rotate_angle)],
[np.sin(rotate_angle),
np.cos(rotate_angle)]]
action = (np.dot(
displacement_rear,
rotate_matrix)).tolist() #dot()返回的是两个数组的点积(dot product)
#print(" action ##############################################")
#print( action )
action_forward = np.dot(displacement_rear_forward, rotate_matrix)
candidates = np.add([x, y], action)
#print(" candidates ###########################################")
#print( candidates )
candidates_forward = np.add([x_forward, y_forward], action_forward)
candidates_round = np.round(candidates).astype(int)
heading_state = np.add(heading_state, theta)
#print(" len(candidates_round) ###########################################")
#print( len(candidates_round) )
invalid_ID = [
((candidates_round[i] == path).all(1).any() |
(candidates_round[i] == self.danger_zone).all(1).any() |
(candidates_round[i] == tree_leaf).all(1).any())
for i in range(len(candidates_round))
]
remove_ID = np.unique(
np.where((candidates < 0) | (candidates > self.mapsize))[0])
candidates = np.delete(candidates, remove_ID, axis=0)
candidates_forward = np.delete(candidates_forward,
remove_ID,
axis=0)
heading_state = np.delete(heading_state, remove_ID, axis=0)
candidates = np.delete(candidates, np.where(invalid_ID), axis=0)
candidates_forward = np.delete(candidates_forward,
np.where(invalid_ID),
axis=0)
heading_state = np.delete(heading_state,
np.where(invalid_ID),
axis=0)
################################################################################################################################
# calculate the cost and add the candidates's cost to the cost_list
#Due to the usage of Bezier spline, no expand grid or heuristic layer is pre-computed as in Min_cost, making it very efficient.
if len(candidates) > 0:
cost_list = []
#print(" len(candidates) ###########################################")
#print( len(candidates) )
for i in range(len(candidates)):
diff = np.square(
candidates[i] -
self.bezier_spline) # using the Bezier spline
min_dis = min(np.sqrt(np.sum(diff, axis=1)))
diff_forward = np.square(
candidates_forward[i] -
self.bezier_spline) # using the Bezier spline
min_dis_forward = min(np.sqrt(np.sum(diff_forward,
axis=1)))
total_cost = min_dis + min_dis_forward
cost_list.append([
total_cost, candidates[i], heading_state[i],
steering_step[i]
])
else:
# the very first step 1 / the raw initilaztion
search += 1
if (next_state[1] == tree_leaf).all(1).any():
tree_leaf.append(np.round([x_prev, y_prev]))
else:
tree_leaf.append((np.round([x, y])).tolist())
x = start[0]
y = start[1]
x_forward = x
y_forward = y
x_prev = x
y_prev = y
heading_state = self.start_heading
rotate_angle = heading_state
steering_state = self.start_steering
found = 0
cost_list = [[0, [x, y], heading_state, steering_state]]
next_state = cost_list
path_discrete = list([]) # Initialize discrete path
path_tree.append(path)
path = [] # Initialize continuous path
step = 1
# for the last point
bz_last = Bezier(next_state[1], goal, next_state[2], self.goal_heading,
self.start_steering, self.mapsize, self.car_length,
self.car_width, self.wheelbase, self.tolerance * 3,
"Found")
bz_last = bz_last.calculation()
#print(" bz_last ###########################################")
#print( bz_last )
path.append(bz_last)
return path, path_tree
def setPos(self, xs, ys):
self._poscurve = Bezier(xs,ys)
class BezierLine(object):
def __init__(self):
self._poscurve = Bezier([],[])
self._hcurve = Bezier([],[])
self._scurve = Bezier([],[])
self._vcurve = Bezier([],[])
self._radcurve = Bezier([],[])
self._alphacurve = Bezier([],[])
def setPos(self, xs, ys):
self._poscurve = Bezier(xs,ys)
def setHSV(self, hs, ss, vs):
self._hcurve = Bezier(hs,[0]*len(hs))
self._scurve = Bezier(ss,[0]*len(ss))
self._vcurve = Bezier(vs,[0]*len(vs))
def setRad(self, rads):
self._radcurve = Bezier(rads,[0]*len(rads))
def setAlpha(self, alpha):
self._alphacurve = Bezier(alpha,[0]*len(alpha))
def getPos(self, t):
return self._poscurve(t)
def getHSV(self, t):
return self._hcurve(t)[0],self._scurve(t)[0],self._vcurve(t)[0]
def getRGB(self, t):
val = colorsys.hsv_to_rgb(self._hcurve(t)[0],self._scurve(t)[0],self._vcurve(t)[0])
val2 = []
for i in range(len(val)):
val2 += [val[i]*256]
return tuple(val2)
def getRad(self, t):
return self._radcurve(t)[0]
def getAlpha(self, t):
return self._alphacurve(t)[0]
def draw(self, drw, mirror=True):
time = 0
dt = .005
xold,yold = self.getPos(time)
x,y = self.getPos(time +dt)
ds = ((x-xold)**2 + (y-yold)**2)**.5
rad = self.getRad(0)
time += dt
while time < 1:
x,y = self.getPos(time)
alpha = self.getAlpha(time)
ds = ((x-xold)**2 + (y-yold)**2)**.5
rad = self.getRad(0)
r,g,b = self.getRGB(time)
drw.brush(x,y, r,g,b, rad, ds,alpha)
if mirror:
drw.brush(drw.getWidth()-x, y, r,g,b,rad, ds, alpha)
time += dt
xold, yold = x, y
def copy(self):
bz = BezierLine()
bz._poscurve = self._poscurve.copy()
bz._hcurve = self._hcurve.copy()
bz._scurve = self._scurve.copy()
bz._vcurve = self._vcurve.copy()
bz._radcurve = self._radcurve.copy()
bz._alphacurve = self._alphacurve.copy()
return bz
def randomize(self, positionmax=0, colormax=0, radiusmax=0,alphamax=0):
self._poscurve.randomjostle(positionmax)
self._hcurve.randomjostle(colormax)
self._scurve.randomjostle(colormax)
self._vcurve.randomjostle(colormax)
self._radcurve.randomjostle(radiusmax)
self._alphacurve.randomjostle(alphamax)
def redistributePoints(self, n):
self._poscurve.redistributePoints(n)
self._hcurve.redistributePoints(n)
self._scurve.redistributePoints(n)
self._vcurve.redistributePoints(n)
self._radcurve.redistributePoints(n)
self._alphacurve.redistributePoints(n)
def __init__(self):
self._poscurve = Bezier([], [])
self._hcurve = Bezier([], [])
self._scurve = Bezier([], [])
self._vcurve = Bezier([], [])
self._radcurve = Bezier([], [])
self._alphacurve = Bezier([], [])
def plan_flight(p0,
v0,
psi0,
t0,
trgt,
trgt_cpts,
pastCpts,
pastTimes,
params,
tf=None):
"""
"""
randomizer = 0
while True:
ndim = 2
vf = params.monSpeed
psif = 2 * np.pi * np.random.rand()
if tf is None:
tf = t0 + params.tflight
dt = tf - t0
assert dt >= 0, f'dt should be >= 0, t0: {t0}, tf: {tf}'
nveh = pastCpts.shape[0] // ndim
trgt_traj = Bezier(trgt_cpts, t0=t0, tf=tf)
bounds = Bounds(min(p0) - 250, max(p0) + 250)
x0 = init_guess_f(p0, trgt, v0, vf, psi0, psif, dt, params.deg)
x0 += np.random.randn() * randomizer
def fn(x):
return cost_f(x, trgt, p0, v0, vf, psi0, psif, dt, params.deg,
params)
cons = [{
'type':
'ineq',
'fun':
lambda x:
nonlinear_constraints_f(x, p0, v0, vf, psi0, psif, t0, tf, nveh,
trgt_traj, pastCpts, pastTimes, params)
}]
results = minimize(fn,
x0,
constraints=cons,
bounds=bounds,
method='SLSQP',
options={
'maxiter': 250,
'disp': True,
'iprint': params.iprint
})
if not results.success:
print(results.message)
print(f'ETarget: {trgt}')
print(f'Psif: {psif}')
print(f'Cost: {fn(results.x)}')
print(f'Nonlcon:')
temp = cons[0]['fun'](results.x)
names = iter(['sep', 'max', 'min', 'max ang', 'NFZ'])
print(next(names))
for val in temp:
if val == 99999:
print('###')
print(next(names))
else:
print(np.round(val, 3), end=', ')
cost_f(results.x,
trgt,
p0,
v0,
vf,
psi0,
psif,
dt,
params.deg,
params,
disp=True)
print()
randomizer += 5
# if randomizer > 10:
# raise Exception('Maximum iterations met!')
#
y = reshape_f(results.x, p0, v0, vf, psi0, psif, dt, params.deg)
newTraj = Bezier(y, t0=t0, tf=tf)
#
# newTraj.plot()
# plt.title('Trajectory')
# newTraj.diff().normSquare().elev(params.degElev).plot()
# plt.title('Norm Square')
# 0/0
# if min(cons[0]['fun'](results.x)) < -100000:
# newTraj.diff().normSquare().elev(params.degElev).plot()
# plt.title('Norm Square')
# 0/0
# if np.any(temp[-31:] < 0):
# (newTraj.elev(params.degElev) - trgt_traj).normSquare().plot()
y = reshape_f(results.x, p0, v0, vf, psi0, psif, dt, params.deg)
newTraj = Bezier(y, t0=t0, tf=tf)
if results.success:
break
elif randomizer > 15:
break
break
return newTraj
def setHSV(self, hs, ss, vs):
self._hcurve = Bezier(hs, [0] * len(hs))
self._scurve = Bezier(ss, [0] * len(ss))
self._vcurve = Bezier(vs, [0] * len(vs))
def setAlpha(self, alpha):
self._alphacurve = Bezier(alpha,[0]*len(alpha))
def setAlpha(self, alpha):
self._alphacurve = Bezier(alpha, [0] * len(alpha))
class TestBezier(unittest.TestCase):
def setUp(self):
self.b0 = Bezier([(1, 1), (2, 3), (4, 3), (3, 1)])
self.b1 = Bezier([(-1,1), (3,3), (4,-1), (2,-1)])
def test_bezier(self):
test = np.array([self.b0.point(i/10.0) for i in range(11)])
ref = np.array([(1.000, 1.00),
(1.326, 1.54),
(1.688, 1.96),
(2.062, 2.26),
(2.424, 2.44),
(2.750, 2.50),
(3.016, 2.44),
(3.198, 2.26),
(3.272, 1.96),
(3.214, 1.54),
(3.000, 1.00)])
self.assertTrue(np.allclose(test, ref))
def test_eq(self):
self.assertEqual(self.b0, copy(self.b0))
def test_elevate(self):
test1 = self.b1.elevate()
ref1 = Bezier([(-1.0, 1.0), (2.0, 2.5), (3.5, 1.0), (3.5, -1.0), (2.0, -1.0)])
test2 = self.b1.elevate(degree=2)
ref2 = Bezier([(-1.0, 1.0), (1.4, 2.2), (2.9, 1.6), (3.5, 0.2), (3.2, -1.0), (2.0, -1.0)])
self.assertEqual(test1, ref1)
self.assertEqual(test2, ref2)
def test_split(self):
ref_l = Bezier([(-1.0, 1.0), (1.0, 2.0), (2.25, 1.5), (2.75, 0.75)])
ref_r = Bezier([(2.75, 0.75), (3.25, 0.0), (3.0, -1.0), (2.0, -1.0)])
test_l, test_r = self.b1.split(t=0.5)
self.assertEqual(test_l, ref_l)
self.assertEqual(test_r, ref_r)
def test_curvature(self):
ref = np.array([
0.134164,
0.219124,
0.340136,
0.471343,
0.566666,
0.625815,
0.713340,
0.904176,
1.155625,
1.095216,
0.670820])
test = np.array([self.b1.curvature(t=t) for t in np.linspace(0.0, 1.0, num=11)])
self.assertTrue(np.allclose(test, ref))
def test_derivated(self):
b = Bezier([(-4.0, 0.00), (4.0, 0.00), (-2.0, 0.00), (2.0, 0.00)])
d = b.derivative
print(d.point(t=0.08))
def bezier_2_circles(p1, p2, p3, p4, scale_factor):
"""
This function converts a provided cubic bezier curve into a series of G-Code
executable circular interpolations
When called, this function will directly print to screen the necessary G-Code
commands to draw the provided bezier curve
"""
# Create Bezier Curve from Points
curve = Bezier(p1, p2, p3, p4)
# Set current Bezier curve to the curve we receive
current_bezier = curve
# Set inflexion flag to 0
curvature_changed = 0
# Create buffer to store all pushed Bezier curve segments
bezier_buffer = []
# Define error threshold to determine how close the estimates should be
error_threshold = 0.0000001
# Continue until we go through the entire curve
while (True):
# Find the first inflexion point on the curve
if current_bezier.get_inflexion_point() == -1:
# If no inflexion point found on curve, skip this step
pass
else:
if curvature_changed:
# Ignore future inflexion detections
pass
else:
# If an inflexion point was found
# Calculate the split point of the Bezier at this found inflexion
[b1, b2] = current_bezier.split_bezier(
current_bezier.get_inflexion_point())
# Set the first segment to this
current_bezier = b1
# Push the remaining curve onto the buffer
bezier_buffer.append(b2)
# Indicate an inflexion occurred by setting the flag to 1
curvature_changed = 1
# Determine approximate angle of the arc
_angle = current_bezier.get_angle()
# Determine what angular range the curve segment falls into
if abs(_angle) <= np.pi / 2:
pass
elif abs(_angle) <= np.pi:
# Split the curve in half
[b1, b2] = current_bezier.split_bezier(0.5)
# Set first section to the current arc
current_bezier = b1
# Push remainder onto the buffer
bezier_buffer.append(b2)
else:
# Loop until curve section is under 90 degrees
t = 0.5
while True:
[b1, b2] = current_bezier.split_bezier(t)
# Continue dividing until the first segment is under 90 degrees
if abs(b1.get_angle()) < np.pi / 2:
# If first section is under 90 degrees, break from loop
break
else:
t *= 0.5
# Set current bezier to this section
current_bezier = b1
bezier_buffer.append(b2)
# ----- Approximate the Bezier Segment -----
# Get intersection point of the two tangents
A = generate_line(current_bezier.p1, current_bezier.p2)
B = generate_line(current_bezier.p3, current_bezier.p4)
# Determine the incentre points of lines A and B
X = find_intersection(A, B)
if X == None and len(bezier_buffer) > 0:
# Just move onto the next section for now
current_bezier = bezier_buffer.pop(len(bezier_buffer) - 1)
elif X == None and len(bezier_buffer) == 0:
# CHECK WHY THE BUFFER BECOMES EMPTY!!
# If there's no intersection, skip
print bezier_buffer
break
else:
# Find the incentre of points 1,4 and X
G = find_incentre(current_bezier.p1, current_bezier.p4, X)
# Create a circle from points 1,4 and G, find the centre point
centre = generate_circle(current_bezier.p1, current_bezier.p4, G)
# Get the radius of the generated circle
radius = np.linalg.norm([
current_bezier.p1.x - centre.x, current_bezier.p1.y - centre.y
])
# Define two test points to determine the error of the arc to the curve
# Chose 25% and 75% along the curve for good estimates
tp_1 = current_bezier.get_point(0.25)
tp_2 = current_bezier.get_point(0.75)
# Determine the average distance of the Bezier endpoints to the circle centre
distance_1 = np.linalg.norm([tp_1.x - centre.x, tp_1.y - centre.y])
distance_2 = np.linalg.norm([tp_2.x - centre.x, tp_2.y - centre.y])
# Get sum of squares of the two errors
error = (radius - distance_1) * (radius - distance_1) + (
radius - distance_2) * (radius - distance_2)
if error > error_threshold:
# Split the Curve in Half
[b1, b2] = current_bezier.split_bezier(0.5)
# Push the other half to the buffer
current_bezier = b1
bezier_buffer.append(b2)
else:
# Determine if we need to move around the circle in a clockwise or anticlockwise direction
# Determine the chord P1 to P4
L = generate_line(current_bezier.p1, current_bezier.p4)
# Assess direction of L
if current_bezier.p4.x >= current_bezier.p1.x:
if centre.y < -(L[0] * centre.x + L[2]) / L[1]:
# If circle centre is below L, move in clockwise fashion
direction = 2
else:
direction = 3
else:
# If line points to the left
if centre.y < -(L[0] * centre.x + L[2]) / L[1]:
# Move in an anticlockwise direction
direction = 3
else:
direction = 2
# Return the circular command with 6 decimal place accuracy
print "G{0:d} ".format(direction) + \
"X{0:.6f} ".format(current_bezier.p4.x * scale_factor) + \
"Y{0:.6f} ".format(current_bezier.p4.y * scale_factor) + \
"I{0:.6f} ".format((centre.x - current_bezier.p1.x) * scale_factor) + \
"J{0:.6f}".format((centre.y - current_bezier.p1.y)* scale_factor)
if len(bezier_buffer) == 0:
break
else:
# Get the next Bezier segment from the buffer to work on in the next loop
current_bezier = bezier_buffer.pop(len(bezier_buffer) - 1)
#!/usr/bin/env python3 # -*- coding: utf-8 -*- """ Created on Thu Jul 20 17:09:14 2017 @author: Montanari, Daniel ([email protected]) """ #import quadraticBezier as Bezier from bezier import BezierCurve as Bezier #bezier = Bezier.quadraticBezier(0, 0, 50, 50) bezier = Bezier() xControl = [0 , 25, 50, 75] yControl = [0 , 10, -8, -35] bezier.setControlPoints(xControl, yControl) bezier.setDeltaT(0.002) bezier.generateBezierCurve() bezier.draw(showGrid = True, showArrows=True)
print('Testing reshape_f')
ytrue = np.array([[0, 1, 2, 3, 4, 5], [3, 3, 6, 2, 9, 9]], dtype=float)
x = np.array([2, 3, 5, 6, 2, 9])
y = reshape_f(x, p0, v0, vf, psi0, psif, dt, deg)
if not np.all(y == ytrue):
print('--> [!] Test failed')
else:
print('--> Test passed')
print('Testing init_guess_f')
x0true = np.array([2, 3, 5, 5, 7, 9], dtype=float)
x0 = init_guess_f(p0, trgt, v0, vf, psi0, psif, dt, deg)
if not np.all(x0 == x0true):
print('--> [!] Test failed')
else:
print('--> Test passed')
print('Testing init_guess_m')
x0true = np.array([], dtype=float)
x0 = init_guess_m(p0, trgt, v0, vf, psi0, dt, deg, params)
c = Bezier(reshape_m(x0, p0, v0, psi0, dt, deg, trgt, params.innerR))
ax = c.plot()
ax.add_artist(Circle(trgt, radius=params.innerR, fill=None))
ax.set_aspect('equal')
# cpts1 = np.array([[0, 1, 2, 3, 4, 5],
# [3, 4, 6, 2, 7, 9]], dtype=float)
# cpts2 = np.array([[5, 4, 3, 2, 1, 0],
# [8, 3, 6, 6, 2, 5]], dtype=float)
,
simulate=simulate_plot)
# capture all the specified control points
# they are relative to the plotter's starting point
specified_control_points = [plotter.initial_position]
for point in sys.argv[1].split(' '):
specified_control_points.append([
plotter.initial_position[0] + float(point.split(',')[0]),
plotter.initial_position[1] + float(point.split(',')[1])
])
if '--control' in sys.argv:
for point in specified_control_points:
if point == plotter.initial_position:
continue
plotter.move_to(point)
# back to start
plotter.move_to(plotter.initial_position)
b = Bezier(specified_control_points)
path = b.generate_path()
# plot curve points
for p in path:
plotter.move_to(p)
# wraps things up with the sim
plotter.finish()
