Merge pull request #13 from fwalch/SWDEV-561-moveit-fixes

panda_moveit_config: Fixes for demo.launch, gripper support

CHANGELOG.md

 # CHANGELOG
 
+## 0.4.0 - UNRELEASED
+
+  * **BREAKING** Removed `arm_id` from launchfiles
+  * **BREAKING** Changed namespace of `franka_control` controller manager
+  * **BREAKING** Changed behavior of `gripper_action` for compatibility with MoveIt
+  * Changes in `panda_moveit_config`:
+    * Updated joint limits from URDF
+    * Removed `home` poses
+    * Fixed fake execution
+    * Add `load_gripper` argument (default: `true`) to `panda_moveit.launch`
+    * Conditionally load controllers/SRDFs based on `load_gripper`
+    * Add gripper controller configuration (requires running `franka_gripper_node`)
+  * Added `mimic` tag for gripper fingers to URDF and fixed velocity limits
+
 ## 0.3.0 - 2018-02-22
 
 Requires `libfranka` >= 0.3.0

franka_control/config/default_controllers.yaml

@@ -63,4 +63,12 @@ effort_joint_trajectory_controller:
 franka_state_controller:
   type: franka_control/FrankaStateController
   publish_rate: 30  # [Hz]
-
+  joint_names:
+    - panda_joint1
+    - panda_joint2
+    - panda_joint3
+    - panda_joint4
+    - panda_joint5
+    - panda_joint6
+    - panda_joint7
+  arm_id: panda

franka_control/launch/franka_control.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" default="panda" />
   <arg name="load_gripper" default="true" />
 
-  <node name="$(arg arm_id)" pkg="franka_control" type="franka_control_node" output="screen" required="true">
-    <rosparam command="load" file="$(find franka_control)/config/franka_control.yaml" />
-    <param name="robot_ip" value="$(arg robot_ip)" />
-    <param name="arm_id" value="$(arg arm_id)" />
   <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" if="$(arg load_gripper)" />
   <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm.urdf.xacro'" unless="$(arg load_gripper)" />
+
+  <node name="franka_control" pkg="franka_control" type="franka_control_node" output="screen" required="true">
+    <rosparam command="load" file="$(find franka_control)/config/franka_control_node.yaml" />
+    <param name="robot_ip" value="$(arg robot_ip)" />
   </node>
 
-  <group ns="$(arg arm_id)">
   <rosparam command="load" file="$(find franka_control)/config/default_controllers.yaml" />
   <node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_state_controller"/>
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
   <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
-      <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" />
-      <rosparam  if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper_node/joint_states] </rosparam>
+    <rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
     <rosparam  unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam>
     <param name="rate" value="30"/>
   </node>
-  </group>
 </launch>

franka_control/src/franka_control_node.cpp

@@ -30,7 +30,8 @@ class ServiceContainer {
 };
 
 int main(int argc, char** argv) {
-  ros::init(argc, argv, "franka_hw");
+  ros::init(argc, argv, "franka_control_node");
+  ros::NodeHandle public_node_handle;
   ros::NodeHandle node_handle("~");
 
   std::vector<std::string> joint_names_vector;
@@ -42,9 +43,16 @@ int main(int argc, char** argv) {
   std::copy(joint_names_vector.cbegin(), joint_names_vector.cend(), joint_names.begin());
 
   std::string robot_ip;
-  node_handle.getParam("robot_ip", robot_ip);
+  if (!node_handle.getParam("robot_ip", robot_ip)) {
+    ROS_ERROR("Invalid or no robot_ip parameter provided");
+    return 1;
+  }
+
   std::string arm_id;
-  node_handle.getParam("arm_id", arm_id);
+  if (!node_handle.getParam("arm_id", arm_id)) {
+    ROS_ERROR("Invalid or no arm_id parameter provided");
+    return 1;
+  }
   franka::Robot robot(robot_ip);
 
   // Set default collision behavior
@@ -87,6 +95,7 @@ int main(int argc, char** argv) {
           robot.automaticErrorRecovery();
           has_error = false;
           recovery_action_server.setSucceeded();
+          ROS_INFO("Recovered from error");
         } catch (const franka::Exception& ex) {
           recovery_action_server.setAborted(franka_control::ErrorRecoveryResult(), ex.what());
         }
@@ -94,12 +103,12 @@ int main(int argc, char** argv) {
       false);
 
   franka::Model model = robot.loadModel();
-  franka_hw::FrankaHW franka_control(joint_names, arm_id, node_handle, model);
+  franka_hw::FrankaHW franka_control(joint_names, arm_id, public_node_handle, model);
 
   // Initialize robot state before loading any controller
   franka_control.update(robot.readOnce());
 
-  controller_manager::ControllerManager control_manager(&franka_control, node_handle);
+  controller_manager::ControllerManager control_manager(&franka_control, public_node_handle);
 
   recovery_action_server.start();
 

franka_control/src/franka_state_controller.cpp

@@ -151,7 +151,7 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
     ROS_ERROR("FrankaStateController: Could not get Franka state interface from hardware");
     return false;
   }
-  if (!root_node_handle.getParam("arm_id", arm_id_)) {
+  if (!controller_node_handle.getParam("arm_id", arm_id_)) {
     ROS_ERROR("FrankaStateController: Could not get parameter arm_id");
     return false;
   }
@@ -163,7 +163,7 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
                     << publish_rate << " [Hz].");
   }
 
-  if (!root_node_handle.getParam("joint_names", joint_names_) ||
+  if (!controller_node_handle.getParam("joint_names", joint_names_) ||
       joint_names_.size() != robot_state_.q.size()) {
     ROS_ERROR(
         "FrankaStateController: Invalid or no joint_names parameters provided, aborting "

franka_description/robots/hand.xacro

@@ -51,14 +51,15 @@
       <child link="${ns}_leftfinger"/>
       <origin xyz="0 0 0.0584" rpy="0 0 0"/>
       <axis xyz="0 1 0"/>
-      <limit effort="20" lower="-0.001" upper="0.04" velocity="0.3"/>
+      <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
     </joint>
     <joint name="${ns}_finger_joint2" type="prismatic">
       <parent link="${ns}_hand"/>
       <child link="${ns}_rightfinger"/>
       <origin xyz="0 0 0.0584" rpy="0 0 0"/>
       <axis xyz="0 -1 0"/>
-      <limit effort="20" lower="-0.001" upper="0.04" velocity="0.3"/>
+      <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
+      <mimic joint="${ns}_finger_joint1" />
     </joint>
   </xacro:macro>
 </robot>

franka_example_controllers/config/franka_example_controllers.yaml

 joint_velocity_example_controller:
     type: franka_example_controllers/JointVelocityExampleController
+    joint_names:
+        - panda_joint1
+        - panda_joint2
+        - panda_joint3
+        - panda_joint4
+        - panda_joint5
+        - panda_joint6
+        - panda_joint7
 
 joint_position_example_controller:
     type: franka_example_controllers/JointPositionExampleController
+    joint_names:
+        - panda_joint1
+        - panda_joint2
+        - panda_joint3
+        - panda_joint4
+        - panda_joint5
+        - panda_joint6
+        - panda_joint7
 
 cartesian_pose_example_controller:
     type: franka_example_controllers/CartesianPoseExampleController
+    arm_id: panda
 
 cartesian_velocity_example_controller:
     type: franka_example_controllers/CartesianVelocityExampleController
+    arm_id: panda
 
 model_example_controller:
     type: franka_example_controllers/ModelExampleController

franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h

@@ -20,9 +20,7 @@ class CartesianPoseExampleController
     : public controller_interface::MultiInterfaceController<franka_hw::FrankaPoseCartesianInterface,
                                                             franka_hw::FrankaStateInterface> {
  public:
-  bool init(hardware_interface::RobotHW* robot_hardware,
-            ros::NodeHandle& root_node_handle,
-            ros::NodeHandle& /* controller_node_handle */) override;
+  bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
   void starting(const ros::Time&) override;
   void update(const ros::Time&, const ros::Duration& period) override;
 

franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h

@@ -18,9 +18,7 @@ class CartesianVelocityExampleController : public controller_interface::MultiInt
                                                franka_hw::FrankaVelocityCartesianInterface,
                                                franka_hw::FrankaStateInterface> {
  public:
-  bool init(hardware_interface::RobotHW* robot_hardware,
-            ros::NodeHandle& root_node_handle,
-            ros::NodeHandle& /* controller_node_handle */) override;
+  bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
   void update(const ros::Time&, const ros::Duration& period) override;
   void starting(const ros::Time&) override;
   void stopping(const ros::Time&) override;

franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h

@@ -17,9 +17,7 @@ namespace franka_example_controllers {
 class JointPositionExampleController : public controller_interface::MultiInterfaceController<
                                            hardware_interface::PositionJointInterface> {
  public:
-  bool init(hardware_interface::RobotHW* robot_hardware,
-            ros::NodeHandle& root_node_handle,
-            ros::NodeHandle& /* controller_node_handle */) override;
+  bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
   void starting(const ros::Time&) override;
   void update(const ros::Time&, const ros::Duration& period) override;
 

franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h

@@ -18,9 +18,7 @@ class JointVelocityExampleController : public controller_interface::MultiInterfa
                                            hardware_interface::VelocityJointInterface,
                                            franka_hw::FrankaStateInterface> {
  public:
-  bool init(hardware_interface::RobotHW* robot_hardware,
-            ros::NodeHandle& root_node_handle,
-            ros::NodeHandle& /* controller_node_handle */) override;
+  bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
   void update(const ros::Time&, const ros::Duration& period) override;
   void starting(const ros::Time&) override;
   void stopping(const ros::Time&) override;

franka_example_controllers/launch/cartesian_impedance_example_controller.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" value="panda" />
   <arg name="load_gripper" default="true" />
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
-    <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
-  <group ns="$(arg arm_id)">
+
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="cartesian_impedance_example_controller"/>
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_description_with_marker.rviz"/>
-    <node name="interactive_marker" pkg="franka_example_controllers" type="interactive_marker.py" required="true" output="screen" />
+  <node name="interactive_marker" pkg="franka_example_controllers" type="interactive_marker.py" required="true" output="screen">
+    <param name="link_name" value="panda_link0" />
+  </node>
   <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
-  </group>
 </launch>

franka_example_controllers/launch/cartesian_pose_example_controller.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" value="panda" />
   <arg name="load_gripper" default="true" />
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
-    <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
-  <group ns="$(arg arm_id)">
+
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="cartesian_pose_example_controller"/>
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-  </group>
 </launch>

franka_example_controllers/launch/cartesian_velocity_example_controller.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" value="panda" />
   <arg name="load_gripper" default="true" />
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
-    <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
-  <group ns="$(arg arm_id)">
+
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="cartesian_velocity_example_controller"/>
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-  </group>
 </launch>

franka_example_controllers/launch/force_example_controller.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" value="panda" />
   <arg name="load_gripper" default="true" />
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
-    <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
-  <group ns="$(arg arm_id)">
+
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="force_example_controller"/>
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
   <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
-  </group>
 </launch>

franka_example_controllers/launch/joint_impedance_example_controller.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" value="panda" />
   <arg name="load_gripper" default="true" />
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
-    <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
-  <group ns="$(arg arm_id)">
+
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="joint_impedance_example_controller"/>
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-  </group>
 </launch>

franka_example_controllers/launch/joint_position_example_controller.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" value="panda" />
   <arg name="load_gripper" default="true" />
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
-    <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
-  <group ns="$(arg arm_id)">
+
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="joint_position_example_controller"/>
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-  </group>
 </launch>

franka_example_controllers/launch/joint_velocity_example_controller.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" value="panda" />
   <arg name="load_gripper" default="true" />
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
-    <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
-  <group ns="$(arg arm_id)">
+
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="joint_velocity_example_controller"/>
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-  </group>
 </launch>

franka_example_controllers/launch/model_example_controller.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" value="panda" />
   <arg name="load_gripper" default="true" />
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
-    <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
-  <group ns="$(arg arm_id)">
+
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="model_example_controller"/>
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-  </group>
 </launch>