class PickupTriangleTest():
def __init__(self, arm=Constants.Arm.Left):
self.arm = arm
if self.arm == Constants.Arm.Left:
self.toolframe = Constants.Frames.LeftTool
else:
self.toolframe = Constants.Frames.RightTool
self.transFrame = Constants.Frames.Link0
self.rotFrame = self.toolframe
self.gripperControl = GripperControlClass(arm, tf.TransformListener())
self.imageDetector = ARImageDetectionClass()
self.homePose = self.imageDetector.getHomePose()
# ar_frame is the target
self.ar_frame = '/stereo_33'
self.objectPose = tfx.pose([0,0,0], frame=self.ar_frame)
tf_ar_to_link0 = tfx.lookupTransform(Constants.Frames.Link0, self.ar_frame, wait=5)
self.objectPose = tf_ar_to_link0 * self.objectPose
self.objectPose = tfx.pose(self.objectPose.position, tfx.tb_angles(-90,90,0))
self.objectPose.position.y -= .002
rospy.sleep(3)
def moveNearTriangle(self):
rospy.loginfo('Moving near the triangle')
deltaPose = Util.deltaPose(self.gripperpose, self.objectPose, self.transFrame, self.rotFrame)
deltaPose.position.z += .05
self.gripperControl.goToGripperPoseDelta(self.gripperControl.getGripperPose(frame=Constants.Frames.Link0), deltaPose)
while not self.gripperControl.isPaused() and not rospy.is_shutdown():
rospy.sleep(.1)
def servoNearTriangle(self):
rospy.loginfo('Servoing near the triangle')
while not self.imageDetector.hasFoundGripper(self.arm) and not rospy.is_shutdown():
rospy.loginfo('Searching for gripper')
rospy.sleep(.1)
rospy.loginfo('Found gripper')
gripperPose = self.imageDetector.getGripperPose(self.arm)
while not self.imageDetector.hasFoundObject() and not rospy.is_shutdown():
rospy.loginfo('Searching for object')
rospy.sleep(.1)
rospy.loginfo('Found object')
self.objectPose = self.imageDetector.getObjectPose() # in link 0
self.objectPose.pose.position.z += 0.02
transBound = .008
rotBound = float("inf")
maxMovement = .015
while not Util.withinBounds(gripperPose, self.objectPose, transBound, rotBound, self.transFrame, self.rotFrame) and not rospy.is_shutdown():
if self.gripperControl.isPaused():
rospy.sleep(1)
self.publishObjPose(self.objectPose)
if self.imageDetector.hasFoundNewGripper(self.arm):
rospy.loginfo('paused and found new gripper')
gripperPose = self.imageDetector.getGripperPose(self.arm)
deltaPose = tfx.pose(Util.deltaPose(gripperPose, self.objectPose, Constants.Frames.Link0, self.toolframe))
deltaPose.position = deltaPose.position.capped(maxMovement)
deltaPose0Link = tfx.pose(Util.deltaPose(gripperPose, self.objectPose, Constants.Frames.Link0, Constants.Frames.Link0))
deltaPose0Link.position = deltaPose.position.capped(maxMovement)
self.publishDeltaPose(deltaPose0Link, tfx.pose(gripperPose))
rospy.loginfo('pres enter to move')
raw_input()
self.gripperControl.goToGripperPoseDelta(self.gripperControl.getGripperPose(frame=Constants.Frames.Link0), deltaPose, ignoreOrientation=True, duration=4)
rospy.sleep(.1)
class ServoVsOpenVsIterTest():
def __init__(self, arm=Constants.Arm.Left):
self.arm = arm
if self.arm == Constants.Arm.Left:
self.toolframe = Constants.Frames.LeftTool
else:
self.toolframe = Constants.Frames.RightTool
self.transFrame = Constants.Frames.Link0
self.rotFrame = self.toolframe
self.gripperControl = GripperControlClass(arm, tf.TransformListener())
self.imageDetector = ARImageDetectionClass()
self.homePose = self.imageDetector.getHomePose()
self.gripperOpenCloseDuration = 2.5
rospy.sleep(3)
def goHome(self):
self.gripperControl.goToGripperPose(self.gripperControl.getGripperPose(frame=Constants.Frames.Link0), self.homePose.pose, ignoreOrientation=True)
while not self.gripperControl.isPaused() and not rospy.is_shutdown():
rospy.sleep(.1)
rospy.loginfo('Opening the gripper')
self.gripperControl.openGripper(duration=self.gripperOpenCloseDuration)
rospy.sleep(self.gripperOpenCloseDuration)
def openTest(self):
self.gripperControl.start()
self.goHome()
while not rospy.is_shutdown():
if self.imageDetector.hasFoundObject:
objectPose = self.imageDetector.getObjectPose()
ravenGripperPose = self.gripperControl.getGripperPose(Constants.Frames.Link0)
rospy.loginfo('Press enter')
raw_input()
self.gripperControl.goToGripperPose(ravenGripperPose, objectPose)
break
rospy.sleep(.1)
rospy.spin()
def oneServoTest(self):
self.gripperControl.start()
self.goHome()
while not rospy.is_shutdown():
if self.imageDetector.hasFoundGripper(self.arm) and self.imageDetector.hasFoundObject():
gripperPose = self.imageDetector.getGripperPose(self.arm)
objectPose = self.imageDetector.getObjectPose()
deltaPose = Util.deltaPose(gripperPose, objectPose, self.transFrame, self.rotFrame)
ravenGripperPose = self.gripperControl.getGripperPose(Constants.Frames.Link0)
rospy.loginfo('Press enter')
raw_input()
self.gripperControl.goToGripperPoseDelta(ravenGripperPose, deltaPose)
break
rospy.sleep(.1)
rospy.spin()
def multipleServoTest(self):
self.gripperControl.start()
self.goHome()
while not self.imageDetector.hasFoundGripper(self.arm) and not rospy.is_shutdown():
rospy.loginfo('Searching for gripper')
rospy.sleep(.1)
rospy.loginfo('Found gripper')
gripperPose = self.imageDetector.getGripperPose(self.arm)
while not self.imageDetector.hasFoundObject() and not rospy.is_shutdown():
rospy.loginfo('Searching for object')
rospy.sleep(.1)
rospy.loginfo('Found object')
self.objectPose = self.imageDetector.getObjectPose()
transBound = .008
rotBound = float("inf")
maxMovement = .015
while not Util.withinBounds(gripperPose, self.objectPose, transBound, rotBound, self.transFrame, self.rotFrame) and not rospy.is_shutdown():
if self.gripperControl.isPaused():
rospy.sleep(.5)
if self.imageDetector.hasFoundNewGripper(self.arm):
rospy.loginfo('paused and found new gripper')
gripperPose = self.imageDetector.getGripperPose(self.arm)
deltaPose = tfx.pose(Util.deltaPose(gripperPose, self.objectPose, Constants.Frames.Link0, self.toolframe))
deltaPose.position = deltaPose.position.capped(maxMovement)
self.gripperControl.goToGripperPoseDelta(self.gripperControl.getGripperPose(frame=Constants.Frames.Link0), deltaPose)
rospy.sleep(.1)
