diff --git a/CHANGELOG.md b/CHANGELOG.md index 72d90464..ddd36f2b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,16 @@ # Changelog +## 0.1.9 - 2023-12-04 + +Requires libfranka >= 0.13.0, required ROS 2 Humble + +* franka\_hardware: add state interfaces for initial position, cartesian pose and elbow. +* franka\_hardware: support cartesian pose interface. +* franka\_semantic\_component: support cartesian pose interface. +* franka\_example\_controllers: add cartesian pose example controller +* franka\_example\_controllers: add cartesian elbow controller +* franka\_example\_controllers: add cartesian orientation controller + ## 0.1.8 - 2023-11-16 Requires libfranka >= 0.13.0, required ROS 2 Humble diff --git a/franka_bringup/config/controllers.yaml b/franka_bringup/config/controllers.yaml index e1d9384f..bbfc1148 100644 --- a/franka_bringup/config/controllers.yaml +++ b/franka_bringup/config/controllers.yaml @@ -16,6 +16,15 @@ controller_manager: cartesian_velocity_example_controller: type: franka_example_controllers/CartesianVelocityExampleController + + cartesian_pose_example_controller: + type: franka_example_controllers/CartesianPoseExampleController + + cartesian_pose_elbow_example_controller: + type: franka_example_controllers/CartesianElbowExampleController + + cartesian_orientation_example_controller: + type: franka_example_controllers/CartesianOrientationExampleController elbow_example_controller: type: franka_example_controllers/ElbowExampleController diff --git a/franka_bringup/launch/cartesian_elbow_example_controller.launch.py b/franka_bringup/launch/cartesian_elbow_example_controller.launch.py new file mode 100644 index 00000000..00b0204e --- /dev/null +++ b/franka_bringup/launch/cartesian_elbow_example_controller.launch.py @@ -0,0 +1,77 @@ +# 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. + + +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=['cartesian_pose_elbow_example_controller'], + output='screen', + ), + ]) diff --git a/franka_bringup/launch/cartesian_orientation_example_controller.launch.py b/franka_bringup/launch/cartesian_orientation_example_controller.launch.py new file mode 100644 index 00000000..7e4768e9 --- /dev/null +++ b/franka_bringup/launch/cartesian_orientation_example_controller.launch.py @@ -0,0 +1,77 @@ +# 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. + + +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=['cartesian_orientation_example_controller'], + output='screen', + ), + ]) diff --git a/franka_bringup/launch/cartesian_pose_example_controller.launch.py b/franka_bringup/launch/cartesian_pose_example_controller.launch.py new file mode 100644 index 00000000..9037d5fd --- /dev/null +++ b/franka_bringup/launch/cartesian_pose_example_controller.launch.py @@ -0,0 +1,77 @@ +# 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. + + +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=['cartesian_pose_example_controller'], + output='screen', + ), + ]) diff --git a/franka_bringup/package.xml b/franka_bringup/package.xml index 9a44b6f8..cbb21670 100644 --- a/franka_bringup/package.xml +++ b/franka_bringup/package.xml @@ -2,7 +2,7 @@ franka_bringup - 0.1.8 + 0.1.9 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 72ab1101..7c55d9f5 100644 --- a/franka_description/package.xml +++ b/franka_description/package.xml @@ -2,7 +2,7 @@ franka_description - 0.1.8 + 0.1.9 franka_description contains URDF files and meshes of Franka Emika robots Franka Emika GmbH Apache 2.0 diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt index b4aaba5c..a40d8a7e 100644 --- a/franka_example_controllers/CMakeLists.txt +++ b/franka_example_controllers/CMakeLists.txt @@ -33,6 +33,9 @@ add_library( src/joint_velocity_example_controller.cpp src/joint_position_example_controller.cpp src/cartesian_velocity_example_controller.cpp + src/cartesian_pose_example_controller.cpp + src/cartesian_elbow_example_controller.cpp + src/cartesian_orientation_example_controller.cpp src/elbow_example_controller.cpp src/model_example_controller.cpp src/move_to_start_example_controller.cpp diff --git a/franka_example_controllers/franka_example_controllers.xml b/franka_example_controllers/franka_example_controllers.xml index 5f278271..942a7cfe 100644 --- a/franka_example_controllers/franka_example_controllers.xml +++ b/franka_example_controllers/franka_example_controllers.xml @@ -29,6 +29,24 @@ The cartesian velocity example controller commands linear velocity with x and z components. + + + The cartesian pose example controller commands to translation on x and z components. + + + + + The cartesian pose example controller commands the elbow with the same initial pose configuration. + + + + + The cartesian orientation example controller commands to rotation around x axis. + + diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_elbow_example_controller.hpp b/franka_example_controllers/include/franka_example_controllers/cartesian_elbow_example_controller.hpp new file mode 100644 index 00000000..ed81c66c --- /dev/null +++ b/franka_example_controllers/include/franka_example_controllers/cartesian_elbow_example_controller.hpp @@ -0,0 +1,56 @@ +// 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 + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +namespace franka_example_controllers { + +/** + * The elbow example controller through cartesian pose command interface + */ +class CartesianElbowExampleController : 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; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + private: + std::unique_ptr franka_cartesian_pose_; + const bool k_elbow_activated_{true}; + std::vector initial_cartesian_pose_and_elbow; + bool initialization_flag_{true}; + std::array initial_elbow_configuration_{0.0, 0.0}; + std::array initial_pose_configuration_{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + double elapsed_time_{0.0}; + const double traj_frequency_{0.001}; +}; + +} // namespace franka_example_controllers \ No newline at end of file diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_orientation_example_controller.hpp b/franka_example_controllers/include/franka_example_controllers/cartesian_orientation_example_controller.hpp new file mode 100644 index 00000000..112fd8ef --- /dev/null +++ b/franka_example_controllers/include/franka_example_controllers/cartesian_orientation_example_controller.hpp @@ -0,0 +1,57 @@ +// 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 + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +namespace franka_example_controllers { + +/** + * The cartesian pose example controller + */ +class CartesianOrientationExampleController : 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; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + private: + std::unique_ptr franka_cartesian_pose_; + + Eigen::Quaterniond orientation_; + Eigen::Vector3d position_; + double trajectory_period_{0.001}; + const bool k_elbow_activated_{false}; + bool initialization_flag_{true}; + + double elapsed_time_{0.0}; +}; + +} // namespace franka_example_controllers \ No newline at end of file diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.hpp b/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.hpp new file mode 100644 index 00000000..b21138ab --- /dev/null +++ b/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.hpp @@ -0,0 +1,58 @@ +// 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 + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +namespace franka_example_controllers { + +/** + * The cartesian pose example controller + */ +class CartesianPoseExampleController : 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; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + private: + std::unique_ptr franka_cartesian_pose_; + + Eigen::Quaterniond orientation_; + Eigen::Vector3d position_; + + double trajectory_period_{0.001}; + const bool k_elbow_activated_{false}; + bool initialization_flag_{true}; + + double elapsed_time_{0.0}; +}; + +} // namespace franka_example_controllers \ No newline at end of file diff --git a/franka_example_controllers/include/franka_example_controllers/default_robot_behavior_utils.hpp b/franka_example_controllers/include/franka_example_controllers/default_robot_behavior_utils.hpp new file mode 100644 index 00000000..20c5c54d --- /dev/null +++ b/franka_example_controllers/include/franka_example_controllers/default_robot_behavior_utils.hpp @@ -0,0 +1,37 @@ +// 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 + +namespace DefaultRobotBehavior { + +inline franka_msgs::srv::SetFullCollisionBehavior::Request::SharedPtr +getDefaultCollisionBehaviorRequest() { + auto request = std::make_shared(); + + request->lower_torque_thresholds_nominal = {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}; + request->upper_torque_thresholds_nominal = {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}; + request->lower_torque_thresholds_acceleration = {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}; + request->upper_torque_thresholds_acceleration = {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}; + request->lower_force_thresholds_nominal = {30.0, 30.0, 30.0, 25.0, 25.0, 25.0}; + request->upper_force_thresholds_nominal = {40.0, 40.0, 40.0, 35.0, 35.0, 35.0}; + request->lower_force_thresholds_acceleration = {30.0, 30.0, 30.0, 25.0, 25.0, 25.0}; + request->upper_force_thresholds_acceleration = {40.0, 40.0, 40.0, 35.0, 35.0, 35.0}; + + return request; +} + +} // namespace DefaultRobotBehavior \ No newline at end of file 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 6bd3dcf0..9b4d1c0d 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 @@ -47,6 +47,7 @@ class JointPositionExampleController : public controller_interface::ControllerIn const double trajectory_period{0.001}; double elapsed_time_ = 0.0; std::string arm_id{"panda"}; + const std::string k_HW_IF_INITIAL_POSITION = "initial_joint_position"; bool initialization_flag_{true}; rclcpp::Time start_time_; diff --git a/franka_example_controllers/package.xml b/franka_example_controllers/package.xml index 18afb684..34b91490 100644 --- a/franka_example_controllers/package.xml +++ b/franka_example_controllers/package.xml @@ -2,7 +2,7 @@ franka_example_controllers - 0.1.8 + 0.1.9 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_elbow_example_controller.cpp b/franka_example_controllers/src/cartesian_elbow_example_controller.cpp new file mode 100644 index 00000000..6b512a01 --- /dev/null +++ b/franka_example_controllers/src/cartesian_elbow_example_controller.cpp @@ -0,0 +1,118 @@ +// 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 + +using namespace std::chrono_literals; + +namespace franka_example_controllers { + +controller_interface::InterfaceConfiguration +CartesianElbowExampleController::command_interface_configuration() const { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names = franka_cartesian_pose_->get_command_interface_names(); + + return config; +} + +controller_interface::InterfaceConfiguration +CartesianElbowExampleController::state_interface_configuration() const { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names = franka_cartesian_pose_->get_state_interface_names(); + + return config; +} + +controller_interface::return_type CartesianElbowExampleController::update( + const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) { + if (initialization_flag_) { + // Get initial elbow configuration values + initial_elbow_configuration_ = franka_cartesian_pose_->getInitialElbowConfiguration(); + // Get the initial pose + initial_pose_configuration_ = franka_cartesian_pose_->getInitialPoseMatrix(); + initialization_flag_ = false; + } + elapsed_time_ = elapsed_time_ + traj_frequency_; + + double angle = M_PI / 15.0 * (1.0 - std::cos(M_PI / 5.0 * elapsed_time_)); + std::array pose_command = initial_pose_configuration_; + std::array elbow_command = { + {initial_elbow_configuration_[0] + angle, initial_elbow_configuration_[1]}}; + + if (franka_cartesian_pose_->setCommand(pose_command, elbow_command)) { + return controller_interface::return_type::OK; + } else { + RCLCPP_FATAL(get_node()->get_logger(), + "Set command failed. Did you activate the elbow command interface?"); + return controller_interface::return_type::ERROR; + } +} + +CallbackReturn CartesianElbowExampleController::on_init() { + return CallbackReturn::SUCCESS; +} + +CallbackReturn CartesianElbowExampleController::on_configure( + const rclcpp_lifecycle::State& /*previous_state*/) { + franka_cartesian_pose_ = + std::make_unique( + franka_semantic_components::FrankaCartesianPoseInterface(k_elbow_activated_)); + + auto request = DefaultRobotBehavior::getDefaultCollisionBehaviorRequest(); + auto client = get_node()->create_client( + "service_server/set_full_collision_behavior"); + + auto future_result = client->async_send_request(request); + future_result.wait_for(1000ms); + auto success = future_result.get(); + if (!success) { + RCLCPP_FATAL(get_node()->get_logger(), "Failed to set default collision behavior."); + return CallbackReturn::ERROR; + } else { + RCLCPP_INFO(get_node()->get_logger(), "Default collision behavior set."); + } + + return CallbackReturn::SUCCESS; +} + +CallbackReturn CartesianElbowExampleController::on_activate( + const rclcpp_lifecycle::State& /*previous_state*/) { + initialization_flag_ = true; + elapsed_time_ = 0.0; + franka_cartesian_pose_->assign_loaned_command_interfaces(command_interfaces_); + franka_cartesian_pose_->assign_loaned_state_interfaces(state_interfaces_); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn CartesianElbowExampleController::on_deactivate( + const rclcpp_lifecycle::State& /*previous_state*/) { + franka_cartesian_pose_->release_interfaces(); + return CallbackReturn::SUCCESS; +} + +} // namespace franka_example_controllers +#include "pluginlib/class_list_macros.hpp" +// NOLINTNEXTLINE +PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianElbowExampleController, + controller_interface::ControllerInterface) \ No newline at end of file diff --git a/franka_example_controllers/src/cartesian_orientation_example_controller.cpp b/franka_example_controllers/src/cartesian_orientation_example_controller.cpp new file mode 100644 index 00000000..3184b87c --- /dev/null +++ b/franka_example_controllers/src/cartesian_orientation_example_controller.cpp @@ -0,0 +1,124 @@ +// 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 + +using namespace std::chrono_literals; + +namespace franka_example_controllers { + +controller_interface::InterfaceConfiguration +CartesianOrientationExampleController::command_interface_configuration() const { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names = franka_cartesian_pose_->get_command_interface_names(); + + return config; +} + +controller_interface::InterfaceConfiguration +CartesianOrientationExampleController::state_interface_configuration() const { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names = franka_cartesian_pose_->get_state_interface_names(); + + return config; +} + +controller_interface::return_type CartesianOrientationExampleController::update( + const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) { + if (initialization_flag_) { + std::tie(orientation_, position_) = + franka_cartesian_pose_->getInitialOrientationAndTranslation(); + + initialization_flag_ = false; + } + + elapsed_time_ = elapsed_time_ + trajectory_period_; + double angle = M_PI / 8 * (1 - std::cos(M_PI / 5.0 * elapsed_time_)); + + Eigen::Quaterniond new_orientation; + Eigen::Vector3d new_position; + + new_position = position_; + new_orientation = orientation_; + + new_orientation = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX()) * new_orientation; + + if (franka_cartesian_pose_->setCommand(new_orientation, new_position)) { + return controller_interface::return_type::OK; + } else { + RCLCPP_FATAL(get_node()->get_logger(), + "Set command failed. Did you activate the elbow command interface?"); + return controller_interface::return_type::ERROR; + } +} + +CallbackReturn CartesianOrientationExampleController::on_init() { + franka_cartesian_pose_ = + std::make_unique( + franka_semantic_components::FrankaCartesianPoseInterface(k_elbow_activated_)); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn CartesianOrientationExampleController::on_configure( + const rclcpp_lifecycle::State& /*previous_state*/) { + auto client = get_node()->create_client( + "service_server/set_full_collision_behavior"); + auto request = DefaultRobotBehavior::getDefaultCollisionBehaviorRequest(); + + auto future_result = client->async_send_request(request); + + future_result.wait_for(1000ms); + auto success = future_result.get(); + if (!success) { + RCLCPP_FATAL(get_node()->get_logger(), "Failed to set default collision behavior."); + return CallbackReturn::ERROR; + } else { + RCLCPP_INFO(get_node()->get_logger(), "Default collision behavior set."); + } + + return CallbackReturn::SUCCESS; +} + +CallbackReturn CartesianOrientationExampleController::on_activate( + const rclcpp_lifecycle::State& /*previous_state*/) { + initialization_flag_ = true; + elapsed_time_ = 0.0; + + franka_cartesian_pose_->assign_loaned_command_interfaces(command_interfaces_); + franka_cartesian_pose_->assign_loaned_state_interfaces(state_interfaces_); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn CartesianOrientationExampleController::on_deactivate( + const rclcpp_lifecycle::State& /*previous_state*/) { + franka_cartesian_pose_->release_interfaces(); + return CallbackReturn::SUCCESS; +} + +} // namespace franka_example_controllers +#include "pluginlib/class_list_macros.hpp" +// NOLINTNEXTLINE +PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianOrientationExampleController, + controller_interface::ControllerInterface) \ No newline at end of file diff --git a/franka_example_controllers/src/cartesian_pose_example_controller.cpp b/franka_example_controllers/src/cartesian_pose_example_controller.cpp new file mode 100644 index 00000000..27c4d0bc --- /dev/null +++ b/franka_example_controllers/src/cartesian_pose_example_controller.cpp @@ -0,0 +1,128 @@ +// 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 + +using namespace std::chrono_literals; + +namespace franka_example_controllers { + +controller_interface::InterfaceConfiguration +CartesianPoseExampleController::command_interface_configuration() const { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names = franka_cartesian_pose_->get_command_interface_names(); + + return config; +} + +controller_interface::InterfaceConfiguration +CartesianPoseExampleController::state_interface_configuration() const { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names = franka_cartesian_pose_->get_state_interface_names(); + + return config; +} + +controller_interface::return_type CartesianPoseExampleController::update( + const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) { + if (initialization_flag_) { + // Get initial orientation and translation + std::tie(orientation_, position_) = + franka_cartesian_pose_->getInitialOrientationAndTranslation(); + initialization_flag_ = false; + } + + elapsed_time_ = elapsed_time_ + trajectory_period_; + double radius = 0.1; + double angle = M_PI / 4 * (1 - std::cos(M_PI / 5.0 * elapsed_time_)); + + double delta_x = radius * std::sin(angle); + double delta_z = radius * (std::cos(angle) - 1); + + Eigen::Quaterniond new_orientation; + Eigen::Vector3d new_position; + + new_position = position_; + new_orientation = orientation_; + + new_position(0) -= delta_x; + new_position(2) -= delta_z; + + if (franka_cartesian_pose_->setCommand(new_orientation, new_position)) { + return controller_interface::return_type::OK; + } else { + RCLCPP_FATAL(get_node()->get_logger(), + "Set command failed. Did you activate the elbow command interface?"); + return controller_interface::return_type::ERROR; + } +} + +CallbackReturn CartesianPoseExampleController::on_init() { + franka_cartesian_pose_ = + std::make_unique( + franka_semantic_components::FrankaCartesianPoseInterface(k_elbow_activated_)); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn CartesianPoseExampleController::on_configure( + const rclcpp_lifecycle::State& /*previous_state*/) { + auto client = get_node()->create_client( + "service_server/set_full_collision_behavior"); + auto request = DefaultRobotBehavior::getDefaultCollisionBehaviorRequest(); + + auto future_result = client->async_send_request(request); + future_result.wait_for(1000ms); + + auto success = future_result.get(); + if (!success) { + RCLCPP_FATAL(get_node()->get_logger(), "Failed to set default collision behavior."); + return CallbackReturn::ERROR; + } else { + RCLCPP_INFO(get_node()->get_logger(), "Default collision behavior set."); + } + + return CallbackReturn::SUCCESS; +} + +CallbackReturn CartesianPoseExampleController::on_activate( + const rclcpp_lifecycle::State& /*previous_state*/) { + initialization_flag_ = true; + elapsed_time_ = 0.0; + franka_cartesian_pose_->assign_loaned_command_interfaces(command_interfaces_); + franka_cartesian_pose_->assign_loaned_state_interfaces(state_interfaces_); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn CartesianPoseExampleController::on_deactivate( + const rclcpp_lifecycle::State& /*previous_state*/) { + franka_cartesian_pose_->release_interfaces(); + return CallbackReturn::SUCCESS; +} + +} // namespace franka_example_controllers +#include "pluginlib/class_list_macros.hpp" +// NOLINTNEXTLINE +PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianPoseExampleController, + controller_interface::ControllerInterface) \ No newline at end of file diff --git a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp index 1c956561..862b7b9c 100644 --- a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include #include @@ -77,18 +77,18 @@ CallbackReturn CartesianVelocityExampleController::on_configure( auto client = get_node()->create_client( "service_server/set_full_collision_behavior"); - auto request = std::make_shared(); - - request->lower_torque_thresholds_nominal = {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}; - request->upper_torque_thresholds_nominal = {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}; - request->lower_torque_thresholds_acceleration = {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}; - request->upper_torque_thresholds_acceleration = {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}; - request->lower_force_thresholds_nominal = {30.0, 30.0, 30.0, 25.0, 25.0, 25.0}; - request->upper_force_thresholds_nominal = {40.0, 40.0, 40.0, 35.0, 35.0, 35.0}; - request->lower_force_thresholds_acceleration = {30.0, 30.0, 30.0, 25.0, 25.0, 25.0}; - request->upper_force_thresholds_acceleration = {40.0, 40.0, 40.0, 35.0, 35.0, 35.0}; - - client->async_send_request(request); + auto request = DefaultRobotBehavior::getDefaultCollisionBehaviorRequest(); + + auto future_result = client->async_send_request(request); + future_result.wait_for(1000ms); + + auto success = future_result.get(); + if (!success) { + RCLCPP_FATAL(get_node()->get_logger(), "Failed to set default collision behavior."); + return CallbackReturn::ERROR; + } else { + RCLCPP_INFO(get_node()->get_logger(), "Default collision behavior set."); + } return CallbackReturn::SUCCESS; } diff --git a/franka_example_controllers/src/elbow_example_controller.cpp b/franka_example_controllers/src/elbow_example_controller.cpp index d894729f..ea21d515 100644 --- a/franka_example_controllers/src/elbow_example_controller.cpp +++ b/franka_example_controllers/src/elbow_example_controller.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include #include #include @@ -35,22 +35,18 @@ ElbowExampleController::command_interface_configuration() const { controller_interface::InterfaceConfiguration ElbowExampleController::state_interface_configuration() const { - return controller_interface::InterfaceConfiguration{ - controller_interface::interface_configuration_type::NONE}; + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names = franka_cartesian_velocity_->get_state_interface_names(); + + return config; } controller_interface::return_type ElbowExampleController::update( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { if (initialization_flag_) { - // Get initial elbow configuration values - if (!franka_cartesian_velocity_->getCommandedElbowConfiguration(initial_elbow_configuration_)) { - RCLCPP_FATAL(get_node()->get_logger(), - "Can't get the initial elbow configuration. Did you activate the elbow in " - "cartesian velocity interface?"); - return controller_interface::return_type::ERROR; - } - + initial_elbow_configuration_ = franka_cartesian_velocity_->getInitialElbowConfiguration(); initialization_flag_ = false; } elapsed_time_ = elapsed_time_ + traj_frequency_; @@ -76,26 +72,24 @@ CallbackReturn ElbowExampleController::on_init() { CallbackReturn ElbowExampleController::on_configure( const rclcpp_lifecycle::State& /*previous_state*/) { - initial_cartesian_velocity_and_elbow.reserve(8); - franka_cartesian_velocity_ = std::make_unique( franka_semantic_components::FrankaCartesianVelocityInterface(k_elbow_activated_)); auto client = get_node()->create_client( "service_server/set_full_collision_behavior"); - auto request = std::make_shared(); + auto request = DefaultRobotBehavior::getDefaultCollisionBehaviorRequest(); - request->lower_torque_thresholds_nominal = {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}; - request->upper_torque_thresholds_nominal = {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}; - request->lower_torque_thresholds_acceleration = {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}; - request->upper_torque_thresholds_acceleration = {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}; - request->lower_force_thresholds_nominal = {30.0, 30.0, 30.0, 25.0, 25.0, 25.0}; - request->upper_force_thresholds_nominal = {40.0, 40.0, 40.0, 35.0, 35.0, 35.0}; - request->lower_force_thresholds_acceleration = {30.0, 30.0, 30.0, 25.0, 25.0, 25.0}; - request->upper_force_thresholds_acceleration = {40.0, 40.0, 40.0, 35.0, 35.0, 35.0}; + auto future_result = client->async_send_request(request); + future_result.wait_for(1000ms); - client->async_send_request(request); + auto success = future_result.get(); + if (!success) { + RCLCPP_FATAL(get_node()->get_logger(), "Failed to set default collision behavior."); + return CallbackReturn::ERROR; + } else { + RCLCPP_INFO(get_node()->get_logger(), "Default collision behavior set."); + } return CallbackReturn::SUCCESS; } @@ -103,6 +97,8 @@ CallbackReturn ElbowExampleController::on_configure( CallbackReturn ElbowExampleController::on_activate( const rclcpp_lifecycle::State& /*previous_state*/) { franka_cartesian_velocity_->assign_loaned_command_interfaces(command_interfaces_); + franka_cartesian_velocity_->assign_loaned_state_interfaces(state_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 2b88f32f..00a41352 100644 --- a/franka_example_controllers/src/joint_position_example_controller.cpp +++ b/franka_example_controllers/src/joint_position_example_controller.cpp @@ -35,16 +35,20 @@ JointPositionExampleController::command_interface_configuration() const { controller_interface::InterfaceConfiguration JointPositionExampleController::state_interface_configuration() const { - return controller_interface::InterfaceConfiguration{ - controller_interface::interface_configuration_type::NONE}; + 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) + "/" + k_HW_IF_INITIAL_POSITION); + } + return config; } 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(); + for (int i = 0; i < num_joints; ++i) { + initial_q_.at(i) = state_interfaces_[i].get_value(); } initialization_flag_ = false; } @@ -52,7 +56,7 @@ controller_interface::return_type JointPositionExampleController::update( 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) { + for (int i = 0; i < num_joints; ++i) { if (i == 4) { command_interfaces_[i].set_value(initial_q_.at(i) - delta_angle); } else { diff --git a/franka_example_controllers/src/joint_velocity_example_controller.cpp b/franka_example_controllers/src/joint_velocity_example_controller.cpp index b85e953b..8ca6c0de 100644 --- a/franka_example_controllers/src/joint_velocity_example_controller.cpp +++ b/franka_example_controllers/src/joint_velocity_example_controller.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include #include #include @@ -87,23 +87,18 @@ CallbackReturn JointVelocityExampleController::on_configure( auto client = get_node()->create_client( "service_server/set_full_collision_behavior"); - auto request = std::make_shared(); - - request->lower_torque_thresholds_acceleration = {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}; - request->upper_torque_thresholds_acceleration = {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}; - request->lower_torque_thresholds_nominal = {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}; - request->upper_torque_thresholds_nominal = {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}; - request->lower_force_thresholds_acceleration = {20.0, 20.0, 20.0, 25.0, 25.0, 25.0}; - request->upper_force_thresholds_acceleration = {20.0, 20.0, 20.0, 25.0, 25.0, 25.0}; - request->lower_force_thresholds_nominal = {20.0, 20.0, 20.0, 25.0, 25.0, 25.0}; - request->upper_force_thresholds_nominal = {20.0, 20.0, 20.0, 25.0, 25.0, 25.0}; - - if (!client->wait_for_service(20s)) { - RCLCPP_FATAL(get_node()->get_logger(), "service server can't be found."); - return CallbackReturn::FAILURE; - } + auto request = DefaultRobotBehavior::getDefaultCollisionBehaviorRequest(); + + auto future_result = client->async_send_request(request); + future_result.wait_for(1000ms); - client->async_send_request(request); + auto success = future_result.get(); + if (!success) { + RCLCPP_FATAL(get_node()->get_logger(), "Failed to set default collision behavior."); + return CallbackReturn::ERROR; + } else { + RCLCPP_INFO(get_node()->get_logger(), "Default collision behavior set."); + } return CallbackReturn::SUCCESS; } diff --git a/franka_gripper/package.xml b/franka_gripper/package.xml index 63fe81cc..b1b544d9 100644 --- a/franka_gripper/package.xml +++ b/franka_gripper/package.xml @@ -2,7 +2,7 @@ franka_gripper - 0.1.8 + 0.1.9 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 e96983e1..5e232b8f 100644 --- a/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp +++ b/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp @@ -74,6 +74,10 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface { // Initialize joint position commands in the first pass bool first_elbow_update_{true}; bool first_position_update_{true}; + bool first_cartesian_pose_update_{true}; + bool initial_robot_state_update_{true}; + bool initial_elbow_state_update_{true}; + bool initial_joint_position_update_{true}; std::shared_ptr robot_; std::shared_ptr node_; @@ -90,6 +94,10 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface { std::array hw_positions_{0, 0, 0, 0, 0, 0, 0}; std::array hw_velocities_{0, 0, 0, 0, 0, 0, 0}; std::array hw_efforts_{0, 0, 0, 0, 0, 0, 0}; + std::array initial_joint_positions_{0, 0, 0, 0, 0, 0, 0}; + // Cartesian States + std::array initial_robot_pose_{1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; + std::array initial_elbow_state_{0, 0}; /** * Desired Cartesian velocity with respect to the o-frame @@ -99,6 +107,9 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface { std::array hw_cartesian_velocities_names_{"vx", "vy", "vz", "wx", "wy", "wz"}; std::array hw_cartesian_velocities_{0, 0, 0, 0, 0, 0}; + // Pose is represented as a column-major homogeneous transformation matrix. + std::array hw_cartesian_pose_{1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; + /** * Elbow configuration. * @@ -115,8 +126,13 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface { std::array hw_elbow_command_{0, 0}; const std::string k_HW_IF_CARTESIAN_VELOCITY = "cartesian_velocity"; + const std::string k_HW_IF_CARTESIAN_POSE = "cartesian_pose"; const std::string k_HW_IF_ELBOW_COMMAND = "elbow_command"; + const std::string k_HW_IF_INITIAL_CARTESIAN_POSE = "initial_cartesian_pose"; + const std::string k_HW_IF_INITIAL_ELBOW_STATE = "initial_elbow_state"; + const std::string k_HW_IF_INITIAL_POSITION = "initial_joint_position"; + const std::vector command_interfaces_info_; franka::RobotState hw_franka_robot_state_; @@ -135,6 +151,9 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface { bool velocity_cartesian_interface_claimed_ = false; bool velocity_cartesian_interface_running_ = false; + bool pose_cartesian_interface_claimed_ = false; + bool pose_cartesian_interface_running_ = false; + bool elbow_command_interface_claimed_ = false; bool elbow_command_interface_running_ = false; diff --git a/franka_hardware/include/franka_hardware/robot.hpp b/franka_hardware/include/franka_hardware/robot.hpp index 2952651e..2826fed3 100644 --- a/franka_hardware/include/franka_hardware/robot.hpp +++ b/franka_hardware/include/franka_hardware/robot.hpp @@ -80,7 +80,10 @@ class Robot { /// Starts the active control for cartesian velocity control virtual void initializeCartesianVelocityInterface(); - /// stops the read continuous communication with the connected robot + /// Starts the active control for cartesian pose control + virtual void initializeCartesianPoseInterface(); + + /// Stops the continuous communication read with the connected robot virtual void stopRobot(); /** @@ -102,13 +105,6 @@ class Robot { */ virtual void writeOnce(const std::array& joint_hardware_command); - /** - * @brief Preprocessing includes rate limiting and low pass filtering, if activated - * - * @param cartesian_velocities cartesian velocity in libfranka format - */ - virtual void preProcessCartesianVelocities(franka::CartesianVelocities& cartesian_velocities); - /** * Cartesian velocity command * @param[in] cartesian_velocity_command cartesian level velocity command in format @@ -125,6 +121,22 @@ class Robot { virtual void writeOnce(const std::array& cartesian_velocity_command, const std::array& elbow_command); + /** + * Cartesian pose command + * @param[in] cartesian_pose_command cartesian level pose command in column major format [4x4] + * homogeneous transformation matrix + */ + virtual void writeOnce(const std::array& cartesian_pose_command); + + /** + * Cartesian pose command with elbow command + * @param[in] cartesian_pose_command cartesian level pose command in column major format [4x4] + * homogeneous transformation matrix + * @param[in] elbow_command elbow command representing joint3_position in rad and joint4 sign + */ + virtual void writeOnce(const std::array& cartesian_pose_command, + const std::array& elbow_command); + /** * Sets the impedance for each joint in the internal controller. * @@ -258,13 +270,25 @@ class Robot { * @param[in] joint_position joint position command. */ virtual void writeOnceJointPositions(const std::array& positions); + + /** + * @brief Preprocessing includes rate limiting and low pass filtering, if activated + * + * @param cartesian_velocities cartesian velocity in libfranka format + * + * @return franka::CartesianVelocities filtered and limit-rated cartesian_velocities + */ + franka::CartesianVelocities preProcessCartesianVelocities( + const franka::CartesianVelocities& cartesian_velocities); + /** - * @brief Checks if control loop is activated for active control. + * @brief Preprocessing includes low pass filtering, if activated + * + * @param cartesian_pose cartesian pose in libfranka format * - * @return true when active control started either with effort or velocity command. - * @return false when active control is not started. + * @return franka::CartesianPose filtered and limit-rated cartesian_pose */ - virtual bool isControlLoopActive(); + franka::CartesianPose preProcessCartesianPose(const franka::CartesianPose& cartesian_pose); std::mutex write_mutex_; std::mutex control_mutex_; @@ -278,12 +302,16 @@ class Robot { bool joint_velocity_interface_active_{false}; bool joint_position_interface_active_{false}; bool cartesian_velocity_interface_active_{false}; + bool cartesian_pose_interface_active_{false}; bool torque_command_rate_limiter_active_{true}; bool velocity_command_rate_limit_active_{false}; bool cartesian_velocity_command_rate_limit_active_{false}; - bool cartesian_velocity_low_pass_filter_active{false}; + bool cartesian_velocity_low_pass_filter_active_{false}; + + bool cartesian_pose_low_pass_filter_active_{false}; + bool cartesian_pose_command_rate_limit_active_{false}; bool joint_position_command_rate_limit_active_{false}; bool joint_position_command_low_pass_filter_active_{false}; diff --git a/franka_hardware/package.xml b/franka_hardware/package.xml index ac72c788..fb504aa6 100644 --- a/franka_hardware/package.xml +++ b/franka_hardware/package.xml @@ -2,7 +2,7 @@ franka_hardware - 0.1.8 + 0.1.9 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 27c2638a..9542b7f0 100644 --- a/franka_hardware/src/franka_hardware_interface.cpp +++ b/franka_hardware/src/franka_hardware_interface.cpp @@ -46,6 +46,7 @@ FrankaHardwareInterface::FrankaHardwareInterface() {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_}, + {k_HW_IF_CARTESIAN_POSE, hw_cartesian_pose_.size(), pose_cartesian_interface_claimed_}, }) {} std::vector FrankaHardwareInterface::export_state_interfaces() { @@ -57,6 +58,8 @@ std::vector FrankaHardwareInterface::export_state_interfaces() { info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_.at(i))); state_interfaces.emplace_back( StateInterface(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_efforts_.at(i))); + state_interfaces.emplace_back(StateInterface(info_.joints[i].name, k_HW_IF_INITIAL_POSITION, + &initial_joint_positions_.at(i))); } state_interfaces.emplace_back(StateInterface( @@ -67,6 +70,19 @@ std::vector FrankaHardwareInterface::export_state_interfaces() { k_robot_name, k_robot_model_interface_name, reinterpret_cast( // NOLINT(cppcoreguidelines-pro-type-reinterpret-cast) &hw_franka_model_ptr_))); + + // initial cartesian pose state interface 16 element pose matrix + for (auto i = 0U; i < 16; i++) { + state_interfaces.emplace_back(StateInterface(std::to_string(i), k_HW_IF_INITIAL_CARTESIAN_POSE, + &initial_robot_pose_.at(i))); + } + + // initial elbow state interface + for (auto i = 0U; i < hw_elbow_command_names_.size(); i++) { + state_interfaces.emplace_back(StateInterface( + hw_elbow_command_names_.at(i), k_HW_IF_INITIAL_ELBOW_STATE, &initial_elbow_state_.at(i))); + } + return state_interfaces; } @@ -89,6 +105,12 @@ std::vector FrankaHardwareInterface::export_command_interfaces &hw_cartesian_velocities_.at(i))); } + // cartesian pose command interface 16 element pose matrix + for (auto i = 0U; i < 16; i++) { + command_interfaces.emplace_back( + CommandInterface(std::to_string(i), k_HW_IF_CARTESIAN_POSE, &hw_cartesian_pose_.at(i))); + } + // elbow command interface for (auto i = 0U; i < hw_elbow_command_names_.size(); i++) { command_interfaces.emplace_back(CommandInterface( @@ -128,8 +150,16 @@ void initializeCommand(bool& first_update, void FrankaHardwareInterface::initializePositionCommands(const franka::RobotState& robot_state) { initializeCommand(first_elbow_update_, elbow_command_interface_running_, hw_elbow_command_, robot_state.elbow_c); + initializeCommand(initial_elbow_state_update_, elbow_command_interface_running_, + initial_elbow_state_, robot_state.elbow_c); initializeCommand(first_position_update_, position_joint_interface_running_, hw_position_commands_, robot_state.q_d); + initializeCommand(first_cartesian_pose_update_, pose_cartesian_interface_running_, + hw_cartesian_pose_, robot_state.O_T_EE_d); + initializeCommand(initial_robot_state_update_, pose_cartesian_interface_running_, + initial_robot_pose_, robot_state.O_T_EE_d); + initializeCommand(initial_joint_position_update_, position_joint_interface_running_, + initial_joint_positions_, robot_state.q_d); } hardware_interface::return_type FrankaHardwareInterface::read(const rclcpp::Time& /*time*/, @@ -137,7 +167,6 @@ hardware_interface::return_type FrankaHardwareInterface::read(const rclcpp::Time if (hw_franka_model_ptr_ == nullptr) { hw_franka_model_ptr_ = robot_->getModel(); } - hw_franka_robot_state_ = robot_->readOnce(); initializePositionCommands(hw_franka_robot_state_); @@ -159,7 +188,7 @@ hardware_interface::return_type FrankaHardwareInterface::write(const rclcpp::Tim const rclcpp::Duration& /*period*/) { if (hasInfinite(hw_position_commands_) || hasInfinite(hw_effort_commands_) || hasInfinite(hw_velocity_commands_) || hasInfinite(hw_cartesian_velocities_) || - hasInfinite(hw_elbow_command_)) { + hasInfinite(hw_elbow_command_) || hasInfinite(hw_cartesian_pose_)) { return hardware_interface::return_type::ERROR; } @@ -167,13 +196,23 @@ hardware_interface::return_type FrankaHardwareInterface::write(const rclcpp::Tim robot_->writeOnce(hw_velocity_commands_); } else if (effort_interface_running_) { robot_->writeOnce(hw_effort_commands_); - } else if (position_joint_interface_running_ && !first_position_update_) { + } else if (position_joint_interface_running_ && !first_position_update_ && + !initial_joint_position_update_) { robot_->writeOnce(hw_position_commands_); } else if (velocity_cartesian_interface_running_ && elbow_command_interface_running_ && !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_); + } else if (pose_cartesian_interface_running_ && elbow_command_interface_running_ && + !first_cartesian_pose_update_ && !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_pose_, hw_elbow_command_); + } else if (pose_cartesian_interface_running_ && !first_cartesian_pose_update_) { + // Wait until the first read pass after robot controller is activated to write the cartesian + // pose + robot_->writeOnce(hw_cartesian_pose_); } else if (velocity_cartesian_interface_running_ && !elbow_command_interface_running_) { robot_->writeOnce(hw_cartesian_velocities_); } @@ -284,6 +323,7 @@ hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_sw robot_->initializeJointPositionInterface(); position_joint_interface_running_ = true; first_position_update_ = true; + initial_joint_position_update_ = true; } else if (position_joint_interface_running_ && !position_joint_interface_claimed_) { robot_->stopRobot(); position_joint_interface_running_ = false; @@ -308,8 +348,30 @@ hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_sw velocity_cartesian_interface_running_ = false; } + if (!pose_cartesian_interface_running_ && pose_cartesian_interface_claimed_) { + robot_->stopRobot(); + robot_->initializeCartesianPoseInterface(); + if (!elbow_command_interface_running_ && elbow_command_interface_claimed_) { + elbow_command_interface_running_ = true; + first_elbow_update_ = true; + initial_elbow_state_update_ = true; + } + pose_cartesian_interface_running_ = true; + initial_robot_state_update_ = true; + first_cartesian_pose_update_ = true; + } else if (pose_cartesian_interface_running_ && !pose_cartesian_interface_claimed_) { + robot_->stopRobot(); + // Elbow command interface can't be commanded without cartesian pose or pose interface + if (elbow_command_interface_running_) { + elbow_command_interface_running_ = false; + elbow_command_interface_claimed_ = false; + } + pose_cartesian_interface_running_ = false; + } + // check if the elbow command is activated without cartesian command interface - if (elbow_command_interface_claimed_ && !velocity_cartesian_interface_claimed_) { + if (elbow_command_interface_claimed_ && + !(velocity_cartesian_interface_claimed_ || pose_cartesian_interface_claimed_)) { RCLCPP_FATAL(getLogger(), "Elbow cannot be commanded without cartesian velocity or pose interface"); return hardware_interface::return_type::ERROR; diff --git a/franka_hardware/src/robot.cpp b/franka_hardware/src/robot.cpp index 44253933..6471cd0e 100644 --- a/franka_hardware/src/robot.cpp +++ b/franka_hardware/src/robot.cpp @@ -49,7 +49,7 @@ Robot::~Robot() { franka::RobotState Robot::readOnce() { std::lock_guard lock(control_mutex_); - if (!isControlLoopActive()) { + if (!active_control_) { current_state_ = robot_->readOnce(); } else { current_state_ = readOnceActiveControl(); @@ -58,11 +58,12 @@ franka::RobotState Robot::readOnce() { } void Robot::stopRobot() { - if (isControlLoopActive()) { + if (active_control_) { effort_interface_active_ = false; joint_velocity_interface_active_ = false; joint_position_interface_active_ = false; cartesian_velocity_interface_active_ = false; + cartesian_pose_interface_active_ = false; active_control_.reset(); } } @@ -132,39 +133,49 @@ void Robot::writeOnceJointPositions(const std::array& positions) { active_control_->writeOnce(position_command); } -void Robot::preProcessCartesianVelocities(franka::CartesianVelocities& velocity_command) { - if (cartesian_velocity_low_pass_filter_active) { +franka::CartesianVelocities Robot::preProcessCartesianVelocities( + const franka::CartesianVelocities& velocity_command) { + franka::CartesianVelocities filtered_velocity_command = velocity_command; + if (cartesian_velocity_low_pass_filter_active_) { for (size_t i = 0; i < 6; i++) { - velocity_command.O_dP_EE.at(i) = + filtered_velocity_command.O_dP_EE.at(i) = franka::lowpassFilter(franka::kDeltaT, velocity_command.O_dP_EE.at(i), current_state_.O_dP_EE_c.at(i), low_pass_filter_cut_off_freq); } - if (velocity_command.hasElbow()) { - velocity_command.elbow[0] = - franka::lowpassFilter(franka::kDeltaT, velocity_command.elbow[0], - current_state_.elbow_c[0], low_pass_filter_cut_off_freq); - } } // If you are experiencing issues with robot error. You can try activating the rate // limiter. Rate limiter is default deactivated (cartesian_velocity_command_rate_limit_active_) if (cartesian_velocity_command_rate_limit_active_) { - velocity_command.O_dP_EE = franka::limitRate( + filtered_velocity_command.O_dP_EE = franka::limitRate( franka::kMaxTranslationalVelocity, franka::kMaxTranslationalAcceleration, franka::kMaxTranslationalJerk, franka::kMaxRotationalVelocity, franka::kMaxRotationalAcceleration, franka::kMaxRotationalJerk, velocity_command.O_dP_EE, current_state_.O_dP_EE_c, current_state_.O_ddP_EE_c); - if (velocity_command.hasElbow()) { - velocity_command.elbow[0] = franka::limitRate( - franka::kMaxElbowVelocity, franka::kMaxElbowAcceleration, franka::kMaxElbowJerk, - velocity_command.elbow[0], current_state_.elbow_c[0], current_state_.delbow_c[0], - current_state_.ddelbow_c[0]); - } } - franka::checkFinite(velocity_command.O_dP_EE); - if (velocity_command.hasElbow()) { - franka::checkElbow(velocity_command.elbow); + + return filtered_velocity_command; +} + +franka::CartesianPose Robot::preProcessCartesianPose(const franka::CartesianPose& cartesian_pose) { + franka::CartesianPose filtered_cartesian_pose = cartesian_pose; + + if (cartesian_pose_low_pass_filter_active_) { + filtered_cartesian_pose.O_T_EE = + franka::cartesianLowpassFilter(franka::kDeltaT, filtered_cartesian_pose.O_T_EE, + current_state_.O_T_EE_c, low_pass_filter_cut_off_freq); + } + + if (cartesian_pose_command_rate_limit_active_) { + filtered_cartesian_pose.O_T_EE = + franka::limitRate(franka::kMaxTranslationalVelocity, franka::kMaxTranslationalAcceleration, + franka::kMaxTranslationalJerk, franka::kMaxRotationalVelocity, + franka::kMaxRotationalAcceleration, franka::kMaxRotationalJerk, + filtered_cartesian_pose.O_T_EE, current_state_.O_T_EE_c, + current_state_.O_dP_EE_c, current_state_.O_ddP_EE_c); } + + return filtered_cartesian_pose; } void Robot::writeOnce(const std::array& cartesian_velocities) { @@ -175,9 +186,8 @@ void Robot::writeOnce(const std::array& cartesian_velocities) { std::lock_guard lock(control_mutex_); auto velocity_command = franka::CartesianVelocities(cartesian_velocities); - preProcessCartesianVelocities(velocity_command); - - active_control_->writeOnce(velocity_command); + auto filtered_velocity_command = preProcessCartesianVelocities(velocity_command); + active_control_->writeOnce(filtered_velocity_command); } void Robot::writeOnce(const std::array& cartesian_velocities, @@ -189,9 +199,37 @@ void Robot::writeOnce(const std::array& cartesian_velocities, std::lock_guard lock(control_mutex_); auto velocity_command = franka::CartesianVelocities(cartesian_velocities, elbow_command); - preProcessCartesianVelocities(velocity_command); + auto filtered_velocity_command = preProcessCartesianVelocities(velocity_command); - active_control_->writeOnce(velocity_command); + active_control_->writeOnce(filtered_velocity_command); +} + +void Robot::writeOnce(const std::array& cartesian_pose) { + if (!active_control_) { + throw std::runtime_error("Control hasn't been started"); + } + + std::lock_guard lock(control_mutex_); + + auto pose_command = franka::CartesianPose(cartesian_pose); + auto filtered_pose = preProcessCartesianPose(pose_command); + + active_control_->writeOnce(filtered_pose); +} + +void Robot::writeOnce(const std::array& cartesian_pose, + const std::array& elbow_command) { + if (!active_control_) { + throw std::runtime_error("Control hasn't been started"); + } + + std::lock_guard lock(control_mutex_); + + auto pose_command = franka::CartesianPose(cartesian_pose, elbow_command); + + auto filtered_pose = preProcessCartesianPose(pose_command); + + active_control_->writeOnce(filtered_pose); } franka::RobotState Robot::readOnceActiveControl() { @@ -252,9 +290,16 @@ void Robot::initializeCartesianVelocityInterface() { cartesian_velocity_interface_active_ = true; } -bool Robot::isControlLoopActive() { - return joint_position_interface_active_ || joint_velocity_interface_active_ || - effort_interface_active_ || cartesian_velocity_interface_active_; +void Robot::initializeCartesianPoseInterface() { + try { + active_control_ = robot_->startCartesianPoseControl( + research_interface::robot::Move::ControllerMode::kJointImpedance); + } catch (const franka::ControlException& e) { + robot_->automaticErrorRecovery(); + active_control_ = robot_->startCartesianPoseControl( + research_interface::robot::Move::ControllerMode::kJointImpedance); + } + cartesian_pose_interface_active_ = true; } 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 4ac9fe32..11a6868a 100644 --- a/franka_hardware/test/franka_hardware_cartesian_command_interface_test.cpp +++ b/franka_hardware/test/franka_hardware_cartesian_command_interface_test.cpp @@ -9,15 +9,16 @@ TEST_F(FrankaCartesianCommandInterfaceTest, cartesian_command_interface_number_i const auto command_interfaces = franka_hardware_interface.export_command_interfaces(); // cartesian velocity plus elbow command interfaces - const auto number_cartesian_velocity_command_interface = - k_hw_cartesian_velocities_names.size() + k_hw_elbow_command_names.size(); + const auto number_cartesian_velocity_command_interface = k_hw_cartesian_velocities_names.size() + + k_hw_elbow_command_names.size() + + k_hw_cartesian_pose_names.size(); ASSERT_EQ(command_interfaces.size(), number_cartesian_velocity_command_interface); } TEST_P( FrankaCartesianCommandInterfaceTest, - given_correct_number_of_start_cartesian_velocity_interface_when_prepare_command_mode_interface_is_called_expect_success) { + given_correct_number_of_start_cartesian_command_interface_when_prepare_command_mode_interface_is_called_expect_success) { auto [command_interfaces, command_interface_name] = GetParam(); auto mock_robot = std::make_shared(); @@ -47,7 +48,7 @@ TEST_P( TEST_P( FrankaCartesianCommandInterfaceTest, - given_correct_number_of_start_cartesian_velocity_and_elbow_command_interface_when_prepare_command_mode_interface_is_called_expect_success) { + given_correct_number_of_start_cartesian_command_and_elbow_command_interface_when_prepare_command_mode_interface_is_called_expect_success) { auto [command_interfaces, command_interface_name] = GetParam(); auto mock_robot = std::make_shared(); @@ -125,7 +126,7 @@ TEST_F(FrankaCartesianCommandInterfaceTest, EXPECT_CALL(*mock_robot, stopRobot()); EXPECT_CALL(*mock_robot, initializeCartesianVelocityInterface()); - 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)); @@ -162,7 +163,7 @@ TEST_F(FrankaCartesianCommandInterfaceTest, EXPECT_CALL(*mock_robot, initializeCartesianVelocityInterface()); EXPECT_CALL(*mock_robot, readOnce()).Times(1); - EXPECT_CALL(*mock_robot, writeOnce(_, _)).Times(1); + EXPECT_CALL(*mock_robot, writeOnce(std::array{}, _)).Times(1); EXPECT_CALL(*mock_robot, writeOnce(std::array{})).Times(0); franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot)); @@ -204,7 +205,7 @@ TEST_F( 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); EXPECT_CALL(*mock_robot, writeOnce(std::array{})).Times(0); franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot)); @@ -271,7 +272,7 @@ TEST_F(FrankaCartesianCommandInterfaceTest, TEST_F( FrankaCartesianCommandInterfaceTest, - given_only_elbow_command_is_claimed_withoud_cartesian_velocity_when_perform_mode_switch_is_called_expect_error) { + given_only_elbow_command_is_claimed_without_cartesian_velocity_when_perform_mode_switch_is_called_expect_error) { auto mock_robot = std::make_shared(); franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot)); @@ -285,9 +286,165 @@ TEST_F( std::vector stop_interface = {}; + ASSERT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface), + hardware_interface::return_type::OK); + ASSERT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface), + hardware_interface::return_type::ERROR); +} + +TEST_F( + FrankaCartesianCommandInterfaceTest, + given_cartesian_pose_interface_is_ready_when_write_called_without_read_robot_write_once_will_not_be_called) { + auto mock_robot = std::make_shared(); + + EXPECT_CALL(*mock_robot, stopRobot()); + EXPECT_CALL(*mock_robot, initializeCartesianPoseInterface()); + + EXPECT_CALL(*mock_robot, readOnce()).Times(0); + EXPECT_CALL(*mock_robot, writeOnce(std::array{}, _)).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_pose_names.size(); i++) { + const std::string name = k_hw_cartesian_pose_names[i]; + start_interface.push_back(name + "/" + k_cartesian_pose_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); + 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( + FrankaCartesianCommandInterfaceTest, + given_cartesian_pose_interface_is_ready_when_write_called_with_read_robot_write_once_will_be_called) { + auto mock_robot = std::make_shared(); + franka::RobotState robot_state; + robot_state.O_T_EE_d = std::array{0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, + 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0}; + + EXPECT_CALL(*mock_robot, stopRobot()); + EXPECT_CALL(*mock_robot, initializeCartesianPoseInterface()); + + EXPECT_CALL(*mock_robot, readOnce()).WillOnce(testing::Return(robot_state)); + EXPECT_CALL(*mock_robot, writeOnce(robot_state.O_T_EE_d)).Times(1); + + franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot)); + + std::vector start_interface; + + for (size_t i = 0; i < k_hw_cartesian_pose_names.size(); i++) { + const std::string name = k_hw_cartesian_pose_names[i]; + start_interface.push_back(name + "/" + k_cartesian_pose_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); + 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::OK); +} + +TEST_F( + FrankaCartesianCommandInterfaceTest, + given_cartesian_pose_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, initializeCartesianPoseInterface()); + + EXPECT_CALL(*mock_robot, readOnce()).WillOnce(testing::Return(robot_state)); + EXPECT_CALL(*mock_robot, writeOnce(std::array{}, _)).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_pose_names.size(); i++) { + const std::string name = k_hw_cartesian_pose_names[i]; + start_interface.push_back(name + "/" + k_cartesian_pose_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_pose_interface_is_ready_with_elbow_when_read_and_write_called_expect_correct_elbow_and_cartesian_pose) { + auto mock_robot = std::make_shared(); + franka::RobotState robot_state; + robot_state.O_T_EE_d = std::array{0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, + 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0}; + robot_state.elbow_c = std::array{0.0, 2.0}; + + EXPECT_CALL(*mock_robot, stopRobot()); + EXPECT_CALL(*mock_robot, initializeCartesianPoseInterface()); + + EXPECT_CALL(*mock_robot, readOnce()).WillOnce(testing::Return(robot_state)); + EXPECT_CALL(*mock_robot, writeOnce(robot_state.O_T_EE_d, robot_state.elbow_c)).Times(1); + + franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot)); + + std::vector start_interface; + + for (size_t i = 0; i < k_hw_cartesian_pose_names.size(); i++) { + const std::string name = k_hw_cartesian_pose_names[i]; + start_interface.push_back(name + "/" + k_cartesian_pose_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); + 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::OK); } \ No newline at end of file diff --git a/franka_hardware/test/franka_hardware_cartesian_command_interface_test.hpp b/franka_hardware/test/franka_hardware_cartesian_command_interface_test.hpp index 3028651a..f247c3f5 100644 --- a/franka_hardware/test/franka_hardware_cartesian_command_interface_test.hpp +++ b/franka_hardware/test/franka_hardware_cartesian_command_interface_test.hpp @@ -27,8 +27,8 @@ class FrankaCartesianCommandInterfaceTest : public ::testing::TestWithParam, std::string>> { protected: - // hardware_interface::HardwareInfo setUpHWInterfaces(); - + const std::vector k_hw_cartesian_pose_names{ + "0", "1", "2", "3", "4", "5", "6", "7", "8", "9", "10", "11", "12", "13", "14", "15"}; const std::vector k_hw_cartesian_velocities_names{"vx", "vy", "vz", "wx", "wy", "wz"}; const std::vector k_hw_elbow_command_names{"joint_3_position", "joint_4_sign"}; @@ -37,6 +37,7 @@ class FrankaCartesianCommandInterfaceTest const size_t k_number_of_joints{7}; const std::string k_cartesian_velocity_command_interface_name{"cartesian_velocity"}; + const std::string k_cartesian_pose_command_interface_name{"cartesian_pose"}; const std::string k_elbow_command_interface_name{"elbow_command"}; }; @@ -45,5 +46,9 @@ INSTANTIATE_TEST_SUITE_P( FrankaCartesianCommandInterfaceTest, ::testing::Values(std::make_tuple(std::vector{"vx", "vy", "vz", "wx", "wy", "wz"}, "cartesian_velocity"), + std::make_tuple(std::vector{"0", "1", "2", "3", "4", "5", "6", + "7", "8", "9", "10", "11", "12", + "13", "14", "15"}, + "cartesian_pose"), std::make_tuple(std::vector{"joint_3_position", "joint_4_sign"}, - "elbow_command"))); \ No newline at end of file + "elbow_command"))); diff --git a/franka_hardware/test/franka_hardware_interface_test.cpp b/franka_hardware/test/franka_hardware_interface_test.cpp index dbaec22c..633e714e 100644 --- a/franka_hardware/test/franka_hardware_interface_test.cpp +++ b/franka_hardware/test/franka_hardware_interface_test.cpp @@ -153,8 +153,10 @@ TEST_F( FrankaHardwareInterfaceTest, given_that_the_robot_interfaces_are_set_when_call_export_state_return_zero_values_and_correct_interface_names) { franka::RobotState robot_state; - const size_t state_interface_size = 23; // position, effort and velocity states for - // every joint + robot state and model + const size_t state_interface_size = + 48; // position, effort and velocity states for + // every joint + robot state and model// every joint + robot state and model + initial + // pose(16) + initial elbow(2) + position auto mock_robot = std::make_shared(); MockModel mock_model; MockModel* model_address = &mock_model; @@ -173,20 +175,22 @@ TEST_F( auto states = franka_hardware_interface.export_state_interfaces(); size_t joint_index = 0; - // Get all the states except the last two reserved for robot state - for (size_t i = 0; i < states.size() - 2; i++) { - if (i % 3 == 0) { + // Get all the states except the last two reserved for robot state + initial pose, elbow, position + for (size_t i = 0; i < states.size() - 20; i++) { + if (i % 4 == 0) { joint_index++; } const std::string joint_name = k_joint_name + std::to_string(joint_index); - if (i % 3 == 0) { + if (i % 4 == 0) { ASSERT_EQ(states[i].get_name(), joint_name + "/" + k_position_controller); - } else if (i % 3 == 1) { + } else if (i % 4 == 1) { ASSERT_EQ(states[i].get_name(), joint_name + "/" + k_velocity_controller); - } else { + } else if (i % 4 == 2) { ASSERT_EQ(states[i].get_name(), joint_name + "/" + k_effort_controller); - } - ASSERT_EQ(states[i].get_value(), 0.0); + } else if (i % 4 == 3) { + ASSERT_EQ(states[i].get_name(), joint_name + "/" + "initial_joint_position"); + } else + ASSERT_EQ(states[i].get_value(), 0.0); } ASSERT_EQ(states.size(), state_interface_size); @@ -196,8 +200,9 @@ TEST_F( FrankaHardwareInterfaceTest, given_that_the_robot_interfaces_are_set_when_call_export_state_interface_robot_model_interface_exists) { franka::RobotState robot_state; - const size_t state_interface_size = 23; // position, effort and velocity states for - // every joint + robot state and model + const size_t state_interface_size = 48; // position, effort and velocity states for + // every joint + robot state and model + initial pose(16) + // + initial elbow(2) + initial joint position(7) auto mock_robot = std::make_shared(); MockModel mock_model; @@ -214,9 +219,11 @@ TEST_F( auto return_type = franka_hardware_interface.read(time, duration); ASSERT_EQ(return_type, hardware_interface::return_type::OK); auto states = franka_hardware_interface.export_state_interfaces(); - 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(), + ASSERT_EQ(states[state_interface_size - 19].get_name(), + "panda/robot_model"); // Last state interface is the robot model state + + // initial_pose(16) + inital_elbow(2) + initial position(7) + EXPECT_NEAR(states[state_interface_size - 19] + .get_value(), // initial_pose(16), initial_pose(2) + initial position(7) *reinterpret_cast(&model_address), k_EPS); // testing that the casted mock_model ptr // is correctly pushed to state interface @@ -225,7 +232,7 @@ TEST_F( TEST_F( FrankaHardwareInterfaceTest, given_that_the_robot_interfaces_are_set_when_call_export_state_interface_robot_state_interface_exists) { - const size_t state_interface_size = 23; // position, effort and velocity states for + const size_t state_interface_size = 30; // position, effort and velocity states for // every joint + robot state and model auto mock_robot = std::make_shared(); diff --git a/franka_hardware/test/franka_robot_test.cpp b/franka_hardware/test/franka_robot_test.cpp index ee11abf1..f70f0141 100644 --- a/franka_hardware/test/franka_robot_test.cpp +++ b/franka_hardware/test/franka_robot_test.cpp @@ -34,6 +34,14 @@ TEST_F(FrankaRobotTests, robot.initializeCartesianVelocityInterface(); } +TEST_F(FrankaRobotTests, whenInitializeCartesianPoseInterfaceCalled_thenStartCartesianPoseControl) { + EXPECT_CALL(*mock_libfranka_robot, startCartesianPoseControl(testing::_)).Times(1); + + franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model)); + + robot.initializeCartesianPoseInterface(); +} + TEST_F(FrankaRobotTests, givenCartesianVelocityControlIsStarted_whenReadOnceIsCalled_expectCorrectRobotState) { franka::RobotState robot_state; @@ -53,6 +61,25 @@ TEST_F(FrankaRobotTests, ASSERT_EQ(robot_state.q_d, actual_state.q_d); } +TEST_F(FrankaRobotTests, + givenCartesianPoseControlIsStarted_whenReadOnceIsCalled_expectCorrectRobotState) { + franka::RobotState robot_state; + franka::Duration duration; + robot_state.q_d = std::array{1, 2, 3, 1, 2, 3, 1}; + auto active_control_read_return_tuple = std::make_pair(robot_state, duration); + + EXPECT_CALL(*mock_active_control, readOnce()) + .WillOnce(testing::Return(active_control_read_return_tuple)); + EXPECT_CALL(*mock_libfranka_robot, startCartesianPoseControl(testing::_)) + .WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control))))); + + franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model)); + + robot.initializeCartesianPoseInterface(); + auto actual_state = robot.readOnce(); + ASSERT_EQ(robot_state.q_d, actual_state.q_d); +} + TEST_F(FrankaRobotTests, givenJointControlIsNotStarted_whenReadOnceIsCalled_expectCorrectRobotState) { franka::RobotState robot_state; @@ -69,7 +96,7 @@ TEST_F(FrankaRobotTests, TEST_F( FrankaRobotTests, givenCartesianVelocityControlIsStarted_whenWriteOnceIsCalled_expectActiveControlWriteOnceCalled) { - const std::array& cartesian_velocities{1, 0, 0, 0, 0, 0}; + const std::array& cartesian_velocities{1, 2, 3, 1, 2, 3}; const franka::CartesianVelocities expected_cartesian_velocities(cartesian_velocities); auto expectCallFunction = [this]() { @@ -77,12 +104,28 @@ TEST_F( .WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control))))); }; - testWriteOnce>( + testReadWriteOnce>( &franka_hardware::Robot::initializeCartesianVelocityInterface, expectCallFunction, cartesian_velocities, expected_cartesian_velocities); } +TEST_F(FrankaRobotTests, + givenCartesianPoseControlIsStart_whenWriteOnceIsCalled_expectActiveControlWriteOnceCalled) { + const std::array& cartesian_pose{2, 0, 0, 0, 0, 2, 0, 0, 0, 0, 2, 0, 2, 3, 4, 1}; + const franka::CartesianPose expected_cartesian_pose(cartesian_pose); + + auto expectCallFunction = [this]() { + EXPECT_CALL(*mock_libfranka_robot, startCartesianPoseControl(testing::_)) + .WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control))))); + }; + + testReadWriteOnce>( + &franka_hardware::Robot::initializeCartesianPoseInterface, expectCallFunction, cartesian_pose, + expected_cartesian_pose); +} + TEST_F( FrankaRobotTests, givenJointPositionControlIsControlIsStarted_whenWriteOnceIsCalled_expectActiveControlWriteOnceCalled) { @@ -94,7 +137,8 @@ TEST_F( .WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control))))); }; - testWriteOnce>( + testReadWriteOnce>( &franka_hardware::Robot::initializeJointPositionInterface, expectCallFunction, joint_positions, expected_joint_positions); } @@ -110,7 +154,8 @@ TEST_F( .WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control))))); }; - testWriteOnce>( + testReadWriteOnce>( &franka_hardware::Robot::initializeJointVelocityInterface, expectCallFunction, joint_velocities, expected_joint_velocities); } @@ -126,24 +171,38 @@ TEST_F(FrankaRobotTests, .WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control))))); }; - testWriteOnce>( + testReadWriteOnce>( &franka_hardware::Robot::initializeTorqueInterface, expectCallFunction, joint_torques, expected_joint_torques); } TEST_F(FrankaRobotTests, - givenControlIsNotStart_whenWriteOnceIsCalled_expectRuntimeExceptionToBeThrown) { + givenControlIsNotStarted_whenWriteOnceIsCalled_expectRuntimeExceptionToBeThrown) { const std::array& joint_torques{1, 0, 0, 0, 0, 0, 0}; const franka::Torques joint_torques_franka(joint_torques); const std::array& cartesian_velocities{1, 0, 0, 0, 0, 0}; - const franka::CartesianVelocities cartesian_franka(cartesian_velocities); + const franka::CartesianVelocities cartesian_franka_velocities(cartesian_velocities); + + const std::array& cartesian_pose{1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; + const franka::CartesianPose expected_cartesian_pose(cartesian_pose); + + const std::array& elbow{0.0, 0.0}; + const franka::CartesianPose expected_cartesian_pose_with_elbow(cartesian_pose, elbow); + const franka::CartesianVelocities expected_cartesian_velocities_with_elbow(cartesian_velocities, + elbow); EXPECT_CALL(*mock_active_control, writeOnce(joint_torques_franka)).Times(0); - EXPECT_CALL(*mock_active_control, writeOnce(cartesian_franka)).Times(0); + EXPECT_CALL(*mock_active_control, writeOnce(cartesian_franka_velocities)).Times(0); + EXPECT_CALL(*mock_active_control, writeOnce(expected_cartesian_pose)).Times(0); + EXPECT_CALL(*mock_active_control, writeOnce(expected_cartesian_velocities_with_elbow)).Times(0); + EXPECT_CALL(*mock_active_control, writeOnce(expected_cartesian_pose_with_elbow)).Times(0); franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model)); EXPECT_THROW(robot.writeOnce(joint_torques), std::runtime_error); EXPECT_THROW(robot.writeOnce(cartesian_velocities), std::runtime_error); + EXPECT_THROW(robot.writeOnce(cartesian_velocities, elbow), std::runtime_error); + EXPECT_THROW(robot.writeOnce(cartesian_pose), std::runtime_error); + EXPECT_THROW(robot.writeOnce(cartesian_pose, elbow), std::runtime_error); } diff --git a/franka_hardware/test/franka_robot_test.hpp b/franka_hardware/test/franka_robot_test.hpp index d1fe6987..8a86fb67 100644 --- a/franka_hardware/test/franka_robot_test.hpp +++ b/franka_hardware/test/franka_robot_test.hpp @@ -73,6 +73,10 @@ bool operator==(const CartesianVelocities& lhs, const CartesianVelocities& rhs) return compareWithTolerance(lhs.O_dP_EE, rhs.O_dP_EE); } +bool operator==(const CartesianPose& lhs, const CartesianPose& rhs) { + return compareWithTolerance(lhs.O_T_EE, rhs.O_T_EE); +} + bool operator==(const JointPositions& lhs, const JointPositions& rhs) { return compareWithTolerance(lhs.q, rhs.q); } @@ -93,15 +97,16 @@ class FrankaRobotTests : public ::testing::Test { std::unique_ptr mock_active_control; template - void testWriteOnce(RobotInitFunction initFunction, - std::function expectCallFunction, - const RawControlInputType& control_input, - const ControlType& expected_active_control_input) { + void testReadWriteOnce(RobotInitFunction initFunction, + std::function expectCallFunction, + const RawControlInputType& control_input, + const ControlType& expected_active_control_input) { EXPECT_CALL(*mock_active_control, writeOnce(expected_active_control_input)); + EXPECT_CALL(*mock_active_control, readOnce()); expectCallFunction(); franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model)); - (robot.*initFunction)(); + robot.readOnce(); robot.writeOnce(control_input); } diff --git a/franka_hardware/test/test_utils.hpp b/franka_hardware/test/test_utils.hpp index 243327b8..3a4c0064 100644 --- a/franka_hardware/test/test_utils.hpp +++ b/franka_hardware/test/test_utils.hpp @@ -26,6 +26,7 @@ class MockRobot : public franka_hardware::Robot { public: MOCK_METHOD(void, initializeJointPositionInterface, (), (override)); MOCK_METHOD(void, initializeCartesianVelocityInterface, (), (override)); + MOCK_METHOD(void, initializeCartesianPoseInterface, (), (override)); MOCK_METHOD(void, initializeTorqueInterface, (), (override)); MOCK_METHOD(void, initializeJointVelocityInterface, (), (override)); MOCK_METHOD(void, stopRobot, (), (override)); @@ -38,7 +39,12 @@ class MockRobot : public franka_hardware::Robot { ((const std::array&)cartesian_velocity, (const std::array&)elbow_command), (override)); - // MOCK_METHOD(void, writeOnce, ((const std::array&)cartesian_velocity), (override)); + MOCK_METHOD(void, writeOnce, ((const std::array&)cartesian_pose), (override)); + MOCK_METHOD(void, + writeOnce, + ((const std::array&)cartesian_pose, + (const std::array&)elbow_command), + (override)); MOCK_METHOD(void, setJointStiffness, (const franka_msgs::srv::SetJointStiffness::Request::SharedPtr&), diff --git a/franka_moveit_config/package.xml b/franka_moveit_config/package.xml index 84a581da..a5067ae1 100644 --- a/franka_moveit_config/package.xml +++ b/franka_moveit_config/package.xml @@ -2,7 +2,7 @@ franka_moveit_config - 0.1.8 + 0.1.9 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 380aaad4..34ad7966 100644 --- a/franka_msgs/package.xml +++ b/franka_msgs/package.xml @@ -2,7 +2,7 @@ franka_msgs - 0.1.8 + 0.1.9 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 8097c674..0997c647 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.8 + 0.1.9 Broadcaster to publish robot states Franka Emika GmbH Apache 2.0 diff --git a/franka_semantic_components/CMakeLists.txt b/franka_semantic_components/CMakeLists.txt index 497981f3..65335f4f 100644 --- a/franka_semantic_components/CMakeLists.txt +++ b/franka_semantic_components/CMakeLists.txt @@ -21,13 +21,15 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) endforeach() find_package(Franka 0.13.0 REQUIRED) +find_package(Eigen3 REQUIRED) add_library(franka_semantic_components SHARED src/franka_robot_state.cpp src/franka_robot_model.cpp src/franka_cartesian_velocity_interface.cpp + src/franka_cartesian_pose_interface.cpp src/franka_semantic_component_interface.cpp) -target_include_directories(franka_semantic_components PRIVATE include) +target_include_directories(franka_semantic_components PRIVATE include ${EIGEN3_INCLUDE_DIRS}) target_link_libraries(franka_semantic_components Franka::Franka) ament_target_dependencies(franka_semantic_components Franka @@ -116,6 +118,22 @@ if(BUILD_TESTING) franka_cartesian_velocity_interface_test hardware_interface ) + + ament_add_gmock( + franka_cartesian_pose_interface_test + test/franka_cartesian_pose_interface_test.cpp + ) + + target_include_directories(franka_cartesian_pose_interface_test PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include ${EIGEN3_INCLUDE_DIRS}) + + target_link_libraries(franka_cartesian_pose_interface_test + franka_semantic_components + ) + + ament_target_dependencies( + franka_cartesian_pose_interface_test + hardware_interface + ) endif() ament_export_include_directories(include) diff --git a/franka_semantic_components/include/franka_semantic_components/franka_cartesian_pose_interface.hpp b/franka_semantic_components/include/franka_semantic_components/franka_cartesian_pose_interface.hpp new file mode 100644 index 00000000..08538fed --- /dev/null +++ b/franka_semantic_components/include/franka_semantic_components/franka_cartesian_pose_interface.hpp @@ -0,0 +1,171 @@ +// 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/control_types.h" +#include "franka/robot_state.h" +#include "franka_semantic_components/franka_semantic_component_interface.hpp" + +namespace franka_semantic_components { +/** + * @brief Franka Cartesian Pose interface abstraction on top of hardware_interface to set the + * full cartesian pose. The Command should either have the form of a 4x4 column major homogenous + * transformation matrix or a quaternion and translation vector. Optionally, the elbow can be + * commanded. [joint_3_position, joint_4_sign] + */ +class FrankaCartesianPoseInterface + : public FrankaSemanticComponentInterface { // NOLINT(cppcoreguidelines-special-member-functions) + public: + /** + * Initializes the franka cartesian velocity interface with access to the hardware + * interface command interfaces. + * + * @param[in] command_elbow_active insert true to activate the elbow commanding together with the + * cartesian velocity input, otherwise the elbow commanding is not allowed. + * + */ + explicit FrankaCartesianPoseInterface(bool command_elbow_activate); + FrankaCartesianPoseInterface(const FrankaCartesianPoseInterface&) = delete; + FrankaCartesianPoseInterface& operator=(const FrankaCartesianPoseInterface& other) = delete; + FrankaCartesianPoseInterface& operator=(FrankaCartesianPoseInterface&& other) = delete; + FrankaCartesianPoseInterface(FrankaCartesianPoseInterface&& other) = default; + + ~FrankaCartesianPoseInterface() override = default; + + /** + * Sets the given orientation and translation command, when elbow is not activated. + * + * @param[in] quaternion rotation represented in quaternion format [x,y,z,w] + * @param[in] translation translation represented in Vector3d format [x,y,z] + * + * @return if command was set successfully true, else when elbow is activated false. + */ + bool setCommand(const Eigen::Quaterniond& quaternion, const Eigen::Vector3d& translation); + + /** + * @brief Sets the given command. Based on rotation and translation and the elbow command. + * + * @param[in] quaternion rotation represented in quaternion format [x,y,z,w] + * @param[in] translation translation represented in Vector3d format [x,y,z] + * @param[in] elbow_command elbow format [joint_3_position, joint_4_sign] + * @return true when command was set successfully + * @return false when elbow is not activated + */ + bool setCommand(const Eigen::Quaterniond& quaternion, + const Eigen::Vector3d& translation, + const std::array& elbow_command); + + /** + * Sends the given pose command to the robot. + * + * @param[in] pose_command column major homogenous transformation matrix. + * + * @return true when command was set successfully + * @return false when elbow is activated + */ + bool setCommand(const std::array& pose_command); + + /** + * Sets the given command. + * + * @param[in] cartesian_pose_command Rotation plus translation commands in column major homogenous + * transformation matrix. + * @param[in] elbow elbow format [joint_3_position, joint_4_sign] + * + * @return true when command was set successfully + * @return false when elbow is not activated + */ + bool setCommand(const std::array& pose_command, const std::array& elbow); + + /** + * Get the commanded elbow interface elbow values. + + * @throws std::runtime_error if the elbow is not activated. + + * @return elbow_configuration [joint3_position, joint4_sign] + */ + std::array getCommandedElbowConfiguration(); + + /** + * Get the commanded pose interface values. + * + * @return pose_configuration commanded pose values in column major homogenous transformation + * + */ + std::array getCommandedPoseMatrix(); + + /** + * Get the commanded orientation and translation values. + * + * @return std::tuple commanded orientation values in + * quaternion format. Initial translation values in Vector3d format [x,y,z]. + */ + std::tuple getCommandedOrientationAndTranslation(); + + /** + * @brief Get the initial elbow configuration + * + * @throws std::runtime_error if the elbow is not activated. + * + * @return std::array elbow configuration [joint_3_position, joint_4_sign] + */ + std::array getInitialElbowConfiguration(); + + /** + * @brief Get the Initial Orientation And Translation + * + * @return std::tuple initial orientation values in + * quaternion format. Initial translation values in Vector3d format [x,y,z]. + */ + std::tuple getInitialOrientationAndTranslation(); + + /** + * @brief Get the initial pose matrix + * + * @return std::array Initial pose matrix column major homogenous transformation + */ + std::array getInitialPoseMatrix(); + + private: + /** + * @brief returns the column major transformation matrix from the given quaternion and translation + * + * @param quaternion rotation represented in quaternion format [x,y,z,w] + * @param translation translation represented in Vector3d format [x,y,z] + * @return std::vector column major transformation matrix [4x4] + */ + std::vector createColumnMajorTransformationMatrix(const Eigen::Quaterniond& quaternion, + const Eigen::Vector3d& translation); + + const std::array hw_elbow_names_{"joint_3_position", "joint_4_sign"}; + + const size_t full_command_interface_size_{18}; + const size_t command_interface_size_{16}; + const size_t elbow_command_interface_size_{2}; + + bool command_elbow_active_; + + const std::string cartesian_pose_command_interface_name_{"cartesian_pose"}; + const std::string elbow_command_interface_name_{"elbow_command"}; + const std::string cartesian_initial_pose_state_interface_name_{"initial_cartesian_pose"}; + const std::string elbow_initial_state_interface_name_{"initial_elbow_state"}; +}; + +} // namespace franka_semantic_components diff --git a/franka_semantic_components/include/franka_semantic_components/franka_cartesian_velocity_interface.hpp b/franka_semantic_components/include/franka_semantic_components/franka_cartesian_velocity_interface.hpp index ca3325e9..b318857f 100644 --- a/franka_semantic_components/include/franka_semantic_components/franka_cartesian_velocity_interface.hpp +++ b/franka_semantic_components/include/franka_semantic_components/franka_cartesian_velocity_interface.hpp @@ -42,6 +42,10 @@ class FrankaCartesianVelocityInterface : public FrankaSemanticComponentInterface * */ explicit FrankaCartesianVelocityInterface(bool command_elbow_activate); + FrankaCartesianVelocityInterface(const FrankaCartesianVelocityInterface&) = delete; + FrankaCartesianVelocityInterface& operator=(FrankaCartesianVelocityInterface const&) = delete; + FrankaCartesianVelocityInterface(FrankaCartesianVelocityInterface&&) = default; + FrankaCartesianVelocityInterface& operator=(FrankaCartesianVelocityInterface&&) = default; virtual ~FrankaCartesianVelocityInterface() = default; @@ -49,6 +53,8 @@ class FrankaCartesianVelocityInterface : public FrankaSemanticComponentInterface * Sets the given command. * * @param[in] command Command to set. + * + * @return if successful true, else when elbow is activated false. */ bool setCommand(const std::array& command); @@ -57,17 +63,26 @@ class FrankaCartesianVelocityInterface : public FrankaSemanticComponentInterface * * @param[in] cartesian_velocity_command Command to set. * @param[in] elbow Elbow to set. + * + * @return if successful true, else when elbow is not activated false. */ bool setCommand(const std::array& command, const std::array& elbow); /** * Get the commanded elbow interface elbow values. + *. + * @throws std::runtime_error if the elbow is not activated. + * + * @return elbow_configuration [joint3_position, joint4_sign] + */ + std::array getCommandedElbowConfiguration(); - * @param[in, out] elbow_configuration commanded elbow values. - - * @return true, if elbow is activated else false + /** + * @brief Get the initial elbow configuration when the controller started + * + * @return elbow_configuration [joint3_position, joint4_sign] */ - bool getCommandedElbowConfiguration(std::array& elbow_configuration); + std::array getInitialElbowConfiguration(); private: const std::array hw_cartesian_velocities_names_{"vx", "vy", "vz", @@ -79,6 +94,7 @@ class FrankaCartesianVelocityInterface : public FrankaSemanticComponentInterface const std::string cartesian_velocity_command_interface_name_{"cartesian_velocity"}; const std::string elbow_command_interface_name_{"elbow_command"}; + const std::string elbow_initial_state_interface_name_{"initial_elbow_state"}; }; } // namespace franka_semantic_components diff --git a/franka_semantic_components/include/franka_semantic_components/franka_semantic_component_interface.hpp b/franka_semantic_components/include/franka_semantic_components/franka_semantic_component_interface.hpp index 014370e5..2703ce41 100644 --- a/franka_semantic_components/include/franka_semantic_components/franka_semantic_component_interface.hpp +++ b/franka_semantic_components/include/franka_semantic_components/franka_semantic_component_interface.hpp @@ -41,6 +41,10 @@ class FrankaSemanticComponentInterface { explicit FrankaSemanticComponentInterface(const std::string& name, size_t state_interface_size = 0, size_t command_interface_size = 0); + FrankaSemanticComponentInterface(const FrankaSemanticComponentInterface&) = delete; + FrankaSemanticComponentInterface& operator=(FrankaSemanticComponentInterface const&) = delete; + FrankaSemanticComponentInterface(FrankaSemanticComponentInterface&&) = default; + FrankaSemanticComponentInterface& operator=(FrankaSemanticComponentInterface&&) = default; virtual ~FrankaSemanticComponentInterface() = default; @@ -92,35 +96,42 @@ class FrankaSemanticComponentInterface { /** * Return all values of the state interfaces. * - * \param[out] state_interface_values is overwritten with the state interface values - * - * \return true if it gets all the values, else false + * \return std::vector state_interface_values */ - bool get_values_state_interfaces(std::vector& state_interface_values) const; + std::vector get_values_state_interfaces() const; /** * Return all values for the command interfaces * - * \param[out] command_interface_values is overwritten with the comman interface values - * \return true if it gets all the values, else false + * \return std::vector command_interface_values */ - bool get_values_command_interfaces(std::vector& command_interface_values) const; + std::vector get_values_command_interfaces() const; /** * Set all values for the command interfaces * - * \return true if it gets all the values, else false + * \param[in] std::vector commanded values to be set + * + * \return true if commanded_values size matches the loaned command_interface size, else false */ bool set_values(const std::vector& commanded_values); protected: - std::string name_; - std::vector state_interface_names_; - std::vector command_interface_names_; - - std::vector> state_interfaces_; + std::string name_; // NOLINT(cppcoreguidelines-non-private-member-variables-in-classes, + // misc-non-private-member-variables-in-classes) + std::vector + state_interface_names_; // NOLINT(cppcoreguidelines-non-private-member-variables-in-classes, + // misc-non-private-member-variables-in-classes) + std::vector + command_interface_names_; // NOLINT(cppcoreguidelines-non-private-member-variables-in-classes, + // misc-non-private-member-variables-in-classes) + + std::vector> + state_interfaces_; // NOLINT(cppcoreguidelines-non-private-member-variables-in-classes, + // misc-non-private-member-variables-in-classes) std::vector> - command_interfaces_; + command_interfaces_; // NOLINT(cppcoreguidelines-non-private-member-variables-in-classes, + // misc-non-private-member-variables-in-classes) }; } // namespace franka_semantic_components diff --git a/franka_semantic_components/src/franka_cartesian_pose_interface.cpp b/franka_semantic_components/src/franka_cartesian_pose_interface.cpp new file mode 100644 index 00000000..67476e8a --- /dev/null +++ b/franka_semantic_components/src/franka_cartesian_pose_interface.cpp @@ -0,0 +1,222 @@ +// 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 "franka_semantic_components/franka_cartesian_pose_interface.hpp" + +#include +#include +#include "rclcpp/logging.hpp" + +#include + +namespace { +std::vector combineArraysToVector(const std::array& cartesian_pose_command, + const std::array& elbow_command) { + std::vector full_command; + full_command.reserve(cartesian_pose_command.size() + elbow_command.size()); + full_command.insert(full_command.end(), cartesian_pose_command.begin(), + cartesian_pose_command.end()); + full_command.insert(full_command.end(), elbow_command.begin(), elbow_command.end()); + + return full_command; +} +} // namespace + +namespace franka_semantic_components { + +FrankaCartesianPoseInterface::FrankaCartesianPoseInterface(bool command_elbow_active) + : FrankaSemanticComponentInterface("cartesian_pose_command", 16, 16), + command_elbow_active_(command_elbow_active) { + if (command_elbow_active_) { + command_interface_names_.reserve(full_command_interface_size_); + command_interfaces_.reserve(full_command_interface_size_); + state_interface_names_.reserve(full_command_interface_size_); + state_interfaces_.reserve(full_command_interface_size_); + } + + for (auto i = 0U; i < 16; i++) { + auto full_interface_name = std::to_string(i) + "/" + cartesian_pose_command_interface_name_; + auto state_interface_name = + std::to_string(i) + "/" + cartesian_initial_pose_state_interface_name_; + command_interface_names_.emplace_back(full_interface_name); + state_interface_names_.emplace_back(state_interface_name); + } + if (command_elbow_active_) { + for (const auto& elbow_name : hw_elbow_names_) { + auto full_elbow_command_name = elbow_name + "/" + elbow_command_interface_name_; + auto full_elbow_state_name = elbow_name + "/" + elbow_initial_state_interface_name_; + command_interface_names_.emplace_back(full_elbow_command_name); + state_interface_names_.emplace_back(full_elbow_state_name); + } + } +} + +std::vector FrankaCartesianPoseInterface::createColumnMajorTransformationMatrix( + const Eigen::Quaterniond& quaternion, + const Eigen::Vector3d& translation) { + Eigen::Matrix4d pose = Eigen::Matrix4d::Identity(); + pose.block<3, 3>(0, 0) = quaternion.toRotationMatrix(); + pose.block<3, 1>(0, 3) = translation; + + std::vector full_command; + + full_command.reserve(16); + + Eigen::Map(full_command.data(), pose.size()) = + Eigen::VectorXd::Map(pose.data(), pose.size()); + + return full_command; +} + +bool FrankaCartesianPoseInterface::setCommand(const Eigen::Quaterniond& quaternion, + const Eigen::Vector3d& translation) { + if (command_elbow_active_) { + RCLCPP_ERROR(rclcpp::get_logger("franka_cartesian_velocity_interface"), + "Elbow command interface must not claimed, if elbow is not commanded."); + return false; + } + + auto full_command = createColumnMajorTransformationMatrix(quaternion, translation); + + return set_values(full_command); +} + +bool FrankaCartesianPoseInterface::setCommand(const Eigen::Quaterniond& quaternion, + const Eigen::Vector3d& translation, + const std::array& elbow_command) { + if (!command_elbow_active_) { + RCLCPP_ERROR(rclcpp::get_logger("franka_cartesian_pose_interface"), + "Elbow command interface must be claimed to command elbow."); + return false; + } + + auto pose = createColumnMajorTransformationMatrix(quaternion, translation); + + auto pose_array = std::array(); + std::copy_n(pose.begin(), pose_array.size(), pose_array.begin()); + + auto full_command = combineArraysToVector(pose_array, elbow_command); + + return set_values(full_command); +} + +bool FrankaCartesianPoseInterface::setCommand(const std::array& cartesian_pose_command, + const std::array& elbow_command) { + if (!command_elbow_active_) { + RCLCPP_ERROR(rclcpp::get_logger("franka_cartesian_pose_interface"), + "Elbow command interface must be claimed to command elbow."); + return false; + } + auto full_command = combineArraysToVector(cartesian_pose_command, elbow_command); + + return set_values(full_command); +} + +bool FrankaCartesianPoseInterface::setCommand( + const std::array& cartesian_pose_command) { + if (command_elbow_active_) { + RCLCPP_ERROR(rclcpp::get_logger("franka_cartesian_velocity_interface"), + "Elbow command interface must not claimed, if elbow is not commanded. If elbow is " + "activated, Use " + "setCommand(pose_command, elbow_command) interface."); + return false; + } + + std::vector full_command; + + full_command.insert(full_command.end(), cartesian_pose_command.begin(), + cartesian_pose_command.end()); + + return set_values(full_command); +} + +std::array FrankaCartesianPoseInterface::getCommandedPoseMatrix() { + std::vector full_configuration; + std::array pose_configuration{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + + full_configuration = get_values_command_interfaces(); + + std::copy_n(full_configuration.begin(), pose_configuration.size(), pose_configuration.begin()); + return pose_configuration; +}; + +std::array FrankaCartesianPoseInterface::getCommandedElbowConfiguration() { + if (!command_elbow_active_) { + throw std::runtime_error( + "Elbow command interface must be claimed to receive elbow command state."); + } + std::array elbow_configuration{0, 0}; + auto full_configuration = get_values_command_interfaces(); + + std::copy_n(full_configuration.begin() + + command_interface_size_, // NOLINT(cppcoreguidelines-narrowing-conversions) + elbow_command_interface_size_, elbow_configuration.begin()); + + return elbow_configuration; +}; + +std::array FrankaCartesianPoseInterface::getInitialPoseMatrix() { + std::array initial_pose{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + + auto pose_configuration = get_values_state_interfaces(); + + std::copy_n(pose_configuration.begin(), command_interface_size_, initial_pose.begin()); + + return initial_pose; +} + +std::tuple +FrankaCartesianPoseInterface::getInitialOrientationAndTranslation() { + std::array initial_pose{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + + auto pose_configuration = get_values_state_interfaces(); + std::copy_n(pose_configuration.begin(), command_interface_size_, initial_pose.begin()); + + Eigen::Matrix4d pose = + Eigen::Map>(initial_pose.data()); + Eigen::Quaterniond quaternion = Eigen::Quaterniond(pose.block<3, 3>(0, 0)); + Eigen::Vector3d translation = pose.block<3, 1>(0, 3); + + return std::make_tuple(quaternion, translation); +}; + +std::tuple +FrankaCartesianPoseInterface::getCommandedOrientationAndTranslation() { + std::array pose_matrix = getCommandedPoseMatrix(); + + Eigen::Matrix4d pose = + Eigen::Map>(pose_matrix.data()); + + Eigen::Quaterniond quaternion = Eigen::Quaterniond(pose.block<3, 3>(0, 0)); + Eigen::Vector3d translation = pose.block<3, 1>(0, 3); + + return std::make_tuple(quaternion, translation); +} + +std::array FrankaCartesianPoseInterface::getInitialElbowConfiguration() { + if (!command_elbow_active_) { + throw std::runtime_error("Elbow command interface must be claimed to receive elbow state."); + } + + std::array elbow_configuration{0, 0}; + auto full_configuration = get_values_state_interfaces(); + + std::copy_n(full_configuration.begin() + + command_interface_size_, // NOLINT(cppcoreguidelines-narrowing-conversions) + elbow_command_interface_size_, elbow_configuration.begin()); + + return elbow_configuration; +} + +} // namespace franka_semantic_components \ No newline at end of file diff --git a/franka_semantic_components/src/franka_cartesian_velocity_interface.cpp b/franka_semantic_components/src/franka_cartesian_velocity_interface.cpp index d9b2ba44..43cf167e 100644 --- a/franka_semantic_components/src/franka_cartesian_velocity_interface.cpp +++ b/franka_semantic_components/src/franka_cartesian_velocity_interface.cpp @@ -40,6 +40,8 @@ FrankaCartesianVelocityInterface::FrankaCartesianVelocityInterface(bool command_ if (command_elbow_active_) { command_interface_names_.reserve(full_command_interface_size_); command_interfaces_.reserve(full_command_interface_size_); + state_interface_names_.reserve(hw_elbow_command_names_.size()); + state_interfaces_.reserve(hw_elbow_command_names_.size()); } for (const auto& velocity_command_name : hw_cartesian_velocities_names_) { @@ -50,16 +52,14 @@ FrankaCartesianVelocityInterface::FrankaCartesianVelocityInterface(bool command_ if (command_elbow_active_) { for (const auto& elbow_command_name : hw_elbow_command_names_) { auto full_elbow_command_name = elbow_command_name + "/" + elbow_command_interface_name_; + auto full_initial_elbow_state_name = + elbow_command_name + "/" + elbow_initial_state_interface_name_; command_interface_names_.emplace_back(full_elbow_command_name); + state_interface_names_.emplace_back(full_initial_elbow_state_name); } } } -/** - * Sets the given command. - * - * @param[in] command Command to set. - */ bool FrankaCartesianVelocityInterface::setCommand( const std::array& cartesian_velocity_command, const std::array& elbow_command) { @@ -90,22 +90,31 @@ bool FrankaCartesianVelocityInterface::setCommand( return set_values(full_command); } -bool FrankaCartesianVelocityInterface::getCommandedElbowConfiguration( - std::array& elbow_configuration) { +std::array FrankaCartesianVelocityInterface::getCommandedElbowConfiguration() { if (!command_elbow_active_) { - return false; + throw std::runtime_error( + "Elbow command interface must be claimed to receive elbow command state."); } - std::vector full_configuration; + std::array elbow_configuration; + auto full_configuration = get_values_command_interfaces(); - full_configuration.reserve(full_command_interface_size_); + std::copy_n(full_configuration.begin() + hw_cartesian_velocities_names_.size(), + hw_elbow_command_names_.size(), elbow_configuration.begin()); - get_values_command_interfaces(full_configuration); + return elbow_configuration; +}; - for (size_t i = 0; i < hw_elbow_command_names_.size(); i++) { - elbow_configuration[i] = full_configuration[i + hw_cartesian_velocities_names_.size()]; +std::array FrankaCartesianVelocityInterface::getInitialElbowConfiguration() { + if (!command_elbow_active_) { + throw std::runtime_error("Elbow command interface must be claimed to receive elbow state."); } - return true; + std::array elbow_configuration; + auto full_configuration = get_values_state_interfaces(); + std::copy_n(full_configuration.begin(), hw_elbow_command_names_.size(), + elbow_configuration.begin()); + + return elbow_configuration; }; -} // namespace franka_semantic_components \ No newline at end of file +} // namespace franka_semantic_components diff --git a/franka_semantic_components/src/franka_semantic_component_interface.cpp b/franka_semantic_components/src/franka_semantic_component_interface.cpp index 075db7ec..17481f08 100644 --- a/franka_semantic_components/src/franka_semantic_component_interface.cpp +++ b/franka_semantic_components/src/franka_semantic_component_interface.cpp @@ -65,30 +65,23 @@ std::vector FrankaSemanticComponentInterface::get_command_interface return command_interface_names_; } -bool FrankaSemanticComponentInterface::get_values_state_interfaces( - std::vector& state_interface_values) const { - // check we have sufficient memory - if (state_interface_values.capacity() != state_interfaces_.size()) { - return false; - } +std::vector FrankaSemanticComponentInterface::get_values_state_interfaces() const { + std::vector state_interface_values; // insert all the state_interface_values for (const auto& state_interface : state_interfaces_) { state_interface_values.emplace_back(state_interface.get().get_value()); } - return true; + return state_interface_values; } -bool FrankaSemanticComponentInterface::get_values_command_interfaces( - std::vector& command_interface_values) const { - // check we have sufficient memory - if (command_interface_values.capacity() != command_interfaces_.size()) { - return false; - } +std::vector FrankaSemanticComponentInterface::get_values_command_interfaces() const { + std::vector command_interface_values; // insert all the command_interface_values for (const auto& command_interface : command_interfaces_) { command_interface_values.emplace_back(command_interface.get().get_value()); } - return true; + + return command_interface_values; } bool FrankaSemanticComponentInterface::set_values(const std::vector& values) { diff --git a/franka_semantic_components/test/franka_cartesian_pose_interface_test.cpp b/franka_semantic_components/test/franka_cartesian_pose_interface_test.cpp new file mode 100644 index 00000000..9d7005f5 --- /dev/null +++ b/franka_semantic_components/test/franka_cartesian_pose_interface_test.cpp @@ -0,0 +1,315 @@ +// 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 "franka_cartesian_pose_interface_test.hpp" + +#include +#include +#include +#include + +void FrankaCartesianPoseTest::TearDown() { + franka_cartesian_command_friend.reset(nullptr); +} + +void FrankaCartesianPoseTest::constructFrankaCartesianPoseInterface(bool elbow_activate) { + franka_cartesian_command_friend = std::make_unique(elbow_activate); + setUpHWCommandInterfaces(elbow_activate); + setUpHWStateInterfaces(elbow_activate); +} + +void FrankaCartesianPoseTest::setUpHWStateInterfaces(bool elbow_activate) { + temp_state_interfaces.clear(); + + for (auto i = 0U; i < hw_cartesian_pose_.size(); i++) { + pose_state_interfaces_container.push_back( + std::make_shared(hardware_interface::StateInterface{ + std::to_string(i), cartesian_initial_pose_state_interface_name_, + &initial_cartesian_pose_.at(i)})); + } + for (auto& pose_state_interface : pose_state_interfaces_container) { + temp_state_interfaces.emplace_back(*pose_state_interface.get()); + } + if (elbow_activate) { + for (auto i = 0U; i < hw_elbow_command_names_.size(); i++) { + elbow_state_interfaces_container.push_back( + std::make_shared(hardware_interface::StateInterface{ + hw_elbow_command_names_[i], initial_elbow_state_interface_name_, + &initial_elbow_state_.at(i)})); + } + for (auto& elbow_state_interface : elbow_state_interfaces_container) { + temp_state_interfaces.emplace_back(*elbow_state_interface.get()); + } + } + + franka_cartesian_command_friend->assign_loaned_state_interfaces(temp_state_interfaces); +} + +void FrankaCartesianPoseTest::setUpHWCommandInterfaces(bool elbow_activate) { + temp_command_interfaces.clear(); + + for (auto i = 0U; i < hw_cartesian_pose_.size(); i++) { + pose_command_interfaces_container.push_back( + std::make_shared(hardware_interface::CommandInterface{ + std::to_string(i), cartesian_pose_command_interface_name_, &hw_cartesian_pose_.at(i)})); + } + for (auto& pose_command_interface : pose_command_interfaces_container) { + temp_command_interfaces.emplace_back(*pose_command_interface.get()); + } + if (elbow_activate) { + for (auto i = 0U; i < hw_elbow_command_names_.size(); i++) { + elbow_command_interfaces_container.push_back( + std::make_shared( + hardware_interface::CommandInterface{hw_elbow_command_names_[i], + elbow_command_interface_name_, + &hw_elbow_command_.at(i)})); + } + for (auto& elbow_command_interface : elbow_command_interfaces_container) { + temp_command_interfaces.emplace_back(*elbow_command_interface.get()); + } + } + franka_cartesian_command_friend->assign_loaned_command_interfaces(temp_command_interfaces); +} + +TEST_F(FrankaCartesianPoseTest, + given_correct_interfaces_set_carte_with_elbow_command_expect_successful) { + constructFrankaCartesianPoseInterface(true); + std::array new_hw_cartesian_pose{0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, + 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 8.0}; + std::array new_elbow_command{0.0, 2.0}; + + auto success = + franka_cartesian_command_friend->setCommand(new_hw_cartesian_pose, new_elbow_command); + + ASSERT_TRUE(success); + + ASSERT_EQ(hw_cartesian_pose_, new_hw_cartesian_pose); + ASSERT_EQ(hw_elbow_command_, new_elbow_command); + + franka_cartesian_command_friend->release_interfaces(); +} + +TEST_F(FrankaCartesianPoseTest, + given_correct_interfaces_when_get_elbow_command_values_called_expect_successful) { + constructFrankaCartesianPoseInterface(true); + std::array new_hw_cartesian_pose{0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, + 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 8.0}; + std::array new_elbow_command{0.0, 2.0}; + + auto success_set_command = + franka_cartesian_command_friend->setCommand(new_hw_cartesian_pose, new_elbow_command); + + ASSERT_TRUE(success_set_command); + + ASSERT_EQ(hw_cartesian_pose_, new_hw_cartesian_pose); + ASSERT_EQ(hw_elbow_command_, new_elbow_command); + + std::array elbow_configuration; + elbow_configuration = franka_cartesian_command_friend->getCommandedElbowConfiguration(); + + ASSERT_EQ(new_elbow_command, elbow_configuration); + + franka_cartesian_command_friend->release_interfaces(); +} + +TEST_F(FrankaCartesianPoseTest, + given_elbow_is_not_activated_when_elbow_command_get_value_is_called_expect_throw) { + constructFrankaCartesianPoseInterface(false); + std::array new_hw_cartesian_pose{0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, + 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 8.0}; + + auto success_set_command = franka_cartesian_command_friend->setCommand(new_hw_cartesian_pose); + + ASSERT_TRUE(success_set_command); + + ASSERT_EQ(hw_cartesian_pose_, new_hw_cartesian_pose); + + ASSERT_THROW(franka_cartesian_command_friend->getCommandedElbowConfiguration(), + std::runtime_error); + + franka_cartesian_command_friend->release_interfaces(); +} + +TEST_F(FrankaCartesianPoseTest, + given_elbow_and_cartesian_pose_claimed_when_set_pose_command_without_elbow_expect_failure) { + constructFrankaCartesianPoseInterface(true); + std::array new_hw_cartesian_pose{0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, + 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 8.0}; + + auto success = franka_cartesian_command_friend->setCommand(new_hw_cartesian_pose); + + ASSERT_FALSE(success); + + franka_cartesian_command_friend->release_interfaces(); +} + +TEST_F( + FrankaCartesianPoseTest, + given_only_cartesian_velocity_claimed_without_elbow_when_set_velocity_command_cartesian_vel_expect_successful) { + constructFrankaCartesianPoseInterface(false); + std::array new_hw_cartesian_pose{0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, + 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 8.0}; + std::array default_zero_elbow_command{0.0, 0.0}; + + auto success = franka_cartesian_command_friend->setCommand(new_hw_cartesian_pose, + default_zero_elbow_command); + + ASSERT_FALSE(success); + + franka_cartesian_command_friend->release_interfaces(); +} + +TEST_F(FrankaCartesianPoseTest, + given_incorrect_command_interfaces_set_velocity_expect_unsuccesful) { + franka_cartesian_command_friend = std::make_unique(true); + hardware_interface::CommandInterface dummy_command_interface{"dummy", "dummy_cartesian_pose", + &hw_elbow_command_.at(0)}; + std::array new_hw_cartesian_pose{0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, + 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 8.0}; + + temp_command_interfaces.emplace_back(dummy_command_interface); + franka_cartesian_command_friend->assign_loaned_command_interfaces(temp_command_interfaces); + + auto success = franka_cartesian_command_friend->setCommand(new_hw_cartesian_pose); + + ASSERT_FALSE(success); + + franka_cartesian_command_friend->release_interfaces(); +} + +TEST_F( + FrankaCartesianPoseTest, + given_correct_interfaces_when_set_command_called_with_quaternion_and_translation_expect_the_correct_pose) { + constructFrankaCartesianPoseInterface(false); + Eigen::Quaterniond quaternion(0.0, 1.0, 2.0, 3.0); + Eigen::Vector3d translation(4.0, 5.0, 6.0); + std::array expected_command{-25, 4, 6, 0, 4, -19, 12, 0, 6, 12, -9, 0, 4, 5, 6, 1}; + + auto success = franka_cartesian_command_friend->setCommand(quaternion, translation); + + ASSERT_TRUE(success); + + for (auto i = 0U; i < expected_command.size(); i++) { + ASSERT_NEAR(expected_command.at(i), hw_cartesian_pose_.at(i), 1e-6); + } + ASSERT_EQ(hw_cartesian_pose_, expected_command); + + franka_cartesian_command_friend->release_interfaces(); +} + +TEST_F( + FrankaCartesianPoseTest, + given_correct_interfaces_when_set_command_called_with_quaternion_and_translation_and_elbow_expect_the_correct_pose) { + constructFrankaCartesianPoseInterface(true); + Eigen::Quaterniond quaternion(0.0, 1.0, 2.0, 3.0); + Eigen::Vector3d translation(4.0, 5.0, 6.0); + std::array elbow{0.0, 1.0}; + + std::array expected_command{-25, 4, 6, 0, 4, -19, 12, 0, 6, 12, -9, 0, 4, 5, 6, 1}; + + auto success = franka_cartesian_command_friend->setCommand(quaternion, translation, elbow); + + ASSERT_TRUE(success); + + for (auto i = 0U; i < expected_command.size(); i++) { + ASSERT_NEAR(expected_command.at(i), hw_cartesian_pose_.at(i), 1e-6); + } + + ASSERT_EQ(hw_elbow_command_, elbow); + + franka_cartesian_command_friend->release_interfaces(); +} + +TEST_F(FrankaCartesianPoseTest, + given_correct_interfaces_when_get_commanded_pose_called_expect_the_correct_pose) { + constructFrankaCartesianPoseInterface(false); + std::array received_pose; + + received_pose = franka_cartesian_command_friend->getCommandedPoseMatrix(); + ASSERT_EQ(hw_cartesian_pose_, received_pose); + + auto [received_quaternion, received_translation] = + franka_cartesian_command_friend->getCommandedOrientationAndTranslation(); + + Eigen::Matrix4d pose = + Eigen::Map>(hw_cartesian_pose_.data()); + + Eigen::Quaterniond quaternion = Eigen::Quaterniond(pose.block<3, 3>(0, 0)); + Eigen::Vector3d translation = pose.block<3, 1>(0, 3); + + ASSERT_EQ(received_quaternion, quaternion); + ASSERT_EQ(received_translation, translation); + + franka_cartesian_command_friend->release_interfaces(); +} + +TEST_F(FrankaCartesianPoseTest, + given_correct_interfaces_when_get_elbow_commanded_called_expect_the_correct_elbow) { + constructFrankaCartesianPoseInterface(true); + std::array elbow_configuration; + + elbow_configuration = franka_cartesian_command_friend->getCommandedElbowConfiguration(); + + ASSERT_EQ(hw_elbow_command_, elbow_configuration); + + franka_cartesian_command_friend->release_interfaces(); +} + +TEST_F(FrankaCartesianPoseTest, + given_correct_interfaces_when_initial_elbow_request_expect_correct_state) { + constructFrankaCartesianPoseInterface(true); + std::array initial_elbow_configuration; + + initial_elbow_configuration = franka_cartesian_command_friend->getInitialElbowConfiguration(); + + ASSERT_EQ(initial_elbow_state_, initial_elbow_configuration); + + franka_cartesian_command_friend->release_interfaces(); +} + +TEST_F(FrankaCartesianPoseTest, + given_elbow_not_activated_when_initial_elbow_requested_expect_throw) { + constructFrankaCartesianPoseInterface(false); + + ASSERT_THROW(franka_cartesian_command_friend->getInitialElbowConfiguration(), std::runtime_error); + + franka_cartesian_command_friend->release_interfaces(); +} + +TEST_F(FrankaCartesianPoseTest, + given_correct_interfaces_when_initial_orientation_request_expect_correct_pose) { + constructFrankaCartesianPoseInterface(true); + auto [received_orientation, received_translation] = + franka_cartesian_command_friend->getInitialOrientationAndTranslation(); + + Eigen::Matrix4d pose = + Eigen::Map>(initial_cartesian_pose_.data()); + + Eigen::Quaterniond expected_orientation = Eigen::Quaterniond(pose.block<3, 3>(0, 0)); + Eigen::Vector3d expected_translation = pose.block<3, 1>(0, 3); + + ASSERT_EQ(received_orientation, expected_orientation); + ASSERT_EQ(received_translation, expected_translation); + + franka_cartesian_command_friend->release_interfaces(); +} + +TEST_F(FrankaCartesianPoseTest, given_correct_interfaces_when_pose_requested_expect_correct_pose) { + constructFrankaCartesianPoseInterface(true); + auto received_pose = franka_cartesian_command_friend->getInitialPoseMatrix(); + + ASSERT_EQ(received_pose, initial_cartesian_pose_); + + franka_cartesian_command_friend->release_interfaces(); +} \ No newline at end of file diff --git a/franka_semantic_components/test/franka_cartesian_pose_interface_test.hpp b/franka_semantic_components/test/franka_cartesian_pose_interface_test.hpp new file mode 100644 index 00000000..a10c3c26 --- /dev/null +++ b/franka_semantic_components/test/franka_cartesian_pose_interface_test.hpp @@ -0,0 +1,67 @@ +// 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 "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" + +#include "franka_semantic_components/franka_cartesian_pose_interface.hpp" +#include "gmock/gmock.h" + +class FrankaCartesianPoseTestFriend + : public franka_semantic_components::FrankaCartesianPoseInterface { + public: + explicit FrankaCartesianPoseTestFriend(const bool elbow_activate) + : franka_semantic_components::FrankaCartesianPoseInterface(elbow_activate) {} + + virtual ~FrankaCartesianPoseTestFriend() = default; +}; + +class FrankaCartesianPoseTest : public ::testing::Test { + public: + void constructFrankaCartesianPoseInterface(bool elbow_active); + void TearDown(); + + private: + void setUpHWCommandInterfaces(bool elbow_active); + void setUpHWStateInterfaces(bool elbow_active); + + protected: + std::vector> + pose_command_interfaces_container, elbow_command_interfaces_container; + std::vector> pose_state_interfaces_container, + elbow_state_interfaces_container; + std::array hw_elbow_command_names_{"joint_3_position", "joint_4_sign"}; + + std::array hw_cartesian_pose_{0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, + 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0}; + std::array hw_elbow_command_{0.0, 0.0}; + + std::array initial_cartesian_pose_{1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}; + std::array initial_elbow_state_{1.0, 1.0}; + + std::unique_ptr franka_cartesian_command_friend; + std::vector temp_command_interfaces; + std::vector temp_state_interfaces; + + const std::string cartesian_pose_command_interface_name_{"cartesian_pose"}; + const std::string elbow_command_interface_name_{"elbow_command"}; + const std::string cartesian_initial_pose_state_interface_name_{"initial_cartesian_pose"}; + const std::string initial_elbow_state_interface_name_{"initial_elbow_state"}; +}; diff --git a/franka_semantic_components/test/franka_cartesian_velocity_interface_test.cpp b/franka_semantic_components/test/franka_cartesian_velocity_interface_test.cpp index c9222721..174eb789 100644 --- a/franka_semantic_components/test/franka_cartesian_velocity_interface_test.cpp +++ b/franka_semantic_components/test/franka_cartesian_velocity_interface_test.cpp @@ -22,9 +22,33 @@ void FrankaCartesianVelocityTest::TearDown() { franka_cartesian_command_friend.reset(nullptr); } -void FrankaCartesianVelocityTest::setUpHWCommandInterfaces(bool elbow_activate) { +void FrankaCartesianVelocityTest::setUpInterfaces(bool elbow_activate) { franka_cartesian_command_friend = std::make_unique(elbow_activate); + setUpHWStateInterfaces(elbow_activate); + setUpHWCommandInterfaces(elbow_activate); +} + +void FrankaCartesianVelocityTest::setUpHWStateInterfaces(bool elbow_activate) { + temp_state_interfaces.clear(); + + if (elbow_activate) { + for (auto i = 0U; i < hw_elbow_command_names_.size(); i++) { + elbow_state_interfaces_container.push_back( + std::make_shared(hardware_interface::StateInterface{ + hw_elbow_command_names_[i], elbow_initial_state_interface_name_, + &initial_elbow_state_.at(i)})); + } + + for (auto& elbow_state_interface : elbow_state_interfaces_container) { + temp_state_interfaces.emplace_back(*elbow_state_interface.get()); + } + + franka_cartesian_command_friend->assign_loaned_state_interfaces(temp_state_interfaces); + } +} + +void FrankaCartesianVelocityTest::setUpHWCommandInterfaces(bool elbow_activate) { temp_command_interfaces.clear(); for (auto i = 0U; i < hw_cartesian_velocities_names_.size(); i++) { @@ -33,26 +57,30 @@ void FrankaCartesianVelocityTest::setUpHWCommandInterfaces(bool elbow_activate) hw_cartesian_velocities_names_[i], cartesian_velocity_command_interface_name_, &hw_cartesian_velocities_.at(i)})); } - for (auto i = 0U; i < hw_elbow_command_names_.size(); i++) { - elbow_command_interfaces_container.push_back( - std::make_shared(hardware_interface::CommandInterface{ - hw_elbow_command_names_[i], elbow_command_interface_name_, &hw_elbow_command_.at(i)})); + + if (elbow_activate) { + for (auto i = 0U; i < hw_elbow_command_names_.size(); i++) { + elbow_command_interfaces_container.push_back( + std::make_shared( + hardware_interface::CommandInterface{hw_elbow_command_names_[i], + elbow_command_interface_name_, + &hw_elbow_command_.at(i)})); + } + for (auto& elbow_command_interface : elbow_command_interfaces_container) { + temp_command_interfaces.emplace_back(*elbow_command_interface.get()); + } } for (auto& velocity_command_interface : velocity_command_interfaces_container) { temp_command_interfaces.emplace_back(*velocity_command_interface.get()); } - for (auto& elbow_command_interface : elbow_command_interfaces_container) { - temp_command_interfaces.emplace_back(*elbow_command_interface.get()); - } - franka_cartesian_command_friend->assign_loaned_command_interfaces(temp_command_interfaces); } TEST_F(FrankaCartesianVelocityTest, given_correct_interfaces_set_velocity_with_elbow_command_expect_successful) { - setUpHWCommandInterfaces(true); + setUpInterfaces(true); std::array new_hw_cartesian_velocities{0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; std::array new_elbow_command{0.0, 2.0}; @@ -69,7 +97,7 @@ TEST_F(FrankaCartesianVelocityTest, TEST_F(FrankaCartesianVelocityTest, given_correct_interfaces_when_get_elbow_command_values_called_expect_successful) { - setUpHWCommandInterfaces(true); + setUpInterfaces(true); std::array new_hw_cartesian_velocities{0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; std::array new_elbow_command{0.0, 2.0}; @@ -82,20 +110,17 @@ TEST_F(FrankaCartesianVelocityTest, ASSERT_EQ(hw_elbow_command_, new_elbow_command); std::array elbow_configuration; - auto success_elbow_configuration = - franka_cartesian_command_friend->getCommandedElbowConfiguration(elbow_configuration); + elbow_configuration = franka_cartesian_command_friend->getCommandedElbowConfiguration(); - ASSERT_TRUE(success_elbow_configuration); ASSERT_EQ(new_elbow_command, elbow_configuration); franka_cartesian_command_friend->release_interfaces(); } TEST_F(FrankaCartesianVelocityTest, - given_elbow_is_not_activated_when_elbow_command_get_value_is_called_expect_failure) { - setUpHWCommandInterfaces(false); + given_elbow_is_not_activated_when_elbow_command_get_value_is_called_expect_throw) { + setUpInterfaces(false); std::array new_hw_cartesian_velocities{0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; - // std::array new_elbow_command{0.0, 2.0}; auto success_set_command = franka_cartesian_command_friend->setCommand(new_hw_cartesian_velocities); @@ -104,12 +129,8 @@ TEST_F(FrankaCartesianVelocityTest, ASSERT_EQ(hw_cartesian_velocities_, new_hw_cartesian_velocities); - std::array elbow_configuration; - auto success_elbow_configuration = - franka_cartesian_command_friend->getCommandedElbowConfiguration(elbow_configuration); - - ASSERT_FALSE(success_elbow_configuration); - // ASSERT_EQ(new_elbow_command, elbow_configuration); + ASSERT_THROW(franka_cartesian_command_friend->getCommandedElbowConfiguration(), + std::runtime_error); franka_cartesian_command_friend->release_interfaces(); } @@ -117,7 +138,7 @@ TEST_F(FrankaCartesianVelocityTest, TEST_F( FrankaCartesianVelocityTest, given_elbow_and_cartesian_velocity_claimed_when_set_velocity_command_without_elbow_expect_failure) { - setUpHWCommandInterfaces(true); + setUpInterfaces(true); std::array new_hw_cartesian_velocities_{0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; auto success = franka_cartesian_command_friend->setCommand(new_hw_cartesian_velocities_); @@ -130,7 +151,7 @@ TEST_F( TEST_F( FrankaCartesianVelocityTest, given_only_cartesian_velocity_claimed_without_elbow_when_set_velocity_command_cartesian_vel_expect_successful) { - setUpHWCommandInterfaces(false); + setUpInterfaces(false); std::array new_hw_cartesian_velocities_{0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; std::array default_zero_elbow_command{0.0, 0.0}; @@ -157,4 +178,19 @@ TEST_F(FrankaCartesianVelocityTest, ASSERT_FALSE(success); franka_cartesian_command_friend->release_interfaces(); +} + +TEST_F(FrankaCartesianVelocityTest, + given_correct_interface_when_initial_elbow_state_requested_expect_correct) { + setUpInterfaces(true); + + auto received_elbow_state = franka_cartesian_command_friend->getInitialElbowConfiguration(); + ASSERT_EQ(received_elbow_state, initial_elbow_state_); +} + +TEST_F(FrankaCartesianVelocityTest, + given_correct_interface_when_initial_elbow_state_requested_expect_throw) { + setUpInterfaces(false); + + ASSERT_THROW(franka_cartesian_command_friend->getInitialElbowConfiguration(), std::runtime_error); } \ No newline at end of file diff --git a/franka_semantic_components/test/franka_cartesian_velocity_interface_test.hpp b/franka_semantic_components/test/franka_cartesian_velocity_interface_test.hpp index a53ebe25..1a4147d4 100644 --- a/franka_semantic_components/test/franka_cartesian_velocity_interface_test.hpp +++ b/franka_semantic_components/test/franka_cartesian_velocity_interface_test.hpp @@ -33,22 +33,30 @@ class FrankaCartesianVelocityTestFriend class FrankaCartesianVelocityTest : public ::testing::Test { public: - void setUpHWCommandInterfaces(bool elbow_active); - + void setUpInterfaces(bool elbow_activate); void TearDown(); + private: + void setUpHWCommandInterfaces(bool elbow_active); + void setUpHWStateInterfaces(bool elbow_active); + protected: std::vector> velocity_command_interfaces_container, elbow_command_interfaces_container; + + std::vector> elbow_state_interfaces_container; std::array hw_cartesian_velocities_names_{"vx", "vy", "vz", "wx", "wy", "wz"}; std::array hw_elbow_command_names_{"joint_3_position", "joint_4_sign"}; std::array hw_cartesian_velocities_{0.0, 1.0, 2.0, 3.0, 4.0, 5.0}; std::array hw_elbow_command_{0.0, 0.0}; + std::array initial_elbow_state_{1.0, 1.0}; std::unique_ptr franka_cartesian_command_friend; std::vector temp_command_interfaces; + std::vector temp_state_interfaces; const std::string cartesian_velocity_command_interface_name_{"cartesian_velocity"}; const std::string elbow_command_interface_name_{"elbow_command"}; + const std::string elbow_initial_state_interface_name_{"initial_elbow_state"}; }; diff --git a/franka_semantic_components/test/franka_semantic_interface_test.cpp b/franka_semantic_components/test/franka_semantic_interface_test.cpp index 7054341e..0565a170 100644 --- a/franka_semantic_components/test/franka_semantic_interface_test.cpp +++ b/franka_semantic_components/test/franka_semantic_interface_test.cpp @@ -168,11 +168,7 @@ TEST_F(SemanticComponentInterfaceTest, validate_state_interfaces) { // validate the values of state_interfaces_ which should be // in order as per interface_names_ std::vector temp_values; - ASSERT_FALSE(semantic_component_->get_values_state_interfaces(temp_values)); - - // reserve memory for the vector and call get_values - temp_values.reserve(3); - ASSERT_TRUE(semantic_component_->get_values_state_interfaces(temp_values)); + temp_values = semantic_component_->get_values_state_interfaces(); ASSERT_EQ(temp_values, interface_values); // release the state_interfaces_ @@ -220,6 +216,9 @@ TEST_F(SemanticComponentInterfaceTest, validate_command_interfaces) { // validate the count of command_interfaces_ ASSERT_EQ(semantic_component_->command_interfaces_.size(), 3u); + auto command_values = semantic_component_->get_values_command_interfaces(); + ASSERT_EQ(command_values, interface_values); + // validate the values of command_interfaces_ which should be // in order as per interface_names_ std::vector temp_values; @@ -240,4 +239,4 @@ TEST_F(SemanticComponentInterfaceTest, validate_command_interfaces) { ASSERT_TRUE(std::equal(semantic_component_->command_interface_names_.begin(), semantic_component_->command_interface_names_.end(), command_interface_names.begin(), command_interface_names.end())); -} \ No newline at end of file +} diff --git a/integration_launch_testing/package.xml b/integration_launch_testing/package.xml index fbdab53f..663e7bdf 100644 --- a/integration_launch_testing/package.xml +++ b/integration_launch_testing/package.xml @@ -2,7 +2,7 @@ integration_launch_testing - 0.1.8 + 0.1.9 Functional integration tests for franka controllers Franka Emika GmbH Apache 2.0