From dc40dfe879602192ab8677b672163c9b86d6cf56 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Sat, 27 Jun 2020 10:48:34 -0700 Subject: [PATCH 01/12] Added MoveUntilTouchCartVelController --- CMakeLists.txt | 3 +- .../MoveUntilTouchCartVelocityController.hpp | 110 ++++++++++ include/rewd_controllers/helpers.hpp | 3 + src/MoveUntilTouchCartVelocityController.cpp | 194 ++++++++++++++++++ 4 files changed, 309 insertions(+), 1 deletion(-) create mode 100644 include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp create mode 100644 src/MoveUntilTouchCartVelocityController.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 75553af..604beab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(rewd_controllers) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") include_directories("include") @@ -66,6 +66,7 @@ add_library("${PROJECT_NAME}" SHARED src/MoveUntilTouchController.cpp src/MoveUntilTouchTopicController.cpp src/FTThresholdClient.cpp + src/MoveUntilTouchCartVelocityController.cpp ) target_link_libraries("${PROJECT_NAME}" ${DART_LIBRARIES} diff --git a/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp b/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp new file mode 100644 index 0000000..4d5cee2 --- /dev/null +++ b/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp @@ -0,0 +1,110 @@ +#ifndef REWD_CONTROLLERS_MOVEUNTILTOUCHCARTVELOCITY_HPP_ +#define REWD_CONTROLLERS_MOVEUNTILTOUCHCARTVELOCITY_HPP_ + +#include +#include +#include +#include +#include +#include "helpers.hpp" +#include +#include + +#include +#include + +namespace rewd_controllers +{ +class MoveUntilTouchCartVelocityController + : public controller_interface:: + MultiInterfaceController +{ +public: + MoveUntilTouchCartVelocityController(); + virtual ~MoveUntilTouchCartVelocityController(); + + /** \brief The init function is called to initialize the controller from a + * non-realtime thread with a pointer to the hardware interface, itself, + * instead of a pointer to a RobotHW. + * + * \param robot The specific hardware interface used by this controller. + * + * \param n A NodeHandle in the namespace from which the controller + * should read its configuration, and where it should set up its ROS + * interface. + * + * \returns True if initialization was successful and the controller + * is ready to be started. + */ + bool init(hardware_interface::RobotHW* robot, ros::NodeHandle& n); + + /*! + * \brief Issues commands to the joint. Should be called at regular intervals + */ + void update(const ros::Time& time, const ros::Duration& period); + + /** \brief This is called from within the realtime thread just before the + * first call to \ref update + * + * \param time The current time + */ + void starting(const ros::Time& time) override; + void stopping(const ros::Time& time) override; + +protected: + using Action = pr_control_msgs::SetCartesianVelocityAction; + using ActionServer = actionlib::ActionServer; + using GoalHandle = ActionServer::GoalHandle; + + using Result = pr_control_msgs::SetCartesianVelocityResult; + + using JointModes = hardware_interface::JointCommandModes; + +private: + /** \brief Actionlib callback to accept new Goal. + */ + void goalCallback(GoalHandle goalHandle); + + /** \brief Actionlib callback to cancel previously accepted goal. + */ + void cancelCallback(GoalHandle goalHandle); + + /** \brief Contains all data needed to execute the currently + * requested velocity. Shared between real time and non-real + * time threads. + * + * This structure must be used AS A WHOLE, and reassigned + * in its entirety. Assigning to individual components + * is UNSAFE with the exception of the mCompleted atomic. + */ + struct CartVelContext { + ros::Time mStartTime; + ros::Duration mDuration; + Eigen::VectorXd mDesiredVelocity; + GoalHandle mGoalHandle; + std::atomic_bool mCompleted; + }; + using CartVelContextPtr = std::shared_ptr; + + // For position feedback: + dart::dynamics::SkeletonPtr mSkeleton; + std::unique_ptr mSkeletonUpdater; + + // Action server variables + std::unique_ptr mActionServer; + + // TODO: It would be better to use std::atomic> here. + // However, this will not be implemented until C++20. + realtime_tools::RealtimeBox mCurrentCartVel; + + // For communication with robot + pr_hardware_interfaces::CartesianVelocityHandle mCartVelHandle; + hardware_interface::JointModeHandle mJointModeHandle; + JointModes lastMode; +}; + +} // namespace + +#endif diff --git a/include/rewd_controllers/helpers.hpp b/include/rewd_controllers/helpers.hpp index 7ccdb82..f9ebbaa 100644 --- a/include/rewd_controllers/helpers.hpp +++ b/include/rewd_controllers/helpers.hpp @@ -7,11 +7,14 @@ #include #include #include +#include #include #include #include "JointAdapter.hpp" #include "JointAdapterFactory.hpp" +#include + namespace rewd_controllers { //============================================================================= diff --git a/src/MoveUntilTouchCartVelocityController.cpp b/src/MoveUntilTouchCartVelocityController.cpp new file mode 100644 index 0000000..2f8f0ba --- /dev/null +++ b/src/MoveUntilTouchCartVelocityController.cpp @@ -0,0 +1,194 @@ +#include + +#include +#include +#include +#include + +#include +#include + +#define SE3_SIZE 6 + +namespace rewd_controllers +{ +namespace +{ +//============================================================================= +std::vector toVector(const Eigen::VectorXd& input) +{ + return std::vector{input.data(), input.data() + input.size()}; +} + +} // namespace + +//============================================================================= +MoveUntilTouchCartVelocityController::MoveUntilTouchCartVelocityController() + : MultiInterfaceController(true) // allow_optional_interfaces +{} + +//============================================================================= +MoveUntilTouchCartVelocityController::~MoveUntilTouchCartVelocityController() {} + +//============================================================================= +bool MoveUntilTouchCartVelocityController::init(hardware_interface::RobotHW *robot, + ros::NodeHandle &n) +{ + using hardware_interface::JointStateInterface; + using hardware_interface::JointModeInterface; + using pr_hardware_interfaces::CartesianVelocityInterface; + + // Load the URDF as a Skeleton. + mSkeleton = loadRobotFromParameter(n, "robot_description_parameter"); + if (!mSkeleton) return false; + + // Have skeleton update from joint state interface + const auto jointStateInterface = robot->get(); + if (!jointStateInterface) { + ROS_ERROR("Unable to get JointStateInterface from RobotHW instance."); + return false; + } + + mSkeletonUpdater.reset( + new SkeletonJointStateUpdater{mSkeleton, jointStateInterface}); + + + // Load control interfaces and handles + const auto jointModeInterface = robot->get(); + if (!jointModeInterface) { + ROS_ERROR("Unable to get JointModeInterface from RobotHW instance."); + return false; + } + + try { + mJointModeHandle = jointModeInterface->getHandle("joint_mode"); + } catch (const hardware_interface::HardwareInterfaceException &e) { + ROS_ERROR_STREAM("Unable to get joint mode interface for robot"); + return false; + } + + const auto cartVelInterface = robot->get(); + if (!cartVelInterface) { + ROS_ERROR("Unable to get CartesianVelocityInterface from RobotHW instance."); + return false; + } + + try { + mCartVelHandle = cartVelInterface->getHandle("cart_vel"); + } catch (const hardware_interface::HardwareInterfaceException &e) { + ROS_ERROR_STREAM("Unable to get cartesian velocity interface for robot."); + return false; + } + + // init local vars + mCurrentCartVel.set(nullptr); + + // TODO: Initialize FT sensor + + // Start the action server. This must be last. + using std::placeholders::_1; + mActionServer.reset(new actionlib::ActionServer{ + n, "cart_velocity", + std::bind(&MoveUntilTouchCartVelocityController::goalCallback, this, _1), + std::bind(&MoveUntilTouchCartVelocityController::cancelCallback, this, _1), + false}); + mActionServer->start(); + + ROS_INFO("MoveUntilTouchCartVelocityController initialized successfully"); + return true; +} + +//============================================================================= +void MoveUntilTouchCartVelocityController::starting(const ros::Time& time) +{ + // Set Joint Mode to Other + lastMode = mJointModeHandle.getMode(); + mJointModeHandle.setMode(JointModes::NOMODE); +} + +//============================================================================= +void MoveUntilTouchCartVelocityController::stopping(const ros::Time& time) +{ + // Return joint mode to what it was before + mJointModeHandle.setMode(lastMode); +} + +//============================================================================= +void MoveUntilTouchCartVelocityController::update(const ros::Time &time, + const ros::Duration &period) +{ + mSkeletonUpdater->update(); + + Eigen::VectorXd setVelocity; + setVelocity.resize(SE3_SIZE); + setVelocity.setZero(); + + // Load current command + std::shared_ptr context; + mCurrentCartVel.get(context); + + if (context && !context->mCompleted.load()) + { + // check duration + ros::Duration runTime = time - context->mStartTime; + if (runTime >= context->mDuration) { + // Successful run + Result result; + result.error_code = Result::SUCCESSFUL; + result.error_string = ""; + context->mGoalHandle.setSucceeded(result); + context->mCompleted.store(true); + } + // TODO: check FT sensor + else { + setVelocity = context->mDesiredVelocity; + } + } + + // Execute velocity command + mCartVelHandle.setCommand(toVector(setVelocity)); +} + +//============================================================================= +void MoveUntilTouchCartVelocityController::goalCallback(GoalHandle goalHandle) +{ + const auto goal = goalHandle.getGoal(); + ROS_INFO_STREAM("Received cartesian velocity '" + << goalHandle.getGoalID().id << "'."); + + // Setup the new trajectory. + const auto newContext = std::make_shared(); + newContext->mStartTime = ros::Time::now(); + newContext->mGoalHandle = goalHandle; + newContext->mDuration = goal->exec_time; + newContext->mDesiredVelocity.resize(SE3_SIZE); + newContext->mCompleted.store(false); + + // Copy over velocity data + newContext->mDesiredVelocity(0) = goal->command.linear.x; + newContext->mDesiredVelocity(1) = goal->command.linear.y; + newContext->mDesiredVelocity(2) = goal->command.linear.z; + newContext->mDesiredVelocity(3) = goal->command.angular.x; + newContext->mDesiredVelocity(4) = goal->command.angular.y; + newContext->mDesiredVelocity(5) = goal->command.angular.z; + + // TODO: add velocity checks + + // We've accepted the goal + newContext->mGoalHandle.setAccepted(); + mCurrentCartVel.set(newContext); +} + +//============================================================================= +void MoveUntilTouchCartVelocityController::cancelCallback(GoalHandle goalHandle) +{ + ROS_INFO_STREAM("Requesting cancelation of velocity '" + << goalHandle.getGoalID().id << "'."); + goalHandle.setAccepted(); + mCurrentCartVel.set(nullptr); +} + +} // namespace rewd_controllers + +PLUGINLIB_EXPORT_CLASS(rewd_controllers::MoveUntilTouchCartVelocityController, + controller_interface::ControllerBase) From 8bf30f3344db3e61545a3f7c0685bf994e576fdc Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Sat, 27 Jun 2020 10:48:34 -0700 Subject: [PATCH 02/12] Added MoveUntilTouchCartVelController --- CMakeLists.txt | 3 +- .../MoveUntilTouchCartVelocityController.hpp | 110 ++++++++++ include/rewd_controllers/helpers.hpp | 3 + src/MoveUntilTouchCartVelocityController.cpp | 194 ++++++++++++++++++ 4 files changed, 309 insertions(+), 1 deletion(-) create mode 100644 include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp create mode 100644 src/MoveUntilTouchCartVelocityController.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 75553af..604beab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(rewd_controllers) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") include_directories("include") @@ -66,6 +66,7 @@ add_library("${PROJECT_NAME}" SHARED src/MoveUntilTouchController.cpp src/MoveUntilTouchTopicController.cpp src/FTThresholdClient.cpp + src/MoveUntilTouchCartVelocityController.cpp ) target_link_libraries("${PROJECT_NAME}" ${DART_LIBRARIES} diff --git a/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp b/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp new file mode 100644 index 0000000..4d5cee2 --- /dev/null +++ b/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp @@ -0,0 +1,110 @@ +#ifndef REWD_CONTROLLERS_MOVEUNTILTOUCHCARTVELOCITY_HPP_ +#define REWD_CONTROLLERS_MOVEUNTILTOUCHCARTVELOCITY_HPP_ + +#include +#include +#include +#include +#include +#include "helpers.hpp" +#include +#include + +#include +#include + +namespace rewd_controllers +{ +class MoveUntilTouchCartVelocityController + : public controller_interface:: + MultiInterfaceController +{ +public: + MoveUntilTouchCartVelocityController(); + virtual ~MoveUntilTouchCartVelocityController(); + + /** \brief The init function is called to initialize the controller from a + * non-realtime thread with a pointer to the hardware interface, itself, + * instead of a pointer to a RobotHW. + * + * \param robot The specific hardware interface used by this controller. + * + * \param n A NodeHandle in the namespace from which the controller + * should read its configuration, and where it should set up its ROS + * interface. + * + * \returns True if initialization was successful and the controller + * is ready to be started. + */ + bool init(hardware_interface::RobotHW* robot, ros::NodeHandle& n); + + /*! + * \brief Issues commands to the joint. Should be called at regular intervals + */ + void update(const ros::Time& time, const ros::Duration& period); + + /** \brief This is called from within the realtime thread just before the + * first call to \ref update + * + * \param time The current time + */ + void starting(const ros::Time& time) override; + void stopping(const ros::Time& time) override; + +protected: + using Action = pr_control_msgs::SetCartesianVelocityAction; + using ActionServer = actionlib::ActionServer; + using GoalHandle = ActionServer::GoalHandle; + + using Result = pr_control_msgs::SetCartesianVelocityResult; + + using JointModes = hardware_interface::JointCommandModes; + +private: + /** \brief Actionlib callback to accept new Goal. + */ + void goalCallback(GoalHandle goalHandle); + + /** \brief Actionlib callback to cancel previously accepted goal. + */ + void cancelCallback(GoalHandle goalHandle); + + /** \brief Contains all data needed to execute the currently + * requested velocity. Shared between real time and non-real + * time threads. + * + * This structure must be used AS A WHOLE, and reassigned + * in its entirety. Assigning to individual components + * is UNSAFE with the exception of the mCompleted atomic. + */ + struct CartVelContext { + ros::Time mStartTime; + ros::Duration mDuration; + Eigen::VectorXd mDesiredVelocity; + GoalHandle mGoalHandle; + std::atomic_bool mCompleted; + }; + using CartVelContextPtr = std::shared_ptr; + + // For position feedback: + dart::dynamics::SkeletonPtr mSkeleton; + std::unique_ptr mSkeletonUpdater; + + // Action server variables + std::unique_ptr mActionServer; + + // TODO: It would be better to use std::atomic> here. + // However, this will not be implemented until C++20. + realtime_tools::RealtimeBox mCurrentCartVel; + + // For communication with robot + pr_hardware_interfaces::CartesianVelocityHandle mCartVelHandle; + hardware_interface::JointModeHandle mJointModeHandle; + JointModes lastMode; +}; + +} // namespace + +#endif diff --git a/include/rewd_controllers/helpers.hpp b/include/rewd_controllers/helpers.hpp index 7ccdb82..f9ebbaa 100644 --- a/include/rewd_controllers/helpers.hpp +++ b/include/rewd_controllers/helpers.hpp @@ -7,11 +7,14 @@ #include #include #include +#include #include #include #include "JointAdapter.hpp" #include "JointAdapterFactory.hpp" +#include + namespace rewd_controllers { //============================================================================= diff --git a/src/MoveUntilTouchCartVelocityController.cpp b/src/MoveUntilTouchCartVelocityController.cpp new file mode 100644 index 0000000..2f8f0ba --- /dev/null +++ b/src/MoveUntilTouchCartVelocityController.cpp @@ -0,0 +1,194 @@ +#include + +#include +#include +#include +#include + +#include +#include + +#define SE3_SIZE 6 + +namespace rewd_controllers +{ +namespace +{ +//============================================================================= +std::vector toVector(const Eigen::VectorXd& input) +{ + return std::vector{input.data(), input.data() + input.size()}; +} + +} // namespace + +//============================================================================= +MoveUntilTouchCartVelocityController::MoveUntilTouchCartVelocityController() + : MultiInterfaceController(true) // allow_optional_interfaces +{} + +//============================================================================= +MoveUntilTouchCartVelocityController::~MoveUntilTouchCartVelocityController() {} + +//============================================================================= +bool MoveUntilTouchCartVelocityController::init(hardware_interface::RobotHW *robot, + ros::NodeHandle &n) +{ + using hardware_interface::JointStateInterface; + using hardware_interface::JointModeInterface; + using pr_hardware_interfaces::CartesianVelocityInterface; + + // Load the URDF as a Skeleton. + mSkeleton = loadRobotFromParameter(n, "robot_description_parameter"); + if (!mSkeleton) return false; + + // Have skeleton update from joint state interface + const auto jointStateInterface = robot->get(); + if (!jointStateInterface) { + ROS_ERROR("Unable to get JointStateInterface from RobotHW instance."); + return false; + } + + mSkeletonUpdater.reset( + new SkeletonJointStateUpdater{mSkeleton, jointStateInterface}); + + + // Load control interfaces and handles + const auto jointModeInterface = robot->get(); + if (!jointModeInterface) { + ROS_ERROR("Unable to get JointModeInterface from RobotHW instance."); + return false; + } + + try { + mJointModeHandle = jointModeInterface->getHandle("joint_mode"); + } catch (const hardware_interface::HardwareInterfaceException &e) { + ROS_ERROR_STREAM("Unable to get joint mode interface for robot"); + return false; + } + + const auto cartVelInterface = robot->get(); + if (!cartVelInterface) { + ROS_ERROR("Unable to get CartesianVelocityInterface from RobotHW instance."); + return false; + } + + try { + mCartVelHandle = cartVelInterface->getHandle("cart_vel"); + } catch (const hardware_interface::HardwareInterfaceException &e) { + ROS_ERROR_STREAM("Unable to get cartesian velocity interface for robot."); + return false; + } + + // init local vars + mCurrentCartVel.set(nullptr); + + // TODO: Initialize FT sensor + + // Start the action server. This must be last. + using std::placeholders::_1; + mActionServer.reset(new actionlib::ActionServer{ + n, "cart_velocity", + std::bind(&MoveUntilTouchCartVelocityController::goalCallback, this, _1), + std::bind(&MoveUntilTouchCartVelocityController::cancelCallback, this, _1), + false}); + mActionServer->start(); + + ROS_INFO("MoveUntilTouchCartVelocityController initialized successfully"); + return true; +} + +//============================================================================= +void MoveUntilTouchCartVelocityController::starting(const ros::Time& time) +{ + // Set Joint Mode to Other + lastMode = mJointModeHandle.getMode(); + mJointModeHandle.setMode(JointModes::NOMODE); +} + +//============================================================================= +void MoveUntilTouchCartVelocityController::stopping(const ros::Time& time) +{ + // Return joint mode to what it was before + mJointModeHandle.setMode(lastMode); +} + +//============================================================================= +void MoveUntilTouchCartVelocityController::update(const ros::Time &time, + const ros::Duration &period) +{ + mSkeletonUpdater->update(); + + Eigen::VectorXd setVelocity; + setVelocity.resize(SE3_SIZE); + setVelocity.setZero(); + + // Load current command + std::shared_ptr context; + mCurrentCartVel.get(context); + + if (context && !context->mCompleted.load()) + { + // check duration + ros::Duration runTime = time - context->mStartTime; + if (runTime >= context->mDuration) { + // Successful run + Result result; + result.error_code = Result::SUCCESSFUL; + result.error_string = ""; + context->mGoalHandle.setSucceeded(result); + context->mCompleted.store(true); + } + // TODO: check FT sensor + else { + setVelocity = context->mDesiredVelocity; + } + } + + // Execute velocity command + mCartVelHandle.setCommand(toVector(setVelocity)); +} + +//============================================================================= +void MoveUntilTouchCartVelocityController::goalCallback(GoalHandle goalHandle) +{ + const auto goal = goalHandle.getGoal(); + ROS_INFO_STREAM("Received cartesian velocity '" + << goalHandle.getGoalID().id << "'."); + + // Setup the new trajectory. + const auto newContext = std::make_shared(); + newContext->mStartTime = ros::Time::now(); + newContext->mGoalHandle = goalHandle; + newContext->mDuration = goal->exec_time; + newContext->mDesiredVelocity.resize(SE3_SIZE); + newContext->mCompleted.store(false); + + // Copy over velocity data + newContext->mDesiredVelocity(0) = goal->command.linear.x; + newContext->mDesiredVelocity(1) = goal->command.linear.y; + newContext->mDesiredVelocity(2) = goal->command.linear.z; + newContext->mDesiredVelocity(3) = goal->command.angular.x; + newContext->mDesiredVelocity(4) = goal->command.angular.y; + newContext->mDesiredVelocity(5) = goal->command.angular.z; + + // TODO: add velocity checks + + // We've accepted the goal + newContext->mGoalHandle.setAccepted(); + mCurrentCartVel.set(newContext); +} + +//============================================================================= +void MoveUntilTouchCartVelocityController::cancelCallback(GoalHandle goalHandle) +{ + ROS_INFO_STREAM("Requesting cancelation of velocity '" + << goalHandle.getGoalID().id << "'."); + goalHandle.setAccepted(); + mCurrentCartVel.set(nullptr); +} + +} // namespace rewd_controllers + +PLUGINLIB_EXPORT_CLASS(rewd_controllers::MoveUntilTouchCartVelocityController, + controller_interface::ControllerBase) From a3befb84ba7c89c1b852ef8544dcd832a0bc558b Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Sat, 27 Jun 2020 10:59:06 -0700 Subject: [PATCH 03/12] Added clang-format --- .gitignore | 3 +- CMakeLists.txt | 19 +++++++++ cmake/ClangFormat.cmake | 90 +++++++++++++++++++++++++++++++++++++++++ 3 files changed, 111 insertions(+), 1 deletion(-) create mode 100644 cmake/ClangFormat.cmake diff --git a/.gitignore b/.gitignore index 8e695ec..0ab36af 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ -doc +doc/ +build/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 75553af..9de034c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -73,6 +73,25 @@ target_link_libraries("${PROJECT_NAME}" ${catkin_LIBRARIES} ) +#============================================================================== +# Format +# + +list(INSERT CMAKE_MODULE_PATH 0 "${PROJECT_SOURCE_DIR}/cmake") +include(ClangFormat) + +clang_format_setup(VERSION 6.0) + +if (CLANG_FORMAT_EXECUTABLE) + file(GLOB_RECURSE ALL_SOURCE_FILES + LIST_DIRECTORIES false + include/*.h include/*.hpp src/*.c src/*.cpp tests/*.cpp tests/*.hpp) + + clang_format_add_sources(${ALL_SOURCE_FILES}) + + clang_format_add_targets() +endif() + #============================================================================== # Installation # diff --git a/cmake/ClangFormat.cmake b/cmake/ClangFormat.cmake new file mode 100644 index 0000000..a1338e6 --- /dev/null +++ b/cmake/ClangFormat.cmake @@ -0,0 +1,90 @@ +function(clang_format_setup) + cmake_parse_arguments( + CF_ARG # prefix + "" # no boolean args + "VERSION" # clang-format version + "" # no multi-value args + ${ARGN} + ) + if(NOT CF_ARG_VERSION) + set(CF_NAME clang-format) + else() + set(CF_NAME clang-format-${CF_ARG_VERSION}) + endif() + find_program(CLANG_FORMAT_EXECUTABLE NAMES ${CF_NAME}) + + if(NOT CLANG_FORMAT_EXECUTABLE) + message(STATUS "Looking for clang-format - NOT found, please install " + "${CF_NAME} to enable automatic code formatting." + ) + return() + endif() + + message(STATUS "Found ${CF_NAME}.") +endfunction() + +#=============================================================================== +function(_property_add property_name) + get_property(is_defined GLOBAL PROPERTY ${property_name} DEFINED) + if(NOT is_defined) + define_property(GLOBAL PROPERTY ${property_name} + BRIEF_DOCS "${property_name}" + FULL_DOCS "Global properties for ${property_name}" + ) + endif() + foreach(item ${ARGN}) + set_property(GLOBAL APPEND PROPERTY ${property_name} "${item}") + endforeach() +endfunction() + +#=============================================================================== +function(clang_format_add_sources) + foreach(source ${ARGN}) + if(IS_ABSOLUTE "${source}") + set(source_abs "${source}") + else() + get_filename_component(source_abs + "${CMAKE_CURRENT_LIST_DIR}/${source}" ABSOLUTE) + endif() + if(EXISTS "${source_abs}") + _property_add(CLANG_FORMAT_FORMAT_FILES "${source_abs}") + else() + message(FATAL_ERROR + "Source file '${source}' does not exist at absolute path" + " '${source_abs}'. This should never happen. Did you recently delete" + " this file or modify 'CMAKE_CURRENT_LIST_DIR'") + endif() + endforeach() +endfunction() + +#=============================================================================== +function(clang_format_add_targets) + get_property(formatting_files GLOBAL PROPERTY CLANG_FORMAT_FORMAT_FILES) + list(LENGTH formatting_files formatting_files_length) + + if(formatting_files) + message(STATUS "Formatting on ${formatting_files_length} source files.") + + add_custom_target(format + COMMAND ${CMAKE_COMMAND} -E echo "Formatting ${formatting_files_length} files..." + COMMAND ${CLANG_FORMAT_EXECUTABLE} -style=file -i ${formatting_files} + COMMAND ${CMAKE_COMMAND} -E echo "Done." + DEPENDS ${CLANG_FORMAT_EXECUTABLE} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + ) + add_custom_target(check-format + COMMAND ${CMAKE_COMMAND} -E echo "Checking ${formatting_files_length} files..." + COMMAND ${CMAKE_SOURCE_DIR}/tools/check_format.sh ${CLANG_FORMAT_EXECUTABLE} ${formatting_files} + COMMAND ${CMAKE_COMMAND} -E echo "Done." + DEPENDS ${CLANG_FORMAT_EXECUTABLE} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + ) + else() + add_custom_target(format + COMMAND ${CMAKE_COMMAND} -E echo "No file to format code style." + ) + add_custom_target(check-format + COMMAND ${CMAKE_COMMAND} -E echo "No file to check code style." + ) + endif() +endfunction() From 1fe127892530e81cbc4866a90e232121d8fb2fc8 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Sat, 27 Jun 2020 10:59:46 -0700 Subject: [PATCH 04/12] Ran clang-format-6.0 on all sources --- .../rewd_controllers/FTThresholdClient.hpp | 19 +- .../GravityCompensationController.hpp | 20 +- include/rewd_controllers/JointAdapter.hpp | 72 ++--- .../rewd_controllers/JointAdapterFactory.hpp | 27 +- .../JointGroupPositionController.hpp | 27 +- .../JointTrajectoryController.hpp | 31 +- .../JointTrajectoryControllerBase.hpp | 46 +-- .../MoveUntilTouchController.hpp | 48 ++- .../MoveUntilTouchTopicController.hpp | 53 ++-- .../MultiInterfaceController.hpp | 292 +++++++++--------- .../detail/JointAdapterFactory-impl.hpp | 52 ++-- include/rewd_controllers/helpers.hpp | 52 ++-- src/FTThresholdClient.cpp | 48 ++- src/GravityCompensationController.cpp | 28 +- src/JointAdapter.cpp | 124 +++----- src/JointAdapterFactory.cpp | 27 +- src/JointGroupPositionController.cpp | 101 +++--- src/JointTrajectoryController.cpp | 30 +- src/JointTrajectoryControllerBase.cpp | 192 ++++++------ src/MoveUntilTouchController.cpp | 52 ++-- src/MoveUntilTouchTopicController.cpp | 89 +++--- src/helpers.cpp | 169 +++++----- 22 files changed, 741 insertions(+), 858 deletions(-) diff --git a/include/rewd_controllers/FTThresholdClient.hpp b/include/rewd_controllers/FTThresholdClient.hpp index 66c93a2..5fef97d 100644 --- a/include/rewd_controllers/FTThresholdClient.hpp +++ b/include/rewd_controllers/FTThresholdClient.hpp @@ -10,29 +10,28 @@ namespace rewd_controllers { /// The FTThresholdClient configures the MoveUntilTouch(Topic)Controller's /// thresholds. /// When those thresholds are exceeded, the controller stops the movement. -class FTThresholdClient -{ +class FTThresholdClient { public: /// Constructor. - FTThresholdClient(const std::string& controllerThresholdTopic); + FTThresholdClient(const std::string &controllerThresholdTopic); /// Sets the MoveUntilTouchControllers Thresholds. /// Blocks until the threshold could be set successfully. /// Can be aborted with !ros::ok(), in which case it returns false - bool trySetThresholdsRepeatedly(double forceThreshold, double torqueThreshold); + bool trySetThresholdsRepeatedly(double forceThreshold, + double torqueThreshold); /// Sets the MoveUntilTouchControllers thresholds. /// Returns true if the thresholds were set successfully. - bool setThresholds(double forceThreshold, double torqueThreshold, double timeout = 3.0); + bool setThresholds(double forceThreshold, double torqueThreshold, + double timeout = 3.0); private: - - std::unique_ptr> + std::unique_ptr> mFTThresholdActionClient; }; -} +} // namespace rewd_controllers #endif diff --git a/include/rewd_controllers/GravityCompensationController.hpp b/include/rewd_controllers/GravityCompensationController.hpp index a6a8577..719a7e5 100644 --- a/include/rewd_controllers/GravityCompensationController.hpp +++ b/include/rewd_controllers/GravityCompensationController.hpp @@ -1,19 +1,17 @@ #ifndef REWD_CONTROLLERS__GRAVITY_COMPENSATION_CONTROLLER_H #define REWD_CONTROLLERS__GRAVITY_COMPENSATION_CONTROLLER_H -#include +#include "helpers.hpp" #include #include +#include #include -#include "helpers.hpp" -namespace rewd_controllers -{ +namespace rewd_controllers { class GravityCompensationController - : public controller_interface:: - MultiInterfaceController -{ + : public controller_interface::MultiInterfaceController< + hardware_interface::EffortJointInterface, + hardware_interface::JointStateInterface> { public: GravityCompensationController(); virtual ~GravityCompensationController(); @@ -31,12 +29,12 @@ class GravityCompensationController * \returns True if initialization was successful and the controller * is ready to be started. */ - bool init(hardware_interface::RobotHW* robot, ros::NodeHandle& n); + bool init(hardware_interface::RobotHW *robot, ros::NodeHandle &n); /*! * \brief Issues commands to the joint. Should be called at regular intervals */ - void update(const ros::Time& time, const ros::Duration& period); + void update(const ros::Time &time, const ros::Duration &period); private: JointAdapterFactory mAdapterFactory; @@ -49,6 +47,6 @@ class GravityCompensationController Eigen::VectorXd mCalculatedForces; }; -} // namespace +} // namespace rewd_controllers #endif diff --git a/include/rewd_controllers/JointAdapter.hpp b/include/rewd_controllers/JointAdapter.hpp index 57e3acf..02e6f82 100644 --- a/include/rewd_controllers/JointAdapter.hpp +++ b/include/rewd_controllers/JointAdapter.hpp @@ -1,15 +1,14 @@ #ifndef REWD_CONTROLLERS_JOINTADAPTER_HPP_ #define REWD_CONTROLLERS_JOINTADAPTER_HPP_ -#include +#include #include #include -#include #include +#include namespace rewd_controllers { -class JointAdapter -{ +class JointAdapter { public: JointAdapter(); @@ -17,13 +16,12 @@ class JointAdapter virtual void setDesiredPosition(double desiredPosition); - virtual bool initialize(const ros::NodeHandle& nodeHandle) = 0; + virtual bool initialize(const ros::NodeHandle &nodeHandle) = 0; - virtual void update( - const ros::Time& time, const ros::Duration& period, - double actualPosition, double desiredPosition, - double actualVelocity, double desiredVelocity, - double nominalEffort) = 0; + virtual void update(const ros::Time &time, const ros::Duration &period, + double actualPosition, double desiredPosition, + double actualVelocity, double desiredVelocity, + double nominalEffort) = 0; virtual void reset() = 0; @@ -32,47 +30,43 @@ class JointAdapter }; //============================================================================= -class JointPositionAdapter : public JointAdapter -{ +class JointPositionAdapter : public JointAdapter { public: JointPositionAdapter(hardware_interface::JointHandle positionHandle, - dart::dynamics::DegreeOfFreedom* dof); + dart::dynamics::DegreeOfFreedom *dof); - bool initialize(const ros::NodeHandle& nodeHandle) override; + bool initialize(const ros::NodeHandle &nodeHandle) override; - void update( - const ros::Time& time, const ros::Duration& period, - double actualPosition, double desiredPosition, - double actualVelocity, double desiredVelocity, - double nominalEffort) override; + void update(const ros::Time &time, const ros::Duration &period, + double actualPosition, double desiredPosition, + double actualVelocity, double desiredVelocity, + double nominalEffort) override; void reset() override; private: hardware_interface::JointHandle mPositionHandle; - dart::dynamics::DegreeOfFreedom* mDof; + dart::dynamics::DegreeOfFreedom *mDof; }; //============================================================================= -class JointVelocityAdapter : public JointAdapter -{ +class JointVelocityAdapter : public JointAdapter { public: JointVelocityAdapter(hardware_interface::JointHandle velocityHandle, - dart::dynamics::DegreeOfFreedom* dof); + dart::dynamics::DegreeOfFreedom *dof); - bool initialize(const ros::NodeHandle& nodeHandle) override; + bool initialize(const ros::NodeHandle &nodeHandle) override; - void update( - const ros::Time& time, const ros::Duration& period, - double actualPosition, double desiredPosition, - double actualVelocity, double desiredVelocity, - double nominalEffort) override; + void update(const ros::Time &time, const ros::Duration &period, + double actualPosition, double desiredPosition, + double actualVelocity, double desiredVelocity, + double nominalEffort) override; void reset() override; private: hardware_interface::JointHandle mVelocityHandle; - dart::dynamics::DegreeOfFreedom* mDof; + dart::dynamics::DegreeOfFreedom *mDof; control_toolbox::Pid mPid; // Storing value from DART on construction @@ -80,25 +74,23 @@ class JointVelocityAdapter : public JointAdapter }; //============================================================================= -class JointEffortAdapter : public JointAdapter -{ +class JointEffortAdapter : public JointAdapter { public: JointEffortAdapter(hardware_interface::JointHandle effortHandle, - dart::dynamics::DegreeOfFreedom* dof); + dart::dynamics::DegreeOfFreedom *dof); - bool initialize(const ros::NodeHandle& nodeHandle) override; + bool initialize(const ros::NodeHandle &nodeHandle) override; - void update( - const ros::Time& time, const ros::Duration& period, - double actualPosition, double desiredPosition, - double actualVelocity, double desiredVelocity, - double nominalEffort) override; + void update(const ros::Time &time, const ros::Duration &period, + double actualPosition, double desiredPosition, + double actualVelocity, double desiredVelocity, + double nominalEffort) override; void reset() override; private: hardware_interface::JointHandle mEffortHandle; - dart::dynamics::DegreeOfFreedom* mDof; + dart::dynamics::DegreeOfFreedom *mDof; control_toolbox::Pid mPid; }; diff --git a/include/rewd_controllers/JointAdapterFactory.hpp b/include/rewd_controllers/JointAdapterFactory.hpp index 743f3fb..0e312f5 100644 --- a/include/rewd_controllers/JointAdapterFactory.hpp +++ b/include/rewd_controllers/JointAdapterFactory.hpp @@ -1,33 +1,32 @@ #ifndef REWD_CONTROLLERS_JOINTADAPTERFACTORY_HPP_ #define REWD_CONTROLLERS_JOINTADAPTERFACTORY_HPP_ +#include "JointAdapter.hpp" +#include +#include #include #include #include -#include -#include -#include "JointAdapter.hpp" namespace rewd_controllers { -class JointAdapterFactory final -{ +class JointAdapterFactory final { public: JointAdapterFactory(); ~JointAdapterFactory(); template - void registerFactory(const std::string& type); + void registerFactory(const std::string &type); - std::unique_ptr create(const std::string& type, - hardware_interface::RobotHW* hardwareInterface, - dart::dynamics::DegreeOfFreedom* dof) const; + std::unique_ptr + create(const std::string &type, + hardware_interface::RobotHW *hardwareInterface, + dart::dynamics::DegreeOfFreedom *dof) const; private: - using FactoryFunction = std::function< - JointAdapter* (hardware_interface::RobotHW*, - dart::dynamics::DegreeOfFreedom*)>; - - hardware_interface::RobotHW* mHardwareInterface; + using FactoryFunction = std::function; + + hardware_interface::RobotHW *mHardwareInterface; std::unordered_map mFactories; }; diff --git a/include/rewd_controllers/JointGroupPositionController.hpp b/include/rewd_controllers/JointGroupPositionController.hpp index 06f9b48..1c4f798 100644 --- a/include/rewd_controllers/JointGroupPositionController.hpp +++ b/include/rewd_controllers/JointGroupPositionController.hpp @@ -1,23 +1,22 @@ #ifndef REWD_CONTROLLERS_JOINTGROUPPOSITIONCONTROLLER_HPP_ #define REWD_CONTROLLERS_JOINTGROUPPOSITIONCONTROLLER_HPP_ -#include +#include "helpers.hpp" #include +#include +#include #include #include #include -#include -#include "helpers.hpp" namespace rewd_controllers { class JointGroupPositionController - : public controller_interface::MultiInterfaceController< - hardware_interface::PositionJointInterface, - hardware_interface::VelocityJointInterface, - hardware_interface::EffortJointInterface, - hardware_interface::JointStateInterface> -{ + : public controller_interface::MultiInterfaceController< + hardware_interface::PositionJointInterface, + hardware_interface::VelocityJointInterface, + hardware_interface::EffortJointInterface, + hardware_interface::JointStateInterface> { public: JointGroupPositionController(); virtual ~JointGroupPositionController(); @@ -35,19 +34,19 @@ class JointGroupPositionController * \returns True if initialization was successful and the controller * is ready to be started. */ - bool init(hardware_interface::RobotHW* robot, ros::NodeHandle& n) override; + bool init(hardware_interface::RobotHW *robot, ros::NodeHandle &n) override; /** \brief This is called from within the realtime thread just before the * first call to \ref update * * \param time The current time */ - void starting(const ros::Time& time) override; + void starting(const ros::Time &time) override; /*! * \brief Issues commands to the joint. Should be called at regular intervals */ - void update(const ros::Time& time, const ros::Duration& period) override; + void update(const ros::Time &time, const ros::Duration &period) override; /*! * \brief Give set position of the joint for next update: revolute (angle) @@ -55,7 +54,7 @@ class JointGroupPositionController * * \param command */ - void setCommand(const sensor_msgs::JointState& msg); + void setCommand(const sensor_msgs::JointState &msg); private: JointAdapterFactory mAdapterFactory; @@ -66,7 +65,7 @@ class JointGroupPositionController realtime_tools::RealtimeBuffer mDesiredPositionBuffer; Eigen::VectorXd mDesiredPosition; - ros::Subscriber mCommandSubscriber; + ros::Subscriber mCommandSubscriber; }; } // namespace rewd_controllers diff --git a/include/rewd_controllers/JointTrajectoryController.hpp b/include/rewd_controllers/JointTrajectoryController.hpp index 66c99c3..c523071 100644 --- a/include/rewd_controllers/JointTrajectoryController.hpp +++ b/include/rewd_controllers/JointTrajectoryController.hpp @@ -2,20 +2,17 @@ #define REWD_CONTROLLERS_JOINTTRAJECTORYCONTROLLER_HPP_ #include -#include #include +#include -namespace rewd_controllers -{ +namespace rewd_controllers { class JointTrajectoryController final - : public MultiInterfaceController, - public JointTrajectoryControllerBase -{ + : public MultiInterfaceController< + hardware_interface::PositionJointInterface, + hardware_interface::VelocityJointInterface, + hardware_interface::EffortJointInterface, + hardware_interface::JointStateInterface>, + public JointTrajectoryControllerBase { public: JointTrajectoryController(); virtual ~JointTrajectoryController(); @@ -33,20 +30,20 @@ class JointTrajectoryController final * \returns True if initialization was successful and the controller * is ready to be started. */ - bool init(hardware_interface::RobotHW* robot, ros::NodeHandle& n) override; + bool init(hardware_interface::RobotHW *robot, ros::NodeHandle &n) override; /** \brief This is called from within the realtime thread just before the * first call to \ref update * * \param time The current time */ - void starting(const ros::Time& time) override; - void stopping(const ros::Time& time) override; + void starting(const ros::Time &time) override; + void stopping(const ros::Time &time) override; /*! * \brief Issues commands to the joint. Should be called at regular intervals */ - void update(const ros::Time& time, const ros::Duration& period) override; + void update(const ros::Time &time, const ros::Duration &period) override; protected: /** \brief The JointTrajectoryControllerBase should accept new trajectories @@ -57,6 +54,6 @@ class JointTrajectoryController final bool shouldAcceptRequests() override; }; -} // namespace rewd_controllers +} // namespace rewd_controllers -#endif // REWD_CONTROLLERS_JOINTTRAJECTORYCONTROLLER_HPP_ +#endif // REWD_CONTROLLERS_JOINTTRAJECTORYCONTROLLER_HPP_ diff --git a/include/rewd_controllers/JointTrajectoryControllerBase.hpp b/include/rewd_controllers/JointTrajectoryControllerBase.hpp index bb4e194..8bd8610 100644 --- a/include/rewd_controllers/JointTrajectoryControllerBase.hpp +++ b/include/rewd_controllers/JointTrajectoryControllerBase.hpp @@ -5,8 +5,8 @@ #include #include #include -#include #include +#include #include #include @@ -19,13 +19,12 @@ #include #include -namespace rewd_controllers -{ +namespace rewd_controllers { -/// The JointTrajectoryControllerBase uses a bunch of ros parameters as configuration. -/// It uses the constraints/joint_name/goal parameters to determine whether it has reached its goal. -class JointTrajectoryControllerBase -{ +/// The JointTrajectoryControllerBase uses a bunch of ros parameters as +/// configuration. It uses the constraints/joint_name/goal parameters to +/// determine whether it has reached its goal. +class JointTrajectoryControllerBase { protected: using Action = control_msgs::FollowJointTrajectoryAction; using ActionServer = actionlib::ActionServer; @@ -50,26 +49,26 @@ class JointTrajectoryControllerBase * \returns True if initialization was successful and the controller * is ready to be started. */ - bool initController(hardware_interface::RobotHW* robot, ros::NodeHandle& n); + bool initController(hardware_interface::RobotHW *robot, ros::NodeHandle &n); /** \brief This is called from within the realtime thread just before the * first call to \ref update * * \param time The current time */ - void startController(const ros::Time& time); + void startController(const ros::Time &time); /** \brief This is called from within the realtime thread just after the * last call to \ref update * * \param time The current time */ - void stopController(const ros::Time& time); + void stopController(const ros::Time &time); /** * \brief Issues commands to the joint. Should be called at regular intervals */ - void updateStep(const ros::Time& time, const ros::Duration& period); + void updateStep(const ros::Time &time, const ros::Duration &period); /** \brief Whether the controller should accept new service requests. */ virtual bool shouldAcceptRequests() = 0; @@ -77,10 +76,11 @@ class JointTrajectoryControllerBase /** * \brief Called every update step in the real-time thread to let * subclassing controllers specify an early termination condition. - * - * \param message If the execution should be stopped, this contains reason for stopping the execution. + * + * \param message If the execution should be stopped, this contains reason for + * stopping the execution. */ - virtual bool shouldStopExecution(std::string& message); + virtual bool shouldStopExecution(std::string &message); private: /** \brief Contains all data needed to execute the currently @@ -110,12 +110,12 @@ class JointTrajectoryControllerBase /** \brief Called by timer to manage new, canceled, and completed trajectory * requests. */ - void nonRealtimeCallback(const ros::TimerEvent& event); + void nonRealtimeCallback(const ros::TimerEvent &event); /** \brief Helper function to publish feedback on currently executing * trajectory in \ref nonRealtimecallback. */ - void publishFeedback(const ros::Time& currentTime); + void publishFeedback(const ros::Time ¤tTime); /** \brief Helper function used to remove a cancel request from the list of * new trajectory requests in \ref nonRealtimecallback. @@ -126,7 +126,7 @@ class JointTrajectoryControllerBase * \returns True if ghToCancel was properly canceled and should be removed * from the queue of cancel requests. */ - bool processCancelRequest(GoalHandle& ghToCancel); + bool processCancelRequest(GoalHandle &ghToCancel); std::unique_ptr mNodeHandle; JointAdapterFactory mAdapterFactory; @@ -158,9 +158,8 @@ class JointTrajectoryControllerBase Eigen::VectorXd mCurrentTrajectoryOffset; /// Cartesian product space equivalent to mControlledSpace - /// but with all R1 joints instead. - std::shared_ptr - mCompoundSpace; + /// but with all R1 joints instead. + std::shared_ptr mCompoundSpace; // TODO: It would be better to use std::atomic> here. // However, this is not fully implemented in GCC 4.8.4, shipped with Ubuntu @@ -170,7 +169,8 @@ class JointTrajectoryControllerBase // Difference between canceling and aborting a trajectory: // If the higher level code stops the trajectory, it is "canceled". // If the controller itself stops the trajectory, it is "aborted". - // This information is reflected in the goal handle and therefore accessible to the higher level code. + // This information is reflected in the goal handle and therefore accessible + // to the higher level code. std::atomic_bool mCancelCurrentTrajectory; std::atomic_bool mAbortCurrentTrajectory; std::string mAbortReason; @@ -180,6 +180,6 @@ class JointTrajectoryControllerBase std::unordered_map mGoalConstraints; }; -} // namespace rewd_controllers +} // namespace rewd_controllers -#endif // ifndef REWD_CONTROLLERS_JOINTTRAJECTORYCONTROLLERBASE_HPP_ +#endif // ifndef REWD_CONTROLLERS_JOINTTRAJECTORYCONTROLLERBASE_HPP_ diff --git a/include/rewd_controllers/MoveUntilTouchController.hpp b/include/rewd_controllers/MoveUntilTouchController.hpp index 89007c5..8db61ad 100644 --- a/include/rewd_controllers/MoveUntilTouchController.hpp +++ b/include/rewd_controllers/MoveUntilTouchController.hpp @@ -1,36 +1,31 @@ #ifndef REWD_CONTROLLERS_MOVEUNTILTOUCHCONTROLLER_HPP_ #define REWD_CONTROLLERS_MOVEUNTILTOUCHCONTROLLER_HPP_ -#include #include +#include #include #include #include #include -#include #include +#include -namespace rewd_controllers -{ +namespace rewd_controllers { class MoveUntilTouchController final - : public MultiInterfaceController, - public JointTrajectoryControllerBase -{ + : public MultiInterfaceController< + hardware_interface::PositionJointInterface, + hardware_interface::VelocityJointInterface, + hardware_interface::EffortJointInterface, + hardware_interface::JointStateInterface, + hardware_interface::ForceTorqueSensorInterface, + pr_hardware_interfaces::TriggerableInterface>, + public JointTrajectoryControllerBase { public: MoveUntilTouchController(); ~MoveUntilTouchController(); // Documentation inherited - bool init(hardware_interface::RobotHW* robot, ros::NodeHandle& n) override; + bool init(hardware_interface::RobotHW *robot, ros::NodeHandle &n) override; /** \brief This is called from within the realtime thread just before the * first call to \ref update. It triggers a Tare on the controlled @@ -38,13 +33,13 @@ class MoveUntilTouchController final * * \param time The current time */ - void starting(const ros::Time& time) override; + void starting(const ros::Time &time) override; // Documentation inherited - void stopping(const ros::Time& time) override; + void stopping(const ros::Time &time) override; // Documentation inherited. - void update(const ros::Time& time, const ros::Duration& period) override; + void update(const ros::Time &time, const ros::Duration &period) override; protected: /** \brief The JointTrajectoryControllerBase should accept new trajectories @@ -58,13 +53,14 @@ class MoveUntilTouchController final * \brief Called from the real-time thread every control cycle, this method * reads the current force/torque sensor state and compares with * tolerances specified by the set_forcetorque_threshold service. - * - * \param message If the execution should be stopped, this contains reason for stopping the execution. - * + * + * \param message If the execution should be stopped, this contains reason for + * stopping the execution. + * * \returns True if current wrench exceeds force/torque threshold. If a * threshold is set to `0.0` (the default), it is ignored. */ - bool shouldStopExecution(std::string& message) override; + bool shouldStopExecution(std::string &message) override; private: using SetFTThresholdAction = pr_control_msgs::SetForceTorqueThresholdAction; @@ -88,6 +84,6 @@ class MoveUntilTouchController final void setForceTorqueThreshold(FTThresholdGoalHandle gh); }; -} // namespace rewd_controllers +} // namespace rewd_controllers -#endif // REWD_CONTROLLERS_MOVEUNTILTOUCHCONTROLLER_HPP_ +#endif // REWD_CONTROLLERS_MOVEUNTILTOUCHCONTROLLER_HPP_ diff --git a/include/rewd_controllers/MoveUntilTouchTopicController.hpp b/include/rewd_controllers/MoveUntilTouchTopicController.hpp index c5db632..97f6a19 100644 --- a/include/rewd_controllers/MoveUntilTouchTopicController.hpp +++ b/include/rewd_controllers/MoveUntilTouchTopicController.hpp @@ -1,23 +1,21 @@ #ifndef REWD_CONTROLLERS_MOVEUNTILTOUCHTOPICCONTROLLER_HPP_ #define REWD_CONTROLLERS_MOVEUNTILTOUCHTOPICCONTROLLER_HPP_ +#include +#include #include -#include #include -#include -#include +#include #include #include +#include #include #include #include -#include -#include #include +#include - -namespace rewd_controllers -{ +namespace rewd_controllers { /// Uses a standard JointTrajectoryControllerBase and aborts the trajectory if /// the forces or torques are too big. @@ -26,21 +24,19 @@ namespace rewd_controllers /// The other difference is, that taring is instigated through an action client /// instead of a hardware interface. class MoveUntilTouchTopicController final - : public MultiInterfaceController, - public JointTrajectoryControllerBase -{ + : public MultiInterfaceController< + hardware_interface::PositionJointInterface, + hardware_interface::VelocityJointInterface, + hardware_interface::EffortJointInterface, + hardware_interface::JointStateInterface>, + public JointTrajectoryControllerBase { public: MoveUntilTouchTopicController(); ~MoveUntilTouchTopicController(); // Documentation inherited - bool init(hardware_interface::RobotHW* robot, ros::NodeHandle& n) override; + bool init(hardware_interface::RobotHW *robot, ros::NodeHandle &n) override; /** \brief This is called from within the realtime thread just before the * first call to \ref update. It triggers a Tare on the controlled @@ -48,13 +44,13 @@ class MoveUntilTouchTopicController final * * \param time The current time */ - void starting(const ros::Time& time) override; + void starting(const ros::Time &time) override; // Documentation inherited - void stopping(const ros::Time& time) override; + void stopping(const ros::Time &time) override; // Documentation inherited. - void update(const ros::Time& time, const ros::Duration& period) override; + void update(const ros::Time &time, const ros::Duration &period) override; protected: /** \brief The JointTrajectoryControllerBase should accept new trajectories @@ -72,15 +68,15 @@ class MoveUntilTouchTopicController final * \returns True if current wrench exceeds force/torque threshold. If a * threshold is set to `0.0` (the default), it is ignored. */ - bool shouldStopExecution(std::string& message) override; + bool shouldStopExecution(std::string &message) override; private: - using SetFTThresholdAction = pr_control_msgs::SetForceTorqueThresholdAction; using FTThresholdActionServer = actionlib::ActionServer; using FTThresholdGoalHandle = FTThresholdActionServer::GoalHandle; using FTThresholdResult = pr_control_msgs::SetForceTorqueThresholdResult; - using TareActionClient = actionlib::ActionClient; + using TareActionClient = + actionlib::ActionClient; // \brief Protects mForce and mTorque from simultaneous access. std::mutex mForceTorqueDataMutex; @@ -119,7 +115,8 @@ class MoveUntilTouchTopicController final std::atomic mTorqueThreshold; // \brief Keeps track of the last update time - std::chrono::time_point mTimeOfLastSensorDataReceived; + std::chrono::time_point + mTimeOfLastSensorDataReceived; /** * \brief Callback for pr_control_msgs::SetForceTorqueThresholdAction. @@ -129,14 +126,14 @@ class MoveUntilTouchTopicController final /** * \brief Called whenever a new Force/Torque message arrives on the ros topic */ - void forceTorqueDataCallback(const geometry_msgs::WrenchStamped& msg); + void forceTorqueDataCallback(const geometry_msgs::WrenchStamped &msg); /** * \brief Called whenever the status of taring changes */ - void taringTransitionCallback(const TareActionClient::GoalHandle& goalHandle); + void taringTransitionCallback(const TareActionClient::GoalHandle &goalHandle); }; -} // namespace rewd_controllers +} // namespace rewd_controllers -#endif // REWD_CONTROLLERS_MOVEUNTILTOUCHTOPICCONTROLLER_HPP_ +#endif // REWD_CONTROLLERS_MOVEUNTILTOUCHTOPICCONTROLLER_HPP_ diff --git a/include/rewd_controllers/MultiInterfaceController.hpp b/include/rewd_controllers/MultiInterfaceController.hpp index 4b9b206..ee8dc5c 100644 --- a/include/rewd_controllers/MultiInterfaceController.hpp +++ b/include/rewd_controllers/MultiInterfaceController.hpp @@ -34,43 +34,38 @@ #include #include +#include #include #include -#include #include -namespace rewd_controllers -{ +namespace rewd_controllers { using ControllerBase = controller_interface::ControllerBase; /** \cond HIDDEN_SYMBOLS */ -namespace internal -{ +namespace internal { -template -bool hasInterface(hardware_interface::RobotHW* robot_hw); +template bool hasInterface(hardware_interface::RobotHW *robot_hw); -template -void clearClaims(hardware_interface::RobotHW* robot_hw); +template void clearClaims(hardware_interface::RobotHW *robot_hw); template -void extractInterfaceResources(hardware_interface::RobotHW* robot_hw_in, - hardware_interface::RobotHW* robot_hw_out); +void extractInterfaceResources(hardware_interface::RobotHW *robot_hw_in, + hardware_interface::RobotHW *robot_hw_out); template -void populateClaimedResources(hardware_interface::RobotHW* robot_hw, - ControllerBase::ClaimedResources& claimed_resources); +void populateClaimedResources( + hardware_interface::RobotHW *robot_hw, + ControllerBase::ClaimedResources &claimed_resources); template -std::string enumerateElements(const T& val, - const std::string& delimiter = ", ", - const std::string& prefix = "", - const std::string& suffix = ""); +std::string enumerateElements(const T &val, const std::string &delimiter = ", ", + const std::string &prefix = "", + const std::string &suffix = ""); -} // namespace +} // namespace internal /** \endcond */ - /** * \brief %Controller able to claim resources from multiple hardware interfaces. * @@ -85,12 +80,12 @@ std::string enumerateElements(const T& val, * (non-exclusive) resource handling policy. * * By default, all specified hardware interfaces are required, and their - * existence will be enforced by \ref initRequest. It is possible to make hardware - * interfaces optional by means of the \c allow_optional_interfaces - * \ref MultiInterfaceController::MultiInterfaceController "constructor" parameter. - * This allows to write controllers where some interfaces are mandatory, and - * others, if present, improve controller performance, but whose absence does not - * prevent the controller from running. + * existence will be enforced by \ref initRequest. It is possible to make + * hardware interfaces optional by means of the \c allow_optional_interfaces + * \ref MultiInterfaceController::MultiInterfaceController "constructor" + * parameter. This allows to write controllers where some interfaces are + * mandatory, and others, if present, improve controller performance, but whose + * absence does not prevent the controller from running. * * The following is an example of a controller claiming resources from velocity- * and effort-controlled joints. @@ -143,13 +138,15 @@ std::string enumerateElements(const T& val, * // interfaces to be optional * VelEffController() * : rewd_controllers::MultiInterfaceController (true) + * EffortJointInterface> + * (true) * {} * * bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle &n) * { * // robot_hw pointer contains at most the two interfaces requested by the - * // controller. It may have none, only one or both, depending on whether the + * // controller. It may have none, only one or both, depending on whether + * the * // robot exposes them * * // v is a required interface @@ -159,8 +156,10 @@ std::string enumerateElements(const T& val, * return false; * } * - * // e is an optional interface. If present, additional features are enabled. - * // Controller can still function if interface or some of its resources are + * // e is an optional interface. If present, additional features are + * enabled. + * // Controller can still function if interface or some of its resources + * are * // absent * EffortJointInterface* e = robot_hw->get; * @@ -191,9 +190,9 @@ std::string enumerateElements(const T& val, * \pre When specified, template parameters \c T1 to \c T4 must be different * types. */ -template -class MultiInterfaceController: public ControllerBase -{ +template +class MultiInterfaceController : public ControllerBase { public: /** * \param allow_optional_interfaces If set to true, \ref initRequest will @@ -201,8 +200,9 @@ class MultiInterfaceController: public ControllerBase * If set to false (the default), all requested interfaces are required. */ MultiInterfaceController(bool allow_optional_interfaces = false) - : allow_optional_interfaces_(allow_optional_interfaces) - {state_ = CONSTRUCTED;} + : allow_optional_interfaces_(allow_optional_interfaces) { + state_ = CONSTRUCTED; + } virtual ~MultiInterfaceController() {} @@ -216,8 +216,8 @@ class MultiInterfaceController: public ControllerBase * non real-time initialization is performed, such as setup of ROS interfaces * and resource pre-allocation. * - * \param robot_hw Robot hardware abstraction containing a subset of the entire - * robot. If \ref MultiInterfaceController::MultiInterfaceController + * \param robot_hw Robot hardware abstraction containing a subset of the + * entire robot. If \ref MultiInterfaceController::MultiInterfaceController * "MultiInterfaceController" was called with \c allow_optional_interfaces set * to \c false (the default), this parameter contains all the interfaces * requested by the controller. @@ -226,16 +226,17 @@ class MultiInterfaceController: public ControllerBase * on whether the robot exposes them. Please refer to the code examples in the * \ref MultiInterfaceController "class description". * - * \param controller_nh A NodeHandle in the namespace from which the controller - * should read its configuration, and where it should set up its ROS - * interface. + * \param controller_nh A NodeHandle in the namespace from which the + * controller should read its configuration, and where it should set up its + * ROS interface. * * \returns True if initialization was successful and the controller * is ready to be started. */ - virtual bool init(hardware_interface::RobotHW* /*robot_hw*/, - ros::NodeHandle& /*controller_nh*/) - {return true;} + virtual bool init(hardware_interface::RobotHW * /*robot_hw*/, + ros::NodeHandle & /*controller_nh*/) { + return true; + } /** * \brief Custom controller initialization logic. @@ -244,8 +245,8 @@ class MultiInterfaceController: public ControllerBase * non real-time initialization is performed, such as setup of ROS interfaces * and resource pre-allocation. * - * \param robot_hw Robot hardware abstraction containing a subset of the entire - * robot. If \ref MultiInterfaceController::MultiInterfaceController + * \param robot_hw Robot hardware abstraction containing a subset of the + * entire robot. If \ref MultiInterfaceController::MultiInterfaceController * "MultiInterfaceController" was called with \c allow_optional_interfaces set * to \c false (the default), this parameter contains all the interfaces * requested by the controller. @@ -254,8 +255,9 @@ class MultiInterfaceController: public ControllerBase * on whether the robot exposes them. Please refer to the code examples in the * \ref MultiInterfaceController "class description". * - * \param root_nh A NodeHandle in the root of the controller manager namespace. - * This is where the ROS interfaces are setup (publishers, subscribers, services). + * \param root_nh A NodeHandle in the root of the controller manager + * namespace. This is where the ROS interfaces are setup (publishers, + * subscribers, services). * * \param controller_nh A NodeHandle in the namespace of the controller. * This is where the controller-specific configuration resides. @@ -263,10 +265,11 @@ class MultiInterfaceController: public ControllerBase * \returns True if initialization was successful and the controller * is ready to be started. */ - virtual bool init(hardware_interface::RobotHW* /*robot_hw*/, - ros::NodeHandle& /*root_nh*/, - ros::NodeHandle& /*controller_nh*/) - {return true;} + virtual bool init(hardware_interface::RobotHW * /*robot_hw*/, + ros::NodeHandle & /*root_nh*/, + ros::NodeHandle & /*controller_nh*/) { + return true; + } protected: /** @@ -279,8 +282,9 @@ class MultiInterfaceController: public ControllerBase * * \param robot_hw The robot hardware abstraction. * - * \param root_nh A NodeHandle in the root of the controller manager namespace. - * This is where the ROS interfaces are setup (publishers, subscribers, services). + * \param root_nh A NodeHandle in the root of the controller manager + * namespace. This is where the ROS interfaces are setup (publishers, + * subscribers, services). * * \param controller_nh A NodeHandle in the namespace of the controller. * This is where the controller-specific configuration resides. @@ -291,28 +295,31 @@ class MultiInterfaceController: public ControllerBase * \returns True if initialization was successful and the controller * is ready to be started. */ - virtual bool initRequest(hardware_interface::RobotHW* robot_hw, - ros::NodeHandle& root_nh, - ros::NodeHandle& controller_nh, - ClaimedResources& claimed_resources) - { + virtual bool initRequest(hardware_interface::RobotHW *robot_hw, + ros::NodeHandle &root_nh, + ros::NodeHandle &controller_nh, + ClaimedResources &claimed_resources) { // check if construction finished cleanly - if (state_ != CONSTRUCTED){ - ROS_ERROR("Cannot initialize this controller because it failed to be constructed"); + if (state_ != CONSTRUCTED) { + ROS_ERROR("Cannot initialize this controller because it failed to be " + "constructed"); return false; } // check for required hardware interfaces - if (!allow_optional_interfaces_ && !hasRequiredInterfaces(robot_hw)) {return false;} + if (!allow_optional_interfaces_ && !hasRequiredInterfaces(robot_hw)) { + return false; + } - // populate robot hardware abstraction containing only controller hardware interfaces (subset of robot) - hardware_interface::RobotHW* robot_hw_ctrl_p = &robot_hw_ctrl_; + // populate robot hardware abstraction containing only controller hardware + // interfaces (subset of robot) + hardware_interface::RobotHW *robot_hw_ctrl_p = &robot_hw_ctrl_; extractInterfaceResources(robot_hw, robot_hw_ctrl_p); // custom controller initialization clearClaims(robot_hw_ctrl_p); // claims will be populated on controller init - if (!init(robot_hw_ctrl_p, controller_nh) || !init(robot_hw_ctrl_p, root_nh, controller_nh)) - { + if (!init(robot_hw_ctrl_p, controller_nh) || + !init(robot_hw_ctrl_p, root_nh, controller_nh)) { ROS_ERROR("Failed to initialize the controller"); return false; } @@ -321,9 +328,9 @@ class MultiInterfaceController: public ControllerBase claimed_resources.clear(); populateClaimedResources(robot_hw_ctrl_p, claimed_resources); clearClaims(robot_hw_ctrl_p); - // NOTE: Above, claims are cleared since we only want to know what they are and report them back - // as an output parameter. Actual resource claiming by the controller is done when the controller - // is start()ed + // NOTE: Above, claims are cleared since we only want to know what they are + // and report them back as an output parameter. Actual resource claiming by + // the controller is done when the controller is start()ed // initialization successful state_ = INITIALIZED; @@ -333,29 +340,24 @@ class MultiInterfaceController: public ControllerBase /*\}*/ /** - * \brief Check if robot hardware abstraction contains all required interfaces. - * \param robot_hw Robot hardware abstraction. - * \return true if all required hardware interfaces are exposed by \c robot_hw, - * false otherwise. + * \brief Check if robot hardware abstraction contains all required + * interfaces. \param robot_hw Robot hardware abstraction. \return true if all + * required hardware interfaces are exposed by \c robot_hw, false otherwise. */ - static bool hasRequiredInterfaces(hardware_interface::RobotHW* robot_hw) - { + static bool hasRequiredInterfaces(hardware_interface::RobotHW *robot_hw) { using internal::hasInterface; - return hasInterface(robot_hw) && - hasInterface(robot_hw) && - hasInterface(robot_hw) && - hasInterface(robot_hw); - hasInterface(robot_hw); - hasInterface(robot_hw); + return hasInterface(robot_hw) && hasInterface(robot_hw) && + hasInterface(robot_hw) && hasInterface(robot_hw); + hasInterface(robot_hw); + hasInterface(robot_hw); } /** - * \brief Clear claims from all hardware interfaces requested by this controller. - * \param robot_hw Robot hardware abstraction containing the interfaces whose - * claims will be cleared. + * \brief Clear claims from all hardware interfaces requested by this + * controller. \param robot_hw Robot hardware abstraction containing the + * interfaces whose claims will be cleared. */ - static void clearClaims(hardware_interface::RobotHW* robot_hw) - { + static void clearClaims(hardware_interface::RobotHW *robot_hw) { using internal::clearClaims; clearClaims(robot_hw); clearClaims(robot_hw); @@ -373,9 +375,9 @@ class MultiInterfaceController: public ControllerBase * \param[out] robot_hw_out Robot hardware abstraction containing \e only the * interfaces requested by this controller. */ - static void extractInterfaceResources(hardware_interface::RobotHW* robot_hw_in, - hardware_interface::RobotHW* robot_hw_out) - { + static void + extractInterfaceResources(hardware_interface::RobotHW *robot_hw_in, + hardware_interface::RobotHW *robot_hw_out) { using internal::extractInterfaceResources; extractInterfaceResources(robot_hw_in, robot_hw_out); extractInterfaceResources(robot_hw_in, robot_hw_out); @@ -393,9 +395,8 @@ class MultiInterfaceController: public ControllerBase * \param[out] claimed_resources The resources claimed by this controller. * They can belong to multiple hardware interfaces. */ - static void populateClaimedResources(hardware_interface::RobotHW* robot_hw, - ClaimedResources& claimed_resources) - { + static void populateClaimedResources(hardware_interface::RobotHW *robot_hw, + ClaimedResources &claimed_resources) { using internal::populateClaimedResources; populateClaimedResources(robot_hw, claimed_resources); populateClaimedResources(robot_hw, claimed_resources); @@ -405,31 +406,33 @@ class MultiInterfaceController: public ControllerBase populateClaimedResources(robot_hw, claimed_resources); } - /** Robot hardware abstraction containing only the subset of interfaces requested by the controller. */ + /** Robot hardware abstraction containing only the subset of interfaces + * requested by the controller. */ hardware_interface::RobotHW robot_hw_ctrl_; - /** Flag to indicate if hardware interfaces are considered optional (i.e. non-required). */ + /** Flag to indicate if hardware interfaces are considered optional (i.e. + * non-required). */ bool allow_optional_interfaces_; private: - MultiInterfaceController(const MultiInterfaceController& c); - MultiInterfaceController& operator =(const MultiInterfaceController& c); + MultiInterfaceController(const MultiInterfaceController &c); + MultiInterfaceController &operator=(const MultiInterfaceController &c); }; - -namespace internal -{ +namespace internal { template -inline bool hasInterface(hardware_interface::RobotHW* robot_hw) -{ - T* hw = robot_hw->get(); - if (!hw) - { - const std::string hw_name = hardware_interface::internal::demangledTypeName(); - ROS_ERROR_STREAM("This controller requires a hardware interface of type '" << hw_name << "', " << - "but is not exposed by the robot. Available interfaces in robot:\n" << - enumerateElements(robot_hw->getNames(), "\n", "- '", "'")); // delimiter, prefix, suffux +inline bool hasInterface(hardware_interface::RobotHW *robot_hw) { + T *hw = robot_hw->get(); + if (!hw) { + const std::string hw_name = + hardware_interface::internal::demangledTypeName(); + ROS_ERROR_STREAM( + "This controller requires a hardware interface of type '" + << hw_name << "', " + << "but is not exposed by the robot. Available interfaces in robot:\n" + << enumerateElements(robot_hw->getNames(), "\n", "- '", + "'")); // delimiter, prefix, suffux return false; } return true; @@ -437,41 +440,46 @@ inline bool hasInterface(hardware_interface::RobotHW* robot_hw) // Specialization for unused template parameters defaulting to void template <> -inline bool hasInterface(hardware_interface::RobotHW* /*robot_hw*/) {return true;} +inline bool hasInterface(hardware_interface::RobotHW * /*robot_hw*/) { + return true; +} -template -void clearClaims(hardware_interface::RobotHW* robot_hw) -{ - T* hw = robot_hw->get(); - if (hw) {hw->clearClaims();} +template void clearClaims(hardware_interface::RobotHW *robot_hw) { + T *hw = robot_hw->get(); + if (hw) { + hw->clearClaims(); + } } // Specialization for unused template parameters defaulting to void template <> -inline void clearClaims(hardware_interface::RobotHW* /*robot_hw*/) {} +inline void clearClaims(hardware_interface::RobotHW * /*robot_hw*/) {} template -inline void extractInterfaceResources(hardware_interface::RobotHW* robot_hw_in, - hardware_interface::RobotHW* robot_hw_out) -{ - T* hw = robot_hw_in->get(); - if (hw) {robot_hw_out->registerInterface(hw);} +inline void +extractInterfaceResources(hardware_interface::RobotHW *robot_hw_in, + hardware_interface::RobotHW *robot_hw_out) { + T *hw = robot_hw_in->get(); + if (hw) { + robot_hw_out->registerInterface(hw); + } } // Specialization for unused template parameters defaulting to void template <> -inline void extractInterfaceResources(hardware_interface::RobotHW* /*robot_hw_in*/, - hardware_interface::RobotHW* /*robot_hw_out*/) {} +inline void extractInterfaceResources( + hardware_interface::RobotHW * /*robot_hw_in*/, + hardware_interface::RobotHW * /*robot_hw_out*/) {} template -inline void populateClaimedResources(hardware_interface::RobotHW* robot_hw, - ControllerBase::ClaimedResources& claimed_resources) -{ - T* hw = robot_hw->get(); - if (hw) - { +inline void +populateClaimedResources(hardware_interface::RobotHW *robot_hw, + ControllerBase::ClaimedResources &claimed_resources) { + T *hw = robot_hw->get(); + if (hw) { hardware_interface::InterfaceResources iface_res; - iface_res.hardware_interface = hardware_interface::internal::demangledTypeName(); + iface_res.hardware_interface = + hardware_interface::internal::demangledTypeName(); iface_res.resources = hw->getClaims(); claimed_resources.push_back(iface_res); } @@ -479,29 +487,33 @@ inline void populateClaimedResources(hardware_interface::RobotHW* robot_hw, // Specialization for unused template parameters defaulting to void template <> -inline void populateClaimedResources(hardware_interface::RobotHW* /*robot_hw*/, - ControllerBase::ClaimedResources& /*claimed_resources*/) {} +inline void populateClaimedResources( + hardware_interface::RobotHW * /*robot_hw*/, + ControllerBase::ClaimedResources & /*claimed_resources*/) {} template -inline std::string enumerateElements(const T& val, - const std::string& delimiter, - const std::string& prefix, - const std::string& suffix) -{ +inline std::string enumerateElements(const T &val, const std::string &delimiter, + const std::string &prefix, + const std::string &suffix) { std::string ret; - if (val.empty()) {return ret;} + if (val.empty()) { + return ret; + } - const std::string sdp = suffix+delimiter+prefix; + const std::string sdp = suffix + delimiter + prefix; std::stringstream ss; ss << prefix; - std::copy(val.begin(), val.end(), std::ostream_iterator(ss, sdp.c_str())); + std::copy(val.begin(), val.end(), + std::ostream_iterator(ss, sdp.c_str())); ret = ss.str(); - if (!ret.empty()) {ret.erase(ret.size() - delimiter.size() - prefix.size());} + if (!ret.empty()) { + ret.erase(ret.size() - delimiter.size() - prefix.size()); + } return ret; } -} // namespace +} // namespace internal -} // namespace +} // namespace rewd_controllers #endif diff --git a/include/rewd_controllers/detail/JointAdapterFactory-impl.hpp b/include/rewd_controllers/detail/JointAdapterFactory-impl.hpp index dab9c21..2a15017 100644 --- a/include/rewd_controllers/detail/JointAdapterFactory-impl.hpp +++ b/include/rewd_controllers/detail/JointAdapterFactory-impl.hpp @@ -2,37 +2,31 @@ namespace rewd_controllers { //============================================================================= template -void JointAdapterFactory::registerFactory(const std::string& type) -{ - mFactories.emplace(type, - [](hardware_interface::RobotHW* hardwareInterface, - dart::dynamics::DegreeOfFreedom* dof) - -> JointAdapter* - { - const auto interface = hardwareInterface->get(); - if (!interface) - { - ROS_ERROR_STREAM("RobotHW has no interface of type '" - << typeid(Interface).name() << "'."); - return nullptr; - } +void JointAdapterFactory::registerFactory(const std::string &type) { + mFactories.emplace( + type, + [](hardware_interface::RobotHW *hardwareInterface, + dart::dynamics::DegreeOfFreedom *dof) -> JointAdapter * { + const auto interface = hardwareInterface->get(); + if (!interface) { + ROS_ERROR_STREAM("RobotHW has no interface of type '" + << typeid(Interface).name() << "'."); + return nullptr; + } - const auto dofName = dof->getName(); - typename Interface::ResourceHandleType handle; - try - { - handle = interface->getHandle(dofName); - } - catch (const hardware_interface::HardwareInterfaceException& e) - { - ROS_ERROR_STREAM("Unable to get interface of type '" - << typeid(Interface).name() << "' for joint '" << dofName << "'."); - return nullptr; - } + const auto dofName = dof->getName(); + typename Interface::ResourceHandleType handle; + try { + handle = interface->getHandle(dofName); + } catch (const hardware_interface::HardwareInterfaceException &e) { + ROS_ERROR_STREAM("Unable to get interface of type '" + << typeid(Interface).name() << "' for joint '" + << dofName << "'."); + return nullptr; + } - return new Adapter{handle, dof}; - } - ); + return new Adapter{handle, dof}; + }); } } // namespace rewd_controllers diff --git a/include/rewd_controllers/helpers.hpp b/include/rewd_controllers/helpers.hpp index 7ccdb82..0812dac 100644 --- a/include/rewd_controllers/helpers.hpp +++ b/include/rewd_controllers/helpers.hpp @@ -1,33 +1,31 @@ #ifndef REWD_CONTROLLERS_HELPERS_HPP_ #define REWD_CONTROLLERS_HELPERS_HPP_ -#include -#include -#include -#include +#include "JointAdapter.hpp" +#include "JointAdapterFactory.hpp" #include #include #include #include #include -#include "JointAdapter.hpp" -#include "JointAdapterFactory.hpp" +#include +#include +#include +#include namespace rewd_controllers { //============================================================================= -struct JointParameter -{ +struct JointParameter { std::string mName; std::string mType; }; //============================================================================= -class SkeletonJointStateUpdater final -{ +class SkeletonJointStateUpdater final { public: SkeletonJointStateUpdater( - dart::dynamics::SkeletonPtr skeleton, - hardware_interface::JointStateInterface* jointStateInterface); + dart::dynamics::SkeletonPtr skeleton, + hardware_interface::JointStateInterface *jointStateInterface); void update(); @@ -40,25 +38,27 @@ class SkeletonJointStateUpdater final }; //============================================================================= -dart::dynamics::SkeletonPtr loadRobotFromParameter( - const ros::NodeHandle& nodeHandle, const std::string& nameParameter); +dart::dynamics::SkeletonPtr +loadRobotFromParameter(const ros::NodeHandle &nodeHandle, + const std::string &nameParameter); -std::vector loadJointsFromParameter( - const ros::NodeHandle& nodeHandle, const std::string& jointsParameter, - const std::string& defaultType); +std::vector +loadJointsFromParameter(const ros::NodeHandle &nodeHandle, + const std::string &jointsParameter, + const std::string &defaultType); std::unordered_map loadGoalConstraintsFromParameter( - const ros::NodeHandle& nodeHandle, - const std::vector& jointParameters); + const ros::NodeHandle &nodeHandle, + const std::vector &jointParameters); -dart::dynamics::MetaSkeletonPtr getControlledMetaSkeleton( - const dart::dynamics::SkeletonPtr& skeleton, - const std::vector& parameters, - const std::string& name); +dart::dynamics::MetaSkeletonPtr +getControlledMetaSkeleton(const dart::dynamics::SkeletonPtr &skeleton, + const std::vector ¶meters, + const std::string &name); -ros::NodeHandle createDefaultAdapterNodeHandle( - const ros::NodeHandle& parentNodeHandle, - const dart::dynamics::DegreeOfFreedom* dof); +ros::NodeHandle +createDefaultAdapterNodeHandle(const ros::NodeHandle &parentNodeHandle, + const dart::dynamics::DegreeOfFreedom *dof); } // namespace rewd_controllers diff --git a/src/FTThresholdClient.cpp b/src/FTThresholdClient.cpp index 632c6d9..8442f86 100644 --- a/src/FTThresholdClient.cpp +++ b/src/FTThresholdClient.cpp @@ -4,12 +4,12 @@ namespace rewd_controllers { using SetFTThresholdAction = pr_control_msgs::SetForceTorqueThresholdAction; -using FTThresholdActionClient - = actionlib::SimpleActionClient; +using FTThresholdActionClient = + actionlib::SimpleActionClient; //============================================================================= -FTThresholdClient::FTThresholdClient(const std::string& controllerThresholdTopic) -{ +FTThresholdClient::FTThresholdClient( + const std::string &controllerThresholdTopic) { mFTThresholdActionClient = std::unique_ptr( new FTThresholdActionClient(controllerThresholdTopic)); ROS_INFO("Waiting for FT Threshold Action Server to start..."); @@ -18,10 +18,9 @@ FTThresholdClient::FTThresholdClient(const std::string& controllerThresholdTopic } //============================================================================= -bool FTThresholdClient::trySetThresholdsRepeatedly(double forceThreshold, double torqueThreshold) -{ - while (ros::ok()) - { +bool FTThresholdClient::trySetThresholdsRepeatedly(double forceThreshold, + double torqueThreshold) { + while (ros::ok()) { if (setThresholds(forceThreshold, torqueThreshold)) return true; std::this_thread::sleep_for(std::chrono::milliseconds(1000)); @@ -30,37 +29,28 @@ bool FTThresholdClient::trySetThresholdsRepeatedly(double forceThreshold, double } //============================================================================= -bool FTThresholdClient::setThresholds(double forceThreshold, double torqueThreshold, double timeout) -{ +bool FTThresholdClient::setThresholds(double forceThreshold, + double torqueThreshold, double timeout) { pr_control_msgs::SetForceTorqueThresholdGoal goal; goal.force_threshold = forceThreshold; goal.torque_threshold = torqueThreshold; mFTThresholdActionClient->sendGoal(goal); - bool finished_before_timeout - = mFTThresholdActionClient->waitForResult(ros::Duration(timeout)); + bool finished_before_timeout = + mFTThresholdActionClient->waitForResult(ros::Duration(timeout)); + actionlib::SimpleClientGoalState state = mFTThresholdActionClient->getState(); - actionlib::SimpleClientGoalState state - = mFTThresholdActionClient->getState(); - - if (!finished_before_timeout) - { + if (!finished_before_timeout) { return false; - } - else if (state == actionlib::SimpleClientGoalState::StateEnum::SUCCEEDED) - { + } else if (state == actionlib::SimpleClientGoalState::StateEnum::SUCCEEDED) { return true; - } - else if (state == actionlib::SimpleClientGoalState::StateEnum::ABORTED) - { + } else if (state == actionlib::SimpleClientGoalState::StateEnum::ABORTED) { return false; - } - else - { + } else { throw std::runtime_error( - "F/T Thresholds could not be set: " + state.toString() + ", " - + mFTThresholdActionClient->getResult()->message); + "F/T Thresholds could not be set: " + state.toString() + ", " + + mFTThresholdActionClient->getResult()->message); } } -} +} // namespace rewd_controllers diff --git a/src/GravityCompensationController.cpp b/src/GravityCompensationController.cpp index 334b0f3..bf479b3 100644 --- a/src/GravityCompensationController.cpp +++ b/src/GravityCompensationController.cpp @@ -1,15 +1,14 @@ #include -#include -#include #include #include +#include +#include -namespace rewd_controllers -{ +namespace rewd_controllers { //============================================================================= GravityCompensationController::GravityCompensationController() - : MultiInterfaceController(true) // allow_optional_interfaces + : MultiInterfaceController(true) // allow_optional_interfaces {} //============================================================================= @@ -17,14 +16,14 @@ GravityCompensationController::~GravityCompensationController() {} //============================================================================= bool GravityCompensationController::init(hardware_interface::RobotHW *robot, - ros::NodeHandle &n) -{ - using hardware_interface::JointStateInterface; + ros::NodeHandle &n) { using hardware_interface::EffortJointInterface; + using hardware_interface::JointStateInterface; // Build up the list of controlled DOFs. const auto jointParameters = loadJointsFromParameter(n, "joints", "effort"); - if (jointParameters.empty()) return false; + if (jointParameters.empty()) + return false; ROS_INFO_STREAM("Controlling " << jointParameters.size() << " joints:"); for (const auto ¶m : jointParameters) { @@ -41,12 +40,14 @@ bool GravityCompensationController::init(hardware_interface::RobotHW *robot, // Load the URDF as a Skeleton. mSkeleton = loadRobotFromParameter(n, "robot_description_parameter"); - if (!mSkeleton) return false; + if (!mSkeleton) + return false; // Extract the subset of the Skeleton that is being controlled. mControlledSkeleton = getControlledMetaSkeleton(mSkeleton, jointParameters, "Controlled"); - if (!mControlledSkeleton) return false; + if (!mControlledSkeleton) + return false; // the full skeleton. const auto jointStateInterface = robot->get(); @@ -88,8 +89,7 @@ bool GravityCompensationController::init(hardware_interface::RobotHW *robot, //============================================================================= void GravityCompensationController::update(const ros::Time &time, - const ros::Duration &period) -{ + const ros::Duration &period) { mSkeletonUpdater->update(); // Compute inverse dynamics torques for the current configuration @@ -102,7 +102,7 @@ void GravityCompensationController::update(const ros::Time &time, } } -} // namespace rewd_controllers +} // namespace rewd_controllers PLUGINLIB_EXPORT_CLASS(rewd_controllers::GravityCompensationController, controller_interface::ControllerBase) diff --git a/src/JointAdapter.cpp b/src/JointAdapter.cpp index 18d72b7..8d7c2ba 100644 --- a/src/JointAdapter.cpp +++ b/src/JointAdapter.cpp @@ -7,81 +7,66 @@ namespace rewd_controllers { //============================================================================= -JointAdapter::JointAdapter() - : mDesiredPosition{0.} -{ -} +JointAdapter::JointAdapter() : mDesiredPosition{0.} {} //============================================================================= -JointAdapter::~JointAdapter() -{ -} +JointAdapter::~JointAdapter() {} //============================================================================= -void JointAdapter::setDesiredPosition(double desiredPosition) -{ +void JointAdapter::setDesiredPosition(double desiredPosition) { mDesiredPosition = desiredPosition; } //============================================================================= JointPositionAdapter::JointPositionAdapter( - hardware_interface::JointHandle positionHandle, - dart::dynamics::DegreeOfFreedom* dof) - : mPositionHandle{positionHandle} - , mDof{dof} -{ -} + hardware_interface::JointHandle positionHandle, + dart::dynamics::DegreeOfFreedom *dof) + : mPositionHandle{positionHandle}, mDof{dof} {} //============================================================================= -bool JointPositionAdapter::initialize(const ros::NodeHandle& nodeHandle) -{ +bool JointPositionAdapter::initialize(const ros::NodeHandle &nodeHandle) { return true; // Do nothing. } //============================================================================= -void JointPositionAdapter::update( - const ros::Time& /*time*/, const ros::Duration& /*period*/, - double /*actualPosition*/, double desiredPosition, - double /*actualVelocity*/, double /*desiredVelocity*/, - double /*nominalEffort*/) -{ +void JointPositionAdapter::update(const ros::Time & /*time*/, + const ros::Duration & /*period*/, + double /*actualPosition*/, + double desiredPosition, + double /*actualVelocity*/, + double /*desiredVelocity*/, + double /*nominalEffort*/) { mPositionHandle.setCommand(desiredPosition); } //============================================================================= -void JointPositionAdapter::reset() -{ +void JointPositionAdapter::reset() { // Do nothing. } //============================================================================= JointVelocityAdapter::JointVelocityAdapter( - hardware_interface::JointHandle effortHandle, - dart::dynamics::DegreeOfFreedom* dof) - : mVelocityHandle{effortHandle} - , mDof{dof} -{ + hardware_interface::JointHandle effortHandle, + dart::dynamics::DegreeOfFreedom *dof) + : mVelocityHandle{effortHandle}, mDof{dof} { mUpperVelLimit = mDof->getVelocityUpperLimit(); mLowerVelLimit = mDof->getVelocityLowerLimit(); } //============================================================================= -bool JointVelocityAdapter::initialize(const ros::NodeHandle& nodeHandle) -{ +bool JointVelocityAdapter::initialize(const ros::NodeHandle &nodeHandle) { return mPid.init(nodeHandle); } //============================================================================= -void JointVelocityAdapter::update( - const ros::Time& /*time*/, const ros::Duration& period, - double actualPosition, double desiredPosition, - double actualVelocity, double desiredVelocity, - double /*nominalEffort*/) -{ - const auto pidVelocity = mPid.computeCommand( - desiredPosition - actualPosition, - desiredVelocity - actualVelocity, - period); +void JointVelocityAdapter::update(const ros::Time & /*time*/, + const ros::Duration &period, + double actualPosition, double desiredPosition, + double actualVelocity, double desiredVelocity, + double /*nominalEffort*/) { + const auto pidVelocity = + mPid.computeCommand(desiredPosition - actualPosition, + desiredVelocity - actualVelocity, period); if (std::isnan(desiredVelocity)) throw std::range_error("desiredVelocity is NaN"); @@ -90,13 +75,16 @@ void JointVelocityAdapter::update( auto commandedVelocity = desiredVelocity + pidVelocity; - if (commandedVelocity > mUpperVelLimit || commandedVelocity < mLowerVelLimit) - { - ROS_ERROR_STREAM("Velocity limit exceeded wtih desired pose " << desiredPosition - << " and actual pose " << actualPosition); + if (commandedVelocity > mUpperVelLimit || + commandedVelocity < mLowerVelLimit) { + ROS_ERROR_STREAM("Velocity limit exceeded wtih desired pose " + << desiredPosition << " and actual pose " + << actualPosition); std::stringstream ss; - ss << "Overall velocity [" << desiredVelocity + pidVelocity << "] is beyond the velocity" - << " limits [" << mLowerVelLimit << ", " << mUpperVelLimit << "]" << std::endl; + ss << "Overall velocity [" << desiredVelocity + pidVelocity + << "] is beyond the velocity" + << " limits [" << mLowerVelLimit << ", " << mUpperVelLimit << "]" + << std::endl; throw std::range_error(ss.str()); } @@ -104,38 +92,29 @@ void JointVelocityAdapter::update( } //============================================================================= -void JointVelocityAdapter::reset() -{ - mPid.reset(); -} +void JointVelocityAdapter::reset() { mPid.reset(); } //============================================================================= JointEffortAdapter::JointEffortAdapter( - hardware_interface::JointHandle effortHandle, - dart::dynamics::DegreeOfFreedom* dof) - : mEffortHandle{effortHandle} - , mDof{dof} -{ -} + hardware_interface::JointHandle effortHandle, + dart::dynamics::DegreeOfFreedom *dof) + : mEffortHandle{effortHandle}, mDof{dof} {} //============================================================================= -bool JointEffortAdapter::initialize(const ros::NodeHandle& nodeHandle) -{ +bool JointEffortAdapter::initialize(const ros::NodeHandle &nodeHandle) { return mPid.init(nodeHandle); } //============================================================================= -void JointEffortAdapter::update( - const ros::Time& /*time*/, const ros::Duration& period, - double actualPosition, double desiredPosition, - double actualVelocity, double desiredVelocity, - double nominalEffort) -{ +void JointEffortAdapter::update(const ros::Time & /*time*/, + const ros::Duration &period, + double actualPosition, double desiredPosition, + double actualVelocity, double desiredVelocity, + double nominalEffort) { // TODO: Handle position wrapping on SO(2) joints. - const auto pidEffort = mPid.computeCommand( - desiredPosition - actualPosition, - desiredVelocity - actualVelocity, - period); + const auto pidEffort = + mPid.computeCommand(desiredPosition - actualPosition, + desiredVelocity - actualVelocity, period); if (std::isnan(nominalEffort)) throw std::range_error("nominalEffort is NaN"); if (std::isnan(pidEffort)) @@ -145,9 +124,6 @@ void JointEffortAdapter::update( } //============================================================================= -void JointEffortAdapter::reset() -{ - mPid.reset(); -} +void JointEffortAdapter::reset() { mPid.reset(); } } // namespace rewd_controllers diff --git a/src/JointAdapterFactory.cpp b/src/JointAdapterFactory.cpp index 14d151f..05452f5 100644 --- a/src/JointAdapterFactory.cpp +++ b/src/JointAdapterFactory.cpp @@ -3,31 +3,24 @@ namespace rewd_controllers { //============================================================================= -JointAdapterFactory::JointAdapterFactory() -{ -} +JointAdapterFactory::JointAdapterFactory() {} //============================================================================= -JointAdapterFactory::~JointAdapterFactory() -{ -} +JointAdapterFactory::~JointAdapterFactory() {} //============================================================================= -std::unique_ptr JointAdapterFactory::create( - const std::string& type, - hardware_interface::RobotHW* hardwareInterface, - dart::dynamics::DegreeOfFreedom* dof) const -{ +std::unique_ptr +JointAdapterFactory::create(const std::string &type, + hardware_interface::RobotHW *hardwareInterface, + dart::dynamics::DegreeOfFreedom *dof) const { const auto it = mFactories.find(type); - if (it == std::end(mFactories)) - { + if (it == std::end(mFactories)) { ROS_ERROR_STREAM("Unknown joint type '" << type << "'."); return nullptr; } - - const auto& factoryFunction = it->second; - return std::unique_ptr{ - factoryFunction(hardwareInterface, dof)}; + + const auto &factoryFunction = it->second; + return std::unique_ptr{factoryFunction(hardwareInterface, dof)}; } } // namespace rewd_controllers diff --git a/src/JointGroupPositionController.cpp b/src/JointGroupPositionController.cpp index 150fc5f..722a8a2 100644 --- a/src/JointGroupPositionController.cpp +++ b/src/JointGroupPositionController.cpp @@ -8,29 +8,26 @@ namespace rewd_controllers { //============================================================================= JointGroupPositionController::JointGroupPositionController() - : MultiInterfaceController(true) // allow_optional_interfaces + : MultiInterfaceController(true) // allow_optional_interfaces { using hardware_interface::EffortJointInterface; using hardware_interface::PositionJointInterface; using hardware_interface::VelocityJointInterface; - mAdapterFactory.registerFactory< - PositionJointInterface, JointPositionAdapter>("position"); - mAdapterFactory.registerFactory< - VelocityJointInterface, JointVelocityAdapter>("velocity"); - mAdapterFactory.registerFactory< - EffortJointInterface, JointEffortAdapter>("effort"); + mAdapterFactory.registerFactory( + "position"); + mAdapterFactory.registerFactory( + "velocity"); + mAdapterFactory.registerFactory( + "effort"); } //============================================================================= -JointGroupPositionController::~JointGroupPositionController() -{ -} +JointGroupPositionController::~JointGroupPositionController() {} //============================================================================= -bool JointGroupPositionController::init( - hardware_interface::RobotHW *robot, ros::NodeHandle &n) -{ +bool JointGroupPositionController::init(hardware_interface::RobotHW *robot, + ros::NodeHandle &n) { using hardware_interface::JointStateInterface; // Build up the list of controlled DOFs. @@ -44,29 +41,27 @@ bool JointGroupPositionController::init( return false; // Extract the subset of the Skeleton that is being controlled. - mControlledSkeleton = getControlledMetaSkeleton( - mSkeleton, jointParameters, "Controlled"); + mControlledSkeleton = + getControlledMetaSkeleton(mSkeleton, jointParameters, "Controlled"); if (!mControlledSkeleton) return false; // Retreive JointStateHandles required to update the position and velocity of // the full skeleton. const auto jointStateInterface = robot->get(); - if (!jointStateInterface) - { + if (!jointStateInterface) { ROS_ERROR("Unable to get JointStateInterface from RobotHW instance."); return false; } mSkeletonUpdater.reset( - new SkeletonJointStateUpdater(mSkeleton, jointStateInterface)); + new SkeletonJointStateUpdater(mSkeleton, jointStateInterface)); // Create adaptors to provide a uniform interface to different types. const auto numControlledDofs = mControlledSkeleton->getNumDofs(); mAdapters.resize(mControlledSkeleton->getNumDofs()); - for (size_t idof = 0; idof < numControlledDofs; ++idof) - { + for (size_t idof = 0; idof < numControlledDofs; ++idof) { const auto dof = mControlledSkeleton->getDof(idof); const auto param = jointParameters[idof]; @@ -75,7 +70,8 @@ bool JointGroupPositionController::init( return false; ros::NodeHandle adapterNodeHandle = createDefaultAdapterNodeHandle(n, dof); - if (!adapter->initialize(adapterNodeHandle)) return false; + if (!adapter->initialize(adapterNodeHandle)) + return false; mAdapters[idof] = std::move(adapter); } @@ -86,31 +82,29 @@ bool JointGroupPositionController::init( // Start command subscriber mCommandSubscriber = n.subscribe( - "command", 1, &JointGroupPositionController::setCommand, this); + "command", 1, &JointGroupPositionController::setCommand, this); return true; } //============================================================================= -void JointGroupPositionController::starting(const ros::Time& time) -{ +void JointGroupPositionController::starting(const ros::Time &time) { // Initialize the setpoint to the current joint positions. mSkeletonUpdater->update(); mDesiredPosition = mControlledSkeleton->getPositions(); mDesiredPositionBuffer.initRT(mDesiredPosition); - ROS_INFO_STREAM("Initialized desired position to: " - << mDesiredPosition.transpose()); + ROS_INFO_STREAM( + "Initialized desired position to: " << mDesiredPosition.transpose()); // Reset any internal state in the adapters (e.g. integral windup). - for (const auto& adapter : mAdapters) + for (const auto &adapter : mAdapters) adapter->reset(); } //============================================================================= -void JointGroupPositionController::update( - const ros::Time& time, const ros::Duration& period) -{ +void JointGroupPositionController::update(const ros::Time &time, + const ros::Duration &period) { const auto desiredVelocity = 0.; mDesiredPosition = *mDesiredPositionBuffer.readFromRT(); @@ -122,53 +116,47 @@ void JointGroupPositionController::update( // Delegate to the adapter for each joint. This directly writes to the // robot's hardware interface. - for (size_t idof = 0; idof < mAdapters.size(); ++idof) - { - mAdapters[idof]->update(time, period, - mControlledSkeleton->getPosition(idof), mDesiredPosition[idof], - mControlledSkeleton->getVelocity(idof), desiredVelocity, - mControlledSkeleton->getForce(idof)); + for (size_t idof = 0; idof < mAdapters.size(); ++idof) { + mAdapters[idof]->update( + time, period, mControlledSkeleton->getPosition(idof), + mDesiredPosition[idof], mControlledSkeleton->getVelocity(idof), + desiredVelocity, mControlledSkeleton->getForce(idof)); } } //============================================================================= void JointGroupPositionController::setCommand( - const sensor_msgs::JointState& msg) -{ + const sensor_msgs::JointState &msg) { const auto numControlledDofs = mAdapters.size(); - if (msg.name.size() != numControlledDofs) - { + if (msg.name.size() != numControlledDofs) { ROS_ERROR_STREAM("Received command with incorrect number of names:" - << " expected " << numControlledDofs << ", got " << msg.name.size()); + << " expected " << numControlledDofs << ", got " + << msg.name.size()); return; } - if (msg.position.size() != numControlledDofs) - { + if (msg.position.size() != numControlledDofs) { ROS_ERROR_STREAM("Received command with incorrect number of positions:" - << " expected " << numControlledDofs << ", got " << msg.position.size()); + << " expected " << numControlledDofs << ", got " + << msg.position.size()); return; } - if (!msg.velocity.empty()) - { + if (!msg.velocity.empty()) { ROS_ERROR("Received command with velocities. Only position is supported."); return; } - if (!msg.effort.empty()) - { + if (!msg.effort.empty()) { ROS_ERROR("Received command with velocities. Only position is supported."); return; } Eigen::VectorXd desiredPosition(numControlledDofs); - for (size_t i = 0; i < numControlledDofs; ++i) - { + for (size_t i = 0; i < numControlledDofs; ++i) { const auto dof = mSkeleton->getDof(msg.name[i]); const auto dofIndex = mControlledSkeleton->getIndexOf(dof, false); - if (dofIndex == dart::dynamics::INVALID_INDEX) - { - ROS_ERROR_STREAM("There is DegreeOfFreedom named '" << dof->getName() - << "' under control."); + if (dofIndex == dart::dynamics::INVALID_INDEX) { + ROS_ERROR_STREAM("There is DegreeOfFreedom named '" + << dof->getName() << "' under control."); return; } @@ -181,6 +169,5 @@ void JointGroupPositionController::setCommand( } // namespace rewd_controllers //============================================================================= -PLUGINLIB_EXPORT_CLASS( - rewd_controllers::JointGroupPositionController, - controller_interface::ControllerBase) +PLUGINLIB_EXPORT_CLASS(rewd_controllers::JointGroupPositionController, + controller_interface::ControllerBase) diff --git a/src/JointTrajectoryController.cpp b/src/JointTrajectoryController.cpp index 99f2c02..d9b6356 100644 --- a/src/JointTrajectoryController.cpp +++ b/src/JointTrajectoryController.cpp @@ -1,48 +1,42 @@ -#include #include +#include -namespace rewd_controllers -{ +namespace rewd_controllers { //============================================================================= JointTrajectoryController::JointTrajectoryController() - : MultiInterfaceController{true} // allow_optional_interfaces - , JointTrajectoryControllerBase{} -{ -} + : MultiInterfaceController{true} // allow_optional_interfaces + , + JointTrajectoryControllerBase{} {} //============================================================================= JointTrajectoryController::~JointTrajectoryController() {} //============================================================================= -bool JointTrajectoryController::init(hardware_interface::RobotHW* robot, - ros::NodeHandle& n) -{ +bool JointTrajectoryController::init(hardware_interface::RobotHW *robot, + ros::NodeHandle &n) { return initController(robot, n); } //============================================================================= -void JointTrajectoryController::starting(const ros::Time& time) -{ +void JointTrajectoryController::starting(const ros::Time &time) { startController(time); } //============================================================================= -void JointTrajectoryController::stopping(const ros::Time& time) -{ +void JointTrajectoryController::stopping(const ros::Time &time) { stopController(time); } //============================================================================= -void JointTrajectoryController::update(const ros::Time& time, - const ros::Duration& period) -{ +void JointTrajectoryController::update(const ros::Time &time, + const ros::Duration &period) { updateStep(time, period); } //============================================================================= bool JointTrajectoryController::shouldAcceptRequests() { return isRunning(); } -} // namespace rewd_controllers +} // namespace rewd_controllers //============================================================================= PLUGINLIB_EXPORT_CLASS(rewd_controllers::JointTrajectoryController, diff --git a/src/JointTrajectoryControllerBase.cpp b/src/JointTrajectoryControllerBase.cpp index 59b3319..a7b0d7d 100644 --- a/src/JointTrajectoryControllerBase.cpp +++ b/src/JointTrajectoryControllerBase.cpp @@ -5,13 +5,13 @@ #include #include +#include #include -#include #include #include +#include #include #include -#include #include #include #include @@ -20,21 +20,17 @@ using aikido::statespace::dart::MetaSkeletonStateSpace; using aikido::statespace::dart::R1Joint; -namespace rewd_controllers -{ -namespace -{ +namespace rewd_controllers { +namespace { //============================================================================= -std::vector toVector(const Eigen::VectorXd& input) -{ +std::vector toVector(const Eigen::VectorXd &input) { return std::vector{input.data(), input.data() + input.size()}; } -} // namespace +} // namespace //============================================================================= -JointTrajectoryControllerBase::JointTrajectoryControllerBase() -{ +JointTrajectoryControllerBase::JointTrajectoryControllerBase() { using hardware_interface::EffortJointInterface; using hardware_interface::PositionJointInterface; using hardware_interface::VelocityJointInterface; @@ -52,8 +48,7 @@ JointTrajectoryControllerBase::~JointTrajectoryControllerBase() {} //============================================================================= bool JointTrajectoryControllerBase::initController( - hardware_interface::RobotHW* robot, ros::NodeHandle& n) -{ + hardware_interface::RobotHW *robot, ros::NodeHandle &n) { mNodeHandle.reset(new ros::NodeHandle{n}); if (!mCancelCurrentTrajectory.is_lock_free()) { ROS_ERROR_STREAM( @@ -71,22 +66,28 @@ bool JointTrajectoryControllerBase::initController( ROS_ERROR("Failed to load 'control_type' parameter."); return false; } - if (control_type != "position" && control_type != "velocity" && control_type != "effort") { - ROS_ERROR_STREAM("Invalid 'control_type' parameter. Must be 'position', 'velocity', or 'effort', but is " << control_type); + if (control_type != "position" && control_type != "velocity" && + control_type != "effort") { + ROS_ERROR_STREAM("Invalid 'control_type' parameter. Must be 'position', " + "'velocity', or 'effort', but is " + << control_type); return false; } // Build up the list of controlled DOFs. - const auto jointParameters = loadJointsFromParameter(n, "joints", control_type); - if (jointParameters.empty()) return false; + const auto jointParameters = + loadJointsFromParameter(n, "joints", control_type); + if (jointParameters.empty()) + return false; ROS_INFO_STREAM("Controlling " << jointParameters.size() << " joints:"); - for (const auto& param : jointParameters) + for (const auto ¶m : jointParameters) ROS_INFO_STREAM("- " << param.mName << " (type: " << param.mType << ")"); // Load the URDF as a Skeleton. mSkeleton = loadRobotFromParameter(n, "robot_description_parameter"); - if (!mSkeleton) return false; + if (!mSkeleton) + return false; // Check for zero-mass bodies that will be used incorrectly in calculations bool hasZeroMassBody = false; @@ -97,23 +98,24 @@ bool JointTrajectoryControllerBase::initController( hasZeroMassBody = true; } } - if (hasZeroMassBody) return false; // TODO is this actually a problem? + if (hasZeroMassBody) + return false; // TODO is this actually a problem? // Extract the subset of the Skeleton that is being controlled. mControlledSkeleton = getControlledMetaSkeleton(mSkeleton, jointParameters, "Controlled"); - if (!mControlledSkeleton) return false; + if (!mControlledSkeleton) + return false; mControlledSpace = std::make_shared(mControlledSkeleton.get()); std::vector subspaces; - for (std::size_t i = 0; i < mControlledSpace->getDimension(); ++i) - { - subspaces.emplace_back(std::make_shared()); + for (std::size_t i = 0; i < mControlledSpace->getDimension(); ++i) { + subspaces.emplace_back(std::make_shared()); } - mCompoundSpace - = std::move(std::make_shared(subspaces)); + mCompoundSpace = std::move( + std::make_shared(subspaces)); // the full skeleton. const auto jointStateInterface = robot->get(); @@ -137,11 +139,13 @@ bool JointTrajectoryControllerBase::initController( const auto param = jointParameters[idof]; auto adapter = mAdapterFactory.create(param.mType, robot, dof); - if (!adapter) return false; + if (!adapter) + return false; // Initialize the adapter using parameters stored on the parameter server. ros::NodeHandle adapterNodeHandle = createDefaultAdapterNodeHandle(n, dof); - if (!adapter->initialize(adapterNodeHandle)) return false; + if (!adapter->initialize(adapterNodeHandle)) + return false; mAdapters[idof] = std::move(adapter); } @@ -169,8 +173,7 @@ bool JointTrajectoryControllerBase::initController( } //============================================================================= -void JointTrajectoryControllerBase::startController(const ros::Time& time) -{ +void JointTrajectoryControllerBase::startController(const ros::Time &time) { mSkeletonUpdater->update(); // Hold the current position. @@ -186,7 +189,8 @@ void JointTrajectoryControllerBase::startController(const ros::Time& time) "Initialized desired acceleration: " << mDesiredAcceleration.transpose()); // Reset any internal state in the adapters (e.g. integral windup). - for (const auto& adapter : mAdapters) adapter->reset(); + for (const auto &adapter : mAdapters) + adapter->reset(); ROS_DEBUG("Reset joint adapters."); @@ -203,15 +207,13 @@ void JointTrajectoryControllerBase::startController(const ros::Time& time) } //============================================================================= -void JointTrajectoryControllerBase::stopController(const ros::Time& time) -{ +void JointTrajectoryControllerBase::stopController(const ros::Time &time) { mNonRealtimeTimer.stop(); } //============================================================================= -void JointTrajectoryControllerBase::updateStep(const ros::Time& time, - const ros::Duration& period) -{ +void JointTrajectoryControllerBase::updateStep(const ros::Time &time, + const ros::Duration &period) { auto mDesiredState = mCompoundSpace->createState(); @@ -224,9 +226,8 @@ void JointTrajectoryControllerBase::updateStep(const ros::Time& time, mActualVelocity = mControlledSkeleton->getVelocities(); mActualEffort = mControlledSkeleton->getForces(); - if (context && !context->mCompleted.load()) - { - const auto& trajectory = context->mTrajectory; + if (context && !context->mCompleted.load()) { + const auto &trajectory = context->mTrajectory; const auto timeFromStart = std::min((time - context->mStartTime).toSec(), trajectory->getEndTime()); @@ -244,26 +245,23 @@ void JointTrajectoryControllerBase::updateStep(const ros::Time& time, // Check goal constraints. bool goalConstraintsSatisfied = true; - for (const auto& dof : mControlledSkeleton->getDofs()) - { + for (const auto &dof : mControlledSkeleton->getDofs()) { auto goalIt = mGoalConstraints.find(dof->getName()); - if (goalIt != mGoalConstraints.end()) - { + if (goalIt != mGoalConstraints.end()) { std::size_t index = mControlledSkeleton->getIndexOf(dof); auto diff = std::abs(mDesiredPosition[index] - mActualPosition[index]); // Check for SO2 auto jointSpace = mControlledSpace->getJointSpace(index); auto r1Joint = std::dynamic_pointer_cast(jointSpace); - if(!r1Joint) { + if (!r1Joint) { diff = std::fmod(diff, 2.0 * M_PI); - if(diff > M_PI) { + if (diff > M_PI) { diff = 2 * M_PI - diff; } } - if (diff > (*goalIt).second) - { + if (diff > (*goalIt).second) { goalConstraintsSatisfied = false; break; } @@ -274,19 +272,17 @@ void JointTrajectoryControllerBase::updateStep(const ros::Time& time, bool shouldStopExec = shouldStopExecution(stopReason); // Terminate the current trajectory. - if (timeFromStart >= trajectory->getDuration() && goalConstraintsSatisfied) - { + if (timeFromStart >= trajectory->getDuration() && + goalConstraintsSatisfied) { context->mCompleted.store(true); - } - else if (shouldStopExec || mCancelCurrentTrajectory.load()) - { + } else if (shouldStopExec || mCancelCurrentTrajectory.load()) { // TODO: if there is no other work that needs done here, we can get rid of - // the cancel atomic_bool. We do not make the desired velocity and acceleration - // zero since the trajectory can potentially have been appended with another. + // the cancel atomic_bool. We do not make the desired velocity and + // acceleration zero since the trajectory can potentially have been + // appended with another. context->mCompleted.store(true); - if (shouldStopExec) - { + if (shouldStopExec) { mDesiredVelocity.fill(0.0); mDesiredAcceleration.fill(0.0); @@ -310,33 +306,31 @@ void JointTrajectoryControllerBase::updateStep(const ros::Time& time, mControlledSkeleton->setPositions(mActualPosition); mControlledSkeleton->setVelocities(mActualVelocity); - for (size_t idof = 0; idof < mAdapters.size(); ++idof) - { + for (size_t idof = 0; idof < mAdapters.size(); ++idof) { // Check for SO2 auto actualPos = mActualPosition[idof]; auto desiredPos = mDesiredPosition[idof]; auto jointSpace = mControlledSpace->getJointSpace(idof); auto r1Joint = std::dynamic_pointer_cast(jointSpace); - if(!r1Joint) { + if (!r1Joint) { desiredPos = std::fmod(desiredPos, 2.0 * M_PI); actualPos = std::fmod(actualPos, 2.0 * M_PI); - if(desiredPos - actualPos > M_PI) { - desiredPos -= 2*M_PI; - } else if(desiredPos - actualPos < -M_PI) { - desiredPos += 2*M_PI; + if (desiredPos - actualPos > M_PI) { + desiredPos -= 2 * M_PI; + } else if (desiredPos - actualPos < -M_PI) { + desiredPos += 2 * M_PI; } } // Call Adapter - mAdapters[idof]->update(time, period, actualPos, - desiredPos, mActualVelocity[idof], - mDesiredVelocity[idof], mDesiredEffort[idof]); + mAdapters[idof]->update(time, period, actualPos, desiredPos, + mActualVelocity[idof], mDesiredVelocity[idof], + mDesiredEffort[idof]); } } //============================================================================= -void JointTrajectoryControllerBase::goalCallback(GoalHandle goalHandle) -{ +void JointTrajectoryControllerBase::goalCallback(GoalHandle goalHandle) { const auto goal = goalHandle.getGoal(); ROS_INFO_STREAM("Received trajectory '" << goalHandle.getGoalID().id << "' with " @@ -354,8 +348,9 @@ void JointTrajectoryControllerBase::goalCallback(GoalHandle goalHandle) std::shared_ptr trajectory; try { trajectory = aikido::control::ros::toSplineJointTrajectory( - mControlledSpace, goal->trajectory, mControlledSkeleton->getPositions()); - } catch (const std::runtime_error& e) { + mControlledSpace, goal->trajectory, + mControlledSkeleton->getPositions()); + } catch (const std::runtime_error &e) { Result result; result.error_code = Result::INVALID_GOAL; result.error_string = e.what(); @@ -378,9 +373,8 @@ void JointTrajectoryControllerBase::goalCallback(GoalHandle goalHandle) startTime = specifiedStartTime; } else { startTime = now; - ROS_INFO( - "Trajectory does not have an explicit start time, so" - " we assume that it will start immediately."); + ROS_INFO("Trajectory does not have an explicit start time, so" + " we assume that it will start immediately."); } if (startTime < now) { @@ -420,23 +414,21 @@ void JointTrajectoryControllerBase::goalCallback(GoalHandle goalHandle) // The offset is a strict multiple of 2*M_PI to disallow any arbitrary // jumps in the execution i.e. if the next trajectory is offset from 2pi // in its start state, we still expect a smoother transition. - auto multiplier = offset / (2*M_PI); - for (int i = 0; i < offset.size(); ++i) - { - offset(i) = round(multiplier(i))*2*M_PI; + auto multiplier = offset / (2 * M_PI); + for (int i = 0; i < offset.size(); ++i) { + offset(i) = round(multiplier(i)) * 2 * M_PI; } mCurrentTrajectoryOffset = offset; newContext->mGoalHandle.setAccepted(); - { // enter critical section + { // enter critical section std::lock_guard newTrajectoryLock{mNewTrajectoryRequestsMutex}; mNewTrajectoryRequests.push_back(newContext); - } // exit critical section + } // exit critical section } //============================================================================= -void JointTrajectoryControllerBase::cancelCallback(GoalHandle goalHandle) -{ +void JointTrajectoryControllerBase::cancelCallback(GoalHandle goalHandle) { ROS_INFO_STREAM("Requesting cancelation of trajectory '" << goalHandle.getGoalID().id << "'."); if (!shouldAcceptRequests()) { @@ -453,11 +445,10 @@ void JointTrajectoryControllerBase::cancelCallback(GoalHandle goalHandle) //============================================================================= void JointTrajectoryControllerBase::nonRealtimeCallback( - const ros::TimerEvent& event) -{ + const ros::TimerEvent &event) { TrajectoryContextPtr latestTrajRequest; - { // enter critical section + { // enter critical section std::lock(mNewTrajectoryRequestsMutex, mCancelRequestsMutex); std::lock_guard newTrajectoryLock{mNewTrajectoryRequestsMutex, std::adopt_lock}; @@ -480,7 +471,7 @@ void JointTrajectoryControllerBase::nonRealtimeCallback( mNextTrajectory = mNewTrajectoryRequests.front(); mNewTrajectoryRequests.pop_front(); } - } // exit critical section + } // exit critical section TrajectoryContextPtr currentTraj; mCurrentTrajectory.get(currentTraj); @@ -496,7 +487,8 @@ void JointTrajectoryControllerBase::nonRealtimeCallback( // if completed due to being aborted else if (mAbortCurrentTrajectory.load()) { ROS_INFO_STREAM("Aborted trajectory '" - << currentTraj->mGoalHandle.getGoalID().id << "'. Reason: " << mAbortReason); + << currentTraj->mGoalHandle.getGoalID().id + << "'. Reason: " << mAbortReason); currentTraj->mGoalHandle.setAborted(Result(), mAbortReason); } // if completed due to finishing trajectory set success and reset @@ -509,31 +501,30 @@ void JointTrajectoryControllerBase::nonRealtimeCallback( // reset trajectory upon completion mCancelCurrentTrajectory.store(false); mAbortCurrentTrajectory.store(false); - mCurrentTrajectory.set( - mNextTrajectory); // either sets to nullptr or next - // trajectory if available + mCurrentTrajectory.set(mNextTrajectory); // either sets to nullptr or next + // trajectory if available mNextTrajectory.reset(); } } // if no active trajectory directly start next trajectory else { mCancelCurrentTrajectory.store(false); - mCurrentTrajectory.set(mNextTrajectory); // either sets to nullptr or next - // trajectory if available + mCurrentTrajectory.set(mNextTrajectory); // either sets to nullptr or next + // trajectory if available mNextTrajectory.reset(); } publishFeedback(event.current_real); } //============================================================================= -bool JointTrajectoryControllerBase::processCancelRequest(GoalHandle& ghToCancel) -{ +bool JointTrajectoryControllerBase::processCancelRequest( + GoalHandle &ghToCancel) { std::string ghIdToCancel = ghToCancel.getGoalID().id; // check if should cancel active trajectory TrajectoryContextPtr existingTraj; mCurrentTrajectory.get(existingTraj); - if (existingTraj - && ghIdToCancel == existingTraj->mGoalHandle.getGoalID().id) { + if (existingTraj && + ghIdToCancel == existingTraj->mGoalHandle.getGoalID().id) { mCancelCurrentTrajectory.store(true); return true; } else { @@ -554,8 +545,7 @@ bool JointTrajectoryControllerBase::processCancelRequest(GoalHandle& ghToCancel) //============================================================================= void JointTrajectoryControllerBase::publishFeedback( - const ros::Time& currentTime) -{ + const ros::Time ¤tTime) { std::shared_ptr context; mCurrentTrajectory.get(context); @@ -563,7 +553,7 @@ void JointTrajectoryControllerBase::publishFeedback( const ros::Duration timeFromStart{currentTime - context->mStartTime}; Feedback feedback; - feedback.header.stamp = currentTime; // TODO: Use control loop time. + feedback.header.stamp = currentTime; // TODO: Use control loop time. for (const auto dof : mControlledSkeleton->getDofs()) feedback.joint_names.emplace_back(dof->getName()); @@ -587,5 +577,7 @@ void JointTrajectoryControllerBase::publishFeedback( } //============================================================================= -bool JointTrajectoryControllerBase::shouldStopExecution(std::string& message) { return false; } -} // namespace rewd_controllers +bool JointTrajectoryControllerBase::shouldStopExecution(std::string &message) { + return false; +} +} // namespace rewd_controllers diff --git a/src/MoveUntilTouchController.cpp b/src/MoveUntilTouchController.cpp index 72d88c4..a657d36 100644 --- a/src/MoveUntilTouchController.cpp +++ b/src/MoveUntilTouchController.cpp @@ -3,30 +3,24 @@ #include #include -namespace rewd_controllers -{ +namespace rewd_controllers { //============================================================================= MoveUntilTouchController::MoveUntilTouchController() - : MultiInterfaceController{true} // allow_optional_interfaces - , JointTrajectoryControllerBase{} - , mTaringCompleted{false} - , mForceThreshold{0.0} - , mTorqueThreshold{0.0} -{ -} + : MultiInterfaceController{true} // allow_optional_interfaces + , + JointTrajectoryControllerBase{}, mTaringCompleted{false}, + mForceThreshold{0.0}, mTorqueThreshold{0.0} {} //============================================================================= MoveUntilTouchController::~MoveUntilTouchController() {} //============================================================================= -bool MoveUntilTouchController::init(hardware_interface::RobotHW* robot, - ros::NodeHandle& nh) -{ +bool MoveUntilTouchController::init(hardware_interface::RobotHW *robot, + ros::NodeHandle &nh) { // check that doubles are lock-free atomics if (!mForceThreshold.is_lock_free()) { - ROS_ERROR( - "Double atomics not lock-free on this system. Cannot guarantee " - "realtime safety."); + ROS_ERROR("Double atomics not lock-free on this system. Cannot guarantee " + "realtime safety."); return false; } @@ -72,7 +66,7 @@ bool MoveUntilTouchController::init(hardware_interface::RobotHW* robot, mForceTorqueHandle = ft_interface->getHandle(ft_wrench_name); ROS_INFO_STREAM("Reading force/torque data from '" << ft_wrench_name << "'."); - } catch (const hardware_interface::HardwareInterfaceException& e) { + } catch (const hardware_interface::HardwareInterfaceException &e) { ROS_ERROR_STREAM("Unable to get 'ForceTorqueSensorHandle' for '" << ft_wrench_name << "'."); return false; @@ -87,7 +81,7 @@ bool MoveUntilTouchController::init(hardware_interface::RobotHW* robot, try { mTareHandle = tare_interface->getHandle(ft_tare_name); ROS_INFO_STREAM("Triggering tares on '" << ft_tare_name << "'."); - } catch (const hardware_interface::HardwareInterfaceException& e) { + } catch (const hardware_interface::HardwareInterfaceException &e) { ROS_ERROR_STREAM("Unable to get 'TriggerHandle' for '" << ft_tare_name << "'."); return false; @@ -107,8 +101,7 @@ bool MoveUntilTouchController::init(hardware_interface::RobotHW* robot, } //============================================================================= -void MoveUntilTouchController::starting(const ros::Time& time) -{ +void MoveUntilTouchController::starting(const ros::Time &time) { // start asynchronous tare request mTareHandle.trigger(); // start base trajectory controller @@ -116,16 +109,14 @@ void MoveUntilTouchController::starting(const ros::Time& time) } //============================================================================= -void MoveUntilTouchController::stopping(const ros::Time& time) -{ +void MoveUntilTouchController::stopping(const ros::Time &time) { // stop base trajectory controller stopController(time); } //============================================================================= -void MoveUntilTouchController::update(const ros::Time& time, - const ros::Duration& period) -{ +void MoveUntilTouchController::update(const ros::Time &time, + const ros::Duration &period) { // check async tare request mTaringCompleted.store(mTareHandle.isTriggerComplete()); // update base trajectory controller @@ -136,21 +127,20 @@ void MoveUntilTouchController::update(const ros::Time& time, bool MoveUntilTouchController::shouldAcceptRequests() { return isRunning(); } //============================================================================= -bool MoveUntilTouchController::shouldStopExecution(std::string& message) -{ +bool MoveUntilTouchController::shouldStopExecution(std::string &message) { // inelegent to just terminate any running trajectory, // but we must guarantee taring completes before starting if (!mTaringCompleted.load()) { return true; } - const double* fArray = mForceTorqueHandle.getForce(); + const double *fArray = mForceTorqueHandle.getForce(); if (fArray == nullptr) { return true; } Eigen::Map force{fArray}; - const double* tArray = mForceTorqueHandle.getTorque(); + const double *tArray = mForceTorqueHandle.getTorque(); if (tArray == nullptr) { return true; } @@ -166,8 +156,8 @@ bool MoveUntilTouchController::shouldStopExecution(std::string& message) } //============================================================================= -void MoveUntilTouchController::setForceTorqueThreshold(FTThresholdGoalHandle gh) -{ +void MoveUntilTouchController::setForceTorqueThreshold( + FTThresholdGoalHandle gh) { const auto goal = gh.getGoal(); ROS_INFO_STREAM("Setting thresholds: force = " << goal->force_threshold << ", torque = " @@ -218,7 +208,7 @@ void MoveUntilTouchController::setForceTorqueThreshold(FTThresholdGoalHandle gh) gh.setSucceeded(result); } } -} // namespace rewd_controllers +} // namespace rewd_controllers //============================================================================= PLUGINLIB_EXPORT_CLASS(rewd_controllers::MoveUntilTouchController, diff --git a/src/MoveUntilTouchTopicController.cpp b/src/MoveUntilTouchTopicController.cpp index 29762b3..ccde49e 100644 --- a/src/MoveUntilTouchTopicController.cpp +++ b/src/MoveUntilTouchTopicController.cpp @@ -3,18 +3,16 @@ #include #include -static const std::chrono::milliseconds MAX_DELAY = std::chrono::milliseconds(400); +static const std::chrono::milliseconds MAX_DELAY = + std::chrono::milliseconds(400); -namespace rewd_controllers -{ +namespace rewd_controllers { //============================================================================= MoveUntilTouchTopicController::MoveUntilTouchTopicController() - : MultiInterfaceController{true} // allow_optional_interfaces - , JointTrajectoryControllerBase{} - , mTaringCompleted{false} - , mForceThreshold{0.0} - , mTorqueThreshold{0.0} -{ + : MultiInterfaceController{true} // allow_optional_interfaces + , + JointTrajectoryControllerBase{}, mTaringCompleted{false}, + mForceThreshold{0.0}, mTorqueThreshold{0.0} { // Do nothing. } @@ -24,14 +22,12 @@ MoveUntilTouchTopicController::~MoveUntilTouchTopicController() { } //============================================================================= -bool MoveUntilTouchTopicController::init(hardware_interface::RobotHW* robot, - ros::NodeHandle& nh) -{ +bool MoveUntilTouchTopicController::init(hardware_interface::RobotHW *robot, + ros::NodeHandle &nh) { // check that doubles are lock-free atomics if (!mForceThreshold.is_lock_free()) { - ROS_ERROR( - "Double atomics not lock-free on this system. Cannot guarantee " - "realtime safety."); + ROS_ERROR("Double atomics not lock-free on this system. Cannot guarantee " + "realtime safety."); return false; } @@ -67,17 +63,20 @@ bool MoveUntilTouchTopicController::init(hardware_interface::RobotHW* robot, } // subscribe to sensor data - mForceTorqueDataSub = nh.subscribe(ft_wrench_name, 1, &MoveUntilTouchTopicController::forceTorqueDataCallback, this); + mForceTorqueDataSub = nh.subscribe( + ft_wrench_name, 1, + &MoveUntilTouchTopicController::forceTorqueDataCallback, this); // action client to kick off taring - mTareActionClient = std::unique_ptr(new TareActionClient(nh, ft_tare_name)); + mTareActionClient = + std::unique_ptr(new TareActionClient(nh, ft_tare_name)); // start action server mFTThresholdActionServer.reset( new actionlib::ActionServer{ nh, "set_forcetorque_threshold", - std::bind(&MoveUntilTouchTopicController::setForceTorqueThreshold, this, - std::placeholders::_1), + std::bind(&MoveUntilTouchTopicController::setForceTorqueThreshold, + this, std::placeholders::_1), false}); mFTThresholdActionServer->start(); @@ -86,8 +85,8 @@ bool MoveUntilTouchTopicController::init(hardware_interface::RobotHW* robot, } //============================================================================= -void MoveUntilTouchTopicController::forceTorqueDataCallback(const geometry_msgs::WrenchStamped& msg) -{ +void MoveUntilTouchTopicController::forceTorqueDataCallback( + const geometry_msgs::WrenchStamped &msg) { std::lock_guard lock(mForceTorqueDataMutex); mForce.x() = msg.wrench.force.x; mForce.y() = msg.wrench.force.y; @@ -99,7 +98,8 @@ void MoveUntilTouchTopicController::forceTorqueDataCallback(const geometry_msgs: } //============================================================================= -void MoveUntilTouchTopicController::taringTransitionCallback(const TareActionClient::GoalHandle& goalHandle) { +void MoveUntilTouchTopicController::taringTransitionCallback( + const TareActionClient::GoalHandle &goalHandle) { if (goalHandle.getResult() && goalHandle.getResult()->success) { ROS_INFO("Taring completed!"); mTimeOfLastSensorDataReceived = std::chrono::steady_clock::now(); @@ -108,31 +108,31 @@ void MoveUntilTouchTopicController::taringTransitionCallback(const TareActionCli } //============================================================================= -void MoveUntilTouchTopicController::starting(const ros::Time& time) -{ +void MoveUntilTouchTopicController::starting(const ros::Time &time) { // start asynchronous tare request ROS_INFO("Starting Taring"); pr_control_msgs::TriggerGoal goal; - mTareGoalHandle = mTareActionClient->sendGoal(goal, boost::bind(&MoveUntilTouchTopicController::taringTransitionCallback, this, _1)); + mTareGoalHandle = mTareActionClient->sendGoal( + goal, + boost::bind(&MoveUntilTouchTopicController::taringTransitionCallback, + this, _1)); // start base trajectory controller startController(time); } //============================================================================= -void MoveUntilTouchTopicController::stopping(const ros::Time& time) -{ +void MoveUntilTouchTopicController::stopping(const ros::Time &time) { // stop base trajectory controller stopController(time); } //============================================================================= -void MoveUntilTouchTopicController::update(const ros::Time& time, - const ros::Duration& period) -{ - if (mTaringCompleted.load() - && (std::chrono::steady_clock::now() - mTimeOfLastSensorDataReceived > MAX_DELAY)) - { +void MoveUntilTouchTopicController::update(const ros::Time &time, + const ros::Duration &period) { + if (mTaringCompleted.load() && + (std::chrono::steady_clock::now() - mTimeOfLastSensorDataReceived > + MAX_DELAY)) { throw std::runtime_error("Lost connection to F/T sensor!"); } @@ -141,11 +141,12 @@ void MoveUntilTouchTopicController::update(const ros::Time& time, } //============================================================================= -bool MoveUntilTouchTopicController::shouldAcceptRequests() { return isRunning(); } +bool MoveUntilTouchTopicController::shouldAcceptRequests() { + return isRunning(); +} //============================================================================= -bool MoveUntilTouchTopicController::shouldStopExecution(std::string& message) -{ +bool MoveUntilTouchTopicController::shouldStopExecution(std::string &message) { // inelegent to just terminate any running trajectory, // but we must guarantee taring completes before starting if (!mTaringCompleted.load()) { @@ -162,17 +163,17 @@ bool MoveUntilTouchTopicController::shouldStopExecution(std::string& message) if (forceThresholdExceeded) { std::stringstream messageStream; - messageStream << "Force Threshold exceeded! Threshold: " - << forceThreshold << " Force: " << mForce.x() - << ", " << mForce.y() << ", " << mForce.z(); + messageStream << "Force Threshold exceeded! Threshold: " << forceThreshold + << " Force: " << mForce.x() << ", " << mForce.y() << ", " + << mForce.z(); message = messageStream.str(); ROS_WARN(message.c_str()); } if (torqueThresholdExceeded) { std::stringstream messageStream; messageStream << "Torque Threshold exceeded! Threshold: " - << torqueThreshold << " Torque: " << mTorque.x() - << ", " << mTorque.y() << ", " << mTorque.z(); + << torqueThreshold << " Torque: " << mTorque.x() << ", " + << mTorque.y() << ", " << mTorque.z(); message = messageStream.str(); ROS_WARN(message.c_str()); } @@ -181,8 +182,8 @@ bool MoveUntilTouchTopicController::shouldStopExecution(std::string& message) } //============================================================================= -void MoveUntilTouchTopicController::setForceTorqueThreshold(FTThresholdGoalHandle gh) -{ +void MoveUntilTouchTopicController::setForceTorqueThreshold( + FTThresholdGoalHandle gh) { const auto goal = gh.getGoal(); ROS_INFO_STREAM("Setting thresholds: force = " << goal->force_threshold << ", torque = " @@ -234,7 +235,7 @@ void MoveUntilTouchTopicController::setForceTorqueThreshold(FTThresholdGoalHandl gh.setSucceeded(result); } } -} // namespace rewd_controllers +} // namespace rewd_controllers //============================================================================= PLUGINLIB_EXPORT_CLASS(rewd_controllers::MoveUntilTouchTopicController, diff --git a/src/helpers.cpp b/src/helpers.cpp index 82ca603..959b5e2 100644 --- a/src/helpers.cpp +++ b/src/helpers.cpp @@ -5,24 +5,23 @@ namespace rewd_controllers { //============================================================================= -dart::dynamics::SkeletonPtr loadRobotFromParameter( - const ros::NodeHandle& nodeHandle, const std::string& nameParameter) -{ +dart::dynamics::SkeletonPtr +loadRobotFromParameter(const ros::NodeHandle &nodeHandle, + const std::string &nameParameter) { using aikido::io::CatkinResourceRetriever; static const dart::common::Uri emptyUri{}; // Get the name of the "robot_description" parameter. std::string parameterName; - nodeHandle.param( - nameParameter, parameterName, "/robot_description"); + nodeHandle.param(nameParameter, parameterName, + "/robot_description"); // Load the URDF from the parameter server. std::string robotDescription; - if (!nodeHandle.getParam(parameterName, robotDescription)) - { + if (!nodeHandle.getParam(parameterName, robotDescription)) { ROS_ERROR_STREAM("Failed loading URDF from '" << parameterName - << "' parameter."); + << "' parameter."); return nullptr; } @@ -30,10 +29,9 @@ dart::dynamics::SkeletonPtr loadRobotFromParameter( dart::utils::DartLoader urdfLoader; const auto resourceRetriever = std::make_shared(); const auto skeleton = urdfLoader.parseSkeletonString( - robotDescription, emptyUri, resourceRetriever); + robotDescription, emptyUri, resourceRetriever); - if (!skeleton) - { + if (!skeleton) { ROS_ERROR_STREAM("Failed parsing URDF into a Skeleton."); return nullptr; } @@ -42,66 +40,58 @@ dart::dynamics::SkeletonPtr loadRobotFromParameter( } //============================================================================= -std::vector loadJointsFromParameter( - const ros::NodeHandle& nodeHandle, - const std::string& jointsParameter, - const std::string& defaultType) -{ +std::vector +loadJointsFromParameter(const ros::NodeHandle &nodeHandle, + const std::string &jointsParameter, + const std::string &defaultType) { using XmlRpc::XmlRpcValue; static const std::vector emptyResult; XmlRpcValue jointsXml; - if (!nodeHandle.getParam("joints", jointsXml)) - { - ROS_ERROR_STREAM("Parameter '" << nodeHandle.getNamespace() - << "/joints' is required."); + if (!nodeHandle.getParam("joints", jointsXml)) { + ROS_ERROR_STREAM("Parameter '" << nodeHandle.getNamespace() + << "/joints' is required."); } - if (jointsXml.getType() != XmlRpcValue::TypeArray) - { + if (jointsXml.getType() != XmlRpcValue::TypeArray) { ROS_ERROR_STREAM("Parameter '" << nodeHandle.getNamespace() - << "/joints' is not an array."); + << "/joints' is not an array."); return emptyResult; } std::vector output; - for (int i = 0; i < jointsXml.size(); ++i) - { + for (int i = 0; i < jointsXml.size(); ++i) { JointParameter jointParameters; - auto& jointXml = jointsXml[i]; + auto &jointXml = jointsXml[i]; // Simple case where everything is effort-controlled. - if (jointXml.getType() == XmlRpcValue::TypeString) - { + if (jointXml.getType() == XmlRpcValue::TypeString) { jointParameters.mName = static_cast(jointXml); jointParameters.mType = defaultType; } // Advanced case where there are heterogeneous actuator types. - else if (jointXml.getType() == XmlRpcValue::TypeStruct) - { - auto& nameXml = jointXml["name"]; - if (nameXml.getType() != XmlRpcValue::TypeString) - { - ROS_ERROR_STREAM("Parameter '" << nodeHandle.getNamespace() << "/joints[" - << i << "]/name' is not a string."); + else if (jointXml.getType() == XmlRpcValue::TypeStruct) { + auto &nameXml = jointXml["name"]; + if (nameXml.getType() != XmlRpcValue::TypeString) { + ROS_ERROR_STREAM("Parameter '" << nodeHandle.getNamespace() + << "/joints[" << i + << "]/name' is not a string."); return emptyResult; } jointParameters.mName = static_cast(nameXml); - auto& typeXml = jointXml["type"]; - if (typeXml.getType() != XmlRpcValue::TypeString) - { - ROS_ERROR_STREAM("Parameter '" << nodeHandle.getNamespace() << "/joints[" - << i << "]/type' is not a string."); + auto &typeXml = jointXml["type"]; + if (typeXml.getType() != XmlRpcValue::TypeString) { + ROS_ERROR_STREAM("Parameter '" << nodeHandle.getNamespace() + << "/joints[" << i + << "]/type' is not a string."); return emptyResult; } jointParameters.mType = static_cast(typeXml); - } - else - { + } else { ROS_ERROR_STREAM("Parameter '" << nodeHandle.getNamespace() << "/joints[" - << i << "]' is not a struct."); + << i << "]' is not a struct."); return emptyResult; } @@ -112,46 +102,45 @@ std::vector loadJointsFromParameter( } std::unordered_map loadGoalConstraintsFromParameter( - const ros::NodeHandle& nodeHandle, - const std::vector& jointParameters) -{ + const ros::NodeHandle &nodeHandle, + const std::vector &jointParameters) { std::unordered_map goalConstraints; - for (const auto& jointParam : jointParameters) { + for (const auto &jointParam : jointParameters) { std::string jointName = jointParam.mName; double goalConstraint; - if (nodeHandle.getParam("constraints/" + jointName + "/goal", goalConstraint)) { + if (nodeHandle.getParam("constraints/" + jointName + "/goal", + goalConstraint)) { goalConstraints[jointName] = goalConstraint; } } if (goalConstraints.empty()) { - ROS_WARN("No goal constraint arguments specified. Define parameters like this: /constraint/jointname/goal: 0.1"); + ROS_WARN("No goal constraint arguments specified. Define parameters like " + "this: /constraint/jointname/goal: 0.1"); } else { - ROS_INFO_STREAM("Goal constraints loaded for " << goalConstraints.size() << " joints."); + ROS_INFO_STREAM("Goal constraints loaded for " << goalConstraints.size() + << " joints."); } return goalConstraints; } //============================================================================= -dart::dynamics::MetaSkeletonPtr getControlledMetaSkeleton( - const dart::dynamics::SkeletonPtr& skeleton, - const std::vector& parameters, - const std::string& name) -{ +dart::dynamics::MetaSkeletonPtr +getControlledMetaSkeleton(const dart::dynamics::SkeletonPtr &skeleton, + const std::vector ¶meters, + const std::string &name) { using dart::dynamics::Group; - std::vector dofs; + std::vector dofs; dofs.reserve(parameters.size()); - for (const auto& param : parameters) - { - const auto& dofName = param.mName; + for (const auto ¶m : parameters) { + const auto &dofName = param.mName; const auto dof = skeleton->getDof(dofName); - if (!dof) - { - ROS_ERROR_STREAM( - "Skeleton has no DegreeOfFreedom named '" << dofName << "'."); + if (!dof) { + ROS_ERROR_STREAM("Skeleton has no DegreeOfFreedom named '" << dofName + << "'."); return nullptr; } @@ -159,14 +148,12 @@ dart::dynamics::MetaSkeletonPtr getControlledMetaSkeleton( } const auto controlledMetaSkeleton = Group::create(name, dofs, false, true); - if (!controlledMetaSkeleton) - { + if (!controlledMetaSkeleton) { ROS_ERROR_STREAM("Failed creating MetaSkeleton of controlled DOFs."); return nullptr; } - if (controlledMetaSkeleton->getNumDofs() != parameters.size()) - { + if (controlledMetaSkeleton->getNumDofs() != parameters.size()) { ROS_ERROR_STREAM("Only single-DOF joints are supported."); return nullptr; } @@ -176,19 +163,16 @@ dart::dynamics::MetaSkeletonPtr getControlledMetaSkeleton( //============================================================================= SkeletonJointStateUpdater::SkeletonJointStateUpdater( - dart::dynamics::SkeletonPtr skeleton, - hardware_interface::JointStateInterface* jointStateInterface) - : mSkeleton{skeleton} - , mDefaultPosition(skeleton->getNumDofs()) - , mDefaultVelocity(skeleton->getNumDofs()) - , mDefaultEffort(skeleton->getNumDofs()) -{ + dart::dynamics::SkeletonPtr skeleton, + hardware_interface::JointStateInterface *jointStateInterface) + : mSkeleton{skeleton}, mDefaultPosition(skeleton->getNumDofs()), + mDefaultVelocity(skeleton->getNumDofs()), + mDefaultEffort(skeleton->getNumDofs()) { std::set missingJointNames; mHandles.reserve(skeleton->getNumDofs()); - for (size_t idof = 0; idof < skeleton->getNumDofs(); ++idof) - { + for (size_t idof = 0; idof < skeleton->getNumDofs(); ++idof) { const auto dof = skeleton->getDof(idof); const auto dofName = dof->getName(); @@ -197,31 +181,27 @@ SkeletonJointStateUpdater::SkeletonJointStateUpdater( mDefaultEffort[idof] = 0.; hardware_interface::JointStateHandle handle; - try - { + try { handle = jointStateInterface->getHandle(dofName); - } - catch (const hardware_interface::HardwareInterfaceException& e) - { + } catch (const hardware_interface::HardwareInterfaceException &e) { missingJointNames.emplace(dofName); // Use the default position, velocity, and effort. handle = hardware_interface::JointStateHandle{ - dofName, &mDefaultPosition[idof], &mDefaultVelocity[idof], - &mDefaultEffort[idof]}; + dofName, &mDefaultPosition[idof], &mDefaultVelocity[idof], + &mDefaultEffort[idof]}; } mHandles.emplace_back(handle); } - if (!mHandles.empty()) - { + if (!mHandles.empty()) { std::stringstream msg; msg << "Failed to get JointStateHandles for " << missingJointNames.size() << " joints. The following joints will be assumed to have their" << " position and velocity for dynamics calculations:"; - for (const auto& dofName : missingJointNames) + for (const auto &dofName : missingJointNames) msg << " '" << dofName << "'"; ROS_WARN_STREAM(msg.str()); @@ -229,22 +209,19 @@ SkeletonJointStateUpdater::SkeletonJointStateUpdater( } //============================================================================= -void SkeletonJointStateUpdater::update() -{ - for (size_t idof = 0; idof < mSkeleton->getNumDofs(); ++idof) - { +void SkeletonJointStateUpdater::update() { + for (size_t idof = 0; idof < mSkeleton->getNumDofs(); ++idof) { const auto dof = mSkeleton->getDof(idof); - const auto& jointStateHandle = mHandles[idof]; + const auto &jointStateHandle = mHandles[idof]; dof->setPosition(jointStateHandle.getPosition()); dof->setVelocity(jointStateHandle.getVelocity()); dof->setForce(jointStateHandle.getEffort()); } } -ros::NodeHandle createDefaultAdapterNodeHandle( - const ros::NodeHandle& parentNodeHandle, - const dart::dynamics::DegreeOfFreedom* dof) -{ +ros::NodeHandle +createDefaultAdapterNodeHandle(const ros::NodeHandle &parentNodeHandle, + const dart::dynamics::DegreeOfFreedom *dof) { auto dofName = dof->getName(); while (dofName.at(0) == '/') { // a leading slash creates a root level namespace From 743c19903445217de3f7de18be0cb03ceb0522f2 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Sat, 27 Jun 2020 10:48:34 -0700 Subject: [PATCH 05/12] Added MoveUntilTouchCartVelController --- CMakeLists.txt | 3 +- .../MoveUntilTouchCartVelocityController.hpp | 110 ++++++++++ include/rewd_controllers/helpers.hpp | 3 + src/MoveUntilTouchCartVelocityController.cpp | 194 ++++++++++++++++++ 4 files changed, 309 insertions(+), 1 deletion(-) create mode 100644 include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp create mode 100644 src/MoveUntilTouchCartVelocityController.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9de034c..674fe3a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(rewd_controllers) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") include_directories("include") @@ -66,6 +66,7 @@ add_library("${PROJECT_NAME}" SHARED src/MoveUntilTouchController.cpp src/MoveUntilTouchTopicController.cpp src/FTThresholdClient.cpp + src/MoveUntilTouchCartVelocityController.cpp ) target_link_libraries("${PROJECT_NAME}" ${DART_LIBRARIES} diff --git a/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp b/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp new file mode 100644 index 0000000..4d5cee2 --- /dev/null +++ b/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp @@ -0,0 +1,110 @@ +#ifndef REWD_CONTROLLERS_MOVEUNTILTOUCHCARTVELOCITY_HPP_ +#define REWD_CONTROLLERS_MOVEUNTILTOUCHCARTVELOCITY_HPP_ + +#include +#include +#include +#include +#include +#include "helpers.hpp" +#include +#include + +#include +#include + +namespace rewd_controllers +{ +class MoveUntilTouchCartVelocityController + : public controller_interface:: + MultiInterfaceController +{ +public: + MoveUntilTouchCartVelocityController(); + virtual ~MoveUntilTouchCartVelocityController(); + + /** \brief The init function is called to initialize the controller from a + * non-realtime thread with a pointer to the hardware interface, itself, + * instead of a pointer to a RobotHW. + * + * \param robot The specific hardware interface used by this controller. + * + * \param n A NodeHandle in the namespace from which the controller + * should read its configuration, and where it should set up its ROS + * interface. + * + * \returns True if initialization was successful and the controller + * is ready to be started. + */ + bool init(hardware_interface::RobotHW* robot, ros::NodeHandle& n); + + /*! + * \brief Issues commands to the joint. Should be called at regular intervals + */ + void update(const ros::Time& time, const ros::Duration& period); + + /** \brief This is called from within the realtime thread just before the + * first call to \ref update + * + * \param time The current time + */ + void starting(const ros::Time& time) override; + void stopping(const ros::Time& time) override; + +protected: + using Action = pr_control_msgs::SetCartesianVelocityAction; + using ActionServer = actionlib::ActionServer; + using GoalHandle = ActionServer::GoalHandle; + + using Result = pr_control_msgs::SetCartesianVelocityResult; + + using JointModes = hardware_interface::JointCommandModes; + +private: + /** \brief Actionlib callback to accept new Goal. + */ + void goalCallback(GoalHandle goalHandle); + + /** \brief Actionlib callback to cancel previously accepted goal. + */ + void cancelCallback(GoalHandle goalHandle); + + /** \brief Contains all data needed to execute the currently + * requested velocity. Shared between real time and non-real + * time threads. + * + * This structure must be used AS A WHOLE, and reassigned + * in its entirety. Assigning to individual components + * is UNSAFE with the exception of the mCompleted atomic. + */ + struct CartVelContext { + ros::Time mStartTime; + ros::Duration mDuration; + Eigen::VectorXd mDesiredVelocity; + GoalHandle mGoalHandle; + std::atomic_bool mCompleted; + }; + using CartVelContextPtr = std::shared_ptr; + + // For position feedback: + dart::dynamics::SkeletonPtr mSkeleton; + std::unique_ptr mSkeletonUpdater; + + // Action server variables + std::unique_ptr mActionServer; + + // TODO: It would be better to use std::atomic> here. + // However, this will not be implemented until C++20. + realtime_tools::RealtimeBox mCurrentCartVel; + + // For communication with robot + pr_hardware_interfaces::CartesianVelocityHandle mCartVelHandle; + hardware_interface::JointModeHandle mJointModeHandle; + JointModes lastMode; +}; + +} // namespace + +#endif diff --git a/include/rewd_controllers/helpers.hpp b/include/rewd_controllers/helpers.hpp index 0812dac..e882e58 100644 --- a/include/rewd_controllers/helpers.hpp +++ b/include/rewd_controllers/helpers.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -12,6 +13,8 @@ #include #include +#include + namespace rewd_controllers { //============================================================================= diff --git a/src/MoveUntilTouchCartVelocityController.cpp b/src/MoveUntilTouchCartVelocityController.cpp new file mode 100644 index 0000000..2f8f0ba --- /dev/null +++ b/src/MoveUntilTouchCartVelocityController.cpp @@ -0,0 +1,194 @@ +#include + +#include +#include +#include +#include + +#include +#include + +#define SE3_SIZE 6 + +namespace rewd_controllers +{ +namespace +{ +//============================================================================= +std::vector toVector(const Eigen::VectorXd& input) +{ + return std::vector{input.data(), input.data() + input.size()}; +} + +} // namespace + +//============================================================================= +MoveUntilTouchCartVelocityController::MoveUntilTouchCartVelocityController() + : MultiInterfaceController(true) // allow_optional_interfaces +{} + +//============================================================================= +MoveUntilTouchCartVelocityController::~MoveUntilTouchCartVelocityController() {} + +//============================================================================= +bool MoveUntilTouchCartVelocityController::init(hardware_interface::RobotHW *robot, + ros::NodeHandle &n) +{ + using hardware_interface::JointStateInterface; + using hardware_interface::JointModeInterface; + using pr_hardware_interfaces::CartesianVelocityInterface; + + // Load the URDF as a Skeleton. + mSkeleton = loadRobotFromParameter(n, "robot_description_parameter"); + if (!mSkeleton) return false; + + // Have skeleton update from joint state interface + const auto jointStateInterface = robot->get(); + if (!jointStateInterface) { + ROS_ERROR("Unable to get JointStateInterface from RobotHW instance."); + return false; + } + + mSkeletonUpdater.reset( + new SkeletonJointStateUpdater{mSkeleton, jointStateInterface}); + + + // Load control interfaces and handles + const auto jointModeInterface = robot->get(); + if (!jointModeInterface) { + ROS_ERROR("Unable to get JointModeInterface from RobotHW instance."); + return false; + } + + try { + mJointModeHandle = jointModeInterface->getHandle("joint_mode"); + } catch (const hardware_interface::HardwareInterfaceException &e) { + ROS_ERROR_STREAM("Unable to get joint mode interface for robot"); + return false; + } + + const auto cartVelInterface = robot->get(); + if (!cartVelInterface) { + ROS_ERROR("Unable to get CartesianVelocityInterface from RobotHW instance."); + return false; + } + + try { + mCartVelHandle = cartVelInterface->getHandle("cart_vel"); + } catch (const hardware_interface::HardwareInterfaceException &e) { + ROS_ERROR_STREAM("Unable to get cartesian velocity interface for robot."); + return false; + } + + // init local vars + mCurrentCartVel.set(nullptr); + + // TODO: Initialize FT sensor + + // Start the action server. This must be last. + using std::placeholders::_1; + mActionServer.reset(new actionlib::ActionServer{ + n, "cart_velocity", + std::bind(&MoveUntilTouchCartVelocityController::goalCallback, this, _1), + std::bind(&MoveUntilTouchCartVelocityController::cancelCallback, this, _1), + false}); + mActionServer->start(); + + ROS_INFO("MoveUntilTouchCartVelocityController initialized successfully"); + return true; +} + +//============================================================================= +void MoveUntilTouchCartVelocityController::starting(const ros::Time& time) +{ + // Set Joint Mode to Other + lastMode = mJointModeHandle.getMode(); + mJointModeHandle.setMode(JointModes::NOMODE); +} + +//============================================================================= +void MoveUntilTouchCartVelocityController::stopping(const ros::Time& time) +{ + // Return joint mode to what it was before + mJointModeHandle.setMode(lastMode); +} + +//============================================================================= +void MoveUntilTouchCartVelocityController::update(const ros::Time &time, + const ros::Duration &period) +{ + mSkeletonUpdater->update(); + + Eigen::VectorXd setVelocity; + setVelocity.resize(SE3_SIZE); + setVelocity.setZero(); + + // Load current command + std::shared_ptr context; + mCurrentCartVel.get(context); + + if (context && !context->mCompleted.load()) + { + // check duration + ros::Duration runTime = time - context->mStartTime; + if (runTime >= context->mDuration) { + // Successful run + Result result; + result.error_code = Result::SUCCESSFUL; + result.error_string = ""; + context->mGoalHandle.setSucceeded(result); + context->mCompleted.store(true); + } + // TODO: check FT sensor + else { + setVelocity = context->mDesiredVelocity; + } + } + + // Execute velocity command + mCartVelHandle.setCommand(toVector(setVelocity)); +} + +//============================================================================= +void MoveUntilTouchCartVelocityController::goalCallback(GoalHandle goalHandle) +{ + const auto goal = goalHandle.getGoal(); + ROS_INFO_STREAM("Received cartesian velocity '" + << goalHandle.getGoalID().id << "'."); + + // Setup the new trajectory. + const auto newContext = std::make_shared(); + newContext->mStartTime = ros::Time::now(); + newContext->mGoalHandle = goalHandle; + newContext->mDuration = goal->exec_time; + newContext->mDesiredVelocity.resize(SE3_SIZE); + newContext->mCompleted.store(false); + + // Copy over velocity data + newContext->mDesiredVelocity(0) = goal->command.linear.x; + newContext->mDesiredVelocity(1) = goal->command.linear.y; + newContext->mDesiredVelocity(2) = goal->command.linear.z; + newContext->mDesiredVelocity(3) = goal->command.angular.x; + newContext->mDesiredVelocity(4) = goal->command.angular.y; + newContext->mDesiredVelocity(5) = goal->command.angular.z; + + // TODO: add velocity checks + + // We've accepted the goal + newContext->mGoalHandle.setAccepted(); + mCurrentCartVel.set(newContext); +} + +//============================================================================= +void MoveUntilTouchCartVelocityController::cancelCallback(GoalHandle goalHandle) +{ + ROS_INFO_STREAM("Requesting cancelation of velocity '" + << goalHandle.getGoalID().id << "'."); + goalHandle.setAccepted(); + mCurrentCartVel.set(nullptr); +} + +} // namespace rewd_controllers + +PLUGINLIB_EXPORT_CLASS(rewd_controllers::MoveUntilTouchCartVelocityController, + controller_interface::ControllerBase) From 01894ccd236a97afa26f7556123e6de98dd8aa80 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Sat, 27 Jun 2020 10:48:34 -0700 Subject: [PATCH 06/12] Added MoveUntilTouchCartVelController --- include/rewd_controllers/helpers.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/rewd_controllers/helpers.hpp b/include/rewd_controllers/helpers.hpp index e882e58..eccb1e5 100644 --- a/include/rewd_controllers/helpers.hpp +++ b/include/rewd_controllers/helpers.hpp @@ -15,6 +15,8 @@ #include +#include + namespace rewd_controllers { //============================================================================= From 339ebb50d9d81145e1bbda95eb7ff92815476a21 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Sat, 27 Jun 2020 11:07:02 -0700 Subject: [PATCH 07/12] Ran clang-format-6.0 --- .../MoveUntilTouchCartVelocityController.hpp | 32 +++++----- include/rewd_controllers/helpers.hpp | 2 +- src/MoveUntilTouchCartVelocityController.cpp | 61 ++++++++----------- 3 files changed, 43 insertions(+), 52 deletions(-) diff --git a/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp b/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp index 4d5cee2..b66a850 100644 --- a/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp +++ b/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp @@ -1,26 +1,24 @@ #ifndef REWD_CONTROLLERS_MOVEUNTILTOUCHCARTVELOCITY_HPP_ #define REWD_CONTROLLERS_MOVEUNTILTOUCHCARTVELOCITY_HPP_ -#include +#include "helpers.hpp" +#include #include #include #include -#include -#include "helpers.hpp" -#include +#include #include +#include -#include #include +#include -namespace rewd_controllers -{ +namespace rewd_controllers { class MoveUntilTouchCartVelocityController - : public controller_interface:: - MultiInterfaceController -{ + : public controller_interface::MultiInterfaceController< + pr_hardware_interfaces::CartesianVelocityInterface, + hardware_interface::JointStateInterface, + hardware_interface::JointModeInterface> { public: MoveUntilTouchCartVelocityController(); virtual ~MoveUntilTouchCartVelocityController(); @@ -38,20 +36,20 @@ class MoveUntilTouchCartVelocityController * \returns True if initialization was successful and the controller * is ready to be started. */ - bool init(hardware_interface::RobotHW* robot, ros::NodeHandle& n); + bool init(hardware_interface::RobotHW *robot, ros::NodeHandle &n); /*! * \brief Issues commands to the joint. Should be called at regular intervals */ - void update(const ros::Time& time, const ros::Duration& period); + void update(const ros::Time &time, const ros::Duration &period); /** \brief This is called from within the realtime thread just before the * first call to \ref update * * \param time The current time */ - void starting(const ros::Time& time) override; - void stopping(const ros::Time& time) override; + void starting(const ros::Time &time) override; + void stopping(const ros::Time &time) override; protected: using Action = pr_control_msgs::SetCartesianVelocityAction; @@ -105,6 +103,6 @@ class MoveUntilTouchCartVelocityController JointModes lastMode; }; -} // namespace +} // namespace rewd_controllers #endif diff --git a/include/rewd_controllers/helpers.hpp b/include/rewd_controllers/helpers.hpp index ae4df26..1be3580 100644 --- a/include/rewd_controllers/helpers.hpp +++ b/include/rewd_controllers/helpers.hpp @@ -4,8 +4,8 @@ #include "JointAdapterFactory.hpp" #include #include -#include #include +#include #include #include #include diff --git a/src/MoveUntilTouchCartVelocityController.cpp b/src/MoveUntilTouchCartVelocityController.cpp index 2f8f0ba..028cbad 100644 --- a/src/MoveUntilTouchCartVelocityController.cpp +++ b/src/MoveUntilTouchCartVelocityController.cpp @@ -1,46 +1,43 @@ #include -#include -#include #include #include +#include +#include -#include #include +#include #define SE3_SIZE 6 -namespace rewd_controllers -{ -namespace -{ +namespace rewd_controllers { +namespace { //============================================================================= -std::vector toVector(const Eigen::VectorXd& input) -{ +std::vector toVector(const Eigen::VectorXd &input) { return std::vector{input.data(), input.data() + input.size()}; } -} // namespace +} // namespace //============================================================================= MoveUntilTouchCartVelocityController::MoveUntilTouchCartVelocityController() - : MultiInterfaceController(true) // allow_optional_interfaces + : MultiInterfaceController(true) // allow_optional_interfaces {} //============================================================================= MoveUntilTouchCartVelocityController::~MoveUntilTouchCartVelocityController() {} //============================================================================= -bool MoveUntilTouchCartVelocityController::init(hardware_interface::RobotHW *robot, - ros::NodeHandle &n) -{ - using hardware_interface::JointStateInterface; +bool MoveUntilTouchCartVelocityController::init( + hardware_interface::RobotHW *robot, ros::NodeHandle &n) { using hardware_interface::JointModeInterface; + using hardware_interface::JointStateInterface; using pr_hardware_interfaces::CartesianVelocityInterface; // Load the URDF as a Skeleton. mSkeleton = loadRobotFromParameter(n, "robot_description_parameter"); - if (!mSkeleton) return false; + if (!mSkeleton) + return false; // Have skeleton update from joint state interface const auto jointStateInterface = robot->get(); @@ -52,7 +49,6 @@ bool MoveUntilTouchCartVelocityController::init(hardware_interface::RobotHW *rob mSkeletonUpdater.reset( new SkeletonJointStateUpdater{mSkeleton, jointStateInterface}); - // Load control interfaces and handles const auto jointModeInterface = robot->get(); if (!jointModeInterface) { @@ -69,7 +65,8 @@ bool MoveUntilTouchCartVelocityController::init(hardware_interface::RobotHW *rob const auto cartVelInterface = robot->get(); if (!cartVelInterface) { - ROS_ERROR("Unable to get CartesianVelocityInterface from RobotHW instance."); + ROS_ERROR( + "Unable to get CartesianVelocityInterface from RobotHW instance."); return false; } @@ -90,7 +87,8 @@ bool MoveUntilTouchCartVelocityController::init(hardware_interface::RobotHW *rob mActionServer.reset(new actionlib::ActionServer{ n, "cart_velocity", std::bind(&MoveUntilTouchCartVelocityController::goalCallback, this, _1), - std::bind(&MoveUntilTouchCartVelocityController::cancelCallback, this, _1), + std::bind(&MoveUntilTouchCartVelocityController::cancelCallback, this, + _1), false}); mActionServer->start(); @@ -99,24 +97,21 @@ bool MoveUntilTouchCartVelocityController::init(hardware_interface::RobotHW *rob } //============================================================================= -void MoveUntilTouchCartVelocityController::starting(const ros::Time& time) -{ +void MoveUntilTouchCartVelocityController::starting(const ros::Time &time) { // Set Joint Mode to Other lastMode = mJointModeHandle.getMode(); mJointModeHandle.setMode(JointModes::NOMODE); } //============================================================================= -void MoveUntilTouchCartVelocityController::stopping(const ros::Time& time) -{ +void MoveUntilTouchCartVelocityController::stopping(const ros::Time &time) { // Return joint mode to what it was before mJointModeHandle.setMode(lastMode); } //============================================================================= void MoveUntilTouchCartVelocityController::update(const ros::Time &time, - const ros::Duration &period) -{ + const ros::Duration &period) { mSkeletonUpdater->update(); Eigen::VectorXd setVelocity; @@ -127,8 +122,7 @@ void MoveUntilTouchCartVelocityController::update(const ros::Time &time, std::shared_ptr context; mCurrentCartVel.get(context); - if (context && !context->mCompleted.load()) - { + if (context && !context->mCompleted.load()) { // check duration ros::Duration runTime = time - context->mStartTime; if (runTime >= context->mDuration) { @@ -150,11 +144,10 @@ void MoveUntilTouchCartVelocityController::update(const ros::Time &time, } //============================================================================= -void MoveUntilTouchCartVelocityController::goalCallback(GoalHandle goalHandle) -{ +void MoveUntilTouchCartVelocityController::goalCallback(GoalHandle goalHandle) { const auto goal = goalHandle.getGoal(); - ROS_INFO_STREAM("Received cartesian velocity '" - << goalHandle.getGoalID().id << "'."); + ROS_INFO_STREAM("Received cartesian velocity '" << goalHandle.getGoalID().id + << "'."); // Setup the new trajectory. const auto newContext = std::make_shared(); @@ -180,15 +173,15 @@ void MoveUntilTouchCartVelocityController::goalCallback(GoalHandle goalHandle) } //============================================================================= -void MoveUntilTouchCartVelocityController::cancelCallback(GoalHandle goalHandle) -{ +void MoveUntilTouchCartVelocityController::cancelCallback( + GoalHandle goalHandle) { ROS_INFO_STREAM("Requesting cancelation of velocity '" << goalHandle.getGoalID().id << "'."); goalHandle.setAccepted(); mCurrentCartVel.set(nullptr); } -} // namespace rewd_controllers +} // namespace rewd_controllers PLUGINLIB_EXPORT_CLASS(rewd_controllers::MoveUntilTouchCartVelocityController, controller_interface::ControllerBase) From dfc815c071336150b7b11ba573f1c7abf816175c Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Tue, 30 Jun 2020 14:25:22 -0700 Subject: [PATCH 08/12] Added class to plugins.xml so it can be seen --- rewd_controllers_plugins.xml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/rewd_controllers_plugins.xml b/rewd_controllers_plugins.xml index 3a02b25..d6f5574 100644 --- a/rewd_controllers_plugins.xml +++ b/rewd_controllers_plugins.xml @@ -68,4 +68,11 @@ SetForceTorqueThreshold service is exceeded. + + + TODO + + From 3456e2c7e95081470bb51a13a63b7436afd0a223 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Tue, 30 Jun 2020 17:26:32 -0700 Subject: [PATCH 09/12] Add FT-sensor functionality --- .../MoveUntilTouchCartVelocityController.hpp | 6 ++ src/MoveUntilTouchCartVelocityController.cpp | 68 +++++++++++++++++-- 2 files changed, 70 insertions(+), 4 deletions(-) diff --git a/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp b/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp index b66a850..3d44c66 100644 --- a/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp +++ b/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp @@ -13,6 +13,8 @@ #include #include +#include + namespace rewd_controllers { class MoveUntilTouchCartVelocityController : public controller_interface::MultiInterfaceController< @@ -61,6 +63,10 @@ class MoveUntilTouchCartVelocityController using JointModes = hardware_interface::JointCommandModes; private: + // \brief Force-Torque Thresholding Server + std::shared_ptr mFTThresholdServer; + bool mUseFT; + /** \brief Actionlib callback to accept new Goal. */ void goalCallback(GoalHandle goalHandle); diff --git a/src/MoveUntilTouchCartVelocityController.cpp b/src/MoveUntilTouchCartVelocityController.cpp index 028cbad..3947fc6 100644 --- a/src/MoveUntilTouchCartVelocityController.cpp +++ b/src/MoveUntilTouchCartVelocityController.cpp @@ -22,7 +22,9 @@ std::vector toVector(const Eigen::VectorXd &input) { //============================================================================= MoveUntilTouchCartVelocityController::MoveUntilTouchCartVelocityController() : MultiInterfaceController(true) // allow_optional_interfaces -{} +{ + mUseFT = false; +} //============================================================================= MoveUntilTouchCartVelocityController::~MoveUntilTouchCartVelocityController() {} @@ -34,6 +36,52 @@ bool MoveUntilTouchCartVelocityController::init( using hardware_interface::JointStateInterface; using pr_hardware_interfaces::CartesianVelocityInterface; + // Enable force/torque functionality if requested + mUseFT = false; + n.getParam("enableFT", mUseFT); + + if(mUseFT) { + // load name of force/torque sensor handle from paramter + std::string ft_wrench_name; + if (!n.getParam("forcetorque_wrench_name", ft_wrench_name)) { + ROS_ERROR("Failed to load 'forcetorque_wrench_name' parameter."); + return false; + } + // load name of force/torque tare handle from paramter + std::string ft_tare_name; + if (!n.getParam("forcetorque_tare_name", ft_tare_name)) { + ROS_ERROR("Failed to load 'forcetorque_tare_name' parameter."); + return false; + } + + // load force/torque saturation limits from parameter + double forceLimit = 0.0; + if (!n.getParam("sensor_force_limit", forceLimit)) { + ROS_ERROR("Failed to load 'sensor_force_limit' parameter."); + return false; + } + double torqueLimit = 0.0; + if (!n.getParam("sensor_torque_limit", torqueLimit)) { + ROS_ERROR("Failed to load 'sensor_torque_limit' parameter."); + return false; + } + if (forceLimit < 0) { + ROS_ERROR("sensor_force_limit must be positive or zero"); + return false; + } + if (torqueLimit < 0) { + ROS_ERROR("sensor_torque_limit must be positive or zero"); + return false; + } + + // Init FT Threshold Server + mFTThresholdServer.reset(new FTThresholdServer{n, + ft_wrench_name, + ft_tare_name, + forceLimit, + torqueLimit}); + } + // Load the URDF as a Skeleton. mSkeleton = loadRobotFromParameter(n, "robot_description_parameter"); if (!mSkeleton) @@ -118,6 +166,13 @@ void MoveUntilTouchCartVelocityController::update(const ros::Time &time, setVelocity.resize(SE3_SIZE); setVelocity.setZero(); + // Check FT Threshold + bool ftThresholdTripped = false; + std::string message = ""; + if(mUseFT) { + ftThresholdTripped = mFTThresholdServer->shouldStopExecution(message); + } + // Load current command std::shared_ptr context; mCurrentCartVel.get(context); @@ -132,9 +187,14 @@ void MoveUntilTouchCartVelocityController::update(const ros::Time &time, result.error_string = ""; context->mGoalHandle.setSucceeded(result); context->mCompleted.store(true); - } - // TODO: check FT sensor - else { + } else if (ftThresholdTripped) { + // Aborted run + Result result; + result.error_code = Result::FORCE_THRESH; + result.error_string = message; + context->mGoalHandle.setAborted(result); + context->mCompleted.store(true); + } else { setVelocity = context->mDesiredVelocity; } } From 4d1e6e4456577c977e55981f71df0a5cf00c8797 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Thu, 2 Jul 2020 20:09:10 -0700 Subject: [PATCH 10/12] Switch to Jacobian-based velocity controller --- .../MoveUntilTouchCartVelocityController.hpp | 6 +- src/MoveUntilTouchCartVelocityController.cpp | 94 ++++++++++++++----- 2 files changed, 75 insertions(+), 25 deletions(-) diff --git a/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp b/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp index 3d44c66..64217c4 100644 --- a/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp +++ b/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp @@ -18,7 +18,7 @@ namespace rewd_controllers { class MoveUntilTouchCartVelocityController : public controller_interface::MultiInterfaceController< - pr_hardware_interfaces::CartesianVelocityInterface, + hardware_interface::VelocityJointInterface, hardware_interface::JointStateInterface, hardware_interface::JointModeInterface> { public: @@ -95,6 +95,8 @@ class MoveUntilTouchCartVelocityController // For position feedback: dart::dynamics::SkeletonPtr mSkeleton; std::unique_ptr mSkeletonUpdater; + dart::dynamics::MetaSkeletonPtr mControlledSkeleton; + std::string mEEName; // Action server variables std::unique_ptr mActionServer; @@ -104,7 +106,7 @@ class MoveUntilTouchCartVelocityController realtime_tools::RealtimeBox mCurrentCartVel; // For communication with robot - pr_hardware_interfaces::CartesianVelocityHandle mCartVelHandle; + std::vector mControlledJointHandles; hardware_interface::JointModeHandle mJointModeHandle; JointModes lastMode; }; diff --git a/src/MoveUntilTouchCartVelocityController.cpp b/src/MoveUntilTouchCartVelocityController.cpp index 3947fc6..c539ef9 100644 --- a/src/MoveUntilTouchCartVelocityController.cpp +++ b/src/MoveUntilTouchCartVelocityController.cpp @@ -6,7 +6,6 @@ #include #include -#include #define SE3_SIZE 6 @@ -34,7 +33,7 @@ bool MoveUntilTouchCartVelocityController::init( hardware_interface::RobotHW *robot, ros::NodeHandle &n) { using hardware_interface::JointModeInterface; using hardware_interface::JointStateInterface; - using pr_hardware_interfaces::CartesianVelocityInterface; + using hardware_interface::VelocityJointInterface; // Enable force/torque functionality if requested mUseFT = false; @@ -97,6 +96,34 @@ bool MoveUntilTouchCartVelocityController::init( mSkeletonUpdater.reset( new SkeletonJointStateUpdater{mSkeleton, jointStateInterface}); + // Build up the list of controlled joints. + const auto jointParameters = loadJointsFromParameter(n, "joints", "velocity"); + if (jointParameters.empty()) + return false; + + ROS_INFO_STREAM("Controlling " << jointParameters.size() << " joints:"); + for (const auto ¶m : jointParameters) { + ROS_INFO_STREAM("- " << param.mName << " (type: " << param.mType << ")"); + + if (param.mType != "velocity") { + ROS_ERROR_STREAM("Joint '" + << param.mName + << "' is not velocity-controlled and cannot be " + "used in a cartesian velocity controller"); + return false; + } + } + + // Extract the subset of the Skeleton that is being controlled. + mControlledSkeleton = + getControlledMetaSkeleton(mSkeleton, jointParameters, "Controlled"); + if (!mControlledSkeleton) { + return false; + } + + const auto numControlledDofs = mControlledSkeleton->getNumDofs(); + mControlledJointHandles.resize(numControlledDofs); + // Load control interfaces and handles const auto jointModeInterface = robot->get(); if (!jointModeInterface) { @@ -110,25 +137,31 @@ bool MoveUntilTouchCartVelocityController::init( ROS_ERROR_STREAM("Unable to get joint mode interface for robot"); return false; } - - const auto cartVelInterface = robot->get(); - if (!cartVelInterface) { - ROS_ERROR( - "Unable to get CartesianVelocityInterface from RobotHW instance."); + const auto velocityJointInterface = robot->get(); + if (!velocityJointInterface) { + ROS_ERROR("Unable to get VelocityJointInterface from RobotHW instance."); return false; } - try { - mCartVelHandle = cartVelInterface->getHandle("cart_vel"); - } catch (const hardware_interface::HardwareInterfaceException &e) { - ROS_ERROR_STREAM("Unable to get cartesian velocity interface for robot."); - return false; + for (size_t idof = 0; idof < numControlledDofs; ++idof) { + const auto dofName = mControlledSkeleton->getDof(idof)->getName(); + try { + auto handle = velocityJointInterface->getHandle(dofName); + mControlledJointHandles[idof] = handle; + } catch (const hardware_interface::HardwareInterfaceException &e) { + ROS_ERROR_STREAM( + "Unable to get interface of type 'VelocityJointInterface' for joint '" + << dofName << "'."); + return false; + } } // init local vars mCurrentCartVel.set(nullptr); - - // TODO: Initialize FT sensor + if (!n.getParam("ee", mEEName)) { + ROS_ERROR_STREAM("Parameter '" << n.getNamespace() + << "/ee' is required."); + } // Start the action server. This must be last. using std::placeholders::_1; @@ -148,7 +181,7 @@ bool MoveUntilTouchCartVelocityController::init( void MoveUntilTouchCartVelocityController::starting(const ros::Time &time) { // Set Joint Mode to Other lastMode = mJointModeHandle.getMode(); - mJointModeHandle.setMode(JointModes::NOMODE); + mJointModeHandle.setMode(JointModes::MODE_VELOCITY); } //============================================================================= @@ -200,7 +233,23 @@ void MoveUntilTouchCartVelocityController::update(const ros::Time &time, } // Execute velocity command - mCartVelHandle.setCommand(toVector(setVelocity)); + /* Get full (with orientation) Jacobian of our end-effector */ + Eigen::VectorXd jointVels(mControlledSkeleton->getNumDofs()); + jointVels.setZero(); + if (setVelocity.norm() != 0) { + Eigen::MatrixXd J = mSkeleton->getBodyNode(mEEName)->getWorldJacobian(); + Eigen::MatrixXd JtJ = J.transpose() * J; + if (JtJ.determinant() != 0) { + jointVels = JtJ.inverse() * J.transpose() * setVelocity; + } else { + ROS_ERROR_STREAM("Error: at singularity. Cartesian control impossible."); + } + } + + for (size_t idof = 0; idof < mControlledJointHandles.size(); ++idof) { + auto jointHandle = mControlledJointHandles[idof]; + jointHandle.setCommand(jointVels(idof)); + } } //============================================================================= @@ -218,12 +267,12 @@ void MoveUntilTouchCartVelocityController::goalCallback(GoalHandle goalHandle) { newContext->mCompleted.store(false); // Copy over velocity data - newContext->mDesiredVelocity(0) = goal->command.linear.x; - newContext->mDesiredVelocity(1) = goal->command.linear.y; - newContext->mDesiredVelocity(2) = goal->command.linear.z; - newContext->mDesiredVelocity(3) = goal->command.angular.x; - newContext->mDesiredVelocity(4) = goal->command.angular.y; - newContext->mDesiredVelocity(5) = goal->command.angular.z; + newContext->mDesiredVelocity(0) = goal->command.angular.x; + newContext->mDesiredVelocity(1) = goal->command.angular.y; + newContext->mDesiredVelocity(2) = goal->command.angular.z; + newContext->mDesiredVelocity(3) = goal->command.linear.x; + newContext->mDesiredVelocity(4) = goal->command.linear.y; + newContext->mDesiredVelocity(5) = goal->command.linear.z; // TODO: add velocity checks @@ -237,7 +286,6 @@ void MoveUntilTouchCartVelocityController::cancelCallback( GoalHandle goalHandle) { ROS_INFO_STREAM("Requesting cancelation of velocity '" << goalHandle.getGoalID().id << "'."); - goalHandle.setAccepted(); mCurrentCartVel.set(nullptr); } From 2821ca7bedfd71900736e07b9081408dc13aa29c Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Thu, 29 Oct 2020 22:12:37 -0700 Subject: [PATCH 11/12] Actually start FTThresholdServer --- src/MoveUntilTouchCartVelocityController.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/MoveUntilTouchCartVelocityController.cpp b/src/MoveUntilTouchCartVelocityController.cpp index c539ef9..c486128 100644 --- a/src/MoveUntilTouchCartVelocityController.cpp +++ b/src/MoveUntilTouchCartVelocityController.cpp @@ -182,12 +182,18 @@ void MoveUntilTouchCartVelocityController::starting(const ros::Time &time) { // Set Joint Mode to Other lastMode = mJointModeHandle.getMode(); mJointModeHandle.setMode(JointModes::MODE_VELOCITY); + + // start FTThresholdServer + mFTThresholdServer->start(); } //============================================================================= void MoveUntilTouchCartVelocityController::stopping(const ros::Time &time) { // Return joint mode to what it was before mJointModeHandle.setMode(lastMode); + + // stop FTThresholdServer + mFTThresholdServer->stop(); } //============================================================================= From c06b845b3caa71e04a6c122b30a6593f6b49a735 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Wed, 23 Dec 2020 17:14:58 -0800 Subject: [PATCH 12/12] Compilation broken, require ControllerState explicitly --- include/rewd_controllers/MultiInterfaceController.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/rewd_controllers/MultiInterfaceController.hpp b/include/rewd_controllers/MultiInterfaceController.hpp index ee8dc5c..d091338 100644 --- a/include/rewd_controllers/MultiInterfaceController.hpp +++ b/include/rewd_controllers/MultiInterfaceController.hpp @@ -201,7 +201,7 @@ class MultiInterfaceController : public ControllerBase { */ MultiInterfaceController(bool allow_optional_interfaces = false) : allow_optional_interfaces_(allow_optional_interfaces) { - state_ = CONSTRUCTED; + state_ = ControllerState::CONSTRUCTED; } virtual ~MultiInterfaceController() {} @@ -300,7 +300,7 @@ class MultiInterfaceController : public ControllerBase { ros::NodeHandle &controller_nh, ClaimedResources &claimed_resources) { // check if construction finished cleanly - if (state_ != CONSTRUCTED) { + if (state_ != ControllerState::CONSTRUCTED) { ROS_ERROR("Cannot initialize this controller because it failed to be " "constructed"); return false; @@ -333,7 +333,7 @@ class MultiInterfaceController : public ControllerBase { // the controller is done when the controller is start()ed // initialization successful - state_ = INITIALIZED; + state_ = ControllerState::INITIALIZED; return true; }