├── .gitignore ├── .gitmodules ├── README.md ├── analyze_experiment.py ├── collect_place.py ├── collect_pull.py ├── databases ├── franka_carter-baker-side-place.json ├── franka_carter-baker_joint-pull.json ├── franka_carter-chewie_door_left-side-place.json ├── franka_carter-chewie_door_right-side-place.json ├── franka_carter-chewie_door_right_joint-pull.json ├── franka_carter-chewie_left-side-place.json ├── franka_carter-chewie_right-side-place.json ├── franka_carter-dagger_door_left-side-place.json ├── franka_carter-dagger_door_left_joint-pull.json ├── franka_carter-dagger_left-side-place.json ├── franka_carter-front_left_knob-press.json ├── franka_carter-front_left_stove-side-place.json ├── franka_carter-front_left_stove-top-place.json ├── franka_carter-front_right_knob-press.json ├── franka_carter-front_right_stove-side-place.json ├── franka_carter-front_right_stove-top-place.json ├── franka_carter-hitman_tmp-side-place.json ├── franka_carter-hitman_tmp-top-place.json ├── franka_carter-indigo_drawer_bottom-top-place.json ├── franka_carter-indigo_drawer_bottom_joint-pull.json ├── franka_carter-indigo_drawer_top-top-place.json ├── franka_carter-indigo_drawer_top_joint-pull.json ├── franka_carter-indigo_tmp-side-place.json ├── franka_carter-indigo_tmp-top-place.json ├── franka_carter-range-side-place.json └── franka_carter-range-top-place.json ├── images ├── put_spam.png └── stow_block.png ├── kitchen_poses.json ├── pddl ├── domain.pddl └── stream.pddl ├── run_experiment.py ├── run_pybullet.py └── src ├── __init__.py ├── belief.py ├── command.py ├── database.py ├── inference.py ├── observe.py ├── planner.py ├── policy.py ├── problem.py ├── replan.py ├── stream.py ├── streams ├── __init__.py ├── move.py ├── pick.py ├── pour.py ├── press.py └── pull.py ├── task.py ├── utils.py ├── visualization.py └── world.py /.gitignore: -------------------------------------------------------------------------------- 1 | .idea 2 | *.pyc 3 | statistics 4 | temp 5 | videos 6 | *.mp4 7 | *.avi 8 | *.so 9 | .DS_Store 10 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "pddlstream"] 2 | path = pddlstream 3 | url = https://github.com/caelan/pddlstream 4 | [submodule "ss-pybullet"] 5 | path = ss-pybullet 6 | url = https://github.com/caelan/ss-pybullet 7 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SS-Replan 2 | 3 | Online observation, estimatation, planning, and control for a Franka Panda Robot operating in [NVIDIA SRL](https://www.nvidia.com/en-us/research/robotics/)'s simulated kitchen environment. 4 | 5 | ## Installation 6 | 7 | SS-Replan supports both Python 2 and Python 3. 8 | 9 | 10 | * [Install Git LFS](https://github.com/git-lfs/git-lfs/wiki/Installation) 11 | * `$ pip install numpy scipy pybullet sklearn` 12 | * `$ git lfs clone --branch master --recurse-submodules https://github.com/caelan/SS-Replan.git` 13 | * `$ cd SS-Replan` 14 | * `SS-Replan$ git lfs install` 15 | * `SS-Replan$ ./pddlstream/FastDownward/build.py release64` 16 | * `SS-Replan$ cd ss-pybullet/pybullet_tools/ikfast/franka_panda` 17 | * `SS-Replan/ss-pybullet/pybullet_tools/ikfast/franka_panda$ ./setup.py` 18 | 19 | It's also possible to use [TRAC-IK](http://wiki.ros.org/trac_ik) instead of [IKFast](http://openrave.org/docs/0.8.2/openravepy/ikfast/); however it requires installing ROS (`$ sudo apt install ros-kinetic-trac-ik`). 20 | 21 | 22 | 23 | ## PyBullet Examples 24 | 25 | 26 | * `SS-Replan$ git pull` 27 | * `SS-Replan$ git submodule update --init --recursive` 28 | * `SS-Replan$ ./run_pybullet.py [-h]` 29 | 30 | [](https://youtu.be/TvZqMDBZEnc) 31 | 32 | 33 | 34 | ## IsaacSim Examples 35 | 36 | Executed using the [IssacSim](https://developer.nvidia.com/isaac-sim) 3D robot simulation environment. 37 | 38 | [](https://youtu.be/XSZbCp0M1rw) 39 | 40 | ## Real-World Examples 41 | 42 | [](https://youtu.be/-Jl6GtvtWb8) 43 | 44 | 45 | 46 | ## Citation 47 | 48 | Caelan R. Garrett, Chris Paxton, Tomás Lozano-Pérez, Leslie P. Kaelbling, Dieter Fox. Online Replanning in Belief Space for Partially Observable Task and Motion Problems, IEEE International Conference on Robotics and Automation (ICRA), 2020. 49 | 50 | ## Publications 51 | 52 | * [Online Replanning in Belief Space for Partially Observable Task and Motion Problems](https://arxiv.org/abs/1911.04577) 53 | * [PDDLStream: Integrating Symbolic Planners and Blackbox Samplers via Optimistic Adaptive Planning](https://arxiv.org/abs/1802.08705) 54 | 55 | ## Videos 56 | 57 | * [SS-Replan](https://www.youtube.com/watch?v=o_RW91sm9PU&list=PLNpZKR7uv5ARTi1sNQRcd5rpa8XxamW2l) 58 | * [PDDLStream](https://www.youtube.com/playlist?list=PLNpZKR7uv5AQIyT6Az31a3WqiXyQJX7Rx) 59 | 60 | ## Resources 61 | 62 | * This repository uses [PDDLStream](https://github.com/caelan/pddlstream) to perform hybrid robotic planning. 63 | * PDDLStream leverages [FastDownward](http://www.fast-downward.org/), a classical planner, as a discrete search subroutine. 64 | * Common robotics primitives are implemented using [PyBullet](https://pypi.org/project/pybullet/). 65 | 66 | ### PDDLStream 67 | 68 | 69 | * [PDDLStream Paper](https://arxiv.org/abs/1802.08705) 70 | * [PDDLStream Github Repository](https://github.com/caelan/pddlstream) 71 | * [PDDLStream Tutorial](http://web.mit.edu/caelan/www/presentations/6.881_19-11-12.pdf) 72 | 73 | ### PDDL and FastDownward 74 | 75 | * [Planning Domain Definition Language (PDDL)](http://users.cecs.anu.edu.au/~patrik/pddlman/writing.html) 76 | * [PDDL Derived Predicates](https://www.cs.cmu.edu/afs/cs/project/jair/pub/volume28/coles07a-html/node18.html) 77 | * [FastDownward Homepage](http://www.fast-downward.org/) 78 | 79 | ### PyBullet 80 | 81 | * [PyBullet Package](https://pypi.org/project/pybullet/) 82 | * [PyBullet Quickstart Guide](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit) 83 | 84 | 87 | -------------------------------------------------------------------------------- /analyze_experiment.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | from __future__ import print_function 4 | 5 | import argparse 6 | import os 7 | import sys 8 | import numpy as np 9 | #import matplotlib.pyplot as plt 10 | 11 | sys.path.extend(os.path.abspath(os.path.join(os.getcwd(), d)) 12 | for d in ['pddlstream', 'ss-pybullet']) 13 | 14 | from itertools import islice 15 | 16 | #from run_experiment import DIRECTORY, MAX_TIME 17 | from pddlstream.utils import str_from_object, implies 18 | from pybullet_tools.utils import read_json, SEPARATOR, INF 19 | 20 | #from tabulate import tabulate 21 | 22 | from run_experiment import TASK_NAMES, POLICIES, MAX_TIME, name_from_policy, ERROR_OUTCOME 23 | 24 | # https://github.mit.edu/caelan/pddlstream-experiments/blob/master/analyze_experiment.py 25 | # https://github.mit.edu/Learning-and-Intelligent-Systems/ltamp_pr2/blob/d1e6024c5c13df7edeab3a271b745e656a794b02/learn_tools/analyze_experiment.py 26 | 27 | MAX_TRIALS = INF 28 | 29 | PRINT_ATTRIBUTES = [ 30 | 'achieved_goal', 31 | 'total_time', 32 | 'error', 33 | 'plan_time', 34 | #'num_iterations', 35 | #'num_constrained', 36 | #'num_unconstrained', 37 | #'num_successes', 38 | #'num_actions', 39 | 'peak_memory', 40 | 'num_commands', 41 | #'total_cost', 42 | ] 43 | 44 | ACHIEVED_GOAL = [ 45 | 'total_time', 46 | 'plan_time', 47 | 'num_actions', 48 | 'total_cost', 49 | ] 50 | 51 | POLICY_NAMES = [ 52 | 'Constain', 53 | 'Defer', 54 | 'Constrain+Defer', 55 | ] 56 | 57 | ################################################################################ 58 | 59 | def take(iterable, n=INF): 60 | if n == INF: 61 | n = None # NOTE - islice takes None instead of INF 62 | return islice(iterable, n) 63 | 64 | ################################################################################ 65 | 66 | # https://github.mit.edu/caelan/ss/blob/master/openrave/serial_analyze.py 67 | 68 | def main(): 69 | parser = argparse.ArgumentParser() 70 | parser.add_argument('experiments', nargs='+', help='Name of the experiment') 71 | args = parser.parse_args() 72 | 73 | outcomes_per_task = {} 74 | for path in args.experiments: 75 | for result in read_json(path): 76 | experiment = result['experiment'] 77 | problem = experiment['problem'] 78 | outcome = result['outcome'] 79 | #policy = frozenset(experiment['policy'].items()) 80 | policy = name_from_policy(experiment['policy']) 81 | outcomes_per_task.setdefault(problem['task'], {}).setdefault(policy, []).append(outcome) 82 | #outcomes_per_task['inspect_drawer']['constrain=0_defer=1'].append(ERROR_OUTCOME) 83 | #outcomes_per_task['detect_block']['constrain=1_defer=0'].append(ERROR_OUTCOME) 84 | 85 | # TODO: robust poses 86 | # TODO: intelligent IR for pour 87 | table = '' 88 | for task in TASK_NAMES: 89 | if task not in outcomes_per_task: 90 | continue 91 | print('\nTask: {}'.format(task)) 92 | items = [task] 93 | for policy in POLICIES: 94 | policy = name_from_policy(policy) 95 | if policy not in outcomes_per_task[task]: 96 | continue 97 | outcomes = list(take(outcomes_per_task[task][policy], MAX_TRIALS)) 98 | value_per_attribute = {} 99 | for outcome in outcomes: 100 | if outcome['error']: 101 | outcome.update(ERROR_OUTCOME) 102 | if MAX_TIME < outcome.get('total_time', INF): 103 | outcome['achieved_goal'] = False 104 | if not outcome['achieved_goal']: 105 | outcome['total_time'] = MAX_TIME 106 | outcome['plan_time'] = MAX_TIME 107 | for attribute, value in outcome.items(): 108 | if (attribute not in ['policy']) and (attribute in PRINT_ATTRIBUTES) and \ 109 | not isinstance(value, str) and implies(attribute in ACHIEVED_GOAL, 110 | outcome['achieved_goal']): 111 | value_per_attribute.setdefault(attribute, []).append(float(value)) 112 | 113 | statistics = {attribute: np.round(np.average(values), 3) # '{:.2f}'.format( 114 | for attribute, values in value_per_attribute.items()} # median, min, max of solved? 115 | statistics['trials'] = len(outcomes) 116 | print('{}: {}'.format(policy, str_from_object(statistics))) 117 | items += [ 118 | '{:.0f}'.format(100*statistics['achieved_goal']), 119 | '{:.0f}'.format(statistics['plan_time']), 120 | ] 121 | table += '{}\n\\\\ \hline\n'.format(' & '.join(items)) 122 | print(SEPARATOR) 123 | print(POLICIES) 124 | print(table) 125 | 126 | 127 | if __name__ == '__main__': 128 | main() -------------------------------------------------------------------------------- /collect_place.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | from __future__ import print_function 4 | 5 | import argparse 6 | import os 7 | import random 8 | import sys 9 | import time 10 | 11 | sys.path.extend(os.path.abspath(os.path.join(os.getcwd(), d)) 12 | for d in ['pddlstream', 'ss-pybullet']) 13 | 14 | from itertools import product 15 | 16 | from pybullet_tools.utils import wait_for_user, elapsed_time, multiply, \ 17 | invert, get_link_pose, has_gui, write_json, get_body_name, get_link_name, \ 18 | RED, BLUE, LockRenderer, child_link_from_joint, get_date, SEPARATOR, dump_body, safe_remove 19 | from src.utils import get_block_path, BLOCK_SIZES, BLOCK_COLORS, GRASP_TYPES, TOP_GRASP, \ 20 | SIDE_GRASP, BASE_JOINTS, joint_from_name, ALL_SURFACES, FRANKA_CARTER, EVE, DRAWERS, \ 21 | OPEN_SURFACES, ENV_SURFACES, CABINETS, ZED_LEFT_SURFACES 22 | from src.world import World 23 | from src.stream import get_stable_gen, get_grasp_gen, Z_EPSILON 24 | from src.streams.pick import get_pick_gen_fn 25 | from src.database import DATABASE_DIRECTORY, PLACE_IR_FILENAME, get_surface_reference_pose, get_place_path 26 | 27 | # TODO: condition on the object type (but allow a default object) 28 | # TODO: generalize to any manipulation with a movable entity 29 | # TODO: extend to pouring 30 | 31 | def collect_place(world, object_name, surface_name, grasp_type, args): 32 | date = get_date() 33 | #set_seed(args.seed) 34 | 35 | #dump_body(world.robot) 36 | surface_pose = get_surface_reference_pose(world.kitchen, surface_name) # TODO: assumes the drawer is open 37 | stable_gen_fn = get_stable_gen(world, z_offset=Z_EPSILON, visibility=False, 38 | learned=False, collisions=not args.cfree) 39 | grasp_gen_fn = get_grasp_gen(world) 40 | ik_ir_gen = get_pick_gen_fn(world, learned=False, collisions=not args.cfree, teleport=args.teleport) 41 | 42 | stable_gen = stable_gen_fn(object_name, surface_name) 43 | grasps = list(grasp_gen_fn(object_name, grasp_type)) 44 | 45 | robot_name = get_body_name(world.robot) 46 | path = get_place_path(robot_name, surface_name, grasp_type) 47 | print(SEPARATOR) 48 | print('Robot name: {} | Object name: {} | Surface name: {} | Grasp type: {} | Filename: {}'.format( 49 | robot_name, object_name, surface_name, grasp_type, path)) 50 | 51 | entries = [] 52 | start_time = time.time() 53 | failures = 0 54 | while (len(entries) < args.num_samples) and \ 55 | (elapsed_time(start_time) < args.max_time): #and (failures <= max_failures): 56 | (rel_pose,) = next(stable_gen) 57 | if rel_pose is None: 58 | break 59 | (grasp,) = random.choice(grasps) 60 | with LockRenderer(lock=True): 61 | result = next(ik_ir_gen(object_name, rel_pose, grasp), None) 62 | if result is None: 63 | print('Failure! | {} / {} [{:.3f}]'.format( 64 | len(entries), args.num_samples, elapsed_time(start_time))) 65 | failures += 1 66 | continue 67 | # TODO: ensure an arm motion exists 68 | bq, aq, at = result 69 | rel_pose.assign() 70 | bq.assign() 71 | aq.assign() 72 | base_pose = get_link_pose(world.robot, world.base_link) 73 | object_pose = rel_pose.get_world_from_body() 74 | tool_pose = multiply(object_pose, invert(grasp.grasp_pose)) 75 | entries.append({ 76 | 'tool_from_base': multiply(invert(tool_pose), base_pose), 77 | 'surface_from_object': multiply(invert(surface_pose), object_pose), 78 | 'base_from_object': multiply(invert(base_pose), object_pose), 79 | }) 80 | print('Success! | {} / {} [{:.3f}]'.format( 81 | len(entries), args.num_samples, elapsed_time(start_time))) 82 | if has_gui(): 83 | wait_for_user() 84 | #visualize_database(tool_from_base_list) 85 | if not entries: 86 | safe_remove(path) 87 | return None 88 | 89 | # Assuming the kitchen is fixed but the objects might be open world 90 | data = { 91 | 'date': date, 92 | 'robot_name': robot_name, # get_name | get_body_name | get_base_name | world.robot_name 93 | 'base_link': get_link_name(world.robot, world.base_link), 94 | 'tool_link': get_link_name(world.robot, world.tool_link), 95 | 'kitchen_name': get_body_name(world.kitchen), 96 | 'surface_name': surface_name, 97 | 'object_name': object_name, 98 | 'grasp_type': grasp_type, 99 | 'entries': entries, 100 | 'failures': failures, 101 | 'successes': len(entries), 102 | } 103 | 104 | write_json(path, data) 105 | print('Saved', path) 106 | return data 107 | 108 | ################################################################################ 109 | 110 | def main(): 111 | parser = argparse.ArgumentParser() 112 | #parser.add_argument('-attempts', default=100, type=int, 113 | # help='The number of attempts') 114 | parser.add_argument('-cfree', action='store_true', 115 | help='When enabled, disables collision checking (for debugging).') 116 | #parser.add_argument('-grasp_type', default=GRASP_TYPES[0], 117 | # help='Specifies the type of grasp.') 118 | #parser.add_argument('-problem', default='test_block', 119 | # help='The name of the problem to solve.') 120 | parser.add_argument('-max_time', default=10*60, type=float, 121 | help='The maximum runtime') 122 | parser.add_argument('-num_samples', default=1000, type=int, 123 | help='The number of samples') 124 | parser.add_argument('-robot', default=FRANKA_CARTER, choices=[FRANKA_CARTER, EVE], 125 | help='The robot to use.') 126 | parser.add_argument('-seed', default=None, 127 | help='The random seed to use.') 128 | parser.add_argument('-teleport', action='store_true', 129 | help='Uses unit costs') 130 | parser.add_argument('-visualize', action='store_true', 131 | help='When enabled, visualizes planning rather than the world (for debugging).') 132 | args = parser.parse_args() 133 | 134 | world = World(use_gui=args.visualize, robot_name=args.robot) 135 | #dump_body(world.robot) 136 | for joint in world.kitchen_joints: 137 | world.open_door(joint) # open_door | close_door 138 | world.open_gripper() 139 | # TODO: sample from set of objects? 140 | object_name = '{}_{}_block{}'.format(BLOCK_SIZES[-1], BLOCK_COLORS[0], 0) 141 | world.add_body(object_name) 142 | # TODO: could constrain Eve to be within a torso cone 143 | 144 | grasp_colors = { 145 | TOP_GRASP: RED, 146 | SIDE_GRASP: BLUE, 147 | } 148 | #combinations = list(product(OPEN_SURFACES, GRASP_TYPES)) \ 149 | # + [(surface_name, TOP_GRASP) for surface_name in DRAWERS] \ 150 | # + [(surface_name, SIDE_GRASP) for surface_name in CABINETS] # ENV_SURFACES 151 | combinations = [] 152 | for surface_name in ZED_LEFT_SURFACES: 153 | if surface_name in (OPEN_SURFACES + DRAWERS): 154 | combinations.append((surface_name, TOP_GRASP)) 155 | if surface_name in (OPEN_SURFACES + CABINETS): 156 | combinations.append((surface_name, SIDE_GRASP)) 157 | 158 | # TODO: parallelize 159 | print('Combinations:', combinations) 160 | wait_for_user('Start?') 161 | for surface_name, grasp_type in combinations: 162 | #draw_picks(world, object_name, surface_name, grasp_type, color=grasp_colors[grasp_type]) 163 | collect_place(world, object_name, surface_name, grasp_type, args) 164 | world.destroy() 165 | 166 | if __name__ == '__main__': 167 | main() 168 | 169 | -------------------------------------------------------------------------------- /collect_pull.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | from __future__ import print_function 4 | 5 | import argparse 6 | import os 7 | import sys 8 | import time 9 | 10 | sys.path.extend(os.path.abspath(os.path.join(os.getcwd(), d)) 11 | for d in ['pddlstream', 'ss-pybullet']) 12 | 13 | from pybullet_tools.pr2_primitives import Conf 14 | from pybullet_tools.utils import wait_for_user, elapsed_time, multiply, \ 15 | invert, get_link_pose, has_gui, write_json, get_body_name, get_link_name, \ 16 | get_joint_name, joint_from_name, get_date, SEPARATOR, safe_remove, link_from_name 17 | from src.utils import CABINET_JOINTS, DRAWER_JOINTS, KNOBS, ZED_LEFT_JOINTS 18 | from src.world import World 19 | from src.streams.press import get_press_gen_fn 20 | from src.streams.pull import get_pull_gen_fn 21 | from src.database import get_joint_reference_pose, get_pull_path, is_press 22 | 23 | # TODO: generalize to any manipulation with a fixed entity 24 | 25 | def collect_pull(world, joint_name, args): 26 | date = get_date() 27 | #set_seed(args.seed) 28 | 29 | robot_name = get_body_name(world.robot) 30 | if is_press(joint_name): 31 | press_gen = get_press_gen_fn(world, collisions=not args.cfree, teleport=args.teleport, learned=False) 32 | else: 33 | joint = joint_from_name(world.kitchen, joint_name) 34 | open_conf = Conf(world.kitchen, [joint], [world.open_conf(joint)]) 35 | closed_conf = Conf(world.kitchen, [joint], [world.closed_conf(joint)]) 36 | pull_gen = get_pull_gen_fn(world, collisions=not args.cfree, teleport=args.teleport, learned=False) 37 | #handle_link, handle_grasp, _ = get_handle_grasp(world, joint) 38 | 39 | path = get_pull_path(robot_name, joint_name) 40 | print(SEPARATOR) 41 | print('Robot name {} | Joint name: {} | Filename: {}'.format(robot_name, joint_name, path)) 42 | 43 | entries = [] 44 | failures = 0 45 | start_time = time.time() 46 | while (len(entries) < args.num_samples) and \ 47 | (elapsed_time(start_time) < args.max_time): 48 | if is_press(joint_name): 49 | result = next(press_gen(joint_name), None) 50 | else: 51 | result = next(pull_gen(joint_name, open_conf, closed_conf), None) # Open to closed 52 | if result is None: 53 | print('Failure! | {} / {} [{:.3f}]'.format( 54 | len(entries), args.num_samples, elapsed_time(start_time))) 55 | failures += 1 56 | continue 57 | if not is_press(joint_name): 58 | open_conf.assign() 59 | joint_pose = get_joint_reference_pose(world.kitchen, joint_name) 60 | bq, aq1 = result[:2] 61 | bq.assign() 62 | aq1.assign() 63 | #next(at.commands[2].iterate(None, None)) 64 | base_pose = get_link_pose(world.robot, world.base_link) 65 | #handle_pose = get_link_pose(world.robot, base_link) 66 | entries.append({ 67 | 'joint_from_base': multiply(invert(joint_pose), base_pose), 68 | }) 69 | print('Success! | {} / {} [{:.3f}]'.format( 70 | len(entries), args.num_samples, elapsed_time(start_time))) 71 | if has_gui(): 72 | wait_for_user() 73 | if not entries: 74 | safe_remove(path) 75 | return None 76 | #visualize_database(joint_from_base_list) 77 | 78 | # Assuming the kitchen is fixed but the objects might be open world 79 | # TODO: could store per data point 80 | data = { 81 | 'date': date, 82 | 'robot_name': robot_name, # get_name | get_body_name | get_base_name | world.robot_name 83 | 'base_link': get_link_name(world.robot, world.base_link), 84 | 'tool_link': get_link_name(world.robot, world.tool_link), 85 | 'kitchen_name': get_body_name(world.kitchen), 86 | 'joint_name': joint_name, 87 | 'entries': entries, 88 | 'failures': failures, 89 | 'successes': len(entries), 90 | } 91 | if not is_press(joint_name): 92 | data.update({ 93 | 'open_conf': open_conf.values, 94 | 'closed_conf': closed_conf.values, 95 | }) 96 | 97 | write_json(path, data) 98 | print('Saved', path) 99 | return data 100 | 101 | ################################################################################ 102 | 103 | def main(): 104 | parser = argparse.ArgumentParser() 105 | #parser.add_argument('-attempts', default=100, type=int, 106 | # help='The number of attempts') 107 | parser.add_argument('-cfree', action='store_true', 108 | help='When enabled, disables collision checking (for debugging).') 109 | parser.add_argument('-max_time', default=10 * 60, type=float, 110 | help='The maximum runtime') 111 | parser.add_argument('-num_samples', default=1000, type=int, 112 | help='The number of samples') 113 | parser.add_argument('-seed', default=None, 114 | help='The random seed to use.') 115 | parser.add_argument('-teleport', action='store_true', 116 | help='Uses unit costs') 117 | parser.add_argument('-visualize', action='store_true', 118 | help='When enabled, visualizes planning rather than the world (for debugging).') 119 | args = parser.parse_args() 120 | # TODO: could record the full trajectories here 121 | 122 | world = World(use_gui=args.visualize) 123 | world.open_gripper() 124 | 125 | #joint_names = DRAWER_JOINTS + CABINET_JOINTS 126 | joint_names = ZED_LEFT_JOINTS 127 | print('Joints:', joint_names) 128 | print('Knobs:', KNOBS) 129 | wait_for_user('Start?') 130 | for joint_name in joint_names: 131 | collect_pull(world, joint_name, args) 132 | for knob_name in KNOBS: 133 | collect_pull(world, knob_name, args) 134 | 135 | world.destroy() 136 | 137 | if __name__ == '__main__': 138 | main() 139 | 140 | -------------------------------------------------------------------------------- /databases/franka_carter-chewie_door_right_joint-pull.json: -------------------------------------------------------------------------------- 1 | { 2 | "base_link": "chassis_link", 3 | "closed_conf": [ 4 | 0.0 5 | ], 6 | "date": "19-08-28_18-00-03", 7 | "entries": [ 8 | { 9 | "joint_from_base": [ 10 | [ 11 | -0.6932004690170288, 12 | -0.042288411408662796, 13 | -1.2169452905654907 14 | ], 15 | [ 16 | 0.0, 17 | 0.0, 18 | -0.7777998447418213, 19 | 0.6285121440887451 20 | ] 21 | ] 22 | }, 23 | { 24 | "joint_from_base": [ 25 | [ 26 | -0.6796948909759521, 27 | 0.2507636547088623, 28 | -1.2169452905654907 29 | ], 30 | [ 31 | 0.0, 32 | 0.0, 33 | 0.3088754415512085, 34 | 0.9511024951934814 35 | ] 36 | ] 37 | }, 38 | { 39 | "joint_from_base": [ 40 | [ 41 | -0.674193263053894, 42 | 0.20217692852020264, 43 | -1.2169452905654907 44 | ], 45 | [ 46 | 0.0, 47 | 0.0, 48 | 0.23623493313789368, 49 | 0.9716959595680237 50 | ] 51 | ] 52 | }, 53 | { 54 | "joint_from_base": [ 55 | [ 56 | -0.7105271816253662, 57 | 0.12671725451946259, 58 | -1.2169452905654907 59 | ], 60 | [ 61 | 0.0, 62 | 0.0, 63 | -0.3499926030635834, 64 | 0.9367524981498718 65 | ] 66 | ] 67 | }, 68 | { 69 | "joint_from_base": [ 70 | [ 71 | -0.7887805104255676, 72 | 0.16731426119804382, 73 | -1.2169452905654907 74 | ], 75 | [ 76 | 0.0, 77 | 0.0, 78 | 0.998428463935852, 79 | -0.05604110285639763 80 | ] 81 | ] 82 | }, 83 | { 84 | "joint_from_base": [ 85 | [ 86 | -0.7776607871055603, 87 | 0.08335064351558685, 88 | -1.2169452905654907 89 | ], 90 | [ 91 | 0.0, 92 | 0.0, 93 | -0.8380532264709473, 94 | 0.5455883741378784 95 | ] 96 | ] 97 | }, 98 | { 99 | "joint_from_base": [ 100 | [ 101 | -0.7569770216941833, 102 | 0.21547456085681915, 103 | -1.2169452905654907 104 | ], 105 | [ 106 | 0.0, 107 | 0.0, 108 | 0.8336388468742371, 109 | 0.5523099899291992 110 | ] 111 | ] 112 | }, 113 | { 114 | "joint_from_base": [ 115 | [ 116 | -0.7215292453765869, 117 | 0.17659586668014526, 118 | -1.2169452905654907 119 | ], 120 | [ 121 | 0.0, 122 | 0.0, 123 | -0.47966092824935913, 124 | 0.8774539232254028 125 | ] 126 | ] 127 | }, 128 | { 129 | "joint_from_base": [ 130 | [ 131 | -0.7359917163848877, 132 | 0.15873242914676666, 133 | -1.2169452905654907 134 | ], 135 | [ 136 | 0.0, 137 | 0.0, 138 | -0.308987021446228, 139 | 0.951066255569458 140 | ] 141 | ] 142 | }, 143 | { 144 | "joint_from_base": [ 145 | [ 146 | -0.6701152920722961, 147 | 0.22033168375492096, 148 | -1.2169452905654907 149 | ], 150 | [ 151 | 0.0, 152 | 0.0, 153 | 0.6059957146644592, 154 | 0.7954678535461426 155 | ] 156 | ] 157 | }, 158 | { 159 | "joint_from_base": [ 160 | [ 161 | -0.6994938254356384, 162 | 0.27128395438194275, 163 | -1.2169452905654907 164 | ], 165 | [ 166 | 0.0, 167 | 0.0, 168 | 0.7605664730072021, 169 | 0.6492601037025452 170 | ] 171 | ] 172 | }, 173 | { 174 | "joint_from_base": [ 175 | [ 176 | -0.7303630709648132, 177 | 0.11493568122386932, 178 | -1.2169452905654907 179 | ], 180 | [ 181 | 0.0, 182 | 0.0, 183 | -0.4007868468761444, 184 | 0.9161713123321533 185 | ] 186 | ] 187 | }, 188 | { 189 | "joint_from_base": [ 190 | [ 191 | -0.7069926857948303, 192 | 0.1391979604959488, 193 | -1.2169452905654907 194 | ], 195 | [ 196 | 0.0, 197 | 0.0, 198 | 0.13978953659534454, 199 | 0.9901812672615051 200 | ] 201 | ] 202 | }, 203 | { 204 | "joint_from_base": [ 205 | [ 206 | -0.660511314868927, 207 | 0.24047622084617615, 208 | -1.2169452905654907 209 | ], 210 | [ 211 | 0.0, 212 | 0.0, 213 | 0.06566781550645828, 214 | 0.9978415369987488 215 | ] 216 | ] 217 | }, 218 | { 219 | "joint_from_base": [ 220 | [ 221 | -0.7796976566314697, 222 | 0.10772522538900375, 223 | -1.2169452905654907 224 | ], 225 | [ 226 | 0.0, 227 | 0.0, 228 | 0.9929993152618408, 229 | 0.11812034994363785 230 | ] 231 | ] 232 | }, 233 | { 234 | "joint_from_base": [ 235 | [ 236 | -0.8232045769691467, 237 | 0.15023671090602875, 238 | -1.2169452905654907 239 | ], 240 | [ 241 | 0.0, 242 | 0.0, 243 | 0.9398326277732849, 244 | -0.34163522720336914 245 | ] 246 | ] 247 | }, 248 | { 249 | "joint_from_base": [ 250 | [ 251 | -0.7283162474632263, 252 | 0.17689739167690277, 253 | -1.2169452905654907 254 | ], 255 | [ 256 | 0.0, 257 | 0.0, 258 | -0.7333201169967651, 259 | 0.6798835396766663 260 | ] 261 | ] 262 | }, 263 | { 264 | "joint_from_base": [ 265 | [ 266 | -0.655180037021637, 267 | 0.24750860035419464, 268 | -1.2169452905654907 269 | ], 270 | [ 271 | 0.0, 272 | 0.0, 273 | 0.7210490107536316, 274 | 0.692884087562561 275 | ] 276 | ] 277 | }, 278 | { 279 | "joint_from_base": [ 280 | [ 281 | -0.7033132314682007, 282 | -0.03720506280660629, 283 | -1.2169452905654907 284 | ], 285 | [ 286 | 0.0, 287 | 0.0, 288 | -0.8067070245742798, 289 | 0.5909515619277954 290 | ] 291 | ] 292 | }, 293 | { 294 | "joint_from_base": [ 295 | [ 296 | -0.6962878704071045, 297 | 0.27078408002853394, 298 | -1.2169452905654907 299 | ], 300 | [ 301 | 0.0, 302 | 0.0, 303 | 0.8631197214126587, 304 | 0.5049993395805359 305 | ] 306 | ] 307 | }, 308 | { 309 | "joint_from_base": [ 310 | [ 311 | -0.6700754165649414, 312 | 0.1942470818758011, 313 | -1.2169452905654907 314 | ], 315 | [ 316 | 0.0, 317 | 0.0, 318 | 0.5804010033607483, 319 | 0.8143308162689209 320 | ] 321 | ] 322 | }, 323 | { 324 | "joint_from_base": [ 325 | [ 326 | -0.7259858846664429, 327 | 0.19296401739120483, 328 | -1.2169452905654907 329 | ], 330 | [ 331 | 0.0, 332 | 0.0, 333 | 0.7978453636169434, 334 | 0.6028621792793274 335 | ] 336 | ] 337 | }, 338 | { 339 | "joint_from_base": [ 340 | [ 341 | -0.7079815864562988, 342 | 0.13766057789325714, 343 | -1.2169452905654907 344 | ], 345 | [ 346 | 0.0, 347 | 0.0, 348 | 0.7020055055618286, 349 | 0.7121715545654297 350 | ] 351 | ] 352 | }, 353 | { 354 | "joint_from_base": [ 355 | [ 356 | -0.7506356239318848, 357 | 0.1348080337047577, 358 | -1.2169452905654907 359 | ], 360 | [ 361 | 0.0, 362 | 0.0, 363 | -0.1178731620311737, 364 | 0.9930286407470703 365 | ] 366 | ] 367 | }, 368 | { 369 | "joint_from_base": [ 370 | [ 371 | -0.6751003265380859, 372 | 0.17517435550689697, 373 | -1.2169452905654907 374 | ], 375 | [ 376 | 0.0, 377 | 0.0, 378 | 0.581085741519928, 379 | 0.8138423562049866 380 | ] 381 | ] 382 | }, 383 | { 384 | "joint_from_base": [ 385 | [ 386 | -0.7065150737762451, 387 | 0.15276335179805756, 388 | -1.2169452905654907 389 | ], 390 | [ 391 | 0.0, 392 | 0.0, 393 | -0.26981642842292786, 394 | 0.9629117846488953 395 | ] 396 | ] 397 | }, 398 | { 399 | "joint_from_base": [ 400 | [ 401 | -0.667980432510376, 402 | 0.2561432719230652, 403 | -1.2169452905654907 404 | ], 405 | [ 406 | 0.0, 407 | 0.0, 408 | 0.26617231965065, 409 | 0.9639254808425903 410 | ] 411 | ] 412 | } 413 | ], 414 | "failures": 210, 415 | "filename": "franka_carter-chewie_door_right_joint-pull.json", 416 | "joint_name": "chewie_door_right_joint", 417 | "kitchen_name": "kitchen_part_right", 418 | "open_conf": [ 419 | 1.3962634015954636 420 | ], 421 | "robot_name": "franka_carter", 422 | "successes": 27, 423 | "tool_link": "right_gripper" 424 | } -------------------------------------------------------------------------------- /databases/franka_carter-dagger_door_left_joint-pull.json: -------------------------------------------------------------------------------- 1 | { 2 | "base_link": "chassis_link", 3 | "closed_conf": [ 4 | 0.0 5 | ], 6 | "date": "19-08-28_18-10-05", 7 | "entries": [ 8 | { 9 | "joint_from_base": [ 10 | [ 11 | -0.7662492394447327, 12 | -0.22688740491867065, 13 | -1.2169452905654907 14 | ], 15 | [ 16 | 0.0, 17 | 0.0, 18 | 0.9997261762619019, 19 | -0.023398539051413536 20 | ] 21 | ] 22 | }, 23 | { 24 | "joint_from_base": [ 25 | [ 26 | -0.7498241066932678, 27 | -0.24519532918930054, 28 | -1.2169452905654907 29 | ], 30 | [ 31 | 0.0, 32 | 0.0, 33 | 0.9486181735992432, 34 | -0.31642305850982666 35 | ] 36 | ] 37 | }, 38 | { 39 | "joint_from_base": [ 40 | [ 41 | -0.7176094055175781, 42 | -0.24561363458633423, 43 | -1.2169452905654907 44 | ], 45 | [ 46 | 0.0, 47 | 0.0, 48 | -0.7705910801887512, 49 | 0.6373298168182373 50 | ] 51 | ] 52 | }, 53 | { 54 | "joint_from_base": [ 55 | [ 56 | -0.6886841654777527, 57 | -0.10480785369873047, 58 | -1.2169452905654907 59 | ], 60 | [ 61 | 0.0, 62 | 0.0, 63 | -0.37385430932044983, 64 | 0.9274874329566956 65 | ] 66 | ] 67 | }, 68 | { 69 | "joint_from_base": [ 70 | [ 71 | -0.673018753528595, 72 | -0.17942219972610474, 73 | -1.2169452905654907 74 | ], 75 | [ 76 | 0.0, 77 | 0.0, 78 | -0.2808111310005188, 79 | 0.9597630500793457 80 | ] 81 | ] 82 | }, 83 | { 84 | "joint_from_base": [ 85 | [ 86 | -0.6933398842811584, 87 | -0.2330377697944641, 88 | -1.2169452905654907 89 | ], 90 | [ 91 | 0.0, 92 | 0.0, 93 | -0.5258731245994568, 94 | 0.8505630493164062 95 | ] 96 | ] 97 | }, 98 | { 99 | "joint_from_base": [ 100 | [ 101 | -0.6705242395401001, 102 | -0.14936965703964233, 103 | -1.2169452905654907 104 | ], 105 | [ 106 | 0.0, 107 | 0.0, 108 | -0.5857842564582825, 109 | 0.8104670643806458 110 | ] 111 | ] 112 | }, 113 | { 114 | "joint_from_base": [ 115 | [ 116 | -0.7442880868911743, 117 | -0.2227063775062561, 118 | -1.2169452905654907 119 | ], 120 | [ 121 | 0.0, 122 | 0.0, 123 | 0.9999974370002747, 124 | 0.002266519470140338 125 | ] 126 | ] 127 | }, 128 | { 129 | "joint_from_base": [ 130 | [ 131 | -0.7155713438987732, 132 | -0.16850072145462036, 133 | -1.2169452905654907 134 | ], 135 | [ 136 | 0.0, 137 | 0.0, 138 | -0.6676768660545349, 139 | 0.7444512248039246 140 | ] 141 | ] 142 | }, 143 | { 144 | "joint_from_base": [ 145 | [ 146 | -0.7192732095718384, 147 | -0.08819580078125, 148 | -1.2169452905654907 149 | ], 150 | [ 151 | 0.0, 152 | 0.0, 153 | -0.3994907736778259, 154 | 0.9167371988296509 155 | ] 156 | ] 157 | }, 158 | { 159 | "joint_from_base": [ 160 | [ 161 | -0.6763319373130798, 162 | -0.2569482922554016, 163 | -1.2169452905654907 164 | ], 165 | [ 166 | 0.0, 167 | 0.0, 168 | -0.6044235825538635, 169 | 0.7966631054878235 170 | ] 171 | ] 172 | }, 173 | { 174 | "joint_from_base": [ 175 | [ 176 | -0.7156413793563843, 177 | -0.1304713487625122, 178 | -1.2169452905654907 179 | ], 180 | [ 181 | 0.0, 182 | 0.0, 183 | 0.7120883464813232, 184 | 0.7020899057388306 185 | ] 186 | ] 187 | }, 188 | { 189 | "joint_from_base": [ 190 | [ 191 | -0.7083742022514343, 192 | -0.09547531604766846, 193 | -1.2169452905654907 194 | ], 195 | [ 196 | 0.0, 197 | 0.0, 198 | 0.7907062768936157, 199 | 0.6121957302093506 200 | ] 201 | ] 202 | }, 203 | { 204 | "joint_from_base": [ 205 | [ 206 | -0.6891417503356934, 207 | -0.12778747081756592, 208 | -1.2169452905654907 209 | ], 210 | [ 211 | 0.0, 212 | 0.0, 213 | -0.4570354223251343, 214 | 0.8894485235214233 215 | ] 216 | ] 217 | }, 218 | { 219 | "joint_from_base": [ 220 | [ 221 | -0.7046915888786316, 222 | -0.1383976936340332, 223 | -1.2169452905654907 224 | ], 225 | [ 226 | 0.0, 227 | 0.0, 228 | 0.5132229328155518, 229 | 0.8582552671432495 230 | ] 231 | ] 232 | }, 233 | { 234 | "joint_from_base": [ 235 | [ 236 | -0.7359642386436462, 237 | -0.10315173864364624, 238 | -1.2169452905654907 239 | ], 240 | [ 241 | 0.0, 242 | 0.0, 243 | -0.7330292463302612, 244 | 0.6801971197128296 245 | ] 246 | ] 247 | }, 248 | { 249 | "joint_from_base": [ 250 | [ 251 | -0.7324673533439636, 252 | -0.06063920259475708, 253 | -1.2169452905654907 254 | ], 255 | [ 256 | 0.0, 257 | 0.0, 258 | -0.6334324479103088, 259 | 0.7737979888916016 260 | ] 261 | ] 262 | }, 263 | { 264 | "joint_from_base": [ 265 | [ 266 | -0.7866590023040771, 267 | -0.22075289487838745, 268 | -1.2169452905654907 269 | ], 270 | [ 271 | 0.0, 272 | 0.0, 273 | -0.6639237999916077, 274 | 0.7478002905845642 275 | ] 276 | ] 277 | }, 278 | { 279 | "joint_from_base": [ 280 | [ 281 | -0.6971032023429871, 282 | -0.13308227062225342, 283 | -1.2169452905654907 284 | ], 285 | [ 286 | 0.0, 287 | 0.0, 288 | -0.6087929606437683, 289 | 0.793329119682312 290 | ] 291 | ] 292 | }, 293 | { 294 | "joint_from_base": [ 295 | [ 296 | -0.719485878944397, 297 | -0.1416233777999878, 298 | -1.2169452905654907 299 | ], 300 | [ 301 | 0.0, 302 | 0.0, 303 | -0.6287273168563843, 304 | 0.7776258587837219 305 | ] 306 | ] 307 | }, 308 | { 309 | "joint_from_base": [ 310 | [ 311 | -0.7028821110725403, 312 | -0.22472673654556274, 313 | -1.2169452905654907 314 | ], 315 | [ 316 | 0.0, 317 | 0.0, 318 | -0.6420638561248779, 319 | 0.7666511535644531 320 | ] 321 | ] 322 | }, 323 | { 324 | "joint_from_base": [ 325 | [ 326 | -0.7056621313095093, 327 | -0.20560258626937866, 328 | -1.2169452905654907 329 | ], 330 | [ 331 | 0.0, 332 | 0.0, 333 | -0.5132318139076233, 334 | 0.8582500219345093 335 | ] 336 | ] 337 | }, 338 | { 339 | "joint_from_base": [ 340 | [ 341 | -0.7108388543128967, 342 | -0.0024167895317077637, 343 | -1.2169452905654907 344 | ], 345 | [ 346 | 0.0, 347 | 0.0, 348 | 0.8267821669578552, 349 | 0.5625222325325012 350 | ] 351 | ] 352 | }, 353 | { 354 | "joint_from_base": [ 355 | [ 356 | -0.6966313719749451, 357 | -0.19307547807693481, 358 | -1.2169452905654907 359 | ], 360 | [ 361 | 0.0, 362 | 0.0, 363 | -0.3163325786590576, 364 | 0.9486483335494995 365 | ] 366 | ] 367 | }, 368 | { 369 | "joint_from_base": [ 370 | [ 371 | -0.7675226926803589, 372 | -0.23986917734146118, 373 | -1.2169452905654907 374 | ], 375 | [ 376 | 0.0, 377 | 0.0, 378 | 0.951967179775238, 379 | -0.3062001168727875 380 | ] 381 | ] 382 | }, 383 | { 384 | "joint_from_base": [ 385 | [ 386 | -0.7442502379417419, 387 | -0.09266543388366699, 388 | -1.2169452905654907 389 | ], 390 | [ 391 | 0.0, 392 | 0.0, 393 | 0.9196993708610535, 394 | 0.39262330532073975 395 | ] 396 | ] 397 | }, 398 | { 399 | "joint_from_base": [ 400 | [ 401 | -0.7434874773025513, 402 | -0.09652280807495117, 403 | -1.2169452905654907 404 | ], 405 | [ 406 | 0.0, 407 | 0.0, 408 | -0.499350905418396, 409 | 0.8663998246192932 410 | ] 411 | ] 412 | }, 413 | { 414 | "joint_from_base": [ 415 | [ 416 | -0.7280686497688293, 417 | -0.24332255125045776, 418 | -1.2169452905654907 419 | ], 420 | [ 421 | 0.0, 422 | 0.0, 423 | 0.8879951238632202, 424 | -0.45985284447669983 425 | ] 426 | ] 427 | }, 428 | { 429 | "joint_from_base": [ 430 | [ 431 | -0.7312548160552979, 432 | -0.1340985894203186, 433 | -1.2169452905654907 434 | ], 435 | [ 436 | 0.0, 437 | 0.0, 438 | -0.7363916635513306, 439 | 0.6765554547309875 440 | ] 441 | ] 442 | }, 443 | { 444 | "joint_from_base": [ 445 | [ 446 | -0.8184146285057068, 447 | -0.19347447156906128, 448 | -1.2169452905654907 449 | ], 450 | [ 451 | 0.0, 452 | 0.0, 453 | 0.9869767427444458, 454 | -0.16086305677890778 455 | ] 456 | ] 457 | }, 458 | { 459 | "joint_from_base": [ 460 | [ 461 | -0.7051253914833069, 462 | -0.09400230646133423, 463 | -1.2169452905654907 464 | ], 465 | [ 466 | 0.0, 467 | 0.0, 468 | -0.699666440486908, 469 | 0.7144696116447449 470 | ] 471 | ] 472 | }, 473 | { 474 | "joint_from_base": [ 475 | [ 476 | -0.6979915499687195, 477 | -0.009939730167388916, 478 | -1.2169452905654907 479 | ], 480 | [ 481 | 0.0, 482 | 0.0, 483 | -0.6572491526603699, 484 | 0.7536734342575073 485 | ] 486 | ] 487 | }, 488 | { 489 | "joint_from_base": [ 490 | [ 491 | -0.7284576892852783, 492 | 0.06514358520507812, 493 | -1.2169452905654907 494 | ], 495 | [ 496 | 0.0, 497 | 0.0, 498 | 0.7419798374176025, 499 | 0.6704221963882446 500 | ] 501 | ] 502 | } 503 | ], 504 | "failures": 133, 505 | "filename": "franka_carter-dagger_door_left_joint-pull.json", 506 | "joint_name": "dagger_door_left_joint", 507 | "kitchen_name": "kitchen_part_right", 508 | "open_conf": [ 509 | -1.3962634015954636 510 | ], 511 | "robot_name": "franka_carter", 512 | "successes": 33, 513 | "tool_link": "right_gripper" 514 | } -------------------------------------------------------------------------------- /images/put_spam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/caelan/SS-Replan/da4ae12fe8e345ba636aff4cc787f9e35c4d12e7/images/put_spam.png -------------------------------------------------------------------------------- /images/stow_block.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/caelan/SS-Replan/da4ae12fe8e345ba636aff4cc787f9e35c4d12e7/images/stow_block.png -------------------------------------------------------------------------------- /kitchen_poses.json: -------------------------------------------------------------------------------- 1 | { 2 | "axe": [ 3 | [ 4 | -0.37000009536743228, 5 | -2.6373074483871433, 6 | 0.0099777364730835316 7 | ], 8 | [ 9 | 0.0, 10 | 0.0, 11 | 0.70710678118654757, 12 | 0.70710678118654746 13 | ] 14 | ], 15 | "baker": [ 16 | [ 17 | -0.37000009536743317, 18 | -0.93190376281738008, 19 | 0.0099777364730835316 20 | ], 21 | [ 22 | 0.0, 23 | 0.0, 24 | 0.70710678118654757, 25 | 0.70710678118654746 26 | ] 27 | ], 28 | "chassis_link": [ 29 | [ 30 | 0.72944477081298587, 31 | 0.79999979019165313, 32 | -1.2190527385473251 33 | ], 34 | [ 35 | -0.0020655463157518571, 36 | 5.7540325868062326e-08, 37 | 0.99999786656239875, 38 | 1.9724724925916221e-05 39 | ] 40 | ], 41 | "chewie": [ 42 | [ 43 | -0.37000009536743317, 44 | -0.35949031829833711, 45 | 0.0099777364730835316 46 | ], 47 | [ 48 | 0.0, 49 | 0.0, 50 | 0.70710678118654757, 51 | 0.70710678118654746 52 | ] 53 | ], 54 | "dagger": [ 55 | [ 56 | -0.37000009536743317, 57 | 1.1900621223449734, 58 | 0.0099777364730835316 59 | ], 60 | [ 61 | 0.0, 62 | 0.0, 63 | 0.70710678118654757, 64 | 0.70710678118654746 65 | ] 66 | ], 67 | "dishwasher": [ 68 | [ 69 | -0.36777946472168122, 70 | -1.3406650257110568, 71 | -1.4600004600000238 72 | ], 73 | [ 74 | 0.0, 75 | 0.0, 76 | 0.70710678118654757, 77 | 0.70710678118654746 78 | ] 79 | ], 80 | "echo": [ 81 | [ 82 | -0.37000009536743228, 83 | -2.8762687873840305, 84 | -1.46 85 | ], 86 | [ 87 | 0.0, 88 | 0.0, 89 | 0.70710678118654757, 90 | 0.70710678118654746 91 | ] 92 | ], 93 | "extractor_hood": [ 94 | [ 95 | -0.36905405044555817, 96 | 0.41537960052490508, 97 | 0.071379222869873082 98 | ], 99 | [ 100 | 0.0, 101 | 0.0, 102 | 0.70710678118654757, 103 | 0.70710678118654746 104 | ] 105 | ], 106 | "fox": [ 107 | [ 108 | -0.37000009536743228, 109 | -2.4873519372940036, 110 | -1.46 111 | ], 112 | [ 113 | 0.0, 114 | 0.0, 115 | 0.70710678118654757, 116 | 0.70710678118654746 117 | ] 118 | ], 119 | "fridge": [ 120 | [ 121 | -0.334190578460694, 122 | -3.9947367739677402, 123 | -1.4600001499999911 124 | ], 125 | [ 126 | 0.0, 127 | 0.0, 128 | 0.70710678118654757, 129 | 0.70710678118654746 130 | ] 131 | ], 132 | "golf": [ 133 | [ 134 | -0.37000009536743228, 135 | -1.8082445096969577, 136 | -1.46 137 | ], 138 | [ 139 | 0.0, 140 | 0.0, 141 | 0.70710678118654757, 142 | 0.70710678118654746 143 | ] 144 | ], 145 | "hitman": [ 146 | [ 147 | -0.37000009536743317, 148 | -0.34191553115844453, 149 | -1.46 150 | ], 151 | [ 152 | 0.0, 153 | 0.0, 154 | 0.70710678118654757, 155 | 0.70710678118654746 156 | ] 157 | ], 158 | "indigo": [ 159 | [ 160 | -0.37000009536743317, 161 | 1.1968627738952664, 162 | -1.46 163 | ], 164 | [ 165 | 0.0, 166 | 0.0, 167 | 0.70710678118654757, 168 | 0.70710678118654746 169 | ] 170 | ], 171 | "range": [ 172 | [ 173 | -0.37000009536743317, 174 | 0.047457532882693165, 175 | -1.46 176 | ], 177 | [ 178 | 0.0, 179 | 0.0, 180 | 0.70710678118654757, 181 | 0.70710678118654746 182 | ] 183 | ], 184 | "table": [ 185 | [ 186 | 2.9685929107666009, 187 | -1.596018118858334, 188 | -1.4600001525878872 189 | ], 190 | [ 191 | 0.0, 192 | 0.0, 193 | 0.99999999999967937, 194 | -8.0079010167403293e-07 195 | ] 196 | ], 197 | "zed_left": [ 198 | [ 199 | 0.83000019073486087, 200 | 1.1799999046325711, 201 | 0.24000004768371586 202 | ], 203 | [ 204 | -0.2554418281651456, 205 | -0.052707618116508051, 206 | 0.95427802498440328, 207 | -0.14603023812895388 208 | ] 209 | ] 210 | } -------------------------------------------------------------------------------- /pddl/domain.pddl: -------------------------------------------------------------------------------- 1 | (define (domain nvidia-tamp) 2 | (:requirements :strips :equality) 3 | (:constants 4 | @world @gripper @stove @none 5 | @open @closed 6 | @rest_aq ; @calibrate_aq 7 | @top @side 8 | @open_gq @closed_gq 9 | @bq0 10 | ) 11 | (:predicates 12 | (Type ?t ?b) 13 | (Stackable ?o ?r) 14 | (Cookable ?o) 15 | (Stove ?r) 16 | (NoisyBase) 17 | (Obstacle ?o) 18 | (Counter ?o) 19 | (Drawer ?o) 20 | (Entity ?o) 21 | (Graspable ?o) 22 | (Joint ?j) 23 | (StoveKnob ?s ?k) 24 | (Above ?j1 ?j2) 25 | (Adjacent ?j1 ?j2) 26 | (Pressed ?k) 27 | 28 | (CheckNearby ?o) 29 | (NearPose ?o2 ?wp2 ?bq) 30 | (NearJoint ?j ?bq) 31 | (InitBConf ?bq) 32 | 33 | (Pick ?o ?wp ?g ?bq ?aq ?at) 34 | (Pull ?j ?q1 ?q2 ?bq ?aq1 ?aq2 ?at) 35 | (Press ?k ?bq ?aq ?at) 36 | (BaseMotion ?bq1 ?bq2 ?aq ?bt) 37 | (ArmMotion ?bq ?aq1 ?aq2 ?at) 38 | (GripperMotion ?gq1 ?gq2 ?gt) 39 | (CalibrateMotion ?bq ?aq ?at) 40 | 41 | (Detect ?o ?wp ?r) 42 | (Sample ?s) 43 | (Dist ?d) 44 | (Value ?wp) 45 | (BeliefUpdate ?o ?rp1 ?obs ?rp2) 46 | 47 | (Grasp ?o ?g) 48 | (BTraj ?bt) 49 | (ATraj ?at) 50 | (BConf ?bq) 51 | (AConf ?bq ?aq) 52 | (GConf ?gq) 53 | (Angle ?j ?a) 54 | (Ray ?r) 55 | 56 | (CFreeRelPoseRelPose ?o1 ?rp1 ?o2 ?rp2 ?s) 57 | (CFreeBConfPose ?bq ?o2 ?wp2) 58 | (CFreeApproachPose ?o1 ?wp1 ?g ?o2 ?wp2) 59 | (CFreeTrajPose ?t ?o2 ?wp2) 60 | (CFreeWorldPose ?o1 ?wp1) 61 | (CFreeWorldPoseWorldPose ?o1 ?wp1 ?o2 ?wp2) 62 | (CFreeAngleTraj ?j1 ?a1 ?a2 ?o2 ?wp2) 63 | 64 | (OFreeRayPose ?r ?o ?wp) 65 | (OFreeRayGrasp ?r ?bq ?aq ?o ?g) 66 | 67 | (HandEmpty) 68 | (AtBConf ?bq) 69 | (AtAConf ?aq) 70 | (AtGConf ?gq) 71 | (AtRelPose ?o1 ?rp ?o2) 72 | (AtWorldPose ?o ?wp) 73 | (AtGrasp ?o ?g) 74 | (AtAngle ?j ?q) 75 | 76 | (CanMoveBase) 77 | (CanMoveArm) 78 | (CanMoveGripper) 79 | (Cooked ?o) 80 | (Calibrated) 81 | (Localized ?o) 82 | 83 | (OpenGripper) 84 | (OpenGConf ?gq) 85 | 86 | (Bowl ?bowl) 87 | (Pourable ?cup) 88 | (Pour ?bowl ?wp ?cup ?g ?bq ?aq ?at) 89 | (Liquid ?liquid) 90 | (HasLiquid ?cup ?liquid) 91 | 92 | (Status ?s) 93 | (DoorStatus ?j ?s) 94 | (AngleWithin ?j ?a ?s) 95 | (SurfaceJoint ?s ?j) 96 | 97 | (On ?o1 ?o2) 98 | (Holding ?o) 99 | (Accessible ?o) 100 | (Unsafe) 101 | (UnsafeRelPose ?o ?rp ?s) 102 | (UnsafeApproach ?o ?wp ?g) 103 | (UnsafeAngleTraj ?j ?a1 ?a2) 104 | (UnsafeATraj ?at) 105 | 106 | (UnsafePull ?j) 107 | (Occluded ?j) 108 | (OccludedRay ?r) 109 | (CloseTo ?q1 ?q2) 110 | 111 | (RelPose ?o1 ?rp ?o2) 112 | (WorldPose ?o ?wp) 113 | (PoseKin ?o1 ?wp1 ?rp ?o2 ?wp2) 114 | (Connected ?o ?j) 115 | (AngleKin ?o ?wp ?j ?a) 116 | 117 | (AdmitsGrasp ?o1 ?g ?o2) 118 | (ValidGraspType ?o ?gty) 119 | (IsGraspType ?o ?g ?gty) 120 | (AdmitsGraspType ?o2 ?gty) 121 | (identical ?bq1 ?bq2) 122 | (Stationary) 123 | ) 124 | (:functions 125 | (Distance ?bq1 ?bq2) 126 | (MoveBaseCost) 127 | (MoveArmCost) 128 | (MoveGripperCost) 129 | (PickCost) 130 | (PlaceCost) 131 | (PullCost) 132 | (PressCost) 133 | (DetectCost ?o ?rp1 ?obs ?rp2) 134 | ) 135 | ; Enforce that it chooses new for all but initial 136 | 137 | ; TODO: prevent the robot from moving to the same spot? 138 | ; TODO: force the search to select new base poses after one manipulation is performed 139 | ; TODO: robot still needs to recalibrate after no base movement... 140 | (:action move_base 141 | ;:parameters (?bq1 ?bq2 ?aq ?bt) 142 | ;:precondition (and (BaseMotion ?bq1 ?bq2 ?aq ?bt) 143 | ; (AtBConf ?bq1) (AtAConf ?aq) 144 | :parameters (?bq1 ?bq2 ?bt) 145 | :precondition (and (BaseMotion ?bq1 ?bq2 @rest_aq ?bt) 146 | (or (Stationary) (not (= ?bq1 @bq0))) 147 | (not (identical ?bq1 ?bq2)) 148 | ; (not (= ?bq1 ?bq2)) ; Causes shared optimistic failure when pick/place two objects 149 | ; TODO: could a (!= ?o1 ?o2) predicate 150 | (AtBConf ?bq1) (AtAConf @rest_aq) 151 | (Calibrated) (CanMoveBase) 152 | (not (Unsafe))) 153 | :effect (and (AtBConf ?bq2) 154 | (not (Stationary)) 155 | (CanMoveArm) 156 | (not (AtBConf ?bq1)) 157 | (not (CanMoveBase)) 158 | ;(not (Calibrated)) 159 | ;(when (NoisyBase) (not (Calibrated))) 160 | ;(increase (total-cost) (Distance ?bq1 ?bq2))) 161 | (increase (total-cost) (MoveBaseCost)))) 162 | (:action move_arm 163 | :parameters (?bq ?aq1 ?aq2 ?at) 164 | :precondition (and (ArmMotion ?bq ?aq1 ?aq2 ?at) (not (identical ?aq1 ?aq2)) 165 | ; (not (= ?aq1 ?aq2)) ; Causes shared optimistic failure 166 | (AtBConf ?bq) (AtAConf ?aq1) 167 | (CanMoveArm) 168 | (Calibrated) ; TODO: require calibration? 169 | (not (Unsafe))) 170 | :effect (and (AtAConf ?aq2) 171 | (not (AtAConf ?aq1)) (not (CanMoveArm)) 172 | (increase (total-cost) (MoveArmCost)))) 173 | (:action move_gripper 174 | :parameters (?gq1 ?gq2 ?gt) 175 | :precondition (and (GripperMotion ?gq1 ?gq2 ?gt) ; (not (= ?gq1 ?gq2)) 176 | (AtGConf ?gq1) (HandEmpty) 177 | ;(CanMoveGripper) 178 | (not (Unsafe))) 179 | :effect (and (AtGConf ?gq2) 180 | (not (AtGConf ?gq1)) ; (not (CanMoveGripper)) 181 | (increase (total-cost) (MoveGripperCost)))) 182 | ;(:action replan 183 | ; :parameters () 184 | ; :precondition (and) 185 | ; :effect (and ) 186 | ;) 187 | ;(:action calibrate 188 | ; ;:parameters (?bq ?aq ?at) 189 | ; ;:precondition (and (CalibrateMotion ?bq ?aq ?at) 190 | ; :parameters () 191 | ; :precondition (and ; (AConf ?bq @calibrate_aq) 192 | ; ; (AtBConf ?bq) (AtAConf @calibrate_aq) 193 | ; ; (not (Calibrated)) 194 | ; ; TODO: visibility constraints 195 | ; ) 196 | ; :effect (and (Calibrated) (CanMoveArm) 197 | ; ; (not (AtBConf ?bq)) ; Could make this be a new pose ?bq2 198 | ; (increase (total-cost) (CalibrateCost))) 199 | ;) 200 | (:action pick 201 | :parameters (?o1 ?wp1 ?g ?rp ?o2 ?wp2 ?bq ?aq ?gq ?at) 202 | :precondition (and (Pick ?o1 ?wp1 ?g ?bq ?aq ?at) (PoseKin ?o1 ?wp1 ?rp ?o2 ?wp2) 203 | (GConf ?gq) (= ?gq @open_gq) ; (OpenGConf ?gq)) 204 | (AtRelPose ?o1 ?rp ?o2) (AtWorldPose ?o1 ?wp1) (HandEmpty) 205 | (AtBConf ?bq) (AtAConf ?aq) (AtGConf ?gq) 206 | (Calibrated) (Localized ?o1) ; TODO: detect precondition? 207 | (AdmitsGrasp ?o1 ?g ?o2) (Accessible ?o2) 208 | (not (UnsafeApproach ?o1 ?wp1 ?g)) 209 | (not (UnsafeATraj ?at)) 210 | (not (Unsafe))) 211 | :effect (and (AtGrasp ?o1 ?g) 212 | (CanMoveBase) (CanMoveArm) 213 | ; (AtGConf @closed_gq) ; No need to save closed state 214 | (not (AtGConf ?gq)) 215 | (Holding ?o1) (not (On ?o1 ?o2)) 216 | (not (AtRelPose ?o1 ?rp ?o2)) (not (AtWorldPose ?o1 ?wp1)) 217 | (not (HandEmpty)) 218 | (increase (total-cost) (PickCost)))) 219 | 220 | (:action place 221 | :parameters (?o1 ?wp1 ?g ?rp ?o2 ?wp2 ?bq ?aq ?at) ; ?gq 222 | :precondition (and (Pick ?o1 ?wp1 ?g ?bq ?aq ?at) (PoseKin ?o1 ?wp1 ?rp ?o2 ?wp2) (Value ?wp1) ; (GConf ?gq) 223 | (AtGrasp ?o1 ?g) (AtWorldPose ?o2 ?wp2) 224 | (AtBConf ?bq) (AtAConf ?aq) ; (AtGConf ?gq) 225 | (Calibrated) (AdmitsGrasp ?o1 ?g ?o2) (Accessible ?o2) 226 | (not (UnsafeRelPose ?o1 ?rp ?o2)) 227 | (not (UnsafeApproach ?o1 ?wp1 ?g)) 228 | (not (UnsafeATraj ?at)) 229 | (not (Unsafe))) 230 | :effect (and (AtRelPose ?o1 ?rp ?o2) (AtWorldPose ?o1 ?wp1) (HandEmpty) 231 | (On ?o1 ?o2) (not (Holding ?o1)) 232 | (AtGConf @open_gq) ; (not (AtGConf ?gq)) 233 | (CanMoveBase) (CanMoveArm) 234 | (not (AtGrasp ?o1 ?g)) 235 | ; (not (Localized ?o1)) ; Forces an observation action 236 | (increase (total-cost) (PlaceCost)))) 237 | 238 | (:action pull 239 | :parameters (?j ?a1 ?a2 ?o ?wp1 ?wp2 ?bq ?aq1 ?aq2 ?gq ?at) 240 | :precondition (and (Pull ?j ?a1 ?a2 ?bq ?aq1 ?aq2 ?at) (not (identical ?a1 ?a2)) 241 | (GConf ?gq) (= ?gq @open_gq) ; (OpenGConf ?gq) ; (OpenGripper) 242 | (AngleKin ?o ?wp1 ?j ?a1) (AngleKin ?o ?wp2 ?j ?a2) 243 | (AtAngle ?j ?a1) (AtWorldPose ?o ?wp1) 244 | (AtBConf ?bq) (AtAConf ?aq1) (AtGConf ?gq) 245 | (HandEmpty) (Calibrated) 246 | (not (UnsafePull ?j)) 247 | ; (not (UnsafeAngle ?j ?a1)) (not (UnsafeAngle ?j ?a2)) 248 | (not (UnsafeAngleTraj ?j ?a1 ?a2)) 249 | (not (UnsafeATraj ?at)) ; TODO: approach pull trajectories 250 | (not (Unsafe)) 251 | ) 252 | ; TODO: use the full pull trajectory 253 | :effect (and (AtAngle ?j ?a2) (AtWorldPose ?o ?wp2) (AtAConf ?aq2) 254 | ; (AtGConf @open_gq) (not (AtGConf ?gq)) 255 | (CanMoveBase) (CanMoveArm) 256 | ; TODO: could treat both objects and drawers are fixed to the joint 257 | (not (AtAngle ?j ?a1)) (not (AtWorldPose ?o ?wp1)) (not (AtAConf ?aq1)) 258 | (forall (?o3 ?wp3 ?rp3) (when (and (PoseKin ?o3 ?wp3 ?rp3 ?o ?wp1) (AtRelPose ?o3 ?rp3 ?o)) 259 | (not (AtWorldPose ?o3 ?wp3)))) 260 | (forall (?o4 ?wp4 ?rp4) (when (and (PoseKin ?o4 ?wp4 ?rp4 ?o ?wp2) (AtRelPose ?o4 ?rp4 ?o)) 261 | (AtWorldPose ?o4 ?wp4))) 262 | (increase (total-cost) (PullCost)))) 263 | 264 | (:action detect ; TODO: or in carry conf 265 | :parameters (?o1 ?wp1 ?rp1 ?obs ?wp2 ?rp2 ?o0 ?wp0 ?r) 266 | :precondition (and (PoseKin ?o1 ?wp1 ?rp1 ?o0 ?wp0) (PoseKin ?o1 ?wp2 ?rp2 ?o0 ?wp0) 267 | (Detect ?o1 ?wp2 ?r) (BeliefUpdate ?o1 ?rp1 ?obs ?rp2) 268 | (AtWorldPose ?o1 ?wp1) ; (AtRelPose ?o1 ?rp1 ?o0) (AtWorldPose ?o0 ?wp0) 269 | (Accessible ?o0) (AtAConf @rest_aq) ; (HandEmpty) 270 | ; (not (UnsafeRelPose ?o1 ?rp2 ?o0)) 271 | (not (OccludedRay ?r)) 272 | (not (Unsafe)) 273 | ) 274 | :effect (and (Localized ?o1) (On ?o1 ?o0) 275 | (AtRelPose ?o1 ?rp2 ?o0) (AtWorldPose ?o1 ?wp2) 276 | (CanMoveArm) 277 | ;(not (AtRelPose ?o1 ?rp1 ?o0)) (not (AtWorldPose ?o1 ?wp1)) 278 | (forall (?rp3) (when (and (RelPose ?o1 ?rp3 ?o0) (AtRelPose ?o1 ?rp3 ?o0)) 279 | (not (AtRelPose ?o3 ?rp3 ?o0)))) 280 | (forall (?wp3) (when (and (WorldPose ?o1 ?wp3) (AtWorldPose ?o1 ?wp3)) 281 | (not (AtWorldPose ?o1 ?wp3)))) 282 | (increase (total-cost) (DetectCost ?o1 ?rp1 ?obs ?rp2)))) 283 | 284 | (:action press-on 285 | :parameters (?s ?k ?o ?bq ?aq ?gq ?at) 286 | :precondition (and (Press ?k ?bq ?aq ?at) (StoveKnob ?s ?k) (Cookable ?o) 287 | (GConf ?gq) (= ?gq @closed_gq) ; (OpenGConf ?gq) 288 | (AtBConf ?bq) (AtAConf ?aq) (AtGConf ?gq) 289 | (HandEmpty) (Calibrated) (On ?o ?s) (not (Pressed ?k)) 290 | (not (UnsafeATraj ?at)) 291 | (not (Unsafe))) 292 | :effect (and (CanMoveBase) (CanMoveArm) 293 | (Pressed ?k) 294 | ;(when (Cold ?s) (and (Hot ?s) (not (Cold ?s)))) 295 | ;(when (Hot ?s) (and (Cold ?s) (not (Hot ?s)))) 296 | (Cooked ?o) 297 | ;(forall (?o) (when (and (Cookable ?o) (On ?o ?s)) 298 | ; (Cooked ?o))) 299 | (forall (?l) (when (and (Liquid ?l) (HasLiquid ?o ?l)) 300 | (Cooked ?l))) 301 | (increase (total-cost) (PressCost)))) 302 | 303 | (:action press-off 304 | :parameters (?s ?k ?o ?bq ?aq ?gq ?at) 305 | :precondition (and (Press ?k ?bq ?aq ?at) (StoveKnob ?s ?k) (Cookable ?o) 306 | (GConf ?gq) (= ?gq @closed_gq) ; (OpenGConf ?gq) 307 | (AtBConf ?bq) (AtAConf ?aq) (AtGConf ?gq) 308 | (HandEmpty) (Calibrated) (On ?o ?s) (Pressed ?k) 309 | (not (UnsafeATraj ?at)) 310 | (not (Unsafe))) 311 | :effect (and (CanMoveBase) (CanMoveArm) 312 | (not (Pressed ?k)) 313 | (increase (total-cost) (PressCost)))) 314 | 315 | (:action pour 316 | :parameters (?bowl ?wp ?cup ?g ?liquid ?bq ?aq ?at) 317 | :precondition (and (Pour ?bowl ?wp ?cup ?g ?bq ?aq ?at) (Liquid ?liquid) (IsGraspType ?cup ?g @side) 318 | (HasLiquid ?cup ?liquid) 319 | (AtWorldPose ?bowl ?wp) (AtGrasp ?cup ?g) 320 | (AtBConf ?bq) (AtAConf ?aq) 321 | (not (= ?bowl ?cup)) (not (UnsafeATraj ?at))) 322 | :effect (and (CanMoveBase) (CanMoveArm) (HasLiquid ?bowl ?liquid) ; TODO: forall 323 | (not (HasLiquid ?cup ?liquid))) 324 | ) 325 | 326 | ;(:derived (OpenGripper) 327 | ; (AtGConf @open_gq) 328 | ; ;(exists (?gq) (and (OpenGConf ?gq) 329 | ; ; (AtGConf ?gq))) 330 | ;) 331 | 332 | (:derived (Accessible ?s) (or (Counter ?s) 333 | (exists (?j ?a) (and (SurfaceJoint ?s ?j) (Angle ?j ?a) (AngleWithin ?j ?a @open) 334 | (not (Occluded ?j)) (AtAngle ?j ?a)))) 335 | ) 336 | (:derived (Occluded ?j1) (and (Drawer ?j1) ; Both drawers can't be open at the same time anyways 337 | (exists (?j2 ?a2) (and (Angle ?j2 ?a2) (Above ?j2 ?j1) 338 | ; (AngleWithin ?j2 ?a2 @open) 339 | (not (AngleWithin ?j2 ?a2 @closed)) 340 | (AtAngle ?j2 ?a2))))) 341 | (:derived (UnsafePull ?j1) (and (Drawer ?j1) 342 | (exists (?j2 ?a2) (and (Angle ?j2 ?a2) (Adjacent ?j1 ?j2) 343 | ; (AngleWithin ?j2 ?a2 @open) 344 | (not (AngleWithin ?j2 ?a2 @closed)) 345 | (AtAngle ?j2 ?a2))))) 346 | 347 | 348 | ; https://github.mit.edu/mtoussai/KOMO-stream/blob/master/03-Caelans-pddlstreamExample/retired/domain.pddl 349 | ;(:derived (AtWorldPose ?o1 ?wp1) (or 350 | ; (and (RelPose ?o1 ?wp1 @world) 351 | ; (AtRelPose ?o1 ?wp1 @world)) 352 | ; (exists (?rp ?o2 ?wp2) (and (PoseKin ?o1 ?wp1 ?rp ?o2 ?wp2) 353 | ; (AtWorldPose ?o2 ?wp2) (AtRelPose ?o1 ?rp ?o2))) 354 | ; (exists (?j ?a) (and (AngleKin ?o1 ?wp1 ?j ?a) 355 | ; (AtAngle ?j ?a))) ; TODO: could compose arbitrary chains 356 | ;)) 357 | 358 | ; TODO: general debug condition that disables these 359 | (:derived (Unsafe) (or 360 | (exists (?o1 ?wp1) (and (WorldPose ?o1 ?wp1) (Entity ?o1) 361 | (not (CFreeWorldPose ?o1 ?wp1)) 362 | (AtWorldPose ?o1 ?wp1))) 363 | (exists (?o1 ?wp1 ?o2 ?wp2) (and (WorldPose ?o1 ?wp1) (WorldPose ?o2 ?wp2) 364 | (Entity ?o1) (Drawer ?o2) 365 | (not (CFreeWorldPoseWorldPose ?o1 ?wp1 ?o2 ?wp2)) 366 | (AtWorldPose ?o1 ?wp1) (AtWorldPose ?o2 ?wp2))) 367 | (exists (?bq ?o2 ?wp2) (and (BConf ?bq) (WorldPose ?o2 ?wp2) (Drawer ?o2) 368 | (not (CFreeBConfPose ?bq ?o2 ?wp2)) 369 | (AtBConf ?bq) (AtWorldPose ?o2 ?wp2))))) 370 | (:derived (UnsafeRelPose ?o1 ?rp1 ?s) (and (RelPose ?o1 ?rp1 ?s) (Entity ?o1) 371 | (exists (?o2 ?rp2) (and (RelPose ?o2 ?rp2 ?s) (Entity ?o2) (not (= ?o1 ?o2)) ; (Sample ?rp2) 372 | (not (CFreeRelPoseRelPose ?o1 ?rp1 ?o2 ?rp2 ?s)) 373 | (AtRelPose ?o2 ?rp2 ?s))) 374 | )) 375 | (:derived (UnsafeApproach ?o1 ?wp1 ?g) (and (WorldPose ?o1 ?wp1) (Grasp ?o1 ?g) 376 | (exists (?o2 ?wp2) (and (WorldPose ?o2 ?wp2) (Obstacle ?o2) (not (= ?o1 ?o2)) ; (Sample ?wp2) 377 | (not (CFreeApproachPose ?o1 ?wp1 ?g ?o2 ?wp2)) 378 | (AtWorldPose ?o2 ?wp2))) 379 | )) 380 | (:derived (UnsafeAngleTraj ?j1 ?a1 ?a2) (and (Angle ?j1 ?a1) (Angle ?j1 ?a2) (Drawer ?j1) 381 | (exists (?o2 ?wp2) (and (WorldPose ?o2 ?wp2) (Drawer ?o2) ; analog of UnsafeApproach 382 | (not (CFreeAngleTraj ?j1 ?a1 ?a2 ?o2 ?wp2)) 383 | (AtWorldPose ?o2 ?wp2))) 384 | )) 385 | (:derived (UnsafeATraj ?at) (and (ATraj ?at) 386 | (exists (?o2 ?wp2) (and (WorldPose ?o2 ?wp2) (Obstacle ?o2) ; (Sample ?wp2) 387 | (not (CFreeTrajPose ?at ?o2 ?wp2)) 388 | (AtWorldPose ?o2 ?wp2))) 389 | )) 390 | (:derived (OccludedRay ?r) (and (Ray ?r) (or 391 | (exists (?o ?wp) (and (WorldPose ?o ?wp) (Obstacle ?o) ; (Sample ?wp) 392 | (not (OFreeRayPose ?r ?o ?wp)) 393 | (AtWorldPose ?o ?wp))) 394 | (exists (?bq ?aq ?o ?g) (and (AConf ?bq ?aq) (Grasp ?o ?g) 395 | (not (OFreeRayGrasp ?r ?bq ?aq ?o ?g)) 396 | (AtBConf ?bq) (AtAConf ?aq) (AtGrasp ?o ?g))) 397 | ))) 398 | ) -------------------------------------------------------------------------------- /pddl/stream.pddl: -------------------------------------------------------------------------------- 1 | (define (stream nvidia-tamp) 2 | (:rule 3 | :inputs (?o ?g ?gty ?o2) 4 | :domain (and (IsGraspType ?o ?g ?gty) (AdmitsGraspType ?o2 ?gty)) 5 | :certified (AdmitsGrasp ?o ?g ?o2)) 6 | (:rule 7 | :inputs (?o1 ?wp1 ?rp ?o2 ?wp2) 8 | :domain (and (PoseKin ?o1 ?wp1 ?rp ?o2 ?wp2) (Value ?rp)) 9 | :certified (Value ?wp1)) ; Selected by planner 10 | (:rule 11 | :inputs (?o1 ?wp1 ?rp ?o2 ?wp2) 12 | :domain (and (PoseKin ?o1 ?wp1 ?rp ?o2 ?wp2) (Sample ?rp)) 13 | :certified (Sample ?wp1)) ; High confidence belief 14 | (:rule 15 | :inputs (?o1 ?wp1 ?rp ?o2 ?wp2) 16 | :domain (and (PoseKin ?o1 ?wp1 ?rp ?o2 ?wp2) (Dist ?rp)) 17 | :certified (Dist ?wp1)) ; Low confidence belief 18 | (:rule 19 | :inputs (?o1 ?wp1 ?rp ?o2 ?wp2 ?bq) 20 | :domain (and (PoseKin ?o1 ?wp1 ?rp ?o2 ?wp2) (NearPose ?o2 ?wp2 ?bq) (Posterior ?rp)) 21 | :certified (NearPose ?o1 ?wp1 ?bq)) 22 | 23 | ;(:stream test-not-equal 24 | ; :inputs (?o1 ?2) 25 | ; :domain (and (Object ?o1) (Object ?o2)) 26 | ; :certified (NEq ?o1 ?o2)) 27 | 28 | (:stream sample-grasp 29 | :inputs (?o ?gty) 30 | :domain (ValidGraspType ?o ?gty) 31 | :outputs (?g) 32 | :certified (and (Grasp ?o ?g) (IsGraspType ?o ?g ?gty))) ; TODO: produce carry conf for ?o 33 | 34 | 35 | (:stream sample-pose 36 | :inputs (?o ?r) 37 | :domain (and (MovableBase) (Stackable ?o ?r)) 38 | :outputs (?rp) 39 | :certified (and (RelPose ?o ?rp ?r) 40 | (Value ?rp) (Sample ?rp) (BeliefUpdate ?o ?rp @none ?rp) 41 | )) 42 | (:stream sample-nearby-pose 43 | :inputs (?o1 ?o2 ?wp2 ?bq) 44 | :domain (and (NearPose ?o2 ?wp2 ?bq) (Stackable ?o1 ?o2)) ; TODO: ensure door is open? 45 | :outputs (?wp1 ?rp) 46 | :certified (and (RelPose ?o1 ?rp ?o2) (NearPose ?o1 ?wp1 ?bq) 47 | (Value ?wp1) (Sample ?wp1) 48 | (Value ?rp) (Sample ?rp) (BeliefUpdate ?o1 ?rp @none ?rp) 49 | (WorldPose ?o1 ?wp1) (PoseKin ?o1 ?wp1 ?rp ?o2 ?wp2))) 50 | 51 | 52 | (:stream test-near-pose 53 | :inputs (?o ?wp ?bq) 54 | :domain (and (WorldPose ?o ?wp) (CheckNearby ?o) (Sample ?wp) (InitBConf ?bq)) 55 | :certified (NearPose ?o ?wp ?bq)) 56 | (:stream fixed-plan-pick 57 | :inputs (?o ?wp ?g ?bq) 58 | :domain (and (NearPose ?o ?wp ?bq) ; (InitBConf ?bq) 59 | (WorldPose ?o ?wp) (Sample ?wp) (Grasp ?o ?g)) 60 | :outputs (?aq ?at) 61 | :certified (and (ATraj ?at) (AConf ?bq ?aq) 62 | (Pick ?o ?wp ?g ?bq ?aq ?at))) 63 | (:stream plan-pick 64 | :inputs (?o ?wp ?g) ; ?aq0) 65 | :domain (and (MovableBase) (WorldPose ?o ?wp) (Sample ?wp) (Grasp ?o ?g)) ; (RestAConf ?aq0)) 66 | :outputs (?bq ?aq ?at) 67 | :certified (and (BConf ?bq) (ATraj ?at) 68 | ; (AConf ?bq ?aq0) 69 | (AConf ?bq @rest_aq) ; (AConf ?bq @calibrate_aq) 70 | (AConf ?bq ?aq) 71 | (Pick ?o ?wp ?g ?bq ?aq ?at))) 72 | 73 | (:stream test-near-joint 74 | :inputs (?j ?bq) 75 | :domain (and (Joint ?j) (InitBConf ?bq)) 76 | :certified (NearJoint ?j ?bq)) 77 | (:stream fixed-plan-pull 78 | :inputs (?j ?a1 ?a2 ?bq) 79 | :domain (and (Angle ?j ?a1) (Angle ?j ?a2) (NearJoint ?j ?bq)) 80 | :outputs (?aq1 ?aq2 ?at) 81 | :certified (and (ATraj ?at) (AConf ?bq ?aq1) (AConf ?bq ?aq2) 82 | (Pull ?j ?a1 ?a2 ?bq ?aq1 ?aq2 ?at))) 83 | (:stream plan-pull 84 | :inputs (?j ?a1 ?a2) ; ?aq0) 85 | :domain (and (MovableBase) (Angle ?j ?a1) (Angle ?j ?a2)) ; (RestAConf ?aq0)) 86 | :outputs (?bq ?aq1 ?aq2 ?at) 87 | :certified (and (BConf ?bq) (ATraj ?at) 88 | ; (AConf ?bq ?aq0) 89 | ; TODO: strange effect with plan constraints 90 | (AConf ?bq @rest_aq) ; (AConf ?bq @calibrate_aq) 91 | (AConf ?bq ?aq1) (AConf ?bq ?aq2) 92 | (Pull ?j ?a1 ?a2 ?bq ?aq1 ?aq2 ?at))) 93 | 94 | 95 | (:stream fixed-plan-press 96 | :inputs (?k ?bq) 97 | :domain (and (NearJoint ?k ?bq) (Knob ?k)) ; TODO: use pose instead? 98 | :outputs (?aq ?at) 99 | :certified (and (ATraj ?at) (AConf ?bq ?aq) 100 | (Press ?k ?bq ?aq ?at))) 101 | (:stream plan-press 102 | :inputs (?k) 103 | :domain (and (MovableBase) (Knob ?k)) ; TODO: (WorldPose ?o ?wp) 104 | :outputs (?bq ?aq ?at) 105 | :certified (and (BConf ?bq) (ATraj ?at) 106 | ; (AConf ?bq ?aq0) 107 | (AConf ?bq @rest_aq) ; (AConf ?bq @calibrate_aq) 108 | (AConf ?bq ?aq) 109 | (Press ?k ?bq ?aq ?at))) 110 | 111 | (:stream fixed-plan-pour 112 | :inputs (?bowl ?wp ?cup ?g ?bq) ; TODO: can pour ?bowl & ?cup 113 | :domain (and (Bowl ?bowl) (WorldPose ?bowl ?wp) (Sample ?wp) (NearPose ?bowl ?wp ?bq) 114 | (Pourable ?cup) (Grasp ?cup ?g)) 115 | :outputs (?aq ?at) 116 | :certified (and (Pour ?bowl ?wp ?cup ?g ?bq ?aq ?at) 117 | (AConf ?bq ?aq) (ATraj ?at))) 118 | (:stream plan-pour 119 | :inputs (?bowl ?wp ?cup ?g) 120 | :domain (and (Bowl ?bowl) (WorldPose ?bowl ?wp) (Sample ?wp) (Pourable ?cup) (Grasp ?cup ?g)) 121 | :outputs (?bq ?aq ?at) 122 | :certified (and (Pour ?bowl ?wp ?cup ?g ?bq ?aq ?at) 123 | (BConf ?bq) (AConf ?bq @rest_aq) (AConf ?bq ?aq) (ATraj ?at))) 124 | 125 | (:stream plan-base-motion 126 | ;:fluents (AtGConf AtWorldPose AtGrasp) ; AtBConf, AtAConf 127 | :fluents (AtGConf AtAngle AtRelPose AtGrasp) ; AtBConf, AtAConf 128 | :inputs (?bq1 ?bq2 ?aq) ; TODO: just rest_aq? 129 | :domain (and (MovableBase) (AConf ?bq1 ?aq) (AConf ?bq2 ?aq)) 130 | :outputs (?bt) 131 | :certified (BaseMotion ?bq1 ?bq2 ?aq ?bt)) 132 | (:stream plan-arm-motion 133 | ;:fluents (AtGConf AtWorldPose AtGrasp) ; AtBConf, AtAConf 134 | :fluents (AtGConf AtAngle AtRelPose AtGrasp) ; AtBConf, AtAConf 135 | ; TODO: disjunctive stream conditions 136 | :inputs (?bq ?aq1 ?aq2) 137 | :domain (and (AConf ?bq ?aq1) (AConf ?bq ?aq2)) 138 | :outputs (?at) 139 | :certified (ArmMotion ?bq ?aq1 ?aq2 ?at)) 140 | (:stream plan-gripper-motion 141 | :inputs (?gq1 ?gq2) 142 | :domain (and (GConf ?gq1) (GConf ?gq2)) 143 | :outputs (?gt) 144 | :certified (GripperMotion ?gq1 ?gq2 ?gt)) 145 | ;(:stream plan-calibrate-motion 146 | ; :inputs (?bq) ; ?aq0) 147 | ; :domain (and (BConf ?bq)) ; (RestAConf ?aq0)) 148 | ; :outputs (?at) ; ?aq 149 | ; :certified (and (ATraj ?at) 150 | ; ;(CalibrateMotion ?bq ?aq0 ?at)) 151 | ; (CalibrateMotion ?bq @rest_aq ?at)) 152 | ;) 153 | 154 | (:stream sample-observation ; TODO: sample-nearby-observation? 155 | :inputs (?o1 ?rp1 ?o2) 156 | :domain (and (RelPose ?o1 ?rp1 ?o2) (Dist ?rp1)) 157 | :outputs (?obs) 158 | :certified (Observation ?o1 ?o2 ?obs)) 159 | (:stream update-belief 160 | :inputs (?o1 ?rp1 ?o2 ?obs) 161 | :domain (and (RelPose ?o1 ?rp1 ?o2) (Dist ?rp1) (Observation ?o1 ?o2 ?obs)) 162 | :outputs (?rp2) ; TODO: could add parameter ?o2 as well 163 | :certified (and (RelPose ?o1 ?rp2 ?o2) (BeliefUpdate ?o1 ?rp1 ?obs ?rp2) 164 | (Posterior ?rp2) (Sample ?rp2))) 165 | (:stream compute-detect 166 | :inputs (?o ?wp) 167 | :domain (and (WorldPose ?o ?wp) (Sample ?wp)) 168 | :outputs (?r) 169 | :certified (and (Ray ?r) (Detect ?o ?wp ?r))) 170 | 171 | (:stream compute-pose-kin 172 | :inputs (?o1 ?rp ?o2 ?wp2) 173 | :domain (and (RelPose ?o1 ?rp ?o2) (WorldPose ?o2 ?wp2)) 174 | :outputs (?wp1) 175 | :certified (and (WorldPose ?o1 ?wp1) (PoseKin ?o1 ?wp1 ?rp ?o2 ?wp2)) 176 | ; (PoseTriplet ?o1 ?wp1 ?rp) ; For instantiation? 177 | ) 178 | ;(:stream compute-angle-kin 179 | ; :inputs (?o ?j ?a) 180 | ; :domain (and (Connected ?o ?j) (Angle ?j ?a)) 181 | ; :outputs (?wp) 182 | ; :certified (and (WorldPose ?o ?wp) (AngleKin ?o ?wp ?j ?a)) 183 | ;) 184 | 185 | (:stream test-cfree-worldpose 186 | :inputs (?o1 ?wp1) 187 | :domain (WorldPose ?o1 ?wp1) 188 | :certified (CFreeWorldPose ?o1 ?wp1)) 189 | (:stream test-cfree-worldpose-worldpose 190 | :inputs (?o1 ?wp1 ?o2 ?wp2) 191 | :domain (and (WorldPose ?o1 ?wp1) (WorldPose ?o2 ?wp2)) 192 | :certified (CFreeWorldPoseWorldPose ?o1 ?wp1 ?o2 ?wp2)) 193 | (:stream test-cfree-pose-pose 194 | :inputs (?o1 ?rp1 ?o2 ?rp2 ?s) 195 | :domain (and (RelPose ?o1 ?rp1 ?s) (RelPose ?o2 ?rp2 ?s)) 196 | :certified (CFreeRelPoseRelPose ?o1 ?rp1 ?o2 ?rp2 ?s)) 197 | (:stream test-cfree-approach-pose 198 | :inputs (?o1 ?wp1 ?g1 ?o2 ?wp2) 199 | :domain (and (WorldPose ?o1 ?wp1) (Grasp ?o1 ?g1) (WorldPose ?o2 ?wp2)) 200 | :certified (CFreeApproachPose ?o1 ?wp1 ?g1 ?o2 ?wp2)) 201 | (:stream test-cfree-angle-angle 202 | :inputs (?j1 ?a1 ?a2 ?o2 ?wp2) 203 | :domain (and (Angle ?j1 ?a1) (Angle ?j1 ?a2) (WorldPose ?o2 ?wp2)) 204 | :certified (CFreeAngleTraj ?j1 ?a1 ?a2 ?o2 ?wp2)) 205 | (:stream test-cfree-bconf-pose 206 | :inputs (?bq ?o2 ?wp2) 207 | :domain (and (BConf ?bq) (WorldPose ?o2 ?wp2)) 208 | :certified (CFreeBConfPose ?bq ?o2 ?wp2)) 209 | (:stream test-cfree-traj-pose 210 | :inputs (?at ?o2 ?wp2) 211 | :domain (and (ATraj ?at) (WorldPose ?o2 ?wp2)) 212 | :certified (CFreeTrajPose ?at ?o2 ?wp2)) 213 | 214 | (:stream test-ofree-ray-pose 215 | :inputs (?r ?o ?wp) 216 | :domain (and (Ray ?r) (WorldPose ?o ?wp)) 217 | :certified (OFreeRayPose ?r ?o ?wp)) 218 | (:stream test-ofree-ray-grasp 219 | :inputs (?r ?bq ?aq ?o ?g) 220 | :domain (and (Ray ?r) (AConf ?bq ?aq) (Grasp ?o ?g)) 221 | :certified (OFreeRayGrasp ?r ?bq ?aq ?o ?g)) 222 | 223 | ; TODO: these could also just be populated in the initial state 224 | ;(:stream test-gripper 225 | ; :inputs (?gq) 226 | ; :domain (GConf ?gq) 227 | ; :certified (OpenGConf ?gq)) 228 | (:stream test-door 229 | :inputs (?j ?a ?s) 230 | :domain (and (Angle ?j ?a) (Status ?s)) 231 | :certified (AngleWithin ?j ?a ?s)) 232 | 233 | (:function (DetectCost ?o ?rp1 ?obs ?rp2) ; TODO: pass in risk level 234 | (BeliefUpdate ?o ?rp1 ?obs ?rp2)) 235 | ;(:function (Distance ?bq1 ?bq2) 236 | ; (and (BConf ?bq1) (BConf ?bq2)) 237 | ;) 238 | ;(:function (MoveCost ?t) 239 | ; (and (BTraj ?t)) 240 | ;) 241 | ) -------------------------------------------------------------------------------- /run_experiment.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | from __future__ import print_function 4 | 5 | # https://github.mit.edu/Learning-and-Intelligent-Systems/ltamp_pr2/blob/d1e6024c5c13df7edeab3a271b745e656a794b02/learn_tools/collect_simulation.py 6 | # https://github.mit.edu/caelan/pddlstream-experiments/blob/master/run_experiment.py 7 | 8 | import argparse 9 | import numpy as np 10 | import time 11 | import datetime 12 | import math 13 | import numpy 14 | import random 15 | import os 16 | import sys 17 | import traceback 18 | import resource 19 | import copy 20 | import psutil 21 | 22 | PACKAGES = ['pddlstream', 'ss-pybullet'] 23 | 24 | def add_packages(packages): 25 | sys.path.extend(os.path.abspath(os.path.join(os.getcwd(), d)) for d in packages) 26 | 27 | add_packages(PACKAGES) 28 | 29 | np.set_printoptions(precision=3, threshold=3, edgeitems=1, suppress=True) #, linewidth=1000) 30 | 31 | import pddlstream.language.statistics 32 | pddlstream.language.statistics.LOAD_STATISTICS = False 33 | pddlstream.language.statistics.SAVE_STATISTICS = False 34 | 35 | from pybullet_tools.utils import has_gui, elapsed_time, user_input, ensure_dir, \ 36 | wrap_numpy_seed, timeout, write_json, SEPARATOR, WorldSaver, \ 37 | get_random_seed, get_numpy_seed, set_random_seed, set_numpy_seed, wait_for_user, get_date, is_darwin, INF 38 | from pddlstream.utils import str_from_object, safe_rm_dir, Verbose, KILOBYTES_PER_GIGABYTE, BYTES_PER_KILOBYTE 39 | from pddlstream.algorithms.algorithm import reset_globals 40 | 41 | from src.command import create_state, iterate_commands 42 | from src.observe import observe_pybullet 43 | from src.world import World 44 | from src.policy import run_policy 45 | from src.task import cook_block, TASKS_FNS 46 | from run_pybullet import create_parser 47 | 48 | from multiprocessing import Pool, TimeoutError, cpu_count 49 | 50 | EXPERIMENTS_DIRECTORY = 'experiments/' 51 | TEMP_DIRECTORY = 'temp_parallel/' 52 | MAX_TIME = 10*60 53 | TIME_BUFFER = 60 54 | SERIAL = is_darwin() 55 | VERBOSE = SERIAL 56 | SERIALIZE_TASK = True 57 | 58 | MEAN_TIME_PER_TRIAL = 300 # trial / sec 59 | HOURS_TO_SECS = 60 * 60 60 | 61 | N_TRIALS = 1 # 1 62 | MAX_MEMORY = 3.5*KILOBYTES_PER_GIGABYTE 63 | SPARE_CORES = 4 64 | 65 | POLICIES = [ 66 | {'constrain': False, 'defer': False}, 67 | {'constrain': True, 'defer': False}, 68 | {'constrain': False, 'defer': True}, # Move actions grow immensely 69 | {'constrain': True, 'defer': True}, 70 | # TODO: serialize 71 | ] 72 | # 8Gb memory limit 73 | # https://ipc2018-classical.bitbucket.io/ 74 | 75 | # Tasks 76 | # 1) Inspect drawers 77 | # 2) Swap drawers (uniform prior) 78 | # 3) Object behind one of two objects 79 | # 4) Cook meal 80 | # 6) Object on drawer that needs to be moved 81 | # 7) Crowded surface 82 | # 8) Scaling to longer tasks (no point if serializing) 83 | # 9) Packing into drawer 84 | # 10) Fixed base manipulation 85 | # 11) Regrasp using the cabinet 86 | # 12) Irrelevant distractors that aren't picked up 87 | 88 | TASK_NAMES = [ 89 | 'inspect_drawer', 90 | 'sugar_drawer', 91 | 'swap_drawers', 92 | 'detect_block', 93 | 94 | #'cook_meal', 95 | #'regrasp_block', 96 | #'hold_block', 97 | #'cook_block', 98 | #'stow_block', 99 | ] 100 | 101 | # TODO: CPU usage at 300% due to TracIK or the visualizer? 102 | # TODO: could check collisions only with real (non-observed) values 103 | 104 | ERROR_OUTCOME = { 105 | 'error': True, 106 | 'achieved_goal': False, 107 | 'total_time': INF, 108 | 'plan_time': INF, 109 | 'num_iterations': 0, 110 | 'num_constrained': 0, 111 | 'num_unconstrained': 0, 112 | 'num_successes': 0, 113 | 'num_actions': INF, 114 | 'num_commands': INF, 115 | 'total_cost': INF, 116 | } 117 | 118 | # TODO: doesn't work on flakey 119 | 120 | ################################################################################ 121 | 122 | def map_parallel(fn, inputs, num_cores=None): #, timeout=None): 123 | # Processes rather than threads (shared memory) 124 | # TODO: with statement on Pool 125 | if SERIAL: 126 | for outputs in map(fn, inputs): 127 | yield outputs 128 | return 129 | pool = Pool(processes=num_cores) #, initializer=mute) 130 | generator = pool.imap_unordered(fn, inputs) #, chunksize=1) 131 | # pool_result = pool.map_async(worker, args) 132 | #return generator 133 | while True: 134 | # TODO: need to actually retrieve the info about which thread failed 135 | try: 136 | yield generator.next() # timeout=timeout) 137 | except StopIteration: 138 | break 139 | #except MemoryError: # as e: 140 | # traceback.print_exc() 141 | # continue 142 | #except TimeoutError: # as e: 143 | # traceback.print_exc() # Kills all jobs 144 | # continue 145 | if pool is not None: 146 | pool.close() 147 | pool.terminate() 148 | pool.join() 149 | #import psutil 150 | #if parallel: 151 | # process = psutil.Process(os.getpid()) 152 | # print(process) 153 | # print(process.get_memory_info()) 154 | 155 | ################################################################################ 156 | 157 | def name_from_policy(policy): 158 | return '_'.join('{}={:d}'.format(key, value) for key, value in sorted(policy.items())) 159 | 160 | def set_memory_limits(): 161 | # ulimit -a 162 | # soft, hard = resource.getrlimit(name) # resource.RLIM_INFINITY 163 | # resource.setrlimit(resource.RLIMIT_AS, (soft, hard)) 164 | process = psutil.Process(os.getpid()) 165 | soft_memory = int(BYTES_PER_KILOBYTE*MAX_MEMORY) 166 | hard_memory = soft_memory 167 | process.rlimit(psutil.RLIMIT_AS, (soft_memory, hard_memory)) 168 | # TODO: AttributeError: 'Process' object has no attribute 'rlimit' 169 | #soft_time = MAX_TIME + 2*60 # I think this kills the wrong things 170 | #hard_time = soft_time 171 | #process.rlimit(psutil.RLIMIT_CPU, (soft_time, hard_time)) 172 | 173 | ################################################################################ 174 | 175 | def run_experiment(experiment): 176 | problem = experiment['problem'] 177 | task_name = problem['task'].name if SERIALIZE_TASK else problem['task'] 178 | trial = problem['trial'] 179 | policy = experiment['policy'] 180 | set_memory_limits() 181 | 182 | if not VERBOSE: 183 | sys.stdout = open(os.devnull, 'w') 184 | stdout = sys.stdout 185 | if not SERIAL: 186 | current_wd = os.getcwd() 187 | # trial_wd = os.path.join(current_wd, TEMP_DIRECTORY, '{}/'.format(os.getpid())) 188 | trial_wd = os.path.join(current_wd, TEMP_DIRECTORY, 't={}_n={}_{}/'.format( 189 | task_name, trial, name_from_policy(policy))) 190 | safe_rm_dir(trial_wd) 191 | ensure_dir(trial_wd) 192 | os.chdir(trial_wd) 193 | 194 | parser = create_parser() 195 | args = parser.parse_args() 196 | 197 | task_fn_from_name = {fn.__name__: fn for fn in TASKS_FNS} 198 | task_fn = task_fn_from_name[task_name] 199 | world = World(use_gui=SERIAL) 200 | if SERIALIZE_TASK: 201 | task_fn(world, fixed=args.fixed) 202 | task = problem['task'] 203 | world.task = task 204 | task.world = world 205 | else: 206 | # TODO: assumes task_fn is deterministic wrt task 207 | task_fn(world, fixed=args.fixed) 208 | problem['saver'].restore() 209 | world._update_initial() 210 | problem['task'] = task_name # for serialization 211 | del problem['saver'] 212 | 213 | random.seed(hash((0, task_name, trial, time.time()))) 214 | numpy.random.seed(hash((1, task_name, trial, time.time())) % (2**32)) 215 | #seed1, seed2 = problem['seeds'] # No point unless you maintain the same random state per generator 216 | #set_random_seed(seed1) 217 | #set_random_seed(seed2) 218 | #random.setstate(state1) 219 | #numpy.random.set_state(state2) 220 | reset_globals() 221 | real_state = create_state(world) 222 | #start_time = time.time() 223 | #if has_gui(): 224 | # wait_for_user() 225 | 226 | observation_fn = lambda belief: observe_pybullet(world) 227 | transition_fn = lambda belief, commands: iterate_commands(real_state, commands, time_step=0) 228 | outcome = dict(ERROR_OUTCOME) 229 | try: 230 | with timeout(MAX_TIME + TIME_BUFFER): 231 | outcome = run_policy(task, args, observation_fn, transition_fn, max_time=MAX_TIME, **policy) 232 | outcome['error'] = False 233 | except KeyboardInterrupt: 234 | raise KeyboardInterrupt() 235 | except: 236 | traceback.print_exc() 237 | #outcome = {'error': True} 238 | 239 | world.destroy() 240 | if not SERIAL: 241 | os.chdir(current_wd) 242 | safe_rm_dir(trial_wd) 243 | if not VERBOSE: 244 | sys.stdout.close() 245 | sys.stdout = stdout 246 | 247 | result = { 248 | 'experiment': experiment, 249 | 'outcome': outcome, 250 | } 251 | return result 252 | 253 | ################################################################################ 254 | 255 | def create_problems(args): 256 | task_fn_from_name = {fn.__name__: fn for fn in TASKS_FNS} 257 | problems = [] 258 | for num in range(N_TRIALS): 259 | for task_name in TASK_NAMES: 260 | print('Trial: {} / {} | Task: {}'.format(num, N_TRIALS, task_name)) 261 | random.seed(hash((0, task_name, num, time.time()))) 262 | numpy.random.seed(wrap_numpy_seed(hash((1, task_name, num, time.time())))) 263 | world = World(use_gui=False) # SERIAL 264 | task_fn = task_fn_from_name[task_name] 265 | task = task_fn(world, fixed=args.fixed) 266 | task.world = None 267 | if not SERIALIZE_TASK: 268 | task = task_name 269 | saver = WorldSaver() 270 | problems.append({ 271 | 'task': task, 272 | 'trial': num, 273 | 'saver': saver, 274 | #'seeds': [get_random_seed(), get_numpy_seed()], 275 | #'seeds': [random.getstate(), numpy.random.get_state()], 276 | }) 277 | #print(world.body_from_name) # TODO: does not remain the same 278 | #wait_for_user() 279 | #world.reset() 280 | #if has_gui(): 281 | # wait_for_user() 282 | world.destroy() 283 | return problems 284 | 285 | ################################################################################ 286 | 287 | def main(): 288 | parser = create_parser() 289 | args = parser.parse_args() 290 | print(args) 291 | 292 | # https://stackoverflow.com/questions/15314189/python-multiprocessing-pool-hangs-at-join 293 | # https://stackoverflow.com/questions/39884898/large-amount-of-multiprocessing-process-causing-deadlock 294 | # TODO: alternatively don't destroy the world 295 | num_cores = max(1, cpu_count() - SPARE_CORES) 296 | json_path = os.path.abspath(os.path.join(EXPERIMENTS_DIRECTORY, '{}.json'.format(get_date()))) 297 | 298 | #memory_per_core = float(MAX_RAM) / num_cores # gigabytes 299 | #set_soft_limit(resource.RLIMIT_AS, int(BYTES_PER_GIGABYTE * memory_per_core)) # bytes 300 | #set_soft_limit(resource.RLIMIT_CPU, 2*MAX_TIME) # seconds 301 | # RLIMIT_MEMLOCK, RLIMIT_STACK, RLIMIT_DATA 302 | 303 | print('Results:', json_path) 304 | print('Num Cores:', num_cores) 305 | #print('Memory per Core: {:.2f}'.format(memory_per_core)) 306 | print('Tasks: {} | {}'.format(len(TASK_NAMES), TASK_NAMES)) 307 | print('Policies: {} | {}'.format(len(POLICIES), POLICIES)) 308 | print('Num Trials:', N_TRIALS) 309 | num_experiments = len(TASK_NAMES) * len(POLICIES) * N_TRIALS 310 | print('Num Experiments:', num_experiments) 311 | max_parallel = math.ceil(float(num_experiments) / num_cores) 312 | print('Estimated duration: {:.2f} hours'.format(MEAN_TIME_PER_TRIAL * max_parallel / HOURS_TO_SECS)) 313 | user_input('Begin?') 314 | print(SEPARATOR) 315 | 316 | print('Creating problems') 317 | start_time = time.time() 318 | problems = create_problems(args) 319 | experiments = [{'problem': copy.deepcopy(problem), 'policy': policy} #, 'args': args} 320 | for problem in problems for policy in POLICIES] 321 | print('Created {} problems and {} experiments in {:.3f} seconds'.format( 322 | len(problems), len(experiments), elapsed_time(start_time))) 323 | print(SEPARATOR) 324 | 325 | ensure_dir(EXPERIMENTS_DIRECTORY) 326 | safe_rm_dir(TEMP_DIRECTORY) 327 | ensure_dir(TEMP_DIRECTORY) 328 | start_time = time.time() 329 | results = [] 330 | try: 331 | for result in map_parallel(run_experiment, experiments, num_cores=num_cores): 332 | results.append(result) 333 | print('{}\nExperiments: {} / {} | Time: {:.3f}'.format( 334 | SEPARATOR, len(results), len(experiments), elapsed_time(start_time))) 335 | print('Experiment:', str_from_object(result['experiment'])) 336 | print('Outcome:', str_from_object(result['outcome'])) 337 | write_json(json_path, results) 338 | #except BaseException as e: 339 | # traceback.print_exc() # e 340 | finally: 341 | if results: 342 | write_json(json_path, results) 343 | print(SEPARATOR) 344 | print('Saved:', json_path) 345 | print('Results:', len(results)) 346 | print('Duration / experiment: {:.3f}'.format(num_cores*elapsed_time(start_time) / len(experiments))) 347 | print('Duration: {:.2f} hours'.format(elapsed_time(start_time) / HOURS_TO_SECS)) 348 | safe_rm_dir(TEMP_DIRECTORY) 349 | # TODO: dump results automatically? 350 | return results 351 | 352 | # ./run_experiment.py 2>&1 | tee log.txt 353 | 354 | if __name__ == '__main__': 355 | main() 356 | 357 | """ 358 | _memory: 1571984.0, plan_time: 210.628690958, total_cost: 2075, total_time: 211.089640856} 359 | WARNING: overflow on h^add! Costs clamped to 100000000 360 | Traceback (most recent call last): 361 | File "./run_experiment.py", line 202, in run_experiment 362 | max_time=MAX_TIME, **policy) 363 | File "/home/caelan/Programs/srlstream/src/policy.py", line 113, in run_policy 364 | max_cost=plan_cost, replan_actions=defer_actions) 365 | File "/home/caelan/Programs/srlstream/src/policy.py", line 34, in random_restart 366 | plan, plan_cost, certificate = solve_pddlstream(belief, problem, args, max_time=remaining_time, **kwargs) 367 | File "/home/caelan/Programs/srlstream/src/planner.py", line 167, in solve_pddlstream 368 | search_sample_ratio=search_sample_ratio) 369 | File "/home/caelan/Programs/srlstream/pddlstream/pddlstream/algorithms/focused.py", line 134, in solve_focused 370 | File "/home/caelan/Programs/srlstream/pddlstream/pddlstream/algorithms/reorder.py", line 167, in reorder_stream_plan 371 | ordering = dynamic_programming(nodes, valid_combine, stats_fn, **kwargs) 372 | File "/home/caelan/Programs/srlstream/pddlstream/pddlstream/algorithms/reorder.py", line 127, in dynamic_programming 373 | new_subset = frozenset([v]) | subset 374 | MemoryError 375 | """ 376 | -------------------------------------------------------------------------------- /run_pybullet.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | from __future__ import print_function 4 | 5 | import sys 6 | import argparse 7 | import os 8 | import numpy as np 9 | 10 | sys.path.extend(os.path.abspath(os.path.join(os.getcwd(), d)) 11 | for d in ['pddlstream', 'ss-pybullet']) 12 | 13 | from pybullet_tools.utils import wait_for_user, set_random_seed, set_numpy_seed, LockRenderer, \ 14 | get_random_seed, get_numpy_seed, VideoSaver, set_camera, set_camera_pose, get_point, wait_for_duration 15 | from src.command import create_state, iterate_commands, simulate_commands, DEFAULT_TIME_STEP 16 | from src.visualization import add_markers 17 | from src.observe import observe_pybullet 18 | #from src.debug import test_observation 19 | from src.planner import VIDEO_TEMPLATE 20 | from src.world import World 21 | from src.task import TASKS_FNS 22 | from src.policy import run_policy 23 | #from src.debug import dump_link_cross_sections, test_rays 24 | 25 | def create_parser(): 26 | parser = argparse.ArgumentParser() 27 | parser.add_argument('-anytime', action='store_true', 28 | help='Runs the planner in an anytime mode.') 29 | parser.add_argument('-cfree', action='store_true', 30 | help='When enabled, disables collision checking (for debugging).') 31 | #parser.add_argument('-defer', action='store_true', 32 | # help='When enabled, defers evaluation of motion planning streams.') 33 | parser.add_argument('-deterministic', action='store_true', 34 | help='Treats actions as having deterministic effects.') 35 | parser.add_argument('-fixed', action='store_true', 36 | help="When enabled, fixes the robot's base.") 37 | parser.add_argument('-max_time', default=5*60, type=int, 38 | help='The max computation time across execution.') 39 | parser.add_argument('-num', default=1, type=int, 40 | help='The number of objects (when applicable).') 41 | parser.add_argument('-observable', action='store_true', 42 | help='Treats the state as being fully observable.') 43 | #parser.add_argument('-seed', default=None, 44 | # help='The random seed to use.') 45 | parser.add_argument('-simulate', action='store_true', 46 | help='When enabled, trajectories are simulated') 47 | parser.add_argument('-teleport', action='store_true', 48 | help='When enabled, transit motion planning is skipped (for debugging).') 49 | parser.add_argument('-unit', action='store_true', 50 | help='When enabled, uses unit action costs.') 51 | parser.add_argument('-visualize', action='store_true', 52 | help='When enabled, visualizes the planning world rather than the simulated world (for debugging).') 53 | return parser 54 | # TODO: get rid of funky orientations by dropping them from some height 55 | 56 | ################################################################################ 57 | 58 | def main(): 59 | task_names = [fn.__name__ for fn in TASKS_FNS] 60 | print('Tasks:', task_names) 61 | parser = create_parser() 62 | parser.add_argument('-problem', default=task_names[-1], choices=task_names, 63 | help='The name of the problem to solve.') 64 | parser.add_argument('-record', action='store_true', 65 | help='When enabled, records and saves a video at {}'.format( 66 | VIDEO_TEMPLATE.format(''))) 67 | args = parser.parse_args() 68 | #if args.seed is not None: 69 | # set_seed(args.seed) 70 | #set_random_seed(0) # Doesn't ensure deterministic 71 | #set_numpy_seed(1) 72 | print('Random seed:', get_random_seed()) 73 | print('Numpy seed:', get_numpy_seed()) 74 | 75 | np.set_printoptions(precision=3, suppress=True) 76 | world = World(use_gui=True) 77 | task_fn_from_name = {fn.__name__: fn for fn in TASKS_FNS} 78 | task_fn = task_fn_from_name[args.problem] 79 | 80 | task = task_fn(world, num=args.num, fixed=args.fixed) 81 | wait_for_duration(0.1) 82 | world._update_initial() 83 | print('Objects:', task.objects) 84 | #target_point = get_point(world.get_body(task.objects[0])) 85 | #set_camera_pose(camera_point=target_point+np.array([-1, 0, 1]), target_point=target_point) 86 | 87 | #if not args.record: 88 | # with LockRenderer(): 89 | # add_markers(task, inverse_place=False) 90 | #wait_for_user() 91 | # TODO: FD instantiation is slightly slow to a deepcopy 92 | # 4650801/25658 2.695 0.000 8.169 0.000 /home/caelan/Programs/srlstream/pddlstream/pddlstream/algorithms/skeleton.py:114(do_evaluate_helper) 93 | #test_observation(world, entity_name='big_red_block0') 94 | #return 95 | 96 | # TODO: mechanism that pickles the state of the world 97 | real_state = create_state(world) 98 | video = None 99 | if args.record: 100 | wait_for_user('Start?') 101 | video_path = VIDEO_TEMPLATE.format(args.problem) 102 | video = VideoSaver(video_path) 103 | time_step = None if args.teleport else DEFAULT_TIME_STEP 104 | 105 | def observation_fn(belief): 106 | return observe_pybullet(world) 107 | 108 | def transition_fn(belief, commands): 109 | # if not args.record: # Video doesn't include planning time 110 | # wait_for_user() 111 | # restore real_state just in case? 112 | # wait_for_user() 113 | #if args.fixed: # args.simulate 114 | return simulate_commands(real_state, commands) 115 | #return iterate_commands(real_state, commands, time_step=time_step, pause=False) 116 | 117 | run_policy(task, args, observation_fn, transition_fn) 118 | 119 | if video: 120 | print('Saved', video_path) 121 | video.restore() 122 | world.destroy() 123 | # TODO: make the sink extrude from the mesh 124 | 125 | if __name__ == '__main__': 126 | main() 127 | -------------------------------------------------------------------------------- /src/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/caelan/SS-Replan/da4ae12fe8e345ba636aff4cc787f9e35c4d12e7/src/__init__.py -------------------------------------------------------------------------------- /src/belief.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import time 4 | import math 5 | import copy 6 | 7 | from pddlstream.utils import str_from_object 8 | from examples.discrete_belief.dist import UniformDist, DeltaDist 9 | #from examples.discrete_belief.run import continue_mdp_cost 10 | #from examples.pybullet.pr2_belief.primitives import get_observation_fn 11 | #from examples.pybullet.pr2_belief.problems import BeliefState, BeliefTask 12 | 13 | #from examples.discrete_belief.run import geometric_cost 14 | from pybullet_tools.utils import BodySaver, joint_from_name, LockRenderer, spaced_colors, WorldSaver, \ 15 | pairwise_collision, elapsed_time, randomize, remove_handles, wait_for_duration, wait_for_user, \ 16 | get_joint_positions, get_joint_name, get_joint_position, GREEN 17 | from src.command import State, TIN_OBJECTS 18 | from src.inference import NUM_PARTICLES, PoseDist 19 | from src.observe import fix_detections, relative_detections, ELSEWHERE 20 | from src.stream import get_stable_gen 21 | from src.utils import create_relative_pose, RelPose, FConf, are_confs_close, type_from_name 22 | 23 | # TODO: prior on the number of false detections to ensure correlated 24 | # TODO: could do open world or closed world. For open world, can sum independent probabilities 25 | # TODO: use a proper probabilistic programming library rather than dist.py 26 | # TODO: the really can just be viewed as a change in the kernel applied 27 | 28 | # Detect preconditions and cost 29 | # * Most general would be conditioning the success prob on the full state via a cost 30 | # * Does not admit factoring though 31 | # * Instead, produce a detection for a subset of the region 32 | # * Preconditions involving the likelihood something is interfering 33 | 34 | # https://github.com/tlpmit/hpn 35 | # https://github.mit.edu/tlp/bhpn 36 | # https://github.com/caelan/pddlstream/tree/stable/examples/discrete_belief 37 | # https://github.mit.edu/caelan/stripstream/blob/master/scripts/openrave/run_belief_online.py 38 | # https://github.mit.edu/caelan/stripstream/blob/master/robotics/openrave/belief_tamp.py 39 | # https://github.mit.edu/caelan/ss/blob/master/belief/belief_online.py 40 | # https://github.com/caelan/pddlstream/blob/stable/examples/pybullet/pr2_belief/run.py 41 | 42 | MIN_GRASP_WIDTH = 0.005 43 | REPAIR_DETECTIONS = True 44 | STOCHASTIC_PLACE = False 45 | 46 | ################################################################################ 47 | 48 | # TODO: point estimates and confidence intervals/regions 49 | # TODO: mixture between discrete and growing Mixture of Gaussian 50 | # TODO: belief fluents 51 | 52 | ARM_TOLERANCE = math.radians(2) # Can afford to be larger (just move to new initial conf) 53 | GRIPPER_TOLERANCE = 1e-2 54 | 55 | class Belief(object): 56 | def __init__(self, world, pose_dists={}, grasped=None): 57 | self.world = world 58 | self.pose_dists = pose_dists 59 | self.grasped = grasped # grasped or holding? 60 | #colors = spaced_colors(len(self.objects)) 61 | colors = [GREEN]*len(self.objects) 62 | self.color_from_name = dict(zip(self.objects, colors)) 63 | self.observations = [] 64 | self.handles = [] 65 | 66 | # TODO: store state history 67 | self.base_conf = None 68 | self.arm_conf = None 69 | self.gripper_conf = None 70 | self.door_confs = {} 71 | self.pressed = set() 72 | self.cooked = set() 73 | self.liquid = set() 74 | def update_state(self): 75 | # TODO: apply this directly from observations 76 | # No use applying this to base confs 77 | self.base_conf = FConf(self.world.robot, self.world.base_joints, init=True) 78 | arm_conf = FConf(self.world.robot, self.world.arm_joints, init=True) 79 | if (self.arm_conf is None) or not are_confs_close(arm_conf, self.arm_conf, tol=ARM_TOLERANCE): 80 | self.arm_conf = arm_conf 81 | else: 82 | print('At anticipated arm conf') 83 | gripper_conf = FConf(self.world.robot, self.world.gripper_joints, init=True) 84 | if (self.gripper_conf is None) or not are_confs_close(gripper_conf, self.gripper_conf, tol=GRIPPER_TOLERANCE): 85 | self.gripper_conf = gripper_conf 86 | else: 87 | print('At anticipated gripper conf') 88 | 89 | # TODO: do I still need to test if the current values are equal to the last ones? 90 | for joint in self.world.kitchen_joints: 91 | name = get_joint_name(self.world.kitchen, joint) 92 | position = get_joint_position(self.world.kitchen, joint) 93 | self.update_door_conf(name, position) 94 | self.update_door_conf(name, position) 95 | #wait_for_user() 96 | return self.check_consistent() 97 | def update_door_conf(self, name, position): 98 | joint = joint_from_name(self.world.kitchen, name) 99 | conf = FConf(self.world.kitchen, [joint], [position], init=True) 100 | if (name not in self.door_confs) or not are_confs_close(conf, self.door_confs[name], tol=1e-3): 101 | # TODO: different threshold for drawers and doors 102 | self.door_confs[name] = conf 103 | else: 104 | print('At anticipated conf for door {}'.format(name)) 105 | return self.door_confs[name] 106 | @property 107 | def holding(self): 108 | if self.grasped is None: 109 | return None 110 | return self.grasped.body_name 111 | @property 112 | def placed(self): 113 | return sorted(set(self.pose_dists.keys())) 114 | @property 115 | def objects(self): 116 | objects = set(self.placed) 117 | if self.holding is not None: 118 | objects.add(self.holding) 119 | return sorted(objects) 120 | def is_gripper_closed(self): 121 | # TODO: base this on the object type 122 | if self.holding is not None: 123 | obj_type = type_from_name(self.holding) 124 | if obj_type not in TIN_OBJECTS: 125 | return False 126 | # each joint in [0.00, 0.04] (units coincide with meters on the physical gripper) 127 | current_gq = get_joint_positions(self.world.robot, self.world.gripper_joints) 128 | gripper_width = sum(current_gq) 129 | return gripper_width <= MIN_GRASP_WIDTH 130 | def check_consistent(self): 131 | # https://github.mit.edu/Learning-and-Intelligent-Systems/ltamp_pr2/blob/d1e6024c5c13df7edeab3a271b745e656a794b02/control_tools/execution.py#L163 132 | # https://github.mit.edu/Learning-and-Intelligent-Systems/ltamp_pr2/blob/master/control_tools/pr2_controller.py#L93 133 | # https://github.mit.edu/caelan/mudfish/blob/master/scripts/planner.py#L346 134 | if (self.grasped is not None) and self.is_gripper_closed(): 135 | # TODO: need to add the grasped object back into the dist 136 | self.grasped = None 137 | print('Inconsistent belief!') 138 | return False 139 | return True 140 | def update(self, detections, n_samples=25): 141 | start_time = time.time() 142 | self.observations.append(detections) 143 | # Processing detected first 144 | # Could simply sample from the set of worlds and update 145 | # Would need to sample many worlds with name at different poses 146 | # Instead, let the moving object take on different poses 147 | with LockRenderer(): 148 | with WorldSaver(): 149 | if REPAIR_DETECTIONS: 150 | detections = fix_detections(self, detections) # TODO: skip if in sim 151 | detections = relative_detections(self, detections) 152 | order = [name for name in detections] # Detected 153 | order.extend(set(self.pose_dists) - set(order)) # Not detected 154 | for name in order: 155 | self.pose_dists[name] = self.pose_dists[name].update( 156 | self, detections, n_samples=n_samples) 157 | self.update_state() 158 | print('Update time: {:.3f} sec for {} objects and {} samples'.format( 159 | elapsed_time(start_time), len(order), n_samples)) 160 | return self 161 | 162 | def sample(self, discrete=True): 163 | # TODO: timeout if unable to find 164 | while True: 165 | poses = {} 166 | for name, pose_dist in randomize(self.pose_dists.items()): 167 | body = self.world.get_body(name) 168 | pose = pose_dist.sample_discrete() if discrete else pose_dist.sample() 169 | pose.assign() 170 | if any(pairwise_collision(body, self.world.get_body(other)) for other in poses): 171 | break 172 | poses[name] = pose 173 | else: 174 | return poses 175 | 176 | def sample_state(self, **kwargs): 177 | pose_from_name = self.sample(**kwargs) 178 | world_saver = WorldSaver() 179 | attachments = [] 180 | for pose in pose_from_name.values(): 181 | attachments.extend(pose.confs) 182 | if self.grasped is not None: 183 | attachments.append(self.grasped.get_attachment()) 184 | return State(self.world, savers=[world_saver], attachments=attachments) 185 | #def resample(self): 186 | # for pose_dist in self.pose_dists: 187 | # pose_dist.resample() # Need to update distributions 188 | def dump(self): 189 | print(self) 190 | for i, name in enumerate(sorted(self.pose_dists)): 191 | #self.pose_dists[name].dump() 192 | print(i, name, self.pose_dists[name]) 193 | def draw(self, **kwargs): 194 | with LockRenderer(True): 195 | remove_handles(self.handles) 196 | self.handles = [] 197 | with WorldSaver(): 198 | for name, pose_dist in self.pose_dists.items(): 199 | self.handles.extend(pose_dist.draw( 200 | color=self.color_from_name[name], **kwargs)) 201 | def __repr__(self): 202 | return '{}(holding={}, placed={})'.format(self.__class__.__name__, self.holding, str_from_object( 203 | {name: self.pose_dists[name].surface_dist for name in self.placed})) 204 | 205 | ################################################################################ 206 | 207 | def create_observable_pose_dist(world, obj_name): 208 | body = world.get_body(obj_name) 209 | surface_name = world.get_supporting(obj_name) 210 | if surface_name is None: 211 | pose = RelPose(body, init=True) # Treats as obstacle 212 | else: 213 | pose = create_relative_pose(world, obj_name, surface_name, init=True) 214 | return PoseDist(world, obj_name, DeltaDist(pose)) 215 | 216 | def create_observable_belief(world, **kwargs): 217 | with WorldSaver(): 218 | belief = Belief(world, pose_dists={ 219 | name: create_observable_pose_dist(world, name) 220 | for name in world.movable}, **kwargs) 221 | belief.task = world.task 222 | return belief 223 | 224 | def create_surface_pose_dist(world, obj_name, surface_dist, n=NUM_PARTICLES): 225 | # TODO: likely easier to just make a null surface below ground 226 | placement_gen = get_stable_gen(world, max_attempts=100, learned=True, 227 | pos_scale=1e-3, rot_scale=1e-2) 228 | poses = [] 229 | with LockRenderer(): 230 | with BodySaver(world.get_body(obj_name)): 231 | while len(poses) < n: 232 | surface_name = surface_dist.sample() 233 | assert surface_name is not ELSEWHERE 234 | result = next(placement_gen(obj_name, surface_name), None) 235 | if result is None: 236 | surface_dist = surface_dist.condition(lambda s: s != surface_name) 237 | else: 238 | (rel_pose,) = result 239 | rel_pose.init = True 240 | poses.append(rel_pose) 241 | return PoseDist(world, obj_name, UniformDist(poses)) 242 | 243 | def create_surface_belief(world, surface_dists, **kwargs): 244 | with WorldSaver(): 245 | belief = Belief(world, pose_dists={ 246 | name: create_surface_pose_dist(world, name, surface_dist) 247 | for name, surface_dist in surface_dists.items()}, **kwargs) 248 | belief.task = world.task 249 | return belief 250 | 251 | ################################################################################ 252 | 253 | def delocalize_belief(belief, o, rp): 254 | dist = UniformDist([rp, copy.copy(rp)]) 255 | belief.pose_dists[o] = PoseDist(belief.world, o, dist) 256 | return dist 257 | 258 | def transition_belief_update(belief, plan): 259 | if plan is None: 260 | return False 261 | success = True 262 | for action, params in plan: 263 | if action in ['move_base', 'calibrate', 'detect']: 264 | pass 265 | elif action == 'press-on': 266 | s, k, o, bq, aq, gq, at = params 267 | belief.pressed.add(k) 268 | belief.cooked.add(o) 269 | for bowl, liquid in belief.liquid: 270 | if bowl == o: 271 | belief.cooked.add(liquid) 272 | elif action == 'press-off': 273 | s, k, o, bq, aq, gq, at = params 274 | belief.pressed.remove(k) 275 | elif action == 'move_arm': 276 | bq, aq1, aq2, at = params 277 | belief.arm_conf = aq2 278 | elif action == 'move_gripper': 279 | gq1, gq2, gt = params 280 | belief.gripper_conf = gq2 281 | elif action == 'pull': 282 | j, a1, a2, o, wp1, wp2, bq, aq1, aq2, gq, at = params 283 | belief.door_confs[j] = a2 284 | belief.arm_conf = aq2 285 | elif action == 'pour': 286 | bowl, wp, cup, g, liquid, bq, aq, at = params 287 | belief.liquid.discard((cup, liquid)) 288 | belief.liquid.add((bowl, liquid)) 289 | elif action == 'pick': 290 | o, p, g, rp = params[:4] 291 | obj_type = type_from_name(o) 292 | if (obj_type not in TIN_OBJECTS) or not belief.is_gripper_closed(): 293 | del belief.pose_dists[o] 294 | belief.grasped = g 295 | # TODO: open gripper afterwards to ensure not in hand 296 | else: 297 | delocalize_belief(belief, o, rp) 298 | print('Failed to grasp! Delocalizing belief') 299 | success = False 300 | break 301 | elif action == 'place': 302 | o, p, g, rp = params[:4] 303 | belief.grasped = None 304 | if STOCHASTIC_PLACE and belief.world.is_real(): 305 | delocalize_belief(belief, o, rp) 306 | else: 307 | belief.pose_dists[o] = PoseDist(belief.world, o, DeltaDist(rp)) 308 | elif action == 'cook': 309 | pass 310 | else: 311 | raise NotImplementedError(action) 312 | # TODO: replan after every action 313 | return success 314 | -------------------------------------------------------------------------------- /src/command.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import math 4 | import numpy as np 5 | import time 6 | 7 | from pybullet_tools.utils import get_moving_links, set_joint_positions, create_attachment, \ 8 | wait_for_duration, flatten_links, remove_handles, \ 9 | batch_ray_collision, draw_ray, wait_for_user, WorldSaver, adjust_path, waypoints_from_path 10 | from pybullet_tools.retime import interpolate_path, decompose_into_paths 11 | from src.utils import create_surface_attachment, SPAM, TOMATO_SOUP, MUSTARD, SUGAR, CHEEZIT, DEBUG 12 | 13 | DEFAULT_TIME_STEP = 0.02 14 | DEFAULT_SLEEP = 0.5 15 | FORCE = 50 # 20 | 50 | 100 16 | 17 | TIN_EFFORT = 60 18 | PLASTIC_EFFORT = 50 19 | CARDBOARD_EFFORT = 60 20 | 21 | TIN_OBJECTS = [SPAM] 22 | 23 | EFFORT_FROM_OBJECT = { 24 | SPAM: TIN_EFFORT, 25 | TOMATO_SOUP: TIN_EFFORT, 26 | MUSTARD: PLASTIC_EFFORT, 27 | SUGAR: CARDBOARD_EFFORT, 28 | CHEEZIT: CARDBOARD_EFFORT, 29 | } 30 | # TODO: grasps per object 31 | 32 | ################################################################################ 33 | 34 | class State(object): 35 | # TODO: rename to be world state? 36 | def __init__(self, world, savers=[], attachments=[]): 37 | # a part of the state separate from PyBullet 38 | self.world = world 39 | self.savers = tuple(savers) 40 | self.attachments = {attachment.child: attachment for attachment in attachments} 41 | @property 42 | def bodies(self): 43 | raise NotImplementedError() 44 | #return {saver.body for saver in self.savers} | set(self.attachments) 45 | def derive(self): 46 | for attachment in self.attachments.values(): 47 | # Derived values 48 | # TODO: topological sort 49 | attachment.assign() 50 | def copy(self): 51 | return State(self.world, self.savers, self.attachments.values()) 52 | #return copy.deepcopy(self) 53 | def assign(self): 54 | for saver in self.savers: 55 | saver.restore() 56 | self.derive() 57 | def __repr__(self): 58 | return '{}({}, {})'.format(self.__class__.__name__, list(self.savers), self.attachments) 59 | 60 | 61 | def create_state(world): 62 | # TODO: support initially holding 63 | # TODO: would be better to explicitly keep the state around 64 | # world.initial_saver.restore() 65 | world_saver = WorldSaver() 66 | attachments = [] 67 | for obj_name in world.movable: 68 | surface_name = world.get_supporting(obj_name) 69 | if surface_name is not None: 70 | attachments.append(create_surface_attachment(world, obj_name, surface_name)) 71 | return State(world, savers=[world_saver], attachments=attachments) 72 | 73 | ################################################################################ 74 | 75 | class Command(object): 76 | def __init__(self, world): 77 | self.world = world 78 | #@property 79 | #def robot(self): 80 | # return self.world.robot 81 | @property 82 | def bodies(self): 83 | raise NotImplementedError() 84 | @property 85 | def cost(self): 86 | raise NotImplementedError() 87 | def reverse(self): 88 | raise NotImplementedError() 89 | def iterate(self, state): 90 | raise NotImplementedError() 91 | def simulate(self, state, time_per_step=DEFAULT_TIME_STEP, **kwargs): 92 | for j, _ in enumerate(self.iterate(state)): 93 | state.derive() 94 | if j != 0: 95 | wait_for_duration(time_per_step) 96 | def execute(self, interface): 97 | raise NotImplementedError() 98 | 99 | class Sequence(object): 100 | def __init__(self, context, commands=[], name=None): 101 | self.context = context 102 | self.commands = tuple(commands) 103 | self.name = self.__class__.__name__.lower() if name is None else name 104 | @property 105 | def bodies(self): 106 | bodies = set(self.context.bodies) 107 | for command in self.commands: 108 | bodies.update(command.bodies) 109 | return bodies 110 | @property 111 | def cost(self): 112 | return sum([0] + [command.cost for command in self.commands]) 113 | def reverse(self): 114 | return Sequence(self.context, [command.reverse() for command in reversed(self.commands)], name=self.name) 115 | def __repr__(self): 116 | #return '[{}]'.format('->'.join(map(repr, self.commands))) 117 | return '{}({})'.format(self.name, len(self.commands)) 118 | 119 | ################################################################################ 120 | 121 | class Trajectory(Command): 122 | def __init__(self, world, robot, joints, path, speed=1.0): 123 | super(Trajectory, self).__init__(world) 124 | self.robot = robot 125 | self.joints = tuple(joints) 126 | self.path = tuple(path) 127 | self.speed = speed 128 | @property 129 | def bodies(self): 130 | # TODO: decompose into dependents and moving? 131 | return flatten_links(self.robot, get_moving_links(self.robot, self.joints)) 132 | @property 133 | def cost(self): 134 | return len(self.path) 135 | def reverse(self): 136 | return self.__class__(self.world, self.robot, self.joints, self.path[::-1]) 137 | def iterate(self, state): 138 | #time_parameterization(self.robot, self.joints, self.path) 139 | for positions in self.path: 140 | set_joint_positions(self.robot, self.joints, positions) 141 | yield 142 | def simulate(self, state, real_per_sim=1, time_step=1./60, **kwargs): 143 | 144 | path = list(self.path) 145 | path = adjust_path(self.robot, self.joints, path) 146 | path = waypoints_from_path(path) 147 | if len(path) <= 1: 148 | return True 149 | for joints, path in decompose_into_paths(self.joints, path): 150 | positions_curve = interpolate_path(self.robot, joints, path) 151 | print('Following {} {}-DOF waypoints in {:.3f} seconds'.format(len(path), len(joints), positions_curve.x[-1])) 152 | for t in np.arange(positions_curve.x[0], positions_curve.x[-1], step=time_step): 153 | positions = positions_curve(t) 154 | set_joint_positions(self.robot, joints, positions) 155 | state.derive() 156 | wait_for_duration(real_per_sim*time_step) 157 | return True 158 | def __repr__(self): 159 | return '{}({}x{})'.format(self.__class__.__name__, len(self.joints), len(self.path)) 160 | 161 | ################################################################################ 162 | 163 | class ApproachTrajectory(Trajectory): 164 | def __init__(self, objects=[], *args, **kwargs): 165 | super(ApproachTrajectory, self).__init__(*args, **kwargs) 166 | assert self.joints == self.world.arm_joints 167 | self.speed = 0.25 168 | self.objects = set(objects) 169 | @property 170 | def bodies(self): 171 | bodies = set(super(ApproachTrajectory, self).bodies) # TODO: rename to bodies 172 | for name in self.objects: 173 | bodies.update(flatten_links(self.world.get_body(name))) 174 | return bodies 175 | def reverse(self): 176 | return self.__class__(self.objects, self.world, self.robot, self.joints, self.path[::-1]) 177 | 178 | 179 | class DoorTrajectory(Command): # TODO: extend Trajectory 180 | def __init__(self, world, robot, robot_joints, robot_path, 181 | door, door_joints, door_path): 182 | super(DoorTrajectory, self).__init__(world) 183 | self.robot = robot 184 | self.robot_joints = tuple(robot_joints) 185 | self.robot_path = tuple(robot_path) 186 | self.door = door 187 | self.door_joints = tuple(door_joints) 188 | self.door_path = tuple(door_path) 189 | self.do_pull = (door_path[0][0] < door_path[-1][0]) 190 | assert len(self.robot_path) == len(self.door_path) 191 | @property 192 | def joints(self): 193 | return self.robot_joints 194 | @property 195 | def path(self): 196 | return self.robot_path 197 | @property 198 | def bodies(self): 199 | return flatten_links(self.robot, get_moving_links(self.robot, self.robot_joints)) | \ 200 | flatten_links(self.world.kitchen, get_moving_links(self.world.kitchen, self.door_joints)) 201 | @property 202 | def cost(self): 203 | return len(self.path) 204 | def reverse(self): 205 | return self.__class__(self.world, self.robot, self.robot_joints, self.robot_path[::-1], 206 | self.door, self.door_joints, self.door_path[::-1]) 207 | def iterate(self, state): 208 | for robot_conf, door_conf in zip(self.robot_path, self.door_path): 209 | set_joint_positions(self.robot, self.robot_joints, robot_conf) 210 | set_joint_positions(self.door, self.door_joints, door_conf) 211 | yield 212 | def simulate(self, state, **kwargs): 213 | # TODO: linearly interpolate for drawer 214 | # TODO: interpolate drawer and robot individually 215 | # TODO: find drawer joint angle that minimizes deviation from transform 216 | super(DoorTrajectory, self).simulate(state, time_per_step=2*DEFAULT_TIME_STEP) 217 | def __repr__(self): 218 | return '{}({}x{})'.format(self.__class__.__name__, len(self.robot_joints) + len(self.door_joints), 219 | len(self.robot_path)) 220 | 221 | ################################################################################s 222 | 223 | class Attach(Command): 224 | def __init__(self, world, robot, link, body): 225 | # TODO: names or bodies? 226 | super(Attach, self).__init__(world) 227 | self.robot = robot 228 | self.link = link 229 | self.body = body 230 | @property 231 | def bodies(self): 232 | return set() 233 | #return {self.robot, self.body} 234 | @property 235 | def cost(self): 236 | return 0 237 | def reverse(self): 238 | return Detach(self.world, self.robot, self.link, self.body) 239 | def attach(self): 240 | return create_attachment(self.robot, self.link, self.body) 241 | def iterate(self, state): 242 | state.attachments[self.body] = self.attach() 243 | yield 244 | def execute(self, interface): 245 | return True 246 | def __repr__(self): 247 | return '{}({})'.format(self.__class__.__name__, self.world.get_name(self.body)) 248 | 249 | class AttachGripper(Attach): 250 | def __init__(self, world, body, grasp=None): 251 | super(AttachGripper, self).__init__(world, world.robot, world.tool_link, body) 252 | self.grasp = grasp 253 | 254 | #class AttachSurface(Attach): 255 | # def __init__(self, world, obj_name, surface_name): 256 | # body = world.get_body(obj_name) 257 | # surface = surface_from_name(surface_name) 258 | # surface_link = link_from_name(world.kitchen, surface.link) 259 | # super(AttachSurface, self).__init__(world, world.kitchen, surface_link, body) 260 | # self.obj_name = obj_name 261 | # self.surface_name = surface_name 262 | 263 | class Detach(Command): 264 | def __init__(self, world, robot, link, body): 265 | super(Detach, self).__init__(world) 266 | self.robot = robot 267 | self.link = link 268 | self.body = body 269 | @property 270 | def bodies(self): 271 | return set() 272 | #return {self.robot, self.body} 273 | @property 274 | def cost(self): 275 | return 0 276 | def reverse(self): 277 | return Attach(self.world, self.robot, self.link, self.body) 278 | def iterate(self, state): 279 | assert self.body in state.attachments 280 | del state.attachments[self.body] 281 | yield 282 | def __repr__(self): 283 | return '{}({})'.format(self.__class__.__name__, self.world.get_name(self.body)) 284 | 285 | ################################################################################s 286 | 287 | class Detect(Command): 288 | duration = 2.0 289 | def __init__(self, world, camera, name, pose, rays): 290 | super(Detect, self).__init__(world) 291 | self.camera = camera 292 | self.name = name 293 | self.pose = pose # Object pose 294 | self.rays = tuple(rays) 295 | # TODO: could instead use cones for full detection 296 | # TODO: bodies? 297 | @property 298 | def cost(self): 299 | return 0 300 | @property 301 | def surface_name(self): 302 | return self.pose.support 303 | def ray_collision(self): 304 | return batch_ray_collision(self.rays) 305 | def compute_occluding(self): 306 | # TODO: compute as a fraction of the rays 307 | return {(result.objectUniqueId, frozenset([result.linkIndex])) 308 | for result in self.ray_collision() if result.objectUniqueId != -1} 309 | def draw(self): 310 | handles = [] 311 | for ray, result in zip(self.rays, self.ray_collision()): 312 | handles.extend(draw_ray(ray, result)) 313 | return handles 314 | def iterate(self, state): 315 | handles = self.draw() if DEBUG else [] 316 | steps = int(math.ceil(self.duration / 0.02)) 317 | for _ in range(steps): 318 | yield 319 | remove_handles(handles) 320 | def execute(self, interface): 321 | return True 322 | def __repr__(self): 323 | return '{}({}, {}, {})'.format( 324 | self.__class__.__name__, self.camera, self.name, self.surface_name) 325 | 326 | class Wait(Command): 327 | def __init__(self, world, steps=1, duration=1.0): 328 | super(Wait, self).__init__(world) 329 | self.steps = steps 330 | self.duration = duration 331 | @property 332 | def bodies(self): 333 | return set() 334 | @property 335 | def cost(self): 336 | return 0 337 | def reverse(self): 338 | return self 339 | def iterate(self, state): 340 | for _ in range(self.steps+1): 341 | yield 342 | def simulate(self, state, **kwargs): 343 | wait_for_duration(self.duration) 344 | def execute(self, interface): 345 | time.sleep(self.duration) 346 | #import rospy 347 | #rospy.sleep(self.duration) 348 | return True 349 | def __repr__(self): 350 | return '{}({})'.format(self.__class__.__name__, self.steps) 351 | 352 | # TODO: cook that includes a wait 353 | 354 | ################################################################################s 355 | 356 | def iterate_commands(state, commands, time_step=DEFAULT_TIME_STEP, pause=False): 357 | if commands is None: 358 | return False 359 | for i, command in enumerate(commands): 360 | print('\nCommand {:2}/{:2}: {}'.format(i + 1, len(commands), command)) 361 | # TODO: skip to end 362 | # TODO: downsample 363 | for j, _ in enumerate(command.iterate(state)): 364 | state.derive() 365 | if j == 0: 366 | continue 367 | if time_step is None: 368 | wait_for_duration(1e-2) 369 | wait_for_user('Command {:2}/{:2} | step {:2} | Next?'.format(i + 1, len(commands), j)) 370 | elif time_step == 0: 371 | pass 372 | else: 373 | wait_for_duration(time_step) 374 | if pause: 375 | wait_for_user('Continue?') 376 | return True 377 | 378 | def simulate_commands(state, commands, **kwargs): 379 | if commands is None: 380 | return False 381 | # TODO: simulate commands simultaneously 382 | for i, command in enumerate(commands): 383 | print('\nCommand {:2}/{:2}: {}'.format(i + 1, len(commands), command)) 384 | command.simulate(state, **kwargs) 385 | return True 386 | 387 | def execute_commands(interface, commands): 388 | if commands is None: 389 | return False 390 | for command in commands: 391 | success = command.execute(interface) 392 | if success: 393 | print('Successfully executed command', command) 394 | else: 395 | print('Failed to execute command', command) 396 | return False 397 | return True 398 | -------------------------------------------------------------------------------- /src/database.py: -------------------------------------------------------------------------------- 1 | import os 2 | import random 3 | 4 | from pybullet_tools.utils import read_json, link_from_name, get_link_pose, multiply, \ 5 | euler_from_quat, draw_point, wait_for_user, set_joint_positions, joints_from_names, parent_link_from_joint, has_gui, \ 6 | point_from_pose, RED, child_link_from_joint, get_pose, get_point, invert, base_values_from_pose 7 | from src.utils import GRASP_TYPES, surface_from_name, BASE_JOINTS, joint_from_name, unit_pose, ALL_SURFACES, KNOBS 8 | 9 | DATABASE_DIRECTORY = os.path.join(os.path.dirname(os.path.abspath(__file__)), os.pardir, 'databases/') 10 | PLACE_IR_FILENAME = '{robot_name}-{surface_name}-{grasp_type}-place.json' 11 | #PULL_IR_FILENAME = '{robot_name}-{joint_name}-pull.json' 12 | #PRESS_IR_FILENAME = '{robot_name}-{knob_name}-press.json' 13 | PULL_IR_FILENAME = '{}-{}-pull.json' 14 | PRESS_IR_FILENAME = '{}-{}-press.json' 15 | 16 | def get_surface_reference_pose(kitchen, surface_name): 17 | surface = surface_from_name(surface_name) 18 | link = link_from_name(kitchen, surface.link) 19 | return get_link_pose(kitchen, link) 20 | 21 | def project_base_pose(base_pose): 22 | #return base_values_from_pose(base_pose) 23 | base_point, base_quat = base_pose 24 | x, y, _ = base_point 25 | _, _, theta = euler_from_quat(base_quat) 26 | base_values = (x, y, theta) 27 | return base_values 28 | 29 | ################################################################################ 30 | 31 | def get_place_path(robot_name, surface_name, grasp_type): 32 | return os.path.abspath(os.path.join(DATABASE_DIRECTORY, PLACE_IR_FILENAME.format( 33 | robot_name=robot_name, surface_name=surface_name, grasp_type=grasp_type))) 34 | 35 | def has_place_database(robot_name, surface_name, grasp_type): 36 | return os.path.exists(get_place_path(robot_name, surface_name, grasp_type)) 37 | 38 | def load_place_entries(robot_name, surface_name, grasp_type): 39 | if not has_place_database(robot_name, surface_name, grasp_type): 40 | return [] 41 | return read_json(get_place_path(robot_name, surface_name, grasp_type)).get('entries', []) 42 | 43 | def load_place_database(robot_name, surface_name, grasp_type, field): 44 | return [entry[field] for entry in load_place_entries(robot_name, surface_name, grasp_type)] 45 | 46 | def load_placements(world, surface_name, grasp_types=GRASP_TYPES): 47 | # TODO: could also annotate which grasp came with which placement 48 | placements = [] 49 | for grasp_type in grasp_types: 50 | placements.extend(load_place_database(world.robot_name, surface_name, grasp_type, 51 | field='surface_from_object')) 52 | random.shuffle(placements) 53 | return placements 54 | 55 | def load_forward_placements(world, surface_names=ALL_SURFACES, grasp_types=GRASP_TYPES): 56 | base_from_objects = [] 57 | for surface_name in surface_names: 58 | for grasp_type in grasp_types: 59 | base_from_objects.extend(load_place_database(world.robot_name, surface_name, grasp_type, 60 | field='base_from_object')) 61 | return base_from_objects 62 | 63 | def load_place_base_poses(world, tool_pose, surface_name, grasp_type): 64 | # TODO: Gaussian perturbation 65 | gripper_from_base_list = load_place_database(world.robot_name, surface_name, grasp_type, 66 | field='tool_from_base') 67 | random.shuffle(gripper_from_base_list) 68 | handles = [] 69 | for gripper_from_base in gripper_from_base_list: 70 | #world_from_model = get_pose(world.robot) 71 | world_from_model = unit_pose() 72 | base_values = project_base_pose(multiply(invert(world_from_model), tool_pose, gripper_from_base)) 73 | #x, y, _ = base_values 74 | #_, _, z = get_point(world.floor) 75 | #set_joint_positions(world.robot, joints_from_names(world.robot, BASE_JOINTS), base_values) 76 | #handles.extend(draw_point(np.array([x, y, z + 0.01]), color=(1, 0, 0), size=0.05)) 77 | #wait_for_user() 78 | yield base_values 79 | 80 | def load_inverse_placements(world, surface_name, grasp_types=GRASP_TYPES): 81 | surface_from_bases = [] 82 | for grasp_type in grasp_types: 83 | for entry in load_place_entries(world.robot_name, surface_name, grasp_type): 84 | surface_from_bases.append(multiply(entry['surface_from_object'], 85 | invert(entry['base_from_object']))) 86 | random.shuffle(surface_from_bases) 87 | return surface_from_bases 88 | 89 | def load_pour_base_poses(world, surface_name, **kwargs): 90 | world_from_surface = get_surface_reference_pose(world.kitchen, surface_name) 91 | for surface_from_base in load_inverse_placements(world, surface_name, **kwargs): 92 | base_values = project_base_pose(multiply(world_from_surface, surface_from_base)) 93 | #world.set_base_conf(base_values) 94 | #wait_for_user() 95 | yield base_values 96 | 97 | ################################################################################ 98 | 99 | def is_press(joint_name): 100 | return joint_name in KNOBS 101 | 102 | def get_joint_reference_pose(kitchen, joint_name): 103 | if is_press(joint_name): 104 | return get_link_pose(kitchen, link_from_name(kitchen, joint_name)) 105 | joint = joint_from_name(kitchen, joint_name) 106 | link = parent_link_from_joint(kitchen, joint) 107 | return get_link_pose(kitchen, link) 108 | 109 | def get_pull_path(robot_name, joint_name): 110 | ir_filename = PRESS_IR_FILENAME if is_press(joint_name) else PULL_IR_FILENAME 111 | return os.path.abspath(os.path.join(DATABASE_DIRECTORY, ir_filename.format(robot_name, joint_name))) 112 | 113 | def load_pull_database(robot_name, joint_name): 114 | data = {} 115 | path = get_pull_path(robot_name, joint_name) 116 | if os.path.exists(path): 117 | data = read_json(path) 118 | return [entry['joint_from_base'] for entry in data.get('entries', [])] 119 | 120 | def load_pull_base_poses(world, joint_name): 121 | joint_from_base_list = load_pull_database(world.robot_name, joint_name) 122 | parent_pose = get_joint_reference_pose(world.kitchen, joint_name) 123 | random.shuffle(joint_from_base_list) 124 | handles = [] 125 | for joint_from_base in joint_from_base_list: 126 | #world_from_model = get_pose(world.robot) 127 | world_from_model = unit_pose() 128 | base_values = project_base_pose(multiply(invert(world_from_model), parent_pose, joint_from_base)) 129 | #set_joint_positions(world.robot, joints_from_names(world.robot, BASE_JOINTS), base_values) 130 | #x, y, _ = base_values 131 | #handles.extend(draw_point(np.array([x, y, -0.1]), color=(1, 0, 0), size=0.05)) 132 | yield base_values 133 | #wait_for_user() 134 | 135 | ################################################################################ 136 | 137 | def visualize_database(tool_from_base_list): 138 | #tool_from_base_list 139 | handles = [] 140 | if not has_gui(): 141 | return handles 142 | for gripper_from_base in tool_from_base_list: 143 | # TODO: move away from the environment 144 | handles.extend(draw_point(point_from_pose(gripper_from_base), color=RED)) 145 | wait_for_user() 146 | return handles 147 | -------------------------------------------------------------------------------- /src/observe.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import math 4 | import numpy as np 5 | import random 6 | 7 | from pybullet_tools.pr2_utils import is_visible_point 8 | from pybullet_tools.utils import get_pose, point_from_pose, Ray, batch_ray_collision, has_gui, add_line, BLUE, \ 9 | wait_for_duration, remove_handles, Pose, Point, Euler, multiply, set_pose, aabb_contains_point, tform_point, angle_between 10 | from src.utils import CAMERA_MATRIX, KINECT_DEPTH, create_relative_pose, create_world_pose 11 | 12 | OBS_P_FP, OBS_P_FN = 0.0, 0.0 13 | #OBS_POS_STD, OBS_ORI_STD = 0.01, np.pi / 8 14 | OBS_POS_STD, OBS_ORI_STD = 0., 0. 15 | ELSEWHERE = None # symbol for elsewhere pose 16 | 17 | 18 | ################################################################################ 19 | 20 | def are_visible(world): 21 | ray_names = [] 22 | rays = [] 23 | for name in world.movable: 24 | for camera, info in world.cameras.items(): 25 | camera_pose = get_pose(info.body) 26 | camera_point = point_from_pose(camera_pose) 27 | point = point_from_pose(get_pose(world.get_body(name))) 28 | if is_visible_point(CAMERA_MATRIX, KINECT_DEPTH, point, camera_pose=camera_pose): 29 | ray_names.append(name) 30 | rays.append(Ray(camera_point, point)) 31 | ray_results = batch_ray_collision(rays) 32 | visible_indices = [idx for idx, (name, result) in enumerate(zip(ray_names, ray_results)) 33 | if result.objectUniqueId == world.get_body(name)] 34 | visible_names = {ray_names[idx] for idx in visible_indices} 35 | print('Detected:', sorted(visible_names)) 36 | if has_gui(): 37 | handles = [add_line(rays[idx].start, rays[idx].end, color=BLUE) 38 | for idx in visible_indices] 39 | wait_for_duration(1.0) 40 | remove_handles(handles) 41 | # TODO: the object drop seems to happen around here 42 | return visible_names 43 | 44 | ################################################################################ 45 | 46 | def fully_observe_pybullet(world): 47 | return {name: get_pose(body) for name, body in world.body_from_name.items()} 48 | 49 | 50 | def observe_pybullet(world): 51 | # TODO: randomize robot's pose 52 | # TODO: probabilities based on whether in viewcone or not 53 | # TODO: sample from poses on table 54 | # world_saver = WorldSaver() 55 | visible_entities = are_visible(world) 56 | detections = {} 57 | assert OBS_P_FP == 0 58 | for name in visible_entities: 59 | # TODO: false positives 60 | if random.random() < OBS_P_FN: 61 | continue 62 | body = world.get_body(name) 63 | pose = get_pose(body) 64 | dx, dy = np.random.multivariate_normal( 65 | mean=np.zeros(2), cov=math.pow(OBS_POS_STD, 2) * np.eye(2)) 66 | dyaw, = np.random.multivariate_normal( 67 | mean=np.zeros(1), cov=math.pow(OBS_ORI_STD, 2) * np.eye(1)) 68 | print('{}: dx={:.3f}, dy={:.3f}, dyaw={:.5f}'.format(name, dx, dy, dyaw)) 69 | noise_pose = Pose(Point(x=dx, y=dy), Euler(yaw=dyaw)) 70 | observed_pose = multiply(pose, noise_pose) 71 | #world.get_body_type(name) 72 | detections.setdefault(name, []).append(observed_pose) # TODO: use type instead 73 | #world_saver.restore() 74 | return detections 75 | 76 | ################################################################################ 77 | 78 | def fix_detections(belief, detections, **kwargs): 79 | # TODO: move directly to belief? 80 | world = belief.world 81 | fixed_detections = {} 82 | for name in detections: 83 | if name == belief.holding: 84 | continue 85 | for observed_pose in detections[name]: 86 | fixed_pose, support = world.fix_pose(name, observed_pose, **kwargs) 87 | if fixed_pose is not None: 88 | fixed_detections.setdefault(name, []).append(fixed_pose) 89 | return fixed_detections 90 | 91 | 92 | def relative_detections(belief, detections): 93 | world = belief.world 94 | rel_detections = {} 95 | world_aabb = world.get_world_aabb() 96 | for name in detections: 97 | if name == belief.holding: 98 | continue 99 | body = world.get_body(name) 100 | for observed_pose in detections[name]: 101 | world_z_axis = np.array([0, 0, 1]) 102 | local_z_axis = tform_point(observed_pose, world_z_axis) 103 | if np.pi/2 < angle_between(world_z_axis, local_z_axis): 104 | observed_pose = multiply(observed_pose, Pose(euler=Euler(roll=np.pi))) 105 | if not aabb_contains_point(point_from_pose(observed_pose), world_aabb): 106 | continue 107 | set_pose(body, observed_pose) 108 | support = world.get_supporting(name) 109 | #assert support is not None 110 | # Could also fix as relative to the world 111 | if support is None: 112 | # TODO: prune if nowhere near a surface (e.g. on the robot) 113 | relative_pose = create_world_pose(world, name, init=True) 114 | else: 115 | relative_pose = create_relative_pose(world, name, support, init=True) 116 | rel_detections.setdefault(name, []).append(relative_pose) 117 | # relative_pose.assign() 118 | return rel_detections 119 | -------------------------------------------------------------------------------- /src/planner.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import cProfile 4 | import pstats 5 | import math 6 | 7 | #from examples.discrete_belief.run import MAX_COST 8 | from examples.discrete_belief.run import clip_cost 9 | from pddlstream.algorithms.constraints import PlanConstraints, OrderedSkeleton 10 | from pddlstream.algorithms.focused import solve_focused 11 | from pddlstream.algorithms.algorithm import reset_globals 12 | from pddlstream.algorithms.downward import set_cost_scale 13 | 14 | from pddlstream.language.constants import print_solution 15 | from pddlstream.language.stream import StreamInfo, PartialInputs 16 | from pddlstream.language.function import FunctionInfo 17 | from pddlstream.language.object import SharedOptValue 18 | from pddlstream.utils import INF, KILOBYTES_PER_GIGABYTE 19 | 20 | from pybullet_tools.utils import LockRenderer, WorldSaver, wait_for_user, VideoSaver, wait_for_duration 21 | from src.command import Wait, iterate_commands, Trajectory, DEFAULT_TIME_STEP 22 | from src.stream import detect_cost_fn, compute_detect_cost, COST_SCALE 23 | from src.replan import INTERNAL_ACTIONS 24 | 25 | # TODO: use the same objects for poses and configs 26 | from src.utils import RelPose 27 | 28 | VIDEO_TEMPLATE = '{}.mp4' 29 | #COST_BOUND = 1000.0 # TODO: be careful about the cost bound... 30 | COST_BOUND = INF 31 | #MAX_MEMORY = 3.0 * KILOBYTES_PER_GIGABYTE 32 | MAX_MEMORY = INF 33 | # TODO: multiple cost bounds 34 | 35 | ################################################################################ 36 | 37 | def opt_detect_cost_fn(obj_name, rp_dist, obs, rp_sample): 38 | # TODO: prune these surfaces if the cost is already too high 39 | if isinstance(rp_sample, RelPose) or (rp_dist == rp_sample): 40 | # This shouldn't be needed if eager=True 41 | return detect_cost_fn(obj_name, rp_dist, obs, rp_sample) 42 | prob = rp_dist.surface_prob(rp_dist.surface_name) 43 | #print(rp_dist.surface_name, prob) 44 | cost = clip_cost(compute_detect_cost(prob)) 45 | print('{}) Opt Detect Prob: {:.3f} | Opt Detect Cost: {:.3f} | Type: {}'.format( 46 | rp_dist.surface_name, prob, cost, rp_sample.__class__.__name__)) 47 | return cost 48 | 49 | def opt_move_base_test(bq1, bq2, aq): #, fluents=[]): 50 | # TODO: what did these previously do then? 51 | #print(bq1, bq2, bq1 != bq2) 52 | if isinstance(bq1, SharedOptValue) or isinstance(bq2, SharedOptValue): 53 | return True 54 | #if isinstance(UniqueOptValue, bq1) and (aq1 == bq2): 55 | # pass 56 | return bq1 != bq2 57 | 58 | def opt_move_arm_gen_test(bq, aq1, aq2): #, fluents=[]): 59 | if isinstance(aq1, SharedOptValue) or isinstance(aq2, SharedOptValue): 60 | return True 61 | return aq1 != aq2 62 | 63 | def get_stream_info(): 64 | opt_gen_fn = PartialInputs(unique=False) 65 | stream_info = { 66 | 'test-gripper': StreamInfo(p_success=0, eager=True), 67 | 'test-door': StreamInfo(p_success=0, eager=True), 68 | 'test-near-pose': StreamInfo(p_success=0, eager=True), 69 | 'test-near-joint': StreamInfo(p_success=0, eager=True), 70 | 71 | # TODO: need to be careful about conditional effects 72 | 'compute-pose-kin': StreamInfo(opt_gen_fn=PartialInputs(unique=True), 73 | p_success=0.5, eager=True), 74 | # 'compute-angle-kin': StreamInfo(p_success=0.5, eager=True), 75 | 'sample-pose': StreamInfo(opt_gen_fn=opt_gen_fn), 76 | 'sample-nearby-pose': StreamInfo(opt_gen_fn=opt_gen_fn), 77 | 'sample-grasp': StreamInfo(opt_gen_fn=opt_gen_fn), 78 | 79 | 'compute-detect': StreamInfo(opt_gen_fn=opt_gen_fn, p_success=1e-4), 80 | 81 | 'fixed-plan-pick': StreamInfo(opt_gen_fn=opt_gen_fn, overhead=1e1), 82 | 'plan-pick': StreamInfo(opt_gen_fn=opt_gen_fn, overhead=1e1), 83 | 84 | 'fixed-plan-pull': StreamInfo(opt_gen_fn=opt_gen_fn, overhead=1e1), 85 | # TODO: can't defer this because collision streams depend on it 86 | 'plan-pull': StreamInfo(opt_gen_fn=opt_gen_fn, overhead=1e1, defer=False), 87 | 88 | 'fixed-plan-press': StreamInfo(opt_gen_fn=opt_gen_fn, overhead=1e1), 89 | 'plan-press': StreamInfo(opt_gen_fn=opt_gen_fn, overhead=1e1), 90 | 91 | 'fixed-plan-pour': StreamInfo(opt_gen_fn=opt_gen_fn, overhead=1e1), 92 | 'plan-pour': StreamInfo(opt_gen_fn=opt_gen_fn, overhead=1e1), 93 | 94 | # 'plan-calibrate-motion': StreamInfo(opt_gen_fn=opt_gen_fn), 95 | 'plan-base-motion': StreamInfo(opt_gen_fn=PartialInputs(),#test=opt_move_base_test), 96 | overhead=1e3, defer=True), 97 | 'plan-arm-motion': StreamInfo(opt_gen_fn=PartialInputs(),#test=opt_move_arm_gen_test), 98 | overhead=1e2, defer=True), 99 | # 'plan-gripper-motion': StreamInfo(opt_gen_fn=opt_gen_fn), 100 | 101 | 'test-cfree-worldpose': StreamInfo(p_success=1e-3, negate=True, verbose=False), 102 | 'test-cfree-worldpose-worldpose': StreamInfo(p_success=1e-3, negate=True, verbose=False), 103 | 'test-cfree-pose-pose': StreamInfo(p_success=1e-3, negate=True, verbose=False), 104 | 'test-cfree-bconf-pose': StreamInfo(p_success=1e-3, negate=True, verbose=False), 105 | 'test-cfree-approach-pose': StreamInfo(p_success=1e-2, negate=True, verbose=False), 106 | 'test-cfree-angle-angle': StreamInfo(p_success=1e-2, negate=True, verbose=False), 107 | 'test-cfree-traj-pose': StreamInfo(p_success=1e-1, negate=True, verbose=False), 108 | 'test-ofree-ray-pose': StreamInfo(p_success=1e-3, negate=True, verbose=False), 109 | 'test-ofree-ray-grasp': StreamInfo(p_success=1e-3, negate=True, verbose=False), 110 | 111 | 'DetectCost': FunctionInfo(opt_detect_cost_fn, eager=True), 112 | # 'Distance': FunctionInfo(p_success=0.99, opt_fn=lambda bq1, bq2: BASE_CONSTANT), 113 | # 'MoveCost': FunctionInfo(lambda t: BASE_CONSTANT), 114 | } 115 | return stream_info 116 | 117 | ################################################################################ 118 | 119 | def create_ordered_skeleton(skeleton): 120 | if skeleton is None: 121 | return None 122 | #skeletons = [skeleton] 123 | #return skeletons 124 | orders = set() 125 | #orders = linear_order(skeleton) 126 | last_step = None 127 | for step, (name, args) in enumerate(skeleton): 128 | if last_step is not None: 129 | orders.add((last_step, step)) 130 | if name not in INTERNAL_ACTIONS: 131 | last_step = step 132 | return [OrderedSkeleton(skeleton, orders)] 133 | 134 | def solve_pddlstream(belief, problem, args, skeleton=None, replan_actions=set(), 135 | max_time=INF, max_memory=MAX_MEMORY, max_cost=INF): 136 | set_cost_scale(COST_SCALE) 137 | reset_globals() 138 | stream_info = get_stream_info() 139 | #print(set(stream_map) - set(stream_info)) 140 | skeletons = create_ordered_skeleton(skeleton) 141 | max_cost = min(max_cost, COST_BOUND) 142 | print('Max cost: {:.3f} | Max runtime: {:.3f}'.format(max_cost, max_time)) 143 | constraints = PlanConstraints(skeletons=skeletons, max_cost=max_cost, exact=True) 144 | 145 | success_cost = 0 if args.anytime else INF 146 | planner = 'ff-astar' if args.anytime else 'ff-wastar2' 147 | search_sample_ratio = 0.5 # 0.5 148 | max_planner_time = 10 149 | # TODO: max number of samples per iteration flag 150 | # TODO: don't greedily expand samples with too high of a complexity if out of time 151 | 152 | pr = cProfile.Profile() 153 | pr.enable() 154 | saver = WorldSaver() 155 | sim_state = belief.sample_state() 156 | sim_state.assign() 157 | wait_for_duration(0.1) 158 | with LockRenderer(lock=not args.visualize): 159 | # TODO: option to only consider costs during local optimization 160 | # effort_weight = 0 if args.anytime else 1 161 | effort_weight = 1e-3 if args.anytime else 1 162 | #effort_weight = 0 163 | #effort_weight = None 164 | solution = solve_focused(problem, constraints=constraints, stream_info=stream_info, 165 | replan_actions=replan_actions, initial_complexity=5, 166 | planner=planner, max_planner_time=max_planner_time, 167 | unit_costs=args.unit, success_cost=success_cost, 168 | max_time=max_time, max_memory=max_memory, verbose=True, debug=False, 169 | unit_efforts=True, effort_weight=effort_weight, max_effort=INF, 170 | # bind=True, max_skeletons=None, 171 | search_sample_ratio=search_sample_ratio) 172 | saver.restore() 173 | 174 | # print([(s.cost, s.time) for s in SOLUTIONS]) 175 | # print(SOLUTIONS) 176 | print_solution(solution) 177 | pr.disable() 178 | pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime 179 | return solution 180 | 181 | ################################################################################ 182 | 183 | def extract_plan_prefix(plan, replan_actions=set()): 184 | if plan is None: 185 | return None 186 | prefix = [] 187 | for action in plan: 188 | name, args = action 189 | prefix.append(action) 190 | if name in replan_actions: 191 | break 192 | return prefix 193 | 194 | def combine_commands(commands): 195 | combined_commands = [] 196 | for command in commands: 197 | if not combined_commands: 198 | combined_commands.append(command) 199 | continue 200 | prev_command = combined_commands[-1] 201 | if isinstance(prev_command, Trajectory) and isinstance(command, Trajectory) and \ 202 | (prev_command.joints == command.joints): 203 | prev_command.path = (prev_command.path + command.path) 204 | else: 205 | combined_commands.append(command) 206 | return combined_commands 207 | 208 | def commands_from_plan(world, plan): 209 | if plan is None: 210 | return None 211 | # TODO: propagate the state 212 | commands = [] 213 | for action, params in plan: 214 | # TODO: break if the action is a StreamAction 215 | if action in ['move_base', 'move_arm', 'move_gripper', 'pick', 'pull', 'pour', 'press-on', 'press-off']: 216 | commands.extend(params[-1].commands) 217 | elif action == 'detect': 218 | commands.append(params[-1]) 219 | elif action == 'place': 220 | commands.extend(params[-1].reverse().commands) 221 | elif action in ['cook', 'calibrate']: 222 | # TODO: calibrate action that uses fixed_base_suppressor 223 | #steps = int(math.ceil(2.0 / DEFAULT_TIME_STEP)) 224 | steps = 0 225 | commands.append(Wait(world, steps=steps)) 226 | else: 227 | raise NotImplementedError(action) 228 | return commands 229 | #return combine_commands(commands) 230 | -------------------------------------------------------------------------------- /src/policy.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import time 4 | import traceback 5 | 6 | from pybullet_tools.utils import wait_for_duration, wait_for_user, \ 7 | print_separator, INF, elapsed_time 8 | from pddlstream.utils import get_peak_memory_in_kb, str_from_object 9 | from pddlstream.language.constants import Certificate, PDDLProblem 10 | from src.belief import create_observable_belief, transition_belief_update, create_observable_pose_dist 11 | from src.planner import solve_pddlstream, extract_plan_prefix, commands_from_plan 12 | from src.problem import pdddlstream_from_problem, get_streams 13 | from src.replan import get_plan_postfix, make_exact_skeleton, reuse_facts, OBSERVATION_ACTIONS, \ 14 | STOCHASTIC_ACTIONS, make_wild_skeleton 15 | from src.utils import BOWL, DEBUG 16 | 17 | # TODO: max time spent reattempting streams flag (might not be needed actually) 18 | # TODO: process binding blows up for detect_drawer 19 | UNCONSTRAINED_FIXED_BASE = False 20 | MAX_RESTART_TIME = 2*60 21 | REPLAN_ITERATIONS = 1 # 1 | INF 22 | REPLAN_FAILURES = False 23 | 24 | 25 | def random_restart(belief, args, problem, max_time=INF, max_iterations=INF, 26 | max_planner_time=INF, **kwargs): 27 | domain_pddl, constant_map, _, _, init, goal_formula = problem 28 | start_time = time.time() 29 | task = belief.task 30 | world = task.world 31 | iterations = 0 32 | while (elapsed_time(start_time) < max_time) and (iterations < max_iterations): 33 | iterations += 1 34 | try: 35 | stream_pddl, stream_map = get_streams(world, teleport_base=task.teleport_base, 36 | collisions=not args.cfree, teleport=args.teleport) 37 | problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula) 38 | remaining_time = min(max_time - elapsed_time(start_time), max_planner_time) 39 | plan, plan_cost, certificate = solve_pddlstream(belief, problem, args, max_time=remaining_time, **kwargs) 40 | if plan is not None: 41 | return plan, plan_cost, certificate 42 | except KeyboardInterrupt as e: 43 | raise e 44 | except MemoryError as e: 45 | traceback.print_exc() 46 | if not REPLAN_FAILURES: 47 | raise e 48 | break 49 | except Exception as e: 50 | traceback.print_exc() 51 | if not REPLAN_FAILURES: 52 | raise e 53 | # FastDownward translator runs out of memory 54 | return None, INF, Certificate(all_facts=[], preimage_facts=[]) 55 | 56 | def run_policy(task, args, observation_fn, transition_fn, constrain=True, defer=True, # serialize=True, 57 | max_time=10*60, max_constrained_time=1.5*60, max_unconstrained_time=INF): 58 | replan_actions = OBSERVATION_ACTIONS if args.deterministic else STOCHASTIC_ACTIONS 59 | defer_actions = replan_actions if defer else set() 60 | world = task.world 61 | if args.observable: 62 | # TODO: problematic if not observable 63 | belief = create_observable_belief(world) # Fast 64 | else: 65 | belief = task.create_belief() 66 | if BOWL in task.objects: # TODO: hack for bowl 67 | belief.pose_dists[BOWL] = create_observable_pose_dist(world, BOWL) 68 | belief.liquid.update(task.init_liquid) 69 | print('Prior:', belief) 70 | 71 | previous_facts = [] 72 | previous_skeleton = None 73 | total_start_time = time.time() 74 | plan_time = 0 75 | achieved_goal = False 76 | num_iterations = num_constrained = num_unconstrained = num_successes = 0 77 | num_actions = num_commands = total_cost = 0 78 | while elapsed_time(total_start_time) < max_time: 79 | print_separator(n=50) 80 | num_iterations += 1 81 | # TODO: could allow this to be an arbitrary belief transformation 82 | observation = observation_fn(belief) 83 | print('Observation:', observation) 84 | belief.update(observation) 85 | print('Belief:', belief) 86 | if DEBUG: 87 | belief.draw() 88 | 89 | #wait_for_user('Plan?') 90 | fixed_base = UNCONSTRAINED_FIXED_BASE or not task.movable_base or not constrain 91 | plan_start_time = time.time() 92 | plan, plan_cost = None, INF 93 | if constrain and (previous_skeleton is not None): 94 | # TODO: could constrain by comparing to the previous plan cost 95 | num_constrained += 1 96 | print_separator(n=25) 97 | print('Skeleton:', previous_skeleton) 98 | print('Reused facts:', sorted(previous_facts, key=lambda f: f[0])) 99 | problem = pdddlstream_from_problem(belief, additional_init=previous_facts, 100 | collisions=not args.cfree, teleport=args.teleport) 101 | planning_time = min(max_time - elapsed_time(total_start_time), max_constrained_time) #, args.max_time) 102 | plan, plan_cost, certificate = random_restart(belief, args, problem, max_time=planning_time, max_iterations=1, 103 | skeleton=previous_skeleton, replan_actions=defer_actions) 104 | if plan is None: 105 | print('Failed to solve with plan constraints') 106 | #wait_for_user() 107 | #elif not fixed_base: 108 | # num_unconstrained += 1 109 | # problem = pdddlstream_from_problem(belief, additional_init=previous_facts, 110 | # collisions=not args.cfree, teleport=args.teleport) 111 | # print_separator(n=25) 112 | # planning_time = min(max_time - elapsed_time(total_start_time)) # , args.max_time) 113 | # plan, plan_cost, certificate = solve_pddlstream(belief, problem, args, max_time=planning_time, 114 | # replan_actions=defer_actions) 115 | # if plan is None: 116 | # print('Failed to solve when allowing fixed base') 117 | 118 | if plan is None: 119 | # TODO: might be helpful to add additional facts here in the future 120 | num_unconstrained += 1 # additional_init=previous_facts, 121 | problem = pdddlstream_from_problem(belief, fixed_base=fixed_base, 122 | collisions=not args.cfree, teleport=args.teleport) 123 | print_separator(n=25) 124 | planning_time = min(max_time - elapsed_time(total_start_time), max_unconstrained_time) #, args.max_time) 125 | plan, plan_cost, certificate = random_restart(belief, args, problem, max_time=planning_time, 126 | max_planner_time=MAX_RESTART_TIME, 127 | max_iterations=REPLAN_ITERATIONS, 128 | max_cost=plan_cost, replan_actions=defer_actions) 129 | 130 | plan_time += elapsed_time(plan_start_time) 131 | #wait_for_duration(elapsed_time(plan_start_time)) # Mocks the real planning time 132 | if plan is None: 133 | break 134 | #print('Preimage:', sorted(certificate.preimage_facts, key=lambda f: f[0])) 135 | if not plan: 136 | achieved_goal = True 137 | break 138 | print_separator(n=25) 139 | plan_prefix = extract_plan_prefix(plan, replan_actions=replan_actions) 140 | print('Prefix:', plan_prefix) 141 | # sequences = [plan_prefix] 142 | sequences = [[action] for action in plan_prefix] 143 | 144 | success = belief.check_consistent() 145 | for i, sequence in enumerate(sequences): 146 | if not success: 147 | break 148 | print(i, sequence) 149 | num_actions += len(sequence) 150 | commands = commands_from_plan(world, sequence) 151 | num_commands += len(commands) 152 | print('Commands:', commands) 153 | success &= transition_fn(belief, commands) and \ 154 | transition_belief_update(belief, sequence) and \ 155 | belief.check_consistent() 156 | total_cost += sum(command.cost for command in commands) 157 | num_successes += success 158 | 159 | # TODO: store history of stream evaluations 160 | if success and constrain: 161 | plan_postfix = get_plan_postfix(plan, plan_prefix) 162 | # TODO: exit if plan_postfix is empty? 163 | # TODO: make_exact_skeleton still has as bug in it 164 | previous_skeleton = make_wild_skeleton(world, plan_postfix) 165 | #previous_skeleton = make_exact_skeleton(world, plan_postfix) # make_exact_skeleton | make_wild_skeleton 166 | previous_facts = reuse_facts(problem, certificate, previous_skeleton) # [] 167 | else: 168 | previous_skeleton = None 169 | previous_facts = [] 170 | 171 | if achieved_goal: 172 | print('Success!') 173 | else: 174 | print('Failure!') 175 | # TODO: timed out flag 176 | # TODO: store current and peak memory usage 177 | data = { 178 | 'achieved_goal': achieved_goal, 179 | 'total_time': elapsed_time(total_start_time), 180 | 'plan_time': plan_time, 181 | 'num_iterations': num_iterations, 182 | 'num_constrained': num_constrained, 183 | 'num_unconstrained': num_unconstrained, 184 | 'num_successes': num_successes, 185 | 'num_actions': num_actions, 186 | 'num_commands': num_commands, 187 | 'peak_memory': get_peak_memory_in_kb(), 188 | 'total_cost': total_cost, 189 | } 190 | print('Data:', str_from_object(data)) 191 | return data 192 | -------------------------------------------------------------------------------- /src/replan.py: -------------------------------------------------------------------------------- 1 | from itertools import count 2 | 3 | from pddlstream.algorithms.constraints import WILD, ORDER_PREDICATE 4 | from pddlstream.language.constants import Action, EQ, get_prefix, get_args, is_cost, is_parameter 5 | from pddlstream.language.object import OPT_PREFIX 6 | from pddlstream.algorithms.downward import get_fluents 7 | from pddlstream.algorithms.algorithm import parse_domain 8 | from pddlstream.utils import INF, implies, hash_or_id 9 | #from src.utils import FConf 10 | from src.problem import ACTION_COSTS 11 | 12 | 13 | OBSERVATION_ACTIONS = {'detect'} 14 | NOISY_ACTIONS = {'place'} #, 'pull'} # 'calibrate', 'pick']) 15 | STOCHASTIC_ACTIONS = OBSERVATION_ACTIONS | {'move_base'} # | NOISY_ACTIONS 16 | INTERNAL_ACTIONS = {'detect', 'calibrate'} # Fake, skippable, etc... 17 | 18 | REUSE_ARGUMENTS = { 19 | # TODO: this should really be done by type instead 20 | 'pick': [0, 2], # ?o1 ?wp1 ?g ?rp ?o2 ?wp2 21 | 'place': [0, 2, 3, 4], # ?o1 ?wp1 ?g ?rp ?o2 ?wp2 22 | # TODO: need to maintain the original distribution as well then... 23 | 'detect': [0, 3, 6], # ?o1 ?wp1 ?rp1 ?obs ?wp2 ?rp2 ?o0 ?wp0 24 | # The previous pick error I had was because it moved to the carry_aq and then detected 25 | # However, it was next constrained to move the base rather than the arm 26 | } 27 | 28 | # TODO: could keep around previous base plans as long as we don't reuse them 29 | # Don't need to replan safe plans form teh same location 30 | # My worry is that the ground plane will shift 31 | 32 | ################################################################################ 33 | 34 | def is_optimistic(arg): 35 | return isinstance(arg, str) and arg.startswith(OPT_PREFIX) 36 | 37 | def test_reusable(world, name, index, arg): 38 | if is_optimistic(arg): 39 | return False 40 | indices = REUSE_ARGUMENTS.get(name, []) 41 | return (index in indices) or isinstance(arg, str) # or (isinstance(arg, FConf) and (arg in world.constants)) 42 | 43 | def make_wild_skeleton(world, plan): 44 | # Can always constrain grasps and selected poses 45 | # Could store previous values to suggest new ones 46 | # Recover skeleton 47 | # Save parameters that shouldn't change 48 | # And keep their evaluations 49 | # If all args the same 50 | skeleton = [] 51 | for name, args in plan: 52 | new_args = [arg if test_reusable(world, name, index, arg) else WILD 53 | for index, arg in enumerate(args)] 54 | skeleton.append(Action(name, new_args)) 55 | #print(len(skeleton), skeleton[-1]) 56 | return skeleton 57 | 58 | def make_exact_skeleton(world, plan): 59 | # TODO: spend more effort on samples that were previously discovered 60 | # TODO: possibly reuse the kinematic solutions as seeds 61 | #arg_from_id = {} 62 | var_from_id = {} 63 | count_from_prefix = {} 64 | skeleton = [] 65 | for name, args in plan: 66 | new_args = [] 67 | for idx, arg in enumerate(args): 68 | #arg_from_id[id(arg)] = arg 69 | if test_reusable(world, name, idx, arg): 70 | new_args.append(arg) 71 | else: 72 | key = id(arg) 73 | if key not in var_from_id: 74 | if is_optimistic(arg): 75 | var_from_id[key] = '?{}'.format(arg[len(OPT_PREFIX):]) # WILD 76 | else: 77 | prefix = str(arg)[:2].lower() # 'w' 78 | num = next(count_from_prefix.setdefault(prefix, count())) 79 | var_from_id[key] = '?{}{}'.format(prefix, num) 80 | new_args.append(var_from_id[key]) 81 | skeleton.append(Action(name, new_args)) 82 | #print(skeleton[-1]) 83 | #for i, var in sorted(var_from_id.items(), key=lambda pair: pair[-1]): 84 | # print(arg_from_id[i], var) 85 | # TODO: could fall back on wild if this fails 86 | # TODO: this fails for placing (due to carry_conf / rest_conf disagreement) 87 | return skeleton 88 | 89 | ################################################################################ 90 | 91 | def reuse_facts(problem, certificate, skeleton): 92 | # TODO: repackage streams 93 | # TODO: recover the full axiom + action plan 94 | # TODO: recover the plan preimage annotated with use time 95 | # Some supporting args are quantified out and thus lack some facts 96 | new_facts = [] 97 | if skeleton is None: 98 | return new_facts 99 | reuse_objs = set() 100 | for action, args in skeleton: 101 | for arg in args: 102 | if (arg != WILD) and not is_parameter(arg): 103 | reuse_objs.add(hash_or_id(arg)) 104 | 105 | # The reuse relpose omission is due to the fact that the initial pose was selected 106 | # (which is populated in the initial state) 107 | order_predicate = ORDER_PREDICATE.format('') 108 | domain = parse_domain(problem.domain_pddl) 109 | fluents = get_fluents(domain) 110 | for fact in certificate.preimage_facts: 111 | predicate = get_prefix(fact) 112 | if (predicate in {order_predicate, EQ}) or (predicate in fluents): 113 | # Could technically evaluate functions as well 114 | continue 115 | if all(isinstance(arg, str) or (hash_or_id(arg) in reuse_objs) 116 | for arg in get_args(fact)): 117 | new_facts.append(fact) 118 | return new_facts 119 | 120 | ################################################################################ 121 | 122 | def compute_plan_cost(plan): 123 | if plan is None: 124 | return INF 125 | cost = 0 126 | for name, args in plan: 127 | cost += ACTION_COSTS[name] 128 | return cost 129 | 130 | 131 | def get_plan_postfix(plan, plan_prefix): 132 | return [action for action in plan[len(plan_prefix):] 133 | if isinstance(action, Action)] 134 | -------------------------------------------------------------------------------- /src/streams/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/caelan/SS-Replan/da4ae12fe8e345ba636aff4cc787f9e35c4d12e7/src/streams/__init__.py -------------------------------------------------------------------------------- /src/streams/move.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from pybullet_tools.utils import BodySaver, plan_nonholonomic_motion, set_renderer, wait_for_user, plan_joint_motion, \ 4 | get_extend_fn, child_link_from_joint 5 | from src.command import Sequence, State, Trajectory 6 | from src.inference import SurfaceDist 7 | from src.stream import ARM_RESOLUTION, SELF_COLLISIONS, GRIPPER_RESOLUTION 8 | from src.utils import get_link_obstacles, FConf, get_descendant_obstacles 9 | 10 | PAUSE_MOTION_FAILURES = False 11 | 12 | def parse_fluents(world, fluents): 13 | obstacles = set() 14 | for fluent in fluents: 15 | predicate, args = fluent[0], fluent[1:] 16 | if predicate in {p.lower() for p in ['AtBConf', 'AtAConf', 'AtGConf']}: 17 | q, = args 18 | q.assign() 19 | elif predicate == 'AtAngle'.lower(): 20 | j, a = args 21 | a.assign() 22 | link = child_link_from_joint(a.joints[0]) 23 | obstacles.update(get_descendant_obstacles(a.body, link)) 24 | elif predicate in 'AtWorldPose'.lower(): 25 | # TODO: conditional effects are not being correctly updated in pddlstream 26 | #b, p = args 27 | #if isinstance(p, SurfaceDist): 28 | # continue 29 | #p.assign() 30 | #obstacles.update(get_link_obstacles(world, b)) 31 | raise RuntimeError() 32 | elif predicate in 'AtRelPose'.lower(): 33 | pass 34 | elif predicate == 'AtGrasp'.lower(): 35 | pass 36 | else: 37 | raise NotImplementedError(predicate) 38 | 39 | attachments = [] 40 | for fluent in fluents: 41 | predicate, args = fluent[0], fluent[1:] 42 | if predicate in {p.lower() for p in ['AtBConf', 'AtAConf', 'AtGConf']}: 43 | pass 44 | elif predicate == 'AtAngle'.lower(): 45 | pass 46 | elif predicate in 'AtWorldPose'.lower(): 47 | raise RuntimeError() 48 | elif predicate in 'AtRelPose'.lower(): 49 | o1, rp, o2 = args 50 | if isinstance(rp, SurfaceDist): 51 | continue 52 | rp.assign() 53 | obstacles.update(get_link_obstacles(world, o1)) 54 | elif predicate == 'AtGrasp'.lower(): 55 | o, g = args 56 | if o is not None: 57 | attachments.append(g.get_attachment()) 58 | attachments[-1].assign() 59 | else: 60 | raise NotImplementedError(predicate) 61 | return attachments, obstacles 62 | 63 | ################################################################################ 64 | 65 | # TODO: more efficient collision checking 66 | 67 | def get_base_motion_fn(world, teleport_base=False, collisions=True, teleport=False, 68 | restarts=4, iterations=75, smooth=100): 69 | # TODO: lazy planning on a common base roadmap 70 | 71 | def fn(bq1, bq2, aq, fluents=[]): 72 | #if bq1 == bq2: 73 | # return None 74 | aq.assign() 75 | attachments, obstacles = parse_fluents(world, fluents) 76 | obstacles.update(world.static_obstacles) 77 | if not collisions: 78 | obstacles = set() 79 | 80 | start_path, end_path = [], [] 81 | if hasattr(bq1, 'nearby_bq'): 82 | bq1.assign() 83 | start_path = plan_nonholonomic_motion(world.robot, bq2.joints, bq1.nearby_bq.values, attachments=attachments, 84 | obstacles=obstacles, custom_limits=world.custom_limits, 85 | reversible=True, self_collisions=False, restarts=-1) 86 | if start_path is None: 87 | print('Failed to find nearby base conf!') 88 | return 89 | bq1 = bq1.nearby_bq 90 | if hasattr(bq2, 'nearby_bq'): 91 | bq2.nearby_bq.assign() 92 | end_path = plan_nonholonomic_motion(world.robot, bq2.joints, bq2.values, attachments=attachments, 93 | obstacles=obstacles, custom_limits=world.custom_limits, 94 | reversible=True, self_collisions=False, restarts=-1) 95 | if end_path is None: 96 | print('Failed to find nearby base conf!') 97 | return 98 | bq2 = bq2.nearby_bq 99 | 100 | bq1.assign() 101 | robot_saver = BodySaver(world.robot) 102 | if (bq1 == bq2) or teleport_base or teleport: 103 | path = [bq1.values, bq2.values] 104 | else: 105 | # It's important that the extend function is reversible to avoid getting trapped 106 | path = plan_nonholonomic_motion(world.robot, bq2.joints, bq2.values, attachments=attachments, 107 | obstacles=obstacles, custom_limits=world.custom_limits, 108 | reversible=True, self_collisions=False, 109 | restarts=restarts, iterations=iterations, smooth=smooth) 110 | if path is None: 111 | print('Failed to find an arm motion plan for {}->{}'.format(bq1, bq2)) 112 | if PAUSE_MOTION_FAILURES: 113 | set_renderer(enable=True) 114 | print(fluents) 115 | for bq in [bq1, bq2]: 116 | bq.assign() 117 | wait_for_user() 118 | set_renderer(enable=False) 119 | return None 120 | 121 | # TODO: could actually plan with all joints as long as we return to the same config 122 | cmd = Sequence(State(world, savers=[robot_saver]), commands=[ 123 | Trajectory(world, world.robot, world.base_joints, path) 124 | for path in [start_path, path, end_path] if path], name='base') 125 | return (cmd,) 126 | return fn 127 | 128 | 129 | def get_reachability_test(world, **kwargs): 130 | base_motion_fn = get_base_motion_fn(world, restarts=2, iterations=50, smooth=0, **kwargs) 131 | bq0 = FConf(world.robot, world.base_joints) 132 | # TODO: can check for arm motions as well 133 | 134 | def test(bq): 135 | aq = world.carry_conf 136 | outputs = base_motion_fn(aq, bq0, bq, fluents=[]) 137 | return outputs is not None 138 | return test 139 | 140 | 141 | def get_arm_motion_gen(world, collisions=True, teleport=False): 142 | resolutions = ARM_RESOLUTION * np.ones(len(world.arm_joints)) 143 | 144 | def fn(bq, aq1, aq2, fluents=[]): 145 | #if aq1 == aq2: 146 | # return None 147 | bq.assign() 148 | aq1.assign() 149 | attachments, obstacles = parse_fluents(world, fluents) 150 | obstacles.update(world.static_obstacles) 151 | if not collisions: 152 | obstacles = set() 153 | robot_saver = BodySaver(world.robot) 154 | if teleport: 155 | path = [aq1.values, aq2.values] 156 | else: 157 | path = plan_joint_motion(world.robot, aq2.joints, aq2.values, 158 | attachments=attachments, obstacles=obstacles, 159 | self_collisions=SELF_COLLISIONS, 160 | disabled_collisions=world.disabled_collisions, 161 | custom_limits=world.custom_limits, resolutions=resolutions, 162 | restarts=2, iterations=50, smooth=50) 163 | if path is None: 164 | print('Failed to find an arm motion plan for {}->{}'.format(aq1, aq2)) 165 | if PAUSE_MOTION_FAILURES: 166 | set_renderer(enable=True) 167 | print(fluents) 168 | for bq in [aq1, aq2]: 169 | bq.assign() 170 | wait_for_user() 171 | set_renderer(enable=False) 172 | return None 173 | cmd = Sequence(State(world, savers=[robot_saver]), commands=[ 174 | Trajectory(world, world.robot, world.arm_joints, path), 175 | ], name='arm') 176 | return (cmd,) 177 | return fn 178 | 179 | 180 | def get_gripper_motion_gen(world, teleport=False, **kwargs): 181 | resolutions = GRIPPER_RESOLUTION * np.ones(len(world.gripper_joints)) 182 | 183 | def fn(gq1, gq2): 184 | #if gq1 == gq2: 185 | # return None 186 | if teleport: 187 | path = [gq1.values, gq2.values] 188 | else: 189 | extend_fn = get_extend_fn(gq2.body, gq2.joints, resolutions=resolutions) 190 | path = [gq1.values] + list(extend_fn(gq1.values, gq2.values)) 191 | cmd = Sequence(State(world), commands=[ 192 | Trajectory(world, gq2.body, gq2.joints, path), 193 | ], name='gripper') 194 | return (cmd,) 195 | return fn 196 | -------------------------------------------------------------------------------- /src/streams/pick.py: -------------------------------------------------------------------------------- 1 | import random 2 | from itertools import cycle 3 | 4 | from pybullet_tools.utils import BodySaver, get_sample_fn, set_joint_positions, multiply, invert, get_moving_links, \ 5 | pairwise_collision, uniform_pose_generator, get_movable_joints, wait_for_user, INF 6 | from src.command import Sequence, State, ApproachTrajectory, Detach, AttachGripper 7 | from src.database import load_place_base_poses 8 | from src.stream import PRINT_FAILURES, plan_approach, MOVE_ARM, P_RANDOMIZE_IK, inverse_reachability, FIXED_FAILURES 9 | from src.streams.move import get_gripper_motion_gen 10 | from src.utils import FConf, create_surface_attachment, get_surface_obstacles, iterate_approach_path 11 | 12 | def is_approach_safe(world, obj_name, pose, grasp, obstacles): 13 | assert pose.support is not None 14 | obj_body = world.get_body(obj_name) 15 | pose.assign() # May set the drawer confs as well 16 | set_joint_positions(world.gripper, get_movable_joints(world.gripper), world.open_gq.values) 17 | #set_renderer(enable=True) 18 | for _ in iterate_approach_path(world, pose, grasp, body=obj_body): 19 | #for link in get_all_links(world.gripper): 20 | # set_color(world.gripper, apply_alpha(np.zeros(3)), link) 21 | #wait_for_user() 22 | if any(pairwise_collision(world.gripper, obst) # or pairwise_collision(obj_body, obst) 23 | for obst in obstacles): 24 | print('Unsafe approach!') 25 | #wait_for_user() 26 | return False 27 | return True 28 | 29 | def plan_pick(world, obj_name, pose, grasp, base_conf, obstacles, randomize=True, **kwargs): 30 | # TODO: check if within database convex hull 31 | # TODO: flag to check if initially in collision 32 | 33 | obj_body = world.get_body(obj_name) 34 | pose.assign() 35 | base_conf.assign() 36 | world.open_gripper() 37 | robot_saver = BodySaver(world.robot) 38 | obj_saver = BodySaver(obj_body) 39 | 40 | if randomize: 41 | sample_fn = get_sample_fn(world.robot, world.arm_joints) 42 | set_joint_positions(world.robot, world.arm_joints, sample_fn()) 43 | else: 44 | world.carry_conf.assign() 45 | world_from_body = pose.get_world_from_body() 46 | gripper_pose = multiply(world_from_body, invert(grasp.grasp_pose)) # w_f_g = w_f_o * (g_f_o)^-1 47 | full_grasp_conf = world.solve_inverse_kinematics(gripper_pose) 48 | if full_grasp_conf is None: 49 | if PRINT_FAILURES: print('Grasp kinematic failure') 50 | return 51 | moving_links = get_moving_links(world.robot, world.arm_joints) 52 | robot_obstacle = (world.robot, frozenset(moving_links)) 53 | #robot_obstacle = get_descendant_obstacles(world.robot, child_link_from_joint(world.arm_joints[0])) 54 | #robot_obstacle = world.robot 55 | if any(pairwise_collision(robot_obstacle, b) for b in obstacles): 56 | if PRINT_FAILURES: print('Grasp collision failure') 57 | #set_renderer(enable=True) 58 | #wait_for_user() 59 | #set_renderer(enable=False) 60 | return 61 | approach_pose = multiply(world_from_body, invert(grasp.pregrasp_pose)) 62 | approach_path = plan_approach(world, approach_pose, # attachments=[grasp.get_attachment()], 63 | obstacles=obstacles, **kwargs) 64 | if approach_path is None: 65 | if PRINT_FAILURES: print('Approach plan failure') 66 | return 67 | if MOVE_ARM: 68 | aq = FConf(world.robot, world.arm_joints, approach_path[0]) 69 | else: 70 | aq = world.carry_conf 71 | 72 | gripper_motion_fn = get_gripper_motion_gen(world, **kwargs) 73 | finger_cmd, = gripper_motion_fn(world.open_gq, grasp.get_gripper_conf()) 74 | attachment = create_surface_attachment(world, obj_name, pose.support) 75 | objects = [obj_name] 76 | cmd = Sequence(State(world, savers=[robot_saver, obj_saver], 77 | attachments=[attachment]), commands=[ 78 | ApproachTrajectory(objects, world, world.robot, world.arm_joints, approach_path), 79 | finger_cmd.commands[0], 80 | Detach(world, attachment.parent, attachment.parent_link, attachment.child), 81 | AttachGripper(world, obj_body, grasp=grasp), 82 | ApproachTrajectory(objects, world, world.robot, world.arm_joints, reversed(approach_path)), 83 | ], name='pick') 84 | yield (aq, cmd,) 85 | 86 | ################################################################################ 87 | 88 | def get_fixed_pick_gen_fn(world, max_attempts=25, collisions=True, **kwargs): 89 | 90 | def gen(obj_name, pose, grasp, base_conf): 91 | obstacles = world.static_obstacles | get_surface_obstacles(world, pose.support) # | {obj_body} 92 | #if not collisions: 93 | # obstacles = set() 94 | if not is_approach_safe(world, obj_name, pose, grasp, obstacles): 95 | return 96 | # TODO: increase timeouts if a previously successful value 97 | # TODO: seed IK using the previous solution 98 | max_failures = FIXED_FAILURES if world.task.movable_base else INF 99 | failures = 0 100 | while failures <= max_failures: 101 | for i in range(max_attempts): 102 | randomize = (random.random() < P_RANDOMIZE_IK) 103 | ik_outputs = next(plan_pick(world, obj_name, pose, grasp, base_conf, obstacles, 104 | randomize=randomize, **kwargs), None) 105 | if ik_outputs is not None: 106 | print('Fixed pick succeeded after {} attempts'.format(i)) 107 | yield ik_outputs 108 | break # return 109 | else: 110 | if PRINT_FAILURES: print('Fixed pick failure after {} attempts'.format(max_attempts)) 111 | #if not pose.init: 112 | # break 113 | yield None 114 | failures += 1 115 | return gen 116 | 117 | 118 | def get_pick_gen_fn(world, max_attempts=25, collisions=True, learned=True, **kwargs): 119 | # TODO: sample in the neighborhood of the base conf to ensure robust 120 | 121 | def gen(obj_name, pose, grasp, *args): 122 | obstacles = world.static_obstacles | get_surface_obstacles(world, pose.support) 123 | #if not collisions: 124 | # obstacles = set() 125 | if not is_approach_safe(world, obj_name, pose, grasp, obstacles): 126 | return 127 | 128 | # TODO: check collisions with obj at pose 129 | gripper_pose = multiply(pose.get_world_from_body(), invert(grasp.grasp_pose)) # w_f_g = w_f_o * (g_f_o)^-1 130 | if learned: 131 | base_generator = cycle(load_place_base_poses(world, gripper_pose, pose.support, grasp.grasp_type)) 132 | else: 133 | base_generator = uniform_pose_generator(world.robot, gripper_pose) 134 | safe_base_generator = inverse_reachability(world, base_generator, obstacles=obstacles, **kwargs) 135 | while True: 136 | for i in range(max_attempts): 137 | try: 138 | base_conf = next(safe_base_generator) 139 | except StopIteration: 140 | return 141 | if base_conf is None: 142 | yield None 143 | continue # TODO: could break if not pose.init 144 | randomize = (random.random() < P_RANDOMIZE_IK) 145 | ik_outputs = next(plan_pick(world, obj_name, pose, grasp, base_conf, obstacles, 146 | randomize=randomize, **kwargs), None) 147 | if ik_outputs is not None: 148 | print('Pick succeeded after {} attempts'.format(i)) 149 | yield (base_conf,) + ik_outputs 150 | break 151 | else: 152 | if PRINT_FAILURES: print('Pick failure after {} attempts'.format(max_attempts)) 153 | #if not pose.init: # Might be an intended placement blocked by a drawer 154 | # break 155 | yield None 156 | return gen 157 | -------------------------------------------------------------------------------- /src/streams/pour.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import random 3 | 4 | from itertools import cycle 5 | 6 | from pybullet_tools.utils import approximate_as_cylinder, approximate_as_prism, \ 7 | multiply, invert, BodySaver, Euler, set_pose, wait_for_user, \ 8 | Point, Pose, uniform_pose_generator 9 | from pybullet_tools.pr2_utils import get_top_grasps 10 | from src.database import load_place_base_poses, load_inverse_placements, project_base_pose, load_pour_base_poses 11 | from src.stream import plan_approach, MOVE_ARM, inverse_reachability, P_RANDOMIZE_IK, PRINT_FAILURES 12 | from src.command import Sequence, ApproachTrajectory, State, Wait 13 | from src.stream import MOVE_ARM, plan_workspace 14 | from src.utils import FConf, type_from_name, MUSTARD, TOP_GRASP, TOOL_POSE, set_tool_pose 15 | 16 | Z_OFFSET = 0.03 17 | 18 | def pour_path_from_parameter(world, bowl_name, cup_name): 19 | bowl_body = world.get_body(bowl_name) 20 | bowl_center, (bowl_d, bowl_h) = approximate_as_cylinder(bowl_body) 21 | cup_body = world.get_body(cup_name) 22 | cup_center, (cup_d, _, cup_h) = approximate_as_prism(cup_body) 23 | 24 | ##### 25 | 26 | obj_type = type_from_name(cup_name) 27 | if obj_type in [MUSTARD]: 28 | initial_pitch = final_pitch = -np.pi 29 | radius = 0 30 | else: 31 | initial_pitch = 0 # different if mustard 32 | final_pitch = -3 * np.pi / 4 33 | radius = bowl_d / 2 34 | 35 | #axis_in_cup_center_x = -0.05 36 | axis_in_cup_center_x = 0 # meters 37 | #axis_in_cup_center_z = -cup_h/2. 38 | axis_in_cup_center_z = 0. # meters 39 | #axis_in_cup_center_z = +cup_h/2. 40 | 41 | # tl := top left | tr := top right 42 | cup_tl_in_center = np.array([-cup_d/2, 0, cup_h/2]) 43 | cup_tl_in_axis = cup_tl_in_center - Point(z=axis_in_cup_center_z) 44 | cup_tl_angle = np.math.atan2(cup_tl_in_axis[2], cup_tl_in_axis[0]) 45 | cup_tl_pour_pitch = final_pitch - cup_tl_angle 46 | 47 | cup_radius2d = np.linalg.norm([cup_tl_in_axis]) 48 | pivot_in_bowl_tr = Point( 49 | x=-(cup_radius2d * np.math.cos(cup_tl_pour_pitch) + 0.01), 50 | z=(cup_radius2d * np.math.sin(cup_tl_pour_pitch) + Z_OFFSET)) 51 | 52 | pivot_in_bowl_center = Point(x=radius, z=bowl_h / 2) + pivot_in_bowl_tr 53 | base_from_pivot = Pose(Point(x=axis_in_cup_center_x, z=axis_in_cup_center_z)) 54 | 55 | ##### 56 | 57 | assert -np.pi <= final_pitch <= initial_pitch 58 | pitches = [initial_pitch] 59 | if final_pitch != initial_pitch: 60 | pitches = list(np.arange(final_pitch, initial_pitch, np.pi/16)) + pitches 61 | cup_path_in_bowl = [] 62 | for pitch in pitches: 63 | rotate_pivot = Pose(euler=Euler(pitch=pitch)) # Can also interpolate directly between start and end quat 64 | cup_path_in_bowl.append(multiply(Pose(point=bowl_center), Pose(pivot_in_bowl_center), 65 | rotate_pivot, invert(base_from_pivot), 66 | invert(Pose(point=cup_center)))) 67 | return cup_path_in_bowl 68 | 69 | def visualize_cartesian_path(body, pose_path): 70 | for i, pose in enumerate(pose_path): 71 | set_pose(body, pose) 72 | print('{}/{}) continue?'.format(i, len(pose_path))) 73 | wait_for_user() 74 | #handles = draw_pose(get_pose(body)) 75 | #handles.extend(draw_aabb(get_aabb(body))) 76 | #print('Finish?') 77 | #wait_for_user() 78 | #for h in handles: 79 | # remove_debug(h) 80 | 81 | def get_fixed_pour_gen_fn(world, max_attempts=50, collisions=True, teleport=False, **kwargs): 82 | def gen(bowl_name, wp, cup_name, grasp, bq): 83 | # https://github.mit.edu/Learning-and-Intelligent-Systems/ltamp_pr2/blob/d1e6024c5c13df7edeab3a271b745e656a794b02/plan_tools/samplers/pour.py 84 | if bowl_name == cup_name: 85 | return 86 | #attachment = get_grasp_attachment(world, arm, grasp) 87 | bowl_body = world.get_body(bowl_name) 88 | #cup_body = world.get_body(cup_name) 89 | obstacles = (world.static_obstacles | {bowl_body}) if collisions else set() 90 | cup_path_bowl = pour_path_from_parameter(world, bowl_name, cup_name) 91 | while True: 92 | for _ in range(max_attempts): 93 | bowl_pose = wp.get_world_from_body() 94 | rotate_bowl = Pose(euler=Euler(yaw=random.uniform(-np.pi, np.pi))) 95 | rotate_cup = Pose(euler=Euler(yaw=random.uniform(-np.pi, np.pi))) 96 | cup_path = [multiply(bowl_pose, invert(rotate_bowl), cup_pose_bowl, rotate_cup) 97 | for cup_pose_bowl in cup_path_bowl] 98 | #visualize_cartesian_path(cup_body, cup_path) 99 | #if cartesian_path_collision(cup_body, cup_path, obstacles + [bowl_body]): 100 | # continue 101 | tool_path = [multiply(p, invert(grasp.grasp_pose)) for p in cup_path] 102 | # TODO: extra collision test for visibility 103 | # TODO: orientation constraint while moving 104 | 105 | bq.assign() 106 | grasp.set_gripper() 107 | world.carry_conf.assign() 108 | arm_path = plan_workspace(world, tool_path, obstacles, randomize=True) # tilt to upright 109 | if arm_path is None: 110 | continue 111 | assert MOVE_ARM 112 | aq = FConf(world.robot, world.arm_joints, arm_path[-1]) 113 | robot_saver = BodySaver(world.robot) 114 | 115 | obj_type = type_from_name(cup_name) 116 | duration = 5.0 if obj_type in [MUSTARD] else 1.0 117 | objects = [bowl_name, cup_name] 118 | cmd = Sequence(State(world, savers=[robot_saver]), commands=[ 119 | ApproachTrajectory(objects, world, world.robot, world.arm_joints, arm_path[::-1]), 120 | Wait(world, duration=duration), 121 | ApproachTrajectory(objects, world, world.robot, world.arm_joints, arm_path), 122 | ], name='pour') 123 | yield (aq, cmd,) 124 | break 125 | else: 126 | yield None 127 | return gen 128 | 129 | 130 | def get_pour_gen_fn(world, max_attempts=50, learned=True, **kwargs): 131 | ik_gen = get_fixed_pour_gen_fn(world, max_attempts=1, **kwargs) 132 | 133 | def gen(bowl_name, pose, cup_name, grasp): 134 | if bowl_name == cup_name: 135 | return 136 | obstacles = world.static_obstacles 137 | bowl_body = world.get_body(bowl_name) 138 | bowl_pose = pose.get_world_from_body() 139 | if learned: 140 | # TODO: do this properly 141 | #grasp_pose = next(iter(get_top_grasps(bowl_body, tool_pose=TOOL_POSE, grasp_length=-Z_OFFSET, max_width=np.inf))) 142 | #gripper_pose = multiply(bowl_pose, invert(grasp_pose)) # w_f_g = w_f_o * (g_f_o)^-1 143 | #set_tool_pose(world, gripper_pose) 144 | #base_generator = cycle(load_place_base_poses(world, gripper_pose, pose.support, TOP_GRASP)) 145 | base_generator = cycle(load_pour_base_poses(world, pose.support)) 146 | else: 147 | base_generator = uniform_pose_generator(world.robot, bowl_pose) 148 | safe_base_generator = inverse_reachability(world, base_generator, obstacles=obstacles, **kwargs) 149 | while True: 150 | for i in range(max_attempts): 151 | try: 152 | base_conf = next(safe_base_generator) 153 | except StopIteration: 154 | return 155 | if base_conf is None: 156 | yield None 157 | continue 158 | #randomize = (random.random() < P_RANDOMIZE_IK) 159 | ik_outputs = next(ik_gen(bowl_name, pose, cup_name, grasp, base_conf), None) 160 | if ik_outputs is not None: 161 | print('Pour succeeded after {} attempts'.format(i)) 162 | yield (base_conf,) + ik_outputs 163 | break 164 | else: 165 | if PRINT_FAILURES: print('Pour failure after {} attempts'.format(max_attempts)) 166 | #if not pose.init: 167 | # break 168 | yield None 169 | return gen 170 | -------------------------------------------------------------------------------- /src/streams/press.py: -------------------------------------------------------------------------------- 1 | import random 2 | from itertools import cycle 3 | 4 | from pybullet_tools.pr2_utils import get_top_presses 5 | from pybullet_tools.utils import BodySaver, get_sample_fn, set_joint_positions, multiply, invert, get_moving_links, \ 6 | pairwise_collision, link_from_name, get_unit_vector, unit_point, Pose, get_link_pose, \ 7 | uniform_pose_generator, INF 8 | from src.command import Sequence, State, ApproachTrajectory, Wait 9 | from src.stream import plan_approach, MOVE_ARM, inverse_reachability, P_RANDOMIZE_IK, PRINT_FAILURES, FIXED_FAILURES 10 | from src.utils import FConf, APPROACH_DISTANCE, TOOL_POSE, FINGER_EXTENT, Grasp, TOP_GRASP 11 | from src.database import load_pull_base_poses 12 | 13 | def get_grasp_presses(world, knob, pre_distance=APPROACH_DISTANCE): 14 | knob_link = link_from_name(world.kitchen, knob) 15 | pre_direction = pre_distance * get_unit_vector([0, 0, 1]) 16 | post_direction = unit_point() 17 | for i, grasp_pose in enumerate(get_top_presses(world.kitchen, link=knob_link, 18 | tool_pose=TOOL_POSE, top_offset=FINGER_EXTENT[0]/2 + 5e-3)): 19 | pregrasp_pose = multiply(Pose(point=pre_direction), grasp_pose, 20 | Pose(point=post_direction)) 21 | grasp = Grasp(world, knob, TOP_GRASP, i, grasp_pose, pregrasp_pose) 22 | yield grasp 23 | 24 | def plan_press(world, knob_name, pose, grasp, base_conf, obstacles, randomize=True, **kwargs): 25 | base_conf.assign() 26 | world.close_gripper() 27 | robot_saver = BodySaver(world.robot) 28 | 29 | if randomize: 30 | sample_fn = get_sample_fn(world.robot, world.arm_joints) 31 | set_joint_positions(world.robot, world.arm_joints, sample_fn()) 32 | else: 33 | world.carry_conf.assign() 34 | gripper_pose = multiply(pose, invert(grasp.grasp_pose)) # w_f_g = w_f_o * (g_f_o)^-1 35 | #set_joint_positions(world.gripper, get_movable_joints(world.gripper), world.closed_gq.values) 36 | #set_tool_pose(world, gripper_pose) 37 | full_grasp_conf = world.solve_inverse_kinematics(gripper_pose) 38 | #wait_for_user() 39 | if full_grasp_conf is None: 40 | # if PRINT_FAILURES: print('Grasp kinematic failure') 41 | return 42 | robot_obstacle = (world.robot, frozenset(get_moving_links(world.robot, world.arm_joints))) 43 | if any(pairwise_collision(robot_obstacle, b) for b in obstacles): 44 | #if PRINT_FAILURES: print('Grasp collision failure') 45 | return 46 | approach_pose = multiply(pose, invert(grasp.pregrasp_pose)) 47 | approach_path = plan_approach(world, approach_pose, obstacles=obstacles, **kwargs) 48 | if approach_path is None: 49 | return 50 | aq = FConf(world.robot, world.arm_joints, approach_path[0]) if MOVE_ARM else world.carry_conf 51 | 52 | #gripper_motion_fn = get_gripper_motion_gen(world, **kwargs) 53 | #finger_cmd, = gripper_motion_fn(world.open_gq, world.closed_gq) 54 | objects = [] 55 | cmd = Sequence(State(world, savers=[robot_saver]), commands=[ 56 | #finger_cmd.commands[0], 57 | ApproachTrajectory(objects, world, world.robot, world.arm_joints, approach_path), 58 | ApproachTrajectory(objects, world, world.robot, world.arm_joints, reversed(approach_path)), 59 | #finger_cmd.commands[0].reverse(), 60 | Wait(world, duration=1.0), 61 | ], name='press') 62 | yield (aq, cmd,) 63 | 64 | ################################################################################ 65 | 66 | def get_fixed_press_gen_fn(world, max_attempts=25, collisions=True, teleport=False, **kwargs): 67 | 68 | def gen(knob_name, base_conf): 69 | knob_link = link_from_name(world.kitchen, knob_name) 70 | pose = get_link_pose(world.kitchen, knob_link) 71 | presses = cycle(get_grasp_presses(world, knob_name)) 72 | max_failures = FIXED_FAILURES if world.task.movable_base else INF 73 | failures = 0 74 | while failures <= max_failures: 75 | for i in range(max_attempts): 76 | grasp = next(presses) 77 | randomize = (random.random() < P_RANDOMIZE_IK) 78 | ik_outputs = next(plan_press(world, knob_name, pose, grasp, base_conf, world.static_obstacles, 79 | randomize=randomize, **kwargs), None) 80 | if ik_outputs is not None: 81 | print('Fixed press succeeded after {} attempts'.format(i)) 82 | yield ik_outputs 83 | break # return 84 | else: 85 | if PRINT_FAILURES: print('Fixed pull failure after {} attempts'.format(max_attempts)) 86 | yield None 87 | max_failures += 1 88 | return gen 89 | 90 | def get_press_gen_fn(world, max_attempts=50, collisions=True, teleport=False, learned=True, **kwargs): 91 | def gen(knob_name): 92 | obstacles = world.static_obstacles 93 | knob_link = link_from_name(world.kitchen, knob_name) 94 | pose = get_link_pose(world.kitchen, knob_link) 95 | #pose = RelPose(world.kitchen, knob_link, init=True) 96 | presses = cycle(get_grasp_presses(world, knob_name)) 97 | grasp = next(presses) 98 | gripper_pose = multiply(pose, invert(grasp.grasp_pose)) # w_f_g = w_f_o * (g_f_o)^-1 99 | if learned: 100 | base_generator = cycle(load_pull_base_poses(world, knob_name)) 101 | else: 102 | base_generator = uniform_pose_generator(world.robot, gripper_pose) 103 | safe_base_generator = inverse_reachability(world, base_generator, obstacles=obstacles, **kwargs) 104 | while True: 105 | for i in range(max_attempts): 106 | try: 107 | base_conf = next(safe_base_generator) 108 | except StopIteration: 109 | return 110 | if base_conf is None: 111 | yield None 112 | continue 113 | grasp = next(presses) 114 | randomize = (random.random() < P_RANDOMIZE_IK) 115 | ik_outputs = next(plan_press(world, knob_name, pose, grasp, base_conf, obstacles, 116 | randomize=randomize, **kwargs), None) 117 | if ik_outputs is not None: 118 | print('Press succeeded after {} attempts'.format(i)) 119 | yield (base_conf,) + ik_outputs 120 | break 121 | else: 122 | if PRINT_FAILURES: print('Press failure after {} attempts'.format(max_attempts)) 123 | #if not pose.init: 124 | # break 125 | yield None 126 | return gen 127 | -------------------------------------------------------------------------------- /src/streams/pull.py: -------------------------------------------------------------------------------- 1 | import random 2 | from itertools import cycle 3 | 4 | from pybullet_tools.pr2_utils import close_until_collision 5 | from pybullet_tools.utils import multiply, joint_from_name, set_joint_positions, invert, \ 6 | pairwise_collision, BodySaver, uniform_pose_generator, INF 7 | from src.command import ApproachTrajectory, DoorTrajectory, Sequence, State 8 | from src.database import load_pull_base_poses 9 | from src.stream import PRINT_FAILURES, plan_workspace, plan_approach, MOVE_ARM, \ 10 | P_RANDOMIZE_IK, inverse_reachability, compute_door_paths, FIXED_FAILURES 11 | from src.streams.move import get_gripper_motion_gen 12 | from src.utils import get_descendant_obstacles, FConf 13 | 14 | 15 | def is_pull_safe(world, door_joint, door_plan): 16 | obstacles = get_descendant_obstacles(world.kitchen, door_joint) 17 | door_path, handle_path, handle_plan, tool_path = door_plan 18 | for door_conf in [door_path[0], door_path[-1]]: 19 | # TODO: check the whole door trajectory 20 | set_joint_positions(world.kitchen, [door_joint], door_conf) 21 | # TODO: just check collisions with the base of the robot 22 | if any(pairwise_collision(world.robot, b) for b in obstacles): 23 | if PRINT_FAILURES: print('Door start/end failure') 24 | return False 25 | return True 26 | 27 | 28 | def plan_pull(world, door_joint, door_plan, base_conf, 29 | randomize=True, collisions=True, teleport=False, **kwargs): 30 | door_path, handle_path, handle_plan, tool_path = door_plan 31 | handle_link, handle_grasp, handle_pregrasp = handle_plan 32 | 33 | door_obstacles = get_descendant_obstacles(world.kitchen, door_joint) # if collisions else set() 34 | obstacles = (world.static_obstacles | door_obstacles) # if collisions else set() 35 | 36 | base_conf.assign() 37 | world.open_gripper() 38 | world.carry_conf.assign() 39 | robot_saver = BodySaver(world.robot) # TODO: door_saver? 40 | if not is_pull_safe(world, door_joint, door_plan): 41 | return 42 | 43 | arm_path = plan_workspace(world, tool_path, world.static_obstacles, 44 | randomize=randomize, teleport=collisions) 45 | if arm_path is None: 46 | return 47 | approach_paths = [] 48 | for index in [0, -1]: 49 | set_joint_positions(world.kitchen, [door_joint], door_path[index]) 50 | set_joint_positions(world.robot, world.arm_joints, arm_path[index]) 51 | tool_pose = multiply(handle_path[index], invert(handle_pregrasp)) 52 | approach_path = plan_approach(world, tool_pose, obstacles=obstacles, teleport=teleport, **kwargs) 53 | if approach_path is None: 54 | return 55 | approach_paths.append(approach_path) 56 | 57 | if MOVE_ARM: 58 | aq1 = FConf(world.robot, world.arm_joints, approach_paths[0][0]) 59 | aq2 = FConf(world.robot, world.arm_joints, approach_paths[-1][0]) 60 | else: 61 | aq1 = world.carry_conf 62 | aq2 = aq1 63 | 64 | set_joint_positions(world.kitchen, [door_joint], door_path[0]) 65 | set_joint_positions(world.robot, world.arm_joints, arm_path[0]) 66 | grasp_width = close_until_collision(world.robot, world.gripper_joints, 67 | bodies=[(world.kitchen, [handle_link])]) 68 | gripper_motion_fn = get_gripper_motion_gen(world, teleport=teleport, collisions=collisions, **kwargs) 69 | gripper_conf = FConf(world.robot, world.gripper_joints, [grasp_width] * len(world.gripper_joints)) 70 | finger_cmd, = gripper_motion_fn(world.open_gq, gripper_conf) 71 | 72 | objects = [] 73 | commands = [ 74 | ApproachTrajectory(objects, world, world.robot, world.arm_joints, approach_paths[0]), 75 | DoorTrajectory(world, world.robot, world.arm_joints, arm_path, 76 | world.kitchen, [door_joint], door_path), 77 | ApproachTrajectory(objects, world, world.robot, world.arm_joints, reversed(approach_paths[-1])), 78 | ] 79 | door_path, _, _, _ = door_plan 80 | sign = world.get_door_sign(door_joint) 81 | pull = (sign*door_path[0][0] < sign*door_path[-1][0]) 82 | if pull: 83 | commands.insert(1, finger_cmd.commands[0]) 84 | commands.insert(3, finger_cmd.commands[0].reverse()) 85 | cmd = Sequence(State(world, savers=[robot_saver]), commands, name='pull') 86 | yield (aq1, aq2, cmd,) 87 | 88 | ################################################################################ 89 | 90 | def get_fixed_pull_gen_fn(world, max_attempts=25, collisions=True, teleport=False, **kwargs): 91 | 92 | def gen(joint_name, door_conf1, door_conf2, base_conf): 93 | #if door_conf1 == door_conf2: 94 | # return 95 | # TODO: check if within database convex hull 96 | door_joint = joint_from_name(world.kitchen, joint_name) 97 | obstacles = (world.static_obstacles | get_descendant_obstacles( 98 | world.kitchen, door_joint)) # if collisions else set() 99 | 100 | base_conf.assign() 101 | world.carry_conf.assign() 102 | door_plans = [door_plan for door_plan in compute_door_paths( 103 | world, joint_name, door_conf1, door_conf2, obstacles, teleport=teleport) 104 | if is_pull_safe(world, door_joint, door_plan)] 105 | if not door_plans: 106 | print('Unable to open door {} at fixed config'.format(joint_name)) 107 | return 108 | max_failures = FIXED_FAILURES if world.task.movable_base else INF 109 | failures = 0 110 | while failures <= max_failures: 111 | for i in range(max_attempts): 112 | door_path = random.choice(door_plans) 113 | # TracIK is itself stochastic 114 | randomize = (random.random() < P_RANDOMIZE_IK) 115 | ik_outputs = next(plan_pull(world, door_joint, door_path, base_conf, 116 | randomize=randomize, collisions=collisions, teleport=teleport, **kwargs), 117 | None) 118 | if ik_outputs is not None: 119 | print('Fixed pull succeeded after {} attempts'.format(i)) 120 | yield ik_outputs 121 | break # return 122 | else: 123 | if PRINT_FAILURES: print('Fixed pull failure') 124 | yield None 125 | failures += 1 126 | return gen 127 | 128 | 129 | def get_pull_gen_fn(world, max_attempts=50, collisions=True, teleport=False, learned=True, **kwargs): 130 | # TODO: could condition pick/place into cabinet on the joint angle 131 | obstacles = world.static_obstacles 132 | #if not collisions: 133 | # obstacles = set() 134 | 135 | def gen(joint_name, door_conf1, door_conf2, *args): 136 | if door_conf1 == door_conf2: 137 | return 138 | door_joint = joint_from_name(world.kitchen, joint_name) 139 | door_paths = compute_door_paths(world, joint_name, door_conf1, door_conf2, obstacles, teleport=teleport) 140 | if not door_paths: 141 | return 142 | if learned: 143 | base_generator = cycle(load_pull_base_poses(world, joint_name)) 144 | else: 145 | _, _, _, tool_path = door_paths[0] 146 | index = int(len(tool_path) / 2) # index = 0 147 | target_pose = tool_path[index] 148 | base_generator = uniform_pose_generator(world.robot, target_pose) 149 | safe_base_generator = inverse_reachability(world, base_generator, obstacles=obstacles, **kwargs) 150 | while True: 151 | for i in range(max_attempts): 152 | try: 153 | base_conf = next(safe_base_generator) 154 | except StopIteration: 155 | return 156 | if base_conf is None: 157 | yield None 158 | continue 159 | door_path = random.choice(door_paths) 160 | randomize = (random.random() < P_RANDOMIZE_IK) 161 | ik_outputs = next(plan_pull(world, door_joint, door_path, base_conf, 162 | randomize=randomize, collisions=collisions, teleport=teleport, **kwargs), None) 163 | if ik_outputs is not None: 164 | print('Pull succeeded after {} attempts'.format(i)) 165 | yield (base_conf,) + ik_outputs 166 | break 167 | else: 168 | if PRINT_FAILURES: print('Pull failure') 169 | yield None 170 | return gen -------------------------------------------------------------------------------- /src/visualization.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from pybullet_tools.utils import get_point, convex_hull, Point, add_segments, convex_centroid, add_text, spaced_colors, \ 4 | multiply, point_from_pose, get_pose, invert, link_from_name, grow_polygon, GREEN, get_link_pose 5 | from src.database import load_pull_base_poses, get_surface_reference_pose, load_placements, \ 6 | load_place_base_poses, load_forward_placements, load_inverse_placements 7 | from src.utils import ALL_JOINTS, ALL_SURFACES, get_grasps, surface_from_name, STOVES 8 | 9 | GROW_INVERSE_BASE = 0.05 # 0.05 | 0.1 10 | GROW_FORWARD_RADIUS = 0.25 # Done for block. Incorrect for other object types 11 | 12 | def get_floor_z(world, floor_z=0.005): 13 | return get_point(world.floor)[2] + floor_z 14 | 15 | def visualize_base_confs(world, name, base_confs, **kwargs): 16 | handles = [] 17 | if not base_confs: 18 | return handles 19 | z = get_floor_z(world) 20 | # for x, y in base_points: 21 | # handles.extend(draw_point(Point(x, y, z), color=color)) 22 | vertices = grow_polygon(base_confs, radius=GROW_INVERSE_BASE) 23 | points = [Point(x, y, z) for x, y, in vertices] 24 | handles.extend(add_segments(points, closed=True, **kwargs)) 25 | cx, cy = convex_centroid(vertices) 26 | centroid = [cx, cy, z] 27 | # draw_point(centroid, color=color) 28 | handles.append(add_text(name, position=centroid, **kwargs)) 29 | return handles 30 | 31 | 32 | def add_markers(task, placements=True, forward_place=True, pull_bases=True, inverse_place=False): 33 | # TODO: decompose 34 | world = task.world 35 | handles = [] 36 | if placements: 37 | for surface_name in ALL_SURFACES: 38 | surface = surface_from_name(surface_name) 39 | surface_link = link_from_name(world.kitchen, surface.link) 40 | surface_point = point_from_pose(get_link_pose(world.kitchen, surface_link)) 41 | for grasp_type, color in zip(task.grasp_types, spaced_colors(len(task.grasp_types))): 42 | object_points = list(map(point_from_pose, load_placements(world, surface_name, 43 | grasp_types=[grasp_type]))) 44 | if (surface_name not in STOVES) and object_points: 45 | #for object_point in object_points: 46 | # handles.extend(draw_point(object_point, color=color)) 47 | _, _, z = np.average(object_points, axis=0) 48 | object_points = [Point(x, y, z) for x, y in grow_polygon(object_points, radius=0.0)] 49 | handles.extend(add_segments(object_points, color=color, closed=True, 50 | parent=world.kitchen, parent_link=surface_link)) 51 | base_points = list(map(point_from_pose, load_inverse_placements(world, surface_name, 52 | grasp_types=[grasp_type]))) 53 | if (surface_name in STOVES) and base_points: # and inverse_place 54 | #continue 55 | #_, _, z = np.average(base_points, axis=0) 56 | z = get_floor_z(world) - surface_point[2] 57 | base_points = [Point(x, y, z) for x, y in grow_polygon(base_points, radius=GROW_INVERSE_BASE)] 58 | handles.extend(add_segments(base_points, color=color, closed=True, 59 | parent=world.kitchen, parent_link=surface_link)) 60 | 61 | if forward_place: 62 | # TODO: do this by taking the union of all grasps 63 | object_points = list(map(point_from_pose, load_forward_placements(world))) 64 | robot_point = point_from_pose(get_link_pose(world.robot, world.base_link)) 65 | #z = 0. 66 | z = get_floor_z(world) - robot_point[2] 67 | object_points = [Point(x, y, z) for x, y in grow_polygon(object_points, radius=GROW_FORWARD_RADIUS)] 68 | handles.extend(add_segments(object_points, color=GREEN, closed=True, 69 | parent=world.robot, parent_link=world.base_link)) 70 | 71 | if pull_bases: 72 | for joint_name, color in zip(ALL_JOINTS, spaced_colors(len(ALL_JOINTS))): 73 | base_confs = list(load_pull_base_poses(world, joint_name)) 74 | handles.extend(visualize_base_confs(world, joint_name, base_confs, color=color)) 75 | 76 | #if inverse_place: 77 | # for name in world.movable: 78 | # body = world.get_body(name) 79 | # pose = get_pose(body) 80 | # surface_name = world.get_supporting(name) 81 | # if surface_name is None: 82 | # continue 83 | # for grasp_type, color in zip(GRASP_TYPES, spaced_colors(len(GRASP_TYPES))): 84 | # base_confs = [] 85 | # for grasp in get_grasps(world, name, grasp_types=[grasp_type]): 86 | # tool_pose = multiply(pose, invert(grasp.grasp_pose)) 87 | # base_confs.extend(load_place_base_poses(world, tool_pose, surface_name, grasp_type)) 88 | # handles.extend(visualize_base_confs(world, grasp_type, base_confs, color=color)) 89 | 90 | return handles --------------------------------------------------------------------------------