Skip to content

Commit

Permalink
Merge pull request #13 from jhu-lcsr/z_height_v2
Browse files Browse the repository at this point in the history
Z height v2 fixes - added situation removal (0 reward on backwards progress)
  • Loading branch information
ahundt authored Sep 10, 2019
2 parents 856d1b2 + c86ffed commit 1911fc2
Show file tree
Hide file tree
Showing 4 changed files with 201 additions and 28 deletions.
114 changes: 114 additions & 0 deletions .vscode/.ropeproject/config.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,114 @@
# The default ``config.py``
# flake8: noqa


def set_prefs(prefs):
"""This function is called before opening the project"""

# Specify which files and folders to ignore in the project.
# Changes to ignored resources are not added to the history and
# VCSs. Also they are not returned in `Project.get_files()`.
# Note that ``?`` and ``*`` match all characters but slashes.
# '*.pyc': matches 'test.pyc' and 'pkg/test.pyc'
# 'mod*.pyc': matches 'test/mod1.pyc' but not 'mod/1.pyc'
# '.svn': matches 'pkg/.svn' and all of its children
# 'build/*.o': matches 'build/lib.o' but not 'build/sub/lib.o'
# 'build//*.o': matches 'build/lib.o' and 'build/sub/lib.o'
prefs['ignored_resources'] = ['*.pyc', '*~', '.ropeproject',
'.hg', '.svn', '_svn', '.git', '.tox']

# Specifies which files should be considered python files. It is
# useful when you have scripts inside your project. Only files
# ending with ``.py`` are considered to be python files by
# default.
# prefs['python_files'] = ['*.py']

# Custom source folders: By default rope searches the project
# for finding source folders (folders that should be searched
# for finding modules). You can add paths to that list. Note
# that rope guesses project source folders correctly most of the
# time; use this if you have any problems.
# The folders should be relative to project root and use '/' for
# separating folders regardless of the platform rope is running on.
# 'src/my_source_folder' for instance.
# prefs.add('source_folders', 'src')

# You can extend python path for looking up modules
# prefs.add('python_path', '~/python/')

# Should rope save object information or not.
prefs['save_objectdb'] = True
prefs['compress_objectdb'] = False

# If `True`, rope analyzes each module when it is being saved.
prefs['automatic_soa'] = True
# The depth of calls to follow in static object analysis
prefs['soa_followed_calls'] = 0

# If `False` when running modules or unit tests "dynamic object
# analysis" is turned off. This makes them much faster.
prefs['perform_doa'] = True

# Rope can check the validity of its object DB when running.
prefs['validate_objectdb'] = True

# How many undos to hold?
prefs['max_history_items'] = 32

# Shows whether to save history across sessions.
prefs['save_history'] = True
prefs['compress_history'] = False

# Set the number spaces used for indenting. According to
# :PEP:`8`, it is best to use 4 spaces. Since most of rope's
# unit-tests use 4 spaces it is more reliable, too.
prefs['indent_size'] = 4

# Builtin and c-extension modules that are allowed to be imported
# and inspected by rope.
prefs['extension_modules'] = []

# Add all standard c-extensions to extension_modules list.
prefs['import_dynload_stdmods'] = True

# If `True` modules with syntax errors are considered to be empty.
# The default value is `False`; When `False` syntax errors raise
# `rope.base.exceptions.ModuleSyntaxError` exception.
prefs['ignore_syntax_errors'] = False

# If `True`, rope ignores unresolvable imports. Otherwise, they
# appear in the importing namespace.
prefs['ignore_bad_imports'] = False

# If `True`, rope will insert new module imports as
# `from <package> import <module>` by default.
prefs['prefer_module_from_imports'] = False

# If `True`, rope will transform a comma list of imports into
# multiple separate import statements when organizing
# imports.
prefs['split_imports'] = False

# If `True`, rope will remove all top-level import statements and
# reinsert them at the top of the module when making changes.
prefs['pull_imports_to_top'] = True

# If `True`, rope will sort imports alphabetically by module name instead
# of alphabetically by import statement, with from imports after normal
# imports.
prefs['sort_imports_alphabetically'] = False

# Location of implementation of
# rope.base.oi.type_hinting.interfaces.ITypeHintingFactory In general
# case, you don't have to change this value, unless you're an rope expert.
# Change this value to inject you own implementations of interfaces
# listed in module rope.base.oi.type_hinting.providers.interfaces
# For example, you can add you own providers for Django Models, or disable
# the search type-hinting in a class hierarchy, etc.
prefs['type_hinting_factory'] = (
'rope.base.oi.type_hinting.factory.default_type_hinting_factory')


def project_opened(project):
"""This function is called after opening the project"""
# Do whatever you like here!
Binary file added .vscode/.ropeproject/objectdb
Binary file not shown.
88 changes: 68 additions & 20 deletions main.py
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,10 @@ def current_sequence_progress(self):
After three next() calls it will return [0, 1, 3, 2].
After four next() calls a new sequence will be generated and it will return one element again.
"""
return self.object_color_sequence[:self.object_color_index+1]
if self.is_goal_conditioned_task:
return self.object_color_sequence[:self.object_color_index+1]
else:
return None

def next(self):
self.total_steps += 1
Expand All @@ -109,7 +112,7 @@ def main(args):
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)
workspace_limits = np.asarray([[-0.724, -0.276], [-0.224, 0.224], [-0.0001, 0.5]]) # 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 = args.heightmap_resolution # Meters per pixel of heightmap
Expand All @@ -127,6 +130,7 @@ def main(args):
grasp_only = args.grasp_only
check_row = args.check_row
check_z_height = args.check_z_height
check_z_height_goal = args.check_z_height_goal
pretrained = not args.random_weights
max_iter = args.max_iter
no_height_reward = args.no_height_reward
Expand Down Expand Up @@ -218,11 +222,20 @@ def main(args):
'trial_success_rate': np.inf,
'replay_iteration': 0,
'trial_complete': False,
'finalize_prev_trial_log': False}
'finalize_prev_trial_log': False,
'prev_stack_height': 1}

if check_z_height:
nonlocal_variables['stack_height'] = 0.0
nonlocal_variables['prev_stack_height'] = 0.0
best_stack_rate = np.inf

if check_z_height:
is_goal_conditioned = False
else:
is_goal_conditioned = grasp_color_task or place
# Choose the first color block to grasp, or None if not running in goal conditioned mode
nonlocal_variables['stack'] = StackSequence(num_obj - num_extra_obj, grasp_color_task or place)
nonlocal_variables['stack'] = StackSequence(num_obj - num_extra_obj, is_goal_conditioned)
if place:
# If we are stacking we actually skip to the second block which needs to go on the first
nonlocal_variables['stack'].next()
Expand All @@ -234,7 +247,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, input_img=None):
def check_stack_update_goal(place_check=False, top_idx=-1, depth_img=None):
""" Check nonlocal_variables for a good stack and reset if it does not match the current goal.
# Params
Expand All @@ -253,26 +266,38 @@ def check_stack_update_goal(place_check=False, top_idx=-1, input_img=None):
if place_check:
# Only reset while placing if the stack decreases in height!
stack_shift = 1
else:
elif current_stack_goal is not None:
# 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)
if check_row:
stack_matches_goal, nonlocal_variables['stack_height'] = robot.check_row(current_stack_goal, num_obj=num_obj)
elif check_z_height:
stack_matches_goal, nonlocal_variables['stack_height'] = robot.check_incremental_height(input_img, current_stack_goal)
# TODO(ahundt) make decrease threshold more accessible, perhaps a command line parameter
decrease_threshold = 0.1
# decrease_threshold = None # None means decrease_threshold will be disabled
stack_matches_goal, nonlocal_variables['stack_height'] = robot.check_z_height(depth_img, nonlocal_variables['prev_stack_height'])
# if it falls we will just keep going, and allow nonaction or objects out of scene checks to handle resets
needed_to_reset = False
max_workspace_height = nonlocal_variables['prev_stack_height'] - decrease_threshold
if decrease_threshold is not None and nonlocal_variables['stack_height'] < max_workspace_height:
needed_to_reset = True
# TODO(hkwon214) add a separate case for incremental height
# stack_matches_goal, nonlocal_variables['stack_height'] = robot.check_incremental_height(input_img, current_stack_goal)
else:
stack_matches_goal, nonlocal_variables['stack_height'] = robot.check_stack(current_stack_goal, top_idx=top_idx)
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
nonlocal_variables['partial_stack_success'] = False
nonlocal_variables['stack_success'] = False

max_workspace_height = len(current_stack_goal) - stack_shift
# Has that stack gotten shorter than it was before? If so we need to reset
needed_to_reset = nonlocal_variables['stack_height'] < max_workspace_height
if not check_z_height:
if nonlocal_variables['stack_height'] == 1:
# A stack of size 1 does not meet the criteria for a partial stack success
nonlocal_variables['partial_stack_success'] = False
nonlocal_variables['stack_success'] = False
max_workspace_height = len(current_stack_goal) - stack_shift
# Has that stack gotten shorter than it was before? If so we need to reset
needed_to_reset = nonlocal_variables['stack_height'] < max_workspace_height


print('check_stack() stack_height: ' + str(nonlocal_variables['stack_height']) + ' stack matches current goal: ' + str(stack_matches_goal) + ' partial_stack_success: ' +
str(nonlocal_variables['partial_stack_success']) + ' Does the code think a reset is needed: ' + str(needed_to_reset))
# if place and needed_to_reset:
Expand Down Expand Up @@ -440,7 +465,7 @@ 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(input_img=valid_depth_heightmap_push)
needed_to_reset = check_stack_update_goal(depth_img=valid_depth_heightmap_push)
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':
Expand All @@ -461,7 +486,7 @@ 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, input_img=valid_depth_heightmap_grasp)
needed_to_reset = check_stack_update_goal(top_idx=top_idx, depth_img=valid_depth_heightmap_grasp)
if nonlocal_variables['grasp_success']:
# robot.restart_sim()
successful_grasp_count += 1
Expand All @@ -486,12 +511,14 @@ def process_actions():
nonlocal_variables['place_success'] = robot.place(primitive_position, best_rotation_angle)
#TODO(hkwon214) Get image after executing place action. save also? better place to put?
valid_depth_heightmap_place, color_heightmap_place, depth_heightmap_place, color_img_place, depth_img_place = get_and_save_images(robot, workspace_limits, heightmap_resolution, logger, trainer, save_image=False)
needed_to_reset = check_stack_update_goal(place_check=True, input_img=valid_depth_heightmap_place)
needed_to_reset = check_stack_update_goal(place_check=True, depth_img=valid_depth_heightmap_place)
if not needed_to_reset and nonlocal_variables['place_success'] and nonlocal_variables['partial_stack_success']:
partial_stack_count += 1
nonlocal_variables['stack'].next()
next_stack_goal = nonlocal_variables['stack'].current_sequence_progress()
if len(next_stack_goal) < len(current_stack_goal):
if ((check_z_height and nonlocal_variables['stack_height'] > check_z_height_goal) or
(not check_z_height and len(next_stack_goal) < len(current_stack_goal))):
print('TRIAL ' + str(nonlocal_variables['stack'].trial) + ' SUCCESS!!!!!!!!!!!!!!!!!!!!!!!!!!!!!')
nonlocal_variables['stack_success'] = True
stack_count += 1
# full stack complete! reset the scene
Expand All @@ -509,7 +536,7 @@ def process_actions():
trainer.color_success_log.append([int(nonlocal_variables['color_success'])])
if place:
# place trainer logs are updated in process_actions()
trainer.stack_height_log.append([int(nonlocal_variables['stack_height'])])
trainer.stack_height_log.append([float(nonlocal_variables['stack_height'])])
trainer.partial_stack_success_log.append([int(nonlocal_variables['partial_stack_success'])])
trainer.place_success_log.append([int(nonlocal_variables['place_success'])])

Expand All @@ -527,6 +554,12 @@ def process_actions():
' partial_stack_successes: ' + str(partial_stack_count) +
' stack_successes: ' + str(stack_count) + ' trial_success_rate: ' + str(trial_rate) + ' stack goal: ' + str(current_stack_goal))

if check_z_height and nonlocal_variables['trial_complete']:
# Zero out the height because the trial is done.
# Note these lines must be after the logging of these variables is complete.
nonlocal_variables['stack_height'] = 0.0
nonlocal_variables['prev_stack_height'] = 0.0

nonlocal_variables['executing_action'] = False
# TODO(ahundt) this should really be using proper threading and locking algorithms
time.sleep(0.01)
Expand Down Expand Up @@ -760,8 +793,21 @@ def process_actions():
# TODO(ahundt) double check that this doesn't screw up state completely for future trials...
print('ERROR: PROBLEM DETECTED IN SCENE, NO CHANGES FOR OVER 20 SECONDS, RESETTING THE OBJECTS TO RECOVER...')
get_and_save_images(robot, workspace_limits, heightmap_resolution, logger, trainer, '1')
robot.check_sim()
robot.reposition_objects()
nonlocal_variables['trial_complete'] = True
if place:
nonlocal_variables['stack'].reset_sequence()
nonlocal_variables['stack'].next()
if check_z_height:
# Zero out the height because the trial is done.
# Note these lines must normally be after the
# logging of these variables is complete,
# but this is a special (hopefully rare) recovery scenario.
nonlocal_variables['stack_height'] = 0.0
nonlocal_variables['prev_stack_height'] = 0.0
# don't reset again for 20 more seconds
iteration_time_0 = time.time()

if exit_called:
# shut down the simulation or robot
Expand All @@ -781,6 +827,7 @@ def process_actions():
prev_partial_stack_success = nonlocal_variables['partial_stack_success']
# stack_height will just always be 1 if we are not actually stacking
prev_stack_height = nonlocal_variables['stack_height']
nonlocal_variables['prev_stack_height'] = nonlocal_variables['stack_height']
prev_push_predictions = push_predictions.copy()
prev_grasp_predictions = grasp_predictions.copy()
prev_place_predictions = place_predictions
Expand Down Expand Up @@ -992,6 +1039,7 @@ def experience_replay(method, prev_primitive_action, prev_reward_value, trainer,
parser.add_argument('--grasp_count', dest='grasp_cout', type=int, action='store', default=0, help='number of successful task based grasps')
parser.add_argument('--transfer_grasp_to_place', dest='transfer_grasp_to_place', action='store_true', default=False, help='Load the grasping weights as placing weights.')
parser.add_argument('--check_z_height', dest='check_z_height', action='store_true', default=False, help='use check_z_height instead of check_stacks for any stacks')
parser.add_argument('--check_z_height_goal', dest='check_z_height_goal', action='store', type=float, default=2.0, help='check_z_height goal height in meters')

# -------------- Testing options --------------
parser.add_argument('--is_testing', dest='is_testing', action='store_true', default=False)
Expand Down
Loading

0 comments on commit 1911fc2

Please sign in to comment.