diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp index 0bddc7e1f..024f143f2 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp @@ -251,7 +251,8 @@ bool BasicCartesianControl::presetStreamingCommand(int command) switch (command) { - case VOCAB_CC_MOVI: + case VOCAB_CC_POSE: + case VOCAB_CC_MOVI: // deprecated return setControlModes(VOCAB_CM_POSITION_DIRECT); case VOCAB_CC_TWIST: return setControlModes(VOCAB_CM_VELOCITY); diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp index 9ddc658dd..d5cffe7c3 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp +++ b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp @@ -125,7 +125,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, bool wait(double timeout) override; bool tool(const std::vector & x) override; bool act(int command) override; - void movi(const std::vector & x) override; + void pose(const std::vector & x) override; void twist(const std::vector & xdot) override; void wrench(const std::vector & w) override; bool setParameter(int vocab, double value) override; diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp index bea641500..71656c56c 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp @@ -426,9 +426,10 @@ bool BasicCartesianControl::act(int command) // ----------------------------------------------------------------------------- -void BasicCartesianControl::movi(const std::vector &x) +void BasicCartesianControl::pose(const std::vector &x) { - if (getCurrentState() != VOCAB_CC_NOT_CONTROLLING || streamingCommand != VOCAB_CC_MOVI + if (getCurrentState() != VOCAB_CC_NOT_CONTROLLING || streamingCommand != VOCAB_CC_POSE + || streamingCommand != VOCAB_CC_MOVI // deprecated || !checkControlModes(VOCAB_CM_POSITION_DIRECT)) { yCError(BCC) << "Streaming command not preset"; diff --git a/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp b/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp index 8bc605920..097f435d9 100644 --- a/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp +++ b/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp @@ -66,7 +66,7 @@ class CartesianControlClient : public yarp::dev::DeviceDriver, bool wait(double timeout) override; bool tool(const std::vector &x) override; bool act(int command) override; - void movi(const std::vector &x) override; + void pose(const std::vector &x) override; void twist(const std::vector &xdot) override; void wrench(const std::vector &w) override; bool setParameter(int vocab, double value) override; diff --git a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp index cf01bc0e0..cc2c18e26 100644 --- a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp @@ -276,16 +276,16 @@ bool CartesianControlClient::act(int command) // ----------------------------------------------------------------------------- -void CartesianControlClient::twist(const std::vector &xdot) +void CartesianControlClient::pose(const std::vector &x) { - handleStreamingConsumerCmd(VOCAB_CC_TWIST, xdot); + handleStreamingConsumerCmd(VOCAB_CC_POSE, x); } // ----------------------------------------------------------------------------- -void CartesianControlClient::movi(const std::vector &x) +void CartesianControlClient::twist(const std::vector &xdot) { - handleStreamingConsumerCmd(VOCAB_CC_MOVI, x); + handleStreamingConsumerCmd(VOCAB_CC_TWIST, xdot); } // ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp index 63722ad67..035cf5e3e 100644 --- a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp +++ b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp @@ -188,7 +188,7 @@ void RpcResponder::makeUsage() std::stringstream ss_cmd; ss_cmd << "(config param) preset streaming command, available:"; - ss_cmd << " [" << Vocab::decode(VOCAB_CC_MOVI) << "]"; + ss_cmd << " [" << Vocab::decode(VOCAB_CC_POSE) << "]"; ss_cmd << " [" << Vocab::decode(VOCAB_CC_TWIST) << "]"; ss_cmd << " [" << Vocab::decode(VOCAB_CC_WRENCH) << "]"; diff --git a/libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp b/libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp index c1b88379c..d1f57cf13 100644 --- a/libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp +++ b/libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp @@ -19,8 +19,9 @@ void StreamResponder::onRead(yarp::os::Bottle& b) switch (b.get(0).asVocab32()) { - case VOCAB_CC_MOVI: - handleConsumerCmdMsg(b, &ICartesianControl::movi); + case VOCAB_CC_POSE: + case VOCAB_CC_MOVI: // deprecated + handleConsumerCmdMsg(b, &ICartesianControl::pose); break; case VOCAB_CC_TWIST: handleConsumerCmdMsg(b, &ICartesianControl::twist); diff --git a/libraries/YarpPlugins/ICartesianControl.h b/libraries/YarpPlugins/ICartesianControl.h index 0caaa01b3..a8f91d654 100644 --- a/libraries/YarpPlugins/ICartesianControl.h +++ b/libraries/YarpPlugins/ICartesianControl.h @@ -73,7 +73,11 @@ constexpr int VOCAB_CC_ACT = yarp::os::createVocab32('a','c','t'); ///< Act */ // Streaming commands -constexpr int VOCAB_CC_MOVI = yarp::os::createVocab32('m','o','v','i'); ///< Achieve pose (position control) +constexpr int VOCAB_CC_POSE = yarp::os::createVocab32('p','o','s','e'); ///< Achieve pose +#ifndef SWIG_PREPROCESSOR_SHOULD_SKIP_THIS +[[deprecated("use `VOCAB_CC_POSE` instead")]] +constexpr int VOCAB_CC_MOVI = yarp::os::createVocab32('m','o','v','i'); +#endif constexpr int VOCAB_CC_TWIST = yarp::os::createVocab32('t','w','s','t'); ///< Instantaneous velocity steps constexpr int VOCAB_CC_WRENCH = yarp::os::createVocab32('w','r','n','c'); ///< Exert force @@ -330,9 +334,18 @@ class ICartesianControl * expected other than computing the inverse kinematics. * * @param x 6-element vector describing desired instantaneous pose in cartesian space; - * first three elements denote translation (meters), last three denote rotation (radians). + * first three elements denote translation (meters), last three denote rotation in scaled + * axis-angle representation (radians). */ - virtual void movi(const std::vector &x) = 0; + virtual void pose(const std::vector &x) = 0; + +#ifndef SWIG_PREPROCESSOR_SHOULD_SKIP_THIS + [[deprecated("use `pose` instead")]] + virtual void movi(const std::vector &x) + { + pose(x); + } +#endif /** * @brief Instantaneous velocity steps diff --git a/programs/keyboardController/KeyboardController.cpp b/programs/keyboardController/KeyboardController.cpp index 7403e636d..7037f75bc 100644 --- a/programs/keyboardController/KeyboardController.cpp +++ b/programs/keyboardController/KeyboardController.cpp @@ -220,9 +220,11 @@ bool KeyboardController::configure(yarp::os::ResourceFinder & rf) currentCartVels.resize(NUM_CART_COORDS, 0.0); - usingThread = rf.check("movi", "use MOVI command"); + usingThread = rf.check("pose", "use POSE command"); + usingThread = usingThread || rf.check("movi", "use POSE command"); // deprecated - int threadMs = rf.check("moviPeriodMs", yarp::os::Value(DEFAULT_THREAD_MS), "MOVI thread period [ms]").asInt32(); + // `moviPeriodMs` is deprecated + int threadMs = rf.check("posePeriodMs", rf.check("moviPeriodMs", yarp::os::Value(DEFAULT_THREAD_MS)), "POSE thread period [ms]").asInt32(); if (usingThread) { @@ -239,7 +241,7 @@ bool KeyboardController::configure(yarp::os::ResourceFinder & rf) if (!linTrajThread->start()) { - yCError(KC) << "Unable to start MOVI thread"; + yCError(KC) << "Unable to start POSE thread"; return false; } } diff --git a/programs/keyboardController/LinearTrajectoryThread.cpp b/programs/keyboardController/LinearTrajectoryThread.cpp index 6a1474f8d..7b8d8c001 100644 --- a/programs/keyboardController/LinearTrajectoryThread.cpp +++ b/programs/keyboardController/LinearTrajectoryThread.cpp @@ -51,7 +51,7 @@ bool LinearTrajectoryThread::checkStreamingConfig() bool LinearTrajectoryThread::configure(const std::vector & vels) { - if (usingStreamingCommandConfig && !iCartesianControl->setParameter(VOCAB_CC_CONFIG_STREAMING_CMD, VOCAB_CC_MOVI)) + if (usingStreamingCommandConfig && !iCartesianControl->setParameter(VOCAB_CC_CONFIG_STREAMING_CMD, VOCAB_CC_POSE)) { yCWarning(KC) << "Unable to preset streaming command"; return false; @@ -110,7 +110,7 @@ void LinearTrajectoryThread::run() deltaX = this->deltaX; mtx.unlock(); - iCartesianControl->movi(deltaX); + iCartesianControl->pose(deltaX); } else { @@ -119,6 +119,6 @@ void LinearTrajectoryThread::run() mtx.unlock(); std::vector position = KdlVectorConverter::frameToVector(H); - iCartesianControl->movi(position); + iCartesianControl->pose(position); } } diff --git a/programs/streamingDeviceController/LeapMotionSensorDevice.cpp b/programs/streamingDeviceController/LeapMotionSensorDevice.cpp index d547b2e2c..a6448e175 100644 --- a/programs/streamingDeviceController/LeapMotionSensorDevice.cpp +++ b/programs/streamingDeviceController/LeapMotionSensorDevice.cpp @@ -15,10 +15,10 @@ using namespace roboticslab; -LeapMotionSensorDevice::LeapMotionSensorDevice(yarp::os::Searchable & config, bool usingMovi) +LeapMotionSensorDevice::LeapMotionSensorDevice(yarp::os::Searchable & config, bool usingPose) : StreamingDevice(config), iAnalogSensor(nullptr), - usingMovi(usingMovi), + usingPose(usingPose), previousTimestamp(0.0), hasActuator(false), grab(false), pinch(false) @@ -60,7 +60,7 @@ bool LeapMotionSensorDevice::initialize(bool usingStreamingPreset) { if (usingStreamingPreset) { - int cmd = usingMovi ? VOCAB_CC_MOVI : VOCAB_CC_TWIST; + int cmd = usingPose ? VOCAB_CC_POSE : VOCAB_CC_TWIST; if (!iCartesianControl->setParameter(VOCAB_CC_CONFIG_STREAMING_CMD, cmd)) { @@ -211,9 +211,9 @@ int LeapMotionSensorDevice::getActuatorState() void LeapMotionSensorDevice::sendMovementCommand(double timestamp) { - if (usingMovi) + if (usingPose) { - iCartesianControl->movi(data); + iCartesianControl->pose(data); } else { diff --git a/programs/streamingDeviceController/LeapMotionSensorDevice.hpp b/programs/streamingDeviceController/LeapMotionSensorDevice.hpp index d951ee087..aa4cf980f 100644 --- a/programs/streamingDeviceController/LeapMotionSensorDevice.hpp +++ b/programs/streamingDeviceController/LeapMotionSensorDevice.hpp @@ -22,7 +22,7 @@ class LeapMotionSensorDevice : public StreamingDevice { public: //! Constructor - LeapMotionSensorDevice(yarp::os::Searchable & config, bool usingMovi); + LeapMotionSensorDevice(yarp::os::Searchable & config, bool usingPose); bool acquireInterfaces() override; @@ -42,7 +42,7 @@ class LeapMotionSensorDevice : public StreamingDevice private: yarp::dev::IAnalogSensor * iAnalogSensor; - bool usingMovi; + bool usingPose; std::vector initialTcpOffset; std::vector initialLeapOffset; diff --git a/programs/streamingDeviceController/SpnavSensorDevice.cpp b/programs/streamingDeviceController/SpnavSensorDevice.cpp index abf798691..1f09d33cc 100644 --- a/programs/streamingDeviceController/SpnavSensorDevice.cpp +++ b/programs/streamingDeviceController/SpnavSensorDevice.cpp @@ -7,10 +7,10 @@ using namespace roboticslab; -SpnavSensorDevice::SpnavSensorDevice(yarp::os::Searchable & config, bool usingMovi, double gain) +SpnavSensorDevice::SpnavSensorDevice(yarp::os::Searchable & config, bool usingPose, double gain) : StreamingDevice(config), iAnalogSensor(nullptr), - usingMovi(usingMovi), + usingPose(usingPose), gain(gain), buttonClose(false), buttonOpen(false) @@ -31,15 +31,15 @@ bool SpnavSensorDevice::acquireInterfaces() bool SpnavSensorDevice::initialize(bool usingStreamingPreset) { - if (usingMovi && gain <= 0.0) + if (usingPose && gain <= 0.0) { - yCWarning(SDC) << "Invalid gain for movi command:" << gain; + yCWarning(SDC) << "Invalid gain for pose command:" << gain; return false; } if (usingStreamingPreset) { - int cmd = usingMovi ? VOCAB_CC_MOVI : VOCAB_CC_TWIST; + int cmd = usingPose ? VOCAB_CC_POSE : VOCAB_CC_TWIST; if (!iCartesianControl->setParameter(VOCAB_CC_CONFIG_STREAMING_CMD, cmd)) { @@ -54,7 +54,7 @@ bool SpnavSensorDevice::initialize(bool usingStreamingPreset) return false; } - if (usingMovi && !iCartesianControl->stat(currentX)) + if (usingPose && !iCartesianControl->stat(currentX)) { yCWarning(SDC) << "Unable to stat initial position, assuming zero"; currentX.resize(6, 0.0); @@ -92,7 +92,7 @@ bool SpnavSensorDevice::acquireData() bool SpnavSensorDevice::transformData(double scaling) { - if (usingMovi) + if (usingPose) { for (int i = 0; i < 6; i++) { @@ -145,7 +145,7 @@ int SpnavSensorDevice::getActuatorState() bool SpnavSensorDevice::hasValidMovementData() const { - if (usingMovi) + if (usingPose) { for (int i = 0; i < 6; i++) { @@ -165,9 +165,9 @@ bool SpnavSensorDevice::hasValidMovementData() const void SpnavSensorDevice::sendMovementCommand(double timestamp) { - if (usingMovi) + if (usingPose) { - iCartesianControl->movi(data); + iCartesianControl->pose(data); for (int i = 0; i < 6; i++) { @@ -182,7 +182,7 @@ void SpnavSensorDevice::sendMovementCommand(double timestamp) void SpnavSensorDevice::stopMotion() { - if (!usingMovi) + if (!usingPose) { std::vector zeros(6, 0.0); iCartesianControl->twist(zeros); diff --git a/programs/streamingDeviceController/SpnavSensorDevice.hpp b/programs/streamingDeviceController/SpnavSensorDevice.hpp index fb9b7144d..b16823b5e 100644 --- a/programs/streamingDeviceController/SpnavSensorDevice.hpp +++ b/programs/streamingDeviceController/SpnavSensorDevice.hpp @@ -41,7 +41,7 @@ class SpnavSensorDevice : public StreamingDevice std::vector currentX; - bool usingMovi; + bool usingPose; double gain; bool buttonClose; bool buttonOpen; diff --git a/programs/streamingDeviceController/StreamingDevice.cpp b/programs/streamingDeviceController/StreamingDevice.cpp index 77993ffa5..6f3f13673 100644 --- a/programs/streamingDeviceController/StreamingDevice.cpp +++ b/programs/streamingDeviceController/StreamingDevice.cpp @@ -15,25 +15,26 @@ using namespace roboticslab; StreamingDevice * StreamingDeviceFactory::makeDevice(const std::string & deviceName, yarp::os::Searchable & config) { auto & deviceConfig = config.findGroup(deviceName.c_str()); - bool usingMovi = config.check("movi", "enable movi command"); + bool usingPose = config.check("pose", "enable pose command"); + usingPose = usingPose || config.check("movi", "enable pose command"); // deprecated yCDebug(SDC) << "Device configuration:" << deviceConfig.toString(); // https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/186 - yCWarning(SDC) << "Using MOVI commands, beware NOT TO EXCEED JOINT LIMITS"; + yCWarning(SDC) << "Using POSE commands, beware NOT TO EXCEED JOINT LIMITS"; if (deviceName == "SpaceNavigator") { double gain = config.check("gain", yarp::os::Value(0.0)).asFloat64(); - return new SpnavSensorDevice(deviceConfig, usingMovi, gain); + return new SpnavSensorDevice(deviceConfig, usingPose, gain); } else if (deviceName == "LeapMotionSensor") { - return new LeapMotionSensorDevice(deviceConfig, usingMovi); + return new LeapMotionSensorDevice(deviceConfig, usingPose); } else if (deviceName == "WiimoteSensor") { - return new WiimoteSensorDevice(deviceConfig, usingMovi); + return new WiimoteSensorDevice(deviceConfig, usingPose); } else { diff --git a/programs/streamingDeviceController/WiimoteSensorDevice.cpp b/programs/streamingDeviceController/WiimoteSensorDevice.cpp index 859fd31cd..ca63edc0f 100644 --- a/programs/streamingDeviceController/WiimoteSensorDevice.cpp +++ b/programs/streamingDeviceController/WiimoteSensorDevice.cpp @@ -10,11 +10,11 @@ using namespace roboticslab; -WiimoteSensorDevice::WiimoteSensorDevice(yarp::os::Searchable & config, bool usingMovi) +WiimoteSensorDevice::WiimoteSensorDevice(yarp::os::Searchable & config, bool usingPose) : StreamingDevice(config), iAnalogSensor(nullptr), mode(NONE), - usingMovi(usingMovi), + usingPose(usingPose), step(0.0) { data.resize(3); // already called by base constructor @@ -37,7 +37,7 @@ bool WiimoteSensorDevice::acquireInterfaces() bool WiimoteSensorDevice::initialize(bool usingStreamingPreset) { - if (usingMovi && step <= 0.0) + if (usingPose && step <= 0.0) { yCWarning(SDC) << "Invalid step:" << step; return false; @@ -45,7 +45,7 @@ bool WiimoteSensorDevice::initialize(bool usingStreamingPreset) if (usingStreamingPreset) { - int cmd = usingMovi ? VOCAB_CC_MOVI : VOCAB_CC_TWIST; + int cmd = usingPose ? VOCAB_CC_POSE : VOCAB_CC_TWIST; if (!iCartesianControl->setParameter(VOCAB_CC_CONFIG_STREAMING_CMD, cmd)) { @@ -149,9 +149,9 @@ void WiimoteSensorDevice::sendMovementCommand(double timestamp) return; } - if (usingMovi) + if (usingPose) { - iCartesianControl->movi(xdot); + iCartesianControl->pose(xdot); } else { @@ -161,7 +161,7 @@ void WiimoteSensorDevice::sendMovementCommand(double timestamp) void WiimoteSensorDevice::stopMotion() { - if (!usingMovi) + if (!usingPose) { std::vector zeros(6, 0.0); iCartesianControl->twist(zeros); diff --git a/programs/streamingDeviceController/WiimoteSensorDevice.hpp b/programs/streamingDeviceController/WiimoteSensorDevice.hpp index 9d2f9b3da..baeedbdc7 100644 --- a/programs/streamingDeviceController/WiimoteSensorDevice.hpp +++ b/programs/streamingDeviceController/WiimoteSensorDevice.hpp @@ -23,7 +23,7 @@ class WiimoteSensorDevice : public StreamingDevice { public: //! Constructor - WiimoteSensorDevice(yarp::os::Searchable & config, bool usingMovi); + WiimoteSensorDevice(yarp::os::Searchable & config, bool usingPose); bool acquireInterfaces() override; @@ -48,7 +48,7 @@ class WiimoteSensorDevice : public StreamingDevice std::vector buffer; - bool usingMovi; + bool usingPose; double step; };