Migrate to Position based control, add custom controller for invalid configuration spawns.

CMakeLists.txt

 cmake_minimum_required(VERSION 2.8.3)
 project(panda_simulation)
 
-## Compile as C++11, supported in ROS Kinetic and newer
+# Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++14)
 set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS}")
 
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-  controller_manager
+# Find catkin macros and libraries if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) is used, also find other
+# catkin packages
+find_package(catkin REQUIRED
+             COMPONENTS controller_manager
                         effort_controllers
                         gazebo_ros
                         joint_state_controller
@@ -25,39 +24,45 @@ find_package(catkin REQUIRED COMPONENTS
                         moveit_visual_tools
                         moveit_ros_planning
                         moveit_ros_planning_interface
-)
+                        controller_interface
+                        hardware_interface)
 
-## System dependencies are found with CMake's conventions
+# System dependencies are found with CMake's conventions
 find_package(Boost REQUIRED COMPONENTS filesystem)
 
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(JSONCPP jsoncpp)
 message(${JSONCPP_LIBRARIES})
 
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-catkin_package(
-  CATKIN_DEPENDS
+# ################################################################################################################################
+# catkin specific configuration ##
+# ################################################################################################################################
+# The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects
+catkin_package(CATKIN_DEPENDS
                moveit_core
                moveit_visual_tools
                moveit_ros_planning_interface
-)
+               controller_interface
+               hardware_interface
+               pluginlib
+               DEPENDS
+               system_lib)
 
-###########
-## Build ##
-###########
+# ################################################################################################################################
+# Build ##
+# ################################################################################################################################
 
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-  ${catkin_INCLUDE_DIRS}
-)
+# Specify additional locations of header files Your package locations should be listed before other locations
+include_directories(${catkin_INCLUDE_DIRS})
 
 add_executable(robot_control_node src/robot_control_node.cpp)
 add_executable(box_publisher_node src/box_publisher.cpp)
 
 target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES})
 target_link_libraries(box_publisher_node ${catkin_LIBRARIES})
+
+# add custom controller as library
+add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)
+
+# Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})

config/panda_control.yaml

     # Publish joint states
     joint_state_controller:
         type: joint_state_controller/JointStateController
-        publish_rate: 50
+        publish_rate: 100
 
     panda_arm_controller:
-        type: effort_controllers/JointTrajectoryController
+        type: position_controllers/JointTrajectoryController
         joints:
             - panda_joint1
             - panda_joint2
@@ -14,28 +14,29 @@
             - panda_joint6
             - panda_joint7
 
-        gains:
-            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 }
-
         constraints:
             goal_time: 2.0
 
-        state_publish_rate: 25
+        state_publish_rate: 100
 
     panda_hand_controller:
-        type: effort_controllers/JointTrajectoryController
+        type: position_controllers/JointTrajectoryController
         joints:
             - panda_finger_joint1
             - panda_finger_joint2
 
-        gains:
-            panda_finger_joint1: { p: 5, d: 3.0, i: 0, i_clamp: 1 }
-            panda_finger_joint2: { p: 5, d: 1.0, i: 0, i_clamp: 1 }
-
         state_publish_rate: 25
+    joint_position_controller:
+        type: panda_simulation/JointPositionController
+        arm_id: panda
+        joint_names:
+            - panda_joint1
+            - panda_joint2
+            - panda_joint3
+            - panda_joint4
+            - panda_joint5
+            - panda_joint6
+            - panda_joint7
+        gains: [
+            1, 1, 1, 1, 1, 1, 1
+        ]
\ No newline at end of file

controller_plugins.xml

+<library path="lib/libpanda_simulation_controllers_lib">
+    <class name="panda_simulation/JointPositionController" type="panda_simulation::JointPositionController" base_class_type="controller_interface::ControllerBase" />
+</library>
\ No newline at end of file

package.xml

@@ -38,10 +38,13 @@
   <depend>moveit_ros_planning_interface</depend>
   <depend>moveit_ros_perception</depend>
   <depend>moveit_visual_tools</depend>
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+
+  <exec_depend>pluginlib</exec_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
-    <!-- Other tools can request additional information be placed here -->
-
+    <controller_interface plugin="${prefix}/controller_plugins.xml" />
   </export>
 </package>

src/joint_position_controller.cpp

+#include <controller_interface/controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <std_msgs/Float64MultiArray.h>
+
+#include <vector>
+
+namespace panda_simulation {
+
+class JointPositionController : public controller_interface::Controller<hardware_interface::PositionJointInterface> {
+  bool init(hardware_interface::PositionJointInterface *hw, ros::NodeHandle &n) {
+    std::vector<std::string> joint_names;
+    if (!n.getParam("joint_names", joint_names)) {
+      ROS_ERROR("Could not read joint names from param server");
+      return false;
+    }
+
+    // retrieve gains
+    if (!n.getParam("gains", gains_vec_)) {
+      ROS_ERROR("Could not read joint gains from param server");
+      return false;
+    }
+
+    for (auto &joint_name : joint_names) {
+      joint_handles_.push_back(hw->getHandle(joint_name));
+    }
+
+    for (auto &joint_handle : joint_handles_) {
+      command_.push_back(joint_handle.getPosition());
+    }
+
+    sub_command_ = n.subscribe<std_msgs::Float64MultiArray>(std::string("command"), 1,
+                                                            &JointPositionController::setCommandCallback, this);
+
+    return true;
+  }
+
+  void update(const ros::Time &time, const ros::Duration &period) {
+    for (size_t i = 0; i < joint_handles_.size(); i++) {
+      double error = command_.at(i) - joint_handles_.at(i).getPosition();
+      double commanded_effort = error * gains_vec_.at(i);
+
+      joint_handles_.at(i).setCommand(commanded_effort);
+    }
+  }
+
+  void setCommandCallback(const std_msgs::Float64MultiArrayConstPtr &msg) { command_ = msg->data; }
+
+  void starting(const ros::Time &time) {}
+
+  void stopping(const ros::Time &time) {}
+
+private:
+  std::vector<hardware_interface::JointHandle> joint_handles_;
+  std::vector<double> gains_vec_;
+  std::vector<double> command_;
+  ros::Subscriber sub_command_;
+};
+
+PLUGINLIB_EXPORT_CLASS(panda_simulation::JointPositionController, controller_interface::ControllerBase);
+
+} // namespace panda_simulation
\ No newline at end of file