diff --git a/omnigibson/controllers/multi_finger_gripper_controller.py b/omnigibson/controllers/multi_finger_gripper_controller.py index b56bd1583..b5d2be316 100644 --- a/omnigibson/controllers/multi_finger_gripper_controller.py +++ b/omnigibson/controllers/multi_finger_gripper_controller.py @@ -3,6 +3,7 @@ from omnigibson.macros import create_module_macros from omnigibson.controllers import IsGraspingState, ControlType, GripperController from omnigibson.utils.python_utils import assert_valid_key +import omnigibson.utils.transform_utils as T VALID_MODES = { "binary", @@ -41,6 +42,7 @@ def __init__( inverted=False, mode="binary", limit_tolerance=0.001, + use_delta_commands=False, ): """ Args: @@ -76,6 +78,7 @@ def __init__( 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") @@ -84,6 +87,7 @@ 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 @@ -92,6 +96,13 @@ def __init__( if mode == "binary": 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" + ), "Cannot use 'default' command output limits in delta commands mode of JointController. Try None instead." + + # Run super init super().__init__( control_freq=control_freq, @@ -146,6 +157,29 @@ def _command_to_control(self, command, control_dict): if command[0] >= 0.0 else self._control_limits[ControlType.get_type(self._motor_type)][0][self.dof_idx] ) + # If we're using delta commands, add this value + elif self._use_delta_commands: + # 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 else: # Use continuous signal u = command @@ -251,6 +285,14 @@ 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) diff --git a/omnigibson/tasks/grasp_task.py b/omnigibson/tasks/grasp_task.py index 78f15ee0f..f77184936 100644 --- a/omnigibson/tasks/grasp_task.py +++ b/omnigibson/tasks/grasp_task.py @@ -90,7 +90,9 @@ def _reset_agent(self, env): robot_pose = self._primitive_controller._get_robot_pose_from_2d_pose(sampled_pose_2d) robot.set_position_orientation(*robot_pose) - self._primitive_controller._settle_robot() + # Settle robot + for i in range(100): + og.sim.step() print("Reset robot pose to: ", robot_pose) # Overwrite reset by only removeing reset scene diff --git a/rl/test_reset.py b/rl/test_reset.py index fce12e2ed..23a900bc1 100644 --- a/rl/test_reset.py +++ b/rl/test_reset.py @@ -43,7 +43,7 @@ def main(iterations): "obs_modalities": ["rgb", "depth_linear", "seg_instance", "seg_semantic", "proprio"], "proprio_obs": ["robot_pos", "robot_2d_ori", "joint_qpos", "joint_qvel", "eef_0_pos", "eef_0_quat", "grasp_0"], "scale": 1.0, - "self_collisions": False, + "self_collisions": True, "action_normalize": False, "action_type": "continuous", "grasping_mode": "sticky", @@ -139,6 +139,7 @@ def main(iterations): # Testing random actions with env ############################# + from IPython import embed; embed() for i in tqdm(range(int(iterations))): try: done = False