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')