Skip to content

Commit

Permalink
mostly done
Browse files Browse the repository at this point in the history
  • Loading branch information
@wmcclinton
wmcclinton committed Jul 18, 2023
1 parent 37ceca2 commit abf131b
Show file tree
Hide file tree
Showing 5 changed files with 67 additions and 117 deletions.
45 changes: 32 additions & 13 deletions predicators/envs/spot_env.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -549,7 +549,7 @@ def __init__(self, use_gui: bool = True) -> None:
self._tool_type = Type("tool", ["x", "y", "z", "lost", "in_view"])
self._surface_type = Type("flat_surface", ["x", "y", "z"])
self._bag_type = Type("bag", ["x", "y", "z"])
self._platform_type = Type("platform", ["x", "y", "z"])
self._platform_type = Type("platform", ["x", "y", "z", "lost", "in_view"])
self._floor_type = Type("floor", ["x", "y", "z"])

# Predicates
Expand Down Expand Up @@ -589,6 +589,9 @@ def __init__(self, use_gui: bool = True) -> None:
self._InViewTool = Predicate("InViewTool",
[self._robot_type, self._tool_type],
self._tool_in_view_classifier)
self._InViewPlatform = Predicate("InViewPlatform",
[self._robot_type, self._platform_type],
self._platform_in_view_classifier)
self._ReachableBag = Predicate("ReachableBag",
[self._robot_type, self._bag_type],
self._reachable_classifier)
Expand All @@ -604,12 +607,9 @@ def __init__(self, use_gui: bool = True) -> None:
self._SurfaceNotTooHigh = Predicate(
"SurfaceNotTooHigh", [self._robot_type, self._surface_type],
self._surface_not_too_high_classifier)
self._temp_PlatformNear = Predicate(
"PlatformNear", [self._platform_type, self._surface_type],
lambda s, o: False)
self._PlatformNear = Predicate(
"PlatformNear", [self._platform_type, self._surface_type],
_create_dummy_predicate_classifier(self._temp_PlatformNear))
self._platform_is_near)

# STRIPS Operators (needed for option creation)
# MoveToToolOnSurface
Expand All @@ -620,7 +620,7 @@ def __init__(self, use_gui: bool = True) -> None:
add_effs = {LiftedAtom(self._InViewTool, [spot, tool])}
ignore_effs = {
self._ReachableBag, self._ReachableSurface,
self._ReachablePlatform, self._InViewTool
self._ReachablePlatform, self._InViewTool, self._InViewPlatform
}
self._MoveToToolOnSurfaceOp = STRIPSOperator("MoveToToolOnSurface",
[spot, tool, surface],
Expand All @@ -634,7 +634,7 @@ def __init__(self, use_gui: bool = True) -> None:
add_effs = {LiftedAtom(self._InViewTool, [spot, tool])}
ignore_effs = {
self._ReachableBag, self._ReachableSurface,
self._ReachablePlatform, self._InViewTool
self._ReachablePlatform, self._InViewTool, self._InViewPlatform
}
self._MoveToToolOnFloorOp = STRIPSOperator("MoveToToolOnFloor",
[spot, tool, floor],
Expand All @@ -646,18 +646,19 @@ def __init__(self, use_gui: bool = True) -> None:
add_effs = {LiftedAtom(self._ReachableSurface, [spot, surface])}
ignore_effs = {
self._ReachableBag, self._ReachableSurface,
self._ReachablePlatform, self._InViewTool
self._ReachablePlatform, self._InViewTool, self._InViewPlatform
}
self._MoveToSurfaceOp = STRIPSOperator("MoveToSurface",
[spot, surface], set(),
add_effs, set(), ignore_effs)
# MoveToPlatform
spot = Variable("?robot", self._robot_type)
platform = Variable("?platform", self._platform_type)
add_effs = {LiftedAtom(self._ReachablePlatform, [spot, platform])}
add_effs = {LiftedAtom(self._ReachablePlatform, [spot, platform]),
LiftedAtom(self._InViewPlatform, [spot, platform])}
ignore_effs = {
self._ReachableBag, self._ReachableSurface,
self._ReachablePlatform, self._InViewTool
self._ReachablePlatform, self._InViewTool, self._InViewPlatform
}
self._MoveToPlatformOp = STRIPSOperator("MoveToPlatform",
[spot, platform], set(),
Expand All @@ -668,7 +669,7 @@ def __init__(self, use_gui: bool = True) -> None:
add_effs = {LiftedAtom(self._ReachableBag, [spot, bag])}
ignore_effs = {
self._ReachableBag, self._ReachableSurface,
self._ReachablePlatform, self._InViewTool
self._ReachablePlatform, self._InViewTool, self._InViewPlatform
}
self._MoveToBagOp = STRIPSOperator("MoveToBag", [spot, bag], set(),
add_effs, set(), ignore_effs)
Expand Down Expand Up @@ -723,6 +724,7 @@ def __init__(self, use_gui: bool = True) -> None:
preconds = {
LiftedAtom(self._ReachablePlatform, [spot, platform]),
LiftedAtom(self._HandEmpty, [spot]),
LiftedAtom(self._InViewPlatform, [spot, platform])
}
add_effs = {
LiftedAtom(self._HoldingPlatformLeash, [spot, platform]),
Expand All @@ -731,6 +733,7 @@ def __init__(self, use_gui: bool = True) -> None:
del_effs = {
LiftedAtom(self._HandEmpty, [spot]),
LiftedAtom(self._ReachablePlatform, [spot, platform]),
LiftedAtom(self._InViewPlatform, [spot, platform])
}
self._GraspPlatformLeashOp = STRIPSOperator("GraspPlatformLeash",
[spot, platform], preconds,
Expand Down Expand Up @@ -892,7 +895,7 @@ def predicates(self) -> Set[Predicate]:
self._HoldingBag, self._HoldingPlatformLeash, self._ReachableBag,
self._ReachablePlatform, self._ReachableSurface,
self._SurfaceTooHigh, self._SurfaceNotTooHigh, self._PlatformNear,
self._notHandEmpty, self._InViewTool, self._OnFloor
self._notHandEmpty, self._InViewTool, self._InViewPlatform, self._OnFloor
}

@staticmethod
Expand Down Expand Up @@ -993,6 +996,22 @@ def _tool_in_view_classifier(state: State,
objects: Sequence[Object]) -> bool:
_, tool = objects
return state.get(tool, "in_view") > 0.5

@staticmethod
def _platform_in_view_classifier(state: State,
objects: Sequence[Object]) -> bool:
_, platform = objects
return state.get(platform, "in_view") > 0.5

@staticmethod
def _platform_is_near(state: State,
objects: Sequence[Object]) -> bool:
platform, surface = objects
px = state.get(platform, "x")
py = state.get(platform, "y")
sx = state.get(surface, "x")
sy = state.get(surface, "y")
return np.allclose(np.array([px, py]),np.array([sx, sy]),atol=0.7)

@classmethod
def get_name(cls) -> str:
Expand All @@ -1004,7 +1023,7 @@ def percept_predicates(self) -> Set[Predicate]:
return {
self._HandEmpty, self._notHandEmpty, self._HoldingTool, self._On,
self._SurfaceTooHigh, self._SurfaceNotTooHigh, self._ReachableBag,
self._ReachablePlatform, self._InViewTool
self._ReachablePlatform, self._InViewTool, self._InViewPlatform
}

def _get_initial_nonpercept_atoms(self) -> Set[GroundAtom]:
Expand Down
4 changes: 2 additions & 2 deletions predicators/ground_truth_models/spot/nsrts.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,9 +131,9 @@ def _drag_sampler(spot_interface: _SpotInterface, state: State,
fiducial_in_robot_frame = world_to_robot.inverse() * world_fiducial
fiducial_pose = list(fiducial_in_robot_frame) + [spot_interface.hand_z]

dx, dy, force = 0.0, 0.0, 2.0
dx, dy = 0.0, -0.4

return np.array([np.clip(fiducial_pose[0] + dx, -0.5, 0.5), np.clip(fiducial_pose[1] + dy, -0.5, 0.5), force])
return np.array([fiducial_pose[0] + dx, fiducial_pose[1] + dy])


_NAME_TO_SPOT_INTERFACE_SAMPLER = {
Expand Down
2 changes: 1 addition & 1 deletion predicators/perception/spot_bike_perceiver.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -164,7 +164,7 @@ def _create_state(self) -> State:
"y": y,
"z": z,
}
if obj.type.name == "tool":
if obj.type.name == "tool" or obj.type.name == "platform":
# Detect if the object is in view currently.
if obj in self._known_objects_in_hand_view:
in_view_val = 1.0
Expand Down
1 change: 0 additions & 1 deletion predicators/planning.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1147,7 +1147,6 @@ def run_task_plan_once(
else:
raise ValueError("Unrecognized sesame_task_planner: "
f"{CFG.sesame_task_planner}")
import ipdb; ipdb.set_trace()
return plan, atoms_seq, metrics


Expand Down
132 changes: 32 additions & 100 deletions predicators/spot_utils/spot_utils.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -443,7 +443,7 @@ def params_spaces(self) -> Dict[str, Box]:
"navigate": Box(-5.0, 5.0, (3, )),
"grasp": Box(-1.0, 1.0, (4, )),
"placeOnTop": Box(-5.0, 5.0, (3, )),
"drag": Box(-5.0, 5.0, (3, )),
"drag": Box(-5.0, 5.0, (2, )),
"noop": Box(0, 1, (0, ))
}

Expand Down Expand Up @@ -620,7 +620,7 @@ def placeOntopController(self, objs: Sequence[Object],
def dragController(self, objs: Sequence[Object], params: Array) -> None:
"""Drag Controller."""
print("Drag", objs)
assert len(params) == 3 # [x, y, z] vector for direction and [f] force
assert len(params) == 2 # [x, y] vector for direction
self.drag_impedence_control(params)
time.sleep(1.0)
self.stow_arm()
Expand Down Expand Up @@ -946,6 +946,8 @@ def hand_movement(
open_gripper: bool = True,
relative_to_default_pose: bool = True,
keep_hand_pose: bool = True,
keep_body_pose: bool = False,
clip_z: bool = True,
angle: Tuple[float, float, float,
float] = (np.cos(np.pi / 4), 0, np.sin(np.pi / 4), 0)
) -> None:
Expand Down Expand Up @@ -981,8 +983,12 @@ def hand_movement(
# robot respectively.
x_clipped = np.clip(x, self.hand_x_bounds[0], self.hand_x_bounds[1])
y_clipped = np.clip(y, self.hand_y_bounds[0], self.hand_y_bounds[1])
z_clipped = np.clip(z, self.hand_z_bounds[0], self.hand_z_bounds[1])
self.relative_move((x - x_clipped), (y - y_clipped), 0.0)
if clip_z:
z_clipped = np.clip(z, self.hand_z_bounds[0], self.hand_z_bounds[1])
else:
z_clipped = z
if not keep_body_pose:
self.relative_move((x - x_clipped), (y - y_clipped), 0.0)
x = x_clipped
y = y_clipped
z = z_clipped
Expand Down Expand Up @@ -1134,107 +1140,33 @@ def navigate_to_position(self, params: Array) -> None:

def drag_impedence_control(self, params: Array) -> None:
"""Simple drag impedence controller."""
assert len(params) == 3

# First, let's pick a task frame that is in front of the robot.
assert len(params) == 2
# Move Body
robot_state = self.robot_state_client.get_robot_state()
body_T_hand = get_a_tform_b(
robot_state.kinematic_state.transforms_snapshot,
BODY_FRAME_NAME, "hand")
odom_T_grav_body = get_a_tform_b(
robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME,
GRAV_ALIGNED_BODY_FRAME_NAME)

odom_T_gpe = get_a_tform_b(
robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME,
GROUND_PLANE_FRAME_NAME)

# Get the frame on the ground right underneath the center of the body.
odom_T_ground_body = odom_T_grav_body
odom_T_ground_body.z = odom_T_gpe.z

# Now, get the frame that is 60cm in front of this frame.
odom_T_task = odom_T_ground_body * math_helpers.SE3Pose(
x=0.6, y=0, z=0, rot=math_helpers.Quat(w=1, x=0, y=0, z=0))

# Now, let's set our tool frame to be the tip of the robot's
# bottom jaw. Flip the orientation so that when the hand is
# pointed downwards, the tool's z-axis is pointed upward.
wr1_T_tool = math_helpers.SE3Pose(
0.23589, 0, -0.03943, math_helpers.Quat.from_pitch(-math.pi / 2))

# Now, let's move along the surface of the ground, exerting a downward
# force while dragging the arm sideways.
robot_cmd = robot_command_pb2.RobotCommand()
impedance_cmd = robot_cmd.synchronized_command.\
arm_command.arm_impedance_command

# Set up our root frame, task frame, and tool frame.
impedance_cmd.root_frame_name = ODOM_FRAME_NAME
impedance_cmd.root_tform_task.CopyFrom(odom_T_task.to_proto())
impedance_cmd.wrist_tform_tool.CopyFrom(wr1_T_tool.to_proto())

# Compute unit vector
x = params[0]
y = params[1]
force = params[2]
unit_vec = np.array([x, y]) / np.linalg.norm(np.array([x, y]))

# Set up downward force.
impedance_cmd.feed_forward_wrench_at_tool_in_desired_tool.\
force.x = unit_vec[0] * force
impedance_cmd.feed_forward_wrench_at_tool_in_desired_tool.\
force.y = unit_vec[0] * force # Newtons
impedance_cmd.feed_forward_wrench_at_tool_in_desired_tool.\
force.z = 0
impedance_cmd.feed_forward_wrench_at_tool_in_desired_tool.\
torque.x = 0
impedance_cmd.feed_forward_wrench_at_tool_in_desired_tool.\
torque.y = 0
impedance_cmd.feed_forward_wrench_at_tool_in_desired_tool.\
torque.z = 0

# Set up stiffness and damping matrices. Note that we've set the
# stiffness in the z-axis to 0 since we're commanding a constant
# downward force, regardless of where the tool is in z relative to
# our `desired_tool` frame.
impedance_cmd.diagonal_stiffness_matrix.CopyFrom(
geometry_pb2.Vector(values=[500, 500, 0, 60, 60, 60]))
impedance_cmd.diagonal_damping_matrix.CopyFrom(
geometry_pb2.Vector(values=[2.5, 2.5, 2.5, 1.0, 1.0, 1.0]))

# Set up our `desired_tool` trajectory. This is where we want the
# tool to be with respect to the task frame. The stiffness we set will
# drag the tool towards `desired_tool`.
# Transform
qw, qx, qy, qz = body_T_hand.rot.w, body_T_hand.rot.x,\
body_T_hand.rot.y, body_T_hand.rot.z
flat_body_Q_hand = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz)
flat_body_T_hand = geometry_pb2.SE3Pose(position=geometry_pb2.Vec3(x=body_T_hand.x, y=body_T_hand.y, z=body_T_hand.z),
rotation=flat_body_Q_hand)
odom_T_flat_body = get_a_tform_b(
robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME,
GRAV_ALIGNED_BODY_FRAME_NAME)
odom_T_hand = odom_T_flat_body * flat_body_T_hand
#
GRAV_ALIGNED_BODY_FRAME_NAME, "hand")
self.relative_move(dx=params[0] - body_T_hand.x, dy=params[1] - body_T_hand.y, dyaw=0.0)

traj = impedance_cmd.task_tform_desired_tool
pt1 = traj.points.add()
pt1.time_since_reference.CopyFrom(seconds_to_duration(2.0))
pt1.pose.CopyFrom(odom_T_hand)
pt2 = traj.points.add()
pt2.time_since_reference.CopyFrom(seconds_to_duration(4.0))
pt2.pose.CopyFrom(
math_helpers.SE3Pose(params[0], params[1], odom_T_hand.z,
math_helpers.Quat(odom_T_hand.rot.w, odom_T_hand.rot.x, odom_T_hand.rot.y, odom_T_hand.rot.z)).to_proto())

# Execute the impedance command
self.robot_command_client.robot_command(robot_cmd)
time.sleep(5.0)
# Move Hand
robot_state = self.robot_state_client.get_robot_state()
body_T_hand = get_a_tform_b(
robot_state.kinematic_state.transforms_snapshot,
GRAV_ALIGNED_BODY_FRAME_NAME, "hand")
self.hand_movement(np.array([params[0], params[1], body_T_hand.z]),
keep_hand_pose=True,
keep_body_pose=True,
clip_z=False,
relative_to_default_pose=False,
open_gripper=False)
# Open Gripper
gripper_command = RobotCommandBuilder.\
claw_gripper_open_fraction_command(1.0)

start_time = time.perf_counter()
if (time.perf_counter() - start_time) > COMMAND_TIMEOUT:
logging.info("Timed out waiting for movement to execute!")
# Send the request
cmd_id = self.robot_command_client.robot_command(gripper_command)
self.robot.logger.debug('Opening Gripper.')
time.sleep(1)

# Stow the arm
stow_cmd = RobotCommandBuilder.arm_stow_command()
Expand Down

0 comments on commit abf131b

Please sign in to comment.