improve usability of the simulator

examples/README.md

@@ -0,0 +1,2 @@
+# Dummy examples created by @ll7
+

examples/example01.py

@@ -0,0 +1,13 @@
+"""
+# Example 01: basic import test
+"""
+import pysocialforce as pysf
+
+
+simulator = pysf.Simulator_v2()
+
+for step in range(10):
+    simulator.step()
+    print(f"step {step}")
+    print(simulator.states.ped_positions)
+    print("=================")

examples/example02.py

@@ -0,0 +1,26 @@
+"""
+# Example 02: test the configuration of the simulator
+"""
+import pysocialforce as pysf
+
+obstacle01 = pysf.map_config.Obstacle(
+    [(10, 10), (15,10), (15, 15), (10, 15)])
+obstacle02 = pysf.map_config.Obstacle(
+    [(20, 10), (25,10), (25, 15), (20, 15)])
+
+route01 = pysf.map_config.GlobalRoute(
+    [(0, 0), (10, 10), (20, 10), (30, 0)])
+crowded_zone01 = ((10, 10), (20, 10), (20, 20))
+
+map_def = pysf.map_config.MapDefinition(
+    obstacles=[obstacle01, obstacle02],
+    routes=[route01],
+    crowded_zones=[crowded_zone01])
+
+simulator = pysf.Simulator_v2(map_def)
+
+for step in range(10):
+    simulator.step()
+    print(f"step {step}")
+    print(simulator.states.ped_positions)
+    print("=================")

examples/example03.py

@@ -0,0 +1,36 @@
+"""
+# Example 03: simulate with visual rendering
+"""
+import pysocialforce as pysf
+import numpy as np
+
+obstacle01 = pysf.map_config.Obstacle(
+    [(10, 10), (15,10), (15, 15), (10, 15)])
+obstacle02 = pysf.map_config.Obstacle(
+    [(20, 10), (25,10), (25, 15), (20, 15)])
+
+route01 = pysf.map_config.GlobalRoute(
+    [(0, 0), (10, 10), (20, 10), (30, 0)])
+crowded_zone01 = ((10, 10), (20, 10), (20, 20))
+
+map_def = pysf.map_config.MapDefinition(
+    obstacles=[obstacle01, obstacle02],
+    routes=[route01],
+    crowded_zones=[crowded_zone01])
+
+simulator = pysf.Simulator_v2(map_def)
+sim_view = pysf.SimulationView(obstacles=map_def.obstacles, scaling=10)
+sim_view.show()
+
+for step in range(10_000):
+    simulator.step()
+    ped_pos = np.array(simulator.states.ped_positions)
+    ped_vel = np.array(simulator.states.ped_velocities)
+    actions = np.concatenate((
+            np.expand_dims(ped_pos, axis=1),
+            np.expand_dims(ped_pos + ped_vel, axis=1)
+        ), axis=1)
+    state = pysf.sim_view.VisualizableSimState(step, ped_pos, actions)
+    sim_view.render(state, fps=10)
+
+sim_view.exit()

pysocialforce/__init__.py

@@ -5,3 +5,4 @@
 from .logging import *
 from .simulator import Simulator, Simulator_v2
 from .forces import *
+from .sim_view import SimulationView, VisualizableSimState

pysocialforce/config.py

@@ -7,7 +7,7 @@
 class SceneConfig:
     enable_group: bool = True
     agent_radius: float = 0.35
-    step_width: float = 1.0
+    dt_secs: float = 0.1
     max_speed_multiplier: float = 1.3
     tau: float = 0.5
     resolution: float = 10

pysocialforce/map_config.py

@@ -6,11 +6,22 @@
 
 Vec2D = Tuple[float, float]
 Line2D = Tuple[float, float, float, float]
+Circle = Tuple[Vec2D, float]
 Rect = Tuple[Vec2D, Vec2D, Vec2D]
 Zone = Tuple[Vec2D, Vec2D, Vec2D] # rect ABC with sides |A B|, |B C| and diagonal |A C|
 
 
 def sample_zone(zone: Zone, num_samples: int) -> List[Vec2D]:
+    """
+    Sample points within a given zone.
+
+    Args:
+        zone (Zone): The zone defined by three points.
+        num_samples (int): The number of points to sample.
+
+    Returns:
+        List[Vec2D]: A list of sampled points within the zone.
+    """
     a, b, c = zone
     a, b, c = np.array(a), np.array(b), np.array(c)
     vec_ba, vec_bc = a - b, c - b
@@ -20,8 +31,36 @@ def sample_zone(zone: Zone, num_samples: int) -> List[Vec2D]:
     return [(x, y) for x, y in points]
 
 
+def sample_circle(circle: Circle, num_samples: int) -> List[Vec2D]:
+    """
+    Sample points within a given circle.
+
+    Args:
+        circle (Circle): The circle defined by center point and radius.
+        num_samples (int): The number of points to sample.
+
+    Returns:
+        List[Vec2D]: A list of sampled points within the zone.
+    """
+    center, radius = circle
+    rot = np.random.uniform(0, np.pi*2, (num_samples, 1))
+    radius = np.random.uniform(0, radius, (num_samples, 1))
+    rel_x, rel_y = np.cos(rot) * radius, np.sin(rot) * radius
+    points = np.concatenate((rel_x, rel_y), axis=1) + np.array([center])
+    return [(x, y) for x, y in points]
+
+
 @dataclass
 class Obstacle:
+    """
+    Represents an obstacle in the map.
+
+    Attributes:
+        vertices (List[Vec2D]): The vertices of the obstacle.
+        lines (List[Line2D]): The lines formed by connecting the vertices.
+        vertices_np (np.ndarray): The vertices as a NumPy array.
+    """
+
     vertices: List[Vec2D]
     lines: List[Line2D] = field(init=False)
     vertices_np: np.ndarray = field(init=False)
@@ -43,30 +82,62 @@ def __post_init__(self):
 
 @dataclass
 class GlobalRoute:
-    spawn_id: int
-    goal_id: int
+    """
+    Represents a global route from a spawn point to a goal point in a map.
+    """
     waypoints: List[Vec2D]
-    spawn_zone: Rect
-    goal_zone: Rect
+    spawn_radius: float = 5.0
 
     def __post_init__(self):
-        if self.spawn_id < 0:
-            raise ValueError('Spawn id needs to be an integer >= 0!')
-        if self.goal_id < 0:
-            raise ValueError('Goal id needs to be an integer >= 0!')
+        """
+        Initializes the GlobalRoute object.
+
+        Raises:
+
+            ValueError: If the route contains no waypoints.
+        """
+
         if len(self.waypoints) < 1:
-            raise ValueError(f'Route {self.spawn_id} -> {self.goal_id} contains no waypoints!')
+            raise ValueError(f'Route contains no waypoints!')
+
+    @property
+    def spawn_circle(self) -> Circle:
+        """
+        Returns the spawn circle of the route.
+
+        Returns:
+            Circle: The spawn circle of the route.
+        """
+        return (self.waypoints[0], self.spawn_radius)
 
     @property
     def sections(self) -> List[Tuple[Vec2D, Vec2D]]:
+        """
+        Returns a list of sections along the route.
+
+        Returns:
+            List[Tuple[Vec2D, Vec2D]]: The list of sections, where each section is represented by a tuple of two waypoints.
+        """
         return [] if len(self.waypoints) < 2 else list(zip(self.waypoints[:-1], self.waypoints[1:]))
 
     @property
     def section_lengths(self) -> List[float]:
+        """
+        Returns a list of lengths of each section along the route.
+
+        Returns:
+            List[float]: The list of section lengths.
+        """
         return [dist(p1, p2) for p1, p2 in self.sections]
 
     @property
     def section_offsets(self) -> List[float]:
+        """
+        Returns a list of offsets for each section along the route.
+
+        Returns:
+            List[float]: The list of section offsets.
+        """
         lengths = self.section_lengths
         offsets = []
         temp_offset = 0.0
@@ -77,11 +148,25 @@ def section_offsets(self) -> List[float]:
 
     @property
     def total_length(self) -> float:
+        """
+        Returns the total length of the route.
+
+        Returns:
+            float: The total length of the route.
+        """
         return 0 if len(self.waypoints) < 2 else sum(self.section_lengths)
 
 
 @dataclass
 class MapDefinition:
+    """
+    Represents the definition of a map.
+
+    Attributes:
+        obstacles (List[Obstacle]): A list of obstacles in the map.
+        routes (List[GlobalRoute]): A list of global routes in the map.
+        crowded_zones (List[Zone]): A list of crowded zones in the map.
+    """
     obstacles: List[Obstacle]
     routes: List[GlobalRoute]
     crowded_zones: List[Zone]

pysocialforce/navigation.py

@@ -8,6 +8,17 @@
 
 @dataclass
 class RouteNavigator:
+    """
+    A class that represents a route navigator for navigating through waypoints.
+
+    Attributes:
+        waypoints (List[Vec2D]): The list of waypoints to navigate.
+        waypoint_id (int): The index of the current waypoint.
+        proximity_threshold (float): The proximity threshold for reaching a waypoint.
+        pos (Vec2D): The current position of the navigator.
+        reached_waypoint (bool): Indicates whether the current waypoint has been reached.
+    """
+
     waypoints: List[Vec2D] = field(default_factory=list)
     waypoint_id: int = 0
     proximity_threshold: float = 1.0 # info: should be set to vehicle radius + goal radius
@@ -16,30 +27,66 @@ class RouteNavigator:
 
     @property
     def reached_destination(self) -> bool:
+        """
+        Check if the destination has been reached.
+
+        Returns:
+            bool: True if the destination has been reached, False otherwise.
+        """
         return len(self.waypoints) == 0 or \
             dist(self.waypoints[-1], self.pos) <= self.proximity_threshold
 
     @property
     def current_waypoint(self) -> Vec2D:
+        """
+        Get the current waypoint.
+
+        Returns:
+            Vec2D: The current waypoint.
+        """
         return self.waypoints[self.waypoint_id]
 
     @property
     def next_waypoint(self) -> Optional[Vec2D]:
+        """
+        Get the next waypoint.
+
+        Returns:
+            Optional[Vec2D]: The next waypoint if available, None otherwise.
+        """
         return self.waypoints[self.waypoint_id + 1] \
             if self.waypoint_id + 1 < len(self.waypoints) else None
 
     @property
     def initial_orientation(self) -> float:
+        """
+        Get the initial orientation of the navigator.
+
+        Returns:
+            float: The initial orientation in radians.
+        """
         return atan2(self.waypoints[1][1] - self.waypoints[0][1],
                      self.waypoints[1][0] - self.waypoints[0][0])
 
     def update_position(self, pos: Vec2D):
+        """
+        Update the position of the navigator.
+
+        Args:
+            pos (Vec2D): The new position of the navigator.
+        """
         reached_waypoint = dist(self.current_waypoint, pos) <= self.proximity_threshold
         if reached_waypoint:
             self.waypoint_id = min(len(self.waypoints) - 1, self.waypoint_id + 1)
         self.pos = pos
         self.reached_waypoint = reached_waypoint
 
     def new_route(self, route: List[Vec2D]):
+        """
+        Set a new route for the navigator.
+
+        Args:
+            route (List[Vec2D]): The new route to navigate.
+        """
         self.waypoints = route
         self.waypoint_id = 0