From e2a2a4ae937829864706da845f1410233f8decda Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Sat, 4 Nov 2023 11:17:19 +0100 Subject: [PATCH 1/7] feat: joint position command interface --- franka_bringup/config/controllers.yaml | 3 + ...oint_position_example_controller.launch.py | 77 +++++++++++++++ .../robots/panda_arm.ros2_control.xacro | 1 + franka_example_controllers/CMakeLists.txt | 1 + .../franka_example_controllers.xml | 6 ++ .../joint_position_example_controller.hpp | 55 +++++++++++ .../src/joint_position_example_controller.cpp | 98 +++++++++++++++++++ .../franka_hardware_interface.hpp | 3 + .../include/franka_hardware/robot.hpp | 13 ++- .../src/franka_hardware_interface.cpp | 28 +++++- franka_hardware/src/robot.cpp | 29 +++++- .../test/franka_hardware_interface_test.cpp | 45 ++++++++- ..._joint_position_command_interface_test.hpp | 0 franka_hardware/test/test_utils.hpp | 1 + 14 files changed, 350 insertions(+), 10 deletions(-) create mode 100644 franka_bringup/launch/joint_position_example_controller.launch.py create mode 100644 franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.hpp create mode 100644 franka_example_controllers/src/joint_position_example_controller.cpp create mode 100644 franka_hardware/test/franka_hardware_joint_position_command_interface_test.hpp 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_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..8caf3644 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 joint 4 and 5 in a periodic movement. + + 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..a0bee89c --- /dev/null +++ b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.hpp @@ -0,0 +1,55 @@ +// 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 +#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 joint 4 and 5 in a very compliant periodic movement. + */ +class JointPositionExampleController : public controller_interface::ControllerInterface { + public: + using Vector7d = Eigen::Matrix; + [[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; + Vector7d initial_q_; + double elapsed_time_ = 0.0; + std::string arm_id{"panda"}; + + bool first_time_{true}; + rclcpp::Time start_time_; +}; + +} // namespace franka_example_controllers \ No newline at end of file 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..3edc8f6f --- /dev/null +++ b/franka_example_controllers/src/joint_position_example_controller.cpp @@ -0,0 +1,98 @@ +// 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 (first_time_) { + for (size_t i = 0; i < 7; ++i) { + initial_q_(i) = command_interfaces_[i].get_value(); + } + first_time_ = false; + } + // std::cout << "initial q4: " << initial_q_[4] << "time: " << robot_state_ptr->time.toSec() + // << std::endl; + elapsed_time_ = elapsed_time_ + 0.001; + // std::cout << "Period: " << period << std::endl; + // std::cout << "Elapsed time: " << elapsed_time_ << std::endl; + 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_(i) - delta_angle); + } else { + command_interfaces_[i].set_value(initial_q_(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*/) { + 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_hardware/include/franka_hardware/franka_hardware_interface.hpp b/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp index 99c7632c..52c24f33 100644 --- a/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp +++ b/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp @@ -121,6 +121,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..1e54cb2c 100644 --- a/franka_hardware/include/franka_hardware/robot.hpp +++ b/franka_hardware/include/franka_hardware/robot.hpp @@ -66,7 +66,10 @@ 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 @@ -241,7 +244,12 @@ class Robot { * @param[in] joint_velocities joint velocity command. */ virtual void writeOnceJointVelocities(const std::array& joint_velocities); - + + /** + * The robot will use these position 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,6 +270,7 @@ 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}; diff --git a/franka_hardware/src/franka_hardware_interface.cpp b/franka_hardware/src/franka_hardware_interface.cpp index 861324e0..123e948d 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_}, @@ -77,6 +78,8 @@ std::vector FrankaHardwareInterface::export_command_interfaces info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_commands_.at(i))); command_interfaces.emplace_back(CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_.at(i))); + command_interfaces.emplace_back(CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_.at(i))); } // cartesian velocity command interface 6 in order: dx, dy, dz, wx, wy, wz @@ -120,9 +123,10 @@ 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 + if (first_pass_ && (elbow_command_interface_running_ || position_joint_interface_running_)) { + // position commands should be initialized with the first desired commands from the robot hw_elbow_command_ = hw_franka_robot_state_.elbow_c; + hw_commands_ = hw_franka_robot_state_.q_d; first_pass_ = false; } @@ -148,6 +152,8 @@ hardware_interface::return_type FrankaHardwareInterface::write(const rclcpp::Tim if (velocity_joint_interface_running_ || effort_interface_running_) { robot_->writeOnce(hw_commands_); + } else if (position_joint_interface_running_ && !first_pass_) { + robot_->writeOnce(hw_commands_); } else if (velocity_cartesian_interface_running_ && elbow_command_interface_running_ && !first_pass_) { // Wait until the first read pass after robot controller is activated to write the elbow @@ -171,13 +177,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(), @@ -257,6 +264,16 @@ 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; + } else if (position_joint_interface_running_ && !position_joint_interface_claimed_) { + robot_->stopRobot(); + position_joint_interface_running_ = false; + first_pass_ = false; + } + if (!velocity_cartesian_interface_running_ && velocity_cartesian_interface_claimed_) { hw_cartesian_velocities_.fill(0); robot_->stopRobot(); @@ -271,6 +288,7 @@ hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_sw if (elbow_command_interface_running_) { elbow_command_interface_running_ = false; elbow_command_interface_claimed_ = false; + first_pass_ = false; } velocity_cartesian_interface_running_ = false; } diff --git a/franka_hardware/src/robot.cpp b/franka_hardware/src/robot.cpp index 24432054..f313a652 100644 --- a/franka_hardware/src/robot.cpp +++ b/franka_hardware/src/robot.cpp @@ -68,6 +68,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 +101,25 @@ 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 (position_command_rate_limit_active_) { + // position_command.dq = franka::limitRate( + // franka::computeUpperLimitsJointposition(current_state_.q_d), + // franka::computeLowerLimitsJointposition(current_state_.q_d), franka::kMaxJointAcceleration, + // franka::kMaxJointJerk, position_command.dq, 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++) { @@ -174,6 +195,12 @@ void Robot::initializeJointVelocityInterface() { joint_velocity_interface_active_ = true; } +void Robot::initializeJointPositionInterface() { + 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); @@ -181,7 +208,7 @@ void Robot::initializeCartesianVelocityInterface() { } bool Robot::isControlLoopActive() { - return joint_velocity_interface_active_ || effort_interface_active_ || + return joint_position_interface_active_ || joint_velocity_interface_active_ || effort_interface_active_ || cartesian_velocity_interface_active_; } diff --git a/franka_hardware/test/franka_hardware_interface_test.cpp b/franka_hardware/test/franka_hardware_interface_test.cpp index 2525efc4..9694ba64 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; @@ -366,6 +369,40 @@ TEST_P(FrankaHardwareInterfaceTest, when_write_called_expect_ok) { const auto time = rclcpp::Time(0, 0); const auto duration = rclcpp::Duration(0, 0); + if (command_interface == k_position_controller) { + EXPECT_EQ(franka_hardware_interface.read(time, duration), hardware_interface::return_type::OK); + } + EXPECT_EQ(franka_hardware_interface.write(time, duration), hardware_interface::return_type::OK); +} + +TEST_P( + 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 = {}; + + EXPECT_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), + 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); } @@ -407,6 +444,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); @@ -691,4 +730,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/franka_hardware_joint_position_command_interface_test.hpp b/franka_hardware/test/franka_hardware_joint_position_command_interface_test.hpp new file mode 100644 index 00000000..e69de29b 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)); From 52bf8ed6aa030440b9678aa8a4ada8f7c22f71aa Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Mon, 6 Nov 2023 14:44:03 +0100 Subject: [PATCH 2/7] hotfix: current state during the control assigned correctly --- .../joint_position_example_controller.hpp | 1 + .../src/joint_position_example_controller.cpp | 11 ++---- .../include/franka_hardware/robot.hpp | 5 ++- franka_hardware/src/robot.cpp | 37 +++++++++++-------- ..._joint_position_command_interface_test.hpp | 0 5 files changed, 29 insertions(+), 25 deletions(-) delete mode 100644 franka_hardware/test/franka_hardware_joint_position_command_interface_test.hpp 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 index a0bee89c..91848fa1 100644 --- 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 @@ -45,6 +45,7 @@ class JointPositionExampleController : public controller_interface::ControllerIn std::string arm_id_; const int num_joints = 7; Vector7d initial_q_; + const double trajectory_period{0.001}; double elapsed_time_ = 0.0; std::string arm_id{"panda"}; diff --git a/franka_example_controllers/src/joint_position_example_controller.cpp b/franka_example_controllers/src/joint_position_example_controller.cpp index 3edc8f6f..c603741f 100644 --- a/franka_example_controllers/src/joint_position_example_controller.cpp +++ b/franka_example_controllers/src/joint_position_example_controller.cpp @@ -35,7 +35,7 @@ JointPositionExampleController::command_interface_configuration() const { controller_interface::InterfaceConfiguration JointPositionExampleController::state_interface_configuration() const { - return controller_interface::InterfaceConfiguration{ + return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE}; } @@ -48,11 +48,8 @@ controller_interface::return_type JointPositionExampleController::update( } first_time_ = false; } - // std::cout << "initial q4: " << initial_q_[4] << "time: " << robot_state_ptr->time.toSec() - // << std::endl; - elapsed_time_ = elapsed_time_ + 0.001; - // std::cout << "Period: " << period << std::endl; - // std::cout << "Elapsed time: " << elapsed_time_ << std::endl; + + 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) { @@ -66,7 +63,6 @@ controller_interface::return_type JointPositionExampleController::update( return controller_interface::return_type::OK; } - CallbackReturn JointPositionExampleController::on_init() { try { auto_declare("arm_id", "panda"); @@ -90,7 +86,6 @@ CallbackReturn JointPositionExampleController::on_activate( return CallbackReturn::SUCCESS; } - } // namespace franka_example_controllers #include "pluginlib/class_list_macros.hpp" // NOLINTNEXTLINE diff --git a/franka_hardware/include/franka_hardware/robot.hpp b/franka_hardware/include/franka_hardware/robot.hpp index 1e54cb2c..8cccf6de 100644 --- a/franka_hardware/include/franka_hardware/robot.hpp +++ b/franka_hardware/include/franka_hardware/robot.hpp @@ -72,7 +72,7 @@ class Robot { /// 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(); /** @@ -276,6 +276,9 @@ class Robot { bool velocity_command_rate_limit_active_{false}; bool cartesian_velocity_command_rate_limit_active_{false}; bool cartesian_velocity_low_pass_filter_active{false}; + bool joint_position_command_rate_limit_active_{false}; + bool joint_position_command_low_pass_filter_active_{false}; + double low_pass_filter_cut_off_freq{1000.0}; franka::RobotState current_state_; diff --git a/franka_hardware/src/robot.cpp b/franka_hardware/src/robot.cpp index f313a652..88c13fbb 100644 --- a/franka_hardware/src/robot.cpp +++ b/franka_hardware/src/robot.cpp @@ -48,10 +48,10 @@ 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() { @@ -68,7 +68,7 @@ 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_){ + } else if (joint_position_interface_active_) { writeOnceJointPositions(joint_commands); } } @@ -101,7 +101,6 @@ 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_); @@ -109,17 +108,23 @@ void Robot::writeOnceJointPositions(const std::array& positions) { // If you are experiencing issues with robot error. You can try activating the rate limiter. // Rate limiter is default deactivated. - // if (position_command_rate_limit_active_) { - // position_command.dq = franka::limitRate( - // franka::computeUpperLimitsJointposition(current_state_.q_d), - // franka::computeLowerLimitsJointposition(current_state_.q_d), franka::kMaxJointAcceleration, - // franka::kMaxJointJerk, position_command.dq, current_state_.dq_d, current_state_.ddq_d); - // } - + if (joint_position_command_low_pass_filter_active_) { + for (size_t i = 0; i < 7; i++) { + position_command.q[i] = + franka::lowpassFilter(franka::kDeltaT, position_command.q[i], current_state_.q_d[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++) { @@ -176,8 +181,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() { @@ -208,8 +213,8 @@ void Robot::initializeCartesianVelocityInterface() { } bool Robot::isControlLoopActive() { - return joint_position_interface_active_ || 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_joint_position_command_interface_test.hpp b/franka_hardware/test/franka_hardware_joint_position_command_interface_test.hpp deleted file mode 100644 index e69de29b..00000000 From 1198828928742626b53b3429bbec39b0e2af7f82 Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Fri, 10 Nov 2023 10:39:09 +0100 Subject: [PATCH 3/7] hotfix: automatic acknowledge for reflex errors refactor hardware interface code. Add tests for infinite commands. Default cut-off-freq assigned to 100. --- .../elbow_example_controller.hpp | 2 +- .../joint_position_example_controller.hpp | 2 +- .../cartesian_velocity_example_controller.cpp | 2 +- .../src/elbow_example_controller.cpp | 9 +-- .../src/joint_position_example_controller.cpp | 6 +- .../src/joint_velocity_example_controller.cpp | 1 + .../franka_hardware_interface.hpp | 16 +++-- .../include/franka_hardware/robot.hpp | 6 +- .../src/franka_hardware_interface.cpp | 69 +++++++++++-------- franka_hardware/src/robot.cpp | 40 +++++++++-- ...dware_cartesian_command_interface_test.cpp | 45 +++++++++++- .../test/franka_hardware_interface_test.cpp | 39 ++++++++++- 12 files changed, 185 insertions(+), 52 deletions(-) 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_position_example_controller.hpp b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.hpp index 91848fa1..d7ac2742 100644 --- 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 @@ -49,7 +49,7 @@ class JointPositionExampleController : public controller_interface::ControllerIn double elapsed_time_ = 0.0; std::string arm_id{"panda"}; - bool first_time_{true}; + bool initialization_flag_{true}; rclcpp::Time start_time_; }; 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_position_example_controller.cpp b/franka_example_controllers/src/joint_position_example_controller.cpp index c603741f..59a613bb 100644 --- a/franka_example_controllers/src/joint_position_example_controller.cpp +++ b/franka_example_controllers/src/joint_position_example_controller.cpp @@ -42,11 +42,11 @@ JointPositionExampleController::state_interface_configuration() const { controller_interface::return_type JointPositionExampleController::update( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - if (first_time_) { + if (initialization_flag_) { for (size_t i = 0; i < 7; ++i) { initial_q_(i) = command_interfaces_[i].get_value(); } - first_time_ = false; + initialization_flag_ = false; } elapsed_time_ = elapsed_time_ + trajectory_period; @@ -83,6 +83,8 @@ CallbackReturn JointPositionExampleController::on_configure( CallbackReturn JointPositionExampleController::on_activate( const rclcpp_lifecycle::State& /*previous_state*/) { + initialization_flag_ = true; + elapsed_time_ = 0.0; return CallbackReturn::SUCCESS; } 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_hardware/include/franka_hardware/franka_hardware_interface.hpp b/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp index 52c24f33..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}; diff --git a/franka_hardware/include/franka_hardware/robot.hpp b/franka_hardware/include/franka_hardware/robot.hpp index 8cccf6de..6dcb9dad 100644 --- a/franka_hardware/include/franka_hardware/robot.hpp +++ b/franka_hardware/include/franka_hardware/robot.hpp @@ -244,7 +244,7 @@ class Robot { * @param[in] joint_velocities joint velocity command. */ virtual void writeOnceJointVelocities(const std::array& joint_velocities); - + /** * The robot will use these position until a different set of position are commanded. * @param[in] joint_position joint position command. @@ -274,12 +274,14 @@ class Robot { 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}; + bool joint_position_command_rate_limit_active_{false}; bool joint_position_command_low_pass_filter_active_{false}; - double low_pass_filter_cut_off_freq{1000.0}; + double low_pass_filter_cut_off_freq{100.0}; franka::RobotState current_state_; }; diff --git a/franka_hardware/src/franka_hardware_interface.cpp b/franka_hardware/src/franka_hardware_interface.cpp index 123e948d..27c2638a 100644 --- a/franka_hardware/src/franka_hardware_interface.cpp +++ b/franka_hardware/src/franka_hardware_interface.cpp @@ -75,11 +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_commands_.at(i))); + 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 @@ -100,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"); @@ -115,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) { @@ -123,12 +140,7 @@ hardware_interface::return_type FrankaHardwareInterface::read(const rclcpp::Time hw_franka_robot_state_ = robot_->readOnce(); - if (first_pass_ && (elbow_command_interface_running_ || position_joint_interface_running_)) { - // position commands should be initialized with the first desired commands from the robot - hw_elbow_command_ = hw_franka_robot_state_.elbow_c; - hw_commands_ = hw_franka_robot_state_.q_d; - first_pass_ = false; - } + initializePositionCommands(hw_franka_robot_state_); hw_positions_ = hw_franka_robot_state_.q; hw_velocities_ = hw_franka_robot_state_.dq; @@ -137,25 +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_); - } else if (position_joint_interface_running_ && !first_pass_) { - 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_); @@ -245,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; @@ -255,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; @@ -268,10 +283,10 @@ hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_sw 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; - first_pass_ = false; } if (!velocity_cartesian_interface_running_ && velocity_cartesian_interface_claimed_) { @@ -280,6 +295,7 @@ hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_sw 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_) { @@ -288,7 +304,6 @@ hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_sw if (elbow_command_interface_running_) { elbow_command_interface_running_ = false; elbow_command_interface_claimed_ = false; - first_pass_ = false; } velocity_cartesian_interface_running_ = false; } diff --git a/franka_hardware/src/robot.cpp b/franka_hardware/src/robot.cpp index 88c13fbb..f290a178 100644 --- a/franka_hardware/src/robot.cpp +++ b/franka_hardware/src/robot.cpp @@ -58,6 +58,7 @@ 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(); } @@ -190,25 +191,50 @@ 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() { - active_control_ = robot_->startJointPositionControl( - research_interface::robot::Move::ControllerMode::kJointImpedance); + 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; } 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 9694ba64..503de9bc 100644 --- a/franka_hardware/test/franka_hardware_interface_test.cpp +++ b/franka_hardware/test/franka_hardware_interface_test.cpp @@ -375,7 +375,42 @@ TEST_P(FrankaHardwareInterfaceTest, when_write_called_expect_ok) { EXPECT_EQ(franka_hardware_interface.write(time, duration), hardware_interface::return_type::OK); } -TEST_P( +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 = {}; + + EXPECT_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), + hardware_interface::return_type::OK); + + const auto time = rclcpp::Time(0, 0); + const auto duration = rclcpp::Duration(0, 0); + + EXPECT_EQ(franka_hardware_interface.read(time, duration), hardware_interface::return_type::OK); + EXPECT_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(); @@ -444,7 +479,7 @@ 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) { + } else if (command_interface == k_position_controller) { EXPECT_CALL(*mock_robot, initializeJointPositionInterface()); } From 37ce4129af80f42f3f5b20c7b94a431fc0c59c8a Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Fri, 10 Nov 2023 11:01:42 +0100 Subject: [PATCH 4/7] chore: devcontainer packages update --- .devcontainer/Dockerfile | 3 +++ franka_example_controllers/franka_example_controllers.xml | 2 +- franka_hardware/include/franka_hardware/robot.hpp | 6 +++--- 3 files changed, 7 insertions(+), 4 deletions(-) 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/franka_example_controllers/franka_example_controllers.xml b/franka_example_controllers/franka_example_controllers.xml index 8caf3644..5f278271 100644 --- a/franka_example_controllers/franka_example_controllers.xml +++ b/franka_example_controllers/franka_example_controllers.xml @@ -20,7 +20,7 @@ - The joint position example controller moves joint 4 and 5 in a periodic movement. + The joint position example controller moves in a periodic movement. & 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 these position until a different set of position are commanded. + * 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); From fa4ad6cd6cbbeeb456fc31c840009db2a4f2413e Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Fri, 10 Nov 2023 11:15:49 +0100 Subject: [PATCH 5/7] chore: fix linting errors --- .../joint_position_example_controller.hpp | 7 +- .../src/joint_position_example_controller.cpp | 6 +- franka_hardware/src/robot.cpp | 4 +- .../test/franka_hardware_interface_test.cpp | 66 +++++++++---------- 4 files changed, 41 insertions(+), 42 deletions(-) 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 index d7ac2742..6bd3dcf0 100644 --- 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 @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Franka Emika GmbH +// 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. @@ -26,11 +26,10 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface namespace franka_example_controllers { /** - * The joint position example controller moves joint 4 and 5 in a very compliant periodic movement. + * The joint position example controller moves in a periodic movement. */ class JointPositionExampleController : public controller_interface::ControllerInterface { public: - using Vector7d = Eigen::Matrix; [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration() const override; [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration() @@ -44,7 +43,7 @@ class JointPositionExampleController : public controller_interface::ControllerIn private: std::string arm_id_; const int num_joints = 7; - Vector7d initial_q_; + 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"}; diff --git a/franka_example_controllers/src/joint_position_example_controller.cpp b/franka_example_controllers/src/joint_position_example_controller.cpp index 59a613bb..2b88f32f 100644 --- a/franka_example_controllers/src/joint_position_example_controller.cpp +++ b/franka_example_controllers/src/joint_position_example_controller.cpp @@ -44,7 +44,7 @@ controller_interface::return_type JointPositionExampleController::update( const rclcpp::Duration& /*period*/) { if (initialization_flag_) { for (size_t i = 0; i < 7; ++i) { - initial_q_(i) = command_interfaces_[i].get_value(); + initial_q_.at(i) = command_interfaces_[i].get_value(); } initialization_flag_ = false; } @@ -54,9 +54,9 @@ controller_interface::return_type JointPositionExampleController::update( for (size_t i = 0; i < 7; ++i) { if (i == 4) { - command_interfaces_[i].set_value(initial_q_(i) - delta_angle); + command_interfaces_[i].set_value(initial_q_.at(i) - delta_angle); } else { - command_interfaces_[i].set_value(initial_q_(i) + delta_angle); + command_interfaces_[i].set_value(initial_q_.at(i) + delta_angle); } } diff --git a/franka_hardware/src/robot.cpp b/franka_hardware/src/robot.cpp index f290a178..5b2ca57b 100644 --- a/franka_hardware/src/robot.cpp +++ b/franka_hardware/src/robot.cpp @@ -111,8 +111,8 @@ void Robot::writeOnceJointPositions(const std::array& positions) { // Rate limiter is default deactivated. if (joint_position_command_low_pass_filter_active_) { for (size_t i = 0; i < 7; i++) { - position_command.q[i] = - franka::lowpassFilter(franka::kDeltaT, position_command.q[i], current_state_.q_d[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); } } diff --git a/franka_hardware/test/franka_hardware_interface_test.cpp b/franka_hardware/test/franka_hardware_interface_test.cpp index 503de9bc..dbaec22c 100644 --- a/franka_hardware/test/franka_hardware_interface_test.cpp +++ b/franka_hardware/test/franka_hardware_interface_test.cpp @@ -127,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); } @@ -146,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( @@ -169,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; @@ -180,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( @@ -212,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), @@ -243,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), @@ -269,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); } @@ -313,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); } @@ -360,19 +360,19 @@ 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); if (command_interface == k_position_controller) { - EXPECT_EQ(franka_hardware_interface.read(time, duration), hardware_interface::return_type::OK); + ASSERT_EQ(franka_hardware_interface.read(time, duration), hardware_interface::return_type::OK); } - EXPECT_EQ(franka_hardware_interface.write(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) { @@ -396,17 +396,17 @@ TEST_F(FrankaHardwareInterfaceTest, when_write_called_with_inifite_command_expec 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.read(time, duration), hardware_interface::return_type::OK); - EXPECT_EQ(franka_hardware_interface.write(time, duration), + 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); } @@ -429,16 +429,16 @@ TEST_F( 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); + ASSERT_EQ(franka_hardware_interface.write(time, duration), hardware_interface::return_type::OK); } TEST_F(FrankaHardwareInterfaceTest, when_on_activate_called_expect_success) { @@ -453,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); } @@ -465,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); } @@ -496,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); } @@ -523,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++) { @@ -536,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); } From 980b23c573116c29253e7d506fa1a3f00b64f1ee Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Fri, 10 Nov 2023 14:03:22 +0100 Subject: [PATCH 6/7] chore: update joint impedance example controller time --- .../joint_impedance_example_controller.hpp | 2 +- .../src/joint_impedance_example_controller.cpp | 11 +++++++---- 2 files changed, 8 insertions(+), 5 deletions(-) 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/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; } From 9868cde2d966e8d3dbd86729382ec12d93de1a96 Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Fri, 10 Nov 2023 14:07:46 +0100 Subject: [PATCH 7/7] bump version 0.1.7 --- CHANGELOG.md | 9 +++++++++ franka_bringup/package.xml | 2 +- franka_description/package.xml | 2 +- franka_example_controllers/package.xml | 2 +- franka_gripper/package.xml | 2 +- franka_hardware/package.xml | 2 +- franka_moveit_config/package.xml | 2 +- franka_msgs/package.xml | 2 +- franka_robot_state_broadcaster/package.xml | 2 +- integration_launch_testing/package.xml | 2 +- 10 files changed, 18 insertions(+), 9 deletions(-) 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/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_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_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/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_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