diff --git a/rbs_bringup/config/rbs.rviz b/rbs_bringup/config/rbs.rviz
index 74b8327..df9e15c 100644
--- a/rbs_bringup/config/rbs.rviz
+++ b/rbs_bringup/config/rbs.rviz
@@ -5,10 +5,10 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
-        - /TF1
         - /TF1/Frames1
+        - /MarkerArray2
       Splitter Ratio: 0.49496981501579285
-    Tree Height: 1468
+    Tree Height: 981
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -89,30 +89,6 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
-          box1_place:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          box2_place:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          box3_place:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          box4_place:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          box5_place:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          box6_place:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
           flange:
             Alpha: 1
             Show Axes: false
@@ -136,55 +112,25 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
-          robotiq_85_base_link:
+          schunk_grasp_point:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          schunk_gripper_base_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          robotiq_85_left_finger_link:
+          schunk_l_finger_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          robotiq_85_left_finger_tip_link:
+          schunk_r_finger_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          robotiq_85_left_inner_knuckle_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          robotiq_85_left_knuckle_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          robotiq_85_right_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          robotiq_85_right_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          robotiq_85_right_inner_knuckle_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          robotiq_85_right_knuckle_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          robotiq_grasp_point:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
           shoulder_link:
             Alpha: 1
             Show Axes: false
@@ -233,61 +179,23 @@ Visualization Manager:
           Value: false
         base_link_inertia:
           Value: false
-        box1:
-          Value: true
-        box1_place:
-          Value: true
-        box2:
-          Value: true
-        box2_place:
-          Value: true
-        box3:
-          Value: true
-        box3_place:
-          Value: true
-        box4:
-          Value: true
-        box4_place:
-          Value: true
-        box5:
-          Value: true
-        box5_place:
-          Value: true
-        box6:
-          Value: true
-        box6_place:
-          Value: true
         flange:
           Value: false
         forearm_link:
           Value: false
-        fork_gt:
-          Value: true
         ft_frame:
           Value: false
         inner_rgbd_camera:
           Value: false
         outer_rgbd_camera:
           Value: false
-        robotiq_85_base_link:
-          Value: false
-        robotiq_85_left_finger_link:
-          Value: false
-        robotiq_85_left_finger_tip_link:
-          Value: false
-        robotiq_85_left_inner_knuckle_link:
-          Value: false
-        robotiq_85_left_knuckle_link:
-          Value: false
-        robotiq_85_right_finger_link:
-          Value: false
-        robotiq_85_right_finger_tip_link:
-          Value: false
-        robotiq_85_right_inner_knuckle_link:
-          Value: false
-        robotiq_85_right_knuckle_link:
-          Value: false
-        robotiq_grasp_point:
+        schunk_grasp_point:
+          Value: true
+        schunk_gripper_base_link:
+          Value: true
+        schunk_l_finger_link:
+          Value: true
+        schunk_r_finger_link:
           Value: true
         shoulder_link:
           Value: false
@@ -324,49 +232,15 @@ Visualization Manager:
                             tool0:
                               inner_rgbd_camera:
                                 {}
-                              robotiq_85_base_link:
-                                robotiq_85_left_inner_knuckle_link:
+                              schunk_grasp_point:
+                                {}
+                              schunk_gripper_base_link:
+                                schunk_l_finger_link:
                                   {}
-                                robotiq_85_left_knuckle_link:
-                                  robotiq_85_left_finger_link:
-                                    robotiq_85_left_finger_tip_link:
-                                      {}
-                                robotiq_85_right_inner_knuckle_link:
-                                  {}
-                                robotiq_85_right_knuckle_link:
-                                  robotiq_85_right_finger_link:
-                                    robotiq_85_right_finger_tip_link:
-                                      {}
-                                robotiq_grasp_point:
+                                schunk_r_finger_link:
                                   {}
                           ft_frame:
                             {}
-          box1:
-            {}
-          box1_place:
-            {}
-          box2:
-            {}
-          box2_place:
-            {}
-          box3:
-            {}
-          box3_place:
-            {}
-          box4:
-            {}
-          box4_place:
-            {}
-          box5:
-            {}
-          box5_place:
-            {}
-          box6:
-            {}
-          box6_place:
-            {}
-          fork_gt:
-            {}
           outer_rgbd_camera:
             {}
       Update Interval: 0
@@ -394,30 +268,6 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        box1_place:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        box2_place:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        box3_place:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        box4_place:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        box5_place:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        box6_place:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
         flange:
           Alpha: 1
           Show Axes: false
@@ -441,55 +291,25 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        robotiq_85_base_link:
+        schunk_grasp_point:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        schunk_gripper_base_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        robotiq_85_left_finger_link:
+        schunk_l_finger_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        robotiq_85_left_finger_tip_link:
+        schunk_r_finger_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        robotiq_85_left_inner_knuckle_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        robotiq_85_left_knuckle_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        robotiq_85_right_finger_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        robotiq_85_right_finger_tip_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        robotiq_85_right_inner_knuckle_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        robotiq_85_right_knuckle_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        robotiq_grasp_point:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
         shoulder_link:
           Alpha: 1
           Show Axes: false
@@ -536,6 +356,18 @@ Visualization Manager:
       Trajectory Topic: /display_planned_path
       Use Sim Time: false
       Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: workspace
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -597,18 +429,18 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6253980398178101
+      Pitch: 0.5753980875015259
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 1.2853964567184448
+      Yaw: 1.1453965902328491
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1872
+  Height: 1385
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Tool Properties:
@@ -617,6 +449,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 3076
-  X: 124
-  Y: 54
+  Width: 2307
+  X: 436
+  Y: 66