diff --git a/air_hardware_interface/config/hardware.yaml b/air_hardware_interface/config/hardware.yaml
index eb6ae43..0a07271 100644
--- a/air_hardware_interface/config/hardware.yaml
+++ b/air_hardware_interface/config/hardware.yaml
@@ -1,6 +1,6 @@
 arm:
   hardware_interface:
-    loop_hz: 50
+    loop_hz: 100
     joints:
       - s1j
       - s2j
diff --git a/air_moveit_config/config/mtc.rviz b/air_moveit_config/config/mtc.rviz
index dabf369..c937804 100644
--- a/air_moveit_config/config/mtc.rviz
+++ b/air_moveit_config/config/mtc.rviz
@@ -6,7 +6,7 @@ Panels:
       Expanded:
         - /Motion Planning Tasks1
       Splitter Ratio: 0.5393258333206177
-    Tree Height: 533
+    Tree Height: 135
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -17,12 +17,13 @@ Panels:
   - Class: moveit_task_constructor/Motion Planning Tasks
     Global Settings:
       Task View Settings:
+        Old task handling: Keep
         Show Computation Times: true
         Task Expansion: All Expanded
     Name: Motion Planning Tasks
     Tasks View:
       property_splitter:
-        - 540
+        - 539
         - 0
       solution_sorting:
         column: 1
@@ -30,14 +31,12 @@ Panels:
       solutions_splitter:
         - 306
         - 98
-      solutions_view_columns:
-        - 38
-        - 52
-        - 0
+      solutions_view_columns: ~
       tasks_view_columns:
-        - 226
-        - 38
-        - 38
+        - 133
+        - 57
+        - 57
+        - 57
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -84,6 +83,222 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          camera_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          e:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          pitch:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_end_effector_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_index_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_index_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_index_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_little_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_little_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_little_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_middle_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_middle_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_middle_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_palm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_ring_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_ring_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_synergy_tendon_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_thumb_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_thumb_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_thumb_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_thumb_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_thumb_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          roll:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          root_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          s1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          s2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          s3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: true
@@ -104,6 +319,222 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          camera_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          e:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          pitch:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_end_effector_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_index_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_index_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_index_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_little_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_little_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_little_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_middle_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_middle_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_middle_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_palm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_ring_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_ring_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_synergy_tendon_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_thumb_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_thumb_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_thumb_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_thumb_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_thumb_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          roll:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          root_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          s1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          s2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          s3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: true
@@ -121,9 +552,523 @@ Visualization Manager:
       State Display Time: REALTIME
       Task Solution Topic: /mtc_tutorial/solution
       Tasks:
-        Cartesian Path: 1
+        {}
       Trail Step Size: 1
       Value: true
+    - Acceleration_Scaling_Factor: 1
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        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
+            Value: true
+          camera_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          e:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          pitch:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_end_effector_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_index_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_index_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_index_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_little_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_little_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_little_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_middle_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_middle_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_middle_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_palm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_ring_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_ring_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_synergy_tendon_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_thumb_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_thumb_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_thumb_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_thumb_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_thumb_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          roll:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          root_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          s1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          s2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          s3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Loop Animation: false
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 3x
+        Trail Step Size: 1
+        Trajectory Topic: move_group/display_planned_path
+        Use Sim Time: false
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0
+        Joint Violation Color: 255; 0; 255
+        Planning Group: gripper
+        Query Goal State: true
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: move_group/monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        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
+            Value: true
+          camera_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          e:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          pitch:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_end_effector_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_index_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_index_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_index_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_little_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_little_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_little_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_middle_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_middle_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_middle_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_palm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_ring_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_ring_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_synergy_tendon_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_thumb_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_thumb_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_thumb_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_thumb_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_thumb_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          roll:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          root_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          s1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          s2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          s3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+      Velocity_Scaling_Factor: 1
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -140,7 +1085,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 1.3878222703933716
+      Distance: 1.9497904777526855
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -150,15 +1095,15 @@ Visualization Manager:
       Focal Point:
         X: 0.30880630016326904
         Y: -0.1259305477142334
-        Z: 1.3560062370743253e-6
+        Z: 1.3560062370743253e-06
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.264797568321228
+      Pitch: 0.4747973680496216
       Target Frame: base_link
-      Yaw: 4.939944744110107
+      Yaw: 5.124948501586914
     Saved: ~
 Window Geometry:
   Displays:
@@ -172,9 +1117,13 @@ Window Geometry:
     collapsed: false
   Motion Planning Tasks - Slider:
     collapsed: false
-  QMainWindow State: 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
+  MotionPlanning:
+    collapsed: false
+  MotionPlanning - Trajectory Slider:
+    collapsed: false
+  QMainWindow State: 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
   Views:
     collapsed: false
   Width: 1621
   X: 249
-  Y: 25
+  Y: 27
diff --git a/air_moveit_config/config/urdf.srdf b/air_moveit_config/config/urdf.srdf
index c059a71..f9a0640 100644
--- a/air_moveit_config/config/urdf.srdf
+++ b/air_moveit_config/config/urdf.srdf
@@ -92,7 +92,7 @@
         <joint name="qbhand_synergy_joint" value="0.5"/>
     </group_state>
     <group_state name="full_closed" group="gripper">
-        <joint name="qbhand_synergy_joint" value="0.9"/>
+        <joint name="qbhand_synergy_joint" value="0.8"/>
     </group_state>
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
     <end_effector name="gripper" parent_link="pitch" group="gripper" parent_group="air"/>
diff --git a/air_moveit_config/launch/air_control.launch b/air_moveit_config/launch/air_control.launch
index 35cbd2b..8f1a60a 100644
--- a/air_moveit_config/launch/air_control.launch
+++ b/air_moveit_config/launch/air_control.launch
@@ -11,7 +11,7 @@
   </include>
 
 <!-- Given the published joint states, publish tf for the robot links -->
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" args="-publish_frequency 50.0"/>
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" args="-publish_frequency 100.0"/>
  
   <include file="$(dirname)/demo.launch" pass_all_args="true">
     <!-- robot description is loaded by gazebo.launch, to enable Gazebo features -->
diff --git a/air_moveit_config/launch/demo.launch b/air_moveit_config/launch/demo.launch
index fa97740..3b328a0 100644
--- a/air_moveit_config/launch/demo.launch
+++ b/air_moveit_config/launch/demo.launch
@@ -55,7 +55,7 @@
 
   <!-- Run Rviz and load the default config to see the state of the move_group node -->
   <include file="$(dirname)/moveit_rviz.launch" if="$(arg use_rviz)">
-    <arg name="rviz_config" value="$(dirname)/moveit.rviz"/>
+    <arg name="rviz_config" value="$(find air_moveit_config)/config/mtc.rviz" />
     <arg name="debug" value="$(arg debug)"/>
   </include>
 
@@ -63,7 +63,7 @@
   <include file="$(dirname)/default_warehouse_db.launch" if="$(arg db)">
     <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
   </include>
-
+  
   <group if="$(eval arg('moveit_controller_manager') == 'ros_control')">
     <include file="$(find qb_hand_control)/launch/control_qbhand.launch">
       <arg name="device_name" value="qbhand" /> 
diff --git a/air_moveit_config/launch/demo_ros_control.launch b/air_moveit_config/launch/demo_ros_control.launch
index c40c184..8e248eb 100644
--- a/air_moveit_config/launch/demo_ros_control.launch
+++ b/air_moveit_config/launch/demo_ros_control.launch
@@ -9,7 +9,7 @@
 
 <!-- Given the published joint states, publish tf for the robot links -->
   <group if="$(eval arg('moveit_controller_manager') == 'ros_control')">
-    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" args="-publish_frequency 50.0"/>
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" args="-publish_frequency 100.0"/>
     <include file="$(dirname)/ros_controllers.launch">
       <arg name="hw" value="real"/>
     </include>
@@ -20,4 +20,6 @@
     <arg name="load_robot_description" value="true" />
     <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
   </include>
+  <param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" />
+
 </launch>
diff --git a/air_moveit_config/launch/moveit.rviz b/air_moveit_config/launch/moveit.rviz
index a9a65d5..0f8765f 100644
--- a/air_moveit_config/launch/moveit.rviz
+++ b/air_moveit_config/launch/moveit.rviz
@@ -1,6 +1,6 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 84
+    Help Height: 70
     Name: Displays
     Property Tree Widget:
       Expanded:
@@ -11,7 +11,7 @@ Panels:
         - /MotionPlanning1/Planning Metrics1
         - /MotionPlanning1/Planned Path1
       Splitter Ratio: 0.5
-    Tree Height: 171
+    Tree Height: 70
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -22,7 +22,30 @@ Panels:
   - Class: rviz/Time
     Name: Time
     SyncMode: 1
-    SyncSource: PointCloud2
+    SyncSource: ""
+  - Class: moveit_task_constructor/Motion Planning Tasks
+    Global Settings:
+      Task View Settings:
+        Old task handling: Keep
+        Show Computation Times: true
+        Task Expansion: All Expanded
+    Name: Motion Planning Tasks
+    Tasks View:
+      property_splitter:
+        - 87
+        - 87
+      solution_sorting:
+        column: 0
+        order: 1
+      solutions_splitter:
+        - 365
+        - 182
+      solutions_view_columns: ~
+      tasks_view_columns:
+        - 192
+        - 57
+        - 57
+        - 57
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -602,6 +625,258 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: false
+    - Class: moveit_task_constructor/Motion Planning Tasks
+      Enabled: true
+      Interrupt Display: false
+      Loop Animation: false
+      Markers:
+        All at once?: false
+        Value: true
+      Name: Motion Planning Tasks
+      Robot:
+        Fixed Robot Color: 150; 50; 150
+        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
+            Value: true
+          camera_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          e:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          pitch:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_end_effector_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_index_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_index_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_index_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_index_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_little_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_little_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_little_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_little_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_middle_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_middle_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_middle_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_middle_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_palm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_ring_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_middle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_middle_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_ring_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_ring_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_synergy_tendon_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_thumb_distal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_thumb_distal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          qbhand_thumb_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_thumb_proximal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          qbhand_thumb_proximal_virtual_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          roll:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          root_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          s1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          s2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          s3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Robot Alpha: 0.5
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Use Fixed Robot Color: false
+        Value: ""
+      Robot Description: robot_description
+      Scene:
+        Attached Body Color: 150; 50; 150
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Value: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Show Trail: false
+      State Display Time: 0.05 s
+      Task Solution Topic: ""
+      Tasks:
+        {}
+      Trail Step Size: 1
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -648,15 +923,19 @@ Window Geometry:
   Hide Right Dock: false
   Image:
     collapsed: false
+  Motion Planning Tasks:
+    collapsed: false
+  Motion Planning Tasks - Slider:
+    collapsed: false
   MotionPlanning:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000002270000039efc0200000009fb000000100044006900730070006c006100790073010000003d0000013c000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017f000001880000017d00fffffffb0000000800540069006d006500000003a2000000390000003900fffffffb0000000a0049006d006100670065010000030d000000ce0000001600ffffff000005170000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000002270000039efc020000000bfb000000100044006900730070006c006100790073010000003d000000c9000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000010c0000017d0000017d00fffffffb0000000800540069006d006500000003a2000000390000003900fffffffb0000000a0049006d006100670065010000028f0000005f0000001600fffffffb000000120020002d00200053006c00690064006500720000000000ffffffff0000004100fffffffb0000002a004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b007301000002f4000000e7000000e700ffffff000005250000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Time:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1860
-  X: 60
+  Width: 1874
+  X: 46
   Y: 27
diff --git a/air_moveit_config/launch/ros_controllers.launch b/air_moveit_config/launch/ros_controllers.launch
index daa9144..ebc483a 100644
--- a/air_moveit_config/launch/ros_controllers.launch
+++ b/air_moveit_config/launch/ros_controllers.launch
@@ -13,9 +13,18 @@
   <!-- load other controllers for servo control -->
      <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false" output="screen" args="load arm/joint_group_position_controller" />
   </group>
-
+  
   <group if="$(eval arg('hw') == 'real')"> 
     <!-- Load the controllers -->
+    
+    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+      <param name="rate" value="100"/>
+      <rosparam param="source_list">
+        - /qbhand/control/joint_states
+        - /air/joint_states
+      </rosparam>    
+    </node>
+    <remap from="/joint_states" to="/air/joint_states"/>
     <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
           output="screen" args="arm/joint_state_controller arm/arm_position_controller"/>
     <include file="$(find air_hardware_interface)/launch/air_hardware_interface.launch"/>