From 9636ecf6974e11a73e843bdde273d4c58412f2ec Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Fri, 19 Jan 2018 11:34:56 +0900
Subject: [PATCH] Place ompl_planning.yaml to each robot

---
 .../include/moveit/common/move_group.launch   |  4 +-
 .../common/ompl_planning_pipeline.launch      |  4 +-
 .../moveit_config/baxter/baxter.srdf.xacro    |  4 +-
 .../{common => baxter}/ompl_planning.yaml     |  0
 .../baxterlgv7/baxterlgv7.srdf.xacro          |  4 +-
 .../baxterlgv7/ompl_planning.yaml             | 80 +++++++++++++++++++
 .../gripper_v6.srdf.xacro                     |  0
 .../gripper_v7.srdf.xacro                     |  0
 8 files changed, 90 insertions(+), 6 deletions(-)
 rename jsk_arc2017_baxter/robots/moveit_config/{common => baxter}/ompl_planning.yaml (100%)
 create mode 100644 jsk_arc2017_baxter/robots/moveit_config/baxterlgv7/ompl_planning.yaml
 rename jsk_arc2017_baxter/robots/moveit_config/{common => grippers}/gripper_v6.srdf.xacro (100%)
 rename jsk_arc2017_baxter/robots/moveit_config/{common => grippers}/gripper_v7.srdf.xacro (100%)

diff --git a/jsk_arc2017_baxter/launch/setup/include/moveit/common/move_group.launch b/jsk_arc2017_baxter/launch/setup/include/moveit/common/move_group.launch
index a44fdf105..ff0191893 100644
--- a/jsk_arc2017_baxter/launch/setup/include/moveit/common/move_group.launch
+++ b/jsk_arc2017_baxter/launch/setup/include/moveit/common/move_group.launch
@@ -20,7 +20,9 @@
   <arg name="publish_monitored_planning_scene" default="true"/>
 
   <!-- Planning Functionality -->
-  <include ns="move_group" file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/common/ompl_planning_pipeline.launch" />
+  <include ns="move_group" file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/common/ompl_planning_pipeline.launch">
+    <arg name="robot" value="$(arg robot)"/>
+  </include>
 
   <!-- Trajectory Execution Functionality -->
   <include ns="move_group" file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/$(arg robot)/$(arg robot)_trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
diff --git a/jsk_arc2017_baxter/launch/setup/include/moveit/common/ompl_planning_pipeline.launch b/jsk_arc2017_baxter/launch/setup/include/moveit/common/ompl_planning_pipeline.launch
index 2599f13be..fc66255f2 100644
--- a/jsk_arc2017_baxter/launch/setup/include/moveit/common/ompl_planning_pipeline.launch
+++ b/jsk_arc2017_baxter/launch/setup/include/moveit/common/ompl_planning_pipeline.launch
@@ -1,5 +1,7 @@
 <launch>
 
+  <arg name="robot" default="baxter"/>
+
   <!-- OMPL Plugin for MoveIt! -->
   <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />
 
@@ -18,6 +20,6 @@
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
 
   <rosparam command="load" file="$(find baxter_moveit_config)/config/kinematics.yaml"/>
-  <rosparam command="load" file="$(find jsk_arc2017_baxter)/robots/moveit_config/common/ompl_planning.yaml"/>
+  <rosparam command="load" file="$(find jsk_arc2017_baxter)/robots/moveit_config/$(arg robot)/ompl_planning.yaml"/>
 
 </launch>
diff --git a/jsk_arc2017_baxter/robots/moveit_config/baxter/baxter.srdf.xacro b/jsk_arc2017_baxter/robots/moveit_config/baxter/baxter.srdf.xacro
index ddc5a4f0c..883882ed7 100644
--- a/jsk_arc2017_baxter/robots/moveit_config/baxter/baxter.srdf.xacro
+++ b/jsk_arc2017_baxter/robots/moveit_config/baxter/baxter.srdf.xacro
@@ -5,9 +5,9 @@
   <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/baxter/baxter_base.srdf.xacro" />
   <xacro:baxter_base left_tip_name="$(arg left_tip_name)" right_tip_name="$(arg right_tip_name)"/>
   <!--Left End Effector Collisions-->
-  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/common/gripper_v6.srdf.xacro" />
+  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/grippers/gripper_v6.srdf.xacro" />
   <xacro:vacuum_gripper side="left"/>
   <!--Right End Effector Collisions-->
-  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/common/gripper_v6.srdf.xacro" />
+  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/grippers/gripper_v6.srdf.xacro" />
   <xacro:vacuum_gripper side="right"/>
 </robot>
diff --git a/jsk_arc2017_baxter/robots/moveit_config/common/ompl_planning.yaml b/jsk_arc2017_baxter/robots/moveit_config/baxter/ompl_planning.yaml
similarity index 100%
rename from jsk_arc2017_baxter/robots/moveit_config/common/ompl_planning.yaml
rename to jsk_arc2017_baxter/robots/moveit_config/baxter/ompl_planning.yaml
diff --git a/jsk_arc2017_baxter/robots/moveit_config/baxterlgv7/baxterlgv7.srdf.xacro b/jsk_arc2017_baxter/robots/moveit_config/baxterlgv7/baxterlgv7.srdf.xacro
index 5bad49fa9..8cc1d568b 100644
--- a/jsk_arc2017_baxter/robots/moveit_config/baxterlgv7/baxterlgv7.srdf.xacro
+++ b/jsk_arc2017_baxter/robots/moveit_config/baxterlgv7/baxterlgv7.srdf.xacro
@@ -5,9 +5,9 @@
   <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/baxterlgv7/baxterlgv7_base.srdf.xacro" />
   <xacro:baxterlgv7_base left_tip_name="$(arg left_tip_name)" right_tip_name="$(arg right_tip_name)"/>
   <!--Left End Effector Collisions-->
-  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/common/gripper_v7.srdf.xacro" />
+  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/grippers/gripper_v7.srdf.xacro" />
   <xacro:vacuum_gripper side="left"/>
   <!--Right End Effector Collisions-->
-  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/common/gripper_v6.srdf.xacro" />
+  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/grippers/gripper_v6.srdf.xacro" />
   <xacro:vacuum_gripper side="right"/>
 </robot>
diff --git a/jsk_arc2017_baxter/robots/moveit_config/baxterlgv7/ompl_planning.yaml b/jsk_arc2017_baxter/robots/moveit_config/baxterlgv7/ompl_planning.yaml
new file mode 100644
index 000000000..ee73102fc
--- /dev/null
+++ b/jsk_arc2017_baxter/robots/moveit_config/baxterlgv7/ompl_planning.yaml
@@ -0,0 +1,80 @@
+planner_configs:
+  SBLkConfigDefault:
+    type: geometric::SBL
+  LBKPIECEkConfigDefault:
+    type: geometric::LBKPIECE
+  RRTkConfigDefault:
+    type: geometric::RRT
+  RRTConnectkConfigDefault:
+    type: geometric::RRTConnect
+  LazyRRTkConfigDefault:
+    type: geometric::LazyRRT
+  ESTkConfigDefault:
+    type: geometric::EST
+  KPIECEkConfigDefault:
+    type: geometric::KPIECE
+  RRTStarkConfigDefault:
+    type: geometric::RRTstar
+  BKPIECEkConfigDefault:
+    type: geometric::BKPIECE
+left_arm:
+  default_planner_config: RRTConnectkConfigDefault
+  planner_configs:
+    - SBLkConfigDefault
+    - LBKPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - ESTkConfigDefault
+    - KPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - RRTStarkConfigDefault
+  projection_evaluator: joints(left_s0,left_s1)
+  longest_valid_segment_fraction: 0.01
+right_arm:
+  default_planner_config: RRTConnectkConfigDefault
+  planner_configs:
+    - SBLkConfigDefault
+    - LBKPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - ESTkConfigDefault
+    - KPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - RRTStarkConfigDefault
+  projection_evaluator: joints(right_s0,right_s1)
+  longest_valid_segment_fraction: 0.01
+both_arms:
+  default_planner_config: RRTConnectkConfigDefault
+  planner_configs:
+    - SBLkConfigDefault
+    - LBKPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - ESTkConfigDefault
+    - KPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - RRTStarkConfigDefault
+  projection_evaluator: joints(right_s0,right_s1)
+  longest_valid_segment_fraction: 0.01
+left_hand:
+  default_planner_config: RRTConnectkConfigDefault
+  planner_configs:
+    - SBLkConfigDefault
+    - LBKPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - ESTkConfigDefault
+    - KPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - RRTStarkConfigDefault
+right_hand:
+  default_planner_config: RRTConnectkConfigDefault
+  planner_configs:
+    - SBLkConfigDefault
+    - LBKPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - ESTkConfigDefault
+    - KPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - RRTStarkConfigDefault
diff --git a/jsk_arc2017_baxter/robots/moveit_config/common/gripper_v6.srdf.xacro b/jsk_arc2017_baxter/robots/moveit_config/grippers/gripper_v6.srdf.xacro
similarity index 100%
rename from jsk_arc2017_baxter/robots/moveit_config/common/gripper_v6.srdf.xacro
rename to jsk_arc2017_baxter/robots/moveit_config/grippers/gripper_v6.srdf.xacro
diff --git a/jsk_arc2017_baxter/robots/moveit_config/common/gripper_v7.srdf.xacro b/jsk_arc2017_baxter/robots/moveit_config/grippers/gripper_v7.srdf.xacro
similarity index 100%
rename from jsk_arc2017_baxter/robots/moveit_config/common/gripper_v7.srdf.xacro
rename to jsk_arc2017_baxter/robots/moveit_config/grippers/gripper_v7.srdf.xacro