diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml new file mode 100644 index 0000000..f268a8c --- /dev/null +++ b/.github/workflows/cmake.yml @@ -0,0 +1,21 @@ +name: CMake + +on: + push: + branches: [main] + pull_request: + branches: [main] + workflow_dispatch: + +env: + # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) + BUILD_TYPE: Release + +jobs: + build-ubuntu-22: + # GitHub-hosted Ubuntu 22.04 runner + runs-on: ubuntu-22.04 + # Use shared steps + steps: + - uses: actions/checkout@v3 + - uses: ./.github/workflows/shared_steps diff --git a/.github/workflows/shared_steps/action.yml b/.github/workflows/shared_steps/action.yml new file mode 100644 index 0000000..90defda --- /dev/null +++ b/.github/workflows/shared_steps/action.yml @@ -0,0 +1,31 @@ +name: "Shared Build Steps" +runs: + using: "composite" + # Note: working directory will be reset to the repo root for each new step + steps: + # Build and install all dependencies to RDK installation directory. + - name: Build and install dependencies + shell: bash + run: | + pwd + cd thirdparty + bash build_and_install_dependencies.sh ~/sim_plugin_install 4 + + # Configure CMake, then build and install flexiv_rdk library to RDK installation directory. + - name: Build and install library + shell: bash + run: | + pwd + mkdir -p build && cd build + cmake .. -DCMAKE_INSTALL_PREFIX=~/sim_plugin_install + cmake --build . --target install --config Release + + # Find and link to flexiv_rdk library, then build all example programs. + - name: Build examples + shell: bash + run: | + pwd + cd example + mkdir -p build && cd build + cmake .. -DCMAKE_PREFIX_PATH=~/sim_plugin_install + cmake --build . --config Release -j 4 diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100755 index 0000000..2520a8a --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 3.16.3) + +# =================================================================== +# PROJECT SETUP +# =================================================================== +project(flexiv_sim_plugin VERSION 1.1.0) + +# Configure build type +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE) +endif() +set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Release" "Debug" "RelWithDebInfo") + +# Set static library according to platform +message(STATUS "OS: ${CMAKE_SYSTEM_NAME}") +message(STATUS "Processor: ${CMAKE_SYSTEM_PROCESSOR}") +if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") + if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64") + set(SIM_PLUGIN_STATIC_LIB "libflexiv_sim_plugin.x86_64-linux-gnu.a") + else() + message(FATAL_ERROR "Linux with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") + endif() +elseif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + message(FATAL_ERROR "macOS is currently not supported.") +elseif(${CMAKE_SYSTEM_NAME} MATCHES "Windows") + message(FATAL_ERROR "Windows is currently not supported.") +endif() + +# =================================================================== +# PROJECT DEPENDENCIES +# =================================================================== +# Threads +set(THREADS_PREFER_PTHREAD_FLAG ON) +find_package(Threads REQUIRED) +if(Threads_FOUND) + message(STATUS "Found Threads: HAVE_PTHREAD = ${THREADS_HAVE_PTHREAD_ARG}") +endif() + +# spdlog +find_package(spdlog REQUIRED) +if(spdlog_FOUND) + message(STATUS "Found spdlog: ${spdlog_DIR}") +endif() + +# Fast-DDS (Fast-RTPS) +find_package(fastrtps 2.6.7 REQUIRED) +if(fastrtps_FOUND) + message(STATUS "Found fastrtps: ${fastrtps_DIR}") +endif() + +# =================================================================== +# CREATE LIBRARY +# =================================================================== +# Create an INTERFACE library with no source file to compile +add_library(${PROJECT_NAME} INTERFACE) + +# 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}) + +target_include_directories(${PROJECT_NAME} INTERFACE + $ + $ +) + +target_link_libraries(${PROJECT_NAME} INTERFACE + ${CMAKE_CURRENT_SOURCE_DIR}/lib/${SIM_PLUGIN_STATIC_LIB} + Threads::Threads + spdlog::spdlog + fastrtps +) + +# Use moderate compiler warning option +if(CMAKE_HOST_UNIX) + target_compile_options(${PROJECT_NAME} INTERFACE -Wall -Wextra) +else() + target_compile_options(${PROJECT_NAME} INTERFACE /W1) +endif() + +# Install the INTERFACE library +include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/FlexivInstallLibrary.cmake) +FlexivInstallLibrary() diff --git a/README.md b/README.md index 49f2660..ff233af 100644 --- a/README.md +++ b/README.md @@ -11,6 +11,108 @@ A middleware plugin to connect Flexiv Elements Studio to any external simulator | --------------------- | ------------ | -------------------- | ---------------------- | | Linux (Ubuntu 20.04+) | x86_64 | GCC v9.4+ | 3.8, 3.10, 3.12 | +## Tested External Simulators + +The following external simulators are tested and known to work with Flexiv Sim Plugin: + +- NVIDIA [Isaac Sim](https://developer.nvidia.com/isaac/sim) (a template workspace is available [here](https://github.com/flexivrobotics/isaac_sim_ws)) +- [MuJoCo](https://mujoco.org/) + +In theory, any simulator that meets the following criteria should work: + +1. Has a C++ or Python interface. +2. Provides joint positions and velocities of the simulated robot. +3. The joints of the simulated robot are actuated by torque. + +## Flexiv Elements Studio Setup + +### Install Elements Studio + +1. Prepare a Ubuntu 22.04 computer, all operations below are done on this computer. +2. [Contact Flexiv](https://www.flexiv.com/contact) to obtain the installation package of Elements Studio. +3. Extract the package to a non-root directory. +4. Install Elements Studio: + + bash setup_FlexivElements.sh + +5. Switch physics engine from the default built-in to external: + + bash switch_physics_engine.sh + + Select *External* or *Isaac Sim* when prompted. + +### Create a simulated robot in Elements Studio + +1. Start Flexiv Elements Studio from the application menu. +2. In the Robot Connection window, select *Simulator*, and click *CREATE*. +3. Choose "Create according to the selected robot type" and select one from the list, then click *CONFIRM*. A new simulated robot will be added to the simulator list. +4. Toggle on the *Connect* button for the newly added one, then wait for loading. +5. When loading is finished, you'll see a robot at its upright pose, with an "Exception" error at the bottom right corner. This is expected because the external simulator is not started yet. But if you see a normally operating robot, that again means you are running the wrong version of Elements Studio that only supports the built-in physics engine. +6. At the bottom of the window, click on the small robot icon with a "SIM" tag on it, then a small window will pop up, note down the displayed robot serial number. +7. In the same small pop-up window, click *CHANGE CONNECTION*, then toggle off the *Connect* button to close the simulated robot. We will restart it later. Note that you do NOT need to close the whole Elements Studio program. + +## Quick Start - C++ + +### Prepare build tools + +#### Linux + +1. Install compiler kit using package manager: + + sudo apt install build-essential + +2. Install CMake using package manager: + + sudo apt install cmake + +### Install the C++ library + +The following steps are identical on all supported platforms. + +1. Choose a directory for installing the C++ library of Sim Plugin and its dependencies. This directory can be under system path or not, depending on whether you want Sim Plugin to be globally discoverable by CMake. For example, a new folder named ``sim_plugin_install`` under the home directory. +2. In a new Terminal, run the provided script to compile and install all dependencies to the installation directory chosen in step 1: + + cd flexiv_sim_plugin/thirdparty + bash build_and_install_dependencies.sh ~/sim_plugin_install + +3. In a new Terminal, configure the ``flexiv_sim_plugin`` CMake project: + + cd flexiv_sim_plugin + mkdir build && cd build + cmake .. -DCMAKE_INSTALL_PREFIX=~/sim_plugin_install + + NOTE: ``-D`` followed by ``CMAKE_INSTALL_PREFIX`` sets the absolute path of the installation directory, which should be the one chosen in step 1. + +4. Install ``flexiv_sim_plugin`` C++ library to ``CMAKE_INSTALL_PREFIX`` path, which may or may not be globally discoverable by CMake: + + cd flexiv_sim_plugin/build + cmake --build . --target install --config Release + +### Use the installed C++ library + +After the library is installed as ``flexiv_sim_plugin`` CMake target, it can be linked from any other CMake projects. Using the provided `flexiv_sim_plugin-examples` project for instance: + + cd flexiv_sim_plugin/example + mkdir build && cd build + cmake .. -DCMAKE_PREFIX_PATH=~/sim_plugin_install + cmake --build . --config Release -j 4 + +NOTE: ``-D`` followed by ``CMAKE_PREFIX_PATH`` tells the user project's CMake where to find the installed C++ library. This argument can be skipped if the Sim Plugin library and its dependencies are installed to a globally discoverable location. + +### Run the example C++ program + +An example program that mocks an external simulator is provided and can be used to test the plugin with the following steps: + +1. Start the mock program: + + cd flexiv_sim_plugin/example/build + ./mock_external_simulator [robot_serial_number] + + NOTE: the robot serial number provided to the program is the same one you noted down when creating the simulated robot in Flexiv Elements Studio. + +2. Go back to Elements Studio, then restart the exited simulator by toggling ON the *Connect* button. +3. Wait for the connection to establish. If the connection is successful, you should see the visualized robot in Elements Studio moving every joint back and forth. A software error should occur in Elements Studio which is expected because the mock external simulator did not close the loop by applying the calculated joint torques command to the simulated robot in it. This won't happen to real external simulators. + ## Quick Start - Python ### Install the Python package @@ -32,6 +134,19 @@ After the ``flexivsimplugin`` Python package is installed, it can be imported fr The program should print some info messages and "Connected = False" at the end. +### Run the example Python script + +An example script that mocks an external simulator is provided and can be used to test the plugin with the following steps: + +1. Start the mock program: + + cd flexiv_sim_plugin/example_py + python3.x ./mock_external_simulator.py [robot_serial_number] + + NOTE: the robot serial number provided to the program is the same one you noted down when creating the simulated robot in Flexiv Elements Studio. + +2. The remaining steps are the same as documented in [Run the example C++ program](#run-the-example-c-program). + ## API Documentation The API documentation can be generated using Doxygen. For example, on Linux: diff --git a/cmake/FlexivInstallLibrary.cmake b/cmake/FlexivInstallLibrary.cmake new file mode 100644 index 0000000..1d2d0d6 --- /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 ${PROJECT_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 Sim Plugin") + 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_sim_plugin-config.cmake.in b/cmake/flexiv_sim_plugin-config.cmake.in new file mode 100644 index 0000000..2666693 --- /dev/null +++ b/cmake/flexiv_sim_plugin-config.cmake.in @@ -0,0 +1,10 @@ +include(CMakeFindDependencyMacro) + +# Find dependency +set(THREADS_PREFER_PTHREAD_FLAG ON) +find_dependency(Threads REQUIRED) +find_dependency(spdlog REQUIRED) +find_dependency(fastrtps 2.6.7 REQUIRED) + +# Add targets file +include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake") diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt new file mode 100644 index 0000000..ca19629 --- /dev/null +++ b/example/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.16.3) +project(flexiv_sim_plugin-examples) + +# Show verbose build info +SET(CMAKE_VERBOSE_MAKEFILE ON) + +message("OS: ${CMAKE_SYSTEM_NAME}") +message("Processor: ${CMAKE_SYSTEM_PROCESSOR}") + +# Configure build type +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE) +endif() + +# Minimum example list for all OS +set(EXAMPLE_LIST + mock_external_simulator +) + +# Find flexiv_sim_plugin INTERFACE library +find_package(flexiv_sim_plugin REQUIRED) + +# Build all selected examples +foreach(example ${EXAMPLE_LIST}) + add_executable(${example} ${example}.cpp) + target_link_libraries(${example} flexiv::flexiv_sim_plugin) + + # C++17 required + set_target_properties(${example} PROPERTIES + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED ON) +endforeach() diff --git a/example/mock_external_simulator.cpp b/example/mock_external_simulator.cpp new file mode 100644 index 0000000..943669c --- /dev/null +++ b/example/mock_external_simulator.cpp @@ -0,0 +1,131 @@ +/** + * @example mock_external_simulator.cpp + * A simple example demonstrating how to use Flexiv Sim Plugin in an external simulator. Please + * refer to README on how to run this example. + * @copyright Copyright (C) 2016-2025 Flexiv Ltd. All Rights Reserved. + * @author Flexiv + */ + +#include +#include + +#include +#include +#include +#include + +using namespace flexiv; + +namespace { +// Frequency of the external simulator's physics loop [Hz] +constexpr unsigned int kPhysicsFreq = 2000; +// Period of the external simulator's physics loop [sec] +constexpr double kPhysicsPeriod = 1.0 / kPhysicsFreq; + +// Sine-sweep trajectory amplitude and frequency +constexpr double kSineAmp = 0.035; +constexpr double kSineFreq = 0.3; + +// Initial joint positions when the robot is at home posture [rad] +constexpr std::array kInitQ + = {0.0, -0.698132, 0.0, 1.5708, 0.0, 0.698132, 0.0}; + +// Servo cycle of the physics loop +unsigned int g_servo_cycle = 0; +} + +/** @brief Print program usage help */ +void PrintHelp() +{ + // clang-format off + std::cout << "Required arguments: [robot_sn]" << std::endl; + std::cout << " robot_sn: Serial number of the robot in Flexiv Elements Studio to connect. Remove any space, e.g. Rizon4s-123456" << std::endl; + std::cout << "Optional arguments: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + +/** @brief Step once the external simulator's physics loop. */ +void StepPhysics(sim_plugin::UserNode& user_node) +{ + // Record time point at the beginning of this cycle + auto current_time = std::chrono::steady_clock::now(); + + // Step 1: obtain robot states from the external simulator + // ============================================================================================= + // Mocked robot states + sim_plugin::SimRobotStates robot_states; + + // Set and increment servo cycle per physics step + robot_states.servo_cycle = g_servo_cycle++; + + // Set joint positions to sine waves + for (size_t i = 0; i < sim_plugin::kJointDoF; i++) { + robot_states.q[i] + = kInitQ[i] + kSineAmp * sin(2 * M_PI * kSineFreq * g_servo_cycle * kPhysicsPeriod); + } + + // Set joint velocities to corresponding cosine waves + for (size_t i = 0; i < sim_plugin::kJointDoF; i++) { + robot_states.dq[i] = kSineAmp * 2 * M_PI * kSineFreq + * cos(2 * M_PI * kSineFreq * g_servo_cycle * kPhysicsPeriod); + } + + // Step 2: send robot states to Flexiv Elements Studio + // ============================================================================================= + if (!user_node.SendRobotStates(robot_states)) { + spdlog::error("Failed to send robot states data"); + } + + // Step 3: wait for Elements Studio to calculate and deliver new robot commands + // ============================================================================================= + // Must set a timeout value to avoid deadlock + constexpr unsigned int kWaitTimeoutMs = 100; + if (!user_node.WaitForRobotCommands(kWaitTimeoutMs)) { + spdlog::error("WaitForRobotCommands() timeout"); + } + + // Step 4: apply joint torques command to the simulated robot in the external simulator + // ============================================================================================= + auto target_joint_torques = user_node.robot_commands().tau_d; + // Call the external simulator's API to apply the target joint torques + + // End the current physics step with kPhysicsPeriod as the total loop period + auto loop_period_us = static_cast(kPhysicsPeriod * 1'000'000); + std::this_thread::sleep_until(current_time + std::chrono::microseconds(loop_period_us)); +} + +int main(int argc, char* argv[]) +{ + // Program Setup + // ============================================================================================= + // Parse parameters + if (argc < 2) { + PrintHelp(); + return 1; + } + // Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456 + std::string robot_sn = argv[1]; + + // Print description + spdlog::info( + ">>> Tutorial description <<<\nA simple example demonstrating how to use Flexiv Sim Plugin " + "in an external simulator.\n"); + + // Create user node + sim_plugin::UserNode user_node(robot_sn); + + // Wait for connected + while (!user_node.connected()) { + spdlog::warn("Waiting for connection with Flexiv Elements Studio"); + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + spdlog::info("Connected with Flexiv Elements Studio, starting mocked physics loop"); + + // Run mocked physics loop + while (user_node.connected()) { + StepPhysics(user_node); + } + + return 0; +} diff --git a/example_py/mock_external_simulator.py b/example_py/mock_external_simulator.py new file mode 100644 index 0000000..09d1e86 --- /dev/null +++ b/example_py/mock_external_simulator.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python + +"""mock_external_simulator.py + +A simple example demonstrating how to use Flexiv Sim Plugin in an external simulator. Please +refer to README on how to run this example. +""" + +__copyright__ = "Copyright (C) 2016-2025 Flexiv Ltd. All Rights Reserved." +__author__ = "Flexiv" + +import time +import math +import argparse +import spdlog # pip install +import flexivsimplugin # pip install + +# Frequency of the external simulator's physics loop [Hz] +PHYSICS_FREQ = 2000 +# Period of the external simulator's physics loop [sec] +PHYSICS_PERIOD = 1.0 / PHYSICS_FREQ + +# Sine-sweep trajectory amplitude and frequency +SINE_AMP = 0.035 +SINE_FREQ = 0.3 + +# Initial joint positions when the robot is at home posture [rad] +INIT_Q = [0.0, -0.698132, 0.0, 1.5708, 0.0, 0.698132, 0.0] + +# Joint degrees of freedom +JOINT_DOF = 7 + +# Servo cycle of the physics loop +g_servo_cycle = 0 + + +def step_physics(user_node, logger): + """ + Step once the external simulator's physics loop. + + """ + + # Step 1: obtain robot states from the external simulator + # ============================================================================================= + # Mocked robot states + robot_states = flexivsimplugin.SimRobotStates() + + # Set and increment servo cycle per physics step + global g_servo_cycle + g_servo_cycle += 1 + robot_states.servo_cycle = g_servo_cycle + + # robot_states.q and dq are fixed-size tuples and don't support per-element assignment + temp_list = [0] * JOINT_DOF + + # Set joint positions to sine waves + for i in range(JOINT_DOF): + temp_list[i] = INIT_Q[i] + SINE_AMP * math.sin( + 2 * math.pi * SINE_FREQ * g_servo_cycle * PHYSICS_PERIOD + ) + robot_states.q = temp_list + + # Set joint velocities to corresponding cosine waves + for i in range(JOINT_DOF): + temp_list[i] = ( + SINE_AMP + * 2 + * math.pi + * SINE_FREQ + * math.cos(2 * math.pi * SINE_FREQ * g_servo_cycle * PHYSICS_PERIOD) + ) + robot_states.dq = temp_list + + # Step 2: send robot states to Flexiv Elements Studio + # ============================================================================================= + if not user_node.SendRobotStates(robot_states): + logger.error("Failed to send robot states data") + + # Step 3: wait for Elements Studio to calculate and deliver new robot commands + # ============================================================================================= + # Must set a timeout value to avoid deadlock + WAIT_TIMEOUT_MS = 100 + if not user_node.WaitForRobotCommands(WAIT_TIMEOUT_MS): + logger.error("WaitForRobotCommands() timeout") + + # Step 4: apply joint torques command to the simulated robot in the external simulator + # ============================================================================================= + target_joint_torques = user_node.robot_commands().tau_d + # Call the external simulator's API to apply the target joint torques + + # End the current physics step with kPhysicsPeriod as roughly the total loop period + time.sleep(PHYSICS_PERIOD) + + +def main(): + # Program Setup + # ============================================================================================== + # Parse arguments + argparser = argparse.ArgumentParser() + argparser.add_argument( + "robot_sn", + help="Serial number of the robot in Flexiv Elements Studio to connect. Remove any space, e.g. Rizon4s-123456", + ) + args = argparser.parse_args() + + # Create logger + logger = spdlog.ConsoleLogger("Example") + + # Print description + logger.info( + ">>> Tutorial description <<<\nA simple example demonstrating how to use Flexiv Sim Plugin " + "in an external simulator.\n" + ) + + # Create user node + user_node = flexivsimplugin.UserNode(args.robot_sn) + + # Wait for connected + while not user_node.connected(): + logger.warn("Waiting for connection with Flexiv Elements Studio") + time.sleep(1) + logger.info("Connected with Flexiv Elements Studio, starting mocked physics loop") + + # Run mocked physics loop + while user_node.connected(): + step_physics(user_node, logger) + + +if __name__ == "__main__": + main() diff --git a/include/flexiv/sim_plugin/data.hpp b/include/flexiv/sim_plugin/data.hpp new file mode 100644 index 0000000..67eaf47 --- /dev/null +++ b/include/flexiv/sim_plugin/data.hpp @@ -0,0 +1,70 @@ +/** + * @file data.hpp + * @brief Header file containing various constant expressions and data structures. + * @copyright Copyright (C) 2016-2025 Flexiv Ltd. All Rights Reserved. + */ + +#ifndef FLEXIV_SIM_PLUGIN_DATA_HPP_ +#define FLEXIV_SIM_PLUGIN_DATA_HPP_ + +#include +#include + +namespace flexiv { +namespace sim_plugin { + +/** Joint-space degrees of freedom of the simulated robot */ +constexpr size_t kJointDoF = 7; + +/** Number of simulated digital IO ports */ +constexpr size_t kIOPorts = 16; + +/** States data of a simulated robot in the external simulator */ +struct SimRobotStates +{ + /** @brief Customized constructor */ + SimRobotStates(uint64_t _servo_cycle, const std::array& _q, + const std::array& _dq) + : servo_cycle(_servo_cycle) + , q(_q) + , dq(_dq) + { + } + /** @brief Default constructor */ + SimRobotStates() = default; + + /** Servo cycle incremented once per physics step of the external simulator */ + uint64_t servo_cycle = 0; + + /** Current joint positions of the simulated robot [rad] */ + std::array q = {}; + + /** Current joint velocities of the simulated robot [rad/s] */ + std::array dq = {}; +}; + +/** Commands data for a simulated robot in the external simulator */ +struct SimRobotCommands +{ + /** @brief Customized constructor */ + SimRobotCommands(const std::array& _tau_d, + const std::array& _digital_outputs) + : tau_d(_tau_d) + , digital_outputs(_digital_outputs) + { + } + /** @brief Default constructor */ + SimRobotCommands() = default; + + /** Target joint torques for the simulated robot [Nm]. */ + std::array tau_d = {}; + + /** Desired digital outputs for the simulated robot. The index of this boolean array corresponds + * to that of the digital output ports. True: port high, false: port low. */ + std::array digital_outputs = {}; +}; + +} /* namespace sim_plugin */ +} /* namespace flexiv */ + +#endif /* FLEXIV_SIM_PLUGIN_DATA_HPP_ */ diff --git a/include/flexiv/sim_plugin/user_node.hpp b/include/flexiv/sim_plugin/user_node.hpp new file mode 100644 index 0000000..1d69b49 --- /dev/null +++ b/include/flexiv/sim_plugin/user_node.hpp @@ -0,0 +1,68 @@ +/** + * @file user_node.hpp + * @copyright Copyright (C) 2016-2025 Flexiv Ltd. All Rights Reserved. + */ + +#ifndef FLEXIV_SIM_PLUGIN_USER_NODE_HPP_ +#define FLEXIV_SIM_PLUGIN_USER_NODE_HPP_ + +#include "data.hpp" +#include + +namespace flexiv { +namespace sim_plugin { + +/** + * @class UserNode + * @brief A communication node for an external simulator to publish robot states to and receive + * robot commands from Flexiv Elements Studio. + */ +class UserNode final +{ +public: + /** + * @brief Create the node. + * @param[in] robot_sn Serial number of the simulated robot created in Flexiv Elements Studio. + * The accepted formats are: "Rizon 4-123456" and "Rizon4-123456". + * @param[in] debug_print Print debug info. + */ + UserNode(const std::string& robot_sn, bool debug_print = false); + virtual ~UserNode(); + + /** + * @brief Whether this node is connected with Flexiv Elements Studio. + */ + bool connected() const; + + /** + * @brief Send robot states data to Flexiv Elements Studio. + * @param[in] robot_states States data of the simulated robot in the external simulator. + * @return True: data sent successfully; false: failed to send data. + */ + bool SendRobotStates(const SimRobotStates& robot_states); + + /** + * @brief Blocks until the next data packet of robot commands from Flexiv Elements Studio is + * received or timeout. + * @param[in] timeout_ms Maximum wait time before this function returns. Must set a value to + * avoid deadlock. Unit: [ms]. + * @return True: packet received before timeout; false: packet not received before timeout. + */ + bool WaitForRobotCommands(unsigned int timeout_ms); + + /** + * @brief Current commands data to apply to the simulated robot in the external simulator. + * @return Value copy of SimRobotCommands struct. + * @warning Call this function after WaitForRobotCommands() to get the latest data. + */ + SimRobotCommands robot_commands() const; + +private: + class Impl; + std::unique_ptr pimpl_; +}; + +} /* namespace sim_plugin */ +} /* namespace flexiv */ + +#endif /* FLEXIV_SIM_PLUGIN_USER_NODE_HPP_ */ diff --git a/lib/libflexiv_sim_plugin.x86_64-linux-gnu.a b/lib/libflexiv_sim_plugin.x86_64-linux-gnu.a new file mode 100644 index 0000000..5200750 Binary files /dev/null and b/lib/libflexiv_sim_plugin.x86_64-linux-gnu.a differ diff --git a/thirdparty/build_and_install_dependencies.sh b/thirdparty/build_and_install_dependencies.sh new file mode 100644 index 0000000..4a43448 --- /dev/null +++ b/thirdparty/build_and_install_dependencies.sh @@ -0,0 +1,41 @@ +#!/bin/sh +# This script builds from source and installs all dependencies of flexiv_sim_plugin. + +# Absolute path of this script +SCRIPTPATH="$(dirname $(readlink -f $0))" +set -e + +# Check script arguments +if [ "$#" -lt 1 ]; then + echo "Error: invalid script argument" + echo "Required argument: [install_directory_path]" + echo " install_directory_path: directory to install all dependencies, should be the same as the install directory of flexiv_sim_plugin" + echo "Optional argument: [num_parallel_jobs]" + echo " num_parallel_jobs: number of parallel jobs used to build, use 4 if not specified" + exit +fi + +# Get dependencies install directory from script argument, should be the same as the install directory of flexiv_sim_plugin +INSTALL_DIR=$1 +echo "Dependencies will be installed to: $INSTALL_DIR" + +# Use specified number for parallel build jobs, otherwise use number of cores +if [ -n "$2" ] ;then + NUM_JOBS=$2 +else + NUM_JOBS=4 +fi +echo "Number of parallel build jobs: $NUM_JOBS" + + +# Clone all dependencies in a subfolder +mkdir -p cloned && cd cloned + +# Build and install all dependencies to INSTALL_DIR +bash $SCRIPTPATH/scripts/install_spdlog.sh $INSTALL_DIR $NUM_JOBS +bash $SCRIPTPATH/scripts/install_tinyxml2.sh $INSTALL_DIR $NUM_JOBS +bash $SCRIPTPATH/scripts/install_foonathan_memory.sh $INSTALL_DIR $NUM_JOBS +bash $SCRIPTPATH/scripts/install_Fast-CDR.sh $INSTALL_DIR $NUM_JOBS +bash $SCRIPTPATH/scripts/install_Fast-DDS.sh $INSTALL_DIR $NUM_JOBS + +echo ">>>>>>>>>> Finished <<<<<<<<<<" diff --git a/thirdparty/scripts/install_Fast-CDR.sh b/thirdparty/scripts/install_Fast-CDR.sh new file mode 100644 index 0000000..f582c3a --- /dev/null +++ b/thirdparty/scripts/install_Fast-CDR.sh @@ -0,0 +1,34 @@ +#!/bin/bash +set -e +echo "Installing Fast-CDR" + +# Get install directory and number of parallel build jobs as script arguments +INSTALL_DIR=$1 +NUM_JOBS=$2 + +# Clone source code +if [ ! -d Fast-CDR ] ; then + git clone https://github.com/eProsima/Fast-CDR.git + cd Fast-CDR +else + cd Fast-CDR +fi + +# Use specific version +git fetch -p +git checkout v1.0.24 +git submodule update --init --recursive + +# Configure CMake +mkdir -p build && cd build +cmake .. -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_SHARED_LIBS=OFF \ + -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ + -DCMAKE_INSTALL_PREFIX=$INSTALL_DIR \ + -DCMAKE_PREFIX_PATH=$INSTALL_DIR \ + -DCOMPILE_EXAMPLES=OFF + +# Build and install +cmake --build . --target install --config Release -j $NUM_JOBS + +echo "Installed Fast-CDR" diff --git a/thirdparty/scripts/install_Fast-DDS.sh b/thirdparty/scripts/install_Fast-DDS.sh new file mode 100644 index 0000000..443978e --- /dev/null +++ b/thirdparty/scripts/install_Fast-DDS.sh @@ -0,0 +1,38 @@ +#!/bin/bash +# Depends on: foonathan_memory, tinyxml2, Fast-CDR +set -e +echo "Installing Fast-DDS" + +# Get install directory and number of parallel build jobs as script arguments +INSTALL_DIR=$1 +NUM_JOBS=$2 + +# Clone source code +if [ ! -d Fast-DDS ] ; then + git clone https://github.com/eProsima/Fast-DDS.git + cd Fast-DDS +else + cd Fast-DDS +fi + +# Use specific version +git fetch -p +git checkout v2.6.7 +git submodule update --init --recursive + +# Configure CMake +mkdir -p build && cd build +cmake .. -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_SHARED_LIBS=OFF \ + -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ + -DCMAKE_INSTALL_PREFIX=$INSTALL_DIR \ + -DCMAKE_PREFIX_PATH=$INSTALL_DIR \ + -DTHIRDPARTY_Asio=ON \ + -DCOMPILE_EXAMPLES=OFF \ + -DSQLITE3_SUPPORT=OFF \ + -DOPENSSL_USE_STATIC_LIBS=ON + +# Build and install +cmake --build . --target install --config Release -j $NUM_JOBS + +echo "Installed Fast-DDS" diff --git a/thirdparty/scripts/install_foonathan_memory.sh b/thirdparty/scripts/install_foonathan_memory.sh new file mode 100644 index 0000000..5a6c3c5 --- /dev/null +++ b/thirdparty/scripts/install_foonathan_memory.sh @@ -0,0 +1,33 @@ +#!/bin/bash +set -e +echo "Installing foonathan_memory" + +# Get install directory and number of parallel build jobs as script arguments +INSTALL_DIR=$1 +NUM_JOBS=$2 + +# Clone source code +if [ ! -d foonathan_memory_vendor ] ; then + git clone https://github.com/eProsima/foonathan_memory_vendor.git + cd foonathan_memory_vendor +else + cd foonathan_memory_vendor +fi + +# Use specific version +git fetch -p +git checkout v1.2.1 +git submodule update --init --recursive + +# Configure CMake +mkdir -p build && cd build +cmake .. -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_SHARED_LIBS=OFF \ + -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ + -DCMAKE_INSTALL_PREFIX=$INSTALL_DIR \ + -DCMAKE_PREFIX_PATH=$INSTALL_DIR + +# Build and install +cmake --build . --target install --config Release -j $NUM_JOBS + +echo "Installed foonathan_memory" diff --git a/thirdparty/scripts/install_spdlog.sh b/thirdparty/scripts/install_spdlog.sh new file mode 100644 index 0000000..3fc6b42 --- /dev/null +++ b/thirdparty/scripts/install_spdlog.sh @@ -0,0 +1,33 @@ +#!/bin/bash +set -e +echo "Installing spdlog" + +# Get install directory and number of parallel build jobs as script arguments +INSTALL_DIR=$1 +NUM_JOBS=$2 + +# Clone source code +if [ ! -d spdlog ] ; then + git clone https://github.com/gabime/spdlog.git + cd spdlog +else + cd spdlog +fi + +# Use specific version +git fetch -p +git checkout v1.14.1 +git submodule update --init --recursive + +# Configure CMake +mkdir -p build && cd build +cmake .. -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_SHARED_LIBS=OFF \ + -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ + -DCMAKE_INSTALL_PREFIX=$INSTALL_DIR \ + -DCMAKE_PREFIX_PATH=$INSTALL_DIR + +# Build and install +cmake --build . --target install --config Release -j $NUM_JOBS + +echo "Installed spdlog" diff --git a/thirdparty/scripts/install_tinyxml2.sh b/thirdparty/scripts/install_tinyxml2.sh new file mode 100644 index 0000000..7458139 --- /dev/null +++ b/thirdparty/scripts/install_tinyxml2.sh @@ -0,0 +1,33 @@ +#!/bin/bash +set -e +echo "Installing tinyxml2" + +# Get install directory and number of parallel build jobs as script arguments +INSTALL_DIR=$1 +NUM_JOBS=$2 + +# Clone source code +if [ ! -d tinyxml2 ] ; then + git clone https://github.com/leethomason/tinyxml2.git + cd tinyxml2 +else + cd tinyxml2 +fi + +# Use specific version +git fetch -p +git checkout 8.0.0 +git submodule update --init --recursive + +# Configure CMake +mkdir -p build && cd build +cmake .. -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_SHARED_LIBS=OFF \ + -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ + -DCMAKE_INSTALL_PREFIX=$INSTALL_DIR \ + -DCMAKE_PREFIX_PATH=$INSTALL_DIR + +# Build and install +cmake --build . --target install --config Release -j $NUM_JOBS + +echo "Installed tinyxml2"