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] 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>