From 5be9fa9ff2cdd28b52acc1f0e84ddd32eafeaf34 Mon Sep 17 00:00:00 2001 From: Heeyeon Kwon Date: Tue, 6 Aug 2019 22:15:55 -0400 Subject: [PATCH 01/12] Added stack_classifier_data_gen.py to get data to train image classifier --- logger_for_classifier.py | 119 ++++++++++++++++++++++++++++++++++ stack_classifier_data_gen.py | 122 +++++++++++++++++++++++++++++++++++ 2 files changed, 241 insertions(+) create mode 100755 logger_for_classifier.py create mode 100644 stack_classifier_data_gen.py diff --git a/logger_for_classifier.py b/logger_for_classifier.py new file mode 100755 index 00000000..f20f2dcc --- /dev/null +++ b/logger_for_classifier.py @@ -0,0 +1,119 @@ +import time +import datetime +import os +import numpy as np +import cv2 +import torch +# import h5py + +class Logger(): + + def __init__(self, continue_logging, logging_directory): + + # Create directory to save data + timestamp = time.time() + timestamp_value = datetime.datetime.fromtimestamp(timestamp) + self.continue_logging = continue_logging + if self.continue_logging: + self.base_directory = logging_directory + print('Pre-loading data logging session: %s' % (self.base_directory)) + else: + self.base_directory = os.path.join(logging_directory, timestamp_value.strftime('%Y-%m-%d.%H:%M:%S')) + print('Creating data logging session: %s' % (self.base_directory)) + self.info_directory = os.path.join(self.base_directory, 'info') + self.color_images_directory = os.path.join(self.base_directory, 'data', 'color-images') + self.depth_images_directory = os.path.join(self.base_directory, 'data', 'depth-images') + self.color_heightmaps_directory = os.path.join(self.base_directory, 'data', 'color-heightmaps') + self.depth_heightmaps_directory = os.path.join(self.base_directory, 'data', 'depth-heightmaps') + self.models_directory = os.path.join(self.base_directory, 'models') + self.visualizations_directory = os.path.join(self.base_directory, 'visualizations') + self.recordings_directory = os.path.join(self.base_directory, 'recordings') + self.transitions_directory = os.path.join(self.base_directory, 'transitions') + + if not os.path.exists(self.info_directory): + os.makedirs(self.info_directory) + if not os.path.exists(self.color_images_directory): + os.makedirs(self.color_images_directory) + if not os.path.exists(self.depth_images_directory): + os.makedirs(self.depth_images_directory) + if not os.path.exists(self.color_heightmaps_directory): + os.makedirs(self.color_heightmaps_directory) + if not os.path.exists(self.depth_heightmaps_directory): + os.makedirs(self.depth_heightmaps_directory) + if not os.path.exists(self.models_directory): + os.makedirs(self.models_directory) + if not os.path.exists(self.visualizations_directory): + os.makedirs(self.visualizations_directory) + if not os.path.exists(self.recordings_directory): + os.makedirs(self.recordings_directory) + if not os.path.exists(self.transitions_directory): + os.makedirs(os.path.join(self.transitions_directory, 'data')) + + def save_camera_info(self, intrinsics, pose, depth_scale): + np.savetxt(os.path.join(self.info_directory, 'camera-intrinsics.txt'), intrinsics, delimiter=' ') + np.savetxt(os.path.join(self.info_directory, 'camera-pose.txt'), pose, delimiter=' ') + np.savetxt(os.path.join(self.info_directory, 'camera-depth-scale.txt'), [depth_scale], delimiter=' ') + + def save_heightmap_info(self, boundaries, resolution): + np.savetxt(os.path.join(self.info_directory, 'heightmap-boundaries.txt'), boundaries, delimiter=' ') + np.savetxt(os.path.join(self.info_directory, 'heightmap-resolution.txt'), [resolution], delimiter=' ') + + def save_images(self, iteration, color_image, depth_image, mode): + color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR) + cv2.imwrite(os.path.join(self.color_images_directory, '%06d.%s.color.png' % (iteration, mode)), color_image) + depth_image = np.round(depth_image * 10000).astype(np.uint16) # Save depth in 1e-4 meters + cv2.imwrite(os.path.join(self.depth_images_directory, '%06d.%s.depth.png' % (iteration, mode)), depth_image) + + def save_images_stack(self, iteration, color_image, depth_image, stack_class): + color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR) + cv2.imwrite(os.path.join(self.color_images_directory, '%06d.%s.color.png' % (iteration, stack_class)), color_image) + depth_image = np.round(depth_image * 10000).astype(np.uint16) # Save depth in 1e-4 meters + cv2.imwrite(os.path.join(self.depth_images_directory, '%06d.%s.depth.png' % (iteration, stack_class)), depth_image) + + def save_heightmaps(self, iteration, color_heightmap, depth_heightmap, mode): + color_heightmap = cv2.cvtColor(color_heightmap, cv2.COLOR_RGB2BGR) + cv2.imwrite(os.path.join(self.color_heightmaps_directory, '%06d.%s.color.png' % (iteration, mode)), color_heightmap) + depth_heightmap = np.round(depth_heightmap * 100000).astype(np.uint16) # Save depth in 1e-5 meters + cv2.imwrite(os.path.join(self.depth_heightmaps_directory, '%06d.%s.depth.png' % (iteration, mode)), depth_heightmap) + + + def save_heightmaps_stack(self, iteration, color_heightmap, depth_heightmap, stack_class): + color_heightmap = cv2.cvtColor(color_heightmap, cv2.COLOR_RGB2BGR) + cv2.imwrite(os.path.join(self.color_heightmaps_directory, '%06d.%s.color.png' % (iteration, stack_class)), color_heightmap) + depth_heightmap = np.round(depth_heightmap * 100000).astype(np.uint16) # Save depth in 1e-5 meters + cv2.imwrite(os.path.join(self.depth_heightmaps_directory, '%06d.%s.depth.png' % (iteration, stack_class)), depth_heightmap) + + def write_to_log(self, log_name, log): + np.savetxt(os.path.join(self.transitions_directory, '%s.log.txt' % log_name), log, delimiter=' ') + + def save_model(self, model, name): + torch.save(model.cpu().state_dict(), os.path.join(self.models_directory, 'snapshot.%s.pth' % (name))) + + def save_backup_model(self, model, name): + torch.save(model.state_dict(), os.path.join(self.models_directory, 'snapshot-backup.%s.pth' % (name))) + + def save_visualizations(self, iteration, affordance_vis, name): + cv2.imwrite(os.path.join(self.visualizations_directory, '%06d.%s.png' % (iteration,name)), affordance_vis) + + # def save_state_features(self, iteration, state_feat): + # h5f = h5py.File(os.path.join(self.visualizations_directory, '%06d.state.h5' % (iteration)), 'w') + # h5f.create_dataset('state', data=state_feat.cpu().data.numpy()) + # h5f.close() + + # Record RGB-D video while executing primitive + # recording_directory = logger.make_new_recording_directory(iteration) + # camera.start_recording(recording_directory) + # camera.stop_recording() + def make_new_recording_directory(self, iteration): + recording_directory = os.path.join(self.recordings_directory, '%06d' % (iteration)) + if not os.path.exists(recording_directory): + os.makedirs(recording_directory) + return recording_directory + + def save_transition(self, iteration, transition): + depth_heightmap = np.round(transition.state * 100000).astype(np.uint16) # Save depth in 1e-5 meters + cv2.imwrite(os.path.join(self.transitions_directory, 'data', '%06d.0.depth.png' % (iteration)), depth_heightmap) + next_depth_heightmap = np.round(transition.next_state * 100000).astype(np.uint16) # Save depth in 1e-5 meters + cv2.imwrite(os.path.join(self.transitions_directory, 'data', '%06d.1.depth.png' % (iteration)), next_depth_heightmap) + # np.savetxt(os.path.join(self.transitions_directory, '%06d.action.txt' % (iteration)), [1 if (transition.action == 'grasp') else 0], delimiter=' ') + # np.savetxt(os.path.join(self.transitions_directory, '%06d.reward.txt' % (iteration)), [reward_value], delimiter=' ') diff --git a/stack_classifier_data_gen.py b/stack_classifier_data_gen.py new file mode 100644 index 00000000..2caa8c83 --- /dev/null +++ b/stack_classifier_data_gen.py @@ -0,0 +1,122 @@ +import time +import os +import random +import threading +import argparse +import matplotlib.pyplot as plt +import numpy as np +import scipy as sc +import cv2 +from collections import namedtuple +import torch +from torch.autograd import Variable +from robot import Robot +from trainer import Trainer +from logger_for_classifier import Logger +import utils +from main import StackSequence + + +############### Testing Block Stacking ####### +is_sim = True# Run in simulation? +obj_mesh_dir = os.path.abspath('objects/blocks') if is_sim else None # Directory containing 3D mesh files (.obj) of objects to be added to simulation +num_obj = 4 if is_sim else None # Number of objects to add to simulation +tcp_host_ip = args.tcp_host_ip if not is_sim else None # IP and port to robot arm as TCP client (UR5) +tcp_port = args.tcp_port if not is_sim else None +rtc_host_ip = args.rtc_host_ip if not is_sim else None # IP and port to robot arm as real-time client (UR5) +rtc_port = args.rtc_port if not is_sim else None +if is_sim: + workspace_limits = np.asarray([[-0.724, -0.276], [-0.224, 0.224], [-0.0001, 0.4]]) # Cols: min max, Rows: x y z (define workspace limits in robot coordinates) +else: + workspace_limits = np.asarray([[0.3, 0.748], [-0.224, 0.224], [-0.255, -0.1]]) # Cols: min max, Rows: x y z (define workspace limits in robot coordinates) +heightmap_resolution = 0.002 # Meters per pixel of heightmap +random_seed = 1234 +force_cpu = False + + +# -------------- Testing options -------------- +is_testing = False +max_test_trials = 1 # Maximum number of test runs per case/scenario +test_preset_cases = False +test_preset_file = os.path.abspath(args.test_preset_file) if test_preset_cases else None + +# ------ Pre-loading and logging options ------ +# load_snapshot = args.load_snapshot # Load pre-trained snapshot of model? +# snapshot_file = os.path.abspath(args.snapshot_file) if load_snapshot else None +continue_logging = False # Continue logging from previous session +logging_directory = 'logs_for_classifier' +logging_directory = os.path.abspath(logging_directory) if continue_logging else os.path.abspath('logs_for_classifier') +# save_visualizations = args.save_visualizations # Save visualizations of FCN predictions? Takes 0.6s per training step if set to True + +# Initialize data logger +logger = Logger(continue_logging, logging_directory) +# logger.save_camera_info(robot.cam_intrinsics, robot.cam_pose, robot.cam_depth_scale) # Save camera intrinsics and pose +# logger.save_heightmap_info(workspace_limits, heightmap_resolution) # Save heightmap parameters + +# Set random seed +np.random.seed(random_seed) +# Do we care about color? Switch to +# True to run a color order stacking test, +# False tests stacking order does not matter. +grasp_color_task = False +# are we doing a stack even if we don't care about colors +place_task = True + +robot = Robot(is_sim, obj_mesh_dir, num_obj, workspace_limits, + tcp_host_ip, tcp_port, rtc_host_ip, rtc_port, + is_testing, test_preset_cases, test_preset_file, + place=place_task, grasp_color_task=grasp_color_task) +stacksequence = StackSequence(num_obj, is_goal_conditioned_task=grasp_color_task or place_task) + +print('full stack sequence: ' + str(stacksequence.object_color_sequence)) +best_rotation_angle = 3.14 +blocks_to_move = num_obj - 1 +num_stacks = 10 +iteration = 0 +for stack in range(num_stacks): + print('++++++++++++++++++++++++++++++++++++++++++++++++++') + print('+++++++ Making New Stack ++++++++') + print('++++++++++++++++++++++++++++++++++++++++++++++++++') + for i in range(blocks_to_move): + print('----------------------------------------------') + stacksequence.next() + stack_goal = stacksequence.current_sequence_progress() + block_to_move = stack_goal[-1] + print('move block: ' + str(i) + ' current stack goal: ' + str(stack_goal)) + block_positions = robot.get_obj_positions_and_orientations()[0] + primitive_position = block_positions[block_to_move] + robot.grasp(primitive_position, best_rotation_angle, object_color=block_to_move) + block_positions = robot.get_obj_positions_and_orientations()[0] + base_block_to_place = stack_goal[-2] + primitive_position = block_positions[base_block_to_place] + place = robot.place(primitive_position, best_rotation_angle) + print('place ' + str(i) + ' : ' + str(place)) + # check if we don't care about color + if not grasp_color_task: + # Deliberately change the goal stack order to test the non-ordered check + stack_goal = np.random.permutation(stack_goal) + print('fake stack goal to test any stack order: ' + str(stack_goal)) + stack_success, height_count = robot.check_stack(stack_goal) + + ####################################### + stack_class = height_count - 1 + # Get latest RGB-D image + color_img, depth_img = robot.get_camera_data() + depth_img = depth_img * robot.cam_depth_scale # Apply depth scale from calibration + + # Get heightmap from RGB-D image (by re-projecting 3D point cloud) + color_heightmap, depth_heightmap = utils.get_heightmap(color_img, depth_img, robot.cam_intrinsics, robot.cam_pose, workspace_limits, heightmap_resolution) + valid_depth_heightmap = depth_heightmap.copy() + valid_depth_heightmap[np.isnan(valid_depth_heightmap)] = 0 + + # Save RGB-D images and RGB-D heightmaps + logger.save_images_stack(iteration, color_img, depth_img, stack_class) + logger.save_heightmaps_stack(iteration, color_heightmap, valid_depth_heightmap, stack_class) + ########################################### + + print('stack success part ' + str(i+1) + ' of ' + str(blocks_to_move) + ': ' + str(stack_success) + ':' + str(height_count) ) + iteration +=1 + # reset scene + robot.reposition_objects() + # determine first block to grasp + stacksequence.next() From ea68eabc15b5fc8ded3cc6b7ce94db453cff5f23 Mon Sep 17 00:00:00 2001 From: Heeyeon Kwon Date: Wed, 7 Aug 2019 00:10:39 -0400 Subject: [PATCH 02/12] Added stack_classifier_data_gen.py to get data to train image classifier --- logger_for_classifier.py | 8 ++++++++ stack_classifier_data_gen.py | 15 ++++++++++++--- 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/logger_for_classifier.py b/logger_for_classifier.py index f20f2dcc..83c70b8e 100755 --- a/logger_for_classifier.py +++ b/logger_for_classifier.py @@ -4,6 +4,7 @@ import numpy as np import cv2 import torch +import csv # import h5py class Logger(): @@ -85,6 +86,13 @@ def save_heightmaps_stack(self, iteration, color_heightmap, depth_heightmap, sta def write_to_log(self, log_name, log): np.savetxt(os.path.join(self.transitions_directory, '%s.log.txt' % log_name), log, delimiter=' ') + + def save_label(self, log_name, log): + np.savetxt(os.path.join(self.color_images_directory, '%s.log.txt' % log_name), log, delimiter=' ') + + def save_label_1(self, log_name, log): + with open(os.path.join(self.color_images_directory, '%s.txt' % log_name), 'w') as f: + csv.writer(f, delimiter=' ').writerows(log) def save_model(self, model, name): torch.save(model.cpu().state_dict(), os.path.join(self.models_directory, 'snapshot.%s.pth' % (name))) diff --git a/stack_classifier_data_gen.py b/stack_classifier_data_gen.py index 2caa8c83..d4bc6e5e 100644 --- a/stack_classifier_data_gen.py +++ b/stack_classifier_data_gen.py @@ -15,6 +15,7 @@ from logger_for_classifier import Logger import utils from main import StackSequence +import csv ############### Testing Block Stacking ####### @@ -71,8 +72,11 @@ print('full stack sequence: ' + str(stacksequence.object_color_sequence)) best_rotation_angle = 3.14 blocks_to_move = num_obj - 1 -num_stacks = 10 +num_stacks = 3400 iteration = 0 +labels = [] +data_num = 1 + for stack in range(num_stacks): print('++++++++++++++++++++++++++++++++++++++++++++++++++') print('+++++++ Making New Stack ++++++++') @@ -113,10 +117,15 @@ logger.save_images_stack(iteration, color_img, depth_img, stack_class) logger.save_heightmaps_stack(iteration, color_heightmap, valid_depth_heightmap, stack_class) ########################################### + filename = '%06d.%s.color.png' % (iteration, stack_class) + labels.append([filename,iteration,stack_class]) + print('stack success part ' + str(i+1) + ' of ' + str(blocks_to_move) + ': ' + str(stack_success) + ':' + str(height_count) +':' + str(stack_class)) + iteration += 1 - print('stack success part ' + str(i+1) + ' of ' + str(blocks_to_move) + ': ' + str(stack_success) + ':' + str(height_count) ) - iteration +=1 # reset scene robot.reposition_objects() # determine first block to grasp stacksequence.next() + +# save labels +logger.save_label_1('stack_label', labels) \ No newline at end of file From 324f3940262d9db81c7b141fc795ce861818290f Mon Sep 17 00:00:00 2001 From: Heeyeon Kwon Date: Sun, 11 Aug 2019 15:25:10 -0400 Subject: [PATCH 03/12] update saving text for stack_classifier_data_gen.py --- stack_classifier_data_gen.py | 30 +++++++++++++++++++++++++----- 1 file changed, 25 insertions(+), 5 deletions(-) diff --git a/stack_classifier_data_gen.py b/stack_classifier_data_gen.py index d4bc6e5e..fce4a9ab 100644 --- a/stack_classifier_data_gen.py +++ b/stack_classifier_data_gen.py @@ -45,7 +45,7 @@ # load_snapshot = args.load_snapshot # Load pre-trained snapshot of model? # snapshot_file = os.path.abspath(args.snapshot_file) if load_snapshot else None continue_logging = False # Continue logging from previous session -logging_directory = 'logs_for_classifier' +logging_directory = './logs_for_classifier/training' logging_directory = os.path.abspath(logging_directory) if continue_logging else os.path.abspath('logs_for_classifier') # save_visualizations = args.save_visualizations # Save visualizations of FCN predictions? Takes 0.6s per training step if set to True @@ -72,10 +72,18 @@ print('full stack sequence: ' + str(stacksequence.object_color_sequence)) best_rotation_angle = 3.14 blocks_to_move = num_obj - 1 -num_stacks = 3400 -iteration = 0 +num_stacks = 500 labels = [] data_num = 1 +continue_gen = True +if continue_gen == False: + iteration = 0 +else: + label_text = "./logs_for_classifier/training/data/color-images/stack_label.txt" + myfile = open(label_text, 'r') + myfiles = [line.split(' ') for line in myfile.readlines()] + iteration = 6033 + for stack in range(num_stacks): print('++++++++++++++++++++++++++++++++++++++++++++++++++') @@ -118,7 +126,16 @@ logger.save_heightmaps_stack(iteration, color_heightmap, valid_depth_heightmap, stack_class) ########################################### filename = '%06d.%s.color.png' % (iteration, stack_class) + if continue_gen: + + print(myfiles) + myfiles.append([filename,iteration,stack_class]) + # with open(label_text, "a") as myfile: + # title = [filename,iteration,stack_class] + # myfile.write('%d' % number) + #logger.save_label_1('stack_label', myfiles) labels.append([filename,iteration,stack_class]) + logger.save_label_1('stack_label', labels) print('stack success part ' + str(i+1) + ' of ' + str(blocks_to_move) + ': ' + str(stack_success) + ':' + str(height_count) +':' + str(stack_class)) iteration += 1 @@ -127,5 +144,8 @@ # determine first block to grasp stacksequence.next() -# save labels -logger.save_label_1('stack_label', labels) \ No newline at end of file +## save labels +#if continue_gen == False: +# logger.save_label_1('stack_label', labels) +#else: +# logger.save_label_1('stack_label_cont', labels) From 2762e8d5dfead4b948da8c9f01910f1678b20fe4 Mon Sep 17 00:00:00 2001 From: Heeyeon Kwon Date: Tue, 13 Aug 2019 22:40:11 -0400 Subject: [PATCH 04/12] fixed continue_generation parameter for data collection --- stack_classifier_data_gen.py | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/stack_classifier_data_gen.py b/stack_classifier_data_gen.py index fce4a9ab..d256f190 100644 --- a/stack_classifier_data_gen.py +++ b/stack_classifier_data_gen.py @@ -79,11 +79,10 @@ if continue_gen == False: iteration = 0 else: - label_text = "./logs_for_classifier/training/data/color-images/stack_label.txt" + label_text = "./logs_for_classifier/test/data/color-images/stack_label.txt" myfile = open(label_text, 'r') myfiles = [line.split(' ') for line in myfile.readlines()] - iteration = 6033 - + iteration = int(myfiles[-1][1]) +1 for stack in range(num_stacks): print('++++++++++++++++++++++++++++++++++++++++++++++++++') @@ -128,14 +127,22 @@ filename = '%06d.%s.color.png' % (iteration, stack_class) if continue_gen: - print(myfiles) - myfiles.append([filename,iteration,stack_class]) + with open(label_text,"a") as f: + f.writelines("\n") + filename = ['000005.0.color.png',' ', str(iteration),' ', str(stack_class)] + f.writelines(filename) + #opened_file.write("%r\n" %new_string) + f.close() + + # print(myfiles) + # myfiles.append([filename,iteration,stack_class]) # with open(label_text, "a") as myfile: # title = [filename,iteration,stack_class] # myfile.write('%d' % number) #logger.save_label_1('stack_label', myfiles) - labels.append([filename,iteration,stack_class]) - logger.save_label_1('stack_label', labels) + else: + labels.append([filename,iteration,stack_class]) + logger.save_label_1('stack_label', labels) print('stack success part ' + str(i+1) + ' of ' + str(blocks_to_move) + ': ' + str(stack_success) + ':' + str(height_count) +':' + str(stack_class)) iteration += 1 From 0cc1e6110cf81a2e9472fecea1277c232be32ce7 Mon Sep 17 00:00:00 2001 From: Heeyeon Kwon Date: Thu, 15 Aug 2019 22:18:38 -0400 Subject: [PATCH 05/12] modified continue logging parameter --- stack_classifier_data_gen.py | 41 +++++++++++++++++++++++++++--------- 1 file changed, 31 insertions(+), 10 deletions(-) diff --git a/stack_classifier_data_gen.py b/stack_classifier_data_gen.py index d256f190..149de51b 100644 --- a/stack_classifier_data_gen.py +++ b/stack_classifier_data_gen.py @@ -41,11 +41,30 @@ test_preset_cases = False test_preset_file = os.path.abspath(args.test_preset_file) if test_preset_cases else None +##################################################################### +## Modify +#continue_gen = True +split = 'test' #['train', 'val', 'test'] +## + +root = './logs_for_classifier' +logging_directory = os.path.join(root, split) +if split == 'train': + num_stacks = 33400 + max_iterations = 100000 +elif split == 'test' or split == 'val': + num_stacks = 3400 + max_iterations = 10000 + +labels = [] +data_num = 1 +##################################################################### + # ------ Pre-loading and logging options ------ # load_snapshot = args.load_snapshot # Load pre-trained snapshot of model? # snapshot_file = os.path.abspath(args.snapshot_file) if load_snapshot else None -continue_logging = False # Continue logging from previous session -logging_directory = './logs_for_classifier/training' +continue_logging = True # Continue logging from previous session +#logging_directory = './logs_for_classifier/training' logging_directory = os.path.abspath(logging_directory) if continue_logging else os.path.abspath('logs_for_classifier') # save_visualizations = args.save_visualizations # Save visualizations of FCN predictions? Takes 0.6s per training step if set to True @@ -72,11 +91,8 @@ print('full stack sequence: ' + str(stacksequence.object_color_sequence)) best_rotation_angle = 3.14 blocks_to_move = num_obj - 1 -num_stacks = 500 -labels = [] -data_num = 1 -continue_gen = True -if continue_gen == False: + +if continue_logging == False: iteration = 0 else: label_text = "./logs_for_classifier/test/data/color-images/stack_label.txt" @@ -124,13 +140,14 @@ logger.save_images_stack(iteration, color_img, depth_img, stack_class) logger.save_heightmaps_stack(iteration, color_heightmap, valid_depth_heightmap, stack_class) ########################################### + print(stack) filename = '%06d.%s.color.png' % (iteration, stack_class) - if continue_gen: + if continue_logging: with open(label_text,"a") as f: f.writelines("\n") - filename = ['000005.0.color.png',' ', str(iteration),' ', str(stack_class)] - f.writelines(filename) + name = [filename,' ', str(iteration),' ', str(stack_class)] + f.writelines(name) #opened_file.write("%r\n" %new_string) f.close() @@ -146,10 +163,14 @@ print('stack success part ' + str(i+1) + ' of ' + str(blocks_to_move) + ': ' + str(stack_success) + ':' + str(height_count) +':' + str(stack_class)) iteration += 1 + + # reset scene robot.reposition_objects() # determine first block to grasp stacksequence.next() + if iteration > max_iterations: + break ## save labels #if continue_gen == False: From bdffc6706cea7e135831cdbdf4e8d491f37d936a Mon Sep 17 00:00:00 2001 From: Andrew Hundt Date: Thu, 15 Aug 2019 22:49:59 -0400 Subject: [PATCH 06/12] parameters changed in stack_classifier_data_gen.py --- stack_classifier_data_gen.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/stack_classifier_data_gen.py b/stack_classifier_data_gen.py index 149de51b..15883bff 100644 --- a/stack_classifier_data_gen.py +++ b/stack_classifier_data_gen.py @@ -42,15 +42,15 @@ test_preset_file = os.path.abspath(args.test_preset_file) if test_preset_cases else None ##################################################################### -## Modify +## Modify #continue_gen = True -split = 'test' #['train', 'val', 'test'] +split = 'train' #['train', 'val', 'test'] ## root = './logs_for_classifier' logging_directory = os.path.join(root, split) if split == 'train': - num_stacks = 33400 + num_stacks = 33400 max_iterations = 100000 elif split == 'test' or split == 'val': num_stacks = 3400 @@ -63,7 +63,7 @@ # ------ Pre-loading and logging options ------ # load_snapshot = args.load_snapshot # Load pre-trained snapshot of model? # snapshot_file = os.path.abspath(args.snapshot_file) if load_snapshot else None -continue_logging = True # Continue logging from previous session +continue_logging = False # Continue logging from previous session #logging_directory = './logs_for_classifier/training' logging_directory = os.path.abspath(logging_directory) if continue_logging else os.path.abspath('logs_for_classifier') # save_visualizations = args.save_visualizations # Save visualizations of FCN predictions? Takes 0.6s per training step if set to True @@ -95,7 +95,8 @@ if continue_logging == False: iteration = 0 else: - label_text = "./logs_for_classifier/test/data/color-images/stack_label.txt" + #label_text = "./logs_for_classifier/test/data/color-images/stack_label.txt" + label_text = os.path.join(logging_directory, 'data','color-images', 'stack_label.txt') myfile = open(label_text, 'r') myfiles = [line.split(' ') for line in myfile.readlines()] iteration = int(myfiles[-1][1]) +1 @@ -146,7 +147,7 @@ with open(label_text,"a") as f: f.writelines("\n") - name = [filename,' ', str(iteration),' ', str(stack_class)] + name = [filename,' ', str(iteration),' ', str(stack_class)] f.writelines(name) #opened_file.write("%r\n" %new_string) f.close() From 4bb135ed28146c4fc2bf570e4de6a00e09e483e7 Mon Sep 17 00:00:00 2001 From: Heeyeon Kwon Date: Sat, 31 Aug 2019 13:08:02 -0400 Subject: [PATCH 07/12] deleted logger_for_classifier.py and used logger.py --- logger.py | 6 ++ logger_for_classifier.py | 127 ----------------------------------- stack_classifier_data_gen.py | 38 +++-------- 3 files changed, 15 insertions(+), 156 deletions(-) delete mode 100755 logger_for_classifier.py diff --git a/logger.py b/logger.py index cc8cd744..99b1509d 100755 --- a/logger.py +++ b/logger.py @@ -4,6 +4,7 @@ import numpy as np import cv2 import torch +import csv # import h5py class Logger(): @@ -73,6 +74,11 @@ def save_heightmaps(self, iteration, color_heightmap, depth_heightmap, mode): def write_to_log(self, log_name, log): np.savetxt(os.path.join(self.transitions_directory, '%s.log.txt' % log_name), log, delimiter=' ') + # For stack classifier data (might not need) + def save_label(self, log_name, log): + with open(os.path.join(self.color_images_directory, '%s.txt' % log_name), 'w') as f: + csv.writer(f, delimiter=' ').writerows(log) + def save_model(self, model, name): torch.save(model.cpu().state_dict(), os.path.join(self.models_directory, 'snapshot.%s.pth' % (name))) diff --git a/logger_for_classifier.py b/logger_for_classifier.py deleted file mode 100755 index 83c70b8e..00000000 --- a/logger_for_classifier.py +++ /dev/null @@ -1,127 +0,0 @@ -import time -import datetime -import os -import numpy as np -import cv2 -import torch -import csv -# import h5py - -class Logger(): - - def __init__(self, continue_logging, logging_directory): - - # Create directory to save data - timestamp = time.time() - timestamp_value = datetime.datetime.fromtimestamp(timestamp) - self.continue_logging = continue_logging - if self.continue_logging: - self.base_directory = logging_directory - print('Pre-loading data logging session: %s' % (self.base_directory)) - else: - self.base_directory = os.path.join(logging_directory, timestamp_value.strftime('%Y-%m-%d.%H:%M:%S')) - print('Creating data logging session: %s' % (self.base_directory)) - self.info_directory = os.path.join(self.base_directory, 'info') - self.color_images_directory = os.path.join(self.base_directory, 'data', 'color-images') - self.depth_images_directory = os.path.join(self.base_directory, 'data', 'depth-images') - self.color_heightmaps_directory = os.path.join(self.base_directory, 'data', 'color-heightmaps') - self.depth_heightmaps_directory = os.path.join(self.base_directory, 'data', 'depth-heightmaps') - self.models_directory = os.path.join(self.base_directory, 'models') - self.visualizations_directory = os.path.join(self.base_directory, 'visualizations') - self.recordings_directory = os.path.join(self.base_directory, 'recordings') - self.transitions_directory = os.path.join(self.base_directory, 'transitions') - - if not os.path.exists(self.info_directory): - os.makedirs(self.info_directory) - if not os.path.exists(self.color_images_directory): - os.makedirs(self.color_images_directory) - if not os.path.exists(self.depth_images_directory): - os.makedirs(self.depth_images_directory) - if not os.path.exists(self.color_heightmaps_directory): - os.makedirs(self.color_heightmaps_directory) - if not os.path.exists(self.depth_heightmaps_directory): - os.makedirs(self.depth_heightmaps_directory) - if not os.path.exists(self.models_directory): - os.makedirs(self.models_directory) - if not os.path.exists(self.visualizations_directory): - os.makedirs(self.visualizations_directory) - if not os.path.exists(self.recordings_directory): - os.makedirs(self.recordings_directory) - if not os.path.exists(self.transitions_directory): - os.makedirs(os.path.join(self.transitions_directory, 'data')) - - def save_camera_info(self, intrinsics, pose, depth_scale): - np.savetxt(os.path.join(self.info_directory, 'camera-intrinsics.txt'), intrinsics, delimiter=' ') - np.savetxt(os.path.join(self.info_directory, 'camera-pose.txt'), pose, delimiter=' ') - np.savetxt(os.path.join(self.info_directory, 'camera-depth-scale.txt'), [depth_scale], delimiter=' ') - - def save_heightmap_info(self, boundaries, resolution): - np.savetxt(os.path.join(self.info_directory, 'heightmap-boundaries.txt'), boundaries, delimiter=' ') - np.savetxt(os.path.join(self.info_directory, 'heightmap-resolution.txt'), [resolution], delimiter=' ') - - def save_images(self, iteration, color_image, depth_image, mode): - color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR) - cv2.imwrite(os.path.join(self.color_images_directory, '%06d.%s.color.png' % (iteration, mode)), color_image) - depth_image = np.round(depth_image * 10000).astype(np.uint16) # Save depth in 1e-4 meters - cv2.imwrite(os.path.join(self.depth_images_directory, '%06d.%s.depth.png' % (iteration, mode)), depth_image) - - def save_images_stack(self, iteration, color_image, depth_image, stack_class): - color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR) - cv2.imwrite(os.path.join(self.color_images_directory, '%06d.%s.color.png' % (iteration, stack_class)), color_image) - depth_image = np.round(depth_image * 10000).astype(np.uint16) # Save depth in 1e-4 meters - cv2.imwrite(os.path.join(self.depth_images_directory, '%06d.%s.depth.png' % (iteration, stack_class)), depth_image) - - def save_heightmaps(self, iteration, color_heightmap, depth_heightmap, mode): - color_heightmap = cv2.cvtColor(color_heightmap, cv2.COLOR_RGB2BGR) - cv2.imwrite(os.path.join(self.color_heightmaps_directory, '%06d.%s.color.png' % (iteration, mode)), color_heightmap) - depth_heightmap = np.round(depth_heightmap * 100000).astype(np.uint16) # Save depth in 1e-5 meters - cv2.imwrite(os.path.join(self.depth_heightmaps_directory, '%06d.%s.depth.png' % (iteration, mode)), depth_heightmap) - - - def save_heightmaps_stack(self, iteration, color_heightmap, depth_heightmap, stack_class): - color_heightmap = cv2.cvtColor(color_heightmap, cv2.COLOR_RGB2BGR) - cv2.imwrite(os.path.join(self.color_heightmaps_directory, '%06d.%s.color.png' % (iteration, stack_class)), color_heightmap) - depth_heightmap = np.round(depth_heightmap * 100000).astype(np.uint16) # Save depth in 1e-5 meters - cv2.imwrite(os.path.join(self.depth_heightmaps_directory, '%06d.%s.depth.png' % (iteration, stack_class)), depth_heightmap) - - def write_to_log(self, log_name, log): - np.savetxt(os.path.join(self.transitions_directory, '%s.log.txt' % log_name), log, delimiter=' ') - - def save_label(self, log_name, log): - np.savetxt(os.path.join(self.color_images_directory, '%s.log.txt' % log_name), log, delimiter=' ') - - def save_label_1(self, log_name, log): - with open(os.path.join(self.color_images_directory, '%s.txt' % log_name), 'w') as f: - csv.writer(f, delimiter=' ').writerows(log) - - def save_model(self, model, name): - torch.save(model.cpu().state_dict(), os.path.join(self.models_directory, 'snapshot.%s.pth' % (name))) - - def save_backup_model(self, model, name): - torch.save(model.state_dict(), os.path.join(self.models_directory, 'snapshot-backup.%s.pth' % (name))) - - def save_visualizations(self, iteration, affordance_vis, name): - cv2.imwrite(os.path.join(self.visualizations_directory, '%06d.%s.png' % (iteration,name)), affordance_vis) - - # def save_state_features(self, iteration, state_feat): - # h5f = h5py.File(os.path.join(self.visualizations_directory, '%06d.state.h5' % (iteration)), 'w') - # h5f.create_dataset('state', data=state_feat.cpu().data.numpy()) - # h5f.close() - - # Record RGB-D video while executing primitive - # recording_directory = logger.make_new_recording_directory(iteration) - # camera.start_recording(recording_directory) - # camera.stop_recording() - def make_new_recording_directory(self, iteration): - recording_directory = os.path.join(self.recordings_directory, '%06d' % (iteration)) - if not os.path.exists(recording_directory): - os.makedirs(recording_directory) - return recording_directory - - def save_transition(self, iteration, transition): - depth_heightmap = np.round(transition.state * 100000).astype(np.uint16) # Save depth in 1e-5 meters - cv2.imwrite(os.path.join(self.transitions_directory, 'data', '%06d.0.depth.png' % (iteration)), depth_heightmap) - next_depth_heightmap = np.round(transition.next_state * 100000).astype(np.uint16) # Save depth in 1e-5 meters - cv2.imwrite(os.path.join(self.transitions_directory, 'data', '%06d.1.depth.png' % (iteration)), next_depth_heightmap) - # np.savetxt(os.path.join(self.transitions_directory, '%06d.action.txt' % (iteration)), [1 if (transition.action == 'grasp') else 0], delimiter=' ') - # np.savetxt(os.path.join(self.transitions_directory, '%06d.reward.txt' % (iteration)), [reward_value], delimiter=' ') diff --git a/stack_classifier_data_gen.py b/stack_classifier_data_gen.py index 15883bff..eb0e1dc6 100644 --- a/stack_classifier_data_gen.py +++ b/stack_classifier_data_gen.py @@ -12,7 +12,7 @@ from torch.autograd import Variable from robot import Robot from trainer import Trainer -from logger_for_classifier import Logger +from logger import Logger import utils from main import StackSequence import csv @@ -42,12 +42,11 @@ test_preset_file = os.path.abspath(args.test_preset_file) if test_preset_cases else None ##################################################################### -## Modify -#continue_gen = True -split = 'train' #['train', 'val', 'test'] -## - +# Continue logging from previous session +continue_logging = False # Set True if continue logging from previous session +split = 'train' # Set ['train', 'val', 'test'] root = './logs_for_classifier' + logging_directory = os.path.join(root, split) if split == 'train': num_stacks = 33400 @@ -63,7 +62,7 @@ # ------ Pre-loading and logging options ------ # load_snapshot = args.load_snapshot # Load pre-trained snapshot of model? # snapshot_file = os.path.abspath(args.snapshot_file) if load_snapshot else None -continue_logging = False # Continue logging from previous session +# continue_logging = False # Continue logging from previous session #logging_directory = './logs_for_classifier/training' logging_directory = os.path.abspath(logging_directory) if continue_logging else os.path.abspath('logs_for_classifier') # save_visualizations = args.save_visualizations # Save visualizations of FCN predictions? Takes 0.6s per training step if set to True @@ -95,7 +94,6 @@ if continue_logging == False: iteration = 0 else: - #label_text = "./logs_for_classifier/test/data/color-images/stack_label.txt" label_text = os.path.join(logging_directory, 'data','color-images', 'stack_label.txt') myfile = open(label_text, 'r') myfiles = [line.split(' ') for line in myfile.readlines()] @@ -138,43 +136,25 @@ valid_depth_heightmap[np.isnan(valid_depth_heightmap)] = 0 # Save RGB-D images and RGB-D heightmaps - logger.save_images_stack(iteration, color_img, depth_img, stack_class) - logger.save_heightmaps_stack(iteration, color_heightmap, valid_depth_heightmap, stack_class) + logger.save_images(iteration, color_img, depth_img, stack_class) # Used stack_class instead of mode + logger.save_heightmaps(iteration, color_heightmap, valid_depth_heightmap, stack_class) # Used stack_class instead of mode ########################################### - print(stack) filename = '%06d.%s.color.png' % (iteration, stack_class) if continue_logging: - with open(label_text,"a") as f: f.writelines("\n") name = [filename,' ', str(iteration),' ', str(stack_class)] f.writelines(name) - #opened_file.write("%r\n" %new_string) f.close() - - # print(myfiles) - # myfiles.append([filename,iteration,stack_class]) - # with open(label_text, "a") as myfile: - # title = [filename,iteration,stack_class] - # myfile.write('%d' % number) - #logger.save_label_1('stack_label', myfiles) else: labels.append([filename,iteration,stack_class]) - logger.save_label_1('stack_label', labels) + logger.save_label('stack_label', labels) print('stack success part ' + str(i+1) + ' of ' + str(blocks_to_move) + ': ' + str(stack_success) + ':' + str(height_count) +':' + str(stack_class)) iteration += 1 - - # reset scene robot.reposition_objects() # determine first block to grasp stacksequence.next() if iteration > max_iterations: break - -## save labels -#if continue_gen == False: -# logger.save_label_1('stack_label', labels) -#else: -# logger.save_label_1('stack_label_cont', labels) From 9bb4121d988365627ff391743b07e872c6c9091b Mon Sep 17 00:00:00 2001 From: Heeyeon Kwon Date: Wed, 4 Sep 2019 22:57:02 -0400 Subject: [PATCH 08/12] replacing check_stack function with image classifier --- main.py | 57 ++++++++++++++++++++++++++++++++---- robot.py | 11 ++++++- stack_classifier_data_gen.py | 49 ++++++++++++++++++++++++++++--- 3 files changed, 106 insertions(+), 11 deletions(-) diff --git a/main.py b/main.py index d64540b0..400b16de 100755 --- a/main.py +++ b/main.py @@ -16,6 +16,16 @@ from trainer import Trainer from logger import Logger import utils +try: + import efficientnet_pytorch + from efficientnet_pytorch import EfficientNet +except ImportError: + print('efficientnet_pytorch is not available, using densenet. ' + 'Try installing https://github.com/ahundt/EfficientNet-PyTorch for all features.' + 'A version of EfficientNets without dilation can be installed with the command:' + ' pip3 install efficientnet-pytorch --user --upgrade' + 'See https://github.com/lukemelas/EfficientNet-PyTorch for details') + efficientnet_pytorch = None # to convert action names to the corresponding ID number and vice-versa ACTION_TO_ID = {'push': 0, 'grasp': 1, 'place': 2} @@ -169,6 +179,24 @@ def main(args): else: goal_condition_len = 0 + #TODO(hkwon214) temporary + # ------ Image Classifier options ----- + use_classifier = args.use_classifier + checkpoint_path = args.checkpoint_path + num_class = args.num_class + #TODO(hkwon214) hard coded to use efficientnet for now. modify for future? + if use_classifier: + if checkpoint_path is None: + raise NotImplementedError('No checkpoints') + model = EfficientNet.from_pretrained('efficientnet-b0', num_classes=num_class) + #model = nn.DataParallel(model) + model_stack = model_stack.cuda() + checkpoint = torch.load(checkpoint_path) + model_stack.load_state_dict(checkpoint['state_dict']) + model_stack.eval() + + + # Set random seed np.random.seed(random_seed) @@ -223,7 +251,7 @@ def set_nonlocal_success_variables_false(): nonlocal_variables['grasp_color_success'] = False nonlocal_variables['place_color_success'] = False - def check_stack_update_goal(place_check=False, top_idx=-1): + def check_stack_update_goal(place_check=False, top_idx=-1, use_classifier = False, input_img = None): """ Check nonlocal_variables for a good stack and reset if it does not match the current goal. # Params @@ -246,8 +274,13 @@ def check_stack_update_goal(place_check=False, top_idx=-1): # only the place check expects the current goal to be met current_stack_goal = current_stack_goal[:-1] stack_shift = 0 - # TODO(ahundt) BUG Figure out why a real stack of size 2 or 3 and a push which touches no blocks does not pass the stack_check and ends up a MISMATCH in need of reset. (update: may now be fixed, double check then delete when confirmed) - stack_matches_goal, nonlocal_variables['stack_height'] = robot.check_stack(current_stack_goal, top_idx=top_idx, stack_axis=stack_axis) + if use_classifier: + # TODO(hkwon214) Add image classifier + stack_matches_goal, nonlocal_variables['stack_height'] = robot.stack_reward(model_stack, input_img, current_stack_goal) + else: + # TODO(ahundt) BUG Figure out why a real stack of size 2 or 3 and a push which touches no blocks does not pass the stack_check and ends up a MISMATCH in need of reset. (update: may now be fixed, double check then delete when confirmed) + stack_matches_goal, nonlocal_variables['stack_height'] = robot.check_stack(current_stack_goal, top_idx=top_idx, stack_axis=stack_axis) + nonlocal_variables['partial_stack_success'] = stack_matches_goal if nonlocal_variables['stack_height'] == 1: # A stack of size 1 does not meet the criteria for a partial stack success @@ -422,7 +455,9 @@ def process_actions(): # Check if the push caused a topple, size shift zero because # place operations expect increased height, # while push expects constant height. - needed_to_reset = check_stack_update_goal() + #TODO(hkwon214) temp + #needed_to_reset = check_stack_update_goal() + needed_to_reset = check_stack_update_goal(use_classifier = use_classifier, input_img = color_heightmap) if not place or not needed_to_reset: print('Push motion successful (no crash, need not move blocks): %r' % (nonlocal_variables['push_success'])) elif nonlocal_variables['primitive_action'] == 'grasp': @@ -441,7 +476,9 @@ def process_actions(): top_idx = -2 # check if a failed grasp led to a topple, or if the top block was grasped # TODO(ahundt) in check_stack() support the check after a specific grasp in case of successful grasp topple. Perhaps allow the top block to be specified? - needed_to_reset = check_stack_update_goal(top_idx=top_idx) + #needed_to_reset = check_stack_update_goal(top_idx=top_idx) + #TODO(hkwon214) temp + needed_to_reset = check_stack_update_goal(top_idx=top_idx, use_classifier = use_classifier, input_img = color_heightmap) if nonlocal_variables['grasp_success']: # robot.restart_sim() successful_grasp_count += 1 @@ -464,7 +501,9 @@ def process_actions(): elif nonlocal_variables['primitive_action'] == 'place': place_count += 1 nonlocal_variables['place_success'] = robot.place(primitive_position, best_rotation_angle) - needed_to_reset = check_stack_update_goal(place_check=True) + #TODO(hkwon214) temp + # needed_to_reset = check_stack_update_goal(place_check=True) + needed_to_reset = check_stack_update_goal(place_check=True, use_classifier = use_classifier, input_img = color_heightmap) if not needed_to_reset and nonlocal_variables['place_success'] and nonlocal_variables['partial_stack_success']: partial_stack_count += 1 nonlocal_variables['stack'].next() @@ -971,6 +1010,12 @@ def experience_replay(method, prev_primitive_action, prev_reward_value, trainer, parser.add_argument('--grasp_color_task', dest='grasp_color_task', action='store_true', default=False, help='enable grasping specific colored objects') parser.add_argument('--grasp_count', dest='grasp_cout', type=int, action='store', default=0, help='number of successful task based grasps') + # TODO(hkwon214) + # ------ Image Classifier Options (Temporary) ------ + parser.add_argument('--use_classifier', dest='use_classifier', action='store_true', default=False, help='use image classifier weights') + parser.add_argument('--checkpoint_path', dest='checkpoint_path', action='store', default='objects/blocks', help='directory of image classifier weights') + parser.add_argument('--num_class', dest='num_class', type=int, action='store', default=4, help='number of class for classifier') + # Run main program with specified arguments args = parser.parse_args() main(args) diff --git a/robot.py b/robot.py index 3c9e069b..4ee9514f 100755 --- a/robot.py +++ b/robot.py @@ -6,7 +6,7 @@ import numpy as np import utils from simulation import vrep - +import torch class Robot(object): """ Key member variables: @@ -1243,6 +1243,15 @@ def check_stack(self, object_color_sequence, distance_threshold=0.06, top_idx=-1 # TODO(ahundt) add check_stack for real robot return goal_success, detected_height + # TODO(hkwon214): From image classifier + def stack_reward(self, model, input_img, current_stack_goal): + input_img = torch.from_numpy(input_img) + goal_success = False + stack_class = model(input_img) + detected_height = stack_class + 1 + if current_stack_goal == detected_height: + goal_success = True + return goal_success, detected_height def restart_real(self): diff --git a/stack_classifier_data_gen.py b/stack_classifier_data_gen.py index eb0e1dc6..213eb64f 100644 --- a/stack_classifier_data_gen.py +++ b/stack_classifier_data_gen.py @@ -16,7 +16,26 @@ import utils from main import StackSequence import csv - +import torch._utils +try: + torch._utils._rebuild_tensor_v2 +except AttributeError: + def _rebuild_tensor_v2(storage, storage_offset, size, stride, requires_grad, backward_hooks): + tensor = torch._utils._rebuild_tensor(storage, storage_offset, size, stride) + tensor.requires_grad = requires_grad + tensor._backward_hooks = backward_hooks + return tensor + torch._utils._rebuild_tensor_v2 = _rebuild_tensor_v2 +try: + import efficientnet_pytorch + from efficientnet_pytorch import EfficientNet +except ImportError: + print('efficientnet_pytorch is not available, using densenet. ' + 'Try installing https://github.com/ahundt/EfficientNet-PyTorch for all features.' + 'A version of EfficientNets without dilation can be installed with the command:' + ' pip3 install efficientnet-pytorch --user --upgrade' + 'See https://github.com/lukemelas/EfficientNet-PyTorch for details') + efficientnet_pytorch = None ############### Testing Block Stacking ####### is_sim = True# Run in simulation? @@ -91,6 +110,19 @@ best_rotation_angle = 3.14 blocks_to_move = num_obj - 1 +############## Load Image Classifier Weights ############### +num_class = 4 +checkpoint_path = "./eval-20190818-154803-6ebd1fa-stack_height-efficientnet-0/model_best.pth.tar" +height_count_sum = 0 +stack_success_sum = 0 + +model_stack = EfficientNet.from_pretrained('efficientnet-b0', num_classes=num_class) +#model = nn.DataParallel(model) +model_stack = model_stack.cuda() +checkpoint = torch.load(checkpoint_path) +model_stack.load_state_dict(checkpoint['state_dict']) +model_stack.eval() + if continue_logging == False: iteration = 0 else: @@ -123,7 +155,6 @@ stack_goal = np.random.permutation(stack_goal) print('fake stack goal to test any stack order: ' + str(stack_goal)) stack_success, height_count = robot.check_stack(stack_goal) - ####################################### stack_class = height_count - 1 # Get latest RGB-D image @@ -136,9 +167,12 @@ valid_depth_heightmap[np.isnan(valid_depth_heightmap)] = 0 # Save RGB-D images and RGB-D heightmaps - logger.save_images(iteration, color_img, depth_img, stack_class) # Used stack_class instead of mode - logger.save_heightmaps(iteration, color_heightmap, valid_depth_heightmap, stack_class) # Used stack_class instead of mode + #logger.save_images(iteration, color_img, depth_img, stack_class) # Used stack_class instead of mode + #logger.save_heightmaps(iteration, color_heightmap, valid_depth_heightmap, stack_class) # Used stack_class instead of mode ########################################### + + stack_success_classifier, height_count_classifier= robot.stack_reward(model_stack, depth_heightmap, stack_goal) + filename = '%06d.%s.color.png' % (iteration, stack_class) if continue_logging: with open(label_text,"a") as f: @@ -150,7 +184,14 @@ labels.append([filename,iteration,stack_class]) logger.save_label('stack_label', labels) print('stack success part ' + str(i+1) + ' of ' + str(blocks_to_move) + ': ' + str(stack_success) + ':' + str(height_count) +':' + str(stack_class)) + print('stack success classifier part ' + str(i+1) + ' of ' + str(blocks_to_move) + ': ' + str(stack_success_classifier) + ':' + str(height_count) +':' + str(stack_class_classifier)) iteration += 1 + if height_count_classifier == height_count: + height_count_sum += 1 + if stack_success_classifier== stack_success: + stack_success_sum += 1 + print('stack height classifier accuracy ' + str(i+1) + ' height_count ' + str(height_count_sum/iteration)) + print('stack success classifier accuracy ' + str(i+1) + ' stack_success ' + str(stack_success_sum/iteration)) # reset scene robot.reposition_objects() From 1d2eb569fa126b93f8083ad6ca9bb835f94cd0fb Mon Sep 17 00:00:00 2001 From: Heeyeon Kwon Date: Wed, 4 Sep 2019 23:02:06 -0400 Subject: [PATCH 09/12] replacing check_stack function with image classifier --- robot.py | 1 + stack_classifier_data_gen.py | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/robot.py b/robot.py index 4ee9514f..8e1f3fc3 100755 --- a/robot.py +++ b/robot.py @@ -1248,6 +1248,7 @@ def stack_reward(self, model, input_img, current_stack_goal): input_img = torch.from_numpy(input_img) goal_success = False stack_class = model(input_img) + stack_class = stack_class.item() detected_height = stack_class + 1 if current_stack_goal == detected_height: goal_success = True diff --git a/stack_classifier_data_gen.py b/stack_classifier_data_gen.py index 213eb64f..a3a072f4 100644 --- a/stack_classifier_data_gen.py +++ b/stack_classifier_data_gen.py @@ -172,7 +172,6 @@ def _rebuild_tensor_v2(storage, storage_offset, size, stride, requires_grad, bac ########################################### stack_success_classifier, height_count_classifier= robot.stack_reward(model_stack, depth_heightmap, stack_goal) - filename = '%06d.%s.color.png' % (iteration, stack_class) if continue_logging: with open(label_text,"a") as f: From 57686fd5fce16876e23c63e83392423f1955b476 Mon Sep 17 00:00:00 2001 From: Heeyeon Kwon Date: Thu, 5 Sep 2019 16:57:59 -0400 Subject: [PATCH 10/12] update efficientnet from pretrained to regular --- stack_classifier_data_gen.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/stack_classifier_data_gen.py b/stack_classifier_data_gen.py index a3a072f4..1117ea56 100644 --- a/stack_classifier_data_gen.py +++ b/stack_classifier_data_gen.py @@ -116,9 +116,9 @@ def _rebuild_tensor_v2(storage, storage_offset, size, stride, requires_grad, bac height_count_sum = 0 stack_success_sum = 0 -model_stack = EfficientNet.from_pretrained('efficientnet-b0', num_classes=num_class) +model_stack = EfficientNet.from_name('efficientnet-b0') #model = nn.DataParallel(model) -model_stack = model_stack.cuda() +#model_stack = model_stack.cuda() checkpoint = torch.load(checkpoint_path) model_stack.load_state_dict(checkpoint['state_dict']) model_stack.eval() From a0eb211ae59c62971a7b32e3bc686e4ab594f412 Mon Sep 17 00:00:00 2001 From: Heeyeon Kwon Date: Fri, 6 Sep 2019 15:26:17 -0400 Subject: [PATCH 11/12] update efficientnet from pretrained to regular --- main.py | 6 ++---- robot.py | 1 + stack_classifier_data_gen.py | 15 ++++++++++++++- 3 files changed, 17 insertions(+), 5 deletions(-) diff --git a/main.py b/main.py index 400b16de..0e027ef0 100755 --- a/main.py +++ b/main.py @@ -183,12 +183,11 @@ def main(args): # ------ Image Classifier options ----- use_classifier = args.use_classifier checkpoint_path = args.checkpoint_path - num_class = args.num_class #TODO(hkwon214) hard coded to use efficientnet for now. modify for future? if use_classifier: if checkpoint_path is None: raise NotImplementedError('No checkpoints') - model = EfficientNet.from_pretrained('efficientnet-b0', num_classes=num_class) + model = EfficientNet.from_name('efficientnet-b0') #model = nn.DataParallel(model) model_stack = model_stack.cuda() checkpoint = torch.load(checkpoint_path) @@ -501,7 +500,7 @@ def process_actions(): elif nonlocal_variables['primitive_action'] == 'place': place_count += 1 nonlocal_variables['place_success'] = robot.place(primitive_position, best_rotation_angle) - #TODO(hkwon214) temp + #TODO(hkwon214) robot executed task -> capture image for image classifier # needed_to_reset = check_stack_update_goal(place_check=True) needed_to_reset = check_stack_update_goal(place_check=True, use_classifier = use_classifier, input_img = color_heightmap) if not needed_to_reset and nonlocal_variables['place_success'] and nonlocal_variables['partial_stack_success']: @@ -1014,7 +1013,6 @@ def experience_replay(method, prev_primitive_action, prev_reward_value, trainer, # ------ Image Classifier Options (Temporary) ------ parser.add_argument('--use_classifier', dest='use_classifier', action='store_true', default=False, help='use image classifier weights') parser.add_argument('--checkpoint_path', dest='checkpoint_path', action='store', default='objects/blocks', help='directory of image classifier weights') - parser.add_argument('--num_class', dest='num_class', type=int, action='store', default=4, help='number of class for classifier') # Run main program with specified arguments args = parser.parse_args() diff --git a/robot.py b/robot.py index 8e1f3fc3..e8e33d2d 100755 --- a/robot.py +++ b/robot.py @@ -1246,6 +1246,7 @@ def check_stack(self, object_color_sequence, distance_threshold=0.06, top_idx=-1 # TODO(hkwon214): From image classifier def stack_reward(self, model, input_img, current_stack_goal): input_img = torch.from_numpy(input_img) + print('IMAGE SHAPE: ' + str(input_img.shape)) goal_success = False stack_class = model(input_img) stack_class = stack_class.item() diff --git a/stack_classifier_data_gen.py b/stack_classifier_data_gen.py index 1117ea56..ffc5aecd 100644 --- a/stack_classifier_data_gen.py +++ b/stack_classifier_data_gen.py @@ -116,7 +116,7 @@ def _rebuild_tensor_v2(storage, storage_offset, size, stride, requires_grad, bac height_count_sum = 0 stack_success_sum = 0 -model_stack = EfficientNet.from_name('efficientnet-b0') +model_stack = EfficientNet.from_name('efficientnet-b0',override_params={'num_classes': 4}) #model = nn.DataParallel(model) #model_stack = model_stack.cuda() checkpoint = torch.load(checkpoint_path) @@ -171,6 +171,19 @@ def _rebuild_tensor_v2(storage, storage_offset, size, stride, requires_grad, bac #logger.save_heightmaps(iteration, color_heightmap, valid_depth_heightmap, stack_class) # Used stack_class instead of mode ########################################### + #print('SHAPE: ' + str(color_heightmap.shape)) + depth_heightmap = depth_heightmap[np.newaxis, np.newaxis,:,:] + print('SHAPE: ' + str(depth_heightmap.shape)) + #normalize = transforms.Normalize(mean=[0.485, 0.456, 0.406], + # std=[0.229, 0.224, 0.225]) + + # image_size = EfficientNet.get_image_size(args.arch) + # val_transforms = transforms.Compose([ + # transforms.Resize(image_size, interpolation=PIL.Image.BICUBIC), + # transforms.CenterCrop(image_size), + # transforms.ToTensor(), + # normalize, + # ]) stack_success_classifier, height_count_classifier= robot.stack_reward(model_stack, depth_heightmap, stack_goal) filename = '%06d.%s.color.png' % (iteration, stack_class) if continue_logging: From cf20777c87b4575515db87452351a02bfeb78bb9 Mon Sep 17 00:00:00 2001 From: Heeyeon Kwon Date: Sat, 7 Sep 2019 12:43:24 -0400 Subject: [PATCH 12/12] visualizer.py --- main.py | 23 +++++++++++++--- robot.py | 28 +++++++++++++++++++- stack_classifier_data_gen.py | 51 ++++++++++++++++++++---------------- 3 files changed, 75 insertions(+), 27 deletions(-) diff --git a/main.py b/main.py index 0e027ef0..7877a9cb 100755 --- a/main.py +++ b/main.py @@ -456,7 +456,12 @@ def process_actions(): # while push expects constant height. #TODO(hkwon214) temp #needed_to_reset = check_stack_update_goal() - needed_to_reset = check_stack_update_goal(use_classifier = use_classifier, input_img = color_heightmap) + color_img, depth_img = robot.get_camera_data() + depth_img = depth_img * robot.cam_depth_scale # Apply depth scale from calibration + # Get heightmap from RGB-D image (by re-projecting 3D point cloud) + color_heightmap_after_action, depth_heightmap_after_action = utils.get_heightmap(color_img, depth_img, robot.cam_intrinsics, robot.cam_pose, workspace_limits, heightmap_resolution) + + needed_to_reset = check_stack_update_goal(use_classifier = use_classifier, input_img = depth_heightmap_after_action) if not place or not needed_to_reset: print('Push motion successful (no crash, need not move blocks): %r' % (nonlocal_variables['push_success'])) elif nonlocal_variables['primitive_action'] == 'grasp': @@ -477,7 +482,13 @@ def process_actions(): # TODO(ahundt) in check_stack() support the check after a specific grasp in case of successful grasp topple. Perhaps allow the top block to be specified? #needed_to_reset = check_stack_update_goal(top_idx=top_idx) #TODO(hkwon214) temp - needed_to_reset = check_stack_update_goal(top_idx=top_idx, use_classifier = use_classifier, input_img = color_heightmap) + + color_img, depth_img = robot.get_camera_data() + depth_img = depth_img * robot.cam_depth_scale # Apply depth scale from calibration + # Get heightmap from RGB-D image (by re-projecting 3D point cloud) + color_heightmap_after_action, depth_heightmap_after_action = utils.get_heightmap(color_img, depth_img, robot.cam_intrinsics, robot.cam_pose, workspace_limits, heightmap_resolution) + + needed_to_reset = check_stack_update_goal(top_idx=top_idx, use_classifier = use_classifier, input_img = depth_heightmap_after_action) if nonlocal_variables['grasp_success']: # robot.restart_sim() successful_grasp_count += 1 @@ -500,9 +511,15 @@ def process_actions(): elif nonlocal_variables['primitive_action'] == 'place': place_count += 1 nonlocal_variables['place_success'] = robot.place(primitive_position, best_rotation_angle) + #TODO(hkwon214) robot executed task -> capture image for image classifier + color_img, depth_img = robot.get_camera_data() + depth_img = depth_img * robot.cam_depth_scale # Apply depth scale from calibration + # Get heightmap from RGB-D image (by re-projecting 3D point cloud) + color_heightmap_after_action, depth_heightmap_after_action = utils.get_heightmap(color_img, depth_img, robot.cam_intrinsics, robot.cam_pose, workspace_limits, heightmap_resolution) + # needed_to_reset = check_stack_update_goal(place_check=True) - needed_to_reset = check_stack_update_goal(place_check=True, use_classifier = use_classifier, input_img = color_heightmap) + needed_to_reset = check_stack_update_goal(place_check=True, use_classifier = use_classifier, input_img = depth_heightmap_after_action) if not needed_to_reset and nonlocal_variables['place_success'] and nonlocal_variables['partial_stack_success']: partial_stack_count += 1 nonlocal_variables['stack'].next() diff --git a/robot.py b/robot.py index e8e33d2d..0fade58d 100755 --- a/robot.py +++ b/robot.py @@ -7,6 +7,8 @@ import utils from simulation import vrep import torch +import cv2 +from scipy import ndimage, misc class Robot(object): """ Key member variables: @@ -1245,7 +1247,7 @@ def check_stack(self, object_color_sequence, distance_threshold=0.06, top_idx=-1 # TODO(hkwon214): From image classifier def stack_reward(self, model, input_img, current_stack_goal): - input_img = torch.from_numpy(input_img) + #input_img = torch.from_numpy(input_img) print('IMAGE SHAPE: ' + str(input_img.shape)) goal_success = False stack_class = model(input_img) @@ -1255,6 +1257,30 @@ def stack_reward(self, model, input_img, current_stack_goal): goal_success = True return goal_success, detected_height + def check_incremental_height(self,input_img, current_stack_goal): + goal_success = False + img_median = ndimage.median_filter(input_img, size=5) + max_z = np.max(img_median) + print('MAXZ ' + str(max_z)) + if (max_z > 0.051) and (max_z < 0.052): + detected_height = 1 + elif (max_z > 0.10) and (max_z < 0.11): + detected_height = 2 + elif (max_z > 0.155) and (max_z < 0.156): + detected_height = 3 + elif (max_z > 0.20) and (max_z < 0.21): + detected_height = 4 + if current_stack_goal == detected_height: + goal_success = True + return goal_success, detected_height + + def check_z_height(self,input_img): + # CV or + img_median = ndimage.median_filter(input_img, size=5) + max_z = np.max(img_median) + return max_z + + def restart_real(self): # Compute tool orientation from heightmap rotation angle diff --git a/stack_classifier_data_gen.py b/stack_classifier_data_gen.py index ffc5aecd..6f0e4825 100644 --- a/stack_classifier_data_gen.py +++ b/stack_classifier_data_gen.py @@ -17,6 +17,7 @@ from main import StackSequence import csv import torch._utils +import torchvision.transforms as transforms try: torch._utils._rebuild_tensor_v2 except AttributeError: @@ -116,12 +117,15 @@ def _rebuild_tensor_v2(storage, storage_offset, size, stride, requires_grad, bac height_count_sum = 0 stack_success_sum = 0 -model_stack = EfficientNet.from_name('efficientnet-b0',override_params={'num_classes': 4}) -#model = nn.DataParallel(model) -#model_stack = model_stack.cuda() -checkpoint = torch.load(checkpoint_path) -model_stack.load_state_dict(checkpoint['state_dict']) -model_stack.eval() +# model_stack = EfficientNet.from_name('efficientnet-b0',override_params={'num_classes': 4}) +# #model = nn.DataParallel(model) +# model_stack = model_stack.cuda() +# checkpoint = torch.load(checkpoint_path) +# model_stack.load_state_dict(checkpoint['state_dict']) +# model_stack.eval() + +model_name = 'efficientnet-b0' +image_size = EfficientNet.get_image_size(model_name) if continue_logging == False: iteration = 0 @@ -139,6 +143,7 @@ def _rebuild_tensor_v2(storage, storage_offset, size, stride, requires_grad, bac print('----------------------------------------------') stacksequence.next() stack_goal = stacksequence.current_sequence_progress() + print('STACK GOAL ' + str(len(stack_goal))) block_to_move = stack_goal[-1] print('move block: ' + str(i) + ' current stack goal: ' + str(stack_goal)) block_positions = robot.get_obj_positions_and_orientations()[0] @@ -170,21 +175,21 @@ def _rebuild_tensor_v2(storage, storage_offset, size, stride, requires_grad, bac #logger.save_images(iteration, color_img, depth_img, stack_class) # Used stack_class instead of mode #logger.save_heightmaps(iteration, color_heightmap, valid_depth_heightmap, stack_class) # Used stack_class instead of mode ########################################### + #depth_heightmap = depth_heightmap[np.newaxis, np.newaxis,:,:] + + + # tfms = transforms.Compose([transforms.Resize(image_size), transforms.CenterCrop(image_size), + # transforms.ToTensor(), + # transforms.Normalize([0.485, 0.456, 0.406], [0.229, 0.224, 0.225]),]) + # depth_heightmap = tfms(depth_heightmap).unsqueeze(0) - #print('SHAPE: ' + str(color_heightmap.shape)) - depth_heightmap = depth_heightmap[np.newaxis, np.newaxis,:,:] - print('SHAPE: ' + str(depth_heightmap.shape)) - #normalize = transforms.Normalize(mean=[0.485, 0.456, 0.406], - # std=[0.229, 0.224, 0.225]) - - # image_size = EfficientNet.get_image_size(args.arch) - # val_transforms = transforms.Compose([ - # transforms.Resize(image_size, interpolation=PIL.Image.BICUBIC), - # transforms.CenterCrop(image_size), - # transforms.ToTensor(), - # normalize, - # ]) - stack_success_classifier, height_count_classifier= robot.stack_reward(model_stack, depth_heightmap, stack_goal) + # stack_success_classifier, height_count_classifier= robot.stack_reward(model_stack, depth_heightmap, stack_goal) + stack_success_z, height_count_z = robot.check_incremental_height(valid_depth_heightmap, len(stack_goal)) + stack_class_z = height_count_z - 1 + z_height = robot.check_z_height + print('HEIGHT Z,: ' + str(height_count_z)) + print('HEIGHT Z,: ' + str(np.max(valid_depth_heightmap))) + print('check z,: ' + str(z_height)) filename = '%06d.%s.color.png' % (iteration, stack_class) if continue_logging: with open(label_text,"a") as f: @@ -196,11 +201,11 @@ def _rebuild_tensor_v2(storage, storage_offset, size, stride, requires_grad, bac labels.append([filename,iteration,stack_class]) logger.save_label('stack_label', labels) print('stack success part ' + str(i+1) + ' of ' + str(blocks_to_move) + ': ' + str(stack_success) + ':' + str(height_count) +':' + str(stack_class)) - print('stack success classifier part ' + str(i+1) + ' of ' + str(blocks_to_move) + ': ' + str(stack_success_classifier) + ':' + str(height_count) +':' + str(stack_class_classifier)) + print('stack success classifier part ' + str(i+1) + ' of ' + str(blocks_to_move) + ': ' + str(stack_success_z) + ':' + str(height_count) +':' + str(stack_class_z)) iteration += 1 - if height_count_classifier == height_count: + if height_count_z == height_count: height_count_sum += 1 - if stack_success_classifier== stack_success: + if stack_success_z== stack_success: stack_success_sum += 1 print('stack height classifier accuracy ' + str(i+1) + ' height_count ' + str(height_count_sum/iteration)) print('stack success classifier accuracy ' + str(i+1) + ' stack_success ' + str(stack_success_sum/iteration))