From af91049e072a8df6b6d30b8ce2df89487782b7a1 Mon Sep 17 00:00:00 2001
From: omid1366 <omid@picknik.ai>
Date: Mon, 20 May 2019 20:49:11 -0600
Subject: [PATCH 1/8] added new files for emptyplan

---
 launch/emptyplan_planning_pipeline.launch.xml | 22 +++++
 launch/move_group_emptyplan.launch            | 81 +++++++++++++++++++
 launch/planning_pipeline_emptyplan.launch.xml | 10 +++
 launch/run_benchmark_emptyplan.launch         | 22 +++++
 4 files changed, 135 insertions(+)
 create mode 100644 launch/emptyplan_planning_pipeline.launch.xml
 create mode 100644 launch/move_group_emptyplan.launch
 create mode 100644 launch/planning_pipeline_emptyplan.launch.xml
 create mode 100644 launch/run_benchmark_emptyplan.launch

diff --git a/launch/emptyplan_planning_pipeline.launch.xml b/launch/emptyplan_planning_pipeline.launch.xml
new file mode 100644
index 0000000..6c764d4
--- /dev/null
+++ b/launch/emptyplan_planning_pipeline.launch.xml
@@ -0,0 +1,22 @@
+<launch>
+
+  <!-- EmptyPlan Plugin for MoveIt! -->
+  <arg name="planning_plugin" value="emptyplan_interface/EmptyPlanPlanner" />
+
+  <!-- The request adapters (plugins) used when planning with EmptyPlan. 
+       ORDER MATTERS -->
+  <arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
+				       default_planner_request_adapters/FixWorkspaceBounds
+				       default_planner_request_adapters/FixStartStateBounds
+				       default_planner_request_adapters/FixStartStateCollision
+				       default_planner_request_adapters/FixStartStatePathConstraints" />
+
+  <arg name="start_state_max_bounds_error" value="0.1" />
+
+  <param name="planning_plugin" value="$(arg planning_plugin)" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+
+  <rosparam command="load" file="$(find panda_moveit_config)/config/emptyplan_planning.yaml"/>
+
+</launch>
diff --git a/launch/move_group_emptyplan.launch b/launch/move_group_emptyplan.launch
new file mode 100644
index 0000000..c40ded6
--- /dev/null
+++ b/launch/move_group_emptyplan.launch
@@ -0,0 +1,81 @@
+<launch>
+
+  <arg name="load_gripper" default="true" />
+
+  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+    <arg name="load_gripper" value="$(arg load_gripper)" />
+  </include>
+
+  <arg name="planner" default="emptyplan" />
+
+  <!-- GDB Debug Option -->
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix"
+	   value="gdb -x $(find panda_moveit_config)/launch/gdb_settings.gdb --ex run --args" />
+
+  <!-- Verbose Mode Option -->
+  <arg name="info" default="$(arg debug)" />
+  <arg unless="$(arg info)" name="command_args" value="" />
+  <arg     if="$(arg info)" name="command_args" value="--debug" />
+
+  <!-- move_group settings -->
+  <arg name="allow_trajectory_execution" default="true"/>
+  <arg name="fake_execution" default="false"/>
+  <arg name="max_safe_path_cost" default="1"/>
+  <arg name="jiggle_fraction" default="0.05" />
+  <arg name="publish_monitored_planning_scene" default="true"/>
+
+  <!-- Planning Functionality -->
+  <include ns="move_group" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
+    <arg name="pipeline" value="$(arg planner)" />
+  </include>
+
+  <!-- Trajectory Execution Functionality -->
+  <include ns="move_group" file="$(find panda_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
+    <arg name="moveit_manage_controllers" value="true" />
+    <arg name="moveit_controller_manager" value="panda" if="$(eval not arg('fake_execution') and not arg('load_gripper'))"/>
+    <arg name="moveit_controller_manager" value="panda_gripper" if="$(eval not arg('fake_execution') and arg('load_gripper'))"/>
+    <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
+  </include>
+
+  <!-- Sensors Functionality -->
+  <include ns="move_group" file="$(find panda_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
+    <arg name="moveit_sensor_manager" value="panda" />
+  </include>
+
+  <!-- Start the actual move_group node/action server -->
+  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
+    <!-- Set the display variable, in case OpenGL code is used internally -->
+    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />
+
+    <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
+    <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
+    <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
+
+    <!-- load these non-default MoveGroup capabilities -->
+    <!--
+    <param name="capabilities" value="
+                  a_package/AwsomeMotionPlanningCapability
+                  another_package/GraspPlanningPipeline
+                  " />
+    -->
+
+    <!-- inhibit these default MoveGroup capabilities -->
+    <!--
+    <param name="disable_capabilities" value="
+                  move_group/MoveGroupKinematicsService
+                  move_group/ClearOctomapService
+                  " />
+    -->
+
+    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
+    <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
+    <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
+    <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
+    <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
+
+    <remap from="/joint_states" to="/joint_states_desired" />
+  </node>
+
+</launch>
diff --git a/launch/planning_pipeline_emptyplan.launch.xml b/launch/planning_pipeline_emptyplan.launch.xml
new file mode 100644
index 0000000..f1d4e25
--- /dev/null
+++ b/launch/planning_pipeline_emptyplan.launch.xml
@@ -0,0 +1,10 @@
+<launch>
+
+  <!-- This file makes it easy to include different planning pipelines; 
+       It is assumed that all planning pipelines are named XXX_planning_pipeline.launch  -->  
+
+  <arg name="pipeline" default="emptyplan" />
+
+  <include file="$(find panda_moveit_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml" />
+
+</launch>
diff --git a/launch/run_benchmark_emptyplan.launch b/launch/run_benchmark_emptyplan.launch
new file mode 100644
index 0000000..a97fa72
--- /dev/null
+++ b/launch/run_benchmark_emptyplan.launch
@@ -0,0 +1,22 @@
+<launch>
+
+  <!-- This argument must specify the list of .cfg files to process for benchmarking -->
+  <arg name="cfg" />
+
+  <!-- Load URDF -->
+  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+    <arg name="load_robot_description" value="true"/>
+  </include>
+
+  <!-- Start the database -->
+  <include file="$(find panda_moveit_config)/launch/warehouse.launch">
+    <arg name="moveit_warehouse_database_path" value="moveit_emptyplan_benchmark_warehouse"/>
+  </include>  
+
+  <!-- Start Benchmark Executable -->
+  <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
+    <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+    <rosparam command="load" file="$(find panda_moveit_config)/config/emptyplan_planning.yaml"/>
+  </node>
+
+</launch>

From 7e8324bf27ec61e88cb3df3f1038485e73455515 Mon Sep 17 00:00:00 2001
From: omid1366 <omid@picknik.ai>
Date: Tue, 21 May 2019 09:30:33 -0600
Subject: [PATCH 2/8] edited demo.launch to accept planner arg

---
 launch/demo.launch                            | 2 ++
 launch/emptyplan_planning_pipeline.launch.xml | 4 ++--
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/launch/demo.launch b/launch/demo.launch
index 2ebb63b..5963063 100644
--- a/launch/demo.launch
+++ b/launch/demo.launch
@@ -9,6 +9,8 @@
   <arg name="debug" default="false" />
   <arg name="pipeline" default="ompl" />
 
+  <arg name="planner" default="ompl" />
+
   <!--
   By default, hide joint_state_publisher's GUI
 
diff --git a/launch/emptyplan_planning_pipeline.launch.xml b/launch/emptyplan_planning_pipeline.launch.xml
index 6c764d4..1720958 100644
--- a/launch/emptyplan_planning_pipeline.launch.xml
+++ b/launch/emptyplan_planning_pipeline.launch.xml
@@ -3,7 +3,7 @@
   <!-- EmptyPlan Plugin for MoveIt! -->
   <arg name="planning_plugin" value="emptyplan_interface/EmptyPlanPlanner" />
 
-  <!-- The request adapters (plugins) used when planning with EmptyPlan. 
+  <!-- The request adapters (plugins) used when planning with EmptyPlan.
        ORDER MATTERS -->
   <arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
 				       default_planner_request_adapters/FixWorkspaceBounds
@@ -17,6 +17,6 @@
   <param name="request_adapters" value="$(arg planning_adapters)" />
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
 
-  <rosparam command="load" file="$(find panda_moveit_config)/config/emptyplan_planning.yaml"/>
+  <!--rosparam command="load" file="$(find panda_moveit_config)/config/emptyplan_planning.yaml"/-->   <!-- we do not need parameters for an empty planner  -->
 
 </launch>

From 4859108b347c7a65516c056774e0c2641475009e Mon Sep 17 00:00:00 2001
From: omid1366 <omid@picknik.ai>
Date: Tue, 21 May 2019 16:28:16 -0600
Subject: [PATCH 3/8] Added emptyplan_planning.yaml so it can be used for new
 planners. It is an empty file. Added rosparam in
 emptyplan_planning_pipeline.launch.xml to load its yaml file

---
 .setup_assistant                              |  10 -
 config/emptyplan_planning.yaml                |   0
 launch/demo.launch                            |   7 +
 launch/emptyplan_planning_pipeline.launch.xml |   2 +-
 launch/moveit.rviz                            | 255 ++++++++++++++++--
 5 files changed, 239 insertions(+), 35 deletions(-)
 delete mode 100644 .setup_assistant
 create mode 100644 config/emptyplan_planning.yaml

diff --git a/.setup_assistant b/.setup_assistant
deleted file mode 100644
index 02532ad..0000000
--- a/.setup_assistant
+++ /dev/null
@@ -1,10 +0,0 @@
-moveit_setup_assistant_config:
-  URDF:
-    package: franka_description
-    relative_path: robots/panda_arm_hand.urdf.xacro
-  SRDF:
-    relative_path: config/panda_arm.srdf.xacro
-  CONFIG:
-    author_name: Mike Lautman
-    author_email: mike@picknik.ai
-    generated_timestamp: 1519152260
\ No newline at end of file
diff --git a/config/emptyplan_planning.yaml b/config/emptyplan_planning.yaml
new file mode 100644
index 0000000..e69de29
diff --git a/launch/demo.launch b/launch/demo.launch
index 5963063..eb5cfc5 100644
--- a/launch/demo.launch
+++ b/launch/demo.launch
@@ -7,8 +7,11 @@
 
   <!-- By default, we are not in debug mode -->
   <arg name="debug" default="false" />
+<<<<<<< HEAD
   <arg name="pipeline" default="ompl" />
 
+=======
+>>>>>>> Added emptyplan_planning.yaml so it can be used for new planners. It is an empty file.
   <arg name="planner" default="ompl" />
 
   <!--
@@ -46,7 +49,11 @@
     <arg name="fake_execution" value="true"/>
     <arg name="info" value="true"/>
     <arg name="debug" value="$(arg debug)"/>
+<<<<<<< HEAD
     <arg name="pipeline" value="$(arg pipeline)"  />
+=======
+    <arg name="planner" value="$(arg planner)"  />
+>>>>>>> Added emptyplan_planning.yaml so it can be used for new planners. It is an empty file.
   </include>
 
   <!-- Run Rviz -->
diff --git a/launch/emptyplan_planning_pipeline.launch.xml b/launch/emptyplan_planning_pipeline.launch.xml
index 1720958..1822144 100644
--- a/launch/emptyplan_planning_pipeline.launch.xml
+++ b/launch/emptyplan_planning_pipeline.launch.xml
@@ -17,6 +17,6 @@
   <param name="request_adapters" value="$(arg planning_adapters)" />
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
 
-  <!--rosparam command="load" file="$(find panda_moveit_config)/config/emptyplan_planning.yaml"/-->   <!-- we do not need parameters for an empty planner  -->
+  <rosparam command="load" file="$(find panda_moveit_config)/config/emptyplan_planning.yaml"/>
 
 </launch>
diff --git a/launch/moveit.rviz b/launch/moveit.rviz
index 9a49749..1f5e21f 100644
--- a/launch/moveit.rviz
+++ b/launch/moveit.rviz
@@ -1,13 +1,14 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 84
+    Help Height: 70
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /MarkerArray1
         - /PlanningScene1/Scene Robot1
-      Splitter Ratio: 0.742560029
-    Tree Height: 408
+        - /MotionPlanning1
+      Splitter Ratio: 0.7425600290298462
+    Tree Height: 133
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -17,6 +18,10 @@ Panels:
     Splitter Ratio: 0.5
   - Class: rviz_visual_tools/RvizVisualToolsGui
     Name: RvizVisualToolsGui
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
 Visualization Manager:
   Class: ""
   Displays:
@@ -26,7 +31,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.0299999993
+        Line Width: 0.029999999329447746
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -43,11 +48,7 @@ Visualization Manager:
       Marker Topic: /rviz_visual_tools
       Name: MarkerArray
       Namespaces:
-        Cylinder: true
-        Sphere: true
-        Text: true
-        goal: true
-        start: true
+        {}
       Queue Size: 100
       Value: true
     - Class: moveit_rviz_plugin/Trajectory
@@ -138,9 +139,9 @@ Visualization Manager:
       Planning Scene Topic: /move_group/monitored_planning_scene
       Robot Description: robot_description
       Scene Geometry:
-        Scene Alpha: 0.899999976
+        Scene Alpha: 0.8999999761581421
         Scene Color: 50; 230; 50
-        Scene Display Time: 0.200000003
+        Scene Display Time: 0.20000000298023224
         Show Scene Geometry: true
         Voxel Coloring: Z-Axis
         Voxel Rendering: Occupied Voxels
@@ -215,6 +216,208 @@ Visualization Manager:
         Show Robot Collision: false
         Show Robot Visual: true
       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_Goal_Tolerance: 0
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: true
+      MoveIt_Warehouse_Host: 127.0.0.1
+      MoveIt_Warehouse_Port: 33829
+      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
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        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: 0.05 s
+        Trail Step Size: 1
+        Trajectory Topic: /move_group/display_planned_path
+      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: hand
+        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.20000000298023224
+        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
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        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
@@ -232,35 +435,39 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 2.82759404
+      Distance: 2.827594041824341
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.0599999987
+        Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.113567002
-        Y: 0.105920002
-        Z: 2.23518001e-07
+        X: 0.11356700211763382
+        Y: 0.10592000186443329
+        Z: 2.2351800055275817e-7
       Focal Shape Fixed Size: true
-      Focal Shape Size: 0.0500000007
+      Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.00999999978
-      Pitch: 0.470203578
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.47020357847213745
       Target Frame: panda_link0
       Value: XYOrbit (rviz)
-      Yaw: 6.06496
+      Yaw: 6.06496000289917
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 741
+  Height: 1029
   Help:
     collapsed: false
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000001000000000000017800000282fc020000000efb000000100044006900730070006c006100790073010000004200000232000000dc00fffffffb0000000800480065006c00700000000342000000bb0000007500fffffffb0000000a00560069006500770073000000026d000000b5000000b500fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000294000002410000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069010000027a0000004a0000004800fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb000000120020002d00200053006c006900640065007201000002df000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004800ffffff000003d80000028200000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  MotionPlanning:
+    collapsed: false
+  MotionPlanning - Trajectory Slider:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd000000010000000000000295000003abfc020000000ffb000000100044006900730070006c006100790073010000003d00000108000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000026d000000b5000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000014b000002410000019b00fffffffb00000024005200760069007a00560069007300750061006c0054006f006f006c00730047007500690100000392000000560000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb000000120020002d00200053006c006900640065007201000002df000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000153000001a60000000000000000000002bb000003ab00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   RvizVisualToolsGui:
     collapsed: false
   Trajectory - Trajectory Slider:
@@ -268,5 +475,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1366
-  X: 1906
-  Y: 1440
+  X: 3740
+  Y: 333

From 421239c63ab4545674e928eb78d133419c94a53f Mon Sep 17 00:00:00 2001
From: Omid Heidari <heidomid@isu.edu>
Date: Sat, 1 Jun 2019 09:51:07 -0600
Subject: [PATCH 4/8] added necessary files for LERP

---
 config/{emptyplan_planning.yaml => lerp_planning.yaml}    | 0
 ...eline.launch.xml => lerp_planning_pipeline.launch.xml} | 8 ++++----
 ...nchmark_emptyplan.launch => run_benchmark_lerp.launch} | 4 ++--
 3 files changed, 6 insertions(+), 6 deletions(-)
 rename config/{emptyplan_planning.yaml => lerp_planning.yaml} (100%)
 rename launch/{emptyplan_planning_pipeline.launch.xml => lerp_planning_pipeline.launch.xml} (77%)
 rename launch/{run_benchmark_emptyplan.launch => run_benchmark_lerp.launch} (86%)

diff --git a/config/emptyplan_planning.yaml b/config/lerp_planning.yaml
similarity index 100%
rename from config/emptyplan_planning.yaml
rename to config/lerp_planning.yaml
diff --git a/launch/emptyplan_planning_pipeline.launch.xml b/launch/lerp_planning_pipeline.launch.xml
similarity index 77%
rename from launch/emptyplan_planning_pipeline.launch.xml
rename to launch/lerp_planning_pipeline.launch.xml
index 1822144..3016e79 100644
--- a/launch/emptyplan_planning_pipeline.launch.xml
+++ b/launch/lerp_planning_pipeline.launch.xml
@@ -1,9 +1,9 @@
 <launch>
 
-  <!-- EmptyPlan Plugin for MoveIt! -->
-  <arg name="planning_plugin" value="emptyplan_interface/EmptyPlanPlanner" />
+  <!-- TrajOpt Plugin for MoveIt! -->
+  <arg name="planning_plugin" value="lerp_interface/LERPPlanner" />
 
-  <!-- The request adapters (plugins) used when planning with EmptyPlan.
+  <!-- The request adapters (plugins) used when planning with TrajOpt.
        ORDER MATTERS -->
   <arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
 				       default_planner_request_adapters/FixWorkspaceBounds
@@ -17,6 +17,6 @@
   <param name="request_adapters" value="$(arg planning_adapters)" />
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
 
-  <rosparam command="load" file="$(find panda_moveit_config)/config/emptyplan_planning.yaml"/>
+  <rosparam command="load" file="$(find panda_moveit_config)/config/lerp_planning.yaml"/>
 
 </launch>
diff --git a/launch/run_benchmark_emptyplan.launch b/launch/run_benchmark_lerp.launch
similarity index 86%
rename from launch/run_benchmark_emptyplan.launch
rename to launch/run_benchmark_lerp.launch
index a97fa72..d77a7d9 100644
--- a/launch/run_benchmark_emptyplan.launch
+++ b/launch/run_benchmark_lerp.launch
@@ -10,13 +10,13 @@
 
   <!-- Start the database -->
   <include file="$(find panda_moveit_config)/launch/warehouse.launch">
-    <arg name="moveit_warehouse_database_path" value="moveit_emptyplan_benchmark_warehouse"/>
+    <arg name="moveit_warehouse_database_path" value="moveit_lerp_benchmark_warehouse"/>
   </include>  
 
   <!-- Start Benchmark Executable -->
   <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
     <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
-    <rosparam command="load" file="$(find panda_moveit_config)/config/emptyplan_planning.yaml"/>
+    <rosparam command="load" file="$(find panda_moveit_config)/config/lerp_planning.yaml"/>
   </node>
 
 </launch>

From e314a94f6825aba939286b1be7208f10ad1a2a80 Mon Sep 17 00:00:00 2001
From: omid1366 <omid@picknik.ai>
Date: Thu, 8 Aug 2019 10:59:25 -0600
Subject: [PATCH 5/8] chaged the virtual joint to fixed

---
 config/panda_arm.xacro | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/config/panda_arm.xacro b/config/panda_arm.xacro
index 092b68f..53ddcdf 100644
--- a/config/panda_arm.xacro
+++ b/config/panda_arm.xacro
@@ -21,7 +21,7 @@
     </group_state>
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
-    <virtual_joint name="virtual_joint" type="floating" parent_frame="world" child_link="panda_link0" />
+    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="panda_link0" />
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent" />
     <disable_collisions link1="panda_link0" link2="panda_link2" reason="Never" />

From 04d83ee717ea19a66d5baa8d4cf4e2cb10f4ef35 Mon Sep 17 00:00:00 2001
From: omid1366 <omid@picknik.ai>
Date: Mon, 12 Aug 2019 09:19:09 -0600
Subject: [PATCH 6/8] added yaml file

---
 config/lerp_planning.yaml                     |  1 +
 launch/lerp_planning_pipeline.launch.xml      |  2 +-
 launch/planning_pipeline_emptyplan.launch.xml | 10 ----------
 3 files changed, 2 insertions(+), 11 deletions(-)
 delete mode 100644 launch/planning_pipeline_emptyplan.launch.xml

diff --git a/config/lerp_planning.yaml b/config/lerp_planning.yaml
index e69de29..9d2eebd 100644
--- a/config/lerp_planning.yaml
+++ b/config/lerp_planning.yaml
@@ -0,0 +1 @@
+num_steps: 40
diff --git a/launch/lerp_planning_pipeline.launch.xml b/launch/lerp_planning_pipeline.launch.xml
index 3016e79..c119357 100644
--- a/launch/lerp_planning_pipeline.launch.xml
+++ b/launch/lerp_planning_pipeline.launch.xml
@@ -1,6 +1,6 @@
 <launch>
 
-  <!-- TrajOpt Plugin for MoveIt! -->
+  <!-- LERP Plugin for MoveIt! -->
   <arg name="planning_plugin" value="lerp_interface/LERPPlanner" />
 
   <!-- The request adapters (plugins) used when planning with TrajOpt.
diff --git a/launch/planning_pipeline_emptyplan.launch.xml b/launch/planning_pipeline_emptyplan.launch.xml
deleted file mode 100644
index f1d4e25..0000000
--- a/launch/planning_pipeline_emptyplan.launch.xml
+++ /dev/null
@@ -1,10 +0,0 @@
-<launch>
-
-  <!-- This file makes it easy to include different planning pipelines; 
-       It is assumed that all planning pipelines are named XXX_planning_pipeline.launch  -->  
-
-  <arg name="pipeline" default="emptyplan" />
-
-  <include file="$(find panda_moveit_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml" />
-
-</launch>

From 1e19bd0cf0e158c4c9b392a06651f88abb5aa895 Mon Sep 17 00:00:00 2001
From: omid1366 <omid@picknik.ai>
Date: Tue, 13 Aug 2019 08:42:46 -0600
Subject: [PATCH 7/8] addressed the comments

---
 .setup_assistant                   | 10 ++++
 launch/demo.launch                 |  9 ----
 launch/move_group_emptyplan.launch | 81 ------------------------------
 launch/run_benchmark_lerp.launch   | 22 --------
 4 files changed, 10 insertions(+), 112 deletions(-)
 create mode 100644 .setup_assistant
 delete mode 100644 launch/move_group_emptyplan.launch
 delete mode 100644 launch/run_benchmark_lerp.launch

diff --git a/.setup_assistant b/.setup_assistant
new file mode 100644
index 0000000..02532ad
--- /dev/null
+++ b/.setup_assistant
@@ -0,0 +1,10 @@
+moveit_setup_assistant_config:
+  URDF:
+    package: franka_description
+    relative_path: robots/panda_arm_hand.urdf.xacro
+  SRDF:
+    relative_path: config/panda_arm.srdf.xacro
+  CONFIG:
+    author_name: Mike Lautman
+    author_email: mike@picknik.ai
+    generated_timestamp: 1519152260
\ No newline at end of file
diff --git a/launch/demo.launch b/launch/demo.launch
index eb5cfc5..2ebb63b 100644
--- a/launch/demo.launch
+++ b/launch/demo.launch
@@ -7,13 +7,8 @@
 
   <!-- By default, we are not in debug mode -->
   <arg name="debug" default="false" />
-<<<<<<< HEAD
   <arg name="pipeline" default="ompl" />
 
-=======
->>>>>>> Added emptyplan_planning.yaml so it can be used for new planners. It is an empty file.
-  <arg name="planner" default="ompl" />
-
   <!--
   By default, hide joint_state_publisher's GUI
 
@@ -49,11 +44,7 @@
     <arg name="fake_execution" value="true"/>
     <arg name="info" value="true"/>
     <arg name="debug" value="$(arg debug)"/>
-<<<<<<< HEAD
     <arg name="pipeline" value="$(arg pipeline)"  />
-=======
-    <arg name="planner" value="$(arg planner)"  />
->>>>>>> Added emptyplan_planning.yaml so it can be used for new planners. It is an empty file.
   </include>
 
   <!-- Run Rviz -->
diff --git a/launch/move_group_emptyplan.launch b/launch/move_group_emptyplan.launch
deleted file mode 100644
index c40ded6..0000000
--- a/launch/move_group_emptyplan.launch
+++ /dev/null
@@ -1,81 +0,0 @@
-<launch>
-
-  <arg name="load_gripper" default="true" />
-
-  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
-    <arg name="load_gripper" value="$(arg load_gripper)" />
-  </include>
-
-  <arg name="planner" default="emptyplan" />
-
-  <!-- GDB Debug Option -->
-  <arg name="debug" default="false" />
-  <arg unless="$(arg debug)" name="launch_prefix" value="" />
-  <arg     if="$(arg debug)" name="launch_prefix"
-	   value="gdb -x $(find panda_moveit_config)/launch/gdb_settings.gdb --ex run --args" />
-
-  <!-- Verbose Mode Option -->
-  <arg name="info" default="$(arg debug)" />
-  <arg unless="$(arg info)" name="command_args" value="" />
-  <arg     if="$(arg info)" name="command_args" value="--debug" />
-
-  <!-- move_group settings -->
-  <arg name="allow_trajectory_execution" default="true"/>
-  <arg name="fake_execution" default="false"/>
-  <arg name="max_safe_path_cost" default="1"/>
-  <arg name="jiggle_fraction" default="0.05" />
-  <arg name="publish_monitored_planning_scene" default="true"/>
-
-  <!-- Planning Functionality -->
-  <include ns="move_group" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
-    <arg name="pipeline" value="$(arg planner)" />
-  </include>
-
-  <!-- Trajectory Execution Functionality -->
-  <include ns="move_group" file="$(find panda_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
-    <arg name="moveit_manage_controllers" value="true" />
-    <arg name="moveit_controller_manager" value="panda" if="$(eval not arg('fake_execution') and not arg('load_gripper'))"/>
-    <arg name="moveit_controller_manager" value="panda_gripper" if="$(eval not arg('fake_execution') and arg('load_gripper'))"/>
-    <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
-  </include>
-
-  <!-- Sensors Functionality -->
-  <include ns="move_group" file="$(find panda_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
-    <arg name="moveit_sensor_manager" value="panda" />
-  </include>
-
-  <!-- Start the actual move_group node/action server -->
-  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
-    <!-- Set the display variable, in case OpenGL code is used internally -->
-    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />
-
-    <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
-    <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
-    <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
-
-    <!-- load these non-default MoveGroup capabilities -->
-    <!--
-    <param name="capabilities" value="
-                  a_package/AwsomeMotionPlanningCapability
-                  another_package/GraspPlanningPipeline
-                  " />
-    -->
-
-    <!-- inhibit these default MoveGroup capabilities -->
-    <!--
-    <param name="disable_capabilities" value="
-                  move_group/MoveGroupKinematicsService
-                  move_group/ClearOctomapService
-                  " />
-    -->
-
-    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
-    <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
-    <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
-    <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
-    <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
-
-    <remap from="/joint_states" to="/joint_states_desired" />
-  </node>
-
-</launch>
diff --git a/launch/run_benchmark_lerp.launch b/launch/run_benchmark_lerp.launch
deleted file mode 100644
index d77a7d9..0000000
--- a/launch/run_benchmark_lerp.launch
+++ /dev/null
@@ -1,22 +0,0 @@
-<launch>
-
-  <!-- This argument must specify the list of .cfg files to process for benchmarking -->
-  <arg name="cfg" />
-
-  <!-- Load URDF -->
-  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
-    <arg name="load_robot_description" value="true"/>
-  </include>
-
-  <!-- Start the database -->
-  <include file="$(find panda_moveit_config)/launch/warehouse.launch">
-    <arg name="moveit_warehouse_database_path" value="moveit_lerp_benchmark_warehouse"/>
-  </include>  
-
-  <!-- Start Benchmark Executable -->
-  <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
-    <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
-    <rosparam command="load" file="$(find panda_moveit_config)/config/lerp_planning.yaml"/>
-  </node>
-
-</launch>

From a5b37cad1e6576d0575da7b528f8b4cc7431eb85 Mon Sep 17 00:00:00 2001
From: omid1366 <omid@picknik.ai>
Date: Tue, 13 Aug 2019 11:48:07 -0600
Subject: [PATCH 8/8] revet back to floating for virtual joint and revert
 moveit.rviz

---
 config/panda_arm.xacro |   2 +-
 launch/moveit.rviz     | 255 ++++-------------------------------------
 2 files changed, 25 insertions(+), 232 deletions(-)

diff --git a/config/panda_arm.xacro b/config/panda_arm.xacro
index 53ddcdf..092b68f 100644
--- a/config/panda_arm.xacro
+++ b/config/panda_arm.xacro
@@ -21,7 +21,7 @@
     </group_state>
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
-    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="panda_link0" />
+    <virtual_joint name="virtual_joint" type="floating" parent_frame="world" child_link="panda_link0" />
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent" />
     <disable_collisions link1="panda_link0" link2="panda_link2" reason="Never" />
diff --git a/launch/moveit.rviz b/launch/moveit.rviz
index 1f5e21f..9a49749 100644
--- a/launch/moveit.rviz
+++ b/launch/moveit.rviz
@@ -1,14 +1,13 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 70
+    Help Height: 84
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /MarkerArray1
         - /PlanningScene1/Scene Robot1
-        - /MotionPlanning1
-      Splitter Ratio: 0.7425600290298462
-    Tree Height: 133
+      Splitter Ratio: 0.742560029
+    Tree Height: 408
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -18,10 +17,6 @@ Panels:
     Splitter Ratio: 0.5
   - Class: rviz_visual_tools/RvizVisualToolsGui
     Name: RvizVisualToolsGui
-Preferences:
-  PromptSaveOnExit: true
-Toolbars:
-  toolButtonStyle: 2
 Visualization Manager:
   Class: ""
   Displays:
@@ -31,7 +26,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.029999999329447746
+        Line Width: 0.0299999993
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -48,7 +43,11 @@ Visualization Manager:
       Marker Topic: /rviz_visual_tools
       Name: MarkerArray
       Namespaces:
-        {}
+        Cylinder: true
+        Sphere: true
+        Text: true
+        goal: true
+        start: true
       Queue Size: 100
       Value: true
     - Class: moveit_rviz_plugin/Trajectory
@@ -139,9 +138,9 @@ Visualization Manager:
       Planning Scene Topic: /move_group/monitored_planning_scene
       Robot Description: robot_description
       Scene Geometry:
-        Scene Alpha: 0.8999999761581421
+        Scene Alpha: 0.899999976
         Scene Color: 50; 230; 50
-        Scene Display Time: 0.20000000298023224
+        Scene Display Time: 0.200000003
         Show Scene Geometry: true
         Voxel Coloring: Z-Axis
         Voxel Rendering: Occupied Voxels
@@ -216,208 +215,6 @@ Visualization Manager:
         Show Robot Collision: false
         Show Robot Visual: true
       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_Goal_Tolerance: 0
-      MoveIt_Planning_Attempts: 10
-      MoveIt_Planning_Time: 5
-      MoveIt_Use_Cartesian_Path: false
-      MoveIt_Use_Constraint_Aware_IK: true
-      MoveIt_Warehouse_Host: 127.0.0.1
-      MoveIt_Warehouse_Port: 33829
-      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
-          panda_hand:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_leftfinger:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link0:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link3:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link4:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link5:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link6:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link7:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link8:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          panda_rightfinger:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-        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: 0.05 s
-        Trail Step Size: 1
-        Trajectory Topic: /move_group/display_planned_path
-      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: hand
-        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.20000000298023224
-        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
-          panda_hand:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_leftfinger:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link0:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link3:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link4:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link5:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link6:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link7:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link8:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          panda_rightfinger:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-        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
@@ -435,39 +232,35 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 2.827594041824341
+      Distance: 2.82759404
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
+        Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.11356700211763382
-        Y: 0.10592000186443329
-        Z: 2.2351800055275817e-7
+        X: 0.113567002
+        Y: 0.105920002
+        Z: 2.23518001e-07
       Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
+      Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.47020357847213745
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.470203578
       Target Frame: panda_link0
       Value: XYOrbit (rviz)
-      Yaw: 6.06496000289917
+      Yaw: 6.06496
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1029
+  Height: 741
   Help:
     collapsed: false
   Hide Left Dock: false
   Hide Right Dock: false
-  MotionPlanning:
-    collapsed: false
-  MotionPlanning - Trajectory Slider:
-    collapsed: false
-  QMainWindow State: 000000ff00000000fd000000010000000000000295000003abfc020000000ffb000000100044006900730070006c006100790073010000003d00000108000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000026d000000b5000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000014b000002410000019b00fffffffb00000024005200760069007a00560069007300750061006c0054006f006f006c00730047007500690100000392000000560000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb000000120020002d00200053006c006900640065007201000002df000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000153000001a60000000000000000000002bb000003ab00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000001000000000000017800000282fc020000000efb000000100044006900730070006c006100790073010000004200000232000000dc00fffffffb0000000800480065006c00700000000342000000bb0000007500fffffffb0000000a00560069006500770073000000026d000000b5000000b500fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000294000002410000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069010000027a0000004a0000004800fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb000000120020002d00200053006c006900640065007201000002df000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004800ffffff000003d80000028200000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   RvizVisualToolsGui:
     collapsed: false
   Trajectory - Trajectory Slider:
@@ -475,5 +268,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1366
-  X: 3740
-  Y: 333
+  X: 1906
+  Y: 1440