def open_by_control(): # Initialize the ros node rospy.init_node("open_by_control", anonymous=True, disable_signals=True) # Instantiage the UR5 and gripper interface. ur5 = UR5Interface() grp = moveit_commander.MoveGroupCommander("gripper") # MoveIt! works well if joint limits are smaller (within -pi, pi) if not ur5.check_joint_limits(): raise Exception('Bad joint limits! try running roslaunch with option "limited:=true"') eelink_pose_before_grasp = [-0.00545284639771, 0.340081666162, 0.26178413889301, 1.570795, 0, 1.570795] ur5.goto_pose_target(eelink_pose_before_grasp) eelink_pose_grasp_position = [-0.00545284639771, 0.390081666162, 0.26178413889301, 1.570795, 0, 1.570795] ur5.goto_pose_target(eelink_pose_grasp_position) grp.set_named_target('close0.4') grp.go(wait=True) eelink_pose_after_rotate = [-0.00545284639771, 0.390081666162, 0.26178413889301, 3, 0, 1.570795] ur5.goto_pose_target(eelink_pose_after_rotate) eelink_pose_after_pull = [-0.086040, 0.294412, 0.260207, 3, 0, 1.202130] ur5.goto_pose_target(eelink_pose_after_pull) print(ur5.get_joint_values()) grp.set_named_target('open') grp.go(wait=True)
def mainLoop(): global currState global CMD_in global cmd_pub # #Gripper is a C-Model with a TCP connection # gripper = robotiq_c_model_control.baseCModel.robotiqBaseCModel() # gripper.client = robotiq_modbus_rtu.comModbusRtu.communication() # #We connect to the address received as an argument # gripper.client.connectToDevice(device) rospy.init_node('psocSubscribeTest') # Initialize ur5 interface ur5 = UR5Interface() #Each Sensor Reading is Published to topic 'SensorReading' cmd_pub = rospy.Publisher('cmdToPsoc', cmdToPsoc, queue_size=1) rospy.Subscriber('SensorPacket', SensorPacket, callback) time.sleep(1) print("publishing start cmd") cmd_msg = cmdToPsoc(START_CMD) cmd_pub.publish(cmd_msg) time.sleep(50) cmd_msg = cmdToPsoc(IDLE_CMD) print("publishing idle cmd") cmd_pub.publish(cmd_msg) #We loop while not rospy.is_shutdown(): pass
def test_move_ur5(): rospy.init_node("test_move_ur5", anonymous=True, disable_signals=True) ur5 = UR5Interface() # MoveIt! works well if joint limits are smaller (within -pi, pi) if not ur5.check_joint_limits(): raise Exception( 'Bad joint limits! try running roslaunch with option "limited:=true"' ) current_pose = ur5.get_pose() print("============ Current pose: %s" % current_pose) while (1): ### go to P1 ur5.goto_home_pose() home_pose = ur5.get_pose() ### go to P2 P2_pose = copy.deepcopy(home_pose) P2_pose.position.x += 0.1 ur5.goto_pose_target(P2_pose) ### go to P3 P3_pose = copy.deepcopy(home_pose) P3_pose.position.z += 0.1 ur5.goto_pose_target(P3_pose)
def move_home(): # Initialize the ros node rospy.init_node("test_move_home", anonymous=True, disable_signals=True) # Instantiate the UR5 interface. ur5 = UR5Interface() print(ur5.get_joint_values())
def test(): # Initialize the ros node rospy.init_node("test", anonymous=True, disable_signals=True) # Instantiage the UR5 interface. ur5 = UR5Interface() print(ur5.get_rpy()) print(ur5.get_pose()) print(ur5.get_joint_values())
def move_home(): # Initialize the ros node rospy.init_node("test_move_home", anonymous=True, disable_signals=True) # Instantiate the UR5 interface. ur5 = UR5Interface() # go to home and print the joint values ur5.set_speed(0.1) ur5.goto_home_down() print(ur5.get_joint_values())
def test_move_home(): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node("test_move", anonymous=True, disable_signals=True) ur5 = UR5Interface() # MoveIt! works well if joint limits are smaller (within -pi, pi) if not ur5.check_joint_limits(): raise Exception( 'Bad joint limits! try running roslaunch with option "limited:=true"' ) ### go to P1 ur5.goto_home_pose() print(ur5.get_joint_values())
def test_move_home(): """ Function to demonstrate moving the ur5 to home pose """ # Initialize the ros node rospy.init_node("test_move_home", anonymous=True, disable_signals=True) # Instantiage the UR5 interface. ur5 = UR5Interface() # MoveIt! works well if joint limits are smaller (within -pi, pi) #if not ur5.check_joint_limits(): # raise Exception('Bad joint limits! try running roslaunch with option "limited:=true"') # go to home and print the joint values ur5.goto_home_pose() print(ur5.get_joint_values())
def test_move_ur5_continuous(): rospy.init_node("test_move_ur5_continuous", anonymous=True, disable_signals=True) ur5 = UR5Interface() # MoveIt! works well if joint limits are smaller (within -pi, pi) if not ur5.check_joint_limits(): raise Exception( 'Bad joint limits! try running roslaunch with option "limited:=true"' ) ur5.goto_home_pose() # predefine all the poses that we want to go to home_pose = ur5.get_pose() P2_pose = copy.deepcopy(home_pose) P2_pose.position.x += 0.1 P3_pose = copy.deepcopy(home_pose) P3_pose.position.z += 0.1 print("============ Current pose: %s" % current_pose) # The following commands are just to stall the script print("============ Press `Enter` to continue the movement ...") raw_input() # loop through the waypoints while (1): ### go to P1 ur5.goto_home_pose(wait=False) time.sleep(INTER_COMMAND_DELAY) ### go to P2 ur5.goto_pose_target(P2_pose, wait=False) time.sleep(INTER_COMMAND_DELAY) ### go to P3 ur5.goto_pose_target(P3_pose, wait=False) time.sleep(INTER_COMMAND_DELAY)
def debug_ur5(): rospy.init_node("test_move_ur5", anonymous=True, disable_signals=True) ur5 = UR5Interface() # MoveIt! works well if joint limits are smaller (within -pi, pi) if not ur5.check_joint_limits(): raise Exception( 'Bad joint limits! try running roslaunch with option "limited:=true"' ) current_pose = ur5.get_pose() print("============ Current pose: %s" % current_pose) home_pose = ur5.get_pose() P2_pose = copy.deepcopy(home_pose) P2_pose.position.z -= 0.002 ur5.goto_pose_target(P2_pose) current_value = ur5.get_joint_values() print("joint_values", current_value)
def test_get_xyz(): # Initialize the ros node rospy.init_node("test_get_xyz", anonymous=True, disable_signals=True) # Instantiage the UR5 interface. ur5 = UR5Interface() # MoveIt! works well if joint limits are smaller (within -pi, pi) if not ur5.check_joint_limits(): raise Exception( 'Bad joint limits! try running roslaunch with option "limited:=true"' ) ur5.get_pose() print("get_pose", ur5.get_pose()) ur5.get_rpy() print("get_rpy", ur5.get_rpy()) ur5.get_pose_array() print("pose_array", ur5.get_pose_array()) ur5.get_joint_values() print("get_joint_values", ur5.get_joint_values())
def move_home(): # Initialize the ros node rospy.init_node("test_move_home", anonymous=True, disable_signals=True) # Instantiate the UR5 interface. ur5 = UR5Interface() # go to home and print the joint values ur5.set_speed(0.1) ur5.goto_home_down() maintenance_pose = ur5.get_pose() quat = quaternion_from_euler(0, -3.14159, 0) og_quat = get_quaternion(maintenance_pose) new_quat = quaternion_multiply(quat, og_quat) maintenance_pose = set_quaternion(maintenance_pose, new_quat) maintenance_pose.position.y += 0.3 ur5.goto_pose_target(maintenance_pose, wait=False) print(ur5.get_joint_values())
def test(): # Initialize the ros node rospy.init_node("test", anonymous=True, disable_signals=True) # Instantiage the UR5 interface. ur5 = UR5Interface() #grp = moveit_commander.MoveGroupCommander("gripper") pose = [-0.0856014049344, 0.372985176034, 0.278033073131, 1.5748219755267783, 0.01495954454187348, 1.5931041952740963] # pose = [-0.0886014049344, 0.367985176034, 0.278033073131, 1.5748219755267783, 0.01495954454187348, 1.5931041952740963] ur5.goto_pose_target(pose) init_grp_pose1 = 0.3 init_grp_pose1_list = [init_grp_pose1, init_grp_pose1, -init_grp_pose1, -init_grp_pose1, init_grp_pose1, init_grp_pose1] arr_init_g_pos1 = np.array(init_grp_pose1_list, dtype='float32') for update in range(200): GrpCommand(arr_init_g_pos1) time.sleep(1) print(ur5.get_rpy()) print(ur5.get_pose()) print(ur5.get_joint_values())
def move_home(): # Initialize the ros node rospy.init_node("test_move_home", anonymous=True, disable_signals=True) # Instantiate the UR5 interface. ur5 = UR5Interface() # go to home and print the joint values ur5.set_speed(0.1) ur5.goto_home_front() # # home_pose = ur5.get_pose() # # front_pose = copy.deepcopy(home_pose) # print(front_pose) # # quat_15x = quaternion_from_euler(3.14159/12*2,0,0) # og_quat = get_quaternion(front_pose) # print(og_quat) # # new_quat = quaternion_multiply(quat_15x, og_quat) # front_pose = set_quaternion(front_pose,new_quat) # front_pose.position.z += .05 # print(front_pose) # # ur5.goto_pose_target(front_pose) # # new_quat = quaternion_multiply(quat_15x, new_quat) # front_pose = set_quaternion(front_pose,new_quat) # front_pose.position.z += .05 # ur5.goto_pose_target(front_pose) # print(ur5.get_joint_values())
def pinch_test_arm_control(): """ Function to demonstrate moving the ur5 to home pose """ # Initialize the ros node rospy.init_node("pinch_test_arm_control", anonymous=True, disable_signals=True) # Setup subscription to cmd_motor_controller topic rospy.Subscriber("master_state", String, state_callback) # Publish just z position for plotting later z_pos_pub = rospy.Publisher('ur5_position', z_pos, queue_size=10) # Instantiate the UR5 interface. ur5 = UR5Interface() ur5.goto_home_down() home_pose = ur5.get_pose() move_completed = 1 while not rospy.is_shutdown(): global move_completed if (move_completed == 0): if (state == HOME): print("Moving to home position") ur5.set_speed(.15) ur5.goto_home_down() home_pose = ur5.get_pose() move_completed = 1 start_pose = copy.deepcopy(home_pose) ur5.set_speed(.15) start_pose = relative_pose(home_pose, -0.05, 0.1, .1, 0, 0, 0) ur5.goto_pose_target(start_pose, wait=False) move_completed = 1 # Go down to plate elif (state == START): print("Going to plate") # start_pose = copy.deepcopy(home_pose) ur5.set_speed(.15) start_pose = relative_pose(start_pose, 0, 0, -.06, 0, 0, 0) ur5.goto_pose_target(start_pose, wait=False) move_completed = 1 # Go back up elif (state == MOVE_1): print("Going back up") ur5.set_speed(.15) pose_1 = relative_pose(start_pose, 0, 0, .2, 0, 0, 0) ur5.goto_pose_target(pose_1, wait=False) move_completed = 1 # Rotate and go above dish rack elif (state == MOVE_2): print("Going above dish rack") pose_2 = relative_pose(pose_1, -.05, .1, -.17, 0, -3.14159 / 2 + 0.25, 0) ur5.goto_pose_target(pose_2, wait=False) move_completed = 1 # Go down to dish rack elif (state == MOVE_3): print("Putting plate in shelf") pose_3 = relative_pose(pose_2, 0, 0, 0, 0, 0, 3.14159 / 4) # ur5.goto_pose_target(pose_3, wait = False) move_completed = 1 # Go back up elif (state == MOVE_4): print("Going back up") ur5.goto_home_down() pose_4 = relative_pose(pose_3, 0, 0, 0, 0, 0, 3.14159 / 8) # ur5.goto_pose_target(pose_4, wait=False) move_completed = 1 # Go back down elif (state == MOVE_11): print("Rotate hand") pose_5 = relative_pose(pose_4, 0, 0, 0, 0, 0, 3.14159 / 2) # ur5.goto_pose_target(pose_5, wait=False) move_completed = 1 # Go back down elif (state == MOVE_5): print("Go back down to plate for pinch") pose_5 = relative_pose(pose_5, 0, 0, -.05, 0, 0, 0) ur5.goto_pose_target(pose_5, wait=False) move_completed = 1 # Pick plate up elif (state == MOVE_6): print("Pick up plate") pose_6 = relative_pose(pose_5, 0, 0, .15, 0, 0, 0) ur5.goto_pose_target(pose_6, wait=False) move_completed = 1 # Rotate plate elif (state == MOVE_7): print("Rotate plate") pose_7 = relative_pose(pose_6, 0, 0, 0, 0, -3.14159 / 2 + 0.2, 0) ur5.goto_pose_target(pose_7, wait=False) move_completed = 1 # Insert plate into shelf elif (state == MOVE_8): print("Put plate in shelf") pose_8 = relative_pose(pose_7, .1, 0, 0, 0, 0, 0) ur5.goto_pose_target(pose_8, wait=False) move_completed = 1 # Return to home elif (state == MOVE_9): print("return to home") pose_9 = relative_pose(pose_8, -.15, 0, .05, 0, 3.14159 / 2 - 0.2, 0) ur5.goto_pose_target(pose_9, wait=False) move_completed = 1
def test_move(): global client, gripper_pub, camera_service try: moveit_commander.roscpp_initialize(sys.argv) rospy.init_node("test_move", anonymous=True, disable_signals=True) camera_service = rospy.ServiceProxy('calibrate', Calibrate) pcd_service = rospy.ServiceProxy('acquire', Acquire) #resp = pcd_service('bowl_pcd.pcd') #print("Done") ur5 = UR5Interface() # MoveIt! works well if joint limits are smaller (within -pi, pi) if not ur5.check_joint_limits(): raise Exception( 'Bad joint limits! try running roslaunch with option "limited:=true"' ) current_pose = ur5.get_pose() print "============ Current pose: %s" % current_pose ### go to P1 ur5.goto_home_pose() ### sampling P1 P1_C1 = sample_led(1) P1_C2 = sample_led(2) ### go to P2 P2_pose = ur5.get_pose() P2_pose.position.x += 0.1 ur5.goto_pose_target(P2_pose) ### sampling P2 P2_C1 = sample_led(1) P2_C2 = sample_led(2) ### go to P3 P3_pose = ur5.get_pose() P3_pose.position.z += 0.1 ur5.goto_pose_target(P3_pose) ### sampling P3 P3_C1 = sample_led(1) P3_C2 = sample_led(2) print("Camera1 P1: %s, \nP2: %s, \nP3: %s" % (P1_C1, P2_C1, P3_C1)) # Get the transform from Camera to calibration coordinates F T_C1_F = solve_transform(P1_C1, P2_C1, P3_C1) print("Camera2 P1: %s, \nP2: %s, \nP3: %s" % (P1_C2, P2_C2, P3_C2)) # Get the transform from Camera to calibration coordinates F T_C2_F = solve_transform(P1_C2, P2_C2, P3_C2) # Transform from robot end effector to F T_F_6 = np.eye(4) T_F_6[:3, 3] = np.array([0.0, 0.0375, -0.0115]) T_O_6 = np.eye(4) T_O_6[:3, 3] = np.array([-0.15, 0.35, 0.25]) T_C1_6 = T_C1_F.dot(T_F_6) T_C1_O = T_C1_6.dot(np.linalg.inv(T_O_6)) T_O_C1 = np.linalg.inv(T_C1_O) path = os.path.dirname(os.path.abspath(__file__)) + "/config/" np.save(path + 'T_O_C1.npy', T_O_C1) T_C2_6 = T_C2_F.dot(T_F_6) T_C2_O = T_C2_6.dot(np.linalg.inv(T_O_6)) T_O_C2 = np.linalg.inv(T_C1_O) np.save(path + 'T_O_C2.npy', T_O_C2) np.save(path + 'T_C2_C1.npy', np.linalg.inv(T_O_C2).dot(T_O_C1)) #current_pose = group.get_current_pose().pose #print "============ Current pose: %s" % current_pose # Initialize robotiq publishers #gripper_pub = rospy.Publisher('Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output) #command = outputMsg.Robotiq2FGripper_robot_output() # command to open gripper #command.rPR = 0 #grippe_pub.publish(command) except KeyboardInterrupt: rospy.signal_shutdown("KeyboardInterrupt") raise
def mainLoop(): global dl_service global ur5 rospy.init_node('regraspExp', disable_signals=True) # Make this node a service dl_service = rospy.ServiceProxy('logData', logData) # Initialize ur5 interface ur5 = UR5Interface() ur5.goto_home_pose() time.sleep(2) # Initialize robotiq interface gripper = RobotiqInterface() # Run data logger process os.system("rosrun regrasp_exp exp_data_log.py &") time.sleep(4) # Start logging data resp = dl_service('Start') ur5.goto_joint_target(PREGRASP_POSE, wait=False) time.sleep(2) while not rospy.is_shutdown(): gripper.goto_gripper_pos(180) time.sleep(1) ur5.goto_home_pose(wait=False) time.sleep(1) ur5.goto_joint_target(START_POSE, wait=False) time.sleep(1) gripper.goto_gripper_pos(120) time.sleep(10) ur5.goto_home_pose(wait=False) time.sleep(1) gripper.goto_gripper_pos(180) time.sleep(1) ur5.goto_joint_target(PREGRASP_POSE, wait=False) gripper.goto_gripper_pos(0) time.sleep(1)
def pinch_test_arm_control(): """ Function to demonstrate moving the ur5 to home pose """ # Initialize the ros node rospy.init_node("pinch_test_arm_control", anonymous=True, disable_signals=True) # Setup subscription to cmd_motor_controller topic rospy.Subscriber("master_state", String, state_callback) # Publish just z position for plotting later z_pos_pub = rospy.Publisher('ur5_position', z_pos, queue_size=10) # Instantiate the UR5 interface. ur5 = UR5Interface() ur5.goto_home_down() home_pose = ur5.get_pose() move_completed = 1 while not rospy.is_shutdown(): global move_completed if (move_completed == 0): if (state == HOME): print("Moving to home position") ur5.set_speed(.1) ur5.goto_home_down() home_pose = ur5.get_pose() move_completed = 1 elif (state == START): print("Moving to start position") start_pose = copy.deepcopy(home_pose) ur5.set_speed(.1) start_pose.position.z += 0.1 ur5.goto_pose_target(start_pose, wait = False) move_completed = 1 elif (state == MOVE_1): print("Performing first movement") ur5.set_speed(.05) pose_1 = relative_pose(home_pose, 0, 0, -0.023, 0, 0, 0) ur5.goto_pose_target(pose_1, wait = False) move_completed = 1 elif (state == MOVE_2): print("Performing second movement") pose_2 = relative_pose(pose_1, 0, 0, .1, 0,0,0) ur5.goto_pose_target(pose_2, wait = False) move_completed = 1 elif (state == MOVE_3): print("Performing third movement") ur5.set_speed(.15) pose_3 = relative_pose(pose_2, 0,0,0,0,0,-3.14159/2) ur5.goto_pose_target(pose_3, wait = False) move_completed = 1 elif (state == MOVE_4): print("Performing fourth movement") ur5.set_speed(.15) pose_4 = relative_pose(pose_3, 0, 0, 0, 0, 0, 3.14159/2) ur5.goto_pose_target(pose_4, wait=False) move_completed = 1 elif (state == MOVE_5): print("Performing fifth movement") ur5.set_speed(.05) pose_4 = relative_pose(pose_4, 0, .1, 0, 3.14159 / 4, 0, 0) ur5.goto_pose_target(pose_4, wait=False) move_completed = 1 elif (state == MOVE_6): print("Performing sixth movement") move_completed = 1 elif (state == MOVE_7): print("Performing seventh movement") move_completed = 1 elif (state == MOVE_8): print("Performing eighth movement") move_completed = 1
def pinch_test_arm_control(): """ Function to demonstrate moving the ur5 to home pose """ # Initialize the ros node rospy.init_node("pinch_test_arm_control", anonymous=True, disable_signals=True) # Setup subscription to cmd_motor_controller topic rospy.Subscriber("master_state", String, state_callback) # Publish just z position for plotting later z_pos_pub = rospy.Publisher('ur5_position', z_pos, queue_size=10) # Instantiate the UR5 interface. ur5 = UR5Interface() ur5.goto_home_down() home_pose = ur5.get_pose() move_completed = 1 while not rospy.is_shutdown(): global move_completed if (move_completed == 0): OFFSET_VALUE = 0.006 FULL_DISPLACEMENT = 0.02 if (state == HOME): print("Moving to home position") ur5.set_speed(.1) ur5.goto_home_down() home_pose = ur5.get_pose() move_completed = 1 elif (state == START): print("Moving to start position") start_pose = copy.deepcopy(home_pose) ur5.set_speed(.1) start_pose.position.z -= 0.14 start_pose.position.x -= 0.013 ur5.goto_pose_target(start_pose, wait=False) move_completed = 1 elif (state == DISPLACE_ONE): print("Performing first displacement") ur5.set_speed(.01) first_pose = copy.deepcopy(start_pose) first_pose.position.z += OFFSET_VALUE first_pose.position.x += 0.006 ur5.goto_pose_target(first_pose, wait=False) move_completed = 1 elif (state == DISPLACE_TWO): print("Performing second displacement") ur5.set_speed(.003) second_pose = copy.deepcopy(start_pose) second_pose.position.z += FULL_DISPLACEMENT second_pose.position.x += .012 ur5.goto_pose_target(second_pose, wait=False) move_completed = 1 cur_pose = ur5.get_pose() cur_z_pos = cur_pose.position.z z_pos_pub.publish(cur_z_pos)
def master_state_machine(): # Set up publisher to master_state topic pub_master_state = rospy.Publisher('master_state', String, queue_size=10) # State node at 10hz (speed should be flexible) rospy.init_node('master_state_machine', anonymous=True) # rate = rospy.Rate(10) # 10hz # Instantiate the UR5 interface. ur5 = UR5Interface() # ur5.goto_home_down() home_pose = ur5.get_pose() # Enter main control loop (this is blocking based on user input right now) while not rospy.is_shutdown(): # Display control options print('Enter a Command:') print('[space] = stop all motors') print('[1] = home fingers') print('[2] = test pregrasp') print('[3] = test grasp') print('[4] = tighten tendons') print('[5] = loosen tendons') print('[6] = start grasp sequence') print('[0] = home arm') input_string = raw_input('Input: ') # Handle input and publish appropriate state if (input_string == ' '): state_string = "stop" pub_master_state.publish(state_string) elif (input_string == '1'): state_string = "home_fingers" pub_master_state.publish(state_string) elif (input_string == 'q'): state_string = "grasp_1" pub_master_state.publish(state_string) elif (input_string == 'w'): state_string = "grasp_2" pub_master_state.publish(state_string) elif (input_string == 'e'): state_string = "grasp_3" pub_master_state.publish(state_string) elif (input_string == '4'): state_string = "tighten" pub_master_state.publish(state_string) elif (input_string == '5'): state_string = "loosen" pub_master_state.publish(state_string) elif (input_string == '0'): ur5.set_speed(.1) ur5.goto_home_down() elif (input_string == 'x'): old_pose = ur5.get_pose() while (True): user_input = raw_input("X change: ") if (user_input == ' '): break change = float(user_input) new_pose = relative_pose(old_pose, change, 0, 0, 0, 0, 0) ur5.goto_pose_target(new_pose, wait=False) old_pose = new_pose elif (input_string == 'y'): old_pose = ur5.get_pose() while (True): user_input = raw_input("Y change: ") if (user_input == ' '): break change = float(user_input) new_pose = relative_pose(old_pose, 0, change, 0, 0, 0, 0) ur5.goto_pose_target(new_pose, wait=False) old_pose = new_pose elif (input_string == 'z'): old_pose = ur5.get_pose() while (True): user_input = raw_input("Z change: ") if (user_input == ' '): break change = float(user_input) new_pose = relative_pose(old_pose, 0, 0, change, 0, 0, 0) ur5.goto_pose_target(new_pose, wait=False) old_pose = new_pose elif (input_string == '6'): # Start grasp sequence # Press enter to step through sequence # Type space to break out of sequence and stop fingers # Type 'b' to go backwards a step print("\nTesting sequence started!\n") num_steps = 15 step = 0 manual_stepping = True while (step <= num_steps): if (manual_stepping): user_input = raw_input("\nENTER for next step, type 'b' for previous, and ' ' for stop: ") else: user_input = 'auto' # Handle user input to step through demo sequence if (user_input == ''): step += 1 elif (user_input == 'b'): step -= 1 elif (user_input == ' '): # Stop everything print("Halting test sequence!") break elif (user_input == 'auto'): # Do nothing print("Automatically stepping") elif (user_input == '1'): state_string = "home_fingers" pub_master_state.publish(state_string) elif (input_string == '4'): state_string = "tighten" pub_master_state.publish(state_string) elif (input_string == '5'): state_string = "loosen" pub_master_state.publish(state_string) elif (input_string == '2'): state_string = "pregrasp_wide" pub_master_state.publish(state_string) elif (input_string == '3'): state_string = "grasp_wide" pub_master_state.publish(state_string) # Handle arm movement situation for each step if (step == 1): print("Going to start position") start_pose = relative_pose(home_pose, 0, 0, 0, 0, 0, -3.14159/2) ur5.goto_pose_target(start_pose, wait=False) elif (step == 2): print("Pregrasping") pub_master_state.publish("pregrasp_wide") elif (step == 3): print("Going to second position") pose_2 = relative_pose(start_pose, 0, 0, -0.14, 0, 0, 0) ur5.goto_pose_target(pose_2, wait=False) elif (step == 4): print("Grasping") pub_master_state.publish("grasp_wide") elif (step == 5): print("Picking up") pose_3 = relative_pose(pose_2, 0, 0, 0.1, 0, 0, 0) ur5.goto_pose_target(pose_3, wait=False) elif (step == 6): print("Rotating") pose_4 = relative_pose(pose_3, 0, 0, -.1, 0, 3.14159/2, 0) ur5.goto_pose_target(pose_4, wait=False) elif (step == 7): print("Placing") pose_5 = relative_pose(pose_4, -.1, 0, -.12, 0, 0, 0) ur5.goto_pose_target(pose_5, wait=False) elif (step == 8): print("Releasing") pub_master_state.publish('grasp_4') elif (step == 9): print("Leaving") pose_6 = relative_pose(pose_5, 0, 0, .1, 0, 0, 0) ur5.goto_pose_target(pose_6, wait=False)
def pinch_test_arm_control(): """ Function to demonstrate moving the ur5 to home pose """ # Initialize the ros node rospy.init_node("pinch_test_arm_control", anonymous=True, disable_signals=True) # Setup subscription to cmd_motor_controller topic rospy.Subscriber("master_state", String, state_callback) # Publish just z position for plotting later z_pos_pub = rospy.Publisher('ur5_position', z_pos, queue_size=10) # Instantiate the UR5 interface. ur5 = UR5Interface() ur5.goto_home_down() home_pose = ur5.get_pose() move_completed = 1 while not rospy.is_shutdown(): global move_completed if (move_completed == 0): if (state == HOME): print("Moving to home position") ur5.set_speed(.15) ur5.goto_home_down() home_pose = ur5.get_pose() move_completed = 1 elif (state == START): print("Moving to start position") start_pose = copy.deepcopy(home_pose) ur5.set_speed(.15) start_pose = relative_pose(home_pose, .19, 0, 0, 0, 0, 0) ur5.goto_pose_target(start_pose, wait=False) move_completed = 1 elif (state == MOVE_1): print("Going to grapes") ur5.set_speed(.15) pose_1 = relative_pose(start_pose, 0, 0, -.14, 0, 0, 0) ur5.goto_pose_target(pose_1, wait=False) move_completed = 1 elif (state == MOVE_2): print("Picking grapes up") pose_2 = relative_pose(pose_1, 0, 0, 0.25, 0, 0, -3.14159 / 2) ur5.goto_pose_target(pose_2, wait=False) move_completed = 1 elif (state == MOVE_3): print("Putting grapes down") pose_3 = relative_pose(pose_1, 0, 0, 0, 0, 0, 0) ur5.goto_pose_target(pose_3, wait=False) move_completed = 1 elif (state == MOVE_11): print("Going back up") pose_3 = relative_pose(pose_1, 0, 0, .1, 0, 0, 0) ur5.goto_pose_target(pose_3, wait=False) move_completed = 1 ##### NOW GO HOME ##### elif (state == MOVE_4): print("GO to pomelo") pose_4 = relative_pose(home_pose, 0, 0, -.13, 0, 0, 0) ur5.goto_pose_target(pose_4, wait=False) move_completed = 1 elif (state == MOVE_5): print("Pick up pomelo") pose_5 = relative_pose(pose_4, 0, 0.1, 0.13, 3.14159 / 4, 0, 0) ur5.goto_pose_target(pose_5, wait=False) move_completed = 1 elif (state == MOVE_6): print("Put down pomelo") pose_6 = relative_pose(pose_4, 0, 0, 0, 0, 0, 0) ur5.goto_pose_target(pose_6, wait=False) move_completed = 1 #### GO TO HOME ##### elif (state == MOVE_12): print("Go above pepper") pose_6 = relative_pose(pose_6, -.2, 0, .13, 0, 0, 0) ur5.goto_pose_target(pose_6, wait=False) move_completed = 1 elif (state == MOVE_7): print("Go to pepper") pose_7 = relative_pose(pose_6, 0, 0, -.14, 0, 0, -3.14159 / 4) ur5.goto_pose_target(pose_7, wait=False) move_completed = 1 elif (state == MOVE_8): print("Performing sixth movement") pose_8 = relative_pose(pose_7, 0, 0, 0.05, 0, 0, 0) ur5.goto_pose_target(pose_8, wait=False) move_completed = 1 elif (state == MOVE_9): print("Performing ninth movement") pose_9 = relative_pose(pose_8, -.05, -0.05, -0.06, 0, 0, 0) ur5.goto_pose_target(pose_9, wait=False) move_completed = 1 elif (state == MOVE_10): print("Performing tenth movement") pose_10 = relative_pose(pose_9, 0, 0, .14, 0, 0, 0) ur5.goto_pose_target(pose_10, wait=False) move_completed = 1