def __init__(self, world):
self.world = world
self.current_bin = "A"
self.bin_state = {
"A": {"done": False, "contents": []},
"B": {"done": False, "contents": []},
"C": {"done": False, "contents": []},
"D": {"done": False, "contents": []},
"E": {"done": False, "contents": []},
"F": {"done": False, "contents": []},
"G": {"done": False, "contents": []},
"H": {"done": False, "contents": []},
"I": {"done": False, "contents": []},
"J": {"done": False, "contents": []},
"K": {"done": False, "contents": []},
"L": {"done": False, "contents": []},
}
self.robotModel = world.robot(0)
self.state = INITIAL_STATE
self.config = self.robotModel.getConfig()
self.left_arm_links = [self.robotModel.link(i) for i in LEFT_ARM_LINK_NAMES]
self.right_arm_links = [self.robotModel.link(i) for i in RIGHT_ARM_LINK_NAMES]
id_to_index = dict([(self.robotModel.link(i).getID(), i) for i in range(self.robotModel.numLinks())])
self.left_arm_indices = [id_to_index[i.getID()] for i in self.left_arm_links]
self.right_arm_indices = [id_to_index[i.getID()] for i in self.right_arm_links]
self.Tcamera = se3.identity()
self.Tvacuum = se3.identity()
self.calibratedCameraXform = RIGHT_F200_CAMERA_CALIBRATED_XFORM
self.object_com = [0, 0, 0]
self.points1 = []
self.points2 = []
self.vacuumPc = Geometry3D()
self.vacuumPc.loadFile(VACUUM_PCD_FILE)
# Set up serial
if REAL_VACUUM:
self.serial = serial.Serial()
self.serial.port = ARDUINO_SERIAL_PORT
self.serial.baudrate = 9600
self.serial.open()
if self.serial.isOpen():
self.serial.write("hello")
response = self.serial.read(self.serial.inWaiting())
self.turnOffVacuum()
# Load JSON
with open(PICK_JSON_PATH) as pick_json_file:
raw_json_data = json.load(pick_json_file)
for k in self.bin_state:
self.bin_state[k]["contents"] = raw_json_data["bin_contents"]["bin_" + k]
for my_dict in raw_json_data["work_order"]:
bin_letter = my_dict["bin"][4]
self.bin_state[bin_letter]["target"] = my_dict["item"]
self.current_bin = planning.selectBin(self.bin_state)
def __init__(self, world):
self.world = world
self.current_bin = 'A'
self.bin_state = {'A': {'done': False, 'contents': []},'B': {'done': False, 'contents': []}, 'C': {'done': False, 'contents': []}, 'D': {'done': False, 'contents': []}, 'E': {'done': False, 'contents': []}, 'F': {'done': False, 'contents': []}, 'G': {'done': False, 'contents': []}, 'H': {'done': False, 'contents': []}, 'I': {'done': False, 'contents': []}, 'J': {'done': False, 'contents': []}, 'K': {'done': False, 'contents': []}, 'L': {'done': False, 'contents': []}}
self.robotModel = world.robot(0)
self.state = INITIAL_STATE
self.config = self.robotModel.getConfig()
self.left_arm_links = [self.robotModel.link(i) for i in LEFT_ARM_LINK_NAMES]
self.right_arm_links = [self.robotModel.link(i) for i in RIGHT_ARM_LINK_NAMES]
id_to_index = dict([(self.robotModel.link(i).getID(),i) for i in range(self.robotModel.numLinks())])
self.left_arm_indices = [id_to_index[i.getID()] for i in self.left_arm_links]
self.right_arm_indices = [id_to_index[i.getID()] for i in self.right_arm_links]
self.Tcamera = se3.identity()
self.Tvacuum = se3.identity()
self.calibratedCameraXform = RIGHT_F200_CAMERA_CALIBRATED_XFORM
self.object_com = [0, 0, 0]
self.points1 = []
self.points2 = []
self.vacuumPc = Geometry3D()
self.vacuumPc.loadFile(VACUUM_PCD_FILE)
if REAL_SCALE:
self.scale = scale.Scale();
# Set up serial
if REAL_VACUUM:
self.serial = serial.Serial()
self.serial.port = ARDUINO_SERIAL_PORT
self.serial.baudrate = 9600
self.serial.open()
if self.serial.isOpen():
self.serial.write("hello")
response = self.serial.read(self.serial.inWaiting())
self.turnOffVacuum()
# Load JSON
with open(PICK_JSON_PATH) as pick_json_file:
raw_json_data = json.load(pick_json_file)
for k in self.bin_state:
self.bin_state[k]['contents'] = raw_json_data['bin_contents']['bin_'+k]
for my_dict in raw_json_data['work_order']:
bin_letter = my_dict['bin'][4]
self.bin_state[bin_letter]['target'] = my_dict['item']
self.current_bin = planning.selectBin(self.bin_state)
def loop(self):
try:
while True:
print OKBLUE + "Bin " + str(self.current_bin) + ": " + self.state + END_COLOR
if self.state == "VISUAL_DEBUG":
self.object_com = [1.0839953170961105, -0.25145094946424207, 1.1241831909823194]
# self.set_model_right_arm( [0.1868786927065213, -1.567604604679142, 0.6922768776941961, 1.5862815343628953, -0.005567750307534711, -0.017979599494945674, 0.0035268645585939083])
self.load_real_robot_state()
else:
self.load_real_robot_state()
self.Tcamera = se3.mul(
self.robotModel.link("right_lower_forearm").getTransform(), self.calibratedCameraXform
)
self.Tvacuum = se3.mul(self.robotModel.link("right_wrist").getTransform(), VACUUM_POINT_XFORM)
self.vacuumPc = Geometry3D()
self.vacuumPc.loadFile(VACUUM_PCD_FILE)
temp_xform = self.robotModel.link("right_wrist").getTransform()
self.vacuumPc.transform(self.Tvacuum[0], self.Tvacuum[1])
if self.state == "DEBUG_COLLISION_CHECKER":
temp_planner = planning.LimbPlanner(self.world, self.vacuumPc)
print "Is this configuration collision free?"
print temp_planner.check_collision_free("right")
sys.stdout.flush()
elif self.state == "FAKE_PATH_PLANNING":
self.object_com = [1.114, -0.077, 1.412]
self.right_arm_ik(self.object_com)
self.Tcamera = se3.mul(
self.robotModel.link("right_lower_forearm").getTransform(), self.calibratedCameraXform
)
self.Tvacuum = se3.mul(self.robotModel.link("right_wrist").getTransform(), VACUUM_POINT_XFORM)
time.sleep(2342340)
elif self.state == "START":
self.state = "MOVE_TO_SCAN_BIN"
elif self.state == "MOVE_TO_SCAN_BIN":
for milestone in eval("Q_BEFORE_SCAN_" + self.current_bin):
print "Moving to " + str(milestone)
motion.robot.right_mq.appendLinear(MOVE_TIME, planning.cleanJointConfig(milestone))
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig(eval("Q_SCAN_BIN_" + self.current_bin))
)
self.state = "MOVING_TO_SCAN_BIN"
elif self.state == "MOVING_TO_SCAN_BIN":
if not motion.robot.right_mq.moving():
self.wait_start_time = time.time()
self.state = "WAITING_TO_SCAN_BIN"
elif self.state == "WAITING_TO_SCAN_BIN":
if time.time() - self.wait_start_time > SCAN_WAIT_TIME:
if REAL_PERCEPTION:
self.state = "SCANNING_BIN"
else:
self.state = "FAKE_SCANNING_BIN"
elif self.state == "SCANNING_BIN":
print "Waiting for message from camera"
cloud = rospy.wait_for_message(ROS_DEPTH_TOPIC, PointCloud2)
if perception.isCloudValid(cloud):
cloud = perception.convertPc2ToNp(cloud)
np_cloud = cloud[::STEP]
self.points1 = []
for point in np_cloud:
transformed = se3.apply(self.Tcamera, point)
self.points1.append(transformed)
np_cloud = perception.subtractShelf(np_cloud, self.current_bin)
self.points2 = []
for point in np_cloud:
transformed = se3.apply(self.Tcamera, point)
self.points2.append(transformed)
self.object_com = se3.apply(self.Tcamera, perception.com(np_cloud))
if PRINT_BLOBS:
print np_cloud
sio.savemat(CLOUD_MAT_PATH, {"cloud": np_cloud})
fo = open(CHENYU_GO_PATH, "w")
fo.write("chenyugo")
fo.close()
self.object_com = se3.apply(self.Tcamera, perception.com(np_cloud))
if CALIBRATE_CAMERA:
self.state = "CALIBRATE_CAMERA"
elif SEGMENT:
self.state = "WAITING_FOR_SEGMENTATION"
else:
self.state = "MOVE_TO_GRASP_OBJECT"
else:
print FAIL_COLOR + "Got an invalid cloud, trying again" + END_COLOR
elif self.state == "FAKE_SCANNING_BIN":
self.object_com = [1.1114839836097854, -0.6087936130127559, 0.9899267043340634]
if DRY_RUN:
raw_input("Hit enter to continue: ")
self.state = "MOVE_TO_GRASP_OBJECT"
elif self.state == "CALIBRATE_CAMERA":
self.calibrateCamera()
self.state = "SCANNING_BIN"
elif self.state == "WAITING_FOR_SEGMENTATION":
if os.path.isfile(CHENYU_DONE_PATH):
os.remove(CHENYU_DONE_PATH)
self.state = "POSTPROCESS_SEGMENTATION"
elif self.state == "POSTPROCESS_SEGMENTATION":
object_blobs = []
time.sleep(5)
for i in range(1, 20):
seg_file_path = MAT_PATH + "seg" + str(i) + ".mat"
print seg_file_path
if os.path.isfile(seg_file_path):
print seg_file_path + " exists"
mat_contents = sio.loadmat(seg_file_path)
r = mat_contents["r"]
r = r[r[:, 1] != 0, :]
object_blobs.append(r)
os.remove(seg_file_path)
if PRINT_BLOBS:
print "============="
print "object blobs"
print object_blobs
print "============="
if len(object_blobs) == 0:
self.state = "BIN_DONE"
else:
object_list = [
ITEM_NUMBERS[item_str] for item_str in self.bin_state[self.current_bin]["contents"]
]
target = ITEM_NUMBERS[self.bin_state[self.current_bin]["target"]]
histogram_dict = perception.loadHistogram(object_list)
cloud_label = {} # key is the label of object, value is cloud points
label_score = {} # key is the label, value is the current score for the object
for object_cloud in object_blobs:
if PRINT_BLOBS:
print "====================="
print "object_cloud"
print object_cloud
print "====================="
object_cloud = perception.resample(cloud, object_cloud, 3)
label, score = perception.objectMatch(object_cloud, histogram_dict)
if label in cloud_label:
if label_score[label] < score:
label_score[label] = score
cloud_label[label] = object_cloud
else:
cloud_label[label] = object_cloud
label_score[label] = score
if PRINT_LABELS_AND_SCORES:
print cloud_label
print "============="
print label_score
if target in cloud_label:
self.object_com = se3.apply(self.Tcamera, perception.com(cloud_label[target]))
self.points1 = []
for point in cloud_label[target]:
transformed = se3.apply(self.Tcamera, point)
self.points1.append(transformed)
self.points2 = []
else:
cloud_score = {}
histogram_dict = perception.loadHistogram([target])
for object_cloud in object_blobs:
object_cloud = perception.resample(cloud, object_cloud, 3)
label, score = perception.objectMatch(object_cloud, histogram_dict)
cloud_score[score] = object_cloud
sorted_cloud = sorted(cloud_score.items(), key=operator.itemgetter(0), reverse=True)
score = sorted_cloud[0][0]
com = perception.com(sorted_cloud[0][1])
self.points1 = []
self.points2 = []
for point in sorted_cloud[0][1]:
transformed = se3.apply(self.Tcamera, point)
self.points1.append(transformed)
self.object_com = se3.apply(self.Tcamera, com)
self.state = "MOVE_TO_GRASP_OBJECT"
elif self.state == "MOVE_TO_GRASP_OBJECT":
for milestone in eval("Q_AFTER_SCAN_" + self.current_bin):
print "Moving to " + str(milestone)
motion.robot.right_mq.appendLinear(MOVE_TIME, planning.cleanJointConfig(milestone))
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
)
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
self.load_real_robot_state()
self.Tvacuum = se3.mul(self.robotModel.link("right_wrist").getTransform(), VACUUM_POINT_XFORM)
print WARNING_COLOR + str(self.object_com) + END_COLOR
self.object_com = vectorops.add(self.object_com, COM_ADJUSTMENT)
current_vacuum_point = se3.apply(self.Tvacuum, [0, 0, 0])
milestone = vectorops.add(current_vacuum_point, self.object_com)
milestone = vectorops.div(milestone, 2)
if DRY_RUN:
self.state = "MOVING_TO_GRASP_OBJECT"
else:
if self.right_arm_ik(milestone):
destination = self.robotModel.getConfig()
self.q_milestone = [destination[v] for v in self.right_arm_indices]
print WARNING_COLOR + "IK config for " + str(milestone) + ": " + str(
self.q_milestone
) + END_COLOR
motion.robot.right_mq.appendLinear(MOVE_TIME, planning.cleanJointConfig(self.q_milestone))
else:
print FAIL_COLOR + "Error: IK failed" + END_COLOR
sys.stdout.flush()
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
)
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
if self.right_arm_ik(self.object_com):
destination = self.robotModel.getConfig()
print WARNING_COLOR + "IK config for " + str(self.object_com) + ": " + str(
[destination[v] for v in self.right_arm_indices]
) + END_COLOR
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig([destination[v] for v in self.right_arm_indices])
)
self.state = "MOVING_TO_GRASP_OBJECT"
else:
print FAIL_COLOR + "Error: IK failed" + END_COLOR
sys.stdout.flush()
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
)
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
elif self.state == "MOVING_TO_GRASP_OBJECT":
if not motion.robot.right_mq.moving():
time.sleep(1)
self.state = "GRASP_OBJECT"
elif self.state == "GRASP_OBJECT":
move_target = se3.apply(self.Tvacuum, [0, 0, 0])
move_target[2] = move_target[2] - GRASP_MOVE_DISTANCE - (MEAN_OBJ_HEIGHT / 2)
if self.right_arm_ik(move_target):
self.turnOnVacuum()
destination = self.robotModel.getConfig()
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig([destination[v] for v in self.right_arm_indices])
)
else:
print FAIL_COLOR + "Error: IK failed" + END_COLOR
sys.stdout.flush()
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
)
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
self.wait_start_time = time.time()
self.state = "WAITING_TO_GRASP_OBJECT"
elif self.state == "WAITING_TO_GRASP_OBJECT":
if time.time() - self.wait_start_time > GRASP_WAIT_TIME:
self.state = "MOVE_UP_BEFORE_RETRACT"
elif self.state == "MOVE_UP_BEFORE_RETRACT":
move_target = se3.apply(self.Tvacuum, [0, 0, 0])
move_target[2] = move_target[2] + GRASP_MOVE_DISTANCE + (MEAN_OBJ_HEIGHT / 2)
if self.right_arm_ik(move_target):
self.turnOnVacuum()
destination = self.robotModel.getConfig()
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig([destination[v] for v in self.right_arm_indices])
)
else:
print FAIL_COLOR + "Error: IK failed" + END_COLOR
sys.stdout.flush()
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
)
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
self.state = "MOVE_TO_STOW_OBJECT"
elif self.state == "MOVE_TO_STOW_OBJECT":
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
)
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
if not DRY_RUN:
motion.robot.right_mq.appendLinear(MOVE_TIME, planning.cleanJointConfig(self.q_milestone))
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
for milestone in eval("Q_AFTER_GRASP_" + self.current_bin):
print "Moving to " + str(milestone)
motion.robot.right_mq.appendLinear(MOVE_TIME, planning.cleanJointConfig(milestone))
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
motion.robot.right_mq.appendLinear(MOVE_TIME, Q_STOW)
self.state = "MOVING_TO_STOW_OBJECT"
elif self.state == "MOVING_TO_STOW_OBJECT":
if not motion.robot.right_mq.moving():
self.state = "STOWING_OBJECT"
elif self.state == "STOWING_OBJECT":
self.turnOffVacuum()
self.wait_start_time = time.time()
self.state = "WAITING_FOR_SECURE_STOW"
elif self.state == "WAITING_FOR_SECURE_STOW":
if time.time() - self.wait_start_time > GRASP_WAIT_TIME:
self.state = "BIN_DONE" if SELECT_REAL_BIN else "DONE"
elif self.state == "BIN_DONE":
self.bin_state[self.current_bin]["done"] = True
self.current_bin = planning.selectBin(self.bin_state)
if self.current_bin is None:
self.state = "DONE"
else:
self.state = "START"
elif self.state == "DONE":
print "actual vacuum point: ", se3.apply(self.Tvacuum, [0, 0, 0])
else:
print FAIL_COLOR + "Unknown state" + END_COLOR
time.sleep(1)
except KeyboardInterrupt:
motion.robot.stopMotion()
sys.exit(0)
