--- /dev/null
+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
--- /dev/null
+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)
--- /dev/null
+/******************************************************************************
+ ROS pid_plus_gravity_controller Package
+ PID+Gravity Compensation Controller
+ Copyright (C) 2017 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#ifndef PID_PLUS_GRAVITY_CONTROLLER_PID_PLUS_GRAVITY_CONTROLLER_H
+#define PID_PLUS_GRAVITY_CONTROLLER_PID_PLUS_GRAVITY_CONTROLLER_H
+
+#include <cstddef>
+#include <vector>
+#include <string>
+
+#include <ros/node_handle.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <control_toolbox/pid.h>
+#include <controller_interface/controller.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+#include <sensor_msgs/JointState.h>
+
+#include <Eigen/Core>
+
+#include <kdl/frames.hpp>
+#include <kdl_parser/kdl_parser.hpp>
+#include <kdl/chaindynparam.hpp>
+
+namespace effort_controllers
+{
+ class PidPlusGravityController: public controller_interface::
+ Controller<hardware_interface::EffortJointInterface>
+ {
+ ros::NodeHandle node_;
+
+ hardware_interface::EffortJointInterface *hw_;
+ std::vector<hardware_interface::JointHandle> joints_;
+
+ ros::Subscriber sub_command_;
+
+ KDL::ChainDynParam *chainParam_;
+
+ KDL::JntArray q_;
+ KDL::JntArray qr_;
+ KDL::JntArray gravity_;
+
+ std::vector<control_toolbox::Pid> 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
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>pid_plus_gravity_controller</name>
+ <version>1.0.0</version>
+ <description>The pid_plus_gravity_controller package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/pid_plus_gravity_controller</url> -->
+
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>controller_interface</build_depend>
+ <build_depend>orocos_kdl</build_depend>
+ <build_depend>kdl_parser</build_depend>
+ <build_depend>trajectory_msgs</build_depend>
+ <build_depend>control_msgs</build_depend>
+ <build_depend>urdf</build_depend>
+ <build_depend>cmake_modules</build_depend>
+
+ <run_depend>controller_interface</run_depend>
+ <run_depend>controller_manager</run_depend>
+ <run_depend>control_msgs</run_depend>
+ <run_depend>urdf</run_depend>
+ <run_depend>kdl_parser</run_depend>
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- You can specify that this package is a metapackage here: -->
+ <!-- <metapackage/> -->
+
+ <!-- Other tools can request additional information be placed here -->
+ <controller_interface plugin="${prefix}/pid_plus_gravity_controller_plugins.xml"/>
+ </export>
+</package>
--- /dev/null
+<library path="lib/libpid_plus_gravity_controller">
+
+ <class name="effort_controllers/PidPlusGravityController"
+ type="effort_controllers::PidPlusGravityController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ 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.
+ </description>
+ </class>
+
+</library>
--- /dev/null
+/******************************************************************************
+ ROS pid_plus_gravity_controller Package
+ PID+Gravity Compensation Controller
+ Copyright (C) 2017 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#include <sys/mman.h>
+
+#include <pid_plus_gravity_controller/pid_plus_gravity_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+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<std::string> 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)
projects / pid_plus_gravity_controller.git / commitdiff