From 944a846a5319f7e6f72c3ecd3c0aa095c65dc43b Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Thu, 29 Nov 2018 03:38:20 -0200 Subject: [PATCH 1/1] Initial commit. --- .gitignore | 51 ++++++ CMakeLists.txt | 170 ++++++++++++++++++++ .../pid_plus_gravity_controller.h | 75 +++++++++ package.xml | 66 ++++++++ pid_plus_gravity_controller_plugins.xml | 15 ++ src/pid_plus_gravity_controller.cpp | 177 +++++++++++++++++++++ 6 files changed, 554 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 include/pid_plus_gravity_controller/pid_plus_gravity_controller.h create mode 100644 package.xml create mode 100644 pid_plus_gravity_controller_plugins.xml create mode 100644 src/pid_plus_gravity_controller.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c1edc29 --- /dev/null +++ b/.gitignore @@ -0,0 +1,51 @@ +devel/ +logs/ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +#*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..b1800bb --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,170 @@ +cmake_minimum_required(VERSION 2.8.3) +project(pid_plus_gravity_controller) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + controller_interface + control_msgs + urdf +) +find_package(cmake_modules REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) +find_package(Eigen REQUIRED) +find_package(orocos_kdl REQUIRED) +find_package(kdl_parser REQUIRED) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependencies might have been +## pulled in transitively but can be declared for certainty nonetheless: +## * add a build_depend tag for "message_generation" +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS controller_interface control_msgs urdf +# DEPENDS system_lib + DEPENDS eigen orocos_kdl kdl_parser +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories(include + ${catkin_INCLUDE_DIRS} + ${Eigen_INCLUDE_DIRS} +) + +## Declare a cpp library +add_library(pid_plus_gravity_controller + src/pid_plus_gravity_controller.cpp +) + +## Declare a cpp executable +# add_executable(pid_plus_gravity_controller_node src/pid_plus_gravity_controller_node.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +# add_dependencies(pid_plus_gravity_controller_node pid_plus_gravity_controller_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${orocos_kdl_LIBRARIES} + ${kdl_parser_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_pid_plus_gravity_controller.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/include/pid_plus_gravity_controller/pid_plus_gravity_controller.h b/include/pid_plus_gravity_controller/pid_plus_gravity_controller.h new file mode 100644 index 0000000..ff68c3a --- /dev/null +++ b/include/pid_plus_gravity_controller/pid_plus_gravity_controller.h @@ -0,0 +1,75 @@ +/****************************************************************************** + ROS pid_plus_gravity_controller Package + PID+Gravity Compensation Controller + Copyright (C) 2017 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#ifndef PID_PLUS_GRAVITY_CONTROLLER_PID_PLUS_GRAVITY_CONTROLLER_H +#define PID_PLUS_GRAVITY_CONTROLLER_PID_PLUS_GRAVITY_CONTROLLER_H + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace effort_controllers +{ + class PidPlusGravityController: public controller_interface:: + Controller + { + ros::NodeHandle node_; + + hardware_interface::EffortJointInterface *hw_; + std::vector joints_; + + ros::Subscriber sub_command_; + + KDL::ChainDynParam *chainParam_; + + KDL::JntArray q_; + KDL::JntArray qr_; + KDL::JntArray gravity_; + + std::vector pid_; + + void commandCB(const trajectory_msgs::JointTrajectoryPoint::ConstPtr + &referencePoint); + + public: + PidPlusGravityController(void); + ~PidPlusGravityController(void); + + bool init(hardware_interface::EffortJointInterface *hw, + ros::NodeHandle &n); + void starting(const ros::Time& time); + void update(const ros::Time& time,const ros::Duration& duration); + }; +} +#endif diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..650270b --- /dev/null +++ b/package.xml @@ -0,0 +1,66 @@ + + + pid_plus_gravity_controller + 1.0.0 + The pid_plus_gravity_controller package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + catkin + + controller_interface + orocos_kdl + kdl_parser + trajectory_msgs + control_msgs + urdf + cmake_modules + + controller_interface + controller_manager + control_msgs + urdf + kdl_parser + + + + + + + + + + diff --git a/pid_plus_gravity_controller_plugins.xml b/pid_plus_gravity_controller_plugins.xml new file mode 100644 index 0000000..5654593 --- /dev/null +++ b/pid_plus_gravity_controller_plugins.xml @@ -0,0 +1,15 @@ + + + + + The PidPlusGravityController implements a PID controller with gravity + compensation in joint space for a robot with open chain dynamic model. + The reference inputs (command in the ROS nomenclature) are joint + positions. This type of controller expects an EffortJointInterface + type of hardware interface. + + + + diff --git a/src/pid_plus_gravity_controller.cpp b/src/pid_plus_gravity_controller.cpp new file mode 100644 index 0000000..1d4118c --- /dev/null +++ b/src/pid_plus_gravity_controller.cpp @@ -0,0 +1,177 @@ +/****************************************************************************** + ROS pid_plus_gravity_controller Package + PID+Gravity Compensation Controller + Copyright (C) 2017 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#include + +#include +#include + +namespace effort_controllers +{ + PidPlusGravityController::PidPlusGravityController(void): + q_(0),qr_(0),gravity_(0) + { + } + + PidPlusGravityController::~PidPlusGravityController(void) + { + sub_command_.shutdown(); + } + + bool PidPlusGravityController:: + init(hardware_interface::EffortJointInterface *hw,ros::NodeHandle &n) + { + node_=n; + hw_=hw; + + std::vector joint_names; + if(!node_.getParam("joints",joint_names)) + { + ROS_ERROR("No 'joints' in controller. (namespace: %s)", + node_.getNamespace().c_str()); + return false; + } + + pid_.resize(joint_names.size()); + + for(int i=0; i < joint_names.size();i++) + { + try + { + joints_.push_back(hw->getHandle(joint_names[i])); + } + catch(const hardware_interface::HardwareInterfaceException& e) + { + ROS_ERROR_STREAM("Exception thrown: " << e.what()); + return false; + } + if(!pid_[i].init(ros::NodeHandle(node_,joint_names[i]))) + { + ROS_ERROR_STREAM("Failed to load PID parameters from " << joint_names[i] + "/pid"); + return false; + } + } + + sub_command_=node_.subscribe("command",1, + &PidPlusGravityController::commandCB, this); + + std::string robot_desc_string; + if(!node_.getParam("/robot_description",robot_desc_string)) + { + ROS_ERROR("Could not find '/robot_description'."); + return false; + } + + KDL::Tree tree; + if (!kdl_parser::treeFromString(robot_desc_string,tree)) + { + ROS_ERROR("Failed to construct KDL tree."); + return false; + } + + std::string chainRoot; + if(!node_.getParam("chain/root",chainRoot)) + { + ROS_ERROR("Could not find 'chain_root' parameter."); + return false; + } + + std::string chainTip; + if(!node_.getParam("chain/tip",chainTip)) + { + ROS_ERROR("Could not find 'chain/tip' parameter."); + return false; + } + + KDL::Chain chain; + if (!tree.getChain(chainRoot,chainTip,chain)) + { + ROS_ERROR("Failed to get chain from KDL tree."); + return false; + } + + KDL::Vector g; + node_.param("gravity/x",g[0],0.0); + node_.param("gravity/y",g[1],0.0); + node_.param("gravity/z",g[2],-9.8); + + if((chainParam_=new KDL::ChainDynParam(chain,g)) == NULL) + { + ROS_ERROR("Failed to create ChainDynParam."); + return false; + } + + q_.resize(chain.getNrOfJoints()); + qr_.resize(chain.getNrOfJoints()); + gravity_.resize(chain.getNrOfJoints()); + + return true; + } + + void PidPlusGravityController::starting(const ros::Time& time) + { + for(unsigned int i=0;i < joints_.size();i++) + q_(i)=joints_[i].getPosition(); + qr_=q_; + + struct sched_param param; + if(!node_.getParam("priority",param.sched_priority)) + { + ROS_WARN("No 'priority' configured for controller %s. Using highest possible priority.",node_.getNamespace().c_str()); + param.sched_priority=sched_get_priority_max(SCHED_FIFO); + } + if(sched_setscheduler(0,SCHED_FIFO,¶m) == -1) + { + ROS_WARN("Failed to set real-time scheduler."); + return; + } + if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) + ROS_WARN("Failed to lock memory."); + } + + void PidPlusGravityController::update(const ros::Time& time, + const ros::Duration& duration) + { + for(unsigned int i=0;i < joints_.size();i++) + q_(i)=joints_[i].getPosition(); + + if(chainParam_->JntToGravity(q_,gravity_) < 0) + ROS_ERROR("KDL dynamics solver failed."); + + for(unsigned int i=0;i < joints_.size();i++) + joints_[i].setCommand(gravity_(i)+pid_[i].computeCommand(qr_(i)-q_(i),duration)); + } + + void PidPlusGravityController::commandCB(const trajectory_msgs:: + JointTrajectoryPoint::ConstPtr &referencePoint) + { + if(referencePoint->positions.size()!=qr_.rows()) + { + ROS_ERROR_STREAM("Dimension of command (" << referencePoint->positions.size() << ") does not match number of joints (" << qr_.rows() << ")!"); + return; + } + for(unsigned int i=0;i < qr_.rows();i++) + qr_(i)=referencePoint->positions[i]; + } +} + +PLUGINLIB_EXPORT_CLASS(effort_controllers::PidPlusGravityController, + controller_interface::ControllerBase) -- 2.12.0