diff --git a/franka_combinable_hw/CMakeLists.txt b/franka_combinable_hw/CMakeLists.txt deleted file mode 100644 index 319b2df18a63b98be38dd986d15bc3dfdca253ab..0000000000000000000000000000000000000000 --- a/franka_combinable_hw/CMakeLists.txt +++ /dev/null @@ -1,95 +0,0 @@ -cmake_minimum_required(VERSION 3.4) -project(franka_combinable_hw) - -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_BUILD_TYPE Release) - -find_package(catkin REQUIRED COMPONENTS - actionlib - combined_robot_hw - controller_interface - franka_hw - franka_control - hardware_interface - joint_limits_interface - pluginlib - roscpp - std_msgs -) - -find_package(Franka 0.5.0 REQUIRED) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES franka_combinable_hw combined_robot_hw - CATKIN_DEPENDS - actionlib - combined_robot_hw - controller_interface - franka_hw - franka_control - hardware_interface - joint_limits_interface - pluginlib - roscpp - std_msgs - DEPENDS Franka -) - -add_library(franka_combinable_hw - src/franka_combinable_hw.cpp - src/franka_combined_hw.cpp -) - -add_dependencies(franka_combinable_hw - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) - -target_link_libraries(franka_combinable_hw - ${Franka_LIBRARIES} - ${catkin_LIBRARIES} -) - -target_include_directories(franka_combinable_hw SYSTEM PUBLIC - ${Franka_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) -target_include_directories(franka_combinable_hw PUBLIC - include -) - -## Installation -install(TARGETS franka_combinable_hw - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) -install(FILES franka_combinable_hw_plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -if(CATKIN_ENABLE_TESTING) - add_subdirectory(test) -endif() - -## Tools -include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL - RESULT_VARIABLE CLANG_TOOLS -) -if(CLANG_TOOLS) - file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp) - file(GLOB_RECURSE HEADERS - ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h - ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h - ) - add_format_target(franka_combinable_hw FILES ${SOURCES} ${HEADERS}) - add_tidy_target(franka_combinable_hw - FILES ${SOURCES} - DEPENDS franka_combinable_hw - ) -endif() diff --git a/franka_combinable_hw/franka_combinable_hw_plugin.xml b/franka_combinable_hw/franka_combinable_hw_plugin.xml deleted file mode 100644 index aa7a5c1aeda0c6619f7b6a6c1762ddc10f4d7e71..0000000000000000000000000000000000000000 --- a/franka_combinable_hw/franka_combinable_hw_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ -<library path="lib/libfranka_combinable_hw"> - <class name="franka_combinable_hw/FrankaCombinableHW" type="franka_combinable_hw::FrankaCombinableHW" base_class_type="hardware_interface::RobotHW"> - <description> - A robot hardware interface class for franka robots that is combinable with other FrankaCombinableHW. - </description> - </class> -</library> diff --git a/franka_combinable_hw/include/franka_combinable_hw/franka_combinable_hw.h b/franka_combinable_hw/include/franka_combinable_hw/franka_combinable_hw.h deleted file mode 100644 index ba8c91e49d1078c9bece5f48962d8e4c7380125a..0000000000000000000000000000000000000000 --- a/franka_combinable_hw/include/franka_combinable_hw/franka_combinable_hw.h +++ /dev/null @@ -1,314 +0,0 @@ -// Copyright (c) 2019 Franka Emika GmbH -// Use of this source code is governed by the Apache-2.0 license, see LICENSE -#pragma once - -#include <array> -#include <atomic> -#include <exception> -#include <functional> -#include <string> -#include <thread> - -#include <actionlib/server/simple_action_server.h> -#include <franka/control_types.h> -#include <franka/duration.h> -#include <franka/exception.h> -#include <franka/model.h> -#include <franka/robot.h> -#include <franka/robot_state.h> -#include <hardware_interface/joint_command_interface.h> -#include <hardware_interface/joint_state_interface.h> -#include <hardware_interface/robot_hw.h> -#include <joint_limits_interface/joint_limits_interface.h> -#include <ros/node_handle.h> -#include <ros/time.h> -#include <urdf/model.h> - -#include <franka_control/ErrorRecoveryAction.h> -#include <franka_control/services.h> -#include <franka_hw/control_mode.h> -#include <franka_hw/franka_cartesian_command_interface.h> -#include <franka_hw/franka_model_interface.h> -#include <franka_hw/franka_state_interface.h> - -using franka_control::ServiceContainer; -using franka_hw::ControlMode; - -namespace franka_combinable_hw { - -class FrankaCombinableHW : public hardware_interface::RobotHW { - public: - /** - * Creates an instance of FrankaCombinableHW. - * - */ - FrankaCombinableHW(); - - /** - * Initializes the FrankaCombinableHW class as far as possible without connecting to a robot. This - * includes setting up state, limit and command interfaces, publishers, services and actions. - * Note: This method is mainly for testing purposes. Use the \ref init() method to control real - * hardware. - * - * @param[in] root_nh A NodeHandle in the root of the caller namespace. - * @param[in] robot_hw_nh A NodeHandle in the namespace from which the RobotHW. - * @return True if the disconnected initialization was successful, false otherwise. - */ - bool initROSInterfaces(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh); - - /** - * Initializes the FrankaCombinableHW with model, state, limit and command interfaces and connects - * to a panda robot via an IP provided via the parameter server. - * - * @param[in] root_nh A NodeHandle in the root of the caller namespace. - * @param[in] robot_hw_nh A NodeHandle in the namespace from which the RobotHW. - * should read its configuration. - * - * @return True if initialization was successful, false otherwise. - */ - bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override; - - ~FrankaCombinableHW() override = default; - - /** - * Runs the currently active controller in a realtime loop. - * - * If no controller is active, the function immediately exits. When running a controller, - * the function only exits when ros_callback returns false. - * - * @param[in] robot A libfranka Robot instance. - * - * @throw franka::ControlException if an error related to torque control or motion generation - * occurred. - * @throw franka::InvalidOperationException if a conflicting operation is already running. - * @throw franka::NetworkException if the connection is lost, e.g. after a timeout. - * @throw franka::RealtimeException if realtime priority cannot be set for the current thread. - */ - void control(franka::Robot& robot); - - /** - * Updates the robot state in the controller interfaces from the given robot state. - * - * @param[in] robot_state Current robot state. - */ - void update(const franka::RobotState& robot_state); - - /** - * Indicates whether there is an active controller. - * - * @return True if a controller is currently active, false otherwise. - */ - bool controllerActive() const noexcept; - - /** - * Checks whether a requested controller can be run, based on the resources and interfaces it - * claims. - * - * @param[in] info Controllers to be running at the same time. - * - * @return True in case of a conflict, false in case of valid controllers. - */ - bool checkForConflict(const std::list<hardware_interface::ControllerInfo>& info) const override; - - /** - * Performs controller switching (real-time capable). - */ - void doSwitch(const std::list<hardware_interface::ControllerInfo>&, - const std::list<hardware_interface::ControllerInfo>&) override; - - /** - * Prepares switching between controllers (not real-time capable). - * - * @param[in] start_list Controllers requested to be started. - * @param[in] stop_list Controllers requested to be stopped. - * - * @return True if the preparation has been successful, false otherwise. - */ - bool prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, - const std::list<hardware_interface::ControllerInfo>& stop_list) override; - - /** - * Reads data from the franka robot. - * - * @param[in] time The current time. - * @param[in] period The time passed since the last call to \ref read. - */ - void read(const ros::Time& time, const ros::Duration& period) override; - - /** - * Writes data to the franka robot. - * - * @param[in] time The current time. - * @param[in] period The time passed since the last call to \ref write. - */ - void write(const ros::Time& time, const ros::Duration& period) override; - - /** - * Gets the current joint torque command. - * - * @return Current joint torque command. - */ - std::array<double, 7> getJointEffortCommand() const noexcept; - - /** - * Enforces limits on torque level. - * - * @param[in] period Duration of the current cycle. - */ - void enforceLimits(const ros::Duration& period); - - /** - * Gets arm id. - */ - std::string getArmID(); - - /** - * Trigger error state to stop controller. - */ - void triggerError(); - - /** - * Returns error state. - * - * @return true if there is currently error. - */ - bool hasError(); - - /** - * Reset error state. - */ - void resetError(); - - /** - * Returns whether the controller needs to be reset. - */ - bool controllerNeedsReset(); - - /** - * Returns whether the torque command contains NaN values. - * - * @param[in] command The torque commmand to check. - * - * @return true if the command contains NaN, false otherwise. - */ - static bool commandHasNaN(const franka::Torques& command); - - /** - * Returns whether the Cartesian pose command contains NaN values. - * - * @param[in] command The Cartesian pose commmand to check. - * - * @return true if the command contains NaN, false otherwise. - */ - static bool commandHasNaN(const franka::CartesianPose& command); - - /** - * Returns whether the Cartesian velocity command contains NaN values. - * - * @param[in] command The Cartesian velocity commmand to check. - * - * @return true if the command contains NaN, false otherwise. - */ - static bool commandHasNaN(const franka::CartesianVelocities& command); - - private: - template <size_t size> - static bool arrayHasNaN(const std::array<double, size> array) { - return std::any_of(array.begin(), array.end(), [](const double& e) { return std::isnan(e); }); - } - - template <typename T> - T controlCallback(const T& command, - const franka::RobotState& robot_state, - franka::Duration time_step) { - if (commandHasNaN(command)) { - ROS_ERROR("FrankaCombinableHW: Got NaN value in command!"); - throw franka::CommandException("Got NaN value in command"); - } - - checkJointLimits(); - - libfranka_state_mutex_.lock(); - robot_state_libfranka_ = robot_state; - libfranka_state_mutex_.unlock(); - - libfranka_cmd_mutex_.lock(); - T current_cmd = command; - libfranka_cmd_mutex_.unlock(); - - if (has_error_ || !controller_active_) { - return franka::MotionFinished(current_cmd); - } - return current_cmd; - } - - void controlLoop(); - - void setupServicesAndActionServers(ros::NodeHandle& node_handle); - - static std::array<double, 7> saturateTorqueRate(const std::array<double, 7>& tau_d_calculated, - const std::array<double, 7>& tau_J_d); - - bool initRobot(ros::NodeHandle& robot_hw_nh); - - void publishErrorState(bool error); - - void checkJointLimits(); - - std::unique_ptr<franka::Robot> robot_; - std::unique_ptr<franka::Model> model_; - - hardware_interface::JointStateInterface joint_state_interface_{}; - franka_hw::FrankaStateInterface franka_state_interface_{}; - hardware_interface::EffortJointInterface effort_joint_interface_{}; - franka_hw::FrankaPoseCartesianInterface franka_pose_cartesian_interface_{}; - franka_hw::FrankaVelocityCartesianInterface franka_velocity_cartesian_interface_{}; - franka_hw::FrankaModelInterface franka_model_interface_{}; - - joint_limits_interface::EffortJointSoftLimitsInterface effort_joint_limit_interface_{}; - - urdf::Model urdf_model_; - - franka::RobotState robot_state_libfranka_{}; - franka::RobotState robot_state_ros_{}; - - std::mutex libfranka_state_mutex_; - std::mutex ros_state_mutex_; - - std::array<std::string, 7> joint_names_; - std::string arm_id_; - std::string robot_ip_; - - // command data of libfranka - std::mutex libfranka_cmd_mutex_; - franka::Torques effort_joint_command_libfranka_; - franka::CartesianPose pose_cartesian_command_libfranka_; - franka::CartesianVelocities velocity_cartesian_command_libfranka_; - - // command data of frankahw ros - std::mutex ros_cmd_mutex_; - franka::Torques effort_joint_command_ros_; - franka::CartesianPose pose_cartesian_command_ros_; - franka::CartesianVelocities velocity_cartesian_command_ros_; - - std::function<void(franka::Robot&)> run_function_; - - bool limit_rate_{true}; - std::atomic_bool controller_active_{false}; - ControlMode current_control_mode_ = ControlMode::None; - double joint_limit_warning_threshold_{10 * 3.14 / 180}; - - std::unique_ptr<std::thread> control_loop_thread_; - - ros::Publisher has_error_pub_; - ServiceContainer services_; - std::unique_ptr<actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction>> - recovery_action_server_; - - std::atomic<bool> has_error_; - std::atomic<bool> error_recovered_; - bool controller_needs_reset_; - bool initialized_ = false; -}; - -} // namespace franka_combinable_hw diff --git a/franka_combinable_hw/mainpage.dox b/franka_combinable_hw/mainpage.dox deleted file mode 100644 index 941d0bf97714038b58c39c7dcd816e901e22c023..0000000000000000000000000000000000000000 --- a/franka_combinable_hw/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** - * @mainpage - * @htmlinclude "manifest.html" - * - * Overview page for Franka Emika research robots: https://frankaemika.github.io - */ diff --git a/franka_combinable_hw/package.xml b/franka_combinable_hw/package.xml deleted file mode 100644 index 62015d2c745938e8e8708a67008609e6a402211d..0000000000000000000000000000000000000000 --- a/franka_combinable_hw/package.xml +++ /dev/null @@ -1,35 +0,0 @@ -<?xml version="1.0"?> -<package format="2"> - <name>franka_combinable_hw</name> - <version>0.7.0</version> - <description>franka_combinable_hw provides hardware interfaces for using multiple Panda robots with ros_control</description> - <maintainer email="support@franka.de">Franka Emika GmbH</maintainer> - <license>Apache 2.0</license> - - <url type="website">http://wiki.ros.org/franka_combinable_hw</url> - <url type="repository">https://github.com/frankaemika/franka_ros</url> - <url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url> - <author>Franka Emika GmbH</author> - - <buildtool_depend>catkin</buildtool_depend> - - <depend>libfranka</depend> - <depend>actionlib</depend> - <depend>pluginlib</depend> - <depend>controller_interface</depend> - <depend>franka_hw</depend> - <depend>franka_control</depend> - <depend>hardware_interface</depend> - <depend>joint_limits_interface</depend> - <depend>combined_robot_hw</depend> - <depend>roscpp</depend> - <depend>std_msgs</depend> - - <test_depend>franka_description</test_depend> - <test_depend>gtest</test_depend> - <test_depend>rostest</test_depend> - - <export> - <hardware_interface plugin="${prefix}/franka_combinable_hw_plugin.xml"/> - </export> -</package> diff --git a/franka_combinable_hw/rosdoc.yaml b/franka_combinable_hw/rosdoc.yaml deleted file mode 100644 index 96ee597ef5cacd0f223f676c738f396f0810b78c..0000000000000000000000000000000000000000 --- a/franka_combinable_hw/rosdoc.yaml +++ /dev/null @@ -1,2 +0,0 @@ -- builder: doxygen - javadoc_autobrief: YES diff --git a/franka_combinable_hw/src/franka_combinable_hw.cpp b/franka_combinable_hw/src/franka_combinable_hw.cpp deleted file mode 100644 index 7de5008d02448cf58a952b910b83ee4a95a24512..0000000000000000000000000000000000000000 --- a/franka_combinable_hw/src/franka_combinable_hw.cpp +++ /dev/null @@ -1,663 +0,0 @@ -// Copyright (c) 2019 Franka Emika GmbH -// Use of this source code is governed by the Apache-2.0 license, see LICENSE - -#include <franka/control_types.h> -#include <franka/exception.h> -#include <franka/rate_limiting.h> -#include <franka_combinable_hw/franka_combinable_hw.h> -#include <franka_control/ErrorRecoveryAction.h> -#include <franka_hw/franka_cartesian_command_interface.h> -#include <franka_hw/franka_model_interface.h> -#include <franka_hw/franka_state_interface.h> -#include <franka_hw/resource_helpers.h> - -#include <pluginlib/class_list_macros.h> - -#include <cstdint> -#include <exception> - -#include <hardware_interface/hardware_interface.h> -#include <joint_limits_interface/joint_limits_urdf.h> -#include <std_msgs/Bool.h> - -using franka_control::ErrorRecoveryResult; -using franka_hw::ArmClaimedMap; -using franka_hw::getResourceMap; -using franka_hw::ResourceWithClaimsMap; - -namespace franka_combinable_hw { - -FrankaCombinableHW::FrankaCombinableHW() - : effort_joint_command_libfranka_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), - pose_cartesian_command_libfranka_( - {1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}), - velocity_cartesian_command_libfranka_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), - effort_joint_command_ros_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), - pose_cartesian_command_ros_( - {1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}), - velocity_cartesian_command_ros_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), - has_error_(false), - error_recovered_(false) {} - -/* - * Data from ROS parameter server: - * 1. const std::array<std::string, 7>& joint_names - * 2. const std::string& arm_id - * - : joint_names_(joint_names), - arm_id_(arm_id), -*/ -bool FrankaCombinableHW::initROSInterfaces(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { - if (initialized_) { - ROS_ERROR("FrankaCombinableHW: Cannot be initialized twice."); - return false; - } - - // initialize arm_id and joint_names - if (!robot_hw_nh.getParam("arm_id", arm_id_)) { - ROS_ERROR("FrankaCombinableHW: no parameter arm_id is found."); - return false; - } - - std::vector<std::string> joint_names; - if (robot_hw_nh.getParam("joint_names", joint_names) && - joint_names.size() == joint_names_.size()) { - std::copy_n(joint_names.begin(), joint_names.size(), joint_names_.begin()); - } else { - ROS_ERROR("FrankaCombinableHW: no valid parameter joint_names is found."); - return false; - } - - if (!robot_hw_nh.getParam("joint_limit_warning_threshold", arm_id_)) { - ROS_INFO( - "FrankaCombinableHW: no parameter joint_limit_warning_threshold is found, using default " - "value %f", - joint_limit_warning_threshold_); - } - - for (size_t i = 0; i < joint_names_.size(); ++i) { - hardware_interface::JointStateHandle joint_handle_q(joint_names_[i], &robot_state_ros_.q[i], - &robot_state_ros_.dq[i], - &robot_state_ros_.tau_J[i]); - - joint_state_interface_.registerHandle(joint_handle_q); - - hardware_interface::JointHandle effort_joint_handle(joint_handle_q, - &effort_joint_command_ros_.tau_J[i]); - effort_joint_interface_.registerHandle(effort_joint_handle); - } - - if (root_nh.hasParam("robot_description")) { - if (!urdf_model_.initParamWithNodeHandle("robot_description", root_nh)) { - ROS_ERROR("FrankaCombinableHW: Could not initialize urdf model from robot_description"); - } else { - joint_limits_interface::SoftJointLimits soft_limits; - joint_limits_interface::JointLimits joint_limits; - - for (const auto& joint_name : joint_names_) { - int joint_index(std::stoi(joint_name.substr(joint_name.size() - 1)) - 1); - auto urdf_joint = urdf_model_.getJoint(joint_name); - if (!urdf_joint) { - ROS_ERROR_STREAM("FrankaCombinableHW: Could not get joint " << joint_name - << " from urdf"); - } - if (!urdf_joint->safety) { - ROS_ERROR_STREAM("FrankaCombinableHW: Joint " << joint_name << " has no safety"); - } - if (!urdf_joint->limits) { - ROS_ERROR_STREAM("FrankaCombinableHW: Joint " << joint_name << " has no limits"); - } - - if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) { - if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits)) { - joint_limits.max_acceleration = franka::kMaxJointAcceleration[joint_index]; - joint_limits.has_acceleration_limits = true; - joint_limits.max_jerk = franka::kMaxJointJerk[joint_index]; - joint_limits.has_jerk_limits = true; - joint_limits_interface::EffortJointSoftLimitsHandle effort_limit_handle( - effort_joint_interface_.getHandle(joint_name), joint_limits, soft_limits); - effort_joint_limit_interface_.registerHandle(effort_limit_handle); - } else { - ROS_ERROR_STREAM("FrankaCombinableHW: Could not parse joint limit for joint " - << joint_name << " for joint limit interfaces"); - } - } else { - ROS_ERROR_STREAM("FrankaCombinableHW: Could not parse soft joint limit for joint " - << joint_name << " for joint limit interfaces"); - } - } - } - } else { - ROS_WARN("FrankaCombinableHW: No parameter robot_description found to set joint limits!"); - } - - franka_hw::FrankaStateHandle franka_state_handle(arm_id_ + "_robot", robot_state_ros_); - franka_state_interface_.registerHandle(franka_state_handle); - franka_hw::FrankaCartesianPoseHandle franka_cartesian_pose_handle( - franka_state_handle, pose_cartesian_command_ros_.O_T_EE, pose_cartesian_command_ros_.elbow); - franka_pose_cartesian_interface_.registerHandle(franka_cartesian_pose_handle); - franka_hw::FrankaCartesianVelocityHandle franka_cartesian_velocity_handle( - franka_state_handle, velocity_cartesian_command_ros_.O_dP_EE, - velocity_cartesian_command_ros_.elbow); - franka_velocity_cartesian_interface_.registerHandle(franka_cartesian_velocity_handle); - - registerInterface(&franka_state_interface_); - registerInterface(&joint_state_interface_); - registerInterface(&effort_joint_interface_); - registerInterface(&franka_pose_cartesian_interface_); - registerInterface(&franka_velocity_cartesian_interface_); - - // Setup error state publisher - has_error_pub_ = robot_hw_nh.advertise<std_msgs::Bool>("has_error", 1, true); - publishErrorState(has_error_); - - // Setup ROS services and action server - setupServicesAndActionServers(robot_hw_nh); - - initialized_ = true; - return true; -} - -/* - * Data from ROS parameter server: - * 1. const std::array<std::string, 7>& joint_names - * 2. const std::string& arm_id - * - : joint_names_(joint_names), - arm_id_(arm_id), -*/ -bool FrankaCombinableHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { - // Initialize ROS-interfaces - if (not initROSInterfaces(root_nh, robot_hw_nh)) { - ROS_ERROR("FrankaCombinableHW: Failed to initialize interfaces."); - return false; - } - - // Initialize and connect to franka::Robot, register model interface and start control loop - try { - if (!initRobot(robot_hw_nh)) { - ROS_ERROR("FrankaCombinableHW: Failed to initialize libfranka robot."); - return false; - } - } catch (const std::runtime_error& error) { - ROS_ERROR("FrankaCombinableHW: Failed to initialize libfranka robot. %s", error.what()); - return false; - } - - return true; -} - -bool FrankaCombinableHW::initRobot(ros::NodeHandle& robot_hw_nh) { - if (!robot_hw_nh.getParam("robot_ip", robot_ip_)) { - ROS_ERROR("FrankaCombinableHW: no parameter robot_ip is found."); - return false; - } - - robot_ = std::make_unique<franka::Robot>(robot_ip_); - - // Set default collision behaviour - robot_->setCollisionBehavior( - {{40.0, 40.0, 38.0, 38.0, 32.0, 30.0, 24.0}}, {{40.0, 40.0, 38.0, 36.0, 32.0, 30.0, 24.0}}, - {{40.0, 40.0, 38.0, 38.0, 32.0, 30.0, 24.0}}, {{40.0, 40.0, 38.0, 36.0, 32.0, 30.0, 24.0}}, - {{40.0, 40.0, 40.0, 50.0, 50.0, 50.0}}, {{40.0, 40.0, 40.0, 50.0, 50.0, 50.0}}, - {{40.0, 40.0, 40.0, 50.0, 50.0, 50.0}}, {{40.0, 40.0, 40.0, 50.0, 50.0, 50.0}}); - - // Initialize robot state before loading any controller - update(robot_->readOnce()); - - model_ = std::make_unique<franka::Model>(robot_->loadModel()); - franka_hw::FrankaModelHandle model_handle(arm_id_ + "_model", *model_, robot_state_ros_); - franka_model_interface_.registerHandle(model_handle); - registerInterface(&franka_model_interface_); - control_loop_thread_ = std::make_unique<std::thread>(&FrankaCombinableHW::controlLoop, this); - return true; -} - -void FrankaCombinableHW::publishErrorState(bool error) { - std_msgs::Bool msg; - msg.data = error; // NOLINT (readability-implicit-bool-cast) - has_error_pub_.publish(msg); -} - -void FrankaCombinableHW::checkJointLimits() { - // Check joint limits and print warning if any joint is close to limit - std::string joint_limits_warning; - for (const auto& k_joint_name : joint_names_) { - try { - auto joint_handle = joint_state_interface_.getHandle(k_joint_name); - auto urdf_joint = urdf_model_.getJoint(k_joint_name); - joint_limits_interface::JointLimits joint_limits; - if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits)) { - double joint_lower = joint_limits.min_position; - double joint_upper = joint_limits.max_position; - double joint_position = joint_handle.getPosition(); - double dist = fmin(fabs(joint_position - joint_lower), fabs(joint_position - joint_upper)); - if (dist < joint_limit_warning_threshold_) { - joint_limits_warning += "\n\t" + k_joint_name + ": " + std::to_string(dist * 180 / 3.14) + - " degrees to joint limits (limits: [" + - std::to_string(joint_lower) + ", " + std::to_string(joint_upper) + - "]" + " q: " + std::to_string(joint_position) + ") "; - } - } else { - ROS_ERROR_STREAM_ONCE("FrankaCombinableHW: Could not parse joint limit for joint " - << k_joint_name << " for joint limit interfaces"); - } - } catch (const hardware_interface::HardwareInterfaceException& ex) { - ROS_ERROR_STREAM_ONCE("FrankaCombinableHW: Could not get joint handle " << k_joint_name - << " .\n" - << ex.what()); - return; - } - } - - if (!joint_limits_warning.empty()) { - ROS_WARN_THROTTLE(5, "FrankaCombinableHW: %s", joint_limits_warning.c_str()); - } -} - -void FrankaCombinableHW::update(const franka::RobotState& robot_state) { - ros_state_mutex_.lock(); - robot_state_ros_ = robot_state; - ros_state_mutex_.unlock(); -} - -bool FrankaCombinableHW::controllerActive() const noexcept { - return controller_active_; -} - -void FrankaCombinableHW::controlLoop() { - while (ros::ok()) { - ros::Time last_time = ros::Time::now(); - - // Wait until controller has been activated or error has been recovered - while (!controllerActive() || has_error_) { - if (!controllerActive()) { - ROS_DEBUG_THROTTLE(1, "FrankaCombinableHW::%s::control_loop(): controller is not active.", - arm_id_.c_str()); - } - if (has_error_) { - ROS_DEBUG_THROTTLE(1, "FrankaCombinableHW::%s::control_loop(): an error has occured.", - arm_id_.c_str()); - } - - checkJointLimits(); - - ros_state_mutex_.lock(); - libfranka_state_mutex_.lock(); - robot_state_libfranka_ = robot_->readOnce(); - robot_state_ros_ = robot_->readOnce(); - libfranka_state_mutex_.unlock(); - ros_state_mutex_.unlock(); - if (!ros::ok()) { - return; - } - } - - ROS_INFO("FrankaCombinableHW::%s::control_loop(): controller is active.", arm_id_.c_str()); - - // Reset commands - libfranka_cmd_mutex_.lock(); - pose_cartesian_command_libfranka_ = franka::CartesianPose(robot_state_libfranka_.O_T_EE_d); - effort_joint_command_libfranka_ = franka::Torques({0., 0., 0., 0., 0., 0., 0.}); - velocity_cartesian_command_libfranka_ = franka::CartesianVelocities({0., 0., 0., 0., 0., 0.}); - libfranka_cmd_mutex_.unlock(); - - try { - // Run control loop. It will exit if the controller is switched. - control(*robot_); - } catch (const franka::ControlException& e) { - // Reflex could be caught and it needs to wait for automatic error recovery - ROS_ERROR("%s", e.what()); - has_error_ = true; - publishErrorState(has_error_); - } - } -} - -void FrankaCombinableHW::setupServicesAndActionServers(ros::NodeHandle& node_handle) { - services_ - .advertiseService<franka_control::SetJointImpedance>( - node_handle, "set_joint_impedance", - [this](auto&& req, auto&& res) { - return franka_control::setJointImpedance(*(this->robot_), req, res); - }) - .advertiseService<franka_control::SetCartesianImpedance>( - node_handle, "set_cartesian_impedance", - [this](auto&& req, auto&& res) { - return franka_control::setCartesianImpedance(*(this->robot_), req, res); - }) - .advertiseService<franka_control::SetEEFrame>(node_handle, "set_EE_frame", - [this](auto&& req, auto&& res) { - return franka_control::setEEFrame( - *(this->robot_), req, res); - }) - .advertiseService<franka_control::SetKFrame>(node_handle, "set_K_frame", - [this](auto&& req, auto&& res) { - return franka_control::setKFrame( - *(this->robot_), req, res); - }) - .advertiseService<franka_control::SetForceTorqueCollisionBehavior>( - node_handle, "set_force_torque_collision_behavior", - [this](auto&& req, auto&& res) { - return franka_control::setForceTorqueCollisionBehavior(*(this->robot_), req, res); - }) - .advertiseService<franka_control::SetFullCollisionBehavior>( - node_handle, "set_full_collision_behavior", - [this](auto&& req, auto&& res) { - return franka_control::setFullCollisionBehavior(*(this->robot_), req, res); - }) - .advertiseService<franka_control::SetLoad>( - node_handle, "set_load", [this](auto&& req, auto&& res) { - return franka_control::setLoad(*(this->robot_), req, res); - }); - - recovery_action_server_ = - std::make_unique<actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction>>( - node_handle, "error_recovery", - [&](const franka_control::ErrorRecoveryGoalConstPtr&) { - try { - robot_->automaticErrorRecovery(); - // error recovered => reset controller - if (has_error_) { - error_recovered_ = true; - } - has_error_ = false; - publishErrorState(has_error_); - recovery_action_server_->setSucceeded(); - } catch (const franka::Exception& ex) { - recovery_action_server_->setAborted(ErrorRecoveryResult(), ex.what()); - } - }, - false); - recovery_action_server_->start(); -} - -void FrankaCombinableHW::control(franka::Robot& robot) { - if (!controller_active_) { - return; - } - run_function_(robot); -} - -void FrankaCombinableHW::enforceLimits(const ros::Duration& period) { - if (period.toSec() > 0.0) { - effort_joint_limit_interface_.enforceLimits(period); - } -} - -bool FrankaCombinableHW::checkForConflict( - const std::list<hardware_interface::ControllerInfo>& info) const { - ResourceWithClaimsMap resource_map = getResourceMap(info); - // check for conflicts in single resources: no triple claims, - // for 2 claims it must be one torque and one non-torque claim - for (auto map_it = resource_map.begin(); map_it != resource_map.end(); map_it++) { - if (map_it->second.size() > 2) { - ROS_ERROR_STREAM("FrankaCombinableHW: Resource " - << map_it->first << " claimed with more than two interfaces. Conflict!"); - return true; - } - uint8_t torque_claims = 0; - uint8_t other_claims = 0; - if (map_it->second.size() == 2) { - for (auto& claimed_by : map_it->second) { - if (claimed_by[2] == "hardware_interface::EffortJointInterface") { - torque_claims++; - } else { - other_claims++; - } - } - if (torque_claims == 0) { - ROS_ERROR_STREAM("FrankaCombinableHW: Resource " - << map_it->first - << " is claimed with two non-compatible interfaces. Conflict!"); - return true; - } - } - } - - ArmClaimedMap arm_claim_map; - if (!getArmClaimedMap(resource_map, arm_claim_map)) { - ROS_ERROR("FrankaCombinableHW: Unknown interface claimed. Conflict!"); - return true; - } - - // check for any claim to joint_position or joint velocity interface which are not supported - if (arm_claim_map.find(arm_id_) != arm_claim_map.end()) { - if (arm_claim_map[arm_id_].joint_position_claims + - arm_claim_map[arm_id_].joint_velocity_claims > - 0) { - ROS_ERROR_STREAM("FrankaCombinableHW: Invalid claim joint position or velocity interface." - << "Note: joint position and joint velocity interfaces are not supported" - << " in FrankaCombinableHW. Arm:" << arm_id_ << ". Conflict!"); - return true; - } - } - - // check for any claim to cartesian interfaces without torque interface - if (arm_claim_map.find(arm_id_) != arm_claim_map.end()) { - if (arm_claim_map[arm_id_].cartesian_velocity_claims + - arm_claim_map[arm_id_].cartesian_pose_claims > - 0 && - arm_claim_map[arm_id_].joint_torque_claims == 0) { - ROS_ERROR_STREAM( - "FrankaCombinableHW: Invalid claim cartesian interface without joint torque" - " interface. Arm:" - << arm_id_ << ". Conflict!"); - return true; - } - } - - // check for conflicts between joint and cartesian level for each arm. - // Valid claims are torque claims on joint level in combination with either - // 7 non-torque claims on joint_level or one claim on cartesian level. - if (arm_claim_map.find(arm_id_) != arm_claim_map.end()) { - if ((arm_claim_map[arm_id_].cartesian_velocity_claims + - arm_claim_map[arm_id_].cartesian_pose_claims > - 0 && - arm_claim_map[arm_id_].joint_position_claims + - arm_claim_map[arm_id_].joint_velocity_claims > - 0)) { - ROS_ERROR_STREAM("FrankaCombinableHW: Invalid claims on joint AND cartesian level on arm " - << arm_id_ << ". Conflict!"); - return true; - } - if ((arm_claim_map[arm_id_].joint_position_claims > 0 && - arm_claim_map[arm_id_].joint_position_claims != 7) || - (arm_claim_map[arm_id_].joint_velocity_claims > 0 && - arm_claim_map[arm_id_].joint_velocity_claims != 7) || - (arm_claim_map[arm_id_].joint_torque_claims > 0 && - arm_claim_map[arm_id_].joint_torque_claims != 7)) { - ROS_ERROR_STREAM("FrankaCombinableHW: Non-consistent claims on the joints of " - << arm_id_ << ". Not supported. Conflict!"); - return true; - } - } - return false; -} - -// doSwitch runs on the main realtime thread -void FrankaCombinableHW::doSwitch( - const std::list<hardware_interface::ControllerInfo>& /* start_list */, - const std::list<hardware_interface::ControllerInfo>& /* stop_list */) { - if (current_control_mode_ != ControlMode::None) { - controller_active_ = true; - } -} - -// prepareSwitch runs on the background message handling thread -bool FrankaCombinableHW::prepareSwitch( - const std::list<hardware_interface::ControllerInfo>& start_list, - const std::list<hardware_interface::ControllerInfo>& stop_list) { - ResourceWithClaimsMap start_resource_map = getResourceMap(start_list); - ArmClaimedMap start_arm_claim_map; - if (!getArmClaimedMap(start_resource_map, start_arm_claim_map)) { - ROS_ERROR("FrankaCombinableHW: Unknown interface claimed for starting!"); - return false; - } - ControlMode start_control_mode = getControlMode(arm_id_, start_arm_claim_map); - - ResourceWithClaimsMap stop_resource_map = getResourceMap(stop_list); - ArmClaimedMap stop_arm_claim_map; - if (!getArmClaimedMap(stop_resource_map, stop_arm_claim_map)) { - ROS_ERROR("FrankaCombinableHW: Unknown interface claimed for stopping!"); - return false; - } - ControlMode stop_control_mode = getControlMode(arm_id_, stop_arm_claim_map); - - ControlMode requested_control_mode = current_control_mode_; - requested_control_mode |= start_control_mode; - requested_control_mode &= ~stop_control_mode; - - using std::placeholders::_1; - using std::placeholders::_2; - - switch (requested_control_mode) { - case ControlMode::None: - break; - case ControlMode::JointTorque: - run_function_ = [this](franka::Robot& robot) { - robot.control(std::bind(&FrankaCombinableHW::controlCallback<franka::Torques>, this, - std::cref(effort_joint_command_libfranka_), _1, _2), - limit_rate_); - }; - break; - case ControlMode::JointPosition: - ROS_WARN("FrankaCombinableHW: No valid control mode selected; cannot switch controllers."); - return false; - case ControlMode::JointVelocity: - ROS_WARN("FrankaCombinableHW: No valid control mode selected; cannot switch controllers."); - return false; - case ControlMode::CartesianPose: - ROS_WARN("FrankaCombinableHW: No valid control mode selected; cannot switch controllers."); - return false; - case ControlMode::CartesianVelocity: - ROS_WARN("FrankaCombinableHW: No valid control mode selected; cannot switch controllers."); - return false; - case (ControlMode::JointTorque | ControlMode::JointPosition): - ROS_WARN("FrankaCombinableHW: No valid control mode selected; cannot switch controllers."); - return false; - case (ControlMode::JointTorque | ControlMode::JointVelocity): - ROS_WARN("FrankaCombinableHW: No valid control mode selected; cannot switch controllers."); - return false; - case (ControlMode::JointTorque | ControlMode::CartesianPose): - run_function_ = [this](franka::Robot& robot) { - robot.control(std::bind(&FrankaCombinableHW::controlCallback<franka::Torques>, this, - std::cref(effort_joint_command_libfranka_), _1, _2), - std::bind(&FrankaCombinableHW::controlCallback<franka::CartesianPose>, this, - std::cref(pose_cartesian_command_libfranka_), _1, _2), - limit_rate_); - }; - break; - case (ControlMode::JointTorque | ControlMode::CartesianVelocity): - run_function_ = [this](franka::Robot& robot) { - robot.control(std::bind(&FrankaCombinableHW::controlCallback<franka::Torques>, this, - std::cref(effort_joint_command_libfranka_), _1, _2), - std::bind(&FrankaCombinableHW::controlCallback<franka::CartesianVelocities>, - this, std::cref(velocity_cartesian_command_libfranka_), _1, _2), - limit_rate_); - }; - break; - default: - ROS_WARN("FrankaCombinableHW: No valid control mode selected; cannot switch controllers."); - return false; - } - - if (current_control_mode_ != requested_control_mode) { - ROS_INFO_STREAM("FrankaCombinableHW: Prepared switching controllers to " - << requested_control_mode); - current_control_mode_ = requested_control_mode; - - controller_active_ = false; - } - - return true; -} - -void FrankaCombinableHW::read(const ros::Time&, // NOLINT (readability-identifier-naming) - const ros::Duration&) { // NOLINT [readability-named-parameter] - controller_needs_reset_ = error_recovered_; - ros_state_mutex_.lock(); - libfranka_state_mutex_.lock(); - robot_state_ros_ = robot_state_libfranka_; - libfranka_state_mutex_.unlock(); - ros_state_mutex_.unlock(); -} - -void FrankaCombinableHW::write(const ros::Time&, // NOLINT (readability-identifier-naming) - const ros::Duration& period) { - // if flag `controller_needs_reset_` was updated, then controller_manager. update(..., - // reset_controller) must - // have been executed to reset the controller. - if (controller_needs_reset_ && error_recovered_) { - controller_needs_reset_ = false; - error_recovered_ = false; - } - enforceLimits(period); - ros_cmd_mutex_.lock(); - libfranka_cmd_mutex_.lock(); - effort_joint_command_libfranka_.tau_J = - saturateTorqueRate(effort_joint_command_ros_.tau_J, robot_state_libfranka_.tau_J_d); - pose_cartesian_command_libfranka_ = pose_cartesian_command_ros_; - velocity_cartesian_command_libfranka_ = velocity_cartesian_command_ros_; - libfranka_cmd_mutex_.unlock(); - ros_cmd_mutex_.unlock(); -} - -std::array<double, 7> FrankaCombinableHW::saturateTorqueRate( - const std::array<double, 7>& tau_d_calculated, - const std::array<double, 7>& tau_J_d) { // NOLINT (readability-identifier-naming) - const double kDeltaTauMax = 1.0; - std::array<double, 7> tau_d_saturated{}; - for (size_t i = 0; i < 7; i++) { - double difference = tau_d_calculated[i] - tau_J_d[i]; - tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax); - } - return tau_d_saturated; -} - -std::array<double, 7> FrankaCombinableHW::getJointEffortCommand() const noexcept { - return effort_joint_command_libfranka_.tau_J; -} - -std::string FrankaCombinableHW::getArmID() { - return arm_id_; -} - -void FrankaCombinableHW::triggerError() { - has_error_ = true; - publishErrorState(has_error_); -} - -bool FrankaCombinableHW::hasError() { - return has_error_; -} - -void FrankaCombinableHW::resetError() { - robot_->automaticErrorRecovery(); - // error recovered => reset controller - if (has_error_) { - error_recovered_ = true; - } - has_error_ = false; - publishErrorState(has_error_); -} - -bool FrankaCombinableHW::controllerNeedsReset() { - return controller_needs_reset_; -} - -bool FrankaCombinableHW::commandHasNaN(const franka::Torques& command) { - return arrayHasNaN(command.tau_J); -} - -bool FrankaCombinableHW::commandHasNaN(const franka::CartesianPose& command) { - return arrayHasNaN(command.elbow) || arrayHasNaN(command.O_T_EE); -} - -bool FrankaCombinableHW::commandHasNaN(const franka::CartesianVelocities& command) { - return arrayHasNaN(command.elbow) || arrayHasNaN(command.O_dP_EE); -} - -} // namespace franka_combinable_hw - -PLUGINLIB_EXPORT_CLASS(franka_combinable_hw::FrankaCombinableHW, hardware_interface::RobotHW) diff --git a/franka_combinable_hw/test/CMakeLists.txt b/franka_combinable_hw/test/CMakeLists.txt deleted file mode 100644 index ffd3a6afb628895dc26e950485261a9eef396686..0000000000000000000000000000000000000000 --- a/franka_combinable_hw/test/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -find_package(rostest REQUIRED) - -add_rostest_gtest(franka_combinable_hw_test - launch/franka_combinable_hw_test.test - main.cpp - franka_combinable_hw_controller_switching_test.cpp -) - -add_dependencies(franka_combinable_hw_test - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) - -target_link_libraries(franka_combinable_hw_test - ${catkin_LIBRARIES} - Franka::Franka - franka_combinable_hw -) - -target_include_directories(franka_combinable_hw_test PUBLIC - ${catkin_INCLUDE_DIRS} -) - diff --git a/franka_combinable_hw/test/config/franka_combinable_hw_test_node.yaml b/franka_combinable_hw/test/config/franka_combinable_hw_test_node.yaml deleted file mode 100644 index 7ce0b485fb47754662062033845799206a4d81b6..0000000000000000000000000000000000000000 --- a/franka_combinable_hw/test/config/franka_combinable_hw_test_node.yaml +++ /dev/null @@ -1,13 +0,0 @@ -robot_hardware: - - panda -panda: - type: franka_combinable_hw/FrankaCombinableHW - arm_id: panda - joint_names: - - panda_joint1 - - panda_joint2 - - panda_joint3 - - panda_joint4 - - panda_joint5 - - panda_joint6 - - panda_joint7 diff --git a/franka_combinable_hw/test/config/ros_console_settings_for_tests.conf b/franka_combinable_hw/test/config/ros_console_settings_for_tests.conf deleted file mode 100644 index 82c355359b3090d6bf8c1b62e9c73dc6705d0189..0000000000000000000000000000000000000000 --- a/franka_combinable_hw/test/config/ros_console_settings_for_tests.conf +++ /dev/null @@ -1,4 +0,0 @@ -# Set the default ros output to warning and higher -log4j.logger.ros=WARN -# Override my package to output everything -log4j.logger.ros.franka_hw=FATAL diff --git a/franka_combinable_hw/test/franka_combinable_hw_controller_switching_test.cpp b/franka_combinable_hw/test/franka_combinable_hw_controller_switching_test.cpp deleted file mode 100644 index d6bc475c190529078512f9eca27f8706e32c51ef..0000000000000000000000000000000000000000 --- a/franka_combinable_hw/test/franka_combinable_hw_controller_switching_test.cpp +++ /dev/null @@ -1,208 +0,0 @@ -// Copyright (c) 2019 Franka Emika GmbH -// Use of this source code is governed by the Apache-2.0 license, see LICENSE -#include <array> -#include <list> -#include <random> -#include <set> -#include <string> - -#include <gtest/gtest.h> - -#include <hardware_interface/controller_info.h> -#include <hardware_interface/joint_command_interface.h> -#include <joint_limits_interface/joint_limits.h> -#include <joint_limits_interface/joint_limits_urdf.h> -#include <ros/ros.h> -#include <urdf/model.h> - -#include <franka_hw/franka_cartesian_command_interface.h> -#include <franka_combinable_hw/franka_combinable_hw.h> - -using std::string; -using std::set; -using hardware_interface::InterfaceResources; -using hardware_interface::ControllerInfo; - -std::string arm_id("panda"); - -std::array<std::string, 7> joint_names = {arm_id + "_joint1", - arm_id + "_joint2", - arm_id + "_joint3", - arm_id + "_joint4", - arm_id + "_joint5", - arm_id + "_joint6", - arm_id + "_joint7"}; - -namespace franka_combinable_hw { - -hardware_interface::ControllerInfo newInfo( - const std::string& name, - const std::string& type, - const hardware_interface::InterfaceResources& resource1) { - hardware_interface::ControllerInfo info; - info.claimed_resources.clear(); - info.name = name; - info.type = type; - info.claimed_resources.push_back(resource1); - return info; -} - -hardware_interface::ControllerInfo newInfo( - const std::string& name, - const std::string& type, - const hardware_interface::InterfaceResources& resource1, - const hardware_interface::InterfaceResources& resource2) { - hardware_interface::ControllerInfo info = newInfo(name, type, resource1); - info.claimed_resources.push_back(resource2); - return info; -} - -hardware_interface::ControllerInfo newInfo( - const std::string& name, - const std::string& type, - const hardware_interface::InterfaceResources& resource1, - const hardware_interface::InterfaceResources& resource2, - const hardware_interface::InterfaceResources& resource3) { - hardware_interface::ControllerInfo info = newInfo(name, type, resource1, resource2); - info.claimed_resources.push_back(resource3); - return info; -} - -class ControllerConflict : - public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo> > { -public: - ControllerConflict() : robot_(new FrankaCombinableHW()) { - ros::NodeHandle root_nh; - ros::NodeHandle robot_hw_nh("~/" + arm_id); - robot_->initROSInterfaces(root_nh, robot_hw_nh); - } - bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) { - return robot_->checkForConflict(info_list); - } - bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) { - return robot_->prepareSwitch(info_list, info_list); - } - private: - std::unique_ptr<FrankaCombinableHW> robot_; -}; - -class NoControllerConflict : public - ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo> > { -public: - NoControllerConflict() : robot_(new FrankaCombinableHW()) { - ros::NodeHandle root_nh; - ros::NodeHandle robot_hw_nh("~/" + arm_id); - robot_->initROSInterfaces(root_nh, robot_hw_nh); - } - bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) { - return robot_->checkForConflict(info_list); - } - bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) { - return robot_->prepareSwitch(info_list, info_list); - } - private: - std::unique_ptr<FrankaCombinableHW> robot_; -}; - -string arm_id2("panda2"); -string jp_iface_str("hardware_interface::PositionJointInterface"); -string jv_iface_str("hardware_interface::VelocityJointInterface"); -string jt_iface_str("hardware_interface::EffortJointInterface"); -string cv_iface_str("franka_hw::FrankaVelocityCartesianInterface"); -string cp_iface_str("franka_hw::FrankaPoseCartesianInterface"); -string unknown_iface_str("hardware_interface::UnknownInterface"); -string name_str("some_controller"); -string type_str("SomeControllerClass"); -set<string> joints_set = {joint_names[0], - joint_names[1], - joint_names[2], - joint_names[3], - joint_names[4], - joint_names[5], - joint_names[6]}; -set<string> cartesian_set = {arm_id + "_robot"}; -set<string> cartesian_arm2_set = {arm_id2 + "_robot"}; -set<string> no_id_set = {"joint1"}; -InterfaceResources no_id_res(jp_iface_str, no_id_set); -InterfaceResources unknown_iface_res(unknown_iface_str, joints_set); -InterfaceResources jp_res(jp_iface_str, joints_set); -InterfaceResources jv_res(jv_iface_str, joints_set); -InterfaceResources jt_res(jt_iface_str, joints_set); -InterfaceResources cv_res(cv_iface_str, cartesian_set); -InterfaceResources cp_res(cp_iface_str, cartesian_set); -InterfaceResources cp_arm2_res(cp_iface_str, cartesian_arm2_set); -ControllerInfo cp_arm2_info = newInfo(name_str, type_str, cp_arm2_res); -ControllerInfo no_id_info = newInfo(name_str, type_str, no_id_res); -ControllerInfo unknown_iface_info = newInfo(name_str, type_str, unknown_iface_res); -ControllerInfo jt_jt_info = newInfo(name_str, type_str, jt_res, jt_res); -ControllerInfo jp_jp_info = newInfo(name_str, type_str, jp_res, jp_res); -ControllerInfo jp_jv_info = newInfo(name_str, type_str, jp_res, jv_res); -ControllerInfo jp_cv_info = newInfo(name_str, type_str, jp_res, cv_res); -ControllerInfo jp_cp_info = newInfo(name_str, type_str, jp_res, cp_res); -ControllerInfo jv_jv_info = newInfo(name_str, type_str, jv_res, jv_res); -ControllerInfo jv_cv_info = newInfo(name_str, type_str, jv_res, cv_res); -ControllerInfo jv_cp_info = newInfo(name_str, type_str, jv_res, cp_res); -ControllerInfo cv_cv_info = newInfo(name_str, type_str, cv_res, cv_res); -ControllerInfo cv_cp_info = newInfo(name_str, type_str, cv_res, cp_res); -ControllerInfo cp_cp_info = newInfo(name_str, type_str, cp_res, cp_res); -ControllerInfo jp_jp_jp_info = newInfo(name_str, type_str, jp_res, jp_res, jp_res); -ControllerInfo jp_info = newInfo(name_str, type_str, jp_res); -ControllerInfo jv_info = newInfo(name_str, type_str, jv_res); -ControllerInfo jt_info = newInfo(name_str, type_str, jt_res); -ControllerInfo cv_info = newInfo(name_str, type_str, cv_res); -ControllerInfo cp_info = newInfo(name_str, type_str, cp_res); -ControllerInfo jt_jp_info = newInfo(name_str, type_str, jt_res, jp_res); -ControllerInfo jt_jv_info = newInfo(name_str, type_str, jt_res, jv_res); -ControllerInfo jt_cv_info = newInfo(name_str, type_str, jt_res, cv_res); -ControllerInfo jt_cp_info = newInfo(name_str, type_str, jt_res, cp_res); - -INSTANTIATE_TEST_CASE_P(nonAdmissibleRequests, - ControllerConflict, - ::testing::Values(std::list<ControllerInfo>{no_id_info}, - std::list<ControllerInfo>{unknown_iface_info}, - std::list<ControllerInfo>{jp_info}, - std::list<ControllerInfo>{jv_info}, - std::list<ControllerInfo>{cv_info}, - std::list<ControllerInfo>{cp_info}, - std::list<ControllerInfo>{jt_jp_info}, - std::list<ControllerInfo>{jt_jv_info}, - std::list<ControllerInfo>{jt_jt_info}, - std::list<ControllerInfo>{jp_jp_info}, - std::list<ControllerInfo>{jp_jv_info}, - std::list<ControllerInfo>{jp_cv_info}, - std::list<ControllerInfo>{jp_cp_info}, - std::list<ControllerInfo>{jv_jv_info}, - std::list<ControllerInfo>{jv_cv_info}, - std::list<ControllerInfo>{jv_cp_info}, - std::list<ControllerInfo>{cv_cv_info}, - std::list<ControllerInfo>{cv_cp_info}, - std::list<ControllerInfo>{cp_cp_info}, - std::list<ControllerInfo>{jp_jp_jp_info})); - -INSTANTIATE_TEST_CASE_P(admissibleRequests, - NoControllerConflict, - ::testing::Values(std::list<ControllerInfo>{jt_info}, - std::list<ControllerInfo>{jt_cv_info}, - std::list<ControllerInfo>{jt_cp_info}, - std::list<ControllerInfo>{jt_info, cp_arm2_info})); - -TEST(FrankaCombinableHWTests, CanInitROSInterfaces) { - FrankaCombinableHW hw; - ros::NodeHandle root_nh; - ros::NodeHandle robot_hw_nh("~/" + arm_id); - EXPECT_TRUE(hw.initROSInterfaces(root_nh, robot_hw_nh)); -} - -TEST_P(ControllerConflict, ConflictsForIncompatibleControllers) { - EXPECT_TRUE(callCheckForConflict(GetParam())); -} - -TEST_P(NoControllerConflict, DoesNotConflictForCompatibleControllers) { - EXPECT_FALSE(callCheckForConflict(GetParam())); -} - -TEST_P(NoControllerConflict, CanPrepareSwitchForCompatibleControllers) { - EXPECT_TRUE(callPrepareSwitch(GetParam())); -} - -} // namespace franka_combinable_hw diff --git a/franka_combinable_hw/test/launch/franka_combinable_hw_test.test b/franka_combinable_hw/test/launch/franka_combinable_hw_test.test deleted file mode 100644 index 4e6553b2f3b61780833e2e2860a33c3c8e306ffa..0000000000000000000000000000000000000000 --- a/franka_combinable_hw/test/launch/franka_combinable_hw_test.test +++ /dev/null @@ -1,7 +0,0 @@ -<launch> - <env name="ROSCONSOLE_CONFIG_FILE" value="$(find franka_combinable_hw)/test/config/ros_console_settings_for_tests.conf"/> - <test test-name="franka_combinable_hw_test" pkg="franka_combinable_hw" type="franka_combinable_hw_test"> - <param name="robot_description" command="$(find xacro)/xacro --inorder $(find franka_description)/robots/panda_arm.urdf.xacro" /> - <rosparam command="load" file="$(find franka_combinable_hw)/test/config/franka_combinable_hw_test_node.yaml" /> - </test> -</launch> diff --git a/franka_combinable_hw/test/main.cpp b/franka_combinable_hw/test/main.cpp deleted file mode 100644 index 8e558b2411e69a76bb72d46f57db0a476b4dcdca..0000000000000000000000000000000000000000 --- a/franka_combinable_hw/test/main.cpp +++ /dev/null @@ -1,10 +0,0 @@ -// Copyright (c) 2019 Franka Emika GmbH -// Use of this source code is governed by the Apache-2.0 license, see LICENSE -#include <gtest/gtest.h> -#include <ros/ros.h> - -int main(int argc, char** argv) { - ::testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "franka_hw_test_node"); - return RUN_ALL_TESTS(); -} diff --git a/franka_combined_control/CMakeLists.txt b/franka_combined_control/CMakeLists.txt deleted file mode 100644 index d300733c783ed7b28b8329b78df17043fe09a662..0000000000000000000000000000000000000000 --- a/franka_combined_control/CMakeLists.txt +++ /dev/null @@ -1,63 +0,0 @@ -cmake_minimum_required(VERSION 3.4) -project(franka_combined_control) - -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_BUILD_TYPE Release) - -find_package(catkin REQUIRED COMPONENTS - controller_manager - franka_combinable_hw - roscpp -) - -find_package(Franka 0.5.0 REQUIRED) - -catkin_package( - CATKIN_DEPENDS - controller_manager - franka_combinable_hw - roscpp -) - -add_executable(franka_combined_control_node - src/franka_combined_control_node.cpp -) - -add_dependencies(franka_combined_control_node - ${catkin_EXPORTED_TARGETS} -) - -target_link_libraries(franka_combined_control_node - ${catkin_LIBRARIES} -) - -target_include_directories(franka_combined_control_node SYSTEM PUBLIC - ${catkin_INCLUDE_DIRS} -) - -## Installation -install(TARGETS franka_combined_control_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) -install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -## Tools -include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL - RESULT_VARIABLE CLANG_TOOLS -) -if(CLANG_TOOLS) - file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/franka_combined_control_node.cpp) - add_format_target(franka_combined_control FILES ${SOURCES}) - add_tidy_target(franka_combined_control - FILES ${SOURCES} - DEPENDS franka_combined_control_node - ) -endif() diff --git a/franka_combined_control/config/default_combined_controllers.yaml b/franka_combined_control/config/default_combined_controllers.yaml deleted file mode 100644 index 9d5d77fe1afa5e12ff2889b29358d3c86ec6c95f..0000000000000000000000000000000000000000 --- a/franka_combined_control/config/default_combined_controllers.yaml +++ /dev/null @@ -1,88 +0,0 @@ -effort_joint_trajectory_controller: - type: effort_controllers/JointTrajectoryController - joints: - - panda_left_joint1 - - panda_left_joint2 - - panda_left_joint3 - - panda_left_joint4 - - panda_left_joint5 - - panda_left_joint6 - - panda_left_joint7 - - panda_right_joint1 - - panda_right_joint2 - - panda_right_joint3 - - panda_right_joint4 - - panda_right_joint5 - - panda_right_joint6 - - panda_right_joint7 - constraints: - goal_time: 0.5 - panda_left_joint1: - goal: 0.05 - panda_left_joint2: - goal: 0.05 - panda_left_joint3: - goal: 0.05 - panda_left_joint4: - goal: 0.05 - panda_left_joint5: - goal: 0.05 - panda_left_joint6: - goal: 0.05 - panda_left_joint7: - goal: 0.05 - panda_right_joint1: - goal: 0.05 - panda_right_joint2: - goal: 0.05 - panda_right_joint3: - goal: 0.05 - panda_right_joint4: - goal: 0.05 - panda_right_joint5: - goal: 0.05 - panda_right_joint6: - goal: 0.05 - panda_right_joint7: - goal: 0.05 - gains: - panda_left_joint1: {p: 600, d: 30, i: 0, i_clamp: 1} - panda_left_joint2: {p: 600, d: 30, i: 0, i_clamp: 1} - panda_left_joint3: {p: 600, d: 30, i: 0, i_clamp: 1} - panda_left_joint4: {p: 600, d: 30, i: 0, i_clamp: 1} - panda_left_joint5: {p: 250, d: 10, i: 0, i_clamp: 1} - panda_left_joint6: {p: 150, d: 10, i: 0, i_clamp: 1} - panda_left_joint7: {p: 50, d: 5, i: 0, i_clamp: 1} - panda_right_joint1: {p: 600, d: 30, i: 0, i_clamp: 1} - panda_right_joint2: {p: 600, d: 30, i: 0, i_clamp: 1} - panda_right_joint3: {p: 600, d: 30, i: 0, i_clamp: 1} - panda_right_joint4: {p: 600, d: 30, i: 0, i_clamp: 1} - panda_right_joint5: {p: 250, d: 10, i: 0, i_clamp: 1} - panda_right_joint6: {p: 150, d: 10, i: 0, i_clamp: 1} - panda_right_joint7: {p: 50, d: 5, i: 0, i_clamp: 1} - -panda_left_state_controller: - type: franka_control/FrankaStateController - arm_id: panda_left - joint_names: - - panda_left_joint1 - - panda_left_joint2 - - panda_left_joint3 - - panda_left_joint4 - - panda_left_joint5 - - panda_left_joint6 - - panda_left_joint7 - publish_rate: 30 # [Hz] - -panda_right_state_controller: - type: franka_control/FrankaStateController - arm_id: panda_right - joint_names: - - panda_right_joint1 - - panda_right_joint2 - - panda_right_joint3 - - panda_right_joint4 - - panda_right_joint5 - - panda_right_joint6 - - panda_right_joint7 - publish_rate: 30 # [Hz] diff --git a/franka_combined_control/config/franka_combined_control_node.yaml b/franka_combined_control/config/franka_combined_control_node.yaml deleted file mode 100644 index 71b012ea4c40c0eb77e91192c3e29f76703afc69..0000000000000000000000000000000000000000 --- a/franka_combined_control/config/franka_combined_control_node.yaml +++ /dev/null @@ -1,25 +0,0 @@ -robot_hardware: - - panda_left - - panda_right -panda_left: - type: franka_combinable_hw/FrankaCombinableHW - arm_id: panda_left - joint_names: - - panda_left_joint1 - - panda_left_joint2 - - panda_left_joint3 - - panda_left_joint4 - - panda_left_joint5 - - panda_left_joint6 - - panda_left_joint7 -panda_right: - type: franka_combinable_hw/FrankaCombinableHW - arm_id: panda_right - joint_names: - - panda_right_joint1 - - panda_right_joint2 - - panda_right_joint3 - - panda_right_joint4 - - panda_right_joint5 - - panda_right_joint6 - - panda_right_joint7 diff --git a/franka_combined_control/mainpage.dox b/franka_combined_control/mainpage.dox deleted file mode 100644 index 941d0bf97714038b58c39c7dcd816e901e22c023..0000000000000000000000000000000000000000 --- a/franka_combined_control/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** - * @mainpage - * @htmlinclude "manifest.html" - * - * Overview page for Franka Emika research robots: https://frankaemika.github.io - */ diff --git a/franka_combined_control/package.xml b/franka_combined_control/package.xml deleted file mode 100644 index 883a511d72d52600bf7440dc2fff709d45872ad0..0000000000000000000000000000000000000000 --- a/franka_combined_control/package.xml +++ /dev/null @@ -1,27 +0,0 @@ -<?xml version="1.0"?> -<package format="2"> - <name>franka_combined_control</name> - <version>0.7.0</version> - <description>franka_combined_control provides a hardware node to control multiple Panda robots.</description> - <maintainer email="support@franka.de">Franka Emika GmbH</maintainer> - <license>Apache 2.0</license> - - <url type="website">http://wiki.ros.org/franka_combined_control</url> - <url type="repository">https://github.com/frankaemika/franka_ros</url> - <url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url> - <author>Franka Emika GmbH</author> - - <buildtool_depend>catkin</buildtool_depend> - - <depend>controller_manager</depend> - <depend>franka_combinable_hw</depend> - <depend>roscpp</depend> - - <exec_depend>franka_description</exec_depend> - <exec_depend>joint_state_publisher</exec_depend> - <exec_depend>message_runtime</exec_depend> - <exec_depend>robot_state_publisher</exec_depend> - - <build_export_depend>message_runtime</build_export_depend> - -</package> diff --git a/franka_combined_control/rosdoc.yaml b/franka_combined_control/rosdoc.yaml deleted file mode 100644 index 96ee597ef5cacd0f223f676c738f396f0810b78c..0000000000000000000000000000000000000000 --- a/franka_combined_control/rosdoc.yaml +++ /dev/null @@ -1,2 +0,0 @@ -- builder: doxygen - javadoc_autobrief: YES diff --git a/franka_combined_example_controllers/CMakeLists.txt b/franka_combined_example_controllers/CMakeLists.txt deleted file mode 100644 index e9a0f7f62ab9aacc3115cc49e1ceb5853a43d9ce..0000000000000000000000000000000000000000 --- a/franka_combined_example_controllers/CMakeLists.txt +++ /dev/null @@ -1,110 +0,0 @@ -cmake_minimum_required(VERSION 3.4) -project(franka_combined_example_controllers) - -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_BUILD_TYPE Release) - -find_package(catkin REQUIRED COMPONENTS - actionlib_msgs - controller_interface - dynamic_reconfigure - eigen_conversions - franka_combinable_hw - franka_example_controllers - franka_hw - geometry_msgs - hardware_interface - tf - tf_conversions - pluginlib - realtime_tools - roscpp -) - -find_package(Eigen3 REQUIRED) -find_package(Franka 0.5.0 REQUIRED) - -generate_dynamic_reconfigure_options( - cfg/dual_arm_compliance_param.cfg -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES franka_combined_example_controllers - CATKIN_DEPENDS - controller_interface - dynamic_reconfigure - eigen_conversions - franka_combinable_hw - franka_example_controllers - franka_hw - geometry_msgs - tf - tf_conversions hardware_interface - pluginlib - roscpp - realtime_tools - DEPENDS Franka -) - -add_library(franka_combined_example_controllers - src/dual_arm_cartesian_impedance_example_controller.cpp -) - -target_include_directories(franka_combined_example_controllers PUBLIC - include -) - -target_include_directories(franka_combined_example_controllers SYSTEM PUBLIC - ${catkin_INCLUDE_DIRS} - ${Franka_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) - -add_dependencies(franka_combined_example_controllers - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ${PROJECT_NAME}_gencfg -) - -target_link_libraries(franka_combined_example_controllers PUBLIC - ${catkin_LIBRARIES} - ${Franka_LIBRARIES} -) - -## Installation -install(TARGETS franka_combined_example_controllers - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) -install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) -install(DIRECTORY scripts - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) -install(FILES franka_combined_example_controllers_plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -## Tools -include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL - RESULT_VARIABLE CLANG_TOOLS -) -if(CLANG_TOOLS) - file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp) - file(GLOB_RECURSE HEADERS - ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h - ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h - ) - add_format_target(franka_combined_example_controllers FILES ${SOURCES} ${HEADERS}) - add_tidy_target(franka_combined_example_controllers - FILES ${SOURCES} - DEPENDS franka_combined_example_controllers - ) -endif() diff --git a/franka_combined_example_controllers/config/franka_combined_example_controllers.yaml b/franka_combined_example_controllers/config/franka_combined_example_controllers.yaml deleted file mode 100644 index e9eae02774152578c88ed1bc0ae07c72220db4d8..0000000000000000000000000000000000000000 --- a/franka_combined_example_controllers/config/franka_combined_example_controllers.yaml +++ /dev/null @@ -1,22 +0,0 @@ -dual_arm_cartesian_impedance_example_controller: - type: franka_combined_example_controllers/DualArmCartesianImpedanceExampleController - left: - arm_id: panda_left - joint_names: - - panda_left_joint1 - - panda_left_joint2 - - panda_left_joint3 - - panda_left_joint4 - - panda_left_joint5 - - panda_left_joint6 - - panda_left_joint7 - right: - arm_id: panda_right - joint_names: - - panda_right_joint1 - - panda_right_joint2 - - panda_right_joint3 - - panda_right_joint4 - - panda_right_joint5 - - panda_right_joint6 - - panda_right_joint7 diff --git a/franka_combined_example_controllers/franka_combined_example_controllers_plugin.xml b/franka_combined_example_controllers/franka_combined_example_controllers_plugin.xml deleted file mode 100644 index 52835eea4e895cf144b281745af47cdadd65d5f7..0000000000000000000000000000000000000000 --- a/franka_combined_example_controllers/franka_combined_example_controllers_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ -<library path="lib/libfranka_combined_example_controllers"> - <class name="franka_combined_example_controllers/DualArmCartesianImpedanceExampleController" type="franka_combined_example_controllers::DualArmCartesianImpedanceExampleController" base_class_type="controller_interface::ControllerBase"> - <description> - A controller that renders Cartesian impedance based tracking of separate target poses for 2 panda arms. - </description> - </class> -</library> diff --git a/franka_combined_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch b/franka_combined_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch deleted file mode 100644 index 014514f57e761a4899c1e7989bb56e74b70b5786..0000000000000000000000000000000000000000 --- a/franka_combined_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch +++ /dev/null @@ -1,23 +0,0 @@ -<?xml version="1.0" ?> -<launch> - <arg name="robot_id" default="panda_dual" /> - <arg name="robot_left_ip" default="robot_left.franka.de" /> - <arg name="robot_right_ip" default="robot_right.franka.de" /> - <arg name="rviz" default="true" /> - <arg name="rqt" default="true" /> - - <include file="$(find franka_combined_control)/launch/franka_combined_control.launch" > - <arg name="robot_id" value="$(arg robot_id)" /> - <arg name="robot_left_ip" value="$(arg robot_left_ip)" /> - <arg name="robot_right_ip" value="$(arg robot_right_ip)" /> - </include> - - <group ns="$(arg robot_id)"> - <rosparam command="load" file="$(find franka_combined_example_controllers)/config/franka_combined_example_controllers.yaml" /> - <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="dual_arm_cartesian_impedance_example_controller"/> - <node name="interactive_marker_left" pkg="franka_combined_example_controllers" type="dual_arm_interactive_marker.py" - args="--right_arm_id panda_right --left_arm_id panda_left" required="false" output="screen" /> - <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" if="$(arg rqt)"/> - <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_combined_example_controllers)/launch/rviz/franka_dual_description_with_marker.rviz" if="$(arg rviz)"/> - </group> -</launch> diff --git a/franka_combined_example_controllers/package.xml b/franka_combined_example_controllers/package.xml deleted file mode 100644 index 9ac764edb6f774dd1e1073ea2a823fc79033a664..0000000000000000000000000000000000000000 --- a/franka_combined_example_controllers/package.xml +++ /dev/null @@ -1,40 +0,0 @@ -<?xml version="1.0"?> -<package format="2"> - <name>franka_combined_example_controllers</name> - <version>0.7.0</version> - <description>The franka_combined_example_controllers package</description> - <maintainer email="support@franka.de">Franka Emika GmbH</maintainer> - <license>Apache 2.0</license> - - <url type="website">http://wiki.ros.org/franka_combined_control_example_controllers</url> - <url type="repository">https://github.com/frankaemika/franka_ros</url> - <url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url> - <author>Franka Emika GmbH</author> - - <buildtool_depend>catkin</buildtool_depend> - - <build_depend>eigen</build_depend> - - <depend>controller_interface</depend> - <depend>dynamic_reconfigure</depend> - <depend>eigen_conversions</depend> - <depend>franka_combinable_hw</depend> - <depend>franka_example_controllers</depend> - <depend>franka_hw</depend> - <depend>geometry_msgs</depend> - <depend>hardware_interface</depend> - <depend>pluginlib</depend> - <depend>tf</depend> - <depend>tf_conversions</depend> - <depend>libfranka</depend> - <depend>realtime_tools</depend> - <depend>roscpp</depend> - - <exec_depend>franka_combined_control</exec_depend> - <exec_depend>rospy</exec_depend> - - <export> - <controller_interface plugin="${prefix}/franka_combined_example_controllers_plugin.xml"/> - </export> - -</package> diff --git a/franka_control/CMakeLists.txt b/franka_control/CMakeLists.txt index 219f2d08cde4b7638be4f0c614493a6162dfb5be..065df6aa3fa39fcd57a5d3135346fcab8a84d0ae 100644 --- a/franka_control/CMakeLists.txt +++ b/franka_control/CMakeLists.txt @@ -5,8 +5,6 @@ set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(catkin REQUIRED COMPONENTS - actionlib - actionlib_msgs controller_interface controller_manager franka_hw @@ -23,32 +21,14 @@ find_package(catkin REQUIRED COMPONENTS find_package(Franka 0.5.0 REQUIRED) -add_service_files(FILES - SetCartesianImpedance.srv - SetEEFrame.srv - SetForceTorqueCollisionBehavior.srv - SetFullCollisionBehavior.srv - SetJointImpedance.srv - SetKFrame.srv - SetLoad.srv -) - -add_action_files(FILES - ErrorRecovery.action -) - -generate_messages(DEPENDENCIES actionlib_msgs) - catkin_package( INCLUDE_DIRS include LIBRARIES franka_state_controller franka_control_services CATKIN_DEPENDS - actionlib controller_interface franka_hw franka_msgs geometry_msgs - message_runtime pluginlib realtime_tools roscpp @@ -80,54 +60,48 @@ target_include_directories(franka_state_controller PUBLIC include ) -## franka_control_services -add_library(franka_control_services - src/services.cpp +## franka_control_node +add_executable(franka_control_node + src/franka_control_node.cpp ) -add_dependencies(franka_control_services +add_dependencies(franka_control_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) -target_link_libraries(franka_control_services +target_link_libraries(franka_control_node ${Franka_LIBRARIES} + franka_control_services ${catkin_LIBRARIES} ) -target_include_directories(franka_control_services SYSTEM PUBLIC +target_include_directories(franka_control_node SYSTEM PUBLIC ${Franka_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ) -target_include_directories(franka_control_services PUBLIC - include -) -## franka_control_node -add_executable(franka_control_node - src/franka_control_node.cpp +add_executable(franka_combined_control_node + src/franka_combined_control_node.cpp ) -add_dependencies(franka_control_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} +add_dependencies(franka_combined_control_node + ${catkin_EXPORTED_TARGETS} ) -target_link_libraries(franka_control_node - ${Franka_LIBRARIES} - franka_control_services +target_link_libraries(franka_combined_control_node ${catkin_LIBRARIES} ) -target_include_directories(franka_control_node SYSTEM PUBLIC - ${Franka_INCLUDE_DIRS} +target_include_directories(franka_combined_control_node SYSTEM PUBLIC ${catkin_INCLUDE_DIRS} ) + ## Installation install(TARGETS franka_state_controller - franka_control_services franka_control_node + franka_combined_control_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} @@ -158,6 +132,6 @@ if(CLANG_TOOLS) add_format_target(franka_control FILES ${SOURCES} ${HEADERS}) add_tidy_target(franka_control FILES ${SOURCES} - DEPENDS franka_control_node franka_state_controller + DEPENDS franka_control_node franka_combined_control_node franka_state_controller ) endif() diff --git a/franka_control/config/default_combined_controllers.yaml b/franka_control/config/default_combined_controllers.yaml new file mode 100644 index 0000000000000000000000000000000000000000..e7ef4923f4f38e5d4ab443d1e0d1462b1f29f661 --- /dev/null +++ b/franka_control/config/default_combined_controllers.yaml @@ -0,0 +1,88 @@ +effort_joint_trajectory_controller: + type: effort_controllers/JointTrajectoryController + joints: + - panda_2_joint1 + - panda_2_joint2 + - panda_2_joint3 + - panda_2_joint4 + - panda_2_joint5 + - panda_2_joint6 + - panda_2_joint7 + - panda_1_joint1 + - panda_1_joint2 + - panda_1_joint3 + - panda_1_joint4 + - panda_1_joint5 + - panda_1_joint6 + - panda_1_joint7 + constraints: + goal_time: 0.5 + panda_2_joint1: + goal: 0.05 + panda_2_joint2: + goal: 0.05 + panda_2_joint3: + goal: 0.05 + panda_2_joint4: + goal: 0.05 + panda_2_joint5: + goal: 0.05 + panda_2_joint6: + goal: 0.05 + panda_2_joint7: + goal: 0.05 + panda_1_joint1: + goal: 0.05 + panda_1_joint2: + goal: 0.05 + panda_1_joint3: + goal: 0.05 + panda_1_joint4: + goal: 0.05 + panda_1_joint5: + goal: 0.05 + panda_1_joint6: + goal: 0.05 + panda_1_joint7: + goal: 0.05 + gains: + panda_1_joint1: {p: 600, d: 30, i: 0, i_clamp: 1} + panda_1_joint2: {p: 600, d: 30, i: 0, i_clamp: 1} + panda_1_joint3: {p: 600, d: 30, i: 0, i_clamp: 1} + panda_1_joint4: {p: 600, d: 30, i: 0, i_clamp: 1} + panda_1_joint5: {p: 250, d: 10, i: 0, i_clamp: 1} + panda_1_joint6: {p: 150, d: 10, i: 0, i_clamp: 1} + panda_1_joint7: {p: 50, d: 5, i: 0, i_clamp: 1} + panda_2_joint1: {p: 600, d: 30, i: 0, i_clamp: 1} + panda_2_joint2: {p: 600, d: 30, i: 0, i_clamp: 1} + panda_2_joint3: {p: 600, d: 30, i: 0, i_clamp: 1} + panda_2_joint4: {p: 600, d: 30, i: 0, i_clamp: 1} + panda_2_joint5: {p: 250, d: 10, i: 0, i_clamp: 1} + panda_2_joint6: {p: 150, d: 10, i: 0, i_clamp: 1} + panda_2_joint7: {p: 50, d: 5, i: 0, i_clamp: 1} + +panda_1_state_controller: + type: franka_control/FrankaStateController + arm_id: panda_1 + joint_names: + - panda_1_joint1 + - panda_1_joint2 + - panda_1_joint3 + - panda_1_joint4 + - panda_1_joint5 + - panda_1_joint6 + - panda_1_joint7 + publish_rate: 30 # [Hz] + +panda_2_state_controller: + type: franka_control/FrankaStateController + arm_id: panda_2 + joint_names: + - panda_2_joint1 + - panda_2_joint2 + - panda_2_joint3 + - panda_2_joint4 + - panda_2_joint5 + - panda_2_joint6 + - panda_2_joint7 + publish_rate: 30 # [Hz] diff --git a/franka_control/config/franka_combined_control_node.yaml b/franka_control/config/franka_combined_control_node.yaml new file mode 100644 index 0000000000000000000000000000000000000000..8fd271c34c7ba1dd1b617ee7c017c606530b38fb --- /dev/null +++ b/franka_control/config/franka_combined_control_node.yaml @@ -0,0 +1,63 @@ +robot_hardware: + - panda_1 + - panda_2 + +panda_1: + type: franka_hw/FrankaCombinableHW + arm_id: panda_1 + joint_names: + - panda_1_joint1 + - panda_1_joint2 + - panda_1_joint3 + - panda_1_joint4 + - panda_1_joint5 + - panda_1_joint6 + - panda_1_joint7 + # Configure the threshold angle for printing joint limit warnings. + joint_limit_warning_threshold: 0.1 # [rad] + # Activate rate limiter? [true|false] + rate_limiting: true + # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate. + cutoff_frequency: 100 + # Internal controller for motion generators [joint_impedance|cartesian_impedance] + internal_controller: joint_impedance + # Configure the initial defaults for the collision behavior reflexes. + collision_config: + lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] + upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] + lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] + upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] + lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] + upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] + lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] + upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] + +panda_2: + type: franka_hw/FrankaCombinableHW + arm_id: panda_2 + joint_names: + - panda_2_joint1 + - panda_2_joint2 + - panda_2_joint3 + - panda_2_joint4 + - panda_2_joint5 + - panda_2_joint6 + - panda_2_joint7 + # Configure the threshold angle for printing joint limit warnings. + joint_limit_warning_threshold: 0.1 # [rad] + # Activate rate limiter? [true|false] + rate_limiting: true + # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate. + cutoff_frequency: 100 + # Internal controller for motion generators [joint_impedance|cartesian_impedance] + internal_controller: joint_impedance + # Configure the initial defaults for the collision behavior reflexes. + collision_config: + lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] + upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] + lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] + upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] + lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] + upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] + lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] + upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] diff --git a/franka_control/config/franka_control_node.yaml b/franka_control/config/franka_control_node.yaml index fd2524afc14bc81163d105843f6eddd57436117a..5a0c78c807802643ad90b38ca25c2c6e731dc335 100644 --- a/franka_control/config/franka_control_node.yaml +++ b/franka_control/config/franka_control_node.yaml @@ -7,9 +7,21 @@ joint_names: - panda_joint6 - panda_joint7 arm_id: panda +# Configure the threshold angle for printing joint limit warnings. +joint_limit_warning_threshold: 0.1 # [rad] # Activate rate limiter? [true|false] rate_limiting: true # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate. cutoff_frequency: 100 # Internal controller for motion generators [joint_impedance|cartesian_impedance] internal_controller: joint_impedance +# Configure the initial defaults for the collision behavior reflexes. +collision_config: + lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] + upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] + lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] + upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] + lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] + upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] + lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] + upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] diff --git a/franka_control/include/franka_control/services.h b/franka_control/include/franka_control/services.h deleted file mode 100644 index 5589a15a84fd99a1913254e82aa9fd0997271561..0000000000000000000000000000000000000000 --- a/franka_control/include/franka_control/services.h +++ /dev/null @@ -1,72 +0,0 @@ -// Copyright (c) 2017 Franka Emika GmbH -// Use of this source code is governed by the Apache-2.0 license, see LICENSE -#pragma once - -#include <franka/exception.h> -#include <franka/robot.h> -#include <ros/console.h> -#include <ros/node_handle.h> -#include <ros/service_server.h> - -#include <franka_control/SetCartesianImpedance.h> -#include <franka_control/SetEEFrame.h> -#include <franka_control/SetForceTorqueCollisionBehavior.h> -#include <franka_control/SetFullCollisionBehavior.h> -#include <franka_control/SetJointImpedance.h> -#include <franka_control/SetKFrame.h> -#include <franka_control/SetLoad.h> - -#include <vector> - -namespace franka_control { - -template <typename T> -ros::ServiceServer advertiseService( - ros::NodeHandle& node_handle, - const std::string& name, - std::function<void(typename T::Request&, typename T::Response&)> handler) { - return node_handle.advertiseService<typename T::Request, typename T::Response>( - name, [name, handler](typename T::Request& request, typename T::Response& response) { - try { - handler(request, response); - response.success = true; - ROS_DEBUG_STREAM(name << " succeeded."); - } catch (const franka::Exception& ex) { - ROS_ERROR_STREAM(name << " failed: " << ex.what()); - response.success = false; - response.error = ex.what(); - } - return true; - }); -} - -class ServiceContainer { - public: - template <typename T, typename... TArgs> - ServiceContainer& advertiseService(TArgs&&... args) { - ros::ServiceServer server = franka_control::advertiseService<T>(std::forward<TArgs>(args)...); - services_.push_back(server); - return *this; - } - - private: - std::vector<ros::ServiceServer> services_; -}; - -void setCartesianImpedance(franka::Robot& robot, - const SetCartesianImpedance::Request& req, - SetCartesianImpedance::Response& res); -void setJointImpedance(franka::Robot& robot, - const SetJointImpedance::Request& req, - SetJointImpedance::Response& res); -void setEEFrame(franka::Robot& robot, const SetEEFrame::Request& req, SetEEFrame::Response& res); -void setKFrame(franka::Robot& robot, const SetKFrame::Request& req, SetKFrame::Response& res); -void setForceTorqueCollisionBehavior(franka::Robot& robot, - const SetForceTorqueCollisionBehavior::Request& req, - SetForceTorqueCollisionBehavior::Response& res); -void setFullCollisionBehavior(franka::Robot& robot, - const SetFullCollisionBehavior::Request& req, - SetFullCollisionBehavior::Response& res); -void setLoad(franka::Robot& robot, const SetLoad::Request& req, SetLoad::Response& res); - -} // namespace franka_control diff --git a/franka_combined_control/launch/franka_combined_control.launch b/franka_control/launch/franka_combined_control.launch similarity index 53% rename from franka_combined_control/launch/franka_combined_control.launch rename to franka_control/launch/franka_combined_control.launch index 8bd89bb4a8daa4407af9e657cd0cb9b71b8d53f3..c17e7b927a4458adcce542cc7cb3804b713028dc 100644 --- a/franka_combined_control/launch/franka_combined_control.launch +++ b/franka_control/launch/franka_combined_control.launch @@ -7,31 +7,31 @@ <!-- Additional XACRO args. Be sure to escape colons and equal signs with backslashes, because XACRO use same syntax as roslaunch: roslaunch <pkg> <launchfile> robot:=path/to/urdf.xacro args:="foo\:\=bar other\:\=true" --> - <arg name="args" default="" /> - <arg name="robot_id" default="panda_dual" /> - <arg name="robot_left_ip" default="robot_left.franka.de" /> - <arg name="robot_right_ip" default="robot_right.franka.de" /> + <arg name="robot_id" default="combined_panda" /> + <arg name="robot_1_ip" default="robot_1.franka.de" /> + <arg name="robot_2_ip" default="robot_2.franka.de" /> + <arg name="hw_config_file" default="$(find franka_control)/config/franka_combined_control_node.yaml"/> - <node name="$(arg robot_id)" pkg="franka_combined_control" type="franka_combined_control_node" output="screen" required="true" > - <rosparam command="load" file="$(find franka_combined_control)/config/franka_combined_control_node.yaml" /> - <param name="panda_left/robot_ip" value="$(arg robot_left_ip)" /> - <param name="panda_right/robot_ip" value="$(arg robot_right_ip)" /> + <node name="$(arg robot_id)" pkg="franka_control" type="franka_combined_control_node" output="screen" required="true" > + <rosparam command="load" file="$(arg hw_config_file)" /> + <param name="panda_1/robot_ip" value="$(arg robot_1_ip)" /> + <param name="panda_2/robot_ip" value="$(arg robot_2_ip)" /> <param name="robot_description" command="xacro --inorder $(arg robot) $(arg args)" /> </node> <group ns="$(arg robot_id)"> - <rosparam command="load" file="$(find franka_combined_control)/config/default_combined_controllers.yaml" /> + <rosparam command="load" file="$(find franka_control)/config/default_combined_controllers.yaml" /> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/> <node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" - args="panda_left_state_controller panda_right_state_controller"/> + args="panda_1_state_controller panda_2_state_controller"/> <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen"> <param name="robot_description" command="xacro --inorder $(arg robot) $(arg args)" /> - <rosparam param="source_list">[panda_left_state_controller/joint_states, - panda_right_state_controller/joint_states, - panda_left/franka_gripper/joint_states, - panda_right/franka_gripper/joint_states] + <rosparam param="source_list">[panda_1_state_controller/joint_states, + panda_2_state_controller/joint_states, + panda_1/franka_gripper/joint_states, + panda_2/franka_gripper/joint_states] </rosparam> <param name="rate" value="30"/> </node> diff --git a/franka_control/package.xml b/franka_control/package.xml index c01d2bd7726f86affcde02c44a9a56c0beb6a6bf..af060e9eecabd971fb5c79570f57f47e30f4e4d0 100644 --- a/franka_control/package.xml +++ b/franka_control/package.xml @@ -13,11 +13,7 @@ <buildtool_depend>catkin</buildtool_depend> - <build_depend>message_generation</build_depend> - <depend>libfranka</depend> - <depend>actionlib</depend> - <depend>actionlib_msgs</depend> <depend>controller_interface</depend> <depend>controller_manager</depend> <depend>franka_hw</depend> @@ -33,11 +29,8 @@ <exec_depend>franka_description</exec_depend> <exec_depend>franka_gripper</exec_depend> <exec_depend>joint_state_publisher</exec_depend> - <exec_depend>message_runtime</exec_depend> <exec_depend>robot_state_publisher</exec_depend> - <build_export_depend>message_runtime</build_export_depend> - <export> <controller_interface plugin="${prefix}/franka_controller_plugins.xml"/> </export> diff --git a/franka_combined_control/src/franka_combined_control_node.cpp b/franka_control/src/franka_combined_control_node.cpp similarity index 65% rename from franka_combined_control/src/franka_combined_control_node.cpp rename to franka_control/src/franka_combined_control_node.cpp index 2553c3dc45706a6b9cf75f822e1986004ff6b74c..a5211c767131a3a795039df672adf4229d559220 100644 --- a/franka_combined_control/src/franka_combined_control_node.cpp +++ b/franka_control/src/franka_combined_control_node.cpp @@ -2,11 +2,11 @@ // Use of this source code is governed by the Apache-2.0 license, see LICENSE #include <controller_manager/controller_manager.h> -#include <franka_combinable_hw/franka_combined_hw.h> +#include <franka_hw/franka_combined_hw.h> #include <ros/ros.h> #include <sched.h> -#include <stdexcept> +#include <string> int main(int argc, char** argv) { ros::init(argc, argv, "franka_combined_control_node"); @@ -14,21 +14,13 @@ int main(int argc, char** argv) { ros::AsyncSpinner spinner(4); spinner.start(); - ros::NodeHandle nh("~"); - franka_combinable_hw::FrankaCombinedHW hw; - bool init_success = hw.init(nh, nh); - - if (!init_success) { - throw std::runtime_error( - "franka_combined_control_node:: Initialization of FrankaCombinedHW failed!"); + ros::NodeHandle private_node_handle("~"); + franka_hw::FrankaCombinedHW franka_control; + if (!franka_control.init(private_node_handle, private_node_handle)) { + ROS_ERROR("franka_combined_control_node:: Initialization of FrankaCombinedHW failed!"); return 1; } - controller_manager::ControllerManager cm(&hw, nh); - - ros::Duration period(0.001); - ros::Rate rate(period); - // set current control_loop thread to real-time const int kThreadPriority = sched_get_priority_max(SCHED_FIFO); if (kThreadPriority == -1) { @@ -44,11 +36,16 @@ int main(int argc, char** argv) { return 1; } + controller_manager::ControllerManager cm(&franka_control, private_node_handle); + ros::Duration period(0.001); + ros::Rate rate(period); + while (ros::ok()) { rate.sleep(); - hw.read(ros::Time::now(), period); - cm.update(ros::Time::now(), period, hw.controllerNeedsReset()); - hw.write(ros::Time::now(), period); + ros::Time now = ros::Time::now(); + franka_control.read(now, period); + cm.update(now, period, franka_control.controllerNeedsReset()); + franka_control.write(now, period); } return 0; diff --git a/franka_control/src/franka_control_node.cpp b/franka_control/src/franka_control_node.cpp index 0b0a2fab92866fa2fdbe83c3243c1d918fdf0e82..fae2f0f6c2c316c8e258dec4b8c46051951c8c18 100644 --- a/franka_control/src/franka_control_node.cpp +++ b/franka_control/src/franka_control_node.cpp @@ -11,146 +11,45 @@ #include <franka/exception.h> #include <franka/robot.h> #include <franka_hw/franka_hw.h> +#include <franka_hw/services.h> +#include <franka_msgs/ErrorRecoveryAction.h> #include <ros/ros.h> -#include <franka_control/ErrorRecoveryAction.h> -#include <franka_control/services.h> - -using franka_control::ServiceContainer; +using franka_hw::ServiceContainer; int main(int argc, char** argv) { ros::init(argc, argv, "franka_control_node"); + ros::NodeHandle public_node_handle; ros::NodeHandle node_handle("~"); - std::vector<std::string> joint_names_vector; - if (!node_handle.getParam("joint_names", joint_names_vector) || joint_names_vector.size() != 7) { - ROS_ERROR("Invalid or no joint_names parameters provided"); + franka_hw::FrankaHW franka_control; + if (!franka_control.init(public_node_handle, node_handle)) { + ROS_ERROR("franka_control_node: Failed to initialize FrankaHW class. Shutting down!"); return 1; } - std::array<std::string, 7> joint_names; - std::copy(joint_names_vector.cbegin(), joint_names_vector.cend(), joint_names.begin()); - - bool rate_limiting; - if (!node_handle.getParamCached("rate_limiting", rate_limiting)) { - ROS_ERROR("Invalid or no rate_limiting parameter provided"); - return 1; - } - - double cutoff_frequency; - if (!node_handle.getParamCached("cutoff_frequency", cutoff_frequency)) { - ROS_ERROR("Invalid or no cutoff_frequency parameter provided"); - return 1; - } - - std::string internal_controller; - if (!node_handle.getParam("internal_controller", internal_controller)) { - ROS_ERROR("No internal_controller parameter provided"); - return 1; - } - - urdf::Model urdf_model; - if (!urdf_model.initParamWithNodeHandle("robot_description", public_node_handle)) { - ROS_ERROR("Could not initialize URDF model from robot_description"); - return 1; - } - - std::string robot_ip; - if (!node_handle.getParam("robot_ip", robot_ip)) { - ROS_ERROR("Invalid or no robot_ip parameter provided"); - return 1; - } - - std::string arm_id; - if (!node_handle.getParam("arm_id", arm_id)) { - ROS_ERROR("Invalid or no arm_id parameter provided"); - return 1; - } - franka::Robot robot(robot_ip); - - // Set default collision behavior - robot.setCollisionBehavior( - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); + franka::Robot& robot = franka_control.robot(); std::atomic_bool has_error(false); ServiceContainer services; - services - .advertiseService<franka_control::SetJointImpedance>( - node_handle, "set_joint_impedance", - [&robot](auto&& req, auto&& res) { - return franka_control::setJointImpedance(robot, req, res); - }) - .advertiseService<franka_control::SetCartesianImpedance>( - node_handle, "set_cartesian_impedance", - [&robot](auto&& req, auto&& res) { - return franka_control::setCartesianImpedance(robot, req, res); - }) - .advertiseService<franka_control::SetEEFrame>( - node_handle, "set_EE_frame", - [&robot](auto&& req, auto&& res) { return franka_control::setEEFrame(robot, req, res); }) - .advertiseService<franka_control::SetKFrame>( - node_handle, "set_K_frame", - [&robot](auto&& req, auto&& res) { return franka_control::setKFrame(robot, req, res); }) - .advertiseService<franka_control::SetForceTorqueCollisionBehavior>( - node_handle, "set_force_torque_collision_behavior", - [&robot](auto&& req, auto&& res) { - return franka_control::setForceTorqueCollisionBehavior(robot, req, res); - }) - .advertiseService<franka_control::SetFullCollisionBehavior>( - node_handle, "set_full_collision_behavior", - [&robot](auto&& req, auto&& res) { - return franka_control::setFullCollisionBehavior(robot, req, res); - }) - .advertiseService<franka_control::SetLoad>( - node_handle, "set_load", - [&robot](auto&& req, auto&& res) { return franka_control::setLoad(robot, req, res); }); - - actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction> recovery_action_server( + franka_hw::setupServices(robot, node_handle, services); + + actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction> recovery_action_server( node_handle, "error_recovery", - [&](const franka_control::ErrorRecoveryGoalConstPtr&) { + [&](const franka_msgs::ErrorRecoveryGoalConstPtr&) { try { robot.automaticErrorRecovery(); has_error = false; recovery_action_server.setSucceeded(); ROS_INFO("Recovered from error"); } catch (const franka::Exception& ex) { - recovery_action_server.setAborted(franka_control::ErrorRecoveryResult(), ex.what()); + recovery_action_server.setAborted(franka_msgs::ErrorRecoveryResult(), ex.what()); } }, false); - franka::Model model = robot.loadModel(); - auto get_rate_limiting = [&]() { - node_handle.getParamCached("rate_limiting", rate_limiting); - return rate_limiting; - }; - auto get_internal_controller = [&]() { - node_handle.getParamCached("internal_controller", internal_controller); - - franka::ControllerMode controller_mode; - if (internal_controller == "joint_impedance") { - controller_mode = franka::ControllerMode::kJointImpedance; - } else if (internal_controller == "cartesian_impedance") { - controller_mode = franka::ControllerMode::kCartesianImpedance; - } else { - ROS_WARN("Invalid internal_controller parameter provided, falling back to joint impedance"); - controller_mode = franka::ControllerMode::kJointImpedance; - } - - return controller_mode; - }; - auto get_cutoff_frequency = [&]() { - node_handle.getParamCached("cutoff_frequency", cutoff_frequency); - return cutoff_frequency; - }; - franka_hw::FrankaHW franka_control(joint_names, arm_id, urdf_model, model, get_rate_limiting, - get_cutoff_frequency, get_internal_controller); - // Initialize robot state before loading any controller franka_control.update(robot.readOnce()); @@ -181,7 +80,7 @@ int main(int argc, char** argv) { try { // Run control loop. Will exit if the controller is switched. - franka_control.control(robot, [&](const ros::Time& now, const ros::Duration& period) { + franka_control.control([&](const ros::Time& now, const ros::Duration& period) { if (period.toSec() == 0.0) { // Reset controllers before starting a motion control_manager.update(now, period, true); diff --git a/franka_description/robots/dual_panda_example.urdf.xacro b/franka_description/robots/dual_panda_example.urdf.xacro index cd7dbebae17d4d9f66909cf124ff8e4d767854da..f98c133bf96cdfbd61c5d76549a9d2b8f17d494f 100644 --- a/franka_description/robots/dual_panda_example.urdf.xacro +++ b/franka_description/robots/dual_panda_example.urdf.xacro @@ -1,8 +1,11 @@ <?xml version="1.0" encoding="utf-8"?> <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda"> + <xacro:arg name="arm_id_1" default="panda_1" /> + <xacro:arg name="arm_id_2" default="panda_2" /> + <xacro:include filename="$(find franka_description)/robots/panda_arm.xacro"/> <xacro:include filename="$(find franka_description)/robots/hand.xacro"/> - + <!-- box shaped table as base for the 2 Pandas --> <link name="base"> <visual> @@ -21,13 +24,13 @@ </geometry> </collision> </link> - - <!-- left arm with gripper --> - <xacro:panda_arm arm_id="panda_left" connected_to="base" xyz="0 0.5 1" safety_distance="0.03"/> - <xacro:hand ns="panda_left" rpy="0 0 ${-pi/4}" connected_to="panda_left_link8" safety_distance="0.03"/> - + <!-- right arm with gripper --> - <xacro:panda_arm arm_id="panda_right" connected_to="base" xyz="0 -0.5 1" safety_distance="0.03"/> - <xacro:hand ns="panda_right" rpy="0 0 ${-pi/4}" connected_to="panda_right_link8" safety_distance="0.03"/> - + <xacro:panda_arm arm_id="$(arg arm_id_1)" connected_to="base" xyz="0 -0.5 1" safety_distance="0.03"/> + <xacro:hand ns="$(arg arm_id_1)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_1)_link8" safety_distance="0.03"/> + + <!-- left arm with gripper --> + <xacro:panda_arm arm_id="$(arg arm_id_2)" connected_to="base" xyz="0 0.5 1" safety_distance="0.03"/> + <xacro:hand ns="$(arg arm_id_2)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_2)_link8" safety_distance="0.03"/> + </robot> diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt index 7942e577ce64f8a9798812e7bfbcde9154c3c3b2..669121caa156141cc3abbc5b36f15dddcd1a2e8c 100644 --- a/franka_example_controllers/CMakeLists.txt +++ b/franka_example_controllers/CMakeLists.txt @@ -2,16 +2,18 @@ cmake_minimum_required(VERSION 3.4) project(franka_example_controllers) set(CMAKE_BUILD_TYPE Release) - set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(catkin REQUIRED COMPONENTS controller_interface dynamic_reconfigure + eigen_conversions franka_hw geometry_msgs hardware_interface + tf + tf_conversions message_generation pluginlib realtime_tools @@ -31,6 +33,7 @@ generate_messages() generate_dynamic_reconfigure_options( cfg/compliance_param.cfg cfg/desired_mass_param.cfg + cfg/dual_arm_compliance_param.cfg ) catkin_package( @@ -39,9 +42,12 @@ catkin_package( CATKIN_DEPENDS controller_interface dynamic_reconfigure + eigen_conversions franka_hw geometry_msgs hardware_interface + tf + tf_conversions message_runtime pluginlib realtime_tools @@ -59,6 +65,7 @@ add_library(franka_example_controllers src/joint_impedance_example_controller.cpp src/cartesian_impedance_example_controller.cpp src/force_example_controller.cpp + src/dual_arm_cartesian_impedance_example_controller.cpp ) add_dependencies(franka_example_controllers @@ -99,7 +106,7 @@ install(FILES franka_example_controllers_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) catkin_install_python( - PROGRAMS scripts/interactive_marker.py scripts/move_to_start.py + PROGRAMS scripts/interactive_marker.py scripts/move_to_start.py scripts/dual_arm_interactive_marker.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/franka_combined_example_controllers/cfg/dual_arm_compliance_param.cfg b/franka_example_controllers/cfg/dual_arm_compliance_param.cfg similarity index 100% rename from franka_combined_example_controllers/cfg/dual_arm_compliance_param.cfg rename to franka_example_controllers/cfg/dual_arm_compliance_param.cfg diff --git a/franka_example_controllers/config/franka_example_controllers.yaml b/franka_example_controllers/config/franka_example_controllers.yaml index 4b7707efc53ad63104834189ee45cdd48d510c33..b82b7efa8cebbf5563287afa94962c2ef4ca4e8b 100644 --- a/franka_example_controllers/config/franka_example_controllers.yaml +++ b/franka_example_controllers/config/franka_example_controllers.yaml @@ -92,3 +92,26 @@ cartesian_impedance_example_controller: - panda_joint5 - panda_joint6 - panda_joint7 + +dual_arm_cartesian_impedance_example_controller: + type: franka_example_controllers/DualArmCartesianImpedanceExampleController + right: + arm_id: panda_1 + joint_names: + - panda_1_joint1 + - panda_1_joint2 + - panda_1_joint3 + - panda_1_joint4 + - panda_1_joint5 + - panda_1_joint6 + - panda_1_joint7 + left: + arm_id: panda_2 + joint_names: + - panda_2_joint1 + - panda_2_joint2 + - panda_2_joint3 + - panda_2_joint4 + - panda_2_joint5 + - panda_2_joint6 + - panda_2_joint7 diff --git a/franka_example_controllers/franka_example_controllers_plugin.xml b/franka_example_controllers/franka_example_controllers_plugin.xml index 559a9f63eccaa1f924094702ffe1d38ec5dd9789..e770f046880f32432fe13f282b05e3f103cb2b65 100644 --- a/franka_example_controllers/franka_example_controllers_plugin.xml +++ b/franka_example_controllers/franka_example_controllers_plugin.xml @@ -36,13 +36,17 @@ </class> <class name="franka_example_controllers/CartesianImpedanceExampleController" type="franka_example_controllers::CartesianImpedanceExampleController" base_class_type="controller_interface::ControllerBase"> <description> - A controller that renders a spring damper system in cartesian space. Compliance parameters and the equilibrium pose can be modified online with dynamic reconfigure and an interactive marker respectively. + A controller that renders a spring damper system in cartesian space. Compliance parameters and the equilibrium pose can be modified online with dynamic reconfigure and an interactive marker respectively. </description> </class> <class name="franka_example_controllers/ForceExampleController" type="franka_example_controllers::ForceExampleController" base_class_type="controller_interface::ControllerBase"> <description> - A PI controller that applies a force corresponding to a user-provided desired mass in the z axis. The desired mass value can be modified online with dynamic reconfigure. + A PI controller that applies a force corresponding to a user-provided desired mass in the z axis. The desired mass value can be modified online with dynamic reconfigure. </description> </class> - + <class name="franka_example_controllers/DualArmCartesianImpedanceExampleController" type="franka_example_controllers::DualArmCartesianImpedanceExampleController" base_class_type="controller_interface::ControllerBase"> + <description> + A controller that renders Cartesian impedance based tracking of separate target poses for 2 panda arms. + </description> + </class> </library> diff --git a/franka_combined_example_controllers/include/franka_combined_example_controllers/dual_arm_cartesian_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/dual_arm_cartesian_impedance_example_controller.h similarity index 97% rename from franka_combined_example_controllers/include/franka_combined_example_controllers/dual_arm_cartesian_impedance_example_controller.h rename to franka_example_controllers/include/franka_example_controllers/dual_arm_cartesian_impedance_example_controller.h index d8d5aeb8a329529ffbbc877e8536aeed7bfdef5b..4af1a6ccc36f34f002b12eeba8b399070f6ede78 100644 --- a/franka_combined_example_controllers/include/franka_combined_example_controllers/dual_arm_cartesian_impedance_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/dual_arm_cartesian_impedance_example_controller.h @@ -16,12 +16,12 @@ #include <ros/time.h> #include <Eigen/Dense> -#include <franka_combined_example_controllers/dual_arm_compliance_paramConfig.h> +#include <franka_example_controllers/dual_arm_compliance_paramConfig.h> #include <franka_hw/franka_model_interface.h> #include <franka_hw/franka_state_interface.h> #include <franka_hw/trigger_rate.h> -namespace franka_combined_example_controllers { +namespace franka_example_controllers { /** * This container holds all data and parameters used to control one panda arm with a Cartesian @@ -179,4 +179,4 @@ class DualArmCartesianImpedanceExampleController void publishCenteringPose(); }; -} // namespace franka_combined_example_controllers +} // namespace franka_example_controllers diff --git a/franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch b/franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch new file mode 100644 index 0000000000000000000000000000000000000000..186fdcb3d854768d97e99d9b46ad5882c91aab43 --- /dev/null +++ b/franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch @@ -0,0 +1,23 @@ +<?xml version="1.0" ?> +<launch> + <arg name="robot_id" default="panda_dual" /> + <arg name="robot_right_ip" default="robot_1.franka.de" /> + <arg name="robot_left_ip" default="robot_2.franka.de" /> + <arg name="rviz" default="true" /> + <arg name="rqt" default="true" /> + + <include file="$(find franka_control)/launch/franka_combined_control.launch" > + <arg name="robot_id" value="$(arg robot_id)" /> + <arg name="robot_1_ip" value="$(arg robot_right_ip)" /> + <arg name="robot_2_ip" value="$(arg robot_left_ip)" /> + </include> + + <group ns="$(arg robot_id)"> + <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> + <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="dual_arm_cartesian_impedance_example_controller"/> + <node name="interactive_marker_left" pkg="franka_example_controllers" type="dual_arm_interactive_marker.py" + args="--right_arm_id panda_1 --left_arm_id panda_2" required="false" output="screen" /> + <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" if="$(arg rqt)"/> + <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_dual_description_with_marker.rviz" if="$(arg rviz)"/> + </group> +</launch> diff --git a/franka_example_controllers/launch/joint_impedance_example_controller.launch b/franka_example_controllers/launch/joint_impedance_example_controller.launch index 380c5e1f83553942c7426c0c8586e5aa0400940b..326617e1532ef9fd200f688321096e8b6f3afdb4 100644 --- a/franka_example_controllers/launch/joint_impedance_example_controller.launch +++ b/franka_example_controllers/launch/joint_impedance_example_controller.launch @@ -9,5 +9,4 @@ <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_impedance_example_controller"/> - <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/> </launch> diff --git a/franka_combined_example_controllers/launch/rviz/franka_dual_description_with_marker.rviz b/franka_example_controllers/launch/rviz/franka_dual_description_with_marker.rviz similarity index 92% rename from franka_combined_example_controllers/launch/rviz/franka_dual_description_with_marker.rviz rename to franka_example_controllers/launch/rviz/franka_dual_description_with_marker.rviz index 00a20474a596ddf367b67d57ea20eb038e3a9382..bdd4b593a530fb3c89e7423473e06f7e7635fa04 100644 --- a/franka_combined_example_controllers/launch/rviz/franka_dual_description_with_marker.rviz +++ b/franka_example_controllers/launch/rviz/franka_dual_description_with_marker.rviz @@ -6,6 +6,7 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /RobotModel1 - /InteractiveMarkers1 Splitter Ratio: 0.655555546 Tree Height: 820 @@ -80,122 +81,122 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - panda_left_hand: + panda_1_hand: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_left_leftfinger: + panda_1_leftfinger: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_left_link0: + panda_1_link0: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_left_link1: + panda_1_link1: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_left_link2: + panda_1_link2: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_left_link3: + panda_1_link3: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_left_link4: + panda_1_link4: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_left_link5: + panda_1_link5: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_left_link6: + panda_1_link6: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_left_link7: + panda_1_link7: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_left_link8: + panda_1_link8: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_left_rightfinger: + panda_1_rightfinger: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_right_hand: + panda_2_hand: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_right_leftfinger: + panda_2_leftfinger: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_right_link0: + panda_2_link0: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_right_link1: + panda_2_link1: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_right_link2: + panda_2_link2: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_right_link3: + panda_2_link3: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_right_link4: + panda_2_link4: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_right_link5: + panda_2_link5: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_right_link6: + panda_2_link6: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_right_link7: + panda_2_link7: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_right_link8: + panda_2_link8: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_right_rightfinger: + panda_2_rightfinger: Alpha: 1 Show Axes: false Show Trail: false diff --git a/franka_example_controllers/package.xml b/franka_example_controllers/package.xml index a382e13c87eb07e8320ed21682350db9d58d0f9b..65636db264699a3852f5d9f3bba7f9311424c0fc 100644 --- a/franka_example_controllers/package.xml +++ b/franka_example_controllers/package.xml @@ -20,9 +20,12 @@ <depend>controller_interface</depend> <depend>dynamic_reconfigure</depend> + <depend>eigen_conversions</depend> <depend>franka_hw</depend> <depend>geometry_msgs</depend> <depend>hardware_interface</depend> + <depend>tf</depend> + <depend>tf_conversions</depend> <depend>libfranka</depend> <depend>pluginlib</depend> <depend>realtime_tools</depend> diff --git a/franka_combined_example_controllers/scripts/dual_arm_interactive_marker.py b/franka_example_controllers/scripts/dual_arm_interactive_marker.py similarity index 90% rename from franka_combined_example_controllers/scripts/dual_arm_interactive_marker.py rename to franka_example_controllers/scripts/dual_arm_interactive_marker.py index 0cb22bbfc1f9d187ad03c31951f9af5f3271be2c..d2e6657c1dc436415202fbeb7b79b26e19f8677b 100755 --- a/franka_combined_example_controllers/scripts/dual_arm_interactive_marker.py +++ b/franka_example_controllers/scripts/dual_arm_interactive_marker.py @@ -12,8 +12,8 @@ from interactive_markers.interactive_marker_server import \ InteractiveMarkerServer, InteractiveMarkerFeedback from visualization_msgs.msg import InteractiveMarker, \ InteractiveMarkerControl, Marker +from franka_msgs.msg import FrankaState from geometry_msgs.msg import PoseStamped -from std_msgs.msg import Bool marker_pose = PoseStamped() @@ -53,25 +53,31 @@ def publish_target_pose(): pose_pub.publish(marker_pose) -def left_has_error_callback(msg): +def left_franka_state_callback(msg): """ This callback function set `has_error` variable to True if the left arm is having an error. - :param msg: error msg data + :param msg: FrankaState msg data :return: None """ - global has_error, left_has_error, right_has_error - left_has_error = msg.data + global has_error, left_has_error + if msg.robot_mode == FrankaState.ROBOT_MODE_MOVE: + left_has_error = False + else: + left_has_error = True has_error = left_has_error or right_has_error -def right_has_error_callback(msg): +def right_franka_state_callback(msg): """ This callback function set `has_error` variable to True if the right arm is having an error. - :param msg: error msg data + :param msg: FrankaState msg data :return: None """ - global has_error, left_has_error, right_has_error - right_has_error = msg.data + global has_error, right_has_error + if msg.robot_mode == FrankaState.ROBOT_MODE_MOVE: + right_has_error = False + else: + right_has_error = True has_error = left_has_error or right_has_error @@ -142,10 +148,11 @@ if __name__ == "__main__": right_arm_id = args.right_arm_id # Initialize subscribers for error states of the arms - left_error_sub = rospy.Subscriber(left_arm_id + "/has_error", Bool, - left_has_error_callback) - right_error_sub = rospy.Subscriber(right_arm_id + "/has_error", Bool, - right_has_error_callback) + left_state_sub = rospy.Subscriber(left_arm_id + "_state_controller/franka_states", + FrankaState, left_franka_state_callback) + + right_state_sub = rospy.Subscriber(right_arm_id + "_state_controller/franka_states", + FrankaState, right_franka_state_callback) # Set marker pose to be the current "middle pose" of both EEs reset_marker_pose_blocking() diff --git a/franka_combined_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp similarity index 98% rename from franka_combined_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp rename to franka_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp index c29e8f77141f1a0a4262686e99ac1b9f613bf957..8bb703c1301258b95a13062983e6f6d42461c6f3 100644 --- a/franka_combined_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp +++ b/franka_example_controllers/src/dual_arm_cartesian_impedance_example_controller.cpp @@ -1,6 +1,6 @@ // Copyright (c) 2019 Franka Emika GmbH // Use of this source code is governed by the Apache-2.0 license, see LICENSE -#include <franka_combined_example_controllers/dual_arm_cartesian_impedance_example_controller.h> +#include <franka_example_controllers/dual_arm_cartesian_impedance_example_controller.h> #include <cmath> #include <functional> @@ -18,7 +18,7 @@ #include <tf/transform_listener.h> #include <tf_conversions/tf_eigen.h> -namespace franka_combined_example_controllers { +namespace franka_example_controllers { bool DualArmCartesianImpedanceExampleController::initArm( hardware_interface::RobotHW* robot_hw, @@ -406,8 +406,7 @@ void DualArmCartesianImpedanceExampleController::publishCenteringPose() { } } -} // namespace franka_combined_example_controllers +} // namespace franka_example_controllers -PLUGINLIB_EXPORT_CLASS( - franka_combined_example_controllers::DualArmCartesianImpedanceExampleController, - controller_interface::ControllerBase) +PLUGINLIB_EXPORT_CLASS(franka_example_controllers::DualArmCartesianImpedanceExampleController, + controller_interface::ControllerBase) diff --git a/franka_hw/CMakeLists.txt b/franka_hw/CMakeLists.txt index db3f0916e77478974fbd3e4cbd2f615471d83665..90b93b8a7db2bf50394e0757988e0837aa07be80 100644 --- a/franka_hw/CMakeLists.txt +++ b/franka_hw/CMakeLists.txt @@ -5,11 +5,16 @@ set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(catkin REQUIRED COMPONENTS + actionlib controller_interface + combined_robot_hw hardware_interface joint_limits_interface roscpp + pluginlib urdf + franka_msgs + message_generation ) find_package(Franka 0.5.0 REQUIRED) @@ -17,13 +22,25 @@ find_package(Franka 0.5.0 REQUIRED) catkin_package( INCLUDE_DIRS include LIBRARIES franka_hw - CATKIN_DEPENDS controller_interface hardware_interface joint_limits_interface roscpp urdf + CATKIN_DEPENDS + actionlib_msgs + actionlib + controller_interface + combined_robot_hw + hardware_interface + joint_limits_interface + roscpp + pluginlib + urdf + franka_msgs DEPENDS Franka ) add_library(franka_hw src/control_mode.cpp src/franka_hw.cpp + src/franka_combinable_hw.cpp + src/franka_combined_hw.cpp src/resource_helpers.cpp src/trigger_rate.cpp ) @@ -36,6 +53,7 @@ add_dependencies(franka_hw target_link_libraries(franka_hw ${Franka_LIBRARIES} ${catkin_LIBRARIES} + franka_control_services ) target_include_directories(franka_hw SYSTEM PUBLIC @@ -56,6 +74,29 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) +## franka_control_services +add_library(franka_control_services + src/services.cpp +) + +add_dependencies(franka_control_services + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +target_link_libraries(franka_control_services + ${Franka_LIBRARIES} + ${catkin_LIBRARIES} +) + +target_include_directories(franka_control_services SYSTEM PUBLIC + ${Franka_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) +target_include_directories(franka_control_services PUBLIC + include +) + if(CATKIN_ENABLE_TESTING) add_subdirectory(test) endif() diff --git a/franka_hw/franka_combinable_hw_plugin.xml b/franka_hw/franka_combinable_hw_plugin.xml new file mode 100644 index 0000000000000000000000000000000000000000..12763ff6b121246a221708af40c67016e5cde51b --- /dev/null +++ b/franka_hw/franka_combinable_hw_plugin.xml @@ -0,0 +1,7 @@ +<library path="lib/libfranka_hw"> + <class name="franka_hw/FrankaCombinableHW" type="franka_hw::FrankaCombinableHW" base_class_type="hardware_interface::RobotHW"> + <description> + A robot hardware interface class for franka robots that is combinable with other FrankaCombinableHW. + </description> + </class> +</library> diff --git a/franka_hw/include/franka_hw/franka_combinable_hw.h b/franka_hw/include/franka_hw/franka_combinable_hw.h new file mode 100644 index 0000000000000000000000000000000000000000..4d2ed2191c1b09ecf3de9efaee3e2ab7d0084dde --- /dev/null +++ b/franka_hw/include/franka_hw/franka_combinable_hw.h @@ -0,0 +1,172 @@ +// Copyright (c) 2019 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#pragma once + +#include <atomic> +#include <functional> +#include <memory> +#include <string> +#include <thread> + +#include <franka/exception.h> +#include <franka/robot_state.h> + +#include <actionlib/server/simple_action_server.h> +#include <franka_hw/franka_hw.h> +#include <franka_hw/services.h> +#include <franka_msgs/ErrorRecoveryAction.h> +#include <ros/ros.h> + +namespace franka_hw { + +/** + * A hardware class for a Panda robot based on the ros_control framework. + * This class is ready to be combined with other hardware classes e.g. to + * control mulitple robots from a single controller. + * Note: This class allows for torque (effort) control only due to the lack of synchronization + * between master controllers of different robots. For more information see the documentation at + * https://frankaemika.github.io/docs/franka_ros.html . + */ +class FrankaCombinableHW : public FrankaHW { + public: + /** + * Creates an instance of FrankaCombinableHW. + */ + FrankaCombinableHW(); + + /** + * Initializes the class in terms of ros_control interfaces. + * Note: You have to call initParameters beforehand. Use the complete initialization routine + * \ref init() method to control robots. + * + * @param[in] robot_hw_nh A node handle in the namespace of the robot hardware. + * @return True if successful, false otherwise. + */ + void initROSInterfaces(ros::NodeHandle& robot_hw_nh) override; + + /** + * Runs the currently active controller in a realtime loop. If no controller is active, the + * function immediately exits. + * + * @param[in] ros_callback A callback function that is executed at each time step. + * + * @throw franka::ControlException if an error related to torque control occured. + * @throw franka::InvalidOperationException if a conflicting operation is already running. + * @throw franka::NetworkException if the connection is lost, e.g. after a timeout. + * @throw franka::RealtimeException if realtime priority cannot be set for the current thread. + */ + void control(const std::function<bool(const ros::Time&, const ros::Duration&)>& + ros_callback = // NOLINT (google-default-arguments) + [](const ros::Time&, const ros::Duration&) { + return true; + }) const override; // NOLINT (google-default-arguments) + + /** + * Checks whether a requested controller can be run, based on the resources and interfaces it + * claims. Note: FrankaCombinableHW allows torque control only. + * + * @param[in] info Controllers to be running at the same time. + * + * @return True in case of a conflict, false in case of valid controllers. + */ + bool checkForConflict(const std::list<hardware_interface::ControllerInfo>& info) const override; + + /** + * Reads data from the franka robot. + * + * @param[in] time The current time. Not used in this class. + * @param[in] period The time passed since the last call to \ref read. Not used in this class. + */ + void read(const ros::Time& /*time*/, const ros::Duration& /*period*/) override; + + /** + * Writes data to the franka robot. + * + * @param[in] time The current time. Not used in this class. + * @param[in] period The time passed since the last call to \ref write. + */ + void write(const ros::Time& /*time*/, const ros::Duration& period) override; + + /** + * Getter method for the arm_id which is used to distinguish between mulitple + * instances of FrankaCombinableHW. + * + * @return A copy of the arm_id string indentifying the class instance. + */ + std::string getArmID() const noexcept; + + /** + * Triggers a stop of the controlLoop. This interface is used to stop all combined + * robots together when at one robot encounters an error. + */ + void triggerError(); + + /** + * Getter for the error flag of the class. + * + * @return True in case of an error false otherwise. + */ + bool hasError() const noexcept; + + /** + * Recovers the libfranka robot, resets the error flag and publishes the error state. + */ + void resetError(); + + /** + * Returns whether the controller needs to be reset e.g. after error recovery. + * + * @return A copy of the controller_needs_reset flag. + */ + bool controllerNeedsReset() const noexcept; + + private: + template <typename T> + T libfrankaUpdateCallback(const T& command, + const franka::RobotState& robot_state, + franka::Duration time_step) { + if (commandHasNaN(command)) { + std::string error_message = "FrankaCombinableHW: Got NaN value in command!"; + ROS_FATAL("%s", error_message.c_str()); + throw std::invalid_argument(error_message); + } + checkJointLimits(); + libfranka_state_mutex_.lock(); + robot_state_libfranka_ = robot_state; + libfranka_state_mutex_.unlock(); + libfranka_cmd_mutex_.lock(); + T current_cmd = command; + libfranka_cmd_mutex_.unlock(); + if (has_error_ || !controller_active_) { + return franka::MotionFinished(current_cmd); + } + return current_cmd; + } + + void publishErrorState(const bool error); + + void setupServicesAndActionServers(ros::NodeHandle& node_handle); + + void initRobot() override; + + bool setRunFunction(const ControlMode& requested_control_mode, + const bool limit_rate, + const double cutoff_frequency, + const franka::ControllerMode internal_controller) override; + + void controlLoop(); + + static std::array<double, 7> saturateTorqueRate(const std::array<double, 7>& tau_d_calculated, + const std::array<double, 7>& tau_J_d); + + std::unique_ptr<std::thread> control_loop_thread_; + ServiceContainer services_; + std::unique_ptr<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>> + recovery_action_server_; + std::atomic_bool has_error_{false}; + ros::Publisher has_error_pub_; + std::atomic_bool error_recovered_{false}; + std::atomic_bool controller_needs_reset_{false}; +}; + +} // namespace franka_hw diff --git a/franka_combinable_hw/include/franka_combinable_hw/franka_combined_hw.h b/franka_hw/include/franka_hw/franka_combined_hw.h similarity index 86% rename from franka_combinable_hw/include/franka_combinable_hw/franka_combined_hw.h rename to franka_hw/include/franka_hw/franka_combined_hw.h index d1a733a956b9920426eafcd3e5dcfc1275effb1f..ea92d54a333bc3b0836da371390f35efd2cf7e0c 100644 --- a/franka_combinable_hw/include/franka_combinable_hw/franka_combined_hw.h +++ b/franka_hw/include/franka_hw/franka_combined_hw.h @@ -3,8 +3,8 @@ #pragma once #include <combined_robot_hw/combined_robot_hw.h> -#include <franka_combinable_hw/franka_combinable_hw.h> -#include <franka_control/ErrorRecoveryAction.h> +#include <franka_hw/franka_combinable_hw.h> +#include <franka_msgs/ErrorRecoveryAction.h> #include <actionlib/server/simple_action_server.h> #include <ros/node_handle.h> @@ -12,7 +12,7 @@ #include <memory> -namespace franka_combinable_hw { +namespace franka_hw { class FrankaCombinedHW : public combined_robot_hw::CombinedRobotHW { public: @@ -51,7 +51,7 @@ class FrankaCombinedHW : public combined_robot_hw::CombinedRobotHW { bool controllerNeedsReset(); protected: - std::unique_ptr<actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction>> + std::unique_ptr<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>> combined_recovery_action_server_; private: @@ -61,4 +61,4 @@ class FrankaCombinedHW : public combined_robot_hw::CombinedRobotHW { bool is_recovering_{false}; }; -} // namespace franka_combinable_hw +} // namespace franka_hw diff --git a/franka_hw/include/franka_hw/franka_hw.h b/franka_hw/include/franka_hw/franka_hw.h index 99f1fc1e64fca8fef17b3b99ff4198b02f323dd3..0140d0228012869a9e8002f009bb8c42e10da53a 100644 --- a/franka_hw/include/franka_hw/franka_hw.h +++ b/franka_hw/include/franka_hw/franka_hw.h @@ -4,6 +4,7 @@ #include <array> #include <atomic> +#include <exception> #include <functional> #include <list> #include <string> @@ -13,11 +14,6 @@ #include <franka/rate_limiting.h> #include <franka/robot.h> #include <franka/robot_state.h> -#include <franka_hw/control_mode.h> -#include <franka_hw/franka_cartesian_command_interface.h> -#include <franka_hw/franka_model_interface.h> -#include <franka_hw/franka_state_interface.h> -#include <franka_hw/resource_helpers.h> #include <hardware_interface/joint_command_interface.h> #include <hardware_interface/joint_state_interface.h> #include <hardware_interface/robot_hw.h> @@ -26,67 +22,75 @@ #include <ros/time.h> #include <urdf/model.h> +#include <franka_hw/control_mode.h> +#include <franka_hw/franka_cartesian_command_interface.h> +#include <franka_hw/franka_model_interface.h> +#include <franka_hw/franka_state_interface.h> +#include <franka_hw/resource_helpers.h> + namespace franka_hw { +/** + * This class wraps the functionality of libfranka for controlling Panda robots into the ros_control + * framework. + */ class FrankaHW : public hardware_interface::RobotHW { public: - FrankaHW() = delete; - - /** - * Creates an instance of FrankaHW that does not provide a model interface. - * - * @param[in] joint_names An array of joint names being controlled. - * @param[in] arm_id Unique identifier for the robot being controlled. - * @param[in] urdf_model A URDF model to initialize joint limits from. - * @param[in] get_limit_rate Getter that should return true if the rate limiter - * should be used, false otherwise. Defaults to true. - * @param[in] get_cutoff_frequency Getter for cutoff frequency for the low-pass filter. - * Defaults to franka::kDefaultCutoffFrequency. - * @param[in] get_internal_controller Getter for an internal controller to - * be used for control loops using only motion generation. Defaults to joint impedance. - */ - FrankaHW(const std::array<std::string, 7>& joint_names, - const std::string& arm_id, - const urdf::Model& urdf_model, - std::function<bool()> get_limit_rate = []() { return true; }, - std::function<double()> get_cutoff_frequency = - []() { return franka::kDefaultCutoffFrequency; }, - std::function<franka::ControllerMode()> get_internal_controller = - []() { return franka::ControllerMode::kJointImpedance; }); - - /** - * Creates an instance of FrankaHW that provides a model interface. - * - * @param[in] joint_names An array of joint names being controlled. - * @param[in] arm_id Unique identifier for the robot being controlled. - * @param[in] urdf_model A URDF model to initialize joint limits from. - * @param[in] model Robot model. - * @param[in] get_limit_rate Getter that should return true if the rate limiter - * should be used, false otherwise. Defaults to true. - * @param[in] get_cutoff_frequency Getter for cutoff frequency for the low-pass filter. - * Defaults to franka::kDefaultCutoffFrequency. - * @param[in] get_internal_controller Getter for an internal controller to - * be used for control loops using only motion generation. Defaults to joint impedance. - */ - FrankaHW(const std::array<std::string, 7>& joint_names, - const std::string& arm_id, - const urdf::Model& urdf_model, - franka::Model& model, - std::function<bool()> get_limit_rate = []() { return true; }, - std::function<double()> get_cutoff_frequency = - []() { return franka::kDefaultCutoffFrequency; }, - std::function<franka::ControllerMode()> get_internal_controller = - []() { return franka::ControllerMode::kJointImpedance; }); + /** + * Default constructor. + * Note: Be sure to call the init() method before operation. + */ + FrankaHW(); virtual ~FrankaHW() override = default; + /** + * Initializes the FrankaHW class to be fully operational. This involves parsing required + * configurations from the ROS parameter server, connecting to the robot and setting up interfaces + * for the ros_control framework. + * + * @param[in] root_nh A node handle in the root namespace of the control node. + * @param[in] robot_hw_nh A node handle in the namespace of the robot hardware. + * + * @return True if successful, false otherwise. + */ + virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override; + + /** + * Reads the parameterizsation of the hardware class from the ROS parameter server + * (e.g. arm_id, robot_ip joint_names etc.) + * + * @param[in] root_nh A node handle in the root namespace of the control node. + * @param[in] robot_hw_nh A node handle in the namespace of the robot hardware. + * + * @return True if successful, false otherwise. + */ + virtual bool initParameters(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh); + + /** + * Initializes the class in terms of ros_control interfaces. + * Note: You have to call initParameters beforehand. Use the complete initialization routine + * \ref init() method to control robots. + * + * @param[in] robot_hw_nh A node handle in the namespace of the robot hardware. + * @return True if successful, false otherwise. + */ + virtual void initROSInterfaces(ros::NodeHandle& robot_hw_nh); + + /** + * Initializes the callbacks for on-the-fly reading the parameters for rate limiting, + * internal controllers and cutoff frequency. + * + * @param[in] robot_hw_nh A node handle in the namespace of the robot hardware. + */ + virtual void setupParameterCallbacks(ros::NodeHandle& robot_hw_nh); + /** * Runs the currently active controller in a realtime loop. * * If no controller is active, the function immediately exits. When running a controller, * the function only exits when ros_callback returns false. * - * @param[in] robot Robot instance. * @param[in] ros_callback A callback function that is executed at each time step and runs * all ROS-side functionality of the hardware. Execution is stopped if it returns false. * @@ -97,16 +101,12 @@ class FrankaHW : public hardware_interface::RobotHW { * @throw franka::RealtimeException if realtime priority cannot be set for the current thread. */ virtual void control( - franka::Robot& robot, - const std::function<bool(const ros::Time&, const ros::Duration&)>& ros_callback); + const std::function<bool(const ros::Time&, const ros::Duration&)>& ros_callback) const; /** * Updates the controller interfaces from the given robot state. * * @param[in] robot_state Current robot state. - * - * @throw franka::InvalidOperationException if a conflicting operation is already running. - * @throw franka::NetworkException if the connection is lost. */ virtual void update(const franka::RobotState& robot_state); @@ -131,8 +131,9 @@ class FrankaHW : public hardware_interface::RobotHW { /** * Performs controller switching (real-time capable). */ - virtual void doSwitch(const std::list<hardware_interface::ControllerInfo>&, - const std::list<hardware_interface::ControllerInfo>&) override; + virtual void doSwitch( + const std::list<hardware_interface::ControllerInfo>& /*start_list*/, + const std::list<hardware_interface::ControllerInfo>& /*stop_list*/) override; /** * Prepares switching between controllers (not real-time capable). @@ -149,28 +150,28 @@ class FrankaHW : public hardware_interface::RobotHW { /** * Gets the current joint position command. * - * @return Current joint position command. + * @return The current joint position command. */ virtual std::array<double, 7> getJointPositionCommand() const noexcept; /** * Gets the current joint velocity command. * - * @return Current joint velocity command. + * @return The current joint velocity command. */ virtual std::array<double, 7> getJointVelocityCommand() const noexcept; /** * Gets the current joint torque command. * - * @return Current joint torque command. + * @return The current joint torque command. */ virtual std::array<double, 7> getJointEffortCommand() const noexcept; /** * Enforces limits on position, velocity, and torque level. * - * @param[in] period Duration of the current cycle. + * @param[in] period The duration of the current cycle. */ virtual void enforceLimits(const ros::Duration& period); @@ -185,29 +186,137 @@ class FrankaHW : public hardware_interface::RobotHW { */ virtual void checkJointLimits(); + /** + * Reads data from the franka robot. + * + * @param[in] time The current time. + * @param[in] period The time passed since the last call to \ref read. + */ + virtual void read(const ros::Time& time, const ros::Duration& period) override; + + /** + * Writes data to the franka robot. + * + * @param[in] time The current time. + * @param[in] period The time passed since the last call to \ref write. + */ + virtual void write(const ros::Time& time, const ros::Duration& period) override; + + /** + * Getter for the libfranka robot instance. + */ + virtual franka::Robot& robot() const; + + /** + * Checks a command for NaN values. + * + * @param[in] command The command to check. + * + * @return True if the command contains NaN, false otherwise. + */ + static bool commandHasNaN(const franka::Torques& command); + + /** + * Checks a command for NaN values. + * + * @param[in] command The command to check. + * + * @return True if the command contains NaN, false otherwise. + */ + static bool commandHasNaN(const franka::JointPositions& command); + + /** + * Checks a command for NaN values. + * + * @param[in] command The command to check. + * + * @return True if the command contains NaN, false otherwise. + */ + static bool commandHasNaN(const franka::JointVelocities& command); + + /** + * Checks a command for NaN values. + * + * @param[in] command The command to check. + * + * @return True if the command contains NaN, false otherwise. + */ + static bool commandHasNaN(const franka::CartesianPose& command); + + /** + * Checks a command for NaN values. + * + * @param[in] command The command to check. + * + * @return True if the command contains NaN, false otherwise. + */ + static bool commandHasNaN(const franka::CartesianVelocities& command); + protected: + /** + * Checks whether an array of doubles contains NaN values. + * + * @param[in] command array The array to check. + * + * @return True if the array contains NaN values, false otherwise. + */ + template <size_t size> + static bool arrayHasNaN(const std::array<double, size> array) { + return std::any_of(array.begin(), array.end(), [](const double& e) { return std::isnan(e); }); + } + using Callback = std::function<bool(const franka::RobotState&, franka::Duration)>; + + /** + * Callback for the libfranka control loop. This method is designed to incorporate a + * second callback named ros_callback that will be called on each iteration of the callback. + * + * @param[in] command The datafield containing the command to send to the robot. + * @param[in] ros_callback An additional callback that is executed every time step. + * @param[in] robot_state The current robot state to compute commands with. + * @param[in] time_step Time since last call to the callback. + * @throw std::invalid_argument When a command contains NaN values. + * + * @return The command to be sent to the robot via libfranka. + */ template <typename T> T controlCallback(const T& command, Callback ros_callback, const franka::RobotState& robot_state, franka::Duration time_step) { - robot_state_ = robot_state; + robot_state_libfranka_ = robot_state; + ros::Time now = ros::Time(0); + read(now, ros::Duration(time_step.toSec())); + if (!controller_active_ || (ros_callback && !ros_callback(robot_state, time_step))) { return franka::MotionFinished(command); } + + write(now, ros::Duration(time_step.toSec())); + if (commandHasNaN(command)) { + std::string error_message = "FrankaHW::controlCallback: Got NaN command!"; + ROS_FATAL("%s", error_message.c_str()); + throw std::invalid_argument(error_message); + } + return command; } + /** + * Configures a limit interface to enforce limits on effort, velocity or position level + * on joint commands. + * + * @param[out] limit_interface The limit interface to set up. + * @param[out] command_interface The command interface to hook the limit interface to. + */ template <typename T> - void setupLimitInterface(const urdf::Model& urdf_model, - joint_limits_interface::JointLimitsInterface<T>& limit_interface, + void setupLimitInterface(joint_limits_interface::JointLimitsInterface<T>& limit_interface, hardware_interface::JointCommandInterface& command_interface) { joint_limits_interface::SoftJointLimits soft_limits; joint_limits_interface::JointLimits joint_limits; for (size_t i = 0; i < joint_names_.size(); i++) { const std::string& joint_name = joint_names_[i]; - auto urdf_joint = urdf_model.getJoint(joint_name); + auto urdf_joint = urdf_model_.getJoint(joint_name); if (!urdf_joint || !urdf_joint->safety || !urdf_joint->limits) { ROS_WARN( "FrankaHW: Joint %s has incomplete limits and safety specs. Skipping it in the joint " @@ -236,8 +345,22 @@ class FrankaHW : public hardware_interface::RobotHW { } } + /** + * Configures and registers the joint state interface in ros_control. + * + * @param[in] robot_state The data field holding the updated robot state. + */ virtual void setupJointStateInterface(franka::RobotState& robot_state); + /** + * Configures and registers a joint command interface for positions velocities or efforts in + * ros_control. + * + * @param[in] command The data field holding the command to execute. + * @param[in] state The data field holding the updated robot state. + * @param[in] use_q_d Flag to configure using desired values as joint_states. + * @param[out] interface The command interface to configure. + */ template <typename T> void setupJointCommandInterface(std::array<double, 7>& command, franka::RobotState& state, @@ -258,22 +381,86 @@ class FrankaHW : public hardware_interface::RobotHW { registerInterface(&interface); } + /** + * Configures and registers the state interface offering the full franka::RobotState in + * ros_control. + * + * @param[in] robot_state The data field holding the updated robot state. + */ virtual void setupFrankaStateInterface(franka::RobotState& robot_state); + /** + * Configures and registers the command interface for Cartesian poses in ros_control. + * + * @param[in] pose_cartesian_command The data field holding the command to execute. + */ virtual void setupFrankaCartesianPoseInterface(franka::CartesianPose& pose_cartesian_command); + /** + * Configures and registers the command interface for Cartesian velocities in ros_control. + * + * @param[in] velocity_cartesian_command The data field holding the command to execute. + */ virtual void setupFrankaCartesianVelocityInterface( franka::CartesianVelocities& velocity_cartesian_command); - virtual void setupFrankaModelInterface(franka::Model& model, franka::RobotState& robot_state); + /** + * Configures and registers the model interface offering kinematics and dynamics in ros_control. + * + * @param[in] robot_state A reference to the data field storing the current robot state. This + * state is used to evaluate model qunatities (by default) at the current state. + */ + virtual void setupFrankaModelInterface(franka::RobotState& robot_state); + /** + * Configures the run function which is used as libfranka control callback based on the requested + * control mode. + * + * @param[in] requested_control_mode The control mode to configure (e.g. torque/position/velocity + * etc.) + * @param[in] limit_rate Flag to enable/disable rate limiting to smoothen the commands. + * @param[in] cutoff_frequency The cutoff frequency applied for command smoothing. + * @param[in] internal_controller The internal controller to use when using position or velocity + * modes. + * + * @return True if successful, false otherwise. + */ virtual bool setRunFunction(const ControlMode& requested_control_mode, const bool limit_rate, const double cutoff_frequency, const franka::ControllerMode internal_controller); + /** + * Uses the robot_ip_ to connect to the robot via libfranka and loads the libfranka model. + */ + virtual void initRobot(); - virtual void init(); - + /** + * Parses a set of collision thresholds from the parameter server. The methods returns + * the default values if no parameter was found or the size of the array did not match + * the defaults dimension. + * + * @param[in] name The name of the parameter to look for. + * @param[in] robot_hw_nh A node handle in the namespace of the robot hardware. + * @param[in] defaults A set of default values that also specify the size the parameter must have + * to be valid. + * @return A set parsed parameters if valid parameters where found, the default values otherwise. + */ + static std::vector<double> getCollisionThresholds(const std::string& name, + ros::NodeHandle& robot_hw_nh, + const std::vector<double>& defaults); + + struct CollisionConfig { + std::array<double, 7> lower_torque_thresholds_acceleration; + std::array<double, 7> upper_torque_thresholds_acceleration; + std::array<double, 7> lower_torque_thresholds_nominal; + std::array<double, 7> upper_torque_thresholds_nominal; + std::array<double, 6> lower_force_thresholds_acceleration; + std::array<double, 6> upper_force_thresholds_acceleration; + std::array<double, 6> lower_force_thresholds_nominal; + std::array<double, 6> upper_force_thresholds_nominal; + }; + + CollisionConfig collision_config_; hardware_interface::JointStateInterface joint_state_interface_{}; FrankaStateInterface franka_state_interface_{}; hardware_interface::PositionJointInterface position_joint_interface_{}; @@ -286,27 +473,42 @@ class FrankaHW : public hardware_interface::RobotHW { joint_limits_interface::VelocityJointSoftLimitsInterface velocity_joint_limit_interface_{}; joint_limits_interface::EffortJointSoftLimitsInterface effort_joint_limit_interface_{}; - franka::RobotState robot_state_{}; + std::mutex libfranka_state_mutex_; + std::mutex ros_state_mutex_; + franka::RobotState robot_state_libfranka_{}; + franka::RobotState robot_state_ros_{}; - std::array<std::string, 7> joint_names_; - const std::string arm_id_; - std::function<franka::ControllerMode()> get_internal_controller_; - std::function<bool()> get_limit_rate_; - std::function<double()> get_cutoff_frequency_; + std::mutex libfranka_cmd_mutex_; + franka::JointPositions position_joint_command_libfranka_; + franka::JointVelocities velocity_joint_command_libfranka_; + franka::Torques effort_joint_command_libfranka_; + franka::CartesianPose pose_cartesian_command_libfranka_; + franka::CartesianVelocities velocity_cartesian_command_libfranka_; - franka::JointPositions position_joint_command_; - franka::JointVelocities velocity_joint_command_; - franka::Torques effort_joint_command_; - franka::CartesianPose pose_cartesian_command_; - franka::CartesianVelocities velocity_cartesian_command_; + std::mutex ros_cmd_mutex_; + franka::JointPositions position_joint_command_ros_; + franka::JointVelocities velocity_joint_command_ros_; + franka::Torques effort_joint_command_ros_; + franka::CartesianPose pose_cartesian_command_ros_; + franka::CartesianVelocities velocity_cartesian_command_ros_; - std::function<void(franka::Robot&, Callback)> run_function_; + std::unique_ptr<franka::Robot> robot_; + std::unique_ptr<franka::Model> model_; + std::array<std::string, 7> joint_names_; + std::string arm_id_; + std::string robot_ip_; urdf::Model urdf_model_; double joint_limit_warning_threshold_{0.1}; + bool initialized_{false}; std::atomic_bool controller_active_{false}; ControlMode current_control_mode_ = ControlMode::None; + + std::function<franka::ControllerMode()> get_internal_controller_; + std::function<bool()> get_limit_rate_; + std::function<double()> get_cutoff_frequency_; + std::function<void(franka::Robot&, Callback)> run_function_; }; } // namespace franka_hw diff --git a/franka_hw/include/franka_hw/resource_helpers.h b/franka_hw/include/franka_hw/resource_helpers.h index 4f2b2440bd7453469d96fbe4ca1796cb86d3fcd4..f2231ba58d3d3867f2942b6e22224d19087e6e46 100644 --- a/franka_hw/include/franka_hw/resource_helpers.h +++ b/franka_hw/include/franka_hw/resource_helpers.h @@ -40,4 +40,6 @@ bool hasConflictingJointAndCartesianClaim(const ArmClaimedMap& arm_claim_map, bool partiallyClaimsArmJoints(const ArmClaimedMap& arm_claim_map, const std::string& arm_id); +bool hasTrajectoryClaim(const ArmClaimedMap& arm_claim_map, const std::string& arm_id); + } // namespace franka_hw diff --git a/franka_hw/include/franka_hw/services.h b/franka_hw/include/franka_hw/services.h new file mode 100644 index 0000000000000000000000000000000000000000..a7b3c55e4e0e722bbc37e5df5b7e0305397d8e70 --- /dev/null +++ b/franka_hw/include/franka_hw/services.h @@ -0,0 +1,161 @@ +// Copyright (c) 2017 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#pragma once + +#include <franka/exception.h> +#include <franka/robot.h> +#include <ros/console.h> +#include <ros/node_handle.h> +#include <ros/service_server.h> + +#include <franka_msgs/SetCartesianImpedance.h> +#include <franka_msgs/SetEEFrame.h> +#include <franka_msgs/SetForceTorqueCollisionBehavior.h> +#include <franka_msgs/SetFullCollisionBehavior.h> +#include <franka_msgs/SetJointImpedance.h> +#include <franka_msgs/SetKFrame.h> +#include <franka_msgs/SetLoad.h> + +#include <vector> + +namespace franka_hw { + +/** + * Advertises a service that acts according to the handler function which is used in the service + * callback. + * + * @param[in] node_handle The NodeHandle in the namespace at which to advertise the service. + * @param[in] name The name of the service. + * @param[in] handler The callback method for the service. + * @return The service server. + */ +template <typename T> +ros::ServiceServer advertiseService( + ros::NodeHandle& node_handle, + const std::string& name, + std::function<void(typename T::Request&, typename T::Response&)> handler) { + return node_handle.advertiseService<typename T::Request, typename T::Response>( + name, [name, handler](typename T::Request& request, typename T::Response& response) { + try { + handler(request, response); + response.success = true; + ROS_DEBUG_STREAM(name << " succeeded."); + } catch (const franka::Exception& ex) { + ROS_ERROR_STREAM(name << " failed: " << ex.what()); + response.success = false; + response.error = ex.what(); + } + return true; + }); +} + +/** + * This class serves as container that gathers all possible service interfaces to a libfranka robot + * instance. + */ +class ServiceContainer { + public: + /** + * Advertises and adds a service to the container class. + * + * @return A reference to the service container to allow a fluent API. + */ + template <typename T, typename... TArgs> + ServiceContainer& advertiseService(TArgs&&... args) { + ros::ServiceServer server = franka_hw::advertiseService<T>(std::forward<TArgs>(args)...); + services_.push_back(server); + return *this; + } + + private: + std::vector<ros::ServiceServer> services_; +}; + +/** + * Sets up all services relevant for a libfranka robot inside a service container. + * + * @param[in] robot The libfranka robot for which to set up services interfaces. + * @param[in] node_handle The NodeHandle in the namespace at which to advertise the services. + * @param[in] services The container to store the service servers. + */ +void setupServices(franka::Robot& robot, ros::NodeHandle& node_handle, ServiceContainer& services); + +/** + * Callback for the service interface to franka::robot::setCartesianImpedance. + * + * @param[in] robot The libfranka robot for which to set up the service. + * @param[in] req The service request. + * @param[out] res The service response. + */ +void setCartesianImpedance(franka::Robot& robot, + const franka_msgs::SetCartesianImpedance::Request& req, + franka_msgs::SetCartesianImpedance::Response& res); + +/** + * Callback for the service interface to franka::robot::setJointImpedance. + * + * @param[in] robot The libfranka robot for which to set up the service. + * @param[in] req The service request. + * @param[out] res The service response. + */ +void setJointImpedance(franka::Robot& robot, + const franka_msgs::SetJointImpedance::Request& req, + franka_msgs::SetJointImpedance::Response& res); + +/** + * Callback for the service interface to franka::robot::setEEFrame. + * + * @param[in] robot The libfranka robot for which to set up the service. + * @param[in] req The service request. + * @param[out] res The service response. + */ +void setEEFrame(franka::Robot& robot, + const franka_msgs::SetEEFrame::Request& req, + franka_msgs::SetEEFrame::Response& res); + +/** + * Callback for the service interface to franka::robot::setKFrame. + * + * @param[in] robot The libfranka robot for which to set up the service. + * @param[in] req The service request. + * @param[out] res The service response. + */ +void setKFrame(franka::Robot& robot, + const franka_msgs::SetKFrame::Request& req, + franka_msgs::SetKFrame::Response& res); + +/** + * Callback for the service interface to franka::robot::setForceTorqueCollisionBehavior. + * + * @param[in] robot The libfranka robot for which to set up the service. + * @param[in] req The service request. + * @param[out] res The service response. + */ +void setForceTorqueCollisionBehavior( + franka::Robot& robot, + const franka_msgs::SetForceTorqueCollisionBehavior::Request& req, + franka_msgs::SetForceTorqueCollisionBehavior::Response& res); + +/** + * Callback for the service interface to franka::robot::setFullCollisionBehavior. + * + * @param[in] robot The libfranka robot for which to set up the service. + * @param[in] req The service request. + * @param[out] res The service response. + */ +void setFullCollisionBehavior(franka::Robot& robot, + const franka_msgs::SetFullCollisionBehavior::Request& req, + franka_msgs::SetFullCollisionBehavior::Response& res); + +/** + * Callback for the service interface to franka::robot::setLoad. + * + * @param[in] robot The libfranka robot for which to set up the service. + * @param[in] req The service request. + * @param[out] res The service response. + */ +void setLoad(franka::Robot& robot, + const franka_msgs::SetLoad::Request& req, + franka_msgs::SetLoad::Response& res); + +} // namespace franka_hw diff --git a/franka_hw/package.xml b/franka_hw/package.xml index 0ff25b93cf76894735bb1950e059774ef0a374d6..a61cec93a6301996d8073a7fe3c23025c099bc52 100644 --- a/franka_hw/package.xml +++ b/franka_hw/package.xml @@ -13,14 +13,25 @@ <buildtool_depend>catkin</buildtool_depend> + <build_depend>message_generation</build_depend> + + <depend>actionlib_msgs</depend> + <depend>actionlib</depend> <depend>controller_interface</depend> + <depend>combined_robot_hw</depend> <depend>hardware_interface</depend> <depend>joint_limits_interface</depend> <depend>libfranka</depend> <depend>roscpp</depend> <depend>urdf</depend> + <depend>pluginlib</depend> + <depend>franka_msgs</depend> <test_depend>franka_description</test_depend> <test_depend>gtest</test_depend> <test_depend>rostest</test_depend> + + <export> + <hardware_interface plugin="${prefix}/franka_combinable_hw_plugin.xml"/> + </export> </package> diff --git a/franka_hw/src/franka_combinable_hw.cpp b/franka_hw/src/franka_combinable_hw.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1451b244deb1f39e7165591c1bea912343076e95 --- /dev/null +++ b/franka_hw/src/franka_combinable_hw.cpp @@ -0,0 +1,226 @@ + +#include <hardware_interface/joint_command_interface.h> +#include <joint_limits_interface/joint_limits_interface.h> +#include <pluginlib/class_list_macros.h> +#include <std_msgs/Bool.h> + +#include <franka_hw/franka_combinable_hw.h> +#include <franka_hw/services.h> + +namespace franka_hw { + +FrankaCombinableHW::FrankaCombinableHW() : has_error_(false), error_recovered_(false) {} + +void FrankaCombinableHW::initROSInterfaces(ros::NodeHandle& robot_hw_nh) { + setupJointStateInterface(robot_state_ros_); + setupJointCommandInterface(effort_joint_command_ros_.tau_J, robot_state_ros_, false, + effort_joint_interface_); + setupLimitInterface<joint_limits_interface::EffortJointSoftLimitsHandle>( + effort_joint_limit_interface_, effort_joint_interface_); + setupFrankaStateInterface(robot_state_ros_); + setupFrankaModelInterface(robot_state_ros_); + + has_error_pub_ = robot_hw_nh.advertise<std_msgs::Bool>("has_error", 1, true); + publishErrorState(has_error_); + + setupServicesAndActionServers(robot_hw_nh); +} + +void FrankaCombinableHW::initRobot() { + FrankaHW::initRobot(); + control_loop_thread_ = std::make_unique<std::thread>(&FrankaCombinableHW::controlLoop, this); +} + +void FrankaCombinableHW::publishErrorState(const bool error) { + std_msgs::Bool msg; + msg.data = static_cast<int>(error); + has_error_pub_.publish(msg); +} + +void FrankaCombinableHW::controlLoop() { + while (ros::ok()) { + ros::Time last_time = ros::Time::now(); + + // Wait until controller has been activated or error has been recovered + while (!controllerActive() || has_error_) { + if (!controllerActive()) { + ROS_DEBUG_THROTTLE(1, "FrankaCombinableHW::%s::control_loop(): controller is not active.", + arm_id_.c_str()); + } + if (has_error_) { + ROS_DEBUG_THROTTLE(1, "FrankaCombinableHW::%s::control_loop(): an error has occured.", + arm_id_.c_str()); + } + + checkJointLimits(); + + ros_state_mutex_.lock(); + libfranka_state_mutex_.lock(); + robot_state_libfranka_ = robot_->readOnce(); + robot_state_ros_ = robot_->readOnce(); + libfranka_state_mutex_.unlock(); + ros_state_mutex_.unlock(); + if (!ros::ok()) { + return; + } + } + ROS_INFO("FrankaCombinableHW::%s::control_loop(): controller is active.", arm_id_.c_str()); + + // Reset commands + libfranka_cmd_mutex_.lock(); + effort_joint_command_libfranka_ = franka::Torques({0., 0., 0., 0., 0., 0., 0.}); + libfranka_cmd_mutex_.unlock(); + + try { + control(); + } catch (const franka::ControlException& e) { + // Reflex could be caught and it needs to wait for automatic error recovery + ROS_ERROR("%s", e.what()); + has_error_ = true; + publishErrorState(has_error_); + } + } +} + +void FrankaCombinableHW::setupServicesAndActionServers(ros::NodeHandle& node_handle) { + setupServices(*robot_, node_handle, services_); + recovery_action_server_ = + std::make_unique<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>>( + node_handle, "error_recovery", + [&](const franka_msgs::ErrorRecoveryGoalConstPtr&) { + try { + robot_->automaticErrorRecovery(); + // error recovered => reset controller + if (has_error_) { + error_recovered_ = true; + } + has_error_ = false; + publishErrorState(has_error_); + recovery_action_server_->setSucceeded(); + } catch (const franka::Exception& ex) { + recovery_action_server_->setAborted(franka_msgs::ErrorRecoveryResult(), ex.what()); + } + }, + false); + recovery_action_server_->start(); +} + +void FrankaCombinableHW::control( // NOLINT (google-default-arguments) + const std::function<bool(const ros::Time&, const ros::Duration&)>& /*ros_callback*/) const { + if (!controller_active_) { + return; + } + auto empty_method = [](const franka::RobotState&, franka::Duration) { return true; }; + run_function_(*robot_, empty_method); +} + +bool FrankaCombinableHW::checkForConflict( + const std::list<hardware_interface::ControllerInfo>& info) const { + ResourceWithClaimsMap resource_map = getResourceMap(info); + + if (hasConflictingMultiClaim(resource_map)) { + return true; + } + + ArmClaimedMap arm_claim_map; + if (!getArmClaimedMap(resource_map, arm_claim_map)) { + ROS_ERROR("FrankaCombinableHW: Unknown interface claimed. Conflict!"); + return true; + } + + // check for any claim to trajectory interfaces (non-torque) which are not supported. + if (hasTrajectoryClaim(arm_claim_map, arm_id_)) { + ROS_ERROR_STREAM("FrankaCombinableHW: Invalid claim joint position or velocity interface." + << "Note: joint position and joint velocity interfaces are not supported" + << " in FrankaCombinableHW. Arm:" << arm_id_ << ". Conflict!"); + return true; + } + + return partiallyClaimsArmJoints(arm_claim_map, arm_id_); +} + +void FrankaCombinableHW::read(const ros::Time& time, const ros::Duration& period) { + controller_needs_reset_ = bool(error_recovered_); + FrankaHW::read(time, period); +} + +void FrankaCombinableHW::write(const ros::Time& time, const ros::Duration& period) { + // if flag `controller_needs_reset_` was updated, then controller_manager. update(..., + // reset_controller) must + // have been executed to reset the controller. + if (controller_needs_reset_ && error_recovered_) { + controller_needs_reset_ = false; + error_recovered_ = false; + } + + enforceLimits(period); + + FrankaHW::write(time, period); +} + +std::array<double, 7> FrankaCombinableHW::saturateTorqueRate( + const std::array<double, 7>& tau_d_calculated, + const std::array<double, 7>& tau_J_d) { // NOLINT (readability-identifier-naming) + const double kDeltaTauMax = 1.0; + std::array<double, 7> tau_d_saturated{}; + for (size_t i = 0; i < 7; i++) { + double difference = tau_d_calculated[i] - tau_J_d[i]; + tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax); + } + return tau_d_saturated; +} + +std::string FrankaCombinableHW::getArmID() const noexcept { + return arm_id_; +} + +void FrankaCombinableHW::triggerError() { + has_error_ = true; + publishErrorState(has_error_); +} + +bool FrankaCombinableHW::hasError() const noexcept { + return has_error_; +} + +void FrankaCombinableHW::resetError() { + robot_->automaticErrorRecovery(); + // error recovered => reset controller + if (has_error_) { + error_recovered_ = true; + } + has_error_ = false; + publishErrorState(has_error_); +} + +bool FrankaCombinableHW::controllerNeedsReset() const noexcept { + return controller_needs_reset_; +} + +bool FrankaCombinableHW::setRunFunction(const ControlMode& requested_control_mode, + const bool limit_rate, + const double cutoff_frequency, + const franka::ControllerMode /*internal_controller*/) { + using Callback = std::function<bool(const franka::RobotState&, franka::Duration)>; + + if (requested_control_mode == ControlMode::None) { + return true; + } + if (requested_control_mode == ControlMode::JointTorque) { + run_function_ = [this, limit_rate, cutoff_frequency](franka::Robot& robot, + Callback /*callback*/) { + robot.control(std::bind(&FrankaCombinableHW::libfrankaUpdateCallback<franka::Torques>, this, + std::cref(effort_joint_command_libfranka_), std::placeholders::_1, + std::placeholders::_2), + limit_rate, cutoff_frequency); + }; + return true; + } + + ROS_ERROR("FrankaCombinableHW: No valid control mode selected; cannot set run function."); + return false; +} + +} // namespace franka_hw + +PLUGINLIB_EXPORT_CLASS(franka_hw::FrankaCombinableHW, hardware_interface::RobotHW) diff --git a/franka_combinable_hw/src/franka_combined_hw.cpp b/franka_hw/src/franka_combined_hw.cpp similarity index 78% rename from franka_combinable_hw/src/franka_combined_hw.cpp rename to franka_hw/src/franka_combined_hw.cpp index 42921fb54e78db1af40f72e9f27cf77d515fe939..a2fce7257ae2baa4edea30cebe9b94ec6e499d71 100644 --- a/franka_combinable_hw/src/franka_combined_hw.cpp +++ b/franka_hw/src/franka_combined_hw.cpp @@ -1,10 +1,10 @@ // Copyright (c) 2019 Franka Emika GmbH // Use of this source code is governed by the Apache-2.0 license, see LICENSE -#include <franka_combinable_hw/franka_combinable_hw.h> +#include <franka_hw/franka_combinable_hw.h> -#include <franka_combinable_hw/franka_combined_hw.h> -#include <franka_control/ErrorRecoveryAction.h> +#include <franka_hw/franka_combined_hw.h> #include <franka_hw/franka_hw.h> +#include <franka_msgs/ErrorRecoveryAction.h> #include <actionlib/server/simple_action_server.h> #include <ros/node_handle.h> @@ -13,7 +13,7 @@ #include <algorithm> #include <memory> -namespace franka_combinable_hw { +namespace franka_hw { FrankaCombinedHW::FrankaCombinedHW() = default; @@ -21,14 +21,14 @@ bool FrankaCombinedHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_ bool success = CombinedRobotHW::init(root_nh, robot_hw_nh); // Error recovery server for all FrankaHWs combined_recovery_action_server_ = - std::make_unique<actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction>>( + std::make_unique<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>>( robot_hw_nh, "error_recovery", - [&](const franka_control::ErrorRecoveryGoalConstPtr&) { + [&](const franka_msgs::ErrorRecoveryGoalConstPtr&) { try { is_recovering_ = true; for (const auto& robot_hw : robot_hw_list_) { auto* franka_combinable_hw_ptr = - dynamic_cast<franka_combinable_hw::FrankaCombinableHW*>(robot_hw.get()); + dynamic_cast<franka_hw::FrankaCombinableHW*>(robot_hw.get()); if (franka_combinable_hw_ptr != nullptr) { franka_combinable_hw_ptr->resetError(); } else { @@ -36,7 +36,7 @@ bool FrankaCombinedHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_ "FrankaCombinedHW: dynamic_cast from RobotHW to FrankaCombinableHW failed."); is_recovering_ = false; combined_recovery_action_server_->setAborted( - franka_control::ErrorRecoveryResult(), + franka_msgs::ErrorRecoveryResult(), "dynamic_cast from RobotHW to FrankaCombinableHW failed"); return; } @@ -45,7 +45,7 @@ bool FrankaCombinedHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_ combined_recovery_action_server_->setSucceeded(); } catch (const franka::Exception& ex) { is_recovering_ = false; - combined_recovery_action_server_->setAborted(franka_control::ErrorRecoveryResult(), + combined_recovery_action_server_->setAborted(franka_msgs::ErrorRecoveryResult(), ex.what()); } }, @@ -64,8 +64,7 @@ bool FrankaCombinedHW::controllerNeedsReset() { // Check if any of the RobotHW object needs a controller reset bool controller_reset = false; for (const auto& robot_hw : robot_hw_list_) { - auto* franka_combinable_hw_ptr = - dynamic_cast<franka_combinable_hw::FrankaCombinableHW*>(robot_hw.get()); + auto* franka_combinable_hw_ptr = dynamic_cast<franka_hw::FrankaCombinableHW*>(robot_hw.get()); if (franka_combinable_hw_ptr != nullptr) { controller_reset = controller_reset || franka_combinable_hw_ptr->controllerNeedsReset(); } else { @@ -86,8 +85,7 @@ void FrankaCombinedHW::handleError() { bool FrankaCombinedHW::hasError() { bool has_error = false; for (const auto& robot_hw : robot_hw_list_) { - auto* franka_combinable_hw_ptr = - dynamic_cast<franka_combinable_hw::FrankaCombinableHW*>(robot_hw.get()); + auto* franka_combinable_hw_ptr = dynamic_cast<franka_hw::FrankaCombinableHW*>(robot_hw.get()); if (franka_combinable_hw_ptr != nullptr) { has_error = has_error || franka_combinable_hw_ptr->hasError(); } else { @@ -101,8 +99,7 @@ bool FrankaCombinedHW::hasError() { void FrankaCombinedHW::triggerError() { // Trigger error state of all RobotHW objects. for (const auto& robot_hw : robot_hw_list_) { - auto* franka_combinable_hw_ptr = - dynamic_cast<franka_combinable_hw::FrankaCombinableHW*>(robot_hw.get()); + auto* franka_combinable_hw_ptr = dynamic_cast<franka_hw::FrankaCombinableHW*>(robot_hw.get()); if (franka_combinable_hw_ptr != nullptr) { franka_combinable_hw_ptr->triggerError(); } else { @@ -111,4 +108,4 @@ void FrankaCombinedHW::triggerError() { } } -} // namespace franka_combinable_hw +} // namespace franka_hw diff --git a/franka_hw/src/franka_hw.cpp b/franka_hw/src/franka_hw.cpp index fe66d59f654c1f5e4f4d2ad2027320c7641574ff..c4cce12ab6c5bb9a7815e5d982152b39bfe8495e 100644 --- a/franka_hw/src/franka_hw.cpp +++ b/franka_hw/src/franka_hw.cpp @@ -5,8 +5,10 @@ #include <array> #include <cstdint> +#include <exception> #include <functional> #include <list> +#include <mutex> #include <ostream> #include <sstream> #include <string> @@ -40,45 +42,133 @@ std::string toStringWithPrecision(const double value, const size_t precision = 6 } // anonymous namespace -FrankaHW::FrankaHW(const std::array<std::string, 7>& joint_names, - const std::string& arm_id, - const urdf::Model& urdf_model, - std::function<bool()> get_limit_rate, - std::function<double()> get_cutoff_frequency, - std::function<franka::ControllerMode()> get_internal_controller) - : joint_names_(joint_names), - arm_id_(arm_id), - get_internal_controller_(std::move(get_internal_controller)), - get_limit_rate_(std::move(get_limit_rate)), - get_cutoff_frequency_(std::move(get_cutoff_frequency)), - position_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), - velocity_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), - effort_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), - pose_cartesian_command_( +FrankaHW::FrankaHW() + : position_joint_command_ros_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), + position_joint_command_libfranka_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), + velocity_joint_command_ros_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), + velocity_joint_command_libfranka_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), + effort_joint_command_ros_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), + effort_joint_command_libfranka_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), + pose_cartesian_command_ros_( {1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}), - velocity_cartesian_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), - urdf_model_(urdf_model) { - init(); -} - -FrankaHW::FrankaHW(const std::array<std::string, 7>& joint_names, - const std::string& arm_id, - const urdf::Model& urdf_model, - franka::Model& model, - std::function<bool()> get_limit_rate, - std::function<double()> get_cutoff_frequency, - std::function<franka::ControllerMode()> get_internal_controller) - : FrankaHW(joint_names, - arm_id, - urdf_model, - std::move(get_limit_rate), - std::move(get_cutoff_frequency), - std::move(get_internal_controller)) { - setupFrankaModelInterface(model, robot_state_); + pose_cartesian_command_libfranka_( + {1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}), + velocity_cartesian_command_ros_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), + velocity_cartesian_command_libfranka_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) {} + +bool FrankaHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { + if (initialized_) { + ROS_ERROR("FrankaHW: Cannot be initialized twice."); + return false; + } + + if (!initParameters(root_nh, robot_hw_nh)) { + ROS_ERROR("FrankaHW: Failed to parse all required parameters."); + return false; + } + try { + initRobot(); + } catch (const std::runtime_error& error) { + ROS_ERROR("FrankaHW: Failed to initialize libfranka robot. %s", error.what()); + return false; + } + initROSInterfaces(robot_hw_nh); + setupParameterCallbacks(robot_hw_nh); + + initialized_ = true; + return true; +} + +bool FrankaHW::initParameters(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { + std::vector<std::string> joint_names_vector; + if (!robot_hw_nh.getParam("joint_names", joint_names_vector) || joint_names_vector.size() != 7) { + ROS_ERROR("Invalid or no joint_names parameters provided"); + return false; + } + std::copy(joint_names_vector.cbegin(), joint_names_vector.cend(), joint_names_.begin()); + + bool rate_limiting; + if (!robot_hw_nh.getParamCached("rate_limiting", rate_limiting)) { + ROS_ERROR("Invalid or no rate_limiting parameter provided"); + return false; + } + + double cutoff_frequency; + if (!robot_hw_nh.getParamCached("cutoff_frequency", cutoff_frequency)) { + ROS_ERROR("Invalid or no cutoff_frequency parameter provided"); + return false; + } + + std::string internal_controller; + if (!robot_hw_nh.getParam("internal_controller", internal_controller)) { + ROS_ERROR("No internal_controller parameter provided"); + return false; + } + + if (!robot_hw_nh.getParam("arm_id", arm_id_)) { + ROS_ERROR("Invalid or no arm_id parameter provided"); + return false; + } + + if (!urdf_model_.initParamWithNodeHandle("robot_description", root_nh)) { + ROS_ERROR("Could not initialize URDF model from robot_description"); + return false; + } + + if (!robot_hw_nh.getParam("robot_ip", robot_ip_)) { + ROS_ERROR("Invalid or no robot_ip parameter provided"); + return false; + } + + if (!robot_hw_nh.getParam("joint_limit_warning_threshold", joint_limit_warning_threshold_)) { + ROS_INFO( + "No parameter joint_limit_warning_threshold is found, using default " + "value %f", + joint_limit_warning_threshold_); + } + + // Get full collision behavior config from the parameter server. + std::vector<double> thresholds = + getCollisionThresholds("lower_torque_thresholds_acceleration", robot_hw_nh, + {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}); + std::copy(thresholds.begin(), thresholds.end(), + collision_config_.lower_torque_thresholds_acceleration.begin()); + thresholds = getCollisionThresholds("upper_torque_thresholds_acceleration", robot_hw_nh, + {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}); + std::copy(thresholds.begin(), thresholds.end(), + collision_config_.upper_torque_thresholds_acceleration.begin()); + thresholds = getCollisionThresholds("lower_torque_thresholds_nominal", robot_hw_nh, + {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}); + std::copy(thresholds.begin(), thresholds.end(), + collision_config_.lower_torque_thresholds_nominal.begin()); + thresholds = getCollisionThresholds("upper_torque_thresholds_nominal", robot_hw_nh, + {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}); + std::copy(thresholds.begin(), thresholds.end(), + collision_config_.upper_torque_thresholds_nominal.begin()); + thresholds.resize(6); + thresholds = getCollisionThresholds("lower_force_thresholds_acceleration", robot_hw_nh, + {20.0, 20.0, 20.0, 25.0, 25.0, 25.0}); + std::copy(thresholds.begin(), thresholds.end(), + collision_config_.lower_force_thresholds_acceleration.begin()); + thresholds = getCollisionThresholds("upper_force_thresholds_acceleration", robot_hw_nh, + {20.0, 20.0, 20.0, 25.0, 25.0, 25.0}); + std::copy(thresholds.begin(), thresholds.end(), + collision_config_.upper_force_thresholds_acceleration.begin()); + thresholds = getCollisionThresholds("lower_force_thresholds_nominal", robot_hw_nh, + {20.0, 20.0, 20.0, 25.0, 25.0, 25.0}); + std::copy(thresholds.begin(), thresholds.end(), + collision_config_.lower_force_thresholds_nominal.begin()); + thresholds = getCollisionThresholds("upper_force_thresholds_nominal", robot_hw_nh, + {20.0, 20.0, 20.0, 25.0, 25.0, 25.0}); + std::copy(thresholds.begin(), thresholds.end(), + collision_config_.upper_force_thresholds_nominal.begin()); + + return true; } void FrankaHW::update(const franka::RobotState& robot_state) { - robot_state_ = robot_state; + std::lock_guard<std::mutex> ros_lock(ros_state_mutex_); + robot_state_ros_ = robot_state; } bool FrankaHW::controllerActive() const noexcept { @@ -86,18 +176,22 @@ bool FrankaHW::controllerActive() const noexcept { } void FrankaHW::control( - franka::Robot& robot, - const std::function<bool(const ros::Time&, const ros::Duration&)>& ros_callback) { + const std::function<bool(const ros::Time&, const ros::Duration&)>& ros_callback) const { + if (!initialized_) { + ROS_ERROR("FrankaHW: Call to control before initialization!"); + return; + } if (!controller_active_) { return; } - franka::Duration last_time = robot_state_.time; + franka::Duration last_time = robot_state_ros_.time; - run_function_(robot, [this, ros_callback, &last_time](const franka::RobotState& robot_state, - franka::Duration time_step) { + run_function_(*robot_, [this, ros_callback, &last_time](const franka::RobotState& robot_state, + franka::Duration time_step) { if (last_time != robot_state.time) { last_time = robot_state.time; + return ros_callback(ros::Time::now(), ros::Duration(time_step.toSec())); } return true; @@ -144,6 +238,7 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo> ROS_ERROR("FrankaHW: Unknown interface claimed for starting!"); return false; } + ControlMode start_control_mode = getControlMode(arm_id_, start_arm_claim_map); ResourceWithClaimsMap stop_resource_map = getResourceMap(stop_list); @@ -162,6 +257,7 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo> get_internal_controller_())) { return false; } + if (current_control_mode_ != requested_control_mode) { ROS_INFO_STREAM("FrankaHW: Prepared switching controllers to " << requested_control_mode << " with parameters " @@ -176,15 +272,15 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo> } std::array<double, 7> FrankaHW::getJointPositionCommand() const noexcept { - return position_joint_command_.q; + return position_joint_command_ros_.q; } std::array<double, 7> FrankaHW::getJointVelocityCommand() const noexcept { - return velocity_joint_command_.dq; + return velocity_joint_command_ros_.dq; } std::array<double, 7> FrankaHW::getJointEffortCommand() const noexcept { - return effort_joint_command_.tau_J; + return effort_joint_command_ros_.tau_J; } void FrankaHW::reset() { @@ -225,6 +321,31 @@ void FrankaHW::checkJointLimits() { } } +franka::Robot& FrankaHW::robot() const { + if (!initialized_ || !robot_) { + std::string error_message = "FrankaHW: Atempt to access robot before initialization!"; + ROS_ERROR("%s", error_message.c_str()); + throw std::logic_error(error_message); + } + return *robot_; +} + +void FrankaHW::read(const ros::Time& /*time*/, const ros::Duration& /*period*/) { + std::lock_guard<std::mutex> ros_lock(ros_state_mutex_); + std::lock_guard<std::mutex> libfranka_lock(libfranka_state_mutex_); + robot_state_ros_ = robot_state_libfranka_; +} + +void FrankaHW::write(const ros::Time& /*time*/, const ros::Duration& /*period*/) { + std::lock_guard<std::mutex> ros_lock(ros_cmd_mutex_); + std::lock_guard<std::mutex> libfranka_lock(libfranka_cmd_mutex_); + pose_cartesian_command_libfranka_ = pose_cartesian_command_ros_; + velocity_cartesian_command_libfranka_ = velocity_cartesian_command_ros_; + effort_joint_command_libfranka_ = effort_joint_command_ros_; + position_joint_command_libfranka_ = position_joint_command_ros_; + velocity_joint_command_libfranka_ = velocity_joint_command_ros_; +} + void FrankaHW::setupJointStateInterface(franka::RobotState& robot_state) { for (size_t i = 0; i < joint_names_.size(); i++) { hardware_interface::JointStateHandle joint_handle_q(joint_names_[i], &robot_state.q[i], @@ -257,10 +378,12 @@ void FrankaHW::setupFrankaCartesianVelocityInterface( registerInterface(&franka_velocity_cartesian_interface_); } -void FrankaHW::setupFrankaModelInterface(franka::Model& model, franka::RobotState& robot_state) { - franka_hw::FrankaModelHandle model_handle(arm_id_ + "_model", model, robot_state); - franka_model_interface_.registerHandle(model_handle); - registerInterface(&franka_model_interface_); +void FrankaHW::setupFrankaModelInterface(franka::RobotState& robot_state) { + if (model_) { + franka_hw::FrankaModelHandle model_handle(arm_id_ + "_model", *model_, robot_state); + franka_model_interface_.registerHandle(model_handle); + registerInterface(&franka_model_interface_); + } } bool FrankaHW::setRunFunction(const ControlMode& requested_control_mode, @@ -277,72 +400,74 @@ bool FrankaHW::setRunFunction(const ControlMode& requested_control_mode, case ControlMode::JointTorque: run_function_ = [=](franka::Robot& robot, Callback ros_callback) { robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this, - std::cref(effort_joint_command_), ros_callback, _1, _2), + std::cref(effort_joint_command_libfranka_), ros_callback, _1, _2), limit_rate, cutoff_frequency); }; break; case ControlMode::JointPosition: run_function_ = [=](franka::Robot& robot, Callback ros_callback) { robot.control(std::bind(&FrankaHW::controlCallback<franka::JointPositions>, this, - std::cref(position_joint_command_), ros_callback, _1, _2), + std::cref(position_joint_command_libfranka_), ros_callback, _1, _2), internal_controller, limit_rate, cutoff_frequency); }; break; case ControlMode::JointVelocity: run_function_ = [=](franka::Robot& robot, Callback ros_callback) { robot.control(std::bind(&FrankaHW::controlCallback<franka::JointVelocities>, this, - std::cref(velocity_joint_command_), ros_callback, _1, _2), + std::cref(velocity_joint_command_libfranka_), ros_callback, _1, _2), internal_controller, limit_rate, cutoff_frequency); }; break; case ControlMode::CartesianPose: run_function_ = [=](franka::Robot& robot, Callback ros_callback) { robot.control(std::bind(&FrankaHW::controlCallback<franka::CartesianPose>, this, - std::cref(pose_cartesian_command_), ros_callback, _1, _2), + std::cref(pose_cartesian_command_libfranka_), ros_callback, _1, _2), internal_controller, limit_rate, cutoff_frequency); }; break; case ControlMode::CartesianVelocity: run_function_ = [=](franka::Robot& robot, Callback ros_callback) { - robot.control(std::bind(&FrankaHW::controlCallback<franka::CartesianVelocities>, this, - std::cref(velocity_cartesian_command_), ros_callback, _1, _2), - internal_controller, limit_rate, cutoff_frequency); + robot.control( + std::bind(&FrankaHW::controlCallback<franka::CartesianVelocities>, this, + std::cref(velocity_cartesian_command_libfranka_), ros_callback, _1, _2), + internal_controller, limit_rate, cutoff_frequency); }; break; case (ControlMode::JointTorque | ControlMode::JointPosition): run_function_ = [=](franka::Robot& robot, Callback ros_callback) { robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this, - std::cref(effort_joint_command_), ros_callback, _1, _2), + std::cref(effort_joint_command_libfranka_), ros_callback, _1, _2), std::bind(&FrankaHW::controlCallback<franka::JointPositions>, this, - std::cref(position_joint_command_), ros_callback, _1, _2), + std::cref(position_joint_command_libfranka_), ros_callback, _1, _2), limit_rate, cutoff_frequency); }; break; case (ControlMode::JointTorque | ControlMode::JointVelocity): run_function_ = [=](franka::Robot& robot, Callback ros_callback) { robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this, - std::cref(effort_joint_command_), ros_callback, _1, _2), + std::cref(effort_joint_command_libfranka_), ros_callback, _1, _2), std::bind(&FrankaHW::controlCallback<franka::JointVelocities>, this, - std::cref(velocity_joint_command_), ros_callback, _1, _2), + std::cref(velocity_joint_command_libfranka_), ros_callback, _1, _2), limit_rate, cutoff_frequency); }; break; case (ControlMode::JointTorque | ControlMode::CartesianPose): run_function_ = [=](franka::Robot& robot, Callback ros_callback) { robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this, - std::cref(effort_joint_command_), ros_callback, _1, _2), + std::cref(effort_joint_command_libfranka_), ros_callback, _1, _2), std::bind(&FrankaHW::controlCallback<franka::CartesianPose>, this, - std::cref(pose_cartesian_command_), ros_callback, _1, _2), + std::cref(pose_cartesian_command_libfranka_), ros_callback, _1, _2), limit_rate, cutoff_frequency); }; break; case (ControlMode::JointTorque | ControlMode::CartesianVelocity): run_function_ = [=](franka::Robot& robot, Callback ros_callback) { - robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this, - std::cref(effort_joint_command_), ros_callback, _1, _2), - std::bind(&FrankaHW::controlCallback<franka::CartesianVelocities>, this, - std::cref(velocity_cartesian_command_), ros_callback, _1, _2), - limit_rate, cutoff_frequency); + robot.control( + std::bind(&FrankaHW::controlCallback<franka::Torques>, this, + std::cref(effort_joint_command_libfranka_), ros_callback, _1, _2), + std::bind(&FrankaHW::controlCallback<franka::CartesianVelocities>, this, + std::cref(velocity_cartesian_command_libfranka_), ros_callback, _1, _2), + limit_rate, cutoff_frequency); }; break; default: @@ -352,23 +477,104 @@ bool FrankaHW::setRunFunction(const ControlMode& requested_control_mode, return true; } -void FrankaHW::init() { - setupJointStateInterface(robot_state_); - setupJointCommandInterface(position_joint_command_.q, robot_state_, true, +void FrankaHW::initROSInterfaces(ros::NodeHandle& /*robot_hw_nh*/) { + setupJointStateInterface(robot_state_ros_); + setupJointCommandInterface(position_joint_command_ros_.q, robot_state_ros_, true, position_joint_interface_); - setupJointCommandInterface(velocity_joint_command_.dq, robot_state_, true, + setupJointCommandInterface(velocity_joint_command_ros_.dq, robot_state_ros_, true, velocity_joint_interface_); - setupJointCommandInterface(effort_joint_command_.tau_J, robot_state_, false, + setupJointCommandInterface(effort_joint_command_ros_.tau_J, robot_state_ros_, false, effort_joint_interface_); setupLimitInterface<joint_limits_interface::PositionJointSoftLimitsHandle>( - urdf_model_, position_joint_limit_interface_, position_joint_interface_); + position_joint_limit_interface_, position_joint_interface_); setupLimitInterface<joint_limits_interface::VelocityJointSoftLimitsHandle>( - urdf_model_, velocity_joint_limit_interface_, velocity_joint_interface_); + velocity_joint_limit_interface_, velocity_joint_interface_); setupLimitInterface<joint_limits_interface::EffortJointSoftLimitsHandle>( - urdf_model_, effort_joint_limit_interface_, effort_joint_interface_); - setupFrankaStateInterface(robot_state_); - setupFrankaCartesianPoseInterface(pose_cartesian_command_); - setupFrankaCartesianVelocityInterface(velocity_cartesian_command_); + effort_joint_limit_interface_, effort_joint_interface_); + setupFrankaStateInterface(robot_state_ros_); + setupFrankaCartesianPoseInterface(pose_cartesian_command_ros_); + setupFrankaCartesianVelocityInterface(velocity_cartesian_command_ros_); + setupFrankaModelInterface(robot_state_ros_); +} + +void FrankaHW::initRobot() { + robot_ = std::make_unique<franka::Robot>(robot_ip_); + model_ = std::make_unique<franka::Model>(robot_->loadModel()); + robot_->setCollisionBehavior(collision_config_.lower_torque_thresholds_acceleration, + collision_config_.upper_torque_thresholds_acceleration, + collision_config_.lower_torque_thresholds_nominal, + collision_config_.upper_torque_thresholds_nominal, + collision_config_.lower_force_thresholds_acceleration, + collision_config_.upper_force_thresholds_acceleration, + collision_config_.lower_force_thresholds_nominal, + collision_config_.upper_force_thresholds_nominal); + update(robot_->readOnce()); +} + +void FrankaHW::setupParameterCallbacks(ros::NodeHandle& robot_hw_nh) { + get_limit_rate_ = [robot_hw_nh]() { + bool rate_limiting; + robot_hw_nh.getParamCached("rate_limiting", rate_limiting); + return rate_limiting; + }; + + get_internal_controller_ = [robot_hw_nh]() { + std::string internal_controller; + robot_hw_nh.getParamCached("internal_controller", internal_controller); + franka::ControllerMode controller_mode; + if (internal_controller == "joint_impedance") { + controller_mode = franka::ControllerMode::kJointImpedance; + } else if (internal_controller == "cartesian_impedance") { + controller_mode = franka::ControllerMode::kCartesianImpedance; + } else { + ROS_WARN("Invalid internal_controller parameter provided, falling back to joint impedance"); + controller_mode = franka::ControllerMode::kJointImpedance; + } + return controller_mode; + }; + + get_cutoff_frequency_ = [robot_hw_nh]() { + double cutoff_frequency; + robot_hw_nh.getParamCached("cutoff_frequency", cutoff_frequency); + return cutoff_frequency; + }; +} + +bool FrankaHW::commandHasNaN(const franka::Torques& command) { + return arrayHasNaN(command.tau_J); +} + +bool FrankaHW::commandHasNaN(const franka::JointPositions& command) { + return arrayHasNaN(command.q); +} + +bool FrankaHW::commandHasNaN(const franka::JointVelocities& command) { + return arrayHasNaN(command.dq); +} + +bool FrankaHW::commandHasNaN(const franka::CartesianPose& command) { + return arrayHasNaN(command.elbow) || arrayHasNaN(command.O_T_EE); +} + +bool FrankaHW::commandHasNaN(const franka::CartesianVelocities& command) { + return arrayHasNaN(command.elbow) || arrayHasNaN(command.O_dP_EE); +} + +std::vector<double> FrankaHW::getCollisionThresholds(const std::string& name, + ros::NodeHandle& robot_hw_nh, + const std::vector<double>& defaults) { + std::vector<double> thresholds; + if (!robot_hw_nh.getParam("collision_config/" + name, thresholds) || + thresholds.size() != defaults.size()) { + std::string message; + for (const double& threshold : defaults) { + message += std::to_string(threshold); + message += " "; + } + ROS_INFO("No parameter %s found, using default values: %s", name.c_str(), message.c_str()); + return defaults; + } + return thresholds; } } // namespace franka_hw diff --git a/franka_hw/src/resource_helpers.cpp b/franka_hw/src/resource_helpers.cpp index 91c386b7d1108717067a06080e947f05903d2fb0..cf22c58126be252100a3afcff35bd2a1c590d93c 100644 --- a/franka_hw/src/resource_helpers.cpp +++ b/franka_hw/src/resource_helpers.cpp @@ -200,4 +200,17 @@ bool partiallyClaimsArmJoints(const ArmClaimedMap& arm_claim_map, const std::str return false; } +bool hasTrajectoryClaim(const ArmClaimedMap& arm_claim_map, const std::string& arm_id) { + if (arm_claim_map.find(arm_id) != arm_claim_map.end()) { + if (arm_claim_map.at(arm_id).joint_position_claims + + arm_claim_map.at(arm_id).joint_velocity_claims + + arm_claim_map.at(arm_id).cartesian_velocity_claims + + arm_claim_map.at(arm_id).cartesian_pose_claims > + 0) { + return true; + } + } + return false; +} + } // namespace franka_hw diff --git a/franka_control/src/services.cpp b/franka_hw/src/services.cpp similarity index 61% rename from franka_control/src/services.cpp rename to franka_hw/src/services.cpp index c7a47c91b6f8a98d5e2a76cf86b07e5a4c9cafcf..355797756959febc2350cdaf7f304dcd9ef42acd 100644 --- a/franka_control/src/services.cpp +++ b/franka_hw/src/services.cpp @@ -1,12 +1,45 @@ // Copyright (c) 2017 Franka Emika GmbH // Use of this source code is governed by the Apache-2.0 license, see LICENSE -#include <franka_control/services.h> +#include <franka_hw/services.h> -namespace franka_control { +namespace franka_hw { + +void setupServices(franka::Robot& robot, ros::NodeHandle& node_handle, ServiceContainer& services) { + services + .advertiseService<franka_msgs::SetJointImpedance>(node_handle, "set_joint_impedance", + [&robot](auto&& req, auto&& res) { + return franka_hw::setJointImpedance( + robot, req, res); + }) + .advertiseService<franka_msgs::SetCartesianImpedance>( + node_handle, "set_cartesian_impedance", + [&robot](auto&& req, auto&& res) { + return franka_hw::setCartesianImpedance(robot, req, res); + }) + .advertiseService<franka_msgs::SetEEFrame>( + node_handle, "set_EE_frame", + [&robot](auto&& req, auto&& res) { return franka_hw::setEEFrame(robot, req, res); }) + .advertiseService<franka_msgs::SetKFrame>( + node_handle, "set_K_frame", + [&robot](auto&& req, auto&& res) { return franka_hw::setKFrame(robot, req, res); }) + .advertiseService<franka_msgs::SetForceTorqueCollisionBehavior>( + node_handle, "set_force_torque_collision_behavior", + [&robot](auto&& req, auto&& res) { + return franka_hw::setForceTorqueCollisionBehavior(robot, req, res); + }) + .advertiseService<franka_msgs::SetFullCollisionBehavior>( + node_handle, "set_full_collision_behavior", + [&robot](auto&& req, auto&& res) { + return franka_hw::setFullCollisionBehavior(robot, req, res); + }) + .advertiseService<franka_msgs::SetLoad>( + node_handle, "set_load", + [&robot](auto&& req, auto&& res) { return franka_hw::setLoad(robot, req, res); }); +} void setCartesianImpedance(franka::Robot& robot, - const SetCartesianImpedance::Request& req, - SetCartesianImpedance::Response& /* res */) { + const franka_msgs::SetCartesianImpedance::Request& req, + franka_msgs::SetCartesianImpedance::Response& /* res */) { std::array<double, 6> cartesian_stiffness; std::copy(req.cartesian_stiffness.cbegin(), req.cartesian_stiffness.cend(), cartesian_stiffness.begin()); @@ -14,32 +47,33 @@ void setCartesianImpedance(franka::Robot& robot, } void setJointImpedance(franka::Robot& robot, - const SetJointImpedance::Request& req, - SetJointImpedance::Response& /* res */) { + const franka_msgs::SetJointImpedance::Request& req, + franka_msgs::SetJointImpedance::Response& /* res */) { std::array<double, 7> joint_stiffness; std::copy(req.joint_stiffness.cbegin(), req.joint_stiffness.cend(), joint_stiffness.begin()); robot.setJointImpedance(joint_stiffness); } void setEEFrame(franka::Robot& robot, - const SetEEFrame::Request& req, - SetEEFrame::Response& /* res */) { + const franka_msgs::SetEEFrame::Request& req, + franka_msgs::SetEEFrame::Response& /* res */) { std::array<double, 16> F_T_EE; // NOLINT [readability-identifier-naming] std::copy(req.F_T_EE.cbegin(), req.F_T_EE.cend(), F_T_EE.begin()); robot.setEE(F_T_EE); } void setKFrame(franka::Robot& robot, - const SetKFrame::Request& req, - SetKFrame::Response& /* res */) { + const franka_msgs::SetKFrame::Request& req, + franka_msgs::SetKFrame::Response& /* res */) { std::array<double, 16> EE_T_K; // NOLINT [readability-identifier-naming] std::copy(req.EE_T_K.cbegin(), req.EE_T_K.cend(), EE_T_K.begin()); robot.setK(EE_T_K); } -void setForceTorqueCollisionBehavior(franka::Robot& robot, - const SetForceTorqueCollisionBehavior::Request& req, - SetForceTorqueCollisionBehavior::Response& /* res */ +void setForceTorqueCollisionBehavior( + franka::Robot& robot, + const franka_msgs::SetForceTorqueCollisionBehavior::Request& req, + franka_msgs::SetForceTorqueCollisionBehavior::Response& /* res */ ) { std::array<double, 7> lower_torque_thresholds_nominal; std::copy(req.lower_torque_thresholds_nominal.cbegin(), @@ -59,8 +93,8 @@ void setForceTorqueCollisionBehavior(franka::Robot& robot, } void setFullCollisionBehavior(franka::Robot& robot, - const SetFullCollisionBehavior::Request& req, - SetFullCollisionBehavior::Response& /* res */) { + const franka_msgs::SetFullCollisionBehavior::Request& req, + franka_msgs::SetFullCollisionBehavior::Response& /* res */) { std::array<double, 7> lower_torque_thresholds_acceleration; std::copy(req.lower_torque_thresholds_acceleration.cbegin(), req.lower_torque_thresholds_acceleration.cend(), @@ -96,7 +130,9 @@ void setFullCollisionBehavior(franka::Robot& robot, upper_force_thresholds_nominal); } -void setLoad(franka::Robot& robot, const SetLoad::Request& req, SetLoad::Response& /* res */) { +void setLoad(franka::Robot& robot, + const franka_msgs::SetLoad::Request& req, + franka_msgs::SetLoad::Response& /* res */) { double mass(req.mass); std::array<double, 3> F_x_center_load; // NOLINT [readability-identifier-naming] std::copy(req.F_x_center_load.cbegin(), req.F_x_center_load.cend(), F_x_center_load.begin()); @@ -106,4 +142,4 @@ void setLoad(franka::Robot& robot, const SetLoad::Request& req, SetLoad::Respons robot.setLoad(mass, F_x_center_load, load_inertia); } -} // namespace franka_control +} // namespace franka_hw diff --git a/franka_hw/test/CMakeLists.txt b/franka_hw/test/CMakeLists.txt index d14888ffa734fc0d23b25caa70a84dd4ce49192b..3601d19e9f2a5e7780d338b2ae5cdbadc76ad3d3 100644 --- a/franka_hw/test/CMakeLists.txt +++ b/franka_hw/test/CMakeLists.txt @@ -5,6 +5,7 @@ add_rostest_gtest(franka_hw_test main.cpp franka_hw_controller_switching_test.cpp franka_hw_interfaces_test.cpp + franka_combinable_hw_controller_switching_test.cpp ) add_dependencies(franka_hw_test @@ -21,4 +22,3 @@ target_link_libraries(franka_hw_test target_include_directories(franka_hw_test PUBLIC ${catkin_INCLUDE_DIRS} ) - diff --git a/franka_hw/test/franka_combinable_hw_controller_switching_test.cpp b/franka_hw/test/franka_combinable_hw_controller_switching_test.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0f2837fb301f2b4e897ecdd4d48b4526b8649dff --- /dev/null +++ b/franka_hw/test/franka_combinable_hw_controller_switching_test.cpp @@ -0,0 +1,198 @@ +// Copyright (c) 2019 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#include <array> +#include <list> +#include <memory> +#include <random> +#include <set> +#include <string> + +#include <gtest/gtest.h> +#include <hardware_interface/controller_info.h> +#include <hardware_interface/joint_command_interface.h> +#include <joint_limits_interface/joint_limits.h> +#include <joint_limits_interface/joint_limits_urdf.h> +#include <ros/ros.h> +#include <urdf/model.h> + +#include <franka_hw/franka_cartesian_command_interface.h> +#include <franka_hw/franka_combinable_hw.h> + +extern std::string arm_id; +extern std::array<std::string, 7> joint_names; + +hardware_interface::ControllerInfo newInfo( + const std::string& name, + const std::string& type, + const hardware_interface::InterfaceResources& resource1) { + hardware_interface::ControllerInfo info; + info.claimed_resources.clear(); + info.name = name; + info.type = type; + info.claimed_resources.push_back(resource1); + return info; +} + +hardware_interface::ControllerInfo newInfo( + const std::string& name, + const std::string& type, + const hardware_interface::InterfaceResources& resource1, + const hardware_interface::InterfaceResources& resource2) { + hardware_interface::ControllerInfo info = newInfo(name, type, resource1); + info.claimed_resources.push_back(resource2); + return info; +} + +hardware_interface::ControllerInfo newInfo( + const std::string& name, + const std::string& type, + const hardware_interface::InterfaceResources& resource1, + const hardware_interface::InterfaceResources& resource2, + const hardware_interface::InterfaceResources& resource3) { + hardware_interface::ControllerInfo info = newInfo(name, type, resource1, resource2); + info.claimed_resources.push_back(resource3); + return info; +} + +class CombinableControllerConflict + : public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo>> { + public: + CombinableControllerConflict() : robot_(std::make_unique<franka_hw::FrankaCombinableHW>()) { + ros::NodeHandle root_nh; + ros::NodeHandle robot_hw_nh("~"); + robot_->initParameters(root_nh, robot_hw_nh); + robot_->initROSInterfaces(robot_hw_nh); + robot_->setupParameterCallbacks(robot_hw_nh); + } + bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) { + return robot_->checkForConflict(info_list); + } + bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) { + return robot_->prepareSwitch(info_list, info_list); + } + + private: + std::unique_ptr<franka_hw::FrankaCombinableHW> robot_; +}; + +class CombinableNoControllerConflict + : public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo>> { + public: + CombinableNoControllerConflict() : robot_(std::make_unique<franka_hw::FrankaCombinableHW>()) { + ros::NodeHandle root_nh; + ros::NodeHandle robot_hw_nh("~"); + robot_->initParameters(root_nh, robot_hw_nh); + robot_->initROSInterfaces(robot_hw_nh); + robot_->setupParameterCallbacks(robot_hw_nh); + } + bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) { + return robot_->checkForConflict(info_list); + } + bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) { + return robot_->prepareSwitch(info_list, info_list); + } + + private: + std::unique_ptr<franka_hw::FrankaCombinableHW> robot_; +}; + +std::string arm_id2("panda2"); +std::string jp_iface_str("hardware_interface::PositionJointInterface"); +std::string jv_iface_str("hardware_interface::VelocityJointInterface"); +std::string jt_iface_str("hardware_interface::EffortJointInterface"); +std::string cv_iface_str("franka_hw::FrankaVelocityCartesianInterface"); +std::string cp_iface_str("franka_hw::FrankaPoseCartesianInterface"); +std::string unknown_iface_str("hardware_interface::UnknownInterface"); +std::string name_str("some_controller"); +std::string type_str("SomeControllerClass"); +std::set<std::string> joints_set = {joint_names[0], joint_names[1], joint_names[2], joint_names[3], + joint_names[4], joint_names[5], joint_names[6]}; +std::set<std::string> cartesian_set = {arm_id + "_robot"}; +std::set<std::string> cartesian_arm2_set = {arm_id2 + "_robot"}; +std::set<std::string> no_id_set = {"joint1"}; +hardware_interface::InterfaceResources no_id_res(jp_iface_str, no_id_set); +hardware_interface::InterfaceResources unknown_iface_res(unknown_iface_str, joints_set); +hardware_interface::InterfaceResources jp_res(jp_iface_str, joints_set); +hardware_interface::InterfaceResources jv_res(jv_iface_str, joints_set); +hardware_interface::InterfaceResources jt_res(jt_iface_str, joints_set); +hardware_interface::InterfaceResources cv_res(cv_iface_str, cartesian_set); +hardware_interface::InterfaceResources cp_res(cp_iface_str, cartesian_set); +hardware_interface::InterfaceResources cp_arm2_res(cp_iface_str, cartesian_arm2_set); +hardware_interface::ControllerInfo cp_arm2_info = newInfo(name_str, type_str, cp_arm2_res); +hardware_interface::ControllerInfo no_id_info = newInfo(name_str, type_str, no_id_res); +hardware_interface::ControllerInfo unknown_iface_info = + newInfo(name_str, type_str, unknown_iface_res); +hardware_interface::ControllerInfo jt_jt_info = newInfo(name_str, type_str, jt_res, jt_res); +hardware_interface::ControllerInfo jp_jp_info = newInfo(name_str, type_str, jp_res, jp_res); +hardware_interface::ControllerInfo jp_jv_info = newInfo(name_str, type_str, jp_res, jv_res); +hardware_interface::ControllerInfo jp_cv_info = newInfo(name_str, type_str, jp_res, cv_res); +hardware_interface::ControllerInfo jp_cp_info = newInfo(name_str, type_str, jp_res, cp_res); +hardware_interface::ControllerInfo jv_jv_info = newInfo(name_str, type_str, jv_res, jv_res); +hardware_interface::ControllerInfo jv_cv_info = newInfo(name_str, type_str, jv_res, cv_res); +hardware_interface::ControllerInfo jv_cp_info = newInfo(name_str, type_str, jv_res, cp_res); +hardware_interface::ControllerInfo cv_cv_info = newInfo(name_str, type_str, cv_res, cv_res); +hardware_interface::ControllerInfo cv_cp_info = newInfo(name_str, type_str, cv_res, cp_res); +hardware_interface::ControllerInfo cp_cp_info = newInfo(name_str, type_str, cp_res, cp_res); +hardware_interface::ControllerInfo jp_jp_jp_info = + newInfo(name_str, type_str, jp_res, jp_res, jp_res); +hardware_interface::ControllerInfo jp_info = newInfo(name_str, type_str, jp_res); +hardware_interface::ControllerInfo jv_info = newInfo(name_str, type_str, jv_res); +hardware_interface::ControllerInfo jt_info = newInfo(name_str, type_str, jt_res); +hardware_interface::ControllerInfo cv_info = newInfo(name_str, type_str, cv_res); +hardware_interface::ControllerInfo cp_info = newInfo(name_str, type_str, cp_res); +hardware_interface::ControllerInfo jt_jp_info = newInfo(name_str, type_str, jt_res, jp_res); +hardware_interface::ControllerInfo jt_jv_info = newInfo(name_str, type_str, jt_res, jv_res); +hardware_interface::ControllerInfo jt_cv_info = newInfo(name_str, type_str, jt_res, cv_res); +hardware_interface::ControllerInfo jt_cp_info = newInfo(name_str, type_str, jt_res, cp_res); + +INSTANTIATE_TEST_CASE_P( + combinableNonAdmissibleRequests, + CombinableControllerConflict, + ::testing::Values(std::list<hardware_interface::ControllerInfo>{no_id_info}, + std::list<hardware_interface::ControllerInfo>{unknown_iface_info}, + std::list<hardware_interface::ControllerInfo>{jp_info}, + std::list<hardware_interface::ControllerInfo>{jv_info}, + std::list<hardware_interface::ControllerInfo>{cv_info}, + std::list<hardware_interface::ControllerInfo>{cp_info}, + std::list<hardware_interface::ControllerInfo>{jt_jp_info}, + std::list<hardware_interface::ControllerInfo>{jt_jv_info}, + std::list<hardware_interface::ControllerInfo>{jt_jt_info}, + std::list<hardware_interface::ControllerInfo>{jp_jp_info}, + std::list<hardware_interface::ControllerInfo>{jp_jv_info}, + std::list<hardware_interface::ControllerInfo>{jp_cv_info}, + std::list<hardware_interface::ControllerInfo>{jp_cp_info}, + std::list<hardware_interface::ControllerInfo>{jv_jv_info}, + std::list<hardware_interface::ControllerInfo>{jv_cv_info}, + std::list<hardware_interface::ControllerInfo>{jv_cp_info}, + std::list<hardware_interface::ControllerInfo>{cv_cv_info}, + std::list<hardware_interface::ControllerInfo>{cv_cp_info}, + std::list<hardware_interface::ControllerInfo>{cp_cp_info}, + std::list<hardware_interface::ControllerInfo>{jp_jp_jp_info}, + std::list<hardware_interface::ControllerInfo>{jt_cv_info}, + std::list<hardware_interface::ControllerInfo>{jt_cp_info})); + +INSTANTIATE_TEST_CASE_P(combinableAdmissibleRequests, + CombinableNoControllerConflict, + ::testing::Values(std::list<hardware_interface::ControllerInfo>{jt_info}, + std::list<hardware_interface::ControllerInfo>{ + jt_info, cp_arm2_info})); + +TEST(FrankaCombinableHWTests, CanInitROSInterfaces) { + franka_hw::FrankaCombinableHW hw; + ros::NodeHandle root_nh; + ros::NodeHandle robot_hw_nh("~"); + EXPECT_TRUE(hw.initParameters(root_nh, robot_hw_nh)); + EXPECT_NO_THROW(hw.initROSInterfaces(robot_hw_nh)); +} + +TEST_P(CombinableControllerConflict, ConflictsForIncompatibleControllers) { + EXPECT_TRUE(callCheckForConflict(GetParam())); +} + +TEST_P(CombinableNoControllerConflict, DoesNotConflictForCompatibleControllers) { + EXPECT_FALSE(callCheckForConflict(GetParam())); +} + +TEST_P(CombinableNoControllerConflict, CanPrepareSwitchForCompatibleControllers) { + EXPECT_TRUE(callPrepareSwitch(GetParam())); +} diff --git a/franka_hw/test/franka_hw_controller_switching_test.cpp b/franka_hw/test/franka_hw_controller_switching_test.cpp index 58639da26544821748e43a479ebf8e04d27e3c78..59646b7c4468cbf69887c907de3afa837d7502c2 100644 --- a/franka_hw/test/franka_hw_controller_switching_test.cpp +++ b/franka_hw/test/franka_hw_controller_switching_test.cpp @@ -24,13 +24,9 @@ using hardware_interface::InterfaceResources; using hardware_interface::ControllerInfo; std::string arm_id("panda"); -std::array<std::string, 7> joint_names = {arm_id + "_joint1", - arm_id + "_joint2", - arm_id + "_joint3", - arm_id + "_joint4", - arm_id + "_joint5", - arm_id + "_joint6", - arm_id + "_joint7"}; +std::array<std::string, 7> joint_names = { + arm_id + "_joint1", arm_id + "_joint2", arm_id + "_joint3", arm_id + "_joint4", + arm_id + "_joint5", arm_id + "_joint6", arm_id + "_joint7"}; namespace franka_hw { @@ -67,32 +63,45 @@ hardware_interface::ControllerInfo newInfo( return info; } -class ControllerConflict : - public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo> > { -public: - ControllerConflict() : robot_(new FrankaHW(joint_names, arm_id, urdf::Model())) {} - bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) { - return robot_->checkForConflict(info_list); - } - bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) { - return robot_->prepareSwitch(info_list, info_list); - } +class ControllerConflict + : public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo>> { + public: + ControllerConflict() : robot_(std::make_unique<FrankaHW>()) { + ros::NodeHandle private_nh("~"); + ros::NodeHandle root_nh; + EXPECT_TRUE(robot_->initParameters(root_nh, private_nh)); + robot_->initROSInterfaces(private_nh); + } + bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) { + return robot_->checkForConflict(info_list); + } + bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) { + return robot_->prepareSwitch(info_list, info_list); + } + private: - std::unique_ptr<FrankaHW> robot_; + std::unique_ptr<FrankaHW> robot_; }; -class NoControllerConflict : public - ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo> > { -public: - NoControllerConflict() : robot_(new FrankaHW(joint_names, arm_id, urdf::Model())) {} - bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) { - return robot_->checkForConflict(info_list); - } - bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) { - return robot_->prepareSwitch(info_list, info_list); - } +class NoControllerConflict + : public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo>> { + public: + NoControllerConflict() : robot_(std::make_unique<FrankaHW>()) { + ros::NodeHandle private_nh("~"); + ros::NodeHandle root_nh; + EXPECT_TRUE(robot_->initParameters(root_nh, private_nh)); + robot_->initROSInterfaces(private_nh); + robot_->setupParameterCallbacks(private_nh); + } + bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) { + return robot_->checkForConflict(info_list); + } + bool callPrepareSwitch(const std::list<hardware_interface::ControllerInfo> info_list) { + return robot_->prepareSwitch(info_list, info_list); + } + private: - std::unique_ptr<FrankaHW> robot_; + std::unique_ptr<FrankaHW> robot_; }; string arm_id2("panda2"); @@ -104,13 +113,8 @@ string cp_iface_str("franka_hw::FrankaPoseCartesianInterface"); string unknown_iface_str("hardware_interface::UnknownInterface"); string name_str("some_controller"); string type_str("SomeControllerClass"); -set<string> joints_set = {joint_names[0], - joint_names[1], - joint_names[2], - joint_names[3], - joint_names[4], - joint_names[5], - joint_names[6]}; +set<string> joints_set = {joint_names[0], joint_names[1], joint_names[2], joint_names[3], + joint_names[4], joint_names[5], joint_names[6]}; set<string> cartesian_set = {arm_id + "_robot"}; set<string> cartesian_arm2_set = {arm_id2 + "_robot"}; set<string> no_id_set = {"joint1"}; @@ -186,7 +190,7 @@ TEST_P(NoControllerConflict, DoesNotConflictForCompatibleControllers) { } TEST_P(NoControllerConflict, CanPrepareSwitchForCompatibleControllers) { - EXPECT_TRUE(callPrepareSwitch(GetParam())); + EXPECT_TRUE(callPrepareSwitch(GetParam())); } } // namespace franka_hw diff --git a/franka_hw/test/franka_hw_interfaces_test.cpp b/franka_hw/test/franka_hw_interfaces_test.cpp index 7984e1e2f71dfb3f43a00c06b42ceaa05c928a5a..7087632c21ef527817e3461ddb5e745e013106ed 100644 --- a/franka_hw/test/franka_hw_interfaces_test.cpp +++ b/franka_hw/test/franka_hw_interfaces_test.cpp @@ -23,21 +23,24 @@ extern std::array<std::string, 7> joint_names; namespace franka_hw { TEST(FrankaHWTests, InterfacesWorkForReadAndCommand) { - std::unique_ptr<FrankaHW> robotptr(new FrankaHW(joint_names, arm_id, urdf::Model())); + auto robot_ptr = std::make_unique<FrankaHW>(); + ros::NodeHandle private_nh("~"); + ros::NodeHandle root_nh; + EXPECT_TRUE(robot_ptr->initParameters(root_nh, private_nh)); + robot_ptr->initROSInterfaces(private_nh); + hardware_interface::JointStateInterface* js_interface = - robotptr->get<hardware_interface::JointStateInterface>(); + robot_ptr->get<hardware_interface::JointStateInterface>(); hardware_interface::PositionJointInterface* pj_interface = - robotptr->get<hardware_interface::PositionJointInterface>(); + robot_ptr->get<hardware_interface::PositionJointInterface>(); hardware_interface::VelocityJointInterface* vj_interface = - robotptr->get<hardware_interface::VelocityJointInterface>(); + robot_ptr->get<hardware_interface::VelocityJointInterface>(); hardware_interface::EffortJointInterface* ej_interface = - robotptr->get<hardware_interface::EffortJointInterface>(); - FrankaPoseCartesianInterface* fpc_interface = - robotptr->get<FrankaPoseCartesianInterface>(); + robot_ptr->get<hardware_interface::EffortJointInterface>(); + FrankaPoseCartesianInterface* fpc_interface = robot_ptr->get<FrankaPoseCartesianInterface>(); FrankaVelocityCartesianInterface* fvc_interface = - robotptr->get<FrankaVelocityCartesianInterface>(); - FrankaStateInterface* fs_interface = - robotptr->get<FrankaStateInterface>(); + robot_ptr->get<FrankaVelocityCartesianInterface>(); + FrankaStateInterface* fs_interface = robot_ptr->get<FrankaStateInterface>(); ASSERT_NE(nullptr, js_interface); ASSERT_NE(nullptr, pj_interface); @@ -48,23 +51,20 @@ TEST(FrankaHWTests, InterfacesWorkForReadAndCommand) { ASSERT_NE(nullptr, fs_interface); // Model interface not available with this signature - FrankaModelInterface* fm_interface = robotptr->get<FrankaModelInterface>(); + FrankaModelInterface* fm_interface = robot_ptr->get<FrankaModelInterface>(); ASSERT_EQ(nullptr, fm_interface); EXPECT_NO_THROW(FrankaCartesianPoseHandle fpc_handle = - fpc_interface->getHandle(arm_id + "_robot")); - FrankaCartesianPoseHandle fpc_handle = - fpc_interface->getHandle(arm_id + "_robot"); - std::array<double, 16> pose_command = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, - 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, - 1.0, 1.0, 1.0, 1.0}; + fpc_interface->getHandle(arm_id + "_robot")); + FrankaCartesianPoseHandle fpc_handle = fpc_interface->getHandle(arm_id + "_robot"); + std::array<double, 16> pose_command = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; fpc_handle.setCommand(pose_command); EXPECT_EQ(pose_command, fpc_handle.getCommand()); EXPECT_NO_THROW(FrankaCartesianVelocityHandle fvc_handle = - fvc_interface->getHandle(arm_id + "_robot")); - FrankaCartesianVelocityHandle fvc_handle = - fvc_interface->getHandle(arm_id + "_robot"); + fvc_interface->getHandle(arm_id + "_robot")); + FrankaCartesianVelocityHandle fvc_handle = fvc_interface->getHandle(arm_id + "_robot"); std::array<double, 6> vel_command = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; fvc_handle.setCommand(vel_command); EXPECT_EQ(vel_command, fvc_handle.getCommand()); @@ -73,10 +73,10 @@ TEST(FrankaHWTests, InterfacesWorkForReadAndCommand) { } TEST(FrankaHWTests, JointLimitInterfacesEnforceLimitsOnCommands) { - urdf::Model urdf_model; - ASSERT_TRUE(urdf_model.initParamWithNodeHandle("robot_description")); - - std::unique_ptr<FrankaHW> robot_ptr(new FrankaHW(joint_names, arm_id, urdf_model)); + auto robot_ptr = std::make_unique<FrankaHW>(); + ros::NodeHandle nh("~"); + ASSERT_TRUE(robot_ptr->initParameters(nh, nh)); + robot_ptr->initROSInterfaces(nh); hardware_interface::PositionJointInterface* pj_interface = robot_ptr->get<hardware_interface::PositionJointInterface>(); @@ -93,16 +93,15 @@ TEST(FrankaHWTests, JointLimitInterfacesEnforceLimitsOnCommands) { std::vector<hardware_interface::JointHandle> velocity_handles(7); std::vector<hardware_interface::JointHandle> effort_handles(7); + urdf::Model urdf_model; + ASSERT_TRUE(urdf_model.initParamWithNodeHandle("robot_description")); + for (size_t i = 0; i < joint_names.size(); ++i) { auto urdf_joint = urdf_model.getJoint(joint_names[i]); - ASSERT_TRUE( - joint_limits_interface::getJointLimits(urdf_joint, joint_limits[i])); - ASSERT_NO_THROW(position_handles[i] = - pj_interface->getHandle(joint_names[i])); - ASSERT_NO_THROW(velocity_handles[i] = - vj_interface->getHandle(joint_names[i])); - ASSERT_NO_THROW(effort_handles[i] = - ej_interface->getHandle(joint_names[i])); + ASSERT_TRUE(joint_limits_interface::getJointLimits(urdf_joint, joint_limits[i])); + ASSERT_NO_THROW(position_handles[i] = pj_interface->getHandle(joint_names[i])); + ASSERT_NO_THROW(velocity_handles[i] = vj_interface->getHandle(joint_names[i])); + ASSERT_NO_THROW(effort_handles[i] = ej_interface->getHandle(joint_names[i])); } std::uniform_real_distribution<double> uniform_distribution(0.1, 3.0); @@ -113,13 +112,12 @@ TEST(FrankaHWTests, JointLimitInterfacesEnforceLimitsOnCommands) { uniform_distribution(random_engine)); velocity_handles[i].setCommand(joint_limits[i].max_velocity + uniform_distribution(random_engine)); - effort_handles[i].setCommand(joint_limits[i].max_effort + - uniform_distribution(random_engine)); + effort_handles[i].setCommand(joint_limits[i].max_effort + uniform_distribution(random_engine)); } robot_ptr->enforceLimits(ros::Duration(0.001)); for (size_t i = 0; i < joint_names.size(); ++i) { EXPECT_LE(position_handles[i].getCommand(), joint_limits[i].max_position); - EXPECT_GE(position_handles[i].getCommand(),joint_limits[i].min_position); + EXPECT_GE(position_handles[i].getCommand(), joint_limits[i].min_position); EXPECT_LE(velocity_handles[i].getCommand(), joint_limits[i].max_velocity); EXPECT_GE(velocity_handles[i].getCommand(), -joint_limits[i].max_velocity); EXPECT_LE(effort_handles[i].getCommand(), joint_limits[i].max_effort); @@ -131,18 +129,17 @@ TEST(FrankaHWTests, JointLimitInterfacesEnforceLimitsOnCommands) { uniform_distribution(random_engine)); velocity_handles[i].setCommand(-joint_limits[i].max_velocity - uniform_distribution(random_engine)); - effort_handles[i].setCommand(-joint_limits[i].max_effort - - uniform_distribution(random_engine)); + effort_handles[i].setCommand(-joint_limits[i].max_effort - uniform_distribution(random_engine)); } robot_ptr->enforceLimits(ros::Duration(0.001)); for (size_t i = 0; i < joint_names.size(); ++i) { - EXPECT_LE(position_handles[i].getCommand(), joint_limits[i].max_position); - EXPECT_GE(position_handles[i].getCommand(),joint_limits[i].min_position); - EXPECT_LE(velocity_handles[i].getCommand(), joint_limits[i].max_velocity); - EXPECT_GE(velocity_handles[i].getCommand(), -joint_limits[i].max_velocity); - EXPECT_LE(effort_handles[i].getCommand(), joint_limits[i].max_effort); - EXPECT_GE(effort_handles[i].getCommand(), -joint_limits[i].max_effort); } + EXPECT_LE(position_handles[i].getCommand(), joint_limits[i].max_position); + EXPECT_GE(position_handles[i].getCommand(), joint_limits[i].min_position); + EXPECT_LE(velocity_handles[i].getCommand(), joint_limits[i].max_velocity); + EXPECT_GE(velocity_handles[i].getCommand(), -joint_limits[i].max_velocity); + EXPECT_LE(effort_handles[i].getCommand(), joint_limits[i].max_effort); + EXPECT_GE(effort_handles[i].getCommand(), -joint_limits[i].max_effort); + } } } // namespace franka_hw - diff --git a/franka_hw/test/launch/franka_hw_test.test b/franka_hw/test/launch/franka_hw_test.test index 53dfa12ed4015436ad3dfc2257c2197e90fa92bb..64ffded744f2e8ff753e3d4464efbc2f3221c5c2 100644 --- a/franka_hw/test/launch/franka_hw_test.test +++ b/franka_hw/test/launch/franka_hw_test.test @@ -1,5 +1,8 @@ <launch> <env name="ROSCONSOLE_CONFIG_FILE" value="$(find franka_hw)/test/config/ros_console_settings_for_tests.conf"/> <param name="robot_description" command="$(find xacro)/xacro --inorder $(find franka_description)/robots/panda_arm.urdf.xacro" /> - <test test-name="franka_hw_test" pkg="franka_hw" type="franka_hw_test" /> + <test test-name="franka_hw_test" pkg="franka_hw" type="franka_hw_test" > + <rosparam command="load" file="$(find franka_control)/config/franka_control_node.yaml" /> + <param name="robot_ip" value="unused_dummy_ip" /> + </test> </launch> diff --git a/franka_msgs/CMakeLists.txt b/franka_msgs/CMakeLists.txt index 49816727b832637c32d3075794185a542341495e..1f6f926597e947e268865a2a7827ea0e30570d8e 100644 --- a/franka_msgs/CMakeLists.txt +++ b/franka_msgs/CMakeLists.txt @@ -1,8 +1,22 @@ cmake_minimum_required(VERSION 3.4) project(franka_msgs) -find_package(catkin REQUIRED COMPONENTS message_generation std_msgs) +find_package(catkin REQUIRED COMPONENTS message_generation std_msgs actionlib_msgs) add_message_files(FILES Errors.msg FrankaState.msg) -generate_messages(DEPENDENCIES std_msgs) -catkin_package(CATKIN_DEPENDS message_runtime std_msgs) + +add_service_files(FILES + SetCartesianImpedance.srv + SetEEFrame.srv + SetForceTorqueCollisionBehavior.srv + SetFullCollisionBehavior.srv + SetJointImpedance.srv + SetKFrame.srv + SetLoad.srv +) + +add_action_files(FILES ErrorRecovery.action) + +generate_messages(DEPENDENCIES std_msgs actionlib_msgs) + +catkin_package(CATKIN_DEPENDS message_runtime std_msgs actionlib_msgs) diff --git a/franka_control/action/ErrorRecovery.action b/franka_msgs/action/ErrorRecovery.action similarity index 100% rename from franka_control/action/ErrorRecovery.action rename to franka_msgs/action/ErrorRecovery.action diff --git a/franka_msgs/package.xml b/franka_msgs/package.xml index e5b2e8dc330ee85bec6bd1b14fbb50ec117cba46..c8bcef0cbad6707c4b4867f2b36792de5e0f7bb1 100644 --- a/franka_msgs/package.xml +++ b/franka_msgs/package.xml @@ -13,9 +13,13 @@ <buildtool_depend>catkin</buildtool_depend> - <depend>std_msgs</depend> <build_depend>message_generation</build_depend> + + <depend>std_msgs</depend> + <depend>actionlib_msgs</depend> + <exec_depend>message_runtime</exec_depend> + <build_export_depend>message_runtime</build_export_depend> <export> diff --git a/franka_control/srv/SetCartesianImpedance.srv b/franka_msgs/srv/SetCartesianImpedance.srv similarity index 100% rename from franka_control/srv/SetCartesianImpedance.srv rename to franka_msgs/srv/SetCartesianImpedance.srv diff --git a/franka_control/srv/SetEEFrame.srv b/franka_msgs/srv/SetEEFrame.srv similarity index 100% rename from franka_control/srv/SetEEFrame.srv rename to franka_msgs/srv/SetEEFrame.srv diff --git a/franka_control/srv/SetForceTorqueCollisionBehavior.srv b/franka_msgs/srv/SetForceTorqueCollisionBehavior.srv similarity index 100% rename from franka_control/srv/SetForceTorqueCollisionBehavior.srv rename to franka_msgs/srv/SetForceTorqueCollisionBehavior.srv diff --git a/franka_control/srv/SetFullCollisionBehavior.srv b/franka_msgs/srv/SetFullCollisionBehavior.srv similarity index 100% rename from franka_control/srv/SetFullCollisionBehavior.srv rename to franka_msgs/srv/SetFullCollisionBehavior.srv diff --git a/franka_control/srv/SetJointImpedance.srv b/franka_msgs/srv/SetJointImpedance.srv similarity index 100% rename from franka_control/srv/SetJointImpedance.srv rename to franka_msgs/srv/SetJointImpedance.srv diff --git a/franka_control/srv/SetKFrame.srv b/franka_msgs/srv/SetKFrame.srv similarity index 100% rename from franka_control/srv/SetKFrame.srv rename to franka_msgs/srv/SetKFrame.srv diff --git a/franka_control/srv/SetLoad.srv b/franka_msgs/srv/SetLoad.srv similarity index 100% rename from franka_control/srv/SetLoad.srv rename to franka_msgs/srv/SetLoad.srv