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
