From 3e96e92a6d70eeef32dea10e5f5a484e669d827a Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Wed, 12 Dec 2018 15:54:08 -0200 Subject: [PATCH] Initial commit. --- .gitignore | 51 +++++ CMakeLists.txt | 212 +++++++++++++++++++++ computed_torque_controller_plugins.xml | 13 ++ doc/urdf2kdl.txt | 9 + .../computed_torque_controller.h | 83 ++++++++ package.xml | 68 +++++++ src/computed_torque_controller.cpp | 205 ++++++++++++++++++++ 7 files changed, 641 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 computed_torque_controller_plugins.xml create mode 100644 doc/urdf2kdl.txt create mode 100644 include/computed_torque_controller/computed_torque_controller.h create mode 100644 package.xml create mode 100644 src/computed_torque_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..2e7a3be --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,212 @@ +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) diff --git a/computed_torque_controller_plugins.xml b/computed_torque_controller_plugins.xml new file mode 100644 index 0000000..8c137af --- /dev/null +++ b/computed_torque_controller_plugins.xml @@ -0,0 +1,13 @@ + + + + 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. + + + diff --git a/doc/urdf2kdl.txt b/doc/urdf2kdl.txt new file mode 100644 index 0000000..0f5af05 --- /dev/null +++ b/doc/urdf2kdl.txt @@ -0,0 +1,9 @@ +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. diff --git a/include/computed_torque_controller/computed_torque_controller.h b/include/computed_torque_controller/computed_torque_controller.h new file mode 100644 index 0000000..d9ffae3 --- /dev/null +++ b/include/computed_torque_controller/computed_torque_controller.h @@ -0,0 +1,83 @@ +/****************************************************************************** + ROS computed_torque_controller Package + Computed Torque Controller + Copyright (C) 2013..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 COMPUTED_TORQUE_CONTROLLER_COMPUTED_TORQUE_CONTROLLER +#define COMPUTED_TORQUE_CONTROLLER_COMPUTED_TORQUE_CONTROLLER + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace effort_controllers +{ + class ComputedTorqueController: public controller_interface:: + Controller + { + ros::NodeHandle node_; + + hardware_interface::EffortJointInterface *hw_; + std::vector 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 diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..4a1cd29 --- /dev/null +++ b/package.xml @@ -0,0 +1,68 @@ + + + computed_torque_controller + 3.0.0 + The computed_torque_controller package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + catkin + + controller_manager + controller_interface + orocos_kdl + kdl_parser + trajectory_msgs + urdf + cmake_modules + + controller_manager + controller_interface + orocos_kdl + urdf + kdl_parser + trajectory_msgs + + + + + + + + + + + diff --git a/src/computed_torque_controller.cpp b/src/computed_torque_controller.cpp new file mode 100644 index 0000000..d4445c0 --- /dev/null +++ b/src/computed_torque_controller.cpp @@ -0,0 +1,205 @@ +/****************************************************************************** + ROS computed_torque_controller Package + Computed Torque Controller + Copyright (C) 2013..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 +{ + 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 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 KpVec; + if(!node_.getParam("Kp",KpVec)) + { + ROS_ERROR("No 'Kp' in controller %s.",node_.getNamespace().c_str()); + return false; + } + Kp_=Eigen::Map(KpVec.data(),nJoints_,nJoints_).transpose(); + + std::vector KdVec; + if(!node_.getParam("Kd",KdVec)) + { + ROS_ERROR("No 'Kd' in controller %s.",node_.getNamespace().c_str()); + return false; + } + Kd_=Eigen::Map(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) -- 2.12.0