From c9382a82778a72bf16d48a353efbf93488b1cf6e Mon Sep 17 00:00:00 2001
From: ArmarX User <armarx@kit.edu>
Date: Thu, 7 Feb 2019 18:32:21 +0100
Subject: [PATCH] added more sensor processing
---
.../ParallelGripperPositionController.cpp | 3 +-
.../ParallelGripperVelocityController.cpp | 2 +-
.../KITGripperBasisBoard.cpp | 30 ++++++++-
.../KITGripperBasisBoardData.cpp | 66 ++++++++++++++++---
.../KITGripperBasisBoardData.h | 18 ++++-
5 files changed, 103 insertions(+), 16 deletions(-)
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
index 3dcc0692d..969260fe5 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
+++ b/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
{
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp
index c04ca78ab..a4473c5b2 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp
+++ b/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();
}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
index d14e66c6a..ca6298143 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
+++ b/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();
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
index a3550e98c..aa0fe7399 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
+++ b/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
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
index d93760904..f0d9ae529 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
+++ b/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;
--
GitLab