F exploring hand ctrl

sr_mechanism_controllers/CMakeLists.txt

@@ -1,15 +1,22 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(sr_mechanism_controllers)
 add_compile_options(-std=c++11)
-find_package(catkin REQUIRED COMPONENTS rostest std_msgs std_srvs roscpp actionlib controller_manager_msgs sr_robot_msgs
+find_package(catkin REQUIRED COMPONENTS rostest std_msgs std_srvs dynamic_reconfigure roscpp actionlib controller_manager_msgs sr_robot_msgs
         sr_utilities controller_interface ros_ethercat_model realtime_tools velocity_controllers
         pluginlib rosconsole angles control_toolbox sr_hardware_interface xmlrpcpp cmake_modules)
 find_package(TinyXML2 REQUIRED)
 include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${TinyXML2_INCLUDE_DIRS})
 
+#add dynamic reconfigure api
+#find_package(catkin REQUIRED dynamic_reconfigure)
+generate_dynamic_reconfigure_options(
+  cfg/Tendons.cfg
+  #...
+)
+
 catkin_package(
         DEPENDS xmlrpcpp
-        CATKIN_DEPENDS std_msgs std_srvs roscpp actionlib controller_manager_msgs sr_robot_msgs sr_utilities controller_interface ros_ethercat_model
+        CATKIN_DEPENDS std_msgs std_srvs roscpp actionlib dynamic_reconfigure controller_manager_msgs sr_robot_msgs sr_utilities controller_interface ros_ethercat_model
         realtime_tools velocity_controllers pluginlib rosconsole angles control_toolbox sr_hardware_interface
         INCLUDE_DIRS include
         LIBRARIES sr_mechanism_controllers
@@ -27,6 +34,12 @@ else(DEFINED debug)
     message(STATUS " [ ] Debug publisher")
 endif (DEFINED debug)
 
+
+
+# make sure configure headers are built before any node using them
+add_dependencies(sr_mechanism_controllers ${PROJECT_NAME}_gencfg)
+
+
 add_executable(joint_trajectory_action_controller src/joint_trajectory_action_controller.cpp)
 add_dependencies(joint_trajectory_action_controller ${catkin_EXPORTED_TARGETS})
 target_link_libraries(joint_trajectory_action_controller ${catkin_LIBRARIES})

sr_mechanism_controllers/cfg/Tendons.cfg

@@ -0,0 +1,10 @@
+#!/usr/bin/env python
+PACKAGE = "sr_mechanism_controllers"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("ignore_threshold",    int_t,    0, "Command threshold ", 0,  0, 200)
+
+exit(gen.generate(PACKAGE, "sr_mechanism_controllers", "Tendons"))

sr_mechanism_controllers/include/sr_mechanism_controllers/srh_joint_position_controller.hpp

@@ -29,7 +29,7 @@
 #define _SRH_JOINT_POSITION_CONTROLLER_HPP_
 
 #include <sr_mechanism_controllers/sr_controller.hpp>
-
+#include <sr_mechanism_controllers/TendonsConfig.h>
 
 namespace controller
 {
@@ -58,9 +58,15 @@ class SrhJointPositionController :
   /// Internal PID controller for the position loop.
   boost::scoped_ptr<control_toolbox::Pid> pid_controller_position_;
 
+  void callback(sr_mechanism_controllers::TendonsConfig &config, uint32_t level);
+
   /// the position deadband value used in the hysteresis_deadband
   double position_deadband;
 
+  double positive_threshold;
+
+  double negative_threshold;
+
   /// We're using an hysteresis deadband.
   sr_deadband::HysteresisDeadband<double> hysteresis_deadband;
 
@@ -71,6 +77,8 @@ class SrhJointPositionController :
   void setCommandCB(const std_msgs::Float64ConstPtr &msg);
 
   void resetJointState();
+
+  int ignore_threshold;
 };
 }  // namespace controller
 

sr_mechanism_controllers/package.xml

@@ -40,6 +40,7 @@
     <build_depend>cmake_modules</build_depend>
     <build_depend>tinyxml2</build_depend>
     <build_depend>ros_ethercat_model</build_depend>
+<build_depend>dynamic_reconfigure</build_depend>
 
     <!-- Dependencies needed after this package is compiled. -->
     <run_depend>std_msgs</run_depend>
@@ -60,6 +61,9 @@
     <run_depend>sr_hardware_interface</run_depend>
     <run_depend>xmlrpcpp</run_depend>
     <run_depend>ros_ethercat_model</run_depend>
+<run_depend>dynamic_reconfigure</run_depend>
+
+
 
     <!-- Dependencies needed only for running tests. -->
     <test_depend>gtest</test_depend>

sr_mechanism_controllers/src/srh_joint_position_controller.cpp

@@ -32,6 +32,8 @@
 #include <algorithm>
 #include <math.h>
 #include "sr_utilities/sr_math_utils.hpp"
+#include <dynamic_reconfigure/server.h>
+#include <sr_mechanism_controllers/TendonsConfig.h>
 
 #include <std_msgs/Float64.h>
 
@@ -48,6 +50,11 @@ namespace controller
   {
   }
 
+  void SrhJointPositionController::callback(sr_mechanism_controllers::TendonsConfig &config, uint32_t level) {
+    ROS_INFO("Reconfigure Request: %d", config.ignore_threshold);
+    // controller::SrhJointPositionController::ignore_threshold = config.ignore_threshold;
+  }
+
   bool SrhJointPositionController::init(ros_ethercat_model::RobotStateInterface *robot, ros::NodeHandle &n)
   {
     ROS_ASSERT(robot);
@@ -146,12 +153,15 @@ namespace controller
     ROS_INFO_STREAM("Setting new PID parameters. P:" << req.p << " / I:" << req.i <<
                     " / D:" << req.d << " / IClamp:" << req.i_clamp << ", max force: " <<
                     req.max_force << ", friction deadband: " << req.friction_deadband <<
-                    " pos deadband: " << req.deadband);
+                    " pos deadband: " << req.deadband << ", negative_threshold: " <<
+                    req.negative_threshold << ", positive_threshold: " << req.positive_threshold);
 
     pid_controller_position_->setGains(req.p, req.i, req.d, req.i_clamp, -req.i_clamp);
     max_force_demand = req.max_force;
     friction_deadband = req.friction_deadband;
     position_deadband = req.deadband;
+    positive_threshold = req.positive_threshold;
+    negative_threshold = req.negative_threshold;
 
     // Setting the new parameters in the parameter server
     node_.setParam("pid/p", req.p);
@@ -161,6 +171,8 @@ namespace controller
     node_.setParam("pid/max_force", max_force_demand);
     node_.setParam("pid/position_deadband", position_deadband);
     node_.setParam("pid/friction_deadband", friction_deadband);
+    node_.setParam("pid/positive_threshold", positive_threshold);
+    node_.setParam("pid/negative_threshold", negative_threshold);
 
     return true;
   }
@@ -256,10 +268,19 @@ namespace controller
       }
     }
 
+
+    if ((commanded_effort > 0))
+        commanded_effort = commanded_effort + 40.0;
+
+
+    if ((commanded_effort < 0))
+        commanded_effort = commanded_effort - 40.0;
+
+
     joint_state_->commanded_effort_ = commanded_effort;
 
-    if (loop_count_ % 10 == 0)
-    {
+    //if (loop_count_ % 10 == 0)
+    //{
       if (controller_state_publisher_ && controller_state_publisher_->trylock())
       {
         controller_state_publisher_->msg_.header.stamp = time;
@@ -287,7 +308,7 @@ namespace controller
                  dummy);
         controller_state_publisher_->unlockAndPublish();
       }
-    }
+    //}
     loop_count_++;
   }
 
@@ -296,6 +317,9 @@ namespace controller
     node_.param<double>("pid/max_force", max_force_demand, 1023.0);
     node_.param<double>("pid/position_deadband", position_deadband, 0.015);
     node_.param<int>("pid/friction_deadband", friction_deadband, 5);
+    node_.param<double>("pid/negative_threshold", negative_threshold, 0);
+    node_.param<double>("pid/positive_threshold", positive_threshold, 0);
+
   }
 
   void SrhJointPositionController::setCommandCB(const std_msgs::Float64ConstPtr &msg)