Commit ceaf63a2 authored by Brian Hou's avatar Brian Hou
Browse files

Update Project 4: Planning.

Vectorize Dubins path computation. This is orders of magnitudes faster than the
previous implementation, so graph caching is now less necessary. As a result,
only graphs for ROS maps are cached; the other test maps are too small to merit
the hassle.

Fix a serious issue with PlanarProblems. Distances were previously measured in
pixels rather than meters. As a result, the connection radius and curvature
parameters didn't have a clear physical interpretation, and would differ based
on the resolution of the map (the conversion factor between meters and pixels).

Add a more efficient edge visualization to RViz, which incrementally sends edges
to be visualized.

Fix minor bugs with zero edges and zero valid samples.

Set a minimum controller speed to avoid issues with reference velocities of
zero. This would occur at the end of a path.
parent 1025d23a
Pipeline #425031 failed with stage
in 14 minutes and 34 seconds
......@@ -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(
......
......@@ -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]])
......@@ -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)
......
......@@ -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),
......
......@@ -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,
......
......@@ -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,
}
......
......@@ -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,
}
......
......@@ -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.
......
......@@ -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)" />
......
......@@ -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)" />
......
......@@ -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
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,
)
elif args.text_map:
permissible_region = np.loadtxt(args.text_map, dtype=bool)
else:
permissible_region = np.ones((10, 10), dtype=bool)
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")
......
......@@ -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)
......@@ -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
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)
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)
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