Spline UI: improve json exports
Change-Id: I2aac5c6eda3122a627bb99185d0db2b17b3bcd16
diff --git a/frc971/control_loops/python/graph.py b/frc971/control_loops/python/graph.py
index 9c69f9c..9ba87ab 100644
--- a/frc971/control_loops/python/graph.py
+++ b/frc971/control_loops/python/graph.py
@@ -39,6 +39,7 @@
if mypoints.getLibsplines():
distanceSpline = DistanceSpline(mypoints.getLibsplines())
traj = Trajectory(distanceSpline)
+ mypoints.addConstraintsToTrajectory(traj)
traj.Plan()
XVA = traj.GetPlanXVA(dT)
if len(XVA[0]) > 0:
diff --git a/frc971/control_loops/python/libspline.py b/frc971/control_loops/python/libspline.py
index 8facbb5..4e68221 100755
--- a/frc971/control_loops/python/libspline.py
+++ b/frc971/control_loops/python/libspline.py
@@ -176,9 +176,9 @@
def __del__(self):
libSpline.deleteTrajectory(self.__trajectory)
- def SetLongitudalAcceleration(self, accel):
- libSpline.TrajectorySetLongitualAcceleration(self.__trajectory,
- ct.c_double(accel))
+ def SetLongitudinalAcceleration(self, accel):
+ libSpline.TrajectorySetLongitudinalAcceleration(
+ self.__trajectory, ct.c_double(accel))
def SetLateralAcceleration(self, accel):
libSpline.TrajectorySetLateralAcceleration(self.__trajectory,
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 2e9ed95..ce87aff 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -337,9 +337,11 @@
else:
# Will export to json file
self.mode = Mode.kEditing
- exportList = [l.tolist() for l in self.points.getSplines()]
+
+ multi_spline = self.points.toMultiSpline()
+ print(multi_spline)
with open(self.path_to_export, mode='w') as points_file:
- json.dump(exportList, points_file)
+ json.dump(multi_spline, points_file)
def import_json(self, file_name):
self.path_to_export = os.path.join(self.module_path,
@@ -349,13 +351,25 @@
else:
# import from json file
self.mode = Mode.kEditing
- self.points.resetPoints()
- self.points.resetSplines()
print("LOADING LOAD FROM " + file_name) # Load takes a few seconds
with open(self.path_to_export) as points_file:
- self.points.setUpSplines(json.load(points_file))
+ multi_spline = json.load(points_file)
- self.points.update_lib_spline()
+ # if people messed with the spline json,
+ # it might not be the right length
+ # so give them a nice error message
+ try: # try to salvage as many segments of the spline as possible
+ self.points.fromMultiSpline(multi_spline)
+ except IndexError:
+ # check if they're both 6+5*(k-1) long
+ expected_length = 6 + 5 * (multi_spline["spline_count"] - 1)
+ x_len = len(multi_spline["spline_x"])
+ y_len = len(multi_spline["spline_x"])
+ if x_len is not expected_length:
+ print("Error: spline x values were not the expected length; expected {} got {}".format(expected_length, x_len))
+ elif y_len is not expected_length:
+ print("Error: spline y values were not the expected length; expected {} got {}".format(expected_length, y_len))
+
print("SPLINES LOADED")
def do_key_press(self, event, file_name):
diff --git a/frc971/control_loops/python/points.py b/frc971/control_loops/python/points.py
index 8f94e3f..4444a72 100644
--- a/frc971/control_loops/python/points.py
+++ b/frc971/control_loops/python/points.py
@@ -8,6 +8,18 @@
self.points = [] # Holds all points not yet in spline
self.libsplines = [] # Formatted for libspline library usage
self.splines = [] # Formatted for drawing
+ self.constraints = [ # default constraints
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 3
+ }, {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 2
+ }, {
+ "constraint_type": "VOLTAGE",
+ "value": 10
+ }
+ ]
def getPoints(self):
return self.points
@@ -18,6 +30,21 @@
def getLibsplines(self):
return self.libsplines
+ def setConstraint(self, id, value):
+ for constraint in self.constraints:
+ if constraint["constraint_type"] == id:
+ constraint["value"] = value
+ break
+
+ def addConstraintsToTrajectory(self, trajectory):
+ for constraint in self.constraints:
+ if constraint["constraint_type"] == "VOLTAGE":
+ trajectory.SetVoltageLimit(constraint["value"])
+ elif constraint["constraint_type"] == "LATERAL_ACCELERATION":
+ trajectory.SetLateralAcceleration(constraint["value"])
+ elif constraint["constraint_type"] == "LONGITUDINAL_ACCELERATION":
+ trajectory.SetLongitudinalAcceleration(constraint["value"])
+
def splineExtrapolate(self, o_spline_edit):
spline_edit = o_spline_edit
if not spline_edit == len(self.splines) - 1:
@@ -82,15 +109,41 @@
spline = Spline(np.ascontiguousarray(np.transpose(array)))
self.libsplines.append(spline)
+ def toMultiSpline(self):
+ multi_spline = {
+ "spline_count": 0,
+ "spline_x": [],
+ "spline_y": [],
+ "constraints": self.constraints,
+ }
+ for points in self.splines:
+ multi_spline["spline_count"] += 1
+ for j, point in enumerate(points):
+ if j == 0 and multi_spline["spline_count"] > 1:
+ continue # skip overlapping points
+ multi_spline["spline_x"].append(point[0])
+ multi_spline["spline_y"].append(point[1])
+ return multi_spline
+
+ def fromMultiSpline(self, multi_spline):
+ self.constraints = multi_spline["constraints"]
+ self.splines = []
+ self.points = []
+
+ i = 0
+ for j in range(multi_spline["spline_count"]):
+ # get the last point of the last spline
+ # and read in another 6 points
+ for i in range(i, i + 6):
+ self.points.append(
+ [multi_spline["spline_x"][i], multi_spline["spline_y"][i]])
+ self.splines.append(np.array(self.points))
+ self.points = []
+ self.update_lib_spline()
+
def getSplines(self):
return self.splines
- def resetSplines(self):
- self.splines = []
-
- def setUpSplines(self, newSplines):
- self.splines = newSplines
-
def setSplines(self, spline_edit, index_of_edit, x, y):
self.splines[spline_edit][index_of_edit] = [x, y]