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]))