From f36d0a26b8347f59ff4776dbdaba2ff9c3ca115b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Thu, 1 Aug 2024 15:43:20 +0200 Subject: [PATCH 01/49] add forward wrench controller to kuka controllers --- controllers/kuka_controllers/package.xml | 1 + controllers/wrench_controller/CHANGELOG.rst | 16 ++++ controllers/wrench_controller/CMakeLists.txt | 69 ++++++++++++++ .../wrench_controller/controller_plugins.xml | 7 ++ .../wrench_controller/visibility_control.h | 49 ++++++++++ .../wrench_controller/wrench_controller.hpp | 47 ++++++++++ controllers/wrench_controller/package.xml | 22 +++++ .../src/wrench_controller.cpp | 91 +++++++++++++++++++ .../src/wrench_controller_parameters.yaml | 11 +++ 9 files changed, 313 insertions(+) create mode 100644 controllers/wrench_controller/CHANGELOG.rst create mode 100644 controllers/wrench_controller/CMakeLists.txt create mode 100644 controllers/wrench_controller/controller_plugins.xml create mode 100644 controllers/wrench_controller/include/wrench_controller/visibility_control.h create mode 100644 controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp create mode 100644 controllers/wrench_controller/package.xml create mode 100644 controllers/wrench_controller/src/wrench_controller.cpp create mode 100644 controllers/wrench_controller/src/wrench_controller_parameters.yaml diff --git a/controllers/kuka_controllers/package.xml b/controllers/kuka_controllers/package.xml index d17e7244..d200989a 100644 --- a/controllers/kuka_controllers/package.xml +++ b/controllers/kuka_controllers/package.xml @@ -13,6 +13,7 @@ fri_configuration_controller fri_state_broadcaster joint_group_impedance_controller + wrench_controller ament_cmake diff --git a/controllers/wrench_controller/CHANGELOG.rst b/controllers/wrench_controller/CHANGELOG.rst new file mode 100644 index 00000000..5870f82b --- /dev/null +++ b/controllers/wrench_controller/CHANGELOG.rst @@ -0,0 +1,16 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package wrench_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.9.2 (2024-07-10) +------------------ +* Fix GCC warning causing unstable build + +0.9.1 (2024-07-08) +------------------ +* Add missing test dependency + +0.9.0 (2024-07-08) +------------------ +* Add controller for updating stiffness and damping command interfaces +* Contributors: Aron Svastits diff --git a/controllers/wrench_controller/CMakeLists.txt b/controllers/wrench_controller/CMakeLists.txt new file mode 100644 index 00000000..6ef324fe --- /dev/null +++ b/controllers/wrench_controller/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.5) +project(wrench_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(pluginlib REQUIRED) +find_package(forward_command_controller) +find_package(kuka_drivers_core) +find_package(generate_parameter_library) + +include_directories(include) + +generate_parameter_library( + wrench_controller_parameters + src/wrench_controller_parameters.yaml +) + +add_library(${PROJECT_NAME} SHARED + src/wrench_controller.cpp) + +target_include_directories(${PROJECT_NAME} PRIVATE + include +) + +ament_target_dependencies(${PROJECT_NAME} forward_command_controller kuka_drivers_core +) +target_link_libraries(${PROJECT_NAME} wrench_controller_parameters) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "WRENCH_CONTROLLER_BUILDING_LIBRARY") +# prevent pluginlib from using boost +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include +) + +install(FILES controller_plugins.xml + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + +endif() + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + +ament_export_include_directories( + include +) + +ament_export_libraries( + ${PROJECT_NAME} +) + +ament_package() diff --git a/controllers/wrench_controller/controller_plugins.xml b/controllers/wrench_controller/controller_plugins.xml new file mode 100644 index 00000000..8162e891 --- /dev/null +++ b/controllers/wrench_controller/controller_plugins.xml @@ -0,0 +1,7 @@ + + + + This controller sends the wrench commands in real time + + + diff --git a/controllers/wrench_controller/include/wrench_controller/visibility_control.h b/controllers/wrench_controller/include/wrench_controller/visibility_control.h new file mode 100644 index 00000000..2cf8ba57 --- /dev/null +++ b/controllers/wrench_controller/include/wrench_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2023 Aron Svastits +// +// 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. + +#ifndef WRENCH_CONTROLLER__VISIBILITY_CONTROL_H_ +#define WRENCH_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define WRENCH_CONTROLLER_EXPORT __attribute__((dllexport)) +#define WRENCH_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define WRENCH_CONTROLLER_EXPORT __declspec(dllexport) +#define WRENCH_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef WRENCH_CONTROLLER_BUILDING_LIBRARY +#define WRENCH_CONTROLLER_PUBLIC WRENCH_CONTROLLER_EXPORT +#else +#define WRENCH_CONTROLLER_PUBLIC WRENCH_CONTROLLER_IMPORT +#endif +#define WRENCH_CONTROLLER_PUBLIC_TYPE WRENCH_CONTROLLER_PUBLIC +#define WRENCH_CONTROLLER_LOCAL +#else +#define WRENCH_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define WRENCH_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define WRENCH_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define WRENCH_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define WRENCH_CONTROLLER_PUBLIC +#define WRENCH_CONTROLLER_LOCAL +#endif +#define WRENCH_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // WRENCH_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp b/controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp new file mode 100644 index 00000000..5fd9e689 --- /dev/null +++ b/controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp @@ -0,0 +1,47 @@ +// Copyright 2022 Aron Svastits +// +// 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. + +#ifndef WRENCH_CONTROLLER__WRENCH_CONTROLLER_HPP_ +#define WRENCH_CONTROLLER__WRENCH_CONTROLLER_HPP_ + +#include +#include +#include + +#include "forward_command_controller/multi_interface_forward_command_controller.hpp" + +#include "wrench_controller/visibility_control.h" +#include "wrench_controller_parameters.hpp" + +namespace kuka_controllers +{ +class WrenchController : public forward_command_controller::ForwardControllersBase +{ +public: + WRENCH_CONTROLLER_PUBLIC WrenchController(); + WRENCH_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; + +private: + WRENCH_CONTROLLER_LOCAL void declare_parameters() override; + WRENCH_CONTROLLER_LOCAL controller_interface::CallbackReturn read_parameters() + override; + + using Params = wrench_controller::Params; + using ParamListener = wrench_controller::ParamListener; + + std::shared_ptr param_listener_; + Params params_; +}; +} // namespace kuka_controllers +#endif // WRENCH_CONTROLLER__WRENCH_CONTROLLER_HPP_ diff --git a/controllers/wrench_controller/package.xml b/controllers/wrench_controller/package.xml new file mode 100644 index 00000000..b80e5d13 --- /dev/null +++ b/controllers/wrench_controller/package.xml @@ -0,0 +1,22 @@ + + + + wrench_controller + 0.9.2 + Controller for forwarding wrench commands + + Mihaly Kristofi + + Apache-2.0 + + ament_cmake + + forward_command_controller + pluginlib + kuka_drivers_core + generate_parameter_library + + + ament_cmake + + diff --git a/controllers/wrench_controller/src/wrench_controller.cpp b/controllers/wrench_controller/src/wrench_controller.cpp new file mode 100644 index 00000000..8527c0d6 --- /dev/null +++ b/controllers/wrench_controller/src/wrench_controller.cpp @@ -0,0 +1,91 @@ +// Copyright 2022 Aron Svastits +// +// 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 "pluginlib/class_list_macros.hpp" + +#include "kuka_drivers_core/hardware_interface_types.hpp" + +#include "wrench_controller/wrench_controller.hpp" + +namespace kuka_controllers +{ + +WrenchController::WrenchController() : ForwardControllersBase() {} + +void WrenchController::declare_parameters() +{ + param_listener_ = std::make_shared(get_node()); +} + +controller_interface::CallbackReturn WrenchController::read_parameters() +{ + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); + + if (params_.joints.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } + + if (params_.interface_names.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'interfaces' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } + + for (const auto & joint : params_.joints) + { + for (const auto & interface : params_.interface_names) + { + command_interface_types_.push_back(joint + "/" + interface); + } + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn WrenchController::on_init() +{ + auto ret = ForwardControllersBase::on_init(); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + + try + { + // Explicitly set the interfaces parameter declared by the + // forward_command_controller + get_node()->set_parameter(rclcpp::Parameter( + "interface_names", + std::vector{ + "x", "y", "z"})); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} +} // namespace kuka_controllers + +PLUGINLIB_EXPORT_CLASS( + kuka_controllers::WrenchController, controller_interface::ControllerInterface) diff --git a/controllers/wrench_controller/src/wrench_controller_parameters.yaml b/controllers/wrench_controller/src/wrench_controller_parameters.yaml new file mode 100644 index 00000000..9c3831c8 --- /dev/null +++ b/controllers/wrench_controller/src/wrench_controller_parameters.yaml @@ -0,0 +1,11 @@ +wrench_controller: + joints: { + type: string_array, + default_value: [], + description: "Name of the joints to control", + } + interface_names: { + type: string_array, + default_value: [], + description: "Names of the interfaces to command", + } From c2a52c4bf73e1ef881bfcd29e7761dbda89aeb79 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Thu, 1 Aug 2024 15:58:32 +0200 Subject: [PATCH 02/49] add cartesian impedance controller for stifness/damping values --- .../kuka_drivers_core/controller_names.hpp | 2 + ...cartesian_impedance_controller_config.yaml | 9 +++ .../config/driver_config.yaml | 2 + .../config/ros2_controller_config.yaml | 2 + .../robot_manager_node.hpp | 6 +- .../launch/startup.launch.py | 11 +++ .../src/hardware_interface.cpp | 36 +++++++++ .../src/robot_manager_node.cpp | 79 ++++++++++++++++++- 8 files changed, 142 insertions(+), 5 deletions(-) create mode 100644 kuka_sunrise_fri_driver/config/cartesian_impedance_controller_config.yaml diff --git a/kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp b/kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp index 41a0bcf6..955674e6 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp @@ -27,6 +27,8 @@ static constexpr char FRI_STATE_BROADCASTER[] = "fri_state_broadcaster"; // Controller names with default values static constexpr char JOINT_TRAJECTORY_CONTROLLER[] = "joint_trajectory_controller"; static constexpr char JOINT_GROUP_IMPEDANCE_CONTROLLER[] = "joint_group_impedance_controller"; +static constexpr char CARTESIAN_IMPEDANCE_CONTROLLER[] = "cartesian_impedance_controller"; + } // namespace kuka_drivers_core #endif // KUKA_DRIVERS_CORE__CONTROLLER_NAMES_HPP_ diff --git a/kuka_sunrise_fri_driver/config/cartesian_impedance_controller_config.yaml b/kuka_sunrise_fri_driver/config/cartesian_impedance_controller_config.yaml new file mode 100644 index 00000000..d5a5daee --- /dev/null +++ b/kuka_sunrise_fri_driver/config/cartesian_impedance_controller_config.yaml @@ -0,0 +1,9 @@ +cartesian_impedance_controller: + ros__parameters: + joints: + - CartDOF.X + - CartDOF.Y + - CartDOF.Z + - CartDOF.A + - CartDOF.B + - CartDOF.C diff --git a/kuka_sunrise_fri_driver/config/driver_config.yaml b/kuka_sunrise_fri_driver/config/driver_config.yaml index 8d550527..14d0787b 100644 --- a/kuka_sunrise_fri_driver/config/driver_config.yaml +++ b/kuka_sunrise_fri_driver/config/driver_config.yaml @@ -7,3 +7,5 @@ robot_manager: send_period_ms: 10 joint_damping: [0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7] joint_stiffness: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0] + cartesian_damping: [0.7, 0.7, 0.7, 0.7, 0.7, 0.7] + cartesian_stiffness: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0] diff --git a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml b/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml index d340c2b7..597ddead 100755 --- a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml @@ -16,6 +16,8 @@ controller_manager: # Configuration controllers joint_group_impedance_controller: type: kuka_controllers/JointGroupImpedanceController + cartesian_impedance_controller: + type: kuka_controllers/JointGroupImpedanceController fri_configuration_controller: type: kuka_controllers/FRIConfigurationController control_mode_handler: diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp index 3c192467..5eb4267b 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp @@ -70,6 +70,7 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode rclcpp::Publisher::SharedPtr fri_config_pub_; rclcpp::Publisher::SharedPtr control_mode_pub_; rclcpp::Publisher::SharedPtr joint_imp_pub_; + rclcpp::Publisher::SharedPtr cart_imp_pub_; rclcpp::Subscription::SharedPtr event_subscriber_; std_msgs::msg::UInt32 control_mode_msg_; @@ -80,7 +81,8 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode std::string joint_torque_controller_name_; std::vector joint_stiffness_ = std::vector(7, 100.0); std::vector joint_damping_ = std::vector(7, 0.7); - + std::vector cartesian_stiffness_ = {2000.0, 2000.0, 2000.0, 200.0, 200.0, 200.0}; + std::vector cartesian_damping_ = std::vector(6, 0.7); std::string GetControllerName() const; bool onControlModeChangeRequest(int control_mode); bool onRobotModelChangeRequest(const std::string & robot_model); @@ -92,6 +94,8 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode std::string_view controller_name, kuka_drivers_core::ControllerType controller_type); bool onJointDampingChangeRequest(const std::vector & joint_damping); bool onJointStiffnessChangeRequest(const std::vector & joint_stiffness); + bool onCartesianStiffnessChangeRequest(const std::vector & cartesian_stiffness); + bool onCartesianDampingChangeRequest(const std::vector & cartesian_damping); void setFriConfiguration(int send_period_ms, int receive_multiplier) const; void EventSubscriptionCallback(const std_msgs::msg::UInt8::SharedPtr msg); diff --git a/kuka_sunrise_fri_driver/launch/startup.launch.py b/kuka_sunrise_fri_driver/launch/startup.launch.py index 7ab660f0..7d4cf6e9 100644 --- a/kuka_sunrise_fri_driver/launch/startup.launch.py +++ b/kuka_sunrise_fri_driver/launch/startup.launch.py @@ -39,6 +39,8 @@ def launch_setup(context, *args, **kwargs): jtc_config = LaunchConfiguration("jtc_config") jic_config = LaunchConfiguration("jic_config") ec_config = LaunchConfiguration("ec_config") + cic_config = LaunchConfiguration("cic_config") + if ns.perform(context) == "": tf_prefix = "" else: @@ -116,6 +118,7 @@ def launch_setup(context, *args, **kwargs): jtc_config, jic_config, ec_config, + cic_config, { "hardware_components_initial_state": { "unconfigured": [tf_prefix + robot_model.perform(context)] @@ -166,6 +169,7 @@ def controller_spawner(controller_names, activate=False): "effort_controller", "control_mode_handler", "event_broadcaster", + "cartesian_impedance_controller", ] controller_spawners = [controller_spawner(name) for name in controller_names] @@ -222,4 +226,11 @@ def generate_launch_description(): + "/config/effort_controller_config.yaml", ) ) + launch_arguments.append( + DeclareLaunchArgument( + "cic_config", + default_value=get_package_share_directory("kuka_sunrise_fri_driver") + + "/config/cartesian_impedance_controller_config.yaml", + ) + ) return LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 4557e65a..5edb7b86 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -460,6 +460,42 @@ KukaFRIHardwareInterface::export_command_interfaces() command_interfaces.emplace_back( info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_torque_commands_[i]); } + command_interfaces.emplace_back( + std::string("CartDOF") + "." + std::string("X"), + hardware_interface::HW_IF_STIFFNESS, &hw_stiffness_commands_[0]); + command_interfaces.emplace_back( + std::string("CartDOF") + "." + std::string("Y"), + hardware_interface::HW_IF_STIFFNESS, &hw_stiffness_commands_[1]); + command_interfaces.emplace_back( + std::string("CartDOF") + "." + std::string("Z"), + hardware_interface::HW_IF_STIFFNESS, &hw_stiffness_commands_[2]); + command_interfaces.emplace_back( + std::string("CartDOF") + "." + std::string("A"), + hardware_interface::HW_IF_STIFFNESS, &hw_stiffness_commands_[3]); + command_interfaces.emplace_back( + std::string("CartDOF") + "." + std::string("B"), + hardware_interface::HW_IF_STIFFNESS, &hw_stiffness_commands_[4]); + command_interfaces.emplace_back( + std::string("CartDOF") + "." + std::string("C"), + hardware_interface::HW_IF_STIFFNESS, &hw_stiffness_commands_[5]); + command_interfaces.emplace_back( + std::string("CartDOF") + "." + std::string("X"), + hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[0]); + command_interfaces.emplace_back( + std::string("CartDOF") + "." + std::string("Y"), + hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[1]); + command_interfaces.emplace_back( + std::string("CartDOF") + "." + std::string("Z"), + hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[2]); + command_interfaces.emplace_back( + std::string("CartDOF") + "." + std::string("A"), + hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[3]); + command_interfaces.emplace_back( + std::string("CartDOF") + "." + std::string("B"), + hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[4]); + command_interfaces.emplace_back( + std::string("CartDOF") + "." + std::string("C"), + hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[5]); return command_interfaces; } diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index 85466a23..2b96ead9 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -59,6 +59,9 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ joint_imp_pub_ = this->create_publisher( "joint_group_impedance_controller/commands", rclcpp::SystemDefaultsQoS()); + + cart_imp_pub_ = this->create_publisher( + "cartesian_impedance_controller/commands", rclcpp::SystemDefaultsQoS()); rclcpp::SubscriptionOptions sub_options; sub_options.callback_group = event_cbg_; @@ -115,6 +118,16 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ "joint_damping", joint_damping_, kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const std::vector & joint_damping) { return this->onJointDampingChangeRequest(joint_damping); }); + + registerParameter>( + "cartesian_stiffness", cartesian_stiffness_, kuka_drivers_core::ParameterSetAccessRights{true, false}, + [this](const std::vector & cartesian_stiffness) + { return this->onCartesianStiffnessChangeRequest(cartesian_stiffness); }); + + registerParameter>( + "cartesian_damping", cartesian_damping_, kuka_drivers_core::ParameterSetAccessRights{true, false}, + [this](const std::vector & cartesian_damping) + { return this->onCartesianDampingChangeRequest(cartesian_damping); }); } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn @@ -139,7 +152,8 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) if (!kuka_drivers_core::changeControllerState( change_controller_state_client_, {kuka_drivers_core::FRI_CONFIGURATION_CONTROLLER, kuka_drivers_core::CONTROL_MODE_HANDLER, - kuka_drivers_core::EVENT_BROADCASTER, kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER}, + kuka_drivers_core::EVENT_BROADCASTER, kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER, + kuka_drivers_core::CARTESIAN_IMPEDANCE_CONTROLLER}, {})) { RCLCPP_ERROR(get_logger(), "Could not activate configuration controllers"); @@ -161,7 +175,8 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) if (!kuka_drivers_core::changeControllerState( change_controller_state_client_, {}, {kuka_drivers_core::FRI_CONFIGURATION_CONTROLLER, kuka_drivers_core::CONTROL_MODE_HANDLER, - kuka_drivers_core::EVENT_BROADCASTER, kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER}, + kuka_drivers_core::EVENT_BROADCASTER, kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER, + kuka_drivers_core::CARTESIAN_IMPEDANCE_CONTROLLER}, SwitchController::Request::BEST_EFFORT)) { RCLCPP_ERROR(get_logger(), "Could not stop controllers"); @@ -201,6 +216,15 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) } joint_imp_pub_->publish(joint_imp_msg); + // Publish the values of the cartesian impedance parameters to the controller + std_msgs::msg::Float64MultiArray cart_imp_msg; + for (int i = 0; i < 6; i++) + { + cart_imp_msg.data.push_back(cartesian_stiffness_[i]); + cart_imp_msg.data.push_back(cartesian_damping_[i]); + } + cart_imp_pub_->publish(cart_imp_msg); + // Activate hardware interface if (!kuka_drivers_core::changeHardwareState( change_hardware_state_client_, robot_model_, @@ -218,7 +242,8 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) change_controller_state_client_, {kuka_drivers_core::JOINT_STATE_BROADCASTER, kuka_drivers_core::FRI_STATE_BROADCASTER, GetControllerName()}, - {kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER})) + {kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER, + kuka_drivers_core::CARTESIAN_IMPEDANCE_CONTROLLER})) { RCLCPP_ERROR(get_logger(), "Could not activate RT controllers"); this->on_deactivate(get_current_state()); @@ -243,7 +268,7 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) // Stop RT controllers // With best effort strictness, deactivation succeeds if specific controller is not active if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER}, + change_controller_state_client_, {kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER, kuka_drivers_core::CARTESIAN_IMPEDANCE_CONTROLLER}, {GetControllerName(), kuka_drivers_core::JOINT_STATE_BROADCASTER, kuka_drivers_core::FRI_STATE_BROADCASTER}, SwitchController::Request::BEST_EFFORT)) @@ -456,6 +481,52 @@ bool RobotManagerNode::onJointDampingChangeRequest(const std::vector & j return true; } +bool RobotManagerNode::onCartesianStiffnessChangeRequest(const std::vector & cartesian_stiffness) +{ + if (cartesian_stiffness.size() != 6) + { + RCLCPP_ERROR(get_logger(), "Invalid parameter array length for parameter cartesian stiffness"); + return false; + } + for (std::size_t i=0; i<3; i++){ + if (cartesian_stiffness[i] < 0 || cartesian_stiffness[i] > 5000) + { + RCLCPP_ERROR(get_logger(), "Translational stiffness values must be between 0.0 and 5000.0 (N/m)"); + return false; + } + } + for (std::size_t i=3; i<6; i++){ + if (cartesian_stiffness[i] < 0 || cartesian_stiffness[i] > 300) + { + RCLCPP_ERROR(get_logger(), "Rotaional stiffness values must be between 0.0 and 300.0 (N/rad)"); + return false; + } + } + + cartesian_stiffness_ = cartesian_stiffness; + return true; +} + +bool RobotManagerNode::onCartesianDampingChangeRequest(const std::vector & cartesian_damping) +{ + if (cartesian_damping.size() != 6) + { + RCLCPP_ERROR(get_logger(), "Invalid parameter array length for parameter cartesian damping"); + return false; + } + for (double cd : cartesian_damping) + { + if (cd < 0 || cd > 1) + { + RCLCPP_ERROR(get_logger(), "Cartesian damping values must be >=0 && <=1"); + return false; + } + } + cartesian_damping_ = cartesian_damping; + return true; +} + + void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::UInt8::SharedPtr msg) { switch (static_cast(msg->data)) From 56834f2923e9135e94497806a8516b142a77fef5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Thu, 1 Aug 2024 16:01:27 +0200 Subject: [PATCH 03/49] add cart imp ctr mode, wrench cmd mode to fri_connection; use wrench controller, +configs --- .../hardware_interface_types.hpp | 12 +++++- .../config/driver_config.yaml | 5 ++- .../config/ros2_controller_config.yaml | 4 +- .../config/wrench_controller_config.yaml | 12 ++++++ .../fri_connection.hpp | 6 ++- .../hardware_interface.hpp | 2 + .../robot_manager_node.hpp | 1 + .../launch/startup.launch.py | 10 +++++ .../src/connection_helpers/fri_connection.cpp | 21 ++++++++++ .../src/hardware_interface.cpp | 42 ++++++++++++++++++- .../src/robot_manager_node.cpp | 15 +++++++ 11 files changed, 123 insertions(+), 7 deletions(-) create mode 100644 kuka_sunrise_fri_driver/config/wrench_controller_config.yaml diff --git a/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp b/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp index 3338082c..68fa929b 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp @@ -25,6 +25,13 @@ static constexpr char HW_IF_DAMPING[] = "damping"; // Constant defining external torque interface static constexpr char HW_IF_EXTERNAL_TORQUE[] = "external_torque"; + +// Constand defining cartesian coordinate interfaces +static constexpr char HW_IF_X[] = "x"; +static constexpr char HW_IF_Y[] = "y"; +static constexpr char HW_IF_Z[] = "z"; + + /* Interface prefixes */ // Constant defining prefix for I/O interfaces static constexpr char IO_PREFIX[] = "gpio"; @@ -34,7 +41,10 @@ static constexpr char CONFIG_PREFIX[] = "runtime_config"; static constexpr char FRI_STATE_PREFIX[] = "fri_state"; // Constant defining prefix for states static constexpr char STATE_PREFIX[] = "state"; - +// Constant defining wrench interface prefixes +static constexpr char HW_IF_WRENCH_PREFIX[] = "wrench"; +static constexpr char HW_IF_FORCE_PREFIX[] = "force"; +static constexpr char HW_IF_TORQUE_PREFIX[] = "torque"; /* Configuration interfaces */ // Constant defining control_mode configuration interface static constexpr char CONTROL_MODE[] = "control_mode"; diff --git a/kuka_sunrise_fri_driver/config/driver_config.yaml b/kuka_sunrise_fri_driver/config/driver_config.yaml index 14d0787b..4390ff26 100644 --- a/kuka_sunrise_fri_driver/config/driver_config.yaml +++ b/kuka_sunrise_fri_driver/config/driver_config.yaml @@ -1,10 +1,11 @@ robot_manager: ros__parameters: - control_mode: 1 + control_mode: 8 position_controller_name: "joint_trajectory_controller" torque_controller_name: "effort_controller" + wrench_controller_name: "wrench_controller" receive_multiplier: 1 - send_period_ms: 10 + send_period_ms: 5 joint_damping: [0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7] joint_stiffness: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0] cartesian_damping: [0.7, 0.7, 0.7, 0.7, 0.7, 0.7] diff --git a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml b/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml index 597ddead..bc10789f 100755 --- a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 100 # Hz + update_rate: 200 # Hz joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController @@ -12,6 +12,8 @@ controller_manager: type: kuka_controllers/FRIStateBroadcaster event_broadcaster: type: kuka_controllers/EventBroadcaster + wrench_controller: + type: kuka_controllers/WrenchController # Configuration controllers joint_group_impedance_controller: diff --git a/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml b/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml new file mode 100644 index 00000000..942604d4 --- /dev/null +++ b/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml @@ -0,0 +1,12 @@ +wrench_controller: + ros__parameters: + joints: + - wrench + + interface_names: + - force/x + - force/z + - force/y + - torque/x + - torque/y + - torque/z \ No newline at end of file diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/fri_connection.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/fri_connection.hpp index a895e06c..62d396a8 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/fri_connection.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/fri_connection.hpp @@ -60,12 +60,14 @@ enum CommandSuccess : std::uint8_t enum ControlModeID : std::uint8_t { POSITION_CONTROL_MODE = 1, - JOINT_IMPEDANCE_CONTROL_MODE = 2 + JOINT_IMPEDANCE_CONTROL_MODE = 2, + CARTESIAN_IMPEDANCE_CONTROL_MODE = 3 }; enum ClientCommandModeID : std::uint8_t { POSITION_COMMAND_MODE = 1, + WRENCH_COMMAND_MODE = 2, TORQUE_COMMAND_MODE = 3 }; @@ -88,6 +90,8 @@ class FRIConnection bool setPositionControlMode(); bool setJointImpedanceControlMode( const std::vector & joint_stiffness, const std::vector & joint_damping); + bool setCartesianImpedanceControlMode( + const std::vector & cart_stiffness, const std::vector & cart_damping); bool setClientCommandMode(ClientCommandModeID client_command_mode); // bool getControlMode(); bool setFRIConfig( diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp index 661f67d5..3b7d2365 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp @@ -126,6 +126,8 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, std::vector hw_torque_commands_; std::vector hw_stiffness_commands_; std::vector hw_damping_commands_; + std::vector hw_wrench_commands_; + std::vector hw_position_states_; std::vector hw_torque_states_; diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp index 5eb4267b..11413e73 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp @@ -79,6 +79,7 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode std::string robot_model_; std::string joint_pos_controller_name_; std::string joint_torque_controller_name_; + std::string wrench_controller_name_; std::vector joint_stiffness_ = std::vector(7, 100.0); std::vector joint_damping_ = std::vector(7, 0.7); std::vector cartesian_stiffness_ = {2000.0, 2000.0, 2000.0, 200.0, 200.0, 200.0}; diff --git a/kuka_sunrise_fri_driver/launch/startup.launch.py b/kuka_sunrise_fri_driver/launch/startup.launch.py index 7d4cf6e9..4f40e2ef 100644 --- a/kuka_sunrise_fri_driver/launch/startup.launch.py +++ b/kuka_sunrise_fri_driver/launch/startup.launch.py @@ -39,6 +39,7 @@ def launch_setup(context, *args, **kwargs): jtc_config = LaunchConfiguration("jtc_config") jic_config = LaunchConfiguration("jic_config") ec_config = LaunchConfiguration("ec_config") + wc_config = LaunchConfiguration("wc_config") cic_config = LaunchConfiguration("cic_config") if ns.perform(context) == "": @@ -119,6 +120,7 @@ def launch_setup(context, *args, **kwargs): jic_config, ec_config, cic_config, + wc_config, { "hardware_components_initial_state": { "unconfigured": [tf_prefix + robot_model.perform(context)] @@ -170,6 +172,7 @@ def controller_spawner(controller_names, activate=False): "control_mode_handler", "event_broadcaster", "cartesian_impedance_controller", + "wrench_controller" ] controller_spawners = [controller_spawner(name) for name in controller_names] @@ -226,6 +229,13 @@ def generate_launch_description(): + "/config/effort_controller_config.yaml", ) ) + launch_arguments.append( + DeclareLaunchArgument( + "wc_config", + default_value=get_package_share_directory("kuka_sunrise_fri_driver") + + "/config/wrench_controller_config.yaml", + ) + ) launch_arguments.append( DeclareLaunchArgument( "cic_config", diff --git a/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp b/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp index d56082d6..2d16c7b7 100644 --- a/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp +++ b/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp @@ -118,6 +118,27 @@ bool FRIConnection::setJointImpedanceControlMode( return sendCommandAndWait(SET_CONTROL_MODE, serialized); } +bool FRIConnection::setCartesianImpedanceControlMode( + const std::vector & cart_stiffness, const std::vector & cart_damping) +{ + std::vector serialized; + serialized.reserve(1 + CONTROL_MODE_HEADER.size() + 2 * 6 * sizeof(double)); + serialized.emplace_back(CARTESIAN_IMPEDANCE_CONTROL_MODE); + for (std::uint8_t byte : CONTROL_MODE_HEADER) + { + serialized.emplace_back(byte); + } + for (double cs : cart_stiffness) + { + serializeNext(cs, serialized); + } + for (double cd : cart_damping) + { + serializeNext(cd, serialized); + } + return sendCommandAndWait(SET_CONTROL_MODE, serialized); +} + bool FRIConnection::setClientCommandMode(ClientCommandModeID client_command_mode) { std::vector command_data = {client_command_mode}; diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 5edb7b86..2a879c88 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -42,6 +42,7 @@ CallbackReturn KukaFRIHardwareInterface::on_init( hw_torque_states_.resize(info_.joints.size()); hw_ext_torque_states_.resize(info_.joints.size()); hw_torque_commands_.resize(info_.joints.size()); + hw_wrench_commands_.resize(6); // it's always 6 dof: force x,y,z; torque x,y,z if (info_.gpios.size() != 1) { @@ -203,7 +204,12 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta std::vector(DOF, 0.0), std::vector(DOF, 0.0)); fri_connection_->setClientCommandMode(ClientCommandModeID::TORQUE_COMMAND_MODE); break; - + case kuka_drivers_core::ControlMode::WRENCH_CONTROL: + hw_stiffness_commands_.resize(6); // TODO(misi) find a better place for this + hw_damping_commands_.resize(6); + fri_connection_->setCartesianImpedanceControlMode(hw_stiffness_commands_, hw_damping_commands_); // Misi TODO this method + fri_connection_->setClientCommandMode(ClientCommandModeID::WRENCH_COMMAND_MODE); + break; default: RCLCPP_ERROR(rclcpp::get_logger("KukaFRIHardwareInterface"), "Unsupported control mode"); return CallbackReturn::ERROR; @@ -360,6 +366,17 @@ void KukaFRIHardwareInterface::updateCommand(const rclcpp::Time &) robotCommand().setTorque(joint_torques_); break; } + case kuka_drivers_core::ControlMode::WRENCH_CONTROL: + { + const double * wrench_efforts = hw_wrench_commands_.data(); + const double * joint_pos = robotState().getMeasuredJointPosition(); + std::array joint_pos_corr; + std::copy(joint_pos, joint_pos + DOF, joint_pos_corr.begin()); + activateFrictionCompensation(joint_pos_corr.data()); + robotCommand().setJointPosition(joint_pos_corr.data()); + robotCommand().setWrench(wrench_efforts); + break; + } default: RCLCPP_ERROR( rclcpp::get_logger("KukaFRIHardwareInterface"), @@ -460,6 +477,25 @@ KukaFRIHardwareInterface::export_command_interfaces() command_interfaces.emplace_back( info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_torque_commands_[i]); } + command_interfaces.emplace_back( + std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + std::string(hardware_interface::HW_IF_FORCE_PREFIX), + hardware_interface::HW_IF_X, &hw_wrench_commands_[0]); + command_interfaces.emplace_back( + std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + std::string(hardware_interface::HW_IF_FORCE_PREFIX), + hardware_interface::HW_IF_Y, &hw_wrench_commands_[1]); + command_interfaces.emplace_back( + std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + std::string(hardware_interface::HW_IF_FORCE_PREFIX), + hardware_interface::HW_IF_Z, &hw_wrench_commands_[2]); + command_interfaces.emplace_back( + std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + std::string(hardware_interface::HW_IF_TORQUE_PREFIX), + hardware_interface::HW_IF_X, &hw_wrench_commands_[3]); + command_interfaces.emplace_back( + std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + std::string(hardware_interface::HW_IF_TORQUE_PREFIX), + hardware_interface::HW_IF_Y, &hw_wrench_commands_[4]); + command_interfaces.emplace_back( + std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + std::string(hardware_interface::HW_IF_TORQUE_PREFIX), + hardware_interface::HW_IF_Z, &hw_wrench_commands_[5]); + command_interfaces.emplace_back( std::string("CartDOF") + "." + std::string("X"), hardware_interface::HW_IF_STIFFNESS, &hw_stiffness_commands_[0]); @@ -504,7 +540,9 @@ void KukaFRIHardwareInterface::activateFrictionCompensation(double * values) con { for (int i = 0; i < DOF; i++) { - values[i] -= (values[i] / fabs(values[i]) * 0.1); + if (values[i]!=0.0){ // Check for devision by zero (very unlikely, but can happen after mastering) + values[i] -= (values[i] / fabs(values[i]) * 0.1); + } } } diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index 2b96ead9..d6a505f3 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -108,6 +108,14 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ return this->onControllerNameChangeRequest( controller_name, kuka_drivers_core::ControllerType::TORQUE_CONTROLLER_TYPE); }); + + registerParameter( + "wrench_controller_name", "", kuka_drivers_core::ParameterSetAccessRights{true, false}, + [this](const std::string & controller_name) + { + return this->onControllerNameChangeRequest( + controller_name, kuka_drivers_core::ControllerType::WRENCH_CONTROLLER_TYPE); + }); registerParameter>( "joint_stiffness", joint_stiffness_, kuka_drivers_core::ParameterSetAccessRights{true, false}, @@ -306,6 +314,8 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) case kuka_drivers_core::ControlMode::JOINT_IMPEDANCE_CONTROL: // TODO(Svastits): check whether this is necessary for impedance mode too [[fallthrough]]; + case kuka_drivers_core::ControlMode::WRENCH_CONTROL: + [[fallthrough]]; case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: if (send_period_ms_ > 5) { @@ -410,6 +420,9 @@ bool RobotManagerNode::onControllerNameChangeRequest( case kuka_drivers_core::ControllerType::TORQUE_CONTROLLER_TYPE: joint_torque_controller_name_ = controller_name; break; + case kuka_drivers_core::ControllerType::WRENCH_CONTROLLER_TYPE: + wrench_controller_name_ = controller_name; + break; default: // This should actually never happen RCLCPP_ERROR(get_logger(), "Invalid controller type"); @@ -430,6 +443,8 @@ std::string RobotManagerNode::GetControllerName() const return joint_pos_controller_name_; case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: return joint_torque_controller_name_; + case kuka_drivers_core::ControlMode::WRENCH_CONTROL: + return wrench_controller_name_; default: throw std::runtime_error("Stored control mode is not allowed"); } From 064400147906889db233b7b6428acf1a04300aa5 Mon Sep 17 00:00:00 2001 From: Kristofi Misi Date: Fri, 2 Aug 2024 14:00:40 +0200 Subject: [PATCH 04/49] seperate joint and cart stiffness and damping, fix s/d values not being published at the corrent time of configuration, fix cart damping min value --- .../hardware_interface.hpp | 7 +-- .../src/hardware_interface.cpp | 41 ++++++++--------- .../src/robot_manager_node.cpp | 46 ++++++++++--------- 3 files changed, 49 insertions(+), 45 deletions(-) diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp index 3b7d2365..c925fa39 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp @@ -124,10 +124,11 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, // State and command interfaces std::vector hw_position_commands_; std::vector hw_torque_commands_; - std::vector hw_stiffness_commands_; - std::vector hw_damping_commands_; + std::vector hw_joint_stiffness_commands_; + std::vector hw_joint_damping_commands_; std::vector hw_wrench_commands_; - + std::vector hw_cart_stiffness_commands_; + std::vector hw_cart_damping_commands_; std::vector hw_position_states_; std::vector hw_torque_states_; diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 2a879c88..79545137 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -37,13 +37,14 @@ CallbackReturn KukaFRIHardwareInterface::on_init( hw_position_states_.resize(info_.joints.size()); hw_position_commands_.resize(info_.joints.size()); - hw_stiffness_commands_.resize(info_.joints.size()); - hw_damping_commands_.resize(info_.joints.size()); + hw_joint_stiffness_commands_.resize(info_.joints.size()); + hw_joint_damping_commands_.resize(info_.joints.size()); hw_torque_states_.resize(info_.joints.size()); hw_ext_torque_states_.resize(info_.joints.size()); hw_torque_commands_.resize(info_.joints.size()); hw_wrench_commands_.resize(6); // it's always 6 dof: force x,y,z; torque x,y,z - + hw_cart_stiffness_commands_.resize(6,150); + hw_cart_damping_commands_.resize(6,0.15); if (info_.gpios.size() != 1) { RCLCPP_FATAL(rclcpp::get_logger("KukaFRIHardwareInterface"), "expecting exactly 1 GPIO"); @@ -196,7 +197,7 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta fri_connection_->setClientCommandMode(ClientCommandModeID::POSITION_COMMAND_MODE); break; case kuka_drivers_core::ControlMode::JOINT_IMPEDANCE_CONTROL: - fri_connection_->setJointImpedanceControlMode(hw_stiffness_commands_, hw_damping_commands_); + fri_connection_->setJointImpedanceControlMode(hw_joint_stiffness_commands_, hw_joint_damping_commands_); fri_connection_->setClientCommandMode(ClientCommandModeID::POSITION_COMMAND_MODE); break; case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: @@ -205,9 +206,7 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta fri_connection_->setClientCommandMode(ClientCommandModeID::TORQUE_COMMAND_MODE); break; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: - hw_stiffness_commands_.resize(6); // TODO(misi) find a better place for this - hw_damping_commands_.resize(6); - fri_connection_->setCartesianImpedanceControlMode(hw_stiffness_commands_, hw_damping_commands_); // Misi TODO this method + fri_connection_->setCartesianImpedanceControlMode(std::vector(DOF, 0.0), std::vector(DOF, 0.1)); fri_connection_->setClientCommandMode(ClientCommandModeID::WRENCH_COMMAND_MODE); break; default: @@ -471,9 +470,9 @@ KukaFRIHardwareInterface::export_command_interfaces() command_interfaces.emplace_back( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_position_commands_[i]); command_interfaces.emplace_back( - info_.joints[i].name, hardware_interface::HW_IF_STIFFNESS, &hw_stiffness_commands_[i]); + info_.joints[i].name, hardware_interface::HW_IF_STIFFNESS, &hw_joint_stiffness_commands_[i]); command_interfaces.emplace_back( - info_.joints[i].name, hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[i]); + info_.joints[i].name, hardware_interface::HW_IF_DAMPING, &hw_joint_damping_commands_[i]); command_interfaces.emplace_back( info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_torque_commands_[i]); } @@ -498,40 +497,40 @@ KukaFRIHardwareInterface::export_command_interfaces() command_interfaces.emplace_back( std::string("CartDOF") + "." + std::string("X"), - hardware_interface::HW_IF_STIFFNESS, &hw_stiffness_commands_[0]); + hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[0]); command_interfaces.emplace_back( std::string("CartDOF") + "." + std::string("Y"), - hardware_interface::HW_IF_STIFFNESS, &hw_stiffness_commands_[1]); + hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[1]); command_interfaces.emplace_back( std::string("CartDOF") + "." + std::string("Z"), - hardware_interface::HW_IF_STIFFNESS, &hw_stiffness_commands_[2]); + hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[2]); command_interfaces.emplace_back( std::string("CartDOF") + "." + std::string("A"), - hardware_interface::HW_IF_STIFFNESS, &hw_stiffness_commands_[3]); + hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[3]); command_interfaces.emplace_back( std::string("CartDOF") + "." + std::string("B"), - hardware_interface::HW_IF_STIFFNESS, &hw_stiffness_commands_[4]); + hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[4]); command_interfaces.emplace_back( std::string("CartDOF") + "." + std::string("C"), - hardware_interface::HW_IF_STIFFNESS, &hw_stiffness_commands_[5]); + hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[5]); command_interfaces.emplace_back( std::string("CartDOF") + "." + std::string("X"), - hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[0]); + hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[0]); command_interfaces.emplace_back( std::string("CartDOF") + "." + std::string("Y"), - hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[1]); + hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[1]); command_interfaces.emplace_back( std::string("CartDOF") + "." + std::string("Z"), - hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[2]); + hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[2]); command_interfaces.emplace_back( std::string("CartDOF") + "." + std::string("A"), - hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[3]); + hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[3]); command_interfaces.emplace_back( std::string("CartDOF") + "." + std::string("B"), - hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[4]); + hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[4]); command_interfaces.emplace_back( std::string("CartDOF") + "." + std::string("C"), - hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[5]); + hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[5]); return command_interfaces; } diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index d6a505f3..b922be09 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -167,6 +167,27 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) RCLCPP_ERROR(get_logger(), "Could not activate configuration controllers"); return FAILURE; } + // Publish the values of the joint impedance parameters to the controller + std_msgs::msg::Float64MultiArray joint_imp_msg; + for (int i = 0; i < 7; i++) + { + joint_imp_msg.data.push_back(joint_stiffness_[i]); + joint_imp_msg.data.push_back(joint_damping_[i]); + RCLCPP_ERROR(get_logger(), "joint stiff: %f, joint damp: %f", + joint_stiffness_[i], joint_damping_[i]); + } + joint_imp_pub_->publish(joint_imp_msg); + // Publish the values of the cartesian impedance parameters to the controller + std_msgs::msg::Float64MultiArray cart_imp_msg; + for (int i = 0; i < 6; i++) + { + cart_imp_msg.data.push_back(cartesian_stiffness_[i]); + cart_imp_msg.data.push_back(cartesian_damping_[i]); + RCLCPP_ERROR(get_logger(), "cart stiff: %f, cart damp: %f", + cartesian_stiffness_[i], cartesian_damping_[i]); + + } + cart_imp_pub_->publish(cart_imp_msg); is_configured_pub_->on_activate(); is_configured_msg_.data = true; @@ -215,24 +236,7 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) { - // Publish the values of the joint impedance parameters to the controller - std_msgs::msg::Float64MultiArray joint_imp_msg; - for (int i = 0; i < 7; i++) - { - joint_imp_msg.data.push_back(joint_stiffness_[i]); - joint_imp_msg.data.push_back(joint_damping_[i]); - } - joint_imp_pub_->publish(joint_imp_msg); - - // Publish the values of the cartesian impedance parameters to the controller - std_msgs::msg::Float64MultiArray cart_imp_msg; - for (int i = 0; i < 6; i++) - { - cart_imp_msg.data.push_back(cartesian_stiffness_[i]); - cart_imp_msg.data.push_back(cartesian_damping_[i]); - } - cart_imp_pub_->publish(cart_imp_msg); - + // Activate hardware interface if (!kuka_drivers_core::changeHardwareState( change_hardware_state_client_, robot_model_, @@ -517,7 +521,7 @@ bool RobotManagerNode::onCartesianStiffnessChangeRequest(const std::vector } for (double cd : cartesian_damping) { - if (cd < 0 || cd > 1) + if (cd < 0.1 || cd > 1) { - RCLCPP_ERROR(get_logger(), "Cartesian damping values must be >=0 && <=1"); + RCLCPP_ERROR(get_logger(), "Cartesian damping values must be >=0.1 && <=1"); return false; } } From 65edcb721b3c0009ed08c78f48ca2ddb7b4cbc0c Mon Sep 17 00:00:00 2001 From: Kristofi Misi Date: Tue, 6 Aug 2024 14:47:44 +0200 Subject: [PATCH 05/49] wrench control impedance size fix, remove init in controller --- .../wrench_controller/controller_plugins.xml | 2 +- .../wrench_controller/wrench_controller.hpp | 1 - .../src/wrench_controller.cpp | 24 ------------------- .../config/driver_config.yaml | 2 +- .../src/hardware_interface.cpp | 2 +- 5 files changed, 3 insertions(+), 28 deletions(-) diff --git a/controllers/wrench_controller/controller_plugins.xml b/controllers/wrench_controller/controller_plugins.xml index 8162e891..7d4df2d0 100644 --- a/controllers/wrench_controller/controller_plugins.xml +++ b/controllers/wrench_controller/controller_plugins.xml @@ -1,7 +1,7 @@ - This controller sends the wrench commands in real time + This controller forwards the wrench commands in real time diff --git a/controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp b/controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp index 5fd9e689..6a0e7d6a 100644 --- a/controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp +++ b/controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp @@ -30,7 +30,6 @@ class WrenchController : public forward_command_controller::ForwardControllersBa { public: WRENCH_CONTROLLER_PUBLIC WrenchController(); - WRENCH_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; private: WRENCH_CONTROLLER_LOCAL void declare_parameters() override; diff --git a/controllers/wrench_controller/src/wrench_controller.cpp b/controllers/wrench_controller/src/wrench_controller.cpp index 8527c0d6..cfa5609e 100644 --- a/controllers/wrench_controller/src/wrench_controller.cpp +++ b/controllers/wrench_controller/src/wrench_controller.cpp @@ -60,31 +60,7 @@ controller_interface::CallbackReturn WrenchController::read_parameters() return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn WrenchController::on_init() -{ - auto ret = ForwardControllersBase::on_init(); - if (ret != CallbackReturn::SUCCESS) - { - return ret; - } - try - { - // Explicitly set the interfaces parameter declared by the - // forward_command_controller - get_node()->set_parameter(rclcpp::Parameter( - "interface_names", - std::vector{ - "x", "y", "z"})); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - - return CallbackReturn::SUCCESS; -} } // namespace kuka_controllers PLUGINLIB_EXPORT_CLASS( diff --git a/kuka_sunrise_fri_driver/config/driver_config.yaml b/kuka_sunrise_fri_driver/config/driver_config.yaml index 4390ff26..90218317 100644 --- a/kuka_sunrise_fri_driver/config/driver_config.yaml +++ b/kuka_sunrise_fri_driver/config/driver_config.yaml @@ -9,4 +9,4 @@ robot_manager: joint_damping: [0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7] joint_stiffness: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0] cartesian_damping: [0.7, 0.7, 0.7, 0.7, 0.7, 0.7] - cartesian_stiffness: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0] + cartesian_stiffness: [2000.0, 2000.0, 2000.0, 200.0, 200.0, 200.0] diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 79545137..dc0da170 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -206,7 +206,7 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta fri_connection_->setClientCommandMode(ClientCommandModeID::TORQUE_COMMAND_MODE); break; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: - fri_connection_->setCartesianImpedanceControlMode(std::vector(DOF, 0.0), std::vector(DOF, 0.1)); + fri_connection_->setCartesianImpedanceControlMode(std::vector(6, 0.0), std::vector(6, 0.1)); fri_connection_->setClientCommandMode(ClientCommandModeID::WRENCH_COMMAND_MODE); break; default: From 5897bd0900a958d39544c99fd06dfa69373eed53 Mon Sep 17 00:00:00 2001 From: Kristofi Misi Date: Tue, 6 Aug 2024 14:50:17 +0200 Subject: [PATCH 06/49] refactor impedance values publishing --- .../robot_manager_node.hpp | 3 +- .../src/robot_manager_node.cpp | 44 +++++++++---------- 2 files changed, 23 insertions(+), 24 deletions(-) diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp index 11413e73..2cf7689d 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp @@ -98,7 +98,8 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode bool onCartesianStiffnessChangeRequest(const std::vector & cartesian_stiffness); bool onCartesianDampingChangeRequest(const std::vector & cartesian_damping); void setFriConfiguration(int send_period_ms, int receive_multiplier) const; - + void setImpedanceConfiguration(const rclcpp::Publisher::SharedPtr & pub, + const std::vector & stiffness, const std::vector & damping) const; void EventSubscriptionCallback(const std_msgs::msg::UInt8::SharedPtr msg); }; diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index b922be09..291da414 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -146,6 +146,10 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) // Publish FRI configuration to notify fri_configuration_controller of initial values setFriConfiguration(send_period_ms_, receive_multiplier_); + // Publish the values of the cartesian impedance parameters to the controller + setImpedanceConfiguration(cart_imp_pub_, cartesian_stiffness_, cartesian_damping_); + // Publish the values of the joint impedance parameters to the controller + setImpedanceConfiguration(joint_imp_pub_, joint_stiffness_, joint_damping_); // Configure hardware interface if (!kuka_drivers_core::changeHardwareState( @@ -167,28 +171,6 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) RCLCPP_ERROR(get_logger(), "Could not activate configuration controllers"); return FAILURE; } - // Publish the values of the joint impedance parameters to the controller - std_msgs::msg::Float64MultiArray joint_imp_msg; - for (int i = 0; i < 7; i++) - { - joint_imp_msg.data.push_back(joint_stiffness_[i]); - joint_imp_msg.data.push_back(joint_damping_[i]); - RCLCPP_ERROR(get_logger(), "joint stiff: %f, joint damp: %f", - joint_stiffness_[i], joint_damping_[i]); - } - joint_imp_pub_->publish(joint_imp_msg); - // Publish the values of the cartesian impedance parameters to the controller - std_msgs::msg::Float64MultiArray cart_imp_msg; - for (int i = 0; i < 6; i++) - { - cart_imp_msg.data.push_back(cartesian_stiffness_[i]); - cart_imp_msg.data.push_back(cartesian_damping_[i]); - RCLCPP_ERROR(get_logger(), "cart stiff: %f, cart damp: %f", - cartesian_stiffness_[i], cartesian_damping_[i]); - - } - cart_imp_pub_->publish(cart_imp_msg); - is_configured_pub_->on_activate(); is_configured_msg_.data = true; is_configured_pub_->publish(is_configured_msg_); @@ -462,6 +444,18 @@ void RobotManagerNode::setFriConfiguration(int send_period_ms, int receive_multi fri_config_pub_->publish(msg); } +void RobotManagerNode::setImpedanceConfiguration(const rclcpp::Publisher::SharedPtr & pub, + const std::vector & stiffness, const std::vector & damping) const +{ + std_msgs::msg::Float64MultiArray msg; + for (std::size_t i = 0; i < stiffness.size(); i++) + { + msg.data.push_back(stiffness[i]); + msg.data.push_back(damping[i]); + } + pub->publish(msg); +} + bool RobotManagerNode::onJointStiffnessChangeRequest(const std::vector & joint_stiffness) { if (joint_stiffness.size() != 7) @@ -478,6 +472,7 @@ bool RobotManagerNode::onJointStiffnessChangeRequest(const std::vector & } } joint_stiffness_ = joint_stiffness; + setImpedanceConfiguration(joint_imp_pub_, joint_stiffness_, joint_damping_); return true; } @@ -497,6 +492,7 @@ bool RobotManagerNode::onJointDampingChangeRequest(const std::vector & j } } joint_damping_ = joint_damping; + setImpedanceConfiguration(joint_imp_pub_, joint_stiffness_, joint_damping_); return true; } @@ -521,8 +517,9 @@ bool RobotManagerNode::onCartesianStiffnessChangeRequest(const std::vector } } cartesian_damping_ = cartesian_damping; + setImpedanceConfiguration(cart_imp_pub_, cartesian_stiffness_, cartesian_damping_); return true; } From 41d9914350c3c2a0bb19ca1bde0d412301c85302 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Tue, 6 Aug 2024 15:25:33 +0200 Subject: [PATCH 07/49] robot application add cartesian impedance control mode --- .../src/ros2/modules/FRIManager.java | 10 ++- .../src/ros2/modules/ROS2Connection.java | 28 ++++++-- .../src/ros2/modules/TCPConnection.java | 1 - ...ianImpedanceControlModeExternalizable.java | 71 +++++++++++++++++++ .../ros2/serialization/ControlModeParams.java | 18 ++++- 5 files changed, 113 insertions(+), 15 deletions(-) create mode 100644 kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java index 4dc69e58..e98e4214 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java @@ -41,9 +41,10 @@ public enum CommandResult{ private ClientCommandMode _clientCommandMode; private IMotionContainer _motionContainer; private FRIMotionErrorHandler _friMotionErrorHandler = new FRIMotionErrorHandler(); - //private IApplicationControl _applicationControl; - + private static double[] stiffness_ = new double[7]; + + public FRIManager(LBR lbr, IApplicationControl applicationControl){ _currentState = new InactiveState(); @@ -52,9 +53,7 @@ public FRIManager(LBR lbr, IApplicationControl applicationControl){ _FRIConfiguration = new FRIConfigurationParams().toFRIConfiguration(_lbr); Arrays.fill(stiffness_, 200); _controlMode = new JointImpedanceControlMode(stiffness_); - //_controlMode = new PositionControlMode(); _clientCommandMode = ClientCommandMode.POSITION; - //_applicationControl = applicationControl; applicationControl.registerMoveAsyncErrorHandler(_friMotionErrorHandler); } @@ -247,11 +246,9 @@ public ErrorHandlingAction handleError(Device device, switch(sessionState){ case IDLE: FRIManager.this._ROS2Connection.handleFRIEndedError(); - //FRIManager.this._currentState = new InactiveState(); break; default: FRIManager.this._ROS2Connection.handleControlEndedError(); - //FRIManager.this._currentState = new FRIActiveState(); break; } System.out.println("Failed container: " + failedContainer.toString() + "."); @@ -264,3 +261,4 @@ public ErrorHandlingAction handleError(Device device, } } + diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java index c44e569c..0fae9bc0 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java @@ -4,12 +4,14 @@ import java.util.Arrays; import ros2.modules.FRIManager; +import ros2.serialization.CartesianImpedanceControlModeExternalizable; import ros2.serialization.ControlModeParams; import ros2.serialization.FRIConfigurationParams; import ros2.serialization.JointImpedanceControlModeExternalizable; import ros2.serialization.MessageEncoding; import com.kuka.connectivity.fastRobotInterface.ClientCommandMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.CartesianImpedanceControlMode; import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; @@ -95,8 +97,9 @@ private SuccessSignalID(byte value){ private enum ControlModeID{ POSITION( (byte)1), - JOINT_IMPEDANCE((byte)2); - + JOINT_IMPEDANCE((byte)2), + CARTESIAN_IMPEDANCE((byte)3); + public final byte value; ControlModeID(byte value){ @@ -244,7 +247,7 @@ private byte[] connect(byte[] cmdData){ private byte[] disconnect(byte[] cmdData){ _FRIManager.close(); _disconnect = true; - //_TCPConnection.closeConnection(); //TODO: close connection after feedback was sent + //TODO: close connection after feedback was sent return null; } @@ -323,6 +326,9 @@ private byte[] getControlMode(byte[] cmdData){ } else if (controlMode instanceof JointImpedanceControlMode){ controlModeID = ControlModeID.JOINT_IMPEDANCE; controlModeData = MessageEncoding.Encode(new JointImpedanceControlModeExternalizable((JointImpedanceControlMode)controlMode), JointImpedanceControlModeExternalizable.length); + } else if (controlMode instanceof CartesianImpedanceControlMode){ + controlModeID = ControlModeID.CARTESIAN_IMPEDANCE; + controlModeData = MessageEncoding.Encode(new CartesianImpedanceControlModeExternalizable((CartesianImpedanceControlMode)controlMode), CartesianImpedanceControlModeExternalizable.length); } else { throw new RuntimeException("Control mode not supported"); } @@ -340,13 +346,23 @@ private byte[] setControlMode(byte[] cmdData){ switch(controlModeID){ case POSITION: controlMode = new PositionControlMode(); + System.out.println("Control mode POSITION selected"); break; case JOINT_IMPEDANCE: ensureArrayLength(controlModeData, JointImpedanceControlModeExternalizable.length + 6); - JointImpedanceControlModeExternalizable externalizable = new JointImpedanceControlModeExternalizable(); + JointImpedanceControlModeExternalizable jointexternalizable = new JointImpedanceControlModeExternalizable(); + System.out.println("Decoding params"); + MessageEncoding.Decode(controlModeData, jointexternalizable); + controlMode = jointexternalizable.toControlMode(); + System.out.println("Control mode JOINT_IMPEDANCE selected"); + break; + case CARTESIAN_IMPEDANCE: + ensureArrayLength(controlModeData, CartesianImpedanceControlModeExternalizable.length + 6); + CartesianImpedanceControlModeExternalizable cartexternalizable = new CartesianImpedanceControlModeExternalizable(); System.out.println("Decoding params"); - MessageEncoding.Decode(controlModeData, externalizable); - controlMode = externalizable.toControlMode(); + MessageEncoding.Decode(controlModeData, cartexternalizable); + controlMode = cartexternalizable.toControlMode(); + System.out.println("Control mode CARTESIAN_IMPEDANCE selected"); break; } System.out.println("Control mode decoded."); diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/TCPConnection.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/TCPConnection.java index f39018f5..a895269a 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/TCPConnection.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/TCPConnection.java @@ -99,7 +99,6 @@ public void sendBytes(byte[] message){ try{ DataOutputStream outToClient = new DataOutputStream(_tcpClient.getOutputStream()); outToClient.write(message); - //outToClient.close(); }catch(IOException e){ e.printStackTrace(); } diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java new file mode 100644 index 00000000..ecef94a4 --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java @@ -0,0 +1,71 @@ +package ros2.serialization; + +import java.io.Externalizable; +import java.io.IOException; +import java.io.ObjectInput; +import java.io.ObjectOutput; + +import ros2.tools.Conversions; + +import com.kuka.roboticsAPI.geometricModel.CartDOF; +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.CartesianImpedanceControlMode; + +public class CartesianImpedanceControlModeExternalizable extends CartesianImpedanceControlMode implements Externalizable{ + + public final static int length = 96; + + + public CartesianImpedanceControlModeExternalizable(CartesianImpedanceControlMode other){ + super(other); + } + + public CartesianImpedanceControlModeExternalizable(){ + super(); + } + + public IMotionControlMode toControlMode(){ + CartesianImpedanceControlMode controlMode = new CartesianImpedanceControlMode((CartesianImpedanceControlMode)this); + return (IMotionControlMode)controlMode; + } + + @Override + public void writeExternal(ObjectOutput out) throws IOException { + for(double cartStiffness : getStiffness()){ + out.writeDouble(cartStiffness); + } + for(double cartDamping : getDamping()){ + out.writeDouble(cartDamping); + } + } + + @Override + public void readExternal(ObjectInput in) throws IOException, + ClassNotFoundException { + double[] cartStiffness = new double[getStiffness().length]; + for(int i = 0; i < getStiffness().length; i++){ + cartStiffness[i] = in.readDouble(); + } + this.parametrize(CartDOF.X).setStiffness(cartStiffness[0]); + this.parametrize(CartDOF.Y).setStiffness(cartStiffness[1]); + this.parametrize(CartDOF.Z).setStiffness(cartStiffness[2]); + this.parametrize(CartDOF.A).setStiffness(cartStiffness[3]); + this.parametrize(CartDOF.B).setStiffness(cartStiffness[4]); + this.parametrize(CartDOF.C).setStiffness(cartStiffness[5]); + + double[] cartDamping = new double[getDamping().length]; + for(int i = 0; i < getDamping().length; i++){ + cartDamping[i] = in.readDouble(); + } + this.parametrize(CartDOF.X).setDamping(cartDamping[0]); + this.parametrize(CartDOF.Y).setDamping(cartDamping[1]); + this.parametrize(CartDOF.Z).setDamping(cartDamping[2]); + this.parametrize(CartDOF.A).setDamping(cartDamping[3]); + this.parametrize(CartDOF.B).setDamping(cartDamping[4]); + this.parametrize(CartDOF.C).setDamping(cartDamping[5]); + + } + + + +} diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java index 4872d1ca..f27a235f 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java @@ -10,14 +10,16 @@ import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.CartesianImpedanceControlMode; public abstract class ControlModeParams implements Externalizable{ public static int length = 0; private enum ControlModeID{ POSITION( (byte)1), - JOINT_IMPEDANCE((byte)2); - + JOINT_IMPEDANCE((byte)2), + CARTESIAN_IMPEDANCE((byte)3); + public final byte value; ControlModeID(byte value){ @@ -47,6 +49,9 @@ public static ControlModeParams fromSerialData(byte[] serialData){ case JOINT_IMPEDANCE: controlModeParams = new JointImpedanceControlModeParams(); break; + case CARTESIAN_IMPEDANCE: + controlModeParams = new CartesianImpedanceControlModeParams(); + break; } serialData = Arrays.copyOfRange(serialData, 1, serialData.length); MessageEncoding.Decode(serialData, controlModeParams); @@ -95,3 +100,12 @@ public JointImpedanceControlModeParams(JointImpedanceControlMode controlMode){ } } + +class CartesianImpedanceControlModeParams extends ControlModeParams{ + public CartesianImpedanceControlModeParams(){ + + } + public CartesianImpedanceControlModeParams(CartesianImpedanceControlMode controlMode){ + + } +} From ef7c966b6ddf6d3d5ff89bb6d0d1201b908517fb Mon Sep 17 00:00:00 2001 From: Kristofi Misi Date: Tue, 6 Aug 2024 16:41:24 +0200 Subject: [PATCH 08/49] format --- .../wrench_controller/wrench_controller.hpp | 3 +- .../src/wrench_controller.cpp | 1 - .../hardware_interface_types.hpp | 2 - .../config/wrench_controller_config.yaml | 4 +- .../robot_manager_node.hpp | 3 +- .../launch/startup.launch.py | 2 +- .../src/connection_helpers/fri_connection.cpp | 2 +- .../src/hardware_interface.cpp | 93 ++++++++++--------- .../src/robot_manager_node.cpp | 41 ++++---- 9 files changed, 83 insertions(+), 68 deletions(-) diff --git a/controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp b/controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp index 6a0e7d6a..caa4c51d 100644 --- a/controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp +++ b/controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp @@ -33,8 +33,7 @@ class WrenchController : public forward_command_controller::ForwardControllersBa private: WRENCH_CONTROLLER_LOCAL void declare_parameters() override; - WRENCH_CONTROLLER_LOCAL controller_interface::CallbackReturn read_parameters() - override; + WRENCH_CONTROLLER_LOCAL controller_interface::CallbackReturn read_parameters() override; using Params = wrench_controller::Params; using ParamListener = wrench_controller::ParamListener; diff --git a/controllers/wrench_controller/src/wrench_controller.cpp b/controllers/wrench_controller/src/wrench_controller.cpp index cfa5609e..b545f01f 100644 --- a/controllers/wrench_controller/src/wrench_controller.cpp +++ b/controllers/wrench_controller/src/wrench_controller.cpp @@ -60,7 +60,6 @@ controller_interface::CallbackReturn WrenchController::read_parameters() return controller_interface::CallbackReturn::SUCCESS; } - } // namespace kuka_controllers PLUGINLIB_EXPORT_CLASS( diff --git a/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp b/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp index 68fa929b..d3c45c60 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp @@ -25,13 +25,11 @@ static constexpr char HW_IF_DAMPING[] = "damping"; // Constant defining external torque interface static constexpr char HW_IF_EXTERNAL_TORQUE[] = "external_torque"; - // Constand defining cartesian coordinate interfaces static constexpr char HW_IF_X[] = "x"; static constexpr char HW_IF_Y[] = "y"; static constexpr char HW_IF_Z[] = "z"; - /* Interface prefixes */ // Constant defining prefix for I/O interfaces static constexpr char IO_PREFIX[] = "gpio"; diff --git a/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml b/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml index 942604d4..0856d93c 100644 --- a/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml @@ -5,8 +5,8 @@ wrench_controller: interface_names: - force/x - - force/z + - force/z - force/y - torque/x - torque/y - - torque/z \ No newline at end of file + - torque/z diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp index 2cf7689d..3fd244cb 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp @@ -98,7 +98,8 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode bool onCartesianStiffnessChangeRequest(const std::vector & cartesian_stiffness); bool onCartesianDampingChangeRequest(const std::vector & cartesian_damping); void setFriConfiguration(int send_period_ms, int receive_multiplier) const; - void setImpedanceConfiguration(const rclcpp::Publisher::SharedPtr & pub, + void setImpedanceConfiguration( + const rclcpp::Publisher::SharedPtr & pub, const std::vector & stiffness, const std::vector & damping) const; void EventSubscriptionCallback(const std_msgs::msg::UInt8::SharedPtr msg); }; diff --git a/kuka_sunrise_fri_driver/launch/startup.launch.py b/kuka_sunrise_fri_driver/launch/startup.launch.py index 4f40e2ef..2f85e337 100644 --- a/kuka_sunrise_fri_driver/launch/startup.launch.py +++ b/kuka_sunrise_fri_driver/launch/startup.launch.py @@ -172,7 +172,7 @@ def controller_spawner(controller_names, activate=False): "control_mode_handler", "event_broadcaster", "cartesian_impedance_controller", - "wrench_controller" + "wrench_controller", ] controller_spawners = [controller_spawner(name) for name in controller_names] diff --git a/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp b/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp index 2d16c7b7..d29f56f9 100644 --- a/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp +++ b/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp @@ -119,7 +119,7 @@ bool FRIConnection::setJointImpedanceControlMode( } bool FRIConnection::setCartesianImpedanceControlMode( - const std::vector & cart_stiffness, const std::vector & cart_damping) + const std::vector & cart_stiffness, const std::vector & cart_damping) { std::vector serialized; serialized.reserve(1 + CONTROL_MODE_HEADER.size() + 2 * 6 * sizeof(double)); diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index dc0da170..c4c86f14 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -42,9 +42,9 @@ CallbackReturn KukaFRIHardwareInterface::on_init( hw_torque_states_.resize(info_.joints.size()); hw_ext_torque_states_.resize(info_.joints.size()); hw_torque_commands_.resize(info_.joints.size()); - hw_wrench_commands_.resize(6); // it's always 6 dof: force x,y,z; torque x,y,z - hw_cart_stiffness_commands_.resize(6,150); - hw_cart_damping_commands_.resize(6,0.15); + hw_wrench_commands_.resize(6); // it's always 6 dof: force x,y,z; torque x,y,z + hw_cart_stiffness_commands_.resize(6, 150); + hw_cart_damping_commands_.resize(6, 0.15); if (info_.gpios.size() != 1) { RCLCPP_FATAL(rclcpp::get_logger("KukaFRIHardwareInterface"), "expecting exactly 1 GPIO"); @@ -197,7 +197,8 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta fri_connection_->setClientCommandMode(ClientCommandModeID::POSITION_COMMAND_MODE); break; case kuka_drivers_core::ControlMode::JOINT_IMPEDANCE_CONTROL: - fri_connection_->setJointImpedanceControlMode(hw_joint_stiffness_commands_, hw_joint_damping_commands_); + fri_connection_->setJointImpedanceControlMode( + hw_joint_stiffness_commands_, hw_joint_damping_commands_); fri_connection_->setClientCommandMode(ClientCommandModeID::POSITION_COMMAND_MODE); break; case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: @@ -206,7 +207,8 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta fri_connection_->setClientCommandMode(ClientCommandModeID::TORQUE_COMMAND_MODE); break; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: - fri_connection_->setCartesianImpedanceControlMode(std::vector(6, 0.0), std::vector(6, 0.1)); + fri_connection_->setCartesianImpedanceControlMode( + std::vector(6, 0.0), std::vector(6, 0.1)); fri_connection_->setClientCommandMode(ClientCommandModeID::WRENCH_COMMAND_MODE); break; default: @@ -477,60 +479,66 @@ KukaFRIHardwareInterface::export_command_interfaces() info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_torque_commands_[i]); } command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + std::string(hardware_interface::HW_IF_FORCE_PREFIX), - hardware_interface::HW_IF_X, &hw_wrench_commands_[0]); + std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_FORCE_PREFIX), + hardware_interface::HW_IF_X, &hw_wrench_commands_[0]); command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + std::string(hardware_interface::HW_IF_FORCE_PREFIX), - hardware_interface::HW_IF_Y, &hw_wrench_commands_[1]); + std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_FORCE_PREFIX), + hardware_interface::HW_IF_Y, &hw_wrench_commands_[1]); command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + std::string(hardware_interface::HW_IF_FORCE_PREFIX), - hardware_interface::HW_IF_Z, &hw_wrench_commands_[2]); + std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_FORCE_PREFIX), + hardware_interface::HW_IF_Z, &hw_wrench_commands_[2]); command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + std::string(hardware_interface::HW_IF_TORQUE_PREFIX), - hardware_interface::HW_IF_X, &hw_wrench_commands_[3]); + std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_TORQUE_PREFIX), + hardware_interface::HW_IF_X, &hw_wrench_commands_[3]); command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + std::string(hardware_interface::HW_IF_TORQUE_PREFIX), - hardware_interface::HW_IF_Y, &hw_wrench_commands_[4]); + std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_TORQUE_PREFIX), + hardware_interface::HW_IF_Y, &hw_wrench_commands_[4]); command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + std::string(hardware_interface::HW_IF_TORQUE_PREFIX), - hardware_interface::HW_IF_Z, &hw_wrench_commands_[5]); + std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_TORQUE_PREFIX), + hardware_interface::HW_IF_Z, &hw_wrench_commands_[5]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("X"), - hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[0]); + std::string("CartDOF") + "." + std::string("X"), hardware_interface::HW_IF_STIFFNESS, + &hw_cart_stiffness_commands_[0]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("Y"), - hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[1]); + std::string("CartDOF") + "." + std::string("Y"), hardware_interface::HW_IF_STIFFNESS, + &hw_cart_stiffness_commands_[1]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("Z"), - hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[2]); + std::string("CartDOF") + "." + std::string("Z"), hardware_interface::HW_IF_STIFFNESS, + &hw_cart_stiffness_commands_[2]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("A"), - hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[3]); + std::string("CartDOF") + "." + std::string("A"), hardware_interface::HW_IF_STIFFNESS, + &hw_cart_stiffness_commands_[3]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("B"), - hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[4]); + std::string("CartDOF") + "." + std::string("B"), hardware_interface::HW_IF_STIFFNESS, + &hw_cart_stiffness_commands_[4]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("C"), - hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[5]); + std::string("CartDOF") + "." + std::string("C"), hardware_interface::HW_IF_STIFFNESS, + &hw_cart_stiffness_commands_[5]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("X"), - hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[0]); + std::string("CartDOF") + "." + std::string("X"), hardware_interface::HW_IF_DAMPING, + &hw_cart_damping_commands_[0]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("Y"), - hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[1]); + std::string("CartDOF") + "." + std::string("Y"), hardware_interface::HW_IF_DAMPING, + &hw_cart_damping_commands_[1]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("Z"), - hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[2]); + std::string("CartDOF") + "." + std::string("Z"), hardware_interface::HW_IF_DAMPING, + &hw_cart_damping_commands_[2]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("A"), - hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[3]); + std::string("CartDOF") + "." + std::string("A"), hardware_interface::HW_IF_DAMPING, + &hw_cart_damping_commands_[3]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("B"), - hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[4]); + std::string("CartDOF") + "." + std::string("B"), hardware_interface::HW_IF_DAMPING, + &hw_cart_damping_commands_[4]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("C"), - hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[5]); + std::string("CartDOF") + "." + std::string("C"), hardware_interface::HW_IF_DAMPING, + &hw_cart_damping_commands_[5]); return command_interfaces; } @@ -539,7 +547,8 @@ void KukaFRIHardwareInterface::activateFrictionCompensation(double * values) con { for (int i = 0; i < DOF; i++) { - if (values[i]!=0.0){ // Check for devision by zero (very unlikely, but can happen after mastering) + if (values[i] != 0.0) + { // Check for division by zero (very unlikely, but can happen after mastering) values[i] -= (values[i] / fabs(values[i]) * 0.1); } } diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index 291da414..94d13d3d 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -59,7 +59,7 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ joint_imp_pub_ = this->create_publisher( "joint_group_impedance_controller/commands", rclcpp::SystemDefaultsQoS()); - + cart_imp_pub_ = this->create_publisher( "cartesian_impedance_controller/commands", rclcpp::SystemDefaultsQoS()); @@ -108,7 +108,7 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ return this->onControllerNameChangeRequest( controller_name, kuka_drivers_core::ControllerType::TORQUE_CONTROLLER_TYPE); }); - + registerParameter( "wrench_controller_name", "", kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const std::string & controller_name) @@ -128,12 +128,14 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ { return this->onJointDampingChangeRequest(joint_damping); }); registerParameter>( - "cartesian_stiffness", cartesian_stiffness_, kuka_drivers_core::ParameterSetAccessRights{true, false}, + "cartesian_stiffness", cartesian_stiffness_, + kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const std::vector & cartesian_stiffness) { return this->onCartesianStiffnessChangeRequest(cartesian_stiffness); }); registerParameter>( - "cartesian_damping", cartesian_damping_, kuka_drivers_core::ParameterSetAccessRights{true, false}, + "cartesian_damping", cartesian_damping_, + kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const std::vector & cartesian_damping) { return this->onCartesianDampingChangeRequest(cartesian_damping); }); } @@ -148,7 +150,7 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) setFriConfiguration(send_period_ms_, receive_multiplier_); // Publish the values of the cartesian impedance parameters to the controller setImpedanceConfiguration(cart_imp_pub_, cartesian_stiffness_, cartesian_damping_); - // Publish the values of the joint impedance parameters to the controller + // Publish the values of the joint impedance parameters to the controller setImpedanceConfiguration(joint_imp_pub_, joint_stiffness_, joint_damping_); // Configure hardware interface @@ -218,7 +220,6 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) { - // Activate hardware interface if (!kuka_drivers_core::changeHardwareState( change_hardware_state_client_, robot_model_, @@ -262,7 +263,9 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) // Stop RT controllers // With best effort strictness, deactivation succeeds if specific controller is not active if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER, kuka_drivers_core::CARTESIAN_IMPEDANCE_CONTROLLER}, + change_controller_state_client_, + {kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER, + kuka_drivers_core::CARTESIAN_IMPEDANCE_CONTROLLER}, {GetControllerName(), kuka_drivers_core::JOINT_STATE_BROADCASTER, kuka_drivers_core::FRI_STATE_BROADCASTER}, SwitchController::Request::BEST_EFFORT)) @@ -444,8 +447,9 @@ void RobotManagerNode::setFriConfiguration(int send_period_ms, int receive_multi fri_config_pub_->publish(msg); } -void RobotManagerNode::setImpedanceConfiguration(const rclcpp::Publisher::SharedPtr & pub, - const std::vector & stiffness, const std::vector & damping) const +void RobotManagerNode::setImpedanceConfiguration( + const rclcpp::Publisher::SharedPtr & pub, + const std::vector & stiffness, const std::vector & damping) const { std_msgs::msg::Float64MultiArray msg; for (std::size_t i = 0; i < stiffness.size(); i++) @@ -496,24 +500,29 @@ bool RobotManagerNode::onJointDampingChangeRequest(const std::vector & j return true; } -bool RobotManagerNode::onCartesianStiffnessChangeRequest(const std::vector & cartesian_stiffness) +bool RobotManagerNode::onCartesianStiffnessChangeRequest( + const std::vector & cartesian_stiffness) { if (cartesian_stiffness.size() != 6) { RCLCPP_ERROR(get_logger(), "Invalid parameter array length for parameter cartesian stiffness"); return false; } - for (std::size_t i=0; i<3; i++){ + for (std::size_t i = 0; i < 3; i++) + { if (cartesian_stiffness[i] < 0 || cartesian_stiffness[i] > 5000) { - RCLCPP_ERROR(get_logger(), "Translational stiffness values must be between 0.0 and 5000.0 (N/m)"); + RCLCPP_ERROR( + get_logger(), "Translational stiffness values must be between 0.0 and 5000.0 (N/m)"); return false; } } - for (std::size_t i=3; i<6; i++){ + for (std::size_t i = 3; i < 6; i++) + { if (cartesian_stiffness[i] < 0 || cartesian_stiffness[i] > 300) { - RCLCPP_ERROR(get_logger(), "Rotaional stiffness values must be between 0.0 and 300.0 (N/rad)"); + RCLCPP_ERROR( + get_logger(), "Rotaional stiffness values must be between 0.0 and 300.0 (N/rad)"); return false; } } @@ -523,7 +532,8 @@ bool RobotManagerNode::onCartesianStiffnessChangeRequest(const std::vector & cartesian_damping) +bool RobotManagerNode::onCartesianDampingChangeRequest( + const std::vector & cartesian_damping) { if (cartesian_damping.size() != 6) { @@ -543,7 +553,6 @@ bool RobotManagerNode::onCartesianDampingChangeRequest(const std::vector return true; } - void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::UInt8::SharedPtr msg) { switch (static_cast(msg->data)) From 78360e5ff8e8465a86c28e4f18b4f9efad552a16 Mon Sep 17 00:00:00 2001 From: Kristofi Misi Date: Tue, 6 Aug 2024 16:43:56 +0200 Subject: [PATCH 09/49] format java --- .../src/ros2/modules/FRIManager.java | 7 +++---- .../src/ros2/modules/ROS2Connection.java | 2 +- ...tesianImpedanceControlModeExternalizable.java | 16 ++++++++-------- .../ros2/serialization/ControlModeParams.java | 6 +++--- 4 files changed, 15 insertions(+), 16 deletions(-) diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java index e98e4214..4e7d55b1 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java @@ -41,10 +41,10 @@ public enum CommandResult{ private ClientCommandMode _clientCommandMode; private IMotionContainer _motionContainer; private FRIMotionErrorHandler _friMotionErrorHandler = new FRIMotionErrorHandler(); - + private static double[] stiffness_ = new double[7]; - - + + public FRIManager(LBR lbr, IApplicationControl applicationControl){ _currentState = new InactiveState(); @@ -261,4 +261,3 @@ public ErrorHandlingAction handleError(Device device, } } - diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java index 0fae9bc0..4f259ba8 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java @@ -99,7 +99,7 @@ private enum ControlModeID{ POSITION( (byte)1), JOINT_IMPEDANCE((byte)2), CARTESIAN_IMPEDANCE((byte)3); - + public final byte value; ControlModeID(byte value){ diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java index ecef94a4..fc3ae5c6 100644 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java @@ -14,21 +14,21 @@ public class CartesianImpedanceControlModeExternalizable extends CartesianImpedanceControlMode implements Externalizable{ public final static int length = 96; - - + + public CartesianImpedanceControlModeExternalizable(CartesianImpedanceControlMode other){ super(other); } - + public CartesianImpedanceControlModeExternalizable(){ super(); } - + public IMotionControlMode toControlMode(){ CartesianImpedanceControlMode controlMode = new CartesianImpedanceControlMode((CartesianImpedanceControlMode)this); return (IMotionControlMode)controlMode; } - + @Override public void writeExternal(ObjectOutput out) throws IOException { for(double cartStiffness : getStiffness()){ @@ -42,7 +42,7 @@ public void writeExternal(ObjectOutput out) throws IOException { @Override public void readExternal(ObjectInput in) throws IOException, ClassNotFoundException { - double[] cartStiffness = new double[getStiffness().length]; + double[] cartStiffness = new double[getStiffness().length]; for(int i = 0; i < getStiffness().length; i++){ cartStiffness[i] = in.readDouble(); } @@ -53,7 +53,7 @@ public void readExternal(ObjectInput in) throws IOException, this.parametrize(CartDOF.B).setStiffness(cartStiffness[4]); this.parametrize(CartDOF.C).setStiffness(cartStiffness[5]); - double[] cartDamping = new double[getDamping().length]; + double[] cartDamping = new double[getDamping().length]; for(int i = 0; i < getDamping().length; i++){ cartDamping[i] = in.readDouble(); } @@ -63,7 +63,7 @@ public void readExternal(ObjectInput in) throws IOException, this.parametrize(CartDOF.A).setDamping(cartDamping[3]); this.parametrize(CartDOF.B).setDamping(cartDamping[4]); this.parametrize(CartDOF.C).setDamping(cartDamping[5]); - + } diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java index f27a235f..8e2b93b4 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java @@ -19,7 +19,7 @@ private enum ControlModeID{ POSITION( (byte)1), JOINT_IMPEDANCE((byte)2), CARTESIAN_IMPEDANCE((byte)3); - + public final byte value; ControlModeID(byte value){ @@ -103,9 +103,9 @@ public JointImpedanceControlModeParams(JointImpedanceControlMode controlMode){ class CartesianImpedanceControlModeParams extends ControlModeParams{ public CartesianImpedanceControlModeParams(){ - + } public CartesianImpedanceControlModeParams(CartesianImpedanceControlMode controlMode){ - + } } From 84db2330f53ee08656b2d763e0693ceb70cf4073 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Wed, 7 Aug 2024 10:40:48 +0200 Subject: [PATCH 10/49] typos, parameter set access rights changed, constexpressions added --- .../wrench_controller/visibility_control.h | 2 +- .../wrench_controller/wrench_controller.hpp | 2 +- controllers/wrench_controller/package.xml | 2 +- .../src/wrench_controller.cpp | 2 +- .../hardware_interface_types.hpp | 4 +++ ...cartesian_impedance_controller_config.yaml | 12 +++---- .../src/hardware_interface.cpp | 36 ++++++++++++------- .../src/robot_manager_node.cpp | 12 +++---- 8 files changed, 44 insertions(+), 28 deletions(-) diff --git a/controllers/wrench_controller/include/wrench_controller/visibility_control.h b/controllers/wrench_controller/include/wrench_controller/visibility_control.h index 2cf8ba57..63acc0e6 100644 --- a/controllers/wrench_controller/include/wrench_controller/visibility_control.h +++ b/controllers/wrench_controller/include/wrench_controller/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright 2023 Aron Svastits +// Copyright 2024 Mihaly Kristofi // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp b/controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp index caa4c51d..8b22e230 100644 --- a/controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp +++ b/controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Aron Svastits +// Copyright 2024 Mihaly Kristofi // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/controllers/wrench_controller/package.xml b/controllers/wrench_controller/package.xml index b80e5d13..e3365a26 100644 --- a/controllers/wrench_controller/package.xml +++ b/controllers/wrench_controller/package.xml @@ -5,7 +5,7 @@ 0.9.2 Controller for forwarding wrench commands - Mihaly Kristofi + Mihaly Kristofi Apache-2.0 diff --git a/controllers/wrench_controller/src/wrench_controller.cpp b/controllers/wrench_controller/src/wrench_controller.cpp index b545f01f..9d7316a2 100644 --- a/controllers/wrench_controller/src/wrench_controller.cpp +++ b/controllers/wrench_controller/src/wrench_controller.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Aron Svastits +// Copyright 2024 Mihaly Kristofi // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp b/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp index d3c45c60..a5b8669c 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp @@ -29,6 +29,10 @@ static constexpr char HW_IF_EXTERNAL_TORQUE[] = "external_torque"; static constexpr char HW_IF_X[] = "x"; static constexpr char HW_IF_Y[] = "y"; static constexpr char HW_IF_Z[] = "z"; +static constexpr char HW_IF_A[] = "a"; +static constexpr char HW_IF_B[] = "b"; +static constexpr char HW_IF_C[] = "c"; +static constexpr char HW_IF_CART_PREFIX[] = "dummy_cart_joint"; /* Interface prefixes */ // Constant defining prefix for I/O interfaces diff --git a/kuka_sunrise_fri_driver/config/cartesian_impedance_controller_config.yaml b/kuka_sunrise_fri_driver/config/cartesian_impedance_controller_config.yaml index d5a5daee..3dfe76fd 100644 --- a/kuka_sunrise_fri_driver/config/cartesian_impedance_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/cartesian_impedance_controller_config.yaml @@ -1,9 +1,9 @@ cartesian_impedance_controller: ros__parameters: joints: - - CartDOF.X - - CartDOF.Y - - CartDOF.Z - - CartDOF.A - - CartDOF.B - - CartDOF.C + - dummy_cart_joint/x + - dummy_cart_joint/y + - dummy_cart_joint/z + - dummy_cart_joint/a + - dummy_cart_joint/b + - dummy_cart_joint/c diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index c4c86f14..2bca9a45 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -504,40 +504,52 @@ KukaFRIHardwareInterface::export_command_interfaces() hardware_interface::HW_IF_Z, &hw_wrench_commands_[5]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("X"), hardware_interface::HW_IF_STIFFNESS, + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_X), hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[0]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("Y"), hardware_interface::HW_IF_STIFFNESS, + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_Y), hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[1]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("Z"), hardware_interface::HW_IF_STIFFNESS, + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_Z), hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[2]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("A"), hardware_interface::HW_IF_STIFFNESS, + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_A), hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[3]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("B"), hardware_interface::HW_IF_STIFFNESS, + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_B), hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[4]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("C"), hardware_interface::HW_IF_STIFFNESS, + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_C), hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[5]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("X"), hardware_interface::HW_IF_DAMPING, + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_X), hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[0]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("Y"), hardware_interface::HW_IF_DAMPING, + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_Y), hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[1]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("Z"), hardware_interface::HW_IF_DAMPING, + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_Z), hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[2]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("A"), hardware_interface::HW_IF_DAMPING, + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_A), hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[3]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("B"), hardware_interface::HW_IF_DAMPING, + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_B), hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[4]); command_interfaces.emplace_back( - std::string("CartDOF") + "." + std::string("C"), hardware_interface::HW_IF_DAMPING, + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + + std::string(hardware_interface::HW_IF_C), hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[5]); return command_interfaces; } diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index 94d13d3d..4ed99a21 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -80,11 +80,11 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ [this](const std::string & controller_ip) { return this->ValidateIPAdress(controller_ip); }); registerParameter( - "send_period_ms", 10, kuka_drivers_core::ParameterSetAccessRights{true, false}, + "send_period_ms", 10, kuka_drivers_core::ParameterSetAccessRights{false, false}, [this](const int & send_period) { return this->onSendPeriodChangeRequest(send_period); }); registerParameter( - "receive_multiplier", 1, kuka_drivers_core::ParameterSetAccessRights{true, false}, + "receive_multiplier", 1, kuka_drivers_core::ParameterSetAccessRights{false, false}, [this](const int & receive_multiplier) { return this->onReceiveMultiplierChangeRequest(receive_multiplier); }); @@ -118,24 +118,24 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ }); registerParameter>( - "joint_stiffness", joint_stiffness_, kuka_drivers_core::ParameterSetAccessRights{true, false}, + "joint_stiffness", joint_stiffness_, kuka_drivers_core::ParameterSetAccessRights{false, false}, [this](const std::vector & joint_stiffness) { return this->onJointStiffnessChangeRequest(joint_stiffness); }); registerParameter>( - "joint_damping", joint_damping_, kuka_drivers_core::ParameterSetAccessRights{true, false}, + "joint_damping", joint_damping_, kuka_drivers_core::ParameterSetAccessRights{false, false}, [this](const std::vector & joint_damping) { return this->onJointDampingChangeRequest(joint_damping); }); registerParameter>( "cartesian_stiffness", cartesian_stiffness_, - kuka_drivers_core::ParameterSetAccessRights{true, false}, + kuka_drivers_core::ParameterSetAccessRights{false, false}, [this](const std::vector & cartesian_stiffness) { return this->onCartesianStiffnessChangeRequest(cartesian_stiffness); }); registerParameter>( "cartesian_damping", cartesian_damping_, - kuka_drivers_core::ParameterSetAccessRights{true, false}, + kuka_drivers_core::ParameterSetAccessRights{false, false}, [this](const std::vector & cartesian_damping) { return this->onCartesianDampingChangeRequest(cartesian_damping); }); } From 91fb08b36bf06a187a71cd726cded10d0bd7761e Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Wed, 7 Aug 2024 14:29:52 +0200 Subject: [PATCH 11/49] comment, exec depend --- kuka_sunrise_fri_driver/package.xml | 1 + kuka_sunrise_fri_driver/src/hardware_interface.cpp | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/kuka_sunrise_fri_driver/package.xml b/kuka_sunrise_fri_driver/package.xml index 78e0db7b..e804a68c 100644 --- a/kuka_sunrise_fri_driver/package.xml +++ b/kuka_sunrise_fri_driver/package.xml @@ -29,6 +29,7 @@ kuka_event_broadcaster joint_group_impedance_controller kuka_lbr_iiwa_support + wrench_controller ros2lifecycle launch_testing_ament_cmake diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 2bca9a45..088b213d 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -208,7 +208,8 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta break; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: fri_connection_->setCartesianImpedanceControlMode( - std::vector(6, 0.0), std::vector(6, 0.1)); + std::vector(6, 0.0), std::vector(6, 0.1)); // 0.1 is the min accepted value for damping, it probably doesn't matter since sthe stiffness is 0 + fri_connection_->setClientCommandMode(ClientCommandModeID::WRENCH_COMMAND_MODE); break; default: From e566cdcf60f6448f83bcc3371ae6f9e9780bb0e4 Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Fri, 9 Aug 2024 15:37:45 +0200 Subject: [PATCH 12/49] wrench control interface joint names change, refactor export control interfaces --- .../config/driver_config.yaml | 2 +- .../config/wrench_controller_config.yaml | 14 +-- .../src/hardware_interface.cpp | 92 ++++--------------- 3 files changed, 26 insertions(+), 82 deletions(-) diff --git a/kuka_sunrise_fri_driver/config/driver_config.yaml b/kuka_sunrise_fri_driver/config/driver_config.yaml index 90218317..8224b86c 100644 --- a/kuka_sunrise_fri_driver/config/driver_config.yaml +++ b/kuka_sunrise_fri_driver/config/driver_config.yaml @@ -1,6 +1,6 @@ robot_manager: ros__parameters: - control_mode: 8 + control_mode: 1 position_controller_name: "joint_trajectory_controller" torque_controller_name: "effort_controller" wrench_controller_name: "wrench_controller" diff --git a/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml b/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml index 0856d93c..2b6bf99a 100644 --- a/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml @@ -1,12 +1,12 @@ wrench_controller: ros__parameters: joints: - - wrench + - dummy_cart_jointx + - dummy_cart_jointy + - dummy_cart_jointz + - dummy_cart_jointa + - dummy_cart_jointb + - dummy_cart_jointc interface_names: - - force/x - - force/z - - force/y - - torque/x - - torque/y - - torque/z + - effort diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 088b213d..826635a5 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -208,7 +208,9 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta break; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: fri_connection_->setCartesianImpedanceControlMode( - std::vector(6, 0.0), std::vector(6, 0.1)); // 0.1 is the min accepted value for damping, it probably doesn't matter since sthe stiffness is 0 + std::vector(6, 0.0), + std::vector(6, 0.1)); // 0.1 is the min accepted value for damping, it probably + // doesn't matter since sthe stiffness is 0 fri_connection_->setClientCommandMode(ClientCommandModeID::WRENCH_COMMAND_MODE); break; @@ -479,79 +481,21 @@ KukaFRIHardwareInterface::export_command_interfaces() command_interfaces.emplace_back( info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_torque_commands_[i]); } - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_FORCE_PREFIX), - hardware_interface::HW_IF_X, &hw_wrench_commands_[0]); - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_FORCE_PREFIX), - hardware_interface::HW_IF_Y, &hw_wrench_commands_[1]); - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_FORCE_PREFIX), - hardware_interface::HW_IF_Z, &hw_wrench_commands_[2]); - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_TORQUE_PREFIX), - hardware_interface::HW_IF_X, &hw_wrench_commands_[3]); - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_TORQUE_PREFIX), - hardware_interface::HW_IF_Y, &hw_wrench_commands_[4]); - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_WRENCH_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_TORQUE_PREFIX), - hardware_interface::HW_IF_Z, &hw_wrench_commands_[5]); - - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_X), hardware_interface::HW_IF_STIFFNESS, - &hw_cart_stiffness_commands_[0]); - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_Y), hardware_interface::HW_IF_STIFFNESS, - &hw_cart_stiffness_commands_[1]); - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_Z), hardware_interface::HW_IF_STIFFNESS, - &hw_cart_stiffness_commands_[2]); - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_A), hardware_interface::HW_IF_STIFFNESS, - &hw_cart_stiffness_commands_[3]); - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_B), hardware_interface::HW_IF_STIFFNESS, - &hw_cart_stiffness_commands_[4]); - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_C), hardware_interface::HW_IF_STIFFNESS, - &hw_cart_stiffness_commands_[5]); - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_X), hardware_interface::HW_IF_DAMPING, - &hw_cart_damping_commands_[0]); - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_Y), hardware_interface::HW_IF_DAMPING, - &hw_cart_damping_commands_[1]); - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_Z), hardware_interface::HW_IF_DAMPING, - &hw_cart_damping_commands_[2]); - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_A), hardware_interface::HW_IF_DAMPING, - &hw_cart_damping_commands_[3]); - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_B), hardware_interface::HW_IF_DAMPING, - &hw_cart_damping_commands_[4]); - command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + - std::string(hardware_interface::HW_IF_C), hardware_interface::HW_IF_DAMPING, - &hw_cart_damping_commands_[5]); + std::vector cart_joints_list = { + hardware_interface::HW_IF_X, hardware_interface::HW_IF_Y, hardware_interface::HW_IF_Z, + hardware_interface::HW_IF_A, hardware_interface::HW_IF_B, hardware_interface::HW_IF_C}; + for (size_t i = 0; i < cart_joints_list.size(); i++) + { + command_interfaces.emplace_back( + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + std::string(cart_joints_list[i]), + hardware_interface::HW_IF_EFFORT, &hw_wrench_commands_[i]); + command_interfaces.emplace_back( + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + std::string(cart_joints_list[i]), + hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[i]); + command_interfaces.emplace_back( + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + std::string(cart_joints_list[i]), + hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[i]); + } return command_interfaces; } From 06a75975a29d5a846180e7cbd8cf6ded72142871 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Mon, 12 Aug 2024 17:04:10 +0200 Subject: [PATCH 13/49] 2.5 FRI client sdk --- .../src/fri_client_sdk/FRIMessages.pb.c | 233 +++-- .../src/fri_client_sdk/FRIMessages.pb.h | 900 +++++++----------- .../fri_client_sdk/friClientApplication.cpp | 412 ++++---- .../src/fri_client_sdk/friClientData.h | 484 +++++----- .../friCommandMessageEncoder.cpp | 247 ++--- .../fri_client_sdk/friCommandMessageEncoder.h | 233 ++--- .../src/fri_client_sdk/friDataHelper.cpp | 178 ++++ .../src/fri_client_sdk/friLBRClient.cpp | 247 ++--- .../src/fri_client_sdk/friLBRCommand.cpp | 262 ++--- .../src/fri_client_sdk/friLBRState.cpp | 503 +++++----- .../friMonitoringMessageDecoder.cpp | 293 +++--- .../friMonitoringMessageDecoder.h | 263 ++--- .../friTransformationClient.cpp | 393 ++++---- .../src/fri_client_sdk/friUdpConnection.cpp | 477 +++++----- .../fri_client_sdk/pb_frimessages_callbacks.c | 536 ++++++----- .../fri_client_sdk/pb_frimessages_callbacks.h | 251 +++-- 16 files changed, 3026 insertions(+), 2886 deletions(-) create mode 100644 kuka_sunrise_fri_driver/src/fri_client_sdk/friDataHelper.cpp diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.c b/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.c index 711f8cfa..17ac32be 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.c +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.c @@ -1,71 +1,162 @@ -/* Automatically generated nanopb constant definitions */ -/* Generated by nanopb-0.4.8-dev */ - -#include "FRIMessages.pb.h" -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -PB_BIND(JointValues, JointValues, AUTO) - - -PB_BIND(TimeStamp, TimeStamp, AUTO) - - -PB_BIND(CartesianVector, CartesianVector, AUTO) - - -PB_BIND(Checksum, Checksum, AUTO) - - -PB_BIND(Transformation, Transformation, AUTO) - - -PB_BIND(FriIOValue, FriIOValue, AUTO) - - -PB_BIND(MessageHeader, MessageHeader, AUTO) - - -PB_BIND(ConnectionInfo, ConnectionInfo, AUTO) - - -PB_BIND(RobotInfo, RobotInfo, AUTO) - - -PB_BIND(MessageMonitorData, MessageMonitorData, 2) - - -PB_BIND(MessageIpoData, MessageIpoData, AUTO) - - -PB_BIND(MessageCommandData, MessageCommandData, 2) - - -PB_BIND(MessageEndOf, MessageEndOf, AUTO) - - -PB_BIND(FRIMonitoringMessage, FRIMonitoringMessage, 2) - - -PB_BIND(FRICommandMessage, FRICommandMessage, 2) - - - - - - - - - - - - - -#ifndef PB_CONVERT_DOUBLE_FLOAT -/* On some platforms (such as AVR), double is really float. - * To be able to encode/decode double on these platforms, you need. - * to define PB_CONVERT_DOUBLE_FLOAT in pb.h or compiler command line. - */ -PB_STATIC_ASSERT(sizeof(double) == 8, DOUBLE_MUST_BE_8_BYTES) -#endif +/* Automatically generated nanopb constant definitions */ +/* Generated by nanopb-0.2.8 at Mon Jul 29 08:35:17 2019. */ + +#include "FRIMessages.pb.h" + + + +const pb_field_t JointValues_fields[2] = { + PB_FIELD2( 1, DOUBLE , REPEATED, CALLBACK, FIRST, JointValues, value, value, 0), + PB_LAST_FIELD +}; + +const pb_field_t TimeStamp_fields[3] = { + PB_FIELD2( 1, UINT32 , REQUIRED, STATIC , FIRST, TimeStamp, sec, sec, 0), + PB_FIELD2( 2, UINT32 , REQUIRED, STATIC , OTHER, TimeStamp, nanosec, sec, 0), + PB_LAST_FIELD +}; + +const pb_field_t CartesianVector_fields[2] = { + PB_FIELD2( 1, DOUBLE , REPEATED, STATIC , FIRST, CartesianVector, element, element, 0), + PB_LAST_FIELD +}; + +const pb_field_t Checksum_fields[2] = { + PB_FIELD2( 1, INT32 , OPTIONAL, STATIC , FIRST, Checksum, crc32, crc32, 0), + PB_LAST_FIELD +}; + +const pb_field_t QuaternionTransformation_fields[4] = { + PB_FIELD2( 1, STRING , REQUIRED, STATIC , FIRST, QuaternionTransformation, name, name, 0), + PB_FIELD2( 2, DOUBLE , REPEATED, STATIC , OTHER, QuaternionTransformation, element, name, 0), + PB_FIELD2( 3, MESSAGE , OPTIONAL, STATIC , OTHER, QuaternionTransformation, timestamp, element, &TimeStamp_fields), + PB_LAST_FIELD +}; + +const pb_field_t FriIOValue_fields[6] = { + PB_FIELD2( 1, STRING , REQUIRED, STATIC , FIRST, FriIOValue, name, name, 0), + PB_FIELD2( 2, ENUM , REQUIRED, STATIC , OTHER, FriIOValue, type, name, 0), + PB_FIELD2( 3, ENUM , REQUIRED, STATIC , OTHER, FriIOValue, direction, type, 0), + PB_FIELD2( 4, UINT64 , OPTIONAL, STATIC , OTHER, FriIOValue, digitalValue, direction, 0), + PB_FIELD2( 5, DOUBLE , OPTIONAL, STATIC , OTHER, FriIOValue, analogValue, digitalValue, 0), + PB_LAST_FIELD +}; + +const pb_field_t RedundancyInformation_fields[3] = { + PB_FIELD2( 1, ENUM , REQUIRED, STATIC , FIRST, RedundancyInformation, strategy, strategy, 0), + PB_FIELD2( 2, DOUBLE , REQUIRED, STATIC , OTHER, RedundancyInformation, value, strategy, 0), + PB_LAST_FIELD +}; + +const pb_field_t MessageHeader_fields[4] = { + PB_FIELD2( 1, UINT32 , REQUIRED, STATIC , FIRST, MessageHeader, messageIdentifier, messageIdentifier, 0), + PB_FIELD2( 2, UINT32 , REQUIRED, STATIC , OTHER, MessageHeader, sequenceCounter, messageIdentifier, 0), + PB_FIELD2( 3, UINT32 , REQUIRED, STATIC , OTHER, MessageHeader, reflectedSequenceCounter, sequenceCounter, 0), + PB_LAST_FIELD +}; + +const pb_field_t ConnectionInfo_fields[5] = { + PB_FIELD2( 1, ENUM , REQUIRED, STATIC , FIRST, ConnectionInfo, sessionState, sessionState, 0), + PB_FIELD2( 2, ENUM , REQUIRED, STATIC , OTHER, ConnectionInfo, quality, sessionState, 0), + PB_FIELD2( 3, UINT32 , OPTIONAL, STATIC , OTHER, ConnectionInfo, sendPeriod, quality, 0), + PB_FIELD2( 4, UINT32 , OPTIONAL, STATIC , OTHER, ConnectionInfo, receiveMultiplier, sendPeriod, 0), + PB_LAST_FIELD +}; + +const pb_field_t RobotInfo_fields[6] = { + PB_FIELD2( 1, INT32 , OPTIONAL, STATIC , FIRST, RobotInfo, numberOfJoints, numberOfJoints, 0), + PB_FIELD2( 2, ENUM , OPTIONAL, STATIC , OTHER, RobotInfo, safetyState, numberOfJoints, 0), + PB_FIELD2( 5, ENUM , REPEATED, CALLBACK, OTHER, RobotInfo, driveState, safetyState, 0), + PB_FIELD2( 6, ENUM , OPTIONAL, STATIC , OTHER, RobotInfo, operationMode, driveState, 0), + PB_FIELD2( 7, ENUM , OPTIONAL, STATIC , OTHER, RobotInfo, controlMode, operationMode, 0), + PB_LAST_FIELD +}; + +const pb_field_t MessageMonitorData_fields[9] = { + PB_FIELD2( 1, MESSAGE , OPTIONAL, STATIC , FIRST, MessageMonitorData, measuredJointPosition, measuredJointPosition, &JointValues_fields), + PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, measuredTorque, measuredJointPosition, &JointValues_fields), + PB_FIELD2( 4, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, commandedTorque, measuredTorque, &JointValues_fields), + PB_FIELD2( 5, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, externalTorque, commandedTorque, &JointValues_fields), + PB_FIELD2( 8, MESSAGE , REPEATED, STATIC , OTHER, MessageMonitorData, readIORequest, externalTorque, &FriIOValue_fields), + PB_FIELD2( 9, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, measuredCartesianPose, readIORequest, &QuaternionTransformation_fields), + PB_FIELD2( 10, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, measuredRedundancyInformation, measuredCartesianPose, &RedundancyInformation_fields), + PB_FIELD2( 15, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, timestamp, measuredRedundancyInformation, &TimeStamp_fields), + PB_LAST_FIELD +}; + +const pb_field_t MessageIpoData_fields[7] = { + PB_FIELD2( 1, MESSAGE , OPTIONAL, STATIC , FIRST, MessageIpoData, jointPosition, jointPosition, &JointValues_fields), + PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, MessageIpoData, cartesianPose, jointPosition, &QuaternionTransformation_fields), + PB_FIELD2( 3, MESSAGE , OPTIONAL, STATIC , OTHER, MessageIpoData, redundancyInformation, cartesianPose, &RedundancyInformation_fields), + PB_FIELD2( 10, ENUM , OPTIONAL, STATIC , OTHER, MessageIpoData, clientCommandMode, redundancyInformation, 0), + PB_FIELD2( 11, ENUM , OPTIONAL, STATIC , OTHER, MessageIpoData, overlayType, clientCommandMode, 0), + PB_FIELD2( 12, DOUBLE , OPTIONAL, STATIC , OTHER, MessageIpoData, trackingPerformance, overlayType, 0), + PB_LAST_FIELD +}; + +const pb_field_t MessageCommandData_fields[8] = { + PB_FIELD2( 1, MESSAGE , OPTIONAL, STATIC , FIRST, MessageCommandData, jointPosition, jointPosition, &JointValues_fields), + PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, MessageCommandData, cartesianWrenchFeedForward, jointPosition, &CartesianVector_fields), + PB_FIELD2( 3, MESSAGE , OPTIONAL, STATIC , OTHER, MessageCommandData, jointTorque, cartesianWrenchFeedForward, &JointValues_fields), + PB_FIELD2( 4, MESSAGE , REPEATED, STATIC , OTHER, MessageCommandData, commandedTransformations, jointTorque, &QuaternionTransformation_fields), + PB_FIELD2( 5, MESSAGE , REPEATED, STATIC , OTHER, MessageCommandData, writeIORequest, commandedTransformations, &FriIOValue_fields), + PB_FIELD2( 6, MESSAGE , OPTIONAL, STATIC , OTHER, MessageCommandData, cartesianPose, writeIORequest, &QuaternionTransformation_fields), + PB_FIELD2( 7, MESSAGE , OPTIONAL, STATIC , OTHER, MessageCommandData, redundancyInformation, cartesianPose, &RedundancyInformation_fields), + PB_LAST_FIELD +}; + +const pb_field_t MessageEndOf_fields[3] = { + PB_FIELD2( 1, INT32 , OPTIONAL, STATIC , FIRST, MessageEndOf, messageLength, messageLength, 0), + PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, MessageEndOf, messageChecksum, messageLength, &Checksum_fields), + PB_LAST_FIELD +}; + +const pb_field_t FRIMonitoringMessage_fields[8] = { + PB_FIELD2( 1, MESSAGE , REQUIRED, STATIC , FIRST, FRIMonitoringMessage, header, header, &MessageHeader_fields), + PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, FRIMonitoringMessage, robotInfo, header, &RobotInfo_fields), + PB_FIELD2( 3, MESSAGE , OPTIONAL, STATIC , OTHER, FRIMonitoringMessage, monitorData, robotInfo, &MessageMonitorData_fields), + PB_FIELD2( 4, MESSAGE , OPTIONAL, STATIC , OTHER, FRIMonitoringMessage, connectionInfo, monitorData, &ConnectionInfo_fields), + PB_FIELD2( 5, MESSAGE , OPTIONAL, STATIC , OTHER, FRIMonitoringMessage, ipoData, connectionInfo, &MessageIpoData_fields), + PB_FIELD2( 6, MESSAGE , REPEATED, STATIC , OTHER, FRIMonitoringMessage, requestedTransformations, ipoData, &QuaternionTransformation_fields), + PB_FIELD2( 15, MESSAGE , OPTIONAL, STATIC , OTHER, FRIMonitoringMessage, endOfMessageData, requestedTransformations, &MessageEndOf_fields), + PB_LAST_FIELD +}; + +const pb_field_t FRICommandMessage_fields[4] = { + PB_FIELD2( 1, MESSAGE , REQUIRED, STATIC , FIRST, FRICommandMessage, header, header, &MessageHeader_fields), + PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, FRICommandMessage, commandData, header, &MessageCommandData_fields), + PB_FIELD2( 15, MESSAGE , OPTIONAL, STATIC , OTHER, FRICommandMessage, endOfMessageData, commandData, &MessageEndOf_fields), + PB_LAST_FIELD +}; + + +/* Check that field information fits in pb_field_t */ +#if !defined(PB_FIELD_32BIT) +/* If you get an error here, it means that you need to define PB_FIELD_32BIT + * compile-time option. You can do that in pb.h or on compiler command line. + * + * The reason you need to do this is that some of your messages contain tag + * numbers or field sizes that are larger than what can fit in 8 or 16 bit + * field descriptors. + */ +STATIC_ASSERT((pb_membersize(QuaternionTransformation, timestamp) < 65536 && pb_membersize(MessageMonitorData, measuredJointPosition) < 65536 && pb_membersize(MessageMonitorData, measuredTorque) < 65536 && pb_membersize(MessageMonitorData, commandedTorque) < 65536 && pb_membersize(MessageMonitorData, externalTorque) < 65536 && pb_membersize(MessageMonitorData, readIORequest[0]) < 65536 && pb_membersize(MessageMonitorData, measuredCartesianPose) < 65536 && pb_membersize(MessageMonitorData, measuredRedundancyInformation) < 65536 && pb_membersize(MessageMonitorData, timestamp) < 65536 && pb_membersize(MessageIpoData, jointPosition) < 65536 && pb_membersize(MessageIpoData, cartesianPose) < 65536 && pb_membersize(MessageIpoData, redundancyInformation) < 65536 && pb_membersize(MessageCommandData, jointPosition) < 65536 && pb_membersize(MessageCommandData, cartesianWrenchFeedForward) < 65536 && pb_membersize(MessageCommandData, jointTorque) < 65536 && pb_membersize(MessageCommandData, commandedTransformations[0]) < 65536 && pb_membersize(MessageCommandData, writeIORequest[0]) < 65536 && pb_membersize(MessageCommandData, cartesianPose) < 65536 && pb_membersize(MessageCommandData, redundancyInformation) < 65536 && pb_membersize(MessageEndOf, messageChecksum) < 65536 && pb_membersize(FRIMonitoringMessage, header) < 65536 && pb_membersize(FRIMonitoringMessage, connectionInfo) < 65536 && pb_membersize(FRIMonitoringMessage, robotInfo) < 65536 && pb_membersize(FRIMonitoringMessage, monitorData) < 65536 && pb_membersize(FRIMonitoringMessage, ipoData) < 65536 && pb_membersize(FRIMonitoringMessage, requestedTransformations[0]) < 65536 && pb_membersize(FRIMonitoringMessage, endOfMessageData) < 65536 && pb_membersize(FRICommandMessage, header) < 65536 && pb_membersize(FRICommandMessage, commandData) < 65536 && pb_membersize(FRICommandMessage, endOfMessageData) < 65536), YOU_MUST_DEFINE_PB_FIELD_32BIT_FOR_MESSAGES_JointValues_TimeStamp_CartesianVector_Checksum_QuaternionTransformation_FriIOValue_RedundancyInformation_MessageHeader_ConnectionInfo_RobotInfo_MessageMonitorData_MessageIpoData_MessageCommandData_MessageEndOf_FRIMonitoringMessage_FRICommandMessage) +#endif + +#if !defined(PB_FIELD_16BIT) && !defined(PB_FIELD_32BIT) +/* If you get an error here, it means that you need to define PB_FIELD_16BIT + * compile-time option. You can do that in pb.h or on compiler command line. + * + * The reason you need to do this is that some of your messages contain tag + * numbers or field sizes that are larger than what can fit in the default + * 8 bit descriptors. + */ +STATIC_ASSERT((pb_membersize(QuaternionTransformation, timestamp) < 256 && pb_membersize(MessageMonitorData, measuredJointPosition) < 256 && pb_membersize(MessageMonitorData, measuredTorque) < 256 && pb_membersize(MessageMonitorData, commandedTorque) < 256 && pb_membersize(MessageMonitorData, externalTorque) < 256 && pb_membersize(MessageMonitorData, readIORequest[0]) < 256 && pb_membersize(MessageMonitorData, measuredCartesianPose) < 256 && pb_membersize(MessageMonitorData, measuredRedundancyInformation) < 256 && pb_membersize(MessageMonitorData, timestamp) < 256 && pb_membersize(MessageIpoData, jointPosition) < 256 && pb_membersize(MessageIpoData, cartesianPose) < 256 && pb_membersize(MessageIpoData, redundancyInformation) < 256 && pb_membersize(MessageCommandData, jointPosition) < 256 && pb_membersize(MessageCommandData, cartesianWrenchFeedForward) < 256 && pb_membersize(MessageCommandData, jointTorque) < 256 && pb_membersize(MessageCommandData, commandedTransformations[0]) < 256 && pb_membersize(MessageCommandData, writeIORequest[0]) < 256 && pb_membersize(MessageCommandData, cartesianPose) < 256 && pb_membersize(MessageCommandData, redundancyInformation) < 256 && pb_membersize(MessageEndOf, messageChecksum) < 256 && pb_membersize(FRIMonitoringMessage, header) < 256 && pb_membersize(FRIMonitoringMessage, connectionInfo) < 256 && pb_membersize(FRIMonitoringMessage, robotInfo) < 256 && pb_membersize(FRIMonitoringMessage, monitorData) < 256 && pb_membersize(FRIMonitoringMessage, ipoData) < 256 && pb_membersize(FRIMonitoringMessage, requestedTransformations[0]) < 256 && pb_membersize(FRIMonitoringMessage, endOfMessageData) < 256 && pb_membersize(FRICommandMessage, header) < 256 && pb_membersize(FRICommandMessage, commandData) < 256 && pb_membersize(FRICommandMessage, endOfMessageData) < 256), YOU_MUST_DEFINE_PB_FIELD_16BIT_FOR_MESSAGES_JointValues_TimeStamp_CartesianVector_Checksum_QuaternionTransformation_FriIOValue_RedundancyInformation_MessageHeader_ConnectionInfo_RobotInfo_MessageMonitorData_MessageIpoData_MessageCommandData_MessageEndOf_FRIMonitoringMessage_FRICommandMessage) +#endif + + +/* On some platforms (such as AVR), double is really float. + * These are not directly supported by nanopb, but see example_avr_double. + * To get rid of this error, remove any double fields from your .proto. + */ +STATIC_ASSERT(sizeof(double) == 8, DOUBLE_MUST_BE_8_BYTES) + diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.h b/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.h index 57982d00..052b4330 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.h +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.h @@ -1,567 +1,333 @@ -/* Automatically generated nanopb header */ -/* Generated by nanopb-0.4.8-dev */ - -#ifndef PB_FRIMESSAGES_PB_H_INCLUDED -#define PB_FRIMESSAGES_PB_H_INCLUDED -#include - -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -/* Enum definitions */ -typedef enum _FRISessionState { - FRISessionState_IDLE = 0, - FRISessionState_MONITORING_WAIT = 1, - FRISessionState_MONITORING_READY = 2, - FRISessionState_COMMANDING_WAIT = 3, - FRISessionState_COMMANDING_ACTIVE = 4 -} FRISessionState; - -typedef enum _FRIConnectionQuality { - FRIConnectionQuality_POOR = 0, - FRIConnectionQuality_FAIR = 1, - FRIConnectionQuality_GOOD = 2, - FRIConnectionQuality_EXCELLENT = 3 -} FRIConnectionQuality; - -typedef enum _SafetyState { - SafetyState_NORMAL_OPERATION = 0, - SafetyState_SAFETY_STOP_LEVEL_0 = 1, - SafetyState_SAFETY_STOP_LEVEL_1 = 2, - SafetyState_SAFETY_STOP_LEVEL_2 = 3 -} SafetyState; - -typedef enum _OperationMode { - OperationMode_TEST_MODE_1 = 0, - OperationMode_TEST_MODE_2 = 1, - OperationMode_AUTOMATIC_MODE = 2 -} OperationMode; - -typedef enum _DriveState { - DriveState_OFF = 0, - DriveState_TRANSITIONING = 1, - DriveState_ACTIVE = 2 -} DriveState; - -typedef enum _ControlMode { - ControlMode_POSITION_CONTROLMODE = 0, - ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE = 1, - ControlMode_JOINT_IMPEDANCE_CONTROLMODE = 2, - ControlMode_NO_CONTROLMODE = 3 -} ControlMode; - -typedef enum _ClientCommandMode { - ClientCommandMode_NO_COMMAND_MODE = 0, - ClientCommandMode_POSITION = 1, - ClientCommandMode_WRENCH = 2, - ClientCommandMode_TORQUE = 3 -} ClientCommandMode; - -typedef enum _OverlayType { - OverlayType_NO_OVERLAY = 0, - OverlayType_JOINT = 1, - OverlayType_CARTESIAN = 2 -} OverlayType; - -typedef enum _FriIOType { - FriIOType_BOOLEAN = 0, - FriIOType_DIGITAL = 1, - FriIOType_ANALOG = 2 -} FriIOType; - -typedef enum _FriIODirection { - FriIODirection_INPUT = 0, - FriIODirection_OUTPUT = 1 -} FriIODirection; - -/* Struct definitions */ -typedef struct _JointValues { - pb_callback_t value; -} JointValues; - -typedef struct _TimeStamp { - uint32_t sec; - uint32_t nanosec; -} TimeStamp; - -typedef struct _CartesianVector { - pb_size_t element_count; - double element[6]; -} CartesianVector; - -typedef struct _Checksum { - bool has_crc32; - int32_t crc32; -} Checksum; - -typedef struct _Transformation { - char name[64]; - pb_size_t matrix_count; - double matrix[12]; - bool has_timestamp; - TimeStamp timestamp; -} Transformation; - -typedef struct _FriIOValue { - char name[64]; - FriIOType type; - FriIODirection direction; - bool has_digitalValue; - uint64_t digitalValue; - bool has_analogValue; - double analogValue; -} FriIOValue; - -typedef struct _MessageHeader { - uint32_t messageIdentifier; - uint32_t sequenceCounter; - uint32_t reflectedSequenceCounter; -} MessageHeader; - -typedef struct _ConnectionInfo { - FRISessionState sessionState; - FRIConnectionQuality quality; - bool has_sendPeriod; - uint32_t sendPeriod; - bool has_receiveMultiplier; - uint32_t receiveMultiplier; -} ConnectionInfo; - -typedef struct _RobotInfo { - bool has_numberOfJoints; - int32_t numberOfJoints; - bool has_safetyState; - SafetyState safetyState; - pb_callback_t driveState; - bool has_operationMode; - OperationMode operationMode; - bool has_controlMode; - ControlMode controlMode; -} RobotInfo; - -typedef struct _MessageMonitorData { - bool has_measuredJointPosition; - JointValues measuredJointPosition; - bool has_measuredTorque; - JointValues measuredTorque; - bool has_commandedJointPosition; - JointValues commandedJointPosition; - bool has_commandedTorque; - JointValues commandedTorque; - bool has_externalTorque; - JointValues externalTorque; - pb_size_t readIORequest_count; - FriIOValue readIORequest[10]; - bool has_timestamp; - TimeStamp timestamp; -} MessageMonitorData; - -typedef struct _MessageIpoData { - bool has_jointPosition; - JointValues jointPosition; - bool has_clientCommandMode; - ClientCommandMode clientCommandMode; - bool has_overlayType; - OverlayType overlayType; - bool has_trackingPerformance; - double trackingPerformance; -} MessageIpoData; - -typedef struct _MessageCommandData { - bool has_jointPosition; - JointValues jointPosition; - bool has_cartesianWrenchFeedForward; - CartesianVector cartesianWrenchFeedForward; - bool has_jointTorque; - JointValues jointTorque; - pb_size_t commandedTransformations_count; - Transformation commandedTransformations[5]; - pb_size_t writeIORequest_count; - FriIOValue writeIORequest[10]; -} MessageCommandData; - -typedef struct _MessageEndOf { - bool has_messageLength; - int32_t messageLength; - bool has_messageChecksum; - Checksum messageChecksum; -} MessageEndOf; - -typedef struct _FRIMonitoringMessage { - MessageHeader header; - bool has_robotInfo; - RobotInfo robotInfo; - bool has_monitorData; - MessageMonitorData monitorData; - bool has_connectionInfo; - ConnectionInfo connectionInfo; - bool has_ipoData; - MessageIpoData ipoData; - pb_size_t requestedTransformations_count; - Transformation requestedTransformations[5]; - bool has_endOfMessageData; - MessageEndOf endOfMessageData; -} FRIMonitoringMessage; - -typedef struct _FRICommandMessage { - MessageHeader header; - bool has_commandData; - MessageCommandData commandData; - bool has_endOfMessageData; - MessageEndOf endOfMessageData; -} FRICommandMessage; - - -#ifdef __cplusplus -extern "C" { -#endif - -/* Helper constants for enums */ -#define _FRISessionState_MIN FRISessionState_IDLE -#define _FRISessionState_MAX FRISessionState_COMMANDING_ACTIVE -#define _FRISessionState_ARRAYSIZE ((FRISessionState)(FRISessionState_COMMANDING_ACTIVE+1)) - -#define _FRIConnectionQuality_MIN FRIConnectionQuality_POOR -#define _FRIConnectionQuality_MAX FRIConnectionQuality_EXCELLENT -#define _FRIConnectionQuality_ARRAYSIZE ((FRIConnectionQuality)(FRIConnectionQuality_EXCELLENT+1)) - -#define _SafetyState_MIN SafetyState_NORMAL_OPERATION -#define _SafetyState_MAX SafetyState_SAFETY_STOP_LEVEL_2 -#define _SafetyState_ARRAYSIZE ((SafetyState)(SafetyState_SAFETY_STOP_LEVEL_2+1)) - -#define _OperationMode_MIN OperationMode_TEST_MODE_1 -#define _OperationMode_MAX OperationMode_AUTOMATIC_MODE -#define _OperationMode_ARRAYSIZE ((OperationMode)(OperationMode_AUTOMATIC_MODE+1)) - -#define _DriveState_MIN DriveState_OFF -#define _DriveState_MAX DriveState_ACTIVE -#define _DriveState_ARRAYSIZE ((DriveState)(DriveState_ACTIVE+1)) - -#define _ControlMode_MIN ControlMode_POSITION_CONTROLMODE -#define _ControlMode_MAX ControlMode_NO_CONTROLMODE -#define _ControlMode_ARRAYSIZE ((ControlMode)(ControlMode_NO_CONTROLMODE+1)) - -#define _ClientCommandMode_MIN ClientCommandMode_NO_COMMAND_MODE -#define _ClientCommandMode_MAX ClientCommandMode_TORQUE -#define _ClientCommandMode_ARRAYSIZE ((ClientCommandMode)(ClientCommandMode_TORQUE+1)) - -#define _OverlayType_MIN OverlayType_NO_OVERLAY -#define _OverlayType_MAX OverlayType_CARTESIAN -#define _OverlayType_ARRAYSIZE ((OverlayType)(OverlayType_CARTESIAN+1)) - -#define _FriIOType_MIN FriIOType_BOOLEAN -#define _FriIOType_MAX FriIOType_ANALOG -#define _FriIOType_ARRAYSIZE ((FriIOType)(FriIOType_ANALOG+1)) - -#define _FriIODirection_MIN FriIODirection_INPUT -#define _FriIODirection_MAX FriIODirection_OUTPUT -#define _FriIODirection_ARRAYSIZE ((FriIODirection)(FriIODirection_OUTPUT+1)) - - - - - - -#define FriIOValue_type_ENUMTYPE FriIOType -#define FriIOValue_direction_ENUMTYPE FriIODirection - - -#define ConnectionInfo_sessionState_ENUMTYPE FRISessionState -#define ConnectionInfo_quality_ENUMTYPE FRIConnectionQuality - -#define RobotInfo_safetyState_ENUMTYPE SafetyState -#define RobotInfo_driveState_ENUMTYPE DriveState -#define RobotInfo_operationMode_ENUMTYPE OperationMode -#define RobotInfo_controlMode_ENUMTYPE ControlMode - - -#define MessageIpoData_clientCommandMode_ENUMTYPE ClientCommandMode -#define MessageIpoData_overlayType_ENUMTYPE OverlayType - - - - - - -/* Initializer values for message structs */ -#define JointValues_init_default {{{NULL}, NULL}} -#define TimeStamp_init_default {0, 0} -#define CartesianVector_init_default {0, {0, 0, 0, 0, 0, 0}} -#define Checksum_init_default {false, 0} -#define Transformation_init_default {"", 0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, false, TimeStamp_init_default} -#define FriIOValue_init_default {"", _FriIOType_MIN, _FriIODirection_MIN, false, 0, false, 0} -#define MessageHeader_init_default {0, 0, 0} -#define ConnectionInfo_init_default {_FRISessionState_MIN, _FRIConnectionQuality_MIN, false, 0, false, 0} -#define RobotInfo_init_default {false, 0, false, _SafetyState_MIN, {{NULL}, NULL}, false, _OperationMode_MIN, false, _ControlMode_MIN} -#define MessageMonitorData_init_default {false, JointValues_init_default, false, JointValues_init_default, false, JointValues_init_default, false, JointValues_init_default, false, JointValues_init_default, 0, {FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default}, false, TimeStamp_init_default} -#define MessageIpoData_init_default {false, JointValues_init_default, false, _ClientCommandMode_MIN, false, _OverlayType_MIN, false, 0} -#define MessageCommandData_init_default {false, JointValues_init_default, false, CartesianVector_init_default, false, JointValues_init_default, 0, {Transformation_init_default, Transformation_init_default, Transformation_init_default, Transformation_init_default, Transformation_init_default}, 0, {FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default}} -#define MessageEndOf_init_default {false, 0, false, Checksum_init_default} -#define FRIMonitoringMessage_init_default {MessageHeader_init_default, false, RobotInfo_init_default, false, MessageMonitorData_init_default, false, ConnectionInfo_init_default, false, MessageIpoData_init_default, 0, {Transformation_init_default, Transformation_init_default, Transformation_init_default, Transformation_init_default, Transformation_init_default}, false, MessageEndOf_init_default} -#define FRICommandMessage_init_default {MessageHeader_init_default, false, MessageCommandData_init_default, false, MessageEndOf_init_default} -#define JointValues_init_zero {{{NULL}, NULL}} -#define TimeStamp_init_zero {0, 0} -#define CartesianVector_init_zero {0, {0, 0, 0, 0, 0, 0}} -#define Checksum_init_zero {false, 0} -#define Transformation_init_zero {"", 0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, false, TimeStamp_init_zero} -#define FriIOValue_init_zero {"", _FriIOType_MIN, _FriIODirection_MIN, false, 0, false, 0} -#define MessageHeader_init_zero {0, 0, 0} -#define ConnectionInfo_init_zero {_FRISessionState_MIN, _FRIConnectionQuality_MIN, false, 0, false, 0} -#define RobotInfo_init_zero {false, 0, false, _SafetyState_MIN, {{NULL}, NULL}, false, _OperationMode_MIN, false, _ControlMode_MIN} -#define MessageMonitorData_init_zero {false, JointValues_init_zero, false, JointValues_init_zero, false, JointValues_init_zero, false, JointValues_init_zero, false, JointValues_init_zero, 0, {FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero}, false, TimeStamp_init_zero} -#define MessageIpoData_init_zero {false, JointValues_init_zero, false, _ClientCommandMode_MIN, false, _OverlayType_MIN, false, 0} -#define MessageCommandData_init_zero {false, JointValues_init_zero, false, CartesianVector_init_zero, false, JointValues_init_zero, 0, {Transformation_init_zero, Transformation_init_zero, Transformation_init_zero, Transformation_init_zero, Transformation_init_zero}, 0, {FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero}} -#define MessageEndOf_init_zero {false, 0, false, Checksum_init_zero} -#define FRIMonitoringMessage_init_zero {MessageHeader_init_zero, false, RobotInfo_init_zero, false, MessageMonitorData_init_zero, false, ConnectionInfo_init_zero, false, MessageIpoData_init_zero, 0, {Transformation_init_zero, Transformation_init_zero, Transformation_init_zero, Transformation_init_zero, Transformation_init_zero}, false, MessageEndOf_init_zero} -#define FRICommandMessage_init_zero {MessageHeader_init_zero, false, MessageCommandData_init_zero, false, MessageEndOf_init_zero} - -/* Field tags (for use in manual encoding/decoding) */ -#define JointValues_value_tag 1 -#define TimeStamp_sec_tag 1 -#define TimeStamp_nanosec_tag 2 -#define CartesianVector_element_tag 1 -#define Checksum_crc32_tag 1 -#define Transformation_name_tag 1 -#define Transformation_matrix_tag 2 -#define Transformation_timestamp_tag 3 -#define FriIOValue_name_tag 1 -#define FriIOValue_type_tag 2 -#define FriIOValue_direction_tag 3 -#define FriIOValue_digitalValue_tag 4 -#define FriIOValue_analogValue_tag 5 -#define MessageHeader_messageIdentifier_tag 1 -#define MessageHeader_sequenceCounter_tag 2 -#define MessageHeader_reflectedSequenceCounter_tag 3 -#define ConnectionInfo_sessionState_tag 1 -#define ConnectionInfo_quality_tag 2 -#define ConnectionInfo_sendPeriod_tag 3 -#define ConnectionInfo_receiveMultiplier_tag 4 -#define RobotInfo_numberOfJoints_tag 1 -#define RobotInfo_safetyState_tag 2 -#define RobotInfo_driveState_tag 5 -#define RobotInfo_operationMode_tag 6 -#define RobotInfo_controlMode_tag 7 -#define MessageMonitorData_measuredJointPosition_tag 1 -#define MessageMonitorData_measuredTorque_tag 2 -#define MessageMonitorData_commandedJointPosition_tag 3 -#define MessageMonitorData_commandedTorque_tag 4 -#define MessageMonitorData_externalTorque_tag 5 -#define MessageMonitorData_readIORequest_tag 8 -#define MessageMonitorData_timestamp_tag 15 -#define MessageIpoData_jointPosition_tag 1 -#define MessageIpoData_clientCommandMode_tag 10 -#define MessageIpoData_overlayType_tag 11 -#define MessageIpoData_trackingPerformance_tag 12 -#define MessageCommandData_jointPosition_tag 1 -#define MessageCommandData_cartesianWrenchFeedForward_tag 2 -#define MessageCommandData_jointTorque_tag 3 -#define MessageCommandData_commandedTransformations_tag 4 -#define MessageCommandData_writeIORequest_tag 5 -#define MessageEndOf_messageLength_tag 1 -#define MessageEndOf_messageChecksum_tag 2 -#define FRIMonitoringMessage_header_tag 1 -#define FRIMonitoringMessage_robotInfo_tag 2 -#define FRIMonitoringMessage_monitorData_tag 3 -#define FRIMonitoringMessage_connectionInfo_tag 4 -#define FRIMonitoringMessage_ipoData_tag 5 -#define FRIMonitoringMessage_requestedTransformations_tag 6 -#define FRIMonitoringMessage_endOfMessageData_tag 15 -#define FRICommandMessage_header_tag 1 -#define FRICommandMessage_commandData_tag 2 -#define FRICommandMessage_endOfMessageData_tag 15 - -/* Struct field encoding specification for nanopb */ -#define JointValues_FIELDLIST(X, a) \ -X(a, CALLBACK, REPEATED, DOUBLE, value, 1) -#define JointValues_CALLBACK pb_default_field_callback -#define JointValues_DEFAULT NULL - -#define TimeStamp_FIELDLIST(X, a) \ -X(a, STATIC, REQUIRED, UINT32, sec, 1) \ -X(a, STATIC, REQUIRED, UINT32, nanosec, 2) -#define TimeStamp_CALLBACK NULL -#define TimeStamp_DEFAULT NULL - -#define CartesianVector_FIELDLIST(X, a) \ -X(a, STATIC, REPEATED, DOUBLE, element, 1) -#define CartesianVector_CALLBACK NULL -#define CartesianVector_DEFAULT NULL - -#define Checksum_FIELDLIST(X, a) \ -X(a, STATIC, OPTIONAL, INT32, crc32, 1) -#define Checksum_CALLBACK NULL -#define Checksum_DEFAULT NULL - -#define Transformation_FIELDLIST(X, a) \ -X(a, STATIC, REQUIRED, STRING, name, 1) \ -X(a, STATIC, REPEATED, DOUBLE, matrix, 2) \ -X(a, STATIC, OPTIONAL, MESSAGE, timestamp, 3) -#define Transformation_CALLBACK NULL -#define Transformation_DEFAULT NULL -#define Transformation_timestamp_MSGTYPE TimeStamp - -#define FriIOValue_FIELDLIST(X, a) \ -X(a, STATIC, REQUIRED, STRING, name, 1) \ -X(a, STATIC, REQUIRED, UENUM, type, 2) \ -X(a, STATIC, REQUIRED, UENUM, direction, 3) \ -X(a, STATIC, OPTIONAL, UINT64, digitalValue, 4) \ -X(a, STATIC, OPTIONAL, DOUBLE, analogValue, 5) -#define FriIOValue_CALLBACK NULL -#define FriIOValue_DEFAULT NULL - -#define MessageHeader_FIELDLIST(X, a) \ -X(a, STATIC, REQUIRED, UINT32, messageIdentifier, 1) \ -X(a, STATIC, REQUIRED, UINT32, sequenceCounter, 2) \ -X(a, STATIC, REQUIRED, UINT32, reflectedSequenceCounter, 3) -#define MessageHeader_CALLBACK NULL -#define MessageHeader_DEFAULT NULL - -#define ConnectionInfo_FIELDLIST(X, a) \ -X(a, STATIC, REQUIRED, UENUM, sessionState, 1) \ -X(a, STATIC, REQUIRED, UENUM, quality, 2) \ -X(a, STATIC, OPTIONAL, UINT32, sendPeriod, 3) \ -X(a, STATIC, OPTIONAL, UINT32, receiveMultiplier, 4) -#define ConnectionInfo_CALLBACK NULL -#define ConnectionInfo_DEFAULT NULL - -#define RobotInfo_FIELDLIST(X, a) \ -X(a, STATIC, OPTIONAL, INT32, numberOfJoints, 1) \ -X(a, STATIC, OPTIONAL, UENUM, safetyState, 2) \ -X(a, CALLBACK, REPEATED, UENUM, driveState, 5) \ -X(a, STATIC, OPTIONAL, UENUM, operationMode, 6) \ -X(a, STATIC, OPTIONAL, UENUM, controlMode, 7) -#define RobotInfo_CALLBACK pb_default_field_callback -#define RobotInfo_DEFAULT NULL - -#define MessageMonitorData_FIELDLIST(X, a) \ -X(a, STATIC, OPTIONAL, MESSAGE, measuredJointPosition, 1) \ -X(a, STATIC, OPTIONAL, MESSAGE, measuredTorque, 2) \ -X(a, STATIC, OPTIONAL, MESSAGE, commandedJointPosition, 3) \ -X(a, STATIC, OPTIONAL, MESSAGE, commandedTorque, 4) \ -X(a, STATIC, OPTIONAL, MESSAGE, externalTorque, 5) \ -X(a, STATIC, REPEATED, MESSAGE, readIORequest, 8) \ -X(a, STATIC, OPTIONAL, MESSAGE, timestamp, 15) -#define MessageMonitorData_CALLBACK NULL -#define MessageMonitorData_DEFAULT NULL -#define MessageMonitorData_measuredJointPosition_MSGTYPE JointValues -#define MessageMonitorData_measuredTorque_MSGTYPE JointValues -#define MessageMonitorData_commandedJointPosition_MSGTYPE JointValues -#define MessageMonitorData_commandedTorque_MSGTYPE JointValues -#define MessageMonitorData_externalTorque_MSGTYPE JointValues -#define MessageMonitorData_readIORequest_MSGTYPE FriIOValue -#define MessageMonitorData_timestamp_MSGTYPE TimeStamp - -#define MessageIpoData_FIELDLIST(X, a) \ -X(a, STATIC, OPTIONAL, MESSAGE, jointPosition, 1) \ -X(a, STATIC, OPTIONAL, UENUM, clientCommandMode, 10) \ -X(a, STATIC, OPTIONAL, UENUM, overlayType, 11) \ -X(a, STATIC, OPTIONAL, DOUBLE, trackingPerformance, 12) -#define MessageIpoData_CALLBACK NULL -#define MessageIpoData_DEFAULT NULL -#define MessageIpoData_jointPosition_MSGTYPE JointValues - -#define MessageCommandData_FIELDLIST(X, a) \ -X(a, STATIC, OPTIONAL, MESSAGE, jointPosition, 1) \ -X(a, STATIC, OPTIONAL, MESSAGE, cartesianWrenchFeedForward, 2) \ -X(a, STATIC, OPTIONAL, MESSAGE, jointTorque, 3) \ -X(a, STATIC, REPEATED, MESSAGE, commandedTransformations, 4) \ -X(a, STATIC, REPEATED, MESSAGE, writeIORequest, 5) -#define MessageCommandData_CALLBACK NULL -#define MessageCommandData_DEFAULT NULL -#define MessageCommandData_jointPosition_MSGTYPE JointValues -#define MessageCommandData_cartesianWrenchFeedForward_MSGTYPE CartesianVector -#define MessageCommandData_jointTorque_MSGTYPE JointValues -#define MessageCommandData_commandedTransformations_MSGTYPE Transformation -#define MessageCommandData_writeIORequest_MSGTYPE FriIOValue - -#define MessageEndOf_FIELDLIST(X, a) \ -X(a, STATIC, OPTIONAL, INT32, messageLength, 1) \ -X(a, STATIC, OPTIONAL, MESSAGE, messageChecksum, 2) -#define MessageEndOf_CALLBACK NULL -#define MessageEndOf_DEFAULT NULL -#define MessageEndOf_messageChecksum_MSGTYPE Checksum - -#define FRIMonitoringMessage_FIELDLIST(X, a) \ -X(a, STATIC, REQUIRED, MESSAGE, header, 1) \ -X(a, STATIC, OPTIONAL, MESSAGE, robotInfo, 2) \ -X(a, STATIC, OPTIONAL, MESSAGE, monitorData, 3) \ -X(a, STATIC, OPTIONAL, MESSAGE, connectionInfo, 4) \ -X(a, STATIC, OPTIONAL, MESSAGE, ipoData, 5) \ -X(a, STATIC, REPEATED, MESSAGE, requestedTransformations, 6) \ -X(a, STATIC, OPTIONAL, MESSAGE, endOfMessageData, 15) -#define FRIMonitoringMessage_CALLBACK NULL -#define FRIMonitoringMessage_DEFAULT NULL -#define FRIMonitoringMessage_header_MSGTYPE MessageHeader -#define FRIMonitoringMessage_robotInfo_MSGTYPE RobotInfo -#define FRIMonitoringMessage_monitorData_MSGTYPE MessageMonitorData -#define FRIMonitoringMessage_connectionInfo_MSGTYPE ConnectionInfo -#define FRIMonitoringMessage_ipoData_MSGTYPE MessageIpoData -#define FRIMonitoringMessage_requestedTransformations_MSGTYPE Transformation -#define FRIMonitoringMessage_endOfMessageData_MSGTYPE MessageEndOf - -#define FRICommandMessage_FIELDLIST(X, a) \ -X(a, STATIC, REQUIRED, MESSAGE, header, 1) \ -X(a, STATIC, OPTIONAL, MESSAGE, commandData, 2) \ -X(a, STATIC, OPTIONAL, MESSAGE, endOfMessageData, 15) -#define FRICommandMessage_CALLBACK NULL -#define FRICommandMessage_DEFAULT NULL -#define FRICommandMessage_header_MSGTYPE MessageHeader -#define FRICommandMessage_commandData_MSGTYPE MessageCommandData -#define FRICommandMessage_endOfMessageData_MSGTYPE MessageEndOf - -extern const pb_msgdesc_t JointValues_msg; -extern const pb_msgdesc_t TimeStamp_msg; -extern const pb_msgdesc_t CartesianVector_msg; -extern const pb_msgdesc_t Checksum_msg; -extern const pb_msgdesc_t Transformation_msg; -extern const pb_msgdesc_t FriIOValue_msg; -extern const pb_msgdesc_t MessageHeader_msg; -extern const pb_msgdesc_t ConnectionInfo_msg; -extern const pb_msgdesc_t RobotInfo_msg; -extern const pb_msgdesc_t MessageMonitorData_msg; -extern const pb_msgdesc_t MessageIpoData_msg; -extern const pb_msgdesc_t MessageCommandData_msg; -extern const pb_msgdesc_t MessageEndOf_msg; -extern const pb_msgdesc_t FRIMonitoringMessage_msg; -extern const pb_msgdesc_t FRICommandMessage_msg; - -/* Defines for backwards compatibility with code written before nanopb-0.4.0 */ -#define JointValues_fields &JointValues_msg -#define TimeStamp_fields &TimeStamp_msg -#define CartesianVector_fields &CartesianVector_msg -#define Checksum_fields &Checksum_msg -#define Transformation_fields &Transformation_msg -#define FriIOValue_fields &FriIOValue_msg -#define MessageHeader_fields &MessageHeader_msg -#define ConnectionInfo_fields &ConnectionInfo_msg -#define RobotInfo_fields &RobotInfo_msg -#define MessageMonitorData_fields &MessageMonitorData_msg -#define MessageIpoData_fields &MessageIpoData_msg -#define MessageCommandData_fields &MessageCommandData_msg -#define MessageEndOf_fields &MessageEndOf_msg -#define FRIMonitoringMessage_fields &FRIMonitoringMessage_msg -#define FRICommandMessage_fields &FRICommandMessage_msg - -/* Maximum encoded size of messages (where known) */ -/* JointValues_size depends on runtime parameters */ -/* RobotInfo_size depends on runtime parameters */ -/* MessageMonitorData_size depends on runtime parameters */ -/* MessageIpoData_size depends on runtime parameters */ -/* MessageCommandData_size depends on runtime parameters */ -/* FRIMonitoringMessage_size depends on runtime parameters */ -/* FRICommandMessage_size depends on runtime parameters */ -#define CartesianVector_size 54 -#define Checksum_size 11 -#define ConnectionInfo_size 16 -#define FriIOValue_size 89 -#define MessageEndOf_size 24 -#define MessageHeader_size 18 -#define TimeStamp_size 12 -#define Transformation_size 187 - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif +/* Automatically generated nanopb header */ +/* Generated by nanopb-0.2.8 at Mon Jul 29 08:35:17 2019. */ + +#ifndef _PB_FRIMESSAGES_PB_H_ +#define _PB_FRIMESSAGES_PB_H_ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Enum definitions */ +typedef enum _FRISessionState { + FRISessionState_IDLE = 0, + FRISessionState_MONITORING_WAIT = 1, + FRISessionState_MONITORING_READY = 2, + FRISessionState_COMMANDING_WAIT = 3, + FRISessionState_COMMANDING_ACTIVE = 4 +} FRISessionState; + +typedef enum _FRIConnectionQuality { + FRIConnectionQuality_POOR = 0, + FRIConnectionQuality_FAIR = 1, + FRIConnectionQuality_GOOD = 2, + FRIConnectionQuality_EXCELLENT = 3 +} FRIConnectionQuality; + +typedef enum _SafetyState { + SafetyState_NORMAL_OPERATION = 0, + SafetyState_SAFETY_STOP_LEVEL_0 = 1, + SafetyState_SAFETY_STOP_LEVEL_1 = 2, + SafetyState_SAFETY_STOP_LEVEL_2 = 3 +} SafetyState; + +typedef enum _OperationMode { + OperationMode_TEST_MODE_1 = 0, + OperationMode_TEST_MODE_2 = 1, + OperationMode_AUTOMATIC_MODE = 2 +} OperationMode; + +typedef enum _DriveState { + DriveState_OFF = 0, + DriveState_TRANSITIONING = 1, + DriveState_ACTIVE = 2 +} DriveState; + +typedef enum _ControlMode { + ControlMode_POSITION_CONTROLMODE = 0, + ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE = 1, + ControlMode_JOINT_IMPEDANCE_CONTROLMODE = 2, + ControlMode_NO_CONTROLMODE = 3 +} ControlMode; + +typedef enum _ClientCommandMode { + ClientCommandMode_NO_COMMAND_MODE = 0, + ClientCommandMode_JOINT_POSITION = 1, + ClientCommandMode_WRENCH = 2, + ClientCommandMode_TORQUE = 3, + ClientCommandMode_CARTESIAN_POSE = 4 +} ClientCommandMode; + +typedef enum _OverlayType { + OverlayType_NO_OVERLAY = 0, + OverlayType_JOINT = 1, + OverlayType_CARTESIAN = 2 +} OverlayType; + +typedef enum _FriIOType { + FriIOType_BOOLEAN = 0, + FriIOType_DIGITAL = 1, + FriIOType_ANALOG = 2 +} FriIOType; + +typedef enum _FriIODirection { + FriIODirection_INPUT = 0, + FriIODirection_OUTPUT = 1 +} FriIODirection; + +typedef enum _RedundancyStrategy { + RedundancyStrategy_E1 = 0, + RedundancyStrategy_NONE = 4 +} RedundancyStrategy; + +/* Struct definitions */ +typedef struct _CartesianVector { + size_t element_count; + double element[6]; +} CartesianVector; + +typedef struct _Checksum { + bool has_crc32; + int32_t crc32; +} Checksum; + +typedef struct _ConnectionInfo { + FRISessionState sessionState; + FRIConnectionQuality quality; + bool has_sendPeriod; + uint32_t sendPeriod; + bool has_receiveMultiplier; + uint32_t receiveMultiplier; +} ConnectionInfo; + +typedef struct _FriIOValue { + char name[64]; + FriIOType type; + FriIODirection direction; + bool has_digitalValue; + uint64_t digitalValue; + bool has_analogValue; + double analogValue; +} FriIOValue; + +typedef struct _JointValues { + pb_callback_t value; +} JointValues; + +typedef struct _MessageHeader { + uint32_t messageIdentifier; + uint32_t sequenceCounter; + uint32_t reflectedSequenceCounter; +} MessageHeader; + +typedef struct _RedundancyInformation { + RedundancyStrategy strategy; + double value; +} RedundancyInformation; + +typedef struct _RobotInfo { + bool has_numberOfJoints; + int32_t numberOfJoints; + bool has_safetyState; + SafetyState safetyState; + pb_callback_t driveState; + bool has_operationMode; + OperationMode operationMode; + bool has_controlMode; + ControlMode controlMode; +} RobotInfo; + +typedef struct _TimeStamp { + uint32_t sec; + uint32_t nanosec; +} TimeStamp; + +typedef struct _MessageEndOf { + bool has_messageLength; + int32_t messageLength; + bool has_messageChecksum; + Checksum messageChecksum; +} MessageEndOf; + +typedef struct _QuaternionTransformation { + char name[64]; + size_t element_count; + double element[7]; + bool has_timestamp; + TimeStamp timestamp; +} QuaternionTransformation; + +typedef struct _MessageCommandData { + bool has_jointPosition; + JointValues jointPosition; + bool has_cartesianWrenchFeedForward; + CartesianVector cartesianWrenchFeedForward; + bool has_jointTorque; + JointValues jointTorque; + size_t commandedTransformations_count; + QuaternionTransformation commandedTransformations[5]; + size_t writeIORequest_count; + FriIOValue writeIORequest[10]; + bool has_cartesianPose; + QuaternionTransformation cartesianPose; + bool has_redundancyInformation; + RedundancyInformation redundancyInformation; +} MessageCommandData; + +typedef struct _MessageIpoData { + bool has_jointPosition; + JointValues jointPosition; + bool has_cartesianPose; + QuaternionTransformation cartesianPose; + bool has_redundancyInformation; + RedundancyInformation redundancyInformation; + bool has_clientCommandMode; + ClientCommandMode clientCommandMode; + bool has_overlayType; + OverlayType overlayType; + bool has_trackingPerformance; + double trackingPerformance; +} MessageIpoData; + +typedef struct _MessageMonitorData { + bool has_measuredJointPosition; + JointValues measuredJointPosition; + bool has_measuredTorque; + JointValues measuredTorque; + bool has_commandedTorque; + JointValues commandedTorque; + bool has_externalTorque; + JointValues externalTorque; + size_t readIORequest_count; + FriIOValue readIORequest[10]; + bool has_measuredCartesianPose; + QuaternionTransformation measuredCartesianPose; + bool has_measuredRedundancyInformation; + RedundancyInformation measuredRedundancyInformation; + bool has_timestamp; + TimeStamp timestamp; +} MessageMonitorData; + +typedef struct _FRICommandMessage { + MessageHeader header; + bool has_commandData; + MessageCommandData commandData; + bool has_endOfMessageData; + MessageEndOf endOfMessageData; +} FRICommandMessage; + +typedef struct _FRIMonitoringMessage { + MessageHeader header; + bool has_robotInfo; + RobotInfo robotInfo; + bool has_monitorData; + MessageMonitorData monitorData; + bool has_connectionInfo; + ConnectionInfo connectionInfo; + bool has_ipoData; + MessageIpoData ipoData; + size_t requestedTransformations_count; + QuaternionTransformation requestedTransformations[5]; + bool has_endOfMessageData; + MessageEndOf endOfMessageData; +} FRIMonitoringMessage; + +/* Default values for struct fields */ + +/* Field tags (for use in manual encoding/decoding) */ +#define CartesianVector_element_tag 1 +#define Checksum_crc32_tag 1 +#define ConnectionInfo_sessionState_tag 1 +#define ConnectionInfo_quality_tag 2 +#define ConnectionInfo_sendPeriod_tag 3 +#define ConnectionInfo_receiveMultiplier_tag 4 +#define FriIOValue_name_tag 1 +#define FriIOValue_type_tag 2 +#define FriIOValue_direction_tag 3 +#define FriIOValue_digitalValue_tag 4 +#define FriIOValue_analogValue_tag 5 +#define JointValues_value_tag 1 +#define MessageHeader_messageIdentifier_tag 1 +#define MessageHeader_sequenceCounter_tag 2 +#define MessageHeader_reflectedSequenceCounter_tag 3 +#define RedundancyInformation_strategy_tag 1 +#define RedundancyInformation_value_tag 2 +#define RobotInfo_numberOfJoints_tag 1 +#define RobotInfo_safetyState_tag 2 +#define RobotInfo_driveState_tag 5 +#define RobotInfo_operationMode_tag 6 +#define RobotInfo_controlMode_tag 7 +#define TimeStamp_sec_tag 1 +#define TimeStamp_nanosec_tag 2 +#define MessageEndOf_messageLength_tag 1 +#define MessageEndOf_messageChecksum_tag 2 +#define QuaternionTransformation_name_tag 1 +#define QuaternionTransformation_element_tag 2 +#define QuaternionTransformation_timestamp_tag 3 +#define MessageCommandData_jointPosition_tag 1 +#define MessageCommandData_cartesianWrenchFeedForward_tag 2 +#define MessageCommandData_jointTorque_tag 3 +#define MessageCommandData_commandedTransformations_tag 4 +#define MessageCommandData_writeIORequest_tag 5 +#define MessageCommandData_cartesianPose_tag 6 +#define MessageCommandData_redundancyInformation_tag 7 +#define MessageIpoData_jointPosition_tag 1 +#define MessageIpoData_cartesianPose_tag 2 +#define MessageIpoData_redundancyInformation_tag 3 +#define MessageIpoData_clientCommandMode_tag 10 +#define MessageIpoData_overlayType_tag 11 +#define MessageIpoData_trackingPerformance_tag 12 +#define MessageMonitorData_measuredJointPosition_tag 1 +#define MessageMonitorData_measuredTorque_tag 2 +#define MessageMonitorData_commandedTorque_tag 4 +#define MessageMonitorData_externalTorque_tag 5 +#define MessageMonitorData_readIORequest_tag 8 +#define MessageMonitorData_measuredCartesianPose_tag 9 +#define MessageMonitorData_measuredRedundancyInformation_tag 10 +#define MessageMonitorData_timestamp_tag 15 +#define FRICommandMessage_header_tag 1 +#define FRICommandMessage_commandData_tag 2 +#define FRICommandMessage_endOfMessageData_tag 15 +#define FRIMonitoringMessage_header_tag 1 +#define FRIMonitoringMessage_connectionInfo_tag 4 +#define FRIMonitoringMessage_robotInfo_tag 2 +#define FRIMonitoringMessage_monitorData_tag 3 +#define FRIMonitoringMessage_ipoData_tag 5 +#define FRIMonitoringMessage_requestedTransformations_tag 6 +#define FRIMonitoringMessage_endOfMessageData_tag 15 + +/* Struct field encoding specification for nanopb */ +extern const pb_field_t JointValues_fields[2]; +extern const pb_field_t TimeStamp_fields[3]; +extern const pb_field_t CartesianVector_fields[2]; +extern const pb_field_t Checksum_fields[2]; +extern const pb_field_t QuaternionTransformation_fields[4]; +extern const pb_field_t FriIOValue_fields[6]; +extern const pb_field_t RedundancyInformation_fields[3]; +extern const pb_field_t MessageHeader_fields[4]; +extern const pb_field_t ConnectionInfo_fields[5]; +extern const pb_field_t RobotInfo_fields[6]; +extern const pb_field_t MessageMonitorData_fields[9]; +extern const pb_field_t MessageIpoData_fields[7]; +extern const pb_field_t MessageCommandData_fields[8]; +extern const pb_field_t MessageEndOf_fields[3]; +extern const pb_field_t FRIMonitoringMessage_fields[8]; +extern const pb_field_t FRICommandMessage_fields[4]; + +/* Maximum encoded size of messages (where known) */ +#define TimeStamp_size 12 +#define CartesianVector_size 54 +#define Checksum_size 11 +#define QuaternionTransformation_size 143 +#define FriIOValue_size 98 +#define RedundancyInformation_size 15 +#define MessageHeader_size 18 +#define ConnectionInfo_size 24 +#define MessageEndOf_size 24 + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientApplication.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientApplication.cpp index d94eec77..340c71ce 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientApplication.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientApplication.cpp @@ -1,201 +1,211 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace KUKA::FRI; - -//****************************************************************************** -ClientApplication::ClientApplication(IConnection & connection, IClient & client) -: _connection(connection), _robotClient(&client), _trafoClient(NULL), _data(NULL) -{ - _data = _robotClient->createData(); -} - -//****************************************************************************** -ClientApplication::ClientApplication( - IConnection & connection, IClient & client, - TransformationClient & trafoClient) -: _connection(connection), _robotClient(&client), _trafoClient(&trafoClient), _data(NULL) -{ - _data = _robotClient->createData(); - _trafoClient->linkData(_data); -} - -//****************************************************************************** -ClientApplication::~ClientApplication() -{ - disconnect(); - delete _data; -} - -//****************************************************************************** -bool ClientApplication::connect(int port, const char * remoteHost) -{ - if (_connection.isOpen()) { - std::cout << "Warning: client application already connected!" << std::endl; - return true; - } - - return _connection.open(port, remoteHost); -} - -//****************************************************************************** -void ClientApplication::disconnect() -{ - if (_connection.isOpen()) {_connection.close();} -} - -//****************************************************************************** -bool ClientApplication::step() -{ - if (!_connection.isOpen()) { - std::cout << "Error: client application is not connected!" << std::endl; - return false; - } - - // ************************************************************************** - // Receive and decode new monitoring message - // ************************************************************************** - int size = _connection.receive(_data->receiveBuffer, FRI_MONITOR_MSG_MAX_SIZE); - - if (size <= 0) { // TODO: size == 0 -> connection closed (maybe go to IDLE instead of stopping?) - std::cout << "Error: failed while trying to receive monitoring message!" << std::endl; - return false; - } - - if (!_data->decoder.decode(_data->receiveBuffer, size)) { - return false; - } - - // check message type (so that our wrappers match) - if (_data->expectedMonitorMsgID != _data->monitoringMsg.header.messageIdentifier) { - std::cout << "Error: incompatible IDs for received message, got: " << - _data->monitoringMsg.header.messageIdentifier << " expected: " << _data->expectedMonitorMsgID; - return false; - } - - // ************************************************************************** - // callbacks - // ************************************************************************** - // reset command message before callbacks - _data->resetCommandMessage(); - - // callbacks for robot client - ESessionState currentState = (ESessionState)_data->monitoringMsg.connectionInfo.sessionState; - - if (_data->lastState != currentState) { - _robotClient->onStateChange(_data->lastState, currentState); - _data->lastState = currentState; - } - - switch (currentState) { - case MONITORING_WAIT: - case MONITORING_READY: - _robotClient->monitor(); - break; - case COMMANDING_WAIT: - _robotClient->waitForCommand(); - break; - case COMMANDING_ACTIVE: - _robotClient->command(); - break; - case IDLE: - default: - return true; // nothing to send back - } - - // callback for transformation client - if (_trafoClient != NULL) { - _trafoClient->provide(); - } - - // ************************************************************************** - // Encode and send command message - // ************************************************************************** - - _data->lastSendCounter++; - // check if its time to send an answer - if (_data->lastSendCounter >= _data->monitoringMsg.connectionInfo.receiveMultiplier) { - _data->lastSendCounter = 0; - - // set sequence counters - _data->commandMsg.header.sequenceCounter = _data->sequenceCounter++; - _data->commandMsg.header.reflectedSequenceCounter = - _data->monitoringMsg.header.sequenceCounter; - - if (!_data->encoder.encode(_data->sendBuffer, size)) { - return false; - } - - if (!_connection.send(_data->sendBuffer, size)) { - std::cout << "Error: failed while trying to send command message!" << std::endl; - return false; - } - } - - return true; -} +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#include +#include "friClientApplication.h" +#include "friClientIf.h" +#include "friConnectionIf.h" +#include "friClientData.h" +#include "FRIMessages.pb.h" +#include "friTransformationClient.h" + +using namespace KUKA::FRI; + +//****************************************************************************** +ClientApplication::ClientApplication(IConnection& connection, IClient& client) + : _connection(connection), _robotClient(&client),_trafoClient(NULL), _data(NULL) +{ + _data = _robotClient->createData(); +} + +//****************************************************************************** +ClientApplication::ClientApplication(IConnection& connection, IClient& client, TransformationClient& trafoClient) + : _connection(connection), _robotClient(&client), _trafoClient(&trafoClient), _data(NULL) +{ + _data = _robotClient->createData(); + _trafoClient->linkData(_data); +} + +//****************************************************************************** +ClientApplication::~ClientApplication() +{ + disconnect(); + delete _data; +} + +//****************************************************************************** +bool ClientApplication::connect(int port, const char *remoteHost) +{ + if (_connection.isOpen()) + { + printf("Warning: client application already connected!\n"); + return true; + } + + return _connection.open(port, remoteHost); +} + +//****************************************************************************** +void ClientApplication::disconnect() +{ + if (_connection.isOpen()) _connection.close(); +} + +//****************************************************************************** +bool ClientApplication::step() +{ + if (!_connection.isOpen()) + { + printf("Error: client application is not connected!\n"); + return false; + } + + // ************************************************************************** + // Receive and decode new monitoring message + // ************************************************************************** + int size = _connection.receive(_data->receiveBuffer, FRI_MONITOR_MSG_MAX_SIZE); + + if (size <= 0) + { // TODO: size == 0 -> connection closed (maybe go to IDLE instead of stopping?) + printf("Error: failed while trying to receive monitoring message!\n"); + return false; + } + + if (!_data->decoder.decode(_data->receiveBuffer, size)) + { + return false; + } + + // check message type (so that our wrappers match) + if (_data->expectedMonitorMsgID != _data->monitoringMsg.header.messageIdentifier) + { + printf("Error: incompatible IDs for received message (got: %d expected %d)!\n", + (int)_data->monitoringMsg.header.messageIdentifier, + (int)_data->expectedMonitorMsgID); + return false; + } + + // ************************************************************************** + // callbacks + // ************************************************************************** + // reset command message before callbacks + _data->resetCommandMessage(); + + // callbacks for robot client + ESessionState currentState = (ESessionState)_data->monitoringMsg.connectionInfo.sessionState; + + if (_data->lastState != currentState) + { + _robotClient->onStateChange(_data->lastState, currentState); + _data->lastState = currentState; + } + + switch (currentState) + { + case MONITORING_WAIT: + case MONITORING_READY: + _robotClient->monitor(); + break; + case COMMANDING_WAIT: + _robotClient->waitForCommand(); + break; + case COMMANDING_ACTIVE: + _robotClient->command(); + break; + case IDLE: + default: + return true; // nothing to send back + } + + // callback for transformation client + if(_trafoClient != NULL) + { + _trafoClient->provide(); + } + + // ************************************************************************** + // Encode and send command message + // ************************************************************************** + + _data->lastSendCounter++; + // check if its time to send an answer + if (_data->lastSendCounter >= _data->monitoringMsg.connectionInfo.receiveMultiplier) + { + _data->lastSendCounter = 0; + + // set sequence counters + _data->commandMsg.header.sequenceCounter = _data->sequenceCounter++; + _data->commandMsg.header.reflectedSequenceCounter = + _data->monitoringMsg.header.sequenceCounter; + + if (!_data->encoder.encode(_data->sendBuffer, size)) + { + return false; + } + + if (!_connection.send(_data->sendBuffer, size)) + { + printf("Error: failed while trying to send command message!\n"); + return false; + } + } + + return true; +} + diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientData.h b/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientData.h index 25f7af06..1fb04e5f 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientData.h +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientData.h @@ -1,242 +1,242 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#ifndef _KUKA_FRI_CLIENT_DATA_H -#define _KUKA_FRI_CLIENT_DATA_H - -#include - -#include -#include -#include -#include -#include - -namespace KUKA -{ -namespace FRI -{ - -struct ClientData -{ - char receiveBuffer[FRI_MONITOR_MSG_MAX_SIZE]; //!< monitoring message receive buffer - char sendBuffer[FRI_COMMAND_MSG_MAX_SIZE]; //!< command message send buffer - - FRIMonitoringMessage monitoringMsg; //!< monitoring message struct - FRICommandMessage commandMsg; //!< command message struct - - MonitoringMessageDecoder decoder; //!< monitoring message decoder - CommandMessageEncoder encoder; //!< command message encoder - - ESessionState lastState; //!< last FRI state - uint32_t sequenceCounter; //!< sequence counter for command messages - uint32_t lastSendCounter; //!< steps since last send command - uint32_t expectedMonitorMsgID; //!< expected ID for received monitoring messages - - const size_t MAX_REQUESTED_TRANSFORMATIONS; //!< maximum count of requested transformations - const size_t MAX_SIZE_TRANSFORMATION_ID; //!< maximum size in bytes of a transformation ID - std::vector requestedTrafoIDs; //!< list of requested transformation ids - - ClientData(int numDofs) - : decoder(&monitoringMsg, numDofs), - encoder(&commandMsg, numDofs), - lastState(IDLE), - sequenceCounter(0), - lastSendCounter(0), - expectedMonitorMsgID(0), - MAX_REQUESTED_TRANSFORMATIONS(sizeof(monitoringMsg.requestedTransformations) / - sizeof(monitoringMsg.requestedTransformations[0])), - MAX_SIZE_TRANSFORMATION_ID(sizeof(monitoringMsg.requestedTransformations[0].name)) - { - requestedTrafoIDs.reserve(MAX_REQUESTED_TRANSFORMATIONS); - } - - void resetCommandMessage() - { - commandMsg.commandData.has_jointPosition = false; - commandMsg.commandData.has_cartesianWrenchFeedForward = false; - commandMsg.commandData.has_jointTorque = false; - commandMsg.commandData.commandedTransformations_count = 0; - commandMsg.has_commandData = false; - commandMsg.commandData.writeIORequest_count = 0; - } - - //****************************************************************************** - static const FriIOValue & getBooleanIOValue( - const FRIMonitoringMessage * message, - const char * name) - { - return getIOValue(message, name, FriIOType_BOOLEAN); - } - - //****************************************************************************** - static const FriIOValue & getDigitalIOValue( - const FRIMonitoringMessage * message, - const char * name) - { - return getIOValue(message, name, FriIOType_DIGITAL); - } - - //****************************************************************************** - static const FriIOValue & getAnalogIOValue( - const FRIMonitoringMessage * message, - const char * name) - { - return getIOValue(message, name, FriIOType_ANALOG); - } - - //****************************************************************************** - static void setBooleanIOValue( - FRICommandMessage * message, const char * name, const bool value, - const FRIMonitoringMessage * monMessage) - { - setIOValue(message, name, monMessage, FriIOType_BOOLEAN).digitalValue = value; - } - - //****************************************************************************** - static void setDigitalIOValue( - FRICommandMessage * message, const char * name, const unsigned long long value, - const FRIMonitoringMessage * monMessage) - { - setIOValue(message, name, monMessage, FriIOType_DIGITAL).digitalValue = value; - } - - //****************************************************************************** - static void setAnalogIOValue( - FRICommandMessage * message, const char * name, const double value, - const FRIMonitoringMessage * monMessage) - { - setIOValue(message, name, monMessage, FriIOType_ANALOG).analogValue = value; - } - -protected: - //****************************************************************************** - static const FriIOValue & getIOValue( - const FRIMonitoringMessage * message, const char * name, - const FriIOType ioType) - { - if (message != NULL && message->has_monitorData == true) { - const MessageMonitorData & monData = message->monitorData; - const bool analogValue = (ioType == FriIOType_ANALOG); - const bool digitalValue = (ioType == FriIOType_DIGITAL || ioType == FriIOType_BOOLEAN); - for (size_t i = 0; i < monData.readIORequest_count; i++) { - const FriIOValue & ioValue = monData.readIORequest[i]; - if (strcmp(name, ioValue.name) == 0) { - if (ioValue.type == ioType && - ioValue.has_digitalValue == digitalValue && - ioValue.has_analogValue == analogValue) - { - return ioValue; - } - - const char * ioTypeName; - switch (ioType) { - case FriIOType_ANALOG: ioTypeName = "analog value"; break; - case FriIOType_DIGITAL: ioTypeName = "digital value"; break; - case FriIOType_BOOLEAN: ioTypeName = "boolean"; break; - default: ioTypeName = "?"; break; - } - - throw FRIException("IO %s is not of type %s.", name, ioTypeName); - } - } - } - - throw FRIException("Could not locate IO %s in monitor message.", name); - } - - //****************************************************************************** - static FriIOValue & setIOValue( - FRICommandMessage * message, const char * name, - const FRIMonitoringMessage * monMessage, const FriIOType ioType) - { - MessageCommandData & cmdData = message->commandData; - const size_t maxIOs = sizeof(cmdData.writeIORequest) / sizeof(cmdData.writeIORequest[0]); - if (cmdData.writeIORequest_count < maxIOs) { - // call getter which will raise an exception if the output doesn't exist - // or is of wrong type. - if (getIOValue(monMessage, name, ioType).direction != FriIODirection_OUTPUT) { - throw FRIException("IO %s is not an output value.", name); - } - - // add IO value to command message - FriIOValue & ioValue = cmdData.writeIORequest[cmdData.writeIORequest_count]; - - strncpy(ioValue.name, name, sizeof(ioValue.name) - 1); - ioValue.name[sizeof(ioValue.name) - 1] = 0; // ensure termination - ioValue.type = ioType; - ioValue.has_digitalValue = (ioType == FriIOType_DIGITAL || ioType == FriIOType_BOOLEAN); - ioValue.has_analogValue = (ioType == FriIOType_ANALOG); - ioValue.direction = FriIODirection_OUTPUT; - - cmdData.writeIORequest_count++; - message->has_commandData = true; - - return ioValue; - } else { - throw FRIException("Exceeded maximum number of IOs that can be set."); - } - } -}; - -} -} - - -#endif // _KUKA_FRI_CLIENT_DATA_H +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_CLIENT_DATA_H +#define _KUKA_FRI_CLIENT_DATA_H + +#include + +#include "FRIMessages.pb.h" +#include "friMonitoringMessageDecoder.h" +#include "friCommandMessageEncoder.h" +#include "friClientIf.h" +#include "friException.h" + +namespace KUKA +{ +namespace FRI +{ + + struct ClientData + { + char receiveBuffer[FRI_MONITOR_MSG_MAX_SIZE];//!< monitoring message receive buffer + char sendBuffer[FRI_COMMAND_MSG_MAX_SIZE]; //!< command message send buffer + + FRIMonitoringMessage monitoringMsg; //!< monitoring message struct + FRICommandMessage commandMsg; //!< command message struct + + MonitoringMessageDecoder decoder; //!< monitoring message decoder + CommandMessageEncoder encoder; //!< command message encoder + + ESessionState lastState; //!< last FRI state + uint32_t sequenceCounter; //!< sequence counter for command messages + uint32_t lastSendCounter; //!< steps since last send command + uint32_t expectedMonitorMsgID; //!< expected ID for received monitoring messages + + const size_t MAX_REQUESTED_TRANSFORMATIONS; //!< maximum count of requested transformations + const size_t MAX_SIZE_TRANSFORMATION_ID; //!< maximum size in bytes of a transformation ID + std::vector requestedTrafoIDs; //!< list of requested transformation ids + + ClientData(int numDofs) + : decoder(&monitoringMsg, numDofs), + encoder(&commandMsg, numDofs), + lastState(IDLE), + sequenceCounter(0), + lastSendCounter(0), + expectedMonitorMsgID(0), + MAX_REQUESTED_TRANSFORMATIONS(sizeof(monitoringMsg.requestedTransformations) / + sizeof(monitoringMsg.requestedTransformations[0])), + MAX_SIZE_TRANSFORMATION_ID(sizeof(monitoringMsg.requestedTransformations[0].name)) + { + requestedTrafoIDs.reserve(MAX_REQUESTED_TRANSFORMATIONS); + } + + void resetCommandMessage() + { + commandMsg.commandData.has_jointPosition = false; + commandMsg.commandData.has_cartesianWrenchFeedForward = false; + commandMsg.commandData.has_jointTorque = false; + commandMsg.commandData.commandedTransformations_count = 0; + commandMsg.commandData.has_cartesianPose = false; + commandMsg.commandData.has_redundancyInformation = false; + commandMsg.has_commandData = false; + commandMsg.commandData.writeIORequest_count = 0; + } + + //****************************************************************************** + static const FriIOValue& getBooleanIOValue(const FRIMonitoringMessage* message, const char* name) + { + return getIOValue(message, name, FriIOType_BOOLEAN); + } + + //****************************************************************************** + static const FriIOValue& getDigitalIOValue(const FRIMonitoringMessage* message, const char* name) + { + return getIOValue(message, name, FriIOType_DIGITAL); + } + + //****************************************************************************** + static const FriIOValue& getAnalogIOValue(const FRIMonitoringMessage* message, const char* name) + { + return getIOValue(message, name, FriIOType_ANALOG); + } + + //****************************************************************************** + static void setBooleanIOValue(FRICommandMessage* message, const char* name, const bool value, + const FRIMonitoringMessage* monMessage) + { + setIOValue(message, name, monMessage, FriIOType_BOOLEAN).digitalValue = value; + } + + //****************************************************************************** + static void setDigitalIOValue(FRICommandMessage* message, const char* name, const unsigned long long value, + const FRIMonitoringMessage* monMessage) + { + setIOValue(message, name, monMessage, FriIOType_DIGITAL).digitalValue = value; + } + + //****************************************************************************** + static void setAnalogIOValue(FRICommandMessage* message, const char* name, const double value, + const FRIMonitoringMessage* monMessage) + { + setIOValue(message, name, monMessage, FriIOType_ANALOG).analogValue = value; + } + + protected: + + //****************************************************************************** + static const FriIOValue& getIOValue(const FRIMonitoringMessage* message, const char* name, + const FriIOType ioType) + { + if(message != NULL && message->has_monitorData == true) + { + const MessageMonitorData& monData = message->monitorData; + const bool analogValue = (ioType == FriIOType_ANALOG); + const bool digitalValue = (ioType == FriIOType_DIGITAL) | (ioType == FriIOType_BOOLEAN); + for(size_t i = 0; i < monData.readIORequest_count; i++) + { + const FriIOValue& ioValue = monData.readIORequest[i]; + if(strcmp(name, ioValue.name) == 0) + { + if(ioValue.type == ioType && + ioValue.has_digitalValue == digitalValue && + ioValue.has_analogValue == analogValue) + { + return ioValue; + } + + const char* ioTypeName; + switch(ioType) + { + case FriIOType_ANALOG: ioTypeName = "analog"; break; + case FriIOType_DIGITAL: ioTypeName = "digital"; break; + case FriIOType_BOOLEAN: ioTypeName = "boolean"; break; + default: ioTypeName = "?"; break; + } + + throw FRIException("IO %s is not of type %s.", name, ioTypeName); + } + } + } + + throw FRIException("Could not locate IO %s in monitor message.", name); + } + + //****************************************************************************** + static FriIOValue& setIOValue(FRICommandMessage* message, const char* name, + const FRIMonitoringMessage* monMessage, const FriIOType ioType) + { + MessageCommandData& cmdData = message->commandData; + const size_t maxIOs = sizeof(cmdData.writeIORequest) / sizeof(cmdData.writeIORequest[0]); + if(cmdData.writeIORequest_count < maxIOs) + { + // call getter which will raise an exception if the output doesn't exist + // or is of wrong type. + if(getIOValue(monMessage, name, ioType).direction != FriIODirection_OUTPUT) + { + throw FRIException("IO %s is not an output value.", name); + } + + // add IO value to command message + FriIOValue& ioValue = cmdData.writeIORequest[cmdData.writeIORequest_count]; + + strncpy(ioValue.name, name, sizeof(ioValue.name) - 1); + ioValue.name[sizeof(ioValue.name) - 1] = 0; // ensure termination + ioValue.type = ioType; + ioValue.has_digitalValue = (ioType == FriIOType_DIGITAL) | (ioType == FriIOType_BOOLEAN); + ioValue.has_analogValue = (ioType == FriIOType_ANALOG); + ioValue.direction = FriIODirection_OUTPUT; + + cmdData.writeIORequest_count ++; + message->has_commandData = true; + + return ioValue; + } + else + { + throw FRIException("Exceeded maximum number of outputs that can be set."); + } + } + }; + +} +} + + +#endif // _KUKA_FRI_CLIENT_DATA_H diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.cpp index c4e118f0..751095d1 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.cpp @@ -1,122 +1,125 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#include -#include -#include - -using namespace KUKA::FRI; - -//****************************************************************************** -CommandMessageEncoder::CommandMessageEncoder(FRICommandMessage * pMessage, int num) -: m_nNum(num), m_pMessage(pMessage) -{ - initMessage(); -} - -//****************************************************************************** -CommandMessageEncoder::~CommandMessageEncoder() -{ - -} - -//****************************************************************************** -void CommandMessageEncoder::initMessage() -{ - m_pMessage->has_commandData = false; - m_pMessage->has_endOfMessageData = false; - m_pMessage->commandData.has_jointPosition = false; - m_pMessage->commandData.has_cartesianWrenchFeedForward = false; - m_pMessage->commandData.has_jointTorque = false; - m_pMessage->commandData.commandedTransformations_count = 0; - m_pMessage->header.messageIdentifier = 0; - // init with 0. Necessary for creating the correct reflected sequence count in the monitoring msg - m_pMessage->header.sequenceCounter = 0; - m_pMessage->header.reflectedSequenceCounter = 0; - - m_pMessage->commandData.writeIORequest_count = 0; - - // allocate and map memory for protobuf repeated structures - map_repeatedDouble( - FRI_MANAGER_NANOPB_ENCODE, m_nNum, - &m_pMessage->commandData.jointPosition.value, - &m_tRecvContainer.jointPosition); - map_repeatedDouble( - FRI_MANAGER_NANOPB_ENCODE, m_nNum, - &m_pMessage->commandData.jointTorque.value, - &m_tRecvContainer.jointTorque); - - // nanopb encoding needs to know how many elements the static array contains - // a Cartesian wrench feed forward vector always contains 6 elements - m_pMessage->commandData.cartesianWrenchFeedForward.element_count = 6; -} - -//****************************************************************************** -bool CommandMessageEncoder::encode(char * buffer, int & size) -{ - // generate stream for encoding - pb_ostream_t stream = pb_ostream_from_buffer((uint8_t *)buffer, FRI_COMMAND_MSG_MAX_SIZE); - // encode monitoring Message to stream - bool status = pb_encode(&stream, FRICommandMessage_fields, m_pMessage); - size = stream.bytes_written; - if (!status) { - printf("!!encoding error: %s!!\n", PB_GET_ERROR(&stream)); - } - return status; -} +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#include +#include "friCommandMessageEncoder.h" +#include "pb_encode.h" + +using namespace KUKA::FRI; + +//****************************************************************************** +CommandMessageEncoder::CommandMessageEncoder(FRICommandMessage* pMessage, int num) + : m_nNum(num), m_pMessage(pMessage) +{ + initMessage(); +} + +//****************************************************************************** +CommandMessageEncoder::~CommandMessageEncoder() +{ + +} + +//****************************************************************************** +void CommandMessageEncoder::initMessage() +{ + m_pMessage->has_commandData = false; + m_pMessage->has_endOfMessageData = false; + m_pMessage->commandData.has_jointPosition = false; + m_pMessage->commandData.has_cartesianWrenchFeedForward = false; + m_pMessage->commandData.has_jointTorque = false; + m_pMessage->commandData.has_cartesianPose = false; + m_pMessage->commandData.commandedTransformations_count = 0; + m_pMessage->commandData.has_redundancyInformation = false; + m_pMessage->header.messageIdentifier = 0; + // init with 0. Necessary for creating the correct reflected sequence count in the monitoring msg + m_pMessage->header.sequenceCounter = 0; + m_pMessage->header.reflectedSequenceCounter = 0; + + m_pMessage->commandData.writeIORequest_count = 0; + + // allocate and map memory for protobuf repeated structures + map_repeatedDouble(FRI_MANAGER_NANOPB_ENCODE, m_nNum, + &m_pMessage->commandData.jointPosition.value, + &m_tRecvContainer.jointPosition); + map_repeatedDouble(FRI_MANAGER_NANOPB_ENCODE, m_nNum, + &m_pMessage->commandData.jointTorque.value, + &m_tRecvContainer.jointTorque); + + // nanopb encoding needs to know how many elements the static array contains + // a quaternion always contains 7 elements + m_pMessage->commandData.cartesianPose.element_count = 7; + // a Cartesian wrench feed forward vector always contains 6 elements + m_pMessage->commandData.cartesianWrenchFeedForward.element_count = 6; +} + +//****************************************************************************** +bool CommandMessageEncoder::encode(char* buffer, int& size) +{ + // generate stream for encoding + pb_ostream_t stream = pb_ostream_from_buffer((uint8_t*)buffer, FRI_COMMAND_MSG_MAX_SIZE); + // encode monitoring Message to stream + bool status = pb_encode(&stream, FRICommandMessage_fields, m_pMessage); + size = stream.bytes_written; + if (!status) + { + printf("!!encoding error on Command message: %s!!\n", PB_GET_ERROR(&stream)); + } + return status; +} diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.h b/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.h index b32ddb0a..00dd6422 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.h +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.h @@ -1,115 +1,118 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#ifndef _KUKA_FRI_COMMANDMESSAGEENCODER_H -#define _KUKA_FRI_COMMANDMESSAGEENCODER_H - - -#include -#include - - -namespace KUKA -{ -namespace FRI -{ - -static const int FRI_COMMAND_MSG_MAX_SIZE = 1500; //!< max size of a FRI command message - -class CommandMessageEncoder -{ - -public: - CommandMessageEncoder(FRICommandMessage * pMessage, int num); - - ~CommandMessageEncoder(); - - bool encode(char * buffer, int & size); - -private: - struct LocalCommandDataContainer - { - tRepeatedDoubleArguments jointPosition; - tRepeatedDoubleArguments jointTorque; - - LocalCommandDataContainer() - { - init_repeatedDouble(&jointPosition); - init_repeatedDouble(&jointTorque); - } - - ~LocalCommandDataContainer() - { - free_repeatedDouble(&jointPosition); - free_repeatedDouble(&jointTorque); - } - }; - - int m_nNum; - - LocalCommandDataContainer m_tRecvContainer; - FRICommandMessage * m_pMessage; - - void initMessage(); -}; - -} -} - -#endif // _KUKA_FRI_COMMANDMESSAGEENCODER_H +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_COMMANDMESSAGEENCODER_H +#define _KUKA_FRI_COMMANDMESSAGEENCODER_H + + +#include "FRIMessages.pb.h" +#include "pb_frimessages_callbacks.h" + + + +namespace KUKA +{ +namespace FRI +{ + + static const int FRI_COMMAND_MSG_MAX_SIZE = 1500; //!< max size of a FRI command message + + class CommandMessageEncoder + { + + public: + + CommandMessageEncoder(FRICommandMessage* pMessage, int num); + + ~CommandMessageEncoder(); + + bool encode(char* buffer, int& size); + + private: + + struct LocalCommandDataContainer + { + tRepeatedDoubleArguments jointPosition; + tRepeatedDoubleArguments jointTorque; + + LocalCommandDataContainer() + { + init_repeatedDouble(&jointPosition); + init_repeatedDouble(&jointTorque); + } + + ~LocalCommandDataContainer() + { + free_repeatedDouble(&jointPosition); + free_repeatedDouble(&jointTorque); + } + }; + + int m_nNum; + + LocalCommandDataContainer m_tRecvContainer; + FRICommandMessage* m_pMessage; + + void initMessage(); + }; + +} +} + +#endif // _KUKA_FRI_COMMANDMESSAGEENCODER_H diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friDataHelper.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friDataHelper.cpp new file mode 100644 index 00000000..2d71d8b1 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friDataHelper.cpp @@ -0,0 +1,178 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ + +#include + +#include "friDataHelper.h" + +namespace KUKA +{ + namespace FRI + { + void DataHelper::convertTrafoMatrixToQuaternion(const double (&matrixTrafo)[3][4], + double (&quaternionTrafo)[7]) + { + + double s = 1.0 + matrixTrafo[0][0] + matrixTrafo[1][1] + matrixTrafo[2][2]; // = 4 w^2 + const double epsilon = 1e-11; + + quaternionTrafo[QUAT_TX] = matrixTrafo[0][3]; + quaternionTrafo[QUAT_TY] = matrixTrafo[1][3]; + quaternionTrafo[QUAT_TZ] = matrixTrafo[2][3]; + + if (s > epsilon) + { + quaternionTrafo[QUAT_QW] = 0.5 * sqrt(s); // > 0.5 * sqrt(epsilon) = 0.5 * 1e-6 + s = 0.25 / quaternionTrafo[QUAT_QW]; // = 1/(4w) + quaternionTrafo[QUAT_QX] = (matrixTrafo[2][1] - matrixTrafo[1][2]) * s; // 4wx/(4w) + quaternionTrafo[QUAT_QY] = (matrixTrafo[0][2] - matrixTrafo[2][0]) * s; // 4wy/(4w) + quaternionTrafo[QUAT_QZ] = (matrixTrafo[1][0] - matrixTrafo[0][1]) * s; // 4wz/(4w) + } + else + { + // w is very small (or even vanishing) + if ((matrixTrafo[0][0] > matrixTrafo[1][1]) && (matrixTrafo[0][0] > matrixTrafo[2][2])) + { + // => |x| = max{|x|,|y|,|z|} (and |x| > |w|); choose x > 0 + quaternionTrafo[QUAT_QX] = 0.5 * sqrt(1.0 + matrixTrafo[0][0] - matrixTrafo[1][1] - matrixTrafo[2][2]); + s = 0.25 / quaternionTrafo[QUAT_QX]; // 1/(4x) + quaternionTrafo[QUAT_QW] = (matrixTrafo[2][1] - matrixTrafo[1][2]) * s; // 4wx/(4x) + quaternionTrafo[QUAT_QY] = (matrixTrafo[0][1] + matrixTrafo[1][0]) * s; // 4yx/(4x) + quaternionTrafo[QUAT_QZ] = (matrixTrafo[0][2] + matrixTrafo[2][0]) * s; // 4zx/(4x) + } + else if (matrixTrafo[1][1] > matrixTrafo[2][2]) + { + // => |y| = max{|x|,|y|,|z|} (and |y| > |w|); choose y > 0 + quaternionTrafo[QUAT_QY] = 0.5 * sqrt(1.0 + matrixTrafo[1][1] - matrixTrafo[0][0] - matrixTrafo[2][2]); + s = 0.25 / quaternionTrafo[QUAT_QY]; // 1/(4y) + quaternionTrafo[QUAT_QW] = (matrixTrafo[0][2] - matrixTrafo[2][0]) * s; // 4wy/(4y) + quaternionTrafo[QUAT_QX] = (matrixTrafo[0][1] + matrixTrafo[1][0]) * s; // 4xy/(4y) + quaternionTrafo[QUAT_QZ] = (matrixTrafo[1][2] + matrixTrafo[2][1]) * s; // 4zy/(4y) + } + else + { + // => |z| = max{|x|,|y|,|z|} (and |z| > |w|); choose z > 0 + quaternionTrafo[QUAT_QZ] = 0.5 * sqrt(1.0 + matrixTrafo[2][2] - matrixTrafo[0][0] - matrixTrafo[1][1]); + s = 0.25 / quaternionTrafo[QUAT_QZ]; // 1/(4z) + quaternionTrafo[QUAT_QW] = (matrixTrafo[1][0] - matrixTrafo[0][1]) * s; // 4wz/(4z) + quaternionTrafo[QUAT_QX] = (matrixTrafo[0][2] + matrixTrafo[2][0]) * s; // 4xz/(4z) + quaternionTrafo[QUAT_QY] = (matrixTrafo[1][2] + matrixTrafo[2][1]) * s; // 4yz/(4z) + } + } + + // normalize result to ensure that we obtain a unit quaternion + // (should be superfluous but in case of numerical problems ...) + const double rotProduct = + quaternionTrafo[QUAT_QX] * quaternionTrafo[QUAT_QX] + + quaternionTrafo[QUAT_QY] * quaternionTrafo[QUAT_QY] + + quaternionTrafo[QUAT_QZ] * quaternionTrafo[QUAT_QZ]; + + const double norm = sqrt(quaternionTrafo[QUAT_QW] * quaternionTrafo[QUAT_QW] + rotProduct); + + // normalize to unit length, to obtain an orientation representing quaternion + if (norm > epsilon) + { + quaternionTrafo[QUAT_QW] /= norm; + quaternionTrafo[QUAT_QX] /= norm; + quaternionTrafo[QUAT_QY] /= norm; + quaternionTrafo[QUAT_QZ] /= norm; + } + else + { + // input has vanishing length and is thus far away from a reasonable, + // orientation representing quaternion :-( + // generally normalize vanishing quaternions to the identity + quaternionTrafo[QUAT_QW] = 1.0; + quaternionTrafo[QUAT_QX] = 0; + quaternionTrafo[QUAT_QY] = 0; + quaternionTrafo[QUAT_QZ] = 0; + } + + } + + void DataHelper::convertTrafoQuaternionToMatrix(const double(&quaternionTrafo)[7], + double(&matrixTrafo)[3][4]) + { + + const double qW = quaternionTrafo[QUAT_QW]; + const double qX = quaternionTrafo[QUAT_QX]; + const double qY = quaternionTrafo[QUAT_QY]; + const double qZ = quaternionTrafo[QUAT_QZ]; + + // conversion for unit quaternion to transformation matrix + matrixTrafo[0][0] = 1 - 2 * ((qY * qY) + (qZ * qZ)); + matrixTrafo[0][1] = 2 * ((qX * qY) - (qW * qZ)); + matrixTrafo[0][2] = 2 * ((qX * qZ) + (qW * qY)); + matrixTrafo[0][3] = quaternionTrafo[QUAT_TX]; + + matrixTrafo[1][0] = 2 * ((qX * qY) + (qW * qZ)); + matrixTrafo[1][1] = 1 - 2 * ((qX * qX) + (qZ * qZ)); + matrixTrafo[1][2] = 2 * ((qY * qZ) - (qW * qX)); + matrixTrafo[1][3] = quaternionTrafo[QUAT_TY]; + + matrixTrafo[2][0] = 2 * ((qX * qZ) - (qW * qY)); + matrixTrafo[2][1] = 2 * ((qY * qZ) + (qW * qX)); + matrixTrafo[2][2] = 1 - 2 * ((qX * qX) + (qY * qY)); + matrixTrafo[2][3] = quaternionTrafo[QUAT_TZ]; + + } + + } +} diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp index f5178675..ac46d659 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp @@ -1,121 +1,126 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#include -#include -#include -#include - -using namespace KUKA::FRI; -char FRIException::_buffer[1024] = {0}; - -//****************************************************************************** -LBRClient::LBRClient() -{ - -} - -//****************************************************************************** -LBRClient::~LBRClient() -{ - -} - -//****************************************************************************** -void LBRClient::onStateChange(ESessionState oldState, ESessionState newState) -{ - // TODO: String converter function for states - std::cout << "LBRiiwaClient state changed from " << - oldState << " to " << newState << std::endl; -} - -//****************************************************************************** -void LBRClient::monitor() -{ - robotCommand().setJointPosition(robotState().getCommandedJointPosition()); -} - -//****************************************************************************** -void LBRClient::waitForCommand() -{ - robotCommand().setJointPosition(robotState().getIpoJointPosition()); -} - -//****************************************************************************** -void LBRClient::command() -{ - robotCommand().setJointPosition(robotState().getIpoJointPosition()); -} - -//****************************************************************************** -ClientData * LBRClient::createData() -{ - ClientData * data = new ClientData(_robotState.NUMBER_OF_JOINTS); - - // link monitoring and command message to wrappers - _robotState._message = &data->monitoringMsg; - _robotCommand._cmdMessage = &data->commandMsg; - _robotCommand._monMessage = &data->monitoringMsg; - - // set specific message IDs - data->expectedMonitorMsgID = _robotState.LBRMONITORMESSAGEID; - data->commandMsg.header.messageIdentifier = _robotCommand.LBRCOMMANDMESSAGEID; - - return data; -} +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#include +#include "friLBRClient.h" +#include "friClientData.h" + +using namespace KUKA::FRI; +char FRIException::_buffer[1024] = { 0 }; + +//****************************************************************************** +LBRClient::LBRClient() +{ + +} + +//****************************************************************************** +LBRClient::~LBRClient() +{ + +} + +//****************************************************************************** +void LBRClient::onStateChange(ESessionState oldState, ESessionState newState) +{ + // TODO: String converter function for states + printf("LBRiiwaClient state changed from %d to %d\n", oldState, newState); +} + +//****************************************************************************** +void LBRClient::monitor() +{ + // place your code here to monitor the robot's current state +} + +//****************************************************************************** +void LBRClient::waitForCommand() +{ + if (CARTESIAN_POSE == _robotState.getClientCommandMode()) + robotCommand().setCartesianPose(robotState().getIpoCartesianPose()); + else + robotCommand().setJointPosition(robotState().getIpoJointPosition()); +} + +//****************************************************************************** +void LBRClient::command() +{ + if (CARTESIAN_POSE == _robotState.getClientCommandMode()) + robotCommand().setCartesianPose(robotState().getIpoCartesianPose()); + else + robotCommand().setJointPosition(robotState().getIpoJointPosition()); +} + +//****************************************************************************** +ClientData* LBRClient::createData() +{ + ClientData* data = new ClientData(_robotState.NUMBER_OF_JOINTS); + + // link monitoring and command message to wrappers + _robotState._message = &data->monitoringMsg; + _robotCommand._cmdMessage = &data->commandMsg; + _robotCommand._monMessage = &data->monitoringMsg; + + // set specific message IDs + data->expectedMonitorMsgID = _robotState.LBRMONITORMESSAGEID; + data->commandMsg.header.messageIdentifier = _robotCommand.LBRCOMMANDMESSAGEID; + + return data; +} + diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRCommand.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRCommand.cpp index 471973fe..80e47ba6 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRCommand.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRCommand.cpp @@ -1,113 +1,149 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#include -#include -#include -#include - -using namespace KUKA::FRI; - -//****************************************************************************** -void LBRCommand::setJointPosition(const double * values) -{ - _cmdMessage->has_commandData = true; - _cmdMessage->commandData.has_jointPosition = true; - tRepeatedDoubleArguments * dest = - (tRepeatedDoubleArguments *)_cmdMessage->commandData.jointPosition.value.arg; - memcpy(dest->value, values, LBRState::NUMBER_OF_JOINTS * sizeof(double)); -} - -//****************************************************************************** -void LBRCommand::setWrench(const double * wrench) -{ - _cmdMessage->has_commandData = true; - _cmdMessage->commandData.has_cartesianWrenchFeedForward = true; - - double * dest = _cmdMessage->commandData.cartesianWrenchFeedForward.element; - memcpy(dest, wrench, 6 * sizeof(double)); -} -//****************************************************************************** -void LBRCommand::setTorque(const double * torques) -{ - _cmdMessage->has_commandData = true; - _cmdMessage->commandData.has_jointTorque = true; - - tRepeatedDoubleArguments * dest = - (tRepeatedDoubleArguments *)_cmdMessage->commandData.jointTorque.value.arg; - memcpy(dest->value, torques, LBRState::NUMBER_OF_JOINTS * sizeof(double)); -} - -//****************************************************************************** -void LBRCommand::setBooleanIOValue(const char * name, const bool value) -{ - ClientData::setBooleanIOValue(_cmdMessage, name, value, _monMessage); -} - -//****************************************************************************** -void LBRCommand::setAnalogIOValue(const char * name, const double value) -{ - ClientData::setAnalogIOValue(_cmdMessage, name, value, _monMessage); -} - -//****************************************************************************** -void LBRCommand::setDigitalIOValue(const char * name, const unsigned long long value) -{ - ClientData::setDigitalIOValue(_cmdMessage, name, value, _monMessage); -} +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#include "friLBRState.h" +#include "friLBRCommand.h" +#include "friClientData.h" +#include "pb_frimessages_callbacks.h" +#include "friDataHelper.h" + +using namespace KUKA::FRI; + +//****************************************************************************** +void LBRCommand::setJointPosition(const double* values) +{ + _cmdMessage->has_commandData = true; + _cmdMessage->commandData.has_jointPosition = true; + tRepeatedDoubleArguments *dest = + (tRepeatedDoubleArguments*)_cmdMessage->commandData.jointPosition.value.arg; + memcpy(dest->value, values, LBRState::NUMBER_OF_JOINTS * sizeof(double)); +} + +//****************************************************************************** +void LBRCommand::setWrench(const double* wrench) +{ + _cmdMessage->has_commandData = true; + _cmdMessage->commandData.has_cartesianWrenchFeedForward = true; + + double *dest = _cmdMessage->commandData.cartesianWrenchFeedForward.element; + memcpy(dest, wrench, 6 * sizeof(double)); +} + +//****************************************************************************** +void LBRCommand::setTorque(const double* torques) +{ + _cmdMessage->has_commandData = true; + _cmdMessage->commandData.has_jointTorque= true; + + tRepeatedDoubleArguments *dest = (tRepeatedDoubleArguments*)_cmdMessage->commandData.jointTorque.value.arg; + memcpy(dest->value, torques, LBRState::NUMBER_OF_JOINTS * sizeof(double)); +} + +//****************************************************************************** +void LBRCommand::setCartesianPose(const double* cartesianPoseQuaternion, + double const *const redundancyValue) +{ + _cmdMessage->has_commandData = true; + _cmdMessage->commandData.has_cartesianPose = true; + _cmdMessage->commandData.cartesianPose.name[0] = '\0'; + memcpy(_cmdMessage->commandData.cartesianPose.element, cartesianPoseQuaternion, 7 * sizeof(double)); + + _cmdMessage->commandData.has_redundancyInformation = true; + _cmdMessage->commandData.redundancyInformation.strategy = + _monMessage->monitorData.measuredRedundancyInformation.strategy; + + if (NULL != redundancyValue) + { + //set value if provided + + _cmdMessage->commandData.redundancyInformation.value = *redundancyValue; + } + else + { + // use interpolated redundancy value if no value is commanded + _cmdMessage->commandData.redundancyInformation.value = _monMessage->ipoData.redundancyInformation.value; + } +} + +//****************************************************************************** +void LBRCommand::setCartesianPoseAsMatrix(const double(&measuredCartesianPose)[3][4], + double const *const redundancyValue) +{ + double quaternion[7]; + DataHelper::convertTrafoMatrixToQuaternion(measuredCartesianPose, quaternion); + setCartesianPose(quaternion, redundancyValue); +} + +//****************************************************************************** +void LBRCommand::setBooleanIOValue(const char* name, const bool value) +{ + ClientData::setBooleanIOValue(_cmdMessage, name, value, _monMessage); +} + +//****************************************************************************** +void LBRCommand::setAnalogIOValue(const char* name, const double value) +{ + ClientData::setAnalogIOValue(_cmdMessage, name, value, _monMessage); +} + +//****************************************************************************** +void LBRCommand::setDigitalIOValue(const char* name, const unsigned long long value) +{ + ClientData::setDigitalIOValue(_cmdMessage, name, value, _monMessage); +} diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRState.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRState.cpp index 90ebeedc..a5ae3dba 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRState.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRState.cpp @@ -1,232 +1,271 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#include -#include -#include - -using namespace KUKA::FRI; - - -LBRState::LBRState() -: _message(0) -{ - -} -//****************************************************************************** -double LBRState::getSampleTime() const -{ - return _message->connectionInfo.sendPeriod * 0.001; -} - -//****************************************************************************** -ESessionState LBRState::getSessionState() const -{ - return (ESessionState)_message->connectionInfo.sessionState; -} - -//****************************************************************************** -EConnectionQuality LBRState::getConnectionQuality() const -{ - return (EConnectionQuality)_message->connectionInfo.quality; -} - -//****************************************************************************** -ESafetyState LBRState::getSafetyState() const -{ - return (ESafetyState)_message->robotInfo.safetyState; -} - -//****************************************************************************** -EOperationMode LBRState::getOperationMode() const -{ - return (EOperationMode)_message->robotInfo.operationMode; -} - -//****************************************************************************** -EDriveState LBRState::getDriveState() const -{ - tRepeatedIntArguments * values = - (tRepeatedIntArguments *)_message->robotInfo.driveState.arg; - int firstState = (int)values->value[0]; - for (int i = 1; i < NUMBER_OF_JOINTS; i++) { - int state = (int)values->value[i]; - if (state != firstState) { - return TRANSITIONING; - } - } - return (EDriveState)firstState; -} - - -//******************************************************************************** -EOverlayType LBRState::getOverlayType() const -{ - return (EOverlayType)_message->ipoData.overlayType; -} - -//******************************************************************************** -EClientCommandMode LBRState::getClientCommandMode() const -{ - return (EClientCommandMode)_message->ipoData.clientCommandMode; -} - - -//****************************************************************************** -EControlMode LBRState::getControlMode() const -{ - return (EControlMode)_message->robotInfo.controlMode; -} - -//****************************************************************************** -unsigned int LBRState::getTimestampSec() const -{ - return _message->monitorData.timestamp.sec; -} - -//****************************************************************************** -unsigned int LBRState::getTimestampNanoSec() const -{ - return _message->monitorData.timestamp.nanosec; -} - -//****************************************************************************** -const double * LBRState::getMeasuredJointPosition() const -{ - tRepeatedDoubleArguments * values = - (tRepeatedDoubleArguments *)_message->monitorData.measuredJointPosition.value.arg; - return (double *)values->value; -} - -//****************************************************************************** -const double * LBRState::getCommandedJointPosition() const -{ - tRepeatedDoubleArguments * values = - (tRepeatedDoubleArguments *)_message->monitorData.commandedJointPosition.value.arg; - return (double *)values->value; -} - -//****************************************************************************** -const double * LBRState::getMeasuredTorque() const -{ - tRepeatedDoubleArguments * values = - (tRepeatedDoubleArguments *)_message->monitorData.measuredTorque.value.arg; - return (double *)values->value; -} - -//****************************************************************************** -const double * LBRState::getCommandedTorque() const -{ - tRepeatedDoubleArguments * values = - (tRepeatedDoubleArguments *)_message->monitorData.commandedTorque.value.arg; - return (double *)values->value; -} - -//****************************************************************************** -const double * LBRState::getExternalTorque() const -{ - tRepeatedDoubleArguments * values = - (tRepeatedDoubleArguments *)_message->monitorData.externalTorque.value.arg; - return (double *)values->value; -} - -//****************************************************************************** -const double * LBRState::getIpoJointPosition() const -{ - if (!_message->ipoData.has_jointPosition) { - throw FRIException("IPO joint position not available in monitoring mode."); - return NULL; - } - - tRepeatedDoubleArguments * values = - (tRepeatedDoubleArguments *)_message->ipoData.jointPosition.value.arg; - return (double *)values->value; -} - -//****************************************************************************** -double LBRState::getTrackingPerformance() const -{ - if (!_message->ipoData.has_trackingPerformance) {return 0.0;} - - return _message->ipoData.trackingPerformance; -} - -//****************************************************************************** -bool LBRState::getBooleanIOValue(const char * name) const -{ - return ClientData::getBooleanIOValue(_message, name).digitalValue != 0; -} - -//****************************************************************************** -unsigned long long LBRState::getDigitalIOValue(const char * name) const -{ - return ClientData::getDigitalIOValue(_message, name).digitalValue; -} - -//****************************************************************************** -double LBRState::getAnalogIOValue(const char * name) const -{ - return ClientData::getAnalogIOValue(_message, name).analogValue; -} - -//****************************************************************************** -/*const std::vector& LBRState::getRequestedIO_IDs() const -{ - return _clientData->getRequestedIO_IDs(); -}*/ +/** + + The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + + \file + \version {2.5} + */ +#include "friLBRState.h" +#include "friClientData.h" +#include "friDataHelper.h" +#include "pb_frimessages_callbacks.h" + +using namespace KUKA::FRI; + +LBRState::LBRState() : + _message(0) +{ + +} +//****************************************************************************** +double LBRState::getSampleTime() const +{ + return _message->connectionInfo.sendPeriod * 0.001; +} + +//****************************************************************************** +ESessionState LBRState::getSessionState() const +{ + return (ESessionState) _message->connectionInfo.sessionState; +} + +//****************************************************************************** +EConnectionQuality LBRState::getConnectionQuality() const +{ + return (EConnectionQuality) _message->connectionInfo.quality; +} + +//****************************************************************************** +ESafetyState LBRState::getSafetyState() const +{ + return (ESafetyState) _message->robotInfo.safetyState; +} + +//****************************************************************************** +EOperationMode LBRState::getOperationMode() const +{ + return (EOperationMode) _message->robotInfo.operationMode; +} + +//****************************************************************************** +EDriveState LBRState::getDriveState() const +{ + tRepeatedIntArguments *values = (tRepeatedIntArguments *) _message->robotInfo.driveState.arg; + int firstState = (int) values->value[0]; + for (int i = 1; i < NUMBER_OF_JOINTS; i++) + { + int state = (int) values->value[i]; + if (state != firstState) + { + return TRANSITIONING; + } + } + return (EDriveState) firstState; +} + +//******************************************************************************** +EOverlayType LBRState::getOverlayType() const +{ + return (EOverlayType) _message->ipoData.overlayType; +} + +//******************************************************************************** +EClientCommandMode LBRState::getClientCommandMode() const +{ + return (EClientCommandMode) _message->ipoData.clientCommandMode; +} + +//****************************************************************************** +EControlMode LBRState::getControlMode() const +{ + return (EControlMode) _message->robotInfo.controlMode; +} + +//****************************************************************************** +unsigned int LBRState::getTimestampSec() const +{ + return _message->monitorData.timestamp.sec; +} + +//****************************************************************************** +unsigned int LBRState::getTimestampNanoSec() const +{ + return _message->monitorData.timestamp.nanosec; +} + +//****************************************************************************** +const double* LBRState::getMeasuredJointPosition() const +{ + tRepeatedDoubleArguments *values = (tRepeatedDoubleArguments*) _message->monitorData.measuredJointPosition.value.arg; + return (double*) values->value; +} + +//****************************************************************************** +const double* LBRState::getMeasuredTorque() const +{ + tRepeatedDoubleArguments *values = (tRepeatedDoubleArguments*) _message->monitorData.measuredTorque.value.arg; + return (double*) values->value; +} + +//****************************************************************************** +const double* LBRState::getCommandedTorque() const +{ + tRepeatedDoubleArguments *values = (tRepeatedDoubleArguments*) _message->monitorData.commandedTorque.value.arg; + return (double*) values->value; +} + +//****************************************************************************** +const double* LBRState::getExternalTorque() const +{ + tRepeatedDoubleArguments *values = (tRepeatedDoubleArguments*) _message->monitorData.externalTorque.value.arg; + return (double*) values->value; +} + +//****************************************************************************** +const double* LBRState::getIpoJointPosition() const +{ + if (!_message->ipoData.has_jointPosition) + { + throw FRIException("IPO joint position not available when FRI Joint Overlay is not active."); + return NULL; + } + + tRepeatedDoubleArguments *values = (tRepeatedDoubleArguments*) _message->ipoData.jointPosition.value.arg; + return (double*) values->value; +} + +//****************************************************************************** +double LBRState::getTrackingPerformance() const +{ + if (!_message->ipoData.has_trackingPerformance) + return 0.0; + + return _message->ipoData.trackingPerformance; +} + +//****************************************************************************** +bool LBRState::getBooleanIOValue(const char* name) const +{ + return ClientData::getBooleanIOValue(_message, name).digitalValue != 0; +} + +//****************************************************************************** +unsigned long long LBRState::getDigitalIOValue(const char* name) const +{ + return ClientData::getDigitalIOValue(_message, name).digitalValue; +} + +//****************************************************************************** +double LBRState::getAnalogIOValue(const char* name) const +{ + return ClientData::getAnalogIOValue(_message, name).analogValue; +} + +//****************************************************************************** +const double* LBRState::getMeasuredCartesianPose() const +{ + return _message->monitorData.measuredCartesianPose.element; +} + +//****************************************************************************** +void LBRState::getMeasuredCartesianPoseAsMatrix(double(&measuredCartesianPose)[3][4]) const +{ + DataHelper::convertTrafoQuaternionToMatrix(_message->monitorData.measuredCartesianPose.element, + measuredCartesianPose); +} + +//****************************************************************************** +const double* LBRState::getIpoCartesianPose() const +{ + if (!_message->ipoData.has_cartesianPose) + { + throw FRIException("IPO Cartesian Pose not available when FRI Cartesian Overlay is not active."); + return NULL; + } + + return _message->ipoData.cartesianPose.element; +} + +//****************************************************************************** +void LBRState::getIpoCartesianPoseAsMatrix(double(&ipoCartesianPose)[3][4]) const +{ + if (!_message->ipoData.has_cartesianPose) + { + throw FRIException("IPO Cartesian Pose not available when FRI Cartesian Overlay is not active."); + } + DataHelper::convertTrafoQuaternionToMatrix(_message->ipoData.cartesianPose.element, ipoCartesianPose); +} + +//****************************************************************************** +double LBRState::getMeasuredRedundancyValue() const +{ + return _message->monitorData.measuredRedundancyInformation.value; +} + +//****************************************************************************** +double LBRState::getIpoRedundancyValue() const +{ + if (!_message->ipoData.has_redundancyInformation) + { + throw FRIException("IPO redundancy value not available when FRI Cartesian Overlay is not active."); + } + return _message->ipoData.redundancyInformation.value; +} + +//****************************************************************************** +ERedundancyStrategy LBRState::getRedundancyStrategy() const +{ + return (ERedundancyStrategy)_message->monitorData.measuredRedundancyInformation.strategy; +} diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friMonitoringMessageDecoder.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friMonitoringMessageDecoder.cpp index f39c34f2..c6426db9 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friMonitoringMessageDecoder.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friMonitoringMessageDecoder.cpp @@ -1,151 +1,142 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#include -#include -#include - - -using namespace KUKA::FRI; - -//****************************************************************************** -MonitoringMessageDecoder::MonitoringMessageDecoder(FRIMonitoringMessage * pMessage, int num) -: m_nNum(num), m_pMessage(pMessage) -{ - initMessage(); -} - -//****************************************************************************** -MonitoringMessageDecoder::~MonitoringMessageDecoder() -{ - -} - -//****************************************************************************** -void MonitoringMessageDecoder::initMessage() -{ - // set initial data - // it is assumed that no robot information and monitoring data is available and therefore the - // optional fields are initialized with false - m_pMessage->has_robotInfo = false; - m_pMessage->has_monitorData = false; - m_pMessage->has_connectionInfo = true; - m_pMessage->has_ipoData = false; - m_pMessage->requestedTransformations_count = 0; - m_pMessage->has_endOfMessageData = false; - - - m_pMessage->header.messageIdentifier = 0; - m_pMessage->header.reflectedSequenceCounter = 0; - m_pMessage->header.sequenceCounter = 0; - - m_pMessage->connectionInfo.sessionState = FRISessionState_IDLE; - m_pMessage->connectionInfo.quality = FRIConnectionQuality_POOR; - - m_pMessage->monitorData.readIORequest_count = 0; - - // allocate and map memory for protobuf repeated structures - map_repeatedDouble( - FRI_MANAGER_NANOPB_DECODE, m_nNum, - &m_pMessage->monitorData.measuredJointPosition.value, - &m_tSendContainer.m_AxQMsrLocal); - - map_repeatedDouble( - FRI_MANAGER_NANOPB_DECODE, m_nNum, - &m_pMessage->monitorData.measuredTorque.value, - &m_tSendContainer.m_AxTauMsrLocal); - - map_repeatedDouble( - FRI_MANAGER_NANOPB_DECODE, m_nNum, - &m_pMessage->monitorData.commandedJointPosition.value, - &m_tSendContainer.m_AxQCmdT1mLocal); - - map_repeatedDouble( - FRI_MANAGER_NANOPB_DECODE, m_nNum, - &m_pMessage->monitorData.commandedTorque.value, - &m_tSendContainer.m_AxTauCmdLocal); - - map_repeatedDouble( - FRI_MANAGER_NANOPB_DECODE, m_nNum, - &m_pMessage->monitorData.externalTorque.value, - &m_tSendContainer.m_AxTauExtMsrLocal); - - map_repeatedDouble( - FRI_MANAGER_NANOPB_DECODE, m_nNum, - &m_pMessage->ipoData.jointPosition.value, - &m_tSendContainer.m_AxQCmdIPO); - - map_repeatedInt( - FRI_MANAGER_NANOPB_DECODE, m_nNum, - &m_pMessage->robotInfo.driveState, - &m_tSendContainer.m_AxDriveStateLocal); -} - -//****************************************************************************** -bool MonitoringMessageDecoder::decode(char * buffer, int size) -{ - pb_istream_t stream = pb_istream_from_buffer((uint8_t *)buffer, size); - - bool status = pb_decode(&stream, FRIMonitoringMessage_fields, m_pMessage); - if (!status) { - printf("!!decoding error: %s!!\n", PB_GET_ERROR(&stream)); - } - - return status; -} +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#include +#include "friMonitoringMessageDecoder.h" +#include "pb_decode.h" + + +using namespace KUKA::FRI; + +//****************************************************************************** +MonitoringMessageDecoder::MonitoringMessageDecoder(FRIMonitoringMessage* pMessage, int num) + : m_nNum(num), m_pMessage(pMessage) +{ + initMessage(); +} + +//****************************************************************************** +MonitoringMessageDecoder::~MonitoringMessageDecoder() +{ + +} + +//****************************************************************************** +void MonitoringMessageDecoder::initMessage() +{ + // set initial data + // it is assumed that no robot information and monitoring data is available and therefore the + // optional fields are initialized with false + m_pMessage->has_robotInfo = false; + m_pMessage->has_monitorData = false; + m_pMessage->has_connectionInfo = true; + m_pMessage->has_ipoData = false; + m_pMessage->requestedTransformations_count = 0; + m_pMessage->has_endOfMessageData = false; + + + m_pMessage->header.messageIdentifier = 0; + m_pMessage->header.reflectedSequenceCounter = 0; + m_pMessage->header.sequenceCounter = 0; + + m_pMessage->connectionInfo.sessionState = FRISessionState_IDLE; + m_pMessage->connectionInfo.quality = FRIConnectionQuality_POOR; + + m_pMessage->monitorData.readIORequest_count = 0; + m_pMessage->monitorData.measuredCartesianPose.element_count = 0; + + // allocate and map memory for protobuf repeated structures + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.measuredJointPosition.value, + &m_tSendContainer.m_AxQMsrLocal); + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.measuredTorque.value, + &m_tSendContainer.m_AxTauMsrLocal); + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.commandedTorque.value, + &m_tSendContainer.m_AxTauCmdLocal); + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.externalTorque.value, + &m_tSendContainer.m_AxTauExtMsrLocal); + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE,m_nNum, + &m_pMessage->ipoData.jointPosition.value, + &m_tSendContainer.m_AxQCmdIPO); + + map_repeatedInt(FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->robotInfo.driveState, + &m_tSendContainer.m_AxDriveStateLocal); +} + +//****************************************************************************** +bool MonitoringMessageDecoder::decode(char* buffer, int size) +{ + pb_istream_t stream = pb_istream_from_buffer((uint8_t*)buffer, size); + + bool status = pb_decode(&stream, FRIMonitoringMessage_fields, m_pMessage); + if (!status) + { + printf("!!decoding error on Monitor message: %s!!\n", PB_GET_ERROR(&stream)); + } + + return status; +} diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friMonitoringMessageDecoder.h b/kuka_sunrise_fri_driver/src/fri_client_sdk/friMonitoringMessageDecoder.h index b0be5a59..795c5175 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friMonitoringMessageDecoder.h +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friMonitoringMessageDecoder.h @@ -1,130 +1,133 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#ifndef _KUKA_FRI_MONITORINGMESSAGEDECODER_H -#define _KUKA_FRI_MONITORINGMESSAGEDECODER_H - -#include -#include - - -namespace KUKA -{ -namespace FRI -{ - -static const int FRI_MONITOR_MSG_MAX_SIZE = 1500; //!< max size of a FRI monitoring message - - -class MonitoringMessageDecoder -{ - -public: - MonitoringMessageDecoder(FRIMonitoringMessage * pMessage, int num); - - ~MonitoringMessageDecoder(); - - bool decode(char * buffer, int size); - -private: - struct LocalMonitoringDataContainer - { - tRepeatedDoubleArguments m_AxQMsrLocal; - tRepeatedDoubleArguments m_AxTauMsrLocal; - tRepeatedDoubleArguments m_AxQCmdT1mLocal; - tRepeatedDoubleArguments m_AxTauCmdLocal; - tRepeatedDoubleArguments m_AxTauExtMsrLocal; - tRepeatedIntArguments m_AxDriveStateLocal; - tRepeatedDoubleArguments m_AxQCmdIPO; - - LocalMonitoringDataContainer() - { - init_repeatedDouble(&m_AxQMsrLocal); - init_repeatedDouble(&m_AxTauMsrLocal); - init_repeatedDouble(&m_AxQCmdT1mLocal); - init_repeatedDouble(&m_AxTauCmdLocal); - init_repeatedDouble(&m_AxTauExtMsrLocal); - init_repeatedDouble(&m_AxQCmdIPO); - init_repeatedInt(&m_AxDriveStateLocal); - } - - ~LocalMonitoringDataContainer() - { - free_repeatedDouble(&m_AxQMsrLocal); - free_repeatedDouble(&m_AxTauMsrLocal); - free_repeatedDouble(&m_AxQCmdT1mLocal); - free_repeatedDouble(&m_AxTauCmdLocal); - free_repeatedDouble(&m_AxTauExtMsrLocal); - free_repeatedDouble(&m_AxQCmdIPO); - free_repeatedInt(&m_AxDriveStateLocal); - } - }; - - int m_nNum; - - LocalMonitoringDataContainer m_tSendContainer; - FRIMonitoringMessage * m_pMessage; - - void initMessage(); -}; - -} -} - -#endif // _KUKA_FRI_MONITORINGMESSAGEDECODER_H +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_MONITORINGMESSAGEDECODER_H +#define _KUKA_FRI_MONITORINGMESSAGEDECODER_H + +#include "FRIMessages.pb.h" +#include "pb_frimessages_callbacks.h" + + +namespace KUKA +{ +namespace FRI +{ + + static const int FRI_MONITOR_MSG_MAX_SIZE = 1500; //!< max size of a FRI monitoring message + + + class MonitoringMessageDecoder + { + + public: + + MonitoringMessageDecoder(FRIMonitoringMessage* pMessage, int num); + + ~MonitoringMessageDecoder(); + + bool decode(char* buffer, int size); + + + private: + + struct LocalMonitoringDataContainer + { + tRepeatedDoubleArguments m_AxQMsrLocal; + tRepeatedDoubleArguments m_AxTauMsrLocal; + tRepeatedDoubleArguments m_AxQCmdT1mLocal; + tRepeatedDoubleArguments m_AxTauCmdLocal; + tRepeatedDoubleArguments m_AxTauExtMsrLocal; + tRepeatedIntArguments m_AxDriveStateLocal; + tRepeatedDoubleArguments m_AxQCmdIPO; + + LocalMonitoringDataContainer() + { + init_repeatedDouble(&m_AxQMsrLocal); + init_repeatedDouble(&m_AxTauMsrLocal); + init_repeatedDouble(&m_AxQCmdT1mLocal); + init_repeatedDouble(&m_AxTauCmdLocal); + init_repeatedDouble(&m_AxTauExtMsrLocal); + init_repeatedDouble(&m_AxQCmdIPO); + init_repeatedInt(&m_AxDriveStateLocal); + } + + ~LocalMonitoringDataContainer() + { + free_repeatedDouble(&m_AxQMsrLocal); + free_repeatedDouble(&m_AxTauMsrLocal); + free_repeatedDouble(&m_AxQCmdT1mLocal); + free_repeatedDouble(&m_AxTauCmdLocal); + free_repeatedDouble(&m_AxTauExtMsrLocal); + free_repeatedDouble(&m_AxQCmdIPO); + free_repeatedInt(&m_AxDriveStateLocal); + } + }; + + int m_nNum; + + LocalMonitoringDataContainer m_tSendContainer; + FRIMonitoringMessage* m_pMessage; + + void initMessage(); + }; + +} +} + +#endif // _KUKA_FRI_MONITORINGMESSAGEDECODER_H diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friTransformationClient.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friTransformationClient.cpp index 70fc14f6..08b4e031 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friTransformationClient.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friTransformationClient.cpp @@ -1,188 +1,205 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ - -#include -#include - -#include -#include - -#include -#include - -using namespace KUKA::FRI; - -//****************************************************************************** -TransformationClient::TransformationClient() -{ -} - -//****************************************************************************** -TransformationClient::~TransformationClient() -{ -} - -//****************************************************************************** -const std::vector & TransformationClient::getRequestedTransformationIDs() const -{ - unsigned int trafoCount = _data->monitoringMsg.requestedTransformations_count; - _data->requestedTrafoIDs.resize(trafoCount); - for (unsigned int i = 0; i < trafoCount; i++) { - _data->requestedTrafoIDs[i] = _data->monitoringMsg.requestedTransformations[i].name; - } - return _data->requestedTrafoIDs; -} - -//****************************************************************************** -unsigned int TransformationClient::getTimestampSec() const -{ - return _data->monitoringMsg.monitorData.timestamp.sec; -} - -//****************************************************************************** -unsigned int TransformationClient::getTimestampNanoSec() const -{ - return _data->monitoringMsg.monitorData.timestamp.nanosec; -} - -//****************************************************************************** -void TransformationClient::setTransformation( - const char * transformationID, - const double transformationMatrix[3][4], unsigned int timeSec, unsigned int timeNanoSec) -{ - _data->commandMsg.has_commandData = true; - - unsigned int currentSize = _data->commandMsg.commandData.commandedTransformations_count; - - if (currentSize < _data->MAX_REQUESTED_TRANSFORMATIONS) { - _data->commandMsg.commandData.commandedTransformations_count++; - Transformation & dest = _data->commandMsg.commandData.commandedTransformations[currentSize]; - strncpy(dest.name, transformationID, _data->MAX_SIZE_TRANSFORMATION_ID); - dest.name[_data->MAX_SIZE_TRANSFORMATION_ID - 1] = '\0'; - dest.matrix_count = 12; - memcpy(dest.matrix, transformationMatrix, 12 * sizeof(double)); - dest.has_timestamp = true; - dest.timestamp.sec = timeSec; - dest.timestamp.nanosec = timeNanoSec; - } else { - throw FRIException("Exceeded maximum number of transformations."); - } -} - -//****************************************************************************** -void TransformationClient::linkData(ClientData * clientData) -{ - _data = clientData; -} - -//****************************************************************************** -double TransformationClient::getSampleTime() const -{ - return _data->monitoringMsg.connectionInfo.sendPeriod * 0.001; -} - -//****************************************************************************** -EConnectionQuality TransformationClient::getConnectionQuality() const -{ - return (EConnectionQuality)_data->monitoringMsg.connectionInfo.quality; -} - - -//****************************************************************************** -void TransformationClient::setBooleanIOValue(const char * name, const bool value) -{ - ClientData::setBooleanIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg); -} - -//****************************************************************************** -void TransformationClient::setAnalogIOValue(const char * name, const double value) -{ - ClientData::setAnalogIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg); -} - -//****************************************************************************** -void TransformationClient::setDigitalIOValue(const char * name, const unsigned long long value) -{ - ClientData::setDigitalIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg); -} - -//****************************************************************************** -bool TransformationClient::getBooleanIOValue(const char * name) const -{ - return ClientData::getBooleanIOValue(&_data->monitoringMsg, name).digitalValue != 0; -} - -//****************************************************************************** -unsigned long long TransformationClient::getDigitalIOValue(const char * name) const -{ - return ClientData::getDigitalIOValue(&_data->monitoringMsg, name).digitalValue; -} - -//****************************************************************************** -double TransformationClient::getAnalogIOValue(const char * name) const -{ - return ClientData::getAnalogIOValue(&_data->monitoringMsg, name).analogValue; -} - -//****************************************************************************** -/*const std::vector& TransformationClient::getRequestedIO_IDs() const -{ - return _data->getRequestedIO_IDs(); -}*/ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ + +#include +#include +#include + +#include "friTransformationClient.h" +#include "friClientData.h" +#include "friDataHelper.h" +#include "FRIMessages.pb.h" +#include "pb_frimessages_callbacks.h" + +using namespace KUKA::FRI; + +//****************************************************************************** +TransformationClient::TransformationClient() +{ +} + +//****************************************************************************** +TransformationClient::~TransformationClient() +{ +} + +//****************************************************************************** +const std::vector& TransformationClient::getRequestedTransformationIDs() const +{ + unsigned int trafoCount = _data->monitoringMsg.requestedTransformations_count; + _data->requestedTrafoIDs.resize(trafoCount); + for (unsigned int i=0; irequestedTrafoIDs[i] = _data->monitoringMsg.requestedTransformations[i].name; + } + return _data->requestedTrafoIDs; +} + +//****************************************************************************** +const unsigned int TransformationClient::getTimestampSec() const +{ + return _data->monitoringMsg.monitorData.timestamp.sec; +} + +//****************************************************************************** +const unsigned int TransformationClient::getTimestampNanoSec() const +{ + return _data->monitoringMsg.monitorData.timestamp.nanosec; +} + +//****************************************************************************** +void TransformationClient::setTransformation(const char* transformationID, + const double (&transformationQuaternion)[7], + const unsigned int timeSec, const unsigned int timeNanoSec) +{ + _data->commandMsg.has_commandData = true; + + const unsigned int currentSize = _data->commandMsg.commandData.commandedTransformations_count; + + if (currentSize < _data->MAX_REQUESTED_TRANSFORMATIONS) + { + _data->commandMsg.commandData.commandedTransformations_count++; + QuaternionTransformation& dest = _data->commandMsg.commandData.commandedTransformations[currentSize]; + dest.element_count = 7; + strncpy(dest.name, transformationID, _data->MAX_SIZE_TRANSFORMATION_ID); + dest.name[_data->MAX_SIZE_TRANSFORMATION_ID - 1] = '\0'; + memcpy(dest.element, transformationQuaternion, 7 * sizeof(double)); + dest.has_timestamp = true; + dest.timestamp.sec = timeSec; + dest.timestamp.nanosec = timeNanoSec; + } + else + { + throw FRIException("Exceeded maximum number of transformations."); + } +} + +//****************************************************************************** +void TransformationClient::setTransformation(const char* transformationID, + const double (&transformationMatrix)[3][4], + const unsigned int timeSec, const unsigned int timeNanoSec) +{ + + double tmpQauternionTrafo[7]; + DataHelper::convertTrafoMatrixToQuaternion(transformationMatrix, tmpQauternionTrafo); + + setTransformation(transformationID, tmpQauternionTrafo, timeSec, timeNanoSec); +} + +//****************************************************************************** +void TransformationClient::linkData(ClientData* clientData) +{ + _data = clientData; +} + +//****************************************************************************** +double TransformationClient::getSampleTime() const +{ + return _data->monitoringMsg.connectionInfo.sendPeriod * 0.001; +} + +//****************************************************************************** +EConnectionQuality TransformationClient::getConnectionQuality() const +{ + return (EConnectionQuality)_data->monitoringMsg.connectionInfo.quality; +} + + +//****************************************************************************** +void TransformationClient::setBooleanIOValue(const char* name, const bool value) +{ + ClientData::setBooleanIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg); +} + +//****************************************************************************** +void TransformationClient::setAnalogIOValue(const char* name, const double value) +{ + ClientData::setAnalogIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg); +} + +//****************************************************************************** +void TransformationClient::setDigitalIOValue(const char* name, const unsigned long long value) +{ + ClientData::setDigitalIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg); +} + +//****************************************************************************** +bool TransformationClient::getBooleanIOValue(const char* name) const +{ + return ClientData::getBooleanIOValue(&_data->monitoringMsg, name).digitalValue != 0; +} + +//****************************************************************************** +unsigned long long TransformationClient::getDigitalIOValue(const char* name) const +{ + return ClientData::getDigitalIOValue(&_data->monitoringMsg, name).digitalValue; +} + +//****************************************************************************** +double TransformationClient::getAnalogIOValue(const char* name) const +{ + return ClientData::getAnalogIOValue(&_data->monitoringMsg, name).analogValue; +} + +//****************************************************************************** +/*const std::vector& TransformationClient::getRequestedIO_IDs() const +{ + return _data->getRequestedIO_IDs(); +}*/ diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friUdpConnection.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friUdpConnection.cpp index e222e273..bd41c884 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friUdpConnection.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friUdpConnection.cpp @@ -1,234 +1,243 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#include -#include -#ifndef _MSC_VER -#include -#endif - -#include - - -#ifdef WIN32 -#include -#include -#ifdef _MSC_VER -#pragma comment(lib, "ws2_32.lib") -#endif -#endif - -using namespace KUKA::FRI; - -//****************************************************************************** -UdpConnection::UdpConnection(unsigned int receiveTimeout) -: _udpSock(-1), - _receiveTimeout(receiveTimeout) -{ -#ifdef WIN32 - WSADATA WSAData; - WSAStartup(MAKEWORD(2, 0), &WSAData); -#endif -} - -//****************************************************************************** -UdpConnection::~UdpConnection() -{ - close(); -#ifdef WIN32 - WSACleanup(); -#endif -} - -//****************************************************************************** -bool UdpConnection::open(int port, const char * controllerAddress) -{ - struct sockaddr_in servAddr; - memset(&servAddr, 0, sizeof(servAddr)); - memset(&_controllerAddr, 0, sizeof(_controllerAddr)); - - // socket creation - _udpSock = socket(PF_INET, SOCK_DGRAM, 0); - if (_udpSock < 0) { - printf("opening socket failed!\n"); - return false; - } - - // use local server port - servAddr.sin_family = AF_INET; - servAddr.sin_port = htons(port); - servAddr.sin_addr.s_addr = htonl(INADDR_ANY); - - if (bind(_udpSock, (struct sockaddr *)&servAddr, sizeof(servAddr)) < 0) { - printf("binding port number %d failed!\n", port); - close(); - return false; - } - // initialize the socket properly - _controllerAddr.sin_family = AF_INET; - _controllerAddr.sin_port = htons(port); - if (controllerAddress) { -#ifndef __MINGW32__ - inet_pton(AF_INET, controllerAddress, &_controllerAddr.sin_addr); -#else - _controllerAddr.sin_addr.s_addr = inet_addr(controllerAddress); -#endif - if (connect(_udpSock, (struct sockaddr *)&_controllerAddr, sizeof(_controllerAddr)) < 0) { - printf("connecting to controller with address %s failed !\n", controllerAddress); - close(); - return false; - } - } else { - _controllerAddr.sin_addr.s_addr = htonl(INADDR_ANY); - } - return true; -} - -//****************************************************************************** -void UdpConnection::close() -{ - if (isOpen()) { -#ifdef WIN32 - closesocket(_udpSock); -#else - ::close(_udpSock); -#endif - } - _udpSock = -1; -} - -//****************************************************************************** -bool UdpConnection::isOpen() const -{ - return _udpSock >= 0; -} - -//****************************************************************************** -int UdpConnection::receive(char * buffer, int maxSize) -{ - if (isOpen()) { - /** HAVE_SOCKLEN_T - Yes - unbelievable: There are differences in standard calling parameters (types) to recvfrom - Windows winsock, VxWorks and QNX use int - newer Posix (most Linuxes) use socklen_t - */ -#ifdef HAVE_SOCKLEN_T - socklen_t sockAddrSize; -#else - unsigned int sockAddrSize; -#endif - sockAddrSize = sizeof(struct sockaddr_in); - /** check for timeout - Because SO_RCVTIMEO is in Windows not correctly implemented, select is used for the receive time out. - If a timeout greater than 0 is given, wait until the timeout is reached or a message was received. - If t, abort the function with an error. - */ - if (_receiveTimeout > 0) { - - // Set up struct timeval - struct timeval tv; - tv.tv_sec = _receiveTimeout / 1000; - tv.tv_usec = (_receiveTimeout % 1000) * 1000; - - // initialize file descriptor - /** - * Replace FD_ZERO with memset, because bzero is not available for VxWorks - * User Space Applications(RTPs). Therefore the macro FD_ZERO does not compile. - */ -#ifndef VXWORKS - FD_ZERO(&_filedescriptor); -#else - memset((char *)(&_filedescriptor), 0, sizeof(*(&_filedescriptor))); -#endif - FD_SET(_udpSock, &_filedescriptor); - - // wait until something was received - int numberActiveFileDescr = select(_udpSock + 1, &_filedescriptor, NULL, NULL, &tv); - // 0 indicates a timeout - if (numberActiveFileDescr == 0) { - printf("The connection has timed out. Timeout is %d\n", _receiveTimeout); - return -1; - } - // a negative value indicates an error - else if (numberActiveFileDescr == -1) { - printf("An error has occurred \n"); - return -1; - } - } - - return recvfrom( - _udpSock, buffer, maxSize, 0, (struct sockaddr *)&_controllerAddr, - &sockAddrSize); - } - return -1; -} - -//****************************************************************************** -bool UdpConnection::send(const char * buffer, int size) -{ - if ((isOpen()) && (ntohs(_controllerAddr.sin_port) != 0)) { - int sent = sendto( - _udpSock, const_cast(buffer), size, 0, - (struct sockaddr *)&_controllerAddr, sizeof(_controllerAddr)); - if (sent == size) { - return true; - } - } - return false; -} +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#include +#include +#ifndef _MSC_VER +#include +#endif + +#include "friUdpConnection.h" + + +#ifdef WIN32 +#include +#include +#ifdef _MSC_VER +#pragma comment(lib, "ws2_32.lib") +#endif +#endif + +using namespace KUKA::FRI; + +//****************************************************************************** +UdpConnection::UdpConnection(unsigned int receiveTimeout) : + _udpSock(-1), + _receiveTimeout(receiveTimeout) +{ +#ifdef WIN32 + WSADATA WSAData; + WSAStartup(MAKEWORD(2,0), &WSAData); +#endif +} + +//****************************************************************************** +UdpConnection::~UdpConnection() +{ + close(); +#ifdef WIN32 + WSACleanup(); +#endif +} + +//****************************************************************************** +bool UdpConnection::open(int port, const char *controllerAddress) +{ + struct sockaddr_in servAddr; + memset(&servAddr, 0, sizeof(servAddr)); + memset(&_controllerAddr, 0, sizeof(_controllerAddr)); + + // socket creation + _udpSock = socket(PF_INET, SOCK_DGRAM, 0); + if (_udpSock < 0) + { + printf("opening socket failed!\n"); + return false; + } + + // use local server port + servAddr.sin_family = AF_INET; + servAddr.sin_port = htons(port); + servAddr.sin_addr.s_addr = htonl(INADDR_ANY); + + if (bind(_udpSock, (struct sockaddr *)&servAddr, sizeof(servAddr)) < 0) + { + printf("binding port number %d failed!\n", port); + close(); + return false; + } + // initialize the socket properly + _controllerAddr.sin_family = AF_INET; + _controllerAddr.sin_port = htons(port); + if (controllerAddress) + { +#ifndef __MINGW32__ + inet_pton(AF_INET, controllerAddress, &_controllerAddr.sin_addr); +#else + _controllerAddr.sin_addr.s_addr = inet_addr(controllerAddress); +#endif + if ( connect(_udpSock, (struct sockaddr *)&_controllerAddr, sizeof(_controllerAddr)) < 0) + { + printf("connecting to controller with address %s failed !\n", controllerAddress); + close(); + return false; + } + } + else + { + _controllerAddr.sin_addr.s_addr = htonl(INADDR_ANY); + } + return true; +} + +//****************************************************************************** +void UdpConnection::close() +{ + if (isOpen()) + { +#ifdef WIN32 + closesocket(_udpSock); +#else + ::close(_udpSock); +#endif + } + _udpSock = -1; +} + +//****************************************************************************** +bool UdpConnection::isOpen() const +{ + return (_udpSock >= 0); +} + +//****************************************************************************** +int UdpConnection::receive(char *buffer, int maxSize) +{ + if (isOpen()) + { + /** HAVE_SOCKLEN_T + Yes - unbelievable: There are differences in standard calling parameters (types) to recvfrom + Windows winsock, VxWorks and QNX use int + newer Posix (most Linuxes) use socklen_t + */ +#ifdef HAVE_SOCKLEN_T + socklen_t sockAddrSize; +#else + int sockAddrSize; +#endif + sockAddrSize = sizeof(struct sockaddr_in); + /** check for timeout + Because SO_RCVTIMEO is in Windows not correctly implemented, select is used for the receive time out. + If a timeout greater than 0 is given, wait until the timeout is reached or a message was received. + If t, abort the function with an error. + */ + if(_receiveTimeout > 0) + { + + // Set up struct timeval + struct timeval tv; + tv.tv_sec = _receiveTimeout / 1000; + tv.tv_usec = (_receiveTimeout % 1000) * 1000; + + // initialize file descriptor + /** + * Replace FD_ZERO with memset, because bzero is not available for VxWorks + * User Space Aplications(RTPs). Therefore the macro FD_ZERO does not compile. + */ +#ifndef VXWORKS + FD_ZERO(&_filedescriptor); +#else + memset((char *)(&_filedescriptor), 0, sizeof(*(&_filedescriptor))); +#endif + FD_SET(_udpSock, &_filedescriptor); + + // wait until something was received + int numberActiveFileDescr = select(_udpSock+1, &_filedescriptor,NULL,NULL,&tv); + // 0 indicates a timeout + if(numberActiveFileDescr == 0) + { + printf("The connection has timed out. Timeout is %d\n", _receiveTimeout); + return -1; + } + // a negative value indicates an error + else if(numberActiveFileDescr == -1) + { + printf("An error has occured \n"); + return -1; + } + } + + return recvfrom(_udpSock, buffer, maxSize, 0, (struct sockaddr *)&_controllerAddr, &sockAddrSize); + } + return -1; +} + +//****************************************************************************** +bool UdpConnection::send(const char* buffer, int size) +{ + if ((isOpen()) && (ntohs(_controllerAddr.sin_port) != 0)) + { + int sent = sendto(_udpSock, const_cast(buffer), size, 0, (struct sockaddr *)&_controllerAddr, sizeof(_controllerAddr)); + if (sent == size) + { + return true; + } + } + return false; +} diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.c b/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.c index 9588fd97..998583c2 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.c +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.c @@ -1,269 +1,267 @@ -/** - - The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - - - \file - \version {1.15} - */ -#include -#include - -#include -#include -#include - -bool encode_repeatedDouble(pb_ostream_t *stream, const pb_field_t *field, void * const *arg) -{ - size_t i = 0; - - tRepeatedDoubleArguments* arguments = 0; - size_t count = 0; - double* values = 0; - - if (arg == NULL || *arg == NULL) - { - return false; - } - arguments = ((tRepeatedDoubleArguments*) (*arg)); - - count = arguments->max_size; - values = arguments->value; - - for (i = 0; i < count; i++) - { - - if (!pb_encode_tag_for_field(stream, field)) - { - return false; - } - - if (!pb_encode_fixed64(stream, &values[i])) - { - return false; - } - } - return true; -} - -bool decode_repeatedDouble(pb_istream_t *stream, const pb_field_t *field, void **arg) -{ - PB_UNUSED(field); - tRepeatedDoubleArguments* arguments = 0; - size_t i = 0; - double* values = 0; - - if (arg == NULL || *arg == NULL) - { - return false; - } - - arguments = (tRepeatedDoubleArguments*) *arg; - i = arguments->size; - values = arguments->value; - - if (values == NULL) - { - return true; - } - - if (!pb_decode_fixed64(stream, &values[i])) - { - return false; - } - - arguments->size++; - if (arguments->size >= arguments->max_size) - { - arguments->size = 0; - } - return true; -} - -bool encode_repeatedInt(pb_ostream_t *stream, const pb_field_t *field, void * const *arg) -{ - int i = 0; - tRepeatedIntArguments* arguments = 0; - int count = 0; - int64_t* values = 0; - - if (arg == NULL || *arg == NULL) - { - return false; - } - arguments = (tRepeatedIntArguments*) *arg; - count = arguments->max_size; - values = arguments->value; - for (i = 0; i < count; i++) - { - - if (!pb_encode_tag_for_field(stream, field)) - { - return false; - } - if (!pb_encode_varint(stream, values[i])) - { - return false; - } - } - return true; -} - -bool decode_repeatedInt(pb_istream_t *stream, const pb_field_t *field, void **arg) -{ - PB_UNUSED(field); - tRepeatedIntArguments* arguments = 0; - size_t i = 0; - uint64_t* values = 0; - - if (arg == NULL || *arg == NULL) - { - return false; - } - arguments = (tRepeatedIntArguments*) *arg; - - i = arguments->size; - values = (uint64_t*) arguments->value; - if (values == NULL) - { - return true; - } - - if (!pb_decode_varint(stream, &values[i])) - { - return false; - } - - arguments->size++; - if (arguments->size >= arguments->max_size) - { - arguments->size = 0; - } - return true; -} - -void map_repeatedDouble(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedDoubleArguments *arg) -{ - // IMPORTANT: the callbacks are stored in a union, therefore a message object - // must be exclusive defined for transmission or reception - if (dir == FRI_MANAGER_NANOPB_ENCODE) - { - values->funcs.encode = &encode_repeatedDouble; - } - else - { - values->funcs.decode = &decode_repeatedDouble; - } - // map the local container data to the message data fields - arg->max_size = numDOF; - arg->size = 0; - if (numDOF > 0) - { - arg->value = (double*) malloc(numDOF * sizeof(double)); - - } - values->arg = arg; -} - -void map_repeatedInt(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedIntArguments *arg) -{ - // IMPORTANT: the callbacks are stored in a union, therefore a message object - // must be exclusive defined for transmission or reception - if (dir == FRI_MANAGER_NANOPB_ENCODE) - { - // set the encode callback function - values->funcs.encode = &encode_repeatedInt; - } - else - { - // set the decode callback function - values->funcs.decode = &decode_repeatedInt; - } - // map the robot drive state from the container to message field - arg->max_size = numDOF; - arg->size = 0; - if (numDOF > 0) - { - arg->value = (int64_t*) malloc(numDOF * sizeof(int64_t)); - - } - values->arg = arg; -} - -void init_repeatedDouble(tRepeatedDoubleArguments *arg) -{ - arg->size = 0; - arg->max_size = 0; - arg->value = NULL; -} - -void init_repeatedInt(tRepeatedIntArguments *arg) -{ - arg->size = 0; - arg->max_size = 0; - arg->value = NULL; -} - -void free_repeatedDouble(tRepeatedDoubleArguments *arg) -{ - if (arg->value != NULL) - free(arg->value); -} - -void free_repeatedInt(tRepeatedIntArguments *arg) -{ - if (arg->value != NULL) - free(arg->value); -} +/** + + The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + + + \file + \version {2.5} + */ +#include +#include + +#include "pb_frimessages_callbacks.h" +#include "pb_encode.h" +#include "pb_decode.h" + +bool encode_repeatedDouble(pb_ostream_t *stream, const pb_field_t *field, void * const *arg) +{ + size_t i = 0; + + tRepeatedDoubleArguments* arguments = 0; + size_t count = 0; + double* values = 0; + + if (arg == NULL || *arg == NULL) + { + return false; + } + arguments = ((tRepeatedDoubleArguments*) (*arg)); + + count = arguments->max_size; + values = arguments->value; + + for (i = 0; i < count; i++) + { + + if (!pb_encode_tag_for_field(stream, field)) + { + return false; + } + + if (!pb_encode_fixed64(stream, &values[i])) + { + return false; + } + } + return true; +} + +bool decode_repeatedDouble(pb_istream_t *stream, const pb_field_t *field, void **arg) +{ + tRepeatedDoubleArguments* arguments = 0; + size_t i = 0; + double* values = 0; + + if (arg == NULL || *arg == NULL) + { + return false; + } + + arguments = (tRepeatedDoubleArguments*) *arg; + i = arguments->size; + values = arguments->value; + + if (values == NULL) + { + return true; + } + + if (!pb_decode_fixed64(stream, &values[i])) + { + return false; + } + + arguments->size++; + if (arguments->size >= arguments->max_size) + { + arguments->size = 0; + } + return true; +} + +bool encode_repeatedInt(pb_ostream_t *stream, const pb_field_t *field, void * const *arg) +{ + int i = 0; + tRepeatedIntArguments* arguments = 0; + int count = 0; + int64_t* values = 0; + + if (arg == NULL || *arg == NULL) + { + return false; + } + arguments = (tRepeatedIntArguments*) *arg; + count = arguments->max_size; + values = arguments->value; + for (i = 0; i < count; i++) + { + + if (!pb_encode_tag_for_field(stream, field)) + { + return false; + } + if (!pb_encode_varint(stream, values[i])) + { + return false; + } + } + return true; +} + +bool decode_repeatedInt(pb_istream_t *stream, const pb_field_t *field, void **arg) +{ + tRepeatedIntArguments* arguments = 0; + size_t i = 0; + uint64_t* values = 0; + + if (arg == NULL || *arg == NULL) + { + return false; + } + arguments = (tRepeatedIntArguments*) *arg; + + i = arguments->size; + values = (uint64_t*) arguments->value; + if (values == NULL) + { + return true; + } + + if (!pb_decode_varint(stream, &values[i])) + { + return false; + } + + arguments->size++; + if (arguments->size >= arguments->max_size) + { + arguments->size = 0; + } + return true; +} + +void map_repeatedDouble(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedDoubleArguments *arg) +{ + // IMPORTANT: the callbacks are stored in a union, therefor a message object + // must be exclusive defined for transmission or reception + if (dir == FRI_MANAGER_NANOPB_ENCODE) + { + values->funcs.encode = &encode_repeatedDouble; + } + else + { + values->funcs.decode = &decode_repeatedDouble; + } + // map the local container data to the message data fields + arg->max_size = numDOF; + arg->size = 0; + if (numDOF > 0) + { + arg->value = (double*) malloc(numDOF * sizeof(double)); + + } + values->arg = arg; +} + +void map_repeatedInt(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedIntArguments *arg) +{ + // IMPORTANT: the callbacks are stored in a union, therefor a message object + // must be exclusive defined for transmission or reception + if (dir == FRI_MANAGER_NANOPB_ENCODE) + { + // set the encode callback function + values->funcs.encode = &encode_repeatedInt; + } + else + { + // set the decode callback function + values->funcs.decode = &decode_repeatedInt; + } + // map the robot drive state from the container to message field + arg->max_size = numDOF; + arg->size = 0; + if (numDOF > 0) + { + arg->value = (int64_t*) malloc(numDOF * sizeof(int64_t)); + + } + values->arg = arg; +} + +void init_repeatedDouble(tRepeatedDoubleArguments *arg) +{ + arg->size = 0; + arg->max_size = 0; + arg->value = NULL; +} + +void init_repeatedInt(tRepeatedIntArguments *arg) +{ + arg->size = 0; + arg->max_size = 0; + arg->value = NULL; +} + +void free_repeatedDouble(tRepeatedDoubleArguments *arg) +{ + if (arg->value != NULL) + free(arg->value); +} + +void free_repeatedInt(tRepeatedIntArguments *arg) +{ + if (arg->value != NULL) + free(arg->value); +} diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.h b/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.h index 98597689..fdc1f4e7 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.h +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.h @@ -1,130 +1,121 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#ifndef _pb_frimessages_callbacks_H -#define _pb_frimessages_callbacks_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include - -/** container for repeated double elements */ -typedef struct repeatedDoubleArguments -{ - size_t size; - size_t max_size; - double * value; -} tRepeatedDoubleArguments; - -/** container for repeated integer elements */ -typedef struct repeatedIntArguments -{ - size_t size; - size_t max_size; - int64_t * value; -} tRepeatedIntArguments; - -/** enumeration for direction (encoding/decoding) */ -typedef enum DIRECTION -{ - FRI_MANAGER_NANOPB_DECODE = 0, //!< Argument um eine - FRI_MANAGER_NANOPB_ENCODE = 1 //!< -} eNanopbCallbackDirection; - - -bool encode_repeatedDouble( - pb_ostream_t * stream, const pb_field_t * field, - void * const * arg); - -bool decode_repeatedDouble( - pb_istream_t * stream, const pb_field_t * field, - void ** arg); - -bool encode_repeatedInt( - pb_ostream_t * stream, const pb_field_t * field, - void * const * arg); - -bool decode_repeatedInt( - pb_istream_t * stream, const pb_field_t * field, - void ** arg); - -void map_repeatedDouble( - eNanopbCallbackDirection dir, int numDOF, - pb_callback_t * values, tRepeatedDoubleArguments * arg); - -void map_repeatedInt( - eNanopbCallbackDirection dir, int numDOF, - pb_callback_t * values, tRepeatedIntArguments * arg); - -void init_repeatedDouble(tRepeatedDoubleArguments * arg); - -void init_repeatedInt(tRepeatedIntArguments * arg); - -void free_repeatedDouble(tRepeatedDoubleArguments * arg); - -void free_repeatedInt(tRepeatedIntArguments * arg); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _pb_frimessages_callbacks_H +#define _pb_frimessages_callbacks_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "pb.h" +#include "FRIMessages.pb.h" + +/** container for repeated double elements */ +typedef struct repeatedDoubleArguments { + size_t size; + size_t max_size; + double* value; +} tRepeatedDoubleArguments; + +/** container for repeated integer elements */ +typedef struct repeatedIntArguments { + size_t size; + size_t max_size; + int64_t* value; +} tRepeatedIntArguments; + +/** enumeration for direction (encoding/decoding) */ +typedef enum DIRECTION { + FRI_MANAGER_NANOPB_DECODE = 0, //!< Argument um eine + FRI_MANAGER_NANOPB_ENCODE = 1 //!< +} eNanopbCallbackDirection; + + +bool encode_repeatedDouble(pb_ostream_t *stream, const pb_field_t *field, + void * const *arg); + +bool decode_repeatedDouble(pb_istream_t *stream, const pb_field_t *field, + void **arg); + +bool encode_repeatedInt(pb_ostream_t *stream, const pb_field_t *field, + void * const *arg); + +bool decode_repeatedInt(pb_istream_t *stream, const pb_field_t *field, + void **arg); + +void map_repeatedDouble(eNanopbCallbackDirection dir, int numDOF, + pb_callback_t *values, tRepeatedDoubleArguments *arg); + +void map_repeatedInt(eNanopbCallbackDirection dir, int numDOF, + pb_callback_t *values, tRepeatedIntArguments *arg); + +void init_repeatedDouble(tRepeatedDoubleArguments *arg); + +void init_repeatedInt(tRepeatedIntArguments *arg); + +void free_repeatedDouble(tRepeatedDoubleArguments *arg); + +void free_repeatedInt(tRepeatedIntArguments *arg); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif From c4a709d593c4678685430be3659fbab10117c9b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Mon, 12 Aug 2024 17:15:36 +0200 Subject: [PATCH 14/49] robot application modified to work with fri sdk 2.5 --- .../src/application/ROS2_Control.java | 15 ++-- .../src/ros2/modules/FRIManager.java | 76 +++++++++++-------- .../src/ros2/modules/ROS2Connection.java | 22 +++--- ...ianImpedanceControlModeExternalizable.java | 25 +++--- .../ros2/serialization/ControlModeParams.java | 16 ++-- .../serialization/FRIConfigurationParams.java | 6 +- ...intImpedanceControlModeExternalizable.java | 3 +- 7 files changed, 88 insertions(+), 75 deletions(-) diff --git a/kuka_sunrise_fri_driver/robot_application/src/application/ROS2_Control.java b/kuka_sunrise_fri_driver/robot_application/src/application/ROS2_Control.java index d5fc6c6a..9adf72cf 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/application/ROS2_Control.java +++ b/kuka_sunrise_fri_driver/robot_application/src/application/ROS2_Control.java @@ -1,13 +1,15 @@ package application; +import com.kuka.io.IIoDefinitionProvider; +import com.kuka.roboticsAPI.applicationModel.IApplicationControl; +import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; +import com.kuka.sensitivity.LBR; +import com.kuka.task.RoboticsAPITask; import javax.inject.Inject; - import ros2.modules.FRIManager; import ros2.modules.ROS2Connection; import ros2.modules.TCPConnection; -import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; -import com.kuka.roboticsAPI.deviceModel.LBR; /** * Implementation of a robot application. @@ -30,18 +32,17 @@ public class ROS2_Control extends RoboticsAPIApplication { @Inject private LBR _lbr; - + private TCPConnection _TCPConnection; private ROS2Connection _ROS2Connection; private FRIManager _FRIManager; - + @Inject private IApplicationControl appControl; @Override public void initialize() { // initialize your application here _TCPConnection = new TCPConnection(30000); _ROS2Connection = new ROS2Connection(); - _FRIManager = new FRIManager(_lbr, getApplicationControl()); - + _FRIManager = new FRIManager(_lbr, appControl); _FRIManager.registerROS2ConnectionModule(_ROS2Connection); _TCPConnection.registerROS2ConnectionModule(_ROS2Connection); _ROS2Connection.registerTCPConnectionModule(_TCPConnection); diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java index 4e7d55b1..47651f31 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java @@ -1,30 +1,28 @@ package ros2.modules; +import com.kuka.fri.FRIConfiguration; +import com.kuka.fri.FRIJointOverlay; +import com.kuka.fri.FRISession; + +import com.kuka.fri.common.ClientCommandMode; +import com.kuka.fri.common.FRISessionState; +import com.kuka.io.IIoDefinitionProvider; +import com.kuka.motion.IMotionContainer; +import com.kuka.device.RoboticArm; +import com.kuka.roboticsAPI.applicationModel.IApplicationControl; +import com.kuka.roboticsAPI.motionModel.ErrorHandlingAction; +import com.kuka.roboticsAPI.motionModel.IMotionErrorHandler; +import com.kuka.roboticsAPI.motionModel.PositionHold; +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.sensitivity.LBR; +import com.kuka.sensitivity.controlmode.JointImpedanceControlMode; import java.util.Arrays; import java.util.List; import java.util.concurrent.TimeUnit; import java.util.concurrent.TimeoutException; - +import javax.inject.Inject; import ros2.serialization.FRIConfigurationParams; -import com.kuka.connectivity.fastRobotInterface.ClientCommandMode; -import com.kuka.connectivity.fastRobotInterface.FRIChannelInformation.FRISessionState; -import com.kuka.connectivity.fastRobotInterface.FRIChannelInformation; -import com.kuka.connectivity.fastRobotInterface.FRIConfiguration; -import com.kuka.connectivity.fastRobotInterface.FRIJointOverlay; -import com.kuka.connectivity.fastRobotInterface.FRISession; -import com.kuka.roboticsAPI.applicationModel.IApplicationControl; -import com.kuka.roboticsAPI.deviceModel.Device; -import com.kuka.roboticsAPI.deviceModel.LBR; -import com.kuka.roboticsAPI.motionModel.ErrorHandlingAction; -import com.kuka.roboticsAPI.motionModel.IErrorHandler; -import com.kuka.roboticsAPI.motionModel.IMotionContainer; -import com.kuka.roboticsAPI.motionModel.PositionHold; -import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; -import com.kuka.roboticsAPI.requestModel.GetApplicationOverrideRequest; - public class FRIManager{ public enum CommandResult{ EXECUTED, @@ -41,11 +39,10 @@ public enum CommandResult{ private ClientCommandMode _clientCommandMode; private IMotionContainer _motionContainer; private FRIMotionErrorHandler _friMotionErrorHandler = new FRIMotionErrorHandler(); - + private static double[] stiffness_ = new double[7]; - - - + + public FRIManager(LBR lbr, IApplicationControl applicationControl){ _currentState = new InactiveState(); _lbr = lbr; @@ -53,8 +50,8 @@ public FRIManager(LBR lbr, IApplicationControl applicationControl){ _FRIConfiguration = new FRIConfigurationParams().toFRIConfiguration(_lbr); Arrays.fill(stiffness_, 200); _controlMode = new JointImpedanceControlMode(stiffness_); - _clientCommandMode = ClientCommandMode.POSITION; - applicationControl.registerMoveAsyncErrorHandler(_friMotionErrorHandler); + _clientCommandMode = ClientCommandMode.JOINT_POSITION; + applicationControl.registerMotionErrorHandler(_friMotionErrorHandler); } public void registerROS2ConnectionModule(ROS2Connection ros2ConnectionModule){ @@ -213,7 +210,7 @@ public CommandResult activateControl(){ PositionHold motion = new PositionHold(FRIManager.this._controlMode, -1, null); FRIManager.this._motionContainer = - FRIManager.this._lbr.moveAsync(motion.addMotionOverlay(friJointOverlay)); + FRIManager.this._lbr.getFlange().moveAsync(motion.addMotionOverlay(friJointOverlay)); return CommandResult.EXECUTED; } @Override @@ -236,10 +233,10 @@ public CommandResult deactivateControl(){ } } - private class FRIMotionErrorHandler implements IErrorHandler{ + private class FRIMotionErrorHandler implements IMotionErrorHandler{ @Override - public ErrorHandlingAction handleError(Device device, + public ErrorHandlingAction handleExecutionError( IMotionContainer failedContainer, List canceledContainers) { FRISessionState sessionState = _FRISession.getFRIChannelInformation().getFRISessionState(); @@ -249,14 +246,33 @@ public ErrorHandlingAction handleError(Device device, break; default: FRIManager.this._ROS2Connection.handleControlEndedError(); - break; + break; } System.out.println("Failed container: " + failedContainer.toString() + "."); System.out.println("Error: " + failedContainer.getErrorMessage()); System.out.println("Runtime data: " + failedContainer.getRuntimeData()); System.out.println("Cancelled containers: " + canceledContainers.toString()); - return ErrorHandlingAction.Ignore; + return ErrorHandlingAction.IGNORE; } + @Override + public ErrorHandlingAction handleMaintainingError(IMotionContainer lastContainer, + List canceledContainers, + String errorMessage) { + FRISessionState sessionState = _FRISession.getFRIChannelInformation().getFRISessionState(); + switch(sessionState){ + case IDLE: + FRIManager.this._ROS2Connection.handleFRIEndedError(); + break; + default: + FRIManager.this._ROS2Connection.handleControlEndedError(); + break; + } + System.out.println("Last container: " + lastContainer.toString() + "."); + System.out.println("Error: " + lastContainer.getErrorMessage()); + System.out.println("Runtime data: " + lastContainer.getRuntimeData()); + System.out.println("Cancelled containers: " + canceledContainers.toString()); + return ErrorHandlingAction.IGNORE; + } } diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java index 4f259ba8..d5326d92 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java @@ -1,21 +1,17 @@ package ros2.modules; -import java.io.Externalizable; +import com.kuka.fri.common.ClientCommandMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.sensitivity.controlmode.JointImpedanceControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; +import com.kuka.sensitivity.controlmode.CartesianImpedanceControlMode; +import com.kuka.sensitivity.controlmode.JointImpedanceControlMode; import java.util.Arrays; - -import ros2.modules.FRIManager; import ros2.serialization.CartesianImpedanceControlModeExternalizable; -import ros2.serialization.ControlModeParams; import ros2.serialization.FRIConfigurationParams; import ros2.serialization.JointImpedanceControlModeExternalizable; import ros2.serialization.MessageEncoding; -import com.kuka.connectivity.fastRobotInterface.ClientCommandMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.CartesianImpedanceControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; - public class ROS2Connection { private TCPConnection _TCPConnection; @@ -99,7 +95,7 @@ private enum ControlModeID{ POSITION( (byte)1), JOINT_IMPEDANCE((byte)2), CARTESIAN_IMPEDANCE((byte)3); - + public final byte value; ControlModeID(byte value){ @@ -174,6 +170,8 @@ public void handleMessageFromROS(byte[] inMessage){ case SET_COMMAND_MODE: feedbackData = setCommandMode(inMessageData); break; + default: + break; } System.out.println("Command executed."); feedbackCommandSuccess(command, feedbackData); @@ -364,6 +362,8 @@ private byte[] setControlMode(byte[] cmdData){ controlMode = cartexternalizable.toControlMode(); System.out.println("Control mode CARTESIAN_IMPEDANCE selected"); break; + default: + break; } System.out.println("Control mode decoded."); FRIManager.CommandResult commandResult = _FRIManager.setControlMode(controlMode); diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java index fc3ae5c6..32dca2ce 100644 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java @@ -1,34 +1,31 @@ package ros2.serialization; +import com.kuka.roboticsAPI.motionModel.controlModeModel.CartDOF; +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.sensitivity.controlmode.CartesianImpedanceControlMode; import java.io.Externalizable; import java.io.IOException; import java.io.ObjectInput; import java.io.ObjectOutput; -import ros2.tools.Conversions; - -import com.kuka.roboticsAPI.geometricModel.CartDOF; -import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.CartesianImpedanceControlMode; - public class CartesianImpedanceControlModeExternalizable extends CartesianImpedanceControlMode implements Externalizable{ public final static int length = 96; - - + + public CartesianImpedanceControlModeExternalizable(CartesianImpedanceControlMode other){ super(other); } - + public CartesianImpedanceControlModeExternalizable(){ super(); } - + public IMotionControlMode toControlMode(){ CartesianImpedanceControlMode controlMode = new CartesianImpedanceControlMode((CartesianImpedanceControlMode)this); return (IMotionControlMode)controlMode; } - + @Override public void writeExternal(ObjectOutput out) throws IOException { for(double cartStiffness : getStiffness()){ @@ -42,7 +39,7 @@ public void writeExternal(ObjectOutput out) throws IOException { @Override public void readExternal(ObjectInput in) throws IOException, ClassNotFoundException { - double[] cartStiffness = new double[getStiffness().length]; + double[] cartStiffness = new double[getStiffness().length]; for(int i = 0; i < getStiffness().length; i++){ cartStiffness[i] = in.readDouble(); } @@ -53,7 +50,7 @@ public void readExternal(ObjectInput in) throws IOException, this.parametrize(CartDOF.B).setStiffness(cartStiffness[4]); this.parametrize(CartDOF.C).setStiffness(cartStiffness[5]); - double[] cartDamping = new double[getDamping().length]; + double[] cartDamping = new double[getDamping().length]; for(int i = 0; i < getDamping().length; i++){ cartDamping[i] = in.readDouble(); } @@ -63,7 +60,7 @@ public void readExternal(ObjectInput in) throws IOException, this.parametrize(CartDOF.A).setDamping(cartDamping[3]); this.parametrize(CartDOF.B).setDamping(cartDamping[4]); this.parametrize(CartDOF.C).setDamping(cartDamping[5]); - + } diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java index 8e2b93b4..88f1dc00 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java @@ -1,17 +1,15 @@ package ros2.serialization; +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; +import com.kuka.sensitivity.controlmode.CartesianImpedanceControlMode; +import com.kuka.sensitivity.controlmode.JointImpedanceControlMode; import java.io.Externalizable; import java.io.IOException; import java.io.ObjectInput; import java.io.ObjectOutput; import java.util.Arrays; - -import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.CartesianImpedanceControlMode; - public abstract class ControlModeParams implements Externalizable{ public static int length = 0; @@ -19,7 +17,7 @@ private enum ControlModeID{ POSITION( (byte)1), JOINT_IMPEDANCE((byte)2), CARTESIAN_IMPEDANCE((byte)3); - + public final byte value; ControlModeID(byte value){ @@ -103,9 +101,9 @@ public JointImpedanceControlModeParams(JointImpedanceControlMode controlMode){ class CartesianImpedanceControlModeParams extends ControlModeParams{ public CartesianImpedanceControlModeParams(){ - + } public CartesianImpedanceControlModeParams(CartesianImpedanceControlMode controlMode){ - + } } diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/FRIConfigurationParams.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/FRIConfigurationParams.java index 6e2bb022..d719cb3c 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/FRIConfigurationParams.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/FRIConfigurationParams.java @@ -1,11 +1,11 @@ package ros2.serialization; +import com.kuka.fri.FRIConfiguration; +import com.kuka.device.RoboticArm; import java.io.Externalizable; import java.io.IOException; import java.io.ObjectInput; import java.io.ObjectOutput; -import com.kuka.connectivity.fastRobotInterface.FRIConfiguration; -import com.kuka.roboticsAPI.deviceModel.Device; public class FRIConfigurationParams implements Externalizable { @@ -53,7 +53,7 @@ public FRIConfigurationParams(FRIConfiguration friConfiguration){ _receiveMultiplier = friConfiguration.getReceiveMultiplier(); } - public FRIConfiguration toFRIConfiguration(Device device){ + public FRIConfiguration toFRIConfiguration(RoboticArm device){ FRIConfiguration friConfiguration = FRIConfiguration.createRemoteConfiguration(device, _remoteIP); friConfiguration.setPortOnRemote(_remotePort); friConfiguration.setSendPeriodMilliSec(_sendPeriodMilliSec); diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/JointImpedanceControlModeExternalizable.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/JointImpedanceControlModeExternalizable.java index 618205a2..838707bb 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/JointImpedanceControlModeExternalizable.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/JointImpedanceControlModeExternalizable.java @@ -8,7 +8,8 @@ import ros2.tools.Conversions; import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; +import com.kuka.sensitivity.controlmode.JointImpedanceControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.AbstractMotionControlMode; public class JointImpedanceControlModeExternalizable extends JointImpedanceControlMode implements Externalizable{ From 5ac53a8569a7acd0408f71e619a02b4d3836f804 Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Mon, 26 Aug 2024 10:35:35 +0200 Subject: [PATCH 15/49] wrench config typo --- .../config/wrench_controller_config.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml b/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml index 2b6bf99a..f6f02342 100644 --- a/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml @@ -1,12 +1,12 @@ wrench_controller: ros__parameters: joints: - - dummy_cart_jointx - - dummy_cart_jointy - - dummy_cart_jointz - - dummy_cart_jointa - - dummy_cart_jointb - - dummy_cart_jointc + - dummy_cart_joint/x + - dummy_cart_joint/y + - dummy_cart_joint/z + - dummy_cart_joint/a + - dummy_cart_joint/b + - dummy_cart_joint/c interface_names: - effort From 9aef28de9ff1570ae4bb83f06885b7d67c797510 Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Mon, 26 Aug 2024 11:36:40 +0200 Subject: [PATCH 16/49] industrial ci upstream robot description branch to fri wrench control --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 5ccbf26e..42e55e23 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -33,7 +33,7 @@ jobs: BUILDER: colcon ANALYZER: sonarqube TEST_COVERAGE: true - UPSTREAM_WORKSPACE: 'github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka-external-control-sdk#master' + UPSTREAM_WORKSPACE: 'github:kroshu/kuka_robot_descriptions#fri_wrench_control github:kroshu/kuka-external-control-sdk#master' ROS_DISTRO: humble CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) EVENT_NAME: ${{ github.event_name }} From e82711c91b930f3d742f0d15009b45dacd0b7bfc Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Mon, 26 Aug 2024 11:41:45 +0200 Subject: [PATCH 17/49] branch typo --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 42e55e23..d358829e 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -33,7 +33,7 @@ jobs: BUILDER: colcon ANALYZER: sonarqube TEST_COVERAGE: true - UPSTREAM_WORKSPACE: 'github:kroshu/kuka_robot_descriptions#fri_wrench_control github:kroshu/kuka-external-control-sdk#master' + UPSTREAM_WORKSPACE: 'github:kroshu/kuka_robot_descriptions#feature/fri_wrench_control github:kroshu/kuka-external-control-sdk#master' ROS_DISTRO: humble CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) EVENT_NAME: ${{ github.event_name }} From 56f6c89bebdfb6fc9aef799a78dfa96dd8e1dbfb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Wed, 28 Aug 2024 10:04:15 +0200 Subject: [PATCH 18/49] missing file --- .../include/fri_client_sdk/friDataHelper.h | 129 ++++++++++++++++++ 1 file changed, 129 insertions(+) create mode 100644 kuka_sunrise_fri_driver/include/fri_client_sdk/friDataHelper.h diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friDataHelper.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friDataHelper.h new file mode 100644 index 00000000..e287ebbc --- /dev/null +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friDataHelper.h @@ -0,0 +1,129 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_DATA_HELPER_H +#define _KUKA_FRI_DATA_HELPER_H + +#include + +namespace KUKA +{ + namespace FRI + { + + /** + * \brief Data helper class containing conversion functions. + */ + struct DataHelper + { + + /** + * \brief Helper enum to access quaternion entries. + */ + enum QUATERNION_IDX + { + QUAT_TX = 0, + QUAT_TY, + QUAT_TZ, + QUAT_QW, + QUAT_QX, + QUAT_QY, + QUAT_QZ + }; + + + /** + * \brief Function to convert a matrix transformation to a normalized quaternion transformation. + * + * The resulting quaternion transformation is provided as [t_x, t_y, t_z, q_w, q_x, q_y, q_z], + * with a unit quaternion, i.e. the length of vector [q_w, q_x, q_y, q_z] must be 1. + * The input transformation matrix has 3x4 elements. It consists of a rotational matrix (3x3 elements) + * and a translational vector (3x1 elements). The complete transformation matrix has the + * following structure: + * [Transformation(3x4)] = [Rotation(3x3) | Translation(3x1) ] + * + * @param[in] matrixTrafo given matrix transformation + * @param[out] quaternionTrafo resulting quaternion transformation + */ + static void convertTrafoMatrixToQuaternion(const double (&matrixTrafo)[3][4], + double (&quaternionTrafo)[7]); + + + /** + * \brief Function to convert a quaternion transformation to a matrix transformation. + * + * The input quaternion transformation must be provided as [t_x, t_y, t_z, q_w, q_x, q_y, q_z], + * with a unit quaternion, i.e. the length of vector [q_w, q_x, q_y, q_z] must be 1. + * The output transformation matrix has 3x4 elements. It consists of a rotational matrix (3x3 elements) + * and a translational vector (3x1 elements). The complete transformation matrix has the + * following structure: + * [Transformation(3x4)] = [Rotation(3x3) | Translation(3x1) ] + * + * @param[in] quaternionTrafo given quaternion transformation + * @param[out] matrixTrafo resulting matrix transformation + */ + static void convertTrafoQuaternionToMatrix(const double(&quaternionTrafo)[7], + double(&matrixTrafo)[3][4]); + + }; + + } +} + +#endif // _KUKA_FRI_DATA_HELPER_H From 531a805c0f01d32d7edecd1cb91ff8c19eaeda18 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Wed, 28 Aug 2024 10:13:44 +0200 Subject: [PATCH 19/49] add missing files --- .../fri_client_sdk/friClientApplication.h | 307 ++++----- .../include/fri_client_sdk/friClientIf.h | 409 +++++------ .../include/fri_client_sdk/friConnectionIf.h | 259 +++---- .../include/fri_client_sdk/friException.h | 305 ++++----- .../include/fri_client_sdk/friLBRClient.h | 288 ++++---- .../include/fri_client_sdk/friLBRCommand.h | 366 +++++----- .../include/fri_client_sdk/friLBRState.h | 645 ++++++++++-------- .../fri_client_sdk/friTransformationClient.h | 558 +++++++-------- .../include/fri_client_sdk/friUdpConnection.h | 324 ++++----- 9 files changed, 1823 insertions(+), 1638 deletions(-) diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friClientApplication.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friClientApplication.h index cdbe4e7d..5a61ebd0 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friClientApplication.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friClientApplication.h @@ -1,152 +1,155 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ - -#ifndef _KUKA_FRI_CLIENT_APPLICATION_H -#define _KUKA_FRI_CLIENT_APPLICATION_H - -/** Kuka namespace */ -namespace KUKA -{ -/** Fast Robot Interface (FRI) namespace */ -namespace FRI -{ - -// forward declarations -class IClient; -class TransformationClient; -class IConnection; -struct ClientData; - -/** - * \brief FRI client application class. - * - * A client application takes an instance of the IConnection interface and - * an instance of an IClient interface to provide the functionality - * needed to set up an FRI client application. It can be used to easily - * integrate the FRI client code within other applications. - * The algorithmic functionality of an FRI client application is implemented - * using the IClient interface. - */ -class ClientApplication -{ - -public: - /** - * \brief Constructor without transformation client. - * - * This constructor takes an instance of the IConnection interface and - * an instance of the IClient interface as parameters. - * @param connection FRI connection class - * @param client FRI client class - */ - ClientApplication(IConnection & connection, IClient & client); - - /** - * \brief Constructor with transformation client. - * - * This constructor takes an instance of the IConnection interface and - * an instance of the IClient interface and an instance of a - * TransformationClient as parameters. - * @param connection FRI connection class - * @param client FRI client class - * @param trafoClient FRI transformation client class - */ - ClientApplication(IConnection & connection, IClient & client, TransformationClient & trafoClient); - - /** \brief Destructor. */ - ~ClientApplication(); - - /** - * \brief Connect the FRI client application with a KUKA Sunrise controller. - * - * @param port The port ID - * @param remoteHost The address of the remote host - * @return True if connection was established - */ - bool connect(int port, const char * remoteHost = NULL); - - /** - * \brief Disconnect the FRI client application from a KUKA Sunrise controller. - */ - void disconnect(); - - /** - * \brief Run a single processing step. - * - * The processing step consists of receiving a new FRI monitoring message, - * calling the corresponding client callback and sending the resulting - * FRI command message back to the KUKA Sunrise controller. - * @return True if all of the substeps succeeded. - */ - bool step(); - -protected: - IConnection & _connection; //!< connection interface - IClient * _robotClient; //!< robot client interface - TransformationClient * _trafoClient; //!< transformation client interface - ClientData * _data; //!< client data structure (for internal use) - -}; - -} -} - - -#endif // _KUKA_FRI_CLIENT_APPLICATION_H +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ + +#ifndef _KUKA_FRI_CLIENT_APPLICATION_H +#define _KUKA_FRI_CLIENT_APPLICATION_H + +/** Kuka namespace */ +namespace KUKA +{ +/** Fast Robot Interface (FRI) namespace */ +namespace FRI +{ + + // forward declarations + class IClient; + class TransformationClient; + class IConnection; + struct ClientData; + + /** + * \brief FRI client application class. + * + * A client application takes an instance of the IConnection interface and + * an instance of an IClient interface to provide the functionality + * needed to set up an FRI client application. It can be used to easily + * integrate the FRI client code within other applications. + * The algorithmic functionality of an FRI client application is implemented + * using the IClient interface. + */ + class ClientApplication + { + + public: + + /** + * \brief Constructor without transformation client. + * + * This constructor takes an instance of the IConnection interface and + * an instance of the IClient interface as parameters. + * @param connection FRI connection class + * @param client FRI client class + */ + ClientApplication(IConnection& connection, IClient& client); + + /** + * \brief Constructor with transformation client. + * + * This constructor takes an instance of the IConnection interface and + * an instance of the IClient interface and an instance of a + * TransformationClient as parameters. + * @param connection FRI connection class + * @param client FRI client class + * @param trafoClient FRI transformation client class + */ + ClientApplication(IConnection& connection, IClient& client, TransformationClient& trafoClient); + + /** \brief Destructor. */ + ~ClientApplication(); + + /** + * \brief Connect the FRI client application with a KUKA Sunrise controller. + * + * @param port The port ID + * @param remoteHost The address of the remote host + * @return True if connection was established + */ + bool connect(int port, const char *remoteHost = NULL); + + /** + * \brief Disconnect the FRI client application from a KUKA Sunrise controller. + */ + void disconnect(); + + /** + * \brief Run a single processing step. + * + * The processing step consists of receiving a new FRI monitoring message, + * calling the corresponding client callback and sending the resulting + * FRI command message back to the KUKA Sunrise controller. + * @return True if all of the substeps succeeded. + */ + bool step(); + + protected: + + IConnection& _connection; //!< connection interface + IClient* _robotClient; //!< robot client interface + TransformationClient* _trafoClient; //!< transformation client interface + ClientData* _data; //!< client data structure (for internal use) + + }; + +} +} + + +#endif // _KUKA_FRI_CLIENT_APPLICATION_H + diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friClientIf.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friClientIf.h index a108e41d..1a2abda9 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friClientIf.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friClientIf.h @@ -1,199 +1,210 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software �KUKA Sunrise.Connectivity FRI Client SDK� is targeted to work in -conjunction with the �KUKA Sunrise.Connectivity FastRobotInterface� toolkit. -In the following, the term �software� refers to all material directly -belonging to the provided SDK �Software development kit�, particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#ifndef _KUKA_FRI_CLIENT_H -#define _KUKA_FRI_CLIENT_H - - -/** Kuka namespace */ -namespace KUKA -{ -/** Fast Robot Interface (FRI) namespace */ -namespace FRI -{ - -// forward declarations -struct ClientData; - - -/** \brief Enumeration of the FRI session state. */ -enum ESessionState -{ - IDLE = 0, //!< no session available - MONITORING_WAIT = 1, //!< monitoring mode with insufficient connection quality - MONITORING_READY = 2, //!< monitoring mode with connection quality sufficient for command mode - COMMANDING_WAIT = 3, //!< command mode about to start (overlay motion queued) - COMMANDING_ACTIVE = 4 //!< command mode active -}; - -/** \brief Enumeration of the FRI connection quality. */ -enum EConnectionQuality -{ - POOR = 0, //!< poor connection quality - FAIR = 1, //!< connection quality insufficient for command mode - GOOD = 2, //!< connection quality sufficient for command mode - EXCELLENT = 3 //!< excellent connection quality -}; - -/** \brief Enumeration of the controller's safety state. */ -enum ESafetyState -{ - NORMAL_OPERATION = 0, //!< No safety stop request present. - SAFETY_STOP_LEVEL_0 = 1, //!< Safety stop request STOP0 or STOP1 present. - SAFETY_STOP_LEVEL_1 = 2, //!< Safety stop request STOP1 (on-path) present. - SAFETY_STOP_LEVEL_2 = 3 //!< Safety stop request STOP2 present. -}; - -/** \brief Enumeration of the controller's current mode of operation. */ -enum EOperationMode -{ - TEST_MODE_1 = 0, //!< test mode 1 with reduced speed (T1) - TEST_MODE_2 = 1, //!< test mode 2 (T2) - AUTOMATIC_MODE = 2 //!< automatic operation mode (AUT) -}; - -/** \brief Enumeration of a drive's state. */ -enum EDriveState -{ - OFF = 0, //!< drive is not being used - TRANSITIONING = 1, //!< drive is in a transitioning state (before or after motion) - ACTIVE = 2 //!< drive is being actively commanded -}; - -/** \brief Enumeration of control mode. */ -enum EControlMode -{ - POSITION_CONTROL_MODE = 0, //!< position control mode - CART_IMP_CONTROL_MODE = 1, //!< cartesian impedance control mode - JOINT_IMP_CONTROL_MODE = 2, //!< joint impedance control mode - NO_CONTROL = 3 //!< drives are not used -}; - - -/** \brief Enumeration of the client command mode. */ -enum EClientCommandMode -{ - NO_COMMAND_MODE = 0, //!< no client command mode available - POSITION = 1, //!< commanding joint positions by the client - WRENCH = 2, //!< commanding wrenches and joint positions by the client - TORQUE = 3 //!< commanding joint torques and joint positions by the client -}; - -/** \brief Enumeration of the overlay type. */ -enum EOverlayType -{ - NO_OVERLAY = 0, //!< no overlay type available - JOINT = 1, //!< joint overlay - CARTESIAN = 2 //!< cartesian overlay -}; - - -/** - * \brief FRI client interface. - * - * This is the callback interface that should be implemented by FRI clients. - * Callbacks are automatically called by the client application - * (ClientApplication) whenever new FRI messages arrive. - */ -class IClient -{ - friend class ClientApplication; - -public: - /** \brief Virtual destructor. */ - virtual ~IClient() {} - - /** - * \brief Callback that is called whenever the FRI session state changes. - * - * @param oldState previous FRI session state - * @param newState current FRI session state - */ - virtual void onStateChange(ESessionState oldState, ESessionState newState) = 0; - - /** - * \brief Callback for the FRI session states 'Monitoring Wait' and 'Monitoring Ready'. - */ - virtual void monitor() = 0; - - /** - * \brief Callback for the FRI session state 'Commanding Wait'. - */ - virtual void waitForCommand() = 0; - - /** - * \brief Callback for the FRI session state 'Commanding'. - */ - virtual void command() = 0; - -protected: - /** - * \brief Method to create and initialize the client data structure (used internally). - * - * @return newly allocated client data structure - */ - virtual ClientData * createData() = 0; - -}; - -} -} - - -#endif // _KUKA_FRI_CLIENT_H +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_CLIENT_H +#define _KUKA_FRI_CLIENT_H + + + +/** Kuka namespace */ +namespace KUKA +{ +/** Fast Robot Interface (FRI) namespace */ +namespace FRI +{ + + // forward declarations + struct ClientData; + + + /** \brief Enumeration of the FRI session state. */ + enum ESessionState + { + IDLE = 0, //!< no session available + MONITORING_WAIT = 1, //!< monitoring mode with insufficient connection quality + MONITORING_READY = 2, //!< monitoring mode with connection quality sufficient for command mode + COMMANDING_WAIT = 3, //!< command mode about to start (overlay motion queued) + COMMANDING_ACTIVE = 4 //!< command mode active + }; + + /** \brief Enumeration of the FRI connection quality. */ + enum EConnectionQuality + { + POOR = 0, //!< poor connection quality + FAIR = 1, //!< connection quality insufficient for command mode + GOOD = 2, //!< connection quality sufficient for command mode + EXCELLENT = 3 //!< excellent connection quality + }; + + /** \brief Enumeration of the controller's safety state. */ + enum ESafetyState + { + NORMAL_OPERATION = 0, //!< No safety stop request present. + SAFETY_STOP_LEVEL_0 = 1,//!< Safety stop request STOP0 or STOP1 present. + SAFETY_STOP_LEVEL_1 = 2,//!< Safety stop request STOP1 (on-path) present. + SAFETY_STOP_LEVEL_2 = 3 //!< Safety stop request STOP2 present. + }; + + /** \brief Enumeration of the controller's current mode of operation. */ + enum EOperationMode + { + TEST_MODE_1 = 0, //!< test mode 1 with reduced speed (T1) + TEST_MODE_2 = 1, //!< test mode 2 (T2) + AUTOMATIC_MODE = 2 //!< automatic operation mode (AUT) + }; + + /** \brief Enumeration of a drive's state. */ + enum EDriveState + { + OFF = 0, //!< drive is not being used + TRANSITIONING = 1, //!< drive is in a transitioning state (before or after motion) + ACTIVE = 2 //!< drive is being actively commanded + }; + + /** \brief Enumeration of control mode. */ + enum EControlMode + { + POSITION_CONTROL_MODE = 0, //!< position control mode + CART_IMP_CONTROL_MODE = 1, //!< cartesian impedance control mode + JOINT_IMP_CONTROL_MODE = 2, //!< joint impedance control mode + NO_CONTROL = 3 //!< drives are not used + }; + + + /** \brief Enumeration of the client command mode. */ + enum EClientCommandMode + { + NO_COMMAND_MODE = 0, //!< no client command mode available + JOINT_POSITION = 1, //!< commanding joint positions by the client + WRENCH = 2, //!< commanding wrenches and joint positions by the client + TORQUE = 3, //!< commanding joint torques and joint positions by the client + CARTESIAN_POSE = 4 //!< commanding Cartesian poses by the client + }; + + /** \brief Enumeration of the overlay type. */ + enum EOverlayType + { + NO_OVERLAY = 0, //!< no overlay type available + JOINT = 1, //!< joint overlay + CARTESIAN = 2 //!< cartesian overlay + }; + + /** \brief Enumeration of redundancy strategies. */ + enum ERedundancyStrategy + { + E1 = 0, //!< E1 redundancy strategy + NO_STRATEGY = 4 //!< No redundancy strategy + }; + + /** + * \brief FRI client interface. + * + * This is the callback interface that should be implemented by FRI clients. + * Callbacks are automatically called by the client application + * (ClientApplication) whenever new FRI messages arrive. + */ + class IClient + { + friend class ClientApplication; + + public: + + /** \brief Virtual destructor. */ + virtual ~IClient() {} + + /** + * \brief Callback that is called whenever the FRI session state changes. + * + * @param oldState previous FRI session state + * @param newState current FRI session state + */ + virtual void onStateChange(ESessionState oldState, ESessionState newState) = 0; + + /** + * \brief Callback for the FRI session states 'Monitoring Wait' and 'Monitoring Ready'. + */ + virtual void monitor() = 0; + + /** + * \brief Callback for the FRI session state 'Commanding Wait'. + */ + virtual void waitForCommand() = 0; + + /** + * \brief Callback for the FRI session state 'Commanding'. + */ + virtual void command() = 0; + + protected: + + /** + * \brief Method to create and initialize the client data structure (used internally). + * + * @return newly allocated client data structure + */ + virtual ClientData* createData() = 0; + + + }; + +} +} + + +#endif // _KUKA_FRI_CLIENT_H diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friConnectionIf.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friConnectionIf.h index 57da47a5..0cd10666 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friConnectionIf.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friConnectionIf.h @@ -1,129 +1,130 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software �KUKA Sunrise.Connectivity FRI Client SDK� is targeted to work in -conjunction with the �KUKA Sunrise.Connectivity FastRobotInterface� toolkit. -In the following, the term �software� refers to all material directly -belonging to the provided SDK �Software development kit�, particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#ifndef _KUKA_FRI_CONNECTION_H -#define _KUKA_FRI_CONNECTION_H - - -/** Kuka namespace */ -namespace KUKA -{ -namespace FRI -{ - -/** - * \brief FRI connection interface. - * - * Connections to the KUKA Sunrise controller have to be implemented using - * this interface. - */ -class IConnection -{ - -public: - /** \brief Virtual destructor. */ - virtual ~IConnection() {} - - /** - * \brief Open a connection to the KUKA Sunrise controller. - * - * @param port The port ID - * @param remoteHost The address of the remote host - * @return True if connection was established - */ - virtual bool open(int port, const char * remoteHost) = 0; - - /** - * \brief Close a connection to the KUKA Sunrise controller. - */ - virtual void close() = 0; - - /** - * \brief Checks whether a connection to the KUKA Sunrise controller is established. - * - * @return True if connection is established - */ - virtual bool isOpen() const = 0; - - /** - * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. - * - * This method blocks until a new message arrives. - * @param buffer Pointer to the allocated buffer that will hold the FRI message - * @param maxSize Size in bytes of the allocated buffer - * @return Number of bytes received (0 when connection was terminated, negative in case of errors) - */ - virtual int receive(char * buffer, int maxSize) = 0; - - /** - * \brief Send a new FRI command message to the KUKA Sunrise controller. - * - * @param buffer Pointer to the buffer holding the FRI message - * @param size Size in bytes of the message to be send - * @return True if successful - */ - virtual bool send(const char * buffer, int size) = 0; - -}; - -} -} - - -#endif // _KUKA_FRI_CONNECTION_H +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_CONNECTION_H +#define _KUKA_FRI_CONNECTION_H + + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + /** + * \brief FRI connection interface. + * + * Connections to the KUKA Sunrise controller have to be implemented using + * this interface. + */ + class IConnection + { + + public: + + /** \brief Virtual destructor. */ + virtual ~IConnection() {} + + /** + * \brief Open a connection to the KUKA Sunrise controller. + * + * @param port The port ID + * @param remoteHost The address of the remote host + * @return True if connection was established + */ + virtual bool open(int port, const char *remoteHost) = 0; + + /** + * \brief Close a connection to the KUKA Sunrise controller. + */ + virtual void close() = 0; + + /** + * \brief Checks whether a connection to the KUKA Sunrise controller is established. + * + * @return True if connection is established + */ + virtual bool isOpen() const = 0; + + /** + * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. + * + * This method blocks until a new message arrives. + * @param buffer Pointer to the allocated buffer that will hold the FRI message + * @param maxSize Size in bytes of the allocated buffer + * @return Number of bytes received (0 when connection was terminated, negative in case of errors) + */ + virtual int receive(char *buffer, int maxSize) = 0; + + /** + * \brief Send a new FRI command message to the KUKA Sunrise controller. + * + * @param buffer Pointer to the buffer holding the FRI message + * @param size Size in bytes of the message to be send + * @return True if successful + */ + virtual bool send(const char* buffer, int size) = 0; + + }; + +} +} + + +#endif // _KUKA_FRI_CONNECTION_H diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friException.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friException.h index 2bda1ffd..ebf33c99 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friException.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friException.h @@ -1,152 +1,153 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#ifndef _KUKA_FRI_EXCEPTION_H -#define _KUKA_FRI_EXCEPTION_H - -#include - -/** Kuka namespace */ -namespace KUKA -{ -namespace FRI -{ - -/** - * \brief Standard exception for the FRI Client - * - * \note For realtime considerations the internal message buffer is static. - * So don't use this exception in more than one thread per process. - */ -class FRIException -{ - -public: - /** - * \brief FRIException Constructor - * - * @param message Error message - */ - FRIException(const char * message) - { - strncpy(_buffer, message, sizeof(_buffer) - 1); - _buffer[sizeof(_buffer) - 1] = 0; // ensure string termination - printf("FRIException: "); - printf("%s", _buffer); - printf("\n"); - } - - /** - * \brief FRIException Constructor - * - * @param message Error message which may contain one "%s" parameter - * @param param1 First format parameter for parameter message. - */ - FRIException(const char * message, const char * param1) - { -#ifdef _MSC_VER - _snprintf( // visual studio compilers (up to VS 2013) only know this method -#else - snprintf( -#endif - _buffer, sizeof(_buffer), message, param1); - printf("FRIException: "); - printf("%s", _buffer); - printf("\n"); - } - - /** - * \brief FRIException Constructor - * - * @param message Error message which may contain two "%s" parameter - * @param param1 First format parameter for parameter message. - * @param param2 Second format parameter for parameter message. - */ - FRIException(const char * message, const char * param1, const char * param2) - { -#ifdef _MSC_VER - _snprintf( // visual studio compilers (up to VS 2013) only know this method -#else - snprintf( -#endif - _buffer, sizeof(_buffer), message, param1, param2); - printf("FRIException: "); - printf("%s", _buffer); - printf("\n"); - } - - /** - * \brief Get error string. - * @return Error message stored in the exception. - */ - const char * getErrorMessage() const {return _buffer;} - - /** \brief Virtual destructor. */ - virtual ~FRIException() {} - -protected: - static char _buffer[1024]; - -}; - -} -} - - -#endif // _KUKA_FRI_EXCEPTION_H +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_EXCEPTION_H +#define _KUKA_FRI_EXCEPTION_H + +#include "stdio.h" + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + /** + * \brief Standard exception for the FRI Client + * + * \note For realtime considerations the internal message buffer is static. + * So don't use this exception in more than one thread per process. + */ + class FRIException + { + + public: + + /** + * \brief FRIException Constructor + * + * @param message Error message + */ + FRIException(const char* message) + { + strncpy(_buffer, message, sizeof(_buffer) - 1); + _buffer[sizeof(_buffer) - 1] = 0; // ensure string termination + printf("FRIException: "); + printf(_buffer); + printf("\n"); + } + + /** + * \brief FRIException Constructor + * + * @param message Error message which may contain one "%s" parameter + * @param param1 First format parameter for parameter message. + */ + FRIException(const char* message, const char* param1) + { +#ifdef _MSC_VER + _snprintf( // visual studio compilers (up to VS 2013) only know this method +#else + snprintf( +#endif + _buffer, sizeof(_buffer), message, param1); + printf("FRIException: "); + printf(_buffer); + printf("\n"); + } + + /** + * \brief FRIException Constructor + * + * @param message Error message which may contain two "%s" parameter + * @param param1 First format parameter for parameter message. + * @param param2 Second format parameter for parameter message. + */ + FRIException(const char* message, const char* param1, const char* param2) + { +#ifdef _MSC_VER + _snprintf( // visual studio compilers (up to VS 2013) only know this method +#else + snprintf( +#endif + _buffer, sizeof(_buffer), message, param1, param2); + printf("FRIException: "); + printf(_buffer); + printf("\n"); + } + + /** + * \brief Get error string. + * @return Error message stored in the exception. + */ + const char* getErrorMessage() const { return _buffer; } + + /** \brief Virtual destructor. */ + virtual ~FRIException() {} + + protected: + static char _buffer[1024]; + + }; + +} +} + + +#endif // _KUKA_FRI_EXCEPTION_H diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRClient.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRClient.h index 06810d36..a1e7bce9 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRClient.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRClient.h @@ -1,143 +1,145 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#ifndef _KUKA_FRI_LBR_CLIENT_H -#define _KUKA_FRI_LBR_CLIENT_H - -#include -#include -#include - -/** Kuka namespace */ -namespace KUKA -{ -namespace FRI -{ - -/** - * \brief Implementation of the IClient interface for the KUKA LBR (lightweight) robots. - * - * Provides access to the current LBR state and the possibility to send new - * commands to the LBR. - */ -class LBRClient : public IClient -{ - -public: - /** \brief Constructor. */ - LBRClient(); - - /** \brief Destructor. */ - ~LBRClient(); - - /** - * \brief Callback that is called whenever the FRI session state changes. - * - * @param oldState previous FRI session state - * @param newState current FRI session state - */ - virtual void onStateChange(ESessionState oldState, ESessionState newState); - - /** - * \brief Callback for the FRI session states 'Monitoring Wait' and 'Monitoring Ready'. - */ - virtual void monitor(); - - /** - * \brief Callback for the FRI session state 'Commanding Wait'. - */ - virtual void waitForCommand(); - - /** - * \brief Callback for the FRI session state 'Commanding'. - */ - virtual void command(); - - /** - * \brief Provide read access to the current robot state. - * - * @return Reference to the LBRState instance - */ - const LBRState & robotState() const {return _robotState;} - - /** - * \brief Provide write access to the robot commands. - * - * @return Reference to the LBRCommand instance - */ - LBRCommand & robotCommand() {return _robotCommand;} - -private: - LBRState _robotState; //!< wrapper class for the FRI monitoring message - LBRCommand _robotCommand; //!< wrapper class for the FRI command message - - /** - * \brief Method to create and initialize the client data structure (used internally). - * - * @return newly allocated client data structure - */ - virtual ClientData * createData(); - -}; - -} -} - - -#endif // _KUKA_FRI_LBR_CLIENT_H +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_LBR_CLIENT_H +#define _KUKA_FRI_LBR_CLIENT_H + +#include +#include +#include + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + /** + * \brief Implementation of the IClient interface for the KUKA LBR (lightweight) robots. + * + * Provides access to the current LBR state and the possibility to send new + * commands to the LBR. + */ + class LBRClient : public IClient + { + + public: + + /** \brief Constructor. */ + LBRClient(); + + /** \brief Destructor. */ + ~LBRClient(); + + /** + * \brief Callback that is called whenever the FRI session state changes. + * + * @param oldState previous FRI session state + * @param newState current FRI session state + */ + virtual void onStateChange(ESessionState oldState, ESessionState newState); + + /** + * \brief Callback for the FRI session states 'Monitoring Wait' and 'Monitoring Ready'. + */ + virtual void monitor(); + + /** + * \brief Callback for the FRI session state 'Commanding Wait'. + */ + virtual void waitForCommand(); + + /** + * \brief Callback for the FRI session state 'Commanding'. + */ + virtual void command(); + + /** + * \brief Provide read access to the current robot state. + * + * @return Reference to the LBRState instance + */ + const LBRState& robotState() const { return _robotState; } + + /** + * \brief Provide write access to the robot commands. + * + * @return Reference to the LBRCommand instance + */ + LBRCommand& robotCommand() { return _robotCommand; } + + private: + + LBRState _robotState; //!< wrapper class for the FRI monitoring message + LBRCommand _robotCommand; //!< wrapper class for the FRI command message + + /** + * \brief Method to create and initialize the client data structure (used internally). + * + * @return newly allocated client data structure + */ + virtual ClientData* createData(); + + }; + +} +} + + +#endif // _KUKA_FRI_LBR_CLIENT_H diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRCommand.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRCommand.h index 8787bfad..190771e4 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRCommand.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRCommand.h @@ -1,160 +1,206 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software �KUKA Sunrise.Connectivity FRI Client SDK� is targeted to work in -conjunction with the �KUKA Sunrise.Connectivity FastRobotInterface� toolkit. -In the following, the term �software� refers to all material directly -belonging to the provided SDK �Software development kit�, particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#ifndef _KUKA_FRI_LBR_COMMAND_H -#define _KUKA_FRI_LBR_COMMAND_H - - -// forward declarations -typedef struct _FRICommandMessage FRICommandMessage; - -/** Kuka namespace */ -namespace KUKA -{ -namespace FRI -{ - -/** - * \brief Wrapper class for the FRI command message for a KUKA LBR (lightweight) robot. - */ -class LBRCommand -{ - friend class LBRClient; - -public: - /** - * \brief Set the joint positions for the current interpolation step. - * - * This method is only effective when the client is in a commanding state. - * @param values Array with the new joint positions (in rad) - */ - void setJointPosition(const double * values); - - /** - * \brief Set the applied wrench vector of the current interpolation step. - * - * The wrench vector consists of: - * [F_x, F_y, F_z, tau_A, tau_B, tau_C] - * - * F ... forces (in N) applied along the Cartesian axes of the - * currently used motion center. - * tau ... torques (in Nm) applied along the orientation angles - * (Euler angles A, B, C) of the currently used motion center. - * - * This method is only effective when the client is in a commanding state. - * The ControlMode of the robot has to be Cartesian impedance control mode. The - * Client Command Mode has to be wrench. - * - * @param wrench Applied Cartesian wrench vector. - */ - void setWrench(const double * wrench); - - /** - * \brief Set the applied joint torques for the current interpolation step. - * - * This method is only effective when the client is in a commanding state. - * The ControlMode of the robot has to be joint impedance control mode. The - * Client Command Mode has to be torque. - * - * @param torques Array with the applied torque values (in Nm) - */ - void setTorque(const double * torques); - - /** - * \brief Set boolean output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. - * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Boolean value to set. - */ - void setBooleanIOValue(const char * name, const bool value); - - /** - * \brief Set digital output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. - * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Digital value to set. - */ - void setDigitalIOValue(const char * name, const unsigned long long value); - - /** - * \brief Set analog output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. - * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Analog value to set. - */ - void setAnalogIOValue(const char * name, const double value); - -protected: - static const int LBRCOMMANDMESSAGEID = 0x34001; //!< type identifier for the FRI command message corresponding to a KUKA LBR robot - FRICommandMessage * _cmdMessage; //!< FRI command message (protobuf struct) - FRIMonitoringMessage * _monMessage; //!< FRI monitoring message (protobuf struct) - -}; - -} -} - - -#endif // _KUKA_FRI_LBR_COMMAND_H +/** + + The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + + \file + \version {2.5} + */ +#ifndef _KUKA_FRI_LBR_COMMAND_H +#define _KUKA_FRI_LBR_COMMAND_H + +#include + +// forward declarations +typedef struct _FRICommandMessage FRICommandMessage; + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + +/** + * \brief Wrapper class for the FRI command message for a KUKA LBR (leightweight) robot. + */ +class LBRCommand +{ + friend class LBRClient; + +public: + + /** + * \brief Set the joint positions for the current interpolation step. + * + * This method is only effective when the client is in a commanding state. + * @param values Array with the new joint positions (in rad) + */ + void setJointPosition(const double* values); + + /** + * \brief Set the applied wrench vector of the current interpolation step. + * + * The wrench vector consists of: + * [F_x, F_y, F_z, tau_A, tau_B, tau_C] + * + * F ... forces (in N) applied along the Cartesian axes of the + * currently used motion center. + * tau ... torques (in Nm) applied along the orientation angles + * (Euler angles A, B, C) of the currently used motion center. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be Cartesian impedance control mode. The + * Client Command Mode has to be wrench. + * + * @param wrench Applied Cartesian wrench vector. + */ + void setWrench(const double* wrench); + + /** + * \brief Set the applied joint torques for the current interpolation step. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be joint impedance control mode. The + * Client Command Mode has to be torque. + * + * @param torques Array with the applied torque values (in Nm) + */ + void setTorque(const double* torques); + + /** + * \brief Set the Cartesian pose for the current interpolation step. + * The pose describes the configured TCP relative to the configured base frame. + * + * The quaternion vector consists of: [t_x, t_y, t_z, q_w, q_x, q_y, q_z], + * where the first three values describe the translation t as a regular 3-dim + * vector, while the last four values describe the rotation q as an unit quaternion. + * + * The unit quaternion q is a 4-dimensional vector, describing the orientation as + * one rotation around the vector v = [v_x, v_y, v_z] about the angle phi. + * + * [ q_w ] = [ cos (phi/2) ] + * [ q_x ] = [ sin (phi/2) * v_x ] + * [ q_y ] = [ sin (phi/2) * v_y ] + * [ q_z ] = [ sin (phi/2) * v_z ] + * + * Setting a redundancy value is optional. If no value is provided, the interpolated + * redundancy value is used. So far, only the E1 redundancy strategy is provided. + * + * This method is only effective when the client is in a commanding state. + * + * @param cartesianPoseQuaternion Array with the new Cartesian pose + * @param redundancyValue pointer to redundancy value, NULL for default behavior + */ + void setCartesianPose(const double* cartesianPoseQuaternion, + double const * const redundancyValue = NULL); + + /** + * \brief Set the Cartesian pose for the current interpolation step as a 3x4 matrix. + * The pose describes the configured TCP relative to the configured base frame. + * + * The first 3 columns represent a rotational matrix and the 4th column a 3-dim + * translation vector for directions x, y, z (in mm). + * + * Setting a redundancy value is optional. If no value is provided, the interpolated + * redundancy value is used. So far, only the E1 redundancy strategy is provided. + * + * @param cartesianPoseAsMatrix 2-dim double array where the requested 3x4 matrix + * should be stored + * @param redundancyValue pointer to redundancy value, NULL for default behavior + */ + void setCartesianPoseAsMatrix(const double(&cartesianPoseAsMatrix)[3][4], + double const * const redundancyValue = NULL); + + /** + * \brief Set boolean output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Boolean value to set. + */ + void setBooleanIOValue(const char* name, const bool value); + + /** + * \brief Set digital output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Digital value to set. + */ + void setDigitalIOValue(const char* name, const unsigned long long value); + + /** + * \brief Set analog output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Analog value to set. + */ + void setAnalogIOValue(const char* name, const double value); + +protected: + + static const int LBRCOMMANDMESSAGEID = 0x34001; //!< type identifier for the FRI command message corresponding to a KUKA LBR robot + FRICommandMessage* _cmdMessage; //!< FRI command message (protobuf struct) + FRIMonitoringMessage* _monMessage; //!< FRI monitoring message (protobuf struct) + +}; + +} +} + +#endif // _KUKA_FRI_LBR_COMMAND_H diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRState.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRState.h index 8e25b443..c605c014 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRState.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRState.h @@ -1,276 +1,369 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#ifndef _KUKA_FRI_LBR_STATE_H -#define _KUKA_FRI_LBR_STATE_H - -#include - -// forward declarations -typedef struct _FRIMonitoringMessage FRIMonitoringMessage; - -/** Kuka namespace */ -namespace KUKA -{ -namespace FRI -{ - -/** - * \brief Wrapper class for the FRI monitoring message for a KUKA LBR (lightweight) robot. - */ -class LBRState -{ - friend class LBRClient; - -public: - enum - { - NUMBER_OF_JOINTS = 7 //!< number of axes of the KUKA LBR robot - }; - - LBRState(); - - /** - * \brief Get the sample time in seconds. - * - * This is the period in which the KUKA Sunrise controller is sending - * FRI packets. - * @return sample time in seconds - */ - double getSampleTime() const; // sec - - /** - * \brief Get the current FRI session state. - * - * @return current FRI session state - */ - ESessionState getSessionState() const; - - /** - * \brief Get the current FRI connection quality. - * - * @return current FRI connection quality - */ - EConnectionQuality getConnectionQuality() const; - - /** - * \brief Get the current safety state of the KUKA Sunrise controller. - * - * @return current safety state - */ - ESafetyState getSafetyState() const; - - /** - * \brief Get the current operation mode of the KUKA Sunrise controller. - * - * @return current operation mode - */ - EOperationMode getOperationMode() const; - - /** - * \brief Get the accumulated drive state over all drives of the KUKA LBR controller. - * - * If the drive states differ between drives, the following rule applies: - * 1) The drive state is OFF if all drives are OFF. - * 2) The drive state is ACTIVE if all drives are ACTIVE. - * 3) otherwise the drive state is TRANSITIONING. - * @return accumulated drive state - */ - EDriveState getDriveState() const; - - /** - * \brief Get the client command mode specified by the client. - * - * @return the client command mode specified by the client. - */ - EClientCommandMode getClientCommandMode() const; - - /** - * \brief Get the overlay type specified by the client. - * - * @return the overlay type specified by the client. - */ - EOverlayType getOverlayType() const; - - /** - * \brief Get the control mode of the KUKA LBR robot. - * - * @return current control mode of the KUKA LBR robot. - */ - EControlMode getControlMode() const; - - /** - * \brief Get the timestamp of the current robot state in Unix time. - * - * This method returns the seconds since 0:00, January 1st, 1970 (UTC). - * Use getTimestampNanoSec() to increase your timestamp resolution when - * seconds are insufficient. - * @return timestamp encoded as Unix time (seconds) - */ - unsigned int getTimestampSec() const; - - /** - * \brief Get the nanoseconds elapsed since the last second (in Unix time). - * - * This method complements getTimestampSec() to get a high resolution - * timestamp. - * @return timestamp encoded as Unix time (nanoseconds part) - */ - unsigned int getTimestampNanoSec() const; - - /** - * \brief Get the currently measured joint positions of the robot. - * - * @return array of the measured joint positions in radians - */ - const double * getMeasuredJointPosition() const; - - /** - * \brief Get the last commanded joint positions of the robot. - * - * @return array of the commanded joint positions in radians - */ - const double * getCommandedJointPosition() const; - - /** - * \brief Get the currently measured joint torques of the robot. - * - * @return array of the measured torques in Nm - */ - const double * getMeasuredTorque() const; - - /** - * \brief Get the last commanded joint torques of the robot. - * - * @return array of the commanded torques in Nm - */ - const double * getCommandedTorque() const; - - /** - * \brief Get the currently measured external joint torques of the robot. - * - * The external torques corresponds to the measured torques when removing - * the torques induced by the robot itself. - * @return array of the external torques in Nm - */ - const double * getExternalTorque() const; - - /** - * \brief Get the joint positions commanded by the interpolator. - * - * When commanding a motion overlay in your robot application, this method - * will give access to the joint positions currently commanded by the - * motion interpolator. - * @throw FRIException This method will throw an FRIException during monitoring mode. - * @return array of the ipo joint positions in radians - */ - const double * getIpoJointPosition() const; - - /** - * \brief Get an indicator for the current tracking performance of the commanded robot. - * - * The tracking performance is an indicator on how well the commanded robot - * is able to follow the commands of the FRI client. The best possible value - * 1.0 is reached when the robot executes the given commands instantaneously. - * The tracking performance drops towards 0 when latencies are induced, - * e.g. when the commanded velocity, acceleration or jerk exceeds the - * capabilities of the robot. - * The tracking performance will always be 0 when the session state does - * not equal COMMANDING_ACTIVE. - * @return current tracking performance - */ - double getTrackingPerformance() const; - - /** - * \brief Get boolean IO value. - * - * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @return Returns IO's boolean value. - */ - bool getBooleanIOValue(const char * name) const; - - /** - * \brief Get digital IO value. - * - * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @return Returns IO's digital value. - */ - unsigned long long getDigitalIOValue(const char * name) const; - - /** - * \brief Get analog IO value. - * - * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @return Returns IO's analog value. - */ - double getAnalogIOValue(const char * name) const; - -protected: - static const int LBRMONITORMESSAGEID = 0x245142; //!< type identifier for the FRI monitoring message corresponding to a KUKA LBR robot - FRIMonitoringMessage * _message; //!< FRI monitoring message (protobuf struct) -}; - -} -} - - -#endif // _KUKA_FRI_LBR_STATE_H +/** + + The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + + \file + \version {2.5} + */ +#ifndef _KUKA_FRI_LBR_STATE_H +#define _KUKA_FRI_LBR_STATE_H + +#include + +// forward declarations +typedef struct _FRIMonitoringMessage FRIMonitoringMessage; + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + +/** + * \brief Wrapper class for the FRI monitoring message for a KUKA LBR (lightweight) robot. + */ +class LBRState +{ + friend class LBRClient; + +public: + + enum + { + NUMBER_OF_JOINTS = 7 //!< number of joints of the KUKA LBR robot + }; + + LBRState(); + + /** + * \brief Get the sample time in seconds. + * + * This is the period in which the KUKA Sunrise controller is sending + * FRI packets. + * @return sample time in seconds + */ + double getSampleTime() const; // sec + + /** + * \brief Get the current FRI session state. + * + * @return current FRI session state + */ + ESessionState getSessionState() const; + + /** + * \brief Get the current FRI connection quality. + * + * @return current FRI connection quality + */ + EConnectionQuality getConnectionQuality() const; + + /** + * \brief Get the current safety state of the KUKA Sunrise controller. + * + * @return current safety state + */ + ESafetyState getSafetyState() const; + + /** + * \brief Get the current operation mode of the KUKA Sunrise controller. + * + * @return current operation mode + */ + EOperationMode getOperationMode() const; + + /** + * \brief Get the accumulated drive state over all drives of the KUKA LBR controller. + * + * If the drive states differ between drives, the following rule applies: + * 1) The drive state is OFF if all drives are OFF. + * 2) The drive state is ACTIVE if all drives are ACTIVE. + * 3) otherwise the drive state is TRANSITIONING. + * @return accumulated drive state + */ + EDriveState getDriveState() const; + + /** + * \brief Get the client command mode specified by the client. + * + * @return the client command mode specified by the client. + */ + EClientCommandMode getClientCommandMode() const; + + /** + * \brief Get the overlay type specified by the client. + * + * @return the overlay type specified by the client. + */ + EOverlayType getOverlayType() const; + + /** + * \brief Get the control mode of the KUKA LBR robot. + * + * @return current control mode of the KUKA LBR robot. + */ + EControlMode getControlMode() const; + + /** + * \brief Get the timestamp of the current robot state in Unix time. + * + * This method returns the seconds since 0:00, January 1st, 1970 (UTC). + * Use getTimestampNanoSec() to increase your timestamp resolution when + * seconds are insufficient. + * @return timestamp encoded as Unix time (seconds) + */ + unsigned int getTimestampSec() const; + + /** + * \brief Get the nanoseconds elapsed since the last second (in Unix time). + * + * This method complements getTimestampSec() to get a high resolution + * timestamp. + * @return timestamp encoded as Unix time (nanoseconds part) + */ + unsigned int getTimestampNanoSec() const; + + /** + * \brief Get the currently measured joint positions of the robot. + * + * @return array of the measured joint positions in radians + */ + const double* getMeasuredJointPosition() const; + + /** + * \brief Get the currently measured joint torques of the robot. + * + * @return array of the measured torques in Nm + */ + const double* getMeasuredTorque() const; + + /** + * \brief Get the last commanded joint torques of the robot. + * + * @return array of the commanded torques in Nm + */ + const double* getCommandedTorque() const; + + /** + * \brief Get the currently measured external joint torques of the robot. + * + * The external torques corresponds to the measured torques when removing + * the torques induced by the robot itself. + * @return array of the external torques in Nm + */ + const double* getExternalTorque() const; + + /** + * \brief Get the joint positions commanded by the interpolator. + * + * When commanding a motion overlay in your robot application, this method + * will give access to the joint positions currently commanded by the + * motion interpolator. + * @throw FRIException This method will throw a FRIException if no FRI Joint Overlay is active. + * @return array of the ipo joint positions in radians + */ + const double* getIpoJointPosition() const; + + /** + * \brief Get an indicator for the current tracking performance of the commanded robot. + * + * The tracking performance is an indicator on how well the commanded robot + * is able to follow the commands of the FRI client. The best possible value + * 1.0 is reached when the robot executes the given commands instantaneously. + * The tracking performance drops towards 0 when latencies are induced, + * e.g. when the commanded velocity, acceleration or jerk exceeds the + * capabilities of the robot. + * The tracking performance will always be 0 when the session state does + * not equal COMMANDING_ACTIVE. + * @return current tracking performance + */ + double getTrackingPerformance() const; + + /** + * \brief Get boolean IO value. + * + * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's boolean value. + */ + bool getBooleanIOValue(const char* name) const; + + /** + * \brief Get digital IO value. + * + * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's digital value. + */ + unsigned long long getDigitalIOValue(const char* name) const; + + /** + * \brief Get analog IO value. + * + * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's analog value. + */ + double getAnalogIOValue(const char* name) const; + + /** + * \brief Get the currently measured Cartesian pose of the robot as a quaternion + * transformation vector. The pose describes the robot tcp relative to the + * base frame. + * + * The quaternion transformation vector consists of: [t_x, t_y, t_z, q_w, q_x, q_y, q_z] + * + * , where the first three values describe the translation t as a regular 3-dim + * vector, while the last four values describe the rotation q as an unit quaternion. + * + * The unit quaternion q is a 4-dimensional vector, describing the orientation as + * one rotation around the vector v = [v_x, v_y, v_z] about the angle phi. + * + * [ q_w ] = [ cos (phi/2) ] + * [ q_x ] = [ sin (phi/2) * v_x ] + * [ q_y ] = [ sin (phi/2) * v_y ] + * [ q_z ] = [ sin (phi/2) * v_z ] + * + * @return measured Cartesian pose as 7-dim quaternion transformation (translation in mm) + */ + const double* getMeasuredCartesianPose() const; + + /** + * \brief Get the currently measured Cartesian pose of the robot as a 3x4 transformation matrix. + * The pose describes the robot tcp relative to the base frame. + * + * The first 3 columns represent a rotational matrix and the 4th column a 3-dim + * translation vector for directions x, y, z (in mm). + * + * @param measuredCartesianPose 2-dim double array where the requested 3x4 matrix + * should be stored + */ + void getMeasuredCartesianPoseAsMatrix(double(&measuredCartesianPose)[3][4]) const; + + /** + * \brief Get the currently interpolated Cartesian pose of the robot as a quaternion + * transformation vector. The pose describes the robot tcp relative to the + * base frame. + * + * The quaternion transformation vector consists of: [t_x, t_y, t_z, q_w, q_x, q_y, q_z] + * + * , where the first three values describe the translation t as a regular 3-dim + * vector, while the last four values describe the rotation q as an unit quaternion. + * + * The unit quaternion q is a 4-dimensional vector, describing the orientation as + * one rotation around the vector v = [v_x, v_y, v_z] about the angle phi. + * + * [ q_w ] = [ cos (phi/2) ] + * [ q_x ] = [ sin (phi/2) * v_x ] + * [ q_y ] = [ sin (phi/2) * v_y ] + * [ q_z ] = [ sin (phi/2) * v_z ] + * + * @throw FRIException This method will throw a FRIException if no FRI Cartesian Overlay is active. + * @return ipo Cartesian pose as 7-dim quaternion transformation (translation in mm) + */ + const double* getIpoCartesianPose() const; + + /** + * \brief Get the currently interpolated Cartesian pose of the robot as a 3x4 transformation matrix. + * The pose describes the robot tcp relative to the base frame. + * + * The first 3 columns represent a rotational matrix and the 4th column a 3-dim + * translation vector for directions x, y, z (in mm). + * + * @throw FRIException This method will throw a FRIException if no FRI Cartesian Overlay is active. + * @param ipoCartesianPose 2-dim double array where the requested 3x4 matrix should be stored + */ + void getIpoCartesianPoseAsMatrix(double(&ipoCartesianPose)[3][4]) const; + + /** + * \brief Get the currently measured redundancy value. + * + * So far, only the E1 redundancy strategy is provided, meaning the value represents the joint position + * of A3 in radians. + * + * @param measured redundancy value in radians + */ + double getMeasuredRedundancyValue() const; + + /** + * \brief Get the current redundancy value of the interpolator. + * + * So far, only the E1 redundancy strategy is provided, meaning the value represents the joint position + * of A3 in radians. + * + * @param redundancy value of the interpolator in radians + */ + double getIpoRedundancyValue() const; + + /** + * \brief Get the redundancy strategy. + * + * So far, only the E1 redundancy strategy is provided, meaning the value represents the joint position + * of A3 in radians. + * + * @param redundancy strategy + */ + ERedundancyStrategy getRedundancyStrategy() const; + +protected: + + static const int LBRMONITORMESSAGEID = 0x245142; //!< type identifier for the FRI monitoring message corresponding to a KUKA LBR robot + FRIMonitoringMessage* _message; //!< FRI monitoring message (protobuf struct) + +}; +} +} + +#endif // _KUKA_FRI_LBR_STATE_H diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friTransformationClient.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friTransformationClient.h index 74c1bf8b..c28a8ef2 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friTransformationClient.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friTransformationClient.h @@ -1,266 +1,292 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#ifndef _KUKA_FRI_TRANSFORMATION_CLIENT_H -#define _KUKA_FRI_TRANSFORMATION_CLIENT_H - -#include -#include - -/** Kuka namespace */ -namespace KUKA -{ -namespace FRI -{ - -// forward declaration -struct ClientData; - -/** - * \brief Abstract FRI transformation client. - * - * A transformation client enables the user to send transformation matrices cyclically to the - * KUKA Sunrise controller for manipulating the transformations of dynamic frames in the - * Sunrise scenegraph. - * Usually, these matrices will be provided by external sensors. - *
- * Custom transformation clients have to be derived from this class and need to - * implement the provide() callback. This callback is called once by the - * client application whenever a new FRI message arrives. - * - * This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions. - */ -class TransformationClient -{ - - friend class ClientApplication; - -public: - /** - *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
- * \brief Constructor. - **/ - TransformationClient(); - - /**
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
- * \brief Virtual destructor. - **/ - virtual ~TransformationClient(); - - /** - *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
- * \brief Callback which is called whenever a new FRI message arrives. - * - * In this callback all requested transformations have to be set. - * - * \see getRequestedTransformationIDs(), setTransformation() - */ - virtual void provide() = 0; - - /** - * \brief Get the sample time in seconds. - * - * This is the period in which the KUKA Sunrise controller is sending - * FRI packets. - * @return sample time in seconds - */ - double getSampleTime() const; // sec - - /** - * \brief Get the current FRI connection quality. - * - * @return current FRI connection quality - */ - EConnectionQuality getConnectionQuality() const; - - /** - *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
- * \brief Returns a vector of identifiers of all requested transformation matrices. - * - * The custom TransformationClient has to provide data for transformation matrices with these - * identifiers. - * - * @return reference to vector of IDs of requested transformations - */ - const std::vector & getRequestedTransformationIDs() const; - - /** - *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
- * - * \brief Get the timestamp of the current received FRI monitor message in Unix time. - * - * This method returns the seconds since 0:00, January 1st, 1970 (UTC). - * Use getTimestampNanoSec() to increase your timestamp resolution when - * seconds are insufficient. - * - * @return timestamp encoded as Unix time (seconds) - */ - unsigned int getTimestampSec() const; - - /** - *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
- * \brief Get the nanoseconds elapsed since the last second (in Unix time). - * - * This method complements getTimestampSec() to get a high resolution - * timestamp. - * - * @return timestamp encoded as Unix time (nanoseconds part) - */ - unsigned int getTimestampNanoSec() const; - - /** - *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
- * \brief Provides a requested transformation matrix. - * - * A transformation matrix has 3x4 elements. It consists of a rotational matrix (3x3 elements) - * and a translational vector (3x1 elements). The complete transformation matrix has the - * following structure:
- * [Transformation(3x4)] = [Rotation(3x3) | Translation(3x1) ] - *

- * All provided transformation matrices need a timestamp that corresponds to their - * time of acquisition. This timestamp must be synchronized to the timestamp - * provided by the KUKA Sunrise controller (see getTimestampSec(), getTimestampNanoSec()). - *

- * If an update to the last transformation is not yet available when the provide() - * callback is executed, the last transformation (including its timestamp) should be - * repeated until a new transformation is available. - * - * @throw FRIException Throws a FRIException if the maximum number of transformations is exceeded. - * @param transformationID Identifier string of the transformation matrix - * @param transformationMatrix Provided transformation matrix - * @param timeSec Timestamp encoded as Unix time (seconds) - * @param timeNanoSec Timestamp encoded as Unix time (nanoseconds part) - */ - void setTransformation( - const char * transformationID, const double transformationMatrix[3][4], - unsigned int timeSec, unsigned int timeNanoSec); - - /** - * \brief Set boolean output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. - * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Boolean value to set. - */ - void setBooleanIOValue(const char * name, const bool value); - - /** - * \brief Set digital output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. - * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Digital value to set. - */ - void setDigitalIOValue(const char * name, const unsigned long long value); - - /** - * \brief Set analog output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. - * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Analog value to set. - */ - void setAnalogIOValue(const char * name, const double value); - - /** - * \brief Get boolean IO value. - * - * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @return Returns IO's boolean value. - */ - bool getBooleanIOValue(const char * name) const; - - /** - * \brief Get digital IO value. - * - * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @return Returns IO's digital value. - */ - unsigned long long getDigitalIOValue(const char * name) const; - - /** - * \brief Get analog IO value. - * - * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @return Returns IO's analog value. - */ - double getAnalogIOValue(const char * name) const; - -private: - ClientData * _data; //!< the client data structure - - /** - * \brief Method to link the client data structure (used internally). - * - * @param clientData the client data structure - */ - void linkData(ClientData * clientData); - -}; - -} -} - -#endif // _KUKA_FRI_TRANSFORMATION_CLIENT_H +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_TRANSFORMATION_CLIENT_H +#define _KUKA_FRI_TRANSFORMATION_CLIENT_H + +#include +#include + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + // forward declaration + struct ClientData; + + /** + * \brief Abstract FRI transformation client. + * + * A transformation client enables the user to send transformation matrices cyclically to the + * KUKA Sunrise controller for manipulating the transformations of dynamic frames in the + * Sunrise scenegraph. + * Usually, these matrices will be provided by external sensors. + *
+ * Custom transformation clients have to be derived from this class and need to + * implement the provide() callback. This callback is called once by the + * client application whenever a new FRI message arrives. + * + */ + class TransformationClient + { + + friend class ClientApplication; + + public: + + /** + * \brief Constructor. + **/ + TransformationClient(); + + /** + * \brief Virtual destructor. + **/ + virtual ~TransformationClient(); + + /** + * \brief Callback which is called whenever a new FRI message arrives. + * + * In this callback all requested transformations have to be set. + * + * \see getRequestedTransformationIDs(), setTransformation() + */ + virtual void provide() = 0; + + /** + * \brief Get the sample time in seconds. + * + * This is the period in which the KUKA Sunrise controller is sending + * FRI packets. + * @return sample time in seconds + */ + double getSampleTime() const; // sec + + /** + * \brief Get the current FRI connection quality. + * + * @return current FRI connection quality + */ + EConnectionQuality getConnectionQuality() const; + + /** + * \brief Returns a vector of identifiers of all requested transformation matrices. + * + * The custom TransformationClient has to provide data for transformation matrices with these + * identifiers. + * + * @return reference to vector of IDs of requested transformations + */ + const std::vector& getRequestedTransformationIDs() const; + + /** + * \brief Get the timestamp of the current received FRI monitor message in Unix time. + * + * This method returns the seconds since 0:00, January 1st, 1970 (UTC). + * Use getTimestampNanoSec() to increase your timestamp resolution when + * seconds are insufficient. + * + * @return timestamp encoded as Unix time (seconds) + */ + const unsigned int getTimestampSec() const; + + /** + * \brief Get the nanoseconds elapsed since the last second (in Unix time). + * + * This method complements getTimestampSec() to get a high resolution + * timestamp. + * + * @return timestamp encoded as Unix time (nanoseconds part) + */ + const unsigned int getTimestampNanoSec() const; + + /** + * \brief Provides a requested transformation as a quaternion transformation vector. + * + * A quaternion transformation vector consists of: [t_x, t_y, t_z, q_w, q_x, q_y, q_z], + * where the first three values describe the translation t as a regular 3-dim + * vector, while the last four values describe the rotation q as an unit quaternion. + *

+ * The unit quaternion q is a 4-dimensional vector, describing the orientation as + * one rotation around the vector v = [v_x, v_y, v_z] about the angle phi. + *

+ * [ q_w ] = [ cos (phi/2) ] + * [ q_x ] = [ sin (phi/2) * v_x ] + * [ q_y ] = [ sin (phi/2) * v_y ] + * [ q_z ] = [ sin (phi/2) * v_z ] + *

+ * All provided transformation need a timestamp that corresponds to their + * time of acquisition. This timestamp must be synchronized to the timestamp + * provided by the KUKA Sunrise controller (see getTimestampSec(), getTimestampNanoSec()). + *

+ * If an update to the last transformation is not yet available when the provide() + * callback is executed, the last transformation (including its timestamp) should be + * repeated until a new transformation is available. + * + * @throw FRIException Throws a FRIException if the maximum number of transformations is exceeded. + * @param transformationID Identifier string of the transformation matrix + * @param transformationQuaternion Provided transformation quaternion + * @param timeSec Timestamp encoded as Unix time (seconds) + * @param timeNanoSec Timestamp encoded as Unix time (nanoseconds part) + */ + void setTransformation(const char* transformationID, const double (&transformationQuaternion)[7], + unsigned int timeSec, unsigned int timeNanoSec); + + /** + * \brief Provides a requested transformation as a homogeneous matrix. + * + * A transformation matrix has 3x4 elements. It consists of a rotational matrix (3x3 elements) + * and a translational vector (3x1 elements). The complete transformation matrix has the + * following structure:
+ * [Transformation(3x4)] = [Rotation(3x3) | Translation(3x1)] + *

+ * All provided transformation matrices need a timestamp that corresponds to their + * time of acquisition. This timestamp must be synchronized to the timestamp + * provided by the KUKA Sunrise controller (see getTimestampSec(), getTimestampNanoSec()). + *

+ * If an update to the last transformation is not yet available when the provide() + * callback is executed, the last transformation (including its timestamp) should be + * repeated until a new transformation is available. + *

+ * + * @throw FRIException Throws a FRIException if the maximum number of transformations is exceeded. + * @param transformationID Identifier string of the transformation matrix + * @param transformationMatrix Provided transformation matrix + * @param timeSec Timestamp encoded as Unix time (seconds) + * @param timeNanoSec Timestamp encoded as Unix time (nanoseconds part) + */ + void setTransformation(const char* transformationID, const double (&transformationMatrix)[3][4], + unsigned int timeSec, unsigned int timeNanoSec); + + /** + * \brief Set boolean output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Boolean value to set. + */ + void setBooleanIOValue(const char* name, const bool value); + + /** + * \brief Set digital output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Digital value to set. + */ + void setDigitalIOValue(const char* name, const unsigned long long value); + + /** + * \brief Set analog output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Analog value to set. + */ + void setAnalogIOValue(const char* name, const double value); + + /** + * \brief Get boolean IO value. + * + * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's boolean value. + */ + bool getBooleanIOValue(const char* name) const; + + /** + * \brief Get digital IO value. + * + * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's digital value. + */ + unsigned long long getDigitalIOValue(const char* name) const; + + /** + * \brief Get analog IO value. + * + * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's analog value. + */ + double getAnalogIOValue(const char* name) const; + + private: + + ClientData* _data; //!< the client data structure + + /** + * \brief Method to link the client data structure (used internally). + * + * @param clientData the client data structure + */ + void linkData(ClientData* clientData); + + }; + +} +} + +#endif // _KUKA_FRI_TRANSFORMATION_CLIENT_H diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friUdpConnection.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friUdpConnection.h index ff5835d7..04fa98d7 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friUdpConnection.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friUdpConnection.h @@ -1,161 +1,163 @@ -/** - -The following license terms and conditions apply, unless a redistribution -agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. - -SCOPE - -The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in -conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. -In the following, the term "software" refers to all material directly -belonging to the provided SDK "Software development kit", particularly source -code, libraries, binaries, manuals and technical documentation. - -COPYRIGHT - -All Rights Reserved -Copyright (C) 2014-2019 -KUKA Roboter GmbH -Augsburg, Germany - -LICENSE - -Redistribution and use of the software in source and binary forms, with or -without modification, are permitted provided that the following conditions are -met: -a) The software is used in conjunction with KUKA products only. -b) Redistributions of source code must retain the above copyright notice, this -list of conditions and the disclaimer. -c) Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the disclaimer in the documentation and/or other -materials provided with the distribution. Altered source code of the -redistribution must be made available upon request with the distribution. -d) Modification and contributions to the original software provided by KUKA -must be clearly marked and the authorship must be stated. -e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to -endorse or promote products derived from this software without specific prior -written permission. - -DISCLAIMER OF WARRANTY - -The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of -any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. -KUKA makes no warranty that the Software is free of defects or is suitable for -any particular purpose. In no event shall KUKA be responsible for loss or -damages arising from the installation or use of the Software, including but -not limited to any indirect, punitive, special, incidental or consequential -damages of any character including, without limitation, damages for loss of -goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. -The entire risk to the quality and performance of the Software is not borne by -KUKA. Should the Software prove defective, KUKA is not liable for the entire -cost of any service and repair. - - - -\file -\version {1.15} -*/ -#ifndef _KUKA_FRI_UDP_CONNECTION_H -#define _KUKA_FRI_UDP_CONNECTION_H - -#include - -#ifdef _WIN32 - #include -#else -// if linux or a other unix system is used, select uses the following include - #ifdef __unix__ - #include - #endif -// for VxWorks - #ifdef VXWORKS - #include - #include - #endif - #include - #include -#endif - -#include - -/** Kuka namespace */ -namespace KUKA -{ -namespace FRI -{ - -/** - * \brief This class implements the IConnection interface using UDP sockets. - */ -class UdpConnection : public IConnection -{ - -public: - /** - * \brief Constructor with an optional parameter for setting a receive timeout. - * - * @param receiveTimeout Timeout (in ms) for receiving a UDP message (0 = wait forever) - * */ - UdpConnection(unsigned int receiveTimeout = 0); - - /** \brief Destructor. */ - ~UdpConnection(); - - /** - * \brief Open a connection to the KUKA Sunrise controller. - * - * @param port The port ID for the connection - * @param controllerAddress The IPv4 address of the KUKA Sunrise controller. - * If NULL, the FRI Client accepts connections from any - * address. - * @return True if connection was established, false otherwise - */ - virtual bool open(int port, const char * controllerAddress = NULL); - - /** - * \brief Close a connection to the KUKA Sunrise controller. - */ - virtual void close(); - - /** - * \brief Checks whether a connection to the KUKA Sunrise controller is established. - * - * @return True if connection is established - */ - virtual bool isOpen() const; - - /** - * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. - * - * This method blocks until a new message arrives. - * @param buffer Pointer to the allocated buffer that will hold the FRI message - * @param maxSize Size in bytes of the allocated buffer - * @return Number of bytes received (0 when connection was terminated, - * negative in case of errors or receive timeout) - */ - virtual int receive(char * buffer, int maxSize); - - /** - * \brief Send a new FRI command message to the KUKA Sunrise controller. - * - * @param buffer Pointer to the buffer holding the FRI message - * @param size Size in bytes of the message to be send - * @return True if successful - */ - virtual bool send(const char * buffer, int size); - -private: - int _udpSock; //!< UDP socket handle - struct sockaddr_in _controllerAddr; //!< the controller's socket address - unsigned int _receiveTimeout; - fd_set _filedescriptor; - -}; - -} -} - - -#endif // _KUKA_FRI_UDP_CONNECTION_H +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_UDP_CONNECTION_H +#define _KUKA_FRI_UDP_CONNECTION_H + +#include + +#ifdef _WIN32 + #include +#else + // if linux or a other unix system is used, select uses the following include + #ifdef __unix__ + #include + #endif + // for VxWorks + #ifdef VXWORKS + #include + #include + #endif + #include + #include +#endif + +#include + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + /** + * \brief This class implements the IConnection interface using UDP sockets. + */ + class UdpConnection : public IConnection + { + + public: + + /** + * \brief Constructor with an optional parameter for setting a receive timeout. + * + * @param receiveTimeout Timeout (in ms) for receiving a UDP message (0 = wait forever) + * */ + UdpConnection(unsigned int receiveTimeout = 0); + + /** \brief Destructor. */ + ~UdpConnection(); + + /** + * \brief Open a connection to the KUKA Sunrise controller. + * + * @param port The port ID for the connection + * @param controllerAddress The IPv4 address of the KUKA Sunrise controller. + * If NULL, the FRI Client accepts connections from any + * address. + * @return True if connection was established, false otherwise + */ + virtual bool open(int port, const char *controllerAddress = NULL); + + /** + * \brief Close a connection to the KUKA Sunrise controller. + */ + virtual void close(); + + /** + * \brief Checks whether a connection to the KUKA Sunrise controller is established. + * + * @return True if connection is established + */ + virtual bool isOpen() const; + + /** + * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. + * + * This method blocks until a new message arrives. + * @param buffer Pointer to the allocated buffer that will hold the FRI message + * @param maxSize Size in bytes of the allocated buffer + * @return Number of bytes received (0 when connection was terminated, + * negative in case of errors or receive timeout) + */ + virtual int receive(char *buffer, int maxSize); + + /** + * \brief Send a new FRI command message to the KUKA Sunrise controller. + * + * @param buffer Pointer to the buffer holding the FRI message + * @param size Size in bytes of the message to be send + * @return True if successful + */ + virtual bool send(const char* buffer, int size); + + private: + + int _udpSock; //!< UDP socket handle + struct sockaddr_in _controllerAddr; //!< the controller's socket address + unsigned int _receiveTimeout; + fd_set _filedescriptor; + + }; + +} +} + + +#endif // _KUKA_FRI_UDP_CONNECTION_H From a21b35a99ae5c0a4aeb9387b77388e702b437a81 Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Wed, 28 Aug 2024 10:21:33 +0200 Subject: [PATCH 20/49] FRI msg generated with new nanopb, includes fixed --- .../src/fri_client_sdk/FRIMessages.pb.c | 238 ++-- .../src/fri_client_sdk/FRIMessages.pb.h | 1013 +++++++++++------ .../fri_client_sdk/friClientApplication.cpp | 20 +- .../src/fri_client_sdk/friClientData.h | 18 +- .../friCommandMessageEncoder.cpp | 12 +- .../fri_client_sdk/friCommandMessageEncoder.h | 12 +- .../src/fri_client_sdk/friDataHelper.cpp | 10 +- .../src/fri_client_sdk/friLBRClient.cpp | 14 +- .../src/fri_client_sdk/friLBRCommand.cpp | 18 +- .../src/fri_client_sdk/friLBRState.cpp | 16 +- .../friTransformationClient.cpp | 14 +- .../src/fri_client_sdk/friUdpConnection.cpp | 12 +- 12 files changed, 829 insertions(+), 568 deletions(-) diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.c b/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.c index 17ac32be..8786d741 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.c +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.c @@ -1,162 +1,76 @@ -/* Automatically generated nanopb constant definitions */ -/* Generated by nanopb-0.2.8 at Mon Jul 29 08:35:17 2019. */ - -#include "FRIMessages.pb.h" - - - -const pb_field_t JointValues_fields[2] = { - PB_FIELD2( 1, DOUBLE , REPEATED, CALLBACK, FIRST, JointValues, value, value, 0), - PB_LAST_FIELD -}; - -const pb_field_t TimeStamp_fields[3] = { - PB_FIELD2( 1, UINT32 , REQUIRED, STATIC , FIRST, TimeStamp, sec, sec, 0), - PB_FIELD2( 2, UINT32 , REQUIRED, STATIC , OTHER, TimeStamp, nanosec, sec, 0), - PB_LAST_FIELD -}; - -const pb_field_t CartesianVector_fields[2] = { - PB_FIELD2( 1, DOUBLE , REPEATED, STATIC , FIRST, CartesianVector, element, element, 0), - PB_LAST_FIELD -}; - -const pb_field_t Checksum_fields[2] = { - PB_FIELD2( 1, INT32 , OPTIONAL, STATIC , FIRST, Checksum, crc32, crc32, 0), - PB_LAST_FIELD -}; - -const pb_field_t QuaternionTransformation_fields[4] = { - PB_FIELD2( 1, STRING , REQUIRED, STATIC , FIRST, QuaternionTransformation, name, name, 0), - PB_FIELD2( 2, DOUBLE , REPEATED, STATIC , OTHER, QuaternionTransformation, element, name, 0), - PB_FIELD2( 3, MESSAGE , OPTIONAL, STATIC , OTHER, QuaternionTransformation, timestamp, element, &TimeStamp_fields), - PB_LAST_FIELD -}; - -const pb_field_t FriIOValue_fields[6] = { - PB_FIELD2( 1, STRING , REQUIRED, STATIC , FIRST, FriIOValue, name, name, 0), - PB_FIELD2( 2, ENUM , REQUIRED, STATIC , OTHER, FriIOValue, type, name, 0), - PB_FIELD2( 3, ENUM , REQUIRED, STATIC , OTHER, FriIOValue, direction, type, 0), - PB_FIELD2( 4, UINT64 , OPTIONAL, STATIC , OTHER, FriIOValue, digitalValue, direction, 0), - PB_FIELD2( 5, DOUBLE , OPTIONAL, STATIC , OTHER, FriIOValue, analogValue, digitalValue, 0), - PB_LAST_FIELD -}; - -const pb_field_t RedundancyInformation_fields[3] = { - PB_FIELD2( 1, ENUM , REQUIRED, STATIC , FIRST, RedundancyInformation, strategy, strategy, 0), - PB_FIELD2( 2, DOUBLE , REQUIRED, STATIC , OTHER, RedundancyInformation, value, strategy, 0), - PB_LAST_FIELD -}; - -const pb_field_t MessageHeader_fields[4] = { - PB_FIELD2( 1, UINT32 , REQUIRED, STATIC , FIRST, MessageHeader, messageIdentifier, messageIdentifier, 0), - PB_FIELD2( 2, UINT32 , REQUIRED, STATIC , OTHER, MessageHeader, sequenceCounter, messageIdentifier, 0), - PB_FIELD2( 3, UINT32 , REQUIRED, STATIC , OTHER, MessageHeader, reflectedSequenceCounter, sequenceCounter, 0), - PB_LAST_FIELD -}; - -const pb_field_t ConnectionInfo_fields[5] = { - PB_FIELD2( 1, ENUM , REQUIRED, STATIC , FIRST, ConnectionInfo, sessionState, sessionState, 0), - PB_FIELD2( 2, ENUM , REQUIRED, STATIC , OTHER, ConnectionInfo, quality, sessionState, 0), - PB_FIELD2( 3, UINT32 , OPTIONAL, STATIC , OTHER, ConnectionInfo, sendPeriod, quality, 0), - PB_FIELD2( 4, UINT32 , OPTIONAL, STATIC , OTHER, ConnectionInfo, receiveMultiplier, sendPeriod, 0), - PB_LAST_FIELD -}; - -const pb_field_t RobotInfo_fields[6] = { - PB_FIELD2( 1, INT32 , OPTIONAL, STATIC , FIRST, RobotInfo, numberOfJoints, numberOfJoints, 0), - PB_FIELD2( 2, ENUM , OPTIONAL, STATIC , OTHER, RobotInfo, safetyState, numberOfJoints, 0), - PB_FIELD2( 5, ENUM , REPEATED, CALLBACK, OTHER, RobotInfo, driveState, safetyState, 0), - PB_FIELD2( 6, ENUM , OPTIONAL, STATIC , OTHER, RobotInfo, operationMode, driveState, 0), - PB_FIELD2( 7, ENUM , OPTIONAL, STATIC , OTHER, RobotInfo, controlMode, operationMode, 0), - PB_LAST_FIELD -}; - -const pb_field_t MessageMonitorData_fields[9] = { - PB_FIELD2( 1, MESSAGE , OPTIONAL, STATIC , FIRST, MessageMonitorData, measuredJointPosition, measuredJointPosition, &JointValues_fields), - PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, measuredTorque, measuredJointPosition, &JointValues_fields), - PB_FIELD2( 4, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, commandedTorque, measuredTorque, &JointValues_fields), - PB_FIELD2( 5, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, externalTorque, commandedTorque, &JointValues_fields), - PB_FIELD2( 8, MESSAGE , REPEATED, STATIC , OTHER, MessageMonitorData, readIORequest, externalTorque, &FriIOValue_fields), - PB_FIELD2( 9, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, measuredCartesianPose, readIORequest, &QuaternionTransformation_fields), - PB_FIELD2( 10, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, measuredRedundancyInformation, measuredCartesianPose, &RedundancyInformation_fields), - PB_FIELD2( 15, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, timestamp, measuredRedundancyInformation, &TimeStamp_fields), - PB_LAST_FIELD -}; - -const pb_field_t MessageIpoData_fields[7] = { - PB_FIELD2( 1, MESSAGE , OPTIONAL, STATIC , FIRST, MessageIpoData, jointPosition, jointPosition, &JointValues_fields), - PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, MessageIpoData, cartesianPose, jointPosition, &QuaternionTransformation_fields), - PB_FIELD2( 3, MESSAGE , OPTIONAL, STATIC , OTHER, MessageIpoData, redundancyInformation, cartesianPose, &RedundancyInformation_fields), - PB_FIELD2( 10, ENUM , OPTIONAL, STATIC , OTHER, MessageIpoData, clientCommandMode, redundancyInformation, 0), - PB_FIELD2( 11, ENUM , OPTIONAL, STATIC , OTHER, MessageIpoData, overlayType, clientCommandMode, 0), - PB_FIELD2( 12, DOUBLE , OPTIONAL, STATIC , OTHER, MessageIpoData, trackingPerformance, overlayType, 0), - PB_LAST_FIELD -}; - -const pb_field_t MessageCommandData_fields[8] = { - PB_FIELD2( 1, MESSAGE , OPTIONAL, STATIC , FIRST, MessageCommandData, jointPosition, jointPosition, &JointValues_fields), - PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, MessageCommandData, cartesianWrenchFeedForward, jointPosition, &CartesianVector_fields), - PB_FIELD2( 3, MESSAGE , OPTIONAL, STATIC , OTHER, MessageCommandData, jointTorque, cartesianWrenchFeedForward, &JointValues_fields), - PB_FIELD2( 4, MESSAGE , REPEATED, STATIC , OTHER, MessageCommandData, commandedTransformations, jointTorque, &QuaternionTransformation_fields), - PB_FIELD2( 5, MESSAGE , REPEATED, STATIC , OTHER, MessageCommandData, writeIORequest, commandedTransformations, &FriIOValue_fields), - PB_FIELD2( 6, MESSAGE , OPTIONAL, STATIC , OTHER, MessageCommandData, cartesianPose, writeIORequest, &QuaternionTransformation_fields), - PB_FIELD2( 7, MESSAGE , OPTIONAL, STATIC , OTHER, MessageCommandData, redundancyInformation, cartesianPose, &RedundancyInformation_fields), - PB_LAST_FIELD -}; - -const pb_field_t MessageEndOf_fields[3] = { - PB_FIELD2( 1, INT32 , OPTIONAL, STATIC , FIRST, MessageEndOf, messageLength, messageLength, 0), - PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, MessageEndOf, messageChecksum, messageLength, &Checksum_fields), - PB_LAST_FIELD -}; - -const pb_field_t FRIMonitoringMessage_fields[8] = { - PB_FIELD2( 1, MESSAGE , REQUIRED, STATIC , FIRST, FRIMonitoringMessage, header, header, &MessageHeader_fields), - PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, FRIMonitoringMessage, robotInfo, header, &RobotInfo_fields), - PB_FIELD2( 3, MESSAGE , OPTIONAL, STATIC , OTHER, FRIMonitoringMessage, monitorData, robotInfo, &MessageMonitorData_fields), - PB_FIELD2( 4, MESSAGE , OPTIONAL, STATIC , OTHER, FRIMonitoringMessage, connectionInfo, monitorData, &ConnectionInfo_fields), - PB_FIELD2( 5, MESSAGE , OPTIONAL, STATIC , OTHER, FRIMonitoringMessage, ipoData, connectionInfo, &MessageIpoData_fields), - PB_FIELD2( 6, MESSAGE , REPEATED, STATIC , OTHER, FRIMonitoringMessage, requestedTransformations, ipoData, &QuaternionTransformation_fields), - PB_FIELD2( 15, MESSAGE , OPTIONAL, STATIC , OTHER, FRIMonitoringMessage, endOfMessageData, requestedTransformations, &MessageEndOf_fields), - PB_LAST_FIELD -}; - -const pb_field_t FRICommandMessage_fields[4] = { - PB_FIELD2( 1, MESSAGE , REQUIRED, STATIC , FIRST, FRICommandMessage, header, header, &MessageHeader_fields), - PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, FRICommandMessage, commandData, header, &MessageCommandData_fields), - PB_FIELD2( 15, MESSAGE , OPTIONAL, STATIC , OTHER, FRICommandMessage, endOfMessageData, commandData, &MessageEndOf_fields), - PB_LAST_FIELD -}; - - -/* Check that field information fits in pb_field_t */ -#if !defined(PB_FIELD_32BIT) -/* If you get an error here, it means that you need to define PB_FIELD_32BIT - * compile-time option. You can do that in pb.h or on compiler command line. - * - * The reason you need to do this is that some of your messages contain tag - * numbers or field sizes that are larger than what can fit in 8 or 16 bit - * field descriptors. - */ -STATIC_ASSERT((pb_membersize(QuaternionTransformation, timestamp) < 65536 && pb_membersize(MessageMonitorData, measuredJointPosition) < 65536 && pb_membersize(MessageMonitorData, measuredTorque) < 65536 && pb_membersize(MessageMonitorData, commandedTorque) < 65536 && pb_membersize(MessageMonitorData, externalTorque) < 65536 && pb_membersize(MessageMonitorData, readIORequest[0]) < 65536 && pb_membersize(MessageMonitorData, measuredCartesianPose) < 65536 && pb_membersize(MessageMonitorData, measuredRedundancyInformation) < 65536 && pb_membersize(MessageMonitorData, timestamp) < 65536 && pb_membersize(MessageIpoData, jointPosition) < 65536 && pb_membersize(MessageIpoData, cartesianPose) < 65536 && pb_membersize(MessageIpoData, redundancyInformation) < 65536 && pb_membersize(MessageCommandData, jointPosition) < 65536 && pb_membersize(MessageCommandData, cartesianWrenchFeedForward) < 65536 && pb_membersize(MessageCommandData, jointTorque) < 65536 && pb_membersize(MessageCommandData, commandedTransformations[0]) < 65536 && pb_membersize(MessageCommandData, writeIORequest[0]) < 65536 && pb_membersize(MessageCommandData, cartesianPose) < 65536 && pb_membersize(MessageCommandData, redundancyInformation) < 65536 && pb_membersize(MessageEndOf, messageChecksum) < 65536 && pb_membersize(FRIMonitoringMessage, header) < 65536 && pb_membersize(FRIMonitoringMessage, connectionInfo) < 65536 && pb_membersize(FRIMonitoringMessage, robotInfo) < 65536 && pb_membersize(FRIMonitoringMessage, monitorData) < 65536 && pb_membersize(FRIMonitoringMessage, ipoData) < 65536 && pb_membersize(FRIMonitoringMessage, requestedTransformations[0]) < 65536 && pb_membersize(FRIMonitoringMessage, endOfMessageData) < 65536 && pb_membersize(FRICommandMessage, header) < 65536 && pb_membersize(FRICommandMessage, commandData) < 65536 && pb_membersize(FRICommandMessage, endOfMessageData) < 65536), YOU_MUST_DEFINE_PB_FIELD_32BIT_FOR_MESSAGES_JointValues_TimeStamp_CartesianVector_Checksum_QuaternionTransformation_FriIOValue_RedundancyInformation_MessageHeader_ConnectionInfo_RobotInfo_MessageMonitorData_MessageIpoData_MessageCommandData_MessageEndOf_FRIMonitoringMessage_FRICommandMessage) -#endif - -#if !defined(PB_FIELD_16BIT) && !defined(PB_FIELD_32BIT) -/* If you get an error here, it means that you need to define PB_FIELD_16BIT - * compile-time option. You can do that in pb.h or on compiler command line. - * - * The reason you need to do this is that some of your messages contain tag - * numbers or field sizes that are larger than what can fit in the default - * 8 bit descriptors. - */ -STATIC_ASSERT((pb_membersize(QuaternionTransformation, timestamp) < 256 && pb_membersize(MessageMonitorData, measuredJointPosition) < 256 && pb_membersize(MessageMonitorData, measuredTorque) < 256 && pb_membersize(MessageMonitorData, commandedTorque) < 256 && pb_membersize(MessageMonitorData, externalTorque) < 256 && pb_membersize(MessageMonitorData, readIORequest[0]) < 256 && pb_membersize(MessageMonitorData, measuredCartesianPose) < 256 && pb_membersize(MessageMonitorData, measuredRedundancyInformation) < 256 && pb_membersize(MessageMonitorData, timestamp) < 256 && pb_membersize(MessageIpoData, jointPosition) < 256 && pb_membersize(MessageIpoData, cartesianPose) < 256 && pb_membersize(MessageIpoData, redundancyInformation) < 256 && pb_membersize(MessageCommandData, jointPosition) < 256 && pb_membersize(MessageCommandData, cartesianWrenchFeedForward) < 256 && pb_membersize(MessageCommandData, jointTorque) < 256 && pb_membersize(MessageCommandData, commandedTransformations[0]) < 256 && pb_membersize(MessageCommandData, writeIORequest[0]) < 256 && pb_membersize(MessageCommandData, cartesianPose) < 256 && pb_membersize(MessageCommandData, redundancyInformation) < 256 && pb_membersize(MessageEndOf, messageChecksum) < 256 && pb_membersize(FRIMonitoringMessage, header) < 256 && pb_membersize(FRIMonitoringMessage, connectionInfo) < 256 && pb_membersize(FRIMonitoringMessage, robotInfo) < 256 && pb_membersize(FRIMonitoringMessage, monitorData) < 256 && pb_membersize(FRIMonitoringMessage, ipoData) < 256 && pb_membersize(FRIMonitoringMessage, requestedTransformations[0]) < 256 && pb_membersize(FRIMonitoringMessage, endOfMessageData) < 256 && pb_membersize(FRICommandMessage, header) < 256 && pb_membersize(FRICommandMessage, commandData) < 256 && pb_membersize(FRICommandMessage, endOfMessageData) < 256), YOU_MUST_DEFINE_PB_FIELD_16BIT_FOR_MESSAGES_JointValues_TimeStamp_CartesianVector_Checksum_QuaternionTransformation_FriIOValue_RedundancyInformation_MessageHeader_ConnectionInfo_RobotInfo_MessageMonitorData_MessageIpoData_MessageCommandData_MessageEndOf_FRIMonitoringMessage_FRICommandMessage) -#endif - - -/* On some platforms (such as AVR), double is really float. - * These are not directly supported by nanopb, but see example_avr_double. - * To get rid of this error, remove any double fields from your .proto. - */ -STATIC_ASSERT(sizeof(double) == 8, DOUBLE_MUST_BE_8_BYTES) - +/* Automatically generated nanopb constant definitions */ +/* Generated by nanopb-0.4.8 */ + +#include "FRIMessages.pb.h" +#if PB_PROTO_HEADER_VERSION != 40 +#error Regenerate this file with the current version of nanopb generator. +#endif + +PB_BIND(JointValues, JointValues, AUTO) + + +PB_BIND(TimeStamp, TimeStamp, AUTO) + + +PB_BIND(CartesianVector, CartesianVector, AUTO) + + +PB_BIND(Checksum, Checksum, AUTO) + + +PB_BIND(QuaternionTransformation, QuaternionTransformation, AUTO) + + +PB_BIND(FriIOValue, FriIOValue, AUTO) + + +PB_BIND(RedundancyInformation, RedundancyInformation, AUTO) + + +PB_BIND(MessageHeader, MessageHeader, AUTO) + + +PB_BIND(ConnectionInfo, ConnectionInfo, AUTO) + + +PB_BIND(RobotInfo, RobotInfo, AUTO) + + +PB_BIND(MessageMonitorData, MessageMonitorData, 2) + + +PB_BIND(MessageIpoData, MessageIpoData, AUTO) + + +PB_BIND(MessageCommandData, MessageCommandData, 2) + + +PB_BIND(MessageEndOf, MessageEndOf, AUTO) + + +PB_BIND(FRIMonitoringMessage, FRIMonitoringMessage, 2) + + +PB_BIND(FRICommandMessage, FRICommandMessage, 2) + + + + + + + + + + + + + + +#ifndef PB_CONVERT_DOUBLE_FLOAT +/* On some platforms (such as AVR), double is really float. + * To be able to encode/decode double on these platforms, you need. + * to define PB_CONVERT_DOUBLE_FLOAT in pb.h or compiler command line. + */ +PB_STATIC_ASSERT(sizeof(double) == 8, DOUBLE_MUST_BE_8_BYTES) +#endif + diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.h b/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.h index 052b4330..d124ca26 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.h +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.h @@ -1,333 +1,680 @@ -/* Automatically generated nanopb header */ -/* Generated by nanopb-0.2.8 at Mon Jul 29 08:35:17 2019. */ - -#ifndef _PB_FRIMESSAGES_PB_H_ -#define _PB_FRIMESSAGES_PB_H_ -#include - -#ifdef __cplusplus -extern "C" { -#endif - -/* Enum definitions */ -typedef enum _FRISessionState { - FRISessionState_IDLE = 0, - FRISessionState_MONITORING_WAIT = 1, - FRISessionState_MONITORING_READY = 2, - FRISessionState_COMMANDING_WAIT = 3, - FRISessionState_COMMANDING_ACTIVE = 4 -} FRISessionState; - -typedef enum _FRIConnectionQuality { - FRIConnectionQuality_POOR = 0, - FRIConnectionQuality_FAIR = 1, - FRIConnectionQuality_GOOD = 2, - FRIConnectionQuality_EXCELLENT = 3 -} FRIConnectionQuality; - -typedef enum _SafetyState { - SafetyState_NORMAL_OPERATION = 0, - SafetyState_SAFETY_STOP_LEVEL_0 = 1, - SafetyState_SAFETY_STOP_LEVEL_1 = 2, - SafetyState_SAFETY_STOP_LEVEL_2 = 3 -} SafetyState; - -typedef enum _OperationMode { - OperationMode_TEST_MODE_1 = 0, - OperationMode_TEST_MODE_2 = 1, - OperationMode_AUTOMATIC_MODE = 2 -} OperationMode; - -typedef enum _DriveState { - DriveState_OFF = 0, - DriveState_TRANSITIONING = 1, - DriveState_ACTIVE = 2 -} DriveState; - -typedef enum _ControlMode { - ControlMode_POSITION_CONTROLMODE = 0, - ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE = 1, - ControlMode_JOINT_IMPEDANCE_CONTROLMODE = 2, - ControlMode_NO_CONTROLMODE = 3 -} ControlMode; - -typedef enum _ClientCommandMode { - ClientCommandMode_NO_COMMAND_MODE = 0, - ClientCommandMode_JOINT_POSITION = 1, - ClientCommandMode_WRENCH = 2, - ClientCommandMode_TORQUE = 3, - ClientCommandMode_CARTESIAN_POSE = 4 -} ClientCommandMode; - -typedef enum _OverlayType { - OverlayType_NO_OVERLAY = 0, - OverlayType_JOINT = 1, - OverlayType_CARTESIAN = 2 -} OverlayType; - -typedef enum _FriIOType { - FriIOType_BOOLEAN = 0, - FriIOType_DIGITAL = 1, - FriIOType_ANALOG = 2 -} FriIOType; - -typedef enum _FriIODirection { - FriIODirection_INPUT = 0, - FriIODirection_OUTPUT = 1 -} FriIODirection; - -typedef enum _RedundancyStrategy { - RedundancyStrategy_E1 = 0, - RedundancyStrategy_NONE = 4 -} RedundancyStrategy; - -/* Struct definitions */ -typedef struct _CartesianVector { - size_t element_count; - double element[6]; -} CartesianVector; - -typedef struct _Checksum { - bool has_crc32; - int32_t crc32; -} Checksum; - -typedef struct _ConnectionInfo { - FRISessionState sessionState; - FRIConnectionQuality quality; - bool has_sendPeriod; - uint32_t sendPeriod; - bool has_receiveMultiplier; - uint32_t receiveMultiplier; -} ConnectionInfo; - -typedef struct _FriIOValue { - char name[64]; - FriIOType type; - FriIODirection direction; - bool has_digitalValue; - uint64_t digitalValue; - bool has_analogValue; - double analogValue; -} FriIOValue; - -typedef struct _JointValues { - pb_callback_t value; -} JointValues; - -typedef struct _MessageHeader { - uint32_t messageIdentifier; - uint32_t sequenceCounter; - uint32_t reflectedSequenceCounter; -} MessageHeader; - -typedef struct _RedundancyInformation { - RedundancyStrategy strategy; - double value; -} RedundancyInformation; - -typedef struct _RobotInfo { - bool has_numberOfJoints; - int32_t numberOfJoints; - bool has_safetyState; - SafetyState safetyState; - pb_callback_t driveState; - bool has_operationMode; - OperationMode operationMode; - bool has_controlMode; - ControlMode controlMode; -} RobotInfo; - -typedef struct _TimeStamp { - uint32_t sec; - uint32_t nanosec; -} TimeStamp; - -typedef struct _MessageEndOf { - bool has_messageLength; - int32_t messageLength; - bool has_messageChecksum; - Checksum messageChecksum; -} MessageEndOf; - -typedef struct _QuaternionTransformation { - char name[64]; - size_t element_count; - double element[7]; - bool has_timestamp; - TimeStamp timestamp; -} QuaternionTransformation; - -typedef struct _MessageCommandData { - bool has_jointPosition; - JointValues jointPosition; - bool has_cartesianWrenchFeedForward; - CartesianVector cartesianWrenchFeedForward; - bool has_jointTorque; - JointValues jointTorque; - size_t commandedTransformations_count; - QuaternionTransformation commandedTransformations[5]; - size_t writeIORequest_count; - FriIOValue writeIORequest[10]; - bool has_cartesianPose; - QuaternionTransformation cartesianPose; - bool has_redundancyInformation; - RedundancyInformation redundancyInformation; -} MessageCommandData; - -typedef struct _MessageIpoData { - bool has_jointPosition; - JointValues jointPosition; - bool has_cartesianPose; - QuaternionTransformation cartesianPose; - bool has_redundancyInformation; - RedundancyInformation redundancyInformation; - bool has_clientCommandMode; - ClientCommandMode clientCommandMode; - bool has_overlayType; - OverlayType overlayType; - bool has_trackingPerformance; - double trackingPerformance; -} MessageIpoData; - -typedef struct _MessageMonitorData { - bool has_measuredJointPosition; - JointValues measuredJointPosition; - bool has_measuredTorque; - JointValues measuredTorque; - bool has_commandedTorque; - JointValues commandedTorque; - bool has_externalTorque; - JointValues externalTorque; - size_t readIORequest_count; - FriIOValue readIORequest[10]; - bool has_measuredCartesianPose; - QuaternionTransformation measuredCartesianPose; - bool has_measuredRedundancyInformation; - RedundancyInformation measuredRedundancyInformation; - bool has_timestamp; - TimeStamp timestamp; -} MessageMonitorData; - -typedef struct _FRICommandMessage { - MessageHeader header; - bool has_commandData; - MessageCommandData commandData; - bool has_endOfMessageData; - MessageEndOf endOfMessageData; -} FRICommandMessage; - -typedef struct _FRIMonitoringMessage { - MessageHeader header; - bool has_robotInfo; - RobotInfo robotInfo; - bool has_monitorData; - MessageMonitorData monitorData; - bool has_connectionInfo; - ConnectionInfo connectionInfo; - bool has_ipoData; - MessageIpoData ipoData; - size_t requestedTransformations_count; - QuaternionTransformation requestedTransformations[5]; - bool has_endOfMessageData; - MessageEndOf endOfMessageData; -} FRIMonitoringMessage; - -/* Default values for struct fields */ - -/* Field tags (for use in manual encoding/decoding) */ -#define CartesianVector_element_tag 1 -#define Checksum_crc32_tag 1 -#define ConnectionInfo_sessionState_tag 1 -#define ConnectionInfo_quality_tag 2 -#define ConnectionInfo_sendPeriod_tag 3 -#define ConnectionInfo_receiveMultiplier_tag 4 -#define FriIOValue_name_tag 1 -#define FriIOValue_type_tag 2 -#define FriIOValue_direction_tag 3 -#define FriIOValue_digitalValue_tag 4 -#define FriIOValue_analogValue_tag 5 -#define JointValues_value_tag 1 -#define MessageHeader_messageIdentifier_tag 1 -#define MessageHeader_sequenceCounter_tag 2 -#define MessageHeader_reflectedSequenceCounter_tag 3 -#define RedundancyInformation_strategy_tag 1 -#define RedundancyInformation_value_tag 2 -#define RobotInfo_numberOfJoints_tag 1 -#define RobotInfo_safetyState_tag 2 -#define RobotInfo_driveState_tag 5 -#define RobotInfo_operationMode_tag 6 -#define RobotInfo_controlMode_tag 7 -#define TimeStamp_sec_tag 1 -#define TimeStamp_nanosec_tag 2 -#define MessageEndOf_messageLength_tag 1 -#define MessageEndOf_messageChecksum_tag 2 -#define QuaternionTransformation_name_tag 1 -#define QuaternionTransformation_element_tag 2 -#define QuaternionTransformation_timestamp_tag 3 -#define MessageCommandData_jointPosition_tag 1 -#define MessageCommandData_cartesianWrenchFeedForward_tag 2 -#define MessageCommandData_jointTorque_tag 3 -#define MessageCommandData_commandedTransformations_tag 4 -#define MessageCommandData_writeIORequest_tag 5 -#define MessageCommandData_cartesianPose_tag 6 -#define MessageCommandData_redundancyInformation_tag 7 -#define MessageIpoData_jointPosition_tag 1 -#define MessageIpoData_cartesianPose_tag 2 -#define MessageIpoData_redundancyInformation_tag 3 -#define MessageIpoData_clientCommandMode_tag 10 -#define MessageIpoData_overlayType_tag 11 -#define MessageIpoData_trackingPerformance_tag 12 -#define MessageMonitorData_measuredJointPosition_tag 1 -#define MessageMonitorData_measuredTorque_tag 2 -#define MessageMonitorData_commandedTorque_tag 4 -#define MessageMonitorData_externalTorque_tag 5 -#define MessageMonitorData_readIORequest_tag 8 -#define MessageMonitorData_measuredCartesianPose_tag 9 -#define MessageMonitorData_measuredRedundancyInformation_tag 10 -#define MessageMonitorData_timestamp_tag 15 -#define FRICommandMessage_header_tag 1 -#define FRICommandMessage_commandData_tag 2 -#define FRICommandMessage_endOfMessageData_tag 15 -#define FRIMonitoringMessage_header_tag 1 -#define FRIMonitoringMessage_connectionInfo_tag 4 -#define FRIMonitoringMessage_robotInfo_tag 2 -#define FRIMonitoringMessage_monitorData_tag 3 -#define FRIMonitoringMessage_ipoData_tag 5 -#define FRIMonitoringMessage_requestedTransformations_tag 6 -#define FRIMonitoringMessage_endOfMessageData_tag 15 - -/* Struct field encoding specification for nanopb */ -extern const pb_field_t JointValues_fields[2]; -extern const pb_field_t TimeStamp_fields[3]; -extern const pb_field_t CartesianVector_fields[2]; -extern const pb_field_t Checksum_fields[2]; -extern const pb_field_t QuaternionTransformation_fields[4]; -extern const pb_field_t FriIOValue_fields[6]; -extern const pb_field_t RedundancyInformation_fields[3]; -extern const pb_field_t MessageHeader_fields[4]; -extern const pb_field_t ConnectionInfo_fields[5]; -extern const pb_field_t RobotInfo_fields[6]; -extern const pb_field_t MessageMonitorData_fields[9]; -extern const pb_field_t MessageIpoData_fields[7]; -extern const pb_field_t MessageCommandData_fields[8]; -extern const pb_field_t MessageEndOf_fields[3]; -extern const pb_field_t FRIMonitoringMessage_fields[8]; -extern const pb_field_t FRICommandMessage_fields[4]; - -/* Maximum encoded size of messages (where known) */ -#define TimeStamp_size 12 -#define CartesianVector_size 54 -#define Checksum_size 11 -#define QuaternionTransformation_size 143 -#define FriIOValue_size 98 -#define RedundancyInformation_size 15 -#define MessageHeader_size 18 -#define ConnectionInfo_size 24 -#define MessageEndOf_size 24 - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif +/* Automatically generated nanopb header */ +/* Generated by nanopb-0.4.8 */ + +#ifndef PB_FRIMESSAGES_PB_H_INCLUDED +#define PB_FRIMESSAGES_PB_H_INCLUDED +#include + +#if PB_PROTO_HEADER_VERSION != 40 +#error Regenerate this file with the current version of nanopb generator. +#endif + +/* Enum definitions */ +/* used to display the state of the FRI session + @KUKA.Internal */ +typedef enum _FRISessionState { + FRISessionState_IDLE = 0, /* idle state. */ + FRISessionState_MONITORING_WAIT = 1, /* connected in monitormode - transmitted commands are used to determine the connection quality. */ + FRISessionState_MONITORING_READY = 2, /* connected in monitormode - transmitted commands are used to determine the connection quality. */ + FRISessionState_COMMANDING_WAIT = 3, /* connected to a queued motion - motion not active */ + FRISessionState_COMMANDING_ACTIVE = 4 /* connected to a running motion - commanding modifies the robot path */ +} FRISessionState; + +/* Quality of the FRI Connection + @KUKA.Internal */ +typedef enum _FRIConnectionQuality { + FRIConnectionQuality_POOR = 0, /* Robot commanding is not possible. Initial value of the connection. */ + FRIConnectionQuality_FAIR = 1, /* Robot commanding is not possible. */ + FRIConnectionQuality_GOOD = 2, /* Robot commanding is possible. */ + FRIConnectionQuality_EXCELLENT = 3 /* Robot commanding is possible. */ +} FRIConnectionQuality; + +/* This enumeration is used to indicate the safety State. + @KUKA.Internal */ +typedef enum _SafetyState { + SafetyState_NORMAL_OPERATION = 0, + SafetyState_SAFETY_STOP_LEVEL_0 = 1, + SafetyState_SAFETY_STOP_LEVEL_1 = 2, + SafetyState_SAFETY_STOP_LEVEL_2 = 3 +} SafetyState; + +/* This enumeration contains the available operation modes of the controller. + @KUKA.Internal */ +typedef enum _OperationMode { + OperationMode_TEST_MODE_1 = 0, + OperationMode_TEST_MODE_2 = 1, + OperationMode_AUTOMATIC_MODE = 2 +} OperationMode; + +/* @KUKA.Internal */ +typedef enum _DriveState { + DriveState_OFF = 0, + DriveState_TRANSITIONING = 1, + DriveState_ACTIVE = 2 +} DriveState; + +/* This enumeration contains the available control modes of a KUKA robot. + @KUKA.Internal */ +typedef enum _ControlMode { + ControlMode_POSITION_CONTROLMODE = 0, + ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE = 1, + ControlMode_JOINT_IMPEDANCE_CONTROLMODE = 2, + ControlMode_NO_CONTROLMODE = 3 +} ControlMode; + +/* This enumeration contains the available client command modes of the interpolator. + @KUKA.Internal */ +typedef enum _ClientCommandMode { + ClientCommandMode_NO_COMMAND_MODE = 0, + ClientCommandMode_JOINT_POSITION = 1, + ClientCommandMode_WRENCH = 2, + ClientCommandMode_TORQUE = 3, + ClientCommandMode_CARTESIAN_POSE = 4 +} ClientCommandMode; + +/* This enumeration contains the currently used overlay type. + @KUKA.Internal */ +typedef enum _OverlayType { + OverlayType_NO_OVERLAY = 0, + OverlayType_JOINT = 1, + OverlayType_CARTESIAN = 2 +} OverlayType; + +/* IO Type + @KUKA.Internal */ +typedef enum _FriIOType { + FriIOType_BOOLEAN = 0, + FriIOType_DIGITAL = 1, + FriIOType_ANALOG = 2 +} FriIOType; + +/* IO Direction + @KUKA.Internal */ +typedef enum _FriIODirection { + FriIODirection_INPUT = 0, + FriIODirection_OUTPUT = 1 +} FriIODirection; + +/* Redundancy strategy + @KUKA.Internal */ +typedef enum _RedundancyStrategy { + RedundancyStrategy_E1 = 0, + RedundancyStrategy_NONE = 4 +} RedundancyStrategy; + +/* Struct definitions */ +/* Container object for Joint Values + @KUKA.Internal */ +typedef struct _JointValues { + pb_callback_t value; /* value of a single joint. */ +} JointValues; + +/* Timestamp container. + @KUKA.Internal */ +typedef struct _TimeStamp { + uint32_t sec; /* Second portion of the timestamp. Time starts at 1.1.1970. */ + uint32_t nanosec; /* Nano second portion of the timestamp. Time starts at 1.1.1970. */ +} TimeStamp; + +/* Cartesian vector container. + KUKA.Internal */ +typedef struct _CartesianVector { + pb_size_t element_count; + double element[6]; /* value of a single vector element */ +} CartesianVector; + +/* Contains possible checksum implementations like CRC32,MD5. + @KUKA.Internal */ +typedef struct _Checksum { + bool has_crc32; + int32_t crc32; /* Storage for CRC32. */ +} Checksum; + +/* Quaternion transformation container. + @KUKA.Internal */ +typedef struct _QuaternionTransformation { + char name[64]; /* switch to 'required' due to nanopb-bug */ + pb_size_t element_count; + double element[7]; + bool has_timestamp; + TimeStamp timestamp; +} QuaternionTransformation; + +/* Request output on value from Fieldbus with Group.Name */ +typedef struct _FriIOValue { + char name[64]; + FriIOType type; + FriIODirection direction; + bool has_digitalValue; + uint64_t digitalValue; + bool has_analogValue; + double analogValue; +} FriIOValue; + +typedef struct _RedundancyInformation { + RedundancyStrategy strategy; + double value; +} RedundancyInformation; + +/* FRI Message Header. Contains the information for timing handshake and the message identifier. + The following messageIdentifiers are currently available: + LBR Monitoring Message: 0x245142 + LBR Command Message: 0x34001 + @KUKA.Internal */ +typedef struct _MessageHeader { + uint32_t messageIdentifier; /* Message identifier. */ + uint32_t sequenceCounter; /* Sequence counter. Checked to determine the timing. */ + uint32_t reflectedSequenceCounter; /* Reflected sequence counter. Checked to determine the timing. */ +} MessageHeader; + +/* FRI Connection info. Contains the connection state and additional informations. + @KUKA.Internal */ +typedef struct _ConnectionInfo { + FRISessionState sessionState; /* state of the FRI session. */ + FRIConnectionQuality quality; /* Quality of the FRI Connection. */ + bool has_sendPeriod; + uint32_t sendPeriod; /* Timing in Milliseconds, on which the Controller sends a new Message. */ + bool has_receiveMultiplier; + uint32_t receiveMultiplier; /* Multiplier of sendPeriod, on which the Controller expects a new CommmandMessage. */ +} ConnectionInfo; + +/* Robot Information Object. Contains all static Information about the robot. e.g. + Number of Joints. + @KUKA.Internal */ +typedef struct _RobotInfo { + bool has_numberOfJoints; + int32_t numberOfJoints; /* availabe number of joints. */ + bool has_safetyState; + SafetyState safetyState; /* Safety state of the controller. */ + pb_callback_t driveState; /* Drivestate of the drives. */ + bool has_operationMode; + OperationMode operationMode; /* OperationMode of the Controller. */ + bool has_controlMode; + ControlMode controlMode; /* Controlmode of the robot. */ +} RobotInfo; + +/* FRI Monitor Data. Contains the cylic Information about the current robot state. + @KUKA.Internal */ +typedef struct _MessageMonitorData { + bool has_measuredJointPosition; + JointValues measuredJointPosition; /* measured joint values. */ + bool has_measuredTorque; + JointValues measuredTorque; /* measured torque values. */ + bool has_commandedTorque; + JointValues commandedTorque; /* last commanded torque value. */ + bool has_externalTorque; + JointValues externalTorque; /* observed external torque. */ + /* optional CartesianVector externalForce = 6; // observed Cartesian external forces and torque in flange coordinates + repeated QuaternionTransformation subscribedTransformations = 7; // selected transformations from controller to client */ + pb_size_t readIORequest_count; + FriIOValue readIORequest[10]; /* used to read FieldBus input value(s) */ + bool has_measuredCartesianPose; + QuaternionTransformation measuredCartesianPose; /* measured Cartesian pose. */ + bool has_measuredRedundancyInformation; + RedundancyInformation measuredRedundancyInformation; /* measured redundancy information */ + bool has_timestamp; + TimeStamp timestamp; /* timestamp of the measurement. */ +} MessageMonitorData; + +/* FRI Interpolator Data. Contains the cyclic commands which are going to be send + to the robot by the interpolator. + @KUKA.Internal */ +typedef struct _MessageIpoData { + bool has_jointPosition; + JointValues jointPosition; /* current joint values of interpolator. */ + bool has_cartesianPose; + QuaternionTransformation cartesianPose; /* Cartesian pose that is commanded to the robot pipe in the current time step */ + bool has_redundancyInformation; + RedundancyInformation redundancyInformation; /* current redundancy information of interpolator */ + bool has_clientCommandMode; + ClientCommandMode clientCommandMode; /* current command mode of the interpolator. */ + bool has_overlayType; + OverlayType overlayType; /* current overlay type. */ + bool has_trackingPerformance; + double trackingPerformance; /* tracking performance of the commands */ +} MessageIpoData; + +/* FRI Command Data. Contains the cyclic commands to modify the robot position. + @KUKA.Internal */ +typedef struct _MessageCommandData { + bool has_jointPosition; + JointValues jointPosition; /* commanded joint Position. */ + bool has_cartesianWrenchFeedForward; + CartesianVector cartesianWrenchFeedForward; /* cartesian wrench feed forward [N,N,N,Nm,Nm,Nm] */ + bool has_jointTorque; + JointValues jointTorque; /* commanded joint torques [Nm, ..., Nm]. */ + pb_size_t commandedTransformations_count; + QuaternionTransformation commandedTransformations[5]; /* commanded transformations from the client as requested by the robot controller */ + pb_size_t writeIORequest_count; + FriIOValue writeIORequest[10]; /* used to write FieldBus output value(s) */ + bool has_cartesianPose; + QuaternionTransformation cartesianPose; /* commanded Cartesian pose. */ + bool has_redundancyInformation; + RedundancyInformation redundancyInformation; /* commanded redundancy information */ +} MessageCommandData; + +/* FRI End of Data. Contains the length and CRC32 of the data before. + @KUKA.Internal */ +typedef struct _MessageEndOf { + bool has_messageLength; + int32_t messageLength; /* Length of the header + Controller data. */ + bool has_messageChecksum; + Checksum messageChecksum; /* checksum for all data before this point muss be last date. */ +} MessageEndOf; + +/* FRI Monitoring Message. Contains the cyclic Information of the robot position and state. + @KUKA.Internal */ +typedef struct _FRIMonitoringMessage { + MessageHeader header; /* Message header. */ + bool has_robotInfo; + RobotInfo robotInfo; /* Robot Information. */ + bool has_monitorData; + MessageMonitorData monitorData; /* data collected during Monitoring. */ + bool has_connectionInfo; + ConnectionInfo connectionInfo; /* Connection Information . */ + bool has_ipoData; + MessageIpoData ipoData; /* output from interpolator */ + pb_size_t requestedTransformations_count; + QuaternionTransformation requestedTransformations[5]; /* transformations requested by the robot controller from the client */ + bool has_endOfMessageData; + MessageEndOf endOfMessageData; /* End of Package contains message length and checksum. */ +} FRIMonitoringMessage; + +/* FRI Command Message. Contains the information of the user to modify the robot commands. + @KUKA.Internal */ +typedef struct _FRICommandMessage { + MessageHeader header; /* Message header. */ + bool has_commandData; + MessageCommandData commandData; /* Command Data. */ + bool has_endOfMessageData; + MessageEndOf endOfMessageData; /* End of Package contains message length and checksum. */ +} FRICommandMessage; + + +#ifdef __cplusplus +extern "C" { +#endif + +/* Helper constants for enums */ +#define _FRISessionState_MIN FRISessionState_IDLE +#define _FRISessionState_MAX FRISessionState_COMMANDING_ACTIVE +#define _FRISessionState_ARRAYSIZE ((FRISessionState)(FRISessionState_COMMANDING_ACTIVE+1)) + +#define _FRIConnectionQuality_MIN FRIConnectionQuality_POOR +#define _FRIConnectionQuality_MAX FRIConnectionQuality_EXCELLENT +#define _FRIConnectionQuality_ARRAYSIZE ((FRIConnectionQuality)(FRIConnectionQuality_EXCELLENT+1)) + +#define _SafetyState_MIN SafetyState_NORMAL_OPERATION +#define _SafetyState_MAX SafetyState_SAFETY_STOP_LEVEL_2 +#define _SafetyState_ARRAYSIZE ((SafetyState)(SafetyState_SAFETY_STOP_LEVEL_2+1)) + +#define _OperationMode_MIN OperationMode_TEST_MODE_1 +#define _OperationMode_MAX OperationMode_AUTOMATIC_MODE +#define _OperationMode_ARRAYSIZE ((OperationMode)(OperationMode_AUTOMATIC_MODE+1)) + +#define _DriveState_MIN DriveState_OFF +#define _DriveState_MAX DriveState_ACTIVE +#define _DriveState_ARRAYSIZE ((DriveState)(DriveState_ACTIVE+1)) + +#define _ControlMode_MIN ControlMode_POSITION_CONTROLMODE +#define _ControlMode_MAX ControlMode_NO_CONTROLMODE +#define _ControlMode_ARRAYSIZE ((ControlMode)(ControlMode_NO_CONTROLMODE+1)) + +#define _ClientCommandMode_MIN ClientCommandMode_NO_COMMAND_MODE +#define _ClientCommandMode_MAX ClientCommandMode_CARTESIAN_POSE +#define _ClientCommandMode_ARRAYSIZE ((ClientCommandMode)(ClientCommandMode_CARTESIAN_POSE+1)) + +#define _OverlayType_MIN OverlayType_NO_OVERLAY +#define _OverlayType_MAX OverlayType_CARTESIAN +#define _OverlayType_ARRAYSIZE ((OverlayType)(OverlayType_CARTESIAN+1)) + +#define _FriIOType_MIN FriIOType_BOOLEAN +#define _FriIOType_MAX FriIOType_ANALOG +#define _FriIOType_ARRAYSIZE ((FriIOType)(FriIOType_ANALOG+1)) + +#define _FriIODirection_MIN FriIODirection_INPUT +#define _FriIODirection_MAX FriIODirection_OUTPUT +#define _FriIODirection_ARRAYSIZE ((FriIODirection)(FriIODirection_OUTPUT+1)) + +#define _RedundancyStrategy_MIN RedundancyStrategy_E1 +#define _RedundancyStrategy_MAX RedundancyStrategy_NONE +#define _RedundancyStrategy_ARRAYSIZE ((RedundancyStrategy)(RedundancyStrategy_NONE+1)) + + + + + + +#define FriIOValue_type_ENUMTYPE FriIOType +#define FriIOValue_direction_ENUMTYPE FriIODirection + +#define RedundancyInformation_strategy_ENUMTYPE RedundancyStrategy + + +#define ConnectionInfo_sessionState_ENUMTYPE FRISessionState +#define ConnectionInfo_quality_ENUMTYPE FRIConnectionQuality + +#define RobotInfo_safetyState_ENUMTYPE SafetyState +#define RobotInfo_driveState_ENUMTYPE DriveState +#define RobotInfo_operationMode_ENUMTYPE OperationMode +#define RobotInfo_controlMode_ENUMTYPE ControlMode + + +#define MessageIpoData_clientCommandMode_ENUMTYPE ClientCommandMode +#define MessageIpoData_overlayType_ENUMTYPE OverlayType + + + + + + +/* Initializer values for message structs */ +#define JointValues_init_default {{{NULL}, NULL}} +#define TimeStamp_init_default {0, 0} +#define CartesianVector_init_default {0, {0, 0, 0, 0, 0, 0}} +#define Checksum_init_default {false, 0} +#define QuaternionTransformation_init_default {"", 0, {0, 0, 0, 0, 0, 0, 0}, false, TimeStamp_init_default} +#define FriIOValue_init_default {"", _FriIOType_MIN, _FriIODirection_MIN, false, 0, false, 0} +#define RedundancyInformation_init_default {_RedundancyStrategy_MIN, 0} +#define MessageHeader_init_default {0, 0, 0} +#define ConnectionInfo_init_default {_FRISessionState_MIN, _FRIConnectionQuality_MIN, false, 0, false, 0} +#define RobotInfo_init_default {false, 0, false, _SafetyState_MIN, {{NULL}, NULL}, false, _OperationMode_MIN, false, _ControlMode_MIN} +#define MessageMonitorData_init_default {false, JointValues_init_default, false, JointValues_init_default, false, JointValues_init_default, false, JointValues_init_default, 0, {FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default}, false, QuaternionTransformation_init_default, false, RedundancyInformation_init_default, false, TimeStamp_init_default} +#define MessageIpoData_init_default {false, JointValues_init_default, false, QuaternionTransformation_init_default, false, RedundancyInformation_init_default, false, _ClientCommandMode_MIN, false, _OverlayType_MIN, false, 0} +#define MessageCommandData_init_default {false, JointValues_init_default, false, CartesianVector_init_default, false, JointValues_init_default, 0, {QuaternionTransformation_init_default, QuaternionTransformation_init_default, QuaternionTransformation_init_default, QuaternionTransformation_init_default, QuaternionTransformation_init_default}, 0, {FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default}, false, QuaternionTransformation_init_default, false, RedundancyInformation_init_default} +#define MessageEndOf_init_default {false, 0, false, Checksum_init_default} +#define FRIMonitoringMessage_init_default {MessageHeader_init_default, false, RobotInfo_init_default, false, MessageMonitorData_init_default, false, ConnectionInfo_init_default, false, MessageIpoData_init_default, 0, {QuaternionTransformation_init_default, QuaternionTransformation_init_default, QuaternionTransformation_init_default, QuaternionTransformation_init_default, QuaternionTransformation_init_default}, false, MessageEndOf_init_default} +#define FRICommandMessage_init_default {MessageHeader_init_default, false, MessageCommandData_init_default, false, MessageEndOf_init_default} +#define JointValues_init_zero {{{NULL}, NULL}} +#define TimeStamp_init_zero {0, 0} +#define CartesianVector_init_zero {0, {0, 0, 0, 0, 0, 0}} +#define Checksum_init_zero {false, 0} +#define QuaternionTransformation_init_zero {"", 0, {0, 0, 0, 0, 0, 0, 0}, false, TimeStamp_init_zero} +#define FriIOValue_init_zero {"", _FriIOType_MIN, _FriIODirection_MIN, false, 0, false, 0} +#define RedundancyInformation_init_zero {_RedundancyStrategy_MIN, 0} +#define MessageHeader_init_zero {0, 0, 0} +#define ConnectionInfo_init_zero {_FRISessionState_MIN, _FRIConnectionQuality_MIN, false, 0, false, 0} +#define RobotInfo_init_zero {false, 0, false, _SafetyState_MIN, {{NULL}, NULL}, false, _OperationMode_MIN, false, _ControlMode_MIN} +#define MessageMonitorData_init_zero {false, JointValues_init_zero, false, JointValues_init_zero, false, JointValues_init_zero, false, JointValues_init_zero, 0, {FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero}, false, QuaternionTransformation_init_zero, false, RedundancyInformation_init_zero, false, TimeStamp_init_zero} +#define MessageIpoData_init_zero {false, JointValues_init_zero, false, QuaternionTransformation_init_zero, false, RedundancyInformation_init_zero, false, _ClientCommandMode_MIN, false, _OverlayType_MIN, false, 0} +#define MessageCommandData_init_zero {false, JointValues_init_zero, false, CartesianVector_init_zero, false, JointValues_init_zero, 0, {QuaternionTransformation_init_zero, QuaternionTransformation_init_zero, QuaternionTransformation_init_zero, QuaternionTransformation_init_zero, QuaternionTransformation_init_zero}, 0, {FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero}, false, QuaternionTransformation_init_zero, false, RedundancyInformation_init_zero} +#define MessageEndOf_init_zero {false, 0, false, Checksum_init_zero} +#define FRIMonitoringMessage_init_zero {MessageHeader_init_zero, false, RobotInfo_init_zero, false, MessageMonitorData_init_zero, false, ConnectionInfo_init_zero, false, MessageIpoData_init_zero, 0, {QuaternionTransformation_init_zero, QuaternionTransformation_init_zero, QuaternionTransformation_init_zero, QuaternionTransformation_init_zero, QuaternionTransformation_init_zero}, false, MessageEndOf_init_zero} +#define FRICommandMessage_init_zero {MessageHeader_init_zero, false, MessageCommandData_init_zero, false, MessageEndOf_init_zero} + +/* Field tags (for use in manual encoding/decoding) */ +#define JointValues_value_tag 1 +#define TimeStamp_sec_tag 1 +#define TimeStamp_nanosec_tag 2 +#define CartesianVector_element_tag 1 +#define Checksum_crc32_tag 1 +#define QuaternionTransformation_name_tag 1 +#define QuaternionTransformation_element_tag 2 +#define QuaternionTransformation_timestamp_tag 3 +#define FriIOValue_name_tag 1 +#define FriIOValue_type_tag 2 +#define FriIOValue_direction_tag 3 +#define FriIOValue_digitalValue_tag 4 +#define FriIOValue_analogValue_tag 5 +#define RedundancyInformation_strategy_tag 1 +#define RedundancyInformation_value_tag 2 +#define MessageHeader_messageIdentifier_tag 1 +#define MessageHeader_sequenceCounter_tag 2 +#define MessageHeader_reflectedSequenceCounter_tag 3 +#define ConnectionInfo_sessionState_tag 1 +#define ConnectionInfo_quality_tag 2 +#define ConnectionInfo_sendPeriod_tag 3 +#define ConnectionInfo_receiveMultiplier_tag 4 +#define RobotInfo_numberOfJoints_tag 1 +#define RobotInfo_safetyState_tag 2 +#define RobotInfo_driveState_tag 5 +#define RobotInfo_operationMode_tag 6 +#define RobotInfo_controlMode_tag 7 +#define MessageMonitorData_measuredJointPosition_tag 1 +#define MessageMonitorData_measuredTorque_tag 2 +#define MessageMonitorData_commandedTorque_tag 4 +#define MessageMonitorData_externalTorque_tag 5 +#define MessageMonitorData_readIORequest_tag 8 +#define MessageMonitorData_measuredCartesianPose_tag 9 +#define MessageMonitorData_measuredRedundancyInformation_tag 10 +#define MessageMonitorData_timestamp_tag 15 +#define MessageIpoData_jointPosition_tag 1 +#define MessageIpoData_cartesianPose_tag 2 +#define MessageIpoData_redundancyInformation_tag 3 +#define MessageIpoData_clientCommandMode_tag 10 +#define MessageIpoData_overlayType_tag 11 +#define MessageIpoData_trackingPerformance_tag 12 +#define MessageCommandData_jointPosition_tag 1 +#define MessageCommandData_cartesianWrenchFeedForward_tag 2 +#define MessageCommandData_jointTorque_tag 3 +#define MessageCommandData_commandedTransformations_tag 4 +#define MessageCommandData_writeIORequest_tag 5 +#define MessageCommandData_cartesianPose_tag 6 +#define MessageCommandData_redundancyInformation_tag 7 +#define MessageEndOf_messageLength_tag 1 +#define MessageEndOf_messageChecksum_tag 2 +#define FRIMonitoringMessage_header_tag 1 +#define FRIMonitoringMessage_robotInfo_tag 2 +#define FRIMonitoringMessage_monitorData_tag 3 +#define FRIMonitoringMessage_connectionInfo_tag 4 +#define FRIMonitoringMessage_ipoData_tag 5 +#define FRIMonitoringMessage_requestedTransformations_tag 6 +#define FRIMonitoringMessage_endOfMessageData_tag 15 +#define FRICommandMessage_header_tag 1 +#define FRICommandMessage_commandData_tag 2 +#define FRICommandMessage_endOfMessageData_tag 15 + +/* Struct field encoding specification for nanopb */ +#define JointValues_FIELDLIST(X, a) \ +X(a, CALLBACK, REPEATED, DOUBLE, value, 1) +#define JointValues_CALLBACK pb_default_field_callback +#define JointValues_DEFAULT NULL + +#define TimeStamp_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, UINT32, sec, 1) \ +X(a, STATIC, REQUIRED, UINT32, nanosec, 2) +#define TimeStamp_CALLBACK NULL +#define TimeStamp_DEFAULT NULL + +#define CartesianVector_FIELDLIST(X, a) \ +X(a, STATIC, REPEATED, DOUBLE, element, 1) +#define CartesianVector_CALLBACK NULL +#define CartesianVector_DEFAULT NULL + +#define Checksum_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, INT32, crc32, 1) +#define Checksum_CALLBACK NULL +#define Checksum_DEFAULT NULL + +#define QuaternionTransformation_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, STRING, name, 1) \ +X(a, STATIC, REPEATED, DOUBLE, element, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, timestamp, 3) +#define QuaternionTransformation_CALLBACK NULL +#define QuaternionTransformation_DEFAULT NULL +#define QuaternionTransformation_timestamp_MSGTYPE TimeStamp + +#define FriIOValue_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, STRING, name, 1) \ +X(a, STATIC, REQUIRED, UENUM, type, 2) \ +X(a, STATIC, REQUIRED, UENUM, direction, 3) \ +X(a, STATIC, OPTIONAL, UINT64, digitalValue, 4) \ +X(a, STATIC, OPTIONAL, DOUBLE, analogValue, 5) +#define FriIOValue_CALLBACK NULL +#define FriIOValue_DEFAULT NULL + +#define RedundancyInformation_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, UENUM, strategy, 1) \ +X(a, STATIC, REQUIRED, DOUBLE, value, 2) +#define RedundancyInformation_CALLBACK NULL +#define RedundancyInformation_DEFAULT NULL + +#define MessageHeader_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, UINT32, messageIdentifier, 1) \ +X(a, STATIC, REQUIRED, UINT32, sequenceCounter, 2) \ +X(a, STATIC, REQUIRED, UINT32, reflectedSequenceCounter, 3) +#define MessageHeader_CALLBACK NULL +#define MessageHeader_DEFAULT NULL + +#define ConnectionInfo_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, UENUM, sessionState, 1) \ +X(a, STATIC, REQUIRED, UENUM, quality, 2) \ +X(a, STATIC, OPTIONAL, UINT32, sendPeriod, 3) \ +X(a, STATIC, OPTIONAL, UINT32, receiveMultiplier, 4) +#define ConnectionInfo_CALLBACK NULL +#define ConnectionInfo_DEFAULT NULL + +#define RobotInfo_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, INT32, numberOfJoints, 1) \ +X(a, STATIC, OPTIONAL, UENUM, safetyState, 2) \ +X(a, CALLBACK, REPEATED, UENUM, driveState, 5) \ +X(a, STATIC, OPTIONAL, UENUM, operationMode, 6) \ +X(a, STATIC, OPTIONAL, UENUM, controlMode, 7) +#define RobotInfo_CALLBACK pb_default_field_callback +#define RobotInfo_DEFAULT NULL + +#define MessageMonitorData_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, MESSAGE, measuredJointPosition, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, measuredTorque, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, commandedTorque, 4) \ +X(a, STATIC, OPTIONAL, MESSAGE, externalTorque, 5) \ +X(a, STATIC, REPEATED, MESSAGE, readIORequest, 8) \ +X(a, STATIC, OPTIONAL, MESSAGE, measuredCartesianPose, 9) \ +X(a, STATIC, OPTIONAL, MESSAGE, measuredRedundancyInformation, 10) \ +X(a, STATIC, OPTIONAL, MESSAGE, timestamp, 15) +#define MessageMonitorData_CALLBACK NULL +#define MessageMonitorData_DEFAULT NULL +#define MessageMonitorData_measuredJointPosition_MSGTYPE JointValues +#define MessageMonitorData_measuredTorque_MSGTYPE JointValues +#define MessageMonitorData_commandedTorque_MSGTYPE JointValues +#define MessageMonitorData_externalTorque_MSGTYPE JointValues +#define MessageMonitorData_readIORequest_MSGTYPE FriIOValue +#define MessageMonitorData_measuredCartesianPose_MSGTYPE QuaternionTransformation +#define MessageMonitorData_measuredRedundancyInformation_MSGTYPE RedundancyInformation +#define MessageMonitorData_timestamp_MSGTYPE TimeStamp + +#define MessageIpoData_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, MESSAGE, jointPosition, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, cartesianPose, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, redundancyInformation, 3) \ +X(a, STATIC, OPTIONAL, UENUM, clientCommandMode, 10) \ +X(a, STATIC, OPTIONAL, UENUM, overlayType, 11) \ +X(a, STATIC, OPTIONAL, DOUBLE, trackingPerformance, 12) +#define MessageIpoData_CALLBACK NULL +#define MessageIpoData_DEFAULT NULL +#define MessageIpoData_jointPosition_MSGTYPE JointValues +#define MessageIpoData_cartesianPose_MSGTYPE QuaternionTransformation +#define MessageIpoData_redundancyInformation_MSGTYPE RedundancyInformation + +#define MessageCommandData_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, MESSAGE, jointPosition, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, cartesianWrenchFeedForward, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, jointTorque, 3) \ +X(a, STATIC, REPEATED, MESSAGE, commandedTransformations, 4) \ +X(a, STATIC, REPEATED, MESSAGE, writeIORequest, 5) \ +X(a, STATIC, OPTIONAL, MESSAGE, cartesianPose, 6) \ +X(a, STATIC, OPTIONAL, MESSAGE, redundancyInformation, 7) +#define MessageCommandData_CALLBACK NULL +#define MessageCommandData_DEFAULT NULL +#define MessageCommandData_jointPosition_MSGTYPE JointValues +#define MessageCommandData_cartesianWrenchFeedForward_MSGTYPE CartesianVector +#define MessageCommandData_jointTorque_MSGTYPE JointValues +#define MessageCommandData_commandedTransformations_MSGTYPE QuaternionTransformation +#define MessageCommandData_writeIORequest_MSGTYPE FriIOValue +#define MessageCommandData_cartesianPose_MSGTYPE QuaternionTransformation +#define MessageCommandData_redundancyInformation_MSGTYPE RedundancyInformation + +#define MessageEndOf_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, INT32, messageLength, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, messageChecksum, 2) +#define MessageEndOf_CALLBACK NULL +#define MessageEndOf_DEFAULT NULL +#define MessageEndOf_messageChecksum_MSGTYPE Checksum + +#define FRIMonitoringMessage_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, MESSAGE, header, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, robotInfo, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, monitorData, 3) \ +X(a, STATIC, OPTIONAL, MESSAGE, connectionInfo, 4) \ +X(a, STATIC, OPTIONAL, MESSAGE, ipoData, 5) \ +X(a, STATIC, REPEATED, MESSAGE, requestedTransformations, 6) \ +X(a, STATIC, OPTIONAL, MESSAGE, endOfMessageData, 15) +#define FRIMonitoringMessage_CALLBACK NULL +#define FRIMonitoringMessage_DEFAULT NULL +#define FRIMonitoringMessage_header_MSGTYPE MessageHeader +#define FRIMonitoringMessage_robotInfo_MSGTYPE RobotInfo +#define FRIMonitoringMessage_monitorData_MSGTYPE MessageMonitorData +#define FRIMonitoringMessage_connectionInfo_MSGTYPE ConnectionInfo +#define FRIMonitoringMessage_ipoData_MSGTYPE MessageIpoData +#define FRIMonitoringMessage_requestedTransformations_MSGTYPE QuaternionTransformation +#define FRIMonitoringMessage_endOfMessageData_MSGTYPE MessageEndOf + +#define FRICommandMessage_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, MESSAGE, header, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, commandData, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, endOfMessageData, 15) +#define FRICommandMessage_CALLBACK NULL +#define FRICommandMessage_DEFAULT NULL +#define FRICommandMessage_header_MSGTYPE MessageHeader +#define FRICommandMessage_commandData_MSGTYPE MessageCommandData +#define FRICommandMessage_endOfMessageData_MSGTYPE MessageEndOf + +extern const pb_msgdesc_t JointValues_msg; +extern const pb_msgdesc_t TimeStamp_msg; +extern const pb_msgdesc_t CartesianVector_msg; +extern const pb_msgdesc_t Checksum_msg; +extern const pb_msgdesc_t QuaternionTransformation_msg; +extern const pb_msgdesc_t FriIOValue_msg; +extern const pb_msgdesc_t RedundancyInformation_msg; +extern const pb_msgdesc_t MessageHeader_msg; +extern const pb_msgdesc_t ConnectionInfo_msg; +extern const pb_msgdesc_t RobotInfo_msg; +extern const pb_msgdesc_t MessageMonitorData_msg; +extern const pb_msgdesc_t MessageIpoData_msg; +extern const pb_msgdesc_t MessageCommandData_msg; +extern const pb_msgdesc_t MessageEndOf_msg; +extern const pb_msgdesc_t FRIMonitoringMessage_msg; +extern const pb_msgdesc_t FRICommandMessage_msg; + +/* Defines for backwards compatibility with code written before nanopb-0.4.0 */ +#define JointValues_fields &JointValues_msg +#define TimeStamp_fields &TimeStamp_msg +#define CartesianVector_fields &CartesianVector_msg +#define Checksum_fields &Checksum_msg +#define QuaternionTransformation_fields &QuaternionTransformation_msg +#define FriIOValue_fields &FriIOValue_msg +#define RedundancyInformation_fields &RedundancyInformation_msg +#define MessageHeader_fields &MessageHeader_msg +#define ConnectionInfo_fields &ConnectionInfo_msg +#define RobotInfo_fields &RobotInfo_msg +#define MessageMonitorData_fields &MessageMonitorData_msg +#define MessageIpoData_fields &MessageIpoData_msg +#define MessageCommandData_fields &MessageCommandData_msg +#define MessageEndOf_fields &MessageEndOf_msg +#define FRIMonitoringMessage_fields &FRIMonitoringMessage_msg +#define FRICommandMessage_fields &FRICommandMessage_msg + +/* Maximum encoded size of messages (where known) */ +/* JointValues_size depends on runtime parameters */ +/* RobotInfo_size depends on runtime parameters */ +/* MessageMonitorData_size depends on runtime parameters */ +/* MessageIpoData_size depends on runtime parameters */ +/* MessageCommandData_size depends on runtime parameters */ +/* FRIMonitoringMessage_size depends on runtime parameters */ +/* FRICommandMessage_size depends on runtime parameters */ +#define CartesianVector_size 54 +#define Checksum_size 11 +#define ConnectionInfo_size 16 +#define FRIMESSAGES_PB_H_MAX_SIZE QuaternionTransformation_size +#define FriIOValue_size 89 +#define MessageEndOf_size 24 +#define MessageHeader_size 18 +#define QuaternionTransformation_size 142 +#define RedundancyInformation_size 11 +#define TimeStamp_size 12 + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientApplication.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientApplication.cpp index 340c71ce..5f26ad21 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientApplication.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientApplication.cpp @@ -5,10 +5,10 @@ agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germa SCOPE -The software “KUKA Sunrise.FRI Client SDK” is targeted to work in -conjunction with the “KUKA Sunrise.FRI” toolkit. -In the following, the term “software” refers to all material directly -belonging to the provided SDK “Software development kit”, particularly source +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source code, libraries, binaries, manuals and technical documentation. COPYRIGHT @@ -58,12 +58,12 @@ cost of any service and repair. \version {2.5} */ #include -#include "friClientApplication.h" -#include "friClientIf.h" -#include "friConnectionIf.h" -#include "friClientData.h" -#include "FRIMessages.pb.h" -#include "friTransformationClient.h" +#include +#include +#include +#include +#include +#include using namespace KUKA::FRI; diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientData.h b/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientData.h index 1fb04e5f..67c11c56 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientData.h +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientData.h @@ -5,10 +5,10 @@ agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germa SCOPE -The software “KUKA Sunrise.FRI Client SDK” is targeted to work in -conjunction with the “KUKA Sunrise.FRI” toolkit. -In the following, the term “software” refers to all material directly -belonging to the provided SDK “Software development kit”, particularly source +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source code, libraries, binaries, manuals and technical documentation. COPYRIGHT @@ -62,11 +62,11 @@ cost of any service and repair. #include -#include "FRIMessages.pb.h" -#include "friMonitoringMessageDecoder.h" -#include "friCommandMessageEncoder.h" -#include "friClientIf.h" -#include "friException.h" +#include +#include +#include +#include +#include namespace KUKA { diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.cpp index 751095d1..5653154c 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.cpp @@ -5,10 +5,10 @@ agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germa SCOPE -The software “KUKA Sunrise.FRI Client SDK” is targeted to work in -conjunction with the “KUKA Sunrise.FRI” toolkit. -In the following, the term “software” refers to all material directly -belonging to the provided SDK “Software development kit”, particularly source +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source code, libraries, binaries, manuals and technical documentation. COPYRIGHT @@ -58,8 +58,8 @@ cost of any service and repair. \version {2.5} */ #include -#include "friCommandMessageEncoder.h" -#include "pb_encode.h" +#include +#include using namespace KUKA::FRI; diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.h b/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.h index 00dd6422..dde55882 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.h +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.h @@ -5,10 +5,10 @@ agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germa SCOPE -The software “KUKA Sunrise.FRI Client SDK” is targeted to work in -conjunction with the “KUKA Sunrise.FRI” toolkit. -In the following, the term “software” refers to all material directly -belonging to the provided SDK “Software development kit”, particularly source +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source code, libraries, binaries, manuals and technical documentation. COPYRIGHT @@ -61,8 +61,8 @@ cost of any service and repair. #define _KUKA_FRI_COMMANDMESSAGEENCODER_H -#include "FRIMessages.pb.h" -#include "pb_frimessages_callbacks.h" +#include +#include diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friDataHelper.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friDataHelper.cpp index 2d71d8b1..d87dbb5a 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friDataHelper.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friDataHelper.cpp @@ -5,10 +5,10 @@ agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germa SCOPE -The software “KUKA Sunrise.FRI Client SDK” is targeted to work in -conjunction with the “KUKA Sunrise.FRI” toolkit. -In the following, the term “software” refers to all material directly -belonging to the provided SDK “Software development kit”, particularly source +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source code, libraries, binaries, manuals and technical documentation. COPYRIGHT @@ -60,7 +60,7 @@ cost of any service and repair. #include -#include "friDataHelper.h" +#include namespace KUKA { diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp index ac46d659..f8231f0c 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp @@ -5,10 +5,10 @@ agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germa SCOPE -The software “KUKA Sunrise.FRI Client SDK” is targeted to work in -conjunction with the “KUKA Sunrise.FRI” toolkit. -In the following, the term “software” refers to all material directly -belonging to the provided SDK “Software development kit”, particularly source +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source code, libraries, binaries, manuals and technical documentation. COPYRIGHT @@ -58,8 +58,8 @@ cost of any service and repair. \version {2.5} */ #include -#include "friLBRClient.h" -#include "friClientData.h" +#include +#include using namespace KUKA::FRI; char FRIException::_buffer[1024] = { 0 }; @@ -92,7 +92,7 @@ void LBRClient::monitor() //****************************************************************************** void LBRClient::waitForCommand() { - if (CARTESIAN_POSE == _robotState.getClientCommandMode()) + if (ClientCommandMode_CARTESIAN_POSE == _robotState.getClientCommandMode()) robotCommand().setCartesianPose(robotState().getIpoCartesianPose()); else robotCommand().setJointPosition(robotState().getIpoJointPosition()); diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRCommand.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRCommand.cpp index 80e47ba6..e1487154 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRCommand.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRCommand.cpp @@ -5,10 +5,10 @@ agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germa SCOPE -The software “KUKA Sunrise.FRI Client SDK” is targeted to work in -conjunction with the “KUKA Sunrise.FRI” toolkit. -In the following, the term “software” refers to all material directly -belonging to the provided SDK “Software development kit”, particularly source +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source code, libraries, binaries, manuals and technical documentation. COPYRIGHT @@ -57,11 +57,11 @@ cost of any service and repair. \file \version {2.5} */ -#include "friLBRState.h" -#include "friLBRCommand.h" -#include "friClientData.h" -#include "pb_frimessages_callbacks.h" -#include "friDataHelper.h" +#include +#include +#include +#include +#include using namespace KUKA::FRI; diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRState.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRState.cpp index a5ae3dba..271ae868 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRState.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRState.cpp @@ -5,10 +5,10 @@ agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germa SCOPE -The software “KUKA Sunrise.FRI Client SDK” is targeted to work in -conjunction with the “KUKA Sunrise.FRI” toolkit. -In the following, the term “software” refers to all material directly -belonging to the provided SDK “Software development kit”, particularly source +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source code, libraries, binaries, manuals and technical documentation. COPYRIGHT @@ -57,10 +57,10 @@ cost of any service and repair. \file \version {2.5} */ -#include "friLBRState.h" -#include "friClientData.h" -#include "friDataHelper.h" -#include "pb_frimessages_callbacks.h" +#include +#include +#include +#include using namespace KUKA::FRI; diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friTransformationClient.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friTransformationClient.cpp index 08b4e031..12285b67 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friTransformationClient.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friTransformationClient.cpp @@ -5,10 +5,10 @@ agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germa SCOPE -The software “KUKA Sunrise.FRI Client SDK” is targeted to work in -conjunction with the “KUKA Sunrise.FRI” toolkit. -In the following, the term “software” refers to all material directly -belonging to the provided SDK “Software development kit”, particularly source +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source code, libraries, binaries, manuals and technical documentation. COPYRIGHT @@ -62,9 +62,9 @@ cost of any service and repair. #include #include -#include "friTransformationClient.h" -#include "friClientData.h" -#include "friDataHelper.h" +#include +#include +#include #include "FRIMessages.pb.h" #include "pb_frimessages_callbacks.h" diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friUdpConnection.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friUdpConnection.cpp index bd41c884..774552ad 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friUdpConnection.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friUdpConnection.cpp @@ -5,10 +5,10 @@ agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germa SCOPE -The software “KUKA Sunrise.FRI Client SDK” is targeted to work in -conjunction with the “KUKA Sunrise.FRI” toolkit. -In the following, the term “software” refers to all material directly -belonging to the provided SDK “Software development kit”, particularly source +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source code, libraries, binaries, manuals and technical documentation. COPYRIGHT @@ -63,7 +63,7 @@ cost of any service and repair. #include #endif -#include "friUdpConnection.h" +#include #ifdef WIN32 @@ -179,7 +179,7 @@ int UdpConnection::receive(char *buffer, int maxSize) #ifdef HAVE_SOCKLEN_T socklen_t sockAddrSize; #else - int sockAddrSize; + unsigned int sockAddrSize; #endif sockAddrSize = sizeof(struct sockaddr_in); /** check for timeout From 9f4767f94e8a85b0dc46710ab37fcf957a2a78ec Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Wed, 28 Aug 2024 10:27:14 +0200 Subject: [PATCH 21/49] typo --- kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp index f8231f0c..df035d3c 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp @@ -92,7 +92,7 @@ void LBRClient::monitor() //****************************************************************************** void LBRClient::waitForCommand() { - if (ClientCommandMode_CARTESIAN_POSE == _robotState.getClientCommandMode()) + if (CARTESIAN_POSE == _robotState.getClientCommandMode()) robotCommand().setCartesianPose(robotState().getIpoCartesianPose()); else robotCommand().setJointPosition(robotState().getIpoJointPosition()); From c8ef56e156e4ead05aca3b55023e6b4e5791a09d Mon Sep 17 00:00:00 2001 From: Svastits Date: Thu, 29 Aug 2024 14:59:29 +0200 Subject: [PATCH 22/49] sorry I stole your branch --- .github/CODEOWNERS | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 4e5c6cf7..d2b36d37 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,4 +1,2 @@ ## code changes will send PR to following users -* @VX792 -* @Svastits -* @kovacsge11 +* @Svastits @kovacsge11 From ebc181e55868795909f30ffaa73ec663866a5500 Mon Sep 17 00:00:00 2001 From: Svastits Date: Thu, 29 Aug 2024 15:01:33 +0200 Subject: [PATCH 23/49] Revert "sorry I stole your branch" This reverts commit c8ef56e156e4ead05aca3b55023e6b4e5791a09d. --- .github/CODEOWNERS | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index d2b36d37..4e5c6cf7 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,2 +1,4 @@ ## code changes will send PR to following users -* @Svastits @kovacsge11 +* @VX792 +* @Svastits +* @kovacsge11 From 360b3fb77baf3e4f5f0468ed6f31ad8aab2e523c Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Mon, 16 Sep 2024 14:22:31 +0200 Subject: [PATCH 24/49] send period < 5 ms is not needed in joint impedance mode --- kuka_sunrise_fri_driver/src/robot_manager_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index 4ed99a21..de6c6250 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -301,8 +301,8 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) case kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL: break; case kuka_drivers_core::ControlMode::JOINT_IMPEDANCE_CONTROL: - // TODO(Svastits): check whether this is necessary for impedance mode too - [[fallthrough]]; + break; + // TODO(Svastits): check whether this is necessary for impedance mode too // (Kristofi) No case kuka_drivers_core::ControlMode::WRENCH_CONTROL: [[fallthrough]]; case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: From 0d60f9ee399b6234a778bf747abb4667cf130745 Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Thu, 19 Sep 2024 13:33:31 +0200 Subject: [PATCH 25/49] publish impedance config values after controllers start --- kuka_sunrise_fri_driver/src/robot_manager_node.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index de6c6250..83670736 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -148,11 +148,7 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) // Publish FRI configuration to notify fri_configuration_controller of initial values setFriConfiguration(send_period_ms_, receive_multiplier_); - // Publish the values of the cartesian impedance parameters to the controller - setImpedanceConfiguration(cart_imp_pub_, cartesian_stiffness_, cartesian_damping_); - // Publish the values of the joint impedance parameters to the controller - setImpedanceConfiguration(joint_imp_pub_, joint_stiffness_, joint_damping_); - + // Configure hardware interface if (!kuka_drivers_core::changeHardwareState( change_hardware_state_client_, robot_model_, @@ -172,7 +168,13 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) { RCLCPP_ERROR(get_logger(), "Could not activate configuration controllers"); return FAILURE; - } + } + + // Publish the values of the cartesian impedance parameters to the controller + setImpedanceConfiguration(cart_imp_pub_, cartesian_stiffness_, cartesian_damping_); + // Publish the values of the joint impedance parameters to the controller + setImpedanceConfiguration(joint_imp_pub_, joint_stiffness_, joint_damping_); + is_configured_pub_->on_activate(); is_configured_msg_.data = true; is_configured_pub_->publish(is_configured_msg_); From 746efeb9fb31f2b0f91914757065d4ceac56a144 Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Fri, 20 Sep 2024 10:26:24 +0200 Subject: [PATCH 26/49] add impedance commands to wrench mode --- kuka_sunrise_fri_driver/src/hardware_interface.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 826635a5..d7f0dc7f 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -208,10 +208,8 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta break; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: fri_connection_->setCartesianImpedanceControlMode( - std::vector(6, 0.0), - std::vector(6, 0.1)); // 0.1 is the min accepted value for damping, it probably - // doesn't matter since sthe stiffness is 0 - + hw_cart_stiffness_commands_, + hw_cart_damping_commands_); fri_connection_->setClientCommandMode(ClientCommandModeID::WRENCH_COMMAND_MODE); break; default: From ac33516264043849af9e52cb398a220c8755c6a4 Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Fri, 20 Sep 2024 10:48:26 +0200 Subject: [PATCH 27/49] fromat --- kuka_sunrise_fri_driver/src/hardware_interface.cpp | 3 +-- kuka_sunrise_fri_driver/src/robot_manager_node.cpp | 9 ++++----- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index d7f0dc7f..e42dce54 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -208,8 +208,7 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta break; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: fri_connection_->setCartesianImpedanceControlMode( - hw_cart_stiffness_commands_, - hw_cart_damping_commands_); + hw_cart_stiffness_commands_, hw_cart_damping_commands_); fri_connection_->setClientCommandMode(ClientCommandModeID::WRENCH_COMMAND_MODE); break; default: diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index 83670736..479d94ee 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -148,7 +148,7 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) // Publish FRI configuration to notify fri_configuration_controller of initial values setFriConfiguration(send_period_ms_, receive_multiplier_); - + // Configure hardware interface if (!kuka_drivers_core::changeHardwareState( change_hardware_state_client_, robot_model_, @@ -168,13 +168,13 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) { RCLCPP_ERROR(get_logger(), "Could not activate configuration controllers"); return FAILURE; - } + } // Publish the values of the cartesian impedance parameters to the controller setImpedanceConfiguration(cart_imp_pub_, cartesian_stiffness_, cartesian_damping_); // Publish the values of the joint impedance parameters to the controller setImpedanceConfiguration(joint_imp_pub_, joint_stiffness_, joint_damping_); - + is_configured_pub_->on_activate(); is_configured_msg_.data = true; is_configured_pub_->publish(is_configured_msg_); @@ -303,8 +303,7 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) case kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL: break; case kuka_drivers_core::ControlMode::JOINT_IMPEDANCE_CONTROL: - break; - // TODO(Svastits): check whether this is necessary for impedance mode too // (Kristofi) No + break; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: [[fallthrough]]; case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: From a64a5637468e8b47519f23dd1d940312b2024838 Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Fri, 20 Sep 2024 11:33:01 +0200 Subject: [PATCH 28/49] removed friction compensation --- .../src/hardware_interface.cpp | 22 ++----------------- 1 file changed, 2 insertions(+), 20 deletions(-) diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index e42dce54..fb52e689 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -360,10 +360,7 @@ void KukaFRIHardwareInterface::updateCommand(const rclcpp::Time &) { const double * joint_torques_ = hw_torque_commands_.data(); const double * joint_pos = robotState().getMeasuredJointPosition(); - std::array joint_pos_corr; - std::copy(joint_pos, joint_pos + DOF, joint_pos_corr.begin()); - activateFrictionCompensation(joint_pos_corr.data()); - robotCommand().setJointPosition(joint_pos_corr.data()); + robotCommand().setJointPosition(joint_pos); robotCommand().setTorque(joint_torques_); break; } @@ -371,10 +368,7 @@ void KukaFRIHardwareInterface::updateCommand(const rclcpp::Time &) { const double * wrench_efforts = hw_wrench_commands_.data(); const double * joint_pos = robotState().getMeasuredJointPosition(); - std::array joint_pos_corr; - std::copy(joint_pos, joint_pos + DOF, joint_pos_corr.begin()); - activateFrictionCompensation(joint_pos_corr.data()); - robotCommand().setJointPosition(joint_pos_corr.data()); + robotCommand().setJointPosition(joint_pos); robotCommand().setWrench(wrench_efforts); break; } @@ -496,18 +490,6 @@ KukaFRIHardwareInterface::export_command_interfaces() return command_interfaces; } -// Friction compensation is activated only if the commanded and measured joint positions differ -void KukaFRIHardwareInterface::activateFrictionCompensation(double * values) const -{ - for (int i = 0; i < DOF; i++) - { - if (values[i] != 0.0) - { // Check for division by zero (very unlikely, but can happen after mastering) - values[i] -= (values[i] / fabs(values[i]) * 0.1); - } - } -} - void KukaFRIHardwareInterface::onError() { std::lock_guard lk(event_mutex_); From be90c177bbfe0f3164a9cd74d97aeb68eb76ba55 Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Mon, 16 Sep 2024 14:22:31 +0200 Subject: [PATCH 29/49] send period < 5 ms is not needed in joint impedance mode --- kuka_sunrise_fri_driver/src/robot_manager_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index 4ed99a21..de6c6250 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -301,8 +301,8 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) case kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL: break; case kuka_drivers_core::ControlMode::JOINT_IMPEDANCE_CONTROL: - // TODO(Svastits): check whether this is necessary for impedance mode too - [[fallthrough]]; + break; + // TODO(Svastits): check whether this is necessary for impedance mode too // (Kristofi) No case kuka_drivers_core::ControlMode::WRENCH_CONTROL: [[fallthrough]]; case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: From b1a458caea6293f4a39ed0aa0b6478a37f8309fe Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Thu, 19 Sep 2024 13:33:31 +0200 Subject: [PATCH 30/49] publish impedance config values after controllers start --- kuka_sunrise_fri_driver/src/robot_manager_node.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index de6c6250..83670736 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -148,11 +148,7 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) // Publish FRI configuration to notify fri_configuration_controller of initial values setFriConfiguration(send_period_ms_, receive_multiplier_); - // Publish the values of the cartesian impedance parameters to the controller - setImpedanceConfiguration(cart_imp_pub_, cartesian_stiffness_, cartesian_damping_); - // Publish the values of the joint impedance parameters to the controller - setImpedanceConfiguration(joint_imp_pub_, joint_stiffness_, joint_damping_); - + // Configure hardware interface if (!kuka_drivers_core::changeHardwareState( change_hardware_state_client_, robot_model_, @@ -172,7 +168,13 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) { RCLCPP_ERROR(get_logger(), "Could not activate configuration controllers"); return FAILURE; - } + } + + // Publish the values of the cartesian impedance parameters to the controller + setImpedanceConfiguration(cart_imp_pub_, cartesian_stiffness_, cartesian_damping_); + // Publish the values of the joint impedance parameters to the controller + setImpedanceConfiguration(joint_imp_pub_, joint_stiffness_, joint_damping_); + is_configured_pub_->on_activate(); is_configured_msg_.data = true; is_configured_pub_->publish(is_configured_msg_); From ba2dafb5ad0d6e62d55c15bb08c7322464ae6056 Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Fri, 20 Sep 2024 10:26:24 +0200 Subject: [PATCH 31/49] add impedance commands to wrench mode --- kuka_sunrise_fri_driver/src/hardware_interface.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 826635a5..d7f0dc7f 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -208,10 +208,8 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta break; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: fri_connection_->setCartesianImpedanceControlMode( - std::vector(6, 0.0), - std::vector(6, 0.1)); // 0.1 is the min accepted value for damping, it probably - // doesn't matter since sthe stiffness is 0 - + hw_cart_stiffness_commands_, + hw_cart_damping_commands_); fri_connection_->setClientCommandMode(ClientCommandModeID::WRENCH_COMMAND_MODE); break; default: From c6a6dd017755d6f8874303bb81e08bb82338fabc Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Fri, 20 Sep 2024 10:48:26 +0200 Subject: [PATCH 32/49] fromat --- kuka_sunrise_fri_driver/src/hardware_interface.cpp | 3 +-- kuka_sunrise_fri_driver/src/robot_manager_node.cpp | 9 ++++----- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index d7f0dc7f..e42dce54 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -208,8 +208,7 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta break; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: fri_connection_->setCartesianImpedanceControlMode( - hw_cart_stiffness_commands_, - hw_cart_damping_commands_); + hw_cart_stiffness_commands_, hw_cart_damping_commands_); fri_connection_->setClientCommandMode(ClientCommandModeID::WRENCH_COMMAND_MODE); break; default: diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index 83670736..479d94ee 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -148,7 +148,7 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) // Publish FRI configuration to notify fri_configuration_controller of initial values setFriConfiguration(send_period_ms_, receive_multiplier_); - + // Configure hardware interface if (!kuka_drivers_core::changeHardwareState( change_hardware_state_client_, robot_model_, @@ -168,13 +168,13 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) { RCLCPP_ERROR(get_logger(), "Could not activate configuration controllers"); return FAILURE; - } + } // Publish the values of the cartesian impedance parameters to the controller setImpedanceConfiguration(cart_imp_pub_, cartesian_stiffness_, cartesian_damping_); // Publish the values of the joint impedance parameters to the controller setImpedanceConfiguration(joint_imp_pub_, joint_stiffness_, joint_damping_); - + is_configured_pub_->on_activate(); is_configured_msg_.data = true; is_configured_pub_->publish(is_configured_msg_); @@ -303,8 +303,7 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) case kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL: break; case kuka_drivers_core::ControlMode::JOINT_IMPEDANCE_CONTROL: - break; - // TODO(Svastits): check whether this is necessary for impedance mode too // (Kristofi) No + break; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: [[fallthrough]]; case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: From f29d295351f737bacf649864300413975c55b87b Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Fri, 20 Sep 2024 11:33:01 +0200 Subject: [PATCH 33/49] removed friction compensation --- .../src/hardware_interface.cpp | 22 ++----------------- 1 file changed, 2 insertions(+), 20 deletions(-) diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index e42dce54..fb52e689 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -360,10 +360,7 @@ void KukaFRIHardwareInterface::updateCommand(const rclcpp::Time &) { const double * joint_torques_ = hw_torque_commands_.data(); const double * joint_pos = robotState().getMeasuredJointPosition(); - std::array joint_pos_corr; - std::copy(joint_pos, joint_pos + DOF, joint_pos_corr.begin()); - activateFrictionCompensation(joint_pos_corr.data()); - robotCommand().setJointPosition(joint_pos_corr.data()); + robotCommand().setJointPosition(joint_pos); robotCommand().setTorque(joint_torques_); break; } @@ -371,10 +368,7 @@ void KukaFRIHardwareInterface::updateCommand(const rclcpp::Time &) { const double * wrench_efforts = hw_wrench_commands_.data(); const double * joint_pos = robotState().getMeasuredJointPosition(); - std::array joint_pos_corr; - std::copy(joint_pos, joint_pos + DOF, joint_pos_corr.begin()); - activateFrictionCompensation(joint_pos_corr.data()); - robotCommand().setJointPosition(joint_pos_corr.data()); + robotCommand().setJointPosition(joint_pos); robotCommand().setWrench(wrench_efforts); break; } @@ -496,18 +490,6 @@ KukaFRIHardwareInterface::export_command_interfaces() return command_interfaces; } -// Friction compensation is activated only if the commanded and measured joint positions differ -void KukaFRIHardwareInterface::activateFrictionCompensation(double * values) const -{ - for (int i = 0; i < DOF; i++) - { - if (values[i] != 0.0) - { // Check for division by zero (very unlikely, but can happen after mastering) - values[i] -= (values[i] / fabs(values[i]) * 0.1); - } - } -} - void KukaFRIHardwareInterface::onError() { std::lock_guard lk(event_mutex_); From dca61796fe61edd2d4cc86a0f8f49182cea02e69 Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Wed, 25 Sep 2024 10:35:15 +0200 Subject: [PATCH 34/49] add cartesian pose control to hwif, rob manager and launch file --- .../hardware_interface_types.hpp | 4 ++ .../cartesian_pose_controller_config.yaml | 13 +++++ .../config/driver_config.yaml | 1 + .../config/ros2_controller_config.yaml | 3 +- .../fri_connection.hpp | 3 +- .../hardware_interface.hpp | 2 + .../robot_manager_node.hpp | 1 + .../launch/startup.launch.py | 10 ++++ kuka_sunrise_fri_driver/package.xml | 1 + .../src/hardware_interface.cpp | 51 +++++++++++++++++-- .../src/robot_manager_node.cpp | 14 +++++ 11 files changed, 96 insertions(+), 7 deletions(-) create mode 100644 kuka_sunrise_fri_driver/config/cartesian_pose_controller_config.yaml diff --git a/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp b/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp index a5b8669c..a3911a58 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp @@ -32,6 +32,10 @@ static constexpr char HW_IF_Z[] = "z"; static constexpr char HW_IF_A[] = "a"; static constexpr char HW_IF_B[] = "b"; static constexpr char HW_IF_C[] = "c"; +static constexpr char HW_IF_QX[] = "qx"; +static constexpr char HW_IF_QY[] = "qy"; +static constexpr char HW_IF_QZ[] = "qz"; +static constexpr char HW_IF_QW[] = "qw"; static constexpr char HW_IF_CART_PREFIX[] = "dummy_cart_joint"; /* Interface prefixes */ diff --git a/kuka_sunrise_fri_driver/config/cartesian_pose_controller_config.yaml b/kuka_sunrise_fri_driver/config/cartesian_pose_controller_config.yaml new file mode 100644 index 00000000..b44be889 --- /dev/null +++ b/kuka_sunrise_fri_driver/config/cartesian_pose_controller_config.yaml @@ -0,0 +1,13 @@ +cartesian_pose_controller: + ros__parameters: + joints: + - dummy_cart_joint/x + - dummy_cart_joint/y + - dummy_cart_joint/z + - dummy_cart_joint/qx + - dummy_cart_joint/qy + - dummy_cart_joint/qz + - dummy_cart_joint/qw + + interface_names: + - position \ No newline at end of file diff --git a/kuka_sunrise_fri_driver/config/driver_config.yaml b/kuka_sunrise_fri_driver/config/driver_config.yaml index 8224b86c..f921b60c 100644 --- a/kuka_sunrise_fri_driver/config/driver_config.yaml +++ b/kuka_sunrise_fri_driver/config/driver_config.yaml @@ -4,6 +4,7 @@ robot_manager: position_controller_name: "joint_trajectory_controller" torque_controller_name: "effort_controller" wrench_controller_name: "wrench_controller" + cart_pose_controller_name: "cartesian_pose_controller" receive_multiplier: 1 send_period_ms: 5 joint_damping: [0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7] diff --git a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml b/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml index bc10789f..a5155298 100755 --- a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml @@ -14,7 +14,8 @@ controller_manager: type: kuka_controllers/EventBroadcaster wrench_controller: type: kuka_controllers/WrenchController - + cartesian_pose_controller: + type: kuka_controllers/CartPoseController # Configuration controllers joint_group_impedance_controller: type: kuka_controllers/JointGroupImpedanceController diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/fri_connection.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/fri_connection.hpp index 62d396a8..24540bb2 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/fri_connection.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/fri_connection.hpp @@ -68,7 +68,8 @@ enum ClientCommandModeID : std::uint8_t { POSITION_COMMAND_MODE = 1, WRENCH_COMMAND_MODE = 2, - TORQUE_COMMAND_MODE = 3 + TORQUE_COMMAND_MODE = 3, + CARTESIAN_POSE_COMMAND_MODE = 4 }; static const std::vector FRI_CONFIG_HEADER = {0xAC, 0xED, 0x00, 0x05, 0x77, 0x10}; diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp index c925fa39..8ef55dce 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp @@ -127,9 +127,11 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, std::vector hw_joint_stiffness_commands_; std::vector hw_joint_damping_commands_; std::vector hw_wrench_commands_; + std::vector hw_cart_pose_commands_; std::vector hw_cart_stiffness_commands_; std::vector hw_cart_damping_commands_; + std::vector hw_cart_pose_states_; std::vector hw_position_states_; std::vector hw_torque_states_; std::vector hw_ext_torque_states_; diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp index 3fd244cb..540be562 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp @@ -80,6 +80,7 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode std::string joint_pos_controller_name_; std::string joint_torque_controller_name_; std::string wrench_controller_name_; + std::string cart_pose_controller_name_; std::vector joint_stiffness_ = std::vector(7, 100.0); std::vector joint_damping_ = std::vector(7, 0.7); std::vector cartesian_stiffness_ = {2000.0, 2000.0, 2000.0, 200.0, 200.0, 200.0}; diff --git a/kuka_sunrise_fri_driver/launch/startup.launch.py b/kuka_sunrise_fri_driver/launch/startup.launch.py index 2f85e337..2efc6cc1 100644 --- a/kuka_sunrise_fri_driver/launch/startup.launch.py +++ b/kuka_sunrise_fri_driver/launch/startup.launch.py @@ -40,6 +40,7 @@ def launch_setup(context, *args, **kwargs): jic_config = LaunchConfiguration("jic_config") ec_config = LaunchConfiguration("ec_config") wc_config = LaunchConfiguration("wc_config") + cpc_config = LaunchConfiguration("cpc_config") cic_config = LaunchConfiguration("cic_config") if ns.perform(context) == "": @@ -121,6 +122,7 @@ def launch_setup(context, *args, **kwargs): ec_config, cic_config, wc_config, + cpc_config, { "hardware_components_initial_state": { "unconfigured": [tf_prefix + robot_model.perform(context)] @@ -173,6 +175,7 @@ def controller_spawner(controller_names, activate=False): "event_broadcaster", "cartesian_impedance_controller", "wrench_controller", + "cartesian_pose_controller", ] controller_spawners = [controller_spawner(name) for name in controller_names] @@ -236,6 +239,13 @@ def generate_launch_description(): + "/config/wrench_controller_config.yaml", ) ) + launch_arguments.append( + DeclareLaunchArgument( + "cpc_config", + default_value=get_package_share_directory("kuka_sunrise_fri_driver") + + "/config/cartesian_pose_controller_config.yaml", + ) + ) launch_arguments.append( DeclareLaunchArgument( "cic_config", diff --git a/kuka_sunrise_fri_driver/package.xml b/kuka_sunrise_fri_driver/package.xml index e804a68c..cc09976e 100644 --- a/kuka_sunrise_fri_driver/package.xml +++ b/kuka_sunrise_fri_driver/package.xml @@ -30,6 +30,7 @@ joint_group_impedance_controller kuka_lbr_iiwa_support wrench_controller + cart_pose_controller ros2lifecycle launch_testing_ament_cmake diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index fb52e689..54b5e58e 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -42,6 +42,9 @@ CallbackReturn KukaFRIHardwareInterface::on_init( hw_torque_states_.resize(info_.joints.size()); hw_ext_torque_states_.resize(info_.joints.size()); hw_torque_commands_.resize(info_.joints.size()); + hw_cart_pose_states_.resize(7);// it's always 7 dof: position x,y,z; orientation quaternion qx,qy,qz,qw + hw_cart_pose_states_.resize(7); + hw_wrench_commands_.resize(6); // it's always 6 dof: force x,y,z; torque x,y,z hw_cart_stiffness_commands_.resize(6, 150); hw_cart_damping_commands_.resize(6, 0.15); @@ -206,6 +209,11 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta std::vector(DOF, 0.0), std::vector(DOF, 0.0)); fri_connection_->setClientCommandMode(ClientCommandModeID::TORQUE_COMMAND_MODE); break; + case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL: + fri_connection_->setClientCommandMode(ClientCommandModeID::CARTESIAN_POSE_COMMAND_MODE); + fri_connection_->setCartesianImpedanceControlMode( + hw_cart_stiffness_commands_, hw_cart_damping_commands_); + break; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: fri_connection_->setCartesianImpedanceControlMode( hw_cart_stiffness_commands_, hw_cart_damping_commands_); @@ -256,6 +264,7 @@ void KukaFRIHardwareInterface::waitForCommand() // Therefore the control signal should not be modified in this callback // TODO(Svastits): check for torque/impedance mode, where state can change hw_position_commands_ = hw_position_states_; + hw_cart_pose_commands_ = hw_cart_pose_states_; rclcpp::Time stamp = ros_clock_.now(); updateCommand(stamp); } @@ -283,7 +292,8 @@ hardware_interface::return_type KukaFRIHardwareInterface::read( const double * external_torque = robotState().getExternalTorque(); hw_ext_torque_states_.assign( external_torque, external_torque + KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - + const double * cart_pose = robotState().getMeasuredCartesianPose(); + hw_cart_pose_states_.assign(cart_pose, cart_pose + KUKA::FRI::LBRState::NUMBER_OF_JOINTS); robot_state_.tracking_performance_ = robotState().getTrackingPerformance(); robot_state_.session_state_ = robotState().getSessionState(); robot_state_.connection_quality_ = robotState().getConnectionQuality(); @@ -364,6 +374,17 @@ void KukaFRIHardwareInterface::updateCommand(const rclcpp::Time &) robotCommand().setTorque(joint_torques_); break; } + case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL: + { + const double * cartesianPoseQuaternion = hw_cart_pose_commands_.data(); + robotCommand().setCartesianPose(cartesianPoseQuaternion); + RCLCPP_ERROR( + rclcpp::get_logger("KukaFRIHardwareInterface"), + "cartesianPoseQuaternion: %f %f %f %f %f %f %f",cartesianPoseQuaternion[0],cartesianPoseQuaternion[1], + cartesianPoseQuaternion[2],cartesianPoseQuaternion[3],cartesianPoseQuaternion[4],cartesianPoseQuaternion[5] + ,cartesianPoseQuaternion[6]); + break; + } case kuka_drivers_core::ControlMode::WRENCH_CONTROL: { const double * wrench_efforts = hw_wrench_commands_.data(); @@ -435,6 +456,16 @@ std::vector KukaFRIHardwareInterface::export state_interfaces.emplace_back( info_.joints[i].name, hardware_interface::HW_IF_EXTERNAL_TORQUE, &hw_ext_torque_states_[i]); } + std::vector cart_joints_list = { + hardware_interface::HW_IF_X, hardware_interface::HW_IF_Y, hardware_interface::HW_IF_Z, + hardware_interface::HW_IF_QX, hardware_interface::HW_IF_QY, hardware_interface::HW_IF_QZ, + hardware_interface::HW_IF_QW}; + for (size_t i = 0; i < cart_joints_list.size(); ++i) + { + state_interfaces.emplace_back( + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + std::string(cart_joints_list[i]), + hardware_interface::HW_IF_POSITION, &hw_cart_pose_states_[i]); + } state_interfaces.emplace_back( hardware_interface::STATE_PREFIX, hardware_interface::SERVER_STATE, &server_state_); @@ -474,17 +505,27 @@ KukaFRIHardwareInterface::export_command_interfaces() } std::vector cart_joints_list = { hardware_interface::HW_IF_X, hardware_interface::HW_IF_Y, hardware_interface::HW_IF_Z, - hardware_interface::HW_IF_A, hardware_interface::HW_IF_B, hardware_interface::HW_IF_C}; - for (size_t i = 0; i < cart_joints_list.size(); i++) + hardware_interface::HW_IF_QX, hardware_interface::HW_IF_QY, hardware_interface::HW_IF_QZ, + hardware_interface::HW_IF_QW}; + for (size_t i = 0; i < cart_joints_list.size(); ++i) { command_interfaces.emplace_back( std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + std::string(cart_joints_list[i]), + hardware_interface::HW_IF_POSITION, &hw_cart_pose_commands_[i]); + } + std::vector cart_effort_list = { + hardware_interface::HW_IF_X, hardware_interface::HW_IF_Y, hardware_interface::HW_IF_Z, + hardware_interface::HW_IF_A, hardware_interface::HW_IF_B, hardware_interface::HW_IF_C}; + for (size_t i = 0; i < cart_effort_list.size(); i++) + { + command_interfaces.emplace_back( + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + std::string(cart_effort_list[i]), hardware_interface::HW_IF_EFFORT, &hw_wrench_commands_[i]); command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + std::string(cart_joints_list[i]), + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + std::string(cart_effort_list[i]), hardware_interface::HW_IF_STIFFNESS, &hw_cart_stiffness_commands_[i]); command_interfaces.emplace_back( - std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + std::string(cart_joints_list[i]), + std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + std::string(cart_effort_list[i]), hardware_interface::HW_IF_DAMPING, &hw_cart_damping_commands_[i]); } return command_interfaces; diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index 479d94ee..a8679718 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -116,6 +116,13 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ return this->onControllerNameChangeRequest( controller_name, kuka_drivers_core::ControllerType::WRENCH_CONTROLLER_TYPE); }); + registerParameter( + "cart_pose_controller_name", "", kuka_drivers_core::ParameterSetAccessRights{true, false}, + [this](const std::string & controller_name) + { + return this->onControllerNameChangeRequest( + controller_name, kuka_drivers_core::ControllerType::CARTESIAN_POSITION_CONTROLLER_TYPE); + }); registerParameter>( "joint_stiffness", joint_stiffness_, kuka_drivers_core::ParameterSetAccessRights{false, false}, @@ -304,6 +311,8 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) break; case kuka_drivers_core::ControlMode::JOINT_IMPEDANCE_CONTROL: break; + case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL: + break; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: [[fallthrough]]; case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: @@ -410,6 +419,9 @@ bool RobotManagerNode::onControllerNameChangeRequest( case kuka_drivers_core::ControllerType::TORQUE_CONTROLLER_TYPE: joint_torque_controller_name_ = controller_name; break; + case kuka_drivers_core::ControllerType::CARTESIAN_POSITION_CONTROLLER_TYPE: + cart_pose_controller_name_ = controller_name; + break; case kuka_drivers_core::ControllerType::WRENCH_CONTROLLER_TYPE: wrench_controller_name_ = controller_name; break; @@ -433,6 +445,8 @@ std::string RobotManagerNode::GetControllerName() const return joint_pos_controller_name_; case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: return joint_torque_controller_name_; + case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL: + return cart_pose_controller_name_; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: return wrench_controller_name_; default: From dc1bf8d7179182dc25da54684866636493bc645a Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Wed, 25 Sep 2024 10:36:26 +0200 Subject: [PATCH 35/49] add feed forward cart pose controller --- .../cart_pose_controller/CHANGELOG.rst | 8 ++ .../cart_pose_controller/CMakeLists.txt | 69 +++++++++ .../controller_plugins.xml | 7 + .../cart_pose_controller.hpp | 45 ++++++ .../cart_pose_controller/visibility_control.h | 49 +++++++ controllers/cart_pose_controller/package.xml | 22 +++ .../src/cart_pose_controller.cpp | 133 ++++++++++++++++++ .../src/cart_pose_controller_parameters.yaml | 11 ++ controllers/kuka_controllers/package.xml | 1 + 9 files changed, 345 insertions(+) create mode 100644 controllers/cart_pose_controller/CHANGELOG.rst create mode 100644 controllers/cart_pose_controller/CMakeLists.txt create mode 100644 controllers/cart_pose_controller/controller_plugins.xml create mode 100644 controllers/cart_pose_controller/include/cart_pose_controller/cart_pose_controller.hpp create mode 100644 controllers/cart_pose_controller/include/cart_pose_controller/visibility_control.h create mode 100644 controllers/cart_pose_controller/package.xml create mode 100644 controllers/cart_pose_controller/src/cart_pose_controller.cpp create mode 100644 controllers/cart_pose_controller/src/cart_pose_controller_parameters.yaml diff --git a/controllers/cart_pose_controller/CHANGELOG.rst b/controllers/cart_pose_controller/CHANGELOG.rst new file mode 100644 index 00000000..540e9276 --- /dev/null +++ b/controllers/cart_pose_controller/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package cart_pose_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.9.0 (2024-09-23) +------------------ +* Add controller for controlling cartesian pose +* Contributors: Mihaly Kristofi diff --git a/controllers/cart_pose_controller/CMakeLists.txt b/controllers/cart_pose_controller/CMakeLists.txt new file mode 100644 index 00000000..6a1cef7c --- /dev/null +++ b/controllers/cart_pose_controller/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.5) +project(cart_pose_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(pluginlib REQUIRED) +find_package(forward_command_controller) +find_package(kuka_drivers_core) +find_package(generate_parameter_library) + +include_directories(include) + +generate_parameter_library( + cart_pose_controller_parameters + src/cart_pose_controller_parameters.yaml +) + +add_library(${PROJECT_NAME} SHARED + src/cart_pose_controller.cpp) + +target_include_directories(${PROJECT_NAME} PRIVATE + include +) + +ament_target_dependencies(${PROJECT_NAME} forward_command_controller kuka_drivers_core +) +target_link_libraries(${PROJECT_NAME} cart_pose_controller_parameters) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "CART_POSE_CONTROLLER_BUILDING_LIBRARY") +# prevent pluginlib from using boost +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include +) + +install(FILES controller_plugins.xml + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + +endif() + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + +ament_export_include_directories( + include +) + +ament_export_libraries( + ${PROJECT_NAME} +) + +ament_package() diff --git a/controllers/cart_pose_controller/controller_plugins.xml b/controllers/cart_pose_controller/controller_plugins.xml new file mode 100644 index 00000000..e9ef07d1 --- /dev/null +++ b/controllers/cart_pose_controller/controller_plugins.xml @@ -0,0 +1,7 @@ + + + + This controller forwards the cartesian pose commands in real time + + + diff --git a/controllers/cart_pose_controller/include/cart_pose_controller/cart_pose_controller.hpp b/controllers/cart_pose_controller/include/cart_pose_controller/cart_pose_controller.hpp new file mode 100644 index 00000000..d18e9596 --- /dev/null +++ b/controllers/cart_pose_controller/include/cart_pose_controller/cart_pose_controller.hpp @@ -0,0 +1,45 @@ +// Copyright 2024 Mihaly Kristofi +// +// 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. + +#ifndef CART_POSE_CONTROLLER__CART_POSE_CONTROLLER_HPP_ +#define CART_POSE_CONTROLLER__CART_POSE_CONTROLLER_HPP_ + +#include +#include +#include + +#include "forward_command_controller/multi_interface_forward_command_controller.hpp" +#include "cart_pose_controller/visibility_control.h" +#include "cart_pose_controller_parameters.hpp" + +namespace kuka_controllers +{ +class CartPoseController : public pid_controller::PidController +{ +public: + CART_POSE_CONTROLLER_PUBLIC CartPoseController(); + CART_POSE_CONTROLLER_PUBLIC controller_interface::return_type update(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; + CART_POSE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; + +private: + CART_POSE_CONTROLLER_LOCAL void declare_parameters() override; + CART_POSE_CONTROLLER_LOCAL controller_interface::CallbackReturn read_parameters() override; + using Params = cart_pose_controller::Params; + using ParamListener = cart_pose_controller::ParamListener; + + std::shared_ptr param_listener_; + Params params_; +}; +} // namespace kuka_controllers +#endif // CART_POSE_CONTROLLER__CART_POSE_CONTROLLER_HPP_ diff --git a/controllers/cart_pose_controller/include/cart_pose_controller/visibility_control.h b/controllers/cart_pose_controller/include/cart_pose_controller/visibility_control.h new file mode 100644 index 00000000..102eda36 --- /dev/null +++ b/controllers/cart_pose_controller/include/cart_pose_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2024 Mihaly Kristofi +// +// 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. + +#ifndef CART_POSE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define CART_POSE_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define CART_POSE_CONTROLLER_EXPORT __attribute__((dllexport)) +#define CART_POSE_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define CART_POSE_CONTROLLER_EXPORT __declspec(dllexport) +#define CART_POSE_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef CART_POSE_CONTROLLER_BUILDING_LIBRARY +#define CART_POSE_CONTROLLER_PUBLIC CART_POSE_CONTROLLER_EXPORT +#else +#define CART_POSE_CONTROLLER_PUBLIC CART_POSE_CONTROLLER_IMPORT +#endif +#define CART_POSE_CONTROLLER_PUBLIC_TYPE CART_POSE_CONTROLLER_PUBLIC +#define CART_POSE_CONTROLLER_LOCAL +#else +#define CART_POSE_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define CART_POSE_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define CART_POSE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define CART_POSE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define CART_POSE_CONTROLLER_PUBLIC +#define CART_POSE_CONTROLLER_LOCAL +#endif +#define CART_POSE_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // CART_POSE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/controllers/cart_pose_controller/package.xml b/controllers/cart_pose_controller/package.xml new file mode 100644 index 00000000..f50abd9a --- /dev/null +++ b/controllers/cart_pose_controller/package.xml @@ -0,0 +1,22 @@ + + + + cart_pose_controller + 0.9.2 + Controller for forwarding cartesian pose commands + + Mihaly Kristofi + + Apache-2.0 + + ament_cmake + + forward_command_controller + pluginlib + kuka_drivers_core + generate_parameter_library + + + ament_cmake + + diff --git a/controllers/cart_pose_controller/src/cart_pose_controller.cpp b/controllers/cart_pose_controller/src/cart_pose_controller.cpp new file mode 100644 index 00000000..d2f3baf9 --- /dev/null +++ b/controllers/cart_pose_controller/src/cart_pose_controller.cpp @@ -0,0 +1,133 @@ +// Copyright 2024 Mihaly Kristofi +// +// 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 "pluginlib/class_list_macros.hpp" + +#include "kuka_drivers_core/hardware_interface_types.hpp" + +#include "cart_pose_controller/cart_pose_controller.hpp" + +namespace kuka_controllers +{ + +CartPoseController::CartPoseController() : ForwardControllersBase() {} + +void CartPoseController::declare_parameters() +{ + param_listener_ = std::make_shared(get_node()); +} + +controller_interface::CallbackReturn CartPoseController::read_parameters() +{ + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); + + if (params_.joints.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } + + if (params_.interface_names.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'interfaces' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } + + for (const auto & joint : params_.joints) + { + for (const auto & interface : params_.interface_names) + { + command_interface_types_.emplace_back(joint + "/" + interface); + RCLCPP_ERROR(get_node()->get_logger(), "cart joints: %s %s ",joint.c_str(),interface.c_str()); + + } + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration CartPoseController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names = command_interface_types_; + for (const auto & interface : command_interface_types_) + { + RCLCPP_ERROR(get_node()->get_logger(), "command_interface_types_: %s , %i",interface.c_str(),command_interface_types_.size()); + + } + return command_interfaces_config; +} + +controller_interface::return_type CartPoseController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + auto joint_commands = rt_command_ptr_.readFromRT(); + + // no command received yet + if (!joint_commands || !(*joint_commands)) + { + return controller_interface::return_type::OK; + } + + if ((*joint_commands)->data.size() != command_interfaces_.size()) + { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), *(get_node()->get_clock()), 1000, + "command size (%zu) does not match number of interfaces (%zu)", + (*joint_commands)->data.size(), command_interfaces_.size()); + return controller_interface::return_type::ERROR; + } + + RCLCPP_ERROR(get_node()->get_logger(), "command size: %i %i %f %f %f %f %f %f %f",command_interfaces_.size(), (*joint_commands)->data.size(), + (*joint_commands)->data[0],(*joint_commands)->data[1],(*joint_commands)->data[2],(*joint_commands)->data[3], + (*joint_commands)->data[4],(*joint_commands)->data[5],(*joint_commands)->data[6],(*joint_commands)->data[7]); + +RCLCPP_ERROR(get_node()->get_logger(), "command name: %s ",command_interfaces_.at(0).get_name().c_str()); +RCLCPP_ERROR(get_node()->get_logger(), "command name: %s ",command_interfaces_.at(0).get_interface_name().c_str()); + + RCLCPP_ERROR(get_node()->get_logger(), "command name: %s ",command_interfaces_.at(1).get_name().c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "command name: %s ",command_interfaces_.at(2).get_name().c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "command name: %s ",command_interfaces_.at(3).get_name().c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "command name: %s ",command_interfaces_.at(4).get_name().c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "command name: %s ",command_interfaces_.at(5).get_name().c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "command name: %s ",command_interfaces_.at(6).get_name().c_str()); + + command_interfaces_.at(0).set_value(0.1); + command_interfaces_.at(1).set_value(0.2); + command_interfaces_.at(2).set_value(0.3); + command_interfaces_.at(3).set_value(0.4); + command_interfaces_.at(4).set_value(0.5); + command_interfaces_.at(5).set_value(0.6); + command_interfaces_.at(6).set_value(0.7); + + + for (auto index = 0ul; index < command_interfaces_.size(); index++) + { + RCLCPP_ERROR(get_node()->get_logger(), "cart joints %i: %f ",index, (*joint_commands)->data[index]); + command_interfaces_.at(index).set_value((*joint_commands)->data[index]); + } + + return controller_interface::return_type::OK; +} + +} // namespace kuka_controllers + +PLUGINLIB_EXPORT_CLASS( + kuka_controllers::CartPoseController, controller_interface::ControllerInterface) diff --git a/controllers/cart_pose_controller/src/cart_pose_controller_parameters.yaml b/controllers/cart_pose_controller/src/cart_pose_controller_parameters.yaml new file mode 100644 index 00000000..98e47f29 --- /dev/null +++ b/controllers/cart_pose_controller/src/cart_pose_controller_parameters.yaml @@ -0,0 +1,11 @@ +cart_pose_controller: + joints: { + type: string_array, + default_value: [], + description: "Name of the joints to control", + } + interface_names: { + type: string_array, + default_value: [], + description: "Names of the interfaces to command", + } diff --git a/controllers/kuka_controllers/package.xml b/controllers/kuka_controllers/package.xml index d200989a..e990f44c 100644 --- a/controllers/kuka_controllers/package.xml +++ b/controllers/kuka_controllers/package.xml @@ -14,6 +14,7 @@ fri_state_broadcaster joint_group_impedance_controller wrench_controller + cart_pose_controller ament_cmake From c75439fc70c770438a3c3886dad446bcd2437222 Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Wed, 25 Sep 2024 10:39:22 +0200 Subject: [PATCH 36/49] typo --- .../include/cart_pose_controller/cart_pose_controller.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/controllers/cart_pose_controller/include/cart_pose_controller/cart_pose_controller.hpp b/controllers/cart_pose_controller/include/cart_pose_controller/cart_pose_controller.hpp index d18e9596..533c6c01 100644 --- a/controllers/cart_pose_controller/include/cart_pose_controller/cart_pose_controller.hpp +++ b/controllers/cart_pose_controller/include/cart_pose_controller/cart_pose_controller.hpp @@ -20,12 +20,13 @@ #include #include "forward_command_controller/multi_interface_forward_command_controller.hpp" + #include "cart_pose_controller/visibility_control.h" #include "cart_pose_controller_parameters.hpp" namespace kuka_controllers { -class CartPoseController : public pid_controller::PidController +class CartPoseController : public forward_command_controller::ForwardControllersBase { public: CART_POSE_CONTROLLER_PUBLIC CartPoseController(); From c200b4618be78d547e2545bba6bf6d3db57732c7 Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Wed, 9 Oct 2024 14:25:24 +0200 Subject: [PATCH 37/49] remove debug code --- .../src/cart_pose_controller.cpp | 32 +------------------ 1 file changed, 1 insertion(+), 31 deletions(-) diff --git a/controllers/cart_pose_controller/src/cart_pose_controller.cpp b/controllers/cart_pose_controller/src/cart_pose_controller.cpp index d2f3baf9..edf5801a 100644 --- a/controllers/cart_pose_controller/src/cart_pose_controller.cpp +++ b/controllers/cart_pose_controller/src/cart_pose_controller.cpp @@ -54,8 +54,6 @@ controller_interface::CallbackReturn CartPoseController::read_parameters() for (const auto & interface : params_.interface_names) { command_interface_types_.emplace_back(joint + "/" + interface); - RCLCPP_ERROR(get_node()->get_logger(), "cart joints: %s %s ",joint.c_str(),interface.c_str()); - } } @@ -67,11 +65,6 @@ controller_interface::InterfaceConfiguration CartPoseController::command_interfa controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; command_interfaces_config.names = command_interface_types_; - for (const auto & interface : command_interface_types_) - { - RCLCPP_ERROR(get_node()->get_logger(), "command_interface_types_: %s , %i",interface.c_str(),command_interface_types_.size()); - - } return command_interfaces_config; } @@ -95,32 +88,9 @@ controller_interface::return_type CartPoseController::update( return controller_interface::return_type::ERROR; } - RCLCPP_ERROR(get_node()->get_logger(), "command size: %i %i %f %f %f %f %f %f %f",command_interfaces_.size(), (*joint_commands)->data.size(), - (*joint_commands)->data[0],(*joint_commands)->data[1],(*joint_commands)->data[2],(*joint_commands)->data[3], - (*joint_commands)->data[4],(*joint_commands)->data[5],(*joint_commands)->data[6],(*joint_commands)->data[7]); - -RCLCPP_ERROR(get_node()->get_logger(), "command name: %s ",command_interfaces_.at(0).get_name().c_str()); -RCLCPP_ERROR(get_node()->get_logger(), "command name: %s ",command_interfaces_.at(0).get_interface_name().c_str()); - - RCLCPP_ERROR(get_node()->get_logger(), "command name: %s ",command_interfaces_.at(1).get_name().c_str()); - RCLCPP_ERROR(get_node()->get_logger(), "command name: %s ",command_interfaces_.at(2).get_name().c_str()); - RCLCPP_ERROR(get_node()->get_logger(), "command name: %s ",command_interfaces_.at(3).get_name().c_str()); - RCLCPP_ERROR(get_node()->get_logger(), "command name: %s ",command_interfaces_.at(4).get_name().c_str()); - RCLCPP_ERROR(get_node()->get_logger(), "command name: %s ",command_interfaces_.at(5).get_name().c_str()); - RCLCPP_ERROR(get_node()->get_logger(), "command name: %s ",command_interfaces_.at(6).get_name().c_str()); - - command_interfaces_.at(0).set_value(0.1); - command_interfaces_.at(1).set_value(0.2); - command_interfaces_.at(2).set_value(0.3); - command_interfaces_.at(3).set_value(0.4); - command_interfaces_.at(4).set_value(0.5); - command_interfaces_.at(5).set_value(0.6); - command_interfaces_.at(6).set_value(0.7); - - for (auto index = 0ul; index < command_interfaces_.size(); index++) { - RCLCPP_ERROR(get_node()->get_logger(), "cart joints %i: %f ",index, (*joint_commands)->data[index]); + //RCLCPP_ERROR(get_node()->get_logger(), "cart joints %i: %f ",index, (*joint_commands)->data[index]); command_interfaces_.at(index).set_value((*joint_commands)->data[index]); } From 6ad65f64ce5466813849ef04d950e4f9b1719847 Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Wed, 9 Oct 2024 14:45:00 +0200 Subject: [PATCH 38/49] fix hw command size --- .../src/hardware_interface.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 54b5e58e..a5102a6f 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -43,7 +43,7 @@ CallbackReturn KukaFRIHardwareInterface::on_init( hw_ext_torque_states_.resize(info_.joints.size()); hw_torque_commands_.resize(info_.joints.size()); hw_cart_pose_states_.resize(7);// it's always 7 dof: position x,y,z; orientation quaternion qx,qy,qz,qw - hw_cart_pose_states_.resize(7); + hw_cart_pose_commands_.resize(7); hw_wrench_commands_.resize(6); // it's always 6 dof: force x,y,z; torque x,y,z hw_cart_stiffness_commands_.resize(6, 150); @@ -303,7 +303,11 @@ hardware_interface::return_type KukaFRIHardwareInterface::read( robot_state_.operation_mode_ = robotState().getOperationMode(); robot_state_.drive_state_ = robotState().getDriveState(); robot_state_.overlay_type_ = robotState().getOverlayType(); - + RCLCPP_DEBUG( + rclcpp::get_logger("KukaFRIHardwareInterface"), + "cartesianPoseQuaternion state: %f %f %f %f %f %f %f",cart_pose[0],cart_pose[1], + cart_pose[2],cart_pose[3],cart_pose[4],cart_pose[5] + ,cart_pose[6]); for (auto & output : gpio_outputs_) { output.getValue(); @@ -377,12 +381,13 @@ void KukaFRIHardwareInterface::updateCommand(const rclcpp::Time &) case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL: { const double * cartesianPoseQuaternion = hw_cart_pose_commands_.data(); - robotCommand().setCartesianPose(cartesianPoseQuaternion); - RCLCPP_ERROR( + RCLCPP_DEBUG( rclcpp::get_logger("KukaFRIHardwareInterface"), - "cartesianPoseQuaternion: %f %f %f %f %f %f %f",cartesianPoseQuaternion[0],cartesianPoseQuaternion[1], + "cartesianPoseQuaternion command: %f %f %f %f %f %f %f",cartesianPoseQuaternion[0],cartesianPoseQuaternion[1], cartesianPoseQuaternion[2],cartesianPoseQuaternion[3],cartesianPoseQuaternion[4],cartesianPoseQuaternion[5] ,cartesianPoseQuaternion[6]); + robotCommand().setCartesianPose(cartesianPoseQuaternion); + break; } case kuka_drivers_core::ControlMode::WRENCH_CONTROL: From c5eeb08149834fd7eea053ea38b8be871c301b1c Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Mon, 14 Oct 2024 10:54:27 +0200 Subject: [PATCH 39/49] fix fri control mode to position control --- kuka_sunrise_fri_driver/src/hardware_interface.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index a5102a6f..52c5e6b2 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -210,9 +210,8 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta fri_connection_->setClientCommandMode(ClientCommandModeID::TORQUE_COMMAND_MODE); break; case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL: + fri_connection_->setPositionControlMode(); fri_connection_->setClientCommandMode(ClientCommandModeID::CARTESIAN_POSE_COMMAND_MODE); - fri_connection_->setCartesianImpedanceControlMode( - hw_cart_stiffness_commands_, hw_cart_damping_commands_); break; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: fri_connection_->setCartesianImpedanceControlMode( From bfacd30164c19a2808542847184f3f44d185c04d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Mon, 14 Oct 2024 15:55:13 +0200 Subject: [PATCH 40/49] update wiki --- doc/wiki/3_Sunrise_FRI.md | 6 +++++- doc/wiki/Home.md | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/doc/wiki/3_Sunrise_FRI.md b/doc/wiki/3_Sunrise_FRI.md index 19f8314f..1daae116 100644 --- a/doc/wiki/3_Sunrise_FRI.md +++ b/doc/wiki/3_Sunrise_FRI.md @@ -31,6 +31,7 @@ The parameters in the driver configuration file can be also changed during runti - `joint_damping`, `joint_stiffness` (double vectors): these parameters change the stiffness and damping attributes of joint impedance control mode. The updated values are sent to the hardware interface using the `joint_group_impedance_controller` to adapt to conventions, but it is not possible to change them in active state due to the constraints of FRI. (Therefore the `joint_group_impedance_controller` is deactivated at driver activation.) - `position_controller_name`: The name of the controller (string) that controls the `position` interface of the robot. It can't be changed in active state. - `torque_controller_name`: The name of the controller (string) that controls the `effort` interface of the robot. It can't be changed in active state. +- `wrench_controller_name`: The name of the controller (string) that controls the `wrench` interface of the robot. It can't be changed in active state. ### Usage @@ -49,7 +50,8 @@ This starts the 3 core components of every driver (described in the [Non-real-ti - `joint_group_impedance_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/joint_impedance_controller_config.yaml)) - `effort_controller` (of type `JointGroupEffortController`, [configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/effort_controller_config.yaml)) - [`control_mode_handler`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#control_mode_handler) (no configuration file) - +- `wrench_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/feature/fri_wrench_control/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml)) + 3. After successful startup, the `robot_manager` node has to be activated to start the cyclic communication with the robot controller (before this only a collapsed robot is visible in `rviz`): ``` ros2 lifecycle set robot_manager configure @@ -74,6 +76,8 @@ Both launch files support the following argument: - `jtc_config`: the location of the configuration file for the `joint_trajectory_controller` (defaults to `kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml`) - `jic_config`: the location of the configuration file for the `joint_impedance_controller` (defaults to `kuka_sunrise_fri_driver/config/joint_impedance_controller_config.yaml`) - `ec_config`: the location of the configuration file for the `effort_controller` (defaults to `kuka_sunrise_fri_driver/config/effort_controller_config.yaml`) +- `cic_config`: the location of the configuration file for the `cartesian_impedance_controller` (defaults to `kuka_sunrise_fri_driver/config/cartesian_impedance_controller_config.yaml`) +- `wc_config`: the location of the configuration file for the `wrench_controller` (defaults to `kuka_sunrise_fri_driver/config/wrench_controller_config.yaml`) The `startup_with_rviz.launch.py` additionally contains one argument: diff --git a/doc/wiki/Home.md b/doc/wiki/Home.md index b158cb01..a374fc4d 100644 --- a/doc/wiki/Home.md +++ b/doc/wiki/Home.md @@ -83,7 +83,7 @@ The following table shows the supported features and control modes of each drive |OS | Joint position control | Joint impedance control | Joint velocity control | Joint torque control | Cartesian position control | Cartesian impedance control | Cartesian velocity control | Wrench control| I/O control| |---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:| |KSS| ✓ | ✗ | ✗ | ✗ | | ✗ | ✗ | ✗ | | -|Sunrise| ✓ | ✓ | ✗ | ✓ | | | ✗ | | | +|Sunrise| ✓ | ✓ | ✗ | ✓ | | | ✗ |✓| | |iiQKA| ✓ | ✓ | ✗ | ✓ | ✗ | ✗ | ✗ | ✗ | ✗ | From 080dc21deebd53d5e31a2e3046e1ed60f546f071 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Mon, 14 Oct 2024 16:33:21 +0200 Subject: [PATCH 41/49] udate wiki --- doc/wiki/3_Sunrise_FRI.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/doc/wiki/3_Sunrise_FRI.md b/doc/wiki/3_Sunrise_FRI.md index 58aa9096..f478fe60 100644 --- a/doc/wiki/3_Sunrise_FRI.md +++ b/doc/wiki/3_Sunrise_FRI.md @@ -29,6 +29,7 @@ The parameters in the driver configuration file can be also changed during runti - `receive_multiplier` (integer): this parameter defines the answer rate factor (the client should sends commands in every `receive_multiplier`*`send_period_ms` milliseconds). It must be at least 1 and can be only changed in `inactive` and `configuring` states. - `control_mode`: The enum value of the control mode should be given, which updates the `ControlMode` and `ClientCommandMode` parameters of FRI. It cannot be changed in active state. - `joint_damping`, `joint_stiffness` (double vectors): these parameters change the stiffness and damping attributes of joint impedance control mode. The updated values are sent to the hardware interface using the `joint_group_impedance_controller` to adapt to conventions, but it is not possible to change them in active state due to the constraints of FRI. (Therefore the `joint_group_impedance_controller` is deactivated at driver activation.) +- `cartesian_damping`, `cartesian_stiffness` (double vectors): these parameters change the stiffness and damping attributes of cartesian impedance control and wrench mode. The updated values are sent to the hardware interface using the `caretsian_impedance_controller` to adapt to conventions, but it is not possible to change them in active state due to the constraints of FRI. (Therefore the `cartesian_impedance_controller` is deactivated at driver activation.) - `position_controller_name`: The name of the controller (string) that controls the `position` interface of the robot. It can't be changed in active state. - `torque_controller_name`: The name of the controller (string) that controls the `effort` interface of the robot. It can't be changed in active state. - `wrench_controller_name`: The name of the controller (string) that controls the `wrench` interface of the robot. It can't be changed in active state. @@ -48,10 +49,11 @@ This starts the 3 core components of every driver (described in the [Non-real-ti - [`fri_configuration_controller`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#fri_configuration_controller) (no configuration file) - [`fri_state_broadcaster`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#fri_state_broadcaster) (no configuration file) - `joint_group_impedance_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/joint_impedance_controller_config.yaml)) +- `cartesian_impedance_controller` (of type `JointGroupImpedanceController`,[configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/cartesian_impedance_controller_config.yaml)) - `effort_controller` (of type `JointGroupEffortController`, [configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/effort_controller_config.yaml)) - [`control_mode_handler`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#control_mode_handler) (no configuration file) - `external_torque_broadcaster` (of type `JointStateBroadcaster`, [configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/external_torque_broadcaster_config.yaml), publishes a `JointState` message type on the topic `external_torque_broadcaster/joint_states` containing the measured external torques for every joint) -- `wrench_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/feature/fri_wrench_control/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml)) +- `wrench_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml)) 1. After successful startup, the `robot_manager` node has to be activated to start the cyclic communication with the robot controller (before this only a collapsed robot is visible in `rviz`): ``` From 5e099f0cb6c88063a17afa772f1d23a2e75fe8d8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Mon, 14 Oct 2024 16:49:18 +0200 Subject: [PATCH 42/49] add cart pose mode to robot application --- .../src/ros2/modules/FRIManager.java | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java index 47651f31..17cd6dc9 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java @@ -205,12 +205,23 @@ public CommandResult endFRI(){ } @Override public CommandResult activateControl(){ - FRIJointOverlay friJointOverlay = + System.out.println("ClientCommandMode: " + FRIManager.this._clientCommandMode + "."); + if (FRIManager.this._clientCommandMode==ClientCommandMode.CARTESIAN_POSE) { + FRICartesianOverlay friCartesianOverlay = + new FRICartesianOverlay(FRIManager.this._FRISession); + PositionHold motion = + new PositionHold(FRIManager.this._controlMode, -1, null); + FRIManager.this._motionContainer = + FRIManager.this._lbr.getFlange().moveAsync(motion.addMotionOverlay(friCartesianOverlay)); + } + else { + FRIJointOverlay friJointOverlay = new FRIJointOverlay(FRIManager.this._FRISession, FRIManager.this._clientCommandMode); - PositionHold motion = + PositionHold motion = new PositionHold(FRIManager.this._controlMode, -1, null); - FRIManager.this._motionContainer = + FRIManager.this._motionContainer = FRIManager.this._lbr.getFlange().moveAsync(motion.addMotionOverlay(friJointOverlay)); + } return CommandResult.EXECUTED; } @Override From 419c591d63dcb204bd98cb2a501af52a4bb25293 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Tue, 15 Oct 2024 14:42:21 +0200 Subject: [PATCH 43/49] update wiki --- doc/wiki/Home.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/wiki/Home.md b/doc/wiki/Home.md index a374fc4d..30b3224c 100644 --- a/doc/wiki/Home.md +++ b/doc/wiki/Home.md @@ -83,7 +83,7 @@ The following table shows the supported features and control modes of each drive |OS | Joint position control | Joint impedance control | Joint velocity control | Joint torque control | Cartesian position control | Cartesian impedance control | Cartesian velocity control | Wrench control| I/O control| |---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:| |KSS| ✓ | ✗ | ✗ | ✗ | | ✗ | ✗ | ✗ | | -|Sunrise| ✓ | ✓ | ✗ | ✓ | | | ✗ |✓| | +|Sunrise| ✓ | ✓ | ✗ | ✓ |✓| | ✗ |✓| | |iiQKA| ✓ | ✓ | ✗ | ✓ | ✗ | ✗ | ✗ | ✗ | ✗ | From f26cf0c54b94d8c18527798e97ebbbe81fc2fd11 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Tue, 15 Oct 2024 15:04:46 +0200 Subject: [PATCH 44/49] add cart imp mode to hwif and manager node --- kuka_sunrise_fri_driver/src/hardware_interface.cpp | 7 +++++++ kuka_sunrise_fri_driver/src/robot_manager_node.cpp | 4 ++++ 2 files changed, 11 insertions(+) diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 52c5e6b2..0e1e5fa5 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -213,6 +213,11 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta fri_connection_->setPositionControlMode(); fri_connection_->setClientCommandMode(ClientCommandModeID::CARTESIAN_POSE_COMMAND_MODE); break; + case kuka_drivers_core::ControlMode::CARTESIAN_IMPEDANCE_CONTROL: + fri_connection_->setCartesianImpedanceControlMode( + hw_cart_stiffness_commands_, hw_cart_damping_commands_); + fri_connection_->setClientCommandMode(ClientCommandModeID::CARTESIAN_POSE_COMMAND_MODE); + break; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: fri_connection_->setCartesianImpedanceControlMode( hw_cart_stiffness_commands_, hw_cart_damping_commands_); @@ -378,6 +383,8 @@ void KukaFRIHardwareInterface::updateCommand(const rclcpp::Time &) break; } case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL: + [[fallthrough]]; + case kuka_drivers_core::ControlMode::CARTESIAN_IMPEDANCE_CONTROL: { const double * cartesianPoseQuaternion = hw_cart_pose_commands_.data(); RCLCPP_DEBUG( diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index e59e527c..93760bf5 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -313,6 +313,8 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) break; case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL: break; + case kuka_drivers_core::ControlMode::CARTESIAN_IMPEDANCE_CONTROL: + break; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: [[fallthrough]]; case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: @@ -447,6 +449,8 @@ std::string RobotManagerNode::GetControllerName() const return joint_torque_controller_name_; case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL: return cart_pose_controller_name_; + case kuka_drivers_core::ControlMode::CARTESIAN_IMPEDANCE_CONTROL: + return cart_pose_controller_name_; case kuka_drivers_core::ControlMode::WRENCH_CONTROL: return wrench_controller_name_; default: From f8cf452696e31b951ea8a8b197f800749e813ae8 Mon Sep 17 00:00:00 2001 From: Kristofi Mihaly Date: Tue, 15 Oct 2024 15:52:53 +0200 Subject: [PATCH 45/49] fix quaternion vector to match FRI standard --- .../config/cartesian_pose_controller_config.yaml | 3 ++- kuka_sunrise_fri_driver/src/hardware_interface.cpp | 14 +++++--------- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/kuka_sunrise_fri_driver/config/cartesian_pose_controller_config.yaml b/kuka_sunrise_fri_driver/config/cartesian_pose_controller_config.yaml index b44be889..beca450c 100644 --- a/kuka_sunrise_fri_driver/config/cartesian_pose_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/cartesian_pose_controller_config.yaml @@ -4,10 +4,11 @@ cartesian_pose_controller: - dummy_cart_joint/x - dummy_cart_joint/y - dummy_cart_joint/z + - dummy_cart_joint/qw - dummy_cart_joint/qx - dummy_cart_joint/qy - dummy_cart_joint/qz - - dummy_cart_joint/qw + interface_names: - position \ No newline at end of file diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 52c5e6b2..8e327494 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -302,11 +302,6 @@ hardware_interface::return_type KukaFRIHardwareInterface::read( robot_state_.operation_mode_ = robotState().getOperationMode(); robot_state_.drive_state_ = robotState().getDriveState(); robot_state_.overlay_type_ = robotState().getOverlayType(); - RCLCPP_DEBUG( - rclcpp::get_logger("KukaFRIHardwareInterface"), - "cartesianPoseQuaternion state: %f %f %f %f %f %f %f",cart_pose[0],cart_pose[1], - cart_pose[2],cart_pose[3],cart_pose[4],cart_pose[5] - ,cart_pose[6]); for (auto & output : gpio_outputs_) { output.getValue(); @@ -462,8 +457,8 @@ std::vector KukaFRIHardwareInterface::export } std::vector cart_joints_list = { hardware_interface::HW_IF_X, hardware_interface::HW_IF_Y, hardware_interface::HW_IF_Z, - hardware_interface::HW_IF_QX, hardware_interface::HW_IF_QY, hardware_interface::HW_IF_QZ, - hardware_interface::HW_IF_QW}; + hardware_interface::HW_IF_QW,hardware_interface::HW_IF_QX, hardware_interface::HW_IF_QY, + hardware_interface::HW_IF_QZ}; for (size_t i = 0; i < cart_joints_list.size(); ++i) { state_interfaces.emplace_back( @@ -509,8 +504,9 @@ KukaFRIHardwareInterface::export_command_interfaces() } std::vector cart_joints_list = { hardware_interface::HW_IF_X, hardware_interface::HW_IF_Y, hardware_interface::HW_IF_Z, - hardware_interface::HW_IF_QX, hardware_interface::HW_IF_QY, hardware_interface::HW_IF_QZ, - hardware_interface::HW_IF_QW}; + hardware_interface::HW_IF_QW, hardware_interface::HW_IF_QX, hardware_interface::HW_IF_QY, + hardware_interface::HW_IF_QZ, + }; for (size_t i = 0; i < cart_joints_list.size(); ++i) { command_interfaces.emplace_back( From b0c10eab8a1d1b60b010c827a060a4e57faabde1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Thu, 17 Oct 2024 11:58:43 +0200 Subject: [PATCH 46/49] format --- .../cart_pose_controller.hpp | 6 +- .../src/cart_pose_controller.cpp | 6 +- doc/wiki/3_Sunrise_FRI.md | 2 +- .../cartesian_pose_controller_config.yaml | 6 +- .../fri_client_sdk/friClientApplication.h | 39 +++-- .../include/fri_client_sdk/friClientIf.h | 64 ++++---- .../include/fri_client_sdk/friConnectionIf.h | 46 +++--- .../include/fri_client_sdk/friDataHelper.h | 10 +- .../include/fri_client_sdk/friException.h | 54 +++---- .../include/fri_client_sdk/friLBRClient.h | 54 +++---- .../include/fri_client_sdk/friLBRCommand.h | 60 +++---- .../include/fri_client_sdk/friLBRState.h | 102 ++++++------ .../fri_client_sdk/friTransformationClient.h | 148 +++++++++--------- .../include/fri_client_sdk/friUdpConnection.h | 56 +++---- .../launch/startup.launch.py | 2 +- .../src/application/ROS2_Control.java | 2 +- .../src/ros2/modules/FRIManager.java | 6 +- .../src/ros2/modules/ROS2Connection.java | 2 +- ...ianImpedanceControlModeExternalizable.java | 16 +- .../ros2/serialization/ControlModeParams.java | 6 +- .../src/fri_client_sdk/FRIMessages.pb.c | 1 - .../src/fri_client_sdk/FRIMessages.pb.h | 62 ++++---- .../fri_client_sdk/friClientApplication.cpp | 47 +++--- .../src/fri_client_sdk/friClientData.h | 66 ++++---- .../friCommandMessageEncoder.cpp | 16 +- .../fri_client_sdk/friCommandMessageEncoder.h | 34 ++-- .../src/fri_client_sdk/friDataHelper.cpp | 14 +- .../src/fri_client_sdk/friLBRClient.cpp | 19 ++- .../src/fri_client_sdk/friLBRCommand.cpp | 16 +- .../src/fri_client_sdk/friLBRState.cpp | 10 +- .../friMonitoringMessageDecoder.cpp | 34 ++-- .../friMonitoringMessageDecoder.h | 32 ++-- .../friTransformationClient.cpp | 20 +-- .../src/fri_client_sdk/friUdpConnection.cpp | 38 ++--- .../fri_client_sdk/pb_frimessages_callbacks.c | 16 +- .../fri_client_sdk/pb_frimessages_callbacks.h | 14 +- .../src/hardware_interface.cpp | 23 +-- 37 files changed, 575 insertions(+), 574 deletions(-) diff --git a/controllers/cart_pose_controller/include/cart_pose_controller/cart_pose_controller.hpp b/controllers/cart_pose_controller/include/cart_pose_controller/cart_pose_controller.hpp index 533c6c01..4df9deca 100644 --- a/controllers/cart_pose_controller/include/cart_pose_controller/cart_pose_controller.hpp +++ b/controllers/cart_pose_controller/include/cart_pose_controller/cart_pose_controller.hpp @@ -30,8 +30,10 @@ class CartPoseController : public forward_command_controller::ForwardControllers { public: CART_POSE_CONTROLLER_PUBLIC CartPoseController(); - CART_POSE_CONTROLLER_PUBLIC controller_interface::return_type update(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; - CART_POSE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; + CART_POSE_CONTROLLER_PUBLIC controller_interface::return_type update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; + CART_POSE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration + command_interface_configuration() const override; private: CART_POSE_CONTROLLER_LOCAL void declare_parameters() override; diff --git a/controllers/cart_pose_controller/src/cart_pose_controller.cpp b/controllers/cart_pose_controller/src/cart_pose_controller.cpp index edf5801a..987709f6 100644 --- a/controllers/cart_pose_controller/src/cart_pose_controller.cpp +++ b/controllers/cart_pose_controller/src/cart_pose_controller.cpp @@ -60,7 +60,8 @@ controller_interface::CallbackReturn CartPoseController::read_parameters() return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::InterfaceConfiguration CartPoseController::command_interface_configuration() const +controller_interface::InterfaceConfiguration CartPoseController::command_interface_configuration() + const { controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -90,7 +91,8 @@ controller_interface::return_type CartPoseController::update( for (auto index = 0ul; index < command_interfaces_.size(); index++) { - //RCLCPP_ERROR(get_node()->get_logger(), "cart joints %i: %f ",index, (*joint_commands)->data[index]); + // RCLCPP_ERROR(get_node()->get_logger(), "cart joints %i: %f ",index, + // (*joint_commands)->data[index]); command_interfaces_.at(index).set_value((*joint_commands)->data[index]); } diff --git a/doc/wiki/3_Sunrise_FRI.md b/doc/wiki/3_Sunrise_FRI.md index f478fe60..e9f73059 100644 --- a/doc/wiki/3_Sunrise_FRI.md +++ b/doc/wiki/3_Sunrise_FRI.md @@ -54,7 +54,7 @@ This starts the 3 core components of every driver (described in the [Non-real-ti - [`control_mode_handler`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#control_mode_handler) (no configuration file) - `external_torque_broadcaster` (of type `JointStateBroadcaster`, [configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/external_torque_broadcaster_config.yaml), publishes a `JointState` message type on the topic `external_torque_broadcaster/joint_states` containing the measured external torques for every joint) - `wrench_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml)) - + 1. After successful startup, the `robot_manager` node has to be activated to start the cyclic communication with the robot controller (before this only a collapsed robot is visible in `rviz`): ``` ros2 lifecycle set robot_manager configure diff --git a/kuka_sunrise_fri_driver/config/cartesian_pose_controller_config.yaml b/kuka_sunrise_fri_driver/config/cartesian_pose_controller_config.yaml index beca450c..0ac1954d 100644 --- a/kuka_sunrise_fri_driver/config/cartesian_pose_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/cartesian_pose_controller_config.yaml @@ -8,7 +8,7 @@ cartesian_pose_controller: - dummy_cart_joint/qx - dummy_cart_joint/qy - dummy_cart_joint/qz - - + + interface_names: - - position \ No newline at end of file + - position diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friClientApplication.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friClientApplication.h index 5a61ebd0..47573bff 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friClientApplication.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friClientApplication.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -76,32 +76,32 @@ namespace FRI /** * \brief FRI client application class. - * + * * A client application takes an instance of the IConnection interface and * an instance of an IClient interface to provide the functionality - * needed to set up an FRI client application. It can be used to easily + * needed to set up an FRI client application. It can be used to easily * integrate the FRI client code within other applications. * The algorithmic functionality of an FRI client application is implemented * using the IClient interface. */ class ClientApplication { - + public: - + /** * \brief Constructor without transformation client. - * + * * This constructor takes an instance of the IConnection interface and * an instance of the IClient interface as parameters. * @param connection FRI connection class * @param client FRI client class */ ClientApplication(IConnection& connection, IClient& client); - + /** * \brief Constructor with transformation client. - * + * * This constructor takes an instance of the IConnection interface and * an instance of the IClient interface and an instance of a * TransformationClient as parameters. @@ -113,33 +113,33 @@ namespace FRI /** \brief Destructor. */ ~ClientApplication(); - + /** * \brief Connect the FRI client application with a KUKA Sunrise controller. - * + * * @param port The port ID * @param remoteHost The address of the remote host * @return True if connection was established */ bool connect(int port, const char *remoteHost = NULL); - + /** * \brief Disconnect the FRI client application from a KUKA Sunrise controller. */ void disconnect(); - + /** * \brief Run a single processing step. - * + * * The processing step consists of receiving a new FRI monitoring message, - * calling the corresponding client callback and sending the resulting + * calling the corresponding client callback and sending the resulting * FRI command message back to the KUKA Sunrise controller. * @return True if all of the substeps succeeded. */ bool step(); protected: - + IConnection& _connection; //!< connection interface IClient* _robotClient; //!< robot client interface TransformationClient* _trafoClient; //!< transformation client interface @@ -152,4 +152,3 @@ namespace FRI #endif // _KUKA_FRI_CLIENT_APPLICATION_H - diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friClientIf.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friClientIf.h index 1a2abda9..f15b2d98 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friClientIf.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friClientIf.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -71,7 +71,7 @@ namespace FRI // forward declarations struct ClientData; - + /** \brief Enumeration of the FRI session state. */ enum ESessionState @@ -116,7 +116,7 @@ namespace FRI TRANSITIONING = 1, //!< drive is in a transitioning state (before or after motion) ACTIVE = 2 //!< drive is being actively commanded }; - + /** \brief Enumeration of control mode. */ enum EControlMode { @@ -125,84 +125,84 @@ namespace FRI JOINT_IMP_CONTROL_MODE = 2, //!< joint impedance control mode NO_CONTROL = 3 //!< drives are not used }; - - + + /** \brief Enumeration of the client command mode. */ enum EClientCommandMode { NO_COMMAND_MODE = 0, //!< no client command mode available JOINT_POSITION = 1, //!< commanding joint positions by the client WRENCH = 2, //!< commanding wrenches and joint positions by the client - TORQUE = 3, //!< commanding joint torques and joint positions by the client - CARTESIAN_POSE = 4 //!< commanding Cartesian poses by the client + TORQUE = 3, //!< commanding joint torques and joint positions by the client + CARTESIAN_POSE = 4 //!< commanding Cartesian poses by the client }; - + /** \brief Enumeration of the overlay type. */ enum EOverlayType { NO_OVERLAY = 0, //!< no overlay type available - JOINT = 1, //!< joint overlay + JOINT = 1, //!< joint overlay CARTESIAN = 2 //!< cartesian overlay }; - + /** \brief Enumeration of redundancy strategies. */ enum ERedundancyStrategy { E1 = 0, //!< E1 redundancy strategy NO_STRATEGY = 4 //!< No redundancy strategy }; - + /** - * \brief FRI client interface. - * - * This is the callback interface that should be implemented by FRI clients. + * \brief FRI client interface. + * + * This is the callback interface that should be implemented by FRI clients. * Callbacks are automatically called by the client application * (ClientApplication) whenever new FRI messages arrive. */ class IClient { friend class ClientApplication; - + public: - + /** \brief Virtual destructor. */ virtual ~IClient() {} - - /** + + /** * \brief Callback that is called whenever the FRI session state changes. - * + * * @param oldState previous FRI session state * @param newState current FRI session state */ virtual void onStateChange(ESessionState oldState, ESessionState newState) = 0; - + /** * \brief Callback for the FRI session states 'Monitoring Wait' and 'Monitoring Ready'. */ virtual void monitor() = 0; - + /** * \brief Callback for the FRI session state 'Commanding Wait'. */ virtual void waitForCommand() = 0; - + /** * \brief Callback for the FRI session state 'Commanding'. */ virtual void command() = 0; - + protected: - + /** * \brief Method to create and initialize the client data structure (used internally). - * + * * @return newly allocated client data structure */ virtual ClientData* createData() = 0; - - + + }; - + } } diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friConnectionIf.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friConnectionIf.h index 0cd10666..e1cd32fc 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friConnectionIf.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friConnectionIf.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -66,63 +66,63 @@ namespace KUKA { namespace FRI { - + /** - * \brief FRI connection interface. - * + * \brief FRI connection interface. + * * Connections to the KUKA Sunrise controller have to be implemented using * this interface. */ class IConnection { - + public: - + /** \brief Virtual destructor. */ virtual ~IConnection() {} - + /** * \brief Open a connection to the KUKA Sunrise controller. - * + * * @param port The port ID * @param remoteHost The address of the remote host * @return True if connection was established */ virtual bool open(int port, const char *remoteHost) = 0; - + /** * \brief Close a connection to the KUKA Sunrise controller. */ virtual void close() = 0; - + /** * \brief Checks whether a connection to the KUKA Sunrise controller is established. - * + * * @return True if connection is established */ virtual bool isOpen() const = 0; - + /** - * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. - * + * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. + * * This method blocks until a new message arrives. * @param buffer Pointer to the allocated buffer that will hold the FRI message * @param maxSize Size in bytes of the allocated buffer * @return Number of bytes received (0 when connection was terminated, negative in case of errors) */ virtual int receive(char *buffer, int maxSize) = 0; - + /** * \brief Send a new FRI command message to the KUKA Sunrise controller. - * + * * @param buffer Pointer to the buffer holding the FRI message * @param size Size in bytes of the message to be send * @return True if successful */ - virtual bool send(const char* buffer, int size) = 0; - + virtual bool send(const char* buffer, int size) = 0; + }; - + } } diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friDataHelper.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friDataHelper.h index e287ebbc..50e05c4b 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friDataHelper.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friDataHelper.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friException.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friException.h index ebf33c99..6c9b2713 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friException.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friException.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -67,39 +67,39 @@ namespace KUKA { namespace FRI { - + /** * \brief Standard exception for the FRI Client - * - * \note For realtime considerations the internal message buffer is static. - * So don't use this exception in more than one thread per process. + * + * \note For realtime considerations the internal message buffer is static. + * So don't use this exception in more than one thread per process. */ class FRIException { - + public: - + /** * \brief FRIException Constructor - * + * * @param message Error message */ - FRIException(const char* message) - { + FRIException(const char* message) + { strncpy(_buffer, message, sizeof(_buffer) - 1); _buffer[sizeof(_buffer) - 1] = 0; // ensure string termination printf("FRIException: "); printf(_buffer); printf("\n"); } - + /** * \brief FRIException Constructor - * - * @param message Error message which may contain one "%s" parameter + * + * @param message Error message which may contain one "%s" parameter * @param param1 First format parameter for parameter message. */ - FRIException(const char* message, const char* param1) + FRIException(const char* message, const char* param1) { #ifdef _MSC_VER _snprintf( // visual studio compilers (up to VS 2013) only know this method @@ -111,15 +111,15 @@ namespace FRI printf(_buffer); printf("\n"); } - + /** * \brief FRIException Constructor - * - * @param message Error message which may contain two "%s" parameter + * + * @param message Error message which may contain two "%s" parameter * @param param1 First format parameter for parameter message. * @param param2 Second format parameter for parameter message. */ - FRIException(const char* message, const char* param1, const char* param2) + FRIException(const char* message, const char* param1, const char* param2) { #ifdef _MSC_VER _snprintf( // visual studio compilers (up to VS 2013) only know this method @@ -131,21 +131,21 @@ namespace FRI printf(_buffer); printf("\n"); } - + /** * \brief Get error string. * @return Error message stored in the exception. */ const char* getErrorMessage() const { return _buffer; } - + /** \brief Virtual destructor. */ virtual ~FRIException() {} - + protected: static char _buffer[1024]; - + }; - + } } diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRClient.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRClient.h index a1e7bce9..49b9239e 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRClient.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRClient.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -72,72 +72,72 @@ namespace FRI /** * \brief Implementation of the IClient interface for the KUKA LBR (lightweight) robots. - * - * Provides access to the current LBR state and the possibility to send new - * commands to the LBR. + * + * Provides access to the current LBR state and the possibility to send new + * commands to the LBR. */ class LBRClient : public IClient { - + public: - + /** \brief Constructor. */ LBRClient(); - + /** \brief Destructor. */ ~LBRClient(); - - /** + + /** * \brief Callback that is called whenever the FRI session state changes. - * + * * @param oldState previous FRI session state * @param newState current FRI session state */ virtual void onStateChange(ESessionState oldState, ESessionState newState); - + /** * \brief Callback for the FRI session states 'Monitoring Wait' and 'Monitoring Ready'. */ virtual void monitor(); - + /** * \brief Callback for the FRI session state 'Commanding Wait'. */ virtual void waitForCommand(); - + /** * \brief Callback for the FRI session state 'Commanding'. */ virtual void command(); - + /** * \brief Provide read access to the current robot state. - * + * * @return Reference to the LBRState instance */ const LBRState& robotState() const { return _robotState; } - + /** * \brief Provide write access to the robot commands. - * + * * @return Reference to the LBRCommand instance */ LBRCommand& robotCommand() { return _robotCommand; } - + private: - + LBRState _robotState; //!< wrapper class for the FRI monitoring message LBRCommand _robotCommand; //!< wrapper class for the FRI command message - + /** * \brief Method to create and initialize the client data structure (used internally). - * + * * @return newly allocated client data structure */ virtual ClientData* createData(); - + }; - + } } diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRCommand.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRCommand.h index 190771e4..c210f716 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRCommand.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRCommand.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -72,7 +72,7 @@ namespace FRI { /** - * \brief Wrapper class for the FRI command message for a KUKA LBR (leightweight) robot. + * \brief Wrapper class for the FRI command message for a KUKA LBR (lightweight) robot. */ class LBRCommand { @@ -82,7 +82,7 @@ class LBRCommand /** * \brief Set the joint positions for the current interpolation step. - * + * * This method is only effective when the client is in a commanding state. * @param values Array with the new joint positions (in rad) */ @@ -90,30 +90,30 @@ class LBRCommand /** * \brief Set the applied wrench vector of the current interpolation step. - * + * * The wrench vector consists of: * [F_x, F_y, F_z, tau_A, tau_B, tau_C] - * - * F ... forces (in N) applied along the Cartesian axes of the + * + * F ... forces (in N) applied along the Cartesian axes of the * currently used motion center. - * tau ... torques (in Nm) applied along the orientation angles + * tau ... torques (in Nm) applied along the orientation angles * (Euler angles A, B, C) of the currently used motion center. - * + * * This method is only effective when the client is in a commanding state. * The ControlMode of the robot has to be Cartesian impedance control mode. The * Client Command Mode has to be wrench. - * + * * @param wrench Applied Cartesian wrench vector. */ void setWrench(const double* wrench); /** * \brief Set the applied joint torques for the current interpolation step. - * + * * This method is only effective when the client is in a commanding state. * The ControlMode of the robot has to be joint impedance control mode. The * Client Command Mode has to be torque. - * + * * @param torques Array with the applied torque values (in Nm) */ void setTorque(const double* torques); @@ -121,7 +121,7 @@ class LBRCommand /** * \brief Set the Cartesian pose for the current interpolation step. * The pose describes the configured TCP relative to the configured base frame. - * + * * The quaternion vector consists of: [t_x, t_y, t_z, q_w, q_x, q_y, q_z], * where the first three values describe the translation t as a regular 3-dim * vector, while the last four values describe the rotation q as an unit quaternion. @@ -133,9 +133,9 @@ class LBRCommand * [ q_x ] = [ sin (phi/2) * v_x ] * [ q_y ] = [ sin (phi/2) * v_y ] * [ q_z ] = [ sin (phi/2) * v_z ] - * + * * Setting a redundancy value is optional. If no value is provided, the interpolated - * redundancy value is used. So far, only the E1 redundancy strategy is provided. + * redundancy value is used. So far, only the E1 redundancy strategy is provided. * * This method is only effective when the client is in a commanding state. * @@ -151,9 +151,9 @@ class LBRCommand * * The first 3 columns represent a rotational matrix and the 4th column a 3-dim * translation vector for directions x, y, z (in mm). - * + * * Setting a redundancy value is optional. If no value is provided, the interpolated - * redundancy value is used. So far, only the E1 redundancy strategy is provided. + * redundancy value is used. So far, only the E1 redundancy strategy is provided. * * @param cartesianPoseAsMatrix 2-dim double array where the requested 3x4 matrix * should be stored @@ -164,37 +164,37 @@ class LBRCommand /** * \brief Set boolean output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Boolean value to set. + * @param value Boolean value to set. */ void setBooleanIOValue(const char* name, const bool value); /** * \brief Set digital output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Digital value to set. + * @param value Digital value to set. */ void setDigitalIOValue(const char* name, const unsigned long long value); /** * \brief Set analog output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Analog value to set. + * @param value Analog value to set. */ void setAnalogIOValue(const char* name, const double value); protected: - static const int LBRCOMMANDMESSAGEID = 0x34001; //!< type identifier for the FRI command message corresponding to a KUKA LBR robot + static const int LBRCOMMANDMESSAGEID = 0x34001; //!< type identifier for the FRI command message corresponding to a KUKA LBR robot FRICommandMessage* _cmdMessage; //!< FRI command message (protobuf struct) FRIMonitoringMessage* _monMessage; //!< FRI monitoring message (protobuf struct) diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRState.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRState.h index c605c014..2c4433b2 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRState.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRState.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -82,15 +82,15 @@ class LBRState enum { - NUMBER_OF_JOINTS = 7 //!< number of joints of the KUKA LBR robot + NUMBER_OF_JOINTS = 7 //!< number of joints of the KUKA LBR robot }; LBRState(); /** - * \brief Get the sample time in seconds. - * - * This is the period in which the KUKA Sunrise controller is sending + * \brief Get the sample time in seconds. + * + * This is the period in which the KUKA Sunrise controller is sending * FRI packets. * @return sample time in seconds */ @@ -98,35 +98,35 @@ class LBRState /** * \brief Get the current FRI session state. - * + * * @return current FRI session state */ ESessionState getSessionState() const; /** * \brief Get the current FRI connection quality. - * + * * @return current FRI connection quality */ EConnectionQuality getConnectionQuality() const; /** * \brief Get the current safety state of the KUKA Sunrise controller. - * + * * @return current safety state */ ESafetyState getSafetyState() const; /** * \brief Get the current operation mode of the KUKA Sunrise controller. - * + * * @return current operation mode */ EOperationMode getOperationMode() const; /** * \brief Get the accumulated drive state over all drives of the KUKA LBR controller. - * + * * If the drive states differ between drives, the following rule applies: * 1) The drive state is OFF if all drives are OFF. * 2) The drive state is ACTIVE if all drives are ACTIVE. @@ -137,93 +137,93 @@ class LBRState /** * \brief Get the client command mode specified by the client. - * + * * @return the client command mode specified by the client. */ EClientCommandMode getClientCommandMode() const; /** * \brief Get the overlay type specified by the client. - * + * * @return the overlay type specified by the client. */ EOverlayType getOverlayType() const; /** * \brief Get the control mode of the KUKA LBR robot. - * + * * @return current control mode of the KUKA LBR robot. */ EControlMode getControlMode() const; - /** - * \brief Get the timestamp of the current robot state in Unix time. - * + /** + * \brief Get the timestamp of the current robot state in Unix time. + * * This method returns the seconds since 0:00, January 1st, 1970 (UTC). * Use getTimestampNanoSec() to increase your timestamp resolution when - * seconds are insufficient. + * seconds are insufficient. * @return timestamp encoded as Unix time (seconds) */ unsigned int getTimestampSec() const; - /** + /** * \brief Get the nanoseconds elapsed since the last second (in Unix time). - * - * This method complements getTimestampSec() to get a high resolution - * timestamp. + * + * This method complements getTimestampSec() to get a high resolution + * timestamp. * @return timestamp encoded as Unix time (nanoseconds part) */ unsigned int getTimestampNanoSec() const; /** * \brief Get the currently measured joint positions of the robot. - * + * * @return array of the measured joint positions in radians */ const double* getMeasuredJointPosition() const; /** * \brief Get the currently measured joint torques of the robot. - * + * * @return array of the measured torques in Nm */ const double* getMeasuredTorque() const; /** * \brief Get the last commanded joint torques of the robot. - * + * * @return array of the commanded torques in Nm */ const double* getCommandedTorque() const; /** * \brief Get the currently measured external joint torques of the robot. - * + * * The external torques corresponds to the measured torques when removing - * the torques induced by the robot itself. + * the torques induced by the robot itself. * @return array of the external torques in Nm */ const double* getExternalTorque() const; /** * \brief Get the joint positions commanded by the interpolator. - * + * * When commanding a motion overlay in your robot application, this method * will give access to the joint positions currently commanded by the * motion interpolator. * @throw FRIException This method will throw a FRIException if no FRI Joint Overlay is active. - * @return array of the ipo joint positions in radians + * @return array of the ipo joint positions in radians */ const double* getIpoJointPosition() const; /** * \brief Get an indicator for the current tracking performance of the commanded robot. - * + * * The tracking performance is an indicator on how well the commanded robot * is able to follow the commands of the FRI client. The best possible value * 1.0 is reached when the robot executes the given commands instantaneously. - * The tracking performance drops towards 0 when latencies are induced, - * e.g. when the commanded velocity, acceleration or jerk exceeds the + * The tracking performance drops towards 0 when latencies are induced, + * e.g. when the commanded velocity, acceleration or jerk exceeds the * capabilities of the robot. * The tracking performance will always be 0 when the session state does * not equal COMMANDING_ACTIVE. @@ -235,7 +235,7 @@ class LBRState * \brief Get boolean IO value. * * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). * @return Returns IO's boolean value. */ bool getBooleanIOValue(const char* name) const; @@ -244,7 +244,7 @@ class LBRState * \brief Get digital IO value. * * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). * @return Returns IO's digital value. */ unsigned long long getDigitalIOValue(const char* name) const; @@ -253,7 +253,7 @@ class LBRState * \brief Get analog IO value. * * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). * @return Returns IO's analog value. */ double getAnalogIOValue(const char* name) const; @@ -281,7 +281,7 @@ class LBRState const double* getMeasuredCartesianPose() const; /** - * \brief Get the currently measured Cartesian pose of the robot as a 3x4 transformation matrix. + * \brief Get the currently measured Cartesian pose of the robot as a 3x4 transformation matrix. * The pose describes the robot tcp relative to the base frame. * * The first 3 columns represent a rotational matrix and the 4th column a 3-dim @@ -309,15 +309,15 @@ class LBRState * [ q_x ] = [ sin (phi/2) * v_x ] * [ q_y ] = [ sin (phi/2) * v_y ] * [ q_z ] = [ sin (phi/2) * v_z ] - * + * * @throw FRIException This method will throw a FRIException if no FRI Cartesian Overlay is active. * @return ipo Cartesian pose as 7-dim quaternion transformation (translation in mm) */ const double* getIpoCartesianPose() const; /** - * \brief Get the currently interpolated Cartesian pose of the robot as a 3x4 transformation matrix. - * The pose describes the robot tcp relative to the base frame. + * \brief Get the currently interpolated Cartesian pose of the robot as a 3x4 transformation matrix. + * The pose describes the robot tcp relative to the base frame. * * The first 3 columns represent a rotational matrix and the 4th column a 3-dim * translation vector for directions x, y, z (in mm). @@ -329,37 +329,37 @@ class LBRState /** * \brief Get the currently measured redundancy value. - * + * * So far, only the E1 redundancy strategy is provided, meaning the value represents the joint position * of A3 in radians. - * + * * @param measured redundancy value in radians */ double getMeasuredRedundancyValue() const; - + /** * \brief Get the current redundancy value of the interpolator. - * + * * So far, only the E1 redundancy strategy is provided, meaning the value represents the joint position * of A3 in radians. - * + * * @param redundancy value of the interpolator in radians */ double getIpoRedundancyValue() const; - + /** * \brief Get the redundancy strategy. - * + * * So far, only the E1 redundancy strategy is provided, meaning the value represents the joint position * of A3 in radians. - * + * * @param redundancy strategy */ ERedundancyStrategy getRedundancyStrategy() const; protected: - static const int LBRMONITORMESSAGEID = 0x245142; //!< type identifier for the FRI monitoring message corresponding to a KUKA LBR robot + static const int LBRMONITORMESSAGEID = 0x245142; //!< type identifier for the FRI monitoring message corresponding to a KUKA LBR robot FRIMonitoringMessage* _message; //!< FRI monitoring message (protobuf struct) }; diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friTransformationClient.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friTransformationClient.h index c28a8ef2..6580083a 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friTransformationClient.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friTransformationClient.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -73,91 +73,91 @@ namespace FRI struct ClientData; /** - * \brief Abstract FRI transformation client. - * - * A transformation client enables the user to send transformation matrices cyclically to the + * \brief Abstract FRI transformation client. + * + * A transformation client enables the user to send transformation matrices cyclically to the * KUKA Sunrise controller for manipulating the transformations of dynamic frames in the * Sunrise scenegraph. - * Usually, these matrices will be provided by external sensors. + * Usually, these matrices will be provided by external sensors. *
* Custom transformation clients have to be derived from this class and need to * implement the provide() callback. This callback is called once by the * client application whenever a new FRI message arrives. - * + * */ class TransformationClient { - + friend class ClientApplication; - + public: - + /** * \brief Constructor. **/ TransformationClient(); - + /** - * \brief Virtual destructor. + * \brief Virtual destructor. **/ virtual ~TransformationClient(); - - /** - * \brief Callback which is called whenever a new FRI message arrives. + + /** + * \brief Callback which is called whenever a new FRI message arrives. * * In this callback all requested transformations have to be set. - * + * * \see getRequestedTransformationIDs(), setTransformation() */ virtual void provide() = 0; - + /** - * \brief Get the sample time in seconds. - * - * This is the period in which the KUKA Sunrise controller is sending + * \brief Get the sample time in seconds. + * + * This is the period in which the KUKA Sunrise controller is sending * FRI packets. * @return sample time in seconds */ double getSampleTime() const; // sec - + /** * \brief Get the current FRI connection quality. - * + * * @return current FRI connection quality */ - EConnectionQuality getConnectionQuality() const; + EConnectionQuality getConnectionQuality() const; - /** + /** * \brief Returns a vector of identifiers of all requested transformation matrices. - * + * * The custom TransformationClient has to provide data for transformation matrices with these - * identifiers. - * + * identifiers. + * * @return reference to vector of IDs of requested transformations */ const std::vector& getRequestedTransformationIDs() const; - - /** - * \brief Get the timestamp of the current received FRI monitor message in Unix time. - * + + /** + * \brief Get the timestamp of the current received FRI monitor message in Unix time. + * * This method returns the seconds since 0:00, January 1st, 1970 (UTC). * Use getTimestampNanoSec() to increase your timestamp resolution when - * seconds are insufficient. - * + * seconds are insufficient. + * * @return timestamp encoded as Unix time (seconds) */ const unsigned int getTimestampSec() const; - - /** + + /** * \brief Get the nanoseconds elapsed since the last second (in Unix time). - * - * This method complements getTimestampSec() to get a high resolution - * timestamp. - * + * + * This method complements getTimestampSec() to get a high resolution + * timestamp. + * * @return timestamp encoded as Unix time (nanoseconds part) */ const unsigned int getTimestampNanoSec() const; - + /** * \brief Provides a requested transformation as a quaternion transformation vector. * @@ -187,12 +187,12 @@ namespace FRI * @param timeSec Timestamp encoded as Unix time (seconds) * @param timeNanoSec Timestamp encoded as Unix time (nanoseconds part) */ - void setTransformation(const char* transformationID, const double (&transformationQuaternion)[7], + void setTransformation(const char* transformationID, const double (&transformationQuaternion)[7], unsigned int timeSec, unsigned int timeNanoSec); /** * \brief Provides a requested transformation as a homogeneous matrix. - * + * * A transformation matrix has 3x4 elements. It consists of a rotational matrix (3x3 elements) * and a translational vector (3x1 elements). The complete transformation matrix has the * following structure:
@@ -204,88 +204,88 @@ namespace FRI *

* If an update to the last transformation is not yet available when the provide() * callback is executed, the last transformation (including its timestamp) should be - * repeated until a new transformation is available. + * repeated until a new transformation is available. *

* - * @throw FRIException Throws a FRIException if the maximum number of transformations is exceeded. + * @throw FRIException Throws a FRIException if the maximum number of transformations is exceeded. * @param transformationID Identifier string of the transformation matrix * @param transformationMatrix Provided transformation matrix * @param timeSec Timestamp encoded as Unix time (seconds) * @param timeNanoSec Timestamp encoded as Unix time (nanoseconds part) */ - void setTransformation(const char* transformationID, const double (&transformationMatrix)[3][4], + void setTransformation(const char* transformationID, const double (&transformationMatrix)[3][4], unsigned int timeSec, unsigned int timeNanoSec); - + /** * \brief Set boolean output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Boolean value to set. + * @param value Boolean value to set. */ void setBooleanIOValue(const char* name, const bool value); - + /** * \brief Set digital output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Digital value to set. + * @param value Digital value to set. */ void setDigitalIOValue(const char* name, const unsigned long long value); - + /** * \brief Set analog output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Analog value to set. + * @param value Analog value to set. */ - void setAnalogIOValue(const char* name, const double value); - + void setAnalogIOValue(const char* name, const double value); + /** * \brief Get boolean IO value. * * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). * @return Returns IO's boolean value. */ bool getBooleanIOValue(const char* name) const; - + /** * \brief Get digital IO value. * * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). * @return Returns IO's digital value. */ unsigned long long getDigitalIOValue(const char* name) const; - + /** * \brief Get analog IO value. * * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). * @return Returns IO's analog value. */ double getAnalogIOValue(const char* name) const; - - private: - + + private: + ClientData* _data; //!< the client data structure - + /** * \brief Method to link the client data structure (used internally). - * + * * @param clientData the client data structure */ void linkData(ClientData* clientData); - + }; - + } } diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friUdpConnection.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friUdpConnection.h index 04fa98d7..f8172042 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friUdpConnection.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friUdpConnection.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -91,71 +91,71 @@ namespace FRI */ class UdpConnection : public IConnection { - + public: - + /** * \brief Constructor with an optional parameter for setting a receive timeout. - * - * @param receiveTimeout Timeout (in ms) for receiving a UDP message (0 = wait forever) + * + * @param receiveTimeout Timeout (in ms) for receiving a UDP message (0 = wait forever) * */ UdpConnection(unsigned int receiveTimeout = 0); - + /** \brief Destructor. */ ~UdpConnection(); - + /** * \brief Open a connection to the KUKA Sunrise controller. - * - * @param port The port ID for the connection - * @param controllerAddress The IPv4 address of the KUKA Sunrise controller. + * + * @param port The port ID for the connection + * @param controllerAddress The IPv4 address of the KUKA Sunrise controller. * If NULL, the FRI Client accepts connections from any - * address. + * address. * @return True if connection was established, false otherwise */ virtual bool open(int port, const char *controllerAddress = NULL); - + /** * \brief Close a connection to the KUKA Sunrise controller. */ virtual void close(); - + /** * \brief Checks whether a connection to the KUKA Sunrise controller is established. - * + * * @return True if connection is established */ virtual bool isOpen() const; - + /** - * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. - * + * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. + * * This method blocks until a new message arrives. * @param buffer Pointer to the allocated buffer that will hold the FRI message * @param maxSize Size in bytes of the allocated buffer - * @return Number of bytes received (0 when connection was terminated, + * @return Number of bytes received (0 when connection was terminated, * negative in case of errors or receive timeout) */ virtual int receive(char *buffer, int maxSize); - + /** * \brief Send a new FRI command message to the KUKA Sunrise controller. - * + * * @param buffer Pointer to the buffer holding the FRI message * @param size Size in bytes of the message to be send * @return True if successful */ virtual bool send(const char* buffer, int size); - + private: - + int _udpSock; //!< UDP socket handle struct sockaddr_in _controllerAddr; //!< the controller's socket address unsigned int _receiveTimeout; fd_set _filedescriptor; - + }; - + } } diff --git a/kuka_sunrise_fri_driver/launch/startup.launch.py b/kuka_sunrise_fri_driver/launch/startup.launch.py index 6fb06bc3..b7ff4a08 100644 --- a/kuka_sunrise_fri_driver/launch/startup.launch.py +++ b/kuka_sunrise_fri_driver/launch/startup.launch.py @@ -41,7 +41,7 @@ def launch_setup(context, *args, **kwargs): ec_config = LaunchConfiguration("ec_config") etb_config = LaunchConfiguration("etb_config") wc_config = LaunchConfiguration("wc_config") - cpc_config = LaunchConfiguration("cpc_config") + cpc_config = LaunchConfiguration("cpc_config") cic_config = LaunchConfiguration("cic_config") if ns.perform(context) == "": diff --git a/kuka_sunrise_fri_driver/robot_application/src/application/ROS2_Control.java b/kuka_sunrise_fri_driver/robot_application/src/application/ROS2_Control.java index 9adf72cf..a9e70893 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/application/ROS2_Control.java +++ b/kuka_sunrise_fri_driver/robot_application/src/application/ROS2_Control.java @@ -32,7 +32,7 @@ public class ROS2_Control extends RoboticsAPIApplication { @Inject private LBR _lbr; - + private TCPConnection _TCPConnection; private ROS2Connection _ROS2Connection; private FRIManager _FRIManager; diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java index 17cd6dc9..54580ec2 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java @@ -39,10 +39,10 @@ public enum CommandResult{ private ClientCommandMode _clientCommandMode; private IMotionContainer _motionContainer; private FRIMotionErrorHandler _friMotionErrorHandler = new FRIMotionErrorHandler(); - + private static double[] stiffness_ = new double[7]; - - + + public FRIManager(LBR lbr, IApplicationControl applicationControl){ _currentState = new InactiveState(); _lbr = lbr; diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java index d5326d92..ee13136a 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java @@ -95,7 +95,7 @@ private enum ControlModeID{ POSITION( (byte)1), JOINT_IMPEDANCE((byte)2), CARTESIAN_IMPEDANCE((byte)3); - + public final byte value; ControlModeID(byte value){ diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java index 32dca2ce..f49ddef9 100644 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java @@ -11,21 +11,21 @@ public class CartesianImpedanceControlModeExternalizable extends CartesianImpedanceControlMode implements Externalizable{ public final static int length = 96; - - + + public CartesianImpedanceControlModeExternalizable(CartesianImpedanceControlMode other){ super(other); } - + public CartesianImpedanceControlModeExternalizable(){ super(); } - + public IMotionControlMode toControlMode(){ CartesianImpedanceControlMode controlMode = new CartesianImpedanceControlMode((CartesianImpedanceControlMode)this); return (IMotionControlMode)controlMode; } - + @Override public void writeExternal(ObjectOutput out) throws IOException { for(double cartStiffness : getStiffness()){ @@ -39,7 +39,7 @@ public void writeExternal(ObjectOutput out) throws IOException { @Override public void readExternal(ObjectInput in) throws IOException, ClassNotFoundException { - double[] cartStiffness = new double[getStiffness().length]; + double[] cartStiffness = new double[getStiffness().length]; for(int i = 0; i < getStiffness().length; i++){ cartStiffness[i] = in.readDouble(); } @@ -50,7 +50,7 @@ public void readExternal(ObjectInput in) throws IOException, this.parametrize(CartDOF.B).setStiffness(cartStiffness[4]); this.parametrize(CartDOF.C).setStiffness(cartStiffness[5]); - double[] cartDamping = new double[getDamping().length]; + double[] cartDamping = new double[getDamping().length]; for(int i = 0; i < getDamping().length; i++){ cartDamping[i] = in.readDouble(); } @@ -60,7 +60,7 @@ public void readExternal(ObjectInput in) throws IOException, this.parametrize(CartDOF.A).setDamping(cartDamping[3]); this.parametrize(CartDOF.B).setDamping(cartDamping[4]); this.parametrize(CartDOF.C).setDamping(cartDamping[5]); - + } diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java index 88f1dc00..6f72b286 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java @@ -17,7 +17,7 @@ private enum ControlModeID{ POSITION( (byte)1), JOINT_IMPEDANCE((byte)2), CARTESIAN_IMPEDANCE((byte)3); - + public final byte value; ControlModeID(byte value){ @@ -101,9 +101,9 @@ public JointImpedanceControlModeParams(JointImpedanceControlMode controlMode){ class CartesianImpedanceControlModeParams extends ControlModeParams{ public CartesianImpedanceControlModeParams(){ - + } public CartesianImpedanceControlModeParams(CartesianImpedanceControlMode controlMode){ - + } } diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.c b/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.c index 8786d741..e421761c 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.c +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.c @@ -73,4 +73,3 @@ PB_BIND(FRICommandMessage, FRICommandMessage, 2) */ PB_STATIC_ASSERT(sizeof(double) == 8, DOUBLE_MUST_BE_8_BYTES) #endif - diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.h b/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.h index d124ca26..9cdb79bc 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.h +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.h @@ -10,7 +10,7 @@ #endif /* Enum definitions */ -/* used to display the state of the FRI session +/* used to display the state of the FRI session @KUKA.Internal */ typedef enum _FRISessionState { FRISessionState_IDLE = 0, /* idle state. */ @@ -20,7 +20,7 @@ typedef enum _FRISessionState { FRISessionState_COMMANDING_ACTIVE = 4 /* connected to a running motion - commanding modifies the robot path */ } FRISessionState; -/* Quality of the FRI Connection +/* Quality of the FRI Connection @KUKA.Internal */ typedef enum _FRIConnectionQuality { FRIConnectionQuality_POOR = 0, /* Robot commanding is not possible. Initial value of the connection. */ @@ -29,7 +29,7 @@ typedef enum _FRIConnectionQuality { FRIConnectionQuality_EXCELLENT = 3 /* Robot commanding is possible. */ } FRIConnectionQuality; -/* This enumeration is used to indicate the safety State. +/* This enumeration is used to indicate the safety State. @KUKA.Internal */ typedef enum _SafetyState { SafetyState_NORMAL_OPERATION = 0, @@ -38,7 +38,7 @@ typedef enum _SafetyState { SafetyState_SAFETY_STOP_LEVEL_2 = 3 } SafetyState; -/* This enumeration contains the available operation modes of the controller. +/* This enumeration contains the available operation modes of the controller. @KUKA.Internal */ typedef enum _OperationMode { OperationMode_TEST_MODE_1 = 0, @@ -53,7 +53,7 @@ typedef enum _DriveState { DriveState_ACTIVE = 2 } DriveState; -/* This enumeration contains the available control modes of a KUKA robot. +/* This enumeration contains the available control modes of a KUKA robot. @KUKA.Internal */ typedef enum _ControlMode { ControlMode_POSITION_CONTROLMODE = 0, @@ -62,7 +62,7 @@ typedef enum _ControlMode { ControlMode_NO_CONTROLMODE = 3 } ControlMode; -/* This enumeration contains the available client command modes of the interpolator. +/* This enumeration contains the available client command modes of the interpolator. @KUKA.Internal */ typedef enum _ClientCommandMode { ClientCommandMode_NO_COMMAND_MODE = 0, @@ -72,7 +72,7 @@ typedef enum _ClientCommandMode { ClientCommandMode_CARTESIAN_POSE = 4 } ClientCommandMode; -/* This enumeration contains the currently used overlay type. +/* This enumeration contains the currently used overlay type. @KUKA.Internal */ typedef enum _OverlayType { OverlayType_NO_OVERLAY = 0, @@ -80,7 +80,7 @@ typedef enum _OverlayType { OverlayType_CARTESIAN = 2 } OverlayType; -/* IO Type +/* IO Type @KUKA.Internal */ typedef enum _FriIOType { FriIOType_BOOLEAN = 0, @@ -88,14 +88,14 @@ typedef enum _FriIOType { FriIOType_ANALOG = 2 } FriIOType; -/* IO Direction +/* IO Direction @KUKA.Internal */ typedef enum _FriIODirection { FriIODirection_INPUT = 0, FriIODirection_OUTPUT = 1 } FriIODirection; -/* Redundancy strategy +/* Redundancy strategy @KUKA.Internal */ typedef enum _RedundancyStrategy { RedundancyStrategy_E1 = 0, @@ -103,34 +103,34 @@ typedef enum _RedundancyStrategy { } RedundancyStrategy; /* Struct definitions */ -/* Container object for Joint Values +/* Container object for Joint Values @KUKA.Internal */ typedef struct _JointValues { pb_callback_t value; /* value of a single joint. */ } JointValues; -/* Timestamp container. +/* Timestamp container. @KUKA.Internal */ typedef struct _TimeStamp { uint32_t sec; /* Second portion of the timestamp. Time starts at 1.1.1970. */ uint32_t nanosec; /* Nano second portion of the timestamp. Time starts at 1.1.1970. */ } TimeStamp; -/* Cartesian vector container. +/* Cartesian vector container. KUKA.Internal */ typedef struct _CartesianVector { pb_size_t element_count; double element[6]; /* value of a single vector element */ } CartesianVector; -/* Contains possible checksum implementations like CRC32,MD5. +/* Contains possible checksum implementations like CRC32,MD5. @KUKA.Internal */ typedef struct _Checksum { bool has_crc32; int32_t crc32; /* Storage for CRC32. */ } Checksum; -/* Quaternion transformation container. +/* Quaternion transformation container. @KUKA.Internal */ typedef struct _QuaternionTransformation { char name[64]; /* switch to 'required' due to nanopb-bug */ @@ -156,10 +156,10 @@ typedef struct _RedundancyInformation { double value; } RedundancyInformation; -/* FRI Message Header. Contains the information for timing handshake and the message identifier. - The following messageIdentifiers are currently available: - LBR Monitoring Message: 0x245142 - LBR Command Message: 0x34001 +/* FRI Message Header. Contains the information for timing handshake and the message identifier. + The following messageIdentifiers are currently available: + LBR Monitoring Message: 0x245142 + LBR Command Message: 0x34001 @KUKA.Internal */ typedef struct _MessageHeader { uint32_t messageIdentifier; /* Message identifier. */ @@ -167,7 +167,7 @@ typedef struct _MessageHeader { uint32_t reflectedSequenceCounter; /* Reflected sequence counter. Checked to determine the timing. */ } MessageHeader; -/* FRI Connection info. Contains the connection state and additional informations. +/* FRI Connection info. Contains the connection state and additional information. @KUKA.Internal */ typedef struct _ConnectionInfo { FRISessionState sessionState; /* state of the FRI session. */ @@ -178,12 +178,12 @@ typedef struct _ConnectionInfo { uint32_t receiveMultiplier; /* Multiplier of sendPeriod, on which the Controller expects a new CommmandMessage. */ } ConnectionInfo; -/* Robot Information Object. Contains all static Information about the robot. e.g. - Number of Joints. +/* Robot Information Object. Contains all static Information about the robot. e.g. + Number of Joints. @KUKA.Internal */ typedef struct _RobotInfo { bool has_numberOfJoints; - int32_t numberOfJoints; /* availabe number of joints. */ + int32_t numberOfJoints; /* available number of joints. */ bool has_safetyState; SafetyState safetyState; /* Safety state of the controller. */ pb_callback_t driveState; /* Drivestate of the drives. */ @@ -193,7 +193,7 @@ typedef struct _RobotInfo { ControlMode controlMode; /* Controlmode of the robot. */ } RobotInfo; -/* FRI Monitor Data. Contains the cylic Information about the current robot state. +/* FRI Monitor Data. Contains the cyclic Information about the current robot state. @KUKA.Internal */ typedef struct _MessageMonitorData { bool has_measuredJointPosition; @@ -204,7 +204,7 @@ typedef struct _MessageMonitorData { JointValues commandedTorque; /* last commanded torque value. */ bool has_externalTorque; JointValues externalTorque; /* observed external torque. */ - /* optional CartesianVector externalForce = 6; // observed Cartesian external forces and torque in flange coordinates + /* optional CartesianVector externalForce = 6; // observed Cartesian external forces and torque in flange coordinates repeated QuaternionTransformation subscribedTransformations = 7; // selected transformations from controller to client */ pb_size_t readIORequest_count; FriIOValue readIORequest[10]; /* used to read FieldBus input value(s) */ @@ -216,8 +216,8 @@ typedef struct _MessageMonitorData { TimeStamp timestamp; /* timestamp of the measurement. */ } MessageMonitorData; -/* FRI Interpolator Data. Contains the cyclic commands which are going to be send - to the robot by the interpolator. +/* FRI Interpolator Data. Contains the cyclic commands which are going to be send + to the robot by the interpolator. @KUKA.Internal */ typedef struct _MessageIpoData { bool has_jointPosition; @@ -234,7 +234,7 @@ typedef struct _MessageIpoData { double trackingPerformance; /* tracking performance of the commands */ } MessageIpoData; -/* FRI Command Data. Contains the cyclic commands to modify the robot position. +/* FRI Command Data. Contains the cyclic commands to modify the robot position. @KUKA.Internal */ typedef struct _MessageCommandData { bool has_jointPosition; @@ -253,7 +253,7 @@ typedef struct _MessageCommandData { RedundancyInformation redundancyInformation; /* commanded redundancy information */ } MessageCommandData; -/* FRI End of Data. Contains the length and CRC32 of the data before. +/* FRI End of Data. Contains the length and CRC32 of the data before. @KUKA.Internal */ typedef struct _MessageEndOf { bool has_messageLength; @@ -262,7 +262,7 @@ typedef struct _MessageEndOf { Checksum messageChecksum; /* checksum for all data before this point muss be last date. */ } MessageEndOf; -/* FRI Monitoring Message. Contains the cyclic Information of the robot position and state. +/* FRI Monitoring Message. Contains the cyclic Information of the robot position and state. @KUKA.Internal */ typedef struct _FRIMonitoringMessage { MessageHeader header; /* Message header. */ @@ -280,7 +280,7 @@ typedef struct _FRIMonitoringMessage { MessageEndOf endOfMessageData; /* End of Package contains message length and checksum. */ } FRIMonitoringMessage; -/* FRI Command Message. Contains the information of the user to modify the robot commands. +/* FRI Command Message. Contains the information of the user to modify the robot commands. @KUKA.Internal */ typedef struct _FRICommandMessage { MessageHeader header; /* Message header. */ diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientApplication.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientApplication.cpp index 5f26ad21..40ce7a89 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientApplication.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientApplication.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -92,15 +92,15 @@ ClientApplication::~ClientApplication() //****************************************************************************** bool ClientApplication::connect(int port, const char *remoteHost) { - if (_connection.isOpen()) + if (_connection.isOpen()) { printf("Warning: client application already connected!\n"); return true; } - + return _connection.open(port, remoteHost); } - + //****************************************************************************** void ClientApplication::disconnect() { @@ -120,18 +120,18 @@ bool ClientApplication::step() // Receive and decode new monitoring message // ************************************************************************** int size = _connection.receive(_data->receiveBuffer, FRI_MONITOR_MSG_MAX_SIZE); - + if (size <= 0) { // TODO: size == 0 -> connection closed (maybe go to IDLE instead of stopping?) printf("Error: failed while trying to receive monitoring message!\n"); return false; } - + if (!_data->decoder.decode(_data->receiveBuffer, size)) { return false; } - + // check message type (so that our wrappers match) if (_data->expectedMonitorMsgID != _data->monitoringMsg.header.messageIdentifier) { @@ -139,23 +139,23 @@ bool ClientApplication::step() (int)_data->monitoringMsg.header.messageIdentifier, (int)_data->expectedMonitorMsgID); return false; - } - + } + // ************************************************************************** // callbacks // ************************************************************************** // reset command message before callbacks _data->resetCommandMessage(); - + // callbacks for robot client ESessionState currentState = (ESessionState)_data->monitoringMsg.connectionInfo.sessionState; - + if (_data->lastState != currentState) { _robotClient->onStateChange(_data->lastState, currentState); _data->lastState = currentState; } - + switch (currentState) { case MONITORING_WAIT: @@ -178,34 +178,33 @@ bool ClientApplication::step() { _trafoClient->provide(); } - + // ************************************************************************** // Encode and send command message // ************************************************************************** - + _data->lastSendCounter++; // check if its time to send an answer if (_data->lastSendCounter >= _data->monitoringMsg.connectionInfo.receiveMultiplier) { _data->lastSendCounter = 0; - + // set sequence counters _data->commandMsg.header.sequenceCounter = _data->sequenceCounter++; - _data->commandMsg.header.reflectedSequenceCounter = + _data->commandMsg.header.reflectedSequenceCounter = _data->monitoringMsg.header.sequenceCounter; - + if (!_data->encoder.encode(_data->sendBuffer, size)) { return false; } - + if (!_connection.send(_data->sendBuffer, size)) { printf("Error: failed while trying to send command message!\n"); return false; } } - + return true; } - diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientData.h b/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientData.h index 67c11c56..bb1b8438 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientData.h +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientData.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -80,21 +80,21 @@ namespace FRI FRIMonitoringMessage monitoringMsg; //!< monitoring message struct FRICommandMessage commandMsg; //!< command message struct - + MonitoringMessageDecoder decoder; //!< monitoring message decoder CommandMessageEncoder encoder; //!< command message encoder - + ESessionState lastState; //!< last FRI state uint32_t sequenceCounter; //!< sequence counter for command messages uint32_t lastSendCounter; //!< steps since last send command - uint32_t expectedMonitorMsgID; //!< expected ID for received monitoring messages + uint32_t expectedMonitorMsgID; //!< expected ID for received monitoring messages const size_t MAX_REQUESTED_TRANSFORMATIONS; //!< maximum count of requested transformations const size_t MAX_SIZE_TRANSFORMATION_ID; //!< maximum size in bytes of a transformation ID std::vector requestedTrafoIDs; //!< list of requested transformation ids - - ClientData(int numDofs) - : decoder(&monitoringMsg, numDofs), + + ClientData(int numDofs) + : decoder(&monitoringMsg, numDofs), encoder(&commandMsg, numDofs), lastState(IDLE), sequenceCounter(0), @@ -106,7 +106,7 @@ namespace FRI { requestedTrafoIDs.reserve(MAX_REQUESTED_TRANSFORMATIONS); } - + void resetCommandMessage() { commandMsg.commandData.has_jointPosition = false; @@ -115,10 +115,10 @@ namespace FRI commandMsg.commandData.commandedTransformations_count = 0; commandMsg.commandData.has_cartesianPose = false; commandMsg.commandData.has_redundancyInformation = false; - commandMsg.has_commandData = false; + commandMsg.has_commandData = false; commandMsg.commandData.writeIORequest_count = 0; } - + //****************************************************************************** static const FriIOValue& getBooleanIOValue(const FRIMonitoringMessage* message, const char* name) { @@ -138,49 +138,49 @@ namespace FRI } //****************************************************************************** - static void setBooleanIOValue(FRICommandMessage* message, const char* name, const bool value, + static void setBooleanIOValue(FRICommandMessage* message, const char* name, const bool value, const FRIMonitoringMessage* monMessage) { setIOValue(message, name, monMessage, FriIOType_BOOLEAN).digitalValue = value; } //****************************************************************************** - static void setDigitalIOValue(FRICommandMessage* message, const char* name, const unsigned long long value, + static void setDigitalIOValue(FRICommandMessage* message, const char* name, const unsigned long long value, const FRIMonitoringMessage* monMessage) { setIOValue(message, name, monMessage, FriIOType_DIGITAL).digitalValue = value; } //****************************************************************************** - static void setAnalogIOValue(FRICommandMessage* message, const char* name, const double value, + static void setAnalogIOValue(FRICommandMessage* message, const char* name, const double value, const FRIMonitoringMessage* monMessage) { setIOValue(message, name, monMessage, FriIOType_ANALOG).analogValue = value; } - + protected: - + //****************************************************************************** - static const FriIOValue& getIOValue(const FRIMonitoringMessage* message, const char* name, + static const FriIOValue& getIOValue(const FRIMonitoringMessage* message, const char* name, const FriIOType ioType) { if(message != NULL && message->has_monitorData == true) { const MessageMonitorData& monData = message->monitorData; - const bool analogValue = (ioType == FriIOType_ANALOG); - const bool digitalValue = (ioType == FriIOType_DIGITAL) | (ioType == FriIOType_BOOLEAN); + const bool analogValue = (ioType == FriIOType_ANALOG); + const bool digitalValue = (ioType == FriIOType_DIGITAL) | (ioType == FriIOType_BOOLEAN); for(size_t i = 0; i < monData.readIORequest_count; i++) { const FriIOValue& ioValue = monData.readIORequest[i]; if(strcmp(name, ioValue.name) == 0) { if(ioValue.type == ioType && - ioValue.has_digitalValue == digitalValue && + ioValue.has_digitalValue == digitalValue && ioValue.has_analogValue == analogValue) { return ioValue; } - + const char* ioTypeName; switch(ioType) { @@ -189,7 +189,7 @@ namespace FRI case FriIOType_BOOLEAN: ioTypeName = "boolean"; break; default: ioTypeName = "?"; break; } - + throw FRIException("IO %s is not of type %s.", name, ioTypeName); } } @@ -197,9 +197,9 @@ namespace FRI throw FRIException("Could not locate IO %s in monitor message.", name); } - + //****************************************************************************** - static FriIOValue& setIOValue(FRICommandMessage* message, const char* name, + static FriIOValue& setIOValue(FRICommandMessage* message, const char* name, const FRIMonitoringMessage* monMessage, const FriIOType ioType) { MessageCommandData& cmdData = message->commandData; @@ -212,29 +212,29 @@ namespace FRI { throw FRIException("IO %s is not an output value.", name); } - + // add IO value to command message FriIOValue& ioValue = cmdData.writeIORequest[cmdData.writeIORequest_count]; - + strncpy(ioValue.name, name, sizeof(ioValue.name) - 1); ioValue.name[sizeof(ioValue.name) - 1] = 0; // ensure termination ioValue.type = ioType; ioValue.has_digitalValue = (ioType == FriIOType_DIGITAL) | (ioType == FriIOType_BOOLEAN); ioValue.has_analogValue = (ioType == FriIOType_ANALOG); ioValue.direction = FriIODirection_OUTPUT; - + cmdData.writeIORequest_count ++; message->has_commandData = true; - + return ioValue; } else { throw FRIException("Exceeded maximum number of outputs that can be set."); } - } + } }; - + } } diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.cpp index 5653154c..afa42c8d 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -95,13 +95,13 @@ void CommandMessageEncoder::initMessage() m_pMessage->commandData.writeIORequest_count = 0; // allocate and map memory for protobuf repeated structures - map_repeatedDouble(FRI_MANAGER_NANOPB_ENCODE, m_nNum, + map_repeatedDouble(FRI_MANAGER_NANOPB_ENCODE, m_nNum, &m_pMessage->commandData.jointPosition.value, &m_tRecvContainer.jointPosition); - map_repeatedDouble(FRI_MANAGER_NANOPB_ENCODE, m_nNum, + map_repeatedDouble(FRI_MANAGER_NANOPB_ENCODE, m_nNum, &m_pMessage->commandData.jointTorque.value, &m_tRecvContainer.jointTorque); - + // nanopb encoding needs to know how many elements the static array contains // a quaternion always contains 7 elements m_pMessage->commandData.cartesianPose.element_count = 7; diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.h b/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.h index dde55882..6edfb0f5 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.h +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friCommandMessageEncoder.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -75,43 +75,43 @@ namespace FRI class CommandMessageEncoder { - + public: - + CommandMessageEncoder(FRICommandMessage* pMessage, int num); - + ~CommandMessageEncoder(); - + bool encode(char* buffer, int& size); - + private: - + struct LocalCommandDataContainer { tRepeatedDoubleArguments jointPosition; tRepeatedDoubleArguments jointTorque; - + LocalCommandDataContainer() { init_repeatedDouble(&jointPosition); init_repeatedDouble(&jointTorque); } - + ~LocalCommandDataContainer() { free_repeatedDouble(&jointPosition); free_repeatedDouble(&jointTorque); } }; - + int m_nNum; - + LocalCommandDataContainer m_tRecvContainer; FRICommandMessage* m_pMessage; - + void initMessage(); }; - + } } diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friDataHelper.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friDataHelper.cpp index d87dbb5a..08fa0267 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friDataHelper.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friDataHelper.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -147,7 +147,7 @@ namespace KUKA } - void DataHelper::convertTrafoQuaternionToMatrix(const double(&quaternionTrafo)[7], + void DataHelper::convertTrafoQuaternionToMatrix(const double(&quaternionTrafo)[7], double(&matrixTrafo)[3][4]) { @@ -156,7 +156,7 @@ namespace KUKA const double qY = quaternionTrafo[QUAT_QY]; const double qZ = quaternionTrafo[QUAT_QZ]; - // conversion for unit quaternion to transformation matrix + // conversion for unit quaternion to transformation matrix matrixTrafo[0][0] = 1 - 2 * ((qY * qY) + (qZ * qZ)); matrixTrafo[0][1] = 2 * ((qX * qY) - (qW * qZ)); matrixTrafo[0][2] = 2 * ((qX * qZ) + (qW * qY)); diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp index df035d3c..3541bbcb 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRClient.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -67,13 +67,13 @@ char FRIException::_buffer[1024] = { 0 }; //****************************************************************************** LBRClient::LBRClient() { - + } //****************************************************************************** LBRClient::~LBRClient() { - + } //****************************************************************************** @@ -111,7 +111,7 @@ void LBRClient::command() ClientData* LBRClient::createData() { ClientData* data = new ClientData(_robotState.NUMBER_OF_JOINTS); - + // link monitoring and command message to wrappers _robotState._message = &data->monitoringMsg; _robotCommand._cmdMessage = &data->commandMsg; @@ -120,7 +120,6 @@ ClientData* LBRClient::createData() // set specific message IDs data->expectedMonitorMsgID = _robotState.LBRMONITORMESSAGEID; data->commandMsg.header.messageIdentifier = _robotCommand.LBRCOMMANDMESSAGEID; - + return data; } - diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRCommand.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRCommand.cpp index e1487154..ba5da8ea 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRCommand.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRCommand.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -105,13 +105,13 @@ void LBRCommand::setCartesianPose(const double* cartesianPoseQuaternion, memcpy(_cmdMessage->commandData.cartesianPose.element, cartesianPoseQuaternion, 7 * sizeof(double)); _cmdMessage->commandData.has_redundancyInformation = true; - _cmdMessage->commandData.redundancyInformation.strategy = + _cmdMessage->commandData.redundancyInformation.strategy = _monMessage->monitorData.measuredRedundancyInformation.strategy; - + if (NULL != redundancyValue) { //set value if provided - + _cmdMessage->commandData.redundancyInformation.value = *redundancyValue; } else diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRState.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRState.cpp index 271ae868..39fe9324 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRState.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friLBRState.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friMonitoringMessageDecoder.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friMonitoringMessageDecoder.cpp index c6426db9..c2528d16 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friMonitoringMessageDecoder.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friMonitoringMessageDecoder.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -81,7 +81,7 @@ MonitoringMessageDecoder::~MonitoringMessageDecoder() void MonitoringMessageDecoder::initMessage() { // set initial data - // it is assumed that no robot information and monitoring data is available and therefore the + // it is assumed that no robot information and monitoring data is available and therefore the // optional fields are initialized with false m_pMessage->has_robotInfo = false; m_pMessage->has_monitorData = false; @@ -89,8 +89,8 @@ void MonitoringMessageDecoder::initMessage() m_pMessage->has_ipoData = false; m_pMessage->requestedTransformations_count = 0; m_pMessage->has_endOfMessageData = false; - - + + m_pMessage->header.messageIdentifier = 0; m_pMessage->header.reflectedSequenceCounter = 0; m_pMessage->header.sequenceCounter = 0; @@ -102,27 +102,27 @@ void MonitoringMessageDecoder::initMessage() m_pMessage->monitorData.measuredCartesianPose.element_count = 0; // allocate and map memory for protobuf repeated structures - map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, &m_pMessage->monitorData.measuredJointPosition.value, &m_tSendContainer.m_AxQMsrLocal); - - map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, &m_pMessage->monitorData.measuredTorque.value, &m_tSendContainer.m_AxTauMsrLocal); - - map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, &m_pMessage->monitorData.commandedTorque.value, &m_tSendContainer.m_AxTauCmdLocal); - - map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, &m_pMessage->monitorData.externalTorque.value, &m_tSendContainer.m_AxTauExtMsrLocal); - + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE,m_nNum, &m_pMessage->ipoData.jointPosition.value, &m_tSendContainer.m_AxQCmdIPO); - map_repeatedInt(FRI_MANAGER_NANOPB_DECODE, m_nNum, + map_repeatedInt(FRI_MANAGER_NANOPB_DECODE, m_nNum, &m_pMessage->robotInfo.driveState, &m_tSendContainer.m_AxDriveStateLocal); } diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friMonitoringMessageDecoder.h b/kuka_sunrise_fri_driver/src/fri_client_sdk/friMonitoringMessageDecoder.h index 795c5175..96138188 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friMonitoringMessageDecoder.h +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friMonitoringMessageDecoder.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -70,7 +70,7 @@ namespace FRI { static const int FRI_MONITOR_MSG_MAX_SIZE = 1500; //!< max size of a FRI monitoring message - + class MonitoringMessageDecoder { @@ -78,14 +78,14 @@ namespace FRI public: MonitoringMessageDecoder(FRIMonitoringMessage* pMessage, int num); - + ~MonitoringMessageDecoder(); - + bool decode(char* buffer, int size); - - + + private: - + struct LocalMonitoringDataContainer { tRepeatedDoubleArguments m_AxQMsrLocal; @@ -93,9 +93,9 @@ namespace FRI tRepeatedDoubleArguments m_AxQCmdT1mLocal; tRepeatedDoubleArguments m_AxTauCmdLocal; tRepeatedDoubleArguments m_AxTauExtMsrLocal; - tRepeatedIntArguments m_AxDriveStateLocal; + tRepeatedIntArguments m_AxDriveStateLocal; tRepeatedDoubleArguments m_AxQCmdIPO; - + LocalMonitoringDataContainer() { init_repeatedDouble(&m_AxQMsrLocal); @@ -106,7 +106,7 @@ namespace FRI init_repeatedDouble(&m_AxQCmdIPO); init_repeatedInt(&m_AxDriveStateLocal); } - + ~LocalMonitoringDataContainer() { free_repeatedDouble(&m_AxQMsrLocal); @@ -123,11 +123,11 @@ namespace FRI LocalMonitoringDataContainer m_tSendContainer; FRIMonitoringMessage* m_pMessage; - + void initMessage(); }; } } - + #endif // _KUKA_FRI_MONITORINGMESSAGEDECODER_H diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friTransformationClient.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friTransformationClient.cpp index 12285b67..2a0029b1 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friTransformationClient.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friTransformationClient.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -74,7 +74,7 @@ using namespace KUKA::FRI; TransformationClient::TransformationClient() { } - + //****************************************************************************** TransformationClient::~TransformationClient() { @@ -88,12 +88,12 @@ const std::vector& TransformationClient::getRequestedTransformation for (unsigned int i=0; irequestedTrafoIDs[i] = _data->monitoringMsg.requestedTransformations[i].name; - } + } return _data->requestedTrafoIDs; } //****************************************************************************** -const unsigned int TransformationClient::getTimestampSec() const +const unsigned int TransformationClient::getTimestampSec() const { return _data->monitoringMsg.monitorData.timestamp.sec; } @@ -132,8 +132,8 @@ void TransformationClient::setTransformation(const char* transformationID, } //****************************************************************************** -void TransformationClient::setTransformation(const char* transformationID, - const double (&transformationMatrix)[3][4], +void TransformationClient::setTransformation(const char* transformationID, + const double (&transformationMatrix)[3][4], const unsigned int timeSec, const unsigned int timeNanoSec) { diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friUdpConnection.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friUdpConnection.cpp index 774552ad..a2dbe8b4 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friUdpConnection.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friUdpConnection.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -77,7 +77,7 @@ cost of any service and repair. using namespace KUKA::FRI; //****************************************************************************** -UdpConnection::UdpConnection(unsigned int receiveTimeout) : +UdpConnection::UdpConnection(unsigned int receiveTimeout) : _udpSock(-1), _receiveTimeout(receiveTimeout) { @@ -86,7 +86,7 @@ UdpConnection::UdpConnection(unsigned int receiveTimeout) : WSAStartup(MAKEWORD(2,0), &WSAData); #endif } - + //****************************************************************************** UdpConnection::~UdpConnection() { @@ -95,7 +95,7 @@ UdpConnection::~UdpConnection() WSACleanup(); #endif } - + //****************************************************************************** bool UdpConnection::open(int port, const char *controllerAddress) { @@ -115,7 +115,7 @@ bool UdpConnection::open(int port, const char *controllerAddress) servAddr.sin_family = AF_INET; servAddr.sin_port = htons(port); servAddr.sin_addr.s_addr = htonl(INADDR_ANY); - + if (bind(_udpSock, (struct sockaddr *)&servAddr, sizeof(servAddr)) < 0) { printf("binding port number %d failed!\n", port); @@ -145,7 +145,7 @@ bool UdpConnection::open(int port, const char *controllerAddress) } return true; } - + //****************************************************************************** void UdpConnection::close() { @@ -165,14 +165,14 @@ bool UdpConnection::isOpen() const { return (_udpSock >= 0); } - + //****************************************************************************** int UdpConnection::receive(char *buffer, int maxSize) { if (isOpen()) { /** HAVE_SOCKLEN_T - Yes - unbelievable: There are differences in standard calling parameters (types) to recvfrom + Yes - unbelievable: There are differences in standard calling parameters (types) to recvfrom Windows winsock, VxWorks and QNX use int newer Posix (most Linuxes) use socklen_t */ @@ -185,11 +185,11 @@ int UdpConnection::receive(char *buffer, int maxSize) /** check for timeout Because SO_RCVTIMEO is in Windows not correctly implemented, select is used for the receive time out. If a timeout greater than 0 is given, wait until the timeout is reached or a message was received. - If t, abort the function with an error. + If t, abort the function with an error. */ if(_receiveTimeout > 0) { - + // Set up struct timeval struct timeval tv; tv.tv_sec = _receiveTimeout / 1000; @@ -197,8 +197,8 @@ int UdpConnection::receive(char *buffer, int maxSize) // initialize file descriptor /** - * Replace FD_ZERO with memset, because bzero is not available for VxWorks - * User Space Aplications(RTPs). Therefore the macro FD_ZERO does not compile. + * Replace FD_ZERO with memset, because bzero is not available for VxWorks + * User Space Applications(RTPs). Therefore the macro FD_ZERO does not compile. */ #ifndef VXWORKS FD_ZERO(&_filedescriptor); @@ -206,7 +206,7 @@ int UdpConnection::receive(char *buffer, int maxSize) memset((char *)(&_filedescriptor), 0, sizeof(*(&_filedescriptor))); #endif FD_SET(_udpSock, &_filedescriptor); - + // wait until something was received int numberActiveFileDescr = select(_udpSock+1, &_filedescriptor,NULL,NULL,&tv); // 0 indicates a timeout @@ -218,7 +218,7 @@ int UdpConnection::receive(char *buffer, int maxSize) // a negative value indicates an error else if(numberActiveFileDescr == -1) { - printf("An error has occured \n"); + printf("An error has occurred \n"); return -1; } } @@ -227,7 +227,7 @@ int UdpConnection::receive(char *buffer, int maxSize) } return -1; } - + //****************************************************************************** bool UdpConnection::send(const char* buffer, int size) { diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.c b/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.c index 998583c2..0fc8ae11 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.c +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.c @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -59,7 +59,7 @@ cost of any service and repair. \version {2.5} */ #include -#include +#include #include "pb_frimessages_callbacks.h" #include "pb_encode.h" @@ -194,7 +194,7 @@ bool decode_repeatedInt(pb_istream_t *stream, const pb_field_t *field, void **ar void map_repeatedDouble(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedDoubleArguments *arg) { - // IMPORTANT: the callbacks are stored in a union, therefor a message object + // IMPORTANT: the callbacks are stored in a union, therefore a message object // must be exclusive defined for transmission or reception if (dir == FRI_MANAGER_NANOPB_ENCODE) { @@ -217,7 +217,7 @@ void map_repeatedDouble(eNanopbCallbackDirection dir, int numDOF, pb_callback_t void map_repeatedInt(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedIntArguments *arg) { - // IMPORTANT: the callbacks are stored in a union, therefor a message object + // IMPORTANT: the callbacks are stored in a union, therefore a message object // must be exclusive defined for transmission or reception if (dir == FRI_MANAGER_NANOPB_ENCODE) { diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.h b/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.h index fdc1f4e7..0e45520e 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.h +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -83,8 +83,8 @@ typedef struct repeatedIntArguments { /** enumeration for direction (encoding/decoding) */ typedef enum DIRECTION { - FRI_MANAGER_NANOPB_DECODE = 0, //!< Argument um eine - FRI_MANAGER_NANOPB_ENCODE = 1 //!< + FRI_MANAGER_NANOPB_DECODE = 0, //!< Argument um eine + FRI_MANAGER_NANOPB_ENCODE = 1 //!< } eNanopbCallbackDirection; diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 41e288e7..dd3cbfbc 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -42,7 +42,8 @@ CallbackReturn KukaFRIHardwareInterface::on_init( hw_torque_states_.resize(info_.joints.size()); hw_ext_torque_states_.resize(info_.joints.size()); hw_torque_commands_.resize(info_.joints.size()); - hw_cart_pose_states_.resize(7);// it's always 7 dof: position x,y,z; orientation quaternion qx,qy,qz,qw + hw_cart_pose_states_.resize( + 7); // it's always 7 dof: position x,y,z; orientation quaternion qx,qy,qz,qw hw_cart_pose_commands_.resize(7); hw_wrench_commands_.resize(6); // it's always 6 dof: force x,y,z; torque x,y,z @@ -384,11 +385,11 @@ void KukaFRIHardwareInterface::updateCommand(const rclcpp::Time &) const double * cartesianPoseQuaternion = hw_cart_pose_commands_.data(); RCLCPP_DEBUG( rclcpp::get_logger("KukaFRIHardwareInterface"), - "cartesianPoseQuaternion command: %f %f %f %f %f %f %f",cartesianPoseQuaternion[0],cartesianPoseQuaternion[1], - cartesianPoseQuaternion[2],cartesianPoseQuaternion[3],cartesianPoseQuaternion[4],cartesianPoseQuaternion[5] - ,cartesianPoseQuaternion[6]); - robotCommand().setCartesianPose(cartesianPoseQuaternion); - + "cartesianPoseQuaternion command: %f %f %f %f %f %f %f", cartesianPoseQuaternion[0], + cartesianPoseQuaternion[1], cartesianPoseQuaternion[2], cartesianPoseQuaternion[3], + cartesianPoseQuaternion[4], cartesianPoseQuaternion[5], cartesianPoseQuaternion[6]); + robotCommand().setCartesianPose(cartesianPoseQuaternion); + break; } case kuka_drivers_core::ControlMode::WRENCH_CONTROL: @@ -463,8 +464,8 @@ std::vector KukaFRIHardwareInterface::export info_.joints[i].name, hardware_interface::HW_IF_EXTERNAL_TORQUE, &hw_ext_torque_states_[i]); } std::vector cart_joints_list = { - hardware_interface::HW_IF_X, hardware_interface::HW_IF_Y, hardware_interface::HW_IF_Z, - hardware_interface::HW_IF_QW,hardware_interface::HW_IF_QX, hardware_interface::HW_IF_QY, + hardware_interface::HW_IF_X, hardware_interface::HW_IF_Y, hardware_interface::HW_IF_Z, + hardware_interface::HW_IF_QW, hardware_interface::HW_IF_QX, hardware_interface::HW_IF_QY, hardware_interface::HW_IF_QZ}; for (size_t i = 0; i < cart_joints_list.size(); ++i) { @@ -510,17 +511,17 @@ KukaFRIHardwareInterface::export_command_interfaces() info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_torque_commands_[i]); } std::vector cart_joints_list = { - hardware_interface::HW_IF_X, hardware_interface::HW_IF_Y, hardware_interface::HW_IF_Z, + hardware_interface::HW_IF_X, hardware_interface::HW_IF_Y, hardware_interface::HW_IF_Z, hardware_interface::HW_IF_QW, hardware_interface::HW_IF_QX, hardware_interface::HW_IF_QY, hardware_interface::HW_IF_QZ, - }; + }; for (size_t i = 0; i < cart_joints_list.size(); ++i) { command_interfaces.emplace_back( std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + std::string(cart_joints_list[i]), hardware_interface::HW_IF_POSITION, &hw_cart_pose_commands_[i]); } - std::vector cart_effort_list = { + std::vector cart_effort_list = { hardware_interface::HW_IF_X, hardware_interface::HW_IF_Y, hardware_interface::HW_IF_Z, hardware_interface::HW_IF_A, hardware_interface::HW_IF_B, hardware_interface::HW_IF_C}; for (size_t i = 0; i < cart_effort_list.size(); i++) From 7c71b59b1bc60b00cf1934413f40320825f02314 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Thu, 17 Oct 2024 16:56:14 +0200 Subject: [PATCH 47/49] add both versions of FRI sdk, configurable in cmake --- kuka_sunrise_fri_driver/CMakeLists.txt | 46 +- .../fri_client_sdk/HWIFClientApplication.hpp | 0 .../fri_client_sdk/friClientApplication.h | 152 ++++ .../FRI_v1_5/fri_client_sdk/friClientIf.h | 199 +++++ .../FRI_v1_5/fri_client_sdk/friConnectionIf.h | 129 ++++ .../FRI_v1_5/fri_client_sdk/friException.h | 152 ++++ .../FRI_v1_5/fri_client_sdk/friLBRClient.h | 143 ++++ .../FRI_v1_5/fri_client_sdk/friLBRCommand.h | 160 +++++ .../FRI_v1_5/fri_client_sdk/friLBRState.h | 276 +++++++ .../fri_client_sdk/friTransformationClient.h | 266 +++++++ .../fri_client_sdk/friUdpConnection.h | 161 +++++ .../fri_client_sdk/HWIFClientApplication.hpp | 34 + .../fri_client_sdk/friClientApplication.h | 155 ++++ .../FRI_v2_5/fri_client_sdk/friClientIf.h | 210 ++++++ .../FRI_v2_5/fri_client_sdk/friConnectionIf.h | 130 ++++ .../FRI_v2_5/fri_client_sdk/friDataHelper.h | 129 ++++ .../FRI_v2_5/fri_client_sdk/friException.h | 153 ++++ .../FRI_v2_5/fri_client_sdk/friLBRClient.h | 145 ++++ .../FRI_v2_5/fri_client_sdk/friLBRCommand.h | 206 ++++++ .../FRI_v2_5/fri_client_sdk/friLBRState.h | 369 ++++++++++ .../fri_client_sdk/friTransformationClient.h | 292 ++++++++ .../fri_client_sdk/friUdpConnection.h | 163 +++++ .../hardware_interface.hpp | 5 +- .../robot_manager_node.hpp | 2 + .../v1_5/src/application/ROS2_Control.java | 67 ++ .../ROS2_Control.java:Zone.Identifier | 0 .../ioAccess/MediaFlangeIOGroup.java | 0 .../MediaFlangeIOGroup.java:Zone.Identifier | 0 .../v1_5/src/ros2/modules/FRIManager.java | 263 +++++++ .../modules/FRIManager.java:Zone.Identifier | 0 .../v1_5/src/ros2/modules/ROS2Connection.java | 411 +++++++++++ .../ROS2Connection.java:Zone.Identifier | 0 .../src/ros2/modules/TCPConnection.java | 0 .../TCPConnection.java:Zone.Identifier | 0 ...ianImpedanceControlModeExternalizable.java | 71 ++ ...rolModeExternalizable.java:Zone.Identifier | 0 .../ros2/serialization/ControlModeParams.java | 111 +++ .../ControlModeParams.java:Zone.Identifier | 0 .../serialization/FRIConfigurationParams.java | 63 ++ ...RIConfigurationParams.java:Zone.Identifier | 0 ...intImpedanceControlModeExternalizable.java | 59 ++ ...rolModeExternalizable.java:Zone.Identifier | 0 .../ros2/serialization/MessageEncoding.java | 0 .../MessageEncoding.java:Zone.Identifier | 0 .../src/ros2/tools/Conversions.java | 0 .../tools/Conversions.java:Zone.Identifier | 0 .../v2_5/src/application/ROS2_Control.java | 68 ++ .../ioAccess/MediaFlangeIOGroup.java | 412 +++++++++++ .../v2_5/src/ros2/modules/FRIManager.java | 290 ++++++++ .../v2_5/src/ros2/modules/ROS2Connection.java | 411 +++++++++++ .../v2_5/src/ros2/modules/TCPConnection.java | 185 +++++ ...ianImpedanceControlModeExternalizable.java | 68 ++ .../ros2/serialization/ControlModeParams.java | 109 +++ .../serialization/FRIConfigurationParams.java | 0 ...intImpedanceControlModeExternalizable.java | 0 .../ros2/serialization/MessageEncoding.java | 39 + .../v2_5/src/ros2/tools/Conversions.java | 13 + .../FRI_v1_5/fri_client_sdk/FRIMessages.pb.c | 71 ++ .../FRI_v1_5/fri_client_sdk/FRIMessages.pb.h | 567 +++++++++++++++ .../fri_client_sdk/HWIFClientApplication.cpp | 0 .../fri_client_sdk/friClientApplication.cpp | 201 ++++++ .../FRI_v1_5/fri_client_sdk/friClientData.h | 242 +++++++ .../friCommandMessageEncoder.cpp | 122 ++++ .../fri_client_sdk/friCommandMessageEncoder.h | 115 +++ .../FRI_v1_5/fri_client_sdk/friLBRClient.cpp | 121 ++++ .../FRI_v1_5/fri_client_sdk/friLBRCommand.cpp | 113 +++ .../FRI_v1_5/fri_client_sdk/friLBRState.cpp | 232 ++++++ .../friMonitoringMessageDecoder.cpp | 151 ++++ .../friMonitoringMessageDecoder.h | 130 ++++ .../friTransformationClient.cpp | 188 +++++ .../fri_client_sdk/friUdpConnection.cpp | 234 ++++++ .../fri_client_sdk/pb_frimessages_callbacks.c | 269 +++++++ .../fri_client_sdk/pb_frimessages_callbacks.h | 130 ++++ .../FRI_v2_5/fri_client_sdk/FRIMessages.pb.c | 76 ++ .../FRI_v2_5/fri_client_sdk/FRIMessages.pb.h | 680 ++++++++++++++++++ .../fri_client_sdk/HWIFClientApplication.cpp | 119 +++ .../fri_client_sdk/friClientApplication.cpp | 211 ++++++ .../FRI_v2_5/fri_client_sdk/friClientData.h | 242 +++++++ .../friCommandMessageEncoder.cpp | 125 ++++ .../fri_client_sdk/friCommandMessageEncoder.h | 118 +++ .../FRI_v2_5/fri_client_sdk/friDataHelper.cpp | 178 +++++ .../FRI_v2_5/fri_client_sdk/friLBRClient.cpp | 126 ++++ .../FRI_v2_5/fri_client_sdk/friLBRCommand.cpp | 149 ++++ .../FRI_v2_5/fri_client_sdk/friLBRState.cpp | 271 +++++++ .../friMonitoringMessageDecoder.cpp | 142 ++++ .../friMonitoringMessageDecoder.h | 133 ++++ .../friTransformationClient.cpp | 205 ++++++ .../fri_client_sdk/friUdpConnection.cpp | 243 +++++++ .../fri_client_sdk/pb_frimessages_callbacks.c | 267 +++++++ .../fri_client_sdk/pb_frimessages_callbacks.h | 121 ++++ .../src/hardware_interface.cpp | 16 +- .../src/robot_manager_node.cpp | 12 +- 92 files changed, 12614 insertions(+), 23 deletions(-) rename kuka_sunrise_fri_driver/include/{ => FRI_v1_5}/fri_client_sdk/HWIFClientApplication.hpp (100%) create mode 100644 kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friClientApplication.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friClientIf.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friConnectionIf.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friException.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friLBRClient.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friLBRCommand.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friLBRState.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friTransformationClient.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friUdpConnection.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/HWIFClientApplication.hpp create mode 100644 kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friClientApplication.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friClientIf.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friConnectionIf.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friDataHelper.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friException.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRClient.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRCommand.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRState.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friTransformationClient.h create mode 100644 kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friUdpConnection.h create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/application/ROS2_Control.java create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/application/ROS2_Control.java:Zone.Identifier rename kuka_sunrise_fri_driver/robot_application/{ => v1_5}/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java (100%) mode change 100755 => 100644 create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java:Zone.Identifier create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/FRIManager.java create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/FRIManager.java:Zone.Identifier create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/ROS2Connection.java create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/ROS2Connection.java:Zone.Identifier rename kuka_sunrise_fri_driver/robot_application/{ => v1_5}/src/ros2/modules/TCPConnection.java (100%) mode change 100755 => 100644 create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/TCPConnection.java:Zone.Identifier create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java:Zone.Identifier create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/ControlModeParams.java create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/ControlModeParams.java:Zone.Identifier create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/FRIConfigurationParams.java create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/FRIConfigurationParams.java:Zone.Identifier create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/JointImpedanceControlModeExternalizable.java create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/JointImpedanceControlModeExternalizable.java:Zone.Identifier rename kuka_sunrise_fri_driver/robot_application/{ => v1_5}/src/ros2/serialization/MessageEncoding.java (100%) mode change 100755 => 100644 create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/MessageEncoding.java:Zone.Identifier rename kuka_sunrise_fri_driver/robot_application/{ => v1_5}/src/ros2/tools/Conversions.java (100%) mode change 100755 => 100644 create mode 100644 kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/tools/Conversions.java:Zone.Identifier create mode 100755 kuka_sunrise_fri_driver/robot_application/v2_5/src/application/ROS2_Control.java create mode 100755 kuka_sunrise_fri_driver/robot_application/v2_5/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java create mode 100755 kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/FRIManager.java create mode 100755 kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/ROS2Connection.java create mode 100755 kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/TCPConnection.java create mode 100644 kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java create mode 100755 kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/ControlModeParams.java rename kuka_sunrise_fri_driver/robot_application/{ => v2_5}/src/ros2/serialization/FRIConfigurationParams.java (100%) rename kuka_sunrise_fri_driver/robot_application/{ => v2_5}/src/ros2/serialization/JointImpedanceControlModeExternalizable.java (100%) create mode 100755 kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/MessageEncoding.java create mode 100755 kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/tools/Conversions.java create mode 100644 kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/FRIMessages.pb.c create mode 100644 kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/FRIMessages.pb.h rename kuka_sunrise_fri_driver/src/{ => FRI_v1_5}/fri_client_sdk/HWIFClientApplication.cpp (100%) create mode 100644 kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friClientApplication.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friClientData.h create mode 100644 kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friCommandMessageEncoder.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friCommandMessageEncoder.h create mode 100644 kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRClient.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRCommand.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRState.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friMonitoringMessageDecoder.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friMonitoringMessageDecoder.h create mode 100644 kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friTransformationClient.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friUdpConnection.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/pb_frimessages_callbacks.c create mode 100644 kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/pb_frimessages_callbacks.h create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/FRIMessages.pb.c create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/FRIMessages.pb.h create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/HWIFClientApplication.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friClientApplication.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friClientData.h create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friCommandMessageEncoder.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friCommandMessageEncoder.h create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friDataHelper.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRClient.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRCommand.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRState.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friMonitoringMessageDecoder.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friMonitoringMessageDecoder.h create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friTransformationClient.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friUdpConnection.cpp create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/pb_frimessages_callbacks.c create mode 100644 kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/pb_frimessages_callbacks.h diff --git a/kuka_sunrise_fri_driver/CMakeLists.txt b/kuka_sunrise_fri_driver/CMakeLists.txt index b1277887..f570dee9 100644 --- a/kuka_sunrise_fri_driver/CMakeLists.txt +++ b/kuka_sunrise_fri_driver/CMakeLists.txt @@ -31,7 +31,16 @@ find_path(NANOPB_INCLUDE_DIR PATHS /usr/include/nanopb /usr/local/include/nanopb ) -include_directories(include src/fri_client_sdk ${NANOPB_INCLUDE_DIR}) +option(WITH_FRI_VERSION_2_5 "Using the FRI 2.5 version" ON) + +if (WITH_FRI_VERSION_2_5) + add_definitions(-DFRI_V2_5) + set(FRI_CLIENT_SDK_VERSION "FRI_v2_5") +else() + set(FRI_CLIENT_SDK_VERSION "FRI_v1_5") +endif() + +include_directories(include src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk include/${FRI_CLIENT_SDK_VERSION} ${NANOPB_INCLUDE_DIR}) add_library(fri_connection SHARED src/connection_helpers/fri_connection.cpp @@ -43,17 +52,17 @@ target_link_libraries(fri_connection file(GLOB fri_client_sources LIST_DIRECTORIES FALSE RELATIVE "${PROJECT_SOURCE_DIR}" - src/fri_client_sdk/HWIFClientApplication.cpp - src/fri_client_sdk/friClientApplication.cpp - src/fri_client_sdk/friCommandMessageEncoder.cpp - src/fri_client_sdk/friLBRClient.cpp - src/fri_client_sdk/friLBRCommand.cpp - src/fri_client_sdk/friLBRState.cpp - src/fri_client_sdk/FRIMessages.pb.c - src/fri_client_sdk/friMonitoringMessageDecoder.cpp - src/fri_client_sdk/friTransformationClient.cpp - src/fri_client_sdk/friUdpConnection.cpp - src/fri_client_sdk/pb_frimessages_callbacks.c + src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk/HWIFClientApplication.cpp + src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk/friClientApplication.cpp + src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk/friCommandMessageEncoder.cpp + src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk/friLBRClient.cpp + src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk/friLBRCommand.cpp + src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk/friLBRState.cpp + src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk/FRIMessages.pb.c + src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk/friMonitoringMessageDecoder.cpp + src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk/friTransformationClient.cpp + src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk/friUdpConnection.cpp + src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk/pb_frimessages_callbacks.c ) # Add the Fast Robot Interface library @@ -62,16 +71,17 @@ add_library(fri_client_sdk SHARED ${fri_client_sources}) file(GLOB private_headers LIST_DIRECTORIES FALSE RELATIVE "${PROJECT_SOURCE_DIR}" - src/fri_client_sdk/friClientData.h - src/fri_client_sdk/friCommandMessageEncoder.h - src/fri_client_sdk/FRIMessages.pb.h - src/fri_client_sdk/friMonitoringMessageDecoder.h - src/fri_client_sdk/pb_frimessages_callbacks.h + src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk/friClientData.h + src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk/friCommandMessageEncoder.h + src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk/FRIMessages.pb.h + src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk/friMonitoringMessageDecoder.h + src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk/pb_frimessages_callbacks.h ) target_link_libraries(fri_client_sdk PRIVATE protobuf-nanopb) -install(DIRECTORY include/fri_client_sdk DESTINATION include) +install(DIRECTORY include/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk DESTINATION include) + install(FILES ${private_headers} DESTINATION include) add_library(${PROJECT_NAME} SHARED diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/HWIFClientApplication.hpp b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/HWIFClientApplication.hpp similarity index 100% rename from kuka_sunrise_fri_driver/include/fri_client_sdk/HWIFClientApplication.hpp rename to kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/HWIFClientApplication.hpp diff --git a/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friClientApplication.h b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friClientApplication.h new file mode 100644 index 00000000..cdbe4e7d --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friClientApplication.h @@ -0,0 +1,152 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ + +#ifndef _KUKA_FRI_CLIENT_APPLICATION_H +#define _KUKA_FRI_CLIENT_APPLICATION_H + +/** Kuka namespace */ +namespace KUKA +{ +/** Fast Robot Interface (FRI) namespace */ +namespace FRI +{ + +// forward declarations +class IClient; +class TransformationClient; +class IConnection; +struct ClientData; + +/** + * \brief FRI client application class. + * + * A client application takes an instance of the IConnection interface and + * an instance of an IClient interface to provide the functionality + * needed to set up an FRI client application. It can be used to easily + * integrate the FRI client code within other applications. + * The algorithmic functionality of an FRI client application is implemented + * using the IClient interface. + */ +class ClientApplication +{ + +public: + /** + * \brief Constructor without transformation client. + * + * This constructor takes an instance of the IConnection interface and + * an instance of the IClient interface as parameters. + * @param connection FRI connection class + * @param client FRI client class + */ + ClientApplication(IConnection & connection, IClient & client); + + /** + * \brief Constructor with transformation client. + * + * This constructor takes an instance of the IConnection interface and + * an instance of the IClient interface and an instance of a + * TransformationClient as parameters. + * @param connection FRI connection class + * @param client FRI client class + * @param trafoClient FRI transformation client class + */ + ClientApplication(IConnection & connection, IClient & client, TransformationClient & trafoClient); + + /** \brief Destructor. */ + ~ClientApplication(); + + /** + * \brief Connect the FRI client application with a KUKA Sunrise controller. + * + * @param port The port ID + * @param remoteHost The address of the remote host + * @return True if connection was established + */ + bool connect(int port, const char * remoteHost = NULL); + + /** + * \brief Disconnect the FRI client application from a KUKA Sunrise controller. + */ + void disconnect(); + + /** + * \brief Run a single processing step. + * + * The processing step consists of receiving a new FRI monitoring message, + * calling the corresponding client callback and sending the resulting + * FRI command message back to the KUKA Sunrise controller. + * @return True if all of the substeps succeeded. + */ + bool step(); + +protected: + IConnection & _connection; //!< connection interface + IClient * _robotClient; //!< robot client interface + TransformationClient * _trafoClient; //!< transformation client interface + ClientData * _data; //!< client data structure (for internal use) + +}; + +} +} + + +#endif // _KUKA_FRI_CLIENT_APPLICATION_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friClientIf.h b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friClientIf.h new file mode 100644 index 00000000..a108e41d --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friClientIf.h @@ -0,0 +1,199 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.Connectivity FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.Connectivity FastRobotInterface� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#ifndef _KUKA_FRI_CLIENT_H +#define _KUKA_FRI_CLIENT_H + + +/** Kuka namespace */ +namespace KUKA +{ +/** Fast Robot Interface (FRI) namespace */ +namespace FRI +{ + +// forward declarations +struct ClientData; + + +/** \brief Enumeration of the FRI session state. */ +enum ESessionState +{ + IDLE = 0, //!< no session available + MONITORING_WAIT = 1, //!< monitoring mode with insufficient connection quality + MONITORING_READY = 2, //!< monitoring mode with connection quality sufficient for command mode + COMMANDING_WAIT = 3, //!< command mode about to start (overlay motion queued) + COMMANDING_ACTIVE = 4 //!< command mode active +}; + +/** \brief Enumeration of the FRI connection quality. */ +enum EConnectionQuality +{ + POOR = 0, //!< poor connection quality + FAIR = 1, //!< connection quality insufficient for command mode + GOOD = 2, //!< connection quality sufficient for command mode + EXCELLENT = 3 //!< excellent connection quality +}; + +/** \brief Enumeration of the controller's safety state. */ +enum ESafetyState +{ + NORMAL_OPERATION = 0, //!< No safety stop request present. + SAFETY_STOP_LEVEL_0 = 1, //!< Safety stop request STOP0 or STOP1 present. + SAFETY_STOP_LEVEL_1 = 2, //!< Safety stop request STOP1 (on-path) present. + SAFETY_STOP_LEVEL_2 = 3 //!< Safety stop request STOP2 present. +}; + +/** \brief Enumeration of the controller's current mode of operation. */ +enum EOperationMode +{ + TEST_MODE_1 = 0, //!< test mode 1 with reduced speed (T1) + TEST_MODE_2 = 1, //!< test mode 2 (T2) + AUTOMATIC_MODE = 2 //!< automatic operation mode (AUT) +}; + +/** \brief Enumeration of a drive's state. */ +enum EDriveState +{ + OFF = 0, //!< drive is not being used + TRANSITIONING = 1, //!< drive is in a transitioning state (before or after motion) + ACTIVE = 2 //!< drive is being actively commanded +}; + +/** \brief Enumeration of control mode. */ +enum EControlMode +{ + POSITION_CONTROL_MODE = 0, //!< position control mode + CART_IMP_CONTROL_MODE = 1, //!< cartesian impedance control mode + JOINT_IMP_CONTROL_MODE = 2, //!< joint impedance control mode + NO_CONTROL = 3 //!< drives are not used +}; + + +/** \brief Enumeration of the client command mode. */ +enum EClientCommandMode +{ + NO_COMMAND_MODE = 0, //!< no client command mode available + POSITION = 1, //!< commanding joint positions by the client + WRENCH = 2, //!< commanding wrenches and joint positions by the client + TORQUE = 3 //!< commanding joint torques and joint positions by the client +}; + +/** \brief Enumeration of the overlay type. */ +enum EOverlayType +{ + NO_OVERLAY = 0, //!< no overlay type available + JOINT = 1, //!< joint overlay + CARTESIAN = 2 //!< cartesian overlay +}; + + +/** + * \brief FRI client interface. + * + * This is the callback interface that should be implemented by FRI clients. + * Callbacks are automatically called by the client application + * (ClientApplication) whenever new FRI messages arrive. + */ +class IClient +{ + friend class ClientApplication; + +public: + /** \brief Virtual destructor. */ + virtual ~IClient() {} + + /** + * \brief Callback that is called whenever the FRI session state changes. + * + * @param oldState previous FRI session state + * @param newState current FRI session state + */ + virtual void onStateChange(ESessionState oldState, ESessionState newState) = 0; + + /** + * \brief Callback for the FRI session states 'Monitoring Wait' and 'Monitoring Ready'. + */ + virtual void monitor() = 0; + + /** + * \brief Callback for the FRI session state 'Commanding Wait'. + */ + virtual void waitForCommand() = 0; + + /** + * \brief Callback for the FRI session state 'Commanding'. + */ + virtual void command() = 0; + +protected: + /** + * \brief Method to create and initialize the client data structure (used internally). + * + * @return newly allocated client data structure + */ + virtual ClientData * createData() = 0; + +}; + +} +} + + +#endif // _KUKA_FRI_CLIENT_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friConnectionIf.h b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friConnectionIf.h new file mode 100644 index 00000000..57da47a5 --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friConnectionIf.h @@ -0,0 +1,129 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.Connectivity FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.Connectivity FastRobotInterface� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#ifndef _KUKA_FRI_CONNECTION_H +#define _KUKA_FRI_CONNECTION_H + + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + +/** + * \brief FRI connection interface. + * + * Connections to the KUKA Sunrise controller have to be implemented using + * this interface. + */ +class IConnection +{ + +public: + /** \brief Virtual destructor. */ + virtual ~IConnection() {} + + /** + * \brief Open a connection to the KUKA Sunrise controller. + * + * @param port The port ID + * @param remoteHost The address of the remote host + * @return True if connection was established + */ + virtual bool open(int port, const char * remoteHost) = 0; + + /** + * \brief Close a connection to the KUKA Sunrise controller. + */ + virtual void close() = 0; + + /** + * \brief Checks whether a connection to the KUKA Sunrise controller is established. + * + * @return True if connection is established + */ + virtual bool isOpen() const = 0; + + /** + * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. + * + * This method blocks until a new message arrives. + * @param buffer Pointer to the allocated buffer that will hold the FRI message + * @param maxSize Size in bytes of the allocated buffer + * @return Number of bytes received (0 when connection was terminated, negative in case of errors) + */ + virtual int receive(char * buffer, int maxSize) = 0; + + /** + * \brief Send a new FRI command message to the KUKA Sunrise controller. + * + * @param buffer Pointer to the buffer holding the FRI message + * @param size Size in bytes of the message to be send + * @return True if successful + */ + virtual bool send(const char * buffer, int size) = 0; + +}; + +} +} + + +#endif // _KUKA_FRI_CONNECTION_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friException.h b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friException.h new file mode 100644 index 00000000..2bda1ffd --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friException.h @@ -0,0 +1,152 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#ifndef _KUKA_FRI_EXCEPTION_H +#define _KUKA_FRI_EXCEPTION_H + +#include + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + +/** + * \brief Standard exception for the FRI Client + * + * \note For realtime considerations the internal message buffer is static. + * So don't use this exception in more than one thread per process. + */ +class FRIException +{ + +public: + /** + * \brief FRIException Constructor + * + * @param message Error message + */ + FRIException(const char * message) + { + strncpy(_buffer, message, sizeof(_buffer) - 1); + _buffer[sizeof(_buffer) - 1] = 0; // ensure string termination + printf("FRIException: "); + printf("%s", _buffer); + printf("\n"); + } + + /** + * \brief FRIException Constructor + * + * @param message Error message which may contain one "%s" parameter + * @param param1 First format parameter for parameter message. + */ + FRIException(const char * message, const char * param1) + { +#ifdef _MSC_VER + _snprintf( // visual studio compilers (up to VS 2013) only know this method +#else + snprintf( +#endif + _buffer, sizeof(_buffer), message, param1); + printf("FRIException: "); + printf("%s", _buffer); + printf("\n"); + } + + /** + * \brief FRIException Constructor + * + * @param message Error message which may contain two "%s" parameter + * @param param1 First format parameter for parameter message. + * @param param2 Second format parameter for parameter message. + */ + FRIException(const char * message, const char * param1, const char * param2) + { +#ifdef _MSC_VER + _snprintf( // visual studio compilers (up to VS 2013) only know this method +#else + snprintf( +#endif + _buffer, sizeof(_buffer), message, param1, param2); + printf("FRIException: "); + printf("%s", _buffer); + printf("\n"); + } + + /** + * \brief Get error string. + * @return Error message stored in the exception. + */ + const char * getErrorMessage() const {return _buffer;} + + /** \brief Virtual destructor. */ + virtual ~FRIException() {} + +protected: + static char _buffer[1024]; + +}; + +} +} + + +#endif // _KUKA_FRI_EXCEPTION_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friLBRClient.h b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friLBRClient.h new file mode 100644 index 00000000..06810d36 --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friLBRClient.h @@ -0,0 +1,143 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#ifndef _KUKA_FRI_LBR_CLIENT_H +#define _KUKA_FRI_LBR_CLIENT_H + +#include +#include +#include + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + +/** + * \brief Implementation of the IClient interface for the KUKA LBR (lightweight) robots. + * + * Provides access to the current LBR state and the possibility to send new + * commands to the LBR. + */ +class LBRClient : public IClient +{ + +public: + /** \brief Constructor. */ + LBRClient(); + + /** \brief Destructor. */ + ~LBRClient(); + + /** + * \brief Callback that is called whenever the FRI session state changes. + * + * @param oldState previous FRI session state + * @param newState current FRI session state + */ + virtual void onStateChange(ESessionState oldState, ESessionState newState); + + /** + * \brief Callback for the FRI session states 'Monitoring Wait' and 'Monitoring Ready'. + */ + virtual void monitor(); + + /** + * \brief Callback for the FRI session state 'Commanding Wait'. + */ + virtual void waitForCommand(); + + /** + * \brief Callback for the FRI session state 'Commanding'. + */ + virtual void command(); + + /** + * \brief Provide read access to the current robot state. + * + * @return Reference to the LBRState instance + */ + const LBRState & robotState() const {return _robotState;} + + /** + * \brief Provide write access to the robot commands. + * + * @return Reference to the LBRCommand instance + */ + LBRCommand & robotCommand() {return _robotCommand;} + +private: + LBRState _robotState; //!< wrapper class for the FRI monitoring message + LBRCommand _robotCommand; //!< wrapper class for the FRI command message + + /** + * \brief Method to create and initialize the client data structure (used internally). + * + * @return newly allocated client data structure + */ + virtual ClientData * createData(); + +}; + +} +} + + +#endif // _KUKA_FRI_LBR_CLIENT_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friLBRCommand.h b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friLBRCommand.h new file mode 100644 index 00000000..8787bfad --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friLBRCommand.h @@ -0,0 +1,160 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.Connectivity FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.Connectivity FastRobotInterface� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#ifndef _KUKA_FRI_LBR_COMMAND_H +#define _KUKA_FRI_LBR_COMMAND_H + + +// forward declarations +typedef struct _FRICommandMessage FRICommandMessage; + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + +/** + * \brief Wrapper class for the FRI command message for a KUKA LBR (lightweight) robot. + */ +class LBRCommand +{ + friend class LBRClient; + +public: + /** + * \brief Set the joint positions for the current interpolation step. + * + * This method is only effective when the client is in a commanding state. + * @param values Array with the new joint positions (in rad) + */ + void setJointPosition(const double * values); + + /** + * \brief Set the applied wrench vector of the current interpolation step. + * + * The wrench vector consists of: + * [F_x, F_y, F_z, tau_A, tau_B, tau_C] + * + * F ... forces (in N) applied along the Cartesian axes of the + * currently used motion center. + * tau ... torques (in Nm) applied along the orientation angles + * (Euler angles A, B, C) of the currently used motion center. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be Cartesian impedance control mode. The + * Client Command Mode has to be wrench. + * + * @param wrench Applied Cartesian wrench vector. + */ + void setWrench(const double * wrench); + + /** + * \brief Set the applied joint torques for the current interpolation step. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be joint impedance control mode. The + * Client Command Mode has to be torque. + * + * @param torques Array with the applied torque values (in Nm) + */ + void setTorque(const double * torques); + + /** + * \brief Set boolean output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Boolean value to set. + */ + void setBooleanIOValue(const char * name, const bool value); + + /** + * \brief Set digital output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Digital value to set. + */ + void setDigitalIOValue(const char * name, const unsigned long long value); + + /** + * \brief Set analog output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Analog value to set. + */ + void setAnalogIOValue(const char * name, const double value); + +protected: + static const int LBRCOMMANDMESSAGEID = 0x34001; //!< type identifier for the FRI command message corresponding to a KUKA LBR robot + FRICommandMessage * _cmdMessage; //!< FRI command message (protobuf struct) + FRIMonitoringMessage * _monMessage; //!< FRI monitoring message (protobuf struct) + +}; + +} +} + + +#endif // _KUKA_FRI_LBR_COMMAND_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friLBRState.h b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friLBRState.h new file mode 100644 index 00000000..8e25b443 --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friLBRState.h @@ -0,0 +1,276 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#ifndef _KUKA_FRI_LBR_STATE_H +#define _KUKA_FRI_LBR_STATE_H + +#include + +// forward declarations +typedef struct _FRIMonitoringMessage FRIMonitoringMessage; + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + +/** + * \brief Wrapper class for the FRI monitoring message for a KUKA LBR (lightweight) robot. + */ +class LBRState +{ + friend class LBRClient; + +public: + enum + { + NUMBER_OF_JOINTS = 7 //!< number of axes of the KUKA LBR robot + }; + + LBRState(); + + /** + * \brief Get the sample time in seconds. + * + * This is the period in which the KUKA Sunrise controller is sending + * FRI packets. + * @return sample time in seconds + */ + double getSampleTime() const; // sec + + /** + * \brief Get the current FRI session state. + * + * @return current FRI session state + */ + ESessionState getSessionState() const; + + /** + * \brief Get the current FRI connection quality. + * + * @return current FRI connection quality + */ + EConnectionQuality getConnectionQuality() const; + + /** + * \brief Get the current safety state of the KUKA Sunrise controller. + * + * @return current safety state + */ + ESafetyState getSafetyState() const; + + /** + * \brief Get the current operation mode of the KUKA Sunrise controller. + * + * @return current operation mode + */ + EOperationMode getOperationMode() const; + + /** + * \brief Get the accumulated drive state over all drives of the KUKA LBR controller. + * + * If the drive states differ between drives, the following rule applies: + * 1) The drive state is OFF if all drives are OFF. + * 2) The drive state is ACTIVE if all drives are ACTIVE. + * 3) otherwise the drive state is TRANSITIONING. + * @return accumulated drive state + */ + EDriveState getDriveState() const; + + /** + * \brief Get the client command mode specified by the client. + * + * @return the client command mode specified by the client. + */ + EClientCommandMode getClientCommandMode() const; + + /** + * \brief Get the overlay type specified by the client. + * + * @return the overlay type specified by the client. + */ + EOverlayType getOverlayType() const; + + /** + * \brief Get the control mode of the KUKA LBR robot. + * + * @return current control mode of the KUKA LBR robot. + */ + EControlMode getControlMode() const; + + /** + * \brief Get the timestamp of the current robot state in Unix time. + * + * This method returns the seconds since 0:00, January 1st, 1970 (UTC). + * Use getTimestampNanoSec() to increase your timestamp resolution when + * seconds are insufficient. + * @return timestamp encoded as Unix time (seconds) + */ + unsigned int getTimestampSec() const; + + /** + * \brief Get the nanoseconds elapsed since the last second (in Unix time). + * + * This method complements getTimestampSec() to get a high resolution + * timestamp. + * @return timestamp encoded as Unix time (nanoseconds part) + */ + unsigned int getTimestampNanoSec() const; + + /** + * \brief Get the currently measured joint positions of the robot. + * + * @return array of the measured joint positions in radians + */ + const double * getMeasuredJointPosition() const; + + /** + * \brief Get the last commanded joint positions of the robot. + * + * @return array of the commanded joint positions in radians + */ + const double * getCommandedJointPosition() const; + + /** + * \brief Get the currently measured joint torques of the robot. + * + * @return array of the measured torques in Nm + */ + const double * getMeasuredTorque() const; + + /** + * \brief Get the last commanded joint torques of the robot. + * + * @return array of the commanded torques in Nm + */ + const double * getCommandedTorque() const; + + /** + * \brief Get the currently measured external joint torques of the robot. + * + * The external torques corresponds to the measured torques when removing + * the torques induced by the robot itself. + * @return array of the external torques in Nm + */ + const double * getExternalTorque() const; + + /** + * \brief Get the joint positions commanded by the interpolator. + * + * When commanding a motion overlay in your robot application, this method + * will give access to the joint positions currently commanded by the + * motion interpolator. + * @throw FRIException This method will throw an FRIException during monitoring mode. + * @return array of the ipo joint positions in radians + */ + const double * getIpoJointPosition() const; + + /** + * \brief Get an indicator for the current tracking performance of the commanded robot. + * + * The tracking performance is an indicator on how well the commanded robot + * is able to follow the commands of the FRI client. The best possible value + * 1.0 is reached when the robot executes the given commands instantaneously. + * The tracking performance drops towards 0 when latencies are induced, + * e.g. when the commanded velocity, acceleration or jerk exceeds the + * capabilities of the robot. + * The tracking performance will always be 0 when the session state does + * not equal COMMANDING_ACTIVE. + * @return current tracking performance + */ + double getTrackingPerformance() const; + + /** + * \brief Get boolean IO value. + * + * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's boolean value. + */ + bool getBooleanIOValue(const char * name) const; + + /** + * \brief Get digital IO value. + * + * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's digital value. + */ + unsigned long long getDigitalIOValue(const char * name) const; + + /** + * \brief Get analog IO value. + * + * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's analog value. + */ + double getAnalogIOValue(const char * name) const; + +protected: + static const int LBRMONITORMESSAGEID = 0x245142; //!< type identifier for the FRI monitoring message corresponding to a KUKA LBR robot + FRIMonitoringMessage * _message; //!< FRI monitoring message (protobuf struct) +}; + +} +} + + +#endif // _KUKA_FRI_LBR_STATE_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friTransformationClient.h b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friTransformationClient.h new file mode 100644 index 00000000..74c1bf8b --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friTransformationClient.h @@ -0,0 +1,266 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#ifndef _KUKA_FRI_TRANSFORMATION_CLIENT_H +#define _KUKA_FRI_TRANSFORMATION_CLIENT_H + +#include +#include + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + +// forward declaration +struct ClientData; + +/** + * \brief Abstract FRI transformation client. + * + * A transformation client enables the user to send transformation matrices cyclically to the + * KUKA Sunrise controller for manipulating the transformations of dynamic frames in the + * Sunrise scenegraph. + * Usually, these matrices will be provided by external sensors. + *
+ * Custom transformation clients have to be derived from this class and need to + * implement the provide() callback. This callback is called once by the + * client application whenever a new FRI message arrives. + * + * This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions. + */ +class TransformationClient +{ + + friend class ClientApplication; + +public: + /** + *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
+ * \brief Constructor. + **/ + TransformationClient(); + + /**
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
+ * \brief Virtual destructor. + **/ + virtual ~TransformationClient(); + + /** + *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
+ * \brief Callback which is called whenever a new FRI message arrives. + * + * In this callback all requested transformations have to be set. + * + * \see getRequestedTransformationIDs(), setTransformation() + */ + virtual void provide() = 0; + + /** + * \brief Get the sample time in seconds. + * + * This is the period in which the KUKA Sunrise controller is sending + * FRI packets. + * @return sample time in seconds + */ + double getSampleTime() const; // sec + + /** + * \brief Get the current FRI connection quality. + * + * @return current FRI connection quality + */ + EConnectionQuality getConnectionQuality() const; + + /** + *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
+ * \brief Returns a vector of identifiers of all requested transformation matrices. + * + * The custom TransformationClient has to provide data for transformation matrices with these + * identifiers. + * + * @return reference to vector of IDs of requested transformations + */ + const std::vector & getRequestedTransformationIDs() const; + + /** + *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
+ * + * \brief Get the timestamp of the current received FRI monitor message in Unix time. + * + * This method returns the seconds since 0:00, January 1st, 1970 (UTC). + * Use getTimestampNanoSec() to increase your timestamp resolution when + * seconds are insufficient. + * + * @return timestamp encoded as Unix time (seconds) + */ + unsigned int getTimestampSec() const; + + /** + *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
+ * \brief Get the nanoseconds elapsed since the last second (in Unix time). + * + * This method complements getTimestampSec() to get a high resolution + * timestamp. + * + * @return timestamp encoded as Unix time (nanoseconds part) + */ + unsigned int getTimestampNanoSec() const; + + /** + *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
+ * \brief Provides a requested transformation matrix. + * + * A transformation matrix has 3x4 elements. It consists of a rotational matrix (3x3 elements) + * and a translational vector (3x1 elements). The complete transformation matrix has the + * following structure:
+ * [Transformation(3x4)] = [Rotation(3x3) | Translation(3x1) ] + *

+ * All provided transformation matrices need a timestamp that corresponds to their + * time of acquisition. This timestamp must be synchronized to the timestamp + * provided by the KUKA Sunrise controller (see getTimestampSec(), getTimestampNanoSec()). + *

+ * If an update to the last transformation is not yet available when the provide() + * callback is executed, the last transformation (including its timestamp) should be + * repeated until a new transformation is available. + * + * @throw FRIException Throws a FRIException if the maximum number of transformations is exceeded. + * @param transformationID Identifier string of the transformation matrix + * @param transformationMatrix Provided transformation matrix + * @param timeSec Timestamp encoded as Unix time (seconds) + * @param timeNanoSec Timestamp encoded as Unix time (nanoseconds part) + */ + void setTransformation( + const char * transformationID, const double transformationMatrix[3][4], + unsigned int timeSec, unsigned int timeNanoSec); + + /** + * \brief Set boolean output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Boolean value to set. + */ + void setBooleanIOValue(const char * name, const bool value); + + /** + * \brief Set digital output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Digital value to set. + */ + void setDigitalIOValue(const char * name, const unsigned long long value); + + /** + * \brief Set analog output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Analog value to set. + */ + void setAnalogIOValue(const char * name, const double value); + + /** + * \brief Get boolean IO value. + * + * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's boolean value. + */ + bool getBooleanIOValue(const char * name) const; + + /** + * \brief Get digital IO value. + * + * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's digital value. + */ + unsigned long long getDigitalIOValue(const char * name) const; + + /** + * \brief Get analog IO value. + * + * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's analog value. + */ + double getAnalogIOValue(const char * name) const; + +private: + ClientData * _data; //!< the client data structure + + /** + * \brief Method to link the client data structure (used internally). + * + * @param clientData the client data structure + */ + void linkData(ClientData * clientData); + +}; + +} +} + +#endif // _KUKA_FRI_TRANSFORMATION_CLIENT_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friUdpConnection.h b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friUdpConnection.h new file mode 100644 index 00000000..ff5835d7 --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v1_5/fri_client_sdk/friUdpConnection.h @@ -0,0 +1,161 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#ifndef _KUKA_FRI_UDP_CONNECTION_H +#define _KUKA_FRI_UDP_CONNECTION_H + +#include + +#ifdef _WIN32 + #include +#else +// if linux or a other unix system is used, select uses the following include + #ifdef __unix__ + #include + #endif +// for VxWorks + #ifdef VXWORKS + #include + #include + #endif + #include + #include +#endif + +#include + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + +/** + * \brief This class implements the IConnection interface using UDP sockets. + */ +class UdpConnection : public IConnection +{ + +public: + /** + * \brief Constructor with an optional parameter for setting a receive timeout. + * + * @param receiveTimeout Timeout (in ms) for receiving a UDP message (0 = wait forever) + * */ + UdpConnection(unsigned int receiveTimeout = 0); + + /** \brief Destructor. */ + ~UdpConnection(); + + /** + * \brief Open a connection to the KUKA Sunrise controller. + * + * @param port The port ID for the connection + * @param controllerAddress The IPv4 address of the KUKA Sunrise controller. + * If NULL, the FRI Client accepts connections from any + * address. + * @return True if connection was established, false otherwise + */ + virtual bool open(int port, const char * controllerAddress = NULL); + + /** + * \brief Close a connection to the KUKA Sunrise controller. + */ + virtual void close(); + + /** + * \brief Checks whether a connection to the KUKA Sunrise controller is established. + * + * @return True if connection is established + */ + virtual bool isOpen() const; + + /** + * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. + * + * This method blocks until a new message arrives. + * @param buffer Pointer to the allocated buffer that will hold the FRI message + * @param maxSize Size in bytes of the allocated buffer + * @return Number of bytes received (0 when connection was terminated, + * negative in case of errors or receive timeout) + */ + virtual int receive(char * buffer, int maxSize); + + /** + * \brief Send a new FRI command message to the KUKA Sunrise controller. + * + * @param buffer Pointer to the buffer holding the FRI message + * @param size Size in bytes of the message to be send + * @return True if successful + */ + virtual bool send(const char * buffer, int size); + +private: + int _udpSock; //!< UDP socket handle + struct sockaddr_in _controllerAddr; //!< the controller's socket address + unsigned int _receiveTimeout; + fd_set _filedescriptor; + +}; + +} +} + + +#endif // _KUKA_FRI_UDP_CONNECTION_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/HWIFClientApplication.hpp b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/HWIFClientApplication.hpp new file mode 100644 index 00000000..b1b7c62c --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/HWIFClientApplication.hpp @@ -0,0 +1,34 @@ +#ifndef FRI__HWIFCLIENTAPPLICATION_HPP_ +#define FRI__HWIFCLIENTAPPLICATION_HPP_ + +#include + +#include +#include +#include +#include +#include + + +namespace KUKA +{ +namespace FRI +{ + +class HWIFClientApplication : public ClientApplication +{ +public: + HWIFClientApplication(IConnection & connection, IClient & client); + + bool client_app_read(); + void client_app_update(); + bool client_app_write(); + +private: + int size_; +}; + +} +} // namespace KUKA::FRI + +#endif // FRI__HWIFCLIENTAPPLICATION_HPP_ diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friClientApplication.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friClientApplication.h new file mode 100644 index 00000000..5a61ebd0 --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friClientApplication.h @@ -0,0 +1,155 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ + +#ifndef _KUKA_FRI_CLIENT_APPLICATION_H +#define _KUKA_FRI_CLIENT_APPLICATION_H + +/** Kuka namespace */ +namespace KUKA +{ +/** Fast Robot Interface (FRI) namespace */ +namespace FRI +{ + + // forward declarations + class IClient; + class TransformationClient; + class IConnection; + struct ClientData; + + /** + * \brief FRI client application class. + * + * A client application takes an instance of the IConnection interface and + * an instance of an IClient interface to provide the functionality + * needed to set up an FRI client application. It can be used to easily + * integrate the FRI client code within other applications. + * The algorithmic functionality of an FRI client application is implemented + * using the IClient interface. + */ + class ClientApplication + { + + public: + + /** + * \brief Constructor without transformation client. + * + * This constructor takes an instance of the IConnection interface and + * an instance of the IClient interface as parameters. + * @param connection FRI connection class + * @param client FRI client class + */ + ClientApplication(IConnection& connection, IClient& client); + + /** + * \brief Constructor with transformation client. + * + * This constructor takes an instance of the IConnection interface and + * an instance of the IClient interface and an instance of a + * TransformationClient as parameters. + * @param connection FRI connection class + * @param client FRI client class + * @param trafoClient FRI transformation client class + */ + ClientApplication(IConnection& connection, IClient& client, TransformationClient& trafoClient); + + /** \brief Destructor. */ + ~ClientApplication(); + + /** + * \brief Connect the FRI client application with a KUKA Sunrise controller. + * + * @param port The port ID + * @param remoteHost The address of the remote host + * @return True if connection was established + */ + bool connect(int port, const char *remoteHost = NULL); + + /** + * \brief Disconnect the FRI client application from a KUKA Sunrise controller. + */ + void disconnect(); + + /** + * \brief Run a single processing step. + * + * The processing step consists of receiving a new FRI monitoring message, + * calling the corresponding client callback and sending the resulting + * FRI command message back to the KUKA Sunrise controller. + * @return True if all of the substeps succeeded. + */ + bool step(); + + protected: + + IConnection& _connection; //!< connection interface + IClient* _robotClient; //!< robot client interface + TransformationClient* _trafoClient; //!< transformation client interface + ClientData* _data; //!< client data structure (for internal use) + + }; + +} +} + + +#endif // _KUKA_FRI_CLIENT_APPLICATION_H + diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friClientIf.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friClientIf.h new file mode 100644 index 00000000..1a2abda9 --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friClientIf.h @@ -0,0 +1,210 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_CLIENT_H +#define _KUKA_FRI_CLIENT_H + + + +/** Kuka namespace */ +namespace KUKA +{ +/** Fast Robot Interface (FRI) namespace */ +namespace FRI +{ + + // forward declarations + struct ClientData; + + + /** \brief Enumeration of the FRI session state. */ + enum ESessionState + { + IDLE = 0, //!< no session available + MONITORING_WAIT = 1, //!< monitoring mode with insufficient connection quality + MONITORING_READY = 2, //!< monitoring mode with connection quality sufficient for command mode + COMMANDING_WAIT = 3, //!< command mode about to start (overlay motion queued) + COMMANDING_ACTIVE = 4 //!< command mode active + }; + + /** \brief Enumeration of the FRI connection quality. */ + enum EConnectionQuality + { + POOR = 0, //!< poor connection quality + FAIR = 1, //!< connection quality insufficient for command mode + GOOD = 2, //!< connection quality sufficient for command mode + EXCELLENT = 3 //!< excellent connection quality + }; + + /** \brief Enumeration of the controller's safety state. */ + enum ESafetyState + { + NORMAL_OPERATION = 0, //!< No safety stop request present. + SAFETY_STOP_LEVEL_0 = 1,//!< Safety stop request STOP0 or STOP1 present. + SAFETY_STOP_LEVEL_1 = 2,//!< Safety stop request STOP1 (on-path) present. + SAFETY_STOP_LEVEL_2 = 3 //!< Safety stop request STOP2 present. + }; + + /** \brief Enumeration of the controller's current mode of operation. */ + enum EOperationMode + { + TEST_MODE_1 = 0, //!< test mode 1 with reduced speed (T1) + TEST_MODE_2 = 1, //!< test mode 2 (T2) + AUTOMATIC_MODE = 2 //!< automatic operation mode (AUT) + }; + + /** \brief Enumeration of a drive's state. */ + enum EDriveState + { + OFF = 0, //!< drive is not being used + TRANSITIONING = 1, //!< drive is in a transitioning state (before or after motion) + ACTIVE = 2 //!< drive is being actively commanded + }; + + /** \brief Enumeration of control mode. */ + enum EControlMode + { + POSITION_CONTROL_MODE = 0, //!< position control mode + CART_IMP_CONTROL_MODE = 1, //!< cartesian impedance control mode + JOINT_IMP_CONTROL_MODE = 2, //!< joint impedance control mode + NO_CONTROL = 3 //!< drives are not used + }; + + + /** \brief Enumeration of the client command mode. */ + enum EClientCommandMode + { + NO_COMMAND_MODE = 0, //!< no client command mode available + JOINT_POSITION = 1, //!< commanding joint positions by the client + WRENCH = 2, //!< commanding wrenches and joint positions by the client + TORQUE = 3, //!< commanding joint torques and joint positions by the client + CARTESIAN_POSE = 4 //!< commanding Cartesian poses by the client + }; + + /** \brief Enumeration of the overlay type. */ + enum EOverlayType + { + NO_OVERLAY = 0, //!< no overlay type available + JOINT = 1, //!< joint overlay + CARTESIAN = 2 //!< cartesian overlay + }; + + /** \brief Enumeration of redundancy strategies. */ + enum ERedundancyStrategy + { + E1 = 0, //!< E1 redundancy strategy + NO_STRATEGY = 4 //!< No redundancy strategy + }; + + /** + * \brief FRI client interface. + * + * This is the callback interface that should be implemented by FRI clients. + * Callbacks are automatically called by the client application + * (ClientApplication) whenever new FRI messages arrive. + */ + class IClient + { + friend class ClientApplication; + + public: + + /** \brief Virtual destructor. */ + virtual ~IClient() {} + + /** + * \brief Callback that is called whenever the FRI session state changes. + * + * @param oldState previous FRI session state + * @param newState current FRI session state + */ + virtual void onStateChange(ESessionState oldState, ESessionState newState) = 0; + + /** + * \brief Callback for the FRI session states 'Monitoring Wait' and 'Monitoring Ready'. + */ + virtual void monitor() = 0; + + /** + * \brief Callback for the FRI session state 'Commanding Wait'. + */ + virtual void waitForCommand() = 0; + + /** + * \brief Callback for the FRI session state 'Commanding'. + */ + virtual void command() = 0; + + protected: + + /** + * \brief Method to create and initialize the client data structure (used internally). + * + * @return newly allocated client data structure + */ + virtual ClientData* createData() = 0; + + + }; + +} +} + + +#endif // _KUKA_FRI_CLIENT_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friConnectionIf.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friConnectionIf.h new file mode 100644 index 00000000..0cd10666 --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friConnectionIf.h @@ -0,0 +1,130 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_CONNECTION_H +#define _KUKA_FRI_CONNECTION_H + + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + /** + * \brief FRI connection interface. + * + * Connections to the KUKA Sunrise controller have to be implemented using + * this interface. + */ + class IConnection + { + + public: + + /** \brief Virtual destructor. */ + virtual ~IConnection() {} + + /** + * \brief Open a connection to the KUKA Sunrise controller. + * + * @param port The port ID + * @param remoteHost The address of the remote host + * @return True if connection was established + */ + virtual bool open(int port, const char *remoteHost) = 0; + + /** + * \brief Close a connection to the KUKA Sunrise controller. + */ + virtual void close() = 0; + + /** + * \brief Checks whether a connection to the KUKA Sunrise controller is established. + * + * @return True if connection is established + */ + virtual bool isOpen() const = 0; + + /** + * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. + * + * This method blocks until a new message arrives. + * @param buffer Pointer to the allocated buffer that will hold the FRI message + * @param maxSize Size in bytes of the allocated buffer + * @return Number of bytes received (0 when connection was terminated, negative in case of errors) + */ + virtual int receive(char *buffer, int maxSize) = 0; + + /** + * \brief Send a new FRI command message to the KUKA Sunrise controller. + * + * @param buffer Pointer to the buffer holding the FRI message + * @param size Size in bytes of the message to be send + * @return True if successful + */ + virtual bool send(const char* buffer, int size) = 0; + + }; + +} +} + + +#endif // _KUKA_FRI_CONNECTION_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friDataHelper.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friDataHelper.h new file mode 100644 index 00000000..e287ebbc --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friDataHelper.h @@ -0,0 +1,129 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_DATA_HELPER_H +#define _KUKA_FRI_DATA_HELPER_H + +#include + +namespace KUKA +{ + namespace FRI + { + + /** + * \brief Data helper class containing conversion functions. + */ + struct DataHelper + { + + /** + * \brief Helper enum to access quaternion entries. + */ + enum QUATERNION_IDX + { + QUAT_TX = 0, + QUAT_TY, + QUAT_TZ, + QUAT_QW, + QUAT_QX, + QUAT_QY, + QUAT_QZ + }; + + + /** + * \brief Function to convert a matrix transformation to a normalized quaternion transformation. + * + * The resulting quaternion transformation is provided as [t_x, t_y, t_z, q_w, q_x, q_y, q_z], + * with a unit quaternion, i.e. the length of vector [q_w, q_x, q_y, q_z] must be 1. + * The input transformation matrix has 3x4 elements. It consists of a rotational matrix (3x3 elements) + * and a translational vector (3x1 elements). The complete transformation matrix has the + * following structure: + * [Transformation(3x4)] = [Rotation(3x3) | Translation(3x1) ] + * + * @param[in] matrixTrafo given matrix transformation + * @param[out] quaternionTrafo resulting quaternion transformation + */ + static void convertTrafoMatrixToQuaternion(const double (&matrixTrafo)[3][4], + double (&quaternionTrafo)[7]); + + + /** + * \brief Function to convert a quaternion transformation to a matrix transformation. + * + * The input quaternion transformation must be provided as [t_x, t_y, t_z, q_w, q_x, q_y, q_z], + * with a unit quaternion, i.e. the length of vector [q_w, q_x, q_y, q_z] must be 1. + * The output transformation matrix has 3x4 elements. It consists of a rotational matrix (3x3 elements) + * and a translational vector (3x1 elements). The complete transformation matrix has the + * following structure: + * [Transformation(3x4)] = [Rotation(3x3) | Translation(3x1) ] + * + * @param[in] quaternionTrafo given quaternion transformation + * @param[out] matrixTrafo resulting matrix transformation + */ + static void convertTrafoQuaternionToMatrix(const double(&quaternionTrafo)[7], + double(&matrixTrafo)[3][4]); + + }; + + } +} + +#endif // _KUKA_FRI_DATA_HELPER_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friException.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friException.h new file mode 100644 index 00000000..ebf33c99 --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friException.h @@ -0,0 +1,153 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_EXCEPTION_H +#define _KUKA_FRI_EXCEPTION_H + +#include "stdio.h" + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + /** + * \brief Standard exception for the FRI Client + * + * \note For realtime considerations the internal message buffer is static. + * So don't use this exception in more than one thread per process. + */ + class FRIException + { + + public: + + /** + * \brief FRIException Constructor + * + * @param message Error message + */ + FRIException(const char* message) + { + strncpy(_buffer, message, sizeof(_buffer) - 1); + _buffer[sizeof(_buffer) - 1] = 0; // ensure string termination + printf("FRIException: "); + printf(_buffer); + printf("\n"); + } + + /** + * \brief FRIException Constructor + * + * @param message Error message which may contain one "%s" parameter + * @param param1 First format parameter for parameter message. + */ + FRIException(const char* message, const char* param1) + { +#ifdef _MSC_VER + _snprintf( // visual studio compilers (up to VS 2013) only know this method +#else + snprintf( +#endif + _buffer, sizeof(_buffer), message, param1); + printf("FRIException: "); + printf(_buffer); + printf("\n"); + } + + /** + * \brief FRIException Constructor + * + * @param message Error message which may contain two "%s" parameter + * @param param1 First format parameter for parameter message. + * @param param2 Second format parameter for parameter message. + */ + FRIException(const char* message, const char* param1, const char* param2) + { +#ifdef _MSC_VER + _snprintf( // visual studio compilers (up to VS 2013) only know this method +#else + snprintf( +#endif + _buffer, sizeof(_buffer), message, param1, param2); + printf("FRIException: "); + printf(_buffer); + printf("\n"); + } + + /** + * \brief Get error string. + * @return Error message stored in the exception. + */ + const char* getErrorMessage() const { return _buffer; } + + /** \brief Virtual destructor. */ + virtual ~FRIException() {} + + protected: + static char _buffer[1024]; + + }; + +} +} + + +#endif // _KUKA_FRI_EXCEPTION_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRClient.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRClient.h new file mode 100644 index 00000000..a1e7bce9 --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRClient.h @@ -0,0 +1,145 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_LBR_CLIENT_H +#define _KUKA_FRI_LBR_CLIENT_H + +#include +#include +#include + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + /** + * \brief Implementation of the IClient interface for the KUKA LBR (lightweight) robots. + * + * Provides access to the current LBR state and the possibility to send new + * commands to the LBR. + */ + class LBRClient : public IClient + { + + public: + + /** \brief Constructor. */ + LBRClient(); + + /** \brief Destructor. */ + ~LBRClient(); + + /** + * \brief Callback that is called whenever the FRI session state changes. + * + * @param oldState previous FRI session state + * @param newState current FRI session state + */ + virtual void onStateChange(ESessionState oldState, ESessionState newState); + + /** + * \brief Callback for the FRI session states 'Monitoring Wait' and 'Monitoring Ready'. + */ + virtual void monitor(); + + /** + * \brief Callback for the FRI session state 'Commanding Wait'. + */ + virtual void waitForCommand(); + + /** + * \brief Callback for the FRI session state 'Commanding'. + */ + virtual void command(); + + /** + * \brief Provide read access to the current robot state. + * + * @return Reference to the LBRState instance + */ + const LBRState& robotState() const { return _robotState; } + + /** + * \brief Provide write access to the robot commands. + * + * @return Reference to the LBRCommand instance + */ + LBRCommand& robotCommand() { return _robotCommand; } + + private: + + LBRState _robotState; //!< wrapper class for the FRI monitoring message + LBRCommand _robotCommand; //!< wrapper class for the FRI command message + + /** + * \brief Method to create and initialize the client data structure (used internally). + * + * @return newly allocated client data structure + */ + virtual ClientData* createData(); + + }; + +} +} + + +#endif // _KUKA_FRI_LBR_CLIENT_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRCommand.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRCommand.h new file mode 100644 index 00000000..190771e4 --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRCommand.h @@ -0,0 +1,206 @@ +/** + + The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + + \file + \version {2.5} + */ +#ifndef _KUKA_FRI_LBR_COMMAND_H +#define _KUKA_FRI_LBR_COMMAND_H + +#include + +// forward declarations +typedef struct _FRICommandMessage FRICommandMessage; + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + +/** + * \brief Wrapper class for the FRI command message for a KUKA LBR (leightweight) robot. + */ +class LBRCommand +{ + friend class LBRClient; + +public: + + /** + * \brief Set the joint positions for the current interpolation step. + * + * This method is only effective when the client is in a commanding state. + * @param values Array with the new joint positions (in rad) + */ + void setJointPosition(const double* values); + + /** + * \brief Set the applied wrench vector of the current interpolation step. + * + * The wrench vector consists of: + * [F_x, F_y, F_z, tau_A, tau_B, tau_C] + * + * F ... forces (in N) applied along the Cartesian axes of the + * currently used motion center. + * tau ... torques (in Nm) applied along the orientation angles + * (Euler angles A, B, C) of the currently used motion center. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be Cartesian impedance control mode. The + * Client Command Mode has to be wrench. + * + * @param wrench Applied Cartesian wrench vector. + */ + void setWrench(const double* wrench); + + /** + * \brief Set the applied joint torques for the current interpolation step. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be joint impedance control mode. The + * Client Command Mode has to be torque. + * + * @param torques Array with the applied torque values (in Nm) + */ + void setTorque(const double* torques); + + /** + * \brief Set the Cartesian pose for the current interpolation step. + * The pose describes the configured TCP relative to the configured base frame. + * + * The quaternion vector consists of: [t_x, t_y, t_z, q_w, q_x, q_y, q_z], + * where the first three values describe the translation t as a regular 3-dim + * vector, while the last four values describe the rotation q as an unit quaternion. + * + * The unit quaternion q is a 4-dimensional vector, describing the orientation as + * one rotation around the vector v = [v_x, v_y, v_z] about the angle phi. + * + * [ q_w ] = [ cos (phi/2) ] + * [ q_x ] = [ sin (phi/2) * v_x ] + * [ q_y ] = [ sin (phi/2) * v_y ] + * [ q_z ] = [ sin (phi/2) * v_z ] + * + * Setting a redundancy value is optional. If no value is provided, the interpolated + * redundancy value is used. So far, only the E1 redundancy strategy is provided. + * + * This method is only effective when the client is in a commanding state. + * + * @param cartesianPoseQuaternion Array with the new Cartesian pose + * @param redundancyValue pointer to redundancy value, NULL for default behavior + */ + void setCartesianPose(const double* cartesianPoseQuaternion, + double const * const redundancyValue = NULL); + + /** + * \brief Set the Cartesian pose for the current interpolation step as a 3x4 matrix. + * The pose describes the configured TCP relative to the configured base frame. + * + * The first 3 columns represent a rotational matrix and the 4th column a 3-dim + * translation vector for directions x, y, z (in mm). + * + * Setting a redundancy value is optional. If no value is provided, the interpolated + * redundancy value is used. So far, only the E1 redundancy strategy is provided. + * + * @param cartesianPoseAsMatrix 2-dim double array where the requested 3x4 matrix + * should be stored + * @param redundancyValue pointer to redundancy value, NULL for default behavior + */ + void setCartesianPoseAsMatrix(const double(&cartesianPoseAsMatrix)[3][4], + double const * const redundancyValue = NULL); + + /** + * \brief Set boolean output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Boolean value to set. + */ + void setBooleanIOValue(const char* name, const bool value); + + /** + * \brief Set digital output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Digital value to set. + */ + void setDigitalIOValue(const char* name, const unsigned long long value); + + /** + * \brief Set analog output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Analog value to set. + */ + void setAnalogIOValue(const char* name, const double value); + +protected: + + static const int LBRCOMMANDMESSAGEID = 0x34001; //!< type identifier for the FRI command message corresponding to a KUKA LBR robot + FRICommandMessage* _cmdMessage; //!< FRI command message (protobuf struct) + FRIMonitoringMessage* _monMessage; //!< FRI monitoring message (protobuf struct) + +}; + +} +} + +#endif // _KUKA_FRI_LBR_COMMAND_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRState.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRState.h new file mode 100644 index 00000000..c605c014 --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRState.h @@ -0,0 +1,369 @@ +/** + + The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + + \file + \version {2.5} + */ +#ifndef _KUKA_FRI_LBR_STATE_H +#define _KUKA_FRI_LBR_STATE_H + +#include + +// forward declarations +typedef struct _FRIMonitoringMessage FRIMonitoringMessage; + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + +/** + * \brief Wrapper class for the FRI monitoring message for a KUKA LBR (lightweight) robot. + */ +class LBRState +{ + friend class LBRClient; + +public: + + enum + { + NUMBER_OF_JOINTS = 7 //!< number of joints of the KUKA LBR robot + }; + + LBRState(); + + /** + * \brief Get the sample time in seconds. + * + * This is the period in which the KUKA Sunrise controller is sending + * FRI packets. + * @return sample time in seconds + */ + double getSampleTime() const; // sec + + /** + * \brief Get the current FRI session state. + * + * @return current FRI session state + */ + ESessionState getSessionState() const; + + /** + * \brief Get the current FRI connection quality. + * + * @return current FRI connection quality + */ + EConnectionQuality getConnectionQuality() const; + + /** + * \brief Get the current safety state of the KUKA Sunrise controller. + * + * @return current safety state + */ + ESafetyState getSafetyState() const; + + /** + * \brief Get the current operation mode of the KUKA Sunrise controller. + * + * @return current operation mode + */ + EOperationMode getOperationMode() const; + + /** + * \brief Get the accumulated drive state over all drives of the KUKA LBR controller. + * + * If the drive states differ between drives, the following rule applies: + * 1) The drive state is OFF if all drives are OFF. + * 2) The drive state is ACTIVE if all drives are ACTIVE. + * 3) otherwise the drive state is TRANSITIONING. + * @return accumulated drive state + */ + EDriveState getDriveState() const; + + /** + * \brief Get the client command mode specified by the client. + * + * @return the client command mode specified by the client. + */ + EClientCommandMode getClientCommandMode() const; + + /** + * \brief Get the overlay type specified by the client. + * + * @return the overlay type specified by the client. + */ + EOverlayType getOverlayType() const; + + /** + * \brief Get the control mode of the KUKA LBR robot. + * + * @return current control mode of the KUKA LBR robot. + */ + EControlMode getControlMode() const; + + /** + * \brief Get the timestamp of the current robot state in Unix time. + * + * This method returns the seconds since 0:00, January 1st, 1970 (UTC). + * Use getTimestampNanoSec() to increase your timestamp resolution when + * seconds are insufficient. + * @return timestamp encoded as Unix time (seconds) + */ + unsigned int getTimestampSec() const; + + /** + * \brief Get the nanoseconds elapsed since the last second (in Unix time). + * + * This method complements getTimestampSec() to get a high resolution + * timestamp. + * @return timestamp encoded as Unix time (nanoseconds part) + */ + unsigned int getTimestampNanoSec() const; + + /** + * \brief Get the currently measured joint positions of the robot. + * + * @return array of the measured joint positions in radians + */ + const double* getMeasuredJointPosition() const; + + /** + * \brief Get the currently measured joint torques of the robot. + * + * @return array of the measured torques in Nm + */ + const double* getMeasuredTorque() const; + + /** + * \brief Get the last commanded joint torques of the robot. + * + * @return array of the commanded torques in Nm + */ + const double* getCommandedTorque() const; + + /** + * \brief Get the currently measured external joint torques of the robot. + * + * The external torques corresponds to the measured torques when removing + * the torques induced by the robot itself. + * @return array of the external torques in Nm + */ + const double* getExternalTorque() const; + + /** + * \brief Get the joint positions commanded by the interpolator. + * + * When commanding a motion overlay in your robot application, this method + * will give access to the joint positions currently commanded by the + * motion interpolator. + * @throw FRIException This method will throw a FRIException if no FRI Joint Overlay is active. + * @return array of the ipo joint positions in radians + */ + const double* getIpoJointPosition() const; + + /** + * \brief Get an indicator for the current tracking performance of the commanded robot. + * + * The tracking performance is an indicator on how well the commanded robot + * is able to follow the commands of the FRI client. The best possible value + * 1.0 is reached when the robot executes the given commands instantaneously. + * The tracking performance drops towards 0 when latencies are induced, + * e.g. when the commanded velocity, acceleration or jerk exceeds the + * capabilities of the robot. + * The tracking performance will always be 0 when the session state does + * not equal COMMANDING_ACTIVE. + * @return current tracking performance + */ + double getTrackingPerformance() const; + + /** + * \brief Get boolean IO value. + * + * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's boolean value. + */ + bool getBooleanIOValue(const char* name) const; + + /** + * \brief Get digital IO value. + * + * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's digital value. + */ + unsigned long long getDigitalIOValue(const char* name) const; + + /** + * \brief Get analog IO value. + * + * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's analog value. + */ + double getAnalogIOValue(const char* name) const; + + /** + * \brief Get the currently measured Cartesian pose of the robot as a quaternion + * transformation vector. The pose describes the robot tcp relative to the + * base frame. + * + * The quaternion transformation vector consists of: [t_x, t_y, t_z, q_w, q_x, q_y, q_z] + * + * , where the first three values describe the translation t as a regular 3-dim + * vector, while the last four values describe the rotation q as an unit quaternion. + * + * The unit quaternion q is a 4-dimensional vector, describing the orientation as + * one rotation around the vector v = [v_x, v_y, v_z] about the angle phi. + * + * [ q_w ] = [ cos (phi/2) ] + * [ q_x ] = [ sin (phi/2) * v_x ] + * [ q_y ] = [ sin (phi/2) * v_y ] + * [ q_z ] = [ sin (phi/2) * v_z ] + * + * @return measured Cartesian pose as 7-dim quaternion transformation (translation in mm) + */ + const double* getMeasuredCartesianPose() const; + + /** + * \brief Get the currently measured Cartesian pose of the robot as a 3x4 transformation matrix. + * The pose describes the robot tcp relative to the base frame. + * + * The first 3 columns represent a rotational matrix and the 4th column a 3-dim + * translation vector for directions x, y, z (in mm). + * + * @param measuredCartesianPose 2-dim double array where the requested 3x4 matrix + * should be stored + */ + void getMeasuredCartesianPoseAsMatrix(double(&measuredCartesianPose)[3][4]) const; + + /** + * \brief Get the currently interpolated Cartesian pose of the robot as a quaternion + * transformation vector. The pose describes the robot tcp relative to the + * base frame. + * + * The quaternion transformation vector consists of: [t_x, t_y, t_z, q_w, q_x, q_y, q_z] + * + * , where the first three values describe the translation t as a regular 3-dim + * vector, while the last four values describe the rotation q as an unit quaternion. + * + * The unit quaternion q is a 4-dimensional vector, describing the orientation as + * one rotation around the vector v = [v_x, v_y, v_z] about the angle phi. + * + * [ q_w ] = [ cos (phi/2) ] + * [ q_x ] = [ sin (phi/2) * v_x ] + * [ q_y ] = [ sin (phi/2) * v_y ] + * [ q_z ] = [ sin (phi/2) * v_z ] + * + * @throw FRIException This method will throw a FRIException if no FRI Cartesian Overlay is active. + * @return ipo Cartesian pose as 7-dim quaternion transformation (translation in mm) + */ + const double* getIpoCartesianPose() const; + + /** + * \brief Get the currently interpolated Cartesian pose of the robot as a 3x4 transformation matrix. + * The pose describes the robot tcp relative to the base frame. + * + * The first 3 columns represent a rotational matrix and the 4th column a 3-dim + * translation vector for directions x, y, z (in mm). + * + * @throw FRIException This method will throw a FRIException if no FRI Cartesian Overlay is active. + * @param ipoCartesianPose 2-dim double array where the requested 3x4 matrix should be stored + */ + void getIpoCartesianPoseAsMatrix(double(&ipoCartesianPose)[3][4]) const; + + /** + * \brief Get the currently measured redundancy value. + * + * So far, only the E1 redundancy strategy is provided, meaning the value represents the joint position + * of A3 in radians. + * + * @param measured redundancy value in radians + */ + double getMeasuredRedundancyValue() const; + + /** + * \brief Get the current redundancy value of the interpolator. + * + * So far, only the E1 redundancy strategy is provided, meaning the value represents the joint position + * of A3 in radians. + * + * @param redundancy value of the interpolator in radians + */ + double getIpoRedundancyValue() const; + + /** + * \brief Get the redundancy strategy. + * + * So far, only the E1 redundancy strategy is provided, meaning the value represents the joint position + * of A3 in radians. + * + * @param redundancy strategy + */ + ERedundancyStrategy getRedundancyStrategy() const; + +protected: + + static const int LBRMONITORMESSAGEID = 0x245142; //!< type identifier for the FRI monitoring message corresponding to a KUKA LBR robot + FRIMonitoringMessage* _message; //!< FRI monitoring message (protobuf struct) + +}; +} +} + +#endif // _KUKA_FRI_LBR_STATE_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friTransformationClient.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friTransformationClient.h new file mode 100644 index 00000000..c28a8ef2 --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friTransformationClient.h @@ -0,0 +1,292 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_TRANSFORMATION_CLIENT_H +#define _KUKA_FRI_TRANSFORMATION_CLIENT_H + +#include +#include + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + // forward declaration + struct ClientData; + + /** + * \brief Abstract FRI transformation client. + * + * A transformation client enables the user to send transformation matrices cyclically to the + * KUKA Sunrise controller for manipulating the transformations of dynamic frames in the + * Sunrise scenegraph. + * Usually, these matrices will be provided by external sensors. + *
+ * Custom transformation clients have to be derived from this class and need to + * implement the provide() callback. This callback is called once by the + * client application whenever a new FRI message arrives. + * + */ + class TransformationClient + { + + friend class ClientApplication; + + public: + + /** + * \brief Constructor. + **/ + TransformationClient(); + + /** + * \brief Virtual destructor. + **/ + virtual ~TransformationClient(); + + /** + * \brief Callback which is called whenever a new FRI message arrives. + * + * In this callback all requested transformations have to be set. + * + * \see getRequestedTransformationIDs(), setTransformation() + */ + virtual void provide() = 0; + + /** + * \brief Get the sample time in seconds. + * + * This is the period in which the KUKA Sunrise controller is sending + * FRI packets. + * @return sample time in seconds + */ + double getSampleTime() const; // sec + + /** + * \brief Get the current FRI connection quality. + * + * @return current FRI connection quality + */ + EConnectionQuality getConnectionQuality() const; + + /** + * \brief Returns a vector of identifiers of all requested transformation matrices. + * + * The custom TransformationClient has to provide data for transformation matrices with these + * identifiers. + * + * @return reference to vector of IDs of requested transformations + */ + const std::vector& getRequestedTransformationIDs() const; + + /** + * \brief Get the timestamp of the current received FRI monitor message in Unix time. + * + * This method returns the seconds since 0:00, January 1st, 1970 (UTC). + * Use getTimestampNanoSec() to increase your timestamp resolution when + * seconds are insufficient. + * + * @return timestamp encoded as Unix time (seconds) + */ + const unsigned int getTimestampSec() const; + + /** + * \brief Get the nanoseconds elapsed since the last second (in Unix time). + * + * This method complements getTimestampSec() to get a high resolution + * timestamp. + * + * @return timestamp encoded as Unix time (nanoseconds part) + */ + const unsigned int getTimestampNanoSec() const; + + /** + * \brief Provides a requested transformation as a quaternion transformation vector. + * + * A quaternion transformation vector consists of: [t_x, t_y, t_z, q_w, q_x, q_y, q_z], + * where the first three values describe the translation t as a regular 3-dim + * vector, while the last four values describe the rotation q as an unit quaternion. + *

+ * The unit quaternion q is a 4-dimensional vector, describing the orientation as + * one rotation around the vector v = [v_x, v_y, v_z] about the angle phi. + *

+ * [ q_w ] = [ cos (phi/2) ] + * [ q_x ] = [ sin (phi/2) * v_x ] + * [ q_y ] = [ sin (phi/2) * v_y ] + * [ q_z ] = [ sin (phi/2) * v_z ] + *

+ * All provided transformation need a timestamp that corresponds to their + * time of acquisition. This timestamp must be synchronized to the timestamp + * provided by the KUKA Sunrise controller (see getTimestampSec(), getTimestampNanoSec()). + *

+ * If an update to the last transformation is not yet available when the provide() + * callback is executed, the last transformation (including its timestamp) should be + * repeated until a new transformation is available. + * + * @throw FRIException Throws a FRIException if the maximum number of transformations is exceeded. + * @param transformationID Identifier string of the transformation matrix + * @param transformationQuaternion Provided transformation quaternion + * @param timeSec Timestamp encoded as Unix time (seconds) + * @param timeNanoSec Timestamp encoded as Unix time (nanoseconds part) + */ + void setTransformation(const char* transformationID, const double (&transformationQuaternion)[7], + unsigned int timeSec, unsigned int timeNanoSec); + + /** + * \brief Provides a requested transformation as a homogeneous matrix. + * + * A transformation matrix has 3x4 elements. It consists of a rotational matrix (3x3 elements) + * and a translational vector (3x1 elements). The complete transformation matrix has the + * following structure:
+ * [Transformation(3x4)] = [Rotation(3x3) | Translation(3x1)] + *

+ * All provided transformation matrices need a timestamp that corresponds to their + * time of acquisition. This timestamp must be synchronized to the timestamp + * provided by the KUKA Sunrise controller (see getTimestampSec(), getTimestampNanoSec()). + *

+ * If an update to the last transformation is not yet available when the provide() + * callback is executed, the last transformation (including its timestamp) should be + * repeated until a new transformation is available. + *

+ * + * @throw FRIException Throws a FRIException if the maximum number of transformations is exceeded. + * @param transformationID Identifier string of the transformation matrix + * @param transformationMatrix Provided transformation matrix + * @param timeSec Timestamp encoded as Unix time (seconds) + * @param timeNanoSec Timestamp encoded as Unix time (nanoseconds part) + */ + void setTransformation(const char* transformationID, const double (&transformationMatrix)[3][4], + unsigned int timeSec, unsigned int timeNanoSec); + + /** + * \brief Set boolean output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Boolean value to set. + */ + void setBooleanIOValue(const char* name, const bool value); + + /** + * \brief Set digital output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Digital value to set. + */ + void setDigitalIOValue(const char* name, const unsigned long long value); + + /** + * \brief Set analog output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Analog value to set. + */ + void setAnalogIOValue(const char* name, const double value); + + /** + * \brief Get boolean IO value. + * + * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's boolean value. + */ + bool getBooleanIOValue(const char* name) const; + + /** + * \brief Get digital IO value. + * + * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's digital value. + */ + unsigned long long getDigitalIOValue(const char* name) const; + + /** + * \brief Get analog IO value. + * + * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's analog value. + */ + double getAnalogIOValue(const char* name) const; + + private: + + ClientData* _data; //!< the client data structure + + /** + * \brief Method to link the client data structure (used internally). + * + * @param clientData the client data structure + */ + void linkData(ClientData* clientData); + + }; + +} +} + +#endif // _KUKA_FRI_TRANSFORMATION_CLIENT_H diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friUdpConnection.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friUdpConnection.h new file mode 100644 index 00000000..04fa98d7 --- /dev/null +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friUdpConnection.h @@ -0,0 +1,163 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_UDP_CONNECTION_H +#define _KUKA_FRI_UDP_CONNECTION_H + +#include + +#ifdef _WIN32 + #include +#else + // if linux or a other unix system is used, select uses the following include + #ifdef __unix__ + #include + #endif + // for VxWorks + #ifdef VXWORKS + #include + #include + #endif + #include + #include +#endif + +#include + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + /** + * \brief This class implements the IConnection interface using UDP sockets. + */ + class UdpConnection : public IConnection + { + + public: + + /** + * \brief Constructor with an optional parameter for setting a receive timeout. + * + * @param receiveTimeout Timeout (in ms) for receiving a UDP message (0 = wait forever) + * */ + UdpConnection(unsigned int receiveTimeout = 0); + + /** \brief Destructor. */ + ~UdpConnection(); + + /** + * \brief Open a connection to the KUKA Sunrise controller. + * + * @param port The port ID for the connection + * @param controllerAddress The IPv4 address of the KUKA Sunrise controller. + * If NULL, the FRI Client accepts connections from any + * address. + * @return True if connection was established, false otherwise + */ + virtual bool open(int port, const char *controllerAddress = NULL); + + /** + * \brief Close a connection to the KUKA Sunrise controller. + */ + virtual void close(); + + /** + * \brief Checks whether a connection to the KUKA Sunrise controller is established. + * + * @return True if connection is established + */ + virtual bool isOpen() const; + + /** + * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. + * + * This method blocks until a new message arrives. + * @param buffer Pointer to the allocated buffer that will hold the FRI message + * @param maxSize Size in bytes of the allocated buffer + * @return Number of bytes received (0 when connection was terminated, + * negative in case of errors or receive timeout) + */ + virtual int receive(char *buffer, int maxSize); + + /** + * \brief Send a new FRI command message to the KUKA Sunrise controller. + * + * @param buffer Pointer to the buffer holding the FRI message + * @param size Size in bytes of the message to be send + * @return True if successful + */ + virtual bool send(const char* buffer, int size); + + private: + + int _udpSock; //!< UDP socket handle + struct sockaddr_in _controllerAddr; //!< the controller's socket address + unsigned int _receiveTimeout; + fd_set _filedescriptor; + + }; + +} +} + + +#endif // _KUKA_FRI_UDP_CONNECTION_H diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp index 8ef55dce..42144228 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp @@ -127,11 +127,12 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, std::vector hw_joint_stiffness_commands_; std::vector hw_joint_damping_commands_; std::vector hw_wrench_commands_; - std::vector hw_cart_pose_commands_; std::vector hw_cart_stiffness_commands_; std::vector hw_cart_damping_commands_; - +#ifdef FRI_V2_5 + std::vector hw_cart_pose_commands_; std::vector hw_cart_pose_states_; +#endif std::vector hw_position_states_; std::vector hw_torque_states_; std::vector hw_ext_torque_states_; diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp index 540be562..ade3fdac 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp @@ -80,7 +80,9 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode std::string joint_pos_controller_name_; std::string joint_torque_controller_name_; std::string wrench_controller_name_; +#ifdef FRI_V2_5 std::string cart_pose_controller_name_; +#endif std::vector joint_stiffness_ = std::vector(7, 100.0); std::vector joint_damping_ = std::vector(7, 0.7); std::vector cartesian_stiffness_ = {2000.0, 2000.0, 2000.0, 200.0, 200.0, 200.0}; diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/application/ROS2_Control.java b/kuka_sunrise_fri_driver/robot_application/v1_5/src/application/ROS2_Control.java new file mode 100644 index 00000000..d5fc6c6a --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/v1_5/src/application/ROS2_Control.java @@ -0,0 +1,67 @@ +package application; + +import javax.inject.Inject; + +import ros2.modules.FRIManager; +import ros2.modules.ROS2Connection; +import ros2.modules.TCPConnection; + +import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; +import com.kuka.roboticsAPI.deviceModel.LBR; + +/** + * Implementation of a robot application. + *

+ * The application provides a {@link RoboticsAPITask#initialize()} and a + * {@link RoboticsAPITask#run()} method, which will be called successively in + * the application lifecycle. The application will terminate automatically after + * the {@link RoboticsAPITask#run()} method has finished or after stopping the + * task. The {@link RoboticsAPITask#dispose()} method will be called, even if an + * exception is thrown during initialization or run. + *

+ * It is imperative to call super.dispose() when overriding the + * {@link RoboticsAPITask#dispose()} method. + * + * @see UseRoboticsAPIContext + * @see #initialize() + * @see #run() + * @see #dispose() + */ +public class ROS2_Control extends RoboticsAPIApplication { + @Inject + private LBR _lbr; + + private TCPConnection _TCPConnection; + private ROS2Connection _ROS2Connection; + private FRIManager _FRIManager; + + @Override + public void initialize() { + // initialize your application here + _TCPConnection = new TCPConnection(30000); + _ROS2Connection = new ROS2Connection(); + _FRIManager = new FRIManager(_lbr, getApplicationControl()); + + _FRIManager.registerROS2ConnectionModule(_ROS2Connection); + _TCPConnection.registerROS2ConnectionModule(_ROS2Connection); + _ROS2Connection.registerTCPConnectionModule(_TCPConnection); + _ROS2Connection.registerFRIManagerModule(_FRIManager); + } + + @Override + public void run() { + // your application execution starts here + _ROS2Connection.acceptCommands(); + _TCPConnection.openConnection(); + _TCPConnection.waitUntilDisconnected(); + _ROS2Connection.rejectCommands(); + _FRIManager.close(); + } + + @Override + public void dispose() { + getLogger().info("disposes"); + _TCPConnection.closeConnection(); + _FRIManager.close(); + } +} diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/application/ROS2_Control.java:Zone.Identifier b/kuka_sunrise_fri_driver/robot_application/v1_5/src/application/ROS2_Control.java:Zone.Identifier new file mode 100644 index 00000000..e69de29b diff --git a/kuka_sunrise_fri_driver/robot_application/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java b/kuka_sunrise_fri_driver/robot_application/v1_5/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java old mode 100755 new mode 100644 similarity index 100% rename from kuka_sunrise_fri_driver/robot_application/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java rename to kuka_sunrise_fri_driver/robot_application/v1_5/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java:Zone.Identifier b/kuka_sunrise_fri_driver/robot_application/v1_5/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java:Zone.Identifier new file mode 100644 index 00000000..e69de29b diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/FRIManager.java b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/FRIManager.java new file mode 100644 index 00000000..4e7d55b1 --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/FRIManager.java @@ -0,0 +1,263 @@ +package ros2.modules; + +import java.util.Arrays; +import java.util.List; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.TimeoutException; + +import ros2.serialization.FRIConfigurationParams; + +import com.kuka.connectivity.fastRobotInterface.ClientCommandMode; +import com.kuka.connectivity.fastRobotInterface.FRIChannelInformation.FRISessionState; +import com.kuka.connectivity.fastRobotInterface.FRIChannelInformation; +import com.kuka.connectivity.fastRobotInterface.FRIConfiguration; +import com.kuka.connectivity.fastRobotInterface.FRIJointOverlay; +import com.kuka.connectivity.fastRobotInterface.FRISession; +import com.kuka.roboticsAPI.applicationModel.IApplicationControl; +import com.kuka.roboticsAPI.deviceModel.Device; +import com.kuka.roboticsAPI.deviceModel.LBR; +import com.kuka.roboticsAPI.motionModel.ErrorHandlingAction; +import com.kuka.roboticsAPI.motionModel.IErrorHandler; +import com.kuka.roboticsAPI.motionModel.IMotionContainer; +import com.kuka.roboticsAPI.motionModel.PositionHold; +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; +import com.kuka.roboticsAPI.requestModel.GetApplicationOverrideRequest; + +public class FRIManager{ + public enum CommandResult{ + EXECUTED, + REJECTED, + ERRORED; + } + private ROS2Connection _ROS2Connection; + + private State _currentState; + private LBR _lbr; + private FRISession _FRISession; + private FRIConfiguration _FRIConfiguration; + private IMotionControlMode _controlMode; + private ClientCommandMode _clientCommandMode; + private IMotionContainer _motionContainer; + private FRIMotionErrorHandler _friMotionErrorHandler = new FRIMotionErrorHandler(); + + private static double[] stiffness_ = new double[7]; + + + + public FRIManager(LBR lbr, IApplicationControl applicationControl){ + _currentState = new InactiveState(); + _lbr = lbr; + _FRISession = null; + _FRIConfiguration = new FRIConfigurationParams().toFRIConfiguration(_lbr); + Arrays.fill(stiffness_, 200); + _controlMode = new JointImpedanceControlMode(stiffness_); + _clientCommandMode = ClientCommandMode.POSITION; + applicationControl.registerMoveAsyncErrorHandler(_friMotionErrorHandler); + } + + public void registerROS2ConnectionModule(ROS2Connection ros2ConnectionModule){ + _ROS2Connection = ros2ConnectionModule; + } + + public void close(){ + if(_currentState instanceof ControlActiveState){ + deactivateControl(); + } + if(_currentState instanceof FRIActiveState){ + endFRI();//TODO: handle error + } + } + + public FRIConfigurationParams getFRIConfig(){ + FRIConfigurationParams friConfigurationParams = new FRIConfigurationParams(_FRIConfiguration); + return friConfigurationParams; + } + + public ClientCommandMode getClientCommandMode(){ + return _clientCommandMode; + } + + public IMotionControlMode getControlMode(){ + return _controlMode; + } + + public FRISessionState getFRISessionState(){ + return _FRISession.getFRIChannelInformation().getFRISessionState(); + } + + /* + * The Following commands change the state of the FRI Manager, and thus are forwarded to the state machine for validation + * */ + + public CommandResult setControlMode(IMotionControlMode controlMode){ + return _currentState.setControlMode(controlMode); + } + + public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ + return _currentState.setCommandMode(clientCommandMode); + } + + public CommandResult setFRIConfig(FRIConfigurationParams friConfigurationParams){ + return _currentState.setFRIConfig(friConfigurationParams); + } + + public CommandResult startFRI(){ + CommandResult commandResult = _currentState.startFRI(); + if(commandResult == CommandResult.EXECUTED){ + _currentState = new FRIActiveState(); + } + return commandResult; + } + + public CommandResult endFRI(){ + CommandResult commandResult = _currentState.endFRI(); + if(commandResult == CommandResult.EXECUTED){ + _currentState = new InactiveState(); + } + return commandResult; + } + + public CommandResult activateControl(){ + CommandResult commandResult = _currentState.activateControl(); + if(commandResult == CommandResult.EXECUTED){ + _currentState = new ControlActiveState(); + } + return commandResult; + } + + public CommandResult deactivateControl(){ + CommandResult commandResult = _currentState.deactivateControl(); + if(commandResult == CommandResult.EXECUTED){ + _currentState = new FRIActiveState(); + } + return commandResult; + } + + private class State{ + /* By default reject all commands */ + public CommandResult startFRI(){ + return CommandResult.REJECTED; + } + + public CommandResult endFRI(){ + return CommandResult.REJECTED; + } + + public CommandResult activateControl(){ + return CommandResult.REJECTED; + } + + public CommandResult deactivateControl(){ + return CommandResult.REJECTED; + } + + public CommandResult setFRIConfig(FRIConfigurationParams friConfigurationParams){ + return CommandResult.REJECTED; + } + + public CommandResult setControlMode(IMotionControlMode controlMode){ + return CommandResult.REJECTED; + } + + public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ + return CommandResult.REJECTED; + } + } + + private class InactiveState extends State{ + @Override + public CommandResult startFRI(){ + FRIManager.this._FRISession = new FRISession(FRIManager.this._FRIConfiguration); + try { + FRIManager.this._FRISession.await(10, TimeUnit.SECONDS); + } catch (TimeoutException e) { + FRIManager.this._FRISession.close(); + return CommandResult.ERRORED; + } + + return CommandResult.EXECUTED; + } + @Override + public CommandResult setFRIConfig(FRIConfigurationParams friConfigurationParams){ + FRIManager.this._FRIConfiguration = friConfigurationParams.toFRIConfiguration(FRIManager.this._lbr); + return CommandResult.EXECUTED; + } + @Override + public CommandResult setControlMode(IMotionControlMode controlMode){ + FRIManager.this._controlMode = controlMode; + return CommandResult.EXECUTED; + } + @Override + public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ + FRIManager.this._clientCommandMode = clientCommandMode; + return CommandResult.EXECUTED; + } + } + + private class FRIActiveState extends State { + @Override + public CommandResult endFRI(){ + try{ + FRIManager.this._FRISession.close(); + } catch(IllegalStateException e){ + return CommandResult.ERRORED; + } + return CommandResult.EXECUTED; + } + @Override + public CommandResult activateControl(){ + FRIJointOverlay friJointOverlay = + new FRIJointOverlay(FRIManager.this._FRISession, FRIManager.this._clientCommandMode); + PositionHold motion = + new PositionHold(FRIManager.this._controlMode, -1, null); + FRIManager.this._motionContainer = + FRIManager.this._lbr.moveAsync(motion.addMotionOverlay(friJointOverlay)); + return CommandResult.EXECUTED; + } + @Override + public CommandResult setControlMode(IMotionControlMode controlMode){ + FRIManager.this._controlMode = controlMode; + return CommandResult.EXECUTED; + } + @Override + public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ + FRIManager.this._clientCommandMode = clientCommandMode; + return CommandResult.EXECUTED; + } + } + + private class ControlActiveState extends State { + @Override + public CommandResult deactivateControl(){ + FRIManager.this._motionContainer.cancel(); + return CommandResult.EXECUTED; + } + } + + private class FRIMotionErrorHandler implements IErrorHandler{ + + @Override + public ErrorHandlingAction handleError(Device device, + IMotionContainer failedContainer, + List canceledContainers) { + FRISessionState sessionState = _FRISession.getFRIChannelInformation().getFRISessionState(); + switch(sessionState){ + case IDLE: + FRIManager.this._ROS2Connection.handleFRIEndedError(); + break; + default: + FRIManager.this._ROS2Connection.handleControlEndedError(); + break; + } + System.out.println("Failed container: " + failedContainer.toString() + "."); + System.out.println("Error: " + failedContainer.getErrorMessage()); + System.out.println("Runtime data: " + failedContainer.getRuntimeData()); + System.out.println("Cancelled containers: " + canceledContainers.toString()); + return ErrorHandlingAction.Ignore; + } + + } + +} diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/FRIManager.java:Zone.Identifier b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/FRIManager.java:Zone.Identifier new file mode 100644 index 00000000..e69de29b diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/ROS2Connection.java b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/ROS2Connection.java new file mode 100644 index 00000000..4f259ba8 --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/ROS2Connection.java @@ -0,0 +1,411 @@ +package ros2.modules; + +import java.io.Externalizable; +import java.util.Arrays; + +import ros2.modules.FRIManager; +import ros2.serialization.CartesianImpedanceControlModeExternalizable; +import ros2.serialization.ControlModeParams; +import ros2.serialization.FRIConfigurationParams; +import ros2.serialization.JointImpedanceControlModeExternalizable; +import ros2.serialization.MessageEncoding; + +import com.kuka.connectivity.fastRobotInterface.ClientCommandMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.CartesianImpedanceControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; + +public class ROS2Connection { + + private TCPConnection _TCPConnection; + private FRIManager _FRIManager; + private boolean _acceptingCommands = false; + private boolean _disconnect = false; + + public void registerTCPConnectionModule(TCPConnection tcpConnectionModule){ + _TCPConnection = tcpConnectionModule; + } + + public void registerFRIManagerModule(FRIManager friManagerModule){ + _FRIManager = friManagerModule; + } + + public void acceptCommands(){ + _acceptingCommands = true; + } + + public void rejectCommands(){ + _acceptingCommands = false; + } + + private enum CommandState{ + ACCEPTED((byte)1), + REJECTED((byte)2), + UNKNOWN((byte)3), + ERROR_CONTROL_ENDED((byte)4), + ERROR_FRI_ENDED((byte)5); + + private final byte value; + + private CommandState(byte value){ + this.value = value; + } + } + + private enum CommandID{ + CONNECT( (byte)1), + DISCONNECT( (byte)2), + START_FRI( (byte)3), + END_FRI( (byte)4), + ACTIVATE_CONTROL( (byte)5), + DEACTIVATE_CONTROL( (byte)6), + GET_FRI_CONFIG( (byte)7), + SET_FRI_CONFIG( (byte)8), + GET_CONTROL_MODE( (byte)9), + SET_CONTROL_MODE( (byte)10), + GET_COMMAND_MODE( (byte)11), + SET_COMMAND_MODE( (byte)12); + + private final byte value; + + private CommandID(byte value){ + this.value = value; + } + + + public static CommandID fromByte(byte value){ + for(CommandID id : CommandID.values()){ + if(value == id.value){ + return id; + } + } + throw new RuntimeException("Byte " + value + " does not represent an InMessageID"); + } + } + + private enum SuccessSignalID { + SUCCESS((byte)1), + NO_SUCCESS((byte)2); + + private final byte value; + + private SuccessSignalID(byte value){ + this.value = value; + } + } + + private enum ControlModeID{ + POSITION( (byte)1), + JOINT_IMPEDANCE((byte)2), + CARTESIAN_IMPEDANCE((byte)3); + + public final byte value; + + ControlModeID(byte value){ + this.value = value; + } + + public static ControlModeID fromByte(byte value){ + for(ControlModeID id : ControlModeID.values()){ + if(value == id.value){ + return id; + } + } + throw new RuntimeException("Byte " + value + " does not represent a ControlModeID"); + } + } + + public void handleMessageFromROS(byte[] inMessage){ + CommandID command = null; + try{ + ensureArrayLength(inMessage, 1); + command = CommandID.fromByte(inMessage[0]); + if(!_acceptingCommands){ + feedbackCommandRejected(command, new String("Not accepting commands").getBytes()); + return; + } + + } catch(RuntimeException e){ + System.out.println(e.getMessage()); + feedbackCommandUnknown(e.getMessage().getBytes()); + return; + } + + try{ + byte[] inMessageData = Arrays.copyOfRange(inMessage, 1, inMessage.length); + byte[] feedbackData = null; + System.out.println("Command received: " + command.toString()); + + switch(command){ + case CONNECT: + feedbackData = connect(inMessageData); + break; + case DISCONNECT: + feedbackData = disconnect(inMessageData); + break; + case START_FRI: + feedbackData = startFRI(inMessageData); + break; + case END_FRI: + feedbackData = endFRI(inMessageData); + break; + case ACTIVATE_CONTROL: + feedbackData = activateControl(inMessageData); + break; + case DEACTIVATE_CONTROL: + feedbackData = deactivateControl(inMessageData); + break; + case GET_FRI_CONFIG: + feedbackData = getFRIConfig(inMessageData); + break; + case SET_FRI_CONFIG: + feedbackData = setFRIConfig(inMessageData); + break; + case GET_CONTROL_MODE: + feedbackData = getControlMode(inMessageData); + break; + case SET_CONTROL_MODE: + feedbackData = setControlMode(inMessageData); + break; + case GET_COMMAND_MODE: + feedbackData = getCommandMode(inMessageData); + break; + case SET_COMMAND_MODE: + feedbackData = setCommandMode(inMessageData); + break; + } + System.out.println("Command executed."); + feedbackCommandSuccess(command, feedbackData); + } catch(RuntimeException e){ + System.out.println(e.getMessage()); + feedbackCommandNoSuccess(command, e.getMessage().getBytes()); + return; + } + } + + public void handleConnectionLost(){ + _FRIManager.deactivateControl(); + _FRIManager.endFRI(); + _FRIManager.close(); + System.out.println("Error: connection lost. FRI ended."); + } + + public void handleControlEndedError(){ + byte[] message = {CommandState.ERROR_CONTROL_ENDED.value}; + System.out.println("Error: control ended"); + _TCPConnection.sendBytes(message); + } + + public void handleFRIEndedError(){ + byte[] message = {CommandState.ERROR_FRI_ENDED.value}; + System.out.println("Error: session ended"); + _TCPConnection.sendBytes(message); + } + + private void feedbackCommandRejected(CommandID command, byte[] feedbackData){ + byte[] message = appendByte(command.value, feedbackData); + message = appendByte(CommandState.REJECTED.value, message); + _TCPConnection.sendBytes(message); + } + + private void feedbackCommandUnknown(byte[] feedbackData){ + byte[] message = appendByte(CommandState.UNKNOWN.value, feedbackData); + _TCPConnection.sendBytes(message); + } + + private void feedbackCommandSuccess(CommandID command, byte[] feedbackData){ + byte[] message = appendByte(SuccessSignalID.SUCCESS.value, feedbackData); + message = appendByte(command.value, message); + message = appendByte(CommandState.ACCEPTED.value, message); + _TCPConnection.sendBytes(message); + } + + private void feedbackCommandNoSuccess(CommandID command, byte[] feedbackData){ + byte[] message = appendByte(SuccessSignalID.NO_SUCCESS.value, feedbackData); + message = appendByte(command.value, message); + message = appendByte(CommandState.ACCEPTED.value, message); + _TCPConnection.sendBytes(message); + } + + private byte[] appendByte(byte id, byte[] data){ + byte[] message = null; + if(data == null){ + message = new byte[]{id}; + } else { + message = new byte[data.length + 1]; + message[0] = id; + System.arraycopy(data, 0, message, 1, data.length); + } + return message; + } + + private byte[] connect(byte[] cmdData){ + return null; + } + + private byte[] disconnect(byte[] cmdData){ + _FRIManager.close(); + _disconnect = true; + //TODO: close connection after feedback was sent + return null; + } + + private byte[] startFRI(byte[] cmdData){ + FRIManager.CommandResult commandResult = _FRIManager.startFRI(); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] endFRI(byte[] cmdData){ + FRIManager.CommandResult commandResult = _FRIManager.endFRI(); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] activateControl(byte[] cmdData){ + FRIManager.CommandResult commandResult = _FRIManager.activateControl(); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] deactivateControl(byte[] cmdData){ + FRIManager.CommandResult commandResult = _FRIManager.deactivateControl(); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] getFRIConfig(byte[] cmdData){ + FRIConfigurationParams friConfigurationParams = _FRIManager.getFRIConfig(); + byte[] feedbackData = MessageEncoding.Encode(friConfigurationParams, FRIConfigurationParams.length); + return feedbackData; + } + + private byte[] setFRIConfig(byte[] cmdData) { + ensureArrayLength(cmdData, FRIConfigurationParams.length + 6); + + FRIConfigurationParams friConfigurationParams = new FRIConfigurationParams(); + MessageEncoding.Decode(cmdData, friConfigurationParams); + FRIManager.CommandResult commandResult = _FRIManager.setFRIConfig(friConfigurationParams); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] getControlMode(byte[] cmdData){ + IMotionControlMode controlMode = _FRIManager.getControlMode(); + ControlModeID controlModeID; + byte[] controlModeData; + if(controlMode instanceof PositionControlMode){ + controlModeID = ControlModeID.POSITION; + controlModeData = null; + } else if (controlMode instanceof JointImpedanceControlMode){ + controlModeID = ControlModeID.JOINT_IMPEDANCE; + controlModeData = MessageEncoding.Encode(new JointImpedanceControlModeExternalizable((JointImpedanceControlMode)controlMode), JointImpedanceControlModeExternalizable.length); + } else if (controlMode instanceof CartesianImpedanceControlMode){ + controlModeID = ControlModeID.CARTESIAN_IMPEDANCE; + controlModeData = MessageEncoding.Encode(new CartesianImpedanceControlModeExternalizable((CartesianImpedanceControlMode)controlMode), CartesianImpedanceControlModeExternalizable.length); + } else { + throw new RuntimeException("Control mode not supported"); + } + byte[] message = appendByte(controlModeID.value, controlModeData); + return message; + } + + private byte[] setControlMode(byte[] cmdData){ + ensureArrayLength(cmdData, 1); + ControlModeID controlModeID = ControlModeID.fromByte(cmdData[0]); + + byte[] controlModeData = Arrays.copyOfRange(cmdData, 1, cmdData.length); + + IMotionControlMode controlMode = null; + switch(controlModeID){ + case POSITION: + controlMode = new PositionControlMode(); + System.out.println("Control mode POSITION selected"); + break; + case JOINT_IMPEDANCE: + ensureArrayLength(controlModeData, JointImpedanceControlModeExternalizable.length + 6); + JointImpedanceControlModeExternalizable jointexternalizable = new JointImpedanceControlModeExternalizable(); + System.out.println("Decoding params"); + MessageEncoding.Decode(controlModeData, jointexternalizable); + controlMode = jointexternalizable.toControlMode(); + System.out.println("Control mode JOINT_IMPEDANCE selected"); + break; + case CARTESIAN_IMPEDANCE: + ensureArrayLength(controlModeData, CartesianImpedanceControlModeExternalizable.length + 6); + CartesianImpedanceControlModeExternalizable cartexternalizable = new CartesianImpedanceControlModeExternalizable(); + System.out.println("Decoding params"); + MessageEncoding.Decode(controlModeData, cartexternalizable); + controlMode = cartexternalizable.toControlMode(); + System.out.println("Control mode CARTESIAN_IMPEDANCE selected"); + break; + } + System.out.println("Control mode decoded."); + FRIManager.CommandResult commandResult = _FRIManager.setControlMode(controlMode); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] getCommandMode(byte[] cmdData){ + ClientCommandMode clientCommandMode = _FRIManager.getClientCommandMode(); + byte[] commandModeData = new byte[1]; + commandModeData[0] = (byte)clientCommandMode.ordinal();//TODO: check if ordinal == value + return commandModeData; + } + + private byte[] setCommandMode(byte[] cmdData){ + ensureArrayLength(cmdData, 1); + ClientCommandMode clientCommandMode = ClientCommandMode.intToVal((int)cmdData[0]); + if(clientCommandMode == ClientCommandMode.NO_COMMAND_MODE){ + throw new RuntimeException("Byte " + cmdData[0] + " does not represent a ClientCommandMode."); + } + FRIManager.CommandResult commandResult = _FRIManager.setCommandMode(clientCommandMode); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private void ensureArrayLength(byte[] array, int requiredLength){ + if(array == null){ + throw new RuntimeException("Array is null"); + } + if(array.length < requiredLength){ + throw new RuntimeException("Array does not satisfy length requirement. Required length: " + requiredLength + ", actual length: " + array.length); + } + } + +} diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/ROS2Connection.java:Zone.Identifier b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/ROS2Connection.java:Zone.Identifier new file mode 100644 index 00000000..e69de29b diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/TCPConnection.java b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/TCPConnection.java old mode 100755 new mode 100644 similarity index 100% rename from kuka_sunrise_fri_driver/robot_application/src/ros2/modules/TCPConnection.java rename to kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/TCPConnection.java diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/TCPConnection.java:Zone.Identifier b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/modules/TCPConnection.java:Zone.Identifier new file mode 100644 index 00000000..e69de29b diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java new file mode 100644 index 00000000..fc3ae5c6 --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java @@ -0,0 +1,71 @@ +package ros2.serialization; + +import java.io.Externalizable; +import java.io.IOException; +import java.io.ObjectInput; +import java.io.ObjectOutput; + +import ros2.tools.Conversions; + +import com.kuka.roboticsAPI.geometricModel.CartDOF; +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.CartesianImpedanceControlMode; + +public class CartesianImpedanceControlModeExternalizable extends CartesianImpedanceControlMode implements Externalizable{ + + public final static int length = 96; + + + public CartesianImpedanceControlModeExternalizable(CartesianImpedanceControlMode other){ + super(other); + } + + public CartesianImpedanceControlModeExternalizable(){ + super(); + } + + public IMotionControlMode toControlMode(){ + CartesianImpedanceControlMode controlMode = new CartesianImpedanceControlMode((CartesianImpedanceControlMode)this); + return (IMotionControlMode)controlMode; + } + + @Override + public void writeExternal(ObjectOutput out) throws IOException { + for(double cartStiffness : getStiffness()){ + out.writeDouble(cartStiffness); + } + for(double cartDamping : getDamping()){ + out.writeDouble(cartDamping); + } + } + + @Override + public void readExternal(ObjectInput in) throws IOException, + ClassNotFoundException { + double[] cartStiffness = new double[getStiffness().length]; + for(int i = 0; i < getStiffness().length; i++){ + cartStiffness[i] = in.readDouble(); + } + this.parametrize(CartDOF.X).setStiffness(cartStiffness[0]); + this.parametrize(CartDOF.Y).setStiffness(cartStiffness[1]); + this.parametrize(CartDOF.Z).setStiffness(cartStiffness[2]); + this.parametrize(CartDOF.A).setStiffness(cartStiffness[3]); + this.parametrize(CartDOF.B).setStiffness(cartStiffness[4]); + this.parametrize(CartDOF.C).setStiffness(cartStiffness[5]); + + double[] cartDamping = new double[getDamping().length]; + for(int i = 0; i < getDamping().length; i++){ + cartDamping[i] = in.readDouble(); + } + this.parametrize(CartDOF.X).setDamping(cartDamping[0]); + this.parametrize(CartDOF.Y).setDamping(cartDamping[1]); + this.parametrize(CartDOF.Z).setDamping(cartDamping[2]); + this.parametrize(CartDOF.A).setDamping(cartDamping[3]); + this.parametrize(CartDOF.B).setDamping(cartDamping[4]); + this.parametrize(CartDOF.C).setDamping(cartDamping[5]); + + } + + + +} diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java:Zone.Identifier b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java:Zone.Identifier new file mode 100644 index 00000000..e69de29b diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/ControlModeParams.java b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/ControlModeParams.java new file mode 100644 index 00000000..8e2b93b4 --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/ControlModeParams.java @@ -0,0 +1,111 @@ +package ros2.serialization; + +import java.io.Externalizable; +import java.io.IOException; +import java.io.ObjectInput; +import java.io.ObjectOutput; +import java.util.Arrays; + + +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.CartesianImpedanceControlMode; + +public abstract class ControlModeParams implements Externalizable{ + public static int length = 0; + + private enum ControlModeID{ + POSITION( (byte)1), + JOINT_IMPEDANCE((byte)2), + CARTESIAN_IMPEDANCE((byte)3); + + public final byte value; + + ControlModeID(byte value){ + this.value = value; + } + + public static ControlModeID fromByte(byte value){ + for(ControlModeID id : ControlModeID.values()){ + if(value == id.value){ + return id; + } + } + throw new RuntimeException("Byte " + value + " does not represent an ControlModeID"); + } + } + + public static ControlModeParams fromSerialData(byte[] serialData){ + if(serialData.length == 0){ + throw new RuntimeException("serialData is empty"); + } + ControlModeID controlModeID = ControlModeID.fromByte(serialData[0]); + ControlModeParams controlModeParams = null; + switch(controlModeID){ + case POSITION: + controlModeParams = new PositionControlModeParams(); + break; + case JOINT_IMPEDANCE: + controlModeParams = new JointImpedanceControlModeParams(); + break; + case CARTESIAN_IMPEDANCE: + controlModeParams = new CartesianImpedanceControlModeParams(); + break; + } + serialData = Arrays.copyOfRange(serialData, 1, serialData.length); + MessageEncoding.Decode(serialData, controlModeParams); + return controlModeParams; + } + + public static ControlModeParams fromMotionControlMode(IMotionControlMode controlMode){ + if(controlMode == null){ + throw new RuntimeException("ControlMode is null"); + } + ControlModeParams controlModeParams; + if(controlMode instanceof PositionControlMode){ + controlModeParams = new PositionControlModeParams(); + } else if (controlMode instanceof JointImpedanceControlMode){ + controlModeParams = new JointImpedanceControlModeParams((JointImpedanceControlMode) controlMode); + } else { + throw new RuntimeException("Control mode not supported"); + } + return controlModeParams; + } + + @Override + public void writeExternal(ObjectOutput out) throws IOException { + + + } + + @Override + public void readExternal(ObjectInput in) throws IOException, + ClassNotFoundException { + + } + +} + +class PositionControlModeParams extends ControlModeParams{ + + +} + +class JointImpedanceControlModeParams extends ControlModeParams{ + public JointImpedanceControlModeParams(){ + + } + public JointImpedanceControlModeParams(JointImpedanceControlMode controlMode){ + + } +} + +class CartesianImpedanceControlModeParams extends ControlModeParams{ + public CartesianImpedanceControlModeParams(){ + + } + public CartesianImpedanceControlModeParams(CartesianImpedanceControlMode controlMode){ + + } +} diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/ControlModeParams.java:Zone.Identifier b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/ControlModeParams.java:Zone.Identifier new file mode 100644 index 00000000..e69de29b diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/FRIConfigurationParams.java b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/FRIConfigurationParams.java new file mode 100644 index 00000000..6e2bb022 --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/FRIConfigurationParams.java @@ -0,0 +1,63 @@ +package ros2.serialization; + +import java.io.Externalizable; +import java.io.IOException; +import java.io.ObjectInput; +import java.io.ObjectOutput; +import com.kuka.connectivity.fastRobotInterface.FRIConfiguration; +import com.kuka.roboticsAPI.deviceModel.Device; + +public class FRIConfigurationParams implements Externalizable { + + public static final int length = 16; // 4 integers + + + private String _remoteIP; + private int _remotePort; + private int _sendPeriodMilliSec; + private int _receiveMultiplier; + + @Override + public void writeExternal(ObjectOutput out) throws IOException { + out.writeBytes(_remoteIP); + out.writeInt(_remotePort); + out.writeInt(_sendPeriodMilliSec); + out.writeInt(_receiveMultiplier); + } + + @Override + public void readExternal(ObjectInput in) throws IOException, + ClassNotFoundException { + _remotePort = in.readInt(); + _sendPeriodMilliSec = in.readInt(); + _receiveMultiplier = in.readInt(); + _remoteIP = "192.168.38.6"; + + int ip = in.readInt(); + _remoteIP = String.format("%d.%d.%d.%d", (ip & 0xff),(ip >> 8 & 0xff), (ip >> 16 & 0xff), (ip >> 24 & 0xff)); + + System.out.println("FRI configuration: client IP: " + _remoteIP + ":" + _remotePort + ", send_period [ms]: " + _sendPeriodMilliSec + ", receive multiplier: " + _receiveMultiplier); + } + + public FRIConfigurationParams() { + _remoteIP = "0.0.0.0"; + _remotePort = 30200; + _sendPeriodMilliSec = 10; + _receiveMultiplier = 1; + } + + public FRIConfigurationParams(FRIConfiguration friConfiguration){ + _remoteIP = friConfiguration.getHostName(); + _remotePort = friConfiguration.getPortOnRemote(); + _sendPeriodMilliSec = friConfiguration.getSendPeriodMilliSec(); + _receiveMultiplier = friConfiguration.getReceiveMultiplier(); + } + + public FRIConfiguration toFRIConfiguration(Device device){ + FRIConfiguration friConfiguration = FRIConfiguration.createRemoteConfiguration(device, _remoteIP); + friConfiguration.setPortOnRemote(_remotePort); + friConfiguration.setSendPeriodMilliSec(_sendPeriodMilliSec); + friConfiguration.setReceiveMultiplier(_receiveMultiplier); + return friConfiguration; + } +} diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/FRIConfigurationParams.java:Zone.Identifier b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/FRIConfigurationParams.java:Zone.Identifier new file mode 100644 index 00000000..e69de29b diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/JointImpedanceControlModeExternalizable.java b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/JointImpedanceControlModeExternalizable.java new file mode 100644 index 00000000..618205a2 --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/JointImpedanceControlModeExternalizable.java @@ -0,0 +1,59 @@ +package ros2.serialization; + +import java.io.Externalizable; +import java.io.IOException; +import java.io.ObjectInput; +import java.io.ObjectOutput; + +import ros2.tools.Conversions; + +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; + +public class JointImpedanceControlModeExternalizable extends JointImpedanceControlMode implements Externalizable{ + + public final static int length = 112; + + public JointImpedanceControlModeExternalizable(){ + super(1000, 1000, 1000, 1000, 1000, 1000, 1000); + } + + public JointImpedanceControlModeExternalizable(JointImpedanceControlMode other){ + super(other); + } + + public IMotionControlMode toControlMode(){ + JointImpedanceControlMode controlMode = new JointImpedanceControlMode((JointImpedanceControlMode)this); + return (IMotionControlMode)controlMode; + } + + @Override + public void writeExternal(ObjectOutput out) throws IOException { + for(double jointStiffness : getStiffness()){ + out.writeDouble(jointStiffness); + } + for(double jointDamping : getDamping()){ + out.writeDouble(jointDamping); + } + } + + @Override + public void readExternal(ObjectInput in) throws IOException, + ClassNotFoundException { + double[] jointStiffness = new double[getStiffness().length]; + for(int i = 0; i < getStiffness().length; i++){ + jointStiffness[i] = in.readDouble(); + } + setStiffness(jointStiffness); + + double[] jointDamping = new double[getDamping().length]; + for(int i = 0; i < getDamping().length; i++){ + jointDamping[i] = in.readDouble(); + } + setDamping(jointDamping); + + } + + + +} diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/JointImpedanceControlModeExternalizable.java:Zone.Identifier b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/JointImpedanceControlModeExternalizable.java:Zone.Identifier new file mode 100644 index 00000000..e69de29b diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/MessageEncoding.java b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/MessageEncoding.java old mode 100755 new mode 100644 similarity index 100% rename from kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/MessageEncoding.java rename to kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/MessageEncoding.java diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/MessageEncoding.java:Zone.Identifier b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/serialization/MessageEncoding.java:Zone.Identifier new file mode 100644 index 00000000..e69de29b diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/tools/Conversions.java b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/tools/Conversions.java old mode 100755 new mode 100644 similarity index 100% rename from kuka_sunrise_fri_driver/robot_application/src/ros2/tools/Conversions.java rename to kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/tools/Conversions.java diff --git a/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/tools/Conversions.java:Zone.Identifier b/kuka_sunrise_fri_driver/robot_application/v1_5/src/ros2/tools/Conversions.java:Zone.Identifier new file mode 100644 index 00000000..e69de29b diff --git a/kuka_sunrise_fri_driver/robot_application/v2_5/src/application/ROS2_Control.java b/kuka_sunrise_fri_driver/robot_application/v2_5/src/application/ROS2_Control.java new file mode 100755 index 00000000..9adf72cf --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/v2_5/src/application/ROS2_Control.java @@ -0,0 +1,68 @@ +package application; + +import com.kuka.io.IIoDefinitionProvider; +import com.kuka.roboticsAPI.applicationModel.IApplicationControl; +import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; +import com.kuka.sensitivity.LBR; +import com.kuka.task.RoboticsAPITask; +import javax.inject.Inject; +import ros2.modules.FRIManager; +import ros2.modules.ROS2Connection; +import ros2.modules.TCPConnection; + + +/** + * Implementation of a robot application. + *

+ * The application provides a {@link RoboticsAPITask#initialize()} and a + * {@link RoboticsAPITask#run()} method, which will be called successively in + * the application lifecycle. The application will terminate automatically after + * the {@link RoboticsAPITask#run()} method has finished or after stopping the + * task. The {@link RoboticsAPITask#dispose()} method will be called, even if an + * exception is thrown during initialization or run. + *

+ * It is imperative to call super.dispose() when overriding the + * {@link RoboticsAPITask#dispose()} method. + * + * @see UseRoboticsAPIContext + * @see #initialize() + * @see #run() + * @see #dispose() + */ +public class ROS2_Control extends RoboticsAPIApplication { + @Inject + private LBR _lbr; + + private TCPConnection _TCPConnection; + private ROS2Connection _ROS2Connection; + private FRIManager _FRIManager; + @Inject private IApplicationControl appControl; + @Override + public void initialize() { + // initialize your application here + _TCPConnection = new TCPConnection(30000); + _ROS2Connection = new ROS2Connection(); + _FRIManager = new FRIManager(_lbr, appControl); + _FRIManager.registerROS2ConnectionModule(_ROS2Connection); + _TCPConnection.registerROS2ConnectionModule(_ROS2Connection); + _ROS2Connection.registerTCPConnectionModule(_TCPConnection); + _ROS2Connection.registerFRIManagerModule(_FRIManager); + } + + @Override + public void run() { + // your application execution starts here + _ROS2Connection.acceptCommands(); + _TCPConnection.openConnection(); + _TCPConnection.waitUntilDisconnected(); + _ROS2Connection.rejectCommands(); + _FRIManager.close(); + } + + @Override + public void dispose() { + getLogger().info("disposes"); + _TCPConnection.closeConnection(); + _FRIManager.close(); + } +} diff --git a/kuka_sunrise_fri_driver/robot_application/v2_5/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java b/kuka_sunrise_fri_driver/robot_application/v2_5/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java new file mode 100755 index 00000000..1fc50a4b --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/v2_5/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java @@ -0,0 +1,412 @@ +package com.kuka.generated.ioAccess; + +import javax.inject.Inject; +import javax.inject.Singleton; + +import com.kuka.roboticsAPI.controllerModel.Controller; +import com.kuka.roboticsAPI.ioModel.AbstractIOGroup; +import com.kuka.roboticsAPI.ioModel.IOTypes; + +/** + * Automatically generated class to abstract I/O access to I/O group MediaFlange.
+ * Please, do not modify! + *

+ * I/O group description:
+ * This I/O Group contains the In-/Outputs for the Media-Flange Touch. + */ +@Singleton +public class MediaFlangeIOGroup extends AbstractIOGroup +{ + /** + * Constructor to create an instance of class 'MediaFlange'.
+ * This constructor is automatically generated. Please, do not modify! + * + * @param controller + * the controller, which has access to the I/O group 'MediaFlange' + */ + @Inject + public MediaFlangeIOGroup(Controller controller) + { + super(controller, "MediaFlange"); + + addInput("InputX3Pin3", IOTypes.BOOLEAN, 1); + addInput("InputX3Pin4", IOTypes.BOOLEAN, 1); + addInput("InputX3Pin10", IOTypes.BOOLEAN, 1); + addInput("InputX3Pin13", IOTypes.BOOLEAN, 1); + addInput("InputX3Pin16", IOTypes.BOOLEAN, 1); + addInput("UserButton", IOTypes.BOOLEAN, 1); + addDigitalOutput("LEDBlue", IOTypes.BOOLEAN, 1); + addDigitalOutput("SwitchOffX3Voltage", IOTypes.BOOLEAN, 1); + addDigitalOutput("OutputX3Pin1", IOTypes.BOOLEAN, 1); + addDigitalOutput("OutputX3Pin2", IOTypes.BOOLEAN, 1); + addDigitalOutput("OutputX3Pin11", IOTypes.BOOLEAN, 1); + addDigitalOutput("OutputX3Pin12", IOTypes.BOOLEAN, 1); + } + + /** + * Gets the value of the digital input 'InputX3Pin3'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital input + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @return current value of the digital input 'InputX3Pin3' + */ + public boolean getInputX3Pin3() + { + return getBooleanIOValue("InputX3Pin3", false); + } + + /** + * Gets the value of the digital input 'InputX3Pin4'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital input + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @return current value of the digital input 'InputX3Pin4' + */ + public boolean getInputX3Pin4() + { + return getBooleanIOValue("InputX3Pin4", false); + } + + /** + * Gets the value of the digital input 'InputX3Pin10'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital input + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @return current value of the digital input 'InputX3Pin10' + */ + public boolean getInputX3Pin10() + { + return getBooleanIOValue("InputX3Pin10", false); + } + + /** + * Gets the value of the digital input 'InputX3Pin13'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital input + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @return current value of the digital input 'InputX3Pin13' + */ + public boolean getInputX3Pin13() + { + return getBooleanIOValue("InputX3Pin13", false); + } + + /** + * Gets the value of the digital input 'InputX3Pin16'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital input + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @return current value of the digital input 'InputX3Pin16' + */ + public boolean getInputX3Pin16() + { + return getBooleanIOValue("InputX3Pin16", false); + } + + /** + * Gets the value of the digital input 'UserButton'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital input + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @return current value of the digital input 'UserButton' + */ + public boolean getUserButton() + { + return getBooleanIOValue("UserButton", false); + } + + /** + * Gets the value of the digital output 'LEDBlue'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital output + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @return current value of the digital output 'LEDBlue' + */ + public boolean getLEDBlue() + { + return getBooleanIOValue("LEDBlue", true); + } + + /** + * Sets the value of the digital output 'LEDBlue'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital output + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @param value + * the value, which has to be written to the digital output 'LEDBlue' + */ + public void setLEDBlue(java.lang.Boolean value) + { + setDigitalOutput("LEDBlue", value); + } + + /** + * Gets the value of the digital output 'SwitchOffX3Voltage'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital output + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @return current value of the digital output 'SwitchOffX3Voltage' + */ + public boolean getSwitchOffX3Voltage() + { + return getBooleanIOValue("SwitchOffX3Voltage", true); + } + + /** + * Sets the value of the digital output 'SwitchOffX3Voltage'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital output + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @param value + * the value, which has to be written to the digital output 'SwitchOffX3Voltage' + */ + public void setSwitchOffX3Voltage(java.lang.Boolean value) + { + setDigitalOutput("SwitchOffX3Voltage", value); + } + + /** + * Gets the value of the digital output 'OutputX3Pin1'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital output + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @return current value of the digital output 'OutputX3Pin1' + */ + public boolean getOutputX3Pin1() + { + return getBooleanIOValue("OutputX3Pin1", true); + } + + /** + * Sets the value of the digital output 'OutputX3Pin1'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital output + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @param value + * the value, which has to be written to the digital output 'OutputX3Pin1' + */ + public void setOutputX3Pin1(java.lang.Boolean value) + { + setDigitalOutput("OutputX3Pin1", value); + } + + /** + * Gets the value of the digital output 'OutputX3Pin2'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital output + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @return current value of the digital output 'OutputX3Pin2' + */ + public boolean getOutputX3Pin2() + { + return getBooleanIOValue("OutputX3Pin2", true); + } + + /** + * Sets the value of the digital output 'OutputX3Pin2'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital output + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @param value + * the value, which has to be written to the digital output 'OutputX3Pin2' + */ + public void setOutputX3Pin2(java.lang.Boolean value) + { + setDigitalOutput("OutputX3Pin2", value); + } + + /** + * Gets the value of the digital output 'OutputX3Pin11'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital output + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @return current value of the digital output 'OutputX3Pin11' + */ + public boolean getOutputX3Pin11() + { + return getBooleanIOValue("OutputX3Pin11", true); + } + + /** + * Sets the value of the digital output 'OutputX3Pin11'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital output + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @param value + * the value, which has to be written to the digital output 'OutputX3Pin11' + */ + public void setOutputX3Pin11(java.lang.Boolean value) + { + setDigitalOutput("OutputX3Pin11", value); + } + + /** + * Gets the value of the digital output 'OutputX3Pin12'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital output + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @return current value of the digital output 'OutputX3Pin12' + */ + public boolean getOutputX3Pin12() + { + return getBooleanIOValue("OutputX3Pin12", true); + } + + /** + * Sets the value of the digital output 'OutputX3Pin12'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital output + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @param value + * the value, which has to be written to the digital output 'OutputX3Pin12' + */ + public void setOutputX3Pin12(java.lang.Boolean value) + { + setDigitalOutput("OutputX3Pin12", value); + } + +} diff --git a/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/FRIManager.java b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/FRIManager.java new file mode 100755 index 00000000..17cd6dc9 --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/FRIManager.java @@ -0,0 +1,290 @@ +package ros2.modules; + +import com.kuka.fri.FRIConfiguration; +import com.kuka.fri.FRIJointOverlay; +import com.kuka.fri.FRISession; + +import com.kuka.fri.common.ClientCommandMode; +import com.kuka.fri.common.FRISessionState; +import com.kuka.io.IIoDefinitionProvider; +import com.kuka.motion.IMotionContainer; +import com.kuka.device.RoboticArm; +import com.kuka.roboticsAPI.applicationModel.IApplicationControl; +import com.kuka.roboticsAPI.motionModel.ErrorHandlingAction; +import com.kuka.roboticsAPI.motionModel.IMotionErrorHandler; +import com.kuka.roboticsAPI.motionModel.PositionHold; +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.sensitivity.LBR; +import com.kuka.sensitivity.controlmode.JointImpedanceControlMode; +import java.util.Arrays; +import java.util.List; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.TimeoutException; +import javax.inject.Inject; +import ros2.serialization.FRIConfigurationParams; + +public class FRIManager{ + public enum CommandResult{ + EXECUTED, + REJECTED, + ERRORED; + } + private ROS2Connection _ROS2Connection; + + private State _currentState; + private LBR _lbr; + private FRISession _FRISession; + private FRIConfiguration _FRIConfiguration; + private IMotionControlMode _controlMode; + private ClientCommandMode _clientCommandMode; + private IMotionContainer _motionContainer; + private FRIMotionErrorHandler _friMotionErrorHandler = new FRIMotionErrorHandler(); + + private static double[] stiffness_ = new double[7]; + + + public FRIManager(LBR lbr, IApplicationControl applicationControl){ + _currentState = new InactiveState(); + _lbr = lbr; + _FRISession = null; + _FRIConfiguration = new FRIConfigurationParams().toFRIConfiguration(_lbr); + Arrays.fill(stiffness_, 200); + _controlMode = new JointImpedanceControlMode(stiffness_); + _clientCommandMode = ClientCommandMode.JOINT_POSITION; + applicationControl.registerMotionErrorHandler(_friMotionErrorHandler); + } + + public void registerROS2ConnectionModule(ROS2Connection ros2ConnectionModule){ + _ROS2Connection = ros2ConnectionModule; + } + + public void close(){ + if(_currentState instanceof ControlActiveState){ + deactivateControl(); + } + if(_currentState instanceof FRIActiveState){ + endFRI();//TODO: handle error + } + } + + public FRIConfigurationParams getFRIConfig(){ + FRIConfigurationParams friConfigurationParams = new FRIConfigurationParams(_FRIConfiguration); + return friConfigurationParams; + } + + public ClientCommandMode getClientCommandMode(){ + return _clientCommandMode; + } + + public IMotionControlMode getControlMode(){ + return _controlMode; + } + + public FRISessionState getFRISessionState(){ + return _FRISession.getFRIChannelInformation().getFRISessionState(); + } + + /* + * The Following commands change the state of the FRI Manager, and thus are forwarded to the state machine for validation + * */ + + public CommandResult setControlMode(IMotionControlMode controlMode){ + return _currentState.setControlMode(controlMode); + } + + public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ + return _currentState.setCommandMode(clientCommandMode); + } + + public CommandResult setFRIConfig(FRIConfigurationParams friConfigurationParams){ + return _currentState.setFRIConfig(friConfigurationParams); + } + + public CommandResult startFRI(){ + CommandResult commandResult = _currentState.startFRI(); + if(commandResult == CommandResult.EXECUTED){ + _currentState = new FRIActiveState(); + } + return commandResult; + } + + public CommandResult endFRI(){ + CommandResult commandResult = _currentState.endFRI(); + if(commandResult == CommandResult.EXECUTED){ + _currentState = new InactiveState(); + } + return commandResult; + } + + public CommandResult activateControl(){ + CommandResult commandResult = _currentState.activateControl(); + if(commandResult == CommandResult.EXECUTED){ + _currentState = new ControlActiveState(); + } + return commandResult; + } + + public CommandResult deactivateControl(){ + CommandResult commandResult = _currentState.deactivateControl(); + if(commandResult == CommandResult.EXECUTED){ + _currentState = new FRIActiveState(); + } + return commandResult; + } + + private class State{ + /* By default reject all commands */ + public CommandResult startFRI(){ + return CommandResult.REJECTED; + } + + public CommandResult endFRI(){ + return CommandResult.REJECTED; + } + + public CommandResult activateControl(){ + return CommandResult.REJECTED; + } + + public CommandResult deactivateControl(){ + return CommandResult.REJECTED; + } + + public CommandResult setFRIConfig(FRIConfigurationParams friConfigurationParams){ + return CommandResult.REJECTED; + } + + public CommandResult setControlMode(IMotionControlMode controlMode){ + return CommandResult.REJECTED; + } + + public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ + return CommandResult.REJECTED; + } + } + + private class InactiveState extends State{ + @Override + public CommandResult startFRI(){ + FRIManager.this._FRISession = new FRISession(FRIManager.this._FRIConfiguration); + try { + FRIManager.this._FRISession.await(10, TimeUnit.SECONDS); + } catch (TimeoutException e) { + FRIManager.this._FRISession.close(); + return CommandResult.ERRORED; + } + + return CommandResult.EXECUTED; + } + @Override + public CommandResult setFRIConfig(FRIConfigurationParams friConfigurationParams){ + FRIManager.this._FRIConfiguration = friConfigurationParams.toFRIConfiguration(FRIManager.this._lbr); + return CommandResult.EXECUTED; + } + @Override + public CommandResult setControlMode(IMotionControlMode controlMode){ + FRIManager.this._controlMode = controlMode; + return CommandResult.EXECUTED; + } + @Override + public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ + FRIManager.this._clientCommandMode = clientCommandMode; + return CommandResult.EXECUTED; + } + } + + private class FRIActiveState extends State { + @Override + public CommandResult endFRI(){ + try{ + FRIManager.this._FRISession.close(); + } catch(IllegalStateException e){ + return CommandResult.ERRORED; + } + return CommandResult.EXECUTED; + } + @Override + public CommandResult activateControl(){ + System.out.println("ClientCommandMode: " + FRIManager.this._clientCommandMode + "."); + if (FRIManager.this._clientCommandMode==ClientCommandMode.CARTESIAN_POSE) { + FRICartesianOverlay friCartesianOverlay = + new FRICartesianOverlay(FRIManager.this._FRISession); + PositionHold motion = + new PositionHold(FRIManager.this._controlMode, -1, null); + FRIManager.this._motionContainer = + FRIManager.this._lbr.getFlange().moveAsync(motion.addMotionOverlay(friCartesianOverlay)); + } + else { + FRIJointOverlay friJointOverlay = + new FRIJointOverlay(FRIManager.this._FRISession, FRIManager.this._clientCommandMode); + PositionHold motion = + new PositionHold(FRIManager.this._controlMode, -1, null); + FRIManager.this._motionContainer = + FRIManager.this._lbr.getFlange().moveAsync(motion.addMotionOverlay(friJointOverlay)); + } + return CommandResult.EXECUTED; + } + @Override + public CommandResult setControlMode(IMotionControlMode controlMode){ + FRIManager.this._controlMode = controlMode; + return CommandResult.EXECUTED; + } + @Override + public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ + FRIManager.this._clientCommandMode = clientCommandMode; + return CommandResult.EXECUTED; + } + } + + private class ControlActiveState extends State { + @Override + public CommandResult deactivateControl(){ + FRIManager.this._motionContainer.cancel(); + return CommandResult.EXECUTED; + } + } + + private class FRIMotionErrorHandler implements IMotionErrorHandler{ + + @Override + public ErrorHandlingAction handleExecutionError( + IMotionContainer failedContainer, + List canceledContainers) { + FRISessionState sessionState = _FRISession.getFRIChannelInformation().getFRISessionState(); + switch(sessionState){ + case IDLE: + FRIManager.this._ROS2Connection.handleFRIEndedError(); + break; + default: + FRIManager.this._ROS2Connection.handleControlEndedError(); + break; + } + System.out.println("Failed container: " + failedContainer.toString() + "."); + System.out.println("Error: " + failedContainer.getErrorMessage()); + System.out.println("Runtime data: " + failedContainer.getRuntimeData()); + System.out.println("Cancelled containers: " + canceledContainers.toString()); + return ErrorHandlingAction.IGNORE; + } + @Override + public ErrorHandlingAction handleMaintainingError(IMotionContainer lastContainer, + List canceledContainers, + String errorMessage) { + FRISessionState sessionState = _FRISession.getFRIChannelInformation().getFRISessionState(); + switch(sessionState){ + case IDLE: + FRIManager.this._ROS2Connection.handleFRIEndedError(); + break; + default: + FRIManager.this._ROS2Connection.handleControlEndedError(); + break; + } + System.out.println("Last container: " + lastContainer.toString() + "."); + System.out.println("Error: " + lastContainer.getErrorMessage()); + System.out.println("Runtime data: " + lastContainer.getRuntimeData()); + System.out.println("Cancelled containers: " + canceledContainers.toString()); + return ErrorHandlingAction.IGNORE; + } + + } + +} diff --git a/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/ROS2Connection.java b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/ROS2Connection.java new file mode 100755 index 00000000..d5326d92 --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/ROS2Connection.java @@ -0,0 +1,411 @@ +package ros2.modules; + +import com.kuka.fri.common.ClientCommandMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.sensitivity.controlmode.JointImpedanceControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; +import com.kuka.sensitivity.controlmode.CartesianImpedanceControlMode; +import com.kuka.sensitivity.controlmode.JointImpedanceControlMode; +import java.util.Arrays; +import ros2.serialization.CartesianImpedanceControlModeExternalizable; +import ros2.serialization.FRIConfigurationParams; +import ros2.serialization.JointImpedanceControlModeExternalizable; +import ros2.serialization.MessageEncoding; + +public class ROS2Connection { + + private TCPConnection _TCPConnection; + private FRIManager _FRIManager; + private boolean _acceptingCommands = false; + private boolean _disconnect = false; + + public void registerTCPConnectionModule(TCPConnection tcpConnectionModule){ + _TCPConnection = tcpConnectionModule; + } + + public void registerFRIManagerModule(FRIManager friManagerModule){ + _FRIManager = friManagerModule; + } + + public void acceptCommands(){ + _acceptingCommands = true; + } + + public void rejectCommands(){ + _acceptingCommands = false; + } + + private enum CommandState{ + ACCEPTED((byte)1), + REJECTED((byte)2), + UNKNOWN((byte)3), + ERROR_CONTROL_ENDED((byte)4), + ERROR_FRI_ENDED((byte)5); + + private final byte value; + + private CommandState(byte value){ + this.value = value; + } + } + + private enum CommandID{ + CONNECT( (byte)1), + DISCONNECT( (byte)2), + START_FRI( (byte)3), + END_FRI( (byte)4), + ACTIVATE_CONTROL( (byte)5), + DEACTIVATE_CONTROL( (byte)6), + GET_FRI_CONFIG( (byte)7), + SET_FRI_CONFIG( (byte)8), + GET_CONTROL_MODE( (byte)9), + SET_CONTROL_MODE( (byte)10), + GET_COMMAND_MODE( (byte)11), + SET_COMMAND_MODE( (byte)12); + + private final byte value; + + private CommandID(byte value){ + this.value = value; + } + + + public static CommandID fromByte(byte value){ + for(CommandID id : CommandID.values()){ + if(value == id.value){ + return id; + } + } + throw new RuntimeException("Byte " + value + " does not represent an InMessageID"); + } + } + + private enum SuccessSignalID { + SUCCESS((byte)1), + NO_SUCCESS((byte)2); + + private final byte value; + + private SuccessSignalID(byte value){ + this.value = value; + } + } + + private enum ControlModeID{ + POSITION( (byte)1), + JOINT_IMPEDANCE((byte)2), + CARTESIAN_IMPEDANCE((byte)3); + + public final byte value; + + ControlModeID(byte value){ + this.value = value; + } + + public static ControlModeID fromByte(byte value){ + for(ControlModeID id : ControlModeID.values()){ + if(value == id.value){ + return id; + } + } + throw new RuntimeException("Byte " + value + " does not represent a ControlModeID"); + } + } + + public void handleMessageFromROS(byte[] inMessage){ + CommandID command = null; + try{ + ensureArrayLength(inMessage, 1); + command = CommandID.fromByte(inMessage[0]); + if(!_acceptingCommands){ + feedbackCommandRejected(command, new String("Not accepting commands").getBytes()); + return; + } + + } catch(RuntimeException e){ + System.out.println(e.getMessage()); + feedbackCommandUnknown(e.getMessage().getBytes()); + return; + } + + try{ + byte[] inMessageData = Arrays.copyOfRange(inMessage, 1, inMessage.length); + byte[] feedbackData = null; + System.out.println("Command received: " + command.toString()); + + switch(command){ + case CONNECT: + feedbackData = connect(inMessageData); + break; + case DISCONNECT: + feedbackData = disconnect(inMessageData); + break; + case START_FRI: + feedbackData = startFRI(inMessageData); + break; + case END_FRI: + feedbackData = endFRI(inMessageData); + break; + case ACTIVATE_CONTROL: + feedbackData = activateControl(inMessageData); + break; + case DEACTIVATE_CONTROL: + feedbackData = deactivateControl(inMessageData); + break; + case GET_FRI_CONFIG: + feedbackData = getFRIConfig(inMessageData); + break; + case SET_FRI_CONFIG: + feedbackData = setFRIConfig(inMessageData); + break; + case GET_CONTROL_MODE: + feedbackData = getControlMode(inMessageData); + break; + case SET_CONTROL_MODE: + feedbackData = setControlMode(inMessageData); + break; + case GET_COMMAND_MODE: + feedbackData = getCommandMode(inMessageData); + break; + case SET_COMMAND_MODE: + feedbackData = setCommandMode(inMessageData); + break; + default: + break; + } + System.out.println("Command executed."); + feedbackCommandSuccess(command, feedbackData); + } catch(RuntimeException e){ + System.out.println(e.getMessage()); + feedbackCommandNoSuccess(command, e.getMessage().getBytes()); + return; + } + } + + public void handleConnectionLost(){ + _FRIManager.deactivateControl(); + _FRIManager.endFRI(); + _FRIManager.close(); + System.out.println("Error: connection lost. FRI ended."); + } + + public void handleControlEndedError(){ + byte[] message = {CommandState.ERROR_CONTROL_ENDED.value}; + System.out.println("Error: control ended"); + _TCPConnection.sendBytes(message); + } + + public void handleFRIEndedError(){ + byte[] message = {CommandState.ERROR_FRI_ENDED.value}; + System.out.println("Error: session ended"); + _TCPConnection.sendBytes(message); + } + + private void feedbackCommandRejected(CommandID command, byte[] feedbackData){ + byte[] message = appendByte(command.value, feedbackData); + message = appendByte(CommandState.REJECTED.value, message); + _TCPConnection.sendBytes(message); + } + + private void feedbackCommandUnknown(byte[] feedbackData){ + byte[] message = appendByte(CommandState.UNKNOWN.value, feedbackData); + _TCPConnection.sendBytes(message); + } + + private void feedbackCommandSuccess(CommandID command, byte[] feedbackData){ + byte[] message = appendByte(SuccessSignalID.SUCCESS.value, feedbackData); + message = appendByte(command.value, message); + message = appendByte(CommandState.ACCEPTED.value, message); + _TCPConnection.sendBytes(message); + } + + private void feedbackCommandNoSuccess(CommandID command, byte[] feedbackData){ + byte[] message = appendByte(SuccessSignalID.NO_SUCCESS.value, feedbackData); + message = appendByte(command.value, message); + message = appendByte(CommandState.ACCEPTED.value, message); + _TCPConnection.sendBytes(message); + } + + private byte[] appendByte(byte id, byte[] data){ + byte[] message = null; + if(data == null){ + message = new byte[]{id}; + } else { + message = new byte[data.length + 1]; + message[0] = id; + System.arraycopy(data, 0, message, 1, data.length); + } + return message; + } + + private byte[] connect(byte[] cmdData){ + return null; + } + + private byte[] disconnect(byte[] cmdData){ + _FRIManager.close(); + _disconnect = true; + //TODO: close connection after feedback was sent + return null; + } + + private byte[] startFRI(byte[] cmdData){ + FRIManager.CommandResult commandResult = _FRIManager.startFRI(); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] endFRI(byte[] cmdData){ + FRIManager.CommandResult commandResult = _FRIManager.endFRI(); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] activateControl(byte[] cmdData){ + FRIManager.CommandResult commandResult = _FRIManager.activateControl(); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] deactivateControl(byte[] cmdData){ + FRIManager.CommandResult commandResult = _FRIManager.deactivateControl(); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] getFRIConfig(byte[] cmdData){ + FRIConfigurationParams friConfigurationParams = _FRIManager.getFRIConfig(); + byte[] feedbackData = MessageEncoding.Encode(friConfigurationParams, FRIConfigurationParams.length); + return feedbackData; + } + + private byte[] setFRIConfig(byte[] cmdData) { + ensureArrayLength(cmdData, FRIConfigurationParams.length + 6); + + FRIConfigurationParams friConfigurationParams = new FRIConfigurationParams(); + MessageEncoding.Decode(cmdData, friConfigurationParams); + FRIManager.CommandResult commandResult = _FRIManager.setFRIConfig(friConfigurationParams); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] getControlMode(byte[] cmdData){ + IMotionControlMode controlMode = _FRIManager.getControlMode(); + ControlModeID controlModeID; + byte[] controlModeData; + if(controlMode instanceof PositionControlMode){ + controlModeID = ControlModeID.POSITION; + controlModeData = null; + } else if (controlMode instanceof JointImpedanceControlMode){ + controlModeID = ControlModeID.JOINT_IMPEDANCE; + controlModeData = MessageEncoding.Encode(new JointImpedanceControlModeExternalizable((JointImpedanceControlMode)controlMode), JointImpedanceControlModeExternalizable.length); + } else if (controlMode instanceof CartesianImpedanceControlMode){ + controlModeID = ControlModeID.CARTESIAN_IMPEDANCE; + controlModeData = MessageEncoding.Encode(new CartesianImpedanceControlModeExternalizable((CartesianImpedanceControlMode)controlMode), CartesianImpedanceControlModeExternalizable.length); + } else { + throw new RuntimeException("Control mode not supported"); + } + byte[] message = appendByte(controlModeID.value, controlModeData); + return message; + } + + private byte[] setControlMode(byte[] cmdData){ + ensureArrayLength(cmdData, 1); + ControlModeID controlModeID = ControlModeID.fromByte(cmdData[0]); + + byte[] controlModeData = Arrays.copyOfRange(cmdData, 1, cmdData.length); + + IMotionControlMode controlMode = null; + switch(controlModeID){ + case POSITION: + controlMode = new PositionControlMode(); + System.out.println("Control mode POSITION selected"); + break; + case JOINT_IMPEDANCE: + ensureArrayLength(controlModeData, JointImpedanceControlModeExternalizable.length + 6); + JointImpedanceControlModeExternalizable jointexternalizable = new JointImpedanceControlModeExternalizable(); + System.out.println("Decoding params"); + MessageEncoding.Decode(controlModeData, jointexternalizable); + controlMode = jointexternalizable.toControlMode(); + System.out.println("Control mode JOINT_IMPEDANCE selected"); + break; + case CARTESIAN_IMPEDANCE: + ensureArrayLength(controlModeData, CartesianImpedanceControlModeExternalizable.length + 6); + CartesianImpedanceControlModeExternalizable cartexternalizable = new CartesianImpedanceControlModeExternalizable(); + System.out.println("Decoding params"); + MessageEncoding.Decode(controlModeData, cartexternalizable); + controlMode = cartexternalizable.toControlMode(); + System.out.println("Control mode CARTESIAN_IMPEDANCE selected"); + break; + default: + break; + } + System.out.println("Control mode decoded."); + FRIManager.CommandResult commandResult = _FRIManager.setControlMode(controlMode); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] getCommandMode(byte[] cmdData){ + ClientCommandMode clientCommandMode = _FRIManager.getClientCommandMode(); + byte[] commandModeData = new byte[1]; + commandModeData[0] = (byte)clientCommandMode.ordinal();//TODO: check if ordinal == value + return commandModeData; + } + + private byte[] setCommandMode(byte[] cmdData){ + ensureArrayLength(cmdData, 1); + ClientCommandMode clientCommandMode = ClientCommandMode.intToVal((int)cmdData[0]); + if(clientCommandMode == ClientCommandMode.NO_COMMAND_MODE){ + throw new RuntimeException("Byte " + cmdData[0] + " does not represent a ClientCommandMode."); + } + FRIManager.CommandResult commandResult = _FRIManager.setCommandMode(clientCommandMode); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private void ensureArrayLength(byte[] array, int requiredLength){ + if(array == null){ + throw new RuntimeException("Array is null"); + } + if(array.length < requiredLength){ + throw new RuntimeException("Array does not satisfy length requirement. Required length: " + requiredLength + ", actual length: " + array.length); + } + } + +} diff --git a/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/TCPConnection.java b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/TCPConnection.java new file mode 100755 index 00000000..a895269a --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/TCPConnection.java @@ -0,0 +1,185 @@ +package ros2.modules; + +import java.net.*; +import java.util.Arrays; +import java.io.*; + +import javax.xml.bind.DatatypeConverter; + +public class TCPConnection{ + private int _tcpServerPort; + private ServerSocket _tcpServer; + private Socket _tcpClient; + private Thread _tcpConnectionThread; + private boolean _closeRequested; + private byte[] _incomingData; + private ROS2Connection _ROS2Connection; + + public TCPConnection(int tcpServerPort){ + _tcpServerPort = tcpServerPort; + _tcpServer = null; + _tcpClient = null; + _tcpConnectionThread = new Thread(new ConnectionHandler()); + _closeRequested = false; + _incomingData = null; + } + + public void registerROS2ConnectionModule(ROS2Connection ros2ConnectionModule){ + _ROS2Connection = ros2ConnectionModule; + } + + public void waitUntilDisconnected(){ + try { + _tcpConnectionThread.join(); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + + private class ConnectionHandler implements Runnable{ + public void run(){ + try{ + while(true){ + waitForConnection(); + if(_closeRequested) { + System.out.println("Stopped waiting for connection."); + break; + } + handleIncomingData(); + if(_closeRequested) { + System.out.println("TCP Read interrupted."); + break; + } + } + }catch (IOException e){ + e.printStackTrace(); + }catch(Exception e){ + e.printStackTrace(); + } + + if(_tcpServer.isClosed() == false){ + try{ + _tcpServer.close(); + }catch(IOException e){ + e.printStackTrace(); + } + } + _closeRequested = false; + } + } + + public void openConnection() { + try { + _tcpServer = new ServerSocket(_tcpServerPort); + } catch (IOException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + _tcpConnectionThread.start(); + } + + public void sendString(String s){ + if(_tcpClient != null && _tcpClient.isConnected() && !_tcpClient.isClosed()){ + try{ + DataOutputStream outToClient = new DataOutputStream(_tcpClient.getOutputStream()); + outToClient.writeBytes(s); + outToClient.close(); + } + catch(IOException e){ + e.printStackTrace(); + } + } else{ + System.out.println("TCP client not connected."); + } + } + + public void sendBytes(byte[] message){ + if(_tcpClient != null && _tcpClient.isConnected() && !_tcpClient.isClosed()){ + try{ + DataOutputStream outToClient = new DataOutputStream(_tcpClient.getOutputStream()); + outToClient.write(message); + }catch(IOException e){ + e.printStackTrace(); + } + } else{ + System.out.println("TCP client not connected."); + } + } + + public byte[] getReceivedData(){ + byte[] dataCopy = _incomingData; + _incomingData = null; + return dataCopy; + } + + public void closeConnection(){ + _closeRequested = true; + try{ + if(_tcpServer.isClosed() == false){ + _tcpServer.close(); + } + if(_tcpClient.isClosed() == false){ + _tcpClient.close(); + } + } + catch(IOException e) + { + e.printStackTrace(); + } + } + + private void waitForConnection() throws Exception { + System.out.println("Waiting for connection..."); + try { + _tcpClient = _tcpServer.accept(); + } catch (SocketException e) { + if(_closeRequested){ + return; + } else { + // closing of connection wasn't requested, error + throw e; + } + + } + System.out.println("Connection established."); + } + + private void handleIncomingData() throws IOException{ + BufferedReader inFromClient = new BufferedReader(new InputStreamReader(_tcpClient.getInputStream())); + while(_tcpClient.isClosed() == false){ + int dataLength = -2; + char[] charArray = new char[1024]; + try{ + dataLength = inFromClient.read(charArray); + } catch (SocketException e) { + if(_closeRequested){ + break; + } else { + // closing of connection wasn't requested, error + throw e; + } + } + if(dataLength < 0){ + _ROS2Connection.handleConnectionLost(); + break; + } + + byte[] byteArray = Arrays.copyOf(new String(charArray).getBytes(), dataLength); + String byteHexString = DatatypeConverter.printHexBinary(byteArray); + + if(_incomingData != null){ + System.out.println("ERROR: Previous data not yet processed! Skipping data: " + byteHexString); + } + else{ + _incomingData = byteArray; + //System.out.println("New data received: " + byteHexString + ", " + _incomingData); + _ROS2Connection.handleMessageFromROS(_incomingData); + _incomingData = null; + } + } + } + + + +} diff --git a/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java new file mode 100644 index 00000000..32dca2ce --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java @@ -0,0 +1,68 @@ +package ros2.serialization; + +import com.kuka.roboticsAPI.motionModel.controlModeModel.CartDOF; +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.sensitivity.controlmode.CartesianImpedanceControlMode; +import java.io.Externalizable; +import java.io.IOException; +import java.io.ObjectInput; +import java.io.ObjectOutput; + +public class CartesianImpedanceControlModeExternalizable extends CartesianImpedanceControlMode implements Externalizable{ + + public final static int length = 96; + + + public CartesianImpedanceControlModeExternalizable(CartesianImpedanceControlMode other){ + super(other); + } + + public CartesianImpedanceControlModeExternalizable(){ + super(); + } + + public IMotionControlMode toControlMode(){ + CartesianImpedanceControlMode controlMode = new CartesianImpedanceControlMode((CartesianImpedanceControlMode)this); + return (IMotionControlMode)controlMode; + } + + @Override + public void writeExternal(ObjectOutput out) throws IOException { + for(double cartStiffness : getStiffness()){ + out.writeDouble(cartStiffness); + } + for(double cartDamping : getDamping()){ + out.writeDouble(cartDamping); + } + } + + @Override + public void readExternal(ObjectInput in) throws IOException, + ClassNotFoundException { + double[] cartStiffness = new double[getStiffness().length]; + for(int i = 0; i < getStiffness().length; i++){ + cartStiffness[i] = in.readDouble(); + } + this.parametrize(CartDOF.X).setStiffness(cartStiffness[0]); + this.parametrize(CartDOF.Y).setStiffness(cartStiffness[1]); + this.parametrize(CartDOF.Z).setStiffness(cartStiffness[2]); + this.parametrize(CartDOF.A).setStiffness(cartStiffness[3]); + this.parametrize(CartDOF.B).setStiffness(cartStiffness[4]); + this.parametrize(CartDOF.C).setStiffness(cartStiffness[5]); + + double[] cartDamping = new double[getDamping().length]; + for(int i = 0; i < getDamping().length; i++){ + cartDamping[i] = in.readDouble(); + } + this.parametrize(CartDOF.X).setDamping(cartDamping[0]); + this.parametrize(CartDOF.Y).setDamping(cartDamping[1]); + this.parametrize(CartDOF.Z).setDamping(cartDamping[2]); + this.parametrize(CartDOF.A).setDamping(cartDamping[3]); + this.parametrize(CartDOF.B).setDamping(cartDamping[4]); + this.parametrize(CartDOF.C).setDamping(cartDamping[5]); + + } + + + +} diff --git a/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/ControlModeParams.java b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/ControlModeParams.java new file mode 100755 index 00000000..88f1dc00 --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/ControlModeParams.java @@ -0,0 +1,109 @@ +package ros2.serialization; + +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; +import com.kuka.sensitivity.controlmode.CartesianImpedanceControlMode; +import com.kuka.sensitivity.controlmode.JointImpedanceControlMode; +import java.io.Externalizable; +import java.io.IOException; +import java.io.ObjectInput; +import java.io.ObjectOutput; +import java.util.Arrays; + +public abstract class ControlModeParams implements Externalizable{ + public static int length = 0; + + private enum ControlModeID{ + POSITION( (byte)1), + JOINT_IMPEDANCE((byte)2), + CARTESIAN_IMPEDANCE((byte)3); + + public final byte value; + + ControlModeID(byte value){ + this.value = value; + } + + public static ControlModeID fromByte(byte value){ + for(ControlModeID id : ControlModeID.values()){ + if(value == id.value){ + return id; + } + } + throw new RuntimeException("Byte " + value + " does not represent an ControlModeID"); + } + } + + public static ControlModeParams fromSerialData(byte[] serialData){ + if(serialData.length == 0){ + throw new RuntimeException("serialData is empty"); + } + ControlModeID controlModeID = ControlModeID.fromByte(serialData[0]); + ControlModeParams controlModeParams = null; + switch(controlModeID){ + case POSITION: + controlModeParams = new PositionControlModeParams(); + break; + case JOINT_IMPEDANCE: + controlModeParams = new JointImpedanceControlModeParams(); + break; + case CARTESIAN_IMPEDANCE: + controlModeParams = new CartesianImpedanceControlModeParams(); + break; + } + serialData = Arrays.copyOfRange(serialData, 1, serialData.length); + MessageEncoding.Decode(serialData, controlModeParams); + return controlModeParams; + } + + public static ControlModeParams fromMotionControlMode(IMotionControlMode controlMode){ + if(controlMode == null){ + throw new RuntimeException("ControlMode is null"); + } + ControlModeParams controlModeParams; + if(controlMode instanceof PositionControlMode){ + controlModeParams = new PositionControlModeParams(); + } else if (controlMode instanceof JointImpedanceControlMode){ + controlModeParams = new JointImpedanceControlModeParams((JointImpedanceControlMode) controlMode); + } else { + throw new RuntimeException("Control mode not supported"); + } + return controlModeParams; + } + + @Override + public void writeExternal(ObjectOutput out) throws IOException { + + + } + + @Override + public void readExternal(ObjectInput in) throws IOException, + ClassNotFoundException { + + } + +} + +class PositionControlModeParams extends ControlModeParams{ + + +} + +class JointImpedanceControlModeParams extends ControlModeParams{ + public JointImpedanceControlModeParams(){ + + } + public JointImpedanceControlModeParams(JointImpedanceControlMode controlMode){ + + } +} + +class CartesianImpedanceControlModeParams extends ControlModeParams{ + public CartesianImpedanceControlModeParams(){ + + } + public CartesianImpedanceControlModeParams(CartesianImpedanceControlMode controlMode){ + + } +} diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/FRIConfigurationParams.java b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/FRIConfigurationParams.java similarity index 100% rename from kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/FRIConfigurationParams.java rename to kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/FRIConfigurationParams.java diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/JointImpedanceControlModeExternalizable.java b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/JointImpedanceControlModeExternalizable.java similarity index 100% rename from kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/JointImpedanceControlModeExternalizable.java rename to kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/JointImpedanceControlModeExternalizable.java diff --git a/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/MessageEncoding.java b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/MessageEncoding.java new file mode 100755 index 00000000..7083509d --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/MessageEncoding.java @@ -0,0 +1,39 @@ +package ros2.serialization; + +import java.io.ByteArrayInputStream; +import java.io.ByteArrayOutputStream; +import java.io.Externalizable; +import java.io.IOException; +import java.io.ObjectOutputStream; +import java.io.ObjectInputStream; + +public class MessageEncoding { + public static byte[] Encode(Externalizable objectIn, int serialLength){ + ByteArrayOutputStream serialDataStream = new ByteArrayOutputStream(); + byte[] serialDataOut = new byte[serialLength + 4]; + try{ + ObjectOutputStream objectStream = new ObjectOutputStream(serialDataStream); + objectIn.writeExternal(objectStream); + objectStream.flush(); + objectStream.close(); + serialDataOut = serialDataStream.toByteArray(); + }catch(IOException e){ + e.printStackTrace(); + } + return serialDataOut; + } + + public static void Decode(byte[] serialDataIn, Externalizable objectOut) throws RuntimeException{ + try{ + ByteArrayInputStream serialDataStream = new ByteArrayInputStream(serialDataIn); + ObjectInputStream objectStream = new ObjectInputStream(serialDataStream); + objectOut.readExternal(objectStream); + objectStream.close(); + } catch(IOException e){ + e.printStackTrace(); + throw new RuntimeException("IO Exception occurred"); + } catch (ClassNotFoundException e) { + throw new RuntimeException("Message could not be decoded"); + } + } +} diff --git a/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/tools/Conversions.java b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/tools/Conversions.java new file mode 100755 index 00000000..2635dd1b --- /dev/null +++ b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/tools/Conversions.java @@ -0,0 +1,13 @@ +package ros2.tools; + +public class Conversions { + public static class Arrays{ + public static float[] DoubleToFloat(double[] doubleArray){ + float[] floatArray = new float[doubleArray.length]; + for(int i = 0; i < doubleArray.length; i++){ + floatArray[i] = (float)doubleArray[i]; + } + return floatArray; + } + } +} diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/FRIMessages.pb.c b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/FRIMessages.pb.c new file mode 100644 index 00000000..711f8cfa --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/FRIMessages.pb.c @@ -0,0 +1,71 @@ +/* Automatically generated nanopb constant definitions */ +/* Generated by nanopb-0.4.8-dev */ + +#include "FRIMessages.pb.h" +#if PB_PROTO_HEADER_VERSION != 40 +#error Regenerate this file with the current version of nanopb generator. +#endif + +PB_BIND(JointValues, JointValues, AUTO) + + +PB_BIND(TimeStamp, TimeStamp, AUTO) + + +PB_BIND(CartesianVector, CartesianVector, AUTO) + + +PB_BIND(Checksum, Checksum, AUTO) + + +PB_BIND(Transformation, Transformation, AUTO) + + +PB_BIND(FriIOValue, FriIOValue, AUTO) + + +PB_BIND(MessageHeader, MessageHeader, AUTO) + + +PB_BIND(ConnectionInfo, ConnectionInfo, AUTO) + + +PB_BIND(RobotInfo, RobotInfo, AUTO) + + +PB_BIND(MessageMonitorData, MessageMonitorData, 2) + + +PB_BIND(MessageIpoData, MessageIpoData, AUTO) + + +PB_BIND(MessageCommandData, MessageCommandData, 2) + + +PB_BIND(MessageEndOf, MessageEndOf, AUTO) + + +PB_BIND(FRIMonitoringMessage, FRIMonitoringMessage, 2) + + +PB_BIND(FRICommandMessage, FRICommandMessage, 2) + + + + + + + + + + + + + +#ifndef PB_CONVERT_DOUBLE_FLOAT +/* On some platforms (such as AVR), double is really float. + * To be able to encode/decode double on these platforms, you need. + * to define PB_CONVERT_DOUBLE_FLOAT in pb.h or compiler command line. + */ +PB_STATIC_ASSERT(sizeof(double) == 8, DOUBLE_MUST_BE_8_BYTES) +#endif diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/FRIMessages.pb.h b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/FRIMessages.pb.h new file mode 100644 index 00000000..57982d00 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/FRIMessages.pb.h @@ -0,0 +1,567 @@ +/* Automatically generated nanopb header */ +/* Generated by nanopb-0.4.8-dev */ + +#ifndef PB_FRIMESSAGES_PB_H_INCLUDED +#define PB_FRIMESSAGES_PB_H_INCLUDED +#include + +#if PB_PROTO_HEADER_VERSION != 40 +#error Regenerate this file with the current version of nanopb generator. +#endif + +/* Enum definitions */ +typedef enum _FRISessionState { + FRISessionState_IDLE = 0, + FRISessionState_MONITORING_WAIT = 1, + FRISessionState_MONITORING_READY = 2, + FRISessionState_COMMANDING_WAIT = 3, + FRISessionState_COMMANDING_ACTIVE = 4 +} FRISessionState; + +typedef enum _FRIConnectionQuality { + FRIConnectionQuality_POOR = 0, + FRIConnectionQuality_FAIR = 1, + FRIConnectionQuality_GOOD = 2, + FRIConnectionQuality_EXCELLENT = 3 +} FRIConnectionQuality; + +typedef enum _SafetyState { + SafetyState_NORMAL_OPERATION = 0, + SafetyState_SAFETY_STOP_LEVEL_0 = 1, + SafetyState_SAFETY_STOP_LEVEL_1 = 2, + SafetyState_SAFETY_STOP_LEVEL_2 = 3 +} SafetyState; + +typedef enum _OperationMode { + OperationMode_TEST_MODE_1 = 0, + OperationMode_TEST_MODE_2 = 1, + OperationMode_AUTOMATIC_MODE = 2 +} OperationMode; + +typedef enum _DriveState { + DriveState_OFF = 0, + DriveState_TRANSITIONING = 1, + DriveState_ACTIVE = 2 +} DriveState; + +typedef enum _ControlMode { + ControlMode_POSITION_CONTROLMODE = 0, + ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE = 1, + ControlMode_JOINT_IMPEDANCE_CONTROLMODE = 2, + ControlMode_NO_CONTROLMODE = 3 +} ControlMode; + +typedef enum _ClientCommandMode { + ClientCommandMode_NO_COMMAND_MODE = 0, + ClientCommandMode_POSITION = 1, + ClientCommandMode_WRENCH = 2, + ClientCommandMode_TORQUE = 3 +} ClientCommandMode; + +typedef enum _OverlayType { + OverlayType_NO_OVERLAY = 0, + OverlayType_JOINT = 1, + OverlayType_CARTESIAN = 2 +} OverlayType; + +typedef enum _FriIOType { + FriIOType_BOOLEAN = 0, + FriIOType_DIGITAL = 1, + FriIOType_ANALOG = 2 +} FriIOType; + +typedef enum _FriIODirection { + FriIODirection_INPUT = 0, + FriIODirection_OUTPUT = 1 +} FriIODirection; + +/* Struct definitions */ +typedef struct _JointValues { + pb_callback_t value; +} JointValues; + +typedef struct _TimeStamp { + uint32_t sec; + uint32_t nanosec; +} TimeStamp; + +typedef struct _CartesianVector { + pb_size_t element_count; + double element[6]; +} CartesianVector; + +typedef struct _Checksum { + bool has_crc32; + int32_t crc32; +} Checksum; + +typedef struct _Transformation { + char name[64]; + pb_size_t matrix_count; + double matrix[12]; + bool has_timestamp; + TimeStamp timestamp; +} Transformation; + +typedef struct _FriIOValue { + char name[64]; + FriIOType type; + FriIODirection direction; + bool has_digitalValue; + uint64_t digitalValue; + bool has_analogValue; + double analogValue; +} FriIOValue; + +typedef struct _MessageHeader { + uint32_t messageIdentifier; + uint32_t sequenceCounter; + uint32_t reflectedSequenceCounter; +} MessageHeader; + +typedef struct _ConnectionInfo { + FRISessionState sessionState; + FRIConnectionQuality quality; + bool has_sendPeriod; + uint32_t sendPeriod; + bool has_receiveMultiplier; + uint32_t receiveMultiplier; +} ConnectionInfo; + +typedef struct _RobotInfo { + bool has_numberOfJoints; + int32_t numberOfJoints; + bool has_safetyState; + SafetyState safetyState; + pb_callback_t driveState; + bool has_operationMode; + OperationMode operationMode; + bool has_controlMode; + ControlMode controlMode; +} RobotInfo; + +typedef struct _MessageMonitorData { + bool has_measuredJointPosition; + JointValues measuredJointPosition; + bool has_measuredTorque; + JointValues measuredTorque; + bool has_commandedJointPosition; + JointValues commandedJointPosition; + bool has_commandedTorque; + JointValues commandedTorque; + bool has_externalTorque; + JointValues externalTorque; + pb_size_t readIORequest_count; + FriIOValue readIORequest[10]; + bool has_timestamp; + TimeStamp timestamp; +} MessageMonitorData; + +typedef struct _MessageIpoData { + bool has_jointPosition; + JointValues jointPosition; + bool has_clientCommandMode; + ClientCommandMode clientCommandMode; + bool has_overlayType; + OverlayType overlayType; + bool has_trackingPerformance; + double trackingPerformance; +} MessageIpoData; + +typedef struct _MessageCommandData { + bool has_jointPosition; + JointValues jointPosition; + bool has_cartesianWrenchFeedForward; + CartesianVector cartesianWrenchFeedForward; + bool has_jointTorque; + JointValues jointTorque; + pb_size_t commandedTransformations_count; + Transformation commandedTransformations[5]; + pb_size_t writeIORequest_count; + FriIOValue writeIORequest[10]; +} MessageCommandData; + +typedef struct _MessageEndOf { + bool has_messageLength; + int32_t messageLength; + bool has_messageChecksum; + Checksum messageChecksum; +} MessageEndOf; + +typedef struct _FRIMonitoringMessage { + MessageHeader header; + bool has_robotInfo; + RobotInfo robotInfo; + bool has_monitorData; + MessageMonitorData monitorData; + bool has_connectionInfo; + ConnectionInfo connectionInfo; + bool has_ipoData; + MessageIpoData ipoData; + pb_size_t requestedTransformations_count; + Transformation requestedTransformations[5]; + bool has_endOfMessageData; + MessageEndOf endOfMessageData; +} FRIMonitoringMessage; + +typedef struct _FRICommandMessage { + MessageHeader header; + bool has_commandData; + MessageCommandData commandData; + bool has_endOfMessageData; + MessageEndOf endOfMessageData; +} FRICommandMessage; + + +#ifdef __cplusplus +extern "C" { +#endif + +/* Helper constants for enums */ +#define _FRISessionState_MIN FRISessionState_IDLE +#define _FRISessionState_MAX FRISessionState_COMMANDING_ACTIVE +#define _FRISessionState_ARRAYSIZE ((FRISessionState)(FRISessionState_COMMANDING_ACTIVE+1)) + +#define _FRIConnectionQuality_MIN FRIConnectionQuality_POOR +#define _FRIConnectionQuality_MAX FRIConnectionQuality_EXCELLENT +#define _FRIConnectionQuality_ARRAYSIZE ((FRIConnectionQuality)(FRIConnectionQuality_EXCELLENT+1)) + +#define _SafetyState_MIN SafetyState_NORMAL_OPERATION +#define _SafetyState_MAX SafetyState_SAFETY_STOP_LEVEL_2 +#define _SafetyState_ARRAYSIZE ((SafetyState)(SafetyState_SAFETY_STOP_LEVEL_2+1)) + +#define _OperationMode_MIN OperationMode_TEST_MODE_1 +#define _OperationMode_MAX OperationMode_AUTOMATIC_MODE +#define _OperationMode_ARRAYSIZE ((OperationMode)(OperationMode_AUTOMATIC_MODE+1)) + +#define _DriveState_MIN DriveState_OFF +#define _DriveState_MAX DriveState_ACTIVE +#define _DriveState_ARRAYSIZE ((DriveState)(DriveState_ACTIVE+1)) + +#define _ControlMode_MIN ControlMode_POSITION_CONTROLMODE +#define _ControlMode_MAX ControlMode_NO_CONTROLMODE +#define _ControlMode_ARRAYSIZE ((ControlMode)(ControlMode_NO_CONTROLMODE+1)) + +#define _ClientCommandMode_MIN ClientCommandMode_NO_COMMAND_MODE +#define _ClientCommandMode_MAX ClientCommandMode_TORQUE +#define _ClientCommandMode_ARRAYSIZE ((ClientCommandMode)(ClientCommandMode_TORQUE+1)) + +#define _OverlayType_MIN OverlayType_NO_OVERLAY +#define _OverlayType_MAX OverlayType_CARTESIAN +#define _OverlayType_ARRAYSIZE ((OverlayType)(OverlayType_CARTESIAN+1)) + +#define _FriIOType_MIN FriIOType_BOOLEAN +#define _FriIOType_MAX FriIOType_ANALOG +#define _FriIOType_ARRAYSIZE ((FriIOType)(FriIOType_ANALOG+1)) + +#define _FriIODirection_MIN FriIODirection_INPUT +#define _FriIODirection_MAX FriIODirection_OUTPUT +#define _FriIODirection_ARRAYSIZE ((FriIODirection)(FriIODirection_OUTPUT+1)) + + + + + + +#define FriIOValue_type_ENUMTYPE FriIOType +#define FriIOValue_direction_ENUMTYPE FriIODirection + + +#define ConnectionInfo_sessionState_ENUMTYPE FRISessionState +#define ConnectionInfo_quality_ENUMTYPE FRIConnectionQuality + +#define RobotInfo_safetyState_ENUMTYPE SafetyState +#define RobotInfo_driveState_ENUMTYPE DriveState +#define RobotInfo_operationMode_ENUMTYPE OperationMode +#define RobotInfo_controlMode_ENUMTYPE ControlMode + + +#define MessageIpoData_clientCommandMode_ENUMTYPE ClientCommandMode +#define MessageIpoData_overlayType_ENUMTYPE OverlayType + + + + + + +/* Initializer values for message structs */ +#define JointValues_init_default {{{NULL}, NULL}} +#define TimeStamp_init_default {0, 0} +#define CartesianVector_init_default {0, {0, 0, 0, 0, 0, 0}} +#define Checksum_init_default {false, 0} +#define Transformation_init_default {"", 0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, false, TimeStamp_init_default} +#define FriIOValue_init_default {"", _FriIOType_MIN, _FriIODirection_MIN, false, 0, false, 0} +#define MessageHeader_init_default {0, 0, 0} +#define ConnectionInfo_init_default {_FRISessionState_MIN, _FRIConnectionQuality_MIN, false, 0, false, 0} +#define RobotInfo_init_default {false, 0, false, _SafetyState_MIN, {{NULL}, NULL}, false, _OperationMode_MIN, false, _ControlMode_MIN} +#define MessageMonitorData_init_default {false, JointValues_init_default, false, JointValues_init_default, false, JointValues_init_default, false, JointValues_init_default, false, JointValues_init_default, 0, {FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default}, false, TimeStamp_init_default} +#define MessageIpoData_init_default {false, JointValues_init_default, false, _ClientCommandMode_MIN, false, _OverlayType_MIN, false, 0} +#define MessageCommandData_init_default {false, JointValues_init_default, false, CartesianVector_init_default, false, JointValues_init_default, 0, {Transformation_init_default, Transformation_init_default, Transformation_init_default, Transformation_init_default, Transformation_init_default}, 0, {FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default}} +#define MessageEndOf_init_default {false, 0, false, Checksum_init_default} +#define FRIMonitoringMessage_init_default {MessageHeader_init_default, false, RobotInfo_init_default, false, MessageMonitorData_init_default, false, ConnectionInfo_init_default, false, MessageIpoData_init_default, 0, {Transformation_init_default, Transformation_init_default, Transformation_init_default, Transformation_init_default, Transformation_init_default}, false, MessageEndOf_init_default} +#define FRICommandMessage_init_default {MessageHeader_init_default, false, MessageCommandData_init_default, false, MessageEndOf_init_default} +#define JointValues_init_zero {{{NULL}, NULL}} +#define TimeStamp_init_zero {0, 0} +#define CartesianVector_init_zero {0, {0, 0, 0, 0, 0, 0}} +#define Checksum_init_zero {false, 0} +#define Transformation_init_zero {"", 0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, false, TimeStamp_init_zero} +#define FriIOValue_init_zero {"", _FriIOType_MIN, _FriIODirection_MIN, false, 0, false, 0} +#define MessageHeader_init_zero {0, 0, 0} +#define ConnectionInfo_init_zero {_FRISessionState_MIN, _FRIConnectionQuality_MIN, false, 0, false, 0} +#define RobotInfo_init_zero {false, 0, false, _SafetyState_MIN, {{NULL}, NULL}, false, _OperationMode_MIN, false, _ControlMode_MIN} +#define MessageMonitorData_init_zero {false, JointValues_init_zero, false, JointValues_init_zero, false, JointValues_init_zero, false, JointValues_init_zero, false, JointValues_init_zero, 0, {FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero}, false, TimeStamp_init_zero} +#define MessageIpoData_init_zero {false, JointValues_init_zero, false, _ClientCommandMode_MIN, false, _OverlayType_MIN, false, 0} +#define MessageCommandData_init_zero {false, JointValues_init_zero, false, CartesianVector_init_zero, false, JointValues_init_zero, 0, {Transformation_init_zero, Transformation_init_zero, Transformation_init_zero, Transformation_init_zero, Transformation_init_zero}, 0, {FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero}} +#define MessageEndOf_init_zero {false, 0, false, Checksum_init_zero} +#define FRIMonitoringMessage_init_zero {MessageHeader_init_zero, false, RobotInfo_init_zero, false, MessageMonitorData_init_zero, false, ConnectionInfo_init_zero, false, MessageIpoData_init_zero, 0, {Transformation_init_zero, Transformation_init_zero, Transformation_init_zero, Transformation_init_zero, Transformation_init_zero}, false, MessageEndOf_init_zero} +#define FRICommandMessage_init_zero {MessageHeader_init_zero, false, MessageCommandData_init_zero, false, MessageEndOf_init_zero} + +/* Field tags (for use in manual encoding/decoding) */ +#define JointValues_value_tag 1 +#define TimeStamp_sec_tag 1 +#define TimeStamp_nanosec_tag 2 +#define CartesianVector_element_tag 1 +#define Checksum_crc32_tag 1 +#define Transformation_name_tag 1 +#define Transformation_matrix_tag 2 +#define Transformation_timestamp_tag 3 +#define FriIOValue_name_tag 1 +#define FriIOValue_type_tag 2 +#define FriIOValue_direction_tag 3 +#define FriIOValue_digitalValue_tag 4 +#define FriIOValue_analogValue_tag 5 +#define MessageHeader_messageIdentifier_tag 1 +#define MessageHeader_sequenceCounter_tag 2 +#define MessageHeader_reflectedSequenceCounter_tag 3 +#define ConnectionInfo_sessionState_tag 1 +#define ConnectionInfo_quality_tag 2 +#define ConnectionInfo_sendPeriod_tag 3 +#define ConnectionInfo_receiveMultiplier_tag 4 +#define RobotInfo_numberOfJoints_tag 1 +#define RobotInfo_safetyState_tag 2 +#define RobotInfo_driveState_tag 5 +#define RobotInfo_operationMode_tag 6 +#define RobotInfo_controlMode_tag 7 +#define MessageMonitorData_measuredJointPosition_tag 1 +#define MessageMonitorData_measuredTorque_tag 2 +#define MessageMonitorData_commandedJointPosition_tag 3 +#define MessageMonitorData_commandedTorque_tag 4 +#define MessageMonitorData_externalTorque_tag 5 +#define MessageMonitorData_readIORequest_tag 8 +#define MessageMonitorData_timestamp_tag 15 +#define MessageIpoData_jointPosition_tag 1 +#define MessageIpoData_clientCommandMode_tag 10 +#define MessageIpoData_overlayType_tag 11 +#define MessageIpoData_trackingPerformance_tag 12 +#define MessageCommandData_jointPosition_tag 1 +#define MessageCommandData_cartesianWrenchFeedForward_tag 2 +#define MessageCommandData_jointTorque_tag 3 +#define MessageCommandData_commandedTransformations_tag 4 +#define MessageCommandData_writeIORequest_tag 5 +#define MessageEndOf_messageLength_tag 1 +#define MessageEndOf_messageChecksum_tag 2 +#define FRIMonitoringMessage_header_tag 1 +#define FRIMonitoringMessage_robotInfo_tag 2 +#define FRIMonitoringMessage_monitorData_tag 3 +#define FRIMonitoringMessage_connectionInfo_tag 4 +#define FRIMonitoringMessage_ipoData_tag 5 +#define FRIMonitoringMessage_requestedTransformations_tag 6 +#define FRIMonitoringMessage_endOfMessageData_tag 15 +#define FRICommandMessage_header_tag 1 +#define FRICommandMessage_commandData_tag 2 +#define FRICommandMessage_endOfMessageData_tag 15 + +/* Struct field encoding specification for nanopb */ +#define JointValues_FIELDLIST(X, a) \ +X(a, CALLBACK, REPEATED, DOUBLE, value, 1) +#define JointValues_CALLBACK pb_default_field_callback +#define JointValues_DEFAULT NULL + +#define TimeStamp_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, UINT32, sec, 1) \ +X(a, STATIC, REQUIRED, UINT32, nanosec, 2) +#define TimeStamp_CALLBACK NULL +#define TimeStamp_DEFAULT NULL + +#define CartesianVector_FIELDLIST(X, a) \ +X(a, STATIC, REPEATED, DOUBLE, element, 1) +#define CartesianVector_CALLBACK NULL +#define CartesianVector_DEFAULT NULL + +#define Checksum_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, INT32, crc32, 1) +#define Checksum_CALLBACK NULL +#define Checksum_DEFAULT NULL + +#define Transformation_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, STRING, name, 1) \ +X(a, STATIC, REPEATED, DOUBLE, matrix, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, timestamp, 3) +#define Transformation_CALLBACK NULL +#define Transformation_DEFAULT NULL +#define Transformation_timestamp_MSGTYPE TimeStamp + +#define FriIOValue_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, STRING, name, 1) \ +X(a, STATIC, REQUIRED, UENUM, type, 2) \ +X(a, STATIC, REQUIRED, UENUM, direction, 3) \ +X(a, STATIC, OPTIONAL, UINT64, digitalValue, 4) \ +X(a, STATIC, OPTIONAL, DOUBLE, analogValue, 5) +#define FriIOValue_CALLBACK NULL +#define FriIOValue_DEFAULT NULL + +#define MessageHeader_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, UINT32, messageIdentifier, 1) \ +X(a, STATIC, REQUIRED, UINT32, sequenceCounter, 2) \ +X(a, STATIC, REQUIRED, UINT32, reflectedSequenceCounter, 3) +#define MessageHeader_CALLBACK NULL +#define MessageHeader_DEFAULT NULL + +#define ConnectionInfo_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, UENUM, sessionState, 1) \ +X(a, STATIC, REQUIRED, UENUM, quality, 2) \ +X(a, STATIC, OPTIONAL, UINT32, sendPeriod, 3) \ +X(a, STATIC, OPTIONAL, UINT32, receiveMultiplier, 4) +#define ConnectionInfo_CALLBACK NULL +#define ConnectionInfo_DEFAULT NULL + +#define RobotInfo_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, INT32, numberOfJoints, 1) \ +X(a, STATIC, OPTIONAL, UENUM, safetyState, 2) \ +X(a, CALLBACK, REPEATED, UENUM, driveState, 5) \ +X(a, STATIC, OPTIONAL, UENUM, operationMode, 6) \ +X(a, STATIC, OPTIONAL, UENUM, controlMode, 7) +#define RobotInfo_CALLBACK pb_default_field_callback +#define RobotInfo_DEFAULT NULL + +#define MessageMonitorData_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, MESSAGE, measuredJointPosition, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, measuredTorque, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, commandedJointPosition, 3) \ +X(a, STATIC, OPTIONAL, MESSAGE, commandedTorque, 4) \ +X(a, STATIC, OPTIONAL, MESSAGE, externalTorque, 5) \ +X(a, STATIC, REPEATED, MESSAGE, readIORequest, 8) \ +X(a, STATIC, OPTIONAL, MESSAGE, timestamp, 15) +#define MessageMonitorData_CALLBACK NULL +#define MessageMonitorData_DEFAULT NULL +#define MessageMonitorData_measuredJointPosition_MSGTYPE JointValues +#define MessageMonitorData_measuredTorque_MSGTYPE JointValues +#define MessageMonitorData_commandedJointPosition_MSGTYPE JointValues +#define MessageMonitorData_commandedTorque_MSGTYPE JointValues +#define MessageMonitorData_externalTorque_MSGTYPE JointValues +#define MessageMonitorData_readIORequest_MSGTYPE FriIOValue +#define MessageMonitorData_timestamp_MSGTYPE TimeStamp + +#define MessageIpoData_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, MESSAGE, jointPosition, 1) \ +X(a, STATIC, OPTIONAL, UENUM, clientCommandMode, 10) \ +X(a, STATIC, OPTIONAL, UENUM, overlayType, 11) \ +X(a, STATIC, OPTIONAL, DOUBLE, trackingPerformance, 12) +#define MessageIpoData_CALLBACK NULL +#define MessageIpoData_DEFAULT NULL +#define MessageIpoData_jointPosition_MSGTYPE JointValues + +#define MessageCommandData_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, MESSAGE, jointPosition, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, cartesianWrenchFeedForward, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, jointTorque, 3) \ +X(a, STATIC, REPEATED, MESSAGE, commandedTransformations, 4) \ +X(a, STATIC, REPEATED, MESSAGE, writeIORequest, 5) +#define MessageCommandData_CALLBACK NULL +#define MessageCommandData_DEFAULT NULL +#define MessageCommandData_jointPosition_MSGTYPE JointValues +#define MessageCommandData_cartesianWrenchFeedForward_MSGTYPE CartesianVector +#define MessageCommandData_jointTorque_MSGTYPE JointValues +#define MessageCommandData_commandedTransformations_MSGTYPE Transformation +#define MessageCommandData_writeIORequest_MSGTYPE FriIOValue + +#define MessageEndOf_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, INT32, messageLength, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, messageChecksum, 2) +#define MessageEndOf_CALLBACK NULL +#define MessageEndOf_DEFAULT NULL +#define MessageEndOf_messageChecksum_MSGTYPE Checksum + +#define FRIMonitoringMessage_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, MESSAGE, header, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, robotInfo, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, monitorData, 3) \ +X(a, STATIC, OPTIONAL, MESSAGE, connectionInfo, 4) \ +X(a, STATIC, OPTIONAL, MESSAGE, ipoData, 5) \ +X(a, STATIC, REPEATED, MESSAGE, requestedTransformations, 6) \ +X(a, STATIC, OPTIONAL, MESSAGE, endOfMessageData, 15) +#define FRIMonitoringMessage_CALLBACK NULL +#define FRIMonitoringMessage_DEFAULT NULL +#define FRIMonitoringMessage_header_MSGTYPE MessageHeader +#define FRIMonitoringMessage_robotInfo_MSGTYPE RobotInfo +#define FRIMonitoringMessage_monitorData_MSGTYPE MessageMonitorData +#define FRIMonitoringMessage_connectionInfo_MSGTYPE ConnectionInfo +#define FRIMonitoringMessage_ipoData_MSGTYPE MessageIpoData +#define FRIMonitoringMessage_requestedTransformations_MSGTYPE Transformation +#define FRIMonitoringMessage_endOfMessageData_MSGTYPE MessageEndOf + +#define FRICommandMessage_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, MESSAGE, header, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, commandData, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, endOfMessageData, 15) +#define FRICommandMessage_CALLBACK NULL +#define FRICommandMessage_DEFAULT NULL +#define FRICommandMessage_header_MSGTYPE MessageHeader +#define FRICommandMessage_commandData_MSGTYPE MessageCommandData +#define FRICommandMessage_endOfMessageData_MSGTYPE MessageEndOf + +extern const pb_msgdesc_t JointValues_msg; +extern const pb_msgdesc_t TimeStamp_msg; +extern const pb_msgdesc_t CartesianVector_msg; +extern const pb_msgdesc_t Checksum_msg; +extern const pb_msgdesc_t Transformation_msg; +extern const pb_msgdesc_t FriIOValue_msg; +extern const pb_msgdesc_t MessageHeader_msg; +extern const pb_msgdesc_t ConnectionInfo_msg; +extern const pb_msgdesc_t RobotInfo_msg; +extern const pb_msgdesc_t MessageMonitorData_msg; +extern const pb_msgdesc_t MessageIpoData_msg; +extern const pb_msgdesc_t MessageCommandData_msg; +extern const pb_msgdesc_t MessageEndOf_msg; +extern const pb_msgdesc_t FRIMonitoringMessage_msg; +extern const pb_msgdesc_t FRICommandMessage_msg; + +/* Defines for backwards compatibility with code written before nanopb-0.4.0 */ +#define JointValues_fields &JointValues_msg +#define TimeStamp_fields &TimeStamp_msg +#define CartesianVector_fields &CartesianVector_msg +#define Checksum_fields &Checksum_msg +#define Transformation_fields &Transformation_msg +#define FriIOValue_fields &FriIOValue_msg +#define MessageHeader_fields &MessageHeader_msg +#define ConnectionInfo_fields &ConnectionInfo_msg +#define RobotInfo_fields &RobotInfo_msg +#define MessageMonitorData_fields &MessageMonitorData_msg +#define MessageIpoData_fields &MessageIpoData_msg +#define MessageCommandData_fields &MessageCommandData_msg +#define MessageEndOf_fields &MessageEndOf_msg +#define FRIMonitoringMessage_fields &FRIMonitoringMessage_msg +#define FRICommandMessage_fields &FRICommandMessage_msg + +/* Maximum encoded size of messages (where known) */ +/* JointValues_size depends on runtime parameters */ +/* RobotInfo_size depends on runtime parameters */ +/* MessageMonitorData_size depends on runtime parameters */ +/* MessageIpoData_size depends on runtime parameters */ +/* MessageCommandData_size depends on runtime parameters */ +/* FRIMonitoringMessage_size depends on runtime parameters */ +/* FRICommandMessage_size depends on runtime parameters */ +#define CartesianVector_size 54 +#define Checksum_size 11 +#define ConnectionInfo_size 16 +#define FriIOValue_size 89 +#define MessageEndOf_size 24 +#define MessageHeader_size 18 +#define TimeStamp_size 12 +#define Transformation_size 187 + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/HWIFClientApplication.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/HWIFClientApplication.cpp similarity index 100% rename from kuka_sunrise_fri_driver/src/fri_client_sdk/HWIFClientApplication.cpp rename to kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/HWIFClientApplication.cpp diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friClientApplication.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friClientApplication.cpp new file mode 100644 index 00000000..d94eec77 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friClientApplication.cpp @@ -0,0 +1,201 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace KUKA::FRI; + +//****************************************************************************** +ClientApplication::ClientApplication(IConnection & connection, IClient & client) +: _connection(connection), _robotClient(&client), _trafoClient(NULL), _data(NULL) +{ + _data = _robotClient->createData(); +} + +//****************************************************************************** +ClientApplication::ClientApplication( + IConnection & connection, IClient & client, + TransformationClient & trafoClient) +: _connection(connection), _robotClient(&client), _trafoClient(&trafoClient), _data(NULL) +{ + _data = _robotClient->createData(); + _trafoClient->linkData(_data); +} + +//****************************************************************************** +ClientApplication::~ClientApplication() +{ + disconnect(); + delete _data; +} + +//****************************************************************************** +bool ClientApplication::connect(int port, const char * remoteHost) +{ + if (_connection.isOpen()) { + std::cout << "Warning: client application already connected!" << std::endl; + return true; + } + + return _connection.open(port, remoteHost); +} + +//****************************************************************************** +void ClientApplication::disconnect() +{ + if (_connection.isOpen()) {_connection.close();} +} + +//****************************************************************************** +bool ClientApplication::step() +{ + if (!_connection.isOpen()) { + std::cout << "Error: client application is not connected!" << std::endl; + return false; + } + + // ************************************************************************** + // Receive and decode new monitoring message + // ************************************************************************** + int size = _connection.receive(_data->receiveBuffer, FRI_MONITOR_MSG_MAX_SIZE); + + if (size <= 0) { // TODO: size == 0 -> connection closed (maybe go to IDLE instead of stopping?) + std::cout << "Error: failed while trying to receive monitoring message!" << std::endl; + return false; + } + + if (!_data->decoder.decode(_data->receiveBuffer, size)) { + return false; + } + + // check message type (so that our wrappers match) + if (_data->expectedMonitorMsgID != _data->monitoringMsg.header.messageIdentifier) { + std::cout << "Error: incompatible IDs for received message, got: " << + _data->monitoringMsg.header.messageIdentifier << " expected: " << _data->expectedMonitorMsgID; + return false; + } + + // ************************************************************************** + // callbacks + // ************************************************************************** + // reset command message before callbacks + _data->resetCommandMessage(); + + // callbacks for robot client + ESessionState currentState = (ESessionState)_data->monitoringMsg.connectionInfo.sessionState; + + if (_data->lastState != currentState) { + _robotClient->onStateChange(_data->lastState, currentState); + _data->lastState = currentState; + } + + switch (currentState) { + case MONITORING_WAIT: + case MONITORING_READY: + _robotClient->monitor(); + break; + case COMMANDING_WAIT: + _robotClient->waitForCommand(); + break; + case COMMANDING_ACTIVE: + _robotClient->command(); + break; + case IDLE: + default: + return true; // nothing to send back + } + + // callback for transformation client + if (_trafoClient != NULL) { + _trafoClient->provide(); + } + + // ************************************************************************** + // Encode and send command message + // ************************************************************************** + + _data->lastSendCounter++; + // check if its time to send an answer + if (_data->lastSendCounter >= _data->monitoringMsg.connectionInfo.receiveMultiplier) { + _data->lastSendCounter = 0; + + // set sequence counters + _data->commandMsg.header.sequenceCounter = _data->sequenceCounter++; + _data->commandMsg.header.reflectedSequenceCounter = + _data->monitoringMsg.header.sequenceCounter; + + if (!_data->encoder.encode(_data->sendBuffer, size)) { + return false; + } + + if (!_connection.send(_data->sendBuffer, size)) { + std::cout << "Error: failed while trying to send command message!" << std::endl; + return false; + } + } + + return true; +} diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friClientData.h b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friClientData.h new file mode 100644 index 00000000..25f7af06 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friClientData.h @@ -0,0 +1,242 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#ifndef _KUKA_FRI_CLIENT_DATA_H +#define _KUKA_FRI_CLIENT_DATA_H + +#include + +#include +#include +#include +#include +#include + +namespace KUKA +{ +namespace FRI +{ + +struct ClientData +{ + char receiveBuffer[FRI_MONITOR_MSG_MAX_SIZE]; //!< monitoring message receive buffer + char sendBuffer[FRI_COMMAND_MSG_MAX_SIZE]; //!< command message send buffer + + FRIMonitoringMessage monitoringMsg; //!< monitoring message struct + FRICommandMessage commandMsg; //!< command message struct + + MonitoringMessageDecoder decoder; //!< monitoring message decoder + CommandMessageEncoder encoder; //!< command message encoder + + ESessionState lastState; //!< last FRI state + uint32_t sequenceCounter; //!< sequence counter for command messages + uint32_t lastSendCounter; //!< steps since last send command + uint32_t expectedMonitorMsgID; //!< expected ID for received monitoring messages + + const size_t MAX_REQUESTED_TRANSFORMATIONS; //!< maximum count of requested transformations + const size_t MAX_SIZE_TRANSFORMATION_ID; //!< maximum size in bytes of a transformation ID + std::vector requestedTrafoIDs; //!< list of requested transformation ids + + ClientData(int numDofs) + : decoder(&monitoringMsg, numDofs), + encoder(&commandMsg, numDofs), + lastState(IDLE), + sequenceCounter(0), + lastSendCounter(0), + expectedMonitorMsgID(0), + MAX_REQUESTED_TRANSFORMATIONS(sizeof(monitoringMsg.requestedTransformations) / + sizeof(monitoringMsg.requestedTransformations[0])), + MAX_SIZE_TRANSFORMATION_ID(sizeof(monitoringMsg.requestedTransformations[0].name)) + { + requestedTrafoIDs.reserve(MAX_REQUESTED_TRANSFORMATIONS); + } + + void resetCommandMessage() + { + commandMsg.commandData.has_jointPosition = false; + commandMsg.commandData.has_cartesianWrenchFeedForward = false; + commandMsg.commandData.has_jointTorque = false; + commandMsg.commandData.commandedTransformations_count = 0; + commandMsg.has_commandData = false; + commandMsg.commandData.writeIORequest_count = 0; + } + + //****************************************************************************** + static const FriIOValue & getBooleanIOValue( + const FRIMonitoringMessage * message, + const char * name) + { + return getIOValue(message, name, FriIOType_BOOLEAN); + } + + //****************************************************************************** + static const FriIOValue & getDigitalIOValue( + const FRIMonitoringMessage * message, + const char * name) + { + return getIOValue(message, name, FriIOType_DIGITAL); + } + + //****************************************************************************** + static const FriIOValue & getAnalogIOValue( + const FRIMonitoringMessage * message, + const char * name) + { + return getIOValue(message, name, FriIOType_ANALOG); + } + + //****************************************************************************** + static void setBooleanIOValue( + FRICommandMessage * message, const char * name, const bool value, + const FRIMonitoringMessage * monMessage) + { + setIOValue(message, name, monMessage, FriIOType_BOOLEAN).digitalValue = value; + } + + //****************************************************************************** + static void setDigitalIOValue( + FRICommandMessage * message, const char * name, const unsigned long long value, + const FRIMonitoringMessage * monMessage) + { + setIOValue(message, name, monMessage, FriIOType_DIGITAL).digitalValue = value; + } + + //****************************************************************************** + static void setAnalogIOValue( + FRICommandMessage * message, const char * name, const double value, + const FRIMonitoringMessage * monMessage) + { + setIOValue(message, name, monMessage, FriIOType_ANALOG).analogValue = value; + } + +protected: + //****************************************************************************** + static const FriIOValue & getIOValue( + const FRIMonitoringMessage * message, const char * name, + const FriIOType ioType) + { + if (message != NULL && message->has_monitorData == true) { + const MessageMonitorData & monData = message->monitorData; + const bool analogValue = (ioType == FriIOType_ANALOG); + const bool digitalValue = (ioType == FriIOType_DIGITAL || ioType == FriIOType_BOOLEAN); + for (size_t i = 0; i < monData.readIORequest_count; i++) { + const FriIOValue & ioValue = monData.readIORequest[i]; + if (strcmp(name, ioValue.name) == 0) { + if (ioValue.type == ioType && + ioValue.has_digitalValue == digitalValue && + ioValue.has_analogValue == analogValue) + { + return ioValue; + } + + const char * ioTypeName; + switch (ioType) { + case FriIOType_ANALOG: ioTypeName = "analog value"; break; + case FriIOType_DIGITAL: ioTypeName = "digital value"; break; + case FriIOType_BOOLEAN: ioTypeName = "boolean"; break; + default: ioTypeName = "?"; break; + } + + throw FRIException("IO %s is not of type %s.", name, ioTypeName); + } + } + } + + throw FRIException("Could not locate IO %s in monitor message.", name); + } + + //****************************************************************************** + static FriIOValue & setIOValue( + FRICommandMessage * message, const char * name, + const FRIMonitoringMessage * monMessage, const FriIOType ioType) + { + MessageCommandData & cmdData = message->commandData; + const size_t maxIOs = sizeof(cmdData.writeIORequest) / sizeof(cmdData.writeIORequest[0]); + if (cmdData.writeIORequest_count < maxIOs) { + // call getter which will raise an exception if the output doesn't exist + // or is of wrong type. + if (getIOValue(monMessage, name, ioType).direction != FriIODirection_OUTPUT) { + throw FRIException("IO %s is not an output value.", name); + } + + // add IO value to command message + FriIOValue & ioValue = cmdData.writeIORequest[cmdData.writeIORequest_count]; + + strncpy(ioValue.name, name, sizeof(ioValue.name) - 1); + ioValue.name[sizeof(ioValue.name) - 1] = 0; // ensure termination + ioValue.type = ioType; + ioValue.has_digitalValue = (ioType == FriIOType_DIGITAL || ioType == FriIOType_BOOLEAN); + ioValue.has_analogValue = (ioType == FriIOType_ANALOG); + ioValue.direction = FriIODirection_OUTPUT; + + cmdData.writeIORequest_count++; + message->has_commandData = true; + + return ioValue; + } else { + throw FRIException("Exceeded maximum number of IOs that can be set."); + } + } +}; + +} +} + + +#endif // _KUKA_FRI_CLIENT_DATA_H diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friCommandMessageEncoder.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friCommandMessageEncoder.cpp new file mode 100644 index 00000000..c4e118f0 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friCommandMessageEncoder.cpp @@ -0,0 +1,122 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#include +#include +#include + +using namespace KUKA::FRI; + +//****************************************************************************** +CommandMessageEncoder::CommandMessageEncoder(FRICommandMessage * pMessage, int num) +: m_nNum(num), m_pMessage(pMessage) +{ + initMessage(); +} + +//****************************************************************************** +CommandMessageEncoder::~CommandMessageEncoder() +{ + +} + +//****************************************************************************** +void CommandMessageEncoder::initMessage() +{ + m_pMessage->has_commandData = false; + m_pMessage->has_endOfMessageData = false; + m_pMessage->commandData.has_jointPosition = false; + m_pMessage->commandData.has_cartesianWrenchFeedForward = false; + m_pMessage->commandData.has_jointTorque = false; + m_pMessage->commandData.commandedTransformations_count = 0; + m_pMessage->header.messageIdentifier = 0; + // init with 0. Necessary for creating the correct reflected sequence count in the monitoring msg + m_pMessage->header.sequenceCounter = 0; + m_pMessage->header.reflectedSequenceCounter = 0; + + m_pMessage->commandData.writeIORequest_count = 0; + + // allocate and map memory for protobuf repeated structures + map_repeatedDouble( + FRI_MANAGER_NANOPB_ENCODE, m_nNum, + &m_pMessage->commandData.jointPosition.value, + &m_tRecvContainer.jointPosition); + map_repeatedDouble( + FRI_MANAGER_NANOPB_ENCODE, m_nNum, + &m_pMessage->commandData.jointTorque.value, + &m_tRecvContainer.jointTorque); + + // nanopb encoding needs to know how many elements the static array contains + // a Cartesian wrench feed forward vector always contains 6 elements + m_pMessage->commandData.cartesianWrenchFeedForward.element_count = 6; +} + +//****************************************************************************** +bool CommandMessageEncoder::encode(char * buffer, int & size) +{ + // generate stream for encoding + pb_ostream_t stream = pb_ostream_from_buffer((uint8_t *)buffer, FRI_COMMAND_MSG_MAX_SIZE); + // encode monitoring Message to stream + bool status = pb_encode(&stream, FRICommandMessage_fields, m_pMessage); + size = stream.bytes_written; + if (!status) { + printf("!!encoding error: %s!!\n", PB_GET_ERROR(&stream)); + } + return status; +} diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friCommandMessageEncoder.h b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friCommandMessageEncoder.h new file mode 100644 index 00000000..b32ddb0a --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friCommandMessageEncoder.h @@ -0,0 +1,115 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#ifndef _KUKA_FRI_COMMANDMESSAGEENCODER_H +#define _KUKA_FRI_COMMANDMESSAGEENCODER_H + + +#include +#include + + +namespace KUKA +{ +namespace FRI +{ + +static const int FRI_COMMAND_MSG_MAX_SIZE = 1500; //!< max size of a FRI command message + +class CommandMessageEncoder +{ + +public: + CommandMessageEncoder(FRICommandMessage * pMessage, int num); + + ~CommandMessageEncoder(); + + bool encode(char * buffer, int & size); + +private: + struct LocalCommandDataContainer + { + tRepeatedDoubleArguments jointPosition; + tRepeatedDoubleArguments jointTorque; + + LocalCommandDataContainer() + { + init_repeatedDouble(&jointPosition); + init_repeatedDouble(&jointTorque); + } + + ~LocalCommandDataContainer() + { + free_repeatedDouble(&jointPosition); + free_repeatedDouble(&jointTorque); + } + }; + + int m_nNum; + + LocalCommandDataContainer m_tRecvContainer; + FRICommandMessage * m_pMessage; + + void initMessage(); +}; + +} +} + +#endif // _KUKA_FRI_COMMANDMESSAGEENCODER_H diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRClient.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRClient.cpp new file mode 100644 index 00000000..f5178675 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRClient.cpp @@ -0,0 +1,121 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#include +#include +#include +#include + +using namespace KUKA::FRI; +char FRIException::_buffer[1024] = {0}; + +//****************************************************************************** +LBRClient::LBRClient() +{ + +} + +//****************************************************************************** +LBRClient::~LBRClient() +{ + +} + +//****************************************************************************** +void LBRClient::onStateChange(ESessionState oldState, ESessionState newState) +{ + // TODO: String converter function for states + std::cout << "LBRiiwaClient state changed from " << + oldState << " to " << newState << std::endl; +} + +//****************************************************************************** +void LBRClient::monitor() +{ + robotCommand().setJointPosition(robotState().getCommandedJointPosition()); +} + +//****************************************************************************** +void LBRClient::waitForCommand() +{ + robotCommand().setJointPosition(robotState().getIpoJointPosition()); +} + +//****************************************************************************** +void LBRClient::command() +{ + robotCommand().setJointPosition(robotState().getIpoJointPosition()); +} + +//****************************************************************************** +ClientData * LBRClient::createData() +{ + ClientData * data = new ClientData(_robotState.NUMBER_OF_JOINTS); + + // link monitoring and command message to wrappers + _robotState._message = &data->monitoringMsg; + _robotCommand._cmdMessage = &data->commandMsg; + _robotCommand._monMessage = &data->monitoringMsg; + + // set specific message IDs + data->expectedMonitorMsgID = _robotState.LBRMONITORMESSAGEID; + data->commandMsg.header.messageIdentifier = _robotCommand.LBRCOMMANDMESSAGEID; + + return data; +} diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRCommand.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRCommand.cpp new file mode 100644 index 00000000..471973fe --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRCommand.cpp @@ -0,0 +1,113 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#include +#include +#include +#include + +using namespace KUKA::FRI; + +//****************************************************************************** +void LBRCommand::setJointPosition(const double * values) +{ + _cmdMessage->has_commandData = true; + _cmdMessage->commandData.has_jointPosition = true; + tRepeatedDoubleArguments * dest = + (tRepeatedDoubleArguments *)_cmdMessage->commandData.jointPosition.value.arg; + memcpy(dest->value, values, LBRState::NUMBER_OF_JOINTS * sizeof(double)); +} + +//****************************************************************************** +void LBRCommand::setWrench(const double * wrench) +{ + _cmdMessage->has_commandData = true; + _cmdMessage->commandData.has_cartesianWrenchFeedForward = true; + + double * dest = _cmdMessage->commandData.cartesianWrenchFeedForward.element; + memcpy(dest, wrench, 6 * sizeof(double)); +} +//****************************************************************************** +void LBRCommand::setTorque(const double * torques) +{ + _cmdMessage->has_commandData = true; + _cmdMessage->commandData.has_jointTorque = true; + + tRepeatedDoubleArguments * dest = + (tRepeatedDoubleArguments *)_cmdMessage->commandData.jointTorque.value.arg; + memcpy(dest->value, torques, LBRState::NUMBER_OF_JOINTS * sizeof(double)); +} + +//****************************************************************************** +void LBRCommand::setBooleanIOValue(const char * name, const bool value) +{ + ClientData::setBooleanIOValue(_cmdMessage, name, value, _monMessage); +} + +//****************************************************************************** +void LBRCommand::setAnalogIOValue(const char * name, const double value) +{ + ClientData::setAnalogIOValue(_cmdMessage, name, value, _monMessage); +} + +//****************************************************************************** +void LBRCommand::setDigitalIOValue(const char * name, const unsigned long long value) +{ + ClientData::setDigitalIOValue(_cmdMessage, name, value, _monMessage); +} diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRState.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRState.cpp new file mode 100644 index 00000000..90ebeedc --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRState.cpp @@ -0,0 +1,232 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#include +#include +#include + +using namespace KUKA::FRI; + + +LBRState::LBRState() +: _message(0) +{ + +} +//****************************************************************************** +double LBRState::getSampleTime() const +{ + return _message->connectionInfo.sendPeriod * 0.001; +} + +//****************************************************************************** +ESessionState LBRState::getSessionState() const +{ + return (ESessionState)_message->connectionInfo.sessionState; +} + +//****************************************************************************** +EConnectionQuality LBRState::getConnectionQuality() const +{ + return (EConnectionQuality)_message->connectionInfo.quality; +} + +//****************************************************************************** +ESafetyState LBRState::getSafetyState() const +{ + return (ESafetyState)_message->robotInfo.safetyState; +} + +//****************************************************************************** +EOperationMode LBRState::getOperationMode() const +{ + return (EOperationMode)_message->robotInfo.operationMode; +} + +//****************************************************************************** +EDriveState LBRState::getDriveState() const +{ + tRepeatedIntArguments * values = + (tRepeatedIntArguments *)_message->robotInfo.driveState.arg; + int firstState = (int)values->value[0]; + for (int i = 1; i < NUMBER_OF_JOINTS; i++) { + int state = (int)values->value[i]; + if (state != firstState) { + return TRANSITIONING; + } + } + return (EDriveState)firstState; +} + + +//******************************************************************************** +EOverlayType LBRState::getOverlayType() const +{ + return (EOverlayType)_message->ipoData.overlayType; +} + +//******************************************************************************** +EClientCommandMode LBRState::getClientCommandMode() const +{ + return (EClientCommandMode)_message->ipoData.clientCommandMode; +} + + +//****************************************************************************** +EControlMode LBRState::getControlMode() const +{ + return (EControlMode)_message->robotInfo.controlMode; +} + +//****************************************************************************** +unsigned int LBRState::getTimestampSec() const +{ + return _message->monitorData.timestamp.sec; +} + +//****************************************************************************** +unsigned int LBRState::getTimestampNanoSec() const +{ + return _message->monitorData.timestamp.nanosec; +} + +//****************************************************************************** +const double * LBRState::getMeasuredJointPosition() const +{ + tRepeatedDoubleArguments * values = + (tRepeatedDoubleArguments *)_message->monitorData.measuredJointPosition.value.arg; + return (double *)values->value; +} + +//****************************************************************************** +const double * LBRState::getCommandedJointPosition() const +{ + tRepeatedDoubleArguments * values = + (tRepeatedDoubleArguments *)_message->monitorData.commandedJointPosition.value.arg; + return (double *)values->value; +} + +//****************************************************************************** +const double * LBRState::getMeasuredTorque() const +{ + tRepeatedDoubleArguments * values = + (tRepeatedDoubleArguments *)_message->monitorData.measuredTorque.value.arg; + return (double *)values->value; +} + +//****************************************************************************** +const double * LBRState::getCommandedTorque() const +{ + tRepeatedDoubleArguments * values = + (tRepeatedDoubleArguments *)_message->monitorData.commandedTorque.value.arg; + return (double *)values->value; +} + +//****************************************************************************** +const double * LBRState::getExternalTorque() const +{ + tRepeatedDoubleArguments * values = + (tRepeatedDoubleArguments *)_message->monitorData.externalTorque.value.arg; + return (double *)values->value; +} + +//****************************************************************************** +const double * LBRState::getIpoJointPosition() const +{ + if (!_message->ipoData.has_jointPosition) { + throw FRIException("IPO joint position not available in monitoring mode."); + return NULL; + } + + tRepeatedDoubleArguments * values = + (tRepeatedDoubleArguments *)_message->ipoData.jointPosition.value.arg; + return (double *)values->value; +} + +//****************************************************************************** +double LBRState::getTrackingPerformance() const +{ + if (!_message->ipoData.has_trackingPerformance) {return 0.0;} + + return _message->ipoData.trackingPerformance; +} + +//****************************************************************************** +bool LBRState::getBooleanIOValue(const char * name) const +{ + return ClientData::getBooleanIOValue(_message, name).digitalValue != 0; +} + +//****************************************************************************** +unsigned long long LBRState::getDigitalIOValue(const char * name) const +{ + return ClientData::getDigitalIOValue(_message, name).digitalValue; +} + +//****************************************************************************** +double LBRState::getAnalogIOValue(const char * name) const +{ + return ClientData::getAnalogIOValue(_message, name).analogValue; +} + +//****************************************************************************** +/*const std::vector& LBRState::getRequestedIO_IDs() const +{ + return _clientData->getRequestedIO_IDs(); +}*/ diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friMonitoringMessageDecoder.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friMonitoringMessageDecoder.cpp new file mode 100644 index 00000000..f39c34f2 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friMonitoringMessageDecoder.cpp @@ -0,0 +1,151 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#include +#include +#include + + +using namespace KUKA::FRI; + +//****************************************************************************** +MonitoringMessageDecoder::MonitoringMessageDecoder(FRIMonitoringMessage * pMessage, int num) +: m_nNum(num), m_pMessage(pMessage) +{ + initMessage(); +} + +//****************************************************************************** +MonitoringMessageDecoder::~MonitoringMessageDecoder() +{ + +} + +//****************************************************************************** +void MonitoringMessageDecoder::initMessage() +{ + // set initial data + // it is assumed that no robot information and monitoring data is available and therefore the + // optional fields are initialized with false + m_pMessage->has_robotInfo = false; + m_pMessage->has_monitorData = false; + m_pMessage->has_connectionInfo = true; + m_pMessage->has_ipoData = false; + m_pMessage->requestedTransformations_count = 0; + m_pMessage->has_endOfMessageData = false; + + + m_pMessage->header.messageIdentifier = 0; + m_pMessage->header.reflectedSequenceCounter = 0; + m_pMessage->header.sequenceCounter = 0; + + m_pMessage->connectionInfo.sessionState = FRISessionState_IDLE; + m_pMessage->connectionInfo.quality = FRIConnectionQuality_POOR; + + m_pMessage->monitorData.readIORequest_count = 0; + + // allocate and map memory for protobuf repeated structures + map_repeatedDouble( + FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.measuredJointPosition.value, + &m_tSendContainer.m_AxQMsrLocal); + + map_repeatedDouble( + FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.measuredTorque.value, + &m_tSendContainer.m_AxTauMsrLocal); + + map_repeatedDouble( + FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.commandedJointPosition.value, + &m_tSendContainer.m_AxQCmdT1mLocal); + + map_repeatedDouble( + FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.commandedTorque.value, + &m_tSendContainer.m_AxTauCmdLocal); + + map_repeatedDouble( + FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.externalTorque.value, + &m_tSendContainer.m_AxTauExtMsrLocal); + + map_repeatedDouble( + FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->ipoData.jointPosition.value, + &m_tSendContainer.m_AxQCmdIPO); + + map_repeatedInt( + FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->robotInfo.driveState, + &m_tSendContainer.m_AxDriveStateLocal); +} + +//****************************************************************************** +bool MonitoringMessageDecoder::decode(char * buffer, int size) +{ + pb_istream_t stream = pb_istream_from_buffer((uint8_t *)buffer, size); + + bool status = pb_decode(&stream, FRIMonitoringMessage_fields, m_pMessage); + if (!status) { + printf("!!decoding error: %s!!\n", PB_GET_ERROR(&stream)); + } + + return status; +} diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friMonitoringMessageDecoder.h b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friMonitoringMessageDecoder.h new file mode 100644 index 00000000..b0be5a59 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friMonitoringMessageDecoder.h @@ -0,0 +1,130 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#ifndef _KUKA_FRI_MONITORINGMESSAGEDECODER_H +#define _KUKA_FRI_MONITORINGMESSAGEDECODER_H + +#include +#include + + +namespace KUKA +{ +namespace FRI +{ + +static const int FRI_MONITOR_MSG_MAX_SIZE = 1500; //!< max size of a FRI monitoring message + + +class MonitoringMessageDecoder +{ + +public: + MonitoringMessageDecoder(FRIMonitoringMessage * pMessage, int num); + + ~MonitoringMessageDecoder(); + + bool decode(char * buffer, int size); + +private: + struct LocalMonitoringDataContainer + { + tRepeatedDoubleArguments m_AxQMsrLocal; + tRepeatedDoubleArguments m_AxTauMsrLocal; + tRepeatedDoubleArguments m_AxQCmdT1mLocal; + tRepeatedDoubleArguments m_AxTauCmdLocal; + tRepeatedDoubleArguments m_AxTauExtMsrLocal; + tRepeatedIntArguments m_AxDriveStateLocal; + tRepeatedDoubleArguments m_AxQCmdIPO; + + LocalMonitoringDataContainer() + { + init_repeatedDouble(&m_AxQMsrLocal); + init_repeatedDouble(&m_AxTauMsrLocal); + init_repeatedDouble(&m_AxQCmdT1mLocal); + init_repeatedDouble(&m_AxTauCmdLocal); + init_repeatedDouble(&m_AxTauExtMsrLocal); + init_repeatedDouble(&m_AxQCmdIPO); + init_repeatedInt(&m_AxDriveStateLocal); + } + + ~LocalMonitoringDataContainer() + { + free_repeatedDouble(&m_AxQMsrLocal); + free_repeatedDouble(&m_AxTauMsrLocal); + free_repeatedDouble(&m_AxQCmdT1mLocal); + free_repeatedDouble(&m_AxTauCmdLocal); + free_repeatedDouble(&m_AxTauExtMsrLocal); + free_repeatedDouble(&m_AxQCmdIPO); + free_repeatedInt(&m_AxDriveStateLocal); + } + }; + + int m_nNum; + + LocalMonitoringDataContainer m_tSendContainer; + FRIMonitoringMessage * m_pMessage; + + void initMessage(); +}; + +} +} + +#endif // _KUKA_FRI_MONITORINGMESSAGEDECODER_H diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friTransformationClient.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friTransformationClient.cpp new file mode 100644 index 00000000..70fc14f6 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friTransformationClient.cpp @@ -0,0 +1,188 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ + +#include +#include + +#include +#include + +#include +#include + +using namespace KUKA::FRI; + +//****************************************************************************** +TransformationClient::TransformationClient() +{ +} + +//****************************************************************************** +TransformationClient::~TransformationClient() +{ +} + +//****************************************************************************** +const std::vector & TransformationClient::getRequestedTransformationIDs() const +{ + unsigned int trafoCount = _data->monitoringMsg.requestedTransformations_count; + _data->requestedTrafoIDs.resize(trafoCount); + for (unsigned int i = 0; i < trafoCount; i++) { + _data->requestedTrafoIDs[i] = _data->monitoringMsg.requestedTransformations[i].name; + } + return _data->requestedTrafoIDs; +} + +//****************************************************************************** +unsigned int TransformationClient::getTimestampSec() const +{ + return _data->monitoringMsg.monitorData.timestamp.sec; +} + +//****************************************************************************** +unsigned int TransformationClient::getTimestampNanoSec() const +{ + return _data->monitoringMsg.monitorData.timestamp.nanosec; +} + +//****************************************************************************** +void TransformationClient::setTransformation( + const char * transformationID, + const double transformationMatrix[3][4], unsigned int timeSec, unsigned int timeNanoSec) +{ + _data->commandMsg.has_commandData = true; + + unsigned int currentSize = _data->commandMsg.commandData.commandedTransformations_count; + + if (currentSize < _data->MAX_REQUESTED_TRANSFORMATIONS) { + _data->commandMsg.commandData.commandedTransformations_count++; + Transformation & dest = _data->commandMsg.commandData.commandedTransformations[currentSize]; + strncpy(dest.name, transformationID, _data->MAX_SIZE_TRANSFORMATION_ID); + dest.name[_data->MAX_SIZE_TRANSFORMATION_ID - 1] = '\0'; + dest.matrix_count = 12; + memcpy(dest.matrix, transformationMatrix, 12 * sizeof(double)); + dest.has_timestamp = true; + dest.timestamp.sec = timeSec; + dest.timestamp.nanosec = timeNanoSec; + } else { + throw FRIException("Exceeded maximum number of transformations."); + } +} + +//****************************************************************************** +void TransformationClient::linkData(ClientData * clientData) +{ + _data = clientData; +} + +//****************************************************************************** +double TransformationClient::getSampleTime() const +{ + return _data->monitoringMsg.connectionInfo.sendPeriod * 0.001; +} + +//****************************************************************************** +EConnectionQuality TransformationClient::getConnectionQuality() const +{ + return (EConnectionQuality)_data->monitoringMsg.connectionInfo.quality; +} + + +//****************************************************************************** +void TransformationClient::setBooleanIOValue(const char * name, const bool value) +{ + ClientData::setBooleanIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg); +} + +//****************************************************************************** +void TransformationClient::setAnalogIOValue(const char * name, const double value) +{ + ClientData::setAnalogIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg); +} + +//****************************************************************************** +void TransformationClient::setDigitalIOValue(const char * name, const unsigned long long value) +{ + ClientData::setDigitalIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg); +} + +//****************************************************************************** +bool TransformationClient::getBooleanIOValue(const char * name) const +{ + return ClientData::getBooleanIOValue(&_data->monitoringMsg, name).digitalValue != 0; +} + +//****************************************************************************** +unsigned long long TransformationClient::getDigitalIOValue(const char * name) const +{ + return ClientData::getDigitalIOValue(&_data->monitoringMsg, name).digitalValue; +} + +//****************************************************************************** +double TransformationClient::getAnalogIOValue(const char * name) const +{ + return ClientData::getAnalogIOValue(&_data->monitoringMsg, name).analogValue; +} + +//****************************************************************************** +/*const std::vector& TransformationClient::getRequestedIO_IDs() const +{ + return _data->getRequestedIO_IDs(); +}*/ diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friUdpConnection.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friUdpConnection.cpp new file mode 100644 index 00000000..e222e273 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friUdpConnection.cpp @@ -0,0 +1,234 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#include +#include +#ifndef _MSC_VER +#include +#endif + +#include + + +#ifdef WIN32 +#include +#include +#ifdef _MSC_VER +#pragma comment(lib, "ws2_32.lib") +#endif +#endif + +using namespace KUKA::FRI; + +//****************************************************************************** +UdpConnection::UdpConnection(unsigned int receiveTimeout) +: _udpSock(-1), + _receiveTimeout(receiveTimeout) +{ +#ifdef WIN32 + WSADATA WSAData; + WSAStartup(MAKEWORD(2, 0), &WSAData); +#endif +} + +//****************************************************************************** +UdpConnection::~UdpConnection() +{ + close(); +#ifdef WIN32 + WSACleanup(); +#endif +} + +//****************************************************************************** +bool UdpConnection::open(int port, const char * controllerAddress) +{ + struct sockaddr_in servAddr; + memset(&servAddr, 0, sizeof(servAddr)); + memset(&_controllerAddr, 0, sizeof(_controllerAddr)); + + // socket creation + _udpSock = socket(PF_INET, SOCK_DGRAM, 0); + if (_udpSock < 0) { + printf("opening socket failed!\n"); + return false; + } + + // use local server port + servAddr.sin_family = AF_INET; + servAddr.sin_port = htons(port); + servAddr.sin_addr.s_addr = htonl(INADDR_ANY); + + if (bind(_udpSock, (struct sockaddr *)&servAddr, sizeof(servAddr)) < 0) { + printf("binding port number %d failed!\n", port); + close(); + return false; + } + // initialize the socket properly + _controllerAddr.sin_family = AF_INET; + _controllerAddr.sin_port = htons(port); + if (controllerAddress) { +#ifndef __MINGW32__ + inet_pton(AF_INET, controllerAddress, &_controllerAddr.sin_addr); +#else + _controllerAddr.sin_addr.s_addr = inet_addr(controllerAddress); +#endif + if (connect(_udpSock, (struct sockaddr *)&_controllerAddr, sizeof(_controllerAddr)) < 0) { + printf("connecting to controller with address %s failed !\n", controllerAddress); + close(); + return false; + } + } else { + _controllerAddr.sin_addr.s_addr = htonl(INADDR_ANY); + } + return true; +} + +//****************************************************************************** +void UdpConnection::close() +{ + if (isOpen()) { +#ifdef WIN32 + closesocket(_udpSock); +#else + ::close(_udpSock); +#endif + } + _udpSock = -1; +} + +//****************************************************************************** +bool UdpConnection::isOpen() const +{ + return _udpSock >= 0; +} + +//****************************************************************************** +int UdpConnection::receive(char * buffer, int maxSize) +{ + if (isOpen()) { + /** HAVE_SOCKLEN_T + Yes - unbelievable: There are differences in standard calling parameters (types) to recvfrom + Windows winsock, VxWorks and QNX use int + newer Posix (most Linuxes) use socklen_t + */ +#ifdef HAVE_SOCKLEN_T + socklen_t sockAddrSize; +#else + unsigned int sockAddrSize; +#endif + sockAddrSize = sizeof(struct sockaddr_in); + /** check for timeout + Because SO_RCVTIMEO is in Windows not correctly implemented, select is used for the receive time out. + If a timeout greater than 0 is given, wait until the timeout is reached or a message was received. + If t, abort the function with an error. + */ + if (_receiveTimeout > 0) { + + // Set up struct timeval + struct timeval tv; + tv.tv_sec = _receiveTimeout / 1000; + tv.tv_usec = (_receiveTimeout % 1000) * 1000; + + // initialize file descriptor + /** + * Replace FD_ZERO with memset, because bzero is not available for VxWorks + * User Space Applications(RTPs). Therefore the macro FD_ZERO does not compile. + */ +#ifndef VXWORKS + FD_ZERO(&_filedescriptor); +#else + memset((char *)(&_filedescriptor), 0, sizeof(*(&_filedescriptor))); +#endif + FD_SET(_udpSock, &_filedescriptor); + + // wait until something was received + int numberActiveFileDescr = select(_udpSock + 1, &_filedescriptor, NULL, NULL, &tv); + // 0 indicates a timeout + if (numberActiveFileDescr == 0) { + printf("The connection has timed out. Timeout is %d\n", _receiveTimeout); + return -1; + } + // a negative value indicates an error + else if (numberActiveFileDescr == -1) { + printf("An error has occurred \n"); + return -1; + } + } + + return recvfrom( + _udpSock, buffer, maxSize, 0, (struct sockaddr *)&_controllerAddr, + &sockAddrSize); + } + return -1; +} + +//****************************************************************************** +bool UdpConnection::send(const char * buffer, int size) +{ + if ((isOpen()) && (ntohs(_controllerAddr.sin_port) != 0)) { + int sent = sendto( + _udpSock, const_cast(buffer), size, 0, + (struct sockaddr *)&_controllerAddr, sizeof(_controllerAddr)); + if (sent == size) { + return true; + } + } + return false; +} diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/pb_frimessages_callbacks.c b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/pb_frimessages_callbacks.c new file mode 100644 index 00000000..9588fd97 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/pb_frimessages_callbacks.c @@ -0,0 +1,269 @@ +/** + + The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + + + \file + \version {1.15} + */ +#include +#include + +#include +#include +#include + +bool encode_repeatedDouble(pb_ostream_t *stream, const pb_field_t *field, void * const *arg) +{ + size_t i = 0; + + tRepeatedDoubleArguments* arguments = 0; + size_t count = 0; + double* values = 0; + + if (arg == NULL || *arg == NULL) + { + return false; + } + arguments = ((tRepeatedDoubleArguments*) (*arg)); + + count = arguments->max_size; + values = arguments->value; + + for (i = 0; i < count; i++) + { + + if (!pb_encode_tag_for_field(stream, field)) + { + return false; + } + + if (!pb_encode_fixed64(stream, &values[i])) + { + return false; + } + } + return true; +} + +bool decode_repeatedDouble(pb_istream_t *stream, const pb_field_t *field, void **arg) +{ + PB_UNUSED(field); + tRepeatedDoubleArguments* arguments = 0; + size_t i = 0; + double* values = 0; + + if (arg == NULL || *arg == NULL) + { + return false; + } + + arguments = (tRepeatedDoubleArguments*) *arg; + i = arguments->size; + values = arguments->value; + + if (values == NULL) + { + return true; + } + + if (!pb_decode_fixed64(stream, &values[i])) + { + return false; + } + + arguments->size++; + if (arguments->size >= arguments->max_size) + { + arguments->size = 0; + } + return true; +} + +bool encode_repeatedInt(pb_ostream_t *stream, const pb_field_t *field, void * const *arg) +{ + int i = 0; + tRepeatedIntArguments* arguments = 0; + int count = 0; + int64_t* values = 0; + + if (arg == NULL || *arg == NULL) + { + return false; + } + arguments = (tRepeatedIntArguments*) *arg; + count = arguments->max_size; + values = arguments->value; + for (i = 0; i < count; i++) + { + + if (!pb_encode_tag_for_field(stream, field)) + { + return false; + } + if (!pb_encode_varint(stream, values[i])) + { + return false; + } + } + return true; +} + +bool decode_repeatedInt(pb_istream_t *stream, const pb_field_t *field, void **arg) +{ + PB_UNUSED(field); + tRepeatedIntArguments* arguments = 0; + size_t i = 0; + uint64_t* values = 0; + + if (arg == NULL || *arg == NULL) + { + return false; + } + arguments = (tRepeatedIntArguments*) *arg; + + i = arguments->size; + values = (uint64_t*) arguments->value; + if (values == NULL) + { + return true; + } + + if (!pb_decode_varint(stream, &values[i])) + { + return false; + } + + arguments->size++; + if (arguments->size >= arguments->max_size) + { + arguments->size = 0; + } + return true; +} + +void map_repeatedDouble(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedDoubleArguments *arg) +{ + // IMPORTANT: the callbacks are stored in a union, therefore a message object + // must be exclusive defined for transmission or reception + if (dir == FRI_MANAGER_NANOPB_ENCODE) + { + values->funcs.encode = &encode_repeatedDouble; + } + else + { + values->funcs.decode = &decode_repeatedDouble; + } + // map the local container data to the message data fields + arg->max_size = numDOF; + arg->size = 0; + if (numDOF > 0) + { + arg->value = (double*) malloc(numDOF * sizeof(double)); + + } + values->arg = arg; +} + +void map_repeatedInt(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedIntArguments *arg) +{ + // IMPORTANT: the callbacks are stored in a union, therefore a message object + // must be exclusive defined for transmission or reception + if (dir == FRI_MANAGER_NANOPB_ENCODE) + { + // set the encode callback function + values->funcs.encode = &encode_repeatedInt; + } + else + { + // set the decode callback function + values->funcs.decode = &decode_repeatedInt; + } + // map the robot drive state from the container to message field + arg->max_size = numDOF; + arg->size = 0; + if (numDOF > 0) + { + arg->value = (int64_t*) malloc(numDOF * sizeof(int64_t)); + + } + values->arg = arg; +} + +void init_repeatedDouble(tRepeatedDoubleArguments *arg) +{ + arg->size = 0; + arg->max_size = 0; + arg->value = NULL; +} + +void init_repeatedInt(tRepeatedIntArguments *arg) +{ + arg->size = 0; + arg->max_size = 0; + arg->value = NULL; +} + +void free_repeatedDouble(tRepeatedDoubleArguments *arg) +{ + if (arg->value != NULL) + free(arg->value); +} + +void free_repeatedInt(tRepeatedIntArguments *arg) +{ + if (arg->value != NULL) + free(arg->value); +} diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/pb_frimessages_callbacks.h b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/pb_frimessages_callbacks.h new file mode 100644 index 00000000..98597689 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/pb_frimessages_callbacks.h @@ -0,0 +1,130 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Roboter GmbH, Augsburg, Germany. + +SCOPE + +The software "KUKA Sunrise.Connectivity FRI Client SDK" is targeted to work in +conjunction with the "KUKA Sunrise.Connectivity FastRobotInterface" toolkit. +In the following, the term "software" refers to all material directly +belonging to the provided SDK "Software development kit", particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2019 +KUKA Roboter GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.15} +*/ +#ifndef _pb_frimessages_callbacks_H +#define _pb_frimessages_callbacks_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/** container for repeated double elements */ +typedef struct repeatedDoubleArguments +{ + size_t size; + size_t max_size; + double * value; +} tRepeatedDoubleArguments; + +/** container for repeated integer elements */ +typedef struct repeatedIntArguments +{ + size_t size; + size_t max_size; + int64_t * value; +} tRepeatedIntArguments; + +/** enumeration for direction (encoding/decoding) */ +typedef enum DIRECTION +{ + FRI_MANAGER_NANOPB_DECODE = 0, //!< Argument um eine + FRI_MANAGER_NANOPB_ENCODE = 1 //!< +} eNanopbCallbackDirection; + + +bool encode_repeatedDouble( + pb_ostream_t * stream, const pb_field_t * field, + void * const * arg); + +bool decode_repeatedDouble( + pb_istream_t * stream, const pb_field_t * field, + void ** arg); + +bool encode_repeatedInt( + pb_ostream_t * stream, const pb_field_t * field, + void * const * arg); + +bool decode_repeatedInt( + pb_istream_t * stream, const pb_field_t * field, + void ** arg); + +void map_repeatedDouble( + eNanopbCallbackDirection dir, int numDOF, + pb_callback_t * values, tRepeatedDoubleArguments * arg); + +void map_repeatedInt( + eNanopbCallbackDirection dir, int numDOF, + pb_callback_t * values, tRepeatedIntArguments * arg); + +void init_repeatedDouble(tRepeatedDoubleArguments * arg); + +void init_repeatedInt(tRepeatedIntArguments * arg); + +void free_repeatedDouble(tRepeatedDoubleArguments * arg); + +void free_repeatedInt(tRepeatedIntArguments * arg); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/FRIMessages.pb.c b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/FRIMessages.pb.c new file mode 100644 index 00000000..8786d741 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/FRIMessages.pb.c @@ -0,0 +1,76 @@ +/* Automatically generated nanopb constant definitions */ +/* Generated by nanopb-0.4.8 */ + +#include "FRIMessages.pb.h" +#if PB_PROTO_HEADER_VERSION != 40 +#error Regenerate this file with the current version of nanopb generator. +#endif + +PB_BIND(JointValues, JointValues, AUTO) + + +PB_BIND(TimeStamp, TimeStamp, AUTO) + + +PB_BIND(CartesianVector, CartesianVector, AUTO) + + +PB_BIND(Checksum, Checksum, AUTO) + + +PB_BIND(QuaternionTransformation, QuaternionTransformation, AUTO) + + +PB_BIND(FriIOValue, FriIOValue, AUTO) + + +PB_BIND(RedundancyInformation, RedundancyInformation, AUTO) + + +PB_BIND(MessageHeader, MessageHeader, AUTO) + + +PB_BIND(ConnectionInfo, ConnectionInfo, AUTO) + + +PB_BIND(RobotInfo, RobotInfo, AUTO) + + +PB_BIND(MessageMonitorData, MessageMonitorData, 2) + + +PB_BIND(MessageIpoData, MessageIpoData, AUTO) + + +PB_BIND(MessageCommandData, MessageCommandData, 2) + + +PB_BIND(MessageEndOf, MessageEndOf, AUTO) + + +PB_BIND(FRIMonitoringMessage, FRIMonitoringMessage, 2) + + +PB_BIND(FRICommandMessage, FRICommandMessage, 2) + + + + + + + + + + + + + + +#ifndef PB_CONVERT_DOUBLE_FLOAT +/* On some platforms (such as AVR), double is really float. + * To be able to encode/decode double on these platforms, you need. + * to define PB_CONVERT_DOUBLE_FLOAT in pb.h or compiler command line. + */ +PB_STATIC_ASSERT(sizeof(double) == 8, DOUBLE_MUST_BE_8_BYTES) +#endif + diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/FRIMessages.pb.h b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/FRIMessages.pb.h new file mode 100644 index 00000000..d124ca26 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/FRIMessages.pb.h @@ -0,0 +1,680 @@ +/* Automatically generated nanopb header */ +/* Generated by nanopb-0.4.8 */ + +#ifndef PB_FRIMESSAGES_PB_H_INCLUDED +#define PB_FRIMESSAGES_PB_H_INCLUDED +#include + +#if PB_PROTO_HEADER_VERSION != 40 +#error Regenerate this file with the current version of nanopb generator. +#endif + +/* Enum definitions */ +/* used to display the state of the FRI session + @KUKA.Internal */ +typedef enum _FRISessionState { + FRISessionState_IDLE = 0, /* idle state. */ + FRISessionState_MONITORING_WAIT = 1, /* connected in monitormode - transmitted commands are used to determine the connection quality. */ + FRISessionState_MONITORING_READY = 2, /* connected in monitormode - transmitted commands are used to determine the connection quality. */ + FRISessionState_COMMANDING_WAIT = 3, /* connected to a queued motion - motion not active */ + FRISessionState_COMMANDING_ACTIVE = 4 /* connected to a running motion - commanding modifies the robot path */ +} FRISessionState; + +/* Quality of the FRI Connection + @KUKA.Internal */ +typedef enum _FRIConnectionQuality { + FRIConnectionQuality_POOR = 0, /* Robot commanding is not possible. Initial value of the connection. */ + FRIConnectionQuality_FAIR = 1, /* Robot commanding is not possible. */ + FRIConnectionQuality_GOOD = 2, /* Robot commanding is possible. */ + FRIConnectionQuality_EXCELLENT = 3 /* Robot commanding is possible. */ +} FRIConnectionQuality; + +/* This enumeration is used to indicate the safety State. + @KUKA.Internal */ +typedef enum _SafetyState { + SafetyState_NORMAL_OPERATION = 0, + SafetyState_SAFETY_STOP_LEVEL_0 = 1, + SafetyState_SAFETY_STOP_LEVEL_1 = 2, + SafetyState_SAFETY_STOP_LEVEL_2 = 3 +} SafetyState; + +/* This enumeration contains the available operation modes of the controller. + @KUKA.Internal */ +typedef enum _OperationMode { + OperationMode_TEST_MODE_1 = 0, + OperationMode_TEST_MODE_2 = 1, + OperationMode_AUTOMATIC_MODE = 2 +} OperationMode; + +/* @KUKA.Internal */ +typedef enum _DriveState { + DriveState_OFF = 0, + DriveState_TRANSITIONING = 1, + DriveState_ACTIVE = 2 +} DriveState; + +/* This enumeration contains the available control modes of a KUKA robot. + @KUKA.Internal */ +typedef enum _ControlMode { + ControlMode_POSITION_CONTROLMODE = 0, + ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE = 1, + ControlMode_JOINT_IMPEDANCE_CONTROLMODE = 2, + ControlMode_NO_CONTROLMODE = 3 +} ControlMode; + +/* This enumeration contains the available client command modes of the interpolator. + @KUKA.Internal */ +typedef enum _ClientCommandMode { + ClientCommandMode_NO_COMMAND_MODE = 0, + ClientCommandMode_JOINT_POSITION = 1, + ClientCommandMode_WRENCH = 2, + ClientCommandMode_TORQUE = 3, + ClientCommandMode_CARTESIAN_POSE = 4 +} ClientCommandMode; + +/* This enumeration contains the currently used overlay type. + @KUKA.Internal */ +typedef enum _OverlayType { + OverlayType_NO_OVERLAY = 0, + OverlayType_JOINT = 1, + OverlayType_CARTESIAN = 2 +} OverlayType; + +/* IO Type + @KUKA.Internal */ +typedef enum _FriIOType { + FriIOType_BOOLEAN = 0, + FriIOType_DIGITAL = 1, + FriIOType_ANALOG = 2 +} FriIOType; + +/* IO Direction + @KUKA.Internal */ +typedef enum _FriIODirection { + FriIODirection_INPUT = 0, + FriIODirection_OUTPUT = 1 +} FriIODirection; + +/* Redundancy strategy + @KUKA.Internal */ +typedef enum _RedundancyStrategy { + RedundancyStrategy_E1 = 0, + RedundancyStrategy_NONE = 4 +} RedundancyStrategy; + +/* Struct definitions */ +/* Container object for Joint Values + @KUKA.Internal */ +typedef struct _JointValues { + pb_callback_t value; /* value of a single joint. */ +} JointValues; + +/* Timestamp container. + @KUKA.Internal */ +typedef struct _TimeStamp { + uint32_t sec; /* Second portion of the timestamp. Time starts at 1.1.1970. */ + uint32_t nanosec; /* Nano second portion of the timestamp. Time starts at 1.1.1970. */ +} TimeStamp; + +/* Cartesian vector container. + KUKA.Internal */ +typedef struct _CartesianVector { + pb_size_t element_count; + double element[6]; /* value of a single vector element */ +} CartesianVector; + +/* Contains possible checksum implementations like CRC32,MD5. + @KUKA.Internal */ +typedef struct _Checksum { + bool has_crc32; + int32_t crc32; /* Storage for CRC32. */ +} Checksum; + +/* Quaternion transformation container. + @KUKA.Internal */ +typedef struct _QuaternionTransformation { + char name[64]; /* switch to 'required' due to nanopb-bug */ + pb_size_t element_count; + double element[7]; + bool has_timestamp; + TimeStamp timestamp; +} QuaternionTransformation; + +/* Request output on value from Fieldbus with Group.Name */ +typedef struct _FriIOValue { + char name[64]; + FriIOType type; + FriIODirection direction; + bool has_digitalValue; + uint64_t digitalValue; + bool has_analogValue; + double analogValue; +} FriIOValue; + +typedef struct _RedundancyInformation { + RedundancyStrategy strategy; + double value; +} RedundancyInformation; + +/* FRI Message Header. Contains the information for timing handshake and the message identifier. + The following messageIdentifiers are currently available: + LBR Monitoring Message: 0x245142 + LBR Command Message: 0x34001 + @KUKA.Internal */ +typedef struct _MessageHeader { + uint32_t messageIdentifier; /* Message identifier. */ + uint32_t sequenceCounter; /* Sequence counter. Checked to determine the timing. */ + uint32_t reflectedSequenceCounter; /* Reflected sequence counter. Checked to determine the timing. */ +} MessageHeader; + +/* FRI Connection info. Contains the connection state and additional informations. + @KUKA.Internal */ +typedef struct _ConnectionInfo { + FRISessionState sessionState; /* state of the FRI session. */ + FRIConnectionQuality quality; /* Quality of the FRI Connection. */ + bool has_sendPeriod; + uint32_t sendPeriod; /* Timing in Milliseconds, on which the Controller sends a new Message. */ + bool has_receiveMultiplier; + uint32_t receiveMultiplier; /* Multiplier of sendPeriod, on which the Controller expects a new CommmandMessage. */ +} ConnectionInfo; + +/* Robot Information Object. Contains all static Information about the robot. e.g. + Number of Joints. + @KUKA.Internal */ +typedef struct _RobotInfo { + bool has_numberOfJoints; + int32_t numberOfJoints; /* availabe number of joints. */ + bool has_safetyState; + SafetyState safetyState; /* Safety state of the controller. */ + pb_callback_t driveState; /* Drivestate of the drives. */ + bool has_operationMode; + OperationMode operationMode; /* OperationMode of the Controller. */ + bool has_controlMode; + ControlMode controlMode; /* Controlmode of the robot. */ +} RobotInfo; + +/* FRI Monitor Data. Contains the cylic Information about the current robot state. + @KUKA.Internal */ +typedef struct _MessageMonitorData { + bool has_measuredJointPosition; + JointValues measuredJointPosition; /* measured joint values. */ + bool has_measuredTorque; + JointValues measuredTorque; /* measured torque values. */ + bool has_commandedTorque; + JointValues commandedTorque; /* last commanded torque value. */ + bool has_externalTorque; + JointValues externalTorque; /* observed external torque. */ + /* optional CartesianVector externalForce = 6; // observed Cartesian external forces and torque in flange coordinates + repeated QuaternionTransformation subscribedTransformations = 7; // selected transformations from controller to client */ + pb_size_t readIORequest_count; + FriIOValue readIORequest[10]; /* used to read FieldBus input value(s) */ + bool has_measuredCartesianPose; + QuaternionTransformation measuredCartesianPose; /* measured Cartesian pose. */ + bool has_measuredRedundancyInformation; + RedundancyInformation measuredRedundancyInformation; /* measured redundancy information */ + bool has_timestamp; + TimeStamp timestamp; /* timestamp of the measurement. */ +} MessageMonitorData; + +/* FRI Interpolator Data. Contains the cyclic commands which are going to be send + to the robot by the interpolator. + @KUKA.Internal */ +typedef struct _MessageIpoData { + bool has_jointPosition; + JointValues jointPosition; /* current joint values of interpolator. */ + bool has_cartesianPose; + QuaternionTransformation cartesianPose; /* Cartesian pose that is commanded to the robot pipe in the current time step */ + bool has_redundancyInformation; + RedundancyInformation redundancyInformation; /* current redundancy information of interpolator */ + bool has_clientCommandMode; + ClientCommandMode clientCommandMode; /* current command mode of the interpolator. */ + bool has_overlayType; + OverlayType overlayType; /* current overlay type. */ + bool has_trackingPerformance; + double trackingPerformance; /* tracking performance of the commands */ +} MessageIpoData; + +/* FRI Command Data. Contains the cyclic commands to modify the robot position. + @KUKA.Internal */ +typedef struct _MessageCommandData { + bool has_jointPosition; + JointValues jointPosition; /* commanded joint Position. */ + bool has_cartesianWrenchFeedForward; + CartesianVector cartesianWrenchFeedForward; /* cartesian wrench feed forward [N,N,N,Nm,Nm,Nm] */ + bool has_jointTorque; + JointValues jointTorque; /* commanded joint torques [Nm, ..., Nm]. */ + pb_size_t commandedTransformations_count; + QuaternionTransformation commandedTransformations[5]; /* commanded transformations from the client as requested by the robot controller */ + pb_size_t writeIORequest_count; + FriIOValue writeIORequest[10]; /* used to write FieldBus output value(s) */ + bool has_cartesianPose; + QuaternionTransformation cartesianPose; /* commanded Cartesian pose. */ + bool has_redundancyInformation; + RedundancyInformation redundancyInformation; /* commanded redundancy information */ +} MessageCommandData; + +/* FRI End of Data. Contains the length and CRC32 of the data before. + @KUKA.Internal */ +typedef struct _MessageEndOf { + bool has_messageLength; + int32_t messageLength; /* Length of the header + Controller data. */ + bool has_messageChecksum; + Checksum messageChecksum; /* checksum for all data before this point muss be last date. */ +} MessageEndOf; + +/* FRI Monitoring Message. Contains the cyclic Information of the robot position and state. + @KUKA.Internal */ +typedef struct _FRIMonitoringMessage { + MessageHeader header; /* Message header. */ + bool has_robotInfo; + RobotInfo robotInfo; /* Robot Information. */ + bool has_monitorData; + MessageMonitorData monitorData; /* data collected during Monitoring. */ + bool has_connectionInfo; + ConnectionInfo connectionInfo; /* Connection Information . */ + bool has_ipoData; + MessageIpoData ipoData; /* output from interpolator */ + pb_size_t requestedTransformations_count; + QuaternionTransformation requestedTransformations[5]; /* transformations requested by the robot controller from the client */ + bool has_endOfMessageData; + MessageEndOf endOfMessageData; /* End of Package contains message length and checksum. */ +} FRIMonitoringMessage; + +/* FRI Command Message. Contains the information of the user to modify the robot commands. + @KUKA.Internal */ +typedef struct _FRICommandMessage { + MessageHeader header; /* Message header. */ + bool has_commandData; + MessageCommandData commandData; /* Command Data. */ + bool has_endOfMessageData; + MessageEndOf endOfMessageData; /* End of Package contains message length and checksum. */ +} FRICommandMessage; + + +#ifdef __cplusplus +extern "C" { +#endif + +/* Helper constants for enums */ +#define _FRISessionState_MIN FRISessionState_IDLE +#define _FRISessionState_MAX FRISessionState_COMMANDING_ACTIVE +#define _FRISessionState_ARRAYSIZE ((FRISessionState)(FRISessionState_COMMANDING_ACTIVE+1)) + +#define _FRIConnectionQuality_MIN FRIConnectionQuality_POOR +#define _FRIConnectionQuality_MAX FRIConnectionQuality_EXCELLENT +#define _FRIConnectionQuality_ARRAYSIZE ((FRIConnectionQuality)(FRIConnectionQuality_EXCELLENT+1)) + +#define _SafetyState_MIN SafetyState_NORMAL_OPERATION +#define _SafetyState_MAX SafetyState_SAFETY_STOP_LEVEL_2 +#define _SafetyState_ARRAYSIZE ((SafetyState)(SafetyState_SAFETY_STOP_LEVEL_2+1)) + +#define _OperationMode_MIN OperationMode_TEST_MODE_1 +#define _OperationMode_MAX OperationMode_AUTOMATIC_MODE +#define _OperationMode_ARRAYSIZE ((OperationMode)(OperationMode_AUTOMATIC_MODE+1)) + +#define _DriveState_MIN DriveState_OFF +#define _DriveState_MAX DriveState_ACTIVE +#define _DriveState_ARRAYSIZE ((DriveState)(DriveState_ACTIVE+1)) + +#define _ControlMode_MIN ControlMode_POSITION_CONTROLMODE +#define _ControlMode_MAX ControlMode_NO_CONTROLMODE +#define _ControlMode_ARRAYSIZE ((ControlMode)(ControlMode_NO_CONTROLMODE+1)) + +#define _ClientCommandMode_MIN ClientCommandMode_NO_COMMAND_MODE +#define _ClientCommandMode_MAX ClientCommandMode_CARTESIAN_POSE +#define _ClientCommandMode_ARRAYSIZE ((ClientCommandMode)(ClientCommandMode_CARTESIAN_POSE+1)) + +#define _OverlayType_MIN OverlayType_NO_OVERLAY +#define _OverlayType_MAX OverlayType_CARTESIAN +#define _OverlayType_ARRAYSIZE ((OverlayType)(OverlayType_CARTESIAN+1)) + +#define _FriIOType_MIN FriIOType_BOOLEAN +#define _FriIOType_MAX FriIOType_ANALOG +#define _FriIOType_ARRAYSIZE ((FriIOType)(FriIOType_ANALOG+1)) + +#define _FriIODirection_MIN FriIODirection_INPUT +#define _FriIODirection_MAX FriIODirection_OUTPUT +#define _FriIODirection_ARRAYSIZE ((FriIODirection)(FriIODirection_OUTPUT+1)) + +#define _RedundancyStrategy_MIN RedundancyStrategy_E1 +#define _RedundancyStrategy_MAX RedundancyStrategy_NONE +#define _RedundancyStrategy_ARRAYSIZE ((RedundancyStrategy)(RedundancyStrategy_NONE+1)) + + + + + + +#define FriIOValue_type_ENUMTYPE FriIOType +#define FriIOValue_direction_ENUMTYPE FriIODirection + +#define RedundancyInformation_strategy_ENUMTYPE RedundancyStrategy + + +#define ConnectionInfo_sessionState_ENUMTYPE FRISessionState +#define ConnectionInfo_quality_ENUMTYPE FRIConnectionQuality + +#define RobotInfo_safetyState_ENUMTYPE SafetyState +#define RobotInfo_driveState_ENUMTYPE DriveState +#define RobotInfo_operationMode_ENUMTYPE OperationMode +#define RobotInfo_controlMode_ENUMTYPE ControlMode + + +#define MessageIpoData_clientCommandMode_ENUMTYPE ClientCommandMode +#define MessageIpoData_overlayType_ENUMTYPE OverlayType + + + + + + +/* Initializer values for message structs */ +#define JointValues_init_default {{{NULL}, NULL}} +#define TimeStamp_init_default {0, 0} +#define CartesianVector_init_default {0, {0, 0, 0, 0, 0, 0}} +#define Checksum_init_default {false, 0} +#define QuaternionTransformation_init_default {"", 0, {0, 0, 0, 0, 0, 0, 0}, false, TimeStamp_init_default} +#define FriIOValue_init_default {"", _FriIOType_MIN, _FriIODirection_MIN, false, 0, false, 0} +#define RedundancyInformation_init_default {_RedundancyStrategy_MIN, 0} +#define MessageHeader_init_default {0, 0, 0} +#define ConnectionInfo_init_default {_FRISessionState_MIN, _FRIConnectionQuality_MIN, false, 0, false, 0} +#define RobotInfo_init_default {false, 0, false, _SafetyState_MIN, {{NULL}, NULL}, false, _OperationMode_MIN, false, _ControlMode_MIN} +#define MessageMonitorData_init_default {false, JointValues_init_default, false, JointValues_init_default, false, JointValues_init_default, false, JointValues_init_default, 0, {FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default}, false, QuaternionTransformation_init_default, false, RedundancyInformation_init_default, false, TimeStamp_init_default} +#define MessageIpoData_init_default {false, JointValues_init_default, false, QuaternionTransformation_init_default, false, RedundancyInformation_init_default, false, _ClientCommandMode_MIN, false, _OverlayType_MIN, false, 0} +#define MessageCommandData_init_default {false, JointValues_init_default, false, CartesianVector_init_default, false, JointValues_init_default, 0, {QuaternionTransformation_init_default, QuaternionTransformation_init_default, QuaternionTransformation_init_default, QuaternionTransformation_init_default, QuaternionTransformation_init_default}, 0, {FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default, FriIOValue_init_default}, false, QuaternionTransformation_init_default, false, RedundancyInformation_init_default} +#define MessageEndOf_init_default {false, 0, false, Checksum_init_default} +#define FRIMonitoringMessage_init_default {MessageHeader_init_default, false, RobotInfo_init_default, false, MessageMonitorData_init_default, false, ConnectionInfo_init_default, false, MessageIpoData_init_default, 0, {QuaternionTransformation_init_default, QuaternionTransformation_init_default, QuaternionTransformation_init_default, QuaternionTransformation_init_default, QuaternionTransformation_init_default}, false, MessageEndOf_init_default} +#define FRICommandMessage_init_default {MessageHeader_init_default, false, MessageCommandData_init_default, false, MessageEndOf_init_default} +#define JointValues_init_zero {{{NULL}, NULL}} +#define TimeStamp_init_zero {0, 0} +#define CartesianVector_init_zero {0, {0, 0, 0, 0, 0, 0}} +#define Checksum_init_zero {false, 0} +#define QuaternionTransformation_init_zero {"", 0, {0, 0, 0, 0, 0, 0, 0}, false, TimeStamp_init_zero} +#define FriIOValue_init_zero {"", _FriIOType_MIN, _FriIODirection_MIN, false, 0, false, 0} +#define RedundancyInformation_init_zero {_RedundancyStrategy_MIN, 0} +#define MessageHeader_init_zero {0, 0, 0} +#define ConnectionInfo_init_zero {_FRISessionState_MIN, _FRIConnectionQuality_MIN, false, 0, false, 0} +#define RobotInfo_init_zero {false, 0, false, _SafetyState_MIN, {{NULL}, NULL}, false, _OperationMode_MIN, false, _ControlMode_MIN} +#define MessageMonitorData_init_zero {false, JointValues_init_zero, false, JointValues_init_zero, false, JointValues_init_zero, false, JointValues_init_zero, 0, {FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero}, false, QuaternionTransformation_init_zero, false, RedundancyInformation_init_zero, false, TimeStamp_init_zero} +#define MessageIpoData_init_zero {false, JointValues_init_zero, false, QuaternionTransformation_init_zero, false, RedundancyInformation_init_zero, false, _ClientCommandMode_MIN, false, _OverlayType_MIN, false, 0} +#define MessageCommandData_init_zero {false, JointValues_init_zero, false, CartesianVector_init_zero, false, JointValues_init_zero, 0, {QuaternionTransformation_init_zero, QuaternionTransformation_init_zero, QuaternionTransformation_init_zero, QuaternionTransformation_init_zero, QuaternionTransformation_init_zero}, 0, {FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero, FriIOValue_init_zero}, false, QuaternionTransformation_init_zero, false, RedundancyInformation_init_zero} +#define MessageEndOf_init_zero {false, 0, false, Checksum_init_zero} +#define FRIMonitoringMessage_init_zero {MessageHeader_init_zero, false, RobotInfo_init_zero, false, MessageMonitorData_init_zero, false, ConnectionInfo_init_zero, false, MessageIpoData_init_zero, 0, {QuaternionTransformation_init_zero, QuaternionTransformation_init_zero, QuaternionTransformation_init_zero, QuaternionTransformation_init_zero, QuaternionTransformation_init_zero}, false, MessageEndOf_init_zero} +#define FRICommandMessage_init_zero {MessageHeader_init_zero, false, MessageCommandData_init_zero, false, MessageEndOf_init_zero} + +/* Field tags (for use in manual encoding/decoding) */ +#define JointValues_value_tag 1 +#define TimeStamp_sec_tag 1 +#define TimeStamp_nanosec_tag 2 +#define CartesianVector_element_tag 1 +#define Checksum_crc32_tag 1 +#define QuaternionTransformation_name_tag 1 +#define QuaternionTransformation_element_tag 2 +#define QuaternionTransformation_timestamp_tag 3 +#define FriIOValue_name_tag 1 +#define FriIOValue_type_tag 2 +#define FriIOValue_direction_tag 3 +#define FriIOValue_digitalValue_tag 4 +#define FriIOValue_analogValue_tag 5 +#define RedundancyInformation_strategy_tag 1 +#define RedundancyInformation_value_tag 2 +#define MessageHeader_messageIdentifier_tag 1 +#define MessageHeader_sequenceCounter_tag 2 +#define MessageHeader_reflectedSequenceCounter_tag 3 +#define ConnectionInfo_sessionState_tag 1 +#define ConnectionInfo_quality_tag 2 +#define ConnectionInfo_sendPeriod_tag 3 +#define ConnectionInfo_receiveMultiplier_tag 4 +#define RobotInfo_numberOfJoints_tag 1 +#define RobotInfo_safetyState_tag 2 +#define RobotInfo_driveState_tag 5 +#define RobotInfo_operationMode_tag 6 +#define RobotInfo_controlMode_tag 7 +#define MessageMonitorData_measuredJointPosition_tag 1 +#define MessageMonitorData_measuredTorque_tag 2 +#define MessageMonitorData_commandedTorque_tag 4 +#define MessageMonitorData_externalTorque_tag 5 +#define MessageMonitorData_readIORequest_tag 8 +#define MessageMonitorData_measuredCartesianPose_tag 9 +#define MessageMonitorData_measuredRedundancyInformation_tag 10 +#define MessageMonitorData_timestamp_tag 15 +#define MessageIpoData_jointPosition_tag 1 +#define MessageIpoData_cartesianPose_tag 2 +#define MessageIpoData_redundancyInformation_tag 3 +#define MessageIpoData_clientCommandMode_tag 10 +#define MessageIpoData_overlayType_tag 11 +#define MessageIpoData_trackingPerformance_tag 12 +#define MessageCommandData_jointPosition_tag 1 +#define MessageCommandData_cartesianWrenchFeedForward_tag 2 +#define MessageCommandData_jointTorque_tag 3 +#define MessageCommandData_commandedTransformations_tag 4 +#define MessageCommandData_writeIORequest_tag 5 +#define MessageCommandData_cartesianPose_tag 6 +#define MessageCommandData_redundancyInformation_tag 7 +#define MessageEndOf_messageLength_tag 1 +#define MessageEndOf_messageChecksum_tag 2 +#define FRIMonitoringMessage_header_tag 1 +#define FRIMonitoringMessage_robotInfo_tag 2 +#define FRIMonitoringMessage_monitorData_tag 3 +#define FRIMonitoringMessage_connectionInfo_tag 4 +#define FRIMonitoringMessage_ipoData_tag 5 +#define FRIMonitoringMessage_requestedTransformations_tag 6 +#define FRIMonitoringMessage_endOfMessageData_tag 15 +#define FRICommandMessage_header_tag 1 +#define FRICommandMessage_commandData_tag 2 +#define FRICommandMessage_endOfMessageData_tag 15 + +/* Struct field encoding specification for nanopb */ +#define JointValues_FIELDLIST(X, a) \ +X(a, CALLBACK, REPEATED, DOUBLE, value, 1) +#define JointValues_CALLBACK pb_default_field_callback +#define JointValues_DEFAULT NULL + +#define TimeStamp_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, UINT32, sec, 1) \ +X(a, STATIC, REQUIRED, UINT32, nanosec, 2) +#define TimeStamp_CALLBACK NULL +#define TimeStamp_DEFAULT NULL + +#define CartesianVector_FIELDLIST(X, a) \ +X(a, STATIC, REPEATED, DOUBLE, element, 1) +#define CartesianVector_CALLBACK NULL +#define CartesianVector_DEFAULT NULL + +#define Checksum_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, INT32, crc32, 1) +#define Checksum_CALLBACK NULL +#define Checksum_DEFAULT NULL + +#define QuaternionTransformation_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, STRING, name, 1) \ +X(a, STATIC, REPEATED, DOUBLE, element, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, timestamp, 3) +#define QuaternionTransformation_CALLBACK NULL +#define QuaternionTransformation_DEFAULT NULL +#define QuaternionTransformation_timestamp_MSGTYPE TimeStamp + +#define FriIOValue_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, STRING, name, 1) \ +X(a, STATIC, REQUIRED, UENUM, type, 2) \ +X(a, STATIC, REQUIRED, UENUM, direction, 3) \ +X(a, STATIC, OPTIONAL, UINT64, digitalValue, 4) \ +X(a, STATIC, OPTIONAL, DOUBLE, analogValue, 5) +#define FriIOValue_CALLBACK NULL +#define FriIOValue_DEFAULT NULL + +#define RedundancyInformation_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, UENUM, strategy, 1) \ +X(a, STATIC, REQUIRED, DOUBLE, value, 2) +#define RedundancyInformation_CALLBACK NULL +#define RedundancyInformation_DEFAULT NULL + +#define MessageHeader_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, UINT32, messageIdentifier, 1) \ +X(a, STATIC, REQUIRED, UINT32, sequenceCounter, 2) \ +X(a, STATIC, REQUIRED, UINT32, reflectedSequenceCounter, 3) +#define MessageHeader_CALLBACK NULL +#define MessageHeader_DEFAULT NULL + +#define ConnectionInfo_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, UENUM, sessionState, 1) \ +X(a, STATIC, REQUIRED, UENUM, quality, 2) \ +X(a, STATIC, OPTIONAL, UINT32, sendPeriod, 3) \ +X(a, STATIC, OPTIONAL, UINT32, receiveMultiplier, 4) +#define ConnectionInfo_CALLBACK NULL +#define ConnectionInfo_DEFAULT NULL + +#define RobotInfo_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, INT32, numberOfJoints, 1) \ +X(a, STATIC, OPTIONAL, UENUM, safetyState, 2) \ +X(a, CALLBACK, REPEATED, UENUM, driveState, 5) \ +X(a, STATIC, OPTIONAL, UENUM, operationMode, 6) \ +X(a, STATIC, OPTIONAL, UENUM, controlMode, 7) +#define RobotInfo_CALLBACK pb_default_field_callback +#define RobotInfo_DEFAULT NULL + +#define MessageMonitorData_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, MESSAGE, measuredJointPosition, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, measuredTorque, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, commandedTorque, 4) \ +X(a, STATIC, OPTIONAL, MESSAGE, externalTorque, 5) \ +X(a, STATIC, REPEATED, MESSAGE, readIORequest, 8) \ +X(a, STATIC, OPTIONAL, MESSAGE, measuredCartesianPose, 9) \ +X(a, STATIC, OPTIONAL, MESSAGE, measuredRedundancyInformation, 10) \ +X(a, STATIC, OPTIONAL, MESSAGE, timestamp, 15) +#define MessageMonitorData_CALLBACK NULL +#define MessageMonitorData_DEFAULT NULL +#define MessageMonitorData_measuredJointPosition_MSGTYPE JointValues +#define MessageMonitorData_measuredTorque_MSGTYPE JointValues +#define MessageMonitorData_commandedTorque_MSGTYPE JointValues +#define MessageMonitorData_externalTorque_MSGTYPE JointValues +#define MessageMonitorData_readIORequest_MSGTYPE FriIOValue +#define MessageMonitorData_measuredCartesianPose_MSGTYPE QuaternionTransformation +#define MessageMonitorData_measuredRedundancyInformation_MSGTYPE RedundancyInformation +#define MessageMonitorData_timestamp_MSGTYPE TimeStamp + +#define MessageIpoData_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, MESSAGE, jointPosition, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, cartesianPose, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, redundancyInformation, 3) \ +X(a, STATIC, OPTIONAL, UENUM, clientCommandMode, 10) \ +X(a, STATIC, OPTIONAL, UENUM, overlayType, 11) \ +X(a, STATIC, OPTIONAL, DOUBLE, trackingPerformance, 12) +#define MessageIpoData_CALLBACK NULL +#define MessageIpoData_DEFAULT NULL +#define MessageIpoData_jointPosition_MSGTYPE JointValues +#define MessageIpoData_cartesianPose_MSGTYPE QuaternionTransformation +#define MessageIpoData_redundancyInformation_MSGTYPE RedundancyInformation + +#define MessageCommandData_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, MESSAGE, jointPosition, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, cartesianWrenchFeedForward, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, jointTorque, 3) \ +X(a, STATIC, REPEATED, MESSAGE, commandedTransformations, 4) \ +X(a, STATIC, REPEATED, MESSAGE, writeIORequest, 5) \ +X(a, STATIC, OPTIONAL, MESSAGE, cartesianPose, 6) \ +X(a, STATIC, OPTIONAL, MESSAGE, redundancyInformation, 7) +#define MessageCommandData_CALLBACK NULL +#define MessageCommandData_DEFAULT NULL +#define MessageCommandData_jointPosition_MSGTYPE JointValues +#define MessageCommandData_cartesianWrenchFeedForward_MSGTYPE CartesianVector +#define MessageCommandData_jointTorque_MSGTYPE JointValues +#define MessageCommandData_commandedTransformations_MSGTYPE QuaternionTransformation +#define MessageCommandData_writeIORequest_MSGTYPE FriIOValue +#define MessageCommandData_cartesianPose_MSGTYPE QuaternionTransformation +#define MessageCommandData_redundancyInformation_MSGTYPE RedundancyInformation + +#define MessageEndOf_FIELDLIST(X, a) \ +X(a, STATIC, OPTIONAL, INT32, messageLength, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, messageChecksum, 2) +#define MessageEndOf_CALLBACK NULL +#define MessageEndOf_DEFAULT NULL +#define MessageEndOf_messageChecksum_MSGTYPE Checksum + +#define FRIMonitoringMessage_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, MESSAGE, header, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, robotInfo, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, monitorData, 3) \ +X(a, STATIC, OPTIONAL, MESSAGE, connectionInfo, 4) \ +X(a, STATIC, OPTIONAL, MESSAGE, ipoData, 5) \ +X(a, STATIC, REPEATED, MESSAGE, requestedTransformations, 6) \ +X(a, STATIC, OPTIONAL, MESSAGE, endOfMessageData, 15) +#define FRIMonitoringMessage_CALLBACK NULL +#define FRIMonitoringMessage_DEFAULT NULL +#define FRIMonitoringMessage_header_MSGTYPE MessageHeader +#define FRIMonitoringMessage_robotInfo_MSGTYPE RobotInfo +#define FRIMonitoringMessage_monitorData_MSGTYPE MessageMonitorData +#define FRIMonitoringMessage_connectionInfo_MSGTYPE ConnectionInfo +#define FRIMonitoringMessage_ipoData_MSGTYPE MessageIpoData +#define FRIMonitoringMessage_requestedTransformations_MSGTYPE QuaternionTransformation +#define FRIMonitoringMessage_endOfMessageData_MSGTYPE MessageEndOf + +#define FRICommandMessage_FIELDLIST(X, a) \ +X(a, STATIC, REQUIRED, MESSAGE, header, 1) \ +X(a, STATIC, OPTIONAL, MESSAGE, commandData, 2) \ +X(a, STATIC, OPTIONAL, MESSAGE, endOfMessageData, 15) +#define FRICommandMessage_CALLBACK NULL +#define FRICommandMessage_DEFAULT NULL +#define FRICommandMessage_header_MSGTYPE MessageHeader +#define FRICommandMessage_commandData_MSGTYPE MessageCommandData +#define FRICommandMessage_endOfMessageData_MSGTYPE MessageEndOf + +extern const pb_msgdesc_t JointValues_msg; +extern const pb_msgdesc_t TimeStamp_msg; +extern const pb_msgdesc_t CartesianVector_msg; +extern const pb_msgdesc_t Checksum_msg; +extern const pb_msgdesc_t QuaternionTransformation_msg; +extern const pb_msgdesc_t FriIOValue_msg; +extern const pb_msgdesc_t RedundancyInformation_msg; +extern const pb_msgdesc_t MessageHeader_msg; +extern const pb_msgdesc_t ConnectionInfo_msg; +extern const pb_msgdesc_t RobotInfo_msg; +extern const pb_msgdesc_t MessageMonitorData_msg; +extern const pb_msgdesc_t MessageIpoData_msg; +extern const pb_msgdesc_t MessageCommandData_msg; +extern const pb_msgdesc_t MessageEndOf_msg; +extern const pb_msgdesc_t FRIMonitoringMessage_msg; +extern const pb_msgdesc_t FRICommandMessage_msg; + +/* Defines for backwards compatibility with code written before nanopb-0.4.0 */ +#define JointValues_fields &JointValues_msg +#define TimeStamp_fields &TimeStamp_msg +#define CartesianVector_fields &CartesianVector_msg +#define Checksum_fields &Checksum_msg +#define QuaternionTransformation_fields &QuaternionTransformation_msg +#define FriIOValue_fields &FriIOValue_msg +#define RedundancyInformation_fields &RedundancyInformation_msg +#define MessageHeader_fields &MessageHeader_msg +#define ConnectionInfo_fields &ConnectionInfo_msg +#define RobotInfo_fields &RobotInfo_msg +#define MessageMonitorData_fields &MessageMonitorData_msg +#define MessageIpoData_fields &MessageIpoData_msg +#define MessageCommandData_fields &MessageCommandData_msg +#define MessageEndOf_fields &MessageEndOf_msg +#define FRIMonitoringMessage_fields &FRIMonitoringMessage_msg +#define FRICommandMessage_fields &FRICommandMessage_msg + +/* Maximum encoded size of messages (where known) */ +/* JointValues_size depends on runtime parameters */ +/* RobotInfo_size depends on runtime parameters */ +/* MessageMonitorData_size depends on runtime parameters */ +/* MessageIpoData_size depends on runtime parameters */ +/* MessageCommandData_size depends on runtime parameters */ +/* FRIMonitoringMessage_size depends on runtime parameters */ +/* FRICommandMessage_size depends on runtime parameters */ +#define CartesianVector_size 54 +#define Checksum_size 11 +#define ConnectionInfo_size 16 +#define FRIMESSAGES_PB_H_MAX_SIZE QuaternionTransformation_size +#define FriIOValue_size 89 +#define MessageEndOf_size 24 +#define MessageHeader_size 18 +#define QuaternionTransformation_size 142 +#define RedundancyInformation_size 11 +#define TimeStamp_size 12 + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/HWIFClientApplication.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/HWIFClientApplication.cpp new file mode 100644 index 00000000..249ab8d3 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/HWIFClientApplication.cpp @@ -0,0 +1,119 @@ +#include +#include + + +namespace KUKA +{ +namespace FRI +{ + +HWIFClientApplication::HWIFClientApplication(IConnection & connection, IClient & client) +: ClientApplication(connection, client) {} + +bool HWIFClientApplication::client_app_read() +{ + if (!_connection.isOpen()) { + std::cout << "Error: client application is not connected!" << std::endl; + return false; + } + + // ************************************************************************** + // Receive and decode new monitoring message + // ************************************************************************** + size_ = _connection.receive(_data->receiveBuffer, FRI_MONITOR_MSG_MAX_SIZE); + + if (size_ <= 0) { // TODO: size_ == 0 -> connection closed (maybe go to IDLE instead of stopping?) + std::cout << "Error: failed while trying to receive monitoring message!" << std::endl; + return false; + } + + if (!_data->decoder.decode(_data->receiveBuffer, size_)) { + std::cout << "Error: failed to decode message" << std::endl; + return false; + } + + // check message type (so that our wrappers match) + if (_data->expectedMonitorMsgID != _data->monitoringMsg.header.messageIdentifier) { + std::cout << "Error: incompatible IDs for received message, got: " << + _data->monitoringMsg.header.messageIdentifier << " expected: " << + _data->expectedMonitorMsgID << std::endl; + return false; + } + + return true; +} + +void HWIFClientApplication::client_app_update() +{ + // ************************************************************************** + // callbacks + // ************************************************************************** + // reset command message before callbacks + _data->resetCommandMessage(); + + // callbacks for robot client + ESessionState currentState = (ESessionState)_data->monitoringMsg.connectionInfo.sessionState; + + if (_data->lastState != currentState) { + _robotClient->onStateChange(_data->lastState, currentState); + _data->lastState = currentState; + } + + switch (currentState) { + case MONITORING_WAIT: + case MONITORING_READY: + _robotClient->monitor(); + break; + case COMMANDING_WAIT: + _robotClient->waitForCommand(); + break; + case COMMANDING_ACTIVE: + _robotClient->command(); + break; + case IDLE: + default: + return; // nothing to send back + } + + // callback for transformation client + if (_trafoClient != NULL) { + _trafoClient->provide(); + } +} + + +bool HWIFClientApplication::client_app_write() +{ + // ************************************************************************** + // Encode and send command message + // ************************************************************************** + + _data->lastSendCounter++; + // check if its time to send an answer + if (_data->lastSendCounter >= _data->monitoringMsg.connectionInfo.receiveMultiplier) { + _data->lastSendCounter = 0; + + // set sequence counters + _data->commandMsg.header.sequenceCounter = _data->sequenceCounter++; + _data->commandMsg.header.reflectedSequenceCounter = + _data->monitoringMsg.header.sequenceCounter; + + if (!_data->encoder.encode(_data->sendBuffer, size_)) { + return false; + } + + if (!_connection.isOpen()) { + std::cout << "Client application connection closed" << std::endl; + return false; + } + + if (!_connection.send(_data->sendBuffer, size_)) { + std::cout << "Error: failed while trying to send command message!" << std::endl; + return false; + } + } + + return true; +} +} +} // namespace KUKA::FRI diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friClientApplication.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friClientApplication.cpp new file mode 100644 index 00000000..5f26ad21 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friClientApplication.cpp @@ -0,0 +1,211 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#include +#include +#include +#include +#include +#include +#include + +using namespace KUKA::FRI; + +//****************************************************************************** +ClientApplication::ClientApplication(IConnection& connection, IClient& client) + : _connection(connection), _robotClient(&client),_trafoClient(NULL), _data(NULL) +{ + _data = _robotClient->createData(); +} + +//****************************************************************************** +ClientApplication::ClientApplication(IConnection& connection, IClient& client, TransformationClient& trafoClient) + : _connection(connection), _robotClient(&client), _trafoClient(&trafoClient), _data(NULL) +{ + _data = _robotClient->createData(); + _trafoClient->linkData(_data); +} + +//****************************************************************************** +ClientApplication::~ClientApplication() +{ + disconnect(); + delete _data; +} + +//****************************************************************************** +bool ClientApplication::connect(int port, const char *remoteHost) +{ + if (_connection.isOpen()) + { + printf("Warning: client application already connected!\n"); + return true; + } + + return _connection.open(port, remoteHost); +} + +//****************************************************************************** +void ClientApplication::disconnect() +{ + if (_connection.isOpen()) _connection.close(); +} + +//****************************************************************************** +bool ClientApplication::step() +{ + if (!_connection.isOpen()) + { + printf("Error: client application is not connected!\n"); + return false; + } + + // ************************************************************************** + // Receive and decode new monitoring message + // ************************************************************************** + int size = _connection.receive(_data->receiveBuffer, FRI_MONITOR_MSG_MAX_SIZE); + + if (size <= 0) + { // TODO: size == 0 -> connection closed (maybe go to IDLE instead of stopping?) + printf("Error: failed while trying to receive monitoring message!\n"); + return false; + } + + if (!_data->decoder.decode(_data->receiveBuffer, size)) + { + return false; + } + + // check message type (so that our wrappers match) + if (_data->expectedMonitorMsgID != _data->monitoringMsg.header.messageIdentifier) + { + printf("Error: incompatible IDs for received message (got: %d expected %d)!\n", + (int)_data->monitoringMsg.header.messageIdentifier, + (int)_data->expectedMonitorMsgID); + return false; + } + + // ************************************************************************** + // callbacks + // ************************************************************************** + // reset command message before callbacks + _data->resetCommandMessage(); + + // callbacks for robot client + ESessionState currentState = (ESessionState)_data->monitoringMsg.connectionInfo.sessionState; + + if (_data->lastState != currentState) + { + _robotClient->onStateChange(_data->lastState, currentState); + _data->lastState = currentState; + } + + switch (currentState) + { + case MONITORING_WAIT: + case MONITORING_READY: + _robotClient->monitor(); + break; + case COMMANDING_WAIT: + _robotClient->waitForCommand(); + break; + case COMMANDING_ACTIVE: + _robotClient->command(); + break; + case IDLE: + default: + return true; // nothing to send back + } + + // callback for transformation client + if(_trafoClient != NULL) + { + _trafoClient->provide(); + } + + // ************************************************************************** + // Encode and send command message + // ************************************************************************** + + _data->lastSendCounter++; + // check if its time to send an answer + if (_data->lastSendCounter >= _data->monitoringMsg.connectionInfo.receiveMultiplier) + { + _data->lastSendCounter = 0; + + // set sequence counters + _data->commandMsg.header.sequenceCounter = _data->sequenceCounter++; + _data->commandMsg.header.reflectedSequenceCounter = + _data->monitoringMsg.header.sequenceCounter; + + if (!_data->encoder.encode(_data->sendBuffer, size)) + { + return false; + } + + if (!_connection.send(_data->sendBuffer, size)) + { + printf("Error: failed while trying to send command message!\n"); + return false; + } + } + + return true; +} + diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friClientData.h b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friClientData.h new file mode 100644 index 00000000..67c11c56 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friClientData.h @@ -0,0 +1,242 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_CLIENT_DATA_H +#define _KUKA_FRI_CLIENT_DATA_H + +#include + +#include +#include +#include +#include +#include + +namespace KUKA +{ +namespace FRI +{ + + struct ClientData + { + char receiveBuffer[FRI_MONITOR_MSG_MAX_SIZE];//!< monitoring message receive buffer + char sendBuffer[FRI_COMMAND_MSG_MAX_SIZE]; //!< command message send buffer + + FRIMonitoringMessage monitoringMsg; //!< monitoring message struct + FRICommandMessage commandMsg; //!< command message struct + + MonitoringMessageDecoder decoder; //!< monitoring message decoder + CommandMessageEncoder encoder; //!< command message encoder + + ESessionState lastState; //!< last FRI state + uint32_t sequenceCounter; //!< sequence counter for command messages + uint32_t lastSendCounter; //!< steps since last send command + uint32_t expectedMonitorMsgID; //!< expected ID for received monitoring messages + + const size_t MAX_REQUESTED_TRANSFORMATIONS; //!< maximum count of requested transformations + const size_t MAX_SIZE_TRANSFORMATION_ID; //!< maximum size in bytes of a transformation ID + std::vector requestedTrafoIDs; //!< list of requested transformation ids + + ClientData(int numDofs) + : decoder(&monitoringMsg, numDofs), + encoder(&commandMsg, numDofs), + lastState(IDLE), + sequenceCounter(0), + lastSendCounter(0), + expectedMonitorMsgID(0), + MAX_REQUESTED_TRANSFORMATIONS(sizeof(monitoringMsg.requestedTransformations) / + sizeof(monitoringMsg.requestedTransformations[0])), + MAX_SIZE_TRANSFORMATION_ID(sizeof(monitoringMsg.requestedTransformations[0].name)) + { + requestedTrafoIDs.reserve(MAX_REQUESTED_TRANSFORMATIONS); + } + + void resetCommandMessage() + { + commandMsg.commandData.has_jointPosition = false; + commandMsg.commandData.has_cartesianWrenchFeedForward = false; + commandMsg.commandData.has_jointTorque = false; + commandMsg.commandData.commandedTransformations_count = 0; + commandMsg.commandData.has_cartesianPose = false; + commandMsg.commandData.has_redundancyInformation = false; + commandMsg.has_commandData = false; + commandMsg.commandData.writeIORequest_count = 0; + } + + //****************************************************************************** + static const FriIOValue& getBooleanIOValue(const FRIMonitoringMessage* message, const char* name) + { + return getIOValue(message, name, FriIOType_BOOLEAN); + } + + //****************************************************************************** + static const FriIOValue& getDigitalIOValue(const FRIMonitoringMessage* message, const char* name) + { + return getIOValue(message, name, FriIOType_DIGITAL); + } + + //****************************************************************************** + static const FriIOValue& getAnalogIOValue(const FRIMonitoringMessage* message, const char* name) + { + return getIOValue(message, name, FriIOType_ANALOG); + } + + //****************************************************************************** + static void setBooleanIOValue(FRICommandMessage* message, const char* name, const bool value, + const FRIMonitoringMessage* monMessage) + { + setIOValue(message, name, monMessage, FriIOType_BOOLEAN).digitalValue = value; + } + + //****************************************************************************** + static void setDigitalIOValue(FRICommandMessage* message, const char* name, const unsigned long long value, + const FRIMonitoringMessage* monMessage) + { + setIOValue(message, name, monMessage, FriIOType_DIGITAL).digitalValue = value; + } + + //****************************************************************************** + static void setAnalogIOValue(FRICommandMessage* message, const char* name, const double value, + const FRIMonitoringMessage* monMessage) + { + setIOValue(message, name, monMessage, FriIOType_ANALOG).analogValue = value; + } + + protected: + + //****************************************************************************** + static const FriIOValue& getIOValue(const FRIMonitoringMessage* message, const char* name, + const FriIOType ioType) + { + if(message != NULL && message->has_monitorData == true) + { + const MessageMonitorData& monData = message->monitorData; + const bool analogValue = (ioType == FriIOType_ANALOG); + const bool digitalValue = (ioType == FriIOType_DIGITAL) | (ioType == FriIOType_BOOLEAN); + for(size_t i = 0; i < monData.readIORequest_count; i++) + { + const FriIOValue& ioValue = monData.readIORequest[i]; + if(strcmp(name, ioValue.name) == 0) + { + if(ioValue.type == ioType && + ioValue.has_digitalValue == digitalValue && + ioValue.has_analogValue == analogValue) + { + return ioValue; + } + + const char* ioTypeName; + switch(ioType) + { + case FriIOType_ANALOG: ioTypeName = "analog"; break; + case FriIOType_DIGITAL: ioTypeName = "digital"; break; + case FriIOType_BOOLEAN: ioTypeName = "boolean"; break; + default: ioTypeName = "?"; break; + } + + throw FRIException("IO %s is not of type %s.", name, ioTypeName); + } + } + } + + throw FRIException("Could not locate IO %s in monitor message.", name); + } + + //****************************************************************************** + static FriIOValue& setIOValue(FRICommandMessage* message, const char* name, + const FRIMonitoringMessage* monMessage, const FriIOType ioType) + { + MessageCommandData& cmdData = message->commandData; + const size_t maxIOs = sizeof(cmdData.writeIORequest) / sizeof(cmdData.writeIORequest[0]); + if(cmdData.writeIORequest_count < maxIOs) + { + // call getter which will raise an exception if the output doesn't exist + // or is of wrong type. + if(getIOValue(monMessage, name, ioType).direction != FriIODirection_OUTPUT) + { + throw FRIException("IO %s is not an output value.", name); + } + + // add IO value to command message + FriIOValue& ioValue = cmdData.writeIORequest[cmdData.writeIORequest_count]; + + strncpy(ioValue.name, name, sizeof(ioValue.name) - 1); + ioValue.name[sizeof(ioValue.name) - 1] = 0; // ensure termination + ioValue.type = ioType; + ioValue.has_digitalValue = (ioType == FriIOType_DIGITAL) | (ioType == FriIOType_BOOLEAN); + ioValue.has_analogValue = (ioType == FriIOType_ANALOG); + ioValue.direction = FriIODirection_OUTPUT; + + cmdData.writeIORequest_count ++; + message->has_commandData = true; + + return ioValue; + } + else + { + throw FRIException("Exceeded maximum number of outputs that can be set."); + } + } + }; + +} +} + + +#endif // _KUKA_FRI_CLIENT_DATA_H diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friCommandMessageEncoder.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friCommandMessageEncoder.cpp new file mode 100644 index 00000000..5653154c --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friCommandMessageEncoder.cpp @@ -0,0 +1,125 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#include +#include +#include + +using namespace KUKA::FRI; + +//****************************************************************************** +CommandMessageEncoder::CommandMessageEncoder(FRICommandMessage* pMessage, int num) + : m_nNum(num), m_pMessage(pMessage) +{ + initMessage(); +} + +//****************************************************************************** +CommandMessageEncoder::~CommandMessageEncoder() +{ + +} + +//****************************************************************************** +void CommandMessageEncoder::initMessage() +{ + m_pMessage->has_commandData = false; + m_pMessage->has_endOfMessageData = false; + m_pMessage->commandData.has_jointPosition = false; + m_pMessage->commandData.has_cartesianWrenchFeedForward = false; + m_pMessage->commandData.has_jointTorque = false; + m_pMessage->commandData.has_cartesianPose = false; + m_pMessage->commandData.commandedTransformations_count = 0; + m_pMessage->commandData.has_redundancyInformation = false; + m_pMessage->header.messageIdentifier = 0; + // init with 0. Necessary for creating the correct reflected sequence count in the monitoring msg + m_pMessage->header.sequenceCounter = 0; + m_pMessage->header.reflectedSequenceCounter = 0; + + m_pMessage->commandData.writeIORequest_count = 0; + + // allocate and map memory for protobuf repeated structures + map_repeatedDouble(FRI_MANAGER_NANOPB_ENCODE, m_nNum, + &m_pMessage->commandData.jointPosition.value, + &m_tRecvContainer.jointPosition); + map_repeatedDouble(FRI_MANAGER_NANOPB_ENCODE, m_nNum, + &m_pMessage->commandData.jointTorque.value, + &m_tRecvContainer.jointTorque); + + // nanopb encoding needs to know how many elements the static array contains + // a quaternion always contains 7 elements + m_pMessage->commandData.cartesianPose.element_count = 7; + // a Cartesian wrench feed forward vector always contains 6 elements + m_pMessage->commandData.cartesianWrenchFeedForward.element_count = 6; +} + +//****************************************************************************** +bool CommandMessageEncoder::encode(char* buffer, int& size) +{ + // generate stream for encoding + pb_ostream_t stream = pb_ostream_from_buffer((uint8_t*)buffer, FRI_COMMAND_MSG_MAX_SIZE); + // encode monitoring Message to stream + bool status = pb_encode(&stream, FRICommandMessage_fields, m_pMessage); + size = stream.bytes_written; + if (!status) + { + printf("!!encoding error on Command message: %s!!\n", PB_GET_ERROR(&stream)); + } + return status; +} diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friCommandMessageEncoder.h b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friCommandMessageEncoder.h new file mode 100644 index 00000000..dde55882 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friCommandMessageEncoder.h @@ -0,0 +1,118 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_COMMANDMESSAGEENCODER_H +#define _KUKA_FRI_COMMANDMESSAGEENCODER_H + + +#include +#include + + + +namespace KUKA +{ +namespace FRI +{ + + static const int FRI_COMMAND_MSG_MAX_SIZE = 1500; //!< max size of a FRI command message + + class CommandMessageEncoder + { + + public: + + CommandMessageEncoder(FRICommandMessage* pMessage, int num); + + ~CommandMessageEncoder(); + + bool encode(char* buffer, int& size); + + private: + + struct LocalCommandDataContainer + { + tRepeatedDoubleArguments jointPosition; + tRepeatedDoubleArguments jointTorque; + + LocalCommandDataContainer() + { + init_repeatedDouble(&jointPosition); + init_repeatedDouble(&jointTorque); + } + + ~LocalCommandDataContainer() + { + free_repeatedDouble(&jointPosition); + free_repeatedDouble(&jointTorque); + } + }; + + int m_nNum; + + LocalCommandDataContainer m_tRecvContainer; + FRICommandMessage* m_pMessage; + + void initMessage(); + }; + +} +} + +#endif // _KUKA_FRI_COMMANDMESSAGEENCODER_H diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friDataHelper.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friDataHelper.cpp new file mode 100644 index 00000000..d87dbb5a --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friDataHelper.cpp @@ -0,0 +1,178 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ + +#include + +#include + +namespace KUKA +{ + namespace FRI + { + void DataHelper::convertTrafoMatrixToQuaternion(const double (&matrixTrafo)[3][4], + double (&quaternionTrafo)[7]) + { + + double s = 1.0 + matrixTrafo[0][0] + matrixTrafo[1][1] + matrixTrafo[2][2]; // = 4 w^2 + const double epsilon = 1e-11; + + quaternionTrafo[QUAT_TX] = matrixTrafo[0][3]; + quaternionTrafo[QUAT_TY] = matrixTrafo[1][3]; + quaternionTrafo[QUAT_TZ] = matrixTrafo[2][3]; + + if (s > epsilon) + { + quaternionTrafo[QUAT_QW] = 0.5 * sqrt(s); // > 0.5 * sqrt(epsilon) = 0.5 * 1e-6 + s = 0.25 / quaternionTrafo[QUAT_QW]; // = 1/(4w) + quaternionTrafo[QUAT_QX] = (matrixTrafo[2][1] - matrixTrafo[1][2]) * s; // 4wx/(4w) + quaternionTrafo[QUAT_QY] = (matrixTrafo[0][2] - matrixTrafo[2][0]) * s; // 4wy/(4w) + quaternionTrafo[QUAT_QZ] = (matrixTrafo[1][0] - matrixTrafo[0][1]) * s; // 4wz/(4w) + } + else + { + // w is very small (or even vanishing) + if ((matrixTrafo[0][0] > matrixTrafo[1][1]) && (matrixTrafo[0][0] > matrixTrafo[2][2])) + { + // => |x| = max{|x|,|y|,|z|} (and |x| > |w|); choose x > 0 + quaternionTrafo[QUAT_QX] = 0.5 * sqrt(1.0 + matrixTrafo[0][0] - matrixTrafo[1][1] - matrixTrafo[2][2]); + s = 0.25 / quaternionTrafo[QUAT_QX]; // 1/(4x) + quaternionTrafo[QUAT_QW] = (matrixTrafo[2][1] - matrixTrafo[1][2]) * s; // 4wx/(4x) + quaternionTrafo[QUAT_QY] = (matrixTrafo[0][1] + matrixTrafo[1][0]) * s; // 4yx/(4x) + quaternionTrafo[QUAT_QZ] = (matrixTrafo[0][2] + matrixTrafo[2][0]) * s; // 4zx/(4x) + } + else if (matrixTrafo[1][1] > matrixTrafo[2][2]) + { + // => |y| = max{|x|,|y|,|z|} (and |y| > |w|); choose y > 0 + quaternionTrafo[QUAT_QY] = 0.5 * sqrt(1.0 + matrixTrafo[1][1] - matrixTrafo[0][0] - matrixTrafo[2][2]); + s = 0.25 / quaternionTrafo[QUAT_QY]; // 1/(4y) + quaternionTrafo[QUAT_QW] = (matrixTrafo[0][2] - matrixTrafo[2][0]) * s; // 4wy/(4y) + quaternionTrafo[QUAT_QX] = (matrixTrafo[0][1] + matrixTrafo[1][0]) * s; // 4xy/(4y) + quaternionTrafo[QUAT_QZ] = (matrixTrafo[1][2] + matrixTrafo[2][1]) * s; // 4zy/(4y) + } + else + { + // => |z| = max{|x|,|y|,|z|} (and |z| > |w|); choose z > 0 + quaternionTrafo[QUAT_QZ] = 0.5 * sqrt(1.0 + matrixTrafo[2][2] - matrixTrafo[0][0] - matrixTrafo[1][1]); + s = 0.25 / quaternionTrafo[QUAT_QZ]; // 1/(4z) + quaternionTrafo[QUAT_QW] = (matrixTrafo[1][0] - matrixTrafo[0][1]) * s; // 4wz/(4z) + quaternionTrafo[QUAT_QX] = (matrixTrafo[0][2] + matrixTrafo[2][0]) * s; // 4xz/(4z) + quaternionTrafo[QUAT_QY] = (matrixTrafo[1][2] + matrixTrafo[2][1]) * s; // 4yz/(4z) + } + } + + // normalize result to ensure that we obtain a unit quaternion + // (should be superfluous but in case of numerical problems ...) + const double rotProduct = + quaternionTrafo[QUAT_QX] * quaternionTrafo[QUAT_QX] + + quaternionTrafo[QUAT_QY] * quaternionTrafo[QUAT_QY] + + quaternionTrafo[QUAT_QZ] * quaternionTrafo[QUAT_QZ]; + + const double norm = sqrt(quaternionTrafo[QUAT_QW] * quaternionTrafo[QUAT_QW] + rotProduct); + + // normalize to unit length, to obtain an orientation representing quaternion + if (norm > epsilon) + { + quaternionTrafo[QUAT_QW] /= norm; + quaternionTrafo[QUAT_QX] /= norm; + quaternionTrafo[QUAT_QY] /= norm; + quaternionTrafo[QUAT_QZ] /= norm; + } + else + { + // input has vanishing length and is thus far away from a reasonable, + // orientation representing quaternion :-( + // generally normalize vanishing quaternions to the identity + quaternionTrafo[QUAT_QW] = 1.0; + quaternionTrafo[QUAT_QX] = 0; + quaternionTrafo[QUAT_QY] = 0; + quaternionTrafo[QUAT_QZ] = 0; + } + + } + + void DataHelper::convertTrafoQuaternionToMatrix(const double(&quaternionTrafo)[7], + double(&matrixTrafo)[3][4]) + { + + const double qW = quaternionTrafo[QUAT_QW]; + const double qX = quaternionTrafo[QUAT_QX]; + const double qY = quaternionTrafo[QUAT_QY]; + const double qZ = quaternionTrafo[QUAT_QZ]; + + // conversion for unit quaternion to transformation matrix + matrixTrafo[0][0] = 1 - 2 * ((qY * qY) + (qZ * qZ)); + matrixTrafo[0][1] = 2 * ((qX * qY) - (qW * qZ)); + matrixTrafo[0][2] = 2 * ((qX * qZ) + (qW * qY)); + matrixTrafo[0][3] = quaternionTrafo[QUAT_TX]; + + matrixTrafo[1][0] = 2 * ((qX * qY) + (qW * qZ)); + matrixTrafo[1][1] = 1 - 2 * ((qX * qX) + (qZ * qZ)); + matrixTrafo[1][2] = 2 * ((qY * qZ) - (qW * qX)); + matrixTrafo[1][3] = quaternionTrafo[QUAT_TY]; + + matrixTrafo[2][0] = 2 * ((qX * qZ) - (qW * qY)); + matrixTrafo[2][1] = 2 * ((qY * qZ) + (qW * qX)); + matrixTrafo[2][2] = 1 - 2 * ((qX * qX) + (qY * qY)); + matrixTrafo[2][3] = quaternionTrafo[QUAT_TZ]; + + } + + } +} diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRClient.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRClient.cpp new file mode 100644 index 00000000..df035d3c --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRClient.cpp @@ -0,0 +1,126 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#include +#include +#include + +using namespace KUKA::FRI; +char FRIException::_buffer[1024] = { 0 }; + +//****************************************************************************** +LBRClient::LBRClient() +{ + +} + +//****************************************************************************** +LBRClient::~LBRClient() +{ + +} + +//****************************************************************************** +void LBRClient::onStateChange(ESessionState oldState, ESessionState newState) +{ + // TODO: String converter function for states + printf("LBRiiwaClient state changed from %d to %d\n", oldState, newState); +} + +//****************************************************************************** +void LBRClient::monitor() +{ + // place your code here to monitor the robot's current state +} + +//****************************************************************************** +void LBRClient::waitForCommand() +{ + if (CARTESIAN_POSE == _robotState.getClientCommandMode()) + robotCommand().setCartesianPose(robotState().getIpoCartesianPose()); + else + robotCommand().setJointPosition(robotState().getIpoJointPosition()); +} + +//****************************************************************************** +void LBRClient::command() +{ + if (CARTESIAN_POSE == _robotState.getClientCommandMode()) + robotCommand().setCartesianPose(robotState().getIpoCartesianPose()); + else + robotCommand().setJointPosition(robotState().getIpoJointPosition()); +} + +//****************************************************************************** +ClientData* LBRClient::createData() +{ + ClientData* data = new ClientData(_robotState.NUMBER_OF_JOINTS); + + // link monitoring and command message to wrappers + _robotState._message = &data->monitoringMsg; + _robotCommand._cmdMessage = &data->commandMsg; + _robotCommand._monMessage = &data->monitoringMsg; + + // set specific message IDs + data->expectedMonitorMsgID = _robotState.LBRMONITORMESSAGEID; + data->commandMsg.header.messageIdentifier = _robotCommand.LBRCOMMANDMESSAGEID; + + return data; +} + diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRCommand.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRCommand.cpp new file mode 100644 index 00000000..e1487154 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRCommand.cpp @@ -0,0 +1,149 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#include +#include +#include +#include +#include + +using namespace KUKA::FRI; + +//****************************************************************************** +void LBRCommand::setJointPosition(const double* values) +{ + _cmdMessage->has_commandData = true; + _cmdMessage->commandData.has_jointPosition = true; + tRepeatedDoubleArguments *dest = + (tRepeatedDoubleArguments*)_cmdMessage->commandData.jointPosition.value.arg; + memcpy(dest->value, values, LBRState::NUMBER_OF_JOINTS * sizeof(double)); +} + +//****************************************************************************** +void LBRCommand::setWrench(const double* wrench) +{ + _cmdMessage->has_commandData = true; + _cmdMessage->commandData.has_cartesianWrenchFeedForward = true; + + double *dest = _cmdMessage->commandData.cartesianWrenchFeedForward.element; + memcpy(dest, wrench, 6 * sizeof(double)); +} + +//****************************************************************************** +void LBRCommand::setTorque(const double* torques) +{ + _cmdMessage->has_commandData = true; + _cmdMessage->commandData.has_jointTorque= true; + + tRepeatedDoubleArguments *dest = (tRepeatedDoubleArguments*)_cmdMessage->commandData.jointTorque.value.arg; + memcpy(dest->value, torques, LBRState::NUMBER_OF_JOINTS * sizeof(double)); +} + +//****************************************************************************** +void LBRCommand::setCartesianPose(const double* cartesianPoseQuaternion, + double const *const redundancyValue) +{ + _cmdMessage->has_commandData = true; + _cmdMessage->commandData.has_cartesianPose = true; + _cmdMessage->commandData.cartesianPose.name[0] = '\0'; + memcpy(_cmdMessage->commandData.cartesianPose.element, cartesianPoseQuaternion, 7 * sizeof(double)); + + _cmdMessage->commandData.has_redundancyInformation = true; + _cmdMessage->commandData.redundancyInformation.strategy = + _monMessage->monitorData.measuredRedundancyInformation.strategy; + + if (NULL != redundancyValue) + { + //set value if provided + + _cmdMessage->commandData.redundancyInformation.value = *redundancyValue; + } + else + { + // use interpolated redundancy value if no value is commanded + _cmdMessage->commandData.redundancyInformation.value = _monMessage->ipoData.redundancyInformation.value; + } +} + +//****************************************************************************** +void LBRCommand::setCartesianPoseAsMatrix(const double(&measuredCartesianPose)[3][4], + double const *const redundancyValue) +{ + double quaternion[7]; + DataHelper::convertTrafoMatrixToQuaternion(measuredCartesianPose, quaternion); + setCartesianPose(quaternion, redundancyValue); +} + +//****************************************************************************** +void LBRCommand::setBooleanIOValue(const char* name, const bool value) +{ + ClientData::setBooleanIOValue(_cmdMessage, name, value, _monMessage); +} + +//****************************************************************************** +void LBRCommand::setAnalogIOValue(const char* name, const double value) +{ + ClientData::setAnalogIOValue(_cmdMessage, name, value, _monMessage); +} + +//****************************************************************************** +void LBRCommand::setDigitalIOValue(const char* name, const unsigned long long value) +{ + ClientData::setDigitalIOValue(_cmdMessage, name, value, _monMessage); +} diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRState.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRState.cpp new file mode 100644 index 00000000..271ae868 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRState.cpp @@ -0,0 +1,271 @@ +/** + + The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + + \file + \version {2.5} + */ +#include +#include +#include +#include + +using namespace KUKA::FRI; + +LBRState::LBRState() : + _message(0) +{ + +} +//****************************************************************************** +double LBRState::getSampleTime() const +{ + return _message->connectionInfo.sendPeriod * 0.001; +} + +//****************************************************************************** +ESessionState LBRState::getSessionState() const +{ + return (ESessionState) _message->connectionInfo.sessionState; +} + +//****************************************************************************** +EConnectionQuality LBRState::getConnectionQuality() const +{ + return (EConnectionQuality) _message->connectionInfo.quality; +} + +//****************************************************************************** +ESafetyState LBRState::getSafetyState() const +{ + return (ESafetyState) _message->robotInfo.safetyState; +} + +//****************************************************************************** +EOperationMode LBRState::getOperationMode() const +{ + return (EOperationMode) _message->robotInfo.operationMode; +} + +//****************************************************************************** +EDriveState LBRState::getDriveState() const +{ + tRepeatedIntArguments *values = (tRepeatedIntArguments *) _message->robotInfo.driveState.arg; + int firstState = (int) values->value[0]; + for (int i = 1; i < NUMBER_OF_JOINTS; i++) + { + int state = (int) values->value[i]; + if (state != firstState) + { + return TRANSITIONING; + } + } + return (EDriveState) firstState; +} + +//******************************************************************************** +EOverlayType LBRState::getOverlayType() const +{ + return (EOverlayType) _message->ipoData.overlayType; +} + +//******************************************************************************** +EClientCommandMode LBRState::getClientCommandMode() const +{ + return (EClientCommandMode) _message->ipoData.clientCommandMode; +} + +//****************************************************************************** +EControlMode LBRState::getControlMode() const +{ + return (EControlMode) _message->robotInfo.controlMode; +} + +//****************************************************************************** +unsigned int LBRState::getTimestampSec() const +{ + return _message->monitorData.timestamp.sec; +} + +//****************************************************************************** +unsigned int LBRState::getTimestampNanoSec() const +{ + return _message->monitorData.timestamp.nanosec; +} + +//****************************************************************************** +const double* LBRState::getMeasuredJointPosition() const +{ + tRepeatedDoubleArguments *values = (tRepeatedDoubleArguments*) _message->monitorData.measuredJointPosition.value.arg; + return (double*) values->value; +} + +//****************************************************************************** +const double* LBRState::getMeasuredTorque() const +{ + tRepeatedDoubleArguments *values = (tRepeatedDoubleArguments*) _message->monitorData.measuredTorque.value.arg; + return (double*) values->value; +} + +//****************************************************************************** +const double* LBRState::getCommandedTorque() const +{ + tRepeatedDoubleArguments *values = (tRepeatedDoubleArguments*) _message->monitorData.commandedTorque.value.arg; + return (double*) values->value; +} + +//****************************************************************************** +const double* LBRState::getExternalTorque() const +{ + tRepeatedDoubleArguments *values = (tRepeatedDoubleArguments*) _message->monitorData.externalTorque.value.arg; + return (double*) values->value; +} + +//****************************************************************************** +const double* LBRState::getIpoJointPosition() const +{ + if (!_message->ipoData.has_jointPosition) + { + throw FRIException("IPO joint position not available when FRI Joint Overlay is not active."); + return NULL; + } + + tRepeatedDoubleArguments *values = (tRepeatedDoubleArguments*) _message->ipoData.jointPosition.value.arg; + return (double*) values->value; +} + +//****************************************************************************** +double LBRState::getTrackingPerformance() const +{ + if (!_message->ipoData.has_trackingPerformance) + return 0.0; + + return _message->ipoData.trackingPerformance; +} + +//****************************************************************************** +bool LBRState::getBooleanIOValue(const char* name) const +{ + return ClientData::getBooleanIOValue(_message, name).digitalValue != 0; +} + +//****************************************************************************** +unsigned long long LBRState::getDigitalIOValue(const char* name) const +{ + return ClientData::getDigitalIOValue(_message, name).digitalValue; +} + +//****************************************************************************** +double LBRState::getAnalogIOValue(const char* name) const +{ + return ClientData::getAnalogIOValue(_message, name).analogValue; +} + +//****************************************************************************** +const double* LBRState::getMeasuredCartesianPose() const +{ + return _message->monitorData.measuredCartesianPose.element; +} + +//****************************************************************************** +void LBRState::getMeasuredCartesianPoseAsMatrix(double(&measuredCartesianPose)[3][4]) const +{ + DataHelper::convertTrafoQuaternionToMatrix(_message->monitorData.measuredCartesianPose.element, + measuredCartesianPose); +} + +//****************************************************************************** +const double* LBRState::getIpoCartesianPose() const +{ + if (!_message->ipoData.has_cartesianPose) + { + throw FRIException("IPO Cartesian Pose not available when FRI Cartesian Overlay is not active."); + return NULL; + } + + return _message->ipoData.cartesianPose.element; +} + +//****************************************************************************** +void LBRState::getIpoCartesianPoseAsMatrix(double(&ipoCartesianPose)[3][4]) const +{ + if (!_message->ipoData.has_cartesianPose) + { + throw FRIException("IPO Cartesian Pose not available when FRI Cartesian Overlay is not active."); + } + DataHelper::convertTrafoQuaternionToMatrix(_message->ipoData.cartesianPose.element, ipoCartesianPose); +} + +//****************************************************************************** +double LBRState::getMeasuredRedundancyValue() const +{ + return _message->monitorData.measuredRedundancyInformation.value; +} + +//****************************************************************************** +double LBRState::getIpoRedundancyValue() const +{ + if (!_message->ipoData.has_redundancyInformation) + { + throw FRIException("IPO redundancy value not available when FRI Cartesian Overlay is not active."); + } + return _message->ipoData.redundancyInformation.value; +} + +//****************************************************************************** +ERedundancyStrategy LBRState::getRedundancyStrategy() const +{ + return (ERedundancyStrategy)_message->monitorData.measuredRedundancyInformation.strategy; +} diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friMonitoringMessageDecoder.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friMonitoringMessageDecoder.cpp new file mode 100644 index 00000000..c6426db9 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friMonitoringMessageDecoder.cpp @@ -0,0 +1,142 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#include +#include "friMonitoringMessageDecoder.h" +#include "pb_decode.h" + + +using namespace KUKA::FRI; + +//****************************************************************************** +MonitoringMessageDecoder::MonitoringMessageDecoder(FRIMonitoringMessage* pMessage, int num) + : m_nNum(num), m_pMessage(pMessage) +{ + initMessage(); +} + +//****************************************************************************** +MonitoringMessageDecoder::~MonitoringMessageDecoder() +{ + +} + +//****************************************************************************** +void MonitoringMessageDecoder::initMessage() +{ + // set initial data + // it is assumed that no robot information and monitoring data is available and therefore the + // optional fields are initialized with false + m_pMessage->has_robotInfo = false; + m_pMessage->has_monitorData = false; + m_pMessage->has_connectionInfo = true; + m_pMessage->has_ipoData = false; + m_pMessage->requestedTransformations_count = 0; + m_pMessage->has_endOfMessageData = false; + + + m_pMessage->header.messageIdentifier = 0; + m_pMessage->header.reflectedSequenceCounter = 0; + m_pMessage->header.sequenceCounter = 0; + + m_pMessage->connectionInfo.sessionState = FRISessionState_IDLE; + m_pMessage->connectionInfo.quality = FRIConnectionQuality_POOR; + + m_pMessage->monitorData.readIORequest_count = 0; + m_pMessage->monitorData.measuredCartesianPose.element_count = 0; + + // allocate and map memory for protobuf repeated structures + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.measuredJointPosition.value, + &m_tSendContainer.m_AxQMsrLocal); + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.measuredTorque.value, + &m_tSendContainer.m_AxTauMsrLocal); + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.commandedTorque.value, + &m_tSendContainer.m_AxTauCmdLocal); + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.externalTorque.value, + &m_tSendContainer.m_AxTauExtMsrLocal); + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE,m_nNum, + &m_pMessage->ipoData.jointPosition.value, + &m_tSendContainer.m_AxQCmdIPO); + + map_repeatedInt(FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->robotInfo.driveState, + &m_tSendContainer.m_AxDriveStateLocal); +} + +//****************************************************************************** +bool MonitoringMessageDecoder::decode(char* buffer, int size) +{ + pb_istream_t stream = pb_istream_from_buffer((uint8_t*)buffer, size); + + bool status = pb_decode(&stream, FRIMonitoringMessage_fields, m_pMessage); + if (!status) + { + printf("!!decoding error on Monitor message: %s!!\n", PB_GET_ERROR(&stream)); + } + + return status; +} diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friMonitoringMessageDecoder.h b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friMonitoringMessageDecoder.h new file mode 100644 index 00000000..795c5175 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friMonitoringMessageDecoder.h @@ -0,0 +1,133 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _KUKA_FRI_MONITORINGMESSAGEDECODER_H +#define _KUKA_FRI_MONITORINGMESSAGEDECODER_H + +#include "FRIMessages.pb.h" +#include "pb_frimessages_callbacks.h" + + +namespace KUKA +{ +namespace FRI +{ + + static const int FRI_MONITOR_MSG_MAX_SIZE = 1500; //!< max size of a FRI monitoring message + + + class MonitoringMessageDecoder + { + + public: + + MonitoringMessageDecoder(FRIMonitoringMessage* pMessage, int num); + + ~MonitoringMessageDecoder(); + + bool decode(char* buffer, int size); + + + private: + + struct LocalMonitoringDataContainer + { + tRepeatedDoubleArguments m_AxQMsrLocal; + tRepeatedDoubleArguments m_AxTauMsrLocal; + tRepeatedDoubleArguments m_AxQCmdT1mLocal; + tRepeatedDoubleArguments m_AxTauCmdLocal; + tRepeatedDoubleArguments m_AxTauExtMsrLocal; + tRepeatedIntArguments m_AxDriveStateLocal; + tRepeatedDoubleArguments m_AxQCmdIPO; + + LocalMonitoringDataContainer() + { + init_repeatedDouble(&m_AxQMsrLocal); + init_repeatedDouble(&m_AxTauMsrLocal); + init_repeatedDouble(&m_AxQCmdT1mLocal); + init_repeatedDouble(&m_AxTauCmdLocal); + init_repeatedDouble(&m_AxTauExtMsrLocal); + init_repeatedDouble(&m_AxQCmdIPO); + init_repeatedInt(&m_AxDriveStateLocal); + } + + ~LocalMonitoringDataContainer() + { + free_repeatedDouble(&m_AxQMsrLocal); + free_repeatedDouble(&m_AxTauMsrLocal); + free_repeatedDouble(&m_AxQCmdT1mLocal); + free_repeatedDouble(&m_AxTauCmdLocal); + free_repeatedDouble(&m_AxTauExtMsrLocal); + free_repeatedDouble(&m_AxQCmdIPO); + free_repeatedInt(&m_AxDriveStateLocal); + } + }; + + int m_nNum; + + LocalMonitoringDataContainer m_tSendContainer; + FRIMonitoringMessage* m_pMessage; + + void initMessage(); + }; + +} +} + +#endif // _KUKA_FRI_MONITORINGMESSAGEDECODER_H diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friTransformationClient.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friTransformationClient.cpp new file mode 100644 index 00000000..12285b67 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friTransformationClient.cpp @@ -0,0 +1,205 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ + +#include +#include +#include + +#include +#include +#include +#include "FRIMessages.pb.h" +#include "pb_frimessages_callbacks.h" + +using namespace KUKA::FRI; + +//****************************************************************************** +TransformationClient::TransformationClient() +{ +} + +//****************************************************************************** +TransformationClient::~TransformationClient() +{ +} + +//****************************************************************************** +const std::vector& TransformationClient::getRequestedTransformationIDs() const +{ + unsigned int trafoCount = _data->monitoringMsg.requestedTransformations_count; + _data->requestedTrafoIDs.resize(trafoCount); + for (unsigned int i=0; irequestedTrafoIDs[i] = _data->monitoringMsg.requestedTransformations[i].name; + } + return _data->requestedTrafoIDs; +} + +//****************************************************************************** +const unsigned int TransformationClient::getTimestampSec() const +{ + return _data->monitoringMsg.monitorData.timestamp.sec; +} + +//****************************************************************************** +const unsigned int TransformationClient::getTimestampNanoSec() const +{ + return _data->monitoringMsg.monitorData.timestamp.nanosec; +} + +//****************************************************************************** +void TransformationClient::setTransformation(const char* transformationID, + const double (&transformationQuaternion)[7], + const unsigned int timeSec, const unsigned int timeNanoSec) +{ + _data->commandMsg.has_commandData = true; + + const unsigned int currentSize = _data->commandMsg.commandData.commandedTransformations_count; + + if (currentSize < _data->MAX_REQUESTED_TRANSFORMATIONS) + { + _data->commandMsg.commandData.commandedTransformations_count++; + QuaternionTransformation& dest = _data->commandMsg.commandData.commandedTransformations[currentSize]; + dest.element_count = 7; + strncpy(dest.name, transformationID, _data->MAX_SIZE_TRANSFORMATION_ID); + dest.name[_data->MAX_SIZE_TRANSFORMATION_ID - 1] = '\0'; + memcpy(dest.element, transformationQuaternion, 7 * sizeof(double)); + dest.has_timestamp = true; + dest.timestamp.sec = timeSec; + dest.timestamp.nanosec = timeNanoSec; + } + else + { + throw FRIException("Exceeded maximum number of transformations."); + } +} + +//****************************************************************************** +void TransformationClient::setTransformation(const char* transformationID, + const double (&transformationMatrix)[3][4], + const unsigned int timeSec, const unsigned int timeNanoSec) +{ + + double tmpQauternionTrafo[7]; + DataHelper::convertTrafoMatrixToQuaternion(transformationMatrix, tmpQauternionTrafo); + + setTransformation(transformationID, tmpQauternionTrafo, timeSec, timeNanoSec); +} + +//****************************************************************************** +void TransformationClient::linkData(ClientData* clientData) +{ + _data = clientData; +} + +//****************************************************************************** +double TransformationClient::getSampleTime() const +{ + return _data->monitoringMsg.connectionInfo.sendPeriod * 0.001; +} + +//****************************************************************************** +EConnectionQuality TransformationClient::getConnectionQuality() const +{ + return (EConnectionQuality)_data->monitoringMsg.connectionInfo.quality; +} + + +//****************************************************************************** +void TransformationClient::setBooleanIOValue(const char* name, const bool value) +{ + ClientData::setBooleanIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg); +} + +//****************************************************************************** +void TransformationClient::setAnalogIOValue(const char* name, const double value) +{ + ClientData::setAnalogIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg); +} + +//****************************************************************************** +void TransformationClient::setDigitalIOValue(const char* name, const unsigned long long value) +{ + ClientData::setDigitalIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg); +} + +//****************************************************************************** +bool TransformationClient::getBooleanIOValue(const char* name) const +{ + return ClientData::getBooleanIOValue(&_data->monitoringMsg, name).digitalValue != 0; +} + +//****************************************************************************** +unsigned long long TransformationClient::getDigitalIOValue(const char* name) const +{ + return ClientData::getDigitalIOValue(&_data->monitoringMsg, name).digitalValue; +} + +//****************************************************************************** +double TransformationClient::getAnalogIOValue(const char* name) const +{ + return ClientData::getAnalogIOValue(&_data->monitoringMsg, name).analogValue; +} + +//****************************************************************************** +/*const std::vector& TransformationClient::getRequestedIO_IDs() const +{ + return _data->getRequestedIO_IDs(); +}*/ diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friUdpConnection.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friUdpConnection.cpp new file mode 100644 index 00000000..774552ad --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friUdpConnection.cpp @@ -0,0 +1,243 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.FRI� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#include +#include +#ifndef _MSC_VER +#include +#endif + +#include + + +#ifdef WIN32 +#include +#include +#ifdef _MSC_VER +#pragma comment(lib, "ws2_32.lib") +#endif +#endif + +using namespace KUKA::FRI; + +//****************************************************************************** +UdpConnection::UdpConnection(unsigned int receiveTimeout) : + _udpSock(-1), + _receiveTimeout(receiveTimeout) +{ +#ifdef WIN32 + WSADATA WSAData; + WSAStartup(MAKEWORD(2,0), &WSAData); +#endif +} + +//****************************************************************************** +UdpConnection::~UdpConnection() +{ + close(); +#ifdef WIN32 + WSACleanup(); +#endif +} + +//****************************************************************************** +bool UdpConnection::open(int port, const char *controllerAddress) +{ + struct sockaddr_in servAddr; + memset(&servAddr, 0, sizeof(servAddr)); + memset(&_controllerAddr, 0, sizeof(_controllerAddr)); + + // socket creation + _udpSock = socket(PF_INET, SOCK_DGRAM, 0); + if (_udpSock < 0) + { + printf("opening socket failed!\n"); + return false; + } + + // use local server port + servAddr.sin_family = AF_INET; + servAddr.sin_port = htons(port); + servAddr.sin_addr.s_addr = htonl(INADDR_ANY); + + if (bind(_udpSock, (struct sockaddr *)&servAddr, sizeof(servAddr)) < 0) + { + printf("binding port number %d failed!\n", port); + close(); + return false; + } + // initialize the socket properly + _controllerAddr.sin_family = AF_INET; + _controllerAddr.sin_port = htons(port); + if (controllerAddress) + { +#ifndef __MINGW32__ + inet_pton(AF_INET, controllerAddress, &_controllerAddr.sin_addr); +#else + _controllerAddr.sin_addr.s_addr = inet_addr(controllerAddress); +#endif + if ( connect(_udpSock, (struct sockaddr *)&_controllerAddr, sizeof(_controllerAddr)) < 0) + { + printf("connecting to controller with address %s failed !\n", controllerAddress); + close(); + return false; + } + } + else + { + _controllerAddr.sin_addr.s_addr = htonl(INADDR_ANY); + } + return true; +} + +//****************************************************************************** +void UdpConnection::close() +{ + if (isOpen()) + { +#ifdef WIN32 + closesocket(_udpSock); +#else + ::close(_udpSock); +#endif + } + _udpSock = -1; +} + +//****************************************************************************** +bool UdpConnection::isOpen() const +{ + return (_udpSock >= 0); +} + +//****************************************************************************** +int UdpConnection::receive(char *buffer, int maxSize) +{ + if (isOpen()) + { + /** HAVE_SOCKLEN_T + Yes - unbelievable: There are differences in standard calling parameters (types) to recvfrom + Windows winsock, VxWorks and QNX use int + newer Posix (most Linuxes) use socklen_t + */ +#ifdef HAVE_SOCKLEN_T + socklen_t sockAddrSize; +#else + unsigned int sockAddrSize; +#endif + sockAddrSize = sizeof(struct sockaddr_in); + /** check for timeout + Because SO_RCVTIMEO is in Windows not correctly implemented, select is used for the receive time out. + If a timeout greater than 0 is given, wait until the timeout is reached or a message was received. + If t, abort the function with an error. + */ + if(_receiveTimeout > 0) + { + + // Set up struct timeval + struct timeval tv; + tv.tv_sec = _receiveTimeout / 1000; + tv.tv_usec = (_receiveTimeout % 1000) * 1000; + + // initialize file descriptor + /** + * Replace FD_ZERO with memset, because bzero is not available for VxWorks + * User Space Aplications(RTPs). Therefore the macro FD_ZERO does not compile. + */ +#ifndef VXWORKS + FD_ZERO(&_filedescriptor); +#else + memset((char *)(&_filedescriptor), 0, sizeof(*(&_filedescriptor))); +#endif + FD_SET(_udpSock, &_filedescriptor); + + // wait until something was received + int numberActiveFileDescr = select(_udpSock+1, &_filedescriptor,NULL,NULL,&tv); + // 0 indicates a timeout + if(numberActiveFileDescr == 0) + { + printf("The connection has timed out. Timeout is %d\n", _receiveTimeout); + return -1; + } + // a negative value indicates an error + else if(numberActiveFileDescr == -1) + { + printf("An error has occured \n"); + return -1; + } + } + + return recvfrom(_udpSock, buffer, maxSize, 0, (struct sockaddr *)&_controllerAddr, &sockAddrSize); + } + return -1; +} + +//****************************************************************************** +bool UdpConnection::send(const char* buffer, int size) +{ + if ((isOpen()) && (ntohs(_controllerAddr.sin_port) != 0)) + { + int sent = sendto(_udpSock, const_cast(buffer), size, 0, (struct sockaddr *)&_controllerAddr, sizeof(_controllerAddr)); + if (sent == size) + { + return true; + } + } + return false; +} diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/pb_frimessages_callbacks.c b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/pb_frimessages_callbacks.c new file mode 100644 index 00000000..998583c2 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/pb_frimessages_callbacks.c @@ -0,0 +1,267 @@ +/** + + The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + + + \file + \version {2.5} + */ +#include +#include + +#include "pb_frimessages_callbacks.h" +#include "pb_encode.h" +#include "pb_decode.h" + +bool encode_repeatedDouble(pb_ostream_t *stream, const pb_field_t *field, void * const *arg) +{ + size_t i = 0; + + tRepeatedDoubleArguments* arguments = 0; + size_t count = 0; + double* values = 0; + + if (arg == NULL || *arg == NULL) + { + return false; + } + arguments = ((tRepeatedDoubleArguments*) (*arg)); + + count = arguments->max_size; + values = arguments->value; + + for (i = 0; i < count; i++) + { + + if (!pb_encode_tag_for_field(stream, field)) + { + return false; + } + + if (!pb_encode_fixed64(stream, &values[i])) + { + return false; + } + } + return true; +} + +bool decode_repeatedDouble(pb_istream_t *stream, const pb_field_t *field, void **arg) +{ + tRepeatedDoubleArguments* arguments = 0; + size_t i = 0; + double* values = 0; + + if (arg == NULL || *arg == NULL) + { + return false; + } + + arguments = (tRepeatedDoubleArguments*) *arg; + i = arguments->size; + values = arguments->value; + + if (values == NULL) + { + return true; + } + + if (!pb_decode_fixed64(stream, &values[i])) + { + return false; + } + + arguments->size++; + if (arguments->size >= arguments->max_size) + { + arguments->size = 0; + } + return true; +} + +bool encode_repeatedInt(pb_ostream_t *stream, const pb_field_t *field, void * const *arg) +{ + int i = 0; + tRepeatedIntArguments* arguments = 0; + int count = 0; + int64_t* values = 0; + + if (arg == NULL || *arg == NULL) + { + return false; + } + arguments = (tRepeatedIntArguments*) *arg; + count = arguments->max_size; + values = arguments->value; + for (i = 0; i < count; i++) + { + + if (!pb_encode_tag_for_field(stream, field)) + { + return false; + } + if (!pb_encode_varint(stream, values[i])) + { + return false; + } + } + return true; +} + +bool decode_repeatedInt(pb_istream_t *stream, const pb_field_t *field, void **arg) +{ + tRepeatedIntArguments* arguments = 0; + size_t i = 0; + uint64_t* values = 0; + + if (arg == NULL || *arg == NULL) + { + return false; + } + arguments = (tRepeatedIntArguments*) *arg; + + i = arguments->size; + values = (uint64_t*) arguments->value; + if (values == NULL) + { + return true; + } + + if (!pb_decode_varint(stream, &values[i])) + { + return false; + } + + arguments->size++; + if (arguments->size >= arguments->max_size) + { + arguments->size = 0; + } + return true; +} + +void map_repeatedDouble(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedDoubleArguments *arg) +{ + // IMPORTANT: the callbacks are stored in a union, therefor a message object + // must be exclusive defined for transmission or reception + if (dir == FRI_MANAGER_NANOPB_ENCODE) + { + values->funcs.encode = &encode_repeatedDouble; + } + else + { + values->funcs.decode = &decode_repeatedDouble; + } + // map the local container data to the message data fields + arg->max_size = numDOF; + arg->size = 0; + if (numDOF > 0) + { + arg->value = (double*) malloc(numDOF * sizeof(double)); + + } + values->arg = arg; +} + +void map_repeatedInt(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedIntArguments *arg) +{ + // IMPORTANT: the callbacks are stored in a union, therefor a message object + // must be exclusive defined for transmission or reception + if (dir == FRI_MANAGER_NANOPB_ENCODE) + { + // set the encode callback function + values->funcs.encode = &encode_repeatedInt; + } + else + { + // set the decode callback function + values->funcs.decode = &decode_repeatedInt; + } + // map the robot drive state from the container to message field + arg->max_size = numDOF; + arg->size = 0; + if (numDOF > 0) + { + arg->value = (int64_t*) malloc(numDOF * sizeof(int64_t)); + + } + values->arg = arg; +} + +void init_repeatedDouble(tRepeatedDoubleArguments *arg) +{ + arg->size = 0; + arg->max_size = 0; + arg->value = NULL; +} + +void init_repeatedInt(tRepeatedIntArguments *arg) +{ + arg->size = 0; + arg->max_size = 0; + arg->value = NULL; +} + +void free_repeatedDouble(tRepeatedDoubleArguments *arg) +{ + if (arg->value != NULL) + free(arg->value); +} + +void free_repeatedInt(tRepeatedIntArguments *arg) +{ + if (arg->value != NULL) + free(arg->value); +} diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/pb_frimessages_callbacks.h b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/pb_frimessages_callbacks.h new file mode 100644 index 00000000..fdc1f4e7 --- /dev/null +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/pb_frimessages_callbacks.h @@ -0,0 +1,121 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software “KUKA Sunrise.FRI Client SDK” is targeted to work in +conjunction with the “KUKA Sunrise.FRI” toolkit. +In the following, the term “software” refers to all material directly +belonging to the provided SDK “Software development kit”, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2021 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {2.5} +*/ +#ifndef _pb_frimessages_callbacks_H +#define _pb_frimessages_callbacks_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "pb.h" +#include "FRIMessages.pb.h" + +/** container for repeated double elements */ +typedef struct repeatedDoubleArguments { + size_t size; + size_t max_size; + double* value; +} tRepeatedDoubleArguments; + +/** container for repeated integer elements */ +typedef struct repeatedIntArguments { + size_t size; + size_t max_size; + int64_t* value; +} tRepeatedIntArguments; + +/** enumeration for direction (encoding/decoding) */ +typedef enum DIRECTION { + FRI_MANAGER_NANOPB_DECODE = 0, //!< Argument um eine + FRI_MANAGER_NANOPB_ENCODE = 1 //!< +} eNanopbCallbackDirection; + + +bool encode_repeatedDouble(pb_ostream_t *stream, const pb_field_t *field, + void * const *arg); + +bool decode_repeatedDouble(pb_istream_t *stream, const pb_field_t *field, + void **arg); + +bool encode_repeatedInt(pb_ostream_t *stream, const pb_field_t *field, + void * const *arg); + +bool decode_repeatedInt(pb_istream_t *stream, const pb_field_t *field, + void **arg); + +void map_repeatedDouble(eNanopbCallbackDirection dir, int numDOF, + pb_callback_t *values, tRepeatedDoubleArguments *arg); + +void map_repeatedInt(eNanopbCallbackDirection dir, int numDOF, + pb_callback_t *values, tRepeatedIntArguments *arg); + +void init_repeatedDouble(tRepeatedDoubleArguments *arg); + +void init_repeatedInt(tRepeatedIntArguments *arg); + +void free_repeatedDouble(tRepeatedDoubleArguments *arg); + +void free_repeatedInt(tRepeatedIntArguments *arg); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index dd3cbfbc..270c2489 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -42,10 +42,11 @@ CallbackReturn KukaFRIHardwareInterface::on_init( hw_torque_states_.resize(info_.joints.size()); hw_ext_torque_states_.resize(info_.joints.size()); hw_torque_commands_.resize(info_.joints.size()); +#ifdef FRI_V2_5 hw_cart_pose_states_.resize( 7); // it's always 7 dof: position x,y,z; orientation quaternion qx,qy,qz,qw hw_cart_pose_commands_.resize(7); - +#endif hw_wrench_commands_.resize(6); // it's always 6 dof: force x,y,z; torque x,y,z hw_cart_stiffness_commands_.resize(6, 150); hw_cart_damping_commands_.resize(6, 0.15); @@ -210,6 +211,7 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta std::vector(DOF, 0.0), std::vector(DOF, 0.0)); fri_connection_->setClientCommandMode(ClientCommandModeID::TORQUE_COMMAND_MODE); break; +#ifdef FRI_V2_5 //only available in FRI version 2.5 case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL: fri_connection_->setPositionControlMode(); fri_connection_->setClientCommandMode(ClientCommandModeID::CARTESIAN_POSE_COMMAND_MODE); @@ -219,6 +221,7 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta hw_cart_stiffness_commands_, hw_cart_damping_commands_); fri_connection_->setClientCommandMode(ClientCommandModeID::CARTESIAN_POSE_COMMAND_MODE); break; +#endif case kuka_drivers_core::ControlMode::WRENCH_CONTROL: fri_connection_->setCartesianImpedanceControlMode( hw_cart_stiffness_commands_, hw_cart_damping_commands_); @@ -269,7 +272,9 @@ void KukaFRIHardwareInterface::waitForCommand() // Therefore the control signal should not be modified in this callback // TODO(Svastits): check for torque/impedance mode, where state can change hw_position_commands_ = hw_position_states_; +#ifdef FRI_V2_5 hw_cart_pose_commands_ = hw_cart_pose_states_; +#endif rclcpp::Time stamp = ros_clock_.now(); updateCommand(stamp); } @@ -297,8 +302,10 @@ hardware_interface::return_type KukaFRIHardwareInterface::read( const double * external_torque = robotState().getExternalTorque(); hw_ext_torque_states_.assign( external_torque, external_torque + KUKA::FRI::LBRState::NUMBER_OF_JOINTS); +#ifdef FRI_V2_5 const double * cart_pose = robotState().getMeasuredCartesianPose(); hw_cart_pose_states_.assign(cart_pose, cart_pose + KUKA::FRI::LBRState::NUMBER_OF_JOINTS); +#endif robot_state_.tracking_performance_ = robotState().getTrackingPerformance(); robot_state_.session_state_ = robotState().getSessionState(); robot_state_.connection_quality_ = robotState().getConnectionQuality(); @@ -378,6 +385,7 @@ void KukaFRIHardwareInterface::updateCommand(const rclcpp::Time &) robotCommand().setTorque(joint_torques_); break; } +#ifdef FRI_V2_5 case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL: [[fallthrough]]; case kuka_drivers_core::ControlMode::CARTESIAN_IMPEDANCE_CONTROL: @@ -392,6 +400,7 @@ void KukaFRIHardwareInterface::updateCommand(const rclcpp::Time &) break; } +#endif case kuka_drivers_core::ControlMode::WRENCH_CONTROL: { const double * wrench_efforts = hw_wrench_commands_.data(); @@ -463,6 +472,7 @@ std::vector KukaFRIHardwareInterface::export state_interfaces.emplace_back( info_.joints[i].name, hardware_interface::HW_IF_EXTERNAL_TORQUE, &hw_ext_torque_states_[i]); } +#ifdef FRI_V2_5 std::vector cart_joints_list = { hardware_interface::HW_IF_X, hardware_interface::HW_IF_Y, hardware_interface::HW_IF_Z, hardware_interface::HW_IF_QW, hardware_interface::HW_IF_QX, hardware_interface::HW_IF_QY, @@ -473,7 +483,7 @@ std::vector KukaFRIHardwareInterface::export std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + std::string(cart_joints_list[i]), hardware_interface::HW_IF_POSITION, &hw_cart_pose_states_[i]); } - +#endif state_interfaces.emplace_back( hardware_interface::STATE_PREFIX, hardware_interface::SERVER_STATE, &server_state_); return state_interfaces; @@ -510,6 +520,7 @@ KukaFRIHardwareInterface::export_command_interfaces() command_interfaces.emplace_back( info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_torque_commands_[i]); } +#ifdef FRI_V2_5 std::vector cart_joints_list = { hardware_interface::HW_IF_X, hardware_interface::HW_IF_Y, hardware_interface::HW_IF_Z, hardware_interface::HW_IF_QW, hardware_interface::HW_IF_QX, hardware_interface::HW_IF_QY, @@ -521,6 +532,7 @@ KukaFRIHardwareInterface::export_command_interfaces() std::string(hardware_interface::HW_IF_CART_PREFIX) + "/" + std::string(cart_joints_list[i]), hardware_interface::HW_IF_POSITION, &hw_cart_pose_commands_[i]); } +#endif std::vector cart_effort_list = { hardware_interface::HW_IF_X, hardware_interface::HW_IF_Y, hardware_interface::HW_IF_Z, hardware_interface::HW_IF_A, hardware_interface::HW_IF_B, hardware_interface::HW_IF_C}; diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index 93760bf5..24298d1a 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -116,6 +116,7 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ return this->onControllerNameChangeRequest( controller_name, kuka_drivers_core::ControllerType::WRENCH_CONTROLLER_TYPE); }); +#ifdef FRI_V2_5 registerParameter( "cart_pose_controller_name", "", kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const std::string & controller_name) @@ -123,7 +124,7 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ return this->onControllerNameChangeRequest( controller_name, kuka_drivers_core::ControllerType::CARTESIAN_POSITION_CONTROLLER_TYPE); }); - +#endif registerParameter>( "joint_stiffness", joint_stiffness_, kuka_drivers_core::ParameterSetAccessRights{false, false}, [this](const std::vector & joint_stiffness) @@ -162,6 +163,9 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)) { RCLCPP_ERROR(get_logger(), "Could not configure hardware interface"); +#ifdef FRI_V2_5 + RCLCPP_ERROR(get_logger(), "FRI VERSION 2.5"); +#endif return FAILURE; } @@ -311,10 +315,12 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) break; case kuka_drivers_core::ControlMode::JOINT_IMPEDANCE_CONTROL: break; +#ifdef FRI_V2_5 case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL: break; case kuka_drivers_core::ControlMode::CARTESIAN_IMPEDANCE_CONTROL: break; +#endif case kuka_drivers_core::ControlMode::WRENCH_CONTROL: [[fallthrough]]; case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: @@ -421,9 +427,11 @@ bool RobotManagerNode::onControllerNameChangeRequest( case kuka_drivers_core::ControllerType::TORQUE_CONTROLLER_TYPE: joint_torque_controller_name_ = controller_name; break; +#ifdef FRI_V2_5 case kuka_drivers_core::ControllerType::CARTESIAN_POSITION_CONTROLLER_TYPE: cart_pose_controller_name_ = controller_name; break; +#endif case kuka_drivers_core::ControllerType::WRENCH_CONTROLLER_TYPE: wrench_controller_name_ = controller_name; break; @@ -447,10 +455,12 @@ std::string RobotManagerNode::GetControllerName() const return joint_pos_controller_name_; case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: return joint_torque_controller_name_; +#ifdef FRI_V2_5 case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL: return cart_pose_controller_name_; case kuka_drivers_core::ControlMode::CARTESIAN_IMPEDANCE_CONTROL: return cart_pose_controller_name_; +#endif case kuka_drivers_core::ControlMode::WRENCH_CONTROL: return wrench_controller_name_; default: From cae6ee2280ec9748ef2313c4b961ffca7f0ba58d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Tue, 22 Oct 2024 16:04:46 +0200 Subject: [PATCH 48/49] wiki, format --- doc/wiki/3_Sunrise_FRI.md | 2 +- doc/wiki/Home.md | 3 +- .../fri_client_sdk/friClientApplication.h | 39 +++-- .../FRI_v2_5/fri_client_sdk/friClientIf.h | 64 ++++---- .../FRI_v2_5/fri_client_sdk/friConnectionIf.h | 46 +++--- .../FRI_v2_5/fri_client_sdk/friDataHelper.h | 10 +- .../FRI_v2_5/fri_client_sdk/friException.h | 54 +++---- .../FRI_v2_5/fri_client_sdk/friLBRClient.h | 54 +++---- .../FRI_v2_5/fri_client_sdk/friLBRCommand.h | 60 +++---- .../FRI_v2_5/fri_client_sdk/friLBRState.h | 102 ++++++------ .../fri_client_sdk/friTransformationClient.h | 148 +++++++++--------- .../fri_client_sdk/friUdpConnection.h | 56 +++---- .../robot_manager_node.hpp | 2 +- .../v2_5/src/application/ROS2_Control.java | 2 +- .../v2_5/src/ros2/modules/FRIManager.java | 6 +- .../v2_5/src/ros2/modules/ROS2Connection.java | 2 +- ...ianImpedanceControlModeExternalizable.java | 16 +- .../ros2/serialization/ControlModeParams.java | 6 +- .../FRI_v2_5/fri_client_sdk/FRIMessages.pb.c | 1 - .../FRI_v2_5/fri_client_sdk/FRIMessages.pb.h | 62 ++++---- .../fri_client_sdk/friClientApplication.cpp | 47 +++--- .../FRI_v2_5/fri_client_sdk/friClientData.h | 66 ++++---- .../friCommandMessageEncoder.cpp | 16 +- .../fri_client_sdk/friCommandMessageEncoder.h | 34 ++-- .../FRI_v2_5/fri_client_sdk/friDataHelper.cpp | 14 +- .../FRI_v2_5/fri_client_sdk/friLBRClient.cpp | 19 ++- .../FRI_v2_5/fri_client_sdk/friLBRCommand.cpp | 16 +- .../FRI_v2_5/fri_client_sdk/friLBRState.cpp | 10 +- .../friMonitoringMessageDecoder.cpp | 34 ++-- .../friMonitoringMessageDecoder.h | 32 ++-- .../friTransformationClient.cpp | 20 +-- .../fri_client_sdk/friUdpConnection.cpp | 38 ++--- .../fri_client_sdk/pb_frimessages_callbacks.c | 16 +- .../fri_client_sdk/pb_frimessages_callbacks.h | 14 +- .../src/hardware_interface.cpp | 4 +- 35 files changed, 556 insertions(+), 559 deletions(-) diff --git a/doc/wiki/3_Sunrise_FRI.md b/doc/wiki/3_Sunrise_FRI.md index e9f73059..1c9b30b7 100644 --- a/doc/wiki/3_Sunrise_FRI.md +++ b/doc/wiki/3_Sunrise_FRI.md @@ -5,7 +5,7 @@ #### Client side - It is recommended to use the driver on a real-time capable client machine (further information about setting up the PREEMPT_RT patch can be found [here](https://github.com/kroshu/kuka_drivers/wiki/5_Realtime)). - Set a fixed IP in the subnet of the controller for the real-time machine. - +- If you are using FRI version 1.15 set the `WITH_FRI_VERSION_2_5` parameter in the `CMakeLists.txt` to `OFF` #### Controller side - Upload the robot application under `robot_application/src` to the controller using Sunrise Workbench diff --git a/doc/wiki/Home.md b/doc/wiki/Home.md index 30b3224c..f651c0aa 100644 --- a/doc/wiki/Home.md +++ b/doc/wiki/Home.md @@ -83,9 +83,10 @@ The following table shows the supported features and control modes of each drive |OS | Joint position control | Joint impedance control | Joint velocity control | Joint torque control | Cartesian position control | Cartesian impedance control | Cartesian velocity control | Wrench control| I/O control| |---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:| |KSS| ✓ | ✗ | ✗ | ✗ | | ✗ | ✗ | ✗ | | -|Sunrise| ✓ | ✓ | ✗ | ✓ |✓| | ✗ |✓| | +|Sunrise| ✓ | ✓ | ✗ | ✓ | ✓* | ✓* | ✗ |✓| | |iiQKA| ✓ | ✓ | ✗ | ✓ | ✗ | ✗ | ✗ | ✗ | ✗ | +\*=Only supported with Sunrise 2.4 and FRI 2.5 ## Additional packages The repository contains a few other packages aside from the 3 drivers: diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friClientApplication.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friClientApplication.h index 5a61ebd0..47573bff 100644 --- a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friClientApplication.h +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friClientApplication.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -76,32 +76,32 @@ namespace FRI /** * \brief FRI client application class. - * + * * A client application takes an instance of the IConnection interface and * an instance of an IClient interface to provide the functionality - * needed to set up an FRI client application. It can be used to easily + * needed to set up an FRI client application. It can be used to easily * integrate the FRI client code within other applications. * The algorithmic functionality of an FRI client application is implemented * using the IClient interface. */ class ClientApplication { - + public: - + /** * \brief Constructor without transformation client. - * + * * This constructor takes an instance of the IConnection interface and * an instance of the IClient interface as parameters. * @param connection FRI connection class * @param client FRI client class */ ClientApplication(IConnection& connection, IClient& client); - + /** * \brief Constructor with transformation client. - * + * * This constructor takes an instance of the IConnection interface and * an instance of the IClient interface and an instance of a * TransformationClient as parameters. @@ -113,33 +113,33 @@ namespace FRI /** \brief Destructor. */ ~ClientApplication(); - + /** * \brief Connect the FRI client application with a KUKA Sunrise controller. - * + * * @param port The port ID * @param remoteHost The address of the remote host * @return True if connection was established */ bool connect(int port, const char *remoteHost = NULL); - + /** * \brief Disconnect the FRI client application from a KUKA Sunrise controller. */ void disconnect(); - + /** * \brief Run a single processing step. - * + * * The processing step consists of receiving a new FRI monitoring message, - * calling the corresponding client callback and sending the resulting + * calling the corresponding client callback and sending the resulting * FRI command message back to the KUKA Sunrise controller. * @return True if all of the substeps succeeded. */ bool step(); protected: - + IConnection& _connection; //!< connection interface IClient* _robotClient; //!< robot client interface TransformationClient* _trafoClient; //!< transformation client interface @@ -152,4 +152,3 @@ namespace FRI #endif // _KUKA_FRI_CLIENT_APPLICATION_H - diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friClientIf.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friClientIf.h index 1a2abda9..f15b2d98 100644 --- a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friClientIf.h +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friClientIf.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -71,7 +71,7 @@ namespace FRI // forward declarations struct ClientData; - + /** \brief Enumeration of the FRI session state. */ enum ESessionState @@ -116,7 +116,7 @@ namespace FRI TRANSITIONING = 1, //!< drive is in a transitioning state (before or after motion) ACTIVE = 2 //!< drive is being actively commanded }; - + /** \brief Enumeration of control mode. */ enum EControlMode { @@ -125,84 +125,84 @@ namespace FRI JOINT_IMP_CONTROL_MODE = 2, //!< joint impedance control mode NO_CONTROL = 3 //!< drives are not used }; - - + + /** \brief Enumeration of the client command mode. */ enum EClientCommandMode { NO_COMMAND_MODE = 0, //!< no client command mode available JOINT_POSITION = 1, //!< commanding joint positions by the client WRENCH = 2, //!< commanding wrenches and joint positions by the client - TORQUE = 3, //!< commanding joint torques and joint positions by the client - CARTESIAN_POSE = 4 //!< commanding Cartesian poses by the client + TORQUE = 3, //!< commanding joint torques and joint positions by the client + CARTESIAN_POSE = 4 //!< commanding Cartesian poses by the client }; - + /** \brief Enumeration of the overlay type. */ enum EOverlayType { NO_OVERLAY = 0, //!< no overlay type available - JOINT = 1, //!< joint overlay + JOINT = 1, //!< joint overlay CARTESIAN = 2 //!< cartesian overlay }; - + /** \brief Enumeration of redundancy strategies. */ enum ERedundancyStrategy { E1 = 0, //!< E1 redundancy strategy NO_STRATEGY = 4 //!< No redundancy strategy }; - + /** - * \brief FRI client interface. - * - * This is the callback interface that should be implemented by FRI clients. + * \brief FRI client interface. + * + * This is the callback interface that should be implemented by FRI clients. * Callbacks are automatically called by the client application * (ClientApplication) whenever new FRI messages arrive. */ class IClient { friend class ClientApplication; - + public: - + /** \brief Virtual destructor. */ virtual ~IClient() {} - - /** + + /** * \brief Callback that is called whenever the FRI session state changes. - * + * * @param oldState previous FRI session state * @param newState current FRI session state */ virtual void onStateChange(ESessionState oldState, ESessionState newState) = 0; - + /** * \brief Callback for the FRI session states 'Monitoring Wait' and 'Monitoring Ready'. */ virtual void monitor() = 0; - + /** * \brief Callback for the FRI session state 'Commanding Wait'. */ virtual void waitForCommand() = 0; - + /** * \brief Callback for the FRI session state 'Commanding'. */ virtual void command() = 0; - + protected: - + /** * \brief Method to create and initialize the client data structure (used internally). - * + * * @return newly allocated client data structure */ virtual ClientData* createData() = 0; - - + + }; - + } } diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friConnectionIf.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friConnectionIf.h index 0cd10666..e1cd32fc 100644 --- a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friConnectionIf.h +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friConnectionIf.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -66,63 +66,63 @@ namespace KUKA { namespace FRI { - + /** - * \brief FRI connection interface. - * + * \brief FRI connection interface. + * * Connections to the KUKA Sunrise controller have to be implemented using * this interface. */ class IConnection { - + public: - + /** \brief Virtual destructor. */ virtual ~IConnection() {} - + /** * \brief Open a connection to the KUKA Sunrise controller. - * + * * @param port The port ID * @param remoteHost The address of the remote host * @return True if connection was established */ virtual bool open(int port, const char *remoteHost) = 0; - + /** * \brief Close a connection to the KUKA Sunrise controller. */ virtual void close() = 0; - + /** * \brief Checks whether a connection to the KUKA Sunrise controller is established. - * + * * @return True if connection is established */ virtual bool isOpen() const = 0; - + /** - * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. - * + * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. + * * This method blocks until a new message arrives. * @param buffer Pointer to the allocated buffer that will hold the FRI message * @param maxSize Size in bytes of the allocated buffer * @return Number of bytes received (0 when connection was terminated, negative in case of errors) */ virtual int receive(char *buffer, int maxSize) = 0; - + /** * \brief Send a new FRI command message to the KUKA Sunrise controller. - * + * * @param buffer Pointer to the buffer holding the FRI message * @param size Size in bytes of the message to be send * @return True if successful */ - virtual bool send(const char* buffer, int size) = 0; - + virtual bool send(const char* buffer, int size) = 0; + }; - + } } diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friDataHelper.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friDataHelper.h index e287ebbc..50e05c4b 100644 --- a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friDataHelper.h +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friDataHelper.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friException.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friException.h index ebf33c99..6c9b2713 100644 --- a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friException.h +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friException.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -67,39 +67,39 @@ namespace KUKA { namespace FRI { - + /** * \brief Standard exception for the FRI Client - * - * \note For realtime considerations the internal message buffer is static. - * So don't use this exception in more than one thread per process. + * + * \note For realtime considerations the internal message buffer is static. + * So don't use this exception in more than one thread per process. */ class FRIException { - + public: - + /** * \brief FRIException Constructor - * + * * @param message Error message */ - FRIException(const char* message) - { + FRIException(const char* message) + { strncpy(_buffer, message, sizeof(_buffer) - 1); _buffer[sizeof(_buffer) - 1] = 0; // ensure string termination printf("FRIException: "); printf(_buffer); printf("\n"); } - + /** * \brief FRIException Constructor - * - * @param message Error message which may contain one "%s" parameter + * + * @param message Error message which may contain one "%s" parameter * @param param1 First format parameter for parameter message. */ - FRIException(const char* message, const char* param1) + FRIException(const char* message, const char* param1) { #ifdef _MSC_VER _snprintf( // visual studio compilers (up to VS 2013) only know this method @@ -111,15 +111,15 @@ namespace FRI printf(_buffer); printf("\n"); } - + /** * \brief FRIException Constructor - * - * @param message Error message which may contain two "%s" parameter + * + * @param message Error message which may contain two "%s" parameter * @param param1 First format parameter for parameter message. * @param param2 Second format parameter for parameter message. */ - FRIException(const char* message, const char* param1, const char* param2) + FRIException(const char* message, const char* param1, const char* param2) { #ifdef _MSC_VER _snprintf( // visual studio compilers (up to VS 2013) only know this method @@ -131,21 +131,21 @@ namespace FRI printf(_buffer); printf("\n"); } - + /** * \brief Get error string. * @return Error message stored in the exception. */ const char* getErrorMessage() const { return _buffer; } - + /** \brief Virtual destructor. */ virtual ~FRIException() {} - + protected: static char _buffer[1024]; - + }; - + } } diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRClient.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRClient.h index a1e7bce9..49b9239e 100644 --- a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRClient.h +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRClient.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -72,72 +72,72 @@ namespace FRI /** * \brief Implementation of the IClient interface for the KUKA LBR (lightweight) robots. - * - * Provides access to the current LBR state and the possibility to send new - * commands to the LBR. + * + * Provides access to the current LBR state and the possibility to send new + * commands to the LBR. */ class LBRClient : public IClient { - + public: - + /** \brief Constructor. */ LBRClient(); - + /** \brief Destructor. */ ~LBRClient(); - - /** + + /** * \brief Callback that is called whenever the FRI session state changes. - * + * * @param oldState previous FRI session state * @param newState current FRI session state */ virtual void onStateChange(ESessionState oldState, ESessionState newState); - + /** * \brief Callback for the FRI session states 'Monitoring Wait' and 'Monitoring Ready'. */ virtual void monitor(); - + /** * \brief Callback for the FRI session state 'Commanding Wait'. */ virtual void waitForCommand(); - + /** * \brief Callback for the FRI session state 'Commanding'. */ virtual void command(); - + /** * \brief Provide read access to the current robot state. - * + * * @return Reference to the LBRState instance */ const LBRState& robotState() const { return _robotState; } - + /** * \brief Provide write access to the robot commands. - * + * * @return Reference to the LBRCommand instance */ LBRCommand& robotCommand() { return _robotCommand; } - + private: - + LBRState _robotState; //!< wrapper class for the FRI monitoring message LBRCommand _robotCommand; //!< wrapper class for the FRI command message - + /** * \brief Method to create and initialize the client data structure (used internally). - * + * * @return newly allocated client data structure */ virtual ClientData* createData(); - + }; - + } } diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRCommand.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRCommand.h index 190771e4..c210f716 100644 --- a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRCommand.h +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRCommand.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -72,7 +72,7 @@ namespace FRI { /** - * \brief Wrapper class for the FRI command message for a KUKA LBR (leightweight) robot. + * \brief Wrapper class for the FRI command message for a KUKA LBR (lightweight) robot. */ class LBRCommand { @@ -82,7 +82,7 @@ class LBRCommand /** * \brief Set the joint positions for the current interpolation step. - * + * * This method is only effective when the client is in a commanding state. * @param values Array with the new joint positions (in rad) */ @@ -90,30 +90,30 @@ class LBRCommand /** * \brief Set the applied wrench vector of the current interpolation step. - * + * * The wrench vector consists of: * [F_x, F_y, F_z, tau_A, tau_B, tau_C] - * - * F ... forces (in N) applied along the Cartesian axes of the + * + * F ... forces (in N) applied along the Cartesian axes of the * currently used motion center. - * tau ... torques (in Nm) applied along the orientation angles + * tau ... torques (in Nm) applied along the orientation angles * (Euler angles A, B, C) of the currently used motion center. - * + * * This method is only effective when the client is in a commanding state. * The ControlMode of the robot has to be Cartesian impedance control mode. The * Client Command Mode has to be wrench. - * + * * @param wrench Applied Cartesian wrench vector. */ void setWrench(const double* wrench); /** * \brief Set the applied joint torques for the current interpolation step. - * + * * This method is only effective when the client is in a commanding state. * The ControlMode of the robot has to be joint impedance control mode. The * Client Command Mode has to be torque. - * + * * @param torques Array with the applied torque values (in Nm) */ void setTorque(const double* torques); @@ -121,7 +121,7 @@ class LBRCommand /** * \brief Set the Cartesian pose for the current interpolation step. * The pose describes the configured TCP relative to the configured base frame. - * + * * The quaternion vector consists of: [t_x, t_y, t_z, q_w, q_x, q_y, q_z], * where the first three values describe the translation t as a regular 3-dim * vector, while the last four values describe the rotation q as an unit quaternion. @@ -133,9 +133,9 @@ class LBRCommand * [ q_x ] = [ sin (phi/2) * v_x ] * [ q_y ] = [ sin (phi/2) * v_y ] * [ q_z ] = [ sin (phi/2) * v_z ] - * + * * Setting a redundancy value is optional. If no value is provided, the interpolated - * redundancy value is used. So far, only the E1 redundancy strategy is provided. + * redundancy value is used. So far, only the E1 redundancy strategy is provided. * * This method is only effective when the client is in a commanding state. * @@ -151,9 +151,9 @@ class LBRCommand * * The first 3 columns represent a rotational matrix and the 4th column a 3-dim * translation vector for directions x, y, z (in mm). - * + * * Setting a redundancy value is optional. If no value is provided, the interpolated - * redundancy value is used. So far, only the E1 redundancy strategy is provided. + * redundancy value is used. So far, only the E1 redundancy strategy is provided. * * @param cartesianPoseAsMatrix 2-dim double array where the requested 3x4 matrix * should be stored @@ -164,37 +164,37 @@ class LBRCommand /** * \brief Set boolean output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Boolean value to set. + * @param value Boolean value to set. */ void setBooleanIOValue(const char* name, const bool value); /** * \brief Set digital output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Digital value to set. + * @param value Digital value to set. */ void setDigitalIOValue(const char* name, const unsigned long long value); /** * \brief Set analog output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Analog value to set. + * @param value Analog value to set. */ void setAnalogIOValue(const char* name, const double value); protected: - static const int LBRCOMMANDMESSAGEID = 0x34001; //!< type identifier for the FRI command message corresponding to a KUKA LBR robot + static const int LBRCOMMANDMESSAGEID = 0x34001; //!< type identifier for the FRI command message corresponding to a KUKA LBR robot FRICommandMessage* _cmdMessage; //!< FRI command message (protobuf struct) FRIMonitoringMessage* _monMessage; //!< FRI monitoring message (protobuf struct) diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRState.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRState.h index c605c014..2c4433b2 100644 --- a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRState.h +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friLBRState.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -82,15 +82,15 @@ class LBRState enum { - NUMBER_OF_JOINTS = 7 //!< number of joints of the KUKA LBR robot + NUMBER_OF_JOINTS = 7 //!< number of joints of the KUKA LBR robot }; LBRState(); /** - * \brief Get the sample time in seconds. - * - * This is the period in which the KUKA Sunrise controller is sending + * \brief Get the sample time in seconds. + * + * This is the period in which the KUKA Sunrise controller is sending * FRI packets. * @return sample time in seconds */ @@ -98,35 +98,35 @@ class LBRState /** * \brief Get the current FRI session state. - * + * * @return current FRI session state */ ESessionState getSessionState() const; /** * \brief Get the current FRI connection quality. - * + * * @return current FRI connection quality */ EConnectionQuality getConnectionQuality() const; /** * \brief Get the current safety state of the KUKA Sunrise controller. - * + * * @return current safety state */ ESafetyState getSafetyState() const; /** * \brief Get the current operation mode of the KUKA Sunrise controller. - * + * * @return current operation mode */ EOperationMode getOperationMode() const; /** * \brief Get the accumulated drive state over all drives of the KUKA LBR controller. - * + * * If the drive states differ between drives, the following rule applies: * 1) The drive state is OFF if all drives are OFF. * 2) The drive state is ACTIVE if all drives are ACTIVE. @@ -137,93 +137,93 @@ class LBRState /** * \brief Get the client command mode specified by the client. - * + * * @return the client command mode specified by the client. */ EClientCommandMode getClientCommandMode() const; /** * \brief Get the overlay type specified by the client. - * + * * @return the overlay type specified by the client. */ EOverlayType getOverlayType() const; /** * \brief Get the control mode of the KUKA LBR robot. - * + * * @return current control mode of the KUKA LBR robot. */ EControlMode getControlMode() const; - /** - * \brief Get the timestamp of the current robot state in Unix time. - * + /** + * \brief Get the timestamp of the current robot state in Unix time. + * * This method returns the seconds since 0:00, January 1st, 1970 (UTC). * Use getTimestampNanoSec() to increase your timestamp resolution when - * seconds are insufficient. + * seconds are insufficient. * @return timestamp encoded as Unix time (seconds) */ unsigned int getTimestampSec() const; - /** + /** * \brief Get the nanoseconds elapsed since the last second (in Unix time). - * - * This method complements getTimestampSec() to get a high resolution - * timestamp. + * + * This method complements getTimestampSec() to get a high resolution + * timestamp. * @return timestamp encoded as Unix time (nanoseconds part) */ unsigned int getTimestampNanoSec() const; /** * \brief Get the currently measured joint positions of the robot. - * + * * @return array of the measured joint positions in radians */ const double* getMeasuredJointPosition() const; /** * \brief Get the currently measured joint torques of the robot. - * + * * @return array of the measured torques in Nm */ const double* getMeasuredTorque() const; /** * \brief Get the last commanded joint torques of the robot. - * + * * @return array of the commanded torques in Nm */ const double* getCommandedTorque() const; /** * \brief Get the currently measured external joint torques of the robot. - * + * * The external torques corresponds to the measured torques when removing - * the torques induced by the robot itself. + * the torques induced by the robot itself. * @return array of the external torques in Nm */ const double* getExternalTorque() const; /** * \brief Get the joint positions commanded by the interpolator. - * + * * When commanding a motion overlay in your robot application, this method * will give access to the joint positions currently commanded by the * motion interpolator. * @throw FRIException This method will throw a FRIException if no FRI Joint Overlay is active. - * @return array of the ipo joint positions in radians + * @return array of the ipo joint positions in radians */ const double* getIpoJointPosition() const; /** * \brief Get an indicator for the current tracking performance of the commanded robot. - * + * * The tracking performance is an indicator on how well the commanded robot * is able to follow the commands of the FRI client. The best possible value * 1.0 is reached when the robot executes the given commands instantaneously. - * The tracking performance drops towards 0 when latencies are induced, - * e.g. when the commanded velocity, acceleration or jerk exceeds the + * The tracking performance drops towards 0 when latencies are induced, + * e.g. when the commanded velocity, acceleration or jerk exceeds the * capabilities of the robot. * The tracking performance will always be 0 when the session state does * not equal COMMANDING_ACTIVE. @@ -235,7 +235,7 @@ class LBRState * \brief Get boolean IO value. * * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). * @return Returns IO's boolean value. */ bool getBooleanIOValue(const char* name) const; @@ -244,7 +244,7 @@ class LBRState * \brief Get digital IO value. * * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). * @return Returns IO's digital value. */ unsigned long long getDigitalIOValue(const char* name) const; @@ -253,7 +253,7 @@ class LBRState * \brief Get analog IO value. * * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). * @return Returns IO's analog value. */ double getAnalogIOValue(const char* name) const; @@ -281,7 +281,7 @@ class LBRState const double* getMeasuredCartesianPose() const; /** - * \brief Get the currently measured Cartesian pose of the robot as a 3x4 transformation matrix. + * \brief Get the currently measured Cartesian pose of the robot as a 3x4 transformation matrix. * The pose describes the robot tcp relative to the base frame. * * The first 3 columns represent a rotational matrix and the 4th column a 3-dim @@ -309,15 +309,15 @@ class LBRState * [ q_x ] = [ sin (phi/2) * v_x ] * [ q_y ] = [ sin (phi/2) * v_y ] * [ q_z ] = [ sin (phi/2) * v_z ] - * + * * @throw FRIException This method will throw a FRIException if no FRI Cartesian Overlay is active. * @return ipo Cartesian pose as 7-dim quaternion transformation (translation in mm) */ const double* getIpoCartesianPose() const; /** - * \brief Get the currently interpolated Cartesian pose of the robot as a 3x4 transformation matrix. - * The pose describes the robot tcp relative to the base frame. + * \brief Get the currently interpolated Cartesian pose of the robot as a 3x4 transformation matrix. + * The pose describes the robot tcp relative to the base frame. * * The first 3 columns represent a rotational matrix and the 4th column a 3-dim * translation vector for directions x, y, z (in mm). @@ -329,37 +329,37 @@ class LBRState /** * \brief Get the currently measured redundancy value. - * + * * So far, only the E1 redundancy strategy is provided, meaning the value represents the joint position * of A3 in radians. - * + * * @param measured redundancy value in radians */ double getMeasuredRedundancyValue() const; - + /** * \brief Get the current redundancy value of the interpolator. - * + * * So far, only the E1 redundancy strategy is provided, meaning the value represents the joint position * of A3 in radians. - * + * * @param redundancy value of the interpolator in radians */ double getIpoRedundancyValue() const; - + /** * \brief Get the redundancy strategy. - * + * * So far, only the E1 redundancy strategy is provided, meaning the value represents the joint position * of A3 in radians. - * + * * @param redundancy strategy */ ERedundancyStrategy getRedundancyStrategy() const; protected: - static const int LBRMONITORMESSAGEID = 0x245142; //!< type identifier for the FRI monitoring message corresponding to a KUKA LBR robot + static const int LBRMONITORMESSAGEID = 0x245142; //!< type identifier for the FRI monitoring message corresponding to a KUKA LBR robot FRIMonitoringMessage* _message; //!< FRI monitoring message (protobuf struct) }; diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friTransformationClient.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friTransformationClient.h index c28a8ef2..6580083a 100644 --- a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friTransformationClient.h +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friTransformationClient.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -73,91 +73,91 @@ namespace FRI struct ClientData; /** - * \brief Abstract FRI transformation client. - * - * A transformation client enables the user to send transformation matrices cyclically to the + * \brief Abstract FRI transformation client. + * + * A transformation client enables the user to send transformation matrices cyclically to the * KUKA Sunrise controller for manipulating the transformations of dynamic frames in the * Sunrise scenegraph. - * Usually, these matrices will be provided by external sensors. + * Usually, these matrices will be provided by external sensors. *
* Custom transformation clients have to be derived from this class and need to * implement the provide() callback. This callback is called once by the * client application whenever a new FRI message arrives. - * + * */ class TransformationClient { - + friend class ClientApplication; - + public: - + /** * \brief Constructor. **/ TransformationClient(); - + /** - * \brief Virtual destructor. + * \brief Virtual destructor. **/ virtual ~TransformationClient(); - - /** - * \brief Callback which is called whenever a new FRI message arrives. + + /** + * \brief Callback which is called whenever a new FRI message arrives. * * In this callback all requested transformations have to be set. - * + * * \see getRequestedTransformationIDs(), setTransformation() */ virtual void provide() = 0; - + /** - * \brief Get the sample time in seconds. - * - * This is the period in which the KUKA Sunrise controller is sending + * \brief Get the sample time in seconds. + * + * This is the period in which the KUKA Sunrise controller is sending * FRI packets. * @return sample time in seconds */ double getSampleTime() const; // sec - + /** * \brief Get the current FRI connection quality. - * + * * @return current FRI connection quality */ - EConnectionQuality getConnectionQuality() const; + EConnectionQuality getConnectionQuality() const; - /** + /** * \brief Returns a vector of identifiers of all requested transformation matrices. - * + * * The custom TransformationClient has to provide data for transformation matrices with these - * identifiers. - * + * identifiers. + * * @return reference to vector of IDs of requested transformations */ const std::vector& getRequestedTransformationIDs() const; - - /** - * \brief Get the timestamp of the current received FRI monitor message in Unix time. - * + + /** + * \brief Get the timestamp of the current received FRI monitor message in Unix time. + * * This method returns the seconds since 0:00, January 1st, 1970 (UTC). * Use getTimestampNanoSec() to increase your timestamp resolution when - * seconds are insufficient. - * + * seconds are insufficient. + * * @return timestamp encoded as Unix time (seconds) */ const unsigned int getTimestampSec() const; - - /** + + /** * \brief Get the nanoseconds elapsed since the last second (in Unix time). - * - * This method complements getTimestampSec() to get a high resolution - * timestamp. - * + * + * This method complements getTimestampSec() to get a high resolution + * timestamp. + * * @return timestamp encoded as Unix time (nanoseconds part) */ const unsigned int getTimestampNanoSec() const; - + /** * \brief Provides a requested transformation as a quaternion transformation vector. * @@ -187,12 +187,12 @@ namespace FRI * @param timeSec Timestamp encoded as Unix time (seconds) * @param timeNanoSec Timestamp encoded as Unix time (nanoseconds part) */ - void setTransformation(const char* transformationID, const double (&transformationQuaternion)[7], + void setTransformation(const char* transformationID, const double (&transformationQuaternion)[7], unsigned int timeSec, unsigned int timeNanoSec); /** * \brief Provides a requested transformation as a homogeneous matrix. - * + * * A transformation matrix has 3x4 elements. It consists of a rotational matrix (3x3 elements) * and a translational vector (3x1 elements). The complete transformation matrix has the * following structure:
@@ -204,88 +204,88 @@ namespace FRI *

* If an update to the last transformation is not yet available when the provide() * callback is executed, the last transformation (including its timestamp) should be - * repeated until a new transformation is available. + * repeated until a new transformation is available. *

* - * @throw FRIException Throws a FRIException if the maximum number of transformations is exceeded. + * @throw FRIException Throws a FRIException if the maximum number of transformations is exceeded. * @param transformationID Identifier string of the transformation matrix * @param transformationMatrix Provided transformation matrix * @param timeSec Timestamp encoded as Unix time (seconds) * @param timeNanoSec Timestamp encoded as Unix time (nanoseconds part) */ - void setTransformation(const char* transformationID, const double (&transformationMatrix)[3][4], + void setTransformation(const char* transformationID, const double (&transformationMatrix)[3][4], unsigned int timeSec, unsigned int timeNanoSec); - + /** * \brief Set boolean output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Boolean value to set. + * @param value Boolean value to set. */ void setBooleanIOValue(const char* name, const bool value); - + /** * \brief Set digital output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Digital value to set. + * @param value Digital value to set. */ void setDigitalIOValue(const char* name, const unsigned long long value); - + /** * \brief Set analog output value. - * - * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. * @throw FRIException May throw a FRIException if the IO is of wrong type, unknown or not an output. * @param name Full name of the IO (Syntax "IOGroupName.IOName"). - * @param value Analog value to set. + * @param value Analog value to set. */ - void setAnalogIOValue(const char* name, const double value); - + void setAnalogIOValue(const char* name, const double value); + /** * \brief Get boolean IO value. * * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). * @return Returns IO's boolean value. */ bool getBooleanIOValue(const char* name) const; - + /** * \brief Get digital IO value. * * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). * @return Returns IO's digital value. */ unsigned long long getDigitalIOValue(const char* name) const; - + /** * \brief Get analog IO value. * * @throw FRIException May throw a FRIException if the IO is of wrong type or unknown. - * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). * @return Returns IO's analog value. */ double getAnalogIOValue(const char* name) const; - - private: - + + private: + ClientData* _data; //!< the client data structure - + /** * \brief Method to link the client data structure (used internally). - * + * * @param clientData the client data structure */ void linkData(ClientData* clientData); - + }; - + } } diff --git a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friUdpConnection.h b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friUdpConnection.h index 04fa98d7..f8172042 100644 --- a/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friUdpConnection.h +++ b/kuka_sunrise_fri_driver/include/FRI_v2_5/fri_client_sdk/friUdpConnection.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -91,71 +91,71 @@ namespace FRI */ class UdpConnection : public IConnection { - + public: - + /** * \brief Constructor with an optional parameter for setting a receive timeout. - * - * @param receiveTimeout Timeout (in ms) for receiving a UDP message (0 = wait forever) + * + * @param receiveTimeout Timeout (in ms) for receiving a UDP message (0 = wait forever) * */ UdpConnection(unsigned int receiveTimeout = 0); - + /** \brief Destructor. */ ~UdpConnection(); - + /** * \brief Open a connection to the KUKA Sunrise controller. - * - * @param port The port ID for the connection - * @param controllerAddress The IPv4 address of the KUKA Sunrise controller. + * + * @param port The port ID for the connection + * @param controllerAddress The IPv4 address of the KUKA Sunrise controller. * If NULL, the FRI Client accepts connections from any - * address. + * address. * @return True if connection was established, false otherwise */ virtual bool open(int port, const char *controllerAddress = NULL); - + /** * \brief Close a connection to the KUKA Sunrise controller. */ virtual void close(); - + /** * \brief Checks whether a connection to the KUKA Sunrise controller is established. - * + * * @return True if connection is established */ virtual bool isOpen() const; - + /** - * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. - * + * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. + * * This method blocks until a new message arrives. * @param buffer Pointer to the allocated buffer that will hold the FRI message * @param maxSize Size in bytes of the allocated buffer - * @return Number of bytes received (0 when connection was terminated, + * @return Number of bytes received (0 when connection was terminated, * negative in case of errors or receive timeout) */ virtual int receive(char *buffer, int maxSize); - + /** * \brief Send a new FRI command message to the KUKA Sunrise controller. - * + * * @param buffer Pointer to the buffer holding the FRI message * @param size Size in bytes of the message to be send * @return True if successful */ virtual bool send(const char* buffer, int size); - + private: - + int _udpSock; //!< UDP socket handle struct sockaddr_in _controllerAddr; //!< the controller's socket address unsigned int _receiveTimeout; fd_set _filedescriptor; - + }; - + } } diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp index ade3fdac..7aab52b7 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp @@ -80,7 +80,7 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode std::string joint_pos_controller_name_; std::string joint_torque_controller_name_; std::string wrench_controller_name_; -#ifdef FRI_V2_5 +#ifdef FRI_V2_5 std::string cart_pose_controller_name_; #endif std::vector joint_stiffness_ = std::vector(7, 100.0); diff --git a/kuka_sunrise_fri_driver/robot_application/v2_5/src/application/ROS2_Control.java b/kuka_sunrise_fri_driver/robot_application/v2_5/src/application/ROS2_Control.java index 9adf72cf..a9e70893 100755 --- a/kuka_sunrise_fri_driver/robot_application/v2_5/src/application/ROS2_Control.java +++ b/kuka_sunrise_fri_driver/robot_application/v2_5/src/application/ROS2_Control.java @@ -32,7 +32,7 @@ public class ROS2_Control extends RoboticsAPIApplication { @Inject private LBR _lbr; - + private TCPConnection _TCPConnection; private ROS2Connection _ROS2Connection; private FRIManager _FRIManager; diff --git a/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/FRIManager.java b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/FRIManager.java index 17cd6dc9..54580ec2 100755 --- a/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/FRIManager.java +++ b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/FRIManager.java @@ -39,10 +39,10 @@ public enum CommandResult{ private ClientCommandMode _clientCommandMode; private IMotionContainer _motionContainer; private FRIMotionErrorHandler _friMotionErrorHandler = new FRIMotionErrorHandler(); - + private static double[] stiffness_ = new double[7]; - - + + public FRIManager(LBR lbr, IApplicationControl applicationControl){ _currentState = new InactiveState(); _lbr = lbr; diff --git a/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/ROS2Connection.java b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/ROS2Connection.java index d5326d92..ee13136a 100755 --- a/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/ROS2Connection.java +++ b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/modules/ROS2Connection.java @@ -95,7 +95,7 @@ private enum ControlModeID{ POSITION( (byte)1), JOINT_IMPEDANCE((byte)2), CARTESIAN_IMPEDANCE((byte)3); - + public final byte value; ControlModeID(byte value){ diff --git a/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java index 32dca2ce..f49ddef9 100644 --- a/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java +++ b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/CartesianImpedanceControlModeExternalizable.java @@ -11,21 +11,21 @@ public class CartesianImpedanceControlModeExternalizable extends CartesianImpedanceControlMode implements Externalizable{ public final static int length = 96; - - + + public CartesianImpedanceControlModeExternalizable(CartesianImpedanceControlMode other){ super(other); } - + public CartesianImpedanceControlModeExternalizable(){ super(); } - + public IMotionControlMode toControlMode(){ CartesianImpedanceControlMode controlMode = new CartesianImpedanceControlMode((CartesianImpedanceControlMode)this); return (IMotionControlMode)controlMode; } - + @Override public void writeExternal(ObjectOutput out) throws IOException { for(double cartStiffness : getStiffness()){ @@ -39,7 +39,7 @@ public void writeExternal(ObjectOutput out) throws IOException { @Override public void readExternal(ObjectInput in) throws IOException, ClassNotFoundException { - double[] cartStiffness = new double[getStiffness().length]; + double[] cartStiffness = new double[getStiffness().length]; for(int i = 0; i < getStiffness().length; i++){ cartStiffness[i] = in.readDouble(); } @@ -50,7 +50,7 @@ public void readExternal(ObjectInput in) throws IOException, this.parametrize(CartDOF.B).setStiffness(cartStiffness[4]); this.parametrize(CartDOF.C).setStiffness(cartStiffness[5]); - double[] cartDamping = new double[getDamping().length]; + double[] cartDamping = new double[getDamping().length]; for(int i = 0; i < getDamping().length; i++){ cartDamping[i] = in.readDouble(); } @@ -60,7 +60,7 @@ public void readExternal(ObjectInput in) throws IOException, this.parametrize(CartDOF.A).setDamping(cartDamping[3]); this.parametrize(CartDOF.B).setDamping(cartDamping[4]); this.parametrize(CartDOF.C).setDamping(cartDamping[5]); - + } diff --git a/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/ControlModeParams.java b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/ControlModeParams.java index 88f1dc00..6f72b286 100755 --- a/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/ControlModeParams.java +++ b/kuka_sunrise_fri_driver/robot_application/v2_5/src/ros2/serialization/ControlModeParams.java @@ -17,7 +17,7 @@ private enum ControlModeID{ POSITION( (byte)1), JOINT_IMPEDANCE((byte)2), CARTESIAN_IMPEDANCE((byte)3); - + public final byte value; ControlModeID(byte value){ @@ -101,9 +101,9 @@ public JointImpedanceControlModeParams(JointImpedanceControlMode controlMode){ class CartesianImpedanceControlModeParams extends ControlModeParams{ public CartesianImpedanceControlModeParams(){ - + } public CartesianImpedanceControlModeParams(CartesianImpedanceControlMode controlMode){ - + } } diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/FRIMessages.pb.c b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/FRIMessages.pb.c index 8786d741..e421761c 100644 --- a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/FRIMessages.pb.c +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/FRIMessages.pb.c @@ -73,4 +73,3 @@ PB_BIND(FRICommandMessage, FRICommandMessage, 2) */ PB_STATIC_ASSERT(sizeof(double) == 8, DOUBLE_MUST_BE_8_BYTES) #endif - diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/FRIMessages.pb.h b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/FRIMessages.pb.h index d124ca26..9cdb79bc 100644 --- a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/FRIMessages.pb.h +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/FRIMessages.pb.h @@ -10,7 +10,7 @@ #endif /* Enum definitions */ -/* used to display the state of the FRI session +/* used to display the state of the FRI session @KUKA.Internal */ typedef enum _FRISessionState { FRISessionState_IDLE = 0, /* idle state. */ @@ -20,7 +20,7 @@ typedef enum _FRISessionState { FRISessionState_COMMANDING_ACTIVE = 4 /* connected to a running motion - commanding modifies the robot path */ } FRISessionState; -/* Quality of the FRI Connection +/* Quality of the FRI Connection @KUKA.Internal */ typedef enum _FRIConnectionQuality { FRIConnectionQuality_POOR = 0, /* Robot commanding is not possible. Initial value of the connection. */ @@ -29,7 +29,7 @@ typedef enum _FRIConnectionQuality { FRIConnectionQuality_EXCELLENT = 3 /* Robot commanding is possible. */ } FRIConnectionQuality; -/* This enumeration is used to indicate the safety State. +/* This enumeration is used to indicate the safety State. @KUKA.Internal */ typedef enum _SafetyState { SafetyState_NORMAL_OPERATION = 0, @@ -38,7 +38,7 @@ typedef enum _SafetyState { SafetyState_SAFETY_STOP_LEVEL_2 = 3 } SafetyState; -/* This enumeration contains the available operation modes of the controller. +/* This enumeration contains the available operation modes of the controller. @KUKA.Internal */ typedef enum _OperationMode { OperationMode_TEST_MODE_1 = 0, @@ -53,7 +53,7 @@ typedef enum _DriveState { DriveState_ACTIVE = 2 } DriveState; -/* This enumeration contains the available control modes of a KUKA robot. +/* This enumeration contains the available control modes of a KUKA robot. @KUKA.Internal */ typedef enum _ControlMode { ControlMode_POSITION_CONTROLMODE = 0, @@ -62,7 +62,7 @@ typedef enum _ControlMode { ControlMode_NO_CONTROLMODE = 3 } ControlMode; -/* This enumeration contains the available client command modes of the interpolator. +/* This enumeration contains the available client command modes of the interpolator. @KUKA.Internal */ typedef enum _ClientCommandMode { ClientCommandMode_NO_COMMAND_MODE = 0, @@ -72,7 +72,7 @@ typedef enum _ClientCommandMode { ClientCommandMode_CARTESIAN_POSE = 4 } ClientCommandMode; -/* This enumeration contains the currently used overlay type. +/* This enumeration contains the currently used overlay type. @KUKA.Internal */ typedef enum _OverlayType { OverlayType_NO_OVERLAY = 0, @@ -80,7 +80,7 @@ typedef enum _OverlayType { OverlayType_CARTESIAN = 2 } OverlayType; -/* IO Type +/* IO Type @KUKA.Internal */ typedef enum _FriIOType { FriIOType_BOOLEAN = 0, @@ -88,14 +88,14 @@ typedef enum _FriIOType { FriIOType_ANALOG = 2 } FriIOType; -/* IO Direction +/* IO Direction @KUKA.Internal */ typedef enum _FriIODirection { FriIODirection_INPUT = 0, FriIODirection_OUTPUT = 1 } FriIODirection; -/* Redundancy strategy +/* Redundancy strategy @KUKA.Internal */ typedef enum _RedundancyStrategy { RedundancyStrategy_E1 = 0, @@ -103,34 +103,34 @@ typedef enum _RedundancyStrategy { } RedundancyStrategy; /* Struct definitions */ -/* Container object for Joint Values +/* Container object for Joint Values @KUKA.Internal */ typedef struct _JointValues { pb_callback_t value; /* value of a single joint. */ } JointValues; -/* Timestamp container. +/* Timestamp container. @KUKA.Internal */ typedef struct _TimeStamp { uint32_t sec; /* Second portion of the timestamp. Time starts at 1.1.1970. */ uint32_t nanosec; /* Nano second portion of the timestamp. Time starts at 1.1.1970. */ } TimeStamp; -/* Cartesian vector container. +/* Cartesian vector container. KUKA.Internal */ typedef struct _CartesianVector { pb_size_t element_count; double element[6]; /* value of a single vector element */ } CartesianVector; -/* Contains possible checksum implementations like CRC32,MD5. +/* Contains possible checksum implementations like CRC32,MD5. @KUKA.Internal */ typedef struct _Checksum { bool has_crc32; int32_t crc32; /* Storage for CRC32. */ } Checksum; -/* Quaternion transformation container. +/* Quaternion transformation container. @KUKA.Internal */ typedef struct _QuaternionTransformation { char name[64]; /* switch to 'required' due to nanopb-bug */ @@ -156,10 +156,10 @@ typedef struct _RedundancyInformation { double value; } RedundancyInformation; -/* FRI Message Header. Contains the information for timing handshake and the message identifier. - The following messageIdentifiers are currently available: - LBR Monitoring Message: 0x245142 - LBR Command Message: 0x34001 +/* FRI Message Header. Contains the information for timing handshake and the message identifier. + The following messageIdentifiers are currently available: + LBR Monitoring Message: 0x245142 + LBR Command Message: 0x34001 @KUKA.Internal */ typedef struct _MessageHeader { uint32_t messageIdentifier; /* Message identifier. */ @@ -167,7 +167,7 @@ typedef struct _MessageHeader { uint32_t reflectedSequenceCounter; /* Reflected sequence counter. Checked to determine the timing. */ } MessageHeader; -/* FRI Connection info. Contains the connection state and additional informations. +/* FRI Connection info. Contains the connection state and additional information. @KUKA.Internal */ typedef struct _ConnectionInfo { FRISessionState sessionState; /* state of the FRI session. */ @@ -178,12 +178,12 @@ typedef struct _ConnectionInfo { uint32_t receiveMultiplier; /* Multiplier of sendPeriod, on which the Controller expects a new CommmandMessage. */ } ConnectionInfo; -/* Robot Information Object. Contains all static Information about the robot. e.g. - Number of Joints. +/* Robot Information Object. Contains all static Information about the robot. e.g. + Number of Joints. @KUKA.Internal */ typedef struct _RobotInfo { bool has_numberOfJoints; - int32_t numberOfJoints; /* availabe number of joints. */ + int32_t numberOfJoints; /* available number of joints. */ bool has_safetyState; SafetyState safetyState; /* Safety state of the controller. */ pb_callback_t driveState; /* Drivestate of the drives. */ @@ -193,7 +193,7 @@ typedef struct _RobotInfo { ControlMode controlMode; /* Controlmode of the robot. */ } RobotInfo; -/* FRI Monitor Data. Contains the cylic Information about the current robot state. +/* FRI Monitor Data. Contains the cyclic Information about the current robot state. @KUKA.Internal */ typedef struct _MessageMonitorData { bool has_measuredJointPosition; @@ -204,7 +204,7 @@ typedef struct _MessageMonitorData { JointValues commandedTorque; /* last commanded torque value. */ bool has_externalTorque; JointValues externalTorque; /* observed external torque. */ - /* optional CartesianVector externalForce = 6; // observed Cartesian external forces and torque in flange coordinates + /* optional CartesianVector externalForce = 6; // observed Cartesian external forces and torque in flange coordinates repeated QuaternionTransformation subscribedTransformations = 7; // selected transformations from controller to client */ pb_size_t readIORequest_count; FriIOValue readIORequest[10]; /* used to read FieldBus input value(s) */ @@ -216,8 +216,8 @@ typedef struct _MessageMonitorData { TimeStamp timestamp; /* timestamp of the measurement. */ } MessageMonitorData; -/* FRI Interpolator Data. Contains the cyclic commands which are going to be send - to the robot by the interpolator. +/* FRI Interpolator Data. Contains the cyclic commands which are going to be send + to the robot by the interpolator. @KUKA.Internal */ typedef struct _MessageIpoData { bool has_jointPosition; @@ -234,7 +234,7 @@ typedef struct _MessageIpoData { double trackingPerformance; /* tracking performance of the commands */ } MessageIpoData; -/* FRI Command Data. Contains the cyclic commands to modify the robot position. +/* FRI Command Data. Contains the cyclic commands to modify the robot position. @KUKA.Internal */ typedef struct _MessageCommandData { bool has_jointPosition; @@ -253,7 +253,7 @@ typedef struct _MessageCommandData { RedundancyInformation redundancyInformation; /* commanded redundancy information */ } MessageCommandData; -/* FRI End of Data. Contains the length and CRC32 of the data before. +/* FRI End of Data. Contains the length and CRC32 of the data before. @KUKA.Internal */ typedef struct _MessageEndOf { bool has_messageLength; @@ -262,7 +262,7 @@ typedef struct _MessageEndOf { Checksum messageChecksum; /* checksum for all data before this point muss be last date. */ } MessageEndOf; -/* FRI Monitoring Message. Contains the cyclic Information of the robot position and state. +/* FRI Monitoring Message. Contains the cyclic Information of the robot position and state. @KUKA.Internal */ typedef struct _FRIMonitoringMessage { MessageHeader header; /* Message header. */ @@ -280,7 +280,7 @@ typedef struct _FRIMonitoringMessage { MessageEndOf endOfMessageData; /* End of Package contains message length and checksum. */ } FRIMonitoringMessage; -/* FRI Command Message. Contains the information of the user to modify the robot commands. +/* FRI Command Message. Contains the information of the user to modify the robot commands. @KUKA.Internal */ typedef struct _FRICommandMessage { MessageHeader header; /* Message header. */ diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friClientApplication.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friClientApplication.cpp index 5f26ad21..40ce7a89 100644 --- a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friClientApplication.cpp +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friClientApplication.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -92,15 +92,15 @@ ClientApplication::~ClientApplication() //****************************************************************************** bool ClientApplication::connect(int port, const char *remoteHost) { - if (_connection.isOpen()) + if (_connection.isOpen()) { printf("Warning: client application already connected!\n"); return true; } - + return _connection.open(port, remoteHost); } - + //****************************************************************************** void ClientApplication::disconnect() { @@ -120,18 +120,18 @@ bool ClientApplication::step() // Receive and decode new monitoring message // ************************************************************************** int size = _connection.receive(_data->receiveBuffer, FRI_MONITOR_MSG_MAX_SIZE); - + if (size <= 0) { // TODO: size == 0 -> connection closed (maybe go to IDLE instead of stopping?) printf("Error: failed while trying to receive monitoring message!\n"); return false; } - + if (!_data->decoder.decode(_data->receiveBuffer, size)) { return false; } - + // check message type (so that our wrappers match) if (_data->expectedMonitorMsgID != _data->monitoringMsg.header.messageIdentifier) { @@ -139,23 +139,23 @@ bool ClientApplication::step() (int)_data->monitoringMsg.header.messageIdentifier, (int)_data->expectedMonitorMsgID); return false; - } - + } + // ************************************************************************** // callbacks // ************************************************************************** // reset command message before callbacks _data->resetCommandMessage(); - + // callbacks for robot client ESessionState currentState = (ESessionState)_data->monitoringMsg.connectionInfo.sessionState; - + if (_data->lastState != currentState) { _robotClient->onStateChange(_data->lastState, currentState); _data->lastState = currentState; } - + switch (currentState) { case MONITORING_WAIT: @@ -178,34 +178,33 @@ bool ClientApplication::step() { _trafoClient->provide(); } - + // ************************************************************************** // Encode and send command message // ************************************************************************** - + _data->lastSendCounter++; // check if its time to send an answer if (_data->lastSendCounter >= _data->monitoringMsg.connectionInfo.receiveMultiplier) { _data->lastSendCounter = 0; - + // set sequence counters _data->commandMsg.header.sequenceCounter = _data->sequenceCounter++; - _data->commandMsg.header.reflectedSequenceCounter = + _data->commandMsg.header.reflectedSequenceCounter = _data->monitoringMsg.header.sequenceCounter; - + if (!_data->encoder.encode(_data->sendBuffer, size)) { return false; } - + if (!_connection.send(_data->sendBuffer, size)) { printf("Error: failed while trying to send command message!\n"); return false; } } - + return true; } - diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friClientData.h b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friClientData.h index 67c11c56..bb1b8438 100644 --- a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friClientData.h +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friClientData.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -80,21 +80,21 @@ namespace FRI FRIMonitoringMessage monitoringMsg; //!< monitoring message struct FRICommandMessage commandMsg; //!< command message struct - + MonitoringMessageDecoder decoder; //!< monitoring message decoder CommandMessageEncoder encoder; //!< command message encoder - + ESessionState lastState; //!< last FRI state uint32_t sequenceCounter; //!< sequence counter for command messages uint32_t lastSendCounter; //!< steps since last send command - uint32_t expectedMonitorMsgID; //!< expected ID for received monitoring messages + uint32_t expectedMonitorMsgID; //!< expected ID for received monitoring messages const size_t MAX_REQUESTED_TRANSFORMATIONS; //!< maximum count of requested transformations const size_t MAX_SIZE_TRANSFORMATION_ID; //!< maximum size in bytes of a transformation ID std::vector requestedTrafoIDs; //!< list of requested transformation ids - - ClientData(int numDofs) - : decoder(&monitoringMsg, numDofs), + + ClientData(int numDofs) + : decoder(&monitoringMsg, numDofs), encoder(&commandMsg, numDofs), lastState(IDLE), sequenceCounter(0), @@ -106,7 +106,7 @@ namespace FRI { requestedTrafoIDs.reserve(MAX_REQUESTED_TRANSFORMATIONS); } - + void resetCommandMessage() { commandMsg.commandData.has_jointPosition = false; @@ -115,10 +115,10 @@ namespace FRI commandMsg.commandData.commandedTransformations_count = 0; commandMsg.commandData.has_cartesianPose = false; commandMsg.commandData.has_redundancyInformation = false; - commandMsg.has_commandData = false; + commandMsg.has_commandData = false; commandMsg.commandData.writeIORequest_count = 0; } - + //****************************************************************************** static const FriIOValue& getBooleanIOValue(const FRIMonitoringMessage* message, const char* name) { @@ -138,49 +138,49 @@ namespace FRI } //****************************************************************************** - static void setBooleanIOValue(FRICommandMessage* message, const char* name, const bool value, + static void setBooleanIOValue(FRICommandMessage* message, const char* name, const bool value, const FRIMonitoringMessage* monMessage) { setIOValue(message, name, monMessage, FriIOType_BOOLEAN).digitalValue = value; } //****************************************************************************** - static void setDigitalIOValue(FRICommandMessage* message, const char* name, const unsigned long long value, + static void setDigitalIOValue(FRICommandMessage* message, const char* name, const unsigned long long value, const FRIMonitoringMessage* monMessage) { setIOValue(message, name, monMessage, FriIOType_DIGITAL).digitalValue = value; } //****************************************************************************** - static void setAnalogIOValue(FRICommandMessage* message, const char* name, const double value, + static void setAnalogIOValue(FRICommandMessage* message, const char* name, const double value, const FRIMonitoringMessage* monMessage) { setIOValue(message, name, monMessage, FriIOType_ANALOG).analogValue = value; } - + protected: - + //****************************************************************************** - static const FriIOValue& getIOValue(const FRIMonitoringMessage* message, const char* name, + static const FriIOValue& getIOValue(const FRIMonitoringMessage* message, const char* name, const FriIOType ioType) { if(message != NULL && message->has_monitorData == true) { const MessageMonitorData& monData = message->monitorData; - const bool analogValue = (ioType == FriIOType_ANALOG); - const bool digitalValue = (ioType == FriIOType_DIGITAL) | (ioType == FriIOType_BOOLEAN); + const bool analogValue = (ioType == FriIOType_ANALOG); + const bool digitalValue = (ioType == FriIOType_DIGITAL) | (ioType == FriIOType_BOOLEAN); for(size_t i = 0; i < monData.readIORequest_count; i++) { const FriIOValue& ioValue = monData.readIORequest[i]; if(strcmp(name, ioValue.name) == 0) { if(ioValue.type == ioType && - ioValue.has_digitalValue == digitalValue && + ioValue.has_digitalValue == digitalValue && ioValue.has_analogValue == analogValue) { return ioValue; } - + const char* ioTypeName; switch(ioType) { @@ -189,7 +189,7 @@ namespace FRI case FriIOType_BOOLEAN: ioTypeName = "boolean"; break; default: ioTypeName = "?"; break; } - + throw FRIException("IO %s is not of type %s.", name, ioTypeName); } } @@ -197,9 +197,9 @@ namespace FRI throw FRIException("Could not locate IO %s in monitor message.", name); } - + //****************************************************************************** - static FriIOValue& setIOValue(FRICommandMessage* message, const char* name, + static FriIOValue& setIOValue(FRICommandMessage* message, const char* name, const FRIMonitoringMessage* monMessage, const FriIOType ioType) { MessageCommandData& cmdData = message->commandData; @@ -212,29 +212,29 @@ namespace FRI { throw FRIException("IO %s is not an output value.", name); } - + // add IO value to command message FriIOValue& ioValue = cmdData.writeIORequest[cmdData.writeIORequest_count]; - + strncpy(ioValue.name, name, sizeof(ioValue.name) - 1); ioValue.name[sizeof(ioValue.name) - 1] = 0; // ensure termination ioValue.type = ioType; ioValue.has_digitalValue = (ioType == FriIOType_DIGITAL) | (ioType == FriIOType_BOOLEAN); ioValue.has_analogValue = (ioType == FriIOType_ANALOG); ioValue.direction = FriIODirection_OUTPUT; - + cmdData.writeIORequest_count ++; message->has_commandData = true; - + return ioValue; } else { throw FRIException("Exceeded maximum number of outputs that can be set."); } - } + } }; - + } } diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friCommandMessageEncoder.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friCommandMessageEncoder.cpp index 5653154c..afa42c8d 100644 --- a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friCommandMessageEncoder.cpp +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friCommandMessageEncoder.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -95,13 +95,13 @@ void CommandMessageEncoder::initMessage() m_pMessage->commandData.writeIORequest_count = 0; // allocate and map memory for protobuf repeated structures - map_repeatedDouble(FRI_MANAGER_NANOPB_ENCODE, m_nNum, + map_repeatedDouble(FRI_MANAGER_NANOPB_ENCODE, m_nNum, &m_pMessage->commandData.jointPosition.value, &m_tRecvContainer.jointPosition); - map_repeatedDouble(FRI_MANAGER_NANOPB_ENCODE, m_nNum, + map_repeatedDouble(FRI_MANAGER_NANOPB_ENCODE, m_nNum, &m_pMessage->commandData.jointTorque.value, &m_tRecvContainer.jointTorque); - + // nanopb encoding needs to know how many elements the static array contains // a quaternion always contains 7 elements m_pMessage->commandData.cartesianPose.element_count = 7; diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friCommandMessageEncoder.h b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friCommandMessageEncoder.h index dde55882..6edfb0f5 100644 --- a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friCommandMessageEncoder.h +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friCommandMessageEncoder.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -75,43 +75,43 @@ namespace FRI class CommandMessageEncoder { - + public: - + CommandMessageEncoder(FRICommandMessage* pMessage, int num); - + ~CommandMessageEncoder(); - + bool encode(char* buffer, int& size); - + private: - + struct LocalCommandDataContainer { tRepeatedDoubleArguments jointPosition; tRepeatedDoubleArguments jointTorque; - + LocalCommandDataContainer() { init_repeatedDouble(&jointPosition); init_repeatedDouble(&jointTorque); } - + ~LocalCommandDataContainer() { free_repeatedDouble(&jointPosition); free_repeatedDouble(&jointTorque); } }; - + int m_nNum; - + LocalCommandDataContainer m_tRecvContainer; FRICommandMessage* m_pMessage; - + void initMessage(); }; - + } } diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friDataHelper.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friDataHelper.cpp index d87dbb5a..08fa0267 100644 --- a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friDataHelper.cpp +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friDataHelper.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -147,7 +147,7 @@ namespace KUKA } - void DataHelper::convertTrafoQuaternionToMatrix(const double(&quaternionTrafo)[7], + void DataHelper::convertTrafoQuaternionToMatrix(const double(&quaternionTrafo)[7], double(&matrixTrafo)[3][4]) { @@ -156,7 +156,7 @@ namespace KUKA const double qY = quaternionTrafo[QUAT_QY]; const double qZ = quaternionTrafo[QUAT_QZ]; - // conversion for unit quaternion to transformation matrix + // conversion for unit quaternion to transformation matrix matrixTrafo[0][0] = 1 - 2 * ((qY * qY) + (qZ * qZ)); matrixTrafo[0][1] = 2 * ((qX * qY) - (qW * qZ)); matrixTrafo[0][2] = 2 * ((qX * qZ) + (qW * qY)); diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRClient.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRClient.cpp index df035d3c..3541bbcb 100644 --- a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRClient.cpp +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRClient.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -67,13 +67,13 @@ char FRIException::_buffer[1024] = { 0 }; //****************************************************************************** LBRClient::LBRClient() { - + } //****************************************************************************** LBRClient::~LBRClient() { - + } //****************************************************************************** @@ -111,7 +111,7 @@ void LBRClient::command() ClientData* LBRClient::createData() { ClientData* data = new ClientData(_robotState.NUMBER_OF_JOINTS); - + // link monitoring and command message to wrappers _robotState._message = &data->monitoringMsg; _robotCommand._cmdMessage = &data->commandMsg; @@ -120,7 +120,6 @@ ClientData* LBRClient::createData() // set specific message IDs data->expectedMonitorMsgID = _robotState.LBRMONITORMESSAGEID; data->commandMsg.header.messageIdentifier = _robotCommand.LBRCOMMANDMESSAGEID; - + return data; } - diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRCommand.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRCommand.cpp index e1487154..ba5da8ea 100644 --- a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRCommand.cpp +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRCommand.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -105,13 +105,13 @@ void LBRCommand::setCartesianPose(const double* cartesianPoseQuaternion, memcpy(_cmdMessage->commandData.cartesianPose.element, cartesianPoseQuaternion, 7 * sizeof(double)); _cmdMessage->commandData.has_redundancyInformation = true; - _cmdMessage->commandData.redundancyInformation.strategy = + _cmdMessage->commandData.redundancyInformation.strategy = _monMessage->monitorData.measuredRedundancyInformation.strategy; - + if (NULL != redundancyValue) { //set value if provided - + _cmdMessage->commandData.redundancyInformation.value = *redundancyValue; } else diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRState.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRState.cpp index 271ae868..39fe9324 100644 --- a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRState.cpp +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friLBRState.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friMonitoringMessageDecoder.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friMonitoringMessageDecoder.cpp index c6426db9..c2528d16 100644 --- a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friMonitoringMessageDecoder.cpp +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friMonitoringMessageDecoder.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -81,7 +81,7 @@ MonitoringMessageDecoder::~MonitoringMessageDecoder() void MonitoringMessageDecoder::initMessage() { // set initial data - // it is assumed that no robot information and monitoring data is available and therefore the + // it is assumed that no robot information and monitoring data is available and therefore the // optional fields are initialized with false m_pMessage->has_robotInfo = false; m_pMessage->has_monitorData = false; @@ -89,8 +89,8 @@ void MonitoringMessageDecoder::initMessage() m_pMessage->has_ipoData = false; m_pMessage->requestedTransformations_count = 0; m_pMessage->has_endOfMessageData = false; - - + + m_pMessage->header.messageIdentifier = 0; m_pMessage->header.reflectedSequenceCounter = 0; m_pMessage->header.sequenceCounter = 0; @@ -102,27 +102,27 @@ void MonitoringMessageDecoder::initMessage() m_pMessage->monitorData.measuredCartesianPose.element_count = 0; // allocate and map memory for protobuf repeated structures - map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, &m_pMessage->monitorData.measuredJointPosition.value, &m_tSendContainer.m_AxQMsrLocal); - - map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, &m_pMessage->monitorData.measuredTorque.value, &m_tSendContainer.m_AxTauMsrLocal); - - map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, &m_pMessage->monitorData.commandedTorque.value, &m_tSendContainer.m_AxTauCmdLocal); - - map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, &m_pMessage->monitorData.externalTorque.value, &m_tSendContainer.m_AxTauExtMsrLocal); - + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE,m_nNum, &m_pMessage->ipoData.jointPosition.value, &m_tSendContainer.m_AxQCmdIPO); - map_repeatedInt(FRI_MANAGER_NANOPB_DECODE, m_nNum, + map_repeatedInt(FRI_MANAGER_NANOPB_DECODE, m_nNum, &m_pMessage->robotInfo.driveState, &m_tSendContainer.m_AxDriveStateLocal); } diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friMonitoringMessageDecoder.h b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friMonitoringMessageDecoder.h index 795c5175..96138188 100644 --- a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friMonitoringMessageDecoder.h +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friMonitoringMessageDecoder.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -70,7 +70,7 @@ namespace FRI { static const int FRI_MONITOR_MSG_MAX_SIZE = 1500; //!< max size of a FRI monitoring message - + class MonitoringMessageDecoder { @@ -78,14 +78,14 @@ namespace FRI public: MonitoringMessageDecoder(FRIMonitoringMessage* pMessage, int num); - + ~MonitoringMessageDecoder(); - + bool decode(char* buffer, int size); - - + + private: - + struct LocalMonitoringDataContainer { tRepeatedDoubleArguments m_AxQMsrLocal; @@ -93,9 +93,9 @@ namespace FRI tRepeatedDoubleArguments m_AxQCmdT1mLocal; tRepeatedDoubleArguments m_AxTauCmdLocal; tRepeatedDoubleArguments m_AxTauExtMsrLocal; - tRepeatedIntArguments m_AxDriveStateLocal; + tRepeatedIntArguments m_AxDriveStateLocal; tRepeatedDoubleArguments m_AxQCmdIPO; - + LocalMonitoringDataContainer() { init_repeatedDouble(&m_AxQMsrLocal); @@ -106,7 +106,7 @@ namespace FRI init_repeatedDouble(&m_AxQCmdIPO); init_repeatedInt(&m_AxDriveStateLocal); } - + ~LocalMonitoringDataContainer() { free_repeatedDouble(&m_AxQMsrLocal); @@ -123,11 +123,11 @@ namespace FRI LocalMonitoringDataContainer m_tSendContainer; FRIMonitoringMessage* m_pMessage; - + void initMessage(); }; } } - + #endif // _KUKA_FRI_MONITORINGMESSAGEDECODER_H diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friTransformationClient.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friTransformationClient.cpp index 12285b67..2a0029b1 100644 --- a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friTransformationClient.cpp +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friTransformationClient.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -74,7 +74,7 @@ using namespace KUKA::FRI; TransformationClient::TransformationClient() { } - + //****************************************************************************** TransformationClient::~TransformationClient() { @@ -88,12 +88,12 @@ const std::vector& TransformationClient::getRequestedTransformation for (unsigned int i=0; irequestedTrafoIDs[i] = _data->monitoringMsg.requestedTransformations[i].name; - } + } return _data->requestedTrafoIDs; } //****************************************************************************** -const unsigned int TransformationClient::getTimestampSec() const +const unsigned int TransformationClient::getTimestampSec() const { return _data->monitoringMsg.monitorData.timestamp.sec; } @@ -132,8 +132,8 @@ void TransformationClient::setTransformation(const char* transformationID, } //****************************************************************************** -void TransformationClient::setTransformation(const char* transformationID, - const double (&transformationMatrix)[3][4], +void TransformationClient::setTransformation(const char* transformationID, + const double (&transformationMatrix)[3][4], const unsigned int timeSec, const unsigned int timeNanoSec) { diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friUdpConnection.cpp b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friUdpConnection.cpp index 774552ad..a2dbe8b4 100644 --- a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friUdpConnection.cpp +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/friUdpConnection.cpp @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -77,7 +77,7 @@ cost of any service and repair. using namespace KUKA::FRI; //****************************************************************************** -UdpConnection::UdpConnection(unsigned int receiveTimeout) : +UdpConnection::UdpConnection(unsigned int receiveTimeout) : _udpSock(-1), _receiveTimeout(receiveTimeout) { @@ -86,7 +86,7 @@ UdpConnection::UdpConnection(unsigned int receiveTimeout) : WSAStartup(MAKEWORD(2,0), &WSAData); #endif } - + //****************************************************************************** UdpConnection::~UdpConnection() { @@ -95,7 +95,7 @@ UdpConnection::~UdpConnection() WSACleanup(); #endif } - + //****************************************************************************** bool UdpConnection::open(int port, const char *controllerAddress) { @@ -115,7 +115,7 @@ bool UdpConnection::open(int port, const char *controllerAddress) servAddr.sin_family = AF_INET; servAddr.sin_port = htons(port); servAddr.sin_addr.s_addr = htonl(INADDR_ANY); - + if (bind(_udpSock, (struct sockaddr *)&servAddr, sizeof(servAddr)) < 0) { printf("binding port number %d failed!\n", port); @@ -145,7 +145,7 @@ bool UdpConnection::open(int port, const char *controllerAddress) } return true; } - + //****************************************************************************** void UdpConnection::close() { @@ -165,14 +165,14 @@ bool UdpConnection::isOpen() const { return (_udpSock >= 0); } - + //****************************************************************************** int UdpConnection::receive(char *buffer, int maxSize) { if (isOpen()) { /** HAVE_SOCKLEN_T - Yes - unbelievable: There are differences in standard calling parameters (types) to recvfrom + Yes - unbelievable: There are differences in standard calling parameters (types) to recvfrom Windows winsock, VxWorks and QNX use int newer Posix (most Linuxes) use socklen_t */ @@ -185,11 +185,11 @@ int UdpConnection::receive(char *buffer, int maxSize) /** check for timeout Because SO_RCVTIMEO is in Windows not correctly implemented, select is used for the receive time out. If a timeout greater than 0 is given, wait until the timeout is reached or a message was received. - If t, abort the function with an error. + If t, abort the function with an error. */ if(_receiveTimeout > 0) { - + // Set up struct timeval struct timeval tv; tv.tv_sec = _receiveTimeout / 1000; @@ -197,8 +197,8 @@ int UdpConnection::receive(char *buffer, int maxSize) // initialize file descriptor /** - * Replace FD_ZERO with memset, because bzero is not available for VxWorks - * User Space Aplications(RTPs). Therefore the macro FD_ZERO does not compile. + * Replace FD_ZERO with memset, because bzero is not available for VxWorks + * User Space Applications(RTPs). Therefore the macro FD_ZERO does not compile. */ #ifndef VXWORKS FD_ZERO(&_filedescriptor); @@ -206,7 +206,7 @@ int UdpConnection::receive(char *buffer, int maxSize) memset((char *)(&_filedescriptor), 0, sizeof(*(&_filedescriptor))); #endif FD_SET(_udpSock, &_filedescriptor); - + // wait until something was received int numberActiveFileDescr = select(_udpSock+1, &_filedescriptor,NULL,NULL,&tv); // 0 indicates a timeout @@ -218,7 +218,7 @@ int UdpConnection::receive(char *buffer, int maxSize) // a negative value indicates an error else if(numberActiveFileDescr == -1) { - printf("An error has occured \n"); + printf("An error has occurred \n"); return -1; } } @@ -227,7 +227,7 @@ int UdpConnection::receive(char *buffer, int maxSize) } return -1; } - + //****************************************************************************** bool UdpConnection::send(const char* buffer, int size) { diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/pb_frimessages_callbacks.c b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/pb_frimessages_callbacks.c index 998583c2..0fc8ae11 100644 --- a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/pb_frimessages_callbacks.c +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/pb_frimessages_callbacks.c @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -59,7 +59,7 @@ cost of any service and repair. \version {2.5} */ #include -#include +#include #include "pb_frimessages_callbacks.h" #include "pb_encode.h" @@ -194,7 +194,7 @@ bool decode_repeatedInt(pb_istream_t *stream, const pb_field_t *field, void **ar void map_repeatedDouble(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedDoubleArguments *arg) { - // IMPORTANT: the callbacks are stored in a union, therefor a message object + // IMPORTANT: the callbacks are stored in a union, therefore a message object // must be exclusive defined for transmission or reception if (dir == FRI_MANAGER_NANOPB_ENCODE) { @@ -217,7 +217,7 @@ void map_repeatedDouble(eNanopbCallbackDirection dir, int numDOF, pb_callback_t void map_repeatedInt(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedIntArguments *arg) { - // IMPORTANT: the callbacks are stored in a union, therefor a message object + // IMPORTANT: the callbacks are stored in a union, therefore a message object // must be exclusive defined for transmission or reception if (dir == FRI_MANAGER_NANOPB_ENCODE) { diff --git a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/pb_frimessages_callbacks.h b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/pb_frimessages_callbacks.h index fdc1f4e7..0e45520e 100644 --- a/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/pb_frimessages_callbacks.h +++ b/kuka_sunrise_fri_driver/src/FRI_v2_5/fri_client_sdk/pb_frimessages_callbacks.h @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2021 +Copyright (C) 2014-2021 KUKA Deutschland GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -83,8 +83,8 @@ typedef struct repeatedIntArguments { /** enumeration for direction (encoding/decoding) */ typedef enum DIRECTION { - FRI_MANAGER_NANOPB_DECODE = 0, //!< Argument um eine - FRI_MANAGER_NANOPB_ENCODE = 1 //!< + FRI_MANAGER_NANOPB_DECODE = 0, //!< Argument um eine + FRI_MANAGER_NANOPB_ENCODE = 1 //!< } eNanopbCallbackDirection; diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 270c2489..5e0c8b5f 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -42,7 +42,7 @@ CallbackReturn KukaFRIHardwareInterface::on_init( hw_torque_states_.resize(info_.joints.size()); hw_ext_torque_states_.resize(info_.joints.size()); hw_torque_commands_.resize(info_.joints.size()); -#ifdef FRI_V2_5 +#ifdef FRI_V2_5 hw_cart_pose_states_.resize( 7); // it's always 7 dof: position x,y,z; orientation quaternion qx,qy,qz,qw hw_cart_pose_commands_.resize(7); @@ -211,7 +211,7 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta std::vector(DOF, 0.0), std::vector(DOF, 0.0)); fri_connection_->setClientCommandMode(ClientCommandModeID::TORQUE_COMMAND_MODE); break; -#ifdef FRI_V2_5 //only available in FRI version 2.5 +#ifdef FRI_V2_5 // only available in FRI version 2.5 case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL: fri_connection_->setPositionControlMode(); fri_connection_->setClientCommandMode(ClientCommandModeID::CARTESIAN_POSE_COMMAND_MODE); From 996d79640e8d69ab169bce98d3d810526b2610c3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= Date: Thu, 24 Oct 2024 11:22:31 +0200 Subject: [PATCH 49/49] FRI version fix --- kuka_sunrise_fri_driver/CMakeLists.txt | 2 +- .../src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/FRIMessages.pb.c | 0 .../src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/FRIMessages.pb.h | 0 .../fri_client_sdk/HWIFClientApplication.cpp | 0 .../fri_client_sdk/friClientApplication.cpp | 0 .../src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/friClientData.h | 0 .../fri_client_sdk/friCommandMessageEncoder.cpp | 0 .../fri_client_sdk/friCommandMessageEncoder.h | 0 .../src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/friLBRClient.cpp | 0 .../{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/friLBRCommand.cpp | 0 .../src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/friLBRState.cpp | 0 .../fri_client_sdk/friMonitoringMessageDecoder.cpp | 0 .../fri_client_sdk/friMonitoringMessageDecoder.h | 0 .../fri_client_sdk/friTransformationClient.cpp | 0 .../{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/friUdpConnection.cpp | 0 .../fri_client_sdk/pb_frimessages_callbacks.c | 0 .../fri_client_sdk/pb_frimessages_callbacks.h | 0 17 files changed, 1 insertion(+), 1 deletion(-) rename kuka_sunrise_fri_driver/src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/FRIMessages.pb.c (100%) rename kuka_sunrise_fri_driver/src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/FRIMessages.pb.h (100%) rename kuka_sunrise_fri_driver/src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/HWIFClientApplication.cpp (100%) rename kuka_sunrise_fri_driver/src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/friClientApplication.cpp (100%) rename kuka_sunrise_fri_driver/src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/friClientData.h (100%) rename kuka_sunrise_fri_driver/src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/friCommandMessageEncoder.cpp (100%) rename kuka_sunrise_fri_driver/src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/friCommandMessageEncoder.h (100%) rename kuka_sunrise_fri_driver/src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/friLBRClient.cpp (100%) rename kuka_sunrise_fri_driver/src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/friLBRCommand.cpp (100%) rename kuka_sunrise_fri_driver/src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/friLBRState.cpp (100%) rename kuka_sunrise_fri_driver/src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/friMonitoringMessageDecoder.cpp (100%) rename kuka_sunrise_fri_driver/src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/friMonitoringMessageDecoder.h (100%) rename kuka_sunrise_fri_driver/src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/friTransformationClient.cpp (100%) rename kuka_sunrise_fri_driver/src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/friUdpConnection.cpp (100%) rename kuka_sunrise_fri_driver/src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/pb_frimessages_callbacks.c (100%) rename kuka_sunrise_fri_driver/src/{FRI_v1_5 => FRI_v1_15}/fri_client_sdk/pb_frimessages_callbacks.h (100%) diff --git a/kuka_sunrise_fri_driver/CMakeLists.txt b/kuka_sunrise_fri_driver/CMakeLists.txt index f570dee9..c77f3723 100644 --- a/kuka_sunrise_fri_driver/CMakeLists.txt +++ b/kuka_sunrise_fri_driver/CMakeLists.txt @@ -37,7 +37,7 @@ if (WITH_FRI_VERSION_2_5) add_definitions(-DFRI_V2_5) set(FRI_CLIENT_SDK_VERSION "FRI_v2_5") else() - set(FRI_CLIENT_SDK_VERSION "FRI_v1_5") + set(FRI_CLIENT_SDK_VERSION "FRI_v1_15") endif() include_directories(include src/${FRI_CLIENT_SDK_VERSION}/fri_client_sdk include/${FRI_CLIENT_SDK_VERSION} ${NANOPB_INCLUDE_DIR}) diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/FRIMessages.pb.c b/kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/FRIMessages.pb.c similarity index 100% rename from kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/FRIMessages.pb.c rename to kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/FRIMessages.pb.c diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/FRIMessages.pb.h b/kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/FRIMessages.pb.h similarity index 100% rename from kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/FRIMessages.pb.h rename to kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/FRIMessages.pb.h diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/HWIFClientApplication.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/HWIFClientApplication.cpp similarity index 100% rename from kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/HWIFClientApplication.cpp rename to kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/HWIFClientApplication.cpp diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friClientApplication.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friClientApplication.cpp similarity index 100% rename from kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friClientApplication.cpp rename to kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friClientApplication.cpp diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friClientData.h b/kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friClientData.h similarity index 100% rename from kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friClientData.h rename to kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friClientData.h diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friCommandMessageEncoder.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friCommandMessageEncoder.cpp similarity index 100% rename from kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friCommandMessageEncoder.cpp rename to kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friCommandMessageEncoder.cpp diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friCommandMessageEncoder.h b/kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friCommandMessageEncoder.h similarity index 100% rename from kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friCommandMessageEncoder.h rename to kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friCommandMessageEncoder.h diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRClient.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friLBRClient.cpp similarity index 100% rename from kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRClient.cpp rename to kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friLBRClient.cpp diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRCommand.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friLBRCommand.cpp similarity index 100% rename from kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRCommand.cpp rename to kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friLBRCommand.cpp diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRState.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friLBRState.cpp similarity index 100% rename from kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friLBRState.cpp rename to kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friLBRState.cpp diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friMonitoringMessageDecoder.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friMonitoringMessageDecoder.cpp similarity index 100% rename from kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friMonitoringMessageDecoder.cpp rename to kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friMonitoringMessageDecoder.cpp diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friMonitoringMessageDecoder.h b/kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friMonitoringMessageDecoder.h similarity index 100% rename from kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friMonitoringMessageDecoder.h rename to kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friMonitoringMessageDecoder.h diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friTransformationClient.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friTransformationClient.cpp similarity index 100% rename from kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friTransformationClient.cpp rename to kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friTransformationClient.cpp diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friUdpConnection.cpp b/kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friUdpConnection.cpp similarity index 100% rename from kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/friUdpConnection.cpp rename to kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/friUdpConnection.cpp diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/pb_frimessages_callbacks.c b/kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/pb_frimessages_callbacks.c similarity index 100% rename from kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/pb_frimessages_callbacks.c rename to kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/pb_frimessages_callbacks.c diff --git a/kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/pb_frimessages_callbacks.h b/kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/pb_frimessages_callbacks.h similarity index 100% rename from kuka_sunrise_fri_driver/src/FRI_v1_5/fri_client_sdk/pb_frimessages_callbacks.h rename to kuka_sunrise_fri_driver/src/FRI_v1_15/fri_client_sdk/pb_frimessages_callbacks.h