From b658707f24193e27ffdd6790ca6cdef5e7f1f406 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Thu, 23 Sep 2021 16:26:54 -0300 Subject: [PATCH] Change nJoints_ type to unsigned int. --- .../computed_torque_controller.hpp | 2 +- src/computed_torque_controller.cpp | 26 +++++++++++----------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/include/computed_torque_controller/computed_torque_controller.hpp b/include/computed_torque_controller/computed_torque_controller.hpp index f63d7c5..afe8ef0 100644 --- a/include/computed_torque_controller/computed_torque_controller.hpp +++ b/include/computed_torque_controller/computed_torque_controller.hpp @@ -41,7 +41,7 @@ namespace effort_controllers class ComputedTorqueController: public controller_interface::ControllerInterface { std::vector jointNames_; - int nJoints_; + unsigned int nJoints_; rclcpp::Subscription::SharedPtr sub_command_; diff --git a/src/computed_torque_controller.cpp b/src/computed_torque_controller.cpp index 0b00f63..3530442 100644 --- a/src/computed_torque_controller.cpp +++ b/src/computed_torque_controller.cpp @@ -143,7 +143,7 @@ namespace effort_controllers std::vector KpVec=node_->get_parameter("Kp").as_double_array(); if(KpVec.empty()) { - RCLCPP_ERROR(node_->get_logger(),"No 'Kp' in controller %s.",node_->get_namespace()); + RCLCPP_ERROR(node_->get_logger(),"No 'Kp' in controller."); return CallbackReturn::ERROR; } Kp_=Eigen::Map(KpVec.data(),nJoints_,nJoints_).transpose(); @@ -151,13 +151,13 @@ namespace effort_controllers std::vector KdVec=node_->get_parameter("Kd").as_double_array(); if(KdVec.empty()) { - RCLCPP_ERROR(node_->get_logger(),"No 'Kd' in controller %s.",node_->get_namespace()); + RCLCPP_ERROR(node_->get_logger(),"No 'Kd' in controller."); return CallbackReturn::ERROR; } Kd_=Eigen::Map(KdVec.data(),nJoints_,nJoints_).transpose(); if(!node_->get_parameter("priority",priority_)) - RCLCPP_WARN(node_->get_logger(),"No 'priority' configured for controller %s. Using highest possible priority.",node_->get_namespace()); + RCLCPP_WARN(node_->get_logger(),"No 'priority' configured for controller. Using highest possible priority."); return CallbackReturn::SUCCESS; } @@ -189,7 +189,7 @@ namespace effort_controllers CallbackReturn ComputedTorqueController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - for(int i=0;i < nJoints_;i++) + for(unsigned int i=0;i < nJoints_;i++) { q_(i)=state_interfaces_[2*i].get_value(); dq_(i)=state_interfaces_[2*i+1].get_value(); @@ -213,17 +213,17 @@ namespace effort_controllers CallbackReturn ComputedTorqueController::on_deactivate(const rclcpp_lifecycle::State &/*previous_state*/) { - for(int i=0;i < nJoints_;i++) + for(unsigned int i=0;i < nJoints_;i++) q_(i)=state_interfaces_[2*i].get_value(); SetToZero(dqr_); SetToZero(ddqr_); - for(unsigned int i=0;i < fext_.size();i++) fext_[i].Zero(); + for(auto &f : fext_) f.Zero(); if(idsolver_->CartToJnt(q_,dqr_,ddqr_,fext_,torque_) < 0) RCLCPP_ERROR(node_->get_logger(),"KDL inverse dynamics solver failed."); - for(int i=0;i < nJoints_;i++) + for(unsigned int i=0;i < nJoints_;i++) command_interfaces_[i].set_value(torque_(i)); return CallbackReturn::SUCCESS; @@ -234,29 +234,29 @@ namespace effort_controllers auto referencePoint=referencePoint_.readFromRT(); if(referencePoint && *referencePoint) { - if((*referencePoint)->positions.size() >= nJoints_) + if((*referencePoint)->positions.size() == nJoints_) qr_.data=Eigen::VectorXd::Map((*referencePoint)->positions.data(),nJoints_); - if((*referencePoint)->velocities.size() >= nJoints_) + if((*referencePoint)->velocities.size() == nJoints_) dqr_.data=Eigen::VectorXd::Map((*referencePoint)->velocities.data(),nJoints_); - if((*referencePoint)->accelerations.size() >= nJoints_) + if((*referencePoint)->accelerations.size() == nJoints_) ddqr_.data=Eigen::VectorXd::Map((*referencePoint)->accelerations.data(),nJoints_); } - for(int i=0;i < nJoints_;i++) + for(unsigned int i=0;i < nJoints_;i++) { q_(i)=state_interfaces_[2*i].get_value(); dq_(i)=state_interfaces_[2*i+1].get_value(); } - for(unsigned int i=0;i < fext_.size();i++) fext_[i].Zero(); + for(auto &f : fext_) f.Zero(); v_.data=ddqr_.data+Kp_*(qr_.data-q_.data)+Kd_*(dqr_.data-dq_.data); if(idsolver_->CartToJnt(q_,dq_,v_,fext_,torque_) < 0) RCLCPP_ERROR(node_->get_logger(),"KDL inverse dynamics solver failed."); - for(int i=0;i < nJoints_;i++) + for(unsigned int i=0;i < nJoints_;i++) command_interfaces_[i].set_value(torque_(i)); return controller_interface::return_type::OK; -- 2.12.0