Package edu.tufts.hrilab.interfaces
Interface ArmInterface
- All Known Subinterfaces:
FetchInterface
,FetchItInterface
,MockArmInterface
,MoveItInterface
,PR2Interface
- All Known Implementing Classes:
FetchComponent
,FetchItComponent
,MockArmComponent
,MockFetchComponent
,MockFetchItComponent
,MockMoveItComponent
,MockPR2Component
,MockSpotComponent
,MockUR5eComponent
,MoveItComponent
,PR2Component
public interface ArmInterface
-
Method Summary
Modifier and TypeMethodDescriptioncloseGripper
(String groupName) boolean
executeTrajectory
(String trajectoryName) Execute a previously recorded trajectory.org.apache.commons.lang3.tuple.Pair<javax.vecmath.Point3d,
javax.vecmath.Quat4d> Convenience method of getPose that finds the end-effector link based on the passed in groupName.float
getGripperPosition
(String groupName) org.apache.commons.lang3.tuple.Pair<javax.vecmath.Point3d,
javax.vecmath.Quat4d> Get the point and orientation of a link on the robot.boolean
Move robot to previously recorded robot state/pose.boolean
Move robot to previously recorded robot state/pose, using only the specified group name (e.g., right_arm).boolean
goToStartPose
(boolean safe) Go to the start pose saved in the JSON file.graspObject
(String groupName, Symbol refId, float position) Close gripper(s) to specified amount and attach MemoryObject as a collision object.void
loadEEPosesFromFile
(String filename) Load poses previously saved to file and make them accessible via goToPose.void
loadPosesFromFile
(String filename) Load poses previously saved to file and make them accessible via goToPose.void
loadTrajectoriesFromFile
(String filename) Load trajectories from a file that was generated via saveTrajectoriesToFile().boolean
moveGripper
(String groupName, float meters) Move arms(s) to location of chosen grasp specified in MemoryObject.[ Move arms(s) to pose defined by the refId and constrained by a set of constraints.boolean
Move the groupName to a pose.boolean
moveTo
(String groupName, javax.vecmath.Point3d point_l, javax.vecmath.Quat4d orientation_l, javax.vecmath.Point3d point_r, javax.vecmath.Quat4d orientation_r) Move the groupName to specified poses(s).moveToRelative
(String groupName, javax.vecmath.Point3d point, javax.vecmath.Quat4d orientation) Move a certain amount from the current location, without any squeezing or releasing.openGripper
(String groupName) boolean
Point a groupName to an object.pressObject
(String group_name, Symbol refID) Presses an object based off a given symbolpressObject
(String group_name, javax.vecmath.Point3d object_location, javax.vecmath.Quat4d object_orientation) Presses an object based off a given poseboolean
recordPose
(Symbol poseName) Record current robot state (i.e., joint positions), and name it.releaseObject
(String groupName, Symbol refId, float position) Move arm(s) and open gripper(s) to specified amount and detach MemoryObject as collision object.boolean
saveEEPosesToFile
(String filename) Write the poses recorded via recordPose to file.boolean
savePosesToFile
(String filename) Write the poses recorded via recordPose to file.boolean
saveTrajectoriesToFile
(String filename) Take all trajectories saved via startRecordingTrajectory and save them to a file for later.void
startRecordingTrajectory
(String trajectoryName) Start recording a trajectory.void
Stop recording the currently running trajectory recording.
-
Method Details
-
moveTo
Move the groupName to a pose. Uses the default coordinate frame as set by JSON (defaulting to base_link).- Returns:
- true on success, false on failure. This function blocks.
-
moveTo
boolean moveTo(String groupName, javax.vecmath.Point3d point_l, javax.vecmath.Quat4d orientation_l, javax.vecmath.Point3d point_r, javax.vecmath.Quat4d orientation_r) 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.
- Returns:
- true on success, false on failure. This function blocks.
-
moveTo
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.- Returns:
- true on success, false on failure. This function blocks.
-
moveTo
[ 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.- Parameters:
constraints
- a list of predicate constraints- Returns:
- true on success, false on failure. This function blocks.
-
moveToRelative
Justification moveToRelative(String groupName, javax.vecmath.Point3d point, javax.vecmath.Quat4d orientation) Move a certain amount from the current location, without any squeezing or releasing.- Parameters:
groupName
- the name of the arm or arms to movepoint
- the amount of change in base frame the object is movedorientation
- 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
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.- Returns:
- true on success, false on failure. This function blocks.
-
releaseObject
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.- Returns:
- true on success, false on failure. This function blocks.
-
pointTo
Point a groupName to an object.- Returns:
- true on success, false on failure. This function blocks
-
getEEPose
org.apache.commons.lang3.tuple.Pair<javax.vecmath.Point3d,javax.vecmath.Quat4d> getEEPose(String groupName) Convenience method of getPose that finds the end-effector link based on the passed in groupName.- Parameters:
groupName
-- Returns:
-
getPose
org.apache.commons.lang3.tuple.Pair<javax.vecmath.Point3d,javax.vecmath.Quat4d> getPose(String linkName) Get the point and orientation of a link on the robot. -
recordPose
Record current robot state (i.e., joint positions), and name it.- Parameters:
poseName
- The name of the pose to be saved.- Returns:
- true on success, false on failure. This function blocks.
-
saveEEPosesToFile
Write the poses recorded via recordPose to file.- Returns:
- true on success
-
loadEEPosesFromFile
Load poses previously saved to file and make them accessible via goToPose. -
savePosesToFile
Write the poses recorded via recordPose to file.- Returns:
- true on success
-
loadPosesFromFile
Load poses previously saved to file and make them accessible via goToPose. -
goToPose
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.- 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.
-
goToPose
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.- Parameters:
poseName
- name of pose to move into- Returns:
- true on success, false on failure. This function blocks.
-
goToStartPose
boolean goToStartPose(boolean safe) Go to the start pose saved in the JSON file.- Returns:
- success
-
startRecordingTrajectory
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.- Parameters:
trajectoryName
- The name of the trajectory to be saved.
-
stopRecordingTrajectory
void stopRecordingTrajectory()Stop recording the currently running trajectory recording. -
executeTrajectory
Execute a previously recorded trajectory.- Returns:
- true on success, false on failure. This function blocks.
-
saveTrajectoriesToFile
Take all trajectories saved via startRecordingTrajectory and save them to a file for later.- Parameters:
filename
- Path to file- Returns:
- true on success, false on failure.
-
loadTrajectoriesFromFile
Load trajectories from a file that was generated via saveTrajectoriesToFile(). The loaded trajectories will then be executable via executeTrajectory(). -
closeGripper
-
openGripper
-
moveGripper
-
getGripperPosition
-
pressObject
Justification pressObject(String group_name, javax.vecmath.Point3d object_location, javax.vecmath.Quat4d object_orientation) Presses an object based off a given pose- Parameters:
group_name
- The arm group to do the pressing (i.e. "arm")object_location
- The location of the object relative to the base linkobject_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
Presses an object based off a given symbol- 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
-