def getBaxterJointSpace(self, limb = 'right'): # All orientation calclulations voObj = VectorOrientation() voObj.setVectorChange(0.00, 0.00, -1.0) # X, Y and Z Vector Axis Orientation #voObj.setAngleOrientation(-20, 0, 0, 'd') voObj.calcVectorOrientation() # Set gamma in degrees voObj.setGamma(180, 'd') # Trick for baxter orientation change for rigth arm: voObj.setAngleOrientation(-voObj._theta, voObj._phi, voObj._gamma, 'r') # Calculate quaternion quat = voObj.getQuaternionOrientation() # Inverse Kinematics arm = IKSolveBaxter(limb) arm.setArmEndPos(self._del_arm_x, self._del_arm_y, self._del_arm_z, quat[0], quat[1], quat[2], quat[3]) arm.ik_solve(); self._joints_loc = arm.getBaxterJointSpace() arm.setArmEndPos(self._del_arm_x, self._del_arm_y, self._del_arm_z + 0.10, quat[0], quat[1], quat[2], quat[3]) arm.ik_solve(); self._inter_joints_loc1 = arm.getBaxterJointSpace() arm.setArmEndPos(self._del_arm_x, self._del_arm_y, self._del_arm_z + 0.15, quat[0], quat[1], quat[2], quat[3]) arm.ik_solve(); self._inter_joints_loc2 = arm.getBaxterJointSpace() return self._joints_loc, self._inter_joints_loc1, self._inter_joints_loc2
def main(): global img global image_corners global click_count global baxter_endpoints rospy.init_node('RetrieveCamToolTableCalib', disable_signals=True) # Record Baxter Coordinates print print "Record arm endpoints using Cuff Button and Big Button when 4 points are done (Left Arm)" cam_arm = baxter_interface.limb.Limb("left") bbhObj = BaxterButtonHelp("left") baxter_endpoints = [] click_count = 0 while_flag = True while(while_flag): cButtonPressed = bbhObj.getButtonStates()['cState'] bButtonPressed = bbhObj.getButtonStates()['bState'] if(cButtonPressed): pose = cam_arm.endpoint_pose() xyz = pose["position"] baxter_endpoints.append(xyz) click_count += 1 time.sleep(1) print "Baxter Arm EndPoint %s added" % (click_count) if(bButtonPressed): if (click_count == 4): while_flag = False print "Baxter Endpoints Recorded Recorded:" print baxter_endpoints elif (click_count < 4): while_flag = True baxter_endpoints = [] click_count = 0 print "Less than 4 points recorded. Please re-record all corners" else: while_flag = True baxter_endpoints = [] click_count = 0 print "More than 4 points recorded. Please record only the corners" time.sleep(1) ############################################################## print "Storing Retrieval Position" # Store retrieval position print print "Moving baxter cam arm to desired location image frame" cam_arm_x = ((baxter_endpoints[0].x + baxter_endpoints[2].x) / 2) + 0 #As camera not at center cam_arm_y = (baxter_endpoints[0].y + baxter_endpoints[2].y) / 2 + 0.003 #As camera is not at center cam_arm_z = ((baxter_endpoints[0].z + baxter_endpoints[2].z) / 2) + 0 # Height above table # All orientation calclulations voObj = VectorOrientation() voObj.setVectorChange(0.00, 0.00, -1.0) # X, Y and Z Vector Axis Orientation #voObj.setAngleOrientation(-20, 0, 0, 'd') voObj.calcVectorOrientation() # Set gamma in degrees voObj.setGamma(180, 'd') # Trick for baxter orientation change for rigth arm: voObj.setAngleOrientation(-voObj._theta, voObj._phi, voObj._gamma, 'r') # Calculate quaternion quat = voObj.getQuaternionOrientation() # Inverse Kinematics arm = IKSolveBaxter("left") arm.setArmEndPos(cam_arm_x, cam_arm_y, cam_arm_z, quat[0], quat[1], quat[2], quat[3]) arm.ik_solve(); ret_joints = arm.getBaxterJointSpace() bcajpFile = open("BaxterRetArmJointPosFile.dat", "w"); cPickle.dump(ret_joints, bcajpFile); bcajpFile.close() # Move Arm control_arm = baxter_interface.limb.Limb("left") control_arm.set_joint_position_speed(0.5) # Joint movement speed control_arm.move_to_joint_positions(ret_joints, 5.0) time.sleep(1) ############################################################## # Move baxter arm print print "Moving baxter cam arm to desired location image frame" cam_arm_x = ((baxter_endpoints[0].x + baxter_endpoints[2].x) / 2) + 0.075 #As camera not at center cam_arm_y = (baxter_endpoints[0].y + baxter_endpoints[2].y) / 2 + 0.005 #As camera is not at center cam_arm_z = ((baxter_endpoints[0].z + baxter_endpoints[2].z) / 2) + 0.15 # Height above table # All orientation calclulations voObj = VectorOrientation() voObj.setVectorChange(0.00, 0.00, -1.0) # X, Y and Z Vector Axis Orientation #voObj.setAngleOrientation(-20, 0, 0, 'd') voObj.calcVectorOrientation() # Set gamma in degrees voObj.setGamma(180, 'd') # Trick for baxter orientation change for rigth arm: voObj.setAngleOrientation(-voObj._theta, voObj._phi, voObj._gamma, 'r') # Calculate quaternion quat = voObj.getQuaternionOrientation() # Inverse Kinematics arm = IKSolveBaxter("left") arm.setArmEndPos(cam_arm_x, cam_arm_y, cam_arm_z, quat[0], quat[1], quat[2], quat[3]) arm.ik_solve(); joints = arm.getBaxterJointSpace() # Move Arm control_arm = baxter_interface.limb.Limb("left") control_arm.set_joint_position_speed(0.5) # Joint movement speed control_arm.move_to_joint_positions(joints, 5.0) os.system("clear") print "Press C to continue and store baxter camera arm position" while(not sf.Keyboard.is_key_pressed(sf.Keyboard.C)): pass bcajpFile = open("RetrieveBaxterCamArmJointPosFile.dat", "w"); cPickle.dump(joints, bcajpFile); bcajpFile.close() print print "Baxter Camera Arm Position Stored" # Get image frame from baxter hand camera print print "Waiting for image..." sihObj = SubImageHelp("/cameras/left_hand_camera/image") error_flag = True rate = rospy.Rate(1) rate.sleep() while error_flag == True: try: img = sihObj.getCVImage() error_flag = False print "BaxterImageObtained" rate.sleep() except: print print "Error in Obtaining Image" # All recording points stuff print print "Record all 4 corner points using mouse double click and then press key to continue" img_copy = img.copy() while_flag = True while(while_flag): img = img_copy.copy() click_count = 0 image_corners = [] cv2.namedWindow('RetrieveImage') cv2.imshow('RetrieveImage', img) cv2.setMouseCallback('RetrieveImage',handle_mouse) cv2.waitKey(0) if (click_count == 4): while_flag = False print "Image Corners Recorded" print print "The image corners are:" print image_corners[0], image_corners[1], image_corners[2], image_corners[3] elif (click_count < 4): while_flag = True print "Less than 4 points recorded. Please re-record all corners" else: while_flag = True print "More than 4 points recorded. Please record only the corners" #cv2.destroyAllWindows() time.sleep(1) X = numpy.matrix([ [image_corners[0][0], image_corners[2][0], image_corners[3][0]], [image_corners[0][1], image_corners[2][1], image_corners[3][1]], [1.0, 1.0, 1.0] ]) A = numpy.matrix([ [baxter_endpoints[0].x, baxter_endpoints[2].x, baxter_endpoints[3].x], [baxter_endpoints[0].y, baxter_endpoints[2].y, baxter_endpoints[3].y], [baxter_endpoints[0].z, baxter_endpoints[2].z, baxter_endpoints[3].z] ]) # Add Image Clip positions and Display Clipped Image x1 = max( [image_corners[0][0], image_corners[3][0]] ) y1 = max( [image_corners[0][1], image_corners[1][1]] ) x2 = min( [image_corners[1][0], image_corners[2][0]] ) y2 = min( [image_corners[2][1], image_corners[3][1]] ) image_clip = [x1, y1, x2, y2] img = img_copy[:, image_clip[0] : image_clip[2] ] img = img[image_clip[1] : image_clip[3], :] cv2.imwrite('RetrieveImage.png', img) ImageClipPosFile = open("RetrieveImageClipPosFile.dat", "w"); cPickle.dump(image_clip, ImageClipPosFile); ImageClipPosFile.close() print "Image Clip Positions Added To File" time.sleep(1) print print "Press C to continue and calculate tranformation matrix" while(not sf.Keyboard.is_key_pressed(sf.Keyboard.C)): pass os.system("clear") print "Image Corner Pixel Locations:" print X print print "Baxter Endpoint Locations:" print A print XAPosFile = open("RetrieveXAPosFile.dat", "w"); cPickle.dump(X, XAPosFile); cPickle.dump(A, XAPosFile); XAPosFile.close() time.sleep(1) print "X(ImageCorners) and A(BaxterEndpoints) Stored in XAPosFile.dat" print if(numpy.linalg.det(X) == 0): print "Determiant of image corner matrix = 0" print "Please retry calibration" else: T = numpy.matrix( numpy.dot( A, numpy.linalg.inv(X) ) ) print "Tranformation Matrix:" print T print print "Writing to transformation matrix file..." tmatFile = open("RetrieveTransformationMatrixFile.dat", "w"); cPickle.dump(T, tmatFile); tmatFile.close() print "Tranformation matrix added" print time.sleep(1) print "Press C to continue and exit program" while(not sf.Keyboard.is_key_pressed(sf.Keyboard.C)): pass rospy.signal_shutdown("Shutting down ROS Node to calculate transformation") print print "Exit Program" print return 0
import rospy from tf.transformations import * from geometry_msgs.msg import ( PoseStamped, Pose, Point, Quaternion, ) from std_msgs.msg import Header from baxter_core_msgs.srv import ( SolvePositionIK, SolvePositionIKRequest, ) # All orientation calclulations voObj = VectorOrientation() voObj.setVectorChange(0.0, -1.0, 0.0) # X, Y and Z Vector Axis Orientation #voObj.setAngleOrientation(-20, 0, 0, 'd') voObj.calcVectorOrientation() # Set gamma in degrees voObj.setGamma(90, 'd') # Trick for baxter orientation change for rigth arm: voObj.setAngleOrientation(-voObj._theta, voObj._phi, voObj._gamma, 'r') # Calculate quaternion and print angle orienttaion in degrees deg = voObj.getAngleOrientation('d') print deg