diff --git a/source/armarx/control/deprecated_njoint_mp_controller/task_space/NJointTaskSpaceImpedanceDMPController.cpp b/source/armarx/control/deprecated_njoint_mp_controller/task_space/NJointTaskSpaceImpedanceDMPController.cpp
index 0eab5983c6b6c440d426f3b9e1bf5a2dfe2fcd32..86742d217dfcc1bd7bdb56c132da65bfda178394 100644
--- a/source/armarx/control/deprecated_njoint_mp_controller/task_space/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/armarx/control/deprecated_njoint_mp_controller/task_space/NJointTaskSpaceImpedanceDMPController.cpp
@@ -1,21 +1,21 @@
// Simox
-#include <VirtualRobot/Robot.h>
#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Robot.h>
// ArmarXCore
-#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
// RobotAPI
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
// control
-#include <armarx/control/deprecated_njoint_mp_controller/task_space/TaskSpaceImpedanceDMPControllerInterface.h>
#include <armarx/control/deprecated_njoint_mp_controller/TaskSpaceVMP.h>
-#include "NJointTaskSpaceImpedanceDMPController.h"
+#include <armarx/control/deprecated_njoint_mp_controller/task_space/TaskSpaceImpedanceDMPControllerInterface.h>
+#include "NJointTaskSpaceImpedanceDMPController.h"
namespace armarx::control::deprecated_njoint_mp_controller::task_space
{
@@ -94,7 +94,8 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
- dmpCtrl.reset(new tsvmp::TaskSpaceDMPController("DMPController", taskSpaceDMPConfig, false));
+ dmpCtrl.reset(
+ new tsvmp::TaskSpaceDMPController("DMPController", taskSpaceDMPConfig, false));
finished = false;
useNullSpaceJointDMP = cfg->useNullSpaceJointDMP;
@@ -158,7 +159,6 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
return "NJointTaskSpaceImpedanceDMPController";
}
-
void
NJointTaskSpaceImpedanceDMPController::rtPreActivateController()
{
@@ -171,7 +171,6 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
reinitTripleBuffer(initData);
}
-
void
NJointTaskSpaceImpedanceDMPController::controllerRun()
{
@@ -496,7 +495,6 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
debugOutputData.commitWrite();
}
-
void
NJointTaskSpaceImpedanceDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames,
const Ice::Current&)
@@ -558,7 +556,6 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
isNullSpaceJointDMPLearned = true;
}
-
void
NJointTaskSpaceImpedanceDMPController::resetDMP(const Ice::Current&)
{
@@ -589,7 +586,6 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
useNullSpaceJointDMP = enable;
}
-
void
NJointTaskSpaceImpedanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals,
Ice::Double timeDuration,
@@ -611,7 +607,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
while (firstRun || timeForCalibration < cfg->waitTimeForCalibration)
{
- if(prematurely_stopped.load())
+ if (prematurely_stopped.load())
{
ARMARX_WARNING << "StopDMP has been prematurely called; aborting runDMP";
return;
@@ -634,7 +630,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
ARMARX_INFO << "Using Null Space Joint DMP";
}
- if(prematurely_stopped.load())
+ if (prematurely_stopped.load())
{
ARMARX_WARNING << "StopDMP has been prematurely called; aborting runDMP";
return;
@@ -645,7 +641,6 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
// controllerTask->start();
}
-
void
NJointTaskSpaceImpedanceDMPController::onPublish(const SensorAndControl&,
const DebugDrawerInterfacePrx&,
@@ -861,7 +856,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKd(const Eigen::VectorXf& kd,
const Ice::Current&)
{
- ARMARX_CHECK_EQUAL(kd.size(), targets.size());
+ ARMARX_CHECK_EQUAL(kd.size(), static_cast<long>(targets.size()));
ARMARX_INFO << "set nullspace kd " << VAROUT(kd);
LockGuardType guard(controllerMutex);
ctrlParams.getWriteBuffer().dnull = kd;
@@ -872,28 +867,28 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKp(const Eigen::VectorXf& kp,
const Ice::Current&)
{
- ARMARX_CHECK_EQUAL(kp.size(), targets.size());
+ ARMARX_CHECK_EQUAL(kp.size(), static_cast<long>(targets.size()));
ARMARX_INFO << "set linear kp " << VAROUT(kp);
LockGuardType guard(controllerMutex);
ctrlParams.getWriteBuffer().knull = kp;
ctrlParams.commitWrite();
}
-
void
NJointTaskSpaceImpedanceDMPController::setDefaultNullSpaceJointValues(
const Eigen::VectorXf& jointValues,
const Ice::Current&)
{
- ARMARX_CHECK_EQUAL(jointValues.size(), targets.size());
+ ARMARX_CHECK_EQUAL(jointValues.size(), static_cast<long>(targets.size()));
defaultNullSpaceJointValues.getWriteBuffer() = jointValues;
defaultNullSpaceJointValues.commitWrite();
}
- Ice::Double NJointTaskSpaceImpedanceDMPController::getVirtualTime(const Ice::Current&)
+ Ice::Double
+ NJointTaskSpaceImpedanceDMPController::getVirtualTime(const Ice::Current&)
{
return dmpCtrl->canVal;
}
-} // namespace armarx::ctrl::njoint_ctrl::dmp
+} // namespace armarx::control::deprecated_njoint_mp_controller::task_space
diff --git a/source/armarx/control/ethercat/CMakeLists.txt b/source/armarx/control/ethercat/CMakeLists.txt
index 5924ad1d7dbdc873d6fcedfde5fc53180687c096..8d317d24459f0f851f905431415d91263a0b657d 100644
--- a/source/armarx/control/ethercat/CMakeLists.txt
+++ b/source/armarx/control/ethercat/CMakeLists.txt
@@ -2,6 +2,8 @@ armarx_add_library(ethercat
SOURCES
Bus.cpp
DataInterface.cpp
+ LinearConvertedValue.cpp
+ ModularConvertedValue.cpp
DeviceInterface.cpp
RTUnit.cpp
SlaveIdentifier.cpp
@@ -24,6 +26,8 @@ armarx_add_library(ethercat
HEADERS
Bus.h
DataInterface.h
+ LinearConvertedValue.h
+ ModularConvertedValue.h
DeviceInterface.h
RTUnit.h
SlaveIdentifier.h
diff --git a/source/armarx/control/ethercat/DataInterface.h b/source/armarx/control/ethercat/DataInterface.h
index 5162a0de76cc8fc9829889174dc3bc7c33bd50c8..6502324221dbd02f9c7ca93752e6a4d4e231eee2 100644
--- a/source/armarx/control/ethercat/DataInterface.h
+++ b/source/armarx/control/ethercat/DataInterface.h
@@ -28,6 +28,7 @@
#include <cmath>
#include <memory>
#include <type_traits>
+
#include <SimoxUtility/math/periodic/periodic_clamp.h>
#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
@@ -38,258 +39,12 @@
#include <armarx/control/hardware_config/Types.h>
+#include "LinearConvertedValue.h"
+#include "ModularConvertedValue.h"
namespace armarx::control::ethercat
{
- /**
- * @class LinearConvertedValue
- * @ingroup Library-ethercat
- * @brief Brief description of class LinearConvertedValue.
- *
- * Detailed description of class LinearConvertedValue.
- */
- template <typename T>
- class LinearConvertedValue
- {
-
- public:
- LinearConvertedValue()
- {
- raw = nullptr;
- offset = factor = 0;
- offsetBeforeFactor = true;
- }
-
- /**
- * @brief init
- * @param raw
- * @param node
- * @param defaultValue
- * @param offsetBeforeFactor if true the offset is added before multiplying with factor. If false: the other way around.
- * @param nameForDebugging This name is printend in case an error is encountered (Its only purpose is to ease debugging)
- */
- void
- init(T* raw,
- const hardware_config::types::LinearConfig linearConfig,
- float defaultValue = std::nanf("1"),
- bool offsetBeforeFactor = true,
- const char* nameForDebugging = "")
- {
- init(raw,
- linearConfig.getFactor(),
- linearConfig.getOffset(),
- defaultValue,
- offsetBeforeFactor,
- nameForDebugging);
- }
-
- void
- init(T* raw,
- float factor,
- float offset,
- float defaultValue = std::nanf("1"),
- bool offsetBeforeFactor = true,
- const char* nameForDebugging = "")
- {
- const auto rawAsInt = reinterpret_cast<std::uint64_t>(raw);
- ARMARX_CHECK_EXPRESSION((rawAsInt % alignof(T)) == 0)
- << "\nThe alignment is wrong!\nIt has to be " << alignof(T)
- << ", but the data is aligned with " << rawAsInt % alignof(std::max_align_t)
- << "!\nThis is an offset of " << (rawAsInt % alignof(T))
- << " bytes!\nThe datatype is " << GetTypeString<T>() << "\nIts size is "
- << sizeof(T) << "\nraw = " << raw << " bytes\nThe name is " << nameForDebugging;
- this->offsetBeforeFactor = offsetBeforeFactor;
- this->factor = factor;
- this->offset = offset;
- this->raw = raw;
- if (!std::isnan(defaultValue))
- {
- value = defaultValue;
- write();
- }
- else
- {
- value = 0;
- read();
- }
- }
-
- void
- read()
- {
- if (offsetBeforeFactor)
- {
- value = ((*raw) + offset) * factor;
- }
- else
- {
- value = (*raw) * factor + offset;
- }
- }
-
- void
- write()
- {
- if (offsetBeforeFactor)
- {
- *raw = static_cast<T>((value / factor) - offset);
- }
- else
- {
- *raw = static_cast<T>((value)-offset) / factor;
- }
- }
-
- float value;
-
- T
- getRaw() const
- {
- return *raw;
- }
-
- float
- getFactor() const
- {
- return factor;
- }
-
- float
- getOffset() const
- {
- return offset;
- }
-
- bool
- getOffsetBeforeFactor() const
- {
- return offsetBeforeFactor;
- }
-
- private:
- T* raw;
- float offset, factor;
- bool offsetBeforeFactor;
- };
-
-
- template <typename T, typename SignedT = T>
- class ModularConvertedValue
- {
-
- public:
- ModularConvertedValue()
- {
- rawValue = nullptr;
- zeroOffset = 0;
- discontinuityOffset = 0;
- maxValue = 0;
- factor = 0;
- isInverted = false;
- }
-
- void
- init(T* raw,
- const hardware_config::types::ModularConvertedValueConfig modularConfig,
- const char* nameForDebugging = "")
- {
- init(raw,
- modularConfig.getZeroOffset(),
- modularConfig.getDiscontinuityOffset(),
- modularConfig.getMaxValue(),
- modularConfig.getIsInverted(),
- nameForDebugging);
- }
-
- void
- init(T* raw,
- float zeroOffset,
- float discontinuityOffset,
- float maxValue,
- bool isInverted,
- const char* nameForDebugging = "")
- {
- const auto rawAsInt = reinterpret_cast<std::uint64_t>(raw);
- ARMARX_CHECK_EQUAL((rawAsInt % alignof(T)), 0)
- << "\nThe alignment is wrong!"
- << "\nIt has to be " << alignof(T) << ", but the data is aligned with "
- << rawAsInt % alignof(std::max_align_t) << "!"
- << "\nThis is an offset of " << (rawAsInt % alignof(T)) << " bytes!"
- << "\nThe datatype is " << GetTypeString<T>() << "\nIts size is " << sizeof(T)
- << "\nraw = " << raw << " bytes"
- << "\nThe name is '" << nameForDebugging << "'";
-
- this->rawValue = raw;
- this->zeroOffset = static_cast<SignedT>(zeroOffset);
- this->discontinuityOffset = static_cast<SignedT>(discontinuityOffset);
- this->maxValue = static_cast<T>(maxValue);
- this->factor = 2 * M_PIf64 / maxValue;
- this->isInverted = isInverted;
-
- ARMARX_CHECK_POSITIVE(maxValue)
- << "max value must not be zero (cannot divide/modulo by 0).";
-
- read();
- }
-
- void
- read()
- {
- // "%" operator is not the real mathematical modulo operation but some creative ISO
- // reinterpretation. This fixes it.
- auto actual_real_modulo = [](SignedT dividend, SignedT divisor) -> SignedT
- {
- const SignedT result = dividend % divisor;
- return result >= 0 ? result : result + divisor;
- };
-
- const SignedT rawValueSigned = static_cast<SignedT>(*rawValue);
- const SignedT maxValueSigned = static_cast<SignedT>(maxValue);
-
- // Normalize direction of rotation.
- const SignedT sign = isInverted ? -1 : 1;
- const SignedT dirnorm = actual_real_modulo((sign * rawValueSigned), maxValueSigned);
-
- // Raw values within monotonic_raw_value are guaranteed to not fall into a
- // discontinuity.
- const SignedT monotonic_dirnorm =
- actual_real_modulo((dirnorm + discontinuityOffset), maxValueSigned);
-
- // Zero shift.
- const SignedT zeroshift_monotonic_dirnorm = monotonic_dirnorm - zeroOffset;
-
- // Convert ticks to rad.
- const double zeroshift_monotonic_dirnorm_rad =
- static_cast<double>(zeroshift_monotonic_dirnorm) * factor;
-
- value = zeroshift_monotonic_dirnorm_rad;
- }
-
- void
- write()
- {
- //*rawValue = static_cast<T>((value / factor) - offset);
- }
-
- double value;
-
- T
- getRaw() const
- {
- return *rawValue;
- }
-
- private:
- T* rawValue;
- SignedT zeroOffset;
- SignedT discontinuityOffset;
- T maxValue;
- double factor;
- bool isInverted;
- };
-
-
/**
* @class DataInterface
* @ingroup Library-ethercat
diff --git a/source/armarx/control/ethercat/ErrorReporting.cpp b/source/armarx/control/ethercat/ErrorReporting.cpp
index 32bb68b32d784fb37e8e9d29773cb99297c5c885..63f348c7d54f6c0007053a389ac07a68cbc52379 100644
--- a/source/armarx/control/ethercat/ErrorReporting.cpp
+++ b/source/armarx/control/ethercat/ErrorReporting.cpp
@@ -48,7 +48,6 @@ namespace armarx::control::ethercat::error
return stream;
}
-
struct Reporting::QueueImpl
{
static constexpr size_t queueSize = 20;
diff --git a/source/armarx/control/ethercat/LinearConvertedValue.cpp b/source/armarx/control/ethercat/LinearConvertedValue.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4319279e0338a61cce595f189fa5adda4206c484
--- /dev/null
+++ b/source/armarx/control/ethercat/LinearConvertedValue.cpp
@@ -0,0 +1,7 @@
+#include "LinearConvertedValue.h"
+
+namespace armarx::control::ethercat
+{
+
+
+} // namespace armarx::control::ethercat
diff --git a/source/armarx/control/ethercat/LinearConvertedValue.h b/source/armarx/control/ethercat/LinearConvertedValue.h
new file mode 100644
index 0000000000000000000000000000000000000000..9744ba828e0a6efc1c5081451e0e65f8b8ebfd96
--- /dev/null
+++ b/source/armarx/control/ethercat/LinearConvertedValue.h
@@ -0,0 +1,168 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+#include <cmath>
+
+#include <armarx/control/hardware_config/Types.h>
+
+namespace armarx::control::ethercat
+{
+
+ /**
+ * @class LinearConvertedValue
+ * @ingroup Library-ethercat
+ * @brief Brief description of class LinearConvertedValue.
+ *
+ * Detailed description of class LinearConvertedValue.
+ */
+ template <typename T>
+ class LinearConvertedValue
+ {
+
+ public:
+ LinearConvertedValue()
+ {
+ raw = nullptr;
+ offset = factor = 0;
+ offsetBeforeFactor = true;
+ }
+
+ /**
+ * @brief init
+ * @param raw
+ * @param node
+ * @param defaultValue
+ * @param offsetBeforeFactor if true the offset is added before multiplying with factor. If false: the other way around.
+ * @param nameForDebugging This name is printend in case an error is encountered (Its only purpose is to ease debugging)
+ */
+ void
+ init(T* raw,
+ const hardware_config::types::LinearConfig linearConfig,
+ float defaultValue = std::nanf("1"),
+ bool offsetBeforeFactor = true,
+ const char* nameForDebugging = "")
+ {
+ init(raw,
+ linearConfig.getFactor(),
+ linearConfig.getOffset(),
+ defaultValue,
+ offsetBeforeFactor,
+ nameForDebugging);
+ }
+
+ void
+ init(T* raw,
+ float factor,
+ float offset,
+ float defaultValue = std::nanf("1"),
+ bool offsetBeforeFactor = true,
+ const char* nameForDebugging = "")
+ {
+#if 0
+ const auto rawAsInt = reinterpret_cast<std::uint64_t>(raw);
+ ARMARX_CHECK_EQUAL((rawAsInt % alignof(T)), 0)
+ << "\nThe alignment is wrong!\nIt has to be " << alignof(T)
+ << ", but the data is aligned with " << rawAsInt % alignof(std::max_align_t)
+ << "!\nThis is an offset of " << (rawAsInt % alignof(T))
+ << " bytes!\nThe datatype is " << GetTypeString<T>() << "\nIts size is "
+ << sizeof(T) << "\nraw = " << raw << " bytes\nThe name is " << nameForDebugging;
+#endif
+ this->offsetBeforeFactor = offsetBeforeFactor;
+ this->factor = factor;
+ this->offset = offset;
+ this->raw = raw;
+ if (!std::isnan(defaultValue))
+ {
+ value = defaultValue;
+ write();
+ }
+ else
+ {
+ value = 0;
+ read();
+ }
+ }
+
+ void
+ read()
+ {
+ if (offsetBeforeFactor)
+ {
+ value = ((*raw) + offset) * factor;
+ }
+ else
+ {
+ value = (*raw) * factor + offset;
+ }
+ }
+
+ void
+ write()
+ {
+ if (offsetBeforeFactor)
+ {
+ *raw = static_cast<T>((value / factor) - offset);
+ }
+ else
+ {
+ *raw = static_cast<T>((value)-offset) / factor;
+ }
+ }
+
+ float value;
+
+ T
+ getRaw() const
+ {
+ return *raw;
+ }
+
+ float
+ getFactor() const
+ {
+ return factor;
+ }
+
+ float
+ getOffset() const
+ {
+ return offset;
+ }
+
+ bool
+ getOffsetBeforeFactor() const
+ {
+ return offsetBeforeFactor;
+ }
+
+ private:
+ T* raw;
+ float offset, factor;
+ bool offsetBeforeFactor;
+ };
+
+} // namespace armarx::control::ethercat
diff --git a/source/armarx/control/ethercat/ModularConvertedValue.cpp b/source/armarx/control/ethercat/ModularConvertedValue.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0ac2b1e74be885a4ffc94478afc2c3aa4a09f713
--- /dev/null
+++ b/source/armarx/control/ethercat/ModularConvertedValue.cpp
@@ -0,0 +1,6 @@
+#include "ModularConvertedValue.h"
+
+namespace armarx::control::ethercat
+{
+
+} // namespace armarx::control::ethercat
diff --git a/source/armarx/control/ethercat/ModularConvertedValue.h b/source/armarx/control/ethercat/ModularConvertedValue.h
new file mode 100644
index 0000000000000000000000000000000000000000..20784860e515c71edc0d33b94cb1bc9eea7c3784
--- /dev/null
+++ b/source/armarx/control/ethercat/ModularConvertedValue.h
@@ -0,0 +1,172 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+#include <cmath>
+#include <limits>
+#include <type_traits>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <armarx/control/hardware_config/Types.h>
+
+namespace armarx::control::ethercat
+{
+
+ template <typename T, typename SignedT = std::int64_t>
+ class ModularConvertedValue
+ {
+
+ public:
+ ModularConvertedValue()
+ {
+ rawValue = nullptr;
+ zeroOffset = 0;
+ discontinuityOffset = 0;
+ maxValue = 0;
+ factor = 0;
+ isInverted = false;
+
+ static_assert(std::is_signed_v<SignedT>);
+
+ if constexpr(std::is_unsigned_v<T>)
+ {
+ static_assert(sizeof(SignedT) > sizeof(T));
+ }
+
+ if constexpr(std::is_signed_v<T>)
+ {
+ static_assert(sizeof(SignedT) >= sizeof(T));
+ }
+ }
+
+ void
+ init(T* raw,
+ const hardware_config::types::ModularConvertedValueConfig modularConfig,
+ const char* nameForDebugging = "")
+ {
+ init(raw,
+ modularConfig.getZeroOffset(),
+ modularConfig.getDiscontinuityOffset(),
+ modularConfig.getMaxValue(),
+ modularConfig.getIsInverted(),
+ nameForDebugging);
+ }
+
+ void
+ init(T* raw,
+ float zeroOffset,
+ float discontinuityOffset,
+ float maxValue,
+ bool isInverted,
+ const char* nameForDebugging = "")
+ {
+#if 0
+ const auto rawAsInt = reinterpret_cast<std::uint64_t>(raw);
+ ARMARX_CHECK_EQUAL((rawAsInt % alignof(T)), 0)
+ << "\nThe alignment is wrong!"
+ << "\nIt has to be " << alignof(T) << ", but the data is aligned with "
+ << rawAsInt % alignof(std::max_align_t) << "!"
+ << "\nThis is an offset of " << (rawAsInt % alignof(T)) << " bytes!"
+ << "\nThe datatype is " << GetTypeString<T>() << "\nIts size is " << sizeof(T)
+ << "\nraw = " << raw << " bytes"
+ << "\nThe name is '" << nameForDebugging << "'";
+#endif
+ ARMARX_CHECK_POSITIVE(maxValue)
+ << "max value must not be zero (cannot divide/modulo by 0).";
+
+ ARMARX_CHECK_LESS_EQUAL(maxValue, std::numeric_limits<T>::max())
+ << "max value is greater than the maximum value of the datatype, "
+ << "the datatype is " << GetTypeString<T>();
+
+ this->rawValue = raw;
+ this->zeroOffset = static_cast<SignedT>(zeroOffset);
+ this->discontinuityOffset = static_cast<SignedT>(discontinuityOffset);
+ this->maxValue = static_cast<SignedT>(maxValue);
+ this->factor = 2 * M_PIf64 / maxValue;
+ this->isInverted = isInverted;
+
+ read();
+ }
+
+ void
+ read()
+ {
+ // "%" operator is not the real mathematical modulo operation but some creative ISO
+ // reinterpretation. This fixes it.
+ auto actual_real_modulo = [](SignedT dividend, SignedT divisor) -> SignedT
+ {
+ const SignedT result = dividend % divisor;
+ return result >= 0 ? result : result + divisor;
+ };
+
+ const SignedT rawValueSigned = static_cast<SignedT>(*rawValue);
+ const SignedT maxValueSigned = static_cast<SignedT>(maxValue);
+
+
+ // Normalize direction of rotation.
+ const SignedT sign = isInverted ? -1 : 1;
+ const SignedT dirnorm = actual_real_modulo((sign * rawValueSigned), maxValueSigned);
+
+ // Raw values within monotonic_raw_value are guaranteed to not fall into a
+ // discontinuity.
+ const SignedT monotonic_dirnorm =
+ actual_real_modulo((dirnorm + discontinuityOffset), maxValueSigned);
+
+ // Zero shift.
+ const SignedT zeroshift_monotonic_dirnorm = monotonic_dirnorm - zeroOffset;
+
+ // Convert ticks to rad.
+ const double zeroshift_monotonic_dirnorm_rad =
+ static_cast<double>(zeroshift_monotonic_dirnorm) * factor;
+
+ value = zeroshift_monotonic_dirnorm_rad;
+ }
+
+ void
+ write()
+ {
+ //*rawValue = static_cast<T>((value / factor) - offset);
+ }
+
+ double value;
+
+ T
+ getRaw() const
+ {
+ return *rawValue;
+ }
+
+ private:
+ T* rawValue;
+ SignedT zeroOffset;
+ SignedT discontinuityOffset;
+ T maxValue;
+ double factor;
+ bool isInverted;
+ };
+
+} // namespace armarx::control::ethercat