Merge pull request #842 from StanfordVL/curobo

Revive action primitives examples and tests

omnigibson/action_primitives/symbolic_semantic_action_primitives.py

@@ -12,7 +12,7 @@
 from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives
 from omnigibson.objects import DatasetObject
 from omnigibson.robots.robot_base import BaseRobot
-from omnigibson.transition_rules import REGISTERED_RULES, TransitionRuleAPI
+from omnigibson.transition_rules import REGISTERED_RULES, SlicingRule, TransitionRuleAPI
 
 
 class SymbolicSemanticActionPrimitiveSet(IntEnum):
@@ -291,7 +291,7 @@ def _soak_under(self, obj):
         producing_systems = {
             ps
             for ps in obj.scene.system_registry.objects
-            if obj.states[object_states.ParticleSource].check_conditions_for_system(ps)
+            if obj.states[object_states.ParticleSource].check_conditions_for_system(ps.name)
         }
         if not producing_systems:
             raise ActionPrimitiveError(
@@ -309,8 +309,9 @@ def _soak_under(self, obj):
             )
 
         supported_systems = {
-            x for x in producing_systems if obj_in_hand.states[object_states.ParticleRemover].supports_system(x)
+            x for x in producing_systems if obj_in_hand.states[object_states.ParticleRemover].supports_system(x.name)
         }
+
         if not supported_systems:
             raise ActionPrimitiveError(
                 ActionPrimitiveError.Reason.PRE_CONDITION_ERROR,
@@ -328,7 +329,7 @@ def _soak_under(self, obj):
         currently_removable_systems = {
             x
             for x in supported_systems
-            if obj_in_hand.states[object_states.ParticleRemover].check_conditions_for_system(x)
+            if obj_in_hand.states[object_states.ParticleRemover].check_conditions_for_system(x.name)
         }
         if not currently_removable_systems:
             # TODO: This needs to be far more descriptive.
@@ -346,6 +347,9 @@ def _soak_under(self, obj):
         for system in currently_removable_systems:
             obj_in_hand.states[object_states.Saturated].set_value(system, True)
 
+        # Yield some actions
+        yield from self._settle_robot()
+
     def _soak_inside(self, obj):
         # Check that our current object is a particle remover
         obj_in_hand = self._get_obj_in_hand()
@@ -382,7 +386,7 @@ def _soak_inside(self, obj):
             )
 
         supported_systems = {
-            x for x in contained_systems if obj_in_hand.states[object_states.ParticleRemover].supports_system(x)
+            x for x in contained_systems if obj_in_hand.states[object_states.ParticleRemover].supports_system(x.name)
         }
         if not supported_systems:
             raise ActionPrimitiveError(
@@ -401,7 +405,7 @@ def _soak_inside(self, obj):
         currently_removable_systems = {
             x
             for x in supported_systems
-            if obj_in_hand.states[object_states.ParticleRemover].check_conditions_for_system(x)
+            if obj_in_hand.states[object_states.ParticleRemover].check_conditions_for_system(x.name)
         }
         if not currently_removable_systems:
             # TODO: This needs to be far more descriptive.
@@ -419,6 +423,9 @@ def _soak_inside(self, obj):
         for system in currently_removable_systems:
             obj_in_hand.states[object_states.Saturated].set_value(system, True)
 
+        # Yield some actions
+        yield from self._settle_robot()
+
     def _wipe(self, obj):
         # Check that our current object is a particle remover
         obj_in_hand = self._get_obj_in_hand()
@@ -438,7 +445,7 @@ def _wipe(self, obj):
 
         # Check if the target object has any particles on it
         covering_systems = {
-            ps for ps in obj.scene.system_registry.objects if obj.states[object_states.Covered].get_value(ps.states)
+            ps for ps in obj.scene.system_registry.objects if obj.states[object_states.Covered].get_value(ps)
         }
         if not covering_systems:
             raise ActionPrimitiveError(
@@ -456,7 +463,7 @@ def _wipe(self, obj):
             )
 
         supported_systems = {
-            x for x in covering_systems if obj_in_hand.states[object_states.ParticleRemover].supports_system(x)
+            x for x in covering_systems if obj_in_hand.states[object_states.ParticleRemover].supports_system(x.name)
         }
         if not supported_systems:
             raise ActionPrimitiveError(
@@ -475,7 +482,7 @@ def _wipe(self, obj):
         currently_removable_systems = {
             x
             for x in supported_systems
-            if obj_in_hand.states[object_states.ParticleRemover].check_conditions_for_system(x)
+            if obj_in_hand.states[object_states.ParticleRemover].check_conditions_for_system(x.name)
         }
         if not currently_removable_systems:
             # TODO: This needs to be far more descriptive.
@@ -488,10 +495,12 @@ def _wipe(self, obj):
                     "particles the target object is covered by": sorted(x.name for x in covering_systems),
                 },
             )
-
-        # If so, remove the particles.
+        # If so, remove the particles on the target object
         for system in currently_removable_systems:
-            obj_in_hand.states[object_states.Covered].set_value(system, False)
+            obj.states[object_states.Covered].set_value(system, False)
+
+        # Yield some actions
+        yield from self._settle_robot()
 
     def _cut(self, obj):
         # Check that our current object is a slicer
@@ -523,7 +532,8 @@ def _cut(self, obj):
         # TODO: Do some more validation
         added_obj_attrs = []
         removed_objs = []
-        output = REGISTERED_RULES["SlicingRule"].transition({"sliceable": [obj]})
+        (slicing_rule,) = [rule for rule in obj.scene.transition_rule_api.active_rules if isinstance(rule, SlicingRule)]
+        output = slicing_rule.transition({"sliceable": [obj]})
         added_obj_attrs += output.add
         removed_objs += output.remove
 

omnigibson/configs/fetch_primitives.yaml

@@ -45,19 +45,16 @@ robots:
       base:
         name: DifferentialDriveController
       arm_0:
-        name: InverseKinematicsController
-        command_input_limits: default
-        command_output_limits:
-          - [-0.2, -0.2, -0.2, -0.5, -0.5, -0.5]
-          - [0.2, 0.2, 0.2, 0.5, 0.5, 0.5]
-        mode: pose_absolute_ori
-        kp: 300.0
+        name: JointController
+        motor_type: position
+        command_input_limits: null
+        use_delta_commands: false
       gripper_0:
         name: JointController
         motor_type: position
         command_input_limits: [-1, 1]
         command_output_limits: null
-        use_delta_commands: true
+        use_delta_commands: false
       camera:
         name: JointController
         use_delta_commands: False

omnigibson/configs/tiago_primitives.yaml

@@ -0,0 +1,82 @@
+env:
+  action_frequency: 30                  # (int): environment executes action at the action_frequency rate
+  physics_frequency: 120                # (int): physics frequency (1 / physics_timestep for physx)
+  device: null                          # (None or str): specifies the device to be used if running on the gpu with torch backend
+  automatic_reset: false                # (bool): whether to automatic reset after an episode finishes
+  flatten_action_space: false           # (bool): whether to flatten the action space as a sinle 1D-array
+  flatten_obs_space: false              # (bool): whether the observation space should be flattened when generated
+  use_external_obs: false               # (bool): Whether to use external observations or not
+  initial_pos_z_offset: 0.1
+  external_sensors: null                # (None or list): If specified, list of sensor configurations for external sensors to add. Should specify sensor "type" and any additional kwargs to instantiate the sensor. Each entry should be the kwargs passed to @create_sensor, in addition to position, orientation
+
+render:
+  viewer_width: 1280
+  viewer_height: 720
+
+scene:
+  type: InteractiveTraversableScene
+  scene_model: Rs_int
+  trav_map_resolution: 0.1
+  default_erosion_radius: 0.0
+  trav_map_with_objects: true
+  num_waypoints: 1
+  waypoint_resolution: 0.2
+  load_object_categories: null
+  not_load_object_categories: null
+  load_room_types: null
+  load_room_instances: null
+  load_task_relevant_only: false
+  seg_map_resolution: 0.1
+  scene_source: OG
+  include_robots: false
+
+robots:
+  - type: Tiago
+    obs_modalities: [rgb, depth, seg_semantic, normal, seg_instance, seg_instance_id]
+    scale: 1.0
+    self_collisions: true
+    action_normalize: false
+    action_type: continuous
+    grasping_mode: sticky
+    rigid_trunk: false
+    default_trunk_offset: 0.15
+    default_arm_pose: vertical
+    sensor_config:
+      VisionSensor:
+        sensor_kwargs:
+          image_height: 128
+          image_width: 128
+    controller_config:
+      base:
+        name: JointController
+      arm_left:
+        name: JointController
+        motor_type: position
+        command_input_limits: null
+        command_output_limits: null
+        use_delta_commands: false
+      arm_right:
+        name: JointController
+        motor_type: position
+        command_input_limits: null
+        command_output_limits: null
+        use_delta_commands: false
+      gripper_left:
+        name: JointController
+        motor_type: position
+        command_input_limits: [-1, 1]
+        command_output_limits: null
+        use_delta_commands: false
+      gripper_right:
+        name: JointController
+        motor_type: position
+        command_input_limits: [-1, 1]
+        command_output_limits: null
+        use_delta_commands: false
+      camera:
+        name: JointController
+
+objects: []
+
+task:
+  type: DummyTask

omnigibson/controllers/controller_base.py

@@ -169,7 +169,7 @@ def _preprocess_command(self, command):
             Array[float]: Processed command vector
         """
         # Make sure command is a th.tensor
-        command = th.tensor([command]) if type(command) in {int, float} else command
+        command = th.tensor([command], dtype=th.float32) if type(command) in {int, float} else command
         # We only clip and / or scale if self.command_input_limits exists
         if self._command_input_limits is not None:
             # Clip
@@ -336,7 +336,8 @@ def compute_no_op_goal(self, control_dict):
 
     def compute_no_op_action(self, control_dict):
         """
-        Compute no-op action given the goal
+        Compute a no-op action that updates the goal to match the current position
+        Disclaimer: this no-op might cause drift under external load (e.g. when the controller cannot reach the goal position)
         """
         if self._goal is None:
             self._goal = self.compute_no_op_goal(control_dict=control_dict)

omnigibson/controllers/dd_controller.py

@@ -114,7 +114,7 @@ def compute_no_op_goal(self, control_dict):
         return dict(vel=th.zeros(2))
 
     def _compute_no_op_action(self, control_dict):
-        return self._goal["vel"]
+        return th.zeros(2, dtype=th.float32)
 
     def _get_goal_shapes(self):
         # Add (2, )-array representing linear, angular velocity

omnigibson/controllers/ik_controller.py

@@ -352,34 +352,24 @@ def compute_no_op_goal(self, control_dict):
         )
 
     def _compute_no_op_action(self, control_dict):
-
-        target_pos = self._goal["target_pos"]
-        target_quat = self._goal["target_quat"]
         pos_relative = control_dict[f"{self.task_name}_pos_relative"]
         quat_relative = control_dict[f"{self.task_name}_quat_relative"]
 
-        command = th.zeros(6, dtype=th.float32, device=target_pos.device)
+        command = th.zeros(6, dtype=th.float32, device=pos_relative.device)
 
         # Handle position
         if self.mode == "absolute_pose":
-            command[:3] = target_pos
+            command[:3] = pos_relative
         else:
-            command[:3] = target_pos - pos_relative
+            # We can leave it as zero for delta mode.
+            pass
 
         # Handle orientation
-        if self.mode == "position_fixed_ori" or self.mode == "position_compliant_ori":
+        if self.mode in ("pose_absolute_ori", "absolute_pose"):
+            command[3:] = T.quat2axisangle(quat_relative)
+        else:
             # For these modes, we don't need to add orientation to the command
             pass
-        elif self.mode == "pose_absolute_ori" or self.mode == "absolute_pose":
-            command[3:] = T.quat2axisangle(target_quat)
-        else:  # pose_delta_ori control
-            current_rot = T.quat2mat(quat_relative)
-            target_rot = T.quat2mat(target_quat)
-            delta_rot = target_rot @ (current_rot.T)
-
-            # Convert delta rotation to axis-angle representation
-            delta_axisangle = T.quat2axisangle(T.mat2quat(delta_rot))
-            command[3:] = delta_axisangle
 
         return command
 

omnigibson/controllers/joint_controller.py

@@ -237,14 +237,18 @@ def compute_no_op_goal(self, control_dict):
         return dict(target=target)
 
     def _compute_no_op_action(self, control_dict):
-        if self._use_delta_commands:
-            base_value = control_dict[f"joint_{self._motor_type}"][self.dof_idx]
-            # TODO: do we need to care about the entire gimbal lock situation?
-            command = self._goal["target"] - base_value
-        else:
-            command = self._goal["target"]
-
-        return command
+        if self.motor_type == "position":
+            if self._use_delta_commands:
+                return th.zeros(self.command_dim)
+            else:
+                return control_dict[f"joint_position"][self.dof_idx]
+        elif self.motor_type == "velocity":
+            if self._use_delta_commands:
+                return -control_dict[f"joint_velocity"][self.dof_idx]
+            else:
+                return th.zeros(self.command_dim)
+
+        raise ValueError("Cannot compute noop action for effort motor type.")
 
     def _get_goal_shapes(self):
         return dict(target=(self.control_dim,))

omnigibson/controllers/multi_finger_gripper_controller.py

@@ -271,7 +271,25 @@ def compute_no_op_goal(self, control_dict):
         return dict(target=th.zeros(self.command_dim))
 
     def _compute_no_op_action(self, control_dict):
-        return self._goal["target"]
+        # Take care of the special case of binary control
+        if self._mode == "binary":
+            command_val = -1 if self.is_grasping() == IsGraspingState.TRUE else 1
+            if self._inverted:
+                command_val = -1 * command_val
+            return th.tensor([command_val], dtype=th.float32)
+
+        if self._motor_type == "position":
+            command = control_dict[f"joint_position"][self.dof_idx]
+        elif self._motor_type == "velocity":
+            command = th.zeros(self.command_dim)
+        else:
+            raise ValueError("Cannot compute noop action for effort motor type.")
+
+        # Convert to binary / smooth mode if necessary
+        if self._mode == "smooth":
+            command = th.mean(command, dim=-1, keepdim=True)
+
+        return command
 
     def _get_goal_shapes(self):
         return dict(target=(self.command_dim,))