Merge pull request #32 from intuitive-robots/pre/1.3.1

Pre/1.3.1

demos/SimulationFramework/pick_and_place.py

@@ -3,13 +3,11 @@
 from alr_sim.sims.SimFactory import SimRepository
 from alr_sim.sims.universal_sim.PrimitiveObjects import Box
 from simpub.sim.sf_publisher import SFPublisher
-from simpub.xr_device import XRDevice
 
 if __name__ == "__main__":
 
     parser = argparse.ArgumentParser()
     parser.add_argument("--host", type=str, default="127.0.0.1")
-    parser.add_argument("--device", type=str, default="UnityClient")
     args = parser.parse_args()
     host = args.host
 
@@ -70,9 +68,9 @@
 
     assert host is not None, "Please specify the host"
     publisher = SFPublisher(
-        scene, host, no_tracked_objects=["table_plane", "table0"]
+        scene,
+        host, no_tracked_objects=["table_plane", "table0"],
     )
-    device = XRDevice(args.device)
 
     duration = 2
 

demos/SimulationFramework/pick_and_place_with_meta_quest.py

@@ -98,7 +98,7 @@ def getControl(self, robot: MjRobot):
     publisher = SFPublisher(
         scene, args.host, no_tracked_objects=["table_plane", "table0"]
     )
-    meta_quest3 = MetaQuest3("UnityClient")
+    meta_quest3 = MetaQuest3("IRLMQ3-1")
     robot_controller = MetaQuest3Controller(meta_quest3)
     robot_controller.executeController(robot, maxDuration=1000, block=False)
 

demos/SimulationFramework/pick_ycb_object.py

@@ -11,11 +11,12 @@
 
     argparse.ArgumentParser()
     parser = argparse.ArgumentParser()
-    parser.add_argument("--ycb_base_folder", type=str)
+    parser.add_argument("--folder", type=str)
+    parser.add_argument("--host", type=str, default="127.0.0.1")
     parser.add_argument("--object_id", type=str, default="013_apple")
     args = parser.parse_args()
 
-    ycb_base_folder = os.path.join(args.ycb_base_folder, "SF-ObjectDataset/YCB")
+    ycb_base_folder = os.path.join(args.folder, "SF-ObjectDataset/YCB")
     clamp = YCBMujocoObject(
         ycb_base_folder=ycb_base_folder,
         object_id="051_large_clamp",
@@ -70,7 +71,7 @@
     robot = sim_factory.create_robot(scene, dt=0.0005)
     scene.start()
 
-    publisher = SFPublisher(scene)
+    publisher = SFPublisher(scene, args.host)
 
     robot.set_desired_gripper_width(0.4)  # we set the gripper to clos at the beginning
 

demos/libero.py → demos/libero/libero_example.py

@@ -20,7 +20,7 @@ def __init__(self, env: RobosuiteEnv):
         super().__init__(
             env.sim.model._model,
             env.sim.data._data,
-            visible_geoms_groups=[1, 2, 3, 4]
+            # visible_geoms_groups=[1, 2, 3, 4]
         )
 
 
@@ -34,7 +34,7 @@ def __init__(self, env: RobosuiteEnv):
         "controller_configs": controller_config,
     }
 
-    bddl_path = "/home//LIBERO/libero/libero/bddl_files/"
+    bddl_path = "/home/xinkai/repository/LIBERO/libero/libero/bddl_files/"
     bddl_dataset_name = "libero_10"
     bddl_name = "LIVING_ROOM_SCENE2_put_both_the_cream_cheese_box_and_the_butter_in_the_basket.bddl"
     bddl_file = os.path.join(bddl_path, bddl_dataset_name, bddl_name)

demos/metaworld/metaworld_example.py

@@ -0,0 +1,42 @@
+import argparse
+import random
+import metaworld
+from simpub.sim.mj_publisher import MujocoPublisher
+
+
+def main():
+    # Arguments
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--host", type=str, default="127.0.0.1")
+    parser.add_argument("--env_name", type=str, default="basketball-v2")
+    args = parser.parse_args()
+
+    # Initialize MetaWorld Benchmark
+    ml1 = metaworld.ML1(args.env_name)
+
+    # # Create Environment
+    if args.env_name not in ml1.train_classes:
+        raise ValueError(
+            f"Environment '{args.env_name}' is not available."
+            f"Available environments: {list(ml1.train_classes.keys())}")
+
+    env = ml1.train_classes[args.env_name]()
+    task = random.choice(ml1.train_tasks)
+    env.set_task(task)
+
+    # Initialize MujocoPublisher
+    MujocoPublisher(
+        env.model,
+        env.data,
+        host=args.host,
+        visible_geoms_groups=list(range(3))
+    )
+
+    # Main Loop
+    env.reset()
+    while True:
+        continue
+
+
+if __name__ == "__main__":
+    main()

demos/mujoco/table_tennis_two_players/assets/bats.xml

@@ -1,12 +1,12 @@
 <mujocoinclude>
     <body name="bat1" pos="1 0 1" childclass="contact_geom">
-        <geom name="bat1" type="cylinder" size="0.075 0.005" rgba="1 0 0 1" pos="-0.05 0 0" quat="0 0 1 0"/>
+        <geom name="bat1" type="cylinder" size="0.075 0.005" rgba="1 0 0 1" pos="0.05 0 0.03" quat="0.3826834 0 0.9238795 0"/>
         <!-- <geom name="bat1_back" type="cylinder" size="0.075 0.0025" rgba="0 1 0 1" quat="0 0 1 0" pos="0 -0.0026 0"/> -->
-        <geom name="handle1" type="cylinder" size="0.015 0.025" rgba="0.6 0.3 0 1" pos="0 0 0" quat="0.71 0 0.71 0"/>
+        <!-- <geom name="handle1" type="cylinder" size="0.015 0.025" rgba="0.6 0.3 0 1" pos="0 0 0" quat="0.71 0 0.71 0"/> -->
     </body>
     <body name="bat2" pos="-1 0 1" childclass="contact_geom">
-        <geom name="bat2" type="cylinder" size="0.075 0.005" rgba="1 0 0 1" quat="0.71 0 0.71 0"/>
-        <geom name="bat2_back" type="cylinder" size="0.075 0.0025" rgba="0 1 0 1" quat="0.71 0 0.71 0" pos="-0.0026 0 0"/>
-        <geom name="handle2" type="cylinder" size="0.015 0.025" rgba="0.6 0.3 0 1" pos="0 0 0.05" quat="1 0 0 0"/>
+        <geom name="bat2" type="cylinder" size="0.075 0.005" rgba="1 0 0 1" pos="0.05 0 0.03" quat="0.3826834 0 0.9238795 0"/>
+        <!-- <geom name="bat2_back" type="cylinder" size="0.075 0.0025" rgba="0 1 0 1" quat="0.71 0 0.71 0" pos="-0.0026 0 0"/>
+        <geom name="handle2" type="cylinder" size="0.015 0.025" rgba="0.6 0.3 0 1" pos="0 0 0.05" quat="1 0 0 0"/> -->
     </body>
 </mujocoinclude>

demos/mujoco/table_tennis_two_players/table_tennis.py

@@ -1,7 +1,11 @@
 import mujoco
-from mujoco import mj_name2id, mjtObj
+from mujoco import mj_name2id, mjtObj  # type: ignore
 import numpy as np
 import time
+import os
+import argparse
+from typing import Optional, Union, Dict, Callable
+
 from simpub.sim.mj_publisher import MujocoPublisher
 from simpub.xr_device.meta_quest3 import MetaQuest3
 
@@ -16,38 +20,52 @@ def check_episode_and_rest(mj_model, mj_data):
         mj_data.qvel[qposadr:qposadr + 3] = np.array([0, 0, 0])
 
 
-def update_bat(mj_model, mj_data, player1: MetaQuest3, player2: MetaQuest3 = None):
+def update_bat(mj_model, mj_data, player1: MetaQuest3, player2: Optional[MetaQuest3] = None):
     bat1_id = mj_name2id(model, mjtObj.mjOBJ_BODY, "bat1")
     player1_input = player1.get_input_data()
-    # print(player1_input)
     if player1_input is None:
         return
     mj_model.body_pos[bat1_id] = np.array(player1_input["right"]["pos"])
     quat = player1_input["right"]["rot"]
     mj_model.body_quat[bat1_id] = np.array([quat[3], quat[0], quat[1], quat[2]])
-    # bat2_id = mj_name2id(model, mjtObj.mjOBJ_BODY, "bat2")
-    # mj_data.body_pos[bat2_id] = np.array(player1.input_data["left"]["pos"])
-    # mj_data.body_quat[bat2_id] = np.array(player1.input_data["left"]["rot"])
+    if player2 is not None:
+        bat2_id = mj_name2id(model, mjtObj.mjOBJ_BODY, "bat2")
+        player2_input = player2.get_input_data()
+        if player2_input is None:
+            return
+        mj_data.body_pos[bat2_id] = np.array(player2_input["left"]["pos"])
+        quat = player2_input["left"]["rot"]
+        mj_data.body_quat[bat2_id] = np.array([quat[3], quat[0], quat[1], quat[2]])
 
 
 if __name__ == '__main__':
-
-    model = mujoco.MjModel.from_xml_path("assets/table_tennis_env.xml")
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--host", type=str, default="127.0.0.1")
+    args = parser.parse_args()
+
+    xml_path = os.path.join(
+        os.path.dirname(os.path.abspath(__file__)),
+        "assets/table_tennis_env.xml"
+    )
+    model = mujoco.MjModel.from_xml_path(xml_path)
     data = mujoco.MjData(model)
     last_time = time.time()
-    publisher = MujocoPublisher(model, data, host="192.168.0.134")
-    player1 = MetaQuest3("UnityClient")
-    # player2 = MetaQuest3("ALR2")
-    while not player1.connected:
-        time.sleep(0.01)
-    print("Connected to UnityClient")
+    publisher = MujocoPublisher(model, data, args.host)
+    player1 = MetaQuest3("ALRMQ3-2")
+    # # player2 = MetaQuest3("ALR2")
+    # while not player1.connected:
+    #     time.sleep(0.01)
     count = 0
     while True:
-        mujoco.mj_step(model, data)
-        if time.time() - last_time < 0.001:
-            time.sleep(0.001 - (time.time() - last_time))
-        last_time = time.time()
-        if count % 10 == 0:
-            update_bat(model, data, player1)
-            check_episode_and_rest(model, data)
-        count += 1
+        try:
+            mujoco.mj_step(model, data)
+            if time.time() - last_time < 0.001:
+                time.sleep(0.001 - (time.time() - last_time))
+            last_time = time.time()
+            if count % 10 == 0:
+                update_bat(model, data, player1)
+                check_episode_and_rest(model, data)
+            count += 1
+        except KeyboardInterrupt:
+            break
+    publisher.shutdown()

demos/robocasa/robocasa_example.py

@@ -0,0 +1,51 @@
+import argparse
+
+import numpy as np
+import robosuite
+from robosuite.controllers import load_composite_controller_config
+from robosuite.wrappers import VisualizationWrapper
+from termcolor import colored
+import robocasa.models.scenes.scene_registry
+from termcolor import colored
+
+from simpub.sim.robocasa_publisher import RobocasaPublisher
+
+if __name__ == "__main__":
+    # Arguments
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--host", type=str, default="127.0.0.1")
+    parser.add_argument("--layout", type=int, default=0)
+    parser.add_argument("--style", type=int, default=0)
+    parser.add_argument("--robot", type=str, default="PandaOmron")
+    args = parser.parse_args()
+
+    # Create argument configuration
+    config = {
+        "env_name": "PnPCounterToCab",
+        "robots": "PandaOmron",
+        "controller_configs": load_composite_controller_config(robot=args.robot),
+        "translucent_robot": False,
+    }
+    print(colored("Initializing environment...", "yellow"))
+    env = robosuite.make(
+        **config,
+        has_renderer=True,
+        has_offscreen_renderer=False,
+        render_camera=None,
+        ignore_done=True,
+        use_camera_obs=False,
+        control_freq=20,
+        renderer="mjviewer",
+    )
+
+    # Grab reference to controller config and convert it to json-encoded string
+
+    env.layout_and_style_ids = [[args.layout, args.style]]
+    env.reset()
+    env.render()
+
+    publisher = RobocasaPublisher(env, args.host)
+
+    while True:
+        obs, _, _, _ = env.step(np.zeros(env.action_dim))
+        env.render()