diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4357e7153ea0b4922b714261c2103525f9f6a814..b4e5ea8a21eed5be5fca29974e97be16e83504b8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,63 +1,68 @@
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
- effort_controllers
- gazebo_ros
- joint_state_controller
- joint_state_publisher
- joint_trajectory_controller
- robot_state_publisher
- roscpp
- std_msgs
- tf
- xacro
- moveit_core
- moveit_visual_tools
- moveit_ros_planning
- moveit_ros_planning_interface
-)
-
-## System dependencies are found with CMake's conventions
+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
+ effort_controllers
+ gazebo_ros
+ joint_state_controller
+ joint_state_publisher
+ joint_trajectory_controller
+ robot_state_publisher
+ roscpp
+ std_msgs
+ tf
+ xacro
+ moveit_core
+ moveit_visual_tools
+ moveit_ros_planning
+ moveit_ros_planning_interface
+ controller_interface
+ hardware_interface)
+
+# 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
- moveit_core
- moveit_visual_tools
- moveit_ros_planning_interface
-)
-
-###########
-## Build ##
-###########
-
-## 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)
+# ################################################################################################################################
+# 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 ##
+# ################################################################################################################################
+
+# 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})
\ No newline at end of file
+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})
diff --git a/config/panda_control.yaml b/config/panda_control.yaml
index 661fa39c7553edcc3854083f348d097eb430bc74..053fbdcfa29d5b13c8753587b6ebc6b1e2179a53 100644
--- a/config/panda_control.yaml
+++ b/config/panda_control.yaml
@@ -1,10 +1,10 @@
# 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
\ No newline at end of file
+ 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
diff --git a/controller_plugins.xml b/controller_plugins.xml
new file mode 100644
index 0000000000000000000000000000000000000000..c9462ee17c7e2057deb3179864a86fc0ff5537ee
--- /dev/null
+++ b/controller_plugins.xml
@@ -0,0 +1,3 @@
+<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
diff --git a/package.xml b/package.xml
index 2fe021eeaedf37db86c57a2b01898011ec5e1cc1..d614468f8a79fb242b96b56952b679235fc44af0 100644
--- a/package.xml
+++ b/package.xml
@@ -17,7 +17,7 @@
<buildtool_depend>catkin</buildtool_depend>
-
+
<depend>controller_manager</depend>
<depend>effort_controllers</depend>
<depend>gazebo_ros</depend>
@@ -32,16 +32,19 @@
<depend>xacro</depend>
<depend>libjsoncpp-dev</depend>
<depend>moveit_simple_controller_manager</depend>
-
+
<!-- CUSTOM -->
<depend>moveit_core</depend>
<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>
diff --git a/src/joint_position_controller.cpp b/src/joint_position_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6de48be5c47fc30e025f6dc5c86aab794a338a98
--- /dev/null
+++ b/src/joint_position_controller.cpp
@@ -0,0 +1,62 @@
+#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