def infer_at_pose(self, images: List[OrthographicImage], pose: Affine):
input_images = []
for image in images:
draw_around_box(image, box=self.box)
area_mat = get_area_of_interest_new(
image,
pose,
size_cropped=(200, 200),
size_result=(32, 32),
border_color=image.value_from_depth(
get_distance_to_box(image, self.box)),
).mat
area_mat = np.array(area_mat, dtype=np.float32) / np.iinfo(
area_mat.dtype).max # 2 * x - 1.0
area_mat_exp = np.expand_dims(area_mat, axis=-1)
input_images.append(area_mat_exp)
if self.monte_carlo:
input_images_mc = [
np.array([image for i in range(self.monte_carlo)])
for image in input_images
]
estimated_rewards_sampling = self.model.predict(input_images_mc)
estimated_reward = np.mean(estimated_rewards_sampling, axis=0)
estimated_reward_std = self.mutual_information(
estimated_rewards_sampling)
return estimated_reward, estimated_reward_std
return self.model.predict([input_images])[0]
def area_of_interest(self, image, pose):
area = get_area_of_interest_new(
image,
RobotPose(all_data=pose),
size_cropped=self.size_cropped_area,
size_result=self.size_result,
border_color=image.value_from_depth(self.box_distance) / (255 * 255),
)
if len(area.mat.shape) == 2:
return np.expand_dims(area.mat, 2)
return area.mat
def update_image():
after_area = get_area_of_interest_new(place_after,
action.pose,
size_cropped=(200, 200))
goal_area = get_area_of_interest_new(place_goal,
new_pose,
size_cropped=(200, 200))
ontop = np.zeros((200, 200, 3), dtype=np.uint8)
if len(after_area.mat.shape) == 3:
after_area.mat = after_area.mat[:, :, 0]
goal_area.mat = goal_area.mat[:, :, 0]
ontop[:, :, 0] += (after_area.mat / 255).astype(np.uint8)
ontop[:, :, 1] += (after_area.mat / 255).astype(np.uint8)
ontop[:, :, 2] += (goal_area.mat / 255).astype(np.uint8)
dx = action.pose.x - new_pose.x
dy = action.pose.y - new_pose.y
dx_new = np.abs(dx * np.cos(action.pose.a) +
dy * np.sin(action.pose.a))
dy_new = np.abs(-dx * np.sin(action.pose.a) +
dy * np.cos(action.pose.a))
dt = np.sqrt(dx_new**2 + dy_new**2)
da = np.abs(action.pose.a - new_pose.a)
print('---')
print(f'X: {dx_new * 1e3:0.4f} [mm]')
print(f'Y: {dy_new * 1e3:0.4f} [mm]')
print(f'Translation: {dt * 1e3:0.4f} [mm]')
print(f'Rotation: {da * 180/np.pi:0.4f} [deg]')
im = PIL.Image.fromarray(ontop)
imgtk = PIL.ImageTk.PhotoImage(image=im)
return imgtk
def shift_convert(self, action: Action,
images: List[OrthographicImage]) -> None:
image = images[0]
mat_area_image = get_area_of_interest_new(image,
action.pose,
border_color=np.nan).mat
mat_area_image = mat_area_image.astype(np.float32)
mat_area_image[mat_area_image ==
0] = np.nan # Make every not found pixel NaN
# Get distance at gripper for possible collisions
area_center = crop(mat_area_image,
(image.pixel_size * 0.01, image.pixel_size * 0.03))
z_raw = image.depth_from_value(np.nanmax(area_center))
z_raw += self.shift_z_offset
action.pose.z = -z_raw # [m] Move slightly up for gripper center point
def grasp_convert(self, action: Action,
images: List[OrthographicImage]) -> None:
image = images[0]
mat_area_image = get_area_of_interest_new(image,
action.pose,
border_color=np.nan,
project_3d=False,
flags=cv2.INTER_NEAREST).mat
mat_area_image = mat_area_image.astype(np.float32)
mat_area_image[mat_area_image < 10 *
255] = np.nan # Make every not found pixel NaN
# Get distance at gripper for possible collisions
gripper_one_side_size = 0.5 * image.pixel_size * (action.pose.d + 0.002
) # [px]
area_center = crop(
mat_area_image,
(image.pixel_size * 0.012, image.pixel_size * 0.012))
side_gripper_image_size = (image.pixel_size * 0.025,
image.pixel_size * 0.025)
area_left = crop(mat_area_image, side_gripper_image_size,
(-gripper_one_side_size, 0))
area_right = crop(mat_area_image, side_gripper_image_size,
(gripper_one_side_size, 0))
# Z is positive!
z_raw = image.depth_from_value(np.nanmedian(area_center))
if z_raw is np.NaN:
area_center = crop(
mat_area_image,
(image.pixel_size * 0.03, image.pixel_size * 0.03))
z_raw = image.depth_from_value(np.nanmedian(area_center))
z_raw_left = image.depth_from_value(np.nanmin(area_left))
z_raw_right = image.depth_from_value(np.nanmin(area_right))
z_raw += self.grasp_z_offset
z_raw_collision = min(z_raw_left, z_raw_right) - 0.008 # [m]
z_raw_max = min(
z_raw, z_raw_collision) # Get the maximum [m] for impedance mode
action.pose.z = -z_raw_max
def place_convert(self, action: Action,
images: List[OrthographicImage]) -> None:
image = images[0]
mat_area_image = get_area_of_interest_new(image,
action.pose,
border_color=np.nan).mat
mat_area_image = mat_area_image.astype(np.float32)
mat_area_image[mat_area_image ==
0] = np.nan # Make every not found pixel NaN
# Get distance at gripper for possible collisions
gripper_one_side_size = 0.5 * image.pixel_size * (action.pose.d + 0.002
) # [px]
area_center = crop(
mat_area_image,
(image.pixel_size * 0.025, image.pixel_size * 0.025))
side_gripper_image_size = (image.pixel_size * 0.025,
image.pixel_size * 0.025)
area_left = crop(mat_area_image, side_gripper_image_size,
(-gripper_one_side_size, 0))
area_right = crop(mat_area_image, side_gripper_image_size,
(gripper_one_side_size, 0))
z_raw = image.depth_from_value(np.nanmedian(area_center))
z_raw_left = image.depth_from_value(np.nanmin(area_left))
z_raw_right = image.depth_from_value(np.nanmin(area_right))
if z_raw is np.NaN:
area_center = crop(
mat_area_image,
(image.pixel_size * 0.022, image.pixel_size * 0.03))
z_raw = image.depth_from_value(np.nanmedian(area_center))
z_raw += self.place_z_offset
z_raw_collision = min(z_raw_left, z_raw_right) - 0.008 # [m]
# z_raw_max = min(z_raw, z_raw_collision) # Get the maximum [m] for impedance mode
z_raw_max = z_raw
action.pose.z = -z_raw_max # [m] Move slightly up for gripper center point
# draw_pose(image, action.pose, convert_to_rgb=True)
size_input = image.mat.shape[::-1]
size_cropped = (200, 200)
size_result = (32, 32)
scale = 4
image.mat = cv2.resize(image.mat,
(size_input[0] // scale, size_input[1] // scale))
image.pixel_size /= scale
s = time.time()
area_image = get_area_of_interest_new(
image,
action.pose,
size_cropped=(size_cropped[0] // scale, size_cropped[1] // scale),
size_result=size_result,
border_color=70)
print(time.time() - s)
# area_image = get_area_of_interest_new(
# image,
# action.pose,
# size_cropped=size_cropped,
# size_result=size_result,
# border_color=70
# )
cv2.imwrite(str(Path.home() / 'Documents' / 'bin_picking' / 'test.png'),
area_image.mat)
default='ed-v')
parser.add_argument('-m', '--model', dest='model', type=str)
parser.add_argument('-d', '--draw', action='store_true')
parser.add_argument('--area', action='store_true')
parser.add_argument('--convert', action='store_true')
parser.add_argument('--show', action='store_true')
args = parser.parse_args()
action, image = Loader.get_action(args.collection, args.episode,
args.action, args.suffix)
print('Action: ', action)
if args.area:
area_image = get_area_of_interest_new(image,
action.pose,
size_cropped=(200, 200))
if args.convert:
converter = Converter(grasp_z_offset=0.015, box=Config.box)
converter.grasp_convert(action, [image])
if args.show:
cv2.imshow('area_image', area_image.mat)
else:
if args.draw:
draw_around_box(image, box=Config.box)
draw_pose(image, action.pose, convert_to_rgb=True)
if args.show:
'ed-v')
action_place, image_place, image_goal = Loader.get_action(
collection, episode_id, 1, ['ed-v', 'ed-goal'])
draw_around_box(image_grasp, box=Config.box)
draw_around_box(image_place, box=Config.box)
draw_around_box(image_goal, box=Config.box)
pose_grasp = action_grasp.pose
# pose_grasp = RobotPose(Affine(x=-0.0053, y=0.0414, a=1.4708))
pose_place = action_place.pose
# pose_place = RobotPose(Affine(x=-0.0025, y=0.0563, a=-1.4708))
image_grasp_area = get_area_of_interest_new(image_grasp,
pose_grasp,
size_cropped=(200, 200),
size_result=(32, 32)).mat
image_place_area = get_area_of_interest_new(image_place,
pose_place,
size_cropped=(200, 200),
size_result=(32, 32)).mat
image_goal_area = get_area_of_interest_new(image_goal,
pose_place,
size_cropped=(200, 200),
size_result=(32, 32)).mat
g = combined_model.get_layer('grasp')
p = combined_model.get_layer('place')
m = combined_model.get_layer('merge')
grasp_output = ['reward_grasp', 'z_m0']