Small fixes

jimmyyhwu · jimmyyhwu · commit dcf44dff27f4 · 2023-08-31T15:16:16.000-07:00
diff --git a/robot/controller.py b/robot/controller.py
@@ -390,6 +390,7 @@ def __init__(self, robot_idx):
         # Arm setup
         self.arm = KinovaArm()
         self.arm.clear_faults()
+        self.arm.set_high_level_servoing()
         self.arm.set_joint_limits(4 * [80] + 3 * [70], [297] + 6 * [150])
         self.arm.set_max_twist_linear_limit()
         self.arm.move_angular([self.arm.get_heading(), 340, 180, 214, 0, 320, 90])
diff --git a/robot/kinova.py b/robot/kinova.py
@@ -91,13 +91,13 @@ def retract(self, blocking=True):
     def zero(self, blocking=True):
         self._reference_action('Zero', blocking=blocking)
 
-    def move_angular(self, joint_positions, blocking=True):
-        assert len(joint_positions) == self.num_joints
+    def move_angular(self, joint_angles, blocking=True):
+        assert len(joint_angles) == self.num_joints
         action = Base_pb2.Action()
         for i in range(self.num_joints):
             joint_angle = action.reach_joint_angles.joint_angles.joint_angles.add()
             joint_angle.joint_identifier = i
-            joint_angle.value = joint_positions[i]
+            joint_angle.value = joint_angles[i]
         self.end_or_abort_event.clear()
         self.base.ExecuteAction(action)
         if blocking:
@@ -340,12 +340,12 @@ def trapezoidal_motion_profile(pos_diff, max_vel=60, max_accel=80, max_decel=80,
 
         return t, pos
 
-    def execute(self, joint_positions, max_vel=60, max_accel=80, max_decel=80, gripper_release_ms=600):
-        assert len(joint_positions) == self.num_joints
+    def execute(self, joint_angles, max_vel=60, max_accel=80, max_decel=80, gripper_release_ms=600):
+        assert len(joint_angles) == self.num_joints
 
         # Compute motion profile
         self.base_feedback = self.base_cyclic.RefreshFeedback()
-        pos_diffs = [((joint_positions[i] - self.base_feedback.actuators[i].position) + 180) % 360 - 180 for i in range(self.num_joints)]
+        pos_diffs = [((joint_angles[i] - self.base_feedback.actuators[i].position) + 180) % 360 - 180 for i in range(self.num_joints)]
         max_pos_diff = max(abs(pos_diff) for pos_diff in pos_diffs)
         _, pos = self.trapezoidal_motion_profile(max_pos_diff, max_vel=max_vel, max_accel=max_accel, max_decel=max_decel)
         self.trajectory = np.zeros((pos.shape[0], self.num_joints), dtype=np.float32)
diff --git a/server/demo.py b/server/demo.py
@@ -152,7 +152,7 @@ def filter_detection_output(self, output):
             ignore_boxes.append([min_xy[0], min_xy[1], max_xy[0], max_xy[1]])
 
         # Receptacles
-        # Note: Due to camera perspective distortion, receptacles should be placed near edges of overhead camera image frame
+        # Note: Due to camera perspective distortion, receptacles should not be placed near edges of overhead camera image frame
         for receptacle in self.receptacles.values():
             pos_x, pos_y = receptacle['position']
             dim_x, dim_y = receptacle['dimensions']
@@ -316,7 +316,7 @@ def run(self):
         autonomous_running = False
         detection_image_sent = False
         last_time = time.time()
-        controller_state_time = None
+        controller_start_time = None
         while True:
             step_time = time.time() - last_time
             if step_time > 0.5:  # 2 Hz
@@ -325,8 +325,8 @@ def run(self):
 
             # Update robot state
             controller_data = controller.get_controller_data()  # 30 ms
-            if controller_data['start_time'] != controller_state_time:  # Controller was restarted
-                controller_state_time = controller_data['start_time']
+            if controller_data['start_time'] != controller_start_time:  # Controller was restarted
+                controller_start_time = controller_data['start_time']
                 autonomous_running = False
             if controller_data['state'] == 'moving':
                 self.robot_state = 'moving'
diff --git a/server/object_detector_server.py b/server/object_detector_server.py
@@ -87,6 +87,14 @@ def __init__(self, hostname='0.0.0.0', port=6003, debug=False):
         self.listener = Listener((hostname, port), authkey=CONN_AUTHKEY)
         self.lock = Lock()
 
+        # Set up image directory
+        today = datetime.now().strftime('%Y-%m-%d')
+        self.image_dir = Path(f'images/{today}')
+        if not self.image_dir.exists():
+            self.image_dir.mkdir(parents=True)
+        if sum(1 for _ in self.image_dir.iterdir()) > 1000:
+            input(f'Warning: {self.image_dir} contains over 1000 files. Press <Enter> to continue:\n')
+
         # Set up ViLD object detector
         self.detector = VildDetector()
 
@@ -102,13 +110,7 @@ def __init__(self, hostname='0.0.0.0', port=6003, debug=False):
 
     def forward(self, request):
         # Save JPEG image to file
-        today = datetime.now().strftime('%Y-%m-%d')
-        image_dir = Path(f'images/{today}')
-        if not image_dir.exists():
-            image_dir.mkdir(parents=True)
-        if sum(1 for _ in image_dir.iterdir()) > 1000:
-            print(f'Warning: {image_dir} contains over 1000 files')
-        image_path = str(image_dir / f'image-{int(10 * time.time()) % 100000000}.jpg')
+        image_path = str(self.image_dir / f'image-{int(10 * time.time()) % 100000000}.jpg')
         with open(image_path, 'wb') as f:
             f.write(request['encoded_image'])
 
