Skip to content

Commit

Permalink
making some progress; took way longer than expected though...
Browse files Browse the repository at this point in the history
  • Loading branch information
@NishanthJKumar
NishanthJKumar committed Sep 12, 2024
1 parent d255810 commit 803683d
Show file tree
Hide file tree
Showing 12 changed files with 200 additions and 125 deletions.
3 changes: 2 additions & 1 deletion predicators/approaches/bilevel_planning_approach.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,8 @@ def _solve(self, task: Task, timeout: int) -> Callable[[State], Action]:
nsrts = self._get_current_nsrts()
preds = self._get_current_predicates()
utils.abstract(task.init, preds, self._vlm)
import pdb; pdb.set_trace()
import pdb
pdb.set_trace()
# utils.abstract(task.init, preds, self._vlm)

# Run task planning only and then greedily sample and execute in the
Expand Down
136 changes: 59 additions & 77 deletions predicators/envs/spot_env.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,8 @@
brush_prompt, bucket_prompt, football_prompt, train_toy_prompt
from predicators.spot_utils.perception.perception_structs import \
RGBDImageWithContext
from predicators.spot_utils.perception.spot_cameras import capture_images, capture_images_without_context
from predicators.spot_utils.perception.spot_cameras import capture_images, \
capture_images_without_context
from predicators.spot_utils.skills.spot_find_objects import \
init_search_for_objects
from predicators.spot_utils.skills.spot_hand_move import \
Expand Down Expand Up @@ -187,15 +188,17 @@ def get_robot(

@functools.lru_cache(maxsize=None)
def get_robot_only() -> Tuple[Optional[Robot], Optional[LeaseClient]]:
hostname = CFG.spot_robot_ip
sdk = create_standard_sdk("PredicatorsClient-")
robot = sdk.create_robot(hostname)
robot.authenticate("user", "bbbdddaaaiii")
verify_estop(robot)
lease_client = robot.ensure_client(LeaseClient.default_service_name)
lease_client.take()
lease_keepalive = LeaseKeepAlive(lease_client, must_acquire=True, return_at_exit=True)
return robot, lease_client
hostname = CFG.spot_robot_ip
sdk = create_standard_sdk("PredicatorsClient-")
robot = sdk.create_robot(hostname)
robot.authenticate("user", "bbbdddaaaiii")
verify_estop(robot)
lease_client = robot.ensure_client(LeaseClient.default_service_name)
lease_client.take()
lease_keepalive = LeaseKeepAlive(lease_client,
must_acquire=True,
return_at_exit=True)
return robot, lease_client


@functools.lru_cache(maxsize=None)
Expand Down Expand Up @@ -1481,45 +1484,37 @@ def _get_sweeping_surface_for_container(container: Object,
"IsSemanticallyGreaterThan", [_base_object_type, _base_object_type],
_is_semantically_greater_than_classifier)


def _get_vlm_query_str(pred_name: str, objects: Sequence[Object]) -> str:
return pred_name + "(" + ", ".join(str(obj.name) for obj in objects) + ")" # pragma: no cover
_VLMOn = utils.create_vlm_predicate(
"VLMOn",
[_movable_object_type, _base_object_type],
lambda o: _get_vlm_query_str("VLMOn", o)
)
return pred_name + "(" + ", ".join(
str(obj.name) for obj in objects) + ")" # pragma: no cover


_VLMOn = utils.create_vlm_predicate("VLMOn",
[_movable_object_type, _base_object_type],
lambda o: _get_vlm_query_str("VLMOn", o))
_Upright = utils.create_vlm_predicate(
"Upright",
[_movable_object_type],
lambda o: _get_vlm_query_str("Upright", o)
)
"Upright", [_movable_object_type],
lambda o: _get_vlm_query_str("Upright", o))
_Toasted = utils.create_vlm_predicate(
"Toasted",
[_movable_object_type],
lambda o: _get_vlm_query_str("Toasted", o)
)
"Toasted", [_movable_object_type],
lambda o: _get_vlm_query_str("Toasted", o))
_VLMIn = utils.create_vlm_predicate(
"VLMIn",
[_movable_object_type, _immovable_object_type],
lambda o: _get_vlm_query_str("In", o)
)
_Open = utils.create_vlm_predicate(
"Open",
[_movable_object_type],
lambda o: _get_vlm_query_str("Open", o)
)
"VLMIn", [_movable_object_type, _immovable_object_type],
lambda o: _get_vlm_query_str("In", o))
_Open = utils.create_vlm_predicate("Open", [_movable_object_type],
lambda o: _get_vlm_query_str("Open", o))
_Stained = utils.create_vlm_predicate(
"Stained",
[_movable_object_type],
lambda o: _get_vlm_query_str("Stained", o)
)
"Stained", [_movable_object_type],
lambda o: _get_vlm_query_str("Stained", o))

_ALL_PREDICATES = {
_NEq, _On, _TopAbove, _Inside, _NotInsideAnyContainer, _FitsInXY,
_HandEmpty, _Holding, _NotHolding, _InHandView, _InView, _Reachable,
_Blocking, _NotBlocked, _ContainerReadyForSweeping, _IsPlaceable,
_IsNotPlaceable, _IsSweeper, _HasFlatTopSurface, _RobotReadyForSweeping,
_IsSemanticallyGreaterThan, _VLMOn, _Upright, _Toasted, _VLMIn, _Open, _Stained
_IsSemanticallyGreaterThan, _VLMOn, _Upright, _Toasted, _VLMIn, _Open,
_Stained
}
_NONPERCEPT_PREDICATES: Set[Predicate] = set()

Expand Down Expand Up @@ -2452,16 +2447,17 @@ class VLMTestEnv(SpotRearrangementEnv):
@property
def predicates(self) -> Set[Predicate]:
# return set(p for p in _ALL_PREDICATES if p.name in ["VLMOn", "Holding", "HandEmpty", "Pourable", "Toasted", "VLMIn", "Open"])
return set(p for p in _ALL_PREDICATES if p.name in ["VLMOn", "Holding", "HandEmpty", "Upright"])

return set(p for p in _ALL_PREDICATES
if p.name in ["VLMOn", "Holding", "HandEmpty", "Upright"])

@property
def goal_predicates(self) -> Set[Predicate]:
return self.predicates

@classmethod
def get_name(cls) -> str:
return "spot_vlm_test_env"

def _get_dry_task(self, train_or_test: str,
task_idx: int) -> EnvironmentTask:
raise NotImplementedError("No dry task for VLMTestEnv.")
Expand All @@ -2482,35 +2478,31 @@ def _create_operators(self) -> Iterator[STRIPSOperator]:
LiftedAtom(_NotHolding, [robot, obj]),
LiftedAtom(_VLMOn, [obj, table])
}
add_effs: Set[LiftedAtom] = {
LiftedAtom(_Holding, [robot, obj])
}
add_effs: Set[LiftedAtom] = {LiftedAtom(_Holding, [robot, obj])}
del_effs: Set[LiftedAtom] = {
LiftedAtom(_HandEmpty, [robot]),
LiftedAtom(_NotHolding, [robot, obj]),
LiftedAtom(_VLMOn, [obj, table])
}
ignore_effs: Set[LiftedAtom] = set()
yield STRIPSOperator("Pick", parameters, preconds, add_effs, del_effs, ignore_effs)
yield STRIPSOperator("Pick", parameters, preconds, add_effs, del_effs,
ignore_effs)

# Place object
robot = Variable("?robot", _robot_type)
obj = Variable("?object", _movable_object_type)
pan = Variable("?pan", _container_type)
parameters = [robot, obj, pan]
preconds: Set[LiftedAtom] = {
LiftedAtom(_Holding, [robot, obj])
}
preconds: Set[LiftedAtom] = {LiftedAtom(_Holding, [robot, obj])}
add_effs: Set[LiftedAtom] = {
LiftedAtom(_HandEmpty, [robot]),
LiftedAtom(_NotHolding, [robot, obj]),
LiftedAtom(_VLMOn, [obj, pan])
}
del_effs: Set[LiftedAtom] = {
LiftedAtom(_Holding, [robot, obj])
}
del_effs: Set[LiftedAtom] = {LiftedAtom(_Holding, [robot, obj])}
ignore_effs: Set[LiftedAtom] = set()
yield STRIPSOperator("Place", parameters, preconds, add_effs, del_effs, ignore_effs)
yield STRIPSOperator("Place", parameters, preconds, add_effs, del_effs,
ignore_effs)

# def _generate_train_tasks(self) -> List[EnvironmentTask]:
# goal = self._generate_goal_description() # currently just one goal
Expand Down Expand Up @@ -2542,34 +2534,27 @@ def __init__(self, use_gui: bool = True) -> None:
# Create constant objects.
self._spot_object = Object("robot", _robot_type)
op_to_name = {o.name: o for o in self._create_operators()}
op_names_to_keep = {
"Pick",
"Place"
}
op_names_to_keep = {"Pick", "Place"}
self._strips_operators = {op_to_name[o] for o in op_names_to_keep}
self._train_tasks = []
self._test_tasks = []

def _actively_construct_env_task(self) -> EnvironmentTask:
assert self._robot is not None
rgbd_images = capture_images_without_context(self._robot)
gripper_open_percentage = get_robot_gripper_open_percentage(self._robot)
gripper_open_percentage = get_robot_gripper_open_percentage(
self._robot)
objects_in_view = []
obs = _TruncatedSpotObservation(
rgbd_images,
set(objects_in_view),
set(),
set(),
self._spot_object,
gripper_open_percentage
)
obs = _TruncatedSpotObservation(rgbd_images, set(objects_in_view),
set(), set(), self._spot_object,
gripper_open_percentage)
goal_description = self._generate_goal_description()
task = EnvironmentTask(obs, goal_description)
return task

def _generate_goal_description(self) -> GoalDescription:
return "put the cup in the pan"

def reset(self, train_or_test: str, task_idx: int) -> Observation:
prompt = f"Please set up {train_or_test} task {task_idx}!"
utils.prompt_user(prompt)
Expand All @@ -2587,7 +2572,7 @@ def reset(self, train_or_test: str, task_idx: int) -> Observation:
self._current_task_goal_reached = False
self._last_action = None
return self._current_task.init_obs

def step(self, action: Action) -> Observation:
assert self._robot is not None
action_name = action.extra_info.action_name
Expand Down Expand Up @@ -2620,19 +2605,16 @@ def step(self, action: Action) -> Observation:
logging.warning("WARNING: the following retryable error "
f"was encountered. Trying again.\n{e}")
rgbd_images = capture_images_without_context(self._robot)
gripper_open_percentage = get_robot_gripper_open_percentage(self._robot)
gripper_open_percentage = get_robot_gripper_open_percentage(
self._robot)
print(gripper_open_percentage)
objects_in_view = []
obs = _TruncatedSpotObservation(
rgbd_images,
set(objects_in_view),
set(),
set(),
self._spot_object,
gripper_open_percentage
)
obs = _TruncatedSpotObservation(rgbd_images, set(objects_in_view),
set(), set(), self._spot_object,
gripper_open_percentage)
return obs


###############################################################################
# Cube Table Env #
###############################################################################
Expand Down
10 changes: 5 additions & 5 deletions predicators/ground_truth_models/spot_env/nsrts.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -285,11 +285,11 @@ class SpotEnvsGroundTruthNSRTFactory(GroundTruthNSRTFactory):
@classmethod
def get_env_names(cls) -> Set[str]:
return {
"spot_vlm_test_env",
"spot_cube_env", "spot_soda_floor_env", "spot_soda_table_env",
"spot_soda_bucket_env", "spot_soda_chair_env",
"spot_main_sweep_env", "spot_ball_and_cup_sticky_table_env",
"spot_brush_shelf_env", "lis_spot_block_floor_env"
"spot_vlm_test_env", "spot_cube_env", "spot_soda_floor_env",
"spot_soda_table_env", "spot_soda_bucket_env",
"spot_soda_chair_env", "spot_main_sweep_env",
"spot_ball_and_cup_sticky_table_env", "spot_brush_shelf_env",
"lis_spot_block_floor_env"
}

@staticmethod
Expand Down
23 changes: 12 additions & 11 deletions predicators/ground_truth_models/spot_env/options.py
View file
Original file line number Diff line number Diff line change
@@ -1,22 +1,22 @@
"""Ground-truth options for Spot environments."""

import logging
import time
from typing import Callable, Dict, List, Optional, Sequence, Set, Tuple
import logging

import numpy as np
import pbrspot
from bosdyn.client import math_helpers
from bosdyn.client.sdk import Robot
from bosdyn.client.lease import LeaseClient
from bosdyn.client.sdk import Robot
from gym.spaces import Box

from predicators import utils
from predicators.envs import get_or_create_env
from predicators.envs.spot_env import HANDEMPTY_GRIPPER_THRESHOLD, \
SpotRearrangementEnv, _get_sweeping_surface_for_container, \
get_detection_id_for_object, get_robot, get_robot_only, \
get_robot_gripper_open_percentage, get_simulated_object, \
get_detection_id_for_object, get_robot, \
get_robot_gripper_open_percentage, get_robot_only, get_simulated_object, \
get_simulated_robot
from predicators.ground_truth_models import GroundTruthOptionFactory
from predicators.settings import CFG
Expand Down Expand Up @@ -899,9 +899,11 @@ def _move_to_ready_sweep_policy(state: State, memory: Dict,
robot_obj_idx, target_obj_idx, do_gaze,
state, memory, objects, params)

def _teleop_policy(state: State, memory: Dict, objects: Sequence[Object], params: Array) -> Action:

def _teleop_policy(state: State, memory: Dict, objects: Sequence[Object],
params: Array) -> Action:
del state, memory, params

robot, lease_client = get_robot_only()

def _teleop(robot: Robot, lease_client: LeaseClient):
Expand All @@ -914,15 +916,14 @@ def _teleop(robot: Robot, lease_client: LeaseClient):
# Take back control.
robot, lease_client = get_robot_only()
lease_client.take()

fn = _teleop
fn_args = (robot, lease_client)
sim_fn = lambda _: None
sim_fn_args = ()
name = "teleop"
action_extra_info = SpotActionExtraInfo(
name, objects, fn, fn_args, sim_fn, sim_fn_args
)
action_extra_info = SpotActionExtraInfo(name, objects, fn, fn_args, sim_fn,
sim_fn_args)
return utils.create_spot_env_action(action_extra_info)


Expand Down Expand Up @@ -958,7 +959,7 @@ def _teleop(robot: Robot, lease_client: LeaseClient):
"MoveToReadySweep": Box(0, 1, (0, )), # empty
"Pick": Box(0, 1, (0, )), # empty
"Place": Box(0, 1, (0, )) # empty
}
}

# NOTE: the policies MUST be unique because they output actions with extra info
# that includes the name of the operators.
Expand Down
Loading

0 comments on commit 803683d

Please sign in to comment.