diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 14ec21e6..62b2e8f8 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -38,7 +38,10 @@ RUN apt-get update && \ ros-humble-ament-cmake \ ros-humble-ament-cmake-clang-format \ ros-humble-moveit \ + ros-humble-ros2-control \ + ros-humble-ros2-controllers \ ros-humble-joint-state-broadcaster \ + ros-humble-ament-cmake-clang-tidy \ libignition-gazebo6-dev \ ros-humble-joint-trajectory-controller \ libpoco-dev \ diff --git a/CHANGELOG.md b/CHANGELOG.md index 25234130..580027ac 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,14 @@ # Changelog +## 0.1.7 - 2023-11-10 + +Requires libfranka >= 0.12.1, required ROS 2 Humble + +* franka\_hardware: joint position command inteface supported +* franka\_hardware: controller initializer automatically acknowledges error, if arm is in reflex mode +* franka\_example\_controllers: joint position example controller provided +* franka\_example\_controllers: fix second start bug with the example controllers + ## 0.1.6 - 2023-11-03 Requires libfranka >= 0.12.1, required ROS 2 Humble diff --git a/franka_bringup/config/controllers.yaml b/franka_bringup/config/controllers.yaml index b7bb322a..e1d9384f 100644 --- a/franka_bringup/config/controllers.yaml +++ b/franka_bringup/config/controllers.yaml @@ -11,6 +11,9 @@ controller_manager: joint_velocity_example_controller: type: franka_example_controllers/JointVelocityExampleController + joint_position_example_controller: + type: franka_example_controllers/JointPositionExampleController + cartesian_velocity_example_controller: type: franka_example_controllers/CartesianVelocityExampleController diff --git a/franka_bringup/launch/joint_position_example_controller.launch.py b/franka_bringup/launch/joint_position_example_controller.launch.py new file mode 100644 index 00000000..80065e4a --- /dev/null +++ b/franka_bringup/launch/joint_position_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_position_example_controller'], + output='screen', + ), + ]) diff --git a/franka_bringup/package.xml b/franka_bringup/package.xml index a2d89eba..01e80aa7 100644 --- a/franka_bringup/package.xml +++ b/franka_bringup/package.xml @@ -2,7 +2,7 @@ franka_bringup - 0.1.6 + 0.1.7 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 ae2a096d..4d5e54fb 100644 --- a/franka_description/package.xml +++ b/franka_description/package.xml @@ -2,7 +2,7 @@ franka_description - 0.1.6 + 0.1.7 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 811d5a46..e0339d11 100644 --- a/franka_description/robots/panda_arm.ros2_control.xacro +++ b/franka_description/robots/panda_arm.ros2_control.xacro @@ -20,6 +20,7 @@ ${initial_position} + diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt index 8a1e3818..b4aaba5c 100644 --- a/franka_example_controllers/CMakeLists.txt +++ b/franka_example_controllers/CMakeLists.txt @@ -31,6 +31,7 @@ add_library( src/gravity_compensation_example_controller.cpp src/joint_impedance_example_controller.cpp src/joint_velocity_example_controller.cpp + src/joint_position_example_controller.cpp src/cartesian_velocity_example_controller.cpp src/elbow_example_controller.cpp src/model_example_controller.cpp diff --git a/franka_example_controllers/franka_example_controllers.xml b/franka_example_controllers/franka_example_controllers.xml index aa0c15a9..5f278271 100644 --- a/franka_example_controllers/franka_example_controllers.xml +++ b/franka_example_controllers/franka_example_controllers.xml @@ -17,6 +17,12 @@ The joint velocity example controller moves joint 4 and 5 in a periodic movement. + + + The joint position example controller moves in a periodic movement. + + diff --git a/franka_example_controllers/include/franka_example_controllers/elbow_example_controller.hpp b/franka_example_controllers/include/franka_example_controllers/elbow_example_controller.hpp index 2968fe2e..28bd8630 100644 --- a/franka_example_controllers/include/franka_example_controllers/elbow_example_controller.hpp +++ b/franka_example_controllers/include/franka_example_controllers/elbow_example_controller.hpp @@ -47,7 +47,7 @@ class ElbowExampleController : public controller_interface::ControllerInterface const bool k_elbow_activated_{true}; std::vector initial_cartesian_velocity_and_elbow; - bool first_pass_{true}; + bool initialization_flag_{true}; std::array initial_elbow_configuration_{0.0, 0.0}; double elapsed_time_{0.0}; const double traj_frequency_{0.001}; diff --git a/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.hpp b/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.hpp index a28eb00c..ca271df5 100644 --- a/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.hpp +++ b/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.hpp @@ -49,7 +49,7 @@ class JointImpedanceExampleController : public controller_interface::ControllerI Vector7d dq_filtered_; Vector7d k_gains_; Vector7d d_gains_; - rclcpp::Time start_time_; + double elapsed_time_{0.0}; void updateJointStates(); }; diff --git a/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.hpp b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.hpp new file mode 100644 index 00000000..6bd3dcf0 --- /dev/null +++ b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.hpp @@ -0,0 +1,55 @@ +// Copyright (c) 2023 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 +#include +#include "franka_semantic_components/franka_robot_state.hpp" + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +namespace franka_example_controllers { + +/** + * The joint position example controller moves in a periodic movement. + */ +class JointPositionExampleController : 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; + std::array initial_q_{0, 0, 0, 0, 0, 0, 0}; + const double trajectory_period{0.001}; + double elapsed_time_ = 0.0; + std::string arm_id{"panda"}; + + bool initialization_flag_{true}; + rclcpp::Time start_time_; +}; + +} // 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 44c0f9e6..19f8f0fc 100644 --- a/franka_example_controllers/package.xml +++ b/franka_example_controllers/package.xml @@ -2,7 +2,7 @@ franka_example_controllers - 0.1.6 + 0.1.7 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/cartesian_velocity_example_controller.cpp b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp index d425b331..1c956561 100644 --- a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp @@ -96,7 +96,7 @@ CallbackReturn CartesianVelocityExampleController::on_configure( CallbackReturn CartesianVelocityExampleController::on_activate( const rclcpp_lifecycle::State& /*previous_state*/) { franka_cartesian_velocity_->assign_loaned_command_interfaces(command_interfaces_); - + elapsed_time_ = rclcpp::Duration(0, 0); return CallbackReturn::SUCCESS; } diff --git a/franka_example_controllers/src/elbow_example_controller.cpp b/franka_example_controllers/src/elbow_example_controller.cpp index 15089c6f..d894729f 100644 --- a/franka_example_controllers/src/elbow_example_controller.cpp +++ b/franka_example_controllers/src/elbow_example_controller.cpp @@ -42,7 +42,7 @@ controller_interface::InterfaceConfiguration ElbowExampleController::state_inter controller_interface::return_type ElbowExampleController::update( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - if (first_pass_) { + if (initialization_flag_) { // Get initial elbow configuration values if (!franka_cartesian_velocity_->getCommandedElbowConfiguration(initial_elbow_configuration_)) { RCLCPP_FATAL(get_node()->get_logger(), @@ -51,11 +51,11 @@ controller_interface::return_type ElbowExampleController::update( return controller_interface::return_type::ERROR; } - first_pass_ = false; + initialization_flag_ = false; } elapsed_time_ = elapsed_time_ + traj_frequency_; - double angle = M_PI / 10.0 * (1.0 - std::cos(M_PI / 5.0 * elapsed_time_)); + double angle = M_PI / 15.0 * (1.0 - std::cos(M_PI / 5.0 * elapsed_time_)); std::array cartesian_velocity_command = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; std::array elbow_command = { @@ -103,7 +103,8 @@ CallbackReturn ElbowExampleController::on_configure( CallbackReturn ElbowExampleController::on_activate( const rclcpp_lifecycle::State& /*previous_state*/) { franka_cartesian_velocity_->assign_loaned_command_interfaces(command_interfaces_); - + initialization_flag_ = true; + elapsed_time_ = 0.0; return CallbackReturn::SUCCESS; } diff --git a/franka_example_controllers/src/joint_impedance_example_controller.cpp b/franka_example_controllers/src/joint_impedance_example_controller.cpp index 89468d34..6030c340 100644 --- a/franka_example_controllers/src/joint_impedance_example_controller.cpp +++ b/franka_example_controllers/src/joint_impedance_example_controller.cpp @@ -47,11 +47,12 @@ JointImpedanceExampleController::state_interface_configuration() const { controller_interface::return_type JointImpedanceExampleController::update( const rclcpp::Time& /*time*/, - const rclcpp::Duration& /*period*/) { + const rclcpp::Duration& period) { updateJointStates(); Vector7d q_goal = initial_q_; - auto time = this->get_node()->now() - start_time_; - double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time.seconds())); + elapsed_time_ = elapsed_time_ + period.seconds(); + + double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * elapsed_time_)); q_goal(3) += delta_angle; q_goal(4) += delta_angle; @@ -111,8 +112,10 @@ CallbackReturn JointImpedanceExampleController::on_configure( CallbackReturn JointImpedanceExampleController::on_activate( const rclcpp_lifecycle::State& /*previous_state*/) { updateJointStates(); + dq_filtered_.setZero(); initial_q_ = q_; - start_time_ = this->get_node()->now(); + elapsed_time_ = 0.0; + return CallbackReturn::SUCCESS; } diff --git a/franka_example_controllers/src/joint_position_example_controller.cpp b/franka_example_controllers/src/joint_position_example_controller.cpp new file mode 100644 index 00000000..2b88f32f --- /dev/null +++ b/franka_example_controllers/src/joint_position_example_controller.cpp @@ -0,0 +1,95 @@ +// Copyright (c) 2023 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 +JointPositionExampleController::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) + "/position"); + } + return config; +} + +controller_interface::InterfaceConfiguration +JointPositionExampleController::state_interface_configuration() const { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::return_type JointPositionExampleController::update( + const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) { + if (initialization_flag_) { + for (size_t i = 0; i < 7; ++i) { + initial_q_.at(i) = command_interfaces_[i].get_value(); + } + initialization_flag_ = false; + } + + elapsed_time_ = elapsed_time_ + trajectory_period; + double delta_angle = M_PI / 16 * (1 - std::cos(M_PI / 5.0 * elapsed_time_)) * 0.2; + + for (size_t i = 0; i < 7; ++i) { + if (i == 4) { + command_interfaces_[i].set_value(initial_q_.at(i) - delta_angle); + } else { + command_interfaces_[i].set_value(initial_q_.at(i) + delta_angle); + } + } + + return controller_interface::return_type::OK; +} + +CallbackReturn JointPositionExampleController::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 JointPositionExampleController::on_configure( + const rclcpp_lifecycle::State& /*previous_state*/) { + arm_id_ = get_node()->get_parameter("arm_id").as_string(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn JointPositionExampleController::on_activate( + const rclcpp_lifecycle::State& /*previous_state*/) { + initialization_flag_ = true; + elapsed_time_ = 0.0; + return CallbackReturn::SUCCESS; +} + +} // namespace franka_example_controllers +#include "pluginlib/class_list_macros.hpp" +// NOLINTNEXTLINE +PLUGINLIB_EXPORT_CLASS(franka_example_controllers::JointPositionExampleController, + controller_interface::ControllerInterface) \ 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 index ac146339..b85e953b 100644 --- a/franka_example_controllers/src/joint_velocity_example_controller.cpp +++ b/franka_example_controllers/src/joint_velocity_example_controller.cpp @@ -110,6 +110,7 @@ CallbackReturn JointVelocityExampleController::on_configure( CallbackReturn JointVelocityExampleController::on_activate( const rclcpp_lifecycle::State& /*previous_state*/) { + elapsed_time_ = rclcpp::Duration(0, 0); return CallbackReturn::SUCCESS; } diff --git a/franka_gripper/package.xml b/franka_gripper/package.xml index fccf7762..de84f1bc 100644 --- a/franka_gripper/package.xml +++ b/franka_gripper/package.xml @@ -2,7 +2,7 @@ franka_gripper - 0.1.6 + 0.1.7 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 99c7632c..e96983e1 100644 --- a/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp +++ b/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp @@ -69,14 +69,22 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface { bool& claim_flag; }; + void initializePositionCommands(const franka::RobotState& robot_state); + + // Initialize joint position commands in the first pass + bool first_elbow_update_{true}; + bool first_position_update_{true}; + std::shared_ptr robot_; std::shared_ptr node_; std::shared_ptr executor_; - // Initialize joint position commands in the first pass - bool first_pass_{true}; - - std::array hw_commands_{0, 0, 0, 0, 0, 0, 0}; + // Torque joint commands for the effort command interface + std::array hw_effort_commands_{0, 0, 0, 0, 0, 0, 0}; + // Position joint commands for the position command interface + std::array hw_position_commands_{0, 0, 0, 0, 0, 0, 0}; + // Velocity joint commands for the position command interface + std::array hw_velocity_commands_{0, 0, 0, 0, 0, 0, 0}; // Robot joint states std::array hw_positions_{0, 0, 0, 0, 0, 0, 0}; @@ -121,6 +129,9 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface { bool velocity_joint_interface_claimed_ = false; bool velocity_joint_interface_running_ = false; + bool position_joint_interface_claimed_ = false; + bool position_joint_interface_running_ = false; + bool velocity_cartesian_interface_claimed_ = false; bool velocity_cartesian_interface_running_ = false; diff --git a/franka_hardware/include/franka_hardware/robot.hpp b/franka_hardware/include/franka_hardware/robot.hpp index 9bd45d81..fd3d67e7 100644 --- a/franka_hardware/include/franka_hardware/robot.hpp +++ b/franka_hardware/include/franka_hardware/robot.hpp @@ -66,10 +66,13 @@ class Robot { /// Starts the active control for joint velocity control virtual void initializeJointVelocityInterface(); - /// Starts the active control for joint velocity control + /// Starts the active control for joint position control + virtual void initializeJointPositionInterface(); + + /// Starts the active control for cartesian velocity control virtual void initializeCartesianVelocityInterface(); - /// stops the read continous communication with the connected robot + /// stops the read continuous communication with the connected robot virtual void stopRobot(); /** @@ -231,17 +234,22 @@ class Robot { virtual franka::RobotState readOnceActiveControl(); /** - * The robot will use these torques until a different set of torques are commanded. + * The robot will use set of 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. + * The robot will use set of velocities until a different set of velocities are commanded. * @param[in] joint_velocities joint velocity command. */ virtual void writeOnceJointVelocities(const std::array& joint_velocities); + /** + * The robot will use set of positions until a different set of position are commanded. + * @param[in] joint_position joint position command. + */ + virtual void writeOnceJointPositions(const std::array& positions); /** * @brief Checks if control loop is activated for active control. * @@ -262,12 +270,18 @@ class Robot { bool effort_interface_active_{false}; bool joint_velocity_interface_active_{false}; + bool joint_position_interface_active_{false}; bool cartesian_velocity_interface_active_{false}; bool velocity_command_rate_limit_active_{false}; + bool cartesian_velocity_command_rate_limit_active_{false}; bool cartesian_velocity_low_pass_filter_active{false}; - double low_pass_filter_cut_off_freq{1000.0}; + + bool joint_position_command_rate_limit_active_{false}; + bool joint_position_command_low_pass_filter_active_{false}; + + double low_pass_filter_cut_off_freq{100.0}; franka::RobotState current_state_; }; diff --git a/franka_hardware/package.xml b/franka_hardware/package.xml index 3074cb9d..ab0a9c59 100644 --- a/franka_hardware/package.xml +++ b/franka_hardware/package.xml @@ -2,7 +2,7 @@ franka_hardware - 0.1.6 + 0.1.7 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 861324e0..27c2638a 100644 --- a/franka_hardware/src/franka_hardware_interface.cpp +++ b/franka_hardware/src/franka_hardware_interface.cpp @@ -42,6 +42,7 @@ FrankaHardwareInterface::FrankaHardwareInterface() : command_interfaces_info_({ {hardware_interface::HW_IF_EFFORT, kNumberOfJoints, effort_interface_claimed_}, {hardware_interface::HW_IF_VELOCITY, kNumberOfJoints, velocity_joint_interface_claimed_}, + {hardware_interface::HW_IF_POSITION, kNumberOfJoints, position_joint_interface_claimed_}, {k_HW_IF_ELBOW_COMMAND, hw_elbow_command_names_.size(), elbow_command_interface_claimed_}, {k_HW_IF_CARTESIAN_VELOCITY, hw_cartesian_velocities_.size(), velocity_cartesian_interface_claimed_}, @@ -74,9 +75,11 @@ std::vector FrankaHardwareInterface::export_command_interfaces command_interfaces.reserve(info_.joints.size()); 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))); + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_effort_commands_.at(i))); command_interfaces.emplace_back(CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_.at(i))); + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocity_commands_.at(i))); + command_interfaces.emplace_back(CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_position_commands_.at(i))); } // cartesian velocity command interface 6 in order: dx, dy, dz, wx, wy, wz @@ -97,7 +100,6 @@ std::vector FrankaHardwareInterface::export_command_interfaces CallbackReturn FrankaHardwareInterface::on_activate( const rclcpp_lifecycle::State& /*previous_state*/) { - hw_commands_.fill(0); read(rclcpp::Time(0), rclcpp::Duration(0, 0)); // makes sure that the robot state is properly initialized. RCLCPP_INFO(getLogger(), "Started"); @@ -112,6 +114,24 @@ CallbackReturn FrankaHardwareInterface::on_deactivate( return CallbackReturn::SUCCESS; } +template +void initializeCommand(bool& first_update, + const bool& interface_running, + CommandType& hw_command, + const CommandType& new_command) { + if (first_update && interface_running) { + hw_command = new_command; + first_update = false; + } +} + +void FrankaHardwareInterface::initializePositionCommands(const franka::RobotState& robot_state) { + initializeCommand(first_elbow_update_, elbow_command_interface_running_, hw_elbow_command_, + robot_state.elbow_c); + initializeCommand(first_position_update_, position_joint_interface_running_, + hw_position_commands_, robot_state.q_d); +} + hardware_interface::return_type FrankaHardwareInterface::read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { if (hw_franka_model_ptr_ == nullptr) { @@ -120,11 +140,7 @@ hardware_interface::return_type FrankaHardwareInterface::read(const rclcpp::Time hw_franka_robot_state_ = robot_->readOnce(); - if (first_pass_ && elbow_command_interface_running_) { - // elbow command should be initialized with the first elbow_c read from the robot - hw_elbow_command_ = hw_franka_robot_state_.elbow_c; - first_pass_ = false; - } + initializePositionCommands(hw_franka_robot_state_); hw_positions_ = hw_franka_robot_state_.q; hw_velocities_ = hw_franka_robot_state_.dq; @@ -133,23 +149,28 @@ hardware_interface::return_type FrankaHardwareInterface::read(const rclcpp::Time return hardware_interface::return_type::OK; } +template +bool hasInfinite(const CommandType& commands) { + return std::any_of(commands.begin(), commands.end(), + [](double command) { return !std::isfinite(command); }); +} + hardware_interface::return_type FrankaHardwareInterface::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - if (std::any_of(hw_commands_.begin(), hw_commands_.end(), - [](double hw_command) { return !std::isfinite(hw_command); })) { - return hardware_interface::return_type::ERROR; - } - - if (std::any_of( - hw_cartesian_velocities_.begin(), hw_cartesian_velocities_.end(), - [](double hw_cartesian_velocity) { return !std::isfinite(hw_cartesian_velocity); })) { + if (hasInfinite(hw_position_commands_) || hasInfinite(hw_effort_commands_) || + hasInfinite(hw_velocity_commands_) || hasInfinite(hw_cartesian_velocities_) || + hasInfinite(hw_elbow_command_)) { return hardware_interface::return_type::ERROR; } - if (velocity_joint_interface_running_ || effort_interface_running_) { - robot_->writeOnce(hw_commands_); + if (velocity_joint_interface_running_) { + robot_->writeOnce(hw_velocity_commands_); + } else if (effort_interface_running_) { + robot_->writeOnce(hw_effort_commands_); + } else if (position_joint_interface_running_ && !first_position_update_) { + robot_->writeOnce(hw_position_commands_); } else if (velocity_cartesian_interface_running_ && elbow_command_interface_running_ && - !first_pass_) { + !first_elbow_update_) { // Wait until the first read pass after robot controller is activated to write the elbow // command to the robot robot_->writeOnce(hw_cartesian_velocities_, hw_elbow_command_); @@ -171,13 +192,14 @@ CallbackReturn FrankaHardwareInterface::on_init(const hardware_interface::Hardwa } for (const auto& joint : info_.joints) { - if (joint.command_interfaces.size() != 2) { - RCLCPP_FATAL(getLogger(), "Joint '%s' has %zu command interfaces found. 2 expected.", + if (joint.command_interfaces.size() != 3) { + RCLCPP_FATAL(getLogger(), "Joint '%s' has %zu command interfaces found. 3 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) { + joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY && + joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { 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(), @@ -238,7 +260,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); + hw_effort_commands_.fill(0); robot_->stopRobot(); robot_->initializeTorqueInterface(); effort_interface_running_ = true; @@ -248,7 +270,7 @@ hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_sw } if (!velocity_joint_interface_running_ && velocity_joint_interface_claimed_) { - hw_commands_.fill(0); + hw_velocity_commands_.fill(0); robot_->stopRobot(); robot_->initializeJointVelocityInterface(); velocity_joint_interface_running_ = true; @@ -257,12 +279,23 @@ hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_sw velocity_joint_interface_running_ = false; } + if (!position_joint_interface_running_ && position_joint_interface_claimed_) { + robot_->stopRobot(); + robot_->initializeJointPositionInterface(); + position_joint_interface_running_ = true; + first_position_update_ = true; + } else if (position_joint_interface_running_ && !position_joint_interface_claimed_) { + robot_->stopRobot(); + position_joint_interface_running_ = false; + } + if (!velocity_cartesian_interface_running_ && velocity_cartesian_interface_claimed_) { hw_cartesian_velocities_.fill(0); robot_->stopRobot(); robot_->initializeCartesianVelocityInterface(); if (!elbow_command_interface_running_ && elbow_command_interface_claimed_) { elbow_command_interface_running_ = true; + first_elbow_update_ = true; } velocity_cartesian_interface_running_ = true; } else if (velocity_cartesian_interface_running_ && !velocity_cartesian_interface_claimed_) { diff --git a/franka_hardware/src/robot.cpp b/franka_hardware/src/robot.cpp index 24432054..5b2ca57b 100644 --- a/franka_hardware/src/robot.cpp +++ b/franka_hardware/src/robot.cpp @@ -48,16 +48,17 @@ franka::RobotState Robot::readOnce() { std::lock_guard lock(control_mutex_); if (!isControlLoopActive()) { current_state_ = robot_->readOnce(); - return current_state_; } else { - return readOnceActiveControl(); + current_state_ = readOnceActiveControl(); } + return current_state_; } void Robot::stopRobot() { if (isControlLoopActive()) { effort_interface_active_ = false; joint_velocity_interface_active_ = false; + joint_position_interface_active_ = false; cartesian_velocity_interface_active_ = false; active_control_.reset(); } @@ -68,6 +69,8 @@ void Robot::writeOnce(const std::array& joint_commands) { writeOnceEfforts(joint_commands); } else if (joint_velocity_interface_active_) { writeOnceJointVelocities(joint_commands); + } else if (joint_position_interface_active_) { + writeOnceJointPositions(joint_commands); } } @@ -99,6 +102,30 @@ void Robot::writeOnceJointVelocities(const std::array& velocities) { active_control_->writeOnce(velocity_command); } +void Robot::writeOnceJointPositions(const std::array& positions) { + std::lock_guard lock(control_mutex_); + + auto position_command = franka::JointPositions(positions); + + // If you are experiencing issues with robot error. You can try activating the rate limiter. + // Rate limiter is default deactivated. + if (joint_position_command_low_pass_filter_active_) { + for (size_t i = 0; i < 7; i++) { + position_command.q.at(i) = + franka::lowpassFilter(franka::kDeltaT, position_command.q.at(i), current_state_.q_d.at(i), + low_pass_filter_cut_off_freq); + } + } + if (joint_position_command_rate_limit_active_) { + position_command.q = + franka::limitRate(franka::computeUpperLimitsJointVelocity(current_state_.q_d), + franka::computeLowerLimitsJointVelocity(current_state_.q_d), + franka::kMaxJointAcceleration, franka::kMaxJointJerk, position_command.q, + current_state_.q_d, current_state_.dq_d, current_state_.ddq_d); + } + active_control_->writeOnce(position_command); +} + void Robot::preProcessCartesianVelocities(franka::CartesianVelocities& velocity_command) { if (cartesian_velocity_low_pass_filter_active) { for (size_t i = 0; i < 6; i++) { @@ -155,8 +182,8 @@ void Robot::writeOnce(const std::array& cartesian_velocities, franka::RobotState Robot::readOnceActiveControl() { // When controller is active use active control to read the robot state - const auto [current_state_, _] = active_control_->readOnce(); - return current_state_; + const auto [current_state, _] = active_control_->readOnce(); + return current_state; } franka_hardware::Model* Robot::getModel() { @@ -164,25 +191,56 @@ franka_hardware::Model* Robot::getModel() { } void Robot::initializeTorqueInterface() { - active_control_ = robot_->startTorqueControl(); + try { + active_control_ = robot_->startTorqueControl(); + } catch (const franka::ControlException& e) { + robot_->automaticErrorRecovery(); + active_control_ = robot_->startTorqueControl(); + } effort_interface_active_ = true; } void Robot::initializeJointVelocityInterface() { - active_control_ = robot_->startJointVelocityControl( - research_interface::robot::Move::ControllerMode::kJointImpedance); + try { + active_control_ = robot_->startJointVelocityControl( + research_interface::robot::Move::ControllerMode::kJointImpedance); + } catch (const franka::ControlException& e) { + robot_->automaticErrorRecovery(); + active_control_ = robot_->startJointVelocityControl( + research_interface::robot::Move::ControllerMode::kJointImpedance); + } + joint_velocity_interface_active_ = true; } +void Robot::initializeJointPositionInterface() { + try { + active_control_ = robot_->startJointPositionControl( + research_interface::robot::Move::ControllerMode::kJointImpedance); + } catch (const franka::ControlException& e) { + robot_->automaticErrorRecovery(); + active_control_ = robot_->startJointPositionControl( + research_interface::robot::Move::ControllerMode::kJointImpedance); + } + + joint_position_interface_active_ = true; +} + void Robot::initializeCartesianVelocityInterface() { - active_control_ = robot_->startCartesianVelocityControl( - research_interface::robot::Move::ControllerMode::kJointImpedance); + try { + active_control_ = robot_->startCartesianVelocityControl( + research_interface::robot::Move::ControllerMode::kJointImpedance); + } catch (const franka::ControlException& e) { + robot_->automaticErrorRecovery(); + active_control_ = robot_->startCartesianVelocityControl( + research_interface::robot::Move::ControllerMode::kJointImpedance); + } cartesian_velocity_interface_active_ = true; } bool Robot::isControlLoopActive() { - return joint_velocity_interface_active_ || effort_interface_active_ || - cartesian_velocity_interface_active_; + return joint_position_interface_active_ || joint_velocity_interface_active_ || + effort_interface_active_ || cartesian_velocity_interface_active_; } void Robot::setJointStiffness(const franka_msgs::srv::SetJointStiffness::Request::SharedPtr& req) { diff --git a/franka_hardware/test/franka_hardware_cartesian_command_interface_test.cpp b/franka_hardware/test/franka_hardware_cartesian_command_interface_test.cpp index c2fc7b59..4ac9fe32 100644 --- a/franka_hardware/test/franka_hardware_cartesian_command_interface_test.cpp +++ b/franka_hardware/test/franka_hardware_cartesian_command_interface_test.cpp @@ -119,7 +119,7 @@ TEST_P( } TEST_F(FrankaCartesianCommandInterfaceTest, - given_read_is_not_called_when_write_is_called_expec_robot_writeOnce_is_not_called) { + given_read_is_not_called_when_write_is_called_expect_robot_writeOnce_is_not_called) { auto mock_robot = std::make_shared(); EXPECT_CALL(*mock_robot, stopRobot()); @@ -193,6 +193,49 @@ TEST_F(FrankaCartesianCommandInterfaceTest, ASSERT_EQ(franka_hardware_interface.write(time, duration), hardware_interface::return_type::OK); } +TEST_F( + FrankaCartesianCommandInterfaceTest, + given_cartesian_velocity_and_elbow_set_and_elbow_has_infinite_values_when_write_called_expect_error) { + auto mock_robot = std::make_shared(); + franka::RobotState robot_state; + robot_state.elbow_c = std::array{std::numeric_limits::infinity()}; + + EXPECT_CALL(*mock_robot, stopRobot()); + EXPECT_CALL(*mock_robot, initializeCartesianVelocityInterface()); + + EXPECT_CALL(*mock_robot, readOnce()).WillOnce(testing::Return(robot_state)); + EXPECT_CALL(*mock_robot, writeOnce(_, _)).Times(0); + EXPECT_CALL(*mock_robot, writeOnce(std::array{})).Times(0); + + franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot)); + + std::vector start_interface; + + for (size_t i = 0; i < k_hw_cartesian_velocities_names.size(); i++) { + const std::string name = k_hw_cartesian_velocities_names[i]; + start_interface.push_back(name + "/" + k_cartesian_velocity_command_interface_name); + } + for (size_t i = 0; i < k_hw_elbow_command_names.size(); i++) { + const std::string name = k_hw_elbow_command_names[i]; + start_interface.push_back(name + "/" + k_elbow_command_interface_name); + } + + std::vector stop_interface = {}; + + ASSERT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), + hardware_interface::return_type::OK); + // can call write only after performing command mode switch + ASSERT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface), + hardware_interface::return_type::OK); + + const auto time = rclcpp::Time(0, 0); + const auto duration = rclcpp::Duration(0, 0); + + ASSERT_EQ(franka_hardware_interface.read(time, duration), hardware_interface::return_type::OK); + ASSERT_EQ(franka_hardware_interface.write(time, duration), + hardware_interface::return_type::ERROR); +} + TEST_F(FrankaCartesianCommandInterfaceTest, given_cartesian_velocity_is_claimed_when_perform_mode_switch_is_called_expect_success) { auto mock_robot = std::make_shared(); diff --git a/franka_hardware/test/franka_hardware_interface_test.cpp b/franka_hardware/test/franka_hardware_interface_test.cpp index 2525efc4..dbaec22c 100644 --- a/franka_hardware/test/franka_hardware_interface_test.cpp +++ b/franka_hardware/test/franka_hardware_interface_test.cpp @@ -54,7 +54,8 @@ auto createHardwareInfo() -> hardware_interface::HardwareInfo { info.hardware_parameters = hw_params; hardware_interface::InterfaceInfo command_effort_interface, command_velocity_interface, - effort_state_interface, position_state_interface, velocity_state_interface; + command_position_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; @@ -65,6 +66,7 @@ auto createHardwareInfo() -> hardware_interface::HardwareInfo { command_effort_interface.name = k_effort_controller; command_velocity_interface.name = k_velocity_controller; + command_position_interface.name = k_position_controller; for (auto i = 0U; i < k_number_of_joints; i++) { hardware_interface::ComponentInfo joint; @@ -73,6 +75,7 @@ auto createHardwareInfo() -> hardware_interface::HardwareInfo { joint.command_interfaces.push_back(command_effort_interface); joint.command_interfaces.push_back(command_velocity_interface); + joint.command_interfaces.push_back(command_position_interface); joint.state_interfaces = state_interfaces; @@ -124,7 +127,7 @@ TEST_F(FrankaHardwareInterfaceTest, when_on_init_called_expect_success) { franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); auto return_type = franka_hardware_interface.on_init(info); - EXPECT_EQ(return_type, + ASSERT_EQ(return_type, rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); } @@ -143,7 +146,7 @@ TEST_F(FrankaHardwareInterfaceTest, auto time = rclcpp::Time(0, 0); auto duration = rclcpp::Duration(0, 0); auto return_type = franka_hardware_interface.read(time, duration); - EXPECT_EQ(return_type, hardware_interface::return_type::OK); + ASSERT_EQ(return_type, hardware_interface::return_type::OK); } TEST_F( @@ -166,7 +169,7 @@ TEST_F( auto time = rclcpp::Time(0); auto duration = rclcpp::Duration(0, 0); auto return_type = franka_hardware_interface.read(time, duration); - EXPECT_EQ(return_type, hardware_interface::return_type::OK); + ASSERT_EQ(return_type, hardware_interface::return_type::OK); auto states = franka_hardware_interface.export_state_interfaces(); size_t joint_index = 0; @@ -177,16 +180,16 @@ TEST_F( } const std::string joint_name = k_joint_name + std::to_string(joint_index); if (i % 3 == 0) { - EXPECT_EQ(states[i].get_name(), joint_name + "/" + k_position_controller); + ASSERT_EQ(states[i].get_name(), joint_name + "/" + k_position_controller); } else if (i % 3 == 1) { - EXPECT_EQ(states[i].get_name(), joint_name + "/" + k_velocity_controller); + ASSERT_EQ(states[i].get_name(), joint_name + "/" + k_velocity_controller); } else { - EXPECT_EQ(states[i].get_name(), joint_name + "/" + k_effort_controller); + ASSERT_EQ(states[i].get_name(), joint_name + "/" + k_effort_controller); } - EXPECT_EQ(states[i].get_value(), 0.0); + ASSERT_EQ(states[i].get_value(), 0.0); } - EXPECT_EQ(states.size(), state_interface_size); + ASSERT_EQ(states.size(), state_interface_size); } TEST_F( @@ -209,9 +212,9 @@ TEST_F( auto time = rclcpp::Time(0); auto duration = rclcpp::Duration(0, 0); auto return_type = franka_hardware_interface.read(time, duration); - EXPECT_EQ(return_type, hardware_interface::return_type::OK); + ASSERT_EQ(return_type, hardware_interface::return_type::OK); auto states = franka_hardware_interface.export_state_interfaces(); - EXPECT_EQ(states[state_interface_size - 1].get_name(), + ASSERT_EQ(states[state_interface_size - 1].get_name(), "panda/robot_model"); // Last state interface is the robot model state EXPECT_NEAR(states[state_interface_size - 1].get_value(), *reinterpret_cast(&model_address), @@ -240,9 +243,9 @@ TEST_F( auto time = rclcpp::Time(0); auto duration = rclcpp::Duration(0, 0); auto return_type = franka_hardware_interface.read(time, duration); - EXPECT_EQ(return_type, hardware_interface::return_type::OK); + ASSERT_EQ(return_type, hardware_interface::return_type::OK); auto states = franka_hardware_interface.export_state_interfaces(); - EXPECT_EQ(states[state_interface_size - 2].get_name(), + ASSERT_EQ(states[state_interface_size - 2].get_name(), "panda/robot_state"); // Last state interface is the robot model state EXPECT_NEAR(states[state_interface_size - 2].get_value(), *reinterpret_cast(&robot_state_address), @@ -266,7 +269,7 @@ TEST_P(FrankaHardwareInterfaceTest, 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), + ASSERT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), hardware_interface::return_type::OK); } @@ -310,7 +313,7 @@ TEST_P(FrankaHardwareInterfaceTest, std::vector stop_interface = {}; - EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), + ASSERT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), hardware_interface::return_type::OK); } @@ -357,16 +360,85 @@ TEST_P(FrankaHardwareInterfaceTest, when_write_called_expect_ok) { std::vector stop_interface = {}; - EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), + ASSERT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), hardware_interface::return_type::OK); // can call write only after performing command mode switch - EXPECT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface), + ASSERT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface), hardware_interface::return_type::OK); const auto time = rclcpp::Time(0, 0); const auto duration = rclcpp::Duration(0, 0); - EXPECT_EQ(franka_hardware_interface.write(time, duration), hardware_interface::return_type::OK); + if (command_interface == k_position_controller) { + ASSERT_EQ(franka_hardware_interface.read(time, duration), hardware_interface::return_type::OK); + } + ASSERT_EQ(franka_hardware_interface.write(time, duration), hardware_interface::return_type::OK); +} + +TEST_F(FrankaHardwareInterfaceTest, when_write_called_with_inifite_command_expect_error) { + auto mock_robot = std::make_shared(); + franka::RobotState robot_state; + robot_state.q_d = std::array{std::numeric_limits::infinity()}; + + EXPECT_CALL(*mock_robot, readOnce()).WillOnce(testing::Return(robot_state)); + EXPECT_CALL(*mock_robot, writeOnce(std::array{})).Times(0); + + franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot)); + + const auto hardware_info = createHardwareInfo(); + 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_position_controller); + } + + std::vector stop_interface = {}; + + ASSERT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), + hardware_interface::return_type::OK); + // can call write only after performing command mode switch + ASSERT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface), + hardware_interface::return_type::OK); + + const auto time = rclcpp::Time(0, 0); + const auto duration = rclcpp::Duration(0, 0); + + ASSERT_EQ(franka_hardware_interface.read(time, duration), hardware_interface::return_type::OK); + ASSERT_EQ(franka_hardware_interface.write(time, duration), + hardware_interface::return_type::ERROR); +} + +TEST_F( + FrankaHardwareInterfaceTest, + given_position_joint_command_interface_initialized_if_write_called_without_read_expect_write_once_not_to_called) { + auto mock_robot = std::make_shared(); + EXPECT_CALL(*mock_robot, writeOnce(std::array{})).Times(0); + + franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot)); + + const auto hardware_info = createHardwareInfo(); + 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_position_controller); + } + + std::vector stop_interface = {}; + + ASSERT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), + hardware_interface::return_type::OK); + // can call write only after performing command mode switch + ASSERT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface), + hardware_interface::return_type::OK); + + const auto time = rclcpp::Time(0, 0); + const auto duration = rclcpp::Duration(0, 0); + + ASSERT_EQ(franka_hardware_interface.write(time, duration), hardware_interface::return_type::OK); } TEST_F(FrankaHardwareInterfaceTest, when_on_activate_called_expect_success) { @@ -381,7 +453,7 @@ TEST_F(FrankaHardwareInterfaceTest, when_on_activate_called_expect_success) { franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); - EXPECT_EQ(franka_hardware_interface.on_activate(rclcpp_lifecycle::State()), + ASSERT_EQ(franka_hardware_interface.on_activate(rclcpp_lifecycle::State()), rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); } @@ -393,7 +465,7 @@ TEST_F(FrankaHardwareInterfaceTest, when_on_deactivate_called_expect_success) { franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); - EXPECT_EQ(franka_hardware_interface.on_deactivate(rclcpp_lifecycle::State()), + ASSERT_EQ(franka_hardware_interface.on_deactivate(rclcpp_lifecycle::State()), rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); } @@ -407,6 +479,8 @@ TEST_P(FrankaHardwareInterfaceTest, EXPECT_CALL(*mock_robot, initializeTorqueInterface()); } else if (command_interface == k_velocity_controller) { EXPECT_CALL(*mock_robot, initializeJointVelocityInterface()); + } else if (command_interface == k_position_controller) { + EXPECT_CALL(*mock_robot, initializeJointPositionInterface()); } franka_hardware::FrankaHardwareInterface franka_hardware_interface(mock_robot); @@ -422,10 +496,10 @@ TEST_P(FrankaHardwareInterfaceTest, std::vector stop_interface = {}; - EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), + ASSERT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), hardware_interface::return_type::OK); - EXPECT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface), + ASSERT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface), hardware_interface::return_type::OK); } @@ -449,10 +523,10 @@ TEST_P(FrankaHardwareInterfaceTest, std::vector stop_interface = {}; - EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), + ASSERT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), hardware_interface::return_type::OK); - EXPECT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface), + ASSERT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface), hardware_interface::return_type::OK); for (size_t i = 0; i < hardware_info.joints.size(); i++) { @@ -462,10 +536,10 @@ TEST_P(FrankaHardwareInterfaceTest, start_interface.clear(); - EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), + ASSERT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), hardware_interface::return_type::OK); - EXPECT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface), + ASSERT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface), hardware_interface::return_type::OK); } @@ -691,4 +765,6 @@ int main(int argc, char** argv) { INSTANTIATE_TEST_SUITE_P(FrankaHardwareTests, FrankaHardwareInterfaceTest, - ::testing::Values(k_velocity_controller, k_effort_controller)); \ No newline at end of file + ::testing::Values(k_velocity_controller, + k_effort_controller, + k_position_controller)); \ No newline at end of file diff --git a/franka_hardware/test/test_utils.hpp b/franka_hardware/test/test_utils.hpp index aaea5c8a..243327b8 100644 --- a/franka_hardware/test/test_utils.hpp +++ b/franka_hardware/test/test_utils.hpp @@ -24,6 +24,7 @@ class MockModel : public franka_hardware::Model {}; class MockRobot : public franka_hardware::Robot { public: + MOCK_METHOD(void, initializeJointPositionInterface, (), (override)); MOCK_METHOD(void, initializeCartesianVelocityInterface, (), (override)); MOCK_METHOD(void, initializeTorqueInterface, (), (override)); MOCK_METHOD(void, initializeJointVelocityInterface, (), (override)); diff --git a/franka_moveit_config/package.xml b/franka_moveit_config/package.xml index f11008b6..80bd0957 100644 --- a/franka_moveit_config/package.xml +++ b/franka_moveit_config/package.xml @@ -2,7 +2,7 @@ franka_moveit_config - 0.1.6 + 0.1.7 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 3867b231..d9c7b0c1 100644 --- a/franka_msgs/package.xml +++ b/franka_msgs/package.xml @@ -2,7 +2,7 @@ franka_msgs - 0.1.6 + 0.1.7 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 0674002b..c8dcdd25 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.6 + 0.1.7 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 f05573b2..c31b0867 100644 --- a/integration_launch_testing/package.xml +++ b/integration_launch_testing/package.xml @@ -2,7 +2,7 @@ integration_launch_testing - 0.1.6 + 0.1.7 Functional integration tests for franka controllers Franka Emika GmbH Apache 2.0