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] 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: