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 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)
