added more sensor processing

source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp

@@ -23,7 +23,7 @@ ParallelGripperPositionController::ParallelGripperPositionController(const std::
         PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) :
     JointPWMPositionController(deviceName, board, jointData, velocityControllerConfigDataPtr)
 {
-    linkedJointConnectorIndex = jointData->getParallelControlActorIndex();
+    linkedJointConnectorIndex = jointData->getSiblingControlActorIndex();
 }
 
 ParallelGripperPositionController::~ParallelGripperPositionController() noexcept(true)
@@ -39,7 +39,6 @@ void ParallelGripperPositionController::rtRun(const IceUtil::Time& sensorValuesT
         target.position += (linkedDataPtr->getRelativePosition() * linkedPositionFactor);
         ARMARX_RT_LOGF_INFO("target.position %.2f, relative partner pos: %.2f", target.position, linkedDataPtr->getRelativePosition()).deactivateSpam(0.5);
         JointPWMPositionController::rtRun(sensorValuesTimestamp, timeSinceLastIteration);
-
     }
     else
     {

source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp

@@ -21,7 +21,7 @@ ParallelGripperVelocityController::ParallelGripperVelocityController(const std::
         PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) :
     JointPWMVelocityController(deviceName, board, jointData, velocityControllerConfigDataPtr)
 {
-    this->linkedJointConnectorIndex = jointData->getParallelControlActorIndex();
+    this->linkedJointConnectorIndex = jointData->getSiblingControlActorIndex();
 
 }
 

source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp

@@ -174,8 +174,35 @@ namespace armarx
                                              defaultConfigurationNode.first_node("JointPWMZeroTorqueControllerDefaultConfiguration")
                                              .add_node_at_end(configNode.first_node("JointPWMZeroTorqueControllerConfig")));
 
-        if (actorDataPtr->getParallelControlActorIndex() != -1)
+        //        ARMARX_CHECK_EQUAL_W_HINT(
+        //            configNode.has_node("ParallelGripperDecoupplingFactor"),
+        //            configNode.has_node("SiblingConnectorId"),
+        //            "Either both or none have to be set.");
+        //        auto tempConfigNode = defaultConfigurationNode.first_node("KITGripperActorDefaultConfiguration").
+        //                              add_node_at_end(configNode);
+        //        parallelGripperDecouplingFactor = tempConfigNode.first_node("ParallelGripperDecoupplingFactor").value_as_float();
+
+        //        if (configNode.has_node("ParallelGripperDecoupplingFactor"))
+        //        {
+        //            const int siblingConnectorId = configNode.first_node("SiblingConnectorId").value_as_int32();
+
+        //            //get sibling
+        //            for (const ActorRobotUnitDevicePtr& dev : board->devices)
+        //            {
+        //                if (dev->actorIndex == siblingConnectorId)
+        //                {
+        //                    sibling = dev.get();
+        //                    break;
+        //                }
+        //            }
+        //            ARMARX_CHECK_NOT_NULL_W_HINT(sibling, "Sibling with connector index " << siblingConnectorId << " not found");
+        //            ARMARX_INFO << "Device " << board->getDeviceName() << ", actor " << getDeviceName() << " is using "
+        //                        << sibling->getDeviceName() << " as a sibling for relative position sensor values";
+        //        }
+
+        if (false && actorDataPtr->getSiblingControlActorIndex() != -1)
         {
+            ARMARX_IMPORTANT << "Using coupled mode for " << getDeviceName();
             // Add Controllers for ParallelGripper
             if (actorDataPtr->getVelocityControlEnabled())
             {
@@ -240,6 +267,7 @@ namespace armarx
         sensorValue.position = actorDataPtr->getPosition();
         sensorValue.relativePosition = actorDataPtr->getRelativePosition();
         sensorValue.velocity = actorDataPtr->getVelocity();
+        sensorValue.absoluteEncoderVelocity = actorDataPtr->getAbsoluteEncoderVelocity();
         sensorValue.targetPWM = actorDataPtr->getTargetPWM();
         sensorValue.motorCurrent = actorDataPtr->getTargetPWM();
         sensorValue.minPWM = actorDataPtr->getCurrentMinPWM();

source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp

@@ -59,7 +59,9 @@ namespace armarx
                 actorData.at(connectorIndex)->velocityTicks = velocity;
                 actorData.at(connectorIndex)->positionControlEnabled = positionControlEnabled;
                 actorData.at(connectorIndex)->velocityControlEnabled = velocityControlEnabled;
-                actorData.at(connectorIndex)->parallelControlEnabled = configNode.first_node("ParallelControlEnabled").value_as_int32();
+
+                actorData.at(connectorIndex)->parallelGripperDecouplingFactor = configNode.first_node("ParallelGripperDecouplingFactor").value_as_float();
+                actorData.at(connectorIndex)->parallelControlEnabled = configNode.first_node("ParallelControlSiblingIndex").value_as_int32();
                 actorData.at(connectorIndex)->currentPWMBoundGradient = configNode.first_node("CurrentPWMBoundGradient").value_as_float();
                 actorData.at(connectorIndex)->currentPWMBoundOffset = configNode.first_node("CurrentPWMBoundOffset").value_as_int32();
             };
@@ -93,6 +95,25 @@ namespace armarx
             }
             ActorData& d = *ptr;
 
+
+            d.relativePosition.read();
+
+            if (d.getSiblingControlActorIndex() >= 0)
+            {
+                auto& d2 = actorData.at(d.getSiblingControlActorIndex());
+                d.adjustedRelativePosition = d.relativePosition.value +
+                                             d2->relativePosition.value * d.parallelGripperDecouplingFactor;
+            }
+            else
+            {
+                d.adjustedRelativePosition = d.relativePosition.value;
+            }
+
+            if (std::isnan(d.relativePositionOffset)) // initialize on first run
+            {
+                d.relativePositionOffset = -d.relativePosition.value + d.position.value;
+            }
+
             if (i == 0)
             {
                 d.rawABSEncoderTicks = (((uint32_t)sensorOUT->RawABSEncoderValueBytes[0] << 24 | (uint32_t)sensorOUT->RawABSEncoderValueBytes[1] << 16 | (uint32_t)sensorOUT->RawABSEncoderValueBytes[2] << 8 | (uint32_t)sensorOUT->RawABSEncoderValueBytes[3]) & 0xFFFFF000) >> 12;
@@ -106,16 +127,25 @@ namespace armarx
                 d.rawABSEncoderTicks = 0;
             }
 
-            d.position.read();
 
-            d.relativePosition.read();
-            if (std::isnan(d.relativePositionOffset)) // initialize on first run
+            if (i > 1)
             {
-                d.relativePositionOffset = -d.relativePosition.value + d.position.value;
+                d.position.value = d.adjustedRelativePosition;
+            }
+            else
+            {
+                d.position.read();
             }
+            d.sanitizedAbsolutePosition = math::MathUtils::angleModPI(d.position.value);
 
             //ARMARX_RT_LOGF_INFO("position %d, relative position: %d", (int)d.rawABSEncoderTicks, d.relativePosition.value).deactivateSpam(0.5);
+            if (!std::isnan(d.lastAbsolutePosition))
+            {
+                d.absoluteEncoderVelocity = math::MathUtils::AngleDelta(d.lastAbsolutePosition, d.sanitizedAbsolutePosition) / timeSinceLastIteration.toSecondsDouble();
+            }
+            d.lastAbsolutePosition = d.sanitizedAbsolutePosition;
             d.velocity.read();
+            d.velocity.value = d.velocityFilter.update(sensorValuesTimestamp, d.velocity.value);
             d.torque.read();
             d.currentMaxPWM = std::round(*d.velocityTicks * d.currentPWMBoundGradient + d.currentPWMBoundOffset);
             d.currentMinPWM = std::round(*d.velocityTicks * d.currentPWMBoundGradient - d.currentPWMBoundOffset);
@@ -151,6 +181,12 @@ namespace armarx
         return actorData.at(actorIndex);
     }
 
+    ActorData::ActorData() :
+        velocityFilter(10)
+    {
+
+    }
+
     void ActorData::setTargetPWM(int32_t targetPWM)
     {
         targetPWM = math::MathUtils::LimitMinMax(currentMinPWM, currentMaxPWM, targetPWM);
@@ -163,17 +199,17 @@ namespace armarx
 
     float ActorData::getPosition() const
     {
-        return position.value;
+        return sanitizedAbsolutePosition;
     }
 
     float ActorData::getRelativePosition() const
     {
-        return relativePosition.value;
+        return adjustedRelativePosition;
     }
 
-    float ActorData::getAdjustedRelativePosition() const
+    float ActorData::getCompensatedRelativePosition() const
     {
-        return relativePosition.value + relativePositionOffset;
+        return adjustedRelativePosition + relativePositionOffset;
     }
 
     float ActorData::getVelocity() const
@@ -221,7 +257,7 @@ namespace armarx
         return currentMaxPWM;
     }
 
-    int32_t ActorData::getParallelControlActorIndex() const
+    int32_t ActorData::getSiblingControlActorIndex() const
     {
         return parallelControlEnabled;
     }
@@ -246,4 +282,14 @@ namespace armarx
         return currentPWMBoundOffset;
     }
 
+    float ActorData::getParallelGripperDecouplingFactor() const
+    {
+        return parallelGripperDecouplingFactor;
+    }
+
+    float ActorData::getAbsoluteEncoderVelocity() const
+    {
+        return absoluteEncoderVelocity;
+    }
+
 } // namespace armarx

source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h

@@ -27,6 +27,7 @@
 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h>
 #include <VirtualRobot/Nodes/RobotNode.h>
+#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
 #include "KITGripperBasisBoardSlave.h"
 
 namespace armarx
@@ -54,6 +55,7 @@ namespace armarx
         int32_t targetPWM;
         float relativePosition;
         float velocityTicksPerMs;
+        float absoluteEncoderVelocity;
         int32_t maxPWM;
         int32_t minPWM;
 
@@ -67,6 +69,7 @@ namespace armarx
             svi.addMemberVariable(&KITGripperActorSensorData::maxPWM, "maxPWM");
             svi.addMemberVariable(&KITGripperActorSensorData::minPWM, "minPWM");
             svi.addMemberVariable(&KITGripperActorSensorData::velocityTicksPerMs, "velocityTicksPerMs");
+            svi.addMemberVariable(&KITGripperActorSensorData::absoluteEncoderVelocity, "absoluteEncoderVelocity");
             return svi;
         }
 
@@ -76,10 +79,11 @@ namespace armarx
     class ActorData
     {
     public:
+        ActorData();
         void setTargetPWM(int32_t targetPWM);
         float getPosition() const;
         float getRelativePosition() const;
-        float getAdjustedRelativePosition() const;
+        float getCompensatedRelativePosition() const;
         float getVelocity() const;
         float getTorque() const;
         float getSoftLimitHi() const;
@@ -92,7 +96,7 @@ namespace armarx
 
         int32_t getCurrentMaxPWM() const;
 
-        int32_t getParallelControlActorIndex() const;
+        int32_t getSiblingControlActorIndex() const;
 
         bool getPositionControlEnabled() const;
 
@@ -102,12 +106,22 @@ namespace armarx
 
         int32_t getCurrentPWMBoundOffset() const;
 
+        float getParallelGripperDecouplingFactor() const;
+
+        float getAbsoluteEncoderVelocity() const;
+
     private:
         u_int32_t rawABSEncoderTicks;
         LinearConvertedValue<int32_t> relativePosition;
+        float adjustedRelativePosition;
         float relativePositionOffset = std::nan("");
+        float parallelGripperDecouplingFactor = std::nanf("");
         LinearConvertedValue<u_int32_t> position;
+        float sanitizedAbsolutePosition;
         LinearConvertedValue<int32_t> velocity;
+        rtfilters::AverageFilter velocityFilter;
+        float absoluteEncoderVelocity = 0.0f;
+        float lastAbsolutePosition = std::nanf("");
         LinearConvertedValue<int32_t> torque;
         LinearConvertedValue<int32_t> targetPWM;
         int32_t* velocityTicks;