Merge branch 'rl-primitives-training' of https://github.com/StanfordV…

…L/OmniGibson into rl-primitives-training

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)

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

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