github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-18 16:04:33 -05:00
Code Issues Packages Projects Releases Wiki Activity

use pytest for test runner (#452)

Browse Source
This commit is contained in:
Atsushi Sakai
2020-12-31 22:54:40 +09:00
committed by GitHub
parent 581b3cf27a
commit c2debe3b05
8 changed files with 151 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.github/pull_request_template.md vendored
View File

@@ -1,7 +1,7 @@
<!-- <!--
Thanks for contributing a pull request! Thanks for contributing a pull request!
Please ensure that your PR satisfies the checklist before submitting: Please ensure that your PR satisfies the checklist before submitting:
- [] This project only accept codes for python 3.8 or higher. - [] This project only accept codes for python 3.9 or higher.
- [] If you add a new algorithm sample code, please add a unit test file under `test` dir. - [] If you add a new algorithm sample code, please add a unit test file under `test` dir.
This sample test code might help you : https://github.com/AtsushiSakai/PythonRobotics/blob/master/tests/test_a_star.py This sample test code might help you : https://github.com/AtsushiSakai/PythonRobotics/blob/master/tests/test_a_star.py
- [] If you fix a bug of existing code please add a test function in the test code to show the issue was solved. - [] If you fix a bug of existing code please add a test function in the test code to show the issue was solved.

26
ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py
View File

@@ -5,8 +5,10 @@ Left-click the plot to set the goal position of the end effector
Author: Daniel Ingram (daniel-s-ingram) Author: Daniel Ingram (daniel-s-ingram)
Atsushi Sakai (@Atsushi_twi) Atsushi Sakai (@Atsushi_twi)
Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7 p102 Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017,
- [Robotics, Vision and Control \| SpringerLink](https://link.springer.com/book/10.1007/978-3-642-20144-8) ISBN 978-3-319-54413-7 p102
- [Robotics, Vision and Control]
(https://link.springer.com/book/10.1007/978-3-642-20144-8)
""" """
@@ -37,6 +39,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
When out of bounds, rewrite x and y with last correct values When out of bounds, rewrite x and y with last correct values
""" """
global x, y global x, y
x_prev, y_prev = None, None
while True: while True:
try: try:
if x is not None and y is not None: if x is not None and y is not None:
@@ -47,18 +50,20 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
else: else:
theta2_goal = np.arccos( theta2_goal = np.arccos(
(x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2)) (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2))
theta1_goal = np.math.atan2(y, x) - np.math.atan2(l2 * tmp = np.math.atan2(l2 * np.sin(theta2_goal),
np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal))) (l1 + l2 * np.cos(theta2_goal)))
theta1_goal = np.math.atan2(y, x) - tmp
if theta1_goal < 0: if theta1_goal < 0:
theta2_goal = -theta2_goal theta2_goal = -theta2_goal
theta1_goal = np.math.atan2( tmp = np.math.atan2(l2 * np.sin(theta2_goal),
y, x) - np.math.atan2(l2 * np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal))) (l1 + l2 * np.cos(theta2_goal)))
theta1_goal = np.math.atan2(y, x) - tmp
theta1 = theta1 + Kp * ang_diff(theta1_goal, theta1) * dt theta1 = theta1 + Kp * ang_diff(theta1_goal, theta1) * dt
theta2 = theta2 + Kp * ang_diff(theta2_goal, theta2) * dt theta2 = theta2 + Kp * ang_diff(theta2_goal, theta2) * dt
except ValueError as e: except ValueError as e:
print("Unreachable goal") print("Unreachable goal"+e)
except TypeError: except TypeError:
x = x_prev x = x_prev
y = y_prev y = y_prev
@@ -66,6 +71,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
wrist = plot_arm(theta1, theta2, x, y) wrist = plot_arm(theta1, theta2, x, y)
# check goal # check goal
d2goal = None
if x is not None and y is not None: if x is not None and y is not None:
d2goal = np.hypot(wrist[0] - x, wrist[1] - y) d2goal = np.hypot(wrist[0] - x, wrist[1] - y)
@@ -73,7 +79,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
return theta1, theta2 return theta1, theta2
def plot_arm(theta1, theta2, x, y): # pragma: no cover def plot_arm(theta1, theta2, target_x, target_y): # pragma: no cover
shoulder = np.array([0, 0]) shoulder = np.array([0, 0])
elbow = shoulder + np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)]) elbow = shoulder + np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)])
wrist = elbow + \ wrist = elbow + \
@@ -89,8 +95,8 @@ def plot_arm(theta1, theta2, x, y): # pragma: no cover
plt.plot(elbow[0], elbow[1], 'ro') plt.plot(elbow[0], elbow[1], 'ro')
plt.plot(wrist[0], wrist[1], 'ro') plt.plot(wrist[0], wrist[1], 'ro')
plt.plot([wrist[0], x], [wrist[1], y], 'g--') plt.plot([wrist[0], target_x], [wrist[1], target_y], 'g--')
plt.plot(x, y, 'g*') plt.plot(target_x, target_y, 'g*')
plt.xlim(-2, 2) plt.xlim(-2, 2)
plt.ylim(-2, 2) plt.ylim(-2, 2)

176
PathPlanning/Eta3SplinePath/eta3_spline_path.py
View File

@@ -1,13 +1,13 @@
""" """
\eta^3 polynomials planner eta^3 polynomials planner
author: Joe Dinius, Ph.D (https://jwdinius.github.io) author: Joe Dinius, Ph.D (https://jwdinius.github.io)
Atsushi Sakai (@Atsushi_twi) Atsushi Sakai (@Atsushi_twi)
Ref: Ref:
- [eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots]
- [\eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots](https://ieeexplore.ieee.org/document/4339545/) (https://ieeexplore.ieee.org/document/4339545/)
""" """
@@ -15,23 +15,25 @@ import numpy as np
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from scipy.integrate import quad from scipy.integrate import quad
# NOTE: *_pose is a 3-array: 0 - x coord, 1 - y coord, 2 - orientation angle \theta # NOTE: *_pose is a 3-array:
# 0 - x coord, 1 - y coord, 2 - orientation angle \theta
show_animation = True show_animation = True
class eta3_path(object): class Eta3Path(object):
""" """
eta3_path Eta3Path
input input
segments: list of `eta3_path_segment` instances definining a continuous path segments: a list of `Eta3PathSegment` instances
defining a continuous path
""" """
def __init__(self, segments): def __init__(self, segments):
# ensure input has the correct form # ensure input has the correct form
assert(isinstance(segments, list) and isinstance( assert(isinstance(segments, list) and isinstance(
segments[0], eta3_path_segment)) segments[0], Eta3PathSegment))
# ensure that each segment begins from the previous segment's end (continuity) # ensure that each segment begins from the previous segment's end (continuity)
for r, s in zip(segments[:-1], segments[1:]): for r, s in zip(segments[:-1], segments[1:]):
assert(np.array_equal(r.end_pose, s.start_pose)) assert(np.array_equal(r.end_pose, s.start_pose))
@@ -39,7 +41,7 @@ class eta3_path(object):
def calc_path_point(self, u): def calc_path_point(self, u):
""" """
eta3_path::calc_path_point Eta3Path::calc_path_point
input input
normalized interpolation point along path object, 0 <= u <= len(self.segments) normalized interpolation point along path object, 0 <= u <= len(self.segments)
@@ -47,7 +49,7 @@ class eta3_path(object):
2d (x,y) position vector 2d (x,y) position vector
""" """
assert(u >= 0 and u <= len(self.segments)) assert(0 <= u <= len(self.segments))
if np.isclose(u, len(self.segments)): if np.isclose(u, len(self.segments)):
segment_idx = len(self.segments) - 1 segment_idx = len(self.segments) - 1
u = 1. u = 1.
@@ -57,11 +59,12 @@ class eta3_path(object):
return self.segments[segment_idx].calc_point(u) return self.segments[segment_idx].calc_point(u)
class eta3_path_segment(object): class Eta3PathSegment(object):
""" """
eta3_path_segment - constructs an eta^3 path segment based on desired shaping, eta, and curvature vector, kappa. Eta3PathSegment - constructs an eta^3 path segment based on desired
If either, or both, of eta and kappa are not set during initialization, they will shaping, eta, and curvature vector, kappa. If either, or both,
default to zeros. of eta and kappa are not set during initialization,
they will default to zeros.
input input
start_pose - starting pose array (x, y, \theta) start_pose - starting pose array (x, y, \theta)
@@ -110,70 +113,96 @@ class eta3_path_segment(object):
self.coeffs[1, 3] = 1. / 6 * eta[4] * sa + 1. / 6 * \ self.coeffs[1, 3] = 1. / 6 * eta[4] * sa + 1. / 6 * \
(eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * ca (eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * ca
# quartic (u^4) # quartic (u^4)
self.coeffs[0, 4] = 35. * (end_pose[0] - start_pose[0]) - (20. * eta[0] + 5 * eta[2] + 2. / 3 * eta[4]) * ca \ tmp1 = 35. * (end_pose[0] - start_pose[0])
+ (5. * eta[0]**2 * kappa[0] + 2. / 3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * sa \ tmp2 = (20. * eta[0] + 5 * eta[2] + 2. / 3 * eta[4]) * ca
- (15. * eta[1] - 5. / 2 * eta[3] + 1. / 6 * eta[5]) * cb \ tmp3 = (5. * eta[0] ** 2 * kappa[0] + 2. / 3 * eta[0] ** 3 * kappa[1]
- (5. / 2 * eta[1]**2 * kappa[2] - 1. / 6 * eta[1] ** + 2. * eta[0] * eta[2] * kappa[0]) * sa
3 * kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * sb tmp4 = (15. * eta[1] - 5. / 2 * eta[3] + 1. / 6 * eta[5]) * cb
self.coeffs[1, 4] = 35. * (end_pose[1] - start_pose[1]) - (20. * eta[0] + 5. * eta[2] + 2. / 3 * eta[4]) * sa \ tmp5 = (5. / 2 * eta[1] ** 2 * kappa[2] - 1. / 6 * eta[1] ** 3 *
- (5. * eta[0]**2 * kappa[0] + 2. / 3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * ca \ kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * sb
- (15. * eta[1] - 5. / 2 * eta[3] + 1. / 6 * eta[5]) * sb \ self.coeffs[0, 4] = tmp1 - tmp2 + tmp3 - tmp4 - tmp5
+ (5. / 2 * eta[1]**2 * kappa[2] - 1. / 6 * eta[1] ** tmp1 = 35. * (end_pose[1] - start_pose[1])
3 * kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb tmp2 = (20. * eta[0] + 5. * eta[2] + 2. / 3 * eta[4]) * sa
tmp3 = (5. * eta[0] ** 2 * kappa[0] + 2. / 3 * eta[0] ** 3 * kappa[1]
+ 2. * eta[0] * eta[2] * kappa[0]) * ca
tmp4 = (15. * eta[1] - 5. / 2 * eta[3] + 1. / 6 * eta[5]) * sb
tmp5 = (5. / 2 * eta[1] ** 2 * kappa[2] - 1. / 6 * eta[1] ** 3 *
kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
self.coeffs[1, 4] = tmp1 - tmp2 - tmp3 - tmp4 + tmp5
# quintic (u^5) # quintic (u^5)
self.coeffs[0, 5] = -84. * (end_pose[0] - start_pose[0]) + (45. * eta[0] + 10. * eta[2] + eta[4]) * ca \ tmp1 = -84. * (end_pose[0] - start_pose[0])
- (10. * eta[0]**2 * kappa[0] + eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * sa \ tmp2 = (45. * eta[0] + 10. * eta[2] + eta[4]) * ca
+ (39. * eta[1] - 7. * eta[3] + 1. / 2 * eta[5]) * cb \ tmp3 = (10. * eta[0] ** 2 * kappa[0] + eta[0] ** 3 * kappa[1] + 3. *
+ (7. * eta[1]**2 * kappa[2] - 1. / 2 * eta[1]**3 * eta[0] * eta[2] * kappa[0]) * sa
kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * sb tmp4 = (39. * eta[1] - 7. * eta[3] + 1. / 2 * eta[5]) * cb
self.coeffs[1, 5] = -84. * (end_pose[1] - start_pose[1]) + (45. * eta[0] + 10. * eta[2] + eta[4]) * sa \ tmp5 = + (7. * eta[1] ** 2 * kappa[2] - 1. / 2 * eta[1] ** 3 * kappa[3]
+ (10. * eta[0]**2 * kappa[0] + eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * ca \ - 3. / 2 * eta[1] * eta[3] * kappa[2]) * sb
+ (39. * eta[1] - 7. * eta[3] + 1. / 2 * eta[5]) * sb \ self.coeffs[0, 5] = tmp1 + tmp2 - tmp3 + tmp4 + tmp5
- (7. * eta[1]**2 * kappa[2] - 1. / 2 * eta[1]**3 * tmp1 = -84. * (end_pose[1] - start_pose[1])
kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * cb tmp2 = (45. * eta[0] + 10. * eta[2] + eta[4]) * sa
tmp3 = (10. * eta[0] ** 2 * kappa[0] + eta[0] ** 3 * kappa[1] + 3. *
eta[0] * eta[2] * kappa[0]) * ca
tmp4 = (39. * eta[1] - 7. * eta[3] + 1. / 2 * eta[5]) * sb
tmp5 = - (7. * eta[1] ** 2 * kappa[2] - 1. / 2 * eta[1] ** 3 * kappa[3]
- 3. / 2 * eta[1] * eta[3] * kappa[2]) * cb
self.coeffs[1, 5] = tmp1 + tmp2 + tmp3 + tmp4 + tmp5
# sextic (u^6) # sextic (u^6)
self.coeffs[0, 6] = 70. * (end_pose[0] - start_pose[0]) - (36. * eta[0] + 15. / 2 * eta[2] + 2. / 3 * eta[4]) * ca \ tmp1 = 70. * (end_pose[0] - start_pose[0])
+ (15. / 2 * eta[0]**2 * kappa[0] + 2. / 3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * sa \ tmp2 = (36. * eta[0] + 15. / 2 * eta[2] + 2. / 3 * eta[4]) * ca
- (34. * eta[1] - 13. / 2 * eta[3] + 1. / 2 * eta[5]) * cb \ tmp3 = + (15. / 2 * eta[0] ** 2 * kappa[0] + 2. / 3 * eta[0] ** 3 *
- (13. / 2 * eta[1]**2 * kappa[2] - 1. / 2 * eta[1] ** kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * sa
3 * kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * sb tmp4 = (34. * eta[1] - 13. / 2 * eta[3] + 1. / 2 * eta[5]) * cb
self.coeffs[1, 6] = 70. * (end_pose[1] - start_pose[1]) - (36. * eta[0] + 15. / 2 * eta[2] + 2. / 3 * eta[4]) * sa \ tmp5 = - (13. / 2 * eta[1] ** 2 * kappa[2] - 1. / 2 * eta[1] ** 3 *
- (15. / 2 * eta[0]**2 * kappa[0] + 2. / 3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * ca \ kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * sb
- (34. * eta[1] - 13. / 2 * eta[3] + 1. / 2 * eta[5]) * sb \ self.coeffs[0, 6] = tmp1 - tmp2 + tmp3 - tmp4 + tmp5
+ (13. / 2 * eta[1]**2 * kappa[2] - 1. / 2 * eta[1] ** tmp1 = 70. * (end_pose[1] - start_pose[1])
3 * kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * cb tmp2 = - (36. * eta[0] + 15. / 2 * eta[2] + 2. / 3 * eta[4]) * sa
tmp3 = - (15. / 2 * eta[0] ** 2 * kappa[0] + 2. / 3 * eta[0] ** 3 *
kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * ca
tmp4 = - (34. * eta[1] - 13. / 2 * eta[3] + 1. / 2 * eta[5]) * sb
tmp5 = + (13. / 2 * eta[1] ** 2 * kappa[2] - 1. / 2 * eta[1] ** 3 *
kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * cb
self.coeffs[1, 6] = tmp1 + tmp2 + tmp3 + tmp4 + tmp5
# septic (u^7) # septic (u^7)
self.coeffs[0, 7] = -20. * (end_pose[0] - start_pose[0]) + (10. * eta[0] + 2. * eta[2] + 1. / 6 * eta[4]) * ca \ tmp1 = -20. * (end_pose[0] - start_pose[0])
- (2. * eta[0]**2 * kappa[0] + 1. / 6 * eta[0]**3 * kappa[1] + 1. / 2 * eta[0] * eta[2] * kappa[0]) * sa \ tmp2 = (10. * eta[0] + 2. * eta[2] + 1. / 6 * eta[4]) * ca
+ (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * cb \ tmp3 = - (2. * eta[0] ** 2 * kappa[0] + 1. / 6 * eta[0] ** 3 * kappa[1]
+ (2. * eta[1]**2 * kappa[2] - 1. / 6 * eta[1]**3 * + 1. / 2 * eta[0] * eta[2] * kappa[0]) * sa
kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * sb tmp4 = (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * cb
self.coeffs[1, 7] = -20. * (end_pose[1] - start_pose[1]) + (10. * eta[0] + 2. * eta[2] + 1. / 6 * eta[4]) * sa \ tmp5 = (2. * eta[1] ** 2 * kappa[2] - 1. / 6 * eta[1] ** 3 * kappa[3]
+ (2. * eta[0]**2 * kappa[0] + 1. / 6 * eta[0]**3 * kappa[1] + 1. / 2 * eta[0] * eta[2] * kappa[0]) * ca \ - 1. / 2 * eta[1] * eta[3] * kappa[2]) * sb
+ (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * sb \ self.coeffs[0, 7] = tmp1 + tmp2 + tmp3 + tmp4 + tmp5
- (2. * eta[1]**2 * kappa[2] - 1. / 6 * eta[1]**3 *
kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
self.s_dot = lambda u: max(np.linalg.norm(self.coeffs[:, 1:].dot(np.array( tmp1 = -20. * (end_pose[1] - start_pose[1])
[1, 2. * u, 3. * u**2, 4. * u**3, 5. * u**4, 6. * u**5, 7. * u**6]))), 1e-6) tmp2 = (10. * eta[0] + 2. * eta[2] + 1. / 6 * eta[4]) * sa
tmp3 = (2. * eta[0] ** 2 * kappa[0] + 1. / 6 * eta[0] ** 3 * kappa[1]
+ 1. / 2 * eta[0] * eta[2] * kappa[0]) * ca
tmp4 = (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * sb
tmp5 = - (2. * eta[1] ** 2 * kappa[2] - 1. / 6 * eta[1] ** 3 * kappa[3]
- 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
self.coeffs[1, 7] = tmp1 + tmp2 + tmp3 + tmp4 + tmp5
self.s_dot = lambda u: max(np.linalg.norm(
self.coeffs[:, 1:].dot(np.array(
[1, 2. * u, 3. * u**2, 4. * u**3,
5. * u**4, 6. * u**5, 7. * u**6]))), 1e-6)
self.f_length = lambda ue: quad(lambda u: self.s_dot(u), 0, ue) self.f_length = lambda ue: quad(lambda u: self.s_dot(u), 0, ue)
self.segment_length = self.f_length(1)[0] self.segment_length = self.f_length(1)[0]
def calc_point(self, u): def calc_point(self, u):
""" """
eta3_path_segment::calc_point Eta3PathSegment::calc_point
input input
u - parametric representation of a point along the segment, 0 <= u <= 1 u - parametric representation of a point along the segment, 0 <= u <= 1
returns returns
(x,y) of point along the segment (x,y) of point along the segment
""" """
assert(u >= 0 and u <= 1) assert(0 <= u <= 1)
return self.coeffs.dot(np.array([1, u, u**2, u**3, u**4, u**5, u**6, u**7])) return self.coeffs.dot(np.array([1, u, u**2, u**3, u**4, u**5, u**6, u**7]))
def calc_deriv(self, u, order=1): def calc_deriv(self, u, order=1):
""" """
eta3_path_segment::calc_deriv Eta3PathSegment::calc_deriv
input input
u - parametric representation of a point along the segment, 0 <= u <= 1 u - parametric representation of a point along the segment, 0 <= u <= 1
@@ -181,8 +210,8 @@ class eta3_path_segment(object):
(d^nx/du^n,d^ny/du^n) of point along the segment, for 0 < n <= 2 (d^nx/du^n,d^ny/du^n) of point along the segment, for 0 < n <= 2
""" """
assert(u >= 0 and u <= 1) assert(0 <= u <= 1)
assert(order > 0 and order <= 2) assert(0 < order <= 2)
if order == 1: if order == 1:
return self.coeffs[:, 1:].dot(np.array([1, 2. * u, 3. * u**2, 4. * u**3, 5. * u**4, 6. * u**5, 7. * u**6])) return self.coeffs[:, 1:].dot(np.array([1, 2. * u, 3. * u**2, 4. * u**3, 5. * u**4, 6. * u**5, 7. * u**6]))
@@ -199,10 +228,10 @@ def test1():
# NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative # NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative
kappa = [0, 0, 0, 0] kappa = [0, 0, 0, 0]
eta = [i, i, 0, 0, 0, 0] eta = [i, i, 0, 0, 0, 0]
path_segments.append(eta3_path_segment( path_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
path = eta3_path(path_segments) path = Eta3Path(path_segments)
# interpolate at several points along the path # interpolate at several points along the path
ui = np.linspace(0, len(path_segments), 1001) ui = np.linspace(0, len(path_segments), 1001)
@@ -214,8 +243,9 @@ def test1():
# plot the path # plot the path
plt.plot(pos[0, :], pos[1, :]) plt.plot(pos[0, :], pos[1, :])
# for stopping simulation with the esc key. # for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event', plt.gcf().canvas.mpl_connect(
lambda event: [exit(0) if event.key == 'escape' else None]) 'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.pause(1.0) plt.pause(1.0)
if show_animation: if show_animation:
@@ -232,10 +262,10 @@ def test2():
# NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative # NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative
kappa = [0, 0, 0, 0] kappa = [0, 0, 0, 0]
eta = [0, 0, (i - 5) * 20, (5 - i) * 20, 0, 0] eta = [0, 0, (i - 5) * 20, (5 - i) * 20, 0, 0]
path_segments.append(eta3_path_segment( path_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
path = eta3_path(path_segments) path = Eta3Path(path_segments)
# interpolate at several points along the path # interpolate at several points along the path
ui = np.linspace(0, len(path_segments), 1001) ui = np.linspace(0, len(path_segments), 1001)
@@ -261,7 +291,7 @@ def test3():
# NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative # NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative
kappa = [0, 0, 0, 0] kappa = [0, 0, 0, 0]
eta = [4.27, 4.27, 0, 0, 0, 0] eta = [4.27, 4.27, 0, 0, 0, 0]
path_segments.append(eta3_path_segment( path_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
# segment 2: line segment # segment 2: line segment
@@ -269,7 +299,7 @@ def test3():
end_pose = [5.5, 1.5, 0] end_pose = [5.5, 1.5, 0]
kappa = [0, 0, 0, 0] kappa = [0, 0, 0, 0]
eta = [0, 0, 0, 0, 0, 0] eta = [0, 0, 0, 0, 0, 0]
path_segments.append(eta3_path_segment( path_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
# segment 3: cubic spiral # segment 3: cubic spiral
@@ -277,7 +307,7 @@ def test3():
end_pose = [7.4377, 1.8235, 0.6667] end_pose = [7.4377, 1.8235, 0.6667]
kappa = [0, 0, 1, 1] kappa = [0, 0, 1, 1]
eta = [1.88, 1.88, 0, 0, 0, 0] eta = [1.88, 1.88, 0, 0, 0, 0]
path_segments.append(eta3_path_segment( path_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
# segment 4: generic twirl arc # segment 4: generic twirl arc
@@ -285,7 +315,7 @@ def test3():
end_pose = [7.8, 4.3, 1.8] end_pose = [7.8, 4.3, 1.8]
kappa = [1, 1, 0.5, 0] kappa = [1, 1, 0.5, 0]
eta = [7, 10, 10, -10, 4, 4] eta = [7, 10, 10, -10, 4, 4]
path_segments.append(eta3_path_segment( path_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
# segment 5: circular arc # segment 5: circular arc
@@ -293,11 +323,11 @@ def test3():
end_pose = [5.4581, 5.8064, 3.3416] end_pose = [5.4581, 5.8064, 3.3416]
kappa = [0.5, 0, 0.5, 0] kappa = [0.5, 0, 0.5, 0]
eta = [2.98, 2.98, 0, 0, 0, 0] eta = [2.98, 2.98, 0, 0, 0, 0]
path_segments.append(eta3_path_segment( path_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
# construct the whole path # construct the whole path
path = eta3_path(path_segments) path = Eta3Path(path_segments)
# interpolate at several points along the path # interpolate at several points along the path
ui = np.linspace(0, len(path_segments), 1001) ui = np.linspace(0, len(path_segments), 1001)

48
PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py
View File

@@ -1,13 +1,14 @@
""" """
\eta^3 polynomials trajectory planner eta^3 polynomials trajectory planner
author: Joe Dinius, Ph.D (https://jwdinius.github.io) author: Joe Dinius, Ph.D (https://jwdinius.github.io)
Atsushi Sakai (@Atsushi_twi) Atsushi Sakai (@Atsushi_twi)
Refs: Refs:
- https://jwdinius.github.io/blog/2018/eta3traj - https://jwdinius.github.io/blog/2018/eta3traj
- [\eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots](https://ieeexplore.ieee.org/document/4339545/) - [eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots]
(https://ieeexplore.ieee.org/document/4339545/)
""" """
@@ -19,8 +20,8 @@ import os
sys.path.append(os.path.relpath("../Eta3SplinePath")) sys.path.append(os.path.relpath("../Eta3SplinePath"))
try: try:
from eta3_spline_path import eta3_path, eta3_path_segment from eta3_spline_path import Eta3Path, Eta3PathSegment
except: except ImportError:
raise raise
show_animation = True show_animation = True
@@ -32,7 +33,7 @@ class MaxVelocityNotReached(Exception):
actual_vel, max_vel) actual_vel, max_vel)
class eta3_trajectory(eta3_path): class eta3_trajectory(Eta3Path):
""" """
eta3_trajectory eta3_trajectory
@@ -41,7 +42,7 @@ class eta3_trajectory(eta3_path):
""" """
def __init__(self, segments, max_vel, v0=0.0, a0=0.0, max_accel=2.0, max_jerk=5.0): def __init__(self, segments, max_vel, v0=0.0, a0=0.0, max_accel=2.0, max_jerk=5.0):
# ensure that all inputs obey the assumptions of the model # ensure that all inputs obey the assumptions of the model
assert max_vel > 0 and v0 >= 0 and a0 >= 0 and max_accel > 0 and max_jerk > 0 \ assert max_vel > 0 and v0 >= 0 and a0 >= 0 and max_accel > 0 and max_jerk > 0 \
and a0 <= max_accel and v0 <= max_vel and a0 <= max_accel and v0 <= max_vel
super(eta3_trajectory, self).__init__(segments=segments) super(eta3_trajectory, self).__init__(segments=segments)
@@ -61,19 +62,19 @@ class eta3_trajectory(eta3_path):
self.prev_seg_id = 0 self.prev_seg_id = 0
def velocity_profile(self): def velocity_profile(self):
''' /~~~~~----------------~~~~~\ r""" /~~~~~----------------\
/ \ / \
/ \ / \
/ \ / \
/ \ / \
(v=v0, a=a0) ~~~~~ \ (v=v0, a=a0) ~~~~~ \
\ \
\~~~~~ (vf=0, af=0) \ ~~~~~ (vf=0, af=0)
pos.|pos.|neg.| cruise at |neg.| neg. |neg. pos.|pos.|neg.| cruise at |neg.| neg. |neg.
max |max.|max.| max. |max.| max. |max. max |max.|max.| max. |max.| max. |max.
jerk|acc.|jerk| velocity |jerk| acc. |jerk jerk|acc.|jerk| velocity |jerk| acc. |jerk
index 0 1 2 3 (optional) 4 5 6 index 0 1 2 3 (optional) 4 5 6
''' """
# delta_a: accel change from initial position to end of maximal jerk section # delta_a: accel change from initial position to end of maximal jerk section
delta_a = self.max_accel - self.a0 delta_a = self.max_accel - self.a0
# t_s1: time of traversal of maximal jerk section # t_s1: time of traversal of maximal jerk section
@@ -112,7 +113,6 @@ class eta3_trajectory(eta3_path):
self.seg_lengths = np.zeros((7,)) self.seg_lengths = np.zeros((7,))
# Section 0: max jerk up to max acceleration # Section 0: max jerk up to max acceleration
index = 0
self.times[0] = t_s1 self.times[0] = t_s1
self.vels[0] = v_s1 self.vels[0] = v_s1
self.seg_lengths[0] = s_s1 self.seg_lengths[0] = s_s1
@@ -195,7 +195,7 @@ class eta3_trajectory(eta3_path):
def fprime(u): def fprime(u):
return self.segments[seg_id].s_dot(u) return self.segments[seg_id].s_dot(u)
while (ui >= 0 and ui <= 1) and abs(f(ui)) > tol: while (0 <= ui <= 1) and abs(f(ui)) > tol:
ui -= f(ui) / fprime(ui) ui -= f(ui) / fprime(ui)
ui = max(0, min(ui, 1)) ui = max(0, min(ui, 1))
return ui return ui
@@ -301,7 +301,7 @@ def test1(max_vel=0.5):
# NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative # NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative
kappa = [0, 0, 0, 0] kappa = [0, 0, 0, 0]
eta = [i, i, 0, 0, 0, 0] eta = [i, i, 0, 0, 0, 0]
trajectory_segments.append(eta3_path_segment( trajectory_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
traj = eta3_trajectory(trajectory_segments, traj = eta3_trajectory(trajectory_segments,
@@ -335,7 +335,7 @@ def test2(max_vel=0.5):
# NOTE: INTEGRATOR ERROR EXPLODES WHEN eta[:1] IS ZERO! # NOTE: INTEGRATOR ERROR EXPLODES WHEN eta[:1] IS ZERO!
# was: eta = [0, 0, (i - 5) * 20, (5 - i) * 20, 0, 0], now is: # was: eta = [0, 0, (i - 5) * 20, (5 - i) * 20, 0, 0], now is:
eta = [0.1, 0.1, (i - 5) * 20, (5 - i) * 20, 0, 0] eta = [0.1, 0.1, (i - 5) * 20, (5 - i) * 20, 0, 0]
trajectory_segments.append(eta3_path_segment( trajectory_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
traj = eta3_trajectory(trajectory_segments, traj = eta3_trajectory(trajectory_segments,
@@ -366,7 +366,7 @@ def test3(max_vel=2.0):
# NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative # NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative
kappa = [0, 0, 0, 0] kappa = [0, 0, 0, 0]
eta = [4.27, 4.27, 0, 0, 0, 0] eta = [4.27, 4.27, 0, 0, 0, 0]
trajectory_segments.append(eta3_path_segment( trajectory_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
# segment 2: line segment # segment 2: line segment
@@ -376,7 +376,7 @@ def test3(max_vel=2.0):
# NOTE: INTEGRATOR ERROR EXPLODES WHEN eta[:1] IS ZERO! # NOTE: INTEGRATOR ERROR EXPLODES WHEN eta[:1] IS ZERO!
# was: eta = [0, 0, 0, 0, 0, 0], now is: # was: eta = [0, 0, 0, 0, 0, 0], now is:
eta = [0.5, 0.5, 0, 0, 0, 0] eta = [0.5, 0.5, 0, 0, 0, 0]
trajectory_segments.append(eta3_path_segment( trajectory_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
# segment 3: cubic spiral # segment 3: cubic spiral
@@ -384,7 +384,7 @@ def test3(max_vel=2.0):
end_pose = [7.4377, 1.8235, 0.6667] end_pose = [7.4377, 1.8235, 0.6667]
kappa = [0, 0, 1, 1] kappa = [0, 0, 1, 1]
eta = [1.88, 1.88, 0, 0, 0, 0] eta = [1.88, 1.88, 0, 0, 0, 0]
trajectory_segments.append(eta3_path_segment( trajectory_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
# segment 4: generic twirl arc # segment 4: generic twirl arc
@@ -392,7 +392,7 @@ def test3(max_vel=2.0):
end_pose = [7.8, 4.3, 1.8] end_pose = [7.8, 4.3, 1.8]
kappa = [1, 1, 0.5, 0] kappa = [1, 1, 0.5, 0]
eta = [7, 10, 10, -10, 4, 4] eta = [7, 10, 10, -10, 4, 4]
trajectory_segments.append(eta3_path_segment( trajectory_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
# segment 5: circular arc # segment 5: circular arc
@@ -400,7 +400,7 @@ def test3(max_vel=2.0):
end_pose = [5.4581, 5.8064, 3.3416] end_pose = [5.4581, 5.8064, 3.3416]
kappa = [0.5, 0, 0.5, 0] kappa = [0.5, 0, 0.5, 0]
eta = [2.98, 2.98, 0, 0, 0, 0] eta = [2.98, 2.98, 0, 0, 0, 0]
trajectory_segments.append(eta3_path_segment( trajectory_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
# construct the whole path # construct the whole path

2
README.md
View File

@@ -103,6 +103,8 @@ See this paper for more details:
- pandas - pandas
- [cvxpy](https://www.cvxpy.org/index.html) - [cvxpy](https://www.cvxpy.org/index.html)
- pytest (only for unit tests)
# Documentation # Documentation

1
requirements.txt
View File

@@ -3,3 +3,4 @@ scipy == 1.5.4
pandas == 1.2.0 pandas == 1.2.0
matplotlib == 3.3.3 matplotlib == 3.3.3
cvxpy == 1.1.7 cvxpy == 1.1.7
pytest == 6.2.1

8
runtests.sh
View File

@@ -1,6 +1,6 @@
#!/usr/bin/env bash #!/usr/bin/env bash
echo "Run test suites! " echo "Run test suites! "
export PYTHONWARNINGS=default # show warning # tests: include unittest based tests
#python -m unittest discover tests # -Werror: warning as error
#python -Wignore -m unittest discover tests #ignore warning # --durations=0: show ranking of test durations
coverage run -m unittest discover tests # generate coverage file pytest tests -Werror --durations=0

0
tests/__init__.py Normal file
View File