def __init__(self, **params):
self.model = Config.grasp_model
self.watch_for_model_modification = True
self.model_last_modified = Loader.get_model_path(
self.model).stat().st_mtime
self.monte_carlo = 40 if 'mc' in self.model[1] else None
self.with_types = 'types' in self.model[1]
self.output_layer = 'prob' if not self.with_types else ['prob', 'type']
self.inference = InferencePlanarPose(
model=Loader.get_model(self.model, output_layer=self.output_layer),
box=Config.box,
lower_random_pose=Config.lower_random_pose,
upper_random_pose=Config.upper_random_pose,
monte_carlo=self.monte_carlo,
with_types=self.with_types,
)
self.inference.keep_indixes = None
self.indexer = GraspIndexer(gripper_classes=Config.gripper_classes)
self.converter = Converter(grasp_z_offset=Config.grasp_z_offset,
box=Config.box)
# # self.indexer = GraspFinalDIndexer(gripper_classes=Config.gripper_classes, final_d_classes=[0.0, 0.035])
# self.indexer = LateralIndexer(
# angles=[(0, 0), (0.3, 0)],
# gripper_classes=[0.05, 0.07, 0.084],
# )
# self.converter = Converter(grasp_z_offset=Config.grasp_z_offset, box=Config.box)
self.reinfer_next_time = True # Always true in contrast to AgentPredict
def infer(self, images: List[OrthographicImage],
method: SelectionMethod) -> Action:
if self.monte_carlo: # Adapt monte carlo progress parameter s
epoch_in_database = Loader.get_episode_count(Config.grasp_database)
s_not_bounded = (epoch_in_database - 3500) * 1 / (4500 - 3500)
self.inference.current_s = max(min(s_not_bounded, 1.0), 0.0)
current_model_st_mtime = Loader.get_model_path(
self.model).stat().st_mtime
if self.watch_for_model_modification and current_model_st_mtime > self.model_last_modified + 0.5: # [s]
logger.warning(f'Reload model {self.model}.')
try:
self.inference.model = Loader.get_model(
self.model, output_layer=self.output_layer)
self.model_last_modified = Loader.get_model_path(
self.model).stat().st_mtime
except OSError:
logger.info('Could not load model, probabily file locked.')
if len(images) == 3:
images[2].mat = images[2].mat[:, :, ::-1] # BGR to RGB
action = self.inference.infer(images, method)
self.indexer.to_action(action)
estimated_reward_lower_than_threshold = action.estimated_reward < Config.bin_empty_at_max_probability
bin_empty = estimated_reward_lower_than_threshold and Epoch.selection_method_should_be_high(
method)
if bin_empty:
return Action('bin_empty', safe=1)
self.converter.calculate_pose(action, images)
return action
def test_heatmap(self):
_, image = Loader.get_action('cylinder-cube-1', '2019-03-26-09-51-08-827', 'ed-v')
if TEST_WITH_GPU:
model = Loader.get_model('cylinder-cube-1', 'model-6-arch-more-layer', output_layer='prob')
heatmapper = Heatmap(InferencePlanarPose, model, box=self.box)
heatmap = heatmapper.render(image)
imageio.imwrite(str(self.file_path / f'gen-heatmap.png'), heatmap)
def __init__(self, prediction_model):
self.grasp_model = Config.grasp_model
self.shift_model = Config.shift_model
self.with_types = 'types' in self.grasp_model[1]
self.output_layer = 'prob' if not self.with_types else ['prob', 'type']
self.grasp_inference = InferencePlanarPose(
Loader.get_model(self.grasp_model, output_layer=self.output_layer),
box=Config.box,
lower_random_pose=Config.lower_random_pose,
upper_random_pose=Config.upper_random_pose,
with_types=self.with_types,
input_uncertainty=True,
)
self.grasp_inference.keep_indixes = [0, 1]
self.grasp_indexer = GraspIndexer(
gripper_classes=Config.gripper_classes)
self.shift_inference = InferencePlanarPose(
Loader.get_model(self.shift_model, output_layer='prob'),
box=Config.box,
lower_random_pose=Config.lower_random_pose,
upper_random_pose=Config.upper_random_pose,
with_types=False,
)
self.shift_inference.a_space = np.linspace(
-3.0, 3.0, 26) # [rad] # Don't use a=0.0
self.shift_inference.size_original_cropped = (240, 240)
self.shift_indexer = ShiftIndexer(shift_distance=Config.shift_distance)
self.grasp_shift_indexer = GraspShiftIndexer(
gripper_classes=Config.gripper_classes,
shift_distance=Config.shift_distance,
)
self.converter = Converter(grasp_z_offset=Config.grasp_z_offset,
shift_z_offset=0.007,
box=Config.box) # [m]
self.prediction_model = prediction_model
self.monte_carlo = 20
self.actions_since_last_inference = 0
self.actions: List[Action] = []
self.output_path = Path.home() / 'Desktop'
self.reinfer_next_time = True
# First inference is slower
self.prediction_model.predict([
np.zeros((1, 64, 64, 1)),
np.zeros((1, 1, 1, 1)),
np.zeros((1, 1, 1)),
np.zeros((1, 1, 1, 8))
])
def test_agent_predict(self):
# 2019-03-11-14-56-07-284, 2019-03-14-11-26-17-352, 2019-03-12-16-14-54-658
_, image = Loader.get_action('cylinder-cube-1', '2019-03-11-14-56-07-284', 'ed-v')
if TEST_WITH_GPU:
prediction_model = Loader.get_model('cylinder-cube-1', 'predict-generator-3', custom_objects={'_one_hot': one_hot_gen(4)})
grasp_model = Loader.get_model('cylinder-cube-1', 'model-6-arch-more-layer', output_layer='prob')
shift_model = Loader.get_model('shifting', 'model-1', output_layer='prob')
agent = PredictAgent(prediction_model, grasp_model, shift_model)
agent.predict_actions([image], SelectionMethod.Top5, N=5, verbose=True)
def check_for_model_reload(self):
current_model_st_mtime = Loader.get_model_path(
self.model).stat().st_mtime
if self.watch_for_model_modification and current_model_st_mtime > self.model_last_modified + 0.5: # [s]
logger.warning(f'Reload model {self.model}.')
try:
self.inference.model = Loader.get_model(
self.model, output_layer=self.output_layer)
self.model_last_modified = Loader.get_model_path(
self.model).stat().st_mtime
except OSError:
logger.info('Could not load model, probabily file locked.')
def BuildArch(self):
graph = tf.Graph()
with graph.as_default():
self.dataset = Loader(self.cfg)
self.cfg.vocab_size = self.dataset.vocabsize+1
self.cfg.label_size = self.dataset.labelsize
self.placeholders()
self.CapsuleModel()
self.losses()
update_ops = tf.get_collection(tf.GraphKeys.UPDATE_OPS)
with tf.control_dependencies(update_ops):
self.optimizer = tf.train.AdamOptimizer(self.learning_rate)
self.train_op = self.optimizer.minimize(self.loss, name='train_op')
print('model builded')
return graph
def test_difference(self):
_, image1, image2 = Loader.get_action('cube-1',
'2018-10-22-23-42-52-096',
['ed-v', 'ed-after'])
diff = image_difference(image1, image2)
imageio.imwrite(str(self.file_path / f'gen-diff.png'), diff.mat)
def get_mean(episode):
_, image = Loader.get_action(episode[0], episode[1]['id'], 'ed-after')
if image is None:
return {'id': episode[1]['id'], 'mean': 1e6}
return {'id': episode[1]['id'], 'mean': np.mean(image.mat)}
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 infer(self, images: List[OrthographicImage], method: SelectionMethod,
**params) -> List[Action]:
if self.monte_carlo: # Adapt monte carlo progress parameter s
epoch_in_collection = Loader.get_episode_count(Config.collection)
s_not_bounded = (epoch_in_collection - 3500) * 1 / (4500 - 3500)
self.inference.current_s = max(min(s_not_bounded, 1.0), 0.0)
self.check_for_model_reload()
if len(images) == 3:
images[2].mat = images[2].mat[:, :, ::-1] # BGR to RGB
action = self.inference.infer(images, method)
self.indexer.to_action(action)
print(action, method)
estimated_reward_lower_than_threshold = action.estimated_reward < Config.bin_empty_at_max_probability
bin_empty = estimated_reward_lower_than_threshold and Epoch.selection_method_should_be_high(
method)
if bin_empty:
return [Action('bin_empty', safe=1)]
self.converter.calculate_pose(action, images)
return [action]
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 test_agent(self):
_, image = Loader.get_action('cylinder-cube-1', '2019-03-26-09-08-16-480', 'ed-v')
if TEST_WITH_GPU:
agent = Agent()
result = agent.infer([image], SelectionMethod.Max)
self.assertEqual(result.safe, True)
self.assertEqual(result.method, SelectionMethod.Max)
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(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 __init__(self):
# Parameters
self.grasp_model = rospy.get_param('graspro/grasp_model', 'graspro-v1')
self.gripper_classes = rospy.get_param('graspro/gripper_classes')
self.z_offset = rospy.get_param('graspro/z_offset', 0.0)
self.ensenso_depth = rospy.get_param('graspro/camera/ensenso_depth')
self.realsense_depth = rospy.get_param(
'graspro/camera/realsense_depth')
self.realsense_color = rospy.get_param(
'graspro/camera/realsense_color')
self.lower_random_pose = rospy.get_param('graspro/lower_random_pose',
[-0.1, -0.1, 0.0])
self.upper_random_pose = rospy.get_param('graspro/upper_random_pose',
[0.1, 0.1, 0.0])
self.box_center = rospy.get_param('graspro/bin_center', [0, 0, 0])
self.box_size = rospy.get_param('graspro/bin_size', False)
self.box = {'center': self.box_center, 'size': self.box_size}
self.publish_heatmap = rospy.get_param('graspro/publish_heatmap',
False)
# Inference
self.inference = PlanarInference(
model=Loader.get_model(self.grasp_model, output_layer='prob'),
box=self.box,
lower_random_pose=self.lower_random_pose,
upper_random_pose=self.upper_random_pose,
)
self.indexer = GraspIndexer(gripper_classes=self.gripper_classes)
self.converter = Converter(grasp_z_offset=self.z_offset,
box=self.box) # [m]
if self.publish_heatmap:
self.heatmapper = Heatmap(self.inference,
self.inference.model,
box=self.box)
self.heatmap_publisher = rospy.Publisher('graspro/heatmap')
self.bridge = CvBridge()
self.image_publisher = rospy.Publisher('graspro/pose_on_image',
Image,
queue_size=10)
s1 = rospy.Service('graspro/infer_grasp', InferGrasp, self.infer_grasp)
s2 = rospy.Service('graspro/estimate_reward_for_grasp',
EstimateRewardForGrasp,
self.estimate_reward_for_grasp)
rospy.spin()
def test_loader(self):
for suffix in ['ed-v', 'ed-side_b-0_400']:
action, image = Loader.get_action('cylinder-cube-1',
'2019-06-25-15-49-13-551',
suffix)
draw_around_box(image, box=Config.box)
imageio.imwrite(
str(self.file_path / f'gen-draw-around-box-{suffix}.png'),
image.mat)
self.assertEqual(image.mat.dtype, np.uint16)
self.assertEqual(image.pixel_size, 2000.0)
self.assertEqual(action.method, SelectionMethod.Prob)
def __init__(self):
self.grasp_inference = InferencePlanarPose(
model=Loader.get_model(Config.grasp_model, output_layer='prob'),
box=Config.box,
lower_random_pose=Config.lower_random_pose,
upper_random_pose=Config.upper_random_pose,
)
self.grasp_indexer = GraspIndexer(gripper_classes=Config.gripper_classes)
self.converter = Converter(grasp_z_offset=Config.grasp_z_offset, shift_z_offset=0.007, box=Config.box) # [m]
if Config.shift_objects:
self.shift_inference = InferencePlanarPose(
model=Loader.get_model(Config.shift_model, output_layer='prob'),
box=Config.box,
lower_random_pose=Config.lower_random_pose,
upper_random_pose=Config.upper_random_pose,
)
self.shift_inference.a_space = np.linspace(-3.0, 3.0, 26) # [rad] # Don't use a=0.0
self.shift_inference.size_original_cropped = (240, 240)
self.shift_indexer = ShiftIndexer(shift_distance=Config.shift_distance)
self.reinfer_next_time = True # Always true in contrast to AgentPredict
def get_episodes_between(cls,
collection: str,
lower_id: str,
upper_id: str = None,
grasp_success=False,
suffix=('ed-v', )):
query = {'id': {'$gte': lower_id}}
if upper_id:
query['id']['$lte'] = upper_id
if grasp_success:
query['actions.0.reward'] = 1
episodes = Loader.yield_episodes(collection, query=query)
return list((d, e['id'], 0, suffix) for d, e in episodes)
def api_upload_image():
database = flask.request.values.get('database')
episode_id = flask.request.values.get('id')
suffix = flask.request.values.get('suffix', default='v')
filepath = Loader.get_image_path(database, episode_id, suffix)
filepath.parent.mkdir(exist_ok=True, parents=True)
image_data = flask.request.data
if flask.request.files:
image_data = flask.request.files['file'].read()
image_buffer = np.fromstring(image_data, np.uint8)
image = cv2.imdecode(image_buffer, cv2.IMREAD_UNCHANGED)
cv2.imwrite(str(filepath), image)
return flask.jsonify(success=True)
def api_upload_image():
collection = flask.request.values.get('collection')
episode_id = flask.request.values.get('episode_id')
action_id = flask.request.values.get('action_id', type=int)
suffix = flask.request.values.get('suffix')
filepath = Loader.get_image_path(collection,
episode_id,
action_id,
suffix,
image_format='png')
filepath.parent.mkdir(exist_ok=True, parents=True)
image_data = flask.request.data
if flask.request.files:
image_data = flask.request.files['file'].read()
image_buffer = np.fromstring(image_data, np.uint8)
image = cv2.imdecode(image_buffer, cv2.IMREAD_UNCHANGED)
cv2.imwrite(str(filepath), image)
return flask.jsonify(success=True)
def print_before_after_image(episode_id: str):
action, before_image, after_image = Loader.get_action(
'shifting', episode_id, ['ed-v', 'ed-after'])
area_before_image = get_area_of_interest(before_image,
action.pose,
size_cropped=(300, 300),
size_result=(150, 150))
area_after_image = get_area_of_interest(after_image,
action.pose,
size_cropped=(300, 300))
white = [255 * 255] * 3
draw_line(area_before_image,
action.pose,
Affine(0, 0),
Affine(0.036, 0),
color=white,
thickness=2)
draw_line(area_before_image,
action.pose,
Affine(0.036, 0.0),
Affine(0.026, -0.008),
color=white,
thickness=2)
draw_line(area_before_image,
action.pose,
Affine(0.036, 0.0),
Affine(0.026, 0.008),
color=white,
thickness=2)
cv2.imwrite(
str(Path.home() / 'Desktop' / f'example-{episode_id}-before.png'),
area_before_image.mat)
cv2.imwrite(
str(Path.home() / 'Desktop' / f'example-{episode_id}-after.png'),
area_after_image.mat)
print('---')
print(episode_id)
def __init__(self,
databases: Union[str, List[str]],
validation_databases: Union[str, List[str]] = None,
indexer=None):
validation_databases = validation_databases or []
self.databases = [databases] if isinstance(databases,
str) else databases
self.validation_databases = [validation_databases] if isinstance(
validation_databases, str) else validation_databases
self.output_path = Loader.get_database_path(self.databases[0])
self.image_output_path = self.output_path / 'input'
self.model_path = self.output_path / 'models'
self.result_path = self.output_path / 'results'
self.indexer = indexer if indexer else GraspIndexer(
gripper_classes=Config.gripper_classes)
self.box = Config.box
self.percent_validation_set = 0.2
def save_episode(predictor, database, episode_id, reward=1, action_type=1):
action, image, image_after = Loader.get_action(database, episode_id,
['ed-v', 'ed-after'])
draw_around_box(image, box=Config.box)
draw_around_box(image_after, box=Config.box)
# background_color = image.value_from_depth(get_distance_to_box(image, Config.box))
area = get_area_of_interest(image,
action.pose,
size_cropped=(256, 256),
size_result=predictor.size)
area_after = get_area_of_interest(image_after,
action.pose,
size_cropped=(256, 256),
size_result=predictor.size)
result = predictor.predict(area,
reward=reward,
action_type=action_type,
sampling=True,
number=20)
save_dir = Path.home() / 'Desktop' / 'predict-examples' / episode_id
save_dir.mkdir(exist_ok=True)
cv2.imwrite(str(save_dir / f'{predictor.name}_s_bef.png'), area.mat)
cv2.imwrite(str(save_dir / f'{predictor.name}_s_aft.png'), area_after.mat)
cv2.imwrite(str(save_dir / f'{predictor.name}_result.png'),
result[0] * 255)
if predictor.uncertainty:
result[result < 0.1] = np.nan
uncertainty = np.nanvar(result, axis=0)
uncertainty /= np.nanmax(uncertainty) * 0.25
uncertainty = np.clip(uncertainty * 255, 0, 255).astype(np.uint8)
uncertainty = cv2.applyColorMap(uncertainty, cv2.COLORMAP_JET)
cv2.imwrite(str(save_dir / f'{predictor.name}_unc.png'), uncertainty)
import time
from config import Config
from data.loader import Loader
if __name__ == '__main__':
parser = argparse.ArgumentParser(
description='Print summary of saved model.')
parser.add_argument('-m', '--model', dest='model', type=str, required=True)
parser.add_argument('--line-length',
dest='line_length',
type=int,
default=140)
args = parser.parse_args()
model = Loader.get_model(args.model)
model.summary(line_length=args.line_length)
if 'placing' in args.model:
model.get_layer('grasp').summary(line_length=args.line_length)
model.get_layer('place').summary(line_length=args.line_length)
model.get_layer('merge').summary(line_length=args.line_length)
print('Grasp Dropout',
model.get_layer('grasp').get_layer('dropout_6').rate)
print('Place Dropout',
model.get_layer('place').get_layer('dropout_21').rate)
print('Merge Dropout',
model.get_layer('merge').get_layer('dropout_24').rate)
def post(self):
self.response.headers['Content-Type'] = 'text/plain'
loader = Loader(self.response.out.write)
loader.load_data(self.request.get('in'))
import sys
from flask import Flask, request, jsonify
from flask.templating import render_template
from data.loader import Loader
from web.column_stat import Stat
if len(sys.argv) < 2:
print("Usage: python main.py path-to-data-file")
sys.exit(1)
app = Flask(__name__)
df = Loader.load(sys.argv[1])
def get_options(key, value=None):
return [
k.split(':')[1]
for k in request.form
if k.startswith(key + ':') and
(value is None or request.form[k] == value)
]
@app.route('/', methods=['GET'])
def index():
return render_template('index.html', **{
'columns': sorted([
Stat(df, col) for col in df.columns
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 pathlib import Path
import time
import cv2
from config import Config
from data.loader import Loader
from utils.image import draw_around_box, draw_pose, get_area_of_interest_new
if __name__ == '__main__':
lateral = False
suffix = 'ed-lateral_b-0_400' if lateral else 'ed-v'
action, image = Loader.get_action('placing-3', '2019-12-12-16-07-12-857',
0, 'ed-v')
# image = image.translate((0.0, 0.0, 0.05))
# image = image.rotate_x(-0.3, (0.0, 0.25))
draw_around_box(image, box=Config.box)
# 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()
from actions.indexer import GraspIndexer
from config import Config
from data.loader import Loader
from inference.planar import InferencePlanarPose
from picking.image import draw_pose
from picking.param import SelectionMethod
if __name__ == '__main__':
# inference = InferencePlanarPose(
# Loader.get_model('cylinder-cube-mc-1', 'model-1-mc', output_layer='prob'),
# box=Config.box,
# monte_carlo=160
# )
inference = InferencePlanarPose(
Loader.get_model('cylinder-cube-1',
'model-6-arch-more-layer',
output_layer='prob'),
box=Config.box,
)
# inference = InferencePlanarPose(
# Loader.get_model('shifting', 'model-3'),
# box=Config.box,
# )
_, image = Loader.get_action('cylinder-cube-mc-1',
'2019-07-02-13-27-22-246', 'ed-v')
indexer = GraspIndexer(gripper_classes=Config.gripper_classes)
converter = Converter(grasp_z_offset=Config.grasp_z_offset, box=Config.box)
import os
from agents.agent import Agent
from data.loader import Loader
os.environ['CUDA_DEVICE_ORDER'] = 'PCI_BUS_ID'
os.environ['CUDA_VISIBLE_DEVICES'] = str(1)
agent = Agent()
data = []
for i, (d, e) in enumerate(Loader.yield_episodes('cylinder-cube-mc-1')):
action, image = Loader.get_action(d, e['id'], 'ed-v')
if not hasattr(action, 'estimated_reward'):
continue
data.append({
'id': e['id'],
# 'old': action.estimated_reward,
'new': agent.reward_for_action([image], action),
'reward': action.reward
})
sorted_data = sorted(data, key=lambda k: -abs(k['reward'] - k['new']))
for i, e in enumerate(sorted_data[:20]):
print(i, e)