diff --git a/mobiman_simulation/config/config_mobiman_framework.yaml b/mobiman_simulation/config/config_mobiman_framework.yaml
index aca33a9..a36d155 100644
--- a/mobiman_simulation/config/config_mobiman_framework.yaml
+++ b/mobiman_simulation/config/config_mobiman_framework.yaml
@@ -14,7 +14,7 @@ flag_moveit: false
 
 ### Rviz:
 flag_rviz: true
-rviz_config_path: "rviz/mobile_manipulator_ocs2_igibson"
+rviz_config_path: "rviz/igibson/mobile_manipulator_ocs2_igibson"
 
 ### Map Server:
 flag_map_server: true
@@ -34,5 +34,5 @@ collision_points_config_path: "config/config_points_on_robot"
 flag_goal_server: true
 
 ### Mobiman:
-flag_mobiman: false
+flag_mobiman: true
 task_config_path: config/task/task_jackalJaco_igibson.info
\ No newline at end of file
diff --git a/mobiman_simulation/config/task/task_jackalJaco_igibson.info b/mobiman_simulation/config/task/task_jackalJaco_igibson.info
index 3b55e8b..593ee90 100755
--- a/mobiman_simulation/config/task/task_jackalJaco_igibson.info
+++ b/mobiman_simulation/config/task/task_jackalJaco_igibson.info
@@ -3,7 +3,6 @@ model_information
 {             
   envMode            0                ; Mode of the env, 0: sim, 1: exp
   sim                "igibson"        ; Simulator, Default: "gazebo", Others: "igibson" 
-  ns                 "jackalJaco_0"   ; Namespace 
   robotName          "mobiman"        ; Name of the robot in simulation
   robotModelType     2                ; 0: Mobile base, 1: Arm, 2: Mobile manipulator
   
@@ -54,21 +53,21 @@ model_information
 
   ;baseStateMsg                    "/gazebo/link_states"
   ;armStateMsg                     "/arm_controller/state"
-  armStateMsg                     "/jackalJaco_0/gibson_ros/joint_states"
+  armStateMsg                     "/gibson_ros/joint_states"
   baseControlMsg                  "/mobile_base_controller/cmd_vel"
   armControlMsg                   "/arm_controller/cmd_pos"
   selfCollisionMsg                "/self_collision_distances"
   occupancyDistanceBaseMsg        "/occupancy_distances_base"
   occupancyDistanceArmMsg         "/occupancy_distances_arm"
-  pointsOnRobotMsg                "points_on_robot"
+  pointsOnRobotMsg                "/points_on_robot"
   octomapMsg                      "/octomap_scan"
   modelModeMsg                    "/mobile_manipulator_model_mode"
   mpcTargetMsg                    "/mobile_manipulator_mpc_target"
   targetMsg                       "/mobile_manipulator_target"
-  goalFrame                       "grasp"
+  goalFrame                       "/grasp"
 
-  collisionConstraintPoints       "collision_constraint_points"
-  collisionCheckPoints            "collision_check_points"
+  collisionConstraintPoints       "/collision_constraint_points"
+  collisionCheckPoints            "/collision_check_points"
   logSavePathRel                  "dataset/ocs2/" ; If empty, not saved. Ex: "dataset/ocs2/"
 }
 
diff --git a/mobiman_simulation/include/mobiman_simulation/scan_utility.h b/mobiman_simulation/include/mobiman_simulation/scan_utility.h
index d9a367f..69f417b 100755
--- a/mobiman_simulation/include/mobiman_simulation/scan_utility.h
+++ b/mobiman_simulation/include/mobiman_simulation/scan_utility.h
@@ -155,6 +155,8 @@ class ScanUtility
 
     tf::TransformListener* tflistener_;
 
+    bool printOutFlag_ = false;
+
     string obj_name_;
     string pkg_dir_;
     string data_dir_;
diff --git a/mobiman_simulation/launch/utilities/map_server.launch b/mobiman_simulation/launch/utilities/map_server.launch
index 34e879f..296ab81 100644
--- a/mobiman_simulation/launch/utilities/map_server.launch
+++ b/mobiman_simulation/launch/utilities/map_server.launch
@@ -5,9 +5,9 @@
   <!-- CONFIGURATION OPTIONS
     config_map_server
   -->
-  <arg name="robot_ns"            default="robotName_0"/>
   <arg name="config_map_server"  default="config_map_server" />
-
+  <arg name="robot_ns"            default="robotName_0"/>
+  
   <group ns="$(arg robot_ns)">
     <!-- PUBLISH CONFIGURATION PARAMETERS -->
     <rosparam file="$(find mobiman_simulation)/config/$(arg config_map_server).yaml"/>
diff --git a/mobiman_simulation/launch/utilities/ocs2_m4.launch b/mobiman_simulation/launch/utilities/ocs2_m4.launch
index e467934..71d9d1b 100644
--- a/mobiman_simulation/launch/utilities/ocs2_m4.launch
+++ b/mobiman_simulation/launch/utilities/ocs2_m4.launch
@@ -32,18 +32,19 @@
       <arg name="lib_path"                default="codegen/auto_generated/$(arg robot)" />
       <param name="libFolder"             value="$(find mobiman_simulation)/$(arg lib_path)" />
 
-
       <arg name="collision_points_config_path"  default="config/config_points_on_robot" />
       <rosparam file="$(find mobiman_simulation)/$(arg collision_points_config_path).yaml"/>
 
-      <node pkg="ocs2_mobile_manipulator_ros" 
-            type="mobile_manipulator_node" 
-            name="mobile_manipulator_node" 
-            output="screen"
-            launch-prefix="" />
-
-      <!-- Run Rviz visualization -->
-      <group if="$(eval arg('rviz') == true)">
-            <include file="$(find mobiman_simulation)/launch/utilities/rviz.launch" />
+      <group ns="$(arg robot_ns)">
+            <node pkg="ocs2_mobile_manipulator_ros" 
+                  type="mobile_manipulator_node" 
+                  name="mobile_manipulator_node" 
+                  output="screen"
+                  launch-prefix="" />
+
+            <!-- Run Rviz visualization -->
+            <group if="$(eval arg('rviz') == true)">
+                  <include file="$(find mobiman_simulation)/launch/utilities/rviz.launch" />
+            </group>
       </group>
 </launch>
diff --git a/mobiman_simulation/launch/utilities/ocs2_target.launch b/mobiman_simulation/launch/utilities/ocs2_target.launch
index 7308e70..1b5dca2 100644
--- a/mobiman_simulation/launch/utilities/ocs2_target.launch
+++ b/mobiman_simulation/launch/utilities/ocs2_target.launch
@@ -1,6 +1,8 @@
 <?xml version="1.0" ?>
 
 <launch>
+      <arg name="robot_ns"            default="robotName_0"/>
+  
       <!-- Enable rviz visualization -->
       <!--arg name="rviz"        default="true" /-->
 
@@ -36,8 +38,10 @@
       <!--param name="urdfFile"   value="$(arg urdfFile)" />
       <param name="libFolder"  value="$(arg libFolder)" /-->
 
-      <node pkg="ocs2_mobile_manipulator_ros" 
-            type="mobile_manipulator_target" 
-            name="mobile_manipulator_target_node" 
-            output="screen" />
+      <group ns="$(arg robot_ns)">
+            <node pkg="ocs2_mobile_manipulator_ros" 
+                  type="mobile_manipulator_target" 
+                  name="mobile_manipulator_target_node" 
+                  output="screen" />
+      </group>
 </launch>
diff --git a/mobiman_simulation/launch/utilities/rviz.launch b/mobiman_simulation/launch/utilities/rviz.launch
index c015709..1372738 100755
--- a/mobiman_simulation/launch/utilities/rviz.launch
+++ b/mobiman_simulation/launch/utilities/rviz.launch
@@ -1,10 +1,13 @@
 <launch>
   <arg name="rviz_config_path" default="rviz/debug" />
-
-  <node name="rviz" 
-        pkg="rviz" 
-        type="rviz" 
-        args="-d $(find mobiman_simulation)/$(arg rviz_config_path).rviz" 
-        required="true" />
+  <arg name="robot_ns"            default="robotName_0"/>
+  
+  <group ns="$(arg robot_ns)">
+    <node name="rviz" 
+          pkg="rviz" 
+          type="rviz" 
+          args="-d $(find mobiman_simulation)/$(arg rviz_config_path).rviz" 
+          required="true" />
+  </group>
 
 </launch>
\ No newline at end of file
diff --git a/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_0.rviz b/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_0.rviz
new file mode 100644
index 0000000..4701df4
--- /dev/null
+++ b/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_0.rviz
@@ -0,0 +1,723 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /MarkerArray2/Namespaces1
+      Splitter Ratio: 0.5
+    Tree Height: 441
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        chassis_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        d435_cam1camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_frame_gazebo:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_left_ir_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_left_ir_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        d435_cam1camera_right_ir_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_right_ir_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_fender_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        j2n6s300_end_effector:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        j2n6s300_link_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        jaco_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        mid_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        navsat_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_fender_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        rear_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: jackalJaco_0/robot_description
+      TF Prefix: jackalJaco_0
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        jackalJaco_0/base_link:
+          Value: true
+        jackalJaco_0/chassis_link:
+          Value: false
+        jackalJaco_0/d435_cam1camera_bottom_screw_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_color_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_color_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_frame_gazebo:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_left_ir_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_left_ir_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_link:
+          Value: false
+        jackalJaco_0/d435_cam1camera_right_ir_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_right_ir_optical_frame:
+          Value: false
+        jackalJaco_0/front_fender_link:
+          Value: false
+        jackalJaco_0/front_left_wheel_link:
+          Value: false
+        jackalJaco_0/front_mount:
+          Value: false
+        jackalJaco_0/front_right_wheel_link:
+          Value: false
+        jackalJaco_0/imu_link:
+          Value: false
+        jackalJaco_0/j2n6s300_end_effector:
+          Value: true
+        jackalJaco_0/j2n6s300_link_1:
+          Value: false
+        jackalJaco_0/j2n6s300_link_2:
+          Value: false
+        jackalJaco_0/j2n6s300_link_3:
+          Value: false
+        jackalJaco_0/j2n6s300_link_4:
+          Value: false
+        jackalJaco_0/j2n6s300_link_5:
+          Value: false
+        jackalJaco_0/j2n6s300_link_6:
+          Value: false
+        jackalJaco_0/j2n6s300_link_base:
+          Value: false
+        jackalJaco_0/jaco_base_link:
+          Value: false
+        jackalJaco_0/mid_mount:
+          Value: false
+        jackalJaco_0/navsat_link:
+          Value: false
+        jackalJaco_0/odom:
+          Value: false
+        jackalJaco_0/rear_fender_link:
+          Value: false
+        jackalJaco_0/rear_left_wheel_link:
+          Value: false
+        jackalJaco_0/rear_mount:
+          Value: false
+        jackalJaco_0/rear_right_wheel_link:
+          Value: false
+        jackalJaco_1/base_link:
+          Value: false
+        jackalJaco_1/chassis_link:
+          Value: false
+        jackalJaco_1/d435_cam1camera_bottom_screw_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_color_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_color_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_frame_gazebo:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_left_ir_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_left_ir_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_link:
+          Value: false
+        jackalJaco_1/d435_cam1camera_right_ir_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_right_ir_optical_frame:
+          Value: false
+        jackalJaco_1/front_fender_link:
+          Value: false
+        jackalJaco_1/front_left_wheel_link:
+          Value: false
+        jackalJaco_1/front_mount:
+          Value: false
+        jackalJaco_1/front_right_wheel_link:
+          Value: false
+        jackalJaco_1/imu_link:
+          Value: false
+        jackalJaco_1/j2n6s300_end_effector:
+          Value: false
+        jackalJaco_1/j2n6s300_link_1:
+          Value: false
+        jackalJaco_1/j2n6s300_link_2:
+          Value: false
+        jackalJaco_1/j2n6s300_link_3:
+          Value: false
+        jackalJaco_1/j2n6s300_link_4:
+          Value: false
+        jackalJaco_1/j2n6s300_link_5:
+          Value: false
+        jackalJaco_1/j2n6s300_link_6:
+          Value: false
+        jackalJaco_1/j2n6s300_link_base:
+          Value: false
+        jackalJaco_1/jaco_base_link:
+          Value: false
+        jackalJaco_1/mid_mount:
+          Value: false
+        jackalJaco_1/navsat_link:
+          Value: false
+        jackalJaco_1/odom:
+          Value: false
+        jackalJaco_1/rear_fender_link:
+          Value: false
+        jackalJaco_1/rear_left_wheel_link:
+          Value: false
+        jackalJaco_1/rear_mount:
+          Value: false
+        jackalJaco_1/rear_right_wheel_link:
+          Value: false
+        world:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        world:
+          jackalJaco_0/odom:
+            jackalJaco_0/base_link:
+              jackalJaco_0/chassis_link:
+                jackalJaco_0/front_fender_link:
+                  {}
+                jackalJaco_0/front_left_wheel_link:
+                  {}
+                jackalJaco_0/front_right_wheel_link:
+                  {}
+                jackalJaco_0/imu_link:
+                  {}
+                jackalJaco_0/mid_mount:
+                  jackalJaco_0/front_mount:
+                    {}
+                  jackalJaco_0/rear_mount:
+                    {}
+                jackalJaco_0/navsat_link:
+                  {}
+                jackalJaco_0/rear_fender_link:
+                  {}
+                jackalJaco_0/rear_left_wheel_link:
+                  {}
+                jackalJaco_0/rear_right_wheel_link:
+                  {}
+              jackalJaco_0/d435_cam1camera_bottom_screw_frame:
+                jackalJaco_0/d435_cam1camera_link:
+                  jackalJaco_0/d435_cam1camera_depth_frame:
+                    jackalJaco_0/d435_cam1camera_color_frame:
+                      jackalJaco_0/d435_cam1camera_color_optical_frame:
+                        {}
+                    jackalJaco_0/d435_cam1camera_depth_optical_frame:
+                      {}
+                    jackalJaco_0/d435_cam1camera_left_ir_frame:
+                      jackalJaco_0/d435_cam1camera_left_ir_optical_frame:
+                        {}
+                    jackalJaco_0/d435_cam1camera_right_ir_frame:
+                      jackalJaco_0/d435_cam1camera_right_ir_optical_frame:
+                        {}
+                  jackalJaco_0/d435_cam1camera_depth_frame_gazebo:
+                    {}
+              jackalJaco_0/jaco_base_link:
+                jackalJaco_0/j2n6s300_link_base:
+                  jackalJaco_0/j2n6s300_link_1:
+                    jackalJaco_0/j2n6s300_link_2:
+                      jackalJaco_0/j2n6s300_link_3:
+                        jackalJaco_0/j2n6s300_link_4:
+                          jackalJaco_0/j2n6s300_link_5:
+                            jackalJaco_0/j2n6s300_link_6:
+                              jackalJaco_0/j2n6s300_end_effector:
+                                {}
+          jackalJaco_1/odom:
+            jackalJaco_1/base_link:
+              jackalJaco_1/chassis_link:
+                jackalJaco_1/front_fender_link:
+                  {}
+                jackalJaco_1/front_left_wheel_link:
+                  {}
+                jackalJaco_1/front_right_wheel_link:
+                  {}
+                jackalJaco_1/imu_link:
+                  {}
+                jackalJaco_1/mid_mount:
+                  jackalJaco_1/front_mount:
+                    {}
+                  jackalJaco_1/rear_mount:
+                    {}
+                jackalJaco_1/navsat_link:
+                  {}
+                jackalJaco_1/rear_fender_link:
+                  {}
+                jackalJaco_1/rear_left_wheel_link:
+                  {}
+                jackalJaco_1/rear_right_wheel_link:
+                  {}
+              jackalJaco_1/d435_cam1camera_bottom_screw_frame:
+                jackalJaco_1/d435_cam1camera_link:
+                  jackalJaco_1/d435_cam1camera_depth_frame:
+                    jackalJaco_1/d435_cam1camera_color_frame:
+                      jackalJaco_1/d435_cam1camera_color_optical_frame:
+                        {}
+                    jackalJaco_1/d435_cam1camera_depth_optical_frame:
+                      {}
+                    jackalJaco_1/d435_cam1camera_left_ir_frame:
+                      jackalJaco_1/d435_cam1camera_left_ir_optical_frame:
+                        {}
+                    jackalJaco_1/d435_cam1camera_right_ir_frame:
+                      jackalJaco_1/d435_cam1camera_right_ir_optical_frame:
+                        {}
+                  jackalJaco_1/d435_cam1camera_depth_frame_gazebo:
+                    {}
+              jackalJaco_1/jaco_base_link:
+                jackalJaco_1/j2n6s300_link_base:
+                  jackalJaco_1/j2n6s300_link_1:
+                    jackalJaco_1/j2n6s300_link_2:
+                      jackalJaco_1/j2n6s300_link_3:
+                        jackalJaco_1/j2n6s300_link_4:
+                          jackalJaco_1/j2n6s300_link_5:
+                            jackalJaco_1/j2n6s300_link_6:
+                              jackalJaco_1/j2n6s300_end_effector:
+                                {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /jackalJaco_0/target_marker/update
+      Value: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: false
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /jackalJaco_0/auto_target_marker/update
+      Value: false
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /jackalJaco_0/model_mode_marker/update
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: false
+      Name: Map
+      Topic: /occupancy_grid
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /ego_grid_occupancy
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /pc2_scan_bin
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Class: octomap_rviz_plugin/ColorOccupancyGrid
+      Enabled: true
+      Max. Height Display: 3.4028234663852886e+38
+      Max. Octree Depth: 16
+      Min. Height Display: -3.4028234663852886e+38
+      Name: ColorOccupancyGrid
+      Octomap Topic: /jackalJaco_0/octomap_scan
+      Queue Size: 5
+      Value: true
+      Voxel Alpha: 1
+      Voxel Coloring: Z-Axis
+      Voxel Rendering: Occupied Voxels
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /occupancy_distances_base_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /occupancy_distances_arm_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /points_on_robot
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 0
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /jackalJaco_0/self_collision_distances_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 0
+      Value: false
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: false
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Queue Size: 10
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /move_base_simple/goal
+      Unreliable: false
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /jackalJaco_0/optimizedStateTrajectory
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /jackalJaco_0/mobile_manipulator_goal
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /jackalJaco_0/mobile_manipulator_target
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: visualization_marker_array
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 186; 189; 182
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 10000
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 5.194599151611328
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: -0.3674023151397705
+        Y: -0.39812135696411133
+        Z: 0.5430676341056824
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.43979716300964355
+      Target Frame: <Fixed Frame>
+      Yaw: 1.467919945716858
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 1016
+  Hide Left Dock: true
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000019300000335fc020000000efb0000001200530065006c0065006300740069006f006e000000003d0000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000335000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000208000001880000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000208000001880000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000242000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003c000000063fc0100000002fb0000000800540069006d00650100000000000003c0000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003c00000033500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 960
+  X: 1920
+  Y: 27
diff --git a/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_1.rviz b/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_1.rviz
new file mode 100644
index 0000000..01bfb09
--- /dev/null
+++ b/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_1.rviz
@@ -0,0 +1,715 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /MarkerArray2/Namespaces1
+      Splitter Ratio: 0.5
+    Tree Height: 441
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        chassis_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        d435_cam1camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_frame_gazebo:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_left_ir_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_left_ir_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        d435_cam1camera_right_ir_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_right_ir_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_fender_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        j2n6s300_end_effector:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        j2n6s300_link_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        jaco_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        mid_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        navsat_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_fender_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        rear_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: jackalJaco_1/robot_description
+      TF Prefix: jackalJaco_1
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        jackalJaco_0/base_link:
+          Value: false
+        jackalJaco_0/chassis_link:
+          Value: false
+        jackalJaco_0/d435_cam1camera_bottom_screw_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_color_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_color_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_frame_gazebo:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_left_ir_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_left_ir_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_link:
+          Value: false
+        jackalJaco_0/d435_cam1camera_right_ir_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_right_ir_optical_frame:
+          Value: false
+        jackalJaco_0/front_fender_link:
+          Value: false
+        jackalJaco_0/front_left_wheel_link:
+          Value: false
+        jackalJaco_0/front_mount:
+          Value: false
+        jackalJaco_0/front_right_wheel_link:
+          Value: false
+        jackalJaco_0/imu_link:
+          Value: false
+        jackalJaco_0/j2n6s300_end_effector:
+          Value: false
+        jackalJaco_0/j2n6s300_link_1:
+          Value: false
+        jackalJaco_0/j2n6s300_link_2:
+          Value: false
+        jackalJaco_0/j2n6s300_link_3:
+          Value: false
+        jackalJaco_0/j2n6s300_link_4:
+          Value: false
+        jackalJaco_0/j2n6s300_link_5:
+          Value: false
+        jackalJaco_0/j2n6s300_link_6:
+          Value: false
+        jackalJaco_0/j2n6s300_link_base:
+          Value: false
+        jackalJaco_0/jaco_base_link:
+          Value: false
+        jackalJaco_0/mid_mount:
+          Value: false
+        jackalJaco_0/navsat_link:
+          Value: false
+        jackalJaco_0/odom:
+          Value: false
+        jackalJaco_0/rear_fender_link:
+          Value: false
+        jackalJaco_0/rear_left_wheel_link:
+          Value: false
+        jackalJaco_0/rear_mount:
+          Value: false
+        jackalJaco_0/rear_right_wheel_link:
+          Value: false
+        jackalJaco_1/base_link:
+          Value: true
+        jackalJaco_1/chassis_link:
+          Value: false
+        jackalJaco_1/d435_cam1camera_bottom_screw_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_color_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_color_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_frame_gazebo:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_left_ir_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_left_ir_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_link:
+          Value: false
+        jackalJaco_1/d435_cam1camera_right_ir_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_right_ir_optical_frame:
+          Value: false
+        jackalJaco_1/front_fender_link:
+          Value: false
+        jackalJaco_1/front_left_wheel_link:
+          Value: false
+        jackalJaco_1/front_mount:
+          Value: false
+        jackalJaco_1/front_right_wheel_link:
+          Value: false
+        jackalJaco_1/imu_link:
+          Value: false
+        jackalJaco_1/j2n6s300_end_effector:
+          Value: true
+        jackalJaco_1/j2n6s300_link_1:
+          Value: false
+        jackalJaco_1/j2n6s300_link_2:
+          Value: false
+        jackalJaco_1/j2n6s300_link_3:
+          Value: false
+        jackalJaco_1/j2n6s300_link_4:
+          Value: false
+        jackalJaco_1/j2n6s300_link_5:
+          Value: false
+        jackalJaco_1/j2n6s300_link_6:
+          Value: false
+        jackalJaco_1/j2n6s300_link_base:
+          Value: false
+        jackalJaco_1/jaco_base_link:
+          Value: false
+        jackalJaco_1/mid_mount:
+          Value: false
+        jackalJaco_1/navsat_link:
+          Value: false
+        jackalJaco_1/odom:
+          Value: false
+        jackalJaco_1/rear_fender_link:
+          Value: false
+        jackalJaco_1/rear_left_wheel_link:
+          Value: false
+        jackalJaco_1/rear_mount:
+          Value: false
+        jackalJaco_1/rear_right_wheel_link:
+          Value: false
+        world:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        world:
+          jackalJaco_0/odom:
+            jackalJaco_0/base_link:
+              jackalJaco_0/chassis_link:
+                jackalJaco_0/front_fender_link:
+                  {}
+                jackalJaco_0/front_left_wheel_link:
+                  {}
+                jackalJaco_0/front_right_wheel_link:
+                  {}
+                jackalJaco_0/imu_link:
+                  {}
+                jackalJaco_0/mid_mount:
+                  jackalJaco_0/front_mount:
+                    {}
+                  jackalJaco_0/rear_mount:
+                    {}
+                jackalJaco_0/navsat_link:
+                  {}
+                jackalJaco_0/rear_fender_link:
+                  {}
+                jackalJaco_0/rear_left_wheel_link:
+                  {}
+                jackalJaco_0/rear_right_wheel_link:
+                  {}
+              jackalJaco_0/d435_cam1camera_bottom_screw_frame:
+                jackalJaco_0/d435_cam1camera_link:
+                  jackalJaco_0/d435_cam1camera_depth_frame:
+                    jackalJaco_0/d435_cam1camera_color_frame:
+                      jackalJaco_0/d435_cam1camera_color_optical_frame:
+                        {}
+                    jackalJaco_0/d435_cam1camera_depth_optical_frame:
+                      {}
+                    jackalJaco_0/d435_cam1camera_left_ir_frame:
+                      jackalJaco_0/d435_cam1camera_left_ir_optical_frame:
+                        {}
+                    jackalJaco_0/d435_cam1camera_right_ir_frame:
+                      jackalJaco_0/d435_cam1camera_right_ir_optical_frame:
+                        {}
+                  jackalJaco_0/d435_cam1camera_depth_frame_gazebo:
+                    {}
+              jackalJaco_0/jaco_base_link:
+                jackalJaco_0/j2n6s300_link_base:
+                  jackalJaco_0/j2n6s300_link_1:
+                    jackalJaco_0/j2n6s300_link_2:
+                      jackalJaco_0/j2n6s300_link_3:
+                        jackalJaco_0/j2n6s300_link_4:
+                          jackalJaco_0/j2n6s300_link_5:
+                            jackalJaco_0/j2n6s300_link_6:
+                              jackalJaco_0/j2n6s300_end_effector:
+                                {}
+          jackalJaco_1/odom:
+            jackalJaco_1/base_link:
+              jackalJaco_1/chassis_link:
+                jackalJaco_1/front_fender_link:
+                  {}
+                jackalJaco_1/front_left_wheel_link:
+                  {}
+                jackalJaco_1/front_right_wheel_link:
+                  {}
+                jackalJaco_1/imu_link:
+                  {}
+                jackalJaco_1/mid_mount:
+                  jackalJaco_1/front_mount:
+                    {}
+                  jackalJaco_1/rear_mount:
+                    {}
+                jackalJaco_1/navsat_link:
+                  {}
+                jackalJaco_1/rear_fender_link:
+                  {}
+                jackalJaco_1/rear_left_wheel_link:
+                  {}
+                jackalJaco_1/rear_right_wheel_link:
+                  {}
+              jackalJaco_1/d435_cam1camera_bottom_screw_frame:
+                jackalJaco_1/d435_cam1camera_link:
+                  jackalJaco_1/d435_cam1camera_depth_frame:
+                    jackalJaco_1/d435_cam1camera_color_frame:
+                      jackalJaco_1/d435_cam1camera_color_optical_frame:
+                        {}
+                    jackalJaco_1/d435_cam1camera_depth_optical_frame:
+                      {}
+                    jackalJaco_1/d435_cam1camera_left_ir_frame:
+                      jackalJaco_1/d435_cam1camera_left_ir_optical_frame:
+                        {}
+                    jackalJaco_1/d435_cam1camera_right_ir_frame:
+                      jackalJaco_1/d435_cam1camera_right_ir_optical_frame:
+                        {}
+                  jackalJaco_1/d435_cam1camera_depth_frame_gazebo:
+                    {}
+              jackalJaco_1/jaco_base_link:
+                jackalJaco_1/j2n6s300_link_base:
+                  jackalJaco_1/j2n6s300_link_1:
+                    jackalJaco_1/j2n6s300_link_2:
+                      jackalJaco_1/j2n6s300_link_3:
+                        jackalJaco_1/j2n6s300_link_4:
+                          jackalJaco_1/j2n6s300_link_5:
+                            jackalJaco_1/j2n6s300_link_6:
+                              jackalJaco_1/j2n6s300_end_effector:
+                                {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /jackalJaco_1/target_marker/update
+      Value: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: false
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /jackalJaco_1/auto_target_marker/update
+      Value: false
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /jackalJaco_0/model_mode_marker/update
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: false
+      Name: Map
+      Topic: /occupancy_grid
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /ego_grid_occupancy
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /pc2_scan_bin
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Class: octomap_rviz_plugin/ColorOccupancyGrid
+      Enabled: true
+      Max. Height Display: 3.4028234663852886e+38
+      Max. Octree Depth: 16
+      Min. Height Display: -3.4028234663852886e+38
+      Name: ColorOccupancyGrid
+      Octomap Topic: /jackalJaco_1/octomap_scan
+      Queue Size: 5
+      Value: true
+      Voxel Alpha: 1
+      Voxel Coloring: Z-Axis
+      Voxel Rendering: Occupied Voxels
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /occupancy_distances_base_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /occupancy_distances_arm_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /points_on_robot
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 0
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /self_collision_distances_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 0
+      Value: false
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: false
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Queue Size: 10
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /move_base_simple/goal
+      Unreliable: false
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /jackalJaco_1/optimizedStateTrajectory
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /jackalJaco_1/mobile_manipulator_goal
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /jackalJaco_1/mobile_manipulator_target
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 186; 189; 182
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 10000
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 4.7370924949646
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: -0.17908141016960144
+        Y: -0.3189200758934021
+        Z: 0.7639337778091431
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.6647974848747253
+      Target Frame: <Fixed Frame>
+      Yaw: 1.662919044494629
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 1016
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 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
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 960
+  X: 2880
+  Y: 27
diff --git a/mobiman_simulation/rviz/mobile_manipulator_ocs2_igibson.rviz b/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_2.rviz
similarity index 98%
rename from mobiman_simulation/rviz/mobile_manipulator_ocs2_igibson.rviz
rename to mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_2.rviz
index 890530d..4864248 100644
--- a/mobiman_simulation/rviz/mobile_manipulator_ocs2_igibson.rviz
+++ b/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_2.rviz
@@ -7,7 +7,7 @@ Panels:
         - /TF1/Frames1
         - /MarkerArray2/Namespaces1
       Splitter Ratio: 0.5
-    Tree Height: 787
+    Tree Height: 517
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -636,7 +636,7 @@ Visualization Manager:
       Marker Topic: /mobile_manipulator_goal
       Name: MarkerArray
       Namespaces:
-        goal_1: true
+        {}
       Queue Size: 100
       Value: true
     - Class: rviz/MarkerArray
@@ -706,10 +706,10 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1043
+  Height: 773
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 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
+  QMainWindow State: 000000ff00000000fd00000004000000000000026200000242fc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000242000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000208000001880000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000208000001880000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a000000063fc0100000002fb0000000800540069006d00650100000000000005a0000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003380000024200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -718,6 +718,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1920
-  X: 3840
-  Y: 0
+  Width: 1440
+  X: 2167
+  Y: 112
diff --git a/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_3.rviz b/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_3.rviz
new file mode 100644
index 0000000..4864248
--- /dev/null
+++ b/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_3.rviz
@@ -0,0 +1,723 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /MarkerArray2/Namespaces1
+      Splitter Ratio: 0.5
+    Tree Height: 517
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        chassis_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        d435_cam1camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_frame_gazebo:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_left_ir_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_left_ir_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        d435_cam1camera_right_ir_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_right_ir_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_fender_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        j2n6s300_end_effector:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        j2n6s300_link_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        jaco_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        mid_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        navsat_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_fender_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        rear_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: jackalJaco_0/robot_description
+      TF Prefix: jackalJaco_0
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        jackalJaco_0/base_link:
+          Value: true
+        jackalJaco_0/chassis_link:
+          Value: false
+        jackalJaco_0/d435_cam1camera_bottom_screw_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_color_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_color_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_frame_gazebo:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_left_ir_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_left_ir_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_link:
+          Value: false
+        jackalJaco_0/d435_cam1camera_right_ir_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_right_ir_optical_frame:
+          Value: false
+        jackalJaco_0/front_fender_link:
+          Value: false
+        jackalJaco_0/front_left_wheel_link:
+          Value: false
+        jackalJaco_0/front_mount:
+          Value: false
+        jackalJaco_0/front_right_wheel_link:
+          Value: false
+        jackalJaco_0/imu_link:
+          Value: false
+        jackalJaco_0/j2n6s300_end_effector:
+          Value: true
+        jackalJaco_0/j2n6s300_link_1:
+          Value: false
+        jackalJaco_0/j2n6s300_link_2:
+          Value: false
+        jackalJaco_0/j2n6s300_link_3:
+          Value: false
+        jackalJaco_0/j2n6s300_link_4:
+          Value: false
+        jackalJaco_0/j2n6s300_link_5:
+          Value: false
+        jackalJaco_0/j2n6s300_link_6:
+          Value: false
+        jackalJaco_0/j2n6s300_link_base:
+          Value: false
+        jackalJaco_0/jaco_base_link:
+          Value: false
+        jackalJaco_0/mid_mount:
+          Value: false
+        jackalJaco_0/navsat_link:
+          Value: false
+        jackalJaco_0/odom:
+          Value: false
+        jackalJaco_0/rear_fender_link:
+          Value: false
+        jackalJaco_0/rear_left_wheel_link:
+          Value: false
+        jackalJaco_0/rear_mount:
+          Value: false
+        jackalJaco_0/rear_right_wheel_link:
+          Value: false
+        jackalJaco_1/base_link:
+          Value: false
+        jackalJaco_1/chassis_link:
+          Value: false
+        jackalJaco_1/d435_cam1camera_bottom_screw_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_color_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_color_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_frame_gazebo:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_left_ir_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_left_ir_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_link:
+          Value: false
+        jackalJaco_1/d435_cam1camera_right_ir_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_right_ir_optical_frame:
+          Value: false
+        jackalJaco_1/front_fender_link:
+          Value: false
+        jackalJaco_1/front_left_wheel_link:
+          Value: false
+        jackalJaco_1/front_mount:
+          Value: false
+        jackalJaco_1/front_right_wheel_link:
+          Value: false
+        jackalJaco_1/imu_link:
+          Value: false
+        jackalJaco_1/j2n6s300_end_effector:
+          Value: false
+        jackalJaco_1/j2n6s300_link_1:
+          Value: false
+        jackalJaco_1/j2n6s300_link_2:
+          Value: false
+        jackalJaco_1/j2n6s300_link_3:
+          Value: false
+        jackalJaco_1/j2n6s300_link_4:
+          Value: false
+        jackalJaco_1/j2n6s300_link_5:
+          Value: false
+        jackalJaco_1/j2n6s300_link_6:
+          Value: false
+        jackalJaco_1/j2n6s300_link_base:
+          Value: false
+        jackalJaco_1/jaco_base_link:
+          Value: false
+        jackalJaco_1/mid_mount:
+          Value: false
+        jackalJaco_1/navsat_link:
+          Value: false
+        jackalJaco_1/odom:
+          Value: false
+        jackalJaco_1/rear_fender_link:
+          Value: false
+        jackalJaco_1/rear_left_wheel_link:
+          Value: false
+        jackalJaco_1/rear_mount:
+          Value: false
+        jackalJaco_1/rear_right_wheel_link:
+          Value: false
+        world:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        world:
+          jackalJaco_0/odom:
+            jackalJaco_0/base_link:
+              jackalJaco_0/chassis_link:
+                jackalJaco_0/front_fender_link:
+                  {}
+                jackalJaco_0/front_left_wheel_link:
+                  {}
+                jackalJaco_0/front_right_wheel_link:
+                  {}
+                jackalJaco_0/imu_link:
+                  {}
+                jackalJaco_0/mid_mount:
+                  jackalJaco_0/front_mount:
+                    {}
+                  jackalJaco_0/rear_mount:
+                    {}
+                jackalJaco_0/navsat_link:
+                  {}
+                jackalJaco_0/rear_fender_link:
+                  {}
+                jackalJaco_0/rear_left_wheel_link:
+                  {}
+                jackalJaco_0/rear_right_wheel_link:
+                  {}
+              jackalJaco_0/d435_cam1camera_bottom_screw_frame:
+                jackalJaco_0/d435_cam1camera_link:
+                  jackalJaco_0/d435_cam1camera_depth_frame:
+                    jackalJaco_0/d435_cam1camera_color_frame:
+                      jackalJaco_0/d435_cam1camera_color_optical_frame:
+                        {}
+                    jackalJaco_0/d435_cam1camera_depth_optical_frame:
+                      {}
+                    jackalJaco_0/d435_cam1camera_left_ir_frame:
+                      jackalJaco_0/d435_cam1camera_left_ir_optical_frame:
+                        {}
+                    jackalJaco_0/d435_cam1camera_right_ir_frame:
+                      jackalJaco_0/d435_cam1camera_right_ir_optical_frame:
+                        {}
+                  jackalJaco_0/d435_cam1camera_depth_frame_gazebo:
+                    {}
+              jackalJaco_0/jaco_base_link:
+                jackalJaco_0/j2n6s300_link_base:
+                  jackalJaco_0/j2n6s300_link_1:
+                    jackalJaco_0/j2n6s300_link_2:
+                      jackalJaco_0/j2n6s300_link_3:
+                        jackalJaco_0/j2n6s300_link_4:
+                          jackalJaco_0/j2n6s300_link_5:
+                            jackalJaco_0/j2n6s300_link_6:
+                              jackalJaco_0/j2n6s300_end_effector:
+                                {}
+          jackalJaco_1/odom:
+            jackalJaco_1/base_link:
+              jackalJaco_1/chassis_link:
+                jackalJaco_1/front_fender_link:
+                  {}
+                jackalJaco_1/front_left_wheel_link:
+                  {}
+                jackalJaco_1/front_right_wheel_link:
+                  {}
+                jackalJaco_1/imu_link:
+                  {}
+                jackalJaco_1/mid_mount:
+                  jackalJaco_1/front_mount:
+                    {}
+                  jackalJaco_1/rear_mount:
+                    {}
+                jackalJaco_1/navsat_link:
+                  {}
+                jackalJaco_1/rear_fender_link:
+                  {}
+                jackalJaco_1/rear_left_wheel_link:
+                  {}
+                jackalJaco_1/rear_right_wheel_link:
+                  {}
+              jackalJaco_1/d435_cam1camera_bottom_screw_frame:
+                jackalJaco_1/d435_cam1camera_link:
+                  jackalJaco_1/d435_cam1camera_depth_frame:
+                    jackalJaco_1/d435_cam1camera_color_frame:
+                      jackalJaco_1/d435_cam1camera_color_optical_frame:
+                        {}
+                    jackalJaco_1/d435_cam1camera_depth_optical_frame:
+                      {}
+                    jackalJaco_1/d435_cam1camera_left_ir_frame:
+                      jackalJaco_1/d435_cam1camera_left_ir_optical_frame:
+                        {}
+                    jackalJaco_1/d435_cam1camera_right_ir_frame:
+                      jackalJaco_1/d435_cam1camera_right_ir_optical_frame:
+                        {}
+                  jackalJaco_1/d435_cam1camera_depth_frame_gazebo:
+                    {}
+              jackalJaco_1/jaco_base_link:
+                jackalJaco_1/j2n6s300_link_base:
+                  jackalJaco_1/j2n6s300_link_1:
+                    jackalJaco_1/j2n6s300_link_2:
+                      jackalJaco_1/j2n6s300_link_3:
+                        jackalJaco_1/j2n6s300_link_4:
+                          jackalJaco_1/j2n6s300_link_5:
+                            jackalJaco_1/j2n6s300_link_6:
+                              jackalJaco_1/j2n6s300_end_effector:
+                                {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /target_marker/update
+      Value: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: false
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /auto_target_marker/update
+      Value: false
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /model_mode_marker/update
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: false
+      Name: Map
+      Topic: /occupancy_grid
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /ego_grid_occupancy
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /pc2_scan_bin
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Class: octomap_rviz_plugin/ColorOccupancyGrid
+      Enabled: true
+      Max. Height Display: 3.4028234663852886e+38
+      Max. Octree Depth: 16
+      Min. Height Display: -3.4028234663852886e+38
+      Name: ColorOccupancyGrid
+      Octomap Topic: /octomap_scan
+      Queue Size: 5
+      Value: true
+      Voxel Alpha: 1
+      Voxel Coloring: Z-Axis
+      Voxel Rendering: Occupied Voxels
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /occupancy_distances_base_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /occupancy_distances_arm_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /points_on_robot
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 0
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /self_collision_distances_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 0
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Queue Size: 10
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /move_base_simple/goal
+      Unreliable: false
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /mobile_manipulator/optimizedStateTrajectory
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /mobile_manipulator_goal
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /mobile_manipulator_target
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /debug_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 186; 189; 182
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 10000
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 3.889058828353882
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.07714345306158066
+        Y: -0.04144350811839104
+        Z: 0.5177860856056213
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5497972965240479
+      Target Frame: <Fixed Frame>
+      Yaw: 1.6029189825057983
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 773
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 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
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1440
+  X: 2167
+  Y: 112
diff --git a/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_4.rviz b/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_4.rviz
new file mode 100644
index 0000000..4864248
--- /dev/null
+++ b/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_4.rviz
@@ -0,0 +1,723 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /MarkerArray2/Namespaces1
+      Splitter Ratio: 0.5
+    Tree Height: 517
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        chassis_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        d435_cam1camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_frame_gazebo:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_left_ir_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_left_ir_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        d435_cam1camera_right_ir_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_right_ir_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_fender_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        j2n6s300_end_effector:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        j2n6s300_link_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        jaco_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        mid_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        navsat_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_fender_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        rear_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: jackalJaco_0/robot_description
+      TF Prefix: jackalJaco_0
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        jackalJaco_0/base_link:
+          Value: true
+        jackalJaco_0/chassis_link:
+          Value: false
+        jackalJaco_0/d435_cam1camera_bottom_screw_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_color_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_color_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_frame_gazebo:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_left_ir_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_left_ir_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_link:
+          Value: false
+        jackalJaco_0/d435_cam1camera_right_ir_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_right_ir_optical_frame:
+          Value: false
+        jackalJaco_0/front_fender_link:
+          Value: false
+        jackalJaco_0/front_left_wheel_link:
+          Value: false
+        jackalJaco_0/front_mount:
+          Value: false
+        jackalJaco_0/front_right_wheel_link:
+          Value: false
+        jackalJaco_0/imu_link:
+          Value: false
+        jackalJaco_0/j2n6s300_end_effector:
+          Value: true
+        jackalJaco_0/j2n6s300_link_1:
+          Value: false
+        jackalJaco_0/j2n6s300_link_2:
+          Value: false
+        jackalJaco_0/j2n6s300_link_3:
+          Value: false
+        jackalJaco_0/j2n6s300_link_4:
+          Value: false
+        jackalJaco_0/j2n6s300_link_5:
+          Value: false
+        jackalJaco_0/j2n6s300_link_6:
+          Value: false
+        jackalJaco_0/j2n6s300_link_base:
+          Value: false
+        jackalJaco_0/jaco_base_link:
+          Value: false
+        jackalJaco_0/mid_mount:
+          Value: false
+        jackalJaco_0/navsat_link:
+          Value: false
+        jackalJaco_0/odom:
+          Value: false
+        jackalJaco_0/rear_fender_link:
+          Value: false
+        jackalJaco_0/rear_left_wheel_link:
+          Value: false
+        jackalJaco_0/rear_mount:
+          Value: false
+        jackalJaco_0/rear_right_wheel_link:
+          Value: false
+        jackalJaco_1/base_link:
+          Value: false
+        jackalJaco_1/chassis_link:
+          Value: false
+        jackalJaco_1/d435_cam1camera_bottom_screw_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_color_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_color_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_frame_gazebo:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_left_ir_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_left_ir_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_link:
+          Value: false
+        jackalJaco_1/d435_cam1camera_right_ir_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_right_ir_optical_frame:
+          Value: false
+        jackalJaco_1/front_fender_link:
+          Value: false
+        jackalJaco_1/front_left_wheel_link:
+          Value: false
+        jackalJaco_1/front_mount:
+          Value: false
+        jackalJaco_1/front_right_wheel_link:
+          Value: false
+        jackalJaco_1/imu_link:
+          Value: false
+        jackalJaco_1/j2n6s300_end_effector:
+          Value: false
+        jackalJaco_1/j2n6s300_link_1:
+          Value: false
+        jackalJaco_1/j2n6s300_link_2:
+          Value: false
+        jackalJaco_1/j2n6s300_link_3:
+          Value: false
+        jackalJaco_1/j2n6s300_link_4:
+          Value: false
+        jackalJaco_1/j2n6s300_link_5:
+          Value: false
+        jackalJaco_1/j2n6s300_link_6:
+          Value: false
+        jackalJaco_1/j2n6s300_link_base:
+          Value: false
+        jackalJaco_1/jaco_base_link:
+          Value: false
+        jackalJaco_1/mid_mount:
+          Value: false
+        jackalJaco_1/navsat_link:
+          Value: false
+        jackalJaco_1/odom:
+          Value: false
+        jackalJaco_1/rear_fender_link:
+          Value: false
+        jackalJaco_1/rear_left_wheel_link:
+          Value: false
+        jackalJaco_1/rear_mount:
+          Value: false
+        jackalJaco_1/rear_right_wheel_link:
+          Value: false
+        world:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        world:
+          jackalJaco_0/odom:
+            jackalJaco_0/base_link:
+              jackalJaco_0/chassis_link:
+                jackalJaco_0/front_fender_link:
+                  {}
+                jackalJaco_0/front_left_wheel_link:
+                  {}
+                jackalJaco_0/front_right_wheel_link:
+                  {}
+                jackalJaco_0/imu_link:
+                  {}
+                jackalJaco_0/mid_mount:
+                  jackalJaco_0/front_mount:
+                    {}
+                  jackalJaco_0/rear_mount:
+                    {}
+                jackalJaco_0/navsat_link:
+                  {}
+                jackalJaco_0/rear_fender_link:
+                  {}
+                jackalJaco_0/rear_left_wheel_link:
+                  {}
+                jackalJaco_0/rear_right_wheel_link:
+                  {}
+              jackalJaco_0/d435_cam1camera_bottom_screw_frame:
+                jackalJaco_0/d435_cam1camera_link:
+                  jackalJaco_0/d435_cam1camera_depth_frame:
+                    jackalJaco_0/d435_cam1camera_color_frame:
+                      jackalJaco_0/d435_cam1camera_color_optical_frame:
+                        {}
+                    jackalJaco_0/d435_cam1camera_depth_optical_frame:
+                      {}
+                    jackalJaco_0/d435_cam1camera_left_ir_frame:
+                      jackalJaco_0/d435_cam1camera_left_ir_optical_frame:
+                        {}
+                    jackalJaco_0/d435_cam1camera_right_ir_frame:
+                      jackalJaco_0/d435_cam1camera_right_ir_optical_frame:
+                        {}
+                  jackalJaco_0/d435_cam1camera_depth_frame_gazebo:
+                    {}
+              jackalJaco_0/jaco_base_link:
+                jackalJaco_0/j2n6s300_link_base:
+                  jackalJaco_0/j2n6s300_link_1:
+                    jackalJaco_0/j2n6s300_link_2:
+                      jackalJaco_0/j2n6s300_link_3:
+                        jackalJaco_0/j2n6s300_link_4:
+                          jackalJaco_0/j2n6s300_link_5:
+                            jackalJaco_0/j2n6s300_link_6:
+                              jackalJaco_0/j2n6s300_end_effector:
+                                {}
+          jackalJaco_1/odom:
+            jackalJaco_1/base_link:
+              jackalJaco_1/chassis_link:
+                jackalJaco_1/front_fender_link:
+                  {}
+                jackalJaco_1/front_left_wheel_link:
+                  {}
+                jackalJaco_1/front_right_wheel_link:
+                  {}
+                jackalJaco_1/imu_link:
+                  {}
+                jackalJaco_1/mid_mount:
+                  jackalJaco_1/front_mount:
+                    {}
+                  jackalJaco_1/rear_mount:
+                    {}
+                jackalJaco_1/navsat_link:
+                  {}
+                jackalJaco_1/rear_fender_link:
+                  {}
+                jackalJaco_1/rear_left_wheel_link:
+                  {}
+                jackalJaco_1/rear_right_wheel_link:
+                  {}
+              jackalJaco_1/d435_cam1camera_bottom_screw_frame:
+                jackalJaco_1/d435_cam1camera_link:
+                  jackalJaco_1/d435_cam1camera_depth_frame:
+                    jackalJaco_1/d435_cam1camera_color_frame:
+                      jackalJaco_1/d435_cam1camera_color_optical_frame:
+                        {}
+                    jackalJaco_1/d435_cam1camera_depth_optical_frame:
+                      {}
+                    jackalJaco_1/d435_cam1camera_left_ir_frame:
+                      jackalJaco_1/d435_cam1camera_left_ir_optical_frame:
+                        {}
+                    jackalJaco_1/d435_cam1camera_right_ir_frame:
+                      jackalJaco_1/d435_cam1camera_right_ir_optical_frame:
+                        {}
+                  jackalJaco_1/d435_cam1camera_depth_frame_gazebo:
+                    {}
+              jackalJaco_1/jaco_base_link:
+                jackalJaco_1/j2n6s300_link_base:
+                  jackalJaco_1/j2n6s300_link_1:
+                    jackalJaco_1/j2n6s300_link_2:
+                      jackalJaco_1/j2n6s300_link_3:
+                        jackalJaco_1/j2n6s300_link_4:
+                          jackalJaco_1/j2n6s300_link_5:
+                            jackalJaco_1/j2n6s300_link_6:
+                              jackalJaco_1/j2n6s300_end_effector:
+                                {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /target_marker/update
+      Value: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: false
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /auto_target_marker/update
+      Value: false
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /model_mode_marker/update
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: false
+      Name: Map
+      Topic: /occupancy_grid
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /ego_grid_occupancy
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /pc2_scan_bin
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Class: octomap_rviz_plugin/ColorOccupancyGrid
+      Enabled: true
+      Max. Height Display: 3.4028234663852886e+38
+      Max. Octree Depth: 16
+      Min. Height Display: -3.4028234663852886e+38
+      Name: ColorOccupancyGrid
+      Octomap Topic: /octomap_scan
+      Queue Size: 5
+      Value: true
+      Voxel Alpha: 1
+      Voxel Coloring: Z-Axis
+      Voxel Rendering: Occupied Voxels
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /occupancy_distances_base_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /occupancy_distances_arm_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /points_on_robot
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 0
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /self_collision_distances_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 0
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Queue Size: 10
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /move_base_simple/goal
+      Unreliable: false
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /mobile_manipulator/optimizedStateTrajectory
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /mobile_manipulator_goal
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /mobile_manipulator_target
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /debug_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 186; 189; 182
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 10000
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 3.889058828353882
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.07714345306158066
+        Y: -0.04144350811839104
+        Z: 0.5177860856056213
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5497972965240479
+      Target Frame: <Fixed Frame>
+      Yaw: 1.6029189825057983
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 773
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 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
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1440
+  X: 2167
+  Y: 112
diff --git a/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_5.rviz b/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_5.rviz
new file mode 100644
index 0000000..4864248
--- /dev/null
+++ b/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_5.rviz
@@ -0,0 +1,723 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /MarkerArray2/Namespaces1
+      Splitter Ratio: 0.5
+    Tree Height: 517
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        chassis_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        d435_cam1camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_frame_gazebo:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_left_ir_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_left_ir_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        d435_cam1camera_right_ir_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_right_ir_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_fender_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        j2n6s300_end_effector:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        j2n6s300_link_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        jaco_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        mid_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        navsat_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_fender_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        rear_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: jackalJaco_0/robot_description
+      TF Prefix: jackalJaco_0
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        jackalJaco_0/base_link:
+          Value: true
+        jackalJaco_0/chassis_link:
+          Value: false
+        jackalJaco_0/d435_cam1camera_bottom_screw_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_color_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_color_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_frame_gazebo:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_left_ir_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_left_ir_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_link:
+          Value: false
+        jackalJaco_0/d435_cam1camera_right_ir_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_right_ir_optical_frame:
+          Value: false
+        jackalJaco_0/front_fender_link:
+          Value: false
+        jackalJaco_0/front_left_wheel_link:
+          Value: false
+        jackalJaco_0/front_mount:
+          Value: false
+        jackalJaco_0/front_right_wheel_link:
+          Value: false
+        jackalJaco_0/imu_link:
+          Value: false
+        jackalJaco_0/j2n6s300_end_effector:
+          Value: true
+        jackalJaco_0/j2n6s300_link_1:
+          Value: false
+        jackalJaco_0/j2n6s300_link_2:
+          Value: false
+        jackalJaco_0/j2n6s300_link_3:
+          Value: false
+        jackalJaco_0/j2n6s300_link_4:
+          Value: false
+        jackalJaco_0/j2n6s300_link_5:
+          Value: false
+        jackalJaco_0/j2n6s300_link_6:
+          Value: false
+        jackalJaco_0/j2n6s300_link_base:
+          Value: false
+        jackalJaco_0/jaco_base_link:
+          Value: false
+        jackalJaco_0/mid_mount:
+          Value: false
+        jackalJaco_0/navsat_link:
+          Value: false
+        jackalJaco_0/odom:
+          Value: false
+        jackalJaco_0/rear_fender_link:
+          Value: false
+        jackalJaco_0/rear_left_wheel_link:
+          Value: false
+        jackalJaco_0/rear_mount:
+          Value: false
+        jackalJaco_0/rear_right_wheel_link:
+          Value: false
+        jackalJaco_1/base_link:
+          Value: false
+        jackalJaco_1/chassis_link:
+          Value: false
+        jackalJaco_1/d435_cam1camera_bottom_screw_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_color_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_color_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_frame_gazebo:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_left_ir_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_left_ir_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_link:
+          Value: false
+        jackalJaco_1/d435_cam1camera_right_ir_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_right_ir_optical_frame:
+          Value: false
+        jackalJaco_1/front_fender_link:
+          Value: false
+        jackalJaco_1/front_left_wheel_link:
+          Value: false
+        jackalJaco_1/front_mount:
+          Value: false
+        jackalJaco_1/front_right_wheel_link:
+          Value: false
+        jackalJaco_1/imu_link:
+          Value: false
+        jackalJaco_1/j2n6s300_end_effector:
+          Value: false
+        jackalJaco_1/j2n6s300_link_1:
+          Value: false
+        jackalJaco_1/j2n6s300_link_2:
+          Value: false
+        jackalJaco_1/j2n6s300_link_3:
+          Value: false
+        jackalJaco_1/j2n6s300_link_4:
+          Value: false
+        jackalJaco_1/j2n6s300_link_5:
+          Value: false
+        jackalJaco_1/j2n6s300_link_6:
+          Value: false
+        jackalJaco_1/j2n6s300_link_base:
+          Value: false
+        jackalJaco_1/jaco_base_link:
+          Value: false
+        jackalJaco_1/mid_mount:
+          Value: false
+        jackalJaco_1/navsat_link:
+          Value: false
+        jackalJaco_1/odom:
+          Value: false
+        jackalJaco_1/rear_fender_link:
+          Value: false
+        jackalJaco_1/rear_left_wheel_link:
+          Value: false
+        jackalJaco_1/rear_mount:
+          Value: false
+        jackalJaco_1/rear_right_wheel_link:
+          Value: false
+        world:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        world:
+          jackalJaco_0/odom:
+            jackalJaco_0/base_link:
+              jackalJaco_0/chassis_link:
+                jackalJaco_0/front_fender_link:
+                  {}
+                jackalJaco_0/front_left_wheel_link:
+                  {}
+                jackalJaco_0/front_right_wheel_link:
+                  {}
+                jackalJaco_0/imu_link:
+                  {}
+                jackalJaco_0/mid_mount:
+                  jackalJaco_0/front_mount:
+                    {}
+                  jackalJaco_0/rear_mount:
+                    {}
+                jackalJaco_0/navsat_link:
+                  {}
+                jackalJaco_0/rear_fender_link:
+                  {}
+                jackalJaco_0/rear_left_wheel_link:
+                  {}
+                jackalJaco_0/rear_right_wheel_link:
+                  {}
+              jackalJaco_0/d435_cam1camera_bottom_screw_frame:
+                jackalJaco_0/d435_cam1camera_link:
+                  jackalJaco_0/d435_cam1camera_depth_frame:
+                    jackalJaco_0/d435_cam1camera_color_frame:
+                      jackalJaco_0/d435_cam1camera_color_optical_frame:
+                        {}
+                    jackalJaco_0/d435_cam1camera_depth_optical_frame:
+                      {}
+                    jackalJaco_0/d435_cam1camera_left_ir_frame:
+                      jackalJaco_0/d435_cam1camera_left_ir_optical_frame:
+                        {}
+                    jackalJaco_0/d435_cam1camera_right_ir_frame:
+                      jackalJaco_0/d435_cam1camera_right_ir_optical_frame:
+                        {}
+                  jackalJaco_0/d435_cam1camera_depth_frame_gazebo:
+                    {}
+              jackalJaco_0/jaco_base_link:
+                jackalJaco_0/j2n6s300_link_base:
+                  jackalJaco_0/j2n6s300_link_1:
+                    jackalJaco_0/j2n6s300_link_2:
+                      jackalJaco_0/j2n6s300_link_3:
+                        jackalJaco_0/j2n6s300_link_4:
+                          jackalJaco_0/j2n6s300_link_5:
+                            jackalJaco_0/j2n6s300_link_6:
+                              jackalJaco_0/j2n6s300_end_effector:
+                                {}
+          jackalJaco_1/odom:
+            jackalJaco_1/base_link:
+              jackalJaco_1/chassis_link:
+                jackalJaco_1/front_fender_link:
+                  {}
+                jackalJaco_1/front_left_wheel_link:
+                  {}
+                jackalJaco_1/front_right_wheel_link:
+                  {}
+                jackalJaco_1/imu_link:
+                  {}
+                jackalJaco_1/mid_mount:
+                  jackalJaco_1/front_mount:
+                    {}
+                  jackalJaco_1/rear_mount:
+                    {}
+                jackalJaco_1/navsat_link:
+                  {}
+                jackalJaco_1/rear_fender_link:
+                  {}
+                jackalJaco_1/rear_left_wheel_link:
+                  {}
+                jackalJaco_1/rear_right_wheel_link:
+                  {}
+              jackalJaco_1/d435_cam1camera_bottom_screw_frame:
+                jackalJaco_1/d435_cam1camera_link:
+                  jackalJaco_1/d435_cam1camera_depth_frame:
+                    jackalJaco_1/d435_cam1camera_color_frame:
+                      jackalJaco_1/d435_cam1camera_color_optical_frame:
+                        {}
+                    jackalJaco_1/d435_cam1camera_depth_optical_frame:
+                      {}
+                    jackalJaco_1/d435_cam1camera_left_ir_frame:
+                      jackalJaco_1/d435_cam1camera_left_ir_optical_frame:
+                        {}
+                    jackalJaco_1/d435_cam1camera_right_ir_frame:
+                      jackalJaco_1/d435_cam1camera_right_ir_optical_frame:
+                        {}
+                  jackalJaco_1/d435_cam1camera_depth_frame_gazebo:
+                    {}
+              jackalJaco_1/jaco_base_link:
+                jackalJaco_1/j2n6s300_link_base:
+                  jackalJaco_1/j2n6s300_link_1:
+                    jackalJaco_1/j2n6s300_link_2:
+                      jackalJaco_1/j2n6s300_link_3:
+                        jackalJaco_1/j2n6s300_link_4:
+                          jackalJaco_1/j2n6s300_link_5:
+                            jackalJaco_1/j2n6s300_link_6:
+                              jackalJaco_1/j2n6s300_end_effector:
+                                {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /target_marker/update
+      Value: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: false
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /auto_target_marker/update
+      Value: false
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /model_mode_marker/update
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: false
+      Name: Map
+      Topic: /occupancy_grid
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /ego_grid_occupancy
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /pc2_scan_bin
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Class: octomap_rviz_plugin/ColorOccupancyGrid
+      Enabled: true
+      Max. Height Display: 3.4028234663852886e+38
+      Max. Octree Depth: 16
+      Min. Height Display: -3.4028234663852886e+38
+      Name: ColorOccupancyGrid
+      Octomap Topic: /octomap_scan
+      Queue Size: 5
+      Value: true
+      Voxel Alpha: 1
+      Voxel Coloring: Z-Axis
+      Voxel Rendering: Occupied Voxels
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /occupancy_distances_base_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /occupancy_distances_arm_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /points_on_robot
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 0
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /self_collision_distances_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 0
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Queue Size: 10
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /move_base_simple/goal
+      Unreliable: false
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /mobile_manipulator/optimizedStateTrajectory
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /mobile_manipulator_goal
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /mobile_manipulator_target
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /debug_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 186; 189; 182
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 10000
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 3.889058828353882
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.07714345306158066
+        Y: -0.04144350811839104
+        Z: 0.5177860856056213
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5497972965240479
+      Target Frame: <Fixed Frame>
+      Yaw: 1.6029189825057983
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 773
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 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
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1440
+  X: 2167
+  Y: 112
diff --git a/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_debug.rviz b/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_debug.rviz
new file mode 100644
index 0000000..4864248
--- /dev/null
+++ b/mobiman_simulation/rviz/igibson/mobile_manipulator_ocs2_igibson_debug.rviz
@@ -0,0 +1,723 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /MarkerArray2/Namespaces1
+      Splitter Ratio: 0.5
+    Tree Height: 517
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        chassis_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        d435_cam1camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_frame_gazebo:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_left_ir_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_left_ir_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        d435_cam1camera_right_ir_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_cam1camera_right_ir_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_fender_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        j2n6s300_end_effector:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        j2n6s300_link_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        j2n6s300_link_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        jaco_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        mid_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        navsat_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_fender_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rear_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        rear_right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: jackalJaco_0/robot_description
+      TF Prefix: jackalJaco_0
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        jackalJaco_0/base_link:
+          Value: true
+        jackalJaco_0/chassis_link:
+          Value: false
+        jackalJaco_0/d435_cam1camera_bottom_screw_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_color_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_color_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_frame_gazebo:
+          Value: false
+        jackalJaco_0/d435_cam1camera_depth_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_left_ir_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_left_ir_optical_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_link:
+          Value: false
+        jackalJaco_0/d435_cam1camera_right_ir_frame:
+          Value: false
+        jackalJaco_0/d435_cam1camera_right_ir_optical_frame:
+          Value: false
+        jackalJaco_0/front_fender_link:
+          Value: false
+        jackalJaco_0/front_left_wheel_link:
+          Value: false
+        jackalJaco_0/front_mount:
+          Value: false
+        jackalJaco_0/front_right_wheel_link:
+          Value: false
+        jackalJaco_0/imu_link:
+          Value: false
+        jackalJaco_0/j2n6s300_end_effector:
+          Value: true
+        jackalJaco_0/j2n6s300_link_1:
+          Value: false
+        jackalJaco_0/j2n6s300_link_2:
+          Value: false
+        jackalJaco_0/j2n6s300_link_3:
+          Value: false
+        jackalJaco_0/j2n6s300_link_4:
+          Value: false
+        jackalJaco_0/j2n6s300_link_5:
+          Value: false
+        jackalJaco_0/j2n6s300_link_6:
+          Value: false
+        jackalJaco_0/j2n6s300_link_base:
+          Value: false
+        jackalJaco_0/jaco_base_link:
+          Value: false
+        jackalJaco_0/mid_mount:
+          Value: false
+        jackalJaco_0/navsat_link:
+          Value: false
+        jackalJaco_0/odom:
+          Value: false
+        jackalJaco_0/rear_fender_link:
+          Value: false
+        jackalJaco_0/rear_left_wheel_link:
+          Value: false
+        jackalJaco_0/rear_mount:
+          Value: false
+        jackalJaco_0/rear_right_wheel_link:
+          Value: false
+        jackalJaco_1/base_link:
+          Value: false
+        jackalJaco_1/chassis_link:
+          Value: false
+        jackalJaco_1/d435_cam1camera_bottom_screw_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_color_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_color_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_frame_gazebo:
+          Value: false
+        jackalJaco_1/d435_cam1camera_depth_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_left_ir_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_left_ir_optical_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_link:
+          Value: false
+        jackalJaco_1/d435_cam1camera_right_ir_frame:
+          Value: false
+        jackalJaco_1/d435_cam1camera_right_ir_optical_frame:
+          Value: false
+        jackalJaco_1/front_fender_link:
+          Value: false
+        jackalJaco_1/front_left_wheel_link:
+          Value: false
+        jackalJaco_1/front_mount:
+          Value: false
+        jackalJaco_1/front_right_wheel_link:
+          Value: false
+        jackalJaco_1/imu_link:
+          Value: false
+        jackalJaco_1/j2n6s300_end_effector:
+          Value: false
+        jackalJaco_1/j2n6s300_link_1:
+          Value: false
+        jackalJaco_1/j2n6s300_link_2:
+          Value: false
+        jackalJaco_1/j2n6s300_link_3:
+          Value: false
+        jackalJaco_1/j2n6s300_link_4:
+          Value: false
+        jackalJaco_1/j2n6s300_link_5:
+          Value: false
+        jackalJaco_1/j2n6s300_link_6:
+          Value: false
+        jackalJaco_1/j2n6s300_link_base:
+          Value: false
+        jackalJaco_1/jaco_base_link:
+          Value: false
+        jackalJaco_1/mid_mount:
+          Value: false
+        jackalJaco_1/navsat_link:
+          Value: false
+        jackalJaco_1/odom:
+          Value: false
+        jackalJaco_1/rear_fender_link:
+          Value: false
+        jackalJaco_1/rear_left_wheel_link:
+          Value: false
+        jackalJaco_1/rear_mount:
+          Value: false
+        jackalJaco_1/rear_right_wheel_link:
+          Value: false
+        world:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        world:
+          jackalJaco_0/odom:
+            jackalJaco_0/base_link:
+              jackalJaco_0/chassis_link:
+                jackalJaco_0/front_fender_link:
+                  {}
+                jackalJaco_0/front_left_wheel_link:
+                  {}
+                jackalJaco_0/front_right_wheel_link:
+                  {}
+                jackalJaco_0/imu_link:
+                  {}
+                jackalJaco_0/mid_mount:
+                  jackalJaco_0/front_mount:
+                    {}
+                  jackalJaco_0/rear_mount:
+                    {}
+                jackalJaco_0/navsat_link:
+                  {}
+                jackalJaco_0/rear_fender_link:
+                  {}
+                jackalJaco_0/rear_left_wheel_link:
+                  {}
+                jackalJaco_0/rear_right_wheel_link:
+                  {}
+              jackalJaco_0/d435_cam1camera_bottom_screw_frame:
+                jackalJaco_0/d435_cam1camera_link:
+                  jackalJaco_0/d435_cam1camera_depth_frame:
+                    jackalJaco_0/d435_cam1camera_color_frame:
+                      jackalJaco_0/d435_cam1camera_color_optical_frame:
+                        {}
+                    jackalJaco_0/d435_cam1camera_depth_optical_frame:
+                      {}
+                    jackalJaco_0/d435_cam1camera_left_ir_frame:
+                      jackalJaco_0/d435_cam1camera_left_ir_optical_frame:
+                        {}
+                    jackalJaco_0/d435_cam1camera_right_ir_frame:
+                      jackalJaco_0/d435_cam1camera_right_ir_optical_frame:
+                        {}
+                  jackalJaco_0/d435_cam1camera_depth_frame_gazebo:
+                    {}
+              jackalJaco_0/jaco_base_link:
+                jackalJaco_0/j2n6s300_link_base:
+                  jackalJaco_0/j2n6s300_link_1:
+                    jackalJaco_0/j2n6s300_link_2:
+                      jackalJaco_0/j2n6s300_link_3:
+                        jackalJaco_0/j2n6s300_link_4:
+                          jackalJaco_0/j2n6s300_link_5:
+                            jackalJaco_0/j2n6s300_link_6:
+                              jackalJaco_0/j2n6s300_end_effector:
+                                {}
+          jackalJaco_1/odom:
+            jackalJaco_1/base_link:
+              jackalJaco_1/chassis_link:
+                jackalJaco_1/front_fender_link:
+                  {}
+                jackalJaco_1/front_left_wheel_link:
+                  {}
+                jackalJaco_1/front_right_wheel_link:
+                  {}
+                jackalJaco_1/imu_link:
+                  {}
+                jackalJaco_1/mid_mount:
+                  jackalJaco_1/front_mount:
+                    {}
+                  jackalJaco_1/rear_mount:
+                    {}
+                jackalJaco_1/navsat_link:
+                  {}
+                jackalJaco_1/rear_fender_link:
+                  {}
+                jackalJaco_1/rear_left_wheel_link:
+                  {}
+                jackalJaco_1/rear_right_wheel_link:
+                  {}
+              jackalJaco_1/d435_cam1camera_bottom_screw_frame:
+                jackalJaco_1/d435_cam1camera_link:
+                  jackalJaco_1/d435_cam1camera_depth_frame:
+                    jackalJaco_1/d435_cam1camera_color_frame:
+                      jackalJaco_1/d435_cam1camera_color_optical_frame:
+                        {}
+                    jackalJaco_1/d435_cam1camera_depth_optical_frame:
+                      {}
+                    jackalJaco_1/d435_cam1camera_left_ir_frame:
+                      jackalJaco_1/d435_cam1camera_left_ir_optical_frame:
+                        {}
+                    jackalJaco_1/d435_cam1camera_right_ir_frame:
+                      jackalJaco_1/d435_cam1camera_right_ir_optical_frame:
+                        {}
+                  jackalJaco_1/d435_cam1camera_depth_frame_gazebo:
+                    {}
+              jackalJaco_1/jaco_base_link:
+                jackalJaco_1/j2n6s300_link_base:
+                  jackalJaco_1/j2n6s300_link_1:
+                    jackalJaco_1/j2n6s300_link_2:
+                      jackalJaco_1/j2n6s300_link_3:
+                        jackalJaco_1/j2n6s300_link_4:
+                          jackalJaco_1/j2n6s300_link_5:
+                            jackalJaco_1/j2n6s300_link_6:
+                              jackalJaco_1/j2n6s300_end_effector:
+                                {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /target_marker/update
+      Value: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: false
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /auto_target_marker/update
+      Value: false
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /model_mode_marker/update
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: false
+      Name: Map
+      Topic: /occupancy_grid
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /ego_grid_occupancy
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /pc2_scan_bin
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Class: octomap_rviz_plugin/ColorOccupancyGrid
+      Enabled: true
+      Max. Height Display: 3.4028234663852886e+38
+      Max. Octree Depth: 16
+      Min. Height Display: -3.4028234663852886e+38
+      Name: ColorOccupancyGrid
+      Octomap Topic: /octomap_scan
+      Queue Size: 5
+      Value: true
+      Voxel Alpha: 1
+      Voxel Coloring: Z-Axis
+      Voxel Rendering: Occupied Voxels
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /occupancy_distances_base_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /occupancy_distances_arm_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /points_on_robot
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 0
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /self_collision_distances_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 0
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Queue Size: 10
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /move_base_simple/goal
+      Unreliable: false
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /mobile_manipulator/optimizedStateTrajectory
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /mobile_manipulator_goal
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /mobile_manipulator_target
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /debug_visu
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 186; 189; 182
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 10000
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 3.889058828353882
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.07714345306158066
+        Y: -0.04144350811839104
+        Z: 0.5177860856056213
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5497972965240479
+      Target Frame: <Fixed Frame>
+      Yaw: 1.6029189825057983
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 773
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 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
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1440
+  X: 2167
+  Y: 112
diff --git a/mobiman_simulation/scripts/mobiman_framework_launch.py b/mobiman_simulation/scripts/mobiman_framework_launch.py
index ecfb2cd..cd72b34 100755
--- a/mobiman_simulation/scripts/mobiman_framework_launch.py
+++ b/mobiman_simulation/scripts/mobiman_framework_launch.py
@@ -185,13 +185,15 @@ def load_config_file(config_file_path, namespace=""):
     ## Launch Rviz
     if flag_rviz:
 
-        rviz_path = mobiman_launch_path + "utilities/rviz.launch"
-        rviz_args = [rviz_path,
-                     'rviz_config_path:=' + str(rviz_config_path)]
+        for i, rns in enumerate(robot_ns_vec):
+            rviz_path = mobiman_launch_path + "utilities/rviz.launch"
+            rviz_args = [rviz_path,
+                         'robot_ns:=' + str(rns),
+                         'rviz_config_path:=' + str(rviz_config_path) + '_' + str(i)]
 
-        rviz_launch = [ (roslaunch.rlutil.resolve_launch_arguments(rviz_args)[0], rviz_args[1:]) ]
-        rviz = roslaunch.parent.ROSLaunchParent(uuid, rviz_launch)
-        rviz.start()
+            rviz_launch = [ (roslaunch.rlutil.resolve_launch_arguments(rviz_args)[0], rviz_args[1:]) ]
+            rviz = roslaunch.parent.ROSLaunchParent(uuid, rviz_launch)
+            rviz.start()
 
         print("[mobiman_framework_launch:: __main__ ] Launched Rviz!")
         rospy.sleep(1)
@@ -213,28 +215,32 @@ def load_config_file(config_file_path, namespace=""):
     ## Launch Map Server
     if flag_map_server:
 
-        map_server_path = mobiman_launch_path + "utilities/map_server.launch"
-        map_server_args = [map_server_path,
-                           'config_map_server:=' + str(config_map_server)]
+        for i, rns in enumerate(robot_ns_vec):
+            map_server_path = mobiman_launch_path + "utilities/map_server.launch"
+            map_server_args = [map_server_path,
+                               'robot_ns:=' + str(rns),
+                               'config_map_server:=' + str(config_map_server)]
 
-        map_server_launch = [ (roslaunch.rlutil.resolve_launch_arguments(map_server_args)[0], map_server_args[1:]) ]
-        map_server = roslaunch.parent.ROSLaunchParent(uuid, map_server_launch)
-        map_server.start()
+            map_server_launch = [ (roslaunch.rlutil.resolve_launch_arguments(map_server_args)[0], map_server_args[1:]) ]
+            map_server = roslaunch.parent.ROSLaunchParent(uuid, map_server_launch)
+            map_server.start()
 
         print("[mobiman_framework_launch:: __main__ ] Launched Map Server!")
         rospy.sleep(1) 
 
-    ### NUA TODO: Change the name as goal utility!
+    ### NUA TODO: Change the name as goal server!
     ## Launch ocs2 target 
     if flag_goal_server:
 
-        goal_server_path = mobiman_launch_path + "utilities/ocs2_target.launch"
-        goal_server_args = [goal_server_path,
-                           'task_config_path:=' + str(task_config_path)]
+        for i, rns in enumerate(robot_ns_vec):
+            goal_server_path = mobiman_launch_path + "utilities/ocs2_target.launch"
+            goal_server_args = [goal_server_path,
+                                'robot_ns:=' + str(rns),
+                                'task_config_path:=' + str(task_config_path)]
 
-        goal_server_launch = [ (roslaunch.rlutil.resolve_launch_arguments(goal_server_args)[0], goal_server_args[1:]) ]
-        goal_server = roslaunch.parent.ROSLaunchParent(uuid, goal_server_launch)
-        goal_server.start()
+            goal_server_launch = [ (roslaunch.rlutil.resolve_launch_arguments(goal_server_args)[0], goal_server_args[1:]) ]
+            goal_server = roslaunch.parent.ROSLaunchParent(uuid, goal_server_launch)
+            goal_server.start()
 
         print("[mobiman_framework_launch:: __main__ ] Launched Goal Server!")
         rospy.sleep(1)
@@ -242,230 +248,21 @@ def load_config_file(config_file_path, namespace=""):
     ## Launch mobiman 
     if flag_mobiman:
 
-        mobiman_path = mobiman_launch_path + "utilities/ocs2_m4.launch"
-        mobiman_args = [mobiman_path,
-                        'task_config_path:=' + str(task_config_path),
-                        'urdf_path:=' + str(urdf_path_ocs2),
-                        'lib_path:=' + str(lib_path),
-                        'collision_points_config_path:=' + str(collision_points_config_path)]
-
-        mobiman_launch = [ (roslaunch.rlutil.resolve_launch_arguments(mobiman_args)[0], mobiman_args[1:]) ]
-        mobiman = roslaunch.parent.ROSLaunchParent(uuid, mobiman_launch)
-        mobiman.start()
+        for i, rns in enumerate(robot_ns_vec):
+            mobiman_path = mobiman_launch_path + "utilities/ocs2_m4.launch"
+            mobiman_args = [mobiman_path,
+                            'robot_ns:=' + str(rns),
+                            'task_config_path:=' + str(task_config_path),
+                            'urdf_path:=' + str(urdf_path_ocs2),
+                            'lib_path:=' + str(lib_path),
+                            'collision_points_config_path:=' + str(collision_points_config_path)]
+
+            mobiman_launch = [ (roslaunch.rlutil.resolve_launch_arguments(mobiman_args)[0], mobiman_args[1:]) ]
+            mobiman = roslaunch.parent.ROSLaunchParent(uuid, mobiman_launch)
+            mobiman.start()
 
         print("[mobiman_framework_launch:: __main__ ] Launched Mobiman!")
         rospy.sleep(1)
 
-    '''
-    ## Task-Nav Parameters
-    world_name = rospy.get_param('world_name', "")
-    world_frame_name = rospy.get_param('world_frame_name', "")
-    robot_frame_name = rospy.get_param('robot_frame_name', "")
-
-    print("[mobiman_framework_launch:: __main__ ] Task-Nav Parameters:")
-    print("[mobiman_framework_launch:: __main__ ] world_name: " + str(world_name))
-    print("[mobiman_framework_launch:: __main__ ] world_frame_name: " + str(world_frame_name))
-
-    n_goal = rospy.get_param('n_goal', "")
-    print("[mobiman_framework_launch:: __main__ ] n_goal: " + str(n_goal))
-    
-    goals_x = []
-    goals_y = []
-    goals_z = []
-    goals_yaw = []
-
-    for i in range(1, n_goal+1):
-
-        goal_name = "goal" + str(i)
-
-        goal_x = rospy.get_param(goal_name + '_x', 0.0)
-        goal_y = rospy.get_param(goal_name + '_y', 0.0)
-        goal_z = rospy.get_param(goal_name + '_z', 0.0)
-        goal_yaw = rospy.get_param(goal_name + '_yaw', 0.0)
-
-        goals_x.append(goal_x)
-        goals_y.append(goal_y)
-        goals_z.append(goal_z)
-        goals_yaw.append(goal_yaw)
-
-        print("[mobiman_framework_launch:: __main__ ] " + goal_name + ": " + str(goal_x))
-        print("[mobiman_framework_launch:: __main__ ] " + goal_name + ": " + str(goal_y))
-        print("[mobiman_framework_launch:: __main__ ] " + goal_name + ": " + str(goal_z))
-        print("[mobiman_framework_launch:: __main__ ] " + goal_name + ": " + str(goal_yaw))
-
-    robot_name = rospy.get_param('robot_name', "")
-    robot_model = rospy.get_param('robot_model', "")
-    robot_init_pos_x = rospy.get_param('robot_init_pos_x', 0.0)
-    robot_init_pos_y = rospy.get_param('robot_init_pos_y', 0.0)
-    robot_init_pos_z = rospy.get_param('robot_init_pos_z', 0.0)
-    robot_init_yaw = rospy.get_param('robot_init_yaw', 0.0)
-    robot_odometry_msg = rospy.get_param('robot_odometry_msg', "")
-    robot_pose_control_msg = rospy.get_param('robot_pose_control_msg', "")
-    robot_velo_control_msg = rospy.get_param('robot_velo_control_msg', "")
-
-    print("[mobiman_framework_launch:: __main__ ] robot_name: " + str(robot_name))
-    print("[mobiman_framework_launch:: __main__ ] robot_model: " + str(robot_model))
-    print("[mobiman_framework_launch:: __main__ ] robot_init_pos_x: " + str(robot_init_pos_x))
-    print("[mobiman_framework_launch:: __main__ ] robot_init_pos_y: " + str(robot_init_pos_y))
-    print("[mobiman_framework_launch:: __main__ ] robot_init_pos_z: " + str(robot_init_pos_z))
-    print("[mobiman_framework_launch:: __main__ ] robot_init_yaw: " + str(robot_init_yaw))
-    print("[mobiman_framework_launch:: __main__ ] robot_odometry_msg: " + str(robot_odometry_msg))
-    print("[mobiman_framework_launch:: __main__ ] robot_pose_control_msg: " + str(robot_pose_control_msg))
-    print("[mobiman_framework_launch:: __main__ ] robot_velo_control_msg: " + str(robot_velo_control_msg))
-
-    ## Map Utility Parameters
-    config_map_utility = rospy.get_param('config_map_utility', "")
-
-    print("[mobiman_framework_launch:: __main__ ] Map Utility Parameters:")
-    print("[mobiman_framework_launch:: __main__ ] config_map_utility: " + str(config_map_utility))
-
-    ## mobiman Server Parameters
-    config_mobiman_server = rospy.get_param('config_mobiman_server', "")
-
-    print("[mobiman_framework_launch:: __main__ ] mobiman-Server Parameters:")
-    print("[mobiman_framework_launch:: __main__ ] config_mobiman_server: " + str(config_mobiman_server))
-
-    ## Launch Simulation
-    sim_paused = "false"
-
-    if sim == "gazebo":
-        
-        if robot_name == "firefly":
-
-            # Launch Mav-Garden Gazebo
-            mav_garden_path = mobiman_launch_path + "utilities/mav_garden.launch"
-            mav_garden_args = [mav_garden_path,
-                               'world_name:=' + str(world_name),
-                               'world_frame_name:=' + str(world_frame_name),
-                               'robot_name:=' + str(robot_name),
-                               'robot_model:=' + str(robot_model),
-                               'robot_init_pos_x:=' + str(robot_init_pos_x),
-                               'robot_init_pos_y:=' + str(robot_init_pos_y),
-                               'robot_init_pos_z:=' + str(robot_init_pos_z),
-                               'robot_odometry_msg:=' + str(robot_odometry_msg),
-                               'robot_pose_control_msg:=' + str(robot_pose_control_msg)]
-
-            mav_garden_launch = [ (roslaunch.rlutil.resolve_launch_arguments(mav_garden_args)[0], mav_garden_args[1:]) ]
-            mav_garden = roslaunch.parent.ROSLaunchParent(uuid, mav_garden_launch)
-            mav_garden.start()
-
-            print("[mobiman_framework_launch:: __main__ ] Launched Mav-Garden!")
-
-        else:
-
-            if robot_name == "stretch":
-                sim_paused = "true"
-
-            # Launch Mobile-Garden Gazebo
-            mobile_garden_path = mobiman_launch_path + "utilities/mobile_garden.launch"
-            mobile_garden_args = [mobile_garden_path,
-                                  'sim_paused:=' + str(sim_paused),
-                                  'world_name:=' + str(world_name),
-                                  'robot_name:=' + str(robot_name),
-                                  'robot_model:=' + str(robot_model),
-                                  'robot0_init_pos_x:=' + str(robot_init_pos_x),
-                                  'robot0_init_pos_y:=' + str(robot_init_pos_y),
-                                  'robot0_init_yaw:=' + str(robot_init_yaw)]
-
-            mobile_garden_launch = [ (roslaunch.rlutil.resolve_launch_arguments(mobile_garden_args)[0], mobile_garden_args[1:]) ]
-            mobile_garden = roslaunch.parent.ROSLaunchParent(uuid, mobile_garden_launch)
-            mobile_garden.start()
-
-            print("[mobiman_framework_launch:: __main__ ] Launched Mobile-Garden in Gazebo!")
-        
-        rospy.sleep(2)
-
-    elif sim == "igibson":
-
-        ns1 = "turtlebot2_0"
-
-        # Launch Mobile-Garden iGibson
-        mobile_garden_path = mobiman_launch_path + "utilities/mobile_garden_igibson.launch"
-        mobile_garden_args = [mobile_garden_path,
-                                'ns1:=' + str(ns1),
-                                'world_frame_name:=' + str(world_frame_name)]
-
-        mobile_garden_launch = [ (roslaunch.rlutil.resolve_launch_arguments(mobile_garden_args)[0], mobile_garden_args[1:]) ]
-        mobile_garden = roslaunch.parent.ROSLaunchParent(uuid, mobile_garden_launch)
-        mobile_garden.start()
-
-        print("[mobiman_framework_launch:: __main__ ] Launched Mobile-Garden in iGibson!")
-    
-    else:
-        print("[mobiman_framework_launch:: __main__ ] Empty or wrong simulator name!")
-
-    ## Launch Rviz
-    if rviz_flag:
-
-        rviz_path = mobiman_launch_path + "utilities/rviz.launch"
-        rviz_args = [rviz_path,
-                     'robot_name:=' + str(robot_name)]
-
-        rviz_launch = [ (roslaunch.rlutil.resolve_launch_arguments(rviz_args)[0], rviz_args[1:]) ]
-        rviz = roslaunch.parent.ROSLaunchParent(uuid, rviz_launch)
-        rviz.start()
-
-        print("[mobiman_framework_launch:: __main__ ] Launched Rviz!")
-        rospy.sleep(1)
-
-    ## Wait for simulation to be ready!
-    tfBuffer = tf2_ros.Buffer()
-    listener = tf2_ros.TransformListener(tfBuffer)
-    print("Waiting the transform between " + world_frame_name + " and " + robot_frame_name + "...")
-    trans = None
-    while (not rospy.is_shutdown()) and (not trans):
-        try:
-            trans = tfBuffer.lookup_transform(world_frame_name, robot_frame_name, rospy.Time(0))
-        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as ex:
-            rospy.logwarn(str(ex))
-            rospy.sleep(1.0)
-
-    ## Launch Map Utility Server
-    if map_utility_flag:
-
-        map_utility_path = mobiman_launch_path + "utilities/map_utility_server.launch"
-        map_utility_args = [map_utility_path,
-                            'config_map_utility:=' + str(config_map_utility)]
-
-        map_utility_launch = [ (roslaunch.rlutil.resolve_launch_arguments(map_utility_args)[0], map_utility_args[1:]) ]
-        map_utility = roslaunch.parent.ROSLaunchParent(uuid, map_utility_launch)
-        map_utility.start()
-
-        print("[mobiman_framework_launch:: __main__ ] Launched Map Utility Server!")
-        rospy.sleep(1)
-    
-    ## Launch mobiman Server
-    if mobiman_server_flag:
-
-        mobiman_server_path = mobiman_launch_path + "mobiman_server.launch"
-        mobiman_server_args = [mobiman_server_path,
-                                   'config_mobiman_server:=' + str(config_mobiman_server)]
-
-        mobiman_server_launch = [ (roslaunch.rlutil.resolve_launch_arguments(mobiman_server_args)[0], mobiman_server_args[1:]) ]
-        mobiman_server = roslaunch.parent.ROSLaunchParent(uuid, mobiman_server_launch)
-        mobiman_server.start()
-
-        print("[mobiman_framework_launch:: __main__ ] Launched mobiman Server!")
-        rospy.sleep(1)
-
-    ## Launch mobiman-DRL Training/Testing
-    drl_service_flag = rospy.get_param('drl_service_flag', False)
-    mode = rospy.get_param('mode', "")
-    
-    print("[mobiman_framework_launch:: __main__ ] drl_service_flag: " + str(drl_service_flag))
-    print("[mobiman_framework_launch:: __main__ ] mode: " + str(mode))
-    
-    if drl_service_flag:
-
-        mobiman_drl_path = mobiman_launch_path + "mobiman_drl/mobiman_drl_" + mode + ".launch"
-        mobiman_drl_args = [mobiman_drl_path]
-
-        mobiman_drl_launch = [ (roslaunch.rlutil.resolve_launch_arguments(mobiman_drl_args)[0]) ]
-        mobiman_drl = roslaunch.parent.ROSLaunchParent(uuid, mobiman_drl_launch)
-        mobiman_drl.start()
-
-        print("[mobiman_framework_launch:: __main__ ] Launched mobiman-DRL: " + mode + "!")
-    '''
-
     while (not rospy.is_shutdown()):
         pass
\ No newline at end of file
diff --git a/mobiman_simulation/src/map_server.cpp b/mobiman_simulation/src/map_server.cpp
index 11d85f4..3fb95e6 100755
--- a/mobiman_simulation/src/map_server.cpp
+++ b/mobiman_simulation/src/map_server.cpp
@@ -15,7 +15,7 @@
 
 int main(int argc, char** argv)
 {
-  cout << "[map_server::main] START" << endl;
+  //cout << "[map_server::main] START" << endl;
 
   // INITIALIZE ROS
   ros::init(argc, argv, "map_server");
@@ -35,6 +35,8 @@ int main(int argc, char** argv)
   double map_resolution, egrid_resolution, egrid_occ_threshold;
   geometry_msgs::Point egrid_bbx_min, egrid_bbx_max;
 
+  bool printOutFlag = false;
+
   pnh.param<string>("/world_frame_name", world_frame_name, "");
   pnh.param<string>("/gz_model_msg_name", gz_model_msg_name, "");
   if (!pnh.getParam("/name_pkgs_ign", name_pkgs_ign))
@@ -65,27 +67,30 @@ int main(int argc, char** argv)
   pnh.param<double>("/egrid_bbx_max_z", egrid_bbx_max.z, 0.0);
   pnh.param<double>("/egrid_occ_threshold", egrid_occ_threshold, 0.0);
   
-  cout << "[map_server::main] world_frame_name: " << world_frame_name << endl;
-  cout << "[map_server::main] gz_model_msg_name: " << gz_model_msg_name << endl;
-  cout << "[map_server::main] name_pkgs_ign: " << endl;
-  for (size_t i = 0; i < name_pkgs_ign.size(); i++)
-  {
-    cout << i << " -> " << name_pkgs_ign[i] << endl;
-  }
-  cout << "[map_server::main] name_pkgs_man: " << endl;
-  for (size_t i = 0; i < name_pkgs_man.size(); i++)
+  if (printOutFlag)
   {
-    cout << i << " -> " << name_pkgs_man[i] << endl;
+    cout << "[map_server::main] world_frame_name: " << world_frame_name << endl;
+    cout << "[map_server::main] gz_model_msg_name: " << gz_model_msg_name << endl;
+    cout << "[map_server::main] name_pkgs_ign: " << endl;
+    for (size_t i = 0; i < name_pkgs_ign.size(); i++)
+    {
+      cout << i << " -> " << name_pkgs_ign[i] << endl;
+    }
+    cout << "[map_server::main] name_pkgs_man: " << endl;
+    for (size_t i = 0; i < name_pkgs_man.size(); i++)
+    {
+      cout << i << " -> " << name_pkgs_man[i] << endl;
+    }
+    cout << "[map_server::main] egrid_frame_name: " << egrid_frame_name << endl;
+    cout << "[map_server::main] egrid_resolution: " << egrid_resolution << endl;
+    cout << "[map_server::main] egrid_bbx_min_x: " << egrid_bbx_min.x << endl;
+    cout << "[map_server::main] egrid_bbx_min_y: " << egrid_bbx_min.y << endl;
+    cout << "[map_server::main] egrid_bbx_min_z: " << egrid_bbx_min.z << endl;
+    cout << "[map_server::main] egrid_bbx_max_x: " << egrid_bbx_max.x << endl;
+    cout << "[map_server::main] egrid_bbx_max_y: " << egrid_bbx_max.y << endl;
+    cout << "[map_server::main] egrid_bbx_max_z: " << egrid_bbx_max.z << endl;
+    cout << "[map_server::main] egrid_occ_threshold: " << egrid_occ_threshold << endl;
   }
-  cout << "[map_server::main] egrid_frame_name: " << egrid_frame_name << endl;
-  cout << "[map_server::main] egrid_resolution: " << egrid_resolution << endl;
-  cout << "[map_server::main] egrid_bbx_min_x: " << egrid_bbx_min.x << endl;
-  cout << "[map_server::main] egrid_bbx_min_y: " << egrid_bbx_min.y << endl;
-  cout << "[map_server::main] egrid_bbx_min_z: " << egrid_bbx_min.z << endl;
-  cout << "[map_server::main] egrid_bbx_max_x: " << egrid_bbx_max.x << endl;
-  cout << "[map_server::main] egrid_bbx_max_y: " << egrid_bbx_max.y << endl;
-  cout << "[map_server::main] egrid_bbx_max_z: " << egrid_bbx_max.z << endl;
-  cout << "[map_server::main] egrid_occ_threshold: " << egrid_occ_threshold << endl;
 
   // Initialize Scan Utility
   ScanUtility su(nh);
@@ -205,7 +210,7 @@ int main(int argc, char** argv)
   //double map_server_dt = 0.1;
   //ros::Timer timer = nh.createTimer(ros::Duration(scan_server_dt), &MapUtility::mainCallback, &mu);
 
-  cout << "[map_server::main] END" << endl;
+  //cout << "[map_server::main] END" << endl;
 
   return 0;
 }
\ No newline at end of file
diff --git a/mobiman_simulation/src/scan_utility.cpp b/mobiman_simulation/src/scan_utility.cpp
index 2842ebf..851ac9b 100755
--- a/mobiman_simulation/src/scan_utility.cpp
+++ b/mobiman_simulation/src/scan_utility.cpp
@@ -502,10 +502,13 @@ void ScanUtility::PclPointcloudToVec(vector<double>& pcl_pc_scan_x,
     pcl_pc_scan_z.push_back(c.z);
   }
 
-  std::cout << "[ScanUtility::PclPointcloudToVec] pcl_pc_scan_ size: " << pcl_pc_scan_.size() << std::endl;
-  std::cout << "[ScanUtility::PclPointcloudToVec] pcl_pc_scan_x size: " << pcl_pc_scan_x.size() << std::endl;
-  std::cout << "[ScanUtility::PclPointcloudToVec] pcl_pc_scan_y size: " << pcl_pc_scan_y.size() << std::endl;
-  std::cout << "[ScanUtility::PclPointcloudToVec] pcl_pc_scan_z size: " << pcl_pc_scan_z.size() << std::endl;
+  if (printOutFlag_)
+  {
+    std::cout << "[ScanUtility::PclPointcloudToVec] pcl_pc_scan_ size: " << pcl_pc_scan_.size() << std::endl;
+    std::cout << "[ScanUtility::PclPointcloudToVec] pcl_pc_scan_x size: " << pcl_pc_scan_x.size() << std::endl;
+    std::cout << "[ScanUtility::PclPointcloudToVec] pcl_pc_scan_y size: " << pcl_pc_scan_y.size() << std::endl;
+    std::cout << "[ScanUtility::PclPointcloudToVec] pcl_pc_scan_z size: " << pcl_pc_scan_z.size() << std::endl;
+  }
 }
 
 //-------------------------------------------------------------------------------------------------------
@@ -521,10 +524,13 @@ void ScanUtility::vecToPclPointcloud(vector<double>& pcl_pc_scan_x,
     pcl_pc_scan_.push_back(pcl::PointXYZ(pcl_pc_scan_x[i], pcl_pc_scan_y[i], pcl_pc_scan_z[i]));
   }
 
-  std::cout << "[ScanUtility::vecToPclPointcloud] pcl_pc_scan_ size: " << pcl_pc_scan_.size() << std::endl;
-  std::cout << "[ScanUtility::vecToPclPointcloud] pcl_pc_scan_x size: " << pcl_pc_scan_x.size() << std::endl;
-  std::cout << "[ScanUtility::vecToPclPointcloud] pcl_pc_scan_y size: " << pcl_pc_scan_y.size() << std::endl;
-  std::cout << "[ScanUtility::vecToPclPointcloud] pcl_pc_scan_z size: " << pcl_pc_scan_z.size() << std::endl;
+  if (printOutFlag_)
+  {
+    std::cout << "[ScanUtility::vecToPclPointcloud] pcl_pc_scan_ size: " << pcl_pc_scan_.size() << std::endl;
+    std::cout << "[ScanUtility::vecToPclPointcloud] pcl_pc_scan_x size: " << pcl_pc_scan_x.size() << std::endl;
+    std::cout << "[ScanUtility::vecToPclPointcloud] pcl_pc_scan_y size: " << pcl_pc_scan_y.size() << std::endl;
+    std::cout << "[ScanUtility::vecToPclPointcloud] pcl_pc_scan_z size: " << pcl_pc_scan_z.size() << std::endl;
+  } 
 }
 
 //-------------------------------------------------------------------------------------------------------
@@ -648,7 +654,7 @@ void ScanUtility::waitAndCheckForPointCloud2Message(string msg_name, double dura
 //-------------------------------------------------------------------------------------------------------
 void ScanUtility::scanner()
 {
-  std::cout << "[ScanUtility::scanner] START" << std::endl;
+  //std::cout << "[ScanUtility::scanner] START" << std::endl;
 
   waitAndCheckForPointCloud2Message(pc2_msg_name_sensor1_, 10, measured_pc2_msg_sensor1_);
   waitAndCheckForPointCloud2Message(pc2_msg_name_sensor2_, 10, measured_pc2_msg_sensor2_);
@@ -677,7 +683,7 @@ void ScanUtility::scanner()
 
   octomapToPclPointcloud();
 
-  std::cout << "[ScanUtility::scanner] pcl_pc_scan_ size: " << pcl_pc_scan_.size() << std::endl;
+  //std::cout << "[ScanUtility::scanner] pcl_pc_scan_ size: " << pcl_pc_scan_.size() << std::endl;
 
   pcl::toROSMsg(pcl_pc_scan_, pc2_msg_scan_);
 
@@ -686,7 +692,7 @@ void ScanUtility::scanner()
   publishOctMsg();
   publishPC2Msg();
 
-  std::cout << "[ScanUtility::scanner] END" << std::endl << std::endl;
+  //std::cout << "[ScanUtility::scanner] END" << std::endl << std::endl;
 }
 
 //-------------------------------------------------------------------------------------------------------
@@ -694,7 +700,7 @@ void ScanUtility::scanner()
 //-------------------------------------------------------------------------------------------------------
 void ScanUtility::writePointcloud2Data()
 {
-  std::cout << "[ScanUtility::writePointcloud2Data] START" << std::endl;
+  //std::cout << "[ScanUtility::writePointcloud2Data] START" << std::endl;
 
   json j;
   boost::filesystem::create_directories(pkg_dir_ + data_dir_);
@@ -738,7 +744,7 @@ void ScanUtility::writePointcloud2Data()
   std::ofstream o(data_path_);
   o << std::setw(4) << j << std::endl;
 
-  std::cout << "[ScanUtility::writePointcloud2Data] END" << std::endl << std::endl;
+  //std::cout << "[ScanUtility::writePointcloud2Data] END" << std::endl << std::endl;
 }
 
 //-------------------------------------------------------------------------------------------------------
@@ -746,9 +752,9 @@ void ScanUtility::writePointcloud2Data()
 //-------------------------------------------------------------------------------------------------------
 void ScanUtility::readPointcloud2Data(string& data_path)
 {
-  std::cout << "[ScanUtility::readPointcloud2Data] START" << std::endl;
-  std::cout << "[ScanUtility::readPointcloud2Data] pkg_dir_: " << pkg_dir_ << std::endl;
-  std::cout << "[ScanUtility::readPointcloud2Data] data_path: " << data_path << std::endl;
+  //std::cout << "[ScanUtility::readPointcloud2Data] START" << std::endl;
+  //std::cout << "[ScanUtility::readPointcloud2Data] pkg_dir_: " << pkg_dir_ << std::endl;
+  //std::cout << "[ScanUtility::readPointcloud2Data] data_path: " << data_path << std::endl;
 
   std::ifstream f(pkg_dir_ + data_path);
   json data = json::parse(f);
@@ -775,15 +781,18 @@ void ScanUtility::readPointcloud2Data(string& data_path)
   }
   vecToPclPointcloud(pcl_pc_scan_x, pcl_pc_scan_y, pcl_pc_scan_z);
 
-  std::cout << "[ScanUtility::readPointcloud2Data] obj_name_: " << obj_name_ << std::endl;
-  std::cout << "[ScanUtility::readPointcloud2Data] data_path_: " << data_path_ << std::endl;
-  std::cout << "[ScanUtility::readPointcloud2Data] world_frame_name_: " << world_frame_name_ << std::endl;
-  std::cout << "[ScanUtility::readPointcloud2Data] oct_resolution_: " << oct_resolution_ << std::endl;
-  std::cout << "[ScanUtility::scanner] pcl_pc_scan_ size: " << pcl_pc_scan_.size() << std::endl << std::endl;
-
+  if (printOutFlag_)
+  {
+    std::cout << "[ScanUtility::readPointcloud2Data] obj_name_: " << obj_name_ << std::endl;
+    std::cout << "[ScanUtility::readPointcloud2Data] data_path_: " << data_path_ << std::endl;
+    std::cout << "[ScanUtility::readPointcloud2Data] world_frame_name_: " << world_frame_name_ << std::endl;
+    std::cout << "[ScanUtility::readPointcloud2Data] oct_resolution_: " << oct_resolution_ << std::endl;
+    std::cout << "[ScanUtility::scanner] pcl_pc_scan_ size: " << pcl_pc_scan_.size() << std::endl << std::endl;
+  }
+  
   pcl::toROSMsg(pcl_pc_scan_, pc2_msg_scan_);
 
-  std::cout << "[ScanUtility::readPointcloud2Data] END" << std::endl << std::endl;
+  //std::cout << "[ScanUtility::readPointcloud2Data] END" << std::endl << std::endl;
 }
 
 //-------------------------------------------------------------------------------------------------------
@@ -791,9 +800,7 @@ void ScanUtility::readPointcloud2Data(string& data_path)
 //-------------------------------------------------------------------------------------------------------
 void ScanUtility::readObjBbxDim(string& data_path)
 {
-  std::cout << "[ScanUtility::readObjBbxDim] START" << std::endl;
-  std::cout << "[ScanUtility::readObjBbxDim] pkg_dir_: " << pkg_dir_ << std::endl;
-  std::cout << "[ScanUtility::readObjBbxDim] data_path: " << data_path << std::endl;
+  //std::cout << "[ScanUtility::readObjBbxDim] START" << std::endl;
 
   std::ifstream f(pkg_dir_ + data_path);
   json data = json::parse(f);
@@ -810,12 +817,17 @@ void ScanUtility::readObjBbxDim(string& data_path)
   obj_dim_.y = data["obj_dim"]["y"];
   obj_dim_.z = data["obj_dim"]["z"];
 
-  std::cout << "[ScanUtility::readObjBbxDim] obj_bbx_min_: " << std::endl;
-  print(obj_bbx_min_);
-  std::cout << "[ScanUtility::readObjBbxDim] obj_bbx_max_: " << std::endl;
-  print(obj_bbx_max_);
-  std::cout << "[ScanUtility::readObjBbxDim] obj_dim_: " << std::endl;
-  print(obj_dim_);
+  if (printOutFlag_)
+  {
+    std::cout << "[ScanUtility::readObjBbxDim] pkg_dir_: " << pkg_dir_ << std::endl;
+    std::cout << "[ScanUtility::readObjBbxDim] data_path: " << data_path << std::endl;
+    std::cout << "[ScanUtility::readObjBbxDim] obj_bbx_min_: " << std::endl;
+    print(obj_bbx_min_);
+    std::cout << "[ScanUtility::readObjBbxDim] obj_bbx_max_: " << std::endl;
+    print(obj_bbx_max_);
+    std::cout << "[ScanUtility::readObjBbxDim] obj_dim_: " << std::endl;
+    print(obj_dim_);
+  }
 
-  std::cout << "[ScanUtility::readObjBbxDim] END" << std::endl;
+  //std::cout << "[ScanUtility::readObjBbxDim] END" << std::endl;
 }