1+ from typing import List , Tuple
2+ import math
3+
4+ import numpy as np
5+
6+ from python_motion_planning .common .env .types import TYPES
7+ from python_motion_planning .common .env .map .grid import Grid
8+ from python_motion_planning .common .env .robot .base_robot import BaseRobot
9+ from python_motion_planning .common .utils .geometry import Geometry
10+ from python_motion_planning .common .utils .frame_transformer import FrameTransformer
11+ from .path_tracker import PathTracker
12+
13+
14+ class APF (PathTracker ):
15+ """
16+ Artificial Potential Field (APF) path-tracking controller.
17+
18+ Args:
19+ *args: see the parent class.
20+ robot_model: robot model for kinematic parameters
21+ obstacle_grid: occupancy grid map for collision checking
22+ attr_weight: weight factor for attractive potential
23+ rep_weight: weight factor for repulsive potential
24+ rep_range: influence range for repulsive potential
25+ **kwargs: see the parent class.
26+ """
27+ def __init__ (self ,
28+ * args ,
29+ robot_model : BaseRobot ,
30+ obstacle_grid : Grid = None ,
31+ attr_weight : float = 1.0 ,
32+ rep_weight : float = 1.0 ,
33+ rep_range : float = None ,
34+ ** kwargs ):
35+ super ().__init__ (* args , ** kwargs )
36+ if robot_model .dim != self .dim :
37+ raise ValueError ("Dimension of robot model and controller must be the same" )
38+ self .robot_model = robot_model
39+
40+ if obstacle_grid and obstacle_grid .dim != self .dim :
41+ raise ValueError ("Dimension of obstacle grid and controller must be the same" )
42+ self .obstacle_grid = obstacle_grid
43+
44+ # APF parameters
45+ self .attr_weight = attr_weight # Attractive potential weight
46+ self .rep_weight = rep_weight # Repulsive potential weight
47+ self .rep_range = rep_range if rep_range is not None else self .robot_model .radius * 2.0 # Repulsive influence range
48+
49+ def get_action (self , obs : np .ndarray ) -> Tuple [np .ndarray , np .ndarray ]:
50+ """
51+ Get action from observation using Artificial Potential Field method.
52+
53+ Args:
54+ obs: observation ([pos, orient, lin_vel, ang_vel])
55+
56+ Returns:
57+ action: action in robot frame ([lin_acc, ang_acc])
58+ target_pose: lookahead pose ([pos, orient])
59+ """
60+ if self .goal is None :
61+ return np .zeros (self .action_space .shape ), self .goal
62+
63+ pose , vel , pos , orient , lin_vel , ang_vel = self .get_pose_velocity (obs )
64+
65+ # Find the lookahead pose as the target for attractive potential
66+ target_pose = self ._get_lookahead_pose (pos )
67+
68+ # Calculate potential field forces
69+ attractive_force = self ._calculate_attractive_force (pos , target_pose [:self .dim ])
70+ repulsive_force = self ._calculate_repulsive_force (pos )
71+
72+ # Total force is the sum of attractive and repulsive forces
73+ total_force = attractive_force + repulsive_force
74+
75+ # Calculate desired velocity from total force
76+ desired_vel = self ._force_to_velocity (total_force , orient )
77+ desired_vel = self ._stop_if_reached (desired_vel , pose )
78+
79+ # Convert current velocity to robot frame and calculate action
80+ robot_vel = FrameTransformer .vel_world_to_robot (self .dim , vel , orient )
81+ action = self ._get_desired_action (desired_vel , robot_vel , orient )
82+
83+ return action , target_pose
84+
85+ def _calculate_attractive_force (self , current_pos : np .ndarray , target_pos : np .ndarray ) -> np .ndarray :
86+ """
87+ Calculate attractive force towards the target position.
88+
89+ Args:
90+ current_pos: Current position of the robot in world frame
91+ target_pos: Target position (lookahead point) in world frame
92+
93+ Returns:
94+ attractive_force: Attractive force vector in world frame
95+ """
96+ # Vector from current position to target position
97+ direction = target_pos - current_pos
98+ distance = np .linalg .norm (direction )
99+
100+ # If distance is very small, return zero force to avoid division issues
101+ if distance < self .eps :
102+ return np .zeros_like (direction )
103+
104+ # Normalize direction and scale by weight and distance
105+ # For standard APF, attractive force is proportional to distance
106+ attractive_force = self .attr_weight * direction
107+
108+ return attractive_force
109+
110+ def _calculate_repulsive_force (self , current_pos : np .ndarray ) -> np .ndarray :
111+ """
112+ Calculate repulsive force from obstacles using ESDF.
113+
114+ Args:
115+ current_pos: Current position of the robot in world frame
116+
117+ Returns:
118+ repulsive_force: Repulsive force vector in world frame
119+ """
120+ if self .obstacle_grid is None :
121+ return np .zeros (self .dim )
122+
123+ # Convert world position to grid coordinates
124+ grid_pt = self .obstacle_grid .world_to_map (tuple (current_pos [:2 ]))
125+ grid_x , grid_y = grid_pt
126+
127+ # Check if position is out of bounds or in an obstacle
128+ if not self .obstacle_grid .within_bounds (grid_pt ) or self .obstacle_grid .type_map [grid_pt ] == TYPES .OBSTACLE :
129+ # Large repulsive force if in collision
130+ return np .full (self .dim , self .rep_weight * self .rep_range )
131+
132+ # Get distance to nearest obstacle from ESDF (in world units)
133+ dist_to_obstacle = self .obstacle_grid .esdf [grid_pt ] * self .obstacle_grid .resolution
134+
135+ # No repulsive force if outside influence range
136+ if dist_to_obstacle >= self .rep_range :
137+ return np .zeros (self .dim )
138+
139+ # Calculate gradient of repulsive potential using numpy's gradient function
140+ # Extract a small window around current grid point to compute gradient
141+ window_size = 3 # 3x3 window for gradient calculation
142+ half_window = window_size // 2
143+
144+ # Initialize window with current distance value
145+ window = np .zeros ((window_size , window_size ))
146+ window [half_window , half_window ] = self .obstacle_grid .esdf [grid_pt ]
147+
148+ # Fill window with neighboring ESDF values, handling boundary conditions
149+ for i in range (window_size ):
150+ for j in range (window_size ):
151+ # Calculate grid coordinates for this window position
152+ grid_i = grid_x + (i - half_window )
153+ grid_j = grid_y + (j - half_window )
154+
155+ # Check if neighboring grid point is within bounds
156+ if self .obstacle_grid .within_bounds ((grid_i , grid_j )):
157+ window [i , j ] = self .obstacle_grid .esdf [(grid_i , grid_j )]
158+ else :
159+ # For points outside bounds, use distance decreasing away from map
160+ dx = abs (i - half_window )
161+ dy = abs (j - half_window )
162+ dist_from_center = math .sqrt (dx * dx + dy * dy )
163+ window [i , j ] = max (0 , self .obstacle_grid .esdf [grid_pt ] - dist_from_center )
164+
165+ window *= self .obstacle_grid .resolution
166+
167+ # Calculate gradient using numpy.gradient
168+ # The gradient is scaled by grid resolution to get world coordinates
169+ # TODO: Check if this is correct
170+ # gy, gx = np.gradient(window)
171+ gx , gy = np .gradient (window )
172+
173+ # The gradient at the center of the window gives the direction of maximum increase
174+ # This is the direction away from obstacles
175+ gradient_dir = np .array ([gx [half_window , half_window ], gy [half_window , half_window ]])
176+
177+ # Normalize gradient direction
178+ grad_mag = np .linalg .norm (gradient_dir )
179+ if grad_mag < 1e-6 :
180+ return np .zeros (self .dim )
181+ gradient_dir = gradient_dir / grad_mag
182+
183+ # Calculate repulsive force magnitude using standard APF formula
184+ rep_force_magnitude = self .rep_weight * (1.0 / dist_to_obstacle - 1.0 / self .rep_range ) / (dist_to_obstacle ** 2 )
185+
186+ # Scale direction by repulsive force magnitude
187+ repulsive_force = rep_force_magnitude * gradient_dir
188+
189+ return repulsive_force
190+
191+ def _force_to_velocity (self , force : np .ndarray , orient : np .ndarray ) -> np .ndarray :
192+ """
193+ Convert force vector to desired velocity in robot frame.
194+
195+ Args:
196+ force: Total force vector in world frame
197+ orient: Current orientation in world frame
198+
199+ Returns:
200+ desired_vel: Desired velocity in robot frame
201+ """
202+ force_magnitude = np .linalg .norm (force )
203+
204+ # If force is very small, return zero velocity
205+ if force_magnitude < self .eps :
206+ return np .zeros (self .action_space .shape [0 ])
207+
208+ # Desired linear velocity is proportional to force magnitude
209+ desired_lin_speed = min (force_magnitude , self .max_lin_speed )
210+ desired_lin_dir = force / force_magnitude # Direction of force
211+
212+ # Desired angular velocity is based on angle difference between force direction and robot orientation
213+ force_angle = np .array ([math .atan2 (desired_lin_dir [1 ], desired_lin_dir [0 ])])
214+ angle_diff = Geometry .regularize_orient (force_angle - orient )
215+ desired_ang_speed = min (np .linalg .norm (angle_diff ) / self .dt , self .max_ang_speed )
216+ desired_ang_speed *= np .sign (angle_diff [0 ]) # Preserve direction
217+
218+ desired_ang_speed = np .array ([desired_ang_speed ])
219+
220+ # Combine linear and angular velocity
221+ desired_vel_world = np .concatenate ([desired_lin_dir * desired_lin_speed , desired_ang_speed ])
222+
223+ # Convert desired velocity to robot frame
224+ desired_vel_robot = FrameTransformer .vel_world_to_robot (self .dim , desired_vel_world , orient )
225+
226+ return self .clip_velocity (desired_vel_robot )
227+
0 commit comments