Rename ICartesianControl::movi to pose

Closes #198.
roboticslab-uc3m · May 23, 2024 · 25a9c1a · 25a9c1a
1 parent 9a3e621
commit 25a9c1a
Show file tree

Hide file tree

Showing 17 changed files with 73 additions and 54 deletions.
diff --git 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
@@ -125,7 +125,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver,
     bool wait(double timeout) override;
     bool tool(const std::vector<double> & x) override;
     bool act(int command) override;
-    void movi(const std::vector<double> & x) override;
+    void pose(const std::vector<double> & x) override;
     void twist(const std::vector<double> & xdot) override;
     void wrench(const std::vector<double> & w) override;
     bool setParameter(int vocab, double value) override;

diff --git 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<double> &x)
+void BasicCartesianControl::pose(const std::vector<double> &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
@@ -66,7 +66,7 @@ class CartesianControlClient : public yarp::dev::DeviceDriver,
     bool wait(double timeout) override;
     bool tool(const std::vector<double> &x) override;
     bool act(int command) override;
-    void movi(const std::vector<double> &x) override;
+    void pose(const std::vector<double> &x) override;
     void twist(const std::vector<double> &xdot) override;
     void wrench(const std::vector<double> &w) override;
     bool setParameter(int vocab, double value) override;

diff --git 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<double> &xdot)
+void CartesianControlClient::pose(const std::vector<double> &x)
 {
-    handleStreamingConsumerCmd(VOCAB_CC_TWIST, xdot);
+    handleStreamingConsumerCmd(VOCAB_CC_POSE, x);
 }
 
 // -----------------------------------------------------------------------------
 
-void CartesianControlClient::movi(const std::vector<double> &x)
+void CartesianControlClient::twist(const std::vector<double> &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
@@ -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
@@ -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
@@ -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<double> &x) = 0;
+    virtual void pose(const std::vector<double> &x) = 0;
+
+#ifndef SWIG_PREPROCESSOR_SHOULD_SKIP_THIS
+    [[deprecated("use `pose` instead")]]
+    virtual void movi(const std::vector<double> &x)
+    {
+        pose(x);
+    }
+#endif
 
     /**
      * @brief Instantaneous velocity steps

diff --git 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
@@ -51,7 +51,7 @@ bool LinearTrajectoryThread::checkStreamingConfig()
 
 bool LinearTrajectoryThread::configure(const std::vector<double> & 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<double> position = KdlVectorConverter::frameToVector(H);
-        iCartesianControl->movi(position);
+        iCartesianControl->pose(position);
     }
 }
diff --git 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
@@ -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<double> initialTcpOffset;
     std::vector<double> initialLeapOffset;

diff --git 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<double> zeros(6, 0.0);
         iCartesianControl->twist(zeros);

diff --git a/programs/streamingDeviceController/SpnavSensorDevice.hpp b/programs/streamingDeviceController/SpnavSensorDevice.hpp
@@ -41,7 +41,7 @@ class SpnavSensorDevice : public StreamingDevice
 
     std::vector<double> currentX;
 
-    bool usingMovi;
+    bool usingPose;
     double gain;
     bool buttonClose;
     bool buttonOpen;

diff --git 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
@@ -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,15 +37,15 @@ 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;
     }
 
     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<double> zeros(6, 0.0);
         iCartesianControl->twist(zeros);