decoupled gripper control

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

@@ -64,6 +64,8 @@ namespace armarx
                 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();
+
+                // ARMARX_IMPORTANT << "get decoupling factor: " << actorData.at(connectorIndex)->parallelGripperDecouplingFactor << " and enabled: " << actorData.at(connectorIndex)->parallelControlEnabled;
             };
             switch (connectorIndex)
             {
@@ -101,11 +103,13 @@ namespace armarx
             if (d.getSiblingControlActorIndex() >= 0)
             {
                 auto& d2 = actorData.at(d.getSiblingControlActorIndex());
+                ARMARX_IMPORTANT << "decoupling factor: " << d.parallelGripperDecouplingFactor;
                 d.adjustedRelativePosition = d.relativePosition.value +
                                              d2->relativePosition.value * d.parallelGripperDecouplingFactor;
             }
             else
             {
+                //ARMARX_IMPORTANT << "not even in the right place";
                 d.adjustedRelativePosition = d.relativePosition.value;
             }
 
@@ -146,6 +150,13 @@ namespace armarx
             d.lastAbsolutePosition = d.sanitizedAbsolutePosition;
             d.velocity.read();
             d.velocity.value = d.velocityFilter.update(sensorValuesTimestamp, d.velocity.value);
+
+            if (d.getSiblingControlActorIndex() >= 0)
+            {
+                auto& d2 = actorData.at(d.getSiblingControlActorIndex());
+                d.velocity.value = d.velocity.value + d2->velocity.value * d.parallelGripperDecouplingFactor;
+            }
+
             d.torque.read();
             d.currentMaxPWM = std::round(*d.velocityTicks * d.currentPWMBoundGradient + d.currentPWMBoundOffset);
             d.currentMinPWM = std::round(*d.velocityTicks * d.currentPWMBoundGradient - d.currentPWMBoundOffset);