From 2fdc4acca832bed55347a77e3101b8f17c8e6e51 Mon Sep 17 00:00:00 2001 From: Jinwon Kim Date: Fri, 25 Oct 2024 21:49:25 +0900 Subject: [PATCH] feat: add omnigraph --- README.md | 13 ++ exts/StrideSim/StrideSim/__init__.py | 3 +- .../StrideSim/anymal_articulation.py | 6 +- exts/StrideSim/StrideSim/anymal_d.py | 10 +- .../assets/.thumbs/256x256/AnymalD.usd.png | 3 + exts/StrideSim/StrideSim/assets/AnymalD.usd | 3 + exts/StrideSim/StrideSim/omnigraph_input.py | 113 ++++++++++++++++++ .../{omnigraph.py => omnigraph_output.py} | 26 ++-- 8 files changed, 159 insertions(+), 18 deletions(-) create mode 100644 exts/StrideSim/StrideSim/assets/.thumbs/256x256/AnymalD.usd.png create mode 100644 exts/StrideSim/StrideSim/assets/AnymalD.usd create mode 100644 exts/StrideSim/StrideSim/omnigraph_input.py rename exts/StrideSim/StrideSim/{omnigraph.py => omnigraph_output.py} (81%) diff --git a/README.md b/README.md index 580f9ac..8588aab 100644 --- a/README.md +++ b/README.md @@ -15,6 +15,19 @@ StrideSim은 Isaac Lab을 기반으로 한 프로젝트입니다. 이 저장소 2. [Isaac Lab v1.0.0](https://github.com/isaac-sim/IsaacLab/tree/v1.0.0) 설치: [설치 가이드](https://isaac-sim.github.io/IsaacLab/source/setup/installation/index.html) 참조 +3. StrideSim 설치 + +```bash +git clone https://github.com/AuTURBO/StrideSim.git +``` + +```bash +sudo apt-get install -y git-lfs +git lfs install + +cd StrideSim +git lfs pull +``` ## 꿀팁 diff --git a/exts/StrideSim/StrideSim/__init__.py b/exts/StrideSim/StrideSim/__init__.py index 0061bcb..c4cf9f7 100644 --- a/exts/StrideSim/StrideSim/__init__.py +++ b/exts/StrideSim/StrideSim/__init__.py @@ -6,4 +6,5 @@ from .anymal_d import AnymalD from .anymal_d_extension import AnyamlDExtension from .base_sample import * -from .omnigraph import ROS2OmniBackend +from .omnigraph_input import ROS2OmniInput +from .omnigraph_output import ROS2OmniOutput diff --git a/exts/StrideSim/StrideSim/anymal_articulation.py b/exts/StrideSim/StrideSim/anymal_articulation.py index 484f342..070047e 100644 --- a/exts/StrideSim/StrideSim/anymal_articulation.py +++ b/exts/StrideSim/StrideSim/anymal_articulation.py @@ -23,8 +23,6 @@ sys.path.append(ISAACLAB_LAB_TASKS) sys.path.append(ISAACLAB_LAB_ASSETS) -from omni.isaac.lab.utils.assets import ISAACLAB_NUCLEUS_DIR - class AnymalD_Atriculation: """The AnymalD quadruped""" @@ -61,7 +59,7 @@ def __init__( if assets_root_path is None: carb.log_error("Could not find Isaac Sim assets folder") - asset_path = f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-D/anymal_d.usd" + asset_path = os.path.join(os.path.dirname(__file__), "assets/AnymalD.usd") prim.GetReferences().AddReference(asset_path) @@ -210,7 +208,7 @@ def initialize(self, physics_sim_view=None) -> None: self.robot.initialize(physics_sim_view=physics_sim_view) self.robot.get_articulation_controller().set_effort_modes("force") self.robot.get_articulation_controller().switch_control_mode("position") - self.robot._articulation_view.set_gains(np.zeros(12) + 60, np.zeros(12) + 1.5) + self.robot._articulation_view.set_gains(np.zeros(12) + 60, np.zeros(12) + 4) def post_reset(self) -> None: """ diff --git a/exts/StrideSim/StrideSim/anymal_d.py b/exts/StrideSim/StrideSim/anymal_d.py index 24e845a..8931e4d 100644 --- a/exts/StrideSim/StrideSim/anymal_d.py +++ b/exts/StrideSim/StrideSim/anymal_d.py @@ -15,7 +15,8 @@ from StrideSim.anymal_articulation import AnymalD_Atriculation from StrideSim.base_sample import BaseSample -# from StrideSim.omnigraph import ROS2OmniBackend +from StrideSim.omnigraph_input import ROS2OmniInput +from StrideSim.omnigraph_output import ROS2OmniOutput from StrideSim.parameters import DEFAULT_WORLD_SETTINGS # , SIMULATION_ENVIRONMENTS @@ -81,10 +82,11 @@ def setup_scene(self) -> None: name="AnymalD", position=np.array([0, 0, 0.8]), ) - + # action graph backend - # self._backend = ROS2OmniBackend({}) - + self._omni_input = ROS2OmniInput({}) + self._omni_output = ROS2OmniOutput({}) + timeline = omni.timeline.get_timeline_interface() self._event_timer_callback = timeline.get_timeline_event_stream().create_subscription_to_pop_by_type( int(omni.timeline.TimelineEventType.STOP), diff --git a/exts/StrideSim/StrideSim/assets/.thumbs/256x256/AnymalD.usd.png b/exts/StrideSim/StrideSim/assets/.thumbs/256x256/AnymalD.usd.png new file mode 100644 index 0000000..80b5173 --- /dev/null +++ b/exts/StrideSim/StrideSim/assets/.thumbs/256x256/AnymalD.usd.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:28639a7859a30b863474045a594d14f4a54967c8fcebda1312d827cd19aa96b9 +size 4212 diff --git a/exts/StrideSim/StrideSim/assets/AnymalD.usd b/exts/StrideSim/StrideSim/assets/AnymalD.usd new file mode 100644 index 0000000..46b5d38 --- /dev/null +++ b/exts/StrideSim/StrideSim/assets/AnymalD.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:82ac7901e11f15d82b75c824c49d96cc361d542aab97e94d9abd1be05bbd079d +size 1122996 diff --git a/exts/StrideSim/StrideSim/omnigraph_input.py b/exts/StrideSim/StrideSim/omnigraph_input.py new file mode 100644 index 0000000..5bab9c8 --- /dev/null +++ b/exts/StrideSim/StrideSim/omnigraph_input.py @@ -0,0 +1,113 @@ +import omni.graph.core as og +import carb + + +class ROS2OmniInput: + def __init__(self, prim_paths: dict): + super().__init__() + self._prim_paths = self.initialize_prim_paths(prim_paths) + (self._omni_graph, _, _, _) = og.Controller.edit( + {"graph_path": "/action_graph_input", "evaluator_name": "execution"}, {} + ) + self._omni_controller = og.Controller(self._omni_graph) + + self._keys = og.Controller.Keys + self.initialize_omnigraph() + + def initialize_prim_paths(self, prim_paths: dict): + default_paths = { + "namespace": "AnymalD", + } + for key, default in default_paths.items(): + if key not in prim_paths: + prim_paths[key] = default + return prim_paths + + def initialize_omnigraph(self): + self.create_general_nodes() + self.create_ros_nodes() + self.set_general_node_values() + self.set_ros_node_values() + self.connect_general_nodes() + self.connect_ros_nodes() + + def create_general_nodes(self): + nodes = [ + ("PTick", "omni.graph.action.OnPlaybackTick"), + ("Namespace_string", "omni.graph.nodes.ConstantString"), + ] + self.create_nodes(nodes) + + def create_ros_nodes(self): + nodes = [ + ("Ros_Context", "omni.isaac.ros2_bridge.ROS2Context"), + ("Ros_Twist_sub", "omni.isaac.ros2_bridge.ROS2SubscribeTwist"), + ] + self.create_nodes(nodes) + + def create_nodes(self, nodes): + self._omni_controller.edit( + self._omni_graph, + {self._keys.CREATE_NODES: nodes}, + ) + + def set_general_node_values(self): + values = [ + ("Namespace_string.inputs:value", self._prim_paths["namespace"]), + ] + self.set_node_values(values) + + def set_ros_node_values(self): + values = [ + ("Ros_Context.inputs:useDomainIDEnvVar", True), + ("Namespace_string.inputs:value", self._prim_paths["namespace"]), + ] + self.set_node_values(values) + + def set_node_values(self, values): + self._omni_controller.edit( + self._omni_graph, + {self._keys.SET_VALUES: values}, + ) + + def connect_general_nodes(self): + connections = [ + ("PTick.outputs:tick", "Ros_Twist_sub.inputs:execIn"), + ] + self.connect_nodes(connections) + + def connect_ros_nodes(self): + connections = [ + ("Ros_Context.outputs:context", "Ros_Twist_sub.inputs:context"), + ("Namespace_string.inputs:value", "Ros_Twist_sub.inputs:nodeNamespace"), + ] + self.connect_nodes(connections) + + def connect_nodes(self, connections): + self._omni_controller.edit( + self._omni_graph, + {self._keys.CONNECT: connections}, + ) + +# 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" + ) + + # Define callback + def on_value_change(attr): + new_value = attr.get() + carb.log_info(f"Output value updated: {new_value}") + + # Register callback + output_attr.add_value_changed_fn(on_value_change) + + # Get the initial value + initial_value = output_attr.get() + carb.log_info(f"Initial value: {initial_value}") diff --git a/exts/StrideSim/StrideSim/omnigraph.py b/exts/StrideSim/StrideSim/omnigraph_output.py similarity index 81% rename from exts/StrideSim/StrideSim/omnigraph.py rename to exts/StrideSim/StrideSim/omnigraph_output.py index fe395b3..769cfd4 100644 --- a/exts/StrideSim/StrideSim/omnigraph.py +++ b/exts/StrideSim/StrideSim/omnigraph_output.py @@ -1,12 +1,12 @@ import omni.graph.core as og -class ROS2OmniBackend(): +class ROS2OmniOutput(): def __init__(self, prim_paths: dict): super().__init__() self._prim_paths = self.initialize_prim_paths(prim_paths) (self._omni_graph, _, _, _) = og.Controller.edit( - {"graph_path": "/action_graph", "evaluator_name": "execution"}, {} + {"graph_path": "/action_graph_output", "evaluator_name": "execution"}, {} ) self._omni_controller = og.Controller(self._omni_graph) @@ -15,9 +15,10 @@ def __init__(self, prim_paths: dict): def initialize_prim_paths(self, prim_paths: dict): default_paths = { - "Lidar_path": "/World/AnymalD/base/lidar_parent/lidar_PhysX", - "Imu_path": "/World/AnymalD/base/imu_link/Imu_Sensor", - "TF_path": "/World/AnymalD", + "Lidar_path": "/World/AnymalD/AnymalD/base/lidar_parent/Lidar", + "Imu_path": "/World/AnymalD/AnymalD/base/imu_link/Imu_Sensor", + "TF_path": "/World/AnymalD/AnymalD/base", + "Joint_states_path": "/World/AnymalD/AnymalD/base", "namespace": "AnymalD", } for key, default in default_paths.items(): @@ -49,6 +50,7 @@ def create_ros_nodes(self): ("Lidar_Read", "omni.isaac.range_sensor.IsaacReadLidarPointCloud"), ("Ros2_Lidar_Pub", "omni.isaac.ros2_bridge.ROS2PublishPointCloud"), ("Ros2_tf_pub", "omni.isaac.ros2_bridge.ROS2PublishTransformTree"), + ("Ros2_Joint_states_pub", "omni.isaac.ros2_bridge.ROS2PublishJointState"), ] self.create_nodes(nodes) @@ -66,15 +68,19 @@ def set_general_node_values(self): def set_ros_node_values(self): values = [ - ("Ros_Context.inputs:useDomainIDEnvVar", False), + ("Ros_Context.inputs:useDomainIDEnvVar", True), ("Imu_Read.inputs:imuPrim", self._prim_paths["Imu_path"]), ("Lidar_Read.inputs:lidarPrim", self._prim_paths["Lidar_path"]), ("Imu_Read.inputs:readGravity", True), - ("Ros2_Imu_Pub.inputs:frameId", "sim_imu"), + ("Ros2_Imu_Pub.inputs:frameId", "imu_link"), ("Ros2_Imu_Pub.inputs:topicName", "imu"), - ("Ros2_Lidar_Pub.inputs:frameId", "sim_lidar"), + ("Ros2_Lidar_Pub.inputs:frameId", "lidar_parent"), ("Ros2_Lidar_Pub.inputs:topicName", "point_cloud"), ("Ros2_tf_pub.inputs:targetPrims", self._prim_paths["TF_path"]), + ( + "Ros2_Joint_states_pub.inputs:targetPrim", + self._prim_paths["Joint_states_path"], + ), ("Namespace_string.inputs:value", self._prim_paths["namespace"]), ] self.set_node_values(values) @@ -90,9 +96,11 @@ def connect_general_nodes(self): ("PTick.outputs:tick", "Imu_Read.inputs:execIn"), ("PTick.outputs:tick", "Lidar_Read.inputs:execIn"), ("PTick.outputs:tick", "Ros2_tf_pub.inputs:execIn"), + ("PTick.outputs:tick", "Ros2_Joint_states_pub.inputs:execIn"), ("Sim_Time.outputs:simulationTime", "Ros2_Imu_Pub.inputs:timeStamp"), ("Sim_Time.outputs:simulationTime", "Ros2_Lidar_Pub.inputs:timeStamp"), ("Sim_Time.outputs:simulationTime", "Ros2_tf_pub.inputs:timeStamp"), + ("Sim_Time.outputs:simulationTime", "Ros2_Joint_states_pub.inputs:timeStamp"), ] self.connect_nodes(connections) @@ -109,7 +117,7 @@ def connect_ros_nodes(self): ("Ros_Context.outputs:context", "Ros2_tf_pub.inputs:context"), ("Namespace_string.inputs:value", "Ros2_Imu_Pub.inputs:nodeNamespace"), ("Namespace_string.inputs:value", "Ros2_Lidar_Pub.inputs:nodeNamespace"), - ("Namespace_string.inputs:value", "Ros2_tf_pub.inputs:nodeNamespace"), + ("Ros_Context.outputs:context", "Ros2_Joint_states_pub.inputs:context"), ] self.connect_nodes(connections)