Feat/streaming receiver

demos/SimulationFramework/pick_and_place.py

@@ -1,10 +1,16 @@
+import argparse
 from alr_sim.sims.SimFactory import SimRepository
 from alr_sim.sims.universal_sim.PrimitiveObjects import Box
 from simpub.sim.sf_publisher import SFPublisher
 
-host = None  # you need to specify the host like "192.168.1.25"
 
 if __name__ == "__main__":
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--host", type=str, default="127.0.0.1")
+    args = parser.parse_args()
+    host = args.host
+
     box1 = Box(
         name="box1",
         init_pos=[0.5, -0.2, 0.5],

demos/SimulationFramework/pick_and_place_with_meta_quest.py

@@ -0,0 +1,105 @@
+import argparse
+import numpy as np
+
+from alr_sim.sims.SimFactory import SimRepository
+from alr_sim.sims.universal_sim.PrimitiveObjects import Box
+from alr_sim.controllers.IKControllers import CartPosQuatImpedenceController
+from simpub.sim.sf_publisher import SFPublisher
+from simpub.xr_device.meta_quest3 import MetaQuest3
+
+
+class MetaQuest3Controller(CartPosQuatImpedenceController):
+
+    def __init__(self, device):
+        super().__init__()
+        self.device: MetaQuest3 = device
+
+    def getControl(self, robot):
+        input_data = self.device.get_input_data()
+        if input_data is not None:
+            desired_pos = input_data.right_pos
+            desired_quat = input_data.right_rot
+            desired_pos_local = robot._localize_cart_pos(desired_pos)
+            desired_quat_local = robot._localize_cart_quat(desired_quat)
+            desired_quat_local = [0, 1, 0, 0]
+            if input_data.right_index_trigger:
+                robot.close_fingers(duration=0.0)
+            else:
+                robot.open_fingers()
+            self.setSetPoint(np.hstack((desired_pos_local, desired_quat_local)))
+        return super().getControl(robot)
+
+
+if __name__ == "__main__":
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--host", type=str, default="127.0.0.1")
+    args = parser.parse_args()
+
+    box1 = Box(
+        name="box1",
+        init_pos=[0.5, -0.2, 0.5],
+        init_quat=[0, 1, 0, 0],
+        rgba=[0.1, 0.25, 0.3, 1],
+    )
+    box2 = Box(
+        name="box2",
+        init_pos=[0.6, -0.1, 0.5],
+        init_quat=[0, 1, 0, 0],
+        rgba=[0.2, 0.3, 0.7, 1],
+    )
+    box3 = Box(
+        name="box3",
+        init_pos=[0.4, -0.1, 0.5],
+        init_quat=[0, 1, 0, 0],
+        rgba=[1, 0, 0, 1],
+    )
+    box4 = Box(
+        name="box4",
+        init_pos=[0.6, -0.0, 0.5],
+        init_quat=[0, 1, 0, 0],
+        rgba=[1, 0, 0, 1],
+    )
+    box5 = Box(
+        name="box5",
+        init_pos=[0.6, 0.1, 0.5],
+        init_quat=[0, 1, 0, 0],
+        rgba=[1, 1, 1, 1],
+    )
+    box6 = Box(
+        name="box6",
+        init_pos=[0.6, 0.2, 0.5],
+        init_quat=[0, 1, 0, 0],
+        rgba=[1, 0, 0, 1],
+    )
+
+    table = Box(
+        name="table0",
+        init_pos=[0.5, 0.0, 0.2],
+        init_quat=[0, 1, 0, 0],
+        size=[0.25, 0.35, 0.2],
+        static=True,
+    )
+
+    object_list = [box1, box2, box3, box4, box5, box6, table]
+
+    # Setup the scene
+    sim_factory = SimRepository.get_factory("mj_beta")
+
+    scene = sim_factory.create_scene(object_list=object_list)
+    robot = sim_factory.create_robot(scene)
+
+    scene.start()
+
+    publisher = SFPublisher(
+        scene, args.host, no_tracked_objects=["table_plane", "table0"]
+    )
+    meta_quest3 = MetaQuest3(publisher)
+    # meta_quest3 = MetaQuest3(publisher, "192.168.0.102")
+    # meta_quest3 = MetaQuest3(publisher, "192.168.0.143")
+    robot_controller = MetaQuest3Controller(meta_quest3)
+    robot_controller.executeController(robot, maxDuration=1000, block=False)
+    publisher.start()
+
+    while True:
+        scene.next_step()

demos/SimulationFramework/ycb_object.py → demos/SimulationFramework/ycb_object_pick_and_place.py

@@ -1,12 +1,16 @@
+import argparse
 from alr_sim.sims.SimFactory import SimRepository
 from alr_sim.sims.mj_beta.mj_utils.mj_scene_object import YCBMujocoObject
 from simpub.sim.sf_publisher import SFPublisher
 
-host = None  # you need to specify the host like "192.168.1.25"
-
 if __name__ == "__main__":
 
-    ycb_base_folder = "/path/to/SF-ObjectDataset/objects/ycb/"
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--host", type=str, default="127.0.0.1")
+    args = parser.parse_args()
+    host = args.host
+
+    ycb_base_folder = "/home/xinkai/project/SF-ObjectDataset/YCB"
     clamp = YCBMujocoObject(
         ycb_base_folder=ycb_base_folder,
         object_id="051_large_clamp",
@@ -18,7 +22,40 @@
         visual_only=False,
     )
 
-    object_list = [clamp]
+    lemon = YCBMujocoObject(
+        ycb_base_folder=ycb_base_folder,
+        object_id="014_lemon",
+        object_name="lemon",
+        pos=[0.4, 0.2, 0.1],
+        quat=[0, 0, 0, 1],
+        static=False,
+        alpha=1.0,
+        visual_only=False,
+    )
+
+    mug = YCBMujocoObject(
+        ycb_base_folder=ycb_base_folder,
+        object_id="025_mug",
+        object_name="mug",
+        pos=[0.2, 0.1, 0.1],
+        quat=[0, 0, 0, 1],
+        static=False,
+        alpha=1.0,
+        visual_only=False,
+    )
+
+    hammer = YCBMujocoObject(
+        ycb_base_folder=ycb_base_folder,
+        object_id="048_hammer",
+        object_name="hammer",
+        pos=[0.3, -0.2, 0.1],
+        quat=[0, 0, 0, 1],
+        static=False,
+        alpha=1.0,
+        visual_only=False,
+    )
+
+    object_list = [clamp, lemon, mug, hammer]
 
     # Setup the scene
     sim_factory = SimRepository.get_factory("mj_beta")
@@ -51,4 +88,5 @@
 
     robot.open_fingers()
 
-    robot.wait(10)
+    while True:
+        scene.next_step()

setup.py

@@ -5,5 +5,5 @@
     version='0.1',
     install_requires=["zmq", "trimesh", "pillow", "numpy", "scipy"],
     include_package_data=True,
-    packages=['simpub', 'simpub.parser', 'simpub.sim', 'simpub.res']
+    packages=['simpub', 'simpub.parser', 'simpub.sim']
 )

simpub/parser/mjcf/asset_loader.py

@@ -24,7 +24,7 @@ def fromBuiltin(
         img: Image.Image
         if builtin_name == "checker":
             img = Image.open(
-                TextureLoader.RES_PATH / "res/image/builtin/checker_grey.png"
+                TextureLoader.RES_PATH / "parser/mjcf/builtin/checker_grey.png"
             ).convert("RGBA")
         elif builtin_name in {"gradient", "flat"}:
             img = Image.new("RGBA", (256, 256), (1, 1, 1, 1))
@@ -39,11 +39,11 @@ def fromBuiltin(
         texture_hash = md5(tex_data).hexdigest()
 
         texture = SimTexture(
-            tag=name,
+            id=name,
             width=width,
             height=height,
-            textype="2d",
-            dataID=texture_hash
+            textureType="2d",
+            dataHash=texture_hash
         )
         return texture, tex_data
 
@@ -63,11 +63,11 @@ def from_bytes(
         texture_hash = md5(tex_data).hexdigest()
 
         texture = SimTexture(
-            tag=name,
+            id=name,
             width=width,
             height=height,
-            textype=texture_type,
-            dataID=texture_hash,
+            textureType=texture_type,
+            dataHash=texture_hash,
         )
 
         return texture, tex_data
@@ -143,22 +143,22 @@ def from_loaded_mesh(
         indices_layout = bin_buffer.tell(), indices.shape[0]
         bin_buffer.write(indices)
         # Texture coords
-        uv_layout = 0, 0
+        uv_layout = (0, 0)
         if hasattr(mesh.visual, "uv"):
             uvs = mesh.visual.uv.astype(np.float32)
             uvs[:, 1] = 1 - uvs[:, 1]
-            uvs = uvs.flatten() 
+            uvs = uvs.flatten()
             uv_layout = bin_buffer.tell(), uvs.shape[0]
 
         bin_data = bin_buffer.getvalue()
         hash = md5(bin_data).hexdigest()
 
         mesh = SimMesh(
-            tag=name,
+            id=name,
             indicesLayout=indices_layout,
             verticesLayout=vertices_layout,
             normalsLayout=normal_layout,
             uvLayout=uv_layout,
-            dataID=hash
+            dataHash=hash
         )
         return mesh, bin_data

simpub/parser/mjcf/mjcf_parser.py

@@ -177,7 +177,7 @@ def _load_assets(self, xml: XMLNode, mj_scene: MJCFScene) -> None:
                         scale,
                     )
                     mj_scene.meshes.append(mesh)
-                    mj_scene.raw_data[mesh.dataID] = bin_data
+                    mj_scene.raw_data[mesh.dataHash] = bin_data
 
                 elif asset.tag == "material":
                     color = str2list(
@@ -186,7 +186,7 @@ def _load_assets(self, xml: XMLNode, mj_scene: MJCFScene) -> None:
                     emission = float(asset.get("emission", "0.0"))
                     emissionColor = [emission * c for c in color]
                     material = SimMaterial(
-                        tag=asset.get("name") or asset.get("type"),
+                        id=asset.get("name") or asset.get("type"),
                         color=color,
                         emissionColor=emissionColor,
                         specular=float(asset.get("specular", "0.5")),
@@ -210,12 +210,12 @@ def _load_assets(self, xml: XMLNode, mj_scene: MJCFScene) -> None:
                         asset_file = pjoin(
                             self._texturedir, asset.attrib["file"]
                         )
-                        byte_data = Path(asset_file, "rb").read_bytes()
+                        byte_data = Path(asset_file).read_bytes()
                         texture, bin_data = TextureLoader.from_bytes(
                             name, byte_data, asset.get("type", "cube"), tint
                         )
                     mj_scene.textures.append(texture)
-                    mj_scene.raw_data[texture.dataID] = bin_data
+                    mj_scene.raw_data[texture.dataHash] = bin_data
                 else:
                     raise RuntimeError("Invalid asset", asset.tag)
 
@@ -263,6 +263,9 @@ def _load_body(
 
         visuals: List[SimVisual] = list()
         for geom in body.findall("geom"):
+            # geom group 3 is for collision
+            if int(geom.get("group", "2")) == 3:
+                continue
             visual = self._load_visual(geom)
             if visual is not None:
                 visuals.append(visual)

simpub/server.py

@@ -103,12 +103,47 @@ def execute(self):
                 "updateData": self._update_func(),
                 "time": time.monotonic()
             }
-            self.pub_socket.send_string(f"{json.dumps(msg)}")
+            self.pub_socket.send_string(f"{self._topic}:{json.dumps(msg)}")
 
     def on_shutdown(self):
         self.pub_socket.close(0)
 
 
+class SubscribeTask(TaskBase):
+
+    def __init__(
+        self,
+        context: zmq.Context,
+        addr: str,
+        port: int,
+    ):
+        self._context: zmq.Context = context
+        self.running: bool = False
+        self.sub_socket: zmq.Socket = self._context.socket(zmq.SUB)
+        self.sub_socket.connect(f"tcp://{addr}:{port}")
+        self._callback_func_list: Dict[str, Callable[[str], None]] = {}
+        self.sub_socket.setsockopt_string(zmq.SUBSCRIBE, "")
+
+    def execute(self):
+        print("Subscribe task has been started")
+        self.running = True
+        while self.running:
+            message = self.sub_socket.recv_string()
+            topic, msg = message.split(":", 1)
+            if topic in self._callback_func_list:
+                self._callback_func_list[topic](msg)
+
+    def register_callback(
+        self,
+        topic: str,
+        callback_func: Callable[[str], None]
+    ) -> None:
+        self._callback_func_list[topic] = callback_func
+
+    def on_shutdown(self):
+        self.sub_socket.close(0)
+
+
 class MsgService(TaskBase):
 
     def __init__(
@@ -188,6 +223,9 @@ def shutdown(self):
         self.thread.join()
         print("All the threads have been stopped")
 
+    def add_task(self, task: TaskBase):
+        self.tasks.append(task)
+
 
 class MsgServer(ServerBase):
     def __init__(self, host: str = "127.0.0.1"):
@@ -199,7 +237,7 @@ def initialize_task(self):
             "SERVICE": PortSet.SERVICE,
         }
         time_stamp = str(time.time())
-        discovery_message = f"SimPub:{time_stamp}:{json.dumps(discovery_data)}"
+        discovery_message = f"HDAR:{time_stamp}:{json.dumps(discovery_data)}"
         self.broadcast_task = BroadcastTask(discovery_message, self.host)
         self.tasks.append(self.broadcast_task)
 
@@ -236,17 +274,17 @@ def initialize_task(self):
         time_stamp = str(time.time())
         discovery_message = f"SimPub:{time_stamp}:{json.dumps(discovery_data)}"
         self.broadcast_task = BroadcastTask(discovery_message, self.host)
-        self.tasks.append(self.broadcast_task)
+        self.add_task(self.broadcast_task)
 
         self.stream_task = StreamTask(
             self.zmqContext, self.get_update, self.host
         )
-        self.tasks.append(self.stream_task)
+        self.add_task(self.stream_task)
 
         self.msg_service = MsgService(self.zmqContext, self.host)
         self.msg_service.register_action("SCENE", self._on_scene_request)
         self.msg_service.register_action("ASSET", self._on_asset_request)
-        self.tasks.append(self.msg_service)
+        self.add_task(self.msg_service)
 
     def _on_scene_request(self, socket: zmq.Socket, tag: str):
         socket.send_string(self.sim_scene.to_string())