feat: input controller (#57)

DOCKER_INSTALL.md

@@ -139,4 +139,4 @@ ENJOY!
 - [Container Installation](https://docs.omniverse.nvidia.com/isaacsim/latest/installation/install_container.html)
 - [NGC Key](https://docs.nvidia.com/ngc/gpu-cloud/ngc-user-guide/index.html#generating-api-key)
 
-*Back to [README](README.md)*
+*Back to [README](README.md)*

exts/StrideSim/StrideSim/anymal_d.py

@@ -111,6 +111,8 @@ async def setup_post_reset(self) -> None:
 
     def on_physics_step(self, step_size) -> None:
         if self._physics_ready:
+            self._base_command[0:2] = self._omni_input.get_linear_velocity()[0:2]
+            self._base_command[2] = self._omni_input.get_angular_velocity()[2]
             self.AnymalD.advance(step_size, self._base_command)
         else:
             self._physics_ready = True

exts/StrideSim/StrideSim/omnigraph_input.py

@@ -1,3 +1,7 @@
+import numpy as np
+import threading
+import time
+
 import carb  # noqa: F401
 import omni.graph.core as og
 
@@ -14,6 +18,14 @@ def __init__(self, prim_paths: dict):
         self._keys = og.Controller.Keys
         self.initialize_omnigraph()
 
+        self._angular_velocity_path = "/action_graph_input/Ros_Twist_sub.outputs:angularVelocity"
+        self._linear_velocity_path = "/action_graph_input/Ros_Twist_sub.outputs:linearVelocity"
+        self._linear_vel = np.zeros(3)
+        self._angular_vel = np.zeros(3)
+
+        self._graph_monitor_thread = threading.Thread(target=self.setup_graph_monitoring)
+        self._graph_monitor_thread.start()
+
     def initialize_prim_paths(self, prim_paths: dict):
         default_paths = {
             "namespace": "AnymalD",
@@ -89,24 +101,16 @@ def connect_nodes(self, connections):
             {self._keys.CONNECT: connections},
         )
 
+    def get_linear_velocity(self):
+        return self._linear_vel
 
-# TODO: 여기서부터 다시 작업~
-# def setup_graph_monitoring():
-#     # Get a handle to the graph
-#     keys = og.Controller.Keys
-#     (graph_handle, _, _, _) = og.Controller.edit({"graph_path": "/action_graph_input"})
-
-#     # Access the output attribute
-#     output_attr = og.Controller.attribute("/action_graph_input/Ros_Twist_sub.outputs:angularVelocity")
+    def get_angular_velocity(self):
+        return self._angular_vel
 
-#     # Define callback
-#     def on_value_change(attr):
-#         new_value = attr.get()
-#         carb.log_info(f"Output value updated: {new_value}")
+    def setup_graph_monitoring(self):
 
-#     # Register callback
-#     output_attr.add_value_changed_fn(on_value_change)
+        while True:
+            self._linear_vel = og.Controller.get(self._linear_velocity_path)
+            self._angular_vel = og.Controller.get(self._angular_velocity_path)
 
-#     # Get the initial value
-#     initial_value = output_attr.get()
-#     carb.log_info(f"Initial value: {initial_value}")
+            time.sleep(0.02)  # 50Hz = 0.02s period