diff --git a/algorithms/limit_cycle/limit_cycle.py b/algorithms/limit_cycle/limit_cycle.py index 8bbc5da9..c7aeaf0a 100644 --- a/algorithms/limit_cycle/limit_cycle.py +++ b/algorithms/limit_cycle/limit_cycle.py @@ -27,9 +27,11 @@ def filter_func(a, b, c, r, t, o): class LimitCycle(object): def __init__(self, match): ''' - - obstacles: list of the obstacles, not sorted yet - - target_is_ball: if the ball is the target, two virtual obstacles are added - to make the robot head to the goal when arriving + Constructor of the Limit Cycle class + + Parameters + ---------- + obstacles [list(Obstacle)] : list of the obstacles, not sorted yet ''' self.match = match self.robot = None @@ -46,9 +48,25 @@ def __init__(self, match): self.field_w, self.field_h = self.match.game.field.get_dimensions() def set_target(self, target): + ''' + Defines the target position + + Parameters + ---------- + target (tuple[int, int]): target x and y coordinates + ''' + self.target = Target(*target) def add_obstacle(self, obstacle: Obstacle): + ''' + Add an obstacle to the 'obstacles' list + + Parameters + ---------- + obstacle Obstacle(int, int, int, bool): obstacle x and y position, its radius and a 'force_clockwise' property + ''' + self.obstacles.append(Obstacle(*obstacle)) def contour(self, a, b, c, obstacle: Obstacle, fitness): @@ -79,6 +97,15 @@ def contour(self, a, b, c, obstacle: Obstacle, fitness): return (self.robot.x + self.dt*ddx, self.robot.y + self.dt*ddy) def compute(self, robot, fitness=15): + ''' + Runs the Limit Cycle algorithm + + Parameters + ---------- + robot Robot: robot entity from match + fitness int: parameter to increase/decrease the 'fitness' of the trajectory + ''' + self.robot = robot ''' a, b and c are the indexes of a linear equation: a*x + b*y + c = 0 @@ -99,7 +126,7 @@ def compute(self, robot, fitness=15): self.obstacles.sort(key=lambda o: math.sqrt((o.x - self.robot.x)**2 + (o.y - self.robot.y)**2)) ''' - here we have the conditional, if there are obstacles to contour we do so, else we go to the target + here we have the conditional, if there are obstacles to contour we do so, else we go straight to the target ''' if len(self.obstacles) > 0: return self.contour(a, b, c, self.obstacles[0], fitness) diff --git a/algorithms/univector_field/univector_field.py b/algorithms/univector_field/univector_field.py index d1ef4db4..a3901ccb 100644 --- a/algorithms/univector_field/univector_field.py +++ b/algorithms/univector_field/univector_field.py @@ -23,7 +23,23 @@ def reduce_angle(ang): np_reduce_angle = np.vectorize(reduce_angle) class UnivectorField: + """ + An implementation of the uni vector field path planning algotithm + + The UnivectorField will generate vector that guides a given point to the target so that it gets there with an angle + directed at the guiding point. This algorithm is also capable of contouring obstacles and generating a rectangle + behind the target where the vectors have the save directions as the target-guide vector. + """ + def __init__(self, n, rect_size, plot=False, path=''): + """Inits UnivectorField class. + + Parameters + ---------- + n: Constant the related to how much the path will avoid hitting the target from the wrong direction + rect_size: the base side size of the rectangle behind the target where all the vectors are the same + angle of the target-guide line + """ self.obstacles = [] self.N = n self.delta_g = rect_size @@ -37,18 +53,40 @@ def set_target(self, g, r): Parameters ---------- - g (tuple[int, int]): target x and y coordinates - r (tuple[int, int]): guide point x and y coordinates + g (tuple[float, float]): target x and y coordinates + r (tuple[float, float]): guide point x and y coordinates """ self.g = g self.r = r def add_obstacle(self, p, r0, m): + """ + Add one obstacle + + Parameters + ---------- + p (tuple[float, float]): obstacle center x and y coordinates + r0 (float): obstacle radius + m (float): obstacle margin (distance the path will avoid) + + Return + ---------- + obstacle (obstacle): the obstacle object + """ + self.obstacles.append(Obstacle(p, r0, m, m + r0)) return self.obstacles[-1] def del_obstacle(self, *args, all=False): + """ + Delete any amount of obstacles + + Parameters + ---------- + *args (list[obstacles]): one or more obstacle to be deleted + all (bool): whether to delete all obstacles + """ if all: self.obstacles = [] return @@ -78,6 +116,17 @@ def __call__(self, p): return self.compute(p) def compute(self, p): + """ + Calculate the angle for the given position + + Parameters + ---------- + p (tuple[float, float]): the position for which the angle will be calculated + + Return + ---------- + angle (float): the angle of the vector in the field at the given position + """ behind_angle = None ang_pr = angle_between(p, self.r) diff --git a/controller/PID_control.py b/controller/PID_control.py index 720c542c..935b113e 100644 --- a/controller/PID_control.py +++ b/controller/PID_control.py @@ -13,6 +13,33 @@ def angle_adjustment(angle): class PID_control(object): + """ + An implementation of the PID controller on linear and angular speed + + The PID_control will receive a target position (x, y) and calculate the angular speed needed to reach the target + angle (ensure that the robot angle is in the direction of the target) using the PID algorithm. It will also + calculate the linear speed based on the distance from the robot and the target using a P algorithm. + + Attributes + ---------- + K_RHO : float + the linear Kp value + KP : float + the angular Kp value + KD : float + the angular Kd value + KI : float + the angular Ki value + V_MAX : int + the maximum linear speed + V_MIN : int + the minimum linear speed + W_MAX : int + the maximum angular speed + TWO_SIDES: bool + whether the controller will consider the robot as having two equal sides or only one + """ + CONSTANTS = { 'simulation': { # Control params @@ -69,6 +96,13 @@ def __init__(self, robot, default_fps=60, **kwargs): self.last_ki = self.KI def set_desired(self, vector): + """ + Defines the target position + + Parameters + ---------- + vector (tuple[float, float]): target x and y coordinates + """ self.desired = vector def _update_fps(self): @@ -125,6 +159,28 @@ def update(self): class PID_W_control(PID_control): + """ + An implementation of the PID controller on the angular speed + + The PID_control will receive a target position (x, y) and calculate the angular speed needed to reach the target + angle (ensure that the robot angle is in the direction of the target) using the PID algorithm. It will set the robot + linear speed as the maximum all times. + + Attributes + ---------- + KP : float + the angular Kp value + KD : float + the angular Kd value + KI : float + the angular Ki value + V_MAX : int + the maximum linear speed + W_MAX : int + the maximum angular speed + TWO_SIDES: bool + whether the controller will consider the robot as having two equal sides or only one + """ def update(self): v, w = super()._update() diff --git a/controller/uni_controller.py b/controller/uni_controller.py index 15ae0b43..7e113f4b 100644 --- a/controller/uni_controller.py +++ b/controller/uni_controller.py @@ -8,6 +8,28 @@ """ class UniController(object): + """ + An implementation of the Uni controller specified on the soccer robotics article + + The UniController will receive a desired angle for the robot current position, the desired angle for the position in + front of the robot and along with the current robot angle it will calculate the robot angular and linear speed. it + can also be set through the control_speed parameter to slow down when near the target position. + + Attributes + ---------- + K_P : float + the linear Kp value + control_speed : bool + whether the controller will use the P control for linear speed + V_M : int + the maximum linear speed + K_W : float + a positive constant + R_M : int + the maximum turn (v*w) + target: tuple[float, float] + the target position used for calculating the linear speed P + """ CONSTANTS = { 'simulation': { @@ -95,6 +117,14 @@ def control(self): return v, w def set_desired(self, desired): + """ + Defines the target angles + + Parameters + ---------- + desired (tuple[float, float]): the desired angle in the current position and in the desired angle in + front of the robot + """ self.theta_d = desired[0] self.theta_f = desired[1]