Migrates to the 'franka_gazebo' simulation (#21)

This commit adjusts the 'demo_gazebo.launch' launch file such that it uses the gazebo simulation of the [franka_gazebo](https://github.com/frankaemika/franka_ros/tree/noetic-devel/franka_gazebo) package.
rickstaa · Aug 25, 2021 · 482f744 · 482f744
1 parent 16ea1d8
commit 482f744
Show file tree

Hide file tree

Showing 10 changed files with 67 additions and 105 deletions.
diff --git a/config/panda_arm.xacro b/config/panda_arm.xacro
@@ -7,7 +7,6 @@
         <!--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"/>

diff --git a/config/panda_gripper_moveit_controllers.yaml b/config/panda_gripper_moveit_controllers.yaml
@@ -0,0 +1,20 @@
+controller_list:
+  - name: effort_joint_trajectory_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
+      - panda_joint1
+      - panda_joint2
+      - panda_joint3
+      - panda_joint4
+      - panda_joint5
+      - panda_joint6
+      - panda_joint7
+  - name: franka_gripper
+    action_ns: gripper_action
+    type: GripperCommand
+    default: true
+    joints:
+      - panda_finger_joint1
+      - panda_finger_joint2
diff --git a/config/panda_moveit_controllers.yaml b/config/panda_moveit_controllers.yaml
@@ -0,0 +1,13 @@
+controller_list:
+  - name: effort_joint_trajectory_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
+      - panda_joint1
+      - panda_joint2
+      - panda_joint3
+      - panda_joint4
+      - panda_joint5
+      - panda_joint6
+      - panda_joint7
diff --git a/config/ros_controllers.yaml b/config/ros_controllers.yaml
@@ -17,16 +17,10 @@ hardware_interface:
     - 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:
-  []
-position_joint_trajectory_controller:
-  type: position_controllers/JointTrajectoryController
+  sim_control_mode: 1 # 0: position, 1: velocity
+# Configure MoveIt demo controller
+effort_joint_trajectory_controller:
+  type: effort_controllers/JointTrajectoryController
   joints:
     - panda_joint1
     - panda_joint2
@@ -36,38 +30,10 @@ position_joint_trajectory_controller:
     - 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
+    panda_joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }
+    panda_joint2: { p: 30000, d: 100, i: 0.02, i_clamp: 10000 }
+    panda_joint3: { p: 18000, d: 50, i: 0.01, i_clamp: 1 }
+    panda_joint4: { p: 18000, d: 70, i: 0.01, i_clamp: 10000 }
+    panda_joint5: { p: 12000, d: 70, i: 0.01, i_clamp: 1 }
+    panda_joint6: { p: 7000, d: 50, i: 0.01, i_clamp: 1 }
+    panda_joint7: { p: 2000, d: 20, i: 0.0, i_clamp: 1 }
diff --git a/launch/demo_gazebo.launch b/launch/demo_gazebo.launch
@@ -11,48 +11,22 @@
   <!-- 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.urdf.xacro"/>
 
   <!-- launch the gazebo simulator and spawn the robot -->
-  <include file="$(dirname)/gazebo.launch" >
+  <include file="$(find franka_gazebo)/launch/panda.launch">
     <arg name="paused" value="$(arg paused)"/>
-    <arg name="gazebo_gui" value="$(arg gazebo_gui)"/>
-    <arg name="urdf_path" value="$(arg urdf_path)"/>
+    <arg unless="$(arg gazebo_gui)" name="headless" value="true"/>
+    <arg     if="$(arg gazebo_gui)" name="headless" value="false"/>
   </include>
 
-  <!-- If needed, broadcast static tf for robot root -->
-    <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" 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)">
-    <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" />
+  <!-- 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 file="$(dirname)/move_group.launch">
+  <include ns="panda" 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"/>
@@ -61,7 +35,7 @@
   </include>
 
   <!-- Run Rviz and load the default config to see the state of the move_group node -->
-  <include file="$(dirname)/moveit_rviz.launch">
+  <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>

diff --git a/launch/gazebo.launch b/launch/gazebo.launch
diff --git a/launch/move_group.launch b/launch/move_group.launch
@@ -76,7 +76,8 @@
   <!-- Trajectory Execution Functionality -->
   <include ns="move_group" file="$(dirname)/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)"/>
     <arg name="execution_type" value="$(arg execution_type)" />
   </include>

diff --git a/launch/panda_gripper_moveit_controller_manager.launch.xml b/launch/panda_gripper_moveit_controller_manager.launch.xml
@@ -0,0 +1,12 @@
+<launch>
+
+  <!-- this argument is not used here, only necessary so that this launch file has the same args as fake_moveit_controller_manager.launch -->
+  <arg name="execution_type" default="unused" />
+
+  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
+  <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_gripper_moveit_controllers.yaml"/>
+</launch>
diff --git a/launch/panda_moveit_controller_manager.launch.xml b/launch/panda_moveit_controller_manager.launch.xml
@@ -9,5 +9,5 @@
   <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
 
   <!-- loads ros_controllers to the param server -->
-  <rosparam file="$(find panda_moveit_config)/config/ros_controllers.yaml"/>
+  <rosparam file="$(find panda_moveit_config)/config/panda_moveit_controllers.yaml"/>
 </launch>
diff --git a/launch/ros_controllers.launch b/launch/ros_controllers.launch
@@ -6,6 +6,6 @@
 
   <!-- Load the controllers -->
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-    output="screen" args="position_joint_trajectory_controller "/>
+    output="screen" args="effort_joint_trajectory_controller "/>
 
 </launch>