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

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!
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.
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.

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)
Atsushi Sakai (@Atsushi_twi)
Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7 p102
- [Robotics, Vision and Control \| SpringerLink](https://link.springer.com/book/10.1007/978-3-642-20144-8)
Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017,
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
"""
global x, y
x_prev, y_prev = None, None
while True:
try:
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:
theta2_goal = np.arccos(
(x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2))
theta1_goal = np.math.atan2(y, x) - np.math.atan2(l2 *
np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal)))
tmp = np.math.atan2(l2 * np.sin(theta2_goal),
(l1 + l2 * np.cos(theta2_goal)))
theta1_goal = np.math.atan2(y, x) - tmp
if theta1_goal < 0:
theta2_goal = -theta2_goal
theta1_goal = np.math.atan2(
y, x) - np.math.atan2(l2 * np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal)))
tmp = np.math.atan2(l2 * np.sin(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
theta2 = theta2 + Kp * ang_diff(theta2_goal, theta2) * dt
except ValueError as e:
print("Unreachable goal")
print("Unreachable goal"+e)
except TypeError:
x = x_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)
# check goal
d2goal = None
if x is not None and y is not None:
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
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])
elbow = shoulder + np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)])
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(wrist[0], wrist[1], 'ro')
plt.plot([wrist[0], x], [wrist[1], y], 'g--')
plt.plot(x, y, 'g*')
plt.plot([wrist[0], target_x], [wrist[1], target_y], 'g--')
plt.plot(target_x, target_y, 'g*')
plt.xlim(-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)
Atsushi Sakai (@Atsushi_twi)
Ref:
- [\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/)
"""
@@ -15,23 +15,25 @@ import numpy as np
import matplotlib.pyplot as plt
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
class eta3_path(object):
class Eta3Path(object):
"""
eta3_path
Eta3Path
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):
# ensure input has the correct form
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)
for r, s in zip(segments[:-1], segments[1:]):
assert(np.array_equal(r.end_pose, s.start_pose))
@@ -39,7 +41,7 @@ class eta3_path(object):
def calc_path_point(self, u):
"""
eta3_path::calc_path_point
Eta3Path::calc_path_point
input
normalized interpolation point along path object, 0 <= u <= len(self.segments)
@@ -47,7 +49,7 @@ class eta3_path(object):
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)):
segment_idx = len(self.segments) - 1
u = 1.
@@ -57,11 +59,12 @@ class eta3_path(object):
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.
If either, or both, of eta and kappa are not set during initialization, they will
default to zeros.
Eta3PathSegment - constructs an eta^3 path segment based on desired
shaping, eta, and curvature vector, kappa. If either, or both,
of eta and kappa are not set during initialization,
they will default to zeros.
input
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 * \
(eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * ca
# 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 \
+ (5. * eta[0]**2 * kappa[0] + 2. / 3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * sa \
- (15. * eta[1] - 5. / 2 * eta[3] + 1. / 6 * eta[5]) * cb \
- (5. / 2 * eta[1]**2 * kappa[2] - 1. / 6 * eta[1] **
3 * kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * sb
self.coeffs[1, 4] = 35. * (end_pose[1] - start_pose[1]) - (20. * eta[0] + 5. * eta[2] + 2. / 3 * eta[4]) * sa \
- (5. * eta[0]**2 * kappa[0] + 2. / 3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * ca \
- (15. * eta[1] - 5. / 2 * eta[3] + 1. / 6 * eta[5]) * sb \
+ (5. / 2 * eta[1]**2 * kappa[2] - 1. / 6 * eta[1] **
3 * kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
tmp1 = 35. * (end_pose[0] - start_pose[0])
tmp2 = (20. * eta[0] + 5 * eta[2] + 2. / 3 * eta[4]) * ca
tmp3 = (5. * eta[0] ** 2 * kappa[0] + 2. / 3 * eta[0] ** 3 * kappa[1]
+ 2. * eta[0] * eta[2] * kappa[0]) * sa
tmp4 = (15. * eta[1] - 5. / 2 * eta[3] + 1. / 6 * eta[5]) * cb
tmp5 = (5. / 2 * eta[1] ** 2 * kappa[2] - 1. / 6 * eta[1] ** 3 *
kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * sb
self.coeffs[0, 4] = tmp1 - tmp2 + tmp3 - tmp4 - tmp5
tmp1 = 35. * (end_pose[1] - start_pose[1])
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)
self.coeffs[0, 5] = -84. * (end_pose[0] - start_pose[0]) + (45. * eta[0] + 10. * eta[2] + eta[4]) * ca \
- (10. * eta[0]**2 * kappa[0] + eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * sa \
+ (39. * eta[1] - 7. * eta[3] + 1. / 2 * eta[5]) * cb \
+ (7. * eta[1]**2 * kappa[2] - 1. / 2 * eta[1]**3 *
kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * sb
self.coeffs[1, 5] = -84. * (end_pose[1] - start_pose[1]) + (45. * eta[0] + 10. * eta[2] + eta[4]) * sa \
+ (10. * eta[0]**2 * kappa[0] + eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * ca \
+ (39. * eta[1] - 7. * eta[3] + 1. / 2 * eta[5]) * sb \
- (7. * eta[1]**2 * kappa[2] - 1. / 2 * eta[1]**3 *
kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * cb
tmp1 = -84. * (end_pose[0] - start_pose[0])
tmp2 = (45. * eta[0] + 10. * eta[2] + eta[4]) * ca
tmp3 = (10. * eta[0] ** 2 * kappa[0] + eta[0] ** 3 * kappa[1] + 3. *
eta[0] * eta[2] * kappa[0]) * sa
tmp4 = (39. * eta[1] - 7. * eta[3] + 1. / 2 * eta[5]) * cb
tmp5 = + (7. * eta[1] ** 2 * kappa[2] - 1. / 2 * eta[1] ** 3 * kappa[3]
- 3. / 2 * eta[1] * eta[3] * kappa[2]) * sb
self.coeffs[0, 5] = tmp1 + tmp2 - tmp3 + tmp4 + tmp5
tmp1 = -84. * (end_pose[1] - start_pose[1])
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)
self.coeffs[0, 6] = 70. * (end_pose[0] - start_pose[0]) - (36. * eta[0] + 15. / 2 * eta[2] + 2. / 3 * eta[4]) * ca \
+ (15. / 2 * eta[0]**2 * kappa[0] + 2. / 3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * sa \
- (34. * eta[1] - 13. / 2 * eta[3] + 1. / 2 * eta[5]) * cb \
- (13. / 2 * eta[1]**2 * kappa[2] - 1. / 2 * eta[1] **
3 * kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * sb
self.coeffs[1, 6] = 70. * (end_pose[1] - start_pose[1]) - (36. * eta[0] + 15. / 2 * eta[2] + 2. / 3 * eta[4]) * sa \
- (15. / 2 * eta[0]**2 * kappa[0] + 2. / 3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * ca \
- (34. * eta[1] - 13. / 2 * eta[3] + 1. / 2 * eta[5]) * sb \
+ (13. / 2 * eta[1]**2 * kappa[2] - 1. / 2 * eta[1] **
3 * kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * cb
tmp1 = 70. * (end_pose[0] - start_pose[0])
tmp2 = (36. * eta[0] + 15. / 2 * eta[2] + 2. / 3 * eta[4]) * ca
tmp3 = + (15. / 2 * eta[0] ** 2 * kappa[0] + 2. / 3 * eta[0] ** 3 *
kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * sa
tmp4 = (34. * eta[1] - 13. / 2 * eta[3] + 1. / 2 * eta[5]) * cb
tmp5 = - (13. / 2 * eta[1] ** 2 * kappa[2] - 1. / 2 * eta[1] ** 3 *
kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * sb
self.coeffs[0, 6] = tmp1 - tmp2 + tmp3 - tmp4 + tmp5
tmp1 = 70. * (end_pose[1] - start_pose[1])
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)
self.coeffs[0, 7] = -20. * (end_pose[0] - start_pose[0]) + (10. * eta[0] + 2. * eta[2] + 1. / 6 * eta[4]) * ca \
- (2. * eta[0]**2 * kappa[0] + 1. / 6 * eta[0]**3 * kappa[1] + 1. / 2 * eta[0] * eta[2] * kappa[0]) * sa \
+ (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * cb \
+ (2. * eta[1]**2 * kappa[2] - 1. / 6 * eta[1]**3 *
kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * sb
self.coeffs[1, 7] = -20. * (end_pose[1] - start_pose[1]) + (10. * eta[0] + 2. * eta[2] + 1. / 6 * eta[4]) * sa \
+ (2. * eta[0]**2 * kappa[0] + 1. / 6 * eta[0]**3 * kappa[1] + 1. / 2 * eta[0] * eta[2] * kappa[0]) * ca \
+ (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * sb \
- (2. * eta[1]**2 * kappa[2] - 1. / 6 * eta[1]**3 *
kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
tmp1 = -20. * (end_pose[0] - start_pose[0])
tmp2 = (10. * eta[0] + 2. * eta[2] + 1. / 6 * eta[4]) * ca
tmp3 = - (2. * eta[0] ** 2 * kappa[0] + 1. / 6 * eta[0] ** 3 * kappa[1]
+ 1. / 2 * eta[0] * eta[2] * kappa[0]) * sa
tmp4 = (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * cb
tmp5 = (2. * eta[1] ** 2 * kappa[2] - 1. / 6 * eta[1] ** 3 * kappa[3]
- 1. / 2 * eta[1] * eta[3] * kappa[2]) * sb
self.coeffs[0, 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)
tmp1 = -20. * (end_pose[1] - start_pose[1])
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.segment_length = self.f_length(1)[0]
def calc_point(self, u):
"""
eta3_path_segment::calc_point
Eta3PathSegment::calc_point
input
u - parametric representation of a point along the segment, 0 <= u <= 1
returns
(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]))
def calc_deriv(self, u, order=1):
"""
eta3_path_segment::calc_deriv
Eta3PathSegment::calc_deriv
input
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
"""
assert(u >= 0 and u <= 1)
assert(order > 0 and order <= 2)
assert(0 <= u <= 1)
assert(0 < order <= 2)
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]))
@@ -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
kappa = [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))
path = eta3_path(path_segments)
path = Eta3Path(path_segments)
# interpolate at several points along the path
ui = np.linspace(0, len(path_segments), 1001)
@@ -214,8 +243,9 @@ def test1():
# plot the path
plt.plot(pos[0, :], pos[1, :])
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.pause(1.0)
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
kappa = [0, 0, 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))
path = eta3_path(path_segments)
path = Eta3Path(path_segments)
# interpolate at several points along the path
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
kappa = [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))
# segment 2: line segment
@@ -269,7 +299,7 @@ def test3():
end_pose = [5.5, 1.5, 0]
kappa = [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))
# segment 3: cubic spiral
@@ -277,7 +307,7 @@ def test3():
end_pose = [7.4377, 1.8235, 0.6667]
kappa = [0, 0, 1, 1]
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))
# segment 4: generic twirl arc
@@ -285,7 +315,7 @@ def test3():
end_pose = [7.8, 4.3, 1.8]
kappa = [1, 1, 0.5, 0]
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))
# segment 5: circular arc
@@ -293,11 +323,11 @@ def test3():
end_pose = [5.4581, 5.8064, 3.3416]
kappa = [0.5, 0, 0.5, 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))
# construct the whole path
path = eta3_path(path_segments)
path = Eta3Path(path_segments)
# interpolate at several points along the path
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)
Atsushi Sakai (@Atsushi_twi)
Refs:
- 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"))
try:
from eta3_spline_path import eta3_path, eta3_path_segment
except:
from eta3_spline_path import Eta3Path, Eta3PathSegment
except ImportError:
raise
show_animation = True
@@ -32,7 +33,7 @@ class MaxVelocityNotReached(Exception):
actual_vel, max_vel)
class eta3_trajectory(eta3_path):
class eta3_trajectory(Eta3Path):
"""
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):
# 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 \
and a0 <= max_accel and v0 <= max_vel
super(eta3_trajectory, self).__init__(segments=segments)
@@ -61,19 +62,19 @@ class eta3_trajectory(eta3_path):
self.prev_seg_id = 0
def velocity_profile(self):
''' /~~~~~----------------~~~~~\
/ \
/ \
/ \
/ \
(v=v0, a=a0) ~~~~~ \
\
\~~~~~ (vf=0, af=0)
r""" /~~~~~----------------\
/ \
/ \
/ \
/ \
(v=v0, a=a0) ~~~~~ \
\
\ ~~~~~ (vf=0, af=0)
pos.|pos.|neg.| cruise at |neg.| neg. |neg.
max |max.|max.| max. |max.| max. |max.
jerk|acc.|jerk| velocity |jerk| acc. |jerk
index 0 1 2 3 (optional) 4 5 6
'''
"""
# delta_a: accel change from initial position to end of maximal jerk section
delta_a = self.max_accel - self.a0
# t_s1: time of traversal of maximal jerk section
@@ -112,7 +113,6 @@ class eta3_trajectory(eta3_path):
self.seg_lengths = np.zeros((7,))
# Section 0: max jerk up to max acceleration
index = 0
self.times[0] = t_s1
self.vels[0] = v_s1
self.seg_lengths[0] = s_s1
@@ -195,7 +195,7 @@ class eta3_trajectory(eta3_path):
def fprime(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 = max(0, min(ui, 1))
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
kappa = [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))
traj = eta3_trajectory(trajectory_segments,
@@ -335,7 +335,7 @@ def test2(max_vel=0.5):
# NOTE: INTEGRATOR ERROR EXPLODES WHEN eta[:1] IS ZERO!
# 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]
trajectory_segments.append(eta3_path_segment(
trajectory_segments.append(Eta3PathSegment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
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
kappa = [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))
# segment 2: line segment
@@ -376,7 +376,7 @@ def test3(max_vel=2.0):
# NOTE: INTEGRATOR ERROR EXPLODES WHEN eta[:1] IS ZERO!
# was: eta = [0, 0, 0, 0, 0, 0], now is:
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))
# segment 3: cubic spiral
@@ -384,7 +384,7 @@ def test3(max_vel=2.0):
end_pose = [7.4377, 1.8235, 0.6667]
kappa = [0, 0, 1, 1]
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))
# segment 4: generic twirl arc
@@ -392,7 +392,7 @@ def test3(max_vel=2.0):
end_pose = [7.8, 4.3, 1.8]
kappa = [1, 1, 0.5, 0]
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))
# segment 5: circular arc
@@ -400,7 +400,7 @@ def test3(max_vel=2.0):
end_pose = [5.4581, 5.8064, 3.3416]
kappa = [0.5, 0, 0.5, 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))
# construct the whole path

2
README.md
View File

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

1
requirements.txt
View File

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

8
runtests.sh
View File

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

0
tests/__init__.py Normal file
View File