def benchmark_time(arm, START, TARGET, SPEED_CLASS):
""" This method returns an estimate of the speed. """
times = []
for i in range(NUM_RUNS):
move(arm, START,
'Slow') # Just bring it slowly to the starting position.
time.sleep(2)
start_time = time.time()
move(arm, TARGET, SPEED_CLASS) # This is what we measure.
difference = time.time() - start_time
times.append(difference)
time.sleep(2)
times = np.array(times)
print("\ntimes (in seconds!!) for speed class {}:\n{}".format(
SPEED_CLASS, times))
print("mean(times): {:.2f}".format(np.mean(times)))
print("std(times): {:.2f}".format(np.std(times)))
if __name__ == "__main__":
""" See the top of the file for program-wide arguments. """
arm, _, d = utilities.initializeRobots(sleep_time=2)
arm.close_gripper()
START = [HOME[0] - 0.03, HOME[1], HOME[2]]
TARGET = [START[0] + 0.1, START[1], START[2]]
benchmark_time(arm, START, TARGET, SPEED_CLASS='Slow')
benchmark_time(arm, START, TARGET, SPEED_CLASS='Medium')
benchmark_time(arm, START, TARGET, SPEED_CLASS='Fast')
IMDIR = 'images/check_regressors_v' + OUT_VERSION + '/'
assert (OUT_VERSION is not None) and (IN_VERSION is not None)
# To prevent overriding
assert not os.path.exists(OUTPUT_FILE), "OUTPUT_FILE: {}".format(
OUTPUT_FILE)
assert not os.path.exists(IMDIR), "IMDIR: {}".format(IMDIR)
os.makedirs(IMDIR)
PARAMS = pickle.load(
open(
'config/mapping_results/auto_params_matrices_v' + IN_VERSION +
'.p', 'r'))
rotation = sample_rotation(args.fixed_yaw)
print("Our starting rotation: {}".format(rotation))
# Now get the robot arm initialized and test it out!
arm, _, d = utils.initializeRobots()
print("current arm position: {}".format(
arm.get_current_cartesian_position()))
utils.home(arm, rot=rotation)
print("current arm position: {}".format(
arm.get_current_cartesian_position()))
arm.close_gripper()
## ---------------------------------------------------------------------------------
## This will test calibration! Progressively accumulate 36 (or 35...) values. Do the
## same for the left image. Then, with this data, we redo the entire process but we
## know we no longer have to do the repeated skipping I was doing.
## Note: this used to be its own method, but I got confused about the callback code,
## and I'm in a bit of a rush, so sorry!
## ---------------------------------------------------------------------------------
cv2.imwrite(DIR+'calibration_blank_image_'+name+'_v'+VERSION+'.jpg', image)
cv2.rectangle(image, (xx,yy), (xx+ww, yy+hh), (0,255,0), 2)
cv2.putText(img=image, text='{},{}'.format(xx,yy), org=(xx,yy),
fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1, color=(0,0,0), thickness=2)
cv2.putText(img=image, text='{},{}'.format(xx,yy+hh), org=(xx,yy+hh),
fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1, color=(0,0,0), thickness=2)
cv2.putText(img=image, text='{},{}'.format(xx+ww,yy), org=(xx+ww,yy),
fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1, color=(0,0,0), thickness=2)
cv2.putText(img=image, text='{},{}'.format(xx+ww,yy+hh), org=(xx+ww,yy+hh),
fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1, color=(0,0,0), thickness=2)
cv2.imshow("Camera Image w/BBox", image)
cv2.waitKey(0)
cv2.destroyAllWindows()
cv2.imwrite(DIR+'calibration_bbox_image_'+name+'_v'+VERSION+'.jpg', image)
if __name__ == "__main__":
arm1, _, d = utilities.initializeRobots()
arm1.close_gripper()
image = d.left_image.copy()
save_bounding_box(image, left=True)
pickle.dump(d.left_contours, open(DIR+'contours_left_v'+VERSION+'.p', 'w'))
image = d.right_image.copy()
save_bounding_box(image, left=False)
pickle.dump(d.right_contours, open(DIR+'contours_right_v'+VERSION+'.p', 'w'))
