Skip to content

Commit

Permalink
Real world ball and cup environment (#139)
Browse files Browse the repository at this point in the history
* initial simple env

* initial working version

* debugging cup detection

* fixes issue with drafting table location and gets planning theory to go thru!

* some progress

* making slow progress on cup demo

* progress!

* try this

* yapf and some other fixes

* unnecessary changes

* revert unnecessary changes

* comment out plotting robot code

* yeetus!

* probably good to go?

* checks

---------

Co-authored-by: NishanthJKumar <njk@mit.edu>
nkumar-bdai and NishanthJKumar authored Nov 13, 2023
1 parent 4ca8e92 commit 277d746
Showing 83 changed files with 447 additions and 69 deletions.
2 changes: 2 additions & 0 deletions .gitignore
Original file line number Diff line number Diff line change
@@ -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/
228 changes: 191 additions & 37 deletions predicators/envs/spot_env.py

Large diffs are not rendered by default.

67 changes: 50 additions & 17 deletions predicators/ground_truth_models/spot/nsrts.py
Original file line number Diff line number Diff line change
@@ -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,16 +82,23 @@ 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],
rng: np.random.Generator,
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,
90 changes: 82 additions & 8 deletions predicators/ground_truth_models/spot/options.py
Original file line number Diff line number Diff line change
@@ -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,21 +447,26 @@ 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
}

_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
27 changes: 20 additions & 7 deletions predicators/perception/spot_perceiver.py
Original file line number Diff line number Diff line change
@@ -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")
Binary file not shown.
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@

7edge_snapshot_id_akimbo-weaver-Kjot.FH+M3Wk5z6y9wjqXg==
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@

6edge_snapshot_id_irate-afghan-mPTwY5GPCADq6AnfR2zAeA==
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Original file line number Diff line number Diff line change
@@ -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

Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
6 changes: 6 additions & 0 deletions predicators/spot_utils/skills/spot_place.py
Original file line number Diff line number Diff line change
@@ -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)

2 changes: 2 additions & 0 deletions predicators/spot_utils/utils.py
Original file line number Diff line number Diff line change
@@ -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:

0 comments on commit 277d746

Please sign in to comment.