Updated luminances

.gitignore

@@ -33,4 +33,8 @@ __pycache__
 
 # No wandb outputs or runs
 **/wandb/
-**/runs/
+**/runs/
+
+mirage/mirage/ros_ws/build/*
+mirage/mirage/ros_ws/install/*
+mirage/mirage/ros_ws/log/*

README.md

@@ -36,7 +36,6 @@ conda config --env --add channels robostack-staging
 conda config --env --remove channels defaults
 
 mamba install ros-humble-desktop
-mamba install ros-humble-gazebo-ros
 
 # Need to reactivate the environment for changes to take effect
 mamba deactivate
@@ -62,10 +61,33 @@ source install/setup.bash
 ```
 
 ## Usage
+### Robosuite Benchmark
+If inpainting is enabled, only 1 benchmarking process can be run at a given time.
+Depending on the robosuite environment, the ros launch file will be different due to different camera extrinsics.
+Firstly, in one terminal, run the gazebo writer node
+```
+source mirage/mirage/ros_ws/install/setup.bash
+ros2 run gazebo_env write_data_node_robosuite_better.py
+```
+
+Then, launch the gazebo process for the corresponding environment by first running (in a new terminal):
+```
+source mirage/mirage/ros_ws/install/setup.bash
+```
+Run either one of these depending on the environment (three piece assembly is really two piece assembly).
+```
+ros2 launch gazebo_env panda_gazebo_classic_robosuite_can.launch.py
+ros2 launch gazebo_env panda_gazebo_classic_robosuite_lift_square_stack_three_threading.launch.py
+ros2 launch gazebo_env panda_gazebo_classic_robosuite_three_piece_assembly.launch.py
+```
+
 For robosuite, to run an experiment (from the root of this repo), 
 ```
+# Run this command below if inpainting, otherwise skip
+source mirage/mirage/ros_ws/install/setup.bash
+
 cd mirage/mirage/benchmark/robosuite
-python3 run_robosuite_benchmark.py --config_file config/example_config.yaml
+python3 run_robosuite_benchmark.py --config config/example_config.yaml
 ```
 Please take a look at the example_config and the different parameters that can be set to run different tasks, agents, and robots. For the above code to work, you must change the agents to the path for the model checkpoints in robosuite.
 

mirage/mirage/benchmark/robosuite/evaluate_policy_demo_source_robot_server.py

@@ -155,6 +155,7 @@ def __init__(self, robot_name=None, ckpt_path=None, render=False, video_path=Non
                 self.rollout_horizon = config.experiment.rollout.horizon
 
         self.core_env = self.env.env
+
         self.is_diffusion = False
         while hasattr(self.core_env, "env"):
             self.is_diffusion = True

mirage/mirage/benchmark/robosuite/evaluate_policy_demo_target_robot_client.py

@@ -52,6 +52,17 @@ def __init__(self, robot_name=None, ckpt_path=None, render=False, video_path=Non
                         else:
                             print("NOT USING ROS! Will use the groundtruth Franka masks instead")                        
 
+        self.should_kill_early = False
+
+        import threading
+        self.thread = threading.Thread(target=self.wait_for_skip)
+        self.thread.start()
+
+    def wait_for_skip(self):
+        while True:
+            input()
+            self.should_kill_early = True
+
     def image_to_pointcloud(self, depth_map, camera_name, camera_height=84, camera_width=84, segmask=None):
         """
         Convert depth image to point cloud
@@ -340,6 +351,11 @@ def rollout_robot(self, video_skip=5, return_obs=False, camera_names=None, set_o
                 if step_i - source_finished_step >= 10:
                     done = True
 
+            # If human hits enter, kill off the run, it isn't feasible
+            if self.should_kill_early:
+                self.should_kill_early = False
+                done = True
+
             # tell the source robot that the target robot is ready
             # Create an instance of Data() to send to client.
             variable = Data()

mirage/mirage/benchmark/robosuite/robosuite_experiment.py

@@ -98,7 +98,6 @@ def launch(self, override=False) -> None:
             target_agent_args.append("--inpaint_enabled")
 
             if self._config.use_ros:
-                source_agent_args.append("--use_ros")
                 target_agent_args.append("--use_ros")
 
             if self._config.offline_eval:

mirage/mirage/gripper_interpolation/robosuite/gripper_interpolator.py

@@ -50,8 +50,12 @@ def interpolate_gripper(self, gripper_angles: np.array) -> np.array:
         """
         if self.source_robot == self.target_robot:
             print("Returning angles")
-            return 1 * gripper_angles
+            return -1 * gripper_angles
 
+        if (self.source_robot == "Panda" or self.source_robot == "Kinova3") and self.target_robot == "UR5e": 
+        # TODO: add this for real gripper mapping
+            return -0.05702400673569841 * gripper_angles + 0.02670973458948458
+
         relevant_interpoolator = self.interpolators[(self.source_robot, self.target_robot)]
         regression_model = LinearRegression()
 

mirage/mirage/infra/ros_inpaint_publisher_sim.py

@@ -1,6 +1,6 @@
 from input_filenames_msg.msg import InputFilesSimData
 from mirage.infra.ros_inpaint_publisher import ROSInpaintPublisher
-from mirage.mirage.gripper_interpolation.gripper_interpolator import GripperInterpolator
+from mirage.gripper_interpolation.robosuite.gripper_interpolator import GripperInterpolator
 import numpy as np
 
 class ROSInpaintSimData:
@@ -30,7 +30,7 @@ class ROSInpaintPublisherSim(ROSInpaintPublisher):
     to a node that performs inpainting on a target robot.
     """
 
-    def __init__(self, source_robot_info: str, target_robot_info: str):
+    def __init__(self):
         """
         Initializes the ROS2 node.
         :param source_robot_info: the information about the source robot to determine which interpolation scheme to use
@@ -41,11 +41,8 @@ def __init__(self, source_robot_info: str, target_robot_info: str):
         self._publisher = self.node.create_publisher(
             InputFilesSimData, '/input_files_data_sim', 1)
 
-        # TODO: generalize this
-        self.gripper_interpolator = GripperInterpolator(source_robot_info, target_robot_info, ['/home/mirage/x-embody/xembody/xembody_robosuite/paired_trajectories_collection/gripper_interpolation_results_no_task_diff.pkl',
-                                                                                               '/home/mirage/x-embody/xembody/xembody_robosuite/paired_trajectories_collection/gripper_interpolation_results_20_rollouts.pkl'])
-        # self.gripper_interpolator = GripperInterpolator('panda', 'panda', ['/home/mirage/x-embody/xembody/xembody_robosuite/paired_trajectories_collection/gripper_interpolation_results_no_task_diff.pkl'])
-        # self.gripper_interpolator = GripperInterpolator('panda', 'ur5', '/home/mirage/x-embody/xembody/xembody_robosuite/paired_trajectories_collection/gripper_interpolation_results_no_task_diff.pkl')
+        # TODO(kdharmarajandev): generalize this
+        self.gripper_interpolator = GripperInterpolator('Panda', 'Panda')
 
     def publish_to_ros_node(self, data: ROSInpaintSimData):
         """

mirage/mirage/ros_ws/src/gazebo_env/bringup/launch/panda_gazebo_classic_robosuite_can.launch.py

@@ -24,9 +24,27 @@
 )
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
-
+import os
 
 def generate_launch_description():
+
+    # Set the path to the Gazebo ROS package
+    pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')   
+
+    # Set the path to this package.
+    pkg_share = FindPackageShare(package='gazebo_env').find('gazebo_env')
+
+    # Set the path to the world file
+    world_file_name = 'no_shadow_sim.world'
+    world_path = os.path.join(pkg_share, 'worlds', world_file_name)
+
+    world = LaunchConfiguration('world')
+
+    declare_world_cmd = DeclareLaunchArgument(
+        name='world',
+        default_value=world_path,
+        description='Full path to the world model file to load')
+
     # Declare arguments
     declared_arguments = []
     declared_arguments.append(
@@ -37,15 +55,14 @@ def generate_launch_description():
         )
     )
 
+    declared_arguments.append(declare_world_cmd)
+
+    # Initialize Arguments
+    gui = LaunchConfiguration("gui")
+
     gazebo_server = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [
-                PathJoinSubstitution(
-                    [FindPackageShare("gazebo_ros"), "launch", "gzserver.launch.py"]
-                )
-            ]
-        )
-    )
+    PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
+    launch_arguments={'world': world}.items())
     gazebo_client = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             [
@@ -93,12 +110,36 @@ def generate_launch_description():
         output="screen",
     )
 
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "joint_state_broadcaster",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
+
     joint_state_publisher_node = Node(
         package="gazebo_env",
         executable="full_panda_joint_state_publisher_node.py",
         output="screen",
     )
 
+    image_sub_node = Node(
+        package="gazebo_env", executable="gazebo_image_test.py", output="screen"
+    )
+
+    robot_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "forward_position_controller",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
+
     nodes = [
         gazebo_server,
         gazebo_client,

mirage/mirage/ros_ws/src/gazebo_env/bringup/launch/panda_gazebo_classic_robosuite_lift_square_stack_three_threading.launch.py

@@ -24,9 +24,26 @@
 )
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
-
+import os
 
 def generate_launch_description():
+    # Set the path to the Gazebo ROS package
+    pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')   
+
+    # Set the path to this package.
+    pkg_share = FindPackageShare(package='gazebo_env').find('gazebo_env')
+
+    # Set the path to the world file
+    world_file_name = 'no_shadow_sim.world'
+    world_path = os.path.join(pkg_share, 'worlds', world_file_name)
+
+    world = LaunchConfiguration('world')
+
+    declare_world_cmd = DeclareLaunchArgument(
+        name='world',
+        default_value=world_path,
+        description='Full path to the world model file to load')
+
     # Declare arguments
     declared_arguments = []
     declared_arguments.append(
@@ -37,15 +54,14 @@ def generate_launch_description():
         )
     )
 
+    declared_arguments.append(declare_world_cmd)
+
+    # Initialize Arguments
+    gui = LaunchConfiguration("gui")
+
     gazebo_server = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [
-                PathJoinSubstitution(
-                    [FindPackageShare("gazebo_ros"), "launch", "gzserver.launch.py"]
-                )
-            ]
-        )
-    )
+    PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
+    launch_arguments={'world': world}.items())
     gazebo_client = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             [
@@ -93,12 +109,36 @@ def generate_launch_description():
         output="screen",
     )
 
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "joint_state_broadcaster",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
+
     joint_state_publisher_node = Node(
         package="gazebo_env",
         executable="full_panda_joint_state_publisher_node.py",
         output="screen",
     )
 
+    image_sub_node = Node(
+        package="gazebo_env", executable="gazebo_image_test.py", output="screen"
+    )
+
+    robot_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "forward_position_controller",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
+
     nodes = [
         gazebo_server,
         gazebo_client,