def update_bezier_curve(self):
if self.curve_id != CurveMode.NO_MODE:
if self.curve is not None:
self.curve.update_curve(self.points)
else:
self.curve = BezierCurve(self.points)
self.scene.addItem(self.curve)
def follow_path(self, path: BezierCurve, velocity, kP, kD):
start_time = self.time + 0.00001
time = self.time - start_time
t = 1
last_time = 0
last_error = 0
# while self.robot_pos < path.distance:
# while time < 3000:
goal_x = path.x_list[1]
goal_y = path.y_list[1]
while (NerdyMath.distance_formula(self.robot_x, self.robot_y,
path.get_last_x(), path.get_last_y())
> 2) and t != len(path.y_list):
print(time)
print(t)
time = self.time - start_time
if t >= len(path.x_list):
t = len(path.x_list) - 1
goal_x = path.x_list[t]
goal_y = path.y_list[t]
distance = path.dist_list[t]
target_angle = math.degrees(
math.atan2(goal_x - self.robot_x, goal_y - self.robot_y))
angle = -(360 - self.robot_angle_deg) % 360
error = target_angle - angle
if error >= 180:
error -= 360
elif error <= -180:
error += 360
rot_velocity = error * kP + (error - last_error) / (time -
last_time) * kD
self.update(velocity + rot_velocity, velocity - rot_velocity)
# if self.robot_pos > distance:
if NerdyMath.distance_formula(self.robot_x, self.robot_y, goal_x,
goal_y) < 2:
t += 1
last_error = error
last_time = time
# print(t, goal_x, goal_y)
# print(self.robot_x, self.robot_y)
plt.plot(self.x_list, self.y_list)
plt.plot(path.x_list, path.y_list)
plt.xlabel('x')
plt.ylabel('y')
plt.axis([-100, 100, -100, 100])
plt.legend(['robot position', 'path ' + str(kP) + ',' + str(kD)])
plt.show()
def change_curve(self, curve_id):
if curve_id == CurveMode.NO_MODE and (self.curve is not None):
self.curve_id = curve_id
if self.is_merge:
self.merged_group.removeFromGroup(self.curve)
self.scene.removeItem(self.curve)
self.curve = None
else:
was_marge = False
if self.is_merge:
was_marge = True
self.un_merge()
self.curve_id = curve_id
if self.curve is None:
self.curve = BezierCurve(self.points)
else:
self.curve.update_curve(self.points)
if was_marge:
self.merge()
self.scene.addItem(self.curve)
def createCurves(self):
curves = []
if self.controlPointsCount > 0:
sliceStart = 0
sliceEnd = sliceStart + 4
for i in xrange(self.controlPointsCount -
(0 if self.closed else 1)):
curves.append(
BezierCurve(self.allPoints[:, sliceStart:sliceEnd], False))
sliceStart += 3
sliceEnd += 3
return curves
def drive_pure_pursuit_4(self, path: BezierCurve, min_lookahead, velocity,
kP, going_forwards):
start_time = self.time
time = self.time - start_time
t = 0
# while (NerdyMath.distance_formula(self.robot_x, self.robot_y, path.get_last_x(),
# path.get_last_y()) > 1) and t != len(path.y_list):
# while NerdyMath.distance_formula(self.robot_x, self.robot_y, path.get_last_x(), path.get_last_y()) > 2 :
while time < 73.6:
print(t, time)
time = self.time - start_time
# if t >= len(path.x_list):
# t = len(path.x_list) - 1
t = path.get_closest_point(self.robot_x, self.robot_y)
x1 = path.x_list[t]
y1 = path.y_list[t]
x2 = path.x_list[t - 1]
y2 = path.y_list[t - 1]
slope = (y2 - y1) / (x2 - x1)
y_int = y2 - slope * x2
cross_path_x = (self.robot_y - y_int +
(slope**-1) * self.robot_x) / (slope + slope**-1)
cross_path_y = slope * cross_path_x + y_int
cros_path_error = NerdyMath.distance_formula(
self.robot_x, self.robot_y, cross_path_x, cross_path_y)
lookahead = min_lookahead + abs(cros_path_error * kP)
a = (1 + slope**2)
b = (-2 * self.robot_x) + (2 * slope * (y_int - self.robot_y))
c = (self.robot_x**2) + (y_int - self.robot_y)**2 - lookahead**2
# if (numpy.sign(slope) == 1 and going_forwards) or (numpy.sign(slope) == -1 and not going_forwards):
if numpy.sign(slope) == -1:
goal_x = (-b - math.sqrt(b**2 - 4 * a * c)) / (2 * a)
# elif (numpy.sign(slope) == -1 and going_forwards) or (numpy.sign(slope) == 1 and not going_forwards):
elif numpy.sign(slope) == 1:
goal_x = (-b + math.sqrt(b**2 - 4 * a * c)) / (2 * a)
goal_y = slope * goal_x + y_int
drive_radius = (lookahead**2) / (2 * (self.robot_x - goal_x))
# print(drive_radius)
inner_vel = velocity * (drive_radius -
(self.width / 2)) / (drive_radius +
(self.width / 2))
target_angle = math.degrees(
math.atan2(x1 - self.robot_x, y1 - self.robot_y))
angle = -(360 - self.robot_angle_deg) % 360
error = target_angle - angle
if error >= 180:
error -= 360
elif error <= -180:
error += 360
# if numpy.sign(drive_radius) == 1:
# if numpy.sign(error) == 1:
if numpy.sign(slope) == 1:
if (numpy.sign(
math.cos(self.robot_angle) * (goal_x - self.robot_x) -
math.sin(self.robot_angle) *
(goal_y - self.robot_y))) == 1:
self.update(inner_vel, velocity)
else:
self.update(velocity, inner_vel)
elif numpy.sign(slope) == -1:
if (numpy.sign(
math.cos(self.robot_angle) * (goal_x - self.robot_x) -
math.sin(self.robot_angle) *
(goal_y - self.robot_y))) == -1:
self.update(inner_vel, velocity)
else:
self.update(velocity, inner_vel)
elif numpy.sign(slope) == 0:
self.update(velocity, velocity)
# print(velocity, inner_vel, 'vel')
# if NerdyMath.distance_formula(self.robot_x, self.robot_y, x1, y1) < 15:
# t += 1
print(drive_radius**-1)
plt.plot(self.x_list, self.y_list)
plt.plot(path.x_list, path.y_list)
plt.xlabel('x')
plt.ylabel('y')
plt.axis([-100, 200, -100, 200])
plt.legend(['robot position', 'path ' + ',' + str(kP)])
plt.show()
from BezierCurve import BezierCurve
from Point2D import Point2D
import numpy as np
from matplotlib import pyplot as plt
from matplotlib.widgets import Slider, CheckButtons
from matplotlib.patches import Circle
p = [Point2D(0,0), Point2D(5,15)]
B = BezierCurve(p)
B.load_control_points_from_file("control_points.txt")
trackwidth = 1
def plot(B):
plt.style.use("classic")
fig, ax = plt.subplots()
ax.grid()
plt.subplots_adjust(bottom=0.3)
plt.title("Bezier Curve")
plt.xlabel("X [position]")
plt.ylabel("Y [Position]")
ax.set_aspect("equal")
bezier_main_path, = plt.plot([], [], color ='black', linestyle="dashed")
points, = plt.plot([], [], color='gray', marker='x', linestyle="dashed")
left_trajectory, = plt.plot([], [], color="red")
right_trajectory, = plt.plot([], [], color="red")
curvature_circle = Circle((0,0), 0)
curvature_circle.fill = False
curvature_circle.set_visible(False)
ax.add_artist(curvature_circle)
class PointGroup(QtCore.QObject):
def __init__(self, scene, group_id):
super(PointGroup, self).__init__()
print("New group id %s" % group_id)
self.points = []
self.scene = scene
self.group_id = group_id
self.is_visible = True
self.is_merge = False
self.merged_group = None
self.convex_hull = None
self.curve_id = CurveMode.NO_MODE
self.curve = None
def clear_group(self):
self.remove_convex_hull()
self.remove_curve()
for p in self.points:
self.scene.removeItem(p)
def remove_curve(self):
if self.curve_id != CurveMode.NO_MODE:
self.scene.removeItem(self.curve)
self.curve_id = CurveMode.NO_MODE
def increase_degree_by_one(self):
new_points = self.curve.increase_by_one()
for p in self.points:
self.scene.removeItem(p)
if self.is_merge:
self.merged_group.removeFromGroup(p)
self.points = new_points
for p in self.points:
self.scene.addItem(p)
if self.is_merge:
self.merged_group.addToGroup(p)
self.update_convex_hull()
def decrease_degree_by_one(self):
new_points = self.curve.decrease_by_one()
for p in self.points:
self.scene.removeItem(p)
self.points = new_points
for p in self.points:
self.scene.addItem(p)
self.update_convex_hull()
def degree(self):
if self.curve_id == CurveMode.NO_MODE:
return len(self.points)
else:
return self.curve.get_degree()
def change_curve(self, curve_id):
if curve_id == CurveMode.NO_MODE and (self.curve is not None):
self.curve_id = curve_id
if self.is_merge:
self.merged_group.removeFromGroup(self.curve)
self.scene.removeItem(self.curve)
self.curve = None
else:
was_marge = False
if self.is_merge:
was_marge = True
self.un_merge()
self.curve_id = curve_id
if self.curve is None:
self.curve = BezierCurve(self.points)
else:
self.curve.update_curve(self.points)
if was_marge:
self.merge()
self.scene.addItem(self.curve)
def draw_convex_hull(self):
self.convex_hull = ConvexHull(self.points)
if self.is_merge:
self.merged_group.addToGroup(self.convex_hull)
else:
self.scene.addItem(self.convex_hull)
def remove_convex_hull(self):
if self.convex_hull is not None:
if self.is_merge:
self.merged_group.removeFromGroup(self.convex_hull)
else:
self.scene.removeItem(self.convex_hull)
self.convex_hull = None
def set_selected(self, value):
if not self.is_merge:
for p in self.points:
p.setSelected(value)
if self.curve_id != CurveMode.NO_MODE:
self.curve.setSelected(value)
if self.convex_hull is not None:
self.convex_hull.setSelected(value)
else:
self.merged_group.setSelected(value)
def set_flag(self, flag, value):
if not self.is_merge:
for p in self.points:
p.setFlag(flag, value)
if self.convex_hull is not None:
self.convex_hull.setFlag(flag, value)
if self.curve_id != CurveMode.NO_MODE:
self.curve.setFlag(flag, value)
else:
self.merged_group.setFlag(flag, value)
def get_id(self):
return self.group_id
def merge(self):
other_list = []
if self.convex_hull is not None:
other_list.append(self.convex_hull)
if self.curve_id != CurveMode.NO_MODE:
other_list.append(self.curve)
self.merged_group = ItemGroup(self, self.scene, self.points,
other_list)
self.is_merge = True
self.merged_group.setSelected(True)
def un_merge(self):
self.scene.destroyItemGroup(self.merged_group)
self.is_merge = False
self.merged_group = None
if self.curve_id != CurveMode.NO_MODE:
self.change_curve(CurveMode.NO_MODE)
self.change_curve(CurveMode.BEZIER_CURVE)
def set_visible(self, visible):
self.is_visible = visible
if self.is_merge:
self.merged_group.setVisible(visible)
else:
for p in self.points:
p.setVisible(visible)
if self.convex_hull is not None:
self.convex_hull.setVisible(visible)
if self.curve_id != CurveMode.NO_MODE:
self.curve.setVisible(visible)
def is_group_visible(self):
return self.is_visible
def update_convex_hull(self):
if self.convex_hull is not None:
self.remove_convex_hull()
self.draw_convex_hull()
def update_bezier_curve(self):
if self.curve_id != CurveMode.NO_MODE:
if self.curve is not None:
self.curve.update_curve(self.points)
else:
self.curve = BezierCurve(self.points)
self.scene.addItem(self.curve)
def add_point(self, point):
self.points.append(point)
self.scene.addItem(point)
self.update_convex_hull()
if self.is_merge:
self.merged_group.addToGroup(point)
if self.curve_id != CurveMode.NO_MODE:
self.curve.add_point(point)
def delete_point(self, point):
idx = 0
for p in self.points:
if p == point:
del self.points[idx]
idx += 1
if self.is_merge:
self.merged_group.removeFromGroup(point)
else:
self.scene.removeItem(point)
self.update_convex_hull()
self.curve.update_curve(self.points)
def update_group(self):
print("Update group")
if self.is_merge:
self.un_merge()
self.update_convex_hull()
if self.curve_id != CurveMode.NO_MODE:
self.curve.update_curve(self.points)
self.merge()
else:
self.update_convex_hull()
self.update_bezier_curve()
import math
import NerdyMath
from BezierCurve import BezierCurve
from MotionProfile import *
from NerdyTrajectory import NerdyTrajectory
from TestTrajectory import TestTrajectory
from Drivetrain import Drivetrain
bezier = BezierCurve(0, 0, 0, 50, 50, 50, 50, 100, 100)
bezier_reverse = BezierCurve(0, 0, 0, 50, -50, 50, -50, 100, 100)
# bezier.graph()
# bezier_2 = BezierCurve(0, 0, 50, 50, 50, 50, 100, 100, 100)
# bezier_3 = BezierCurve(0, 0, 2, 8, 6, 8, 8, 0, 100)
# bezier_4 = BezierCurve(0, 0, 10, 10, 0, -15, -5, 5, 100)
# bezier_5 = BezierCurve(0, 0, 5, 5, 5, 5, 10, 10, 100)
# print(bezier.distance/100)
print(1)
opposite_scale_auto = BezierCurve(100 - 100, 0, 150 - 100, 390, -70 - 100, 135,
-85 - 100, 300, 375)
# print(opposite_scale_auto.distance/375)
# trajectory_1 = NerdyTrajectory(0, 0, 0, 50, 0, 50, 0, 100, 100, 1, 10, 3)
taco = Drivetrain(10, 0.02)
# taco.turn_to_angle_PID(45, 0.1, 0)
# taco.drive_forward_PID(100, 0.1, 0)
# taco.drive_at_heading(90, 0.01, 1000, 2)
# taco.arc_turn(45, 0.1, False, 1)
# taco.radius_turn(20, 1, 126, False)
# taco.drive_motion_profile(-100, 10, 1)
# taco.drive_to_xy(0, -100, 0.5, 0.01)
# taco.drive_pure_pursuit(bezier, 5, 1, 5, 1, True, 1)
from BezierCurve import BezierCurve
from util import *
import random
h = 1000
w = 1000
window = pyglet.window.Window(height=h, width=w)
order = 3 if len(sys.argv) < 2 else int(sys.argv[1])
selected = None
res = 10 if len(sys.argv) < 3 else int(sys.argv[2])
offset_w = w / 10
offset_h = h / 10
init_pts = list([(int(random.uniform(offset_w, w - offset_w)),
int(random.uniform(offset_h, h - offset_h)))
for _ in range(order + 1)])
bez_curve = BezierCurve(order, init=init_pts)
@window.event
def on_mouse_drag(x, y, dx, dy, buttons, modifiers):
global selected
global bez_curve
if buttons == mouse.RIGHT:
if selected is not None:
bez_curve.move_ctrl(selected, x, y)
@window.event
def on_mouse_press(x, y, buttons, modifiers):
global selected
global bez_curve