def detach_box(self):
self._scene.remove_attached_object(self._eef_link, name=self.box_name)
req_detach = vacuumGripperRequest()
req_detach.activate_vacuum_gripper = False
resp = self._activate_vacuum_gripper(req_detach)
print(resp)
self._scene.remove_world_object(self.box_name)
def attach_box(self, timeout=4):
touch_links = self._eef_link
self._scene.attach_box(self._eef_link,
self.box_name,
touch_links=touch_links)
req_attach = vacuumGripperRequest()
req_attach.activate_vacuum_gripper = True
resp = self._activate_vacuum_gripper(req_attach)
print(resp)
def detach_box(self):
'''DETACH BOX
USED TO DETACH BOX FROM UR5 IN BOTH GAZEBO AND RVIZ.'''
self._scene.remove_attached_object(self._eef_link, name=self.box_name)
req_detach = vacuumGripperRequest()
req_detach.activate_vacuum_gripper = False
resp = self._activate_vacuum_gripper(req_detach)
print resp
self._scene.remove_world_object(self.box_name)
def attach_box(self, timeout=4):
self.box_pose = geometry_msgs.msg.PoseStamped()
self.box_pose.header.frame_id = "ur5_wrist_3_link"
self.box_pose.pose.orientation.w = 1.0
self.box_pose.pose.position.x = 0.0
self.box_pose.pose.position.y = 0.2
self.box_pose.pose.position.z = 0
self.box_name = "box"
self._scene.add_box(self.box_name,
self.box_pose,
size=(0.16, 0.16, 0.16))
touch_links = "ur5_wrist_3_link" #self._eef_link
self._scene.attach_box(self._eef_link,
self.box_name,
touch_links=touch_links)
req_attach = vacuumGripperRequest()
req_attach.activate_vacuum_gripper = True
resp = self._activate_vacuum_gripper(req_attach)
print(resp)
def attach_box(self):
'''ATTACH BOX
FUNCTION USED TO ATTACH BOX TO UR5 ARM IN GAZEBO AND RVIZ BOTH.'''
self.box_pose = geometry_msgs.msg.PoseStamped()
self.box_pose.header.frame_id = "ur5_wrist_3_link"
self.box_pose.pose.orientation.w = 1.0
self.box_pose.pose.position.x = 0.0
self.box_pose.pose.position.y = 0.2
self.box_pose.pose.position.z = 0
self.box_name = "box"
self._scene.add_box(self.box_name,
self.box_pose,
size=(0.16, 0.16, 0.16))
touch_links = "ur5_wrist_3_link" #self._eef_link
self._scene.attach_box(self._eef_link,
self.box_name,
touch_links=touch_links)
req_attach = vacuumGripperRequest()
req_attach.activate_vacuum_gripper = True
resp = self._activate_vacuum_gripper(req_attach)
print resp
def main():
"""Main"""
rospy.sleep(10)
rospy.sleep(10)
ur5 = Ur5Moveit(sys.argv[1])
rospy.sleep(10)
rospy.sleep(10)
rospy.sleep(10)
rospy.sleep(10)
rospy.wait_for_service("/eyrc/vb/ur5/activate_vacuum_gripper/ur5_1")
client = rospy.ServiceProxy("/eyrc/vb/ur5/activate_vacuum_gripper/ur5_1",
vacuumGripper)
ur5_2_home_pose = geometry_msgs.msg.Pose()
ur5_2_home_pose.position.x = -0.8
ur5_2_home_pose.position.y = 0
ur5_2_home_pose.position.z = 1.20
# This to keep EE parallel to Ground Plane
ur5_2_home_pose.orientation.x = -0.5
ur5_2_home_pose.orientation.y = -0.5
ur5_2_home_pose.orientation.z = 0.5
ur5_2_home_pose.orientation.w = 0.5
lst_joint_angles_home = [
math.radians(172),
math.radians(-40),
math.radians(58),
math.radians(-108),
math.radians(-90),
math.radians(-7.8)
]
lst_joint_angles_straight = [
math.radians(0),
math.radians(-90),
math.radians(0),
math.radians(0),
math.radians(0),
math.radians(0)
]
lst_joint_angles_straight_180 = [
math.radians(180),
math.radians(-90),
math.radians(0),
math.radians(0),
math.radians(0),
math.radians(0)
]
lst_joint_angles_1 = [
math.radians(-57),
math.radians(-66),
math.radians(-5),
math.radians(-103),
math.radians(-116),
math.radians(-83)
]
lst_joint_angles_1_back = [
math.radians(-38),
math.radians(-64),
math.radians(-23),
math.radians(-86),
math.radians(-135),
math.radians(-80)
]
lst_joint_angles_2 = [
math.radians(-123),
math.radians(-62),
math.radians(-31),
math.radians(-87.7),
math.radians(-57),
math.radians(0)
]
lst_joint_angles_2_back = [
math.radians(-171),
math.radians(-108),
math.radians(-40),
math.radians(-113),
math.radians(-9),
math.radians(0)
]
lst_joint_angles_3 = [
math.radians(49),
math.radians(-112),
math.radians(-8),
math.radians(-58),
math.radians(132),
math.radians(0)
]
lst_joint_angles_3_back = [
math.radians(35),
math.radians(-100),
math.radians(-28),
math.radians(-50),
math.radians(145),
math.radians(0)
]
lst_joint_angles_4 = [
math.radians(-57),
math.radians(-82),
math.radians(34),
math.radians(50),
math.radians(121),
math.radians(1)
]
lst_joint_angles_4_back = [
math.radians(-40),
math.radians(-91),
math.radians(43),
math.radians(51),
math.radians(137),
math.radians(1)
]
lst_joint_angles_5 = [
math.radians(125),
math.radians(-81),
math.radians(-46),
math.radians(126),
math.radians(-55),
math.radians(0)
]
lst_joint_angles_5_back = [
math.radians(173),
math.radians(-67),
math.radians(-52),
math.radians(116),
math.radians(-8),
math.radians(4)
]
lst_joint_angles_5_back_2 = [
math.radians(152),
math.radians(-68),
math.radians(-48),
math.radians(-66),
math.radians(40),
math.radians(2)
]
lst_joint_angles_6 = [
math.radians(48),
math.radians(-158),
math.radians(70),
math.radians(-90),
math.radians(133),
math.radians(0)
]
lst_joint_angles_6_1 = [
math.radians(49),
math.radians(-92),
math.radians(-69),
math.radians(-17),
math.radians(133),
math.radians(0)
]
lst_joint_angles_6_back = [
math.radians(37),
math.radians(-86),
math.radians(-75),
math.radians(-16),
math.radians(145),
math.radians(0)
]
lst_joint_angles_7 = [
math.radians(-56),
math.radians(-99),
math.radians(88),
math.radians(11),
math.radians(121),
math.radians(0)
]
lst_joint_angles_7_back = [
math.radians(-35),
math.radians(-109),
math.radians(96),
math.radians(13),
math.radians(142),
math.radians(0)
]
lst_joint_angles_8 = [
math.radians(116),
math.radians(-63),
math.radians(-96),
math.radians(159),
math.radians(-64),
math.radians(0)
]
lst_joint_angles_8_back = [
math.radians(155),
math.radians(-44),
math.radians(-101),
math.radians(148),
math.radians(-25),
math.radians(0)
]
lst_joint_angles_9 = [
math.radians(48),
math.radians(-89),
math.radians(-77),
math.radians(166),
math.radians(-132),
math.radians(0)
]
lst_joint_angles_9_back = [
math.radians(37),
math.radians(-83),
math.radians(-82),
math.radians(166),
math.radians(-143),
math.radians(0)
]
lst_joint_angles_10 = [
math.radians(-59),
math.radians(-97),
math.radians(118),
math.radians(-21),
math.radians(118),
math.radians(1)
]
lst_joint_angles_10_back = [
math.radians(-45),
math.radians(-107),
math.radians(126),
math.radians(-18),
math.radians(132),
math.radians(1)
]
lst_joint_angles_11 = [
math.radians(-126),
math.radians(-119),
math.radians(133),
math.radians(-14),
math.radians(51),
math.radians(0)
]
lst_joint_angles_12 = [
math.radians(53),
math.radians(-86),
math.radians(-115),
math.radians(-159),
math.radians(-127),
math.radians(0)
]
lst_joint_angles_12_back = [
math.radians(36),
math.radians(-77),
math.radians(-122),
math.radians(-160),
math.radians(-144),
math.radians(0)
]
while not rospy.is_shutdown():
for i in range(len(ur5.received_msg)):
if ((ur5.received_msg[i] == "HP") and (ur5.color[0] == "red")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_1, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_1_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[0] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "HP") and (ur5.color[1] == "red")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_2, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_2_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[1] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "HP") and (ur5.color[2] == "red")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_3, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_3_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[2] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "HP") and (ur5.color[3] == "red")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_4, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_4_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[3] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "HP") and (ur5.color[4] == "red")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'straight_to_pose5.yaml', 5)
request = vacuumGripperRequest(True)
client(request)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose5_to_pose5back.yaml', 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[4] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "HP") and (ur5.color[5] == "red")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_6_1, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_6_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[5] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "HP") and (ur5.color[6] == "red")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_7, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_7_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[6] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "HP") and (ur5.color[7] == "red")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'straight_to_pose8.yaml', 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_8_back, 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose8back_to_straight.yaml', 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[7] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "HP") and (ur5.color[8] == "red")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'straight_to_pose9.yaml', 5)
request = vacuumGripperRequest(True)
client(request)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose9_to_pose9back.yaml', 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose9back_to_straight.yaml', 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[8] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "HP") and (ur5.color[9] == "red")):
ur5.hard_set_joint_angles(lst_joint_angles_10, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.ee_cartesian_translation(0, 0.5, 0)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[9] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "HP") and (ur5.color[11] == "red")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'straight_to_pose12.yaml', 5)
request = vacuumGripperRequest(True)
client(request)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose12_to_pose12back.yaml', 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose12back_to_straight.yaml', 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[11] = "done"
if ((ur5.received_msg[i] == "MP") and (ur5.color[0] == "yellow")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_1, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_1_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[0] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "MP") and (ur5.color[1] == "yellow")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_2, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_2_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[1] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "MP") and (ur5.color[2] == "yellow")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_3, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_3_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[2] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "MP") and (ur5.color[3] == "yellow")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_4, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_4_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[3] = "done"
if ((ur5.received_msg[i] == "MP") and (ur5.color[5] == "yellow")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_6_1, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_6_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[5] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "MP") and (ur5.color[6] == "yellow")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_7, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_7_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[6] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "MP") and (ur5.color[7] == "yellow")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'straight_to_pose8.yaml', 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_8_back, 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose8back_to_straight.yaml', 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[7] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "MP") and (ur5.color[8] == "yellow")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'straight_to_pose9.yaml', 5)
request = vacuumGripperRequest(True)
client(request)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose9_to_pose9back.yaml', 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose9back_to_straight.yaml', 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[8] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "MP") and (ur5.color[9] == "yellow")):
ur5.hard_set_joint_angles(lst_joint_angles_10, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.ee_cartesian_translation(0, 0.5, 0)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[9] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "MP") and (ur5.color[11] == "yellow")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'straight_to_pose12.yaml', 5)
request = vacuumGripperRequest(True)
client(request)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose12_to_pose12back.yaml', 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose12back_to_straight.yaml', 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[11] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "MP") and (ur5.color[4] == "yellow")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'straight_to_pose5.yaml', 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_5_back, 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose5back_to_straight.yaml', 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[4] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "LP") and (ur5.color[0] == "green")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_1, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_1_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[0] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "LP") and (ur5.color[1] == "green")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_2, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_2_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[1] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "LP") and (ur5.color[2] == "green")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_3, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_3_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[2] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "LP") and (ur5.color[3] == "green")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_4, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_4_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[3] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "LP") and (ur5.color[4] == "green")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'straight_to_pose5.yaml', 5)
request = vacuumGripperRequest(True)
client(request)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose5_to_pose5back.yaml', 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[4] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "LP") and (ur5.color[5] == "green")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_6_1, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_6_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[5] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "LP") and (ur5.color[6] == "green")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_7, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_7_back, 5)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[6] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "LP") and (ur5.color[7] == "green")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'straight_to_pose8.yaml', 5)
request = vacuumGripperRequest(True)
client(request)
ur5.hard_set_joint_angles(lst_joint_angles_8_back, 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose8back_to_straight.yaml', 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[7] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "LP") and (ur5.color[8] == "green")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'straight_to_pose9.yaml', 5)
request = vacuumGripperRequest(True)
client(request)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose9_to_pose9back.yaml', 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose9back_to_straight.yaml', 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[8] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "LP") and (ur5.color[9] == "green")):
ur5.hard_set_joint_angles(lst_joint_angles_10, 5)
request = vacuumGripperRequest(True)
client(request)
ur5.ee_cartesian_translation(0, 0.5, 0)
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[9] = "done"
ur5._pub.publish(str(i))
if ((ur5.received_msg[i] == "LP") and (ur5.color[11] == "green")):
ur5.hard_set_joint_angles(lst_joint_angles_straight, 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'straight_to_pose12.yaml', 5)
request = vacuumGripperRequest(True)
client(request)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose12_to_pose12back.yaml', 5)
ur5.moveit_hard_play_planned_path_from_file(
ur5._file_path, 'pose12back_to_straight.yaml', 5)
ur5.hard_set_joint_angles(lst_joint_angles_home, 5)
request = vacuumGripperRequest(False)
client(request)
ur5.received_msg[i] = "done"
ur5.color[11] = "done"
ur5._pub.publish(str(i))