--- /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(computed_torque_controller)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## 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
+ cmake_modules
+ controller_interface
+ controller_manager
+ kdl_parser
+# orocos_kdl
+ trajectory_msgs
+ urdf
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+#find_package(cmake_modules REQUIRED)
+#find_package(Eigen REQUIRED)
+#find_package(orocos_kdl 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 tag for "message_generation"
+## * 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 dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * 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
+# trajectory_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## 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 computed_torque_controller
+ CATKIN_DEPENDS controller_interface controller_manager kdl_parser trajectory_msgs urdf
+# DEPENDS Eigen orocos_kdl
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+ include
+ ${catkin_INCLUDE_DIRS}
+# TODO: Check names of system library include directories (Eigen)
+ ${Eigen_INCLUDE_DIRS}
+# ${orocos_kdl_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+add_library(${PROJECT_NAME}
+ src/computed_torque_controller.cpp
+)
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/ct_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+ target_link_libraries(${PROJECT_NAME}
+ ${catkin_LIBRARIES}
+ ${Eigen_LIBRARIES}
+# ${orocos_kdl_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_ct.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
+<library path="lib/libcomputed_torque_controller">
+ <class name="effort_controllers/ComputedTorqueController"
+ type="effort_controllers::ComputedTorqueController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ The ComputedTorqueController implements a computed torque controller
+ in joint space for a robot with open chain dynamic model. The
+ reference inputs (command in the ROS nomenclature) are joint
+ positions, velocities and accelerations. This type of controller
+ expects an EffortJointInterface type of hardware interface.
+ </description>
+ </class>
+</library>
--- /dev/null
+KDL specifies the inertia in the reference frame of the link, the URDF
+ specifies the inertia in the inertia reference frame.
+
+A KDL segment is an ideal rigid body to which one single Joint is connected
+and one single tip frame. It contains:
+
+- a Joint located at the root frame of the Segment.
+- a Frame describing the pose between the end of the Joint and the tip
+frame of the Segment.
--- /dev/null
+/******************************************************************************
+ ROS computed_torque_controller Package
+ Computed Torque Controller
+ Copyright (C) 2013..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 COMPUTED_TORQUE_CONTROLLER_COMPUTED_TORQUE_CONTROLLER
+#define COMPUTED_TORQUE_CONTROLLER_COMPUTED_TORQUE_CONTROLLER
+
+#include <cstddef>
+#include <vector>
+#include <string>
+
+#include <ros/node_handle.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+
+#include <Eigen/Core>
+
+#include <kdl/frames.hpp>
+#include <kdl_parser/kdl_parser.hpp>
+#include <kdl/chainidsolver_recursive_newton_euler.hpp>
+
+namespace effort_controllers
+{
+ class ComputedTorqueController: public controller_interface::
+ Controller<hardware_interface::EffortJointInterface>
+ {
+ ros::NodeHandle node_;
+
+ hardware_interface::EffortJointInterface *hw_;
+ std::vector<hardware_interface::JointHandle> joints_;
+ int nJoints_;
+
+ ros::Subscriber sub_command_;
+
+ KDL::ChainIdSolver_RNE *idsolver_;
+
+ KDL::JntArray q_;
+ KDL::JntArray dq_;
+ KDL::JntArray v_;
+
+ KDL::JntArray qr_;
+ KDL::JntArray dqr_;
+ KDL::JntArray ddqr_;
+
+ KDL::JntArray torque_;
+
+ KDL::Wrenches fext_;
+
+ Eigen::MatrixXd Kp_;
+ Eigen::MatrixXd Kd_;
+
+ void commandCB(const trajectory_msgs::JointTrajectoryPoint::
+ ConstPtr &referencePoint);
+
+ public:
+ ComputedTorqueController(void);
+ ~ComputedTorqueController(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>computed_torque_controller</name>
+ <version>3.0.0</version>
+ <description>The computed_torque_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/computed_torque_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_manager</build_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>urdf</build_depend>
+ <build_depend>cmake_modules</build_depend>
+
+ <run_depend>controller_manager</run_depend>
+ <run_depend>controller_interface</run_depend>
+ <build_depend>orocos_kdl</build_depend>
+ <run_depend>urdf</run_depend>
+ <run_depend>kdl_parser</run_depend>
+ <run_depend>trajectory_msgs</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 -->
+ <export>
+ <controller_interface plugin="${prefix}/computed_torque_controller_plugins.xml"/>
+ </export>
+</package>
--- /dev/null
+/******************************************************************************
+ ROS computed_torque_controller Package
+ Computed Torque Controller
+ Copyright (C) 2013..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 <computed_torque_controller/computed_torque_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+namespace effort_controllers
+{
+ ComputedTorqueController::ComputedTorqueController(void):
+ q_(0),dq_(0),v_(0),qr_(0),dqr_(0),ddqr_(0),torque_(0),fext_(0)
+ {
+ }
+
+ ComputedTorqueController::~ComputedTorqueController(void)
+ {
+ sub_command_.shutdown();
+ }
+
+ bool ComputedTorqueController::
+ 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;
+ }
+
+ nJoints_=joint_names.size();
+
+ for(int i=0; i < nJoints_;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;
+ }
+ }
+ sub_command_=node_.subscribe("command",1,
+ &ComputedTorqueController::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((idsolver_=new KDL::ChainIdSolver_RNE(chain,g)) == NULL)
+ {
+ ROS_ERROR("Failed to create ChainIDSolver_RNE.");
+ return false;
+ }
+
+ q_.resize(nJoints_);
+ dq_.resize(nJoints_);
+ v_.resize(nJoints_);
+ qr_.resize(nJoints_);
+ dqr_.resize(nJoints_);
+ ddqr_.resize(nJoints_);
+ torque_.resize(nJoints_);
+
+ fext_.resize(chain.getNrOfSegments());
+
+ Kp_.resize(nJoints_,nJoints_);
+ Kd_.resize(nJoints_,nJoints_);
+
+ std::vector<double> KpVec;
+ if(!node_.getParam("Kp",KpVec))
+ {
+ ROS_ERROR("No 'Kp' in controller %s.",node_.getNamespace().c_str());
+ return false;
+ }
+ Kp_=Eigen::Map<Eigen::MatrixXd>(KpVec.data(),nJoints_,nJoints_).transpose();
+
+ std::vector<double> KdVec;
+ if(!node_.getParam("Kd",KdVec))
+ {
+ ROS_ERROR("No 'Kd' in controller %s.",node_.getNamespace().c_str());
+ return false;
+ }
+ Kd_=Eigen::Map<Eigen::MatrixXd>(KdVec.data(),nJoints_,nJoints_).transpose();
+
+ return true;
+ }
+
+ void ComputedTorqueController::starting(const ros::Time& time)
+ {
+ for(unsigned int i=0;i < nJoints_;i++)
+ {
+ q_(i)=joints_[i].getPosition();
+ dq_(i)=joints_[i].getVelocity();
+ }
+ qr_=q_;
+ dqr_=dq_;
+ SetToZero(ddqr_);
+
+ 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 ComputedTorqueController::update(const ros::Time& time,
+ const ros::Duration& duration)
+ {
+ for(unsigned int i=0;i < nJoints_;i++)
+ {
+ q_(i)=joints_[i].getPosition();
+ dq_(i)=joints_[i].getVelocity();
+ }
+ for(unsigned int i=0;i < fext_.size();i++) fext_[i].Zero();
+
+ v_.data=ddqr_.data+Kp_*(qr_.data-q_.data)+Kd_*(dqr_.data-dq_.data);
+ if(idsolver_->CartToJnt(q_,dq_,v_,fext_,torque_) < 0)
+ ROS_ERROR("KDL inverse dynamics solver failed.");
+
+ for(unsigned int i=0;i < nJoints_;i++)
+ joints_[i].setCommand(torque_(i));
+ }
+
+ void ComputedTorqueController::commandCB(const trajectory_msgs::
+ JointTrajectoryPoint::ConstPtr &referencePoint)
+ {
+ for(unsigned int i=0;i < nJoints_;i++)
+ {
+ qr_(i)=referencePoint->positions[i];
+ dqr_(i)=referencePoint->velocities[i];
+ ddqr_(i)=referencePoint->accelerations[i];
+ }
+ }
+}
+
+PLUGINLIB_EXPORT_CLASS(effort_controllers::ComputedTorqueController,
+ controller_interface::ControllerBase)
projects / computed_torque_controller.git / commitdiff