diff --git a/CHANGELOG.md b/CHANGELOG.md index 25c054ed..7418e7f8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,12 +1,19 @@ # Changelog +## 0.1.5 - 2023-10-13 + +Requires libfranka >= 0.12.1, required ROS 2 Humble + +* franka\_hardware: support joint velocity command interface +* franka\_example\_controllers: implement joint velocity example controller +* franka\_description: add velocity command interface to the control tag + ## 0.1.4 - 2023-09-26 Requires libfranka >= 0.12.1, required ROS 2 Humble * franka\_hardware: adapt to libfranka active control v0.12.1 - ## 0.1.3 - 2023-08-24 Requires libfranka >= 0.11.0, required ROS 2 Humble diff --git a/franka_bringup/config/controllers.yaml b/franka_bringup/config/controllers.yaml index e4ee9c4c..a6c419c8 100644 --- a/franka_bringup/config/controllers.yaml +++ b/franka_bringup/config/controllers.yaml @@ -8,6 +8,9 @@ controller_manager: joint_impedance_example_controller: type: franka_example_controllers/JointImpedanceExampleController + joint_velocity_example_controller: + type: franka_example_controllers/JointVelocityExampleController + move_to_start_example_controller: type: franka_example_controllers/MoveToStartExampleController diff --git a/franka_bringup/launch/joint_velocity_example_controller.launch.py b/franka_bringup/launch/joint_velocity_example_controller.launch.py new file mode 100644 index 00000000..bddf2dea --- /dev/null +++ b/franka_bringup/launch/joint_velocity_example_controller.launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2021 Franka Emika GmbH +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + robot_ip_parameter_name = 'robot_ip' + load_gripper_parameter_name = 'load_gripper' + use_fake_hardware_parameter_name = 'use_fake_hardware' + fake_sensor_commands_parameter_name = 'fake_sensor_commands' + use_rviz_parameter_name = 'use_rviz' + + robot_ip = LaunchConfiguration(robot_ip_parameter_name) + load_gripper = LaunchConfiguration(load_gripper_parameter_name) + use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name) + fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name) + use_rviz = LaunchConfiguration(use_rviz_parameter_name) + + return LaunchDescription([ + DeclareLaunchArgument( + robot_ip_parameter_name, + description='Hostname or IP address of the robot.'), + DeclareLaunchArgument( + use_rviz_parameter_name, + default_value='false', + description='Visualize the robot in Rviz'), + DeclareLaunchArgument( + use_fake_hardware_parameter_name, + default_value='false', + description='Use fake hardware'), + DeclareLaunchArgument( + fake_sensor_commands_parameter_name, + default_value='false', + description="Fake sensor commands. Only valid when '{}' is true".format( + use_fake_hardware_parameter_name)), + DeclareLaunchArgument( + load_gripper_parameter_name, + default_value='true', + description='Use Franka Gripper as an end-effector, otherwise, the robot is loaded ' + 'without an end-effector.'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution( + [FindPackageShare('franka_bringup'), 'launch', 'franka.launch.py'])]), + launch_arguments={robot_ip_parameter_name: robot_ip, + load_gripper_parameter_name: load_gripper, + use_fake_hardware_parameter_name: use_fake_hardware, + fake_sensor_commands_parameter_name: fake_sensor_commands, + use_rviz_parameter_name: use_rviz + }.items(), + ), + + Node( + package='controller_manager', + executable='spawner', + arguments=['joint_velocity_example_controller'], + output='screen', + ), + ]) diff --git a/franka_bringup/package.xml b/franka_bringup/package.xml index 4ff42eb7..9f3b6f6d 100644 --- a/franka_bringup/package.xml +++ b/franka_bringup/package.xml @@ -2,7 +2,7 @@ franka_bringup - 0.1.4 + 0.1.5 Package with launch files and run-time configurations for using Franka Emika research robots with ros2_control Franka Emika GmbH Apache 2.0 diff --git a/franka_description/package.xml b/franka_description/package.xml index 933697bd..5d95787a 100644 --- a/franka_description/package.xml +++ b/franka_description/package.xml @@ -2,7 +2,7 @@ franka_description - 0.1.4 + 0.1.5 franka_description contains URDF files and meshes of Franka Emika robots Franka Emika GmbH Apache 2.0 diff --git a/franka_description/robots/panda_arm.ros2_control.xacro b/franka_description/robots/panda_arm.ros2_control.xacro index 12f2e6a4..811d5a46 100644 --- a/franka_description/robots/panda_arm.ros2_control.xacro +++ b/franka_description/robots/panda_arm.ros2_control.xacro @@ -19,6 +19,7 @@ ${initial_position} + diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt index 4e5aa0d0..c6a26748 100644 --- a/franka_example_controllers/CMakeLists.txt +++ b/franka_example_controllers/CMakeLists.txt @@ -22,7 +22,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(hardware_interface REQUIRED) find_package(franka_msgs REQUIRED) find_package(Eigen3 REQUIRED) -find_package(franka_semantic_components) +find_package(franka_semantic_components REQUIRED) find_package(generate_parameter_library) add_library( @@ -30,6 +30,7 @@ add_library( SHARED src/gravity_compensation_example_controller.cpp src/joint_impedance_example_controller.cpp + src/joint_velocity_example_controller.cpp src/model_example_controller.cpp src/move_to_start_example_controller.cpp src/motion_generator.cpp) diff --git a/franka_example_controllers/franka_example_controllers.xml b/franka_example_controllers/franka_example_controllers.xml index aa6c1b07..8359ff1b 100644 --- a/franka_example_controllers/franka_example_controllers.xml +++ b/franka_example_controllers/franka_example_controllers.xml @@ -11,6 +11,12 @@ The joint impedance example controller moves joint 4 and 5 in a very compliant periodic movement. + + + The joint velocity example controller moves joint 4 and 5 in a periodic movement. + + diff --git a/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.hpp b/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.hpp new file mode 100644 index 00000000..325d0927 --- /dev/null +++ b/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.hpp @@ -0,0 +1,47 @@ +// Copyright (c) 2021 Franka Emika GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include +#include + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +namespace franka_example_controllers { + +/** + * The joint velocity example controller + */ +class JointVelocityExampleController : public controller_interface::ControllerInterface { + public: + [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration() + const override; + [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration() + const override; + controller_interface::return_type update(const rclcpp::Time& time, + const rclcpp::Duration& period) override; + CallbackReturn on_init() override; + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + + private: + std::string arm_id_; + const int num_joints = 7; + rclcpp::Duration elapsed_time_ = rclcpp::Duration(0, 0); +}; + +} // namespace franka_example_controllers \ No newline at end of file diff --git a/franka_example_controllers/package.xml b/franka_example_controllers/package.xml index d8409259..c20a7f37 100644 --- a/franka_example_controllers/package.xml +++ b/franka_example_controllers/package.xml @@ -2,7 +2,7 @@ franka_example_controllers - 0.1.4 + 0.1.5 franka_example_controllers provides example code for controlling Franka Emika research robots with ros2_control Franka Emika GmbH Apache 2.0 diff --git a/franka_example_controllers/src/joint_velocity_example_controller.cpp b/franka_example_controllers/src/joint_velocity_example_controller.cpp new file mode 100644 index 00000000..ac146339 --- /dev/null +++ b/franka_example_controllers/src/joint_velocity_example_controller.cpp @@ -0,0 +1,120 @@ +// Copyright (c) 2021 Franka Emika GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include +#include + +#include + +using namespace std::chrono_literals; + +namespace franka_example_controllers { + +controller_interface::InterfaceConfiguration +JointVelocityExampleController::command_interface_configuration() const { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + for (int i = 1; i <= num_joints; ++i) { + config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/velocity"); + } + return config; +} + +controller_interface::InterfaceConfiguration +JointVelocityExampleController::state_interface_configuration() const { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (int i = 1; i <= num_joints; ++i) { + config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/position"); + config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/velocity"); + } + return config; +} + +controller_interface::return_type JointVelocityExampleController::update( + const rclcpp::Time& /*time*/, + const rclcpp::Duration& period) { + elapsed_time_ = elapsed_time_ + period; + rclcpp::Duration time_max(8.0, 0.0); + double omega_max = 0.1; + double cycle = std::floor(std::pow( + -1.0, (elapsed_time_.seconds() - std::fmod(elapsed_time_.seconds(), time_max.seconds())) / + time_max.seconds())); + double omega = cycle * omega_max / 2.0 * + (1.0 - std::cos(2.0 * M_PI / time_max.seconds() * elapsed_time_.seconds())); + + for (int i = 0; i < num_joints; i++) { + if (i == 3 || i == 4) { + command_interfaces_[i].set_value(omega); + } else { + command_interfaces_[i].set_value(0.0); + } + } + return controller_interface::return_type::OK; +} + +CallbackReturn JointVelocityExampleController::on_init() { + try { + auto_declare("arm_id", "panda"); + + } catch (const std::exception& e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +CallbackReturn JointVelocityExampleController::on_configure( + const rclcpp_lifecycle::State& /*previous_state*/) { + arm_id_ = get_node()->get_parameter("arm_id").as_string(); + + auto client = get_node()->create_client( + "service_server/set_full_collision_behavior"); + auto request = std::make_shared(); + + request->lower_torque_thresholds_acceleration = {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}; + request->upper_torque_thresholds_acceleration = {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}; + request->lower_torque_thresholds_nominal = {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}; + request->upper_torque_thresholds_nominal = {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}; + request->lower_force_thresholds_acceleration = {20.0, 20.0, 20.0, 25.0, 25.0, 25.0}; + request->upper_force_thresholds_acceleration = {20.0, 20.0, 20.0, 25.0, 25.0, 25.0}; + request->lower_force_thresholds_nominal = {20.0, 20.0, 20.0, 25.0, 25.0, 25.0}; + request->upper_force_thresholds_nominal = {20.0, 20.0, 20.0, 25.0, 25.0, 25.0}; + + if (!client->wait_for_service(20s)) { + RCLCPP_FATAL(get_node()->get_logger(), "service server can't be found."); + return CallbackReturn::FAILURE; + } + + client->async_send_request(request); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn JointVelocityExampleController::on_activate( + const rclcpp_lifecycle::State& /*previous_state*/) { + return CallbackReturn::SUCCESS; +} + +} // namespace franka_example_controllers +#include "pluginlib/class_list_macros.hpp" +// NOLINTNEXTLINE +PLUGINLIB_EXPORT_CLASS(franka_example_controllers::JointVelocityExampleController, + controller_interface::ControllerInterface) \ No newline at end of file diff --git a/franka_gripper/package.xml b/franka_gripper/package.xml index fa354fca..02700292 100644 --- a/franka_gripper/package.xml +++ b/franka_gripper/package.xml @@ -2,7 +2,7 @@ franka_gripper - 0.1.4 + 0.1.5 This package implements the franka gripper of type Franka Hand for the use in ROS2 Franka Emika GmbH Apache 2.0 diff --git a/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp b/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp index 9d6d3cd0..745ac323 100644 --- a/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp +++ b/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp @@ -66,6 +66,7 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface { std::array hw_positions_{0, 0, 0, 0, 0, 0, 0}; std::array hw_velocities_{0, 0, 0, 0, 0, 0, 0}; std::array hw_efforts_{0, 0, 0, 0, 0, 0, 0}; + franka::RobotState hw_franka_robot_state_; franka::RobotState* hw_franka_robot_state_addr_ = &hw_franka_robot_state_; Model* hw_franka_model_ptr_ = nullptr; @@ -73,6 +74,9 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface { bool effort_interface_claimed_ = false; bool effort_interface_running_ = false; + bool velocity_joint_interface_claimed_ = false; + bool velocity_joint_interface_running_ = false; + static rclcpp::Logger getLogger(); const std::string k_robot_name{"panda"}; diff --git a/franka_hardware/include/franka_hardware/robot.hpp b/franka_hardware/include/franka_hardware/robot.hpp index bd01dc5f..94344c57 100644 --- a/franka_hardware/include/franka_hardware/robot.hpp +++ b/franka_hardware/include/franka_hardware/robot.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -59,8 +60,11 @@ class Robot { /// Stops the currently running loop and closes the connection with the robot. virtual ~Robot(); - /// Starts a read / write communication with the connected robot - virtual void initializeReadWriteInterface(); + /// Starts the active control for torque control + virtual void initializeTorqueInterface(); + + /// Starts the active control for joint velocity control + virtual void initializeJointVelocityInterface(); /// stops the read continous communication with the connected robot virtual void stopRobot(); @@ -78,11 +82,11 @@ class Robot { virtual franka_hardware::Model* getModel(); /** - * Sends new desired torque commands to the control loop in a thread-safe way. - * The robot will use these torques until a different set of torques are commanded. - * @param[in] efforts torque command for each joint. + * This function will automatically propagate the received hardware active command + * interface + * @param[in] joint_hardware_command joint hardware command either efforts or velocities */ - virtual void writeOnce(const std::array& efforts); + virtual void writeOnce(const std::array& joint_hardware_command); /** * Sets the impedance for each joint in the internal controller. @@ -200,6 +204,26 @@ class Robot { */ virtual franka::RobotState readOnceActiveControl(); + /** + * The robot will use these torques until a different set of torques are commanded. + * @param[in] efforts torque command for each joint. + */ + virtual void writeOnceEfforts(const std::array& efforts); + + /** + * The robot will use these velocities until a different set of velocities are commanded. + * @param[in] joint_velocities joint velocity command. + */ + virtual void writeOnceJointVelocities(const std::array& joint_velocities); + + /** + * @brief Checks if control loop is activated for active control. + * + * @return true when active control started either with effort or velocity command. + * @return false when active control is not started. + */ + virtual bool isControlLoopActive(); + std::mutex write_mutex_; std::mutex control_mutex_; @@ -209,7 +233,11 @@ class Robot { std::unique_ptr franka_hardware_model_; std::array last_desired_torque_ = {0, 0, 0, 0, 0, 0, 0}; - bool control_loop_active_{false}; + + bool effort_interface_active_{false}; + bool joint_velocity_interface_active_{false}; + bool velocity_command_rate_limit_active_{false}; + franka::RobotState current_state_; }; } // namespace franka_hardware diff --git a/franka_hardware/package.xml b/franka_hardware/package.xml index 5c016340..dd0ae9e4 100644 --- a/franka_hardware/package.xml +++ b/franka_hardware/package.xml @@ -2,7 +2,7 @@ franka_hardware - 0.1.4 + 0.1.5 franka_hardware provides hardware interfaces for using Franka Emika research robots with ros2_control Franka Emika GmbH Apache 2.0 diff --git a/franka_hardware/src/franka_hardware_interface.cpp b/franka_hardware/src/franka_hardware_interface.cpp index a052e6d7..dccb8329 100644 --- a/franka_hardware/src/franka_hardware_interface.cpp +++ b/franka_hardware/src/franka_hardware_interface.cpp @@ -63,6 +63,8 @@ std::vector FrankaHardwareInterface::export_command_interfaces for (auto i = 0U; i < info_.joints.size(); i++) { command_interfaces.emplace_back(CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_commands_.at(i))); + command_interfaces.emplace_back(CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_.at(i))); } return command_interfaces; } @@ -104,9 +106,10 @@ hardware_interface::return_type FrankaHardwareInterface::write(const rclcpp::Tim [](double hw_command) { return !std::isfinite(hw_command); })) { return hardware_interface::return_type::ERROR; } - if (effort_interface_running_) { + if (velocity_joint_interface_running_ || effort_interface_running_) { robot_->writeOnce(hw_commands_); } + return hardware_interface::return_type::OK; } @@ -121,15 +124,17 @@ CallbackReturn FrankaHardwareInterface::on_init(const hardware_interface::Hardwa } for (const auto& joint : info_.joints) { - if (joint.command_interfaces.size() != 1) { - RCLCPP_FATAL(getLogger(), "Joint '%s' has %zu command interfaces found. 1 expected.", + if (joint.command_interfaces.size() != 2) { + RCLCPP_FATAL(getLogger(), "Joint '%s' has %zu command interfaces found. 2 expected.", joint.name.c_str(), joint.command_interfaces.size()); return CallbackReturn::ERROR; } - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_EFFORT) { - RCLCPP_FATAL(getLogger(), "Joint '%s' has unexpected command interface '%s'. Expected '%s'", + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_EFFORT && + joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) { + RCLCPP_FATAL(getLogger(), + "Joint '%s' has unexpected command interface '%s'. Expected '%s' and '%s' ", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), - hardware_interface::HW_IF_EFFORT); + hardware_interface::HW_IF_EFFORT, hardware_interface::HW_IF_VELOCITY); return CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 3) { @@ -186,13 +191,24 @@ hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_sw const std::vector& /*start_interfaces*/, const std::vector& /*stop_interfaces*/) { if (!effort_interface_running_ && effort_interface_claimed_) { + hw_commands_.fill(0); robot_->stopRobot(); - robot_->initializeReadWriteInterface(); + robot_->initializeTorqueInterface(); effort_interface_running_ = true; } else if (effort_interface_running_ && !effort_interface_claimed_) { robot_->stopRobot(); effort_interface_running_ = false; } + + if (!velocity_joint_interface_running_ && velocity_joint_interface_claimed_) { + hw_commands_.fill(0); + robot_->stopRobot(); + robot_->initializeJointVelocityInterface(); + velocity_joint_interface_running_ = true; + } else if (velocity_joint_interface_running_ && !velocity_joint_interface_claimed_) { + robot_->stopRobot(); + velocity_joint_interface_running_ = false; + } return hardware_interface::return_type::OK; } @@ -203,29 +219,48 @@ hardware_interface::return_type FrankaHardwareInterface::prepare_command_mode_sw return interface.find(hardware_interface::HW_IF_EFFORT) != std::string::npos; }; - int64_t num_stop_effort_interfaces = - std::count_if(stop_interfaces.begin(), stop_interfaces.end(), is_effort_interface); - if (num_stop_effort_interfaces == kNumberOfJoints) { - effort_interface_claimed_ = false; - } else if (num_stop_effort_interfaces != 0) { - RCLCPP_FATAL(this->getLogger(), "Expected %ld effort interfaces to stop, but got %ld instead.", - kNumberOfJoints, num_stop_effort_interfaces); - std::string error_string = "Invalid number of effort interfaces to stop. Expected "; - error_string += std::to_string(kNumberOfJoints); - throw std::invalid_argument(error_string); - } + auto is_velocity_interface = [](const std::string& interface) { + return interface.find(hardware_interface::HW_IF_VELOCITY) != std::string::npos; + }; + + auto generate_error_message = [&](const std::string& start_stop_command, + const std::string& interface_name, int64_t num_interface) { + RCLCPP_FATAL(this->getLogger(), "Expected %ld %s interfaces to %s, but got %ld instead.", + kNumberOfJoints, interface_name.c_str(), start_stop_command.c_str(), + num_interface); + std::ostringstream error_message_stream; + error_message_stream << "Invalid number of " << interface_name << " interfaces to " + << start_stop_command << ". Expected " << std::to_string(kNumberOfJoints); + std::string error_message = error_message_stream.str(); + + throw std::invalid_argument(error_message); + }; + + auto start_stop_interface = + [&](const std::function& find_interface_function, + const std::string& interface_name, bool& claim_flag) { + int64_t num_stop_interface = + std::count_if(stop_interfaces.begin(), stop_interfaces.end(), find_interface_function); + int64_t num_start_interface = std::count_if( + start_interfaces.begin(), start_interfaces.end(), find_interface_function); + + if (num_stop_interface == kNumberOfJoints) { + claim_flag = false; + } else if (num_stop_interface != 0) { + generate_error_message("stop", interface_name, num_stop_interface); + } + if (num_start_interface == kNumberOfJoints) { + claim_flag = true; + } else if (num_start_interface != 0) { + generate_error_message("start", interface_name, num_start_interface); + } + }; + + start_stop_interface(is_effort_interface, hardware_interface::HW_IF_EFFORT, + effort_interface_claimed_); + start_stop_interface(is_velocity_interface, hardware_interface::HW_IF_VELOCITY, + velocity_joint_interface_claimed_); - int64_t num_start_effort_interfaces = - std::count_if(start_interfaces.begin(), start_interfaces.end(), is_effort_interface); - if (num_start_effort_interfaces == kNumberOfJoints) { - effort_interface_claimed_ = true; - } else if (num_start_effort_interfaces != 0) { - RCLCPP_FATAL(this->getLogger(), "Expected %ld effort interfaces to start, but got %ld instead.", - kNumberOfJoints, num_start_effort_interfaces); - std::string error_string = "Invalid number of effort interfaces to start. Expected "; - error_string += std::to_string(kNumberOfJoints); - throw std::invalid_argument(error_string); - } return hardware_interface::return_type::OK; } } // namespace franka_hardware diff --git a/franka_hardware/src/robot.cpp b/franka_hardware/src/robot.cpp index f580c439..e7ffbf58 100644 --- a/franka_hardware/src/robot.cpp +++ b/franka_hardware/src/robot.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include "franka_hardware/robot.hpp" @@ -45,21 +46,31 @@ Robot::~Robot() { franka::RobotState Robot::readOnce() { std::lock_guard lock(control_mutex_); - if (!control_loop_active_) { - return robot_->readOnce(); + if (!isControlLoopActive()) { + current_state_ = robot_->readOnce(); + return current_state_; } else { return readOnceActiveControl(); } } void Robot::stopRobot() { - if (control_loop_active_) { - control_loop_active_ = false; + if (isControlLoopActive()) { + effort_interface_active_ = false; + joint_velocity_interface_active_ = false; active_control_.reset(); } } -void Robot::writeOnce(const std::array& efforts) { +void Robot::writeOnce(const std::array& joint_commands) { + if (effort_interface_active_) { + writeOnceEfforts(joint_commands); + } else if (joint_velocity_interface_active_) { + writeOnceJointVelocities(joint_commands); + } +} + +void Robot::writeOnceEfforts(const std::array& efforts) { std::lock_guard lock(control_mutex_); auto torque_command = franka::Torques(efforts); @@ -70,19 +81,46 @@ void Robot::writeOnce(const std::array& efforts) { active_control_->writeOnce(torque_command); } +void Robot::writeOnceJointVelocities(const std::array& velocities) { + std::lock_guard lock(control_mutex_); + + auto velocity_command = franka::JointVelocities(velocities); + + // If you are experiencing issues with robot error. You can try activating the rate limiter. + // Rate limiter is default deactivated. + if (velocity_command_rate_limit_active_) { + velocity_command.dq = franka::limitRate( + franka::computeUpperLimitsJointVelocity(current_state_.q_d), + franka::computeLowerLimitsJointVelocity(current_state_.q_d), franka::kMaxJointAcceleration, + franka::kMaxJointJerk, velocity_command.dq, current_state_.dq_d, current_state_.ddq_d); + } + + active_control_->writeOnce(velocity_command); +} + franka::RobotState Robot::readOnceActiveControl() { // When controller is active use active control to read the robot state - const auto [robot_state, _] = active_control_->readOnce(); - return robot_state; + const auto [current_state_, _] = active_control_->readOnce(); + return current_state_; } franka_hardware::Model* Robot::getModel() { return franka_hardware_model_.get(); } -void Robot::initializeReadWriteInterface() { - control_loop_active_ = true; +void Robot::initializeTorqueInterface() { active_control_ = robot_->startTorqueControl(); + effort_interface_active_ = true; +} + +void Robot::initializeJointVelocityInterface() { + active_control_ = robot_->startJointVelocityControl( + research_interface::robot::Move::ControllerMode::kJointImpedance); + joint_velocity_interface_active_ = true; +} + +bool Robot::isControlLoopActive() { + return joint_velocity_interface_active_ || effort_interface_active_; } void Robot::setJointStiffness(const franka_msgs::srv::SetJointStiffness::Request::SharedPtr& req) { diff --git a/franka_hardware/test/franka_hardware_interface_test.cpp b/franka_hardware/test/franka_hardware_interface_test.cpp index 17c167c8..68769e50 100644 --- a/franka_hardware/test/franka_hardware_interface_test.cpp +++ b/franka_hardware/test/franka_hardware_interface_test.cpp @@ -43,11 +43,14 @@ const double k_EPS{1e-5}; using namespace std::chrono_literals; +class FrankaHardwareInterfaceTest : public ::testing::TestWithParam {}; + class MockModel : public franka_hardware::Model {}; class MockRobot : public franka_hardware::Robot { public: - MOCK_METHOD(void, initializeReadWriteInterface, (), (override)); + MOCK_METHOD(void, initializeTorqueInterface, (), (override)); + MOCK_METHOD(void, initializeJointVelocityInterface, (), (override)); MOCK_METHOD(void, stopRobot, (), (override)); MOCK_METHOD(franka::RobotState, readOnce, (), (override)); MOCK_METHOD(MockModel*, getModel, (), (override)); @@ -85,8 +88,8 @@ auto createHardwareInfo() -> hardware_interface::HardwareInfo { hw_params["robot_ip"] = "dummy_ip"; info.hardware_parameters = hw_params; - hardware_interface::InterfaceInfo command_interface, effort_state_interface, - position_state_interface, velocity_state_interface; + hardware_interface::InterfaceInfo command_effort_interface, command_velocity_interface, + effort_state_interface, position_state_interface, velocity_state_interface; effort_state_interface.name = hardware_interface::HW_IF_EFFORT; position_state_interface.name = hardware_interface::HW_IF_POSITION; @@ -95,14 +98,17 @@ auto createHardwareInfo() -> hardware_interface::HardwareInfo { std::vector state_interfaces = { position_state_interface, velocity_state_interface, effort_state_interface}; + command_effort_interface.name = k_effort_controller; + command_velocity_interface.name = k_velocity_controller; + for (auto i = 0U; i < k_number_of_joints; i++) { hardware_interface::ComponentInfo joint; joint.name = k_joint_name + std::to_string(i + 1); - command_interface.name = k_effort_controller; + joint.command_interfaces.push_back(command_effort_interface); + joint.command_interfaces.push_back(command_velocity_interface); - joint.command_interfaces.push_back(command_interface); joint.state_interfaces = state_interfaces; info.joints.push_back(joint); @@ -147,7 +153,8 @@ void get_param_service_response( response = *result.get(); } -TEST(FrankaHardwareInterfaceTest, when_on_init_called_expect_success) { +TEST_P(FrankaHardwareInterfaceTest, when_on_init_called_expect_success) { + std::string command_interface = GetParam(); auto mock_robot = std::make_shared(); const hardware_interface::HardwareInfo info = createHardwareInfo(); franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); @@ -174,9 +181,11 @@ TEST(FrankaHardwareInterfaceTest, given_that_the_robot_interfaces_set_when_read_ EXPECT_EQ(return_type, hardware_interface::return_type::OK); } -TEST( +TEST_P( FrankaHardwareInterfaceTest, given_that_the_robot_interfaces_are_set_when_call_export_state_return_zero_values_and_correct_interface_names) { + std::string command_interface = GetParam(); + franka::RobotState robot_state; const size_t state_interface_size = 23; // position, effort and velocity states for // every joint + robot state and model @@ -217,9 +226,11 @@ TEST( EXPECT_EQ(states.size(), state_interface_size); } -TEST( +TEST_P( FrankaHardwareInterfaceTest, given_that_the_robot_interfaces_are_set_when_call_export_state_interface_robot_model_interface_exists) { + std::string command_interface = GetParam(); + franka::RobotState robot_state; const size_t state_interface_size = 23; // position, effort and velocity states for // every joint + robot state and model @@ -247,9 +258,11 @@ TEST( // is correctly pushed to state interface } -TEST( +TEST_P( FrankaHardwareInterfaceTest, given_that_the_robot_interfaces_are_set_when_call_export_state_interface_robot_state_interface_exists) { + std::string command_interface = GetParam(); + const size_t state_interface_size = 23; // position, effort and velocity states for // every joint + robot state and model auto mock_robot = std::make_shared(); @@ -278,8 +291,10 @@ TEST( // is correctly pushed to state interface } -TEST(FrankaHardwareInterfaceTest, - when_prepare_command_mode_interface_for_stop_effort_interfaces_expect_ok) { +TEST_P(FrankaHardwareInterfaceTest, + when_prepare_command_mode_interface_for_stop_effort_interfaces_expect_ok) { + std::string command_interface = GetParam(); + auto mock_robot = std::make_shared(); franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); @@ -289,16 +304,18 @@ TEST(FrankaHardwareInterfaceTest, for (size_t i = 0; i < hardware_info.joints.size(); i++) { const std::string joint_name = k_joint_name + std::to_string(i); - stop_interface.push_back(joint_name + "/" + k_effort_controller); + stop_interface.push_back(joint_name + "/" + command_interface); } std::vector start_interface = {}; EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), hardware_interface::return_type::OK); } -TEST( +TEST_P( FrankaHardwareInterfaceTest, when_prepare_command_mode_interface_is_called_with_invalid_start_interface_number_expect_throw) { + std::string command_interface = GetParam(); + auto mock_robot = std::make_shared(); franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); @@ -308,7 +325,7 @@ TEST( for (size_t i = 0; i < hardware_info.joints.size(); i++) { const std::string joint_name = k_joint_name + std::to_string(i); - stop_interface.push_back(joint_name + "/" + k_effort_controller); + stop_interface.push_back(joint_name + "/" + command_interface); } std::vector start_interface = {"fr3_joint1/effort"}; EXPECT_THROW( @@ -316,8 +333,10 @@ TEST( std::invalid_argument); } -TEST(FrankaHardwareInterfaceTest, - when_prepare_command_mode_interface_for_start_effort_interfaces_expect_ok) { +TEST_P(FrankaHardwareInterfaceTest, + when_prepare_command_mode_interface_for_start_effort_interfaces_expect_ok) { + std::string command_interface = GetParam(); + auto mock_robot = std::make_shared(); franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); @@ -327,7 +346,7 @@ TEST(FrankaHardwareInterfaceTest, for (size_t i = 0; i < hardware_info.joints.size(); i++) { const std::string joint_name = k_joint_name + std::to_string(i); - start_interface.push_back(joint_name + "/" + k_effort_controller); + start_interface.push_back(joint_name + "/" + command_interface); } std::vector stop_interface = {}; @@ -336,29 +355,33 @@ TEST(FrankaHardwareInterfaceTest, hardware_interface::return_type::OK); } -TEST( +TEST_P( FrankaHardwareInterfaceTest, when_prepare_command_mode_interface_is_called_with_invalid_stop_interface_number_expect_throw) { + std::string command_interface = GetParam(); + auto mock_robot = std::make_shared(); franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); const auto hardware_info = createHardwareInfo(); franka_hardware_interface.on_init(hardware_info); - std::vector start_interface; + std::vector start_interface, stop_interface; for (size_t i = 0; i < hardware_info.joints.size(); i++) { const std::string joint_name = k_joint_name + std::to_string(i); - start_interface.push_back(joint_name + "/" + k_effort_controller); + stop_interface.push_back(joint_name + "/" + command_interface); } - std::vector stop_interface = {"fr3_joint1/effort"}; + start_interface = {"fr3_joint1/effort"}; EXPECT_THROW( franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), std::invalid_argument); } -TEST(FrankaHardwareIntefaceTest, when_write_called_expect_ok) { +TEST_P(FrankaHardwareInterfaceTest, when_write_called_expect_ok) { + std::string command_interface = GetParam(); + auto mock_robot = std::make_shared(); EXPECT_CALL(*mock_robot, writeOnce(testing::_)); @@ -370,14 +393,14 @@ TEST(FrankaHardwareIntefaceTest, when_write_called_expect_ok) { for (size_t i = 0; i < hardware_info.joints.size(); i++) { const std::string joint_name = k_joint_name + std::to_string(i); - start_interface.push_back(joint_name + "/" + k_effort_controller); + start_interface.push_back(joint_name + "/" + command_interface); } std::vector stop_interface = {}; EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), hardware_interface::return_type::OK); - // can call write only after switching to the torque controller + // can call write only after performing command mode switch EXPECT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface), hardware_interface::return_type::OK); @@ -415,10 +438,18 @@ TEST(FrankaHardwareInterfaceTest, when_on_deactivate_called_expect_success) { rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); } -TEST(FrankaHardwareInterfaceTest, - given_start_effort_interface_prepared_when_perform_comamnd_mode_switch_called_expect_ok) { +TEST_P(FrankaHardwareInterfaceTest, + given_start_effort_interface_prepared_when_perform_command_mode_switch_called_expect_ok) { + std::string command_interface = GetParam(); + auto mock_robot = std::make_shared(); + if (command_interface == k_effort_controller) { + EXPECT_CALL(*mock_robot, initializeTorqueInterface()); + } else if (command_interface == k_velocity_controller) { + EXPECT_CALL(*mock_robot, initializeJointVelocityInterface()); + } + franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); const auto hardware_info = createHardwareInfo(); @@ -427,7 +458,7 @@ TEST(FrankaHardwareInterfaceTest, for (size_t i = 0; i < hardware_info.joints.size(); i++) { const std::string joint_name = k_joint_name + std::to_string(i); - start_interface.push_back(joint_name + "/" + k_effort_controller); + start_interface.push_back(joint_name + "/" + command_interface); } std::vector stop_interface = {}; @@ -439,9 +470,12 @@ TEST(FrankaHardwareInterfaceTest, hardware_interface::return_type::OK); } -TEST(FrankaHardwareInterfaceTest, - given_that_effort_control_started_perform_command_mode_switch_stop_expect_ok) { +TEST_P(FrankaHardwareInterfaceTest, + given_that_effort_control_started_perform_command_mode_switch_stop_expect_ok) { + std::string command_interface = GetParam(); + auto mock_robot = std::make_shared(); + EXPECT_CALL(*mock_robot, stopRobot()).Times(2); franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); @@ -451,7 +485,7 @@ TEST(FrankaHardwareInterfaceTest, for (size_t i = 0; i < hardware_info.joints.size(); i++) { const std::string joint_name = k_joint_name + std::to_string(i); - start_interface.push_back(joint_name + "/" + k_effort_controller); + start_interface.push_back(joint_name + "/" + command_interface); } std::vector stop_interface = {}; @@ -464,7 +498,7 @@ TEST(FrankaHardwareInterfaceTest, for (size_t i = 0; i < hardware_info.joints.size(); i++) { const std::string joint_name = k_joint_name + std::to_string(i); - stop_interface.push_back(joint_name + "/" + k_effort_controller); + stop_interface.push_back(joint_name + "/" + command_interface); } start_interface.clear(); @@ -694,4 +728,8 @@ int main(int argc, char** argv) { rclcpp::init(0, nullptr); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} + +INSTANTIATE_TEST_SUITE_P(FrankaHardwareTests, + FrankaHardwareInterfaceTest, + ::testing::Values(k_velocity_controller, k_effort_controller)); \ No newline at end of file diff --git a/franka_moveit_config/package.xml b/franka_moveit_config/package.xml index 408a9a39..0a96cc07 100644 --- a/franka_moveit_config/package.xml +++ b/franka_moveit_config/package.xml @@ -2,7 +2,7 @@ franka_moveit_config - 0.1.4 + 0.1.5 Contains Moveit2 configuration files for Franka Emika research robots Franka Emika GmbH Apache 2.0 diff --git a/franka_msgs/package.xml b/franka_msgs/package.xml index 92427076..ceffde45 100644 --- a/franka_msgs/package.xml +++ b/franka_msgs/package.xml @@ -2,7 +2,7 @@ franka_msgs - 0.1.4 + 0.1.5 franka_msgs provides messages and actions specific to Franka Emika research robots Franka Emika GmbH Apache 2.0 diff --git a/franka_robot_state_broadcaster/package.xml b/franka_robot_state_broadcaster/package.xml index 2251190d..214bcbdd 100644 --- a/franka_robot_state_broadcaster/package.xml +++ b/franka_robot_state_broadcaster/package.xml @@ -1,7 +1,7 @@ franka_robot_state_broadcaster - 0.1.4 + 0.1.5 Broadcaster to publish robot states Franka Emika GmbH Apache 2.0 diff --git a/integration_launch_testing/package.xml b/integration_launch_testing/package.xml index 45f6a666..3b9b08b9 100644 --- a/integration_launch_testing/package.xml +++ b/integration_launch_testing/package.xml @@ -2,7 +2,7 @@ integration_launch_testing - 0.1.4 + 0.1.5 Functional integration tests for franka controllers Franka Emika GmbH Apache 2.0