devel/
+install/
logs/
build/
bin/
# Catkin custom files
CATKIN_IGNORE
+
+# Colcon custom files
+COLCON_IGNORE
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.8)
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
- roscpp
- trajectory_msgs
- urdf
-)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(controller_manager REQUIRED)
+find_package(trajectory_msgs REQUIRED)
+find_package(urdf REQUIRED)
+find_package(kdl_parser REQUIRED)
find_package(orocos_kdl REQUIRED)
-find_package(Eigen3 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 exec_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 exec_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 ##
-################################################
+add_library(computed_torque_controller SHARED src/computed_torque_controller.cpp)
+target_compile_features(computed_torque_controller PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
+target_include_directories(computed_torque_controller PRIVATE include)
+ament_target_dependencies(
+ computed_torque_controller
+ "rclcpp"
+ "controller_interface"
+ "controller_manager"
+ "trajectory_msgs"
+ "urdf"
+ "kdl_parser"
+ "orocos_kdl"
+)
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_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
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(computed_torque_controller PRIVATE "COMPUTED_TORQUE_CONTROLLER_BUILDING_LIBRARY")
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
+pluginlib_export_plugin_description_file(controller_interface computed_torque_controller_plugin.xml)
-###################################
-## 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 your 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 cmake_modules controller_interface controller_manager kdl_parser roscpp trajectory_msgs urdf
-# DEPENDS orocos_kdl Eigen3
+install(
+ DIRECTORY include/
+ DESTINATION include
)
-
-###########
-## 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 (orocos_kdl, Eigen3)
- ${orocos_kdl_INCLUDE_DIRS}
- ${EIGEN3_INCLUDE_DIRS}
+install(
+ TARGETS computed_torque_controller
+ EXPORT export_${PROJECT_NAME}
+ ARCHIVE DESTINATION lib
+ LIBRARY DESTINATION lib
+ RUNTIME DESTINATION bin
)
-## Declare a C++ library
-add_library(${PROJECT_NAME}
- src/computed_torque_controller.cpp
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # uncomment the line when a copyright and license is not present in all source files
+ #set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # uncomment the line when this package is not in a git repo
+ #set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(
+ include
)
-
-## 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/computed_torque_controller_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}
- ${orocos_kdl_LIBRARIES}
-# ${EIGEN3_LIBRARIES}
+ament_export_libraries(
+ computed_torque_controller
)
-
-#############
-## 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
-# catkin_install_python(PROGRAMS
-# scripts/my_python_script
-# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-install(TARGETS ${PROJECT_NAME}
- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+ament_export_targets(
+ export_${PROJECT_NAME}
)
-## 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_computed_torque_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)
+ament_package()
-<library path="lib/libcomputed_torque_controller">
+<library path="computed_torque_controller">
<class name="effort_controllers/ComputedTorqueController"
type="effort_controllers::ComputedTorqueController"
- base_class_type="controller_interface::ControllerBase">
+ base_class_type="controller_interface::ControllerInterface">
<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.
+ expects an effort interface.
</description>
</class>
</library>
+++ /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::Tree tree_;
- KDL::Chain chain_;
- 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
+/******************************************************************************
+ ROS 2 computed_torque_controller Package
+ Computed Torque Controller
+ Copyright (C) 2013..2021 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_HPP_
+#define COMPUTED_TORQUE_CONTROLLER__COMPUTED_TORQUE_CONTROLLER_HPP_
+
+#include <controller_interface/controller_interface.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <trajectory_msgs/msg/joint_trajectory_point.hpp>
+#include <rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp>
+
+#include <kdl_parser/kdl_parser.hpp>
+#include <kdl/chainidsolver_recursive_newton_euler.hpp>
+
+#include <computed_torque_controller/visibility_control.h>
+
+namespace effort_controllers
+{
+ using CallbackReturn=rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+ class ComputedTorqueController: public controller_interface::ControllerInterface
+ {
+ std::vector<std::string> jointNames_;
+ int nJoints_;
+
+ rclcpp::Subscription<trajectory_msgs::msg::JointTrajectoryPoint>::SharedPtr sub_command_;
+
+ std::string robotDescription_;
+ KDL::Chain chain_;
+ 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_;
+
+ int priority_;
+
+ void commandCB(const trajectory_msgs::msg::JointTrajectoryPoint::SharedPtr referencePoint);
+ void robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription);
+
+ public:
+ COMPUTED_TORQUE_CONTROLLER_PUBLIC
+ ComputedTorqueController(void);
+
+ COMPUTED_TORQUE_CONTROLLER_PUBLIC
+ controller_interface::InterfaceConfiguration command_interface_configuration(void) const override;
+
+ COMPUTED_TORQUE_CONTROLLER_PUBLIC
+ controller_interface::InterfaceConfiguration state_interface_configuration(void) const override;
+
+ COMPUTED_TORQUE_CONTROLLER_PUBLIC
+ CallbackReturn on_init(void) override;
+
+ COMPUTED_TORQUE_CONTROLLER_PUBLIC
+ CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override;
+
+ COMPUTED_TORQUE_CONTROLLER_PUBLIC
+ CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override;
+
+ COMPUTED_TORQUE_CONTROLLER_PUBLIC
+ CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
+
+ COMPUTED_TORQUE_CONTROLLER_PUBLIC
+ controller_interface::return_type update(void) override;
+ };
+}
+#endif
--- /dev/null
+/******************************************************************************
+ ROS 2 computed_torque_controller Package
+ Computed Torque Controller Visibility Control
+ Copyright (C) 2013..2021 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__VISIBILITY_CONTROL_H_
+#define COMPUTED_TORQUE_CONTROLLER__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+// https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+ #ifdef __GNUC__
+ #define COMPUTED_TORQUE_CONTROLLER_EXPORT __attribute__ ((dllexport))
+ #define COMPUTED_TORQUE_CONTROLLER_IMPORT __attribute__ ((dllimport))
+ #else
+ #define COMPUTED_TORQUE_CONTROLLER_EXPORT __declspec(dllexport)
+ #define COMPUTED_TORQUE_CONTROLLER_IMPORT __declspec(dllimport)
+ #endif
+ #ifdef COMPUTED_TORQUE_CONTROLLER_BUILDING_LIBRARY
+ #define COMPUTED_TORQUE_CONTROLLER_PUBLIC COMPUTED_TORQUE_CONTROLLER_EXPORT
+ #else
+ #define COMPUTED_TORQUE_CONTROLLER_PUBLIC COMPUTED_TORQUE_CONTROLLER_IMPORT
+ #endif
+ #define COMPUTED_TORQUE_CONTROLLER_PUBLIC_TYPE COMPUTED_TORQUE_CONTROLLER_PUBLIC
+ #define COMPUTED_TORQUE_CONTROLLER_LOCAL
+#else
+ #define COMPUTED_TORQUE_CONTROLLER_EXPORT __attribute__ ((visibility("default")))
+ #define COMPUTED_TORQUE_CONTROLLER_IMPORT
+ #if __GNUC__ >= 4
+ #define COMPUTED_TORQUE_CONTROLLER_PUBLIC __attribute__ ((visibility("default")))
+ #define COMPUTED_TORQUE_CONTROLLER_LOCAL __attribute__ ((visibility("hidden")))
+ #else
+ #define COMPUTED_TORQUE_CONTROLLER_PUBLIC
+ #define COMPUTED_TORQUE_CONTROLLER_LOCAL
+ #endif
+ #define COMPUTED_TORQUE_CONTROLLER_PUBLIC_TYPE
+#endif
+
+#endif // COMPUTED_TORQUE_CONTROLLER__VISIBILITY_CONTROL_H_
<?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
<name>computed_torque_controller</name>
- <version>3.1.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> -->
+ <version>0.0.0</version>
+ <description>The computed_torque_controller implements a computed torque controller in joint space</description>
<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>
+ <buildtool_depend>ament_cmake_ros</buildtool_depend>
- <!-- Url tags are optional, but multiple 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, multiple are allowed, one per tag -->
- <!-- Authors do not have to be maintainers, 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 depend as a shortcut for packages that are both build and exec dependencies -->
- <!-- <depend>roscpp</depend> -->
- <!-- Note that this is equivalent to the following: -->
- <!-- <build_depend>roscpp</build_depend> -->
- <!-- <exec_depend>roscpp</exec_depend> -->
- <!-- Use build_depend for packages you need at compile time: -->
- <!-- <build_depend>message_generation</build_depend> -->
- <!-- Use build_export_depend for packages you need in order to build against this package: -->
- <!-- <build_export_depend>message_generation</build_export_depend> -->
- <!-- Use buildtool_depend for build tool packages: -->
- <!-- <buildtool_depend>catkin</buildtool_depend> -->
- <!-- Use exec_depend for packages you need at runtime: -->
- <!-- <exec_depend>message_runtime</exec_depend> -->
- <!-- Use test_depend for packages you need only for testing: -->
- <!-- <test_depend>gtest</test_depend> -->
- <!-- Use doc_depend for packages you need only for building documentation: -->
- <!-- <doc_depend>doxygen</doc_depend> -->
- <buildtool_depend>catkin</buildtool_depend>
- <build_depend>Eigen3</build_depend>
- <build_depend>cmake_modules</build_depend>
- <build_depend>controller_interface</build_depend>
- <build_depend>controller_manager</build_depend>
- <build_depend>kdl_parser</build_depend>
- <build_depend>orocos_kdl</build_depend>
- <build_depend>roscpp</build_depend>
- <build_depend>trajectory_msgs</build_depend>
- <build_depend>urdf</build_depend>
- <build_export_depend>Eigen3</build_export_depend>
- <build_export_depend>cmake_modules</build_export_depend>
- <build_export_depend>controller_interface</build_export_depend>
- <build_export_depend>controller_manager</build_export_depend>
- <build_export_depend>kdl_parser</build_export_depend>
- <build_export_depend>orocos_kdl</build_export_depend>
- <build_export_depend>roscpp</build_export_depend>
- <build_export_depend>trajectory_msgs</build_export_depend>
- <build_export_depend>urdf</build_export_depend>
- <exec_depend>Eigen3</exec_depend>
- <exec_depend>cmake_modules</exec_depend>
- <exec_depend>controller_interface</exec_depend>
- <exec_depend>controller_manager</exec_depend>
- <exec_depend>kdl_parser</exec_depend>
- <exec_depend>orocos_kdl</exec_depend>
- <exec_depend>roscpp</exec_depend>
- <exec_depend>trajectory_msgs</exec_depend>
- <exec_depend>urdf</exec_depend>
+ <depend>rclcpp</depend>
+ <depend>controller_interface</depend>
+ <depend>controller_manager</depend>
+ <depend>trajectory_msgs</depend>
+ <depend>urdf</depend>
+ <depend>kdl_parser</depend>
+ <depend>orocos_kdl</depend>
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_lint_common</test_depend>
- <!-- The export tag contains other, unspecified, tags -->
<export>
- <!-- Other tools can request additional information be placed here -->
- <controller_interface plugin="${prefix}/computed_torque_controller_plugins.xml"/>
+ <build_type>ament_cmake</build_type>
</export>
</package>
/******************************************************************************
- ROS computed_torque_controller Package
+ ROS 2 computed_torque_controller Package
Computed Torque Controller
- Copyright (C) 2013..2017 Walter Fetter Lages <w.fetter@ieee.org>
+ Copyright (C) 2013..2021 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
#include <sys/mman.h>
-#include <computed_torque_controller/computed_torque_controller.h>
-#include <pluginlib/class_list_macros.h>
+#include <rclcpp/logging.hpp>
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+#include <pluginlib/class_list_macros.hpp>
+
+#include <computed_torque_controller/computed_torque_controller.hpp>
namespace effort_controllers
{
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)
+
+ CallbackReturn ComputedTorqueController::on_init(void)
{
- node_=n;
- hw_=hw;
-
- std::vector<std::string> joint_names;
- if(!node_.getParam("joints",joint_names))
+ try
{
- ROS_ERROR("No 'joints' in controller. (namespace: %s)",
- node_.getNamespace().c_str());
- return false;
+ auto_declare<std::vector<std::string>>("joints",jointNames_);
+
+ auto_declare<std::string>("chain.root","origin_link");
+ auto_declare<std::string>("chain.tip", "tool_link");
+
+ auto_declare<double>("gravity.x",0.0);
+ auto_declare<double>("gravity.y",0.0);
+ auto_declare<double>("gravity.z",-9.8);
+
+ auto_declare<std::vector<double>>("Kp",std::vector<double>());
+ auto_declare<std::vector<double>>("Kd",std::vector<double>());
+
+ auto_declare<int>("priority",sched_get_priority_max(SCHED_FIFO));
}
-
- 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))
+ catch(const std::exception &e)
{
- ROS_ERROR("Could not find '/robot_description'.");
- return false;
+ RCLCPP_ERROR_STREAM(node_->get_logger(),"Exception thrown in on_init() with message: " << e.what() << "\n");
+ return CallbackReturn::ERROR;
}
-
- if (!kdl_parser::treeFromString(robot_desc_string,tree_))
+
+ using std::placeholders::_1;
+ rclcpp::QoS qos(rclcpp::KeepLast(1));
+ qos.transient_local();
+ auto robotDescriptionSubscriber=node_->create_subscription<std_msgs::msg::String>("robot_description",qos,std::bind(&ComputedTorqueController::robotDescriptionCB,this,_1));
+ while(robotDescription_.empty())
{
- ROS_ERROR("Failed to construct KDL tree.");
- return false;
+ RCLCPP_WARN_SKIPFIRST_THROTTLE(node_->get_logger(),*node_->get_clock(),1000,"Waiting for robot model on /robot_description.");
+ rclcpp::spin_some(node_->get_node_base_interface());
}
-
- std::string chainRoot;
- if(!node_.getParam("chain/root",chainRoot))
+
+ return CallbackReturn::SUCCESS;
+ }
+
+ CallbackReturn ComputedTorqueController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
+ {
+ jointNames_=node_->get_parameter("joints").as_string_array();
+ if(jointNames_.empty())
{
- ROS_ERROR("Could not find 'chain_root' parameter.");
- return false;
+ RCLCPP_ERROR(node_->get_logger(),"'joints' parameter was empty,");
+ return CallbackReturn::ERROR;
}
-
- std::string chainTip;
- if(!node_.getParam("chain/tip",chainTip))
+
+ nJoints_=jointNames_.size();
+
+ using std::placeholders::_1;
+ sub_command_=node_->create_subscription<trajectory_msgs::msg::JointTrajectoryPoint>("command",1,
+ std::bind(&ComputedTorqueController::commandCB,this,_1));
+
+ while(robotDescription_.empty())
+ RCLCPP_WARN_SKIPFIRST_THROTTLE(node_->get_logger(),*node_->get_clock(),1000,"Waiting for robot model on /robot_description.");
+
+ KDL::Tree tree;
+ if (!kdl_parser::treeFromString(robotDescription_,tree))
{
- ROS_ERROR("Could not find 'chain/tip' parameter.");
- return false;
+ RCLCPP_ERROR(node_->get_logger(),"Failed to construct KDL tree.");
+ return CallbackReturn::ERROR;
}
-
- if (!tree_.getChain(chainRoot,chainTip,chain_))
+
+ std::string chainRoot=node_->get_parameter("chain.root").as_string();
+ if(chainRoot.empty())
{
- ROS_ERROR("Failed to get chain from KDL tree.");
- return false;
+ RCLCPP_ERROR(node_->get_logger(),"Could not find 'chain.root' parameter.");
+ return CallbackReturn::ERROR;
}
-
+
+ std::string chainTip=node_->get_parameter("chain.tip").as_string();
+ if(chainTip.empty())
+ {
+ RCLCPP_ERROR(node_->get_logger(),"Could not find 'chain,tip' parameter.");
+ return CallbackReturn::ERROR;
+ }
+
+ if (!tree.getChain(chainRoot,chainTip,chain_))
+ {
+ RCLCPP_ERROR(node_->get_logger(),"Failed to get chain from KDL tree.");
+ return CallbackReturn::ERROR;
+ }
+
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);
+ g[0]=node_->get_parameter("gravity.x").get_value<double>();
+ g[1]=node_->get_parameter("gravity.y").get_value<double>();
+ g[2]=node_->get_parameter("gravity.x").get_value<double>();
if((idsolver_=new KDL::ChainIdSolver_RNE(chain_,g)) == NULL)
{
- ROS_ERROR("Failed to create ChainIDSolver_RNE.");
- return false;
+ RCLCPP_ERROR(node_->get_logger(),"Failed to create ChainIDSolver_RNE.");
+ return CallbackReturn::ERROR;
}
q_.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))
+
+ std::vector<double> KpVec=node_->get_parameter("Kp").as_double_array();
+ if(KpVec.empty())
{
- ROS_ERROR("No 'Kp' in controller %s.",node_.getNamespace().c_str());
- return false;
+ RCLCPP_ERROR(node_->get_logger(),"No 'Kp' in controller %s.",node_->get_namespace());
+ return CallbackReturn::ERROR;
}
Kp_=Eigen::Map<Eigen::MatrixXd>(KpVec.data(),nJoints_,nJoints_).transpose();
-
- std::vector<double> KdVec;
- if(!node_.getParam("Kd",KdVec))
+
+ std::vector<double> KdVec=node_->get_parameter("Kd").as_double_array();
+ if(KdVec.empty())
{
- ROS_ERROR("No 'Kd' in controller %s.",node_.getNamespace().c_str());
- return false;
+ RCLCPP_ERROR(node_->get_logger(),"No 'Kd' in controller %s.",node_->get_namespace());
+ return CallbackReturn::ERROR;
}
Kd_=Eigen::Map<Eigen::MatrixXd>(KdVec.data(),nJoints_,nJoints_).transpose();
-
- return true;
+
+ if(!node_->get_parameter("priority",priority_))
+ RCLCPP_WARN(node_->get_logger(),"No 'priority' configured for controller %s. Using highest possible priority.",node_->get_namespace());
+
+ return CallbackReturn::SUCCESS;
}
-
- void ComputedTorqueController::starting(const ros::Time& time)
+
+ controller_interface::InterfaceConfiguration ComputedTorqueController::command_interface_configuration(void) const
{
- for(unsigned int i=0;i < nJoints_;i++)
+ controller_interface::InterfaceConfiguration config;
+ config.type=controller_interface::interface_configuration_type::INDIVIDUAL;
+
+ for(const auto &joint : jointNames_)
+ config.names.push_back(joint + "/" + hardware_interface::HW_IF_EFFORT);
+
+ return config;
+ }
+
+ controller_interface::InterfaceConfiguration ComputedTorqueController::state_interface_configuration(void) const
+ {
+ controller_interface::InterfaceConfiguration config;
+ config.type=controller_interface::interface_configuration_type::INDIVIDUAL;
+
+ for(const auto &joint : jointNames_)
{
- q_(i)=joints_[i].getPosition();
- dq_(i)=joints_[i].getVelocity();
+ config.names.push_back(joint + "/" + hardware_interface::HW_IF_POSITION);
+ config.names.push_back(joint + "/" + hardware_interface::HW_IF_VELOCITY);
+ }
+
+ return config;
+ }
+
+ CallbackReturn ComputedTorqueController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
+ {
+ for(int i=0;i < nJoints_;i++)
+ {
+ q_(i)=state_interfaces_[2*i].get_value();
+ dq_(i)=state_interfaces_[2*i+1].get_value();
}
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);
- }
+ param.sched_priority=priority_;
if(sched_setscheduler(0,SCHED_FIFO,¶m) == -1)
{
- ROS_WARN("Failed to set real-time scheduler.");
- return;
+ RCLCPP_WARN(node_->get_logger(),"Failed to set real-time scheduler.");
+ return CallbackReturn::SUCCESS;
}
if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1)
- ROS_WARN("Failed to lock memory.");
+ RCLCPP_WARN(node_->get_logger(),"Failed to lock memory.");
+
+ return CallbackReturn::SUCCESS;
}
-
- void ComputedTorqueController::update(const ros::Time& time,
- const ros::Duration& duration)
+
+ CallbackReturn ComputedTorqueController::on_deactivate(const rclcpp_lifecycle::State &/*previous_state*/)
+ {
+ for(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();
+
+ 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++)
+ command_interfaces_[i].set_value(torque_(i));
+
+ return CallbackReturn::SUCCESS;
+ }
+
+ controller_interface::return_type ComputedTorqueController::update(void)
{
- for(unsigned int i=0;i < nJoints_;i++)
+ for(int i=0;i < nJoints_;i++)
{
- q_(i)=joints_[i].getPosition();
- dq_(i)=joints_[i].getVelocity();
+ 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();
-
+
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));
+ 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++)
+ command_interfaces_[i].set_value(torque_(i));
+
+ return controller_interface::return_type::OK;
}
-
- void ComputedTorqueController::commandCB(const trajectory_msgs::
- JointTrajectoryPoint::ConstPtr &referencePoint)
+
+ void ComputedTorqueController::commandCB(const trajectory_msgs::msg::JointTrajectoryPoint::SharedPtr referencePoint)
{
- for(unsigned int i=0;i < nJoints_;i++)
+ for(int i=0;i < nJoints_;i++)
{
qr_(i)=referencePoint->positions[i];
dqr_(i)=referencePoint->velocities[i];
ddqr_(i)=referencePoint->accelerations[i];
}
}
+
+ void ComputedTorqueController::robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription)
+ {
+ robotDescription_=robotDescription->data;
+ }
}
-PLUGINLIB_EXPORT_CLASS(effort_controllers::ComputedTorqueController,
- controller_interface::ControllerBase)
+PLUGINLIB_EXPORT_CLASS(effort_controllers::ComputedTorqueController,controller_interface::ControllerInterface)
projects / computed_torque_controller.git / commitdiff