SAMPLE_TEAM.Soccer.Motion.path_planning

The module is designed by team Robokit of Phystech Lyceum and team Starkit of MIPT under mentorship of A. Babaev. module can be used for optimized path planing of Robokit-1 robot. usage: create class PathPlan type object instance and call method path_calc_optimum. Optionally module can be launched stand alone for purpose of tuning and observing result of path planing. Being launched stand alone module draws soccer field with player (white circle), ball (orange circle), obstacles (black circles). Circles are movable by mouse dragging. After each stop of mouse new path is drawing.

Module Contents

Classes

PathPlan	Plans optimized path of humanoid robot from start coordinate to target coordinate.
Glob

Attributes

goalPostRadius
ballRadius
uprightRobotRadius
roundAboutRadiusIncrement

SAMPLE_TEAM.Soccer.Motion.path_planning.goalPostRadius = 0.15

SAMPLE_TEAM.Soccer.Motion.path_planning.ballRadius = 0.1

SAMPLE_TEAM.Soccer.Motion.path_planning.uprightRobotRadius = 0.2

SAMPLE_TEAM.Soccer.Motion.path_planning.roundAboutRadiusIncrement = 0.15

class SAMPLE_TEAM.Soccer.Motion.path_planning.PathPlan(glob)

Plans optimized path of humanoid robot from start coordinate to target coordinate. Coordinates are taken together with orientation. Path is composed from initial arc, final arc and connecting line. Connecting line must be tangent to arcs. In case of obstacles on path line additional arc is added in order to go around obstacle. Only one obstacle can be avoided reliably. Avoiding of second obstacle is not guaranteed. Therefore there are used evaluations of prices of variants of path. The Path with cheaper price is returned. Collision with obstacle in far distance is cheaper than collision with obstacle in near distance. During Path heuristic various radiuses of arcs are considered. Arc with zero radius means turning without changing coordinate.

coord2yaw(self, x, y)

intersection_line_segment_and_line_segment(self, x1, y1, x2, y2, x3, y3, x4, y4)

Checks if 2 line segments have common point. :returns: True - if there is common point

False - if not.

x = x1 + (x2 - x1) * t1 t1 - paramentric coordinate y = y1 + (y2 - y1) * t1 x = x3 + (x4 - x3) * t2 t2 - paramentric coordinate y = y3 + (y4 - y3) * t2 x1 + (x2 - x1) * t1 = x3 + (x4 - x3) * t2 y1 + (y2 - y1) * t1 = y3 + (y4 - y3) * t2 t1 = (x3 + (x4 - x3) * t2 - x1) / (x2 - x1) t1 = (y3 + (y4 - y3) * t2 - y1) / (y2 - y1) y1 + (y2 - y1) * (x3 + (x4 - x3) * t2 - x1) / (x2 - x1) = y3 + (y4 - y3) * t2 (y2 - y1) * (x4 - x3)/ (x2 - x1) * t2 - (y4 - y3) * t2 = y3 - y1 - (y2 - y1) * (x3 - x1) / (x2 - x1) t2 = (y3 - y1 - (y2 - y1) * (x3 - x1) / (x2 - x1)) /((y2 - y1) * (x4 - x3)/ (x2 - x1) - (y4 - y3)) if t1 == 0:

t2 = (y1 - y3)/ (y4 - y3) t2 = (x1 - x3)/ (x4 - x3)

intersection_line_segment_and_circle(self, x1, y1, x2, y2, xc, yc, R)

Checks if line segment and circle have common points. :returns: True - if there is common point

False - if not.

x = x1 + (x2 - x1) * t t - paramentric coordinate y = y1 + (y2 - y1) * t R**2 = (x - xc)**2 + (y - yc)**2 (x1 + (x2 - x1) * t - xc)**2 + (y1 + (y2 - y1) * t - yc)**2 - R**2 = 0 ((x2 - x1) * t)**2 + (x1 - xc)**2 + 2 * (x2 - x1) * (x1 - xc) * t + ((y2 - y1) * t)**2 + (y1 - yc)**2 + 2 * (y2 - y1) * (y1 - yc) * t - R**2 = 0 ((x2 - x1)**2 + (y2 - y1)**2) * t**2 + (2 * (x2 - x1) * (x1 - xc) + 2 * (y2 - y1) * (y1 - yc)) * t + (x1 - xc)**2 + (y1 - yc)**2 - R**2 = 0 a * t**2 + b * t + c = 0 a = (x2 - x1)**2 + (y2 - y1)**2 b = 2 * (x2 - x1) * (x1 - xc) + 2 * (y2 - y1) * (y1 - yc) c = (x1 - xc)**2 + (y1 - yc)**2 - R**2

intersection_circle_segment_and_circle(self, x1, y1, x2, y2, x0, y0, CW, xc, yc, R)

norm_yaw(self, yaw)

delta_yaw(self, start_yaw, dest_yaw, CW)

path_calc_optimum(self, start_coord, target_coord)

Returns optimized humanoid robot path. usage:

list: dest, list: centers, int: number_Of_Cycles = self.path_calc_optimum(list: start_coord, list: target_coord) dest: list of destination point coordinates. Each coordinate is list or tuple of floats [x,y].

Each coordinate is starting or end point of path segment. Path comprises of following segments: circle segment, line segment, n*(circle segment, line segment), circle segment. Where n - iterable.

centers: list of coordinates of circle centers of circle segments of path. Each coordinate is list or tuple of floats [x,y]. number_Of_Cycles: integer which represents price of path. In case if value is >100 then collision with second obstacle on path

is not verified.

start_coord: list or tuple of floats [x, y, yaw] target_coord: list or tuple of floats [x, y, yaw]

path_calc(self, start_coord, target_coord)

arc_path_external(self, x1, y1, yaw1, x2, y2, yaw2)

check_Obstacle(self, xp1, yp1, xp2, yp2)

check_Limits(self, x1, y1, x2, y2, xp1, yp1, xp2, yp2, xc1, yc1, CW1, xc2, yc2, CW2, dest)

check_Price(self, x1, y1, x2, y2, xp1, yp1, xp2, yp2, xc1, yc1, CW1, xc2, yc2, CW2, dest, centers)

number_Of_Cycles_count(self, dest, centers, yaw1, yaw2)

external_tangent_line(self, start, R1, R2, x1, y1, xc1, yc1, xc2, yc2, CW)

arc_path_internal(self, x1, y1, yaw1, x2, y2, yaw2)

internal_tangent_line(self, start, R1, R2, x1, y1, xc1, yc1, xc2, yc2, CW)

square_equation(self, a, b, c)

class SAMPLE_TEAM.Soccer.Motion.path_planning.Glob

import_strategy_data(self, current_work_directory)