From 32de4468589791bacdd2a941fe3313e20ea0788e Mon Sep 17 00:00:00 2001 From: carebare47 Date: Wed, 18 Aug 2021 13:46:24 +0000 Subject: [PATCH 01/11] x10 faster state publishing --- .../src/srh_joint_position_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp index c33d651e6..d5a1aaaff 100644 --- a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp +++ b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp @@ -258,8 +258,8 @@ namespace controller 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 +287,7 @@ namespace controller dummy); controller_state_publisher_->unlockAndPublish(); } - } + //} loop_count_++; } From 66d0d4beda36ef8992a7c555d4a6fc1cfb2728d1 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Wed, 18 Aug 2021 13:56:16 +0000 Subject: [PATCH 02/11] experiment to close pwm deadband --- .../src/srh_joint_position_controller.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp index d5a1aaaff..339c127fa 100644 --- a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp +++ b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp @@ -256,6 +256,15 @@ namespace controller } } + + if (commanded_effort > 0) + commanded_effort = commanded_effort + 60.0; + + + if (commanded_effort < 0) + commanded_effort = commanded_effort - 60.0; + + joint_state_->commanded_effort_ = commanded_effort; //if (loop_count_ % 10 == 0) From e2001ccfc14d33d08f70030524f0ef6b2e0cc546 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Wed, 18 Aug 2021 14:29:39 +0000 Subject: [PATCH 03/11] alternative approach (to return to regular PID efficacy plus this deadband thing) --- .../src/srh_joint_position_controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp index 339c127fa..e35ed352c 100644 --- a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp +++ b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp @@ -257,12 +257,12 @@ namespace controller } - if (commanded_effort > 0) - commanded_effort = commanded_effort + 60.0; + if ((commanded_effort > 0) && (commanded_effort < 60)) + commanded_effort = 60.0; - if (commanded_effort < 0) - commanded_effort = commanded_effort - 60.0; + if ((commanded_effort < 0) && (commanded_effort > -60)) + commanded_effort = -60.0; joint_state_->commanded_effort_ = commanded_effort; From 3de9ef3fb542d029ab42c4c32edaa3bd811c5d79 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 19 Aug 2021 08:53:58 +0000 Subject: [PATCH 04/11] moving --- .../src/srh_joint_position_controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp index e35ed352c..fb1755be3 100644 --- a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp +++ b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp @@ -257,12 +257,12 @@ namespace controller } - if ((commanded_effort > 0) && (commanded_effort < 60)) - commanded_effort = 60.0; + if ((commanded_effort > 0)) + commanded_effort = commanded_effort + 60.0; - if ((commanded_effort < 0) && (commanded_effort > -60)) - commanded_effort = -60.0; + if ((commanded_effort < 0)) + commanded_effort = commanded_effort - 60.0; joint_state_->commanded_effort_ = commanded_effort; From a5c9acad3d59fa4045f2da6bec22b37831e29287 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 19 Aug 2021 09:18:48 +0000 Subject: [PATCH 05/11] dynamic reconfigure threshold param --- sr_mechanism_controllers/CMakeLists.txt | 15 +++++++++++++-- sr_mechanism_controllers/cfg/Tendons.cfg | 10 ++++++++++ .../srh_joint_position_controller.hpp | 6 +++++- sr_mechanism_controllers/package.xml | 4 ++++ .../src/srh_joint_position_controller.cpp | 11 +++++++++-- 5 files changed, 41 insertions(+), 5 deletions(-) create mode 100755 sr_mechanism_controllers/cfg/Tendons.cfg diff --git a/sr_mechanism_controllers/CMakeLists.txt b/sr_mechanism_controllers/CMakeLists.txt index dce51a708..f057f1d1c 100644 --- a/sr_mechanism_controllers/CMakeLists.txt +++ b/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 @@ -64,6 +71,10 @@ add_dependencies(simple_trajectory_compare ${catkin_EXPORTED_TARGETS}) target_link_libraries(simple_trajectory_compare ${catkin_LIBRARIES}) +# make sure configure headers are built before any node using them +add_dependencies(sr_mechanism_controllers ${PROJECT_NAME}_gencfg) + + #if(COMMAND add_rostest_gtest) # add_rostest_gtest(test_joint_pos_controller test/joint_pos_controller.test test/test_joint_pos_controller.cpp test/test_controllers.cpp # src/srh_joint_position_controller.cpp src/sr_controller.cpp) diff --git a/sr_mechanism_controllers/cfg/Tendons.cfg b/sr_mechanism_controllers/cfg/Tendons.cfg new file mode 100755 index 000000000..9de7f4ff0 --- /dev/null +++ b/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")) diff --git a/sr_mechanism_controllers/include/sr_mechanism_controllers/srh_joint_position_controller.hpp b/sr_mechanism_controllers/include/sr_mechanism_controllers/srh_joint_position_controller.hpp index 02cd9e522..cc3eb45df 100644 --- a/sr_mechanism_controllers/include/sr_mechanism_controllers/srh_joint_position_controller.hpp +++ b/sr_mechanism_controllers/include/sr_mechanism_controllers/srh_joint_position_controller.hpp @@ -29,7 +29,7 @@ #define _SRH_JOINT_POSITION_CONTROLLER_HPP_ #include - +#include namespace controller { @@ -58,6 +58,8 @@ class SrhJointPositionController : /// Internal PID controller for the position loop. boost::scoped_ptr 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; @@ -71,6 +73,8 @@ class SrhJointPositionController : void setCommandCB(const std_msgs::Float64ConstPtr &msg); void resetJointState(); + + int ignore_threshold; }; } // namespace controller diff --git a/sr_mechanism_controllers/package.xml b/sr_mechanism_controllers/package.xml index b63826f6d..ac72e8351 100644 --- a/sr_mechanism_controllers/package.xml +++ b/sr_mechanism_controllers/package.xml @@ -40,6 +40,7 @@ cmake_modules tinyxml2 ros_ethercat_model +dynamic_reconfigure std_msgs @@ -60,6 +61,9 @@ sr_hardware_interface xmlrpcpp ros_ethercat_model +dynamic_reconfigure + + gtest diff --git a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp index fb1755be3..7975a252b 100644 --- a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp +++ b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp @@ -32,6 +32,8 @@ #include #include #include "sr_utilities/sr_math_utils.hpp" +#include +#include #include @@ -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); + this->ignore_threshold = config.ignore_threshold; + } + bool SrhJointPositionController::init(ros_ethercat_model::RobotStateInterface *robot, ros::NodeHandle &n) { ROS_ASSERT(robot); @@ -258,11 +265,11 @@ namespace controller if ((commanded_effort > 0)) - commanded_effort = commanded_effort + 60.0; + commanded_effort = commanded_effort + float(this->ignore_threshold); if ((commanded_effort < 0)) - commanded_effort = commanded_effort - 60.0; + commanded_effort = commanded_effort - float(this->ignore_threshold); joint_state_->commanded_effort_ = commanded_effort; From f306575ceea9131f4402adac0b7aed5f6140d07d Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 19 Aug 2021 09:57:00 +0000 Subject: [PATCH 06/11] global --- .../src/srh_joint_position_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp index 7975a252b..631506680 100644 --- a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp +++ b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp @@ -52,7 +52,7 @@ namespace controller void SrhJointPositionController::callback(sr_mechanism_controllers::TendonsConfig &config, uint32_t level) { ROS_INFO("Reconfigure Request: %d", config.ignore_threshold); - this->ignore_threshold = config.ignore_threshold; + controller::SrhJointPositionController::ignore_threshold = config.ignore_threshold; } bool SrhJointPositionController::init(ros_ethercat_model::RobotStateInterface *robot, ros::NodeHandle &n) @@ -265,11 +265,11 @@ namespace controller if ((commanded_effort > 0)) - commanded_effort = commanded_effort + float(this->ignore_threshold); + commanded_effort = commanded_effort + float(controller::SrhJointPositionController::ignore_threshold); if ((commanded_effort < 0)) - commanded_effort = commanded_effort - float(this->ignore_threshold); + commanded_effort = commanded_effort - float(controller::SrhJointPositionController::ignore_threshold); joint_state_->commanded_effort_ = commanded_effort; From c2e1c41525572454583259674997bf1b5ddd5af4 Mon Sep 17 00:00:00 2001 From: Tom Queen Date: Thu, 19 Aug 2021 12:48:11 +0100 Subject: [PATCH 07/11] Update srh_joint_position_controller.cpp --- .../src/srh_joint_position_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp index 631506680..3b04fe721 100644 --- a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp +++ b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp @@ -52,7 +52,7 @@ 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; + // controller::SrhJointPositionController::ignore_threshold = config.ignore_threshold; } bool SrhJointPositionController::init(ros_ethercat_model::RobotStateInterface *robot, ros::NodeHandle &n) @@ -265,11 +265,11 @@ namespace controller if ((commanded_effort > 0)) - commanded_effort = commanded_effort + float(controller::SrhJointPositionController::ignore_threshold); + commanded_effort = commanded_effort + 100.0; if ((commanded_effort < 0)) - commanded_effort = commanded_effort - float(controller::SrhJointPositionController::ignore_threshold); + commanded_effort = commanded_effort - 100.0; joint_state_->commanded_effort_ = commanded_effort; From 1ad034842417a6905771d55a379a5864d8062c59 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 19 Aug 2021 12:30:24 +0000 Subject: [PATCH 08/11] moving --- .../src/srh_joint_position_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp index 3b04fe721..772b5db8a 100644 --- a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp +++ b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp @@ -265,11 +265,11 @@ namespace controller if ((commanded_effort > 0)) - commanded_effort = commanded_effort + 100.0; + commanded_effort = commanded_effort + 40.0; if ((commanded_effort < 0)) - commanded_effort = commanded_effort - 100.0; + commanded_effort = commanded_effort - 40.0; joint_state_->commanded_effort_ = commanded_effort; From 40bd7e301a3077f62b645d68efb13704b7b78dd2 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 19 Aug 2021 12:45:49 +0000 Subject: [PATCH 09/11] config through pid params --- .../srh_joint_position_controller.hpp | 4 ++++ .../src/srh_joint_position_controller.cpp | 8 +++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/sr_mechanism_controllers/include/sr_mechanism_controllers/srh_joint_position_controller.hpp b/sr_mechanism_controllers/include/sr_mechanism_controllers/srh_joint_position_controller.hpp index cc3eb45df..0897172d2 100644 --- a/sr_mechanism_controllers/include/sr_mechanism_controllers/srh_joint_position_controller.hpp +++ b/sr_mechanism_controllers/include/sr_mechanism_controllers/srh_joint_position_controller.hpp @@ -63,6 +63,10 @@ class SrhJointPositionController : /// 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 hysteresis_deadband; diff --git a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp index 772b5db8a..41f434ad1 100644 --- a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp +++ b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp @@ -153,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); @@ -312,6 +315,9 @@ namespace controller node_.param("pid/max_force", max_force_demand, 1023.0); node_.param("pid/position_deadband", position_deadband, 0.015); node_.param("pid/friction_deadband", friction_deadband, 5); + node_.param("pid/negative_threshold", negative_threshold, 0); + node_.param("pid/positive_threshold", positive_threshold, 0); + } void SrhJointPositionController::setCommandCB(const std_msgs::Float64ConstPtr &msg) From 99c684f5f0c2f01b9755cc29b7206fe225332582 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 19 Aug 2021 13:51:34 +0000 Subject: [PATCH 10/11] params --- sr_mechanism_controllers/src/srh_joint_position_controller.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp index 41f434ad1..83f38944f 100644 --- a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp +++ b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp @@ -171,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; } From be8026fb60020a394971db988a143150f18cd375 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 19 Aug 2021 15:15:42 +0000 Subject: [PATCH 11/11] CMake --- sr_mechanism_controllers/CMakeLists.txt | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/sr_mechanism_controllers/CMakeLists.txt b/sr_mechanism_controllers/CMakeLists.txt index f057f1d1c..112e02fba 100644 --- a/sr_mechanism_controllers/CMakeLists.txt +++ b/sr_mechanism_controllers/CMakeLists.txt @@ -34,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}) @@ -71,10 +77,6 @@ add_dependencies(simple_trajectory_compare ${catkin_EXPORTED_TARGETS}) target_link_libraries(simple_trajectory_compare ${catkin_LIBRARIES}) -# make sure configure headers are built before any node using them -add_dependencies(sr_mechanism_controllers ${PROJECT_NAME}_gencfg) - - #if(COMMAND add_rostest_gtest) # add_rostest_gtest(test_joint_pos_controller test/joint_pos_controller.test test/test_joint_pos_controller.cpp test/test_controllers.cpp # src/srh_joint_position_controller.cpp src/sr_controller.cpp)