Merge pull request #331 from StanfordVL/fix-solve-task

Update action primitives examples

omnigibson/configs/tiago_primitives.yaml

@@ -37,30 +37,30 @@ robots:
     obs_modalities: [scan, rgb, depth]
     scale: 1.0
     self_collisions: true
-    # action_normalize: false
     action_normalize: false
     action_type: continuous
     grasping_mode: sticky
-    disable_grasp_handling: true
     rigid_trunk: false
     default_trunk_offset: 0.365
     default_arm_pose: diagonal30
     controller_config:
       base:
         name: JointController
-        motor_type: "velocity"
+        motor_type: velocity
       arm_left:
-        name: JointController
-        motor_type: position
+        name: InverseKinematicsController
+        motor_type: velocity
         command_input_limits: null
         command_output_limits: null
-        use_delta_commands: false
+        mode : pose_absolute_ori
+        kv: 3.0
       arm_right:
-        name: JointController
-        motor_type: position
+        name: InverseKinematicsController
+        motor_type: velocity
         command_input_limits: null
         command_output_limits: null
-        use_delta_commands: false
+        mode : pose_absolute_ori
+        kv: 3.0
       gripper_left:
         name: JointController
         motor_type: position
@@ -75,7 +75,9 @@ robots:
         use_delta_commands: true
       camera:
         name: JointController
-        motor_type: "velocity"
+        motor_type: position
+        command_input_limits: null
+        command_output_limits: null
         use_delta_commands: false
 
 objects: []

omnigibson/examples/action_primitives/solve_simple_task.py

@@ -0,0 +1,85 @@
+"""
+Example script demo'ing robot primitive to solve a task
+"""
+import os
+import yaml
+import numpy as np
+
+import omnigibson as og
+from omnigibson.macros import gm
+from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, StarterSemanticActionPrimitiveSet
+
+# Don't use GPU dynamics and use flatcache for performance boost
+# gm.USE_GPU_DYNAMICS = True
+# gm.ENABLE_FLATCACHE = True
+
+def execute_controller(ctrl_gen, env):
+    for action in ctrl_gen:
+        env.step(action)
+
+def set_start_pose(robot):
+    reset_pose_tiago = np.array([
+        -1.78029833e-04,  3.20231302e-05, -1.85759447e-07, -1.16488536e-07,
+        4.55182843e-08,  2.36128806e-04,  1.50000000e-01,  9.40000000e-01,
+        -1.10000000e+00,  0.00000000e+00, -0.90000000e+00,  1.47000000e+00,
+        0.00000000e+00,  2.10000000e+00,  2.71000000e+00,  1.50000000e+00,
+        1.71000000e+00,  1.30000000e+00, -1.57000000e+00, -1.40000000e+00,
+        1.39000000e+00,  0.00000000e+00,  0.00000000e+00,  4.50000000e-02,
+        4.50000000e-02,  4.50000000e-02,  4.50000000e-02,
+    ])
+    robot.set_joint_positions(reset_pose_tiago)
+    og.sim.step()
+
+def main():
+    # Load the config
+    config_filename = os.path.join(og.example_config_path, "tiago_primitives.yaml")
+    config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader)
+
+    # Update it to create a custom environment and run some actions
+    config["scene"]["scene_model"] = "Rs_int"
+    config["scene"]["load_object_categories"] = ["floors", "ceilings", "walls", "coffee_table"]
+    config["objects"] = [
+        {
+            "type": "DatasetObject",
+            "name": "cologne",
+            "category": "bottle_of_cologne",
+            "model": "lyipur",
+            "position": [-0.3, -0.8, 0.5],
+            "orientation": [0, 0, 0, 1]
+        },
+        {
+            "type": "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]
+        }
+    ]
+
+    # Load the environment
+    env = og.Environment(configs=config)
+    scene = env.scene
+    robot = env.robots[0]
+
+    # Allow user to move camera more easily
+    og.sim.enable_viewer_camera_teleoperation()
+
+    controller = StarterSemanticActionPrimitives(env)
+    set_start_pose(robot)
+
+    # Grasp of cologne
+    grasp_obj = scene.object_registry("name", "cologne")
+    print("Executing controller")
+    execute_controller(controller.apply_ref(StarterSemanticActionPrimitiveSet.GRASP, grasp_obj), env)
+    print("Finished executing grasp")
+
+    # Place cologne on another table
+    print("Executing controller")
+    table = scene.object_registry("name", "table")
+    execute_controller(controller.apply_ref(StarterSemanticActionPrimitiveSet.PLACE_ON_TOP, table), env)
+    print("Finished executing place")
+
+if __name__ == "__main__":
+    main()

omnigibson/examples/action_primitives/wip_solve_behavior_task.py

@@ -0,0 +1,75 @@
+"""
+Example script demo'ing robot primitive to solve a task
+"""
+import os
+import yaml
+import numpy as np
+
+import omnigibson as og
+from omnigibson.macros import gm
+from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, StarterSemanticActionPrimitiveSet
+
+# Don't use GPU dynamics and use flatcache for performance boost
+# gm.USE_GPU_DYNAMICS = True
+# gm.ENABLE_FLATCACHE = True
+
+def execute_controller(ctrl_gen, env):
+    for action in ctrl_gen:
+        env.step(action)
+
+def set_start_pose(robot):
+    reset_pose_tiago = np.array([
+        -1.78029833e-04,  3.20231302e-05, -1.85759447e-07, -1.16488536e-07,
+        4.55182843e-08,  2.36128806e-04,  1.50000000e-01,  9.40000000e-01,
+        -1.10000000e+00,  0.00000000e+00, -0.90000000e+00,  1.47000000e+00,
+        0.00000000e+00,  2.10000000e+00,  2.71000000e+00,  1.50000000e+00,
+        1.71000000e+00,  1.30000000e+00, -1.57000000e+00, -1.40000000e+00,
+        1.39000000e+00,  0.00000000e+00,  0.00000000e+00,  4.50000000e-02,
+        4.50000000e-02,  4.50000000e-02,  4.50000000e-02,
+    ])
+    robot.set_joint_positions(reset_pose_tiago)
+    og.sim.step()
+
+def main():
+    # Load the config
+    config_filename = os.path.join(og.example_config_path, "tiago_primitives.yaml")
+    config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader)
+
+    # Update it to run a grocery shopping task
+    config["scene"]["scene_model"] = "Benevolence_1_int"
+    config["scene"]["load_task_relevant_only"] = True
+    config["scene"]["not_load_object_categories"] = ["ceilings"]
+    config["task"] = {
+        "type": "BehaviorTask",
+        "activity_name": "picking_up_trash",
+        "activity_definition_id": 0,
+        "activity_instance_id": 0,
+        "predefined_problem": None,
+        "online_object_sampling": False,
+    }
+
+    # Load the environment
+    env = og.Environment(configs=config)
+    scene = env.scene
+    robot = env.robots[0]
+
+    # Allow user to move camera more easily
+    og.sim.enable_viewer_camera_teleoperation()
+
+    controller = StarterSemanticActionPrimitives(env)
+    set_start_pose(robot)
+
+    # Grasp can of soda
+    grasp_obj = scene.object_registry("name", "can_of_soda_89")
+    print("Executing controller")
+    execute_controller(controller.apply_ref(StarterSemanticActionPrimitiveSet.GRASP, grasp_obj), env)
+    print("Finished executing grasp")
+
+    # Place can in trash can
+    print("Executing controller")
+    trash = scene.object_registry("name", "trash_can_85")
+    execute_controller(controller.apply_ref(StarterSemanticActionPrimitiveSet.PLACE_INSIDE, trash), env)
+    print("Finished executing place")
+
+if __name__ == "__main__":
+    main()