diff --git a/.github/workflows/build_ubuntu.yml b/.github/workflows/build_ubuntu.yml new file mode 100644 index 0000000..a3caf6a --- /dev/null +++ b/.github/workflows/build_ubuntu.yml @@ -0,0 +1,75 @@ +name: Build Ubuntu + +on: + push: + branches: + - master + - develop + workflow_dispatch: + +env: + BUILD_TYPE: Release + +jobs: + build: + strategy: + matrix: + os: [ ubuntu-22.04, ubuntu-20.04 ] + python-version: [ '3.8', '3.9', '3.10', '3.11' ] + + runs-on: ${{ matrix.os }} + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Setup Python + uses: actions/setup-python@v5 + with: + python-version: ${{ matrix.python-version }} + + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y \ + build-essential \ + cmake \ + libeigen3-dev \ + python3 \ + libpython3-dev \ + python3-pip \ + python3-setuptools + pip install --upgrade pip + pip install patchelf + pip install wheel + pip install auditwheel + + - name: Configure CMake + run: | + cmake -B build \ + -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} \ + -DROMOCC_BUILD_TESTS=ON \ + -DROMOCC_BUILD_EXAMPLES=ON \ + -DROMOCC_BUILD_PYTHON_BINDINGS=ON \ + + - name: Build libromocc + run: cmake --build build --config ${{env.BUILD_TYPE}} -j 4 + + - name: Build pyromocc python wheel + run: cmake --build build --config ${{env.BUILD_TYPE}} --target create_python_wheel + + - name: Determine platform + id: determine_platform + run: | + echo platform=$(python3 -c "import distutils.util; print(distutils.util.get_platform().replace('-', '_').replace('.', '_'))") >> $GITHUB_OUTPUT + + - name: Auditwheel repair + run: | + auditwheel repair ${{github.workspace}}/build/pyromocc/dist/pyromocc-*.whl --plat ${{ steps.determine_platform.outputs.platform }} -w ${{github.workspace}}/build/pyromocc/dist/wheelhouse + + - name: Upload Python wheel + uses: actions/upload-artifact@v4 + with: + name: Python wheel + path: ${{github.workspace}}/build/pyromocc/dist/wheelhouse/pyromocc-*.whl + if-no-files-found: error \ No newline at end of file diff --git a/.github/workflows/build_windows.yml b/.github/workflows/build_windows.yml new file mode 100644 index 0000000..bceecc5 --- /dev/null +++ b/.github/workflows/build_windows.yml @@ -0,0 +1,54 @@ +name: Build Windows + +on: + push: + branches: + - master + - develop + workflow_dispatch: + +env: + BUILD_TYPE: Release + +jobs: + build: + runs-on: windows-2019 + + strategy: + matrix: + python-version: [ '3.8', '3.9', '3.10', '3.11' ] + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Setup Python + uses: actions/setup-python@v5 + with: + python-version: ${{ matrix.python-version }} + + - name: Install dependencies + run: | + pip install --upgrade pip + pip install wheel + + - name: Configure CMake + run: | + cmake ${{github.workspace}} -B ${{github.workspace}}/build ` + -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ` + -DROMOCC_BUILD_TESTS=OFF ` + -DROMOCC_BUILD_EXAMPLES=OFF ` + -DROMOCC_BUILD_PYTHON_BINDINGS=ON ` + + - name: Build libromocc + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} -j 4 + + - name: Build pyromocc python wheel + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} --target create_python_wheel + + - name: Upload Python wheel + uses: actions/upload-artifact@v4 + with: + name: Python wheel + path: ${{github.workspace}}/build/pyromocc/dist/pyromocc-*.whl + if-no-files-found: error \ No newline at end of file diff --git a/.github/workflows/cmake_build.yml b/.github/workflows/cmake_build.yml deleted file mode 100644 index 7ace459..0000000 --- a/.github/workflows/cmake_build.yml +++ /dev/null @@ -1,21 +0,0 @@ -name: CMake build - -on: - push: - branches: [ master, develop ] - pull_request: - branches: [ master, develop ] - -jobs: - build: - - runs-on: ubuntu-18.04 - - steps: - - uses: actions/checkout@v1 - - name: configure - run: mkdir build && cd build && cmake .. - - name: make - run: cd build && make - - name: test - run: cd build/bin && ./testROMOCC diff --git a/CMakeLists.txt b/CMakeLists.txt index d636849..1e39dfe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ project(romocc) # Current version set (VERSION_MAJOR 0) set (VERSION_MINOR 0) -set (VERSION_PATCH 3) +set (VERSION_PATCH 7) set(VERSION ${VERSION_MAJOR}.${VERSION_MINOR}.${VERSION_PATCH}) include(cmake/Macros.cmake) diff --git a/README.md b/README.md index a6ce121..2829b71 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,47 @@ libromocc ========= -[![Build badge](https://github.com/SINTEFMedtek/libromocc/workflows/cmake_build.yml/badge.svg?branch=master&event=push)](https://github.com/SINTEFMedtek/libromocc/actions) +[![Build Ubuntu](https://github.com/SINTEFMedtek/libromocc/actions/workflows/build_ubuntu.yml/badge.svg)](https://github.com/SINTEFMedtek/libromocc/actions/workflows/build_ubuntu.yml) +[![Build Windows](https://github.com/SINTEFMedtek/libromocc/actions/workflows/build_windows.yml/badge.svg)](https://github.com/SINTEFMedtek/libromocc/actions/workflows/build_windows.yml) libromocc is a lightweight C++ library for **ro**bot **mo**delling, **c**ontrol and **c**ommunication. It also contains Python wrappers with a corresponding PyPI distribution. -### Setup and build +### Core features ### +* **Lightweight inferface**: Simple control of robots from Universal Robots. +* **Robot modelling**: Forward and inverse kinematics, Jacobians, etc. +* **Support for old client interfaces**: Supports the real-time interfaces of Universal Robots, but not the new RTDE interface. +* **Python wrappers**: Python wrappers for core functionality. + +### Getting started with Python ### +[![pypi](https://badgen.net/pypi/v/pyromocc)](https://pypi.org/project/pyromocc/) + +Pre-built packages are available for Linux and Windows on PyPI. Supports Ubuntu 20.04, 22.04 and Windows 10+(64-bit). +To install the current release: + +```bash +pip install pyromocc +``` + +Once installed, the following shows a simple sample of use: +```python +from pyromocc import Robot + +# Connect to robot +robot = Robot(ip="192.168.153.131", port=30003, manipulator="UR5") +robot.connect() + +# Print current operational configuration +print(robot.operational_config) + +# Move 50 mm upwards +robot.z += 50 + +# Print current operational configuration +print(robot.operational_config) +``` + +### Setup and build ### ```bash git clone https://github.com/SINTEFMedtek/libromocc.git @@ -17,7 +52,7 @@ cmake .. make -j8 ``` -### Usage +### Usage ### If you use libromocc in your research and would like to cite the library, we suggest you cite the following conference paper: @@ -37,6 +72,6 @@ use under a BSD-2 license. See included licence for more information. The code base is currently undergoing large changes, thus there is no guarantee that internal interfaces will be stable. -### Contributors +### Contributors ### libromocc is developed at [SINTEF Digital](http://www.sintef.no), and the work has been partially funded by the Research Council of Norway under grant number 270941. diff --git a/cmake/ExternalCxxOpts.cmake b/cmake/ExternalCxxOpts.cmake new file mode 100644 index 0000000..760f8d0 --- /dev/null +++ b/cmake/ExternalCxxOpts.cmake @@ -0,0 +1,19 @@ +# Download and set up Eigen + +include(cmake/Externals.cmake) + +ExternalProject_Add(cxxopts + PREFIX ${ROMOCC_EXTERNAL_BUILD_DIR}/cxxopts + BINARY_DIR ${ROMOCC_EXTERNAL_BUILD_DIR}/cxxopts/build + URL "https://github.com/jarro2783/cxxopts/archive/refs/tags/v3.1.1.tar.gz" + INSTALL_DIR ${ROMOCC_EXTERNAL_INSTALL_DIR} + CMAKE_CACHE_ARGS + -DCMAKE_BUILD_TYPE:STRING=Release + -DCMAKE_VERBOSE_MAKEFILE:BOOL=OFF + -DCMAKE_INSTALL_MESSAGE:BOOL=LAZY + -DCMAKE_INSTALL_PREFIX:STRING=${ROMOCC_EXTERNAL_INSTALL_DIR} + -DBUILD_TESTING:BOOL=OFF + ) + +list(APPEND ROMOCC_INCLUDE_DIRS ${ROMOCC_EXTERNAL_INSTALL_DIR}/include/cxxopts/) +list(APPEND ROMOCC_EXTERNAL_DEPENDENCIES cxxopts) \ No newline at end of file diff --git a/cmake/ExternalPybind.cmake b/cmake/ExternalPybind.cmake index 511aa46..dfec1e6 100644 --- a/cmake/ExternalPybind.cmake +++ b/cmake/ExternalPybind.cmake @@ -10,7 +10,7 @@ ExternalProject_Add( pybind11 PREFIX . GIT_REPOSITORY "https://github.com/pybind/pybind11.git" - GIT_TAG "v2.6.2" # v2.6.2 + GIT_TAG "v2.11.1" # v2.11.1 SOURCE_DIR "${CMAKE_BINARY_DIR}/third-party/pybind11" # Override default steps with no action, we just want the clone step. CONFIGURE_COMMAND "" diff --git a/cmake/ExternalZeroMQ.cmake b/cmake/ExternalZeroMQ.cmake index bbfa136..65cb022 100644 --- a/cmake/ExternalZeroMQ.cmake +++ b/cmake/ExternalZeroMQ.cmake @@ -1,12 +1,13 @@ # Download and set up ZeroMQ include(cmake/Externals.cmake) +set(ZMQ_VERSION "4.3.2") ExternalProject_Add(zeromq PREFIX ${ROMOCC_EXTERNAL_BUILD_DIR}/zeromq BINARY_DIR ${ROMOCC_EXTERNAL_BUILD_DIR}/zeromq GIT_REPOSITORY "https://github.com/zeromq/libzmq.git" - GIT_TAG "v4.3.2" + GIT_TAG "v${ZMQ_VERSION}" UPDATE_COMMAND "" CMAKE_ARGS -DCMAKE_BUILD_TYPE:STRING=Release @@ -14,12 +15,22 @@ ExternalProject_Add(zeromq -DBUILD_SHARED_LIBS=OFF -DBUILD_STATIC_LIBS=ON -DBUILD_PACKAGING=OFF - -DBUILD_TESTING=OFF + -DBUILD_TESTS=OFF -DBUILD_NC_TESTS=OFF -DBUILD_CONFIG_TESTS=OFF -DINSTALL_HEADERS=ON ) list(APPEND ROMOCC_INCLUDE_DIRS ${ROMOCC_EXTERNAL_INSTALL_DIR}/include/) -list(APPEND LIBRARIES zmq) + +if(WIN32) + string(REPLACE "." "_" ZMQ_VERSION_US ${ZMQ_VERSION}) + set(ZMQ_LIBRARY + "libzmq-${CMAKE_VS_PLATFORM_TOOLSET}-mt-${ZMQ_VERSION_US}" + "libzmq-${CMAKE_VS_PLATFORM_TOOLSET}-mt-s-${ZMQ_VERSION_US}") +else(WIN32) + set(ZMQ_LIBRARY ${CMAKE_SHARED_LIBRARY_PREFIX}zmq${CMAKE_SHARED_LIBRARY_SUFFIX}) +endif(WIN32) + +list(APPEND LIBRARIES ${ZMQ_LIBRARY}) list(APPEND ROMOCC_EXTERNAL_DEPENDENCIES zeromq) \ No newline at end of file diff --git a/cmake/InstallRomocc.cmake b/cmake/InstallRomocc.cmake index 44f4b4a..83a2cb2 100644 --- a/cmake/InstallRomocc.cmake +++ b/cmake/InstallRomocc.cmake @@ -3,20 +3,25 @@ # Install romocc library if(WIN32) # DLL should be in binary folder -install(TARGETS romocc - DESTINATION libromocc/bin -) + install(TARGETS romocc + DESTINATION libromocc/bin + COMPONENT romocc + ) + set(CMAKE_INSTALL_SYSTEM_RUNTIME_DESTINATION libromocc/bin) + include(InstallRequiredSystemLibraries) # Install vcruntime dlls else() -install(TARGETS romocc - DESTINATION libromocc/lib -) + install(TARGETS romocc + DESTINATION libromocc/lib + COMPONENT romocc + ) endif() if(ROMOCC_BUILD_TESTS) # Install test executable install(TARGETS testROMOCC DESTINATION libromocc/bin - ) + COMPONENT romocc + ) endif() # Examples are installed in the macro project_add_example @@ -24,17 +29,18 @@ endif() # Install dependency libraries install(FILES ${PROJECT_BINARY_DIR}/romoccExport.hpp DESTINATION libromocc/include + COMPONENT romocc ) if(WIN32) - file(GLOB DLLs ${PROJECT_BINARY_DIR}/bin/*.dll) - install(FILES ${DLLs} - DESTINATION libromocc/bin - ) - file(GLOB DLLs ${PROJECT_BINARY_DIR}/lib/*.lib) - install(FILES ${DLLs} - DESTINATION libromocc/lib - ) + install(DIRECTORY ${PROJECT_BINARY_DIR}/bin/ + DESTINATION libromocc/bin/ + COMPONENT romocc + FILES_MATCHING PATTERN "*.dll") + install(DIRECTORY ${PROJECT_BINARY_DIR}/lib/ + DESTINATION libromocc/lib/ + COMPONENT romocc + FILES_MATCHING PATTERN "*.lib") elseif(APPLE) file(GLOB SOs ${PROJECT_BINARY_DIR}/lib/*.dylib) install(FILES ${SOs} diff --git a/cmake/Requirements.cmake b/cmake/Requirements.cmake index 7d31efa..3835b39 100644 --- a/cmake/Requirements.cmake +++ b/cmake/Requirements.cmake @@ -1,18 +1,23 @@ # Setup all dependencies, both internal (have to be installed on the system) # and external (downloaded and built automatically) -list(APPEND SYSTEM_LIBRARIES pthread) +if(CMAKE_SYSTEM_NAME STREQUAL Linux) + find_package(Threads REQUIRED) + list(APPEND SYSTEM_LIBRARIES ${CMAKE_THREAD_LIBS_INIT}) +endif() ## External depedencies include(cmake/ExternalEigen.cmake) include(cmake/ExternalOrocos.cmake) include(cmake/ExternalZeroMQ.cmake) +include(cmake/ExternalCxxOpts.cmake) if(ROMOCC_BUILD_URSIMULATOR) include(cmake/ExternalURSimulator.cmake) endif() if(ROMOCC_BUILD_PYTHON_BINDINGS) + find_package(PythonInterp 3 REQUIRED) find_package(PythonLibs 3 REQUIRED) endif() diff --git a/cmake/RomoccConfig.cmake.in b/cmake/RomoccConfig.cmake.in index 4835d42..5bd83c8 100644 --- a/cmake/RomoccConfig.cmake.in +++ b/cmake/RomoccConfig.cmake.in @@ -35,6 +35,11 @@ set(ROMOCC_LIBRARIES zmq ${X11_LIBRARIES} ) + +if(CMAKE_SYSTEM_NAME STREQUAL Linux) + list(APPEND ROMOCC_LIBRARIES pthread) +endif() + set(ROMOCC_USE_FILE "${CMAKE_CURRENT_LIST_DIR}/RomoccUse.cmake") message("----------------------------------------") diff --git a/cmake/RomoccUse.cmake.in b/cmake/RomoccUse.cmake.in index 2d37db1..c3eb411 100644 --- a/cmake/RomoccUse.cmake.in +++ b/cmake/RomoccUse.cmake.in @@ -10,3 +10,24 @@ endif() include_directories(${ROMOCC_INCLUDE_DIRS}) link_directories (${ROMOCC_LIBRARY_DIRS}) + +if(WIN32) + # Copy all DLLs from ROMOCC to current binary folder + file(GLOB DLLs ${ROMOCC_BINARY_DIR}*.dll) + + add_custom_target(romocc_copy + COMMAND ${CMAKE_COMMAND} -E copy ${DLLs} ${PROJECT_BINARY_DIR}/Release/ COMMAND ${CMAKE_COMMAND} -E echo "ROMOCC DLLs copied (Release)." + COMMAND ${CMAKE_COMMAND} -E copy ${DLLs} ${PROJECT_BINARY_DIR}/Debug/ COMMAND ${CMAKE_COMMAND} -E echo "ROMOCC DLLs copied (Debug)." + ) + + # Create romocc config file, windows needs this because the DLLs are copied, + # so we can't get base path from the DLL path + set(FILE_CONTENT " + DocumentationPath = ${ROMOCC_BINARY_DIR}../doc/ + LibraryPath = ${ROMOCC_BINARY_DIR}../bin/ + file(WRITE ${PROJECT_BINARY_DIR}/Release/romocc_configuration.txt ${FILE_CONTENT}) + file(WRITE ${PROJECT_BINARY_DIR}/Debug/romocc_configuration.txt ${FILE_CONTENT}) +else() + # NoOp command + add_custom_target(romocc_copy COMMENT "noop") +endif() \ No newline at end of file diff --git a/examples/RobotExample.cpp b/examples/RobotExample.cpp index 6c282c4..b770432 100644 --- a/examples/RobotExample.cpp +++ b/examples/RobotExample.cpp @@ -5,66 +5,82 @@ #include #include #include +#include #include "romocc/Robot.h" using namespace romocc; +void printUsage(const char* programName) { + std::cout << "Usage: " << programName << " --ip --manipulator --sw_version \n"; +} + int main(int argc, char *argv[]) { - auto print_message = []() - { - std::cout << "State updated" << "\n"; - }; + cxxopts::Options options("Robot move example", "Simple example of moving robot"); - auto ur5 = Robot::New(); - ur5->configure(Manipulator(), "192.168.153.131", 30003); + options.add_options() + ("ip", "IP address", cxxopts::value()->default_value("192.168.231.131")) + ("manipulator", "Manipulator type", cxxopts::value()->default_value("UR5")) + ("sw_version", "Software version", cxxopts::value()->default_value("3.15")) + ("h,help", "Print usage"); - if(ur5->connect()) - { - ur5->addUpdateSubscription(print_message); + auto result = options.parse(argc, argv); + + if (result.count("help")) { + std::cout << options.help() << std::endl; + return 0; + } - auto initialJointConfig = ur5->getCurrentState()->getJointConfig(); + std::string ip = result["ip"].as(); + std::string manipulator = result["manipulator"].as(); + std::string sw_version = result["sw_version"].as(); + + std::cout << "Connecting to " << manipulator << " -- IP: " << ip << " -- SW: " << sw_version << std::endl; + auto robot = Robot::New(); + robot->configure(Manipulator(manipulator, sw_version), ip, 30003); + + if(robot->connect()) + { + auto initialJointConfig = robot->getCurrentState()->getJointConfig(); std::cout << initialJointConfig.transpose() << std::endl; double previousTime = 0; bool stop = false; std::thread processing_thread([&](){ while(!stop){ - Vector6d jointConfig = ur5->getCurrentState()->getJointConfig(); - double currentTime = ur5->getCurrentState()->getTimestamp(); - Transform3d m_bM1 = ur5->getCurrentState()->getTransformToJoint(1); - Transform3d m_bM2 = ur5->getCurrentState()->getTransformToJoint(2); - Transform3d m_bM3 = ur5->getCurrentState()->getTransformToJoint(3); - Transform3d m_bM4 = ur5->getCurrentState()->getTransformToJoint(4); - Transform3d m_bM5 = ur5->getCurrentState()->getTransformToJoint(5); - Transform3d m_bM6 = ur5->getCurrentState()->getTransformToJoint(6); + Vector6d jointConfig = robot->getCurrentState()->getJointConfig(); + Vector6d operationalConfig = robot->getCurrentState()->getOperationalConfig(); + + double currentTime = robot->getCurrentState()->getTimestamp(); if(currentTime > previousTime) { - std::cout << jointConfig.transpose() << std::endl; - previousTime = ur5->getCurrentState()->getTimestamp(); + std::cout << "Joint config: " << TransformUtils::radToDeg(jointConfig).transpose() << std::endl; + std::cout << "Operational config: " << operationalConfig.transpose() << "\n" << std::endl; + previousTime = robot->getCurrentState()->getTimestamp(); } } }); double targetConfig[] = {190,-400, 150, 1.0,-3.0, 0}; - ur5->move(romocc::MotionType::movep, targetConfig, 100, 50); + robot->move(romocc::MotionType::movep, targetConfig, 100, 50); std::this_thread::sleep_for(std::chrono::seconds(3)); - std::cout << ur5->getCurrentState()->getJointConfig().transpose() << std::endl; - ur5->move(romocc::MotionType::movej, initialJointConfig, 50, 25); + std::cout << robot->getCurrentState()->getJointConfig().transpose() << std::endl; + robot->move(romocc::MotionType::movej, initialJointConfig, 50, 25); std::this_thread::sleep_for(std::chrono::seconds(3)); - std::cout << ur5->getCurrentState()->getJointConfig().transpose() << std::endl; + std::cout << robot->getCurrentState()->getJointConfig().transpose() << std::endl; stop = true; processing_thread.join(); - ur5->disconnect(); + + robot->stopMove(MotionType::stopj, 50); + robot->disconnect(); std::this_thread::sleep_for(std::chrono::seconds(1)); - ur5->connect(); - ur5->addUpdateSubscription(print_message); + robot->connect(); std::this_thread::sleep_for(std::chrono::seconds(1)); - ur5->disconnect(); + robot->disconnect(); } } \ No newline at end of file diff --git a/examples/RobotListen.cpp b/examples/RobotListen.cpp index 3419a7a..73cc04d 100644 --- a/examples/RobotListen.cpp +++ b/examples/RobotListen.cpp @@ -1,45 +1,64 @@ #include #include #include +#include #include "romocc/Robot.h" +#include "romocc/utilities/MathUtils.h" using namespace romocc; +void printUsage(const char* programName) { + std::cout << "Usage: " << programName << " --ip --manipulator --sw_version \n"; +} + int main(int argc, char *argv[]) { - auto print_message = []() - { - std::cout << "State updated" << "\n"; - }; + cxxopts::Options options("Robot listener", "Example of listening to robot states"); - auto ur5 = Robot::New(); - ur5->configure(Manipulator(), "192.168.153.131", 30003); + options.add_options() + ("ip", "IP address", cxxopts::value()->default_value("192.168.231.131")) + ("manipulator", "Manipulator type", cxxopts::value()->default_value("UR5")) + ("sw_version", "Software version", cxxopts::value()->default_value("3.15")) + ("h,help", "Print usage"); - if(ur5->connect()) - { - ur5->addUpdateSubscription(print_message); + auto result = options.parse(argc, argv); + + if (result.count("help")) { + std::cout << options.help() << std::endl; + return 0; + } - auto initialJointConfig = ur5->getCurrentState()->getJointConfig(); + std::string ip = result["ip"].as(); + std::string manipulator = result["manipulator"].as(); + std::string sw_version = result["sw_version"].as(); + + std::cout << "Connecting to " << manipulator << " -- IP: " << ip << " -- SW: " << sw_version << std::endl; + auto robot = Robot::New(); + robot->configure(Manipulator(manipulator, sw_version), ip, 30003); + + if(robot->connect()) + { + auto initialJointConfig = robot->getCurrentState()->getJointConfig(); std::cout << initialJointConfig.transpose() << std::endl; double previousTime = 0; bool stop = false; std::thread processing_thread([&](){ while(!stop){ - Vector6d jointConfig = ur5->getCurrentState()->getJointConfig(); - double currentTime = ur5->getCurrentState()->getTimestamp(); - Transform3d m_bM1 = ur5->getCurrentState()->getTransformToJoint(1); - Transform3d m_bM2 = ur5->getCurrentState()->getTransformToJoint(2); - Transform3d m_bM3 = ur5->getCurrentState()->getTransformToJoint(3); - Transform3d m_bM4 = ur5->getCurrentState()->getTransformToJoint(4); - Transform3d m_bM5 = ur5->getCurrentState()->getTransformToJoint(5); - Transform3d m_bM6 = ur5->getCurrentState()->getTransformToJoint(6); + Vector6d jointConfig = robot->getCurrentState()->getJointConfig(); + Vector6d operationalConfig = robot->getCurrentState()->getOperationalConfig(); + Vector6d operationalVelocity = robot->getCurrentState()->getOperationalVelocity(); + Vector6d operationalForce = robot->getCurrentState()->getOperationalForce(); + double currentTime = robot->getCurrentState()->getTimestamp(); if(currentTime > previousTime) { - std::cout << jointConfig.transpose() << std::endl; - previousTime = ur5->getCurrentState()->getTimestamp(); + std::cout << "Joint config: " << TransformUtils::radToDeg(jointConfig).transpose() << std::endl; + std::cout << "Operational config: " << operationalConfig.transpose() << std::endl; + std::cout << "Operational velocity: " << operationalVelocity.transpose() << std::endl; + std::cout << "Operational force: " << operationalForce.transpose() << "\n" << std::endl; + previousTime = robot->getCurrentState()->getTimestamp(); } } }); @@ -49,5 +68,4 @@ int main(int argc, char *argv[]) processing_thread.join(); } - } \ No newline at end of file diff --git a/source/pyromocc/CMakeLists.txt b/source/pyromocc/CMakeLists.txt index 1f6ebb0..798749c 100644 --- a/source/pyromocc/CMakeLists.txt +++ b/source/pyromocc/CMakeLists.txt @@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.10) project(pyromocc) +message("-- Setup python module pyromocc") + set(DEPENDENCIES romocc) configure_file(${CMAKE_SOURCE_DIR}/cmake/ExternalPybind.cmake ${CMAKE_BINARY_DIR}/external/pybind11/CMakeLists.txt) @@ -19,12 +21,23 @@ add_subdirectory(${CMAKE_BINARY_DIR}/third-party/pybind11 ${CMAKE_BINARY_DIR}/th include_directories(${CMAKE_BINARY_DIR}/third-party/pybind11/include) -pybind11_add_module(pyromocc SHARED source/pyromocc.cpp) -target_link_libraries(pyromocc PRIVATE ${DEPENDENCIES}) +pybind11_add_module(pyromocc source/pyromocc.cpp) +target_link_libraries(pyromocc PRIVATE pybind11::module ${DEPENDENCIES}) set_target_properties(pyromocc PROPERTIES VERSION ${VERSION} SOVERSION "${VERSION_MAJOR}.${VERSION_MINOR}.${VERSION_PATCH}") set_target_properties(pyromocc PROPERTIES PREFIX "" LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/pyromocc) install(TARGETS pyromocc EXPORT romocc DESTINATION pyromocc) -message("-- Setup python module pyromocc") \ No newline at end of file +add_custom_target(copy_python_package + COMMAND ${CMAKE_COMMAND} -E copy_directory ${PROJECT_SOURCE_DIR}/pyromocc ${ROMOCC_CMAKE_BINARY_DIR}/pyromocc/pyromocc + COMMAND ${CMAKE_COMMAND} -E copy ${PROJECT_SOURCE_DIR}/setup.py ${ROMOCC_CMAKE_BINARY_DIR}/pyromocc + COMMAND ${CMAKE_COMMAND} -E copy ${PROJECT_SOURCE_DIR}/README.md ${ROMOCC_CMAKE_BINARY_DIR}/pyromocc + COMMENT "Copy python files") + +add_custom_target(create_python_wheel + COMMAND ${PYTHON_EXECUTABLE} setup.py bdist_wheel WORKING_DIRECTORY "${ROMOCC_CMAKE_BINARY_DIR}/pyromocc" + COMMENT "Create python wheel" +) + +add_dependencies(create_python_wheel copy_python_package pyromocc) \ No newline at end of file diff --git a/source/pyromocc/examples/connect_and_listen.py b/source/pyromocc/examples/connect_and_listen.py index 64d3972..ac4fdf9 100644 --- a/source/pyromocc/examples/connect_and_listen.py +++ b/source/pyromocc/examples/connect_and_listen.py @@ -1,16 +1,26 @@ -from pyromocc import Robot +import time import numpy as np -from time import sleep +import argparse + +from pyromocc import Robot + np.set_printoptions(precision=3) -robot = Robot(ip="192.168.153.131", port=30003, manipulator="UR10") -robot.connect() +parser = argparse.ArgumentParser(description='UR robot listener example') +parser.add_argument('--ip', type=str, default="192.168.199.129", help='Robot IP address') +parser.add_argument('--port', type=int, default=30003, help='Robot port number') +parser.add_argument('--manipulator', type=str, default="UR5", help='Manipulator type') +parser.add_argument('--sw_version', type=str, default="3.15", help='Controller software version') -sleep(1.0) -print(robot.joint_config) +args = parser.parse_args() -robot.movej([0.0, -np.pi/2, -np.pi/2, -np.pi/2, 0, 0], 50, 100) +robot = Robot(ip=args.ip, port=args.port, manipulator=args.manipulator, sw_version=args.sw_version) +robot.connect() -sleep(1.0) -print(robot.joint_config) -print(robot.pose) +t0 = time.time() +while time.time()-t0 < 5: + print(f"Joint config: {robot.joint_config}") + print(f"Joint velocity: {robot.joint_velocity}") + print(f"Operational config {robot.operational_config}") + print(f"Operational velocity {robot.operational_velocity}") + print(f"Operational force {robot.operational_force} \n") diff --git a/source/pyromocc/pyromocc/__init__.py b/source/pyromocc/pyromocc/__init__.py index 97fa65b..e57b9e9 100644 --- a/source/pyromocc/pyromocc/__init__.py +++ b/source/pyromocc/pyromocc/__init__.py @@ -1,3 +1,2 @@ -# python libs (pyd/so) should be copied to this folder from .pyromocc import * -from .robot import Robot \ No newline at end of file +from .robot import Robot diff --git a/source/pyromocc/pyromocc/robot.py b/source/pyromocc/pyromocc/robot.py index d0348eb..bb89b12 100644 --- a/source/pyromocc/pyromocc/robot.py +++ b/source/pyromocc/pyromocc/robot.py @@ -1,6 +1,8 @@ from .pyromocc import * from typing import Union +import numpy as np + class Robot(RobotBase): """ Robot class @@ -24,23 +26,37 @@ class Robot(RobotBase): A list of 6 values representing the axis-angle representation and translation of the end-effector wrt base """ - def __init__(self, ip: str, port: int = 30003, manipulator: str = None, units="mm", **kwargs): + def __init__(self, ip: str, port: int = 30003, manipulator: str = None, units="mm", sw_version="3.15"): RobotBase.__init__(self) self.ip = ip self.port = port - self.sw_version = kwargs.get("sw_version", "5.3") + self.sw_version = sw_version self.units = units # default unit is mm (millimetre) - if manipulator is None or manipulator == 'UR5': - self.manipulator = Manipulator(ManipulatorType.UR5, self.sw_version) - elif manipulator == 'UR10': - self.manipulator = Manipulator(ManipulatorType.UR10, self.sw_version) - else: - raise ValueError("Manipulator of type {} not supported.".format(manipulator)) + if manipulator is None: + manipulator = "UR5" + manipulator_type = self._string_to_manipulator_type(manipulator) + + self.manipulator = Manipulator(manipulator_type, self.sw_version) self.configure(self.manipulator, self.ip, self.port) - def move_to_pose(self, pose, acceleration, velocity): + @staticmethod + def _string_to_manipulator_type(manipulator): + if manipulator == 'UR3': + return ManipulatorType.UR3 + elif manipulator == 'UR3e': + return ManipulatorType.UR3e + elif manipulator == 'UR5': + return ManipulatorType.UR5 + elif manipulator == 'UR5e': + return ManipulatorType.UR5e + elif manipulator == 'UR10': + return ManipulatorType.UR10 + elif manipulator == 'UR10e': + return ManipulatorType.UR10e + + def move_to_pose(self, pose, acceleration, velocity, wait=False): """ Parameters ---------- @@ -50,19 +66,40 @@ def move_to_pose(self, pose, acceleration, velocity): Acceleration to velocity value velocity: float Velocity of motion + wait: bool + Wait for the motion to finish before returning """ if self.units == "mm": - self.movep(pose, acceleration, velocity) + self.movep(pose, acceleration, velocity, 0, 0, wait) elif self.units == "m": # scale to mm before calling movep pose[:3, 3] *= 1000 acceleration *= 1000 velocity *= 1000 - self.movep(pose, acceleration, velocity) + self.movep(pose, acceleration, velocity, 0, 0, wait) else: raise NotImplemented("Unit {} not supported.".format(self.units)) + def translate(self, vec, acceleration, velocity, wait=False): + """ + Translate the end-effector by the specified vector (x, y, z). + + Parameters + ---------- + vec: ndarray, sequence (list, tuple) + Translate with the relative vector (x, y, z) + acceleration: float + Acceleration to velocity value + velocity: float + Velocity of motion + wait: bool + Wait for the motion to finish before returning + """ + pose = self.pose + pose[:3, 3] += vec + self.movep(pose, acceleration, velocity, 0, 0, wait) + @property def joint_config(self): return self.get_state().get_joint_config() @@ -71,9 +108,21 @@ def joint_config(self): def joint_velocity(self): return self.get_state().get_joint_velocity() + @property + def operational_config(self): + return self.get_state().get_operational_config() + + @property + def operational_velocity(self): + return self.get_state().get_operational_config() + + @property + def operational_force(self): + return self.get_state().get_operational_force() + @property def pose(self): - pose = self.get_state().get_pose() + pose = np.copy(self.get_state().get_pose()) if self.units == "mm": return pose elif self.units == "m": @@ -95,7 +144,7 @@ def x(self): def x(self, val): p = self.pose p[0, 3] = val - self.movep(p, 50, 100) + self.movep(p, 50, 100, 0, 0, True) @property def y(self): @@ -105,7 +154,7 @@ def y(self): def y(self, val): p = self.pose p[1, 3] = val - self.movep(p, 50, 100) + self.movep(p, 50, 100, 0, 0, True) @property def z(self): @@ -115,7 +164,7 @@ def z(self): def z(self, val): p = self.pose p[2, 3] = val - self.movep(p, 50, 100) + self.movep(p, 50, 100, 0, 0, True) @property def rx(self): @@ -125,7 +174,7 @@ def rx(self): def rx(self, val): p = self.pose_aa p[3] = val - self.movep(p, 50, 100) + self.movep(p, 50, 100, 0, 0, True) @property def ry(self): @@ -135,7 +184,7 @@ def ry(self): def ry(self, val): p = self.pose_aa p[4] = val - self.movep(p, 50, 100) + self.movep(p, 50, 100, 0, 0, True) @property def rz(self): @@ -145,7 +194,7 @@ def rz(self): def rz(self, val): p = self.pose_aa p[5] = val - self.movep(p, 50, 100) + self.movep(p, 50, 100, 0, 0, True) def forward_kinematics(self, joint_config, format="homogeneous"): pose = self.get_state().joint_to_pose(joint_config) diff --git a/source/pyromocc/setup.py b/source/pyromocc/setup.py index 7360c9b..94cb2cf 100644 --- a/source/pyromocc/setup.py +++ b/source/pyromocc/setup.py @@ -1,28 +1,111 @@ -from setuptools import setup, find_packages import os +import glob +import platform +import shutil + +from os import popen +from setuptools.command.install import install as _install +from setuptools.dist import Distribution +from setuptools import setup, find_packages package_name = "pyromocc" -package_data = {} +version = "0.0.7" +cmdclass = {} -if os.name == 'posix': - package_data[package_name] = ['*.so'] +build_folder = os.path.abspath(os.path.join(os.path.dirname(__file__), '..')) +bin_path = os.path.join(build_folder, 'bin') +library_path = os.path.join(build_folder, 'lib') + +if platform.system() == "Linux": + libraries = glob.glob(os.path.join(library_path, "*.so")) + package_data = {"pyromocc/pyromocc": [*libraries], "pyromocc": ["*.so"]} +elif platform.system() == "Windows": + libraries = glob.glob(os.path.join(library_path, "*.lib")) + dlls = glob.glob(os.path.join(bin_path, "*.dll")) + pyds = glob.glob(os.path.join(library_path, "*.pyd")) + package_data = {"pyromocc": [*libraries, *dlls, *pyds]} else: - package_data[package_name] = ['*.pyd', '*.dll'] + raise NotImplementedError(f"Platform {platform.system()} currently not supported") + + +try: + from wheel.bdist_wheel import bdist_wheel as _bdist_wheel + + class bdist_wheel(_bdist_wheel): + + def finalize_options(self): + _bdist_wheel.finalize_options(self) + if platform.system() == "Windows": + self.root_is_pure = False + + def get_tag(self): + _, _, plat = _bdist_wheel.get_tag(self) + if platform.system() == "Linux": + glibc_major, glibc_minor = popen("ldd --version | head -1").read().split()[-1].split(".") + + if glibc_major == "2" and glibc_minor == "17": + plat = "manylinux_2_17_x86_64.manylinux2014_x86_64" + else: # For manylinux2014 and above, no alias is required + plat = f"manylinux_{glibc_major}_{glibc_minor}_x86_64" + + return _bdist_wheel.get_tag(self)[:2] + (plat,) + + cmdclass['bdist_wheel'] = bdist_wheel +except ImportError as error: + print("Error importing dependencies:") + print(error) + bdist_wheel = None + + +class install(_install): + def finalize_options(self): + _install.finalize_options(self) + + if platform.system() == "Linux": + self.install_libbase = self.install_platlib + self.install_lib = self.install_platlib + else: + module_dir = self.install_lib + pyromocc_dir = os.path.join(module_dir, "pyromocc") + + if not os.path.exists(pyromocc_dir): + os.makedirs(pyromocc_dir) + + for deps in [*libraries]: + shutil.copy(deps, pyromocc_dir) + + if platform.system() == "Windows": + for deps in [*dlls, *pyds]: + shutil.copy(deps, pyromocc_dir) + + +cmdclass['install'] = install + + +class BinaryDistribution(Distribution): + """Distribution which always forces a binary package with platform name""" + def has_ext_modules(self): + return True + setup(name=package_name, - version='0.0.4', + version=version, author="Andreas Oestvik", - packages=find_packages(exclude=['third_party', 'examples']), - install_requires=['numpy'], + packages=find_packages(exclude=['examples']), + install_requires=['wheel', 'numpy'], setup_requires=['wheel'], extras_require={'examples': ["matplotlib"]}, include_package_data=True, classifiers=[ 'Operating System :: POSIX :: Linux', + 'Operating System :: Microsoft :: Windows', 'Programming Language :: C++', - 'Programming Language :: Python :: 3 :: 3.6', - 'Programming Language :: Python :: 3 :: 3.7', - 'Programming Language :: Python :: 3 :: 3.8', + 'Programming Language :: Python :: 3.8', + 'Programming Language :: Python :: 3.9', + 'Programming Language :: Python :: 3.10', ], - python_requires='>=3.6', - package_data=package_data) + python_requires='>=3.8', + package_dir={'pyromocc': 'pyromocc'}, + package_data=package_data, + cmdclass=cmdclass, + distclass=BinaryDistribution) diff --git a/source/pyromocc/source/pyromocc.cpp b/source/pyromocc/source/pyromocc.cpp index d2fd4bb..2f42990 100644 --- a/source/pyromocc/source/pyromocc.cpp +++ b/source/pyromocc/source/pyromocc.cpp @@ -2,6 +2,8 @@ // Created by androst on 31.03.20. // +#include + #include #include #include @@ -26,17 +28,19 @@ PYBIND11_MODULE(pyromocc, m) { .def("get_state", &Robot::getCurrentState) .def("stop_move", &Robot::stopMove); - robot.def("movej", [](Robot& self, Eigen::Ref target, double acc, double vel){ - self.move(romocc::MotionType::movej, target, acc, vel); + robot.def("movej", [](Robot& self, Eigen::Ref target, double acc, double vel, + double time = 0, double blendRad = 0, bool wait=false){ + self.move(romocc::MotionType::movej, target, acc, vel, time, blendRad, wait); }); - robot.def("movep", [](Robot& self, Eigen::Ref pose, double acc, double vel){ + robot.def("movep", [](Robot& self, Eigen::Ref pose, double acc, double vel, + double time = 0, double blendRad = 0, bool wait=false){ if(pose.rows() == 4 && pose.cols() == 4){ Eigen::Affine3d transform; transform.matrix() = pose; - self.move(romocc::MotionType::movep, transform, acc, vel); + self.move(romocc::MotionType::movep, transform, acc, vel, time, blendRad, wait); } else{ - self.move(romocc::MotionType::movep, pose.transpose(), acc, vel); + self.move(romocc::MotionType::movep, pose.transpose(), acc, vel, time, blendRad, wait); } }); @@ -58,6 +62,9 @@ PYBIND11_MODULE(pyromocc, m) { pybind11::class_> robotState(m, "RobotState"); robotState.def("get_joint_config", &RobotState::getJointConfig); robotState.def("get_joint_velocity", &RobotState::getJointVelocity); + robotState.def("get_operational_config", &RobotState::getOperationalConfig); + robotState.def("get_operational_velocity", &RobotState::getOperationalVelocity); + robotState.def("get_operational_force", &RobotState::getOperationalForce); robotState.def("joint_to_pose", [](RobotState& self, Eigen::Ref joint_config){ return self.jointConfigToOperationalConfig(joint_config).matrix(); }); @@ -87,8 +94,12 @@ PYBIND11_MODULE(pyromocc, m) { .def_readwrite("sw_version", &Manipulator::sw_version); py::enum_(m, "ManipulatorType") + .value("UR3", ManipulatorType::UR3) + .value("UR3e", ManipulatorType::UR3e) .value("UR5", ManipulatorType::UR5) - .value("UR10", ManipulatorType::UR10); + .value("UR5e", ManipulatorType::UR5e) + .value("UR10", ManipulatorType::UR10) + .value("UR10e", ManipulatorType::UR10e); py::enum_(m, "MotionType") .value("movej", MotionType::movej) @@ -129,15 +140,17 @@ PYBIND11_MODULE(pyromocc, m) { py::class_ calibration_matrices(m, "CalibrationMatrices"); calibration_matrices.def_property_readonly("pose_x", [](CalibrationMatrices& self){ - return self.prMb.matrix(); + return self.X.matrix(); }); calibration_matrices.def_property_readonly("pose_y", [](CalibrationMatrices& self){ - return self.eeMt.matrix(); + return self.Y.matrix(); }); py::class_ calibration_error(m, "CalibrationError"); calibration_error.def_readonly("translation_error", &CalibrationError::translationError); calibration_error.def_readonly("rotation_error", &CalibrationError::rotationError); + calibration_error.def_readonly("translation_std", &CalibrationError::transStd); + calibration_error.def_readonly("rotation_std", &CalibrationError::rotStd); m.def("load_calibration_file", [](std::string filepath){ auto cal_affine = romocc::load_calibration_file(filepath); @@ -147,7 +160,7 @@ PYBIND11_MODULE(pyromocc, m) { m.def("save_calibration_file", [](std::string filepath, Eigen::Ref pose){ Eigen::Affine3d transform; transform.matrix() = pose; - save_calibration_file(filepath, transform); + romocc::save_calibration_file(filepath, transform); }); py::class_ calibration_methods(m, "CalibrationMethods"); @@ -170,6 +183,45 @@ PYBIND11_MODULE(pyromocc, m) { return CalibrationMethods::Shah(poses_a_affine, poses_b_affine);; }); + calibration_methods.def("calibration_li", [](std::vector> poses_a, + std::vector> poses_b){ + std::vector poses_a_affine; + std::vector poses_b_affine; + + for(auto const& pose: poses_a) { + Eigen::Affine3d transform; + transform.matrix() = pose; + poses_a_affine.push_back(transform); + } + + for(auto const& pose: poses_b) { + Eigen::Affine3d transform; + transform.matrix() = pose; + poses_b_affine.push_back(transform); + } + return CalibrationMethods::Li(poses_a_affine, poses_b_affine);; + }); + + + calibration_methods.def("calibration_park", [](std::vector> poses_a, + std::vector> poses_b){ + std::vector poses_a_affine; + std::vector poses_b_affine; + + for(auto const& pose: poses_a) { + Eigen::Affine3d transform; + transform.matrix() = pose; + poses_a_affine.push_back(transform); + } + + for(auto const& pose: poses_b) { + Eigen::Affine3d transform; + transform.matrix() = pose; + poses_b_affine.push_back(transform); + } + return CalibrationMethods::Park(poses_a_affine, poses_b_affine); + }); + calibration_methods.def("estimate_calibration_error", [](Eigen::Ref pose_x, Eigen::Ref pose_y, std::vector> poses_a, diff --git a/source/romocc/Robot.cpp b/source/romocc/Robot.cpp index 2710c72..ab9a2ce 100644 --- a/source/romocc/Robot.cpp +++ b/source/romocc/Robot.cpp @@ -27,7 +27,7 @@ bool Robot::connect() bool connected = mCommunicationInterface->connectToRobot(); if(!connected) std::cout << "Robot not connected. Please check address." << std::endl; - std::this_thread::sleep_for(std::chrono::milliseconds(500)); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); return connected; } diff --git a/source/romocc/Robot.h b/source/romocc/Robot.h index e06979d..5d3e297 100644 --- a/source/romocc/Robot.h +++ b/source/romocc/Robot.h @@ -46,11 +46,11 @@ class ROMOCC_EXPORT Robot : public Object private: void waitForMove(); - SharedPointer mCommunicationInterface; + std::shared_ptr mCommunicationInterface; void startSubscription(std::function updateSignal); - SharedPointer mCoordinateSystem; - SharedPointer mCurrentState; + std::shared_ptr mCoordinateSystem; + std::shared_ptr mCurrentState; MotionQueue mMotionQueue; std::unique_ptr mThread; bool mActiveSubscription = false; @@ -60,15 +60,45 @@ template void Robot::move(MotionType type, Target target, double acc, double vel, double t, double rad, bool wait) { mCommunicationInterface->move(type, target, acc, vel, t, rad); - if(wait == true) + if(wait) { - auto remainingDistance = TransformUtils::norm(mCurrentState->get_bMee(), target); - while(remainingDistance > 0.005 || isnan(remainingDistance)){ + double distanceThreshold = 0.005; + const int timeoutInSeconds = 10; // Set your desired timeout in seconds + const auto startTime = std::chrono::steady_clock::now(); + + double remainingDistance; + if(type == MotionType::movej){ + auto targetj = target.matrix().reshaped(6,1); + remainingDistance = TransformUtils::norm(mCurrentState->getJointConfig(), targetj); + } else { + distanceThreshold = 0.015; remainingDistance = TransformUtils::norm(mCurrentState->get_bMee(), target); + }; + + while(remainingDistance > distanceThreshold || isnan(remainingDistance)) + { + if(type == MotionType::movej){ + auto targetj = target.matrix().reshaped(6,1); + remainingDistance = TransformUtils::norm(mCurrentState->getJointConfig(), targetj); + } else { + remainingDistance = TransformUtils::norm(mCurrentState->get_bMee(), target); + }; + const auto currentTime = std::chrono::steady_clock::now(); + const auto elapsedTime = std::chrono::duration_cast(currentTime - startTime).count(); + if (elapsedTime > timeoutInSeconds) + { + break; + } } } } +template <> +void Robot::move(MotionType type, double* target, double acc, double vel, double t, double rad, bool wait) +{ + Eigen::Map> targetVector(target, 6); + move(type, targetVector, acc, vel, t, rad, wait); +} } // namespace romocc diff --git a/source/romocc/calibration/CalibrationHelpers.h b/source/romocc/calibration/CalibrationHelpers.h index f060ddd..4da3a12 100644 --- a/source/romocc/calibration/CalibrationHelpers.h +++ b/source/romocc/calibration/CalibrationHelpers.h @@ -7,32 +7,32 @@ namespace romocc { -/** -* Implementation of a calibration helper class. -* -* \author Andreas Østvik -*/ - - -struct ROMOCC_EXPORT CalibrationError{ - double rotationError; - double translationError; - double rotStd; - double transStd; -}; - -struct ROMOCC_EXPORT CalibrationMatrices{ - Transform3d prMb; - Transform3d eeMt; -}; - -std::vector invert_matrices(std::vector matrices); -std::vector compute_linspace(double start, double end, int steps); -double compute_average(std::vector const &v); -double compute_variance(std::vector const &v, double mean); -double sgn(double val); -Transform3d load_calibration_file(std::string filepath); -void save_calibration_file(std::string filepath, Transform3d calibration_matrix); + /** + * Implementation of a calibration helper class. + * + * \author Andreas Østvik + */ + + + struct ROMOCC_EXPORT CalibrationError{ + double rotationError; + double translationError; + double rotStd; + double transStd; + }; + + struct ROMOCC_EXPORT CalibrationMatrices{ + Transform3d X; + Transform3d Y; + }; + + ROMOCC_EXPORT std::vector invert_matrices(std::vector matrices); + ROMOCC_EXPORT std::vector compute_linspace(double start, double end, int steps); + ROMOCC_EXPORT double compute_average(std::vector const &v); + ROMOCC_EXPORT double compute_variance(std::vector const &v, double mean); + ROMOCC_EXPORT double sgn(double val); + ROMOCC_EXPORT Transform3d load_calibration_file(std::string filepath); + ROMOCC_EXPORT void save_calibration_file(std::string filepath, Transform3d calibration_matrix); } diff --git a/source/romocc/calibration/CalibrationMethods.cpp b/source/romocc/calibration/CalibrationMethods.cpp index 0340a4a..67b6083 100644 --- a/source/romocc/calibration/CalibrationMethods.cpp +++ b/source/romocc/calibration/CalibrationMethods.cpp @@ -6,19 +6,21 @@ namespace romocc { -CalibrationMatrices CalibrationMethods::Shah(std::vector prMt, std::vector bMee) +CalibrationMatrices CalibrationMethods::Shah(std::vector A, std::vector B) { // Inspired by Shah method + // M. Shah 2013, Solving the Robot-World/Hand-Eye Calibration Problem Using the Kronecker Product + CalibrationMatrices cMatrices; - size_t nMatrices = prMt.size(); + size_t nMatrices = A.size(); Eigen::MatrixXd K = Eigen::MatrixXd::Zero(9,9); for(int i = 0; i prMt, std: Eigen::MatrixXd AA = Eigen::MatrixXd::Zero(3*nMatrices, 6); Eigen::MatrixXd b = Eigen::MatrixXd::Zero(3*nMatrices, 1); - //Ry.resize(9,1); for(int i = 0; i(3*i,0) = -(bMee.at(i).rotation()).inverse(); -// AA.block<3,3>(3*i,3) = Eigen::MatrixXd::Identity(3,3); -// b.block<3,1>(3*i,0) = (bMee.at(i).inverse()).translation()- -// (Eigen::kroneckerProduct((prMt.at(i).inverse()).translation().transpose(), Eigen::MatrixXd::Identity(3,3)))*(Ry); - AA.block<3,3>(3*i,0) = (bMee.at(i).rotation()).inverse(); + AA.block<3,3>(3*i,0) = (B.at(i).rotation()).inverse(); AA.block<3,3>(3*i,3) = -Eigen::MatrixXd::Identity(3,3); - b.block<3,1>(3*i,0) = Ry*(prMt.at(i).inverse().translation())-(bMee.at(i).inverse()).translation(); + b.block<3,1>(3*i,0) = Ry*(A.at(i).inverse().translation())-(B.at(i).inverse()).translation(); } Eigen::JacobiSVD svdA(AA,Eigen::ComputeThinU | Eigen::ComputeThinV); @@ -66,40 +63,43 @@ CalibrationMatrices CalibrationMethods::Shah(std::vector prMt, std: eMt.block<3,3>(0,0) = Ry; eMt.block<3,1>(0,3) = t.block<3,1>(3,0); - cMatrices.prMb = Transform3d(bMpr).inverse(); - cMatrices.eeMt = Transform3d(eMt); + cMatrices.X = Transform3d(eMt).inverse(); + cMatrices.Y = Transform3d(bMpr).inverse(); return cMatrices; } -CalibrationMatrices CalibrationMethods::Li(std::vector prMt, std::vector bMee) +CalibrationMatrices CalibrationMethods::Li(std::vector A, std::vector B) { // Inspired by Li method + // A. Li, L.Wang and D. Wu, 2010 + // Simultaneous robot-world and hand-eye calibration using dual-quaternions and Kronecker product + CalibrationMatrices cMatrices; - size_t nMatrices = prMt.size(); + size_t nMatrices = A.size(); - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(12*nMatrices, 24); + Eigen::MatrixXd AA = Eigen::MatrixXd::Zero(12*nMatrices, 24); Eigen::MatrixXd b = Eigen::MatrixXd::Zero(12*nMatrices,1); for(int i = 0; i(12*i,0) = Eigen::kroneckerProduct(Eigen::MatrixXd::Identity(3,3),Ra); - A.block<9,9>(12*i,9) = -Eigen::kroneckerProduct(Rb.transpose(),Eigen::MatrixXd::Identity(3,3)); - A.block<3,9>(12*i+9,9) = Eigen::kroneckerProduct(tb.transpose(),Eigen::MatrixXd::Identity(3,3)); - A.block<3,3>(12*i+9,18) = -Ra; - A.block<3,3>(12*i+9,21) = Eigen::MatrixXd::Identity(3,3); + AA.block<9,9>(12*i,0) = Eigen::kroneckerProduct(Eigen::MatrixXd::Identity(3,3),Ra); + AA.block<9,9>(12*i,9) = -Eigen::kroneckerProduct(Rb.transpose(),Eigen::MatrixXd::Identity(3,3)); + AA.block<3,9>(12*i+9,9) = Eigen::kroneckerProduct(tb.transpose(),Eigen::MatrixXd::Identity(3,3)); + AA.block<3,3>(12*i+9,18) = -Ra; + AA.block<3,3>(12*i+9,21) = Eigen::MatrixXd::Identity(3,3); b.block<3,1>(12*i+9,0) = ta; } - Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::JacobiSVD svd(AA, Eigen::ComputeThinU | Eigen::ComputeThinV); Eigen::MatrixXd x = svd.solve(b); Eigen::MatrixXd X = x.block<9,1>(0,0); @@ -128,30 +128,32 @@ CalibrationMatrices CalibrationMethods::Li(std::vector prMt, std::v eMt.block<3,3>(0,0) = Ry; eMt.block<3,1>(0,3) = x.block<3,1>(21,0); - cMatrices.prMb = Transform3d(bMpr).inverse(); - cMatrices.eeMt = Transform3d(eMt); + cMatrices.X = Transform3d(eMt).inverse(); + cMatrices.Y = Transform3d(bMpr).inverse(); return cMatrices; } -CalibrationMatrices CalibrationMethods::Park(std::vector prMt, std::vector bMee) +CalibrationMatrices CalibrationMethods::Park(std::vector A, std::vector B) { // Inspired by Park method - int nMatrices = int(prMt.size()); + // Park and Martin, 1994, Robot sensor calibration: solving AX=XB on the Euclidean group + + int nMatrices = int(A.size()); - std::vector A, B, AA, BB; + std::vector A_pairs, B_pairs, A_inv_pairs, B_inv_pairs; - std::vector eMb = invert_matrices(bMee); - std::vector tMpr = invert_matrices(prMt); + std::vector A_inv = invert_matrices(A); + std::vector B_inv = invert_matrices(B); for(int i = 0; i prMt, std: Eigen::Vector3d a, b, aa, bb; - for(int i = 0; i prMt, std: iMk.block<3,3>(0,0) = ((M.transpose()*M).sqrt()).inverse()*M.transpose(); lMj.block<3,3>(0,0) = ((MM.transpose()*MM).sqrt()).inverse()*MM.transpose(); - Eigen::MatrixXd C(A.size()*3,3); - Eigen::MatrixXd d(A.size()*3,1); - Eigen::MatrixXd CC(A.size()*3,3); - Eigen::MatrixXd dd(A.size()*3,1); + Eigen::MatrixXd C(A_pairs.size()*3,3); + Eigen::MatrixXd d(A_pairs.size()*3,1); + Eigen::MatrixXd CC(A_pairs.size()*3,3); + Eigen::MatrixXd dd(A_pairs.size()*3,1); int j = 0; - for(int i = 0; i < (A.size()*3); i=i+3) + for(int i = 0; i < (A_pairs.size()*3); i=i+3) { - C.block<3,3>(i,0) = I.rotation()-A.at(j).rotation(); - d.block<3,1>(i,0) = A.at(j).translation()-iMk.block<3,3>(0,0)*B.at(j).translation(); - CC.block<3,3>(i,0) = I.rotation()-AA.at(j).rotation(); - dd.block<3,1>(i,0) = AA.at(j).translation()-lMj.block<3,3>(0,0)*BB.at(j).translation(); + C.block<3,3>(i,0) = I.rotation()-A_pairs.at(j).rotation(); + d.block<3,1>(i,0) = A_pairs.at(j).translation()-iMk.block<3,3>(0,0)*B_pairs.at(j).translation(); + CC.block<3,3>(i,0) = I.rotation()-B_inv_pairs.at(j).rotation(); + dd.block<3,1>(i,0) = B_inv_pairs.at(j).translation()-lMj.block<3,3>(0,0)*A_inv_pairs.at(j).translation(); j++; } @@ -211,26 +214,26 @@ CalibrationMatrices CalibrationMethods::Park(std::vector prMt, std: Transform3d jMl = Transform3d(lMj).inverse(); CalibrationMatrices calibMatrices; - calibMatrices.prMb = Transform3d(iMk); - calibMatrices.eeMt = Transform3d(lMj); + calibMatrices.X = Transform3d(jMl); + calibMatrices.Y = Transform3d(iMk); return calibMatrices; } -CalibrationError CalibrationMethods::estimateCalibrationError(Transform3d prMb, Transform3d eeMt, std::vector bMee, std::vector prMt) +CalibrationError CalibrationMethods::estimateCalibrationError(Transform3d X, Transform3d Y, std::vector A, std::vector B) { CalibrationError calibrationError; - int nMatrices = int(bMee.size()); + int nMatrices = int(A.size()); std::vector rotError, transError; for(int i = 0; i < nMatrices; i++) { - Transform3d errorMatrix = prMb*bMee.at(i)*eeMt*prMt.at(i).inverse(); + Transform3d errorMatrix = A.at(i)*X*(Y*B.at(i)).inverse(); rotError.push_back(TransformUtils::Affine::toAxisAngle(errorMatrix).norm()); - Vector3d translError = bMee.at(i).linear()*eeMt.translation()+bMee.at(i).translation()-prMb.linear().inverse()*prMt.at(i).translation()-prMb.inverse().translation(); + Vector3d translError = A.at(i).linear()*X.translation()+A.at(i).translation()-Y.linear()*B.at(i).translation()-Y.translation(); transError.push_back(translError.norm()); } diff --git a/source/romocc/calibration/CalibrationMethods.h b/source/romocc/calibration/CalibrationMethods.h index 5d46360..a54655a 100644 --- a/source/romocc/calibration/CalibrationMethods.h +++ b/source/romocc/calibration/CalibrationMethods.h @@ -12,14 +12,12 @@ namespace romocc class ROMOCC_EXPORT CalibrationMethods { public: - static CalibrationMatrices Shah(std::vector prMt, std::vector bMee); - static CalibrationMatrices Li(std::vector prMt, std::vector bMee); - static CalibrationMatrices Park(std::vector prMt, std::vector bMee); + static CalibrationMatrices Shah(std::vector A, std::vector B); + static CalibrationMatrices Li(std::vector A, std::vector B); + static CalibrationMatrices Park(std::vector A, std::vector B); - static CalibrationError estimateCalibrationError( Transform3d prMb, - Transform3d eeMt, - std::vector bMee, - std::vector prMt); + static CalibrationError estimateCalibrationError(Transform3d A, Transform3d B, + std::vector X, std::vector Y); }; } diff --git a/source/romocc/communication/CommunicationInterface.cpp b/source/romocc/communication/CommunicationInterface.cpp index 9226c0e..5f9d903 100644 --- a/source/romocc/communication/CommunicationInterface.cpp +++ b/source/romocc/communication/CommunicationInterface.cpp @@ -46,7 +46,7 @@ void CommunicationInterface::decodeReceivedPackages() const uint16_t bufferSize = 2048; uint8_t buffer[bufferSize]; - bzero(buffer, bufferSize); + memset(buffer, 0, bufferSize); while(!mStopThread) { diff --git a/source/romocc/communication/CommunicationInterface.h b/source/romocc/communication/CommunicationInterface.h index dfaa7ed..5153d52 100644 --- a/source/romocc/communication/CommunicationInterface.h +++ b/source/romocc/communication/CommunicationInterface.h @@ -38,8 +38,8 @@ class ROMOCC_EXPORT CommunicationInterface : public Object void stopMove(MotionType typeOfStop, double acc); private: - SharedPointer mClient; - SharedPointer mEncoder; + std::shared_ptr mClient; + std::shared_ptr mEncoder; RobotState::pointer mCurrentState; std::string mHost; diff --git a/source/romocc/communication/MessageDecoder.h b/source/romocc/communication/MessageDecoder.h index d1a6e72..cbc22b3 100644 --- a/source/romocc/communication/MessageDecoder.h +++ b/source/romocc/communication/MessageDecoder.h @@ -1,23 +1,26 @@ #ifndef ROMOCC_MESSAGEDECODER_H #define ROMOCC_MESSAGEDECODER_H +#include "romocc/core/Object.h" #include "romocc/core/ForwardDeclarations.h" -#include "romocc/robotics/RobotState.h" namespace romocc { -struct ROMOCC_EXPORT JointState +struct ROMOCC_EXPORT ConfigState { Eigen::RowVectorXd jointConfiguration; Eigen::RowVectorXd jointVelocity; + Eigen::RowVectorXd operationalConfiguration; + Eigen::RowVectorXd operationalVelocity; + Eigen::RowVectorXd operationalForce; double timestamp = 0; }; class ROMOCC_EXPORT MessageDecoder : public Object { public: - virtual JointState analyzeTCPSegment(unsigned char* packet) = 0; + virtual ConfigState analyzeTCPSegment(unsigned char* packet) = 0; }; diff --git a/source/romocc/communication/MessageEncoder.h b/source/romocc/communication/MessageEncoder.h index b1b90c9..6904263 100644 --- a/source/romocc/communication/MessageEncoder.h +++ b/source/romocc/communication/MessageEncoder.h @@ -1,5 +1,6 @@ #ifndef ROMOCC_MESSAGEENCODER_H #define ROMOCC_MESSAGEENCODER_H +#define _CRT_NO_VA_START_VALIDATION #include diff --git a/source/romocc/core/CMakeLists.txt b/source/romocc/core/CMakeLists.txt index 26ac4ad..2ec1431 100644 --- a/source/romocc/core/CMakeLists.txt +++ b/source/romocc/core/CMakeLists.txt @@ -1,5 +1,6 @@ romocc_add_sources( PrecompiledHeaders.h + PortableEndian.h ForwardDeclarations.h SmartPointers.h Object.cpp diff --git a/source/romocc/core/ForwardDeclarations.h b/source/romocc/core/ForwardDeclarations.h index c72b3a8..0e7f938 100644 --- a/source/romocc/core/ForwardDeclarations.h +++ b/source/romocc/core/ForwardDeclarations.h @@ -11,14 +11,38 @@ namespace romocc { // Supported manipulators -typedef enum {UR5, UR10, UR10e} ManipulatorType; +typedef enum {UR3, UR3e, UR5, UR5e, UR10, UR10e} ManipulatorType; struct ROMOCC_EXPORT Manipulator{ ManipulatorType manipulator; std::string sw_version; + ManipulatorType manipulatorTypeFromString(const std::string& manipulator) { + if (manipulator == "UR3") { + return UR3; + } else if (manipulator == "UR3e"){ + return UR3e; + } else if (manipulator == "UR5") { + return UR5; + } else if (manipulator == "UR5e") { + return UR5e; + } else if (manipulator == "UR10") { + return UR10; + } else if (manipulator == "UR10e") { + return UR10e; + } else { + throw std::runtime_error("Invalid manipulator type string"); + } + } + Manipulator(ManipulatorType manipulator = UR5, std::string sw_version = "3.0"){ this->manipulator = manipulator; - this->sw_version = sw_version;}; + this->sw_version = sw_version; + } + + Manipulator(const std::string& manipulator, std::string sw_version = "3.0"){ + this->manipulator = manipulatorTypeFromString(manipulator); + this->sw_version = sw_version; + } }; // Robot motion @@ -39,6 +63,14 @@ typedef KDL::ChainIkSolverPos_LMA IKSolver; typedef KDL::ChainIkSolverVel_pinv IKVelSolver; typedef KDL::ChainJntToJacSolver JacobianSolver; +// Struct packaging +#ifdef __GNUC__ +#define PACK( __Declaration__ ) __Declaration__ __attribute__((__packed__)) +#endif + +#ifdef _MSC_VER +#define PACK( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop)) +#endif } diff --git a/source/romocc/core/Object.h b/source/romocc/core/Object.h index 0dc933f..0277635 100644 --- a/source/romocc/core/Object.h +++ b/source/romocc/core/Object.h @@ -12,7 +12,7 @@ namespace romocc{ class Object { public: - typedef SharedPointer pointer; + typedef std::shared_ptr pointer; virtual ~Object() {}; protected: diff --git a/source/romocc/core/PortableEndian.h b/source/romocc/core/PortableEndian.h new file mode 100644 index 0000000..18937a7 --- /dev/null +++ b/source/romocc/core/PortableEndian.h @@ -0,0 +1,122 @@ +// "License": Public Domain +// I, Mathias Panzenb�ck, place this file hereby into the public domain. Use it at your own risk for whatever you like. +// In case there are jurisdictions that don't support putting things in the public domain you can also consider it to +// be "dual licensed" under the BSD, MIT and Apache licenses, if you want to. This code is trivial anyway. Consider it +// an example on how to get the endian conversion functions on different platforms. + +#ifndef LIBROMOCC_PORTABLEENDIAN_H +#define LIBROMOCC_PORTABLEENDIAN_H + + +#if (defined(_WIN16) || defined(_WIN32) || defined(_WIN64)) && !defined(__WINDOWS__) + +# define __WINDOWS__ + +#endif + +#if defined(__linux__) || defined(__CYGWIN__) + +# include + +#elif defined(__APPLE__) + +# include + +# define htobe16(x) OSSwapHostToBigInt16(x) +# define htole16(x) OSSwapHostToLittleInt16(x) +# define be16toh(x) OSSwapBigToHostInt16(x) +# define le16toh(x) OSSwapLittleToHostInt16(x) + +# define htobe32(x) OSSwapHostToBigInt32(x) +# define htole32(x) OSSwapHostToLittleInt32(x) +# define be32toh(x) OSSwapBigToHostInt32(x) +# define le32toh(x) OSSwapLittleToHostInt32(x) + +# define htobe64(x) OSSwapHostToBigInt64(x) +# define htole64(x) OSSwapHostToLittleInt64(x) +# define be64toh(x) OSSwapBigToHostInt64(x) +# define le64toh(x) OSSwapLittleToHostInt64(x) + +# define __BYTE_ORDER BYTE_ORDER +# define __BIG_ENDIAN BIG_ENDIAN +# define __LITTLE_ENDIAN LITTLE_ENDIAN +# define __PDP_ENDIAN PDP_ENDIAN + +#elif defined(__OpenBSD__) + +# include + +#elif defined(__NetBSD__) || defined(__FreeBSD__) || defined(__DragonFly__) + +# include + +# define be16toh(x) betoh16(x) +# define le16toh(x) letoh16(x) + +# define be32toh(x) betoh32(x) +# define le32toh(x) letoh32(x) + +# define be64toh(x) betoh64(x) +# define le64toh(x) letoh64(x) + +#elif defined(__WINDOWS__) + +# include + +# if BYTE_ORDER == LITTLE_ENDIAN + +# if defined(_MSC_VER) +# include +# define htobe16(x) _byteswap_ushort(x) +# define htole16(x) (x) +# define be16toh(x) _byteswap_ushort(x) +# define le16toh(x) (x) + +# define htobe32(x) _byteswap_ulong(x) +# define htole32(x) (x) +# define be32toh(x) _byteswap_ulong(x) +# define le32toh(x) (x) + +# define htobe64(x) _byteswap_uint64(x) +# define htole64(x) (x) +# define be64toh(x) _byteswap_uint64(x) +# define le64toh(x) (x) + +# elif defined(__GNUC__) || defined(__clang__) + +# define htobe16(x) __builtin_bswap16(x) +# define htole16(x) (x) +# define be16toh(x) __builtin_bswap16(x) +# define le16toh(x) (x) + +# define htobe32(x) __builtin_bswap32(x) +# define htole32(x) (x) +# define be32toh(x) __builtin_bswap32(x) +# define le32toh(x) (x) + +# define htobe64(x) __builtin_bswap64(x) +# define htole64(x) (x) +# define be64toh(x) __builtin_bswap64(x) +# define le64toh(x) (x) +# else +# error platform not supported +# endif + +# else + +# error byte order not supported + +# endif + +# define __BYTE_ORDER BYTE_ORDER +# define __BIG_ENDIAN BIG_ENDIAN +# define __LITTLE_ENDIAN LITTLE_ENDIAN +# define __PDP_ENDIAN PDP_ENDIAN + +#else + +# error platform not supported + +#endif + +#endif //LIBROMOCC_PORTABLEENDIAN_H diff --git a/source/romocc/core/SmartPointers.h b/source/romocc/core/SmartPointers.h index 1b40d30..5be6735 100644 --- a/source/romocc/core/SmartPointers.h +++ b/source/romocc/core/SmartPointers.h @@ -10,9 +10,9 @@ #define ROMOCC_OBJECT(className) \ public: \ - typedef SharedPointer pointer; \ - static SharedPointer New() { \ - SharedPointer smartPtr(new className()); \ + typedef std::shared_ptr pointer; \ + static std::shared_ptr New() { \ + std::shared_ptr smartPtr(new className()); \ smartPtr->setPtr(smartPtr); \ \ return smartPtr; \ @@ -29,41 +29,4 @@ } \ - -#ifdef WIN32 - -namespace romocc { - -template -class SharedPointer : public std::shared_ptr { - using std::shared_ptr::shared_ptr; // inherit constructor - -}; - -}; // end namespace echobot - -// Custom hashing functions for the smart pointers so that they can be used in unordered_map etc. -namespace std { -template -class hash >{ - public: - size_t operator()(const romocc::SharedPointer &object) const { - return (std::size_t)object.get(); - } -}; - -} // end namespace std - -#else - -namespace romocc { - - template - using SharedPointer = std::shared_ptr; - -} - -#endif - - #endif //ROMOCC_SMARTPOINTERS_H diff --git a/source/romocc/manipulators/ur/UrKDLDefinition.h b/source/romocc/manipulators/ur/UrKDLDefinition.h index 47a1bc4..c141d67 100644 --- a/source/romocc/manipulators/ur/UrKDLDefinition.h +++ b/source/romocc/manipulators/ur/UrKDLDefinition.h @@ -4,6 +4,9 @@ #include #include +// Reference: +// https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/ + namespace romocc { @@ -114,6 +117,71 @@ namespace Ur10e } } +namespace Ur3 +{ + constexpr double dh_d[6] = {0.1519, 0, 0, 0.11235, 0.08535, 0.0819}; + constexpr double dh_a[6] = {0,-0.24365,-0.21325,0,0,0}; + constexpr double dh_alpha[6] = {M_PI_2, 0, 0, M_PI_2, -M_PI_2, 0}; + constexpr double dh_home[6] = {0, -M_PI_2, -M_PI_2 ,-M_PI_2, M_PI_2, 0}; + + KDL::Chain KDLChain(){ + KDL::Chain Ur3; + for (unsigned int i = 0; i < 6; i++) { + Ur3.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame::DH(dh_a[i], dh_alpha[i], dh_d[i], 0.0))); + } + return Ur3; + } +} + +namespace Ur3e +{ + constexpr double dh_d[6] = {0.15185, 0, 0, 0.13105, 0.08535, 0.0921}; + constexpr double dh_a[6] = {0,-0.24355,-0.2132,0,0,0}; + constexpr double dh_alpha[6] = {M_PI_2, 0, 0, M_PI_2, -M_PI_2, 0}; + constexpr double dh_home[6] = {0, -M_PI_2, -M_PI_2 ,-M_PI_2, M_PI_2, 0}; + + KDL::Chain KDLChain(){ + KDL::Chain Ur3e; + for (unsigned int i = 0; i < 6; i++) { + Ur3e.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame::DH(dh_a[i], dh_alpha[i], dh_d[i], 0.0))); + } + return Ur3e; + } +} + +namespace Ur5e +{ + constexpr double dh_d[6] = {0.1625, 0, 0, 0.1333, 0.0997, 0.0996}; + constexpr double dh_a[6] = {0,-0.42500,-0.3922,0,0,0}; + constexpr double dh_alpha[6] = {M_PI_2, 0, 0, M_PI_2, -M_PI_2, 0}; + constexpr double dh_home[6] = {0, -M_PI_2, -M_PI_2 ,-M_PI_2, M_PI_2, 0}; + + KDL::Chain KDLChain(){ + KDL::Chain Ur5e; + for (unsigned int i = 0; i < 6; i++) { + Ur5e.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame::DH(dh_a[i], dh_alpha[i], dh_d[i], 0.0))); + } + return Ur5e; + } +} + +KDL::Chain setupKDLChain(Manipulator manipulator) { + if (manipulator.manipulator == ManipulatorType::UR3){ + return Ur3::KDLChain(); + } else if (manipulator.manipulator == ManipulatorType::UR3e){ + return Ur3e::KDLChain(); + } else if (manipulator.manipulator == ManipulatorType::UR5){ + return Ur5::KDLChain(); + } else if (manipulator.manipulator == ManipulatorType::UR5e){ + return Ur5e::KDLChain(); + } else if (manipulator.manipulator == ManipulatorType::UR10){ + return Ur10::KDLChain(); + } else if (manipulator.manipulator == ManipulatorType::UR10e){ + return Ur10e::KDLChain(); + } else { + throw std::runtime_error("Manipulator not supported"); + } +} } #endif //ROMOCC_URKDLDEFINITION_H diff --git a/source/romocc/manipulators/ur/UrMessageDecoder.cpp b/source/romocc/manipulators/ur/UrMessageDecoder.cpp index dc71534..205d1b3 100644 --- a/source/romocc/manipulators/ur/UrMessageDecoder.cpp +++ b/source/romocc/manipulators/ur/UrMessageDecoder.cpp @@ -1,25 +1,34 @@ #include "UrMessageDecoder.h" + +#include "romocc/utilities/MathUtils.h" #include "romocc/utilities/ZMQUtils.h" -#include -#include namespace romocc { -JointState UrMessageDecoder::analyzeTCPSegment(unsigned char* packet) +ConfigState UrMessageDecoder::analyzeTCPSegment(unsigned char* packet) { - JointState state; + ConfigState state; if(romocc::packageSize(packet)>=764 || romocc::packageSize(packet)<=1116) { raw_ur_state* raw_state = reinterpret_cast(packet); double timestamp = ntohd(raw_state->time_); double* jointConfiguration = romocc::arrayToLittleEndian(raw_state->actual_positions_); double* jointVelocity = romocc::arrayToLittleEndian(raw_state->actual_velocities_); + double* operationalConfiguration = romocc::arrayToLittleEndian(raw_state->actual_tool_coordinates_); + double* operationalVelocity = romocc::arrayToLittleEndian(raw_state->actual_tool_speed_); + double* operationalForce = romocc::arrayToLittleEndian(raw_state->generalised_tool_forces_); + + TransformUtils::scaleTranslation(operationalConfiguration, 1000); + TransformUtils::scaleTranslation(operationalVelocity, 1000); state.timestamp = timestamp; state.jointConfiguration = RowVector6d(jointConfiguration); state.jointVelocity = RowVector6d(jointVelocity); + state.operationalConfiguration = RowVector6d(operationalConfiguration); + state.operationalVelocity = RowVector6d(operationalVelocity); + state.operationalForce = RowVector6d(operationalForce); } return state; } diff --git a/source/romocc/manipulators/ur/UrMessageDecoder.h b/source/romocc/manipulators/ur/UrMessageDecoder.h index ee3942c..492b797 100644 --- a/source/romocc/manipulators/ur/UrMessageDecoder.h +++ b/source/romocc/manipulators/ur/UrMessageDecoder.h @@ -21,47 +21,47 @@ class ROMOCC_EXPORT UrMessageDecoder : public MessageDecoder ROMOCC_OBJECT(UrMessageDecoder) public: - virtual JointState analyzeTCPSegment(unsigned char* packet); + ConfigState analyzeTCPSegment(unsigned char* packet) override; private: static const int NUMBER_OF_JOINTS = 6; - struct __attribute__((packed)) raw_ur_state + PACK(struct raw_ur_state { - int32_t message_size_; - uint64_t time_; - double target_positions_[NUMBER_OF_JOINTS]; - double target_velocities_[NUMBER_OF_JOINTS]; - double target_accelerations_[NUMBER_OF_JOINTS]; - double target_currents_[NUMBER_OF_JOINTS]; - double target_torques_[NUMBER_OF_JOINTS]; - double actual_positions_[NUMBER_OF_JOINTS]; - double actual_velocities_[NUMBER_OF_JOINTS]; - double actual_currents_[NUMBER_OF_JOINTS]; - double joint_control_torques_[NUMBER_OF_JOINTS]; - double actual_tool_coordinates_[NUMBER_OF_JOINTS]; - double actual_tool_speed_[NUMBER_OF_JOINTS]; - double generalised_tool_forces_[NUMBER_OF_JOINTS]; - double target_tool_coordinates_[NUMBER_OF_JOINTS]; - double target_tool_speed_[NUMBER_OF_JOINTS]; - int64_t digital_pins_; - double motor_temperatures_[NUMBER_OF_JOINTS]; - double controller_timer_; - double test_value_; - int64_t robot_mode_; - int64_t joint_modes_[NUMBER_OF_JOINTS]; - int64_t safety_mode_; - int64_t unused_1_[6]; - double tool_accelerometer_values_[3]; - int64_t unused_2_[6]; - double speed_scaling_; - double linear_momentum_norm_; - int64_t unused_3_[2]; - double masterboard_main_voltage_; - double masterboard_robot_voltage_; - double masterboard_robot_current_; - double actual_joint_voltages_[NUMBER_OF_JOINTS]; - }; + int32_t message_size_; + uint64_t time_; + double target_positions_[UrMessageDecoder::NUMBER_OF_JOINTS]; + double target_velocities_[UrMessageDecoder::NUMBER_OF_JOINTS]; + double target_accelerations_[UrMessageDecoder::NUMBER_OF_JOINTS]; + double target_currents_[UrMessageDecoder::NUMBER_OF_JOINTS]; + double target_torques_[UrMessageDecoder::NUMBER_OF_JOINTS]; + double actual_positions_[UrMessageDecoder::NUMBER_OF_JOINTS]; + double actual_velocities_[UrMessageDecoder::NUMBER_OF_JOINTS]; + double actual_currents_[UrMessageDecoder::NUMBER_OF_JOINTS]; + double joint_control_torques_[UrMessageDecoder::NUMBER_OF_JOINTS]; + double actual_tool_coordinates_[UrMessageDecoder::NUMBER_OF_JOINTS]; + double actual_tool_speed_[UrMessageDecoder::NUMBER_OF_JOINTS]; + double generalised_tool_forces_[UrMessageDecoder::NUMBER_OF_JOINTS]; + double target_tool_coordinates_[UrMessageDecoder::NUMBER_OF_JOINTS]; + double target_tool_speed_[UrMessageDecoder::NUMBER_OF_JOINTS]; + int64_t digital_pins_; + double motor_temperatures_[UrMessageDecoder::NUMBER_OF_JOINTS]; + double controller_timer_; + double test_value_; + int64_t robot_mode_; + int64_t joint_modes_[UrMessageDecoder::NUMBER_OF_JOINTS]; + int64_t safety_mode_; + int64_t unused_1_[6]; + double tool_accelerometer_values_[3]; + int64_t unused_2_[6]; + double speed_scaling_; + double linear_momentum_norm_; + int64_t unused_3_[2]; + double masterboard_main_voltage_; + double masterboard_robot_voltage_; + double masterboard_robot_current_; + double actual_joint_voltages_[UrMessageDecoder::NUMBER_OF_JOINTS]; + }); }; diff --git a/source/romocc/robotics/RobotCoordinateSystem.h b/source/romocc/robotics/RobotCoordinateSystem.h index a1a6776..7ba3de7 100644 --- a/source/romocc/robotics/RobotCoordinateSystem.h +++ b/source/romocc/robotics/RobotCoordinateSystem.h @@ -11,7 +11,7 @@ namespace romocc { -class RobotCoordinateSystem : public Object +class ROMOCC_EXPORT RobotCoordinateSystem : public Object { ROMOCC_OBJECT(RobotCoordinateSystem) diff --git a/source/romocc/robotics/RobotState.cpp b/source/romocc/robotics/RobotState.cpp index d875058..0e793ca 100644 --- a/source/romocc/robotics/RobotState.cpp +++ b/source/romocc/robotics/RobotState.cpp @@ -16,23 +16,20 @@ RobotState::RobotState() : void RobotState::unpack(uint8_t *buffer) { mValueLock.lock(); - JointState jointState = mDecoder->analyzeTCPSegment(buffer); - this->setState(jointState.jointConfiguration, jointState.jointVelocity, jointState.timestamp); + ConfigState configState = mDecoder->analyzeTCPSegment(buffer); + this->setState(configState); mValueLock.unlock(); } void RobotState::setManipulator(romocc::Manipulator manipulator) { this->setKDLchain(manipulator); this->setDecoder(manipulator); + mManipulator = manipulator; } void RobotState::setKDLchain(Manipulator manipulator) { - if (manipulator.manipulator == ManipulatorType::UR5) { - mKDLChain = Ur5::KDLChain(); - } else if (manipulator.manipulator == ManipulatorType::UR10) { - mKDLChain = Ur10::KDLChain(); - } + mKDLChain = setupKDLChain(manipulator); KDL::JntArray q_min(mKDLChain.getNrOfJoints()), q_max(mKDLChain.getNrOfJoints()); for (int i = 0; i < mKDLChain.getNrOfJoints(); i++) { @@ -47,17 +44,25 @@ void RobotState::setKDLchain(Manipulator manipulator) { mJacSolver = std::shared_ptr(new JacobianSolver(mKDLChain)); } -void RobotState::setDecoder(Manipulator manipulator) { - if (manipulator.manipulator == ManipulatorType::UR5 || manipulator.manipulator == ManipulatorType::UR10) - mDecoder = UrMessageDecoder::New(); +void RobotState::setDecoder(romocc::Manipulator manipulator) { + mDecoder = UrMessageDecoder::New(); } -void RobotState::setState(RowVector6d jointConfig, RowVector6d jointVel, double timestamp) { - mTimestamp = timestamp; - mJointConfiguration = jointConfig; - mJointVelocity = jointVel; - m_bMee = transform_to_joint(jointConfig); - mOperationalConfiguration = TransformUtils::Affine::toVector6D(m_bMee); +void RobotState::setState(romocc::ConfigState configState) { + mTimestamp = configState.timestamp; + mJointConfiguration = configState.jointConfiguration; + mJointVelocity = configState.jointVelocity; + mOperationalForce = configState.operationalForce; + + if(mManipulator.manipulator == UR5e || mManipulator.manipulator == UR10e || mManipulator.manipulator == UR3e){ + mOperationalConfiguration = configState.operationalConfiguration; + mOperationalVelocity = configState.operationalVelocity; + m_bMee = TransformUtils::Affine::toAffine3DFromVector6D(mOperationalConfiguration); + } else{ + mOperationalVelocity = getJacobian() * mJointVelocity; + m_bMee = transform_to_joint(configState.jointConfiguration); + mOperationalConfiguration = TransformUtils::Affine::toVector6D(m_bMee); + } } Transform3d RobotState::transform_to_joint(RowVector6d jointConfig, int jointNr) { @@ -74,13 +79,9 @@ Transform3d RobotState::transform_to_joint(RowVector6d jointConfig, int jointNr) Matrix6d RobotState::getJacobian(int jointNr) { KDL::JntArray input_q(mKDLChain.getNrOfJoints()); - Vector6d jointConfig = getJointConfig(); - - for (unsigned int i = 0; i < mKDLChain.getNrOfJoints(); i++) { input_q(i) = jointConfig[i]; } - + for (unsigned int i = 0; i < mKDLChain.getNrOfJoints(); i++) { input_q(i) = mJointConfiguration[i]; } KDL::Jacobian output_jac(mKDLChain.getNrOfJoints()); mJacSolver->JntToJac(input_q, output_jac, jointNr); - return output_jac.data; } @@ -146,9 +147,16 @@ double RobotState::getTimestamp() { Vector6d RobotState::getOperationalVelocity() { Vector6d ret; - Matrix6d jacobian = getJacobian(); mValueLock.lock(); - ret = jacobian * mJointVelocity; + ret = mOperationalVelocity; + mValueLock.unlock(); + return ret; +} + +Vector6d RobotState::getOperationalForce() { + Vector6d ret; + mValueLock.lock(); + ret = mOperationalForce; mValueLock.unlock(); return ret; } diff --git a/source/romocc/robotics/RobotState.h b/source/romocc/robotics/RobotState.h index 4892cb8..2ba3380 100644 --- a/source/romocc/robotics/RobotState.h +++ b/source/romocc/robotics/RobotState.h @@ -1,9 +1,13 @@ #ifndef ROBOTSTATE_H #define ROBOTSTATE_H +#define _USE_MATH_DEFINES #include +#include + #include "romocc/core/Object.h" #include "romocc/utilities/MathUtils.h" +#include "romocc/communication/MessageDecoder.h" namespace romocc { @@ -37,6 +41,7 @@ class ROMOCC_EXPORT RobotState : public Object Vector6d getJointVelocity(); Vector6d getOperationalConfig(); Vector6d getOperationalVelocity(); + Vector6d getOperationalForce(); Vector6d operationalConfigToJointConfig(Transform3d transform); Transform3d jointConfigToOperationalConfig(Vector6d jointConfig); @@ -44,16 +49,19 @@ class ROMOCC_EXPORT RobotState : public Object std::shared_ptr getIKSolver(); private: - SharedPointer mDecoder; + std::shared_ptr mDecoder; void setKDLchain(Manipulator manipulator); void setDecoder(Manipulator manipulator); - void setState(RowVector6d q, RowVector6d q_vel, double timestamp); + void setState(romocc::ConfigState configState); double mTimestamp; Vector6d mJointConfiguration; Vector6d mJointVelocity; Vector6d mOperationalConfiguration; + Vector6d mOperationalVelocity; + Vector6d mOperationalForce; Transform3d m_bMee; + Manipulator mManipulator; std::mutex mValueLock; diff --git a/source/romocc/utilities/MathUtils.cpp b/source/romocc/utilities/MathUtils.cpp index 99ef851..0b500e9 100644 --- a/source/romocc/utilities/MathUtils.cpp +++ b/source/romocc/utilities/MathUtils.cpp @@ -10,6 +10,23 @@ Eigen::Affine3d TransformUtils::Affine::scaleTranslation(Eigen::Affine3d affine, return affine; } +Eigen::Matrix TransformUtils::scaleTranslation(Eigen::Matrix vec, double scale) +{ + vec[0] = vec[0]*scale; + vec[1] = vec[1]*scale; + vec[2] = vec[2]*scale; + return vec; +} + +double* TransformUtils::scaleTranslation(double* vec, double scale) +{ + vec[0] = vec[0]*scale; + vec[1] = vec[1]*scale; + vec[2] = vec[2]*scale; + return vec; +} + + Eigen::Vector3d TransformUtils::Affine::toAxisAngle(Eigen::Affine3d affine) { return (Eigen::AngleAxisd(affine.linear())).angle()*(Eigen::AngleAxisd(affine.linear())).axis(); @@ -98,4 +115,11 @@ KDL::JntArray TransformUtils::kdl::fromVector6D(Eigen::Matrix vect return jnt_array; } +Vector6d TransformUtils::radToDeg(Vector6d vector) { + Vector6d degVector; + for(int i = 0; i < 6; ++i) + degVector[i] = vector[i]*180/M_PI; + return degVector; +} + } \ No newline at end of file diff --git a/source/romocc/utilities/MathUtils.h b/source/romocc/utilities/MathUtils.h index c25f41e..a8b36d1 100644 --- a/source/romocc/utilities/MathUtils.h +++ b/source/romocc/utilities/MathUtils.h @@ -1,3 +1,7 @@ +#define _USE_MATH_DEFINES + +#include + #include "romocc/core/ForwardDeclarations.h" namespace romocc @@ -22,6 +26,11 @@ namespace TransformUtils{ ROMOCC_EXPORT double norm(Transform3d a, Transform3d b); ROMOCC_EXPORT double norm(Transform3d a, Vector6d b); ROMOCC_EXPORT double norm(Transform3d a, double b[6]); + + ROMOCC_EXPORT Vector6d radToDeg(Vector6d vector); + + ROMOCC_EXPORT Eigen::Matrix scaleTranslation(Eigen::Matrix vector, double scale); + ROMOCC_EXPORT double* scaleTranslation(double* vector, double scale); } } // namespace romocc \ No newline at end of file diff --git a/source/romocc/utilities/ZMQUtils.h b/source/romocc/utilities/ZMQUtils.h index 3d13556..2e3499b 100644 --- a/source/romocc/utilities/ZMQUtils.h +++ b/source/romocc/utilities/ZMQUtils.h @@ -7,12 +7,14 @@ #include #include + #include "romocc/core/Object.h" +#include "romocc/core/PortableEndian.h" namespace romocc { -class ZMQUtils { +class ROMOCC_EXPORT ZMQUtils { public: static void*& getContext() diff --git a/tests/CalibrationTests.cpp b/tests/CalibrationTests.cpp index 0dde633..9c37a28 100644 --- a/tests/CalibrationTests.cpp +++ b/tests/CalibrationTests.cpp @@ -1,4 +1,7 @@ #include "catch/catch.hpp" +#define _USE_MATH_DEFINES + +#include #include #include @@ -21,7 +24,7 @@ TEST_CASE("Dummy test Shah calibration", "[romocc][Calibration]") { matStack.push_back(affine); auto calibMatrices = CalibrationMethods::Shah(matStack, matStack); - std::cout << calibMatrices.eeMt.matrix() << std::endl; + std::cout << calibMatrices.X.matrix() << std::endl; } TEST_CASE("Load calibration file", "[romocc][Calibration]"){ diff --git a/tests/MathUtilsTests.cpp b/tests/MathUtilsTests.cpp index 9423449..f8bcef5 100644 --- a/tests/MathUtilsTests.cpp +++ b/tests/MathUtilsTests.cpp @@ -1,4 +1,7 @@ #include "catch/catch.hpp" +#define _USE_MATH_DEFINES + +#include #include #include "romocc/core/ForwardDeclarations.h" diff --git a/tests/RobotTests.cpp b/tests/RobotTests.cpp index e1472ea..c6d6cba 100644 --- a/tests/RobotTests.cpp +++ b/tests/RobotTests.cpp @@ -2,12 +2,13 @@ // Created by androst on 05.06.19. // -#include "catch/catch.hpp" -#include "romocc/Robot.h" #include +#include #include -#include -#include + +#include "catch/catch.hpp" +#include "romocc/Robot.h" +#include "romocc/manipulators/ur/UrKDLDefinition.h" @@ -114,4 +115,43 @@ TEST_CASE("Move with wait", "[romocc][Robot]"){ robot->move(MotionType::movep, target_a, 500, 500, 0, 0, true); robot->move(MotionType::movep, target_b, 500, 500); } + +TEST_CASE("Move joints with wait", "[romocc][Robot]"){ + Robot::pointer robot = Robot::New(); + robot->configure(Manipulator(ManipulatorType::UR5), "192.168.231.129", 30003); + robot->connect(); + + std::cout << robot->getCurrentState()->getOperationalConfig() << std::endl; + Vector6d target_a; + target_a << -M_PI_2, -M_PI_2, M_PI_2, -M_PI_2, -M_PI_2, 0.; + robot->move(MotionType::movej, target_a, 500, 500, 0, 0, true); +} + +TEST_CASE("Add update subscription", "[romocc][Robot]"){ + auto robot = Robot::New(); + robot->configure(Manipulator(ManipulatorType::UR5), "192.168.231.129", 30003); + robot->connect(); + + auto print_message = []() + { + std::cout << "State updated" << "\n"; + }; + + robot->addUpdateSubscription(print_message); + } + +TEST_CASE("Transform to all joints", "[romocc][Robot]"){ + auto robot = Robot::New(); + robot->configure(Manipulator(ManipulatorType::UR5), "192.168.231.129", 30003); + robot->connect(); + + Transform3d m_bM1 = robot->getCurrentState()->getTransformToJoint(1); + Transform3d m_bM2 = robot->getCurrentState()->getTransformToJoint(2); + Transform3d m_bM3 = robot->getCurrentState()->getTransformToJoint(3); + Transform3d m_bM4 = robot->getCurrentState()->getTransformToJoint(4); + Transform3d m_bM5 = robot->getCurrentState()->getTransformToJoint(5); + Transform3d m_bM6 = robot->getCurrentState()->getTransformToJoint(6); + + std::cout << m_bM2.matrix() << std::endl; + } } diff --git a/tests/catch/catch.hpp b/tests/catch/catch.hpp index 02302b8..d35964b 100644 --- a/tests/catch/catch.hpp +++ b/tests/catch/catch.hpp @@ -1,9 +1,9 @@ /* - * Catch v2.8.0 - * Generated: 2019-05-26 21:29:22.235281 + * Catch v2.13.9 + * Generated: 2022-04-12 22:37:23.260201 * ---------------------------------------------------------- * This file has been merged from multiple headers. Please don't edit it directly - * Copyright (c) 2019 Two Blue Cubes Ltd. All rights reserved. + * Copyright (c) 2022 Two Blue Cubes Ltd. All rights reserved. * * Distributed under the Boost Software License, Version 1.0. (See accompanying * file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt) @@ -14,8 +14,8 @@ #define CATCH_VERSION_MAJOR 2 -#define CATCH_VERSION_MINOR 8 -#define CATCH_VERSION_PATCH 0 +#define CATCH_VERSION_MINOR 13 +#define CATCH_VERSION_PATCH 9 #ifdef __clang__ # pragma clang system_header @@ -36,9 +36,9 @@ # pragma clang diagnostic ignored "-Wcovered-switch-default" # endif #elif defined __GNUC__ - // Because REQUIREs trigger GCC's -Wparentheses, and because still - // supported version of g++ have only buggy support for _Pragmas, - // Wparentheses have to be suppressed globally. +// Because REQUIREs trigger GCC's -Wparentheses, and because still +// supported version of g++ have only buggy support for _Pragmas, +// Wparentheses have to be suppressed globally. # pragma GCC diagnostic ignored "-Wparentheses" // See #674 for details # pragma GCC diagnostic push @@ -66,13 +66,16 @@ #if !defined(CATCH_CONFIG_IMPL_ONLY) // start catch_platform.h +// See e.g.: +// https://opensource.apple.com/source/CarbonHeaders/CarbonHeaders-18.1/TargetConditionals.h.auto.html #ifdef __APPLE__ -# include -# if TARGET_OS_OSX == 1 -# define CATCH_PLATFORM_MAC -# elif TARGET_OS_IPHONE == 1 -# define CATCH_PLATFORM_IPHONE -# endif +# include +# if (defined(TARGET_OS_OSX) && TARGET_OS_OSX == 1) || \ + (defined(TARGET_OS_MAC) && TARGET_OS_MAC == 1) +# define CATCH_PLATFORM_MAC +# elif (defined(TARGET_OS_IPHONE) && TARGET_OS_IPHONE == 1) +# define CATCH_PLATFORM_IPHONE +# endif #elif defined(linux) || defined(__linux) || defined(__linux__) # define CATCH_PLATFORM_LINUX @@ -132,49 +135,64 @@ namespace Catch { #endif -#if defined(CATCH_CPP17_OR_GREATER) -# define CATCH_INTERNAL_CONFIG_CPP17_UNCAUGHT_EXCEPTIONS +// Only GCC compiler should be used in this block, so other compilers trying to +// mask themselves as GCC should be ignored. +#if defined(__GNUC__) && !defined(__clang__) && !defined(__ICC) && !defined(__CUDACC__) && !defined(__LCC__) +# define CATCH_INTERNAL_START_WARNINGS_SUPPRESSION _Pragma( "GCC diagnostic push" ) +# define CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION _Pragma( "GCC diagnostic pop" ) + +# define CATCH_INTERNAL_IGNORE_BUT_WARN(...) (void)__builtin_constant_p(__VA_ARGS__) + #endif -#ifdef __clang__ +#if defined(__clang__) + +# define CATCH_INTERNAL_START_WARNINGS_SUPPRESSION _Pragma( "clang diagnostic push" ) +# define CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION _Pragma( "clang diagnostic pop" ) + +// As of this writing, IBM XL's implementation of __builtin_constant_p has a bug +// which results in calls to destructors being emitted for each temporary, +// without a matching initialization. In practice, this can result in something +// like `std::string::~string` being called on an uninitialized value. +// +// For example, this code will likely segfault under IBM XL: +// ``` +// REQUIRE(std::string("12") + "34" == "1234") +// ``` +// +// Therefore, `CATCH_INTERNAL_IGNORE_BUT_WARN` is not implemented. +# if !defined(__ibmxl__) && !defined(__CUDACC__) +# define CATCH_INTERNAL_IGNORE_BUT_WARN(...) (void)__builtin_constant_p(__VA_ARGS__) /* NOLINT(cppcoreguidelines-pro-type-vararg, hicpp-vararg) */ +# endif + +# define CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wexit-time-destructors\"" ) \ + _Pragma( "clang diagnostic ignored \"-Wglobal-constructors\"") + +# define CATCH_INTERNAL_SUPPRESS_PARENTHESES_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wparentheses\"" ) + +# define CATCH_INTERNAL_SUPPRESS_UNUSED_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wunused-variable\"" ) + +# define CATCH_INTERNAL_SUPPRESS_ZERO_VARIADIC_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wgnu-zero-variadic-macro-arguments\"" ) -# define CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS \ - _Pragma( "clang diagnostic push" ) \ - _Pragma( "clang diagnostic ignored \"-Wexit-time-destructors\"" ) \ - _Pragma( "clang diagnostic ignored \"-Wglobal-constructors\"") -# define CATCH_INTERNAL_UNSUPPRESS_GLOBALS_WARNINGS \ - _Pragma( "clang diagnostic pop" ) - -# define CATCH_INTERNAL_SUPPRESS_PARENTHESES_WARNINGS \ - _Pragma( "clang diagnostic push" ) \ - _Pragma( "clang diagnostic ignored \"-Wparentheses\"" ) -# define CATCH_INTERNAL_UNSUPPRESS_PARENTHESES_WARNINGS \ - _Pragma( "clang diagnostic pop" ) - -# define CATCH_INTERNAL_SUPPRESS_UNUSED_WARNINGS \ - _Pragma( "clang diagnostic push" ) \ - _Pragma( "clang diagnostic ignored \"-Wunused-variable\"" ) -# define CATCH_INTERNAL_UNSUPPRESS_UNUSED_WARNINGS \ - _Pragma( "clang diagnostic pop" ) - -# define CATCH_INTERNAL_SUPPRESS_ZERO_VARIADIC_WARNINGS \ - _Pragma( "clang diagnostic push" ) \ - _Pragma( "clang diagnostic ignored \"-Wgnu-zero-variadic-macro-arguments\"" ) -# define CATCH_INTERNAL_UNSUPPRESS_ZERO_VARIADIC_WARNINGS \ - _Pragma( "clang diagnostic pop" ) +# define CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wunused-template\"" ) #endif // __clang__ //////////////////////////////////////////////////////////////////////////////// // Assume that non-Windows platforms support posix signals by default #if !defined(CATCH_PLATFORM_WINDOWS) - #define CATCH_INTERNAL_CONFIG_POSIX_SIGNALS +#define CATCH_INTERNAL_CONFIG_POSIX_SIGNALS #endif //////////////////////////////////////////////////////////////////////////////// // We know some environments not to support full POSIX signals #if defined(__CYGWIN__) || defined(__QNX__) || defined(__EMSCRIPTEN__) || defined(__DJGPP__) - #define CATCH_INTERNAL_CONFIG_NO_POSIX_SIGNALS +#define CATCH_INTERNAL_CONFIG_NO_POSIX_SIGNALS #endif #ifdef __OS400__ @@ -186,6 +204,7 @@ namespace Catch { // Android somehow still does not support std::to_string #if defined(__ANDROID__) # define CATCH_INTERNAL_CONFIG_NO_CPP11_TO_STRING +# define CATCH_INTERNAL_CONFIG_ANDROID_LOGWRITE #endif //////////////////////////////////////////////////////////////////////////////// @@ -210,20 +229,16 @@ namespace Catch { // some versions of cygwin (most) do not support std::to_string. Use the libstd check. // https://gcc.gnu.org/onlinedocs/gcc-4.8.2/libstdc++/api/a01053_source.html line 2812-2813 # if !((__cplusplus >= 201103L) && defined(_GLIBCXX_USE_C99) \ - && !defined(_GLIBCXX_HAVE_BROKEN_VSWPRINTF)) + && !defined(_GLIBCXX_HAVE_BROKEN_VSWPRINTF)) -# define CATCH_INTERNAL_CONFIG_NO_CPP11_TO_STRING +# define CATCH_INTERNAL_CONFIG_NO_CPP11_TO_STRING # endif #endif // __CYGWIN__ //////////////////////////////////////////////////////////////////////////////// // Visual C++ -#ifdef _MSC_VER - -# if _MSC_VER >= 1900 // Visual Studio 2015 or newer -# define CATCH_INTERNAL_CONFIG_CPP17_UNCAUGHT_EXCEPTIONS -# endif +#if defined(_MSC_VER) // Universal Windows platform does not support SEH // Or console colours (or console at all...) @@ -233,13 +248,25 @@ namespace Catch { # define CATCH_INTERNAL_CONFIG_WINDOWS_SEH # endif +# if !defined(__clang__) // Handle Clang masquerading for msvc + // MSVC traditional preprocessor needs some workaround for __VA_ARGS__ // _MSVC_TRADITIONAL == 0 means new conformant preprocessor // _MSVC_TRADITIONAL == 1 means old traditional non-conformant preprocessor -# if !defined(_MSVC_TRADITIONAL) || (defined(_MSVC_TRADITIONAL) && _MSVC_TRADITIONAL) -# define CATCH_INTERNAL_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR -# endif +# if !defined(_MSVC_TRADITIONAL) || (defined(_MSVC_TRADITIONAL) && _MSVC_TRADITIONAL) +# define CATCH_INTERNAL_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR +# endif // MSVC_TRADITIONAL + +// Only do this if we're not using clang on Windows, which uses `diagnostic push` & `diagnostic pop` +# define CATCH_INTERNAL_START_WARNINGS_SUPPRESSION __pragma( warning(push) ) +# define CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION __pragma( warning(pop) ) +# endif // __clang__ + +#endif // _MSC_VER +#if defined(_REENTRANT) || defined(_MSC_VER) +// Enable async processing, as -pthread is specified or no additional linking is required +# define CATCH_INTERNAL_CONFIG_USE_ASYNC #endif // _MSC_VER //////////////////////////////////////////////////////////////////////////////// @@ -257,7 +284,7 @@ namespace Catch { //////////////////////////////////////////////////////////////////////////////// // Embarcadero C++Build #if defined(__BORLANDC__) - #define CATCH_INTERNAL_CONFIG_POLYFILL_ISNAN +#define CATCH_INTERNAL_CONFIG_POLYFILL_ISNAN #endif //////////////////////////////////////////////////////////////////////////////// @@ -268,44 +295,60 @@ namespace Catch { // Otherwise all supported compilers support COUNTER macro, // but user still might want to turn it off #if ( !defined(__JETBRAINS_IDE__) || __JETBRAINS_IDE__ >= 20170300L ) - #define CATCH_INTERNAL_CONFIG_COUNTER +#define CATCH_INTERNAL_CONFIG_COUNTER #endif //////////////////////////////////////////////////////////////////////////////// -// Check if string_view is available and usable -// The check is split apart to work around v140 (VS2015) preprocessor issue... + +// RTX is a special version of Windows that is real time. +// This means that it is detected as Windows, but does not provide +// the same set of capabilities as real Windows does. +#if defined(UNDER_RTSS) || defined(RTX64_BUILD) +#define CATCH_INTERNAL_CONFIG_NO_WINDOWS_SEH + #define CATCH_INTERNAL_CONFIG_NO_ASYNC + #define CATCH_CONFIG_COLOUR_NONE +#endif + +#if !defined(_GLIBCXX_USE_C99_MATH_TR1) +#define CATCH_INTERNAL_CONFIG_GLOBAL_NEXTAFTER +#endif + +// Various stdlib support checks that require __has_include #if defined(__has_include) +// Check if string_view is available and usable #if __has_include() && defined(CATCH_CPP17_OR_GREATER) # define CATCH_INTERNAL_CONFIG_CPP17_STRING_VIEW #endif -#endif -//////////////////////////////////////////////////////////////////////////////// // Check if optional is available and usable -#if defined(__has_include) # if __has_include() && defined(CATCH_CPP17_OR_GREATER) # define CATCH_INTERNAL_CONFIG_CPP17_OPTIONAL # endif // __has_include() && defined(CATCH_CPP17_OR_GREATER) -#endif // __has_include -//////////////////////////////////////////////////////////////////////////////// +// Check if byte is available and usable +# if __has_include() && defined(CATCH_CPP17_OR_GREATER) +# include + # if defined(__cpp_lib_byte) && (__cpp_lib_byte > 0) + # define CATCH_INTERNAL_CONFIG_CPP17_BYTE + # endif +# endif // __has_include() && defined(CATCH_CPP17_OR_GREATER) + // Check if variant is available and usable -#if defined(__has_include) # if __has_include() && defined(CATCH_CPP17_OR_GREATER) # if defined(__clang__) && (__clang_major__ < 8) - // work around clang bug with libstdc++ https://bugs.llvm.org/show_bug.cgi?id=31852 - // fix should be in clang 8, workaround in libstdc++ 8.2 -# include -# if defined(__GLIBCXX__) && defined(_GLIBCXX_RELEASE) && (_GLIBCXX_RELEASE < 9) -# define CATCH_CONFIG_NO_CPP17_VARIANT -# else -# define CATCH_INTERNAL_CONFIG_CPP17_VARIANT -# endif // defined(__GLIBCXX__) && defined(_GLIBCXX_RELEASE) && (_GLIBCXX_RELEASE < 9) -# else -# define CATCH_INTERNAL_CONFIG_CPP17_VARIANT -# endif // defined(__clang__) && (__clang_major__ < 8) + // work around clang bug with libstdc++ https://bugs.llvm.org/show_bug.cgi?id=31852 + // fix should be in clang 8, workaround in libstdc++ 8.2 + # include + # if defined(__GLIBCXX__) && defined(_GLIBCXX_RELEASE) && (_GLIBCXX_RELEASE < 9) + # define CATCH_CONFIG_NO_CPP17_VARIANT + # else + # define CATCH_INTERNAL_CONFIG_CPP17_VARIANT + # endif // defined(__GLIBCXX__) && defined(_GLIBCXX_RELEASE) && (_GLIBCXX_RELEASE < 9) + # else + # define CATCH_INTERNAL_CONFIG_CPP17_VARIANT + # endif // defined(__clang__) && (__clang_major__ < 8) # endif // __has_include() && defined(CATCH_CPP17_OR_GREATER) -#endif // __has_include +#endif // defined(__has_include) #if defined(CATCH_INTERNAL_CONFIG_COUNTER) && !defined(CATCH_CONFIG_NO_COUNTER) && !defined(CATCH_CONFIG_COUNTER) # define CATCH_CONFIG_COUNTER @@ -330,10 +373,6 @@ namespace Catch { # define CATCH_CONFIG_CPP17_OPTIONAL #endif -#if defined(CATCH_INTERNAL_CONFIG_CPP17_UNCAUGHT_EXCEPTIONS) && !defined(CATCH_CONFIG_NO_CPP17_UNCAUGHT_EXCEPTIONS) && !defined(CATCH_CONFIG_CPP17_UNCAUGHT_EXCEPTIONS) -# define CATCH_CONFIG_CPP17_UNCAUGHT_EXCEPTIONS -#endif - #if defined(CATCH_INTERNAL_CONFIG_CPP17_STRING_VIEW) && !defined(CATCH_CONFIG_NO_CPP17_STRING_VIEW) && !defined(CATCH_CONFIG_CPP17_STRING_VIEW) # define CATCH_CONFIG_CPP17_STRING_VIEW #endif @@ -342,6 +381,10 @@ namespace Catch { # define CATCH_CONFIG_CPP17_VARIANT #endif +#if defined(CATCH_INTERNAL_CONFIG_CPP17_BYTE) && !defined(CATCH_CONFIG_NO_CPP17_BYTE) && !defined(CATCH_CONFIG_CPP17_BYTE) +# define CATCH_CONFIG_CPP17_BYTE +#endif + #if defined(CATCH_CONFIG_EXPERIMENTAL_REDIRECT) # define CATCH_INTERNAL_CONFIG_NEW_CAPTURE #endif @@ -358,21 +401,53 @@ namespace Catch { # define CATCH_CONFIG_POLYFILL_ISNAN #endif +#if defined(CATCH_INTERNAL_CONFIG_USE_ASYNC) && !defined(CATCH_INTERNAL_CONFIG_NO_ASYNC) && !defined(CATCH_CONFIG_NO_USE_ASYNC) && !defined(CATCH_CONFIG_USE_ASYNC) +# define CATCH_CONFIG_USE_ASYNC +#endif + +#if defined(CATCH_INTERNAL_CONFIG_ANDROID_LOGWRITE) && !defined(CATCH_CONFIG_NO_ANDROID_LOGWRITE) && !defined(CATCH_CONFIG_ANDROID_LOGWRITE) +# define CATCH_CONFIG_ANDROID_LOGWRITE +#endif + +#if defined(CATCH_INTERNAL_CONFIG_GLOBAL_NEXTAFTER) && !defined(CATCH_CONFIG_NO_GLOBAL_NEXTAFTER) && !defined(CATCH_CONFIG_GLOBAL_NEXTAFTER) +# define CATCH_CONFIG_GLOBAL_NEXTAFTER +#endif + +// Even if we do not think the compiler has that warning, we still have +// to provide a macro that can be used by the code. +#if !defined(CATCH_INTERNAL_START_WARNINGS_SUPPRESSION) +# define CATCH_INTERNAL_START_WARNINGS_SUPPRESSION +#endif +#if !defined(CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION) +# define CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION +#endif #if !defined(CATCH_INTERNAL_SUPPRESS_PARENTHESES_WARNINGS) # define CATCH_INTERNAL_SUPPRESS_PARENTHESES_WARNINGS -# define CATCH_INTERNAL_UNSUPPRESS_PARENTHESES_WARNINGS #endif #if !defined(CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS) # define CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS -# define CATCH_INTERNAL_UNSUPPRESS_GLOBALS_WARNINGS #endif #if !defined(CATCH_INTERNAL_SUPPRESS_UNUSED_WARNINGS) # define CATCH_INTERNAL_SUPPRESS_UNUSED_WARNINGS -# define CATCH_INTERNAL_UNSUPPRESS_UNUSED_WARNINGS #endif #if !defined(CATCH_INTERNAL_SUPPRESS_ZERO_VARIADIC_WARNINGS) # define CATCH_INTERNAL_SUPPRESS_ZERO_VARIADIC_WARNINGS -# define CATCH_INTERNAL_UNSUPPRESS_ZERO_VARIADIC_WARNINGS +#endif + +// The goal of this macro is to avoid evaluation of the arguments, but +// still have the compiler warn on problems inside... +#if !defined(CATCH_INTERNAL_IGNORE_BUT_WARN) +# define CATCH_INTERNAL_IGNORE_BUT_WARN(...) +#endif + +#if defined(__APPLE__) && defined(__apple_build_version__) && (__clang_major__ < 10) +# undef CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS +#elif defined(__clang__) && (__clang_major__ < 5) +# undef CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS +#endif + +#if !defined(CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS) +# define CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS #endif #if defined(CATCH_CONFIG_DISABLE_EXCEPTIONS) @@ -409,9 +484,9 @@ std::ostream& operator<<(std::ostream&, Catch_global_namespace_dummy); namespace Catch { struct CaseSensitive { enum Choice { - Yes, - No - }; }; + Yes, + No + }; }; class NonCopyable { NonCopyable( NonCopyable const& ) = delete; @@ -428,8 +503,8 @@ namespace Catch { SourceLineInfo() = delete; SourceLineInfo( char const* _file, std::size_t _line ) noexcept - : file( _file ), - line( _line ) + : file( _file ), + line( _line ) {} SourceLineInfo( SourceLineInfo const& other ) = default; @@ -437,7 +512,7 @@ namespace Catch { SourceLineInfo( SourceLineInfo&& ) noexcept = default; SourceLineInfo& operator = ( SourceLineInfo&& ) noexcept = default; - bool empty() const noexcept; + bool empty() const noexcept { return file[0] == '\0'; } bool operator == ( SourceLineInfo const& other ) const noexcept; bool operator < ( SourceLineInfo const& other ) const noexcept; @@ -478,9 +553,10 @@ namespace Catch { } // end namespace Catch #define CATCH_REGISTER_TAG_ALIAS( alias, spec ) \ + CATCH_INTERNAL_START_WARNINGS_SUPPRESSION \ CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS \ namespace{ Catch::RegistrarForTagAliases INTERNAL_CATCH_UNIQUE_NAME( AutoRegisterTagAlias )( alias, spec, CATCH_INTERNAL_LINEINFO ); } \ - CATCH_INTERNAL_UNSUPPRESS_GLOBALS_WARNINGS + CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION // end catch_tag_alias_autoregistrar.h // start catch_test_registry.h @@ -507,6 +583,7 @@ namespace Catch { virtual std::vector const& getAllTestsSorted( IConfig const& config ) const = 0; }; + bool isThrowSafe( TestCase const& testCase, IConfig const& config ); bool matchTest( TestCase const& testCase, TestSpec const& testSpec, IConfig const& config ); std::vector filterTests( std::vector const& testCases, TestSpec const& testSpec, IConfig const& config ); std::vector const& getAllTestCasesSorted( IConfig const& config ); @@ -519,157 +596,97 @@ namespace Catch { #include #include #include +#include namespace Catch { /// A non-owning string class (similar to the forthcoming std::string_view) /// Note that, because a StringRef may be a substring of another string, - /// it may not be null terminated. c_str() must return a null terminated - /// string, however, and so the StringRef will internally take ownership - /// (taking a copy), if necessary. In theory this ownership is not externally - /// visible - but it does mean (substring) StringRefs should not be shared between - /// threads. + /// it may not be null terminated. class StringRef { public: using size_type = std::size_t; + using const_iterator = const char*; private: - friend struct StringRefTestAccess; - - char const* m_start; - size_type m_size; - - char* m_data = nullptr; - - void takeOwnership(); - static constexpr char const* const s_empty = ""; - public: // construction/ assignment - StringRef() noexcept - : StringRef( s_empty, 0 ) - {} - - StringRef( StringRef const& other ) noexcept - : m_start( other.m_start ), - m_size( other.m_size ) - {} + char const* m_start = s_empty; + size_type m_size = 0; - StringRef( StringRef&& other ) noexcept - : m_start( other.m_start ), - m_size( other.m_size ), - m_data( other.m_data ) - { - other.m_data = nullptr; - } + public: // construction + constexpr StringRef() noexcept = default; StringRef( char const* rawChars ) noexcept; - StringRef( char const* rawChars, size_type size ) noexcept - : m_start( rawChars ), - m_size( size ) + constexpr StringRef( char const* rawChars, size_type size ) noexcept + : m_start( rawChars ), + m_size( size ) {} StringRef( std::string const& stdString ) noexcept - : m_start( stdString.c_str() ), - m_size( stdString.size() ) + : m_start( stdString.c_str() ), + m_size( stdString.size() ) {} - ~StringRef() noexcept { - delete[] m_data; - } - - auto operator = ( StringRef const &other ) noexcept -> StringRef& { - delete[] m_data; - m_data = nullptr; - m_start = other.m_start; - m_size = other.m_size; - return *this; + explicit operator std::string() const { + return std::string(m_start, m_size); } - operator std::string() const; - - void swap( StringRef& other ) noexcept; - public: // operators auto operator == ( StringRef const& other ) const noexcept -> bool; - auto operator != ( StringRef const& other ) const noexcept -> bool; + auto operator != (StringRef const& other) const noexcept -> bool { + return !(*this == other); + } - auto operator[] ( size_type index ) const noexcept -> char; + auto operator[] ( size_type index ) const noexcept -> char { + assert(index < m_size); + return m_start[index]; + } public: // named queries - auto empty() const noexcept -> bool { + constexpr auto empty() const noexcept -> bool { return m_size == 0; } - auto size() const noexcept -> size_type { + constexpr auto size() const noexcept -> size_type { return m_size; } - auto numberOfCharacters() const noexcept -> size_type; + // Returns the current start pointer. If the StringRef is not + // null-terminated, throws std::domain_exception auto c_str() const -> char const*; public: // substrings and searches - auto substr( size_type start, size_type size ) const noexcept -> StringRef; + // Returns a substring of [start, start + length). + // If start + length > size(), then the substring is [start, size()). + // If start > size(), then the substring is empty. + auto substr( size_type start, size_type length ) const noexcept -> StringRef; - // Returns the current start pointer. - // Note that the pointer can change when if the StringRef is a substring - auto currentData() const noexcept -> char const*; + // Returns the current start pointer. May not be null-terminated. + auto data() const noexcept -> char const*; - private: // ownership queries - may not be consistent between calls - auto isOwned() const noexcept -> bool; - auto isSubstring() const noexcept -> bool; - }; + constexpr auto isNullTerminated() const noexcept -> bool { + return m_start[m_size] == '\0'; + } - auto operator + ( StringRef const& lhs, StringRef const& rhs ) -> std::string; - auto operator + ( StringRef const& lhs, char const* rhs ) -> std::string; - auto operator + ( char const* lhs, StringRef const& rhs ) -> std::string; + public: // iterators + constexpr const_iterator begin() const { return m_start; } + constexpr const_iterator end() const { return m_start + m_size; } + }; auto operator += ( std::string& lhs, StringRef const& sr ) -> std::string&; auto operator << ( std::ostream& os, StringRef const& sr ) -> std::ostream&; - inline auto operator "" _sr( char const* rawChars, std::size_t size ) noexcept -> StringRef { + constexpr auto operator "" _sr( char const* rawChars, std::size_t size ) noexcept -> StringRef { return StringRef( rawChars, size ); } - } // namespace Catch -inline auto operator "" _catch_sr( char const* rawChars, std::size_t size ) noexcept -> Catch::StringRef { +constexpr auto operator "" _catch_sr( char const* rawChars, std::size_t size ) noexcept -> Catch::StringRef { return Catch::StringRef( rawChars, size ); } // end catch_stringref.h -// start catch_type_traits.hpp - - -#include - -namespace Catch{ - -#ifdef CATCH_CPP17_OR_GREATER - template - inline constexpr auto is_unique = std::true_type{}; - - template - inline constexpr auto is_unique = std::bool_constant< - (!std::is_same_v && ...) && is_unique - >{}; -#else - -template -struct is_unique : std::true_type{}; - -template -struct is_unique : std::integral_constant -::value - && is_unique::value - && is_unique::value ->{}; - -#endif -} - -// end catch_type_traits.hpp // start catch_preprocessor.hpp @@ -754,7 +771,7 @@ struct is_unique : std::integral_constant #define INTERNAL_CATCH_REMOVE_PARENS_4_ARG(_0, _1, _2, _3) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_3_ARG(_1, _2, _3) #define INTERNAL_CATCH_REMOVE_PARENS_5_ARG(_0, _1, _2, _3, _4) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_4_ARG(_1, _2, _3, _4) #define INTERNAL_CATCH_REMOVE_PARENS_6_ARG(_0, _1, _2, _3, _4, _5) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_5_ARG(_1, _2, _3, _4, _5) -#define INTERNAL_CATCH_REMOVE_PARENS_7_ARG(_0, _1, _2, _3, _4, _5, _6) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_6_ARG(_1, _2, _4, _5, _6) +#define INTERNAL_CATCH_REMOVE_PARENS_7_ARG(_0, _1, _2, _3, _4, _5, _6) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_6_ARG(_1, _2, _3, _4, _5, _6) #define INTERNAL_CATCH_REMOVE_PARENS_8_ARG(_0, _1, _2, _3, _4, _5, _6, _7) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_7_ARG(_1, _2, _3, _4, _5, _6, _7) #define INTERNAL_CATCH_REMOVE_PARENS_9_ARG(_0, _1, _2, _3, _4, _5, _6, _7, _8) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_8_ARG(_1, _2, _3, _4, _5, _6, _7, _8) #define INTERNAL_CATCH_REMOVE_PARENS_10_ARG(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_9_ARG(_1, _2, _3, _4, _5, _6, _7, _8, _9) @@ -766,31 +783,49 @@ struct is_unique : std::integral_constant template struct TypeList {};\ template\ constexpr auto get_wrapper() noexcept -> TypeList { return {}; }\ + template class...> struct TemplateTypeList{};\ + template class...Cs>\ + constexpr auto get_wrapper() noexcept -> TemplateTypeList { return {}; }\ + template\ + struct append;\ + template\ + struct rewrap;\ + template class, typename...>\ + struct create;\ + template class, typename>\ + struct convert;\ \ - template class L1, typename...E1, template class L2, typename...E2> \ - constexpr auto append(L1, L2) noexcept -> L1 { return {}; }\ + template \ + struct append { using type = T; };\ template< template class L1, typename...E1, template class L2, typename...E2, typename...Rest>\ - constexpr auto append(L1, L2, Rest...) noexcept -> decltype(append(L1{}, Rest{}...)) { return {}; }\ + struct append, L2, Rest...> { using type = typename append, Rest...>::type; };\ + template< template class L1, typename...E1, typename...Rest>\ + struct append, TypeList, Rest...> { using type = L1; };\ \ template< template class Container, template class List, typename...elems>\ - constexpr auto rewrap(List) noexcept -> TypeList> { return {}; }\ + struct rewrap, List> { using type = TypeList>; };\ template< template class Container, template class List, class...Elems, typename...Elements>\ - constexpr auto rewrap(List,Elements...) noexcept -> decltype(append(TypeList>{}, rewrap(Elements{}...))) { return {}; }\ + struct rewrap, List, Elements...> { using type = typename append>, typename rewrap, Elements...>::type>::type; };\ \ template