Converting JSON Files to Trajectories

autonomous/routines/basic_auto/basic_auto.py

@@ -0,0 +1,24 @@
+import math
+
+from commands2 import SequentialCommandGroup, InstantCommand, WaitCommand
+from wpimath.geometry import Pose2d
+
+import constants
+from autonomous.auto_routine import AutoRoutine
+from autonomous.utils.custom_pathing import FollowPathCustom
+from autonomous.utils.path_planner import generate_trajectories
+from robot_systems import Robot
+
+trajectories = generate_trajectories(1, 2)
+
+path_1 = FollowPathCustom(
+    subsystem=Robot.drivetrain,
+    trajectory=trajectories[0],
+    period=constants.period,
+)
+
+auto = SequentialCommandGroup(
+    path_1
+)
+
+routine = AutoRoutine(Pose2d(0, 0, math.radians(0)), auto)

autonomous/routines/basic_auto/trajectories/trajectory_1.path

@@ -0,0 +1,128 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 0.0,
+        "y": 0.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.7420516744816386,
+        "y": 0.9887504163714477
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [
+          "START"
+        ],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 7.831012515095207,
+        "y": 2.4655667602179507
+      },
+      "prevControl": {
+        "x": 6.784610451047518,
+        "y": 2.4655667602179507
+      },
+      "nextControl": {
+        "x": 8.87741457914289,
+        "y": 2.4655667602179507
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 7.14692536841214,
+        "y": 6.230454824533553
+      },
+      "prevControl": {
+        "x": 10.227181817713419,
+        "y": 8.504787605826579
+      },
+      "nextControl": {
+        "x": 4.066668919110865,
+        "y": 3.956122043240523
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 4.374339149582905,
+        "y": 5.679168149728843
+      },
+      "prevControl": {
+        "x": 4.374339149582905,
+        "y": 4.679168149728843
+      },
+      "nextControl": {
+        "x": 4.374339149582905,
+        "y": 4.679168149728843
+      },
+      "holonomicAngle": 0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 2.5,
+        "y": 2.0
+      },
+      "prevControl": {
+        "x": 1.4448874394832913,
+        "y": 2.0
+      },
+      "nextControl": null,
+      "holonomicAngle": -114.25351410057554,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [
+          "STOP"
+        ],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "markers": []
+}

autonomous/utils/path_planner.py

@@ -0,0 +1,52 @@
+import json
+import os
+import inspect
+import math
+from wpimath.geometry import Pose2d, Translation2d
+from autonomous.utils.trajectory import CustomTrajectory
+
+def generate_trajectories(max_velocity: float, max_accel: float):
+    folder_path = os.path.dirname(inspect.stack()[1].filename) + "/trajectories"
+
+    trajectories = os.listdir(folder_path)
+
+    output_trajectories = []
+
+    for trajectory in trajectories:
+        file_path = os.path.join(folder_path, trajectory)
+
+        with open(file_path, "r") as file:
+            file_contents = file.read()
+
+        waypoints = json.loads(file_contents)["waypoints"]
+
+        start = waypoints[0]
+        end = waypoints[len(waypoints) - 1]
+
+        start_pose = Pose2d(
+            start["anchorPoint"]["x"], 
+            start["anchorPoint"]["y"], 
+            math.radians(start["holonomicAngle"])
+        )
+
+        end_pose = Pose2d(
+            end["anchorPoint"]["x"], 
+            end["anchorPoint"]["y"], 
+            math.radians(end["holonomicAngle"])
+        )
+
+        interior_waypoints = []
+
+        for waypoint in waypoints[1:-1]:
+            coord = waypoint["anchorPoint"]
+            interior_waypoints.append(Translation2d(coord["x"], coord["y"]))
+
+        output_trajectories.append(CustomTrajectory(
+            start_pose,
+            interior_waypoints,
+            end_pose,
+            max_velocity,
+            max_accel
+        ))
+
+    return output_trajectories

simgui-window.json

@@ -1,25 +1,25 @@
 {
   "MainWindow": {
     "GLOBAL": {
-      "height": "1044",
+      "height": "1016",
       "maximized": "0",
       "style": "0",
       "userScale": "2",
       "width": "638",
       "xpos": "641",
-      "ypos": "35"
+      "ypos": "64"
     }
   },
   "Window": {
     "###FMS": {
       "Collapsed": "0",
       "Pos": "5,540",
-      "Size": "283,146"
+      "Size": "32,35"
     },
     "###Joysticks": {
       "Collapsed": "0",
       "Pos": "439,598",
-      "Size": "632,73"
+      "Size": "32,35"
     },
     "###NetworkTables": {
       "Collapsed": "0",
@@ -34,12 +34,12 @@
     "###System Joysticks": {
       "Collapsed": "0",
       "Pos": "14,301",
-      "Size": "192,218"
+      "Size": "32,35"
     },
     "###Timing": {
       "Collapsed": "0",
       "Pos": "5,150",
-      "Size": "135,150"
+      "Size": "32,35"
     },
     "Debug##Default": {
       "Collapsed": "0",
@@ -49,7 +49,7 @@
     "Robot State": {
       "Collapsed": "0",
       "Pos": "5,20",
-      "Size": "92,99"
+      "Size": "32,35"
     }
   }
 }