diff --git a/predicators/envs/spot_env.py b/predicators/envs/spot_env.py index 01c9b07b35..b3304e55cb 100644 --- a/predicators/envs/spot_env.py +++ b/predicators/envs/spot_env.py @@ -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 @@ -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) @@ -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 @@ -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], @@ -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], @@ -646,7 +646,7 @@ 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(), @@ -654,10 +654,11 @@ def __init__(self, use_gui: bool = True) -> None: # 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(), @@ -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) @@ -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]), @@ -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, @@ -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 @@ -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: @@ -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]: diff --git a/predicators/ground_truth_models/spot/nsrts.py b/predicators/ground_truth_models/spot/nsrts.py index c61e9f3e9e..4891deea19 100644 --- a/predicators/ground_truth_models/spot/nsrts.py +++ b/predicators/ground_truth_models/spot/nsrts.py @@ -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 = { diff --git a/predicators/perception/spot_bike_perceiver.py b/predicators/perception/spot_bike_perceiver.py index c0b5568e42..cb89b249e3 100644 --- a/predicators/perception/spot_bike_perceiver.py +++ b/predicators/perception/spot_bike_perceiver.py @@ -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 diff --git a/predicators/planning.py b/predicators/planning.py index a056e1fe76..a6017372bc 100644 --- a/predicators/planning.py +++ b/predicators/planning.py @@ -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 diff --git a/predicators/spot_utils/spot_utils.py b/predicators/spot_utils/spot_utils.py index 61222a3768..e772550d3f 100644 --- a/predicators/spot_utils/spot_utils.py +++ b/predicators/spot_utils/spot_utils.py @@ -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, )) } @@ -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() @@ -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: @@ -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 @@ -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()