diff --git a/.gitignore b/.gitignore index 28349c05a2..92d64bae65 100644 --- a/.gitignore +++ b/.gitignore @@ -28,6 +28,8 @@ predicators/envs/assets/task_jsons/spot/last.json spot_perception_outputs spot_perception_debug_dir/ sas_plan +side-state-view.png +top-down-state-view.png # Jetbrains IDEs .idea/ \ No newline at end of file diff --git a/predicators/envs/spot_env.py b/predicators/envs/spot_env.py index 76abe4d42c..3b28506619 100644 --- a/predicators/envs/spot_env.py +++ b/predicators/envs/spot_env.py @@ -275,6 +275,7 @@ def _build_observation(self, # Get the universe of all object detections. all_object_detection_ids = set(self._detection_id_to_obj) # Get the camera images. + time.sleep(0.5) rgbds = capture_images(self._robot, self._localizer) all_detections, all_artifacts = detect_objects( all_object_detection_ids, rgbds) @@ -359,7 +360,7 @@ def _actively_construct_env_task(self) -> EnvironmentTask: # Have the spot walk around the environment once to construct # an initial observation. objects_in_view = self._actively_construct_initial_object_views() - rgb_images = capture_images(self._robot, self._localizer) + rgbd_images = capture_images(self._robot, self._localizer) gripper_open_percentage = get_robot_gripper_open_percentage( self._robot) self._localizer.localize() @@ -367,7 +368,7 @@ def _actively_construct_env_task(self) -> EnvironmentTask: nonpercept_atoms = self._get_initial_nonpercept_atoms() nonpercept_preds = self.predicates - self.percept_predicates assert all(a.predicate in nonpercept_preds for a in nonpercept_atoms) - obs = _SpotObservation(rgb_images, objects_in_view, set(), + obs = _SpotObservation(rgbd_images, objects_in_view, set(), self._spot_object, gripper_open_percentage, robot_pos, nonpercept_atoms, nonpercept_preds) goal_description = self._generate_goal_description() @@ -396,6 +397,8 @@ def _actively_construct_env_task(self) -> EnvironmentTask: for obj in objects_in_view: if "lost" in obj.type.feature_names: init_json_dict[obj.name]["lost"] = 0.0 + if "in_hand_view" in obj.type.feature_names: + init_json_dict[obj.name]["in_hand_view"] = 1.0 if "in_view" in obj.type.feature_names: init_json_dict[obj.name]["in_view"] = 1.0 if "held" in obj.type.feature_names: @@ -501,8 +504,14 @@ def _actively_construct_initial_object_views( def _run_init_search_for_objects( self, detection_ids: Set[ObjectDetectionID] ) -> Dict[ObjectDetectionID, math_helpers.SE3Pose]: - detections, _ = init_search_for_objects(self._robot, self._localizer, - detection_ids) + detections, artifacts = init_search_for_objects( + self._robot, self._localizer, detection_ids) + outdir = Path(CFG.spot_perception_outdir) + time_str = time.strftime("%Y%m%d-%H%M%S") + detections_outfile = outdir / f"detections_{time_str}.png" + no_detections_outfile = outdir / f"no_detections_{time_str}.png" + visualize_all_artifacts(artifacts, detections_outfile, + no_detections_outfile) return detections @property @@ -529,7 +538,7 @@ def _generate_goal_description(self) -> GoalDescription: _INSIDE_Z_THRESHOLD = 0.25 _ONTOP_SURFACE_BUFFER = 0.1 _INSIDE_SURFACE_BUFFER = 0.1 -_REACHABLE_THRESHOLD = 1.7 +_REACHABLE_THRESHOLD = 1.85 _REACHABLE_YAW_THRESHOLD = 0.95 # higher better _CONTAINER_SWEEP_XY_BUFFER = 1.5 @@ -549,7 +558,7 @@ class _Spot3DShape(Enum): ]) _movable_object_type = Type("movable", list(_base_object_type.feature_names) + - ["held", "lost", "in_view"], + ["held", "lost", "in_hand_view", "in_view"], parent=_base_object_type) _immovable_object_type = Type("immovable", list(_base_object_type.feature_names), @@ -641,7 +650,8 @@ def _on_classifier(state: State, objects: Sequence[Object]) -> bool: expect = state.get(obj_surface, "z") + state.get(obj_surface, "height") / 2 actual = state.get(obj_on, "z") - state.get(obj_on, "height") / 2 - return abs(actual - expect) < _ONTOP_Z_THRESHOLD + classification_val = abs(actual - expect) < _ONTOP_Z_THRESHOLD + return classification_val def _inside_classifier(state: State, objects: Sequence[Object]) -> bool: @@ -669,7 +679,14 @@ def _inside_classifier(state: State, objects: Sequence[Object]) -> bool: return obj_top < container_top + _INSIDE_Z_THRESHOLD -def in_view_classifier(state: State, objects: Sequence[Object]) -> bool: +def in_hand_view_classifier(state: State, objects: Sequence[Object]) -> bool: + """Made public for perceiver.""" + _, tool = objects + return state.get(tool, "in_hand_view") > 0.5 + + +def in_general_view_classifier(state: State, + objects: Sequence[Object]) -> bool: """Made public for perceiver.""" _, tool = objects return state.get(tool, "in_view") > 0.5 @@ -785,13 +802,15 @@ def _container_ready_for_sweeping_classifier( _inside_classifier) # NOTE: currently disabling inside predicate check because we don't have a good # way to do the check, especially after sweeping. -_Inside = Predicate(_Inside.name, _Inside.types, - _create_dummy_predicate_classifier(_Inside)) +_FakeInside = Predicate(_Inside.name, _Inside.types, + _create_dummy_predicate_classifier(_Inside)) _HandEmpty = Predicate("HandEmpty", [_robot_type], _handempty_classifier) _Holding = Predicate("Holding", [_robot_type, _movable_object_type], _holding_classifier) +_InHandView = Predicate("InHandView", [_robot_type, _movable_object_type], + in_hand_view_classifier) _InView = Predicate("InView", [_robot_type, _movable_object_type], - in_view_classifier) + in_general_view_classifier) _Reachable = Predicate("Reachable", [_robot_type, _base_object_type], _reachable_classifier) _Blocking = Predicate("Blocking", [_base_object_type, _base_object_type], @@ -814,20 +833,34 @@ def _create_operators() -> Iterator[STRIPSOperator]: preconds = {LiftedAtom(_NotBlocked, [obj])} add_effs = {LiftedAtom(_Reachable, [robot, obj])} del_effs: Set[LiftedAtom] = set() - ignore_effs = {_Reachable, _InView} + ignore_effs = {_Reachable, _InHandView, _InView} yield STRIPSOperator("MoveToReachObject", parameters, preconds, add_effs, del_effs, ignore_effs) - # MoveToViewObject + # MoveToHandViewObject robot = Variable("?robot", _robot_type) obj = Variable("?object", _movable_object_type) parameters = [robot, obj] preconds = {LiftedAtom(_NotBlocked, [obj])} - add_effs = {LiftedAtom(_InView, [robot, obj])} + add_effs = {LiftedAtom(_InHandView, [robot, obj])} del_effs = set() - ignore_effs = {_Reachable, _InView} - yield STRIPSOperator("MoveToViewObject", parameters, preconds, add_effs, - del_effs, ignore_effs) + ignore_effs = {_Reachable, _InHandView, _InView} + yield STRIPSOperator("MoveToHandViewObject", parameters, preconds, + add_effs, del_effs, ignore_effs) + + # MoveToBodyViewObject + robot = Variable("?robot", _robot_type) + obj = Variable("?object", _movable_object_type) + parameters = [robot, obj] + preconds = {LiftedAtom(_NotBlocked, [obj])} + add_effs = { + LiftedAtom(_InView, [robot, obj]), + LiftedAtom(_Reachable, [robot, obj]) + } + del_effs = set() + ignore_effs = {_Reachable, _InHandView, _InView} + yield STRIPSOperator("MoveToBodyViewObject", parameters, preconds, + add_effs, del_effs, ignore_effs) # PickObjectFromTop robot = Variable("?robot", _robot_type) @@ -837,7 +870,7 @@ def _create_operators() -> Iterator[STRIPSOperator]: preconds = { LiftedAtom(_On, [obj, surface]), LiftedAtom(_HandEmpty, [robot]), - LiftedAtom(_InView, [robot, obj]) + LiftedAtom(_InHandView, [robot, obj]) } add_effs = { LiftedAtom(_Holding, [robot, obj]), @@ -845,9 +878,9 @@ def _create_operators() -> Iterator[STRIPSOperator]: del_effs = { LiftedAtom(_On, [obj, surface]), LiftedAtom(_HandEmpty, [robot]), - LiftedAtom(_InView, [robot, obj]) + LiftedAtom(_InHandView, [robot, obj]) } - ignore_effs = set() + ignore_effs = {_Inside} yield STRIPSOperator("PickObjectFromTop", parameters, preconds, add_effs, del_effs, ignore_effs) @@ -891,6 +924,30 @@ def _create_operators() -> Iterator[STRIPSOperator]: yield STRIPSOperator("DropObjectInside", parameters, preconds, add_effs, del_effs, ignore_effs) + # DropObjectInsideContainerOnTop + robot = Variable("?robot", _robot_type) + held = Variable("?held", _movable_object_type) + container = Variable("?container", _container_type) + surface = Variable("?surface", _immovable_object_type) + parameters = [robot, held, container, surface] + preconds = { + LiftedAtom(_Holding, [robot, held]), + LiftedAtom(_Reachable, [robot, container]), + LiftedAtom(_InView, [robot, container]), + LiftedAtom(_On, [container, surface]), + } + add_effs = { + LiftedAtom(_Inside, [held, container]), + LiftedAtom(_HandEmpty, [robot]), + LiftedAtom(_On, [held, surface]) + } + del_effs = { + LiftedAtom(_Holding, [robot, held]), + } + ignore_effs = set() + yield STRIPSOperator("DropObjectInsideContainerOnTop", parameters, + preconds, add_effs, del_effs, ignore_effs) + # DragToUnblockObject robot = Variable("?robot", _robot_type) blocked = Variable("?blocked", _base_object_type) @@ -908,7 +965,7 @@ def _create_operators() -> Iterator[STRIPSOperator]: LiftedAtom(_Blocking, [blocker, blocked]), LiftedAtom(_Holding, [robot, blocker]), } - ignore_effs = {_InView, _Reachable} + ignore_effs = {_InHandView, _Reachable} yield STRIPSOperator("DragToUnblockObject", parameters, preconds, add_effs, del_effs, ignore_effs) @@ -953,7 +1010,7 @@ def _create_operators() -> Iterator[STRIPSOperator]: del_effs = { LiftedAtom(_Holding, [robot, container]), } - ignore_effs = {_Reachable, _InView} + ignore_effs = {_Reachable, _InHandView} yield STRIPSOperator("PrepareContainerForSweeping", parameters, preconds, add_effs, del_effs, ignore_effs) @@ -973,7 +1030,7 @@ def __init__(self, use_gui: bool = True) -> None: op_to_name = {o.name: o for o in _create_operators()} op_names_to_keep = { "MoveToReachObject", - "MoveToViewObject", + "MoveToHandViewObject", "PickObjectFromTop", "PlaceObjectOnTop", } @@ -999,7 +1056,7 @@ def predicates(self) -> Set[Predicate]: _HandEmpty, _Holding, _Reachable, - _InView, + _InHandView, _Blocking, _NotBlocked, } @@ -1011,7 +1068,7 @@ def percept_predicates(self) -> Set[Predicate]: _HandEmpty, _Holding, _On, - _InView, + _InHandView, _Reachable, _Blocking, _NotBlocked, @@ -1071,7 +1128,7 @@ def __init__(self, use_gui: bool = True) -> None: op_to_name = {o.name: o for o in _create_operators()} op_names_to_keep = { "MoveToReachObject", - "MoveToViewObject", + "MoveToHandViewObject", "PickObjectFromTop", "PlaceObjectOnTop", } @@ -1097,7 +1154,7 @@ def predicates(self) -> Set[Predicate]: _HandEmpty, _Holding, _Reachable, - _InView, + _InHandView, _Blocking, _NotBlocked, } @@ -1109,7 +1166,7 @@ def percept_predicates(self) -> Set[Predicate]: _HandEmpty, _Holding, _On, - _InView, + _InHandView, _Reachable, _Blocking, _NotBlocked, @@ -1165,7 +1222,7 @@ def __init__(self, use_gui: bool = True) -> None: op_to_name = {o.name: o for o in _create_operators()} op_names_to_keep = { "MoveToReachObject", - "MoveToViewObject", + "MoveToHandViewObject", "PickObjectFromTop", "PlaceObjectOnTop", "DropObjectInside", @@ -1193,7 +1250,7 @@ def predicates(self) -> Set[Predicate]: _HandEmpty, _Holding, _Reachable, - _InView, + _InHandView, _Inside, _Blocking, _NotBlocked, @@ -1207,7 +1264,7 @@ def percept_predicates(self) -> Set[Predicate]: _Holding, _On, _Reachable, - _InView, + _InHandView, _Blocking, _NotBlocked, } @@ -1263,7 +1320,7 @@ def __init__(self, use_gui: bool = True) -> None: op_to_name = {o.name: o for o in _create_operators()} op_names_to_keep = { "MoveToReachObject", - "MoveToViewObject", + "MoveToHandViewObject", "PickObjectFromTop", "PlaceObjectOnTop", "DropObjectInside", @@ -1292,7 +1349,7 @@ def predicates(self) -> Set[Predicate]: _HandEmpty, _Holding, _Reachable, - _InView, + _InHandView, _Inside, _Blocking, _NotBlocked, @@ -1306,7 +1363,7 @@ def percept_predicates(self) -> Set[Predicate]: _Holding, _On, _Reachable, - _InView, + _InHandView, _Blocking, _NotBlocked, } @@ -1380,7 +1437,7 @@ def __init__(self, use_gui: bool = True) -> None: op_to_name = {o.name: o for o in _create_operators()} op_names_to_keep = { "MoveToReachObject", - "MoveToViewObject", + "MoveToHandViewObject", "PickObjectFromTop", "PlaceObjectOnTop", "DragToUnblockObject", @@ -1410,7 +1467,7 @@ def predicates(self) -> Set[Predicate]: _HandEmpty, _Holding, _Reachable, - _InView, + _InHandView, _Inside, _Blocking, _NotBlocked, @@ -1425,7 +1482,7 @@ def percept_predicates(self) -> Set[Predicate]: _Holding, _On, _Reachable, - _InView, + _InHandView, # NOTE: we can't easily check that an object is inside a container # after sweeping, because the robot is holding a sweeper, blocking # the hand camera that we'd usually use to check containment. @@ -1477,3 +1534,100 @@ def _get_initial_nonpercept_atoms(self) -> Set[GroundAtom]: def _generate_goal_description(self) -> GoalDescription: return "put the soda in the bucket and hold the brush" + + +############################################################################### +# Real-World Ball and Cup Sticky Table Env # +############################################################################### + + +class SpotBallAndCupStickyTableEnv(SpotRearrangementEnv): + """A real-world version of the ball and cup sticky table environment.""" + + def __init__(self, use_gui: bool = True) -> None: + super().__init__(use_gui) + + op_to_name = {o.name: o for o in _create_operators()} + # NOTE: we do not yet have planning operators sufficient enough + # to make the planning graph fully-connected. These are + # forthcoming. + op_names_to_keep = { + "MoveToReachObject", "MoveToHandViewObject", + "MoveToBodyViewObject", "PickObjectFromTop", "PlaceObjectOnTop", + "DropObjectInsideContainerOnTop" + } + self._strips_operators = {op_to_name[o] for o in op_names_to_keep} + + @classmethod + def get_name(cls) -> str: + return "spot_ball_and_cup_sticky_table_env" + + @property + def types(self) -> Set[Type]: + return { + _robot_type, + _base_object_type, + _movable_object_type, + _immovable_object_type, + _container_type, + } + + @property + def predicates(self) -> Set[Predicate]: + return { + _On, + _HandEmpty, + _Holding, + _Reachable, + _InView, + _InHandView, + _Inside, + } + + @property + def percept_predicates(self) -> Set[Predicate]: + """The predicates that are NOT stored in the simulator state.""" + return { + _HandEmpty, + _Holding, + _On, + _Reachable, + _InView, + _InHandView, + _Inside, + } + + @property + def goal_predicates(self) -> Set[Predicate]: + return {_On} + + @property + def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]: + + detection_id_to_obj: Dict[ObjectDetectionID, Object] = {} + + ball = Object("ball", _movable_object_type) + ball_detection = LanguageObjectDetectionID("small white ball") + detection_id_to_obj[ball_detection] = ball + + cup = Object("cup", _container_type) + cup_detection = LanguageObjectDetectionID("white bowl") + detection_id_to_obj[cup_detection] = cup + + known_immovables = load_spot_metadata()["known-immovable-objects"] + for obj_name, obj_pos in known_immovables.items(): + obj = Object(obj_name, _immovable_object_type) + pose = math_helpers.SE3Pose(obj_pos["x"], + obj_pos["y"], + obj_pos["z"], + rot=math_helpers.Quat()) + detection_id = KnownStaticObjectDetectionID(obj_name, pose) + detection_id_to_obj[detection_id] = obj + + return detection_id_to_obj + + def _get_initial_nonpercept_atoms(self) -> Set[GroundAtom]: + return set() + + def _generate_goal_description(self) -> GoalDescription: + return "put the ball on the table" diff --git a/predicators/ground_truth_models/spot/nsrts.py b/predicators/ground_truth_models/spot/nsrts.py index 652e6c490d..aeed5cdefa 100644 --- a/predicators/ground_truth_models/spot/nsrts.py +++ b/predicators/ground_truth_models/spot/nsrts.py @@ -13,33 +13,59 @@ from predicators.utils import null_sampler -def _move_to_view_object_sampler(state: State, goal: Set[GroundAtom], - rng: np.random.Generator, - objs: Sequence[Object]) -> Array: +def _move_to_body_view_object_sampler(state: State, goal: Set[GroundAtom], + rng: np.random.Generator, + objs: Sequence[Object]) -> Array: # Parameters are relative distance, dyaw (to the object you're moving to). - del state, goal, objs, rng # randomization coming soon + del state, goal, rng # randomization coming soon + home_pose = get_spot_home_pose() # Currently assume that the robot is facing the surface in its home pose. # Soon, we will change this to actually sample angles of approach and do # collision detection. - home_pose = get_spot_home_pose() approach_angle = home_pose.angle - np.pi + approach_dist = 1.2 + + # For the cup on the table, we need to be further back to actually + # view it. + if len(objs) == 2 and objs[1].name == "cup": + approach_dist = 1.75 + approach_angle = home_pose.angle - (np.pi / 2) - return np.array([1.20, approach_angle]) + return np.array([approach_dist, approach_angle]) + + +def _move_to_hand_view_object_sampler(state: State, goal: Set[GroundAtom], + rng: np.random.Generator, + objs: Sequence[Object]) -> Array: + # Parameters are relative distance, dyaw (to the object you're moving to). + del state, goal, objs, rng # randomization coming soon + + home_pose = get_spot_home_pose() + # Currently assume that the robot is facing the surface in its home pose. + # Soon, we will change this to actually sample angles of approach and do + # collision detection. + approach_angle = home_pose.angle - np.pi + approach_dist = 1.2 + return np.array([approach_dist, approach_angle]) def _move_to_reach_object_sampler(state: State, goal: Set[GroundAtom], rng: np.random.Generator, objs: Sequence[Object]) -> Array: # Parameters are relative distance, dyaw (to the object you're moving to). - del state, goal, objs, rng # randomization coming soon + del state, goal, rng # randomization coming soon + + home_pose = get_spot_home_pose() # Currently assume that the robot is facing the surface in its home pose. # Soon, we will change this to actually sample angles of approach and do # collision detection. - home_pose = get_spot_home_pose() approach_angle = home_pose.angle - np.pi + if len(objs) == 2 and objs[1].name == "cup": + approach_angle = home_pose.angle - (np.pi / 2) + # NOTE: closer than move_to_view. Important for placing. return np.array([0.8, approach_angle]) @@ -56,7 +82,7 @@ def _place_object_on_top_sampler(state: State, goal: Set[GroundAtom], objs: Sequence[Object]) -> Array: # Parameters are relative dx, dy, dz (to surface objects center). del state, goal, objs, rng # randomization coming soon - return np.array([0.0, 0.0, 0.25]) + return np.array([0.0, 0.0, 0.05]) def _drop_object_inside_sampler(state: State, goal: Set[GroundAtom], @@ -64,8 +90,15 @@ def _drop_object_inside_sampler(state: State, goal: Set[GroundAtom], objs: Sequence[Object]) -> Array: # Parameters are relative dx, dy, dz to the center of the top of the # container. - del state, goal, objs, rng # randomization coming soon - return np.array([0.0, 0.0, 0.5]) + del state, goal, rng # randomization coming soon + + drop_height = 0.5 + dx = 0.0 + if len(objs) == 4 and objs[2].name == "cup": + drop_height = 0.15 + dx = 0.08 # we benefit from dropping more forward in the x! + + return np.array([dx, 0.0, drop_height]) def _drag_to_unblock_object_sampler(state: State, goal: Set[GroundAtom], @@ -104,11 +137,9 @@ class SpotCubeEnvGroundTruthNSRTFactory(GroundTruthNSRTFactory): @classmethod def get_env_names(cls) -> Set[str]: return { - "spot_cube_env", - "spot_soda_table_env", - "spot_soda_bucket_env", - "spot_soda_chair_env", - "spot_soda_sweep_env", + "spot_cube_env", "spot_soda_table_env", "spot_soda_bucket_env", + "spot_soda_chair_env", "spot_soda_sweep_env", + "spot_ball_and_cup_sticky_table_env" } @staticmethod @@ -122,11 +153,13 @@ def get_nsrts(env_name: str, types: Dict[str, Type], nsrts = set() operator_name_to_sampler: Dict[str, NSRTSampler] = { - "MoveToViewObject": _move_to_view_object_sampler, + "MoveToHandViewObject": _move_to_hand_view_object_sampler, + "MoveToBodyViewObject": _move_to_body_view_object_sampler, "MoveToReachObject": _move_to_reach_object_sampler, "PickObjectFromTop": _pick_object_from_top_sampler, "PlaceObjectOnTop": _place_object_on_top_sampler, "DropObjectInside": _drop_object_inside_sampler, + "DropObjectInsideContainerOnTop": _drop_object_inside_sampler, "DragToUnblockObject": _drag_to_unblock_object_sampler, "SweepIntoContainer": _sweep_into_container_sampler, "PrepareContainerForSweeping": _prepare_sweeping_sampler, diff --git a/predicators/ground_truth_models/spot/options.py b/predicators/ground_truth_models/spot/options.py index 74c69522ef..a207e1c6d0 100644 --- a/predicators/ground_truth_models/spot/options.py +++ b/predicators/ground_truth_models/spot/options.py @@ -29,7 +29,8 @@ from predicators.spot_utils.skills.spot_sweep import sweep from predicators.spot_utils.spot_localization import SpotLocalizer from predicators.spot_utils.utils import DEFAULT_HAND_LOOK_DOWN_POSE, \ - DEFAULT_HAND_LOOK_STRAIGHT_DOWN_POSE, get_relative_se2_from_se3 + DEFAULT_HAND_LOOK_STRAIGHT_DOWN_POSE, \ + DEFAULT_HAND_LOOK_STRAIGHT_DOWN_POSE_HIGH, get_relative_se2_from_se3 from predicators.structs import Action, Array, Object, ParameterizedOption, \ Predicate, State, Type @@ -93,6 +94,27 @@ def _drop_at_relative_position_and_look( move_hand_to_relative_pose(robot, DEFAULT_HAND_LOOK_STRAIGHT_DOWN_POSE) +def _move_closer_and_drop_at_relative_position_and_look( + robot: Robot, rel_pose: math_helpers.SE3Pose) -> None: + # First, check if we're too far away in distance or angle + # to place. + dist_to_object = np.sqrt(rel_pose.x * rel_pose.x + rel_pose.y * rel_pose.y) + if dist_to_object > 0.75: + pose_to_nav_to = math_helpers.SE2Pose( + rel_pose.x - (0.75 * rel_pose.x / dist_to_object), + rel_pose.y - (0.75 * rel_pose.y / dist_to_object), 0.0) + navigate_to_relative_pose(robot, pose_to_nav_to) + # Place with a new rel_pose! + rel_pose.x = rel_pose.x - pose_to_nav_to.x + rel_pose.y = rel_pose.y - pose_to_nav_to.y + place_at_relative_position(robot, rel_pose) + # Close the gripper. + close_gripper(robot) + # Look straight down. + move_hand_to_relative_pose(robot, + DEFAULT_HAND_LOOK_STRAIGHT_DOWN_POSE_HIGH) + + def _drag_and_release(robot: Robot, rel_pose: math_helpers.SE2Pose) -> None: # First navigate to the pose. navigate_to_relative_pose(robot, rel_pose) @@ -196,10 +218,10 @@ def _grasp_policy(name: str, target_obj_idx: int, state: State, memory: Dict, ############################################################################### -def _move_to_view_object_policy(state: State, memory: Dict, - objects: Sequence[Object], - params: Array) -> Action: - name = "MoveToViewObject" +def _move_to_hand_view_object_policy(state: State, memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + name = "MoveToHandViewObject" distance_param_idx = 0 yaw_param_idx = 1 robot_obj_idx = 0 @@ -210,6 +232,20 @@ def _move_to_view_object_policy(state: State, memory: Dict, state, memory, objects, params) +def _move_to_body_view_object_policy(state: State, memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + name = "MoveToBodyViewObject" + distance_param_idx = 0 + yaw_param_idx = 1 + robot_obj_idx = 0 + target_obj_idx = 1 + do_gaze = False + return _move_to_target_policy(name, distance_param_idx, yaw_param_idx, + robot_obj_idx, target_obj_idx, do_gaze, + state, memory, objects, params) + + def _move_to_reach_object_policy(state: State, memory: Dict, objects: Sequence[Object], params: Array) -> Action: @@ -300,6 +336,38 @@ def _drop_object_inside_policy(state: State, memory: Dict, (robot, place_rel_pos)) +def _move_and_drop_object_inside_policy(state: State, memory: Dict, + objects: Sequence[Object], + params: Array) -> Action: + del memory # not used + + name = "MoveAndDropObjectInside" + robot_obj_idx = 0 + container_obj_idx = 2 + + robot, _, _ = get_robot() + + dx, dy, dz = params + + robot_obj = objects[robot_obj_idx] + robot_pose = utils.get_se3_pose_from_state(state, robot_obj) + + container_obj = objects[container_obj_idx] + container_pose = utils.get_se3_pose_from_state(state, container_obj) + # The dz parameter is with respect to the top of the container. + container_half_height = state.get(container_obj, "height") / 2 + + container_rel_pose = robot_pose.inverse() * container_pose + place_z = container_rel_pose.z + container_half_height + dz + place_rel_pos = math_helpers.Vec3(x=container_rel_pose.x + dx, + y=container_rel_pose.y + dy, + z=place_z) + + return utils.create_spot_env_action( + name, objects, _move_closer_and_drop_at_relative_position_and_look, + (robot, place_rel_pos)) + + def _drag_to_unblock_object_policy(state: State, memory: Dict, objects: Sequence[Object], params: Array) -> Action: @@ -379,10 +447,13 @@ def _prepare_container_for_sweeping_policy(state: State, memory: Dict, _OPERATOR_NAME_TO_PARAM_SPACE = { "MoveToReachObject": Box(-np.inf, np.inf, (2, )), # rel dist, dyaw - "MoveToViewObject": Box(-np.inf, np.inf, (2, )), # rel dist, dyaw + "MoveToHandViewObject": Box(-np.inf, np.inf, (2, )), # rel dist, dyaw + "MoveToBodyViewObject": Box(-np.inf, np.inf, (2, )), # rel dist, dyaw "PickObjectFromTop": Box(0, 1, (0, )), "PlaceObjectOnTop": Box(-np.inf, np.inf, (3, )), # rel dx, dy, dz "DropObjectInside": Box(-np.inf, np.inf, (3, )), # rel dx, dy, dz + "DropObjectInsideContainerOnTop": Box(-np.inf, np.inf, + (3, )), # rel dx, dy, dz "DragToUnblockObject": Box(-np.inf, np.inf, (3, )), # rel dx, dy, dyaw "SweepIntoContainer": Box(-np.inf, np.inf, (2, )), # rel dx, dy "PrepareContainerForSweeping": Box(-np.inf, np.inf, (3, )), # dx, dy, dyaw @@ -390,10 +461,12 @@ def _prepare_container_for_sweeping_policy(state: State, memory: Dict, _OPERATOR_NAME_TO_POLICY = { "MoveToReachObject": _move_to_reach_object_policy, - "MoveToViewObject": _move_to_view_object_policy, + "MoveToHandViewObject": _move_to_hand_view_object_policy, + "MoveToBodyViewObject": _move_to_body_view_object_policy, "PickObjectFromTop": _pick_object_from_top_policy, "PlaceObjectOnTop": _place_object_on_top_policy, "DropObjectInside": _drop_object_inside_policy, + "DropObjectInsideContainerOnTop": _move_and_drop_object_inside_policy, "DragToUnblockObject": _drag_to_unblock_object_policy, "SweepIntoContainer": _sweep_into_container_policy, "PrepareContainerForSweeping": _prepare_container_for_sweeping_policy, @@ -431,7 +504,8 @@ class SpotCubeEnvGroundTruthOptionFactory(GroundTruthOptionFactory): def get_env_names(cls) -> Set[str]: return { "spot_cube_env", "spot_soda_table_env", "spot_soda_bucket_env", - "spot_soda_chair_env", "spot_soda_sweep_env" + "spot_soda_chair_env", "spot_soda_sweep_env", + "spot_ball_and_cup_sticky_table_env" } @classmethod diff --git a/predicators/perception/spot_perceiver.py b/predicators/perception/spot_perceiver.py index 0a8964a9b8..b220e0d589 100644 --- a/predicators/perception/spot_perceiver.py +++ b/predicators/perception/spot_perceiver.py @@ -11,7 +11,7 @@ from predicators.envs.spot_env import HANDEMPTY_GRIPPER_THRESHOLD, \ SpotCubeEnv, SpotRearrangementEnv, _container_type, \ _immovable_object_type, _movable_object_type, _PartialPerceptionState, \ - _robot_type, _SpotObservation, in_view_classifier + _robot_type, _SpotObservation, in_hand_view_classifier from predicators.perception.base_perceiver import BasePerceiver from predicators.settings import CFG from predicators.spot_utils.utils import load_spot_metadata @@ -110,7 +110,7 @@ def step(self, observation: Observation) -> State: # be in view to assess whether it was placed correctly. robot, obj = objects[:2] state = self._create_state() - is_in_view = in_view_classifier(state, [robot, obj]) + is_in_view = in_hand_view_classifier(state, [robot, obj]) if not is_in_view: # We lost the object! logging.info("[Perceiver] Object was lost!") @@ -195,10 +195,13 @@ def _create_state(self) -> State: if obj.is_instance(_movable_object_type): # Detect if the object is in view currently. if obj in self._known_objects_in_hand_view: - in_view_val = 1.0 + in_hand_view_val = 1.0 else: - in_view_val = 0.0 - state_dict[obj]["in_view"] = in_view_val + in_hand_view_val = 0.0 + state_dict[obj]["in_hand_view"] = in_hand_view_val + # All objects that we know the pose of are + # in view! + state_dict[obj]["in_view"] = 1.0 # Detect if we have lost the tool. if obj in self._lost_objects: lost_val = 1.0 @@ -268,7 +271,7 @@ def _create_state(self) -> State: # from predicators.envs.spot_env import _object_to_side_view_geom # fig = plt.figure() # ax = fig.gca() - # # Draw the robot as a point. + # Draw the robot as a point. # robot_y = state.get(self._robot, "y") # robot_z = state.get(self._robot, "z") # plt.plot([robot_y], [robot_z], color="red", marker="o") @@ -294,7 +297,6 @@ def _create_state(self) -> State: # bbox=dict(facecolor="gray", edgecolor="gray", alpha=0.5)) # plt.tight_layout() # plt.savefig("side-state-view.png") - # import ipdb; ipdb.set_trace() return state @@ -337,4 +339,15 @@ def _create_goal(self, state: State, GroundAtom(Inside, [can, bucket]), GroundAtom(Holding, [robot, plunger]) } + if goal_description == "put the ball on the table": + ball = Object("ball", _movable_object_type) + cup = Object("cup", _container_type) + drafting_table = Object("drafting_table", _immovable_object_type) + On = pred_name_to_pred["On"] + Inside = pred_name_to_pred["Inside"] + return { + GroundAtom(On, [ball, drafting_table]), + GroundAtom(On, [cup, drafting_table]), + GroundAtom(Inside, [ball, cup]) + } raise NotImplementedError("Unrecognized goal description") diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_agnate-bobcat-uN5RqESdRIzimJMcfW9KmA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_agnate-bobcat-uN5RqESdRIzimJMcfW9KmA== new file mode 100644 index 0000000000..abd7eff3f8 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_agnate-bobcat-uN5RqESdRIzimJMcfW9KmA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_akimbo-weaver-Kjot.FH+M3Wk5z6y9wjqXg== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_akimbo-weaver-Kjot.FH+M3Wk5z6y9wjqXg== new file mode 100644 index 0000000000..9ee764fb72 --- /dev/null +++ b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_akimbo-weaver-Kjot.FH+M3Wk5z6y9wjqXg== @@ -0,0 +1,2 @@ + +7edge_snapshot_id_akimbo-weaver-Kjot.FH+M3Wk5z6y9wjqXg== \ No newline at end of file diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_blowy-python-rlczzhKaXsE8LJMH2VWhgQ== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_blowy-python-rlczzhKaXsE8LJMH2VWhgQ== new file mode 100644 index 0000000000..431d15d511 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_blowy-python-rlczzhKaXsE8LJMH2VWhgQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_bowlegged-dassie-Byqle7ZI6Tzb9os38DAFOw== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_bowlegged-dassie-Byqle7ZI6Tzb9os38DAFOw== new file mode 100644 index 0000000000..03b44f0047 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_bowlegged-dassie-Byqle7ZI6Tzb9os38DAFOw== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_burked-hare-07UNN5PNNvZqC3R5uXsAGQ== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_burked-hare-07UNN5PNNvZqC3R5uXsAGQ== new file mode 100644 index 0000000000..0ce9b6c373 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_burked-hare-07UNN5PNNvZqC3R5uXsAGQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_campy-swine-ab1idGPrRLYMvRD18tFKLw== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_campy-swine-ab1idGPrRLYMvRD18tFKLw== new file mode 100644 index 0000000000..8a110f391a Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_campy-swine-ab1idGPrRLYMvRD18tFKLw== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_canted-deer-IhmmS+buI8NFf9G.QmFriw== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_canted-deer-IhmmS+buI8NFf9G.QmFriw== new file mode 100644 index 0000000000..70fb8fad2f Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_canted-deer-IhmmS+buI8NFf9G.QmFriw== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_cracked-giant-2My7vGwk4edVTxL7+RQIDw== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_cracked-giant-2My7vGwk4edVTxL7+RQIDw== new file mode 100644 index 0000000000..2d7032d355 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_cracked-giant-2My7vGwk4edVTxL7+RQIDw== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_curved-lapdog-NGSZv0N3fspmZz9gJr4sqw== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_curved-lapdog-NGSZv0N3fspmZz9gJr4sqw== new file mode 100644 index 0000000000..3bb67f52c9 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_curved-lapdog-NGSZv0N3fspmZz9gJr4sqw== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_cusped-husky-AOnWCq.RLnSYMJpnh1Y..g== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_cusped-husky-AOnWCq.RLnSYMJpnh1Y..g== new file mode 100644 index 0000000000..081df2344c Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_cusped-husky-AOnWCq.RLnSYMJpnh1Y..g== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_damp-algae-RZeNk2guM6JD0GzgtUuvuQ== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_damp-algae-RZeNk2guM6JD0GzgtUuvuQ== new file mode 100644 index 0000000000..3fdc6f114e Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_damp-algae-RZeNk2guM6JD0GzgtUuvuQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_dandy-fish-52SzRgqJUV9erhoop.8mhA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_dandy-fish-52SzRgqJUV9erhoop.8mhA== new file mode 100644 index 0000000000..8dc5e10e05 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_dandy-fish-52SzRgqJUV9erhoop.8mhA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_fazed-jaguar-6CZzJw.I.Fqoxhh4JP.zTw== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_fazed-jaguar-6CZzJw.I.Fqoxhh4JP.zTw== new file mode 100644 index 0000000000..1e412cba80 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_fazed-jaguar-6CZzJw.I.Fqoxhh4JP.zTw== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_fifty-grub-4wKTMsEAC3scEQRWmSbsGQ== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_fifty-grub-4wKTMsEAC3scEQRWmSbsGQ== new file mode 100644 index 0000000000..6abd191cf6 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_fifty-grub-4wKTMsEAC3scEQRWmSbsGQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_filmed-raven-fOKHaC1PPjLcLdN6urA4JA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_filmed-raven-fOKHaC1PPjLcLdN6urA4JA== new file mode 100644 index 0000000000..1e710a7e06 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_filmed-raven-fOKHaC1PPjLcLdN6urA4JA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_furled-rat-v7.qQUdqd9ocLzygunWpbA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_furled-rat-v7.qQUdqd9ocLzygunWpbA== new file mode 100644 index 0000000000..f101bb7efa Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_furled-rat-v7.qQUdqd9ocLzygunWpbA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_gaunt-llama-avk7TRhMkmf8.AQN3Qnyfg== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_gaunt-llama-avk7TRhMkmf8.AQN3Qnyfg== new file mode 100644 index 0000000000..c6fd7d3b3f Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_gaunt-llama-avk7TRhMkmf8.AQN3Qnyfg== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_grapey-monkey-N3u8ZSPVfmn8BLg71raY0Q== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_grapey-monkey-N3u8ZSPVfmn8BLg71raY0Q== new file mode 100644 index 0000000000..e16e2b1734 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_grapey-monkey-N3u8ZSPVfmn8BLg71raY0Q== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_honey-bunny-6x5+vN24y00wHsoHZcxyxA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_honey-bunny-6x5+vN24y00wHsoHZcxyxA== new file mode 100644 index 0000000000..83647358ee Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_honey-bunny-6x5+vN24y00wHsoHZcxyxA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_irate-afghan-mPTwY5GPCADq6AnfR2zAeA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_irate-afghan-mPTwY5GPCADq6AnfR2zAeA== new file mode 100644 index 0000000000..99278e1512 --- /dev/null +++ b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_irate-afghan-mPTwY5GPCADq6AnfR2zAeA== @@ -0,0 +1,2 @@ + +6edge_snapshot_id_irate-afghan-mPTwY5GPCADq6AnfR2zAeA== \ No newline at end of file diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_known-dove-+XizetW1hSSQfferqnFxkw== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_known-dove-+XizetW1hSSQfferqnFxkw== new file mode 100644 index 0000000000..d81da79d9d Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_known-dove-+XizetW1hSSQfferqnFxkw== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_largo-barker-GVwG6+m2P3P7e.xfXJ4Nyg== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_largo-barker-GVwG6+m2P3P7e.xfXJ4Nyg== new file mode 100644 index 0000000000..21327dbc30 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_largo-barker-GVwG6+m2P3P7e.xfXJ4Nyg== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_leafy-gander-jE6pe+7E8RP3uF7V23Qrjg== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_leafy-gander-jE6pe+7E8RP3uF7V23Qrjg== new file mode 100644 index 0000000000..717ec88cbc Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_leafy-gander-jE6pe+7E8RP3uF7V23Qrjg== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_lento-germ-jY5Ma6bP7oX3J0Bf9IVOSQ== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_lento-germ-jY5Ma6bP7oX3J0Bf9IVOSQ== new file mode 100644 index 0000000000..81698c5031 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_lento-germ-jY5Ma6bP7oX3J0Bf9IVOSQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_moldy-eel-8a04A.6XTewC7z89fzOiuw== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_moldy-eel-8a04A.6XTewC7z89fzOiuw== new file mode 100644 index 0000000000..3a17110884 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_moldy-eel-8a04A.6XTewC7z89fzOiuw== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_nibbed-ermine-x+rbFx5egsZhHByYs.DEnA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_nibbed-ermine-x+rbFx5egsZhHByYs.DEnA== new file mode 100644 index 0000000000..2574d7d9e9 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_nibbed-ermine-x+rbFx5egsZhHByYs.DEnA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_owing-cub-DsXEnr1BYfOtt2YzSvHq.w== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_owing-cub-DsXEnr1BYfOtt2YzSvHq.w== new file mode 100644 index 0000000000..a424f99735 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_owing-cub-DsXEnr1BYfOtt2YzSvHq.w== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_pained-raven-UQ4sJ8Va4zvsGXmac3TgEw== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_pained-raven-UQ4sJ8Va4zvsGXmac3TgEw== new file mode 100644 index 0000000000..d10016144f Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_pained-raven-UQ4sJ8Va4zvsGXmac3TgEw== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_pushy-gaur-SDn8al0+gBfuq2topB84UA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_pushy-gaur-SDn8al0+gBfuq2topB84UA== new file mode 100644 index 0000000000..a4f314798f Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_pushy-gaur-SDn8al0+gBfuq2topB84UA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_rushy-bird-B.k1B.5Ks4L0ig+jWhs0EQ== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_rushy-bird-B.k1B.5Ks4L0ig+jWhs0EQ== new file mode 100644 index 0000000000..3da9e091c9 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_rushy-bird-B.k1B.5Ks4L0ig+jWhs0EQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_seated-guppy-dJWTi0I6JTJfkr+V1x3YYA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_seated-guppy-dJWTi0I6JTJfkr+V1x3YYA== new file mode 100644 index 0000000000..a14c48ecc2 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_seated-guppy-dJWTi0I6JTJfkr+V1x3YYA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_sloped-goat-jTDml64KJPat+5lAwduKoQ== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_sloped-goat-jTDml64KJPat+5lAwduKoQ== new file mode 100644 index 0000000000..49cced05f3 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_sloped-goat-jTDml64KJPat+5lAwduKoQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_sniffy-cayman-O33hsXovGvq7LvtEjIgY+A== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_sniffy-cayman-O33hsXovGvq7LvtEjIgY+A== new file mode 100644 index 0000000000..5539d61cb0 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_sniffy-cayman-O33hsXovGvq7LvtEjIgY+A== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_soiled-lemur-Ggmvg868.Leb.q02042YeQ== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_soiled-lemur-Ggmvg868.Leb.q02042YeQ== new file mode 100644 index 0000000000..51cc9d021f Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_soiled-lemur-Ggmvg868.Leb.q02042YeQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_spruce-badger-DgULvjFzcs4sNUyhANxfjA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_spruce-badger-DgULvjFzcs4sNUyhANxfjA== new file mode 100644 index 0000000000..db6004cb00 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_spruce-badger-DgULvjFzcs4sNUyhANxfjA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_vented-beagle-mefU6RDIbJDrBWb236Qi5Q== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_vented-beagle-mefU6RDIbJDrBWb236Qi5Q== new file mode 100644 index 0000000000..47c4911695 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_vented-beagle-mefU6RDIbJDrBWb236Qi5Q== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_won-carp-tRe3S5y8+azD64z2sud.ww== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_won-carp-tRe3S5y8+azD64z2sud.ww== new file mode 100644 index 0000000000..8a6287c0f0 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/edge_snapshots/edge_snapshot_id_won-carp-tRe3S5y8+azD64z2sud.ww== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/graph b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/graph new file mode 100644 index 0000000000..f7358ac44c Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/graph differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/metadata.yaml b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/metadata.yaml new file mode 100644 index 0000000000..ea5f29d9d3 --- /dev/null +++ b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/metadata.yaml @@ -0,0 +1,90 @@ +# Additional info associated with the map. +--- +spot-home-pose: + x: 1.4 + y: 0.5 + angle: 0.0 +# The info below is used to transform april tag x, y, z detections into object +# x, y, z. For example, if a tag is hanging off the side of a table, we would +# want to use the center of the table as the origin of the table object. +april-tag-offsets: + tag-408: # smooth table + x: 0.25 + y: 0.1 + z: -0.2 + tag-409: # sticky table + x: 0.25 + y: -0.1 + z: -0.2 +# Known immovable objects. Assuming default rotations. +known-immovable-objects: + floor: + x: 0.0 + y: 0.0 + z: -0.5 + white-table: + x: 3.2 + y: 0.5 + z: -0.1 + drafting_table: + x: 2.315 + y: 1.855 + z: -0.05 +# Static object features, including the shapes and sizes of known objects. +static-object-features: + smooth_table: + shape: 2 # cylinder + height: 0.52 + length: 0.51 + width: 0.51 + drafting_table: + shape: 1 # cuboid + height: 0.65 + length: 1.3 # 0.6 + width: 1.3 # 0.4 + cube: + shape: 1 # cuboid + height: 0.083 + length: 0.083 + width: 0.083 + plunger: + shape: 1 # cuboid + height: 0.1 + length: 0.1 + width: 0.1 + soda_can: + shape: 2 + height: 0.114 + length: 0.065 + width: 0.065 + floor: + shape: 1 + height: 0.0001 + length: 10000000 # effectively infinite + width: 10000000 + white-table: + shape: 1 # cuboid + height: 0.74 + length: 1.07 + width: 0.51 + bucket: + shape: 2 # cylinder + height: 0.4 + width: 0.23 + length: 0.23 + chair: + shape: 2 # cylinder + height: 0.8 + length: 0.5 + width: 0.5 + cup: + shape: 2 # cylinder + height: 0.1 + length: 0.15 + width: 0.15 + ball: + shape: 2 # cylinder + height: 0.06 + length: 0.06 + width: 0.06 + diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_aglow-oryx-jp0u72zFPrMECf8copIU6A== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_aglow-oryx-jp0u72zFPrMECf8copIU6A== new file mode 100644 index 0000000000..e198c4e94a Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_aglow-oryx-jp0u72zFPrMECf8copIU6A== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_banned-nasua-qbjYMIe5JTwGr1ZnABWoUg== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_banned-nasua-qbjYMIe5JTwGr1ZnABWoUg== new file mode 100644 index 0000000000..a71cffb63e Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_banned-nasua-qbjYMIe5JTwGr1ZnABWoUg== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_callow-vermin-1XHRegSdVK.mfjBIsl.JrQ== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_callow-vermin-1XHRegSdVK.mfjBIsl.JrQ== new file mode 100644 index 0000000000..eacc5acc90 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_callow-vermin-1XHRegSdVK.mfjBIsl.JrQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_color-maggot-Bdb96NxvNtOWW9JtgJd29g== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_color-maggot-Bdb96NxvNtOWW9JtgJd29g== new file mode 100644 index 0000000000..c99af28816 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_color-maggot-Bdb96NxvNtOWW9JtgJd29g== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_crispy-bovid-BSRUaoH5QztcDQIolCsXpw== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_crispy-bovid-BSRUaoH5QztcDQIolCsXpw== new file mode 100644 index 0000000000..0a858eb9b4 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_crispy-bovid-BSRUaoH5QztcDQIolCsXpw== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_cruel-dassie-KhtMbB35IQA6DeYKkSdc1A== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_cruel-dassie-KhtMbB35IQA6DeYKkSdc1A== new file mode 100644 index 0000000000..380815dc66 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_cruel-dassie-KhtMbB35IQA6DeYKkSdc1A== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_felted-hound-3EDNZLbUUpE7oNE72.HnlA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_felted-hound-3EDNZLbUUpE7oNE72.HnlA== new file mode 100644 index 0000000000..b09b2a0ebe Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_felted-hound-3EDNZLbUUpE7oNE72.HnlA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_frayed-chetah-SQGL8jeCjTC2Bewb4Oi5+g== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_frayed-chetah-SQGL8jeCjTC2Bewb4Oi5+g== new file mode 100644 index 0000000000..dbbaef7138 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_frayed-chetah-SQGL8jeCjTC2Bewb4Oi5+g== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_frizzy-remora-0iPbV7IxFp9UISaLG1mFRA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_frizzy-remora-0iPbV7IxFp9UISaLG1mFRA== new file mode 100644 index 0000000000..3012e37b03 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_frizzy-remora-0iPbV7IxFp9UISaLG1mFRA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_goosy-gull-TKEFtXuhZa9opwP1YcXn3A== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_goosy-gull-TKEFtXuhZa9opwP1YcXn3A== new file mode 100644 index 0000000000..f4aef31650 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_goosy-gull-TKEFtXuhZa9opwP1YcXn3A== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_hammy-parrot-3zZbBvIPsexMa1pAhsOVhQ== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_hammy-parrot-3zZbBvIPsexMa1pAhsOVhQ== new file mode 100644 index 0000000000..7ba42e9868 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_hammy-parrot-3zZbBvIPsexMa1pAhsOVhQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_hushed-filly-5XTALgdGzx1kBkRB4vf9bQ== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_hushed-filly-5XTALgdGzx1kBkRB4vf9bQ== new file mode 100644 index 0000000000..82c0e63e3f Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_hushed-filly-5XTALgdGzx1kBkRB4vf9bQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_indigo-chetah-dEs.ZnjXBooHVlqo3imDyg== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_indigo-chetah-dEs.ZnjXBooHVlqo3imDyg== new file mode 100644 index 0000000000..a594928711 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_indigo-chetah-dEs.ZnjXBooHVlqo3imDyg== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_inland-marmot-MUmCPusxZBZpBnCl+CcYDQ== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_inland-marmot-MUmCPusxZBZpBnCl+CcYDQ== new file mode 100644 index 0000000000..1dbf12a23b Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_inland-marmot-MUmCPusxZBZpBnCl+CcYDQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_lanate-gander-2ESuoSh8AF1OdoYu1fXY0Q== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_lanate-gander-2ESuoSh8AF1OdoYu1fXY0Q== new file mode 100644 index 0000000000..464a442617 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_lanate-gander-2ESuoSh8AF1OdoYu1fXY0Q== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_latish-gnu-oNRZDtMujxAWBoEtc3GIkg== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_latish-gnu-oNRZDtMujxAWBoEtc3GIkg== new file mode 100644 index 0000000000..3e45529ea3 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_latish-gnu-oNRZDtMujxAWBoEtc3GIkg== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_mazy-cuckoo-Pv0zD.aMZFMkwG9ASjI9ww== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_mazy-cuckoo-Pv0zD.aMZFMkwG9ASjI9ww== new file mode 100644 index 0000000000..498b28619e Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_mazy-cuckoo-Pv0zD.aMZFMkwG9ASjI9ww== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_myriad-thrush-M.4QAkRSA5tRX8GcYzZWNw== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_myriad-thrush-M.4QAkRSA5tRX8GcYzZWNw== new file mode 100644 index 0000000000..5071162e33 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_myriad-thrush-M.4QAkRSA5tRX8GcYzZWNw== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_north-tick-jBNKcPoDc3Ltl9OcbtuBGg== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_north-tick-jBNKcPoDc3Ltl9OcbtuBGg== new file mode 100644 index 0000000000..2f491372c8 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_north-tick-jBNKcPoDc3Ltl9OcbtuBGg== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_oozing-redbug-dqH7QmNHQ00UnCfSrbSTFQ== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_oozing-redbug-dqH7QmNHQ00UnCfSrbSTFQ== new file mode 100644 index 0000000000..881cfa6a7c Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_oozing-redbug-dqH7QmNHQ00UnCfSrbSTFQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_pebbly-moray-PPuQwtXYabkaGv13IrtaGA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_pebbly-moray-PPuQwtXYabkaGv13IrtaGA== new file mode 100644 index 0000000000..1cd843fbc7 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_pebbly-moray-PPuQwtXYabkaGv13IrtaGA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_peppy-cayman-qJGlBVSl.PIiK4ZgLvTU8Q== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_peppy-cayman-qJGlBVSl.PIiK4ZgLvTU8Q== new file mode 100644 index 0000000000..74992666c0 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_peppy-cayman-qJGlBVSl.PIiK4ZgLvTU8Q== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_pilous-goat-VVdQ9esoCOcUm2tnEBuhFA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_pilous-goat-VVdQ9esoCOcUm2tnEBuhFA== new file mode 100644 index 0000000000..b5316fdccc Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_pilous-goat-VVdQ9esoCOcUm2tnEBuhFA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_prosy-fox-vyaGZsIGuyZlJmsGfRbdKg== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_prosy-fox-vyaGZsIGuyZlJmsGfRbdKg== new file mode 100644 index 0000000000..db6b24eca1 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_prosy-fox-vyaGZsIGuyZlJmsGfRbdKg== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_proto-beaver-F7gpX4vwQbtl6oCKL.2XnA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_proto-beaver-F7gpX4vwQbtl6oCKL.2XnA== new file mode 100644 index 0000000000..2bbd28cb27 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_proto-beaver-F7gpX4vwQbtl6oCKL.2XnA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_pulpy-cattle-bBBGLXfpoSp6pWaR3iIaMQ== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_pulpy-cattle-bBBGLXfpoSp6pWaR3iIaMQ== new file mode 100644 index 0000000000..2a47101b55 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_pulpy-cattle-bBBGLXfpoSp6pWaR3iIaMQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_quits-isopod-A86eXdvu9tkbL2zcep592A== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_quits-isopod-A86eXdvu9tkbL2zcep592A== new file mode 100644 index 0000000000..1a089ed9f3 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_quits-isopod-A86eXdvu9tkbL2zcep592A== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_rooted-agouti-tBFci4Cym6VTiwRmb0LFBg== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_rooted-agouti-tBFci4Cym6VTiwRmb0LFBg== new file mode 100644 index 0000000000..06804bfc17 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_rooted-agouti-tBFci4Cym6VTiwRmb0LFBg== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_sedgy-pig-yAGhwx+xwSu8VU+4X4l86Q== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_sedgy-pig-yAGhwx+xwSu8VU+4X4l86Q== new file mode 100644 index 0000000000..49ec6e7417 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_sedgy-pig-yAGhwx+xwSu8VU+4X4l86Q== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_shaken-chimp-gXp+QGWQq7rLfZIgGFXrGA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_shaken-chimp-gXp+QGWQq7rLfZIgGFXrGA== new file mode 100644 index 0000000000..929dc92037 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_shaken-chimp-gXp+QGWQq7rLfZIgGFXrGA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_sixth-borzoi-tGQWKqCfv8r0tcnrUdbW8w== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_sixth-borzoi-tGQWKqCfv8r0tcnrUdbW8w== new file mode 100644 index 0000000000..e1af3ab08d Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_sixth-borzoi-tGQWKqCfv8r0tcnrUdbW8w== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_squab-turkey-fu.7IoCCDqVjEMuyZA0Rsg== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_squab-turkey-fu.7IoCCDqVjEMuyZA0Rsg== new file mode 100644 index 0000000000..e91985fe7e Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_squab-turkey-fu.7IoCCDqVjEMuyZA0Rsg== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_sunken-vole-9qPDBiO7wR6nWYZmXkDtWQ== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_sunken-vole-9qPDBiO7wR6nWYZmXkDtWQ== new file mode 100644 index 0000000000..144c4ec6ef Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_sunken-vole-9qPDBiO7wR6nWYZmXkDtWQ== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_surly-dove-FPLcylDTudOZUcBi+hv3RA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_surly-dove-FPLcylDTudOZUcBi+hv3RA== new file mode 100644 index 0000000000..5fa7d26a9c Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_surly-dove-FPLcylDTudOZUcBi+hv3RA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_tinkly-ibex-HIMoMOpnVnDrYocjpX4CFg== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_tinkly-ibex-HIMoMOpnVnDrYocjpX4CFg== new file mode 100644 index 0000000000..6bf3a1db6d Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_tinkly-ibex-HIMoMOpnVnDrYocjpX4CFg== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_wigged-mite-BIJTOSMiyG+bvAILeSTbBA== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_wigged-mite-BIJTOSMiyG+bvAILeSTbBA== new file mode 100644 index 0000000000..c5698b8994 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_wigged-mite-BIJTOSMiyG+bvAILeSTbBA== differ diff --git a/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_wound-brahma-o3YXiCS0JaWmYvM+XelNwQ== b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_wound-brahma-o3YXiCS0JaWmYvM+XelNwQ== new file mode 100644 index 0000000000..f01f849aa0 Binary files /dev/null and b/predicators/spot_utils/graph_nav_maps/floor8-cup-table/waypoint_snapshots/snapshot_wound-brahma-o3YXiCS0JaWmYvM+XelNwQ== differ diff --git a/predicators/spot_utils/skills/spot_place.py b/predicators/spot_utils/skills/spot_place.py index 81013cb5fd..733b11752a 100644 --- a/predicators/spot_utils/skills/spot_place.py +++ b/predicators/spot_utils/skills/spot_place.py @@ -1,5 +1,7 @@ """Interface for spot placing skill.""" +import time + import numpy as np from bosdyn.client import math_helpers from bosdyn.client.sdk import Robot @@ -26,6 +28,10 @@ def place_at_relative_position(robot: Robot, z=body_to_position.z, rot=rot) move_hand_to_relative_pose(robot, body_tform_goal) + # NOTE: short sleep necessary because without it, the robot + # sometimes opens the gripper before the arm has fully + # arrived at its position. + time.sleep(1.5) # Open the hand. open_gripper(robot) diff --git a/predicators/spot_utils/utils.py b/predicators/spot_utils/utils.py index 515df7e901..a0b0b18b6d 100644 --- a/predicators/spot_utils/utils.py +++ b/predicators/spot_utils/utils.py @@ -25,6 +25,8 @@ x=0.80, y=0.0, z=0.25, rot=math_helpers.Quat.from_pitch(np.pi / 3)) DEFAULT_HAND_LOOK_STRAIGHT_DOWN_POSE = math_helpers.SE3Pose( x=0.80, y=0.0, z=0.25, rot=math_helpers.Quat.from_pitch(np.pi / 2)) +DEFAULT_HAND_LOOK_STRAIGHT_DOWN_POSE_HIGH = math_helpers.SE3Pose( + x=0.65, y=0.0, z=0.32, rot=math_helpers.Quat.from_pitch(np.pi / 2.5)) def get_graph_nav_dir() -> Path: