def cartclass_to_cartarg(self, cartclass, cartarg): cartarg = go_to.cartesian_pose_arg(position=[ cartclass.position_x, cartclass.position_y, cartclass.position_z ], orientation=[ cartclass.orientation_x, cartclass.orientation_y, cartclass.orientation_z, cartclass.orientation_w ]) return cartarg
def __init__(self): self.old_time = time.time() self.learner_quotient = 0 self.interaction_counter = 0 self.total_interactions = 0 self.time_elapsed = 0 self.learner_interaction = False self.old_time = time.time() self.new_time = time.time() self.waypoints = [] self.waypoints_back = [] self.waypoint_limit = 99 self.effort = [] self.new_angle = 0 self.old_angle = 0 self.changed_angle = 0 self.eventnumber = 0 self.iframe_sequence_pub = rospy.Publisher('/iframe_sequence', Int32, queue_size=10) self.unique_input_pub = rospy.Publisher('/unique_input', String, queue_size=10) self.zeroG_pub = rospy.Publisher('/zeroG_topic', String, queue_size=10) self.gripper_pub = rospy.Publisher('/gripper_control', Int32, queue_size=10) self.lights_pub = rospy.Publisher('/cuff_light', String, queue_size=10) self.interaction_pub = rospy.Publisher('/interaction_topic', Int32, queue_size=10) rospy.Subscriber('/upper_cuff_button', Int32, self.upper_cuff_button_callback) rospy.Subscriber('/lower_cuff_button', Int32, self.lower_cuff_button_callback) rospy.Subscriber('/bot_sequence', Int32, self.callback) rospy.Subscriber('/save_info', String, self.save_info_callback) rospy.Subscriber('/right_navigator_button', String, self.navigator_callback) rospy.init_node('Sawyer_Sparrow_comm_node', anonymous=True) self.limb = intera_interface.Limb('right') self._gripper = get_current_gripper_interface() self._is_clicksmart = isinstance(self._gripper, SimpleClickSmartGripper) self.startPose = self.limb.endpoint_pose() self.startPose_container = cartPose() self.startPose_arg = go_to.cartesian_pose_arg() self.startJointAngles = self.limb.joint_angles() self.newJointAngles_arg = go_to.joint_angle_arg() self.newCartPose_arg = go_to.cartesian_pose_arg() self.newCartPose_arg2 = go_to.cartesian_pose_arg() self.newCartPose = cartPose() self.newJointAngles = jointAngles() self.waypoint_init_container = cartPose() self.waypoint_init_arg = go_to.cartesian_pose_arg() #Check if gripper is attached properly self.grip = gripper_cuff.GripperConnect() self.savedLocations = [] #Open gripper grip_msg.data = 1 self.gripper_pub.publish(grip_msg) #set constrained mode zeroG.constrained(zeroG_all_constraints) # self.startPose = self.limb.endpoint_pose() # print(self.limb.endpoint_pose()) # self.startPose_container = self.pose_to_cartclass(pose = self.startPose, cartclass = self.startPose_container) # print(self.startPose_container.position_z) # print(self.startPose_container.position_z) # self.startPose_arg = self.cartclass_to_cartarg(cartclass = self.startPose_container, cartarg = self.startPose_arg) # print(self.startPose_container.position_z) # go_to.cartesian_pose(self.startPose_arg) #Check files and set text file to save to if (file_save): self.home = os.path.expanduser("~") self.dir_path = self.home + '/Learner_Responses/module' + str( module_number) + '/' if not os.path.exists(self.dir_path): os.makedirs(self.dir_path) self.file_number = 0 self.file_path = self.dir_path + 'test_' + str( self.file_number) + '.txt' while (os.path.exists(self.file_path)): self.file_number += 1 self.file_path = self.dir_path + 'test_' + str( self.file_number) + '.txt' self.file = open(self.file_path, 'w') self.file.write('pretest \r\n' + str(self.old_time) + '\r\n') self.file.close() go_to.joint_angles(empty_angle_arg) # #Complete grip # grip_msg.data = 0 # self.gripper_pub.publish(grip_msg) #Test code here #effort testing # self.waypoints = [] # self.waypoint_limit = 5 #initialize navigator for user inputs navigator.right()
# #Wrong waypoints were sent. # for x in range(0, int(unique_msg.data[0])+1): # #goes one additional waypoint more than the waypoint of intersection # go_to.cartesian_pose(self.waypoints[x]) # del self.waypoints[:] # int_msg.data = 39 # self.iframe_sequence_pub.publish(int_msg) # elif(data.data == 40): # rospy.sleep(10) if __name__ == '__main__': empty_angle_arg = go_to.joint_angle_arg() empty_cartesian_arg = go_to.cartesian_pose_arg() child = pexpect.spawn( 'rostopic pub -r 10 /robot/limb/right/suppress_cuff_interaction std_msgs/Empty' ) #variables to be used init_cartesian_arg = go_to.cartesian_pose_arg(joint_angles=[ -0.438147460938, -0.678481445312, -0.433721679687, 1.33986621094, -0.763953125, -1.132484375, 0.959416015625 ]) init_joint_arg = go_to.joint_angle_arg(joint_angles=[ -0.438147460938, -0.678481445312, -0.433721679687, 1.33986621094, -0.763953125, -1.132484375, 0.959416015625 ]) fail_init_joint_arg = go_to.joint_angle_arg(joint_angles=[ -0.33585546875, -0.379596679687, -1.60570117188, 1.12274511719,
# int_msg.data = 3 # self.iframe_sequence_pub.publish(int_msg) # elif(data.data == 4): # rospy.sleep(0.1) # self.slideSequence() # self.slideSequence() #calls 6 # self.slideSequence() # rospy.sleep(0.1) # int_msg.data = 4 # self.iframe_sequence_pub.publish(int_msg) if __name__ == '__main__': empty_angle_arg = go_to.joint_angle_arg() empty_cartesian_arg = go_to.cartesian_pose_arg() #variables to be used init_cartesian_arg = go_to.cartesian_pose_arg(joint_angles=[ -0.438147460938, -0.678481445312, -0.433721679687, 1.33986621094, -0.763953125, -1.132484375, 0.959416015625 ]) init_joint_arg = go_to.joint_angle_arg(joint_angles=[ -0.438147460938, -0.678481445312, -0.433721679687, 1.33986621094, -0.763953125, -1.132484375, 0.959416015625 ]) joint_buttons = go_to.joint_angle_arg(joint_angles=[ -0.0393427734375, -0.71621875, 0.022359375, 1.811921875, -0.116017578125, 2.77036035156, -4.48708789063 ],
def __init__(self): self.slideEvent = True self.slideClose = False self.movedBoxes = 0 self.ready = False self.last_msg = Int32() self.zeroG_pub = rospy.Publisher('/zeroG_topic', String, queue_size=10) self.gripper_pub = rospy.Publisher('/gripper_control', Int32, queue_size=10) self.lights_pub = rospy.Publisher('/cuff_light', String, queue_size=10) self.return_pub = rospy.Publisher('/sawyer_slider_return', Int32, queue_size=2) rospy.Subscriber('/sawyer_sequence', Int32, self.callback) #rospy.Subscriber('/sawyer_mode', Int32, self.sawyerMode_callback) #rospy.Subscriber('/sawyer_time', Float32, self.sawyerTime_callback) rospy.init_node('Sawyer_node', anonymous=True) self.limb = intera_interface.Limb('right') self._gripper = get_current_gripper_interface() self._is_clicksmart = isinstance(self._gripper, SimpleClickSmartGripper) self.startPose = self.limb.endpoint_pose() self.startPose_container = cartPose() self.startPose_arg = go_to.cartesian_pose_arg() self.startJointAngles = self.limb.joint_angles() self.newCartPose_container = cartPose() self.newJointAngles_arg = go_to.joint_angle_arg() self.newCartPose_arg = go_to.cartesian_pose_arg() self.newCartPose_arg2 = go_to.cartesian_pose_arg() self.newCartPose = cartPose() self.newJointAngles = jointAngles() self.waypoint_init_container = cartPose() self.waypoint_init_arg = go_to.cartesian_pose_arg() #Check if gripper is attached properly self.grip = gripper_cuff.GripperConnect() #Open gripper grip_msg.data = 1 self.gripper_pub.publish(grip_msg) if (go_to.joint_angles(empty_angle_arg)): int_msg.data = 1 self.return_pub.publish(int_msg) # go_to.joint_angles(step2_conveyor_arg) # self.startPose = self.limb.endpoint_pose() # self.startPose_container = self.pose_to_cartclass(self.startPose, self.startPose_container) # self.startPose_arg = self.cartclass_to_cartarg(self.startPose_container, self.startPose_arg) # self.startPose_container.position_z = z_conveyor + box_height - gripper_width # self.newCartPose_arg = self.cartclass_to_cartarg(self.startPose_container, self.newCartPose_arg) # if(go_to.cartesian_pose(self.newCartPose_arg)): # grip_msg.data = 0 # self.gripper_pub.publish(grip_msg) # rospy.sleep(1) # go_to.cartesian_pose(self.startPose_arg) # go_to.cartesian_pose(self.newCartPose_arg) # grip_msg.data = 1 # self.gripper_pub.publish(grip_msg) # go_to.cartesian_pose(self.startPose_arg) navigator.right()
# elif(data.data == 6): # rospy.sleep(0.1) # status_msg.data = 0 # self.return_pub.publish(status_msg) # go_to.cartesian_pose(pickup_carriage_cart_arg) # grip_msg.data = 0 # self.gripper_pub.publish(grip_msg) # go_to.cartesian_pose(above_carriage_cart_arg) # status_msg.data = 1 # self.return_pub.publish(status_msg) if __name__ == '__main__': empty_angle_arg = go_to.joint_angle_arg() empty_cartesian_arg = go_to.cartesian_pose_arg() #variables to be used init_cartesian_arg = go_to.cartesian_pose_arg(joint_angles=[ -0.438147460938, -0.678481445312, -0.433721679687, 1.33986621094, -0.763953125, -1.132484375, 0.959416015625 ]) init_joint_arg = go_to.joint_angle_arg(joint_angles=[ -0.33585546875, -0.379596679687, -1.60570117188, 1.12274511719, -1.9501328125, -1.73130761719, 1.83596582031 ]) joint_buttons = go_to.joint_angle_arg(joint_angles=[ -0.0393427734375, -0.71621875, 0.022359375, 1.811921875, -0.116017578125, 2.77036035156, -4.48708789063 ],