def execute(self, userdata): if MasterClass.PAUSE_BETWEEN_STATES: pause_func(self) rospy.loginfo('Planning trajectory from gripper to object') objectPose = tfx.pose(userdata.objectPose) gripperPose = tfx.pose(userdata.gripperPose) #objectPose.orientation = gripperPose.orientation transBound = .006 rotBound = float("inf") if Util.withinBounds(gripperPose, objectPose, transBound, rotBound, self.transFrame, self.rotFrame): rospy.loginfo('Closing the gripper') self.ravenArm.closeGripper(duration=self.gripperOpenCloseDuration) userdata.vertAmount = .04 return 'reachedObject' deltaPose = tfx.pose(Util.deltaPose(gripperPose, objectPose, self.transFrame, self.rotFrame)) endPose = Util.endPose(self.ravenArm.getGripperPose(), deltaPose) n_steps = int(self.stepsPerMeter * deltaPose.position.norm) + 1 poseTraj = self.ravenPlanner.getTrajectoryFromPose(self.ravenArm.name, endPose, n_steps=n_steps) rospy.loginfo('deltaPose') rospy.loginfo(deltaPose) rospy.loginfo('n_steps') rospy.loginfo(n_steps) if poseTraj == None: return 'failure' userdata.poseTraj = poseTraj return 'notReachedObject'
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