diff --git a/.gitignore b/.gitignore index 443266f..7372455 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,4 @@ __pycache__/ logs/ extracted_data/ outputs/ +views_test/ diff --git a/configs/collect_data.yaml b/configs/collect_data.yaml index 3628823..c527f5b 100644 --- a/configs/collect_data.yaml +++ b/configs/collect_data.yaml @@ -2,7 +2,7 @@ defaults: - _self_ - camera -storage_path: "/home/siddhant/github/Franka-Teach/data/extracted_data/1229_human_pick" +storage_path: "/home/siddhant/github/Franka-Teach/data/extracted_data/0102_test" demo_num: 0 collect_img: True collect_depth: False diff --git a/frankateach/configs/deoxys_right.yml b/frankateach/configs/deoxys_right.yml index d2feda0..022cf0f 100644 --- a/frankateach/configs/deoxys_right.yml +++ b/frankateach/configs/deoxys_right.yml @@ -15,7 +15,7 @@ ROBOT: CONTROL: STATE_PUBLISHER_RATE: 100 - POLICY_RATE: 60 + POLICY_RATE: 20 #60 TRAJ_RATE: 500 ZMQ_NOBLOCK: true diff --git a/frankateach/configs/osc-pose-controller.yml b/frankateach/configs/osc-pose-controller.yml index f7d2949..0d0311c 100644 --- a/frankateach/configs/osc-pose-controller.yml +++ b/frankateach/configs/osc-pose-controller.yml @@ -7,13 +7,13 @@ traj_interpolator_cfg: time_fraction: 0.3 Kp: - # translation: 350.0 - # rotation: 300.0 translation: 250.0 rotation: 250.0 + # translation: 1000.0 #400.0 + # rotation: 400.0 action_scale: - translation: 1.0 # 0.05 + translation: 0.05 #1.0 rotation: 1.0 residual_mass_vec: [0.0, 0.0, 0.0, 0.0, 0.1, 0.5, 0.5] diff --git a/frankateach/franka_server.py b/frankateach/franka_server.py index 654b92c..e549792 100644 --- a/frankateach/franka_server.py +++ b/frankateach/franka_server.py @@ -68,6 +68,7 @@ def control_daemon(self): franka_control.quat, franka_control.gripper, ) + time.sleep(0.1) self.action_socket.send(self.get_state()) except KeyboardInterrupt: pass @@ -101,31 +102,67 @@ def reset_robot(self): def osc_move(self, target_pos, target_quat, gripper_state): target_mat = transform_utils.pose2mat(pose=(target_pos, target_quat)) - current_quat, current_pos = self.last_eef_quat_and_pos - current_mat = transform_utils.pose2mat( - pose=(current_pos.flatten(), current_quat.flatten()) - ) + idx = 0 + for idx in range(40): + current_quat, current_pos = self.last_eef_quat_and_pos + current_mat = transform_utils.pose2mat( + pose=(current_pos.flatten(), current_quat.flatten()) + ) - pose_error = transform_utils.get_pose_error( - target_pose=target_mat, current_pose=current_mat - ) + pose_error = transform_utils.get_pose_error( + target_pose=target_mat, current_pose=current_mat + ) + # if np.linalg.norm(pose_error[:3]) < 0.01: + # break - if np.dot(target_quat, current_quat) < 0.0: - current_quat = -current_quat + if np.dot(target_quat, current_quat) < 0.0: + current_quat = -current_quat - quat_diff = transform_utils.quat_distance(target_quat, current_quat) - axis_angle_diff = transform_utils.quat2axisangle(quat_diff) + quat_diff = transform_utils.quat_distance(target_quat, current_quat) + axis_angle_diff = transform_utils.quat2axisangle(quat_diff) - action_pos = pose_error[:3] - action_axis_angle = axis_angle_diff.flatten() + # action_pos = pose_error[:3] + # action_axis_angle = axis_angle_diff.flatten() + action_pos = pose_error[:3] * 10 + action_axis_angle = axis_angle_diff.flatten() * 1 + action_pos = np.clip(action_pos, -1.0, 1.0) + action_axis_angle = np.clip(action_axis_angle, -0.5, 0.5) - action = action_pos.tolist() + action_axis_angle.tolist() + [gripper_state] + action = action_pos.tolist() + action_axis_angle.tolist() + [gripper_state] - self.control( - controller_type="OSC_POSE", - action=action, - controller_cfg=self.velocity_controller_cfg, - ) + self.control( + controller_type="OSC_POSE", + action=action, + controller_cfg=self.velocity_controller_cfg, + ) + print(idx) + + # target_axis_angle = transform_utils.quat2axisangle(target_quat) + # current_rot, current_pos = self.last_eef_rot_and_pos + + # for _ in range(80): + # current_pose = self.last_eef_pose + # current_pos = current_pose[:3, 3:] + # current_rot = current_pose[:3, :3] + # current_quat = transform_utils.mat2quat(current_rot) + # if np.dot(target_quat, current_quat) < 0.0: + # current_quat = -current_quat + # quat_diff = transform_utils.quat_distance(target_quat, current_quat) + # current_axis_angle = transform_utils.quat2axisangle(current_quat) + # axis_angle_diff = transform_utils.quat2axisangle(quat_diff) + # action_pos = (target_pos - current_pos).flatten() * 10 + # action_axis_angle = axis_angle_diff.flatten() * 1 + # action_pos = np.clip(action_pos, -1.0, 1.0) + # action_axis_angle = np.clip(action_axis_angle, -0.5, 0.5) + + # action = action_pos.tolist() + action_axis_angle.tolist() + [gripper_state] + # # logger.info(f"Axis angle action {action_axis_angle.tolist()}") + # # print(np.round(action, 2)) + # self.control( + # controller_type="OSC_POSE", + # action=action, + # controller_cfg=self.velocity_controller_cfg, + # ) def reset_joints( self, diff --git a/views_test.py b/views_test.py new file mode 100644 index 0000000..8e82fd3 --- /dev/null +++ b/views_test.py @@ -0,0 +1,45 @@ +import cv2 +import shutil +from pathlib import Path + + +def test_camera_index(index): + cap = cv2.VideoCapture(index) + cap.set(cv2.CAP_PROP_FRAME_WIDTH, 680) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) + if not cap.isOpened(): + print(f"Failed to open camera at index {index}") + return + + idx = 0 + while True: + ret, frame = cap.read() + if not ret: + print(f"Failed to capture frame from camera at index {index}") + break + + idx += 1 + if idx == 10: + print( + "Frame default resolution: (" + + str(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) + + "; " + + str(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) + + ")" + ) + cv2.imwrite(f"views_test/camera_{index}.jpg", frame) + break + + cap.release() + + +# Test camera indices from 0 to 100 +if __name__ == "__main__": + # make directory to save images + dir_path = Path("./views_test") + if dir_path.exists(): + shutil.rmtree(dir_path) + Path("views_test").mkdir(parents=True, exist_ok=True) + + for camera_id in range(100): + test_camera_index(camera_id)