Package edu.tufts.hrilab.arm

Class MockArmComponent

java.lang.Object
edu.tufts.hrilab.diarc.DiarcComponent
edu.tufts.hrilab.arm.MockArmComponent
All Implemented Interfaces:
MockArmInterface, ArmInterface
Direct Known Subclasses:
MockUR5eComponent
public class MockArmComponent extends DiarcComponent implements MockArmInterface

  • Constructor Details

    • MockArmComponent

      public MockArmComponent()

  • Method Details

    • init

      protected void init()
      Description copied from class: DiarcComponent
      Perform any component initialization. This should not be used for setting local fields to default values as was the case in DIARC. This method is called after the constructor, and after parseArgs. Setting default values should be done in field declaration or in the constructor.
      Overrides:
      init in class DiarcComponent

    • moveTo

      public Justification moveTo(String groupName, 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 groupName, 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(javax.vecmath.Point3d point_l, javax.vecmath.Quat4d orientation_l, javax.vecmath.Point3d point_r, javax.vecmath.Quat4d orientation_r)

    • moveTo

      public Justification moveTo(String groupName, Symbol refId, List<? extends Term> 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
      constraints - a list of predicate constraints
      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.

    • pointTo

      public Justification pointTo(String groupName, 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

    • 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 groupName)
      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 linkName)
      Description copied from interface: ArmInterface
      Get the point and orientation of a link on the robot.
      Specified by:
      getPose in interface ArmInterface

    • recordPose

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

    • saveEEPosesToFile

      public Justification 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

    • savePosesToFile

      public Justification 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

    • goToPose

      public Justification goToPose(Symbol poseName)
      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:
      poseName - name of pose to move into
      Returns:
      true on success, false on failure. This function blocks.

    • goToPose

      public Justification goToPose(String poseName, Symbol groupName)
      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:
      poseName - which arm(s) should move (rightArm, leftArm, arms, etc)
      groupName - name of pose to move into
      Returns:
      true on success, false on failure. This function blocks.

    • 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 Justification 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.

    • saveTrajectoriesToFile

      public Justification 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 Justification 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

    • startPouring

      public boolean startPouring(double ang)
      Specified by:
      startPouring in interface MockArmInterface

    • startPouring

      public boolean startPouring(Symbol obj)
      Specified by:
      startPouring in interface MockArmInterface

    • stopPouring

      public boolean stopPouring(Symbol obj)
      Specified by:
      stopPouring in interface MockArmInterface

    • stopPouring

      public boolean stopPouring()
      Specified by:
      stopPouring in interface MockArmInterface

    • moveAbove

      public boolean moveAbove(Symbol s, Symbol s2)
      Specified by:
      moveAbove in interface MockArmInterface

    • additionalUsageInfo

      protected List<org.apache.commons.cli.Option> additionalUsageInfo()
      Description copied from class: DiarcComponent
      Override this method to define command line options available in sub-class. This should be paired with a parseArgs implementation.
      Overrides:
      additionalUsageInfo in class DiarcComponent
      Returns:

    • getMemoryObject

      protected MemoryObject getMemoryObject(Symbol objectRef)

    • convertRPY

      public static javax.vecmath.Quat4d convertRPY(double roll, double pitch, double yaw)

    • goToEEPose

      public boolean goToEEPose(String poseName)