Package edu.tufts.hrilab.moveit

Class MoveItComponent

java.lang.Object
edu.tufts.hrilab.diarc.DiarcComponent
edu.tufts.hrilab.moveit.MoveItComponent
All Implemented Interfaces:
ArmInterface, LearningInterface, MoveItInterface
Direct Known Subclasses:
FetchComponent, PR2Component
public class MoveItComponent extends DiarcComponent implements MoveItInterface

  • Field Details

    • configName

      protected String configName
      If you're extending this class you can set this one to the config name so it's the default.

    • defaultConfigDir

      protected String defaultConfigDir

    • moveItConfig

      protected MoveItConfig moveItConfig

    • grippers

      protected Map<String,GenericManipulator> grippers

    • endEffectorNames

      protected Map<String,String> endEffectorNames

    • jointNames

      protected Map<String,String[]> jointNames

    • eeTransform

      protected javax.vecmath.Matrix4d eeTransform

    • recordedTrajectories

      protected Map<String,edu.tufts.hrilab.diarcros.msg.trajectory_msgs.JointTrajectory> recordedTrajectories
      For storing and recording trajectories.

    • trajectoriesFile

      protected String trajectoriesFile

    • recordingTrajectory

      protected volatile boolean recordingTrajectory

    • moveGroup

      protected GenericMoveGroup moveGroup
      diarcros nodes.

    • jointStateSub

      protected JointStateSub jointStateSub

    • poses

      protected Map<String,edu.tufts.hrilab.diarcros.msg.moveit_msgs.RobotState> poses
      Pose name to robot joint states. Populated from JSON config file.

    • initStartPose

      protected Symbol initStartPose
      Name of initial pose, if one has been specified. NOTE: this pose name must be in the json configuration.

    • eePoses

      protected Map<String,org.apache.commons.lang3.tuple.Pair<javax.vecmath.Point3d,javax.vecmath.Quat4d>> eePoses
      Pose name to robot ee states. Populated at runtime by saving ee poses.

  • Constructor Details

    • MoveItComponent

      public MoveItComponent()
      Default constructor.

  • 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

    • additionalUsageInfo

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

    • parseArgs

      protected void parseArgs(org.apache.commons.cli.CommandLine cmdLine)
      Description copied from class: DiarcComponent
      Called directly after construction to pass runtime values that will override default values. This should parse all the options that additionalUsageInfo provides.zs
      Overrides:
      parseArgs in class DiarcComponent

    • connectToMoveGroup

      protected void connectToMoveGroup()
      To add more ROS versions: - node gen on the movegroup - Drop the generated file into diarcros/moveit/(version)/MoveGroup.java - Update the Ant target in the same way the other versions are built in src-built-targets - Make sure the new class extends GenericMoveGroup (your IDE may complain about duplicate class names, that's OK) - Potentially modify the generated file to include necessary functionality that was not automatically implemented (move_group.shutdown(), for example, is not automatically generated for Kinetic) With all of the functionality added, it should "just work": This setup looks for a package called edu.tufts.hrilab.diarcros.moveit.MoveGroup(), but only the MoveGroup corresponding to your ROS version should come up (thanks to those ant build targets).

    • addLocalStaticTransforms

      public void addLocalStaticTransforms(ai.thinkingrobots.trade.TRADEServiceInfo tsi)

    • 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

    • interrupt

      public boolean interrupt()

    • moveTo

      public Justification moveTo(String groupName, Symbol refId)
      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.

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

    • 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

    • moveToApproach

      protected boolean moveToApproach(String groupName, Grasp grasp, float offset)

    • moveTo

      protected boolean moveTo(String groupName, Grasp grasp, float grasp_offset)

    • pointTo

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

    • pointTo

      public boolean pointTo(String groupName, Symbol refId)
      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

    • notOneArm

      protected boolean notOneArm(String groupName)

    • moveTo

      public boolean 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(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.

    • moveToRelative

      public Justification moveToRelative(String groupName, 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:
      groupName - 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.

    • 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:

    • moveToCartesian

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

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

    • pushObject

      public boolean pushObject(String group_name, float position, MemoryObject object, edu.tufts.hrilab.diarcros.msg.geometry_msgs.Pose destination)

    • 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

    • getPoses

      protected List<edu.tufts.hrilab.diarcros.msg.geometry_msgs.Pose> getPoses(List<String> list_names)

    • getPoses

      protected List<edu.tufts.hrilab.diarcros.msg.geometry_msgs.Pose> getPoses(List<String> link_names, String base_link)

    • getRobotState

      protected edu.tufts.hrilab.diarcros.msg.moveit_msgs.RobotState getRobotState()

    • disableCollisionAvoidance

      public void disableCollisionAvoidance()

    • enableCollisionAvoidance

      public void enableCollisionAvoidance()

    • addCollisionObject

      public void addCollisionObject(edu.tufts.hrilab.diarcros.msg.geometry_msgs.Pose pose, String name, String parentLink, float box_x, float box_y, float box_z)

    • removeCollisionObject

      protected void removeCollisionObject(String id)

    • setObjectCollisions

      protected void setObjectCollisions(String obj_id, Boolean passes_through)

    • removeAllCollisionObjects

      protected boolean removeAllCollisionObjects()

    • attachCollisionObject

      protected void attachCollisionObject(String link_name, MemoryObject object, String second_link_name)

    • attachCollisionObject

      protected void attachCollisionObject(String link_name, MemoryObject object)

    • detachCollisionObject

      protected void detachCollisionObject(String link_name, Symbol refId)

    • detachCollisionObject

      protected void detachCollisionObject(String id)

    • detachAllAttachedCollisionObjects

      protected boolean detachAllAttachedCollisionObjects()

    • executeMotionPlanRequest

      public boolean executeMotionPlanRequest(edu.tufts.hrilab.diarcros.msg.moveit_msgs.MotionPlanRequest req)

    • recordPose

      public boolean 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.

    • recordEEPose

      public boolean recordEEPose(Symbol poseName)

    • goToEEPose

      public boolean goToEEPose(Symbol poseName)

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

    • goToPose

      public boolean goToPose(String groupName, Symbol poseName)
      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:
      groupName - which arm(s) should move (rightArm, leftArm, arms, etc)
      poseName - 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

    • 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

    • savePosesToFile

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

    • saveEEPosesToFile

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

    • 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

    • 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

    • publishPointCloudToRos

      public boolean publishPointCloudToRos(byte[] depthData)
      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:
      depthData - A byte array containing the point cloud data.
      Returns:
      true on success.

    • isMoveGroupConnected

      public boolean isMoveGroupConnected()

    • reconnectToMoveGroup

      public void reconnectToMoveGroup()

    • moveGripper

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

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

    • startRecordingTrajectory

      public void startRecordingTrajectory(String trajectoryName)
      This implementation of recording a trajectory is to save a bunch of trajectory points and remember the order, allowing them to be replayed by going from point to point to point.
      Specified by:
      startRecordingTrajectory in interface ArmInterface
      Parameters:
      trajectoryName -

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

    • 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

    • getGripperPosition

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

    • moveGrippers

      public boolean moveGrippers(String group_name, float f)

    • 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:

    • pour

      public boolean pour(boolean forward)

    • moveTo

      public boolean moveTo(String groupName, javax.vecmath.Point3d point, javax.vecmath.Quat4d orientation, int maxAttempts)

    • 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