From 06a7019f055b38bedb54b52c48021f6521bdfb45 Mon Sep 17 00:00:00 2001
From: Johann Mantel <j-mantel@gmx.net>
Date: Mon, 13 Dec 2021 18:18:25 +0100
Subject: [PATCH] make aborting behavior configurable
---
.../NJointControllers/GazeController.cpp | 28 +++++++++++--------
.../units/RobotUnit/GazeController.ice | 2 ++
2 files changed, 19 insertions(+), 11 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.cpp
index ff379ac4e..daacae417 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.cpp
@@ -79,21 +79,24 @@ namespace armarx
const Eigen::Vector2f currentJointAngles(_rtYawNode->getJointValue(), _rtPitchNode->getJointValue());
const Eigen::Vector3f targetPoint = _currentTarget.targetPoint->toRootEigen(_rtRobot);
const float h = _rtCameraNode->getPositionInRootFrame().z();
- const float yaw = -std::atan2(targetPoint.x(), targetPoint.y());
- const float pitch = std::atan2(h - targetPoint.z(), targetPoint.y());
+ float yaw = -std::atan2(targetPoint.x(), targetPoint.y());
+ float pitch = std::atan2(h - targetPoint.z(), targetPoint.y());
+ const bool targetReachable = _rtYawNode->checkJointLimits(yaw)
+ && _rtPitchNode->checkJointLimits(pitch);
+ if (!targetReachable)
+ {
+ //limit joint ranges to avoid damage
+ yaw = std::clamp(yaw, _rtYawNode->getJointLimitLo(), _rtYawNode->getJointLimitHi());
+ pitch = std::clamp(pitch, _rtPitchNode->getJointLimitLo(), _rtPitchNode->getJointLimitHi());
+ }
const Eigen::Vector2f targetJointAngles(yaw, pitch);
_pid->update(timeSinceLastIteration.toSecondsDouble(), currentJointAngles, targetJointAngles);
Eigen::Vector2f ctrlVel = _pid->getControlValue();
- //check end conditions
- const bool targetReachable = _rtYawNode->checkJointLimits(targetJointAngles.x())
- && _rtPitchNode->checkJointLimits(targetJointAngles.y());
- const Eigen::Vector2f angleDiff = targetJointAngles - currentJointAngles;
- const bool targetReached = std::abs(angleDiff.x()) < _config->yawAngleTolerance &&
- std::abs(angleDiff.y()) < _config->pitchAngleTolerance;
- if (!targetReachable)
+ if (!targetReachable && _config->abortIfUnreachable)
{
+ //abort the target
_currentTarget.abortedTimestampMilliSeconds = sensorValuesTimestamp.toMilliSeconds();
_tripRt2NonRt.getWriteBuffer() = _currentTarget;
_tripRt2NonRt.commitWrite();
@@ -101,9 +104,12 @@ namespace armarx
_pid->reset();
_controlActive = false;
}
- else if (targetReached)
+ const Eigen::Vector2f angleDiff = targetJointAngles - currentJointAngles;
+ const bool targetReached = std::abs(angleDiff.x()) < _config->yawAngleTolerance &&
+ std::abs(angleDiff.y()) < _config->pitchAngleTolerance;
+ if (targetReached)
{
- if (!_currentTarget.isReached())
+ if (targetReachable && !_currentTarget.isReached())
{
_currentTarget.reachedTimestampMilliSeconds = sensorValuesTimestamp.toMilliSeconds();
_tripRt2NonRt.getWriteBuffer() = _currentTarget;
diff --git a/source/RobotAPI/interface/units/RobotUnit/GazeController.ice b/source/RobotAPI/interface/units/RobotUnit/GazeController.ice
index d8dbc1b2a..04fe7aa27 100644
--- a/source/RobotAPI/interface/units/RobotUnit/GazeController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/GazeController.ice
@@ -44,6 +44,8 @@ module armarx
float yawAngleTolerance = 0.005;
float pitchAngleTolerance = 0.005;
+
+ bool abortIfUnreachable = true;
};
interface GazeControllerInterface extends NJointControllerInterface
--
GitLab