From bf1c4834747e3818178c5d65e3bb5a5e2c6b755b Mon Sep 17 00:00:00 2001
From: ArmarX User <armarx@kit.edu>
Date: Mon, 11 Feb 2019 17:44:38 +0100
Subject: [PATCH] decoupled gripper control
---
.../KITGripperBasisBoard/KITGripperBasisBoardData.cpp | 11 +++++++++++
1 file changed, 11 insertions(+)
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
index aa0fe7399..de73c55fc 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);
--
GitLab