From 7cd2138c8df301edf471b89a80b2dcf698339695 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Thu, 4 Jan 2024 20:04:00 +0100 Subject: [PATCH] Upload code from roboticslab-uc3m/yarp-devices --- .github/workflows/ci.yml | 118 +++++ .gitignore | 18 + CMakeLists.txt | 80 ++++ CONTRIBUTING.md | 10 + README.md | 35 ++ doc/CMakeLists.txt | 32 ++ doc/dextra-yarp-devices-install.md | 26 ++ doc/groups.dox | 49 ++ libraries/CMakeLists.txt | 2 + .../DextraRawControlBoardLib/CMakeLists.txt | 32 ++ .../DeviceDriverImpl.cpp | 15 + .../DextraRawControlBoard.cpp | 59 +++ .../DextraRawControlBoard.hpp | 165 +++++++ .../IAxisInfoRawImpl.cpp | 31 ++ .../IControlLimitsRawImpl.cpp | 50 ++ .../IControlModeRawImpl.cpp | 87 ++++ .../IEncodersRawImpl.cpp | 134 ++++++ .../IPositionControlRawImpl.cpp | 432 ++++++++++++++++++ .../IPositionDirectRawImpl.cpp | 33 ++ .../DextraRawControlBoardLib/LogComponent.cpp | 3 + .../DextraRawControlBoardLib/LogComponent.hpp | 8 + libraries/DextraRawControlBoardLib/README.md | 3 + .../DextraRawControlBoardLib/Synapse.cpp | 91 ++++ .../DextraRawControlBoardLib/Synapse.hpp | 56 +++ libraries/YarpPlugins/CMakeLists.txt | 3 + .../DextraCanControlBoard/CMakeLists.txt | 25 + .../DextraCanControlBoard/CanSynapse.cpp | 42 ++ .../DeviceDriverImpl.cpp | 43 ++ .../DextraCanControlBoard.hpp | 72 +++ .../ICanBusSharerImpl.cpp | 36 ++ .../DextraCanControlBoard/README.md | 4 + .../DextraSerialControlBoard/CMakeLists.txt | 24 + .../DeviceDriverImpl.cpp | 66 +++ .../DextraSerialControlBoard.hpp | 228 +++++++++ .../DextraSerialControlBoard/README.md | 9 + .../SerialSynapse.cpp | 23 + 36 files changed, 2144 insertions(+) create mode 100644 .github/workflows/ci.yml create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 CONTRIBUTING.md create mode 100644 README.md create mode 100644 doc/CMakeLists.txt create mode 100644 doc/dextra-yarp-devices-install.md create mode 100644 doc/groups.dox create mode 100644 libraries/CMakeLists.txt create mode 100644 libraries/DextraRawControlBoardLib/CMakeLists.txt create mode 100644 libraries/DextraRawControlBoardLib/DeviceDriverImpl.cpp create mode 100644 libraries/DextraRawControlBoardLib/DextraRawControlBoard.cpp create mode 100644 libraries/DextraRawControlBoardLib/DextraRawControlBoard.hpp create mode 100644 libraries/DextraRawControlBoardLib/IAxisInfoRawImpl.cpp create mode 100644 libraries/DextraRawControlBoardLib/IControlLimitsRawImpl.cpp create mode 100644 libraries/DextraRawControlBoardLib/IControlModeRawImpl.cpp create mode 100644 libraries/DextraRawControlBoardLib/IEncodersRawImpl.cpp create mode 100644 libraries/DextraRawControlBoardLib/IPositionControlRawImpl.cpp create mode 100644 libraries/DextraRawControlBoardLib/IPositionDirectRawImpl.cpp create mode 100644 libraries/DextraRawControlBoardLib/LogComponent.cpp create mode 100644 libraries/DextraRawControlBoardLib/LogComponent.hpp create mode 100644 libraries/DextraRawControlBoardLib/README.md create mode 100644 libraries/DextraRawControlBoardLib/Synapse.cpp create mode 100644 libraries/DextraRawControlBoardLib/Synapse.hpp create mode 100644 libraries/YarpPlugins/CMakeLists.txt create mode 100644 libraries/YarpPlugins/DextraCanControlBoard/CMakeLists.txt create mode 100644 libraries/YarpPlugins/DextraCanControlBoard/CanSynapse.cpp create mode 100644 libraries/YarpPlugins/DextraCanControlBoard/DeviceDriverImpl.cpp create mode 100644 libraries/YarpPlugins/DextraCanControlBoard/DextraCanControlBoard.hpp create mode 100644 libraries/YarpPlugins/DextraCanControlBoard/ICanBusSharerImpl.cpp create mode 100644 libraries/YarpPlugins/DextraCanControlBoard/README.md create mode 100644 libraries/YarpPlugins/DextraSerialControlBoard/CMakeLists.txt create mode 100644 libraries/YarpPlugins/DextraSerialControlBoard/DeviceDriverImpl.cpp create mode 100644 libraries/YarpPlugins/DextraSerialControlBoard/DextraSerialControlBoard.hpp create mode 100644 libraries/YarpPlugins/DextraSerialControlBoard/README.md create mode 100644 libraries/YarpPlugins/DextraSerialControlBoard/SerialSynapse.cpp diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..dcb0859 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,118 @@ +name: Continuous Integration + +on: + push: + paths-ignore: + - 'doc/**' + - '**.md' + pull_request: + workflow_dispatch: + +env: + CMAKE_CCACHE_LAUNCHER: -DCMAKE_C_COMPILER_LAUNCHER=ccache -DCMAKE_CXX_COMPILER_LAUNCHER=ccache + ARAVIS_VER: 0.6.4 + PEAK_DRIVER_VER: 8.11.0 + +jobs: + maybe_skip: + runs-on: ubuntu-latest + outputs: + should_skip: ${{steps.skip_check.outputs.should_skip}} + steps: + - uses: fkirc/skip-duplicate-actions@v5 + id: skip_check + with: + cancel_others: 'true' + + build: + name: build (${{matrix.os}}, ${{matrix.robotology.yarp}}, ${{matrix.compiler.cc}}) + runs-on: ${{matrix.os}} + needs: maybe_skip + if: ${{needs.maybe_skip.outputs.should_skip != 'true'}} + + strategy: + fail-fast: false + matrix: + os: [ubuntu-20.04, ubuntu-22.04] + robotology: + - { yarp: yarp-3.8, cmake: 3.16.x } + - { yarp: yarp-3.9, cmake: 3.16.x } + - { yarp: master, cmake: 3.16.x } + compiler: + - { cc: gcc, cxx: g++ } + - { cc: clang, cxx: clang++ } + experimental: + - ${{github.event_name == 'schedule' || github.event_name == 'workflow_dispatch'}} + exclude: + - { experimental: false, robotology: { yarp: master } } + + steps: + - name: Check out main project + uses: actions/checkout@v4 + + - name: Check out YCM + uses: actions/checkout@v4 + with: + repository: robotology/ycm + path: .deps/ycm + + - name: Check out YARP + uses: actions/checkout@v4 + with: + repository: robotology/yarp + ref: ${{matrix.robotology.yarp}} + path: .deps/yarp + + - name: Check out roboticslab-uc3m/yarp-devices + uses: actions/checkout@v4 + with: + repository: roboticslab-uc3m/yarp-devices + path: .deps/yarp-devices + + - name: Install dependencies via apt + run: sudo apt-get update && sudo apt-get install -qq ccache + + - name: Set up CMake + uses: jwlawson/actions-setup-cmake@v1 + with: + cmake-version: ${{matrix.robotology.cmake}} + + - name: Set up Ccache + uses: hendrikmuhs/ccache-action@v1 + with: + key: ${{matrix.os}}-${{matrix.robotology.yarp}}-${{matrix.compiler.cc}} + + - name: Set environment variables + run: | + echo "CC=${{matrix.compiler.cc}}" >> $GITHUB_ENV + echo "CXX=${{matrix.compiler.cxx}}" >> $GITHUB_ENV + + - name: Build YCM + run: | + cmake -S .deps/ycm -B .deps/ycm/build + cmake --build .deps/ycm/build + sudo cmake --install .deps/ycm/build + + - name: Build YARP + run: | + cmake -S .deps/yarp -B .deps/yarp/build $CMAKE_CCACHE_LAUNCHER -DSKIP_ACE=ON -DYARP_DISABLE_VERSION_SOURCE=ON + cmake --build .deps/yarp/build + sudo cmake --install .deps/yarp/build + + - name: Build roboticslab-uc3m/yarp-devices + run: | + cmake -S .deps/yarp-devices -B .deps/yarp-devices/build $CMAKE_CCACHE_LAUNCHER + cmake --build .deps/yarp-devices/build + sudo cmake --install .deps/yarp-devices/build + + - name: Configure main project + run: cmake -S . -B build $CMAKE_CCACHE_LAUNCHER + + - name: Compile main project + run: cmake --build build + + - name: Install main project + run: sudo cmake --install build && sudo ldconfig + + - name: Uninstall main project + run: sudo cmake --build build --target uninstall diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ec11bde --- /dev/null +++ b/.gitignore @@ -0,0 +1,18 @@ +**/build*/ +*~ +*.swp +*.orig +tags +CMakeLists.txt.user +doc/html +doc/doxygen_sqlite3.db +/.cproject +/.project +/.settings/ +/.vscode/ +/.vs/ +.idea +venv +**/__pycache__ +/external/ +**/*.mcw diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..b696ca4 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,80 @@ +cmake_minimum_required(VERSION 3.16) + +project(DEXTRA_YARP_DEVICES LANGUAGES CXX) + +# Let the user specify a configuration (only single-config generators). +if(NOT CMAKE_CONFIGURATION_TYPES) + # Possible values. + set(_configurations Debug Release MinSizeRel RelWithDebInfo) + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS ${_configurations}) + + foreach(_conf ${_configurations}) + set(_conf_string "${_conf_string} ${_conf}") + endforeach() + + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY HELPSTRING + "Choose the type of build, options are:${_conf_string}") + + if(NOT CMAKE_BUILD_TYPE) + # Encourage the user to specify build type. + message(STATUS "Setting build type to 'Release' as none was specified.") + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE Release) + endif() +endif() + +# Pick up our CMake modules. +list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake + ${CMAKE_SOURCE_DIR}/cmake/find-modules) + +# Hard dependencies. +find_package(YCM 0.11 REQUIRED) +find_package(YARP 3.8 REQUIRED COMPONENTS os dev) + +# Soft dependencies. +find_package(ROBOTICSLAB_YARP_DEVICES QUIET) +find_package(Doxygen QUIET) +#find_package(GTestSources 1.8 QUIET) + +# Always build YARP devices as MODULE libraries. +set(YARP_FORCE_DYNAMIC_PLUGINS TRUE CACHE INTERNAL "Force dynamic plugins") + +# Configure installation paths for YARP resources. +yarp_configure_external_installation(dextra-yarp-devices WITH_PLUGINS) + +# Standard installation directories. +include(GNUInstallDirs) + +# Control where libraries and executables are placed during the build. +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}) +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}) +set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}) + +# Create targets if specific requirements are satisfied. +include(CMakeDependentOption) + +# Acknowledge this is a CTest-friendly project. +#enable_testing() + +# Add main contents. +add_subdirectory(libraries) +# add_subdirectory(programs) +# add_subdirectory(tests) +# add_subdirectory(share) +add_subdirectory(doc) + +# Store the package in the user registry. +set(CMAKE_EXPORT_PACKAGE_REGISTRY ON) + +# Create and install config files. +include(InstallBasicPackageFiles) + +install_basic_package_files(DEXTRA_YARP_DEVICES + VERSION 0.1.0 + COMPATIBILITY AnyNewerVersion + NO_EXPORT + NO_SET_AND_CHECK_MACRO + NO_CHECK_REQUIRED_COMPONENTS_MACRO + NAMESPACE ROBOTICSLAB::) + +# Configure and create uninstall target. +include(AddUninstallTarget) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..186dd8a --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,10 @@ +Have a feature request or found a bug? + +Before opening a fresh issue, do a search on [GitHub](https://github.com/roboticslab-uc3m/dextra-yarp-devices/issues?utf8=%E2%9C%93&q=is%3Aissue) and make sure it hasn't already been addressed. Here are some notes on how to detail feature requests or bug reports: +* Explain, as detailed as possible, how to reproduce the issue or the specific behaviour for the feature. +* Include what you expected to happen and what actually happened. +* Please include information on what operating system and version you are working with. Also add any other relevant details. +* Feel free to attach any other information illustrating the issue if copying and pasting text is not an option. + +Notes: +* We follow the [Forking](https://www.atlassian.com/git/tutorials/comparing-workflows/forking-workflow/) Git workflow. diff --git a/README.md b/README.md new file mode 100644 index 0000000..5a2108a --- /dev/null +++ b/README.md @@ -0,0 +1,35 @@ +# dextra-yarp-devices + +YARP devices for the [Dextra prosthetic hand](https://github.com/roboticslab-uc3m/Dextra). + +## Installation + +Installation instructions for installing from source can be found [here](doc/dextra-yarp-devices-install.md). + +## Contributing + +#### Posting Issues + +1. Read [CONTRIBUTING.md](CONTRIBUTING.md) +2. [Post an issue / Feature request / Specific documentation request](https://github.com/roboticslab-uc3m/dextra-yarp-devices/issues) + +#### Fork & Pull Request + +1. [Fork the repository](https://github.com/roboticslab-uc3m/dextra-yarp-devices/fork) +2. Create your feature branch (`git checkout -b my-new-feature`) off the `master` branch, following the [Forking Git workflow](https://www.atlassian.com/git/tutorials/comparing-workflows/forking-workflow) +3. Commit your changes +4. Push to the branch (`git push origin my-new-feature`) +5. Create a new Pull Request + +## Status + +[![CI (Linux)](https://github.com/roboticslab-uc3m/dextra-yarp-devices/workflows/Continuous%20Integration/badge.svg)](https://github.com/roboticslab-uc3m/dextra-yarp-devices/actions) + +[![Issues](https://img.shields.io/github/issues/roboticslab-uc3m/dextra-yarp-devices.svg?label=Issues)](https://github.com/roboticslab-uc3m/dextra-yarp-devices/issues) + +## Similar and Related Projects + +- [roboticslab-uc3m/yarp-devices](https://github.com/roboticslab-uc3m/yarp-devices) + - [issue #176](https://github.com/roboticslab-uc3m/yarp-devices/issues/176) + - [issue #227](https://github.com/roboticslab-uc3m/yarp-devices/issues/227) +- [roboticslab-uc3m/Dextra](https://github.com/roboticslab-uc3m/Dextra) diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt new file mode 100644 index 0000000..0d15e20 --- /dev/null +++ b/doc/CMakeLists.txt @@ -0,0 +1,32 @@ +# https://stackoverflow.com/a/25217937 +get_directory_property(_has_parent PARENT_DIRECTORY) + +if(NOT _has_parent) + cmake_minimum_required(VERSION 3.9) + project(doxygen NONE) + find_package(Doxygen REQUIRED) +elseif(NOT DOXYGEN_FOUND) + return() +endif() + +set(DOXYGEN_PROJECT_NAME "dextra-yarp-devices") +set(DOXYGEN_REPEAT_BRIEF NO) +set(DOXYGEN_EXTRACT_PRIVATE YES) +set(DOXYGEN_EXTRACT_STATIC YES) +set(DOXYGEN_EXAMPLE_PATH ./examples) +set(DOXYGEN_EXAMPLE_RECURSIVE YES) +set(DOXYGEN_IMAGE_PATH ./doc/fig) +set(DOXYGEN_HTML_TIMESTAMP YES) +set(DOXYGEN_USE_MATHJAX YES) +set(DOXYGEN_USE_MDFILE_AS_MAINPAGE ./README.md) + +set(_doxygen_input README.md + doc + examples + libraries + programs + share + tests) + +doxygen_add_docs(dox ${_doxygen_input} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/..) diff --git a/doc/dextra-yarp-devices-install.md b/doc/dextra-yarp-devices-install.md new file mode 100644 index 0000000..417fe10 --- /dev/null +++ b/doc/dextra-yarp-devices-install.md @@ -0,0 +1,26 @@ +# Installation from Source Code + +First install the dependencies: +- [Install CMake 3.16+](https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-cmake.md/) +- [Install YCM 0.11+](https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-ycm.md/) +- [Install YARP 3.8+](https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-yarp.md/) +- [Install YARP 3.8+](https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-yarp.md/) + +### Components with known additional/specific dependencies +- [../libraries/YarpPlugins/DextraCanControlBoard](../libraries/YarpPlugins/DextraCanControlBoard#requirements) + +## Installation (Ubuntu) + +Once the required dependencies have been installed, the code has to be compiled and installed. Note that you will be prompted for your password upon using `sudo` a couple of times: + +```bash +cd # go home +mkdir -p repos; cd repos # make $HOME/repos if it doesn't exist; then, enter it +git clone https://github.com/roboticslab-uc3m/dextra-yarp-devices.git # Download yarp-devices software from the repository +cd dextra-yarp-devices; mkdir build; cd build; cmake .. # Configure the dextra-yarp-devices software +make -j$(nproc) # Compile +sudo make install # Install :-) +sudo ldconfig # Just in case +``` + +For additional options use `ccmake` instead of `cmake`. diff --git a/doc/groups.dox b/doc/groups.dox new file mode 100644 index 0000000..7d7e8de --- /dev/null +++ b/doc/groups.dox @@ -0,0 +1,49 @@ + +/** +@brief The main, catch-all namespace for Robotics Lab UC3M. + */ +namespace roboticslab +{ + /** + * @brief Contains classes related to unit testing. + */ + namespace test {} +} + +/** + * @defgroup dextra_yarp_devices_libraries Libraries + * @brief dextra-yarp-devices libraries. + */ + +/** + * @ingroup dextra_yarp_devices_libraries + * @defgroup YarpPlugins YARP plugins + * + * @brief Contains libraries that implement YARP device interfaces + * and therefore can be invoked as YARP plugins. + */ + +/** + * @defgroup dextra_yarp_devices_programs Programs + * @brief dextra-yarp-devices programs. + */ + +/** + * @defgroup dextra_yarp_devices_tests Tests + * @brief dextra-yarp-devices tests. + */ + +/** + * @defgroup dextra_yarp_devices_examples Examples + * @brief dextra-yarp-devices examples. + */ + +/** + * @ingroup dextra_yarp_devices_examples + * @defgroup dextra_yarp_devices_examples_cpp C++ examples + */ + +/** + * @ingroup dextra_yarp_devices_examples + * @defgroup dextra_yarp_devices_examples_py Python examples + */ diff --git a/libraries/CMakeLists.txt b/libraries/CMakeLists.txt new file mode 100644 index 0000000..f8ae1a1 --- /dev/null +++ b/libraries/CMakeLists.txt @@ -0,0 +1,2 @@ +add_subdirectory(DextraRawControlBoardLib) +add_subdirectory(YarpPlugins) diff --git a/libraries/DextraRawControlBoardLib/CMakeLists.txt b/libraries/DextraRawControlBoardLib/CMakeLists.txt new file mode 100644 index 0000000..d22a605 --- /dev/null +++ b/libraries/DextraRawControlBoardLib/CMakeLists.txt @@ -0,0 +1,32 @@ +option(ENABLE_DextraRawControlBoardLib "Enable/disable DextraRawControlBoardLib library" ON) + +if(ENABLE_DextraRawControlBoardLib) + + add_library(DextraRawControlBoardLib SHARED DextraRawControlBoard.hpp + DextraRawControlBoard.cpp + DeviceDriverImpl.cpp + IAxisInfoRawImpl.cpp + IControlLimitsRawImpl.cpp + IControlModeRawImpl.cpp + IEncodersRawImpl.cpp + IPositionControlRawImpl.cpp + IPositionDirectRawImpl.cpp + LogComponent.hpp + LogComponent.cpp + Synapse.hpp + Synapse.cpp) + + target_link_libraries(DextraRawControlBoardLib PUBLIC YARP::YARP_dev + PRIVATE YARP::YARP_os) + + target_include_directories(DextraRawControlBoardLib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + + install(TARGETS DextraRawControlBoardLib) + + add_library(ROBOTICSLAB::DextraRawControlBoardLib ALIAS DextraRawControlBoardLib) + +else() + + set(ENABLE_DextraRawControlBoardLib OFF CACHE BOOL "Enable/disable DextraRawControlBoardLib library" FORCE) + +endif() diff --git a/libraries/DextraRawControlBoardLib/DeviceDriverImpl.cpp b/libraries/DextraRawControlBoardLib/DeviceDriverImpl.cpp new file mode 100644 index 0000000..e048a48 --- /dev/null +++ b/libraries/DextraRawControlBoardLib/DeviceDriverImpl.cpp @@ -0,0 +1,15 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "DextraRawControlBoard.hpp" + +using namespace roboticslab; + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::open(yarp::os::Searchable & config) +{ + axisPrefix = config.check("axisPrefix", yarp::os::Value(""), "common refix for all axis names").asString(); + return true; +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/DextraRawControlBoardLib/DextraRawControlBoard.cpp b/libraries/DextraRawControlBoardLib/DextraRawControlBoard.cpp new file mode 100644 index 0000000..6235b50 --- /dev/null +++ b/libraries/DextraRawControlBoardLib/DextraRawControlBoard.cpp @@ -0,0 +1,59 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "DextraRawControlBoard.hpp" + +#include + +using namespace roboticslab; + +// ----------------------------------------------------------------------------- + +void DextraRawControlBoard::acquireSynapseHandle(Synapse * _synapse) +{ + synapse = _synapse; +} + +// ----------------------------------------------------------------------------- + +void DextraRawControlBoard::destroySynapse() +{ + if (synapse) + { + delete synapse; + synapse = nullptr; + } +} + +// ----------------------------------------------------------------------------- + +double DextraRawControlBoard::getSetpoint(int j) +{ + std::lock_guard lock(setpointMutex); + return setpoints[j]; +} + +// ----------------------------------------------------------------------------- + +void DextraRawControlBoard::getSetpoints(Synapse::Setpoints & setpoints) +{ + std::lock_guard lock(setpointMutex); + std::copy(std::cbegin(this->setpoints), std::cend(this->setpoints), std::begin(setpoints)); +} + +// ----------------------------------------------------------------------------- + +void DextraRawControlBoard::setSetpoint(int j, Synapse::setpoint_t setpoint) +{ + std::lock_guard lock(setpointMutex); + setpoints[j] = setpoint; +} + +// ----------------------------------------------------------------------------- + +void DextraRawControlBoard::setSetpoints(const Synapse::Setpoints & setpoints) +{ + std::lock_guard lock(setpointMutex); + std::copy(std::cbegin(setpoints), std::cend(setpoints), std::begin(this->setpoints)); +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/DextraRawControlBoardLib/DextraRawControlBoard.hpp b/libraries/DextraRawControlBoardLib/DextraRawControlBoard.hpp new file mode 100644 index 0000000..64b36ce --- /dev/null +++ b/libraries/DextraRawControlBoardLib/DextraRawControlBoard.hpp @@ -0,0 +1,165 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#ifndef __DEXTRA_RAW_CONTROL_BOARD_HPP__ +#define __DEXTRA_RAW_CONTROL_BOARD_HPP__ + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#if YARP_VERSION_COMPARE(<, 3, 9, 0) +# include +#endif + +#include "Synapse.hpp" + +#define CHECK_JOINT(j) do { if ((j) < 0 || (j) >= Synapse::DATA_POINTS) return false; } while (0) + +namespace roboticslab +{ + +/** + * @ingroup yarp_devices_libraries + * @defgroup DextraRawControlBoard + * @brief Contains roboticslab::DextraRawControlBoard. + */ + +/** + * @ingroup DextraRawControlBoard + * @brief Base implementation for the custom UC3M Dextra Hand control board interfaces. + */ +class DextraRawControlBoard : public yarp::dev::DeviceDriver, + public yarp::dev::IAxisInfoRaw, + public yarp::dev::IControlLimitsRaw, + public yarp::dev::IControlModeRaw, + public yarp::dev::IEncodersTimedRaw, + public yarp::dev::IPositionControlRaw, + public yarp::dev::IPositionDirectRaw +#if YARP_VERSION_COMPARE(<, 3, 9, 0) + , public yarp::dev::IVelocityControlRaw +#endif +{ +public: + ~DextraRawControlBoard() override = default; + + void acquireSynapseHandle(Synapse * synapse); + void destroySynapse(); + + // --------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp --------- + + bool open(yarp::os::Searchable & config) override; + + // --------- IAxisInfoRaw declarations. Implementation in IAxisInfoRawImpl.cpp --------- + + bool getAxisNameRaw(int axis, std::string & name) override; + bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum & type) override; + + // --------- IControlLimitsRaw declarations. Implementation in IControlLimitsRawImpl.cpp --------- + + bool setLimitsRaw(int axis, double min, double max) override; + bool getLimitsRaw(int axis, double * min, double * max) override; + bool setVelLimitsRaw(int axis, double min, double max) override; + bool getVelLimitsRaw(int axis, double * min, double * max) override; + + // --------- IControlModeRaw declarations. Implementation in IControlModeRawImpl.cpp --------- + + bool getControlModeRaw(int j, int * mode) override; + bool getControlModesRaw(int * modes) override; + bool getControlModesRaw(int n_joint, const int * joints, int * modes) override; + bool setControlModeRaw(int j, int mode) override; + bool setControlModesRaw(int * modes) override; + bool setControlModesRaw(int n_joint, const int * joints, int * modes) override; + + // ---------- IEncodersRaw declarations. Implementation in IEncodersRawImpl.cpp ---------- + + bool getAxes(int * ax) override; + bool resetEncoderRaw(int j) override; + bool resetEncodersRaw() override; + bool setEncoderRaw(int j, double val) override; + bool setEncodersRaw(const double * vals) override; + bool getEncoderRaw(int j, double * v) override; + bool getEncodersRaw(double * encs) override; + bool getEncoderSpeedRaw(int j, double * sp) override; + bool getEncoderSpeedsRaw(double * spds) override; + bool getEncoderAccelerationRaw(int j, double * spds) override; + bool getEncoderAccelerationsRaw(double * accs) override; + + // ---------- IEncodersTimedRaw declarations. Implementation in IEncodersRawImpl.cpp ---------- + + bool getEncoderTimedRaw(int j, double * encs, double * time) override; + bool getEncodersTimedRaw(double * encs, double * time) override; + + // ------- IPositionControlRaw declarations. Implementation in IPositionControlRawImpl.cpp ------- + + //bool getAxes(int * ax) override; + bool positionMoveRaw(int j, double ref) override; + bool positionMoveRaw(const double * refs) override; + bool positionMoveRaw(int n_joint, const int * joints, const double * refs) override; + bool relativeMoveRaw(int j, double delta) override; + bool relativeMoveRaw(const double * deltas) override; + bool relativeMoveRaw(int n_joint, const int * joints, const double * deltas) override; + bool checkMotionDoneRaw(int j, bool * flag) override; + bool checkMotionDoneRaw(bool * flag) override; + bool checkMotionDoneRaw(int n_joint, const int * joints, bool * flag) override; + bool setRefSpeedRaw(int j, double sp) override; + bool setRefSpeedsRaw(const double * spds) override; + bool setRefSpeedsRaw(int n_joint, const int * joints, const double * spds) override; + bool setRefAccelerationRaw(int j, double acc) override; + bool setRefAccelerationsRaw(const double * accs) override; + bool setRefAccelerationsRaw(int n_joint, const int * joints, const double * accs) override; + bool getRefSpeedRaw(int j, double * ref) override; + bool getRefSpeedsRaw(double * spds) override; + bool getRefSpeedsRaw(int n_joint, const int * joints, double * spds) override; + bool getRefAccelerationRaw(int j, double * acc) override; + bool getRefAccelerationsRaw(double * accs) override; + bool getRefAccelerationsRaw(int n_joint, const int * joints, double * accs) override; + bool stopRaw(int j) override; + bool stopRaw() override; + bool stopRaw(int n_joint, const int * joints) override; + bool getTargetPositionRaw(int joint, double * ref) override; + bool getTargetPositionsRaw(double * refs) override; + bool getTargetPositionsRaw(int n_joint, const int * joints, double * refs) override; + + // ------- IPositionDirectRaw declarations. Implementation in IPositionDirectRawImpl.cpp ------- + + bool setPositionRaw(int j, double ref) override; + bool setPositionsRaw(const int n_joint, const int *joints, const double *refs) override; + bool setPositionsRaw(const double *refs) override; + +#if YARP_VERSION_COMPARE(<, 3, 9, 0) + // --------- IVelocityControlRaw declarations and stub implementations. --------- + + // re-implemented methods have been omitted + bool velocityMoveRaw(int j, double sp) override { return false; } + bool velocityMoveRaw(const double * sp) override { return false; } + bool velocityMoveRaw(int n_joint, const int * joints, const double * spds) override { return false; } + bool getRefVelocityRaw(int joint, double * vel) override { return false; } + bool getRefVelocitiesRaw(double * vels) override { return false; } + bool getRefVelocitiesRaw(int n_joint, const int * joints, double * vels) override { return false; } +#endif + +protected: + Synapse * synapse {nullptr}; + std::string axisPrefix; + +private: + double getSetpoint(int j); + void getSetpoints(Synapse::Setpoints & setpoints); + void setSetpoint(int j, Synapse::setpoint_t setpoint); + void setSetpoints(const Synapse::Setpoints & setpoints); + + Synapse::Setpoints setpoints; + mutable std::mutex setpointMutex; +}; + +} // namespace roboticslab + +#endif // __DEXTRA_RAW_CONTROL_BOARD_HPP__ diff --git a/libraries/DextraRawControlBoardLib/IAxisInfoRawImpl.cpp b/libraries/DextraRawControlBoardLib/IAxisInfoRawImpl.cpp new file mode 100644 index 0000000..b46588d --- /dev/null +++ b/libraries/DextraRawControlBoardLib/IAxisInfoRawImpl.cpp @@ -0,0 +1,31 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "DextraRawControlBoard.hpp" + +#include + +#include "LogComponent.hpp" + +using namespace roboticslab; + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::getAxisNameRaw(int axis, std::string & name) +{ + yCITrace(DEXTRA, id(), "%d", axis); + CHECK_JOINT(axis); + name = axisPrefix + Synapse::LABELS[axis]; + return true; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::getJointTypeRaw(int axis, yarp::dev::JointTypeEnum & type) +{ + yCITrace(DEXTRA, id(), "%d", axis); + CHECK_JOINT(axis); + type = yarp::dev::JointTypeEnum::VOCAB_JOINTTYPE_REVOLUTE; + return true; +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/DextraRawControlBoardLib/IControlLimitsRawImpl.cpp b/libraries/DextraRawControlBoardLib/IControlLimitsRawImpl.cpp new file mode 100644 index 0000000..271cbe2 --- /dev/null +++ b/libraries/DextraRawControlBoardLib/IControlLimitsRawImpl.cpp @@ -0,0 +1,50 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "DextraRawControlBoard.hpp" + +#include + +#include "LogComponent.hpp" + +using namespace roboticslab; + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::setLimitsRaw(int axis, double min, double max) +{ + yCITrace(DEXTRA, id(), "%d %f %f", axis, min, max); + CHECK_JOINT(axis); + return false; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::getLimitsRaw(int axis, double * min, double * max) +{ + yCITrace(DEXTRA, id(), "%d", axis); + CHECK_JOINT(axis); + + const auto & [_min, _max] = Synapse::LIMITS[axis]; + *min = _min; + *max = _max; + + return true; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::setVelLimitsRaw(int axis, double min, double max) +{ + yCIWarning(DEXTRA, id(), "setVelLimitsRaw() not supported"); + return false; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::getVelLimitsRaw(int axis, double * min, double * max) +{ + yCIWarning(DEXTRA, id(), "getVelLimitsRaw() not supported"); + return false; +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/DextraRawControlBoardLib/IControlModeRawImpl.cpp b/libraries/DextraRawControlBoardLib/IControlModeRawImpl.cpp new file mode 100644 index 0000000..dc74382 --- /dev/null +++ b/libraries/DextraRawControlBoardLib/IControlModeRawImpl.cpp @@ -0,0 +1,87 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "DextraRawControlBoard.hpp" + +#include +#include + +#include "LogComponent.hpp" + +using namespace roboticslab; + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::getControlModeRaw(int j, int * mode) +{ + yCITrace(DEXTRA, id(), "%d", j); + CHECK_JOINT(j); + *mode = VOCAB_CM_POSITION; + return true; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::getControlModesRaw(int * modes) +{ + bool ok = true; + + for (int j = 0; j < Synapse::DATA_POINTS; j++) + { + ok &= getControlModeRaw(j, &modes[j]); + } + + return ok; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::getControlModesRaw(int n_joint, const int * joints, int * modes) +{ + bool ok = true; + + for (int i = 0; i < n_joint; i++) + { + ok &= getControlModeRaw(joints[i], &modes[i]); + } + + return ok; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::setControlModeRaw(int j, int mode) +{ + yCITrace(DEXTRA, id(), "%d %s", j, yarp::os::Vocab32::decode(mode).c_str()); + CHECK_JOINT(j); + return false; // don't allow any control modes other than position direct, for now +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::setControlModesRaw(int * modes) +{ + bool ok = true; + + for (int j = 0; j < Synapse::DATA_POINTS; j++) + { + ok &= setControlModeRaw(j, modes[j]); + } + + return ok; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::setControlModesRaw(int n_joint, const int * joints, int * modes) +{ + bool ok = true; + + for (int i = 0; i < n_joint; i++) + { + ok &= setControlModeRaw(joints[i], modes[i]); + } + + return ok; +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/DextraRawControlBoardLib/IEncodersRawImpl.cpp b/libraries/DextraRawControlBoardLib/IEncodersRawImpl.cpp new file mode 100644 index 0000000..f1ffe7c --- /dev/null +++ b/libraries/DextraRawControlBoardLib/IEncodersRawImpl.cpp @@ -0,0 +1,134 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "DextraRawControlBoard.hpp" + +#include + +#include +#include + +#include "LogComponent.hpp" + +using namespace roboticslab; + +// ------------------------------------------------------------------------------ + +bool DextraRawControlBoard::resetEncoderRaw(int j) +{ + yCITrace(DEXTRA, id(), "%d", j); + return setEncoderRaw(j, 0.0); +} + +// ------------------------------------------------------------------------------ + +bool DextraRawControlBoard::resetEncodersRaw() +{ + yCITrace(DEXTRA, id(), ""); + Synapse::Setpoints setpoints = {0}; + setSetpoints(setpoints); + return true; +} + +// ------------------------------------------------------------------------------ + +bool DextraRawControlBoard::setEncoderRaw(int j, double val) +{ + yCITrace(DEXTRA, id(), "%d %f", j, val); + CHECK_JOINT(j); + setSetpoint(j, val); + return true; +} + +// ------------------------------------------------------------------------------ + +bool DextraRawControlBoard::setEncodersRaw(const double * vals) +{ + yCITrace(DEXTRA, id(), ""); + Synapse::Setpoints setpoints; + std::copy(vals, vals + Synapse::DATA_POINTS, std::begin(setpoints)); + setSetpoints(setpoints); + return true; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::getEncoderRaw(int j, double * v) +{ + yCITrace(DEXTRA, id(), "%d", j); + CHECK_JOINT(j); + *v = getSetpoint(j); + return true; +} + +// ------------------------------------------------------------------------------ + +bool DextraRawControlBoard::getEncodersRaw(double *encs) +{ + Synapse::Setpoints setpoints; + getSetpoints(setpoints); + std::copy(std::cbegin(setpoints), std::cend(setpoints), encs); + return true; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::getEncoderSpeedRaw(int j, double * sp) +{ + CHECK_JOINT(j); + return false; +} + +// ------------------------------------------------------------------------------ + +bool DextraRawControlBoard::getEncoderSpeedsRaw(double * spds) +{ + bool ok = true; + + for (int j = 0; j < Synapse::DATA_POINTS; j++) + { + ok &= getEncoderSpeedRaw(j, &spds[j]); + } + + return ok; +} + +// ------------------------------------------------------------------------------ + +bool DextraRawControlBoard::getEncoderAccelerationRaw(int j, double * accs) +{ + CHECK_JOINT(j); + return false; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::getEncoderAccelerationsRaw(double * accs) +{ + bool ok = true; + + for (int j = 0; j < Synapse::DATA_POINTS; j++) + { + ok &= getEncoderAccelerationRaw(j, &accs[j]); + } + + return ok; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::getEncoderTimedRaw(int j, double * enc, double * time) +{ + CHECK_JOINT(j); + *time = yarp::os::Time::now(); + return getEncoderRaw(j, enc); +} + +// ------------------------------------------------------------------------------ + +bool DextraRawControlBoard::getEncodersTimedRaw(double * encs, double * time) +{ + *time = yarp::os::Time::now(); + return getEncodersRaw(encs); +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/DextraRawControlBoardLib/IPositionControlRawImpl.cpp b/libraries/DextraRawControlBoardLib/IPositionControlRawImpl.cpp new file mode 100644 index 0000000..e052106 --- /dev/null +++ b/libraries/DextraRawControlBoardLib/IPositionControlRawImpl.cpp @@ -0,0 +1,432 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "DextraRawControlBoard.hpp" + +#include + +#include + +#include + +#include "LogComponent.hpp" + +using namespace roboticslab; + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::getAxes(int *ax) +{ + *ax = Synapse::DATA_POINTS; + return true; +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::positionMoveRaw(int j, double ref) +{ + yCITrace(DEXTRA, id(), "%d %f", j, ref); + CHECK_JOINT(j); + + Synapse::Setpoints setpoints; + getSetpoints(setpoints); + setpoints[j] = ref; + + if (!synapse->writeSetpointList(setpoints)) + { + return false; + } + + setSetpoint(j, ref); + return true; +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::positionMoveRaw(const double * refs) +{ + yCITrace(DEXTRA, id(), ""); + + Synapse::Setpoints setpoints; + std::copy(refs, refs + Synapse::DATA_POINTS, std::begin(setpoints)); + + if (!synapse->writeSetpointList(setpoints)) + { + return false; + } + + setSetpoints(setpoints); + return true; +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::positionMoveRaw(int n_joint, const int * joints, const double * refs) +{ + yCITrace(DEXTRA, id(), "%d", n_joint); + + double encs[Synapse::DATA_POINTS]; + + if (!getEncodersRaw(encs)) + { + return false; + } + + for (int i = 0; i < n_joint; i++) + { + encs[joints[i]] = refs[i]; + } + + return positionMoveRaw(encs); +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::relativeMoveRaw(int j, double delta) +{ + yCITrace(DEXTRA, id(), "%d %f", j, delta); + CHECK_JOINT(j); + + double ref; + + if (!getEncoderRaw(j, &ref)) + { + return false; + } + + return positionMoveRaw(j, ref + delta); +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::relativeMoveRaw(const double * deltas) +{ + yCITrace(DEXTRA, id(), ""); + + double encs[Synapse::DATA_POINTS]; + + if (!getEncodersRaw(encs)) + { + return false; + } + + for (int j = 0; j < Synapse::DATA_POINTS; j++) + { + encs[j] += deltas[j]; + } + + return positionMoveRaw(encs); +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::relativeMoveRaw(int n_joint, const int * joints, const double * deltas) +{ + yCITrace(DEXTRA, id(), "%d", n_joint); + + double encs[Synapse::DATA_POINTS]; + + if (!getEncodersRaw(encs)) + { + return false; + } + + for (int i = 0; i < n_joint; i++) + { + encs[joints[i]] += deltas[i]; + } + + return positionMoveRaw(encs); +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::checkMotionDoneRaw(int j, bool * flag) +{ + yCITrace(DEXTRA, id(), "%d", j); + CHECK_JOINT(j); + *flag = true; + return true; +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::checkMotionDoneRaw(bool * flag) +{ + yCITrace(DEXTRA, id(), ""); + + bool ok = true; + + for (int j = 0; j < Synapse::DATA_POINTS; j++) + { + bool localFlag; + ok &= checkMotionDoneRaw(j, &localFlag); + *flag &= localFlag; + } + + return ok; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::checkMotionDoneRaw(int n_joint, const int * joints, bool * flag) +{ + yCITrace(DEXTRA, id(), "%d", n_joint); + + bool ok = true; + + for (int i = 0; i < n_joint; i++) + { + bool localFlag; + ok &= checkMotionDoneRaw(joints[i], &localFlag); + *flag &= localFlag; + } + + return ok; +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::setRefSpeedRaw(int j, double sp) +{ + yCITrace(DEXTRA, id(), "%d %f", j ,sp); + CHECK_JOINT(j); + return false; +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::setRefSpeedsRaw(const double * spds) +{ + yCITrace(DEXTRA, id(), ""); + + bool ok = true; + + for (int j = 0; j < Synapse::DATA_POINTS; j++) + { + ok &= setRefSpeedRaw(j, spds[j]); + } + + return ok; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::setRefSpeedsRaw(int n_joint, const int * joints, const double * spds) +{ + yCITrace(DEXTRA, id(), ""); + + bool ok = true; + + for (int i = 0; i < n_joint; i++) + { + ok &= setRefSpeedRaw(joints[i], spds[i]); + } + + return ok; +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::setRefAccelerationRaw(int j, double acc) +{ + yCITrace(DEXTRA, id(), "%d %f", j, acc); + CHECK_JOINT(j); + return false; +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::setRefAccelerationsRaw(const double * accs) +{ + yCITrace(DEXTRA, id(), ""); + + bool ok = true; + + for (int j = 0; j < Synapse::DATA_POINTS; j++) + { + ok &= setRefAccelerationRaw(j, accs[j]); + } + + return ok; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::setRefAccelerationsRaw(int n_joint, const int * joints, const double * accs) +{ + yCITrace(DEXTRA, id(), "%d", n_joint); + + bool ok = true; + + for (int i = 0; i < n_joint; i++) + { + ok &= setRefAccelerationRaw(joints[i], accs[i]); + } + + return ok; +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::getRefSpeedRaw(int j, double * ref) +{ + yCITrace(DEXTRA, id(), "%d", j); + CHECK_JOINT(j); + return false; +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::getRefSpeedsRaw(double * spds) +{ + yCITrace(DEXTRA, id(), ""); + + bool ok = true; + + for (int j = 0; j < Synapse::DATA_POINTS; j++) + { + ok &= getRefSpeedRaw(j, &spds[j]); + } + + return ok; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::getRefSpeedsRaw(int n_joint, const int * joints, double * spds) +{ + yCITrace(DEXTRA, id(), "%d", n_joint); + + bool ok = true; + + for (int i = 0; i < n_joint; i++) + { + ok &= getRefSpeedRaw(joints[i], &spds[i]); + } + + return ok; +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::getRefAccelerationRaw(int j, double * acc) +{ + yCITrace(DEXTRA, id(), "%d", j); + CHECK_JOINT(j); + return false; +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::getRefAccelerationsRaw(double * accs) +{ + yCITrace(DEXTRA, id(), ""); + + bool ok = true; + + for (int j = 0; j < Synapse::DATA_POINTS; j++) + { + ok &= getRefAccelerationRaw(j, &accs[j]); + } + + return ok; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::getRefAccelerationsRaw(int n_joint, const int * joints, double * accs) +{ + yCITrace(DEXTRA, id(), "%d", n_joint); + + bool ok = true; + + for (int i = 0; i < n_joint; i++) + { + ok &= getRefAccelerationRaw(joints[i], &accs[i]); + } + + return ok; +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::stopRaw(int j) +{ + yCITrace(DEXTRA, id(), "%d", j); + CHECK_JOINT(j); + return false; +} + +// ----------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::stopRaw() +{ + yCITrace(DEXTRA, id(), ""); + + bool ok = true; + + for (int j = 0; j < Synapse::DATA_POINTS; j++) + { + ok &= stopRaw(j); + } + + return ok; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::stopRaw(int n_joint, const int * joints) +{ + yCITrace(DEXTRA, id(), "%d", n_joint); + + bool ok = true; + + for (int i = 0; i < n_joint; i++) + { + ok &= stopRaw(joints[i]); + } + + return ok; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::getTargetPositionRaw(int joint, double * ref) +{ + yCITrace(DEXTRA, id(), "%d", joint); + return false; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::getTargetPositionsRaw(double * refs) +{ + yCITrace(DEXTRA, id(), ""); + + bool ok = true; + + for (int j = 0; j < Synapse::DATA_POINTS; j++) + { + ok &= getTargetPositionRaw(j, &refs[j]); + } + + return ok; +} + +// ----------------------------------------------------------------------------- + +bool DextraRawControlBoard::getTargetPositionsRaw(int n_joint, const int * joints, double * refs) +{ + yCITrace(DEXTRA, id(), "%d", n_joint); + + bool ok = true; + + for (int i = 0; i < n_joint; i++) + { + ok &= getTargetPositionRaw(joints[i], &refs[i]); + } + + return ok; +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/DextraRawControlBoardLib/IPositionDirectRawImpl.cpp b/libraries/DextraRawControlBoardLib/IPositionDirectRawImpl.cpp new file mode 100644 index 0000000..8fd38d2 --- /dev/null +++ b/libraries/DextraRawControlBoardLib/IPositionDirectRawImpl.cpp @@ -0,0 +1,33 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "DextraRawControlBoard.hpp" + +#include + +#include "LogComponent.hpp" + +using namespace roboticslab; + +// ---------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::setPositionRaw(int j, double ref) +{ + yCTrace(DEXTRA, "%d %f", j, ref); + return positionMoveRaw(j, ref); +} + +// ---------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::setPositionsRaw(const double * refs) +{ + return positionMoveRaw(refs); +} + +// ---------------------------------------------------------------------------------------- + +bool DextraRawControlBoard::setPositionsRaw(int n_joint, const int * joints, const double * refs) +{ + return positionMoveRaw(n_joint, joints, refs); +} + +// ---------------------------------------------------------------------------------------- diff --git a/libraries/DextraRawControlBoardLib/LogComponent.cpp b/libraries/DextraRawControlBoardLib/LogComponent.cpp new file mode 100644 index 0000000..40889c6 --- /dev/null +++ b/libraries/DextraRawControlBoardLib/LogComponent.cpp @@ -0,0 +1,3 @@ +#include "LogComponent.hpp" + +YARP_LOG_COMPONENT(DEXTRA, "rl.DextraRawControlBoard") diff --git a/libraries/DextraRawControlBoardLib/LogComponent.hpp b/libraries/DextraRawControlBoardLib/LogComponent.hpp new file mode 100644 index 0000000..8629b97 --- /dev/null +++ b/libraries/DextraRawControlBoardLib/LogComponent.hpp @@ -0,0 +1,8 @@ +#ifndef __DEXTRA_RAW_CONTROL_BOARD_LOG_COMPONENT_HPP__ +#define __DEXTRA_RAW_CONTROL_BOARD_LOG_COMPONENT_HPP__ + +#include + +YARP_DECLARE_LOG_COMPONENT(DEXTRA) + +#endif // __DEXTRA_RAW_CONTROL_BOARD_LOG_COMPONENT_HPP__ diff --git a/libraries/DextraRawControlBoardLib/README.md b/libraries/DextraRawControlBoardLib/README.md new file mode 100644 index 0000000..bd42f55 --- /dev/null +++ b/libraries/DextraRawControlBoardLib/README.md @@ -0,0 +1,3 @@ +# DextraRawControlBoardLib + +The firmware this library interfaces with is located at: https://github.com/roboticslab-uc3m/Dextra. diff --git a/libraries/DextraRawControlBoardLib/Synapse.cpp b/libraries/DextraRawControlBoardLib/Synapse.cpp new file mode 100644 index 0000000..01a9925 --- /dev/null +++ b/libraries/DextraRawControlBoardLib/Synapse.cpp @@ -0,0 +1,91 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "Synapse.hpp" + +#include +#include + +using namespace roboticslab; + +constexpr unsigned char HEADER = 0x7E; +constexpr unsigned char FOOTER = 0x7E; +constexpr int FLOAT_SIZE = sizeof(Synapse::setpoint_t); +constexpr int MESSAGE_SIZE = (FLOAT_SIZE + 1) * Synapse::DATA_POINTS; +constexpr unsigned char FINGER_ADDRESS[Synapse::DATA_POINTS] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06}; + +const std::array, Synapse::DATA_POINTS> Synapse::LIMITS = {{ + {0.0f, 90.0f}, + {0.0f, 10.0f}, + {0.0f, 20.0f}, + {0.0f, 20.0f}, + {0.0f, 20.0f}, + {0.0f, 20.0f} +}}; + +const std::array Synapse::LABELS = { + "HandAbductor", + "ThumbFinger", + "IndexFinger", + "MiddleFinger", + "RingFinger", + "PinkyFinger" +}; + +void Synapse::configure(void * handle) +{} + +bool Synapse::readDataList(Setpoints & setpoints) +{ + assert(configured); + + unsigned char msg[MESSAGE_SIZE + 2]; // data (MESSAGE_SIZE) + check (1) + footer (1) + + if (!getMessage(msg, HEADER, MESSAGE_SIZE + 2)) + { + return false; + } + + int i = 0; + + for (int j = 0; j < DATA_POINTS; j++) + { + if (msg[i] == FINGER_ADDRESS[j]) + { + i++; + double setpoint; + std::memcpy(&setpoint, msg + i, FLOAT_SIZE); + i += FLOAT_SIZE; + setpoints[j] = setpoint; + } + } + + return true; +} + +bool Synapse::writeSetpointList(const Setpoints & setpoints) +{ + assert(configured); + + unsigned char check = 0x00; + unsigned char msg[MESSAGE_SIZE + 3]; // header (1) + data (MESSAGE_SIZE) + check (1) + footer (1) + + msg[0] = HEADER; + + for (int i = 0; i < DATA_POINTS; i++) + { + const int offset = 1 + (FLOAT_SIZE + 1) * i; + msg[offset] = FINGER_ADDRESS[i]; + std::memcpy(msg + offset + 1, &setpoints[i], FLOAT_SIZE); + check ^= FINGER_ADDRESS[i]; + + for (int k = 0; k < FLOAT_SIZE; k++) + { + check ^= msg[offset + 1 + k]; + } + } + + msg[MESSAGE_SIZE + 3 - 2] = check; + msg[MESSAGE_SIZE + 3 - 1] = FOOTER; + + return sendMessage(msg, MESSAGE_SIZE + 3); +} diff --git a/libraries/DextraRawControlBoardLib/Synapse.hpp b/libraries/DextraRawControlBoardLib/Synapse.hpp new file mode 100644 index 0000000..a5abd97 --- /dev/null +++ b/libraries/DextraRawControlBoardLib/Synapse.hpp @@ -0,0 +1,56 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#ifndef __SYNAPSE_HPP__ +#define __SYNAPSE_HPP__ + +#include + +#include +#include +#include + +namespace roboticslab +{ + +/** + * @ingroup DextraRawControlBoard + * @brief Base comms layer to interface with the Dextra's Arduino board. + * + * C++ port of synapse.py. + */ +class Synapse +{ +public: + static constexpr int DATA_POINTS = 6; ///< Number of controlled motors + + using setpoint_t = float; ///< 4-byte long float + using Setpoints = std::array; ///< Container of Dextra setpoints. + + static const std::array, DATA_POINTS> LIMITS; ///< Joint limits per motor + static const std::array LABELS; ///< String labels that identify each motor + + //! Virtual destructor. + virtual ~Synapse() = default; + + //! Configure handle for the comms interface, if necessary. + virtual void configure(void * handle); + + //! Request current setpoints from the board. + bool readDataList(Setpoints & setpoints); + + //! Write new setpoints to the board. + bool writeSetpointList(const Setpoints & setpoints); + +protected: + //! Retrieve single message via comms interface. + virtual bool getMessage(unsigned char * msg, char stopByte, int size) = 0; + + //! Forward generated message via comms interface. + virtual bool sendMessage(unsigned char * msg, int size) = 0; + + bool configured = false; +}; + +} // namespace roboticslab + +#endif // __SYNAPSE_HPP__ diff --git a/libraries/YarpPlugins/CMakeLists.txt b/libraries/YarpPlugins/CMakeLists.txt new file mode 100644 index 0000000..f23a3dc --- /dev/null +++ b/libraries/YarpPlugins/CMakeLists.txt @@ -0,0 +1,3 @@ +# YARP devices. +add_subdirectory(DextraCanControlBoard) +add_subdirectory(DextraSerialControlBoard) diff --git a/libraries/YarpPlugins/DextraCanControlBoard/CMakeLists.txt b/libraries/YarpPlugins/DextraCanControlBoard/CMakeLists.txt new file mode 100644 index 0000000..0502fc7 --- /dev/null +++ b/libraries/YarpPlugins/DextraCanControlBoard/CMakeLists.txt @@ -0,0 +1,25 @@ +yarp_prepare_plugin(DextraCanControlBoard + CATEGORY device + TYPE roboticslab::DextraCanControlBoard + INCLUDE DextraCanControlBoard.hpp + DEFAULT ON + DEPENDS "TARGET ROBOTICSLAB::CanBusSharerLib;ENABLE_DextraRawControlBoardLib") + +if(NOT SKIP_DextraCanControlBoard) + + yarp_add_plugin(DextraCanControlBoard DextraCanControlBoard.hpp + DeviceDriverImpl.cpp + ICanBusSharerImpl.cpp + CanSynapse.cpp) + + target_link_libraries(DextraCanControlBoard ROBOTICSLAB::DextraRawControlBoardLib + YARP::YARP_os + YARP::YARP_dev + ROBOTICSLAB::CanBusSharerLib) + + yarp_install(TARGETS DextraCanControlBoard + LIBRARY DESTINATION ${DEXTRA-YARP-DEVICES_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${DEXTRA-YARP-DEVICES_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${DEXTRA-YARP-DEVICES_PLUGIN_MANIFESTS_INSTALL_DIR}) + +endif() diff --git a/libraries/YarpPlugins/DextraCanControlBoard/CanSynapse.cpp b/libraries/YarpPlugins/DextraCanControlBoard/CanSynapse.cpp new file mode 100644 index 0000000..a6d566a --- /dev/null +++ b/libraries/YarpPlugins/DextraCanControlBoard/CanSynapse.cpp @@ -0,0 +1,42 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "DextraCanControlBoard.hpp" + +#include +#include +#include + +using namespace roboticslab; + +CanSynapse::CanSynapse(unsigned int _canId) + : canId(_canId), + sender(nullptr) +{} + +void CanSynapse::configure(void * handle) +{ + sender = static_cast(handle); + configured = true; +} + +bool CanSynapse::getMessage(unsigned char * msg, char stopByte, int size) +{ + return false; +} + +bool CanSynapse::sendMessage(unsigned char * msg, int size) +{ + const int n = std::ceil(size / 8.0f); + + for (int i = 0; i < n; i++) + { + const unsigned int bytes = ((i + 1) * 8 <= size) ? 8 : size % 8; + + if (sender && !sender->prepareMessage({canId, bytes, msg})) + { + return false; + } + } + + return true; +} diff --git a/libraries/YarpPlugins/DextraCanControlBoard/DeviceDriverImpl.cpp b/libraries/YarpPlugins/DextraCanControlBoard/DeviceDriverImpl.cpp new file mode 100644 index 0000000..08e507c --- /dev/null +++ b/libraries/YarpPlugins/DextraCanControlBoard/DeviceDriverImpl.cpp @@ -0,0 +1,43 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "DextraCanControlBoard.hpp" + +#include +#include +#include + +using namespace roboticslab; + +namespace +{ + YARP_LOG_COMPONENT(DEXTRA, "rl.DextraCanControlBoard") +} + +// ----------------------------------------------------------------------------- + +bool DextraCanControlBoard::open(yarp::os::Searchable & config) +{ + canId = config.check("canId", yarp::os::Value(0), "can bus ID").asInt32(); + + if (canId == 0) + { + yCError(DEXTRA) << "Could not create device with canId 0"; + return false; + } + + yarp::dev::DeviceDriver::setId("ID" + std::to_string(canId)); + + acquireSynapseHandle(new CanSynapse(canId)); + + return DextraRawControlBoard::open(config); // parses axisPrefix +} + +// ----------------------------------------------------------------------------- + +bool DextraCanControlBoard::close() +{ + destroySynapse(); + return true; +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/DextraCanControlBoard/DextraCanControlBoard.hpp b/libraries/YarpPlugins/DextraCanControlBoard/DextraCanControlBoard.hpp new file mode 100644 index 0000000..cb14d42 --- /dev/null +++ b/libraries/YarpPlugins/DextraCanControlBoard/DextraCanControlBoard.hpp @@ -0,0 +1,72 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#ifndef __DEXTRA_CAN_CONTROL_BOARD_HPP__ +#define __DEXTRA_CAN_CONTROL_BOARD_HPP__ + +#include + +#include "DextraRawControlBoard.hpp" +#include "Synapse.hpp" +#include "ICanBusSharer.hpp" + +namespace roboticslab +{ + +/** + * @ingroup YarpPlugins + * @defgroup DextraCanControlBoard + * @brief Contains roboticslab::DextraCanControlBoard. + */ + +/** + * @ingroup DextraCanControlBoard + * @brief Synapse interface for a CAN network. + */ +class CanSynapse : public Synapse +{ +public: + //! Constructor. + CanSynapse(unsigned int canId); + + void configure(void * handle) override; + +protected: + bool getMessage(unsigned char * msg, char stopByte, int size) override; + bool sendMessage(unsigned char * msg, int size) override; + +private: + unsigned int canId; + ICanSenderDelegate * sender; +}; + +/** + * @ingroup DextraCanControlBoard + * @brief Implementation of a CAN-based raw control board for a Dextra hand. + */ +class DextraCanControlBoard : public DextraRawControlBoard, + public ICanBusSharer +{ +public: + DextraCanControlBoard() : canId(0) + { } + + // --------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp --------- + bool open(yarp::os::Searchable & config) override; + bool close() override; + + // --------- ICanBusSharer declarations. Implementation in ICanBusSharerImpl.cpp --------- + unsigned int getId() override; + bool initialize() override; + bool finalize() override; + bool notifyMessage(const can_message & message) override; + bool registerSender(ICanSenderDelegate * sender) override; + bool synchronize(double timestamp) override; + +protected: + unsigned int canId; + DextraRawControlBoard raw; +}; + +} // namespace roboticslab + +#endif // __DEXTRA_CAN_CONTROL_BOARD_HPP__ diff --git a/libraries/YarpPlugins/DextraCanControlBoard/ICanBusSharerImpl.cpp b/libraries/YarpPlugins/DextraCanControlBoard/ICanBusSharerImpl.cpp new file mode 100644 index 0000000..b7ee6e7 --- /dev/null +++ b/libraries/YarpPlugins/DextraCanControlBoard/ICanBusSharerImpl.cpp @@ -0,0 +1,36 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "DextraCanControlBoard.hpp" + +using namespace roboticslab; + +unsigned int DextraCanControlBoard::getId() +{ + return canId; +} + +bool DextraCanControlBoard::initialize() +{ + return true; +} + +bool DextraCanControlBoard::finalize() +{ + return true; +} + +bool DextraCanControlBoard::registerSender(ICanSenderDelegate * sender) +{ + synapse->configure(sender); + return true; +} + +bool DextraCanControlBoard::notifyMessage(const can_message & message) +{ + return true; +} + +bool DextraCanControlBoard::synchronize(double timestamp) +{ + return true; +} diff --git a/libraries/YarpPlugins/DextraCanControlBoard/README.md b/libraries/YarpPlugins/DextraCanControlBoard/README.md new file mode 100644 index 0000000..34cb932 --- /dev/null +++ b/libraries/YarpPlugins/DextraCanControlBoard/README.md @@ -0,0 +1,4 @@ +# DextraCanControlBoard + +## Requirements +Depends on https://github.com/roboticslab-uc3m/yarp-devices. diff --git a/libraries/YarpPlugins/DextraSerialControlBoard/CMakeLists.txt b/libraries/YarpPlugins/DextraSerialControlBoard/CMakeLists.txt new file mode 100644 index 0000000..00c8609 --- /dev/null +++ b/libraries/YarpPlugins/DextraSerialControlBoard/CMakeLists.txt @@ -0,0 +1,24 @@ +yarp_prepare_plugin(DextraSerialControlBoard + CATEGORY device + TYPE roboticslab::DextraSerialControlBoard + INCLUDE DextraSerialControlBoard.hpp + DEFAULT ON + DEPENDS ENABLE_DextraRawControlBoardLib + EXTRA_CONFIG WRAPPER=controlBoard_nws_yarp) + +if(NOT SKIP_DextraSerialControlBoard) + + yarp_add_plugin(DextraSerialControlBoard DextraSerialControlBoard.hpp + DeviceDriverImpl.cpp + SerialSynapse.cpp) + + target_link_libraries(DextraSerialControlBoard ROBOTICSLAB::DextraRawControlBoardLib + YARP::YARP_os + YARP::YARP_dev) + + yarp_install(TARGETS DextraSerialControlBoard + LIBRARY DESTINATION ${DEXTRA-YARP-DEVICES_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${DEXTRA-YARP-DEVICES_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${DEXTRA-YARP-DEVICES_PLUGIN_MANIFESTS_INSTALL_DIR}) + +endif() diff --git a/libraries/YarpPlugins/DextraSerialControlBoard/DeviceDriverImpl.cpp b/libraries/YarpPlugins/DextraSerialControlBoard/DeviceDriverImpl.cpp new file mode 100644 index 0000000..bdf86a8 --- /dev/null +++ b/libraries/YarpPlugins/DextraSerialControlBoard/DeviceDriverImpl.cpp @@ -0,0 +1,66 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "DextraSerialControlBoard.hpp" + +#include +#include +#include +#include + +using namespace roboticslab; + +namespace +{ + YARP_LOG_COMPONENT(DEXTRA, "rl.DextraSerialControlBoard") +} + +constexpr auto DEFAULT_PORT = "/dev/ttyACM0"; // also /dev/ttyUSB0 + +// ----------------------------------------------------------------------------- + +bool DextraSerialControlBoard::open(yarp::os::Searchable & config) +{ + std::string port = config.check("port", yarp::os::Value(DEFAULT_PORT), "serial port").asString(); + + // Should match https://github.com/roboticslab-uc3m/Dextra/blob/master/Control/synapse.py + // See also https://github.com/pyserial/pyserial/blob/master/serial/serialposix.py + + yarp::os::Property serialOptions { + {"device", yarp::os::Value("serialport")}, + {"comport", yarp::os::Value(port)}, + {"baudrate", yarp::os::Value(115200)}, + {"paritymode", yarp::os::Value("NONE")}, + {"databits", yarp::os::Value(8)}, + }; + + yCDebug(DEXTRA) << "Serial device options:" << serialOptions.toString(); + + if (!serialDevice.open(serialOptions)) + { + yCError(DEXTRA) << "Unable to open" << serialOptions.find("device").asString() << "device"; + return false; + } + + yarp::dev::ISerialDevice * iSerialDevice; + + if (!serialDevice.view(iSerialDevice)) + { + yCError(DEXTRA) << "Unable to view iSerialDevice"; + return false; + } + + raw.acquireSynapseHandle(new SerialSynapse(iSerialDevice)); + + return raw.open(config); // parses axisPrefix +} + +// ----------------------------------------------------------------------------- + +bool DextraSerialControlBoard::close() +{ + raw.destroySynapse(); + serialDevice.close(); + return true; +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/DextraSerialControlBoard/DextraSerialControlBoard.hpp b/libraries/YarpPlugins/DextraSerialControlBoard/DextraSerialControlBoard.hpp new file mode 100644 index 0000000..e3d0417 --- /dev/null +++ b/libraries/YarpPlugins/DextraSerialControlBoard/DextraSerialControlBoard.hpp @@ -0,0 +1,228 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#ifndef __DEXTRA_SERIAL_CONTROL_BOARD_HPP__ +#define __DEXTRA_SERIAL_CONTROL_BOARD_HPP__ + +#include + +#include +#include +#include +#include +#include +#include +#include +#if YARP_VERSION_COMPARE(<, 3, 9, 0) +# include +#endif + +#include +#include + +#include "DextraRawControlBoard.hpp" +#include "Synapse.hpp" + +namespace roboticslab +{ + +/** + * @ingroup YarpPlugins + * @defgroup DextraSerialControlBoard + * @brief Contains roboticslab::DextraSerialControlBoard. + */ + +/** + * @ingroup DextraSerialControlBoard + * @brief Synapse interface for a serial bus. + */ +class SerialSynapse : public Synapse +{ +public: + //! Constructor. + SerialSynapse(yarp::dev::ISerialDevice * iSerialDevice); + +protected: + bool getMessage(unsigned char * msg, char stopByte, int size) override; + bool sendMessage(unsigned char * msg, int size) override; + +private: + yarp::dev::ISerialDevice * iSerialDevice; +}; + +/** + * @ingroup DextraSerialControlBoard + * @brief Implementation of a serial-based stand-alone control board for a Dextra hand. + */ +class DextraSerialControlBoard : public yarp::dev::DeviceDriver, + public yarp::dev::IAxisInfo, + public yarp::dev::IControlLimits, + public yarp::dev::IControlMode, + public yarp::dev::IEncodersTimed, + public yarp::dev::IPositionControl, + public yarp::dev::IPositionDirect +#if YARP_VERSION_COMPARE(<, 3, 9, 0) + , public yarp::dev::IVelocityControl +#endif +{ +public: + // --------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp --------- + + virtual bool open(yarp::os::Searchable & config) override; + virtual bool close() override; + + // --------- IAxisInfo declarations --------- + + virtual bool getAxisName(int axis, std::string & name) override + { return raw.getAxisNameRaw(axis, name); } + virtual bool getJointType(int axis, yarp::dev::JointTypeEnum & type) override + { return raw.getJointTypeRaw(axis, type); } + + // --------- IControlLimits declarations --------- + + virtual bool setLimits(int axis, double min, double max) override + { return raw.setLimitsRaw(axis, min, max); } + virtual bool getLimits(int axis, double * min, double * max) override + { return raw.getLimitsRaw(axis, min, max); } + virtual bool setVelLimits(int axis, double min, double max) override + { return raw.setVelLimitsRaw(axis, min, max); } + virtual bool getVelLimits(int axis, double * min, double * max) override + { return raw.getVelLimitsRaw(axis, min, max); } + + // --------- IControlMode declarations --------- + + virtual bool getControlMode(int j, int * mode) override + { return raw.getControlModeRaw(j, mode); } + virtual bool getControlModes(int * modes) override + { return raw.getControlModesRaw(modes); } + virtual bool getControlModes(int n_joint, const int * joints, int * modes) override + { return raw.getControlModesRaw(n_joint, joints, modes); } + virtual bool setControlMode(int j, int mode) override + { return raw.setControlModeRaw(j, mode); } + virtual bool setControlModes(int * modes) override + { return raw.setControlModesRaw(modes); } + virtual bool setControlModes(int n_joint, const int * joints, int * modes) override + { return raw.setControlModesRaw(n_joint, joints, modes); } + + // ---------- IEncoders declarations ---------- + + virtual bool resetEncoder(int j) override + { return raw.resetEncoderRaw(j); } + virtual bool resetEncoders() override + { return raw.resetEncodersRaw(); } + virtual bool setEncoder(int j, double val) override + { return raw.setEncoderRaw(j, val); } + virtual bool setEncoders(const double *vals) override + { return raw.setEncodersRaw(vals); } + virtual bool getEncoder(int j, double * v) override + { return raw.getEncoderRaw(j, v); } + virtual bool getEncoders(double *encs) override + { return raw.getEncodersRaw(encs); } + virtual bool getEncoderSpeed(int j, double * sp) override + { return raw.getEncoderSpeedRaw(j, sp); } + virtual bool getEncoderSpeeds(double * spds) override + { return raw.getEncoderSpeedsRaw(spds); } + virtual bool getEncoderAcceleration(int j, double * spds) override + { return raw.getEncoderAccelerationRaw(j, spds); } + virtual bool getEncoderAccelerations(double * accs) override + { return raw.getEncoderAccelerationsRaw(accs); } + + // ---------- IEncodersTimed declarations ---------- + + virtual bool getEncoderTimed(int j, double * encs, double * time) override + { return raw.getEncoderTimedRaw(j, encs, time); } + virtual bool getEncodersTimed(double * encs, double * time) override + { return raw.getEncodersRaw(encs); } + + // ------- IPositionControl declarations ------- + + virtual bool getAxes(int * ax) override + { return raw.getAxes(ax); } + virtual bool positionMove(int j, double ref) override + { return raw.positionMoveRaw(j, ref); } + virtual bool positionMove(const double * refs) override + { return raw.positionMoveRaw(refs); } + virtual bool positionMove(int n_joint, const int * joints, const double * refs) override + { return raw.positionMoveRaw(n_joint, joints, refs); } + virtual bool relativeMove(int j, double delta) override + { return raw.relativeMoveRaw(j, delta); } + virtual bool relativeMove(const double * deltas) override + { return raw.relativeMoveRaw(deltas); } + virtual bool relativeMove(int n_joint, const int * joints, const double * deltas) override + { return raw.relativeMoveRaw(n_joint, joints, deltas); } + virtual bool checkMotionDone(int j, bool * flag) override + { return raw.checkMotionDoneRaw(j, flag); } + virtual bool checkMotionDone(bool * flag) override + { return raw.checkMotionDoneRaw(flag); } + virtual bool checkMotionDone(int n_joint, const int * joints, bool * flag) override + { return raw.checkMotionDoneRaw(n_joint, joints, flag); } + virtual bool setRefSpeed(int j, double sp) override + { return raw.setRefSpeedRaw(j, sp); } + virtual bool setRefSpeeds(const double * spds) override + { return raw.setRefSpeedsRaw(spds); } + virtual bool setRefSpeeds(int n_joint, const int * joints, const double * spds) override + { return raw.setRefSpeedsRaw(n_joint, joints, spds); } + virtual bool setRefAcceleration(int j, double acc) override + { return raw.setRefAccelerationRaw(j, acc); } + virtual bool setRefAccelerations(const double * accs) override + { return raw.setRefAccelerationsRaw(accs); } + virtual bool setRefAccelerations(int n_joint, const int * joints, const double * accs) override + { return raw.setRefAccelerationsRaw(n_joint, joints, accs); } + virtual bool getRefSpeed(int j, double * ref) override + { return raw.getRefSpeedRaw(j, ref); } + virtual bool getRefSpeeds(double * spds) override + { return raw.getRefSpeedsRaw(spds); } + virtual bool getRefSpeeds(int n_joint, const int * joints, double * spds) override + { return raw.getRefSpeedsRaw(n_joint, joints, spds); } + virtual bool getRefAcceleration(int j, double * acc) override + { return raw.getRefAccelerationRaw(j, acc); } + virtual bool getRefAccelerations(double * accs) override + { return raw.getRefAccelerationsRaw(accs); } + virtual bool getRefAccelerations(int n_joint, const int * joints, double * accs) override + { return raw.getRefAccelerationsRaw(n_joint, joints, accs); } + virtual bool stop(int j) override + { return raw.stopRaw(j); } + virtual bool stop() override + { return raw.stopRaw(); } + virtual bool stop(int n_joint, const int * joints) override + { return raw.stopRaw(n_joint, joints); } + virtual bool getTargetPosition(int joint, double * ref) override + { return raw.getTargetPositionsRaw(joint, &joint, ref); } + virtual bool getTargetPositions(double * refs) override + { return raw.getTargetPositionsRaw(refs); } + virtual bool getTargetPositions(int n_joint, const int * joints, double * refs) override + { return raw.getTargetPositionsRaw(n_joint, joints, refs); } + + // ------- IPositionDirect declarations ------- + + virtual bool setPosition(int j, double ref) override + { return raw.setPositionRaw(j, ref); } + virtual bool setPositions(const double * refs) override + { return raw.setPositionsRaw(refs); } + virtual bool setPositions(int n_joint, const int * joints, const double * refs) override + { return raw.setPositionsRaw(n_joint, joints, refs); } + +#if YARP_VERSION_COMPARE(<, 3, 9, 0) + // --------- IVelocityControl declarations --------- + + virtual bool velocityMove(int j, double sp) override + { return raw.velocityMoveRaw(j, sp); } + virtual bool velocityMove(const double * sp) override + { return raw.velocityMoveRaw(sp); } + virtual bool velocityMove(int n_joint, const int * joints, const double * spds) override + { return raw.velocityMoveRaw(n_joint, joints, spds); } + virtual bool getRefVelocity(int joint, double * vel) override + { return raw.getRefVelocityRaw(joint, vel); } + virtual bool getRefVelocities(double * vels) override + { return raw.getRefVelocitiesRaw(vels); } + virtual bool getRefVelocities(int n_joint, const int * joints, double * vels) override + { return raw.getRefVelocitiesRaw(n_joint, joints, vels); } +#endif + +protected: + DextraRawControlBoard raw; + yarp::dev::PolyDriver serialDevice; +}; + +} // namespace roboticslab + +#endif // __DEXTRA_SERIAL_CONTROL_BOARD_HPP__ diff --git a/libraries/YarpPlugins/DextraSerialControlBoard/README.md b/libraries/YarpPlugins/DextraSerialControlBoard/README.md new file mode 100644 index 0000000..9f715d7 --- /dev/null +++ b/libraries/YarpPlugins/DextraSerialControlBoard/README.md @@ -0,0 +1,9 @@ +# DextraSerialControlBoard + +``` +yarpdev --device DextraSerialControlBoard --port /dev/ttyACM0 --name /dextra/rightHand +``` + +``` +yarpmotorgui --robot dextra --parts "(rightHand)" +``` diff --git a/libraries/YarpPlugins/DextraSerialControlBoard/SerialSynapse.cpp b/libraries/YarpPlugins/DextraSerialControlBoard/SerialSynapse.cpp new file mode 100644 index 0000000..7465da0 --- /dev/null +++ b/libraries/YarpPlugins/DextraSerialControlBoard/SerialSynapse.cpp @@ -0,0 +1,23 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "DextraSerialControlBoard.hpp" + +using namespace roboticslab; + +SerialSynapse::SerialSynapse(yarp::dev::ISerialDevice * _iSerialDevice) + : iSerialDevice(_iSerialDevice) +{ + configured = true; +} + +bool SerialSynapse::getMessage(unsigned char * msg, char stopByte, int size) +{ + char chr; + while (!iSerialDevice->receiveChar(chr) || chr != stopByte) {} + return iSerialDevice->receiveBytes(msg, size) != 0; +} + +bool SerialSynapse::sendMessage(unsigned char * msg, int size) +{ + return iSerialDevice->send(reinterpret_cast(msg), size); +}