class ServoVsOpenTest():
def __init__(self, arm=Constants.Arm.Left):
self.arm = arm
if self.arm == Constants.Arm.Left:
self.toolframe = Constants.Frames.LeftTool
else:
self.toolframe = Constants.Frames.RightTool
self.transFrame = Constants.Frames.Link0
self.rotFrame = self.toolframe
self.gripperControl = GripperControlClass(arm, tf.TransformListener())
self.imageDetector = ARImageDetectionClass()
# manually add by checking print_state
self.startPose = None
# ar_frame is the target
self.ar_frame = '/stereo_32'
self.objectPose = tfx.pose([0,0,0],tfx.tb_angles(-90,90,0),frame=self.ar_frame)
self.listener = tf.TransformListener()
self.tf_br = tf.TransformBroadcaster()
self.delta_pub = rospy.Publisher('delta_pose', PoseStamped)
self.obj_pub = rospy.Publisher('object_pose', PoseStamped)
rospy.sleep(3)
def publishObjPose(self, pose):
self.obj_pub.publish(pose)
"""
pos = pose.position
ori = pose.orientation
self.tf_br.sendTransform((pos.x, pos.y, pos.z), (ori.x, ori.y, ori.z, ori.w),
rospy.Time.now(), 'object_frame', pose.frame)
"""
def publishDeltaPose(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.delta_pub.publish(pose)
def openTest(self):
self.gripperControl.start()
while not rospy.is_shutdown():
if self.imageDetector.hasFoundGripper(self.arm):
gripperPose = self.imageDetector.getGripperPose(self.arm)
objectPose = tfx.pose(self.objectPose,stamp=rospy.Time.now())
self.publishObjPose(self.objectPose)
deltaPose = Util.deltaPose(gripperPose, objectPose, self.transFrame, self.rotFrame)
deltaPose.position.z += .02
ravenGripperPose = self.gripperControl.getGripperPose(Constants.Frames.Link0)
rospy.loginfo('Press enter to move to ' + self.ar_frame)
raw_input()
self.gripperControl.goToGripperPoseDelta(ravenGripperPose, deltaPose)
break
rospy.sleep(.1)
rospy.spin()
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 run(self):
arm = Constants.Arm.Left
listener = tf.TransformListener()
ar_frames = ['stereo_36', 'stereo_33', 'stereo_35']
if arm == Constants.Arm.Left:
toolframe = Constants.Frames.LeftTool
else:
toolframe = Constants.Frames.RightTool
transFrame = Constants.Frames.Link0
rotFrame = toolframe
rospy.init_node('precision_test_node',anonymous=True)
gripperControl = GripperControlClass(arm, listener)
imageDetector = ARImageDetectionClass()
rospy.sleep(4)
gripperControl.start()
"""
while not rospy.is_shutdown():
if imageDetector.hasFoundGripper(arm) and self.foamPose != None:
rospy.loginfo('Press enter to move')
raw_input()
gripperPose = imageDetector.getGripperPose(arm)
deltaPose = Util.deltaPose(gripperPose, self.foamPose, transFrame, rotFrame)
deltaPose.position.z += .01
code.interact(local=locals())
duration = 6
gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(Constants.Frames.Link0), deltaPose, duration=duration, ignoreOrientation=True)
rospy.sleep(duration)
self.foamPose = None
rospy.sleep(.5)
"""
raw_input()
for ar_frame in ar_frames:
while not rospy.is_shutdown():
if imageDetector.hasFoundGripper(arm):
rospy.loginfo('Press enter to move to ' + ar_frame)
rospy.sleep(1)
#raw_input()
gripperPose = imageDetector.getGripperPose(arm)
arPose = tfx.pose([0,0,0],tfx.tb_angles(-90,90,0),frame=ar_frame)
deltaPose = Util.deltaPose(gripperPose, arPose, transFrame, rotFrame)
deltaPose.position.z += .02
#code.interact(local=locals())
duration = 6
gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(Constants.Frames.Link0), deltaPose, duration=duration, ignoreOrientation=False)
rospy.sleep(duration)
break
rospy.sleep(.5)
class ServoVsOpenVsIterTest():
def __init__(self, arm=Constants.Arm.Left):
self.arm = arm
if self.arm == Constants.Arm.Left:
self.toolframe = Constants.Frames.LeftTool
else:
self.toolframe = Constants.Frames.RightTool
self.transFrame = Constants.Frames.Link0
self.rotFrame = self.toolframe
self.gripperControl = GripperControlClass(arm, tf.TransformListener())
self.imageDetector = ARImageDetectionClass()
self.homePose = self.imageDetector.getHomePose()
self.gripperOpenCloseDuration = 2.5
rospy.sleep(3)
def goHome(self):
self.gripperControl.goToGripperPose(self.gripperControl.getGripperPose(frame=Constants.Frames.Link0), self.homePose.pose, ignoreOrientation=True)
while not self.gripperControl.isPaused() and not rospy.is_shutdown():
rospy.sleep(.1)
rospy.loginfo('Opening the gripper')
self.gripperControl.openGripper(duration=self.gripperOpenCloseDuration)
rospy.sleep(self.gripperOpenCloseDuration)
def openTest(self):
self.gripperControl.start()
self.goHome()
while not rospy.is_shutdown():
if self.imageDetector.hasFoundObject:
objectPose = self.imageDetector.getObjectPose()
ravenGripperPose = self.gripperControl.getGripperPose(Constants.Frames.Link0)
rospy.loginfo('Press enter')
raw_input()
self.gripperControl.goToGripperPose(ravenGripperPose, objectPose)
break
rospy.sleep(.1)
rospy.spin()
def oneServoTest(self):
self.gripperControl.start()
self.goHome()
while not rospy.is_shutdown():
if self.imageDetector.hasFoundGripper(self.arm) and self.imageDetector.hasFoundObject():
gripperPose = self.imageDetector.getGripperPose(self.arm)
objectPose = self.imageDetector.getObjectPose()
deltaPose = Util.deltaPose(gripperPose, objectPose, self.transFrame, self.rotFrame)
ravenGripperPose = self.gripperControl.getGripperPose(Constants.Frames.Link0)
rospy.loginfo('Press enter')
raw_input()
self.gripperControl.goToGripperPoseDelta(ravenGripperPose, deltaPose)
break
rospy.sleep(.1)
rospy.spin()
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)