diff --git a/omnigibson/controllers/multi_finger_gripper_controller.py b/omnigibson/controllers/multi_finger_gripper_controller.py index 0da127399..b85a53087 100644 --- a/omnigibson/controllers/multi_finger_gripper_controller.py +++ b/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)