diff --git a/control/src/control/control_ros.py b/control/src/control/control_ros.py
index caa937b85790a6ef210701466de153a79c59af97..2ef6f8beffd288b3b97c8210d5b2507ef5914d30 100644
--- a/control/src/control/control_ros.py
+++ b/control/src/control/control_ros.py
@@ -188,7 +188,9 @@ class ControlROS:
"""
if not self.controller.is_alive():
raise RuntimeError("Path command set before controller was started")
- path_xytv = time_parameterize_ramp_up_ramp_down(path_xyt, speed)
+ path_xytv = time_parameterize_ramp_up_ramp_down(
+ path_xyt, speed, self.controller.min_speed
+ )
self.follow_path(path_xytv)
def follow_path(self, path_xytv):
@@ -294,6 +296,7 @@ def get_ros_params():
"finish_threshold": float(rospy.get_param("~finish_threshold", 0.3)),
"exceed_threshold": float(rospy.get_param("~exceed_threshold", 4.00)),
"distance_lookahead": float(rospy.get_param("~distance_lookahead", 0.6)),
+ "min_speed": float(rospy.get_param("~min_speed", 0.5)),
}
controller_type = rospy.get_param("~type", "").lower()
@@ -307,12 +310,14 @@ def get_ros_params():
override_param(params, "~pid/finish_threshold", float)
override_param(params, "~pid/exceed_threshold", float)
override_param(params, "~pid/distance_lookahead", float)
+ override_param(params, "~pid/min_speed", float)
elif controller_type == "pp":
params = {}
override_param(params, "~pp/frequency", float)
override_param(params, "~pp/finish_threshold", float)
override_param(params, "~pp/exceed_threshold", float)
override_param(params, "~pp/distance_lookahead", float)
+ override_param(params, "~pp/min_speed", float)
override_param(params, "~pp/car_length", float)
elif controller_type == "mpc":
params = {
@@ -338,6 +343,7 @@ def get_ros_params():
override_param(params, "~mpc/finish_threshold", float)
override_param(params, "~mpc/exceed_threshold", float)
override_param(params, "~mpc/distance_lookahead", float)
+ override_param(params, "~mpc/min_speed", float)
override_param(params, "~mpc/car_length", float)
else:
raise RuntimeError(
diff --git a/control/src/control/controller.py b/control/src/control/controller.py
index ba9101a63f2d079c87faae732c9214af982357b3..b5d6fe22cee6941ad91a9751e8fa86652235974d 100644
--- a/control/src/control/controller.py
+++ b/control/src/control/controller.py
@@ -27,6 +27,7 @@ class BaseController(object):
"finish_threshold",
"exceed_threshold",
"distance_lookahead",
+ "min_speed",
}
if not self.__properties == set(kwargs):
raise ValueError(
@@ -305,7 +306,7 @@ class BaseController(object):
return True
-def time_parameterize_ramp_up_ramp_down(path_xyt, speed):
+def time_parameterize_ramp_up_ramp_down(path_xyt, speed, min_speed):
"""Parameterize a geometric path of states with a desired speed.
Vehicles can't instantly reach the desired speed, so we need to ramp up to
@@ -323,6 +324,7 @@ def time_parameterize_ramp_up_ramp_down(path_xyt, speed):
# guarantee forward movement).
if path_xyt.shape[0] < 4:
speeds = speed * np.ones(path_xyt.shape[0])
+ speeds = np.maximum(speeds, min_speed)
return np.hstack([path_xyt, speeds[:, np.newaxis]])
ramp_distance = 0.5
@@ -340,4 +342,5 @@ def time_parameterize_ramp_up_ramp_down(path_xyt, speed):
[0.0, change_points[0] / len(path_xyt), change_points[1] / len(path_xyt), 1.0],
[0, speed, speed, 0],
)
+ speeds = np.maximum(speeds, min_speed)
return np.hstack([path_xyt, speeds[:, np.newaxis]])
diff --git a/control/test/controller.py b/control/test/controller.py
index c1e0a70dd3d1d4b12191598c49b8cab7fec95851..df2e27b57344000e1cf856fbd20740bbe6b6a643 100644
--- a/control/test/controller.py
+++ b/control/test/controller.py
@@ -14,6 +14,7 @@ class TestController(unittest.TestCase):
"finish_threshold": 0.2,
"exceed_threshold": 2,
"distance_lookahead": 0.8,
+ "min_speed": 0.5,
}
self.controller = BaseController(**self.defaults)
diff --git a/control/test/controller_performance.py b/control/test/controller_performance.py
index 66cbded1d086408d7c8d1dabe5a6809072bf404d..9e2901e032d5e759bb50068a9d43ffb6afa7e136 100755
--- a/control/test/controller_performance.py
+++ b/control/test/controller_performance.py
@@ -69,6 +69,7 @@ if __name__ == "__main__":
# test (defined above) in different configurations and present the correct
# facade to the test runner.
controller_type, params = get_ros_params()
+ params["min_speed"] = 0.0 # remain consistent with old behavior for tests
performance_expectations = [
("line", line(), 0.12),
diff --git a/control/test/mpc.py b/control/test/mpc.py
index be4437a83af1f0aaab2f866741e79f049c2068ce..d4732723f939ae131527e5c2995e1201a8579b91 100644
--- a/control/test/mpc.py
+++ b/control/test/mpc.py
@@ -19,6 +19,7 @@ class TestMPCController(unittest.TestCase):
"finish_threshold": 0.2,
"exceed_threshold": 4.0,
"distance_lookahead": 1.0,
+ "min_speed": 0.5,
"car_length": 0.33,
"car_width": 0.15,
"min_delta": -0.34,
diff --git a/control/test/pid.py b/control/test/pid.py
index a35ab22d34eb0bbc421bbee65984c460d59ef937..32b265f8009cf8dacfe7b448e19fbbad8db31a8c 100644
--- a/control/test/pid.py
+++ b/control/test/pid.py
@@ -14,6 +14,7 @@ class TestPIDController(unittest.TestCase):
"finish_threshold": 0.2,
"exceed_threshold": 4.0,
"distance_lookahead": 1.0,
+ "min_speed": 0.5,
"kp": 1.0,
"kd": 1.0,
}
diff --git a/control/test/purepursuit.py b/control/test/purepursuit.py
index 9ff7b99d910ecaba57c61d9b950b6c86b501be23..59d2ced452a442a28030b9332b3fe11dd399874b 100644
--- a/control/test/purepursuit.py
+++ b/control/test/purepursuit.py
@@ -14,6 +14,7 @@ class TestPPController(unittest.TestCase):
"distance_lookahead": 0.1,
"finish_threshold": 1.0,
"exceed_threshold": 4.0,
+ "min_speed": 0.5,
"car_length": 1,
}
diff --git a/cse478/src/cse478/utils.py b/cse478/src/cse478/utils.py
index 3390008daaee50ed0dafaedcca4a76b39f0f7a20..d8f6933b0dc6ee037d7992a0374c64ed1b05440b 100644
--- a/cse478/src/cse478/utils.py
+++ b/cse478/src/cse478/utils.py
@@ -267,36 +267,32 @@ def get_map(map_topic):
return map_img, map_msg.info
-def map_to_world(poses, map_info):
+def map_to_world(poses, map_info, out=None):
"""Convert an array of pixel locations in the map to poses in the world.
Args:
poses: Pixel poses in the map, converted in place. Should be nx3 np.array.
map_info: Info about the map (returned by get_map)
-
- Returns:
- Poses in the world as a np.array
+ out: Optional output buffer to store results in. Defaults to the poses array
"""
+ poses = np.atleast_2d(poses)
+ if out is None:
+ out = poses
scale = map_info.resolution
angle = quaternion_to_angle(map_info.origin.orientation)
# Rotation
- c, s = np.cos(angle), np.sin(angle)
-
- # Store the x coordinates since they will be overwritten
- temp = np.copy(poses[:, 0])
- poses[:, 0] = c * poses[:, 0] - s * poses[:, 1]
- poses[:, 1] = s * temp + c * poses[:, 1]
+ out[:, [0, 1]] = np.matmul(poses[:, [0, 1]], rotation_matrix(angle))
+ out[:, 2] = poses[:, 2] + angle
# Scale
- poses[:, :2] *= float(scale)
+ out[:, :2] *= float(scale)
# Translate
- poses[:, 0] += map_info.origin.position.x
- poses[:, 1] += map_info.origin.position.y
- poses[:, 2] += angle
+ out[:, 0] += map_info.origin.position.x
+ out[:, 1] += map_info.origin.position.y
- return poses
+ return out
def world_to_map(poses, map_info, out=None):
@@ -307,6 +303,7 @@ def world_to_map(poses, map_info, out=None):
map_info: Info about the map (returned by get_map)
out: Optional output buffer to store results in. Defaults to the poses array
"""
+ poses = np.atleast_2d(poses)
if out is None:
out = poses
scale = map_info.resolution
@@ -323,6 +320,8 @@ def world_to_map(poses, map_info, out=None):
out[:, [0, 1]] = np.matmul(out[:, [0, 1]], rotation_matrix(angle))
out[:, 2] = poses[:, 2] + angle
+ return out
+
def estimation_error(estimates, references):
"""Compute the error between the estimated and ground truth states.
diff --git a/planning/launch/make_roadmap.launch b/planning/launch/make_roadmap.launch
index 80d381db2cbc3b4f7682d9db0faccf21be29e18e..58133918855fd18fe63906e966be7f926e6e0553 100644
--- a/planning/launch/make_roadmap.launch
+++ b/planning/launch/make_roadmap.launch
@@ -10,8 +10,8 @@
<arg name="sampler" default="halton"/>
<arg name="problem" default="se2"/>
<arg name="num_vertices" default="200"/>
- <arg name="connection_radius" default="100.0"/>
- <arg name="curvature" default="0.05"/>
+ <arg name="connection_radius" default="10.0"/>
+ <arg name="curvature" default="1.0"/>
<include file="$(find cse478)/launch/teleop.launch">
<arg name="fake_localization" value="$(arg fake_localization)" />
diff --git a/planning/launch/planner_sim.launch b/planning/launch/planner_sim.launch
index 508fdf23ab35bd2f989fd975dedf8a57883ec9f3..67558dc32a4fe9169b505413aed38b105f514c56 100644
--- a/planning/launch/planner_sim.launch
+++ b/planning/launch/planner_sim.launch
@@ -10,8 +10,8 @@
<arg name="rviz" default="true" />
<arg name="num_vertices" default="200"/>
- <arg name="connection_radius" default="100.0"/>
- <arg name="curvature" default="0.05"/>
+ <arg name="connection_radius" default="10.0"/>
+ <arg name="curvature" default="1.0"/>
<include file="$(find cse478)/launch/teleop.launch">
<arg name="fake_localization" value="$(arg fake_localization)" />
diff --git a/planning/scripts/roadmap b/planning/scripts/roadmap
index d03f7b09138c30d8660245830f4654efeb50a2d1..49d31d204551b169631f49d812a9982891e3aac6 100755
--- a/planning/scripts/roadmap
+++ b/planning/scripts/roadmap
@@ -17,29 +17,28 @@ from planning.samplers import samplers
def main(args):
+ map_info = None
+ saveto = None
if args.use_ros_map:
permissible_region, map_info = utils.get_map("/static_map")
- map_name = None
+ saveto = graph_location(
+ args.problem,
+ args.sampler,
+ args.num_vertices,
+ args.connection_radius,
+ args.curvature,
+ )
elif args.text_map:
permissible_region = np.loadtxt(args.text_map, dtype=bool)
- _, fname = os.path.split(args.text_map)
- map_name, _ = os.path.splitext(fname)
else:
permissible_region = np.ones((10, 10), dtype=bool)
- map_name = "empty"
- saveto = graph_location(
- args.problem,
- args.sampler,
- args.num_vertices,
- args.connection_radius,
- args.curvature,
- map_name,
- )
if args.problem == "r2":
- problem = R2Problem(permissible_region)
+ problem = R2Problem(permissible_region, map_info=map_info)
else:
- problem = SE2Problem(permissible_region, curvature=args.curvature)
+ problem = SE2Problem(
+ permissible_region, map_info=map_info, curvature=args.curvature
+ )
sampler = samplers[args.sampler](problem.extents)
rm = Roadmap(
problem,
@@ -56,7 +55,7 @@ if __name__ == "__main__":
parser = argparse.ArgumentParser(description="generate and visualize roadmaps")
parser.add_argument("-n", "--num-vertices", type=int, default=100)
parser.add_argument("-r", "--connection-radius", type=float, default=2.0)
- parser.add_argument("-c", "--curvature", type=float, default=0.05)
+ parser.add_argument("-c", "--curvature", type=float, default=1.0)
parser.add_argument("--sampler", choices=samplers, default="halton")
parser.add_argument("--lazy", action="store_true")
parser.add_argument("--show-edges", action="store_true")
diff --git a/planning/scripts/run_search b/planning/scripts/run_search
index 31ee24ab6b78eac04763058922c9b9de9232facf..2e45e242e99a211f16a6043a56b70fdbd716bc82 100755
--- a/planning/scripts/run_search
+++ b/planning/scripts/run_search
@@ -63,7 +63,7 @@ if __name__ == "__main__":
)
parser.add_argument("-m", "--text-map", type=str, help="the environment to plan on")
parser.add_argument("-n", "--num-vertices", type=int, required=True)
- parser.add_argument("-r", "--connection-radius", type=float, default=2.0)
+ parser.add_argument("-r", "--connection-radius", type=float, default=1.0)
parser.add_argument("--sampler", type=str, default="halton", choices=samplers)
parser.add_argument("--lazy", action="store_true")
parser.add_argument("--shortcut", action="store_true")
@@ -76,7 +76,7 @@ if __name__ == "__main__":
se2_parser = subparsers.add_parser("se2", help="SE2 options")
se2_parser.add_argument("-s", "--start", nargs=3, type=int, required=True)
se2_parser.add_argument("-g", "--goal", nargs=3, type=int, required=True)
- se2_parser.add_argument("-c", "--curvature", type=float, default=0.05)
+ se2_parser.add_argument("-c", "--curvature", type=float, default=1.0)
args = parser.parse_args()
main(args)
diff --git a/planning/src/planning/dubins.py b/planning/src/planning/dubins.py
index b1dfede444023c55a32b378bcfe0a97be82a1117..7778c219b16eb86e5d95446a477de47ecdf41339 100644
--- a/planning/src/planning/dubins.py
+++ b/planning/src/planning/dubins.py
@@ -10,9 +10,10 @@ import math
import matplotlib.pyplot as plt
import numpy as np
from cse478 import utils
+import warnings
-def path_planning(start, end, curvature, resolution=0.1):
+def path_planning(start, end, curvature, resolution=0.1, interpolate_line=True):
"""Return the Dubins path between two states.
Args:
@@ -41,7 +42,7 @@ def path_planning(start, end, curvature, resolution=0.1):
end[2] -= start[2]
path, mode, length = path_planning_from_origin(
- end, curvature, resolution=resolution
+ end, curvature, resolution=resolution, interpolate_line=interpolate_line
)
# Transform back into original reference frame
inv_rot = utils.rotation_matrix(-start_orientation)
@@ -58,28 +59,35 @@ def path_planning(start, end, curvature, resolution=0.1):
return path, real_path_length
-def path_length(s, e, c):
- """Return the length of the Dubins path between two states.
+def path_length(start, end, curvature):
+ """Return the length of the Dubins path between pairs of states.
Args:
- s: see documentation for start above
- e: see documentation for end above
- c: see documentation for curvature above
+ start: start configurations shape (N, 3)
+ end: end configurations shape (N, 3)
+ curvature: shape N
Returns:
- length: length of Dubins path
+ length: lengths of Dubins paths shape N
"""
# Transform to make start of path the origin
- start = np.array(s, dtype=float)
- end = np.array(e, dtype=float)
- end[:2] -= start[:2]
- start_orientation = start[2]
+ start = np.atleast_2d(start)
+ end = np.atleast_2d(end)
+ # We'll gracefully handle mismatched argument dimensions
+ # in some situations where broadcasting semantics apply;
+ # if either start or end have a singleton dimension, they'll
+ # be broadcast to match the larger argument.
+ end_broad = np.empty(np.broadcast(start, end).shape)
+ end_broad[:, :2] = end[:, :2] - start[:, :2]
+ end_broad[:, 2] = end[:, 2]
+ start_orientation = start[:, 2]
rotation = utils.rotation_matrix(start_orientation)
- end[:2] = np.matmul(end[:2], rotation)
- end[2] -= start[2]
+ rotation = np.moveaxis(rotation, -1, 0)
+ end_broad[:, :2] = np.matmul(end_broad[:, :2][:, np.newaxis], rotation).squeeze()
+ end_broad[:, 2] -= start[:, 2]
- lengths, mode, cost = get_best_plan_from_origin(end, c)
- return cost * (1 / c)
+ _, _, cost = get_best_plan_from_origin(end_broad, curvature)
+ return cost * (1 / curvature)
##########################
@@ -87,165 +95,138 @@ def path_length(s, e, c):
##########################
-def mod2pi(theta):
- return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi)
+def mod2pi(theta, out=None):
+ if out is None:
+ return theta - 2.0 * np.pi * np.floor(theta / 2.0 / np.pi)
+ else:
+ out[:] = theta - 2.0 * np.pi * np.floor(theta / 2.0 / np.pi)
def pi_2_pi(angles):
+ """
+ Wrap angles to (-pi, pi]
+ Args:
+ angles:
+ """
angles[:] = (angles[:] - np.pi) % (2 * np.pi) + np.pi
-def lsl(alpha, beta, d):
- sa = math.sin(alpha)
- sb = math.sin(beta)
- ca = math.cos(alpha)
- cb = math.cos(beta)
- c_ab = math.cos(alpha - beta)
+planner_modes = ["LSL", "RSR", "LSR", "RSL", "RLR", "LRL"]
- tmp0 = d + sa - sb
- mode = "LSL"
+def planner(alpha, beta, d):
+ """
+ Args:
+ alpha: shape N
+ beta: shape N
+ d: shape N
+
+ Returns: np.array of path parameters with shape (N, 6, 3) where
+ the second dimension indexes the planner mode and
+ the third dimension indexes the segment parameter.
+ Turning segments are in radians and lines are in
+ distance units.
+ """
+ out = np.empty((len(alpha), 6, 3))
+ sa = np.sin(alpha)
+ sb = np.sin(beta)
+ ca = np.cos(alpha)
+ cb = np.cos(beta)
+ c_ab = np.cos(alpha - beta)
+
+ # LSL
+ tmp0 = d + sa - sb
p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sa - sb))
- if p_squared < 0:
- return None, None, None, mode
- tmp1 = math.atan2((cb - ca), tmp0)
- t = mod2pi(-alpha + tmp1)
- p = math.sqrt(p_squared)
- q = mod2pi(beta - tmp1)
-
- return t, p, q, mode
-
-
-def rsr(alpha, beta, d):
- sa = math.sin(alpha)
- sb = math.sin(beta)
- ca = math.cos(alpha)
- cb = math.cos(beta)
- c_ab = math.cos(alpha - beta)
+ tmp1 = np.arctan2((cb - ca), tmp0)
+ out[:, 0, 0] = -alpha + tmp1
+ out[:, 0, 1] = np.sqrt(p_squared)
+ out[:, 0, 2] = beta - tmp1
+ # RSR
tmp0 = d - sa + sb
- mode = "RSR"
p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sb - sa))
- if p_squared < 0:
- return None, None, None, mode
- tmp1 = math.atan2((ca - cb), tmp0)
- t = mod2pi(alpha - tmp1)
- p = math.sqrt(p_squared)
- q = mod2pi(-beta + tmp1)
-
- return t, p, q, mode
-
-
-def lsr(alpha, beta, d):
- sa = math.sin(alpha)
- sb = math.sin(beta)
- ca = math.cos(alpha)
- cb = math.cos(beta)
- c_ab = math.cos(alpha - beta)
+ tmp1 = np.arctan2((ca - cb), tmp0)
+ out[:, 1, 0] = alpha - tmp1
+ out[:, 1, 1] = np.sqrt(p_squared)
+ out[:, 1, 2] = -beta + tmp1
+ # LSR
p_squared = -2 + (d * d) + (2 * c_ab) + (2 * d * (sa + sb))
- mode = "LSR"
- if p_squared < 0:
- return None, None, None, mode
- p = math.sqrt(p_squared)
- tmp2 = math.atan2((-ca - cb), (d + sa + sb)) - math.atan2(-2.0, p)
- t = mod2pi(-alpha + tmp2)
- q = mod2pi(-mod2pi(beta) + tmp2)
-
- return t, p, q, mode
-
-
-def rsl(alpha, beta, d):
- sa = math.sin(alpha)
- sb = math.sin(beta)
- ca = math.cos(alpha)
- cb = math.cos(beta)
- c_ab = math.cos(alpha - beta)
+ out[:, 2, 1] = np.sqrt(p_squared)
+ p = out[:, 2, 1]
+ tmp2 = np.arctan2((-ca - cb), (d + sa + sb)) - np.arctan2(-2.0, p)
+ out[:, 2, 0] = -alpha + tmp2
+ out[:, 2, 2] = -mod2pi(beta) + tmp2
+ # RSL
p_squared = (d * d) - 2 + (2 * c_ab) - (2 * d * (sa + sb))
- mode = "RSL"
- if p_squared < 0:
- return None, None, None, mode
- p = math.sqrt(p_squared)
- tmp2 = math.atan2((ca + cb), (d - sa - sb)) - math.atan2(2.0, p)
- t = mod2pi(alpha - tmp2)
- q = mod2pi(beta - tmp2)
-
- return t, p, q, mode
+ out[:, 3, 1] = np.sqrt(p_squared)
+ p = out[:, 3, 1]
+ tmp2 = np.arctan2((ca + cb), (d - sa - sb)) - np.arctan2(2.0, p)
+ out[:, 3, 0] = alpha - tmp2
+ out[:, 3, 2] = beta - tmp2
-
-def rlr(alpha, beta, d):
- sa = math.sin(alpha)
- sb = math.sin(beta)
- ca = math.cos(alpha)
- cb = math.cos(beta)
- c_ab = math.cos(alpha - beta)
-
- mode = "RLR"
+ # RLR
tmp_rlr = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (sa - sb)) / 8.0
- if abs(tmp_rlr) > 1.0:
- return None, None, None, mode
-
- p = mod2pi(2 * math.pi - math.acos(tmp_rlr))
- t = mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + mod2pi(p / 2.0))
- q = mod2pi(alpha - beta - t + mod2pi(p))
- return t, p, q, mode
-
+ out[:, 4, 1] = mod2pi(2 * np.pi - np.arccos(tmp_rlr))
+ p = out[:, 4, 1]
+ out[:, 4, 0] = alpha - np.arctan2(ca - cb, d - sa + sb) + mod2pi(p / 2.0)
+ rlr_t = out[:, 4, 0]
+ out[:, 4, 2] = alpha - beta - rlr_t + mod2pi(p)
-def lrl(alpha, beta, d):
- sa = math.sin(alpha)
- sb = math.sin(beta)
- ca = math.cos(alpha)
- cb = math.cos(beta)
- c_ab = math.cos(alpha - beta)
-
- mode = "LRL"
+ # LRL
tmp_lrl = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (-sa + sb)) / 8.0
- if abs(tmp_lrl) > 1:
- return None, None, None, mode
- p = mod2pi(2 * math.pi - math.acos(tmp_lrl))
- t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.0)
- q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p))
+ out[:, 5, 1] = mod2pi(2 * np.pi - np.arccos(tmp_lrl))
+ p = out[:, 5, 1]
+ out[:, 5, 0] = -alpha - np.arctan2(ca - cb, d + sa - sb) + p / 2.0
+ lrl_t = out[:, 5, 0]
+ out[:, 5, 2] = mod2pi(beta) - alpha - lrl_t + mod2pi(p)
- return t, p, q, mode
+ mod2pi(out[:, :, 0], out=out[:, :, 0])
+ mod2pi(out[:, :, 2], out=out[:, :, 2])
+ return out
def get_best_plan_from_origin(goal, curvature):
# nomalize
- d = math.sqrt(goal[0] ** 2.0 + goal[1] ** 2.0)
+ d = np.sqrt(goal[:, 0] ** 2.0 + goal[:, 1] ** 2.0)
d = d * curvature
# print(dx, dy, D, d)
- theta = mod2pi(math.atan2(goal[1], goal[0]))
+ theta = mod2pi(np.arctan2(goal[:, 1], goal[:, 0]))
alpha = mod2pi(-theta)
- beta = mod2pi(goal[2] - theta)
+ beta = mod2pi(goal[:, 2] - theta)
# print(theta, alpha, beta, d)
- planners = [lsl, rsr, lsr, rsl, rlr, lrl]
-
- bcost = float("inf")
- bt, bp, bq, bmode = None, None, None, None
-
- for planner in planners:
- t, p, q, mode = planner(alpha, beta, d)
- if t is None:
- continue
-
- cost = abs(t) + abs(p) + abs(q)
- if bcost > cost:
- bt, bp, bq, bmode = t, p, q, mode
- bcost = cost
-
- return [bt, bp, bq], bmode, bcost
-
-
-def path_planning_from_origin(goal, curvature, resolution=0.1):
+ # Some configurations can't be connected with some (or all)
+ # Dubins path configurations. This will manifest as domain errors
+ # in calls to trig functions. Numpy gracefully propogates NaNs
+ # and we will hide the warning because the code can handle them
+ # just fine.
+ with warnings.catch_warnings():
+ warnings.simplefilter("ignore")
+ all_plans = planner(alpha, beta, d)
+ all_i = np.arange(len(all_plans))
+ cost = np.abs(all_plans).sum(-1)
+ best_i = np.nanargmin(cost, axis=1)
+ best_plan = all_plans[all_i, best_i]
+ best_cost = cost[all_i, best_i]
+ return best_plan, list(map(lambda x: planner_modes[x], best_i)), best_cost
+
+
+def path_planning_from_origin(goal, curvature, resolution=0.1, interpolate_line=False):
if np.all(np.array(goal) == 0.0):
return np.zeros((1, 3)), [], 0.0
- lengths, modes, cost = get_best_plan_from_origin(goal, curvature)
+ lengths, modes, cost = get_best_plan_from_origin(np.atleast_2d(goal), curvature)
- path = generate_course(lengths, modes, curvature, step_size=resolution)
+ path = generate_course(
+ lengths.squeeze(),
+ modes,
+ curvature,
+ step_size=resolution,
+ interpolate_line=interpolate_line,
+ )
return path, modes, cost
@@ -273,14 +254,17 @@ def turn(radius, angle, waypoint_sep=0.1):
return turn_poses
-def generate_course(lengths, mode, curvature, step_size=0.1):
+def generate_course(lengths, mode, curvature, step_size=0.1, interpolate_line=False):
segments = []
- for m, length in zip(mode, lengths):
+ for m, length in zip(mode[0], lengths):
# 0 length segments are just a noop
if length == 0.0:
continue
if m == "S":
- segments.append(line(length / curvature, step_size))
+ if interpolate_line:
+ segments.append(line(length / curvature, step_size))
+ else:
+ segments.append(np.array([[0, 0, 0], [length / curvature, 0, 0]]))
elif m == "L":
segments.append(turn(1 / curvature, length, step_size))
elif m == "R":
diff --git a/planning/src/planning/planner_ros.py b/planning/src/planning/planner_ros.py
index 3b005567b10ff2ee2fea016a64447f34481eaec3..0ab49dea09f51f7f64e63cc3a1dc2a06cc099430 100644
--- a/planning/src/planning/planner_ros.py
+++ b/planning/src/planning/planner_ros.py
@@ -55,7 +55,10 @@ class PlannerROS:
self.permissible_region, self.map_info = utils.get_map("/static_map")
self.problem = SE2Problem(
- self.permissible_region, check_resolution=0.1, curvature=curvature
+ self.permissible_region,
+ map_info=self.map_info,
+ check_resolution=0.1,
+ curvature=curvature,
)
self.rm = None
@@ -87,7 +90,7 @@ class PlannerROS:
)
load_time = time.time() - start_stamp
rospy.loginfo("Roadmap constructed in {:2.2f}s".format(load_time))
- self.visualize()
+ rospy.Timer(rospy.Duration(1), lambda x: self.visualize(), oneshot=True)
# self.rm.visualize(saveto="graph.png")
def plan_to_goal(self, start, goal):
@@ -105,12 +108,14 @@ class PlannerROS:
# Call the Lazy A* planner
try:
rospy.loginfo("Planning...")
+ start_edges_evaluated = self.rm.edges_evaluated
start_time = time.time()
path, _ = search.astar(self.rm, start_id, goal_id)
end_time = time.time()
+ edges_evaluated = self.rm.edges_evaluated - start_edges_evaluated
rospy.loginfo("Path length: {}".format(self.rm.compute_path_length(path)))
rospy.loginfo("Planning time: {}".format(end_time - start_time))
- rospy.loginfo("Edges evaluated: {}".format(self.rm.edges_evaluated))
+ rospy.loginfo("Edges evaluated: {}".format(edges_evaluated))
# self.rm.visualize(vpath=path, saveto="planned_path.png")
except nx.NetworkXNoPath:
rospy.loginfo("Failed to find a plan")
@@ -128,15 +133,8 @@ class PlannerROS:
rospy.loginfo("Shortcut time: {}".format(end_time - start_time))
# self.rm.visualize(vpath=path, saveto="shortcut_path.png")
- states = []
- for i in range(1, len(path)):
- u, v = path[i - 1 : i + 1]
- q1 = self.rm.vertices[u, :]
- q2 = self.rm.vertices[v, :]
- edge, _ = self.rm.problem.steer(q1, q2)
- states.append(edge)
- states = np.vstack(states)
- return utils.map_to_world(states, self.map_info)
+ # Return interpolated path
+ return self.rm.compute_qpath(path)
def get_goal(self, msg):
@@ -144,13 +142,8 @@ class PlannerROS:
if self.rm is None:
return False
- self.goal = np.array([utils.pose_to_particle(msg.pose)])
- utils.world_to_map(self.goal, self.map_info)
- self.goal = self.goal.squeeze()
-
- start = self._get_car_pose()[np.newaxis, :]
- utils.world_to_map(start, self.map_info)
- start = start.squeeze()
+ self.goal = np.array(utils.pose_to_particle(msg.pose))
+ start = self._get_car_pose()
path_states = self.plan_to_goal(start, self.goal)
if path_states is None:
@@ -198,36 +191,56 @@ class PlannerROS:
def visualize(self):
"""Visualize the nodes and edges of the roadmap."""
vertices = self.rm.vertices.copy()
- utils.map_to_world(vertices, self.map_info)
poses = list(map(utils.particle_to_pose, vertices))
msg = PoseArray(header=Header(frame_id="map"), poses=poses)
self.nodes_viz.publish(msg)
- return
- all_edges = []
- edges = list(self.rm.graph.edges())
- for u, v in edges:
- if not self.rm.check_edge_validity(u, v):
- continue
- q1 = self.rm.vertices[u, :]
- q2 = self.rm.vertices[v, :]
- edge, _ = self.problem.steer(q1, q2)
- utils.map_to_world(edge, self.map_info)
- with_repeats = np.repeat(edge[:, :2], 2, 0).reshape(-1, 2)[1:-1]
- all_edges.append(with_repeats)
- all_edges = np.vstack(all_edges)
- points = list(map(lambda x: Point(x=x[0], y=x[1]), all_edges))
- msg = Marker(
- header=Header(frame_id="map"), type=Marker.LINE_LIST, points=points
+ # There are lots of edges, so we'll shuffle and incrementally visualize
+ # them. This is more work overall, but gives more immediate feedback
+ # about the graph.
+ all_edges = np.empty((0, 2), dtype=int)
+ edges = np.array(self.rm.graph.edges(), dtype=int)
+ np.random.shuffle(edges)
+ split_indices = np.array(
+ [500, 1000, 2000, 5000]
+ + [10000, 20000, 50000]
+ + list(range(100000, edges.shape[0], 100000)),
+ dtype=int,
)
- msg.scale.x = 0.01
- msg.pose.orientation.w = 1.0
- msg.color.a = 0.1
- self.edges_viz.publish(msg)
+ for batch in np.split(edges, split_indices, axis=0):
+ batch_edges = []
+ for u, v in batch:
+ q1 = self.rm.vertices[u, :]
+ q2 = self.rm.vertices[v, :]
+ # Check edge validity on the problem rather than roadmap to
+ # circumvent edge collision-checking count
+ if not self.rm.problem.check_edge_validity(q1, q2):
+ continue
+ edge, _ = self.problem.steer(
+ q1, q2, resolution=0.25, interpolate_line=False
+ )
+ with_repeats = np.repeat(edge[:, :2], 2, 0).reshape(-1, 2)[1:-1]
+ batch_edges.append(with_repeats)
+ if not batch_edges:
+ continue
+ batch_edges = np.vstack(batch_edges)
+ all_edges = np.vstack((all_edges, batch_edges))
+ points = list(map(lambda x: Point(x=x[0], y=x[1], z=-1), all_edges))
+ msg = Marker(
+ header=Header(frame_id="map"), type=Marker.LINE_LIST, points=points
+ )
+ msg.scale.x = 0.01
+ msg.pose.orientation.w = 1.0
+ msg.color.a = 0.1
+ self.edges_viz.publish(msg)
def mkdir_p(path):
- """Equivalent to mkdir -p path."""
+ """Equivalent to mkdir -p path.
+
+ The exist_ok flag for os.makedirs was introduced in Python 3.2.
+ This function provides Python 2 support.
+ """
try:
os.makedirs(path)
except OSError as exc: # Python ≥ 2.5
@@ -247,13 +260,12 @@ def graph_location(
map_name=None,
):
"""Return the name of this graph in the cache."""
- param_string = "_{}_{}_{}_{}".format(
- problem_name, sampler_name, num_vertices, connection_radius
- )
- if problem_name == "se2":
- param_string += "_{}".format(curvature)
cache_dir = os.path.expanduser("~/.ros/graph_cache/")
mkdir_p(cache_dir)
if map_name is None:
- map_name = os.path.basename(rospy.get_param("/map_file")).split(".")[0]
- return os.path.join(cache_dir, map_name + param_string + ".pkl")
+ map_name, _ = os.path.splitext(os.path.basename(rospy.get_param("/map_file")))
+ params = [map_name, problem_name, sampler_name, num_vertices, connection_radius]
+ if problem_name == "se2":
+ params.append(curvature)
+ name = "_".join(str(p) for p in params)
+ return os.path.join(cache_dir, name + ".pkl")
diff --git a/planning/src/planning/problems.py b/planning/src/planning/problems.py
index cfe0a13c912e73d15517cf75eb8764092b9fcc39..30e840a84837ac440329397da29ed7558c2b8bdc 100644
--- a/planning/src/planning/problems.py
+++ b/planning/src/planning/problems.py
@@ -1,24 +1,33 @@
import numpy as np
+from cse478 import utils
from planning import dubins
class PlanarProblem(object):
- def __init__(self, permissible_region, check_resolution=0.1):
+ def __init__(self, permissible_region, map_info=None, check_resolution=0.1):
"""Construct a planar planning problem.
Args:
permissible_region: Boolean np.array with shape map height x map width,
where one indicates that the location is permissible
+ map_info: map information, returned by get_map
check_resolution: collision-checking resolution
"""
self.permissible_region = permissible_region
+ self.map_info = map_info
self.check_resolution = check_resolution
- self.height, self.width = permissible_region.shape
- self.extents = np.zeros((2, 2))
- self.extents[0, 1] = self.width
- self.extents[1, 1] = self.height
+ height, width = permissible_region.shape
+ self.extents = np.zeros((3, 2))
+ self.extents[0, 1] = width
+ self.extents[1, 1] = height
+
+ if map_info is not None:
+ map_angle = utils.quaternion_to_angle(map_info.origin.orientation)
+ assert map_angle == 0
+ utils.map_to_world(self.extents.T, map_info)
+ self.extents = self.extents[:2, :]
def check_state_validity(self, states):
"""Return whether states are valid.
@@ -40,18 +49,28 @@ class PlanarProblem(object):
"*** REPLACE THIS LINE ***"
# END QUESTION 1.2
+ # The units of the state are meters and radians. We need to convert the
+ # meters to pixels, in order to index into the permissible region. This
+ # function converts them in place.
+ if self.map_info is not None:
+ utils.world_to_map(states, self.map_info)
+
# For states within the extents of the map, collision check by reading
# the corresponding entry of self.permissible_region. For simplicity,
# we'll assume the robot is a point robot: just index directly with the
- # robot state x and y indices into self.permissible_region.
+ # robot state x and y pixel indices into self.permissible_region.
#
- # Hint: use the `astype` method to cast the x and y positions into
+ # Hint: use the `astype` method to cast the x and y pixel positions into
# integers. Then, index into self.permissible_region, remembering that
# the zeroth dimension is the height.
# BEGIN QUESTION 1.2
"*** REPLACE THIS LINE ***"
# END QUESTION 1.2
+ # Convert the units back from pixels to meters for the caller
+ if self.map_info is not None:
+ utils.map_to_world(states, self.map_info)
+
return valid
def check_edge_validity(self, q1, q2):
@@ -91,7 +110,7 @@ class PlanarProblem(object):
heuristic_cost[i] = length
return heuristic_cost
- def steer(self, q1, q2):
+ def steer(self, q1, q2, **kwargs):
"""Return a local path connecting two states.
Intermediate states are used for edge collision-checking.
@@ -118,31 +137,38 @@ class R2Problem(PlanarProblem):
"""
return np.linalg.norm(np.atleast_2d(q2) - np.atleast_2d(q1), axis=1)
- def steer(self, q1, q2):
+ def steer(self, q1, q2, resolution=None, interpolate_line=True):
"""Return a straight-line path connecting two R^2 states.
Args:
q1, q2: np.arrays with shape 2
+ resolution: the space between waypoints in the resulting path
+ interpolate_line: whether to provide fine waypoint discretization
+ for line segments
Returns:
path: sequence of states between q1 and q2
length: length of local path
"""
+ if resolution is None:
+ resolution = self.check_resolution
q1 = q1.reshape((1, -1))
q2 = q2.reshape((1, -1))
dist = np.linalg.norm(q2 - q1)
- if dist < self.check_resolution:
+ if not interpolate_line or dist < resolution:
return np.vstack((q1, q2)), dist
q1_toward_q2 = (q2 - q1) / dist
- steps = np.hstack(
- (np.arange(0, dist, self.check_resolution), np.array([dist]))
- ).reshape((-1, 1))
+ steps = np.hstack((np.arange(0, dist, resolution), np.array([dist]))).reshape(
+ (-1, 1)
+ )
return q1 + q1_toward_q2 * steps, dist
class SE2Problem(PlanarProblem):
- def __init__(self, permissible_region, check_resolution=0.01, curvature=3):
- super(SE2Problem, self).__init__(permissible_region, check_resolution)
+ def __init__(
+ self, permissible_region, map_info=None, check_resolution=0.01, curvature=1.0
+ ):
+ super(SE2Problem, self).__init__(permissible_region, map_info, check_resolution)
self.curvature = curvature
self.extents = np.vstack((self.extents, np.array([[-np.pi, np.pi]])))
@@ -150,34 +176,37 @@ class SE2Problem(PlanarProblem):
"""Compute the length of the Dubins path between two SE(2) states.
Args:
- q1, q2: np.arrays with shape 3
+ q1, q2: np.arrays with shape (N, 3)
Returns:
heuristic: cost estimate between two states
"""
start, end = np.atleast_2d(q1), np.atleast_2d(q2)
- start_ind = np.arange(start.shape[0])
- end_ind = np.arange(end.shape[0])
- broad = np.broadcast(start_ind, end_ind)
- num_pairs = broad.size
- heuristic_cost = np.empty((num_pairs))
- for i, (start_i, end_i) in enumerate(zip(*broad.iters)):
- heuristic_cost[i] = dubins.path_length(
- start[start_i], end[end_i], self.curvature
- )
+ # This function will handle broadcasting start and end,
+ # if they're compatible
+ heuristic_cost = dubins.path_length(start, end, self.curvature)
return heuristic_cost
- def steer(self, q1, q2):
+ def steer(self, q1, q2, resolution=None, interpolate_line=True):
"""Return a Dubins path connecting two SE(2) states.
Args:
q1, q2: np.arrays with shape 3
+ resolution: the space between waypoints in the resulting path
+ interpolate_line: whether to provide fine waypoint discretization
+ for line segments
Returns:
path: sequence of states on Dubins path between q1 and q2
length: length of local path
"""
+ if resolution is None:
+ resolution = self.check_resolution
path, length = dubins.path_planning(
- q1, q2, self.curvature, resolution=self.check_resolution
+ q1,
+ q2,
+ self.curvature,
+ resolution=resolution,
+ interpolate_line=interpolate_line,
)
return path, length
diff --git a/planning/src/planning/roadmap.py b/planning/src/planning/roadmap.py
index 2bf45de7a756b599754d9c9e329e5ad6f88a4ab4..fc988c8d6a62ac428ae02cdd010007ad215beaf2 100644
--- a/planning/src/planning/roadmap.py
+++ b/planning/src/planning/roadmap.py
@@ -1,6 +1,8 @@
from __future__ import division
import os
+
+import matplotlib.collections
import networkx as nx
import numpy as np
import matplotlib.pyplot as plt
@@ -60,7 +62,7 @@ class Roadmap(object):
return self.problem.compute_heuristic(
self.vertices[n1, :],
self.vertices[n2, :],
- )
+ ).item()
def check_edge_validity(self, n1, n2):
"""Collision check the edge between two nodes in the roadmap.
@@ -89,6 +91,9 @@ class Roadmap(object):
weighted_edges: a subset of the original weighted_edges, where only
rows that correspond to collision-free edges are preserved
"""
+ if weighted_edges.shape[0] == 0:
+ return weighted_edges
+
# uv contains only the node labels for each edge (source, target).
uv = weighted_edges[:, :2].astype(int)
@@ -191,7 +196,8 @@ class Roadmap(object):
# This adaptively changes the sample batch size based on how hard it
# is to get collision-free vertices.
est_validity = valid_samples / total_samples
- batch_size = int((self.num_vertices - valid_samples) / est_validity) + 1
+ if valid_samples > 0:
+ batch_size = int((self.num_vertices - valid_samples) / est_validity) + 1
configs = np.vstack(configs)
assert configs.shape[0] >= self.num_vertices
return configs[: self.num_vertices, :]
@@ -233,7 +239,8 @@ class Roadmap(object):
Returns:
new_index: the integer label for the added state
"""
- assert len(state.shape) == 1
+ state = state.astype(float)
+ assert state.ndim == 1
valid = self.problem.check_state_validity(state.reshape((1, -1))).item()
if not valid:
raise ValueError("state {} is invalid".format(state))
@@ -246,7 +253,6 @@ class Roadmap(object):
if is_start:
self.start = index
h = self.problem.compute_heuristic(state, self.vertices)
-
else:
self.goal = index
h = self.problem.compute_heuristic(self.vertices, state)
@@ -279,11 +285,8 @@ class Roadmap(object):
Returns:
length: path length
"""
- total = 0
- for i in range(1, len(vpath)):
- u, v = vpath[i - 1 : i + 1]
- total += self.heuristic(u, v)
- return total
+ q = self.vertices[vpath, :]
+ return self.problem.compute_heuristic(q[:-1, :], q[1:, :]).sum()
def compute_qpath(self, vpath):
"""Compute a sequence of states from a sequence of vertices.
@@ -323,11 +326,18 @@ class Roadmap(object):
)
if show_edges:
+ edges = []
for u, v in self.graph.edges():
q1 = self.vertices[u, :]
q2 = self.vertices[v, :]
- edge, _ = self.problem.steer(q1, q2)
- plt.plot(edge[:, 0], edge[:, 1], c="#dddddd", zorder=1)
+ edge, _ = self.problem.steer(
+ q1, q2, resolution=0.1, interpolate_line=False
+ )
+ edges.append(edge[:, :2])
+ edges = matplotlib.collections.LineCollection(
+ edges, colors="#dddddd", zorder=1
+ )
+ plt.gca().add_collection(edges)
if vpath is not None:
qpath = self.compute_qpath(vpath)
@@ -348,8 +358,8 @@ class Roadmap(object):
c="r",
zorder=3,
)
- plt.xlim((0, self.problem.extents[0, 1]))
- plt.ylim((0, self.problem.extents[1, 1]))
+ plt.xlim(self.problem.extents[0, :])
+ plt.ylim(self.problem.extents[1, :])
if saveto is not None:
plt.savefig(saveto, bbox_inches="tight")
diff --git a/planning/test/roadmap.py b/planning/test/roadmap.py
index 6b1facc79c6104758bb54efd83f5c19649a730a4..d330c7bfba40a8273de24d5b350a56bd26e681c2 100644
--- a/planning/test/roadmap.py
+++ b/planning/test/roadmap.py
@@ -25,6 +25,12 @@ class TestRoadmap(unittest.TestCase):
counter[v] += 1
return counter
+ def test_r2problem_roadmap_disconnected(self):
+ num_vertices = 9
+ connection_radius = 0.1
+ lazy = False
+ Roadmap(self.problem, self.sampler, num_vertices, connection_radius, lazy)
+
def test_r2problem_roadmap_edge_collisions(self):
# This lattice roadmap has vertices for x and y = (1.67, 5.0, 8.33).
# Setting the connection radius to 15 creates a fully-connected graph.