├── .github └── workflows │ ├── build_master.yml │ ├── build_pr.yml │ ├── format.yml │ └── pr_todo_checks.yml ├── .gitignore ├── CHANGELOG.md ├── CMakeLists.txt ├── CONTRIBUTING.md ├── README.md ├── demos ├── CMakeLists.txt ├── demo.cpp ├── demo_multiprocess_backend.cpp ├── demo_multiprocess_frontend.cpp └── types.hpp ├── doc ├── about_architecture.rst ├── breathing_cat.toml ├── custom_driver.rst ├── custom_sensor.rst ├── desired_vs_applied_action.rst ├── images │ ├── action_observation_timing.png │ └── robot_interfaces_architecture.png ├── installation.rst ├── preempt_rt_kernel.rst ├── quick_start_example.rst ├── real_time.rst ├── robot_data.rst └── timeseries.rst ├── doc_mainpage.rst ├── include └── robot_interfaces │ ├── example.hpp │ ├── finger_types.hpp │ ├── loggable.hpp │ ├── monitored_robot_driver.hpp │ ├── n_finger_observation.hpp │ ├── n_joint_action.hpp │ ├── n_joint_observation.hpp │ ├── n_joint_robot_types.hpp │ ├── pybind_finger.hpp │ ├── pybind_helper.hpp │ ├── robot.hpp │ ├── robot_backend.hpp │ ├── robot_data.hpp │ ├── robot_driver.hpp │ ├── robot_frontend.hpp │ ├── robot_log_entry.hpp │ ├── robot_log_reader.hpp │ ├── robot_logger.hpp │ ├── sensors │ ├── pybind_sensors.hpp │ ├── sensor_backend.hpp │ ├── sensor_data.hpp │ ├── sensor_driver.hpp │ ├── sensor_frontend.hpp │ ├── sensor_log_reader.hpp │ └── sensor_logger.hpp │ ├── status.hpp │ ├── types.hpp │ └── utils.hpp ├── license.txt ├── package.xml ├── python └── robot_interfaces │ └── __init__.py ├── scripts └── plot_trifinger_log.py ├── src └── convert_old_robot_log.cpp ├── srcpy ├── py_finger_types.cpp ├── py_generic.cpp ├── py_one_joint_types.cpp ├── py_solo_eight_types.cpp ├── py_trifinger_types.cpp └── py_two_joint_types.cpp └── tests ├── CMakeLists.txt ├── dummy_sensor_driver.hpp ├── main.cpp ├── test_pybind_pickle.py ├── test_robot_backend.cpp ├── test_robot_logger.cpp ├── test_sensor_interface.cpp └── test_sensor_logger.cpp /.github/workflows/build_master.yml: -------------------------------------------------------------------------------- 1 | # Build the package and run tests, using the latest trifinger_user Apptainer 2 | # image. 3 | # The Apptainer image has a full ROBOT_FINGERS workspace installed, so all 4 | # dependencies are there and only the checked package needs to be build, not the 5 | # whole workspace. 6 | # 7 | # Note: The Apptainer image only gets automatically rebuild, if something in the 8 | # image definition changes, not when other packages are changed. So to avoid 9 | # that the workspace in the container gets outdated, manual builds need to be 10 | # triggered regularly. 11 | 12 | name: Build and Test 13 | 14 | on: 15 | push: 16 | branches: 17 | - master 18 | 19 | jobs: 20 | build_and_test: 21 | runs-on: ubuntu-latest 22 | 23 | # Grant GITHUB_TOKEN the permissions required to make a Pages deployment 24 | permissions: 25 | pages: write # to deploy to Pages 26 | id-token: write # to verify the deployment originates from an appropriate source 27 | 28 | # Deploy to the github-pages environment 29 | environment: 30 | name: github-pages 31 | url: ${{ steps.doc.outputs.page_url }} 32 | 33 | steps: 34 | - name: Check out code 35 | uses: actions/checkout@v4 36 | 37 | - name: Setup Apptainer 38 | uses: open-dynamic-robot-initiative/trifinger-build-action/setup_apptainer@v2 39 | 40 | - name: Build 41 | uses: open-dynamic-robot-initiative/trifinger-build-action/build@v2 42 | 43 | - name: Documentation 44 | id: doc 45 | uses: open-dynamic-robot-initiative/trifinger-build-action/build_and_deploy_docs@v2 46 | 47 | - name: Tests 48 | uses: open-dynamic-robot-initiative/trifinger-build-action/run_tests@v2 49 | -------------------------------------------------------------------------------- /.github/workflows/build_pr.yml: -------------------------------------------------------------------------------- 1 | # Build the package and run tests, using the latest trifinger_user Apptainer 2 | # image. 3 | # The Apptainer image has a full ROBOT_FINGERS workspace installed, so all 4 | # dependencies are there and only the checked package needs to be build, not the 5 | # whole workspace. 6 | # 7 | # Note: The Apptainer image only gets automatically rebuild, if something in the 8 | # image definition changes, not when other packages are changed. So to avoid 9 | # that the workspace in the container gets outdated, manual builds need to be 10 | # triggered regularly. 11 | 12 | name: Build and Test 13 | 14 | on: pull_request 15 | 16 | jobs: 17 | build_and_test: 18 | runs-on: ubuntu-latest 19 | steps: 20 | - name: Check out code 21 | uses: actions/checkout@v4 22 | 23 | - name: Setup Apptainer 24 | uses: open-dynamic-robot-initiative/trifinger-build-action/setup_apptainer@v2 25 | 26 | - name: Build 27 | uses: open-dynamic-robot-initiative/trifinger-build-action/build@v2 28 | 29 | - name: Tests 30 | uses: open-dynamic-robot-initiative/trifinger-build-action/run_tests@v2 31 | -------------------------------------------------------------------------------- /.github/workflows/format.yml: -------------------------------------------------------------------------------- 1 | name: Formatting 2 | 3 | on: pull_request 4 | 5 | jobs: 6 | clang-format: 7 | name: C++ Formatting 8 | runs-on: ubuntu-latest 9 | steps: 10 | - uses: actions/checkout@v4 11 | - uses: actions/setup-python@v5 12 | - run: wget https://raw.githubusercontent.com/machines-in-motion/mpi_cmake_modules/master/scripts/run-clang-format 13 | - run: wget https://raw.githubusercontent.com/machines-in-motion/mpi_cmake_modules/master/resources/_clang-format 14 | - run: python ./run-clang-format -r . 15 | -------------------------------------------------------------------------------- /.github/workflows/pr_todo_checks.yml: -------------------------------------------------------------------------------- 1 | name: PR TODO checks 2 | 3 | on: pull_request 4 | 5 | jobs: 6 | fixmes: 7 | name: New FIXMEs 8 | runs-on: ubuntu-latest 9 | steps: 10 | - uses: actions/checkout@v4 11 | - name: Check for FIXMEs 12 | uses: luator/github_action_check_new_todos@v2 13 | with: 14 | label: FIXME 15 | base_ref: origin/${{ github.base_ref }} 16 | 17 | todos: 18 | name: New TODOs 19 | runs-on: ubuntu-latest 20 | steps: 21 | - uses: actions/checkout@v4 22 | - name: Check for TODOs 23 | uses: luator/github_action_check_new_todos@v2 24 | with: 25 | label: TODO 26 | base_ref: origin/${{ github.base_ref }} 27 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.user 2 | *~ 3 | -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # Changelog 2 | 3 | ## [Unreleased] 4 | ### Added 5 | - Add `Ptr` and `ConstPtr` typedefs in classes for more conveniently defining 6 | shared pointers. 7 | - Function `create_interface_python_bindings()` to create Python bindings for 8 | the interface classes (but not the robot-specific action and observation 9 | classes). 10 | - Method `RobotDriver::get_idle_action()` that is expected to return an action 11 | that is safe to apply while the robot is idle. 12 | - Option to provide static information about a sensor (e.g. camera calibration 13 | coefficients, frame rate, etc.). For this, a method `get_sensor_info()` is added to 14 | `SensorFrontend`. It defaults to an empty struct for backward compatibility. To use 15 | it, implement the method with the same name in `SensorDriver`. 16 | - Add `is_running()` to `RobotLogger` and `SensorLogger`. Can, for example, be used to 17 | detect if the buffer is full (in which case the logger stops automatically). 18 | 19 | ### Changed 20 | - The return type of `RobotDriver::get_error()` is changed to 21 | `std::optional`. This way instantiation of an actual string 22 | (which can involve dynamic memory allocation) is only needed if there actually 23 | is an error. 24 | - The `create_python_bindings()` function (which includes the N-joint 25 | action/observation) is moved to `pybind_finger.hpp` and renamed to 26 | `create_blmc_can_robot_python_bindings()`. 27 | - The backend now applies an "idle action" between initialisation and reception 28 | of the first action by the user. The idle action is provided by the robot 29 | driver. The default implementation simply creates an action using the default 30 | constructor but for some robots this might not be appropriate; override the 31 | method with a custom implementation in this case. 32 | This change is to prevent hardware timeout issues but can also be useful to 33 | have nicer behaviour of the robot after initialisation (e.g. by holding the 34 | joints in place). 35 | - If an error message given to `Status::set_error()` is cut due to being too 36 | long, this is now indicated by setting '~' as last character. 37 | 38 | ### Fixed 39 | - pybind11 build error on Ubuntu 22.04 40 | 41 | ### Removed 42 | - Removed deprecated methods of RobotLogger: 43 | - `start_continous_writing` and `stop_continous_writing` are removed. 44 | - `write_current_buffer` and `write_current_buffer_binary` are replaced with 45 | `save_current_robot_data[_binary]`. 46 | 47 | 48 | ## [1.2.0] - 2022-06-28 49 | ### Added 50 | - Measurement (and optional logging) of the RobotBackend frequency. 51 | - Documentation on setup of the real-time Linux kernel. 52 | 53 | 54 | ## [1.1.0] - 2021-06-28 55 | 56 | There is no changelog for this or earlier versions. 57 | 58 | 59 | [Unreleased]: https://github.com/open-dynamic-robot-initiative/robot_interfaces/compare/v1.2.0...HEAD 60 | [1.2.0]: https://github.com/open-dynamic-robot-initiative/robot_interfaces/compare/v1.1.0...v1.2.0 61 | [1.1.0]: https://github.com/open-dynamic-robot-initiative/robot_interfaces/releases/tag/v1.1.0 62 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019, New York University and Max Planck Gesellschaft. 3 | # 4 | # License BSD-3 clause 5 | # 6 | 7 | # 8 | # Set up the project. 9 | # 10 | cmake_minimum_required(VERSION 3.10.2) 11 | 12 | project(robot_interfaces) 13 | 14 | # Using C++17. 15 | if(NOT CMAKE_CXX_STANDARD) 16 | set(CMAKE_CXX_STANDARD 17) 17 | endif() 18 | 19 | # 20 | # Dependencies. 21 | # 22 | # pybind11 needs to be first, otherwise other packages which also search for 23 | # Python can cause an 'Unknown CMake command "python3_add_library"' error. 24 | # Probably related to how Python is found, see 25 | # https://github.com/pybind/pybind11/issues/3996 26 | find_package(pybind11 REQUIRED) 27 | 28 | find_package(ament_cmake REQUIRED) 29 | find_package(ament_cmake_python REQUIRED) 30 | find_package(mpi_cmake_modules REQUIRED) 31 | find_package(Eigen3 REQUIRED) 32 | find_package(real_time_tools REQUIRED) 33 | find_package(time_series REQUIRED) 34 | find_package(signal_handler REQUIRED) 35 | find_package(serialization_utils REQUIRED) 36 | find_package(Threads) 37 | find_package(rt) 38 | 39 | # export the dependencies 40 | ament_export_dependencies(pybind11 Eigen3 Boost real_time_tools time_series 41 | signal_handler serialization_utils) 42 | 43 | ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR python/${PROJECT_NAME}) 44 | 45 | # prepare to export all needed targets 46 | set(all_targets) 47 | set(all_target_exports) 48 | 49 | # 50 | # Add main target. 51 | # 52 | add_library(${PROJECT_NAME} INTERFACE) 53 | # Add the include dependencies 54 | target_include_directories( 55 | ${PROJECT_NAME} 56 | INTERFACE $ 57 | $) 58 | # Link the dependencies 59 | target_link_libraries(${PROJECT_NAME} INTERFACE rt::rt) 60 | target_link_libraries(${PROJECT_NAME} INTERFACE Eigen3::Eigen) 61 | target_link_libraries(${PROJECT_NAME} INTERFACE real_time_tools::real_time_tools) 62 | target_link_libraries(${PROJECT_NAME} INTERFACE time_series::time_series) 63 | target_link_libraries(${PROJECT_NAME} INTERFACE signal_handler::signal_handler) 64 | target_link_libraries(${PROJECT_NAME} INTERFACE serialization_utils::serialization_utils) 65 | target_link_libraries(${PROJECT_NAME} INTERFACE serialization_utils::gzip_iostream) 66 | target_link_libraries(${PROJECT_NAME} INTERFACE Threads::Threads) 67 | target_link_libraries(${PROJECT_NAME} INTERFACE pybind11::embed) 68 | target_link_libraries(${PROJECT_NAME} INTERFACE Boost::iostreams) 69 | # Export the target. 70 | ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) 71 | list(APPEND all_targets ${PROJECT_NAME}) 72 | list(APPEND all_target_exports export_${PROJECT_NAME}) 73 | 74 | 75 | add_executable(convert_old_robot_log src/convert_old_robot_log.cpp) 76 | target_link_libraries(convert_old_robot_log ${PROJECT_NAME}) 77 | list(APPEND all_targets convert_old_robot_log) 78 | 79 | # 80 | # manage the demos. 81 | # 82 | add_executable(demo demos/demo.cpp) 83 | target_link_libraries(demo ${PROJECT_NAME}) 84 | list(APPEND all_targets demo) 85 | 86 | add_executable(demo_multiprocess_backend demos/demo_multiprocess_backend.cpp) 87 | target_link_libraries(demo_multiprocess_backend ${PROJECT_NAME}) 88 | list(APPEND all_targets demo_multiprocess_backend) 89 | 90 | add_executable(demo_multiprocess_frontend demos/demo_multiprocess_frontend.cpp) 91 | target_link_libraries(demo_multiprocess_frontend ${PROJECT_NAME}) 92 | list(APPEND all_targets demo_multiprocess_frontend) 93 | 94 | # 95 | # manage the unit tests. 96 | # 97 | if(BUILD_TESTING) 98 | add_subdirectory(tests) 99 | endif() 100 | 101 | # 102 | # python bindings. 103 | # 104 | add_pybind11_module(py_generic srcpy/py_generic.cpp 105 | LINK_LIBRARIES ${PROJECT_NAME}) 106 | add_pybind11_module(py_finger_types srcpy/py_finger_types.cpp 107 | LINK_LIBRARIES ${PROJECT_NAME}) 108 | add_pybind11_module(py_trifinger_types srcpy/py_trifinger_types.cpp 109 | LINK_LIBRARIES ${PROJECT_NAME}) 110 | add_pybind11_module(py_one_joint_types srcpy/py_one_joint_types.cpp 111 | LINK_LIBRARIES ${PROJECT_NAME}) 112 | add_pybind11_module(py_two_joint_types srcpy/py_two_joint_types.cpp 113 | LINK_LIBRARIES ${PROJECT_NAME}) 114 | add_pybind11_module(py_solo_eight_types srcpy/py_solo_eight_types.cpp 115 | LINK_LIBRARIES ${PROJECT_NAME}) 116 | 117 | # 118 | # building documentation 119 | # 120 | add_documentation() 121 | 122 | # 123 | # Install the package 124 | # 125 | install(DIRECTORY include/ DESTINATION include) 126 | 127 | install( 128 | TARGETS ${all_targets} 129 | EXPORT ${all_target_exports} 130 | LIBRARY DESTINATION lib 131 | ARCHIVE DESTINATION lib 132 | RUNTIME DESTINATION lib/${PROJECT_NAME} 133 | INCLUDES 134 | DESTINATION include) 135 | 136 | install_scripts( 137 | scripts/plot_trifinger_log.py 138 | 139 | DESTINATION lib/${PROJECT_NAME} 140 | ) 141 | 142 | 143 | # 144 | # Export the package as ament 145 | # 146 | ament_package() 147 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | How to Contribute 2 | ================= 3 | 4 | Bug reports are always welcome. Please open an issue on GitHub. 5 | 6 | If you want to contribute code to this package, please see the 7 | [TriFinger Contribution Guide](https://open-dynamic-robot-initiative.github.io/trifinger_docs/contribute.html). 8 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robot Interfaces 2 | 3 | Generic interface for a controlling a real-time-critical robot from a 4 | non-real-time process. 5 | 6 | For more information see the 7 | [documentation](https://open-dynamic-robot-initiative.github.io/robot_interfaces/). 8 | 9 | 10 | ## Authors 11 | 12 | - Manuel Wuethrich 13 | - Felix Widmaier 14 | 15 | 16 | ## License 17 | 18 | BSD 3-Clause License 19 | 20 | Copyright(c) 2018 Max Planck Gesellschaft 21 | -------------------------------------------------------------------------------- /demos/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ##################### 2 | # create some demos # 3 | ##################### 4 | 5 | include_directories( 6 | ${PROJECT_SOURCE_DIR}/demos 7 | ) 8 | 9 | -------------------------------------------------------------------------------- /demos/demo.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file demo.cpp 3 | * @author Vincent Berenz 4 | * license License BSD-3-Clause 5 | * @copyright Copyright (c) 2019, Max Planck Gesellschaft. 6 | * 7 | * @brief Minimal demo of robot driver, backend and frontend 8 | */ 9 | 10 | #include "robot_interfaces/example.hpp" 11 | #include "robot_interfaces/monitored_robot_driver.hpp" 12 | #include "robot_interfaces/robot.hpp" 13 | #include "robot_interfaces/robot_backend.hpp" 14 | #include "robot_interfaces/robot_driver.hpp" 15 | #include "robot_interfaces/robot_frontend.hpp" 16 | #include "robot_interfaces/status.hpp" 17 | 18 | #include 19 | 20 | /** 21 | * \example demo.cpp 22 | * This demo shows robot_interfaces of a 23 | * dummy "2dof" robot, in which a dof "position" 24 | * is represented by an integer 25 | */ 26 | 27 | using namespace robot_interfaces::example; 28 | 29 | int main() 30 | { 31 | typedef robot_interfaces::RobotBackend Backend; 32 | typedef robot_interfaces::SingleProcessRobotData Data; 33 | typedef robot_interfaces::RobotFrontend Frontend; 34 | 35 | // max time allowed for the robot to apply an action. 36 | double max_action_duration_s = 0.02; 37 | 38 | // max time between for 2 successive actions 39 | double max_inter_action_duration_s = 0.05; 40 | 41 | // demo showing the separated usage of backend and frontend 42 | { 43 | std::cout << "\n -- * -- Frontend and Backend -- * --\n" << std::endl; 44 | 45 | std::shared_ptr driver_ptr = std::make_shared(0, 1000); 46 | // Wrap the driver in a MonitoredRobotDriver to automatically run a 47 | // timing watchdog. If timing is violated, the robot will immediately 48 | // be shut down. 49 | // If no time monitoring is needed in your application, you can simply 50 | // use the `driver_ptr` directly, without the wrapper. 51 | auto monitored_driver_ptr = 52 | std::make_shared>( 53 | driver_ptr, max_action_duration_s, max_inter_action_duration_s); 54 | 55 | std::shared_ptr data_ptr = std::make_shared(); 56 | 57 | Backend backend(monitored_driver_ptr, data_ptr); 58 | backend.initialize(); 59 | 60 | Frontend frontend(data_ptr); 61 | 62 | Action action; 63 | Observation observation; 64 | 65 | // simulated action : 66 | // 1 dof going from 200 to 300 67 | // The other going from 300 to 200 68 | 69 | for (uint value = 200; value <= 300; value++) 70 | { 71 | action.values[0] = value; 72 | action.values[1] = 500 - value; 73 | // this action will be stored at index 74 | robot_interfaces::TimeIndex index = 75 | frontend.append_desired_action(action); 76 | // getting the observation corresponding to the applied 77 | // action, i.e. at the same index 78 | observation = frontend.get_observation(index); 79 | std::cout << "value: " << value << " | "; 80 | action.print(false); 81 | observation.print(true); 82 | } 83 | } 84 | 85 | // demo representing usage of frontend and backend 86 | // encapsulated in the same instance 87 | { 88 | std::cout << "\n -- * -- Robot -- * --\n" << std::endl; 89 | 90 | typedef robot_interfaces::Robot Robot; 91 | 92 | int min = 0; 93 | int max = 100; 94 | Robot robot( 95 | max_action_duration_s, max_inter_action_duration_s, min, max); 96 | 97 | robot.initialize(); 98 | 99 | Action action; 100 | Observation observation; 101 | 102 | // simulated action : 103 | // 1 dof going from 200 to 300 104 | // The other going from 300 to 200 105 | 106 | for (uint value = 200; value <= 300; value++) 107 | { 108 | action.values[0] = value; 109 | action.values[1] = 500 - value; 110 | // this action will be stored at index 111 | robot_interfaces::TimeIndex index = 112 | robot.append_desired_action(action); 113 | // getting the observation corresponding to the applied 114 | // action, i.e. at the same index 115 | observation = robot.get_observation(index); 116 | std::cout << "value: " << value << " | "; 117 | action.print(false); 118 | observation.print(true); 119 | } 120 | } 121 | } 122 | -------------------------------------------------------------------------------- /demos/demo_multiprocess_backend.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @author Vincent Berenz, Felix Widmaier 4 | * license License BSD-3-Clause 5 | * @copyright Copyright (c) 2019-2020, Max Planck Gesellschaft. 6 | * 7 | * @brief Minimal demo of robot driver/backend running in its own process 8 | */ 9 | 10 | #include 11 | 12 | #include "robot_interfaces/robot_backend.hpp" 13 | #include "robot_interfaces/robot_driver.hpp" 14 | 15 | #include "types.hpp" 16 | 17 | using namespace robot_interfaces::demo; 18 | 19 | /** 20 | * \example demo_multiprocess_backend.cpp 21 | * Robot backend for a dummy "2dof" robot in a multi process setup. 22 | */ 23 | 24 | // TODO put driver in separate file so no duplication to demo.cpp? Discuss 25 | // with Vincent. 26 | 27 | // Send command to the robot and read observation from the robot 28 | // The dof positions simply becomes the ones set by the latest action, 29 | // capped between a min and a max value (0 and 1000) 30 | class Driver : public robot_interfaces::RobotDriver 31 | { 32 | public: 33 | Driver() 34 | { 35 | } 36 | 37 | // at init dof are at min value 38 | void initialize() 39 | { 40 | state_[0] = Driver::MIN; 41 | state_[1] = Driver::MIN; 42 | } 43 | 44 | // just clip desired values 45 | // between 0 and 1000 46 | Action apply_action(const Action &action_to_apply) 47 | { 48 | std::cout << "received action "; 49 | action_to_apply.print(true); 50 | 51 | Action applied; 52 | for (unsigned int i = 0; i < 2; i++) 53 | { 54 | if (action_to_apply.values[i] > Driver::MAX) 55 | { 56 | applied.values[i] = Driver::MAX; 57 | } 58 | else if (action_to_apply.values[i] < Driver::MIN) 59 | { 60 | applied.values[i] = Driver::MIN; 61 | } 62 | else 63 | { 64 | applied.values[i] = action_to_apply.values[i]; 65 | } 66 | // simulating the time if could take for a real 67 | // robot to perform the action 68 | usleep(1000); 69 | state_[i] = applied.values[i]; 70 | } 71 | return applied; 72 | } 73 | 74 | Observation get_latest_observation() 75 | { 76 | Observation observation; 77 | observation.values[0] = state_[0]; 78 | observation.values[1] = state_[1]; 79 | return observation; 80 | } 81 | 82 | std::optional get_error() 83 | { 84 | return std::nullopt; // no error 85 | } 86 | 87 | void shutdown() 88 | { 89 | // nothing to do 90 | } 91 | 92 | private: 93 | int state_[2]; 94 | 95 | const static int MAX = 1000; 96 | const static int MIN = 0; 97 | }; 98 | 99 | int main() 100 | { 101 | typedef robot_interfaces::RobotBackend Backend; 102 | typedef robot_interfaces::MultiProcessRobotData 103 | MultiProcessData; 104 | 105 | auto driver_ptr = std::make_shared(); 106 | // the backend process acts as master for the shared memory 107 | auto data_ptr = 108 | std::make_shared("multiprocess_demo", true); 109 | 110 | Backend backend(driver_ptr, data_ptr); 111 | backend.initialize(); 112 | 113 | // TODO would be nicer to check if backend loop is still running 114 | while (true) 115 | { 116 | } 117 | } 118 | -------------------------------------------------------------------------------- /demos/demo_multiprocess_frontend.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @author Vincent Berenz, Felix Widmaier 4 | * license License BSD-3-Clause 5 | * @copyright Copyright (c) 2019-2020, Max Planck Gesellschaft. 6 | * 7 | * @brief Minimal demo of robot frontend running in its own process 8 | */ 9 | 10 | #include 11 | 12 | #include "robot_interfaces/robot_frontend.hpp" 13 | 14 | #include "types.hpp" 15 | 16 | using namespace robot_interfaces::demo; 17 | 18 | /** 19 | * \example demo_multiprocess_frontend.cpp 20 | * Robot frontend for a dummy "2dof" robot in a multi process setup. 21 | */ 22 | 23 | int main() 24 | { 25 | typedef robot_interfaces::RobotFrontend Frontend; 26 | typedef robot_interfaces::MultiProcessRobotData 27 | MultiProcessData; 28 | 29 | // The shared memory is managed by the backend process, so set the 30 | // is_master argument to false. 31 | auto data_ptr = 32 | std::make_shared("multiprocess_demo", false); 33 | Frontend frontend(data_ptr); 34 | 35 | Action action; 36 | Observation observation; 37 | 38 | // simulated action : 39 | // 1 dof going from 200 to 300 40 | // The other going from 300 to 200 41 | 42 | for (uint value = 200; value <= 300; value++) 43 | { 44 | action.values[0] = value; 45 | action.values[1] = 500 - value; 46 | // this action will be stored at index 47 | robot_interfaces::TimeIndex index = 48 | frontend.append_desired_action(action); 49 | // getting the observation corresponding to the applied 50 | // action, i.e. at the same index 51 | observation = frontend.get_observation(index); 52 | std::cout << "value: " << value << " | "; 53 | action.print(false); 54 | observation.print(true); 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /demos/types.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * license License BSD-3-Clause 4 | * @copyright Copyright (c) 2019-2020, Max Planck Gesellschaft. 5 | * 6 | * @brief Simple Action and Observation types that are used by some demos. 7 | */ 8 | 9 | #pragma once 10 | 11 | namespace robot_interfaces 12 | { 13 | namespace demo 14 | { 15 | /** 16 | * Actions to be performed by robot, will be received by Driver. An action 17 | * simply encapsulate two desired position value, one for each dof 18 | */ 19 | class Action 20 | { 21 | public: 22 | int values[2]; 23 | 24 | void print(bool backline) const 25 | { 26 | std::cout << "action: " << values[0] << " " << values[1] << " "; 27 | if (backline) std::cout << "\n"; 28 | } 29 | 30 | template 31 | void serialize(Archive& ar) 32 | { 33 | ar(values); 34 | } 35 | }; 36 | 37 | /** 38 | * Read from the robot by Driver. An observation is the current position for 39 | * each dof. 40 | */ 41 | class Observation 42 | { 43 | public: 44 | int values[2]; 45 | 46 | void print(bool backline) const 47 | { 48 | std::cout << "observation: " << values[0] << " " << values[1] << " "; 49 | if (backline) std::cout << "\n"; 50 | } 51 | 52 | template 53 | void serialize(Archive& ar) 54 | { 55 | ar(values); 56 | } 57 | }; 58 | 59 | } // namespace demo 60 | } // namespace robot_interfaces 61 | -------------------------------------------------------------------------------- /doc/about_architecture.rst: -------------------------------------------------------------------------------- 1 | .. _architecture: 2 | 3 | About the Software Architecture 4 | =============================== 5 | 6 | Nomenclature 7 | ------------ 8 | 9 | In our software we are using the terms *observation* for sensor data 10 | provided by the robot (e.g. joint positions, velocities, etc.) 11 | and *action* for commands sent to the robot (target torque or position). 12 | 13 | Architecture 14 | ------------ 15 | 16 | .. image:: images/robot_interfaces_architecture.png 17 | 18 | The software is designed in a modular way. There is a central 19 | module, *RobotData*, which manages all the data (sensor readings, 20 | commands sent by the user, etc.). All other modules connect to the *robot 21 | data* and use it as a sole channel of communication, i.e. there is no 22 | direct connection between the modules. See the figure above for an 23 | illustration. 24 | 25 | Existing modules: 26 | 27 | - Back end 28 | - Front end 29 | - Logger 30 | 31 | All modules operate independent of each other and communicate 32 | only through the *RobotData*. This makes it easy to add new modules as 33 | needed, e.g. one could easily implement a ROS publisher module such that 34 | the robot could be visualized in ROS tools like RViz. Further, when 35 | using the multi-process robot data, modules like the logger or a 36 | potential ROS publisher can be started while the robot is already 37 | running. 38 | 39 | Everything is implemented in C++ but Python bindings are provided so 40 | user code can be written in either of the two languages. This allows the 41 | user to interface the robot from Python and, for example, easily 42 | integrate it with Python-based learning libraries (e.g. Gym), while 43 | still ensuring efficient execution of the timing-critical parts. 44 | 45 | 46 | Back End 47 | ~~~~~~~~ 48 | 49 | The back end (:cpp:class:`~robot_interfaces::RobotBackend`) is responsible for 50 | talking with the actual hardware. It runs a loop that performs the following 51 | steps: 52 | 53 | - get sensor data (called *observations*) from the robot and push them to the 54 | robot data. 55 | - get commands (*actions*) from the robot data and send them to the robot. 56 | 57 | The back end itself is implemented in a robot-agnostic way, it just gets 58 | observations and actions and passes them on. The actual types of the 59 | latter are specified as C++ template arguments. For talking with the 60 | hardware, the back end holds an instance of a 61 | robot-specific *driver* class internally, which takes care of the actual 62 | communication with the robot (i.e. retrieving observations and sending 63 | actions). 64 | 65 | 66 | Driver 67 | ^^^^^^ 68 | 69 | The *driver* class has to be based on 70 | :cpp:class:`~robot_interfaces::RobotDriver` and implements a few methods that 71 | are needed to talk to the actual hardware. Most important are the ones for 72 | getting observations and sending actions. Robot-specific initialization (e.g. 73 | homing of the joints) is also performed here. 74 | 75 | To ensure safe operation of the robot, the driver may modify 76 | the desired action sent by the user to what will be the 77 | actually *applied* action. For example in the case of our TriFinger robot 78 | (implemented in the :doc:`robot_fingers package `), this 79 | includes clamping to an allowed maximum torque ``max_torque`` and velocity 80 | damping using a D-gain as follows: 81 | 82 | :: 83 | 84 | applied_torque = desired_torque - K_d * velocity 85 | 86 | We set ``max_torque`` and ``K_d`` such that the robot should not be able to 87 | damage itself, even when sending random commands. 88 | 89 | .. todo:: exact implementation of safety checks for TriFinger should be moved to 90 | the docs of robot_fingers. 91 | 92 | The resulting *applied action* is reported back to the user, so the user 93 | can know what action really got applied to the robot. 94 | 95 | See also :doc:`desired_vs_applied_action`. 96 | 97 | 98 | Front End 99 | ~~~~~~~~~ 100 | 101 | The *front end* (:cpp:class:`~robot_interfaces::RobotFrontend`) provides the 102 | user with an interface to access the *RobotData*. More specifically, it 103 | implements methods :cpp:func:`~robot_interfaces::RobotFrontend::get_observation` 104 | to access the latest observations and 105 | :cpp:func:`~robot_interfaces::RobotFrontend::append_desired_action` to send 106 | actions to the robot. 107 | 108 | The front end class itself robot-agnostic. Only the types of observations and 109 | actions are robot-specific and provided through template arguments. 110 | 111 | It would, in principle, be possible to directly access the data from 112 | *RobotData*. However, the front end, provides a more user-friendly interfaces 113 | that ensures that the data in the correct way. It does, for example, only 114 | provide reading access to observations and does not allow to change them. 115 | 116 | Note that the user can only *append* actions to the time series, not apply 117 | them directly. An appended action will be applied by the back end once 118 | the time step to which it corresponds is reached. This gives the user 119 | the option to directly append actions for the next few steps at once. 120 | 121 | 122 | Logger 123 | ~~~~~~ 124 | 125 | When running experiments on the robot, it is often desired to store all 126 | data so they can later be analysed offline. For this, we provide a 127 | logger module that accesses the *RobotData* independently of the other 128 | modules and simply writes everything into a text file. This file can 129 | later easily be loaded, e.g. in a Python script that plots the data. 130 | 131 | It is implemented in :cpp:class:`robot_interfaces::RobotLogger`. 132 | 133 | 134 | Time Series 135 | ----------- 136 | 137 | The *RobotData* module stores all information in time series. A time 138 | series is basically a buffer in which each entry is associated to a 139 | specific time step. When at time step t a new element is added, it is 140 | appended to the buffer and assigned to step ``t + 1``. The time series 141 | provides access to the data through the time index t. However, to keep 142 | memory usage constant, only the last n elements are kept in memory 143 | where n is a configurable parameter which is set to 1000 in our current 144 | implementation. 145 | 146 | When accessing an element ``t'`` that is in the future (i.e. ``t' > t_now``), 147 | the corresponding method will block and wait until that time step is 148 | reached. This makes it easy for the user to write code that runs at the 149 | control rate of the robot without having to add explicit sleeps. 150 | 151 | Example Code: 152 | 153 | .. code-block:: python 154 | 155 | while (True): 156 | t = robot.append_desired_action(action) 157 | 158 | # This call will sleep until time step t is reached and the observation 159 | # is available. 160 | observation = robot.get_observation(t) 161 | 162 | # In case it is relevant, the user can also see the actually applied 163 | # action to see how how the desired action got modified by the safety 164 | # checks 165 | applied_action = robot.get_applied_action(t) 166 | 167 | action = compute_new_action(observation) 168 | 169 | See :doc:`timeseries` on how the action and observation time series are 170 | synchronised. 171 | 172 | 173 | Multi-process Architecture 174 | -------------------------- 175 | 176 | It is possible to run the different modules (front end, back end, ...) in 177 | separate processes by using the "multi-process robot data". For more 178 | information see :doc:`robot_data`. 179 | 180 | 181 | Example Implementations 182 | ----------------------- 183 | 184 | The following packages implement observation, action and driver classes for some 185 | actual robots and may serve as examples: 186 | 187 | - TriFinger robots: :doc:`robot_fingers ` 188 | - Solo: :doc:`robot_interfaces_solo ` 189 | - Bolt: :doc:`robot_interfaces_bolt ` 190 | -------------------------------------------------------------------------------- /doc/breathing_cat.toml: -------------------------------------------------------------------------------- 1 | [intersphinx.mapping] 2 | trifinger_docs = "https://open-dynamic-robot-initiative.github.io/trifinger_docs" 3 | robot_fingers = "https://open-dynamic-robot-initiative.github.io/robot_fingers" 4 | robot_interfaces_solo = "https://open-dynamic-robot-initiative.github.io/robot_interfaces_solo" 5 | robot_interfaces_bolt = "https://open-dynamic-robot-initiative.github.io/robot_interfaces_bolt" 6 | 7 | [mainpage] 8 | title = "Robot Interfaces Documentation" 9 | auto_general_docs = false 10 | -------------------------------------------------------------------------------- /doc/custom_driver.rst: -------------------------------------------------------------------------------- 1 | How to Implement a Custom RobotDriver 2 | ===================================== 3 | 4 | To use robot_interfaces with your own robot, you need to provide implementations 5 | for 6 | 7 | - the Action type 8 | - the Observation type 9 | - the RobotDriver 10 | 11 | 12 | Action and Observation 13 | ---------------------- 14 | 15 | Action and observation can be any arbitrary type, whatever is needed for your 16 | robot. There are only the following restrictions: 17 | 18 | - If you want to use the :cpp:class:`~robot_interfaces::MultiProcessRobotData`, 19 | the action and observation types need to be serializable with cereal_. 20 | - If you want to use the :cpp:class:`~robot_interfaces::RobotLogger`, the action 21 | and observation types need to inherit from 22 | :cpp:class:`~robot_interfaces::Loggable`. 23 | 24 | 25 | RobotDriver 26 | ----------- 27 | 28 | Your robot driver needs to be a class that inherits from 29 | :cpp:class:`~robot_interfaces::RobotDriver`, using your ``Action`` and 30 | ``Observation`` types (see above). 31 | 32 | 33 | Example 34 | ------- 35 | 36 | See the example implementation of a dummy robot driver in `example.hpp 37 | `_. 38 | This dummy driver is also used in the 39 | `demos `_. 40 | 41 | 42 | .. _cereal: https://uscilab.github.io/cereal/ 43 | -------------------------------------------------------------------------------- /doc/custom_sensor.rst: -------------------------------------------------------------------------------- 1 | **************** 2 | Sensor Interface 3 | **************** 4 | 5 | ``robot_interfaces`` also provides base classes for a pure sensor interface. In 6 | contrast to a robot, a sensor only provides observations but does not take actions. 7 | Apart from that, the overall structure is mostly the same as for a robot. That is, 8 | there are :cpp:class:`~robot_interfaces::SensorBackend` and 9 | :cpp:class:`~robot_interfaces::SensorFrontend` which communicate via 10 | :cpp:class:`~robot_interfaces::SensorData` (either single- or multi-process). 11 | 12 | For implementing an interface for your sensor, you only need to implement an observation 13 | type and a driver class based on :cpp:class:`~robot_interfaces::SensorDriver`. Then 14 | simply create data/backend/frontend using those custom types as template arguments. 15 | 16 | Optionally, your driver class can also provide a "sensor info" object by implementing 17 | :cpp:func:`~robot_interfaces::SensorDriver::get_sensor_info`. This object is then 18 | accessible by the user via 19 | :cpp:func:`~robot_interfaces::SensorFrontend::get_sensor_info`. You may use this, to 20 | provide static information about the sensor, that does not change over time (e.g. frame 21 | rate of a camera). 22 | If you don't implement the corresponding method in the driver, the front end will return 23 | an empty struct as placeholder. 24 | -------------------------------------------------------------------------------- /doc/desired_vs_applied_action.rst: -------------------------------------------------------------------------------- 1 | Desired vs Applied Action 2 | ========================= 3 | 4 | The action given by the user is called the *desired* action. Depending on the 5 | implementation of the :cpp:class:`~robot_interfaces::RobotDriver`, this action 6 | may be altered before it is actually applied on the robot, e.g. by some safety 7 | checks limiting torque and maximum velocity. This altered action is called the 8 | *applied* action. You can use 9 | :cpp:func:`robot_interfaces::RobotFrontend::get_applied_action` to see what 10 | action actually got applied on the robot. 11 | -------------------------------------------------------------------------------- /doc/images/action_observation_timing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-dynamic-robot-initiative/robot_interfaces/0efc531577be9f15dc80a9530606cae4a5766bd2/doc/images/action_observation_timing.png -------------------------------------------------------------------------------- /doc/images/robot_interfaces_architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-dynamic-robot-initiative/robot_interfaces/0efc531577be9f15dc80a9530606cae4a5766bd2/doc/images/robot_interfaces_architecture.png -------------------------------------------------------------------------------- /doc/installation.rst: -------------------------------------------------------------------------------- 1 | Build Instructions 2 | ================== 3 | 4 | .. important:: 5 | 6 | If you intend to use this interface to control your own robot, this package 7 | (and its dependencies) is enough, and you can follow the instructions below. 8 | 9 | However, if you are looking for the interface of the TriFinger robot, see 10 | the installation instructions in the :ref:`TriFinger documentation 11 | ` instead (this includes 12 | ``robot_interfaces`` and the TriFinger-specific packages). 13 | 14 | 15 | Dependencies 16 | ------------ 17 | 18 | We are using colcon_ as build tool and typically use `ros2 run` to execute our 19 | applications. While we are not really depending on any ROS_ packages, this 20 | means a basic ROS 2 installation is recommended. 21 | 22 | We are testing on Ubuntu 20.04 with ROS Foxy. Other versions may work as well 23 | but are not officially supported. 24 | 25 | .. note:: 26 | 27 | We provide a Singularity image with all dependencies for the TriFinger robot 28 | which also covers everything needed for ``robot_interfaces``. See the 29 | documentation of the :ref:`TriFinger documentation 30 | ` for more information. 31 | 32 | 33 | Get the Source 34 | -------------- 35 | 36 | ``robot_interfaces`` depends on several other of our packages which are 37 | organized in separate repositories. We therefore use the management tool treep_ 38 | which allows easy cloning of multi-repository projects. 39 | 40 | treep can be installed via pip:: 41 | 42 | pip install treep 43 | 44 | Clone the treep configuration containing the "ROBOT_INTERFACES" project:: 45 | 46 | git clone git@github.com:machines-in-motion/treep_machines_in_motion.git 47 | 48 | .. note:: 49 | 50 | treep searches for a configuration directory from the current working 51 | directory upwards. So you can use treep in the directory in which you 52 | invoked the ``git clone`` command above or any subdirectory. 53 | 54 | Now clone the project:: 55 | 56 | treep --clone ROBOT_INTERFACES 57 | 58 | .. important:: 59 | 60 | treep uses SSH to clone from github. So for the above command to work, you 61 | need a github account with a registered SSH key. Further this key needs to 62 | work without asking for a password everytime. To achieve this, run 63 | 64 | :: 65 | 66 | ssh-add 67 | 68 | first. 69 | 70 | You should now have the following directory structure:: 71 | 72 | ├── treep_machines_in_motion 73 | └── workspace 74 | └── src 75 | ├── googletest 76 | ├── mpi_cmake_modules 77 | ├── package_template 78 | ├── pybind11 79 | ├── real_time_tools 80 | ├── robot_interfaces 81 | ├── serialization_utils 82 | ├── shared_memory 83 | ├── signal_handler 84 | ├── time_series 85 | └── yaml_utils 86 | 87 | 88 | Build 89 | ----- 90 | 91 | To build, cd into the ``workspace`` directory and call 92 | 93 | :: 94 | 95 | colcon build 96 | 97 | to build the whole workspace. 98 | 99 | 100 | .. _colcon: https://colcon.readthedocs.io/en/released/index.html 101 | .. _ROS: https://www.ros.org 102 | .. _treep: https://pypi.org/project/treep/ 103 | -------------------------------------------------------------------------------- /doc/preempt_rt_kernel.rst: -------------------------------------------------------------------------------- 1 | :orphan: 2 | 3 | ********************************************* 4 | Build and Install the PREEMPT_RT Linux Kernel 5 | ********************************************* 6 | 7 | To get good real-time performance on Linux, we recommend using a Linux kernel 8 | with the `PREEMPT_RT patch`_. Unfortunately, this requires one to build the kernel 9 | from source (at least on Ubuntu). We provide some help for this in the 10 | following. 11 | 12 | Note that Nvidia drivers do officially not support the PREEMPT_RT patch. It may still 13 | work (see :ref:`nvidia_preempt_rt` below) but you are at your own risk. 14 | 15 | An alternative may be the "lowlatency" kernel. It is easier to install and 16 | works with Nvidia drivers but has weaker real-time capabilities (see 17 | :ref:`lowlatency_kernel`). 18 | 19 | 20 | Install PREEMPT_RT Kernel 21 | ========================= 22 | 23 | To install the patched kernel on Ubuntu, you may use our script 24 | ``install_rt_preempt_kernel.sh``. We provide adapted versions for different Ubuntu 25 | versions, which can be found `here `__. 26 | 27 | The ``VERSION_`` variables at the top of the script refer to the kernel version that 28 | will be installed. Usually you can leave the default values, but you can change it here 29 | if you want a different version. See preempt_rt_versions_ for available versions. 30 | 31 | Then simply execute the script in a terminal. Internally, sudo is used in some 32 | steps, so the user executing it needs to have sudo-permission. 33 | 34 | .. note:: 35 | 36 | In the beginning (after downloading some things) you will be asked to manually 37 | adjust some configuration settings. Before entering the menu, the script prints 38 | instructions like the following:: 39 | 40 | Please apply the following configurations in the next step: 41 | 42 | [...] 43 | 44 | General setup [Enter] 45 | Preemption Model (Voluntary Kernel Preemption (Desktop)) [Enter] 46 | Fully Preemptible Kernel (RT) [Enter] #Select 47 | 48 | However, depending on the kernel version the "Preemption Model" setting may be 49 | found in the "Processor type and features" menu instead. 50 | 51 | The script will automatically download, build and install the kernel. This will 52 | take a while. 53 | 54 | When finished, go back to :doc:`real_time` and follow the further steps to configure 55 | your system for real-time usage. 56 | 57 | 58 | .. _nvidia_preempt_rt: 59 | 60 | Using Nvidia Drivers with the PREEMPT_RT Kernel 61 | =============================================== 62 | 63 | Officially, Nvidia drivers do not support the PREEMPT_RT kernel. However, at least with 64 | more recent versions, it seems to work in practice. However, you need set the 65 | environment variable ``IGNORE_PREEMPT_RT_PRESENCE=1`` when installing it with apt. 66 | Complete steps: 67 | 68 | 1. First uninstall any Nvidia drivers 69 | 2. Install the PREEMPT_RT kernel (see above) 70 | 3. Install the drivers with the following command (adjust driver version to the desired 71 | one):: 72 | 73 | sudo IGNORE_PREEMPT_RT_PRESENCE=1 apt install nvidia-driver-530 74 | 75 | Please note that this variable also needs to be set when upgrading packages, so you may 76 | want to set it in a global place like `/etc/environment` and disable unattended upgrades 77 | for the driver. 78 | 79 | 80 | .. _PREEMPT_RT patch: https://wiki.linuxfoundation.org/realtime/start 81 | .. _install_rt_preempt_kernel.sh: https://github.com/machines-in-motion/ubuntu_installation_scripts/tree/master/rt-preempt 82 | .. _preempt_rt_versions: https://wiki.linuxfoundation.org/realtime/preempt_rt_versions 83 | -------------------------------------------------------------------------------- /doc/quick_start_example.rst: -------------------------------------------------------------------------------- 1 | Quick Start Example 2 | =================== 3 | 4 | Sending actions to and getting observations from the robot is very easy. See 5 | the following example, using the TriFinger robot, that simply sends a constant 6 | position command. 7 | 8 | .. note:: 9 | 10 | This example shows only the frontend part of a multi-process setup. The 11 | backend for the actual robot needs to be run in a separate process. 12 | 13 | 14 | Python 15 | ------ 16 | 17 | .. code-block:: Python 18 | 19 | import robot_interfaces 20 | 21 | robot_data = robot_interfaces.trifinger.MultiProcessData( 22 | "trifinger", False) 23 | frontend = robot_interfaces.trifinger.Frontend(robot_data) 24 | 25 | position = [ 26 | 0.0, # Finger 1, Upper Joint 27 | -0.9, # Finger 1, Middle Joint 28 | -1.7, # Finger 1, Lower Joint 29 | 0.0, # Finger 2, Upper Joint 30 | -0.9, # Finger 2, Middle Joint 31 | -1.7, # Finger 2, Lower Joint 32 | 0.0, # Finger 3, Upper Joint 33 | -0.9, # Finger 3, Middle Joint 34 | -1.7, # Finger 3, Lower Joint 35 | ] 36 | 37 | while True: 38 | # construct an action with a position command 39 | action = robot_interfaces.trifinger.Action(position=position) 40 | # send the action to the robot (will be applied in time step t) 41 | t = frontend.append_desired_action(action) 42 | # wait until time step t and get observation 43 | observation = frontend.get_observation(t) 44 | 45 | print("Observed Position: {}".format(observation.position)) 46 | 47 | 48 | 49 | C++ 50 | --- 51 | 52 | .. code-block:: c++ 53 | 54 | #include 55 | 56 | // Some convenience typedefs to make the code below more compact 57 | typedef robot_interfaces::TriFingerTypes::MultiProcessData RobotData; 58 | typedef robot_interfaces::TriFingerTypes::Frontend RobotFrontend; 59 | typedef robot_interfaces::TriFingerTypes::Action Action; 60 | 61 | int main() 62 | { 63 | auto robot_data = std::make_shared("trifinger", false); 64 | auto frontend = RobotFrontend(robot_data); 65 | 66 | Action::Vector position; // <- this is an "Eigen::Vector9d" 67 | position << 0.0, // Finger 1, Upper Joint 68 | -0.9, // Finger 1, Middle Joint 69 | -1.7, // Finger 1, Lower Joint 70 | 0.0, // Finger 2, Upper Joint 71 | -0.9, // Finger 2, Middle Joint 72 | -1.7, // Finger 2, Lower Joint 73 | 0.0, // Finger 3, Upper Joint 74 | -0.9, // Finger 3, Middle Joint 75 | -1.7; // Finger 3, Lower Joint 76 | 77 | while (true) 78 | { 79 | // construct an action with a position command 80 | Action action = Action::Position(position); 81 | // send the action to the robot (will be applied in time step t) 82 | auto t = frontend.append_desired_action(action); 83 | // wait until time step t and get observation 84 | auto observation = frontend.get_observation(t); 85 | 86 | std::cout << "Observed Position: " 87 | << observation.position 88 | << std::endl; 89 | } 90 | 91 | return 0; 92 | } 93 | 94 | When using C++ you need to add the package ``robot_interfaces`` as build 95 | dependency to your package. 96 | 97 | 98 | More Examples 99 | ------------- 100 | 101 | For more examples, see the `C++ demos of the robot_interfaces package 102 | `_ 103 | and the `Python demos in the robot_fingers package 104 | `_. 105 | -------------------------------------------------------------------------------- /doc/real_time.rst: -------------------------------------------------------------------------------- 1 | *************** 2 | Real Time Setup 3 | *************** 4 | 5 | Since a constant update rate of the :cpp:class:`~robot_interfaces::RobotBackend` 6 | is important for stable control of the robot, a real-time capable operating 7 | system should be used. 8 | 9 | For this we are using a standard Linux distribution (Ubuntu in our case) but install a 10 | kernel with better real-time capabilities. There are two options: the "low-latency" 11 | kernel which is easy to install but doesn't give full real-time guarantees, and the 12 | preempt_rt kernel, which has better real-time performance but is more tedious to 13 | install. 14 | 15 | Below are instructions for both of them and on how to configure the system to use them. 16 | 17 | 18 | .. _lowlatency_kernel: 19 | 20 | Installation: Low-latency vs PREEMPT_RT Kernel 21 | ============================================== 22 | 23 | We recommend one of the following Linux kernel variants: 24 | 25 | **low-latency:** 26 | 27 | - Fewer latency guarantees as with PREEMPT_RT but often good enough. Latency can 28 | increase if the system load is high, though. It can be useful to assign separate CPU 29 | cores for the real-time critical processes. 30 | - Can be used with Nvidia drivers. 31 | - Easy to install. On Ubuntu it's just:: 32 | 33 | sudo apt install linux-lowlatency 34 | 35 | 36 | **PREEMPT_RT patch:** 37 | 38 | - Better real-time guarantees. 39 | - Officially not supported by Nvidia drivers (but may still kind of work, see 40 | :ref:`nvidia_preempt_rt`). 41 | - Difficult to install as it has to be built from source. See :doc:`preempt_rt_kernel`. 42 | 43 | 44 | Historically we used the PREEMPT_RT kernel (and still do on many of our robots) but 45 | lately also used the low-latency kernel in some projects without problems (you have to 46 | be a bit more careful to not overload the CPU, though). 47 | Since the low-latency Kernel is much easier to install, you may want to try with that 48 | one first and only move to the PREEMPT_RT version if you notice timing-related issues. 49 | 50 | The further system configuration described in the following is the same in both cases. 51 | 52 | 53 | .. _boot_rt_kernel: 54 | 55 | Boot with the real-time Kernel 56 | ============================== 57 | 58 | Once the real-time kernel is installed, you need to reboot and select the corresponding 59 | kernel ("preempt-rt" or "low-latency") kernel in the GRUB menu (go to "Advanced options 60 | for Ubuntu", it should be listed there). 61 | 62 | When the system is running, you can check which kernel is running with 63 | ``uname -a``. It should print something containing "PREEMPT_RT"/"lowlatency". 64 | 65 | 66 | Select real-time Kernel by Default 67 | ---------------------------------- 68 | 69 | You can configure GRUB to automatically select a specific kernel when booting the 70 | computer. We provide a script to semi-automatically update the configuration (option 71 | 1), but you may also edit it manually (option 2). 72 | 73 | Option 1: Semi-automatic 74 | ~~~~~~~~~~~~~~~~~~~~~~~~ 75 | 76 | We provide a script grub_select_default_kernel.py_ for this. Download it and execute 77 | with 78 | 79 | .. code-block:: bash 80 | 81 | sudo python3 grub_select_default_kernel.py 82 | 83 | It will automatically parse the GRUB configuration and list the available boot options. 84 | Select the desired kernel and press enter. It will then show a diff of the change it 85 | intends to apply to ``/etc/default/grub``. **Review the diff carefully and only confirm 86 | if it looks good!** It should only modify the line starting with ``GRUB_DEFAULT``, i.e. 87 | like this: 88 | 89 | .. code-block:: diff 90 | 91 | --- current 92 | +++ new 93 | @@ -3,7 +3,7 @@ 94 | # For full documentation of the options in this file, see: 95 | # info -f grub -n 'Simple configuration' 96 | 97 | -GRUB_DEFAULT=0 98 | +GRUB_DEFAULT="gnulinux-advanced-cc71b1fb-b530-4694-a839-aecf600f1f49>gnulinux-5.15.0-112-generic-advanced-cc71b1fb-b530-4694-a839-aecf600f1f49" 99 | GRUB_TIMEOUT_STYLE=hidden 100 | GRUB_TIMEOUT=0 101 | GRUB_DISTRIBUTOR=`lsb_release -i -s 2> /dev/null || echo Debian` 102 | 103 | If all is good and you confirmed, you still need to run 104 | 105 | .. code-block:: bash 106 | 107 | sudo update-grub 108 | 109 | to apply the change. Then reboot and verify that the correct kernel is used. 110 | 111 | 112 | Option 2: Manually 113 | ~~~~~~~~~~~~~~~~~~ 114 | 115 | If you don't want to use the script mentioned above, you may also edit the GRUB 116 | configuration manually. 117 | 118 | For this, first the identifier of the kernel needs to be determined. Open a terminal and run 119 | 120 | .. code-block:: bash 121 | 122 | cat /boot/grub/grub.cfg | grep -w -e menuentry -e submenu 123 | 124 | It should print something like this:: 125 | 126 | menuentry 'Ubuntu' --class ubuntu --class gnu-linux --class gnu --class os $menuentry_id_option 'gnulinux-simple-1a26991b-b045-48dd-bb12-064a2725b80b' { 127 | submenu 'Advanced options for Ubuntu' $menuentry_id_option 'gnulinux-advanced-1a26991b-b045-48dd-bb12-064a2725b80b' { 128 | menuentry 'Ubuntu, with Linux 5.4.93-rt51-preempt-rt' --class ubuntu --class gnu-linux --class gnu --class os $menuentry_id_option 'gnulinux-5.4.93-rt51-preempt-rt-advanced-1a26991b-b045-48dd-bb12-064a2725b80b' { 129 | menuentry 'Ubuntu, with Linux 5.4.93-rt51-preempt-rt (recovery mode)' --class ubuntu --class gnu-linux --class gnu --class os $menuentry_id_option 'gnulinux-5.4.93-rt51-preempt-rt-recovery-1a26991b-b045-48dd-bb12-064a2725b80b' { 130 | menuentry 'Ubuntu, with Linux 5.4.0-65-generic' --class ubuntu --class gnu-linux --class gnu --class os $menuentry_id_option 'gnulinux-5.4.0-65-generic-advanced-1a26991b-b045-48dd-bb12-064a2725b80b' { 131 | menuentry 'Ubuntu, with Linux 5.4.0-65-generic (recovery mode)' --class ubuntu --class gnu-linux --class gnu --class os $menuentry_id_option 'gnulinux-5.4.0-65-generic-recovery-1a26991b-b045-48dd-bb12-064a2725b80b' { 132 | 133 | 134 | For ``GRUB_DEFAULT`` the path full submenu/menuentry is needed, using the id of 135 | each step (the last part in the line, "gnulinux-..."), separated by ">". In 136 | this specific case, the setting for starting the rt-kernel would be:: 137 | 138 | GRUB_DEFAULT = "gnulinux-advanced-1a26991b-b045-48dd-bb12-064a2725b80b>gnulinux-5.4.93-rt51-preempt-rt-advanced-471e9718-013f-4cbb-91a7-d22635173b70" 139 | 140 | After saving the changes in ``/etc/default/grub`` you need to run the following 141 | command for the changes to become active:: 142 | 143 | sudo update-grub 144 | 145 | Then reboot and verify that the correct kernel is used. 146 | 147 | 148 | 149 | System Configuration 150 | ==================== 151 | 152 | Normally root permission is needed to run real-time processes. To allow 153 | non-root users to run real-time processes, apply the configuration described in 154 | this section. 155 | 156 | 157 | Create the "realtime" Group and Add Users 158 | ----------------------------------------- 159 | 160 | If you used our install script to install the PREEMPT_RT kernel, the "realtime" 161 | group is already created automatically. Otherwise, you can create it manually 162 | with this command: 163 | 164 | .. code-block:: sh 165 | 166 | sudo groupadd realtime 167 | 168 | Then add users that should be able to run real-time applications to the group: 169 | 170 | .. code-block:: sh 171 | 172 | sudo usermod -aG realtime 173 | 174 | 175 | Set rtprio and memlock limits 176 | ----------------------------- 177 | 178 | **Note:** This step can be skipped if you used our install script to install the 179 | PREEMPT_RT kernel. 180 | 181 | Create a file ``/etc/security/limits.d/99-realtime.conf`` with the following 182 | content to increase the rtprio and memlock limit for users in the realtime 183 | group:: 184 | 185 | @realtime - rtprio 99 186 | @realtime - memlock unlimited 187 | 188 | 189 | 190 | Set Permissions of /dev/cpu_dma_latency 191 | --------------------------------------- 192 | 193 | The following command needs to be run after each reboot to allow non-root users 194 | to set the CPU DMA latency:: 195 | 196 | sudo chmod 0666 /dev/cpu_dma_latency 197 | 198 | 199 | Alternatively you can set up a udev rule to do this automatically. For this, 200 | simply create a file ``/etc/udev/rules.d/55-cpu_dma_latency.rules`` with the 201 | following content: 202 | 203 | .. code-block:: 204 | 205 | KERNEL=="cpu_dma_latency", NAME="cpu_dma_latency", MODE="0666" 206 | 207 | 208 | Building robot_interfaces for the Real-Time System 209 | ================================================== 210 | 211 | To be able to run the robot backend in a real-time thread, some options need to 212 | be set at build time. This is automatically done when building on a system that 213 | is running the PREEMPT_RT or lowlatency kernel. However, if you previously 214 | built while running a different kernel, you'll need to rebuild. 215 | 216 | You can also explicitly build with real-time settings on any kernel with the 217 | following command:: 218 | 219 | colcon build --cmake-args -DOS_VERSION=preempt-rt 220 | 221 | 222 | .. note:: 223 | 224 | If you see the following output during initialisation of the robot, this 225 | means you are running a non-real-time build. 226 | 227 | .. code-block:: text 228 | 229 | Warning this thread is not going to be real time. 230 | 231 | 232 | .. _grub_select_default_kernel.py: https://github.com/machines-in-motion/ubuntu_installation_scripts/blob/master/rt-preempt/grub_select_default_kernel.py 233 | -------------------------------------------------------------------------------- /doc/robot_data.rst: -------------------------------------------------------------------------------- 1 | RobotData -- Single or Multi Process 2 | ==================================== 3 | 4 | The *RobotData* serves as a communication link between the back end and the 5 | front end (see :ref:`architecture`). It contains time series structures, in 6 | which all the data (actions, observations, etc.) is stored. 7 | 8 | The interface is defined in the base class 9 | :cpp:class:`~robot_interfaces::RobotData`. There are two different 10 | implementations that implement the interface: 11 | 12 | - A "single-process" implementation 13 | (:cpp:class:`~robot_interfaces::SingleProcessRobotData`), keeping the data in 14 | a buffer in normal memory. This allows fastest access but all modules (front 15 | end, back end, ...) must run in the same process. 16 | This is easiest to use for simple setups where everything is in one script. 17 | - A "multi-process" implementation 18 | (:cpp:class:`~robot_interfaces::MultiProcessRobotData`) using a `shared memory 19 | `_ which can be accessed 20 | by multiple processes. With this, front end, back end, etc. can be executed as 21 | separate processes. This allows a more secure setup as user code (using the 22 | front end) and back-end configuration can be decoupled, thus not allowing the 23 | user to change safety-critical settings of the robot (e.g. the maximum motor 24 | current). 25 | 26 | See the demos_ for implementations with both the single- and the multi-process 27 | RobotData. 28 | 29 | 30 | .. _demos: https://github.com/open-dynamic-robot-initiative/robot_interfaces/blob/master/demos 31 | -------------------------------------------------------------------------------- /doc/timeseries.rst: -------------------------------------------------------------------------------- 1 | Logic of Actions and Observations 2 | ================================= 3 | 4 | In this section the logic of the *time series* used for communication between 5 | front and back end and for synchronization between actions and observations is 6 | explained. For more details, please see our paper_ on the open-source version 7 | of the TriFinger robot. 8 | 9 | 10 | On Time Series and Time Relation of Actions and Observations 11 | ------------------------------------------------------------ 12 | 13 | 14 | All data transfer between the front end (= user code) and the back end (= robot 15 | hardware) goes through so called time series. When calling 16 | ``append_desired_action(action)``, the action is not applied immediately but is 17 | *appended* to the time series of desired actions which serves as a queue. 18 | 19 | At each time step, identified by a *time index t*, the backend takes the action 20 | at position *t* from the "desired actions" time series and sends it to the robot 21 | driver. At the same time an observation is acquired from the robot and added to 22 | the "observation" time series. This means that the effect of the desired action 23 | ``a_t`` is not yet visible in the observation ``y_t`` as is illustrated below 24 | (``a'_t`` corresponds to the *applied action*, see 25 | :doc:`desired_vs_applied_action`). 26 | 27 | .. image:: images/action_observation_timing.png 28 | :alt: Time relation of actions and observations 29 | 30 | 31 | ``append_desired_action()`` returns the time index ``t`` at which the appended 32 | action will be executed. Methods like ``get_observation()`` expect a time index 33 | as input. If the specified time step has already passed, they immediately 34 | return the value from the corresponding step. If it lies in the future, the 35 | method will block and wait until the specified time step is reached and then 36 | return. 37 | 38 | Note that the buffer size of the time series is limited (see the 39 | ``history_length`` argument of ``SingleProcessRobotData`` and 40 | ``MultiProcessRobotData``). If the buffer is full, the oldest element is 41 | discarded. Trying to access an time index that is not in the buffer anymore 42 | results in an exception. 43 | 44 | 45 | This design allows for simple code that is automatically executed at the control 46 | rate of the robot: 47 | 48 | .. code-block:: Python 49 | 50 | # send zero-torque action to get first observation, see explanation below 51 | zero_torque_action = robot_interfaces.trifinger.Action() 52 | t = frontend.append_desired_action(zero_torque_action) 53 | observation = frontend.get_observation(t) 54 | 55 | while True: 56 | action = smart_algorithm_to_compute_next_action(observation) 57 | 58 | t = frontend.append_desired_action(action) 59 | # The t given above refers to the moment the given action will be 60 | # executed. Right now, this is in the future, so the following call 61 | # will automatically wait until the action is actually applied to the 62 | # platform 63 | observation = frontend.get_observation(t) 64 | 65 | 66 | Send Action to Start Backend 67 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 68 | 69 | In the beginning of the program execution, the back end is idle and 70 | waiting for the first action. Only after the first action is received, 71 | the loop is started that applies actions and writes observations to the 72 | time series. 73 | 74 | This means **you first have to send an action before you can read the 75 | first observation!** 76 | 77 | There are applications where an observation is needed before sending the 78 | first real action (e.g. when the action depends on the current 79 | position). In this case you need to send a "neutral" action first. How this 80 | action may look is robot dependent. The TriFinger robot, for example, can 81 | safely be started with a zero-torque action: 82 | 83 | 84 | Python: 85 | 86 | .. code-block:: Python 87 | 88 | # an action without arguments defaults to zero torque 89 | zero_torque_action = robot_interfaces.trifinger.Action() 90 | t = frontend.append_desired_action(zero_torque_action) 91 | first_observation = frontend.get_observation(t) 92 | 93 | C++: 94 | 95 | .. code-block:: C++ 96 | 97 | Action zero_torque_action = Action::Zero(); 98 | auto t = frontend.append_desired_action(zero_torque_action); 99 | auto first_observation = frontend.get_observation(t); 100 | 101 | Note that the creation of the zero torque action in the above example is 102 | specific to the _TriFinger_ robot. For other robots, the creation of the action 103 | would need to be adjusted to the action type of that specific robot. 104 | 105 | 106 | When Next Action Is Not Provided In Time 107 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 108 | 109 | If the back end reaches a time step ``t`` but the user did not yet provide 110 | an action for this time step (e.g. because the user code is running 111 | slower than 1 kHz), the back end automatically sets the desired action 112 | for step ``t`` to the same as the one of ``t - 1``. 113 | 114 | This is indicated to the user through the ``action_repetitions`` field in 115 | the status message which contains the number of times the current action 116 | has been repeated. 117 | 118 | 119 | .. _paper: https://arxiv.org/abs/2008.03596 120 | -------------------------------------------------------------------------------- /doc_mainpage.rst: -------------------------------------------------------------------------------- 1 | This is the documentation of the **robot_interfaces** package. 2 | 3 | The source code is hosted on GitHub_. Please also use the issue system there if 4 | you have a question or want to report a bug. 5 | 6 | For more information, on the general architecture of the software, see also our 7 | paper_ on the open-source version of the TriFinger robot. 8 | 9 | 10 | .. toctree:: 11 | :caption: How-to Guides 12 | :hidden: 13 | 14 | doc/installation.rst 15 | doc/real_time.rst 16 | doc/quick_start_example.rst 17 | doc/custom_driver.rst 18 | doc/custom_sensor.rst 19 | 20 | .. toctree:: 21 | :caption: Topic Guides 22 | :hidden: 23 | 24 | doc/about_architecture.rst 25 | doc/timeseries.rst 26 | doc/robot_data.rst 27 | doc/desired_vs_applied_action.rst 28 | 29 | 30 | 31 | 32 | 33 | 34 | Links 35 | ----- 36 | 37 | - Code on GitHub_ 38 | - `Bug Tracker`_ 39 | - The implementation of the TriFinger robot driver can be found in the 40 | :doc:`robot_fingers ` package. For Solo12 see 41 | :doc:`robot_interfaces_solo `. 42 | 43 | 44 | .. _GitHub: https://github.com/open-dynamic-robot-initiative/robot_interfaces 45 | .. _Bug Tracker: https://github.com/open-dynamic-robot-initiative/robot_interfaces/issues 46 | .. _paper: https://arxiv.org/abs/2008.03596 47 | -------------------------------------------------------------------------------- /include/robot_interfaces/example.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * license License BSD-3-Clause 4 | * @copyright Copyright (c) 2019, Max Planck Gesellschaft. 5 | * 6 | * @brief Example driver and types for demo and testing purposes. 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace robot_interfaces 17 | { 18 | namespace example 19 | { 20 | /** 21 | * @brief Actions to be performed by robot, will be received by Driver. 22 | * 23 | * An action simply encapsulate two desired position value, one for each DOF. 24 | */ 25 | class Action : public Loggable 26 | { 27 | public: 28 | int values[2]; 29 | 30 | void print(bool backline) 31 | { 32 | std::cout << "action: " << values[0] << " " << values[1] << " "; 33 | if (backline) 34 | { 35 | std::cout << "\n"; 36 | } 37 | } 38 | 39 | // the following methods are only needed when you want to use RobotLogger 40 | 41 | template 42 | void serialize(Archive &archive) 43 | { 44 | archive(values[0], values[1]); 45 | } 46 | 47 | virtual std::vector get_name() 48 | { 49 | return {"values"}; 50 | } 51 | 52 | virtual std::vector> get_data() 53 | { 54 | return { 55 | {static_cast(values[0]), static_cast(values[1])}}; 56 | } 57 | }; 58 | 59 | /** 60 | * @brief Observation read from the robot by Driver 61 | * 62 | * An observation is the current position for each DOF. 63 | */ 64 | class Observation : public Loggable 65 | { 66 | public: 67 | int values[2]; 68 | 69 | void print(bool backline) 70 | { 71 | std::cout << "observation: " << values[0] << " " << values[1] << " "; 72 | if (backline) 73 | { 74 | std::cout << "\n"; 75 | } 76 | } 77 | 78 | template 79 | void serialize(Archive &archive) 80 | { 81 | archive(values[0], values[1]); 82 | } 83 | 84 | virtual std::vector get_name() 85 | { 86 | return {"values"}; 87 | } 88 | 89 | virtual std::vector> get_data() 90 | { 91 | return { 92 | {static_cast(values[0]), static_cast(values[1])}}; 93 | } 94 | }; 95 | 96 | /** 97 | * @brief Example Robot Driver. 98 | * 99 | * Send command to the robot and read observation from the robot. 100 | * The DOF positions simply becomes the ones set by the latest action, capped 101 | * between a min and a max value. 102 | */ 103 | class Driver : public robot_interfaces::RobotDriver 104 | { 105 | public: 106 | Driver(int min, int max) : min_(min), max_(max) 107 | { 108 | } 109 | 110 | // at init dof are at min value 111 | void initialize() 112 | { 113 | state_[0] = min_; 114 | state_[1] = min_; 115 | } 116 | 117 | // just clip desired values 118 | // between 0 and 1000 119 | Action apply_action(const Action &action_to_apply) 120 | { 121 | Action applied; 122 | for (unsigned int i = 0; i < 2; i++) 123 | { 124 | if (action_to_apply.values[i] > max_) 125 | { 126 | applied.values[i] = max_; 127 | } 128 | else if (action_to_apply.values[i] < min_) 129 | { 130 | applied.values[i] = min_; 131 | } 132 | else 133 | { 134 | applied.values[i] = action_to_apply.values[i]; 135 | } 136 | // simulating the time if could take for a real 137 | // robot to perform the action 138 | usleep(1000); 139 | state_[i] = applied.values[i]; 140 | } 141 | return applied; 142 | } 143 | 144 | Observation get_latest_observation() 145 | { 146 | Observation observation; 147 | observation.values[0] = state_[0]; 148 | observation.values[1] = state_[1]; 149 | return observation; 150 | } 151 | 152 | std::optional get_error() 153 | { 154 | return std::nullopt; // no error 155 | } 156 | 157 | void shutdown() 158 | { 159 | } 160 | 161 | private: 162 | int state_[2]; 163 | int min_; 164 | int max_; 165 | }; 166 | 167 | } // namespace example 168 | } // namespace robot_interfaces 169 | -------------------------------------------------------------------------------- /include/robot_interfaces/finger_types.hpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // BSD 3-Clause License 3 | // 4 | // Copyright (C) 2018-2019, Max Planck Gesellschaft 5 | // Copyright note valid unless otherwise stated in individual files. 6 | // All rights reserved. 7 | /////////////////////////////////////////////////////////////////////////////// 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | namespace robot_interfaces 14 | { 15 | constexpr size_t JOINTS_PER_FINGER = 3; 16 | constexpr size_t BOARDS_PER_FINGER = 2; 17 | 18 | /** 19 | * @brief Types for the Finger robot (basic 3-joint robot). 20 | */ 21 | template 22 | struct FingerTypes 23 | : public RobotInterfaceTypes, 24 | NFingerObservation> 25 | { 26 | }; 27 | 28 | // typedefs for common number of fingers 29 | typedef FingerTypes<1> MonoFingerTypes; 30 | typedef FingerTypes<3> TriFingerTypes; 31 | 32 | } // namespace robot_interfaces 33 | -------------------------------------------------------------------------------- /include/robot_interfaces/loggable.hpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // BSD 3-Clause License 3 | // 4 | // Copyright (C) 2018-2019, Max Planck Gesellschaft 5 | // Copyright note valid unless otherwise stated in individual files. 6 | // All rights reserved. 7 | /////////////////////////////////////////////////////////////////////////////// 8 | 9 | #pragma once 10 | 11 | namespace robot_interfaces 12 | { 13 | /* 14 | * @brief Contains definitions of the methods to be implemented by all the robot 15 | * data types. 16 | */ 17 | 18 | class Loggable 19 | { 20 | public: 21 | // virtual destructor is needed for class with virtual methods 22 | virtual ~Loggable() 23 | { 24 | } 25 | 26 | /* 27 | * @brief Return the names of the fields in the structure. 28 | * Current restriction: a returned name should not have spaces between 29 | * the words, use "_" instead. 30 | */ 31 | virtual std::vector get_name() = 0; 32 | 33 | /* 34 | * @brief Return the data in the fields of the structure. 35 | */ 36 | virtual std::vector> get_data() = 0; 37 | }; 38 | 39 | } // namespace robot_interfaces 40 | -------------------------------------------------------------------------------- /include/robot_interfaces/monitored_robot_driver.hpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // BSD 3-Clause License 3 | // 4 | // Copyright (C) 2018-2019, Max Planck Gesellschaft 5 | // Copyright note valid unless otherwise stated in individual files. 6 | // All rights reserved. 7 | /////////////////////////////////////////////////////////////////////////////// 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | namespace robot_interfaces 25 | { 26 | /** 27 | * @brief Wrapper for RobotDriver that monitors timing. 28 | * 29 | * Takes a RobotDriver instance as input and forwards all method calls to it. A 30 | * background loop monitors timing of actions to ensure the following 31 | * constraints: 32 | * 33 | * 1. The execution of an action does not take longer than 34 | * `max_action_duration_s_` seconds. 35 | * 2. The time interval between termination of the previous action and 36 | * receival of the next one (through `apply_action()`) does not exceed 37 | * `max_inter_action_duration_s_`. 38 | * 39 | * If these timing constraints are not satisfied, the robot will be shutdown, 40 | * and no more actions from the outside will be accepted. 41 | * 42 | * This wrapper also makes sure that the `shutdown()` method of the given 43 | * RobotDriver is called when wrapper is destroyed, so the robot should always 44 | * be left in a safe state. 45 | * 46 | * @tparam Action 47 | * @tparam Observation 48 | */ 49 | template 50 | class MonitoredRobotDriver 51 | : public RobotDriver 52 | { 53 | public: 54 | typedef std::shared_ptr RobotDriverPtr; 55 | 56 | /** 57 | * @brief Starts a thread for monitoring timing of action execution. 58 | * 59 | * @param robot_driver The actual robot driver instance. 60 | * @param max_action_duration_s Maximum time allowed for an action to be 61 | * executed. 62 | * @param max_inter_action_duration_s Maximum time allowed between end of 63 | * the previous action and receival of the next one. 64 | */ 65 | MonitoredRobotDriver(RobotDriverPtr robot_driver, 66 | const double max_action_duration_s, 67 | const double max_inter_action_duration_s) 68 | : robot_driver_(robot_driver), 69 | max_action_duration_s_(max_action_duration_s), 70 | max_inter_action_duration_s_(max_inter_action_duration_s), 71 | is_shutdown_(false), 72 | action_start_logger_(1000), 73 | action_end_logger_(1000) 74 | { 75 | thread_ = std::make_shared(); 76 | 77 | // if both timeouts are infinite, there is no need to start the loop at 78 | // all 79 | if (std::isfinite(max_action_duration_s_) && 80 | std::isfinite(max_inter_action_duration_s_)) 81 | { 82 | thread_->create_realtime_thread(&MonitoredRobotDriver::loop, this); 83 | } 84 | else 85 | { 86 | std::cerr 87 | << "WARNING: MonitoredRobotDriver was created with a " 88 | "non-finite timeout. The monitoring loop is NOT executed. " 89 | "If monitoring is not needed, consider using the driver " 90 | "directly without the MonitoredRobotDriver-wrapper." 91 | << std::endl; 92 | } 93 | } 94 | 95 | /** 96 | * @brief Shuts down the robot and stops the monitoring thread. 97 | */ 98 | ~MonitoredRobotDriver() 99 | { 100 | shutdown(); 101 | thread_->join(); 102 | } 103 | 104 | /** 105 | * @brief Apply desired action on the robot. 106 | * 107 | * If the robot is shut down, no more actions will be applied (the method 108 | * will just ignore them silently. 109 | * 110 | * @param desired_action The desired action. 111 | * @return The action that is actually applied on the robot (may differ 112 | * from desired action due to safety limitations). 113 | */ 114 | typename Driver::Action apply_action( 115 | const typename Driver::Action &desired_action) final override 116 | { 117 | if (is_shutdown_) 118 | { 119 | // FIXME I don't think it makes sense to return the desired action 120 | // in case of shutdown. Shouldn't it rather be s.th. like Zero()? 121 | return desired_action; 122 | } 123 | action_start_logger_.append(true); 124 | typename Driver::Action applied_action = 125 | robot_driver_->apply_action(desired_action); 126 | action_end_logger_.append(true); 127 | return applied_action; 128 | } 129 | 130 | void initialize() override 131 | { 132 | robot_driver_->initialize(); 133 | } 134 | 135 | typename Driver::Action get_idle_action() override 136 | { 137 | return robot_driver_->get_idle_action(); 138 | } 139 | 140 | typename Driver::Observation get_latest_observation() override 141 | { 142 | return robot_driver_->get_latest_observation(); 143 | } 144 | 145 | std::optional get_error() override 146 | { 147 | auto driver_error = robot_driver_->get_error(); 148 | if (driver_error) 149 | { 150 | return driver_error; 151 | } 152 | 153 | switch (error_) 154 | { 155 | case ErrorType::ACTION_START_TIMEOUT: 156 | return "Action did not start on time, shutting down."; 157 | case ErrorType::ACTION_END_TIMEOUT: 158 | return "Action did not end on time, shutting down."; 159 | case ErrorType::NONE: 160 | default: 161 | break; 162 | } 163 | 164 | return std::nullopt; 165 | } 166 | 167 | /** 168 | * @brief Shut down the robot safely. 169 | * 170 | * After shutdown, actions sent by the user are ignored. 171 | */ 172 | void shutdown() final override 173 | { 174 | if (!is_shutdown_) 175 | { 176 | is_shutdown_ = true; 177 | robot_driver_->shutdown(); 178 | } 179 | } 180 | 181 | private: 182 | enum class ErrorType 183 | { 184 | NONE, 185 | ACTION_START_TIMEOUT, 186 | ACTION_END_TIMEOUT 187 | }; 188 | 189 | //! \brief The actual robot driver. 190 | RobotDriverPtr robot_driver_; 191 | //! \brief Max. time for executing an action. 192 | double max_action_duration_s_; 193 | //! \brief Max. idle time between actions. 194 | double max_inter_action_duration_s_; 195 | 196 | //! \brief Whether shutdown was initiated. 197 | std::atomic is_shutdown_; 198 | 199 | time_series::TimeSeries action_start_logger_; 200 | time_series::TimeSeries action_end_logger_; 201 | 202 | std::shared_ptr thread_; 203 | 204 | std::atomic error_ = ErrorType::NONE; 205 | 206 | /** 207 | * @brief Monitor the timing of action execution. 208 | * 209 | * If one of the timing constrains is violated, the robot is immediately 210 | * shut down. 211 | */ 212 | void loop() 213 | { 214 | // wait for the first data 215 | while (!is_shutdown_ && 216 | !action_start_logger_.wait_for_timeindex(0, 0.1)) 217 | { 218 | } 219 | 220 | // loop until shutdown and monitor action timing 221 | for (size_t t = 0; !is_shutdown_; t++) 222 | { 223 | bool action_has_ended_on_time = 224 | action_end_logger_.wait_for_timeindex(t, 225 | max_action_duration_s_); 226 | if (!action_has_ended_on_time) 227 | { 228 | error_ = ErrorType::ACTION_END_TIMEOUT; 229 | shutdown(); 230 | return; 231 | } 232 | 233 | bool action_has_started_on_time = 234 | action_start_logger_.wait_for_timeindex( 235 | t + 1, max_inter_action_duration_s_); 236 | if (!action_has_started_on_time) 237 | { 238 | error_ = ErrorType::ACTION_START_TIMEOUT; 239 | shutdown(); 240 | return; 241 | } 242 | } 243 | } 244 | 245 | static void *loop(void *instance_pointer) 246 | { 247 | ((MonitoredRobotDriver *)(instance_pointer))->loop(); 248 | return nullptr; 249 | } 250 | }; 251 | 252 | } // namespace robot_interfaces 253 | -------------------------------------------------------------------------------- /include/robot_interfaces/n_finger_observation.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Observation of a Finger robot. 4 | * @copyright 2020, Max Planck Gesellschaft. All rights reserved. 5 | * @license BSD 3-clause 6 | */ 7 | #pragma once 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | namespace robot_interfaces 18 | { 19 | /** 20 | * @brief Observation of a Finger robot. 21 | * 22 | * 23 | * Values like angular position, velocity or torque of the joints are 24 | * represented as a 1-dimensional vector with one element per joint. The order 25 | * of the joints in these vectors is as follows: 26 | * 27 | * 0. Finger 1, upper joint 28 | * 1. Finger 1, middle joint 29 | * 2. Finger 1, lower joint 30 | * 3. Finger 2, upper joint 31 | * 4. Finger 2, middle joint 32 | * 5. Finger 2, lower joint 33 | * ... 34 | * #. Finger n, upper joint 35 | * #. Finger n, middle joint 36 | * #. Finger n, lower joint 37 | * 38 | * @tparam N_FINGERS Number of fingers. 39 | */ 40 | template 41 | struct NFingerObservation : public Loggable 42 | { 43 | static constexpr size_t num_fingers = N_FINGERS; 44 | static constexpr size_t num_joints = N_FINGERS * 3; 45 | 46 | typedef Eigen::Matrix JointVector; 47 | typedef Eigen::Matrix FingerVector; 48 | 49 | //! @brief Measured angular position of all joints in radian. 50 | JointVector position = JointVector::Zero(); 51 | 52 | //! @brief Measured velocity of all joints in radian/second. 53 | JointVector velocity = JointVector::Zero(); 54 | 55 | //! @brief Measured torques of all joints in Nm. 56 | JointVector torque = JointVector::Zero(); 57 | 58 | /** 59 | * @brief Measurements of the pressure sensors at the finger tips. 60 | * 61 | * One per finger. Ranges between 0 and 1 without specific unit. Note that 62 | * not all fingers are equipped with an actual sensor! For fingers without 63 | * sensor, this value is undefined. 64 | */ 65 | FingerVector tip_force = FingerVector::Zero(); 66 | 67 | template 68 | void serialize(Archive& archive) 69 | { 70 | archive(position, velocity, torque, tip_force); 71 | } 72 | 73 | std::vector get_name() override 74 | { 75 | return {"position", "velocity", "torque", "tip_force"}; 76 | } 77 | 78 | std::vector> get_data() override 79 | { 80 | typedef std::vector vecd; 81 | 82 | std::vector result = {vecd(position.size()), 83 | vecd(velocity.size()), 84 | vecd(torque.size()), 85 | vecd(tip_force.size())}; 86 | 87 | JointVector::Map(&result[0][0], position.size()) = position; 88 | JointVector::Map(&result[1][0], velocity.size()) = velocity; 89 | JointVector::Map(&result[2][0], torque.size()) = torque; 90 | FingerVector::Map(&result[3][0], tip_force.size()) = tip_force; 91 | 92 | return result; 93 | } 94 | }; 95 | } // namespace robot_interfaces 96 | -------------------------------------------------------------------------------- /include/robot_interfaces/n_joint_action.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Action of a generic n-joint robot. 4 | * @copyright 2020, Max Planck Gesellschaft. All rights reserved. 5 | * @license BSD 3-clause 6 | */ 7 | #pragma once 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | namespace robot_interfaces 19 | { 20 | /** 21 | * @brief Action of a generic n-joint robot. 22 | * 23 | * This action type can be used for all n-joint robots that expect torque or 24 | * position commands on joint-level. 25 | * 26 | * @tparam N Number of joints. 27 | */ 28 | template 29 | struct NJointAction : public Loggable 30 | { 31 | //! @brief Number of joints. 32 | static constexpr size_t num_joints = N; 33 | 34 | typedef Eigen::Matrix Vector; 35 | 36 | //! Desired torque command (in addition to position controller). 37 | Vector torque; 38 | //! Desired position. Set to NaN to disable position controller. 39 | Vector position; 40 | //! P-gain for position controller. If NaN, default is used. 41 | Vector position_kp; 42 | //! D-gain for position controller. If NaN, default is used. 43 | Vector position_kd; 44 | 45 | template 46 | void serialize(Archive& archive) 47 | { 48 | archive(torque, position, position_kp, position_kd); 49 | } 50 | 51 | std::vector get_name() override 52 | { 53 | return {"torque", "position", "position_kp", "position_kd"}; 54 | } 55 | 56 | std::vector> get_data() override 57 | { 58 | // first map the Eigen vectors to std::vectors 59 | std::vector torque_; 60 | torque_.resize(torque.size()); 61 | Vector::Map(&torque_[0], torque.size()) = torque; 62 | 63 | std::vector position_; 64 | position_.resize(position.size()); 65 | Vector::Map(&position_[0], position.size()) = position; 66 | 67 | std::vector position_kp_; 68 | position_kp_.resize(position_kp.size()); 69 | Vector::Map(&position_kp_[0], position_kp.size()) = position_kp; 70 | 71 | std::vector position_kd_; 72 | position_kd_.resize(position_kd.size()); 73 | Vector::Map(&position_kd_[0], position_kd.size()) = position_kd; 74 | 75 | // then return them in a fixed size vector of vectors to avoid 76 | // copying due to pushing back value of information! 77 | std::vector> result; 78 | result = {torque_, position_, position_kp_, position_kd_}; 79 | 80 | return result; 81 | } 82 | 83 | /** 84 | * @brief Create action with desired torque and (optional) position. 85 | * 86 | * The resulting torque command sent to the robot is 87 | * 88 | * sent_torque = torque + PD(position) 89 | * 90 | * To disable the position controller, set the target position to NaN. 91 | * The controller is executed joint-wise, so it is possible to run it 92 | * only for some joints by setting a target position for these joints 93 | * and setting the others to NaN. 94 | * 95 | * The specified torque is always added to the result of the position 96 | * controller, so if you only want to run the position controller, make 97 | * sure to set `torque` to zero for all joints. 98 | * 99 | * For more explicit code, the static factory methods `Torque`, 100 | * `Position`, `TorqueAndPosition` and `Zero` should be used instead 101 | * directly creating actions through this constructor. 102 | * 103 | * @param torque Desired torque. 104 | * @param position Desired position. Set values to NaN to disable 105 | * position controller for the corresponding joints 106 | * @param position_kp P-gains for the position controller. Set to NaN 107 | * to use default values. 108 | * @param position_kd D-gains for the position controller. Set to NaN 109 | * to use default values. 110 | */ 111 | NJointAction(Vector torque = Vector::Zero(), 112 | Vector position = None(), 113 | Vector position_kp = None(), 114 | Vector position_kd = None()) 115 | : torque(torque), 116 | position(position), 117 | position_kp(position_kp), 118 | position_kd(position_kd) 119 | { 120 | } 121 | 122 | /** 123 | * @brief Create an action that only contains a torque command. 124 | * 125 | * @param torque Desired torque. 126 | * 127 | * @return Pure "torque action". 128 | */ 129 | static NJointAction Torque(Vector torque) 130 | { 131 | return NJointAction(torque); 132 | } 133 | 134 | /** 135 | * @brief Create an action that only contains a position command. 136 | * 137 | * @param position Desired position. 138 | * @param kp P-gain for position controller. If not set, default is 139 | * used. Set to NaN for specific joints to use default for this 140 | * joint. 141 | * @param kd D-gain for position controller. If not set, default is 142 | * used. Set to NaN for specific joints to use default for this 143 | * joint. 144 | * 145 | * @return Pure "position action". 146 | */ 147 | static NJointAction Position(Vector position, 148 | Vector kp = None(), 149 | Vector kd = None()) 150 | { 151 | return NJointAction(Vector::Zero(), position, kp, kd); 152 | } 153 | 154 | /** 155 | * @brief Create an action with both torque and position commands. 156 | * 157 | * @param torque Desired torque. 158 | * @param position Desired position. Set to NaN for specific joints to 159 | * disable position control for this joint. 160 | * @param kp P-gain for position controller. If not set, default is 161 | * used. Set to NaN for specific joints to use default for this 162 | * joint. 163 | * @param kd D-gain for position controller. If not set, default is 164 | * used. Set to NaN for specific joints to use default for this 165 | * joint. 166 | * 167 | * @return Action with both torque and position commands. 168 | */ 169 | static NJointAction TorqueAndPosition(Vector torque, 170 | Vector position, 171 | Vector position_kp = None(), 172 | Vector position_kd = None()) 173 | { 174 | return NJointAction(torque, position, position_kp, position_kd); 175 | } 176 | 177 | /** 178 | * @brief Create a zero-torque action. 179 | * 180 | * @return Zero-torque action with position control disabled. 181 | */ 182 | static NJointAction Zero() 183 | { 184 | return NJointAction(); 185 | } 186 | 187 | /** 188 | * @brief Create a NaN-Vector. Helper function to set defaults for 189 | * position. 190 | * 191 | * @return Vector with all elements set to NaN. 192 | */ 193 | static Vector None() 194 | { 195 | return Vector::Constant(std::numeric_limits::quiet_NaN()); 196 | } 197 | }; 198 | 199 | } // namespace robot_interfaces 200 | -------------------------------------------------------------------------------- /include/robot_interfaces/n_joint_observation.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Observation of a generic n-joint robot. 4 | * @copyright 2020, Max Planck Gesellschaft. All rights reserved. 5 | * @license BSD 3-clause 6 | */ 7 | #pragma once 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | namespace robot_interfaces 18 | { 19 | /** 20 | * @brief Basic observation for a generic n-joint robot. 21 | * 22 | * Simple observation type with position, velocity and torque for each joint. 23 | * 24 | * @tparam N Number of joints. 25 | */ 26 | template 27 | struct NJointObservation : public Loggable 28 | { 29 | //! @brief Number of joints. 30 | static constexpr size_t num_joints = N; 31 | 32 | typedef Eigen::Matrix Vector; 33 | 34 | Vector position = Vector::Zero(); 35 | Vector velocity = Vector::Zero(); 36 | Vector torque = Vector::Zero(); 37 | 38 | template 39 | void serialize(Archive& archive) 40 | { 41 | archive(position, velocity, torque); 42 | } 43 | 44 | std::vector get_name() override 45 | { 46 | return {"position", "velocity", "torque"}; 47 | } 48 | 49 | std::vector> get_data() override 50 | { 51 | typedef std::vector vecd; 52 | 53 | std::vector result = { 54 | vecd(position.size()), vecd(velocity.size()), vecd(torque.size())}; 55 | 56 | Vector::Map(&result[0][0], position.size()) = position; 57 | Vector::Map(&result[1][0], velocity.size()) = velocity; 58 | Vector::Map(&result[2][0], torque.size()) = torque; 59 | 60 | return result; 61 | } 62 | }; 63 | 64 | } // namespace robot_interfaces 65 | -------------------------------------------------------------------------------- /include/robot_interfaces/n_joint_robot_types.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Types for an n-joint robot. 4 | * @copyright 2020, Max Planck Gesellschaft. All rights reserved. 5 | * @license BSD 3-clause 6 | */ 7 | #pragma once 8 | 9 | #include "n_finger_observation.hpp" 10 | #include "n_joint_action.hpp" 11 | #include "n_joint_observation.hpp" 12 | #include "types.hpp" 13 | 14 | namespace robot_interfaces 15 | { 16 | /** 17 | * @brief Collection of types for a generic N-joint BLMC robot. 18 | * 19 | * Defines all the types needed to set up an interface to a generic N-joint BLMC 20 | * robot that expects as Action a simple vector of N torque commands and 21 | * provides N observations containing measured joint angle, velocity and torque. 22 | * 23 | * @tparam N Number of joints 24 | */ 25 | template 26 | struct SimpleNJointRobotTypes 27 | : public RobotInterfaceTypes, NJointObservation> 28 | { 29 | }; 30 | 31 | } // namespace robot_interfaces 32 | -------------------------------------------------------------------------------- /include/robot_interfaces/pybind_finger.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Functions for creating Python bindings for BLMC CAN robots. 4 | * @copyright 2019, Max Planck Gesellschaft. All rights reserved. 5 | * @license BSD 3-clause 6 | */ 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | namespace robot_interfaces 21 | { 22 | /** 23 | * @bind Add Python bindings for Types::Observaton::tip_force if it exists. 24 | * 25 | * Uses black SFINAE magic to add bindings for "tip_force" if it exists. 26 | * Further the pickle functions differ due to this, so handle this here as well. 27 | * 28 | * Usage: 29 | * 30 | * BindTipForceIfExists::bind(pybind_class); 31 | * 32 | * This is based on https://stackoverflow.com/a/16000226, see there for an 33 | * explanation how/why this works. 34 | */ 35 | template 36 | struct BindTipForceIfExists 37 | { 38 | static void bind(pybind11::class_ &c) 39 | { 40 | c.def(pybind11::pickle( 41 | [](const typename Types::Observation &o) { // __getstate__ 42 | return pybind11::make_tuple(o.position, o.velocity, o.torque); 43 | }, 44 | [](pybind11::tuple t) { // __setstate__ 45 | if (t.size() != 3) 46 | { 47 | throw std::runtime_error("Invalid state!"); 48 | } 49 | 50 | typename Types::Observation obs; 51 | obs.position = t[0].cast(); 52 | obs.velocity = t[1].cast(); 53 | obs.torque = t[2].cast(); 54 | 55 | return obs; 56 | })); 57 | } 58 | }; 59 | template 60 | struct BindTipForceIfExists 62 | { 63 | static void bind(pybind11::class_ &c) 64 | { 65 | c.def_readwrite("tip_force", 66 | &Types::Observation::tip_force, 67 | "Measurements of the push sensors at the finger tips, " 68 | "one per finger. Ranges between 0 and 1."); 69 | 70 | c.def(pybind11::pickle( 71 | [](const typename Types::Observation &o) { // __getstate__ 72 | return pybind11::make_tuple( 73 | o.position, o.velocity, o.torque, o.tip_force); 74 | }, 75 | [](pybind11::tuple t) { // __setstate__ 76 | if (t.size() != 4) 77 | { 78 | throw std::runtime_error("Invalid state!"); 79 | } 80 | 81 | typename Types::Observation obs; 82 | obs.position = 83 | t[0].cast(); 84 | obs.velocity = 85 | t[1].cast(); 86 | obs.torque = 87 | t[2].cast(); 88 | obs.tip_force = 89 | t[3].cast(); 90 | 91 | return obs; 92 | })); 93 | } 94 | }; 95 | 96 | /** 97 | * \brief Create Python bindings for the specified BLMC-CAN-robot Types. 98 | * 99 | * With this function, Python bindings can easily be created for new robots that 100 | * are based on the NJointRobotTypes. Example: 101 | * 102 | * PYBIND11_MODULE(py_fortytwo_types, m) 103 | * { 104 | * create_blmc_can_python_bindings>(m); 105 | * } 106 | * 107 | * \tparam Types An instance of NJointRobotTypes. 108 | * \param m The second argument of the PYBIND11_MODULE macro. 109 | */ 110 | template 111 | void create_blmc_can_robot_python_bindings(pybind11::module &m) 112 | { 113 | pybind11::options options; 114 | // disable automatic function signature generation as this does not look too 115 | // nice in the Sphinx documentation. 116 | options.disable_function_signatures(); 117 | 118 | create_interface_python_bindings(m); 120 | 121 | pybind11::class_(m, 122 | "Action", 123 | R"XXX( 124 | Action(torque=[0] * n_joints, position=[nan] * n_joints, position_kp=[nan] * n_joints, position_kd=[nan] * n_joints) 125 | 126 | Action with desired torque and (optional) position. 127 | 128 | The resulting torque command sent to the robot is:: 129 | 130 | torque_command = torque + PD(position) 131 | 132 | To disable the position controller, set the target position to 133 | NaN. The controller is executed joint-wise, so it is possible 134 | to run it only for some joints by setting a target position for 135 | these joints and setting the others to NaN. 136 | 137 | The specified torque is always added to the result of the 138 | position controller, so if you only want to run the position 139 | controller, make sure to set `torque` to zero for all joints. 140 | 141 | Args: 142 | torque: Desired torque. 143 | position: Desired position. Set values to NaN to disable 144 | position controller for the corresponding joints 145 | position_kp: P-gains for the position controller. Set to 146 | NaN to use default values. 147 | position_kd: D-gains for the position controller. Set to 148 | NaN to use default values. 149 | )XXX") 150 | .def_readwrite("torque", 151 | &Types::Action::torque, 152 | "List of desired torques, one per joint.") 153 | .def_readwrite( 154 | "position", 155 | &Types::Action::position, 156 | "List of desired positions, one per joint. If set, a PD " 157 | "position controller is run and the resulting torque is " 158 | "added to :attr:`torque`. Set to NaN to disable " 159 | "position controller (default).") 160 | .def_readwrite("position_kp", 161 | &Types::Action::position_kp, 162 | "P-gains for position controller, one per joint. If " 163 | "NaN, default is used.") 164 | .def_readwrite("position_kd", 165 | &Types::Action::position_kd, 166 | "D-gains for position controller, one per joint. If " 167 | "NaN, default is used.") 168 | .def(pybind11::init(), 172 | pybind11::arg("torque") = Types::Action::Vector::Zero(), 173 | pybind11::arg("position") = Types::Action::None(), 174 | pybind11::arg("position_kp") = Types::Action::None(), 175 | pybind11::arg("position_kd") = Types::Action::None()) 176 | .def(pybind11::pickle( 177 | [](const typename Types::Action &a) { // __getstate__ 178 | // Return a tuple that fully encodes the state of the object 179 | return pybind11::make_tuple( 180 | a.torque, a.position, a.position_kp, a.position_kd); 181 | }, 182 | [](pybind11::tuple t) { // __setstate__ 183 | if (t.size() != 4) 184 | { 185 | throw std::runtime_error("Invalid state!"); 186 | } 187 | 188 | // Create a new C++ instance 189 | typename Types::Action action( 190 | t[0].cast(), 191 | t[1].cast(), 192 | t[2].cast(), 193 | t[3].cast()); 194 | 195 | return action; 196 | })); 197 | 198 | auto obs = 199 | pybind11::class_(m, "Observation") 200 | .def(pybind11::init<>()) 201 | .def_readwrite( 202 | "position", 203 | &Types::Observation::position, 204 | "List of angular joint positions [rad], one per joint.") 205 | .def_readwrite( 206 | "velocity", 207 | &Types::Observation::velocity, 208 | "List of angular joint velocities [rad/s], one per joint.") 209 | .def_readwrite("torque", 210 | &Types::Observation::torque, 211 | "List of torques [Nm], one per joint."); 212 | BindTipForceIfExists::bind(obs); 213 | } 214 | 215 | } // namespace robot_interfaces 216 | -------------------------------------------------------------------------------- /include/robot_interfaces/pybind_helper.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Helper functions for creating Python bindings. 4 | * @copyright 2019, Max Planck Gesellschaft. All rights reserved. 5 | * @license BSD 3-clause 6 | */ 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | 18 | namespace robot_interfaces 19 | { 20 | //! Create Python bindings for the RobotData classes. 21 | template 22 | void create_robot_data_python_bindings(pybind11::module &m) 23 | { 24 | typedef RobotInterfaceTypes Types; 25 | 26 | pybind11::options options; 27 | // disable automatic function signature generation as this does not look too 28 | // nice in the Sphinx documentation. 29 | options.disable_function_signatures(); 30 | 31 | // bindings for the different time series types 32 | time_series::create_python_bindings( 33 | m, "_ActionTimeSeries"); 34 | time_series::create_multiprocesses_python_bindings( 35 | m, "_ActionMultiProcessTimeSeries"); 36 | time_series::create_python_bindings( 37 | m, "_ObservationTimeSeries"); 38 | time_series::create_multiprocesses_python_bindings< 39 | typename Types::Observation>(m, "_ObservationMultiProcessTimeSeries"); 40 | 41 | pybind11::class_( 42 | m, "BaseData"); 43 | 44 | pybind11::class_(m, "SingleProcessData") 47 | .def(pybind11::init(), pybind11::arg("history_size") = 1000) 48 | .def_readonly("desired_action", 49 | &Types::SingleProcessData::desired_action) 50 | .def_readonly("applied_action", 51 | &Types::SingleProcessData::applied_action) 52 | .def_readonly("observation", &Types::SingleProcessData::observation) 53 | .def_readonly("status", &Types::SingleProcessData::status); 54 | 55 | pybind11::class_(m, "MultiProcessData") 58 | .def(pybind11::init(), 59 | pybind11::arg("shared_memory_id_prefix"), 60 | pybind11::arg("is_master"), 61 | pybind11::arg("history_size") = 1000) 62 | .def_readonly("desired_action", 63 | &Types::MultiProcessData::desired_action) 64 | .def_readonly("applied_action", 65 | &Types::MultiProcessData::applied_action) 66 | .def_readonly("observation", &Types::MultiProcessData::observation) 67 | .def_readonly("status", &Types::MultiProcessData::status); 68 | } 69 | 70 | //! Create Python bindings for RobotBackend 71 | template 72 | void create_robot_backend_python_bindings(pybind11::module &m) 73 | { 74 | typedef RobotInterfaceTypes Types; 75 | 76 | pybind11::options options; 77 | // disable automatic function signature generation as this does not look too 78 | // nice in the Sphinx documentation. 79 | options.disable_function_signatures(); 80 | 81 | pybind11::class_( 82 | m, "Backend") 83 | .def("initialize", 84 | &Types::Backend::initialize, 85 | pybind11::call_guard()) 86 | .def("request_shutdown", 87 | &Types::Backend::request_shutdown, 88 | pybind11::call_guard()) 89 | .def("wait_until_first_action", 90 | &Types::Backend::wait_until_first_action, 91 | pybind11::call_guard()) 92 | .def("wait_until_terminated", 93 | &Types::Backend::wait_until_terminated, 94 | pybind11::call_guard()) 95 | .def("is_running", 96 | &Types::Backend::is_running, 97 | pybind11::call_guard()) 98 | .def("get_termination_reason", 99 | &Types::Backend::get_termination_reason, 100 | pybind11::call_guard()); 101 | } 102 | 103 | //! Create Python bindings for RobotFrontend 104 | template 105 | void create_robot_frontend_python_bindings(pybind11::module &m) 106 | { 107 | typedef RobotInterfaceTypes Types; 108 | 109 | pybind11::options options; 110 | // disable automatic function signature generation as this does not look too 111 | // nice in the Sphinx documentation. 112 | options.disable_function_signatures(); 113 | 114 | // Release the GIL when calling any of the front-end functions, so in case 115 | // there are subthreads running Python, they have a chance to acquire the 116 | // GIL. 117 | pybind11::class_( 118 | m, "Frontend") 119 | .def(pybind11::init()) 120 | .def("get_observation", 121 | &Types::Frontend::get_observation, 122 | pybind11::call_guard()) 123 | .def("get_desired_action", 124 | &Types::Frontend::get_desired_action, 125 | pybind11::call_guard()) 126 | .def("get_applied_action", 127 | &Types::Frontend::get_applied_action, 128 | pybind11::call_guard()) 129 | .def("get_status", 130 | &Types::Frontend::get_status, 131 | pybind11::call_guard()) 132 | .def("get_timestamp_ms", 133 | &Types::Frontend::get_timestamp_ms, 134 | pybind11::call_guard()) 135 | .def("append_desired_action", 136 | &Types::Frontend::append_desired_action, 137 | pybind11::call_guard()) 138 | .def("wait_until_timeindex", 139 | &Types::Frontend::wait_until_timeindex, 140 | pybind11::call_guard()) 141 | .def("get_current_timeindex", 142 | &Types::Frontend::get_current_timeindex, 143 | pybind11::call_guard()); 144 | } 145 | 146 | //! Create Python bindings for RobotLogger and related classes. 147 | template 148 | void create_robot_logger_python_bindings(pybind11::module &m) 149 | { 150 | typedef RobotInterfaceTypes Types; 151 | 152 | pybind11::options options; 153 | // disable automatic function signature generation as this does not look too 154 | // nice in the Sphinx documentation. 155 | options.disable_function_signatures(); 156 | 157 | pybind11::class_( 158 | m, "LogEntry", "Represents the logged of one time step.") 159 | .def_readwrite("timeindex", &Types::LogEntry::timeindex) 160 | .def_readwrite("timestamp", &Types::LogEntry::timestamp) 161 | .def_readwrite("status", &Types::LogEntry::status) 162 | .def_readwrite("observation", &Types::LogEntry::observation) 163 | .def_readwrite("desired_action", &Types::LogEntry::desired_action) 164 | .def_readwrite("applied_action", &Types::LogEntry::applied_action); 165 | 166 | pybind11::class_ logger(m, "Logger"); 167 | logger 168 | .def(pybind11::init(), 169 | pybind11::arg("robot_data"), 170 | pybind11::arg("buffer_limit")) 171 | .def("start", 172 | &Types::Logger::start, 173 | pybind11::call_guard()) 174 | .def("stop", 175 | &Types::Logger::stop, 176 | pybind11::call_guard()) 177 | .def("stop_and_save", 178 | &Types::Logger::stop_and_save, 179 | pybind11::call_guard()) 180 | .def("is_running", 181 | &Types::Logger::is_running, 182 | pybind11::call_guard()) 183 | .def("reset", 184 | &Types::Logger::reset, 185 | pybind11::call_guard()) 186 | .def("save_current_robot_data", 187 | &Types::Logger::save_current_robot_data, 188 | pybind11::arg("filename"), 189 | pybind11::arg("start_index") = 0, 190 | pybind11::arg("end_index") = -1, 191 | pybind11::call_guard()) 192 | .def("save_current_robot_data_binary", 193 | &Types::Logger::save_current_robot_data_binary, 194 | pybind11::arg("filename"), 195 | pybind11::arg("start_index") = 0, 196 | pybind11::arg("end_index") = -1, 197 | pybind11::call_guard()); 198 | 199 | pybind11::enum_(logger, "Format") 200 | .value("BINARY", Types::Logger::Format::BINARY) 201 | .value("CSV", Types::Logger::Format::CSV) 202 | .value("CSV_GZIP", Types::Logger::Format::CSV_GZIP); 203 | 204 | pybind11::class_>( 206 | m, 207 | "BinaryLogReader", 208 | "BinaryLogReader(filename: str)\n\nSee :meth:`read_file`.") 209 | .def(pybind11::init()) 210 | .def("read_file", 211 | &Types::BinaryLogReader::read_file, 212 | pybind11::arg("filename"), 213 | R"XXX( 214 | read_file(filename: str) 215 | 216 | Read data from the specified binary robot log file. 217 | 218 | The data is stored to :attr:`data`. 219 | 220 | Args: 221 | filename (str): Path to the robot log file. 222 | )XXX") 223 | .def_readonly("data", 224 | &Types::BinaryLogReader::data, 225 | "List[LogEntry]: Contains the log entries."); 226 | } 227 | 228 | /** 229 | * \brief Create Python bindings for the robot interface classes. 230 | * 231 | * Creates bindings for RobotData, RobotBackend, RobotFrontend and 232 | * logging-related classes. 233 | * 234 | * Usage Example: 235 | * 236 | * PYBIND11_MODULE(my_robot, m) 237 | * { 238 | * create_interface_python_bindings(m); 239 | * 240 | * // You still need provide bindings for MyAction and MyObservation! 241 | * } 242 | * 243 | * \tparam Action The action type that is used for the robot. 244 | * \tparam Observation The observation type that is used for the robot. 245 | * \param m The second argument of the PYBIND11_MODULE macro. 246 | */ 247 | template 248 | void create_interface_python_bindings(pybind11::module &m) 249 | { 250 | pybind11::options options; 251 | // disable automatic function signature generation as this does not look too 252 | // nice in the Sphinx documentation. 253 | options.disable_function_signatures(); 254 | 255 | create_robot_data_python_bindings(m); 256 | create_robot_backend_python_bindings(m); 257 | create_robot_frontend_python_bindings(m); 258 | create_robot_logger_python_bindings(m); 259 | } 260 | 261 | } // namespace robot_interfaces 262 | -------------------------------------------------------------------------------- /include/robot_interfaces/robot.hpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // BSD 3-Clause License 3 | // 4 | // Copyright (C) 2018-2019, Max Planck Gesellschaft 5 | // Copyright note valid unless otherwise stated in individual files. 6 | // All rights reserved. 7 | /////////////////////////////////////////////////////////////////////////////// 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace robot_interfaces 18 | { 19 | /** 20 | * @brief RobotFrontend that construct and encapsulates 21 | * its related RobotBackend. It also construct and starts 22 | * the robot driver. 23 | */ 24 | template > 28 | class Robot : public RobotFrontend 29 | { 30 | typedef std::shared_ptr DataPtr; 31 | typedef std::shared_ptr RobotDriverPtr; 32 | 33 | public: 34 | /** 35 | * @param max_action_duration_s See MonitoredRobotDriver. 36 | * @param max_inter_action_duration_s See MonitoredRobotDriver. 37 | * @param args Arguments for instantiating Driver 38 | */ 39 | template 40 | Robot(double max_action_duration_s, 41 | double max_inter_action_duration_s, 42 | Args... args) 43 | : RobotFrontend(std::make_shared()), 44 | driver_ptr_(std::make_shared(args...)), 45 | backend_( 46 | std::make_shared>( 47 | driver_ptr_, 48 | max_action_duration_s, 49 | max_inter_action_duration_s), 50 | this->robot_data_, 51 | true, // real time mode 52 | std::numeric_limits::infinity(), // first_action_timeout 53 | 0) // max_number_of_actions 54 | { 55 | // compile time checking template Driver inherate from RobotDriver 56 | static_assert( 57 | std::is_base_of, Driver>::value, 58 | "template Driver must be a subclass of " 59 | "robot_interfaces::RobotDriver"); 60 | } 61 | 62 | /** Robot which instantiates a non real time mode backend 63 | * @param max_action_duration_s See MonitoredRobotDriver. 64 | * @param max_inter_action_duration_s See MonitoredRobotDriver. 65 | * @param args Arguments for instantiating Driver 66 | */ 67 | template 68 | Robot(Args... args) 69 | : RobotFrontend(std::make_shared()), 70 | driver_ptr_(std::make_shared(args...)), 71 | backend_( 72 | std::make_shared>( 73 | driver_ptr_, 74 | std::numeric_limits< 75 | double>::infinity(), // max_action_duration_s, 76 | std::numeric_limits< 77 | double>::infinity()), // max_inter_action_duration_s) 78 | this->robot_data_, 79 | false, // real time mode 80 | std::numeric_limits::infinity(), // first_action_timeout 81 | std::numeric_limits::infinity()) // max_number_of_actions 82 | { 83 | // compile time checking template Driver inherate from RobotDriver 84 | static_assert( 85 | std::is_base_of, Driver>::value, 86 | "template Driver must be a subclass of " 87 | "robot_interfaces::RobotDriver"); 88 | } 89 | 90 | /** 91 | * initialize the backend 92 | */ 93 | void initialize() 94 | { 95 | backend_.initialize(); 96 | } 97 | 98 | /** 99 | * return the data shared by the frontend and the backend. 100 | */ 101 | const Data& get_data() const 102 | { 103 | return *(this->robot_data_); 104 | } 105 | 106 | private: 107 | RobotDriverPtr driver_ptr_; 108 | RobotBackend backend_; 109 | }; 110 | } // namespace robot_interfaces 111 | -------------------------------------------------------------------------------- /include/robot_interfaces/robot_data.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief RobotData classes for both single- and multi-process applications. 8 | */ 9 | 10 | #pragma once 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | #include "status.hpp" 20 | 21 | namespace robot_interfaces 22 | { 23 | /** 24 | * @brief Contains all the input and output data of the robot. 25 | * 26 | * This means the 27 | * - `desired_action` which was requested by the robot user 28 | * - `applied_action` which was actually applied and may not be 29 | * and may not be identical to desired_action 30 | * for safety reasons 31 | * - `observation` made by the robot 32 | * - `status` which keeps track of timing issues and errors. 33 | * 34 | * See this graph to understand how they relate to each other precisely 35 | * in terms of time: 36 | * 37 | * @verbatim 38 | |------ t = 0 ------|------ t = 1 ------| 39 | |----- action0 -----|----- action1 -----| 40 | o o o 41 | b b b 42 | s s s 43 | 0 1 2 44 | @endverbatim 45 | * 46 | * @tparam Action Type of the actions. 47 | * @tparam Observation Type of the observations. 48 | */ 49 | template 50 | class RobotData 51 | { 52 | public: 53 | typedef std::shared_ptr> Ptr; 54 | typedef std::shared_ptr> ConstPtr; 55 | 56 | //! @brief Time series of the desired actions. 57 | std::shared_ptr> desired_action; 58 | //! @brief Time series of the actually applied actions (due to safety 59 | // checks). 60 | std::shared_ptr> applied_action; 61 | //! @brief Time series of the observations retrieved from the robot. 62 | std::shared_ptr> observation; 63 | //! @brief Time series of status messages. 64 | std::shared_ptr> status; 65 | 66 | protected: 67 | // make constructor protected to prevent instantiation of the base class 68 | RobotData(){}; 69 | }; 70 | 71 | /** 72 | * @brief RobotData instance using single process time series. 73 | * 74 | * Use this class if all modules accessing the data are running in the same 75 | * process. If modules run in separate processes, use MultiProcessRobotData 76 | * instead. 77 | * 78 | * @copydoc RobotData 79 | * @see MultiProcessRobotData 80 | */ 81 | template 82 | class SingleProcessRobotData : public RobotData 83 | { 84 | public: 85 | /** 86 | * @brief Construct the time series for the robot data. 87 | * 88 | * @param history_length History length of the time series. 89 | */ 90 | SingleProcessRobotData(size_t history_length = 1000) 91 | { 92 | std::cout << "Using single process time series." << std::endl; 93 | this->desired_action = 94 | std::make_shared>(history_length); 95 | this->applied_action = 96 | std::make_shared>(history_length); 97 | this->observation = 98 | std::make_shared>( 99 | history_length); 100 | this->status = 101 | std::make_shared>(history_length); 102 | } 103 | }; 104 | 105 | /** 106 | * @brief RobotData instance using multi process time series. 107 | * 108 | * Use this class if modules accessing the data are running in separate 109 | * processes. When all modules run as threads in the same process, this class 110 | * can be used as well, however, SingleProcessRobotData might be more efficient 111 | * in that case. 112 | * 113 | * @copydoc RobotData 114 | * @see SingleProcessRobotData 115 | */ 116 | template 117 | class MultiProcessRobotData : public RobotData 118 | { 119 | public: 120 | /** 121 | * @brief Construct the time series for the robot data. 122 | * 123 | * @param shared_memory_id_prefix Prefix for the shared memory IDs. Since 124 | * each time series needs its own memory ID, the given value is used as 125 | * prefix and unique suffixes are appended. Make sure to use a prefix 126 | * that cannot lead to name collisions on your system. 127 | * @param is_master If set to true, this instance will clear the shared 128 | * memory on construction and destruction. Only one instance should 129 | * act as master in a multi-process setup. 130 | * @param history_length History length of the time series. Ignored if 131 | * `is_master == false`. 132 | * 133 | * @todo Make this constructor protected and implement factory methods like 134 | * in MultiprocessTimeSeries.. 135 | */ 136 | MultiProcessRobotData(const std::string &shared_memory_id_prefix, 137 | bool is_master, 138 | size_t history_length = 1000) 139 | { 140 | std::cout << "Using multi process time series." << std::endl; 141 | 142 | // each time series needs its own shared memory ID, so add unique 143 | // suffixes to the given ID. 144 | const std::string id_desired_action = 145 | shared_memory_id_prefix + "_desired_action"; 146 | const std::string id_applied_action = 147 | shared_memory_id_prefix + "_applied_action"; 148 | const std::string id_observation = 149 | shared_memory_id_prefix + "_observation"; 150 | const std::string id_status = shared_memory_id_prefix + "_status"; 151 | 152 | typedef time_series::MultiprocessTimeSeries TS_Action; 153 | typedef time_series::MultiprocessTimeSeries TS_Observation; 154 | typedef time_series::MultiprocessTimeSeries TS_Status; 155 | 156 | if (is_master) 157 | { 158 | // the master instance is in charge of cleaning the memory 159 | time_series::clear_memory(id_desired_action); 160 | time_series::clear_memory(id_applied_action); 161 | time_series::clear_memory(id_observation); 162 | time_series::clear_memory(id_status); 163 | 164 | this->desired_action = 165 | TS_Action::create_leader_ptr(id_desired_action, history_length); 166 | this->applied_action = 167 | TS_Action::create_leader_ptr(id_applied_action, history_length); 168 | this->observation = TS_Observation::create_leader_ptr( 169 | id_observation, history_length); 170 | this->status = 171 | TS_Status::create_leader_ptr(id_status, history_length); 172 | } 173 | else 174 | { 175 | this->desired_action = 176 | TS_Action::create_follower_ptr(id_desired_action); 177 | this->applied_action = 178 | TS_Action::create_follower_ptr(id_applied_action); 179 | this->observation = 180 | TS_Observation::create_follower_ptr(id_observation); 181 | this->status = TS_Status::create_follower_ptr(id_status); 182 | } 183 | } 184 | }; 185 | 186 | } // namespace robot_interfaces 187 | -------------------------------------------------------------------------------- /include/robot_interfaces/robot_driver.hpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // BSD 3-Clause License 3 | // 4 | // Copyright (C) 2018-2019, Max Planck Gesellschaft 5 | // Copyright note valid unless otherwise stated in individual files. 6 | // All rights reserved. 7 | /////////////////////////////////////////////////////////////////////////////// 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace robot_interfaces 15 | { 16 | /** 17 | * @brief Driver for interfacing the actual robot hardware or simulation. 18 | * 19 | * Interface to the robot used by the subsequent classes. Any robot (be it real 20 | * or simulation) has to derive from this class and implement the functions 21 | * apply_action(), get_latest_observation() and shutdown(). 22 | * This Base class provides some timing logic around those three functions. It 23 | * makes sure that after the first call of apply_action(), it is always called 24 | * again after some specified time, otherwise the shutdown() method will 25 | * be called. This Base class also makes sure that the apply_action() function 26 | * itself does not take more time than expected. 27 | * 28 | * @tparam Action 29 | * @tparam Observation 30 | */ 31 | template 32 | class RobotDriver 33 | { 34 | public: 35 | typedef TAction Action; 36 | typedef TObservation Observation; 37 | 38 | // virtual destructor is needed for class with virtual methods 39 | virtual ~RobotDriver() 40 | { 41 | } 42 | 43 | /** 44 | * @brief Initialize the robot. 45 | * 46 | * Any initialization procedures that need to be done before sending 47 | * actions to the robot should be done in this method (e.g. homing to find 48 | * the absolute position). 49 | */ 50 | virtual void initialize() = 0; 51 | 52 | /** 53 | * @brief Apply action immediately and block until it is executed. 54 | * 55 | * This method must apply the desired_action immediately when it is called, 56 | * and only return once the action has been executed completely. This way 57 | * we can accommodate both simulators and real robots with this interface. 58 | * 59 | * @param desired_action The action we want to apply. 60 | * @return The action that was actually applied (since due to safety 61 | * reasons it might not be possible to apply the desired action). 62 | */ 63 | virtual Action apply_action(const Action &desired_action) = 0; 64 | 65 | /** 66 | * @brief Return the latest observation immediately. 67 | * 68 | * @return Observation 69 | */ 70 | virtual Observation get_latest_observation() = 0; 71 | 72 | /** 73 | * @brief Get error message if there is any error. 74 | * 75 | * Uses std::optional for the return type, so an actual string only needs to 76 | * be created if there is an error. This is relevant as std::string is in 77 | * general not real-time safe and should thus be avoided. In case of an 78 | * error this does not matter, as the control loop will be stopped anyway. 79 | * 80 | * @return Returns an error message or std::nullopt if there is no error. 81 | */ 82 | virtual std::optional get_error() = 0; 83 | 84 | /** 85 | * @brief Shut down the robot safely. 86 | * 87 | * Use this method if your robot needs to perform some action when shutting 88 | * down, e.g. to move it to a defined rest position. 89 | */ 90 | virtual void shutdown() = 0; 91 | 92 | /** 93 | * @brief Return action that is safe to apply while the robot is idle. 94 | * 95 | * Typically this can be an action applying zero torque to all joints but it 96 | * might be more involved depending on the robot (it could, for example, 97 | * also be an action to hold the joints in place). 98 | * 99 | * The default implementation simply uses the default constructor of Action, 100 | * assuming that this is safe to use. 101 | */ 102 | virtual Action get_idle_action() 103 | { 104 | // by default, assume that the default constructor of Action provides a 105 | // suitable action. 106 | return Action(); 107 | } 108 | }; 109 | 110 | } // namespace robot_interfaces 111 | -------------------------------------------------------------------------------- /include/robot_interfaces/robot_frontend.hpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // BSD 3-Clause License 3 | // 4 | // Copyright (C) 2018-2019, Max Planck Gesellschaft 5 | // Copyright note valid unless otherwise stated in individual files. 6 | // All rights reserved. 7 | /////////////////////////////////////////////////////////////////////////////// 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace robot_interfaces 19 | { 20 | template 21 | using Timeseries = time_series::TimeSeries; 22 | 23 | typedef time_series::Index TimeIndex; 24 | 25 | /** 26 | * @brief Communication link between RobotData and the user. 27 | * 28 | * Takes care of communication between the RobotData and the user. It is just a 29 | * thin wrapper around RobotData to facilitate interaction and also to make sure 30 | * the user cannot use RobotData in incorrect ways. 31 | * 32 | * @tparam Action 33 | * @tparam Observation 34 | */ 35 | template 36 | class RobotFrontend 37 | { 38 | public: 39 | typedef std::shared_ptr> Ptr; 40 | typedef std::shared_ptr> ConstPtr; 41 | typedef time_series::Timestamp TimeStamp; 42 | 43 | RobotFrontend(std::shared_ptr> robot_data) 44 | : robot_data_(robot_data) 45 | { 46 | } 47 | 48 | /** 49 | * @brief Get observation of time step t. 50 | * 51 | * @param t Index of the time step. If t is in the future, this method will 52 | * block and wait. 53 | * @return The observation of time step t. 54 | * @throws std::invalid_argument if t is too old and not in the time series 55 | * buffer anymore. 56 | */ 57 | Observation get_observation(const TimeIndex &t) const 58 | { 59 | return (*robot_data_->observation)[t]; 60 | } 61 | 62 | /** 63 | * @brief Get the desired action of time step t. 64 | * 65 | * The desired action is the action as it is passed by the user in @ref 66 | * append_desired_action. 67 | * 68 | * @param t Index of the time step. If t is in the future, this method will 69 | * block and wait. 70 | * @return The desired action of time step t. 71 | * @throws std::invalid_argument if t is too old and not in the time series 72 | * buffer anymore. 73 | */ 74 | Action get_desired_action(const TimeIndex &t) const 75 | { 76 | return (*robot_data_->desired_action)[t]; 77 | } 78 | 79 | /** 80 | * @brief Get the applied action of time step t. 81 | * 82 | * The applied action is the one that was actually applied to the robot 83 | * based on the desired action of that time step. It may differ from the 84 | * desired one e.g. due to some safety checks which limit the maximum 85 | * torque. If and how the action is modified depends on the implementation 86 | * of the @ref RobotDriver. 87 | * 88 | * @param t Index of the time step. If t is in the future, this method will 89 | * block and wait. 90 | * @return The applied action of time step t. 91 | * @throws std::invalid_argument if t is too old and not in the time series 92 | * buffer anymore. 93 | */ 94 | Action get_applied_action(const TimeIndex &t) const 95 | { 96 | return (*robot_data_->applied_action)[t]; 97 | } 98 | Status get_status(const TimeIndex &t) const 99 | { 100 | return (*robot_data_->status)[t]; 101 | } 102 | 103 | /** 104 | * @brief Get the timestamp of time step t. 105 | * 106 | * @param t Index of the time step. If t is in the future, this method will 107 | * block and wait. 108 | * @return Timestamp of time step t. 109 | * @throws std::invalid_argument if t is too old and not in the time series 110 | * buffer anymore. 111 | */ 112 | TimeStamp get_timestamp_ms(const TimeIndex &t) const 113 | { 114 | return robot_data_->observation->timestamp_ms(t); 115 | } 116 | 117 | /** 118 | * @brief Get the current time index. 119 | * 120 | * @return The latest time index for which observations are available. 121 | */ 122 | TimeIndex get_current_timeindex() const 123 | { 124 | return robot_data_->observation->newest_timeindex(); 125 | } 126 | 127 | /** 128 | * @brief Append a desired action to the action time series. 129 | * 130 | * This will append an action to the "desired actions" time series. Note 131 | * that this does not block until the action is actually executed. The time 132 | * series acts like a queue from which the @ref RobotBackend takes the 133 | * actions one by one to send them to the actual robot. It is possible to 134 | * call this method multiple times in a row to already provide actions for 135 | * the next time steps. 136 | * 137 | * The time step at which the given action will be applied is returned by 138 | * this method. 139 | * 140 | * @param desired_action The action that shall be applied on the robot. 141 | * Note that the actually applied action might be different depending on 142 | * the implementation of the @ref RobotDriver (see @ref 143 | * get_applied_action). 144 | * @return Time step at which the action will be applied. 145 | */ 146 | TimeIndex append_desired_action(const Action &desired_action) 147 | { 148 | // check error state. do not allow appending actions if there is an 149 | // error 150 | if (robot_data_->status->length() > 0) 151 | { 152 | const Status status = robot_data_->status->newest_element(); 153 | switch (status.error_status) 154 | { 155 | case Status::ErrorStatus::NO_ERROR: 156 | break; 157 | case Status::ErrorStatus::DRIVER_ERROR: 158 | throw std::runtime_error("Driver Error: " + 159 | status.get_error_message()); 160 | case Status::ErrorStatus::BACKEND_ERROR: 161 | throw std::runtime_error("Backend Error: " + 162 | status.get_error_message()); 163 | default: 164 | throw std::runtime_error("Unknown Error: " + 165 | status.get_error_message()); 166 | } 167 | } 168 | 169 | // since the timeseries has a finite memory, we need to make sure that 170 | // by appending new actions we do not forget about actions which have 171 | // not been applied yet 172 | if (robot_data_->desired_action->length() == 173 | robot_data_->desired_action->max_length() && 174 | robot_data_->desired_action->oldest_timeindex() == // FIXME >= 175 | get_current_timeindex()) 176 | { 177 | std::cout 178 | << "you have been appending actions too fast, waiting for " 179 | "RobotBackend to catch up with executing actions." 180 | << std::endl; 181 | wait_until_timeindex( 182 | robot_data_->desired_action->oldest_timeindex() + 1); 183 | } 184 | 185 | robot_data_->desired_action->append(desired_action); 186 | return robot_data_->desired_action->newest_timeindex(); 187 | } 188 | 189 | /** 190 | * @brief Wait until the specified time step is reached. 191 | * 192 | * @param t Time step until which is waited. 193 | * @throws std::invalid_argument if t is too old and not in the time series 194 | * buffer anymore. 195 | */ 196 | void wait_until_timeindex(const TimeIndex &t) const 197 | { 198 | robot_data_->observation->timestamp_ms(t); 199 | } 200 | 201 | protected: 202 | std::shared_ptr> robot_data_; 203 | }; 204 | 205 | } // namespace robot_interfaces 206 | -------------------------------------------------------------------------------- /include/robot_interfaces/robot_log_entry.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file 3 | * \copyright Copyright (c) 2020, Max Planck Gesellschaft. 4 | */ 5 | #pragma once 6 | 7 | #include 8 | 9 | #include 10 | 11 | namespace robot_interfaces 12 | { 13 | /** 14 | * @brief Robot log entry used for binary log format. 15 | * 16 | * Contains all the robot data of one time step. 17 | */ 18 | template 19 | struct RobotLogEntry 20 | { 21 | time_series::Index timeindex; 22 | time_series::Timestamp timestamp; 23 | Status_t status; 24 | Observation observation; 25 | Action desired_action; 26 | Action applied_action; 27 | 28 | template 29 | void serialize(Archive& archive) 30 | { 31 | archive(timeindex, 32 | timestamp, 33 | status, 34 | observation, 35 | desired_action, 36 | applied_action); 37 | } 38 | }; 39 | } // namespace robot_interfaces 40 | -------------------------------------------------------------------------------- /include/robot_interfaces/robot_log_reader.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief API to read the data from a robot log file. 4 | * @copyright 2020, Max Planck Gesellschaft. All rights reserved. 5 | * @license BSD 3-clause 6 | */ 7 | #pragma once 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | namespace robot_interfaces 22 | { 23 | /** 24 | * @brief Read the data from a robot log file. 25 | * 26 | * The data is read from the specified file and stored to the `data` member 27 | * where it can be accessed. 28 | */ 29 | template 30 | class RobotBinaryLogReader 31 | { 32 | public: 33 | static constexpr uint32_t FORMAT_VERSION = 2; 34 | 35 | typedef RobotLogEntry LogEntry; 36 | 37 | std::vector data; 38 | 39 | RobotBinaryLogReader() 40 | { 41 | } 42 | 43 | //! @copydoc RobotBinaryLogReader::read_file() 44 | RobotBinaryLogReader(const std::string &filename) 45 | { 46 | read_file(filename); 47 | } 48 | 49 | /** 50 | * @brief Read data from the specified file. 51 | * 52 | * The data is stored to RobotBinaryLogReader::data. 53 | * 54 | * @param filename Path to the robot log file. 55 | */ 56 | void read_file(const std::string &filename) 57 | { 58 | std::ifstream infile(filename, std::ios::binary); 59 | if (!infile) 60 | { 61 | throw std::runtime_error("Failed to open file " + filename); 62 | } 63 | 64 | auto infile_compressed = serialization_utils::gzip_istream(infile); 65 | cereal::BinaryInputArchive archive(*infile_compressed); 66 | 67 | std::uint32_t format_version; 68 | archive(format_version); 69 | 70 | if (format_version != FORMAT_VERSION) 71 | { 72 | throw std::runtime_error("Incompatible log file format."); 73 | } 74 | 75 | archive(data); 76 | } 77 | 78 | /** 79 | * @brief Write data to the specified file. 80 | * 81 | * @param filename Path to the output file. 82 | */ 83 | void write_file(const std::string &filename) 84 | { 85 | std::ofstream outfile(filename, std::ios::binary); 86 | if (!outfile) 87 | { 88 | throw std::runtime_error("Failed to open file " + filename); 89 | } 90 | auto outfile_compressed = serialization_utils::gzip_ostream(outfile); 91 | cereal::BinaryOutputArchive archive(*outfile_compressed); 92 | 93 | archive(FORMAT_VERSION, data); 94 | } 95 | }; 96 | 97 | } // namespace robot_interfaces 98 | -------------------------------------------------------------------------------- /include/robot_interfaces/sensors/pybind_sensors.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Binds methods and objects to enable access from python 4 | * @copyright 2020, New York University, Max Planck Gesellschaft. All rights 5 | * reserved. 6 | * @license BSD 3-clause 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace robot_interfaces 22 | { 23 | /** 24 | * @brief Create python bindings for different sensor types. 25 | * 26 | * @tparam The ObservationType 27 | */ 28 | template 29 | void create_sensor_bindings(pybind11::module& m) 30 | { 31 | pybind11::options options; 32 | // disable automatic function signature generation as this does not look too 33 | // nice in the Sphinx documentation. 34 | options.disable_function_signatures(); 35 | 36 | // some typedefs to keep code below shorter 37 | typedef SensorData BaseData; 38 | typedef SingleProcessSensorData SingleProcData; 39 | typedef MultiProcessSensorData MultiProcData; 40 | typedef SensorLogger Logger; 41 | typedef SensorLogReader LogReader; 42 | 43 | pybind11::class_>( 44 | m, "None", pybind11::module_local()); 45 | 46 | pybind11::class_>(m, "BaseData"); 47 | 48 | pybind11::class_, BaseData>( 49 | m, "SingleProcessData") 50 | .def(pybind11::init(), pybind11::arg("history_size") = 1000); 51 | 52 | pybind11::class_, BaseData>( 53 | m, "MultiProcessData") 54 | .def(pybind11::init(), 55 | pybind11::arg("shared_memory_id_prefix"), 56 | pybind11::arg("is_master"), 57 | pybind11::arg("history_size") = 1000); 58 | 59 | pybind11::class_, 60 | std::shared_ptr>>( 61 | m, "Driver"); 62 | 63 | pybind11::class_>(m, "Backend") 64 | .def(pybind11::init< 65 | typename std::shared_ptr>, 66 | typename std::shared_ptr>()) 67 | .def("shutdown", 68 | &SensorBackend::shutdown, 69 | pybind11::call_guard()); 70 | 71 | pybind11::class_>(m, "Frontend") 72 | .def(pybind11::init>()) 73 | .def("get_sensor_info", 74 | &SensorFrontend::get_sensor_info, 75 | pybind11::call_guard()) 76 | .def("get_latest_observation", 77 | &SensorFrontend::get_latest_observation, 78 | pybind11::call_guard()) 79 | .def("get_observation", 80 | &SensorFrontend::get_observation, 81 | pybind11::call_guard()) 82 | .def("get_timestamp_ms", 83 | &SensorFrontend::get_timestamp_ms, 84 | pybind11::call_guard()) 85 | .def("get_current_timeindex", 86 | &SensorFrontend::get_current_timeindex, 87 | pybind11::call_guard()); 88 | 89 | pybind11::class_>(m, "Logger") 90 | .def(pybind11::init, size_t>()) 91 | .def("start", 92 | &Logger::start, 93 | pybind11::call_guard()) 94 | .def("stop", 95 | &Logger::stop, 96 | pybind11::call_guard()) 97 | .def("is_running", 98 | &Logger::is_running, 99 | pybind11::call_guard()) 100 | .def("reset", 101 | &Logger::reset, 102 | pybind11::call_guard()) 103 | .def("stop_and_save", 104 | &Logger::stop_and_save, 105 | pybind11::call_guard()); 106 | 107 | pybind11::class_>(m, 108 | "LogReader", 109 | R"XXX( 110 | LogReader(filename: str) 111 | 112 | See :meth:`read_file` 113 | )XXX") 114 | .def(pybind11::init()) 115 | .def("read_file", 116 | &LogReader::read_file, 117 | pybind11::call_guard(), 118 | pybind11::arg("filename"), 119 | R"XXX( 120 | read_file(filename: str) 121 | 122 | Read data from the specified camera log file. 123 | 124 | The data is stored in :attr:`data` and :attr:`timestamps`. 125 | 126 | Args: 127 | filename (str): Path to the camera log file. 128 | )XXX") 129 | .def_readonly("data", 130 | &LogReader::data, 131 | pybind11::call_guard(), 132 | "List of camera observations from the log file.") 133 | .def_readonly("timestamps", 134 | &LogReader::timestamps, 135 | pybind11::call_guard(), 136 | "List of timestamps of the camera observations."); 137 | } 138 | 139 | } // namespace robot_interfaces 140 | -------------------------------------------------------------------------------- /include/robot_interfaces/sensors/sensor_backend.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Connects the driver with sensor data 4 | * @copyright 2020, New York University, Max Planck Gesellschaft. All rights 5 | * reserved. 6 | * @license BSD 3-clause 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | namespace robot_interfaces 21 | { 22 | /** 23 | * @brief Communication link between SensorData and SensorDriver. 24 | * 25 | * At each instant, it checks if the sensor can be accessed, and 26 | * then gets the observation from it (the observation type depends 27 | * on the sensor) and appends it to the sensor data. 28 | * 29 | * @tparam ObservationType 30 | */ 31 | template 32 | class SensorBackend 33 | { 34 | public: 35 | typedef std::shared_ptr> Ptr; 36 | typedef std::shared_ptr> 37 | ConstPtr; 38 | 39 | /** 40 | * @param sensor_driver Driver instance for the sensor. 41 | * @param sensor_data Data is sent to/retrieved from here. 42 | */ 43 | SensorBackend( 44 | std::shared_ptr> sensor_driver, 45 | std::shared_ptr> sensor_data) 46 | : sensor_driver_(sensor_driver), 47 | sensor_data_(sensor_data), 48 | shutdown_requested_(false) 49 | { 50 | // populate the sensor information field 51 | InfoType info = sensor_driver_->get_sensor_info(); 52 | sensor_data_->sensor_info->append(info); 53 | 54 | thread_ = 55 | std::thread(&SensorBackend::loop, this); 56 | } 57 | 58 | // reinstate the implicit move constructor 59 | // See https://stackoverflow.com/a/27474070 60 | SensorBackend(SensorBackend &&) = default; 61 | 62 | //! @brief Stop the backend thread. 63 | void shutdown() 64 | { 65 | shutdown_requested_ = true; 66 | if (thread_.joinable()) 67 | { 68 | thread_.join(); 69 | } 70 | } 71 | 72 | virtual ~SensorBackend() 73 | { 74 | shutdown(); 75 | } 76 | 77 | private: 78 | std::shared_ptr> sensor_driver_; 79 | std::shared_ptr> sensor_data_; 80 | 81 | bool shutdown_requested_; 82 | 83 | std::thread thread_; 84 | 85 | /** 86 | * @brief Main loop. 87 | */ 88 | void loop() 89 | { 90 | for (long int t = 0; !shutdown_requested_; t++) 91 | { 92 | ObservationType sensor_observation; 93 | try 94 | { 95 | sensor_observation = sensor_driver_->get_observation(); 96 | } 97 | catch (const std::runtime_error &e) 98 | { 99 | std::cerr << e.what() << std::endl; 100 | } 101 | sensor_data_->observation->append(sensor_observation); 102 | } 103 | } 104 | }; 105 | 106 | } // namespace robot_interfaces 107 | -------------------------------------------------------------------------------- /include/robot_interfaces/sensors/sensor_data.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief To store all the data from all the sensors in use 4 | * @copyright 2020, New York University, Max Planck Gesellschaft. All rights 5 | * reserved. 6 | * @license BSD 3-clause 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | namespace robot_interfaces 21 | { 22 | /** 23 | * @brief Contains the data coming from the sensors. 24 | * 25 | * @tparam Observation Type of the sensor observation. 26 | */ 27 | template 28 | class SensorData 29 | { 30 | public: 31 | typedef std::shared_ptr> Ptr; 32 | typedef std::shared_ptr> ConstPtr; 33 | 34 | /** 35 | * @brief Static information about the sensor 36 | * 37 | * Note: A time series is used here for convenience to handle the shared 38 | * memory aspect. However, this is intended to only hold one element that 39 | * doesn't change over time. 40 | */ 41 | std::shared_ptr> sensor_info; 42 | 43 | //! @brief Time series of the sensor observations. 44 | std::shared_ptr> observation; 45 | 46 | protected: 47 | // make constructor protected to prevent instantiation of the base class 48 | SensorData(){}; 49 | }; 50 | 51 | /** 52 | * @brief SensorData instance using single process time series. 53 | * 54 | * Use this class if all modules accessing the data are running in the same 55 | * process. If modules run in separate processes, use MultiProcessSensorData 56 | * instead. 57 | * 58 | * @copydoc SensorData 59 | * @see MultiProcessSensorData 60 | */ 61 | template 62 | class SingleProcessSensorData : public SensorData 63 | { 64 | public: 65 | SingleProcessSensorData(size_t history_length = 1000) 66 | { 67 | // sensor_info only contains a single static element, so length is set 68 | // to 1 69 | this->sensor_info = std::make_shared>(1); 70 | this->observation = 71 | std::make_shared>( 72 | history_length); 73 | } 74 | }; 75 | 76 | /** 77 | * @brief SensorData instance using multi process time series. 78 | * 79 | * Use this class if modules accessing the data are running in separate 80 | * processes. When all modules run as threads in the same process, this class 81 | * can be used as well, however, SingleProcessSensorData might be more efficient 82 | * in that case. 83 | * 84 | * @copydoc SensorData 85 | * @see SingleProcessSensorData 86 | */ 87 | template 88 | class MultiProcessSensorData : public SensorData 89 | { 90 | public: 91 | MultiProcessSensorData(const std::string &shared_memory_id, 92 | bool is_master, 93 | size_t history_length = 1000) 94 | { 95 | // each time series needs its own shared memory ID, so add unique 96 | // suffixes to the given ID. 97 | const std::string shm_id_info = shared_memory_id + "_info"; 98 | const std::string shm_id_observation = 99 | shared_memory_id + "_observation"; 100 | 101 | if (is_master) 102 | { 103 | // the master instance is in charge of cleaning the memory 104 | time_series::clear_memory(shm_id_info); 105 | time_series::clear_memory(shm_id_observation); 106 | 107 | // sensor_info only contains a single static element, so length is 108 | // set to 1 109 | this->sensor_info = 110 | time_series::MultiprocessTimeSeries::create_leader_ptr( 111 | shm_id_info, 1); 112 | 113 | this->observation = time_series::MultiprocessTimeSeries< 114 | Observation>::create_leader_ptr(shm_id_observation, 115 | history_length); 116 | } 117 | else 118 | { 119 | this->sensor_info = 120 | time_series::MultiprocessTimeSeries::create_follower_ptr( 121 | shm_id_info); 122 | 123 | this->observation = time_series::MultiprocessTimeSeries< 124 | Observation>::create_follower_ptr(shm_id_observation); 125 | } 126 | } 127 | }; 128 | 129 | } // namespace robot_interfaces 130 | -------------------------------------------------------------------------------- /include/robot_interfaces/sensors/sensor_driver.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Base driver for the sensors. 4 | * @copyright 2020, New York University, Max Planck Gesellschaft. All rights 5 | * reserved. 6 | * @license BSD 3-clause 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | #include 14 | 15 | namespace robot_interfaces 16 | { 17 | /** 18 | * @brief Base driver class from which all specific sensor 19 | * drivers should derive. 20 | * 21 | * @tparam ObservationType 22 | */ 23 | template 24 | class SensorDriver 25 | { 26 | public: 27 | // virtual destructor is needed for class with virtual methods 28 | virtual ~SensorDriver() 29 | { 30 | } 31 | 32 | /** 33 | * @brief Return static information about the sensor. 34 | * 35 | * This information is expected to be constructed during initialization and 36 | * to not change later on. 37 | */ 38 | virtual InfoType get_sensor_info() 39 | { 40 | return InfoType(); 41 | } 42 | 43 | /** 44 | * @brief return the observation 45 | * @return depends on the observation structure 46 | * of the sensor being interacted with 47 | */ 48 | virtual ObservationType get_observation() = 0; 49 | }; 50 | } // namespace robot_interfaces 51 | -------------------------------------------------------------------------------- /include/robot_interfaces/sensors/sensor_frontend.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Consists of methods that are exposed to the user to interact 4 | * with the sensors. 5 | * @copyright 2020, New York University, Max Planck Gesellschaft. All rights 6 | * reserved. 7 | * @license BSD 3-clause 8 | */ 9 | 10 | #pragma once 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | namespace robot_interfaces 21 | { 22 | /** 23 | * @brief Communication link between SensorData and the user. 24 | * 25 | * Exposes the sensor data to the user to enable the user to get 26 | * observations, timestamps, and timeindices from the timeseries. 27 | * 28 | * @tparam ObservationType 29 | */ 30 | template 31 | class SensorFrontend 32 | { 33 | public: 34 | template 35 | using Timeseries = time_series::TimeSeries; 36 | 37 | typedef std::shared_ptr> Ptr; 38 | typedef std::shared_ptr> 39 | ConstPtr; 40 | typedef time_series::Timestamp TimeStamp; 41 | typedef time_series::Index TimeIndex; 42 | 43 | SensorFrontend( 44 | std::shared_ptr> sensor_data) 45 | : sensor_data_(sensor_data) 46 | { 47 | } 48 | 49 | InfoType get_sensor_info() const 50 | { 51 | return sensor_data_->sensor_info->newest_element(); 52 | } 53 | 54 | ObservationType get_observation(const TimeIndex t) const 55 | { 56 | return (*sensor_data_->observation)[t]; 57 | } 58 | 59 | ObservationType get_latest_observation() const 60 | { 61 | return sensor_data_->observation->newest_element(); 62 | } 63 | 64 | TimeStamp get_timestamp_ms(const TimeIndex t) const 65 | { 66 | return sensor_data_->observation->timestamp_ms(t); 67 | } 68 | TimeIndex get_current_timeindex() const 69 | { 70 | return sensor_data_->observation->newest_timeindex(); 71 | } 72 | 73 | private: 74 | std::shared_ptr> sensor_data_; 75 | }; 76 | 77 | } // namespace robot_interfaces 78 | -------------------------------------------------------------------------------- /include/robot_interfaces/sensors/sensor_log_reader.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief API to read the data from a sensor log file. 4 | * @copyright 2020, Max Planck Gesellschaft. All rights reserved. 5 | * @license BSD 3-clause 6 | */ 7 | #pragma once 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | namespace robot_interfaces 18 | { 19 | /** 20 | * @brief Read the data from a sensor log file. 21 | * 22 | * The data is read from the specified file and stored to the `data` member 23 | * where it can be accessed. 24 | * 25 | * @tparam Observation Type of the sensor observation. 26 | */ 27 | template 28 | class SensorLogReader 29 | { 30 | public: 31 | typedef typename std::tuple StampedObservation; 32 | 33 | //! @brief Observations from the log file. 34 | //! @todo rename to "observations" 35 | std::vector data; 36 | 37 | //! @brief Timestamps of the time series from which the observations were 38 | //! logged. 39 | std::vector timestamps; 40 | 41 | //! @copydoc SensorLogReader::read_file() 42 | SensorLogReader(const std::string &filename) 43 | { 44 | read_file(filename); 45 | } 46 | 47 | /** 48 | * @brief Read data from the specified file. 49 | * 50 | * The data is stored to SensorLogReader::data. 51 | * 52 | * @param filename Path to the sensor log file. 53 | */ 54 | void read_file(const std::string &filename) 55 | { 56 | std::ifstream infile(filename, std::ios::binary); 57 | 58 | if (!infile) 59 | { 60 | throw std::runtime_error("Failed to open file " + filename); 61 | } 62 | 63 | auto infile_compressed = serialization_utils::gzip_istream(infile); 64 | cereal::BinaryInputArchive archive(*infile_compressed); 65 | 66 | std::uint32_t format_version; 67 | archive(format_version); 68 | 69 | if (format_version != 1) 70 | { 71 | throw std::runtime_error("Incompatible log file format."); 72 | } 73 | 74 | std::vector stamped_data; 75 | archive(stamped_data); 76 | 77 | data.reserve(stamped_data.size()); 78 | timestamps.reserve(stamped_data.size()); 79 | for (auto [timestamp, observation] : stamped_data) 80 | { 81 | data.push_back(observation); 82 | timestamps.push_back(timestamp); 83 | } 84 | } 85 | }; 86 | 87 | } // namespace robot_interfaces 88 | -------------------------------------------------------------------------------- /include/robot_interfaces/sensors/sensor_logger.hpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // BSD 3-Clause License 3 | // 4 | // Copyright (C) 2020, Max Planck Gesellschaft 5 | // Copyright note valid unless otherwise stated in individual files. 6 | // All rights reserved. 7 | /////////////////////////////////////////////////////////////////////////////// 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | #include "sensor_data.hpp" 24 | 25 | namespace robot_interfaces 26 | { 27 | /** 28 | * @brief Record sensor observations and store them to a file. 29 | * 30 | * Fetches observations from the given SensorData and buffers them in memory. 31 | * Buffered observations can be written to a file. For writing to file cereal 32 | * is used, so the Observation type has to be serializable by cereal. 33 | * 34 | * Usage Example: 35 | * 36 | * @code 37 | * auto logger = SensorLogger(sensor_data, BUFFER_LIMIT); 38 | * logger.start(); 39 | * // do something 40 | * logger.stop_and_save("/tmp/sensordata.log"); 41 | * @endcode 42 | * 43 | * 44 | * @tparam Observation Typ of the observation that is recorded. 45 | */ 46 | template 47 | class SensorLogger 48 | { 49 | public: 50 | typedef std::shared_ptr> DataPtr; 51 | typedef typename std::tuple StampedObservation; 52 | 53 | /** 54 | * @brief Initialize the logger. 55 | * 56 | * @param sensor_data Pointer to the SensorData instance from which 57 | * observations are obtained. 58 | * @param buffer_limit Maximum number of observations that are logged. 59 | * When this limit is reached, the logger will stop automatically, that 60 | * is new observations are not logged anymore. 61 | */ 62 | SensorLogger(DataPtr sensor_data, size_t buffer_limit) 63 | : sensor_data_(sensor_data), 64 | buffer_limit_(buffer_limit), 65 | enabled_(false) 66 | { 67 | // directly reserve the memory for the full buffer so it does not need 68 | // to move data around during run time 69 | buffer_.reserve(buffer_limit); 70 | } 71 | 72 | // reinstate the implicit move constructor 73 | // See https://stackoverflow.com/a/27474070 74 | SensorLogger(SensorLogger &&) = default; 75 | 76 | ~SensorLogger() 77 | { 78 | stop(); 79 | } 80 | 81 | /** 82 | * @brief Start logging. 83 | * 84 | * If the logger is already running, this is a noop. 85 | */ 86 | void start() 87 | { 88 | if (!enabled_) 89 | { 90 | enabled_ = true; 91 | buffer_thread_ = 92 | std::thread(&SensorLogger::loop, this); 93 | } 94 | } 95 | 96 | /** 97 | * @brief Stop logging. 98 | * 99 | * If the logger is already stopped, this is a noop. 100 | */ 101 | void stop() 102 | { 103 | enabled_ = false; 104 | if (buffer_thread_.joinable()) 105 | { 106 | buffer_thread_.join(); 107 | } 108 | } 109 | 110 | /** 111 | * @brief Check if the logger is currently running. 112 | * 113 | * After calling @ref start(), the logger is running until @ref stop() is 114 | * called or the buffer is full (in which case it will stop automatically). 115 | */ 116 | bool is_running() const 117 | { 118 | return enabled_; 119 | } 120 | 121 | //! @brief Clear the log buffer. 122 | void reset() 123 | { 124 | buffer_.clear(); 125 | } 126 | 127 | /** 128 | * @brief Stop logging and save logged messages to a file. 129 | * 130 | * @param filename Path to the output file. Existing files will be 131 | * overwritten. 132 | */ 133 | void stop_and_save(const std::string &filename) 134 | { 135 | stop(); 136 | 137 | std::ofstream outfile(filename, std::ios::binary); 138 | 139 | auto outfile_compressed = serialization_utils::gzip_ostream(outfile); 140 | cereal::BinaryOutputArchive archive(*outfile_compressed); 141 | 142 | // add version information to the output file (this can be used while 143 | // loading when the data format changes 144 | const std::uint32_t format_version = 1; 145 | 146 | archive(format_version, buffer_); 147 | } 148 | 149 | private: 150 | DataPtr sensor_data_; 151 | std::vector buffer_; 152 | size_t buffer_limit_; 153 | std::thread buffer_thread_; 154 | bool enabled_; 155 | 156 | //! Get observations from sensor_data_ and add them to the buffer. 157 | void loop() 158 | { 159 | // Get the oldest available timeindex as starting point of the log. In 160 | // case there is no data yet (t == EMPTY), start with t = 0. 161 | time_series::Index t = 162 | sensor_data_->observation->oldest_timeindex(false); 163 | if (t == time_series::EMPTY) 164 | { 165 | t = 0; 166 | } 167 | 168 | while (enabled_) 169 | { 170 | // wait for the next time step but check if the logger was stopped 171 | // from time to time 172 | constexpr double wait_timeout_s = 0.2; 173 | while (!sensor_data_->observation->wait_for_timeindex( 174 | t, wait_timeout_s)) 175 | { 176 | if (!enabled_) 177 | { 178 | return; 179 | } 180 | } 181 | 182 | try 183 | { 184 | // FIXME: this will block if the sensor has stopped 185 | auto timestamp = sensor_data_->observation->timestamp_ms(t); 186 | auto observation = (*sensor_data_->observation)[t]; 187 | buffer_.push_back(std::make_tuple(timestamp, observation)); 188 | } 189 | catch (const std::invalid_argument &e) 190 | { 191 | std::cerr << "ERROR: " << e.what() << "\nSkip observation." 192 | << std::endl; 193 | } 194 | t++; 195 | 196 | // Stop logging if buffer limit is reached 197 | if (buffer_.size() >= buffer_limit_) 198 | { 199 | std::cerr << "WARNING: SensorLogger buffer limit is reached. " 200 | "Stop logging." 201 | << std::endl; 202 | enabled_ = false; 203 | } 204 | } 205 | } 206 | }; 207 | 208 | } // namespace robot_interfaces 209 | -------------------------------------------------------------------------------- /include/robot_interfaces/status.hpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // BSD 3-Clause License 3 | // 4 | // Copyright (C) 2018-2019, Max Planck Gesellschaft 5 | // Copyright note valid unless otherwise stated in individual files. 6 | // All rights reserved. 7 | /////////////////////////////////////////////////////////////////////////////// 8 | 9 | /** 10 | * @file 11 | * @brief Defines the Status struct. 12 | */ 13 | 14 | #pragma once 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace robot_interfaces 22 | { 23 | /** 24 | * @brief Status information from the backend. 25 | * 26 | * Used to report status information that is not directly robot-related from the 27 | * backend to the frontend. 28 | */ 29 | struct Status : public Loggable 30 | { 31 | //! Maximum length of error messages (including terminating \0) 32 | static constexpr unsigned int ERROR_MESSAGE_LENGTH = 64; 33 | 34 | //! @brief Different types of errors that can occur in the backend. 35 | enum class ErrorStatus 36 | { 37 | //! @brief Indicates that there is no error. 38 | NO_ERROR = 0, 39 | 40 | /** 41 | * @brief Error reported from the @ref RobotDriver. 42 | * 43 | * An error reported by the low level robot driver (see @ref 44 | * RobotDriver). This is depending on the driver implementation. It 45 | * can, for example, be used to report some hardware failure). 46 | */ 47 | DRIVER_ERROR, 48 | 49 | /** 50 | * @brief Error from the @ref RobotBackend. 51 | * 52 | * An error which is issued by the back end itself, for example if no 53 | * new action is provided and the allowed number of repetitions is 54 | * exceeded. 55 | */ 56 | BACKEND_ERROR 57 | }; 58 | 59 | /** 60 | * @brief Number of times the current action has been repeated. 61 | * 62 | * If the back end wants to apply the next action but no new action was 63 | * provided by the user in time, it may (depending on configuration) repeat 64 | * the previous action. Each time this happens, `action_repetitions` is 65 | * increased by one. Once a new action is provided, it will be reset to 66 | * zero. 67 | * 68 | * See also @ref next-action-not-in-time. 69 | */ 70 | uint32_t action_repetitions = 0; 71 | 72 | /** 73 | * @brief Indicates if there is an error and, if yes, in which component. 74 | * 75 | * @note If there is an error reported in the status, the robot is not in an 76 | * operational state anymore. Trying to append another action in the 77 | * @ref RobotFrontend will result in an exception in this case. 78 | * 79 | * @see error_message for more information on the error. 80 | * @see has_error() 81 | */ 82 | ErrorStatus error_status = ErrorStatus::NO_ERROR; 83 | 84 | /** 85 | * @brief Set error. 86 | * 87 | * If another error was set before, the old one is kept and the new one 88 | * ignored. 89 | * 90 | * @param error_type The type of the error. 91 | * @param message Error message. Will be shortened if it exceeds @ref 92 | * ERROR_MESSAGE_LENGTH. 93 | */ 94 | void set_error(ErrorStatus error_type, const std::string& message) 95 | { 96 | // do not overwrite existing errors 97 | if (!has_error()) 98 | { 99 | this->error_status = error_type; 100 | 101 | std::strncpy( 102 | this->error_message, message.c_str(), ERROR_MESSAGE_LENGTH - 1); 103 | // in case message is too long and needs to be cut, indicate this by 104 | // setting ~ as last character 105 | if (message.size() > ERROR_MESSAGE_LENGTH - 1) 106 | { 107 | this->error_message[ERROR_MESSAGE_LENGTH - 2] = '~'; 108 | } 109 | // make sure it is terminated 110 | this->error_message[ERROR_MESSAGE_LENGTH - 1] = '\0'; 111 | } 112 | } 113 | 114 | /** 115 | * @brief Check if an error is set. 116 | * 117 | * @note If there is an error reported in the status, the robot is not in an 118 | * operational state anymore. Trying to append another action in the 119 | * @ref RobotFrontend will result in an exception in this case. 120 | * 121 | * See @ref error_status and @ref error_message for more details on the 122 | * error. 123 | */ 124 | bool has_error() const 125 | { 126 | return this->error_status != Status::ErrorStatus::NO_ERROR; 127 | } 128 | 129 | //! Get the error message as std::string. 130 | std::string get_error_message() const 131 | { 132 | return std::string(this->error_message); 133 | } 134 | 135 | template 136 | void serialize(Archive& archive) 137 | { 138 | archive(action_repetitions, error_status, error_message); 139 | } 140 | 141 | std::vector get_name() override 142 | { 143 | return {"action_repetitions", "error_status"}; 144 | } 145 | 146 | std::vector> get_data() override 147 | { 148 | // FIXME error message cannot be logged because only numeric types are 149 | // supported 150 | return {{static_cast(action_repetitions)}, 151 | {static_cast(error_status)}}; 152 | } 153 | 154 | private: 155 | /** 156 | * @brief Human-readable message describing the error. 157 | * 158 | * Value is undefined if `error_status == NO_ERROR`. 159 | */ 160 | char error_message[ERROR_MESSAGE_LENGTH] = ""; 161 | }; 162 | 163 | } // namespace robot_interfaces 164 | -------------------------------------------------------------------------------- /include/robot_interfaces/types.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Observation of a Finger robot. 4 | * @copyright 2020, Max Planck Gesellschaft. All rights reserved. 5 | * @license BSD 3-clause 6 | */ 7 | #pragma once 8 | 9 | #include 10 | 11 | #include "robot_backend.hpp" 12 | #include "robot_data.hpp" 13 | #include "robot_frontend.hpp" 14 | #include "robot_log_entry.hpp" 15 | #include "robot_log_reader.hpp" 16 | #include "robot_logger.hpp" 17 | 18 | namespace robot_interfaces 19 | { 20 | template 21 | struct RobotInterfaceTypes 22 | { 23 | typedef Action_t Action; 24 | typedef Observation_t Observation; 25 | 26 | typedef RobotBackend Backend; 27 | typedef std::shared_ptr BackendPtr; 28 | 29 | typedef RobotData BaseData; 30 | typedef std::shared_ptr BaseDataPtr; 31 | typedef SingleProcessRobotData SingleProcessData; 32 | typedef std::shared_ptr SingleProcessDataPtr; 33 | typedef MultiProcessRobotData MultiProcessData; 34 | typedef std::shared_ptr MultiProcessDataPtr; 35 | 36 | typedef RobotFrontend Frontend; 37 | typedef std::shared_ptr FrontendPtr; 38 | 39 | typedef RobotLogEntry LogEntry; 40 | typedef RobotLogger Logger; 41 | typedef RobotBinaryLogReader BinaryLogReader; 42 | }; 43 | 44 | } // namespace robot_interfaces 45 | -------------------------------------------------------------------------------- /include/robot_interfaces/utils.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @copyright 2020, New York University, Max Planck Gesellschaft. All rights 4 | * reserved. 5 | * @license BSD-3-clause 6 | */ 7 | #pragma once 8 | 9 | #include 10 | 11 | namespace robot_interfaces 12 | { 13 | //! @brief Empty struct that can be used as placeholder. 14 | struct None 15 | { 16 | template 17 | void serialize(Archive& archive) 18 | { 19 | // need to serialize some dummy value here, as an actual empty type will 20 | // cause trouble for our shared_memory implementation 21 | uint8_t dummy = 0; 22 | archive(dummy); 23 | } 24 | }; 25 | } // namespace robot_interfaces 26 | -------------------------------------------------------------------------------- /license.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, New York University and Max Planck Gesellschaft. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | robot_interfaces 10 | 1.2.0 11 | 12 | 13 | This package provides C++ interfaces (purely virtual classes) for 14 | different robots. The idea is to use this interface both for the real 15 | robots as well as the simulators. 16 | 17 | 18 | Manuel Wuthrich 19 | Manuel Wuthrich 20 | Felix Widmaier 21 | 22 | BSD 3-clause 23 | 24 | 25 | ament_cmake 26 | ament_cmake_python 27 | pybind11_vendor 28 | mpi_cmake_modules 29 | 30 | 31 | pybind11 32 | Eigen3 33 | real_time_tools 34 | time_series 35 | signal_handler 36 | serialization_utils 37 | Threads 38 | rt 39 | 40 | 41 | ament_cmake_gtest 42 | ament_cmake_pytest 43 | Boost 44 | 45 | 46 | ament_cmake 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /python/robot_interfaces/__init__.py: -------------------------------------------------------------------------------- 1 | # use "noqa" comments to silence flake8 warnings 2 | # F401 = unused import, F403 = complaint about `import *`. 3 | from robot_interfaces.py_generic import * # noqa: F401,F403 4 | import robot_interfaces.py_finger_types as finger # noqa: F401 5 | import robot_interfaces.py_trifinger_types as trifinger # noqa: F401 6 | import robot_interfaces.py_one_joint_types as one_joint # noqa: F401 7 | import robot_interfaces.py_two_joint_types as two_joint # noqa: F401 8 | import robot_interfaces.py_solo_eight_types as solo_eight # noqa: F401 9 | -------------------------------------------------------------------------------- /scripts/plot_trifinger_log.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """Plot selected data fields of a TriFinger robot log. 3 | 4 | Specify one or more fields that are to be plotted. The names correspond to the 5 | data entries of the ``RobotLogEntry`` class. For fields that contain vectors 6 | (e.g. joint positions), add a index to the name. Example: 7 | 8 | status.action_repetitions 9 | observation.position[0] <- plot observed positions of joint 0 10 | """ 11 | import argparse 12 | import re 13 | import sys 14 | import typing 15 | 16 | import matplotlib.pyplot as plt 17 | 18 | import robot_interfaces 19 | 20 | 21 | def get_data( 22 | log: robot_interfaces.trifinger.BinaryLogReader, field_name: str 23 | ) -> typing.List[typing.Any]: 24 | """Extract data of the given field from the robot log. 25 | 26 | Args: 27 | log: Robot log. 28 | field_name: Name of the data field. Corresponds to the name of the 29 | attribute in the ``RobotLogEntry``, e.g. 30 | "status.action_repetitions" or "observation.position[0]" ([0] 31 | specifies to take the values of joint 0). 32 | 33 | Returns: 34 | The extracted data. 35 | """ 36 | # extract index if one is given 37 | index = None 38 | m = re.match(r"(.*)\[(\d+)\]$", field_name) 39 | if m: 40 | field_name = m.group(1) 41 | index = int(m.group(2)) 42 | 43 | data = log.data 44 | for field_component in field_name.split("."): 45 | data = [getattr(entry, field_component) for entry in data] 46 | 47 | if index is not None: 48 | data = [entry[index] for entry in data] 49 | 50 | return data 51 | 52 | 53 | def main(): 54 | parser = argparse.ArgumentParser( 55 | description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter 56 | ) 57 | parser.add_argument("filename", type=str, help="The trifinger robot log file.") 58 | parser.add_argument( 59 | "fields", 60 | type=str, 61 | nargs="+", 62 | help="One or more data fields that are added to the plot.", 63 | ) 64 | parser.add_argument( 65 | "--base", 66 | type=str, 67 | default="timeindex", 68 | help="Data field that is used for the x-axis. Default: %(default)s", 69 | ) 70 | args = parser.parse_args() 71 | 72 | log = robot_interfaces.trifinger.BinaryLogReader(args.filename) 73 | 74 | base_data = get_data(log, args.base) 75 | for field in args.fields: 76 | field_data = get_data(log, field) 77 | plt.plot(base_data, field_data, label=field) 78 | 79 | plt.legend() 80 | plt.xlabel(args.base) 81 | plt.show() 82 | 83 | return 0 84 | 85 | 86 | if __name__ == "__main__": 87 | sys.exit(main()) 88 | -------------------------------------------------------------------------------- /src/convert_old_robot_log.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Convert old TriFinger robot logs from before the fix in 4 | * Status::error_message. 5 | * 6 | * In the past Status::error_message was a variable-sized std::string. Since 7 | * this is incompatible with the fixed-size requirement for shared memory time 8 | * series, it was changed to a fixed-size char-array in commit 4ca02a17. 9 | * Unfortunately, this makes old logfiles incompatible with the RobotLogReader 10 | * using the fixed type. 11 | * 12 | * This file provides a utility to load log files of the old format and convert 13 | * them to the new format, so that they can be processed with the latest version 14 | * of the code. 15 | * 16 | * @copyright Copyright (c) 2021, Max Planck Gesellschaft. 17 | * @license BSD 3-clause 18 | */ 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | using namespace robot_interfaces; 30 | 31 | //! Old version of the Status message (taken from commit 4ca02a17^, stripped 32 | //! down to the relevant parts). 33 | struct OldStatus 34 | { 35 | uint32_t action_repetitions = 0; 36 | Status::ErrorStatus error_status = Status::ErrorStatus::NO_ERROR; 37 | std::string error_message; 38 | 39 | template 40 | void serialize(Archive& archive) 41 | { 42 | archive(action_repetitions, error_status, error_message); 43 | } 44 | }; 45 | 46 | typedef RobotBinaryLogReader 49 | OldLogReader; 50 | 51 | int main(int argc, char* argv[]) 52 | { 53 | if (argc != 3) 54 | { 55 | std::cerr << "Error: Invalid number of arguments." << std::endl; 56 | std::cerr << "Usage: " << argv[0] << " " 57 | << std::endl; 58 | return 1; 59 | } 60 | 61 | std::string old_logfile(argv[1]); 62 | std::string new_logfile(argv[2]); 63 | 64 | if (!std::filesystem::is_regular_file(old_logfile)) 65 | { 66 | std::cout << "Error: Input " << old_logfile << " is not a regular file." 67 | << std::endl; 68 | return 2; 69 | } 70 | if (std::filesystem::exists(new_logfile)) 71 | { 72 | std::cout << "Error: Output destination " << new_logfile 73 | << " already exists." << std::endl; 74 | return 3; 75 | } 76 | 77 | OldLogReader old_log(old_logfile); 78 | TriFingerTypes::BinaryLogReader new_log; 79 | 80 | // copy data 81 | for (OldLogReader::LogEntry old_entry : old_log.data) 82 | { 83 | TriFingerTypes::BinaryLogReader::LogEntry new_entry; 84 | 85 | // copy all fields that are unchanged 86 | new_entry.timeindex = old_entry.timeindex; 87 | new_entry.timestamp = old_entry.timestamp; 88 | new_entry.observation = old_entry.observation; 89 | new_entry.desired_action = old_entry.desired_action; 90 | new_entry.applied_action = old_entry.applied_action; 91 | 92 | // copy status 93 | new_entry.status.action_repetitions = 94 | old_entry.status.action_repetitions; 95 | new_entry.status.set_error(old_entry.status.error_status, 96 | old_entry.status.error_message); 97 | 98 | new_log.data.push_back(new_entry); 99 | } 100 | 101 | new_log.write_file(new_logfile); 102 | } 103 | -------------------------------------------------------------------------------- /srcpy/py_finger_types.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright [2017] Max Planck Society. All rights reserved. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | /** 19 | * \file 20 | * \brief Create bindings for One-Joint robot types 21 | */ 22 | #include 23 | #include 24 | 25 | using namespace robot_interfaces; 26 | 27 | PYBIND11_MODULE(py_finger_types, m) 28 | { 29 | create_blmc_can_robot_python_bindings(m); 30 | } 31 | -------------------------------------------------------------------------------- /srcpy/py_generic.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Create Python bindings for generic types 4 | * @copyright 2019, Max Planck Gesellschaft. All rights reserved. 5 | * @license BSD 3-clause 6 | */ 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | using namespace robot_interfaces; 14 | 15 | PYBIND11_MODULE(py_generic, m) 16 | { 17 | pybind11::class_ pystatus(m, "Status"); 18 | pystatus.def(pybind11::init<>()) 19 | .def_readwrite( 20 | "action_repetitions", 21 | &Status::action_repetitions, 22 | "int: Number of times the current action has been repeated.") 23 | .def_readonly("error_status", 24 | &Status::error_status, 25 | "ErrorStatus: Current error status.") 26 | .def("set_error", &Status::set_error) 27 | .def("get_error_message", &Status::get_error_message) 28 | .def(pybind11::pickle( 29 | [](const Status &status) { // __getstate__ 30 | return pybind11::make_tuple(status.action_repetitions, 31 | status.error_status, 32 | status.get_error_message()); 33 | }, 34 | [](pybind11::tuple t) { // __setstate__ 35 | if (t.size() != 3) 36 | { 37 | throw std::runtime_error("Invalid state!"); 38 | } 39 | 40 | Status status; 41 | status.action_repetitions = t[0].cast(); 42 | status.set_error(t[1].cast(), 43 | t[2].cast()); 44 | 45 | return status; 46 | })); 47 | 48 | pybind11::enum_(pystatus, "ErrorStatus") 49 | .value("NO_ERROR", 50 | Status::ErrorStatus::NO_ERROR, 51 | "Indicates that there is no error.") 52 | .value("DRIVER_ERROR", 53 | Status::ErrorStatus::DRIVER_ERROR, 54 | "Error from the low level robot driver (e.g. some hardware " 55 | "failure).") 56 | .value( 57 | "BACKEND_ERROR", 58 | Status::ErrorStatus::BACKEND_ERROR, 59 | "Error from the robot back end (e.g. some communication issue)."); 60 | 61 | time_series::create_python_bindings(m, "_StatusTimeSeries"); 62 | time_series::create_multiprocesses_python_bindings( 63 | m, "_StatusMultiProcessTimeSeries"); 64 | 65 | pybind11::enum_( 66 | m, "RobotBackendTerminationReason") 67 | .value("NOT_TERMINATED", RobotBackendTerminationReason::NOT_TERMINATED) 68 | .value("SHUTDOWN_REQUESTED", 69 | RobotBackendTerminationReason::SHUTDOWN_REQUESTED) 70 | .value("MAXIMUM_NUMBER_OF_ACTIONS_REACHED", 71 | RobotBackendTerminationReason::MAXIMUM_NUMBER_OF_ACTIONS_REACHED) 72 | .value("DRIVER_ERROR", RobotBackendTerminationReason::DRIVER_ERROR) 73 | .value("FIRST_ACTION_TIMEOUT", 74 | RobotBackendTerminationReason::FIRST_ACTION_TIMEOUT) 75 | .value("NEXT_ACTION_TIMEOUT", 76 | RobotBackendTerminationReason::NEXT_ACTION_TIMEOUT); 77 | } 78 | -------------------------------------------------------------------------------- /srcpy/py_one_joint_types.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright [2017] Max Planck Society. All rights reserved. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | /** 19 | * \file 20 | * \brief Create bindings for One-Joint robot types 21 | */ 22 | #include 23 | #include 24 | 25 | using namespace robot_interfaces; 26 | 27 | PYBIND11_MODULE(py_one_joint_types, m) 28 | { 29 | create_blmc_can_robot_python_bindings>(m); 30 | } 31 | -------------------------------------------------------------------------------- /srcpy/py_solo_eight_types.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright [2017] Max Planck Society. All rights reserved. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | /** 19 | * \file 20 | * \brief Create bindings for Solo8 robot types 21 | */ 22 | #include 23 | #include 24 | 25 | using namespace robot_interfaces; 26 | 27 | PYBIND11_MODULE(py_solo_eight_types, m) 28 | { 29 | create_blmc_can_robot_python_bindings>(m); 30 | } 31 | -------------------------------------------------------------------------------- /srcpy/py_trifinger_types.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright [2017] Max Planck Society. All rights reserved. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | /** 19 | * \file 20 | * \brief Create bindings for TriFinger robot types 21 | */ 22 | #include 23 | #include 24 | 25 | using namespace robot_interfaces; 26 | 27 | PYBIND11_MODULE(py_trifinger_types, m) 28 | { 29 | create_blmc_can_robot_python_bindings(m); 30 | } 31 | -------------------------------------------------------------------------------- /srcpy/py_two_joint_types.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright [2017] Max Planck Society. All rights reserved. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | /** 19 | * \file 20 | * \brief Create bindings for Two-Joint robot types 21 | */ 22 | #include 23 | #include 24 | 25 | using namespace robot_interfaces; 26 | 27 | PYBIND11_MODULE(py_two_joint_types, m) 28 | { 29 | create_blmc_can_robot_python_bindings>(m); 30 | } 31 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Add unit tests. 3 | # 4 | 5 | find_package(ament_cmake_gtest REQUIRED) 6 | find_package(ament_cmake_pytest REQUIRED) 7 | find_package(Boost REQUIRED COMPONENTS filesystem) 8 | 9 | macro(create_unittest test_name) 10 | 11 | # create the executable 12 | ament_add_gtest(${test_name} main.cpp ${test_name}.cpp) 13 | target_link_libraries(${test_name} ${PROJECT_NAME} Boost::filesystem) 14 | 15 | endmacro(create_unittest test_name) 16 | 17 | create_unittest(test_robot_backend) 18 | create_unittest(test_robot_logger) 19 | create_unittest(test_sensor_interface) 20 | create_unittest(test_sensor_logger) 21 | 22 | # Python tests 23 | ament_add_pytest_test(test_pybind_pickle_py test_pybind_pickle.py) 24 | -------------------------------------------------------------------------------- /tests/dummy_sensor_driver.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Dummy sensor driver for testing. 4 | * @copyright 2020, Max Planck Gesellschaft. All rights reserved. 5 | * @license BSD 3-clause 6 | */ 7 | #pragma once 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | namespace robot_interfaces 15 | { 16 | namespace testing 17 | { 18 | /** 19 | * @brief Simple dummy sensor driver for testing. 20 | * 21 | * This driver uses `int` as observation type and returns as observation a 22 | * sequence of increasing numbers, starting at zero. 23 | */ 24 | class DummySensorDriver : public robot_interfaces::SensorDriver 25 | { 26 | public: 27 | int get_observation() override 28 | { 29 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 30 | return counter++; 31 | } 32 | 33 | private: 34 | int counter = 0; 35 | }; 36 | } // namespace testing 37 | } // namespace robot_interfaces 38 | -------------------------------------------------------------------------------- /tests/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file 3 | * \brief gtest main 4 | * \author Maximilien Naveau 5 | * \date 2018 6 | * 7 | * Main file that runs all unittest using gtest 8 | * @see 9 | * https://git-amd.tuebingen.mpg.de/amd-clmc/ci_example/wikis/catkin:-how-to-implement-unit-tests 10 | */ 11 | 12 | #include 13 | 14 | int main(int argc, char **argv) 15 | { 16 | ::testing::InitGoogleTest(&argc, argv); 17 | return RUN_ALL_TESTS(); 18 | } 19 | -------------------------------------------------------------------------------- /tests/test_pybind_pickle.py: -------------------------------------------------------------------------------- 1 | """Unit tests verifying that pickling of bound types works as expected.""" 2 | import pickle 3 | import numpy as np 4 | 5 | import robot_interfaces 6 | 7 | 8 | def test_pickle_finger_action(): 9 | action = robot_interfaces.finger.Action( 10 | torque=[1, 2, 3], 11 | position=[4, 5, 6], 12 | position_kp=[0.1, 0.2, 0.3], 13 | position_kd=[11, 22, 33], 14 | ) 15 | 16 | pickled = pickle.dumps(action) 17 | unpickled = pickle.loads(pickled) 18 | 19 | np.testing.assert_array_equal(action.torque, unpickled.torque) 20 | np.testing.assert_array_equal(action.position, unpickled.position) 21 | np.testing.assert_array_equal(action.position_kp, unpickled.position_kp) 22 | np.testing.assert_array_equal(action.position_kd, unpickled.position_kd) 23 | 24 | 25 | def test_pickle_trifinger_action(): 26 | action = robot_interfaces.trifinger.Action( 27 | torque=[1, 2, 3, 4, 5, 6, 7, 8, 9], 28 | position=[11, 22, 33, 44, 55, 66, 77, 88, 99], 29 | position_kp=[0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9], 30 | position_kd=[1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9], 31 | ) 32 | 33 | pickled = pickle.dumps(action) 34 | unpickled = pickle.loads(pickled) 35 | 36 | np.testing.assert_array_equal(action.torque, unpickled.torque) 37 | np.testing.assert_array_equal(action.position, unpickled.position) 38 | np.testing.assert_array_equal(action.position_kp, unpickled.position_kp) 39 | np.testing.assert_array_equal(action.position_kd, unpickled.position_kd) 40 | 41 | 42 | def test_pickle_finger_observation(): 43 | obs = robot_interfaces.finger.Observation() 44 | obs.torque = [1, 2, 3] 45 | obs.velocity = [11, 22, 33] 46 | obs.position = [111, 222, 333] 47 | obs.tip_force = [1111] 48 | 49 | pickled = pickle.dumps(obs) 50 | unpickled = pickle.loads(pickled) 51 | 52 | np.testing.assert_array_equal(obs.torque, unpickled.torque) 53 | np.testing.assert_array_equal(obs.velocity, unpickled.velocity) 54 | np.testing.assert_array_equal(obs.position, unpickled.position) 55 | np.testing.assert_array_equal(obs.tip_force, unpickled.tip_force) 56 | 57 | 58 | def test_pickle_trifinger_observation(): 59 | obs = robot_interfaces.trifinger.Observation() 60 | obs.torque = [1, 2, 3, 4, 5, 6, 7, 8, 9] 61 | obs.velocity = [11, 22, 33, 44, 55, 66, 77, 88, 99] 62 | obs.position = [111, 222, 333, 444, 555, 666, 777, 888, 999] 63 | obs.tip_force = [1111, 2222, 3333] 64 | 65 | pickled = pickle.dumps(obs) 66 | unpickled = pickle.loads(pickled) 67 | 68 | np.testing.assert_array_equal(obs.torque, unpickled.torque) 69 | np.testing.assert_array_equal(obs.velocity, unpickled.velocity) 70 | np.testing.assert_array_equal(obs.position, unpickled.position) 71 | np.testing.assert_array_equal(obs.tip_force, unpickled.tip_force) 72 | 73 | 74 | def test_pickle_one_joint_observation(): 75 | # one_joint observations do not have a "tip_force" 76 | obs = robot_interfaces.one_joint.Observation() 77 | obs.torque = [1] 78 | obs.velocity = [11] 79 | obs.position = [111] 80 | 81 | pickled = pickle.dumps(obs) 82 | unpickled = pickle.loads(pickled) 83 | 84 | np.testing.assert_array_equal(obs.torque, unpickled.torque) 85 | np.testing.assert_array_equal(obs.velocity, unpickled.velocity) 86 | np.testing.assert_array_equal(obs.position, unpickled.position) 87 | 88 | 89 | def test_pickle_status(): 90 | status = robot_interfaces.Status() 91 | status.action_repetitions = 42 92 | status.set_error(status.ErrorStatus.DRIVER_ERROR, "some message") 93 | 94 | pickled = pickle.dumps(status) 95 | unpickled = pickle.loads(pickled) 96 | 97 | assert unpickled.action_repetitions == 42 98 | assert unpickled.error_status == status.ErrorStatus.DRIVER_ERROR 99 | assert unpickled.get_error_message() == "some message" 100 | -------------------------------------------------------------------------------- /tests/test_robot_backend.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Tests for RobotBackend 4 | * @copyright Copyright (c) 2019, Max Planck Gesellschaft. 5 | */ 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace robot_interfaces; 12 | 13 | /** 14 | * @brief Fixture for the backend tests. 15 | */ 16 | class TestRobotBackend : public ::testing::Test 17 | { 18 | protected: 19 | typedef example::Action Action; 20 | typedef example::Observation Observation; 21 | typedef robot_interfaces::Status Status; 22 | typedef robot_interfaces::RobotBackend Backend; 23 | typedef robot_interfaces::SingleProcessRobotData Data; 24 | typedef robot_interfaces::RobotFrontend Frontend; 25 | 26 | std::shared_ptr driver; 27 | std::shared_ptr data; 28 | 29 | void SetUp() override 30 | { 31 | driver = std::make_shared(0, 1000); 32 | data = std::make_shared(); 33 | } 34 | }; 35 | 36 | // Test if the "max_number_of_actions" feature is working as expected 37 | TEST_F(TestRobotBackend, max_number_of_actions) 38 | { 39 | constexpr bool real_time_mode = true; 40 | constexpr double first_action_timeout = 41 | std::numeric_limits::infinity(); 42 | constexpr uint32_t max_number_of_actions = 10; 43 | 44 | Backend backend(driver, 45 | data, 46 | real_time_mode, 47 | first_action_timeout, 48 | max_number_of_actions); 49 | backend.initialize(); 50 | Frontend frontend(data); 51 | 52 | Action action; 53 | action.values[0] = 42; 54 | action.values[1] = 42; 55 | 56 | robot_interfaces::TimeIndex t; 57 | for (uint32_t i = 0; i < max_number_of_actions; i++) 58 | { 59 | t = frontend.append_desired_action(action); 60 | Status status = frontend.get_status(t); 61 | 62 | ASSERT_FALSE(status.has_error()); 63 | } 64 | 65 | auto status = frontend.get_status(t + 1); 66 | ASSERT_TRUE(status.has_error()); 67 | ASSERT_EQ(Status::ErrorStatus::BACKEND_ERROR, status.error_status); 68 | ASSERT_EQ("Maximum number of actions reached.", status.get_error_message()); 69 | } 70 | -------------------------------------------------------------------------------- /tests/test_robot_logger.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Tests for RobotLogger 4 | * @copyright Copyright (c) 2019, Max Planck Gesellschaft. 5 | */ 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | using namespace robot_interfaces; 23 | 24 | /** 25 | * @brief Fixture for the backend tests. 26 | */ 27 | class TestRobotLogger : public ::testing::Test 28 | { 29 | protected: 30 | typedef example::Action Action; 31 | typedef example::Observation Observation; 32 | typedef robot_interfaces::Status Status; 33 | typedef robot_interfaces::RobotBackend Backend; 34 | typedef robot_interfaces::SingleProcessRobotData Data; 35 | typedef robot_interfaces::RobotFrontend Frontend; 36 | typedef robot_interfaces::RobotLogger Logger; 37 | typedef robot_interfaces::RobotBinaryLogReader 38 | BinaryLogReader; 39 | 40 | static constexpr bool real_time_mode = false; 41 | static constexpr double first_action_timeout = 42 | std::numeric_limits::infinity(); 43 | static constexpr uint32_t max_number_of_actions = 10; 44 | 45 | std::shared_ptr driver; 46 | std::shared_ptr data; 47 | std::shared_ptr backend; 48 | std::shared_ptr frontend; 49 | 50 | std::filesystem::path logfile; 51 | 52 | void SetUp() override 53 | { 54 | driver = std::make_shared(0, 1000); 55 | data = std::make_shared(); 56 | 57 | backend = std::make_shared(driver, 58 | data, 59 | real_time_mode, 60 | first_action_timeout, 61 | max_number_of_actions); 62 | frontend = std::make_shared(data); 63 | 64 | auto tmp_dir = std::filesystem::temp_directory_path(); 65 | logfile = tmp_dir / "test_robot_logger.log"; 66 | } 67 | 68 | void TearDown() override 69 | { 70 | // clean up 71 | std::filesystem::remove(logfile); 72 | } 73 | 74 | /** 75 | * @brief Check whether the given CSV log is valid. 76 | * 77 | * @param filename The log file. 78 | * @param start_t Expected first time index in the log. 79 | * @param end_t Expected last time index in the log. 80 | * @param use_gzip If true, assume the file is gzip-compressed. 81 | */ 82 | void check_csv_log(const std::string &filename, 83 | uint32_t start_t, 84 | uint32_t end_t, 85 | bool use_gzip) 86 | { 87 | std::string line; 88 | 89 | // open file, potentially with gzip decompression 90 | std::ifstream infile_raw(filename); 91 | boost::iostreams::filtering_istream infile; 92 | if (use_gzip) 93 | { 94 | infile.push(boost::iostreams::gzip_decompressor()); 95 | } 96 | infile.push(infile_raw); 97 | 98 | // check header 99 | { 100 | ASSERT_TRUE(std::getline(infile, line)) 101 | << "Failed to read header line"; 102 | 103 | // remove trailing whitespaces 104 | line.erase(line.find_last_not_of(" \n") + 1); 105 | 106 | ASSERT_EQ( 107 | line, 108 | "#time_index timestamp status_action_repetitions " 109 | "status_error_status observation_values_0 observation_values_1 " 110 | "applied_action_values_0 applied_action_values_1 " 111 | "desired_action_values_0 desired_action_values_1"); 112 | } 113 | 114 | // check data 115 | for (uint32_t i = start_t; i < end_t; i++) 116 | { 117 | int time_index; 118 | double time_stamp, action_repetitions, error_status, obs_0, obs_1, 119 | applied_0, applied_1, desired_0, desired_1; 120 | 121 | ASSERT_TRUE(std::getline(infile, line)) 122 | << "Failed to read line " << i; 123 | std::istringstream iss(line); 124 | 125 | ASSERT_TRUE(iss >> time_index >> time_stamp >> action_repetitions >> 126 | error_status >> obs_0 >> obs_1 >> applied_0 >> 127 | applied_1 >> desired_0 >> desired_1); 128 | 129 | // check some of the values 130 | ASSERT_EQ(time_index, static_cast(i)); 131 | ASSERT_EQ(desired_0, static_cast(i)); 132 | ASSERT_EQ(desired_1, 42); 133 | } 134 | } 135 | }; 136 | 137 | TEST_F(TestRobotLogger, save_current_robot_data) 138 | { 139 | Logger logger(data, 0); 140 | 141 | backend->initialize(); 142 | 143 | Action action; 144 | action.values[0] = 42; 145 | action.values[1] = 42; 146 | 147 | robot_interfaces::TimeIndex t; 148 | for (uint32_t i = 0; i < max_number_of_actions; i++) 149 | { 150 | action.values[0] = i; 151 | t = frontend->append_desired_action(action); 152 | // wait for applied action to ensure all data of that time step is there 153 | frontend->get_applied_action(t); 154 | } 155 | 156 | // TODO: Why is the sleep needed here? 157 | // wait a moment to give the logger time to catch up 158 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 159 | 160 | logger.save_current_robot_data(logfile); 161 | 162 | check_csv_log(logfile, 0, max_number_of_actions, false); 163 | } 164 | 165 | TEST_F(TestRobotLogger, save_current_robot_data_binary) 166 | { 167 | Logger logger(data, 0); 168 | 169 | backend->initialize(); 170 | 171 | Action action; 172 | action.values[0] = 42; 173 | action.values[1] = 42; 174 | 175 | robot_interfaces::TimeIndex t; 176 | for (uint32_t i = 0; i < max_number_of_actions; i++) 177 | { 178 | action.values[0] = i; 179 | t = frontend->append_desired_action(action); 180 | // wait for applied action to ensure all data of that time step is there 181 | frontend->get_applied_action(t); 182 | } 183 | logger.save_current_robot_data_binary(logfile); 184 | 185 | // read the log for verification 186 | BinaryLogReader log(logfile); 187 | ASSERT_EQ(log.data.size(), max_number_of_actions); 188 | for (uint32_t i = 0; i < log.data.size(); i++) 189 | { 190 | ASSERT_EQ(log.data[i].desired_action.values[0], static_cast(i)); 191 | ASSERT_EQ(log.data[i].desired_action.values[1], 42); 192 | } 193 | } 194 | 195 | // TODO add another test where time series is smaller than the log 196 | TEST_F(TestRobotLogger, start_stop_binary) 197 | { 198 | // make sure the logger buffer is large enough 199 | uint32_t logger_buffer_limit = 2 * max_number_of_actions; 200 | Logger logger(data, logger_buffer_limit); 201 | 202 | backend->initialize(); 203 | 204 | Action action; 205 | action.values[0] = 42; 206 | action.values[1] = 42; 207 | 208 | logger.start(); 209 | 210 | robot_interfaces::TimeIndex t; 211 | for (uint32_t i = 0; i < max_number_of_actions; i++) 212 | { 213 | action.values[0] = i; 214 | t = frontend->append_desired_action(action); 215 | // wait for applied action to ensure all data of that time step is there 216 | frontend->get_applied_action(t); 217 | } 218 | 219 | // wait a moment to give the logger time to catch up 220 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 221 | 222 | logger.stop_and_save(logfile, Logger::Format::BINARY); 223 | 224 | // read the log for verification 225 | BinaryLogReader log(logfile); 226 | ASSERT_EQ(log.data.size(), max_number_of_actions); 227 | for (uint32_t i = 0; i < log.data.size(); i++) 228 | { 229 | ASSERT_EQ(log.data[i].desired_action.values[0], static_cast(i)); 230 | ASSERT_EQ(log.data[i].desired_action.values[1], 42); 231 | } 232 | } 233 | 234 | TEST_F(TestRobotLogger, start_stop_csv) 235 | { 236 | // make sure the logger buffer is large enough 237 | uint32_t logger_buffer_limit = 2 * max_number_of_actions; 238 | Logger logger(data, logger_buffer_limit); 239 | 240 | backend->initialize(); 241 | 242 | Action action; 243 | action.values[0] = 42; 244 | action.values[1] = 42; 245 | 246 | logger.start(); 247 | 248 | robot_interfaces::TimeIndex t; 249 | for (uint32_t i = 0; i < max_number_of_actions; i++) 250 | { 251 | action.values[0] = i; 252 | t = frontend->append_desired_action(action); 253 | // wait for applied action to ensure all data of that time step is there 254 | frontend->get_applied_action(t); 255 | } 256 | 257 | // wait a moment to give the logger time to catch up 258 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 259 | 260 | logger.stop_and_save(logfile, Logger::Format::CSV); 261 | 262 | check_csv_log(logfile, 0, max_number_of_actions, false); 263 | } 264 | 265 | TEST_F(TestRobotLogger, start_stop_csv_gzip) 266 | { 267 | // make sure the logger buffer is large enough 268 | uint32_t logger_buffer_limit = 2 * max_number_of_actions; 269 | Logger logger(data, logger_buffer_limit); 270 | 271 | backend->initialize(); 272 | 273 | Action action; 274 | action.values[0] = 42; 275 | action.values[1] = 42; 276 | 277 | logger.start(); 278 | 279 | robot_interfaces::TimeIndex t; 280 | for (uint32_t i = 0; i < max_number_of_actions; i++) 281 | { 282 | action.values[0] = i; 283 | t = frontend->append_desired_action(action); 284 | // wait for applied action to ensure all data of that time step is there 285 | frontend->get_applied_action(t); 286 | } 287 | 288 | // wait a moment to give the logger time to catch up 289 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 290 | 291 | logger.stop_and_save(logfile, Logger::Format::CSV_GZIP); 292 | 293 | check_csv_log(logfile, 0, max_number_of_actions, true); 294 | } 295 | 296 | TEST_F(TestRobotLogger, start_stop_empty_binary) 297 | { 298 | Logger logger(data, 10); 299 | backend->initialize(); 300 | 301 | // start and directly stop again without sending any actions 302 | logger.start(); 303 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 304 | logger.stop_and_save(logfile, Logger::Format::BINARY); 305 | 306 | // log should be empty 307 | BinaryLogReader log(logfile); 308 | ASSERT_TRUE(log.data.empty()); 309 | } 310 | 311 | TEST_F(TestRobotLogger, start_stop_empty_csv) 312 | { 313 | Logger logger(data, 10); 314 | backend->initialize(); 315 | 316 | // start and directly stop again without sending any actions 317 | logger.start(); 318 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 319 | logger.stop_and_save(logfile, Logger::Format::BINARY); 320 | 321 | // log should be empty (apart from header line) 322 | std::string line; 323 | std::ifstream infile(logfile); 324 | ASSERT_TRUE(std::getline(infile, line)) << "Failed to read header line"; 325 | ASSERT_FALSE(std::getline(infile, line)) 326 | << "Unexpected second line: " << line; 327 | } 328 | -------------------------------------------------------------------------------- /tests/test_sensor_interface.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Basic functionality of sensor interface. 4 | * @copyright Copyright (c) 2019, Max Planck Gesellschaft. 5 | */ 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "dummy_sensor_driver.hpp" 13 | 14 | using namespace robot_interfaces; 15 | 16 | // test if getting observations through the frontend works as expected 17 | TEST(TestSensorInterface, basic_pipeline) 18 | { 19 | auto data = std::make_shared>(); 20 | auto driver = 21 | std::make_shared(); 22 | auto frontend = SensorFrontend(data); 23 | auto backend = SensorBackend(driver, data); 24 | 25 | for (int t = 0; t < 20; t++) 26 | { 27 | int obs = frontend.get_observation(t); 28 | ASSERT_EQ(obs, t); 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /tests/test_sensor_logger.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Basic functionality of sensor interface. 4 | * @copyright Copyright (c) 2019, Max Planck Gesellschaft. 5 | */ 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "dummy_sensor_driver.hpp" 18 | 19 | using namespace robot_interfaces; 20 | 21 | //! Test fixture to create and delete a temporary log file 22 | class TestSensorLogger : public ::testing::Test 23 | { 24 | protected: 25 | std::string log_file; 26 | 27 | void SetUp() override 28 | { 29 | boost::filesystem::path temp = 30 | boost::filesystem::temp_directory_path() / 31 | boost::filesystem::unique_path(); 32 | log_file = temp.native(); 33 | } 34 | 35 | void TearDown() override 36 | { 37 | // clean up 38 | std::remove(log_file.c_str()); 39 | } 40 | }; 41 | 42 | // test if writing and reading the sensor log file is working correctly 43 | TEST_F(TestSensorLogger, write_and_read_log) 44 | { 45 | constexpr int NUM_OBSERVATIONS = 20; 46 | constexpr int BUFFER_LIMIT = 50; 47 | 48 | // write the log 49 | { 50 | auto data = std::make_shared>(); 51 | auto driver = 52 | std::make_shared(); 53 | auto frontend = SensorFrontend(data); 54 | auto logger = SensorLogger(data, BUFFER_LIMIT); 55 | logger.start(); 56 | 57 | // create backend last to ensure no message is missed 58 | auto backend = SensorBackend(driver, data); 59 | 60 | for (int t = 0; t < NUM_OBSERVATIONS; t++) 61 | { 62 | int obs = frontend.get_observation(t); 63 | // ensure that the observations written to the log are actually what 64 | // we expect 65 | ASSERT_EQ(obs, t); 66 | } 67 | logger.stop_and_save(log_file); 68 | } 69 | 70 | // read the log 71 | { 72 | auto log = SensorLogReader(log_file); 73 | ASSERT_GE(log.data.size(), static_cast(NUM_OBSERVATIONS)); 74 | for (int t = 0; t < NUM_OBSERVATIONS; t++) 75 | { 76 | ASSERT_EQ(log.data[t], t); 77 | } 78 | } 79 | } 80 | 81 | TEST_F(TestSensorLogger, buffer_limit) 82 | { 83 | constexpr int NUM_OBSERVATIONS = 20; 84 | constexpr int BUFFER_LIMIT = 10; 85 | 86 | // write the log 87 | { 88 | auto data = std::make_shared>(); 89 | auto driver = 90 | std::make_shared(); 91 | auto frontend = SensorFrontend(data); 92 | 93 | // set buffer 94 | auto logger = SensorLogger(data, BUFFER_LIMIT); 95 | logger.start(); 96 | 97 | // create backend last to ensure no message is missed 98 | auto backend = SensorBackend(driver, data); 99 | 100 | for (int t = 0; t < NUM_OBSERVATIONS; t++) 101 | { 102 | int obs = frontend.get_observation(t); 103 | // ensure that the observations written to the log are actually what 104 | // we expect 105 | ASSERT_EQ(obs, t); 106 | } 107 | logger.stop_and_save(log_file); 108 | } 109 | 110 | // read the log 111 | { 112 | auto log = SensorLogReader(log_file); 113 | 114 | // make sure the log size equals the buffer limit 115 | ASSERT_EQ(log.data.size(), static_cast(BUFFER_LIMIT)); 116 | 117 | // also verify that the messages are the expected ones (i.e. the ones 118 | // from the start, before the limit is reached). 119 | for (std::size_t t = 0; t < log.data.size(); t++) 120 | { 121 | ASSERT_EQ(log.data[t], static_cast(t)); 122 | } 123 | } 124 | } 125 | --------------------------------------------------------------------------------