Cleaning up plotting again

Author: Federico-PizarroBejarano

mm_run/nodes/isaac_sim_ros.py

@@ -1,20 +1,20 @@
+import argparse
+import datetime
 import os
 import sys
 
+import numpy as np
+import rospy
 from omni.isaac.kit import SimulationApp
 
+from mm_utils import parsing
+from mm_utils.logging import DataLogger
+
 # URDF import, configuration and simulation sample
 simulation_app = SimulationApp({"headless": False})
 
-import argparse  # noqa: E402
-
-import numpy as np  # noqa: E402
-import rospy  # noqa: E402
 from omni.isaac.core.utils import extensions  # noqa: E402
 
-from mm_utils import parsing  # noqa: E402
-from mm_utils.logging import DataLogger  # noqa: E402
-
 extensions.enable_extension("omni.isaac.ros_bridge")
 from mm_sim_isaac.isaac_sim_env import IsaacSimEnv  # noqa: E402
 from omni.isaac.core.utils.rotations import euler_to_rot_matrix  # noqa: E402
@@ -44,6 +44,13 @@ def main():
 
     rospy.init_node("isaac_sim_ros")
 
+    # Create shared timestamp for logging (format: YYYY-MM-DD_HH-MM-SS)
+    timestamp = datetime.datetime.now()
+    session_timestamp = timestamp.strftime("%Y-%m-%d_%H-%M-%S")
+
+    # Set ROS parameter so control node can use the same timestamp
+    rospy.set_param("/experiment_timestamp", session_timestamp)
+
     sim = IsaacSimEnv(sim_config)
     robot = sim.robot
     world = sim.world
@@ -109,7 +116,7 @@ def main():
         sim.step(render=True)
         sim.publish_ros_topics()
 
-    logger.save()
+    logger.save(session_timestamp=session_timestamp)
     simulation_app.close()
 
 

mm_run/nodes/mpc_ros.py

@@ -108,6 +108,12 @@ def __init__(self):
         self.logger.add("nx", self.ctrl_config["robot"]["dims"]["x"])
         self.logger.add("nu", self.ctrl_config["robot"]["dims"]["u"])
 
+        # Get shared timestamp from ROS parameter (set by sim node)
+        # Wait for sim node to set it
+        while not rospy.has_param("/experiment_timestamp"):
+            rospy.sleep(0.1)
+        self.session_timestamp = rospy.get_param("/experiment_timestamp")
+
         # ROS Related
         self.robot_interface = MobileManipulatorROSInterface()
         self.vicon_tool_interface = ViconObjectInterface(
@@ -186,7 +192,7 @@ def __init__(self):
     def shutdownhook(self):
         self.ctrl_c = True
         self.robot_interface.brake()
-        self.logger.save()
+        self.logger.save(session_timestamp=self.session_timestamp)
 
     def _publish_cmd_vel(self, event):
         if self.mpc_plan is not None:

mm_run/nodes/planner_test_ros.py

@@ -1,4 +1,5 @@
 import argparse
+import datetime
 import logging
 import os
 import sys
@@ -81,6 +82,11 @@ def __init__(self):
 
         # init logger
         self.logger = DataLogger(config.copy(), name="control")
+
+        # Create timestamp for logging
+        timestamp = datetime.datetime.now()
+        self.session_timestamp = timestamp.strftime("%Y-%m-%d_%H-%M-%S")
+
         # ROS Related
         self.robot_interface = MobileManipulatorROSInterface()
         self.vicon_tool_interface = ViconObjectInterface(
@@ -124,7 +130,7 @@ def __init__(self):
     def shutdownhook(self):
         self.ctrl_c = True
         self.robot_interface.brake()
-        self.logger.save()
+        self.logger.save(session_timestamp=self.session_timestamp)
 
     def _publish_planner_data(self, event):
         self.sot_lock.acquire()

mm_run/nodes/sim_ros.py

@@ -64,6 +64,13 @@ def main():
     # initial time, state, input
     t = 0.0
 
+    # Create shared timestamp for logging (format: YYYY-MM-DD_HH-MM-SS)
+    session_timestamp = timestamp.strftime("%Y-%m-%d_%H-%M-%S")
+
+    # Set ROS parameter so control node can use the same timestamp
+    rospy.init_node("sim_ros")
+    rospy.set_param("/experiment_timestamp", session_timestamp)
+
     # init logger
     logger = DataLogger(config, name="sim")
     logger.add("sim_timestep", sim.timestep)
@@ -75,7 +82,6 @@ def main():
     logger.add("nu", sim_config["robot"]["dims"]["u"])
 
     # ros interface
-    rospy.init_node("sim_ros")
     ros_interface = SimulatedMobileManipulatorROSInterface()
     ros_interface.publish_time(t)
 
@@ -123,7 +129,7 @@ def main():
         t, _ = sim.step(t)
         time.sleep(sim.timestep)
 
-    logger.save()
+    logger.save(session_timestamp=session_timestamp)
 
 
 if __name__ == "__main__":

mm_run/scripts/experiment.py

@@ -223,7 +223,8 @@ def main():
 
         time.sleep(sim.timestep)
 
-    logger.save()
+    session_timestamp = timestamp.strftime("%Y-%m-%d_%H-%M-%S")
+    logger.save(session_timestamp=session_timestamp)
 
 
 if __name__ == "__main__":

mm_utils/src/mm_utils/logging.py

@@ -1,4 +1,3 @@
-from datetime import datetime
 from pathlib import Path
 
 import numpy as np
@@ -53,16 +52,16 @@ def append(self, key, value):
             # Start new list
             self.data[key] = [a]
 
-    def save(self):
+    def save(self, session_timestamp):
         """Save the data and configuration to a timestamped directory.
 
         Directory structure:
             <base_directory>/<session_timestamp>/<name>/
                 data.npz
                 config.yaml
+
+        :param session_timestamp: Timestamp string in format "%Y-%m-%d_%H-%M-%S".
         """
-        # Create timestamped directory for this logging session
-        session_timestamp = datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
         dir_path = self.base_directory / session_timestamp / self.name
         dir_path.mkdir(parents=True, exist_ok=True)
 

mm_utils/src/mm_utils/plotting/__init__.py

@@ -8,9 +8,9 @@
 - Utility functions: Logger construction
 """
 
-from mm_utils.plotting.core import DataPlotter, construct_logger
-from mm_utils.plotting.mpc import MPCPlotterMixin
-from mm_utils.plotting.trajectory import TrajectoryPlotterMixin
+from mm_utils.plotting.plot_mpc import MPCPlotterMixin
+from mm_utils.plotting.plot_trajectory import TrajectoryPlotterMixin
+from mm_utils.plotting.plotting_core import DataPlotter, construct_logger
 
 __all__ = [
     "DataPlotter",

mm_utils/src/mm_utils/plotting/mpc.py …_utils/src/mm_utils/plotting/plot_mpc.pymm_utils/src/mm_utils/plotting/mpc.py renamed to mm_utils/src/mm_utils/plotting/plot_mpc.py mm_utils/src/mm_utils/plotting/mpc.py renamed to mm_utils/src/mm_utils/plotting/plot_mpc.py

@@ -16,8 +16,8 @@
 import mm_control.MPC as MPC
 from mm_utils import math, parsing
 from mm_utils.math import wrap_pi_array
-from mm_utils.plotting.mpc import MPCPlotterMixin
-from mm_utils.plotting.trajectory import TrajectoryPlotterMixin
+from mm_utils.plotting.plot_mpc import MPCPlotterMixin
+from mm_utils.plotting.plot_trajectory import TrajectoryPlotterMixin
 
 # =============================================================================
 # UTILITY FUNCTIONS
@@ -227,8 +227,6 @@ def _post_processing(self):
         nq = self.data["nq"]
         qs = self.data["xs"][:, :nq]
 
-        print(self.data["xs"].shape)
-
         # keyed by obstacle names or "self"
         names = ["self", "static_obstacles"]
         params = {"self": [], "static_obstacles": []}
@@ -355,7 +353,6 @@ def _post_processing(self):
         self.data["mpc_ee_predictions"] = []
         self.data["mpc_base_predictions"] = []
 
-        print(self.data["mpc_x_bars"].shape)
         for t_index in range(N):
             x_bar = self.data["mpc_x_bars"][t_index]
             ee_bar, base_bar = self.controller._getEEBaseTrajectories(x_bar)
@@ -493,7 +490,6 @@ def _get_statistics(self):
             "max": run_time_states[1],
             "min": run_time_states[2],
         }
-        print(self.data["statistics"]["constraints_violation"]["num"])
 
     def summary(self, stat_names):
         """get a summary of statistics