Providing a GUI for the spline caculations
Fixed bug with import
Added functionality for when e is pressed, points exported to csv
Added functionality for editing points
Added functionality for adding points
Change-Id: Ied5e6155a8ebfbbd6d2eecba34532245197609a8
diff --git a/y2018/control_loops/python/path_edit.py b/y2018/control_loops/python/path_edit.py
new file mode 100644
index 0000000..885b36c
--- /dev/null
+++ b/y2018/control_loops/python/path_edit.py
@@ -0,0 +1,360 @@
+from __future__ import print_function
+import os
+import basic_window
+import random
+import gi
+import numpy as np
+import scipy.spatial.distance
+gi.require_version('Gtk', '3.0')
+from gi.repository import Gdk
+import cairo
+
+import enum
+import csv # For writing to csv files
+
+from basic_window import OverrideMatrix, identity, quit_main_loop
+
+LENGTH_OF_FIELD = 323.65
+PIXELS_ON_SCREEN = 300
+
+def pxToIn(p):
+ return p*LENGTH_OF_FIELD/PIXELS_ON_SCREEN
+
+def inToPx(i):
+ return i*PIXELS_ON_SCREEN/LENGTH_OF_FIELD
+
+def px(cr):
+ return OverrideMatrix(cr, identity)
+
+def draw_px_cross(cr, x, y, length_px, r, g, b):
+ """Draws a cross with fixed dimensions in pixel space."""
+ cr.set_source_rgb(r, g, b)
+ cr.move_to(x, y - length_px)
+ cr.line_to(x, y + length_px)
+ cr.stroke()
+
+ cr.move_to(x - length_px, y)
+ cr.line_to(x + length_px, y)
+ cr.stroke()
+
+def draw_px_x(cr, x, y, length_px1, r, g, b):
+ """Draws a x with fixed dimensions in pixel space."""
+ length_px = length_px1 / np.sqrt(2)
+ cr.set_source_rgb(r, g, b)
+ cr.move_to(x - length_px, y - length_px)
+ cr.line_to(x + length_px, y + length_px)
+ cr.stroke()
+
+ cr.move_to(x - length_px, y + length_px)
+ cr.line_to(x + length_px, y - length_px)
+ cr.stroke()
+
+def draw_points(cr, p, size):
+ for i in range(0, len(p)):
+ draw_px_cross(cr, p[i][0], p[i][1], size, 0, np.sqrt(0.2 * i), 0)
+
+class Mode(enum.Enum):
+ kViewing = 0
+ kPlacing = 1
+ kEditing = 2
+ kExporting = 3
+ kImporting = 4
+
+def display_text(cr, text, widtha, heighta, widthb, heightb):
+ cr.scale(widtha, -heighta)
+ cr.show_text(text)
+ cr.scale(widthb, -heightb)
+
+# Create a GTK+ widget on which we will draw using Cairo
+class GTK_Widget(basic_window.BaseWindow):
+ def __init__(self):
+ super(GTK_Widget, self).__init__()
+
+ # init field drawing
+ # add default spline for testing purposes
+ # init editing / viewing modes and pointer location
+ self.mode = Mode.kPlacing
+ self.x = 0
+ self.y = 0
+
+ self.switch = True
+
+ # update list of control points
+ self.point_selected = False
+ # self.adding_spline = False
+ self.index_of_selected = -1
+ self.new_point = []
+
+ # For the editing mode
+ self.index_of_edit = -1 # Can't be zero beause array starts at 0
+ self.held_x = 0
+
+ # Theo take them from here?
+ self.selected_points = []
+
+ self.reinit_extents()
+
+ #John also wrote this
+ def add_point(self, x, y):
+ if(len(self.selected_points)<4):
+ self.selected_points.append([x,y])
+
+ """set extents on images, this needs to be redone with proper distances"""
+ def reinit_extents(self):
+ self.extents_x_min = -800
+ self.extents_x_max = 800
+ self.extents_y_min = -800
+ self.extents_y_max = 800
+
+ # this needs to be rewritten with numpy, i dont think this ought to have
+ # SciPy as a dependecy
+ def get_index_of_nearest_point(self):
+ cur_p = [[self.x, self.y]]
+ distances = scipy.spatial.distance.cdist(cur_p, self.all_controls)
+
+ return np.argmin(distances)
+
+ # return the closest point to the loc of the click event
+ def get_nearest_point(self):
+ return self.all_controls[self.get_index_of_nearest_point()]
+
+ # Handle the expose-event by updating the Window and drawing
+ def handle_draw(self, cr):
+ print(self.new_point)
+ print("SELF.POINT_SELECTED: " + str(self.point_selected))
+
+ # begin drawing
+ # Fill the background color of the window with grey
+ cr.set_source_rgb(0.5, 0.5, 0.5)
+ cr.paint()
+
+ # Draw a extents rectangle
+ cr.set_source_rgb(1.0, 1.0, 1.0)
+ cr.rectangle(self.extents_x_min, self.extents_y_min,
+ (self.extents_x_max - self.extents_x_min),
+ self.extents_y_max - self.extents_y_min)
+ cr.fill()
+
+ #Drawing the switch and scale in the field
+ cr.move_to(0, 50)
+ cr.show_text('Press "e" to export')
+ cr.show_text('Press "i" to import')
+
+ cr.set_source_rgb(0.3,0.3,0.3)
+ cr.rectangle(-150,-150,300,300)
+ cr.fill()
+ cr.set_source_rgb(0, 0, 0)
+ cr.rectangle(-150,-150,300,300)
+ cr.set_line_join(cairo.LINE_JOIN_ROUND)
+ cr.stroke()
+ cr.set_source_rgb(0, 0, 0)
+ cr.rectangle(inToPx(140-161.825),inToPx(76.575),inToPx(56),inToPx(-153.15))
+ cr.set_line_join(cairo.LINE_JOIN_ROUND)
+ cr.stroke()
+
+ cr.rectangle(inToPx(161.825-24),inToPx(90),inToPx(24),inToPx(-180))
+ cr.set_line_join(cairo.LINE_JOIN_ROUND)
+ cr.stroke()
+
+ cr.set_source_rgb(0.2, 0.2, 0.2)
+ cr.rectangle(inToPx(140-161.825),inToPx(76.575),inToPx(56),inToPx(-153.15))
+ cr.fill()
+
+ cr.rectangle(inToPx(161.825-24),inToPx(90),inToPx(24),inToPx(-180))
+ cr.fill()
+
+ # update all the things
+
+ if self.mode == Mode.kViewing:
+ cr.set_source_rgb(0, 0, 0)
+ cr.move_to(-300, 170)
+ cr.show_text("VIEWING")
+ cr.set_source_rgb(0.5, 0.5, 0.5)
+ # its gonna check for points_added from button_press_action
+ # The behavior of the click is such that it runs twice
+ # This is consistant with graph_edit.py which someone smart wrote
+ # So I'm just going to delete the last element in order to not get
+ # repeating points
+ if len(self.selected_points) > 0:
+ print("SELECTED_POINTS: " + str(len(self.selected_points)))
+ print("ITEMS:")
+ for item in self.selected_points:
+ print(str(item))
+ for i, point in enumerate(self.selected_points):
+ print("I: " + str(i))
+ draw_px_x(cr, point[0], point[1], 10, 0,
+ 0, 0)
+ cr.move_to(point[0], point[1]-15)
+ display_text(cr, str(i), 0.5, 0.5, 2, 2)
+
+ if self.mode == Mode.kPlacing:
+ cr.set_source_rgb(0, 0, 0)
+ cr.move_to(-300, 170)
+ display_text(cr, "ADD", 1, 1, 1, 1)
+ cr.set_source_rgb(0.5, 0.5, 0.5)
+ # its gonna check for points_added from button_press_action
+ # The behavior of the click is such that it runs twice
+ # This is consistant with graph_edit.py which someone smart wrote
+ # So I'm just going to delete the last element in order to not get
+ # repeating points
+ if len(self.selected_points) > 0:
+ print("SELECTED_POINTS: " + str(len(self.selected_points)))
+ print("ITEMS:")
+ for item in self.selected_points:
+ print(str(item))
+ for i, point in enumerate(self.selected_points):
+ print("I: " + str(i))
+ draw_px_x(cr, point[0], point[1], 10, 0,
+ 0, 0)
+ cr.move_to(point[0], point[1]-15)
+ display_text(cr, str(i), 0.5, 0.5, 2, 2)
+ if(i==3):
+ self.mode = Mode.kEditing
+
+ elif self.mode == Mode.kEditing:
+ cr.set_source_rgb(0, 0, 0)
+ cr.move_to(-300, 170)
+ display_text(cr, "EDITING", 1, 1, 1, 1)
+ cr.set_source_rgb(0.5, 0.5, 0.5)
+ if len(self.selected_points) > 0:
+ print("SELECTED_POINTS: " + str(len(self.selected_points)))
+ print("ITEMS:")
+ for item in self.selected_points:
+ print(str(item))
+ for i, point in enumerate(self.selected_points):
+ print("I: " + str(i))
+ draw_px_x(cr, point[0], point[1], 10, 0,
+ 0, 0)
+ cr.move_to(point[0], point[1]-15)
+ display_text(cr, str(i), 0.5, 0.5, 2, 2)
+
+ elif self.mode == Mode.kExporting:
+ cr.set_source_rgb(0, 0, 0)
+ cr.move_to(-300, 170)
+ display_text(cr, "VIEWING", 1, 1, 1, 1)
+ cr.set_source_rgb(0.5, 0.5, 0.5)
+ #its gonna check for points_added from button_press_action
+
+ # The behavior of the click is such that it runs twice
+ # This is consistant with graph_edit.py which someone smart wrote
+ # So I'm just going to delete the last element in order to not get
+ # repeating points
+ if len(self.selected_points) > 0:
+ print("SELECTED_POINTS: " + str(len(self.selected_points)))
+ print("ITEMS:")
+ for item in self.selected_points:
+ print(str(item))
+ for i, point in enumerate(self.selected_points):
+ print("I: " + str(i))
+ draw_px_x(cr, point[0], point[1], 10, 0,
+ 0, 0)
+ cr.move_to(point[0], point[1]-15)
+ display_text(cr, str(i), 0.5, 0.5, 2, 2)
+ elif self.mode == Mode.kImporting:
+ cr.set_source_rgb(0, 0, 0)
+ cr.move_to(-300, 170)
+ display_text(cr, "VIEWING", 1, 1, 1, 1)
+ cr.set_source_rgb(0.5, 0.5, 0.5)
+ # its gonna check for points_added from button_press_action
+
+ # The behavior of the click is such that it runs twice
+ # This is consistant with graph_edit.py which someone smart wrote
+ # So I'm just going to delete the last element in order to not get
+ # repeating points
+ if len(self.selected_points) > 0:
+ print("SELECTED_POINTS: " + str(len(self.selected_points)))
+ print("ITEMS:")
+ for item in self.selected_points:
+ print(str(item))
+ for i, point in enumerate(self.selected_points):
+ print("I: " + str(i))
+ draw_px_x(cr, point[0], point[1], 10, 0,
+ 0, 0)
+ cr.move_to(point[0], point[1]-15)
+ display_text(cr, str(i), 0.5, 0.5, 2, 2)
+
+ cr.paint_with_alpha(.65)
+
+ draw_px_cross(cr, self.x, self.y, 10, 1, 0, 0)
+
+ def do_key_press(self, event):
+ keyval = Gdk.keyval_to_lower(event.keyval)
+ print("Gdk.KEY_" + Gdk.keyval_name(keyval))
+ if keyval == Gdk.KEY_q:
+ print("Found q key and exiting.")
+ quit_main_loop()
+ if keyval == Gdk.KEY_e:
+ self.mode = Mode.kExporting
+ # Will export to csv file
+ with open('points_for_pathedit.csv', mode='w') as points_file:
+ writer = csv.writer(points_file, delimiter=',', quotechar='"',
+ quoting=csv.QUOTE_MINIMAL)
+ for item in self.selected_points:
+ writer.writerow([str(item[0]), str(item[1])])
+ print("Wrote: " + str(item[0]) + " " + str(item[1]))
+ if keyval == Gdk.KEY_i:
+ self.mode = Mode.kImporting
+ # import from csv file
+ self.selected_points = []
+ with open('points_for_pathedit.csv') as points_file:
+ reader = csv.reader(points_file, delimiter=',')
+ for row in reader:
+ self.add_point(float(row[0]), float(row[1]))
+ print("Added: " + row[0] + " " + row[1])
+
+ self.redraw()
+
+ def button_press_action(self):
+ if self.switch:
+ self.switch = False
+ if self.mode == Mode.kPlacing:
+ #Check that the point clicked is on the field
+ if(self.x<150 and self.x>-150 and self.y <150 and self.y >-150):
+ self.add_point(self.x, self.y)
+ if self.mode == Mode.kEditing:
+ # Now after index_of_edit is not -1, the point is selected, so
+ # user can click for new point
+ print("INDEX OF EDIT: " + str(self.index_of_edit))
+
+ if self.index_of_edit > -1 and self.held_x != self.x:
+ print("INDEX OF EDIT: " + str(self.index_of_edit))
+ self.selected_points[self.index_of_edit] = [self.x, self.y]
+ self.index_of_edit = -1
+ else:
+ print("mode == 2")
+ # Get clicked point
+ # Find nearest
+ # Move nearest to clicked
+ cur_p = [self.x, self.y]
+ print("CUR_P: " + str(self.x) + " " + str(self.y))
+ # What I wanna do is get each point
+ # Get the distance between each for x and y
+ # Save the index of the point closest
+ nearest = 1000
+ index = 0
+ for ind, i in enumerate(self.selected_points):
+ # pythagorean
+ distance = np.sqrt((cur_p[0] - i[0])**2 + (cur_p[1] - i[1])**2)
+ if distance < nearest:
+ nearest = distance
+ index = ind
+ print("Nearest: " + str(nearest))
+ print("Index: " + str(index))
+ self.index_of_edit = index
+ self.held_x = self.x
+ else:
+ self.switch = True
+
+ self.redraw()
+
+
+ def do_button_press(self, event):
+ print("button press activated")
+ self.x = event.x
+ self.y = event.y
+ self.button_press_action()
+
+
+
+silly = GTK_Widget()
+basic_window.RunApp()
diff --git a/y2018/control_loops/python/spline_generate.py b/y2018/control_loops/python/spline_generate.py
new file mode 100644
index 0000000..4376f37
--- /dev/null
+++ b/y2018/control_loops/python/spline_generate.py
@@ -0,0 +1,366 @@
+#!/usr/bin/python
+import numpy as np
+import matplotlib.pyplot as plt
+from frc971.control_loops.python import drivetrain
+
+# used to define properties of the drivetrain, changes depending on robot
+# see yXXXX/control_loops/python/drivetrain.py for the current values
+
+kDrivetrain = drivetrain.DrivetrainParams(
+ J = 6.0,
+ mass=68.0,
+ robot_radius=0.616 / 2.0,
+ wheel_radius=0.127 / 2.0 * 120.0 / 118.0,
+ G_low=46.0 / 60.0 * 20.0 / 48.0 * 14.0 / 62.0,
+ G_high=62.0 / 44.0 * 20.0 / 48.0 * 14.0 / 62.0,
+ q_pos_low=0.12,
+ q_pos_high=0.14,
+ q_vel_low=1.0,
+ q_vel_high=0.95,
+ efficiency=0.70,
+ has_imu=True,
+ force=True,
+ kf_q_voltage=13.0,
+ controller_poles=[0.82, 0.82],
+)
+
+drivetrain = drivetrain.Drivetrain(kDrivetrain)
+# set up coefficients for Hemrite basis function evaluation
+coeffs = np.array([[1, 0, 0, -10, 15, -6], [0, 1, 0, -6, 8, -3], [0, 0, 0.5, -1.5, 1.5, -0.5], [0, 0, 0, 0.5, -1, 0.5], [0, 0, 0, -4, 7, -3], [0, 0, 0, 10, -15, 6]])
+coeffs_prime = np.empty_like(coeffs)
+for ii in range(0, len(coeffs)):
+ for jj in range(0, len(coeffs[ii]) - 1):
+ coeffs_prime[ii][jj] = (jj + 1) * coeffs[ii][jj]
+
+def RungeKutta(f, x, dt):
+ """4th order RungeKutta integration of F starting at X."""
+ a = f(x)
+ b = f(x + dt / 2.0 * a)
+ c = f(x + dt / 2.0 * b)
+ d = f(x + dt * c)
+
+ return x + dt * (a + 2.0 * b + 2.0 * c + d) / 6.0
+
+def normalize(v):
+ norm = np.linalg.norm(v)
+ return v / norm
+
+def theta(v):
+ return np.arctan2(v[1], v[0])
+
+# evaluate Nth hermite basis function at t
+def nth_H(N, t):
+ return coeffs[N][0] + coeffs[N][1]*t + coeffs[N][2]*t**2 + coeffs[N][3]*t**3 + coeffs[N][4]*t**4 + coeffs[N][5]*t**5
+
+def nth_H_prime(N, t):
+ return coeffs[N][0] + coeffs[N][1]*t + coeffs[N][2]*t**2 + coeffs[N][3]*t**3 + coeffs[N][4]*t**4
+
+# class defining a quintic Hermite spline, with utilities for modification and plotting
+class Hermite_Spline:
+ # init method given known parameters, ie savefile loading(if necessary)
+ def __init__(self, start, control1, control2, end, resolution = 200):
+ self.start = start
+ self.end = end
+ self.control1 = control1
+ self.control2 = control2
+
+ self.points = np.array([])
+ self.velocities = []
+ self.accelerations = []
+ self.arc_lengths = []
+ self.thetas = []
+ self.omegas = []
+ self.curvatures = []
+
+ self.shifted_points = []
+
+ self.Ks = []
+ self.dKs = []
+
+ # coefficients are po, v0, a0, a1, v1, p1
+ self.coeffs = np.array([])
+ self.compute_coefficients()
+ self.resolution = resolution
+ self.setup()
+
+ # take paramters and compute coeffcicents for Hermite basis functions, to be called every time he change control points
+ def compute_coefficients(self):
+ self.coeffs = np.append(self.coeffs, np.array(self.start))
+ self.coeffs = np.append(self.coeffs, np.array(self.control1) - np.array(self.start))
+ self.coeffs = np.append(self.coeffs, [0,0])
+ self.coeffs = np.append(self.coeffs, [0,0])
+ self.coeffs = np.append(self.coeffs, np.array(self.end) - np.array(self.control2))
+ self.coeffs = np.append(self.coeffs, np.array(self.end))
+
+ self.coeffs = np.reshape(self.coeffs, newshape = (6, 2))
+
+ # setters for control points, set coefficients
+ def set_positions(self, p1 = None, p2 = None):
+ if p1 != None:
+ self.start = p1
+ if p2 != None:
+ self.end = p2
+
+ self.compute_coefficients()
+
+ def set_controls(self, c1 = None, c2 = None):
+ if c1 != None:
+ self.control1 = c1
+ if c2 != None:
+ self.control2 = c2
+
+ self.compute_coefficients()
+
+ def set_velocities(self, v1 = None, v2 = None):
+ if v1 != None:
+ self.control1 = self.start + v1
+ if v2 != None:
+ self.control2 = self.end + v2
+
+ self.compute_coefficients()
+
+ def get_smoothness(self):
+ K = self.get_curvature()
+ return np.sum(np.abs(np.gradient(K)))
+
+ # given Basis functions and controls compute coordinate given t
+ def spline_eval_hermite(self, t):
+ return np.array(self.coeffs[0]*nth_H(0, t) + self.coeffs[1]*nth_H(1, t)+ self.coeffs[2]*nth_H(2, t) + self.coeffs[3]*nth_H(3, t) + self.coeffs[4]* nth_H(4, t)+ self.coeffs[5]*nth_H(5, t))
+
+ # given Basis functions and controls compute velocity given t
+ def spline_eval_hermite_v(self, t):
+ return normalize(np.array(self.coeffs[0]*nth_H_prime(0, t) + self.coeffs[1]*nth_H_prime(1, t)+ self.coeffs[2]*nth_H_prime(2, t) + self.coeffs[3]*nth_H_prime(3, t) + self.coeffs[4]* nth_H_prime(4, t)+ self.coeffs[5]*nth_H_prime(5, t)))
+
+ # take coefficients and compute spline points/properties
+ def setup(self, resolution_multiplier = 10, dt = .000001):
+ points = []
+ velocities = []
+ accelerations = []
+ s = []
+ thetas = []
+ omegas = []
+ curvatures = []
+
+ last_point = self.spline_eval_hermite(0)
+ distance = 0
+
+ # iterate through interim points and compute pos_vectors, and at predefined points arc length,
+ # velocity, and acceleration vectors and store them at their associated index
+ for i in range(0, self.resolution * resolution_multiplier):
+ t = i / (1.0 * self.resolution * resolution_multiplier)
+
+ current_point = self.spline_eval_hermite(t)
+ current_point_dt = self.spline_eval_hermite(t + dt)
+ current_s = np.linalg.norm(current_point - last_point)
+
+ ds = np.linalg.norm(current_point_dt - current_point)
+
+ distance = current_s + distance
+ # at important points compute important values and store
+ if i % resolution_multiplier == 0:
+ s.append(distance)
+ points.append(current_point)
+
+ v = self.spline_eval_hermite_v(t)
+ v_dt = self.spline_eval_hermite_v(t + dt)
+ theta_t = theta(v)
+ theta_dt = theta(v_dt)
+
+ a = (v_dt - v) / ds
+ omega = (theta_dt - theta_t) / ds
+ if np.linalg.norm(v) == 0:
+ curvature = 0
+ else:
+ curvature = np.linalg.det(np.column_stack((v, a)) / (np.linalg.norm(v)**(3/2)))
+
+ velocities.append(v)
+ accelerations.append(a)
+ thetas.append(theta_t)
+ omegas.append(omega)
+ if curvature == 0:
+ curvatures.append(0.0001)
+ else:
+ curvatures.append(curvature)
+
+ last_point = current_point
+
+ self.arc_lengths = np.array(s)
+ self.points = np.reshape(points, newshape = (-1, 2))
+ self.velocities = np.reshape(velocities, newshape = (-1, 2))
+ self.accelerations = np.reshape(accelerations, newshape = (-1, 2))
+ self.thetas = np.array(thetas)
+ self.omegas = np.array(omegas)
+ self.curvatures = np.array(curvatures)
+
+
+ def plot_diagnostics(self):
+ plt.figure("Spline")
+ plt.title('Spline')
+ plt.plot(self.points[:, 0], self.points[:, 1])
+ # plt.scatter(self.points[:, 0], self.points[:, 1])
+
+ plt.figure("Diagnostics")
+
+ plt.subplot(2, 2, 1)
+ plt.title('theta')
+ plt.xlabel('arc_length')
+ plt.ylabel('theta')
+ theta, = plt.plot(self.arc_lengths, self.thetas, label = 'theta')
+ plt.legend(handles = [theta])
+
+ plt.subplot(2, 2, 2)
+ plt.title('omegas')
+ plt.xlabel('arc_length')
+ plt.ylabel('omega')
+ omega, = plt.plot(self.arc_lengths, self.omegas, label = 'omega')
+ plt.legend(handles = [omega])
+
+ plt.subplot(2, 2, 3)
+ plt.title('Velocities')
+ plt.xlabel('arc_length')
+ plt.ylabel('velocity')
+ dxds, = plt.plot(self.arc_lengths, self.velocities[:, 0], label = 'dx/ds')
+ dyds, = plt.plot(self.arc_lengths, self.velocities[:, 1], label = 'dy/ds')
+ plt.legend(handles = [dxds, dyds])
+
+ plt.subplot(2, 2, 4)
+ plt.title('Accelerations')
+ plt.xlabel('arc_length')
+ plt.ylabel('acceleration')
+ dx2ds2, = plt.plot(self.arc_lengths, self.accelerations[:, 0], label = 'd^2x/ds^2')
+ dy2ds2, = plt.plot(self.arc_lengths, self.accelerations[:, 1], label = 'd^2x/ds^2')
+ plt.legend(handles = [dx2ds2, dy2ds2])
+
+# class defining a number of splines with convinience methods
+class Path:
+ def __init__(self):
+ self.splines = []
+ self.knot_accels = []
+
+ def add_spline(self, spline):
+ self.splines.append(spline)
+
+ def get_K(self):
+ curvatures = []
+ for spline in self.splines:
+ curvatures.append(spline.curvatures)
+ return np.array(curvatures).flatten()
+
+ def get_S(self):
+ arc_lengths = []
+ for spline in self.splines:
+ arc_lengths.append(spline.arc_lengths)
+ return np.array(arc_lengths).flatten()
+
+ def get_points(self):
+ points = []
+ for spline in self.splines:
+ points.append(spline.points)
+ return points
+
+ def get_velocities(self, i):
+ velocities = []
+ for spline in self.splines:
+ velocities.append(spline.points)
+ return velocities
+
+ def remove_spine(self, i):
+ if i < len(self.splines):
+ self.splines.pop(i)
+ else:
+ print("index %f out of bounds, no spline of that index" % i)
+
+ def join(self, first_priority = False):
+ for i in range(0, len(self.splines)):
+ if first_priority & i != len(self.splines):
+ print("unfinished")
+
+
+# class which takes a Path object along with constraints and reparamterizes it with respect to time
+class Trajectory:
+ def __init__(self, path, max_angular_accel=3, max_voltage=11, max_normal_accel = .2):
+ self.path = path
+ self.A = drivetrain.A_continuous
+ self.B = drivetrain.B_continuous
+ self.robot_radius = drivetrain.robot_radius
+ self.Kv = 100
+ self.robot_radius = 3
+ self.max_angular_accel = max_angular_accel
+ self.max_voltage = max_voltage
+ self.max_normal_accel = max_normal_accel
+
+ self.max_velocities_adhering_to_normal_accel = []
+ self.max_velocities_adhering_to_voltage = []
+ self.path.splines[0].setup(resolution_multiplier = 100)
+
+ self.set_max_v_adhering_to_normal_accel()
+ self.max_voltageK_pass()
+
+ def set_max_v_adhering_to_normal_accel(self):
+ Ks = self.path.get_K()
+ accels = np.full_like(Ks, fill_value = self.max_normal_accel)
+ max_velocities = np.sqrt(np.abs(accels / Ks))
+ self.max_velocities_adhering_to_normal_accel = max_velocities
+
+ def max_voltageK_pass(self):
+ max_ds_dt = []
+ Ks = self.path.get_K()
+ turning_radii = np.full_like(Ks, fill_value = 1) / np.abs(Ks)
+
+
+
+ # compute max steady-state velocity given voltage constraints
+ for i in range(0, len(Ks)):
+ v_ratio = (turning_radii[i] + self.robot_radius) / (turning_radii[i] - self.robot_radius)
+ matrix = np.array([[self.A[1, 1], self.A[1, 3], self.B[1, 1]], [self.A[3, 1] - 1, self.A[3, 3], self.B[3, 1]], [-1, v_ratio, 0]])
+ sols = np.array([-1 * self.max_voltage * self.B[1, 0], -1 * self.max_voltage * self.B[3, 0], 0])
+ Vs = np.dot(np.linalg.inv(matrix), sols)
+ max_ds_dt.append((Vs[0] + Vs[1]) / 2)
+
+ self.max_velocities_adhering_to_voltage = max_ds_dt
+ # compute the maximum acceleration we can ask for given voltage and, ya know, staying on the path.
+
+
+ '''
+ These methods use the continuous form of our drivetrain state equation
+ in order to compute the maximum acceleration which adheres to the path
+ and voltage constraints, as well as any arbitary set of constraints
+ on velocity as a function of arc_length
+ '''
+
+ def forward_accel_pass(self):
+ points = self.path.get_points()
+ velocities = self.path.get_velocities()
+ curvatures = self.path.get_K()
+ arc_lenghts = self.path.get_S()
+
+ for i in range(0, len(points)):
+ Xn1 =
+
+
+ def backward_accelaration_pass(self):
+
+ print("max backward accel pass")
+
+
+ def plot_diagnostics(self, i = 0):
+
+ plt.figure('max velocity')
+ plt.title('max_v_normal_accel')
+ plt.xlabel('arc_length')
+ plt.ylabel('max V')
+ max_v_normal = plt.plot(self.path.get_S(), self.max_velocities_adhering_to_normal_accel, label = 'ds/dt (normal)')# , label = 'ds/dt')
+ curvature = plt.plot(self.path.get_S(), 1000 * np.abs(self.path.get_K()), label = 'K')
+ max_v_K_V = plt.plot(self.path.get_S(), self.max_velocities_adhering_to_voltage, label = 'ds/dt (voltage)')
+ plt.legend(handles = [max_v_normal[0], curvature[0], max_v_K_V[0]])
+
+def main():
+ A = Hermite_Spline(np.array([0,0]), np.array([0,400]), np.array([200,300]), np.array([200,200]), resolution = 200)
+ A.plot_diagnostics()
+ path = Path()
+ path.add_spline(A)
+ trajectory = Trajectory(path, 0)
+ trajectory.plot_diagnostics()
+ plt.show()
+
+main()