From 4776ff767def1e1e9622e4c115bc8880783ef58c Mon Sep 17 00:00:00 2001
From: Johann Mantel <j-mantel@gmx.net>
Date: Mon, 13 Dec 2021 14:28:41 +0100
Subject: [PATCH] add GazeController to NJointControllers in RobotUnit
---
.../components/units/RobotUnit/CMakeLists.txt | 2 +
.../NJointControllers/GazeController.cpp | 200 ++++++++++++++++++
.../NJointControllers/GazeController.h | 157 ++++++++++++++
source/RobotAPI/interface/CMakeLists.txt | 2 +-
.../units/RobotUnit/GazeController.ice | 56 +++++
5 files changed, 416 insertions(+), 1 deletion(-)
create mode 100644 source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.cpp
create mode 100644 source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.h
create mode 100644 source/RobotAPI/interface/units/RobotUnit/GazeController.ice
diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index c33e816f5..4e0ebbf68 100755
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -52,6 +52,7 @@ set(LIB_FILES
NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
NJointControllers/NJointCartesianWaypointController.cpp
NJointControllers/NJointCartesianNaturalPositionController.cpp
+ NJointControllers/GazeController.cpp
ControllerParts/CartesianImpedanceController.cpp
@@ -123,6 +124,7 @@ set(LIB_HEADERS
NJointControllers/NJointCartesianVelocityControllerWithRamp.h
NJointControllers/NJointCartesianWaypointController.h
NJointControllers/NJointCartesianNaturalPositionController.h
+ NJointControllers/GazeController.h
ControllerParts/CartesianImpedanceController.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.cpp
new file mode 100644
index 000000000..08ddf641e
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.cpp
@@ -0,0 +1,200 @@
+
+#include "GazeController.h"
+
+namespace armarx
+{
+ GazeController::GazeController(RobotUnitPtr robotUnit, const GazeControllerConfigPtr& config, const VirtualRobot::RobotPtr&):
+ _config(config)
+ {
+ ARMARX_CHECK_NOT_NULL(_config);
+ //robot
+ {
+ _rtRobot = useSynchronizedRtRobot();
+ ARMARX_CHECK_NOT_NULL(_rtRobot);
+ //std::vector<VirtualRobot::RobotNodePtr> nodes = _rtRobot->getRobotNodes();
+ _rtCameraNode = _rtRobot->getRobotNode(_config->cameraNodeName);
+ _rtPitchNode = _rtRobot->getRobotNode(_config->pitchNodeName);
+ _rtYawNode = _rtRobot->getRobotNode(_config->yawNodeName);
+ _rtTorsoNode = _rtRobot->getRobotNode(_config->torsoNodeName);
+ ARMARX_CHECK_NOT_NULL(_rtCameraNode);
+ ARMARX_CHECK_NOT_NULL(_rtPitchNode);
+ ARMARX_CHECK_NOT_NULL(_rtYawNode);
+ ARMARX_CHECK_NOT_NULL(_rtTorsoNode);
+ //Joint velocities to be controlled
+ ControlTarget1DoFActuatorVelocity* pitchCtrlTarget = useControlTarget<ControlTarget1DoFActuatorVelocity>(_config->pitchNodeName, ControlModes::Velocity1DoF);
+ ARMARX_CHECK_NOT_NULL(pitchCtrlTarget);
+ _rtPitchCtrlVel = &(pitchCtrlTarget->velocity);
+ ControlTarget1DoFActuatorVelocity* yawCtrlTarget = useControlTarget<ControlTarget1DoFActuatorVelocity>(_config->yawNodeName, ControlModes::Velocity1DoF);
+ ARMARX_CHECK_NOT_NULL(yawCtrlTarget);
+ _rtYawCtrlVel = &(yawCtrlTarget->velocity);
+ }
+
+ _pid.reset(new MultiDimPIDControllerTemplate<2>(_config->Kp, _config->Ki, _config->Kd, _config->maxControlValue, _config->maxDerivation, true));
+ }
+
+ GazeController::~GazeController()
+ {
+ }
+
+ std::string GazeController::getClassName(const Ice::Current&) const
+ {
+ return "GazeController";
+ }
+
+ void GazeController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ if (_tripBufTarget.updateReadBuffer())
+ {
+ auto& targetBuf = _tripBufTarget._getNonConstReadBuffer();
+ if (targetBuf.stopRequest)
+ {
+ targetBuf.stopRequest = false;
+ if (_controlActive)
+ {
+ _currentTarget.releasedTimestampMilliSeconds = sensorValuesTimestamp.toMilliSeconds();
+ _tripRt2NonRt.getWriteBuffer() = _currentTarget;
+ _tripRt2NonRt.commitWrite();
+ }
+ _controlActive = false;
+ }
+ if (targetBuf.targetRequest)
+ {
+ targetBuf.targetRequest = false;
+ if (_controlActive)
+ {
+ _currentTarget.releasedTimestampMilliSeconds = sensorValuesTimestamp.toMilliSeconds();
+ _tripRt2NonRt.getWriteBuffer() = _currentTarget;
+ _tripRt2NonRt.commitWrite();
+ }
+ _currentTarget = targetBuf.controlTarget;
+ _controlActive = true;
+ }
+ }
+
+ if (_controlActive)
+ {
+ 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());
+ 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)
+ {
+ _currentTarget.abortedTimestampMilliSeconds = sensorValuesTimestamp.toMilliSeconds();
+ _tripRt2NonRt.getWriteBuffer() = _currentTarget;
+ _tripRt2NonRt.commitWrite();
+ ctrlVel.setZero();
+ _pid->reset();
+ _controlActive = false;
+ }
+ else if (targetReached)
+ {
+ if (!_currentTarget.isReached())
+ {
+ _currentTarget.reachedTimestampMilliSeconds = sensorValuesTimestamp.toMilliSeconds();
+ _tripRt2NonRt.getWriteBuffer() = _currentTarget;
+ _tripRt2NonRt.commitWrite();
+ }
+ ctrlVel.setZero();
+ _pid->reset();
+ //keep tracking
+ }
+ //report debugging variables
+ _publishCurrentYawAngle = currentJointAngles.x();
+ _publishCurrentPitchAngle = currentJointAngles.y();
+ _publishTargetYawAngle = targetJointAngles.x();
+ _publishTargetPitchAngle = targetJointAngles.y();
+ _publishYawControlVel = ctrlVel.x();
+ _publishPitchControlVel = ctrlVel.y();
+ //apply velocities
+ *_rtYawCtrlVel = ctrlVel.x();
+ *_rtPitchCtrlVel = ctrlVel.y();
+ }
+ else
+ {
+ *_rtYawCtrlVel = 0;
+ *_rtPitchCtrlVel = 0;
+ }
+ }
+
+ void GazeController::rtPreActivateController()
+ {}
+
+ void GazeController::rtPostDeactivateController()
+ {}
+
+ void GazeController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObserver)
+ {
+
+ const float currentPitchAngle = _publishCurrentPitchAngle;
+ const float currentYawAngle = _publishCurrentYawAngle;
+ const float targetPitchAngle = _publishTargetPitchAngle;
+ const float targetYawAngle = _publishTargetYawAngle;
+ const float pitchControlVel = _publishPitchControlVel;
+ const float yawControlVel = _publishYawControlVel;
+
+ StringVariantBaseMap datafields;
+ datafields["currentPitchAngle"] = new Variant(currentPitchAngle);
+ datafields["currentYawAngle"] = new Variant(currentYawAngle);
+ datafields["targetPitchAngle"] = new Variant(targetPitchAngle);
+ datafields["targetYawAngle"] = new Variant(targetYawAngle);
+ datafields["pitchControlVel"] = new Variant(pitchControlVel);
+ datafields["yawControlVel"] = new Variant(yawControlVel);
+
+ if (_tripRt2NonRt.updateReadBuffer())
+ {
+ GazeTarget target = _tripRt2NonRt.getReadBuffer();
+ datafields["targetRequestTimestamp"] = new Variant(static_cast<long>(target.requestTimestampMilliSeconds));
+ datafields["targetReachedTimestamp"] = new Variant(static_cast<long>(target.reachedTimestampMilliSeconds));
+ datafields["targetReleasedTimestamp"] = new Variant(static_cast<long>(target.releasedTimestampMilliSeconds));
+ datafields["targetAbortedTimestamp"] = new Variant(static_cast<long>(target.abortedTimestampMilliSeconds));
+ }
+
+ debugObserver->setDebugChannel(getInstanceName(), datafields);
+ }
+
+ void GazeController::submitTarget(const FramedPositionBasePtr& targetPoint, const Ice::Current&)
+ {
+ FramedPositionPtr p = FramedPositionPtr::dynamicCast(targetPoint);
+ ARMARX_INFO << "set new target: " << p;
+
+ std::lock_guard g{_tripBufTargetMutex};
+ auto& w = _tripBufTarget.getWriteBuffer();
+ long requestTimestampMilliSeconds = TimeUtil::GetTime().toMilliSeconds();
+ w.stopRequest = false;
+ w.targetRequest = true;
+ w.controlTarget = {requestTimestampMilliSeconds, 0, 0, 0, p};
+ _tripBufTarget.commitWrite();
+ }
+
+ void GazeController::removeTarget(const Ice::Current&)
+ {
+ ARMARX_INFO << "Stop request received";
+ std::lock_guard g(_tripBufTargetMutex);
+ auto& w = _tripBufTarget.getWriteBuffer();
+ w.stopRequest = true;
+ w.targetRequest = false;
+ w.controlTarget = {0, 0, 0, 0, nullptr};
+ _tripBufTarget.commitWrite();
+ }
+
+ void GazeController::removeTargetAfter(long durationMilliSeconds, const Ice::Current&)
+ {
+ std::async(std::launch::async, [](long durationMilliSeconds, GazeController * self)
+ {
+ std::this_thread::sleep_for(std::chrono::milliseconds(durationMilliSeconds));
+ self->removeTarget();
+ }, durationMilliSeconds, this);
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.h
new file mode 100644
index 000000000..4e68beee2
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.h
@@ -0,0 +1,157 @@
+
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+//#include <ArmarXCore/core/Component.h>
+#include <mutex>
+#include <future>
+//#include <ArmarXCore/core/util/TripleBuffer.h>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+#include <RobotAPI/interface/units/RobotUnit/GazeController.h>
+
+namespace armarx
+{
+ TYPEDEF_PTRS_HANDLE(GazeController);
+ /**
+ * @class GazeControlPropertyDefinitions
+ * @brief Property definitions of `GazeControl`.
+ */
+ class GazeControlPropertyDefinitions :
+ public armarx::ComponentPropertyDefinitions
+ {
+ public:
+ GazeControlPropertyDefinitions(std::string prefix);
+ };
+
+ /**
+ * @defgroup Component-GazeControl GazeControl
+ * @ingroup ActiveVision-Components
+ * A description of the component GazeControl.
+ *
+ * @class GazeControl
+ * @ingroup Component-GazeControl
+ * @brief Brief description of class GazeControl.
+ *
+ * Detailed description of class GazeControl.
+ */
+ class GazeController :
+ public NJointController,
+ public GazeControllerInterface
+ {
+ public:
+ using ConfigPtrT = GazeControllerConfigPtr;
+
+ GazeController(RobotUnitPtr robotUnit, const GazeControllerConfigPtr& config, const VirtualRobot::RobotPtr& robot);
+
+ ~GazeController();
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override;
+
+ // NJointController interface
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+
+ // NJointController interface
+ void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
+
+ //GazeController interface
+ void submitTarget(const FramedPositionBasePtr& target, const Ice::Current& c = ::Ice::Current()) override;
+
+ //GazeController interface
+ void removeTarget(const Ice::Current& c = ::Ice::Current()) override;
+ //GazeController interface
+ void removeTargetAfter(long durationMilliSeconds, const Ice::Current& c = ::Ice::Current()) override;
+
+ protected:
+ void rtPreActivateController() override;
+ void rtPostDeactivateController() override;
+
+ private:
+
+ //data fields
+ struct GazeTarget
+ {
+ long requestTimestampMilliSeconds = 0;
+ long reachedTimestampMilliSeconds = 0;
+ long releasedTimestampMilliSeconds = 0;
+ long abortedTimestampMilliSeconds = 0;
+ FramedPositionPtr targetPoint = nullptr;
+ bool isReached()
+ {
+ return reachedTimestampMilliSeconds > 0;
+ }
+ bool isAborted()
+ {
+ return abortedTimestampMilliSeconds > 0;
+ }
+ };
+ struct ControlData
+ {
+ bool stopRequest = false;
+ bool targetRequest = false;
+ GazeTarget controlTarget;
+ };
+
+ //rt variables
+ const GazeControllerConfigPtr _config;
+
+ VirtualRobot::RobotPtr _rtRobot;
+ VirtualRobot::RobotNodePtr _rtYawNode;
+ VirtualRobot::RobotNodePtr _rtPitchNode;
+ VirtualRobot::RobotNodePtr _rtCameraNode;
+ VirtualRobot::RobotNodePtr _rtTorsoNode;
+
+ float* _rtPitchCtrlVel;
+ float* _rtYawCtrlVel;
+
+ std::unique_ptr<MultiDimPIDControllerTemplate<2>> _pid;
+ GazeTarget _currentTarget;
+ bool _controlActive = false;
+
+ //commit buffer
+ mutable std::recursive_mutex _tripBufTargetMutex;
+ WriteBufferedTripleBuffer<ControlData> _tripBufTarget;
+
+ //publishing
+ mutable std::recursive_mutex _tripRt2NonRtMutex;
+ TripleBuffer<GazeTarget> _tripRt2NonRt;
+
+ std::atomic<float> _publishCurrentPitchAngle;
+ std::atomic<float> _publishCurrentYawAngle;
+ std::atomic<float> _publishTargetPitchAngle;
+ std::atomic<float> _publishTargetYawAngle;
+ std::atomic<float> _publishPitchControlVel;
+ std::atomic<float> _publishYawControlVel;
+ };
+}
+
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index a4810afdc..4aebcc67e 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -72,7 +72,7 @@ set(SLICE_FILES
units/RobotUnit/NJointCartesianWaypointController.ice
units/RobotUnit/NJointCartesianNaturalPositionController.ice
units/RobotUnit/RobotUnitInterface.ice
-
+ units/RobotUnit/GazeController.ice
units/RobotUnit/NJointBimanualForceController.ice
diff --git a/source/RobotAPI/interface/units/RobotUnit/GazeController.ice b/source/RobotAPI/interface/units/RobotUnit/GazeController.ice
new file mode 100644
index 000000000..cb4ef9389
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/GazeController.ice
@@ -0,0 +1,56 @@
+
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::RobotAPI
+* @author Raphael Grimm
+* @copyright 2019 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <RobotAPI/interface/core/FramedPoseBase.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
+
+module armarx
+{
+
+ class GazeControllerConfig extends NJointControllerConfig
+ {
+ string cameraFrameName = "DepthCamera";
+ string yawNodeName = "Neck_1_Yaw";
+ string pitchNodeName = "Neck_2_Pitch";
+ string cameraNodeName = "DepthCamera";
+ string torsoNodeName = "TorsoJoint";
+
+ float Kp = 1.9f;
+ float Ki = 0.0f;
+ float Kd = 0.0f;
+ double maxControlValue = 1.0;
+ double maxDerivation = 0.5;
+
+ float yawAngleTolerance = 0.005;
+ float pitchAngleTolerance = 0.005;
+ };
+
+ interface GazeControllerInterface extends NJointControllerInterface
+ {
+ void submitTarget(FramedPositionBase target);
+ void removeTarget();
+ void removeTargetAfter(long durationMilliSeconds);
+ };
+};
--
GitLab