From e020f8952222def8aa308c335e44ac5b6d31d553 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Sun, 19 Mar 2023 19:20:53 -0300 Subject: [PATCH] Chane update() to use the period received as argument, instead of computing it from rclcpp::Time. --- .../pid_plus_gravity_controller.hpp | 2 -- src/pid_plus_gravity_controller.cpp | 11 +++-------- 2 files changed, 3 insertions(+), 10 deletions(-) diff --git a/include/pid_plus_gravity_controller/pid_plus_gravity_controller.hpp b/include/pid_plus_gravity_controller/pid_plus_gravity_controller.hpp index b51de9f..18a4ef9 100644 --- a/include/pid_plus_gravity_controller/pid_plus_gravity_controller.hpp +++ b/include/pid_plus_gravity_controller/pid_plus_gravity_controller.hpp @@ -57,8 +57,6 @@ namespace effort_controllers int priority_; - rclcpp::Time lastTime_; - realtime_tools::RealtimeBuffer> referencePoint_; void commandCB(const trajectory_msgs::msg::JointTrajectoryPoint::SharedPtr referencePoint); diff --git a/src/pid_plus_gravity_controller.cpp b/src/pid_plus_gravity_controller.cpp index d1fadc4..fd6d597 100644 --- a/src/pid_plus_gravity_controller.cpp +++ b/src/pid_plus_gravity_controller.cpp @@ -175,8 +175,6 @@ namespace effort_controllers else if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) RCLCPP_WARN(get_node()->get_logger(),"Failed to lock memory."); - lastTime_=get_node()->get_clock()->now(); - return controller_interface::CallbackReturn::SUCCESS; } @@ -190,13 +188,12 @@ namespace effort_controllers auto time=get_node()->get_clock()->now(); for(unsigned int i=0;i < nJoints_;i++) - command_interfaces_[i].set_value(gravity_(i)+pid_[i]->computeCommand(0,time-lastTime_)); - lastTime_=time; + command_interfaces_[i].set_value(gravity_(i)); return controller_interface::CallbackReturn::SUCCESS; } - controller_interface::return_type PidPlusGravityController::update(const rclcpp::Time &/*time*/,const rclcpp::Duration &/*period*/) + controller_interface::return_type PidPlusGravityController::update(const rclcpp::Time &/*time*/,const rclcpp::Duration & period) { auto referencePoint=referencePoint_.readFromRT(); if(referencePoint && *referencePoint) @@ -209,10 +206,8 @@ namespace effort_controllers if(chainParam_->JntToGravity(q_,gravity_) < 0) RCLCPP_ERROR(get_node()->get_logger(),"KDL dynamics solver failed."); - auto time=get_node()->get_clock()->now(); for(unsigned int i=0;i < nJoints_;i++) - command_interfaces_[i].set_value(gravity_(i)+pid_[i]->computeCommand(qr_(i)-q_(i),time-lastTime_)); - lastTime_=time; + command_interfaces_[i].set_value(gravity_(i)+pid_[i]->computeCommand(qr_(i)-q_(i),period)); return controller_interface::return_type::OK; } -- 2.12.0