From 07d4a4d4c227322c9a6b926ea2b70cc2d5505d84 Mon Sep 17 00:00:00 2001
From: Tyler Weaver <tyler@picknik.ai>
Date: Thu, 1 Oct 2020 16:19:46 -0600
Subject: [PATCH 01/10] Generated using the latest features from MoveIt

---
 .setup_assistant                              |   9 +-
 CMakeLists.txt                                |   2 +-
 config/chomp_planning.yaml                    |  16 +-
 config/fake_controllers.yaml                  |   5 +-
 config/joint_limits.yaml                      |  70 +-
 config/kinematics.yaml                        |   2 +-
 config/ompl_planning.yaml                     | 178 ++---
 config/panda.srdf                             |  84 ++
 config/panda_arm.xacro                        |  37 +-
 config/ros_controllers.yaml                   |  73 ++
 config/sensors_3d.yaml                        |   3 +
 launch/chomp_planning_pipeline.launch.xml     |  21 +-
 launch/demo.launch                            |  36 +-
 launch/demo_gazebo.launch                     |  73 ++
 launch/gazebo.launch                          |  23 +
 launch/gdb_settings.gdb                       |   1 +
 launch/joystick_control.launch                |   2 +-
 launch/move_group.launch                      |  55 +-
 launch/moveit.rviz                            | 723 ++++++++++++++----
 launch/moveit_rviz.launch                     |   7 +-
 launch/ompl_planning_pipeline.launch.xml      |  21 +-
 ...panda_moveit_controller_manager.launch.xml |  13 +-
 launch/planning_context.launch                |   9 +-
 launch/planning_pipeline.launch.xml           |   4 +-
 launch/ros_controllers.launch                 |  11 +
 launch/run_benchmark_ompl.launch              |   3 +-
 launch/sensor_manager.launch.xml              |   7 +-
 launch/setup_assistant.launch                 |   2 +-
 launch/trajectory_execution.launch.xml        |   8 +-
 launch/warehouse.launch                       |   4 +-
 launch/warehouse_settings.launch.xml          |  12 +-
 package.xml                                   |  17 +-
 32 files changed, 1092 insertions(+), 439 deletions(-)
 create mode 100644 config/panda.srdf
 create mode 100644 config/ros_controllers.yaml
 create mode 100644 config/sensors_3d.yaml
 create mode 100644 launch/demo_gazebo.launch
 create mode 100644 launch/gazebo.launch
 create mode 100644 launch/gdb_settings.gdb
 create mode 100644 launch/ros_controllers.launch

diff --git a/.setup_assistant b/.setup_assistant
index 02532ad..a7e1293 100644
--- a/.setup_assistant
+++ b/.setup_assistant
@@ -2,9 +2,10 @@ moveit_setup_assistant_config:
   URDF:
     package: franka_description
     relative_path: robots/panda_arm_hand.urdf.xacro
+    xacro_args: ""
   SRDF:
-    relative_path: config/panda_arm.srdf.xacro
+    relative_path: config/panda.srdf
   CONFIG:
-    author_name: Mike Lautman
-    author_email: mike@picknik.ai
-    generated_timestamp: 1519152260
\ No newline at end of file
+    author_name: Tyler Weaver
+    author_email: tyler@picknik.ai
+    generated_timestamp: 1601757998
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5c4d559..675c9ac 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.1.3)
 project(panda_moveit_config)
 
 find_package(catkin REQUIRED)
diff --git a/config/chomp_planning.yaml b/config/chomp_planning.yaml
index a082293..75258e5 100644
--- a/config/chomp_planning.yaml
+++ b/config/chomp_planning.yaml
@@ -4,25 +4,15 @@ max_iterations_after_collision_free: 5
 smoothness_cost_weight: 0.1
 obstacle_cost_weight: 1.0
 learning_rate: 0.01
-animate_path: true
-add_randomness: false
 smoothness_cost_velocity: 0.0
 smoothness_cost_acceleration: 1.0
 smoothness_cost_jerk: 0.0
-hmc_discretization: 0.01
-hmc_stochasticity: 0.01
-hmc_annealing_factor: 0.99
-use_hamiltonian_monte_carlo: false
-ridge_factor: 0.0
+ridge_factor: 0.01
 use_pseudo_inverse: false
 pseudo_inverse_ridge_factor: 1e-4
-animate_endeffector: false
-animate_endeffector_segment: "panda_rightfinger"
 joint_update_limit: 0.1
 collision_clearence: 0.2
 collision_threshold: 0.07
-random_jump_amount: 1.0
 use_stochastic_descent: true
-enable_failure_recovery: false
-max_recovery_attempts: 5
-trajectory_initialization_method: "quintic-spline"
+enable_failure_recovery: true
+max_recovery_attempts: 5
\ No newline at end of file
diff --git a/config/fake_controllers.yaml b/config/fake_controllers.yaml
index 111ff4c..0502653 100644
--- a/config/fake_controllers.yaml
+++ b/config/fake_controllers.yaml
@@ -11,9 +11,8 @@ controller_list:
   - name: fake_hand_controller
     joints:
       - panda_finger_joint1
-      - panda_finger_joint2
-initial:
+initial:  # Define initial robot poses.
   - group: panda_arm
     pose: ready
   - group: hand
-    pose: open
+    pose: open
\ No newline at end of file
diff --git a/config/joint_limits.yaml b/config/joint_limits.yaml
index a2f08ed..c3472cc 100644
--- a/config/joint_limits.yaml
+++ b/config/joint_limits.yaml
@@ -1,55 +1,55 @@
 # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
-# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
-# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
 
-# As MoveIt! does not support jerk limits, the acceleration limits provided here are the highest values that guarantee
-# that no jerk limits will be violated. More precisely, applying Euler differentiation in the worst case (from min accel
-# to max accel in 1 ms) the acceleration limits are the ones that satisfy
-# max_jerk = (max_acceleration - min_acceleration) / 0.001
+# For beginners, we downscale velocity and acceleration limits.
+# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
+default_velocity_scaling_factor: 0.1
+default_acceleration_scaling_factor: 0.1
 
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
 joint_limits:
+  panda_finger_joint1:
+    has_velocity_limits: true
+    max_velocity: 0.2
+    has_acceleration_limits: false
+    max_acceleration: 0
+  panda_finger_joint2:
+    has_velocity_limits: true
+    max_velocity: 0.2
+    has_acceleration_limits: false
+    max_acceleration: 0
   panda_joint1:
     has_velocity_limits: true
-    max_velocity: 2.1750
-    has_acceleration_limits: true
-    max_acceleration: 3.75
+    max_velocity: 2.175
+    has_acceleration_limits: false
+    max_acceleration: 0
   panda_joint2:
     has_velocity_limits: true
-    max_velocity: 2.1750
-    has_acceleration_limits: true
-    max_acceleration: 1.875
+    max_velocity: 2.175
+    has_acceleration_limits: false
+    max_acceleration: 0
   panda_joint3:
     has_velocity_limits: true
-    max_velocity: 2.1750
-    has_acceleration_limits: true
-    max_acceleration: 2.5
+    max_velocity: 2.175
+    has_acceleration_limits: false
+    max_acceleration: 0
   panda_joint4:
     has_velocity_limits: true
-    max_velocity: 2.1750
-    has_acceleration_limits: true
-    max_acceleration: 3.125
+    max_velocity: 2.175
+    has_acceleration_limits: false
+    max_acceleration: 0
   panda_joint5:
     has_velocity_limits: true
-    max_velocity: 2.6100
-    has_acceleration_limits: true
-    max_acceleration: 3.75
+    max_velocity: 2.61
+    has_acceleration_limits: false
+    max_acceleration: 0
   panda_joint6:
     has_velocity_limits: true
-    max_velocity: 2.6100
-    has_acceleration_limits: true
-    max_acceleration: 5
-  panda_joint7:
-    has_velocity_limits: true
-    max_velocity: 2.6100
-    has_acceleration_limits: true
-    max_acceleration: 5
-  panda_finger_joint1:
-    has_velocity_limits: true
-    max_velocity: 0.1
+    max_velocity: 2.61
     has_acceleration_limits: false
     max_acceleration: 0
-  panda_finger_joint2:
+  panda_joint7:
     has_velocity_limits: true
-    max_velocity: 0.1
+    max_velocity: 2.61
     has_acceleration_limits: false
-    max_acceleration: 0
+    max_acceleration: 0
\ No newline at end of file
diff --git a/config/kinematics.yaml b/config/kinematics.yaml
index e721385..60737fa 100644
--- a/config/kinematics.yaml
+++ b/config/kinematics.yaml
@@ -1,4 +1,4 @@
 panda_arm:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
-  kinematics_solver_timeout: 0.05
+  kinematics_solver_timeout: 0.005
\ No newline at end of file
diff --git a/config/ompl_planning.yaml b/config/ompl_planning.yaml
index 491e829..6f8d2f7 100644
--- a/config/ompl_planning.yaml
+++ b/config/ompl_planning.yaml
@@ -1,42 +1,49 @@
 planner_configs:
-  SBLkConfigDefault:
+  AnytimePathShortening:
+    type: geometric::AnytimePathShortening
+    shortcut: true  # Attempt to shortcut all new solution paths
+    hybridize: true  # Compute hybrid solution trajectories
+    max_hybrid_paths: 24  # Number of hybrid paths generated per iteration
+    num_planners: 4  # The number of default planners to use for planning
+    planners: ""  # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]"
+  SBL:
     type: geometric::SBL
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-  ESTkConfigDefault:
+  EST:
     type: geometric::EST
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
     goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
-  LBKPIECEkConfigDefault:
+  LBKPIECE:
     type: geometric::LBKPIECE
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
     border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
     min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
-  BKPIECEkConfigDefault:
+  BKPIECE:
     type: geometric::BKPIECE
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
     border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
     failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
     min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
-  KPIECEkConfigDefault:
+  KPIECE:
     type: geometric::KPIECE
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
     goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
     border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9 (0.0,1.]
     failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
     min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
-  RRTkConfigDefault:
+  RRT:
     type: geometric::RRT
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
     goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
-  RRTConnectkConfigDefault:
+  RRTConnect:
     type: geometric::RRTConnect
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-  RRTstarkConfigDefault:
+  RRTstar:
     type: geometric::RRTstar
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
     goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
     delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1
-  TRRTkConfigDefault:
+  TRRT:
     type: geometric::TRRT
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
     goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
@@ -47,12 +54,12 @@ planner_configs:
     frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
     frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
     k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()
-  PRMkConfigDefault:
+  PRM:
     type: geometric::PRM
     max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
-  PRMstarkConfigDefault:
+  PRMstar:
     type: geometric::PRMstar
-  FMTkConfigDefault:
+  FMT:
     type: geometric::FMT
     num_samples: 1000  # number of states that the planner should sample. default: 1000
     radius_multiplier: 1.1  # multiplier used for the nearest neighbors search radius. default: 1.1
@@ -60,7 +67,7 @@ planner_configs:
     cache_cc: 1  # use collision checking cache. default: 1
     heuristics: 0  # activate cost to go heuristics. default: 0
     extended_fmt: 1  # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
-  BFMTkConfigDefault:
+  BFMT:
     type: geometric::BFMT
     num_samples: 1000  # number of states that the planner should sample. default: 1000
     radius_multiplier: 1.0  # multiplier used for the nearest neighbors search radius. default: 1.0
@@ -70,9 +77,9 @@ planner_configs:
     heuristics: 1  # activates cost to go heuristics. default: 1
     cache_cc: 1  # use the collision checking cache. default: 1
     extended_fmt: 1  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
-  PDSTkConfigDefault:
+  PDST:
     type: geometric::PDST
-  STRIDEkConfigDefault:
+  STRIDE:
     type: geometric::STRIDE
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
     goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
@@ -83,7 +90,7 @@ planner_configs:
     max_pts_per_leaf: 6  # max points per leaf in the GNAT. default: 6
     estimated_dimension: 0.0  # estimated dimension of the free space. default: 0.0
     min_valid_path_fraction: 0.2  # Accept partially valid moves above fraction. default: 0.2
-  BiTRRTkConfigDefault:
+  BiTRRT:
     type: geometric::BiTRRT
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
     temp_change_factor: 0.1  # how much to increase or decrease temp. default: 0.1
@@ -91,113 +98,84 @@ planner_configs:
     frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
     frountier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
     cost_threshold: 1e300  # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
-  LBTRRTkConfigDefault:
+  LBTRRT:
     type: geometric::LBTRRT
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
     goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
     epsilon: 0.4  # optimality approximation factor. default: 0.4
-  BiESTkConfigDefault:
+  BiEST:
     type: geometric::BiEST
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-  ProjESTkConfigDefault:
+  ProjEST:
     type: geometric::ProjEST
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
     goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
-  LazyPRMkConfigDefault:
+  LazyPRM:
     type: geometric::LazyPRM
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-  LazyPRMstarkConfigDefault:
+  LazyPRMstar:
     type: geometric::LazyPRMstar
-  SPARSkConfigDefault:
+  SPARS:
     type: geometric::SPARS
     stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
     sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
     dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
     max_failures: 1000  # maximum consecutive failure limit. default: 1000
-  SPARStwokConfigDefault:
+  SPARStwo:
     type: geometric::SPARStwo
     stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
     sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
     dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
     max_failures: 5000  # maximum consecutive failure limit. default: 5000
-  TrajOptDefault:
-    type: geometric::TrajOpt
-
 panda_arm:
   planner_configs:
-    - SBLkConfigDefault
-    - ESTkConfigDefault
-    - LBKPIECEkConfigDefault
-    - BKPIECEkConfigDefault
-    - KPIECEkConfigDefault
-    - RRTkConfigDefault
-    - RRTConnectkConfigDefault
-    - RRTstarkConfigDefault
-    - TRRTkConfigDefault
-    - PRMkConfigDefault
-    - PRMstarkConfigDefault
-    - FMTkConfigDefault
-    - BFMTkConfigDefault
-    - PDSTkConfigDefault
-    - STRIDEkConfigDefault
-    - BiTRRTkConfigDefault
-    - LBTRRTkConfigDefault
-    - BiESTkConfigDefault
-    - ProjESTkConfigDefault
-    - LazyPRMkConfigDefault
-    - LazyPRMstarkConfigDefault
-    - SPARSkConfigDefault
-    - SPARStwokConfigDefault
-    - TrajOptDefault
-panda_arm_hand:
-  planner_configs:
-    - SBLkConfigDefault
-    - ESTkConfigDefault
-    - LBKPIECEkConfigDefault
-    - BKPIECEkConfigDefault
-    - KPIECEkConfigDefault
-    - RRTkConfigDefault
-    - RRTConnectkConfigDefault
-    - RRTstarkConfigDefault
-    - TRRTkConfigDefault
-    - PRMkConfigDefault
-    - PRMstarkConfigDefault
-    - FMTkConfigDefault
-    - BFMTkConfigDefault
-    - PDSTkConfigDefault
-    - STRIDEkConfigDefault
-    - BiTRRTkConfigDefault
-    - LBTRRTkConfigDefault
-    - BiESTkConfigDefault
-    - ProjESTkConfigDefault
-    - LazyPRMkConfigDefault
-    - LazyPRMstarkConfigDefault
-    - SPARSkConfigDefault
-    - SPARStwokConfigDefault
-    - TrajOptDefault
+    - AnytimePathShortening
+    - SBL
+    - EST
+    - LBKPIECE
+    - BKPIECE
+    - KPIECE
+    - RRT
+    - RRTConnect
+    - RRTstar
+    - TRRT
+    - PRM
+    - PRMstar
+    - FMT
+    - BFMT
+    - PDST
+    - STRIDE
+    - BiTRRT
+    - LBTRRT
+    - BiEST
+    - ProjEST
+    - LazyPRM
+    - LazyPRMstar
+    - SPARS
+    - SPARStwo
 hand:
   planner_configs:
-    - SBLkConfigDefault
-    - ESTkConfigDefault
-    - LBKPIECEkConfigDefault
-    - BKPIECEkConfigDefault
-    - KPIECEkConfigDefault
-    - RRTkConfigDefault
-    - RRTConnectkConfigDefault
-    - RRTstarkConfigDefault
-    - TRRTkConfigDefault
-    - PRMkConfigDefault
-    - PRMstarkConfigDefault
-    - FMTkConfigDefault
-    - BFMTkConfigDefault
-    - PDSTkConfigDefault
-    - STRIDEkConfigDefault
-    - BiTRRTkConfigDefault
-    - LBTRRTkConfigDefault
-    - BiESTkConfigDefault
-    - ProjESTkConfigDefault
-    - LazyPRMkConfigDefault
-    - LazyPRMstarkConfigDefault
-    - SPARSkConfigDefault
-    - SPARStwokConfigDefault
-    - TrajOptDefault
+    - AnytimePathShortening
+    - SBL
+    - EST
+    - LBKPIECE
+    - BKPIECE
+    - KPIECE
+    - RRT
+    - RRTConnect
+    - RRTstar
+    - TRRT
+    - PRM
+    - PRMstar
+    - FMT
+    - BFMT
+    - PDST
+    - STRIDE
+    - BiTRRT
+    - LBTRRT
+    - BiEST
+    - ProjEST
+    - LazyPRM
+    - LazyPRMstar
+    - SPARS
+    - SPARStwo
diff --git a/config/panda.srdf b/config/panda.srdf
new file mode 100644
index 0000000..e9f15b1
--- /dev/null
+++ b/config/panda.srdf
@@ -0,0 +1,84 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot name="panda">
+    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+    <group name="panda_arm">
+        <joint name="virtual_joint"/>
+        <joint name="panda_joint1"/>
+        <joint name="panda_joint2"/>
+        <joint name="panda_joint3"/>
+        <joint name="panda_joint4"/>
+        <joint name="panda_joint5"/>
+        <joint name="panda_joint6"/>
+        <joint name="panda_joint7"/>
+        <joint name="panda_joint8"/>
+    </group>
+    <group name="hand">
+        <joint name="panda_hand_joint"/>
+        <joint name="panda_finger_joint1"/>
+        <joint name="panda_finger_joint2"/>
+    </group>
+    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+    <group_state name="ready" group="panda_arm">
+        <joint name="panda_joint1" value="0"/>
+        <joint name="panda_joint2" value="-0.785"/>
+        <joint name="panda_joint3" value="0"/>
+        <joint name="panda_joint4" value="-2.356"/>
+        <joint name="panda_joint5" value="0"/>
+        <joint name="panda_joint6" value="1.571"/>
+        <joint name="panda_joint7" value="-0.7855"/>
+    </group_state>
+    <group_state name="extended" group="panda_arm">
+        <joint name="panda_joint1" value="0"/>
+        <joint name="panda_joint2" value="-0.3927"/>
+        <joint name="panda_joint3" value="0"/>
+        <joint name="panda_joint4" value="-0.7855"/>
+        <joint name="panda_joint5" value="0"/>
+        <joint name="panda_joint6" value="3.142"/>
+        <joint name="panda_joint7" value="-0.785"/>
+    </group_state>
+    <group_state name="open" group="hand">
+        <joint name="panda_finger_joint1" value="0.035"/>
+        <joint name="panda_finger_joint2" value="0"/>
+    </group_state>
+    <group_state name="close" group="hand">
+        <joint name="panda_finger_joint1" value="0"/>
+        <joint name="panda_finger_joint2" value="0"/>
+    </group_state>
+    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+    <end_effector name="hand" parent_link="panda_link8" group="hand"/>
+    <!--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"/>
+    <!--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_hand" link2="panda_link4" reason="Never"/>
+    <disable_collisions link1="panda_hand" link2="panda_link5" reason="Default"/>
+    <disable_collisions link1="panda_hand" link2="panda_link6" reason="Never"/>
+    <disable_collisions link1="panda_hand" link2="panda_link7" reason="Default"/>
+    <disable_collisions link1="panda_hand" link2="panda_link8" reason="Adjacent"/>
+    <disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent"/>
+    <disable_collisions link1="panda_link0" link2="panda_link2" reason="Never"/>
+    <disable_collisions link1="panda_link0" link2="panda_link3" reason="Never"/>
+    <disable_collisions link1="panda_link0" link2="panda_link4" reason="Never"/>
+    <disable_collisions link1="panda_link1" link2="panda_link2" reason="Adjacent"/>
+    <disable_collisions link1="panda_link1" link2="panda_link3" reason="Default"/>
+    <disable_collisions link1="panda_link1" link2="panda_link4" reason="Never"/>
+    <disable_collisions link1="panda_link2" link2="panda_link3" reason="Adjacent"/>
+    <disable_collisions link1="panda_link2" link2="panda_link4" reason="Never"/>
+    <disable_collisions link1="panda_link3" link2="panda_link4" reason="Adjacent"/>
+    <disable_collisions link1="panda_link3" link2="panda_link6" reason="Never"/>
+    <disable_collisions link1="panda_link4" link2="panda_link5" reason="Adjacent"/>
+    <disable_collisions link1="panda_link4" link2="panda_link6" reason="Never"/>
+    <disable_collisions link1="panda_link4" link2="panda_link7" reason="Never"/>
+    <disable_collisions link1="panda_link4" link2="panda_link8" reason="Never"/>
+    <disable_collisions link1="panda_link5" link2="panda_link6" reason="Adjacent"/>
+    <disable_collisions link1="panda_link5" link2="panda_link7" reason="Default"/>
+    <disable_collisions link1="panda_link6" link2="panda_link7" reason="Adjacent"/>
+    <disable_collisions link1="panda_link7" link2="panda_link8" reason="Adjacent"/>
+</robot>
diff --git a/config/panda_arm.xacro b/config/panda_arm.xacro
index 4a4e9f2..a551906 100644
--- a/config/panda_arm.xacro
+++ b/config/panda_arm.xacro
@@ -11,31 +11,22 @@
     </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="ready" group="panda_arm">
-      <joint name="panda_joint1" value="0" />
-      <joint name="panda_joint2" value="-0.785" />
-      <joint name="panda_joint3" value="0" />
-      <joint name="panda_joint4" value="-2.356" />
-      <joint name="panda_joint5" value="0" />
-      <joint name="panda_joint6" value="1.571" />
-      <joint name="panda_joint7" value="0.785" />
+        <joint name="panda_joint1" value="0"/>
+        <joint name="panda_joint2" value="-0.785"/>
+        <joint name="panda_joint3" value="0"/>
+        <joint name="panda_joint4" value="-2.356"/>
+        <joint name="panda_joint5" value="0"/>
+        <joint name="panda_joint6" value="1.571"/>
+        <joint name="panda_joint7" value="-0.7855"/>
     </group_state>
     <group_state name="extended" group="panda_arm">
-      <joint name="panda_joint1" value="0" />
-      <joint name="panda_joint2" value="0" />
-      <joint name="panda_joint3" value="0" />
-      <joint name="panda_joint4" value="0" />
-      <joint name="panda_joint5" value="0" />
-      <joint name="panda_joint6" value="0" />
-      <joint name="panda_joint7" value="0.785" />
-    </group_state>
-    <group_state name="transport" group="panda_arm">
-      <joint name="panda_joint1" value="0" />
-      <joint name="panda_joint2" value="-0.5599" />
-      <joint name="panda_joint3" value="0" />
-      <joint name="panda_joint4" value="-2.97" />
-      <joint name="panda_joint5" value="0" />
-      <joint name="panda_joint6" value="0" />
-      <joint name="panda_joint7" value="0.785" />
+        <joint name="panda_joint1" value="0"/>
+        <joint name="panda_joint2" value="-0.3927"/>
+        <joint name="panda_joint3" value="0"/>
+        <joint name="panda_joint4" value="-0.7855"/>
+        <joint name="panda_joint5" value="0"/>
+        <joint name="panda_joint6" value="3.142"/>
+        <joint name="panda_joint7" value="-0.785"/>
     </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)-->
diff --git a/config/ros_controllers.yaml b/config/ros_controllers.yaml
new file mode 100644
index 0000000..0698869
--- /dev/null
+++ b/config/ros_controllers.yaml
@@ -0,0 +1,73 @@
+# Simulation settings for using moveit_sim_controllers
+moveit_sim_hw_interface:
+  joint_model_group: panda_arm
+  joint_model_group_pose: ready
+# Settings for ros_control_boilerplate control loop
+generic_hw_control_loop:
+  loop_hz: 300
+  cycle_time_error_threshold: 0.01
+# Settings for ros_control hardware interface
+hardware_interface:
+  joints:
+    - panda_joint1
+    - panda_joint2
+    - panda_joint3
+    - panda_joint4
+    - panda_joint5
+    - panda_joint6
+    - panda_joint7
+    - panda_finger_joint1
+  sim_control_mode: 1  # 0: position, 1: velocity
+# Publish all joint states
+# Creates the /joint_states topic necessary in ROS
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
+controller_list:
+  []
+arm_position_controller:
+  type: position_controllers/JointPositionController
+  joints:
+    - panda_joint1
+    - panda_joint2
+    - panda_joint3
+    - panda_joint4
+    - panda_joint5
+    - panda_joint6
+    - panda_joint7
+  gains:
+    panda_joint1:
+      p: 100
+      d: 1
+      i: 1
+      i_clamp: 1
+    panda_joint2:
+      p: 100
+      d: 1
+      i: 1
+      i_clamp: 1
+    panda_joint3:
+      p: 100
+      d: 1
+      i: 1
+      i_clamp: 1
+    panda_joint4:
+      p: 100
+      d: 1
+      i: 1
+      i_clamp: 1
+    panda_joint5:
+      p: 100
+      d: 1
+      i: 1
+      i_clamp: 1
+    panda_joint6:
+      p: 100
+      d: 1
+      i: 1
+      i_clamp: 1
+    panda_joint7:
+      p: 100
+      d: 1
+      i: 1
+      i_clamp: 1
\ No newline at end of file
diff --git a/config/sensors_3d.yaml b/config/sensors_3d.yaml
new file mode 100644
index 0000000..d2955dc
--- /dev/null
+++ b/config/sensors_3d.yaml
@@ -0,0 +1,3 @@
+# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it
+sensors:
+  - {}
\ No newline at end of file
diff --git a/launch/chomp_planning_pipeline.launch.xml b/launch/chomp_planning_pipeline.launch.xml
index 0fb092f..a4d5525 100644
--- a/launch/chomp_planning_pipeline.launch.xml
+++ b/launch/chomp_planning_pipeline.launch.xml
@@ -1,17 +1,20 @@
 <launch>
-   <!-- CHOMP Plugin for MoveIt! -->
+   <!-- CHOMP Plugin for MoveIt -->
    <arg name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
    <arg name="start_state_max_bounds_error" value="0.1" />
+   <!-- The request adapters (plugins) used when planning.
+        ORDER MATTERS -->
+   <arg name="planning_adapters"
+        value="default_planner_request_adapters/AddTimeParameterization
+               default_planner_request_adapters/ResolveConstraintFrames
+               default_planner_request_adapters/FixWorkspaceBounds
+               default_planner_request_adapters/FixStartStateBounds
+               default_planner_request_adapters/FixStartStateCollision
+               default_planner_request_adapters/FixStartStatePathConstraints"
+               />
 
    <param name="planning_plugin" value="$(arg planning_plugin)" />
-   <param name="request_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
-       default_planner_request_adapters/ResolveConstraintFrames"
-       />
+   <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/chomp_planning.yaml" />
diff --git a/launch/demo.launch b/launch/demo.launch
index 2ebb63b..9e48ded 100644
--- a/launch/demo.launch
+++ b/launch/demo.launch
@@ -1,5 +1,8 @@
 <launch>
 
+  <!-- specify the planning pipeline -->
+  <arg name="pipeline" default="ompl" />
+
   <!-- By default, we do not start a database (it can be large) -->
   <arg name="db" default="false" />
   <!-- Allow user to specify database location -->
@@ -7,33 +10,31 @@
 
   <!-- By default, we are not in debug mode -->
   <arg name="debug" default="false" />
-  <arg name="pipeline" default="ompl" />
+
+  <!-- By default, we will load or override the robot_description -->
+  <arg name="load_robot_description" default="true"/>
 
   <!--
   By default, hide joint_state_publisher's GUI
 
-  MoveIt!'s "demo" mode replaces the real robot driver with the joint_state_publisher.
+  MoveIt's "demo" mode replaces the real robot driver with the joint_state_publisher.
   The latter one maintains and publishes the current joint configuration of the simulated robot.
   It also provides a GUI to move the simulated robot around "manually".
   This corresponds to moving around the real robot without the use of MoveIt.
   -->
-  <arg name="rviz_tutorial" default="false" />
   <arg name="use_gui" default="false" />
-
-  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
-    <arg name="load_robot_description" value="true"/>
-  </include>
+  <arg name="use_rviz" default="true" />
 
   <!-- If needed, broadcast static tf for robot root -->
-  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world panda_link0" />
+  
 
   <!-- We do not have a robot connected, so publish fake joint states -->
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="/use_gui" value="$(arg use_gui)"/>
-    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
+    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
+  </node>
+  <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
+    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
   </node>
-  <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
 
   <!-- 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" />
@@ -44,12 +45,13 @@
     <arg name="fake_execution" value="true"/>
     <arg name="info" value="true"/>
     <arg name="debug" value="$(arg debug)"/>
-    <arg name="pipeline" value="$(arg pipeline)"  />
+    <arg name="pipeline" value="$(arg pipeline)"/>
+    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
   </include>
 
-  <!-- Run Rviz -->
-  <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch">
-    <arg name="rviz_tutorial" value="$(arg rviz_tutorial)"/>
+  <!-- Run Rviz and load the default config to see the state of the move_group node -->
+  <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" if="$(arg use_rviz)">
+    <arg name="rviz_config" value="$(find panda_moveit_config)/launch/moveit.rviz"/>
     <arg name="debug" value="$(arg debug)"/>
   </include>
 
diff --git a/launch/demo_gazebo.launch b/launch/demo_gazebo.launch
new file mode 100644
index 0000000..9ce0fd4
--- /dev/null
+++ b/launch/demo_gazebo.launch
@@ -0,0 +1,73 @@
+<launch>
+
+  <!-- By default, we do not start a database (it can be large) -->
+  <arg name="db" default="false" />
+  <!-- Allow user to specify database location -->
+  <arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db" />
+
+  <!-- By default, we are not in debug mode -->
+  <arg name="debug" default="false" />
+
+  <!-- By default, we won't load or override the robot_description -->
+  <arg name="load_robot_description" default="false"/>
+
+  <!--
+  By default, hide joint_state_publisher's GUI
+
+  MoveIt's "demo" mode replaces the real robot driver with the joint_state_publisher.
+  The latter one maintains and publishes the current joint configuration of the simulated robot.
+  It also provides a GUI to move the simulated robot around "manually".
+  This corresponds to moving around the real robot without the use of MoveIt.
+  -->
+  <arg name="use_gui" default="false" />
+
+  <!-- Gazebo specific options -->
+  <arg name="gazebo_gui" default="true"/>
+  <arg name="paused" default="false"/>
+  <!-- By default, use the urdf location provided from the package -->
+  <arg name="urdf_path" default="$(find franka_description)/robots/panda_arm_hand.urdf.xacro"/>
+
+  <!-- launch the gazebo simulator and spawn the robot -->
+  <include file="$(find panda_moveit_config)/launch/gazebo.launch" >
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="gazebo_gui" value="$(arg gazebo_gui)"/>
+    <arg name="urdf_path" value="$(arg urdf_path)"/>
+  </include>
+
+  <!-- If needed, broadcast static tf for robot root -->
+  
+
+  <!-- We do not have a robot connected, so publish fake joint states -->
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
+    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
+    <rosparam param="source_list">[/joint_states]</rosparam>
+  </node>
+  <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
+    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
+    <rosparam param="source_list">[/joint_states]</rosparam>
+  </node>
+
+  <!-- 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" />
+
+  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
+  <include file="$(find panda_moveit_config)/launch/move_group.launch">
+    <arg name="allow_trajectory_execution" value="true"/>
+    <arg name="fake_execution" value="false"/>
+    <arg name="info" value="true"/>
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
+  </include>  
+
+  <!-- Run Rviz and load the default config to see the state of the move_group node -->
+  <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch">
+    <arg name="rviz_config" value="$(find panda_moveit_config)/launch/moveit.rviz"/>
+    <arg name="debug" value="$(arg debug)"/>
+  </include>
+
+  <!-- If database loading was enabled, start mongodb as well -->
+  <include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
+    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
+  </include>
+
+</launch>
diff --git a/launch/gazebo.launch b/launch/gazebo.launch
new file mode 100644
index 0000000..df41640
--- /dev/null
+++ b/launch/gazebo.launch
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="paused" default="false"/>
+  <arg name="gazebo_gui" default="true"/>
+  <arg name="urdf_path" default="$(find franka_description)/robots/panda_arm_hand.urdf.xacro"/>
+
+  <!-- startup simulated world -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" default="worlds/empty.world"/>
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="gui" value="$(arg gazebo_gui)"/>
+  </include>
+
+  <!-- send robot urdf to param server -->
+  <param name="robot_description" textfile="$(arg urdf_path)" />
+
+  <!-- push robot_description to factory and spawn robot in gazebo at the origin, change x,y,z arguments to spawn in a different position -->
+  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 0"
+    respawn="false" output="screen" />
+
+  <include file="$(find panda_moveit_config)/launch/ros_controllers.launch"/>
+
+</launch>
diff --git a/launch/gdb_settings.gdb b/launch/gdb_settings.gdb
new file mode 100644
index 0000000..ad44f4f
--- /dev/null
+++ b/launch/gdb_settings.gdb
@@ -0,0 +1 @@
+b robot_poses_widget.cpp:520
\ No newline at end of file
diff --git a/launch/joystick_control.launch b/launch/joystick_control.launch
index f741735..9411f6e 100644
--- a/launch/joystick_control.launch
+++ b/launch/joystick_control.launch
@@ -13,5 +13,5 @@
 
   <!-- Launch python interface -->
   <node pkg="moveit_ros_visualization" type="moveit_joy.py" output="screen" name="moveit_joy"/>
-        
+
 </launch>
diff --git a/launch/move_group.launch b/launch/move_group.launch
index f5f9e7d..488a842 100644
--- a/launch/move_group.launch
+++ b/launch/move_group.launch
@@ -1,13 +1,5 @@
 <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="pipeline" default="ompl" />
-
   <!-- GDB Debug Option -->
   <arg name="debug" default="false" />
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
@@ -20,12 +12,37 @@
   <arg     if="$(arg info)" name="command_args" value="--debug" />
 
   <!-- move_group settings -->
+  <arg name="pipeline" default="ompl" />
   <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"/>
 
+  <arg name="capabilities" default=""/>
+  <arg name="disable_capabilities" default=""/>
+  <!-- load these non-default MoveGroup capabilities (space seperated) -->
+  <!--
+  <arg name="capabilities" value="
+                a_package/AwsomeMotionPlanningCapability
+                another_package/GraspPlanningPipeline
+                " />
+  -->
+
+  <!-- inhibit these default MoveGroup capabilities (space seperated) -->
+  <!--
+  <arg name="disable_capabilities" value="
+                move_group/MoveGroupKinematicsService
+                move_group/ClearOctomapService
+                " />
+  -->
+
+  <arg name="load_robot_description" default="true" />
+  <!-- load URDF, SRDF and joint_limits configuration -->
+  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+    <arg name="load_robot_description" value="$(arg load_robot_description)" />
+  </include>
+
   <!-- Planning Functionality -->
   <include ns="move_group" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
     <arg name="pipeline" value="$(arg pipeline)" />
@@ -34,8 +51,7 @@
   <!-- 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="panda" unless="$(arg fake_execution)"/>
     <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
   </include>
 
@@ -52,30 +68,15 @@
     <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)" />
+    <param name="capabilities" value="$(arg capabilities)"/>
+    <param name="disable_capabilities" value="$(arg disable_capabilities)"/>
 
-    <!-- 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/moveit.rviz b/launch/moveit.rviz
index 9a49749..c4d6ae5 100644
--- a/launch/moveit.rviz
+++ b/launch/moveit.rviz
@@ -4,10 +4,9 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /MarkerArray1
-        - /PlanningScene1/Scene Robot1
-      Splitter Ratio: 0.742560029
-    Tree Height: 408
+        - /MotionPlanning1
+      Splitter Ratio: 0.74256
+    Tree Height: 664
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -15,8 +14,6 @@ Panels:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
-  - Class: rviz_visual_tools/RvizVisualToolsGui
-    Name: RvizVisualToolsGui
 Visualization Manager:
   Class: ""
   Displays:
@@ -26,7 +23,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.0299999993
+        Line Width: 0.03
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -38,235 +35,655 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Class: rviz/MarkerArray
+    - Class: moveit_rviz_plugin/MotionPlanning
       Enabled: true
-      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
-      Color Enabled: false
-      Enabled: true
-      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
-      Name: Trajectory
-      Robot Alpha: 0.5
-      Robot Color: 150; 50; 150
-      Robot Description: robot_description
-      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
-      Value: true
-    - Class: moveit_rviz_plugin/PlanningScene
-      Enabled: true
-      Move Group Namespace: ""
-      Name: PlanningScene
-      Planning Scene Topic: /move_group/monitored_planning_scene
+      MoveIt_Goal_Tolerance: 0
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Constraint_Aware_IK: true
+      MoveIt_Warehouse_Host: 127.0.0.1
+      MoveIt_Warehouse_Port: 33829
+      Name: MotionPlanning
+      Planned Path:
+        Links:
+          base_bellow_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          base_footprint:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          bl_caster_l_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          bl_caster_r_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          bl_caster_rotation_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          br_caster_l_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          br_caster_r_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          br_caster_rotation_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          double_stereo_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          fl_caster_l_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          fl_caster_r_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          fl_caster_rotation_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          fr_caster_l_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          fr_caster_r_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          fr_caster_rotation_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          head_mount_kinect_ir_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          head_mount_kinect_rgb_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          head_mount_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          head_mount_prosilica_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          head_pan_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          head_plate_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          head_tilt_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_elbow_flex_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_forearm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_forearm_roll_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_gripper_l_finger_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_gripper_l_finger_tip_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_gripper_motor_accelerometer_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_gripper_palm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_gripper_r_finger_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_gripper_r_finger_tip_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_shoulder_lift_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_shoulder_pan_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_upper_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_upper_arm_roll_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_wrist_flex_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_wrist_roll_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          laser_tilt_mount_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_elbow_flex_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_forearm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_forearm_roll_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_gripper_l_finger_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_gripper_l_finger_tip_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_gripper_motor_accelerometer_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_gripper_palm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_gripper_r_finger_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_gripper_r_finger_tip_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_shoulder_lift_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_shoulder_pan_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_upper_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_upper_arm_roll_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_wrist_flex_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_wrist_roll_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          sensor_mount_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          torso_lift_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Loop Animation: true
+        Robot Alpha: 0.5
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 0.05 s
+        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
+      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: left_arm
+        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.899999976
+        Scene Alpha: 1
         Scene Color: 50; 230; 50
-        Scene Display Time: 0.200000003
+        Scene Display Time: 0.2
         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:
+          base_bellow_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          panda_leftfinger:
+          base_footprint:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          panda_link0:
+          base_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          panda_link1:
+          bl_caster_l_wheel_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          panda_link2:
+          bl_caster_r_wheel_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          panda_link3:
+          bl_caster_rotation_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          panda_link4:
+          br_caster_l_wheel_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          panda_link5:
+          br_caster_r_wheel_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          panda_link6:
+          br_caster_rotation_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          panda_link7:
+          double_stereo_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          panda_link8:
+          fl_caster_l_wheel_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          panda_rightfinger:
+            Value: true
+          fl_caster_r_wheel_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-        Robot Alpha: 1
-        Show Robot Collision: false
-        Show Robot Visual: true
+          fl_caster_rotation_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          fr_caster_l_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          fr_caster_r_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          fr_caster_rotation_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          head_mount_kinect_ir_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          head_mount_kinect_rgb_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          head_mount_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          head_mount_prosilica_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          head_pan_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          head_plate_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          head_tilt_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_elbow_flex_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_forearm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_forearm_roll_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_gripper_l_finger_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_gripper_l_finger_tip_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_gripper_motor_accelerometer_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_gripper_palm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_gripper_r_finger_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_gripper_r_finger_tip_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_shoulder_lift_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_shoulder_pan_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_upper_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_upper_arm_roll_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_wrist_flex_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          l_wrist_roll_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          laser_tilt_mount_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_elbow_flex_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_forearm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_forearm_roll_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_gripper_l_finger_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_gripper_l_finger_tip_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_gripper_motor_accelerometer_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_gripper_palm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_gripper_r_finger_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_gripper_r_finger_tip_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_shoulder_lift_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_shoulder_pan_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_upper_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_upper_arm_roll_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_wrist_flex_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          r_wrist_roll_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          sensor_mount_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          torso_lift_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Robot Alpha: 0.5
+        Show Scene Robot: true
       Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Default Light: true
     Fixed Frame: panda_link0
-    Frame Rate: 30
   Name: root
   Tools:
     - Class: rviz/Interact
       Hide Inactive Objects: true
     - Class: rviz/MoveCamera
     - Class: rviz/Select
-    - Class: rviz_visual_tools/KeyTool
   Value: true
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 2.82759404
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.0599999987
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
+      Distance: 2.9965
       Focal Point:
-        X: 0.113567002
-        Y: 0.105920002
-        Z: 2.23518001e-07
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.0500000007
-      Invert Z Axis: false
+        X: 0.113567
+        Y: 0.10592
+        Z: 2.23518e-07
       Name: Current View
-      Near Clip Distance: 0.00999999978
-      Pitch: 0.470203578
-      Target Frame: panda_link0
+      Near Clip Distance: 0.01
+      Pitch: 0.509797
+      Target Frame: /panda_link0
       Value: XYOrbit (rviz)
-      Yaw: 6.06496
+      Yaw: 5.65995
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 741
+  Height: 1337
   Help:
     collapsed: false
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000001000000000000017800000282fc020000000efb000000100044006900730070006c006100790073010000004200000232000000dc00fffffffb0000000800480065006c00700000000342000000bb0000007500fffffffb0000000a00560069006500770073000000026d000000b5000000b500fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000294000002410000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069010000027a0000004a0000004800fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb000000120020002d00200053006c006900640065007201000002df000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004800ffffff000003d80000028200000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  RvizVisualToolsGui:
-    collapsed: false
-  Trajectory - Trajectory Slider:
+  Motion Planning:
     collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
-  Width: 1366
-  X: 1906
-  Y: 1440
+  Width: 1828
+  X: 459
+  Y: -243
diff --git a/launch/moveit_rviz.launch b/launch/moveit_rviz.launch
index 016e5dc..615b2f1 100644
--- a/launch/moveit_rviz.launch
+++ b/launch/moveit_rviz.launch
@@ -4,13 +4,12 @@
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
   <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
 
-  <arg name="rviz_tutorial" default="false" />
-  <arg unless="$(arg rviz_tutorial)" name="command_args" value="-d $(find panda_moveit_config)/launch/moveit.rviz" />
-  <arg     if="$(arg rviz_tutorial)" name="command_args" value="-d $(find panda_moveit_config)/launch/moveit_empty.rviz" />
+  <arg name="rviz_config" default="" />
+  <arg     if="$(eval rviz_config=='')" name="command_args" value="" />
+  <arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)" />
 
   <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
 	args="$(arg command_args)" output="screen">
-    <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
   </node>
 
 </launch>
diff --git a/launch/ompl_planning_pipeline.launch.xml b/launch/ompl_planning_pipeline.launch.xml
index de9a542..649a253 100644
--- a/launch/ompl_planning_pipeline.launch.xml
+++ b/launch/ompl_planning_pipeline.launch.xml
@@ -1,18 +1,19 @@
 <launch>
 
-  <!-- OMPL Plugin for MoveIt! -->
+  <!-- OMPL Plugin for MoveIt -->
   <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />
 
-  <!-- The request adapters (plugins) used when planning with OMPL. 
+  <!-- The request adapters (plugins) used when planning with OMPL.
        ORDER MATTERS -->
-  <arg name="planning_adapters" default="
-       default_planner_request_adapters/AddTimeParameterization
-       default_planner_request_adapters/ResolveConstraintFrames
-       default_planner_request_adapters/FixWorkspaceBounds
-       default_planner_request_adapters/FixStartStateBounds
-       default_planner_request_adapters/FixStartStateCollision
-       default_planner_request_adapters/FixStartStatePathConstraints"
-       />
+  <arg name="planning_adapters"
+       value="default_planner_request_adapters/AddTimeParameterization
+              default_planner_request_adapters/ResolveConstraintFrames
+              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)" />
diff --git a/launch/panda_moveit_controller_manager.launch.xml b/launch/panda_moveit_controller_manager.launch.xml
index ea4c0ad..5536e61 100644
--- a/launch/panda_moveit_controller_manager.launch.xml
+++ b/launch/panda_moveit_controller_manager.launch.xml
@@ -1,7 +1,10 @@
- <launch>
-  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
+<launch>
+
+  <!-- loads moveit_controller_manager on the parameter server which is taken as argument 
+    if no argument is passed, moveit_simple_controller_manager will be set -->
   <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
   <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
-  <!-- load controller_list -->
-  <rosparam file="$(find panda_moveit_config)/config/panda_controllers.yaml"/>
- </launch>
+
+  <!-- loads ros_controllers to the param server -->
+  <rosparam file="$(find panda_moveit_config)/config/ros_controllers.yaml"/>
+</launch>
diff --git a/launch/planning_context.launch b/launch/planning_context.launch
index f0ec470..8cadd11 100644
--- a/launch/planning_context.launch
+++ b/launch/planning_context.launch
@@ -1,6 +1,4 @@
 <launch>
-  <arg name="load_gripper" default="true" />
-
   <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
   <arg name="load_robot_description" default="false"/>
 
@@ -8,12 +6,10 @@
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param if="$(eval arg('load_robot_description') and arg('load_gripper'))" name="$(arg robot_description)" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'"/>
-  <param if="$(eval arg('load_robot_description') and not arg('load_gripper'))" name="$(arg robot_description)" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm.urdf.xacro'"/>
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="xacro  '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'"/>
 
   <!-- The semantic description that corresponds to the URDF -->
-  <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find panda_moveit_config)/config/panda_arm_hand.srdf.xacro'" if="$(arg load_gripper)" />
-  <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find panda_moveit_config)/config/panda_arm.srdf.xacro'" unless="$(arg load_gripper)" />
+  <param name="$(arg robot_description)_semantic" textfile="$(find panda_moveit_config)/config/panda.srdf" />
 
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">
@@ -23,6 +19,7 @@
   <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
   <group ns="$(arg robot_description)_kinematics">
     <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+
   </group>
 
 </launch>
diff --git a/launch/planning_pipeline.launch.xml b/launch/planning_pipeline.launch.xml
index ac6838d..64ec360 100644
--- a/launch/planning_pipeline.launch.xml
+++ b/launch/planning_pipeline.launch.xml
@@ -1,7 +1,7 @@
 <launch>
 
-  <!-- This file makes it easy to include different planning pipelines; 
-       It is assumed that all planning pipelines are named XXX_planning_pipeline.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="ompl" />
 
diff --git a/launch/ros_controllers.launch b/launch/ros_controllers.launch
new file mode 100644
index 0000000..f272954
--- /dev/null
+++ b/launch/ros_controllers.launch
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- Load joint controller configurations from YAML file to parameter server -->
+  <rosparam file="$(find panda_moveit_config)/config/ros_controllers.yaml" command="load"/>
+
+  <!-- Load the controllers -->
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+    output="screen" args="arm_position_controller "/>
+
+</launch>
diff --git a/launch/run_benchmark_ompl.launch b/launch/run_benchmark_ompl.launch
index 6bf8ba2..0252481 100644
--- a/launch/run_benchmark_ompl.launch
+++ b/launch/run_benchmark_ompl.launch
@@ -11,11 +11,10 @@
   <!-- Start the database -->
   <include file="$(find panda_moveit_config)/launch/warehouse.launch">
     <arg name="moveit_warehouse_database_path" value="moveit_ompl_benchmark_warehouse"/>
-  </include>  
+  </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/ompl_planning.yaml"/>
   </node>
 
diff --git a/launch/sensor_manager.launch.xml b/launch/sensor_manager.launch.xml
index 67aa2fd..dbbbd95 100644
--- a/launch/sensor_manager.launch.xml
+++ b/launch/sensor_manager.launch.xml
@@ -2,13 +2,12 @@
 
   <!-- This file makes it easy to include the settings for sensor managers -->
 
-  <!-- Param for kinect config -->
-  <rosparam command="load" file="$(find panda_moveit_config)/config/sensors_kinect_pointcloud.yaml" />
+  <!-- Params for 3D sensors config -->
+  <rosparam command="load" file="$(find panda_moveit_config)/config/sensors_3d.yaml" />
 
   <!-- Params for the octomap monitor -->
   <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
-  <param name="octomap_frame" type="string" value="camera_rgb_optical_frame" />
-  <param name="octomap_resolution" type="double" value="0.05" />
+  <param name="octomap_resolution" type="double" value="0.025" />
   <param name="max_range" type="double" value="5.0" />
 
   <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
diff --git a/launch/setup_assistant.launch b/launch/setup_assistant.launch
index 95d88d7..70ae6ff 100644
--- a/launch/setup_assistant.launch
+++ b/launch/setup_assistant.launch
@@ -7,7 +7,7 @@
   <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
 
   <!-- Run -->
-  <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant" 
+  <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
 	args="--config_pkg=panda_moveit_config"
 	launch-prefix="$(arg launch_prefix)"
         output="screen" />
diff --git a/launch/trajectory_execution.launch.xml b/launch/trajectory_execution.launch.xml
index b6c03c9..e74530b 100644
--- a/launch/trajectory_execution.launch.xml
+++ b/launch/trajectory_execution.launch.xml
@@ -1,8 +1,8 @@
 <launch>
 
-  <!-- This file makes it easy to include the settings for trajectory execution  -->  
+  <!-- This file makes it easy to include the settings for trajectory execution  -->
 
-  <!-- Flag indicating whether MoveIt! is allowed to load/unload  or switch controllers -->
+  <!-- Flag indicating whether MoveIt is allowed to load/unload  or switch controllers -->
   <arg name="moveit_manage_controllers" default="true"/>
   <param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
 
@@ -12,9 +12,9 @@
   <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
   <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
   <param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->
-  
+
   <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
   <arg name="moveit_controller_manager" default="panda" />
   <include file="$(find panda_moveit_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />
-  
+
 </launch>
diff --git a/launch/warehouse.launch b/launch/warehouse.launch
index a4a1374..7f020e1 100644
--- a/launch/warehouse.launch
+++ b/launch/warehouse.launch
@@ -1,9 +1,9 @@
 <launch>
-  
+
   <!-- The path to the database must be specified -->
   <arg name="moveit_warehouse_database_path" />
 
-  <!-- Load warehouse parameters -->  
+  <!-- Load warehouse parameters -->
   <include file="$(find panda_moveit_config)/launch/warehouse_settings.launch.xml" />
 
   <!-- Run the DB server -->
diff --git a/launch/warehouse_settings.launch.xml b/launch/warehouse_settings.launch.xml
index 772c922..e473b08 100644
--- a/launch/warehouse_settings.launch.xml
+++ b/launch/warehouse_settings.launch.xml
@@ -1,14 +1,16 @@
 <launch>
-  <!-- Set the parameters for the warehouse. -->
+  <!-- Set the parameters for the warehouse and run the mongodb server. -->
 
-  <!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->  
-  <arg name="moveit_warehouse_port" default="33829" /> 
+  <!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->
+  <arg name="moveit_warehouse_port" default="33829" />
 
   <!-- The default DB host for moveit -->
-  <arg name="moveit_warehouse_host" default="localhost" /> 
-  
+  <arg name="moveit_warehouse_host" default="localhost" />
+
   <!-- Set parameters for the warehouse -->
   <param name="warehouse_port" value="$(arg moveit_warehouse_port)"/>
   <param name="warehouse_host" value="$(arg moveit_warehouse_host)"/>
+  <param name="warehouse_exec" value="mongod" />
   <param name="warehouse_plugin" value="warehouse_ros_mongo::MongoDatabaseConnection" />
+
 </launch>
diff --git a/package.xml b/package.xml
index e275029..6120061 100644
--- a/package.xml
+++ b/package.xml
@@ -1,11 +1,12 @@
 <package>
+
   <name>panda_moveit_config</name>
   <version>0.7.4</version>
   <description>
-     An automatically generated package with all the configuration and launch files for using the panda with the MoveIt! Motion Planning Framework
+     An automatically generated package with all the configuration and launch files for using the panda with the MoveIt Motion Planning Framework
   </description>
-  <author email="mike@picknik.ai">Mike Lautman</author>
-  <maintainer email="mike@picknik.ai">Mike Lautman</maintainer>
+  <author email="tyler@picknik.ai">Tyler Weaver</author>
+  <maintainer email="tyler@picknik.ai">Tyler Weaver</maintainer>
 
   <license>BSD</license>
 
@@ -15,18 +16,20 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <run_depend>franka_description</run_depend>
   <run_depend>moveit_ros_move_group</run_depend>
   <run_depend>moveit_fake_controller_manager</run_depend>
   <run_depend>moveit_kinematics</run_depend>
   <run_depend>moveit_planners_ompl</run_depend>
   <run_depend>moveit_ros_visualization</run_depend>
+  <run_depend>moveit_setup_assistant</run_depend>
   <run_depend>joint_state_publisher</run_depend>
   <run_depend>joint_state_publisher_gui</run_depend>
   <run_depend>robot_state_publisher</run_depend>
+  <run_depend>tf2_ros</run_depend>
   <run_depend>xacro</run_depend>
-  <run_depend>topic_tools</run_depend>
-  <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment.
-       Commented the dependency until this works. -->
+  <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
   <!-- <run_depend>warehouse_ros_mongo</run_depend> -->
+    <run_depend>franka_description</run_depend>
+
+
 </package>

From 07e539e366d186ca89b9eb0ae5c5699b92364f68 Mon Sep 17 00:00:00 2001
From: Tyler Weaver <tyler@picknik.ai>
Date: Sat, 3 Oct 2020 12:51:54 -0600
Subject: [PATCH 02/10] restore rviz file

---
 launch/moveit.rviz | 723 ++++++++++-----------------------------------
 1 file changed, 153 insertions(+), 570 deletions(-)

diff --git a/launch/moveit.rviz b/launch/moveit.rviz
index c4d6ae5..9a49749 100644
--- a/launch/moveit.rviz
+++ b/launch/moveit.rviz
@@ -4,9 +4,10 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /MotionPlanning1
-      Splitter Ratio: 0.74256
-    Tree Height: 664
+        - /MarkerArray1
+        - /PlanningScene1/Scene Robot1
+      Splitter Ratio: 0.742560029
+    Tree Height: 408
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -14,6 +15,8 @@ Panels:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
+  - Class: rviz_visual_tools/RvizVisualToolsGui
+    Name: RvizVisualToolsGui
 Visualization Manager:
   Class: ""
   Displays:
@@ -23,7 +26,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.03
+        Line Width: 0.0299999993
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -35,655 +38,235 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Class: moveit_rviz_plugin/MotionPlanning
+    - Class: rviz/MarkerArray
       Enabled: true
-      MoveIt_Goal_Tolerance: 0
-      MoveIt_Planning_Time: 5
-      MoveIt_Use_Constraint_Aware_IK: true
-      MoveIt_Warehouse_Host: 127.0.0.1
-      MoveIt_Warehouse_Port: 33829
-      Name: MotionPlanning
-      Planned Path:
-        Links:
-          base_bellow_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_footprint:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          double_stereo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_kinect_ir_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_kinect_rgb_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_prosilica_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_plate_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_tilt_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_forearm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_forearm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_l_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_l_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_motor_accelerometer_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_palm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_r_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_r_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_shoulder_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_shoulder_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_upper_arm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_upper_arm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_wrist_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          laser_tilt_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_forearm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_forearm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_l_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_l_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_motor_accelerometer_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_palm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_r_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_r_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_shoulder_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_shoulder_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_upper_arm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_upper_arm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_wrist_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          sensor_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          torso_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-        Loop Animation: true
-        Robot Alpha: 0.5
-        Show Robot Collision: false
-        Show Robot Visual: true
-        Show Trail: false
-        State Display Time: 0.05 s
-        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
-      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: left_arm
-        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
+      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
+      Color Enabled: false
+      Enabled: true
+      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
+      Name: Trajectory
+      Robot Alpha: 0.5
+      Robot Color: 150; 50; 150
+      Robot Description: robot_description
+      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
+      Value: true
+    - Class: moveit_rviz_plugin/PlanningScene
+      Enabled: true
+      Move Group Namespace: ""
+      Name: PlanningScene
+      Planning Scene Topic: /move_group/monitored_planning_scene
       Robot Description: robot_description
       Scene Geometry:
-        Scene Alpha: 1
+        Scene Alpha: 0.899999976
         Scene Color: 50; 230; 50
-        Scene Display Time: 0.2
+        Scene Display Time: 0.200000003
         Show Scene Geometry: true
         Voxel Coloring: Z-Axis
         Voxel Rendering: Occupied Voxels
       Scene Robot:
         Attached Body Color: 150; 50; 150
         Links:
-          base_bellow_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_footprint:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          double_stereo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_kinect_ir_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_kinect_rgb_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_prosilica_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_plate_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_tilt_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_forearm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_forearm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_l_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_l_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_motor_accelerometer_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_palm_link:
+          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
-          l_gripper_r_finger_link:
+          panda_leftfinger:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          l_gripper_r_finger_tip_link:
+          panda_link0:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          l_shoulder_lift_link:
+          panda_link1:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          l_shoulder_pan_link:
+          panda_link2:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          l_upper_arm_link:
+          panda_link3:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          l_upper_arm_roll_link:
+          panda_link4:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          l_wrist_flex_link:
+          panda_link5:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          l_wrist_roll_link:
+          panda_link6:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          laser_tilt_mount_link:
+          panda_link7:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          r_elbow_flex_link:
+          panda_link8:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-            Value: true
-          r_forearm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_forearm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_l_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_l_finger_tip_link:
+          panda_rightfinger:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          r_gripper_motor_accelerometer_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_palm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_r_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_r_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_shoulder_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_shoulder_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_upper_arm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_upper_arm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_wrist_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          sensor_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          torso_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-        Robot Alpha: 0.5
-        Show Scene Robot: true
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
       Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
+    Default Light: true
     Fixed Frame: panda_link0
+    Frame Rate: 30
   Name: root
   Tools:
     - Class: rviz/Interact
       Hide Inactive Objects: true
     - Class: rviz/MoveCamera
     - Class: rviz/Select
+    - Class: rviz_visual_tools/KeyTool
   Value: true
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 2.9965
+      Distance: 2.82759404
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
       Focal Point:
-        X: 0.113567
-        Y: 0.10592
-        Z: 2.23518e-07
+        X: 0.113567002
+        Y: 0.105920002
+        Z: 2.23518001e-07
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.01
-      Pitch: 0.509797
-      Target Frame: /panda_link0
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.470203578
+      Target Frame: panda_link0
       Value: XYOrbit (rviz)
-      Yaw: 5.65995
+      Yaw: 6.06496
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1337
+  Height: 741
   Help:
     collapsed: false
   Hide Left Dock: false
   Hide Right Dock: false
-  Motion Planning:
+  QMainWindow State: 000000ff00000000fd00000001000000000000017800000282fc020000000efb000000100044006900730070006c006100790073010000004200000232000000dc00fffffffb0000000800480065006c00700000000342000000bb0000007500fffffffb0000000a00560069006500770073000000026d000000b5000000b500fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000294000002410000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069010000027a0000004a0000004800fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb000000120020002d00200053006c006900640065007201000002df000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004800ffffff000003d80000028200000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  RvizVisualToolsGui:
+    collapsed: false
+  Trajectory - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
-  Width: 1828
-  X: 459
-  Y: -243
+  Width: 1366
+  X: 1906
+  Y: 1440

From 5035646a418ee379b6597bb84da9f51816413f48 Mon Sep 17 00:00:00 2001
From: Tyler Weaver <tyler@picknik.ai>
Date: Mon, 5 Oct 2020 14:54:47 -0600
Subject: [PATCH 03/10] Sync allowed collisions in panda_arm.xacro with
 panda.srdf

---
 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 a551906..4eaf201 100644
--- a/config/panda_arm.xacro
+++ b/config/panda_arm.xacro
@@ -48,8 +48,8 @@
     <disable_collisions link1="panda_link4" link2="panda_link7" reason="Never" />
     <disable_collisions link1="panda_link4" link2="panda_link8" reason="Never" />
     <disable_collisions link1="panda_link5" link2="panda_link6" reason="Adjacent" />
+    <disable_collisions link1="panda_link5" link2="panda_link7" reason="Default" />
     <disable_collisions link1="panda_link6" link2="panda_link7" reason="Adjacent" />
-    <disable_collisions link1="panda_link6" link2="panda_link8" reason="Default" />
     <disable_collisions link1="panda_link7" link2="panda_link8" reason="Adjacent" />
   </xacro:macro>
 </robot>

From d98f4e26669e2cf594af6b426d2ff011e97f3789 Mon Sep 17 00:00:00 2001
From: JafarAbdi <cafer.abdi@gmail.com>
Date: Wed, 16 Dec 2020 19:10:28 +0300
Subject: [PATCH 04/10] Sync with MoveIt master MSA

---
 .setup_assistant                              |   2 +-
 config/cartesian_limits.yaml                  |   5 +
 config/panda.srdf                             | 114 +++++++++---------
 ...otion_planner_planning_pipeline.launch.xml |  28 +++++
 4 files changed, 91 insertions(+), 58 deletions(-)
 create mode 100644 config/cartesian_limits.yaml
 create mode 100644 launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml

diff --git a/.setup_assistant b/.setup_assistant
index a7e1293..c320979 100644
--- a/.setup_assistant
+++ b/.setup_assistant
@@ -8,4 +8,4 @@ moveit_setup_assistant_config:
   CONFIG:
     author_name: Tyler Weaver
     author_email: tyler@picknik.ai
-    generated_timestamp: 1601757998
\ No newline at end of file
+    generated_timestamp: 1608134967
\ No newline at end of file
diff --git a/config/cartesian_limits.yaml b/config/cartesian_limits.yaml
new file mode 100644
index 0000000..7df72f6
--- /dev/null
+++ b/config/cartesian_limits.yaml
@@ -0,0 +1,5 @@
+cartesian_limits:
+  max_trans_vel: 1
+  max_trans_acc: 2.25
+  max_trans_dec: -5
+  max_rot_vel: 1.57
diff --git a/config/panda.srdf b/config/panda.srdf
index e9f15b1..6e5a072 100644
--- a/config/panda.srdf
+++ b/config/panda.srdf
@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="UTF-8"?>
+<?xml version="1.0" ?>
 <!--This does not replace URDF, and is not an extension of URDF.
     This is a format for representing semantic information about the robot structure.
     A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
@@ -10,75 +10,75 @@
     <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
     <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
     <group name="panda_arm">
-        <joint name="virtual_joint"/>
-        <joint name="panda_joint1"/>
-        <joint name="panda_joint2"/>
-        <joint name="panda_joint3"/>
-        <joint name="panda_joint4"/>
-        <joint name="panda_joint5"/>
-        <joint name="panda_joint6"/>
-        <joint name="panda_joint7"/>
-        <joint name="panda_joint8"/>
+        <joint name="virtual_joint" />
+        <joint name="panda_joint1" />
+        <joint name="panda_joint2" />
+        <joint name="panda_joint3" />
+        <joint name="panda_joint4" />
+        <joint name="panda_joint5" />
+        <joint name="panda_joint6" />
+        <joint name="panda_joint7" />
+        <joint name="panda_joint8" />
     </group>
     <group name="hand">
-        <joint name="panda_hand_joint"/>
-        <joint name="panda_finger_joint1"/>
-        <joint name="panda_finger_joint2"/>
+        <joint name="panda_hand_joint" />
+        <joint name="panda_finger_joint1" />
+        <joint name="panda_finger_joint2" />
     </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="ready" group="panda_arm">
-        <joint name="panda_joint1" value="0"/>
-        <joint name="panda_joint2" value="-0.785"/>
-        <joint name="panda_joint3" value="0"/>
-        <joint name="panda_joint4" value="-2.356"/>
-        <joint name="panda_joint5" value="0"/>
-        <joint name="panda_joint6" value="1.571"/>
-        <joint name="panda_joint7" value="-0.7855"/>
+        <joint name="panda_joint1" value="0" />
+        <joint name="panda_joint2" value="-0.785" />
+        <joint name="panda_joint3" value="0" />
+        <joint name="panda_joint4" value="-2.356" />
+        <joint name="panda_joint5" value="0" />
+        <joint name="panda_joint6" value="1.571" />
+        <joint name="panda_joint7" value="-0.7855" />
     </group_state>
     <group_state name="extended" group="panda_arm">
-        <joint name="panda_joint1" value="0"/>
-        <joint name="panda_joint2" value="-0.3927"/>
-        <joint name="panda_joint3" value="0"/>
-        <joint name="panda_joint4" value="-0.7855"/>
-        <joint name="panda_joint5" value="0"/>
-        <joint name="panda_joint6" value="3.142"/>
-        <joint name="panda_joint7" value="-0.785"/>
+        <joint name="panda_joint1" value="0" />
+        <joint name="panda_joint2" value="-0.3927" />
+        <joint name="panda_joint3" value="0" />
+        <joint name="panda_joint4" value="-0.7855" />
+        <joint name="panda_joint5" value="0" />
+        <joint name="panda_joint6" value="3.142" />
+        <joint name="panda_joint7" value="-0.785" />
     </group_state>
     <group_state name="open" group="hand">
-        <joint name="panda_finger_joint1" value="0.035"/>
-        <joint name="panda_finger_joint2" value="0"/>
+        <joint name="panda_finger_joint1" value="0.035" />
+        <joint name="panda_finger_joint2" value="0" />
     </group_state>
     <group_state name="close" group="hand">
-        <joint name="panda_finger_joint1" value="0"/>
-        <joint name="panda_finger_joint2" value="0"/>
+        <joint name="panda_finger_joint1" value="0" />
+        <joint name="panda_finger_joint2" value="0" />
     </group_state>
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
-    <end_effector name="hand" parent_link="panda_link8" group="hand"/>
+    <end_effector name="hand" parent_link="panda_link8" group="hand" />
     <!--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="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_hand" link2="panda_link4" reason="Never"/>
-    <disable_collisions link1="panda_hand" link2="panda_link5" reason="Default"/>
-    <disable_collisions link1="panda_hand" link2="panda_link6" reason="Never"/>
-    <disable_collisions link1="panda_hand" link2="panda_link7" reason="Default"/>
-    <disable_collisions link1="panda_hand" link2="panda_link8" reason="Adjacent"/>
-    <disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent"/>
-    <disable_collisions link1="panda_link0" link2="panda_link2" reason="Never"/>
-    <disable_collisions link1="panda_link0" link2="panda_link3" reason="Never"/>
-    <disable_collisions link1="panda_link0" link2="panda_link4" reason="Never"/>
-    <disable_collisions link1="panda_link1" link2="panda_link2" reason="Adjacent"/>
-    <disable_collisions link1="panda_link1" link2="panda_link3" reason="Default"/>
-    <disable_collisions link1="panda_link1" link2="panda_link4" reason="Never"/>
-    <disable_collisions link1="panda_link2" link2="panda_link3" reason="Adjacent"/>
-    <disable_collisions link1="panda_link2" link2="panda_link4" reason="Never"/>
-    <disable_collisions link1="panda_link3" link2="panda_link4" reason="Adjacent"/>
-    <disable_collisions link1="panda_link3" link2="panda_link6" reason="Never"/>
-    <disable_collisions link1="panda_link4" link2="panda_link5" reason="Adjacent"/>
-    <disable_collisions link1="panda_link4" link2="panda_link6" reason="Never"/>
-    <disable_collisions link1="panda_link4" link2="panda_link7" reason="Never"/>
-    <disable_collisions link1="panda_link4" link2="panda_link8" reason="Never"/>
-    <disable_collisions link1="panda_link5" link2="panda_link6" reason="Adjacent"/>
-    <disable_collisions link1="panda_link5" link2="panda_link7" reason="Default"/>
-    <disable_collisions link1="panda_link6" link2="panda_link7" reason="Adjacent"/>
-    <disable_collisions link1="panda_link7" link2="panda_link8" reason="Adjacent"/>
+    <disable_collisions link1="panda_hand" link2="panda_link4" reason="Never" />
+    <disable_collisions link1="panda_hand" link2="panda_link5" reason="Default" />
+    <disable_collisions link1="panda_hand" link2="panda_link6" reason="Never" />
+    <disable_collisions link1="panda_hand" link2="panda_link7" reason="Default" />
+    <disable_collisions link1="panda_hand" link2="panda_link8" reason="Adjacent" />
+    <disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent" />
+    <disable_collisions link1="panda_link0" link2="panda_link2" reason="Never" />
+    <disable_collisions link1="panda_link0" link2="panda_link3" reason="Never" />
+    <disable_collisions link1="panda_link0" link2="panda_link4" reason="Never" />
+    <disable_collisions link1="panda_link1" link2="panda_link2" reason="Adjacent" />
+    <disable_collisions link1="panda_link1" link2="panda_link3" reason="Default" />
+    <disable_collisions link1="panda_link1" link2="panda_link4" reason="Never" />
+    <disable_collisions link1="panda_link2" link2="panda_link3" reason="Adjacent" />
+    <disable_collisions link1="panda_link2" link2="panda_link4" reason="Never" />
+    <disable_collisions link1="panda_link3" link2="panda_link4" reason="Adjacent" />
+    <disable_collisions link1="panda_link3" link2="panda_link6" reason="Never" />
+    <disable_collisions link1="panda_link4" link2="panda_link5" reason="Adjacent" />
+    <disable_collisions link1="panda_link4" link2="panda_link6" reason="Never" />
+    <disable_collisions link1="panda_link4" link2="panda_link7" reason="Never" />
+    <disable_collisions link1="panda_link4" link2="panda_link8" reason="Never" />
+    <disable_collisions link1="panda_link5" link2="panda_link6" reason="Adjacent" />
+    <disable_collisions link1="panda_link5" link2="panda_link7" reason="Default" />
+    <disable_collisions link1="panda_link6" link2="panda_link7" reason="Adjacent" />
+    <disable_collisions link1="panda_link7" link2="panda_link8" reason="Adjacent" />
 </robot>
diff --git a/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
new file mode 100644
index 0000000..ac6272e
--- /dev/null
+++ b/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
@@ -0,0 +1,28 @@
+<launch>
+
+  <!-- Pilz Command Planner Plugin for MoveIt -->
+  <arg name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner" />
+
+  <!-- The request adapters (plugins) used when planning.
+       ORDER MATTERS -->
+  <arg name="planning_adapters" value="" />
+
+  <!-- define capabilites that are loaded on start (space seperated) -->
+  <arg name="capabilities" default=""/>
+
+  <!-- inhibit capabilites (space seperated) -->
+  <arg name="disable_capabilities" default=""/>
+
+  <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)" />
+
+  <!-- MoveGroup capabilities to load, append sequence capability -->
+  <param name="capabilities" value="$(arg capabilities)
+                                    pilz_industrial_motion_planner/MoveGroupSequenceAction
+                                    pilz_industrial_motion_planner/MoveGroupSequenceService
+                                   " />
+  <param name="disable_capabilities" value="$(arg disable_capabilities)" />
+</launch>

From df0cbd824ba778f4de3c6cb1004c623de627d657 Mon Sep 17 00:00:00 2001
From: JafarAbdi <cafer.abdi@gmail.com>
Date: Fri, 18 Dec 2020 22:08:45 +0300
Subject: [PATCH 05/10] Disable collision between panda_link8 & panda_link6 +
 panda_link7

---
 config/panda.srdf      | 4 +++-
 config/panda_arm.xacro | 4 +++-
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/config/panda.srdf b/config/panda.srdf
index 6e5a072..9cd47c0 100644
--- a/config/panda.srdf
+++ b/config/panda.srdf
@@ -33,7 +33,7 @@
         <joint name="panda_joint4" value="-2.356" />
         <joint name="panda_joint5" value="0" />
         <joint name="panda_joint6" value="1.571" />
-        <joint name="panda_joint7" value="-0.7855" />
+        <joint name="panda_joint7" value="0.785" />
     </group_state>
     <group_state name="extended" group="panda_arm">
         <joint name="panda_joint1" value="0" />
@@ -79,6 +79,8 @@
     <disable_collisions link1="panda_link4" link2="panda_link8" reason="Never" />
     <disable_collisions link1="panda_link5" link2="panda_link6" reason="Adjacent" />
     <disable_collisions link1="panda_link5" link2="panda_link7" reason="Default" />
+    <disable_collisions link1="panda_link5" link2="panda_link8" reason="Default" />
     <disable_collisions link1="panda_link6" link2="panda_link7" reason="Adjacent" />
+    <disable_collisions link1="panda_link6" link2="panda_link8" reason="Default" />
     <disable_collisions link1="panda_link7" link2="panda_link8" reason="Adjacent" />
 </robot>
diff --git a/config/panda_arm.xacro b/config/panda_arm.xacro
index 4eaf201..62733dd 100644
--- a/config/panda_arm.xacro
+++ b/config/panda_arm.xacro
@@ -17,7 +17,7 @@
         <joint name="panda_joint4" value="-2.356"/>
         <joint name="panda_joint5" value="0"/>
         <joint name="panda_joint6" value="1.571"/>
-        <joint name="panda_joint7" value="-0.7855"/>
+        <joint name="panda_joint7" value="0.785"/>
     </group_state>
     <group_state name="extended" group="panda_arm">
         <joint name="panda_joint1" value="0"/>
@@ -49,7 +49,9 @@
     <disable_collisions link1="panda_link4" link2="panda_link8" reason="Never" />
     <disable_collisions link1="panda_link5" link2="panda_link6" reason="Adjacent" />
     <disable_collisions link1="panda_link5" link2="panda_link7" reason="Default" />
+    <disable_collisions link1="panda_link5" link2="panda_link8" reason="Default" />
     <disable_collisions link1="panda_link6" link2="panda_link7" reason="Adjacent" />
+    <disable_collisions link1="panda_link6" link2="panda_link8" reason="Default" />
     <disable_collisions link1="panda_link7" link2="panda_link8" reason="Adjacent" />
   </xacro:macro>
 </robot>

From 2cd5c45dc306ac91666205d9c0cba49809e6a473 Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Wed, 26 May 2021 16:31:45 +0200
Subject: [PATCH 06/10] Adds panda_control_moveit_rviz.launch fixes

---
 launch/move_group.launch                | 25 +++++++------------------
 launch/moveit_rviz.launch               |  7 +++----
 launch/panda_control_moveit_rviz.launch |  6 ++++--
 launch/planning_context.launch          |  8 +++++---
 4 files changed, 19 insertions(+), 27 deletions(-)

diff --git a/launch/move_group.launch b/launch/move_group.launch
index 488a842..44c126e 100644
--- a/launch/move_group.launch
+++ b/launch/move_group.launch
@@ -4,7 +4,7 @@
   <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" />
+    value="gdb -x $(find panda_moveit_config)/launch/gdb_settings.gdb --ex run --args" />
 
   <!-- Verbose Mode Option -->
   <arg name="info" default="$(arg debug)" />
@@ -19,28 +19,16 @@
   <arg name="jiggle_fraction" default="0.05" />
   <arg name="publish_monitored_planning_scene" default="true"/>
 
+  <!-- Other settings -->
+  <arg name="load_robot_description" default="true" />
+  <arg name="load_gripper" default="true" />
   <arg name="capabilities" default=""/>
   <arg name="disable_capabilities" default=""/>
-  <!-- load these non-default MoveGroup capabilities (space seperated) -->
-  <!--
-  <arg name="capabilities" value="
-                a_package/AwsomeMotionPlanningCapability
-                another_package/GraspPlanningPipeline
-                " />
-  -->
-
-  <!-- inhibit these default MoveGroup capabilities (space seperated) -->
-  <!--
-  <arg name="disable_capabilities" value="
-                move_group/MoveGroupKinematicsService
-                move_group/ClearOctomapService
-                " />
-  -->
 
-  <arg name="load_robot_description" default="true" />
   <!-- load URDF, SRDF and joint_limits configuration -->
   <include file="$(find panda_moveit_config)/launch/planning_context.launch">
     <arg name="load_robot_description" value="$(arg load_robot_description)" />
+    <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
 
   <!-- Planning Functionality -->
@@ -51,7 +39,8 @@
   <!-- 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" unless="$(arg fake_execution)"/>
+    <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>
 
diff --git a/launch/moveit_rviz.launch b/launch/moveit_rviz.launch
index 615b2f1..da9fe65 100644
--- a/launch/moveit_rviz.launch
+++ b/launch/moveit_rviz.launch
@@ -2,14 +2,13 @@
 
   <arg name="debug" default="false" />
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
-  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
+  <arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
 
   <arg name="rviz_config" default="" />
-  <arg     if="$(eval rviz_config=='')" name="command_args" value="" />
+  <arg if="$(eval rviz_config=='')" name="command_args" value="" />
   <arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)" />
 
-  <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
-	args="$(arg command_args)" output="screen">
+  <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false" args="$(arg command_args)" output="screen">
   </node>
 
 </launch>
diff --git a/launch/panda_control_moveit_rviz.launch b/launch/panda_control_moveit_rviz.launch
index ddfdcc9..bc2e281 100644
--- a/launch/panda_control_moveit_rviz.launch
+++ b/launch/panda_control_moveit_rviz.launch
@@ -7,7 +7,7 @@
   <arg name="load_gripper" default="true" />
   <arg name="launch_rviz" default="true" />
 
-  <include file="$(find franka_control)/launch/franka_control.launch" >
+  <include file="$(find franka_control)/launch/franka_control.launch">
     <arg name="robot_ip" value="$(arg robot_ip)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
@@ -16,5 +16,7 @@
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
 
-  <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" if="$(arg launch_rviz)" />
+  <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" if="$(arg launch_rviz)">
+    <arg name="rviz_config" value="$(find panda_moveit_config)/launch/moveit.rviz"/>
+  </include>
 </launch>
diff --git a/launch/planning_context.launch b/launch/planning_context.launch
index 8cadd11..b84786b 100644
--- a/launch/planning_context.launch
+++ b/launch/planning_context.launch
@@ -1,15 +1,18 @@
 <launch>
   <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
+  <arg name="load_gripper" default="true" />
   <arg name="load_robot_description" default="false"/>
 
   <!-- The name of the parameter under which the URDF is loaded -->
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="xacro  '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'"/>
+  <param if="$(eval arg('load_robot_description') and arg('load_gripper'))" name="$(arg robot_description)" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'"/>
+  <param if="$(eval arg('load_robot_description') and not arg('load_gripper'))" name="$(arg robot_description)" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm.urdf.xacro'"/>
 
   <!-- The semantic description that corresponds to the URDF -->
-  <param name="$(arg robot_description)_semantic" textfile="$(find panda_moveit_config)/config/panda.srdf" />
+  <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find panda_moveit_config)/config/panda_arm_hand.srdf.xacro'" if="$(arg load_gripper)" />
+  <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find panda_moveit_config)/config/panda_arm.srdf.xacro'" unless="$(arg load_gripper)" />
 
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">
@@ -19,7 +22,6 @@
   <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
   <group ns="$(arg robot_description)_kinematics">
     <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
-
   </group>
 
 </launch>

From 6b8cbe768ef66baf259ce9449f6874b1a8f10c0f Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Fri, 6 Aug 2021 15:21:46 +0200
Subject: [PATCH 07/10] Updates robot_description xacro url because of upstream
 changes (#3)

This commit makes sure the robot_description is loaded from the right
url. This was done since
https://github.com/frankaemika/franka_ros/commit/3438d64f3499a22d297465f267854d9037e88996
in the franka_ros package combines the hand and arm xacro files.
---
 launch/demo_gazebo.launch      | 8 ++++----
 launch/gazebo.launch           | 5 ++---
 launch/planning_context.launch | 3 +--
 3 files changed, 7 insertions(+), 9 deletions(-)

diff --git a/launch/demo_gazebo.launch b/launch/demo_gazebo.launch
index 9ce0fd4..8904c5a 100644
--- a/launch/demo_gazebo.launch
+++ b/launch/demo_gazebo.launch
@@ -25,17 +25,17 @@
   <arg name="gazebo_gui" default="true"/>
   <arg name="paused" default="false"/>
   <!-- By default, use the urdf location provided from the package -->
-  <arg name="urdf_path" default="$(find franka_description)/robots/panda_arm_hand.urdf.xacro"/>
+  <arg name="urdf_path" default="$(find franka_description)/robots/panda_arm.urdf.xacro"/>
 
   <!-- launch the gazebo simulator and spawn the robot -->
-  <include file="$(find panda_moveit_config)/launch/gazebo.launch" >
+  <include file="$(find panda_moveit_config)/launch/gazebo.launch">
     <arg name="paused" value="$(arg paused)"/>
     <arg name="gazebo_gui" value="$(arg gazebo_gui)"/>
     <arg name="urdf_path" value="$(arg urdf_path)"/>
   </include>
 
   <!-- If needed, broadcast static tf for robot root -->
-  
+
 
   <!-- We do not have a robot connected, so publish fake joint states -->
   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
@@ -57,7 +57,7 @@
     <arg name="info" value="true"/>
     <arg name="debug" value="$(arg debug)"/>
     <arg name="load_robot_description" value="$(arg load_robot_description)"/>
-  </include>  
+  </include>
 
   <!-- Run Rviz and load the default config to see the state of the move_group node -->
   <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch">
diff --git a/launch/gazebo.launch b/launch/gazebo.launch
index df41640..71d37ef 100644
--- a/launch/gazebo.launch
+++ b/launch/gazebo.launch
@@ -2,7 +2,7 @@
 <launch>
   <arg name="paused" default="false"/>
   <arg name="gazebo_gui" default="true"/>
-  <arg name="urdf_path" default="$(find franka_description)/robots/panda_arm_hand.urdf.xacro"/>
+  <arg name="urdf_path" default="$(find franka_description)/robots/panda_arm.urdf.xacro"/>
 
   <!-- startup simulated world -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
@@ -15,8 +15,7 @@
   <param name="robot_description" textfile="$(arg urdf_path)" />
 
   <!-- push robot_description to factory and spawn robot in gazebo at the origin, change x,y,z arguments to spawn in a different position -->
-  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 0"
-    respawn="false" output="screen" />
+  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 0" respawn="false" output="screen" />
 
   <include file="$(find panda_moveit_config)/launch/ros_controllers.launch"/>
 
diff --git a/launch/planning_context.launch b/launch/planning_context.launch
index b84786b..05f9c6e 100644
--- a/launch/planning_context.launch
+++ b/launch/planning_context.launch
@@ -7,8 +7,7 @@
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param if="$(eval arg('load_robot_description') and arg('load_gripper'))" name="$(arg robot_description)" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'"/>
-  <param if="$(eval arg('load_robot_description') and not arg('load_gripper'))" name="$(arg robot_description)" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm.urdf.xacro'"/>
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm.urdf.xacro hand:=$(arg load_gripper)"/>
 
   <!-- The semantic description that corresponds to the URDF -->
   <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find panda_moveit_config)/config/panda_arm_hand.srdf.xacro'" if="$(arg load_gripper)" />

From ecda09bf65948092c1f2402ad829ce1995c3bc29 Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Fri, 6 Aug 2021 22:22:51 +0200
Subject: [PATCH 08/10] Adds missing simple_control_manager dependency (#4)

This commit makes suer that the 'moveit_simple_control_manager' is added
as a dependency. This dependency is needed for running the move_group
script with a real robot.
---
 package.xml | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/package.xml b/package.xml
index 6120061..5e11395 100644
--- a/package.xml
+++ b/package.xml
@@ -17,6 +17,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <run_depend>moveit_ros_move_group</run_depend>
+  <run_depend>moveit_simple_controller_manager</run_depend>
   <run_depend>moveit_fake_controller_manager</run_depend>
   <run_depend>moveit_kinematics</run_depend>
   <run_depend>moveit_planners_ompl</run_depend>
@@ -29,7 +30,7 @@
   <run_depend>xacro</run_depend>
   <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
   <!-- <run_depend>warehouse_ros_mongo</run_depend> -->
-    <run_depend>franka_description</run_depend>
+  <run_depend>franka_description</run_depend>
 
 
 </package>

From 764113c01b699111ac072a930f736b05710a2b1f Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Fri, 6 Aug 2021 22:22:59 +0200
Subject: [PATCH 09/10] P/update latest fix melodic noetic diffs (#6)

* Adds rviz_tutorial argument

* Fixes virtual frame world not found warning
---
 .gitignore                |   3 +
 launch/demo.launch        |   4 +-
 launch/demo_gazebo.launch |   3 -
 launch/moveit_rviz.launch |   8 +-
 launch/test.rviz          | 262 ++++++++++++++++++++++++++++++++++++++
 5 files changed, 273 insertions(+), 7 deletions(-)
 create mode 100644 .gitignore
 create mode 100644 launch/test.rviz

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..fa83498
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,3 @@
+# IDE
+.vscode
+*.code-workspace
diff --git a/launch/demo.launch b/launch/demo.launch
index 9e48ded..2417e62 100644
--- a/launch/demo.launch
+++ b/launch/demo.launch
@@ -22,11 +22,12 @@
   It also provides a GUI to move the simulated robot around "manually".
   This corresponds to moving around the real robot without the use of MoveIt.
   -->
+  <arg name="rviz_tutorial" default="false" />
   <arg name="use_gui" default="false" />
   <arg name="use_rviz" default="true" />
 
   <!-- If needed, broadcast static tf for robot root -->
-  
+  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world panda_link0" />
 
   <!-- We do not have a robot connected, so publish fake joint states -->
   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
@@ -51,6 +52,7 @@
 
   <!-- Run Rviz and load the default config to see the state of the move_group node -->
   <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" if="$(arg use_rviz)">
+    <arg name="rviz_tutorial" value="$(arg rviz_tutorial)"/>
     <arg name="rviz_config" value="$(find panda_moveit_config)/launch/moveit.rviz"/>
     <arg name="debug" value="$(arg debug)"/>
   </include>
diff --git a/launch/demo_gazebo.launch b/launch/demo_gazebo.launch
index 8904c5a..9ae0830 100644
--- a/launch/demo_gazebo.launch
+++ b/launch/demo_gazebo.launch
@@ -34,9 +34,6 @@
     <arg name="urdf_path" value="$(arg urdf_path)"/>
   </include>
 
-  <!-- If needed, broadcast static tf for robot root -->
-
-
   <!-- We do not have a robot connected, so publish fake joint states -->
   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
     <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
diff --git a/launch/moveit_rviz.launch b/launch/moveit_rviz.launch
index da9fe65..bc71677 100644
--- a/launch/moveit_rviz.launch
+++ b/launch/moveit_rviz.launch
@@ -4,9 +4,11 @@
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
   <arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
 
-  <arg name="rviz_config" default="" />
-  <arg if="$(eval rviz_config=='')" name="command_args" value="" />
-  <arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)" />
+  <!--Retrieve the right rviz config file-->
+  <arg name="rviz_tutorial" default="false" />
+  <arg name="rviz_config" default="$(find panda_moveit_config)/launch/moveit.rviz" />
+  <arg unless="$(arg rviz_tutorial)" name="command_args" value="-d $(arg rviz_config)"/>
+  <arg     if="$(arg rviz_tutorial)" name="command_args" value="-d $(find panda_moveit_config)/launch/moveit_empty.rviz"/>
 
   <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false" args="$(arg command_args)" output="screen">
   </node>
diff --git a/launch/test.rviz b/launch/test.rviz
new file mode 100644
index 0000000..e298d66
--- /dev/null
+++ b/launch/test.rviz
@@ -0,0 +1,262 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 84
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /MarkerArray1
+        - /PlanningScene1/Scene Robot1
+      Splitter Ratio: 0.7425600290298462
+    Tree Height: 649
+  - Class: rviz/Help
+    Name: Help
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_visual_tools/RvizVisualToolsGui
+    Name: RvizVisualToolsGui
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /rviz_visual_tools
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: moveit_rviz_plugin/Trajectory
+      Color Enabled: false
+      Enabled: true
+      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_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
+          Value: true
+      Loop Animation: false
+      Name: Trajectory
+      Robot Alpha: 0.5
+      Robot Color: 150; 50; 150
+      Robot Description: robot_description
+      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
+      Value: true
+    - Class: moveit_rviz_plugin/PlanningScene
+      Enabled: true
+      Move Group Namespace: ""
+      Name: PlanningScene
+      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_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
+            Value: true
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: panda_link0
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz_visual_tools/KeyTool
+  Value: true
+  Views:
+    Current:
+      Class: rviz/XYOrbit
+      Distance: 2.827594041824341
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.11356700211763382
+        Y: 0.10592000186443329
+        Z: 2.2351800055275817e-07
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.47020357847213745
+      Target Frame: panda_link0
+      Yaw: 6.06496000289917
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 995
+  Help:
+    collapsed: false
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000001000000000000017800000389fc020000000efb000000100044006900730070006c006100790073010000003d0000031a000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000026d000000b5000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000294000002410000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069010000035d000000690000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb000000120020002d00200053006c006900640065007201000002df000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016000006020000038900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  RvizVisualToolsGui:
+    collapsed: false
+  Trajectory - Trajectory Slider:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 1920
+  Y: 456

From 42ca305b17a125d1c78ac1ba2708f445886aae33 Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Wed, 11 Aug 2021 23:06:59 +0200
Subject: [PATCH 10/10] Adds 'moveit_visual_tools' as a dependency. (#7)

---
 package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/package.xml b/package.xml
index 5e11395..990d798 100644
--- a/package.xml
+++ b/package.xml
@@ -22,6 +22,7 @@
   <run_depend>moveit_kinematics</run_depend>
   <run_depend>moveit_planners_ompl</run_depend>
   <run_depend>moveit_ros_visualization</run_depend>
+  <run_depend>moveit_visual_tools</run_depend>
   <run_depend>moveit_setup_assistant</run_depend>
   <run_depend>joint_state_publisher</run_depend>
   <run_depend>joint_state_publisher_gui</run_depend>