Changes on top of raunaq/devel

.gitignore

@@ -3,3 +3,4 @@ __pycache__/
 logs/
 extracted_data/
 outputs/
+views_test/

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

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
 

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]

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,

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)