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