def execAndRecordTraj(arm=Constants.Arm.Right): rospy.init_node('execAndRecordTraj',anonymous=True) rospy.sleep(1) ravenArm = RavenArm(arm) ravenPlanner = RavenPlanner(arm) record = Test(arm) rospy.sleep(2) ravenArm.start() rospy.loginfo('Classes are initialized, press enter') raw_input() startPose = tfx.pose(ravenArm.getGripperPose()) x = .01 y = .06 z = .05 deltaPoses = [] deltaPoses.append(tfx.pose([x,0, -z],frame=Constants.Frames.Link0)) #deltaPoses.append(tfx.pose([0,-y, 0],frame=Constants.Frames.Link0)) deltaPoses.append(tfx.pose([-x,0,z],frame=Constants.Frames.Link0)) #deltaPoses.append(tfx.pose([0,y, 0],frame=Constants.Frames.Link0)) #record.startRecording() while not rospy.is_shutdown(): for deltaPose in deltaPoses: startPose = tfx.pose(ravenArm.getGripperPose()) endPose = tfx.pose(Util.endPose(startPose, deltaPose)) ravenArm.goToGripperPose(endPose) rospy.sleep(4) record.stopRecording() rospy.loginfo('Press enter to print and exit') raw_input() ravenArm.stop() record.printPoses() IPython.embed()
class MasterClass(): """ Contains the master pipeline for RavenDebridement in the run method See control_pipeline.jpeg for the pipeline overview """ def __init__(self, armName, imageDetector): """ armName must be from Constants.Arm imageDetector is instance of ImageDetectionClass """ self.armName = armName if (armName == Constants.Arm.Left): self.gripperName = Constants.Arm.Left self.toolframe = Constants.Frames.LeftTool #self.calibrateGripperState = ImageDetectionClass.State.CalibrateLeft else: self.gripperName = Constants.Arm.Right self.toolframe = Constants.Frames.RightTool #self.calibrateGripperState = ImageDetectionClass.State.CalibrateRight self.listener = tf.TransformListener() # initialize the three main control mechanisms # image detection, gripper control, and arm control self.imageDetector = imageDetector self.ravenArm = RavenArm(self.armName) self.receptaclePose = None self.homePose = None self.objectPose = None self.gripperPose = None # translation frame self.transFrame = Constants.Frames.Link0 # rotation frame self.rotFrame = self.toolframe # height offset for foam self.objectHeightOffset = .005 # in cm/sec, I think self.openLoopSpeed = .04 self.gripperOpenCloseDuration = 2.5 # debugging outputs self.des_pose_pub = rospy.Publisher('desired_pose', PoseStamped) self.obj_pub = rospy.Publisher('object_pose', PoseStamped) self.timeout = Util.Timeout(30) self.findGripperTimeout = Util.Timeout(2) self.rotateBy = -30 def findReceptacle(self, failMethod=None, successMethod=None): failMethod = failMethod or self.findReceptacle successMethod = successMethod or self.findHome rospy.loginfo('Searching for the receptacle') if not self.imageDetector.hasFoundReceptacle(): rospy.loginfo('Did not find receptacle') return failMethod self.receptaclePose = self.imageDetector.getReceptaclePose() rospy.loginfo('Found receptacle') return successMethod def findHome(self, failMethod=None, successMethod=None): failMethod = failMethod or self.findHome successMethod = successMethod or self.moveToReceptacle rospy.loginfo('Searching for the home position') if not self.imageDetector.hasFoundHome(): rospy.loginfo('Did not find home position') return failMethod self.homePose = self.imageDetector.getHomePose() rospy.loginfo('Found home position') return successMethod def findObject(self, failMethod=None, successMethod=None): failMethod = failMethod or self.findObject successMethod = successMethod or self.findGripper # reset rotateBy self.rotateBy = -30 rospy.loginfo('Searching for object point') # find object point and pose if not self.imageDetector.hasFoundObject(): rospy.loginfo('Did not find object') return failMethod # get object w.r.t. toolframe self.objectPose = self.imageDetector.getObjectPose(self.toolframe) self.objectPose.pose.position.z += self.objectHeightOffset self.publishObjectPose(self.objectPose) rospy.loginfo('Found object') return successMethod def findGripper(self, failMethod=None, successMethod=None): failMethod = failMethod or self.rotateGripper successMethod = successMethod or self.moveNearObject rospy.loginfo('Searching for ' + self.gripperName) # find gripper point self.imageDetector.ignoreOldGripper(self.gripperName) self.findGripperTimeout.start() while (not self.imageDetector.hasFoundGripper(self.gripperName)) or (not self.imageDetector.hasFoundNewGripper(self.gripperName)): if self.findGripperTimeout.hasTimedOut(): rospy.loginfo('Did not find gripper') return failMethod rospy.sleep(.05) self.gripperPose = self.imageDetector.getGripperPose(self.gripperName) rospy.loginfo('Found gripper') return successMethod def rotateGripper(self, failMethod=None, successMethod=None): failMethod = failMethod or self.findGripper successMethod = successMethod or self.findGripper global numGripperRotations numGripperRotations += 1 rospy.loginfo('Rotating the gripper by ' + str(self.rotateBy) + ' degrees') deltaPose = tfx.pose([0,0,.001], tfx.tb_angles(0,0,self.rotateBy)) self.ravenArm.goToGripperPoseDelta(deltaPose, duration=2) self.rotateBy = -math.copysign(abs(self.rotateBy)+5, self.rotateBy) return successMethod def moveNearObject(self, failMethod=None, successMethod=None): """ Goes near the object Returns servoToObject """ failMethod = failMethod or self.moveNearObject successMethod = successMethod or self.servoToObject rospy.loginfo('Moving near the object point') # go to near the object point deltaPose = Util.deltaPose(self.gripperPose, self.objectPose, self.transFrame, self.rotFrame) deltaPose.position.z += .03 self.ravenArm.goToGripperPoseDelta(deltaPose, speed=self.openLoopSpeed) return successMethod def servoToObject(self, failMethod=None, successMethod=None): """ In close-loop, servos to the object. Then, closes the gripper. """ failMethod2 = lambda: self.findGripper(self.moveToHome, self.servoToObject) failMethod1 = lambda: self.findObject(self.moveToHome, failMethod2) failMethod0 = lambda vertAmount: self.moveVertical(self.moveToReceptacle, failMethod1, vertAmount) failMethod = failMethod or failMethod0 successMethod = successMethod or self.moveVertical rospy.loginfo('Servoing to the object point') # servo to the object point transBound = .008 rotBound = float("inf") maxMovement = .015 deltaPose = uncappedDeltaPose = tfx.pose([0,0,0]) # if can't find gripper at start, go back to receptacle rospy.sleep(1) if self.imageDetector.hasFoundGripper(self.gripperName) and self.imageDetector.hasFoundNewGripper(self.gripperName): self.gripperPose = self.imageDetector.getGripperPose(self.gripperName) else: return self.moveToHome self.timeout.start() while not Util.withinBounds(self.gripperPose, self.objectPose, transBound, rotBound, self.transFrame, self.rotFrame): if self.ravenArm.isPaused(): rospy.sleep(1) if self.imageDetector.hasFoundNewGripper(self.gripperName): rospy.loginfo('paused and found new gripper') self.gripperPose = self.imageDetector.getGripperPose(self.gripperName) deltaPose = uncappedDeltaPose = tfx.pose(Util.deltaPose(self.gripperPose, self.objectPose, Constants.Frames.Link0, self.toolframe)) deltaPose.position = deltaPose.position.capped(maxMovement) #if abs(deltaPose.position.z) > maxMovement: # deltaPose.position.z = math.copysign(maxMovement, deltaPose.position.z) deltaPose0Link = tfx.pose(Util.deltaPose(self.gripperPose, self.objectPose, Constants.Frames.Link0, Constants.Frames.Link0)) deltaPose0Link.position = deltaPose.position.capped(maxMovement) self.publishDesiredPose(deltaPose0Link, tfx.pose(self.gripperPose)) rospy.loginfo('delta pose') rospy.loginfo(deltaPose) self.ravenArm.goToGripperPoseDelta(deltaPose) rospy.sleep(1) else: rospy.loginfo('paused but did NOT find new gripper') deltaPose.position = uncappedDeltaPose.position - deltaPose.position self.ravenArm.goToGripperPoseDelta(deltaPose, ignoreOrientation=True) break if self.timeout.hasTimedOut() or rospy.is_shutdown(): rospy.loginfo('Timed out') self.objectHeightOffset = self.objectHeightOffset - .0005 if self.objectHeightOffset > 0 else 0 global numFailedPickups numFailedPickups += 1 return failMethod(.02) rospy.sleep(.1) rospy.loginfo('Closing the gripper') # close gripper (consider not all the way) self.ravenArm.closeGripper(duration=self.gripperOpenCloseDuration) return successMethod def moveVertical(self, failMethod=None, successMethod=None, vertAmount=.05): """ Move vertical in open-loop """ failMethod = failMethod or self.checkPickup successMethod = successMethod or self.checkPickup rospy.loginfo('Moving vertical with the object') # move vertical with the object deltaPose = tfx.pose([0,0,vertAmount]).msg.Pose() self.ravenArm.goToGripperPoseDelta(deltaPose, speed=self.openLoopSpeed) return successMethod def checkPickup(self, failMethod=None, successMethod=None): """ Checks if the grasper picked up a red foam piece """ failMethod = failMethod or (lambda: self.findObject(self.moveToHome, (lambda: self.findGripper(self.moveToHome, self.servoToObject)))) successMethod = successMethod or (lambda: self.moveToHome(self.moveToReceptacle, self.moveToReceptacle)) rospy.loginfo('Check if red foam piece successfully picked up') rospy.sleep(1) try: rospy.wait_for_service(Constants.Services.isFoamGrasped, timeout=5) foamGraspedService = rospy.ServiceProxy(Constants.Services.isFoamGrasped, ThreshRed) isFoamGrasped = foamGraspedService(0).output if isFoamGrasped == 1: rospy.loginfo('Successful pickup!') return successMethod else: rospy.loginfo('Failed pickup') global numFailedPickups numFailedPickups += 1 rospy.loginfo('Opening the gripper') # open gripper (consider not all the way) self.ravenArm.openGripper(duration=self.gripperOpenCloseDuration) return failMethod except: rospy.loginfo('Service exception, assuming successful pickup') return successMethod def moveToReceptacle(self, failMethod=None, successMethod=None): """ Move to the receptacle in open-loop Then, open the gripper """ failMethod = failMethod or self.moveToHome successMethod = successMethod or self.moveToHome rospy.loginfo('Moving to receptacle') # move to receptacle with object self.ravenArm.goToGripperPose(self.receptaclePose.pose, ignoreOrientation=True, speed=self.openLoopSpeed) rospy.loginfo('Opening the gripper') # open gripper (consider not all the way) self.ravenArm.openGripper(duration=self.gripperOpenCloseDuration) return successMethod def moveToHome(self, failMethod=None, successMethod=None): """ Move to the home position in open-loop """ failMethod = failMethod or self.findObject successMethod = successMethod or self.findObject rospy.loginfo('Moving to home position') # move to home position self.ravenArm.goToGripperPose(self.homePose.pose, ignoreOrientation=True, speed=self.openLoopSpeed) # so when finding object, find newest one self.imageDetector.removeObjectPoint() return successMethod def run(self): """ Loops through the pipeline The pipeline consists of the above methods. Each method has a fail and success return method. """ delay = .5 currentStage = self.findReceptacle self.ravenArm.start() if WRITE_TO_FILE: import datetime startTime = rospy.Time.now() fileName = '/home/gkahn/ros_workspace/RavenDebridement/test_files/OldMasterTest_{0}'.format(datetime.datetime.now()) file = open(fileName,'w') while not rospy.is_shutdown(): rospy.loginfo('Next stage') #rospy.loginfo('Press enter') #raw_input() if WRITE_TO_FILE: stageStartTime = rospy.Time.now() file.write('{0}, Start time = {1}\n'.format(currentStage.__name__,stageStartTime.to_sec()-startTime.to_sec())) currentStage = currentStage() if WRITE_TO_FILE: stageEndTime = rospy.Time.now() file.write('End time = {0}, Stage duration = {1}\n\n'.format(stageEndTime.to_sec()-startTime.to_sec(),stageEndTime.to_sec()-stageStartTime.to_sec())) rospy.sleep(delay) if WRITE_TO_FILE: file.write('Total elapsed time = {0}\n'.format(rospy.Time.now().to_sec()-startTime.to_sec())) file.write('Number of failed pickups = {0}\n'.format(numFailedPickups)) file.write('Number of gripper rotations = {0}\n'.format(numGripperRotations)) file.close() self.ravenArm.stop() def publishObjectPose(self, pose): #self.obj_pub.publish(pose) pass def publishDesiredPose(self, delta_pose, gripper_pose): """ frame = gripper_pose.frame time = self.listener.getLatestCommonTime(frame, Constants.Frames.Link0) pose_stamped = gripper_pose.msg.PoseStamped() pose_stamped.header.stamp = time gripper_pose = self.listener.transformPose(Constants.Frames.Link0, gripper_pose.msg.PoseStamped()) pose = PoseStamped() pose.header.frame_id = Constants.Frames.Link0 pose.header.stamp = rospy.Time.now() pose.pose.position = (gripper_pose.pose.position + delta_pose.position).msg.Point() ori = tfx.pose(gripper_pose).orientation.matrix * delta_pose.orientation.matrix pose.pose.orientation = tfx.pose([0,0,0], ori).orientation.msg.Quaternion() self.des_pose_pub.publish(pose) """ pass
class GripperFOVClass(): def __init__(self, armName): print('Initializing GripperFOVClass') self.armName = armName self.ravenArm = RavenArm(self.armName) self.imageDetector = ImageDetector() self.camFrame = '/left_BC' self.desiredOrientation = tfx.tb_angles(90,0,0) # in self.camFrame self.posFrame = MyConstants.Frames.Link0 self.rotFrame = MyConstants.Frames.RightTool if self.armName == MyConstants.Arm.Right else MyConstants.Frames.LeftTool self.pub = rospy.Publisher('orig_gripper_pose',PoseStamped) self.file = open('/tmp/GripperFOVTest.txt','w') print('Initialized GripperFOVClass') def currentPose(self): currTfPose = Util.convertToFrame(self.ravenArm.getGripperPose(), self.camFrame) currDetectedPose = Util.convertToFrame(self.imageDetector.getGripperPose(self.armName), self.camFrame) currPoseInCamera = tfx.pose(currTfPose.position, currDetectedPose.orientation) return currPoseInCamera def homePose(self): homePose = self.currentPose() homePose.orientation = self.desiredOrientation return homePose def findNewGripper(self, timeout=1): timeout = Util.Timeout(timeout) self.imageDetector.ignoreOldGripper(self.armName) timeout.start() while not timeout.hasTimedOut(): if self.imageDetector.hasFoundNewGripper(self.armName): return tfx.pose(self.imageDetector.getGripperPose(self.armName)) return None def rotateUntilUnseen(self, axis, rotateBy): firstPose = gripperPose = Util.convertToFrame(self.findNewGripper(), MyConstants.Frames.Link0) firstTfPose = Util.convertToFrame(self.ravenArm.getGripperPose(), MyConstants.Frames.Link0) self.pub.publish(firstPose.msg.PoseStamped()) if axis == 'pitch': tb_rotate = tfx.tb_angles(0,rotateBy,0) origAngle = tfx.tb_angles(firstPose.orientation).pitch_deg elif axis == 'roll': tb_rotate = tfx.tb_angles(0,0,rotateBy) origAngle = tfx.tb_angles(firstPose.orientation).roll_deg else: return 0.0 totalRotation = 0.0 rotatePose = tfx.pose([0,0,0], tb_rotate) lastOrientation = tfx.pose(self.ravenArm.getGripperPose()).orientation lastPose = None while gripperPose is not None: #gripperPose = Util.convertToFrame(gripperPose, self.rotFrame) #tfPose = tfx.pose([0,0,0],frame=self.rotFrame) #tapeRotation = gripperPose.orientation #tfRotation = tfPose.orientation #IPython.embed() #return #tfGripperPose = tfx.pose(self.ravenArm.getGripperPose()) #gripperPose = Util.convertToFrame(gripperPose, tfGripperPose.frame) #endPose = tfx.pose(tfGripperPose.position, rotatePose.orientation.matrix * gripperPose.orientation.matrix) #self.ravenArm.goToGripperPose(endPose, duration=1) #endPose = Util.endPose(gripperPose,tfx.pose([0,0,0],rotatePose.orientation, frame=self.camFrame),frame=MyConstants.Frames.Link0) #deltaPose = tfx.pose([0,0,0], endPose.orientation) #self.ravenArm.goToGripperPoseDelta(deltaPose, duration=1) tfGripperPose = tfx.pose(self.ravenArm.getGripperPose()) tfGripperPose = Util.convertToFrame(tfGripperPose, self.camFrame) endPose = Util.endPose(tfGripperPose, tfx.pose([0,0,0],tb_rotate,frame=self.camFrame)) endPose = Util.convertToFrame(endPose, MyConstants.Frames.Link0) endPose.position = firstTfPose.position self.ravenArm.goToGripperPose(endPose, duration=1) print('Rotating...') rospy.sleep(.25) #self.ravenArm.goToGripperPoseDelta(rotatePose, duration=1) totalRotation += rotateBy #print('Total rotation = {0}'.format(totalRotation)) currOrientation = tfx.pose(self.ravenArm.getGripperPose()).orientation if lastOrientation is not None: angleBetween = Util.angleBetweenQuaternions(lastOrientation.msg.Quaternion(), currOrientation.msg.Quaternion()) #print('angleBetween = {0}'.format(angleBetween)) if angleBetween < rotateBy*.5: self.file.write('Hit physical limit\n') print('Hit physical limit') break lastOrientation = currOrientation rospy.sleep(.5) lastPose = Util.convertToFrame(gripperPose, self.camFrame) gripperPose = self.findNewGripper() if lastPose is not None: firstPose = Util.convertToFrame(firstPose, self.camFrame) if axis == 'pitch': firstPitch = tfx.tb_angles(firstPose.orientation).pitch_deg lastPitch = tfx.tb_angles(lastPose.orientation).pitch_deg return copysign(fabs(lastPitch-firstPitch),rotateBy) elif axis == 'roll': firstRoll = tfx.tb_angles(firstPose.orientation).roll_deg lastRoll = tfx.tb_angles(lastPose.orientation).roll_deg return copysign(fabs(lastRoll-firstRoll),rotateBy) return totalRotation def runTest(self): self.ravenArm.start() rospy.sleep(2) currPoseInCamera = self.currentPose() homePose = self.homePose() deltaPose = Util.deltaPose(currPoseInCamera, homePose, self.posFrame, self.rotFrame) self.ravenArm.goToGripperPoseDelta(deltaPose, duration=2) # self.ravenArm.goToGripperPoseDelta(tfx.pose([0,0,0],tfx.tb_angles(0,30,0)),duration=2) #rotatePose = tfx.pose([0,0,0],tfx.tb_angles(0,45,0)) #IPython.embed() #return # z must always stay the same z = currPoseInCamera.position.z xMove = .03 yMove = .01 gridMoves = [[-xMove, 0, 0], [0, yMove, 0], [xMove, 0, 0], [0, -yMove, 0]] rotationAxes = ['pitch', 'roll'] for gridMove in gridMoves: print('Go to next place on grid, press enter') raw_input() deltaPose = Util.deltaPose(self.currentPose(), self.homePose()+gridMove, self.posFrame, self.rotFrame) homePose = Util.endPose(self.ravenArm.getGripperPose(), deltaPose) self.ravenArm.goToGripperPose(homePose) self.file.write('Home pose {0}\n'.format(homePose)) print('Home pose {0}'.format(homePose)) for axis in rotationAxes: if rospy.is_shutdown(): return rotateBy = 5.0 self.file.write('Rotating in positive direction on {0} axis\n'.format(axis)) print('Rotate in positive direction on {0} axis'.format(axis)) #raw_input() maxRotation = self.rotateUntilUnseen(axis, rotateBy) self.ravenArm.goToGripperPose(homePose, duration=1) rospy.sleep(1) self.file.write('Rotating in negative direction on {0} axis\n'.format(axis)) print('Rotate in negative direction on {0} axis'.format(axis)) #raw_input() minRotation = self.rotateUntilUnseen(axis, -rotateBy) self.ravenArm.goToGripperPose(homePose, duration=1) rospy.sleep(1) self.file.write('{0} range of motion is ({1},{2})\n'.format(axis,minRotation,maxRotation)) print('{0} range of motion is ({1},{2})'.format(axis,minRotation,maxRotation)) self.file.close() self.ravenArm.stop()