conversion from cont. to discretized

.gitignore

@@ -4,7 +4,7 @@ examples/debug.py
 examples/test.py
 
 build_vscode/
-
+venv_arch
 # NumPy saves and videos
 results/
 tmp/

tests/test_trajectories.py

@@ -21,6 +21,10 @@ def test_square_trajectory():
     # assert interpolation
     assert(trajectory.get_waypoint(0.1).coordinate == np.asarray([0.4, 0., 1.], dtype=np.float32)).all()
 
+def test_discretized():
+    from trajectories import TrajectoryFactory 
+    trajectory = TrajectoryFactory.get_linear_square_traj_discretized()
+
 def test_pol_trajectory():
     from trajectories import TrajectoryFactory, Waypoint
     import numpy as np

trajectories/discretized_trajectory.py

@@ -1,20 +1,41 @@
 from .waypoint import Waypoint
 from typing import List
+from .trajectory import Trajectory
 
 class DiscretizedTrajectory:
-    waypoints: List[Waypoint]
 
     def __init__(self) -> None:
         pass
 
     def __len__(self) -> int: 
         """
+        Number of waypoints of discretized trajectory.
+
         Must be implemented by child class.
         """
         raise NotImplementedError("Must be implemented by child class.")
 
     def __getitem__(self, idx: int) -> Waypoint:
         """
+        Yields waypoint with index 'idx'.
+
         Must be implemented by child class.
         """
-        raise NotImplementedError("Must be implemented by child class.")
+        raise NotImplementedError("Must be implemented by child class.")
+
+
+class DiscreteTrajectoryFromContinuous:
+    _n_discretization_level: int
+    _cont_trajectory: Trajectory
+
+    def __init__(self, cont_traj: Trajectory, n_discretization_level: int=100) -> None:
+        self._n_discretization_level = n_discretization_level        
+        self._cont_trajectory = cont_traj
+
+    def __len__(self) -> int: 
+        return self._n_discretization_level
+
+    def __getitem__(self, idx: int) -> Waypoint:
+        t = float(idx) / float(self._n_discretization_level)
+        wp = self._cont_trajectory.get_waypoint(t)
+        return wp

trajectories/trajectory_factory.py

@@ -4,6 +4,7 @@
 from .waypoint import Waypoint
 from typing import List
 import numpy as np 
+from .discretized_trajectory import DiscretizedTrajectory, DiscreteTrajectoryFromContinuous
 from typing import Optional
 
 class WAYPOINT_BOOTH:
@@ -58,6 +59,14 @@ def get_linear_square_trajectory(cls, square_scale: float=1, time_scale: float=1
             square_scale, time_scale
         )
 
+    @classmethod 
+    def get_linear_square_traj_discretized(cls, n_discretization_level: int=100, square_scale: float=1, time_scale: float=1) -> DiscretizedTrajectory:
+          sq_traj = SquareLinearTrajectory(
+              square_scale, time_scale
+          )
+          discr_traj = DiscreteTrajectoryFromContinuous(sq_traj, n_discretization_level) 
+          return discr_traj
+
     @classmethod
     def get_pol_discretized_trajectory(cls, t_waypoints: Optional[List[Waypoint]]=None, n_points_discretization_level: Optional[int] = None) -> PolynomialDiscretizedTrajectory: