diff --git a/mobiman_simulation/config/task/task_jackal_jaco.info b/mobiman_simulation/config/task/task_jackal_jaco.info
index 591548d..b5ce552 100644
--- a/mobiman_simulation/config/task/task_jackal_jaco.info
+++ b/mobiman_simulation/config/task/task_jackal_jaco.info
@@ -42,6 +42,7 @@ model_information
     [5] j2n6s300_joint_6
   }
 
+  worldFrame                       "world"
   ; base frame of the robot (from URDF)
   baseFrame                       "base_link"
   ; end-effector frame of the robot (from URDF)
@@ -55,6 +56,8 @@ model_information
 
   baseControlMsg                  "/jackal_velocity_controller/cmd_vel"
   armControlMsg                   "/arm_controller/command"
+  occupancyDistanceMsg            "occupancy_distances"
+  octomapMsg                      "/octomap_scan"
   logSavePathRel                  "dataset/ocs2/"
 }
 
diff --git a/mobiman_simulation/config/voxblox.yaml b/mobiman_simulation/config/voxblox.yaml
index 2260615..04c6a8c 100644
--- a/mobiman_simulation/config/voxblox.yaml
+++ b/mobiman_simulation/config/voxblox.yaml
@@ -1,11 +1,11 @@
-tsdf_voxel_size: 0.1
-tsdf_voxels_per_side: 16
-#publish_esdf_map: true
-#publish_pointclouds: true
-#use_tf_transforms: true
-#allow_clear: true
-#update_mesh_every_n_sec: 0.05
-#clear_sphere_for_planning: false
+#tsdf_voxel_size: 0.1
+#tsdf_voxels_per_side: 16
+##publish_esdf_map: true
+##publish_pointclouds: true
+##use_tf_transforms: true
+##allow_clear: true
+##update_mesh_every_n_sec: 0.05
+##clear_sphere_for_planning: false
 world_frame: world
 
 #collision_points: [[],[],[],[[0.0, 0.5], [0.25, 0.2], [0.5, 0.2], [0.75, 0.2], [1.0, 0.2]],[[0.0, 0.2], [0.25, 0.2], [0.5, 0.2], [0.75, 0.2], [1.0, 0.2]],[],[],[]]
diff --git a/mobiman_simulation/launch/utilities/distance_server.launch b/mobiman_simulation/launch/utilities/distance_server.launch
index ff438f7..0189ad8 100644
--- a/mobiman_simulation/launch/utilities/distance_server.launch
+++ b/mobiman_simulation/launch/utilities/distance_server.launch
@@ -17,6 +17,10 @@
 
   <!-- PUBLISH CONFIGURATION PARAMETERS -->
   <rosparam file="$(find mobiman_simulation)/config/$(arg config_distance_server).yaml"/>
+  <rosparam file="$(find mobiman_simulation)/config/voxblox.yaml"/>
+
+  <!-- The task file for the mpc. -->
+  <arg name="taskFile"    default="$(find mobiman_simulation)/config/task/task_$(arg robot).info" />
 
   <!-- The URDF model of the robot -->
   <!--arg name="urdfFile"    default="$(find mobiman_simulation)/urdf/jackal_fixedWheel_ur5.urdf" /-->
diff --git a/mobiman_simulation/launch/utilities/map_server.launch b/mobiman_simulation/launch/utilities/map_server.launch
index fe21a4f..59ecedb 100644
--- a/mobiman_simulation/launch/utilities/map_server.launch
+++ b/mobiman_simulation/launch/utilities/map_server.launch
@@ -12,7 +12,7 @@
   <rosparam file="$(find mobiman_simulation)/config/voxblox.yaml"/>
 
   <node name="map_server" pkg="mobiman_simulation" type="map_server" output="screen" >
-    <remap from="pointcloud" to="pc2_scan"/>
+    <!--remap from="pointcloud" to="pc2_scan"/>
     <remap from="map_server/esdf_map_out" to="esdf_map" />    
     <param name="tsdf_voxel_size" value="0.1" />
     <param name="tsdf_voxels_per_side" value="16" />
@@ -22,6 +22,6 @@
     <param name="allow_clear" value="true" />
     <param name="update_mesh_every_n_sec" value="0.05" />
     <param name="clear_sphere_for_planning" value="false" />
-    <param name="world_frame" value="world" />
+    <param name="world_frame" value="world" /-->
   </node>
 </launch>
\ No newline at end of file
diff --git a/mobiman_simulation/launch/utilities/ocs2_m4.launch b/mobiman_simulation/launch/utilities/ocs2_m4.launch
index 66c66b9..379caad 100644
--- a/mobiman_simulation/launch/utilities/ocs2_m4.launch
+++ b/mobiman_simulation/launch/utilities/ocs2_m4.launch
@@ -3,7 +3,7 @@
 <launch>
       <!-- Enable rviz visualization -->
       <arg name="rviz"        default="true" />
-      <arg name="simulation"  value="false" />
+      <arg name="simulation"  value="true" />
 
       <!-- Set nodes on debug mode -->
       <!--arg name="debug"       default="false" /-->
@@ -15,11 +15,13 @@
             jackal_jaco
       -->
       <arg name="robot"       default="jackal_jaco" />
+      
       <!-- <arg name="taskFile"    default="" /> -->
       <!-- The task file for the mpc. -->
       <group if="$(arg simulation)">
             <param name="taskFile"    value="$(find mobiman_simulation)/config/task/task_$(arg robot).info" />
       </group>
+
       <group unless="$(arg simulation)">
             <param name="taskFile"    value="$(find mobiman_simulation)/config/task/exp_$(arg robot).info" />
       </group>
diff --git a/mobiman_simulation/rviz/mobile_manipulator_ocs2_gazebo.rviz b/mobiman_simulation/rviz/mobile_manipulator_ocs2_gazebo.rviz
index fd78d8e..64500b1 100644
--- a/mobiman_simulation/rviz/mobile_manipulator_ocs2_gazebo.rviz
+++ b/mobiman_simulation/rviz/mobile_manipulator_ocs2_gazebo.rviz
@@ -5,6 +5,8 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /TF1/Frames1
+        - /MarkerArray1
+        - /MarkerArray2
       Splitter Ratio: 0.5
     Tree Height: 790
   - Class: rviz/Selection
@@ -503,17 +505,19 @@ Visualization Manager:
       Min. Height Display: -3.4028234663852886e+38
       Name: ColorOccupancyGrid
       Octomap Topic: /octomap_scan
-      Queue Size: 5
+      Queue Size: 100
       Value: true
       Voxel Alpha: 1
       Voxel Coloring: Z-Axis
       Voxel Rendering: Occupied Voxels
     - Class: rviz/MarkerArray
       Enabled: true
-      Marker Topic: /occ_dist_array
+      Marker Topic: /occupancy_distances
       Name: MarkerArray
       Namespaces:
-        {}
+        ccupancy_distance_: true
+        cupancy_distance_: true
+        occupancy_distance_: true
       Queue Size: 100
       Value: true
     - Class: rviz/MarkerArray
@@ -521,7 +525,7 @@ Visualization Manager:
       Marker Topic: /points_on_robot
       Name: MarkerArray
       Namespaces:
-        {}
+        "": true
       Queue Size: 100
       Value: true
     - Alpha: 1
@@ -577,7 +581,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 4.527034759521484
+      Distance: 4.945938587188721
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -585,17 +589,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 0.359544962644577
-        Y: -0.12735363841056824
-        Z: 0.8270843625068665
+        X: 0.25343820452690125
+        Y: -1.588194489479065
+        Z: 0.3961728811264038
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.8547976613044739
+      Pitch: 0.544798731803894
       Target Frame: <Fixed Frame>
-      Yaw: 1.6060612201690674
+      Yaw: 1.5610700845718384
     Saved: ~
 Window Geometry:
   Displays:
diff --git a/mobiman_simulation/scripts/plot_ocs2_log.py b/mobiman_simulation/scripts/plot_ocs2_log.py
index 2edff57..95e81ae 100755
--- a/mobiman_simulation/scripts/plot_ocs2_log.py
+++ b/mobiman_simulation/scripts/plot_ocs2_log.py
@@ -79,7 +79,7 @@ def plot_log_state(
     plt.close()
 
 def plot_log_input(   
-        data_x, data1_y, data2_y=None, 
+        data_x=None, data1_y=None, data2_y=None, 
         data3_x=None, data3_y=None,
         input_index=0, state_index=0, time_index=-1,
         limit_min=None, limit_max=None, 
@@ -89,7 +89,8 @@ def plot_log_input(
         fig_num=0, save_path='plot_ocs2_log.png'):
     
     fig = plt.figure()
-    plt.plot(data_x, data1_y[input_index], color=color1, label=label_data1)
+    if  data1_y is not None:
+        plt.plot(data_x, data1_y[input_index], color=color1, label=label_data1)
 
     if  data2_y is not None:
         plt.plot(data_x, data2_y[input_index], color=color2, label=label_data2)
@@ -239,7 +240,7 @@ def extract_and_plot_log(log_path, log_name, mpc_ts_idx=0, save_prefix='',save_p
     state_pos_arm = np.zeros((state_arm_size, data_size))
     for i, single_state in enumerate(state_pos_data):
         for j, ss in enumerate(single_state[state_arm_offset:]):
-            state_pos_arm[j][i] = ss
+            state_pos_arm[j][i] = ss * 180 / math.pi
 
     input_size = state_velo_base_data_size + state_arm_size
     ioff = 0
@@ -248,7 +249,7 @@ def extract_and_plot_log(log_path, log_name, mpc_ts_idx=0, save_prefix='',save_p
     input = np.zeros((input_size, data_size))
     for i, single_input in enumerate(input_data):
         for j, si in enumerate(single_input):
-            input[ioff+j][i] = si
+            input[ioff+j][i] = si * 180 / math.pi
 
     state = np.vstack([state_velo_base, state_pos_arm])
 
@@ -362,16 +363,24 @@ def extract_and_plot_log(log_path, log_name, mpc_ts_idx=0, save_prefix='',save_p
             idxi = input_arm_offset + i
             idxs = state_arm_offset + i
             
-            state[idxi] *= 180 / math.pi
-            input[idxi] *= 180 / math.pi
+            #state[idxi] *= 180 / math.pi
+            #input[idxi] *= 180 / math.pi
 
             plot_log_input(
                 time, state, input, mpc_time, mpc_state, 
                 input_index=idxi, state_index=idxs, time_index=-1,
-                label_data1='State', label_data2='Command input', label_data3='MPC input',
+                label_data1='State', label_data2='Command input', label_data3='MPC state estimate',
                 label_x="Time", label_y='Arm joint angle [deg]',
                 fig_num=3+i,
-                save_path=save_path + 'arm_joint' + str(i) + '.png')
+                save_path=save_path + 'arm_joint' + str(i+1) + '_pos.png')
+            
+            plot_log_input(
+                data3_x=mpc_time, data3_y=mpc_input, 
+                state_index=idxi, time_index=-1,
+                label_data3='MPC input',
+                label_x="Time", label_y='Arm joint angular velocity [deg/sec]',
+                fig_num=3+i,
+                save_path=save_path + 'arm_joint' + str(i+1) + '_velo.png')
             
     return time, state, input, state_pos_base, mpc_time, mpc_state, mpc_input, dt, input_arm_offset, state_arm_offset
 
@@ -417,8 +426,8 @@ def extract_and_plot_log_in_folder(log_path, mpc_ts_idx=0, save_prefix='', save_
                 time_diff = time_concat[-1] - time_tmp[0]
                 time_tmp += time_diff + dt
 
-            if mpc_time_tmp[0][0] <= mpc_time_concat[-1][0]:
-                time_diff = mpc_time_concat[-1][0] - mpc_time_tmp[0][0]
+            if mpc_time_tmp[0][0][0] <= mpc_time_concat[-1][0][0]:
+                time_diff = mpc_time_concat[-1][0][0] - mpc_time_tmp[0][0][0]
                 for tim in time_diff:
                     tim += time_diff + dt
 
@@ -475,7 +484,6 @@ def extract_and_plot_log_in_folder(log_path, mpc_ts_idx=0, save_prefix='', save_
         input_index=0, state_index=0, time_index=-1, 
         label_data1='State', label_data2='Command input', label_data3='MPC input',
         label_x="Time", label_y='Lateral velocity [m/s]',
-        fig_num=1,
         save_path=log_path + save_prefix + 'base_veloX.png')
 
     plot_log_input(
@@ -483,10 +491,9 @@ def extract_and_plot_log_in_folder(log_path, mpc_ts_idx=0, save_prefix='', save_
         input_index=1, state_index=1, time_index=-1, 
         label_data1='State', label_data2='Command input', label_data3='MPC input',
         label_x="Time", label_y='Angular velocity [rad/s]',
-        fig_num=2,
         save_path=log_path + save_prefix + 'base_veloYaw.png')
     
-    ## Arm states (position) - commands
+    ## Arm states (position and velocity) - commands
     for i in range(state_concat.shape[0] - input_arm_offset): # type: ignore
         idxi = input_arm_offset + i # type: ignore
         idxs = state_arm_offset + i # type: ignore
@@ -494,10 +501,16 @@ def extract_and_plot_log_in_folder(log_path, mpc_ts_idx=0, save_prefix='', save_
         plot_log_input(
             time_concat, state_concat, input_concat, mpc_time_concat, mpc_state_concat, 
             input_index=idxi, state_index=idxs, time_index=-1, 
-            label_data1='State', label_data2='Command input', label_data3='MPC input',
+            label_data1='State', label_data2='Command input', label_data3='MPC state estimate',
             label_x="Time", label_y='Arm joint angle [deg]',
-            fig_num=3+i,
-            save_path=log_path + save_prefix + 'arm_joint' + str(i) + '.png') # type: ignore
+            save_path=log_path + save_prefix + 'arm_joint' + str(i+1) + '_pos.png') # type: ignore
+        
+        plot_log_input(
+            data3_x=mpc_time_concat, data3_y=mpc_input_concat, 
+            state_index=idxi, time_index=-1,
+            label_data3='MPC input',
+            label_x="Time", label_y='Arm joint angular velocity [deg/sec]',
+            save_path=log_path + save_prefix + 'arm_joint' + str(i+1) + '_velo.png')
 
 '''
 DESCRIPTION: TODO...
@@ -523,6 +536,6 @@ def extract_and_plot_log_in_folder(log_path, mpc_ts_idx=0, save_prefix='', save_
         extract_and_plot_log(ocs2_log_path, ocs2_log_name, save_prefix=save_prefix, save_plot_flag=True) # type: ignore
         
     if option == 1:
-        extract_and_plot_log_in_folder(ocs2_log_path, save_prefix=save_prefix, save_plot_flag=True) # type: ignore
+        extract_and_plot_log_in_folder(ocs2_log_path, save_prefix=save_prefix, save_plot_flag=False) # type: ignore
     
     print("[plot_ocs2_log::__main__] END")
\ No newline at end of file