def test_draw_robot_full():
costmap = CostMap2D.create_empty((5, 5), 0.05)
array_to_draw = np.zeros((costmap.get_data().shape + (3, )),
dtype=np.uint8)
draw_robot(array_to_draw,
_tricycle_footprint(), (2, 2, np.pi / 4),
costmap.get_resolution(),
costmap.get_origin(),
color=(0, 0, 150))
draw_robot(array_to_draw,
_tricycle_footprint(), (-0.5, -0.5, np.pi / 4),
costmap.get_resolution(),
costmap.get_origin(),
color=(0, 0, 150))
draw_robot(array_to_draw,
_tricycle_footprint(), (4.6, 3, np.pi / 4),
costmap.get_resolution(),
costmap.get_origin(),
color=(0, 0, 150))
draw_robot(array_to_draw,
_tricycle_footprint(), (-4234.6, 3456, np.pi / 4),
costmap.get_resolution(),
costmap.get_origin(),
color=(0, 0, 150))
static_map = CostMap2D.create_empty((5, 5), 0.05)
img = prepare_canvas(static_map.get_data().shape)
draw_robot(img, _tricycle_footprint(), [0., -3, np.pi / 2.],
static_map.get_resolution(), static_map.get_origin())
def box_3d_planning(debug):
costmap = CostMap2D.create_empty((4, 4), 0.05, np.zeros((2,)))
gap = 1.4
add_wall_to_static_map(costmap, (0, 2), (2, 2), width=0.5)
add_wall_to_static_map(costmap, (2+gap, 2), (4, 2), width=0.5)
footprint_width = 0.79
footprint = np.array(
[[0.5*footprint_width, 0.5*footprint_width],
[-0.5 * footprint_width+1e-6, 0.5 * footprint_width],
[-0.5 * footprint_width+1e-6, -0.5 * footprint_width+1e-6],
[0.5 * footprint_width, -0.5 * footprint_width+1e-6]]
)
motion_primitives = exhaustive_geometric_primitives(
costmap.get_resolution(), 10, 32
)
start_pose = np.array([2.3, 1.0, np.pi/4])
goal_pose = np.array([2.6, 3.0, np.pi/4])
plan_xytheta, plan_xytheta_cell, controls, plan_time, solution_eps, environment = perform_single_planning(
planner_name='arastar',
footprint=footprint,
motion_primitives=motion_primitives,
forward_search=True,
costmap=costmap,
start_pose=start_pose,
goal_pose=goal_pose,
target_v=0.65,
target_w=1.0,
allocated_time=np.inf,
cost_scaling_factor=40.,
debug=False)
if debug:
print(environment.xytheta_real_to_cell(start_pose))
print(environment.xytheta_real_to_cell(goal_pose))
print(plan_xytheta_cell)
print("done with the solution of size=%d and sol. eps=%f" % (len(plan_xytheta_cell), solution_eps))
print("actual path (with intermediate poses) size=%d" % len(plan_xytheta))
params = environment.get_params()
costmap = environment.get_costmap()
img = prepare_canvas(costmap.shape)
draw_world_map_with_inflation(img, costmap)
for pose in plan_xytheta:
draw_robot(img, footprint, pose, params.cellsize_m, np.zeros((2,)),
color=70, color_axis=(1, 2))
draw_trajectory(img, params.cellsize_m, np.zeros((2,)), plan_xytheta)
draw_robot(img, footprint, start_pose, params.cellsize_m, np.zeros((2,)))
draw_robot(img, footprint, goal_pose, params.cellsize_m, np.zeros((2,)))
magnify = 4
img = cv2.resize(img, dsize=(0, 0), fx=magnify, fy=magnify, interpolation=cv2.INTER_NEAREST)
cv2.imshow("planning result", img)
cv2.waitKey(-1)
def perform_single_planning(
planner_name,
footprint,
motion_primitives,
forward_search,
costmap,
start_pose,
goal_pose,
target_v=0.65,
target_w = 1.0,
allocated_time=np.inf,
cost_scaling_factor = 4.,
debug=False,
use_full_kernels=False):
assert costmap.get_resolution() == motion_primitives.get_resolution()
cost_possibly_circumscribed_thresh = compute_cost_possibly_circumscribed_thresh(
footprint, costmap.get_resolution(),
cost_scaling_factor=cost_scaling_factor
)
inflated_costmap = inflate_costmap(
costmap, cost_scaling_factor, footprint
)
params = EnvNAVXYTHETALAT_InitParms()
params.size_x = costmap.get_data().shape[1]
params.size_y = costmap.get_data().shape[0]
params.numThetas = motion_primitives.get_number_of_angles()
params.cellsize_m = costmap.get_resolution()
params.nominalvel_mpersecs = target_v
params.timetoturn45degsinplace_secs = 1./target_w/8.
params.obsthresh = 254
params.costinscribed_thresh = 253
params.costcircum_thresh = cost_possibly_circumscribed_thresh
params.startx = 0
params.starty = 0
params.starttheta = 0
params.goalx = 0
params.goaly = 0
params.goaltheta = 0
params.expansion_angle_lower_limit = -np.inf
params.expansion_angle_upper_limit = np.inf
environment = EnvironmentNAVXYTHETALAT(
footprint,
motion_primitives,
inflated_costmap.get_data(),
params,
use_full_kernels=use_full_kernels
)
planner = create_planner(planner_name, environment, forward_search)
start_pose = start_pose.copy()
start_pose[:2] -= costmap.get_origin()
goal_pose = goal_pose.copy()
goal_pose[:2] -= costmap.get_origin()
planner.set_start(start_pose, environment, True)
planner.set_goal(goal_pose, environment, True)
plan_xytheta, plan_xytheta_cell, actions, plan_time, solution_eps = planner.replan(
environment, allocated_time=allocated_time, final_epsilon=1)
if debug:
print("done with the solution of size=%d and sol. eps=%f in %ss" % (len(plan_xytheta_cell), solution_eps, plan_time))
print("actual path (with intermediate poses) size=%d" % len(plan_xytheta))
params = environment.get_params()
costmap = environment.get_costmap()
img = prepare_canvas(costmap.shape)
draw_world_map_with_inflation(img, costmap)
for pose in plan_xytheta:
draw_robot(img, footprint, pose, params.cellsize_m, np.zeros((2,)),
color=70, color_axis=(1, 2))
draw_trajectory(img, params.cellsize_m, np.zeros((2,)), plan_xytheta)
draw_robot(img, footprint, start_pose, params.cellsize_m, np.zeros((2,)))
draw_robot(img, footprint, goal_pose, params.cellsize_m, np.zeros((2,)))
magnify = 2
img = cv2.resize(img, dsize=(0, 0), fx=magnify, fy=magnify, interpolation=cv2.INTER_NEAREST)
cv2.imshow("planning result", img)
cv2.waitKey(-1)
return plan_xytheta, plan_xytheta_cell, actions, plan_time, solution_eps, environment
def box_2d_planning(debug):
costmap = CostMap2D.create_empty((4, 4), 0.2, np.zeros((2,)))
gap = 1.2
add_wall_to_static_map(costmap, (0, 2), (2, 2), width=0.0)
add_wall_to_static_map(costmap, (2+gap, 2), (4, 2), width=0.0)
footprint_width = 0.79
footprint = np.array(
[[0.5*footprint_width, 0.5*footprint_width],
[-0.5 * footprint_width+1e-6, 0.5 * footprint_width],
[-0.5 * footprint_width+1e-6, -0.5 * footprint_width+1e-6],
[0.5 * footprint_width, -0.5 * footprint_width+1e-6]]
)
start_theta_discrete = 0
number_of_intermediate_states = 2
number_of_angles = 1
batch = []
action_cost_multiplier = 1
for i, end_cell in enumerate([[1, 0, 0],
[0, 1, 0],
[-1, 0, 0],
[0, -1, 0]]):
batch.append(create_linear_primitive(
primitive_id=i,
start_theta_discrete=start_theta_discrete,
action_cost_multiplier=action_cost_multiplier,
end_cell=end_cell,
number_of_intermediate_states=number_of_intermediate_states,
resolution=costmap.get_resolution(),
number_of_angles=number_of_angles))
motion_primitives = MotionPrimitives(
resolution=costmap.get_resolution(),
number_of_angles=1,
mprim_list=batch
)
start_pose = np.array([2.3, 1.3, 0.])
goal_pose = np.array([2.6, 2.8, 0.])
plan_xytheta, plan_xytheta_cell, controls, plan_time, solution_eps, environment = perform_single_planning(
planner_name='arastar',
footprint=footprint,
motion_primitives=motion_primitives,
forward_search=True,
costmap=costmap,
start_pose=start_pose,
goal_pose=goal_pose,
target_v=0.65,
target_w=1.0,
allocated_time=np.inf,
cost_scaling_factor=40.,
debug=False)
if debug:
print(environment.xytheta_real_to_cell(start_pose))
print(environment.xytheta_real_to_cell(goal_pose))
print(plan_xytheta_cell)
print("done with the solution of size=%d and sol. eps=%f" % (len(plan_xytheta_cell), solution_eps))
print("actual path (with intermediate poses) size=%d" % len(plan_xytheta))
params = environment.get_params()
costmap = environment.get_costmap()
img = prepare_canvas(costmap.shape)
draw_world_map_with_inflation(img, costmap)
for pose in plan_xytheta:
draw_robot(img, footprint, pose, params.cellsize_m, np.zeros((2,)),
color=70, color_axis=(1, 2))
draw_trajectory(img, params.cellsize_m, np.zeros((2,)), plan_xytheta)
draw_robot(img, footprint, start_pose, params.cellsize_m, np.zeros((2,)))
draw_robot(img, footprint, goal_pose, params.cellsize_m, np.zeros((2,)))
magnify = 16
img = cv2.resize(img, dsize=(0, 0), fx=magnify, fy=magnify, interpolation=cv2.INTER_NEAREST)
cv2.imshow("planning result", img)
cv2.waitKey(-1)
def run_sbpl_tricycle_motion_primitive_planning(
number_of_angles, target_v, target_w, tricycle_angle_samples,
primitives_duration, footprint_scale, do_debug_motion_primitives):
original_costmap, static_path, test_maps = get_random_maps_squeeze_between_obstacle_in_corridor_on_path(
)
test_map = test_maps[0]
resolution = test_map.get_resolution()
motion_primitives = forward_model_tricycle_motion_primitives(
resolution=resolution,
number_of_angles=number_of_angles,
target_v=target_v,
tricycle_angle_samples=tricycle_angle_samples,
primitives_duration=primitives_duration,
front_wheel_rotation_speedup=10,
refine_dt=0.10,
v_samples=1)
if do_debug_motion_primitives:
debug_motion_primitives(motion_primitives)
add_wall_to_static_map(test_map, (1, -4.6), (1.5, -4.6))
footprint = IndustrialTricycleV1Dimensions.footprint() * footprint_scale
plan_xytheta, plan_xytheta_cell, actions, plan_time, solution_eps, environment = perform_single_planning(
planner_name='arastar',
footprint=footprint,
motion_primitives=motion_primitives,
forward_search=False,
costmap=test_map,
start_pose=static_path[0],
goal_pose=static_path[-10],
target_v=target_v,
target_w=target_w,
allocated_time=np.inf,
cost_scaling_factor=4.,
debug=False,
use_full_kernels=True)
params = environment.get_params()
costmap = environment.get_costmap()
img = prepare_canvas(costmap.shape)
draw_world_map_with_inflation(img, costmap)
start_pose = static_path[0]
start_pose[:2] -= test_map.get_origin()
goal_pose = static_path[-10]
goal_pose[:2] -= test_map.get_origin()
trajectory_through_primitives = np.array([start_pose])
plan_xytheta_cell = np.vstack(
([environment.xytheta_real_to_cell(start_pose)], plan_xytheta_cell))
for i in range(len(actions)):
angle_c, motor_prim_id = actions[i]
collisions = environment.get_primitive_collision_pixels(
angle_c, motor_prim_id)
pose_cell = plan_xytheta_cell[i]
assert pose_cell[2] == angle_c
collisions[:, 0] += pose_cell[0]
collisions[:, 1] += pose_cell[1]
primitive_start = pixel_to_world(pose_cell[:2], np.zeros((2, )),
test_map.get_resolution())
primitive = motion_primitives.find_primitive(angle_c, motor_prim_id)
primitive_states = primitive.get_intermediate_states().copy()
primitive_states[:, :2] += primitive_start
trajectory_through_primitives = np.vstack(
(trajectory_through_primitives, primitive_states))
for pose in trajectory_through_primitives:
draw_robot(img,
footprint,
pose,
params.cellsize_m,
np.zeros((2, )),
color=70,
color_axis=(0, 2))
# draw_trajectory(img, params.cellsize_m, np.zeros((2,)), plan_xytheta)
draw_robot(img, footprint, start_pose, params.cellsize_m, np.zeros((2, )))
draw_robot(img, footprint, goal_pose, params.cellsize_m, np.zeros((2, )))
magnify = 2
img = cv2.resize(img,
dsize=(0, 0),
fx=magnify,
fy=magnify,
interpolation=cv2.INTER_NEAREST)
cv2.imshow("planning result", img)
cv2.waitKey(-1)
def planandnavigatexythetalat(environment_config, motion_primitives,
planner_name):
"""
Python port of planandnavigatexythetalat from sbpl test/main.cpp
"""
true_env = EnvironmentNAVXYTHETALAT.create_from_config(environment_config)
params = true_env.get_params()
cost_obstacle, cost_inscribed, cost_possibly_circum = true_env.get_cost_thresholds(
)
true_costmap = true_env.get_costmap()
assert cost_obstacle == CostMap2D.LETHAL_OBSTACLE
assert cost_inscribed == INSCRIBED_INFLATED_OBSTACLE
# check the start and goal obtained from the true environment
print("start: %f %f %f, goal: %f %f %f\n" %
(params.startx, params.starty, params.starttheta, params.goalx,
params.goaly, params.goaltheta))
footprint = np.zeros((4, 2))
halfwidth = 0.01
halflength = 0.01
footprint[0, :] = (-halflength, -halfwidth)
footprint[1, :] = (halflength, -halfwidth)
footprint[2, :] = (halflength, halfwidth)
footprint[3, :] = (-halflength, halfwidth)
empty_map = np.zeros((params.size_y, params.size_x), dtype=np.uint8)
env = EnvironmentNAVXYTHETALAT(footprint, motion_primitives, empty_map,
params)
# compute sensing as a square surrounding the robot with length twice that of the
# longest motion primitive
max_mot_prim_length_squared = 0
for p in env.get_motion_primitives().get_primitives():
dx = p.endcell[0]
dy = p.endcell[1]
primitive_length = dx * dx + dy * dy
if primitive_length > max_mot_prim_length_squared:
print(
"Found a longer motion primitive with dx = %s and dy = %s from starttheta = %s"
% (dx, dy, p.starttheta_c))
max_mot_prim_length_squared = primitive_length
max_motor_primitive_length = np.sqrt(max_mot_prim_length_squared)
print("Maximum motion primitive length: %s" % max_motor_primitive_length)
incremental_sensing = _sbpl_module.IncrementalSensing(
10 * int(max_motor_primitive_length + 0.5))
planner = create_planner(planner_name, env, False)
planner.set_start_goal_from_env(env)
planner.set_planning_params(initial_epsilon=3.0,
search_until_first_solution=False)
params = env.get_params()
goal_pose = np.array([params.goalx, params.goaly, params.goaltheta])
print("goal cell: %s" % env.xytheta_real_to_cell(goal_pose))
goaltol_x = 0.001
goaltol_y = 0.001
goaltol_theta = 0.001
steps_along_the_path = 1
start_pose = np.array((params.startx, params.starty, params.starttheta))
# now comes the main loop
while (abs(start_pose[0] - params.goalx) > goaltol_x
or abs(start_pose[1] - params.goaly) > goaltol_y
or abs(start_pose[2] - params.goaltheta) > goaltol_theta):
changed_cells = incremental_sensing.sense_environment(
start_pose, true_env, env)
planner.apply_environment_changes(changed_cells, env)
print("new planning...")
plan_xytheta, plan_xytheta_cell, controls, plan_time, solution_eps = planner.replan(
env, allocated_time=10., final_epsilon=1.)
print("done with the solution of size=%d and sol. eps=%f" %
(len(plan_xytheta_cell), solution_eps))
print("actual path (with intermediate poses) size=%d" %
len(plan_xytheta))
if len(plan_xytheta_cell):
# move until we move into the end of motion primitive
cell_to_move = plan_xytheta_cell[min(
len(plan_xytheta_cell) - 1, steps_along_the_path)]
print("Moving %s -> %s" %
(env.xytheta_real_to_cell(start_pose), cell_to_move))
# this check is weak since true configuration does not know the actual perimeter of the robot
if not true_env.is_valid_configuration(cell_to_move):
raise Exception(
"ERROR: robot is commanded to move into an invalid configuration according to true environment"
)
new_start_pose = env.xytheta_cell_to_real(cell_to_move)
planner.set_start(new_start_pose, env)
else:
new_start_pose = start_pose
print("No move is made")
img = prepare_canvas(true_costmap.shape)
draw_world_map_with_inflation(img, true_costmap)
draw_trajectory(img, params.cellsize_m, np.zeros((2, )), plan_xytheta)
draw_robot(img, footprint, start_pose, params.cellsize_m,
np.zeros((2, )))
draw_robot(img, footprint, goal_pose, params.cellsize_m, np.zeros(
(2, )))
cv2.imshow("current map", img)
cv2.waitKey(-1)
start_pose = new_start_pose
print('Goal reached')