Merge pull request #315 from StanfordVL/fixes-for-primitives

Fixes for primitives

omnigibson/action_primitives/starter_semantic_action_primitives.py

@@ -64,7 +64,7 @@
 m.MAX_STEPS_FOR_HAND_MOVE_JOINT = 500
 m.MAX_STEPS_FOR_HAND_MOVE_IK = 1000
 m.MAX_STEPS_FOR_GRASP_OR_RELEASE = 30
-m.MAX_STEPS_FOR_WAYPOINT_NAVIGATION = 200
+m.MAX_STEPS_FOR_WAYPOINT_NAVIGATION = 500
 m.MAX_ATTEMPTS_FOR_OPEN_CLOSE = 20
 
 m.MAX_ATTEMPTS_FOR_SAMPLING_POSE_WITH_OBJECT_AND_PREDICATE = 20
@@ -295,7 +295,6 @@ def _postprocess_action(self, action):
                 action_type = "nav"
 
         context = action_type + context_function
-        # print(context)
         return action, context
 
     def _load_robot_copy(self):
@@ -1739,7 +1738,7 @@ def _test_pose(self, pose_2d, context, pose_on_obj=None):
 
         Args:
             pose_2d (Iterable): (x, y, yaw) 2d pose
-            context (Context): Undoable context reference
+            context (Context): Planning context reference
             pose_on_obj (Iterable of arrays): Pose on the object in the world frame
 
         Returns:

omnigibson/robots/manipulation_robot.py

@@ -1093,11 +1093,6 @@ def _handle_assisted_grasping(self, action):
         """
         # Loop over all arms
         for arm in self.arm_names:
-            # Make sure gripper action dimension is only 1
-            cmd_dim = self._controllers[f"gripper_{arm}"].command_dim
-            assert cmd_dim == 1, \
-                f"Gripper {arm} controller command dim must be 1 to use assisted grasping, got: {cmd_dim}."
-
             # We apply a threshold based on the control rather than the command here so that the behavior
             # stays the same across different controllers and control modes (absolute / delta). This way,
             # a zero action will actually keep the AG setting where it already is.

tests/test_primitives.py

@@ -65,18 +65,20 @@ def primitive_tester(load_object_categories, objects, primitives, primitives_arg
                         "motor_type": "position",
                         "command_input_limits": [-1, 1],
                         "command_output_limits": None,
-                        "use_delta_commands": True,
+                        "use_delta_commands": True
                     },
                     "gripper_right": {
                         "name": "JointController",
                         "motor_type": "position",
                         "command_input_limits": [-1, 1],
                         "command_output_limits": None,
-                        "use_delta_commands": True,
+                        "use_delta_commands": True
                     },
                     "camera": {
                         "name": "JointController",
-                        "motor_type": "velocity",
+                        "motor_type": "position",
+                        "command_input_limits": None,
+                        "command_output_limits": None,
                         "use_delta_commands": False
                     }
                 }
@@ -103,9 +105,9 @@ def primitive_tester(load_object_categories, objects, primitives, primitives_arg
         og.sim.step()
 
     controller = StarterSemanticActionPrimitives(env)
+    set_start_pose(robot)
     for primitive, args in zip(primitives, primitives_args):
         try:
-            set_start_pose(robot)
             execute_controller(controller.apply_ref(primitive, *args), env)
         except:
             og.sim.clear()
@@ -116,17 +118,16 @@ def primitive_tester(load_object_categories, objects, primitives, primitives_arg
     return True
 
 def test_navigate():
-    categories = ["floors", "ceilings", "walls", "coffee_table"]
+    categories = ["floors", "ceilings", "walls"]
 
     objects = []
     obj_1 = {
         "object": DatasetObject(
-                name="table",
-                category="breakfast_table",
-                model="rjgmmy",
-                scale=[0.3, 0.3, 0.3]
-            ),
-        "position": [-0.7, -2.0, 0.2],
+            name="cologne",
+            category="bottle_of_cologne",
+            model="lyipur"
+        ),
+        "position": [-0.3, -0.8, 0.5],
         "orientation": [0, 0, 0, 1]
     }
     objects.append(obj_1)
@@ -141,30 +142,18 @@ def test_grasp():
 
     objects = []
     obj_1 = {
-        "object": DatasetObject(
-                name="table",
-                category="breakfast_table",
-                model="rjgmmy",
-                scale=[0.3, 0.3, 0.3]
-            ),
-        "position": [-0.7, 0.5, 0.2],
-        "orientation": [0, 0, 0, 1]
-    }
-    obj_2 = {
         "object": DatasetObject(
             name="cologne",
-            category="cologne",
-            model="lyipur",
-            scale=[0.01, 0.01, 0.01]
+            category="bottle_of_cologne",
+            model="lyipur"
         ),
         "position": [-0.3, -0.8, 0.5],
         "orientation": [0, 0, 0, 1]
     }
     objects.append(obj_1)
-    objects.append(obj_2)
 
     primitives = [StarterSemanticActionPrimitiveSet.GRASP]
-    primitives_args = [(obj_2['object'],)]    
+    primitives_args = [(obj_1['object'],)]    
 
     assert primitive_tester(categories, objects, primitives, primitives_args)
 
@@ -185,9 +174,8 @@ def test_place():
     obj_2 = {
         "object": DatasetObject(
             name="cologne",
-            category="cologne",
-            model="lyipur",
-            scale=[0.01, 0.01, 0.01]
+            category="bottle_of_cologne",
+            model="lyipur"
         ),
         "position": [-0.3, -0.8, 0.5],
         "orientation": [0, 0, 0, 1]
@@ -240,5 +228,4 @@ def test_open_revolute():
     primitives = [StarterSemanticActionPrimitiveSet.OPEN]
     primitives_args = [(obj_1['object'],)]    
 
-    assert primitive_tester(categories, objects, primitives, primitives_args)
-
+    assert primitive_tester(categories, objects, primitives, primitives_args)