def loop(self):
while True:
# Low pass filtering
fc = 1 # (Hz)
dt = self.__interval_ms * 0.001 # (sec)
jaw1_curr_PSM1 = U.LPF(self.__actuator_current_measured_PSM1[5],
self.__jaw1_curr_PSM1_prev, fc, dt)
self.__jaw1_curr_PSM1_prev = jaw1_curr_PSM1
jaw2_curr_PSM1 = U.LPF(self.__actuator_current_measured_PSM1[6],
self.__jaw2_curr_PSM1_prev, fc, dt)
self.__jaw2_curr_PSM1_prev = jaw2_curr_PSM1
jaw1_curr_PSM2 = U.LPF(self.__actuator_current_measured_PSM2[5],
self.__jaw1_curr_PSM2_prev, fc, dt)
self.__jaw1_curr_PSM2_prev = jaw1_curr_PSM2
jaw2_curr_PSM2 = U.LPF(self.__actuator_current_measured_PSM2[6],
self.__jaw2_curr_PSM2_prev, fc, dt)
self.__jaw2_curr_PSM2_prev = jaw2_curr_PSM2
# Current thresholding
if (self.__state_jaw_current_PSM1[0] <= 0) and (
jaw1_curr_PSM1 - jaw2_curr_PSM1 > self.threshold_PSM1):
self.grasp_detected_PSM1 = True
else:
self.grasp_detected_PSM1 = False
if (self.__state_jaw_current_PSM2[0] <= 0) and (
jaw1_curr_PSM2 - jaw2_curr_PSM2 > self.threshold_PSM2):
self.grasp_detected_PSM2 = True
else:
self.grasp_detected_PSM2 = False
self.rate.sleep()
def run(self, stop):
while True:
# Resizing images
img1 = cv2.resize(self.__img_raw_left,
(self.__img_width, self.__img_height))
img2 = cv2.resize(self.__img_raw_right,
(self.__img_width, self.__img_height))
# Low pass filtering
fc = 1 # (Hz)
dt = 0.01 # (ms)
jaw1_curr_PSM1 = U.LPF(self.__actuator_current_measured_PSM1[5],
self.__jaw1_curr_PSM1_prev, fc, dt)
self.__jaw1_curr_PSM1_prev = jaw1_curr_PSM1
jaw2_curr_PSM1 = U.LPF(self.__actuator_current_measured_PSM1[6],
self.__jaw2_curr_PSM1_prev, fc, dt)
self.__jaw2_curr_PSM1_prev = jaw2_curr_PSM1
jaw1_curr_PSM2 = U.LPF(self.__actuator_current_measured_PSM2[5],
self.__jaw1_curr_PSM2_prev, fc, dt)
self.__jaw1_curr_PSM2_prev = jaw1_curr_PSM2
jaw2_curr_PSM2 = U.LPF(self.__actuator_current_measured_PSM2[6],
self.__jaw2_curr_PSM2_prev, fc, dt)
self.__jaw2_curr_PSM2_prev = jaw2_curr_PSM2
# Current thresholding
self.__threshold_PSM1 = 0.3 # (A)
self.__threshold_PSM2 = 0.25 # (A)
if self.__state_jaw_current_PSM1[0] <= 0:
if jaw1_curr_PSM1 - jaw2_curr_PSM1 > self.__threshold_PSM1:
cv2.circle(img1, (540, 300), 30, (0, 255, 0), -1)
else:
cv2.circle(img1, (540, 300), 30, (0, 0, 255), -1)
else:
cv2.circle(img1, (540, 300), 30, (0, 0, 255), -1)
if self.__state_jaw_current_PSM2[0] <= 0:
if jaw1_curr_PSM2 - jaw2_curr_PSM2 > self.__threshold_PSM2:
cv2.circle(img1, (100, 300), 30, (0, 255, 0), -1)
else:
cv2.circle(img1, (100, 300), 30, (0, 0, 255), -1)
else:
cv2.circle(img1, (100, 300), 30, (0, 0, 255), -1)
cv2.imshow("original1", img1)
if self.__video_recording is True:
self.out.write(img1)
self.rate.sleep()
if cv2.waitKey(1) & 0xFF == ord('q'):
if self.__video_recording is True:
print("finishing recording")
self.out.release()
cv2.destroyAllWindows()
stop()
break
plt.rc('legend', fontsize=15) # legend fontsize
plt.rc('figure', titlesize=10) # fontsize of the figure title
# Trajectory check
BD = BallDetectionRGBD()
file_path = root + 'experiment/0_trajectory_extraction/'
arm_traj = np.load(file_path + 'ready_movement.npy')
wrist_traj = np.load(file_path + 'ready_movement.npy')
# arm_traj[:, 2] -= 0.010
# arm_traj = arm_traj[6255-5618:]
# wrist_traj = wrist_traj[1:]
# print(arm_traj.shape, wrist_traj.shape)
fc = 5
dt = 0.01
arm_traj = U.LPF(arm_traj, fc, dt)
wrist_traj = U.LPF(wrist_traj, fc, dt)
arm_traj = arm_traj[::
3] # down sampling by 3 for peg transfer and by 2 for random trajectory
wrist_traj = wrist_traj[::3]
print('data length: ', len(arm_traj), len(wrist_traj))
pos = np.array([
BD.fk_position(q[0], q[1], q[2], 0, 0, 0, L1=BD.L1, L2=BD.L2, L3=0, L4=0)
for q in arm_traj
])
print("min_x=", min(pos[:, 0]))
print("max_x=", max(pos[:, 0]))
print("min_y=", min(pos[:, 1]))
print("max_y=", max(pos[:, 1]))
print("min_z=", min(pos[:, 2]))
