From c51c3560c369f023674b52ae6475e05b7f703f4a Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Tue, 19 Sep 2023 16:30:57 +0200 Subject: [PATCH 1/4] feat/joint-velocity-interface --- franka_bringup/config/controllers.yaml | 3 + ...oint_velocity_example_controller.launch.py | 77 +++++++++++++++ franka_example_controllers/CMakeLists.txt | 1 + .../franka_example_controllers.xml | 6 ++ .../joint_velocity_example_controller.hpp | 47 ++++++++++ .../src/joint_velocity_example_controller.cpp | 93 +++++++++++++++++++ .../franka_hardware_interface.hpp | 4 + .../include/franka_hardware/robot.hpp | 11 ++- .../src/franka_hardware_interface.cpp | 49 ++++++++++ franka_hardware/src/robot.cpp | 27 +++++- .../test/franka_hardware_interface_test.cpp | 11 ++- 11 files changed, 321 insertions(+), 8 deletions(-) create mode 100644 franka_bringup/launch/joint_velocity_example_controller.launch.py create mode 100644 franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.hpp create mode 100644 franka_example_controllers/src/joint_velocity_example_controller.cpp 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_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt index 4e5aa0d0..c2d1e692 100644 --- a/franka_example_controllers/CMakeLists.txt +++ b/franka_example_controllers/CMakeLists.txt @@ -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..155c742b 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 impedance example controller moves joint 4 and 5 in a very compliant 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/src/joint_velocity_example_controller.cpp b/franka_example_controllers/src/joint_velocity_example_controller.cpp new file mode 100644 index 00000000..d5d932e4 --- /dev/null +++ b/franka_example_controllers/src/joint_velocity_example_controller.cpp @@ -0,0 +1,93 @@ +// 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 + +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++) { + command_interfaces_[i].set_value(omega); + } + 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(); + + 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_hardware/include/franka_hardware/franka_hardware_interface.hpp b/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp index 9d6d3cd0..b83e33d5 100644 --- a/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp +++ b/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp @@ -63,6 +63,7 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface { std::shared_ptr executor_; std::array hw_commands_{0, 0, 0, 0, 0, 0, 0}; + std::array hw_velocity_commands_{0, 0, 0, 0, 0, 0, 0}; 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}; @@ -73,6 +74,9 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface { bool effort_interface_claimed_ = false; bool effort_interface_running_ = false; + bool velocity_interface_claimed_ = false; + bool velocity_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..ec0c3e24 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 @@ -62,6 +63,9 @@ class Robot { /// Starts a read / write communication with the connected robot virtual void initializeReadWriteInterface(); + /// Starts a read / write communication with the connected robot + virtual void initializeJointVelocityInterface(); + /// stops the read continous communication with the connected robot virtual void stopRobot(); @@ -78,12 +82,17 @@ 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. */ virtual void writeOnce(const std::array& efforts); + /** + * The robot will use these velocities until a different set of velocities are commanded. + * @param[in] velocities torque command for each joint. + */ + virtual void writeOnceVelocities(const std::array& velocities); + /** * Sets the impedance for each joint in the internal controller. * diff --git a/franka_hardware/src/franka_hardware_interface.cpp b/franka_hardware/src/franka_hardware_interface.cpp index a052e6d7..72cbc808 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_velocity_commands_.at(i))); } return command_interfaces; } @@ -104,8 +106,14 @@ hardware_interface::return_type FrankaHardwareInterface::write(const rclcpp::Tim [](double hw_command) { return !std::isfinite(hw_command); })) { return hardware_interface::return_type::ERROR; } + if (std::any_of(hw_velocity_commands_.begin(), hw_velocity_commands_.end(), + [](double hw_command) { return !std::isfinite(hw_command); })) { + return hardware_interface::return_type::ERROR; + } if (effort_interface_running_) { robot_->writeOnce(hw_commands_); + } else if (velocity_interface_running_) { + robot_->writeOnceVelocities(hw_velocity_commands_); } return hardware_interface::return_type::OK; } @@ -193,6 +201,16 @@ hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_sw robot_->stopRobot(); effort_interface_running_ = false; } + + if (!velocity_interface_running_ && velocity_interface_claimed_) { + robot_->stopRobot(); + robot_->initializeJointVelocityInterface(); + velocity_interface_running_ = true; + } else if (velocity_interface_running_ && !velocity_interface_claimed_) { + robot_->stopRobot(); + hw_velocity_commands_ = {0, 0, 0, 0, 0, 0, 0}; + velocity_interface_running_ = false; + } return hardware_interface::return_type::OK; } @@ -203,6 +221,10 @@ hardware_interface::return_type FrankaHardwareInterface::prepare_command_mode_sw return interface.find(hardware_interface::HW_IF_EFFORT) != std::string::npos; }; + auto is_velocity_interface = [](const std::string& interface) { + return interface.find(hardware_interface::HW_IF_VELOCITY) != 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) { @@ -226,6 +248,33 @@ hardware_interface::return_type FrankaHardwareInterface::prepare_command_mode_sw error_string += std::to_string(kNumberOfJoints); throw std::invalid_argument(error_string); } + + int64_t num_stop_velocity_interfaces = + std::count_if(stop_interfaces.begin(), stop_interfaces.end(), is_velocity_interface); + if (num_stop_velocity_interfaces == kNumberOfJoints) { + velocity_interface_claimed_ = false; + } else if (num_stop_velocity_interfaces != 0) { + RCLCPP_FATAL(this->getLogger(), + "Expected %ld velocity interfaces to stop, but got %ld instead.", kNumberOfJoints, + num_stop_velocity_interfaces); + std::string error_string = "Invalid number of velocity interfaces to stop. Expected "; + error_string += std::to_string(kNumberOfJoints); + throw std::invalid_argument(error_string); + } + + int64_t num_start_velocity_interfaces = + std::count_if(start_interfaces.begin(), start_interfaces.end(), is_velocity_interface); + if (num_start_velocity_interfaces == kNumberOfJoints) { + velocity_interface_claimed_ = true; + } else if (num_start_velocity_interfaces != 0) { + RCLCPP_FATAL(this->getLogger(), + "Expected %ld velocity interfaces to start, but got %ld instead.", kNumberOfJoints, + num_start_velocity_interfaces); + std::string error_string = "Invalid number of velocity 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..8fbf2303 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" @@ -46,7 +47,8 @@ Robot::~Robot() { franka::RobotState Robot::readOnce() { std::lock_guard lock(control_mutex_); if (!control_loop_active_) { - return robot_->readOnce(); + current_state_ = robot_->readOnce(); + return current_state_; } else { return readOnceActiveControl(); } @@ -70,10 +72,23 @@ void Robot::writeOnce(const std::array& efforts) { active_control_->writeOnce(torque_command); } +void Robot::writeOnceVelocities(const std::array& velocities) { + std::lock_guard lock(control_mutex_); + + auto velocity_command = franka::JointVelocities(velocities); + + 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() { @@ -85,6 +100,12 @@ void Robot::initializeReadWriteInterface() { active_control_ = robot_->startTorqueControl(); } +void Robot::initializeJointVelocityInterface() { + control_loop_active_ = true; + active_control_ = robot_->startJointVelocityControl( + research_interface::robot::Move::ControllerMode::kJointImpedance); +} + void Robot::setJointStiffness(const franka_msgs::srv::SetJointStiffness::Request::SharedPtr& req) { std::lock_guard lock(write_mutex_); std::array joint_stiffness{}; diff --git a/franka_hardware/test/franka_hardware_interface_test.cpp b/franka_hardware/test/franka_hardware_interface_test.cpp index 17c167c8..7374a497 100644 --- a/franka_hardware/test/franka_hardware_interface_test.cpp +++ b/franka_hardware/test/franka_hardware_interface_test.cpp @@ -85,8 +85,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 +95,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); From 9785856bd8d074ecdbd9afea13c5f5600604e3ce Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Wed, 11 Oct 2023 18:15:25 +0200 Subject: [PATCH 2/4] test: write velocity command interface tests --- .../franka_example_controllers.xml | 2 +- .../franka_hardware_interface.hpp | 2 +- .../include/franka_hardware/robot.hpp | 33 ++++- .../src/franka_hardware_interface.cpp | 22 ++-- franka_hardware/src/robot.cpp | 41 ++++-- .../test/franka_hardware_interface_test.cpp | 121 ++++++++++++------ 6 files changed, 144 insertions(+), 77 deletions(-) diff --git a/franka_example_controllers/franka_example_controllers.xml b/franka_example_controllers/franka_example_controllers.xml index 155c742b..5649176a 100644 --- a/franka_example_controllers/franka_example_controllers.xml +++ b/franka_example_controllers/franka_example_controllers.xml @@ -14,7 +14,7 @@ - The joint impedance example controller moves joint 4 and 5 in a very compliant periodic movement. + The joint velocity example controller. executor_; std::array hw_commands_{0, 0, 0, 0, 0, 0, 0}; - std::array hw_velocity_commands_{0, 0, 0, 0, 0, 0, 0}; 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; diff --git a/franka_hardware/include/franka_hardware/robot.hpp b/franka_hardware/include/franka_hardware/robot.hpp index ec0c3e24..6cf58414 100644 --- a/franka_hardware/include/franka_hardware/robot.hpp +++ b/franka_hardware/include/franka_hardware/robot.hpp @@ -60,10 +60,10 @@ 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 a read / write communication with the connected robot + /// Starts the active control for joint velocity control virtual void initializeJointVelocityInterface(); /// stops the read continous communication with the connected robot @@ -81,17 +81,32 @@ class Robot { */ virtual franka_hardware::Model* getModel(); + /** + * @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(); + + /** + * This function will receive 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& joint_hardware_command); + /** * The robot will use these torques until a different set of torques are commanded. * @param[in] efforts torque command for each joint. */ - virtual void writeOnce(const std::array& efforts); + virtual void writeOnceEfforts(const std::array& efforts); /** * The robot will use these velocities until a different set of velocities are commanded. - * @param[in] velocities torque command for each joint. + * @param[in] joint_velocities joint velocity command. */ - virtual void writeOnceVelocities(const std::array& velocities); + virtual void writeOnceJointVelocities(const std::array& joint_velocities); /** * Sets the impedance for each joint in the internal controller. @@ -218,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/src/franka_hardware_interface.cpp b/franka_hardware/src/franka_hardware_interface.cpp index 72cbc808..e973cbdd 100644 --- a/franka_hardware/src/franka_hardware_interface.cpp +++ b/franka_hardware/src/franka_hardware_interface.cpp @@ -64,7 +64,7 @@ std::vector FrankaHardwareInterface::export_command_interfaces 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_velocity_commands_.at(i))); + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_.at(i))); } return command_interfaces; } @@ -106,15 +106,9 @@ hardware_interface::return_type FrankaHardwareInterface::write(const rclcpp::Tim [](double hw_command) { return !std::isfinite(hw_command); })) { return hardware_interface::return_type::ERROR; } - if (std::any_of(hw_velocity_commands_.begin(), hw_velocity_commands_.end(), - [](double hw_command) { return !std::isfinite(hw_command); })) { - return hardware_interface::return_type::ERROR; - } - if (effort_interface_running_) { - robot_->writeOnce(hw_commands_); - } else if (velocity_interface_running_) { - robot_->writeOnceVelocities(hw_velocity_commands_); - } + + robot_->writeOnce(hw_commands_); + return hardware_interface::return_type::OK; } @@ -134,8 +128,9 @@ CallbackReturn FrankaHardwareInterface::on_init(const hardware_interface::Hardwa 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' ", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_EFFORT); return CallbackReturn::ERROR; @@ -195,7 +190,7 @@ hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_sw const std::vector& /*stop_interfaces*/) { if (!effort_interface_running_ && effort_interface_claimed_) { robot_->stopRobot(); - robot_->initializeReadWriteInterface(); + robot_->initializeTorqueInterface(); effort_interface_running_ = true; } else if (effort_interface_running_ && !effort_interface_claimed_) { robot_->stopRobot(); @@ -208,7 +203,6 @@ hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_sw velocity_interface_running_ = true; } else if (velocity_interface_running_ && !velocity_interface_claimed_) { robot_->stopRobot(); - hw_velocity_commands_ = {0, 0, 0, 0, 0, 0, 0}; velocity_interface_running_ = false; } return hardware_interface::return_type::OK; diff --git a/franka_hardware/src/robot.cpp b/franka_hardware/src/robot.cpp index 8fbf2303..e7ffbf58 100644 --- a/franka_hardware/src/robot.cpp +++ b/franka_hardware/src/robot.cpp @@ -46,7 +46,7 @@ Robot::~Robot() { franka::RobotState Robot::readOnce() { std::lock_guard lock(control_mutex_); - if (!control_loop_active_) { + if (!isControlLoopActive()) { current_state_ = robot_->readOnce(); return current_state_; } else { @@ -55,13 +55,22 @@ franka::RobotState Robot::readOnce() { } 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); @@ -72,15 +81,19 @@ void Robot::writeOnce(const std::array& efforts) { active_control_->writeOnce(torque_command); } -void Robot::writeOnceVelocities(const std::array& velocities) { +void Robot::writeOnceJointVelocities(const std::array& velocities) { std::lock_guard lock(control_mutex_); auto velocity_command = franka::JointVelocities(velocities); - 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); + // 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); } @@ -95,15 +108,19 @@ 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() { - control_loop_active_ = true; 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 7374a497..ee7001cb 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)); @@ -79,7 +82,7 @@ class MockRobot : public franka_hardware::Robot { (override)); }; -auto createHardwareInfo() -> hardware_interface::HardwareInfo { +auto createHardwareInfo(const std::string& command_interface) -> hardware_interface::HardwareInfo { hardware_interface::HardwareInfo info; std::unordered_map hw_params; hw_params["robot_ip"] = "dummy_ip"; @@ -103,9 +106,11 @@ auto createHardwareInfo() -> hardware_interface::HardwareInfo { joint.name = k_joint_name + std::to_string(i + 1); - joint.command_interfaces.push_back(command_effort_interface); - joint.command_interfaces.push_back(command_velocity_interface); - + if (command_interface == k_effort_controller) { + joint.command_interfaces.push_back(command_effort_interface); + } else if (command_interface == k_velocity_controller) { + joint.command_interfaces.push_back(command_velocity_interface); + } joint.state_interfaces = state_interfaces; info.joints.push_back(joint); @@ -126,7 +131,7 @@ void get_param_service_response( franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); - const auto hardware_info = createHardwareInfo(); + const auto hardware_info = createHardwareInfo(k_effort_controller); franka_hardware_interface.on_init(hardware_info); auto node = rclcpp::Node::make_shared("test_node"); @@ -150,9 +155,10 @@ 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(); + const hardware_interface::HardwareInfo info = createHardwareInfo(command_interface); franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); auto return_type = franka_hardware_interface.on_init(info); @@ -177,9 +183,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 @@ -192,7 +200,7 @@ TEST( franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot)); - const auto hardware_info = createHardwareInfo(); + const auto hardware_info = createHardwareInfo(command_interface); franka_hardware_interface.on_init(hardware_info); auto time = rclcpp::Time(0); auto duration = rclcpp::Duration(0, 0); @@ -220,9 +228,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 @@ -235,7 +245,7 @@ TEST( EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address)); franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); - const auto hardware_info = createHardwareInfo(); + const auto hardware_info = createHardwareInfo(command_interface); franka_hardware_interface.on_init(hardware_info); auto time = rclcpp::Time(0); auto duration = rclcpp::Duration(0, 0); @@ -250,9 +260,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(); @@ -266,7 +278,7 @@ TEST( EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address)); franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); - const auto hardware_info = createHardwareInfo(); + const auto hardware_info = createHardwareInfo(command_interface); franka_hardware_interface.on_init(hardware_info); auto time = rclcpp::Time(0); auto duration = rclcpp::Duration(0, 0); @@ -281,37 +293,41 @@ 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); - const auto hardware_info = createHardwareInfo(); + const auto hardware_info = createHardwareInfo(command_interface); franka_hardware_interface.on_init(hardware_info); std::vector 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); - 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); - const auto hardware_info = createHardwareInfo(); + const auto hardware_info = createHardwareInfo(command_interface); franka_hardware_interface.on_init(hardware_info); std::vector 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); - 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( @@ -319,18 +335,20 @@ 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); - const auto hardware_info = createHardwareInfo(); + const auto hardware_info = createHardwareInfo(command_interface); franka_hardware_interface.on_init(hardware_info); std::vector start_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); + start_interface.push_back(joint_name + "/" + command_interface); } std::vector stop_interface = {}; @@ -339,19 +357,21 @@ 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(); + const auto hardware_info = createHardwareInfo(command_interface); franka_hardware_interface.on_init(hardware_info); std::vector start_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); + start_interface.push_back(joint_name + "/" + command_interface); } std::vector stop_interface = {"fr3_joint1/effort"}; @@ -361,26 +381,28 @@ TEST( 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::_)); franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot)); - const auto hardware_info = createHardwareInfo(); + const auto hardware_info = createHardwareInfo(command_interface); franka_hardware_interface.on_init(hardware_info); std::vector start_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); + 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); @@ -418,19 +440,27 @@ 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(); + const auto hardware_info = createHardwareInfo(command_interface); franka_hardware_interface.on_init(hardware_info); std::vector start_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); + start_interface.push_back(joint_name + "/" + command_interface); } std::vector stop_interface = {}; @@ -442,19 +472,22 @@ 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); - const auto hardware_info = createHardwareInfo(); + const auto hardware_info = createHardwareInfo(command_interface); franka_hardware_interface.on_init(hardware_info); std::vector start_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); + start_interface.push_back(joint_name + "/" + command_interface); } std::vector stop_interface = {}; @@ -467,7 +500,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(); @@ -697,4 +730,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 From 378bb8a62567d7b51d6947ad1fda1f582edb3d93 Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Thu, 12 Oct 2023 16:30:01 +0200 Subject: [PATCH 3/4] update velocity controller example collision thresholds --- .../robots/panda_arm.ros2_control.xacro | 1 + .../franka_example_controllers.xml | 2 +- .../src/joint_velocity_example_controller.cpp | 29 ++++++++++++++++++- .../franka_hardware_interface.hpp | 4 +-- .../src/franka_hardware_interface.cpp | 19 +++++++----- 5 files changed, 43 insertions(+), 12 deletions(-) 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/franka_example_controllers.xml b/franka_example_controllers/franka_example_controllers.xml index 5649176a..8359ff1b 100644 --- a/franka_example_controllers/franka_example_controllers.xml +++ b/franka_example_controllers/franka_example_controllers.xml @@ -14,7 +14,7 @@ - The joint velocity example controller. + The joint velocity example controller moves joint 4 and 5 in a periodic movement. +#include #include #include @@ -21,6 +22,8 @@ #include +using namespace std::chrono_literals; + namespace franka_example_controllers { controller_interface::InterfaceConfiguration @@ -58,7 +61,11 @@ controller_interface::return_type JointVelocityExampleController::update( (1.0 - std::cos(2.0 * M_PI / time_max.seconds() * elapsed_time_.seconds())); for (int i = 0; i < num_joints; i++) { - command_interfaces_[i].set_value(omega); + 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; } @@ -78,6 +85,26 @@ 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; } diff --git a/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp b/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp index 42ad6ec9..745ac323 100644 --- a/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp +++ b/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp @@ -74,8 +74,8 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface { bool effort_interface_claimed_ = false; bool effort_interface_running_ = false; - bool velocity_interface_claimed_ = false; - bool velocity_interface_running_ = false; + bool velocity_joint_interface_claimed_ = false; + bool velocity_joint_interface_running_ = false; static rclcpp::Logger getLogger(); diff --git a/franka_hardware/src/franka_hardware_interface.cpp b/franka_hardware/src/franka_hardware_interface.cpp index e973cbdd..88954319 100644 --- a/franka_hardware/src/franka_hardware_interface.cpp +++ b/franka_hardware/src/franka_hardware_interface.cpp @@ -106,8 +106,9 @@ hardware_interface::return_type FrankaHardwareInterface::write(const rclcpp::Tim [](double hw_command) { return !std::isfinite(hw_command); })) { return hardware_interface::return_type::ERROR; } - - robot_->writeOnce(hw_commands_); + if (velocity_joint_interface_running_ || effort_interface_running_) { + robot_->writeOnce(hw_commands_); + } return hardware_interface::return_type::OK; } @@ -189,6 +190,7 @@ 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_->initializeTorqueInterface(); effort_interface_running_ = true; @@ -197,13 +199,14 @@ hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_sw effort_interface_running_ = false; } - if (!velocity_interface_running_ && velocity_interface_claimed_) { + if (!velocity_joint_interface_running_ && velocity_joint_interface_claimed_) { + hw_commands_.fill(0); robot_->stopRobot(); robot_->initializeJointVelocityInterface(); - velocity_interface_running_ = true; - } else if (velocity_interface_running_ && !velocity_interface_claimed_) { + velocity_joint_interface_running_ = true; + } else if (velocity_joint_interface_running_ && !velocity_joint_interface_claimed_) { robot_->stopRobot(); - velocity_interface_running_ = false; + velocity_joint_interface_running_ = false; } return hardware_interface::return_type::OK; } @@ -246,7 +249,7 @@ hardware_interface::return_type FrankaHardwareInterface::prepare_command_mode_sw int64_t num_stop_velocity_interfaces = std::count_if(stop_interfaces.begin(), stop_interfaces.end(), is_velocity_interface); if (num_stop_velocity_interfaces == kNumberOfJoints) { - velocity_interface_claimed_ = false; + velocity_joint_interface_claimed_ = false; } else if (num_stop_velocity_interfaces != 0) { RCLCPP_FATAL(this->getLogger(), "Expected %ld velocity interfaces to stop, but got %ld instead.", kNumberOfJoints, @@ -259,7 +262,7 @@ hardware_interface::return_type FrankaHardwareInterface::prepare_command_mode_sw int64_t num_start_velocity_interfaces = std::count_if(start_interfaces.begin(), start_interfaces.end(), is_velocity_interface); if (num_start_velocity_interfaces == kNumberOfJoints) { - velocity_interface_claimed_ = true; + velocity_joint_interface_claimed_ = true; } else if (num_start_velocity_interfaces != 0) { RCLCPP_FATAL(this->getLogger(), "Expected %ld velocity interfaces to start, but got %ld instead.", kNumberOfJoints, From 221229e1bb19ae881032456ce979c150c13cefd8 Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Fri, 13 Oct 2023 09:56:08 +0200 Subject: [PATCH 4/4] bump up version --- CHANGELOG.md | 9 +- franka_bringup/package.xml | 2 +- franka_description/package.xml | 2 +- franka_example_controllers/CMakeLists.txt | 2 +- franka_example_controllers/package.xml | 2 +- franka_gripper/package.xml | 2 +- .../include/franka_hardware/robot.hpp | 42 ++++----- franka_hardware/package.xml | 2 +- .../src/franka_hardware_interface.cpp | 89 ++++++++----------- .../test/franka_hardware_interface_test.cpp | 40 ++++----- franka_moveit_config/package.xml | 2 +- franka_msgs/package.xml | 2 +- franka_robot_state_broadcaster/package.xml | 2 +- integration_launch_testing/package.xml | 2 +- 14 files changed, 97 insertions(+), 103 deletions(-) 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/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_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt index c2d1e692..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( 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_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/robot.hpp b/franka_hardware/include/franka_hardware/robot.hpp index 6cf58414..94344c57 100644 --- a/franka_hardware/include/franka_hardware/robot.hpp +++ b/franka_hardware/include/franka_hardware/robot.hpp @@ -82,32 +82,12 @@ class Robot { virtual franka_hardware::Model* getModel(); /** - * @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(); - - /** - * This function will receive automatically propagate the received hardware active command + * 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& joint_hardware_command); - /** - * 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); - /** * Sets the impedance for each joint in the internal controller. * @@ -224,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_; 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 88954319..dccb8329 100644 --- a/franka_hardware/src/franka_hardware_interface.cpp +++ b/franka_hardware/src/franka_hardware_interface.cpp @@ -124,16 +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 && joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) { - RCLCPP_FATAL(getLogger(), "Joint '%s' has unexpected command interface '%s'. Expected '%s' ", + 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) { @@ -222,55 +223,43 @@ hardware_interface::return_type FrankaHardwareInterface::prepare_command_mode_sw return interface.find(hardware_interface::HW_IF_VELOCITY) != 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 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(); - 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); - } + throw std::invalid_argument(error_message); + }; - int64_t num_stop_velocity_interfaces = - std::count_if(stop_interfaces.begin(), stop_interfaces.end(), is_velocity_interface); - if (num_stop_velocity_interfaces == kNumberOfJoints) { - velocity_joint_interface_claimed_ = false; - } else if (num_stop_velocity_interfaces != 0) { - RCLCPP_FATAL(this->getLogger(), - "Expected %ld velocity interfaces to stop, but got %ld instead.", kNumberOfJoints, - num_stop_velocity_interfaces); - std::string error_string = "Invalid number of velocity interfaces to stop. Expected "; - error_string += std::to_string(kNumberOfJoints); - throw std::invalid_argument(error_string); - } + 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); - int64_t num_start_velocity_interfaces = - std::count_if(start_interfaces.begin(), start_interfaces.end(), is_velocity_interface); - if (num_start_velocity_interfaces == kNumberOfJoints) { - velocity_joint_interface_claimed_ = true; - } else if (num_start_velocity_interfaces != 0) { - RCLCPP_FATAL(this->getLogger(), - "Expected %ld velocity interfaces to start, but got %ld instead.", kNumberOfJoints, - num_start_velocity_interfaces); - std::string error_string = "Invalid number of velocity interfaces to start. Expected "; - error_string += std::to_string(kNumberOfJoints); - throw std::invalid_argument(error_string); - } + 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_); return hardware_interface::return_type::OK; } diff --git a/franka_hardware/test/franka_hardware_interface_test.cpp b/franka_hardware/test/franka_hardware_interface_test.cpp index ee7001cb..68769e50 100644 --- a/franka_hardware/test/franka_hardware_interface_test.cpp +++ b/franka_hardware/test/franka_hardware_interface_test.cpp @@ -82,7 +82,7 @@ class MockRobot : public franka_hardware::Robot { (override)); }; -auto createHardwareInfo(const std::string& command_interface) -> hardware_interface::HardwareInfo { +auto createHardwareInfo() -> hardware_interface::HardwareInfo { hardware_interface::HardwareInfo info; std::unordered_map hw_params; hw_params["robot_ip"] = "dummy_ip"; @@ -106,11 +106,9 @@ auto createHardwareInfo(const std::string& command_interface) -> hardware_interf joint.name = k_joint_name + std::to_string(i + 1); - if (command_interface == k_effort_controller) { - joint.command_interfaces.push_back(command_effort_interface); - } else if (command_interface == k_velocity_controller) { - joint.command_interfaces.push_back(command_velocity_interface); - } + joint.command_interfaces.push_back(command_effort_interface); + joint.command_interfaces.push_back(command_velocity_interface); + joint.state_interfaces = state_interfaces; info.joints.push_back(joint); @@ -131,7 +129,7 @@ void get_param_service_response( franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); - const auto hardware_info = createHardwareInfo(k_effort_controller); + const auto hardware_info = createHardwareInfo(); franka_hardware_interface.on_init(hardware_info); auto node = rclcpp::Node::make_shared("test_node"); @@ -158,7 +156,7 @@ void get_param_service_response( 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(command_interface); + const hardware_interface::HardwareInfo info = createHardwareInfo(); franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); auto return_type = franka_hardware_interface.on_init(info); @@ -200,7 +198,7 @@ TEST_P( franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot)); - const auto hardware_info = createHardwareInfo(command_interface); + const auto hardware_info = createHardwareInfo(); franka_hardware_interface.on_init(hardware_info); auto time = rclcpp::Time(0); auto duration = rclcpp::Duration(0, 0); @@ -245,7 +243,7 @@ TEST_P( EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address)); franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); - const auto hardware_info = createHardwareInfo(command_interface); + const auto hardware_info = createHardwareInfo(); franka_hardware_interface.on_init(hardware_info); auto time = rclcpp::Time(0); auto duration = rclcpp::Duration(0, 0); @@ -278,7 +276,7 @@ TEST_P( EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address)); franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); - const auto hardware_info = createHardwareInfo(command_interface); + const auto hardware_info = createHardwareInfo(); franka_hardware_interface.on_init(hardware_info); auto time = rclcpp::Time(0); auto duration = rclcpp::Duration(0, 0); @@ -300,7 +298,7 @@ TEST_P(FrankaHardwareInterfaceTest, auto mock_robot = std::make_shared(); franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); - const auto hardware_info = createHardwareInfo(command_interface); + const auto hardware_info = createHardwareInfo(); franka_hardware_interface.on_init(hardware_info); std::vector stop_interface; @@ -321,7 +319,7 @@ TEST_P( auto mock_robot = std::make_shared(); franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); - const auto hardware_info = createHardwareInfo(command_interface); + const auto hardware_info = createHardwareInfo(); franka_hardware_interface.on_init(hardware_info); std::vector stop_interface; @@ -342,7 +340,7 @@ TEST_P(FrankaHardwareInterfaceTest, auto mock_robot = std::make_shared(); franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); - const auto hardware_info = createHardwareInfo(command_interface); + const auto hardware_info = createHardwareInfo(); franka_hardware_interface.on_init(hardware_info); std::vector start_interface; @@ -365,16 +363,16 @@ TEST_P( auto mock_robot = std::make_shared(); franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); - const auto hardware_info = createHardwareInfo(command_interface); + 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 + "/" + command_interface); + 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), @@ -389,7 +387,7 @@ TEST_P(FrankaHardwareInterfaceTest, when_write_called_expect_ok) { franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot)); - const auto hardware_info = createHardwareInfo(command_interface); + const auto hardware_info = createHardwareInfo(); franka_hardware_interface.on_init(hardware_info); std::vector start_interface; @@ -454,7 +452,7 @@ TEST_P(FrankaHardwareInterfaceTest, franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); - const auto hardware_info = createHardwareInfo(command_interface); + const auto hardware_info = createHardwareInfo(); franka_hardware_interface.on_init(hardware_info); std::vector start_interface; @@ -481,7 +479,7 @@ TEST_P(FrankaHardwareInterfaceTest, franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); - const auto hardware_info = createHardwareInfo(command_interface); + const auto hardware_info = createHardwareInfo(); franka_hardware_interface.on_init(hardware_info); std::vector start_interface; 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