github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

Update the pi_2_pi function to a use of a modulo instead of two while.

Browse Source
This commit is contained in:
Alexis Paques
2018-07-10 09:22:18 +02:00
parent 39aa4f5a6a
commit b243750a13
17 changed files with 19 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py
View File

@@ -221,14 +221,10 @@ class RRT():
return newNode
def pi_2_pi(self, angle):
while(angle > math.pi):
angle = angle - 2.0 * math.pi
return (angle + math.pi) % (2*math.pi) - math.pi
while(angle < -math.pi):
angle = angle + 2.0 * math.pi
return angle
def steer(self, rnd, nind):
# print(rnd)

8
PathPlanning/ClosedLoopRRTStar/unicycle_model.py
View File

@@ -38,13 +38,7 @@ def update(state, a, delta):
def pi_2_pi(angle):
while(angle > math.pi):
angle = angle - 2.0 * math.pi
while(angle < -math.pi):
angle = angle + 2.0 * math.pi
return angle
return (angle + math.pi) % (2*math.pi) - math.pi
if __name__ == '__main__':

8
PathPlanning/DubinsPath/dubins_path_planning.py
View File

@@ -17,13 +17,7 @@ def mod2pi(theta):
def pi_2_pi(angle):
while(angle >= math.pi):
angle = angle - 2.0 * math.pi
while(angle <= -math.pi):
angle = angle + 2.0 * math.pi
return angle
return (angle + math.pi) % (2*math.pi) - math.pi
def LSL(alpha, beta, d):

8
PathPlanning/LQRRRTStar/lqr_rrt_star.py
View File

@@ -109,13 +109,7 @@ class RRT():
return newNode
def pi_2_pi(self, angle):
while(angle > math.pi):
angle = angle - 2.0 * math.pi
while(angle < -math.pi):
angle = angle + 2.0 * math.pi
return angle
return (angle + math.pi) % (2*math.pi) - math.pi
def sample_path(self, wx, wy, step):

8
PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py
View File

@@ -18,13 +18,7 @@ class State:
def pi_2_pi(angle):
while(angle > math.pi):
angle = angle - 2.0 * math.pi
while(angle < -math.pi):
angle = angle + 2.0 * math.pi
return angle
return (angle + math.pi) % (2*math.pi) - math.pi
def update(state, v, delta, dt, L):

8
PathPlanning/RRTDubins/dubins_path_planning.py
View File

@@ -17,13 +17,7 @@ def mod2pi(theta):
def pi_2_pi(angle):
while(angle >= math.pi):
angle = angle - 2.0 * math.pi
while(angle <= -math.pi):
angle = angle + 2.0 * math.pi
return angle
return (angle + math.pi) % (2*math.pi) - math.pi
def LSL(alpha, beta, d):

8
PathPlanning/RRTDubins/rrt_dubins.py
View File

@@ -96,14 +96,10 @@ class RRT():
return newNode
def pi_2_pi(self, angle):
while(angle >= math.pi):
angle = angle - 2.0 * math.pi
return (angle + math.pi) % (2*math.pi) - math.pi
while(angle <= -math.pi):
angle = angle + 2.0 * math.pi
return angle
def steer(self, rnd, nind):
# print(rnd)

8
PathPlanning/RRTStarDubins/dubins_path_planning.py
View File

@@ -13,13 +13,7 @@ def mod2pi(theta):
def pi_2_pi(angle):
while(angle >= math.pi):
angle = angle - 2.0 * math.pi
while(angle <= -math.pi):
angle = angle + 2.0 * math.pi
return angle
return (angle + math.pi) % (2*math.pi) - math.pi
def LSL(alpha, beta, d):

8
PathPlanning/RRTStarDubins/rrt_star_dubins.py
View File

@@ -97,13 +97,7 @@ class RRT():
return newNode
def pi_2_pi(self, angle):
while(angle >= math.pi):
angle = angle - 2.0 * math.pi
while(angle <= -math.pi):
angle = angle + 2.0 * math.pi
return angle
return (angle + math.pi) % (2*math.pi) - math.pi
def steer(self, rnd, nind):
# print(rnd)

8
PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py
View File

@@ -106,13 +106,7 @@ class RRT():
return newNode
def pi_2_pi(self, angle):
while(angle > math.pi):
angle = angle - 2.0 * math.pi
while(angle < -math.pi):
angle = angle + 2.0 * math.pi
return angle
return (angle + math.pi) % (2*math.pi) - math.pi
def steer(self, rnd, nind):

8
PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
View File

@@ -331,13 +331,7 @@ def generate_local_course(L, lengths, mode, maxc, step_size):
def pi_2_pi(angle):
while(angle > math.pi):
angle = angle - 2.0 * math.pi
while(angle < -math.pi):
angle = angle + 2.0 * math.pi
return angle
return (angle + math.pi) % (2*math.pi) - math.pi
def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size):