diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index 265b74f3..b5c7644d 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -2,9 +2,9 @@ name: CMake on: push: - branches: [ main ] + branches: [main] pull_request: - branches: [ main ] + branches: [main] workflow_dispatch: env: @@ -20,23 +20,32 @@ jobs: runs-on: ubuntu-20.04 steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v2 - - name: Configure CMake - # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. - # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type - run: | - cd ${{github.workspace}} - rm -rf build - mkdir -p build && cd build - cmake .. -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} + - name: Build and install library + # Configure CMake, then build and install the flexiv_rdk INTERFACE library to an external directory. + run: | + cd ${{github.workspace}} + rm -rf build && mkdir -p build && cd build + cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install + make install + + - name: Build examples + # Find and link to the flexiv_rdk INTERFACE library, then build all examples. + run: | + cd ${{github.workspace}}/example + rm -rf build && mkdir -p build && cd build + cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install + make -j$(nproc) + + - name: Build tests + # Find and link to the flexiv_rdk INTERFACE library, then build all tests. + run: | + cd ${{github.workspace}}/test + rm -rf build && mkdir -p build && cd build + cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install + make -j$(nproc) - - name: Build - # Build examples and tests - run: | - cd ${{github.workspace}}/build - make -j$(nproc) - build-ubuntu18: # The CMake configure and build commands are platform agnostic and should work equally # well on Windows or Mac. You can convert this to a matrix build if you need @@ -45,21 +54,28 @@ jobs: runs-on: ubuntu-18.04 steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v2 - - name: Configure CMake - # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. - # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type - run: | - cd ${{github.workspace}} - rm -rf build - mkdir -p build && cd build - cmake .. -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} + - name: Build and install library + # Configure CMake, then build and install the flexiv_rdk INTERFACE library to an external directory. + run: | + cd ${{github.workspace}} + rm -rf build && mkdir -p build && cd build + cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install + make install - - name: Build - # Build examples and tests - run: | - cd ${{github.workspace}}/build - make -j$(nproc) + - name: Build examples + # Find and link to the flexiv_rdk INTERFACE library, then build all examples. + run: | + cd ${{github.workspace}}/example + rm -rf build && mkdir -p build && cd build + cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install + make -j$(nproc) - \ No newline at end of file + - name: Build tests + # Find and link to the flexiv_rdk INTERFACE library, then build all tests. + run: | + cd ${{github.workspace}}/test + rm -rf build && mkdir -p build && cd build + cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install + make -j$(nproc) diff --git a/.gitignore b/.gitignore index 5e5bf233..20b2fd39 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,7 @@ .vscode .vs +.DS_Store config.h build/ -html/ \ No newline at end of file +html/ +__pycache__ diff --git a/CMakeLists.txt b/CMakeLists.txt old mode 100644 new mode 100755 index 48547647..1250a93b --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,117 +1,85 @@ -# use cmake 3 which supports targets -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 3.4) -# =================================== -# PROJECT SETUP -# =================================== -project(FlexivRdkExampleAndTest) +# =================================================================== +# PROJECT CONFIG +# =================================================================== +project(flexiv_rdk VERSION 0.6.0) -if( NOT CMAKE_BUILD_TYPE ) - set( CMAKE_BUILD_TYPE Release CACHE STRING - "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel." - FORCE ) -endif() +# C++14 required +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) -option(BUILD_FOR_ARM64 "Link to RDK library for arm64 processor, otherwise link to x64" OFF) +# Configure build type +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE) +endif() -set(CMAKE_VERBOSE_MAKEFILE ON) +# Configure processor platform +set(BUILD_PLATFORM "x86_64" CACHE STRING "Processor platform") +set_property(CACHE BUILD_PLATFORM PROPERTY STRINGS "x86_64" "arm64") + +# Set static library +message("Building for system: ${CMAKE_SYSTEM_NAME}") +if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") + if(${BUILD_PLATFORM} STREQUAL "x86_64") + set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/lib/libFlexivRdk.x86_64-linux-gnu.a") + elseif(${BUILD_PLATFORM} STREQUAL "arm64") + set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/lib/libFlexivRdk.aarch64-linux-gnu.a") + endif() +elseif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + if(${BUILD_PLATFORM} STREQUAL "x86_64") + message(FATAL_ERROR "Mac with x86_64 processor is currently not supported.") + elseif(${BUILD_PLATFORM} STREQUAL "arm64") + set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/lib/libFlexivRdk.arm64-darwin.a") + endif() +elseif(${CMAKE_SYSTEM_NAME} MATCHES "Windows") + if(${BUILD_PLATFORM} STREQUAL "x86_64") + set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/lib/FlexivRdk.x86_64-windows.lib") + elseif(${BUILD_PLATFORM} STREQUAL "arm64") + message(FATAL_ERROR "Windows with arm64 processor is currently not supported.") + endif() +endif() +# =================================================================== +# PROJECT DEPENDENCIES +# =================================================================== +# pthread set(THREADS_PREFER_PTHREAD_FLAG ON) find_package(Threads REQUIRED) -# =================================== -# CONFIGURE ALL EXAMPLES -# =================================== -# examples bin output directory -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/example) - -# list of examples -set(EXAMPLE_LIST - auto_recovery - cartesian_impedance_control - clear_fault - display_robot_states - floating_with_soft_limits - gripper_control - joint_impedance_control - joint_position_control - plan_execution - primitive_execution - robot_dynamics - series_operation - visualization -) - -foreach(example ${EXAMPLE_LIST}) - # create executable from source - add_executable(${example} example/${example}.cpp) - - # link dependencies - target_include_directories(${example} - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/include - ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/eigen3 - ) +# =================================================================== +# PROJECT LIBRARIES +# =================================================================== +# Create an INTERFACE library with no source file to compile +add_library(${PROJECT_NAME} INTERFACE) - # Link basic libraries - target_link_libraries(${example} - Threads::Threads - anl - ) +# Create an alias of the library using flexiv namespace, +# to imitate the install target which uses flexiv namespace. +add_library(flexiv::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) - # Link arm64 or x64 version of libFlexivRdk - if (${BUILD_FOR_ARM64}) - target_link_libraries(${example} - ${CMAKE_CURRENT_SOURCE_DIR}/lib/linux/cpp/arm64/libFlexivRdk.a) - else() - target_link_libraries(${example} - ${CMAKE_CURRENT_SOURCE_DIR}/lib/linux/cpp/x64/libFlexivRdk.a) - endif() - -endforeach() - - -# =================================== -# CONFIGURE ALL TESTS -# =================================== -# tests bin output directory -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/test) - -# list of tests -set(TEST_LIST - test_dynamics_engine - test_dynamics_with_tool - test_endurance - test_log - test_loop_latency - test_scheduler - test_timeliness_monitor +target_include_directories(${PROJECT_NAME} INTERFACE + $ + $ ) -foreach(test ${TEST_LIST}) - # create executable from source - add_executable(${test} test/${test}.cpp) +target_link_libraries(${PROJECT_NAME} INTERFACE + ${RDK_STATIC_LIBRARY} + Threads::Threads +) - # link dependencies - target_include_directories(${test} - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/include - ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/eigen3 - ) +# Use moderate compiler warning option +if(MSVC) + target_compile_options(${PROJECT_NAME} INTERFACE /W1) +else() + target_compile_options(${PROJECT_NAME} INTERFACE -Wall -Wextra) +endif() - # Link basic libraries - target_link_libraries(${test} - Threads::Threads - anl - ) +# Install the INTERFACE library +include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/FlexivInstallLibrary.cmake) +FlexivInstallLibrary() - # Link arm64 or x64 version of libFlexivRdk - if (${BUILD_FOR_ARM64}) - target_link_libraries(${test} - ${CMAKE_CURRENT_SOURCE_DIR}/lib/linux/cpp/arm64/libFlexivRdk.a) - else() - target_link_libraries(${test} - ${CMAKE_CURRENT_SOURCE_DIR}/lib/linux/cpp/x64/libFlexivRdk.a) - endif() - -endforeach() +# Also install Eigen headers +install( + DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/eigen3/Eigen + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) diff --git a/README.md b/README.md index 3ba4fc43..4fa38038 100644 --- a/README.md +++ b/README.md @@ -1,98 +1,94 @@ # Flexiv RDK ![CMake Badge](https://github.com/flexivrobotics/flexiv_rdk/actions/workflows/cmake.yml/badge.svg) -[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) +[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://www.apache.org/licenses/LICENSE-2.0.html) Flexiv RDK (Robotic Development Kit), a key component of the Flexiv Robotic Software Platform, is a powerful development toolkit that enables the users to create complex and customized robotic applications using APIs that provide both low-level real-time (RT) and high-level non-real-time (NRT) access to Flexiv robots. -## License - -Flexiv RDK is licensed under the [Apache 2.0 license](https://www.apache.org/licenses/LICENSE-2.0.html). - ## References -[Flexiv RDK main webpage](https://rdk.flexiv.com/) contains important information like the user manual and API documentation, please read them carefully before proceeding. +[Flexiv RDK Home Page](https://rdk.flexiv.com/) is the main reference. It contains important information including user manual and API documentation. -## OS and language support +## Compatibility Overview -Supported OS: +| **Supported OS** | **Required compiler kit** | **Supported platform** | **Supported language** | +|---------------------------|---------------------------|------------------------|------------------------| +| Ubuntu 18.04/20.04/22.04 | build-essential | x86_64, arm64 | C++, Python | +| macOS 12 (Monterey) | Xcode Command Line Tools | arm64 | C++, Python | +| Windows 10 | MSVC 14.0+ | x86_64 | C++ | -* Linux: Ubuntu 18.04 / 20.04 -* Windows: 10 with MSVC 14.0+ +## Quick Start -Supported programming languages: +**NOTE:** the full documentation is in [Flexiv RDK Manual](https://rdk.flexiv.com/manual/). -* C++ (Linux and Windows) -* Python (Linux only) +### C++ RDK -## Run example programs +The C++ interface of Flexiv RDK is packed into a unified modern CMake project named ``flexiv_rdk``, which can be configured via CMake on all supported OS. -**NOTE:** the instruction below is only a quick reference, assuming you've already gone through [Flexiv RDK Manual](https://rdk.flexiv.com/manual/). +#### Compile and install for Linux -### Linux C++ interface - -1. Configure CMake and compile all C++ example programs for the x64 processor platform: +1. In a new Terminal, use ``cmake-gui`` to configure the top-level ``flexiv_rdk`` CMake project: cd flexiv_rdk mkdir build && cd build - cmake .. - make -j4 - - If compiling for the arm64 processor platform, set the additional CMake option when configuring: - - cmake .. -DBUILD_FOR_ARM64=ON - -2. The compiled program binaries will be output to ``flexiv_rdk/build/example`` -3. Assume the robot is booted and connected, to run an example program from a Linux Terminal: + cmake-gui .. & - cd flexiv_rdk/build/example - ./ <...> +2. *Configure* with default native compiler settings. +3. Choose ``BUILD_PLATFORM`` and set ``CMAKE_INSTALL_PREFIX`` to a dedicated directory (preferably not a system path), for example ``~/rdk_install``. +4. *Configure* and *Generate*, then back to the Terminal to compile and install: - If ``flexiv::Scheduler`` is used in this program, then ``sudo`` is required to grant root privileges to the integrated scheduler: + cd flexiv_rdk/build + make install - sudo ./ <...> +5. The user project can now find and link to the installed ``flexiv_rdk`` CMake package: -### Linux Python interface + cd flexiv_rdk/example + mkdir build && cd build + cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install + make -j4 -**NOTE:** Python 3.8 is required to use this interface, see Flexiv RDK Manual for more details. + Note: ``CMAKE_INSTALL_PREFIX`` is set to the same directory where ``flexiv_rdk`` was installed to. +6. Assuming the system setup detailed in the Flexiv RDK Manual is done, to run an compiled example program: -1. Make sure your Python version is 3.8.x: + ./ [robot_ip] [local_ip] [...] - python3 --version + Note: ``sudo`` is not required unless prompted by the program. -2. Assume the robot is booted and connected, to run an example program from a Linux Terminal: +#### Compile and install for Mac - cd flexiv_rdk/example_py - python3 .py <...> +1. In a Terminal, use ``xcode-select`` command to invoke the installation of Xcode Command Line Tools, then follow the prompted window to finish the installation. +2. Download ``cmake-3.x.x-macos-universal.dmg`` from [CMake download page](https://cmake.org/download/) and install the dmg file. The minimum required version is 3.4. +3. When done, start CMake from Launchpad and navigate to Tools -> How to Install For Command Line Use. Then follow the instruction "Or, to install symlinks to '/usr/local/bin', run:" to install ``cmake`` and ``cmake-gui`` command for use in Terminal. +4. The rest steps are the same as mentioned in [Compile and install for Linux](#compile-and-install-for-linux). - Note that ``sudo`` is not required for Python interface. +#### Compile and install for Windows -### Windows C++ interface +1. Install any edition of Microsoft Visual Studio with version 2010 or above. Choose the "Desktop development with C++" package during installation. +2. Download ``cmake-3.x.x-windows-x86_64.msi`` from [CMake download page](https://cmake.org/download/) and install the msi file. The minimum required version is 3.4. **Add CMake to system PATH** when prompted, so that ``cmake`` and ``cmake-gui`` command can be used from Command Prompt or PowerShell. +3. Configure the ``flexiv_rdk`` CMake project using the same steps mentioned in [Compile and install for Linux](#compile-and-install-for-linux). +4. Instead of ``make install``, use ``cmake`` to compile and install it: -1. Install any edition of Microsoft Visual Studio with version 2010 or above. -2. Use Visual Studio to open the example solution file at ``flexiv_rdk\example\windows\windows.sln``. Several example projects are pre-configured for x64 and x86 processor platforms, with each of them corresponding to an example under ``flexiv_rdk\example\``. -3. Check that the global solution configuration is set to **Release**. This setting can usually be found near the top menu bar. -4. Use *Build* -> *Build Solution* to compile all example projects. The compiled ``.exe`` executable binary files are output to ``flexiv_rdk\build\windows\``. -5. Assume the robot is booted and connected, to run an example program from a Windows Command Prompt: + cmake --build . --target INSTALL --config Release - cd flexiv_rdk_dev\build\windows\\Release\ - .exe <...> +5. Configure the user project (RDK example programs) using the same steps mentioned in [Compile and install for Linux](#compile-and-install-for-linux). +6. Instead of ``make``, use ``cmake`` to compile it: -## Integrated visualization + cmake --build . --config Release -Flexiv RDK has integrated visualization (only available for **Linux C++ interface**) based on Meshcat. To use this feature: +7. To run an compiled example program: -1. Install Meshcat server using ``pip``: + cd Release + .exe [robot_ip] [local_ip] [...] - pip install meshcat +### Python RDK -2. Start the Meshcat server in Terminal: +Python 3.8 is required to run Python RDK, see Flexiv RDK Manual for more details. The instruction below applies to all supported OS. - meshcat-server +1. Check Python version is 3.8.x: -3. It will provide several URLs, open ``web_url`` with a web browser. An empty scene will open up. - -4. Use APIs in ``flexiv::Visualization`` to communicate with the Meshcat server, the empty scene will be populated with robot(s) and user-defined objects if any. Refer to ``example/visualization.cpp``. + python3 --version -**Note:** only STL mesh files are supported, which are provided in ``spec/meshes``. +2. Assume the system setup detailed in Flexiv RDK Manual is done, to run an example Python program: + cd flexiv_rdk/example_py + python3 .py [robot_ip] [local_ip] [...] diff --git a/cmake/FlexivInstallLibrary.cmake b/cmake/FlexivInstallLibrary.cmake new file mode 100644 index 00000000..a33b8d60 --- /dev/null +++ b/cmake/FlexivInstallLibrary.cmake @@ -0,0 +1,95 @@ +# This macro will install ${PROJECT_NAME} to ${CMAKE_INSTALL_PREFIX} when running make install +# +# FlexivInstallLibrary() will install all subfolders of ${CMAKE_CURRENT_SOURCE_DIR}/include +# FlexivInstallLibrary(install_directories) will install only the specified install_directories +# +# Requirements: +# 1. project structure should resemble: +# project +# - README.md +# - CMakeLists.txt that calls this macro +# - cmake/${PROJECT_NAME}-config.cmake.in +# - include/subfolder/*.h or *.hpp +# 2. build the library using cmake target functions +# - add_library(${PROJECT_NAME} ...) before calling this macro +# - target_include_directories(${PROJECT_NAME} ...) +# - target_link_libraries(${PROJECT_NAME} ...) +# - target_compile_features(${PROJECT_NAME} ...) +# - target_compile_options(${PROJECT_NAME} ...) +# +# Installed files: +# - include/subfolder/*.h or *.hpp +# - lib/lib{PROJECT_NAME} +# - lib/cmake/{PROJECT_NAME}/ + +macro(FlexivInstallLibrary) + # copy the executables and libraries to the CMAKE_INSTALL_PREFIX DIRECTORY + # GNUInstallDirs will set CMAKE_INSTALL* to be the standard relative paths + include(GNUInstallDirs) + install(TARGETS ${PROJECT_NAME} + EXPORT "${PROJECT_NAME}-targets" + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) + + if(${ARGC} EQUAL 0) + # install all subfolders of ${CMAKE_CURRENT_SOURCE_DIR}/include + file(GLOB install_directories ${CMAKE_CURRENT_SOURCE_DIR}/include/*) + foreach(install_directory ${install_directories}) + if(IS_DIRECTORY ${install_directory}) + install(DIRECTORY ${install_directory} + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} + FILES_MATCHING + PATTERN "*.h" + PATTERN "*.hpp" + ) + endif() + endforeach() + elseif(${ARGC} EQUAL 1) + # install specified directories only + foreach(install_directory ${ARGV0}) + install(DIRECTORY ${install_directory} + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} + FILES_MATCHING + PATTERN "*.h" + PATTERN "*.hpp" + ) + endforeach() + else() + message(FATAL_ERROR "FlexivInstallLibrary take 0 or 1 argument, but given ${ARGC}") + endif() + + # Create a *config-version.cmake file so that find_package can have a version specified + include(CMakePackageConfigHelpers) + write_basic_package_version_file( + "${PROJECT_NAME}-config-version.cmake" + VERSION ${PACKAGE_VERSION} + COMPATIBILITY AnyNewerVersion + ) + + # copy the *-targets.cmake file to the CMAKE_INSTALL_PREFIX directory + install(EXPORT "${PROJECT_NAME}-targets" + FILE "${PROJECT_NAME}-targets.cmake" + NAMESPACE "flexiv::" + DESTINATION "lib/cmake/${PROJECT_NAME}" + ) + + # copy the *.-config file to the CMAKE_INSTALL_PREFIX directory. This will specify the dependencies. + configure_file("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${PROJECT_NAME}-config.cmake.in" "${PROJECT_NAME}-config.cmake" @ONLY) + install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config-version.cmake" + DESTINATION "lib/cmake/${PROJECT_NAME}" + ) + + # Use the CPack Package Generator + set(CPACK_PACKAGE_VENDOR "Flexiv") + set(CPACK_PACKAGE_CONTACT "support@flexiv.com") + set(CPACK_PACKAGE_DESCRIPTION "Flexiv Robotic Development Kit (RDK)") + set(CPACK_PACKAGE_VERSION_MAJOR ${PROJECT_VERSION_MAJOR}) + set(CPACK_PACKAGE_VERSION_MINOR ${PROJECT_VERSION_MINOR}) + set(CPACK_PACKAGE_VERSION_PATCH ${PROJECT_VERSION_PATCH}) + set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") + set(CPACK_RESOURCE_FILE_README "${CMAKE_CURRENT_SOURCE_DIR}/README.md") + include(CPack) +endmacro() \ No newline at end of file diff --git a/cmake/flexiv_rdk-config.cmake.in b/cmake/flexiv_rdk-config.cmake.in new file mode 100644 index 00000000..76070511 --- /dev/null +++ b/cmake/flexiv_rdk-config.cmake.in @@ -0,0 +1,8 @@ +include(CMakeFindDependencyMacro) + +# Find dependency +set(THREADS_PREFER_PTHREAD_FLAG ON) +find_dependency(Threads REQUIRED) + +# Add targets file +include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake") diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in index df498bf1..bbb18409 100755 --- a/doc/Doxyfile.in +++ b/doc/Doxyfile.in @@ -38,7 +38,7 @@ PROJECT_NAME = "Flexiv RDK APIs" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = "0.5" +PROJECT_NUMBER = "0.6" # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a @@ -795,7 +795,7 @@ INPUT = README.md \ example \ include \ lib \ - spec \ + resources \ test # This tag can be used to specify the character encoding of the source files diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt new file mode 100644 index 00000000..8a81f32b --- /dev/null +++ b/example/CMakeLists.txt @@ -0,0 +1,52 @@ + +cmake_minimum_required(VERSION 3.4) +project(flexiv_rdk-examples) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_VERBOSE_MAKEFILE ON) +message("Building for system: ${CMAKE_SYSTEM_NAME}") + +# Configure build type +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE) +endif() + +# Minumum example list for Windows +set(EXAMPLE_LIST + auto_recovery + clear_fault + display_robot_states + gripper_control + plan_execution + primitive_execution + teach_by_demonstration +) + +# Additional examples for Mac +if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + set(EXAMPLE_LIST ${EXAMPLE_LIST} + robot_dynamics + ) +endif() + +# Additional examples for Linux only +if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") + set(EXAMPLE_LIST ${EXAMPLE_LIST} + cartesian_impedance_control + floating_with_soft_limits + joint_impedance_control + joint_position_control + robot_dynamics + series_operation + visualization + ) +endif() + +# Find flexiv_rdk INTERFACE library +find_package(flexiv_rdk REQUIRED) + +# Build all selected examples +foreach(example ${EXAMPLE_LIST}) + add_executable(${example} ${example}.cpp) + target_link_libraries(${example} flexiv::flexiv_rdk) +endforeach() diff --git a/example/auto_recovery.cpp b/example/auto_recovery.cpp index c64320e5..f9a02c59 100644 --- a/example/auto_recovery.cpp +++ b/example/auto_recovery.cpp @@ -11,10 +11,23 @@ #include #include #include +#include +#include #include #include +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { // Log object for printing message with timestamp and coloring @@ -22,11 +35,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // Check if program has 3 arguments - if (argc != 3) { - log.error("Invalid program arguments. Usage: "); - return 0; + if (argc < 3 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -44,14 +58,7 @@ int main(int argc, char* argv[]) // Enable the robot, make sure the E-stop is released before enabling log.info("Enabling robot ..."); - // TODO: remove this extra try catch block after the destructor bug in - // Windows library is fixed - try { - robot.enable(); - } catch (const flexiv::Exception& e) { - log.error(e.what()); - return 0; - } + robot.enable(); // Application-specific Code //============================================================================= @@ -78,7 +85,7 @@ int main(int argc, char* argv[]) } } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/example/cartesian_impedance_control.cpp b/example/cartesian_impedance_control.cpp index 43ed4c28..6237776b 100644 --- a/example/cartesian_impedance_control.cpp +++ b/example/cartesian_impedance_control.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -31,18 +32,9 @@ const double k_swingFreq = 0.3; std::string motionType = {}; } -/** Helper function to print a std::vector */ -void printVector(const std::vector& vec) -{ - for (auto i : vec) { - std::cout << i << " "; - } - std::cout << std::endl; -} - /** Callback function for realtime periodic task */ -void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, - flexiv::Scheduler* scheduler, flexiv::Log* log) +void periodicTask(flexiv::Robot* robot, flexiv::Scheduler* scheduler, + flexiv::Log* log, flexiv::RobotStates& robotStates) { // Sine counter static unsigned int loopCounter = 0; @@ -63,73 +55,68 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, // Read robot states robot->getRobotStates(robotStates); - // Use target TCP velocity and acceleration = 0 - std::vector tcpVel(6, 0); - std::vector tcpAcc(6, 0); - // Set initial TCP pose if (!isInitPoseSet) { // Check vector size before saving - if (robotStates->m_tcpPose.size() == k_cartPoseSize) { - initTcpPose = robotStates->m_tcpPose; + if (robotStates.tcpPose.size() == k_cartPoseSize) { + initTcpPose = robotStates.tcpPose; isInitPoseSet = true; - std::cout - << "Initial Cartesion pose of robot TCP set to [position " - "3x1 + rotation (quaternion) 4x1]:\n"; - printVector(initTcpPose); + log->info( + "Initial Cartesion pose of robot TCP set to [position " + "3x1 + rotation (quaternion) 4x1]: " + + flexiv::utility::vec2Str(initTcpPose)); } } // Run control only after initial pose is set else { // Set target position based on motion type if (motionType == "hold") { - robot->streamTcpPose(initTcpPose, tcpVel, tcpAcc); + robot->streamTcpPose(initTcpPose); } else if (motionType == "sine-sweep") { auto targetTcpPose = initTcpPose; targetTcpPose[1] = initTcpPose[1] + k_swingAmp * sin(2 * M_PI * k_swingFreq * loopCounter * k_loopPeriod); - robot->streamTcpPose(targetTcpPose, tcpVel, tcpAcc); - - // online change swivel angle at 2 seconds - double preferredAngleDeg = 0; - if (loopCounter % 10000 == 2000) { - preferredAngleDeg = 30; - robot->setSwivelAngle(preferredAngleDeg / 180 * M_PI); - log->info("Preferred swivel angle set to degrees: " - + std::to_string(preferredAngleDeg)); - } - - // online change stiffness to softer at 5 seconds - if (loopCounter % 10000 == 5000) { - std::vector newKp - = {2000, 2000, 2000, 200, 200, 200}; - robot->setCartesianStiffness(newKp); - log->info("Cartesian stiffness set to: "); - printVector(newKp); - } - - // online change swivel angle at 7 seconds - if (loopCounter % 10000 == 7000) { - preferredAngleDeg = -30; - robot->setSwivelAngle(preferredAngleDeg / 180 * M_PI); - log->info("Preferred swivel angle set to degrees: " - + std::to_string(preferredAngleDeg)); - } - - // online reset stiffness to original at 9 seconds - if (loopCounter % 10000 == 9000) { - robot->setCartesianStiffness(); - log->info("Cartesian stiffness is reset"); - } - - loopCounter++; + robot->streamTcpPose(targetTcpPose); } else { log->error("Unknown motion type"); log->info("Accepted motion types: hold, sine-sweep"); exit(1); } + + // online change swivel angle at 2 seconds + double preferredAngleDeg = 0; + if (loopCounter % 10000 == 2000) { + preferredAngleDeg = 10; + robot->setSwivelAngle(preferredAngleDeg / 180 * M_PI); + log->info("Preferred swivel angle set to degrees: " + + std::to_string(preferredAngleDeg)); + } + + // online change stiffness to softer at 5 seconds + if (loopCounter % 10000 == 5000) { + std::vector newKp = {2000, 2000, 2000, 200, 200, 200}; + robot->setCartesianStiffness(newKp); + log->info("Cartesian stiffness set to: " + + flexiv::utility::vec2Str(newKp)); + } + + // online change swivel angle at 7 seconds + if (loopCounter % 10000 == 7000) { + preferredAngleDeg = -10; + robot->setSwivelAngle(preferredAngleDeg / 180 * M_PI); + log->info("Preferred swivel angle set to degrees: " + + std::to_string(preferredAngleDeg)); + } + + // online reset stiffness to original at 9 seconds + if (loopCounter % 10000 == 9000) { + robot->setCartesianStiffness(); + log->info("Cartesian stiffness is reset"); + } + + loopCounter++; } } catch (const flexiv::Exception& e) { @@ -138,6 +125,18 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, } } +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << "Optional arguments: [--hold]" << std::endl; + std::cout << " --hold: robot holds current TCP pose, otherwise do a sine-sweep" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { // Log object for printing message with timestamp and coloring @@ -145,14 +144,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // Check if program has 3 arguments - if (argc != 4) { - log.error( - "Invalid program arguments. Usage: " - ""); - log.info("Accepted motion types: hold, sine-sweep"); - return 0; + if (argc < 3 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -160,7 +157,13 @@ int main(int argc, char* argv[]) std::string localIP = argv[2]; // Type of motion specified by user - motionType = argv[3]; + if (flexiv::utility::programArgsExist(argc, argv, "--hold")) { + log.info("Robot holding current TCP pose"); + motionType = "hold"; + } else { + log.info("Robot running TCP sine-sweep"); + motionType = "sine-sweep"; + } try { // RDK Initialization @@ -180,7 +183,7 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } @@ -190,8 +193,15 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational + int secondsWaited = 0; while (!robot.isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } } log.info("Robot is now operational"); @@ -213,14 +223,14 @@ int main(int argc, char* argv[]) flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority scheduler.addTask( - std::bind(periodicTask, &robot, &robotStates, &scheduler, &log), + std::bind(periodicTask, &robot, &scheduler, &log, robotStates), "HP periodic", 1, 45); // Start all added tasks, this is by default a blocking method scheduler.start(); } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/example/clear_fault.cpp b/example/clear_fault.cpp index 21c7a427..20405935 100644 --- a/example/clear_fault.cpp +++ b/example/clear_fault.cpp @@ -8,10 +8,23 @@ #include #include #include +#include +#include #include #include +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { // Log object for printing message with timestamp and coloring @@ -19,11 +32,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // Check if program has 3 arguments - if (argc != 3) { - log.error("Invalid program arguments. Usage: "); - return 0; + if (argc < 3 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -47,7 +61,7 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } else { @@ -55,7 +69,7 @@ int main(int argc, char* argv[]) } } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/example/display_robot_states.cpp b/example/display_robot_states.cpp index 9d91268a..b2b6d18b 100644 --- a/example/display_robot_states.cpp +++ b/example/display_robot_states.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -15,8 +16,6 @@ /** User-defined periodic task @ 1Hz */ void periodicTask(flexiv::Robot* robot, flexiv::Log* log) { - unsigned int printCounter = 0; - // Data struct for storing robot states flexiv::RobotStates robotStates; @@ -26,15 +25,11 @@ void periodicTask(flexiv::Robot* robot, flexiv::Log* log) std::this_thread::sleep_for(std::chrono::seconds(1)); // Get robot states - robot->getRobotStates(&robotStates); + robot->getRobotStates(robotStates); // Print all robot states in JSON format using the built-in ostream // operator overloading - std::cout << "\n\n[" << ++printCounter << "]"; - std::cout - << "=========================================================" - << std::endl; - + log->info(""); std::cout << robotStates << std::endl; } @@ -44,6 +39,17 @@ void periodicTask(flexiv::Robot* robot, flexiv::Log* log) } } +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { // Log object for printing message with timestamp and coloring @@ -51,11 +57,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // Check if program has 3 arguments - if (argc != 3) { - log.error("Invalid program arguments. Usage: "); - return 0; + if (argc < 3 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -77,25 +84,25 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } // Enable the robot, make sure the E-stop is released before enabling log.info("Enabling robot ..."); - // TODO: remove this extra try catch block after the destructor bug in - // Windows library is fixed - try { - robot.enable(); - } catch (const flexiv::Exception& e) { - log.error(e.what()); - return 0; - } + robot.enable(); // Wait for the robot to become operational + int secondsWaited = 0; while (!robot.isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } } log.info("Robot is now operational"); @@ -110,7 +117,7 @@ int main(int argc, char* argv[]) } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/example/floating_with_soft_limits.cpp b/example/floating_with_soft_limits.cpp index 15d579c7..f2046a96 100644 --- a/example/floating_with_soft_limits.cpp +++ b/example/floating_with_soft_limits.cpp @@ -10,7 +10,9 @@ #include #include #include +#include +#include #include #include @@ -24,8 +26,8 @@ const std::vector k_floatingDamping } /** Callback function for realtime periodic task */ -void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, - flexiv::Scheduler* scheduler, flexiv::Log* log) +void periodicTask(flexiv::Robot* robot, flexiv::Scheduler* scheduler, + flexiv::Log* log, flexiv::RobotStates& robotStates) { try { // Monitor fault on robot server @@ -42,7 +44,7 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, // Add some velocity damping for (size_t i = 0; i < k_robotDofs; ++i) { - torqueDesired[i] = -k_floatingDamping[i] * robotStates->m_dtheta[i]; + torqueDesired[i] = -k_floatingDamping[i] * robotStates.dtheta[i]; } // Send target joint torque to RDK server, enable gravity compensation @@ -55,6 +57,17 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, } } +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { // Log object for printing message with timestamp and coloring @@ -62,11 +75,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // Check if program has 3 arguments - if (argc != 3) { - log.error("Invalid program arguments. Usage: "); - return 0; + if (argc < 3 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -91,7 +105,7 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } @@ -101,8 +115,15 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational + int secondsWaited = 0; while (!robot.isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } } log.info("Robot is now operational"); @@ -119,14 +140,14 @@ int main(int argc, char* argv[]) flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority scheduler.addTask( - std::bind(periodicTask, &robot, &robotStates, &scheduler, &log), + std::bind(periodicTask, &robot, &scheduler, &log, robotStates), "HP periodic", 1, 45); // Start all added tasks, this is by default a blocking method scheduler.start(); } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/example/gripper_control.cpp b/example/gripper_control.cpp index 9908fe54..9dc7a254 100644 --- a/example/gripper_control.cpp +++ b/example/gripper_control.cpp @@ -10,10 +10,23 @@ #include #include #include +#include +#include #include #include +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { // Log object for printing message with timestamp and coloring @@ -21,11 +34,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // Check if program has 3 arguments - if (argc != 3) { - log.error("Invalid program arguments. Usage: "); - return 0; + if (argc < 3 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -47,25 +61,25 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } // Enable the robot, make sure the E-stop is released before enabling log.info("Enabling robot ..."); - // TODO: remove this extra try catch block after the destructor bug in - // Windows library is fixed - try { - robot.enable(); - } catch (const flexiv::Exception& e) { - log.error(e.what()); - return 0; - } + robot.enable(); // Wait for the robot to become operational + int secondsWaited = 0; while (!robot.isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } } log.info("Robot is now operational"); @@ -85,33 +99,37 @@ int main(int argc, char* argv[]) // Instantiate gripper flexiv::Gripper gripper(&robot); - // Position control - // Close to width = 0.02m, using velocity = 0.1m/s - log.info("Closing fingers"); - gripper.move(0.02, 0.1); - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Open to width = 0.08m, using velocity = 0.1m/s - log.info("Opening fingers"); - gripper.move(0.08, 0.1); - std::this_thread::sleep_for(std::chrono::seconds(1)); + // Position control test + log.info("Closing gripper"); + gripper.move(0.01, 0.1, 20); + std::this_thread::sleep_for(std::chrono::seconds(2)); + + log.info("Opening gripper"); + gripper.move(0.09, 0.1, 20); + std::this_thread::sleep_for(std::chrono::seconds(2)); + + // Stop test + log.info("Closing gripper"); + gripper.move(0.01, 0.1, 20); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + log.info("Stopping gripper"); + gripper.stop(); + std::this_thread::sleep_for(std::chrono::seconds(2)); - // Force control - // Close fingers with 10N - log.info("Grasping with constant force"); - gripper.grasp(10); - // Hold for 3 seconds - std::this_thread::sleep_for(std::chrono::seconds(3)); + log.info("Closing gripper"); + gripper.move(0.01, 0.1, 20); + std::this_thread::sleep_for(std::chrono::seconds(2)); - // Open fingers and stop halfway - log.info("Opening fingers"); - gripper.move(0.08, 0.1); + log.info("Opening gripper"); + gripper.move(0.09, 0.1, 20); std::this_thread::sleep_for(std::chrono::milliseconds(500)); log.info("Stopping gripper"); gripper.stop(); + std::this_thread::sleep_for(std::chrono::seconds(2)); } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/example/joint_impedance_control.cpp b/example/joint_impedance_control.cpp index 42742d21..2199bae4 100644 --- a/example/joint_impedance_control.cpp +++ b/example/joint_impedance_control.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -37,18 +38,9 @@ const std::vector k_sineFreq = {0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3}; std::string motionType = {}; } -/** Helper function to print a std::vector */ -void printVector(const std::vector& vec) -{ - for (auto i : vec) { - std::cout << i << " "; - } - std::cout << std::endl; -} - /** Callback function for realtime periodic task */ -void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, - flexiv::Scheduler* scheduler, flexiv::Log* log) +void periodicTask(flexiv::Robot* robot, flexiv::Scheduler* scheduler, + flexiv::Log* log, flexiv::RobotStates& robotStates) { // Sine counter static unsigned int sineCounter = 0; @@ -72,11 +64,11 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, // Set initial joint position if (!isInitPositionSet) { // Check vector size before saving - if (robotStates->m_q.size() == k_robotDofs) { - initPosition = robotStates->m_q; + if (robotStates.q.size() == k_robotDofs) { + initPosition = robotStates.q; isInitPositionSet = true; - log->info("Initial joint position set to:"); - printVector(initPosition); + log->info("Initial joint position set to: " + + flexiv::utility::vec2Str(initPosition)); } } // Run control only after init position is set @@ -106,9 +98,8 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, // Impedance control on all joints for (size_t i = 0; i < k_robotDofs; ++i) { torqueDesired[i] - = k_impedanceKp[i] - * (targetPosition[i] - robotStates->m_q[i]) - - k_impedanceKd[i] * robotStates->m_dtheta[i]; + = k_impedanceKp[i] * (targetPosition[i] - robotStates.q[i]) + - k_impedanceKd[i] * robotStates.dtheta[i]; } // Send target joint torque to RDK server @@ -121,6 +112,18 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, } } +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << "Optional arguments: [--hold]" << std::endl; + std::cout << " --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { // Log object for printing message with timestamp and coloring @@ -128,14 +131,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // Check if program has 3 arguments - if (argc != 4) { - log.error( - "Invalid program arguments. Usage: " - ""); - log.info("Accepted motion types: hold, sine-sweep"); - return 0; + if (argc < 3 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -143,7 +144,13 @@ int main(int argc, char* argv[]) std::string localIP = argv[2]; // Type of motion specified by user - motionType = argv[3]; + if (flexiv::utility::programArgsExist(argc, argv, "--hold")) { + log.info("Robot holding current pose"); + motionType = "hold"; + } else { + log.info("Robot running joint sine-sweep"); + motionType = "sine-sweep"; + } try { // RDK Initialization @@ -163,7 +170,7 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } @@ -173,8 +180,15 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational + int secondsWaited = 0; while (!robot.isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } } log.info("Robot is now operational"); @@ -191,14 +205,14 @@ int main(int argc, char* argv[]) flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority scheduler.addTask( - std::bind(periodicTask, &robot, &robotStates, &scheduler, &log), + std::bind(periodicTask, &robot, &scheduler, &log, robotStates), "HP periodic", 1, 45); // Start all added tasks, this is by default a blocking method scheduler.start(); } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/example/joint_position_control.cpp b/example/joint_position_control.cpp index 0b39a3c9..b5064860 100644 --- a/example/joint_position_control.cpp +++ b/example/joint_position_control.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -31,18 +32,9 @@ const std::vector k_sineFreq = {0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3}; std::string motionType = {}; } -/** Helper function to print a std::vector */ -void printVector(const std::vector& vec) -{ - for (auto i : vec) { - std::cout << i << " "; - } - std::cout << std::endl; -} - /** Callback function for realtime periodic task */ -void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, - flexiv::Scheduler* scheduler, flexiv::Log* log) +void periodicTask(flexiv::Robot* robot, flexiv::Scheduler* scheduler, + flexiv::Log* log, flexiv::RobotStates& robotStates) { // Sine counter static unsigned int sineCounter = 0; @@ -66,11 +58,11 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, // Set initial joint position if (!isInitPositionSet) { // Check vector size before saving - if (robotStates->m_q.size() == k_robotDofs) { - initPosition = robotStates->m_q; + if (robotStates.q.size() == k_robotDofs) { + initPosition = robotStates.q; isInitPositionSet = true; - log->info("Initial joint position set to:"); - printVector(initPosition); + log->info("Initial joint position set to: " + + flexiv::utility::vec2Str(initPosition)); } } // Run control only after init position is set @@ -113,6 +105,18 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, } } +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << "Optional arguments: [--hold]" << std::endl; + std::cout << " --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { // Log object for printing message with timestamp and coloring @@ -120,14 +124,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // Check if program has 3 arguments - if (argc != 4) { - log.error( - "Invalid program arguments. Usage: " - ""); - log.info("Accepted motion types: hold, sine-sweep"); - return 0; + if (argc < 3 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -135,7 +137,13 @@ int main(int argc, char* argv[]) std::string localIP = argv[2]; // Type of motion specified by user - motionType = argv[3]; + if (flexiv::utility::programArgsExist(argc, argv, "--hold")) { + log.info("Robot holding current pose"); + motionType = "hold"; + } else { + log.info("Robot running joint sine-sweep"); + motionType = "sine-sweep"; + } try { // RDK Initialization @@ -155,7 +163,7 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } @@ -165,8 +173,15 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational + int secondsWaited = 0; while (!robot.isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } } log.info("Robot is now operational"); @@ -183,14 +198,14 @@ int main(int argc, char* argv[]) flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority scheduler.addTask( - std::bind(periodicTask, &robot, &robotStates, &scheduler, &log), + std::bind(periodicTask, &robot, &scheduler, &log, robotStates), "HP periodic", 1, 45); // Start all added tasks, this is by default a blocking method scheduler.start(); } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/example/plan_execution.cpp b/example/plan_execution.cpp index 80c1af0b..9d64c418 100644 --- a/example/plan_execution.cpp +++ b/example/plan_execution.cpp @@ -8,109 +8,20 @@ #include #include #include +#include -#include #include -#include -#include #include -#include -namespace { -/** User input string */ -std::string g_userInput; - -/** User input mutex */ -std::mutex g_userInputMutex; - -/** Flag to exit main thread */ -std::atomic g_exitMain = {false}; -} - -/** Callback function for user input state machine */ -void userInputStateMachine(flexiv::Robot* robot) +void printHelp() { - // Log object for printing message with timestamp and coloring - flexiv::Log log; - - try { - // Use while loop to prevent this thread from return - while (true) { - // Wake up every 0.1 second to do something - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - // Monitor fault on robot server - if (robot->isFault()) { - throw flexiv::ServerException( - "userInputStateMachine: Fault occurred on robot server, " - "input anything to exit ..."); - } - - // Check user input - std::string inputBuffer; - { - std::lock_guard lock(g_userInputMutex); - inputBuffer = g_userInput; - } - - if (inputBuffer.empty()) { - // Do nothing, waiting for new user input or plan to finish - } - // List available plans - else if (inputBuffer == "l") { - auto planList = robot->getPlanNameList(); - std::cout << "===================== Plan name list " - "=====================" - << std::endl; - // Print plan list - for (unsigned int i = 0; i < planList.size(); i++) { - std::cout << "[" << i << "] " << planList[i] << std::endl; - } - log.info("Enter index to select a plan to execute:"); - } - // Stop plan execution - else if (inputBuffer == "s") { - robot->stop(); - log.info( - "Execution stopped, enter index to execute another plan:"); - - // After calling stop(), the robot will enter Idle mode, so put - // the robot back to plan execution mode to take more plan - // commands set mode after robot is operational - robot->setMode(flexiv::MODE_PLAN_EXECUTION); - - // Wait for the mode to be switched - while (robot->getMode() != flexiv::MODE_PLAN_EXECUTION) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - } - // Choose plan to execute - else { - // Check system status - flexiv::SystemStatus systemStatus; - robot->getSystemStatus(&systemStatus); - - // Execute new plan if no plan is running - if (systemStatus.m_programRunning == false) { - // Convert string to int - int index = std::stoi(inputBuffer); - // Start executing - robot->executePlanByIndex(index); - } - } - - // Reset user input every loop - { - std::lock_guard lock(g_userInputMutex); - g_userInput.clear(); - } - } - - } catch (const flexiv::Exception& e) { - log.error(e.what()); - g_exitMain = true; - return; - } + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on } int main(int argc, char* argv[]) @@ -120,11 +31,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // Check if program has 3 arguments - if (argc != 3) { - log.error("Invalid program arguments. Usage: "); - return 0; + if (argc < 3 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -146,25 +58,25 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } // Enable the robot, make sure the E-stop is released before enabling log.info("Enabling robot ..."); - // TODO: remove this extra try catch block after the destructor bug in - // Windows library is fixed - try { - robot.enable(); - } catch (const flexiv::Exception& e) { - log.error(e.what()); - return 0; - } + robot.enable(); // Wait for the robot to become operational + int secondsWaited = 0; while (!robot.isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } } log.info("Robot is now operational"); @@ -176,34 +88,95 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::milliseconds(1)); } - // Periodic Tasks + // Application-specific Code //============================================================================= - // Use std::thread for some low-priority tasks, not joining - // (non-blocking) - std::thread lowPriorityThread(std::bind(userInputStateMachine, &robot)); + // Plan info data + flexiv::PlanInfo planInfo; - // User Input - //============================================================================= - log.info("Choose from the following options to continue:"); - std::cout << "[l] - show plan list" << std::endl; - std::cout << "[s] - stop execution of current plan" << std::endl; + while (true) { + // Monitor fault on robot server + if (robot.isFault()) { + throw flexiv::ServerException( + "Fault occurred on robot server, exiting ..."); + } + + // Get user input + log.info("Choose an action:"); + std::cout << "[1] Show available plans" << std::endl; + std::cout << "[2] Execute a plan by index" << std::endl; + std::cout << "[3] Execute a plan by name" << std::endl; - // User input polling - std::string inputBuffer; - while (!g_exitMain) { + std::string inputBuffer; std::cin >> inputBuffer; - { - std::lock_guard lock(g_userInputMutex); - g_userInput = inputBuffer; + int userInput = std::stoi(inputBuffer); + + switch (userInput) { + // Get and show plan list + case 1: { + auto planList = robot.getPlanNameList(); + for (size_t i = 0; i < planList.size(); i++) { + std::cout << "[" << i << "] " << planList[i] + << std::endl; + } + std::cout << std::endl; + } break; + // Execute plan by index + case 2: { + log.info("Enter plan index to execute:"); + int index; + std::cin >> index; + robot.executePlanByIndex(index); + + // Print plan info while the current plan is running + while (robot.isBusy()) { + robot.getPlanInfo(planInfo); + log.info(" "); + // clang-format off + std::cout << "assignedPlanName: " << planInfo.assignedPlanName << std::endl; + std::cout << "ptName: " << planInfo.ptName << std::endl; + std::cout << "nodeName: " << planInfo.nodeName << std::endl; + std::cout << "nodePath: " << planInfo.nodePath << std::endl; + std::cout << "nodePathTimePeriod: " << planInfo.nodePathTimePeriod << std::endl; + std::cout << "nodePathNumber: " << planInfo.nodePathNumber << std::endl; + std::cout << "velocityScale: " << planInfo.velocityScale << std::endl; + std::cout << std::endl; + // clang-format on + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + } break; + // Execute plan by name + case 3: { + log.info("Enter plan name to execute:"); + std::string name; + std::cin >> name; + robot.executePlanByName(name); + + // Print plan info while the current plan is running + while (robot.isBusy()) { + robot.getPlanInfo(planInfo); + log.info(" "); + // clang-format off + std::cout << "assignedPlanName: " << planInfo.assignedPlanName << std::endl; + std::cout << "ptName: " << planInfo.ptName << std::endl; + std::cout << "nodeName: " << planInfo.nodeName << std::endl; + std::cout << "nodePath: " << planInfo.nodePath << std::endl; + std::cout << "nodePathTimePeriod: " << planInfo.nodePathTimePeriod << std::endl; + std::cout << "nodePathNumber: " << planInfo.nodePathNumber << std::endl; + std::cout << "velocityScale: " << planInfo.velocityScale << std::endl; + std::cout << std::endl; + // clang-format on + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + } break; + default: + log.warn("Invalid input"); + break; } } - // Properly exit thread - lowPriorityThread.join(); - } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/example/primitive_execution.cpp b/example/primitive_execution.cpp index 11c6b2ac..1140c9f7 100644 --- a/example/primitive_execution.cpp +++ b/example/primitive_execution.cpp @@ -8,161 +8,20 @@ #include #include #include +#include -#include -#include #include -#include -#include -#include #include -namespace { -/** Set this to true to print primitive states */ -const bool k_printPtStates = true; -} - -/** Callback function for user-defined primitive state machine */ -void primitiveStateMachine(flexiv::Robot* robot) +void printHelp() { - // Log object for printing message with timestamp and coloring - flexiv::Log log; - - // If transitting to the next primitive - bool transitPrimitive = false; - - // Index for iterating through various primitives - unsigned int ptIndex = 1; - - try { - // Start any primitive once to pass the first transition condition check - robot->executePrimitive("Home()"); - - // Use while loop to prevent this thread from return - while (true) { - // Run user periodic tasks at 10Hz in this thread - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - // Monitor fault on robot server - if (robot->isFault()) { - throw flexiv::ServerException( - "primitiveStateMachine: Fault occurred on robot server, " - "exiting ..."); - } - - // Reset transition flag before checking for transition conditions - transitPrimitive = false; - - // Get system status - std::vector ptStates = robot->getPrimitiveStates(); - - // Check for transition conditions - for (const auto& state : ptStates) { - // Parse key information from the string list for transition - // control - std::stringstream ss(state); - std::string buffer; - std::vector parsedState; - while (ss >> buffer) { - parsedState.push_back(buffer); - } - - // Transition condition: "reachedTarget = 1" - if (parsedState.front() == "reachedTarget") { - // Save the corresponding value, it should be at the end - if (std::stoi(parsedState.back()) == 1) { - transitPrimitive = true; - } - } - } - - // Iterate through commanding various primitives - if (transitPrimitive) { - switch (ptIndex) { - case 0: { - // (1) Go home, all parameters of the "Home" primitive - // are optional, thus we can skip the parameters and the - // default values will be used - robot->executePrimitive("Home()"); - - // Execute the next primitive - ptIndex++; - - break; - } - case 1: { - // (2) Move TCP to point A in world (base) frame. The - // mandatory parameter "target" requires 8 values: [x y - // z roll pitch yaw reference_frame reference_point] - // unit: meters and degrees - robot->executePrimitive( - "MoveL(target=0.387 -0.11 0.203 180.0 0.0 180.0 " - "WORLD " - "WORLD_ORIGIN)"); - - // Execute the next primitive - ptIndex++; - - break; - } - case 2: { - // (3) Move TCP to point B in world (base) frame - robot->executePrimitive( - "MoveL(target=0.687 0.0 0.264 180.0 0.0 180.0 " - "WORLD " - "WORLD_ORIGIN)"); - - // Execute the next primitive - ptIndex++; - - break; - } - case 3: { - // (4) Move TCP to point C in world (base) frame - robot->executePrimitive( - "MoveL(target=0.387 0.1 0.1 -90.0 0.0 180.0 WORLD " - "WORLD_ORIGIN)"); - - // Execute the next primitive - ptIndex++; - break; - } - case 4: { - // (5) Move TCP to point D in world (base) frame - robot->executePrimitive( - "MoveL(target=0.5 0.0 0.3 180.0 0.0 180.0 WORLD " - "WORLD_ORIGIN)"); - - // Execute the next primitive - ptIndex++; - break; - } - case 5: { - // Repeat the moves - ptIndex = 0; - - break; - } - - default: - log.error("Invalid ptIndex"); - break; - } - } - - // Print each state in the primitive states string list - if (k_printPtStates) { - for (const auto& state : ptStates) { - std::cout << state << std::endl; - } - // Empty line - std::cout << std::endl; - } - } - } catch (const flexiv::Exception& e) { - log.error(e.what()); - return; - } + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on } int main(int argc, char* argv[]) @@ -172,11 +31,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // Check if program has 3 arguments - if (argc != 3) { - log.error("Invalid program arguments. Usage: "); - return 0; + if (argc < 3 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -201,25 +61,25 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } // Enable the robot, make sure the E-stop is released before enabling log.info("Enabling robot ..."); - // TODO: remove this extra try catch block after the destructor bug in - // Windows library is fixed - try { - robot.enable(); - } catch (const flexiv::Exception& e) { - log.error(e.what()); - return 0; - } + robot.enable(); // Wait for the robot to become operational + int secondsWaited = 0; while (!robot.isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } } log.info("Robot is now operational"); @@ -231,18 +91,104 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::milliseconds(1)); } - // Periodic Tasks + // Application-specific Code //============================================================================= - // Use std::thread for some low-priority tasks, not joining - // (non-blocking) - std::thread lowPriorityThread(std::bind(primitiveStateMachine, &robot)); - // Wait for thread to finish - lowPriorityThread.join(); + // (1) Go to home pose + // ---------------------------------------------------------------------------- + // All parameters of the "Home" primitive are optional, + // thus we can skip the parameters and the default values will be used + log.info("Executing primitive: Home"); + + // Send command to robot + robot.executePrimitive("Home()"); + + // Wait for robot to reach target location by checking for + // "reachedTarget = 1" in the list of current primitive states + while (flexiv::utility::parsePtStates( + robot.getPrimitiveStates(), "reachedTarget") + != "1") { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + // NOTE: can also use robot.isStopped() to indirectly tell if the robot + // has reached target + + // (2) Move robot joints to target positions + // ---------------------------------------------------------------------------- + // The required parameter takes in 7 target joint positions + // Unit: degrees + log.info("Executing primitive: MoveJ"); + + // Send command to robot + robot.executePrimitive("MoveJ(target=30 -45 0 90 0 40 30)"); + + // Wait for reached target + while (flexiv::utility::parsePtStates( + robot.getPrimitiveStates(), "reachedTarget") + != "1") { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + // (3) Move robot TCP to a target position in world (base) frame + // ---------------------------------------------------------------------------- + // Required parameter: + // target: final target position + // [pos_x pos_y pos_z rot_x rot_y rot_z ref_frame ref_point] + // unit: m, deg + // Optional parameter: + // waypoints: waypoints to pass before reaching final target + // [same format as above, but can repeat for number of waypoints] + // maxVel: maximum TCP linear velocity + // unit: m/s + // NOTE: The rotations are using Euler ZYX convention, rot_x means Euler + // ZYX angle around X axis + log.info("Executing primitive: MoveL"); + + // Send command to robot + robot.executePrimitive( + "MoveL(target=0.65 -0.3 0.2 180 0 180 WORLD " + "WORLD_ORIGIN,waypoints=0.45 0.1 0.2 180 0 180 WORLD " + "WORLD_ORIGIN 0.45 -0.3 0.2 180 0 180 WORLD WORLD_ORIGIN, " + "maxVel=0.2)"); + + // Wait for reached target + while (flexiv::utility::parsePtStates( + robot.getPrimitiveStates(), "reachedTarget") + != "1") { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + // (4) Another MoveL + // ---------------------------------------------------------------------------- + // An example to convert quaternion into Euler ZYX angles required by + // the primitive is shown below. + log.info("Executing primitive: MoveL"); + + // Convert target quaternion to Euler ZYX using utility functions + std::vector targetQuat + = {-0.373286, 0.0270976, 0.83113, 0.411274}; + auto targetEulerDeg = flexiv::utility::rad2Deg( + flexiv::utility::quat2EulerZYX(targetQuat)); + + // Send command to robot + robot.executePrimitive("MoveL(target=0.4 -0.1 0.2 " + + flexiv::utility::vec2Str(targetEulerDeg) + + "WORLD WORLD_ORIGIN, maxVel=0.3)"); + + // Wait for reached target + while (flexiv::utility::parsePtStates( + robot.getPrimitiveStates(), "reachedTarget") + != "1") { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + // All done, stop robot and put into IDLE mode + robot.stop(); } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/example/robot_dynamics.cpp b/example/robot_dynamics.cpp index aeb56c91..d734468a 100644 --- a/example/robot_dynamics.cpp +++ b/example/robot_dynamics.cpp @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include #include @@ -18,80 +18,94 @@ #include #include -namespace { -/** Data shared between threads */ -struct SharedData +/** Periodic task running at around 100 Hz */ +int periodicTask(flexiv::Robot* robot) { - int64_t loopTime; - Eigen::VectorXd gravity; -} g_data; - -/** Mutex on the shared data */ -std::mutex g_mutex; + // Log object for printing message with timestamp and coloring + flexiv::Log log; -} + // Data struct for storing robot states + flexiv::RobotStates robotStates; -/** User-defined high-priority periodic task @ 1kHz */ -void highPriorityTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, - flexiv::Scheduler* scheduler, flexiv::Log* log, flexiv::Model* model) -{ try { - // Monitor fault on robot server - if (robot->isFault()) { - throw flexiv::ServerException( - "highPriorityTask: Fault occurred on robot server, exiting " - "..."); - } + // Initialize robot model (dynamics engine) + flexiv::Model model(robot); + + int loopCounter = 0; + while (true) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + loopCounter++; + + // Monitor fault on robot server + if (robot->isFault()) { + throw flexiv::ServerException( + "periodicTask: Fault occurred on robot server, exiting " + "..."); + } - // Mark timer start point - auto tic = std::chrono::high_resolution_clock::now(); + // Mark timer start point + auto tic = std::chrono::high_resolution_clock::now(); - // Read robot states - robot->getRobotStates(robotStates); + // Read robot states + robot->getRobotStates(robotStates); - // Update robot model in dynamics engine - model->updateModel(robotStates->m_q, robotStates->m_dtheta); + // Update robot model in dynamics engine + model.updateModel(robotStates.q, robotStates.dtheta); - // Get and print gravity vector - auto gravity = model->getGravityForce(); + // Compute gravity vector + auto g = model.getGravityForce(); - // Mark timer end point and get loop time - auto toc = std::chrono::high_resolution_clock::now(); - auto loopTime - = std::chrono::duration_cast(toc - tic) - .count(); + // Compute mass matrix + auto M = model.getMassMatrix(); - // Save to global struct for printing in another thread - { - std::lock_guard lock(g_mutex); - g_data.loopTime = loopTime; - g_data.gravity = gravity; - } + // Compute Jacobian + auto J = model.getJacobian("flange"); + // Mark timer end point and get loop time + auto toc = std::chrono::high_resolution_clock::now(); + auto computeTime + = std::chrono::duration_cast( + toc - tic) + .count(); + + // Print at 1Hz + if (loopCounter % 100 == 0) { + // Print time used to compute g, M, J + log.info("Computation time = " + std::to_string(computeTime) + + " us"); + std::cout << std::endl; + + // Print gravity + std::cout << std::fixed << std::setprecision(5) + << "g = " << g.transpose() << "\n" + << std::endl; + + // Print mass matrix + std::cout << std::fixed << std::setprecision(5) << "M = " << M + << "\n" + << std::endl; + + // Print Jacobian + std::cout << std::fixed << std::setprecision(5) << "J = " << J + << "\n" + << std::endl; + } + } } catch (const flexiv::Exception& e) { - log->error(e.what()); - scheduler->stop(); + log.error(e.what()); + return 1; } } -/** User-defined low-priority periodic task @ 1Hz */ -void lowPriorityTask() +void printHelp() { - // Safely read shared data - int loopTime; - Eigen::VectorXd gravity; - { - std::lock_guard lock(g_mutex); - loopTime = g_data.loopTime; - gravity = g_data.gravity; - } - - // Print time interval of high-priority periodic task - std::cout << "Loop time = " << loopTime << " us || "; - - // Print gravity - std::cout << std::fixed << std::setprecision(5) - << "Gravity = " << gravity.transpose() << std::endl; + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on } int main(int argc, char* argv[]) @@ -101,11 +115,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // Check if program has 3 arguments - if (argc != 3) { - log.error("Invalid program arguments. Usage: "); - return 0; + if (argc < 3 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -118,9 +133,6 @@ int main(int argc, char* argv[]) // Instantiate robot interface flexiv::Robot robot(robotIP, localIP); - // Create data struct for storing robot states - flexiv::RobotStates robotStates; - // Clear fault on robot server if any if (robot.isFault()) { log.warn("Fault occurred on robot server, trying to clear ..."); @@ -130,7 +142,7 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } @@ -140,8 +152,15 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational + int secondsWaited = 0; while (!robot.isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } } log.info("Robot is now operational"); @@ -153,47 +172,22 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::milliseconds(1)); } - // Bring Robot To Home - //============================================================================= + // Bring robot to home robot.executePlanByName("PLAN-Home"); - flexiv::SystemStatus systemStatus; - // Wait for the plan to start - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Wait for the execution to finish do { - robot.getSystemStatus(&systemStatus); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } while (systemStatus.m_programRunning); - - // Put mode back to IDLE - robot.setMode(flexiv::MODE_IDLE); - - // Wait for the mode to be switched - while (robot.getMode() != flexiv::MODE_IDLE) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - - // Robot Model (Dynamics Engine) Initialization - //============================================================================= - flexiv::Model model(&robot); + std::this_thread::sleep_for(std::chrono::seconds(1)); + } while (robot.isBusy()); // Periodic Tasks //============================================================================= - flexiv::Scheduler scheduler; - // Add periodic task with 1ms interval and highest applicable priority - scheduler.addTask(std::bind(highPriorityTask, &robot, &robotStates, - &scheduler, &log, &model), - "HP periodic", 1, 45); - // Add periodic task with 1s interval and lowest applicable priority - scheduler.addTask(lowPriorityTask, "LP periodic", 1000, 0); - // Start all added tasks, this is by default a blocking method - scheduler.start(); + std::thread periodicTaskThread(periodicTask, &robot); + periodicTaskThread.join(); } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/example/series_operation.cpp b/example/series_operation.cpp index 91e0559f..7e729f67 100644 --- a/example/series_operation.cpp +++ b/example/series_operation.cpp @@ -10,7 +10,9 @@ #include #include #include +#include +#include #include #include #include @@ -67,8 +69,8 @@ const std::string OperationNames[OP_NUM] = {"Idle", "Stop robot", "Go home", } /** Callback function for realtime periodic task */ -void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, - flexiv::Scheduler* scheduler, flexiv::Log* log) +void periodicTask(flexiv::Robot* robot, flexiv::Scheduler* scheduler, + flexiv::Log* log, flexiv::RobotStates& robotStates) { // Sine counter static unsigned int sineCounter = 0; @@ -193,8 +195,8 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, robot->setMode(flexiv::MODE_JOINT_POSITION); // Set initial joint position - if (robotStates->m_q.size() == k_robotDofs) { - initJointPos = robotStates->m_q; + if (robotStates.q.size() == k_robotDofs) { + initJointPos = robotStates.q; } else { log->error("Invalid size of received joint position"); // Stop robot and terminate program @@ -243,8 +245,8 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, robot->setMode(flexiv::MODE_JOINT_TORQUE); // Set initial joint position - if (robotStates->m_q.size() == k_robotDofs) { - initJointPos = robotStates->m_q; + if (robotStates.q.size() == k_robotDofs) { + initJointPos = robotStates.q; } else { log->error("Invalid size of received joint position"); // Stop robot and terminate program @@ -271,8 +273,8 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, for (size_t i = 0; i < k_robotDofs; ++i) { torqueDesired[i] = k_impedanceKp[i] - * (targetPosition[i] - robotStates->m_q[i]) - - k_impedanceKd[i] * robotStates->m_dtheta[i]; + * (targetPosition[i] - robotStates.q[i]) + - k_impedanceKd[i] * robotStates.dtheta[i]; } // Send command @@ -297,8 +299,8 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, robot->setMode(flexiv::MODE_CARTESIAN_IMPEDANCE); // Set initial TCP pose - if (robotStates->m_tcpPose.size() == k_cartPoseSize) { - initTcpPose = robotStates->m_tcpPose; + if (robotStates.tcpPose.size() == k_cartPoseSize) { + initTcpPose = robotStates.tcpPose; } else { log->error("Invalid size of received TCP pose"); // Stop robot and terminate program @@ -309,9 +311,6 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, sineCounter = 0; } else { auto targetTcpPose = initTcpPose; - // Use target TCP velocity and acceleration = 0 - std::vector targetTcpVel(6, 0); - std::vector targetTcpAcc(6, 0); // Sine-sweep X direction targetTcpPose[1] = initTcpPose[1] @@ -323,8 +322,7 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, sineCounter++; // Send command - robot->streamTcpPose( - targetTcpPose, targetTcpVel, targetTcpAcc); + robot->streamTcpPose(targetTcpPose); // Wait for operation period to timeout if (++opTimer >= 20 * k_secToCount) { @@ -354,6 +352,17 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, } } +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { // Log object for printing message with timestamp and coloring @@ -361,11 +370,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // Check if program has 3 arguments - if (argc != 3) { - log.error("Invalid program arguments. Usage: "); - return 0; + if (argc < 3 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -390,7 +400,7 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } @@ -400,8 +410,15 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational + int secondsWaited = 0; while (!robot.isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } } log.info("Robot is now operational"); @@ -433,14 +450,14 @@ int main(int argc, char* argv[]) flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority scheduler.addTask( - std::bind(periodicTask, &robot, &robotStates, &scheduler, &log), + std::bind(periodicTask, &robot, &scheduler, &log, robotStates), "HP periodic", 1, 45); // Start all added tasks, this is by default a blocking method scheduler.start(); } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/example/teach_by_demonstration.cpp b/example/teach_by_demonstration.cpp new file mode 100644 index 00000000..9151783a --- /dev/null +++ b/example/teach_by_demonstration.cpp @@ -0,0 +1,216 @@ +/** + * @example teach_by_demonstration.cpp + * Free-drive the robot and record a series of Cartesian poses, which are then + * reproduced by the robot. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @author Flexiv + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace { +/** User input string */ +std::string g_userInput; + +/** User input mutex */ +std::mutex g_userInputMutex; + +/** Maximum contact wrench [fx, fy, fz, mx, my, mz] [N][Nm]*/ +const std::vector k_maxContactWrench + = {50.0, 50.0, 50.0, 15.0, 15.0, 15.0}; +} + +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + +int main(int argc, char* argv[]) +{ + // Log object for printing message with timestamp and coloring + flexiv::Log log; + + // Parse Parameters + //============================================================================= + if (argc < 3 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; + } + + // IP of the robot server + std::string robotIP = argv[1]; + + // IP of the workstation PC running this program + std::string localIP = argv[2]; + + try { + // RDK Initialization + //============================================================================= + // Instantiate robot interface + flexiv::Robot robot(robotIP, localIP); + + // Clear fault on robot server if any + if (robot.isFault()) { + log.warn("Fault occurred on robot server, trying to clear ..."); + // Try to clear the fault + robot.clearFault(); + std::this_thread::sleep_for(std::chrono::seconds(2)); + // Check again + if (robot.isFault()) { + log.error("Fault cannot be cleared, exiting ..."); + return 1; + } + log.info("Fault on robot server is cleared"); + } + + // Enable the robot, make sure the E-stop is released before enabling + log.info("Enabling robot ..."); + robot.enable(); + + // Wait for the robot to become operational + int secondsWaited = 0; + while (!robot.isOperational()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } + } + log.info("Robot is now operational"); + + // Application-specific Code + // ============================================================================= + // Recorded robot poses + std::vector> savedPoses = {}; + + // Robot states data + flexiv::RobotStates robotStates = {}; + + // Acceptable user inputs + log.info("Accepted key inputs:"); + std::cout << "[n] - start new teaching process" << std::endl; + std::cout << "[r] - record current robot pose" << std::endl; + std::cout << "[e] - finish recording and start execution" << std::endl; + + // User input polling + std::string inputBuffer; + while (true) { + std::cin >> inputBuffer; + // Start new teaching process + if (inputBuffer == "n") { + // Clear storage + savedPoses.clear(); + + // Put robot to plan execution mode + robot.setMode(flexiv::MODE_PLAN_EXECUTION); + + // Wait for the mode to be switched + while (robot.getMode() != flexiv::MODE_PLAN_EXECUTION) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + // Robot run free drive + robot.executePlanByName("PLAN-FreeDriveAuto"); + + log.info("New teaching process started"); + log.warn( + "Hold down the enabling button on the motion bar to " + "activate free drive"); + } + // Save current robot pose + else if (inputBuffer == "r") { + if (!robot.isBusy()) { + log.warn("Please start a new teaching process first"); + continue; + } + + robot.getRobotStates(robotStates); + savedPoses.push_back(robotStates.tcpPose); + log.info("New pose saved: " + + flexiv::utility::vec2Str(robotStates.tcpPose)); + log.info("Number of saved poses: " + + std::to_string(savedPoses.size())); + } + // Reproduce recorded poses + else if (inputBuffer == "e") { + if (savedPoses.empty()) { + log.warn("No pose is saved yet"); + continue; + } + + // Put robot to primitive execution mode + robot.setMode(flexiv::MODE_PRIMITIVE_EXECUTION); + + // Wait for the mode to be switched + while (robot.getMode() != flexiv::MODE_PRIMITIVE_EXECUTION) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + for (size_t i = 0; i < savedPoses.size(); i++) { + log.info("Executing pose " + std::to_string(i + 1) + "/" + + std::to_string(savedPoses.size())); + + std::vector targetPos = { + savedPoses[i][0], savedPoses[i][1], savedPoses[i][2]}; + // Convert quaternion to Euler ZYX required by + // MoveCompliance primitive + std::vector targetQuat = {savedPoses[i][3], + savedPoses[i][4], savedPoses[i][5], savedPoses[i][6]}; + auto targetEulerDeg = flexiv::utility::rad2Deg( + flexiv::utility::quat2EulerZYX(targetQuat)); + robot.executePrimitive( + "MoveCompliance(target=" + + flexiv::utility::vec2Str(targetPos) + + flexiv::utility::vec2Str(targetEulerDeg) + + "WORLD WORLD_ORIGIN, maxVel=0.3, enableMaxContactWrench=1, maxContactWrench=" + + flexiv::utility::vec2Str(k_maxContactWrench)+ ")"); + + // Wait for reached target + while (flexiv::utility::parsePtStates( + robot.getPrimitiveStates(), "reachedTarget") + != "1") { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + } + + log.info( + "All saved poses are executed, enter 'n' to start a new " + "teaching process, 'r' to record more poses, 'e' to repeat " + "execution"); + + // Put robot back to free drive + robot.setMode(flexiv::MODE_PLAN_EXECUTION); + while (robot.getMode() != flexiv::MODE_PLAN_EXECUTION) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + robot.executePlanByName("PLAN-FreeDriveAuto"); + } else { + log.warn("Invalid input"); + } + } + + } catch (const flexiv::Exception& e) { + log.error(e.what()); + return 1; + } + + return 0; +} diff --git a/example/visualization.cpp b/example/visualization.cpp index 2d88eb09..e0ee4af0 100644 --- a/example/visualization.cpp +++ b/example/visualization.cpp @@ -11,7 +11,9 @@ #include #include #include +#include +#include #include #include #include @@ -41,8 +43,8 @@ std::mutex g_mutex; } /** User-defined high-priority periodic task @ 1kHz */ -void highPriorityTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, - flexiv::Scheduler* scheduler, flexiv::Log* log) +void highPriorityTask(flexiv::Robot* robot, flexiv::Scheduler* scheduler, + flexiv::Log* log, flexiv::RobotStates& robotStates) { // Sine counter static unsigned int sineCounter = 0; @@ -64,15 +66,11 @@ void highPriorityTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, // Read robot states robot->getRobotStates(robotStates); - // Use target TCP velocity and acceleration = 0 - std::vector tcpVel(6, 0); - std::vector tcpAcc(6, 0); - // Set initial TCP pose if (!isInitPoseSet) { // Check vector size before saving - if (robotStates->m_tcpPose.size() == k_cartPoseSize) { - initTcpPose = robotStates->m_tcpPose; + if (robotStates.tcpPose.size() == k_cartPoseSize) { + initTcpPose = robotStates.tcpPose; isInitPoseSet = true; } } @@ -83,14 +81,14 @@ void highPriorityTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, + k_swingAmp * sin(2 * M_PI * k_swingFreq * sineCounter * k_loopPeriod); - robot->streamTcpPose(targetTcpPose, tcpVel, tcpAcc); + robot->streamTcpPose(targetTcpPose); sineCounter++; } // Safely write shared data { std::lock_guard lock(g_mutex); - g_data.jointPositions = robotStates->m_q; + g_data.jointPositions = robotStates.q; } } catch (const flexiv::Exception& e) { @@ -136,6 +134,19 @@ void visualizerTask(flexiv::Visualization* visualizer) visualizer->updateScene("mesh1", newPosition, newOrientation); } +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP] [robot URDF] [scene URDF]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << " robot URDF: path to the robot URDF file" << std::endl; + std::cout << " scene URDF: path to the scene URDF file" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { // Log object for printing message with timestamp and coloring @@ -143,13 +154,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // Check if program has 3 arguments - if (argc != 5) { - log.error( - "Invalid program arguments. Usage: " - " "); - return 0; + if (argc < 5 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -178,7 +188,7 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } @@ -188,8 +198,15 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational + int secondsWaited = 0; while (!robot.isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } } log.info("Robot is now operational"); @@ -212,7 +229,7 @@ int main(int argc, char* argv[]) flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority scheduler.addTask( - std::bind(highPriorityTask, &robot, &robotStates, &scheduler, &log), + std::bind(highPriorityTask, &robot, &scheduler, &log, robotStates), "HP periodic", 1, 45); // Add periodic task with 20ms interval and low priority scheduler.addTask( @@ -222,7 +239,7 @@ int main(int argc, char* argv[]) } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/example/windows/auto_recovery/auto_recovery.vcxproj b/example/windows/auto_recovery/auto_recovery.vcxproj deleted file mode 100644 index fa66e6e2..00000000 --- a/example/windows/auto_recovery/auto_recovery.vcxproj +++ /dev/null @@ -1,102 +0,0 @@ - - - - - Release - Win32 - - - Release - x64 - - - - - - - 16.0 - Win32Proj - {e4d92af2-66f5-4e1f-83ee-70c84bb45a14} - auto_recovery - 10.0 - - - - Application - false - v143 - true - Unicode - - - Application - false - v143 - true - Unicode - - - - - - - - - - - - - - - false - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\ - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\$(ProjectName)\ - - - false - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\ - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\$(ProjectName)\ - - - - Level3 - true - true - true - WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) - true - $(SolutionDir)\..\..\include\;%(AdditionalIncludeDirectories) - - - Console - true - true - true - $(SolutionDir)\..\..\lib\windows\cpp\x86\;%(AdditionalLibraryDirectories) - FvrBase.lib;FlexivRdk.lib;%(AdditionalDependencies) - - - - - Level3 - true - true - true - NDEBUG;_CONSOLE;%(PreprocessorDefinitions) - true - $(SolutionDir)\..\..\include\;%(AdditionalIncludeDirectories) - - - Console - true - true - true - $(SolutionDir)\..\..\lib\windows\cpp\x64\;%(AdditionalLibraryDirectories) - FvrBase.lib;FlexivRdk.lib;%(AdditionalDependencies) - $(OutDir)$(TargetName)$(TargetExt) - - - - - - \ No newline at end of file diff --git a/example/windows/auto_recovery/auto_recovery.vcxproj.filters b/example/windows/auto_recovery/auto_recovery.vcxproj.filters deleted file mode 100644 index 5ca5fa55..00000000 --- a/example/windows/auto_recovery/auto_recovery.vcxproj.filters +++ /dev/null @@ -1,22 +0,0 @@ - - - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd - - - {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} - rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms - - - - - Source Files - - - \ No newline at end of file diff --git a/example/windows/auto_recovery/auto_recovery.vcxproj.user b/example/windows/auto_recovery/auto_recovery.vcxproj.user deleted file mode 100644 index 88a55094..00000000 --- a/example/windows/auto_recovery/auto_recovery.vcxproj.user +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/example/windows/clear_fault/clear_fault.vcxproj b/example/windows/clear_fault/clear_fault.vcxproj deleted file mode 100644 index 9b595f3c..00000000 --- a/example/windows/clear_fault/clear_fault.vcxproj +++ /dev/null @@ -1,102 +0,0 @@ - - - - - Release - Win32 - - - Release - x64 - - - - - - - 16.0 - Win32Proj - {fbccaaba-4e1a-431c-a94a-e8b207d73c08} - clear_fault - 10.0 - - - - Application - false - v143 - true - Unicode - - - Application - false - v143 - true - Unicode - - - - - - - - - - - - - - - false - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\ - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\$(ProjectName)\ - - - false - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\ - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\$(ProjectName)\ - - - - Level3 - true - true - true - WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) - true - $(SolutionDir)\..\..\include\;%(AdditionalIncludeDirectories) - - - Console - true - true - true - $(SolutionDir)\..\..\lib\windows\cpp\x86\;%(AdditionalLibraryDirectories) - FvrBase.lib;FlexivRdk.lib;%(AdditionalDependencies) - - - - - Level3 - true - true - true - NDEBUG;_CONSOLE;%(PreprocessorDefinitions) - true - $(SolutionDir)\..\..\include\;%(AdditionalIncludeDirectories) - - - Console - true - true - true - $(SolutionDir)\..\..\lib\windows\cpp\x64\;%(AdditionalLibraryDirectories) - FvrBase.lib;FlexivRdk.lib;%(AdditionalDependencies) - $(OutDir)$(TargetName)$(TargetExt) - - - - - - \ No newline at end of file diff --git a/example/windows/clear_fault/clear_fault.vcxproj.filters b/example/windows/clear_fault/clear_fault.vcxproj.filters deleted file mode 100644 index acfb6a91..00000000 --- a/example/windows/clear_fault/clear_fault.vcxproj.filters +++ /dev/null @@ -1,22 +0,0 @@ - - - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd - - - {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} - rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms - - - - - Source Files - - - \ No newline at end of file diff --git a/example/windows/clear_fault/clear_fault.vcxproj.user b/example/windows/clear_fault/clear_fault.vcxproj.user deleted file mode 100644 index 88a55094..00000000 --- a/example/windows/clear_fault/clear_fault.vcxproj.user +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/example/windows/display_robot_states/display_robot_states.vcxproj b/example/windows/display_robot_states/display_robot_states.vcxproj deleted file mode 100644 index 77492f00..00000000 --- a/example/windows/display_robot_states/display_robot_states.vcxproj +++ /dev/null @@ -1,102 +0,0 @@ - - - - - Release - Win32 - - - Release - x64 - - - - - - - 16.0 - Win32Proj - {34b0f436-2500-4328-b981-af66ef54a1d6} - displayrobotstates - 10.0 - - - - Application - false - v143 - true - Unicode - - - Application - false - v143 - true - Unicode - - - - - - - - - - - - - - - false - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\ - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\$(ProjectName)\ - - - false - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\ - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\$(ProjectName)\ - - - - Level3 - true - true - true - WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) - true - $(SolutionDir)\..\..\include\;%(AdditionalIncludeDirectories) - - - Console - true - true - true - $(SolutionDir)\..\..\lib\windows\cpp\x86\;%(AdditionalLibraryDirectories) - FvrBase.lib;FlexivRdk.lib;%(AdditionalDependencies) - - - - - Level3 - true - true - true - NDEBUG;_CONSOLE;%(PreprocessorDefinitions) - true - $(SolutionDir)\..\..\include\;%(AdditionalIncludeDirectories) - - - Console - true - true - true - $(SolutionDir)\..\..\lib\windows\cpp\x64\;%(AdditionalLibraryDirectories) - FvrBase.lib;FlexivRdk.lib;%(AdditionalDependencies) - $(OutDir)$(TargetName)$(TargetExt) - - - - - - \ No newline at end of file diff --git a/example/windows/display_robot_states/display_robot_states.vcxproj.filters b/example/windows/display_robot_states/display_robot_states.vcxproj.filters deleted file mode 100644 index ecf08557..00000000 --- a/example/windows/display_robot_states/display_robot_states.vcxproj.filters +++ /dev/null @@ -1,22 +0,0 @@ - - - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd - - - {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} - rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms - - - - - Source Files - - - \ No newline at end of file diff --git a/example/windows/display_robot_states/display_robot_states.vcxproj.user b/example/windows/display_robot_states/display_robot_states.vcxproj.user deleted file mode 100644 index 88a55094..00000000 --- a/example/windows/display_robot_states/display_robot_states.vcxproj.user +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/example/windows/gripper_control/gripper_control.vcxproj b/example/windows/gripper_control/gripper_control.vcxproj deleted file mode 100644 index e4b6495c..00000000 --- a/example/windows/gripper_control/gripper_control.vcxproj +++ /dev/null @@ -1,102 +0,0 @@ - - - - - Release - Win32 - - - Release - x64 - - - - - - - 16.0 - Win32Proj - {9060ade2-9b80-43a6-b354-ee074e2c7862} - gripper_control - 10.0 - - - - Application - false - v143 - true - Unicode - - - Application - false - v143 - true - Unicode - - - - - - - - - - - - - - - false - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\ - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\$(ProjectName)\ - - - false - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\ - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\$(ProjectName)\ - - - - Level3 - true - true - true - WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) - true - $(SolutionDir)\..\..\include\;%(AdditionalIncludeDirectories) - - - Console - true - true - true - $(SolutionDir)\..\..\lib\windows\cpp\x86\;%(AdditionalLibraryDirectories) - FvrBase.lib;FlexivRdk.lib;%(AdditionalDependencies) - - - - - Level3 - true - true - true - NDEBUG;_CONSOLE;%(PreprocessorDefinitions) - true - $(SolutionDir)\..\..\include\;%(AdditionalIncludeDirectories) - - - Console - true - true - true - $(SolutionDir)\..\..\lib\windows\cpp\x64\;%(AdditionalLibraryDirectories) - FvrBase.lib;FlexivRdk.lib;%(AdditionalDependencies) - $(OutDir)$(TargetName)$(TargetExt) - - - - - - \ No newline at end of file diff --git a/example/windows/gripper_control/gripper_control.vcxproj.filters b/example/windows/gripper_control/gripper_control.vcxproj.filters deleted file mode 100644 index 274dae89..00000000 --- a/example/windows/gripper_control/gripper_control.vcxproj.filters +++ /dev/null @@ -1,22 +0,0 @@ - - - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd - - - {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} - rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms - - - - - Source Files - - - \ No newline at end of file diff --git a/example/windows/gripper_control/gripper_control.vcxproj.user b/example/windows/gripper_control/gripper_control.vcxproj.user deleted file mode 100644 index 88a55094..00000000 --- a/example/windows/gripper_control/gripper_control.vcxproj.user +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/example/windows/plan_execution/plan_execution.vcxproj b/example/windows/plan_execution/plan_execution.vcxproj deleted file mode 100644 index a3afbee1..00000000 --- a/example/windows/plan_execution/plan_execution.vcxproj +++ /dev/null @@ -1,102 +0,0 @@ - - - - - Release - Win32 - - - Release - x64 - - - - - - - 16.0 - Win32Proj - {7508d147-1bfe-408a-9843-9d567e845502} - plan_execution - 10.0 - - - - Application - false - v143 - true - Unicode - - - Application - false - v143 - true - Unicode - - - - - - - - - - - - - - - false - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\ - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\$(ProjectName)\ - - - false - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\ - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\$(ProjectName)\ - - - - Level3 - true - true - true - WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) - true - $(SolutionDir)\..\..\include\;%(AdditionalIncludeDirectories) - - - Console - true - true - true - $(SolutionDir)\..\..\lib\windows\cpp\x86\;%(AdditionalLibraryDirectories) - FvrBase.lib;FlexivRdk.lib;%(AdditionalDependencies) - - - - - Level3 - true - true - true - NDEBUG;_CONSOLE;%(PreprocessorDefinitions) - true - $(SolutionDir)\..\..\include\;%(AdditionalIncludeDirectories) - - - Console - true - true - true - $(SolutionDir)\..\..\lib\windows\cpp\x64\;%(AdditionalLibraryDirectories) - FvrBase.lib;FlexivRdk.lib;%(AdditionalDependencies) - $(OutDir)$(TargetName)$(TargetExt) - - - - - - \ No newline at end of file diff --git a/example/windows/plan_execution/plan_execution.vcxproj.filters b/example/windows/plan_execution/plan_execution.vcxproj.filters deleted file mode 100644 index 1d548e30..00000000 --- a/example/windows/plan_execution/plan_execution.vcxproj.filters +++ /dev/null @@ -1,22 +0,0 @@ - - - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd - - - {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} - rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms - - - - - Source Files - - - \ No newline at end of file diff --git a/example/windows/plan_execution/plan_execution.vcxproj.user b/example/windows/plan_execution/plan_execution.vcxproj.user deleted file mode 100644 index 88a55094..00000000 --- a/example/windows/plan_execution/plan_execution.vcxproj.user +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/example/windows/primitive_execution/primitive_execution.vcxproj b/example/windows/primitive_execution/primitive_execution.vcxproj deleted file mode 100644 index 82c7d297..00000000 --- a/example/windows/primitive_execution/primitive_execution.vcxproj +++ /dev/null @@ -1,102 +0,0 @@ - - - - - Release - Win32 - - - Release - x64 - - - - - - - 16.0 - Win32Proj - {781a43e7-b6aa-42e1-ba64-b4d93d9e9c95} - primitive_execution - 10.0 - - - - Application - false - v143 - true - Unicode - - - Application - false - v143 - true - Unicode - - - - - - - - - - - - - - - false - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\ - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\$(ProjectName)\ - - - false - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\ - $(SolutionDir)\..\..\build\windows\$(Platform)\$(Configuration)\$(ProjectName)\ - - - - Level3 - true - true - true - WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) - true - $(SolutionDir)\..\..\include\;%(AdditionalIncludeDirectories) - - - Console - true - true - true - $(SolutionDir)\..\..\lib\windows\cpp\x86\;%(AdditionalLibraryDirectories) - FvrBase.lib;FlexivRdk.lib;%(AdditionalDependencies) - - - - - Level3 - true - true - true - NDEBUG;_CONSOLE;%(PreprocessorDefinitions) - true - $(SolutionDir)\..\..\include\;%(AdditionalIncludeDirectories) - - - Console - true - true - true - $(SolutionDir)\..\..\lib\windows\cpp\x64\;%(AdditionalLibraryDirectories) - FvrBase.lib;FlexivRdk.lib;%(AdditionalDependencies) - $(OutDir)$(TargetName)$(TargetExt) - - - - - - \ No newline at end of file diff --git a/example/windows/primitive_execution/primitive_execution.vcxproj.filters b/example/windows/primitive_execution/primitive_execution.vcxproj.filters deleted file mode 100644 index bd40dabc..00000000 --- a/example/windows/primitive_execution/primitive_execution.vcxproj.filters +++ /dev/null @@ -1,22 +0,0 @@ - - - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd - - - {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} - rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms - - - - - Source Files - - - \ No newline at end of file diff --git a/example/windows/primitive_execution/primitive_execution.vcxproj.user b/example/windows/primitive_execution/primitive_execution.vcxproj.user deleted file mode 100644 index 88a55094..00000000 --- a/example/windows/primitive_execution/primitive_execution.vcxproj.user +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/example/windows/windows.sln b/example/windows/windows.sln deleted file mode 100644 index f97c4f3c..00000000 --- a/example/windows/windows.sln +++ /dev/null @@ -1,79 +0,0 @@ - -Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio Version 17 -VisualStudioVersion = 17.1.32407.343 -MinimumVisualStudioVersion = 10.0.40219.1 -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "display_robot_states", "display_robot_states\display_robot_states.vcxproj", "{34B0F436-2500-4328-B981-AF66EF54A1D6}" -EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "clear_fault", "clear_fault\clear_fault.vcxproj", "{FBCCAABA-4E1A-431C-A94A-E8B207D73C08}" -EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "plan_execution", "plan_execution\plan_execution.vcxproj", "{7508D147-1BFE-408A-9843-9D567E845502}" -EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "auto_recovery", "auto_recovery\auto_recovery.vcxproj", "{E4D92AF2-66F5-4E1F-83EE-70C84BB45A14}" -EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "primitive_execution", "primitive_execution\primitive_execution.vcxproj", "{781A43E7-B6AA-42E1-BA64-B4D93D9E9C95}" -EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "gripper_control", "gripper_control\gripper_control.vcxproj", "{9060ADE2-9B80-43A6-B354-EE074E2C7862}" -EndProject -Global - GlobalSection(SolutionConfigurationPlatforms) = preSolution - Debug|x64 = Debug|x64 - Debug|x86 = Debug|x86 - Release|x64 = Release|x64 - Release|x86 = Release|x86 - EndGlobalSection - GlobalSection(ProjectConfigurationPlatforms) = postSolution - {34B0F436-2500-4328-B981-AF66EF54A1D6}.Debug|x64.ActiveCfg = Release|x64 - {34B0F436-2500-4328-B981-AF66EF54A1D6}.Debug|x86.ActiveCfg = Release|Win32 - {34B0F436-2500-4328-B981-AF66EF54A1D6}.Release|x64.ActiveCfg = Release|x64 - {34B0F436-2500-4328-B981-AF66EF54A1D6}.Release|x64.Build.0 = Release|x64 - {34B0F436-2500-4328-B981-AF66EF54A1D6}.Release|x86.ActiveCfg = Release|Win32 - {34B0F436-2500-4328-B981-AF66EF54A1D6}.Release|x86.Build.0 = Release|Win32 - {FBCCAABA-4E1A-431C-A94A-E8B207D73C08}.Debug|x64.ActiveCfg = Release|x64 - {FBCCAABA-4E1A-431C-A94A-E8B207D73C08}.Debug|x64.Build.0 = Release|x64 - {FBCCAABA-4E1A-431C-A94A-E8B207D73C08}.Debug|x86.ActiveCfg = Release|Win32 - {FBCCAABA-4E1A-431C-A94A-E8B207D73C08}.Debug|x86.Build.0 = Release|Win32 - {FBCCAABA-4E1A-431C-A94A-E8B207D73C08}.Release|x64.ActiveCfg = Release|x64 - {FBCCAABA-4E1A-431C-A94A-E8B207D73C08}.Release|x64.Build.0 = Release|x64 - {FBCCAABA-4E1A-431C-A94A-E8B207D73C08}.Release|x86.ActiveCfg = Release|Win32 - {FBCCAABA-4E1A-431C-A94A-E8B207D73C08}.Release|x86.Build.0 = Release|Win32 - {7508D147-1BFE-408A-9843-9D567E845502}.Debug|x64.ActiveCfg = Release|x64 - {7508D147-1BFE-408A-9843-9D567E845502}.Debug|x64.Build.0 = Release|x64 - {7508D147-1BFE-408A-9843-9D567E845502}.Debug|x86.ActiveCfg = Release|Win32 - {7508D147-1BFE-408A-9843-9D567E845502}.Debug|x86.Build.0 = Release|Win32 - {7508D147-1BFE-408A-9843-9D567E845502}.Release|x64.ActiveCfg = Release|x64 - {7508D147-1BFE-408A-9843-9D567E845502}.Release|x64.Build.0 = Release|x64 - {7508D147-1BFE-408A-9843-9D567E845502}.Release|x86.ActiveCfg = Release|Win32 - {7508D147-1BFE-408A-9843-9D567E845502}.Release|x86.Build.0 = Release|Win32 - {E4D92AF2-66F5-4E1F-83EE-70C84BB45A14}.Debug|x64.ActiveCfg = Release|x64 - {E4D92AF2-66F5-4E1F-83EE-70C84BB45A14}.Debug|x64.Build.0 = Release|x64 - {E4D92AF2-66F5-4E1F-83EE-70C84BB45A14}.Debug|x86.ActiveCfg = Release|Win32 - {E4D92AF2-66F5-4E1F-83EE-70C84BB45A14}.Debug|x86.Build.0 = Release|Win32 - {E4D92AF2-66F5-4E1F-83EE-70C84BB45A14}.Release|x64.ActiveCfg = Release|x64 - {E4D92AF2-66F5-4E1F-83EE-70C84BB45A14}.Release|x64.Build.0 = Release|x64 - {E4D92AF2-66F5-4E1F-83EE-70C84BB45A14}.Release|x86.ActiveCfg = Release|Win32 - {E4D92AF2-66F5-4E1F-83EE-70C84BB45A14}.Release|x86.Build.0 = Release|Win32 - {781A43E7-B6AA-42E1-BA64-B4D93D9E9C95}.Debug|x64.ActiveCfg = Release|x64 - {781A43E7-B6AA-42E1-BA64-B4D93D9E9C95}.Debug|x64.Build.0 = Release|x64 - {781A43E7-B6AA-42E1-BA64-B4D93D9E9C95}.Debug|x86.ActiveCfg = Release|Win32 - {781A43E7-B6AA-42E1-BA64-B4D93D9E9C95}.Debug|x86.Build.0 = Release|Win32 - {781A43E7-B6AA-42E1-BA64-B4D93D9E9C95}.Release|x64.ActiveCfg = Release|x64 - {781A43E7-B6AA-42E1-BA64-B4D93D9E9C95}.Release|x64.Build.0 = Release|x64 - {781A43E7-B6AA-42E1-BA64-B4D93D9E9C95}.Release|x86.ActiveCfg = Release|Win32 - {781A43E7-B6AA-42E1-BA64-B4D93D9E9C95}.Release|x86.Build.0 = Release|Win32 - {9060ADE2-9B80-43A6-B354-EE074E2C7862}.Debug|x64.ActiveCfg = Release|x64 - {9060ADE2-9B80-43A6-B354-EE074E2C7862}.Debug|x64.Build.0 = Release|x64 - {9060ADE2-9B80-43A6-B354-EE074E2C7862}.Debug|x86.ActiveCfg = Release|Win32 - {9060ADE2-9B80-43A6-B354-EE074E2C7862}.Debug|x86.Build.0 = Release|Win32 - {9060ADE2-9B80-43A6-B354-EE074E2C7862}.Release|x64.ActiveCfg = Release|x64 - {9060ADE2-9B80-43A6-B354-EE074E2C7862}.Release|x64.Build.0 = Release|x64 - {9060ADE2-9B80-43A6-B354-EE074E2C7862}.Release|x86.ActiveCfg = Release|Win32 - {9060ADE2-9B80-43A6-B354-EE074E2C7862}.Release|x86.Build.0 = Release|Win32 - EndGlobalSection - GlobalSection(SolutionProperties) = preSolution - HideSolutionNode = FALSE - EndGlobalSection - GlobalSection(ExtensibilityGlobals) = postSolution - SolutionGuid = {504EAA0B-F2BB-4533-BCCB-C3F7F15216AC} - EndGlobalSection -EndGlobal diff --git a/example_py/auto_recovery.py b/example_py/auto_recovery.py index e3653516..6ee120ef 100644 --- a/example_py/auto_recovery.py +++ b/example_py/auto_recovery.py @@ -17,7 +17,7 @@ # Import Flexiv RDK Python library # fmt: off import sys -sys.path.insert(0, "../lib/linux/python/x64/") +sys.path.insert(0, "../lib_py") import flexivrdk # fmt: on @@ -27,7 +27,7 @@ def main(): # ============================================================================= argparser = argparse.ArgumentParser() argparser.add_argument('robot_ip', help='IP address of the robot server') - argparser.add_argument('local_ip', help='IP address of the workstation PC') + argparser.add_argument('local_ip', help='IP address of this PC') args = argparser.parse_args() # Define alias diff --git a/example_py/clear_fault.py b/example_py/clear_fault.py index ec52c287..c4c09385 100644 --- a/example_py/clear_fault.py +++ b/example_py/clear_fault.py @@ -14,7 +14,7 @@ # Import Flexiv RDK Python library # fmt: off import sys -sys.path.insert(0, "../lib/linux/python/x64/") +sys.path.insert(0, "../lib_py") import flexivrdk # fmt: on @@ -24,7 +24,7 @@ def main(): # ============================================================================= argparser = argparse.ArgumentParser() argparser.add_argument('robot_ip', help='IP address of the robot server') - argparser.add_argument('local_ip', help='IP address of the workstation PC') + argparser.add_argument('local_ip', help='IP address of this PC') args = argparser.parse_args() # Define alias diff --git a/example_py/display_robot_states.py b/example_py/display_robot_states.py index 984c2fa8..c1c43665 100644 --- a/example_py/display_robot_states.py +++ b/example_py/display_robot_states.py @@ -14,7 +14,7 @@ # Import Flexiv RDK Python library # fmt: off import sys -sys.path.insert(0, "../lib/linux/python/x64/") +sys.path.insert(0, "../lib_py") import flexivrdk # fmt: on @@ -24,7 +24,7 @@ def main(): # ============================================================================= argparser = argparse.ArgumentParser() argparser.add_argument('robot_ip', help='IP address of the robot server') - argparser.add_argument('local_ip', help='IP address of the workstation PC') + argparser.add_argument('local_ip', help='IP address of this PC') args = argparser.parse_args() # Define alias @@ -56,32 +56,43 @@ def main(): robot.enable() # Wait for the robot to become operational + seconds_waited = 0 while not robot.isOperational(): time.sleep(1) + seconds_waited += 1 + if seconds_waited == 10: + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode") + log.info("Robot is now operational") # Application-specific Code # ============================================================================= while True: robot.getRobotStates(robot_states) - print("q: ", robot_states.m_q) - print("theta: ", robot_states.m_theta) - print("dq: ", robot_states.m_dq) - print("dtheta: ", robot_states.m_dtheta) - print("tau: ", robot_states.m_tau) - print("tau_des: ", robot_states.m_tauDes) - print("tau_dot: ", robot_states.m_tauDot) - print("tau_ext: ", robot_states.m_tauExt) - print("tcp_pose: ", robot_states.m_tcpPose) - print("tcp_pose_d: ", robot_states.m_tcpPoseDes) - print("tcp_velocity: ", robot_states.m_tcpVel) - print("camera_pose: ", robot_states.m_camPose) - print("flange_pose: ", robot_states.m_flangePose) - print("end_link_pose: ", robot_states.m_endLinkPose) - print("F_ext_tcp_frame: ", robot_states.m_extForceInTcpFrame) - print("F_ext_base_frame: ", robot_states.m_extForceInBaseFrame) - log.info("==" * 30) + log.info(" ") + # Round all list values to 2 decimals + # fmt: off + print("q: ", ['%.2f' % i for i in robot_states.q]) + print("theta: ", ['%.2f' % i for i in robot_states.theta]) + print("dq: ", ['%.2f' % i for i in robot_states.dq]) + print("dtheta: ", ['%.2f' % i for i in robot_states.dtheta]) + print("tau: ", ['%.2f' % i for i in robot_states.tau]) + print("tau_des: ", ['%.2f' % i for i in robot_states.tauDes]) + print("tau_dot: ", ['%.2f' % i for i in robot_states.tauDot]) + print("tau_ext: ", ['%.2f' % i for i in robot_states.tauExt]) + print("tcp_pose: ", ['%.2f' % i for i in robot_states.tcpPose]) + print("tcp_pose_d: ", ['%.2f' % i for i in robot_states.tcpPoseDes]) + print("tcp_velocity: ", ['%.2f' % i for i in robot_states.tcpVel]) + print("camera_pose: ", ['%.2f' % i for i in robot_states.camPose]) + print("flange_pose: ", ['%.2f' % i for i in robot_states.flangePose]) + print("end_link_pose: ", ['%.2f' % i for i in robot_states.endLinkPose]) + print("F_ext_tcp_frame: ", ['%.2f' % i for i in robot_states.extForceInTcpFrame]) + print("F_ext_base_frame: ", ['%.2f' % i for i in robot_states.extForceInBaseFrame]) time.sleep(1) + # fmt: on except Exception as e: # Print exception error message diff --git a/example_py/gripper_control.py b/example_py/gripper_control.py index 5e43a2f3..e800977c 100644 --- a/example_py/gripper_control.py +++ b/example_py/gripper_control.py @@ -15,7 +15,7 @@ # Import Flexiv RDK Python library # fmt: off import sys -sys.path.insert(0, "../lib/linux/python/x64/") +sys.path.insert(0, "../lib_py") import flexivrdk # fmt: on @@ -28,7 +28,7 @@ def main(): argparser.add_argument( 'robot_ip', help='IP address of the robot server') argparser.add_argument( - 'local_ip', help='IP address of the workstation PC') + 'local_ip', help='IP address of this PC') args = argparser.parse_args() # Define alias @@ -59,8 +59,16 @@ def main(): robot.enable() # Wait for the robot to become operational + seconds_waited = 0 while not robot.isOperational(): time.sleep(1) + seconds_waited += 1 + if seconds_waited == 10: + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode") + log.info("Robot is now operational") # Set mode after robot is operational @@ -71,35 +79,44 @@ def main(): time.sleep(1) robot.executePlanByName("PLAN-Home") + # Wait for plan to start time.sleep(1) + # Wait for plan to finish + while robot.isBusy(): + time.sleep(1) # Application-specific Code # ============================================================================= # Instantiate gripper gripper = flexivrdk.Gripper(robot) - # Position control - # Close to width = 0.02m, using velocity = 0.1m/s - log.info("Closing fingers") - gripper.move(0.02, 0.1) - time.sleep(1) - # Open to width = 0.08m, using velocity = 0.1m/s - log.info("Opening fingers") - gripper.move(0.08, 0.1) - time.sleep(1) - # Force control - # Close fingers with 10N - log.info("Grasping with constant force") - gripper.grasp(10) - # Hold for 3 seconds - time.sleep(3) + # Move tests + log.info("Closing gripper") + gripper.move(0.01, 0.1, 20) + time.sleep(2) + + log.info("Opening gripper") + gripper.move(0.09, 0.1, 20) + time.sleep(2) + + # Stop tests + log.info("Closing gripper") + gripper.move(0.01, 0.1, 20) + time.sleep(0.5) + log.info("Stopping gripper") + gripper.stop() + time.sleep(2) + + log.info("Closing gripper") + gripper.move(0.01, 0.1, 20) + time.sleep(2) - # Open fingers and stop halfway - log.info("Opening fingers") - gripper.move(0.08, 0.1) + log.info("Opening gripper") + gripper.move(0.09, 0.1, 20) time.sleep(0.5) log.info("Stopping gripper") gripper.stop() + time.sleep(2) except Exception as e: log.error(str(e)) diff --git a/example_py/nrt_cartesian_impedance_control.py b/example_py/nrt_cartesian_impedance_control.py index d80648b5..653730c0 100644 --- a/example_py/nrt_cartesian_impedance_control.py +++ b/example_py/nrt_cartesian_impedance_control.py @@ -15,7 +15,7 @@ # Import Flexiv RDK Python library # fmt: off import sys -sys.path.insert(0, "../lib/linux/python/x64/") +sys.path.insert(0, "../lib_py") import flexivrdk # fmt: on @@ -24,26 +24,24 @@ def main(): # Parse Arguments # ============================================================================= argparser = argparse.ArgumentParser() - argparser.add_argument('robot_ip', help='IP address of the robot server') - argparser.add_argument('local_ip', help='IP address of the workstation PC') + # Required arguments + argparser.add_argument("robot_ip", help="IP address of the robot server") + argparser.add_argument("local_ip", help="IP address of this PC") argparser.add_argument( - 'motion_type', help='Accepted motion types: hold, sine-sweep') + "frequency", help="command frequency, 1 to 1000 [Hz]", type=float) + # Optional arguments argparser.add_argument( - 'frequency', help='Command frequency, 1 to 1000 [Hz]') + "--hold", action="store_true", + help="robot holds current TCP pose, otherwise do a sine-sweep") args = argparser.parse_args() - # Convert string to number - frequency = float(args.frequency) - # Check if arguments are valid - assert (args.motion_type == "hold" or args.motion_type == - "sine-sweep"), "Invalid input" + frequency = args.frequency assert (frequency >= 1 and frequency <= 1000), "Invalid input" # Define alias # ============================================================================= robot_states = flexivrdk.RobotStates() - system_status = flexivrdk.SystemStatus() log = flexivrdk.Log() mode = flexivrdk.Mode @@ -70,8 +68,16 @@ def main(): robot.enable() # Wait for the robot to become operational + seconds_waited = 0 while not robot.isOperational(): time.sleep(1) + seconds_waited += 1 + if seconds_waited == 10: + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode") + log.info("Robot is now operational") # Set mode after robot is operational @@ -90,7 +96,7 @@ def main(): # Use current robot TCP pose as initial pose robot.getRobotStates(robot_states) - init_pose = robot_states.m_tcpPose.copy() + init_pose = robot_states.tcpPose.copy() print("Initial pose set to: ", init_pose) # Initialize target vectors @@ -113,41 +119,40 @@ def main(): raise Exception("Fault occurred on robot server, exiting ...") # Sine-sweep TCP along y axis - if args.motion_type == "sine-sweep": + if not args.hold: target_pose[1] = init_pose[1] + SWING_AMP * \ math.sin(2 * math.pi * SWING_FREQ * loop_counter * period) - - # Online change swivel angle at 2 seconds - preferred_degree = 0 - if ((loop_counter * period) % 10.0 == 2.0): - preferred_degree = 30 - robot.setSwivelAngle(preferred_degree / 180 * math.pi) - log.info("Preferred swivel angle set to degrees: " + - str(preferred_degree)) - - # Online change stiffness to softer at 5 seconds - if ((loop_counter * period) % 10.0 == 5.0): - new_kp = [2000, 2000, 2000, 200, 200, 200] - robot.setCartesianStiffness(new_kp) - log.info("Cartesian stiffness set to: ") - print(new_kp) - - # Online change swivel angle at 7 seconds - if ((loop_counter * period) % 10.0 == 7.0): - preferred_degree = -30 - robot.setSwivelAngle(preferred_degree / 180 * math.pi) - log.info("Preferred swivel angle set to degrees: " - + str(preferred_degree)) - - # Online reset stiffness to original at 9 seconds - if ((loop_counter * period) % 10.0 == 9.0): - default_kp = [4000, 4000, 4000, 300, 300, 300] - robot.setCartesianStiffness(default_kp) - log.info("Cartesian stiffness is reset to: ") - print(default_kp) - # Otherwise robot TCP will hold at initial pose + # Online change swivel angle at 2 seconds + preferred_degree = 0 + if ((loop_counter * period) % 10.0 == 2.0): + preferred_degree = 10 + robot.setSwivelAngle(preferred_degree / 180 * math.pi) + log.info("Preferred swivel angle set to degrees: " + + str(preferred_degree)) + + # Online change stiffness to softer at 5 seconds + if ((loop_counter * period) % 10.0 == 5.0): + new_kp = [2000, 2000, 2000, 200, 200, 200] + robot.setCartesianStiffness(new_kp) + log.info("Cartesian stiffness set to: ") + print(new_kp) + + # Online change swivel angle at 7 seconds + if ((loop_counter * period) % 10.0 == 7.0): + preferred_degree = -10 + robot.setSwivelAngle(preferred_degree / 180 * math.pi) + log.info("Preferred swivel angle set to degrees: " + + str(preferred_degree)) + + # Online reset stiffness to original at 9 seconds + if ((loop_counter * period) % 10.0 == 9.0): + default_kp = [4000, 4000, 4000, 1900, 1900, 1900] + robot.setCartesianStiffness(default_kp) + log.info("Cartesian stiffness is reset to: ") + print(default_kp) + # Send command robot.sendTcpPose(target_pose, max_wrench) diff --git a/example_py/nrt_joint_position_control.py b/example_py/nrt_joint_position_control.py index b5b80758..e64f743b 100644 --- a/example_py/nrt_joint_position_control.py +++ b/example_py/nrt_joint_position_control.py @@ -15,7 +15,7 @@ # Import Flexiv RDK Python library # fmt: off import sys -sys.path.insert(0, "../lib/linux/python/x64/") +sys.path.insert(0, "../lib_py") import flexivrdk # fmt: on @@ -24,26 +24,24 @@ def main(): # Parse Arguments # ============================================================================= argparser = argparse.ArgumentParser() - argparser.add_argument('robot_ip', help='IP address of the robot server') - argparser.add_argument('local_ip', help='IP address of the workstation PC') + # Required arguments + argparser.add_argument("robot_ip", help="IP address of the robot server") + argparser.add_argument("local_ip", help="IP address of this PC") argparser.add_argument( - 'motion_type', help='Accepted motion types: hold, sine-sweep') + "frequency", help="command frequency, 1 to 1000 [Hz]", type=float) + # Optional arguments argparser.add_argument( - 'frequency', help='Command frequency, 1 to 1000 [Hz]') + "--hold", action="store_true", + help="robot holds current joint positions, otherwise do a sine-sweep") args = argparser.parse_args() - # Convert string to number - frequency = float(args.frequency) - # Check if arguments are valid - assert (args.motion_type == "hold" or args.motion_type == - "sine-sweep"), "Invalid input" + frequency = args.frequency assert (frequency >= 1 and frequency <= 1000), "Invalid input" # Define alias # ============================================================================= robot_states = flexivrdk.RobotStates() - system_status = flexivrdk.SystemStatus() log = flexivrdk.Log() mode = flexivrdk.Mode @@ -70,8 +68,16 @@ def main(): robot.enable() # Wait for the robot to become operational + seconds_waited = 0 while not robot.isOperational(): time.sleep(1) + seconds_waited += 1 + if seconds_waited == 10: + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode") + log.info("Robot is now operational") # Set mode after robot is operational @@ -90,11 +96,11 @@ def main(): # Use current robot joint positions as initial positions robot.getRobotStates(robot_states) - init_pos = robot_states.m_q.copy() + init_pos = robot_states.q.copy() print("Initial positions set to: ", init_pos) # Initialize target vectors - DOF = len(robot_states.m_q) + DOF = len(robot_states.q) target_pos = init_pos.copy() target_vel = [0.0] * DOF target_acc = [0.0] * DOF @@ -120,7 +126,7 @@ def main(): raise Exception("Fault occurred on robot server, exiting ...") # Sine-sweep all joints - if args.motion_type == "sine-sweep": + if not args.hold: for i in range(DOF): target_pos[i] = init_pos[i] + SWING_AMP * \ math.sin(2 * math.pi * SWING_FREQ * loop_time) diff --git a/example_py/plan_execution.py b/example_py/plan_execution.py index 94966c8a..ec041bb2 100644 --- a/example_py/plan_execution.py +++ b/example_py/plan_execution.py @@ -14,7 +14,7 @@ # Import Flexiv RDK Python library # fmt: off import sys -sys.path.insert(0, "../lib/linux/python/x64/") +sys.path.insert(0, "../lib_py") import flexivrdk # fmt: on @@ -24,14 +24,14 @@ def main(): # ============================================================================= argparser = argparse.ArgumentParser() argparser.add_argument('robot_ip', help='IP address of the robot server') - argparser.add_argument('local_ip', help='IP address of the workstation PC') + argparser.add_argument('local_ip', help='IP address of this PC') args = argparser.parse_args() # Define alias # ============================================================================= - system_status = flexivrdk.SystemStatus() log = flexivrdk.Log() mode = flexivrdk.Mode + plan_info = flexivrdk.PlanInfo() try: # RDK Initialization @@ -56,8 +56,16 @@ def main(): robot.enable() # Wait for the robot to become operational + seconds_waited = 0 while not robot.isOperational(): time.sleep(1) + seconds_waited += 1 + if seconds_waited == 10: + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode") + log.info("Robot is now operational") # Set mode after robot is operational @@ -75,32 +83,59 @@ def main(): raise Exception("Fault occurred on robot server, exiting ...") # Get user input - input_case = int(input( - "Choose an action:\n \ - [1] Get plan list \n \ - [2] Execute plan by index \n \ - [3] Stop the current plan execution \n" - ) - ) - - # Check if user input is valid - assert (input_case >= 1 and input_case <= 3), "Invalid input" - - # Get plan list - if input_case == 1: + log.info("Choose an action:") + print("[1] Show available plans") + print("[2] Execute a plan by index") + print("[3] Execute a plan by name") + user_input = int(input()) + + # Get and show plan list + if user_input == 1: plan_list = robot.getPlanNameList() - time.sleep(1) for i in range(len(plan_list)): print("[" + str(i) + "]", plan_list[i]) + print("") # Execute plan by index - elif input_case == 2: + elif user_input == 2: index = int(input("Enter plan index to execute:\n")) robot.executePlanByIndex(index) - # Stop the current plan execution - elif input_case == 3: - robot.stop() + # Print plan info while the current plan is running + while robot.isBusy(): + robot.getPlanInfo(plan_info) + log.info(" ") + print("assignedPlanName: ", plan_info.assignedPlanName) + print("ptName: ", plan_info.ptName) + print("nodeName: ", plan_info.nodeName) + print("nodePath: ", plan_info.nodePath) + print("nodePathTimePeriod: ", plan_info.nodePathTimePeriod) + print("nodePathNumber: ", plan_info.nodePathNumber) + print("velocityScale: ", plan_info.velocityScale) + print("") + time.sleep(1) + + # Execute plan by name + elif user_input == 3: + name = str(input("Enter plan name to execute:\n")) + robot.executePlanByName(name) + + # Print plan info while the current plan is running + while robot.isBusy(): + robot.getPlanInfo(plan_info) + log.info(" ") + print("assignedPlanName: ", plan_info.assignedPlanName) + print("ptName: ", plan_info.ptName) + print("nodeName: ", plan_info.nodeName) + print("nodePath: ", plan_info.nodePath) + print("nodePathTimePeriod: ", plan_info.nodePathTimePeriod) + print("nodePathNumber: ", plan_info.nodePathNumber) + print("velocityScale: ", plan_info.velocityScale) + print("") + time.sleep(1) + + else: + log.warn("Invalid input") except Exception as e: # Print exception error message diff --git a/example_py/primitive_execution.py b/example_py/primitive_execution.py index 01d9cb8f..d8deeab1 100644 --- a/example_py/primitive_execution.py +++ b/example_py/primitive_execution.py @@ -2,7 +2,8 @@ """primitive_execution.py -Execute various primitives using RDK's primitive execution API. +Execute various primitives using RDK's primitive execution API. For details on +all available primitives, refer to Flexiv RDK Manual. """ __copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." @@ -11,10 +12,15 @@ import time import argparse +# Utility methods +from utility import quat2eulerZYX +from utility import parse_pt_states +from utility import list2str + # Import Flexiv RDK Python library # fmt: off import sys -sys.path.insert(0, "../lib/linux/python/x64/") +sys.path.insert(0, "../lib_py") import flexivrdk # fmt: on @@ -24,7 +30,7 @@ def main(): # ============================================================================= argparser = argparse.ArgumentParser() argparser.add_argument('robot_ip', help='IP address of the robot server') - argparser.add_argument('local_ip', help='IP address of the workstation PC') + argparser.add_argument('local_ip', help='IP address of this PC') args = argparser.parse_args() # Define alias @@ -35,7 +41,6 @@ def main(): try: # RDK Initialization # ============================================================================= - # Some alias # Instantiate robot interface robot = flexivrdk.Robot(args.robot_ip, args.local_ip) @@ -56,8 +61,16 @@ def main(): robot.enable() # Wait for the robot to become operational + seconds_waited = 0 while not robot.isOperational(): time.sleep(1) + seconds_waited += 1 + if seconds_waited == 10: + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode") + log.info("Robot is now operational") # Set mode after robot is operational @@ -69,98 +82,86 @@ def main(): # Application-specific Code # ============================================================================= - # Flag to transit to the next primitive - transit_primitive = False - # Index for iterating through various primitives - pt_index = 1 + # (1) Go to home pose + # ----------------------------------------------------------------------------- + # All parameters of the "Home" primitive are optional, + # thus we can skip the parameters and the default values will be used + log.info("Executing primitive: Home") - # Go home first + # Send command to robot robot.executePrimitive("Home()") - log.info("Homing ...") - while True: - # Run user periodic task at 10Hz - time.sleep(0.1) + # Wait for robot to reach target location by checking for "reachedTarget = 1" + # in the list of current primitive states + while (parse_pt_states(robot.getPrimitiveStates(), "reachedTarget") != "1"): + time.sleep(1) - # Monitor fault on robot server - if robot.isFault(): - raise Exception("Fault occurred on robot server, exiting ...") - - # Reset flag before checking for transition conditions - transit_primitive = False - - # Get system status which contains primitive states - pt_states = robot.getPrimitiveStates() - - # Check for transition condition "reachedTarget = 1" - for state in pt_states: - print(state) - - # Split the state sentence into words - words = state.split() - - # Check if this state is "reachedTarget" - if words[0] == "reachedTarget": - # Check if the state value is 1 - if words[-1] == "1": - # Set transition flag - transit_primitive = True - log.info("Transitting to next primitive ...") - - # Iterate through various primitives if the previous is finished - if transit_primitive: - if pt_index == 0: - # (1) Go home, all parameters of the "Home" primitive are - # optional, thus we can skip the parameters and the default - # values will be used - robot.executePrimitive("Home()") - - # Execute the next primitive - pt_index += 1 - - elif pt_index == 1: - # (2) Move TCP to point A in world (base) frame. The - # mandatory parameter "target" requires 8 values: [x y z - # roll pitch yaw reference_frame reference_point] - # unit: meters and degrees - robot.executePrimitive( - "MoveL(target=0.387 -0.11 0.203 180.0 0.0 180.0 WORLD " - "WORLD_ORIGIN)") - - # Execute the next primitive - pt_index += 1 - - elif pt_index == 2: - # (3) Move TCP to point B in world (base) frame - robot.executePrimitive( - "MoveL(target=0.687 0.0 0.264 180.0 0.0 180.0 WORLD " - "WORLD_ORIGIN)") - - # Execute the next primitive - pt_index += 1 - - elif pt_index == 3: - # (4) Move TCP to point C in world (base) frame - robot.executePrimitive( - "MoveL(target=0.387 0.1 0.1 -90.0 0.0 180.0 WORLD " - "WORLD_ORIGIN)") - - # Execute the next primitive - pt_index += 1 - - elif pt_index == 4: - # (5) Move TCP to point D in world(base) frame - robot.executePrimitive( - "MoveL(target=0.5 0.0 0.3 180.0 0.0 180.0 WORLD " - "WORLD_ORIGIN)") - - # Execute the next primitive - pt_index += 1 - - elif pt_index == 5: - # Repeat the moves - pt_index = 0 + # NOTE: can also use robot.isStopped() to indirectly tell if the robot has + # reached target + + # (2) Move robot joints to target positions + # ----------------------------------------------------------------------------- + # The required parameter takes in 7 target joint positions + # Unit: degrees + log.info("Executing primitive: MoveJ") + + # Send command to robot + robot.executePrimitive("MoveJ(target=30 -45 0 90 0 40 30)") + + # Wait for reached target + while (parse_pt_states(robot.getPrimitiveStates(), "reachedTarget") != "1"): + time.sleep(1) + + # (3) Move robot TCP to a target position in world (base) frame + # ----------------------------------------------------------------------------- + # Required parameter: + # target: final target position + # [pos_x pos_y pos_z rot_x rot_y rot_z ref_frame ref_point] + # unit: m, deg + # Optional parameter: + # waypoints: waypoints to pass before reaching final target + # [same format as above, but can repeat for number of waypoints] + # maxVel: maximum TCP linear velocity + # unit: m/s + # NOTE: The rotations are using Euler ZYX convention, rot_x means Euler ZYX + # angle around X axis + log.info("Executing primitive: MoveL") + + # Send command to robot + robot.executePrimitive( + "MoveL(target=0.65 -0.3 0.2 180 0 180 WORLD " + "WORLD_ORIGIN,waypoints=0.45 0.1 0.2 180 0 180 WORLD " + "WORLD_ORIGIN 0.45 -0.3 0.2 180 0 180 WORLD WORLD_ORIGIN, maxVel=0.2)") + + # Wait for reached target + while (parse_pt_states(robot.getPrimitiveStates(), "reachedTarget") != "1"): + time.sleep(1) + + # (4) Another MoveL + # ----------------------------------------------------------------------------- + # An example to convert quaternion into Euler ZYX angles required by the + # primitive is shown below. + log.info("Executing primitive: MoveL") + + # Target quaternion in the general [w,x,y,z] order + target_quat = [-0.373286, 0.0270976, 0.83113, 0.411274] + + # Convert target quaternion to Euler ZYX using scipy package's 'xyz' extrinsic rotation + # NOTE: scipy uses [x,y,z,w] order to represent quaternion + eulerZYX_deg = quat2eulerZYX(target_quat, degree=True) + + # Send command to robot + robot.executePrimitive("MoveL(target=0.4 -0.1 0.2 " + + list2str(eulerZYX_deg) + + "WORLD WORLD_ORIGIN, maxVel=0.3)") + + # Wait for reached target + while (parse_pt_states(robot.getPrimitiveStates(), "reachedTarget") != "1"): + time.sleep(1) + + # All done, stop robot and put into IDLE mode + robot.stop() except Exception as e: # Print exception error message diff --git a/example_py/teach_by_demonstration.py b/example_py/teach_by_demonstration.py new file mode 100644 index 00000000..a05ac8b6 --- /dev/null +++ b/example_py/teach_by_demonstration.py @@ -0,0 +1,182 @@ +#!/usr/bin/env python + +"""teach_by_demonstration.py + +Free-drive the robot and record a series of Cartesian poses, which are then +reproduced by the robot. +""" + +__copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." +__author__ = "Flexiv" + +import time +import argparse + +# Utility methods +from utility import quat2eulerZYX +from utility import list2str +from utility import parse_pt_states + +# Import Flexiv RDK Python library +# fmt: off +import sys +sys.path.insert(0, "../lib_py") +import flexivrdk +# fmt: on + +# Maximum contact wrench [fx, fy, fz, mx, my, mz] [N][Nm] +MAX_CONTACT_WRENCH = [50.0, 50.0, 50.0, 15.0, 15.0, 15.0] + + +def main(): + # Parse Arguments + # ============================================================================= + argparser = argparse.ArgumentParser() + argparser.add_argument('robot_ip', help='IP address of the robot server') + argparser.add_argument('local_ip', help='IP address of this PC') + args = argparser.parse_args() + + # Define alias + # ============================================================================= + log = flexivrdk.Log() + mode = flexivrdk.Mode + + try: + # RDK Initialization + # ============================================================================= + # Instantiate robot interface + robot = flexivrdk.Robot(args.robot_ip, args.local_ip) + + # Clear fault on robot server if any + if robot.isFault(): + log.warn("Fault occurred on robot server, trying to clear ...") + # Try to clear the fault + robot.clearFault() + time.sleep(2) + # Check again + if robot.isFault(): + log.error("Fault cannot be cleared, exiting ...") + return + log.info("Fault on robot server is cleared") + + # Enable the robot, make sure the E-stop is released before enabling + log.info("Enabling robot ...") + robot.enable() + + # Wait for the robot to become operational + seconds_waited = 0 + while not robot.isOperational(): + time.sleep(1) + seconds_waited += 1 + if seconds_waited == 10: + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode") + + log.info("Robot is now operational") + + # Application-specific Code + # ============================================================================= + # Recorded robot poses + saved_poses = [] + + # Robot states data + robot_states = flexivrdk.RobotStates() + + # Acceptable user inputs + log.info("Accepted key inputs:") + print("[n] - start new teaching process") + print("[r] - record current robot pose") + print("[e] - finish recording and start execution") + + # User input polling + input_buffer = "" + while True: + input_buffer = str(input()) + # Start new teaching process + if input_buffer == "n": + # Clear storage + saved_poses.clear() + + # Put robot to plan execution mode + robot.setMode(mode.MODE_PLAN_EXECUTION) + + # Wait for the mode to be switched + while (robot.getMode() != mode.MODE_PLAN_EXECUTION): + time.sleep(1) + + # Robot run free drive + robot.executePlanByName("PLAN-FreeDriveAuto") + + log.info("New teaching process started") + log.warn( + "Hold down the enabling button on the motion bar to " + "activate free drive") + # Save current robot pose + elif input_buffer == "r": + if not robot.isBusy(): + log.warn("Please start a new teaching process first") + continue + + robot.getRobotStates(robot_states) + saved_poses.append(robot_states.tcpPose) + log.info("New pose saved: " + str(robot_states.tcpPose)) + log.info("Number of saved poses: " + str(len(saved_poses))) + # Reproduce recorded poses + elif input_buffer == "e": + if len(saved_poses) == 0: + log.warn("No pose is saved yet") + continue + + # Put robot to primitive execution mode + robot.setMode(mode.MODE_PRIMITIVE_EXECUTION) + + # Wait for the mode to be switched + while (robot.getMode() != mode.MODE_PRIMITIVE_EXECUTION): + time.sleep(1) + + for i in range(len(saved_poses)): + log.info("Executing pose " + str(i + 1) + "/" + + str(len(saved_poses))) + + target_pos = [ + saved_poses[i][0], saved_poses[i][1], saved_poses[i][2]] + # Convert quaternion to Euler ZYX required by + # MoveCompliance primitive + target_quat = [saved_poses[i][3], + saved_poses[i][4], saved_poses[i][5], saved_poses[i][6]] + + target_euler_deg = quat2eulerZYX(target_quat, degree=True) + robot.executePrimitive( + "MoveCompliance(target=" + + list2str(target_pos) + + list2str(target_euler_deg) + + "WORLD WORLD_ORIGIN, maxVel=0.3, enableMaxContactWrench=1, maxContactWrench=" + + list2str(MAX_CONTACT_WRENCH) + ")") + + # Wait for robot to reach target location by checking for "reachedTarget = 1" + # in the list of current primitive states + while (parse_pt_states(robot.getPrimitiveStates(), "reachedTarget") != "1"): + time.sleep(1) + + log.info( + "All saved poses are executed, enter 'n' to start a new " + "teaching process, 'r' to record more poses, 'e' to repeat " + "execution") + + # Put robot back to free drive + robot.setMode(mode.MODE_PLAN_EXECUTION) + while (robot.getMode() != mode.MODE_PLAN_EXECUTION): + time.sleep(1) + robot.executePlanByName("PLAN-FreeDriveAuto") + else: + log.warn("Invalid input") + + except Exception as e: + # Print exception error message + log.error(str(e)) + + +if __name__ == "__main__": + main() diff --git a/example_py/utility.py b/example_py/utility.py new file mode 100644 index 00000000..192bf9be --- /dev/null +++ b/example_py/utility.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python + +"""utility.py + +Utility methods. +""" + +__copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." +__author__ = "Flexiv" + +import math +# pip install scipy +from scipy.spatial.transform import Rotation as R + + +def quat2eulerZYX(quat, degree=False): + """ + Convert quaternion to Euler angles with ZYX axis rotations. + + Parameters + ---------- + quat : float list + Quaternion input in [w,x,y,z] order. + degree : bool + Return values in degrees, otherwise in radians. + + Returns + ---------- + float list + Euler angles in [x,y,z] order, radian by default unless specified otherwise. + """ + + # Convert target quaternion to Euler ZYX using scipy package's 'xyz' extrinsic rotation + # NOTE: scipy uses [x,y,z,w] order to represent quaternion + eulerZYX = R.from_quat([quat[1], quat[2], + quat[3], quat[0]]).as_euler('xyz', degrees=degree).tolist() + + return eulerZYX + + +def list2str(ls): + """ + Convert a list to a string. + + Parameters + ---------- + ls : list + Source list of any size. + + Returns + ---------- + str + A string with format "ls[0] ls[1] ... ls[n] ", i.e. each value + followed by a space, including the last one. + """ + + ret_str = "" + for i in ls: + ret_str += str(i) + " " + return ret_str + + +def parse_pt_states(pt_states, parse_target): + """ + Parse the value of a specified primitive state from the pt_states string list. + + Parameters + ---------- + pt_states : str list + Primitive states string list returned from Robot::getPrimitiveStates(). + parse_target : str + Name of the primitive state to parse for. + + Returns + ---------- + str + Value of the specified primitive state in string format. Empty string is + returned if parse_target does not exist. + """ + for state in pt_states: + # Split the state sentence into words + words = state.split() + + if words[0] == parse_target: + return words[-1] + + return "" diff --git a/include/flexiv/Exception.hpp b/include/flexiv/Exception.hpp index d9b9f2ac..6eaa604d 100644 --- a/include/flexiv/Exception.hpp +++ b/include/flexiv/Exception.hpp @@ -32,7 +32,7 @@ class InitException : public Exception /** * @class CommException - * @brief Thrown if the comminication/connection with the robot server has an + * @brief Thrown if the communication/connection with the robot server has an * error. */ class CommException : public Exception diff --git a/include/flexiv/Gripper.hpp b/include/flexiv/Gripper.hpp index 6d9c6e49..ae8341a4 100644 --- a/include/flexiv/Gripper.hpp +++ b/include/flexiv/Gripper.hpp @@ -13,8 +13,7 @@ namespace flexiv { /** * @class Gripper - * @brief Interface with grippers that use the communication protocol template - * provided by Flexiv. + * @brief Interface with grippers supported by Flexiv. */ class Gripper { @@ -28,9 +27,9 @@ class Gripper virtual ~Gripper(); /** - * @brief Grasp with direct force control. + * @brief Grasp with direct force control. Requires the mounted gripper to + * support direct force control. * @param[in] force Target gripping force. Positive direction: closing [N]. - * @return True: success, false: failed. * @note Applicable modes: all modes except MODE_IDLE. * @warning Target inputs outside the valid range (specified in gripper's * configuration file) will be saturated. @@ -42,17 +41,17 @@ class Gripper * @brief Move the gripper fingers with position control. * @param[in] width Target opening width [m]. * @param[in] velocity Closing/opening velocity, cannot be 0 [m/s]. - * @return True: success, false: failed. + * @param[in] forceLimit Maximum output force during movement [N]. If not + * specified, default force limit of the mounted gripper will be used. * @note Applicable modes: all modes except MODE_IDLE. * @warning Target inputs outside the valid range (specified in gripper's * configuration file) will be saturated. * @throw ExecutionException if error occurred during execution. */ - void move(double width, double velocity); + void move(double width, double velocity, double forceLimit = 0); /** * @brief Stop the gripper. - * @return True: success, false: failed. * @note Applicable modes: all modes. * @throw ExecutionException if error occurred during execution. */ diff --git a/include/flexiv/Mode.hpp b/include/flexiv/Mode.hpp index a409a924..effc49ed 100644 --- a/include/flexiv/Mode.hpp +++ b/include/flexiv/Mode.hpp @@ -16,48 +16,55 @@ enum Mode { MODE_UNKNOWN = -1, - /** - * @note This is the default mode upon initialization - */ - MODE_IDLE, ///< The robot holds position and waits for commands + MODE_IDLE, ///< No operation to execute, the robot holds position and waits + ///< for new command. /** - * @note Receive and execute user commands at 1kHz + * @note Real-time (RT) mode + * @see flexiv::Robot::streamJointTorque() */ - MODE_JOINT_TORQUE, ///< Real-time joint torque control + MODE_JOINT_TORQUE, ///< Execute continuous joint torque command @ 1kHz. /** - * @note Receive and execute user commands at 1kHz + * @note Real-time (RT) mode + * @see flexiv::Robot::streamJointPosition() */ - MODE_JOINT_POSITION, ///< Real-time joint position control + MODE_JOINT_POSITION, ///< Execute continuous joint position command @ 1kHz. /** - * @note Receive and execute user commands at < 1kHz + * @note Non-real-time (NRT) mode + * @see flexiv::Robot::sendJointPosition() */ - MODE_JOINT_POSITION_NRT, ///< Non-real-time joint position control + MODE_JOINT_POSITION_NRT, ///< Execute discrete joint position command + ///< (smoothened by internal motion generator). /** - * @note Non-real-time control mode. The user selects a plan and executes - * it. Upon completion, the user can send another plan + * @note Non-real-time (NRT) mode + * @see flexiv::Robot::executePlanByIndex() + * @see flexiv::Robot::executePlanByName() */ - MODE_PLAN_EXECUTION, ///< Execute the plan specified by user + MODE_PLAN_EXECUTION, ///< Execute pre-configured motion plans. /** - * @note Non-real-time control mode. The user sends a primitive command with - * the name and the associated parameters and executes it. Upon completion, - * the user can send another primitive command + * @note Non-real-time (NRT) mode + * @see flexiv::Robot::executePrimitive() */ - MODE_PRIMITIVE_EXECUTION, ///< Execute the primitive specified by user + MODE_PRIMITIVE_EXECUTION, ///< Execute robot primitives. /** - * @note Receive and execute user commands at 1kHz + * @note Real-time (RT) mode + * @see flexiv::Robot::streamTcpPose() */ - MODE_CARTESIAN_IMPEDANCE, ///< Real-time Cartesian impedance control + MODE_CARTESIAN_IMPEDANCE, ///< Execute continuous TCP pose command with + ///< Cartesian impedance controller @ 1kHz. /** - * @note Receive and execute user commands at < 1kHz + * @note Non-real-time (NRT) mode + * @see flexiv::Robot::sendTcpPose() */ - MODE_CARTESIAN_IMPEDANCE_NRT, ///< Non-realtime Cartesian impedance control + MODE_CARTESIAN_IMPEDANCE_NRT, ///< Execute discrete TCP pose command + ///< (smoothened by internal motion generator) + ///< with Cartesian impedance controller. }; } /* namespace flexiv */ diff --git a/include/flexiv/Robot.hpp b/include/flexiv/Robot.hpp index 118fac6b..f1abae34 100644 --- a/include/flexiv/Robot.hpp +++ b/include/flexiv/Robot.hpp @@ -37,7 +37,7 @@ class Robot Robot(const std::string& serverIP, const std::string& localIP); virtual ~Robot(); - //============================ SYSTEM METHODS ============================= + //============================= SYSTEM CONTROL ============================= /** * @brief Enable the robot, if E-stop is released and there's no fault, the * robot will release brakes, and becomes operational a few seconds later. @@ -59,12 +59,30 @@ class Robot */ bool isOperational(void) const; + /** + * @brief Check if the robot is currently executing a task. + * @return True: busy, false: can take new task. + */ + bool isBusy(void) const; + /** * @brief Check if the connection with the robot server is established. * @return True: connected, false: disconnected. */ bool isConnected(void) const; + /** + * @brief Check if the robot is in fault state. + * @return True: robot has fault, false: robot normal. + */ + bool isFault(void) const; + + /** + * @brief Check if the Emergency Stop is released. + * @note True: E-stop released, false: E-stop pressed + */ + bool isEstopReleased(void) const; + /** * @brief Check if the robot system is in recovery state. * @return True: in recovery state, false: not in recovery state. @@ -79,13 +97,6 @@ class Robot */ bool isRecoveryState(void) const; - /** - * @brief Check if the connection with the robot server has timeout, - * according to the heartbeat singal monitoring. - * @return True: timeout, false: not timeout. - */ - bool isTimeout(void) const; - /** * @brief Try establishing connection with the robot server. * @throw CommException if failed to establish connection. @@ -98,19 +109,12 @@ class Robot */ void disconnect(void); - /** - * @brief Check if the robot is in fault state. - * @return True: robot has fault, false: robot normal. - */ - bool isFault(void) const; - /** * @brief Clear minor fault of the robot. * @throw ExecutionException if error occurred during execution. */ void clearFault(void); - //=========================== CONTROL METHODS ============================== /** * @brief Set robot mode, call it after initialization. * @param[in] mode Mode of the robot, see flexiv::Mode. @@ -129,22 +133,14 @@ class Robot */ Mode getMode(void) const; + //============================ ROBOT OPERATION ============================= /** * @brief Get robot states like joint position, velocity, torque, TCP * pose, velocity, etc. - * @param[out] output Pointer to output data. + * @param[out] output Reference of output data object. * @note Call this method periodically to get the latest states. - * @throw InputException if output is null. - */ - void getRobotStates(RobotStates* output); - - /** - * @brief Get system status data like E-stop status, program execution - * status, etc. - * @param[out] output Pointer to output data. - * @throw InputException if output is null. */ - void getSystemStatus(SystemStatus* output); + void getRobotStates(RobotStates& output); /** * @brief Execute a plan by specifying its index. @@ -152,6 +148,7 @@ class Robot * getPlanNameList(). * @throw ExecutionException if error occurred during execution. * @throw InputException if index is invalid. + * @note isBusy() can be used to check if a plan task has finished. * @warning This method will block for 50 ms. */ void executePlanByIndex(unsigned int index); @@ -161,6 +158,7 @@ class Robot * @param[in] name Name of the plan to execute, can be obtained via * getPlanNameList(). * @throw ExecutionException if error occurred during execution. + * @note isBusy() can be used to check if a plan task has finished. * @warning This method will block for 50 ms. */ void executePlanByName(const std::string& name); @@ -179,20 +177,24 @@ class Robot * @brief Get detailed information about the currently running plan. * Contains information like plan name, primitive name, node name, node * path, node path time period, etc. - * @param[out] output Pointer to output data. + * @param[out] output Reference of output data object. * @throw CommException if there's no response from server. * @throw ExecutionException if error occurred during execution. - * @throw InputException if output is null. * @warning This method will block until the request-reply operation with * the server is done. The blocking time varies by communication latency. */ - void getPlanInfo(PlanInfo* output); + void getPlanInfo(PlanInfo& output); /** * @brief Execute a primitive by specifying its name and parameters. * @param[in] ptCmd Primitive command with the format: * ptName(inputParam1=xxx, inputParam2=xxx, ...). * @throw ExecutionException if error occurred during execution. + * @throw InputException if size of the input string is greater than 5kb. + * @note A primitive won't terminate itself upon finish, thus isBusy() + * cannot be used to check if a primitive task is finished, use primitive + * states like "reachedTarget" instead. + * @see getPrimitiveStates() * @warning This method will block for 50 ms. */ void executePrimitive(const std::string& ptCmd); @@ -207,6 +209,31 @@ class Robot */ std::vector getPrimitiveStates(void) const; + /** + * @brief Set global variables for the robot by specifying name and value. + * @param[in] globalVars Command to set global variables using the format: + * globalVar1=value(s), globalVar2=value(s), ... + * @throw ExecutionException if error occurred during execution. + * @throw InputException if size of the input string is greater than 5kb. + */ + void setGlobalVariables(const std::string& globalVars); + + /** + * @brief Get available global variables from the robot. + * @return Global variables in the format of a string list. + * @throw CommException if there's no response from server. + * @throw ExecutionException if error occurred during execution. + * @warning This method will block until the request-reply operation with + * the server is done. The blocking time varies by communication latency. + */ + std::vector getGlobalVariables(void) const; + + /** + * @brief Check if the robot has come to a complete stop. + * @return True: stopped, false: still moving. + */ + bool isStopped(void) const; + /** * @brief If the mounted tool has more than one TCP, switch the TCP being * used by the robot server. Default to the 1st one (index = 0). @@ -226,42 +253,9 @@ class Robot */ void startAutoRecovery(void); + //============================= MOTION CONTROL ============================= /** - * @brief Set stiffness of Cartesian impedance controller. - * @param[in] stiffness \f$ \mathbb{R}^{6 \times 1} \f$ diagonal elements of - * the positive definite stiffness matrix. Defaulted to the nominal - * stiffness. - * @note Applicable modes: MODE_CARTESIAN_IMPEDANCE, - * MODE_CARTESIAN_IMPEDANCE_NRT. - * @throw ExecutionException if error occurred during execution. - * @throw InputException if input is invalid. - */ - void setCartesianStiffness(const std::vector& stiffness - = {4000, 4000, 4000, 300, 300, 300}); - - /** - * @brief During Cartesian impedance modes, set preferred elbow swivel - * angle, which is the angle between the arm plane and the reference plane. - * @param[in] angle Swivel angle \f$ {\phi}~[rad] \f$, valid range: - * [-2.0944, 2.0944] rad, i.e. [-120, 120] deg. Defaulted to the nominal - * swivel angle. - * @par Geometry definitions - * Arm plane: defined by the origin of 3 body frames: link2(shoulder), - * link4(elbow), and link6(wrist). - * Reference plane: defined by the origin of 3 body frames: base, - * link2(shoulder), and link6(wrist). - * Positive direction: defined as from link2 origin to link6 origin, right - * hand rule. - * @note Applicable modes: MODE_CARTESIAN_IMPEDANCE, - * MODE_CARTESIAN_IMPEDANCE_NRT. - * @throw ExecutionException if error occurred during execution. - * @throw InputException if input is invalid. - */ - void setSwivelAngle(double angle = 0); - - //=========================== MOTION METHODS =============================== - /** - * @brief [Real-time] Continuously send joint torque command to robot. + * @brief Continuously send joint torque command to robot. * @param[in] torques \f$ \mathbb{R}^{Dof \times 1} \f$ target torques of * the joints, \f$ {\tau_J}_d~[Nm] \f$. * @param[in] enableGravityComp Enable/disable robot gravity compensation. @@ -269,18 +263,18 @@ class Robot * joints from moving outside the allowed position range, which will * trigger a safety fault that requires recovery operation. * @note Applicable mode: MODE_JOINT_TORQUE. + * @note Real-time (RT). * @throw ExecutionException if error occurred during execution. * @throw InputException if input is invalid. * @warning Always send smooth and continuous commands to avoid sudden * movements. - * @warning C++ only. */ void streamJointTorque(const std::vector& torques, bool enableGravityComp = true, bool enableSoftLimits = true); /** - * @brief [Real-time] Continuously send joint position, velocity and - * acceleration command. + * @brief Continuously send joint position, velocity, and acceleration + * command. * @param[in] positions \f$ \mathbb{R}^{Dof \times 1} \f$ target positions * of the joints, \f$ q_d~[rad] \f$. * @param[in] velocities \f$ \mathbb{R}^{Dof \times 1} \f$ target velocities @@ -288,20 +282,20 @@ class Robot * @param[in] accelerations \f$ \mathbb{R}^{Dof \times 1} \f$ target * accelerations of the joints, \f$ \ddot{q}_d~[rad/s^2] \f$. * @note Applicable mode: MODE_JOINT_POSITION. + * @note Real-time (RT). * @throw ExecutionException if error occurred during execution. * @throw InputException if input is invalid. * @warning Always send smooth and continuous commands to avoid sudden * movements. - * @warning C++ only. */ void streamJointPosition(const std::vector& positions, const std::vector& velocities, const std::vector& accelerations); /** - * @brief [Non-real-time] Discretely send joint position, velocity and - * acceleration command. The internal trajectory generator will interpolate - * between two set points and make the motion smooth. + * @brief Discretely send joint position, velocity, and acceleration + * command. The internal trajectory generator will interpolate between two + * set points and make the motion smooth. * @param[in] positions \f$ \mathbb{R}^{Dof \times 1} \f$ target positions * of the joints, \f$ q_d~[rad] \f$. * @param[in] velocities \f$ \mathbb{R}^{Dof \times 1} \f$ target velocities @@ -325,57 +319,85 @@ class Robot const std::vector& maxJerk); /** - * @brief [Real-time] Continuously send TCP pose, velocity and acceleration - * command. + * @brief Continuously command target TCP pose for the robot to track using + * its Cartesian impedance controller. * @param[in] pose \f$ \mathbb{R}^{7 \times 1} \f$ target TCP pose * in base frame, \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ * \mathbb{R}^{4 \times 1} \f$ quaternion \f$ [x, y, z, q_w, q_x, q_y, * q_z]^T~[m][] \f$. - * @param[in] velocity \f$ \mathbb{R}^{6 \times 1} \f$ target TCP velocity - * in base frame, \f$ \mathbb{R}^{3 \times 1} \f$ linear velocity and \f$ - * \mathbb{R}^{3 \times 1} \f$ angular velocity \f$ [v_x, v_y, v_z, - * \omega_x, \omega_y, \omega_z]^T~[m/s][rad/s] \f$. - * @param[in] acceleration \f$ \mathbb{R}^{6 \times 1} \f$ target TCP - * acceleration in base frame, \f$ \mathbb{R}^{3 \times 1} \f$ linear - * acceleration and \f$ \mathbb{R}^{3 \times 1} \f$ angular acceleration \f$ - * [a_x, a_y, a_z, \alpha_x, \alpha_y, \alpha_z]^T~[m/s^2][rad/s^2] \f$. + * @param[in] maxWrench (Optional) \f$ \mathbb{R}^{6 \times 1} \f$ maximum + * contact wrench in TCP coordinate, the controller will soften if needed to + * keep the actual contact wrench under this value. Default value will be + * used if not specified. \f$ \mathbb{R}^{3 \times 1} \f$ force and \f$ + * \mathbb{R}^{3 \times 1} \f$ moment \f$ [f_x, f_y, f_z, m_x, m_y, + * m_z]^T~[N][Nm] \f$. * @note Applicable mode: MODE_CARTESIAN_IMPEDANCE. + * @note Real-time (RT). * @throw ExecutionException if error occurred during execution. * @throw InputException if input is invalid. * @warning Always send smooth and continuous commands to avoid sudden * movements. - * @warning C++ only. */ void streamTcpPose(const std::vector& pose, - const std::vector& velocity, - const std::vector& acceleration); + const std::vector& maxWrench + = {100.0, 100.0, 100.0, 30.0, 30.0, 30.0}); /** - * @brief [Non-real-time] Discretely send TCP pose command with contact - * force constraints. The internal trajectory generator will interpolate - * between two set points and make the motion smooth. + * @brief Discretely command target TCP pose for the robot to track using + * its Cartesian impedance controller. An internal motion generator will + * smooth the discrete commands. * @param[in] pose \f$ \mathbb{R}^{7 \times 1} \f$ target TCP pose * in base frame, \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ * \mathbb{R}^{4 \times 1} \f$ quaternion \f$ [x, y, z, q_w, q_x, q_y, * q_z]^T~[m][] \f$. - * @param[in] maxWrench \f$ \mathbb{R}^{6 \times 1} \f$ maximum contact - * wrench in TCP coordinate, \f$ \mathbb{R}^{3 \times 1} \f$ force and \f$ + * @param[in] maxWrench (Optional) \f$ \mathbb{R}^{6 \times 1} \f$ maximum + * contact wrench in TCP coordinate, the controller will soften if needed to + * keep the actual contact wrench under this value. Default value will be + * used if not specified. \f$ \mathbb{R}^{3 \times 1} \f$ force and \f$ * \mathbb{R}^{3 \times 1} \f$ moment \f$ [f_x, f_y, f_z, m_x, m_y, * m_z]^T~[N][Nm] \f$. * @note Applicable mode: MODE_CARTESIAN_IMPEDANCE_NRT. * @throw ExecutionException if error occurred during execution. * @throw InputException if input is invalid. */ - void sendTcpPose( - const std::vector& pose, const std::vector& maxWrench); + void sendTcpPose(const std::vector& pose, + const std::vector& maxWrench + = {100.0, 100.0, 100.0, 30.0, 30.0, 30.0}); /** - * @brief Check if the robot has come to a complete stop. - * @return True: stopped, false: still moving. + * @brief Set stiffness of Cartesian impedance controller. + * @param[in] stiffness \f$ \mathbb{R}^{6 \times 1} \f$ diagonal elements of + * the positive definite stiffness matrix. Maximum (nominal) stiffness is + * provided as parameter default. + * @note Applicable modes: MODE_CARTESIAN_IMPEDANCE, + * MODE_CARTESIAN_IMPEDANCE_NRT. + * @throw ExecutionException if error occurred during execution. + * @throw InputException if input is invalid. */ - bool isStopped(void) const; + void setCartesianStiffness(const std::vector& stiffness + = {4000, 4000, 4000, 1900, 1900, 1900}); + + /** + * @brief During Cartesian impedance modes, set preferred elbow swivel + * angle, which is the angle between the arm plane and the reference plane. + * @param[in] angle Swivel angle \f$ {\phi}~[rad] \f$, valid range: + * [-2.0944, 2.0944] rad, i.e. [-120, 120] deg. Default to the nominal + * swivel angle. + * @par Geometry definitions + * Arm plane: defined by the origin of 3 body frames: link2(shoulder), + * link4(elbow), and link6(wrist). + * Reference plane: defined by the origin of 3 body frames: base, + * link2(shoulder), and link6(wrist). + * Positive direction: defined as from link2 origin to link6 origin, right + * hand rule. + * @note Applicable modes: MODE_CARTESIAN_IMPEDANCE, + * MODE_CARTESIAN_IMPEDANCE_NRT. + * @throw ExecutionException if error occurred during execution. + * @throw InputException if input is invalid. + */ + void setSwivelAngle(double angle = 0); - //============================= IO METHODS ================================= + //=============================== IO CONTROL =============================== /** * @brief Set digital output on the control box. * @param[in] portNumber Port to set value to [0 ~ 15]. diff --git a/include/flexiv/Scheduler.hpp b/include/flexiv/Scheduler.hpp index e8c0b448..75bb9fed 100644 --- a/include/flexiv/Scheduler.hpp +++ b/include/flexiv/Scheduler.hpp @@ -64,7 +64,7 @@ class Scheduler * @brief Get number of tasks added to the scheduler * @return Number of added tasks */ - const unsigned int getTaskCount() const; + unsigned int getTaskCount() const; private: class Impl; diff --git a/include/flexiv/StatesData.hpp b/include/flexiv/StatesData.hpp index 2f8ccc4f..5e9b2a5a 100644 --- a/include/flexiv/StatesData.hpp +++ b/include/flexiv/StatesData.hpp @@ -23,56 +23,56 @@ struct RobotStates * * Size: \f$ \mathbb{R}^{Dof \times 1} \f$. */ - std::vector m_q = {}; + std::vector q = {}; /** * Measured motor-side joint positions \f$ \theta~[rad] \f$. * * Size: \f$ \mathbb{R}^{Dof \times 1} \f$. */ - std::vector m_theta = {}; + std::vector theta = {}; /** * Measured link-side joint velocities \f$ \dot{q}~[rad/s] \f$. * * Size: \f$ \mathbb{R}^{Dof \times 1} \f$. */ - std::vector m_dq = {}; + std::vector dq = {}; /** * Measured motor-side joint velocities \f$ \dot{\theta}~[rad/s] \f$. * * Size: \f$ \mathbb{R}^{Dof \times 1} \f$. */ - std::vector m_dtheta = {}; + std::vector dtheta = {}; /** * Measured joint torques \f$ \tau~[Nm] \f$. * * Size: \f$ \mathbb{R}^{Dof \times 1} \f$. */ - std::vector m_tau = {}; + std::vector tau = {}; /** * Desired joint torques \f$ \tau_{d}~[Nm] \f$. * * Size: \f$ \mathbb{R}^{Dof \times 1} \f$. */ - std::vector m_tauDes = {}; + std::vector tauDes = {}; /** * Numerical derivative of joint torques \f$ \dot{\tau}~[Nm/s] \f$. * * Size: \f$ \mathbb{R}^{Dof \times 1} \f$. */ - std::vector m_tauDot = {}; + std::vector tauDot = {}; /** * Estimated external joint torques \f$ \hat \tau_{ext}~[Nm] \f$. * * Size: \f$ \mathbb{R}^{Dof \times 1} \f$. */ - std::vector m_tauExt = {}; + std::vector tauExt = {}; /** * Measured TCP pose in base frame \f$ ^{O}T_{TCP}~[m][] \f$. @@ -81,7 +81,7 @@ struct RobotStates * position and \f$ \mathbb{R}^{4 \times 1} \f$ quaternion \f$ [x, y, z, * q_w, q_x, q_y, q_z]^T \f$. */ - std::vector m_tcpPose = {}; + std::vector tcpPose = {}; /** * Desired TCP pose in base frame \f$ {^{O}T_{TCP}}_{d}~[m][] \f$. @@ -90,7 +90,7 @@ struct RobotStates * position and \f$ \mathbb{R}^{4 \times 1} \f$ quaternion \f$ [x, y, z, * q_w, q_x, q_y, q_z]^T \f$. */ - std::vector m_tcpPoseDes = {}; + std::vector tcpPoseDes = {}; /** * Measured TCP velocity in base frame \f$ ^{O}\dot{X}~[m/s][rad/s] \f$. @@ -99,7 +99,7 @@ struct RobotStates * linear velocity and \f$ \mathbb{R}^{3 \times 1} \f$ angular velocity \f$ * [v_x, v_y, v_z, \omega_x, \omega_y, \omega_z]^T \f$. */ - std::vector m_tcpVel = {}; + std::vector tcpVel = {}; /** * Measured camera pose in base frame \f$ ^{O}T_{cam}~[m][] \f$. @@ -108,7 +108,7 @@ struct RobotStates * position and \f$ \mathbb{R}^{4 \times 1} \f$ quaternion \f$ [x, y, z, * q_w, q_x, q_y, q_z]^T \f$. */ - std::vector m_camPose = {}; + std::vector camPose = {}; /** * Measured flange pose in base frame \f$ ^{O}T_{fl}~[m][] \f$. @@ -117,7 +117,7 @@ struct RobotStates * position and \f$ \mathbb{R}^{4 \times 1} \f$ quaternion \f$ [x, y, z, * q_w, q_x, q_y, q_z]^T \f$. */ - std::vector m_flangePose = {}; + std::vector flangePose = {}; /** * Measured end link pose in base frame \f$ ^{O}T_{el}~[m][] \f$. @@ -126,7 +126,7 @@ struct RobotStates * position and \f$ \mathbb{R}^{4 \times 1} \f$ quaternion \f$ [x, y, z, * q_w, q_x, q_y, q_z]^T \f$. */ - std::vector m_endLinkPose = {}; + std::vector endLinkPose = {}; /** * Estimated external force applied on TCP in TCP frame @@ -136,7 +136,7 @@ struct RobotStates * force and \f$ \mathbb{R}^{3 \times 1} \f$ moment \f$ [f_x, f_y, f_z, m_x, * m_y, m_z]^T \f$. */ - std::vector m_extForceInTcpFrame = {}; + std::vector extForceInTcpFrame = {}; /** * Estimated external force vector applied on TCP in the base frame @@ -146,7 +146,7 @@ struct RobotStates * force and \f$ \mathbb{R}^{3 \times 1} \f$ moment \f$ [f_x, f_y, f_z, m_x, * m_y, m_z]^T \f$. */ - std::vector m_extForceInBaseFrame = {}; + std::vector extForceInBaseFrame = {}; }; /** @@ -159,47 +159,6 @@ struct RobotStates std::ostream& operator<<( std::ostream& ostream, const flexiv::RobotStates& robotStates); -/** - * @struct SystemStatus - * @brief Data struct containing status of the robot system. - */ -struct SystemStatus -{ - /** - * Emergency stop state - * @note True: E-stop released, false: E-stop pressed - */ - bool m_emergencyStop = {}; - - /** - * External active state - * @note True: robot executing user commands, false: robot waiting - * for user commands - */ - bool m_externalActive = {}; - - /** - * If user can send program request - * @note True: server can take new plan/primitive request from the client - */ - bool m_programRequest = {}; - - /** - * If a plan is currently running - */ - bool m_programRunning = {}; - - /** - * If the robot has reached target pose - */ - bool m_reachedTarget = {}; - - /** - * If joint limit is triggered - */ - bool m_jointLimitTriggered = {}; -}; - /** * @struct PlanInfo * @brief Data struct containing information of the on-going primitive/plan. @@ -209,37 +168,37 @@ struct PlanInfo /** * Current primitive name */ - std::string m_ptName = {}; + std::string ptName = {}; /** * Current node name */ - std::string m_nodeName = {}; + std::string nodeName = {}; /** * Current node path */ - std::string m_nodePath = {}; + std::string nodePath = {}; /** * Current node path time period */ - std::string m_nodePathTimePeriod = {}; + std::string nodePathTimePeriod = {}; /** * Current node path number */ - std::string m_nodePathNumber = {}; + std::string nodePathNumber = {}; /** * Assigned plan name */ - std::string m_assignedPlanName = {}; + std::string assignedPlanName = {}; /** * Velocity scale */ - double m_velocityScale = {}; + double velocityScale = {}; }; } /* namespace flexiv */ diff --git a/include/flexiv/Utility.hpp b/include/flexiv/Utility.hpp new file mode 100644 index 00000000..f64bcfa0 --- /dev/null +++ b/include/flexiv/Utility.hpp @@ -0,0 +1,143 @@ +/** + * @file Utility.hpp + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + */ + +#ifndef FLEXIVRDK_UTILITY_HPP_ +#define FLEXIVRDK_UTILITY_HPP_ + +#include +#include +#include + +namespace flexiv { +namespace utility { + +/** + * @brief Convert quaternion to Euler angles with ZYX axis rotations. + * @param[in] quat Quaternion input in [w,x,y,z] order. + * @return Euler angles in [x,y,z] order [rad]. + * @note The return value, when converted to degrees, is the same Euler angles + * used by Move primitives. + * @throw InputException if input is of wrong size. + */ +std::vector quat2EulerZYX(const std::vector& quat) +{ + // Check input size + if (quat.size() != 4) { + throw InputException( + "[flexiv::utility] quat2EulerZYX: input vector is not size 4"); + } + + // Form quaternion + Eigen::Quaterniond q(quat[0], quat[1], quat[2], quat[3]); + + // The returned vector is in [z,y,x] order + auto eulerZYX = q.toRotationMatrix().eulerAngles(2, 1, 0); + + // Convert to general [x,y,z] order + return (std::vector {eulerZYX[2], eulerZYX[1], eulerZYX[0]}); +} + +/** + * @brief Convert radians to degrees for a single value. + */ +double rad2Deg(double rad) +{ + constexpr double k_pi = 3.14159265358979323846; + return (rad / k_pi * 180.0); +} + +/** + * @brief Convert radians to degrees for a vector. + */ +std::vector rad2Deg(const std::vector& radVec) +{ + std::vector degVec = {}; + for (const auto& v : radVec) { + degVec.push_back(rad2Deg(v)); + } + return degVec; +} + +/** + * @brief Convert a std::vector to a string. + * @param[in] vec std::vector of any type and size. + * @return A string with format "vec[0] vec[1] ... vec[n] ", i.e. each value + * followed by a space, including the last one. + */ +template +std::string vec2Str(const std::vector& vec) +{ + std::string str = ""; + for (const auto& v : vec) { + str += std::to_string(v) + " "; + } + return str; +} + +/** + * @brief Check if any provided strings exist in the program arguments. + * @param[in] argc Argument count passed to main() of the program. + * @param[in] argv Argument vector passed to main() of the program, where + * argv[0] is the program name. + * @param[in] refStrs Reference strings to check against. + * @return True if the program arguments contain one or more reference strings. + */ +bool programArgsExistAny( + int argc, char** argv, const std::vector& refStrs) +{ + for (int i = 0; i < argc; i++) { + for (const auto& v : refStrs) { + if (v == std::string(argv[i])) { + return true; + } + } + } + return false; +} + +/** + * @brief Check if one specific string exists in the program arguments. + * @param[in] argc Argument count passed to main() of the program. + * @param[in] argv Argument vector passed to main() of the program, where + * argv[0] is the program name. + * @param[in] refStr Reference string to check against. + * @return True if the program arguments contain this specific reference string. + */ +bool programArgsExist(int argc, char** argv, const std::string& refStr) +{ + return programArgsExistAny(argc, argv, {refStr}); +} + +/** + * @brief Parse the value of a specified primitive state from the ptStates + * string list. + * @param[in] ptStates Primitive states string list returned from + * Robot::getPrimitiveStates(). + * @param[in] parseTarget Name of the primitive state to parse for. + * @return Value of the specified primitive state in string format. Empty string + * is returned if parseTarget does not exist. + */ +std::string parsePtStates( + const std::vector& ptStates, const std::string& parseTarget) +{ + for (const auto& state : ptStates) { + std::stringstream ss(state); + std::string buffer; + std::vector parsedState; + while (ss >> buffer) { + parsedState.push_back(buffer); + } + if (parsedState.front() == parseTarget) { + return parsedState.back(); + } + } + + return ""; +} + +} /* namespace utility */ +} /* namespace flexiv */ + +#endif /* FLEXIVRDK_UTILITY_HPP_ */ diff --git a/include/flexiv/Visualization.hpp b/include/flexiv/Visualization.hpp index f264f618..be378d7a 100644 --- a/include/flexiv/Visualization.hpp +++ b/include/flexiv/Visualization.hpp @@ -58,6 +58,20 @@ class Visualization void updateScene(const std::string& objectName, Eigen::Vector3d position, Eigen::Matrix3d orientation); + /** + * @brief Update scene visualization by specifying new color of an existing + * object in the scene. + * @param[in] objectName Name of the object in the scene to update, as + * specified in the scene URDF. + * @param[in] r Red component, 0 to 1 + * @param[in] g Green component, 0 to 1 + * @param[in] b Blue component, 0 to 1 + * @param[in] alpha Transparency, 0 to 1 + * @throw ExecutionException if error occurred during execution. + */ + void updateScene(const std::string& objectName, double r, double g, + double b, double alpha); + private: class Impl; std::unique_ptr m_pimpl; diff --git a/lib/FlexivRdk.x86_64-windows.lib b/lib/FlexivRdk.x86_64-windows.lib new file mode 100644 index 00000000..e7a6bf37 Binary files /dev/null and b/lib/FlexivRdk.x86_64-windows.lib differ diff --git a/lib/linux/cpp/arm64/libFlexivRdk.a b/lib/libFlexivRdk.aarch64-linux-gnu.a similarity index 61% rename from lib/linux/cpp/arm64/libFlexivRdk.a rename to lib/libFlexivRdk.aarch64-linux-gnu.a index acf3a556..72bb87de 100644 Binary files a/lib/linux/cpp/arm64/libFlexivRdk.a and b/lib/libFlexivRdk.aarch64-linux-gnu.a differ diff --git a/lib/libFlexivRdk.arm64-darwin.a b/lib/libFlexivRdk.arm64-darwin.a new file mode 100644 index 00000000..18ebb987 Binary files /dev/null and b/lib/libFlexivRdk.arm64-darwin.a differ diff --git a/lib/linux/cpp/x64/libFlexivRdk.a b/lib/libFlexivRdk.x86_64-linux-gnu.a similarity index 62% rename from lib/linux/cpp/x64/libFlexivRdk.a rename to lib/libFlexivRdk.x86_64-linux-gnu.a index 1edbeced..ed16e386 100644 Binary files a/lib/linux/cpp/x64/libFlexivRdk.a and b/lib/libFlexivRdk.x86_64-linux-gnu.a differ diff --git a/lib/linux/python/arm64/flexivrdk.so b/lib/linux/python/arm64/flexivrdk.so deleted file mode 100755 index 66a9050a..00000000 Binary files a/lib/linux/python/arm64/flexivrdk.so and /dev/null differ diff --git a/lib/linux/python/x64/flexivrdk.so b/lib/linux/python/x64/flexivrdk.so deleted file mode 100755 index 6fcad88d..00000000 Binary files a/lib/linux/python/x64/flexivrdk.so and /dev/null differ diff --git a/lib/windows/cpp/x64/FlexivRdk.lib b/lib/windows/cpp/x64/FlexivRdk.lib deleted file mode 100644 index e727d2fc..00000000 Binary files a/lib/windows/cpp/x64/FlexivRdk.lib and /dev/null differ diff --git a/lib/windows/cpp/x64/FvrBase.lib b/lib/windows/cpp/x64/FvrBase.lib deleted file mode 100644 index 4f51d07d..00000000 Binary files a/lib/windows/cpp/x64/FvrBase.lib and /dev/null differ diff --git a/lib/windows/cpp/x86/FlexivRdk.lib b/lib/windows/cpp/x86/FlexivRdk.lib deleted file mode 100644 index a0b11a9d..00000000 Binary files a/lib/windows/cpp/x86/FlexivRdk.lib and /dev/null differ diff --git a/lib/windows/cpp/x86/FvrBase.lib b/lib/windows/cpp/x86/FvrBase.lib deleted file mode 100644 index d3afb830..00000000 Binary files a/lib/windows/cpp/x86/FvrBase.lib and /dev/null differ diff --git a/lib_py/flexivrdk.cpython-38-aarch64-linux-gnu.so b/lib_py/flexivrdk.cpython-38-aarch64-linux-gnu.so new file mode 100755 index 00000000..801b91e0 Binary files /dev/null and b/lib_py/flexivrdk.cpython-38-aarch64-linux-gnu.so differ diff --git a/lib_py/flexivrdk.cpython-38-darwin.so b/lib_py/flexivrdk.cpython-38-darwin.so new file mode 100755 index 00000000..fbb4ad65 Binary files /dev/null and b/lib_py/flexivrdk.cpython-38-darwin.so differ diff --git a/lib_py/flexivrdk.cpython-38-x86_64-linux-gnu.so b/lib_py/flexivrdk.cpython-38-x86_64-linux-gnu.so new file mode 100755 index 00000000..91394f13 Binary files /dev/null and b/lib_py/flexivrdk.cpython-38-x86_64-linux-gnu.so differ diff --git a/spec/flexiv_rizon4_kinematics.urdf b/resources/flexiv_rizon4_kinematics.urdf similarity index 100% rename from spec/flexiv_rizon4_kinematics.urdf rename to resources/flexiv_rizon4_kinematics.urdf diff --git a/spec/meshes/collision/link0.stl b/resources/meshes/collision/link0.stl similarity index 100% rename from spec/meshes/collision/link0.stl rename to resources/meshes/collision/link0.stl diff --git a/spec/meshes/collision/link1.stl b/resources/meshes/collision/link1.stl similarity index 100% rename from spec/meshes/collision/link1.stl rename to resources/meshes/collision/link1.stl diff --git a/spec/meshes/collision/link2.stl b/resources/meshes/collision/link2.stl similarity index 100% rename from spec/meshes/collision/link2.stl rename to resources/meshes/collision/link2.stl diff --git a/spec/meshes/collision/link3.stl b/resources/meshes/collision/link3.stl similarity index 100% rename from spec/meshes/collision/link3.stl rename to resources/meshes/collision/link3.stl diff --git a/spec/meshes/collision/link4.stl b/resources/meshes/collision/link4.stl similarity index 100% rename from spec/meshes/collision/link4.stl rename to resources/meshes/collision/link4.stl diff --git a/spec/meshes/collision/link5.stl b/resources/meshes/collision/link5.stl similarity index 100% rename from spec/meshes/collision/link5.stl rename to resources/meshes/collision/link5.stl diff --git a/spec/meshes/collision/link6.stl b/resources/meshes/collision/link6.stl similarity index 100% rename from spec/meshes/collision/link6.stl rename to resources/meshes/collision/link6.stl diff --git a/spec/meshes/collision/link7.stl b/resources/meshes/collision/link7.stl similarity index 100% rename from spec/meshes/collision/link7.stl rename to resources/meshes/collision/link7.stl diff --git a/spec/meshes/visual/link0.stl b/resources/meshes/visual/link0.stl similarity index 100% rename from spec/meshes/visual/link0.stl rename to resources/meshes/visual/link0.stl diff --git a/spec/meshes/visual/link1.stl b/resources/meshes/visual/link1.stl similarity index 100% rename from spec/meshes/visual/link1.stl rename to resources/meshes/visual/link1.stl diff --git a/spec/meshes/visual/link2.stl b/resources/meshes/visual/link2.stl similarity index 100% rename from spec/meshes/visual/link2.stl rename to resources/meshes/visual/link2.stl diff --git a/spec/meshes/visual/link3.stl b/resources/meshes/visual/link3.stl similarity index 100% rename from spec/meshes/visual/link3.stl rename to resources/meshes/visual/link3.stl diff --git a/spec/meshes/visual/link4.stl b/resources/meshes/visual/link4.stl similarity index 100% rename from spec/meshes/visual/link4.stl rename to resources/meshes/visual/link4.stl diff --git a/spec/meshes/visual/link5.stl b/resources/meshes/visual/link5.stl similarity index 100% rename from spec/meshes/visual/link5.stl rename to resources/meshes/visual/link5.stl diff --git a/spec/meshes/visual/link6.stl b/resources/meshes/visual/link6.stl similarity index 100% rename from spec/meshes/visual/link6.stl rename to resources/meshes/visual/link6.stl diff --git a/spec/meshes/visual/link7.stl b/resources/meshes/visual/link7.stl similarity index 100% rename from spec/meshes/visual/link7.stl rename to resources/meshes/visual/link7.stl diff --git a/spec/scene.urdf b/resources/scene.urdf similarity index 100% rename from spec/scene.urdf rename to resources/scene.urdf diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 00000000..b399cd7c --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,38 @@ + +cmake_minimum_required(VERSION 3.4) +project(flexiv_rdk-tests) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_VERBOSE_MAKEFILE ON) +message("Building for system: ${CMAKE_SYSTEM_NAME}") + +# Configure build type +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE) +endif() + +# Tests for Mac/Windows +set(TEST_LIST + test_log +) + +# Additional tests for Linux only +if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") + set(TEST_LIST ${TEST_LIST} + test_dynamics_engine + test_dynamics_with_tool + test_endurance + test_loop_latency + test_scheduler + test_timeliness_monitor + ) +endif() + +# Find flexiv_rdk INTERFACE library +find_package(flexiv_rdk REQUIRED) + +# Build all selected examples +foreach(test ${TEST_LIST}) + add_executable(${test} ${test}.cpp) + target_link_libraries(${test} flexiv::flexiv_rdk) +endforeach() diff --git a/test/test_dynamics_engine.cpp b/test/test_dynamics_engine.cpp index 7f521495..1929e241 100644 --- a/test/test_dynamics_engine.cpp +++ b/test/test_dynamics_engine.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include @@ -43,8 +44,8 @@ std::mutex g_mutex; } /** User-defined high-priority periodic task @ 1kHz */ -void highPriorityTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, - flexiv::Scheduler* scheduler, flexiv::Log* log, flexiv::Model* model) +void highPriorityTask(flexiv::Robot* robot, flexiv::Scheduler* scheduler, + flexiv::Log* log, flexiv::Model* model, flexiv::RobotStates& robotStates) { try { // Monitor fault on robot server @@ -61,7 +62,7 @@ void highPriorityTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, robot->getRobotStates(robotStates); // Update robot model in dynamics engine - model->updateModel(robotStates->m_q, robotStates->m_dtheta); + model->updateModel(robotStates.q, robotStates.dtheta); // Get J, M, G from dynamic engine Eigen::MatrixXd J = model->getJacobian("flange"); @@ -135,6 +136,17 @@ void lowPriorityTask() std::cout << "Norm of delta G: " << deltaG.norm() << '\n' << std::endl; } +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { // log object for printing message with timestamp and coloring @@ -142,11 +154,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // check if program has 3 arguments - if (argc != 3) { - log.error("Invalid program arguments. Usage: "); - return 0; + if (argc < 3 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -171,7 +184,7 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } @@ -181,8 +194,15 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational + int secondsWaited = 0; while (!robot.isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } } log.info("Robot is now operational"); @@ -197,15 +217,11 @@ int main(int argc, char* argv[]) // Bring Robot To Home //============================================================================= robot.executePlanByName("PLAN-Home"); - // Wait for the plan to start - std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait for the execution to finish - flexiv::SystemStatus systemStatus; do { - robot.getSystemStatus(&systemStatus); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } while (systemStatus.m_programRunning); + std::this_thread::sleep_for(std::chrono::seconds(1)); + } while (robot.isBusy()); // Put mode back to IDLE robot.setMode(flexiv::MODE_IDLE); @@ -246,8 +262,8 @@ int main(int argc, char* argv[]) //============================================================================= flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.addTask(std::bind(highPriorityTask, &robot, &robotStates, - &scheduler, &log, &model), + scheduler.addTask(std::bind(highPriorityTask, &robot, &scheduler, &log, + &model, robotStates), "HP periodic", 1, 45); // Add periodic task with 1s interval and lowest applicable priority scheduler.addTask(lowPriorityTask, "LP periodic", 1000, 0); @@ -256,7 +272,7 @@ int main(int argc, char* argv[]) } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/test/test_dynamics_with_tool.cpp b/test/test_dynamics_with_tool.cpp index 0d6f33ea..e07d8342 100644 --- a/test/test_dynamics_with_tool.cpp +++ b/test/test_dynamics_with_tool.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include @@ -39,8 +40,8 @@ std::mutex g_mutex; } /** User-defined high-priority periodic task @ 1kHz */ -void highPriorityTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, - flexiv::Scheduler* scheduler, flexiv::Log* log, flexiv::Model* model) +void highPriorityTask(flexiv::Robot* robot, flexiv::Scheduler* scheduler, + flexiv::Log* log, flexiv::Model* model, flexiv::RobotStates& robotStates) { try { // Monitor fault on robot server @@ -54,7 +55,7 @@ void highPriorityTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, robot->getRobotStates(robotStates); // Update robot model in dynamics engine - model->updateModel(robotStates->m_q, robotStates->m_dtheta); + model->updateModel(robotStates.q, robotStates.dtheta); // Mark timer start point auto tic = std::chrono::high_resolution_clock::now(); @@ -123,6 +124,17 @@ void lowPriorityTask() std::cout << "Norm of delta G: " << deltaG.norm() << '\n' << std::endl; } +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { // log object for printing message with timestamp and coloring @@ -130,11 +142,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // check if program has 3 arguments - if (argc != 3) { - log.error("Invalid program arguments. Usage: "); - return 0; + if (argc < 3 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -159,7 +172,7 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } @@ -168,9 +181,16 @@ int main(int argc, char* argv[]) log.info("Enabling robot ..."); robot.enable(); - // wait for the robot to become operational + // Wait for the robot to become operational + int secondsWaited = 0; while (!robot.isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } } log.info("Robot is now operational"); @@ -185,15 +205,11 @@ int main(int argc, char* argv[]) // Bring Robot To Home //============================================================================= robot.executePlanByName("PLAN-Home"); - // wait for the plan to start - std::this_thread::sleep_for(std::chrono::seconds(1)); // wait for the execution to finish - flexiv::SystemStatus systemStatus; do { - robot.getSystemStatus(&systemStatus); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } while (systemStatus.m_programRunning); + std::this_thread::sleep_for(std::chrono::seconds(1)); + } while (robot.isBusy()); // put mode back to IDLE robot.setMode(flexiv::MODE_IDLE); @@ -239,8 +255,8 @@ int main(int argc, char* argv[]) //============================================================================= flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.addTask(std::bind(highPriorityTask, &robot, &robotStates, - &scheduler, &log, &model), + scheduler.addTask(std::bind(highPriorityTask, &robot, &scheduler, &log, + &model, robotStates), "HP periodic", 1, 45); // Add periodic task with 1s interval and lowest applicable priority scheduler.addTask(lowPriorityTask, "LP periodic", 1000, 0); @@ -249,7 +265,7 @@ int main(int argc, char* argv[]) } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/test/test_endurance.cpp b/test/test_endurance.cpp index 12c9c1f3..a40f1ddb 100644 --- a/test/test_endurance.cpp +++ b/test/test_endurance.cpp @@ -11,7 +11,9 @@ #include #include #include +#include +#include #include #include #include @@ -52,9 +54,8 @@ struct LogData } -// callback function for realtime periodic task -void highPriorityTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, - flexiv::Scheduler* scheduler, flexiv::Log* log) +void highPriorityTask(flexiv::Robot* robot, flexiv::Scheduler* scheduler, + flexiv::Log* log, flexiv::RobotStates& robotStates) { // flag whether initial Cartesian position is set static bool isInitPoseSet = false; @@ -75,15 +76,11 @@ void highPriorityTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, // TCP movement control //===================================================================== - // use target TCP velocity and acceleration = 0 - std::vector tcpVel(6, 0); - std::vector tcpAcc(6, 0); - // set initial TCP pose if (!isInitPoseSet) { // check vector size before saving - if (robotStates->m_tcpPose.size() == k_cartPoseSize) { - initTcpPose = robotStates->m_tcpPose; + if (robotStates.tcpPose.size() == k_cartPoseSize) { + initTcpPose = robotStates.tcpPose; g_currentTcpPose = initTcpPose; isInitPoseSet = true; } @@ -95,13 +92,13 @@ void highPriorityTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, + k_swingAmp * sin(2 * M_PI * k_swingFreq * g_hpLoopCounter * k_loopPeriod); - robot->streamTcpPose(g_currentTcpPose, tcpVel, tcpAcc); + robot->streamTcpPose(g_currentTcpPose); } // save data to global buffer, not using mutex to avoid interruption on // RT loop from potential priority inversion - g_logData.tcpPose = robotStates->m_tcpPose; - g_logData.tcpForce = robotStates->m_extForceInBaseFrame; + g_logData.tcpPose = robotStates.tcpPose; + g_logData.tcpForce = robotStates.extForceInBaseFrame; // increment loop counter g_hpLoopCounter++; @@ -200,6 +197,18 @@ void lowPriorityTask() } } +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP] [test hours]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << " test hours: duration of the test, can have decimals" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { // log object for printing message with timestamp and coloring @@ -207,14 +216,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // check if program has 3 arguments - if (argc != 4) { - log.error( - "Invalid program arguments. Usage: " - ""); - log.info("Note: can be a float number"); - return 0; + if (argc < 4 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -246,7 +253,7 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } @@ -255,9 +262,16 @@ int main(int argc, char* argv[]) log.info("Enabling robot ..."); robot.enable(); - // wait for the robot to become operational + // Wait for the robot to become operational + int secondsWaited = 0; while (!robot.isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } } log.info("Robot is now operational"); @@ -273,18 +287,10 @@ int main(int argc, char* argv[]) robot.executePlanByName("PLAN-Home"); - flexiv::SystemStatus systemStatus; - // wait until execution begin - while (systemStatus.m_programRunning == false) { - robot.getSystemStatus(&systemStatus); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - - // wait until execution finished + // Wait fot the plan to finish do { - robot.getSystemStatus(&systemStatus); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } while (systemStatus.m_programRunning); + std::this_thread::sleep_for(std::chrono::seconds(1)); + } while (robot.isBusy()); // set mode after robot is at home robot.setMode(flexiv::MODE_CARTESIAN_IMPEDANCE); @@ -302,14 +308,14 @@ int main(int argc, char* argv[]) flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority scheduler.addTask( - std::bind(highPriorityTask, &robot, &robotStates, &scheduler, &log), + std::bind(highPriorityTask, &robot, &scheduler, &log, robotStates), "HP periodic", 1, 45); // Start all added tasks, this is by default a blocking method scheduler.start(); } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/test/test_log.cpp b/test/test_log.cpp index 4dc6991f..094ee4ef 100644 --- a/test/test_log.cpp +++ b/test/test_log.cpp @@ -6,19 +6,30 @@ */ #include +#include + +#include #include +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: None" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { + if (flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; + } + // log object for printing message with timestamp and coloring flexiv::Log log; - // check if program has 3 arguments - if (argc != 1) { - log.error("No program argument is needed"); - return 0; - } - // print info message log.info("This is an INFO message with timestamp and GREEN coloring"); std::this_thread::sleep_for(std::chrono::seconds(1)); diff --git a/test/test_loop_latency.cpp b/test/test_loop_latency.cpp index ecd7a396..08e2e222 100644 --- a/test/test_loop_latency.cpp +++ b/test/test_loop_latency.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include @@ -83,6 +84,19 @@ void periodicTask( } } +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP] [serial port name]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << " serial port name: /dev/ttyS0 for COM1, /dev/ttyS1 for " + "COM2, /dev/ttyUSB0 for USB-serial converter" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { // log object for printing message with timestamp and coloring @@ -90,17 +104,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // check if program has 3 arguments - if (argc != 4) { - log.error( - "Invalid program arguments. Usage: " - ""); - log.info( - "Some examples of : /dev/ttyS0 corresponds to " - "COM1, /dev/ttyS1 corresponds to COM2, /dev/ttyUSB0 corresponds to " - "a USB-serial converter"); - return 0; + if (argc < 4 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -125,7 +134,7 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } @@ -134,9 +143,16 @@ int main(int argc, char* argv[]) log.info("Enabling robot ..."); robot.enable(); - // wait for the robot to become operational + // Wait for the robot to become operational + int secondsWaited = 0; while (!robot.isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } } log.info("Robot is now operational"); @@ -164,7 +180,7 @@ int main(int argc, char* argv[]) } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/test/test_scheduler.cpp b/test/test_scheduler.cpp index 18be7cdb..bc02a5c6 100644 --- a/test/test_scheduler.cpp +++ b/test/test_scheduler.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -90,6 +91,15 @@ void lowPriorityTask(flexiv::Log* log) + std::to_string(avgInterval) + " us"); } +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: None" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { // log object for printing message with timestamp and coloring @@ -97,10 +107,9 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // check if program has 3 arguments - if (argc != 1) { - log.error("No program argument is required"); - return 0; + if (flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } try { @@ -123,7 +132,7 @@ int main(int argc, char* argv[]) } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0; diff --git a/test/test_timeliness_monitor.cpp b/test/test_timeliness_monitor.cpp index 694e48a8..af1ed7c6 100644 --- a/test/test_timeliness_monitor.cpp +++ b/test/test_timeliness_monitor.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -33,8 +34,8 @@ const std::vector k_impedanceKd } // callback function for realtime periodic task -void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, - flexiv::Scheduler* scheduler, flexiv::Log* log) +void periodicTask(flexiv::Robot* robot, flexiv::Scheduler* scheduler, + flexiv::Log* log, flexiv::RobotStates& robotStates) { // Loop counter static unsigned int loopCounter = 0; @@ -58,8 +59,8 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, // Set initial joint position if (!isInitPositionSet) { // check vector size before saving - if (robotStates->m_q.size() == k_robotDofs) { - initPosition = robotStates->m_q; + if (robotStates.q.size() == k_robotDofs) { + initPosition = robotStates.q; isInitPositionSet = true; } } @@ -72,9 +73,8 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, // impedance control on all joints for (size_t i = 0; i < k_robotDofs; ++i) { torqueDesired[i] - = k_impedanceKp[i] - * (targetPosition[i] - robotStates->m_q[i]) - - k_impedanceKd[i] * robotStates->m_dtheta[i]; + = k_impedanceKp[i] * (targetPosition[i] - robotStates.q[i]) + - k_impedanceKd[i] * robotStates.dtheta[i]; } // send target joint torque to RDK server @@ -97,6 +97,17 @@ void periodicTask(flexiv::Robot* robot, flexiv::RobotStates* robotStates, } } +void printHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; + std::cout << " robot IP: address of the robot server" << std::endl; + std::cout << " local IP: address of this PC" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + int main(int argc, char* argv[]) { // log object for printing message with timestamp and coloring @@ -104,11 +115,12 @@ int main(int argc, char* argv[]) // Parse Parameters //============================================================================= - // check if program has 3 arguments - if (argc != 3) { - log.error("Invalid program arguments. Usage: "); - return 0; + if (argc < 3 + || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; } + // IP of the robot server std::string robotIP = argv[1]; @@ -133,7 +145,7 @@ int main(int argc, char* argv[]) // Check again if (robot.isFault()) { log.error("Fault cannot be cleared, exiting ..."); - return 0; + return 1; } log.info("Fault on robot server is cleared"); } @@ -142,9 +154,16 @@ int main(int argc, char* argv[]) log.info("Enabling robot ..."); robot.enable(); - // wait for the robot to become operational + // Wait for the robot to become operational + int secondsWaited = 0; while (!robot.isOperational()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++secondsWaited == 10) { + log.warn( + "Still waiting for robot to become operational, please " + "check that the robot 1) has no fault, 2) is booted " + "into Auto mode"); + } } log.info("Robot is now operational"); @@ -165,14 +184,14 @@ int main(int argc, char* argv[]) flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority scheduler.addTask( - std::bind(periodicTask, &robot, &robotStates, &scheduler, &log), + std::bind(periodicTask, &robot, &scheduler, &log, robotStates), "HP periodic", 1, 45); // Start all added tasks, this is by default a blocking method scheduler.start(); } catch (const flexiv::Exception& e) { log.error(e.what()); - return 0; + return 1; } return 0;