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
