Package edu.tufts.hrilab.pr2

Class MockPR2Component

java.lang.Object
edu.tufts.hrilab.diarc.DiarcComponent
edu.tufts.hrilab.pr2.MockPR2Component
All Implemented Interfaces:
ArmInterface, LearningInterface, MoveItInterface, PR2Interface
public class MockPR2Component extends DiarcComponent implements PR2Interface

  • Constructor Details

    • MockPR2Component

      public MockPR2Component()

  • Method Details

    • moveTo

      public boolean moveTo(String group_name, javax.vecmath.Point3d point_l, javax.vecmath.Quat4d orientation_l, javax.vecmath.Point3d point_r, javax.vecmath.Quat4d orientation_r)
      Description copied from interface: ArmInterface
      Move the groupName to specified poses(s). If group name only contains a single arm, use a different moveTo().

      Note that there's a baked-in assumption that the only two-arned nmachines will use a group name that is checked against in the 'isOneArm()' method (currently just "arms"). Update this function if this does not describe your use case. It also assumes that groupName "arms" can be described as independent movement of groupName "left_arm" and groupName "right_arm". At this point in time this is a safe assumption because of the state of the PR2's JSON config. point_l, orientation_l refer to the left arm, point_r and orientation_r refer to the right.

      Specified by:
      moveTo in interface ArmInterface
      Returns:
      true on success, false on failure. This function blocks.

    • moveTo

      public boolean moveTo(String group_name, javax.vecmath.Point3d point, javax.vecmath.Quat4d orientation)
      Description copied from interface: ArmInterface
      Move the groupName to a pose. Uses the default coordinate frame as set by JSON (defaulting to base_link).
      Specified by:
      moveTo in interface ArmInterface
      Returns:
      true on success, false on failure. This function blocks.

    • moveTo

      public Justification moveTo(String group_name, Symbol objectRef)
      Description copied from interface: ArmInterface
      Move arms(s) to location of chosen grasp specified in MemoryObject. Uses the default "grasp_point(X)" constraint to select grasp points. Assumes default coordinate frame as set by JSON, defaulting to base_link. Note: gripper should already be open/closed before calling this method. This is an alternative method to moveTo that takes in POWER ref instead of MemoryObject.
      Specified by:
      moveTo in interface ArmInterface
      Returns:
      true on success, false on failure. This function blocks.

    • moveToJointPositions

      public boolean moveToJointPositions(String[] jointNames, double[] positions)
      Specified by:
      moveToJointPositions in interface MoveItInterface

    • moveToJointPositions

      public boolean moveToJointPositions(String[] joint_names, double[] positions, double[] velocities, double[] efforts)
      Specified by:
      moveToJointPositions in interface MoveItInterface

    • moveTo

      public Justification moveTo(String group_name, Symbol objectRef, List<? extends Term> grasp_constraints)
      Description copied from interface: ArmInterface
      [ Move arms(s) to pose defined by the refId and constrained by a set of constraints. Assumes base_link coordinate frame. Note: gripper should already be open/closed before calling this method.
      Specified by:
      moveTo in interface ArmInterface
      grasp_constraints - a list of predicate constraints
      Returns:
      true on success, false on failure. This function blocks.

    • moveToRelative

      public Justification moveToRelative(String group_name, javax.vecmath.Point3d point, javax.vecmath.Quat4d orientation)
      Description copied from interface: ArmInterface
      Move a certain amount from the current location, without any squeezing or releasing.
      Specified by:
      moveToRelative in interface ArmInterface
      Parameters:
      group_name - the name of the arm or arms to move
      point - the amount of change in base frame the object is moved
      orientation - the amount to rotate the object in goal state from its current rotation in base frame
      Returns:
      success or failure to move. This function blocks.

    • graspObject

      public Justification graspObject(String groupName, Symbol refId, float position)
      Description copied from interface: ArmInterface
      Close gripper(s) to specified amount and attach MemoryObject as a collision object. Alternative method to graspObject that takes in POWER ref instead of MemoryObject.
      Specified by:
      graspObject in interface ArmInterface
      Returns:
      true on success, false on failure. This function blocks.

    • releaseObject

      public Justification releaseObject(String groupName, Symbol refId, float position)
      Description copied from interface: ArmInterface
      Move arm(s) and open gripper(s) to specified amount and detach MemoryObject as collision object. Alternative method to releaseObject that takes in POWER ref instead of MemoryObject.
      Specified by:
      releaseObject in interface ArmInterface
      Returns:
      true on success, false on failure. This function blocks.

    • getEEPose

      public org.apache.commons.lang3.tuple.Pair<javax.vecmath.Point3d,javax.vecmath.Quat4d> getEEPose(String group_name)
      Description copied from interface: ArmInterface
      Convenience method of getPose that finds the end-effector link based on the passed in groupName.
      Specified by:
      getEEPose in interface ArmInterface
      Returns:

    • getPose

      public org.apache.commons.lang3.tuple.Pair<javax.vecmath.Point3d,javax.vecmath.Quat4d> getPose(String link_name)
      Description copied from interface: ArmInterface
      Get the point and orientation of a link on the robot.
      Specified by:
      getPose in interface ArmInterface

    • moveToCartesian

      public boolean moveToCartesian(String group_name, javax.vecmath.Point3d point, javax.vecmath.Quat4d orientation)
      Specified by:
      moveToCartesian in interface MoveItInterface

    • pointHeadTo

      public boolean pointHeadTo(Symbol objectRef)
      Description copied from interface: PR2Interface
      Point head in direction of target object in base_link coordinate frame. Alternative method to pointHeadTo that takes in POWER ref instead of MemoryObject.
      Specified by:
      pointHeadTo in interface PR2Interface
      Returns:

    • pointHeadTo

      public boolean pointHeadTo(javax.vecmath.Point3d target_point)
      Description copied from interface: PR2Interface
      Point head in direction of target point in base_link coordinate frame.
      Specified by:
      pointHeadTo in interface PR2Interface
      Returns:

    • pointTo

      public boolean pointTo(String group_name, Symbol objectRef)
      Description copied from interface: ArmInterface
      Point a groupName to an object.
      Specified by:
      pointTo in interface ArmInterface
      Returns:
      true on success, false on failure. This function blocks

    • pointTo

      public boolean pointTo(String group_name, javax.vecmath.Point3d targetLocation)
      Specified by:
      pointTo in interface MoveItInterface

    • setTorsoPosition

      public boolean setTorsoPosition(double position)
      Description copied from interface: PR2Interface
      Move the torso to desired joint position. TODO: replace this with the more general setJointPosition(String joint, double position) method in MoveItComponent
      Specified by:
      setTorsoPosition in interface PR2Interface
      Parameters:
      position - all the way down is 0.0, and all the way up is approximately 0.3
      Returns:

    • goToStartPose

      public boolean goToStartPose(boolean safe)
      Description copied from interface: ArmInterface
      Go to the start pose saved in the JSON file.
      Specified by:
      goToStartPose in interface ArmInterface
      Returns:
      success

    • recordPose

      public boolean recordPose(Symbol pose_name)
      Description copied from interface: ArmInterface
      Record current robot state (i.e., joint positions), and name it.
      Specified by:
      recordPose in interface ArmInterface
      Parameters:
      pose_name - The name of the pose to be saved.
      Returns:
      true on success, false on failure. This function blocks.

    • saveEEPosesToFile

      public boolean saveEEPosesToFile(String filename)
      Description copied from interface: ArmInterface
      Write the poses recorded via recordPose to file.
      Specified by:
      saveEEPosesToFile in interface ArmInterface
      Returns:
      true on success

    • loadEEPosesFromFile

      public void loadEEPosesFromFile(String filename)
      Description copied from interface: ArmInterface
      Load poses previously saved to file and make them accessible via goToPose.
      Specified by:
      loadEEPosesFromFile in interface ArmInterface

    • goToPose

      public boolean goToPose(Symbol pose_name)
      Description copied from interface: ArmInterface
      Move robot to previously recorded robot state/pose. Currently set to ignore gripper joints so it will leave grippers in current state of open/closed.
      Specified by:
      goToPose in interface ArmInterface
      Parameters:
      pose_name - name of pose to move into
      Returns:
      true on success, false on failure. This function blocks.

    • goToStartPose

      public boolean goToStartPose()
      Specified by:
      goToStartPose in interface MoveItInterface

    • goToPose

      public boolean goToPose(String pose_name, Symbol group_name)
      Description copied from interface: ArmInterface
      Move robot to previously recorded robot state/pose, using only the specified group name (e.g., right_arm). Currently set to ignore gripper joints so it will leave grippers in current state of open/closed.
      Specified by:
      goToPose in interface ArmInterface
      Parameters:
      pose_name - which arm(s) should move (rightArm, leftArm, arms, etc)
      group_name - name of pose to move into
      Returns:
      true on success, false on failure. This function blocks.

    • goToPoseNoPlanning

      public boolean goToPoseNoPlanning(String pose_name)
      Specified by:
      goToPoseNoPlanning in interface PR2Interface

    • startRecordingTrajectory

      public void startRecordingTrajectory(String trajectory_name)
      Description copied from interface: ArmInterface
      Start recording a trajectory. Trajectory recording can be started here, the robot can be physically manipulated, and then stopRecordingTrajectory can be called to stop the current recording.
      Specified by:
      startRecordingTrajectory in interface ArmInterface
      Parameters:
      trajectory_name - The name of the trajectory to be saved.

    • stopRecordingTrajectory

      public void stopRecordingTrajectory()
      Description copied from interface: ArmInterface
      Stop recording the currently running trajectory recording.
      Specified by:
      stopRecordingTrajectory in interface ArmInterface

    • executeTrajectory

      public boolean executeTrajectory(String trajectory_name)
      Description copied from interface: ArmInterface
      Execute a previously recorded trajectory.
      Specified by:
      executeTrajectory in interface ArmInterface
      Returns:
      true on success, false on failure. This function blocks.

    • savePosesToFile

      public boolean savePosesToFile(String filename)
      Description copied from interface: ArmInterface
      Write the poses recorded via recordPose to file.
      Specified by:
      savePosesToFile in interface ArmInterface
      Returns:
      true on success

    • loadPosesFromFile

      public void loadPosesFromFile(String filename)
      Description copied from interface: ArmInterface
      Load poses previously saved to file and make them accessible via goToPose.
      Specified by:
      loadPosesFromFile in interface ArmInterface

    • saveTrajectoriesToFile

      public boolean saveTrajectoriesToFile(String filename)
      Description copied from interface: ArmInterface
      Take all trajectories saved via startRecordingTrajectory and save them to a file for later.
      Specified by:
      saveTrajectoriesToFile in interface ArmInterface
      Parameters:
      filename - Path to file
      Returns:
      true on success, false on failure.

    • loadTrajectoriesFromFile

      public void loadTrajectoriesFromFile(String filename)
      Description copied from interface: ArmInterface
      Load trajectories from a file that was generated via saveTrajectoriesToFile(). The loaded trajectories will then be executable via executeTrajectory().
      Specified by:
      loadTrajectoriesFromFile in interface ArmInterface

    • closeGripper

      public Justification closeGripper(String groupName)
      Specified by:
      closeGripper in interface ArmInterface

    • openGripper

      public Justification openGripper(String groupName)
      Specified by:
      openGripper in interface ArmInterface

    • moveGripper

      public boolean moveGripper(String groupName, float position)
      Specified by:
      moveGripper in interface ArmInterface

    • getGripperPosition

      public float getGripperPosition(String groupName)
      Specified by:
      getGripperPosition in interface ArmInterface

    • pressObject

      public Justification pressObject(String group_name, javax.vecmath.Point3d object_location, javax.vecmath.Quat4d object_orientation)
      Description copied from interface: ArmInterface
      Presses an object based off a given pose
      Specified by:
      pressObject in interface ArmInterface
      Parameters:
      group_name - The arm group to do the pressing (i.e. "arm")
      object_location - The location of the object relative to the base link
      object_orientation - The orientation of the object relative to the base link with the normal vector coming out of the object
      Returns:
      Condition Justification stating if the action is successful or not

    • pressObject

      public Justification pressObject(String group_name, Symbol refID)
      Description copied from interface: ArmInterface
      Presses an object based off a given symbol
      Specified by:
      pressObject in interface ArmInterface
      Parameters:
      group_name - The arm group to do the pressing (i.e. "arm")
      refID - Symbol representing the object to be pressed
      Returns:
      Condition Justification stating if the action is successful or not

    • moveToJointPosition

      public boolean moveToJointPosition(String joint, double position)
      Description copied from interface: MoveItInterface
      Set the position of a single joint. All remaining joints will remain in the same position.
      Specified by:
      moveToJointPosition in interface MoveItInterface
      Parameters:
      joint - The name of the joint to manipulate, as found in `names` of /joint_states
      position - The goal position, as viewed in /joint_states
      Returns:
      true on success, false on failure. This function blocks.

    • enableCarryingConstraints

      public void enableCarryingConstraints()
      Description copied from interface: MoveItInterface
      Turn on carrying orientation constraints. This will apply to all moveTo and goToPose methods.
      Specified by:
      enableCarryingConstraints in interface MoveItInterface

    • disableCarryingConstraints

      public void disableCarryingConstraints()
      Description copied from interface: MoveItInterface
      Turn off carrying orientation constraints.
      Specified by:
      disableCarryingConstraints in interface MoveItInterface

    • publishPointCloudToRos

      public boolean publishPointCloudToRos(byte[] data)
      Description copied from interface: MoveItInterface
      Publish the compressed depth data to ROS after being decompressed and converted to proper point cloud form.
      Specified by:
      publishPointCloudToRos in interface MoveItInterface
      Parameters:
      data - A byte array containing the point cloud data.
      Returns:
      true on success.

    • learn

      public Justification learn(Term learningTerm)
      Description copied from interface: LearningInterface
      Use the passed in term to learn a new descriptor. The term must have two arguments, the first arg 'describing' the new thing-to-learn, and the second arg 'naming' the new thing-to-learn. The description must be composed entirely of things vision already knows about.
      Specified by:
      learn in interface LearningInterface
      Parameters:
      learningTerm - term specifying the thing to learn (e.g., instanceOf(X,Y))
      Returns:
      if thing-to-learn was successfully learned

    • unlearn

      public Justification unlearn(Term learningTerm)
      Description copied from interface: LearningInterface
      Un-learns things that have been learned by the learn method.
      Specified by:
      unlearn in interface LearningInterface
      Returns: