From da44cc99db373d124ba61dad00906ce7ec8dca64 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 25 Aug 2020 20:18:29 +0100 Subject: [PATCH 1/2] [effort_controllers] JointGroupPositionController: Set to current position in starting Currently, the target position upon starting is all zeros. This is not great if a command is not issued immediately. To be safer, set the goal position to the current sensed position and also reset the PID internal state upon starting. --- .../joint_group_position_controller.h | 1 + .../src/joint_group_position_controller.cpp | 14 +++++++++++++- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/effort_controllers/include/effort_controllers/joint_group_position_controller.h b/effort_controllers/include/effort_controllers/joint_group_position_controller.h index 86f720d9f..739fbb63a 100644 --- a/effort_controllers/include/effort_controllers/joint_group_position_controller.h +++ b/effort_controllers/include/effort_controllers/joint_group_position_controller.h @@ -72,6 +72,7 @@ class JointGroupPositionController : public controller_interface::Controller joint_names_; std::vector< hardware_interface::JointHandle > joints_; diff --git a/effort_controllers/src/joint_group_position_controller.cpp b/effort_controllers/src/joint_group_position_controller.cpp index 2cb03411c..21de964d0 100644 --- a/effort_controllers/src/joint_group_position_controller.cpp +++ b/effort_controllers/src/joint_group_position_controller.cpp @@ -120,6 +120,18 @@ namespace effort_controllers return true; } + void JointGroupPositionController::starting(const ros::Time& time) + { + std::vector current_positions(n_joints_, 0.0); + for (std::size_t i = 0; i < n_joints_; ++i) + { + current_positions[i] = joints_[i].getPosition(); + enforceJointLimits(current_positions[i], i); + pid_controllers_[i].reset(); + } + commands_buffer_.writeFromNonRT(current_positions); + } + void JointGroupPositionController::update(const ros::Time& time, const ros::Duration& period) { std::vector & commands = *commands_buffer_.readFromRT(); @@ -127,7 +139,7 @@ namespace effort_controllers { double command_position = commands[i]; - double error; //, vel_error; + double error; double commanded_effort; double current_position = joints_[i].getPosition(); From 997e909023fb5436260f7a79f69762891e730d55 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Fri, 4 Sep 2020 12:01:25 +0100 Subject: [PATCH 2/2] [effort_controllers] JointGroupPositionController: Change to initRT --- effort_controllers/src/joint_group_position_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/effort_controllers/src/joint_group_position_controller.cpp b/effort_controllers/src/joint_group_position_controller.cpp index 21de964d0..49a3fc3dd 100644 --- a/effort_controllers/src/joint_group_position_controller.cpp +++ b/effort_controllers/src/joint_group_position_controller.cpp @@ -129,7 +129,7 @@ namespace effort_controllers enforceJointLimits(current_positions[i], i); pid_controllers_[i].reset(); } - commands_buffer_.writeFromNonRT(current_positions); + commands_buffer_.initRT(current_positions); } void JointGroupPositionController::update(const ros::Time& time, const ros::Duration& period)