Rename to "interpolated position" (ip)

libraries/YarpPlugins/TechnosoftIpos/CMakeLists.txt

@@ -32,8 +32,8 @@ if(NOT SKIP_TechnosoftIpos)
                                    IRemoteVariablesRawImpl.cpp
                                    ITorqueControlRawImpl.cpp
                                    IVelocityControlRawImpl.cpp
-                                   LinearInterpolationBuffer.hpp
-                                   LinearInterpolationBuffer.cpp
+                                   InterpolatedPositionBuffer.hpp
+                                   InterpolatedPositionBuffer.cpp
                                    StateVariables.hpp
                                    StateVariables.cpp)
 

libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp

@@ -193,8 +193,8 @@ bool TechnosoftIpos::close()
     delete monitorThread;
     monitorThread = nullptr;
 
-    delete linInterpBuffer;
-    linInterpBuffer = nullptr;
+    delete ipBuffer;
+    ipBuffer = nullptr;
 
     delete can;
     can = nullptr;

libraries/YarpPlugins/TechnosoftIpos/IControlModeRawImpl.cpp

@@ -108,7 +108,7 @@ bool TechnosoftIpos::setControlModeRaw(int j, int mode)
             && vars.awaitControlMode(mode);
 
     case VOCAB_CM_POSITION_DIRECT:
-        if (linInterpBuffer)
+        if (ipBuffer)
         {
             vars.ipBufferFilled = vars.ipMotionStarted = false;
 
@@ -119,9 +119,9 @@ bool TechnosoftIpos::setControlModeRaw(int j, int mode)
             return can->driveStatus()->requestState(DriveState::OPERATION_ENABLED)
                     && can->rpdo3()->configure(rpdo3Conf)
                     && can->sdo()->download<std::uint16_t>("Auxiliary Settings Register", 0x0000, 0x208E) // legacy pt mode
-                    && can->sdo()->download("Interpolation sub mode select", linInterpBuffer->getSubMode(), 0x60C0)
-                    && can->sdo()->download("Interpolated position buffer length", linInterpBuffer->getBufferSize(), 0x2073)
-                    && can->sdo()->download("Interpolated position buffer configuration", linInterpBuffer->getBufferConfig(), 0x2074)
+                    && can->sdo()->download("Interpolation sub mode select", ipBuffer->getSubMode(), 0x60C0)
+                    && can->sdo()->download("Interpolated position buffer length", ipBuffer->getBufferSize(), 0x2073)
+                    && can->sdo()->download("Interpolated position buffer configuration", ipBuffer->getBufferConfig(), 0x2074)
                     && can->sdo()->download<std::int8_t>("Modes of Operation", 7, 0x6060)
                     && vars.awaitControlMode(VOCAB_CM_POSITION_DIRECT);
         }

libraries/YarpPlugins/TechnosoftIpos/IPositionDirectRawImpl.cpp

@@ -14,19 +14,19 @@ bool TechnosoftIpos::setPositionRaw(int j, double ref)
     CHECK_JOINT(j);
     CHECK_MODE(VOCAB_CM_POSITION_DIRECT);
 
-    if (linInterpBuffer)
+    if (ipBuffer)
     {
-        linInterpBuffer->addSetpoint(ref); // register point in the internal queue
+        ipBuffer->addSetpoint(ref); // register point in the internal queue
 
         // drive's buffer is empty, motion has not started yet, we have enough points in the queue
-        if (!vars.ipBufferFilled && !vars.ipMotionStarted && linInterpBuffer->isQueueReady())
+        if (!vars.ipBufferFilled && !vars.ipMotionStarted && ipBuffer->isQueueReady())
         {
             std::int32_t refInternal = vars.lastEncoderRead->queryPosition();
-            linInterpBuffer->setInitial(vars.internalUnitsToDegrees(refInternal));
+            ipBuffer->setInitial(vars.internalUnitsToDegrees(refInternal));
 
             bool ok = true;
 
-            for (auto setpoint : linInterpBuffer->popBatch(true))
+            for (auto setpoint : ipBuffer->popBatch(true))
             {
                 ok &= can->rpdo3()->write(setpoint); // load point into the buffer
             }
@@ -66,9 +66,9 @@ bool TechnosoftIpos::getRefPositionRaw(int joint, double * ref)
     CHECK_JOINT(joint);
     CHECK_MODE(VOCAB_CM_POSITION_DIRECT);
 
-    if (linInterpBuffer)
+    if (ipBuffer)
     {
-        *ref = linInterpBuffer->getPrevTarget();
+        *ref = ipBuffer->getPrevTarget();
     }
     else
     {

libraries/YarpPlugins/TechnosoftIpos/IRemoteVariablesRawImpl.cpp

@@ -21,15 +21,15 @@ bool TechnosoftIpos::getRemoteVariableRaw(std::string key, yarp::os::Bottle & va
     {
         yarp::os::Property & dict = val.addDict();
 
-        if (!linInterpBuffer)
+        if (!ipBuffer)
         {
             dict.put("enable", false);
         }
         else
         {
             dict.put("enable", true);
-            dict.put("periodMs", linInterpBuffer->getPeriodMs());
-            dict.put("mode", linInterpBuffer->getType());
+            dict.put("periodMs", ipBuffer->getPeriodMs());
+            dict.put("mode", ipBuffer->getType());
         }
 
         return true;
@@ -84,21 +84,21 @@ bool TechnosoftIpos::setRemoteVariableRaw(std::string key, const yarp::os::Bottl
             return false;
         }
 
-        delete linInterpBuffer;
-        linInterpBuffer = nullptr;
+        delete ipBuffer;
+        ipBuffer = nullptr;
 
         if (dict->find("enable").asBool())
         {
-            linInterpBuffer = createInterpolationBuffer(val, vars);
+            ipBuffer = createInterpolationBuffer(val, vars);
 
-            if (!linInterpBuffer)
+            if (!ipBuffer)
             {
-                CD_ERROR("Cannot create linear interpolation buffer (canId: %d).\n", can->getId());
+                CD_ERROR("Cannot create ip buffer (canId: %d).\n", can->getId());
                 return false;
             }
 
             CD_SUCCESS("Created %s buffer with %d points and period %d ms (canId: %d).\n",
-                linInterpBuffer->getType().c_str(), linInterpBuffer->getBufferSize(), linInterpBuffer->getPeriodMs(), can->getId());
+                ipBuffer->getType().c_str(), ipBuffer->getBufferSize(), ipBuffer->getPeriodMs(), can->getId());
         }
         else
         {

libraries/YarpPlugins/TechnosoftIpos/LinearInterpolationBuffer.cpp → libraries/YarpPlugins/TechnosoftIpos/InterpolatedPositionBuffer.cpp

@@ -1,6 +1,6 @@
 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
-#include "LinearInterpolationBuffer.hpp"
+#include "InterpolatedPositionBuffer.hpp"
 
 #include <cmath>
 
@@ -25,7 +25,7 @@ namespace
     constexpr std::size_t BUFFER_LOW = 4; // max: 15
 }
 
-LinearInterpolationBuffer::LinearInterpolationBuffer(const StateVariables & _vars, int periodMs)
+InterpolatedPositionBuffer::InterpolatedPositionBuffer(const StateVariables & _vars, int periodMs)
     : vars(_vars),
       fixedSamples(periodMs * 0.001 / vars.samplingPeriod),
       integrityCounter(0),
@@ -34,34 +34,34 @@ LinearInterpolationBuffer::LinearInterpolationBuffer(const StateVariables & _var
       sampleCount(0)
 { }
 
-void LinearInterpolationBuffer::setInitial(double initialTarget)
+void InterpolatedPositionBuffer::setInitial(double initialTarget)
 {
     std::lock_guard<std::mutex> lock(queueMutex);
     initialTimestamp = 0.0; // dummy timestamp, to be amended later on
     prevTarget = {initialTarget, initialTimestamp};
     sampleCount = 0;
 }
 
-int LinearInterpolationBuffer::getPeriodMs() const
+int InterpolatedPositionBuffer::getPeriodMs() const
 {
     return fixedSamples * vars.samplingPeriod * 1000.0;
 }
 
-std::uint16_t LinearInterpolationBuffer::getBufferConfig() const
+std::uint16_t InterpolatedPositionBuffer::getBufferConfig() const
 {
     std::bitset<16> bits("1011000010000000"); // 0xB080
     bits |= (BUFFER_LOW << 8);
     bits |= ((integrityCounter << 1) >> 1);
     return bits.to_ulong();
 }
 
-void LinearInterpolationBuffer::addSetpoint(double target)
+void InterpolatedPositionBuffer::addSetpoint(double target)
 {
     std::lock_guard<std::mutex> lock(queueMutex);
     pendingTargets.push_back({target, yarp::os::Time::now()});
 }
 
-std::vector<std::uint64_t> LinearInterpolationBuffer::popBatch(bool fullBuffer)
+std::vector<std::uint64_t> InterpolatedPositionBuffer::popBatch(bool fullBuffer)
 {
     std::lock_guard<std::mutex> lock(queueMutex);
 
@@ -110,35 +110,35 @@ std::vector<std::uint64_t> LinearInterpolationBuffer::popBatch(bool fullBuffer)
     return batch;
 }
 
-double LinearInterpolationBuffer::getPrevTarget() const
+double InterpolatedPositionBuffer::getPrevTarget() const
 {
     std::lock_guard<std::mutex> lock(queueMutex);
     return prevTarget.first;
 }
 
-bool LinearInterpolationBuffer::isQueueReady() const
+bool InterpolatedPositionBuffer::isQueueReady() const
 {
     std::lock_guard<std::mutex> lock(queueMutex);
     return pendingTargets.size() >= getBufferSize() + getOffset();
 }
 
-bool LinearInterpolationBuffer::isQueueEmpty() const
+bool InterpolatedPositionBuffer::isQueueEmpty() const
 {
     std::lock_guard<std::mutex> lock(queueMutex);
     return pendingTargets.empty();
 }
 
-std::uint8_t LinearInterpolationBuffer::getIntegrityCounter() const
+std::uint8_t InterpolatedPositionBuffer::getIntegrityCounter() const
 {
     return integrityCounter;
 }
 
-std::size_t LinearInterpolationBuffer::getOffset() const
+std::size_t InterpolatedPositionBuffer::getOffset() const
 {
     return 0;
 }
 
-std::uint16_t LinearInterpolationBuffer::getSampledTime(double currentTimestamp)
+std::uint16_t InterpolatedPositionBuffer::getSampledTime(double currentTimestamp)
 {
     if (fixedSamples != 0)
     {
@@ -154,7 +154,7 @@ std::uint16_t LinearInterpolationBuffer::getSampledTime(double currentTimestamp)
     return currentWindow;
 }
 
-double LinearInterpolationBuffer::getMeanVelocity(const ip_record & earliest, const ip_record & latest) const
+double InterpolatedPositionBuffer::getMeanVelocity(const ip_record & earliest, const ip_record & latest) const
 {
     double distance = latest.first - earliest.first;
 
@@ -243,10 +243,10 @@ std::uint64_t PvtBuffer::makeDataRecord(const ip_record & previous, const ip_rec
 namespace roboticslab
 {
 
-LinearInterpolationBuffer * createInterpolationBuffer(const yarp::os::Searchable & config, const StateVariables & vars)
+InterpolatedPositionBuffer * createInterpolationBuffer(const yarp::os::Searchable & config, const StateVariables & vars)
 {
-    std::string mode = config.check("mode", yarp::os::Value(""), "linear interpolation mode [pt|pvt]").asString();
-    int periodMs = config.check("periodMs", yarp::os::Value(0), "linear interpolation fixed period (ms)").asInt32();
+    std::string mode = config.check("mode", yarp::os::Value(""), "interpolated position submode [pt|pvt]").asString();
+    int periodMs = config.check("periodMs", yarp::os::Value(0), "interpolated position fixed period (ms)").asInt32();
 
     if (periodMs < 0)
     {
@@ -264,7 +264,7 @@ LinearInterpolationBuffer * createInterpolationBuffer(const yarp::os::Searchable
     }
     else
     {
-        CD_ERROR("Unsupported linear interpolation mode: \"%s\".\n", mode.c_str());
+        CD_ERROR("Unsupported interpolated position submode: \"%s\".\n", mode.c_str());
         return nullptr;
     }
 }

libraries/YarpPlugins/TechnosoftIpos/LinearInterpolationBuffer.hpp → libraries/YarpPlugins/TechnosoftIpos/InterpolatedPositionBuffer.hpp

@@ -1,7 +1,7 @@
 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
-#ifndef __LINEAR_INTERPOLATION_BUFFER_HPP__
-#define __LINEAR_INTERPOLATION_BUFFER_HPP__
+#ifndef __INTERPOLATED_POSITION_BUFFER_HPP__
+#define __INTERPOLATED_POSITION_BUFFER_HPP__
 
 #include <cstdint>
 
@@ -24,14 +24,14 @@ namespace roboticslab
  * Stores an internal queue of setpoints aimed to be processed in batches on
  * demand by client code.
  */
-class LinearInterpolationBuffer
+class InterpolatedPositionBuffer
 {
 public:
     //! Constructor, sets internal invariable parameters.
-    LinearInterpolationBuffer(const StateVariables & vars, int periodMs);
+    InterpolatedPositionBuffer(const StateVariables & vars, int periodMs);
 
     //! Virtual destructor.
-    virtual ~LinearInterpolationBuffer() = default;
+    virtual ~InterpolatedPositionBuffer() = default;
 
     //! Store initial position.
     void setInitial(double initialTarget);
@@ -98,12 +98,12 @@ class LinearInterpolationBuffer
 
 /**
  * @ingroup TechnosoftIpos
- * @brief Implementation of a PT buffer.
+ * @brief Implementation of a PT buffer (linear interpolation).
  */
-class PtBuffer : public LinearInterpolationBuffer
+class PtBuffer : public InterpolatedPositionBuffer
 {
 public:
-    using LinearInterpolationBuffer::LinearInterpolationBuffer;
+    using InterpolatedPositionBuffer::InterpolatedPositionBuffer;
 
     std::string getType() const override;
     std::uint16_t getBufferSize() const override;
@@ -115,15 +115,15 @@ class PtBuffer : public LinearInterpolationBuffer
 
 /**
  * @ingroup TechnosoftIpos
- * @brief Implementation of a PVT buffer.
+ * @brief Implementation of a PVT buffer (cubic interpolation).
  *
  * By using the lasts two position targets, this class computes the mean
  * velocity target required by the linear interpolator.
  */
-class PvtBuffer : public LinearInterpolationBuffer
+class PvtBuffer : public InterpolatedPositionBuffer
 {
 public:
-    using LinearInterpolationBuffer::LinearInterpolationBuffer;
+    using InterpolatedPositionBuffer::InterpolatedPositionBuffer;
 
     std::string getType() const override;
     std::uint16_t getBufferSize() const override;
@@ -135,8 +135,8 @@ class PvtBuffer : public LinearInterpolationBuffer
 };
 
 //! Factory method.
-LinearInterpolationBuffer * createInterpolationBuffer(const yarp::os::Searchable & config, const StateVariables & vars);
+InterpolatedPositionBuffer * createInterpolationBuffer(const yarp::os::Searchable & config, const StateVariables & vars);
 
 } // namespace roboticslab
 
-#endif // __LINEAR_INTERPOLATION_BUFFER_HPP__
+#endif // __INTERPOLATED_POSITION_BUFFER_HPP__

libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.cpp

@@ -397,25 +397,25 @@ void TechnosoftIpos::interpretIpStatus(std::uint16_t status)
             "Drive has maintained interpolated position mode after a buffer empty condition.");
     reportBitToggle(report, WARN, 12, "Integrity counter error.", "No integrity counter error.");
 
-    if (reportBitToggle(report, NONE, 13, "Buffer is full.", "Buffer is not full.") && linInterpBuffer
+    if (reportBitToggle(report, NONE, 13, "Buffer is full.", "Buffer is not full.") && ipBuffer
             && !vars.ipMotionStarted)
     {
         // buffer full, ready to enable ip mode
         vars.ipMotionStarted = can->driveStatus()->controlword(can->driveStatus()->controlword().set(4));
     }
 
-    if (reportBitToggle(report, NONE, 14, "Buffer is low.", "Buffer is not low.") && linInterpBuffer
+    if (reportBitToggle(report, NONE, 14, "Buffer is low.", "Buffer is not low.") && ipBuffer
             && vars.ipMotionStarted
-            && !linInterpBuffer->isQueueEmpty())
+            && !ipBuffer->isQueueEmpty())
     {
         // load next batch of points into the drive's buffer
-        for (auto setpoint : linInterpBuffer->popBatch(false))
+        for (auto setpoint : ipBuffer->popBatch(false))
         {
             can->rpdo3()->write(setpoint);
         }
     }
 
-    if (reportBitToggle(report, INFO, 15, "Buffer is empty.", "Buffer is not empty.") && linInterpBuffer
+    if (reportBitToggle(report, INFO, 15, "Buffer is empty.", "Buffer is not empty.") && ipBuffer
             && vars.ipMotionStarted
             && can->driveStatus()->controlword(can->driveStatus()->controlword().reset(4)))
     {

libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp

@@ -22,7 +22,8 @@
 
 #include "CanOpenNode.hpp"
 #include "ICanBusSharer.hpp"
-#include "LinearInterpolationBuffer.hpp"
+
+#include "InterpolatedPositionBuffer.hpp"
 #include "StateVariables.hpp"
 
 #define CHECK_JOINT(j) do { int ax; if (getAxes(&ax), (j) != ax - 1) return false; } while (0)
@@ -77,7 +78,7 @@ class TechnosoftIpos : public yarp::dev::DeviceDriver,
         : can(nullptr),
           iEncodersTimedRawExternal(nullptr),
           iExternalEncoderCanBusSharer(nullptr),
-          linInterpBuffer(nullptr),
+          ipBuffer(nullptr),
           monitorThread(nullptr)
     { }
 
@@ -290,7 +291,7 @@ class TechnosoftIpos : public yarp::dev::DeviceDriver,
 
     StateVariables vars;
 
-    LinearInterpolationBuffer * linInterpBuffer;
+    InterpolatedPositionBuffer * ipBuffer;
 
     yarp::os::Timer * monitorThread;
 };