Add ternary and delta modes to finger controller

omnigibson/controllers/multi_finger_gripper_controller.py

@@ -7,7 +7,9 @@
 
 VALID_MODES = {
     "binary",
+    "ternary",
     "smooth",
+    "smooth_delta"
     "independent",
 }
 
@@ -42,7 +44,6 @@ def __init__(
         inverted=False,
         mode="binary",
         limit_tolerance=0.001,
-        use_delta_commands=False,
     ):
         """
         Args:
@@ -73,12 +74,16 @@ def __init__(
 
                 "binary": 1D command, if preprocessed value > 0 is interpreted as an max open
                     (send max pos / vel / tor signal), otherwise send max close control signals
+                "ternary": 1D command, if preprocessed value > 0.33 is interpreted as an max open
+                    (send max pos / vel / tor signal), < 0.33 as max close control, and otherwise stay
+                    still
                 "smooth": 1D command, sends symmetric signal to both finger joints equal to the preprocessed commands
+                "smooth_delta": 1D command, sends symmetric signal to both finger joints equal to the preprocessed commands
+                    adding the command on top of the existing value
                 "independent": 2D command, sends independent signals to each finger joint equal to the preprocessed command
 
             limit_tolerance (float): sets the tolerance from the joint limit ends, below which controls will be zeroed
                 out if the control is using velocity or torque control
-            use_delta_commands (bool): whether inputted commands should be interpreted as delta or absolute values
         """
         # Store arguments
         assert_valid_key(key=motor_type.lower(), valid_keys=ControlType.VALID_TYPES_STR, name="motor_type")
@@ -87,19 +92,18 @@ def __init__(
         self._inverted = inverted
         self._mode = mode
         self._limit_tolerance = limit_tolerance
-        self._use_delta_commands = use_delta_commands
 
         # Create other args to be filled in at runtime
         self._is_grasping = IsGraspingState.FALSE
 
         # If we're using binary signal, we override the command output limits
-        if mode == "binary":
+        if mode == "binary" or mode == "ternary":
             command_output_limits = (-1.0, 1.0)
 
         # When in delta mode, it doesn't make sense to infer output range using the joint limits (since that's an
         # absolute range and our values are relative). So reject the default mode option in that case.
         assert not (
-            self._use_delta_commands and command_output_limits == "default"
+            mode == "smooth_delta" and command_output_limits == "default"
         ), "Cannot use 'default' command output limits in delta commands mode of JointController. Try None instead."
 
 
@@ -159,30 +163,22 @@ def _command_to_control(self, command, control_dict):
                 else self._control_limits[ControlType.get_type(self._motor_type)][0][self.dof_idx]
             )
 
+        elif self.mode == "ternary":
+            if command > 0.33:
+                u = self._control_limits[ControlType.get_type(self._motor_type)][1][self.dof_idx]
+            elif command < -0.33:
+                u = self._control_limits[ControlType.get_type(self._motor_type)][0][self.dof_idx]
+            else:
+                u = control_dict["joint_{}".format(self._motor_type)][self.dof_idx]
+
         # If we're using delta commands, add this value
-        elif self._use_delta_commands:
+        elif self.mode == "smooth_delta":
             # Compute the base value for the command.
             base_value = control_dict["joint_{}".format(self._motor_type)][self.dof_idx]
 
             # Apply the command to the base value.
             u = base_value + command
 
-            # Correct any gimbal lock issues using the compute_delta_in_quat_space group.
-            for rx_ind, ry_ind, rz_ind in self._compute_delta_in_quat_space:
-                # Grab the starting rotations of these joints.
-                start_rots = base_value[[rx_ind, ry_ind, rz_ind]]
-
-                # Grab the delta rotations.
-                delta_rots = command[[rx_ind, ry_ind, rz_ind]]
-
-                # Compute the final rotations in the quaternion space.
-                _, end_quat = T.pose_transform(np.zeros(3), T.euler2quat(delta_rots),
-                                               np.zeros(3), T.euler2quat(start_rots))
-                end_rots = T.quat2euler(end_quat)
-
-                # Update the command
-                u[[rx_ind, ry_ind, rz_ind]] = end_rots
-
         # Otherwise, control is simply the command itself        
         else:
             u = command
@@ -288,14 +284,6 @@ def is_grasping(self):
         # Return cached value
         return self._is_grasping
 
-    @property
-    def use_delta_commands(self):
-        """
-        Returns:
-            bool: Whether this controller is using delta commands or not
-        """
-        return self._use_delta_commands
-
     @property
     def control_type(self):
         return ControlType.get_type(type_str=self._motor_type)