From 704de8abe5d8a5091ceeff1344f80092a57c8064 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Fri, 19 Jan 2018 17:58:25 +0100 Subject: [PATCH 01/11] Add new parameter methods to ICartesianControl, prepare stub impl (#121) --- .../AmorCartesianControl.hpp | 8 +++ .../ICartesianControlImpl.cpp | 28 +++++++++++ .../BasicCartesianControl.hpp | 8 +++ .../ICartesianControlImpl.cpp | 28 +++++++++++ .../CartesianControlClient.hpp | 8 +++ .../ICartesianControlImpl.cpp | 28 +++++++++++ libraries/YarpPlugins/ICartesianControl.h | 49 +++++++++++++++++++ 7 files changed, 157 insertions(+) diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp index 0bc4a282d..f69d8c570 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp +++ b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp @@ -79,6 +79,14 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo virtual void pose(const std::vector &x, double interval); + virtual bool setParameter(int vocab, const std::string & value); + + virtual bool getParameter(int vocab, std::string & value); + + virtual bool setParameter(int vocab, double value); + + virtual bool setParameter(int vocab, double * value); + // -------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp -------- /** diff --git a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp index 73e6952c9..a0a959423 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp @@ -575,3 +575,31 @@ void roboticslab::AmorCartesianControl::pose(const std::vector &x, doubl } // ----------------------------------------------------------------------------- + +bool roboticslab::AmorCartesianControl::setParameter(int vocab, const std::string & value) +{ + return true; +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::AmorCartesianControl::getParameter(int vocab, std::string & value) +{ + return true; +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::AmorCartesianControl::setParameter(int vocab, double value) +{ + return true; +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::AmorCartesianControl::setParameter(int vocab, double * value) +{ + return true; +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp index 9d44c00c1..d236aa951 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp +++ b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp @@ -150,6 +150,14 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, public ICartesianC virtual void pose(const std::vector &x, double interval); + virtual bool setParameter(int vocab, const std::string & value); + + virtual bool getParameter(int vocab, std::string & value); + + virtual bool setParameter(int vocab, double value); + + virtual bool setParameter(int vocab, double * value); + // -------- RateThread declarations. Implementation in RateThreadImpl.cpp -------- /** Loop function. This is the thread itself. */ diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp index 915fc804b..970114a15 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp @@ -586,3 +586,31 @@ void roboticslab::BasicCartesianControl::pose(const std::vector &x, doub } // ----------------------------------------------------------------------------- + +bool roboticslab::BasicCartesianControl::setParameter(int vocab, const std::string & value) +{ + return true; +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::BasicCartesianControl::getParameter(int vocab, std::string & value) +{ + return true; +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::BasicCartesianControl::setParameter(int vocab, double value) +{ + return true; +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::BasicCartesianControl::setParameter(int vocab, double * value) +{ + return true; +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp b/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp index 06d9c9348..2bdb8d2b5 100644 --- a/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp +++ b/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp @@ -101,6 +101,14 @@ class CartesianControlClient : public yarp::dev::DeviceDriver, public ICartesian virtual void pose(const std::vector &x, double interval); + virtual bool setParameter(int vocab, const std::string & value); + + virtual bool getParameter(int vocab, std::string & value); + + virtual bool setParameter(int vocab, double value); + + virtual bool setParameter(int vocab, double * value); + // -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp -------- /** diff --git a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp index 44ef63bd2..fb9ffdfba 100644 --- a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp @@ -261,3 +261,31 @@ void roboticslab::CartesianControlClient::pose(const std::vector &x, dou } // ----------------------------------------------------------------------------- + +bool roboticslab::CartesianControlClient::setParameter(int vocab, const std::string & value) +{ + return true; +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::CartesianControlClient::getParameter(int vocab, std::string & value) +{ + return true; +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::CartesianControlClient::setParameter(int vocab, double value) +{ + return true; +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::CartesianControlClient::setParameter(int vocab, double * value) +{ + return true; +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/ICartesianControl.h b/libraries/YarpPlugins/ICartesianControl.h index aa16f4e3d..b0017ce8b 100644 --- a/libraries/YarpPlugins/ICartesianControl.h +++ b/libraries/YarpPlugins/ICartesianControl.h @@ -3,6 +3,7 @@ #ifndef __I_CARTESIAN_CONTROL__ #define __I_CARTESIAN_CONTROL__ +#include #include #include @@ -342,6 +343,54 @@ class ICartesianControl */ virtual void pose(const std::vector &x, double interval) = 0; + /** + * @brief Set a configuration parameter. + * + * Ask the controller to store or update a parameter of type std::string. + * + * @param vocab YARP-encoded vocab (parameter key). + * @param value Parameter value encoded as a string. + * + * @return true on success, false otherwise + */ + virtual bool setParameter(int vocab, const std::string & value) = 0; + + /** + * @brief Retrieve a configuration parameter. + * + * Ask the controller to retrieve a parameter of type std::string. + * + * @param vocab YARP-encoded vocab (parameter key). + * @param value Parameter value encoded as a string. + * + * @return true on success, false otherwise + */ + virtual bool getParameter(int vocab, std::string & value) = 0; + + /** + * @brief Set a configuration parameter. + * + * Ask the controller to store or update a parameter of type double. + * + * @param vocab YARP-encoded vocab (parameter key). + * @param value Parameter value encoded as a double. + * + * @return true on success, false otherwise + */ + virtual bool setParameter(int vocab, double value) = 0; + + /** + * @brief Retrieve a configuration parameter. + * + * Ask the controller to retrieve a parameter of type double. + * + * @param vocab YARP-encoded vocab (parameter key). + * @param value Parameter value encoded as a double. + * + * @return true on success, false otherwise + */ + virtual bool setParameter(int vocab, double * value) = 0; + /** @} */ }; From 8f6fbf59ad152bda0e8c20c50cae56ece69e53bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Fri, 19 Jan 2018 18:54:32 +0100 Subject: [PATCH 02/11] Define new controller configuration vocabs in ICartesianControl --- libraries/YarpPlugins/ICartesianControl.h | 29 +++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/libraries/YarpPlugins/ICartesianControl.h b/libraries/YarpPlugins/ICartesianControl.h index b0017ce8b..177fc0d96 100644 --- a/libraries/YarpPlugins/ICartesianControl.h +++ b/libraries/YarpPlugins/ICartesianControl.h @@ -74,6 +74,22 @@ /** @} */ +/** + * @name Controller configuration vocabs + * + * Used by @ref ICartesianControl_config_commands "Configuration accessors". + * + * @{ + */ + +// Controller configuration (parameter keys) +#define VOCAB_CC_CONFIG_GAIN VOCAB4('c','p','c','g') ///< Controller gain +#define VOCAB_CC_CONFIG_MAX_JOINT_VEL VOCAB4('c','p','j','v') ///< Maximum joint velocity +#define VOCAB_CC_CONFIG_TRAJ_DURATION VOCAB4('c','p','t','d') ///< Trajectory duration +#define VOCAB_CC_CONFIG_FRAME VOCAB4('c','p','r','f') ///< Reference frame + +/** @} */ + namespace roboticslab { @@ -343,6 +359,19 @@ class ICartesianControl */ virtual void pose(const std::vector &x, double interval) = 0; + /** @} */ + + //--------------------- Configuration accessors --------------------- + + /** + * @anchor ICartesianControl_config_commands + * @name Configuration accessors + * + * Configuration setters and getters with success/failure response. + * + * @{ + */ + /** * @brief Set a configuration parameter. * From a7d69e11e72f336e295402714f0c3bb79732272b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Fri, 19 Jan 2018 20:16:02 +0100 Subject: [PATCH 03/11] Implement parameter config methods in client/server pair, fix typo --- .../AmorCartesianControl.hpp | 2 +- .../ICartesianControlImpl.cpp | 2 +- .../BasicCartesianControl.hpp | 2 +- .../ICartesianControlImpl.cpp | 2 +- .../CartesianControlClient.hpp | 2 +- .../ICartesianControlImpl.cpp | 56 +++++++- .../CartesianControlServer.hpp | 5 + .../CartesianControlServer/RpcResponder.cpp | 132 +++++++++++++++++- libraries/YarpPlugins/ICartesianControl.h | 6 +- 9 files changed, 195 insertions(+), 14 deletions(-) diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp index f69d8c570..cb53786df 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp +++ b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp @@ -85,7 +85,7 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo virtual bool setParameter(int vocab, double value); - virtual bool setParameter(int vocab, double * value); + virtual bool getParameter(int vocab, double * value); // -------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp -------- diff --git a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp index a0a959423..1ceeea289 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp @@ -597,7 +597,7 @@ bool roboticslab::AmorCartesianControl::setParameter(int vocab, double value) // ----------------------------------------------------------------------------- -bool roboticslab::AmorCartesianControl::setParameter(int vocab, double * value) +bool roboticslab::AmorCartesianControl::getParameter(int vocab, double * value) { return true; } diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp index d236aa951..2af4d5587 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp +++ b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp @@ -156,7 +156,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, public ICartesianC virtual bool setParameter(int vocab, double value); - virtual bool setParameter(int vocab, double * value); + virtual bool getParameter(int vocab, double * value); // -------- RateThread declarations. Implementation in RateThreadImpl.cpp -------- diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp index 970114a15..9d72b335e 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp @@ -608,7 +608,7 @@ bool roboticslab::BasicCartesianControl::setParameter(int vocab, double value) // ----------------------------------------------------------------------------- -bool roboticslab::BasicCartesianControl::setParameter(int vocab, double * value) +bool roboticslab::BasicCartesianControl::getParameter(int vocab, double * value) { return true; } diff --git a/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp b/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp index 2bdb8d2b5..5c95c56a4 100644 --- a/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp +++ b/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp @@ -107,7 +107,7 @@ class CartesianControlClient : public yarp::dev::DeviceDriver, public ICartesian virtual bool setParameter(int vocab, double value); - virtual bool setParameter(int vocab, double * value); + virtual bool getParameter(int vocab, double * value); // -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp -------- diff --git a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp index fb9ffdfba..8387d9f8a 100644 --- a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp @@ -264,6 +264,19 @@ void roboticslab::CartesianControlClient::pose(const std::vector &x, dou bool roboticslab::CartesianControlClient::setParameter(int vocab, const std::string & value) { + yarp::os::Bottle cmd, response; + + cmd.addVocab(VOCAB_CC_CONFIG_SET_STRING); + cmd.addVocab(vocab); + cmd.addString(value); + + rpcClient.write(cmd, response); + + if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_FAILED) + { + return false; + } + return true; } @@ -271,6 +284,20 @@ bool roboticslab::CartesianControlClient::setParameter(int vocab, const std::str bool roboticslab::CartesianControlClient::getParameter(int vocab, std::string & value) { + yarp::os::Bottle cmd, response; + + cmd.addVocab(VOCAB_CC_CONFIG_GET_STRING); + cmd.addVocab(vocab); + + rpcClient.write(cmd, response); + + if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_FAILED) + { + return false; + } + + value = response.get(0).asString(); + return true; } @@ -278,13 +305,40 @@ bool roboticslab::CartesianControlClient::getParameter(int vocab, std::string & bool roboticslab::CartesianControlClient::setParameter(int vocab, double value) { + yarp::os::Bottle cmd, response; + + cmd.addVocab(VOCAB_CC_CONFIG_SET_DOUBLE); + cmd.addVocab(vocab); + cmd.addDouble(value); + + rpcClient.write(cmd, response); + + if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_FAILED) + { + return false; + } + return true; } // ----------------------------------------------------------------------------- -bool roboticslab::CartesianControlClient::setParameter(int vocab, double * value) +bool roboticslab::CartesianControlClient::getParameter(int vocab, double * value) { + yarp::os::Bottle cmd, response; + + cmd.addVocab(VOCAB_CC_CONFIG_GET_DOUBLE); + cmd.addVocab(vocab); + + rpcClient.write(cmd, response); + + if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_FAILED) + { + return false; + } + + *value = response.get(0).asDouble(); + return true; } diff --git a/libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp b/libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp index 4b511582f..510b8a157 100644 --- a/libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp +++ b/libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp @@ -131,6 +131,11 @@ class RpcResponder : public yarp::dev::DeviceResponder bool handleConsumerCmdMsg(const yarp::os::Bottle& in, yarp::os::Bottle& out, ConsumerFun cmd); bool handleFunctionCmdMsg(const yarp::os::Bottle& in, yarp::os::Bottle& out, FunctionFun cmd); + bool handleStringParameterSetter(const yarp::os::Bottle& in, yarp::os::Bottle& out); + bool handleStringParameterGetter(const yarp::os::Bottle& in, yarp::os::Bottle& out); + bool handleDoubleParameterSetter(const yarp::os::Bottle& in, yarp::os::Bottle& out); + bool handleDoubleParameterGetter(const yarp::os::Bottle& in, yarp::os::Bottle& out); + virtual bool transformIncomingData(std::vector& vin) { return true; diff --git a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp index 41ff60cf5..9ac99a82b 100644 --- a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp +++ b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp @@ -2,6 +2,8 @@ #include "CartesianControlServer.hpp" +#include + #include // ------------------- RpcResponder Related ------------------------------------ @@ -33,6 +35,14 @@ bool roboticslab::RpcResponder::respond(const yarp::os::Bottle& in, yarp::os::Bo return handleRunnableCmdMsg(in, out, &ICartesianControl::stopControl); case VOCAB_CC_TOOL: return handleConsumerCmdMsg(in, out, &ICartesianControl::tool); + case VOCAB_CC_CONFIG_SET_STRING: + return handleStringParameterSetter(in, out); + case VOCAB_CC_CONFIG_GET_STRING: + return handleStringParameterGetter(in, out); + case VOCAB_CC_CONFIG_SET_DOUBLE: + return handleDoubleParameterSetter(in, out); + case VOCAB_CC_CONFIG_GET_DOUBLE: + return handleDoubleParameterGetter(in, out); default: return DeviceResponder::respond(in, out); } @@ -43,15 +53,19 @@ bool roboticslab::RpcResponder::respond(const yarp::os::Bottle& in, yarp::os::Bo void roboticslab::RpcResponder::makeUsage() { addUsage("[stat]", "get current position in cartesian space"); - addUsage("[inv] $fCoord1 $fCoord2 ...", "accept desired position in cartesian space, return result in joint space"); - addUsage("[movj] $fCoord1 $fCoord2 ...", "joint move to desired position (absolute coordinates in cartesian space)"); - addUsage("[relj] $fCoord1 $fCoord2 ...", "joint move to desired position (relative coordinates in cartesian space)"); - addUsage("[movl] $fCoord1 $fCoord2 ...", "linear move to desired position (absolute coordinates in cartesian space)"); - addUsage("[movv] $fCoord1 $fCoord2 ...", "velocity move using supplied vector (cartesian space)"); + addUsage("[inv] coord1 coord2 ...", "accept desired position in cartesian space, return result in joint space"); + addUsage("[movj] coord1 coord2 ...", "joint move to desired position (absolute coordinates in cartesian space)"); + addUsage("[relj] coord1 coord2 ...", "joint move to desired position (relative coordinates in cartesian space)"); + addUsage("[movl] coord1 coord2 ...", "linear move to desired position (absolute coordinates in cartesian space)"); + addUsage("[movv] coord1 coord2 ...", "velocity move using supplied vector (cartesian space)"); addUsage("[gcmp]", "enable gravity compensation"); - addUsage("[forc] $fCoord1 $fCoord2 ...", "enable torque control, apply input forces (cartesian space)"); + addUsage("[forc] coord1 coord2 ...", "enable torque control, apply input forces (cartesian space)"); addUsage("[stop]", "stop control"); - addUsage("[tool] $fCoord1 $fCoord2 ...", "append fixed link to end effector"); + addUsage("[tool] coord1 coord2 ...", "append fixed link to end effector"); + addUsage("[cpss] vocab value", "set configuration parameter (string values)"); + addUsage("[cpgs] vocab", "get configuration parameter (string values)"); + addUsage("[cpsd] vocab value", "set configuration parameter (double values)"); + addUsage("[cpgd] vocab", "get configuration parameter (double values)"); } // ----------------------------------------------------------------------------- @@ -167,6 +181,110 @@ bool roboticslab::RpcResponder::handleFunctionCmdMsg(const yarp::os::Bottle& in, // ----------------------------------------------------------------------------- +bool roboticslab::RpcResponder::handleStringParameterSetter(const yarp::os::Bottle& in, yarp::os::Bottle& out) +{ + if (in.size() > 2) + { + int vocab = in.get(1).asVocab(); + std::string value = in.get(2).asString(); + + if (!iCartesianControl->setParameter(vocab, value)) + { + out.addVocab(VOCAB_FAILED); + return false; + } + + out.addVocab(VOCAB_OK); + return true; + } + else + { + CD_ERROR("size error\n"); + out.addVocab(VOCAB_FAILED); + return false; + } +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::RpcResponder::handleStringParameterGetter(const yarp::os::Bottle& in, yarp::os::Bottle& out) +{ + if (in.size() > 1) + { + int vocab = in.get(1).asVocab(); + std::string value; + + if (!iCartesianControl->getParameter(vocab, value)) + { + out.addVocab(VOCAB_FAILED); + return false; + } + + out.addString(value); + return true; + } + else + { + CD_ERROR("size error\n"); + out.addVocab(VOCAB_FAILED); + return false; + } +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::RpcResponder::handleDoubleParameterSetter(const yarp::os::Bottle& in, yarp::os::Bottle& out) +{ + if (in.size() > 2) + { + int vocab = in.get(1).asVocab(); + double value = in.get(2).asDouble(); + + if (!iCartesianControl->setParameter(vocab, value)) + { + out.addVocab(VOCAB_FAILED); + return false; + } + + out.addVocab(VOCAB_OK); + return true; + } + else + { + CD_ERROR("size error\n"); + out.addVocab(VOCAB_FAILED); + return false; + } +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::RpcResponder::handleDoubleParameterGetter(const yarp::os::Bottle& in, yarp::os::Bottle& out) +{ + if (in.size() > 1) + { + int vocab = in.get(1).asVocab(); + double value; + + if (!iCartesianControl->getParameter(vocab, &value)) + { + out.addVocab(VOCAB_FAILED); + return false; + } + + out.addDouble(value); + return true; + } + else + { + CD_ERROR("size error\n"); + out.addVocab(VOCAB_FAILED); + return false; + } +} + +// ----------------------------------------------------------------------------- + bool roboticslab::RpcTransformResponder::transformIncomingData(std::vector& vin) { return KinRepresentation::encodePose(vin, vin, KinRepresentation::CARTESIAN, orient, KinRepresentation::DEGREES); diff --git a/libraries/YarpPlugins/ICartesianControl.h b/libraries/YarpPlugins/ICartesianControl.h index 177fc0d96..9b3667d2e 100644 --- a/libraries/YarpPlugins/ICartesianControl.h +++ b/libraries/YarpPlugins/ICartesianControl.h @@ -83,6 +83,10 @@ */ // Controller configuration (parameter keys) +#define VOCAB_CC_CONFIG_SET_STRING VOCAB4('c','p','s','s') ///< Set parameter (string) +#define VOCAB_CC_CONFIG_GET_STRING VOCAB4('c','p','g','s') ///< Get parameter (string) +#define VOCAB_CC_CONFIG_SET_DOUBLE VOCAB4('c','p','s','d') ///< Set parameter (double) +#define VOCAB_CC_CONFIG_GET_DOUBLE VOCAB4('c','p','g','d') ///< Get parameter (double) #define VOCAB_CC_CONFIG_GAIN VOCAB4('c','p','c','g') ///< Controller gain #define VOCAB_CC_CONFIG_MAX_JOINT_VEL VOCAB4('c','p','j','v') ///< Maximum joint velocity #define VOCAB_CC_CONFIG_TRAJ_DURATION VOCAB4('c','p','t','d') ///< Trajectory duration @@ -418,7 +422,7 @@ class ICartesianControl * * @return true on success, false otherwise */ - virtual bool setParameter(int vocab, double * value) = 0; + virtual bool getParameter(int vocab, double * value) = 0; /** @} */ }; From 31e6c5b771f7d0ba91524eb2d004faf67a5ad681 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Fri, 19 Jan 2018 20:49:04 +0100 Subject: [PATCH 04/11] Prepare switch statements in stub (get|set)Parameter implementations --- .../ICartesianControlImpl.cpp | 46 +++++++++++++++++++ .../ICartesianControlImpl.cpp | 46 +++++++++++++++++++ libraries/YarpPlugins/ICartesianControl.h | 8 ++-- 3 files changed, 96 insertions(+), 4 deletions(-) diff --git a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp index 1ceeea289..e64fa6b5c 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp @@ -4,6 +4,8 @@ #include +#include + #include #include "KinematicRepresentation.hpp" @@ -578,6 +580,15 @@ void roboticslab::AmorCartesianControl::pose(const std::vector &x, doubl bool roboticslab::AmorCartesianControl::setParameter(int vocab, const std::string & value) { + switch (vocab) + { + case VOCAB_CC_CONFIG_FRAME: + break; + default: + CD_ERROR("Unrecognized config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); + return false; + } + return true; } @@ -585,6 +596,15 @@ bool roboticslab::AmorCartesianControl::setParameter(int vocab, const std::strin bool roboticslab::AmorCartesianControl::getParameter(int vocab, std::string & value) { + switch (vocab) + { + case VOCAB_CC_CONFIG_FRAME: + break; + default: + CD_ERROR("Unrecognized config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); + return false; + } + return true; } @@ -592,6 +612,19 @@ bool roboticslab::AmorCartesianControl::getParameter(int vocab, std::string & va bool roboticslab::AmorCartesianControl::setParameter(int vocab, double value) { + switch (vocab) + { + case VOCAB_CC_CONFIG_GAIN: + break; + case VOCAB_CC_CONFIG_MAX_JOINT_VEL: + break; + case VOCAB_CC_CONFIG_TRAJ_DURATION: + break; + default: + CD_ERROR("Unrecognized config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); + return false; + } + return true; } @@ -599,6 +632,19 @@ bool roboticslab::AmorCartesianControl::setParameter(int vocab, double value) bool roboticslab::AmorCartesianControl::getParameter(int vocab, double * value) { + switch (vocab) + { + case VOCAB_CC_CONFIG_GAIN: + break; + case VOCAB_CC_CONFIG_MAX_JOINT_VEL: + break; + case VOCAB_CC_CONFIG_TRAJ_DURATION: + break; + default: + CD_ERROR("Unrecognized config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); + return false; + } + return true; } diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp index 9d72b335e..2c1d20052 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp @@ -6,6 +6,8 @@ #include #include +#include + #include // ------------------- ICartesianControl Related ------------------------------------ @@ -589,6 +591,15 @@ void roboticslab::BasicCartesianControl::pose(const std::vector &x, doub bool roboticslab::BasicCartesianControl::setParameter(int vocab, const std::string & value) { + switch (vocab) + { + case VOCAB_CC_CONFIG_FRAME: + break; + default: + CD_ERROR("Unrecognized config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); + return false; + } + return true; } @@ -596,6 +607,15 @@ bool roboticslab::BasicCartesianControl::setParameter(int vocab, const std::stri bool roboticslab::BasicCartesianControl::getParameter(int vocab, std::string & value) { + switch (vocab) + { + case VOCAB_CC_CONFIG_FRAME: + break; + default: + CD_ERROR("Unrecognized config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); + return false; + } + return true; } @@ -603,6 +623,19 @@ bool roboticslab::BasicCartesianControl::getParameter(int vocab, std::string & v bool roboticslab::BasicCartesianControl::setParameter(int vocab, double value) { + switch (vocab) + { + case VOCAB_CC_CONFIG_GAIN: + break; + case VOCAB_CC_CONFIG_MAX_JOINT_VEL: + break; + case VOCAB_CC_CONFIG_TRAJ_DURATION: + break; + default: + CD_ERROR("Unrecognized config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); + return false; + } + return true; } @@ -610,6 +643,19 @@ bool roboticslab::BasicCartesianControl::setParameter(int vocab, double value) bool roboticslab::BasicCartesianControl::getParameter(int vocab, double * value) { + switch (vocab) + { + case VOCAB_CC_CONFIG_GAIN: + break; + case VOCAB_CC_CONFIG_MAX_JOINT_VEL: + break; + case VOCAB_CC_CONFIG_TRAJ_DURATION: + break; + default: + CD_ERROR("Unrecognized config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); + return false; + } + return true; } diff --git a/libraries/YarpPlugins/ICartesianControl.h b/libraries/YarpPlugins/ICartesianControl.h index 9b3667d2e..7c1f8f224 100644 --- a/libraries/YarpPlugins/ICartesianControl.h +++ b/libraries/YarpPlugins/ICartesianControl.h @@ -87,10 +87,10 @@ #define VOCAB_CC_CONFIG_GET_STRING VOCAB4('c','p','g','s') ///< Get parameter (string) #define VOCAB_CC_CONFIG_SET_DOUBLE VOCAB4('c','p','s','d') ///< Set parameter (double) #define VOCAB_CC_CONFIG_GET_DOUBLE VOCAB4('c','p','g','d') ///< Get parameter (double) -#define VOCAB_CC_CONFIG_GAIN VOCAB4('c','p','c','g') ///< Controller gain -#define VOCAB_CC_CONFIG_MAX_JOINT_VEL VOCAB4('c','p','j','v') ///< Maximum joint velocity -#define VOCAB_CC_CONFIG_TRAJ_DURATION VOCAB4('c','p','t','d') ///< Trajectory duration -#define VOCAB_CC_CONFIG_FRAME VOCAB4('c','p','r','f') ///< Reference frame +#define VOCAB_CC_CONFIG_GAIN VOCAB4('c','p','c','g') ///< Controller gain (double) +#define VOCAB_CC_CONFIG_MAX_JOINT_VEL VOCAB4('c','p','j','v') ///< Maximum joint velocity (double) +#define VOCAB_CC_CONFIG_TRAJ_DURATION VOCAB4('c','p','t','d') ///< Trajectory duration (double) +#define VOCAB_CC_CONFIG_FRAME VOCAB4('c','p','r','f') ///< Reference frame (string) /** @} */ From 62470fe166c5916b05c0e0c19071def6cb381cd3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Fri, 19 Jan 2018 21:27:18 +0100 Subject: [PATCH 05/11] Parameterize gain, maximum joint velocity and trajectory duration --- .../AmorCartesianControl.cpp | 4 +- .../AmorCartesianControl.hpp | 10 ++- .../AmorCartesianControl/DeviceDriverImpl.cpp | 6 ++ .../ICartesianControlImpl.cpp | 30 ++++++--- .../BasicCartesianControl.hpp | 5 +- .../DeviceDriverImpl.cpp | 2 + .../ICartesianControlImpl.cpp | 66 +++++++++++++------ .../BasicCartesianControl/RateThreadImpl.cpp | 6 +- 8 files changed, 90 insertions(+), 39 deletions(-) diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp index 4507b4e5e..b0c5c47fd 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp @@ -40,9 +40,9 @@ bool roboticslab::AmorCartesianControl::checkJointVelocities(const std::vector MAX_ANG_VEL) + if (std::abs(qdot[i]) > maxJointVelocity) { - CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], MAX_ANG_VEL); + CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], maxJointVelocity); return false; } } diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp index cb53786df..d1ecead7b 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp +++ b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp @@ -15,7 +15,8 @@ #define DEFAULT_CAN_LIBRARY "libeddriver.so" #define DEFAULT_CAN_PORT 0 -#define MAX_ANG_VEL 10.0 +#define DEFAULT_GAIN 0.05 +#define DEFAULT_QDOT_LIMIT 10.0 namespace roboticslab { @@ -40,7 +41,9 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo AmorCartesianControl() : handle(AMOR_INVALID_HANDLE), ownsHandle(false), iCartesianSolver(NULL), - currentState(VOCAB_CC_NOT_CONTROLLING) + currentState(VOCAB_CC_NOT_CONTROLLING), + gain(DEFAULT_GAIN), + maxJointVelocity(DEFAULT_QDOT_LIMIT) {} // -- ICartesianControl declarations. Implementation in ICartesianControlImpl.cpp -- @@ -123,6 +126,9 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo roboticslab::ICartesianSolver *iCartesianSolver; int currentState; + + double gain; + double maxJointVelocity; }; } // namespace roboticslab diff --git a/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp index b891034a4..181439000 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp @@ -15,6 +15,12 @@ bool roboticslab::AmorCartesianControl::open(yarp::os::Searchable& config) { CD_DEBUG("AmorCartesianControl config: %s.\n", config.toString().c_str()); + gain = config.check("controllerGain", yarp::os::Value(DEFAULT_GAIN), + "controller gain").asDouble(); + + maxJointVelocity = config.check("maxJointVelocity", yarp::os::Value(DEFAULT_QDOT_LIMIT), + "maximum joint velocity").asDouble(); + std::string kinematicsFile = config.check("kinematics", yarp::os::Value(""), "AMOR kinematics description").asString(); diff --git a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp index e64fa6b5c..33bdb4f72 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp @@ -541,7 +541,7 @@ void roboticslab::AmorCartesianControl::pose(const std::vector &x, doubl } std::vector xdot(xd.size()); - const double factor = 0.05 / interval; // DEFAULT_GAIN = 0.05 + const double factor = gain / interval; for (int i = 0; i < xd.size(); i++) { @@ -585,7 +585,7 @@ bool roboticslab::AmorCartesianControl::setParameter(int vocab, const std::strin case VOCAB_CC_CONFIG_FRAME: break; default: - CD_ERROR("Unrecognized config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); + CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); return false; } @@ -601,7 +601,7 @@ bool roboticslab::AmorCartesianControl::getParameter(int vocab, std::string & va case VOCAB_CC_CONFIG_FRAME: break; default: - CD_ERROR("Unrecognized config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); + CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); return false; } @@ -615,13 +615,25 @@ bool roboticslab::AmorCartesianControl::setParameter(int vocab, double value) switch (vocab) { case VOCAB_CC_CONFIG_GAIN: + if (value < 0.0) + { + CD_ERROR("Controller gain cannot be negative.\n"); + return false; + } + + gain = value; break; case VOCAB_CC_CONFIG_MAX_JOINT_VEL: - break; - case VOCAB_CC_CONFIG_TRAJ_DURATION: + if (value <= 0.0) + { + CD_ERROR("Maximum joint velocity cannot be negative nor zero.\n"); + return false; + } + + maxJointVelocity = value; break; default: - CD_ERROR("Unrecognized config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); + CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); return false; } @@ -635,13 +647,13 @@ bool roboticslab::AmorCartesianControl::getParameter(int vocab, double * value) switch (vocab) { case VOCAB_CC_CONFIG_GAIN: + *value = gain; break; case VOCAB_CC_CONFIG_MAX_JOINT_VEL: - break; - case VOCAB_CC_CONFIG_TRAJ_DURATION: + *value = maxJointVelocity; break; default: - CD_ERROR("Unrecognized config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); + CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); return false; } diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp index 5bf68d6ba..48ec9fc3e 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp +++ b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp @@ -20,9 +20,8 @@ #define DEFAULT_ROBOT "remote_controlboard" #define DEFAULT_INIT_STATE VOCAB_CC_NOT_CONTROLLING #define DEFAULT_MS 50 -#define MAX_ANG_VEL 7.5 #define DEFAULT_GAIN 0.05 -#define DEFAULT_QDOT_LIMIT 10 +#define DEFAULT_QDOT_LIMIT 10.0 namespace roboticslab { @@ -204,6 +203,8 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, public ICartesianC yarp::dev::ITorqueControl *iTorqueControl; yarp::dev::IControlMode *iControlMode; + double gain; + double maxJointVelocity; double duration; // [s] int numRobotJoints, numSolverJoints; diff --git a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp index d9397961e..b38159103 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp @@ -10,6 +10,8 @@ bool roboticslab::BasicCartesianControl::open(yarp::os::Searchable& config) { CD_DEBUG("BasicCartesianControl config: %s.\n", config.toString().c_str()); + gain = config.check("controllerGain",yarp::os::Value(DEFAULT_GAIN),"controller gain").asDouble(); + maxJointVelocity = config.check("maxJointVelocity",yarp::os::Value(DEFAULT_QDOT_LIMIT),"maximum joint velocity").asDouble(); duration = config.check("trajectoryDuration",yarp::os::Value(DEFAULT_DURATION),"trajectory duration").asDouble(); std::string solverStr = config.check("solver",yarp::os::Value(DEFAULT_SOLVER),"cartesian solver").asString(); diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp index 2c1d20052..aa92421e3 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp @@ -68,9 +68,9 @@ bool roboticslab::BasicCartesianControl::movj(const std::vector &xd) for(unsigned int joint=0;joint max_time) + if (std::abs((qd[joint]-currentQ[joint]) / maxJointVelocity) > max_time) { - max_time = std::abs( (qd[joint]-currentQ[joint]) / MAX_ANG_VEL); + max_time = std::abs( (qd[joint]-currentQ[joint]) / maxJointVelocity); CD_INFO(" -->candidate: %f\n",max_time); } } @@ -303,9 +303,9 @@ void roboticslab::BasicCartesianControl::fwd(const std::vector &rot, dou for (unsigned int i = 0; i < qdot.size(); i++) { - if ( std::abs(qdot[i]) > MAX_ANG_VEL ) + if ( std::abs(qdot[i]) > maxJointVelocity ) { - CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], MAX_ANG_VEL); + CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], maxJointVelocity); std::fill(qdot.begin(), qdot.end(), 0.0); iVelocityControl->velocityMove(qdot.data()); return; @@ -349,9 +349,9 @@ void roboticslab::BasicCartesianControl::bkwd(const std::vector &rot, do for (unsigned int i = 0; i < qdot.size(); i++) { - if ( std::abs(qdot[i]) > MAX_ANG_VEL ) + if ( std::abs(qdot[i]) > maxJointVelocity ) { - CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], MAX_ANG_VEL); + CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], maxJointVelocity); std::fill(qdot.begin(), qdot.end(), 0.0); iVelocityControl->velocityMove(qdot.data()); return; @@ -394,9 +394,9 @@ void roboticslab::BasicCartesianControl::rot(const std::vector &rot) for (unsigned int i = 0; i < qdot.size(); i++) { - if ( std::abs(qdot[i]) > MAX_ANG_VEL ) + if ( std::abs(qdot[i]) > maxJointVelocity ) { - CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], MAX_ANG_VEL); + CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], maxJointVelocity); std::fill(qdot.begin(), qdot.end(), 0.0); iVelocityControl->velocityMove(qdot.data()); return; @@ -439,9 +439,9 @@ void roboticslab::BasicCartesianControl::pan(const std::vector &transl) for (unsigned int i = 0; i < qdot.size(); i++) { - if ( std::abs(qdot[i]) > MAX_ANG_VEL ) + if ( std::abs(qdot[i]) > maxJointVelocity ) { - CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], MAX_ANG_VEL); + CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], maxJointVelocity); std::fill(qdot.begin(), qdot.end(), 0.0); iVelocityControl->velocityMove(qdot.data()); return; @@ -479,9 +479,9 @@ void roboticslab::BasicCartesianControl::vmos(const std::vector &xdot) for (unsigned int i = 0; i < qdot.size(); i++) { - if ( std::abs(qdot[i]) > MAX_ANG_VEL ) + if ( std::abs(qdot[i]) > maxJointVelocity ) { - CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], MAX_ANG_VEL); + CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], maxJointVelocity); std::fill(qdot.begin(), qdot.end(), 0.0); iVelocityControl->velocityMove(qdot.data()); return; @@ -519,9 +519,9 @@ void roboticslab::BasicCartesianControl::eff(const std::vector &xdotee) for (unsigned int i = 0; i < qdot.size(); i++) { - if ( std::abs(qdot[i]) > MAX_ANG_VEL ) + if ( std::abs(qdot[i]) > maxJointVelocity ) { - CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], MAX_ANG_VEL); + CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], maxJointVelocity); std::fill(qdot.begin(), qdot.end(), 0.0); iVelocityControl->velocityMove(qdot.data()); return; @@ -554,7 +554,7 @@ void roboticslab::BasicCartesianControl::pose(const std::vector &x, doub } std::vector xdot(xd.size()); - const double factor = DEFAULT_GAIN / interval; + const double factor = gain / interval; std::transform(xd.begin(), xd.end(), xdot.begin(), std::bind1st(std::multiplies(), factor)); for (unsigned int joint = 0; joint < numRobotJoints; joint++) @@ -571,9 +571,9 @@ void roboticslab::BasicCartesianControl::pose(const std::vector &x, doub for (unsigned int i = 0; i < qdot.size(); i++) { - if ( std::abs(qdot[i]) > MAX_ANG_VEL ) + if ( std::abs(qdot[i]) > maxJointVelocity ) { - CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], MAX_ANG_VEL); + CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], maxJointVelocity); std::fill(qdot.begin(), qdot.end(), 0.0); iVelocityControl->velocityMove(qdot.data()); return; @@ -596,7 +596,7 @@ bool roboticslab::BasicCartesianControl::setParameter(int vocab, const std::stri case VOCAB_CC_CONFIG_FRAME: break; default: - CD_ERROR("Unrecognized config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); + CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); return false; } @@ -612,7 +612,7 @@ bool roboticslab::BasicCartesianControl::getParameter(int vocab, std::string & v case VOCAB_CC_CONFIG_FRAME: break; default: - CD_ERROR("Unrecognized config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); + CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); return false; } @@ -626,13 +626,34 @@ bool roboticslab::BasicCartesianControl::setParameter(int vocab, double value) switch (vocab) { case VOCAB_CC_CONFIG_GAIN: + if (value < 0.0) + { + CD_ERROR("Controller gain cannot be negative.\n"); + return false; + } + + gain = value; break; case VOCAB_CC_CONFIG_MAX_JOINT_VEL: + if (value <= 0.0) + { + CD_ERROR("Maximum joint velocity cannot be negative nor zero.\n"); + return false; + } + + maxJointVelocity = value; break; case VOCAB_CC_CONFIG_TRAJ_DURATION: + if (value <= 0.0) + { + CD_ERROR("Trajectory duration cannot be negative nor zero.\n"); + return false; + } + + duration = value; break; default: - CD_ERROR("Unrecognized config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); + CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); return false; } @@ -646,13 +667,16 @@ bool roboticslab::BasicCartesianControl::getParameter(int vocab, double * value) switch (vocab) { case VOCAB_CC_CONFIG_GAIN: + *value = gain; break; case VOCAB_CC_CONFIG_MAX_JOINT_VEL: + *value = maxJointVelocity; break; case VOCAB_CC_CONFIG_TRAJ_DURATION: + *value = duration; break; default: - CD_ERROR("Unrecognized config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); + CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); return false; } diff --git a/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp index 38247092b..2e3c260da 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp @@ -61,7 +61,7 @@ void roboticslab::BasicCartesianControl::handleMovl() for (int i = 0; i < 6; i++) { - commandXdot[i] *= DEFAULT_GAIN * (1000.0 / DEFAULT_MS); + commandXdot[i] *= gain * (1000.0 / DEFAULT_MS); commandXdot[i] += desiredXdot[i]; } @@ -92,7 +92,7 @@ void roboticslab::BasicCartesianControl::handleMovl() for (int i = 0; i < commandQdot.size(); i++) { - if (std::abs(commandQdot[i]) > DEFAULT_QDOT_LIMIT) + if (std::abs(commandQdot[i]) > maxJointVelocity) { CD_ERROR("diffInvKin too dangerous, STOP!!!\n"); stopControl(); @@ -146,7 +146,7 @@ void roboticslab::BasicCartesianControl::handleMovv() for (int i = 0; i < commandQdot.size(); i++) { - if (std::abs(commandQdot[i]) > DEFAULT_QDOT_LIMIT) + if (std::abs(commandQdot[i]) > maxJointVelocity) { CD_ERROR("diffInvKin too dangerous, STOP!!!\n"); stopControl(); From 29ececc790e4d5f7a14bb389e24e826143409c55 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Fri, 19 Jan 2018 22:26:27 +0100 Subject: [PATCH 06/11] Drop configuration getter and setters for string-type parameters --- .../AmorCartesianControl.hpp | 4 - .../ICartesianControlImpl.cpp | 32 -------- .../BasicCartesianControl.hpp | 4 - .../ICartesianControlImpl.cpp | 32 -------- .../CartesianControlClient.hpp | 4 - .../ICartesianControlImpl.cpp | 45 +---------- .../CartesianControlServer.hpp | 6 +- .../CartesianControlServer/RpcResponder.cpp | 74 ++----------------- libraries/YarpPlugins/ICartesianControl.h | 42 ++--------- 9 files changed, 20 insertions(+), 223 deletions(-) diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp index d1ecead7b..ef74e4830 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp +++ b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp @@ -82,10 +82,6 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo virtual void pose(const std::vector &x, double interval); - virtual bool setParameter(int vocab, const std::string & value); - - virtual bool getParameter(int vocab, std::string & value); - virtual bool setParameter(int vocab, double value); virtual bool getParameter(int vocab, double * value); diff --git a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp index 33bdb4f72..bda5def4c 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp @@ -578,38 +578,6 @@ void roboticslab::AmorCartesianControl::pose(const std::vector &x, doubl // ----------------------------------------------------------------------------- -bool roboticslab::AmorCartesianControl::setParameter(int vocab, const std::string & value) -{ - switch (vocab) - { - case VOCAB_CC_CONFIG_FRAME: - break; - default: - CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); - return false; - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool roboticslab::AmorCartesianControl::getParameter(int vocab, std::string & value) -{ - switch (vocab) - { - case VOCAB_CC_CONFIG_FRAME: - break; - default: - CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); - return false; - } - - return true; -} - -// ----------------------------------------------------------------------------- - bool roboticslab::AmorCartesianControl::setParameter(int vocab, double value) { switch (vocab) diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp index 48ec9fc3e..86ef07bd7 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp +++ b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp @@ -149,10 +149,6 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, public ICartesianC virtual void pose(const std::vector &x, double interval); - virtual bool setParameter(int vocab, const std::string & value); - - virtual bool getParameter(int vocab, std::string & value); - virtual bool setParameter(int vocab, double value); virtual bool getParameter(int vocab, double * value); diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp index aa92421e3..63e5c6903 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp @@ -589,38 +589,6 @@ void roboticslab::BasicCartesianControl::pose(const std::vector &x, doub // ----------------------------------------------------------------------------- -bool roboticslab::BasicCartesianControl::setParameter(int vocab, const std::string & value) -{ - switch (vocab) - { - case VOCAB_CC_CONFIG_FRAME: - break; - default: - CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); - return false; - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool roboticslab::BasicCartesianControl::getParameter(int vocab, std::string & value) -{ - switch (vocab) - { - case VOCAB_CC_CONFIG_FRAME: - break; - default: - CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); - return false; - } - - return true; -} - -// ----------------------------------------------------------------------------- - bool roboticslab::BasicCartesianControl::setParameter(int vocab, double value) { switch (vocab) diff --git a/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp b/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp index 5c95c56a4..ed3a03451 100644 --- a/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp +++ b/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp @@ -101,10 +101,6 @@ class CartesianControlClient : public yarp::dev::DeviceDriver, public ICartesian virtual void pose(const std::vector &x, double interval); - virtual bool setParameter(int vocab, const std::string & value); - - virtual bool getParameter(int vocab, std::string & value); - virtual bool setParameter(int vocab, double value); virtual bool getParameter(int vocab, double * value); diff --git a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp index 8387d9f8a..8dccddd40 100644 --- a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp @@ -262,52 +262,11 @@ void roboticslab::CartesianControlClient::pose(const std::vector &x, dou // ----------------------------------------------------------------------------- -bool roboticslab::CartesianControlClient::setParameter(int vocab, const std::string & value) -{ - yarp::os::Bottle cmd, response; - - cmd.addVocab(VOCAB_CC_CONFIG_SET_STRING); - cmd.addVocab(vocab); - cmd.addString(value); - - rpcClient.write(cmd, response); - - if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_FAILED) - { - return false; - } - - return true; -} - -// ----------------------------------------------------------------------------- - -bool roboticslab::CartesianControlClient::getParameter(int vocab, std::string & value) -{ - yarp::os::Bottle cmd, response; - - cmd.addVocab(VOCAB_CC_CONFIG_GET_STRING); - cmd.addVocab(vocab); - - rpcClient.write(cmd, response); - - if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_FAILED) - { - return false; - } - - value = response.get(0).asString(); - - return true; -} - -// ----------------------------------------------------------------------------- - bool roboticslab::CartesianControlClient::setParameter(int vocab, double value) { yarp::os::Bottle cmd, response; - cmd.addVocab(VOCAB_CC_CONFIG_SET_DOUBLE); + cmd.addVocab(VOCAB_CC_CONFIG_SET); cmd.addVocab(vocab); cmd.addDouble(value); @@ -327,7 +286,7 @@ bool roboticslab::CartesianControlClient::getParameter(int vocab, double * value { yarp::os::Bottle cmd, response; - cmd.addVocab(VOCAB_CC_CONFIG_GET_DOUBLE); + cmd.addVocab(VOCAB_CC_CONFIG_GET); cmd.addVocab(vocab); rpcClient.write(cmd, response); diff --git a/libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp b/libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp index 510b8a157..6fef7a4f8 100644 --- a/libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp +++ b/libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp @@ -131,10 +131,8 @@ class RpcResponder : public yarp::dev::DeviceResponder bool handleConsumerCmdMsg(const yarp::os::Bottle& in, yarp::os::Bottle& out, ConsumerFun cmd); bool handleFunctionCmdMsg(const yarp::os::Bottle& in, yarp::os::Bottle& out, FunctionFun cmd); - bool handleStringParameterSetter(const yarp::os::Bottle& in, yarp::os::Bottle& out); - bool handleStringParameterGetter(const yarp::os::Bottle& in, yarp::os::Bottle& out); - bool handleDoubleParameterSetter(const yarp::os::Bottle& in, yarp::os::Bottle& out); - bool handleDoubleParameterGetter(const yarp::os::Bottle& in, yarp::os::Bottle& out); + bool handleParameterSetter(const yarp::os::Bottle& in, yarp::os::Bottle& out); + bool handleParameterGetter(const yarp::os::Bottle& in, yarp::os::Bottle& out); virtual bool transformIncomingData(std::vector& vin) { diff --git a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp index 9ac99a82b..705f70f2a 100644 --- a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp +++ b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp @@ -35,14 +35,10 @@ bool roboticslab::RpcResponder::respond(const yarp::os::Bottle& in, yarp::os::Bo return handleRunnableCmdMsg(in, out, &ICartesianControl::stopControl); case VOCAB_CC_TOOL: return handleConsumerCmdMsg(in, out, &ICartesianControl::tool); - case VOCAB_CC_CONFIG_SET_STRING: - return handleStringParameterSetter(in, out); - case VOCAB_CC_CONFIG_GET_STRING: - return handleStringParameterGetter(in, out); - case VOCAB_CC_CONFIG_SET_DOUBLE: - return handleDoubleParameterSetter(in, out); - case VOCAB_CC_CONFIG_GET_DOUBLE: - return handleDoubleParameterGetter(in, out); + case VOCAB_CC_CONFIG_SET: + return handleParameterSetter(in, out); + case VOCAB_CC_CONFIG_GET: + return handleParameterGetter(in, out); default: return DeviceResponder::respond(in, out); } @@ -62,10 +58,8 @@ void roboticslab::RpcResponder::makeUsage() addUsage("[forc] coord1 coord2 ...", "enable torque control, apply input forces (cartesian space)"); addUsage("[stop]", "stop control"); addUsage("[tool] coord1 coord2 ...", "append fixed link to end effector"); - addUsage("[cpss] vocab value", "set configuration parameter (string values)"); - addUsage("[cpgs] vocab", "get configuration parameter (string values)"); - addUsage("[cpsd] vocab value", "set configuration parameter (double values)"); - addUsage("[cpgd] vocab", "get configuration parameter (double values)"); + addUsage("[cps] vocab value", "set configuration parameter"); + addUsage("[cpg] vocab", "get configuration parameter"); } // ----------------------------------------------------------------------------- @@ -181,59 +175,7 @@ bool roboticslab::RpcResponder::handleFunctionCmdMsg(const yarp::os::Bottle& in, // ----------------------------------------------------------------------------- -bool roboticslab::RpcResponder::handleStringParameterSetter(const yarp::os::Bottle& in, yarp::os::Bottle& out) -{ - if (in.size() > 2) - { - int vocab = in.get(1).asVocab(); - std::string value = in.get(2).asString(); - - if (!iCartesianControl->setParameter(vocab, value)) - { - out.addVocab(VOCAB_FAILED); - return false; - } - - out.addVocab(VOCAB_OK); - return true; - } - else - { - CD_ERROR("size error\n"); - out.addVocab(VOCAB_FAILED); - return false; - } -} - -// ----------------------------------------------------------------------------- - -bool roboticslab::RpcResponder::handleStringParameterGetter(const yarp::os::Bottle& in, yarp::os::Bottle& out) -{ - if (in.size() > 1) - { - int vocab = in.get(1).asVocab(); - std::string value; - - if (!iCartesianControl->getParameter(vocab, value)) - { - out.addVocab(VOCAB_FAILED); - return false; - } - - out.addString(value); - return true; - } - else - { - CD_ERROR("size error\n"); - out.addVocab(VOCAB_FAILED); - return false; - } -} - -// ----------------------------------------------------------------------------- - -bool roboticslab::RpcResponder::handleDoubleParameterSetter(const yarp::os::Bottle& in, yarp::os::Bottle& out) +bool roboticslab::RpcResponder::handleParameterSetter(const yarp::os::Bottle& in, yarp::os::Bottle& out) { if (in.size() > 2) { @@ -259,7 +201,7 @@ bool roboticslab::RpcResponder::handleDoubleParameterSetter(const yarp::os::Bott // ----------------------------------------------------------------------------- -bool roboticslab::RpcResponder::handleDoubleParameterGetter(const yarp::os::Bottle& in, yarp::os::Bottle& out) +bool roboticslab::RpcResponder::handleParameterGetter(const yarp::os::Bottle& in, yarp::os::Bottle& out) { if (in.size() > 1) { diff --git a/libraries/YarpPlugins/ICartesianControl.h b/libraries/YarpPlugins/ICartesianControl.h index 7c1f8f224..eeef10c59 100644 --- a/libraries/YarpPlugins/ICartesianControl.h +++ b/libraries/YarpPlugins/ICartesianControl.h @@ -83,14 +83,12 @@ */ // Controller configuration (parameter keys) -#define VOCAB_CC_CONFIG_SET_STRING VOCAB4('c','p','s','s') ///< Set parameter (string) -#define VOCAB_CC_CONFIG_GET_STRING VOCAB4('c','p','g','s') ///< Get parameter (string) -#define VOCAB_CC_CONFIG_SET_DOUBLE VOCAB4('c','p','s','d') ///< Set parameter (double) -#define VOCAB_CC_CONFIG_GET_DOUBLE VOCAB4('c','p','g','d') ///< Get parameter (double) -#define VOCAB_CC_CONFIG_GAIN VOCAB4('c','p','c','g') ///< Controller gain (double) -#define VOCAB_CC_CONFIG_MAX_JOINT_VEL VOCAB4('c','p','j','v') ///< Maximum joint velocity (double) -#define VOCAB_CC_CONFIG_TRAJ_DURATION VOCAB4('c','p','t','d') ///< Trajectory duration (double) -#define VOCAB_CC_CONFIG_FRAME VOCAB4('c','p','r','f') ///< Reference frame (string) +#define VOCAB_CC_CONFIG_SET VOCAB3('c','p','s') ///< Set parameter +#define VOCAB_CC_CONFIG_GET VOCAB3('c','p','g') ///< Get parameter +#define VOCAB_CC_CONFIG_GAIN VOCAB4('c','p','c','g') ///< Controller gain +#define VOCAB_CC_CONFIG_MAX_JOINT_VEL VOCAB4('c','p','j','v') ///< Maximum joint velocity +#define VOCAB_CC_CONFIG_TRAJ_DURATION VOCAB4('c','p','t','d') ///< Trajectory duration +#define VOCAB_CC_CONFIG_FRAME VOCAB4('c','p','r','f') ///< Reference frame /** @} */ @@ -379,31 +377,7 @@ class ICartesianControl /** * @brief Set a configuration parameter. * - * Ask the controller to store or update a parameter of type std::string. - * - * @param vocab YARP-encoded vocab (parameter key). - * @param value Parameter value encoded as a string. - * - * @return true on success, false otherwise - */ - virtual bool setParameter(int vocab, const std::string & value) = 0; - - /** - * @brief Retrieve a configuration parameter. - * - * Ask the controller to retrieve a parameter of type std::string. - * - * @param vocab YARP-encoded vocab (parameter key). - * @param value Parameter value encoded as a string. - * - * @return true on success, false otherwise - */ - virtual bool getParameter(int vocab, std::string & value) = 0; - - /** - * @brief Set a configuration parameter. - * - * Ask the controller to store or update a parameter of type double. + * Ask the controller to store or update a parameter of 'double' type. * * @param vocab YARP-encoded vocab (parameter key). * @param value Parameter value encoded as a double. @@ -415,7 +389,7 @@ class ICartesianControl /** * @brief Retrieve a configuration parameter. * - * Ask the controller to retrieve a parameter of type double. + * Ask the controller to retrieve a parameter of 'double' type. * * @param vocab YARP-encoded vocab (parameter key). * @param value Parameter value encoded as a double. From d819931c1ae355f04cdedf37643052daac386593 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Fri, 19 Jan 2018 22:51:23 +0100 Subject: [PATCH 07/11] Introduce new vocabs for reference frames, prepare controllers --- .../AmorCartesianControl.hpp | 6 +++++- .../AmorCartesianControl/DeviceDriverImpl.cpp | 17 +++++++++++++++++ .../ICartesianControlImpl.cpp | 18 ++++++++++++++++++ .../BasicCartesianControl.hpp | 3 +++ .../BasicCartesianControl/DeviceDriverImpl.cpp | 17 +++++++++++++++++ .../ICartesianControlImpl.cpp | 18 ++++++++++++++++++ libraries/YarpPlugins/ICartesianControl.h | 11 ++++++++++- 7 files changed, 88 insertions(+), 2 deletions(-) diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp index ef74e4830..7c440594c 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp +++ b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp @@ -17,6 +17,7 @@ #define DEFAULT_GAIN 0.05 #define DEFAULT_QDOT_LIMIT 10.0 +#define DEFAULT_REFERENCE_FRAME "base" namespace roboticslab { @@ -43,7 +44,8 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo iCartesianSolver(NULL), currentState(VOCAB_CC_NOT_CONTROLLING), gain(DEFAULT_GAIN), - maxJointVelocity(DEFAULT_QDOT_LIMIT) + maxJointVelocity(DEFAULT_QDOT_LIMIT), + referenceFrame(BASE_FRAME) {} // -- ICartesianControl declarations. Implementation in ICartesianControlImpl.cpp -- @@ -125,6 +127,8 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo double gain; double maxJointVelocity; + + reference_frame referenceFrame; }; } // namespace roboticslab diff --git a/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp index 181439000..d848f7cbb 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp @@ -21,6 +21,23 @@ bool roboticslab::AmorCartesianControl::open(yarp::os::Searchable& config) maxJointVelocity = config.check("maxJointVelocity", yarp::os::Value(DEFAULT_QDOT_LIMIT), "maximum joint velocity").asDouble(); + std::string referenceFrameStr = config.check("referenceFrame", yarp::os::Value(DEFAULT_REFERENCE_FRAME), + "reference frame").asString(); + + if (referenceFrameStr == "base") + { + referenceFrame = BASE_FRAME; + } + else if (referenceFrameStr == "tcp") + { + referenceFrame = TCP_FRAME; + } + else + { + CD_ERROR("Unsupported reference frame: %s.\n", referenceFrameStr); + return false; + } + std::string kinematicsFile = config.check("kinematics", yarp::os::Value(""), "AMOR kinematics description").asString(); diff --git a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp index bda5def4c..263b4d7ae 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp @@ -599,6 +599,21 @@ bool roboticslab::AmorCartesianControl::setParameter(int vocab, double value) } maxJointVelocity = value; + break; + case VOCAB_CC_CONFIG_FRAME: + switch ((int)value) + { + case VOCAB_CC_CONFIG_FRAME_BASE: + referenceFrame = BASE_FRAME; + break; + case VOCAB_CC_CONFIG_FRAME_TCP: + referenceFrame = TCP_FRAME; + break; + default: + CD_ERROR("Unrecognized of unsupported reference frame vocab: %s.\n", yarp::os::Vocab::decode(value).c_str()); + break; + } + break; default: CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); @@ -620,6 +635,9 @@ bool roboticslab::AmorCartesianControl::getParameter(int vocab, double * value) case VOCAB_CC_CONFIG_MAX_JOINT_VEL: *value = maxJointVelocity; break; + case VOCAB_CC_CONFIG_FRAME: + *value = referenceFrame; + break; default: CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); return false; diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp index 86ef07bd7..11767e633 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp +++ b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp @@ -22,6 +22,7 @@ #define DEFAULT_MS 50 #define DEFAULT_GAIN 0.05 #define DEFAULT_QDOT_LIMIT 10.0 +#define DEFAULT_REFERENCE_FRAME "base" namespace roboticslab { @@ -199,6 +200,8 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, public ICartesianC yarp::dev::ITorqueControl *iTorqueControl; yarp::dev::IControlMode *iControlMode; + reference_frame referenceFrame; + double gain; double maxJointVelocity; double duration; // [s] diff --git a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp index b38159103..ea0a72fdb 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp @@ -14,6 +14,23 @@ bool roboticslab::BasicCartesianControl::open(yarp::os::Searchable& config) { maxJointVelocity = config.check("maxJointVelocity",yarp::os::Value(DEFAULT_QDOT_LIMIT),"maximum joint velocity").asDouble(); duration = config.check("trajectoryDuration",yarp::os::Value(DEFAULT_DURATION),"trajectory duration").asDouble(); + std::string referenceFrameStr = config.check("referenceFrame", yarp::os::Value(DEFAULT_REFERENCE_FRAME), + "reference frame").asString(); + + if (referenceFrameStr == "base") + { + referenceFrame = BASE_FRAME; + } + else if (referenceFrameStr == "tcp") + { + referenceFrame = TCP_FRAME; + } + else + { + CD_ERROR("Unsupported reference frame: %s.\n", referenceFrameStr); + return false; + } + std::string solverStr = config.check("solver",yarp::os::Value(DEFAULT_SOLVER),"cartesian solver").asString(); std::string robotStr = config.check("robot",yarp::os::Value(DEFAULT_ROBOT),"robot device").asString(); diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp index 63e5c6903..a538af477 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp @@ -619,6 +619,21 @@ bool roboticslab::BasicCartesianControl::setParameter(int vocab, double value) } duration = value; + break; + case VOCAB_CC_CONFIG_FRAME: + switch ((int)value) + { + case VOCAB_CC_CONFIG_FRAME_BASE: + referenceFrame = BASE_FRAME; + break; + case VOCAB_CC_CONFIG_FRAME_TCP: + referenceFrame = TCP_FRAME; + break; + default: + CD_ERROR("Unrecognized of unsupported reference frame vocab: %s.\n", yarp::os::Vocab::decode(value).c_str()); + break; + } + break; default: CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); @@ -643,6 +658,9 @@ bool roboticslab::BasicCartesianControl::getParameter(int vocab, double * value) case VOCAB_CC_CONFIG_TRAJ_DURATION: *value = duration; break; + case VOCAB_CC_CONFIG_FRAME: + *value = referenceFrame; + break; default: CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str()); return false; diff --git a/libraries/YarpPlugins/ICartesianControl.h b/libraries/YarpPlugins/ICartesianControl.h index eeef10c59..44fc60ebf 100644 --- a/libraries/YarpPlugins/ICartesianControl.h +++ b/libraries/YarpPlugins/ICartesianControl.h @@ -88,7 +88,9 @@ #define VOCAB_CC_CONFIG_GAIN VOCAB4('c','p','c','g') ///< Controller gain #define VOCAB_CC_CONFIG_MAX_JOINT_VEL VOCAB4('c','p','j','v') ///< Maximum joint velocity #define VOCAB_CC_CONFIG_TRAJ_DURATION VOCAB4('c','p','t','d') ///< Trajectory duration -#define VOCAB_CC_CONFIG_FRAME VOCAB4('c','p','r','f') ///< Reference frame +#define VOCAB_CC_CONFIG_FRAME VOCAB3('c','p','f') ///< Reference frame +#define VOCAB_CC_CONFIG_FRAME_BASE VOCAB4('c','p','f','b') ///< Base frame +#define VOCAB_CC_CONFIG_FRAME_TCP VOCAB4('c','p','f','t') ///< End-effector frame (TCP) /** @} */ @@ -102,6 +104,13 @@ class ICartesianControl { public: + //! Lists supported reference frames. + enum reference_frame + { + BASE_FRAME, //!< Base frame + TCP_FRAME //!< End-effector frame (TCP) + }; + //! Destructor virtual ~ICartesianControl() {} From ec330b323fdc596a210282ec5b99e380b78cc9c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Fri, 19 Jan 2018 23:18:36 +0100 Subject: [PATCH 08/11] Drop several streaming commands, keep only 'twist' and 'pose' --- .../AmorCartesianControl.hpp | 12 +- .../ICartesianControlImpl.cpp | 279 ++---------------- .../BasicCartesianControl.hpp | 12 +- .../ICartesianControlImpl.cpp | 242 ++------------- .../CartesianControlClient.hpp | 12 +- .../ICartesianControlImpl.cpp | 39 +-- .../StreamResponder.cpp | 19 +- libraries/YarpPlugins/ICartesianControl.h | 97 +----- 8 files changed, 62 insertions(+), 650 deletions(-) diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp index 7c440594c..5e3c833d7 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp +++ b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp @@ -70,17 +70,7 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo virtual bool tool(const std::vector &x); - virtual void fwd(const std::vector &rot, double step); - - virtual void bkwd(const std::vector &rot, double step); - - virtual void rot(const std::vector &rot); - - virtual void pan(const std::vector &transl); - - virtual void vmos(const std::vector &xdot); - - virtual void eff(const std::vector &xdotee); + virtual void twist(const std::vector &xdot); virtual void pose(const std::vector &x, double interval); diff --git a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp index 263b4d7ae..acfa56478 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp @@ -223,7 +223,7 @@ bool roboticslab::AmorCartesianControl::tool(const std::vector &x) // ----------------------------------------------------------------------------- -void roboticslab::AmorCartesianControl::fwd(const std::vector &rot, double step) +void roboticslab::AmorCartesianControl::twist(const std::vector &xdot) { AMOR_VECTOR7 positions; @@ -240,257 +240,21 @@ void roboticslab::AmorCartesianControl::fwd(const std::vector &rot, doub currentQ[i] = KinRepresentation::radToDeg(positions[i]); } - std::vector xdotee(6); - xdotee[2] = std::max(step, 0.0); - xdotee[3] = rot[0]; - xdotee[4] = rot[1]; - xdotee[5] = rot[2]; - - if (!iCartesianSolver->diffInvKinEE(currentQ, xdotee, qdot)) - { - CD_ERROR("diffInvKinEE failed.\n"); - return; - } - - if (!checkJointVelocities(qdot)) - { - amor_controlled_stop(handle); - return; - } - - AMOR_VECTOR7 velocities; - - for (int i = 0; i < qdot.size(); i++) - { - velocities[i] = KinRepresentation::degToRad(qdot[i]); - } - - if (amor_set_velocities(handle, velocities) != AMOR_SUCCESS) - { - CD_ERROR("%s\n", amor_error()); - return; - } -} - -// ----------------------------------------------------------------------------- - -void roboticslab::AmorCartesianControl::bkwd(const std::vector &rot, double step) -{ - AMOR_VECTOR7 positions; - - if (amor_get_actual_positions(handle, &positions) != AMOR_SUCCESS) - { - CD_ERROR("%s\n", amor_error()); - return; - } - - std::vector currentQ(AMOR_NUM_JOINTS), qdot; - - for (int i = 0; i < AMOR_NUM_JOINTS; i++) - { - currentQ[i] = KinRepresentation::radToDeg(positions[i]); - } - - std::vector xdotee(6); - xdotee[2] = -std::max(step, 0.0); - xdotee[3] = rot[0]; - xdotee[4] = rot[1]; - xdotee[5] = rot[2]; - - if (!iCartesianSolver->diffInvKinEE(currentQ, xdotee, qdot)) - { - CD_ERROR("diffInvKinEE failed.\n"); - return; - } - - if (!checkJointVelocities(qdot)) - { - amor_controlled_stop(handle); - return; - } - - AMOR_VECTOR7 velocities; - - for (int i = 0; i < qdot.size(); i++) - { - velocities[i] = KinRepresentation::degToRad(qdot[i]); - } - - if (amor_set_velocities(handle, velocities) != AMOR_SUCCESS) - { - CD_ERROR("%s\n", amor_error()); - return; - } -} - -// ----------------------------------------------------------------------------- - -void roboticslab::AmorCartesianControl::rot(const std::vector &rot) -{ - AMOR_VECTOR7 positions; - - if (amor_get_actual_positions(handle, &positions) != AMOR_SUCCESS) - { - CD_ERROR("%s\n", amor_error()); - return; - } - - std::vector currentQ(AMOR_NUM_JOINTS), qdot; - - for (int i = 0; i < AMOR_NUM_JOINTS; i++) - { - currentQ[i] = KinRepresentation::radToDeg(positions[i]); - } - - std::vector xdotee(6); - xdotee[3] = rot[0]; - xdotee[4] = rot[1]; - xdotee[5] = rot[2]; - - if (!iCartesianSolver->diffInvKinEE(currentQ, xdotee, qdot)) - { - CD_ERROR("diffInvKinEE failed.\n"); - return; - } - - if (!checkJointVelocities(qdot)) - { - amor_controlled_stop(handle); - return; - } - - AMOR_VECTOR7 velocities; - - for (int i = 0; i < qdot.size(); i++) - { - velocities[i] = KinRepresentation::degToRad(qdot[i]); - } - - if (amor_set_velocities(handle, velocities) != AMOR_SUCCESS) - { - CD_ERROR("%s\n", amor_error()); - return; - } -} - -// ----------------------------------------------------------------------------- - -void roboticslab::AmorCartesianControl::pan(const std::vector &transl) -{ - AMOR_VECTOR7 positions; - - if (amor_get_actual_positions(handle, &positions) != AMOR_SUCCESS) - { - CD_ERROR("%s\n", amor_error()); - return; - } - - std::vector currentQ(AMOR_NUM_JOINTS), qdot; - - for (int i = 0; i < AMOR_NUM_JOINTS; i++) - { - currentQ[i] = KinRepresentation::radToDeg(positions[i]); - } - - std::vector xdotee(6); - xdotee[0] = transl[0]; - xdotee[1] = transl[1]; - xdotee[2] = transl[2]; - - if (!iCartesianSolver->diffInvKinEE(currentQ, xdotee, qdot)) - { - CD_ERROR("diffInvKinEE failed.\n"); - return; - } - - if (!checkJointVelocities(qdot)) - { - amor_controlled_stop(handle); - return; - } - - AMOR_VECTOR7 velocities; - - for (int i = 0; i < qdot.size(); i++) - { - velocities[i] = KinRepresentation::degToRad(qdot[i]); - } - - if (amor_set_velocities(handle, velocities) != AMOR_SUCCESS) - { - CD_ERROR("%s\n", amor_error()); - return; - } -} - -// ----------------------------------------------------------------------------- - -void roboticslab::AmorCartesianControl::vmos(const std::vector &xdot) -{ - AMOR_VECTOR7 positions; - - if (amor_get_actual_positions(handle, &positions) != AMOR_SUCCESS) - { - CD_ERROR("%s\n", amor_error()); - return; - } - - std::vector currentQ(AMOR_NUM_JOINTS), qdot; - - for (int i = 0; i < AMOR_NUM_JOINTS; i++) + if (referenceFrame == BASE_FRAME) { - currentQ[i] = KinRepresentation::radToDeg(positions[i]); - } - - if (!iCartesianSolver->diffInvKin(currentQ, xdot, qdot)) - { - CD_ERROR("diffInvKin failed.\n"); - return; - } - - if (!checkJointVelocities(qdot)) - { - amor_controlled_stop(handle); - return; - } - - AMOR_VECTOR7 velocities; - - for (int i = 0; i < qdot.size(); i++) - { - velocities[i] = KinRepresentation::degToRad(qdot[i]); - } - - if (amor_set_velocities(handle, velocities) != AMOR_SUCCESS) - { - CD_ERROR("%s\n", amor_error()); - return; - } -} - -// ----------------------------------------------------------------------------- - -void roboticslab::AmorCartesianControl::eff(const std::vector &xdotee) -{ - AMOR_VECTOR7 positions; - - if (amor_get_actual_positions(handle, &positions) != AMOR_SUCCESS) - { - CD_ERROR("%s\n", amor_error()); - return; - } - - std::vector currentQ(AMOR_NUM_JOINTS), qdot; - - for (int i = 0; i < AMOR_NUM_JOINTS; i++) - { - currentQ[i] = KinRepresentation::radToDeg(positions[i]); + if (!iCartesianSolver->diffInvKin(currentQ, xdot, qdot)) + { + CD_ERROR("diffInvKin failed.\n"); + return; + } } - - if (!iCartesianSolver->diffInvKinEE(currentQ, xdotee, qdot)) + else if (referenceFrame == TCP_FRAME) { - CD_ERROR("diffInvKinEE failed.\n"); - return; + if (!iCartesianSolver->diffInvKinEE(currentQ, xdot, qdot)) + { + CD_ERROR("diffInvKinEE failed.\n"); + return; + } } if (!checkJointVelocities(qdot)) @@ -550,10 +314,21 @@ void roboticslab::AmorCartesianControl::pose(const std::vector &x, doubl std::vector qdot; - if (!iCartesianSolver->diffInvKin(currentQ, xdot, qdot)) + if (referenceFrame == BASE_FRAME) { - CD_ERROR("diffInvKin failed.\n"); - return; + if (!iCartesianSolver->diffInvKin(currentQ, xdot, qdot)) + { + CD_ERROR("diffInvKin failed.\n"); + return; + } + } + else if (referenceFrame == TCP_FRAME) + { + if (!iCartesianSolver->diffInvKinEE(currentQ, xdot, qdot)) + { + CD_ERROR("diffInvKinEE failed.\n"); + return; + } } if (!checkJointVelocities(qdot)) diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp index 4b3293416..85ffd9ed7 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp +++ b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp @@ -137,17 +137,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, public ICartesianC virtual bool tool(const std::vector &x); - virtual void fwd(const std::vector &rot, double step); - - virtual void bkwd(const std::vector &rot, double step); - - virtual void rot(const std::vector &rot); - - virtual void pan(const std::vector &transl); - - virtual void vmos(const std::vector &xdot); - - virtual void eff(const std::vector &xdotee); + virtual void twist(const std::vector &xdot); virtual void pose(const std::vector &x, double interval); diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp index ec9a4e543..f5ba7ef40 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp @@ -311,7 +311,7 @@ bool roboticslab::BasicCartesianControl::tool(const std::vector &x) // ----------------------------------------------------------------------------- -void roboticslab::BasicCartesianControl::fwd(const std::vector &rot, double step) +void roboticslab::BasicCartesianControl::twist(const std::vector &xdot) { for (unsigned int joint = 0; joint < numRobotJoints; joint++) { @@ -325,234 +325,23 @@ void roboticslab::BasicCartesianControl::fwd(const std::vector &rot, dou return; } - std::vector xdotee(6); - xdotee[2] = std::max(step, 0.0); - xdotee[3] = rot[0]; - xdotee[4] = rot[1]; - xdotee[5] = rot[2]; - - if ( ! iCartesianSolver->diffInvKinEE( currentQ, xdotee, qdot ) ) - { - CD_ERROR("diffInvKinEE failed.\n"); - return; - } - - for (unsigned int i = 0; i < qdot.size(); i++) + if ( referenceFrame == BASE_FRAME ) { - if ( std::abs(qdot[i]) > maxJointVelocity ) + if ( ! iCartesianSolver->diffInvKin(currentQ, xdot, qdot) ) { - CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], maxJointVelocity); - std::fill(qdot.begin(), qdot.end(), 0.0); - iVelocityControl->velocityMove(qdot.data()); + CD_ERROR("diffInvKin failed.\n"); return; } } - - if ( ! iVelocityControl->velocityMove( qdot.data() ) ) - { - CD_ERROR("velocityMove failed.\n"); - return; - } -} - -// ----------------------------------------------------------------------------- - -void roboticslab::BasicCartesianControl::bkwd(const std::vector &rot, double step) -{ - for (unsigned int joint = 0; joint < numRobotJoints; joint++) - { - iControlMode->setVelocityMode(joint); - } - - std::vector currentQ(numRobotJoints), qdot; - if ( ! iEncoders->getEncoders( currentQ.data() ) ) - { - CD_ERROR("getEncoders failed.\n"); - return; - } - - std::vector xdotee(6); - xdotee[2] = -std::max(step, 0.0); - xdotee[3] = rot[0]; - xdotee[4] = rot[1]; - xdotee[5] = rot[2]; - - if ( ! iCartesianSolver->diffInvKinEE( currentQ, xdotee, qdot ) ) - { - CD_ERROR("diffInvKinEE failed.\n"); - return; - } - - for (unsigned int i = 0; i < qdot.size(); i++) + else if ( referenceFrame == TCP_FRAME ) { - if ( std::abs(qdot[i]) > maxJointVelocity ) + if ( ! iCartesianSolver->diffInvKinEE(currentQ, xdot, qdot) ) { - CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], maxJointVelocity); - std::fill(qdot.begin(), qdot.end(), 0.0); - iVelocityControl->velocityMove(qdot.data()); + CD_ERROR("diffInvKinEE failed.\n"); return; } } - if ( ! iVelocityControl->velocityMove( qdot.data() ) ) - { - CD_ERROR("velocityMove failed.\n"); - return; - } -} - -// ----------------------------------------------------------------------------- - -void roboticslab::BasicCartesianControl::rot(const std::vector &rot) -{ - for (unsigned int joint = 0; joint < numRobotJoints; joint++) - { - iControlMode->setVelocityMode(joint); - } - - std::vector currentQ(numRobotJoints), qdot; - if ( ! iEncoders->getEncoders( currentQ.data() ) ) - { - CD_ERROR("getEncoders failed.\n"); - return; - } - - std::vector xdotee(6); - xdotee[3] = rot[0]; - xdotee[4] = rot[1]; - xdotee[5] = rot[2]; - - if ( ! iCartesianSolver->diffInvKinEE( currentQ, xdotee, qdot ) ) - { - CD_ERROR("diffInvKinEE failed.\n"); - return; - } - - for (unsigned int i = 0; i < qdot.size(); i++) - { - if ( std::abs(qdot[i]) > maxJointVelocity ) - { - CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], maxJointVelocity); - std::fill(qdot.begin(), qdot.end(), 0.0); - iVelocityControl->velocityMove(qdot.data()); - return; - } - } - - if ( ! iVelocityControl->velocityMove( qdot.data() ) ) - { - CD_ERROR("velocityMove failed.\n"); - return; - } -} - -// ----------------------------------------------------------------------------- - -void roboticslab::BasicCartesianControl::pan(const std::vector &transl) -{ - for (unsigned int joint = 0; joint < numRobotJoints; joint++) - { - iControlMode->setVelocityMode(joint); - } - - std::vector currentQ(numRobotJoints), qdot; - if ( ! iEncoders->getEncoders( currentQ.data() ) ) - { - CD_ERROR("getEncoders failed.\n"); - return; - } - - std::vector xdotee(6); - xdotee[0] = transl[0]; - xdotee[1] = transl[1]; - xdotee[2] = transl[2]; - - if ( ! iCartesianSolver->diffInvKinEE( currentQ, xdotee, qdot ) ) - { - CD_ERROR("diffInvKinEE failed.\n"); - return; - } - - for (unsigned int i = 0; i < qdot.size(); i++) - { - if ( std::abs(qdot[i]) > maxJointVelocity ) - { - CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], maxJointVelocity); - std::fill(qdot.begin(), qdot.end(), 0.0); - iVelocityControl->velocityMove(qdot.data()); - return; - } - } - - if ( ! iVelocityControl->velocityMove( qdot.data() ) ) - { - CD_ERROR("velocityMove failed.\n"); - return; - } -} - -// ----------------------------------------------------------------------------- - -void roboticslab::BasicCartesianControl::vmos(const std::vector &xdot) -{ - for (unsigned int joint = 0; joint < numRobotJoints; joint++) - { - iControlMode->setVelocityMode(joint); - } - - std::vector currentQ(numRobotJoints), qdot; - if ( ! iEncoders->getEncoders( currentQ.data() ) ) - { - CD_ERROR("getEncoders failed.\n"); - return; - } - - if ( ! iCartesianSolver->diffInvKin( currentQ, xdot, qdot ) ) - { - CD_ERROR("diffInvKin failed.\n"); - return; - } - - for (unsigned int i = 0; i < qdot.size(); i++) - { - if ( std::abs(qdot[i]) > maxJointVelocity ) - { - CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], maxJointVelocity); - std::fill(qdot.begin(), qdot.end(), 0.0); - iVelocityControl->velocityMove(qdot.data()); - return; - } - } - - if ( ! iVelocityControl->velocityMove( qdot.data() ) ) - { - CD_ERROR("velocityMove failed.\n"); - return; - } -} - -// ----------------------------------------------------------------------------- - -void roboticslab::BasicCartesianControl::eff(const std::vector &xdotee) -{ - for (unsigned int joint = 0; joint < numRobotJoints; joint++) - { - iControlMode->setVelocityMode(joint); - } - - std::vector currentQ(numRobotJoints), qdot; - if ( ! iEncoders->getEncoders( currentQ.data() ) ) - { - CD_ERROR("getEncoders failed.\n"); - return; - } - - if ( ! iCartesianSolver->diffInvKinEE( currentQ, xdotee, qdot ) ) - { - CD_ERROR("diffInvKinEE failed.\n"); - return; - } - for (unsigned int i = 0; i < qdot.size(); i++) { if ( std::abs(qdot[i]) > maxJointVelocity ) @@ -599,10 +388,21 @@ void roboticslab::BasicCartesianControl::pose(const std::vector &x, doub } std::vector qdot; - if ( ! iCartesianSolver->diffInvKin(currentQ, xdot, qdot) ) + if ( referenceFrame == BASE_FRAME ) { - CD_ERROR("diffInvKin failed.\n"); - return; + if ( ! iCartesianSolver->diffInvKin(currentQ, xdot, qdot) ) + { + CD_ERROR("diffInvKin failed.\n"); + return; + } + } + else if ( referenceFrame == TCP_FRAME ) + { + if ( ! iCartesianSolver->diffInvKinEE(currentQ, xdot, qdot) ) + { + CD_ERROR("diffInvKinEE failed.\n"); + return; + } } for (unsigned int i = 0; i < qdot.size(); i++) diff --git a/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp b/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp index ed3a03451..cb84e71e9 100644 --- a/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp +++ b/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp @@ -87,17 +87,7 @@ class CartesianControlClient : public yarp::dev::DeviceDriver, public ICartesian virtual bool tool(const std::vector &x); - virtual void fwd(const std::vector &rot, double step); - - virtual void bkwd(const std::vector &rot, double step); - - virtual void rot(const std::vector &rot); - - virtual void pan(const std::vector &transl); - - virtual void vmos(const std::vector &xdot); - - virtual void eff(const std::vector &xdotee); + virtual void twist(const std::vector &xdot); virtual void pose(const std::vector &x, double interval); diff --git a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp index 8dccddd40..54b59e125 100644 --- a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp @@ -213,44 +213,9 @@ bool roboticslab::CartesianControlClient::tool(const std::vector &x) // ----------------------------------------------------------------------------- -void roboticslab::CartesianControlClient::fwd(const std::vector &rot, double step) +void roboticslab::CartesianControlClient::twist(const std::vector &xdot) { - handleStreamingBiConsumerCmd(VOCAB_CC_FWD, rot, step); -} - -// ----------------------------------------------------------------------------- - -void roboticslab::CartesianControlClient::bkwd(const std::vector &rot, double step) -{ - handleStreamingBiConsumerCmd(VOCAB_CC_BKWD, rot, step); -} - -// ----------------------------------------------------------------------------- - -void roboticslab::CartesianControlClient::rot(const std::vector &rot) -{ - handleStreamingConsumerCmd(VOCAB_CC_ROT, rot); -} - -// ----------------------------------------------------------------------------- - -void roboticslab::CartesianControlClient::pan(const std::vector &transl) -{ - handleStreamingConsumerCmd(VOCAB_CC_PAN, transl); -} - -// ----------------------------------------------------------------------------- - -void roboticslab::CartesianControlClient::vmos(const std::vector &xdot) -{ - handleStreamingConsumerCmd(VOCAB_CC_VMOS, xdot); -} - -// ----------------------------------------------------------------------------- - -void roboticslab::CartesianControlClient::eff(const std::vector &xdotee) -{ - handleStreamingConsumerCmd(VOCAB_CC_EFF, xdotee); + handleStreamingConsumerCmd(VOCAB_CC_TWIST, xdot); } // ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp b/libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp index 2548a6240..ecd14b1b4 100644 --- a/libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp +++ b/libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp @@ -12,23 +12,8 @@ void roboticslab::StreamResponder::onRead(yarp::os::Bottle& b) switch (b.get(0).asVocab()) { - case VOCAB_CC_FWD: - handleBiConsumerCmdMsg(b, &ICartesianControl::fwd); - break; - case VOCAB_CC_BKWD: - handleBiConsumerCmdMsg(b, &ICartesianControl::bkwd); - break; - case VOCAB_CC_ROT: - handleConsumerCmdMsg(b, &ICartesianControl::rot); - break; - case VOCAB_CC_PAN: - handleConsumerCmdMsg(b, &ICartesianControl::pan); - break; - case VOCAB_CC_VMOS: - handleConsumerCmdMsg(b, &ICartesianControl::vmos); - break; - case VOCAB_CC_EFF: - handleConsumerCmdMsg(b, &ICartesianControl::eff); + case VOCAB_CC_TWIST: + handleConsumerCmdMsg(b, &ICartesianControl::twist); break; case VOCAB_CC_POSE: handleBiConsumerCmdMsg(b, &ICartesianControl::pose); diff --git a/libraries/YarpPlugins/ICartesianControl.h b/libraries/YarpPlugins/ICartesianControl.h index 44fc60ebf..b20bf7e7f 100644 --- a/libraries/YarpPlugins/ICartesianControl.h +++ b/libraries/YarpPlugins/ICartesianControl.h @@ -46,13 +46,8 @@ */ // Streaming commands -#define VOCAB_CC_FWD VOCAB3('f','w','d') ///< Move forward (relative to end-effector) -#define VOCAB_CC_BKWD VOCAB4('b','k','w','d') ///< Move backwards (relative to end-effector) -#define VOCAB_CC_ROT VOCAB3('r','o','t') ///< Rotate in end-effector frame -#define VOCAB_CC_PAN VOCAB3('p','a','n') ///< Pan in end-effector frame -#define VOCAB_CC_VMOS VOCAB4('v','m','o','s') ///< Instantaneous velocity steps (inertial frame) -#define VOCAB_CC_EFF VOCAB3('e','f','f') ///< Instantaneous velocity steps (end-effector frame) -#define VOCAB_CC_POSE VOCAB4('p','o','s','e') ///< Achieve pose in inertial frame +#define VOCAB_CC_TWIST VOCAB4('t','w','s','t') ///< Instantaneous velocity steps +#define VOCAB_CC_POSE VOCAB4('p','o','s','e') ///< Achieve pose in inertial frame /** @} */ @@ -269,99 +264,21 @@ class ICartesianControl */ /** - * @brief Move forward (relative to end-effector) + * @brief Instantaneous velocity steps * - * Move along the Z (positive) axis in velocity increments, applying desired angular - * velocities. All coordinates are expressed in terms of the end-effector frame. - * Negative step values will be ignored. - * - * @param rot 3-element vector describing desired angular velocity increments in - * cartesian space, expressed in radians/second. - * @param step Velocity step corresponding to Z axis, expressed in meters/second. - * - * @see bkwd (move backwards) - * @see eff (rotations + translations) - */ - virtual void fwd(const std::vector &rot, double step) = 0; - - /** - * @brief Move backwards (relative to end-effector) - * - * Move along the Z (negative) axis in velocity increments, applying desired angular - * velocities. All coordinates are expressed in terms of the end-effector frame. - * Negative step values will be ignored. - * - * @param 3-element vector describing desired angular velocity increments in - * cartesian space, expressed in radians/second. - * @param step Velocity step corresponding to Z axis, expressed in meters/second. - * - * @see fwd (move forward) - * @see eff (rotations + translations) - */ - virtual void bkwd(const std::vector &rot, double step) = 0; - - /** - * @brief Rotate in end-effector frame - * - * Apply desired angular velocities, but avoid translating, that is, only change the - * orientation of the TCP. All coordinates are expressed in terms of the end-effector frame. - * - * @param 3-element vector describing desired angular velocity increments in - * cartesian space, expressed in radians/second. - * - * @see pan (only translations) - * @see eff (rotations + translations) - */ - virtual void rot(const std::vector &rot) = 0; - - /** - * @brief Pan in end-effector frame - * - * Apply desired linear velocities, but avoid rotating, that is, only change the position - * of the TCP. All coordinates are expressed in terms of the end-effector frame. - * - * @param transl 3-element vector describing desired translational velocity - * increments in cartesian space, expressed in meters/second. - * - * @see pan (only rotations) - * @see eff (rotations + translations) - */ - virtual void pan(const std::vector &transl) = 0; - - /** - * @brief Instantaneous velocity steps (inertial frame) - * - * Move in instantaneous velocity increments using the fixed (inertial) frame as the - * reference coordinate system. + * Move in instantaneous velocity increments. * * @param xdot 6-element vector describing velocity increments in cartesian space; * first three elements denote translational velocity (meters/second), last three * denote angular velocity (radians/second). - * - * @see eff (end-effector frame) - */ - virtual void vmos(const std::vector &xdot) = 0; - - /** - * @brief Instantaneous velocity steps (end-effector frame) - * - * Move in instantaneous velocity increments using the end-effector frame as the - * reference coordinate system. - * - * @param xdotee 6-element vector describing velocity increments in cartesian space; - * first three elements denote translational velocity (meters/second), last three - * denote angular velocity (radians/second). - * - * @see vmos (inertial frame) - * @see fwd, bkwd, rot, pan (only translations/rotations) */ - virtual void eff(const std::vector &xdotee) = 0; + virtual void twist(const std::vector &xdot) = 0; /** - * @brief Achieve pose in inertial frame + * @brief Achieve pose * * Move to desired position, computing the error with respect to the current pose. Then, - * perform numerical differentiation and obtain the final velocity increment (as in @ref vmos). + * perform numerical differentiation and obtain the final velocity increment (as in @ref twist). * * @param x 6-element vector describing desired instantaneous pose in cartesian space; * first three elements denote translation (meters), last three denote rotation (radians). From 81b2698e1108e86d9e4ff2eeb8c29dfeb21aa1b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Fri, 19 Jan 2018 23:20:40 +0100 Subject: [PATCH 09/11] Convert std::string to char* in CD calls Follows up d819931. --- libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp | 2 +- .../YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp index d848f7cbb..5b0abb783 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp @@ -34,7 +34,7 @@ bool roboticslab::AmorCartesianControl::open(yarp::os::Searchable& config) } else { - CD_ERROR("Unsupported reference frame: %s.\n", referenceFrameStr); + CD_ERROR("Unsupported reference frame: %s.\n", referenceFrameStr.c_str()); return false; } diff --git a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp index ea0a72fdb..889722ac9 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp @@ -27,7 +27,7 @@ bool roboticslab::BasicCartesianControl::open(yarp::os::Searchable& config) { } else { - CD_ERROR("Unsupported reference frame: %s.\n", referenceFrameStr); + CD_ERROR("Unsupported reference frame: %s.\n", referenceFrameStr.c_str()); return false; } From ec93b621fafa8a210f890c0e143683187c345b90 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Fri, 19 Jan 2018 23:55:26 +0100 Subject: [PATCH 10/11] Unbreak apps, use new and refurbished ICartesianControl methods --- .../keyboardController/KeyboardController.cpp | 63 ++++++++----------- .../keyboardController/KeyboardController.hpp | 47 -------------- .../SpnavSensorDevice.cpp | 2 +- .../WiimoteSensorDevice.cpp | 23 ++++++- .../WiimoteSensorDevice.hpp | 2 + 5 files changed, 49 insertions(+), 88 deletions(-) diff --git a/programs/keyboardController/KeyboardController.cpp b/programs/keyboardController/KeyboardController.cpp index 7e2c1a963..c0b10a1be 100644 --- a/programs/keyboardController/KeyboardController.cpp +++ b/programs/keyboardController/KeyboardController.cpp @@ -211,13 +211,6 @@ bool roboticslab::KeyboardController::configure(yarp::os::ResourceFinder &rf) currentCartVels.resize(NUM_CART_COORDS, 0.0); cartFrame = INERTIAL; - - cartesianThread = new KeyboardRateThread(iCartesianControl); - - cartesianThread->setCurrentCommand(&ICartesianControl::vmos); - cartesianThread->setCurrentData(currentCartVels); - - cartesianThread->start(); // initialize the new thread } issueStop(); // just in case @@ -380,12 +373,6 @@ double roboticslab::KeyboardController::getPeriod() bool roboticslab::KeyboardController::close() { - if (cartesianControlDevice.isValid()) - { - cartesianThread->stop(); - delete cartesianThread; - } - controlboardDevice.close(); cartesianControlDevice.close(); @@ -467,19 +454,16 @@ void roboticslab::KeyboardController::incrementOrDecrementCartesianVelocity(cart std::cout << "New cartesian velocity: " << roundZeroes(currentCartVels) << std::endl; - cartesianThread->setCurrentData(currentCartVels); - if (roundZeroes(currentCartVels) == ZERO_CARTESIAN_VELOCITY) { - cartesianThread->step(); // send vector of zeroes - cartesianThread->suspend(); - controlMode = NOT_CONTROLLING; + iCartesianControl->twist(ZERO_CARTESIAN_VELOCITY); // send vector of zeroes } else { - cartesianThread->resume(); - controlMode = CARTESIAN_MODE; + iCartesianControl->twist(currentCartVels); } + + controlMode = CARTESIAN_MODE; } void roboticslab::KeyboardController::toggleReferenceFrame() @@ -493,26 +477,39 @@ void roboticslab::KeyboardController::toggleReferenceFrame() issueStop(); - std::cout << "Toggled reference frame for cartesian commands: "; + cart_frames newFrame; + std::string str; + int frameVocab = 0; switch (cartFrame) { case INERTIAL: - cartFrame = END_EFFECTOR; - cartesianThread->setCurrentCommand(&ICartesianControl::eff); - std::cout << "end effector"; + newFrame = END_EFFECTOR; + str = "end effector"; + frameVocab = VOCAB_CC_CONFIG_FRAME_TCP; break; case END_EFFECTOR: - cartFrame = INERTIAL; - cartesianThread->setCurrentCommand(&ICartesianControl::vmos); - std::cout << "inertial"; + newFrame = INERTIAL; + str = "inertial"; + frameVocab = VOCAB_CC_CONFIG_FRAME_BASE; break; default: - std::cout << "unknown"; + str = "unknown"; break; } - std::cout << std::endl; + if (frameVocab) + { + if (!iCartesianControl->setParameter(VOCAB_CC_CONFIG_FRAME, frameVocab)) + { + CD_ERROR("Unable to set reference frame: %s.\n", str.c_str()); + return; + } + + cartFrame = newFrame; + } + + std::cout << "Toggled reference frame for cartesian commands: " << str << std::endl; } void roboticslab::KeyboardController::printJointPositions() @@ -554,14 +551,6 @@ void roboticslab::KeyboardController::issueStop() { if (cartesianControlDevice.isValid()) { - if (!cartesianThread->isSuspended()) - { - cartesianThread->suspend(); - - // ensure that the thread stops before sending a stop command - yarp::os::Time::delay(2 * CMC_RATE_MS / 1000); - } - iCartesianControl->stopControl(); } else if (controlboardDevice.isValid()) diff --git a/programs/keyboardController/KeyboardController.hpp b/programs/keyboardController/KeyboardController.hpp index 7764036da..e93c43377 100644 --- a/programs/keyboardController/KeyboardController.hpp +++ b/programs/keyboardController/KeyboardController.hpp @@ -32,51 +32,6 @@ namespace roboticslab { -/** - * @ingroup keyboardController - * - * @brief Helper thread for sending streaming cartesian - * commands to the controller. - */ -class KeyboardRateThread : public yarp::os::RateThread -{ -public: - typedef std::vector data_type; - typedef void (ICartesianControl::*cart_command)(const data_type &); - - KeyboardRateThread(roboticslab::ICartesianControl * iCartesianControl) - : yarp::os::RateThread(CMC_RATE_MS), - iCartesianControl(iCartesianControl), - currentCommand(&ICartesianControl::vmos) - {} - - virtual void run() - { - (iCartesianControl->*currentCommand)(currentData); - } - - void setCurrentCommand(cart_command cmd) - { - currentCommand = cmd; - } - - void setCurrentData(const data_type & data) - { - currentData = data; - } - - void beforeStart() - { - // prevents execution of first run() step after start() - suspend(); - } - -private: - roboticslab::ICartesianControl * iCartesianControl; - cart_command currentCommand; - data_type currentData; -}; - /** * @ingroup keyboardController * @@ -139,8 +94,6 @@ class KeyboardController : public yarp::os::RFModule roboticslab::ICartesianControl * iCartesianControl; - KeyboardRateThread * cartesianThread; - std::vector maxVelocityLimits; std::vector currentJointVels; std::vector currentCartVels; diff --git a/programs/streamingDeviceController/SpnavSensorDevice.cpp b/programs/streamingDeviceController/SpnavSensorDevice.cpp index d64e9c530..18e0c0a6c 100644 --- a/programs/streamingDeviceController/SpnavSensorDevice.cpp +++ b/programs/streamingDeviceController/SpnavSensorDevice.cpp @@ -40,5 +40,5 @@ bool roboticslab::SpnavSensorDevice::acquireData() void roboticslab::SpnavSensorDevice::sendMovementCommand() { - iCartesianControl->vmos(data); + iCartesianControl->twist(data); } diff --git a/programs/streamingDeviceController/WiimoteSensorDevice.cpp b/programs/streamingDeviceController/WiimoteSensorDevice.cpp index 1dd1c88e5..02dc3e785 100644 --- a/programs/streamingDeviceController/WiimoteSensorDevice.cpp +++ b/programs/streamingDeviceController/WiimoteSensorDevice.cpp @@ -1,6 +1,7 @@ #include "WiimoteSensorDevice.hpp" #include +#include #include @@ -19,6 +20,17 @@ bool roboticslab::WiimoteSensorDevice::acquireInterfaces() return ok; } +bool roboticslab::WiimoteSensorDevice::initialize() +{ + if (!iCartesianControl->setParameter(VOCAB_CC_CONFIG_FRAME, VOCAB_CC_CONFIG_FRAME_TCP)) + { + CD_WARNING("Unable to set TPC reference frame.\n"); + return false; + } + + return true; +} + bool roboticslab::WiimoteSensorDevice::acquireData() { yarp::sig::Vector data; @@ -87,18 +99,23 @@ bool roboticslab::WiimoteSensorDevice::hasValidMovementData() const void roboticslab::WiimoteSensorDevice::sendMovementCommand() { + std::vector xdot(6, 0.0); + std::copy(data.begin(), data.end(), xdot.begin() + 3); + switch (mode) { case FWD: - iCartesianControl->fwd(data, step); + xdot[2] = step; break; case BKWD: - iCartesianControl->bkwd(data, step); + xdot[2] = -step; break; case ROT: - iCartesianControl->rot(data); + xdot[2] = 0.0; // for the sake of explicitness break; default: return; } + + iCartesianControl->twist(xdot); } diff --git a/programs/streamingDeviceController/WiimoteSensorDevice.hpp b/programs/streamingDeviceController/WiimoteSensorDevice.hpp index 83bf53dbc..32493addb 100644 --- a/programs/streamingDeviceController/WiimoteSensorDevice.hpp +++ b/programs/streamingDeviceController/WiimoteSensorDevice.hpp @@ -37,6 +37,8 @@ class WiimoteSensorDevice : public StreamingDevice virtual bool acquireInterfaces(); + virtual bool initialize(); + virtual bool acquireData(); virtual bool transformData(double scaling); From cbabf7714045f3f79bc5522ef21337d19014bea6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Sat, 20 Jan 2018 00:34:54 +0100 Subject: [PATCH 11/11] Actually return false upon faulty reference frame Follows up d819931. --- .../AmorCartesianControl/ICartesianControlImpl.cpp | 4 ++-- .../BasicCartesianControl/ICartesianControlImpl.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp index acfa56478..8b1fd3ac1 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp @@ -385,8 +385,8 @@ bool roboticslab::AmorCartesianControl::setParameter(int vocab, double value) referenceFrame = TCP_FRAME; break; default: - CD_ERROR("Unrecognized of unsupported reference frame vocab: %s.\n", yarp::os::Vocab::decode(value).c_str()); - break; + CD_ERROR("Unrecognized of unsupported reference frame vocab: %s.\n", yarp::os::Vocab::decode((int)value).c_str()); + return false; } break; diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp index f5ba7ef40..f3fc70502 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp @@ -466,8 +466,8 @@ bool roboticslab::BasicCartesianControl::setParameter(int vocab, double value) referenceFrame = TCP_FRAME; break; default: - CD_ERROR("Unrecognized of unsupported reference frame vocab: %s.\n", yarp::os::Vocab::decode(value).c_str()); - break; + CD_ERROR("Unrecognized of unsupported reference frame vocab: %s.\n", yarp::os::Vocab::decode((int)value).c_str()); + return false; } break;