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: ...@@ -188,7 +188,9 @@ class ControlROS:
""" """
if not self.controller.is_alive(): if not self.controller.is_alive():
raise RuntimeError("Path command set before controller was started") 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) self.follow_path(path_xytv)
def follow_path(self, path_xytv): def follow_path(self, path_xytv):
...@@ -294,6 +296,7 @@ def get_ros_params(): ...@@ -294,6 +296,7 @@ def get_ros_params():
"finish_threshold": float(rospy.get_param("~finish_threshold", 0.3)), "finish_threshold": float(rospy.get_param("~finish_threshold", 0.3)),
"exceed_threshold": float(rospy.get_param("~exceed_threshold", 4.00)), "exceed_threshold": float(rospy.get_param("~exceed_threshold", 4.00)),
"distance_lookahead": float(rospy.get_param("~distance_lookahead", 0.6)), "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() controller_type = rospy.get_param("~type", "").lower()
...@@ -307,12 +310,14 @@ def get_ros_params(): ...@@ -307,12 +310,14 @@ def get_ros_params():
override_param(params, "~pid/finish_threshold", float) override_param(params, "~pid/finish_threshold", float)
override_param(params, "~pid/exceed_threshold", float) override_param(params, "~pid/exceed_threshold", float)
override_param(params, "~pid/distance_lookahead", float) override_param(params, "~pid/distance_lookahead", float)
override_param(params, "~pid/min_speed", float)
elif controller_type == "pp": elif controller_type == "pp":
params = {} params = {}
override_param(params, "~pp/frequency", float) override_param(params, "~pp/frequency", float)
override_param(params, "~pp/finish_threshold", float) override_param(params, "~pp/finish_threshold", float)
override_param(params, "~pp/exceed_threshold", float) override_param(params, "~pp/exceed_threshold", float)
override_param(params, "~pp/distance_lookahead", float) override_param(params, "~pp/distance_lookahead", float)
override_param(params, "~pp/min_speed", float)
override_param(params, "~pp/car_length", float) override_param(params, "~pp/car_length", float)
elif controller_type == "mpc": elif controller_type == "mpc":
params = { params = {
...@@ -338,6 +343,7 @@ def get_ros_params(): ...@@ -338,6 +343,7 @@ def get_ros_params():
override_param(params, "~mpc/finish_threshold", float) override_param(params, "~mpc/finish_threshold", float)
override_param(params, "~mpc/exceed_threshold", float) override_param(params, "~mpc/exceed_threshold", float)
override_param(params, "~mpc/distance_lookahead", float) override_param(params, "~mpc/distance_lookahead", float)
override_param(params, "~mpc/min_speed", float)
override_param(params, "~mpc/car_length", float) override_param(params, "~mpc/car_length", float)
else: else:
raise RuntimeError( raise RuntimeError(
......
...@@ -27,6 +27,7 @@ class BaseController(object): ...@@ -27,6 +27,7 @@ class BaseController(object):
"finish_threshold", "finish_threshold",
"exceed_threshold", "exceed_threshold",
"distance_lookahead", "distance_lookahead",
"min_speed",
} }
if not self.__properties == set(kwargs): if not self.__properties == set(kwargs):
raise ValueError( raise ValueError(
...@@ -305,7 +306,7 @@ class BaseController(object): ...@@ -305,7 +306,7 @@ class BaseController(object):
return True 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. """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 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): ...@@ -323,6 +324,7 @@ def time_parameterize_ramp_up_ramp_down(path_xyt, speed):
# guarantee forward movement). # guarantee forward movement).
if path_xyt.shape[0] < 4: if path_xyt.shape[0] < 4:
speeds = speed * np.ones(path_xyt.shape[0]) speeds = speed * np.ones(path_xyt.shape[0])
speeds = np.maximum(speeds, min_speed)
return np.hstack([path_xyt, speeds[:, np.newaxis]]) return np.hstack([path_xyt, speeds[:, np.newaxis]])
ramp_distance = 0.5 ramp_distance = 0.5
...@@ -340,4 +342,5 @@ def time_parameterize_ramp_up_ramp_down(path_xyt, speed): ...@@ -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.0, change_points[0] / len(path_xyt), change_points[1] / len(path_xyt), 1.0],
[0, speed, speed, 0], [0, speed, speed, 0],
) )
speeds = np.maximum(speeds, min_speed)
return np.hstack([path_xyt, speeds[:, np.newaxis]]) return np.hstack([path_xyt, speeds[:, np.newaxis]])
...@@ -14,6 +14,7 @@ class TestController(unittest.TestCase): ...@@ -14,6 +14,7 @@ class TestController(unittest.TestCase):
"finish_threshold": 0.2, "finish_threshold": 0.2,
"exceed_threshold": 2, "exceed_threshold": 2,
"distance_lookahead": 0.8, "distance_lookahead": 0.8,
"min_speed": 0.5,
} }
self.controller = BaseController(**self.defaults) self.controller = BaseController(**self.defaults)
......
...@@ -69,6 +69,7 @@ if __name__ == "__main__": ...@@ -69,6 +69,7 @@ if __name__ == "__main__":
# test (defined above) in different configurations and present the correct # test (defined above) in different configurations and present the correct
# facade to the test runner. # facade to the test runner.
controller_type, params = get_ros_params() controller_type, params = get_ros_params()
params["min_speed"] = 0.0 # remain consistent with old behavior for tests
performance_expectations = [ performance_expectations = [
("line", line(), 0.12), ("line", line(), 0.12),
......
...@@ -19,6 +19,7 @@ class TestMPCController(unittest.TestCase): ...@@ -19,6 +19,7 @@ class TestMPCController(unittest.TestCase):
"finish_threshold": 0.2, "finish_threshold": 0.2,
"exceed_threshold": 4.0, "exceed_threshold": 4.0,
"distance_lookahead": 1.0, "distance_lookahead": 1.0,
"min_speed": 0.5,
"car_length": 0.33, "car_length": 0.33,
"car_width": 0.15, "car_width": 0.15,
"min_delta": -0.34, "min_delta": -0.34,
......
...@@ -14,6 +14,7 @@ class TestPIDController(unittest.TestCase): ...@@ -14,6 +14,7 @@ class TestPIDController(unittest.TestCase):
"finish_threshold": 0.2, "finish_threshold": 0.2,
"exceed_threshold": 4.0, "exceed_threshold": 4.0,
"distance_lookahead": 1.0, "distance_lookahead": 1.0,
"min_speed": 0.5,
"kp": 1.0, "kp": 1.0,
"kd": 1.0, "kd": 1.0,
} }
......
...@@ -14,6 +14,7 @@ class TestPPController(unittest.TestCase): ...@@ -14,6 +14,7 @@ class TestPPController(unittest.TestCase):
"distance_lookahead": 0.1, "distance_lookahead": 0.1,
"finish_threshold": 1.0, "finish_threshold": 1.0,
"exceed_threshold": 4.0, "exceed_threshold": 4.0,
"min_speed": 0.5,
"car_length": 1, "car_length": 1,
} }
......
...@@ -267,36 +267,32 @@ def get_map(map_topic): ...@@ -267,36 +267,32 @@ def get_map(map_topic):
return map_img, map_msg.info 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. """Convert an array of pixel locations in the map to poses in the world.
Args: Args:
poses: Pixel poses in the map, converted in place. Should be nx3 np.array. poses: Pixel poses in the map, converted in place. Should be nx3 np.array.
map_info: Info about the map (returned by get_map) map_info: Info about the map (returned by get_map)
out: Optional output buffer to store results in. Defaults to the poses array
Returns:
Poses in the world as a np.array
""" """
poses = np.atleast_2d(poses)
if out is None:
out = poses
scale = map_info.resolution scale = map_info.resolution
angle = quaternion_to_angle(map_info.origin.orientation) angle = quaternion_to_angle(map_info.origin.orientation)
# Rotation # Rotation
c, s = np.cos(angle), np.sin(angle) out[:, [0, 1]] = np.matmul(poses[:, [0, 1]], rotation_matrix(angle))
out[:, 2] = poses[:, 2] + 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]
# Scale # Scale
poses[:, :2] *= float(scale) out[:, :2] *= float(scale)
# Translate # Translate
poses[:, 0] += map_info.origin.position.x out[:, 0] += map_info.origin.position.x
poses[:, 1] += map_info.origin.position.y out[:, 1] += map_info.origin.position.y
poses[:, 2] += angle
return poses return out
def world_to_map(poses, map_info, out=None): def world_to_map(poses, map_info, out=None):
...@@ -307,6 +303,7 @@ 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) map_info: Info about the map (returned by get_map)
out: Optional output buffer to store results in. Defaults to the poses array out: Optional output buffer to store results in. Defaults to the poses array
""" """
poses = np.atleast_2d(poses)
if out is None: if out is None:
out = poses out = poses
scale = map_info.resolution scale = map_info.resolution
...@@ -323,6 +320,8 @@ def world_to_map(poses, map_info, out=None): ...@@ -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[:, [0, 1]] = np.matmul(out[:, [0, 1]], rotation_matrix(angle))
out[:, 2] = poses[:, 2] + angle out[:, 2] = poses[:, 2] + angle
return out
def estimation_error(estimates, references): def estimation_error(estimates, references):
"""Compute the error between the estimated and ground truth states. """Compute the error between the estimated and ground truth states.
......
...@@ -10,8 +10,8 @@ ...@@ -10,8 +10,8 @@
<arg name="sampler" default="halton"/> <arg name="sampler" default="halton"/>
<arg name="problem" default="se2"/> <arg name="problem" default="se2"/>
<arg name="num_vertices" default="200"/> <arg name="num_vertices" default="200"/>
<arg name="connection_radius" default="100.0"/> <arg name="connection_radius" default="10.0"/>
<arg name="curvature" default="0.05"/> <arg name="curvature" default="1.0"/>
<include file="$(find cse478)/launch/teleop.launch"> <include file="$(find cse478)/launch/teleop.launch">
<arg name="fake_localization" value="$(arg fake_localization)" /> <arg name="fake_localization" value="$(arg fake_localization)" />
......
...@@ -10,8 +10,8 @@ ...@@ -10,8 +10,8 @@
<arg name="rviz" default="true" /> <arg name="rviz" default="true" />
<arg name="num_vertices" default="200"/> <arg name="num_vertices" default="200"/>
<arg name="connection_radius" default="100.0"/> <arg name="connection_radius" default="10.0"/>
<arg name="curvature" default="0.05"/> <arg name="curvature" default="1.0"/>
<include file="$(find cse478)/launch/teleop.launch"> <include file="$(find cse478)/launch/teleop.launch">
<arg name="fake_localization" value="$(arg fake_localization)" /> <arg name="fake_localization" value="$(arg fake_localization)" />
......
...@@ -17,29 +17,28 @@ from planning.samplers import samplers ...@@ -17,29 +17,28 @@ from planning.samplers import samplers
def main(args): def main(args):
map_info = None
saveto = None
if args.use_ros_map: if args.use_ros_map:
permissible_region, map_info = utils.get_map("/static_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: elif args.text_map:
permissible_region = np.loadtxt(args.text_map, dtype=bool) permissible_region = np.loadtxt(args.text_map, dtype=bool)
_, fname = os.path.split(args.text_map)
map_name, _ = os.path.splitext(fname)
else: else:
permissible_region = np.ones((10, 10), dtype=bool) 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": if args.problem == "r2":
problem = R2Problem(permissible_region) problem = R2Problem(permissible_region, map_info=map_info)
else: 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) sampler = samplers[args.sampler](problem.extents)
rm = Roadmap( rm = Roadmap(
problem, problem,
...@@ -56,7 +55,7 @@ if __name__ == "__main__": ...@@ -56,7 +55,7 @@ if __name__ == "__main__":
parser = argparse.ArgumentParser(description="generate and visualize roadmaps") parser = argparse.ArgumentParser(description="generate and visualize roadmaps")
parser.add_argument("-n", "--num-vertices", type=int, default=100) parser.add_argument("-n", "--num-vertices", type=int, default=100)
parser.add_argument("-r", "--connection-radius", type=float, default=2.0) 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("--sampler", choices=samplers, default="halton")
parser.add_argument("--lazy", action="store_true") parser.add_argument("--lazy", action="store_true")
parser.add_argument("--show-edges", action="store_true") parser.add_argument("--show-edges", action="store_true")
......
...@@ -63,7 +63,7 @@ if __name__ == "__main__": ...@@ -63,7 +63,7 @@ if __name__ == "__main__":
) )
parser.add_argument("-m", "--text-map", type=str, help="the environment to plan on") 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("-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("--sampler", type=str, default="halton", choices=samplers)
parser.add_argument("--lazy", action="store_true") parser.add_argument("--lazy", action="store_true")
parser.add_argument("--shortcut", action="store_true") parser.add_argument("--shortcut", action="store_true")
...@@ -76,7 +76,7 @@ if __name__ == "__main__": ...@@ -76,7 +76,7 @@ if __name__ == "__main__":
se2_parser = subparsers.add_parser("se2", help="SE2 options") 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("-s", "--start", nargs=3, type=int, required=True)
se2_parser.add_argument("-g", "--goal", 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() args = parser.parse_args()
main(args) main(args)
...@@ -10,9 +10,10 @@ import math ...@@ -10,9 +10,10 @@ import math
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import numpy as np import numpy as np
from cse478 import utils 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. """Return the Dubins path between two states.
Args: Args:
...@@ -41,7 +42,7 @@ def path_planning(start, end, curvature, resolution=0.1): ...@@ -41,7 +42,7 @@ def path_planning(start, end, curvature, resolution=0.1):
end[2] -= start[2] end[2] -= start[2]
path, mode, length = path_planning_from_origin( 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 # Transform back into original reference frame
inv_rot = utils.rotation_matrix(-start_orientation) inv_rot = utils.rotation_matrix(-start_orientation)
...@@ -58,28 +59,35 @@ def path_planning(start, end, curvature, resolution=0.1): ...@@ -58,28 +59,35 @@ def path_planning(start, end, curvature, resolution=0.1):
return path, real_path_length return path, real_path_length
def path_length(s, e, c): def path_length(start, end, curvature):
"""Return the length of the Dubins path between two states. """Return the length of the Dubins path between pairs of states.
Args: Args:
s: see documentation for start above start: start configurations shape (N, 3)
e: see documentation for end above end: end configurations shape (N, 3)
c: see documentation for curvature above curvature: shape N
Returns: Returns:
length: length of Dubins path length: lengths of Dubins paths shape N
""" """
# Transform to make start of path the origin # Transform to make start of path the origin
start = np.array(s, dtype=float) start = np.atleast_2d(start)
end = np.array(e, dtype=float) end = np.atleast_2d(end)
end[:2] -= start[:2] # We'll gracefully handle mismatched argument dimensions
start_orientation = start[2] # 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) rotation = utils.rotation_matrix(start_orientation)
end[:2] = np.matmul(end[:2], rotation) rotation = np.moveaxis(rotation, -1, 0)
end[2] -= start[2] 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) _, _, cost = get_best_plan_from_origin(end_broad, curvature)
return cost * (1 / c) return cost * (1 / curvature)
########################## ##########################
...@@ -87,165 +95,138 @@ def path_length(s, e, c): ...@@ -87,165 +95,138 @@ def path_length(s, e, c):
########################## ##########################
def mod2pi(theta): def mod2pi(theta, out=None):
return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi) 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): def pi_2_pi(angles):
"""
Wrap angles to (-pi, pi]
Args:
angles:
"""
angles[:] = (angles[:] - np.pi) % (2 * np.pi) + np.pi angles[:] = (angles[:] - np.pi) % (2 * np.pi) + np.pi
def lsl(alpha, beta, d): planner_modes = ["LSL", "RSR", "LSR", "RSL", "RLR", "LRL"]
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
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)) p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sa - sb))
if p_squared < 0: tmp1 = np.arctan2((cb - ca), tmp0)
return None, None, None, mode out[:, 0, 0] = -alpha + tmp1
tmp1 = math.atan2((cb - ca), tmp0) out[:, 0, 1] = np.sqrt(p_squared)
t = mod2pi(-alpha + tmp1) out[:, 0, 2] = beta - 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)
# RSR
tmp0 = d - sa + sb tmp0 = d - sa + sb
mode = "RSR"
p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sb - sa)) p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sb - sa))
if p_squared < 0: tmp1 = np.arctan2((ca - cb), tmp0)
return None, None, None, mode out[:, 1, 0] = alpha - tmp1
tmp1 = math.atan2((ca - cb), tmp0) out[:, 1, 1] = np.sqrt(p_squared)
t = mod2pi(alpha - tmp1) out[:, 1, 2] = -beta + tmp1
p = math.sqrt(p_squared)
q = mod2pi(-beta + tmp1)
return t, p, q, mode