├── .github └── workflows │ ├── ci_bionic.yml │ ├── ci_focal.yml │ └── windows-vcpkg.yml ├── .gitignore ├── CMakeLists.txt ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── cmake └── abb_libegmConfig.cmake.in ├── docs └── images │ ├── egm_sketch.png │ ├── rosin_logo.png │ ├── statemachine_addin_sketch.png │ └── symbio_tic_logo.png ├── include └── abb_libegm │ ├── egm_base_interface.h │ ├── egm_common.h │ ├── egm_common_auxiliary.h │ ├── egm_controller_interface.h │ ├── egm_interpolator.h │ ├── egm_logger.h │ ├── egm_trajectory_interface.h │ └── egm_udp_server.h ├── package.xml ├── proto ├── README.md ├── egm.proto ├── egm_wrapper.proto └── egm_wrapper_trajectory.proto └── src ├── egm_base_interface.cpp ├── egm_common.cpp ├── egm_common_auxiliary.cpp ├── egm_controller_interface.cpp ├── egm_interpolator.cpp ├── egm_logger.cpp ├── egm_trajectory_interface.cpp └── egm_udp_server.cpp /.github/workflows/ci_bionic.yml: -------------------------------------------------------------------------------- 1 | name: CI - Ubuntu Bionic 2 | 3 | on: 4 | # direct pushes to protected branches are not supported 5 | pull_request: 6 | # run every day, at 6am UTC 7 | schedule: 8 | - cron: '0 6 * * *' 9 | # allow manually starting this workflow 10 | workflow_dispatch: 11 | 12 | jobs: 13 | industrial_ci: 14 | name: ROS Melodic (${{ matrix.ros_repo }}) 15 | runs-on: ubuntu-20.04 16 | 17 | strategy: 18 | matrix: 19 | ros_distro: [ melodic ] 20 | ros_repo: [ main, testing ] 21 | 22 | env: 23 | CCACHE_DIR: "${{ github.workspace }}/.ccache" 24 | 25 | steps: 26 | - name: Fetch repository 27 | uses: actions/checkout@v2 28 | 29 | - name: ccache cache 30 | uses: actions/cache@v2 31 | with: 32 | path: ${{ env.CCACHE_DIR }} 33 | # we always want the ccache cache to be persisted, as we cannot easily 34 | # determine whether dependencies have changed, and ccache will manage 35 | # updating the cache for us. Adding 'run_id' to the key will force an 36 | # upload at the end of the job. 37 | key: ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }}-${{github.run_id}} 38 | restore-keys: | 39 | ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }} 40 | - name: Run industrial_ci 41 | uses: ros-industrial/industrial_ci@master 42 | env: 43 | ROS_DISTRO: ${{ matrix.ros_distro }} 44 | ROS_REPO: ${{ matrix.ros_repo }} 45 | -------------------------------------------------------------------------------- /.github/workflows/ci_focal.yml: -------------------------------------------------------------------------------- 1 | name: CI - Ubuntu Focal 2 | 3 | on: 4 | # direct pushes to protected branches are not supported 5 | pull_request: 6 | # run every day, at 6am UTC 7 | schedule: 8 | - cron: '0 6 * * *' 9 | # allow manually starting this workflow 10 | workflow_dispatch: 11 | 12 | jobs: 13 | industrial_ci: 14 | name: ROS Noetic (${{ matrix.ros_repo }}) 15 | runs-on: ubuntu-20.04 16 | 17 | strategy: 18 | matrix: 19 | ros_distro: [ noetic ] 20 | ros_repo: [ main, testing ] 21 | 22 | env: 23 | CCACHE_DIR: "${{ github.workspace }}/.ccache" 24 | 25 | steps: 26 | - name: Fetch repository 27 | uses: actions/checkout@v2 28 | 29 | - name: ccache cache 30 | uses: actions/cache@v2 31 | with: 32 | path: ${{ env.CCACHE_DIR }} 33 | # we always want the ccache cache to be persisted, as we cannot easily 34 | # determine whether dependencies have changed, and ccache will manage 35 | # updating the cache for us. Adding 'run_id' to the key will force an 36 | # upload at the end of the job. 37 | key: ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }}-${{github.run_id}} 38 | restore-keys: | 39 | ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }} 40 | - name: Run industrial_ci 41 | uses: ros-industrial/industrial_ci@master 42 | env: 43 | ROS_DISTRO: ${{ matrix.ros_distro }} 44 | ROS_REPO: ${{ matrix.ros_repo }} 45 | -------------------------------------------------------------------------------- /.github/workflows/windows-vcpkg.yml: -------------------------------------------------------------------------------- 1 | name: Windows vcpkg Workflow 2 | 3 | on: 4 | # direct pushes to protected branches are not supported 5 | pull_request: 6 | # run every day, at 2am UTC 7 | schedule: 8 | - cron: '0 2 * * *' 9 | # allow manually starting this workflow 10 | workflow_dispatch: 11 | 12 | jobs: 13 | build: 14 | name: 'Windows vcpkg job' 15 | runs-on: windows-latest 16 | 17 | steps: 18 | - uses: actions/checkout@v2 19 | 20 | # Restore from cache the previously built ports. If "cache miss" then provision vcpkg, 21 | # install desired ports, finally cache everything for the next run. 22 | - name: Restore from cache and run vcpkg 23 | uses: lukka/run-vcpkg@v5 24 | with: 25 | vcpkgArguments: '--triplet x64-windows boost-asio boost-math boost-smart-ptr protobuf' 26 | # Commit corresponding to vcpkg's master branch on 2020-11-20. 27 | vcpkgGitCommitId: e803bf11296d8e7900dafb41e7b1224778d33dc6 28 | # Workaround for https://github.com/ros-industrial/abb_libegm/pull/101#discussion_r433370614. 29 | # This line can be removed once protobuf is linked via its imported CMake targets. 30 | vcpkgDirectory: '${{ github.workspace }}/../vcpkg' 31 | 32 | - name: Configure the project 33 | shell: bash 34 | run: | 35 | mkdir build 36 | cd build 37 | cmake -DCMAKE_TOOLCHAIN_FILE=${VCPKG_ROOT}/scripts/buildsystems/vcpkg.cmake .. 38 | cmake --build . --config RelWithDebInfo 39 | cmake --install . --config RelWithDebInfo 40 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | # Read version from the package.xml file. 4 | file(READ ${CMAKE_CURRENT_SOURCE_DIR}/package.xml package_xml_str) 5 | if(NOT package_xml_str MATCHES "([0-9]+.[0-9]+.[0-9]+)") 6 | message(FATAL_ERROR "Could not parse project version from package.xml. Aborting.") 7 | endif() 8 | 9 | # At this point we either have a proper version string, or we've errored 10 | # out with a FATAL_ERROR above. So assume CMAKE_MATCH_1 contains our 11 | # package's version. 12 | project(abb_libegm VERSION ${CMAKE_MATCH_1} LANGUAGES CXX) 13 | 14 | include(GNUInstallDirs) 15 | 16 | if(WIN32) 17 | set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) 18 | endif() 19 | 20 | ######################### 21 | ## Boost C++ Libraries ## 22 | ######################### 23 | find_package(Boost REQUIRED COMPONENTS regex system thread) 24 | 25 | ############################# 26 | ## Google Protocol Buffers ## 27 | ############################# 28 | 29 | # Temporary workaround for https://github.com/ms-iot/ROSOnWindows/issues/218 30 | if(WIN32) 31 | foreach(prefix IN ITEMS $ENV{CMAKE_PREFIX_PATH}) 32 | if(${prefix} STREQUAL "C:/opt/rosdeps/x64") 33 | list(APPEND CMAKE_PROGRAM_PATH "C:/opt/rosdeps/x64/tools/protobuf") 34 | endif() 35 | endforeach() 36 | endif() 37 | 38 | find_package(Protobuf REQUIRED) 39 | 40 | # Make sure protoc is present, as apparently the above find_package() doesn't check that. 41 | if(NOT PROTOBUF_PROTOC_EXECUTABLE) 42 | message(FATAL_ERROR "Cannot find required 'protoc', cannot process Protobuf files without it. Aborting.") 43 | endif() 44 | 45 | # Generate C++ for protocol classes (headers and sources 46 | # get written to the CMAKE_CURRENT_BINARY_DIR location). 47 | set(EgmProtoFiles proto/egm.proto proto/egm_wrapper.proto proto/egm_wrapper_trajectory.proto) 48 | if(NOT QUIET) 49 | message(STATUS "Generating protobuf C++ for: ${EgmProtoFiles}") 50 | endif() 51 | if(MSVC) 52 | # Add export macro when using Microsoft Visual C++ compiler. 53 | protobuf_generate_cpp(EgmProtoSources EgmProtoHeaders EXPORT_MACRO ABB_LIBEGM_EXPORT ${EgmProtoFiles}) 54 | else() 55 | protobuf_generate_cpp(EgmProtoSources EgmProtoHeaders ${EgmProtoFiles}) 56 | endif() 57 | 58 | ############# 59 | ## Threads ## 60 | ############# 61 | find_package(Threads REQUIRED) 62 | 63 | # Work around Protobuf exporting 'lpthread' as a library: we let the 64 | # previous find_package(...) determine the system's thread library. 65 | list(REMOVE_ITEM PROTOBUF_LIBRARIES "-lpthread") 66 | 67 | ########### 68 | ## Build ## 69 | ########### 70 | if(NOT DEFINED BUILD_SHARED_LIBS) 71 | option(BUILD_SHARED_LIBS "Build dynamically-linked binaries" ON) 72 | endif() 73 | 74 | set( 75 | SRC_FILES 76 | src/egm_base_interface.cpp 77 | src/egm_common.cpp 78 | src/egm_common_auxiliary.cpp 79 | src/egm_controller_interface.cpp 80 | src/egm_interpolator.cpp 81 | src/egm_logger.cpp 82 | src/egm_udp_server.cpp 83 | src/egm_trajectory_interface.cpp 84 | ${EgmProtoSources} 85 | ) 86 | 87 | add_library(${PROJECT_NAME} ${SRC_FILES}) 88 | add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) 89 | 90 | include(GenerateExportHeader) 91 | generate_export_header(${PROJECT_NAME}) 92 | 93 | target_include_directories(${PROJECT_NAME} PUBLIC 94 | "$" 95 | $/include> 96 | ${PROTOBUF_INCLUDE_DIRS} 97 | ) 98 | 99 | target_link_libraries(${PROJECT_NAME} PUBLIC 100 | Boost::regex 101 | Boost::system 102 | Boost::thread 103 | ${PROTOBUF_LIBRARIES} 104 | Threads::Threads 105 | ) 106 | 107 | if(NOT BUILD_SHARED_LIBS) 108 | target_compile_definitions(${PROJECT_NAME} PUBLIC "ABB_LIBEGM_STATIC_DEFINE") 109 | endif() 110 | 111 | if(MSVC) 112 | # Force include the export header when using Microsoft Visual C++ compiler. 113 | target_compile_options(${PROJECT_NAME} PUBLIC "/FI${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_export.h") 114 | endif() 115 | 116 | ############# 117 | ## Install ## 118 | ############# 119 | install( 120 | DIRECTORY include/ 121 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 122 | ) 123 | 124 | install( 125 | FILES 126 | ${EgmProtoFiles} 127 | ${EgmProtoHeaders} 128 | ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_export.h 129 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME} 130 | ) 131 | 132 | install( 133 | TARGETS ${PROJECT_NAME} 134 | EXPORT export_${PROJECT_NAME} 135 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 136 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 137 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} 138 | ) 139 | 140 | include(CMakePackageConfigHelpers) 141 | 142 | # Create the ${PROJECT_NAME}Config.cmake. 143 | configure_file( 144 | ${CMAKE_CURRENT_SOURCE_DIR}/cmake/${PROJECT_NAME}Config.cmake.in 145 | "${PROJECT_BINARY_DIR}/${CMAKE_FILES_DIRECTORY}/${PROJECT_NAME}Config.cmake" @ONLY 146 | ) 147 | 148 | # Create the ${PROJECT_NAME}ConfigVersion.cmake. 149 | write_basic_package_version_file( 150 | ${PROJECT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake 151 | COMPATIBILITY AnyNewerVersion 152 | ) 153 | 154 | install( 155 | FILES 156 | "${PROJECT_BINARY_DIR}/${CMAKE_FILES_DIRECTORY}/${PROJECT_NAME}Config.cmake" 157 | "${PROJECT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" 158 | "${CMAKE_CURRENT_SOURCE_DIR}/package.xml" 159 | DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME} 160 | ) 161 | 162 | # Export targets. 163 | set(export_targets ${export_targets};${PROJECT_NAME}) 164 | export( 165 | EXPORT export_${PROJECT_NAME} 166 | FILE "${PROJECT_BINARY_DIR}/${PROJECT_NAME}Targets.cmake" 167 | NAMESPACE ${PROJECT_NAME}:: 168 | ) 169 | 170 | install( 171 | EXPORT export_${PROJECT_NAME} 172 | DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME} 173 | FILE ${PROJECT_NAME}Targets.cmake 174 | NAMESPACE ${PROJECT_NAME}:: 175 | ) 176 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | ROS-Industrial is a community project. We welcome contributions from any source, from those who are extremely active to casual users. The following sections outline the steps on how to contribute to ROS-Industrial. It assumes there is an existing repository to which one would like to contribute (item 1 in the figure [found here](https://rosindustrial.org/developmentprocess)) and one is familiar with the Git "Fork and Branch" workflow, detailed [here](http://blog.scottlowe.org/2015/01/27/using-fork-branch-git-workflow/). 2 | 3 | 1. Before any development is undertaken, a contributor would communicate a need and/or issue to the ROS-Industrial community. This can be done by submitting an issue on the appropriate GitHub repo, the [issues repo](https://github.com/ros-industrial/ros_industrial_issues), or by posting a message in the [ROS-Industrial category on ROS Discourse](https://discourse.ros.org/c/ros-industrial). Doing so may save you time if similar development is underway and ensure that whatever approach you take is acceptable to the community of reviewers once it is submitted. 4 | 2. The second step (item 2) is to implement your change. If you are working on a code contribution, we highly recommend you utilize the [ROS Qt-Creator Plug-in](http://rosindustrial.org/news/2016/6/9/ros-qt-ide-plugin). Verify that your change successfully builds and passes all tests. Additionally, the coding style of this repo is based on the [ROS C++ Style Guide](http://wiki.ros.org/CppStyleGuide). 5 | 3. Next, push your changes to a "feature" branch in your personal fork of the repo and issue a pull request (PR)(item 3). The PR allows maintainers to review the submitted code. Before the PR can be accepted, the maintainer and contributor must agree that the contribution is implemented appropriately. This process can take several back-and-forth steps (see [example](https://github.com/ros-industrial/motoman/pull/89)). Contributors should expect to spend as much time reviewing/changing the code as on the initial implementation. This time can be minimized by communicating with the ROS-Industrial community before any contribution is made. 6 | 4. Issuing a Pull Request (PR) triggers the [Travis Continuous Integrations (CI)](https://github.com/ros-industrial/industrial_ci) step (item 4) which happens automatically in the background. The Travis CI performs several operations, and if any of the steps below fail, then the PR is marked accordingly for the maintainer. 7 | * Travis Workflow: 8 | * Installs a barebones ROS distribution on a fresh Ubuntu virtual machine. 9 | * Creates a catkin workspace and puts the repository in it. 10 | * Uses wstool to check out any from-source dependencies (i.e. other repositories). 11 | * Resolves package dependencies using rosdep (i.e. install packages using apt-get). 12 | * Compiles the catkin workspace. 13 | * Runs all available unit tests. 14 | 5. If the PR passes Travis CI and one of the maintainers is satisfied with the changes, they post a +1 as a comment on the PR (item 5). The +1 signifies that the PR is ready to be merged. All PRs require at least one +1 and pass Travis CI before it can be merged. 15 | 6. The next step (item 6) is for the PR to be merged into the main branch. This is done through the GitHub web interface by selecting the “Merge pull request” button. After the PR is merged, all status badges are updated automatically. 16 | 7. After building a new version of the codebase, the developer may have questions, experience issues or it may not have the necessary functionality which should all be reported on the packages GitHub repository as an issue (item 10). If an issue is identified or there is missing functionality that the developer requires, the cycle starts back at (item 2). 17 | 18 | For more details, please refer to the [ROS-I wiki](http://wiki.ros.org/Industrial/DevProcess). 19 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, ABB Schweiz AG 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with 5 | or without modification, are permitted provided that 6 | the following conditions are met: 7 | 8 | * Redistributions of source code must retain the 9 | above copyright notice, this list of conditions 10 | and the following disclaimer. 11 | * Redistributions in binary form must reproduce the 12 | above copyright notice, this list of conditions 13 | and the following disclaimer in the documentation 14 | and/or other materials provided with the 15 | distribution. 16 | * Neither the name of ABB nor the names of its 17 | contributors may be used to endorse or promote 18 | products derived from this software without 19 | specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 25 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 30 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # abb_libegm 2 | 3 | [![Build Status: Ubuntu Bionic (Actions)](https://github.com/ros-industrial/abb_libegm/workflows/CI%20-%20Ubuntu%20Bionic/badge.svg?branch=master)](https://github.com/ros-industrial/abb_libegm/actions?query=workflow%3A%22CI+-+Ubuntu+Bionic%22) 4 | [![Build Status: Ubuntu Focal (Actions)](https://github.com/ros-industrial/abb_libegm/workflows/CI%20-%20Ubuntu%20Focal/badge.svg?branch=master)](https://github.com/ros-industrial/abb_libegm/actions?query=workflow%3A%22CI+-+Ubuntu+Focal%22) 5 | [![Github Issues](https://img.shields.io/github/issues/ros-industrial/abb_libegm.svg)](http://github.com/ros-industrial/abb_libegm/issues) 6 | 7 | [![license - bsd 3 clause](https://img.shields.io/:license-BSD%203--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) 8 | 9 | [![support level: community](https://img.shields.io/badge/support%20level-community-lightgray.svg)](http://rosindustrial.org/news/2016/10/7/better-supporting-a-growing-ros-industrial-software-platform) 10 | 11 | ## Important Notes 12 | 13 | RobotWare versions less than `6.07.01` are now incompatible with *abb_libegm* (due to changes in the EGM communication protocol). 14 | 15 | Pull request [abb_libegm#63](https://github.com/ros-industrial/abb_libegm/pull/63) turned this package from a Catkin package into a plain CMake package. ROS users may use any of the following build tools to build the library: 16 | 17 | * ROS 1: `catkin_make_isolated` or [catkin_tools](https://catkin-tools.readthedocs.io/en/latest/index.html). 18 | * ROS 2: [colcon](https://colcon.readthedocs.io/en/released/). 19 | 20 | ## Overview 21 | 22 | A C++ library for interfacing with ABB robot controllers supporting *Externally Guided Motion* (EGM). See the *Application manual - Externally Guided Motion* (document ID: `3HAC073319-001`, revision: `B`) for a detailed description of what EGM is and how to use it. 23 | 24 | * See [abb_librws](https://github.com/ros-industrial/abb_librws) for a companion library that interfaces with *Robot Web Services* (RWS). 25 | * See StateMachine Add-In ([1.0](https://robotapps.robotstudio.com/#/viewApp/7fa7065f-457f-47ce-98d7-c04882e703ee) or [1.1](https://robotapps.robotstudio.com/#/viewApp/c163de01-792e-4892-a290-37dbe050b6e1)) for an optional *RobotWare Add-In* that can be useful when configuring an ABB robot controller for use with this library. 26 | 27 | Please note that this package has not been productized, it is provided "as-is" and only limited support can be expected. 28 | 29 | ### Sketch 30 | 31 | The following is a conceptual sketch of how this EGM library can be viewed, in relation to an ABB robot controller as well as the RWS companion library mentioned above. The optional *StateMachine Add-In* is related to the robot controller's RAPID program and system configuration. 32 | 33 | ![EGM sketch](docs/images/egm_sketch.png) 34 | 35 | ### Requirements 36 | 37 | * RobotWare version `6.07.01` or higher (lower versions are incompatible due to changes in the EGM communication protocol). 38 | * A license for the RobotWare option *Externally Guided Motion* (`689-1`). 39 | 40 | ### Dependencies 41 | 42 | * [Boost C++ Libraries](https://www.boost.org) 43 | * [Google Protocol Buffers](https://developers.google.com/protocol-buffers) 44 | 45 | ### Limitations 46 | 47 | This library is intended to be used with the UDP variant of EGM, and it supports the following EGM modes: 48 | 49 | * Joint Mode 50 | * Pose Mode 51 | 52 | ### Recommendations 53 | 54 | * This library has been verified to work with RobotWare version `6.08.00.01`. Other versions are expected to work, except versions older than RobotWare `6.07.01` which are incompatible due to changes in the EGM communication protocol. 55 | * It is a good idea to perform RobotStudio simulations before working with a real robot. 56 | * It is prudent to familiarize oneself with general safety regulations (e.g. described in ABB manuals). 57 | * Consider cyber security aspects, before connecting robot controllers to networks. 58 | 59 | ## Usage Hints 60 | 61 | This is a generic library, which can be used together with any RAPID program that is using the RAPID `EGMRunJoint` and/or `EGMRunPose` instructions, and system configuration. The library's primary classes are: 62 | 63 | * [UDPServer](include/abb_libegm/egm_udp_server.h): Sets up and manages asynchronous UDP communication loops. During an EGM communication session, the robot controller requests new references over a UDP channel, at the rate specified with RAPID `EGMAct` instructions. When an `UDPServer` instance receives an EGM message from the robot controller the message is passed on to an EGM interface instance (see below). The interface is expected to generate the reply message, containing the new references, which the server then sends back to the robot controller. 64 | * [AbstractUDPServerInterface](include/abb_libegm/egm_udp_server.h): An abstract interface, which specifies how to interact with the `UDPServer` class. Can be inherited from to implement custom EGM interfaces. 65 | * [EGMBaseInterface](include/abb_libegm/egm_base_interface.h): Inherits from `AbstractUDPServerInterface`, encapsulates an `UDPServer` instance, and implements a basic EGM interface. Can be configured to use demo references, which are intended for testing that EGM communication channels works. 66 | * [EGMControllerInterface](include/abb_libegm/egm_controller_interface.h): Inherits from `EGMBaseInterface` and implements an EGM interface variant for execution inside an external control loop that needs to be implemented by the user. Provides interaction methods, which can be used inside external control loops to affect EGM communication sessions. 67 | * [EGMTrajectoryInterface](include/abb_libegm/egm_trajectory_interface.h): Inherits from `EGMBaseInterface` and implements an EGM interface variant for execution of a queue of trajectories. Provides interaction methods, which can be called by a user to for example add trajectories to the queue and to stop/resume the trajectory execution. 68 | 69 | The optional *StateMachine Add-In* for RobotWare can be used in combination with any of the classes above. 70 | 71 | ### StateMachine Add-In [Optional] 72 | 73 | The purpose of the RobotWare Add-In is to *ease the setup* of ABB robot controllers. It is made for both *real controllers* and *virtual controllers* (simulated in RobotStudio). If the Add-In is selected during a RobotWare system installation, then the Add-In will load several RAPID modules and system configurations based on the system specifications (e.g. number of robots and present options). 74 | 75 | The RAPID modules and configurations constitute a customizable, but ready to run, RAPID program which contains a state machine implementation. Each motion task in the robot system receives its own state machine instance, and the intention is to use this in combination with external systems that require interaction with the robot(s). The following is a conceptual sketch of the RAPID program's execution flow. 76 | 77 |

78 | 79 |

80 | 81 | To install the Add-In: 82 | 83 | 1. Go to the *Add-Ins* tab in RobotStudio. 84 | 2. Search for *StateMachine Add-In* in the *RobotApps* window. 85 | 3. Select the desired Add-In version and retrieve it by pressing the *Add* button. 86 | 4. Verify that the Add-In was added to the list *Installed Packages*. 87 | 5. The Add-In should appear as an option during the installation of a RobotWare system. 88 | 89 | See the Add-In's user manual ([1.0](https://robotapps.blob.core.windows.net/appreferences/docs/27e5bd15-b5ec-401d-986a-30c9d2934e97UserManual.pdf) or [1.1](https://robotapps.blob.core.windows.net/appreferences/docs/cd504500-80e2-4cb6-9419-c60ea4ad6d56UserManual.pdf)) for more details, as well as for install instructions for RobotWare systems. The manual can also be accessed by right-clicking on the Add-In in the *Installed Packages* list and selecting *Documentation*. 90 | 91 | #### Notes 92 | 93 | If the EGM option is selected during system installation, then the Add-In will load RAPID code for using `EGMRunJoint` and `EGMRunPose` RAPID instructions. System configurations will also be loaded, and it is important to update the robot controller's EGM configurations. Especially the *Remote Address* and *Remote Port Number* configurations, under the *Transmission Protocol* topic, are vital to edit so that the robot controller will send EGM messages to the correct host address. This configuration can be found in RobotStudio -> Controller tab -> Configuration -> Communication -> Transmission Protocol -> Edit each `ROB_X` instance. There will be one instance for each robot in the system. 94 | 95 | The RWS companion library contains a class specifically designed to interact with the StateMachine Add-In. It allows for example to control the RAPID program by starting and stopping EGM communication sessions. 96 | 97 | ## Acknowledgements 98 | 99 | The **core development** has been supported by the European Union's Horizon 2020 project [SYMBIO-TIC](http://www.symbio-tic.eu/). 100 | The SYMBIO-TIC project has received funding from the European Union's Horizon 2020 research and innovation programme under grant agreement no. 637107. 101 | 102 | 103 | 104 | The **open-source process** has been supported by the European Union's Horizon 2020 project [ROSIN](http://rosin-project.eu/). 105 | The ROSIN project has received funding from the European Union's Horizon 2020 research and innovation programme under grant agreement no. 732287. 106 | 107 | 108 | 109 | The opinions expressed reflects only the author's view and reflects in no way the European Commission's opinions. 110 | The European Commission is not responsible for any use that may be made of the contained information. 111 | 112 | ### Special Thanks 113 | 114 | Special thanks to [gavanderhoorn](https://github.com/gavanderhoorn) for guidance with open-source practices and ROS-Industrial conventions. 115 | -------------------------------------------------------------------------------- /cmake/abb_libegmConfig.cmake.in: -------------------------------------------------------------------------------- 1 | # - Config file for the abb_libegm package 2 | # It defines the following variable 3 | # abb_libegm_LIBRARIES - libraries to link against 4 | 5 | include(CMakeFindDependencyMacro) 6 | 7 | # Find dependencies 8 | find_dependency(Threads REQUIRED) 9 | find_dependency(Boost REQUIRED COMPONENTS regex system thread) 10 | 11 | # Our library dependencies (contains definitions for IMPORTED targets) 12 | include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") 13 | 14 | # These are IMPORTED targets created by @PROJECT_NAME@Targets.cmake 15 | set(abb_libegm_LIBRARIES @PROJECT_NAME@::@PROJECT_NAME@) 16 | -------------------------------------------------------------------------------- /docs/images/egm_sketch.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial/abb_libegm/2c9350abfb67131cae796805fed8cd6eeb2ab7f9/docs/images/egm_sketch.png -------------------------------------------------------------------------------- /docs/images/rosin_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial/abb_libegm/2c9350abfb67131cae796805fed8cd6eeb2ab7f9/docs/images/rosin_logo.png -------------------------------------------------------------------------------- /docs/images/statemachine_addin_sketch.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial/abb_libegm/2c9350abfb67131cae796805fed8cd6eeb2ab7f9/docs/images/statemachine_addin_sketch.png -------------------------------------------------------------------------------- /docs/images/symbio_tic_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial/abb_libegm/2c9350abfb67131cae796805fed8cd6eeb2ab7f9/docs/images/symbio_tic_logo.png -------------------------------------------------------------------------------- /include/abb_libegm/egm_base_interface.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************************************** 2 | * 3 | * Copyright (c) 2015, ABB Schweiz AG 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with 7 | * or without modification, are permitted provided that 8 | * the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the 11 | * above copyright notice, this list of conditions 12 | * and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the 14 | * above copyright notice, this list of conditions 15 | * and the following disclaimer in the documentation 16 | * and/or other materials provided with the 17 | * distribution. 18 | * * Neither the name of ABB nor the names of its 19 | * contributors may be used to endorse or promote 20 | * products derived from this software without 21 | * specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 32 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *********************************************************************************************************************** 35 | */ 36 | 37 | #ifndef EGM_BASE_INTERFACE_H 38 | #define EGM_BASE_INTERFACE_H 39 | 40 | #include 41 | 42 | #include "abb_libegm_export.h" 43 | 44 | #include "egm.pb.h" // Generated by Google Protocol Buffer compiler protoc 45 | #include "egm_wrapper.pb.h" // Generated by Google Protocol Buffer compiler protoc 46 | 47 | #include "egm_common.h" 48 | #include "egm_logger.h" 49 | #include "egm_udp_server.h" 50 | 51 | namespace abb 52 | { 53 | namespace egm 54 | { 55 | /** 56 | * \brief Class for processing asynchronous callbacks. 57 | * 58 | * The class provides behavior for: 59 | * - Processing asynchronous callbacks from a UDP server. The interface receives the robot controller's 60 | * outbound messages and construct inbound messages to the robot controller. 61 | * - This class can for example be used as a foundation for custom made user interfaces. 62 | */ 63 | class EGMBaseInterface : public AbstractUDPServerInterface 64 | { 65 | public: 66 | /** 67 | * \brief A constructor. 68 | * 69 | * \param io_service for operating boost asio's asynchronous functions. 70 | * \param port_number for the server's UDP socket. 71 | * \param configuration for the interface's configuration. 72 | */ 73 | EGMBaseInterface(boost::asio::io_service& io_service, 74 | const unsigned short port_number, 75 | const BaseConfiguration& configuration = BaseConfiguration()); 76 | 77 | /** 78 | * \brief Checks if the underlying server was successfully initialized or not. 79 | * 80 | * \return bool indicating if the underlying server was successfully initialized or not. 81 | */ 82 | bool isInitialized(); 83 | 84 | /** 85 | * \brief Checks if an EGM communication session is connected or not. 86 | * 87 | * \return bool indicating if a connection exists between the interface, and the robot controller's EGM client. 88 | */ 89 | bool isConnected(); 90 | 91 | /** 92 | * \brief Retrieve the most recently received EGM status message. 93 | * 94 | * The returned status depends on the EGM communication session(s): 95 | * - If no session has been active, then an empty status message is returned. 96 | * - If a session is active, then the most recently received status message is returned. 97 | * - If any session has been active, then the last status message from the latest session is returned. 98 | * 99 | * Note: EGMAct RAPID instructions specifies the frequency of EGM messages, and this affects how often 100 | * the status is updated when a communication session is active. 101 | * 102 | * \return wrapper::Status containing the most recently received EGM status message. 103 | */ 104 | wrapper::Status getStatus(); 105 | 106 | /** 107 | * \brief Retrieve the interface's current configuration. 108 | * 109 | * \return BaseConfiguration containing the current configurations for the interface. 110 | */ 111 | BaseConfiguration getConfiguration(); 112 | 113 | /** 114 | * \brief Update the interface's configuration (update is only applied for new EGM communication sessions). 115 | * 116 | * \param configuration containing the new configurations for the interface. 117 | */ 118 | void setConfiguration(const BaseConfiguration& configuration); 119 | 120 | protected: 121 | /** 122 | * \brief Class for containing inputs from a UDP server. 123 | */ 124 | class InputContainer 125 | { 126 | public: 127 | /** 128 | * \brief Default constructor. 129 | */ 130 | InputContainer(); 131 | 132 | /** 133 | * \brief Parse an array, into an abb::egm::EgmRobot message. 134 | * 135 | * \param data containing the serialized array received from the robot controller. 136 | * \param bytes_transferred for the number of bytes received. 137 | * 138 | * \return bool indicating if the parsing was successful or not. 139 | */ 140 | bool parseFromArray(const char* data, const int bytes_transferred); 141 | 142 | /** 143 | * \brief Extract the parsed information. 144 | * 145 | * \param axes specifying the number of axes of the robot. 146 | * 147 | * \return bool indicating if the extraction was successful or not. 148 | */ 149 | bool extractParsedInformation(const RobotAxes& axes); 150 | 151 | /** 152 | * \brief Update the previous inputs with the current inputs. 153 | */ 154 | void updatePrevious(); 155 | 156 | /** 157 | * \brief Retrieve the initial inputs (i.e initial robot controller outputs). 158 | * 159 | * \return Input with the initial inputs. 160 | */ 161 | const wrapper::Input& initial() const { return initial_; }; 162 | 163 | /** 164 | * \brief Retrieve the current inputs (i.e. current robot controller outputs). 165 | * 166 | * \return Input with the current inputs. 167 | */ 168 | const wrapper::Input& current() const { return current_; }; 169 | 170 | /** 171 | * \brief Retrieve the previous inputs (i.e. previous robot controller outputs). 172 | * 173 | * \return Input with the previous inputs. 174 | */ 175 | const wrapper::Input& previous() const { return previous_; }; 176 | 177 | /** 178 | * \brief Retrieve the estimated sample time [s]. 179 | * 180 | * \return double containing the estimation. 181 | */ 182 | double estimatedSampleTime() const { return estimated_sample_time_; }; 183 | 184 | /** 185 | * \brief Retrieve a flag, indicating if the received message was the first in a communication session. 186 | * 187 | * \return bool indicating if it was the first message or not. 188 | */ 189 | bool isFirstMessage() const { return first_message_; }; 190 | 191 | /** 192 | * \brief Check if the robot controller's states are ok. 193 | * 194 | * I.e. motors are on, RAPID is running and EGM is running. 195 | * 196 | * \return bool indicating if the state are ok or not. 197 | */ 198 | bool statesOk() const; 199 | 200 | private: 201 | /** 202 | * \brief Detect RobotWare and EGM protocol versions from a received EGM message. 203 | * 204 | * Note: Only a rough version detection is possible based on whether certain fields are present or not. 205 | */ 206 | void detectRWAndEGMVersions(); 207 | 208 | /** 209 | * \brief Estimate the sample time. 210 | * 211 | * \return double containing the estimation. 212 | */ 213 | double estimateSampleTime(); 214 | 215 | /** 216 | * \brief Estimate the joint and the Cartesian velocities. 217 | * 218 | * \return bool indicating if the estimation was successful or not. 219 | */ 220 | bool estimateAllVelocities(); 221 | 222 | /** 223 | * \brief Container for the "raw" EGM robot message. 224 | */ 225 | EgmRobot egm_robot_; 226 | 227 | /** 228 | * \brief Container for the initial inputs, extracted from the EGM robot message. 229 | */ 230 | wrapper::Input initial_; 231 | 232 | /** 233 | * \brief Container for the current inputs, extracted from the EGM robot message. 234 | */ 235 | wrapper::Input current_; 236 | 237 | /** 238 | * \brief Container for the previous inputs, extracted from the EGM robot message. 239 | */ 240 | wrapper::Input previous_; 241 | 242 | /** 243 | * \brief Flag indicating if new data has been received. 244 | */ 245 | bool has_new_data_; 246 | 247 | /** 248 | * \brief Flag indicating if the interface's callback has been called before or not. 249 | */ 250 | bool first_call_; 251 | 252 | /** 253 | * \brief Flag indicating if the received message was the first in a communication session or not. 254 | */ 255 | bool first_message_; 256 | 257 | /** 258 | * \brief The estimated sample time [s]. 259 | */ 260 | double estimated_sample_time_; 261 | }; 262 | 263 | /** 264 | * \brief Class for containing outputs to a UDP server. 265 | */ 266 | class OutputContainer 267 | { 268 | public: 269 | /** 270 | * \brief Default constructor. 271 | */ 272 | OutputContainer(); 273 | 274 | /** 275 | * \brief Prepare the outputs. 276 | * 277 | * \param inputs containing the inputs. 278 | */ 279 | void prepareOutputs(const InputContainer& inputs); 280 | 281 | /** 282 | * \brief Generate demo outputs. 283 | * 284 | * \param inputs containing the inputs from the robot controller. 285 | */ 286 | void generateDemoOutputs(const InputContainer& inputs); 287 | 288 | /** 289 | * \brief Construct the reply string. 290 | * 291 | * \param configuration containing the current configurations for the interface. 292 | */ 293 | void constructReply(const BaseConfiguration& configuration); 294 | 295 | /** 296 | * \brief Update the previous outputs with the current outputs. 297 | */ 298 | void updatePrevious(); 299 | 300 | /** 301 | * \brief Retrieve the previous outputs sent to the robot controller. 302 | * 303 | * \return Output with the previous outputs. 304 | */ 305 | const wrapper::Output& previous() const { return previous_; }; 306 | 307 | /** 308 | * \brief Retrieve the current sequence_number. 309 | * 310 | * \return unsigned int containing the sequence number. 311 | */ 312 | unsigned int sequenceNumber() const { return sequence_number_; }; 313 | 314 | /** 315 | * \brief Retrieve the reply string, serialized from the current references. 316 | * 317 | * \return string& containing the reply. 318 | */ 319 | const std::string& reply() const { return reply_; }; 320 | 321 | /** 322 | * \brief Clear the reply content. 323 | */ 324 | void clearReply() { reply_.clear(); }; 325 | 326 | /** 327 | * \brief Container for the current outputs to send to the robot controller. 328 | */ 329 | wrapper::Output current; 330 | 331 | private: 332 | /** 333 | * \brief Generate demo quaternion outputs. 334 | * 335 | * \param inputs containing the inputs from the robot controller. 336 | * \param t for the interpolation parameter [0 <= t <= 1]. 337 | */ 338 | void generateDemoQuaternions(const InputContainer& inputs, const double t); 339 | 340 | /** 341 | * \brief Construct the header. 342 | */ 343 | void constructHeader(); 344 | 345 | /** 346 | * \brief Construct the joint body. 347 | * 348 | * \param configuration containing the current configurations for the interface. 349 | * 350 | * \return bool indicating if the construction was successful or not. 351 | */ 352 | bool constructJointBody(const BaseConfiguration& configuration); 353 | 354 | /** 355 | * \brief Construct the Cartesian body. 356 | * 357 | * \param configuration containing the current configurations for the interface. 358 | * 359 | * \return bool indicating if the construction was successful or not. 360 | */ 361 | bool constructCartesianBody(const BaseConfiguration& configuration); 362 | 363 | /** 364 | * \brief Container for the actual EGM sensor message. 365 | */ 366 | EgmSensor egm_sensor_; 367 | 368 | /** 369 | * \brief Container for the previous outputs sent to the robot controller. 370 | */ 371 | wrapper::Output previous_; 372 | 373 | /** 374 | * \brief The sequence number, in the current communication session. 375 | */ 376 | unsigned int sequence_number_; 377 | 378 | /** 379 | * \brief Container for the reply string. 380 | */ 381 | std::string reply_; 382 | }; 383 | 384 | /** 385 | * \brief Struct for containing data regarding an active EGM communication session. 386 | */ 387 | struct SessionData 388 | { 389 | /** 390 | * \brief Container for the most recently received EGM header message. 391 | */ 392 | wrapper::Header header; 393 | 394 | /** 395 | * \brief Container for the most recently received EGM status message. 396 | */ 397 | wrapper::Status status; 398 | 399 | /** 400 | * \brief Mutex for protecting the session data. 401 | */ 402 | boost::mutex mutex; 403 | }; 404 | 405 | /** 406 | * \brief Struct for containing the base configuration data. 407 | */ 408 | struct BaseConfigurationContainer 409 | { 410 | /** 411 | * \brief A constructor. 412 | * 413 | * \param initial specifying the initial configuration. 414 | */ 415 | BaseConfigurationContainer(const BaseConfiguration& initial) 416 | : 417 | active(initial), 418 | update(initial), 419 | has_pending_update(false) 420 | {} 421 | 422 | /** 423 | * \brief The active configuration. 424 | */ 425 | BaseConfiguration active; 426 | 427 | /** 428 | * \brief The configuration update. 429 | */ 430 | BaseConfiguration update; 431 | 432 | /** 433 | * \brief Flag indicating if the active configuration should be updated. 434 | */ 435 | bool has_pending_update; 436 | 437 | /** 438 | * \brief Mutex for protecting the configuration. 439 | */ 440 | boost::mutex mutex; 441 | }; 442 | 443 | /** 444 | * \brief Log input, from robot controller, and output, to robot controller, into a CSV file. 445 | * 446 | * \param inputs containing the inputs from the robot controller. 447 | * \param outputs containing the outputs to the robot controller. 448 | * \param max_time specifying the max amount of time to log. 449 | */ 450 | void logData(const InputContainer& inputs, const OutputContainer& outputs, const double max_time); 451 | 452 | /** 453 | * \brief Initialize the callback. 454 | * 455 | * \param server_data containing the UDP server's callback data. 456 | * 457 | * \return bool indicating if the initialization succeeded or not. 458 | */ 459 | bool initializeCallback(const UDPServerData& server_data); 460 | 461 | /** 462 | * \brief Static constant wait time [ms] used when determining if a connection has been established or not. 463 | * 464 | * I.e. a connection between the interface's UDP server, and a robot controller's EGM client. 465 | */ 466 | static const unsigned int WAIT_TIME_MS = 100; 467 | 468 | /** 469 | * \brief Container for the inputs, to the interface, from the UDP server. 470 | */ 471 | InputContainer inputs_; 472 | 473 | /** 474 | * \brief Container for the outputs, from the interface, to the UDP server. 475 | */ 476 | OutputContainer outputs_; 477 | 478 | /** 479 | * \brief Container for session data (most recently received header and status messages). 480 | */ 481 | SessionData session_data_; 482 | 483 | /** 484 | * \brief Logger, for logging EGM messages to a CSV file. 485 | */ 486 | boost::shared_ptr p_logger_; 487 | 488 | /** 489 | * \brief The interface's configuration. 490 | */ 491 | BaseConfigurationContainer configuration_; 492 | 493 | /** 494 | * \brief Server for managing the communication with the robot controller. 495 | */ 496 | UDPServer udp_server_; 497 | 498 | private: 499 | /** 500 | * \brief Handle callback requests from an UDP server. 501 | * 502 | * \param server_data containing the UDP server's callback data. 503 | * 504 | * \return string& containing the reply. 505 | */ 506 | const std::string& callback(const UDPServerData& server_data); 507 | }; 508 | 509 | } // end namespace egm 510 | } // end namespace abb 511 | 512 | #endif // EGM_BASE_INTERFACE_H 513 | -------------------------------------------------------------------------------- /include/abb_libegm/egm_common.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************************************** 2 | * 3 | * Copyright (c) 2015, ABB Schweiz AG 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with 7 | * or without modification, are permitted provided that 8 | * the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the 11 | * above copyright notice, this list of conditions 12 | * and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the 14 | * above copyright notice, this list of conditions 15 | * and the following disclaimer in the documentation 16 | * and/or other materials provided with the 17 | * distribution. 18 | * * Neither the name of ABB nor the names of its 19 | * contributors may be used to endorse or promote 20 | * products derived from this software without 21 | * specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 32 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *********************************************************************************************************************** 35 | */ 36 | 37 | #ifndef EGM_COMMON_H 38 | #define EGM_COMMON_H 39 | 40 | #include 41 | #include 42 | 43 | #include "abb_libegm_export.h" 44 | 45 | namespace abb 46 | { 47 | namespace egm 48 | { 49 | /** 50 | * \brief Enum for the number of axes of the robot. 51 | */ 52 | enum RobotAxes 53 | { 54 | None = 0, ///< \brief No robot axes are expected (i.e. only external axes). 55 | Six = 6, ///< \brief A six axes robot. 56 | Seven = 7 ///< \brief A seven axes robot. 57 | }; 58 | 59 | /** 60 | * \brief Enum for the supported EGM modes (i.e. the corresponding RAPID instructions). 61 | */ 62 | enum EGMModes 63 | { 64 | EGMJoint, ///< \brief The EGM joint mode. 65 | EGMPose ///< \brief The EGM pose mode. 66 | }; 67 | 68 | /** 69 | * \brief Struct containing various constant values. 70 | */ 71 | struct Constants 72 | { 73 | /** 74 | * \brief Constants related to the robot controller system. 75 | */ 76 | struct ABB_LIBEGM_EXPORT RobotController 77 | { 78 | /** 79 | * \brief Lowest sample time [s] used in EGM communication. 80 | */ 81 | static const double LOWEST_SAMPLE_TIME; 82 | 83 | /** 84 | * \brief Default port number assumed for EGM communication. 85 | */ 86 | static const unsigned short DEFAULT_PORT_NUMBER; 87 | 88 | /** 89 | * \brief Default number of robot joints. 90 | */ 91 | static const int DEFAULT_NUMBER_OF_ROBOT_JOINTS; 92 | 93 | /** 94 | * \brief Default number of external joints. 95 | */ 96 | static const int DEFAULT_NUMBER_OF_EXTERNAL_JOINTS; 97 | 98 | /** 99 | * \brief Maximum number of joints. 100 | */ 101 | static const int MAX_NUMBER_OF_JOINTS; 102 | }; 103 | 104 | /** 105 | * \brief Constants related to the conversions between units. 106 | */ 107 | struct ABB_LIBEGM_EXPORT Conversion 108 | { 109 | /** 110 | * \brief Conversion value from radians to degrees. 111 | */ 112 | static const double RAD_TO_DEG; 113 | 114 | /** 115 | * \brief Conversion value from degrees to radians. 116 | */ 117 | static const double DEG_TO_RAD; 118 | 119 | /** 120 | * \brief Conversion value from millimeter to meter. 121 | */ 122 | static const double MM_TO_M; 123 | 124 | /** 125 | * \brief Conversion value from milliseconds to seconds. 126 | */ 127 | static const double MS_TO_S; 128 | 129 | /** 130 | * \brief Conversion value from seconds to microseconds. 131 | */ 132 | static const double S_TO_US; 133 | }; 134 | }; 135 | 136 | /** 137 | * \brief Struct for an EGM user interface's base configuration. 138 | */ 139 | struct BaseConfiguration 140 | { 141 | /** 142 | * \brief Default constructor. 143 | */ 144 | BaseConfiguration() 145 | : 146 | axes(Six), 147 | use_demo_outputs(false), 148 | use_velocity_outputs(false), 149 | use_logging(false), 150 | max_logging_duration(60.0) 151 | {} 152 | 153 | /** 154 | * \brief Value specifying if a six or seven axes robot is used. 155 | * 156 | * Note: If set to a seven axes robot, then an implicit mapping of joint values is performed. 157 | */ 158 | RobotAxes axes; 159 | 160 | /** 161 | * \brief Flag indicating if demo outputs should be used. 162 | * 163 | * Note: Overrides any other execution mode. Mainly used to verify that the EGM communication channel 164 | * works as intended. 165 | */ 166 | bool use_demo_outputs; 167 | 168 | /** 169 | * \brief Flag indicating if the messages, sent to the robot controller, should include velocity outputs. 170 | * 171 | * Note: If set to false, then no velocity values are sent (they are optional). 172 | */ 173 | bool use_velocity_outputs; 174 | 175 | /** 176 | * \brief Flag indicating if the interface should log data. 177 | */ 178 | bool use_logging; 179 | 180 | /** 181 | * \brief Maximum duration [s] to log data. 182 | */ 183 | double max_logging_duration; 184 | 185 | /** 186 | * \brief Optional condition variable intended for notifying an external control loop that a new message is available. 187 | * 188 | * Note: This is only intended to be used in the EGMControllerInterface class. 189 | */ 190 | boost::shared_ptr p_new_message_cv; 191 | }; 192 | 193 | /** 194 | * \brief Struct for the EGM trajectory user interface's configuration. 195 | */ 196 | struct TrajectoryConfiguration 197 | { 198 | /** 199 | * \brief Enum for the available spline polynomial interpolation methods. 200 | * 201 | * Note: Cartesian orientation uses Slerp interpolation instead. 202 | * 203 | * Boundary conditions (between trajectory points): 204 | * | 205 | * |--Linear: 206 | * | |-- Start and goal positions. 207 | * | 208 | * |--Square: 209 | * | |-- Start and goal positions. 210 | * | |-- Start velocity. 211 | * | 212 | * |--Cubic: 213 | * | |-- Start and goal positions. 214 | * | |-- Start and goal velocities. 215 | * | 216 | * |--Quintic: 217 | * |-- Start and goal positions. 218 | * |-- Start and goal velocities. 219 | * |-- Start and goal accelerations. 220 | */ 221 | enum SplineMethod 222 | { 223 | Linear, ///< \brief Use a first degree polynomial. 224 | Square, ///< \brief Use a second degree polynomial. 225 | Cubic, ///< \brief Use a third degree polynomial 226 | Quintic ///< \brief Use a fifth degree polynomial. 227 | }; 228 | 229 | /** 230 | * \brief A constructor. 231 | * 232 | * \param base_configuration specifying the base configurations. 233 | */ 234 | TrajectoryConfiguration(const BaseConfiguration& base_configuration = BaseConfiguration()) 235 | : 236 | base(base_configuration), 237 | spline_method(Quintic) 238 | {} 239 | 240 | /** 241 | * \brief The base configurations. 242 | */ 243 | BaseConfiguration base; 244 | 245 | /** 246 | * \brief Value specifying which spline method to use in the interpolation. 247 | */ 248 | SplineMethod spline_method; 249 | }; 250 | 251 | } // end namespace egm 252 | } // end namespace abb 253 | 254 | #endif // EGM_COMMON_H 255 | -------------------------------------------------------------------------------- /include/abb_libegm/egm_common_auxiliary.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************************************** 2 | * 3 | * Copyright (c) 2015, ABB Schweiz AG 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with 7 | * or without modification, are permitted provided that 8 | * the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the 11 | * above copyright notice, this list of conditions 12 | * and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the 14 | * above copyright notice, this list of conditions 15 | * and the following disclaimer in the documentation 16 | * and/or other materials provided with the 17 | * distribution. 18 | * * Neither the name of ABB nor the names of its 19 | * contributors may be used to endorse or promote 20 | * products derived from this software without 21 | * specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 32 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *********************************************************************************************************************** 35 | */ 36 | 37 | #ifndef EGM_COMMON_AUXILIARY_H 38 | #define EGM_COMMON_AUXILIARY_H 39 | 40 | #include "abb_libegm_export.h" 41 | 42 | #include "egm.pb.h" // Generated by Google Protocol Buffer compiler protoc 43 | #include "egm_wrapper.pb.h" // Generated by Google Protocol Buffer compiler protoc 44 | 45 | #include "egm_common.h" 46 | 47 | namespace abb 48 | { 49 | namespace egm 50 | { 51 | /** 52 | * \brief Saturate a specified value between a lower and a upper bound. 53 | * 54 | * \param value for the value to saturate. 55 | * \param lower for the lower bound. 56 | * \param upper for the upper bound. 57 | * 58 | * \return double with the saturated value. 59 | */ 60 | double saturate(const double value, const double lower, const double upper); 61 | 62 | /** 63 | * \brief Multiply all values in a joints object with a factor. 64 | * 65 | * \param p_j for the joints object. 66 | * \param factor for the value to multiply with. 67 | */ 68 | void multiply(wrapper::Joints* p_j, const double factor); 69 | 70 | /** 71 | * \brief Multiply all values in a Cartesian object with a factor. 72 | * 73 | * \param p_c for the Cartesian object. 74 | * \param factor for the value to multiply with. 75 | */ 76 | void multiply(wrapper::Cartesian* p_c, const double factor); 77 | 78 | /** 79 | * \brief Multiply all values in an Euler object with a factor. 80 | * 81 | * \param p_e for the Euler object. 82 | * \param factor for the value to multiply with. 83 | */ 84 | void multiply(wrapper::Euler* p_e, const double factor); 85 | 86 | /** 87 | * \brief Multiply all values in a quaternion object with a factor. 88 | * 89 | * \param p_q for the quaternion object. 90 | * \param factor for the value to multiply with. 91 | */ 92 | void multiply(wrapper::Quaternion* p_q, const double factor); 93 | 94 | /** 95 | * \brief Multiply two quaternions together. 96 | * 97 | * \param q1 for the first quaternion. 98 | * \param q2 for the second quaternion. 99 | * 100 | * \return Quaternion containing the result. 101 | */ 102 | wrapper::Quaternion multiply(const wrapper::Quaternion& q1, const wrapper::Quaternion& q2); 103 | 104 | /** 105 | * \brief Calculate the dot product between two quaternions. 106 | * 107 | * \param q1 for the first quaternion. 108 | * \param q2 for the second quaternion. 109 | * 110 | * \return double containing the dot product value. 111 | */ 112 | double dotProduct(const wrapper::Quaternion& q1, const wrapper::Quaternion& q2); 113 | 114 | /** 115 | * \brief Calculate the Euclidean norm of a quaternion. 116 | * 117 | * \param q for the quaternion to calculate the norm of. 118 | * 119 | * \return double containing the Euclidean norm value. 120 | */ 121 | double euclideanNorm(const wrapper::Quaternion& q); 122 | 123 | /** 124 | * \brief Normalize a quaternion. 125 | * 126 | * \param p_q for the quaternion to normalize. 127 | */ 128 | void normalize(wrapper::Quaternion* p_q); 129 | 130 | /** 131 | * \brief Convert ZYX Euler angles to a quaternion. 132 | * 133 | * \param p_q for containing the calculated quaternion. 134 | * \param e for the ZYX Euler angles to convert. 135 | */ 136 | void convert(wrapper::Quaternion* p_q, const wrapper::Euler& e); 137 | 138 | /** 139 | * \brief Convert a quaternion to ZYX Euler angles. 140 | * 141 | * \param p_e for containing the calculated ZYX Euler angles. 142 | * \param q for the quaternion to convert. 143 | */ 144 | void convert(wrapper::Euler* p_e, const wrapper::Quaternion& q); 145 | 146 | /** 147 | * \brief Convert angular velocities to quaternion derivate. 148 | * 149 | * \param p_dq for containing the calculated quaternion derivate. 150 | * \param previous_q for the previous quaternion. 151 | * \param av for the angular velocities to convert. 152 | */ 153 | void convert(wrapper::Quaternion* p_dq, const wrapper::Quaternion& previous_q, const wrapper::Euler& av); 154 | 155 | /** 156 | * \brief Estimate joint velocities. 157 | * 158 | * The estimation will fail if the sample time is less than or equal to zero. 159 | * 160 | * \param p_estimate for containing the estimated joint velocities. 161 | * \param current current joint positions. 162 | * \param previous previous joint positions. 163 | * \param sample_time current sample time. 164 | * 165 | * \return bool indicating if the estimation succeeded. 166 | */ 167 | bool estimateVelocities(wrapper::Joints* p_estimate, 168 | const wrapper::Joints& current, 169 | const wrapper::Joints& previous, 170 | const double sample_time); 171 | 172 | /** 173 | * \brief Estimate Cartesian angular velocities. 174 | * 175 | * The estimation will fail if the sample time is less than or equal to zero. 176 | * 177 | * \param p_estimate for containing the estimated Cartesian angular velocities. 178 | * \param current current quaternion. 179 | * \param previous previous quaternion. 180 | * \param sample_time current the sample time. 181 | * 182 | * \return bool indicating if the estimation succeeded. 183 | */ 184 | bool estimateVelocities(wrapper::Euler* p_estimate, 185 | const wrapper::Quaternion& current, 186 | const wrapper::Quaternion& previous, 187 | const double sample_time); 188 | 189 | /** 190 | * \brief Estimate Cartesian velocities. 191 | * 192 | * The estimation will fail if the sample time is less than or equal to zero. 193 | * 194 | * \param p_estimate for containing the estimated Cartesian velocities. 195 | * \param current current Cartesian position and orientation. 196 | * \param previous previous Cartesian position and orientation. 197 | * \param sample_time current the sample time. 198 | * 199 | * \return bool indicating if the estimation succeeded. 200 | */ 201 | bool estimateVelocities(wrapper::CartesianVelocity* p_estimate, 202 | const wrapper::CartesianPose& current, 203 | const wrapper::CartesianPose& previous, 204 | const double sample_time); 205 | 206 | /** 207 | * \brief Find the maximum difference between two joints objects. 208 | * 209 | * The maximum difference is determined by the absolute difference for each joint pair. 210 | * The number of joints therefore needs to be the same, and the components should represent 211 | * the same robot joints or external axes. 212 | * 213 | * \param j1 for the first joints object. 214 | * \param j2 for the second joints object. 215 | * 216 | * \return double with the maximum difference. 217 | */ 218 | double findMaxDifference(const wrapper::Joints& j1, const wrapper::Joints& j2); 219 | 220 | /** 221 | * \brief Find the maximum difference between two Cartesian objects. 222 | * 223 | * The maximum difference is determined by the absolute difference for each position pair. 224 | * The components should represent the position of the same robot (in the same frame of reference). 225 | * 226 | * \param c1 for the first Cartesian object. 227 | * \param c2 for the second Cartesian object. 228 | * 229 | * \return double with the maximum difference. 230 | */ 231 | double findMaxDifference(const wrapper::Cartesian& c1, const wrapper::Cartesian& c2); 232 | 233 | /** 234 | * \brief Find the maximum difference between two Euler objects. 235 | * 236 | * The maximum difference is determined by the absolute difference for each orientation pair. 237 | * The components should represent the orientation of a robot (in the same frame of reference). 238 | * 239 | * \param e1 for the first Euler object. 240 | * \param e2 for the second Euler object. 241 | * 242 | * \return double with the maximum difference. 243 | */ 244 | double findMaxDifference(const wrapper::Euler& e1, const wrapper::Euler& e2); 245 | 246 | /** 247 | * \brief Copy the data present in one joints object to another. 248 | * 249 | * \param p_target for the target where the copied data should be put. 250 | * \param source containing the data to copy. 251 | */ 252 | void copyPresent(wrapper::Joints* p_target, const wrapper::Joints& source); 253 | 254 | /** 255 | * \brief Copy the data present in one joint space object to another. 256 | * 257 | * \param p_target for the target where the copied data should be put. 258 | * \param source containing the data to copy. 259 | */ 260 | void copyPresent(wrapper::JointSpace* p_target, const wrapper::JointSpace& source); 261 | 262 | /** 263 | * \brief Copy the data present in one Cartesian object to another. 264 | * 265 | * \param p_target for the target where the copied data should be put. 266 | * \param source containing the data to copy. 267 | */ 268 | void copyPresent(wrapper::Cartesian* p_target, const wrapper::Cartesian& source); 269 | 270 | /** 271 | * \brief Copy the data present in one Euler object to another. 272 | * 273 | * \param p_target for the target where the copied data should be put. 274 | * \param source containing the data to copy. 275 | */ 276 | void copyPresent(wrapper::Euler* p_target, const wrapper::Euler& source); 277 | 278 | /** 279 | * \brief Copy the data present in one quaternion object to another. 280 | * 281 | * \param p_target for the target where the copied data should be put. 282 | * \param source containing the data to copy. 283 | */ 284 | void copyPresent(wrapper::Quaternion* p_target, const wrapper::Quaternion& source); 285 | 286 | /** 287 | * \brief Copy the data present in one Cartesian pose object to another. 288 | * 289 | * \param p_target for the target where the copied data should be put. 290 | * \param source containing the data to copy. 291 | */ 292 | void copyPresent(wrapper::CartesianPose* p_target, const wrapper::CartesianPose& source); 293 | 294 | /** 295 | * \brief Copy the data present in one Cartesian velocity object to another. 296 | * 297 | * \param p_target for the target where the copied data should be put. 298 | * \param source containing the data to copy. 299 | */ 300 | void copyPresent(wrapper::CartesianVelocity* p_target, const wrapper::CartesianVelocity& source); 301 | 302 | /** 303 | * \brief Copy the data present in one Cartesian space object to another. 304 | * 305 | * \param p_target for the target where the copied data should be put. 306 | * \param source containing the data to copy. 307 | */ 308 | void copyPresent(wrapper::CartesianSpace* p_target, const wrapper::CartesianSpace& source); 309 | 310 | /** 311 | * \brief Copy the data present in one robot object to another. 312 | * 313 | * \param p_target for the target where the copied data should be put. 314 | * \param source containing the data to copy. 315 | */ 316 | void copyPresent(wrapper::Robot* p_target, const wrapper::Robot& source); 317 | 318 | /** 319 | * \brief Copy the data present in one external object to another. 320 | * 321 | * \param p_target for the target where the copied data should be put. 322 | * \param source containing the data to copy. 323 | */ 324 | void copyPresent(wrapper::External* p_target, const wrapper::External& source); 325 | 326 | /** 327 | * \brief Copy the data present in one output object to another. 328 | * 329 | * \param p_target for the target where the copied data should be put. 330 | * \param source containing the data to copy. 331 | */ 332 | void copyPresent(wrapper::Output* p_target, const wrapper::Output& source); 333 | 334 | /** 335 | * \brief Parse an abb::egm::EgmHeader object. 336 | * 337 | * \param p_target for containing the parsed data. 338 | * \param source containing data to parse. 339 | * 340 | * \return bool indicating if the parsing was successful or not. 341 | */ 342 | bool parse(wrapper::Header* p_target, const EgmHeader& source); 343 | 344 | /** 345 | * \brief Parse an abb::egm::EgmRobot object. 346 | * 347 | * \param p_target for containing the parsed data. 348 | * \param source containing data to parse. 349 | * 350 | * \return bool indicating if the parsing was successful or not. 351 | */ 352 | bool parse(wrapper::Status* p_target, const EgmRobot& source); 353 | 354 | /** 355 | * \brief Parse an abb::egm::EgmClock object. 356 | * 357 | * \param p_target for containing the parsed data. 358 | * \param source containing data to parse. 359 | * 360 | * \return bool indicating if the parsing was successful or not. 361 | */ 362 | bool parse(wrapper::Clock* p_target, const EgmClock& source); 363 | 364 | /** 365 | * \brief Parse two abb::egm::EgmJoints objects. 366 | * 367 | * \param p_target_robot for containing the parsed robot data. 368 | * \param p_target_external for containing the parsed external data. 369 | * \param source_robot containing robot data to parse. 370 | * \param source_external containing external data to parse. 371 | * \param axes specifying the number of axes of the robot. 372 | * 373 | * \return bool indicating if the parsing was successful or not. 374 | */ 375 | bool parse(wrapper::Joints* p_target_robot, 376 | wrapper::Joints* p_target_external, 377 | const EgmJoints& source_robot, 378 | const EgmJoints& source_external, 379 | const RobotAxes axes); 380 | 381 | /** 382 | * \brief Parse an abb::egm::EgmPose object. 383 | * 384 | * \param p_target for containing the parsed data. 385 | * \param source containing data to parse. 386 | * 387 | * \return bool indicating if the parsing was successful or not. 388 | */ 389 | bool parse(wrapper::CartesianPose* p_target, const EgmPose& source); 390 | 391 | /** 392 | * \brief Parse an abb::egm::EgmFeedBack object. 393 | * 394 | * \param p_target for containing the parsed data. 395 | * \param source containing data to parse. 396 | * \param axes specifying the number of axes of the robot. 397 | * 398 | * \return bool indicating if the parsing was successful or not. 399 | */ 400 | bool parse(wrapper::Feedback* p_target, const EgmFeedBack& source, const RobotAxes axes); 401 | 402 | /** 403 | * \brief Parse an abb::egm::Planned object. 404 | * 405 | * \param p_target for containing the parsed data. 406 | * \param source containing data to parse. 407 | * \param axes specifying the number of axes of the robot. 408 | * 409 | * \return bool indicating if the parsing was successful or not. 410 | */ 411 | bool parse(wrapper::Planned* p_target, const EgmPlanned& source, const RobotAxes axes); 412 | 413 | /** 414 | * \brief Reset all values (i.e. set to zero) in a joints object. 415 | * 416 | * \param p_joints for the object to reset. 417 | * \param number_of_joints for the desired number of joints to reset. 418 | */ 419 | void reset(wrapper::Joints* p_joints, const unsigned int number_of_joints); 420 | 421 | /** 422 | * \brief Reset all values (i.e. set to zero) in a Cartesian object. 423 | * 424 | * \param p_cartesian for the object to reset. 425 | */ 426 | void reset(wrapper::Cartesian* p_cartesian); 427 | 428 | /** 429 | * \brief Reset all values (i.e. set to zero) in a Euler object. 430 | * 431 | * \param p_euler for the object to reset. 432 | */ 433 | void reset(wrapper::Euler* p_euler); 434 | 435 | /** 436 | * \brief Verify that a double value is neither NaN nor infinity. 437 | * 438 | * \param value to check. 439 | * 440 | * \return bool true if the value is ok. 441 | */ 442 | bool verify(const double value); 443 | 444 | /** 445 | * \brief Verify that a joints object contain neither NaN nor infinity values. 446 | * 447 | * \param joints to check. 448 | * 449 | * \return bool true if the values are ok. 450 | */ 451 | bool verify(const wrapper::Joints& joints); 452 | 453 | /** 454 | * \brief Verify that a Cartesian object contain neither NaN nor infinity values. 455 | * 456 | * \param cartesian to check. 457 | * 458 | * \return bool true if the values are ok. 459 | */ 460 | bool verify(const wrapper::Cartesian& cartesian); 461 | 462 | /** 463 | * \brief Verify that a Euler object contain neither NaN nor infinity values. 464 | * 465 | * \param euler to check. 466 | * 467 | * \return bool true if the values are ok. 468 | */ 469 | bool verify(const wrapper::Euler& euler); 470 | 471 | /** 472 | * \brief Verify that a quaternion object contain neither NaN nor infinity values. 473 | * 474 | * \param quaternion to check. 475 | * 476 | * \return bool true if the values are ok. 477 | */ 478 | bool verify(const wrapper::Quaternion& quaternion); 479 | 480 | /** 481 | * \brief Verify that a Cartesian pose object contain neither NaN nor infinity values. 482 | * 483 | * \param pose to check. 484 | * 485 | * \return bool true if the values are ok. 486 | */ 487 | bool verify(const wrapper::CartesianPose& pose); 488 | 489 | /** 490 | * \brief Verify that a Cartesian velocity object contain neither NaN nor infinity values. 491 | * 492 | * \param velocity to check. 493 | * 494 | * \return bool true if the values are ok. 495 | */ 496 | bool verify(const wrapper::CartesianVelocity& velocity); 497 | 498 | } // end namespace egm 499 | } // end namespace abb 500 | 501 | #endif // EGM_COMMON_AUXILIARY_H 502 | -------------------------------------------------------------------------------- /include/abb_libegm/egm_controller_interface.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************************************** 2 | * 3 | * Copyright (c) 2015, ABB Schweiz AG 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with 7 | * or without modification, are permitted provided that 8 | * the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the 11 | * above copyright notice, this list of conditions 12 | * and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the 14 | * above copyright notice, this list of conditions 15 | * and the following disclaimer in the documentation 16 | * and/or other materials provided with the 17 | * distribution. 18 | * * Neither the name of ABB nor the names of its 19 | * contributors may be used to endorse or promote 20 | * products derived from this software without 21 | * specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 32 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *********************************************************************************************************************** 35 | */ 36 | 37 | #ifndef EGM_CONTROLLER_INTERFACE_H 38 | #define EGM_CONTROLLER_INTERFACE_H 39 | 40 | #include "egm_base_interface.h" 41 | 42 | namespace abb 43 | { 44 | namespace egm 45 | { 46 | /** 47 | * \brief Class for an EGM contoller user interface. 48 | * 49 | * The class provides behavior for motion control, and this includes: 50 | * - Processing asynchronous callbacks from an UDP server. 51 | * - Notifying an external control loop about new messages. 52 | * - Incorporating external control loop inputs, and sending them to the robot controller. 53 | * 54 | * Pseudocode for the usage of the class methods (inside an external control loop); 55 | * 1. if(waitForMessage(...)) 56 | * 1.1. read(...) 57 | * 1.2. write(...) 58 | * 1.3. Repeat from 1. 59 | */ 60 | class EGMControllerInterface : public EGMBaseInterface 61 | { 62 | public: 63 | /** 64 | * \brief A constructor. 65 | * 66 | * \param io_service for operating boost asio's asynchronous functions. 67 | * \param port_number for the server's UDP socket. 68 | * \param configuration for the interface's configuration. 69 | */ 70 | EGMControllerInterface(boost::asio::io_service& io_service, 71 | const unsigned short port_number, 72 | const BaseConfiguration& configuration = BaseConfiguration()); 73 | 74 | /** 75 | * \brief Wait for the next EGM message. 76 | * 77 | * \param timeout_ms for specifying a timeout in [ms]. If omitted, then the method waits forever. 78 | * 79 | * \return bool indicating if the wait was successful or not. I.e. returns false if a timeout has occurred. 80 | */ 81 | bool waitForMessage(const unsigned int timeout_ms = 0); 82 | 83 | /** 84 | * \brief Read EGM inputs received from the robot controller. 85 | * 86 | * \param p_inputs for containing the inputs. 87 | */ 88 | void read(wrapper::Input* p_inputs); 89 | 90 | /** 91 | * \brief Write EGM outputs to send to the robot controller. 92 | * 93 | * \param outputs containing the outputs. 94 | */ 95 | void write(const wrapper::Output& outputs); 96 | 97 | private: 98 | /** 99 | * \brief Class for managing controller motion data, between the inner loop and an external control loop. 100 | */ 101 | class ControllerMotion 102 | { 103 | public: 104 | /** 105 | * \brief Default constructor. 106 | */ 107 | ControllerMotion() : read_data_ready_(false), write_data_ready_(false) {} 108 | 109 | /** 110 | * \brief Initialize the motion data for a new communication session. 111 | * 112 | * \param first_message indicating if it is the first message in a communication session. 113 | */ 114 | void initialize(const bool first_message); 115 | 116 | /** 117 | * \brief Wait for the next message. 118 | * 119 | * \param timeout_ms for specifying a timeout in [ms]. If omitted, then the method waits forever. 120 | * 121 | * \return bool indicating if the wait was successful or not. I.e. returns false if a timeout has occurred. 122 | */ 123 | bool waitForMessage(const unsigned int timeout_ms); 124 | 125 | /** 126 | * \brief Write the current inputs (from the inner loop, to the intermediate storage). 127 | * 128 | * \param inputs for containing the inputs. 129 | */ 130 | void writeInputs(const wrapper::Input& inputs); 131 | 132 | /** 133 | * \brief Read the current inputs (from the intermediate storage, to the external loop). 134 | * 135 | * \param p_inputs for containing the inputs. 136 | */ 137 | void readInputs(wrapper::Input* p_inputs); 138 | 139 | /** 140 | * \brief Write the current outputs (from the external loop, to the intermediate storage). 141 | * 142 | * \param outputs for containing the outputs. 143 | */ 144 | void writeOutputs(const wrapper::Output& outputs); 145 | 146 | /** 147 | * \brief Read the current outputs (from the intermediate storage, to the inner loop). 148 | * 149 | * \param p_outputs for containing the outputs. 150 | */ 151 | void readOutputs(wrapper::Output* p_outputs); 152 | 153 | private: 154 | /** 155 | * \brief Static constant timeout [ms] for waiting on external control loop inputs. 156 | */ 157 | static const unsigned int WRITE_TIMEOUT_MS = 24; 158 | 159 | /** 160 | * \brief Mutex for protecting read data. 161 | */ 162 | boost::mutex read_mutex_; 163 | 164 | /** 165 | * \brief Mutex for protecting write data. 166 | */ 167 | boost::mutex write_mutex_; 168 | 169 | /** 170 | * \brief Condition variable for waiting on read data. 171 | */ 172 | boost::condition_variable read_condition_variable_; 173 | 174 | /** 175 | * \brief Condition variable for waiting on write data. 176 | */ 177 | boost::condition_variable write_condition_variable_; 178 | 179 | /** 180 | * \brief Flag indicating if read data is ready. 181 | */ 182 | bool read_data_ready_; 183 | 184 | /** 185 | * \brief Flag indicating if write data is ready. 186 | */ 187 | bool write_data_ready_; 188 | 189 | /** 190 | * \brief Container for the inputs received from the robot controller. 191 | */ 192 | wrapper::Input inputs_; 193 | 194 | /** 195 | * \brief Container for the outputs to send to the robot controller. 196 | */ 197 | wrapper::Output outputs_; 198 | }; 199 | 200 | /** 201 | * \brief Handle callback requests from an UDP server. 202 | * 203 | * \param server_data containing the UDP server's callback data. 204 | * 205 | * \return string& containing the reply. 206 | */ 207 | const std::string& callback(const UDPServerData& server_data); 208 | 209 | /** 210 | * \brief The interface's controller motion data (between internal loop and external controller loop). 211 | */ 212 | ControllerMotion controller_motion_; 213 | }; 214 | 215 | } // end namespace egm 216 | } // end namespace abb 217 | 218 | #endif // EGM_CONTROLLER_INTERFACE_H 219 | -------------------------------------------------------------------------------- /include/abb_libegm/egm_interpolator.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************************************** 2 | * 3 | * Copyright (c) 2015, ABB Schweiz AG 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with 7 | * or without modification, are permitted provided that 8 | * the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the 11 | * above copyright notice, this list of conditions 12 | * and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the 14 | * above copyright notice, this list of conditions 15 | * and the following disclaimer in the documentation 16 | * and/or other materials provided with the 17 | * distribution. 18 | * * Neither the name of ABB nor the names of its 19 | * contributors may be used to endorse or promote 20 | * products derived from this software without 21 | * specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 32 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *********************************************************************************************************************** 35 | */ 36 | 37 | #ifndef EGM_INTERPOLATOR_H 38 | #define EGM_INTERPOLATOR_H 39 | 40 | #include 41 | 42 | #include "abb_libegm_export.h" 43 | 44 | #include "egm_wrapper_trajectory.pb.h" // Generated by Google Protocol Buffer compiler protoc 45 | 46 | #include "egm_common.h" 47 | 48 | namespace abb 49 | { 50 | namespace egm 51 | { 52 | /** 53 | * \brief Class for managing interpolations. 54 | * 55 | * The used approaches, depending on the conditions, are: 56 | * - 5th (or lower) degree spline polynomials. 57 | * - Slerp interpolation. 58 | * - Ramping in or ramping down values. 59 | * 60 | * Warning: No kinematics are considered. I.e. joint limits can be exceeded (which can be a problem). 61 | */ 62 | class EGMInterpolator 63 | { 64 | public: 65 | /** 66 | * \brief Enum for the operations the interpolator can handle. 67 | */ 68 | enum Operation 69 | { 70 | Normal, ///< Normal operation, i.e. use spline and Slerp interpolation. 71 | RampDown, ///< Ramp down operation, i.e. use special spline and orientation interpolation. 72 | RampInPosition, ///< Ramp in position operation, i.e. used for static goals. 73 | RampInVelocity ///< Ramp in velocity operation, i.e. used for static goals. 74 | }; 75 | 76 | /** 77 | * \brief Struct for containing conditions for the interpolator. 78 | */ 79 | struct Conditions 80 | { 81 | /** 82 | * \brief Default constructor. 83 | */ 84 | Conditions() 85 | : 86 | duration(0.0), 87 | mode(EGMJoint), 88 | operation(Normal), 89 | ramp_down_factor(0.0), 90 | spline_method(TrajectoryConfiguration::Quintic) 91 | {} 92 | 93 | /** 94 | * \brief Duration [s] of the interpolation session. 95 | */ 96 | double duration; 97 | 98 | /** 99 | * \brief The active EGM mode. 100 | */ 101 | EGMModes mode; 102 | 103 | /** 104 | * \brief The requested interpolation operation. 105 | */ 106 | Operation operation; 107 | 108 | /** 109 | * \brief Value specifying the factor, of the current velocity, to use as end velocity for ramp down calculations. 110 | * 111 | * Note: The value should be between 0.0 and 1.0. 112 | */ 113 | double ramp_down_factor; 114 | 115 | /** 116 | * \brief The spline method to use for normal operation. 117 | */ 118 | TrajectoryConfiguration::SplineMethod spline_method; 119 | }; 120 | 121 | /** 122 | * \brief Update the interpolator for upcoming calculations. E.g. used after a new goal has been chosen. 123 | * 124 | * \param start containing the start point. 125 | * \param goal containing the goal point. 126 | * \param conditions for specifying conditions for the interpolator. 127 | */ 128 | void update(const wrapper::trajectory::PointGoal& start, 129 | const wrapper::trajectory::PointGoal& goal, 130 | const Conditions& conditions); 131 | 132 | /** 133 | * \brief Evaluate the interpolator at a specific time instance. 134 | * 135 | * \param p_output for storing the evaluated output. 136 | * \param sample_time specifying the used sample time [s]. 137 | * \param t for the time instance [s] that the interpolation should be calculated at. 138 | */ 139 | void evaluate(wrapper::trajectory::PointGoal* p_output, const double sample_time, double t); 140 | 141 | /** 142 | * \brief Retrive the valid duration [s] for the current interpolation session. 143 | * 144 | * \return double containing the duration. 145 | */ 146 | double getDuration() 147 | { 148 | return conditions_.duration; 149 | } 150 | 151 | private: 152 | /** 153 | * \brief Enum for specifying which Cartesian axis to consider in the spline polynomials. 154 | */ 155 | enum Axis 156 | { 157 | X, ///< Cartesian x axis. 158 | Y, ///< Cartesian y axis. 159 | Z ///< Cartesian z axis. 160 | }; 161 | 162 | /** 163 | * \brief Class for containing conditions for a spline polynomial. 164 | */ 165 | class SplineConditions 166 | { 167 | public: 168 | /** 169 | * \brief A constructor. 170 | * 171 | * \param conditions specifying the general conditions for the interpolator. 172 | */ 173 | SplineConditions(const Conditions conditions) 174 | : 175 | duration(conditions.duration), 176 | alfa(0.0), 177 | d_alfa(0.0), 178 | dd_alfa(0.0), 179 | beta(0.0), 180 | d_beta(0.0), 181 | dd_beta(0.0), 182 | spline_method(conditions.spline_method), 183 | do_ramp_down(conditions.operation == RampDown), 184 | ramp_down_factor(conditions.ramp_down_factor) 185 | {} 186 | 187 | /** 188 | * \brief Extract and set the boundary conditions for joint interpolation. 189 | * 190 | * \param index of the joint. 191 | * \param start containing the start values. 192 | * \param goal containing the goal values. 193 | */ 194 | void setConditions(int index, 195 | const wrapper::trajectory::JointGoal& start, 196 | const wrapper::trajectory::JointGoal& goal); 197 | 198 | /** 199 | * \brief Extract and set the boundary conditions for Cartesian interpolation. 200 | * 201 | * \param axis specifying the axis to consider. 202 | * \param start containing the start values. 203 | * \param goal containing the goal values. 204 | */ 205 | void setConditions(const Axis axis, 206 | const wrapper::trajectory::CartesianGoal& start, 207 | const wrapper::trajectory::CartesianGoal& goal); 208 | 209 | /** 210 | * \brief Duration of the interpolation. 211 | */ 212 | double duration; 213 | 214 | /** 215 | * \brief The start position. 216 | */ 217 | double alfa; 218 | 219 | /** 220 | * \brief The start velocity. 221 | */ 222 | double d_alfa; 223 | 224 | /** 225 | * \brief The start acceleration. 226 | */ 227 | double dd_alfa; 228 | 229 | /** 230 | * \param The goal position. 231 | */ 232 | double beta; 233 | 234 | /** 235 | * \param The goal velocity. 236 | */ 237 | double d_beta; 238 | 239 | /** 240 | * \param The goal acceleration. 241 | */ 242 | double dd_beta; 243 | 244 | /** 245 | * \brief Specifies which spline method to use. 246 | */ 247 | TrajectoryConfiguration::SplineMethod spline_method; 248 | 249 | /** 250 | * \brief Flag indicating if ramp down interpolation should be performed or not. 251 | */ 252 | bool do_ramp_down; 253 | 254 | /** 255 | * \brief Value specifying the factor, of the current velocity, to use as end velocity for ramp down calculations. 256 | * 257 | * Note: The value should be between 0.0 and 1.0. 258 | */ 259 | double ramp_down_factor; 260 | }; 261 | 262 | /** 263 | * \brief Class for a spline interpolation polynomial of degree 5 or lower. 264 | * 265 | * I.e. A + B*t + C*t^2 + D*t^3 + E*t^4 + F*t^5. 266 | */ 267 | class SplinePolynomial 268 | { 269 | public: 270 | /** 271 | * \brief Default constructor. 272 | */ 273 | SplinePolynomial() : a_(0.0), b_(0.0), c_(0.0), d_(0.0), e_(0.0), f_(0.0) {} 274 | 275 | /** 276 | * \brief Update the polynomial's coefficients. 277 | * 278 | * \param conditions containing the spline's conditions. 279 | */ 280 | void update(const SplineConditions& conditions); 281 | 282 | /** 283 | * \brief Evaluate the polynomial, for robot or external joints values. 284 | * 285 | * \param p_output for storing the evaluated values. 286 | * \param index of the joint. 287 | * \param t for the time instance [s] to evaluate at. 288 | */ 289 | void evaluate(wrapper::trajectory::JointGoal* p_output, const int index, const double t); 290 | 291 | /** 292 | * \brief Evaluate the polynomial, for Cartesian values. 293 | * 294 | * \param p_output for storing the evaluated values. 295 | * \param axis specifying the axis to consider. 296 | * \param t for the time instance [s] to evaluate at. 297 | */ 298 | void evaluate(wrapper::trajectory::CartesianGoal* p_output, const Axis axis, const double t); 299 | 300 | private: 301 | /** 302 | * \brief Calculate the position. 303 | * 304 | * \param t for the time instance [s] to calculate at. 305 | * 306 | * \return double containing the calculated position. 307 | */ 308 | double calculatePosition(const double t) 309 | { 310 | return a_ + b_*t + c_*std::pow(t, 2) + d_*std::pow(t, 3) + e_*std::pow(t, 4) + f_*std::pow(t, 5); 311 | } 312 | 313 | /** 314 | * \brief Calculate the velocity. 315 | * 316 | * \param t for the time instance [s] to calculate at. 317 | * 318 | * \return double containing the calculated velocity. 319 | */ 320 | double calculateVelocity(const double t) 321 | { 322 | return b_ + 2.0*c_*t + 3.0*d_*std::pow(t, 2) + 4.0*e_*std::pow(t, 3) + 5.0*f_*std::pow(t, 4); 323 | } 324 | 325 | /** 326 | * \brief Calculate the acceleration. 327 | * 328 | * \param t for the time instance [s] to calculate at. 329 | * 330 | * \return double containing the calculated acceleration. 331 | */ 332 | double calculateAcceleration(const double t) 333 | { 334 | return 2.0*c_ + 6.0*d_*t + 12.0*e_*std::pow(t, 2) + 20.0*f_*std::pow(t, 3); 335 | } 336 | 337 | /** 338 | * \brief Coefficient A. 339 | */ 340 | double a_; 341 | 342 | /** 343 | * \brief Coefficient B. 344 | */ 345 | double b_; 346 | 347 | /** 348 | * \brief Coefficient C. 349 | */ 350 | double c_; 351 | 352 | /** 353 | * \brief Coefficient D. 354 | */ 355 | double d_; 356 | 357 | /** 358 | * \brief Coefficient E. 359 | */ 360 | double e_; 361 | 362 | /** 363 | * \brief Coefficient F. 364 | */ 365 | double f_; 366 | }; 367 | 368 | /** 369 | * \brief Class for Slerp (Spherical linear interpolation) for quaternions. 370 | * Slerp with unit quaternions produce a rotation with uniform angular speed. 371 | * 372 | * I.e. Slerp(q0, q1; t) = [sin((1-t)*omega)/sin(omega)]*q0 + [sin(t*omega)/sin(omega)]*q1. 373 | * Where q0 and q1 are quaternions and cos(omega) = q0*q1 (dot product). 374 | * 375 | * Note: 0 <= t <= 1. 376 | * 377 | * See for example https://en.wikipedia.org/wiki/Slerp for the equations. 378 | */ 379 | class Slerp 380 | { 381 | public: 382 | /** 383 | * \brief Default constructor. 384 | */ 385 | Slerp() 386 | : 387 | DOT_PRODUCT_THRESHOLD(0.9995), 388 | duration_(0.0), 389 | omega_(0.0), 390 | use_linear_(false) 391 | { 392 | q0_.set_u0(1.0); 393 | q1_.set_u0(1.0); 394 | } 395 | 396 | /** 397 | * \brief Update the Slerp's coefficient. 398 | * 399 | * \param start containing the start quaternion. 400 | * \param goal containing the goal quaternion. 401 | * \param conditions containing the interpolator's conditions. 402 | */ 403 | void update(const wrapper::Quaternion& start, 404 | const wrapper::Quaternion& goal, 405 | const Conditions& conditions); 406 | 407 | /** 408 | * \brief Evaluate the Slerp. 409 | * 410 | * \param p_output for storing the evaluated values. 411 | * \param t for the time instance [s] that the interpolation should be calculated at. 412 | */ 413 | void evaluate(wrapper::trajectory::CartesianGoal* p_output, double t); 414 | 415 | private: 416 | /** 417 | * \brief Threshold for the dot product, used to decide if linear interpolation should be used. 418 | * 419 | * Note: That is if the quaternions are too close to each other. 420 | */ 421 | const double DOT_PRODUCT_THRESHOLD; 422 | 423 | /** 424 | * \brief Duration [s] of the interpolation session. 425 | */ 426 | double duration_; 427 | 428 | /** 429 | * \brief Coefficient omega. 430 | */ 431 | double omega_; 432 | 433 | /** 434 | * \brief Start quaternion. 435 | */ 436 | wrapper::Quaternion q0_; 437 | 438 | /** 439 | * \brief Goal quaternion. 440 | */ 441 | wrapper::Quaternion q1_; 442 | 443 | /** 444 | * \brief Flag indicating if linear interpolation should be used or not. 445 | */ 446 | bool use_linear_; 447 | }; 448 | 449 | /** 450 | * \brief Class for ramping in positions or velocities. As well as ramping down angular velocities. 451 | * 452 | * The ramp factor is according to: 453 | * - Ramping down angular velocity : 0.5*cos(pi*t) + 0.5 | I.e. from 1 -> 0. 454 | * - Ramping in position or velocity: 0.5*cos(pi*t + pi) + 0.5 | I.e. from 0 -> 1. 455 | * 456 | * Note: 0 <= t <= 1. 457 | */ 458 | class SoftRamp 459 | { 460 | public: 461 | /** 462 | * \brief Default constructor. 463 | */ 464 | SoftRamp() : duration_(0.0), operation_(RampDown) {} 465 | 466 | /** 467 | * \brief Update the ramp's internal data fields. 468 | * 469 | * \param start containing the start point. 470 | * \param goal containing the goal point. 471 | * \param conditions containing the interpolator's conditions. 472 | */ 473 | void update(const wrapper::trajectory::PointGoal& start, 474 | const wrapper::trajectory::PointGoal& goal, 475 | const Conditions& conditions); 476 | 477 | /** 478 | * \brief Evaluate the ramp, for robot or external joints values. 479 | * 480 | * \param p_output for storing the evaluated values. 481 | * \param robot indicating if it is robot joints (otherwise external joints). 482 | * \param sample_time for the used sample time [s]. 483 | * \param t for the time instance [s] to evaluate at. 484 | */ 485 | void evaluate(wrapper::trajectory::JointGoal* p_output, 486 | const bool robot, 487 | const double sample_time, 488 | double t); 489 | 490 | /** 491 | * \brief Evaluate the ramp, for Cartesian values. 492 | * 493 | * \param p_output for storing the evaluated values. 494 | * \param sample_time for the used sample time [s]. 495 | * \param t for the time instance [s] to evaluate at. 496 | */ 497 | void evaluate(wrapper::trajectory::CartesianGoal* p_output, const double sample_time, double t); 498 | 499 | private: 500 | /** 501 | * \brief Duration [s] of the interpolation session. 502 | */ 503 | double duration_; 504 | 505 | /** 506 | * \brief The requested interpolation operation. 507 | */ 508 | Operation operation_; 509 | 510 | /** 511 | * \brief A container for the start point. 512 | */ 513 | wrapper::trajectory::PointGoal start_; 514 | 515 | /** 516 | * \brief A container for the starting angular velocity values. 517 | */ 518 | wrapper::Euler start_angular_velocity_; 519 | 520 | /** 521 | * \brief A container for the goal point. 522 | */ 523 | wrapper::trajectory::PointGoal goal_; 524 | }; 525 | 526 | /** 527 | * \brief Static constant for the max number of spline polynomials. 528 | */ 529 | static const size_t MAX_NUMBER_OF_SPLINES_ = 12; 530 | 531 | /** 532 | * \brief Offset in the spline polynomial array, to the external joint elements. 533 | */ 534 | int offset_; 535 | 536 | /** 537 | * \brief Container for the spline interpolation polynomials. 538 | */ 539 | boost::array spline_polynomials_; 540 | 541 | /** 542 | * \brief Container for the Slerp (for interpolating quaterions). 543 | */ 544 | Slerp slerp_; 545 | 546 | /** 547 | * \brief Container for ramping in positions or velocities. As well as ramping down angular velocities. 548 | */ 549 | SoftRamp soft_ramp_; 550 | 551 | /** 552 | * \brief The interpolator's conditions. 553 | */ 554 | Conditions conditions_; 555 | }; 556 | 557 | } // end namespace egm 558 | } // end namespace abb 559 | 560 | #endif // EGM_INTERPOLATOR_H 561 | -------------------------------------------------------------------------------- /include/abb_libegm/egm_logger.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************************************** 2 | * 3 | * Copyright (c) 2015, ABB Schweiz AG 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with 7 | * or without modification, are permitted provided that 8 | * the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the 11 | * above copyright notice, this list of conditions 12 | * and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the 14 | * above copyright notice, this list of conditions 15 | * and the following disclaimer in the documentation 16 | * and/or other materials provided with the 17 | * distribution. 18 | * * Neither the name of ABB nor the names of its 19 | * contributors may be used to endorse or promote 20 | * products derived from this software without 21 | * specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 32 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *********************************************************************************************************************** 35 | */ 36 | 37 | #ifndef EGM_LOGGER_H 38 | #define EGM_LOGGER_H 39 | 40 | #include 41 | 42 | #include "abb_libegm_export.h" 43 | 44 | #include "egm_wrapper.pb.h" // Generated by Google Protocol Buffer compiler protoc 45 | 46 | namespace abb 47 | { 48 | namespace egm 49 | { 50 | /** 51 | * \brief Class for logging EGM messages into CSV formatted file. 52 | */ 53 | class EGMLogger 54 | { 55 | public: 56 | /** 57 | * \brief A constructor. 58 | * 59 | * \param filename specifying the log's filename. 60 | * \param use_default_header specifying if the default headers should be used in the log. 61 | */ 62 | EGMLogger(const std::string& filename, const bool use_default_headers = true); 63 | 64 | /** 65 | * \brief A destructor. 66 | */ 67 | ~EGMLogger(); 68 | 69 | /** 70 | * \brief Flush the stream object. 71 | */ 72 | void flush(); 73 | 74 | /** 75 | * \brief Add header data to the log stream. 76 | * 77 | * \param header containing the header data to add. 78 | */ 79 | void add(const wrapper::Header& header); 80 | 81 | /** 82 | * \brief Add joint data (i.e. robot and external joints) to the log stream. 83 | * 84 | * \param robot containing the robot joint data to add. 85 | * \param external containing the external joint data to add. 86 | */ 87 | void add(const wrapper::Joints& robot, const wrapper::Joints& external); 88 | 89 | /** 90 | * \brief Add Cartesian pose data to the log stream. 91 | * 92 | * \param pose containing the pose data to add. 93 | */ 94 | void add(const wrapper::CartesianPose& pose); 95 | 96 | /** 97 | * \brief Add Cartesian velocity data to the log stream. 98 | * 99 | * \param velocity containing the velocity data to add. 100 | * \param last indicating if it is the last addition to current the log event. 101 | */ 102 | void add(const wrapper::CartesianVelocity& velocity, const bool last = false); 103 | 104 | /** 105 | * \brief Calculate the amount of time logged. 106 | * 107 | * \param sample_time specifying the sample time. 108 | * 109 | * \return double for the time logged. 110 | */ 111 | double calculateTimeLogged(const double sample_time); 112 | 113 | private: 114 | /** 115 | * \brief Add mock values for missing joint data to the log stream. 116 | * 117 | * \param robot indicating if it is robot joints to add or not (otherwise external joints). 118 | * \param robot_size of the robot joint container. 119 | * \param external_size of the external joint container. 120 | */ 121 | void addMockJoints(const bool robot, const size_t robot_size, const size_t external_size); 122 | 123 | /** 124 | * \brief The number of logged messages. 125 | */ 126 | unsigned int number_of_logged_messages_; 127 | 128 | /** 129 | * \brief Stream object for logging data. 130 | */ 131 | std::ofstream log_stream_; 132 | }; 133 | 134 | } // end namespace egm 135 | } // end namespace abb 136 | 137 | #endif // EGM_LOGGER_H 138 | -------------------------------------------------------------------------------- /include/abb_libegm/egm_udp_server.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************************************** 2 | * 3 | * Copyright (c) 2015, ABB Schweiz AG 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with 7 | * or without modification, are permitted provided that 8 | * the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the 11 | * above copyright notice, this list of conditions 12 | * and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the 14 | * above copyright notice, this list of conditions 15 | * and the following disclaimer in the documentation 16 | * and/or other materials provided with the 17 | * distribution. 18 | * * Neither the name of ABB nor the names of its 19 | * contributors may be used to endorse or promote 20 | * products derived from this software without 21 | * specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 32 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *********************************************************************************************************************** 35 | */ 36 | 37 | #ifndef EGM_UDP_SERVER_H 38 | #define EGM_UDP_SERVER_H 39 | 40 | #include 41 | 42 | namespace abb 43 | { 44 | namespace egm 45 | { 46 | /** 47 | * \brief Struct for containing data from the UDPServer class. 48 | */ 49 | struct UDPServerData 50 | { 51 | /** 52 | * \brief Default constructor. 53 | */ 54 | UDPServerData() 55 | : 56 | port_number(0), 57 | bytes_transferred(0) 58 | {} 59 | 60 | /** 61 | * \brief Port number for the server's UDP socket. 62 | */ 63 | size_t port_number; 64 | 65 | /** 66 | * \brief The received data. 67 | */ 68 | char* p_data; 69 | 70 | /** 71 | * \brief Bytes transferred to the server. 72 | */ 73 | int bytes_transferred; 74 | }; 75 | 76 | /** 77 | * \brief Abstract class for a user interface to the UDPServer class. 78 | */ 79 | class AbstractUDPServerInterface 80 | { 81 | /** 82 | * \brief A friend to the interface. 83 | */ 84 | friend class UDPServer; 85 | 86 | private: 87 | /** 88 | * \brief Pure virtual method for handling callback requests from a UDPServer instance. 89 | * 90 | * \Param server_data containing the UDP server's callback data. 91 | * 92 | * \return string& containing the reply. 93 | */ 94 | virtual const std::string& callback(const UDPServerData& data) = 0; 95 | }; 96 | 97 | /** 98 | * \brief Class for an asynchronous UDP server. 99 | * 100 | * The server receives UDP messages from a client, passes the messages to a callback and returns a reply to the client. 101 | */ 102 | class UDPServer 103 | { 104 | public: 105 | /** 106 | * \brief A constructor. 107 | * 108 | * \param io_service for operating boost asio's asynchronous functions. 109 | * \param port_number for the server's UDP socket. 110 | * \param p_interface that processes the received messages. 111 | */ 112 | UDPServer(boost::asio::io_service& io_service, 113 | unsigned short port_number, 114 | AbstractUDPServerInterface* p_interface); 115 | 116 | /** 117 | * \brief A destructor. 118 | */ 119 | ~UDPServer(); 120 | 121 | /** 122 | * \brief Checks if the server was successfully initialized or not. 123 | * 124 | * \return bool true if and only if the server was initialized correctly. 125 | */ 126 | bool isInitialized() const; 127 | 128 | private: 129 | /** 130 | * \brief Start an asynchronous receive. 131 | */ 132 | void startAsynchronousReceive(); 133 | 134 | /** 135 | * \brief Callback for handling an asynchronous receive. 136 | * 137 | * \param error for containing an error code. 138 | * \param bytes_transferred is the number of bytes received. 139 | */ 140 | void receiveCallback(const boost::system::error_code& error, const std::size_t bytes_transferred); 141 | 142 | /** 143 | * \brief Callback for handling an asynchronous send. 144 | * 145 | * \param error for containing an error code. 146 | * \param bytes_transferred is the number of bytes transmitted. 147 | */ 148 | void sendCallback(const boost::system::error_code& error, const std::size_t bytes_transferred); 149 | 150 | /** 151 | * \brief Static constant for the socket's buffer size. 152 | */ 153 | static const size_t BUFFER_SIZE = 1024; 154 | 155 | /** 156 | * \brief The server's UDP socket. 157 | */ 158 | boost::shared_ptr p_socket_; 159 | 160 | /** 161 | * \brief The address of the calling computer (e.g. an ABB robot controller or a virtual controller in RobotStudio). 162 | */ 163 | boost::asio::ip::udp::endpoint remote_endpoint_; 164 | 165 | /** 166 | * \brief A buffer for storing the server's serialized inbound messages (i.e. the robot's outbound messages). 167 | */ 168 | char receive_buffer_[BUFFER_SIZE]; 169 | 170 | /** 171 | * \brief Pointer to an object that is derived from AbstractUDPSeverInterface, which processes the received messages. 172 | */ 173 | AbstractUDPServerInterface* p_interface_; 174 | 175 | /** 176 | * \brief Container for server data. 177 | */ 178 | UDPServerData server_data_; 179 | 180 | /** 181 | * \brief Flag indicating if the server was initialized successfully or not. 182 | */ 183 | bool initialized_; 184 | }; 185 | 186 | } // end namespace egm 187 | } // end namespace abb 188 | 189 | #endif // EGM_UDP_SERVER_H 190 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | abb_libegm 5 | 1.2.0 6 | A C++ library for interfacing with ABB robot controllers supporting Externally Guided Motion (689-1) 7 | G.A. vd. Hoorn (TU Delft Robotics Institute) 8 | BSD 3-Clause 9 | https://github.com/ros-industrial/abb_libegm 10 | Jon Tjerngren 11 | 12 | cmake 13 | 14 | boost 15 | protobuf-dev 16 | protobuf-dev 17 | protobuf 18 | 19 | 20 | cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /proto/README.md: -------------------------------------------------------------------------------- 1 | # Information 2 | 3 | * [egm.proto](egm.proto): Created by [ABB Robotics](https://new.abb.com/products/robotics) for specifying the EGM communication protocol. 4 | * The file can be found in the installation folder of RobotWare. For example on Windows with RobotWare `6.08.00.01`: `%localappdata%\ABB Industrial IT\Robotics IT\RobotWare\RobotWare_6.08.0135\utility\Template\EGM` 5 | * [egm_wrapper.proto](egm_wrapper.proto): Custom made for `abb_libegm` for wrapping the supported EGM features. 6 | * [egm_wrapper_trajectory.proto](egm_wrapper_trajectory.proto): Custom made for `abb_libegm` for extending `egm_wrapper.proto` with trajectory messages. 7 | -------------------------------------------------------------------------------- /proto/egm.proto: -------------------------------------------------------------------------------- 1 | // Definition of ABB sensor interface V1.1 2 | // 3 | // messages of type EgmRobot are sent out from the robot controller 4 | // messages of type EgmSensor are sent to the robot controller 5 | // 6 | syntax = "proto2"; 7 | package abb.egm; 8 | 9 | message EgmHeader 10 | { 11 | optional uint32 seqno = 1; // sequence number (to be able to find lost messages) 12 | optional uint32 tm = 2; // controller send time stamp in ms 13 | 14 | enum MessageType { 15 | MSGTYPE_UNDEFINED = 0; 16 | MSGTYPE_COMMAND = 1; // for future use 17 | MSGTYPE_DATA = 2; // sent by robot controller 18 | MSGTYPE_CORRECTION = 3; // sent by sensor for position guidance 19 | MSGTYPE_PATH_CORRECTION = 4; // sent by sensor for path correction 20 | } 21 | 22 | optional MessageType mtype = 3 [default = MSGTYPE_UNDEFINED]; 23 | } 24 | 25 | message EgmCartesian // Cartesian position in mm 26 | { 27 | required double x = 1; 28 | required double y = 2; 29 | required double z = 3; 30 | } 31 | 32 | // If you have pose input, i.e. not joint input, you can choose to send orientation data as quaternion or as Euler angles. 33 | // If both are sent, Euler angles have higher priority. 34 | 35 | message EgmQuaternion // Quaternion orientation 36 | { 37 | required double u0 = 1; 38 | required double u1 = 2; 39 | required double u2 = 3; 40 | required double u3 = 4; 41 | } 42 | 43 | message EgmEuler // Euler angle orientation in degrees 44 | { 45 | required double x = 1; 46 | required double y = 2; 47 | required double z = 3; 48 | } 49 | 50 | message EgmClock // Time in seconds and microseconds since 1 Jan 1970 51 | { 52 | required uint64 sec = 1; 53 | required uint64 usec = 2; 54 | } 55 | 56 | message EgmPose // Pose (i.e. cartesian position and Quaternion orientation) relative to the correction frame defined by EGMActPose 57 | { 58 | optional EgmCartesian pos = 1; 59 | optional EgmQuaternion orient = 2; 60 | optional EgmEuler euler = 3; 61 | } 62 | 63 | message EgmCartesianSpeed // Array of 6 speed reference values in mm/s or degrees/s 64 | { 65 | repeated double value = 1; 66 | } 67 | 68 | message EgmJoints // Array of 6 joint values for TCP robot in degrees 69 | { 70 | repeated double joints = 1; 71 | } 72 | 73 | message EgmExternalJoints // Array of 6 joint values for additional axis, in degrees for rotating axis, in mm for linear axis 74 | { 75 | repeated double joints = 1; 76 | } 77 | 78 | message EgmPlanned // Planned position for robot (joints or cartesian) and additional axis (array of 6 values) 79 | { // Is used for position streaming (source: controller) and position guidance (source: sensor) 80 | optional EgmJoints joints = 1; 81 | optional EgmPose cartesian = 2; 82 | optional EgmJoints externalJoints = 3; 83 | optional EgmClock time = 4; 84 | } 85 | 86 | message EgmSpeedRef // Speed reference values for robot (joint or cartesian) and additional axis (array of 6 values) 87 | { 88 | optional EgmJoints joints = 1; 89 | optional EgmCartesianSpeed cartesians = 2; 90 | optional EgmJoints externalJoints = 3; 91 | } 92 | 93 | 94 | message EgmPathCorr // Cartesian path correction and measurement age 95 | { 96 | required EgmCartesian pos = 1; // Sensor measurement (x, y, z) relative the sensor tool coordinate system 97 | required uint32 age = 2; // Sensor measurement age in ms 98 | } 99 | 100 | 101 | message EgmFeedBack // Feed back position, i.e. actual measured position for robot (joints or cartesian) and additional axis (array of 6 values) 102 | { 103 | optional EgmJoints joints = 1; 104 | optional EgmPose cartesian = 2; 105 | optional EgmJoints externalJoints = 3; 106 | optional EgmClock time = 4; 107 | } 108 | 109 | message EgmMotorState // Motor state 110 | { 111 | enum MotorStateType { 112 | MOTORS_UNDEFINED = 0; 113 | MOTORS_ON = 1; 114 | MOTORS_OFF = 2; 115 | } 116 | 117 | required MotorStateType state = 1; 118 | } 119 | 120 | message EgmMCIState // EGM state 121 | { 122 | enum MCIStateType { 123 | MCI_UNDEFINED = 0; 124 | MCI_ERROR = 1; 125 | MCI_STOPPED = 2; 126 | MCI_RUNNING = 3; 127 | } 128 | 129 | required MCIStateType state = 1 [default = MCI_UNDEFINED]; 130 | } 131 | 132 | message EgmRapidCtrlExecState // RAPID execution state 133 | { 134 | enum RapidCtrlExecStateType { 135 | RAPID_UNDEFINED = 0; 136 | RAPID_STOPPED = 1; 137 | RAPID_RUNNING = 2; 138 | }; 139 | 140 | required RapidCtrlExecStateType state = 1 [default = RAPID_UNDEFINED]; 141 | } 142 | 143 | message EgmTestSignals // Test signals 144 | { 145 | repeated double signals = 1; 146 | } 147 | 148 | message EgmMeasuredForce // Array of 6 force values for a robot 149 | { 150 | repeated double force = 1; 151 | } 152 | 153 | // Robot controller outbound message, sent from the controller to the sensor during position guidance and position streaming 154 | message EgmRobot 155 | { 156 | optional EgmHeader header = 1; 157 | optional EgmFeedBack feedBack = 2; 158 | optional EgmPlanned planned = 3; 159 | 160 | optional EgmMotorState motorState = 4; 161 | optional EgmMCIState mciState = 5; 162 | optional bool mciConvergenceMet = 6; 163 | optional EgmTestSignals testSignals = 7; 164 | optional EgmRapidCtrlExecState rapidExecState = 8; 165 | optional EgmMeasuredForce measuredForce = 9; 166 | optional double utilizationRate=10; 167 | } 168 | 169 | 170 | // Robot controller inbound message, sent from sensor to the controller during position guidance 171 | message EgmSensor 172 | { 173 | optional EgmHeader header = 1; 174 | optional EgmPlanned planned = 2; 175 | optional EgmSpeedRef speedRef = 3; 176 | } 177 | 178 | // Robot controller inbound message, sent from sensor during path correction 179 | message EgmSensorPathCorr 180 | { 181 | optional EgmHeader header = 1; 182 | optional EgmPathCorr pathCorr = 2; 183 | } 184 | -------------------------------------------------------------------------------- /proto/egm_wrapper.proto: -------------------------------------------------------------------------------- 1 | //====================================================================================================================== 2 | // 3 | // Wrapper messages. 4 | // 5 | // Note: Used to wrap the actual EGM messages, which are defined in the egm.proto file from ABB Robotics. 6 | // This can be used in intermediate components that are utilizing EGM communication for motion control. 7 | // 8 | //====================================================================================================================== 9 | 10 | syntax = "proto2"; 11 | 12 | package abb.egm.wrapper; 13 | 14 | //====================================================================================================================== 15 | // 16 | // Auxiliary messages. 17 | // 18 | //====================================================================================================================== 19 | 20 | message Header 21 | { 22 | enum MessageType 23 | { 24 | UNDEFINED = 0; 25 | DATA = 1; // Message contains data sent by the robot controller. 26 | } 27 | 28 | // Note: EGM messages received during runtime are inspected to detect the RobotWare version. 29 | enum RWVersion 30 | { 31 | RW_UNKNOWN = 0; 32 | RW_6_10_AND_NEWER = 1; // Selected if all the fields mentioned below exist. 33 | RW_BETWEEN_6_AND_6_06_03 = 2; // Selected if no time field exist (was added in RW6.07). 34 | RW_BETWEEN_6_07_AND_6_09_02 = 3; // Selected if no utilization rate field exist (was added in RW6.10). 35 | } 36 | 37 | // Note: EGM messages received during runtime are inspected to detect the EGM version. 38 | enum EGMVersion 39 | { 40 | EGM_UNKNOWN = 0; 41 | EGM_1_0 = 1; // Selected if no time field exist (was added in 6.07). 42 | EGM_1_1 = 2; // Selected if the field mentioned below exist. 43 | } 44 | 45 | optional uint32 sequence_number = 1; // Sequence number (to be able to detect lost messages). 46 | optional uint32 time_stamp = 2; // Time stamp in milliseconds. 47 | optional MessageType message_type = 3 [default = UNDEFINED]; 48 | optional RWVersion rw_version = 4 [default = RW_UNKNOWN]; 49 | optional EGMVersion egm_version = 5 [default = EGM_UNKNOWN]; 50 | } 51 | 52 | // Note: Status of the robot controller, and the active EGM motion. 53 | message Status 54 | { 55 | enum EGMState 56 | { 57 | EGM_UNDEFINED = 0; 58 | EGM_ERROR = 1; 59 | EGM_STOPPED = 2; 60 | EGM_RUNNING = 3; 61 | } 62 | 63 | enum MotorState 64 | { 65 | MOTORS_UNDEFINED = 0; 66 | MOTORS_ON = 1; 67 | MOTORS_OFF = 2; 68 | } 69 | 70 | enum RAPIDExecutionState 71 | { 72 | RAPID_UNDEFINED = 0; 73 | RAPID_STOPPED = 1; 74 | RAPID_RUNNING = 2; 75 | } 76 | 77 | optional bool egm_convergence_met = 1; 78 | optional EGMState egm_state = 2 [default = EGM_UNDEFINED]; 79 | optional MotorState motor_state = 3 [default = MOTORS_UNDEFINED]; 80 | optional RAPIDExecutionState rapid_execution_state = 4 [default = RAPID_UNDEFINED]; 81 | optional double utilization_rate = 5; 82 | } 83 | 84 | // Note: Time in seconds and microseconds since the epoch (1 Jan 1970) 85 | message Clock 86 | { 87 | optional uint64 sec = 1; 88 | optional uint64 usec = 2; 89 | } 90 | 91 | //=========================================================== 92 | // 93 | // Joint space related messages. 94 | // 95 | //=========================================================== 96 | 97 | // Note: This message is used for joints values, and only up to seven values will be considered. 98 | // 99 | // For six axes robots: 100 | // - If robot joints : 1-6 values. 101 | // - If external joints: 1-6 values. 102 | // For seven axes robots: 103 | // - If robot joints : 1-7 values. 104 | // - If external joints: 1-5 values. 105 | message Joints 106 | { 107 | repeated double values = 1; 108 | } 109 | 110 | message JointSpace 111 | { 112 | optional Joints position = 1; // Units [degrees] 113 | optional Joints velocity = 2; // Units [degrees/s] 114 | } 115 | 116 | //=========================================================== 117 | // 118 | // Cartesian space related messages. 119 | // 120 | // Note 1: Cartesian messages from the robot controller are 121 | // relative to the work object specified when setting 122 | // up EGM. E.g. with EGMActPose RAPID instructions. 123 | // 124 | // Note 2: Cartesian messages to the robot controller are 125 | // interpreted relative to the sensor frame defined 126 | // when setting up EGM. E.g. with EGMActPose RAPID 127 | // instructions. 128 | // 129 | //=========================================================== 130 | 131 | message Cartesian 132 | { 133 | optional double x = 1; 134 | optional double y = 2; 135 | optional double z = 3; 136 | } 137 | 138 | // Note: Higher priority than quaternions. 139 | message Euler 140 | { 141 | optional double x = 1; 142 | optional double y = 2; 143 | optional double z = 3; 144 | } 145 | 146 | // Note: Lower priority than Euler angles. The quaternion is defined as: u0 + u1*i + u2*j + u3*k. 147 | message Quaternion 148 | { 149 | optional double u0 = 1; 150 | optional double u1 = 2; 151 | optional double u2 = 3; 152 | optional double u3 = 4; 153 | } 154 | 155 | message CartesianPose 156 | { 157 | optional Cartesian position = 1; // Units [mm]. 158 | optional Euler euler = 2; // Units [degrees]. 159 | optional Quaternion quaternion = 3; // Units [-]. 160 | } 161 | 162 | message CartesianVelocity 163 | { 164 | optional Cartesian linear = 1; // Units [mm/s]. 165 | optional Euler angular = 2; // Units [degrees/s]. 166 | } 167 | 168 | message CartesianSpace 169 | { 170 | optional CartesianPose pose = 1; 171 | optional CartesianVelocity velocity = 2; 172 | } 173 | 174 | //=========================================================== 175 | // 176 | // Combined joint and Cartesian space auxiliary messages. 177 | // 178 | //=========================================================== 179 | 180 | // Note: Joint or Cartesian motion depends on the used EGM RAPID instructions. 181 | message Robot 182 | { 183 | optional JointSpace joints = 1; 184 | optional CartesianSpace cartesian = 2; 185 | } 186 | 187 | message External 188 | { 189 | optional JointSpace joints = 1; 190 | } 191 | 192 | // Note: The velocity sub fields are estimated from the actual EGM messages. 193 | message Feedback 194 | { 195 | optional Robot robot = 1; 196 | optional External external = 2; 197 | optional Clock time = 3; 198 | } 199 | 200 | // Note: The velocity sub fields are estimated from the actual EGM messages. 201 | message Planned 202 | { 203 | optional Robot robot = 1; 204 | optional External external = 2; 205 | optional Clock time = 3; 206 | } 207 | 208 | //====================================================================================================================== 209 | // 210 | // Primary messages. 211 | // 212 | //====================================================================================================================== 213 | 214 | // Inputs extracted from the robot controller outbound messages (i.e. EgmRobot). 215 | message Input 216 | { 217 | optional Header header = 1; 218 | optional Feedback feedback = 2; 219 | optional Planned planned = 3; 220 | optional Status status = 4; 221 | } 222 | 223 | // Outputs to send to the robot controller. 224 | message Output 225 | { 226 | optional Robot robot = 1; // Outputs to the robot. 227 | optional External external = 2; // Outputs to external axes. 228 | } 229 | -------------------------------------------------------------------------------- /proto/egm_wrapper_trajectory.proto: -------------------------------------------------------------------------------- 1 | //====================================================================================================================== 2 | // 3 | // Wrapper messages for managing trajectories. 4 | // 5 | // These messages are intended to be used by an external EGM trajectory interface. 6 | // 7 | // Note: Used to wrap the actual EGM messages, which are defined in the egm.proto file from ABB Robotics. 8 | // This can be used in intermediate components that are utilizing EGM communication for motion control. 9 | // 10 | //====================================================================================================================== 11 | 12 | syntax = "proto2"; 13 | 14 | import "egm_wrapper.proto"; 15 | 16 | package abb.egm.wrapper.trajectory; 17 | 18 | //====================================================================================================================== 19 | // 20 | // Auxiliary messages. 21 | // 22 | //====================================================================================================================== 23 | 24 | //=========================================================== 25 | // 26 | // Trajectory related messages. 27 | // 28 | //=========================================================== 29 | 30 | // Note: The acceleration field is only used as potential input for interpolation. 31 | message JointGoal 32 | { 33 | optional Joints position = 1; // Units [degrees]. 34 | optional Joints velocity = 2; // Units [degrees/s]. 35 | optional Joints acceleration = 3; // Units [degrees/s^2]. 36 | } 37 | 38 | // Notes: 39 | // - The Euler angles have higher priority than the quaternions (parts of the pose component). 40 | // - Only linear velocity, because the orientation interpolation method (i.e. Slerp) result in uniform angular speed. 41 | // - Only linear acceleration, and it is only used as potential input for interpolation. 42 | message CartesianGoal 43 | { 44 | optional CartesianPose pose = 1; // Units [mm] and [degrees] or [-] 45 | optional Cartesian velocity = 2; // Units [mm/s]. 46 | optional Cartesian acceleration = 3; // Units [mm/s^2]. 47 | } 48 | 49 | // Note: Joint or Cartesian motion depends on the used EGM RAPID instructions. 50 | message RobotGoal 51 | { 52 | optional JointGoal joints = 1; 53 | optional CartesianGoal cartesian = 2; 54 | } 55 | 56 | message ExternalGoal 57 | { 58 | optional JointGoal joints = 1; 59 | } 60 | 61 | // Point goal in a trajectory. 62 | message PointGoal 63 | { 64 | optional double duration = 1; // Units [s]. I.e. the time it should take to reach the point. 65 | optional RobotGoal robot = 2; // Goal for the robot. 66 | optional ExternalGoal external = 3; // Goal for external axes. 67 | optional bool reach = 4; // Flag indicating that the point is important to reach or not. 68 | } 69 | 70 | //=========================================================== 71 | // 72 | // Static goal related messages. 73 | // 74 | //=========================================================== 75 | 76 | // Note: Joint or Cartesian motion depends on the used EGM RAPID instructions. 77 | message RobotPositionGoal 78 | { 79 | optional Joints joints = 1; // Units [degrees]. 80 | optional CartesianPose cartesian = 2; // Units [mm] and [degrees]. 81 | } 82 | 83 | // Note: Joint or Cartesian motion depends on the used EGM RAPID instructions. 84 | message RobotVelocityGoal 85 | { 86 | optional Joints joints = 1; // Units [degrees/s]. 87 | optional CartesianVelocity cartesian = 2; // Units [mm/s] and [degrees/s]. 88 | } 89 | 90 | //====================================================================================================================== 91 | // 92 | // Primary messages. 93 | // 94 | //====================================================================================================================== 95 | 96 | // A trajectory goal that an EGM trajectory interface should follow. 97 | message TrajectoryGoal 98 | { 99 | repeated PointGoal points = 1; 100 | } 101 | 102 | // A static position goal that an EGM trajectory interface should execute. 103 | message StaticPositionGoal 104 | { 105 | optional RobotPositionGoal robot = 1; // Units [degrees] and [mm]. 106 | optional Joints external = 2; // Units [degrees]. 107 | } 108 | 109 | // A static velocity goal that an EGM trajectory interface should execute. 110 | message StaticVelocityGoal 111 | { 112 | optional RobotVelocityGoal robot = 1; // Units [degrees/s] and [mm/s]. 113 | optional Joints external = 2; // Units [degrees/s]. 114 | } 115 | 116 | // An execution progress for an EGM trajectory interface. 117 | message ExecutionProgress 118 | { 119 | // Possible states of the EGM trajectory interface. 120 | enum State 121 | { 122 | UNDEFINED = 0; // The state is undefined. 123 | NORMAL = 1; // Motion references are generated from trajectories added by a user. 124 | RAMP_DOWN = 2; // Motion references are being ramped down. 125 | STATIC_GOAL = 3; // Motion references are generated by tracking a goal point specified by a user. 126 | } 127 | 128 | // Possible sub states of the EGM trajectory interface's state. 129 | enum SubState 130 | { 131 | NONE = 0; // The current state has not been activated yet. 132 | RUNNING = 1; // The current state is being executed. 133 | FINISHED = 2; // The current state has finished. 134 | } 135 | 136 | optional State state = 1 [default = UNDEFINED]; 137 | optional SubState sub_state = 2 [default = NONE]; 138 | optional Input inputs = 3; // The latest inputs received from the robot controller. 139 | optional Output outputs = 4; // The latest outputs sent to the robot controller. 140 | optional double time_passed = 5; // The time [s] passed since the current goal became active. 141 | optional bool goal_active = 6; // Indicates if a goal is currently active or not. 142 | optional PointGoal goal = 7; // The current goal. 143 | optional TrajectoryGoal active_trajectory = 8; // The currently active trajectory (if any has been activated). 144 | optional uint32 pending_trajectories = 9; // The number of pending trajectories in the queue. 145 | } 146 | -------------------------------------------------------------------------------- /src/egm_common.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************************************** 2 | * 3 | * Copyright (c) 2015, ABB Schweiz AG 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with 7 | * or without modification, are permitted provided that 8 | * the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the 11 | * above copyright notice, this list of conditions 12 | * and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the 14 | * above copyright notice, this list of conditions 15 | * and the following disclaimer in the documentation 16 | * and/or other materials provided with the 17 | * distribution. 18 | * * Neither the name of ABB nor the names of its 19 | * contributors may be used to endorse or promote 20 | * products derived from this software without 21 | * specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 32 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *********************************************************************************************************************** 35 | */ 36 | 37 | #define _USE_MATH_DEFINES 38 | 39 | #include 40 | 41 | #include "abb_libegm/egm_common.h" 42 | 43 | namespace abb 44 | { 45 | namespace egm 46 | { 47 | /*********************************************************************************************************************** 48 | * Struct definitions: Constants 49 | */ 50 | 51 | typedef Constants::RobotController RobotController; 52 | 53 | const double RobotController::LOWEST_SAMPLE_TIME = 0.004; 54 | const unsigned short RobotController::DEFAULT_PORT_NUMBER = 6511; 55 | const int RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS = 6; 56 | const int RobotController::DEFAULT_NUMBER_OF_EXTERNAL_JOINTS = 6; 57 | const int RobotController::MAX_NUMBER_OF_JOINTS = RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS + 58 | RobotController::DEFAULT_NUMBER_OF_EXTERNAL_JOINTS; 59 | 60 | const double Constants::Conversion::RAD_TO_DEG = 180.0 / M_PI; 61 | const double Constants::Conversion::DEG_TO_RAD = M_PI / 180.0; 62 | const double Constants::Conversion::MM_TO_M = 0.001; 63 | const double Constants::Conversion::MS_TO_S = 0.001; 64 | const double Constants::Conversion::S_TO_US = 1e6; 65 | 66 | } // end namespace egm 67 | } // end namespace abb 68 | -------------------------------------------------------------------------------- /src/egm_common_auxiliary.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************************************** 2 | * 3 | * Copyright (c) 2015, ABB Schweiz AG 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with 7 | * or without modification, are permitted provided that 8 | * the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the 11 | * above copyright notice, this list of conditions 12 | * and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the 14 | * above copyright notice, this list of conditions 15 | * and the following disclaimer in the documentation 16 | * and/or other materials provided with the 17 | * distribution. 18 | * * Neither the name of ABB nor the names of its 19 | * contributors may be used to endorse or promote 20 | * products derived from this software without 21 | * specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 32 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *********************************************************************************************************************** 35 | */ 36 | 37 | #define _USE_MATH_DEFINES 38 | 39 | #include 40 | 41 | #include 42 | 43 | #include "abb_libegm/egm_common_auxiliary.h" 44 | 45 | namespace abb 46 | { 47 | namespace egm 48 | { 49 | /*********************************************************************************************************************** 50 | * Math functions 51 | */ 52 | 53 | double saturate(const double value, const double lower, const double upper) 54 | { 55 | return (value < lower ? lower : (value > upper ? upper : value)); 56 | } 57 | 58 | void multiply(wrapper::Joints* p_j, const double factor) 59 | { 60 | if (p_j) 61 | { 62 | for (int i = 0; i < p_j->values_size(); ++i) 63 | { 64 | p_j->set_values(i, p_j->values(i)*factor); 65 | } 66 | } 67 | } 68 | 69 | void multiply(wrapper::Cartesian* p_c, const double factor) 70 | { 71 | if (p_c) 72 | { 73 | p_c->set_x(p_c->x()*factor); 74 | p_c->set_y(p_c->y()*factor); 75 | p_c->set_z(p_c->z()*factor); 76 | } 77 | } 78 | 79 | void multiply(wrapper::Euler* p_e, const double factor) 80 | { 81 | if (p_e) 82 | { 83 | p_e->set_x(p_e->x()*factor); 84 | p_e->set_y(p_e->y()*factor); 85 | p_e->set_z(p_e->z()*factor); 86 | } 87 | } 88 | 89 | void multiply(wrapper::Quaternion* p_q, const double factor) 90 | { 91 | if (p_q) 92 | { 93 | p_q->set_u0(p_q->u0()*factor); 94 | p_q->set_u1(p_q->u1()*factor); 95 | p_q->set_u2(p_q->u2()*factor); 96 | p_q->set_u3(p_q->u3()*factor); 97 | } 98 | } 99 | 100 | wrapper::Quaternion multiply(const wrapper::Quaternion& q1, const wrapper::Quaternion& q2) 101 | { 102 | wrapper::Quaternion result; 103 | 104 | result.set_u0(q1.u0()*q2.u0() - q1.u1()*q2.u1() - q1.u2()*q2.u2() - q1.u3()*q2.u3()); 105 | result.set_u1(q1.u0()*q2.u1() + q1.u1()*q2.u0() + q1.u2()*q2.u3() - q1.u3()*q2.u2()); 106 | result.set_u2(q1.u0()*q2.u2() + q1.u2()*q2.u0() + q1.u3()*q2.u1() - q1.u1()*q2.u3()); 107 | result.set_u3(q1.u0()*q2.u3() + q1.u3()*q2.u0() + q1.u1()*q2.u2() - q1.u2()*q2.u1()); 108 | 109 | return result; 110 | } 111 | 112 | double dotProduct(const wrapper::Quaternion& q1, const wrapper::Quaternion& q2) 113 | { 114 | return q1.u0()*q2.u0() + q1.u1()*q2.u1() + q1.u2()*q2.u2() + q1.u3()*q2.u3(); 115 | } 116 | 117 | double euclideanNorm(const wrapper::Quaternion& q) 118 | { 119 | return std::sqrt(dotProduct(q, q)); 120 | } 121 | 122 | void normalize(wrapper::Quaternion* p_q) 123 | { 124 | if (p_q) 125 | { 126 | double norm = euclideanNorm(*p_q); 127 | if (norm != 0.0) 128 | { 129 | p_q->set_u0(p_q->u0() / norm); 130 | p_q->set_u1(p_q->u1() / norm); 131 | p_q->set_u2(p_q->u2() / norm); 132 | p_q->set_u3(p_q->u3() / norm); 133 | } 134 | } 135 | } 136 | 137 | void convert(wrapper::Quaternion* p_q, const wrapper::Euler& e) 138 | { 139 | if (p_q) 140 | { 141 | double z = e.z() * Constants::Conversion::DEG_TO_RAD; 142 | double y = e.y() * Constants::Conversion::DEG_TO_RAD; 143 | double x = e.x() * Constants::Conversion::DEG_TO_RAD; 144 | 145 | // Convert ZYX Euler angles to a rotation matrix. 146 | // See for example https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles for the equations. 147 | 148 | double cx = std::cos(0.5*x); 149 | double sx = std::sin(0.5*x); 150 | double cy = std::cos(0.5*y); 151 | double sy = std::sin(0.5*y); 152 | double cz = std::cos(0.5*z); 153 | double sz = std::sin(0.5*z); 154 | 155 | p_q->set_u0(sx*sy*sz + cx*cy*cz); 156 | p_q->set_u1(-cx*sy*sz + sx*cy*cz); 157 | p_q->set_u2(sx*cy*sz + cx*sy*cz); 158 | p_q->set_u3(cx*cy*sz - sx*sy*cz); 159 | 160 | normalize(p_q); 161 | } 162 | } 163 | 164 | void convert(wrapper::Euler* p_e, const wrapper::Quaternion& q) 165 | { 166 | if(p_e && euclideanNorm(q) != 0.0) 167 | { 168 | // Convert a quaternion to ZYX Euler angles. 169 | // See for example https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles for the equations. 170 | // 171 | // Handle singularities. 172 | // See for example http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm 173 | // for indications of how to derive the equations. 174 | 175 | double y = 0.0; 176 | double z = 0.0; 177 | double x = 0.0; 178 | 179 | double u0 = q.u0(); 180 | double u1 = q.u1(); 181 | double u2 = q.u2(); 182 | double u3 = q.u3(); 183 | 184 | const double SINGULARITY_THRESHOLD = 0.000001; 185 | double singularity_check = u0*u2 - u1*u3; 186 | 187 | // Check for singularity (i.e. when y is close to +- 90 degrees). 188 | // This occur when the argument of y = sin(2*(u0*u2 - u1*u3)) is close to 1.0 -> u0*u2 - u1*u3 = 0.5. 189 | if (std::abs(singularity_check - 0.5) <= SINGULARITY_THRESHOLD) 190 | { 191 | // Y is close to 90 degrees. 192 | x = 2.0*std::atan2(u1, u0); 193 | y = M_PI_2; 194 | } 195 | else if (std::abs(singularity_check + 0.5) <= SINGULARITY_THRESHOLD) 196 | { 197 | // Y is close to -90 degrees. 198 | x = 2.0*std::atan2(u1, u0); 199 | y = -M_PI_2; 200 | } 201 | else 202 | { 203 | x = std::atan2(2.0*(u0*u1 + u2*u3), 1.0 - 2.0*(u1*u1 + u2*u2)); 204 | y = std::asin(2.0*(u0*u2 - u1*u3)); 205 | z = std::atan2(2.0*(u0*u3 + u1*u2), 1.0 - 2.0*(u2*u2 + u3*u3)); 206 | } 207 | 208 | p_e->set_x(x*Constants::Conversion::RAD_TO_DEG); 209 | p_e->set_y(y*Constants::Conversion::RAD_TO_DEG); 210 | p_e->set_z(z*Constants::Conversion::RAD_TO_DEG); 211 | } 212 | } 213 | 214 | void convert(wrapper::Quaternion* p_dq, const wrapper::Quaternion& previous_q, const wrapper::Euler& av) 215 | { 216 | if (p_dq) 217 | { 218 | wrapper::Quaternion temp; 219 | temp.set_u0(0.0); 220 | temp.set_u1(av.x()*Constants::Conversion::DEG_TO_RAD); 221 | temp.set_u2(av.y()*Constants::Conversion::DEG_TO_RAD); 222 | temp.set_u3(av.z()*Constants::Conversion::DEG_TO_RAD); 223 | 224 | p_dq->CopyFrom(multiply(temp, previous_q)); 225 | multiply(p_dq, 0.5); 226 | } 227 | } 228 | 229 | 230 | 231 | 232 | /*********************************************************************************************************************** 233 | * Estimation functions 234 | */ 235 | 236 | bool estimateVelocities(wrapper::Joints* p_estimate, 237 | const wrapper::Joints& current, 238 | const wrapper::Joints& previous, 239 | const double sample_time) 240 | { 241 | bool success = false; 242 | double delta_speed = 0.0; 243 | 244 | if (p_estimate && sample_time > 0.0) 245 | { 246 | p_estimate->Clear(); 247 | 248 | for (int i = 0; i < current.values_size() && i < previous.values_size(); ++i) 249 | { 250 | delta_speed = current.values(i) - previous.values(i); 251 | p_estimate->add_values(delta_speed / sample_time); 252 | } 253 | success = true; 254 | } 255 | 256 | return success; 257 | } 258 | 259 | bool estimateVelocities(wrapper::Euler* p_estimate, 260 | const wrapper::Quaternion& current, 261 | const wrapper::Quaternion& previous, 262 | const double sample_time) 263 | { 264 | bool success = false; 265 | 266 | if (p_estimate && sample_time > 0.0) 267 | { 268 | // Estimate the angular velocity. 269 | // See for example https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions for equations. 270 | // Note: Only valid for orientations, for the same object, at two points close in time. 271 | // Also assumes constant angular velocity between the points. 272 | boost::math::quaternion q1(previous.u0(), previous.u1(), 273 | previous.u2(), previous.u3()); 274 | 275 | boost::math::quaternion q2(current.u0(), current.u1(), 276 | current.u2(), current.u3()); 277 | 278 | boost::math::quaternion estimation = ((2.0*(q2 - q1) / sample_time)*boost::math::conj(q1)); 279 | 280 | p_estimate->set_x(estimation.R_component_2()*Constants::Conversion::RAD_TO_DEG); 281 | p_estimate->set_y(estimation.R_component_3()*Constants::Conversion::RAD_TO_DEG); 282 | p_estimate->set_z(estimation.R_component_4()*Constants::Conversion::RAD_TO_DEG); 283 | 284 | success = true; 285 | } 286 | 287 | return success; 288 | } 289 | 290 | bool estimateVelocities(wrapper::CartesianVelocity* p_estimate, 291 | const wrapper::CartesianPose& current, 292 | const wrapper::CartesianPose& previous, 293 | const double sample_time) 294 | { 295 | bool success = false; 296 | 297 | if (p_estimate && sample_time > 0.0) 298 | { 299 | // Estimate the linear velocity. 300 | p_estimate->mutable_linear()->set_x((current.position().x() - previous.position().x()) / sample_time); 301 | p_estimate->mutable_linear()->set_y((current.position().y() - previous.position().y()) / sample_time); 302 | p_estimate->mutable_linear()->set_z((current.position().z() - previous.position().z()) / sample_time); 303 | 304 | // Estimate the angular velocity. 305 | success = estimateVelocities(p_estimate->mutable_angular(), 306 | current.quaternion(), 307 | previous.quaternion(), 308 | sample_time); 309 | } 310 | 311 | return success; 312 | } 313 | 314 | 315 | 316 | 317 | /*********************************************************************************************************************** 318 | * Find functions 319 | */ 320 | 321 | double findMaxDifference(const wrapper::Joints& j1, const wrapper::Joints& j2) 322 | { 323 | double max_difference = 0.0; 324 | 325 | for (int i = 0; i < j1.values_size() && i < j2.values_size(); ++i) 326 | { 327 | max_difference = std::max(max_difference, std::abs(j1.values(i) - j2.values(i))); 328 | } 329 | 330 | return max_difference; 331 | } 332 | 333 | double findMaxDifference(const wrapper::Cartesian& c1, const wrapper::Cartesian& c2) 334 | { 335 | double max_difference = std::abs(c1.x() - c2.x()); 336 | max_difference = std::max(max_difference, std::abs(c1.y() - c2.y())); 337 | max_difference = std::max(max_difference, std::abs(c1.z() - c2.z())); 338 | 339 | return max_difference; 340 | } 341 | 342 | double findMaxDifference(const wrapper::Euler& e1, const wrapper::Euler& e2) 343 | { 344 | double max_difference = std::abs(e1.x() - e2.x()); 345 | max_difference = std::max(max_difference, std::abs(e1.y() - e2.y())); 346 | max_difference = std::max(max_difference, std::abs(e1.z() - e2.z())); 347 | 348 | return max_difference; 349 | } 350 | 351 | 352 | 353 | 354 | /*********************************************************************************************************************** 355 | * Copy functions 356 | */ 357 | 358 | void copyPresent(wrapper::Joints* p_target, const wrapper::Joints& source) 359 | { 360 | if (p_target) 361 | { 362 | for (int i = 0; i < source.values_size() && i < p_target->values_size(); ++i) 363 | { 364 | p_target->set_values(i, source.values(i)); 365 | } 366 | } 367 | } 368 | 369 | void copyPresent(wrapper::JointSpace* p_target, const wrapper::JointSpace& source) 370 | { 371 | if (p_target) 372 | { 373 | if (source.has_position()) 374 | { 375 | copyPresent(p_target->mutable_position(), source.position()); 376 | } 377 | 378 | if (source.has_velocity()) 379 | { 380 | copyPresent(p_target->mutable_velocity(), source.velocity()); 381 | } 382 | } 383 | } 384 | 385 | void copyPresent(wrapper::Cartesian* p_target, const wrapper::Cartesian& source) 386 | { 387 | if (p_target) 388 | { 389 | if (source.has_x()) 390 | { 391 | p_target->set_x(source.x()); 392 | } 393 | 394 | if (source.has_y()) 395 | { 396 | p_target->set_y(source.y()); 397 | } 398 | 399 | if (source.has_z()) 400 | { 401 | p_target->set_z(source.z()); 402 | } 403 | } 404 | } 405 | 406 | void copyPresent(wrapper::Euler* p_target, const wrapper::Euler& source) 407 | { 408 | if (p_target) 409 | { 410 | if (source.has_x()) 411 | { 412 | p_target->set_x(source.x()); 413 | } 414 | 415 | if (source.has_y()) 416 | { 417 | p_target->set_y(source.y()); 418 | } 419 | 420 | if (source.has_z()) 421 | { 422 | p_target->set_z(source.z()); 423 | } 424 | } 425 | } 426 | 427 | void copyPresent(wrapper::Quaternion* p_target, const wrapper::Quaternion& source) 428 | { 429 | if (p_target) 430 | { 431 | if (source.has_u0()) 432 | { 433 | p_target->set_u0(source.u0()); 434 | } 435 | 436 | if (source.has_u1()) 437 | { 438 | p_target->set_u1(source.u1()); 439 | } 440 | 441 | if (source.has_u2()) 442 | { 443 | p_target->set_u2(source.u2()); 444 | } 445 | 446 | if (source.has_u3()) 447 | { 448 | p_target->set_u3(source.u3()); 449 | } 450 | 451 | normalize(p_target); 452 | } 453 | } 454 | 455 | void copyPresent(wrapper::CartesianPose* p_target, const wrapper::CartesianPose& source) 456 | { 457 | if (p_target) 458 | { 459 | if (source.has_position()) 460 | { 461 | copyPresent(p_target->mutable_position(), source.position()); 462 | } 463 | 464 | if (source.has_euler()) 465 | { 466 | copyPresent(p_target->mutable_euler(), source.euler()); 467 | convert(p_target->mutable_quaternion(), p_target->euler()); 468 | } 469 | else if (source.has_quaternion()) 470 | { 471 | copyPresent(p_target->mutable_quaternion(), source.quaternion()); 472 | normalize(p_target->mutable_quaternion()); 473 | convert(p_target->mutable_euler(), p_target->quaternion()); 474 | } 475 | } 476 | } 477 | 478 | void copyPresent(wrapper::CartesianVelocity* p_target, const wrapper::CartesianVelocity& source) 479 | { 480 | if (p_target) 481 | { 482 | if (source.has_linear()) 483 | { 484 | copyPresent(p_target->mutable_linear(), source.linear()); 485 | } 486 | 487 | if (source.has_angular()) 488 | { 489 | copyPresent(p_target->mutable_angular(), source.angular()); 490 | } 491 | } 492 | } 493 | 494 | void copyPresent(wrapper::CartesianSpace* p_target, const wrapper::CartesianSpace& source) 495 | { 496 | if (p_target) 497 | { 498 | if (source.has_pose()) 499 | { 500 | copyPresent(p_target->mutable_pose(), source.pose()); 501 | } 502 | 503 | if (source.has_velocity()) 504 | { 505 | copyPresent(p_target->mutable_velocity(), source.velocity()); 506 | } 507 | } 508 | } 509 | 510 | void copyPresent(wrapper::Robot* p_target, const wrapper::Robot& source) 511 | { 512 | if (p_target) 513 | { 514 | if (source.has_joints()) 515 | { 516 | copyPresent(p_target->mutable_joints(), source.joints()); 517 | } 518 | 519 | if (source.has_cartesian()) 520 | { 521 | copyPresent(p_target->mutable_cartesian(), source.cartesian()); 522 | } 523 | } 524 | } 525 | 526 | void copyPresent(wrapper::External* p_target, const wrapper::External& source) 527 | { 528 | if (p_target) 529 | { 530 | if (source.has_joints()) 531 | { 532 | copyPresent(p_target->mutable_joints(), source.joints()); 533 | } 534 | } 535 | } 536 | 537 | void copyPresent(wrapper::Output* p_target, const wrapper::Output& source) 538 | { 539 | if (p_target) 540 | { 541 | if (source.has_robot()) 542 | { 543 | copyPresent(p_target->mutable_robot(), source.robot()); 544 | } 545 | 546 | if (source.has_external()) 547 | { 548 | copyPresent(p_target->mutable_external(), source.external()); 549 | } 550 | } 551 | } 552 | 553 | 554 | 555 | 556 | /*********************************************************************************************************************** 557 | * Parse functions 558 | */ 559 | 560 | bool parse(wrapper::Header* p_target, const EgmHeader& source) 561 | { 562 | bool success = false; 563 | 564 | if (p_target && source.has_seqno() && source.has_tm() && source.has_mtype()) 565 | { 566 | p_target->set_sequence_number(source.seqno()); 567 | p_target->set_time_stamp(source.tm()); 568 | 569 | switch (source.mtype()) 570 | { 571 | case EgmHeader_MessageType_MSGTYPE_DATA: 572 | { 573 | p_target->set_message_type(wrapper::Header_MessageType_DATA); 574 | success = true; 575 | } 576 | break; 577 | 578 | case EgmHeader_MessageType_MSGTYPE_UNDEFINED: 579 | case EgmHeader_MessageType_MSGTYPE_COMMAND: 580 | case EgmHeader_MessageType_MSGTYPE_CORRECTION: 581 | case EgmHeader_MessageType_MSGTYPE_PATH_CORRECTION: 582 | default: 583 | { 584 | p_target->set_message_type(wrapper::Header_MessageType_UNDEFINED); 585 | } 586 | } 587 | } 588 | 589 | return success; 590 | } 591 | 592 | bool parse(wrapper::Status* p_target, const EgmRobot& source) 593 | { 594 | bool success = false; 595 | 596 | if (p_target && 597 | source.has_motorstate() && source.motorstate().has_state() && 598 | source.has_mcistate() && source.mcistate().has_state() && 599 | source.has_rapidexecstate() && source.rapidexecstate().has_state() && 600 | source.has_mciconvergencemet()) 601 | { 602 | switch (source.motorstate().state()) 603 | { 604 | case EgmMotorState_MotorStateType_MOTORS_UNDEFINED: 605 | { 606 | p_target->set_motor_state(wrapper::Status_MotorState_MOTORS_UNDEFINED); 607 | } 608 | break; 609 | 610 | case EgmMotorState_MotorStateType_MOTORS_ON: 611 | { 612 | p_target->set_motor_state(wrapper::Status_MotorState_MOTORS_ON); 613 | } 614 | break; 615 | 616 | case EgmMotorState_MotorStateType_MOTORS_OFF: 617 | { 618 | p_target->set_motor_state(wrapper::Status_MotorState_MOTORS_OFF); 619 | } 620 | break; 621 | 622 | default: 623 | { 624 | p_target->set_motor_state(wrapper::Status_MotorState_MOTORS_UNDEFINED); 625 | } 626 | } 627 | 628 | switch (source.mcistate().state()) 629 | { 630 | case EgmMCIState_MCIStateType_MCI_UNDEFINED: 631 | { 632 | p_target->set_egm_state(wrapper::Status_EGMState_EGM_UNDEFINED); 633 | } 634 | break; 635 | 636 | case EgmMCIState_MCIStateType_MCI_ERROR: 637 | { 638 | p_target->set_egm_state(wrapper::Status_EGMState_EGM_ERROR); 639 | } 640 | break; 641 | 642 | case EgmMCIState_MCIStateType_MCI_STOPPED: 643 | { 644 | p_target->set_egm_state(wrapper::Status_EGMState_EGM_STOPPED); 645 | } 646 | break; 647 | 648 | case EgmMCIState_MCIStateType_MCI_RUNNING: 649 | { 650 | p_target->set_egm_state(wrapper::Status_EGMState_EGM_RUNNING); 651 | } 652 | break; 653 | 654 | default: 655 | { 656 | p_target->set_egm_state(wrapper::Status_EGMState_EGM_UNDEFINED); 657 | } 658 | } 659 | 660 | switch (source.rapidexecstate().state()) 661 | { 662 | case EgmRapidCtrlExecState_RapidCtrlExecStateType_RAPID_UNDEFINED: 663 | { 664 | p_target->set_rapid_execution_state(wrapper::Status_RAPIDExecutionState_RAPID_UNDEFINED); 665 | } 666 | break; 667 | 668 | case EgmRapidCtrlExecState_RapidCtrlExecStateType_RAPID_STOPPED: 669 | { 670 | p_target->set_rapid_execution_state(wrapper::Status_RAPIDExecutionState_RAPID_STOPPED); 671 | } 672 | break; 673 | 674 | case EgmRapidCtrlExecState_RapidCtrlExecStateType_RAPID_RUNNING: 675 | { 676 | p_target->set_rapid_execution_state(wrapper::Status_RAPIDExecutionState_RAPID_RUNNING); 677 | } 678 | break; 679 | 680 | default: 681 | { 682 | p_target->set_rapid_execution_state(wrapper::Status_RAPIDExecutionState_RAPID_UNDEFINED); 683 | } 684 | } 685 | 686 | p_target->set_egm_convergence_met(source.mciconvergencemet()); 687 | 688 | if(source.has_utilizationrate()) 689 | { 690 | p_target->set_utilization_rate(source.utilizationrate()); 691 | } 692 | 693 | success = true; 694 | } 695 | 696 | return success; 697 | } 698 | 699 | bool parse(wrapper::Clock* p_target, const EgmClock& source) 700 | { 701 | bool success = true; 702 | 703 | if (p_target && source.has_sec() && source.has_usec()) 704 | { 705 | p_target->set_sec(source.sec()); 706 | p_target->set_usec(source.usec()); 707 | } 708 | else 709 | { 710 | success = false; 711 | } 712 | 713 | return success; 714 | } 715 | 716 | bool parse(wrapper::Joints* p_target_robot, 717 | wrapper::Joints* p_target_external, 718 | const EgmJoints& source_robot, 719 | const EgmJoints& source_external, 720 | const RobotAxes axes) 721 | { 722 | bool success = false; 723 | 724 | if (p_target_robot && p_target_external) 725 | { 726 | p_target_robot->Clear(); 727 | p_target_external->Clear(); 728 | 729 | switch (axes) 730 | { 731 | case None: 732 | { 733 | if (source_robot.joints_size() == 0) 734 | { 735 | for (int i = 0; i < source_external.joints_size(); ++i) 736 | { 737 | p_target_external->add_values(source_external.joints(i)); 738 | } 739 | 740 | success = true; 741 | } 742 | } 743 | break; 744 | 745 | case Six: 746 | { 747 | if (source_robot.joints_size() == Constants::RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS) 748 | { 749 | for (int i = 0; i < source_robot.joints_size(); ++i) 750 | { 751 | p_target_robot->add_values(source_robot.joints(i)); 752 | } 753 | 754 | for (int i = 0; i < source_external.joints_size(); ++i) 755 | { 756 | p_target_external->add_values(source_external.joints(i)); 757 | } 758 | 759 | success = true; 760 | } 761 | } 762 | break; 763 | 764 | case Seven: 765 | { 766 | // If using a seven axes robot (e.g. IRB14000): Map to special case. 767 | if (source_robot.joints_size() == Constants::RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS && 768 | source_external.joints_size() >= 1) 769 | { 770 | p_target_robot->add_values(source_robot.joints(0)); 771 | p_target_robot->add_values(source_robot.joints(1)); 772 | p_target_robot->add_values(source_external.joints(0)); 773 | p_target_robot->add_values(source_robot.joints(2)); 774 | p_target_robot->add_values(source_robot.joints(3)); 775 | p_target_robot->add_values(source_robot.joints(4)); 776 | p_target_robot->add_values(source_robot.joints(5)); 777 | 778 | for (int i = 1; i < source_external.joints_size(); ++i) 779 | { 780 | p_target_external->add_values(source_external.joints(i)); 781 | } 782 | 783 | success = true; 784 | } 785 | } 786 | break; 787 | } 788 | } 789 | 790 | return success; 791 | } 792 | 793 | bool parse(wrapper::CartesianPose* p_target, const EgmPose& source) 794 | { 795 | bool success = true; 796 | 797 | if (p_target) 798 | { 799 | p_target->Clear(); 800 | 801 | if (source.has_pos() && source.pos().has_x() && source.pos().has_y() && source.pos().has_z()) 802 | { 803 | p_target->mutable_position()->set_x(source.pos().x()); 804 | p_target->mutable_position()->set_y(source.pos().y()); 805 | p_target->mutable_position()->set_z(source.pos().z()); 806 | } 807 | else 808 | { 809 | success = false; 810 | } 811 | 812 | if (success && 813 | source.has_orient() && 814 | source.orient().has_u0() && 815 | source.orient().has_u1() && 816 | source.orient().has_u2() && 817 | source.orient().has_u3()) 818 | { 819 | p_target->mutable_quaternion()->set_u0(source.orient().u0()); 820 | p_target->mutable_quaternion()->set_u1(source.orient().u1()); 821 | p_target->mutable_quaternion()->set_u2(source.orient().u2()); 822 | p_target->mutable_quaternion()->set_u3(source.orient().u3()); 823 | } 824 | else 825 | { 826 | success = false; 827 | } 828 | 829 | if (success) 830 | { 831 | if (source.has_euler() && source.euler().has_x() && source.euler().has_y() && source.euler().has_z()) 832 | { 833 | p_target->mutable_euler()->set_x(source.euler().x()); 834 | p_target->mutable_euler()->set_y(source.euler().y()); 835 | p_target->mutable_euler()->set_z(source.euler().z()); 836 | } 837 | else 838 | { 839 | convert(p_target->mutable_euler(), p_target->quaternion()); 840 | } 841 | } 842 | } 843 | 844 | return success; 845 | } 846 | 847 | bool parse(wrapper::Feedback* p_target, const EgmFeedBack& source, const RobotAxes axes) 848 | { 849 | bool success = false; 850 | 851 | if (p_target) 852 | { 853 | success = parse(p_target->mutable_robot()->mutable_joints()->mutable_position(), 854 | p_target->mutable_external()->mutable_joints()->mutable_position(), 855 | source.joints(), source.externaljoints(), axes); 856 | 857 | if (success) 858 | { 859 | if(axes == None) 860 | { 861 | success = !source.has_cartesian(); 862 | } 863 | else 864 | { 865 | success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian()); 866 | } 867 | 868 | if (success) 869 | { 870 | success = parse(p_target->mutable_time(), source.time()); 871 | } 872 | } 873 | } 874 | 875 | return success; 876 | } 877 | 878 | bool parse(wrapper::Planned* p_target, const EgmPlanned& source, const RobotAxes axes) 879 | { 880 | bool success = false; 881 | 882 | if (p_target) 883 | { 884 | success = parse(p_target->mutable_robot()->mutable_joints()->mutable_position(), 885 | p_target->mutable_external()->mutable_joints()->mutable_position(), 886 | source.joints(), source.externaljoints(), axes); 887 | 888 | if (success) 889 | { 890 | if(axes == None) 891 | { 892 | success = !source.has_cartesian(); 893 | } 894 | else 895 | { 896 | success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian()); 897 | } 898 | 899 | if (success) 900 | { 901 | success = parse(p_target->mutable_time(), source.time()); 902 | } 903 | } 904 | } 905 | 906 | return success; 907 | } 908 | 909 | 910 | 911 | 912 | /*********************************************************************************************************************** 913 | * Reset functions 914 | */ 915 | 916 | void reset(wrapper::Joints* p_joints, const unsigned int number_of_joints) 917 | { 918 | if (p_joints) 919 | { 920 | p_joints->Clear(); 921 | 922 | for (unsigned int i = 0; i < number_of_joints; ++i) 923 | { 924 | p_joints->add_values(0.0); 925 | } 926 | } 927 | } 928 | 929 | void reset(wrapper::Cartesian* p_cartesian) 930 | { 931 | if (p_cartesian) 932 | { 933 | p_cartesian->set_x(0.0); 934 | p_cartesian->set_y(0.0); 935 | p_cartesian->set_z(0.0); 936 | } 937 | } 938 | 939 | void reset(wrapper::Euler* p_euler) 940 | { 941 | if (p_euler) 942 | { 943 | p_euler->set_x(0.0); 944 | p_euler->set_y(0.0); 945 | p_euler->set_z(0.0); 946 | } 947 | } 948 | 949 | 950 | 951 | 952 | /*********************************************************************************************************************** 953 | * Verify functions 954 | */ 955 | 956 | bool verify(const double value) 957 | { 958 | return !std::isnan(value) && !std::isinf(value); 959 | } 960 | 961 | bool verify(const wrapper::Joints& joints) 962 | { 963 | for (int i = 0; i < joints.values_size(); ++i) 964 | { 965 | if(!verify(joints.values(i))) 966 | { 967 | return false; 968 | } 969 | } 970 | 971 | return true; 972 | } 973 | 974 | bool verify(const wrapper::Cartesian& cartesian) 975 | { 976 | if((cartesian.has_x() && !verify(cartesian.x())) || 977 | (cartesian.has_y() && !verify(cartesian.y())) || 978 | (cartesian.has_z() && !verify(cartesian.z()))) 979 | { 980 | return false; 981 | } 982 | 983 | return true; 984 | } 985 | 986 | bool verify(const wrapper::Euler& euler) 987 | { 988 | if((euler.has_x() && !verify(euler.x())) || 989 | (euler.has_y() && !verify(euler.y())) || 990 | (euler.has_z() && !verify(euler.z()))) 991 | { 992 | return false; 993 | } 994 | 995 | return true; 996 | } 997 | 998 | bool verify(const wrapper::Quaternion& quaternion) 999 | { 1000 | if((quaternion.has_u0() && !verify(quaternion.u0())) || 1001 | (quaternion.has_u1() && !verify(quaternion.u1())) || 1002 | (quaternion.has_u2() && !verify(quaternion.u2())) || 1003 | (quaternion.has_u3() && !verify(quaternion.u3()))) 1004 | { 1005 | return false; 1006 | } 1007 | 1008 | return true; 1009 | } 1010 | 1011 | bool verify(const wrapper::CartesianPose& pose) 1012 | { 1013 | if((pose.has_position() && !verify(pose.position())) || 1014 | (pose.has_euler() && !verify(pose.euler())) || 1015 | (pose.has_quaternion() && !verify(pose.quaternion()))) 1016 | { 1017 | return false; 1018 | } 1019 | 1020 | return true; 1021 | } 1022 | 1023 | bool verify(const wrapper::CartesianVelocity& velocity) 1024 | { 1025 | if((velocity.has_angular() && !verify(velocity.angular())) || 1026 | (velocity.has_linear() && !verify(velocity.linear()))) 1027 | { 1028 | return false; 1029 | } 1030 | 1031 | return true; 1032 | } 1033 | 1034 | } // end namespace egm 1035 | } // end namespace abb 1036 | -------------------------------------------------------------------------------- /src/egm_controller_interface.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************************************** 2 | * 3 | * Copyright (c) 2015, ABB Schweiz AG 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with 7 | * or without modification, are permitted provided that 8 | * the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the 11 | * above copyright notice, this list of conditions 12 | * and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the 14 | * above copyright notice, this list of conditions 15 | * and the following disclaimer in the documentation 16 | * and/or other materials provided with the 17 | * distribution. 18 | * * Neither the name of ABB nor the names of its 19 | * contributors may be used to endorse or promote 20 | * products derived from this software without 21 | * specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 32 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *********************************************************************************************************************** 35 | */ 36 | 37 | #include 38 | 39 | #include "abb_libegm/egm_common_auxiliary.h" 40 | #include "abb_libegm/egm_controller_interface.h" 41 | 42 | namespace abb 43 | { 44 | namespace egm 45 | { 46 | /*********************************************************************************************************************** 47 | * Class definitions: EGMControllerInterface::ControllerMotion 48 | */ 49 | 50 | // See https://stackoverflow.com/questions/16957458/static-const-in-c-class-undefined-reference/16957554 51 | const unsigned int EGMControllerInterface::ControllerMotion::WRITE_TIMEOUT_MS; 52 | 53 | /************************************************************ 54 | * Primary methods 55 | */ 56 | 57 | void EGMControllerInterface::ControllerMotion::initialize(const bool first_message) 58 | { 59 | if (first_message) 60 | { 61 | boost::lock_guard lock(read_mutex_); 62 | boost::lock_guard lock2(write_mutex_); 63 | 64 | read_data_ready_ = false; 65 | write_data_ready_ = false; 66 | } 67 | } 68 | 69 | void EGMControllerInterface::ControllerMotion::writeInputs(const wrapper::Input& inputs) 70 | { 71 | boost::lock_guard lock(read_mutex_); 72 | 73 | inputs_.CopyFrom(inputs); 74 | 75 | read_data_ready_ = true; 76 | read_condition_variable_.notify_all(); 77 | } 78 | 79 | void EGMControllerInterface::ControllerMotion::readOutputs(wrapper::Output* p_outputs) 80 | { 81 | bool timed_out = false; 82 | 83 | boost::unique_lock lock(write_mutex_); 84 | 85 | while (!write_data_ready_ && !timed_out) 86 | { 87 | timed_out = !write_condition_variable_.timed_wait(lock, boost::posix_time::milliseconds(WRITE_TIMEOUT_MS)); 88 | } 89 | 90 | if (!timed_out && p_outputs) 91 | { 92 | copyPresent(p_outputs, outputs_); 93 | write_data_ready_ = false; 94 | } 95 | } 96 | 97 | bool EGMControllerInterface::ControllerMotion::waitForMessage(const unsigned int timeout_ms) 98 | { 99 | boost::unique_lock lock(read_mutex_); 100 | 101 | bool timed_out = false; 102 | 103 | while (!read_data_ready_ && !timed_out) 104 | { 105 | if (timeout_ms <= 0) 106 | { 107 | read_condition_variable_.wait(lock); 108 | } 109 | else 110 | { 111 | timed_out = !read_condition_variable_.timed_wait(lock, boost::posix_time::milliseconds(timeout_ms)); 112 | } 113 | } 114 | 115 | return !timed_out; 116 | } 117 | 118 | void EGMControllerInterface::ControllerMotion::readInputs(wrapper::Input* p_inputs) 119 | { 120 | boost::lock_guard lock(read_mutex_); 121 | 122 | p_inputs->CopyFrom(inputs_); 123 | read_data_ready_ = false; 124 | } 125 | 126 | void EGMControllerInterface::ControllerMotion::writeOutputs(const wrapper::Output& outputs) 127 | { 128 | boost::lock_guard lock(write_mutex_); 129 | 130 | outputs_.CopyFrom(outputs); 131 | 132 | write_data_ready_ = true; 133 | write_condition_variable_.notify_all(); 134 | } 135 | 136 | 137 | 138 | 139 | /*********************************************************************************************************************** 140 | * Class definitions: EGMControllerInterface 141 | */ 142 | 143 | /************************************************************ 144 | * Primary methods 145 | */ 146 | 147 | EGMControllerInterface::EGMControllerInterface(boost::asio::io_service& io_service, 148 | const unsigned short port_number, 149 | const BaseConfiguration& configuration) 150 | : 151 | EGMBaseInterface(io_service, port_number, configuration) 152 | { 153 | if (configuration_.active.use_logging) 154 | { 155 | std::stringstream ss; 156 | ss << "port_" << port_number << +"_log.csv"; 157 | p_logger_.reset(new EGMLogger(ss.str())); 158 | } 159 | } 160 | 161 | const std::string& EGMControllerInterface::callback(const UDPServerData& server_data) 162 | { 163 | // Initialize the callback by: 164 | // - Parsing and extracting data from the received message. 165 | // - Updating any pending configuration changes. 166 | // - Preparing the outputs. 167 | if (initializeCallback(server_data)) 168 | { 169 | // Additional initialization for direct motion references. 170 | controller_motion_.initialize(inputs_.isFirstMessage()); 171 | 172 | // Handle demo execution or external controller execution. 173 | if (configuration_.active.use_demo_outputs) 174 | { 175 | outputs_.generateDemoOutputs(inputs_); 176 | } 177 | else 178 | { 179 | // Make the current inputs available (to the external control loop), and notify that it is available. 180 | controller_motion_.writeInputs(inputs_.current()); 181 | 182 | // Send a notification via the (optional) external condition variable. 183 | if(configuration_.active.p_new_message_cv) 184 | { 185 | configuration_.active.p_new_message_cv->notify_all(); 186 | } 187 | 188 | if (inputs_.isFirstMessage() || inputs_.statesOk()) 189 | { 190 | // Wait for new outputs (from the external control loop), or until a timeout occurs. 191 | controller_motion_.readOutputs(&outputs_.current); 192 | } 193 | } 194 | 195 | // Log inputs and outputs, if set to do so. 196 | if (configuration_.active.use_logging && p_logger_) 197 | { 198 | logData(inputs_, outputs_, configuration_.active.max_logging_duration); 199 | } 200 | 201 | // Constuct the reply message. 202 | outputs_.constructReply(configuration_.active); 203 | 204 | // Prepare for the next callback. 205 | inputs_.updatePrevious(); 206 | outputs_.updatePrevious(); 207 | } 208 | 209 | // Return the reply. 210 | return outputs_.reply(); 211 | } 212 | 213 | /************************************************************ 214 | * User interaction methods 215 | */ 216 | 217 | bool EGMControllerInterface::waitForMessage(const unsigned int timeout_ms) 218 | { 219 | return controller_motion_.waitForMessage(timeout_ms); 220 | } 221 | 222 | void EGMControllerInterface::read(wrapper::Input* p_inputs) 223 | { 224 | controller_motion_.readInputs(p_inputs); 225 | } 226 | 227 | void EGMControllerInterface::write(const wrapper::Output& outputs) 228 | { 229 | controller_motion_.writeOutputs(outputs); 230 | } 231 | 232 | } // end namespace egm 233 | } // end namespace abb 234 | -------------------------------------------------------------------------------- /src/egm_interpolator.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************************************** 2 | * 3 | * Copyright (c) 2015, ABB Schweiz AG 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with 7 | * or without modification, are permitted provided that 8 | * the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the 11 | * above copyright notice, this list of conditions 12 | * and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the 14 | * above copyright notice, this list of conditions 15 | * and the following disclaimer in the documentation 16 | * and/or other materials provided with the 17 | * distribution. 18 | * * Neither the name of ABB nor the names of its 19 | * contributors may be used to endorse or promote 20 | * products derived from this software without 21 | * specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 32 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *********************************************************************************************************************** 35 | */ 36 | 37 | #define _USE_MATH_DEFINES 38 | 39 | #include 40 | 41 | #include "abb_libegm/egm_common_auxiliary.h" 42 | #include "abb_libegm/egm_interpolator.h" 43 | 44 | namespace abb 45 | { 46 | namespace egm 47 | { 48 | /*********************************************************************************************************************** 49 | * Class definitions: EGMInterpolator::SplineConditions 50 | */ 51 | 52 | /************************************************************ 53 | * Primary methods 54 | */ 55 | 56 | void EGMInterpolator::SplineConditions::setConditions(const int index, 57 | const wrapper::trajectory::JointGoal& start, 58 | const wrapper::trajectory::JointGoal& goal) 59 | { 60 | alfa = start.position().values(index); 61 | d_alfa = start.velocity().values(index); 62 | dd_alfa = start.acceleration().values(index); 63 | beta = goal.position().values(index); 64 | d_beta = goal.velocity().values(index); 65 | dd_beta = goal.acceleration().values(index); 66 | } 67 | 68 | void EGMInterpolator::SplineConditions::setConditions(const EGMInterpolator::Axis axis, 69 | const wrapper::trajectory::CartesianGoal& start, 70 | const wrapper::trajectory::CartesianGoal& goal) 71 | { 72 | switch (axis) 73 | { 74 | case EGMInterpolator::X: 75 | { 76 | alfa = start.pose().position().x(); 77 | d_alfa = start.velocity().x(); 78 | dd_alfa = start.acceleration().x(); 79 | beta = goal.pose().position().x(); 80 | d_beta = goal.velocity().x(); 81 | dd_beta = goal.acceleration().x(); 82 | } 83 | break; 84 | 85 | case EGMInterpolator::Y: 86 | { 87 | alfa = start.pose().position().y(); 88 | d_alfa = start.velocity().y(); 89 | dd_alfa = start.acceleration().y(); 90 | beta = goal.pose().position().y(); 91 | d_beta = goal.velocity().y(); 92 | dd_beta = goal.acceleration().y(); 93 | } 94 | break; 95 | 96 | case EGMInterpolator::Z: 97 | { 98 | alfa = start.pose().position().z(); 99 | d_alfa = start.velocity().z(); 100 | dd_alfa = start.acceleration().z(); 101 | beta = goal.pose().position().z(); 102 | d_beta = goal.velocity().z(); 103 | dd_beta = goal.acceleration().z(); 104 | } 105 | break; 106 | } 107 | } 108 | 109 | 110 | 111 | 112 | /*********************************************************************************************************************** 113 | * Class definitions: EGMInterpolator::SplinePolynomial 114 | */ 115 | 116 | /************************************************************ 117 | * Primary methods 118 | */ 119 | 120 | void EGMInterpolator::SplinePolynomial::update(const SplineConditions& conditions) 121 | { 122 | const double T = conditions.duration; 123 | const double K = saturate(conditions.ramp_down_factor, 0.0, 1.0); 124 | 125 | // Support variables. 126 | double c1 = 0.0; 127 | double c2 = 0.0; 128 | double c3 = 0.0; 129 | double alfa = conditions.alfa; 130 | double d_alfa = conditions.d_alfa; 131 | double dd_alfa = conditions.dd_alfa; 132 | double beta = conditions.beta; 133 | double d_beta = conditions.d_beta; 134 | double dd_beta = conditions.dd_beta; 135 | 136 | if (conditions.do_ramp_down) 137 | { 138 | //--------------------------------------------------------------- 139 | // Calculate the spline polynomial coefficients for: 140 | // S(t) = A + B*t + C*t^2 + D*t^3 141 | // 142 | // Note: Used when ramping down the velocity. 143 | // E.g. for stopping use K = 0.0. 144 | // 145 | //--------------------- 146 | // Conditions: 147 | //--------------------- 148 | // 0 < t <= T 149 | // 150 | // S(0) = alfa 151 | // d_S(0) = d_alfa 152 | // 153 | // d_S(T) = K*d_alfa, K = [0.0, 1.0]; 154 | // dd_S(T) = 0.0 155 | //--------------------------------------------------------------- 156 | a_ = alfa; 157 | b_ = d_alfa; 158 | c_ = ((K - 1.0)*d_alfa) / T; 159 | d_ = (-c_)/(3.0*T); 160 | e_ = 0.0; 161 | f_ = 0.0; 162 | } 163 | else 164 | { 165 | switch (conditions.spline_method) 166 | { 167 | case TrajectoryConfiguration::Linear: 168 | { 169 | //--------------------------------------------------------------- 170 | // Calculate the spline polynomial coefficients for: 171 | // S(t) = A + B*t 172 | // 173 | //--------------------- 174 | // Conditions: 175 | //--------------------- 176 | // 0 <= t <= T 177 | // 178 | // S(0) = alfa 179 | // S(T) = beta 180 | //--------------------------------------------------------------- 181 | a_ = alfa; 182 | b_ = (beta - alfa) / T; 183 | c_ = 0.0; 184 | d_ = 0.0; 185 | e_ = 0.0; 186 | f_ = 0.0; 187 | } 188 | break; 189 | 190 | case TrajectoryConfiguration::Square: 191 | { 192 | //--------------------------------------------------------------- 193 | // Calculate the spline polynomial coefficients for: 194 | // S(t) = A + B*t + C*t^2 195 | // 196 | //--------------------- 197 | // Conditions: 198 | //--------------------- 199 | // 0 <= t <= T 200 | // 201 | // S(0) = alfa 202 | // d_S(0) = d_alfa 203 | // 204 | // S(T) = beta 205 | //--------------------------------------------------------------- 206 | a_ = alfa; 207 | b_ = d_alfa; 208 | c_ = (beta - alfa - d_alfa*T) / std::pow(T, 2); 209 | d_ = 0.0; 210 | e_ = 0.0; 211 | f_ = 0.0; 212 | } 213 | break; 214 | 215 | case TrajectoryConfiguration::Cubic: 216 | { 217 | //--------------------------------------------------------------- 218 | // Calculate the spline polynomial coefficients for: 219 | // S(t) = A + B*t + C*t^2 + D*t^3 220 | // 221 | //--------------------- 222 | // Conditions: 223 | //--------------------- 224 | // 0 <= t <= T 225 | // 226 | // S(0) = alfa 227 | // d_S(0) = d_alfa 228 | // 229 | // S(T) = beta 230 | // d_S(T) = d_beta 231 | //--------------------------------------------------------------- 232 | a_ = alfa; 233 | b_ = d_alfa; 234 | 235 | c1 = beta - alfa - d_alfa*T; 236 | c2 = d_beta - d_alfa; 237 | 238 | c_ = 3.0*c1 / std::pow(T, 2) - c2 / T; 239 | d_ = c1 / std::pow(T, 3) - c_ / T; 240 | e_ = 0.0; 241 | f_ = 0.0; 242 | } 243 | break; 244 | 245 | case TrajectoryConfiguration::Quintic: 246 | { 247 | //--------------------------------------------------------------- 248 | // Calculate the spline polynomial coefficients for: 249 | // S(t) = A + B*t + C*t^2 + D*t^3 + E*t^4 + F*t^5 250 | // 251 | //--------------------- 252 | // Conditions: 253 | //--------------------- 254 | // 0 <= t <= T 255 | // 256 | // S(0) = alfa 257 | // d_S(0) = d_alfa 258 | // dd_S(0) = dd_alfa 259 | // 260 | // S(T) = beta 261 | // d_S(T) = d_beta 262 | // dd_S(T) = dd_beta 263 | //--------------------------------------------------------------- 264 | a_ = alfa; 265 | b_ = d_alfa; 266 | c_ = dd_alfa / 2.0; 267 | 268 | c1 = beta - alfa - d_alfa*T - (dd_alfa / 2.0)*std::pow(T, 2); 269 | c2 = d_beta - d_alfa - dd_alfa*T; 270 | c3 = dd_beta - dd_alfa; 271 | 272 | d_ = 10.0*c1 / std::pow(T, 3) - 4.0 * c2 / std::pow(T, 2) + c3 / (2.0*T); 273 | e_ = 5.0*c1 / std::pow(T, 4) - c2 / std::pow(T, 3) - 2.0*d_ / T; 274 | f_ = c1 / std::pow(T, 5) - d_ / std::pow(T, 2) - e_ / T; 275 | } 276 | break; 277 | } 278 | } 279 | } 280 | 281 | void EGMInterpolator::SplinePolynomial::evaluate(wrapper::trajectory::JointGoal* p_output, 282 | const int index, 283 | const double t) 284 | { 285 | //--------------------------------------------------------------- 286 | // Evaluate: 287 | // S(t) = A + B*t + C*t^2 + D*t^3 + E*t^4 + F*t^5 288 | // S_prime(t) = B + 2C*t + 3D*t^2 + 4E*t^3 + 5F*t^4 289 | // S_bis(t) = 2C + 6D*t + 12E*t^2 + 20F*t^3 290 | // 291 | // Condition: 0 <= t <= T 292 | //--------------------------------------------------------------- 293 | p_output->mutable_position()->set_values(index, calculatePosition(t)); 294 | p_output->mutable_velocity()->set_values(index, calculateVelocity(t)); 295 | p_output->mutable_acceleration()->set_values(index, calculateAcceleration(t)); 296 | } 297 | 298 | void EGMInterpolator::SplinePolynomial::evaluate(wrapper::trajectory::CartesianGoal* p_output, 299 | const Axis axis, 300 | const double t) 301 | { 302 | //--------------------------------------------------------------- 303 | // Evaluate: 304 | // S(t) = A + B*t + C*t^2 + D*t^3 + E*t^4 + F*t^5 305 | // S_prime(t) = B + 2C*t + 3D*t^2 + 4E*t^3 + 5F*t^4 306 | // S_bis(t) = 2C + 6D*t + 12E*t^2 + 20F*t^3 307 | // 308 | // Condition: 0 <= t <= T 309 | //--------------------------------------------------------------- 310 | switch (axis) 311 | { 312 | case X: 313 | { 314 | p_output->mutable_pose()->mutable_position()->set_x(calculatePosition(t)); 315 | p_output->mutable_velocity()->set_x(calculateVelocity(t)); 316 | p_output->mutable_acceleration()->set_x(calculateAcceleration(t)); 317 | } 318 | break; 319 | 320 | case Y: 321 | { 322 | p_output->mutable_pose()->mutable_position()->set_y(calculatePosition(t)); 323 | p_output->mutable_velocity()->set_y(calculateVelocity(t)); 324 | p_output->mutable_acceleration()->set_y(calculateAcceleration(t)); 325 | } 326 | break; 327 | 328 | case Z: 329 | { 330 | p_output->mutable_pose()->mutable_position()->set_z(calculatePosition(t)); 331 | p_output->mutable_velocity()->set_z(calculateVelocity(t)); 332 | p_output->mutable_acceleration()->set_z(calculateAcceleration(t)); 333 | } 334 | break; 335 | } 336 | } 337 | 338 | 339 | 340 | 341 | /*********************************************************************************************************************** 342 | * Class definitions: EGMInterpolator::Slerp 343 | */ 344 | 345 | /************************************************************ 346 | * Primary methods 347 | */ 348 | 349 | void EGMInterpolator::Slerp::update(const wrapper::Quaternion& start, 350 | const wrapper::Quaternion& goal, 351 | const Conditions& conditions) 352 | { 353 | duration_ = conditions.duration; 354 | 355 | q0_.CopyFrom(start); 356 | q1_.CopyFrom(goal); 357 | 358 | normalize(&q0_); 359 | normalize(&q1_); 360 | 361 | double dot_product = dotProduct(q0_, q1_); 362 | 363 | // Check if Slerp or linear interpolation should be used. 364 | use_linear_ = std::abs(dot_product) > DOT_PRODUCT_THRESHOLD; 365 | 366 | if (!use_linear_) 367 | { 368 | // Reverse one of the quaternions, if the dot product is negative. 369 | // This is to make the Slerp to take the shorter path. 370 | if (dot_product < 0.0) 371 | { 372 | multiply(&q1_, -1.0); 373 | dot_product = -dot_product; 374 | } 375 | 376 | // Saturate the dot product to be within acos input range. 377 | dot_product = saturate(dot_product, -1.0, 1.0); 378 | 379 | // Calculate the coefficient. 380 | omega_ = std::acos(dot_product); 381 | } 382 | } 383 | 384 | void EGMInterpolator::Slerp::evaluate(wrapper::trajectory::CartesianGoal* p_output, double t) 385 | { 386 | // Support variables. 387 | double a = 1.0; 388 | double b = 0.0; 389 | double c = 1.0; 390 | double d = 0.0; 391 | double k = 1.0 / std::sin(omega_); 392 | 393 | // Quaternion and angular velocity output to set. 394 | // Note: The Euler field is internally used to contain angular velocities. 395 | wrapper::Quaternion* p_q = p_output->mutable_pose()->mutable_quaternion(); 396 | wrapper::Euler* p_av = p_output->mutable_pose()->mutable_euler(); 397 | 398 | // Saturate t to be within 0.0 and 1.0. 399 | t = saturate(t / duration_, 0.0, 1.0); 400 | 401 | if (use_linear_) 402 | { 403 | // Calculate quaternion and angular velocity with linear interpolation. 404 | a = 1.0 - t; 405 | b = t; 406 | c = -1.0; 407 | d = 1.0; 408 | } 409 | else 410 | { 411 | // Calculate quaternion and angular velocity with Slerp interpolation. 412 | a = k*std::sin((1.0 - t)*omega_); 413 | b = k*std::sin(t*omega_); 414 | c = -omega_*k*std::cos((1.0 - t)*omega_) / duration_; 415 | d = omega_*k*std::cos(t*omega_) / duration_; 416 | } 417 | 418 | // Calculate the quaternion output. 419 | p_q->set_u0(a*q0_.u0() + b*q1_.u0()); 420 | p_q->set_u1(a*q0_.u1() + b*q1_.u1()); 421 | p_q->set_u2(a*q0_.u2() + b*q1_.u2()); 422 | p_q->set_u3(a*q0_.u3() + b*q1_.u3()); 423 | normalize(p_q); 424 | 425 | // Conjugate of the normalized quaternion output. 426 | wrapper::Quaternion conj_q; 427 | conj_q.set_u0(p_q->u0()); 428 | conj_q.set_u1(-p_q->u1()); 429 | conj_q.set_u2(-p_q->u2()); 430 | conj_q.set_u3(-p_q->u3()); 431 | 432 | // Derivate of either linear or Slerp interpolation. 433 | wrapper::Quaternion d_q; 434 | d_q.set_u0(c*q0_.u0() + d*q1_.u0()); 435 | d_q.set_u1(c*q0_.u1() + d*q1_.u1()); 436 | d_q.set_u2(c*q0_.u2() + d*q1_.u2()); 437 | d_q.set_u3(c*q0_.u3() + d*q1_.u3()); 438 | 439 | // Calculate the angular velocity output. 440 | wrapper::Quaternion mult_q(multiply(d_q, conj_q)); 441 | p_av->set_x(2.0*mult_q.u1()*Constants::Conversion::RAD_TO_DEG); 442 | p_av->set_y(2.0*mult_q.u2()*Constants::Conversion::RAD_TO_DEG); 443 | p_av->set_z(2.0*mult_q.u3()*Constants::Conversion::RAD_TO_DEG); 444 | } 445 | 446 | 447 | 448 | 449 | /*********************************************************************************************************************** 450 | * Class definitions: EGMInterpolator::SoftRamp 451 | */ 452 | 453 | /************************************************************ 454 | * Primary methods 455 | */ 456 | 457 | void EGMInterpolator::SoftRamp::update(const wrapper::trajectory::PointGoal& start, 458 | const wrapper::trajectory::PointGoal& goal, 459 | const Conditions& conditions) 460 | { 461 | duration_ = conditions.duration; 462 | operation_ = conditions.operation; 463 | 464 | switch (operation_) 465 | { 466 | case RampDown: 467 | { 468 | // Note: The Euler field is internally used to contain angular velocities. 469 | start_angular_velocity_.CopyFrom(start.robot().cartesian().pose().euler()); 470 | } 471 | break; 472 | 473 | case RampInPosition: 474 | case RampInVelocity: 475 | { 476 | start_.CopyFrom(start); 477 | goal_.CopyFrom(goal); 478 | } 479 | break; 480 | } 481 | } 482 | 483 | void EGMInterpolator::SoftRamp::evaluate(wrapper::trajectory::JointGoal* p_output, 484 | const bool robot, 485 | const double sample_time, 486 | double t) 487 | { 488 | double ramp_factor = 0.0; 489 | double d_ramp_factor = 0.0; 490 | 491 | // Saturate t to be within 0.0 and 1.0. 492 | t = saturate(t / duration_, 0.0, 1.0); 493 | 494 | switch (operation_) 495 | { 496 | case RampInPosition: 497 | { 498 | // Ramp factor that goes from 0.0 to 1.0 and its derivate. 499 | ramp_factor = 0.5*std::cos(M_PI*t + M_PI) + 0.5; 500 | d_ramp_factor = -0.5*M_PI / duration_*std::sin(M_PI*t + M_PI); 501 | 502 | // Output to set. 503 | wrapper::Joints* p_p = p_output->mutable_position(); 504 | wrapper::Joints* p_v = p_output->mutable_velocity(); 505 | wrapper::Joints* p_a = p_output->mutable_acceleration(); 506 | 507 | const wrapper::Joints& start_rj = (robot ? start_.robot().joints().position() 508 | : start_.external().joints().position()); 509 | const wrapper::Joints& goal_rj = (robot ? goal_.robot().joints().position() 510 | : goal_.external().joints().position()); 511 | 512 | // Calculate the position and velocity outputs. 513 | for (int i = 0; 514 | i < p_p->values_size() && i < p_v->values_size() && i < p_a->values_size() && 515 | i < start_rj.values_size() && i < goal_rj.values_size(); 516 | ++i) 517 | { 518 | p_p->set_values(i, start_rj.values(i) + ramp_factor*(goal_rj.values(i) - start_rj.values(i))); 519 | p_v->set_values(i, d_ramp_factor*(goal_rj.values(i) - start_rj.values(i))); 520 | p_a->set_values(i, 0.0); 521 | } 522 | } 523 | 524 | case RampInVelocity: 525 | { 526 | // Ramp factor that goes from 0.0 to 1.0. 527 | ramp_factor = 0.5*std::cos(M_PI*t + M_PI) + 0.5; 528 | 529 | // Output to set. 530 | wrapper::Joints* p_p = p_output->mutable_position(); 531 | wrapper::Joints* p_v = p_output->mutable_velocity(); 532 | wrapper::Joints* p_a = p_output->mutable_acceleration(); 533 | 534 | const wrapper::Joints& start_rj = (robot ? start_.robot().joints().velocity() 535 | : start_.external().joints().velocity()); 536 | const wrapper::Joints& goal_rj = (robot ? goal_.robot().joints().velocity() 537 | : goal_.external().joints().velocity()); 538 | 539 | // Calculate the position and velocity outputs. 540 | for (int i = 0; 541 | i < p_p->values_size() && i < p_v->values_size() && i < p_a->values_size() && 542 | i < start_rj.values_size() && i < goal_rj.values_size(); 543 | ++i) 544 | { 545 | p_a->set_values(i, 0.0); 546 | p_v->set_values(i, start_rj.values(i) + ramp_factor*(goal_rj.values(i) - start_rj.values(i))); 547 | p_p->set_values(i, p_p->values(i) + sample_time*p_v->values(i)); 548 | } 549 | } 550 | break; 551 | } 552 | } 553 | 554 | void EGMInterpolator::SoftRamp::evaluate(wrapper::trajectory::CartesianGoal* p_output, 555 | const double sample_time, 556 | double t) 557 | { 558 | double ramp_factor = 0.0; 559 | double d_ramp_factor = 0.0; 560 | wrapper::Quaternion previous_q(p_output->mutable_pose()->quaternion()); 561 | 562 | // Saturate t to be within 0.0 and 1.0. 563 | t = saturate(t / duration_, 0.0, 1.0); 564 | 565 | switch (operation_) 566 | { 567 | case RampDown: 568 | { 569 | // Output to set. Note: The Euler field is internally used to contain angular velocities. 570 | wrapper::Quaternion* p_q = p_output->mutable_pose()->mutable_quaternion(); 571 | wrapper::Euler* p_av = p_output->mutable_pose()->mutable_euler(); 572 | 573 | // Ramp factor that goes from 1.0 to 0.0. 574 | ramp_factor = 0.5*std::cos(M_PI*t) + 0.5; 575 | 576 | // Calculate the angular velocity output. 577 | p_av->set_x(ramp_factor*start_angular_velocity_.x()); 578 | p_av->set_y(ramp_factor*start_angular_velocity_.y()); 579 | p_av->set_z(ramp_factor*start_angular_velocity_.z()); 580 | 581 | // Calculate the quaternion derivate. Note: p_output contain the previously calculated quaternion. 582 | wrapper::Quaternion d_q; 583 | convert(&d_q, p_output->mutable_pose()->quaternion(), *p_av); 584 | 585 | // Calculate the quaternion output. 586 | p_q->set_u0(p_q->u0() + sample_time*d_q.u0()); 587 | p_q->set_u1(p_q->u1() + sample_time*d_q.u1()); 588 | p_q->set_u2(p_q->u2() + sample_time*d_q.u2()); 589 | p_q->set_u3(p_q->u3() + sample_time*d_q.u3()); 590 | normalize(p_q); 591 | } 592 | break; 593 | 594 | case RampInPosition: 595 | { 596 | // Output to set. Note: The Euler field is internally used to contain angular velocities. 597 | wrapper::Cartesian* p_p = p_output->mutable_pose()->mutable_position(); 598 | wrapper::Cartesian* p_v = p_output->mutable_velocity(); 599 | wrapper::Cartesian* p_a = p_output->mutable_acceleration(); 600 | wrapper::Quaternion* p_q = p_output->mutable_pose()->mutable_quaternion(); 601 | wrapper::Euler* p_av = p_output->mutable_pose()->mutable_euler(); 602 | 603 | // Ramp factor that goes from 0.0 to 1.0 and its derivate. 604 | ramp_factor = 0.5*std::cos(M_PI*t + M_PI) + 0.5; 605 | d_ramp_factor = -0.5*M_PI / duration_*std::sin(M_PI*t + M_PI); 606 | 607 | const wrapper::Cartesian& start_p = start_.robot().cartesian().pose().position(); 608 | const wrapper::Cartesian& goal_p = goal_.robot().cartesian().pose().position(); 609 | 610 | // Calculate the position and velocity outputs. 611 | p_p->set_x(start_p.x() + ramp_factor*(goal_p.x() - start_p.x())); 612 | p_p->set_y(start_p.y() + ramp_factor*(goal_p.y() - start_p.y())); 613 | p_p->set_z(start_p.z() + ramp_factor*(goal_p.z() - start_p.z())); 614 | p_v->set_x(d_ramp_factor*(goal_p.x() - start_p.x())); 615 | p_v->set_y(d_ramp_factor*(goal_p.y() - start_p.y())); 616 | p_v->set_z(d_ramp_factor*(goal_p.z() - start_p.z())); 617 | p_a->set_x(0.0); 618 | p_a->set_y(0.0); 619 | p_a->set_z(0.0); 620 | 621 | const wrapper::Quaternion& start_q = start_.robot().cartesian().pose().quaternion(); 622 | const wrapper::Quaternion& goal_q = goal_.robot().cartesian().pose().quaternion(); 623 | 624 | // Calculate the quaternion and angular velocity outputs. 625 | p_q->set_u0(start_q.u0() + ramp_factor*(goal_q.u0() - start_q.u0())); 626 | p_q->set_u1(start_q.u1() + ramp_factor*(goal_q.u1() - start_q.u1())); 627 | p_q->set_u2(start_q.u2() + ramp_factor*(goal_q.u2() - start_q.u2())); 628 | p_q->set_u3(start_q.u3() + ramp_factor*(goal_q.u3() - start_q.u3())); 629 | 630 | normalize(p_q); 631 | 632 | estimateVelocities(p_av, *p_q, previous_q, sample_time); 633 | } 634 | 635 | case RampInVelocity: 636 | { 637 | // Output to set. Note: The Euler field is internally used to contain angular velocities. 638 | wrapper::Cartesian* p_p = p_output->mutable_pose()->mutable_position(); 639 | wrapper::Cartesian* p_v = p_output->mutable_velocity(); 640 | wrapper::Cartesian* p_a = p_output->mutable_acceleration(); 641 | wrapper::Quaternion* p_q = p_output->mutable_pose()->mutable_quaternion(); 642 | wrapper::Euler* p_av = p_output->mutable_pose()->mutable_euler(); 643 | 644 | // Ramp factor that goes from 0.0 to 1.0. 645 | ramp_factor = 0.5*std::cos(M_PI*t + M_PI) + 0.5; 646 | 647 | const wrapper::Cartesian& start_v = start_.robot().cartesian().velocity(); 648 | const wrapper::Cartesian& goal_v = goal_.robot().cartesian().velocity(); 649 | 650 | // Calculate the position and velocity outputs. 651 | p_a->set_x(0.0); 652 | p_a->set_y(0.0); 653 | p_a->set_z(0.0); 654 | p_v->set_x(start_v.x() + ramp_factor*(goal_v.x() - start_v.x())); 655 | p_v->set_y(start_v.y() + ramp_factor*(goal_v.y() - start_v.y())); 656 | p_v->set_z(start_v.z() + ramp_factor*(goal_v.z() - start_v.z())); 657 | p_p->set_x(p_p->x() + sample_time*p_v->x()); 658 | p_p->set_y(p_p->y() + sample_time*p_v->y()); 659 | p_p->set_z(p_p->z() + sample_time*p_v->z()); 660 | 661 | // Note: The Euler field is internally used to contain angular velocities. 662 | const wrapper::Euler& start_av = start_.robot().cartesian().pose().euler(); 663 | const wrapper::Euler& goal_av = goal_.robot().cartesian().pose().euler(); 664 | 665 | // Calculate the quaternion and angular velocity outputs. 666 | p_av->set_x(start_av.x() + ramp_factor*(goal_av.x() - start_av.x())); 667 | p_av->set_y(start_av.y() + ramp_factor*(goal_av.y() - start_av.y())); 668 | p_av->set_z(start_av.z() + ramp_factor*(goal_av.z() - start_av.z())); 669 | 670 | wrapper::Quaternion d_q; 671 | convert(&d_q, previous_q, *p_av); 672 | 673 | p_q->set_u0(p_q->u0() + sample_time*d_q.u0()); 674 | p_q->set_u1(p_q->u1() + sample_time*d_q.u1()); 675 | p_q->set_u2(p_q->u2() + sample_time*d_q.u2()); 676 | p_q->set_u3(p_q->u3() + sample_time*d_q.u3()); 677 | normalize(p_q); 678 | } 679 | break; 680 | } 681 | } 682 | 683 | 684 | 685 | 686 | /*********************************************************************************************************************** 687 | * Class definitions: EGMInterpolator 688 | */ 689 | 690 | /************************************************************ 691 | * Primary methods 692 | */ 693 | 694 | void EGMInterpolator::update(const wrapper::trajectory::PointGoal& start, 695 | const wrapper::trajectory::PointGoal& goal, 696 | const Conditions& conditions) 697 | { 698 | conditions_ = conditions; 699 | conditions_.duration = std::max(Constants::RobotController::LOWEST_SAMPLE_TIME, conditions_.duration); 700 | 701 | switch (conditions_.operation) 702 | { 703 | case Normal: 704 | case RampDown: 705 | { 706 | offset_ = start.robot().joints().position().values_size(); 707 | SplineConditions spline_conditions(conditions_); 708 | 709 | switch (conditions_.mode) 710 | { 711 | case EGMJoint: 712 | { 713 | // Robot joints. 714 | for (int i = 0; i < start.robot().joints().position().values_size() && i < spline_polynomials_.size(); ++i) 715 | { 716 | spline_conditions.setConditions(i, start.robot().joints(), goal.robot().joints()); 717 | spline_polynomials_[i].update(spline_conditions); 718 | } 719 | 720 | // External joints. 721 | for (int i = 0; i < start.external().joints().position().values_size() && i < spline_polynomials_.size(); ++i) 722 | { 723 | spline_conditions.setConditions(i, start.external().joints(), goal.external().joints()); 724 | spline_polynomials_[i + offset_].update(spline_conditions); 725 | } 726 | } 727 | break; 728 | 729 | case EGMPose: 730 | { 731 | // X, Y and Z. 732 | spline_conditions.setConditions(X, start.robot().cartesian(), goal.robot().cartesian()); 733 | spline_polynomials_[X].update(spline_conditions); 734 | spline_conditions.setConditions(Y, start.robot().cartesian(), goal.robot().cartesian()); 735 | spline_polynomials_[Y].update(spline_conditions); 736 | spline_conditions.setConditions(Z, start.robot().cartesian(), goal.robot().cartesian()); 737 | spline_polynomials_[Z].update(spline_conditions); 738 | 739 | // Orientation. 740 | if (conditions_.operation == Normal) 741 | { 742 | slerp_.update(start.robot().cartesian().pose().quaternion(), 743 | goal.robot().cartesian().pose().quaternion(), conditions); 744 | } 745 | else 746 | { 747 | soft_ramp_.update(start, goal, conditions); 748 | } 749 | 750 | // External joints. 751 | for (int i = 0; i < start.external().joints().position().values_size() && i < spline_polynomials_.size(); ++i) 752 | { 753 | spline_conditions.setConditions(i, start.external().joints(), goal.external().joints()); 754 | spline_polynomials_[i + offset_].update(spline_conditions); 755 | } 756 | } 757 | break; 758 | } 759 | } 760 | break; 761 | 762 | case RampInPosition: 763 | case RampInVelocity: 764 | { 765 | soft_ramp_.update(start, goal, conditions); 766 | } 767 | break; 768 | } 769 | } 770 | 771 | void EGMInterpolator::evaluate(wrapper::trajectory::PointGoal* p_output, const double sample_time, double t) 772 | { 773 | t = saturate(t, 0.0, conditions_.duration); 774 | 775 | switch (conditions_.operation) 776 | { 777 | case Normal: 778 | case RampDown: 779 | { 780 | switch (conditions_.mode) 781 | { 782 | case EGMJoint: 783 | { 784 | // Robot joints. 785 | for (int i = 0; 786 | i < p_output->robot().joints().position().values_size() && i < spline_polynomials_.size(); ++i) 787 | { 788 | spline_polynomials_[i].evaluate(p_output->mutable_robot()->mutable_joints(), i, t); 789 | } 790 | 791 | // External joints. 792 | for (int i = 0; 793 | i < p_output->external().joints().position().values_size() && i < spline_polynomials_.size(); ++i) 794 | { 795 | spline_polynomials_[i + offset_].evaluate(p_output->mutable_external()->mutable_joints(), i, t); 796 | } 797 | } 798 | break; 799 | 800 | case EGMPose: 801 | { 802 | // X, Y and Z. 803 | spline_polynomials_[X].evaluate(p_output->mutable_robot()->mutable_cartesian(), X, t); 804 | spline_polynomials_[Y].evaluate(p_output->mutable_robot()->mutable_cartesian(), Y, t); 805 | spline_polynomials_[Z].evaluate(p_output->mutable_robot()->mutable_cartesian(), Z, t); 806 | 807 | // Orientation. 808 | if (conditions_.operation == Normal) 809 | { 810 | slerp_.evaluate(p_output->mutable_robot()->mutable_cartesian(), t); 811 | } 812 | else 813 | { 814 | soft_ramp_.evaluate(p_output->mutable_robot()->mutable_cartesian(), sample_time, t); 815 | } 816 | 817 | // External joints. 818 | for (int i = 0; 819 | i < p_output->external().joints().position().values_size() && i < spline_polynomials_.size(); ++i) 820 | { 821 | spline_polynomials_[i + offset_].evaluate(p_output->mutable_external()->mutable_joints(), i, t); 822 | } 823 | } 824 | break; 825 | } 826 | } 827 | break; 828 | 829 | case RampInPosition: 830 | case RampInVelocity: 831 | { 832 | switch (conditions_.mode) 833 | { 834 | case EGMJoint: 835 | { 836 | // Robot joints. 837 | soft_ramp_.evaluate(p_output->mutable_robot()->mutable_joints(), true, sample_time, t); 838 | 839 | // External joints. 840 | soft_ramp_.evaluate(p_output->mutable_external()->mutable_joints(), false, sample_time, t); 841 | } 842 | break; 843 | 844 | case EGMPose: 845 | { 846 | // Cartesian pose. 847 | soft_ramp_.evaluate(p_output->mutable_robot()->mutable_cartesian(), sample_time, t); 848 | 849 | // External joints. 850 | soft_ramp_.evaluate(p_output->mutable_external()->mutable_joints(), false, sample_time, t); 851 | } 852 | break; 853 | } 854 | } 855 | break; 856 | } 857 | } 858 | 859 | } // end namespace egm 860 | } // end namespace abb 861 | -------------------------------------------------------------------------------- /src/egm_logger.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************************************** 2 | * 3 | * Copyright (c) 2015, ABB Schweiz AG 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with 7 | * or without modification, are permitted provided that 8 | * the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the 11 | * above copyright notice, this list of conditions 12 | * and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the 14 | * above copyright notice, this list of conditions 15 | * and the following disclaimer in the documentation 16 | * and/or other materials provided with the 17 | * distribution. 18 | * * Neither the name of ABB nor the names of its 19 | * contributors may be used to endorse or promote 20 | * products derived from this software without 21 | * specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 32 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *********************************************************************************************************************** 35 | */ 36 | 37 | #include 38 | 39 | #include "abb_libegm/egm_common.h" 40 | #include "abb_libegm/egm_logger.h" 41 | 42 | namespace abb 43 | { 44 | namespace egm 45 | { 46 | /*********************************************************************************************************************** 47 | * Class definitions: EGMLogger 48 | */ 49 | 50 | /************************************************************ 51 | * Primary methods 52 | */ 53 | 54 | EGMLogger::EGMLogger(const std::string& filename, const bool use_default_headers) 55 | : 56 | number_of_logged_messages_(0) 57 | { 58 | log_stream_.open(filename.c_str(), std::ios::trunc); 59 | 60 | if (use_default_headers) 61 | { 62 | std::stringstream ss; 63 | ss << "TIMESTAMP," 64 | // Robot feedback. 65 | << "R_FB_POS_RJ1,R_FB_POS_RJ2,R_FB_POS_RJ3,R_FB_POS_RJ4,R_FB_POS_RJ5,R_FB_POS_RJ6," 66 | << "R_FB_POS_EJ1,R_FB_POS_EJ2,R_FB_POS_EJ3,R_FB_POS_EJ4,R_FB_POS_EJ5,R_FB_POS_EJ6," 67 | << "R_FB_VEL_RJ1,R_FB_VEL_RJ2,R_FB_VEL_RJ3,R_FB_VEL_RJ4,R_FB_VEL_RJ5,R_FB_VEL_RJ6," 68 | << "R_FB_VEL_EJ1,R_FB_VEL_EJ2,R_FB_VEL_EJ3,R_FB_VEL_EJ4,R_FB_VEL_EJ5,R_FB_VEL_EJ6," 69 | << "R_FB_POS_X,R_FB_POS_Y,R_FB_POS_Z," 70 | << "R_FB_EULER_X,R_FB_EULER_Y,R_FB_EULER_Z," 71 | << "R_FB_QUAT_U0,R_FB_QUAT_U1,R_FB_QUAT_U2,R_FB_QUAT_U3," 72 | << "R_FB_VEL_LX,R_FB_VEL_LY,R_FB_VEL_LZ," 73 | << "R_FB_VEL_AX,R_FB_VEL_AY,R_FB_VEL_AZ," 74 | // Robot planned. 75 | << "R_PL_POS_RJ1,R_PL_POS_RJ2,R_PL_POS_RJ3,R_PL_POS_RJ4,R_PL_POS_RJ5,R_PL_POS_RJ6," 76 | << "R_PL_POS_EJ1,R_PL_POS_EJ2,R_PL_POS_EJ3,R_PL_POS_EJ4,R_PL_POS_EJ5,R_PL_POS_EJ6," 77 | << "R_PL_VEL_RJ1,R_PL_VEL_RJ2,R_PL_VEL_RJ3,R_PL_VEL_RJ4,R_PL_VEL_RJ5,R_PL_VEL_RJ6," 78 | << "R_PL_VEL_EJ1,R_PL_VEL_EJ2,R_PL_VEL_EJ3,R_PL_VEL_EJ4,R_PL_VEL_EJ5,R_PL_VEL_EJ6," 79 | << "R_PL_POS_X,R_PL_POS_Y,R_PL_POS_Z," 80 | << "R_PL_EULER_X,R_PL_EULER_Y,R_PL_EULER_Z," 81 | << "R_PL_QUAT_U0,R_PL_QUAT_U1,R_PL_QUAT_U2,R_PL_QUAT_U3," 82 | << "R_PL_VEL_LX,R_PL_VEL_LY,R_PL_VEL_LZ," 83 | << "R_PL_VEL_AX,R_PL_VEL_AY,R_PL_VEL_AZ," 84 | // Sensor references. 85 | << "S_REF_POS_RJ1,S_REF_POS_RJ2,S_REF_POS_RJ3,S_REF_POS_RJ4,S_REF_POS_RJ5,S_REF_POS_RJ6," 86 | << "S_REF_POS_EJ1,S_REF_POS_EJ2,S_REF_POS_EJ3,S_REF_POS_EJ4,S_REF_POS_EJ5,S_REF_POS_EJ6," 87 | << "S_REF_VEL_RJ1,S_REF_VEL_RJ2,S_REF_VEL_RJ3,S_REF_VEL_RJ4,S_REF_VEL_RJ5,S_REF_VEL_RJ6," 88 | << "S_REF_VEL_EJ1,S_REF_VEL_EJ2,S_REF_VEL_EJ3,S_REF_VEL_EJ4,S_REF_VEL_EJ5,S_REF_VEL_EJ6," 89 | << "S_REF_POS_X,S_REF_POS_Y,S_REF_POS_Z," 90 | << "S_REF_EULER_X,S_REF_EULER_Y,S_REF_EULER_Z," 91 | << "S_REF_QUAT_U0,S_REF_QUAT_U1,S_REF_QUAT_U2,S_REF_QUAT_U3," 92 | << "S_REF_VEL_LX,S_REF_VEL_LY,S_REF_VEL_LZ," 93 | << "S_REF_VEL_AX,S_REF_VEL_AY,S_REF_VEL_AZ\n"; 94 | 95 | log_stream_ << ss.str(); 96 | log_stream_.flush(); 97 | } 98 | } 99 | 100 | EGMLogger::~EGMLogger() 101 | { 102 | log_stream_.close(); 103 | } 104 | 105 | void EGMLogger::flush() 106 | { 107 | log_stream_ << "\n"; 108 | log_stream_.flush(); 109 | ++number_of_logged_messages_; 110 | } 111 | 112 | void EGMLogger::add(const wrapper::Header& header) 113 | { 114 | log_stream_ << header.time_stamp() << ","; 115 | } 116 | 117 | void EGMLogger::add(const wrapper::Joints& robot, const wrapper::Joints& external) 118 | { 119 | google::protobuf::RepeatedField::const_iterator i; 120 | 121 | for (i = robot.values().begin(); i != robot.values().end(); ++i) 122 | { 123 | log_stream_ << *i << ","; 124 | } 125 | addMockJoints(true, robot.values_size(), external.values_size()); 126 | 127 | for (i = external.values().begin(); i != external.values().end(); ++i) 128 | { 129 | log_stream_ << *i << ","; 130 | } 131 | addMockJoints(false, robot.values_size(), external.values_size()); 132 | } 133 | 134 | void EGMLogger::add(const wrapper::CartesianPose& pose) 135 | { 136 | log_stream_ << pose.position().x() << "," 137 | << pose.position().y() << "," 138 | << pose.position().z() << ","; 139 | 140 | log_stream_ << pose.euler().x() << "," 141 | << pose.euler().y() << "," 142 | << pose.euler().z() << ","; 143 | 144 | log_stream_ << pose.quaternion().u0() << "," 145 | << pose.quaternion().u1() << "," 146 | << pose.quaternion().u2() << "," 147 | << pose.quaternion().u3() << ","; 148 | } 149 | 150 | void EGMLogger::add(const wrapper::CartesianVelocity& velocity, const bool last) 151 | { 152 | log_stream_ << velocity.linear().x() << "," 153 | << velocity.linear().y() << "," 154 | << velocity.linear().z() << ","; 155 | 156 | log_stream_ << velocity.angular().x() << "," 157 | << velocity.angular().y() << "," 158 | << velocity.angular().z() << (last ? "" : ","); 159 | } 160 | 161 | /************************************************************ 162 | * Auxiliary methods 163 | */ 164 | 165 | void EGMLogger::addMockJoints(const bool robot, const size_t robot_size, const size_t external_size) 166 | { 167 | if (robot) 168 | { 169 | // Add mock values for the missing robot joint data. 170 | size_t condition = Constants::RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS; 171 | for (size_t i = robot_size; i < condition; ++i) 172 | { 173 | log_stream_ << 0.0 << ","; 174 | } 175 | } 176 | else 177 | { 178 | // Add mock values for the missing external joint data. 179 | size_t condition = Constants::RobotController::MAX_NUMBER_OF_JOINTS - robot_size; 180 | for (size_t i = external_size; i < condition; ++i) 181 | { 182 | log_stream_ << 0.0 << ","; 183 | } 184 | } 185 | } 186 | 187 | double EGMLogger::calculateTimeLogged(const double sample_time) 188 | { 189 | return (double)number_of_logged_messages_*sample_time; 190 | } 191 | 192 | } // end namespace egm 193 | } // end namespace abb 194 | -------------------------------------------------------------------------------- /src/egm_udp_server.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************************************** 2 | * 3 | * Copyright (c) 2015, ABB Schweiz AG 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with 7 | * or without modification, are permitted provided that 8 | * the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the 11 | * above copyright notice, this list of conditions 12 | * and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the 14 | * above copyright notice, this list of conditions 15 | * and the following disclaimer in the documentation 16 | * and/or other materials provided with the 17 | * distribution. 18 | * * Neither the name of ABB nor the names of its 19 | * contributors may be used to endorse or promote 20 | * products derived from this software without 21 | * specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 32 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *********************************************************************************************************************** 35 | */ 36 | 37 | #include 38 | 39 | #include "abb_libegm/egm_udp_server.h" 40 | 41 | namespace abb 42 | { 43 | namespace egm 44 | { 45 | /*********************************************************************************************************************** 46 | * Class definitions: UDPServer 47 | */ 48 | 49 | UDPServer::UDPServer(boost::asio::io_service& io_service, 50 | unsigned short port_number, 51 | AbstractUDPServerInterface* p_interface) 52 | : 53 | initialized_(false), 54 | p_interface_(p_interface) 55 | { 56 | bool success = true; 57 | 58 | try 59 | { 60 | server_data_.port_number = port_number; 61 | p_socket_.reset(new boost::asio::ip::udp::socket(io_service, 62 | boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), 63 | port_number))); 64 | } 65 | catch (std::exception e) 66 | { 67 | success = false; 68 | } 69 | 70 | if (success) 71 | { 72 | initialized_ = true; 73 | startAsynchronousReceive(); 74 | } 75 | } 76 | 77 | UDPServer::~UDPServer() 78 | { 79 | if (p_socket_) 80 | { 81 | p_socket_->close(); 82 | p_socket_.reset(); 83 | } 84 | } 85 | 86 | bool UDPServer::isInitialized() const 87 | { 88 | return initialized_; 89 | } 90 | 91 | void UDPServer::startAsynchronousReceive() 92 | { 93 | if (p_socket_) 94 | { 95 | p_socket_->async_receive_from(boost::asio::buffer(receive_buffer_), 96 | remote_endpoint_, 97 | boost::bind(&UDPServer::receiveCallback, 98 | this, 99 | boost::asio::placeholders::error, 100 | boost::asio::placeholders::bytes_transferred)); 101 | } 102 | } 103 | 104 | void UDPServer::receiveCallback(const boost::system::error_code& error, const std::size_t bytes_transferred) 105 | { 106 | server_data_.p_data = receive_buffer_; 107 | server_data_.bytes_transferred = (int) bytes_transferred; 108 | 109 | if (error == boost::system::errc::success && p_interface_) 110 | { 111 | // Process the received data via the callback method (creates the reply message). 112 | const std::string& reply = p_interface_->callback(server_data_); 113 | 114 | if (!reply.empty() && p_socket_) 115 | { 116 | // Send the response message to the robot controller. 117 | p_socket_->async_send_to(boost::asio::buffer(reply), 118 | remote_endpoint_, 119 | boost::bind(&UDPServer::sendCallback, 120 | this, 121 | boost::asio::placeholders::error, 122 | boost::asio::placeholders::bytes_transferred)); 123 | } 124 | } 125 | 126 | // Add another asynchronous operation to the boost io_service object. 127 | startAsynchronousReceive(); 128 | } 129 | 130 | void UDPServer::sendCallback(const boost::system::error_code& error, const std::size_t bytes_transferred) {} 131 | 132 | } // end namespace egm 133 | } // end namespace abb 134 | --------------------------------------------------------------------------------