def load_image(self, collection, episode_id, action_id, suffix):
image = Loader.get_image(collection, episode_id, action_id, suffix, as_float=True)
draw_around_box(image, box=Config.box)
image.mat = cv2.resize(image.mat, (self.size_input[0] // self.size_memory_scale, self.size_input[1] // self.size_memory_scale))
image.pixel_size /= self.size_memory_scale
return image
def test(self, collection, episode_id):
grasp = (Loader.get_image(collection, episode_id, 0, 'ed-v').mat /
255).astype(np.uint8)
place = (Loader.get_image(collection, episode_id, 1, 'ed-v').mat /
255).astype(np.uint8)
goal = (Loader.get_image(collection, episode_id, 0, 'ed-goal').mat /
255).astype(np.uint8)
grasp_c = cv2.cvtColor(grasp, cv2.COLOR_GRAY2RGB)
goal_c = cv2.cvtColor(goal, cv2.COLOR_GRAY2RGB)
# Difference
diff = cv2.absdiff(place, goal)
diff[:80, :] = 0
diff[-80:, :] = 0
diff[:, :80] = 0
diff[:, -80:] = 0
# Find contours
ret, thresh = cv2.threshold(diff, 20, 255, 0)
contours, hierarchy = cv2.findContours(thresh, cv2.RETR_TREE,
cv2.CHAIN_APPROX_NONE)
print('Number contours: ', len(contours))
cv2.drawContours(goal_c, contours, -1, (255, 0, 0))
# Bounding rect of largest area
c = max(contours, key=cv2.contourArea)
x, y, w, h = cv2.boundingRect(c)
cv2.rectangle(goal_c, (x, y), (x + w, y + h), (0, 255, 0), 2)
# Template matching
template = goal[y:y + h, x:x + w]
res = cv2.matchTemplate(grasp, template, cv2.TM_CCOEFF)
min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(res)
top_left = max_loc
bottom_right = (top_left[0] + w, top_left[1] + h)
cv2.rectangle(grasp_c, top_left, bottom_right, (0, 0, 255), 1)
cv2.imshow('grasp', grasp_c)
cv2.imshow('goal', goal_c)
cv2.waitKey(2000)
def api_image(collection_name: str, episode_id: str, action_id: str,
suffix: str):
def send_image(image):
_, image_encoded = cv2.imencode('.jpg', image)
return flask.send_file(io.BytesIO(image_encoded),
mimetype='image/jpeg')
def send_empty_image():
empty = np.zeros((480, 752, 1))
cv2.putText(empty,
'?', (310, 300),
cv2.FONT_HERSHEY_SIMPLEX,
6,
100,
thickness=6)
return send_image(empty)
if flask.request.values.get('pose'):
action = Action(data=json.loads(flask.request.values.get('pose')))
image = Loader.get_image(collection_name,
episode_id,
int(action_id),
suffix,
images=action.images)
else:
try:
action, image = Loader.get_action(collection_name, episode_id,
int(action_id), suffix)
except Exception:
app.logger.warn('Could not find image:', collection_name,
episode_id, action_id, suffix)
return send_empty_image()
if suffix not in action.images.keys():
app.logger.warn(
f'Could not find suffix {collection_name}-{episode_id}-{action_id}-{suffix}'
)
return send_empty_image()
draw_pose(image, action.pose, convert_to_rgb=True)
# draw_pose(image, action.pose, convert_to_rgb=True, reference_pose=action.images[suffix]['pose'])
if flask.request.values.get('box', default=0, type=int):
draw_around_box(image, box=Config.box, draw_lines=True)
return send_image(image.mat / 255)
def api_image(episode_id):
def send_image(image):
_, image_encoded = cv2.imencode('.jpg', image)
return flask.send_file(io.BytesIO(image_encoded),
mimetype='image/jpeg')
def send_empty_image():
empty = np.zeros((480, 752, 1))
cv2.putText(empty,
'?', (310, 300),
cv2.FONT_HERSHEY_SIMPLEX,
6,
100,
thickness=6)
return send_image(empty)
database_name = flask.request.values.get('database')
suffix = flask.request.values.get('suffix')
if flask.request.values.get('pose'):
action = Action(data=json.loads(flask.request.values.get('pose')))
image = Loader.get_image(database_name,
episode_id,
suffix,
images=action.images)
else:
action, image = Loader.get_action(database_name, episode_id, suffix)
if not action or suffix not in action.images.keys() or not image:
return send_empty_image()
draw_pose(image,
action.pose,
convert_to_rgb=True,
reference_pose=action.images['ed-v']['pose'])
if int(flask.request.values.get('box', default=0)):
draw_around_box(image, box=Config.box, draw_lines=True)
return send_image(image.mat / 255)
def manipulate(self) -> None:
current_bin_episode = None
goal_images = None
for epoch in Config.epochs:
while self.history.total() < epoch.number_episodes:
current_episode = Episode()
current_bin_episode = current_bin_episode if current_bin_episode else current_episode.id
current_selection_method = self.get_current_selection_method(
epoch)
start = time.time()
place_action_in_other_bin = Config.release_in_other_bin and not Config.release_during_grasp_action
place_bin = Frames.get_next_bin(
self.current_bin
) if place_action_in_other_bin else self.current_bin
if (not Config.predict_images) or self.agent.reinfer_next_time:
self.robot.recover_from_errors()
if not place_action_in_other_bin or Config.take_after_images:
self.robot.move_joints(
Frames.bin_joint_values[self.current_bin], self.md)
b, c = random.choice(
Config.overview_image_angles
) if Config.lateral_overview_image else 0, 0
camera_frame_overview = Frames.get_camera_frame(
self.current_bin, b=b, c=c)
if not Frames.is_camera_frame_safe(camera_frame_overview):
continue
if place_action_in_other_bin:
self.robot.move_cartesian(
Frames.camera,
Frames.get_camera_frame(place_bin, b=b, c=c),
self.md)
elif Config.take_goal_images:
self.robot.move_cartesian(Frames.camera,
camera_frame_overview,
self.md)
if Config.take_goal_images:
new_goal_images = self.take_goal_images(
current_bin=place_bin,
current_goal_images=goal_images)
goal_images = new_goal_images if new_goal_images else goal_images
elif Config.use_goal_images:
attr = random.choice(
GoalDatabase.get(Config.goal_images_dataset))
goal_images = [
Loader.get_image(attr[0], attr[1], attr[2], s)
for s in attr[3]
]
if place_action_in_other_bin:
place_image_frame = self.robot.current_pose(
Frames.camera)
place_images = self.take_images(
image_frame=place_image_frame,
current_bin=place_bin)
if Config.mode is Mode.Measure or Config.lateral_overview_image:
self.robot.move_cartesian(Frames.camera,
camera_frame_overview,
self.md)
image_frame = self.robot.current_pose(Frames.camera)
images = self.take_images(image_frame=image_frame)
if not Frames.is_gripper_frame_safe(
self.robot.current_pose(Frames.gripper)):
logger.info('Image frame not safe!')
self.robot.recover_from_errors()
continue
input_images = self.get_input_images(images)
input_place_images = self.get_input_images(
place_images) if place_action_in_other_bin else None
input_goal_images = None
if Config.use_goal_images:
if isinstance(goal_images, list) and isinstance(
goal_images[0], list):
goal_images_single = goal_images.pop(0)
else:
goal_images_single = goal_images
input_goal_images = self.get_input_images(
goal_images_single)
actions = self.agent.infer(
input_images,
current_selection_method,
goal_images=input_goal_images,
place_images=input_place_images,
)
for action_id, action in enumerate(actions):
logger.info(
f'Action ({action_id+1}/{len(actions)}): {action}')
for action_id, action in enumerate(actions):
action.images = {}
action.save = True
action.bin = self.current_bin
action.bin_episode = current_bin_episode
current_action_place_in_other_bin = place_action_in_other_bin and action.type == 'place'
current_image_pose = place_image_frame if current_action_place_in_other_bin else image_frame
current_bin = place_bin if current_action_place_in_other_bin else self.current_bin
if Config.mode is Mode.Measure:
before_images = place_images if current_action_place_in_other_bin else images
self.saver.save_image(before_images,
current_episode.id,
action_id,
'v',
action=action)
if Config.use_goal_images:
self.saver.save_image(goal_images_single,
current_episode.id,
action_id,
'goal',
action=action)
self.saver.save_action_plan(action, current_episode.id)
logger.info(
f'Executing action: {action_id} at time {time.time() - self.overall_start:0.1f}'
)
if Config.set_zero_reward:
action.safe = -1
execute_action = True
if action.type == 'bin_empty':
action.save = False
execute_action = False
elif action.type == 'new_image':
action.save = False
execute_action = False
self.agent.reinfer_next_time = True
if action.safe <= 0:
execute_action = False
action.collision = True
# Set actions after this action to unsafe
for a in actions[action_id + 1:]:
a.safe = action.safe
reason = 'not within box' if action.safe == -1 else 'not a number'
logger.warning(
f'Action (type={action.type}) is {reason} (safe={action.safe}).'
)
if action.safe == 0 and action.type in [
'grasp', 'shift'
]:
logger.warning(f'Episode is not saved.')
current_episode.save = False
break
if action.type == 'place' and action_id > 0:
prior_action = actions[action_id - 1]
if prior_action.type == 'grasp' and prior_action.reward > 0:
central_pose = RobotPose(
affine=Affine(z=-0.28), d=action.pose.d)
action_frame = Frames.get_action_pose(
action_pose=central_pose,
image_pose=current_image_pose)
self.place(current_episode, action_id, action,
action_frame, current_image_pose)
# Dont place if grasp was not successful
if action.type == 'place' and action_id > 0:
prior_action = actions[action_id - 1]
if prior_action.type == 'grasp' and (
prior_action.reward == 0
or prior_action.safe < 1):
execute_action = False
if Config.take_lateral_images and action.save and Config.mode is Mode.Measure:
md_lateral = MotionData().with_dynamics(1.0)
for b, c in Config.lateral_images_angles:
lateral_frame = Frames.get_camera_frame(
current_bin,
a=action.pose.a,
b=b,
c=c,
reference_pose=image_frame)
if not Frames.is_camera_frame_safe(
lateral_frame) or (b == 0.0 and c == 0.0):
continue
lateral_move_succss = self.robot.move_cartesian(
Frames.camera, lateral_frame,
md_lateral) # Remove a for global b, c pose
if lateral_move_succss:
self.saver.save_image(
self.take_images(current_bin=current_bin),
current_episode.id,
action_id,
f'lateral_b{b:0.3f}_c{c:0.3f}'.replace(
'.', '_'),
action=action)
if execute_action:
action_frame = Frames.get_action_pose(
action_pose=action.pose,
image_pose=current_image_pose)
if Config.mode is Mode.Measure and Config.take_direct_images:
self.robot.move_cartesian(
Frames.camera,
Affine(z=0.308) * Affine(b=0.0, c=0.0) *
action_frame)
self.saver.save_image(
self.take_images(current_bin=current_bin),
current_episode.id,
action_id,
'direct',
action=action)
if action.type == 'grasp':
self.grasp(current_episode, action_id, action,
action_frame, current_image_pose)
if Config.use_goal_images and self.last_after_images and not place_action_in_other_bin: # Next action is Place
place_action_id = action_id + 1
actions[
place_action_id].pose.d = self.gripper.width(
) # Use current gripper width for safety analysis
self.agent.converter.calculate_pose(
actions[place_action_id],
self.last_after_images)
elif action.type == 'shift':
old_reward_around_action = 0.0
self.shift(current_episode, action_id, action,
action_frame, current_image_pose)
new_reward_around_action = 0.0
action.reward = new_reward_around_action - old_reward_around_action
elif action.type == 'place':
self.place(current_episode,
action_id,
action,
action_frame,
current_image_pose,
place_bin=place_bin)
action.reward = actions[action_id - 1].reward
else:
if Config.take_after_images:
self.robot.move_cartesian(Frames.camera,
current_image_pose,
self.md)
self.saver.save_image(
self.take_images(current_bin=current_bin),
current_episode.id,
action_id,
'after',
action=action)
action.execution_time = time.time() - start
logger.info(
f'Time for action: {action.execution_time:0.3f} [s]')
if action.save:
current_episode.actions.append(action)
self.history.append(current_episode)
else:
break
logger.info(
f'Episodes (reward / done / total): {self.history.total_reward(action_type="grasp")} / {self.history.total()} / {sum(e.number_episodes for e in Config.epochs)}'
)
logger.info(
f'Last success: {self.history.failed_grasps_since_last_success_in_bin(self.current_bin)} cycles ago.'
)
# history.save_grasp_rate_prediction_step_evaluation(Config.evaluation_path)
# Switch bin
should_change_bin_for_evaluation = (
Config.mode is Mode.Evaluate
and self.history.successful_grasps_in_bin(self.current_bin)
== Config.change_bin_at_number_of_success_grasps)
should_change_bin = (
Config.mode is not Mode.Evaluate
and (self.history.failed_grasps_since_last_success_in_bin(
self.current_bin) >=
Config.change_bin_at_number_of_failed_grasps
or action.type == 'bin_empty'))
if should_change_bin_for_evaluation or (Config.change_bins
and should_change_bin):
if Config.mode is Mode.Evaluate:
pass
# history.save_grasp_rate_prediction_step_evaluation(Config.evaluation_path)
self.current_bin = Frames.get_next_bin(self.current_bin)
self.agent.reinfer_next_time = True
current_bin_episode = None
logger.info('Switch to other bin.')
if Config.mode is not Mode.Perform and Config.home_gripper:
self.gripper.homing()
if Config.mode is Mode.Measure and current_episode.actions and current_episode.save:
logger.info(f'Save episode {current_episode.id}.')
self.saver.save_episode(current_episode)
# Retrain model
if Config.train_model and self.history.total(
) > 0 and not self.history.total(
) % Config.train_model_every_number_cycles:
logger.warning('Retrain model!')
self.retrain_model()
logger.info('Finished cleanly.')
from config import Config
from data.loader import Loader
from utils.image import draw_pose
from utils.param import SelectionMethod
if __name__ == '__main__':
agent_place = Agent(use_goal_images=True)
if not Config.use_goal_images:
raise Exception(f'Does not use goal images!')
collection = 'placing-3'
episode_id = '2020-02-04-00-34-54-455'
if agent_place.network_type == '1':
image = Loader.get_image(collection, episode_id, 0, 'ed-v')
image_goal = Loader.get_image(collection, episode_id, 0, 'ed-goal')
actions = agent_place.infer([image], SelectionMethod.Max, [image_goal])
elif agent_place.network_type == '2':
image = Loader.get_image(collection, episode_id, 0, 'ed-v')
image_place = Loader.get_image(collection, episode_id, 1, 'ed-v')
image_goal = Loader.get_image(collection, episode_id, 1, 'ed-goal')
actions = agent_place.infer([image], SelectionMethod.Max, [image_goal],
[image_place])
actions = agent_place.infer([image], SelectionMethod.Max, [image_goal],
[image_place])
print(
def get_image(self, database: str, episode_id: str, suffix: str):
image = Loader.get_image(database, episode_id, suffix)
if image is None:
print(f'File not found: {database}, {episode_id}, {suffix}')
return image