diff --git a/canonical_assembly.py b/canonical_assembly.py index ceed275..e8af647 100644 --- a/canonical_assembly.py +++ b/canonical_assembly.py @@ -153,7 +153,7 @@ def __init__(self): self.graspConfig["long wire"] = [-0.46015322, 4.47079882, 2.68192519, -2.584758426, -1.74260217, 1.457295330] self.deliveryRotation["long wire"] = 1.0 # self.graspConfig["small box"] = [-2.4191907, 3.9942575, 1.29241768, 3.05926906, -0.50726387, -0.52933128] - self.graspConfig["small box"] =[-2.44125532, 3.83132847, 1.41714937, -2.95087330, -0.85749472, -0.75164206] + self.graspConfig["small box"] =[-2.44054132, 3.90477552, 1.44756147, -2.95127490, -0.86210359, -0.75472121] self.deliveryRotation["small box"] = -1.1 self.graspConfig["tool"] = [-0.32843145, 4.02576609, 1.48440087, -2.87877031, -0.79457283, 1.40310179] self.deliveryRotation["tool"] = 1.05 @@ -236,21 +236,21 @@ def __init__(self): query.setText("Which part(s) do you want?") query.setFont(QFont('Arial', 28)) query.adjustSize() - query.move(145, 135) + query.move(95, 135) # task info assembly_image = QLabel(self) pixmap = QPixmap("src/canonical_task.png") - pixmap = pixmap.scaledToWidth(950) + pixmap = pixmap.scaledToWidth(1125) assembly_image.setPixmap(pixmap) assembly_image.adjustSize() - assembly_image.move(825, 200) + assembly_image.move(660, 145) # inputs options = deepcopy(self.remaining_objects) # print the options - option_x, option_y = 260, 200 + option_x, option_y = 210, 200 buttons = [] for opt in options: opt_button = QPushButton(self) @@ -264,7 +264,7 @@ def __init__(self): self.option_buttons = buttons # button for performing selected actions - option_x = 130 + option_x = 85 option_y += 60 self.selected_button = QPushButton(self) self.selected_button.setText("Give me the selected parts.") @@ -279,7 +279,7 @@ def __init__(self): self.step_label.setText("Current time step: " + str(self.time_step)) self.step_label.setFont(QFont('Arial', 36)) self.step_label.adjustSize() - self.step_label.move(820, 125) + self.step_label.move(715, 65) # update timer self.time_to_respond = 10 @@ -292,7 +292,7 @@ def __init__(self): self.countdown.setFont(QFont('Arial', 36)) self.countdown.setStyleSheet("background-color: khaki") self.countdown.adjustSize() - self.countdown.move(1720, 125) + self.countdown.move(1720, 65) self.countdown_timer = QTimer() self.countdown_timer.timeout.connect(self.timer_update) @@ -425,7 +425,7 @@ def deliver_part(self): self.ada.execute_trajectory(trajectory) # lower gripper - traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., -0.05]) + traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., -0.045]) self.ada.execute_trajectory(traj) # grasp the object diff --git a/data/q_values_42.p b/data/q_values_42.p new file mode 100644 index 0000000..aacc5a6 Binary files /dev/null and b/data/q_values_42.p differ diff --git a/data/q_values_43.p b/data/q_values_43.p new file mode 100644 index 0000000..cdbf4fd Binary files /dev/null and b/data/q_values_43.p differ diff --git a/data/q_values_44.p b/data/q_values_44.p new file mode 100644 index 0000000..a2fd931 Binary files /dev/null and b/data/q_values_44.p differ diff --git a/data/q_values_45.p b/data/q_values_45.p new file mode 100644 index 0000000..13fd720 Binary files /dev/null and b/data/q_values_45.p differ diff --git a/data/q_values_46.p b/data/q_values_46.p new file mode 100644 index 0000000..6148d19 Binary files /dev/null and b/data/q_values_46.p differ diff --git a/data/q_values_48.p b/data/q_values_48.p new file mode 100644 index 0000000..a9ef5ea Binary files /dev/null and b/data/q_values_48.p differ diff --git a/data/q_values_49.p b/data/q_values_49.p new file mode 100644 index 0000000..b5c7201 Binary files /dev/null and b/data/q_values_49.p differ diff --git a/data/q_values_50.p b/data/q_values_50.p new file mode 100644 index 0000000..dda11d2 Binary files /dev/null and b/data/q_values_50.p differ diff --git a/data/states_42.p b/data/states_42.p new file mode 100644 index 0000000..950e5bd Binary files /dev/null and b/data/states_42.p differ diff --git a/data/states_43.p b/data/states_43.p new file mode 100644 index 0000000..950e5bd Binary files /dev/null and b/data/states_43.p differ diff --git a/data/states_44.p b/data/states_44.p new file mode 100644 index 0000000..950e5bd Binary files /dev/null and b/data/states_44.p differ diff --git a/data/states_45.p b/data/states_45.p new file mode 100644 index 0000000..950e5bd Binary files /dev/null and b/data/states_45.p differ diff --git a/data/states_46.p b/data/states_46.p new file mode 100644 index 0000000..950e5bd Binary files /dev/null and b/data/states_46.p differ diff --git a/data/states_48.p b/data/states_48.p new file mode 100644 index 0000000..950e5bd Binary files /dev/null and b/data/states_48.p differ diff --git a/data/states_49.p b/data/states_49.p new file mode 100644 index 0000000..950e5bd Binary files /dev/null and b/data/states_49.p differ diff --git a/data/states_50.p b/data/states_50.p new file mode 100644 index 0000000..950e5bd Binary files /dev/null and b/data/states_50.p differ diff --git a/new_reactive_assembly.py b/new_reactive_assembly.py new file mode 100644 index 0000000..14fc467 --- /dev/null +++ b/new_reactive_assembly.py @@ -0,0 +1,501 @@ +#!/usr/bin/env python3 + +import pdb +import sys +import time +import pickle +import numpy as np +from copy import deepcopy +from threading import Thread + +import adapy +import rospy +from std_msgs.msg import Float64MultiArray +from moveit_ros_planning_interface._moveit_roscpp_initializer import roscpp_init + +from PyQt5.QtWidgets import * +from PyQt5.QtGui import * +from PyQt5.QtCore import * + + +# set to False if operating real robot +IS_SIM = False + + +def createBwMatrixforTSR(): + """ + Creates the bounds matrix for the TSR. + :returns: A 6x2 array Bw + """ + Bw = np.zeros([6, 2]) + Bw[0, 0] = -0.005 + Bw[0, 1] = 0.005 + Bw[1, 0] = -0.005 + Bw[1, 1] = 0.005 + Bw[2, 0] = -0.005 + Bw[2, 1] = 0.005 + Bw[3, 0] = -0.005 + Bw[3, 1] = 0.005 + Bw[4, 0] = -0.005 + Bw[4, 1] = 0.005 + Bw[5, 0] = -0.005 + Bw[5, 1] = 0.005 + + return Bw + + +def createTSR(partPose, graspPose): + """ + Create the TSR for grasping a soda can. + :param partPose: SE(3) transform from world to part. + :param adaHand: ADA hand object + :returns: A fully initialized TSR. + """ + + # set the part TSR at the part pose + partTSR = adapy.get_default_TSR() + T0_w = partTSR.get_T0_w() + T0_w[0:3, 3] = partPose[:3] + partTSR.set_T0_w(T0_w) + + # set the transformed TSR + partTSR.set_Tw_e(graspPose) + Bw = createBwMatrixforTSR() + partTSR.set_Bw(Bw) + + return partTSR + + +def transition(s_from, a): + # preconditions + if a in [0, 1] and s_from[a] < 1: + p = 1.0 + elif a == 2 and s_from[a] < 4 and s_from[0] == 1: + p = 1.0 + elif a == 3 and s_from[a] < 1 and s_from[1] == 1: + p = 1.0 + elif a == 4 and s_from[a] < 4 and s_from[a] + 1 <= s_from[a - 2]: + p = 1.0 + elif a == 5 and s_from[a] < 1 and s_from[a] + 1 <= s_from[a - 2]: + p = 1.0 + elif a == 6 and s_from[a] < 4: + p = 1.0 + elif a == 7 and s_from[a] < 1 and s_from[a - 1] == 4: + p = 1.0 + else: + p = 0.0 + + # transition to next state + if p == 1.0: + s_to = deepcopy(s_from) + s_to[a] += 1 + s_to[-1] = s_from[-2] + s_to[-2] = a + return p, s_to + else: + return p, None + + +# ------------------------------------------------------- MAIN ------------------------------------------------------- # + +class AssemblyController(QMainWindow): + + def __init__(self): + super(AssemblyController, self).__init__() + + # initialize robot + self.ada = adapy.Ada(IS_SIM) + + # ------------------------------------------ Create sim environment ---------------------------------------------- # + + # objects in airplane assembly + storageURDFUri = "package://assembly_demos/urdf_collection/storage.urdf" + storagePose = [0., -0.3, -0.77, 0, 0, 0, 0] + + wingURDFUri = "package://assembly_demos/urdf_collection/abstract_main_wing.urdf" + wingPose = [0.75, -0.3, 0., 0.5, 0.5, 0.5, 0.5] + + tailURDFUri = "package://assembly_demos/urdf_collection/abstract_tail_wing.urdf" + tailPose = [-0.7, -0.25, 0.088, 0.5, 0.5, 0.5, 0.5] + + container1URDFUri = "package://assembly_demos/urdf_collection/container_1.urdf" + container1_1Pose = [0.4, -0.4, 0., 0., 0., 0., 0.] + container1_2Pose = [-0.4, -0.4, 0., 0., 0., 0., 0.] + container1_3Pose = [0.55, -0.3, 0., 0., 0., 0., 0.] + container1_4Pose = [-0.55, -0.3, 0., 0., 0., 0., 0.] + + container2URDFUri = "package://assembly_demos/urdf_collection/container_2.urdf" + container2_1Pose = [0.4, -0.1, 0, 0., 0., 0., 0.] + container2_2Pose = [-0.4, -0.1, 0., 0., 0., 0., 0.] + + container3URDFUri = "package://assembly_demos/urdf_collection/container_3.urdf" + container3_1Pose = [0.6, 0., 0., 0., 0., 0., 0.] + container3_2Pose = [-0.6, 0., 0., 0., 0, 0, 0] + + # grasp TSR and offsets + tailGraspPose = [[1., 0., 0.], [0., 1., 0.], [0., 0., 1]] + tailGraspOffset = [0., 0.175, 0.] + container1GraspPose = [[0., 1., 0., 0.], [1., 0., 0., -0.05], [0., 0., -1, 0.8], [0., 0., 0., 1.]] + container1GraspOffset = [0., 0., -0.07] + container2GraspPose = [[0., 1., 0., 0.], [1., 0., 0., -0.1], [0., 0., -1, 0.1], [0., 0., 0., 1.]] + container2GraspOffset = [0., -0.115, 0.] + container3GraspPose = [[-1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., -1., 0.1], [0., 0., 0., 1.]] + container3GraspOffset = [0., 0., 0.] + + # hard-coded grasps + self.graspConfig, self.deliveryRotation = {}, {} + self.graspConfig["long bolts"] = [-2.11464507, 4.27069802, 2.12562682, -2.9179622, -1.1927828, -0.16230427] + self.deliveryRotation["long bolts"] = -1.34 + self.graspConfig["short bolts"] = [-0.72561783, 4.31588712, 2.28856202, -2.71514972, -1.42200445, 1.01089267] + self.deliveryRotation["short bolts"] = 1.25 + # self.graspConfig["propeller nut"] = [0.49700125, 1.86043184, 3.78425230, 2.63384048, 1.44808279, 1.67817618] + self.graspConfig["propeller nut"] = [-2.03877631, 4.09967790, 1.60438025, -0.19636232, 0.71718155, 2.21799853] + self.deliveryRotation["propeller nut"] = -1.6 + self.graspConfig["tail screw"] = [-0.46015322, 4.47079882, 2.68192519, -2.584758426, -1.74260217, 1.457295330] + self.deliveryRotation["tail screw"] = 1.0 + self.graspConfig["propeller blades"] = [-2.4191907, 3.9942575, 1.29241768, 3.05926906, -0.50726387, -0.52933128] + self.deliveryRotation["propeller blades"] = -1.1 + self.graspConfig["tool"] = [-0.32843145, 4.02576609, 1.48440087, -2.87877031, -0.79457283, 1.40310179] + self.deliveryRotation["tool"] = 1.05 + self.graspConfig["propeller hub"] = [3.00773842, 4.21352853, 1.98663177, -0.17330897, 1.01156224, -0.46210507] + self.deliveryRotation["propeller hub"] = -0.6 + self.graspConfig["tail wing"] = [3.129024, 1.87404028, 3.40826295, 0.53502216, -1.86749865, -0.99044654] + self.deliveryRotation["tail wing"] = 0.7 + self.graspConfig["main wing"] = [-2.86840265, 3.89315136, 1.47980743, -3.07256298, 0.95719655, 2.37149834] + self.deliveryRotation["main wing"] = -0.7 + + # initialize sim environment + self.world = self.ada.get_world() + viewer = self.ada.start_viewer("airplane_assembly_demo", "map") + + # add parts to sim environment + storageInWorld = self.world.add_body_from_urdf(storageURDFUri, storagePose) + container1_1 = self.world.add_body_from_urdf(container1URDFUri, container1_1Pose) + container1_2 = self.world.add_body_from_urdf(container1URDFUri, container1_2Pose) + container1_3 = self.world.add_body_from_urdf(container1URDFUri, container1_3Pose) + container1_4 = self.world.add_body_from_urdf(container1URDFUri, container1_4Pose) + container2_1 = self.world.add_body_from_urdf(container2URDFUri, container2_1Pose) + container2_2 = self.world.add_body_from_urdf(container2URDFUri, container2_2Pose) + container3_1 = self.world.add_body_from_urdf(container3URDFUri, container3_1Pose) + # container3_2 = self.world.add_body_from_urdf(container3URDFUri, container3_2Pose) + tailWing = self.world.add_body_from_urdf(tailURDFUri, tailPose) + mainWing = self.world.add_body_from_urdf(wingURDFUri, wingPose) + + # dict of all objects + self.objects = {"long bolts": [container1_1, container1_1Pose, container1GraspPose, container1GraspOffset], + "short bolts": [container1_2, container1_2Pose, container1GraspPose, container1GraspOffset], + "propeller nut": [container1_3, container1_3Pose, container1GraspPose, container1GraspOffset], + "tail screw": [container1_4, container1_4Pose, container1GraspPose, container1GraspOffset], + "propeller blades": [container2_1, container2_1Pose, container2GraspPose, container2GraspOffset], + "tool": [container2_2, container2_2Pose, container2GraspPose, container2GraspOffset], + "propeller hub": [container3_1, container3_1Pose, container3GraspPose, container3GraspOffset], + "tail wing": [tailWing, tailPose, tailGraspPose, tailGraspOffset], + "main wing": [mainWing], + "airplane body": []} + + # ------------------------------------------------ Get robot config ---------------------------------------------- # + + collision = self.ada.get_self_collision_constraint() + + self.arm_skeleton = self.ada.get_arm_skeleton() + self.arm_state_space = self.ada.get_arm_state_space() + self.hand = self.ada.get_hand() + self.hand_node = self.hand.get_end_effector_body_node() + + viewer.add_frame(self.hand_node) + + # ------------------------------- Start executor for real robot (not needed for sim) ----------------------------- # + + if not IS_SIM: + self.ada.start_trajectory_controllers() + + self.armHome = [-1.57, 3.14, 1.23, -2.19, 1.8, 1.2] + waypoints = [(0.0, self.arm_skeleton.get_positions()), (1.0, self.armHome)] + trajectory = self.ada.compute_joint_space_path(waypoints) # self.ada.plan_to_configuration(self.armHome) + self.ada.execute_trajectory(trajectory) + self.hand.execute_preshape([0.15, 0.15]) + + # ------------------------------------------------- Assembly Info ------------------------------------------------ # + + # objects yet to be delivered + self.remaining_objects = list(self.objects.keys()) + + # subscribe to action recognition + sub_act = rospy.Subscriber("/april_tag_detection", Float64MultiArray, self.callback, queue_size=1) + + # initialize user sequence + self.time_step = 0 + self.user_sequence = [] + self.anticipated_action_name = [] + self.suggested_objects = [] + + # ------------------------------------------------ GUI details --------------------------------------------------- # + + # window title and size + self.setWindowTitle("Robot Commander") + self.setGeometry(0, 0, 1280, 720) + + # prompt + query = QLabel(self) + query.setText("Which part(s) do you want?") + query.setFont(QFont('Arial', 28)) + query.adjustSize() + query.move(95, 135) + + # task info + assembly_image = QLabel(self) + pixmap = QPixmap("src/actual_task.jpg") + pixmap = pixmap.scaledToWidth(1125) + assembly_image.setPixmap(pixmap) + assembly_image.adjustSize() + assembly_image.move(660, 145) + + # inputs + options = deepcopy(self.remaining_objects) + + # print the options + option_x, option_y = 210, 200 + buttons = [] + for opt in options: + opt_button = QPushButton(self) + opt_button.setText(opt) + opt_button.setFont(QFont('Arial', 20)) + opt_button.setGeometry(option_x, option_y, 225, 50) + opt_button.setCheckable(True) + opt_button.setStyleSheet("QPushButton::checked {background-color : lightpink;}") + buttons.append(opt_button) + option_y += 50 + self.option_buttons = buttons + + # button for performing selected actions + option_x = 85 + option_y += 60 + self.selected_button = QPushButton(self) + self.selected_button.setText("Give me the selected parts.") + self.selected_button.setFont(QFont('Arial', 20)) + self.selected_button.setGeometry(option_x, option_y, 500, 50) + self.selected_button.setStyleSheet("background-color : lightpink") + self.selected_button.setCheckable(True) + self.selected_button.clicked.connect(self.deliver_part) + + # print current time step + self.step_label = QLabel(self) + self.step_label.setText("Current time step: " + str(self.time_step)) + self.step_label.setFont(QFont('Arial', 36)) + self.step_label.adjustSize() + self.step_label.move(715, 65) + + # update timer + self.time_to_respond = 10 + self.timer = QTimer() + self.timer.timeout.connect(self.update_application) + + self.time_left = deepcopy(self.time_to_respond) + self.countdown = QLabel(self) + self.countdown.setText(str(self.time_left)) + self.countdown.setFont(QFont('Arial', 36)) + self.countdown.setStyleSheet("background-color: khaki") + self.countdown.adjustSize() + self.countdown.move(1720, 65) + self.countdown_timer = QTimer() + self.countdown_timer.timeout.connect(self.timer_update) + + self.timer.start(self.time_to_respond*1000) + self.countdown_timer.start(1000) + + + def timer_update(self): + self.time_left -=1 + self.countdown.setText(" " + str(self.time_left) + " ") + if self.time_left == 0: + self.time_left = deepcopy(self.time_to_respond) + self.countdown.setText(str(self.time_left)) + + + def update_application(self): + + # update time stamp + self.step_label.setText("Current time step: " + str(self.time_step)) + + # update suggested options + for opt_button in self.option_buttons: + # opt_button.setChecked(False) + if opt_button.text() not in self.remaining_objects: + opt_button.setChecked(False) + opt_button.setCheckable(False) + opt_button.setStyleSheet("QPushButton {color : lightgrey;}") + else: + opt_button.setStyleSheet("QPushButton::checked {background-color : lightpink;}") + + # update action buttons + self.selected_button.setChecked(False) + + + def callback(self, data): + + # current recognised action sequence + detected_sequence = [int(a) for a in data.data] + + # current recognised parts + detected_parts = data.layout.dim[0].label.split(",") + + # update action sequence + self.user_sequence = detected_sequence + self.time_step = len(self.user_sequence) + + # update remaining parts + self.remaining_objects = [rem_obj for rem_obj in self.remaining_objects if rem_obj not in detected_parts] + + + def deliver_part(self): + + # check which objects were selected by the user + if self.selected_button.isChecked(): + objects_to_deliver = [] + for option in self.option_buttons: + if option.isChecked(): + objects_to_deliver.append(option.text()) + else: + objects_to_deliver = [] + + # loop over all objects to be delivered + for chosen_obj in objects_to_deliver: + + # instruct the user to retreive the parts that cannot be delivered by the robot + if chosen_obj in ["airplane body", "none"]: + print("Cannot provide this part.") + msg = QMessageBox() + msg.setText("Get the parts you need while the robot waits.") + msg.setFont(QFont('Arial', 20)) + msg.setWindowTitle("Robot Message") + QTimer.singleShot(10000, msg.close) + msg.exec_() + else: + # deliver parts requested by the user whenever possible + print("Providing the required part.") + + # ---------------------------------------- Collision detection --------------------------------------- # + + # collision_free_constraint = self.ada.set_up_collision_detection(ada.get_arm_state_space(), self.ada.get_arm_skeleton(), + # [obj]) + # full_collision_constraint = self.ada.get_full_collision_constraint(ada.get_arm_state_space(), + # self.ada.get_arm_skeleton(), + # collision_free_constraint) + # collision = self.ada.get_self_collision_constraint() + + + # -------------------------------------- Plan path for grasping -------------------------------------- # + + obj = self.objects[chosen_obj][0] + + # use pre-computed grasp configuration if available + if chosen_obj in self.graspConfig.keys(): + print("Running hard-coded...") + grasp_configuration = self.graspConfig[chosen_obj] + else: + print("Creating new TSR.") + objPose = self.objects[chosen_obj][1] + objGraspPose = self.objects[chosen_obj][2] + + # grasp TSR for object + objTSR = createTSR(objPose, objGraspPose) + # marker = viewer.add_tsr_marker(objTSR) + # input("Marker look good?") + + # perform IK to compute grasp configuration + ik_sampleable = adapy.create_ik(self.arm_skeleton, self.arm_state_space, objTSR, self.hand_node) + ik_generator = ik_sampleable.create_sample_generator() + configurations = [] + samples, max_samples = 0, 10 + while samples < max_samples and ik_generator.can_sample(): + samples += 1 + goal_state = ik_generator.sample(self.arm_state_space) + if len(goal_state) == 0: + continue + configurations.append(goal_state) + print("Found new configuration.") + + grasp_configuration = configurations[0] + + # plan path to grasp configuration + waypoints = [(0.0, self.armHome),(1.0, grasp_configuration)] + trajectory = self.ada.compute_joint_space_path(waypoints) + + # ------------------------------------------ Execute path to grasp object --------------------------------- # + + if not trajectory: + print("Failed to find a solution!") + else: + # execute the planned trajectory + self.ada.execute_trajectory(trajectory) + + input("Get in.") + + # lower gripper + if chosen_obj == 'main wing': + traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0.04, 0., 0.]) + else: + traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., -0.045]) + self.ada.execute_trajectory(traj) + + input("Grasp?") + # grasp the object + self.hand.execute_preshape([1.3, 1.3]) + time.sleep(1.5) + self.hand.grab(obj) + + input("Lift?") + # lift up grasped object + if chosen_obj == 'main wing': + traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., 0.10]) + self.ada.execute_trajectory(traj) + traj = self.ada.plan_to_offset("j2n6s200_hand_base", [-0.2, 0., 0.]) + else: + traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., 0.15]) + self.ada.execute_trajectory(traj) + + input("Move?") + # move grasped object to workbench + current_position = self.arm_skeleton.get_positions() + new_position = current_position.copy() + new_position[0] += self.deliveryRotation[chosen_obj] + waypoints = [(0.0, current_position), (1.0, new_position)] + traj = self.ada.compute_joint_space_path(waypoints) + self.ada.execute_trajectory(traj) + + # ----------------------- Lower grasped object using Jacobian pseudo-inverse ------------------------ # + + if chosen_obj == "main wing": + time.sleep(4) + else: + traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., -0.10]) + self.ada.execute_trajectory(traj) + + self.hand.ungrab() + self.hand.execute_preshape([0.15, 0.15]) + self.world.remove_skeleton(obj) + time.sleep(1) + + # ------------------- Move robot back to home ------------------- # + + waypoints = [(0.0, self.ada.get_arm_positions()), (1.0, self.armHome)] + traj = self.ada.compute_joint_space_path(waypoints) + self.ada.execute_trajectory(traj) + + print("Finished executing actions.") + + +# MAIN +# initialise ros node +rospy.init_node("reactive_assembly") +roscpp_init('reactive_assembly', []) +app = QApplication(sys.argv) +win = AssemblyController() +win.showMaximized() +app.exec_() +try: + rospy.spin() +except KeyboardInterrupt: + print ("Shutting down") + +input("Press Enter to Quit...") diff --git a/proactive_assembly.py b/proactive_assembly.py index f0b4619..e0fcbd1 100644 --- a/proactive_assembly.py +++ b/proactive_assembly.py @@ -213,9 +213,11 @@ def __init__(self): # -------------------------------------- Assembly and Aniticipation Info ----------------------------------------- # + user_id = input("Enter user id: ") + # load the learned q_values for each state - self.qf = pickle.load(open("data/q_values.p", "rb")) - self.states = pickle.load(open("data/states.p", "rb")) + self.qf = pickle.load(open("data/q_values_" + user_id + ".p", "rb")) + self.states = pickle.load(open("data/states_" + user_id + ".p", "rb")) # actions in airplane assembly and objects required for each action self.remaining_user_actions = [0, 1, 2, 3, 4, 5, 6, 7] @@ -246,7 +248,7 @@ def __init__(self): # initialize user sequence self.time_step = 0 self.user_sequence = [] - self.anticipated_action_name = [] + self.anticipated_action_names = [] self.suggested_objects = [] # proactive @@ -264,23 +266,23 @@ def __init__(self): query.setText("Which part(s) do you want?") query.setFont(QFont('Arial', 28)) query.adjustSize() - query.move(145, 135) + query.move(95, 135) # task info assembly_image = QLabel(self) - pixmap = QPixmap("src/task.jpg") - pixmap = pixmap.scaledToWidth(950) + pixmap = QPixmap("src/actual_task.jpg") + pixmap = pixmap.scaledToWidth(1125) assembly_image.setPixmap(pixmap) assembly_image.adjustSize() - assembly_image.move(825, 200) + assembly_image.move(660, 145) # inputs options = deepcopy(self.remaining_objects) - suggestions = deepcopy(self.current_suggestion) - suggestion_text = deepcopy(self.anticipated_action_name) + suggestions = deepcopy(self.suggested_objects) + suggestion_text = deepcopy(self.anticipated_action_names) # print the options - option_x, option_y = 260, 200 + option_x, option_y = 210, 200 buttons = [] for opt in options: opt_button = QPushButton(self) @@ -288,7 +290,7 @@ def __init__(self): opt_button.setFont(QFont('Arial', 20)) opt_button.setGeometry(option_x, option_y, 225, 50) opt_button.setCheckable(True) - if opt == suggestions: + if opt in suggestions: opt_button.setStyleSheet("QPushButton {background-color : lightgreen;} QPushButton::checked {background-color : lightpink;}") else: opt_button.setStyleSheet("QPushButton::checked {background-color : lightpink;}") @@ -297,7 +299,7 @@ def __init__(self): self.option_buttons = buttons # button for performing suggested actions - option_x = 130 + option_x = 85 option_y += 50 self.suggested_button = QPushButton(self) self.suggested_button.setText("YES. Give the parts you suggested.") @@ -308,7 +310,7 @@ def __init__(self): self.suggested_button.clicked.connect(self.deliver_part) # button for performing selected actions - option_x = 130 + option_x = 80 option_y += 75 self.selected_button = QPushButton(self) self.selected_button.setText("NO. Give the parts I selected.") @@ -323,21 +325,21 @@ def __init__(self): self.step_label.setText("Current time step: " + str(self.time_step)) self.step_label.setFont(QFont('Arial', 36)) self.step_label.adjustSize() - self.step_label.move(820, 125) + self.step_label.move(715, 65) # pre-text for suggestion action pre_text = QLabel(self) pre_text.setText("Suggested next action:") pre_text.setFont(QFont('Arial', 36)) pre_text.adjustSize() - pre_text.move(820, 850) + pre_text.move(715, 820) # print the anticipated action self.user_instruction = QLabel(self) self.user_instruction.setText(str(suggestion_text)) self.user_instruction.setFont(QFont('Arial', 32)) self.user_instruction.adjustSize() - self.user_instruction.move(1340, 850) + self.user_instruction.move(1235, 820) self.user_instruction.setStyleSheet("color: green") # update timer @@ -351,7 +353,7 @@ def __init__(self): self.countdown.setFont(QFont('Arial', 36)) self.countdown.setStyleSheet("background-color: khaki") self.countdown.adjustSize() - self.countdown.move(1720, 125) + self.countdown.move(1720, 65) self.countdown_timer = QTimer() self.countdown_timer.timeout.connect(self.timer_update) @@ -373,7 +375,7 @@ def update_application(self): self.step_label.setText("Current time step: " + str(self.time_step)) # update anticipated action - self.user_instruction.setText(str(self.anticipated_action_name)) + self.user_instruction.setText(str(self.anticipated_action_names)) self.user_instruction.adjustSize() # update suggested options @@ -383,7 +385,7 @@ def update_application(self): opt_button.setChecked(False) opt_button.setCheckable(False) opt_button.setStyleSheet("QPushButton {color : lightgrey;}") - elif opt_button.text() == self.current_suggestion: + elif opt_button.text() in self.suggested_objects: opt_button.setStyleSheet("QPushButton {background-color : lightgreen;} QPushButton::checked {background-color : lightpink;}") else: opt_button.setStyleSheet("QPushButton::checked {background-color : lightpink;}") @@ -446,63 +448,78 @@ def callback(self, data): suggested_objs = list(set(suggested_objs)) if not self.delivering_part and set(suggested_objs) != set(self.suggested_objects): - self.current_suggestion = suggested_objs[0] - trajectory = self.reach_part(self.current_suggestion) - self.ada.execute_trajectory(trajectory) + if suggested_objs: + self.current_suggestion = suggested_objs[0] + reached = self.reach_part(self.current_suggestion) + # self.ada.execute_trajectory(trajectory) self.suggested_objects = deepcopy(suggested_objs) - def reach_part(self, chosen_obj): - obj = self.objects[chosen_obj][0] - objPose = self.objects[chosen_obj][1] - objGraspPose = self.objects[chosen_obj][2] + def reach_part(self, chosen_obj, midpoint=False): - # use pre-computed grasp configuration if available - if chosen_obj in self.graspConfig.keys(): - print("Running hard-coded...") - grasp_configuration = self.graspConfig[chosen_obj] - else: - print("Creating new TSR.") - # grasp TSR for object - objTSR = createTSR(objPose, objGraspPose) - # marker = viewer.add_tsr_marker(objTSR) - # input("Marker look good?") - - # perform IK to compute grasp configuration - ik_sampleable = adapy.create_ik(self.arm_skeleton, self.arm_state_space, objTSR, self.hand_node) - ik_generator = ik_sampleable.create_sample_generator() - configurations = [] - samples, max_samples = 0, 10 - while samples < max_samples and ik_generator.can_sample(): - samples += 1 - goal_state = ik_generator.sample(self.arm_state_space) - if len(goal_state) == 0: - continue - configurations.append(goal_state) - print("Found new configuration.") - - grasp_configuration = configurations[0] - - # plan path to grasp configuration - waypoints = [(0.0, self.ada.get_arm_positions()),(1.0, grasp_configuration)] - trajectory = self.ada.compute_joint_space_path(waypoints) - - return trajectory + if chosen_obj not in ["main wing", "airplane body", "none"]: + obj = self.objects[chosen_obj][0] + objPose = self.objects[chosen_obj][1] + objGraspPose = self.objects[chosen_obj][2] + + # use pre-computed grasp configuration if available + if chosen_obj in self.graspConfig.keys(): + print("Running hard-coded...") + grasp_configuration = self.graspConfig[chosen_obj] + else: + print("Creating new TSR.") + # grasp TSR for object + objTSR = createTSR(objPose, objGraspPose) + # marker = viewer.add_tsr_marker(objTSR) + # input("Marker look good?") + + # perform IK to compute grasp configuration + ik_sampleable = adapy.create_ik(self.arm_skeleton, self.arm_state_space, objTSR, self.hand_node) + ik_generator = ik_sampleable.create_sample_generator() + configurations = [] + samples, max_samples = 0, 10 + while samples < max_samples and ik_generator.can_sample(): + samples += 1 + goal_state = ik_generator.sample(self.arm_state_space) + if len(goal_state) == 0: + continue + configurations.append(goal_state) + print("Found new configuration.") + + grasp_configuration = configurations[0] + + # plan path to grasp configuration + if midpoint: + waypoints = [(0.0, self.ada.get_arm_positions()), (1.0, self.armHome), (2.0, grasp_configuration)] + else: + waypoints = [(0.0, self.ada.get_arm_positions()), (1.0, grasp_configuration)] + trajectory = self.ada.compute_joint_space_path(waypoints) + + if not trajectory: + print("Failed to find a solution!") + else: + # execute the planned trajectory + self.ada.execute_trajectory(trajectory) + return True + + return False def deliver_part(self): self.delivering_part = True + set_midpoint = False - # check which objects were selected by the user + # check which objects were selected by the user if self.selected_button.isChecked(): objects_to_deliver = [] for option in self.option_buttons: if option.isChecked(): objects_to_deliver.append(option.text()) + set_midpoint = True elif self.suggested_button.isChecked(): - objects_to_deliver = [self.current_suggestion] + objects_to_deliver = self.suggested_objects else: objects_to_deliver = [] @@ -516,7 +533,7 @@ def deliver_part(self): msg.setText("Get the parts you need while the robot waits.") msg.setFont(QFont('Arial', 20)) msg.setWindowTitle("Robot Message") - QTimer.singleShot(10000, msg.close) + QTimer.singleShot(4000, msg.close) msg.exec_() else: # deliver parts requested by the user whenever possible @@ -534,18 +551,18 @@ def deliver_part(self): # -------------------------------------- Plan path for grasping -------------------------------------- # - trajectory = self.reach_part(chosen_obj) + reached = self.reach_part(chosen_obj, midpoint=set_midpoint) # ------------------------------------------ Execute path to grasp object --------------------------------- # - if not trajectory: - print("Failed to find a solution!") + if not reached: + print("Cannot pick the part.") else: # execute the planned trajectory - self.ada.execute_trajectory(trajectory) + # self.ada.execute_trajectory(trajectory) # lower gripper - traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., -0.05]) + traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., -0.045]) self.ada.execute_trajectory(traj) # grasp the object @@ -567,7 +584,7 @@ def deliver_part(self): # ----------------------- Lower grasped object using Jacobian pseudo-inverse ------------------------ # - traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., -0.12]) + traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., -0.10]) self.ada.execute_trajectory(traj) # self.hand.ungrab() diff --git a/reactive_assembly.py b/reactive_assembly.py index b0c692d..e83d159 100644 --- a/reactive_assembly.py +++ b/reactive_assembly.py @@ -149,6 +149,7 @@ def __init__(self): self.graspConfig["short bolts"] = [-0.72561783, 4.31588712, 2.28856202, -2.71514972, -1.42200445, 1.01089267] self.deliveryRotation["short bolts"] = 1.25 self.graspConfig["propeller nut"] = [0.49700125, 1.86043184, 3.78425230, 2.63384048, 1.44808279, 1.67817618] + # self.graspConfig["propeller nut"] = [-2.03877631, 4.09967790, 1.60438025, -0.19636232, 0.71718155, 2.21799853] self.deliveryRotation["propeller nut"] = -1.1 self.graspConfig["tail screw"] = [-0.46015322, 4.47079882, 2.68192519, -2.584758426, -1.74260217, 1.457295330] self.deliveryRotation["tail screw"] = 1.0 @@ -160,6 +161,8 @@ def __init__(self): self.deliveryRotation["propeller hub"] = -0.6 self.graspConfig["tail wing"] = [3.129024, 1.87404028, 3.40826295, 0.53502216, -1.86749865, -0.99044654] self.deliveryRotation["tail wing"] = 0.7 + self.graspConfig["main wing"] = [-2.86840265, 3.89315136, 1.47980743, -3.07256298, 0.95719655, 2.37149834] + self.deliveryRotation["main wing"] = -0.7 # initialize sim environment self.world = self.ada.get_world() @@ -176,6 +179,7 @@ def __init__(self): container3_1 = self.world.add_body_from_urdf(container3URDFUri, container3_1Pose) # container3_2 = self.world.add_body_from_urdf(container3URDFUri, container3_2Pose) tailWing = self.world.add_body_from_urdf(tailURDFUri, tailPose) + mainWing = self.world.add_body_from_urdf(wingURDFUri, wingPose) # dict of all objects self.objects = {"long bolts": [container1_1, container1_1Pose, container1GraspPose, container1GraspOffset], @@ -186,7 +190,7 @@ def __init__(self): "tool": [container2_2, container2_2Pose, container2GraspPose, container2GraspOffset], "propeller hub": [container3_1, container3_1Pose, container3GraspPose, container3GraspOffset], "tail wing": [tailWing, tailPose, tailGraspPose, tailGraspOffset], - "main wing": [], + "main wing": [mainWing], "airplane body": []} # ------------------------------------------------ Get robot config ---------------------------------------------- # @@ -236,21 +240,21 @@ def __init__(self): query.setText("Which part(s) do you want?") query.setFont(QFont('Arial', 28)) query.adjustSize() - query.move(145, 135) + query.move(95, 135) # task info assembly_image = QLabel(self) - pixmap = QPixmap("src/task.jpg") - pixmap = pixmap.scaledToWidth(950) + pixmap = QPixmap("src/actual_task.jpg") + pixmap = pixmap.scaledToWidth(1125) assembly_image.setPixmap(pixmap) assembly_image.adjustSize() - assembly_image.move(825, 200) + assembly_image.move(660, 145) # inputs options = deepcopy(self.remaining_objects) # print the options - option_x, option_y = 260, 200 + option_x, option_y = 210, 200 buttons = [] for opt in options: opt_button = QPushButton(self) @@ -264,7 +268,7 @@ def __init__(self): self.option_buttons = buttons # button for performing selected actions - option_x = 130 + option_x = 85 option_y += 60 self.selected_button = QPushButton(self) self.selected_button.setText("Give me the selected parts.") @@ -279,7 +283,7 @@ def __init__(self): self.step_label.setText("Current time step: " + str(self.time_step)) self.step_label.setFont(QFont('Arial', 36)) self.step_label.adjustSize() - self.step_label.move(820, 125) + self.step_label.move(715, 65) # update timer self.time_to_respond = 10 @@ -292,7 +296,7 @@ def __init__(self): self.countdown.setFont(QFont('Arial', 36)) self.countdown.setStyleSheet("background-color: khaki") self.countdown.adjustSize() - self.countdown.move(1720, 125) + self.countdown.move(1720, 65) self.countdown_timer = QTimer() self.countdown_timer.timeout.connect(self.timer_update) @@ -383,8 +387,6 @@ def deliver_part(self): # -------------------------------------- Plan path for grasping -------------------------------------- # obj = self.objects[chosen_obj][0] - objPose = self.objects[chosen_obj][1] - objGraspPose = self.objects[chosen_obj][2] # use pre-computed grasp configuration if available if chosen_obj in self.graspConfig.keys(): @@ -392,6 +394,9 @@ def deliver_part(self): grasp_configuration = self.graspConfig[chosen_obj] else: print("Creating new TSR.") + objPose = self.objects[chosen_obj][1] + objGraspPose = self.objects[chosen_obj][2] + # grasp TSR for object objTSR = createTSR(objPose, objGraspPose) # marker = viewer.add_tsr_marker(objTSR) @@ -423,18 +428,26 @@ def deliver_part(self): else: # execute the planned trajectory self.ada.execute_trajectory(trajectory) - + # lower gripper - traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., -0.05]) + if chosen_obj == 'main wing': + traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0.04, 0., 0.]) + else: + traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., -0.045]) self.ada.execute_trajectory(traj) - # grasp the object + # grasp the object self.hand.execute_preshape([1.3, 1.3]) time.sleep(1.5) self.hand.grab(obj) # lift up grasped object - traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., 0.15]) + if chosen_obj == 'main wing': + traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., 0.10]) + self.ada.execute_trajectory(traj) + traj = self.ada.plan_to_offset("j2n6s200_hand_base", [-0.2, 0., 0.]) + else: + traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., 0.15]) self.ada.execute_trajectory(traj) # move grasped object to workbench @@ -447,8 +460,11 @@ def deliver_part(self): # ----------------------- Lower grasped object using Jacobian pseudo-inverse ------------------------ # - traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., -0.12]) - self.ada.execute_trajectory(traj) + if chosen_obj == "main wing": + time.sleep(4) + else: + traj = self.ada.plan_to_offset("j2n6s200_hand_base", [0., 0., -0.10]) + self.ada.execute_trajectory(traj) self.hand.ungrab() self.hand.execute_preshape([0.15, 0.15]) diff --git a/src/action_recognition_aprilTag_openPose.py b/src/action_recognition_aprilTag_openPose.py index 416f28e..e07c316 100644 --- a/src/action_recognition_aprilTag_openPose.py +++ b/src/action_recognition_aprilTag_openPose.py @@ -123,10 +123,20 @@ def has_parts(self, part_set): "1": "main wing", "0": "airplane body"} +# actions_list = [Action([0], 'Insert main wing', [[0, 1]]), #near tag 1 +# Action([2, 4], 'Screw main wing', [[0, 1, 21, 17, 18]]), #near tag 1 +# Action([1], 'Insert tail wing', [[0, 2]]), #near tag 2 +# Action([3, 5], 'Screw tail wing', [[0, 2, 30, 17, 18]]), #near tag 2 +# Action([6], 'Screw propellers', [[5, 6, 19, 20, 14, 22, 17,18]]), +# Action([7], 'Fix propeller hub', [[0, 5, 6, 16, 24]]), +# ] + actions_list = [Action([0], 'Insert main wing', [[0, 1]]), #near tag 1 - Action([2, 4], 'Screw main wing', [[0, 1, 21, 17, 18]]), #near tag 1 + Action([2], 'Insert bolt in main wing', [[0, 1, 21]]), #near tag 1 + Action([4], 'Screw bolt to main wing', [[0, 1, 21, 17, 18]]), #near tag 1 Action([1], 'Insert tail wing', [[0, 2]]), #near tag 2 - Action([3, 5], 'Screw tail wing', [[0, 2, 30, 17, 18]]), #near tag 2 + Action([3], 'Insert bolt in tail wing', [[0, 2, 30]]), #near tag 2 + Action([5], 'Screw bolt to tail wing', [[0, 2, 30, 17, 18]]), #near tag 2 Action([6], 'Screw propellers', [[5, 6, 19, 20, 14, 22, 17,18]]), Action([7], 'Fix propeller hub', [[0, 5, 6, 16, 24]]), ] @@ -223,7 +233,7 @@ def video_demo(): ground_truth_action_sequence = [1,7,8,2,5,6] - capture = cv2.VideoCapture(2) + capture = cv2.VideoCapture(0) capture.set(cv2.CAP_PROP_FRAME_WIDTH, 1920) capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080) # capture.set(cv2.CAP_PROP_POS_FRAMES,6000) @@ -381,6 +391,7 @@ def video_demo(): undone_actions.remove(action) legible_part_list = "" + # print(legible_part_list, part_set) for part_id in part_set: if str(part_id) in parts_list.keys(): legible_part_list += parts_list[str(part_id)] + "," diff --git a/src/action_recognition_aprilTag_openPose_canonical.py b/src/action_recognition_aprilTag_openPose_canonical.py index c315327..a60f1b6 100644 --- a/src/action_recognition_aprilTag_openPose_canonical.py +++ b/src/action_recognition_aprilTag_openPose_canonical.py @@ -91,28 +91,22 @@ def has_parts(self, part_set): return True -# Requires all AprilTag for all parts (except for the tool and long screws) -# actions_list = [Action(1, 'Insert main wing into body', [[0, 1]]), #near tag 1 -# Action(2, 'Screw main wing to body', [[0, 1, 21, 17, 18]), #near tag 1 -# Action(3, 'Insert right wing tip into main wing', [[1, 4]]), -# Action(4, 'Insert left wing tip into main wing', [[1, 3]]), -# Action(5, 'Insert tail wing into body', [[0, 2]]), -# Action(6, 'Screw tail wing to body', [[0, 2, 13, 17], [0, 2, 21, 17], [0, 2, 13, 18], [0, 2, 21, 18]]), -# Action(7, 'Screw propeller to propeller base', [[5, 6, 19, 20, 14, 22, 17], [5, 6, 19, 20, 14, 22, 18]]), -# Action(8, 'Screw propeller base to body', [[0, 5, 6, 16, 24]]), -# Action(9, 'Screw propeller cap to propeller base', [[0, 5, 6, 7, 8, 15, 23, 17], [0, 5, 6, 7, 8, 15, 23, 17]]),] - -# actions_list = [Action([0], 'Insert main wing into body', [[0, 1]]), #near tag 1 -# Action([2, 4], 'Screw main wing to body', [[0, 1, 21, 17, 18]]), #near tag 1 -# Action([1], 'Insert tail wing into body', [[0, 2]]), #near tag 2 -# Action([3, 5], 'Screw tail wing to body', [[0, 2, 30, 17, 18]]), #near tag 2 -# Action([6], 'Screw propeller to propeller base', [[5, 6, 19, 20, 14, 22, 17,18]]), -# Action([7], 'Screw propeller base to body', [[0, 5, 6, 16, 24]]), +# parts_list = {"21": "long bolts", +# "22": "short bolts", +# "24": "short wire", +# "25": "large box", +# "26": "small box", +# "30": "long wire", +# } + +# actions_list = [Action([0, 3], 'Screw long bolt', [[25, 21]]), +# Action([1, 4], 'Screw short bolt', [[26, 22]]), +# Action([2], 'Insert short wire', [[26, 24]]), +# Action([5], 'Insert long wire', [[25, 30]]), # ] - #13 shown: action 2 and action 6 finished - -parts_list = {"21": "long bolts", +parts_list = {"18": "tool", + "21": "long bolts", "22": "short bolts", "24": "short wire", "25": "large box", @@ -120,10 +114,12 @@ def has_parts(self, part_set): "30": "long wire", } -actions_list = [Action([0, 3], 'Screw long bolt', [[25, 21]]), - Action([1, 4], 'Screw short bolt', [[26, 22]]), - Action([2], 'Insert short wire', [[26, 24]]), - Action([5], 'Insert long wire', [[25, 30]]), +actions_list = [Action([0], 'Insert long bolt', [[25, 21]]), + Action([3], 'Screw long bolt', [[18, 25, 21]]), + Action([1], 'Insert short bolt', [[26, 22]]), + Action([4], 'Screw short bolt', [[18, 26, 22]]), + Action([2], 'Insert short wire', [[26, 24]]), + Action([5], 'Insert long wire', [[25, 30]]), ] actions_from_part = defaultdict(set) @@ -219,7 +215,7 @@ def video_demo(): ground_truth_action_sequence = [1,7,8,2,5,6] - capture = cv2.VideoCapture(0) + capture = cv2.VideoCapture(-2) capture.set(cv2.CAP_PROP_FRAME_WIDTH, 1920) capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080) # capture.set(cv2.CAP_PROP_POS_FRAMES,6000) @@ -385,7 +381,7 @@ def video_demo(): legible_part_list = "" for part_id in part_set: if str(part_id) in parts_list.keys(): - legible_part_list += parts_list[str(part_id)] + ", " + legible_part_list += parts_list[str(part_id)] + "," legible_part_list = legible_part_list[:-1] diff --git a/src/actual_task.jpg b/src/actual_task.jpg new file mode 100644 index 0000000..77298f2 Binary files /dev/null and b/src/actual_task.jpg differ diff --git a/src/canonical_task.png b/src/canonical_task.png new file mode 100644 index 0000000..f4b5361 Binary files /dev/null and b/src/canonical_task.png differ