From 1d2d320ff6f1706a72709a97e4323abc895a0934 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Sun, 3 Dec 2023 20:55:00 +0100 Subject: [PATCH] Transfer AMOR-related code to amor-yarp-devices --- .github/workflows/ci.yml | 15 +- CMakeLists.txt | 1 - doc/yarp-devices-install.md | 1 - .../AmorControlBoard/AmorControlBoard.cpp | 60 -- .../AmorControlBoard/AmorControlBoard.hpp | 181 ------ .../AmorControlBoard/CMakeLists.txt | 41 -- .../AmorControlBoard/DeviceDriverImpl.cpp | 124 ----- .../AmorControlBoard/IAxisInfoImpl.cpp | 69 --- .../AmorControlBoard/IControlLimitsImpl.cpp | 85 --- .../AmorControlBoard/IControlModeImpl.cpp | 110 ---- .../AmorControlBoard/ICurrentControlImpl.cpp | 224 -------- .../AmorControlBoard/IEncodersImpl.cpp | 170 ------ .../AmorControlBoard/IPositionControlImpl.cpp | 519 ------------------ .../AmorControlBoard/IVelocityControlImpl.cpp | 152 ----- .../AmorControlBoard/LogComponent.cpp | 3 - .../AmorControlBoard/LogComponent.hpp | 8 - .../YarpPlugins/AmorControlBoard/README.md | 6 - libraries/YarpPlugins/CMakeLists.txt | 1 - .../PortMonitorPlugins/CMakeLists.txt | 1 - .../PortMonitorPlugins/lua/CMakeLists.txt | 2 - .../lua/amor_sensors_modifier.lua | 488 ---------------- 21 files changed, 1 insertion(+), 2260 deletions(-) delete mode 100644 libraries/YarpPlugins/AmorControlBoard/AmorControlBoard.cpp delete mode 100644 libraries/YarpPlugins/AmorControlBoard/AmorControlBoard.hpp delete mode 100644 libraries/YarpPlugins/AmorControlBoard/CMakeLists.txt delete mode 100644 libraries/YarpPlugins/AmorControlBoard/DeviceDriverImpl.cpp delete mode 100644 libraries/YarpPlugins/AmorControlBoard/IAxisInfoImpl.cpp delete mode 100644 libraries/YarpPlugins/AmorControlBoard/IControlLimitsImpl.cpp delete mode 100644 libraries/YarpPlugins/AmorControlBoard/IControlModeImpl.cpp delete mode 100644 libraries/YarpPlugins/AmorControlBoard/ICurrentControlImpl.cpp delete mode 100644 libraries/YarpPlugins/AmorControlBoard/IEncodersImpl.cpp delete mode 100644 libraries/YarpPlugins/AmorControlBoard/IPositionControlImpl.cpp delete mode 100644 libraries/YarpPlugins/AmorControlBoard/IVelocityControlImpl.cpp delete mode 100644 libraries/YarpPlugins/AmorControlBoard/LogComponent.cpp delete mode 100644 libraries/YarpPlugins/AmorControlBoard/LogComponent.hpp delete mode 100644 libraries/YarpPlugins/AmorControlBoard/README.md delete mode 100644 libraries/YarpPlugins/PortMonitorPlugins/lua/CMakeLists.txt delete mode 100644 libraries/YarpPlugins/PortMonitorPlugins/lua/amor_sensors_modifier.lua diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 01e64d8b3..28a96259e 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -75,13 +75,6 @@ jobs: - name: Download Peak CAN driver run: wget -O- -q https://www.peak-system.com/fileadmin/media/linux/files/peak-linux-driver-$PEAK_DRIVER_VER.tar.gz | tar -C .deps -xzf - - - name: Check out amor-api - uses: actions/checkout@v3 - with: - repository: roboticslab-uc3m/amor-api - token: ${{secrets.AMOR_API}} - path: .deps/amor-api - - name: Check out jr3pci-linux uses: actions/checkout@v3 with: @@ -89,7 +82,7 @@ jobs: path: .deps/jr3pci-linux - name: Install dependencies via apt - run: sudo apt-get update && sudo apt-get install -qq ccache libboost-thread-dev libeigen3-dev intltool libxml2-dev libglib2.0-dev libusb-1.0-0-dev libspnav-dev libxwiimote-dev libgtest-dev + run: sudo apt-get update && sudo apt-get install -qq ccache intltool libxml2-dev libglib2.0-dev libusb-1.0-0-dev libspnav-dev libxwiimote-dev libgtest-dev - name: Set up CMake uses: jwlawson/actions-setup-cmake@v1 @@ -137,12 +130,6 @@ jobs: sudo make -C lib install sudo cp -f driver/pcan*.h /usr/include - - name: Build amor-api - run: | - cmake -S .deps/amor-api -B .deps/amor-api/build $CMAKE_CCACHE_LAUNCHER -DENABLE_udev_rules=OFF - cmake --build .deps/amor-api/build - sudo cmake --install .deps/amor-api/build - - name: Install jr3pci-linux header run: sudo make -C .deps/jr3pci-linux install-header diff --git a/CMakeLists.txt b/CMakeLists.txt index 2af48824a..3b4d70aee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,7 +31,6 @@ find_package(YCM 0.11 REQUIRED) find_package(YARP 3.7 REQUIRED COMPONENTS os dev sig) # Soft dependencies. -find_package(AMOR_API QUIET) find_package(ARAVIS 0.4 QUIET) find_package(Doxygen QUIET) find_package(GTestSources 1.8 QUIET) diff --git a/doc/yarp-devices-install.md b/doc/yarp-devices-install.md index 9b4d698d7..7024cf1e0 100644 --- a/doc/yarp-devices-install.md +++ b/doc/yarp-devices-install.md @@ -9,7 +9,6 @@ For unit testing, you'll need the googletest source package. Refer to [Install g ### Components with known additional/specific dependencies -- [../libraries/YarpPlugins/AmorControlBoard](../libraries/YarpPlugins/AmorControlBoard#requirements) - [../libraries/YarpPlugins/AravisGigE](../libraries/YarpPlugins/AravisGigE#requirements) - [../libraries/YarpPlugins/CanBusHico](../libraries/YarpPlugins/CanBusHico#requirements) - [../libraries/YarpPlugins/CanBusPeak](../libraries/YarpPlugins/CanBusPeak#requirements) diff --git a/libraries/YarpPlugins/AmorControlBoard/AmorControlBoard.cpp b/libraries/YarpPlugins/AmorControlBoard/AmorControlBoard.cpp deleted file mode 100644 index bbf31e689..000000000 --- a/libraries/YarpPlugins/AmorControlBoard/AmorControlBoard.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- - -#include "AmorControlBoard.hpp" - -#define _USE_MATH_DEFINES -#include - -#include - -#include "LogComponent.hpp" - -using namespace roboticslab; - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::indexWithinRange(const int& idx) -{ - if (idx >= AMOR_NUM_JOINTS) - { - yCError(AMOR, "Index out of range: %d >= %d", idx, AMOR_NUM_JOINTS); - return false; - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::batchWithinRange(const int& n_joint) -{ - if (n_joint == 0) - { - yCWarning(AMOR, "Passed array of size (n_joint) equal to zero"); - return true; - } - - if (n_joint < 0 || n_joint > AMOR_NUM_JOINTS) - { - yCError(AMOR, "n_joint out of range (< 0 or > %d): %d", AMOR_NUM_JOINTS, n_joint); - return false; - } - - return true; -} - -// ----------------------------------------------------------------------------- - -double AmorControlBoard::toDeg(double rad) -{ - return rad * 180 / M_PI; -} - -// ----------------------------------------------------------------------------- - -double AmorControlBoard::toRad(double deg) -{ - return deg * M_PI / 180; -} - -// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/AmorControlBoard/AmorControlBoard.hpp b/libraries/YarpPlugins/AmorControlBoard/AmorControlBoard.hpp deleted file mode 100644 index 099602788..000000000 --- a/libraries/YarpPlugins/AmorControlBoard/AmorControlBoard.hpp +++ /dev/null @@ -1,181 +0,0 @@ -// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- - -#ifndef __AMOR_CONTROL_BOARD_HPP__ -#define __AMOR_CONTROL_BOARD_HPP__ - -#include -#include - -#include -#include - -#include - -namespace roboticslab -{ - -/** - * @ingroup YarpPlugins - * @defgroup AmorControlBoard - * @brief Contains roboticslab::AmorControlBoard. - */ - -/** -* @ingroup AmorControlBoard -* @brief Implements several yarp::dev:: control board interfaces. -*/ -class AmorControlBoard : public yarp::dev::DeviceDriver, - public yarp::dev::IAxisInfo, - public yarp::dev::IControlLimits, - public yarp::dev::IControlMode, - public yarp::dev::ICurrentControl, - public yarp::dev::IEncodersTimed, - public yarp::dev::IPositionControl, - public yarp::dev::IVelocityControl -{ -public: - - ~AmorControlBoard() override - { close(); } - - // -------- DeviceDriver declarations. Implementation in IDeviceDriverImpl.cpp -------- - - bool open(yarp::os::Searchable& config) override; - bool close() override; - - // ------- IPositionControl declarations. Implementation in IPositionControlImpl.cpp ------- - - bool getAxes(int *ax) override; - bool positionMove(int j, double ref) override; - bool positionMove(const double *refs) override; - bool relativeMove(int j, double delta) override; - bool relativeMove(const double *deltas) override; - bool checkMotionDone(int j, bool *flag) override; - bool checkMotionDone(bool *flag) override; - bool setRefSpeed(int j, double sp) override; - bool setRefSpeeds(const double *spds) override; - bool setRefAcceleration(int j, double acc) override; - bool setRefAccelerations(const double *accs) override; - bool getRefSpeed(int j, double *ref) override; - bool getRefSpeeds(double *spds) override; - bool getRefAcceleration(int j, double *acc) override; - bool getRefAccelerations(double *accs) override; - bool stop(int j) override; - bool stop() override; - bool positionMove(int n_joint, const int *joints, const double *refs) override; - bool relativeMove(int n_joint, const int *joints, const double *deltas) override; - bool checkMotionDone(int n_joint, const int *joints, bool *flags) override; - bool setRefSpeeds(int n_joint, const int *joints, const double *spds) override; - bool setRefAccelerations(int n_joint, const int *joints, const double *accs) override; - bool getRefSpeeds(int n_joint, const int *joints, double *spds) override; - bool getRefAccelerations(int n_joint, const int *joints, double *accs) override; - bool stop(int n_joint, const int *joints) override; - bool getTargetPosition(int joint, double *ref) override; - bool getTargetPositions(double *refs) override; - bool getTargetPositions(int n_joint, const int *joints, double *refs) override; - - // ---------- IEncoders declarations. Implementation in IEncodersImpl.cpp ---------- - - bool resetEncoder(int j) override; - bool resetEncoders() override; - bool setEncoder(int j, double val) override; - bool setEncoders(const double *vals) override; - bool getEncoder(int j, double *v) override; - bool getEncoders(double *encs) override; - bool getEncoderSpeed(int j, double *sp) override; - bool getEncoderSpeeds(double *spds) override; - bool getEncoderAcceleration(int j, double *spds) override; - bool getEncoderAccelerations(double *accs) override; - - // --------- IEncodersTimed declarations. Implementation in IEncodersTimedImpl.cpp --------- - - bool getEncodersTimed(double *encs, double *time) override; - bool getEncoderTimed(int j, double *encs, double *time) override; - - // --------- IVelocityControl Declarations. Implementation in IVelocityControlImpl.cpp --------- - - bool velocityMove(int j, double sp) override; - bool velocityMove(const double *sp) override; - bool velocityMove(const int n_joint, const int *joints, const double *spds) override; - bool getRefVelocity(int joint, double *vel) override; - bool getRefVelocities(double *vels) override; - bool getRefVelocities(int n_joint, const int *joints, double *vels) override; - - // --------- IControlLimits declarations. Implementation in IControlLimitsImpl.cpp --------- - - bool setLimits(int axis, double min, double max) override; - bool getLimits(int axis, double *min, double *max) override; - bool setVelLimits(int axis, double min, double max) override; - bool getVelLimits(int axis, double *min, double *max) override; - - // --------- IControlMode declarations. Implementation in IControlModeImpl.cpp --------- - - bool getControlMode(int j, int *mode) override; - bool getControlModes(int *modes) override; - bool getControlModes(int n_joint, const int *joints, int *modes) override; - bool setControlMode(int j, int mode) override; - bool setControlModes(int n_joint, const int *joints, int *modes) override; - bool setControlModes(int *modes) override; - - // -------- IAxisInfo declarations. Implementation in IAxisInfoImpl.cpp -------- - - bool getAxisName(int axis, std::string& name) override; - bool getJointType(int axis, yarp::dev::JointTypeEnum& type) override; - - // --------- ICurrentControl Declarations. Implementation in ICurrentControlImpl.cpp --------- - - bool getNumberOfMotors(int *ax) override; - bool getCurrent(int m, double *curr) override; - bool getCurrents(double *currs) override; - bool getCurrentRange(int m, double *min, double *max) override; - bool getCurrentRanges(double *min, double *max) override; - bool setRefCurrents(const double *currs) override; - bool setRefCurrent(int m, double curr) override; - bool setRefCurrents(int n_motor, const int *motors, const double *currs) override; - bool getRefCurrents(double *currs) override; - bool getRefCurrent(int m, double *curr) override; - - // ------------------------------- Protected ------------------------------------- - -protected: - - /** - * Check if index is within range (referred to driver vector size). - * @param idx index to check. - * @return true/false on success/failure. - */ - bool indexWithinRange(const int& idx); - - /** - * Check if number of joints is within range. - * @param n_joint index to check. - * @return true/false on success/failure. - */ - bool batchWithinRange(const int& n_joint); - - /** - * Convert from radians to degrees. - * @param rad radians - * @return degrees - */ - static double toDeg(double rad); - - /** - * Convert from degrees to radians. - * @param deg degrees - * @return radians - */ - static double toRad(double deg); - -private: - - AMOR_HANDLE handle {AMOR_INVALID_HANDLE}; - mutable std::mutex handleMutex; - yarp::dev::PolyDriver cartesianControllerDevice; - bool usingCartesianController {false}; - int controlMode {VOCAB_CM_POSITION}; -}; - -} // namespace roboticslab - -#endif // __AMOR_CONTROL_BOARD_HPP__ diff --git a/libraries/YarpPlugins/AmorControlBoard/CMakeLists.txt b/libraries/YarpPlugins/AmorControlBoard/CMakeLists.txt deleted file mode 100644 index 9d332b15b..000000000 --- a/libraries/YarpPlugins/AmorControlBoard/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -if(NOT AMOR_API_FOUND AND (NOT DEFINED ENABLE_AmorControlBoard OR ENABLE_AmorControlBoard)) - message(WARNING "AMOR_API package not found, disabling AmorControlBoard") -endif() - -yarp_prepare_plugin(AmorControlBoard - CATEGORY device - TYPE roboticslab::AmorControlBoard - INCLUDE AmorControlBoard.hpp - DEFAULT ON - DEPENDS AMOR_API_FOUND - EXTRA_CONFIG WRAPPER=controlBoard_nws_yarp) - -if(NOT SKIP_AmorControlBoard) - - yarp_add_plugin(AmorControlBoard AmorControlBoard.cpp - AmorControlBoard.hpp - DeviceDriverImpl.cpp - IAxisInfoImpl.cpp - IControlLimitsImpl.cpp - IControlModeImpl.cpp - ICurrentControlImpl.cpp - IEncodersImpl.cpp - IPositionControlImpl.cpp - IVelocityControlImpl.cpp - LogComponent.hpp - LogComponent.cpp) - - target_link_libraries(AmorControlBoard YARP::YARP_os - YARP::YARP_dev - AMOR::amor_api) - - yarp_install(TARGETS AmorControlBoard - LIBRARY DESTINATION ${ROBOTICSLAB-YARP-DEVICES_DYNAMIC_PLUGINS_INSTALL_DIR} - ARCHIVE DESTINATION ${ROBOTICSLAB-YARP-DEVICES_STATIC_PLUGINS_INSTALL_DIR} - YARP_INI DESTINATION ${ROBOTICSLAB-YARP-DEVICES_PLUGIN_MANIFESTS_INSTALL_DIR}) - -else() - - set(ENABLE_AmorControlBoard OFF CACHE BOOL "Enable/disable AmorControlBoard device" FORCE) - -endif() diff --git a/libraries/YarpPlugins/AmorControlBoard/DeviceDriverImpl.cpp b/libraries/YarpPlugins/AmorControlBoard/DeviceDriverImpl.cpp deleted file mode 100644 index b4396e40a..000000000 --- a/libraries/YarpPlugins/AmorControlBoard/DeviceDriverImpl.cpp +++ /dev/null @@ -1,124 +0,0 @@ -// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- - -#include "AmorControlBoard.hpp" - -#include - -#include - -#include "LogComponent.hpp" - -using namespace roboticslab; - -constexpr auto DEFAULT_CAN_LIBRARY = "libeddriver.so"; -constexpr auto DEFAULT_CAN_PORT = 0; - -// ------------------- DeviceDriver related ------------------------------------ - -bool AmorControlBoard::open(yarp::os::Searchable& config) -{ - int major, minor, build; - amor_get_library_version(&major, &minor, &build); - - yCInfo(AMOR, "AMOR API library version %d.%d.%d", major, minor, build); - yCInfo(AMOR) << "Trying to connect to AMOR..."; - - handle = amor_connect((char *)DEFAULT_CAN_LIBRARY, DEFAULT_CAN_PORT); - - if (handle == AMOR_INVALID_HANDLE) - { - yCError(AMOR) << "Could not get AMOR handle:" << amor_error(); - return false; - } - - yCInfo(AMOR) << "Acquired AMOR handle!"; - - AMOR_JOINT_INFO jointInfo[AMOR_NUM_JOINTS]; - int jointStatus[AMOR_NUM_JOINTS]; - - for (int j = 0; j < AMOR_NUM_JOINTS; j++) - { - if (amor_get_joint_info(handle, j, &jointInfo[j]) != AMOR_SUCCESS) - { - yCError(AMOR) << "amor_get_joint_info() failed for joint" << j << "with error:" << amor_error(); - return false; - } - - if (amor_get_status(handle, j, &jointStatus[j]) != AMOR_SUCCESS) - { - yCError(AMOR) << "amor_get_status() failed for joint" << j << "with error:" << amor_error(); - return false; - } - } - - std::vector positions; - - if (!getEncoders(positions.data())) - { - yCError(AMOR) << "getEncoders() failed"; - return false; - } - - yCInfo(AMOR) << "Current positions (degrees):" << positions; - - // Set position mode. - if (!positionMove(positions.data())) - { - yCError(AMOR) << "positionMove() failed"; - return false; - } - - yarp::os::Value * cartesianControllerName; - - if (config.check("cartesianControllerName", cartesianControllerName, "cartesian controller port")) - { - yCInfo(AMOR) << "Using AMOR cartesian controller device"; - - usingCartesianController = true; - - std::string subdevice = "AmorCartesianControl"; - yarp::os::Value vHandle(&handle, sizeof(handle)); - yarp::os::Value vHandleMutex(&handleMutex, sizeof(handleMutex)); - yarp::os::Property cartesianControllerOptions; - - cartesianControllerOptions.fromString((config.toString())); - cartesianControllerOptions.put("device", "CartesianControlServer"); - cartesianControllerOptions.put("subdevice", subdevice); - cartesianControllerOptions.put("name", cartesianControllerName->asString()); - cartesianControllerOptions.put("handle", vHandle); - cartesianControllerOptions.put("handleMutex", vHandleMutex); - cartesianControllerOptions.setMonitor(config.getMonitor(), subdevice.c_str()); - - cartesianControllerDevice.open(cartesianControllerOptions); - - if (!cartesianControllerDevice.isValid()) - { - yCError(AMOR) << "AMOR cartesian controller device not valid"; - return false; - } - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::close() -{ - if (usingCartesianController) - { - cartesianControllerDevice.close(); - } - - if (handle != AMOR_INVALID_HANDLE) - { - amor_emergency_stop(handle); - amor_release(handle); - - handle = AMOR_INVALID_HANDLE; - } - - return true; -} - -// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/AmorControlBoard/IAxisInfoImpl.cpp b/libraries/YarpPlugins/AmorControlBoard/IAxisInfoImpl.cpp deleted file mode 100644 index b2ac2f2c4..000000000 --- a/libraries/YarpPlugins/AmorControlBoard/IAxisInfoImpl.cpp +++ /dev/null @@ -1,69 +0,0 @@ -// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- - -#include "AmorControlBoard.hpp" - -#include - -#include "LogComponent.hpp" - -using namespace roboticslab; - -// ------------------- IAxisInfo related ------------------------------------ - -bool AmorControlBoard::getAxisName(int axis, std::string& name) -{ - yCTrace(AMOR, "%d", axis); - - if (!indexWithinRange(axis)) - { - return false; - } - - switch (axis) - { - case 0: - name = "A1"; - break; - case 1: - name = "A2"; - break; - case 2: - name = "A2.5"; - break; - case 3: - name = "A3"; - break; - case 4: - name = "A4"; - break; - case 5: - name = "A5"; - break; - case 6: - name = "A6"; - break; - default: - yCError(AMOR, "Unrecognized axis: %d", axis); - return false; - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getJointType(int axis, yarp::dev::JointTypeEnum& type) -{ - yCTrace(AMOR, "%d", axis); - - if (!indexWithinRange(axis)) - { - return false; - } - - type = yarp::dev::VOCAB_JOINTTYPE_REVOLUTE; - - return true; -} - -// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/AmorControlBoard/IControlLimitsImpl.cpp b/libraries/YarpPlugins/AmorControlBoard/IControlLimitsImpl.cpp deleted file mode 100644 index ac01c40be..000000000 --- a/libraries/YarpPlugins/AmorControlBoard/IControlLimitsImpl.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- - -#include "AmorControlBoard.hpp" - -#include - -#include "LogComponent.hpp" - -using namespace roboticslab; - -// ------------------- IControlLimits related ------------------------------------ - -bool AmorControlBoard::setLimits(int axis, double min, double max) -{ - yCError(AMOR, "setLimits() not available"); - return false; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getLimits(int axis, double *min, double *max) -{ - yCTrace(AMOR, "%d", axis); - - if (!indexWithinRange(axis)) - { - return false; - } - - AMOR_JOINT_INFO parameters; - - if (std::lock_guard lock(handleMutex); amor_get_joint_info(handle, axis, ¶meters) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_joint_info() failed: %s", amor_error()); - return false; - } - - if (parameters.lowerJointLimit == 0.0 && parameters.upperJointLimit == 0.0) - { - *min = -180.0; - *max = 180.0; - } - else - { - *min = toDeg(parameters.lowerJointLimit); - *max = toDeg(parameters.upperJointLimit); - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::setVelLimits(int axis, double min, double max) -{ - yCError(AMOR, "setVelLimits() not available"); - return false; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getVelLimits(int axis, double *min, double *max) -{ - yCTrace(AMOR, "%d", axis); - - if (!indexWithinRange(axis)) - { - return false; - } - - AMOR_JOINT_INFO parameters; - - if (std::lock_guard lock(handleMutex); amor_get_joint_info(handle, axis, ¶meters) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_joint_info() failed: %s", amor_error()); - return false; - } - - *max = toDeg(parameters.maxVelocity); - *min = -(*max); - - return true; -} - -// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/AmorControlBoard/IControlModeImpl.cpp b/libraries/YarpPlugins/AmorControlBoard/IControlModeImpl.cpp deleted file mode 100644 index fea6a4eeb..000000000 --- a/libraries/YarpPlugins/AmorControlBoard/IControlModeImpl.cpp +++ /dev/null @@ -1,110 +0,0 @@ -// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- - -#include "AmorControlBoard.hpp" - -#include -#include - -#include "LogComponent.hpp" - -using namespace roboticslab; - -// ------------------- IControlMode related ------------------------------------ - -bool AmorControlBoard::getControlMode(int j, int *mode) -{ - yCTrace(AMOR, "%d", j); - - if (!indexWithinRange(j)) - { - return false; - } - - *mode = controlMode; - return true; -} - -// ----------------------------------------------------------------------------- - - -bool AmorControlBoard::getControlModes(int *modes) -{ - bool ok = true; - - for (unsigned int i = 0; i < AMOR_NUM_JOINTS; i++) - { - ok &= getControlMode(i, &modes[i]); - } - - return ok; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getControlModes(const int n_joint, const int *joints, int *modes) -{ - if (!batchWithinRange(n_joint)) - { - return false; - } - - bool ok = true; - - for (unsigned int i = 0; i < n_joint; i++) - { - ok &= getControlMode(joints[i], &modes[i]); - } - - return ok; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::setControlMode(const int j, const int mode) -{ - yCTrace(AMOR, "%d %s", j, yarp::os::Vocab32::decode(mode).c_str()); - - if (!indexWithinRange(j)) - { - return false; - } - - controlMode = mode; - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::setControlModes(const int n_joint, const int *joints, int *modes) -{ - if (!batchWithinRange(n_joint)) - { - return false; - } - - bool ok = true; - - for (unsigned int i = 0; i < n_joint; i++) - { - ok &= setControlMode(joints[i], modes[i]); - } - - return ok; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::setControlModes(int *modes) -{ - bool ok = true; - - for (unsigned int i = 0; i < AMOR_NUM_JOINTS; i++) - { - ok &= setControlMode(i, modes[i]); - } - - return ok; -} - -// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/AmorControlBoard/ICurrentControlImpl.cpp b/libraries/YarpPlugins/AmorControlBoard/ICurrentControlImpl.cpp deleted file mode 100644 index c89d91c6a..000000000 --- a/libraries/YarpPlugins/AmorControlBoard/ICurrentControlImpl.cpp +++ /dev/null @@ -1,224 +0,0 @@ -// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- - -#include "AmorControlBoard.hpp" - -#include - -#include - -#include "LogComponent.hpp" - -using namespace roboticslab; - -// ------------------ ICurrentControl Related ----------------------------------------- - -bool AmorControlBoard::getNumberOfMotors(int *ax) -{ - return getAxes(ax); -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getCurrent(int m, double *curr) -{ - yCTrace(AMOR, "%d", m); - - if (!indexWithinRange(m)) - { - return false; - } - - AMOR_VECTOR7 currents; - - if (std::lock_guard lock(handleMutex); amor_get_actual_currents(handle, ¤ts) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_actual_currents() failed: %s", amor_error()); - return false; - } - - *curr = currents[m]; - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getCurrents(double *currs) -{ - yCTrace(AMOR, ""); - - AMOR_VECTOR7 currents; - - if (std::lock_guard lock(handleMutex); amor_get_actual_currents(handle, ¤ts) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_actual_currents() failed: %s", amor_error()); - return false; - } - - std::copy(currents, currents + AMOR_NUM_JOINTS, currs); - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getCurrentRange(int m, double *min, double *max) -{ - yCTrace(AMOR, "%d", m); - - if (!indexWithinRange(m)) - { - return false; - } - - AMOR_JOINT_INFO parameters; - - if (std::lock_guard lock(handleMutex); amor_get_joint_info(handle, m, ¶meters) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_joint_info() failed: %s", amor_error()); - return false; - } - - *min = -parameters.maxCurrent; - *max = parameters.maxCurrent; - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getCurrentRanges(double *min, double *max) -{ - yCTrace(AMOR, ""); - - bool ok = true; - - for (int j = 0; j < AMOR_NUM_JOINTS; j++) - { - ok &= getCurrentRange(j, &min[j], &max[j]); - } - - return ok; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::setRefCurrents(const double *currs) -{ - yCTrace(AMOR, ""); - - AMOR_VECTOR7 currents; - - std::copy(currs, currs + AMOR_NUM_JOINTS, currents); - - if (std::lock_guard lock(handleMutex); amor_set_currents(handle, currents) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_set_currents() failed: %s", amor_error()); - return false; - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::setRefCurrent(int m, double curr) -{ - yCTrace(AMOR, "%d", m); - - if (!indexWithinRange(m)) - { - return false; - } - - AMOR_VECTOR7 currents; - - if (std::lock_guard lock(handleMutex); amor_get_actual_currents(handle, ¤ts) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_actual_currents() failed: %s", amor_error()); - return false; - } - - currents[m] = curr; - - if (std::lock_guard lock(handleMutex); amor_set_currents(handle, currents) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_set_currents() failed: %s", amor_error()); - return false; - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::setRefCurrents(const int n_motor, const int *motors, const double *currs) -{ - yCTrace(AMOR, "%d", n_motor); - - AMOR_VECTOR7 currents; - - if (std::lock_guard lock(handleMutex); amor_get_actual_currents(handle, ¤ts) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_actual_currents() failed: %s", amor_error()); - return false; - } - - for (int i = 0; i < n_motor; i++) - { - currents[motors[i]] = currs[i]; - } - - if (std::lock_guard lock(handleMutex); amor_set_currents(handle, currents) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_set_currents() failed: %s", amor_error()); - return false; - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getRefCurrents(double *currs) -{ - yCTrace(AMOR, ""); - - AMOR_VECTOR7 currents; - - if (std::lock_guard lock(handleMutex); amor_get_req_currents(handle, ¤ts) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_req_currents() failed: %s", amor_error()); - return false; - } - - std::copy(currents, currents + AMOR_NUM_JOINTS, currs); - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getRefCurrent(int m, double *curr) -{ - yCTrace(AMOR, "%d", m); - - if (!indexWithinRange(m)) - { - return false; - } - - AMOR_VECTOR7 currents; - - if (std::lock_guard lock(handleMutex); amor_get_req_currents(handle, ¤ts) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_req_currents() failed: %s", amor_error()); - return false; - } - - *curr = currents[m]; - - return true; -} - -// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/AmorControlBoard/IEncodersImpl.cpp b/libraries/YarpPlugins/AmorControlBoard/IEncodersImpl.cpp deleted file mode 100644 index 87908f308..000000000 --- a/libraries/YarpPlugins/AmorControlBoard/IEncodersImpl.cpp +++ /dev/null @@ -1,170 +0,0 @@ -// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- - -#include "AmorControlBoard.hpp" - -#include - -#include "LogComponent.hpp" - -using namespace roboticslab; - -// ------------------ IEncoders related ----------------------------------------- - -bool AmorControlBoard::resetEncoder(int j) -{ - yCError(AMOR, "resetEncoder() not available"); - return false; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::resetEncoders() -{ - yCError(AMOR, "resetEncoders() not available"); - return false; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::setEncoder(int j, double val) -{ - yCError(AMOR, "setEncoder() not available"); - return false; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::setEncoders(const double *vals) -{ - yCError(AMOR, "setEncoders() not available"); - return false; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getEncoder(int j, double *v) -{ - yCTrace(AMOR, "%d", j); - - if (!indexWithinRange(j)) - { - return false; - } - - AMOR_VECTOR7 positions; - - if (std::lock_guard lock(handleMutex); amor_get_actual_positions(handle, &positions) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_actual_positions() failed: %s", amor_error()); - return false; - } - - *v = toDeg(positions[j]); - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getEncoders(double *encs) -{ - yCTrace(AMOR, ""); - - AMOR_VECTOR7 positions; - - if (std::lock_guard lock(handleMutex); amor_get_actual_positions(handle, &positions) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_actual_positions() failed: %s", amor_error()); - return false; - } - - for (int j = 0; j < AMOR_NUM_JOINTS; j++) - { - encs[j] = toDeg(positions[j]); - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getEncoderSpeed(int j, double *sp) -{ - yCTrace(AMOR, "%d", j); - - if (!indexWithinRange(j)) - { - return false; - } - - AMOR_VECTOR7 velocities; - - if (std::lock_guard lock(handleMutex); amor_get_actual_velocities(handle, &velocities) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_actual_velocities() failed: %s", amor_error()); - return false; - } - - *sp = toDeg(velocities[j]); - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getEncoderSpeeds(double *spds) -{ - yCTrace(AMOR, ""); - - AMOR_VECTOR7 velocities; - - if (std::lock_guard lock(handleMutex); amor_get_actual_velocities(handle, &velocities) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_actual_velocities() failed: %s", amor_error()); - return false; - } - - for (int j = 0; j < AMOR_NUM_JOINTS; j++) - { - spds[j] = toDeg(velocities[j]); - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getEncoderAcceleration(int j, double *spds) -{ - //yCError(AMOR, "getEncoderAcceleration() not available"); - return false; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getEncoderAccelerations(double *accs) -{ - //yCError(AMOR, "getEncoderAccelerations() not available"); - return false; -} - -// ------------------ IEncodersTimed related ----------------------------------------- - -bool AmorControlBoard::getEncodersTimed(double *encs, double *time) -{ - yCTrace(AMOR, ""); - double now = yarp::os::Time::now(); - std::fill_n(time, AMOR_NUM_JOINTS, now); - return getEncoders(encs); -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getEncoderTimed(int j, double *encs, double *time) -{ - yCTrace(AMOR, "%d", j); - *time = yarp::os::Time::now(); - return getEncoder(j, encs); -} - -// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/AmorControlBoard/IPositionControlImpl.cpp b/libraries/YarpPlugins/AmorControlBoard/IPositionControlImpl.cpp deleted file mode 100644 index 17255aedd..000000000 --- a/libraries/YarpPlugins/AmorControlBoard/IPositionControlImpl.cpp +++ /dev/null @@ -1,519 +0,0 @@ -// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- - -#include "AmorControlBoard.hpp" - -#include - -#include "LogComponent.hpp" - -using namespace roboticslab; - -// ------------------- IPositionControl related -------------------------------- - -bool AmorControlBoard::getAxes(int *ax) -{ - *ax = AMOR_NUM_JOINTS; - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::positionMove(int j, double ref) -{ - yCTrace(AMOR, "%d %f", j, ref); - - if (!indexWithinRange(j)) - { - return false; - } - - AMOR_VECTOR7 positions; - - if (std::lock_guard lock(handleMutex); amor_get_actual_positions(handle, &positions) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_actual_positions(): %s", amor_error()); - return false; - } - - positions[j] = toRad(ref); - - std::lock_guard lock(handleMutex); - return amor_set_positions(handle, positions) == AMOR_SUCCESS; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::positionMove(const double *refs) -{ - AMOR_VECTOR7 positions; - - for (int j = 0; j < AMOR_NUM_JOINTS; j++) - { - positions[j] = toRad(refs[j]); - } - - std::lock_guard lock(handleMutex); - return amor_set_positions(handle, positions) == AMOR_SUCCESS; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::relativeMove(int j, double delta) -{ - yCTrace(AMOR, "%d %f", j, delta); - - if (!indexWithinRange(j)) - { - return false; - } - - AMOR_VECTOR7 positions; - - if (std::lock_guard lock(handleMutex); amor_get_actual_positions(handle, &positions) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_actual_positions(): %s", amor_error()); - return false; - } - - positions[j] += toRad(delta); - - std::lock_guard lock(handleMutex); - return amor_set_positions(handle, positions) == AMOR_SUCCESS; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::relativeMove(const double *deltas) -{ - AMOR_VECTOR7 positions; - - for (int j = 0; j < AMOR_NUM_JOINTS; j++) - { - positions[j] += toRad(deltas[j]); - } - - std::lock_guard lock(handleMutex); - return amor_set_positions(handle, positions) == AMOR_SUCCESS; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::checkMotionDone(int j, bool *flag) -{ - if (!indexWithinRange(j)) - { - return false; - } - - return checkMotionDone(flag); -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::checkMotionDone(bool *flag) -{ - yCTrace(AMOR, ""); - - amor_movement_status status; - - if (std::lock_guard lock(handleMutex); amor_get_movement_status(handle, &status) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_movement_status(): %s", amor_error()); - return false; - } - - *flag = (status == AMOR_MOVEMENT_STATUS_FINISHED); - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::setRefSpeed(int j, double sp) -{ - yCError(AMOR, "setRefSpeed() not available"); - return false; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::setRefSpeeds(const double *spds) -{ - yCError(AMOR, "setRefSpeeds() not available"); - return false; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::setRefAcceleration(int j, double acc) -{ - yCError(AMOR, "setRefAcceleration() not available"); - return false; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::setRefAccelerations(const double *accs) -{ - yCError(AMOR, "setRefAccelerations() not available"); - return false; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getRefSpeed(int j, double *ref) -{ - yCTrace(AMOR, "%d", j); - - if (!indexWithinRange(j)) - { - return false; - } - - AMOR_JOINT_INFO parameters; - - if (std::lock_guard lock(handleMutex); amor_get_joint_info(handle, j, ¶meters) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_joint_info(): %s", amor_error()); - return false; - } - - *ref = toDeg(parameters.maxVelocity); - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getRefSpeeds(double *spds) -{ - yCTrace(AMOR, ""); - - for (int j = 0; j < AMOR_NUM_JOINTS; j++) - { - AMOR_JOINT_INFO parameters; - - if (std::lock_guard lock(handleMutex); amor_get_joint_info(handle, j, ¶meters) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_joint_info(): %s", amor_error()); - return false; - } - - spds[j] = toDeg(parameters.maxVelocity); - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getRefAcceleration(int j, double *acc) -{ - yCTrace(AMOR, "%d", j); - - if (!indexWithinRange(j)) - { - return false; - } - - AMOR_JOINT_INFO parameters; - - if (std::lock_guard lock(handleMutex); amor_get_joint_info(handle, j, ¶meters) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_joint_info(): %s", amor_error()); - return false; - } - - *acc = toDeg(parameters.maxAcceleration); - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getRefAccelerations(double *accs) -{ - yCTrace(AMOR, ""); - - for (int j = 0; j < AMOR_NUM_JOINTS; j++) - { - AMOR_JOINT_INFO parameters; - - if (std::lock_guard lock(handleMutex); amor_get_joint_info(handle, j, ¶meters) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_joint_info(): %s", amor_error()); - return false; - } - - accs[j] = toDeg(parameters.maxAcceleration); - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::stop(int j) -{ - yCWarning(AMOR, "Selective stop not available, stopping all joints at once (%d)", j); - - if (!indexWithinRange(j)) - { - return false; - } - - return stop(); -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::stop() -{ - yCTrace(AMOR, ""); - std::lock_guard lock(handleMutex); - return amor_controlled_stop(handle) == AMOR_SUCCESS; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::positionMove(const int n_joint, const int *joints, const double *refs) -{ - yCTrace(AMOR, "%d", n_joint); - - if (!batchWithinRange(n_joint)) - { - return false; - } - - AMOR_VECTOR7 positions; - - if (std::lock_guard lock(handleMutex); n_joint < AMOR_NUM_JOINTS && amor_get_actual_positions(handle, &positions) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_actual_positions(): %s", amor_error()); - return false; - } - - for (int j = 0; j < n_joint; j++) - { - positions[joints[j]] = toRad(refs[j]); - } - - std::lock_guard lock(handleMutex); - return amor_set_positions(handle, positions) == AMOR_SUCCESS; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::relativeMove(const int n_joint, const int *joints, const double *deltas) -{ - yCTrace(AMOR, "%d", n_joint); - - if (!batchWithinRange(n_joint)) - { - return false; - } - - AMOR_VECTOR7 positions; - - if (std::lock_guard lock(handleMutex); n_joint < AMOR_NUM_JOINTS && amor_get_actual_positions(handle, &positions) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_actual_positions(): %s", amor_error()); - return false; - } - - for (int j = 0; j < n_joint; j++) - { - positions[joints[j]] += toRad(deltas[j]); - } - - std::lock_guard lock(handleMutex); - return amor_set_positions(handle, positions) == AMOR_SUCCESS; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::checkMotionDone(const int n_joint, const int *joints, bool *flags) -{ - yCTrace(AMOR, "%d", n_joint); - - if (!batchWithinRange(n_joint)) - { - return false; - } - - amor_movement_status status; - - if (std::lock_guard lock(handleMutex); amor_get_movement_status(handle, &status) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_movement_status(): %s", amor_error()); - return false; - } - - bool flag = (status == AMOR_MOVEMENT_STATUS_FINISHED); - - for (int j = 0; j < n_joint; j++) - { - flags[j] = flag; - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::setRefSpeeds(const int n_joint, const int *joints, const double *spds) -{ - yCError(AMOR, "setRefSpeeds() not available"); - return false; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::setRefAccelerations(const int n_joint, const int *joints, const double *accs) -{ - yCError(AMOR, "setRefAccelerations() not available"); - return false; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getRefSpeeds(const int n_joint, const int *joints, double *spds) -{ - yCTrace(AMOR, "%d", n_joint); - - if (!batchWithinRange(n_joint)) - { - return false; - } - - for (int j = 0; j < n_joint; j++) - { - AMOR_JOINT_INFO parameters; - - if (std::lock_guard lock(handleMutex); amor_get_joint_info(handle, joints[j], ¶meters) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_joint_info(): %s", amor_error()); - return false; - } - - spds[j] = toDeg(parameters.maxVelocity); - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getRefAccelerations(const int n_joint, const int *joints, double *accs) -{ - yCTrace(AMOR, "%d", n_joint); - - if (!batchWithinRange(n_joint)) - { - return false; - } - - for (int j = 0; j < n_joint; j++) - { - AMOR_JOINT_INFO parameters; - - if (std::lock_guard lock(handleMutex); amor_get_joint_info(handle, joints[j], ¶meters) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_joint_info(): %s", amor_error()); - return false; - } - - accs[j] = toDeg(parameters.maxAcceleration); - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::stop(const int n_joint, const int *joints) -{ - yCWarning(AMOR, "Selective stop not available, stopping all joints at once (%d)", n_joint); - - if (!batchWithinRange(n_joint)) - { - return false; - } - - return stop(); -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getTargetPosition(const int joint, double *ref) -{ - yCTrace(AMOR, "%d", joint); - - if (!indexWithinRange(joint)) - { - return false; - } - - AMOR_VECTOR7 positions; - - if (std::lock_guard lock(handleMutex); amor_get_req_positions(handle, &positions) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_req_positions(): %s", amor_error()); - return false; - } - - *ref = toDeg(positions[joint]); - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getTargetPositions(double *refs) -{ - yCTrace(AMOR, ""); - - AMOR_VECTOR7 positions; - - if (std::lock_guard lock(handleMutex); amor_get_req_positions(handle, &positions) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_req_positions(): %s", amor_error()); - return false; - } - - for (int j = 0; j < AMOR_NUM_JOINTS; j++) - { - refs[j] = toDeg(positions[j]); - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getTargetPositions(const int n_joint, const int *joints, double *refs) -{ - yCTrace(AMOR, "%d", n_joint); - - if (!batchWithinRange(n_joint)) - { - return false; - } - - AMOR_VECTOR7 positions; - - if (std::lock_guard lock(handleMutex); amor_get_req_positions(handle, &positions) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_req_positions(): %s", amor_error()); - return false; - } - - for (int j = 0; j < n_joint; j++) - { - refs[j] = toDeg(positions[joints[j]]); - } - - return true; -} - -// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/AmorControlBoard/IVelocityControlImpl.cpp b/libraries/YarpPlugins/AmorControlBoard/IVelocityControlImpl.cpp deleted file mode 100644 index 330ecea1c..000000000 --- a/libraries/YarpPlugins/AmorControlBoard/IVelocityControlImpl.cpp +++ /dev/null @@ -1,152 +0,0 @@ -// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- - -#include "AmorControlBoard.hpp" - -#include - -#include "LogComponent.hpp" - -using namespace roboticslab; - -// ------------------ IVelocityControl related ---------------------------------------- - -bool AmorControlBoard::velocityMove(int j, double sp) -{ - yCTrace(AMOR, "%d %f", j, sp); - - if (!indexWithinRange(j)) - { - return false; - } - - AMOR_VECTOR7 velocities; - - if (std::lock_guard lock(handleMutex); amor_get_actual_velocities(handle, &velocities) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_actual_velocities() failed: %s", amor_error()); - return false; - } - - velocities[j] = toRad(sp); - - std::lock_guard lock(handleMutex); - return amor_set_velocities(handle, velocities) == AMOR_SUCCESS; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::velocityMove(const double *sp) -{ - AMOR_VECTOR7 velocities; - - for (int j = 0; j < AMOR_NUM_JOINTS; j++) - { - velocities[j] = toRad(sp[j]); - } - - std::lock_guard lock(handleMutex); - return amor_set_velocities(handle, velocities) == AMOR_SUCCESS; -} - -// ---------------------------------------------------------------------------- - -bool AmorControlBoard::velocityMove(const int n_joint, const int *joints, const double *spds) -{ - yCTrace(AMOR, "%d", n_joint); - - if (!batchWithinRange(n_joint)) - { - return false; - } - - AMOR_VECTOR7 velocities; - - if (std::lock_guard lock(handleMutex); n_joint < AMOR_NUM_JOINTS && amor_get_actual_velocities(handle, &velocities) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_actual_velocities() failed: %s", amor_error()); - return false; - } - - for (int j = 0; j < n_joint; j++) - { - velocities[joints[j]] = toRad(spds[j]); - } - - std::lock_guard lock(handleMutex); - return amor_set_velocities(handle, velocities) == AMOR_SUCCESS; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getRefVelocity(const int joint, double *vel) -{ - yCTrace(AMOR, "%d", joint); - - if (!indexWithinRange(joint)) - { - return false; - } - - AMOR_VECTOR7 velocities; - - if (std::lock_guard lock(handleMutex); amor_get_req_velocities(handle, &velocities) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_req_velocities() failed: %s", amor_error()); - return false; - } - - *vel = toDeg(velocities[joint]); - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getRefVelocities(double *vels) -{ - yCTrace(AMOR, ""); - - AMOR_VECTOR7 velocities; - - if (std::lock_guard lock(handleMutex); amor_get_req_velocities(handle, &velocities) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_req_velocities() failed: %s", amor_error()); - return false; - } - - for (int j = 0; j < AMOR_NUM_JOINTS; j++) - { - vels[j] = toDeg(velocities[j]); - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool AmorControlBoard::getRefVelocities(const int n_joint, const int *joints, double *vels) -{ - yCTrace(AMOR, "%d", n_joint); - - if (!batchWithinRange(n_joint)) - { - return false; - } - - AMOR_VECTOR7 velocities; - - if (std::lock_guard lock(handleMutex); amor_get_req_velocities(handle, &velocities) != AMOR_SUCCESS) - { - yCError(AMOR, "amor_get_req_velocities() failed: %s", amor_error()); - return false; - } - - for (int j = 0; j < n_joint; j++) - { - vels[j] = toDeg(velocities[joints[j]]); - } - - return true; -} - -// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/AmorControlBoard/LogComponent.cpp b/libraries/YarpPlugins/AmorControlBoard/LogComponent.cpp deleted file mode 100644 index 3947b08f8..000000000 --- a/libraries/YarpPlugins/AmorControlBoard/LogComponent.cpp +++ /dev/null @@ -1,3 +0,0 @@ -#include "LogComponent.hpp" - -YARP_LOG_COMPONENT(AMOR, "rl.AmorControlBoard") diff --git a/libraries/YarpPlugins/AmorControlBoard/LogComponent.hpp b/libraries/YarpPlugins/AmorControlBoard/LogComponent.hpp deleted file mode 100644 index cc7b7dc10..000000000 --- a/libraries/YarpPlugins/AmorControlBoard/LogComponent.hpp +++ /dev/null @@ -1,8 +0,0 @@ -#ifndef __AMOR_CONTROL_BOARD_LOG_COMPONENT_HPP__ -#define __AMOR_CONTROL_BOARD_LOG_COMPONENT_HPP__ - -#include - -YARP_DECLARE_LOG_COMPONENT(AMOR) - -#endif // __AMOR_CONTROL_BOARD_LOG_COMPONENT_HPP__ diff --git a/libraries/YarpPlugins/AmorControlBoard/README.md b/libraries/YarpPlugins/AmorControlBoard/README.md deleted file mode 100644 index 5592dd603..000000000 --- a/libraries/YarpPlugins/AmorControlBoard/README.md +++ /dev/null @@ -1,6 +0,0 @@ -# AmorControlBoard - - -## Requirements -Depends on: -- https://github.com/roboticslab-uc3m/amor-api (private) diff --git a/libraries/YarpPlugins/CMakeLists.txt b/libraries/YarpPlugins/CMakeLists.txt index 73be6b7c3..91522873e 100644 --- a/libraries/YarpPlugins/CMakeLists.txt +++ b/libraries/YarpPlugins/CMakeLists.txt @@ -1,5 +1,4 @@ # YARP devices. -add_subdirectory(AmorControlBoard) add_subdirectory(AravisGigE) add_subdirectory(CanBusControlBoard) add_subdirectory(CanBusFake) diff --git a/libraries/YarpPlugins/PortMonitorPlugins/CMakeLists.txt b/libraries/YarpPlugins/PortMonitorPlugins/CMakeLists.txt index c1b251a11..2322a85f7 100644 --- a/libraries/YarpPlugins/PortMonitorPlugins/CMakeLists.txt +++ b/libraries/YarpPlugins/PortMonitorPlugins/CMakeLists.txt @@ -1,2 +1 @@ add_subdirectory(cpp) -add_subdirectory(lua) diff --git a/libraries/YarpPlugins/PortMonitorPlugins/lua/CMakeLists.txt b/libraries/YarpPlugins/PortMonitorPlugins/lua/CMakeLists.txt deleted file mode 100644 index c44a50dd2..000000000 --- a/libraries/YarpPlugins/PortMonitorPlugins/lua/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -yarp_install(FILES amor_sensors_modifier.lua - DESTINATION ${ROBOTICSLAB-YARP-DEVICES_CONTEXTS_INSTALL_DIR}/portmonitor) diff --git a/libraries/YarpPlugins/PortMonitorPlugins/lua/amor_sensors_modifier.lua b/libraries/YarpPlugins/PortMonitorPlugins/lua/amor_sensors_modifier.lua deleted file mode 100644 index 01b268041..000000000 --- a/libraries/YarpPlugins/PortMonitorPlugins/lua/amor_sensors_modifier.lua +++ /dev/null @@ -1,488 +0,0 @@ --- loading lua-yarp binding library -require("yarp") - --- Lua 5.2 lacks a global 'unpack' function -local unpack = table.unpack or unpack - --- Globals -local NUMBER_OF_PREVIOUS_ITERATIONS = 5 -local FILTER_FACTOR = 5 - -local SensorDataProcessor = {} - --- --- Performs a deep copy of provided table (only table children, no metadata). --- From http://lua-users.org/wiki/CopyTable --- --- @param target source Table --- --- @return cloned Table --- -function cloneTable(target) - local target_type = type(target) - local copy - if target_type == 'table' then - copy = {} - for target_key, target_value in next, target, nil do - copy[cloneTable(target_key)] = cloneTable(target_value) - end - else -- number, string, boolean, etc - copy = target - end - return copy -end - --- --- Filters odd values (peaks) from current data set. --- Current implementation: discard data if current and previous iterations --- exceed initial value by a constant factor. --- --- @param reference Table of sensor data for previous iteration --- -function filterPeaks(references, currentData) - local hasPeak = function(ref, tgt) - return tgt > ref * FILTER_FACTOR - end - - local refIter = references[1] - - for i, part in ipairs(currentData) do - for j, decValue in ipairs(part) do - local ref = refIter[i][j] - - if hasPeak(ref, decValue) then - local recurrentPeak = true - - for k = 2, #references do - recurrentPeak = recurrentPeak and hasPeak(ref, references[k][i][j]) - end - - if not recurrentPeak then - part[j] = ref - end - end - end - end -end - --- --- Creates a SensorDataProcessor instance. --- --- @param processor Table of input parameters --- --- @return SensorDataProcessor --- -SensorDataProcessor.new = function(self, processor) - local obj = { - accumulator = "", - dataReady = false, - currentSensorData = nil, - processor = processor, - stamp = 0, - previousIterations = {} - } - - setmetatable(obj, self) - self.__index = self - return obj -end - --- --- Consumes fetched data from sensors. --- --- @param str String of new data to be appended to the accumulator --- -SensorDataProcessor.accept = function(self, str) - str = str or "" - self.accumulator = self.accumulator .. str - self:tryConsume() -end - --- --- Calls further processing methods if conditions are satisfied, --- sets flag if data is ready to be forwarded to receiver. --- -SensorDataProcessor.tryConsume = function(self) - if self.processor.evaluateCondition(self.accumulator) then - self.currentSensorData, self.accumulator = self.processor.process(self.accumulator) - - if self.currentSensorData then - -- TODO: users may want to disable or use other filter - if #self.previousIterations == NUMBER_OF_PREVIOUS_ITERATIONS then - filterPeaks(self.previousIterations, self.currentSensorData) - table.remove(self.previousIterations, 1) - end - - self.dataReady = true - self.stamp = self.stamp + 1 - - table.insert(self.previousIterations, cloneTable(self.currentSensorData)) - else - print("message dropped") - end - end -end - --- --- Get current stamp (incremented after data is ready on each iteration). --- --- @return Number --- -SensorDataProcessor.getStamp = function(self) - return self.stamp -end - --- --- Factory function, creates a mean SensorDataProcessor. --- --- @param properties Table of input parameters --- @param properties.parts String identifier for main data streams (arm, elbow, hand) --- @param properties.subparts String identifier for secondary data streams (X, Y, Z) --- @param properties.hexValues Number of hexadecimal values contained in a single data stream --- @param properties.hexSize Number of characters a hexValue is comprised of --- --- @return Table of output parameters --- @return Table.evaluateCondition Function, see docs below --- @return Table.process Function, see docs below --- -local createMeanProcessor = function(properties) - -- identifier for invalid input values - local invalidValue = tonumber(string.rep("F", properties.hexSize), 16) - --local separators = {"\r\n", "\n", "\r"} - --local storage = {} - - -- - -- Splits input into lines. - -- - -- @param str String of raw data, contains newline characters. - -- - -- @return Table - -- - local extractMessages = function(str) - local lines = {} - - for line in string.gmatch(str, "%S+") do - table.insert(lines, line) - end - - return lines - end - - -- - -- Groups (by part/subpart) and sorts input data table. - -- - -- @param lines Table of full message data (parts + subparts) - -- - local preprocessMessages = function(lines) - local temp = {} - local i = 1 - local nparts, nsubparts = #properties.parts, #properties.subparts - - if #lines < nparts * (nsubparts + 1) then return end - - -- always assume the following order: - -- part {I, J, K} - -- subpart X - -- subpart Y - -- subpart Z - while i <= #lines do - local ttemp = {} -- part + 3 subparts - table.insert(ttemp, lines[i]) - i = i + 1 - - for j = 1, nsubparts do - table.insert(ttemp, lines[i]) - i = i + 1 - end - - table.insert(temp, ttemp) - end - - -- sort alphabetically by first element, i.e. the part ("I", "J", "K") - table.sort(temp, function(a, b) - return a[1] < b[1] - end) - - -- clear original table - for i, v in ipairs(lines) do lines[i] = nil end - - -- fill with correct sequence of parts and subparts, one level of depth - -- remove duplicate parts: https://stackoverflow.com/a/12397571 - i = 1 - while i <= #temp do - if i ~= 1 and temp[i][1] == temp[i - 1][1] then - table.remove(temp, i) - else - for j, group in ipairs(temp[i]) do - table.insert(lines, group) - end - i = i + 1 - end - end - end - - -- - -- Tokenizes input string into elements of constant width. - -- - -- @param str input String - -- @param n Number of characters each token is comprised of - -- - -- @return Table of split tokens - -- - local splitString = function(str, n) - n = math.floor(n or 0) - if n <= 0 then return {""} end - local t = {} - - for i = 1, math.floor(str:len() / n) do - local start = 1 + n * (i - 1) - table.insert(t, str:sub(start, start + n - 1)) - end - - return t - end - - -- - -- Calculates arithmetic mean of input data. - -- - -- @param varargs of input Numbers - -- - -- @return Number as arithmetic mean of input data - -- - local calculateMean = function(...) - if select('#', ...) == 0 then return 0 end - local sum = 0 - - for i, value in ipairs({...}) do - sum = sum + value - end - - return math.floor(sum / select('#', ...)) - end - - -- - -- Extracts sensor value from sanitized streams and applies selected algorithm. - -- - -- @param Table of preprocessed message Strings. - -- - -- @return Table on success, nil on failure - -- - local doWork = function(lines) - local nline = 0 - local storage = {} - - for i, part in ipairs(properties.parts) do - nline = nline + 1 - local line = lines[nline] - if not line or line ~= part then return nil end - local partArray = {} - - for j, subpart in ipairs(properties.subparts) do - nline = nline + 1 - line = lines[nline] - if not line or line:sub(1, 1) ~= subpart then return nil end - local substring = line:sub(2) - if substring:len() ~= properties.hexValues * properties.hexSize then return nil end - local thex = splitString(substring, properties.hexSize) - if #thex ~= properties.hexValues then return nil end - - for k, hexString in ipairs(thex) do - local dec = tonumber(hexString, 16) - if not dec then return nil end - if dec == invalidValue then dec = 0 end - local tuple = partArray[k] or {} - table.insert(tuple, dec) - - if not partArray[k] then - table.insert(partArray, tuple) - end - end - end - - for k, tuple in ipairs(partArray) do - partArray[k] = calculateMean(unpack(tuple)) - end - - table.insert(storage, partArray) - end - - return storage - end - - -- - -- Tests whether provided string represents a complete stream of data (all parts and subparts). - -- - -- @param str input String - -- - -- @return Boolean - -- - local evaluateCondition = function(str) - local temp = {} - local occurrences = 0 - local singleOccurrenceParts = {} - - for i, id in ipairs(properties.parts) do - local j = 1 - - -- find all occurrences of part 'id' - while j <= str:len() do - local n - n, j = str:find(id, j, true) - - -- no more occurrences, exit loop - if not n then break end - - -- store index in associative array for given part - local t = temp[id] or {} - table.insert(t, n) - temp[id] = t - - j = j + 1 - end - - -- a part identifier is missing - if not temp[id] then return false end - - -- if this part has a single occurrence, store its id - if #temp[id] == 1 then - table.insert(singleOccurrenceParts, id) - end - - occurrences = occurrences + #temp[id] - end - - -- must have at least nº of parts + 1 - if not (occurrences > #properties.parts) then return false end - - local lastOccurrencePos = 0 - - -- find position of last occurrence - for id, ocs in pairs(temp) do - for j, n in ipairs(ocs) do - if n > lastOccurrencePos then lastOccurrencePos = n end - end - end - - -- make sure that a part with a single occurrence is not last - for i, id in ipairs(singleOccurrenceParts) do - if temp[id][1] == lastOccurrencePos then return false end - end - - return true - end - - -- - -- Extract the biggest chunk of text containing full part frames. - -- - -- @param str input string - -- - -- @return desired String chunk - -- @return rightmost remainder String chunk - -- - local extractSubstring = function(str) - local first, last = str:len(), str:len() - - for i, part in ipairs(properties.parts) do - local f = str:find(part, 1, true) - if f < first then first = f end - local l = str:reverse():find(part, 1, true) - if l < last then last = l end - end - - last = str:len() - last - return str:sub(first, last), str:sub(last + 1) - end - - -- - -- Main execution block, processes raw data. - -- - -- @param str String of raw data accumulated by the processor in previous iterations - -- - -- @return Table of parsed messages - -- @return String of unprocessed data left by this iteration - -- - local process = function(str) - local substring, remainder = extractSubstring(str) - local lines = extractMessages(substring) - preprocessMessages(lines) - return doWork(lines), remainder - end - - return { - evaluateCondition = evaluateCondition, - process = process - } -end - -local sensorDataProcessor - --- --- Method called when the port monitor is created. --- --- @param options --- --- @return Boolean --- -PortMonitor.create = function(options) - print("INITIALIZING AMOR SENSORS") - -- TODO: read options from .ini file - local meanProcessor = createMeanProcessor{ - parts = {"I", "J", "K"}, - subparts = {"X", "Y", "Z"}, - hexValues = 16, - hexSize = 3 - } - sensorDataProcessor = SensorDataProcessor:new(meanProcessor) - return true; -end - --- --- Method called when port monitor is destroyed. --- -PortMonitor.destroy = function() - print("DESTROYING PORT MONITOR") - sensorDataProcessor = nil -end - --- --- Method called when the port receives new data. --- If false is returned, the data will be ignored --- and update() will never be called. --- --- @param thing The Things abstract data type --- --- @return Boolean --- -PortMonitor.accept = function(thing) - bt = thing:asBottle() - - if bt == nil then - print("bot_modifier.lua: got wrong data type (expected type Bottle)") - return false - end - - sensorDataProcessor:accept(bt:get(0):asString()) - return sensorDataProcessor.dataReady -end - --- --- Method called to forward processed data. --- --- @param thing The Things abstract data type --- --- @return Things --- -PortMonitor.update = function(thing) - print(string.format("in update [%d]", sensorDataProcessor:getStamp())) - sensorDataProcessor.dataReady = false - local vec = yarp.Vector() - - for i, part in ipairs(sensorDataProcessor.currentSensorData) do - for j, decValue in ipairs(part) do - vec:push_back(decValue) - end - end - - thing:setPortWriter(vec) - return thing -end