def openTest(self):
self.gripperControl.start()
while not rospy.is_shutdown():
if self.imageDetector.hasFoundGripper(self.arm):
gripperPose = self.imageDetector.getGripperPose(self.arm)
objectPose = tfx.pose(self.objectPose,stamp=rospy.Time.now())
self.publishObjPose(self.objectPose)
deltaPose = Util.deltaPose(gripperPose, objectPose, self.transFrame, self.rotFrame)
deltaPose.position.z += .02
ravenGripperPose = self.gripperControl.getGripperPose(Constants.Frames.Link0)
rospy.loginfo('Press enter to move to ' + self.ar_frame)
raw_input()
self.gripperControl.goToGripperPoseDelta(ravenGripperPose, deltaPose)
break
rospy.sleep(.1)
rospy.spin()
def servoTest(self):
self.gripperControl.start()
homePose = tfx.pose([-0.047, -0.029, -0.116],{'yaw':79.9, 'pitch':47.1, 'roll':-1.7}, frame=Constants.Frames.Link0)
self.gripperControl.setHomePose(homePose)
print 'home pose', self.gripperControl.getHomePose()
self.gripperControl.goToHomePose()
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)
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)
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)
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 run(self):
arm = Constants.Arm.Left
listener = tf.TransformListener()
ar_frames = ['stereo_36', 'stereo_33', 'stereo_35']
if arm == Constants.Arm.Left:
toolframe = Constants.Frames.LeftTool
else:
toolframe = Constants.Frames.RightTool
transFrame = Constants.Frames.Link0
rotFrame = toolframe
rospy.init_node('precision_test_node',anonymous=True)
gripperControl = GripperControlClass(arm, listener)
imageDetector = ARImageDetectionClass()
rospy.sleep(4)
gripperControl.start()
"""
while not rospy.is_shutdown():
if imageDetector.hasFoundGripper(arm) and self.foamPose != None:
rospy.loginfo('Press enter to move')
raw_input()
gripperPose = imageDetector.getGripperPose(arm)
deltaPose = Util.deltaPose(gripperPose, self.foamPose, transFrame, rotFrame)
deltaPose.position.z += .01
code.interact(local=locals())
duration = 6
gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(Constants.Frames.Link0), deltaPose, duration=duration, ignoreOrientation=True)
rospy.sleep(duration)
self.foamPose = None
rospy.sleep(.5)
"""
raw_input()
for ar_frame in ar_frames:
while not rospy.is_shutdown():
if imageDetector.hasFoundGripper(arm):
rospy.loginfo('Press enter to move to ' + ar_frame)
rospy.sleep(1)
#raw_input()
gripperPose = imageDetector.getGripperPose(arm)
arPose = tfx.pose([0,0,0],tfx.tb_angles(-90,90,0),frame=ar_frame)
deltaPose = Util.deltaPose(gripperPose, arPose, transFrame, rotFrame)
deltaPose.position.z += .02
#code.interact(local=locals())
duration = 6
gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(Constants.Frames.Link0), deltaPose, duration=duration, ignoreOrientation=False)
rospy.sleep(duration)
break
rospy.sleep(.5)
