Add docs for function to compute forces exerted by robot on pedestrians

robot_sf/ped_npc/ped_robot_force.py

@@ -53,17 +53,67 @@ def ped_robot_force(
         robot_pos: Vec2D,
         threshold: float
         ):
-
+    """
+    Compute the forces exerted on pedestrians by a robot.
+
+    This function uses the potential field method to compute the forces. The force is
+    computed for each pedestrian and stored in the `out_forces` array.
+
+    Parameters
+    ----------
+    out_forces : np.ndarray
+        An array where the computed forces will be stored. The array should have the same
+        length as `ped_positions`.
+    ped_positions : np.ndarray
+        An array of the positions of the pedestrians.
+    robot_pos : Vec2D
+        The position of the robot.
+    threshold : float
+        The distance threshold for computing the force. If a pedestrian is farther than
+        this distance from the robot, the force is not computed.
+
+    Returns
+    -------
+    None
+    """
+    # Iterate over all pedestrians
     for i, ped_pos in enumerate(ped_positions):
+        # Compute the Euclidean distance between the pedestrian and the robot
         distance = euclid_dist(robot_pos, ped_pos)
+        # If the distance is less than or equal to the threshold
         if distance <= threshold:
+            # Compute the derivative of the Euclidean distance
             dx_dist, dy_dist = der_euclid_dist(ped_pos, robot_pos, distance)
+            # Compute the force using the potential field method and store it in the
+            # `out_forces` array
             out_forces[i] = potential_field_force(distance, dx_dist, dy_dist)
 
 
 @numba.njit(fastmath=True)
 def euclid_dist(v_1: Vec2D, v_2: Vec2D) -> float:
-    return ((v_1[0] - v_2[0])**2 + (v_1[1] - v_2[1])**2)**0.5
+    """
+    Compute the Euclidean distance between two 2D vectors.
+
+    This function uses the standard formula for Euclidean distance: sqrt((x1 - x2)^2 + (y1 - y2)^2).
+
+    Parameters
+    ----------
+    v_1 : Vec2D
+        The first 2D vector. This is a tuple or list of two numbers representing the x and y coordinates.
+    v_2 : Vec2D
+        The second 2D vector. This is a tuple or list of two numbers representing the x and y coordinates.
+
+    Returns
+    -------
+    float
+        The Euclidean distance between `v_1` and `v_2`.
+    """
+    # Compute the difference in x coordinates and square it
+    x_diff_sq = (v_1[0] - v_2[0])**2
+    # Compute the difference in y coordinates and square it
+    y_diff_sq = (v_1[1] - v_2[1])**2
+    # Return the square root of the sum of the squared differences
+    return (x_diff_sq + y_diff_sq)**0.5
 
 
 @numba.njit(fastmath=True)