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