diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp index aa0fe7399f985b4cf08bd321a6cc48dcb15a20c5..de73c55fc27451cb15d1ecec4d80d06f4a9c23d1 100644 --- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp +++ b/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);