From 761935563acbf28ac9213312221ab253c725e5cb Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Wed, 20 Oct 2021 21:46:45 +0200
Subject: [PATCH 1/3] Add demo_gazebo.launch using franka_gazebo

This commit adds demo_gazebo.launch using the gazebo simulation of the franka_gazebo package
Additionally, the virtual_joint is removed from the move group definition.
---
 config/panda_arm.xacro        | 125 +++++++++++++++++-----------------
 config/ros_controllers.yaml   |  20 ++++++
 launch/demo_gazebo.launch     |  52 ++++++++++++++
 launch/ros_controllers.launch |  10 +++
 package.xml                   |  10 ++-
 5 files changed, 153 insertions(+), 64 deletions(-)
 create mode 100644 config/ros_controllers.yaml
 create mode 100644 launch/demo_gazebo.launch
 create mode 100644 launch/ros_controllers.launch

diff --git a/config/panda_arm.xacro b/config/panda_arm.xacro
index 4a4e9f2..1ab41c5 100644
--- a/config/panda_arm.xacro
+++ b/config/panda_arm.xacro
@@ -1,64 +1,67 @@
 <?xml version="1.0" ?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
-  <xacro:macro name="panda_arm">
-    <!--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">
-      <chain base_link="panda_link0" tip_link="panda_link8" />
-    </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" />
-    </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" />
-    </group_state>
-    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
-    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
-    <virtual_joint name="virtual_joint" type="floating" parent_frame="world" child_link="panda_link0" />
-    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
-    <disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent" />
-    <disable_collisions link1="panda_link0" link2="panda_link2" reason="Never" />
-    <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_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>
+    <xacro:macro name="panda_arm">
+        <!--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="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 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>
+        <!--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_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"/>
+        <!-- Fix self-collisions between too coarse collision geometries provided by franka_description
+             See https://github.com/ros-planning/panda_moveit_config/issues/72#issuecomment-768472329
+                 https://github.com/ros-planning/panda_moveit_config/pull/35#pullrequestreview-390715967
+        -->
+        <disable_collisions link1="panda_link5" link2="panda_link8" reason="Default" />
+        <disable_collisions link1="panda_link6" link2="panda_link8" reason="Default"/>
+    </xacro:macro>
 </robot>
diff --git a/config/ros_controllers.yaml b/config/ros_controllers.yaml
new file mode 100644
index 0000000..a4e9561
--- /dev/null
+++ b/config/ros_controllers.yaml
@@ -0,0 +1,20 @@
+# 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
diff --git a/launch/demo_gazebo.launch b/launch/demo_gazebo.launch
new file mode 100644
index 0000000..d3096c7
--- /dev/null
+++ b/launch/demo_gazebo.launch
@@ -0,0 +1,52 @@
+<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"/>
+  <arg name="load_gripper" default="true" />
+
+  <!-- Gazebo specific options -->
+  <arg name="gazebo_gui" default="true"/>
+  <arg name="paused" default="false"/>
+
+  <!-- launch the gazebo simulator and spawn the robot -->
+  <include file="$(find franka_gazebo)/launch/panda.launch">
+    <arg unless="$(arg gazebo_gui)" name="headless" value="true"/>
+    <arg     if="$(arg gazebo_gui)" name="headless" value="false"/>
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="use_gripper" default="$(arg load_gripper)"/>
+    <arg name="transmission" value="hardware_interface/PositionJointInterface"/>
+  </include>
+
+  <!-- Load position_joint_trajectory_controller -->
+  <include ns="panda" file="$(find panda_moveit_config)/launch/ros_controllers.launch"/>
+
+  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
+  <include ns="panda" file="$(find panda_moveit_config)/launch/move_group.launch">
+    <arg name="info" value="true"/>
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="allow_trajectory_execution" value="true"/>
+    <arg name="fake_execution" value="false"/>
+    <arg name="load_gripper" value="$(arg load_gripper)"/>
+    <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 ns="panda" file="$(dirname)/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="$(dirname)/default_warehouse_db.launch" if="$(arg db)">
+    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
+  </include>
+
+</launch>
diff --git a/launch/ros_controllers.launch b/launch/ros_controllers.launch
new file mode 100644
index 0000000..bb223d3
--- /dev/null
+++ b/launch/ros_controllers.launch
@@ -0,0 +1,10 @@
+<?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="position_joint_trajectory_controller"/>
+
+</launch>
diff --git a/package.xml b/package.xml
index f7b834d..9a71ac3 100644
--- a/package.xml
+++ b/package.xml
@@ -25,8 +25,12 @@
   <run_depend>joint_state_publisher_gui</run_depend>
   <run_depend>robot_state_publisher</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. -->
+  <run_depend>moveit_visual_tools</run_depend>
+  <!-- The next 2 packages are required for the gazebo simulation.
+      We don't include them by default to prevent installing gazebo and all its dependencies. -->
+  <!-- <run_depend>joint_trajectory_controller</run_depend> -->
+  <!-- <run_depend>gazebo_ros_control</run_depend> -->
+  <!-- <run_depend>franka_gazebo</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> -->
 </package>

From 458e66ffec65b10acc90ae844cc87c177f82f776 Mon Sep 17 00:00:00 2001
From: tahsinkose <tahsincan.kose@aeolusbot.com>
Date: Thu, 21 Oct 2021 17:21:37 +0300
Subject: [PATCH 2/3] Fix parameters.

---
 launch/demo_gazebo.launch | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/launch/demo_gazebo.launch b/launch/demo_gazebo.launch
index d3096c7..a59b1a3 100644
--- a/launch/demo_gazebo.launch
+++ b/launch/demo_gazebo.launch
@@ -23,10 +23,11 @@
     <arg name="paused" value="$(arg paused)"/>
     <arg name="use_gripper" default="$(arg load_gripper)"/>
     <arg name="transmission" value="hardware_interface/PositionJointInterface"/>
+    <arg name="controller" value="position_joint_trajectory_controller"/>
   </include>
 
   <!-- Load position_joint_trajectory_controller -->
-  <include ns="panda" file="$(find panda_moveit_config)/launch/ros_controllers.launch"/>
+  <include file="$(find panda_moveit_config)/launch/ros_controllers.launch"/>
 
   <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
   <include ns="panda" file="$(find panda_moveit_config)/launch/move_group.launch">
@@ -35,12 +36,10 @@
     <arg name="allow_trajectory_execution" value="true"/>
     <arg name="fake_execution" value="false"/>
     <arg name="load_gripper" value="$(arg load_gripper)"/>
-    <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 ns="panda" file="$(dirname)/moveit_rviz.launch">
-    <arg name="rviz_config" value="$(find panda_moveit_config)/launch/moveit.rviz"/>
     <arg name="debug" value="$(arg debug)"/>
   </include>
 

From aebdd0afaf9c8fcb623802225b0c989caac206d0 Mon Sep 17 00:00:00 2001
From: tahsinkose <tahsincan.kose@aeolusbot.com>
Date: Thu, 21 Oct 2021 17:22:01 +0300
Subject: [PATCH 3/3] Do not load robot description if it is not explicitly
 asked.

---
 launch/planning_context.launch | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/launch/planning_context.launch b/launch/planning_context.launch
index 3485d67..ba3b7cb 100644
--- a/launch/planning_context.launch
+++ b/launch/planning_context.launch
@@ -8,7 +8,7 @@
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param name="$(arg robot_description)" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm.urdf.xacro hand:=$(arg load_gripper)" />
+  <param name="$(arg robot_description)" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm.urdf.xacro hand:=$(arg load_gripper)" if="$(arg load_robot_description)"/>
 
   <!-- 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)" />