def pixel_to_world(self, pixel_coords):
"""
Convert map pixel coordinates to world coordinates
Note that this does not check whether the coordinates
are in bounds.
:param pixel_coords: either a n-elem numpy array [x, y] or a n x 2 array with n points
:return: same as input, but in world coordinates
"""
return pixel_to_world(pixel_coords, self._origin, self._resolution)
def get_physical_coords_from_drawing(map_shape, resolution, origin,
drawing_coords):
"""
Inverse of the get_drawing_coordinates_from_physical function
:param map_shape: shape of the map
:param resolution: resolution of the map
:param origin: origin of the map
:param drawing_coords: drawing coordinates
:return: phusical coordinates from drawing
"""
# this makes a copy to make sure that we do not change original coords
drawing_coords = np.array(drawing_coords)
assert drawing_coords.ndim <= 2
assert drawing_coords.shape[drawing_coords.ndim - 1] == 2
assert np.array(map_shape).ndim == 1
drawing_coords[..., 1] = map_shape[0] - 1 - drawing_coords[..., 1]
return pixel_to_world(drawing_coords, origin, resolution)
def _override_primitive_kernels(self, motion_primitives, footprint,
use_full_kernels):
resolution = motion_primitives.get_resolution()
print('Setting up motion primitive kernels..')
for p in motion_primitives.get_primitives():
primitive_start = pixel_to_world(np.zeros((2, )), np.zeros((2, )),
resolution)
primitive_states = p.get_intermediate_states().copy()
primitive_states[:, :2] += primitive_start
full_cv_kernel_x = []
full_cv_kernel_y = []
for pose in primitive_states:
kernel = get_pixel_footprint(pose[2], footprint, resolution)
kernel_center = (kernel.shape[1] // 2, kernel.shape[0] // 2)
kernel = np.where(kernel)
px, py = world_to_pixel(pose[:2], np.zeros((2, )), resolution)
full_cv_kernel_x.append(kernel[1] + (px - kernel_center[0]))
full_cv_kernel_y.append(kernel[0] + (py - kernel_center[1]))
full_cv_kernel_x = np.hstack(full_cv_kernel_x)
full_cv_kernel_y = np.hstack(full_cv_kernel_y)
full_cv_kernel = np.column_stack(
(full_cv_kernel_x, full_cv_kernel_y)).astype(np.int32)
row_view = np.ascontiguousarray(full_cv_kernel).view(
np.dtype(
(np.void,
full_cv_kernel.dtype.itemsize * full_cv_kernel.shape[1])))
_, idx = np.unique(row_view, return_index=True)
full_cv_kernel = np.ascontiguousarray(full_cv_kernel[idx])
if use_full_kernels:
self.set_primitive_collision_pixels(p.starttheta_c,
p.motprimID,
full_cv_kernel)
else:
min_x, max_x = np.amin(full_cv_kernel[:, 0]), np.amax(
full_cv_kernel[:, 0])
min_y, max_y = np.amin(full_cv_kernel[:, 1]), np.amax(
full_cv_kernel[:, 1])
temp_img = np.zeros((max_y - min_y + 1, max_x - min_x + 1),
dtype=np.uint8)
temp_img[full_cv_kernel[:, 1] - min_y,
full_cv_kernel[:, 0] - min_x] = 255
_, contours, _ = cv2.findContours(temp_img.copy(),
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
contour = contours[0].reshape(-1, 2)
perimeter_kernel = np.column_stack(
(contour[:, 0] + min_x,
contour[:, 1] + min_y)).astype(np.int32)
self.set_primitive_collision_pixels(p.starttheta_c,
p.motprimID,
perimeter_kernel)
print('Done.')
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 forward_model_tricycle_motion_primitives(resolution,
number_of_angles,
target_v,
tricycle_angle_samples,
primitives_duration,
front_wheel_rotation_speedup,
v_samples,
refine_dt=0.1):
max_front_wheel_angle = IndustrialTricycleV1Dimensions.max_front_wheel_angle(
)
front_wheel_from_axis = IndustrialTricycleV1Dimensions.front_wheel_from_axis(
)
max_front_wheel_speed = IndustrialTricycleV1Dimensions.max_front_wheel_speed(
)
front_column_model_p_gain = IndustrialTricycleV1Dimensions.front_column_model_p_gain(
)
def forward_model(pose, initial_states, dt, control_signals):
current_wheel_angles = initial_states[:, 0]
next_poses, next_angles = tricycle_kinematic_step(
pose,
current_wheel_angles,
dt,
control_signals,
max_front_wheel_angle,
front_wheel_from_axis,
max_front_wheel_speed,
front_column_model_p_gain,
model_front_column_pid=False)
next_state = initial_states.copy()
next_state[:, 0] = next_angles[:]
next_state[:, 1] = control_signals[:, 0]
next_state[:, 2] = 0. # angular velocity is ignored
return next_poses, next_state
pose_evolution, state_evolution, control_evolution, refine_dt = control_choices_tricycle_exhaustive(
forward_model,
wheel_angle=0.,
max_v=target_v,
exhausitve_dt=refine_dt * front_wheel_rotation_speedup,
refine_dt=refine_dt,
n_steps=1,
theta_samples=tricycle_angle_samples,
v_samples=v_samples,
extra_copy_n_steps=primitives_duration - 1,
max_front_wheel_angle=max_front_wheel_angle,
max_front_wheel_speed=max_front_wheel_speed)
primitives = []
for start_theta_discrete in range(number_of_angles):
current_primitive_cells = []
for primitive_id, (ego_poses, controls) in enumerate(
zip(pose_evolution, control_evolution)):
ego_poses = np.vstack(([[0., 0., 0.]], ego_poses))
start_angle = angle_discrete_to_cont(start_theta_discrete,
number_of_angles)
poses = from_egocentric_to_global(
ego_poses,
ego_pose_in_global_coordinates=np.array([0., 0., start_angle]))
# to model precision loss while converting to .mprim file, we round it here
poses = np.around(poses, decimals=4)
last_pose = poses[-1]
end_cell = np.zeros((3, ), dtype=int)
center_cell_shift = pixel_to_world(np.zeros((2, )), np.zeros(
(2, )), resolution)
end_cell[:2] = world_to_pixel(center_cell_shift + last_pose[:2],
np.zeros((2, )), resolution)
perfect_last_pose = np.zeros((3, ), dtype=float)
end_cell[2] = angle_cont_to_discrete(last_pose[2],
number_of_angles)
current_primitive_cells.append(tuple(end_cell))
perfect_last_pose[:2] = pixel_to_world(end_cell[:2], np.zeros(
(2, )), resolution)
perfect_last_pose[2] = angle_discrete_to_cont(
end_cell[2], number_of_angles)
action_cost_multiplier = 1
primitive = MotionPrimitive(
primitive_id=primitive_id,
start_theta_discrete=start_theta_discrete,
action_cost_multiplier=action_cost_multiplier,
end_cell=end_cell,
intermediate_states=poses,
control_signals=controls)
primitives.append(primitive)
return MotionPrimitives(resolution=resolution,
number_of_angles=number_of_angles,
mprim_list=primitives)
def forward_model_diffdrive_motion_primitives(resolution,
number_of_angles,
target_v,
target_w,
w_samples_in_each_direction,
primitives_duration,
refine_dt=0.05):
def forward_model(pose, state, dt, control_signals):
new_pose = kinematic_body_pose_motion_step(
pose=pose,
linear_velocity=control_signals[:, 0],
angular_velocity=control_signals[:, 1],
dt=dt)
next_state = state.copy()
next_state[:, 0] = control_signals[:, 0]
next_state[:, 1] = control_signals[:, 1]
return new_pose, next_state
pose_evolution, state_evolution, control_evolution, refine_dt, control_costs = control_choices_diff_drive_exhaustive(
max_v=target_v,
max_w=target_w,
forward_model=forward_model,
initial_state=(0., 0.),
refine_dt=refine_dt,
exhausitve_dt=refine_dt * primitives_duration,
n_steps=1,
w_samples_in_each_direction=w_samples_in_each_direction,
enable_turn_in_place=True,
v_samples=1)
primitives = []
for start_theta_discrete in range(number_of_angles):
current_primitive_cells = []
for primitive_id, (ego_poses, controls) in enumerate(
zip(pose_evolution, control_evolution)):
ego_poses = np.vstack(([[0., 0., 0.]], ego_poses))
start_angle = angle_discrete_to_cont(start_theta_discrete,
number_of_angles)
poses = from_egocentric_to_global(
ego_poses,
ego_pose_in_global_coordinates=np.array([0., 0., start_angle]))
# to model precision loss while converting to .mprim file, we round it here
poses = np.around(poses, decimals=4)
last_pose = poses[-1]
end_cell = np.zeros((3, ), dtype=int)
center_cell_shift = pixel_to_world(np.zeros((2, )), np.zeros(
(2, )), resolution)
end_cell[:2] = world_to_pixel(center_cell_shift + last_pose[:2],
np.zeros((2, )), resolution)
perfect_last_pose = np.zeros((3, ), dtype=float)
end_cell[2] = angle_cont_to_discrete(last_pose[2],
number_of_angles)
current_primitive_cells.append(tuple(end_cell))
perfect_last_pose[:2] = pixel_to_world(end_cell[:2], np.zeros(
(2, )), resolution)
perfect_last_pose[2] = angle_discrete_to_cont(
end_cell[2], number_of_angles)
# penalize slow movement forward and sudden jerns
if controls[0, 0] < target_v * 0.5 or abs(
controls[0, 1]) > 0.5 * target_w:
action_cost_multiplier = 100
else:
action_cost_multiplier = 1
primitive = MotionPrimitive(
primitive_id=primitive_id,
start_theta_discrete=start_theta_discrete,
action_cost_multiplier=action_cost_multiplier,
end_cell=end_cell,
intermediate_states=poses,
control_signals=controls)
primitives.append(primitive)
return MotionPrimitives(resolution=resolution,
number_of_angles=number_of_angles,
mprim_list=primitives)
def debug_motion_primitives(motion_primitives, only_zero_angle=False):
angle_to_primitive = check_motion_primitives(motion_primitives)
all_angles = normalize_angle(
np.arange(motion_primitives.get_number_of_angles()) * np.pi * 2 /
motion_primitives.get_number_of_angles())
print('All angles: %s' % all_angles)
print('(in degrees: %s)' % np.degrees(all_angles))
for angle_c, primitives in angle_to_primitive.items():
print('------', angle_c)
for p in primitives:
if np.all(p.get_intermediate_states()[0, :2] ==
p.get_intermediate_states()[:, :2]):
turn_in_place = 'turn in place'
else:
turn_in_place = ''
print(turn_in_place, p.endcell,
p.get_intermediate_states()[0],
p.get_intermediate_states()[-1])
final_float_state = pixel_to_world(
p.endcell[:2], np.zeros((2, )),
motion_primitives.get_resolution())
final_float_angle = angle_discrete_to_cont(
p.endcell[2], motion_primitives.get_number_of_angles())
try:
np.testing.assert_array_almost_equal(
final_float_state,
p.get_intermediate_states()[-1][:2])
except AssertionError:
print("POSE DOESN'T LAND TO A CELL", final_float_state,
p.get_intermediate_states()[-1])
try:
np.testing.assert_array_almost_equal(
final_float_angle,
p.get_intermediate_states()[-1][2],
decimal=3)
except AssertionError:
print("ANGLE DOESN'T LAND TO AN ANGLE CELL",
np.degrees(final_float_angle),
np.degrees(p.get_intermediate_states()[-1][2]))
endcells = np.array([p.endcell for p in primitives])
image_half_width = np.amax(np.amax(np.abs(endcells), 0)[:2]) + 1
zoom = 10
img = np.full(
(zoom * image_half_width * 2, zoom * image_half_width * 2, 3),
255,
dtype=np.uint8)
resolution = motion_primitives.get_resolution() / zoom
origin = np.array(
(-image_half_width * motion_primitives.get_resolution(),
-image_half_width * motion_primitives.get_resolution()))
for p in primitives:
draw_arrow(img,
p.get_intermediate_states()[0],
0.7 * motion_primitives.get_resolution(),
origin,
resolution,
color=(0, 0, 0))
draw_trajectory(img,
resolution,
origin,
p.get_intermediate_states()[:, :2],
color=(0, 200, 0))
draw_arrow(img,
p.get_intermediate_states()[-1],
0.5 * motion_primitives.get_resolution(),
origin,
resolution,
color=(0, 0, 200))
cv2.imshow('a', img)
cv2.waitKey(-1)
if only_zero_angle:
return
