From 08c5a5a58ae254cdbbfed4f393b37acbcb4cdeb3 Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Sun, 23 Feb 2025 22:56:25 +0100 Subject: [PATCH] CartesianControl.h now using yarp::dev::ReturnValue added device FakeCartesianControl added test fakeCartesianControl --- src/devices/fake/CMakeLists.txt | 1 + .../fake/fakeCartesianControl/CMakeLists.txt | 55 +++ .../FakeCartesianControl.cpp | 362 ++++++++++++++++++ .../FakeCartesianControl.h | 134 +++++++ .../FakeCartesianControl_params.md | 1 + .../fakeCartesianControl/tests/CMakeLists.txt | 4 + .../tests/fakeCartesianControl_test.cpp | 45 +++ .../src/yarp/dev/CartesianControl.h | 161 ++++---- .../src/yarp/dev/tests/CMakeLists.txt | 2 + .../src/yarp/dev/tests/IBatteryTest.h | 2 +- .../yarp/dev/tests/ICartesianControlTest.cpp | 6 + .../yarp/dev/tests/ICartesianControlTest.h | 325 ++++++++++++++++ 12 files changed, 1017 insertions(+), 81 deletions(-) create mode 100644 src/devices/fake/fakeCartesianControl/CMakeLists.txt create mode 100644 src/devices/fake/fakeCartesianControl/FakeCartesianControl.cpp create mode 100644 src/devices/fake/fakeCartesianControl/FakeCartesianControl.h create mode 100644 src/devices/fake/fakeCartesianControl/FakeCartesianControl_params.md create mode 100644 src/devices/fake/fakeCartesianControl/tests/CMakeLists.txt create mode 100644 src/devices/fake/fakeCartesianControl/tests/fakeCartesianControl_test.cpp create mode 100644 src/libYARP_dev/src/yarp/dev/tests/ICartesianControlTest.cpp create mode 100644 src/libYARP_dev/src/yarp/dev/tests/ICartesianControlTest.h diff --git a/src/devices/fake/CMakeLists.txt b/src/devices/fake/CMakeLists.txt index baef96f915d..9909c643723 100644 --- a/src/devices/fake/CMakeLists.txt +++ b/src/devices/fake/CMakeLists.txt @@ -3,6 +3,7 @@ add_subdirectory(fakeDepthCamera) add_subdirectory(fakebot) +add_subdirectory(fakeCartesianControl) add_subdirectory(fakeChatBotDevice) add_subdirectory(fakeMotionControl) add_subdirectory(fakeMotionControlMicro) diff --git a/src/devices/fake/fakeCartesianControl/CMakeLists.txt b/src/devices/fake/fakeCartesianControl/CMakeLists.txt new file mode 100644 index 00000000000..54bc2bbccd2 --- /dev/null +++ b/src/devices/fake/fakeCartesianControl/CMakeLists.txt @@ -0,0 +1,55 @@ +# SPDX-FileCopyrightText: 2025-2025 Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +if (YARP_COMPILE_ALL_FAKE_DEVICES) + set(ENABLE_yarpmod_fakeCartesianControl ON CACHE BOOL "" FORCE) +endif() + +yarp_prepare_plugin(fakeCartesianControl + CATEGORY device + TYPE FakeCartesianControl + INCLUDE FakeCartesianControl.h + GENERATE_PARSER +) + +if(NOT SKIP_fakeCartesianControl) + yarp_add_plugin(yarp_fakeCartesianControl) + + target_sources(yarp_fakeCartesianControl + PRIVATE + FakeCartesianControl.cpp + FakeCartesianControl.h + # FakeCartesianControl_ParamsParser.cpp + # FakeCartesianControl_ParamsParser.h + ) + + target_link_libraries(yarp_fakeCartesianControl + PRIVATE + YARP::YARP_os + YARP::YARP_sig + YARP::YARP_dev + ) + list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS + YARP_os + YARP_sig + YARP_dev + ) + + yarp_install( + TARGETS yarp_fakeCartesianControl + EXPORT YARP_${YARP_PLUGIN_MASTER} + COMPONENT ${YARP_PLUGIN_MASTER} + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR} + ) + + set(YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS ${YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS} PARENT_SCOPE) + + set_property(TARGET yarp_fakeCartesianControl PROPERTY FOLDER "Plugins/Device/Fake") + + if(YARP_COMPILE_TESTS) + add_subdirectory(tests) + endif() + +endif() diff --git a/src/devices/fake/fakeCartesianControl/FakeCartesianControl.cpp b/src/devices/fake/fakeCartesianControl/FakeCartesianControl.cpp new file mode 100644 index 00000000000..31966b59830 --- /dev/null +++ b/src/devices/fake/fakeCartesianControl/FakeCartesianControl.cpp @@ -0,0 +1,362 @@ +/* + * SPDX-FileCopyrightText: 2025-2025 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include "FakeCartesianControl.h" + +#include +#include +#include +#include + +#include +#include + +using namespace yarp::os; +using namespace yarp::dev; + +namespace { +YARP_LOG_COMPONENT(FAKECARTESIANCONTROL, "yarp.device.fakeCartesianControl") +constexpr double default_period = 0.02; +} + +FakeCartesianControl::FakeCartesianControl() : + PeriodicThread(default_period) +{ +} + +bool FakeCartesianControl::open(yarp::os::Searchable& config) +{ + // if (!this->parseParams(config)) {return false;} + // setPeriod(m_period); + + PeriodicThread::start(); + + return true; +} + +bool FakeCartesianControl::close() +{ + std::lock_guard lock(m_mutex); + + // Stop the thread + PeriodicThread::stop(); + + return true; +} + +void FakeCartesianControl::run() +{ + std::lock_guard lock(m_mutex); +} + +ReturnValue FakeCartesianControl::setTrackingMode(const bool f) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::getTrackingMode(bool *f) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::setReferenceMode(const bool f) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::getReferenceMode(bool *f) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::setPosePriority(const std::string &p) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::getPosePriority(std::string &p) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::getPose(yarp::sig::Vector &x, yarp::sig::Vector &o, + yarp::os::Stamp *stamp) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::getPose(const int axis, yarp::sig::Vector &x, + yarp::sig::Vector &o, + yarp::os::Stamp *stamp) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::goToPose(const yarp::sig::Vector &xd, + const yarp::sig::Vector &od, + const double t) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::goToPosition(const yarp::sig::Vector &xd, + const double t) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::goToPoseSync(const yarp::sig::Vector &xd, + const yarp::sig::Vector &od, + const double t) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::goToPositionSync(const yarp::sig::Vector &xd, + const double t) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::getDesired(yarp::sig::Vector &xdhat, + yarp::sig::Vector &odhat, + yarp::sig::Vector &qdhat) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::askForPose(const yarp::sig::Vector &xd, + const yarp::sig::Vector &od, + yarp::sig::Vector &xdhat, + yarp::sig::Vector &odhat, + yarp::sig::Vector &qdhat) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::askForPose(const yarp::sig::Vector &q0, + const yarp::sig::Vector &xd, + const yarp::sig::Vector &od, + yarp::sig::Vector &xdhat, + yarp::sig::Vector &odhat, + yarp::sig::Vector &qdhat) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::askForPosition(const yarp::sig::Vector &xd, + yarp::sig::Vector &xdhat, + yarp::sig::Vector &odhat, + yarp::sig::Vector &qdhat) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::askForPosition(const yarp::sig::Vector &q0, + const yarp::sig::Vector &xd, + yarp::sig::Vector &xdhat, + yarp::sig::Vector &odhat, + yarp::sig::Vector &qdhat) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::getDOF(yarp::sig::Vector &curDof) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::setDOF(const yarp::sig::Vector &newDof, + yarp::sig::Vector &curDof) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::getRestPos(yarp::sig::Vector &curRestPos) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::setRestPos(const yarp::sig::Vector &newRestPos, + yarp::sig::Vector &curRestPos) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::getRestWeights(yarp::sig::Vector &curRestWeights) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::setRestWeights(const yarp::sig::Vector &newRestWeights, + yarp::sig::Vector &curRestWeights) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::getLimits(const int axis, double *min, double *max) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::setLimits(const int axis, const double min, const double max) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::getTrajTime(double *t) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::setTrajTime(const double t) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::getInTargetTol(double *tol) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::setInTargetTol(const double tol) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::getJointsVelocities(yarp::sig::Vector &qdot) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::getTaskVelocities(yarp::sig::Vector &xdot, + yarp::sig::Vector &odot) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::setTaskVelocities(const yarp::sig::Vector &xdot, + const yarp::sig::Vector &odot) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::attachTipFrame(const yarp::sig::Vector &x, + const yarp::sig::Vector &o) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::getTipFrame(yarp::sig::Vector &x, yarp::sig::Vector &o) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::removeTipFrame() +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::checkMotionDone(bool *f) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::waitMotionDone(const double period, + const double timeout) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::stopControl() +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::storeContext(int *id) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::restoreContext(const int id) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::deleteContext(const int id) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::getInfo(yarp::os::Bottle &info) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::registerEvent(CartesianEvent &event) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::unregisterEvent(CartesianEvent &event) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::tweakSet(const yarp::os::Bottle &options) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} + +ReturnValue FakeCartesianControl::tweakGet(yarp::os::Bottle &options) +{ + std::lock_guard lock(m_mutex); + return ReturnValue_ok; +} diff --git a/src/devices/fake/fakeCartesianControl/FakeCartesianControl.h b/src/devices/fake/fakeCartesianControl/FakeCartesianControl.h new file mode 100644 index 00000000000..9fa9b2a08f3 --- /dev/null +++ b/src/devices/fake/fakeCartesianControl/FakeCartesianControl.h @@ -0,0 +1,134 @@ +/* + * SPDX-FileCopyrightText: 2025-2025 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef YARP_FAKECARTESIANCONTROL_H +#define YARP_FAKECARTESIANCONTROL_H + +#include +#include + +#include +#include + +#include + +//#include "FakeCartesianControl_ParamsParser.h" + + /** + * @ingroup dev_impl_fake + * + * \brief `fakeCartesianControl`: Documentation to be added + * + * Parameters required by this device are shown in class: FakeCartesianControl_ParamsParser + */ +class FakeCartesianControl : + public yarp::os::PeriodicThread, + public yarp::dev::ICartesianControl, + public yarp::dev::DeviceDriver + // public FakeCartesianControl_ParamsParser +{ +protected: + std::mutex m_mutex; + +public: + FakeCartesianControl(); + FakeCartesianControl(const FakeCartesianControl&) = delete; + FakeCartesianControl(FakeCartesianControl&&) = delete; + FakeCartesianControl& operator=(const FakeCartesianControl&) = delete; + FakeCartesianControl& operator=(FakeCartesianControl&&) = delete; + + ~FakeCartesianControl() override = default; + + // yarp::dev::DeviceDriver + bool open(yarp::os::Searchable& config) override; + bool close() override; + + // yarp::os::PeriodicThread + void run() override; + + // yarp::dev::ICartesianControl + yarp::dev::ReturnValue setTrackingMode(const bool f) override; + yarp::dev::ReturnValue getTrackingMode(bool *f) override; + yarp::dev::ReturnValue setReferenceMode(const bool f) override; + yarp::dev::ReturnValue getReferenceMode(bool *f) override; + yarp::dev::ReturnValue setPosePriority(const std::string &p) override; + yarp::dev::ReturnValue getPosePriority(std::string &p) override; + yarp::dev::ReturnValue getPose(yarp::sig::Vector &x, yarp::sig::Vector &o, + yarp::os::Stamp *stamp=NULL) override; + yarp::dev::ReturnValue getPose(const int axis, yarp::sig::Vector &x, + yarp::sig::Vector &o, + yarp::os::Stamp *stamp=NULL) override; + yarp::dev::ReturnValue goToPose(const yarp::sig::Vector &xd, + const yarp::sig::Vector &od, + const double t = 0.0) override; + yarp::dev::ReturnValue goToPosition(const yarp::sig::Vector &xd, + const double t = 0.0) override; + yarp::dev::ReturnValue goToPoseSync(const yarp::sig::Vector &xd, + const yarp::sig::Vector &od, + const double t = 0.0) override; + yarp::dev::ReturnValue goToPositionSync(const yarp::sig::Vector &xd, + const double t = 0.0) override; + yarp::dev::ReturnValue getDesired(yarp::sig::Vector &xdhat, + yarp::sig::Vector &odhat, + yarp::sig::Vector &qdhat) override; + yarp::dev::ReturnValue askForPose(const yarp::sig::Vector &xd, + const yarp::sig::Vector &od, + yarp::sig::Vector &xdhat, + yarp::sig::Vector &odhat, + yarp::sig::Vector &qdhat) override; + yarp::dev::ReturnValue askForPose(const yarp::sig::Vector &q0, + const yarp::sig::Vector &xd, + const yarp::sig::Vector &od, + yarp::sig::Vector &xdhat, + yarp::sig::Vector &odhat, + yarp::sig::Vector &qdhat) override; + yarp::dev::ReturnValue askForPosition(const yarp::sig::Vector &xd, + yarp::sig::Vector &xdhat, + yarp::sig::Vector &odhat, + yarp::sig::Vector &qdhat) override; + yarp::dev::ReturnValue askForPosition(const yarp::sig::Vector &q0, + const yarp::sig::Vector &xd, + yarp::sig::Vector &xdhat, + yarp::sig::Vector &odhat, + yarp::sig::Vector &qdhat) override; + yarp::dev::ReturnValue getDOF(yarp::sig::Vector &curDof) override; + yarp::dev::ReturnValue setDOF(const yarp::sig::Vector &newDof, + yarp::sig::Vector &curDof) override; + yarp::dev::ReturnValue getRestPos(yarp::sig::Vector &curRestPos) override; + yarp::dev::ReturnValue setRestPos(const yarp::sig::Vector &newRestPos, + yarp::sig::Vector &curRestPos) override; + yarp::dev::ReturnValue getRestWeights(yarp::sig::Vector &curRestWeights) override; + yarp::dev::ReturnValue setRestWeights(const yarp::sig::Vector &newRestWeights, + yarp::sig::Vector &curRestWeights) override; + yarp::dev::ReturnValue getLimits(const int axis, double *min, double *max) override; + yarp::dev::ReturnValue setLimits(const int axis, const double min, const double max) override; + yarp::dev::ReturnValue getTrajTime(double *t) override; + yarp::dev::ReturnValue setTrajTime(const double t) override; + yarp::dev::ReturnValue getInTargetTol(double *tol) override; + yarp::dev::ReturnValue setInTargetTol(const double tol) override; + yarp::dev::ReturnValue getJointsVelocities(yarp::sig::Vector &qdot) override; + yarp::dev::ReturnValue getTaskVelocities(yarp::sig::Vector &xdot, + yarp::sig::Vector &odot) override; + yarp::dev::ReturnValue setTaskVelocities(const yarp::sig::Vector &xdot, + const yarp::sig::Vector &odot) override; + yarp::dev::ReturnValue attachTipFrame(const yarp::sig::Vector &x, + const yarp::sig::Vector &o) override; + yarp::dev::ReturnValue getTipFrame(yarp::sig::Vector &x, yarp::sig::Vector &o) override; + yarp::dev::ReturnValue removeTipFrame() override; + yarp::dev::ReturnValue checkMotionDone(bool *f) override; + yarp::dev::ReturnValue waitMotionDone(const double period = 0.1, + const double timeout = 0.0) override; + yarp::dev::ReturnValue stopControl() override; + yarp::dev::ReturnValue storeContext(int *id) override; + yarp::dev::ReturnValue restoreContext(const int id) override; + yarp::dev::ReturnValue deleteContext(const int id) override; + yarp::dev::ReturnValue getInfo(yarp::os::Bottle &info) override; + yarp::dev::ReturnValue registerEvent(yarp::dev::CartesianEvent &event) override; + yarp::dev::ReturnValue unregisterEvent(yarp::dev::CartesianEvent &event) override; + yarp::dev::ReturnValue tweakSet(const yarp::os::Bottle &options) override; + yarp::dev::ReturnValue tweakGet(yarp::os::Bottle &options) override; +}; + +#endif diff --git a/src/devices/fake/fakeCartesianControl/FakeCartesianControl_params.md b/src/devices/fake/fakeCartesianControl/FakeCartesianControl_params.md new file mode 100644 index 00000000000..79994ae4551 --- /dev/null +++ b/src/devices/fake/fakeCartesianControl/FakeCartesianControl_params.md @@ -0,0 +1 @@ + * | | period | double | s | 0.02 | No | thread period | | diff --git a/src/devices/fake/fakeCartesianControl/tests/CMakeLists.txt b/src/devices/fake/fakeCartesianControl/tests/CMakeLists.txt new file mode 100644 index 00000000000..1384887244e --- /dev/null +++ b/src/devices/fake/fakeCartesianControl/tests/CMakeLists.txt @@ -0,0 +1,4 @@ +# SPDX-FileCopyrightText: 2025-2025 Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +create_device_test(fakeCartesianControl) diff --git a/src/devices/fake/fakeCartesianControl/tests/fakeCartesianControl_test.cpp b/src/devices/fake/fakeCartesianControl/tests/fakeCartesianControl_test.cpp new file mode 100644 index 00000000000..09c733bbda0 --- /dev/null +++ b/src/devices/fake/fakeCartesianControl/tests/fakeCartesianControl_test.cpp @@ -0,0 +1,45 @@ +/* + * SPDX-FileCopyrightText: 2025-2025 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include +#include + +#include +#include + +using namespace yarp::dev; +using namespace yarp::os; + +TEST_CASE("dev::fakeCartesianControl", "[yarp::dev]") +{ + YARP_REQUIRE_PLUGIN("fakeCartesianControl", "device"); + + Network::setLocalMode(true); + + SECTION("Checking fakeCartesianControl device") + { + PolyDriver dd; + yarp::dev::ICartesianControl* icart=nullptr; + + ////////"Checking opening device polydrivers" + { + Property p_cfg; + p_cfg.put("device", "fakeCartesianControl"); + REQUIRE(dd.open(p_cfg)); + } + + dd.view(icart); + yarp::dev::tests::exec_iCartesian_test_1(icart); + + //"Close all polydrivers and check" + { + CHECK(dd.close()); + } + } + + Network::setLocalMode(false); +} diff --git a/src/libYARP_dev/src/yarp/dev/CartesianControl.h b/src/libYARP_dev/src/yarp/dev/CartesianControl.h index f3aed8b5439..832bab2ea2c 100644 --- a/src/libYARP_dev/src/yarp/dev/CartesianControl.h +++ b/src/libYARP_dev/src/yarp/dev/CartesianControl.h @@ -10,6 +10,7 @@ #include #include #include +#include #include /*! @@ -140,7 +141,7 @@ class YARP_dev_API yarp::dev::ICartesianControl * In non-tracking mode the controller releases the limb as * soon as the desired pose is reached. */ - virtual bool setTrackingMode(const bool f) = 0; + virtual yarp::dev::ReturnValue setTrackingMode(const bool f) = 0; /*! * Get the current controller mode. [wait for reply] @@ -148,7 +149,7 @@ class YARP_dev_API yarp::dev::ICartesianControl * mode, false otherwise. * \return true/false on success/failure. */ - virtual bool getTrackingMode(bool *f) = 0; + virtual yarp::dev::ReturnValue getTrackingMode(bool *f) = 0; /*! * Ask the controller to close the loop with the low-level joints @@ -163,7 +164,7 @@ class YARP_dev_API yarp::dev::ICartesianControl * useful in a scenario where the commands are executed by * the control boards with resort to torque actuation. */ - virtual bool setReferenceMode(const bool f) = 0; + virtual yarp::dev::ReturnValue setReferenceMode(const bool f) = 0; /*! * Get the current controller reference mode. [wait for reply] @@ -172,7 +173,7 @@ class YARP_dev_API yarp::dev::ICartesianControl * actual encoders feedback. * \return true/false on success/failure. */ - virtual bool getReferenceMode(bool *f) = 0; + virtual yarp::dev::ReturnValue getReferenceMode(bool *f) = 0; /*! * Ask the controller to weigh more either the position or the @@ -180,14 +181,14 @@ class YARP_dev_API yarp::dev::ICartesianControl * \param p can be "position" or "orientation". * \return true/false on success/failure. */ - virtual bool setPosePriority(const std::string &p) = 0; + virtual yarp::dev::ReturnValue setPosePriority(const std::string &p) = 0; /*! * Get the current pose priority. [wait for reply] * \param p here is returned either as "position" or "orientation". * \return true/false on success/failure. */ - virtual bool getPosePriority(std::string &p) = 0; + virtual yarp::dev::ReturnValue getPosePriority(std::string &p) = 0; /*! * Get the current pose of the end-effector. [do not wait for @@ -201,8 +202,8 @@ class YARP_dev_API yarp::dev::ICartesianControl * pose. * \return true/false on success/failure. */ - virtual bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o, - yarp::os::Stamp *stamp=NULL) = 0; + virtual yarp::dev::ReturnValue getPose(yarp::sig::Vector &x, yarp::sig::Vector &o, + yarp::os::Stamp *stamp=NULL) = 0; /*! * Get the current pose of the specified link belonging to the @@ -218,9 +219,9 @@ class YARP_dev_API yarp::dev::ICartesianControl * pose. * \return true/false on success/failure. */ - virtual bool getPose(const int axis, yarp::sig::Vector &x, - yarp::sig::Vector &o, - yarp::os::Stamp *stamp=NULL) = 0; + virtual yarp::dev::ReturnValue getPose(const int axis, yarp::sig::Vector &x, + yarp::sig::Vector &o, + yarp::os::Stamp *stamp=NULL) = 0; /*! * Move the end-effector to a specified pose (position @@ -235,9 +236,9 @@ class YARP_dev_API yarp::dev::ICartesianControl * * \note Intended for streaming mode. */ - virtual bool goToPose(const yarp::sig::Vector &xd, - const yarp::sig::Vector &od, - const double t = 0.0) = 0; + virtual yarp::dev::ReturnValue goToPose(const yarp::sig::Vector &xd, + const yarp::sig::Vector &od, + const double t = 0.0) = 0; /*! * Move the end-effector to a specified position in cartesian @@ -250,8 +251,8 @@ class YARP_dev_API yarp::dev::ICartesianControl * * \note Intended for streaming mode. */ - virtual bool goToPosition(const yarp::sig::Vector &xd, - const double t = 0.0) = 0; + virtual yarp::dev::ReturnValue goToPosition(const yarp::sig::Vector &xd, + const double t = 0.0) = 0; /*! * Move the end-effector to a specified pose (position @@ -267,9 +268,9 @@ class YARP_dev_API yarp::dev::ICartesianControl * \note The reply is returned as soon as the controller has * initiated the movement. */ - virtual bool goToPoseSync(const yarp::sig::Vector &xd, - const yarp::sig::Vector &od, - const double t = 0.0) = 0; + virtual yarp::dev::ReturnValue goToPoseSync(const yarp::sig::Vector &xd, + const yarp::sig::Vector &od, + const double t = 0.0) = 0; /*! * Move the end-effector to a specified position in cartesian @@ -283,8 +284,8 @@ class YARP_dev_API yarp::dev::ICartesianControl * \note The reply is returned as soon as the controller has * initiated the movement. */ - virtual bool goToPositionSync(const yarp::sig::Vector &xd, - const double t = 0.0) = 0; + virtual yarp::dev::ReturnValue goToPositionSync(const yarp::sig::Vector &xd, + const double t = 0.0) = 0; /*! * Get the actual desired pose and joints configuration as result @@ -300,9 +301,9 @@ class YARP_dev_API yarp::dev::ICartesianControl * couple (xdhat,odhat) is achieved [deg]. * \return true/false on success/failure. */ - virtual bool getDesired(yarp::sig::Vector &xdhat, - yarp::sig::Vector &odhat, - yarp::sig::Vector &qdhat) = 0; + virtual yarp::dev::ReturnValue getDesired(yarp::sig::Vector &xdhat, + yarp::sig::Vector &odhat, + yarp::sig::Vector &qdhat) = 0; /*! * Ask for inverting a given pose without actually moving there. @@ -323,11 +324,11 @@ class YARP_dev_API yarp::dev::ICartesianControl * the couple (xdhat,odhat) is achieved [deg]. * \return true/false on success/failure. */ - virtual bool askForPose(const yarp::sig::Vector &xd, - const yarp::sig::Vector &od, - yarp::sig::Vector &xdhat, - yarp::sig::Vector &odhat, - yarp::sig::Vector &qdhat) = 0; + virtual yarp::dev::ReturnValue askForPose(const yarp::sig::Vector &xd, + const yarp::sig::Vector &od, + yarp::sig::Vector &xdhat, + yarp::sig::Vector &odhat, + yarp::sig::Vector &qdhat) = 0; /*! * Ask for inverting a given pose without actually moving there. @@ -351,12 +352,12 @@ class YARP_dev_API yarp::dev::ICartesianControl * the couple (xdhat,odhat) is achieved [deg]. * \return true/false on success/failure. */ - virtual bool askForPose(const yarp::sig::Vector &q0, - const yarp::sig::Vector &xd, - const yarp::sig::Vector &od, - yarp::sig::Vector &xdhat, - yarp::sig::Vector &odhat, - yarp::sig::Vector &qdhat) = 0; + virtual yarp::dev::ReturnValue askForPose(const yarp::sig::Vector &q0, + const yarp::sig::Vector &xd, + const yarp::sig::Vector &od, + yarp::sig::Vector &xdhat, + yarp::sig::Vector &odhat, + yarp::sig::Vector &qdhat) = 0; /*! * Ask for inverting a given position without actually moving @@ -374,10 +375,10 @@ class YARP_dev_API yarp::dev::ICartesianControl * the couple (xdhat,odhat) is achieved [deg]. * \return true/false on success/failure. */ - virtual bool askForPosition(const yarp::sig::Vector &xd, - yarp::sig::Vector &xdhat, - yarp::sig::Vector &odhat, - yarp::sig::Vector &qdhat) = 0; + virtual yarp::dev::ReturnValue askForPosition(const yarp::sig::Vector &xd, + yarp::sig::Vector &xdhat, + yarp::sig::Vector &odhat, + yarp::sig::Vector &qdhat) = 0; /*! * Ask for inverting a given position without actually moving @@ -398,11 +399,11 @@ class YARP_dev_API yarp::dev::ICartesianControl * the couple (xdhat,odhat) is achieved [deg]. * \return true/false on success/failure. */ - virtual bool askForPosition(const yarp::sig::Vector &q0, - const yarp::sig::Vector &xd, - yarp::sig::Vector &xdhat, - yarp::sig::Vector &odhat, - yarp::sig::Vector &qdhat) = 0; + virtual yarp::dev::ReturnValue askForPosition(const yarp::sig::Vector &q0, + const yarp::sig::Vector &xd, + yarp::sig::Vector &xdhat, + yarp::sig::Vector &odhat, + yarp::sig::Vector &qdhat) = 0; /*! * Get the current DOF configuration of the limb. [wait for @@ -416,7 +417,7 @@ class YARP_dev_API yarp::dev::ICartesianControl * associated joint is controlled (i.e. it is an actuated * DOF), 0 otherwise. */ - virtual bool getDOF(yarp::sig::Vector &curDof) = 0; + virtual yarp::dev::ReturnValue getDOF(yarp::sig::Vector &curDof) = 0; /*! * Set a new DOF configuration for the limb. [wait for reply] @@ -433,8 +434,8 @@ class YARP_dev_API yarp::dev::ICartesianControl * special value 2 indicates that the joint status won't be * modified (useful as a placeholder). */ - virtual bool setDOF(const yarp::sig::Vector &newDof, - yarp::sig::Vector &curDof) = 0; + virtual yarp::dev::ReturnValue setDOF(const yarp::sig::Vector &newDof, + yarp::sig::Vector &curDof) = 0; /*! * Get the current joints rest position. [wait for reply] @@ -447,7 +448,7 @@ class YARP_dev_API yarp::dev::ICartesianControl * rest position; further, each rest component may be * weighted differently providing the weights vector. */ - virtual bool getRestPos(yarp::sig::Vector &curRestPos) = 0; + virtual yarp::dev::ReturnValue getRestPos(yarp::sig::Vector &curRestPos) = 0; /*! * Set a new joints rest position. [wait for reply] @@ -463,8 +464,8 @@ class YARP_dev_API yarp::dev::ICartesianControl * rest position; further, each rest component may be * weighted differently providing the weights vector. */ - virtual bool setRestPos(const yarp::sig::Vector &newRestPos, - yarp::sig::Vector &curRestPos) = 0; + virtual yarp::dev::ReturnValue setRestPos(const yarp::sig::Vector &newRestPos, + yarp::sig::Vector &curRestPos) = 0; /*! * Get the current joints rest weights. [wait for reply] @@ -477,7 +478,7 @@ class YARP_dev_API yarp::dev::ICartesianControl * rest position; further, each rest component may be * weighted differently providing the weights vector. */ - virtual bool getRestWeights(yarp::sig::Vector &curRestWeights) = 0; + virtual yarp::dev::ReturnValue getRestWeights(yarp::sig::Vector &curRestWeights) = 0; /*! * Set a new joints rest position. [wait for reply] @@ -493,8 +494,8 @@ class YARP_dev_API yarp::dev::ICartesianControl * rest position; further, each rest component may be * weighted differently providing the weights vector. */ - virtual bool setRestWeights(const yarp::sig::Vector &newRestWeights, - yarp::sig::Vector &curRestWeights) = 0; + virtual yarp::dev::ReturnValue setRestWeights(const yarp::sig::Vector &newRestWeights, + yarp::sig::Vector &curRestWeights) = 0; /*! * Get the current range for the axis. [wait for reply] @@ -504,7 +505,7 @@ class YARP_dev_API yarp::dev::ICartesianControl * \param max where the maximum value is returned [deg]. * \return true/false on success/failure. */ - virtual bool getLimits(const int axis, double *min, double *max) = 0; + virtual yarp::dev::ReturnValue getLimits(const int axis, double *min, double *max) = 0; /*! * Set new range for the axis. Allowed range shall be a valid @@ -515,21 +516,21 @@ class YARP_dev_API yarp::dev::ICartesianControl * \param max the new maximum value [deg]. * \return true/false on success/failure. */ - virtual bool setLimits(const int axis, const double min, const double max) = 0; + virtual yarp::dev::ReturnValue setLimits(const int axis, const double min, const double max) = 0; /*! * Get the current trajectory duration. [wait for reply] * \param t the memory location where the time is returned [s]. * \return true/false on success/failure. */ - virtual bool getTrajTime(double *t) = 0; + virtual yarp::dev::ReturnValue getTrajTime(double *t) = 0; /*! * Set the duration of the trajectory. [wait for reply] * \param t time [s]. * \return true/false on success/failure. */ - virtual bool setTrajTime(const double t) = 0; + virtual yarp::dev::ReturnValue setTrajTime(const double t) = 0; /*! * Return tolerance for in-target check. [wait for reply] @@ -539,7 +540,7 @@ class YARP_dev_API yarp::dev::ICartesianControl * \note The trajectory is supposed to be completed as soon as * norm(xd-end_effector) diff --git a/src/libYARP_dev/src/yarp/dev/tests/ICartesianControlTest.cpp b/src/libYARP_dev/src/yarp/dev/tests/ICartesianControlTest.cpp new file mode 100644 index 00000000000..4d5fac7fbd0 --- /dev/null +++ b/src/libYARP_dev/src/yarp/dev/tests/ICartesianControlTest.cpp @@ -0,0 +1,6 @@ +/* + * SPDX-FileCopyrightText: 2025-2025 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include "ICartesianControlTest.h" diff --git a/src/libYARP_dev/src/yarp/dev/tests/ICartesianControlTest.h b/src/libYARP_dev/src/yarp/dev/tests/ICartesianControlTest.h new file mode 100644 index 00000000000..201142ca0ab --- /dev/null +++ b/src/libYARP_dev/src/yarp/dev/tests/ICartesianControlTest.h @@ -0,0 +1,325 @@ +/* + * SPDX-FileCopyrightText: 2025-2025 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef ICARTESIANTEST_H +#define ICARTESIANTEST_H + +#include +#include + +using namespace yarp::dev; +using namespace yarp::os; + +namespace yarp::dev::tests { +inline void exec_iCartesian_test_1(ICartesianControl* icart) +{ + REQUIRE(icart != nullptr); + + { + yarp::dev::ReturnValue r; + yarp::sig::Vector q0; + yarp::sig::Vector xd; + yarp::sig::Vector od; + yarp::sig::Vector xdhat; + yarp::sig::Vector odhat; + yarp::sig::Vector qdhat; + r = icart->askForPose(q0, + xd, + od, + xdhat, + odhat, + qdhat); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector q0; + yarp::sig::Vector xd; + yarp::sig::Vector xdhat; + yarp::sig::Vector odhat; + yarp::sig::Vector qdhat; + r = icart->askForPosition(q0, + xd, + xdhat, + odhat, + qdhat); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector x; + yarp::sig::Vector o; + r = icart->attachTipFrame(x,o); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + bool done; + r = icart->checkMotionDone(&done); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + int id = 0; + r = icart->deleteContext(id); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector xdhat; + yarp::sig::Vector odhat; + yarp::sig::Vector qdhat; + r = icart->getDesired(xdhat,odhat,qdhat); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector dof; + r = icart->getDOF(dof); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::os::Bottle info; + r = icart->getInfo(info); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + double tol = 0; + r = icart->getInTargetTol(&tol); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector qdot; + r = icart->getJointsVelocities(qdot); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + int axis = 0; + double min = 0; + double max = 0; + r = icart->getLimits(axis,&min,&max); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + int axis = 0; + yarp::sig::Vector x; + yarp::sig::Vector o; + yarp::os::Stamp stamp; + r = icart->getPose(axis, x, o, &stamp); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + std::string s; + r = icart->getPosePriority(s); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + bool mode; + r = icart->getReferenceMode(&mode); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector curRestPos; + r = icart->getRestPos(curRestPos); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector curRestWeights; + r = icart->getRestWeights(curRestWeights); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector xdot; + yarp::sig::Vector odot; + r = icart->setTaskVelocities(xdot,odot); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector x; + yarp::sig::Vector o; + r = icart->getTipFrame(x,o); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + bool mode; + r = icart->getTrackingMode(&mode); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + double t = 0; + r = icart->getTrajTime(&t); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector xd; + yarp::sig::Vector od; + double t = 1; + r = icart->goToPose(xd,od,t); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector xd; + yarp::sig::Vector od; + double t = 1; + r = icart->goToPoseSync(xd,od,t); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector xd; + double t = 1; + r = icart->goToPosition(xd,t); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector xd; + double t = 1; + r = icart->goToPositionSync(xd,t); + CHECK(r); + } + { + /* TO BE COMPLETED + yarp::dev::ReturnValue r; + CartesianEvent event; + r = icart->registerEvent(event); + CHECK(r); + */ + } + { + yarp::dev::ReturnValue r; + r = icart->removeTipFrame(); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + int id = 0; + r = icart->restoreContext(id); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector newDof; + yarp::sig::Vector curDof; + r = icart->setDOF(newDof, curDof); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + double tol = 0; + r = icart->setInTargetTol(tol); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + int axis = 0; + double min = 0; + double max = 0; + r = icart->setLimits(axis,min,max); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + std::string p; + r = icart->setPosePriority(p); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + bool mode = true; + r = icart->setReferenceMode(mode); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector newRestPos; + yarp::sig::Vector curRestPos; + r = icart->setRestPos(newRestPos,curRestPos); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector newRestWeights; + yarp::sig::Vector curRestWeights; + r = icart->setRestWeights(newRestWeights,curRestWeights); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::sig::Vector newRestWeights; + yarp::sig::Vector curRestWeights; + r = icart->setTaskVelocities(newRestWeights,curRestWeights); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + bool mode = true; + r = icart->setTrackingMode(mode); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + double t = 1; + r = icart->setTrajTime(t); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + r = icart->stopControl(); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + int id = 0; + r = icart->storeContext(&id); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::os::Bottle option; + r = icart->tweakGet(option); + CHECK(r); + } + { + yarp::dev::ReturnValue r; + yarp::os::Bottle option; + r = icart->tweakSet(option); + CHECK(r); + } + { + /* TO BE COMPLETED + yarp::dev::ReturnValue r; + CartesianEvent event; + r = icart->unregisterEvent(&event); + CHECK(r); + */ + } + { + yarp::dev::ReturnValue r; + double period = 1; + double timeout = 2; + r = icart->waitMotionDone(period,timeout); + CHECK(r); + } +} + +} + +#endif