Interface proposal

README.md

@@ -1,2 +1,73 @@
 # f1tenth_planning
 Central repo for all motion planners created for F1TENTH.
+
+# Basic API
+
+## Controllers
+### Init
+
+init should take a configuration of some sort and not env or task (might have unecessary leakage to controller in benchmark).
+Up to users to make sure configuration matches the env and the task.
+
+\_\_init\_\_(self, config)
+
+### Plan
+
+1. plan(env_obs) -> (speed, steer)
+2. plan(env_obs) -> (accl, steer_vel)
+
+TODOs:
+- Needs to know what control output type to use.
+- Needs to know what env obs type controller is getting, potentially throw error if no key sensor type is found.
+
+### Update config
+
+should take the same input as init
+
+update(self, config)
+
+## Planners
+### init
+See controller init notes.
+
+\_\_init\_\_(self, config)
+
+### Plan
+
+1. plan(env_obs) -> (speed, steer)
+2. plan(env_obs) -> (accl, steer_vel)
+3. plan(env_obs) -> path
+4. plan(env_obs) -> trajectory (path + vel)
+
+TODOs:
+- Needs to know what output type to use.
+- Needs to know what env obs type controller is getting, potentially throw error if no key sensor type is found.
+
+### Update config
+
+see controller update notes.
+
+update(self, config)
+
+
+## Trajectory dataclass
+### Fields
+- type: str
+- positions: [(x,y)]
+- headings: [Optional(theta)]
+- velocities: [Optional(v)]
+- accelerations: [Optional(a)]
+- steering_angles: [Optional(delta)]
+- steering_velocities: [Optional(delta_dot)]
+
+### Initialization
+- from file (.csv)
+- from planner or controllers (fill fields in)
+
+### Types
+- positions only
+- poses only
+- positions + velocities
+- poses + velocities
+- steering_angles + velocities
+- steering_velocities + accelerations

f1tenth_planning/control/controller.py

@@ -2,54 +2,95 @@
 from abc import abstractmethod, ABC
 
 import numpy as np
-from f110_gym.envs.track import Track
+from f1tenth_gym.envs.track import Track
 
 
 class Controller(ABC):
     @abstractmethod
-    def __init__(self, track: Track, params: dict | str = None) -> None:
-        """
-        Initialize controller.
+    def __init__(self, track: Track, config: dict | str = None) -> None:
+        """Controller init
+
+        Parameters
+        ----------
+        track : Track
+            track object with raceline/centerline
+        config : dict | str, optional
+            dictionary or path to yaml with controller specific parameters, by default None
 
-        Args:
-            track (Track): track object with raceline
-            params (dict | str, optional): dictionary or path to yaml with controller-specific parameters
+        Raises
+        ------
+        NotImplementedError
+            controller init method not implemented
         """
         raise NotImplementedError("controller init method not implemented")
 
     @abstractmethod
     def plan(self, state: dict) -> np.ndarray:
-        """
-        Plan control action given a state observation from the environment.
+        """Plan control action given a state observation from the environment.
+
+        Parameters
+        ----------
+        state : dict
+            observation as returned from the environment.
 
-        Args:
-            state (dict): observation as returned from the environment.
+        Returns
+        -------
+        np.ndarray
+            control action as (steering_angle, speed) or (steering_vel, acceleration)
 
-        Returns:
-            np.ndarray: control action as (steering_angle, speed)
+        Raises
+        ------
+        NotImplementedError
+            control method not implemented
         """
         raise NotImplementedError("control method not implemented")
 
+    @abstractmethod
+    def update(self, config: dict) -> None:
+        """Updates setting of controller
+
+        Parameters
+        ----------
+        config : dict
+            configurations to update
+
+        Raises
+        ------
+        NotImplementedError
+            controller update method not implemented
+        """
+        raise NotImplementedError("controller update method not implemented.")
+
     @property
     def color(self) -> tuple[int, int, int]:
-        """
-        Color as rgb tuple used for controller-specific render.
+        """Color as rgb tuple used for controller-specific render. For example, we can visualize trajectories of different colors for different agents by changing this color.
 
-        For example, we can visualize trajectories of different colors for different agents by changing this color.
+        Returns
+        -------
+        tuple[int, int, int]
+            RGB colors
         """
         return 128, 0, 0
 
     @color.setter
     def color(self, value: tuple[int, int, int]) -> None:
-        """
-        Set color as rgb tuple used for controller-specific render.
+        """Set color as rgb tuple used for controller-specific render.
+
+        Parameters
+        ----------
+        value : tuple[int, int, int]
+            RGB colors
         """
         assert len(value) == 3, f"color must be a tuple of length 3, got {value}"
         self.color = value
 
     def render_waypoints(self, e):
-        """
-        Callback to render waypoints.
+        """Callback to render waypoints.
+
+        Parameters
+        ----------
+        e : _type_
+            _description_
         """
         if self.waypoints is not None:
             points = self.waypoints[:, :2]

f1tenth_planning/planning/planner.py

@@ -0,0 +1,32 @@
+from ..control.controller import Controller
+from ..utils.trajectory import Trajectory
+
+import numpy as np
+from f1tenth_gym.envs.track import Track
+from typing import Sequence, Union
+from abc import abstractmethod
+
+
+class Planner(Controller):
+
+    @abstractmethod
+    def plan(self, state: dict) -> Sequence[float] | np.ndarray | Trajectory:
+        """Plan given a observation directly from env
+
+        Parameters
+        ----------
+        state : dict
+            observation as returned from the environment.
+
+        Returns
+        -------
+        Union[Sequence[float] | np.ndarray | Trajectory]
+               direct control actions of (steering_angle, speed) or (steering_vel, acceleration),
+            or Trajectory dataclass object
+
+        Raises
+        ------
+        NotImplementedError
+            planning method not implemented
+        """
+        raise NotImplementedError("planning method not implemented")

f1tenth_planning/utils/trajectory.py

@@ -0,0 +1,146 @@
+from f1tenth_gym.envs.track.cubic_spline import CubicSpline2D
+from f1tenth_gym.envs.track.raceline import Raceline
+from typing import Sequence
+import numpy as np
+import pathlib
+import matplotlib.pyplot as plt
+
+class Trajectory(Raceline):
+    type: str
+    positions: Sequence[Sequence[float]]
+    poses: Sequence[float] = None
+    theta: Sequence[float] = None
+    v: Sequence[float] = None
+    a: Sequence[float] = None
+    steer: Sequence[float] = None
+    steer_v: Sequence[float] = None
+
+    def __init__(
+        self,
+        xs: np.ndarray,
+        ys: np.ndarray,
+        velxs: np.ndarray | None = None,
+        ss: np.ndarray | None = None,
+        psis: np.ndarray | None = None,
+        kappas: np.ndarray | None = None,
+        accxs: np.ndarray | None = None,
+        spline: CubicSpline2D | None = None,
+    ):
+        super().__init__(xs, ys, velxs, ss, psis, kappas, accxs, spline)
+
+    @staticmethod
+    def from_file(
+        path: str | pathlib.Path,
+        delimiter: str | None = ",",
+        fixed_speed: float | None = 1.0,
+    ):
+        if type(path) == str:
+            path = pathlib.Path(path)
+        raceline = Raceline.from_raceline_path(path, delimiter)
+        if fixed_speed is not None:
+            raceline.velxs = np.full_like(raceline.ss, fixed_speed)
+        return Trajectory(
+            ss=raceline.ss,
+            xs=raceline.xs,
+            ys=raceline.ys,
+            psis=raceline.psis,
+            kappas=raceline.kappas,
+            velxs=raceline.velxs,
+            accxs=raceline.accxs,
+        )
+
+    def check_valid(self):
+        pass
+
+    def to_file(
+        self,
+        path: str | pathlib.Path,
+        delimiter: str | None = ",",
+        fixed_speed: float | None = 1.0,
+    ):
+        """Save trajectory to file, with optional fixed speed.
+
+        Parameters
+        ----------
+        path : str | pathlib.Path
+            path to save trajectory to
+        delimiter : str, optional
+            delimiter to use, by default ","
+
+        Raises
+        ------
+        ValueError
+            If file extension is not supported
+        FileExistsError
+            If file already exists
+        FileNotFoundError
+            If directory does not exist
+        """
+        if type(path) == str:
+            path = pathlib.Path(path)
+        # Check if path ends with a valid extension (csv, txt, npy, etc.)
+        if path.suffix not in [".csv", ".txt", ".npy"]:
+            raise ValueError(f"File extension {path.suffix} not supported.")
+        # Check if file exists 
+        if path.exists():
+            raise FileExistsError(f"File {path} already exists.")
+        # Check if directory exists
+        if not path.parent.exists():
+            raise FileNotFoundError(f"Directory {path.parent} does not exist.")
+
+        if fixed_speed is not None:
+            vels = np.full_like(self.ss, fixed_speed)
+        else:
+            vels = self.velxs
+
+        header = f'{delimiter} '.join(['s_m','x_m','y_m','psi_rad','kappa_radpm','vx_mps','ax_mps2'])
+        np.savetxt(
+            path,
+            np.array([self.ss, self.xs, self.ys, self.psis, self.kappas, vels]).T,
+            header=header,
+            comments="# ",
+            delimiter=delimiter,
+        )
+        return
+
+    def subsample(self, 
+                  jump: int = 1):
+        """Subsample trajectory to reduce number of points.
+
+        Parameters
+        ----------
+        jump : int, optional
+            Number of points to downsample by, by default 1
+
+        Returns
+        -------
+        Trajectory
+            Subsampled trajectory
+
+        Raises
+        ------
+        ValueError
+            If jump is not a positive integer or if jump is greater than trajectory length
+        """
+        if jump <= 0:
+            raise ValueError("Jump must be a positive integer.")
+        if jump > len(self.ss):
+            raise ValueError("Jump must be less than trajectory length.")
+
+        return Trajectory(
+            ss=self.ss[::jump],
+            xs=self.xs[::jump],
+            ys=self.ys[::jump],
+            psis=self.psis[::jump],
+            kappas=self.kappas[::jump],
+            velxs=self.velxs[::jump],
+            accxs=self.accxs[::jump],
+        )
+
+    def render(self):
+        """Render trajectory. Quiver plot of (x, y, psi) colored by velocity.
+        """
+        fig, ax = plt.subplots()
+        q = ax.quiver(self.xs, self.ys, np.cos(self.psis), np.sin(self.psis), self.velxs)
+        plt.show()
+        return

requirements.txt

@@ -1,4 +1,4 @@
--e git+https://github.com/f1tenth/f1tenth_gym.git@v1.0.0#egg=f110_gym
+-e git+https://github.com/f1tenth/f1tenth_gym.git@v1.0.0#egg=f1tenth_gym
 cvxpy
 cycler
 ecos