From 691e0e39734205b5d7662ceb007815feafeb4956 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Tue, 5 Feb 2019 12:59:02 +0100
Subject: [PATCH] moved code from ImagineRT to RobotAPI to support new ARMAR-6
Head
---
.../libraries/ArmarXEtherCAT/EtherCAT.cpp | 84 +++---
source/RobotAPI/libraries/CMakeLists.txt | 1 +
.../KITGripperEtherCAT/CMakeLists.txt | 85 ++++++
...JointKITGripperEmergencyStopController.cpp | 28 ++
.../JointKITGripperEmergencyStopController.h | 36 +++
.../JointKITGripperStopMovementController.cpp | 25 ++
.../JointKITGripperStopMovementController.h | 34 +++
.../JointPWMPositionController.cpp | 245 ++++++++++++++++
.../JointPWMPositionController.h | 62 +++++
.../JointPWMVelocityController.cpp | 246 +++++++++++++++++
.../JointPWMVelocityController.h | 62 +++++
.../JointZeroTorqueController.cpp | 218 +++++++++++++++
.../JointZeroTorqueController.h | 73 +++++
.../JointController/PWMVelocityController.cpp | 102 +++++++
.../JointController/PWMVelocityController.h | 77 ++++++
.../ParallelGripperPositionController.cpp | 66 +++++
.../ParallelGripperPositionController.h | 45 +++
.../ParallelGripperVelocityController.cpp | 61 ++++
.../ParallelGripperVelocityController.h | 44 +++
.../KITGripperBasisBoard.cpp | 259 +++++++++++++++++
.../KITGripperBasisBoard.h | 137 +++++++++
.../KITGripperBasisBoardData.cpp | 249 +++++++++++++++++
.../KITGripperBasisBoardData.h | 150 ++++++++++
.../KITGripperBasisBoardSlave.cpp | 140 ++++++++++
.../KITGripperBasisBoardSlave.h | 139 ++++++++++
.../Misc/TorqueEstimation.h | 261 ++++++++++++++++++
.../Misc/TorqueEstimationWeights.h | 189 +++++++++++++
.../KITGripperEtherCAT/test/CMakeLists.txt | 5 +
.../test/ImagineEtherCATTest.cpp | 48 ++++
29 files changed, 3129 insertions(+), 42 deletions(-)
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.cpp
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.h
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.h
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.h
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.h
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.h
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationWeights.h
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/test/CMakeLists.txt
create mode 100644 source/RobotAPI/libraries/KITGripperEtherCAT/test/ImagineEtherCATTest.cpp
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
index c215f9e38..7bba263b2 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
@@ -325,7 +325,7 @@ bool EtherCAT::startBus(bool createDevices)
//all slaves ar in Op - not only EtherCAT Op also DS 402 for the elmos
busInOperationalMode = true;
- busUpdateLastUpdateTime = IceUtil::Time::now();
+ busUpdateLastUpdateTime = IceUtil::Time::now(IceUtil::Time::Monotonic);
//if we get here all went good,
return true;
}
@@ -378,7 +378,7 @@ bool EtherCAT::setPDOMappings()
ARMARX_IMPORTANT << "current Slave" << slave->getSlaveNumber();
ARMARX_IMPORTANT << "slaveCount: " << ec_slavecount;
-// ARMARX_IMPORTANT << "size in: " << sizeof(ELMO_in_t) << " size out: " << sizeof(ELMO_out_t);
+ // ARMARX_IMPORTANT << "size in: " << sizeof(ELMO_in_t) << " size out: " << sizeof(ELMO_out_t);
ARMARX_IMPORTANT << "iomap ptr: 0x" << std::hex << &IOmap << std::dec;
ARMARX_IMPORTANT << "sn:" << slave->getSlaveNumber() << std::flush;
ARMARX_IMPORTANT << "out: 0x" << std::hex << (long)(ec_slave[slave->getSlaveNumber()].outputs) << std::dec
@@ -425,7 +425,7 @@ std::pair<std::vector<ControlDevicePtr>, std::vector<SensorDevicePtr>> EtherCAT:
const std::string messageSlaveIdentifier = "[Slave index: " + std::to_string(currentSlaveIndex) + "] ";
//DEBUG for EtherCAT HUB
//check the device type to identify the EtherCAT Hub
-// uint32 deviceType = ETHTERCAT_HUB_DEVICE_TYPE;
+ // uint32 deviceType = ETHTERCAT_HUB_DEVICE_TYPE;
//uint32 vendorID = 0;
ARMARX_INFO << messageSlaveIdentifier << " device type: " << ec_slave[currentSlaveIndex].Dtype << "\n itype: " << std::hex << ec_slave[currentSlaveIndex].Itype <<
"\neep_id: " << ec_slave[currentSlaveIndex].eep_id << "\neep_man: " << ec_slave[currentSlaveIndex].eep_man << "\neep_rev: " << ec_slave[currentSlaveIndex].eep_rev;
@@ -497,6 +497,7 @@ bool EtherCAT::updateBus(bool doErrorHandling)
}
else if (switchON_OFF == OFF)
{
+ ARMARX_WARNING << "Could not update bus because it switched off - closing bus (again?)";
closeBus();
return false;
}
@@ -555,7 +556,7 @@ bool EtherCAT::updateBus(bool doErrorHandling)
}
}
- auto delay = (IceUtil::Time::now() - busUpdateLastUpdateTime);
+ auto delay = (IceUtil::Time::now(IceUtil::Time::Monotonic) - busUpdateLastUpdateTime);
if (delay.toMilliSecondsDouble() > 40)
{
ARMARX_RT_LOGF_WARN("Update bus was not called for a long time: %d ms", delay.toMilliSecondsDouble()).deactivateSpam(5);
@@ -565,7 +566,7 @@ bool EtherCAT::updateBus(bool doErrorHandling)
// RT_TIMING_START(UpdatePDO);
updatePDO();
// RT_TIMING_CEND(UpdatePDO, 0.7);
- busUpdateLastUpdateTime = IceUtil::Time::now();
+ busUpdateLastUpdateTime = IceUtil::Time::now(IceUtil::Time::Monotonic);
//error handling
@@ -615,7 +616,6 @@ void EtherCAT::closeBus()
//shutdown the slaves if the bus hasn't died
if (!isBusDead && !error)
{
- auto startCycle = std::chrono::high_resolution_clock::now();
bool found;
do
{
@@ -1201,28 +1201,28 @@ bool EtherCAT::readSDO(uint16_t slave, uint16_t index, uint8_t subindex, uint32_
*/
bool EtherCAT::getVendorID(uint16_t slave, uint32_t& vendorID) const
{
-// uint32 serial;
+ // uint32 serial;
bool retVal = readSDO(slave, 0x1018, 1, vendorID);
-// getSerialNumber(slave, serial);
+ // getSerialNumber(slave, serial);
ARMARX_INFO << "Vendor ID of slave " << slave << " 0x" << std::hex << vendorID << std::dec << " (" << vendorID
<< ")";
-// << " serial number: " << serial;
-// if (retVal && (vendorID == ELMO_VENDOR_ID))
-// {
-// ARMARX_DEBUG << "(Elmomc)";
-// }
-// else if (retVal && (vendorID == H2T_VENDOR_ID))
-// {
-// ARMARX_DEBUG << "(H2T)";
-// }
-// else if (retVal)
-// {
-// ARMARX_WARNING << "Unknown vendor";
-// }
-// else
-// {
-// ARMARX_ERROR << "reading Vendor Id failed";
-// }
+ // << " serial number: " << serial;
+ // if (retVal && (vendorID == ELMO_VENDOR_ID))
+ // {
+ // ARMARX_DEBUG << "(Elmomc)";
+ // }
+ // else if (retVal && (vendorID == H2T_VENDOR_ID))
+ // {
+ // ARMARX_DEBUG << "(H2T)";
+ // }
+ // else if (retVal)
+ // {
+ // ARMARX_WARNING << "Unknown vendor";
+ // }
+ // else
+ // {
+ // ARMARX_ERROR << "reading Vendor Id failed";
+ // }
return retVal;
}
@@ -1239,23 +1239,23 @@ bool EtherCAT::getProductCode(uint16_t slave, uint32_t& productCode) const
bool retVal = readSDO(slave, 0x1018, 2, productCode);
//printf("Product Code of slave %d: 0x%x (%d)", slave, productCode, productCode);
ARMARX_DEBUG << "Product Code of slave " << slave << ": 0x" << std::hex << productCode << std::dec
- << " (" << productCode << ")";
-// if (retVal && (productCode == ELMO_ACTOR_PRODUCT_CODE))
-// {
-// ARMARX_DEBUG << "actor elmo";
-// }
-// else if (retVal && (productCode == H2T_SENSOBOARD_PRODUCT_CODE))
-// {
-// ARMARX_DEBUG << "sensor board";
-// }
-// else if (retVal)
-// {
-// ARMARX_WARNING << "Unknown product code ";
-// }
-// else
-// {
-// ARMARX_ERROR << "reading Product Code failed";
-// }
+ << " (" << productCode << ")";
+ // if (retVal && (productCode == ELMO_ACTOR_PRODUCT_CODE))
+ // {
+ // ARMARX_DEBUG << "actor elmo";
+ // }
+ // else if (retVal && (productCode == H2T_SENSOBOARD_PRODUCT_CODE))
+ // {
+ // ARMARX_DEBUG << "sensor board";
+ // }
+ // else if (retVal)
+ // {
+ // ARMARX_WARNING << "Unknown product code ";
+ // }
+ // else
+ // {
+ // ARMARX_ERROR << "reading Product Code failed";
+ // }
return retVal;
}
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index d0047b51b..d5d62998c 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -1,5 +1,6 @@
add_subdirectory(core)
add_subdirectory(ArmarXEtherCAT)
+add_subdirectory(KITGripperEtherCAT)
add_subdirectory(widgets)
add_subdirectory(SimpleJsonLogger)
add_subdirectory(RobotAPIVariantWidget)
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt b/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt
new file mode 100644
index 000000000..2c8f1cbca
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt
@@ -0,0 +1,85 @@
+set(LIB_NAME ImagineEtherCAT)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+find_package(SOEM QUIET)
+armarx_build_if(SOEM_FOUND "SOEM not available")
+if (SOEM_FOUND)
+ include_directories(SYSTEM ${SOEM_INCLUDE_DIR})
+endif()
+
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+#
+# all include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+# include_directories(${MyLib_INCLUDE_DIRS})
+#endif()
+
+
+if(ARMARX_USE_QT5)
+ #someone gets qt5::Designer in the link flags!
+ #this is a hotfix for this problem
+ armarx_find_qt(Designer OpenGL)
+ list(APPEND LIBS ${QT_LIBRARIES})
+endif()
+
+
+find_package(Eigen3 QUIET)
+find_package(Simox QUIET)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+if (Eigen3_FOUND AND Simox_FOUND)
+ include_directories(${Simox_INCLUDE_DIRS})
+ include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
+endif()
+
+set(LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXEtherCAT
+ ${Simox_LIBRARIES}
+ ${SOEM_LIBRARIES}
+)
+
+set(LIB_FILES
+KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp
+KITGripperBasisBoard/KITGripperBasisBoardData.cpp
+KITGripperBasisBoard/KITGripperBasisBoard.cpp
+KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
+KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
+KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
+KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.cpp
+KITGripperBasisBoard/JointController/PWMVelocityController.cpp
+KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp
+KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
+KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(LIB_HEADERS
+KITGripperBasisBoard/KITGripperBasisBoardSlave.h
+KITGripperBasisBoard/KITGripperBasisBoardData.h
+KITGripperBasisBoard/KITGripperBasisBoard.h
+KITGripperBasisBoard/Misc/TorqueEstimation.h
+KITGripperBasisBoard/Misc/TorqueEstimationWeights.h
+KITGripperBasisBoard/JointController/JointPWMVelocityController.h
+KITGripperBasisBoard/JointController/JointPWMPositionController.h
+KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h
+KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.h
+KITGripperBasisBoard/JointController/PWMVelocityController.h
+KITGripperBasisBoard/JointController/JointZeroTorqueController.h
+KITGripperBasisBoard/JointController/ParallelGripperPositionController.h
+KITGripperBasisBoard/JointController/ParallelGripperVelocityController.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
new file mode 100644
index 000000000..6ed80f25a
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
@@ -0,0 +1,28 @@
+#include "JointKITGripperEmergencyStopController.h"
+
+namespace armarx
+{
+
+ JointKITGripperEmergencyStopController::JointKITGripperEmergencyStopController(ActorDataPtr dataPtr) :
+ dataPtr(dataPtr)
+ {
+
+ }
+ void JointKITGripperEmergencyStopController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+
+ }
+
+ ControlTargetBase* JointKITGripperEmergencyStopController::getControlTarget()
+ {
+ return ⌖
+ }
+
+ void JointKITGripperEmergencyStopController::rtPreActivateController()
+ {
+ // ARMARX_INFO << "Stopping gripper!";
+ dataPtr->setTargetPWM(0);
+ }
+
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h
new file mode 100644
index 000000000..043b63f39
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include "../KITGripperBasisBoardData.h"
+
+
+namespace armarx
+{
+
+ class JointKITGripperEmergencyStopController;
+ typedef std::shared_ptr<JointKITGripperEmergencyStopController> JointKITGripperEmergencyStopControllerPtr;
+
+
+ class JointKITGripperEmergencyStopController : public JointController
+ {
+ public:
+ JointKITGripperEmergencyStopController(ActorDataPtr dataPtr);
+ private:
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ /**
+ * Returns the Target for this controller, but as this is the Emergency controller it will ignored.
+ * As this controller will just break
+ * @return is type VelocityTarget but it will return a nullptr, because it won't be possible to set a target
+ */
+ ControlTargetBase* getControlTarget() override;
+
+ void rtPreActivateController() override;
+ private:
+ DummyControlTargetEmergencyStop target;
+ ActorDataPtr dataPtr;
+ };
+
+
+
+} // namespace
+
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.cpp
new file mode 100644
index 000000000..6f675f437
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.cpp
@@ -0,0 +1,25 @@
+#include "JointKITGripperStopMovementController.h"
+
+namespace armarx
+{
+ JointKITGripperStopMovementController::JointKITGripperStopMovementController(ActorDataPtr dataPtr) :
+ dataPtr(dataPtr)
+ {
+
+ }
+
+ void JointKITGripperStopMovementController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ }
+
+ ControlTargetBase* JointKITGripperStopMovementController::getControlTarget()
+ {
+ return ⌖
+ }
+
+ void JointKITGripperStopMovementController::rtPreActivateController()
+ {
+ // ARMARX_INFO << "Stopping gripper!";
+ dataPtr->setTargetPWM(0);
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.h
new file mode 100644
index 000000000..1a041018f
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include "../KITGripperBasisBoardData.h"
+
+
+namespace armarx
+{
+
+ class JointKITGripperStopMovementController;
+ typedef std::shared_ptr<JointKITGripperStopMovementController> JointKITGripperStopMovementControllerPtr;
+
+
+ class JointKITGripperStopMovementController : public JointController
+ {
+ public:
+ JointKITGripperStopMovementController(ActorDataPtr dataPtr);
+ private:
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ /**
+ * Returns the Target for this controller, but as this is the Emergency controller it will ignored.
+ * As this controller will just break
+ * @return is type VelocityTarget but it will return a nullptr, because it won't be possible to set a target
+ */
+ ControlTargetBase* getControlTarget() override;
+
+ void rtPreActivateController() override;
+ private:
+ DummyControlTargetStopMovement target;
+ ActorDataPtr dataPtr;
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
new file mode 100644
index 000000000..715b81c5f
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
@@ -0,0 +1,245 @@
+#include <chrono>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "JointPWMPositionController.h"
+#include "../KITGripperBasisBoard.h"
+#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+
+#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
+#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
+
+#include <ArmarXCore/core/application/Application.h>
+
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+
+using namespace armarx;
+
+
+JointPWMPositionController::JointPWMPositionController(const std::string deviceName, KITGripperBasisBoardPtr board,
+ ActorDataPtr jointData,
+ PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) : JointController(),
+ config(velocityControllerConfigDataPtr),
+ controller(velocityControllerConfigDataPtr),
+ target(), board(board), deviceName(deviceName)
+{
+ actorIndex = board->getActorIndex(deviceName);
+ dataPtr = jointData;
+
+ posController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
+ posController.deceleration = velocityControllerConfigDataPtr->maxDecelerationRad;
+ posController.maxDt = velocityControllerConfigDataPtr->maxDt;
+ posController.maxV = velocityControllerConfigDataPtr->maxVelocityRad;
+ ARMARX_CHECK_GREATER_EQUAL(jointData->getSoftLimitHi(), jointData->getSoftLimitLo());
+ // controller.positionLimitHiHard = dataPtr->getHardLimitHi();
+ posController.positionLimitHi = jointData->getSoftLimitHi();
+ // controller.positionLimitLoHard = dataPtr->getHardLimitLo();
+ posController.positionLimitLo = jointData->getSoftLimitLo();
+ posController.p = 2.0f;
+ this->isLimitless = jointData->isLimitless();
+
+}
+
+JointPWMPositionController::~JointPWMPositionController() noexcept(true)
+{
+ stopRequested = true;
+ try
+ {
+ threadHandle.join();
+ }
+ catch (...)
+ {
+
+ }
+}
+
+void JointPWMPositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+{
+ if (target.isValid())
+ {
+ auto currentPosition = dataPtr->getPosition();
+ if (isLimitless)
+ {
+ ARMARX_RT_LOGF_WARNING("Position controller not implemented for limitless joints").deactivateSpam(10);
+ }
+ else
+ {
+ posController.currentPosition = currentPosition;
+ }
+ posController.currentV = lastTargetVelocity;
+ posController.dt = timeSinceLastIteration.toSecondsDouble();
+ posController.targetPosition = target.position;
+ double newVel = posController.run();
+ // double newVel = posController.p * (posController.targetPosition - posController.currentPosition);
+ // newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
+ // ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel);
+ if (std::isnan(newVel))
+ {
+ newVel = 0;
+ }
+
+ auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel));
+
+ if (std::isnan(targetPWM))
+ {
+ throw LocalException() << "Target PWM of " << getParent().getDeviceName() << " is NaN!";
+ }
+ dataPtr->setTargetPWM(targetPWM);
+
+ lastTargetVelocity = newVel;
+ // auto name = getParent().getDeviceName().c_str();
+ // ARMARX_RT_LOGF_INFO("%s: position: %.f, target position: %.f, targetvelocity: %.f, target PWM: %d", name,
+ // currentPosition, target.position, newVel, targetPWM).deactivateSpam(1);
+ // ARMARX_INFO << deactivateSpam(1) << VAROUT(name) << VAROUT(currentPosition) << VAROUT(target.position) << VAROUT(newVel) << VAROUT(targetPWM);
+
+
+ }
+ else
+ {
+ ARMARX_ERROR << deactivateSpam(1) << "invalid target set for actor " << getParent().getDeviceName();
+ }
+}
+
+ControlTargetBase* JointPWMPositionController::getControlTarget()
+{
+ return ⌖
+}
+
+void JointPWMPositionController::rtPreActivateController()
+{
+ lastTargetVelocity = dataPtr->getVelocity();
+ controller.reset(dataPtr->getVelocity());
+}
+
+void JointPWMPositionController::rtPostDeactivateController()
+{
+ ARMARX_RT_LOGF_INFO("Setting PWM to 0");
+ dataPtr->setTargetPWM(0);
+}
+
+StringVariantBaseMap JointPWMPositionController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
+{
+
+ if (!remoteGui)
+ {
+ // threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
+ // {
+ // std::string guiTabName;
+ // while (!stopRequested)
+ // {
+ // ManagedIceObjectPtr object;
+ // ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
+ // try
+ // {
+ // object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
+ // ARMARX_CHECK_EXPRESSION(object);
+ // remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
+ // if (!remoteGui)
+ // {
+ // continue;
+ // }
+ // ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
+ // guiTabName = getParent().getDeviceName() + getControlMode();
+ // break;
+ // }
+ // catch (...)
+ // {
+ // sleep(1);
+ // }
+
+ // }
+ // if (remoteGui)
+ // {
+ // ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
+ // using namespace RemoteGui;
+
+
+
+ // auto vLayout = makeVBoxLayout();
+
+ // {
+ // WidgetPtr KpLabel = makeTextLabel("Kp: ");
+
+ // WidgetPtr KiSlider = makeFloatSlider("KpSlider")
+ // .min(0.0f).max(5000.0f)
+ // .value(config->p);
+ // WidgetPtr line = makeHBoxLayout()
+ // .children({KpLabel, KiSlider});
+
+ // vLayout.addChild(line);
+
+ // }
+
+
+ // {
+ // WidgetPtr KiLabel = makeTextLabel("Ki: ");
+ // WidgetPtr KiSlider = makeFloatSlider("KiSlider")
+ // .min(0.0f).max(50000.0f)
+ // .value(config->i);
+
+ // WidgetPtr line = makeHBoxLayout()
+ // .children({KiLabel, KiSlider});
+
+ // vLayout.addChild(line);
+
+ // }
+
+ // {
+ // WidgetPtr KdLabel = makeTextLabel("Kd: ");
+ // WidgetPtr KdSlider = makeFloatSlider("KdSlider")
+ // .min(0.0f).max(50.0f)
+ // .steps(100)
+ // .value(config->d);
+
+ // WidgetPtr line = makeHBoxLayout()
+ // .children({KdLabel, KdSlider});
+
+ // vLayout.addChild(line);
+ // vLayout.addChild(new VSpacer);
+ // }
+
+ // // WidgetPtr spin = makeFloatSpinBox("KpSpin")
+ // // .min(0.0f).max(2.0f)
+ // // .steps(20).decimals(2)
+ // // .value(0.4f);
+
+
+
+
+ // WidgetPtr groupBox = makeGroupBox("GroupBox")
+ // .label("Group")
+ // .child(vLayout);
+
+ // remoteGui->createTab(guiTabName, groupBox);
+
+ // while (!stopRequested)
+ // {
+ // RemoteGui::TabProxy tab(remoteGui, guiTabName);
+ // tab.receiveUpdates();
+ // this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
+ // this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
+ // this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
+ // usleep(100000);
+ // }
+ // }
+
+ // });
+ }
+ return {{"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
+ {"targetPosition", new Variant(posController.targetPosition)},
+ {"error", new Variant(posController.targetPosition - posController.currentPosition)},
+ {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
+ {"pidIntegralCV", new Variant(controller.pid->integral * controller.pid->Ki)},
+ {"pidPropCV", new Variant(controller.pid->previousError * controller.pid->Kp)},
+ {"pidDiffCV", new Variant(controller.pid->derivative * controller.pid->Kd)},
+ {"pidUsed", new Variant(posController.getCurrentlyPIDActive())}
+
+
+ };
+}
+
+
+
+
+
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
new file mode 100644
index 000000000..b5ab09fff
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include <memory>
+#include <chrono>
+
+#include <RobotAPI/libraries/core/PIDController.h>
+
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+#include "../KITGripperBasisBoardData.h"
+#include <ArmarXGui/interface/RemoteGuiInterface.h>
+#include <ArmarXCore/core/services/tasks/ThreadPool.h>
+
+#include <ArmarXCore/observers/filters/AverageFilter.h>
+#include "PWMVelocityController.h"
+
+
+namespace armarx
+{
+ using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
+
+
+ class JointPWMPositionController;
+ typedef std::shared_ptr<JointPWMPositionController> JointPWMPositionControllerPtr;
+
+ class JointPWMPositionController : public JointController
+ {
+ public:
+ JointPWMPositionController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
+ ~JointPWMPositionController() noexcept(true);
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
+ ControlTargetBase* getControlTarget() override;
+
+ void rtPreActivateController() override;
+ protected:
+ PWMVelocityControllerConfigurationCPtr config;
+ PWMVelocityController controller;
+ PositionThroughVelocityControllerWithAccelerationAndPositionBounds posController;
+
+ ControlTarget1DoFActuatorPosition target;
+
+ std::atomic<double> lastTargetVelocity;
+ bool isLimitless;
+
+ ActorDataPtr dataPtr;
+ KITGripperBasisBoardPtr board;
+ const std::string deviceName;
+ size_t actorIndex = 0;
+ mutable RemoteGuiInterfacePrx remoteGui;
+ bool stopRequested = false;
+ mutable ThreadPool::Handle threadHandle;
+ // JointController interface
+ protected:
+ void rtPostDeactivateController() override;
+
+ // JointController interface
+ public:
+ StringVariantBaseMap publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
new file mode 100644
index 000000000..856c244cf
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
@@ -0,0 +1,246 @@
+#include <chrono>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "JointPWMVelocityController.h"
+#include "../KITGripperBasisBoard.h"
+#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+
+#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
+#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
+
+#include <ArmarXCore/core/application/Application.h>
+
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+
+using namespace armarx;
+
+
+JointPWMVelocityController::JointPWMVelocityController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData,
+ PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) : JointController(),
+ config(velocityControllerConfigDataPtr),
+ controller(velocityControllerConfigDataPtr),
+ target(), board(board), deviceName(deviceName)
+{
+ actorIndex = board->getActorIndex(deviceName);
+ dataPtr = jointData;
+
+ velController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
+ velController.deceleration = velocityControllerConfigDataPtr->maxDecelerationRad;
+ velController.maxDt = velocityControllerConfigDataPtr->maxDt;
+ velController.maxV = velocityControllerConfigDataPtr->maxVelocityRad;
+ velController.directSetVLimit = velocityControllerConfigDataPtr->directSetVLimit;
+ ARMARX_CHECK_GREATER_EQUAL(jointData->getSoftLimitHi(), jointData->getSoftLimitLo());
+ // controller.positionLimitHiHard = dataPtr->getHardLimitHi();
+ velController.positionLimitHiSoft = jointData->getSoftLimitHi();
+ // controller.positionLimitLoHard = dataPtr->getHardLimitLo();
+ velController.positionLimitLoSoft = jointData->getSoftLimitLo();
+ this->isLimitless = jointData->isLimitless();
+}
+
+JointPWMVelocityController::~JointPWMVelocityController() noexcept(true)
+{
+ stopRequested = true;
+ try
+ {
+ threadHandle.join();
+ }
+ catch (...)
+ {
+
+ }
+}
+
+void JointPWMVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+{
+ if (target.isValid())
+ {
+
+ {
+ auto currentPosition = dataPtr->getPosition();
+ if (isLimitless)
+ {
+ velController.currentPosition = velController.positionLimitHiSoft - (velController.positionLimitHiSoft - velController.positionLimitLoSoft) * 0.5;
+ // ARMARX_INFO << VAROUT(velController.currentPosition) << VAROUT(velController.positionLimitLoSoft) << VAROUT(velController.positionLimitHiSoft);
+ }
+ else
+ {
+ velController.currentPosition = currentPosition;
+ }
+ velController.currentV = lastTargetVelocity;
+ velController.dt = timeSinceLastIteration.toSecondsDouble();
+ velController.targetV = target.velocity;
+ double newVel = velController.run();
+
+
+ // ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel) << VAROUT(target.velocity);
+ if (std::isnan(newVel))
+ {
+ newVel = 0;
+ }
+ // float newVel = target.velocity;
+ if ((currentPosition > velController.positionLimitHiSoft && target.velocity > 0)
+ || (currentPosition < velController.positionLimitLoSoft && target.velocity < 0))
+ {
+ newVel = 0;
+ ARMARX_INFO << deactivateSpam(1) << "Breaking now at " << dataPtr->getPosition() << " pwm: " << dataPtr->getTargetPWM();
+ }
+
+
+ auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel));
+
+ dataPtr->setTargetPWM(targetPWM);
+
+ lastTargetVelocity = newVel;
+
+ // ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
+ // target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
+
+ }
+ }
+ else
+ {
+ ARMARX_ERROR << "invalid target set for actor";
+ }
+}
+
+ControlTargetBase* JointPWMVelocityController::getControlTarget()
+{
+ return ⌖
+}
+
+void JointPWMVelocityController::rtPreActivateController()
+{
+ lastTargetVelocity = dataPtr->getVelocity();
+ controller.reset(dataPtr->getVelocity());
+}
+
+void JointPWMVelocityController::rtPostDeactivateController()
+{
+ ARMARX_RT_LOGF_INFO("Setting PWM to 0");
+ dataPtr->setTargetPWM(0);
+}
+
+StringVariantBaseMap JointPWMVelocityController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
+{
+
+ if (!remoteGui && !threadHandle.isValid())
+ {
+ // threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
+ // {
+ // std::string guiTabName;
+ // while (!stopRequested)
+ // {
+ // ManagedIceObjectPtr object;
+ // ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
+ // try
+ // {
+ // object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
+ // ARMARX_CHECK_EXPRESSION(object);
+ // remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
+ // if (!remoteGui)
+ // {
+ // return;
+ // }
+ // ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
+ // guiTabName = getParent().getDeviceName() + getControlMode();
+ // break;
+ // }
+ // catch (...)
+ // {
+ // handleExceptions();
+ // sleep(1);
+ // }
+
+ // }
+ // if (remoteGui)
+ // {
+ // ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
+ // using namespace RemoteGui;
+
+
+
+ // auto vLayout = makeVBoxLayout();
+
+ // {
+ // WidgetPtr KpLabel = makeTextLabel("Kp: ");
+
+ // WidgetPtr KiSlider = makeFloatSlider("KpSlider")
+ // .min(0.0f).max(5000.0f)
+ // .value(config->p);
+ // WidgetPtr line = makeHBoxLayout()
+ // .children({KpLabel, KiSlider});
+
+ // vLayout.addChild(line);
+
+ // }
+
+
+ // {
+ // WidgetPtr KiLabel = makeTextLabel("Ki: ");
+ // WidgetPtr KiSlider = makeFloatSlider("KiSlider")
+ // .min(0.0f).max(50000.0f)
+ // .value(config->i);
+
+ // WidgetPtr line = makeHBoxLayout()
+ // .children({KiLabel, KiSlider});
+
+ // vLayout.addChild(line);
+
+ // }
+
+ // {
+ // WidgetPtr KdLabel = makeTextLabel("Kd: ");
+ // WidgetPtr KdSlider = makeFloatSlider("KdSlider")
+ // .min(0.0f).max(50.0f)
+ // .steps(100)
+ // .value(config->d);
+
+ // WidgetPtr line = makeHBoxLayout()
+ // .children({KdLabel, KdSlider});
+
+ // vLayout.addChild(line);
+ // vLayout.addChild(new VSpacer);
+ // }
+
+ // // WidgetPtr spin = makeFloatSpinBox("KpSpin")
+ // // .min(0.0f).max(2.0f)
+ // // .steps(20).decimals(2)
+ // // .value(0.4f);
+
+
+
+
+ // WidgetPtr groupBox = makeGroupBox("GroupBox")
+ // .label("Group")
+ // .child(vLayout);
+
+ // remoteGui->createTab(guiTabName, groupBox);
+
+ // while (!stopRequested)
+ // {
+ // RemoteGui::TabProxy tab(remoteGui, guiTabName);
+ // tab.receiveUpdates();
+ // this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
+ // this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
+ // this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
+ // usleep(100000);
+ // }
+ // }
+
+ // });
+ }
+ return {{"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
+ {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
+ {"pidIntegralCV", new Variant(controller.pid->integral * controller.pid->Ki)},
+ {"pidPropCV", new Variant(controller.pid->previousError * controller.pid->Kp)},
+ {"pidDiffCV", new Variant(controller.pid->derivative * controller.pid->Kd)}
+
+ };
+}
+
+
+
+
+
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.h
new file mode 100644
index 000000000..34312460d
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include <memory>
+#include <chrono>
+
+#include <RobotAPI/libraries/core/PIDController.h>
+
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+#include "../KITGripperBasisBoardData.h"
+#include <ArmarXGui/interface/RemoteGuiInterface.h>
+#include <ArmarXCore/core/services/tasks/ThreadPool.h>
+
+#include <ArmarXCore/observers/filters/AverageFilter.h>
+#include "PWMVelocityController.h"
+
+
+namespace armarx
+{
+ using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
+
+
+ class JointPWMVelocityController;
+ typedef std::shared_ptr<JointPWMVelocityController> JointPWMVelocityControllerPtr;
+
+ class JointPWMVelocityController : public JointController
+ {
+ public:
+ JointPWMVelocityController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
+ ~JointPWMVelocityController() noexcept(true);
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
+ ControlTargetBase* getControlTarget() override;
+
+ void rtPreActivateController() override;
+ protected:
+ PWMVelocityControllerConfigurationCPtr config;
+ PWMVelocityController controller;
+ VelocityControllerWithAccelerationAndPositionBounds velController;
+
+ ControlTarget1DoFActuatorVelocity target;
+
+ std::atomic<double> lastTargetVelocity;
+ bool isLimitless;
+
+ ActorDataPtr dataPtr;
+ KITGripperBasisBoardPtr board;
+ const std::string deviceName;
+ size_t actorIndex = 0;
+ mutable RemoteGuiInterfacePrx remoteGui;
+ bool stopRequested = false;
+ mutable ThreadPool::Handle threadHandle;
+ // JointController interface
+ protected:
+ void rtPostDeactivateController() override;
+
+ // JointController interface
+ public:
+ StringVariantBaseMap publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp
new file mode 100644
index 000000000..c55b17723
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp
@@ -0,0 +1,218 @@
+#include <chrono>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "JointZeroTorqueController.h"
+#include "../KITGripperBasisBoard.h"
+#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+
+#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
+#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
+
+#include <ArmarXCore/core/application/Application.h>
+
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+
+using namespace armarx;
+
+
+PWMZeroTorqueControllerConfigurationCPtr PWMZeroTorqueControllerConfiguration::CreateConfigDataFromXml(DefaultRapidXmlReaderNode node)
+{
+ PWMZeroTorqueControllerConfiguration configData;
+
+ configData.feedforwardVelocityToPWMFactor = node.first_node("feedforwardVelocityToPWMFactor").value_as_float();
+ configData.PWMDeadzone = node.first_node("PWMDeadzone").value_as_float();
+
+
+ return std::make_shared<PWMZeroTorqueControllerConfiguration>(configData);
+
+}
+
+JointPWMZeroTorqueController::JointPWMZeroTorqueController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData,
+ PWMZeroTorqueControllerConfigurationCPtr config) : JointController(),
+ config(config), target(), board(board), deviceName(deviceName)
+{
+ actorIndex = board->getActorIndex(deviceName);
+ dataPtr = jointData;
+
+
+ this->isLimitless = jointData->isLimitless();
+
+}
+
+JointPWMZeroTorqueController::~JointPWMZeroTorqueController() noexcept(true)
+{
+ stopRequested = true;
+ try
+ {
+ threadHandle.join();
+ }
+ catch (...)
+ {
+
+ }
+}
+
+void JointPWMZeroTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+{
+ if (target.isValid())
+ {
+ float targetPWM = dataPtr->getVelocity() * config->feedforwardVelocityToPWMFactor;
+ targetPWM += math::MathUtils::Sign(dataPtr->getVelocity()) * config->PWMDeadzone;
+ // targetPWM = math::MathUtils::LimitTo(targetPWM, 1500);
+ dataPtr->setTargetPWM(targetPWM);
+
+ // ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
+ // target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
+
+
+ }
+ else
+ {
+ ARMARX_ERROR << "invalid target set for actor";
+ }
+}
+
+ControlTargetBase* JointPWMZeroTorqueController::getControlTarget()
+{
+ return ⌖
+}
+
+void JointPWMZeroTorqueController::rtPreActivateController()
+{
+ lastTargetVelocity = dataPtr->getVelocity();
+ // controller.reset(dataPtr->getVelocity());
+}
+
+void JointPWMZeroTorqueController::rtPostDeactivateController()
+{
+ ARMARX_RT_LOGF_INFO("Setting PWM to 0");
+ dataPtr->setTargetPWM(0);
+}
+
+StringVariantBaseMap JointPWMZeroTorqueController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
+{
+
+ if (!remoteGui && !threadHandle.isValid())
+ {
+ threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
+ {
+ return;
+ // std::string guiTabName;
+ // while (!stopRequested)
+ // {
+ // ManagedIceObjectPtr object;
+ // ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
+ // try
+ // {
+ // object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
+ // ARMARX_CHECK_EXPRESSION(object);
+ // remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
+ // if (!remoteGui)
+ // {
+ // return;
+ // }
+ // ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
+ // guiTabName = getParent().getDeviceName() + getControlMode();
+ // break;
+ // }
+ // catch (...)
+ // {
+ // handleExceptions();
+ // sleep(1);
+ // }
+
+ // }
+ // if (remoteGui)
+ // {
+ // ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
+ // using namespace RemoteGui;
+
+
+
+ // // auto vLayout = makeVBoxLayout();
+
+ // // {
+ // // WidgetPtr KpLabel = makeTextLabel("Kp: ");
+
+ // // WidgetPtr KiSlider = makeFloatSlider("KpSlider")
+ // // .min(0.0f).max(5000.0f)
+ // // .value(config->p);
+ // // WidgetPtr line = makeHBoxLayout()
+ // // .children({KpLabel, KiSlider});
+
+ // // vLayout.addChild(line);
+
+ // // }
+
+
+ // // {
+ // // WidgetPtr KiLabel = makeTextLabel("Ki: ");
+ // // WidgetPtr KiSlider = makeFloatSlider("KiSlider")
+ // // .min(0.0f).max(50000.0f)
+ // // .value(config->i);
+
+ // // WidgetPtr line = makeHBoxLayout()
+ // // .children({KiLabel, KiSlider});
+
+ // // vLayout.addChild(line);
+
+ // // }
+
+ // // {
+ // // WidgetPtr KdLabel = makeTextLabel("Kd: ");
+ // // WidgetPtr KdSlider = makeFloatSlider("KdSlider")
+ // // .min(0.0f).max(50.0f)
+ // // .steps(100)
+ // // .value(config->d);
+
+ // // WidgetPtr line = makeHBoxLayout()
+ // // .children({KdLabel, KdSlider});
+
+ // // vLayout.addChild(line);
+ // // vLayout.addChild(new VSpacer);
+ // // }
+
+ // // WidgetPtr spin = makeFloatSpinBox("KpSpin")
+ // // .min(0.0f).max(2.0f)
+ // // .steps(20).decimals(2)
+ // // .value(0.4f);
+
+
+
+
+ // WidgetPtr groupBox = makeGroupBox("GroupBox")
+ // .label("Group")
+ // .child(vLayout);
+
+ // remoteGui->createTab(guiTabName, groupBox);
+
+ // while (!stopRequested)
+ // {
+ // RemoteGui::TabProxy tab(remoteGui, guiTabName);
+ // tab.receiveUpdates();
+ // this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
+ // this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
+ // this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
+ // usleep(100000);
+ // }
+ // }
+
+ });
+ }
+ return {};
+ // return {{"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
+ // {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
+ // {"pidIntegralCV", new Variant(controller.pid->integral * controller.pid->Ki)},
+ // {"pidPropCV", new Variant(controller.pid->previousError * controller.pid->Kp)},
+ // {"pidDiffCV", new Variant(controller.pid->derivative * controller.pid->Kd)}
+}
+
+
+
+
+
+
+
+
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.h
new file mode 100644
index 000000000..0d71e5971
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.h
@@ -0,0 +1,73 @@
+#pragma once
+
+#include <memory>
+#include <chrono>
+
+#include <RobotAPI/libraries/core/PIDController.h>
+
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+#include "../KITGripperBasisBoardData.h"
+#include <ArmarXGui/interface/RemoteGuiInterface.h>
+#include <ArmarXCore/core/services/tasks/ThreadPool.h>
+
+#include <ArmarXCore/observers/filters/AverageFilter.h>
+#include "PWMVelocityController.h"
+
+
+namespace armarx
+{
+ using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
+
+
+ typedef std::shared_ptr<class PWMZeroTorqueControllerConfiguration> PWMZeroTorqueControllerConfigurationPtr;
+ typedef std::shared_ptr<const PWMZeroTorqueControllerConfiguration> PWMZeroTorqueControllerConfigurationCPtr;
+
+ class PWMZeroTorqueControllerConfiguration
+ {
+ public:
+ PWMZeroTorqueControllerConfiguration() {}
+ static PWMZeroTorqueControllerConfigurationCPtr CreateConfigDataFromXml(DefaultRapidXmlReaderNode node);
+ float feedforwardVelocityToPWMFactor;
+ float PWMDeadzone;
+ };
+
+
+
+ class JointPWMZeroTorqueController;
+ typedef std::shared_ptr<JointPWMZeroTorqueController> JointPWMZeroTorqueControllerPtr;
+
+ class JointPWMZeroTorqueController : public JointController
+ {
+ public:
+ JointPWMZeroTorqueController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMZeroTorqueControllerConfigurationCPtr config);
+ ~JointPWMZeroTorqueController() noexcept(true);
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
+ ControlTargetBase* getControlTarget() override;
+
+ void rtPreActivateController() override;
+ private:
+ PWMZeroTorqueControllerConfigurationCPtr config;
+ ControlTarget1DoFActuatorZeroTorque target;
+
+ std::atomic<double> lastTargetVelocity;
+ bool isLimitless;
+
+ ActorDataPtr dataPtr;
+ KITGripperBasisBoardPtr board;
+ const std::string deviceName;
+ size_t actorIndex = 0;
+ mutable RemoteGuiInterfacePrx remoteGui;
+ bool stopRequested = false;
+ mutable ThreadPool::Handle threadHandle;
+ // JointController interface
+ protected:
+ void rtPostDeactivateController() override;
+
+ // JointController interface
+ public:
+ StringVariantBaseMap publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp
new file mode 100644
index 000000000..544f76a1c
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp
@@ -0,0 +1,102 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, 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/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "PWMVelocityController.h"
+
+namespace armarx
+{
+
+ PWMVelocityController::PWMVelocityController(PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) :
+ config(velocityControllerConfigDataPtr)
+ {
+
+
+
+ pid.reset(new PIDController(velocityControllerConfigDataPtr->p,
+ velocityControllerConfigDataPtr->i,
+ velocityControllerConfigDataPtr->d));
+ pid->maxIntegral = velocityControllerConfigDataPtr->maxIntegral;
+ pid->conditionalIntegralErrorTreshold = velocityControllerConfigDataPtr->conditionalIntegralErrorTreshold;
+ pid->threadSafe = false;
+ }
+
+ double PWMVelocityController::run(IceUtil::Time const& deltaT, double currentVelocity, double targetVelocity)
+ {
+ double targetPWM = 0;
+ if (!this->config->feedForwardMode)
+ {
+ lastActualVelocity = lastActualVelocity * (1.0 - config->velocityUpdatePercent) + currentVelocity * config->velocityUpdatePercent;
+ pid->update(deltaT.toSecondsDouble(), lastActualVelocity, targetVelocity);
+ targetPWM = pid->getControlValue();
+ }
+ // TODO: add feedforward term based on gravity
+
+
+ //feed forward
+ if (std::abs(targetVelocity) > 0.001)
+ {
+ // targetPWM += config->PWMDeadzone * math::MathUtils::Sign(targetVelocity); // deadzone
+ }
+ targetPWM += config->feedforwardVelocityToPWMFactor * targetVelocity; // approx. feedforward vel
+
+
+
+
+ // ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
+ // target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
+
+
+
+ return targetPWM;
+ }
+
+ void PWMVelocityController::reset(double currentVelocity)
+ {
+ lastActualVelocity = currentVelocity;
+ pid->reset();
+ }
+
+ PWMVelocityControllerConfigurationCPtr PWMVelocityControllerConfiguration::CreatePWMVelocityControllerConfigDataFromXml(DefaultRapidXmlReaderNode node)
+ {
+ PWMVelocityControllerConfiguration configData;
+
+ configData.maxVelocityRad = node.first_node("maxVelocityRad").value_as_float();
+ configData.maxAccelerationRad = node.first_node("maxAccelerationRad").value_as_float();
+ configData.maxDecelerationRad = node.first_node("maxDecelerationRad").value_as_float();
+ configData.maxDt = node.first_node("maxDt").value_as_float();
+ configData.maxDt = node.first_node("directSetVLimit").value_as_float();
+ configData.p = node.first_node("p").value_as_float();
+ configData.i = node.first_node("i").value_as_float();
+ configData.d = node.first_node("d").value_as_float();
+ configData.maxIntegral = node.first_node("maxIntegral").value_as_float();
+ configData.feedforwardVelocityToPWMFactor = node.first_node("feedforwardVelocityToPWMFactor").value_as_float();
+ configData.PWMDeadzone = node.first_node("PWMDeadzone").value_as_float();
+ configData.velocityUpdatePercent = node.first_node("velocityUpdatePercent").value_as_float();
+ configData.conditionalIntegralErrorTreshold = node.first_node("conditionalIntegralErrorTreshold").value_as_float();
+ configData.feedForwardMode = node.first_node("FeedForwardMode").value_as_bool("1", "0");
+
+ return std::make_shared<PWMVelocityControllerConfiguration>(configData);
+
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h
new file mode 100644
index 000000000..47bc0c2e9
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h
@@ -0,0 +1,77 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, 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/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+#include <atomic>
+#include <RobotAPI/libraries/core/PIDController.h>
+
+
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+
+#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
+
+#include "PWMVelocityController.h"
+
+namespace armarx
+{
+
+ typedef std::shared_ptr<class PWMVelocityControllerConfiguration> PWMVelocityControllerConfigurationPtr;
+ typedef std::shared_ptr<const PWMVelocityControllerConfiguration> PWMVelocityControllerConfigurationCPtr;
+
+ class PWMVelocityControllerConfiguration
+ {
+ public:
+ PWMVelocityControllerConfiguration() {}
+ static PWMVelocityControllerConfigurationCPtr CreatePWMVelocityControllerConfigDataFromXml(DefaultRapidXmlReaderNode node);
+ float maxVelocityRad;
+ float maxAccelerationRad;
+ float maxDecelerationRad;
+ float maxDt;
+ float directSetVLimit;
+ float p;
+ float i;
+ float d;
+ float maxIntegral;
+ float feedforwardVelocityToPWMFactor;
+ float PWMDeadzone;
+ float velocityUpdatePercent;
+ float conditionalIntegralErrorTreshold;
+ bool feedForwardMode;
+ };
+
+ class PWMVelocityController
+ {
+ public:
+ PWMVelocityController(PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
+ double run(IceUtil::Time const& deltaT, double currentVelocity, double targetVelocity);
+ void reset(double currentVelocity);
+
+ PWMVelocityControllerConfigurationCPtr config;
+
+ PIDControllerPtr pid;
+ std::atomic<double> lastActualVelocity;
+
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
new file mode 100644
index 000000000..3dcc0692d
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
@@ -0,0 +1,66 @@
+#include <chrono>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "ParallelGripperPositionController.h"
+#include "../KITGripperBasisBoard.h"
+#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+
+#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
+#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
+
+#include <ArmarXCore/core/application/Application.h>
+
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+
+using namespace armarx;
+
+
+ParallelGripperPositionController::ParallelGripperPositionController(const std::string deviceName, KITGripperBasisBoardPtr board,
+ ActorDataPtr jointData,
+ PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) :
+ JointPWMPositionController(deviceName, board, jointData, velocityControllerConfigDataPtr)
+{
+ linkedJointConnectorIndex = jointData->getParallelControlActorIndex();
+}
+
+ParallelGripperPositionController::~ParallelGripperPositionController() noexcept(true)
+{
+
+}
+
+void ParallelGripperPositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+{
+ if (target.isValid())
+ {
+ float linkedPositionFactor = 2.0 / 3.0;
+ 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
+ {
+ ARMARX_ERROR << deactivateSpam(1) << "invalid target set for actor " << getParent().getDeviceName();
+ }
+}
+
+void ParallelGripperPositionController::rtPreActivateController()
+{
+ linkedDataPtr = board->getDevices().at(linkedJointConnectorIndex)->getActorDataPtr();
+ ARMARX_CHECK_EXPRESSION_W_HINT(linkedDataPtr, "index: " << linkedJointConnectorIndex);
+ JointPWMPositionController::rtPreActivateController();
+}
+
+
+
+
+
+
+
+
+
+
+
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.h
new file mode 100644
index 000000000..ab776fa91
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.h
@@ -0,0 +1,45 @@
+#pragma once
+
+#include <memory>
+#include <chrono>
+
+#include <RobotAPI/libraries/core/PIDController.h>
+
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+#include "../KITGripperBasisBoardData.h"
+#include <ArmarXGui/interface/RemoteGuiInterface.h>
+#include <ArmarXCore/core/services/tasks/ThreadPool.h>
+
+#include <ArmarXCore/observers/filters/AverageFilter.h>
+#include "JointPWMPositionController.h"
+#include "PWMVelocityController.h"
+
+
+namespace armarx
+{
+ using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
+
+
+ class ParallelGripperPositionController;
+ typedef std::shared_ptr<ParallelGripperPositionController> ParallelGripperPositionControllerPtr;
+
+ class ParallelGripperPositionController : public JointPWMPositionController
+ {
+ public:
+ ParallelGripperPositionController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
+ ~ParallelGripperPositionController() noexcept(true);
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
+ private:
+ uint32_t linkedJointConnectorIndex = -1;
+ ActorDataPtr linkedDataPtr;
+
+
+
+ // JointController interface
+ protected:
+ void rtPreActivateController() override;
+ };
+}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp
new file mode 100644
index 000000000..c04ca78ab
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp
@@ -0,0 +1,61 @@
+#include <chrono>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "ParallelGripperVelocityController.h"
+#include "../KITGripperBasisBoard.h"
+#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+
+#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
+#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
+
+#include <ArmarXCore/core/application/Application.h>
+
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+
+using namespace armarx;
+
+
+ParallelGripperVelocityController::ParallelGripperVelocityController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData,
+ PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) :
+ JointPWMVelocityController(deviceName, board, jointData, velocityControllerConfigDataPtr)
+{
+ this->linkedJointConnectorIndex = jointData->getParallelControlActorIndex();
+
+}
+
+ParallelGripperVelocityController::~ParallelGripperVelocityController() noexcept(true)
+{
+
+}
+
+void ParallelGripperVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+{
+ if (target.isValid())
+ {
+ float linkedVelocityFactor = 2.0f / 3.0f;
+ target.velocity += linkedVelocityFactor * linkedDataPtr->getVelocity();
+ JointPWMVelocityController::rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+ }
+ else
+ {
+ ARMARX_ERROR << "invalid target set for actor";
+ }
+}
+
+
+void ParallelGripperVelocityController::rtPreActivateController()
+{
+ linkedDataPtr = board->getDevices().at(linkedJointConnectorIndex)->getActorDataPtr();
+ ARMARX_CHECK_EXPRESSION_W_HINT(linkedDataPtr, "index: " << linkedJointConnectorIndex);
+ JointPWMVelocityController::rtPreActivateController();
+}
+
+
+
+
+
+
+
+
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.h
new file mode 100644
index 000000000..9ea5eca89
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.h
@@ -0,0 +1,44 @@
+#pragma once
+
+#include <memory>
+#include <chrono>
+
+#include <RobotAPI/libraries/core/PIDController.h>
+
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+#include "../KITGripperBasisBoardData.h"
+#include <ArmarXGui/interface/RemoteGuiInterface.h>
+#include <ArmarXCore/core/services/tasks/ThreadPool.h>
+
+#include <ArmarXCore/observers/filters/AverageFilter.h>
+#include "JointPWMVelocityController.h"
+#include "PWMVelocityController.h"
+
+
+namespace armarx
+{
+ using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
+
+
+ class ParallelGripperVelocityController;
+ typedef std::shared_ptr<ParallelGripperVelocityController> ParallelGripperVelocityControllerPtr;
+
+ class ParallelGripperVelocityController : public JointPWMVelocityController
+ {
+ public:
+ ParallelGripperVelocityController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
+ ~ParallelGripperVelocityController() noexcept(true);
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
+
+ void rtPreActivateController() override;
+ private:
+
+ uint32_t linkedJointConnectorIndex = -1;
+
+ ActorDataPtr linkedDataPtr;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
new file mode 100644
index 000000000..d14e66c6a
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
@@ -0,0 +1,259 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, 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/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "KITGripperBasisBoard.h"
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "JointController/JointPWMPositionController.h"
+#include "JointController/JointZeroTorqueController.h"
+#include "JointController/JointKITGripperEmergencyStopController.h"
+#include "JointController/JointKITGripperStopMovementController.h"
+#include "JointController/JointPWMVelocityController.h"
+#include "JointController/ParallelGripperPositionController.h"
+#include "JointController/ParallelGripperVelocityController.h"
+#include "Misc/TorqueEstimation.h"
+namespace armarx
+{
+ VirtualDeviceFactory::SubClassRegistry KITGripperBasisBoard::registry("KITGripperBasisBoard", &VirtualDeviceFactory::createInstance<KITGripperBasisBoard>);
+
+ KITGripperBasisBoard::KITGripperBasisBoard(RapidXmlReaderNode node, DefaultRapidXmlReaderNode defaultConfigurationNode, const VirtualRobot::RobotPtr& robot) :
+ DeviceBase(node.attribute_value("name")),
+ SensorDevice(node.attribute_value("name")),
+ // ControlDevice(node.attribute_value("name")),
+ AbstractFunctionalDevice(defaultConfigurationNode.first_node("KITGripperBasisBoardDefaultConfiguration")
+ .add_node_at_end(node)),
+ configNode(node),
+ defaultConfigurationNode(defaultConfigurationNode),
+ robot(robot),
+ slaveIdentifier(node.first_node("Identifier"))
+ {
+ slaveIdentifier.humanName = node.attribute_value("name");
+ ARMARX_VERBOSE << "found " << configNode.nodes("Actor").size() << " actors";
+ for (auto motorNode : configNode.nodes("Actor"))
+ {
+ auto connectorIndex = motorNode.attribute_as_uint("connector");
+ auto name = motorNode.attribute_value("name");
+ auto enabled = motorNode.attribute_as_bool("enabled", "true", "false");
+
+ if (enabled)
+ {
+ ARMARX_VERBOSE << "Found motor configuration for connector index " << connectorIndex;
+ // auto actorData = dataPtr->getActorData(connectorIndex);
+ // ARMARX_CHECK_EXPRESSION_W_HINT(actorData, name);
+ auto robotNode = robot->getRobotNode(name);
+ ARMARX_CHECK_EXPRESSION_W_HINT(robotNode, name);
+ ARMARX_INFO << "Creating actor class for " << name;
+ KITGripperBasisBoard::ActorRobotUnitDevicePtr ptr = std::make_shared<KITGripperBasisBoard::ActorRobotUnitDevicePtr::element_type>(connectorIndex, name, robotNode);
+ devices.push_back(ptr);
+
+ }
+ else
+ {
+ ARMARX_INFO << "motor at Index " << connectorIndex << " disabled";
+ }
+ }
+ }
+
+ void KITGripperBasisBoard::init(KITGripperBasisBoardSlavePtr slave)
+ {
+ this->slave = slave;
+ initialized = true;
+
+ }
+
+ void KITGripperBasisBoard::initData()
+ {
+ dataPtr = std::make_shared<KITGripperBasisBoardData>(configNode, defaultConfigurationNode,
+ slave->getOutputs(), slave->getInputs(), robot);
+
+ for (auto motorNode : configNode.nodes("Actor"))
+ {
+ auto enabled = motorNode.attribute_as_bool("enabled", "true", "false");
+ auto name = motorNode.attribute_value("name");
+ if (enabled)
+ {
+ auto i = getActorIndex(name);
+ devices.at(i)->init(std::dynamic_pointer_cast<KITGripperBasisBoard>(shared_from_this()), dataPtr,
+ motorNode, defaultConfigurationNode);
+ }
+
+ }
+
+ }
+
+ const SensorValueBase* KITGripperBasisBoard::getSensorValue() const
+ {
+ return &sensorValue;
+ }
+
+ const SlaveIdentifier& KITGripperBasisBoard::getSlaveIdentifier() const
+ {
+ return slaveIdentifier;
+ }
+
+ void KITGripperBasisBoard::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ dataPtr->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration);
+
+ // TODO: read IMU
+ }
+
+ // void KITGripperBasisBoard::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ // {
+ // // TODO: write LED targets
+ // }
+
+ const std::vector<KITGripperBasisBoard::ActorRobotUnitDevicePtr>& KITGripperBasisBoard::getDevices() const
+ {
+ return devices;
+ }
+
+ size_t KITGripperBasisBoard::getActorIndex(const std::string& actorName)
+ {
+ size_t i = 0;
+ for (auto& actor : devices)
+ {
+ if (actor->getRobotNode()->getName() == actorName)
+ {
+ return i;
+ }
+ i++;
+ }
+ throw LocalException() << "Could not find actor with name: " << actorName << "\nactors:\n" << ARMARX_STREAM_PRINTER { for (auto& actor : devices)
+ {
+ out << actor->getDeviceName();
+ }
+ };
+ }
+
+ KITGripperBasisBoard::ActorRobotUnitDevice::ActorRobotUnitDevice(size_t connectorIndex, const std::string& deviceName, VirtualRobot::RobotNodePtr robotNode) :
+ DeviceBase(deviceName),
+ ControlDevice(deviceName),
+ SensorDevice(deviceName),
+ actorIndex(connectorIndex)
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(robotNode, deviceName);
+ this->robotNode = robotNode;
+ ARMARX_INFO << deviceName << " actor created";
+ }
+
+ void KITGripperBasisBoard::ActorRobotUnitDevice::init(KITGripperBasisBoardPtr dev, KITGripperBasisBoardDataPtr dataPtr,
+ RapidXmlReaderNode configNode, DefaultRapidXmlReaderNode defaultConfigurationNode)
+ {
+ this->board = dev;
+ this->actorDataPtr = dataPtr->getActorData(actorIndex);
+ emergencyController.reset(new JointKITGripperEmergencyStopController(actorDataPtr));
+ addJointController(emergencyController.get());
+ stopMovementController.reset(new JointKITGripperStopMovementController(actorDataPtr));
+ addJointController(stopMovementController.get());
+ auto velocityControllerCfg = PWMVelocityControllerConfiguration::CreatePWMVelocityControllerConfigDataFromXml(
+ defaultConfigurationNode.first_node("JointPWMVelocityControllerDefaultConfiguration")
+ .add_node_at_end(configNode.first_node("JointPWMVelocityControllerConfig")));
+ auto zeroTorqueControllerCfg = PWMZeroTorqueControllerConfiguration::CreateConfigDataFromXml(
+ defaultConfigurationNode.first_node("JointPWMZeroTorqueControllerDefaultConfiguration")
+ .add_node_at_end(configNode.first_node("JointPWMZeroTorqueControllerConfig")));
+
+ if (actorDataPtr->getParallelControlActorIndex() != -1)
+ {
+ // Add Controllers for ParallelGripper
+ if (actorDataPtr->getVelocityControlEnabled())
+ {
+ velocityController.reset(new ParallelGripperVelocityController(getDeviceName(), dev, actorDataPtr, velocityControllerCfg));
+ addJointController(velocityController.get());
+ }
+ else
+ {
+ ARMARX_VERBOSE << "Velocity Control disabled for " << getDeviceName();
+ }
+ if (actorDataPtr->getPositionControlEnabled())
+ {
+ positionController.reset(new ParallelGripperPositionController(getDeviceName(), dev, actorDataPtr, velocityControllerCfg));
+ addJointController(positionController.get());
+ }
+ else
+ {
+ ARMARX_VERBOSE << "Position Control disabled for " << getDeviceName();
+ }
+ // //TODO: Does PG get zero torque controller? If so, a special one?
+ // zeroTorqueController.reset(new JointPWMZeroTorqueController(getDeviceName(), dev, actorPtr, zeroTorqueControllerCfg));
+ // addJointController(zeroTorqueController.get());
+ return;
+ }
+ else
+ {
+ if (actorDataPtr->getVelocityControlEnabled())
+ {
+ velocityController.reset(new JointPWMVelocityController(getDeviceName(), dev, actorDataPtr, velocityControllerCfg));
+ addJointController(velocityController.get());
+ }
+ else
+ {
+ ARMARX_VERBOSE << "Velocity Control disabled for " << getDeviceName();
+ }
+ zeroTorqueController.reset(new JointPWMZeroTorqueController(getDeviceName(), dev, actorDataPtr, zeroTorqueControllerCfg));
+ addJointController(zeroTorqueController.get());
+ if (actorDataPtr->getPositionControlEnabled())
+ {
+ positionController.reset(new JointPWMPositionController(getDeviceName(), dev, actorDataPtr, velocityControllerCfg));
+ addJointController(positionController.get());
+ }
+ else
+ {
+ ARMARX_VERBOSE << "Position Control disabled for " << getDeviceName();
+ }
+ }
+ }
+
+ ActorDataPtr KITGripperBasisBoard::ActorRobotUnitDevice::getActorDataPtr() const
+ {
+ return actorDataPtr;
+ }
+
+ const VirtualRobot::RobotNodePtr& KITGripperBasisBoard::ActorRobotUnitDevice::getRobotNode() const
+ {
+ return robotNode;
+ }
+
+ void KITGripperBasisBoard::ActorRobotUnitDevice::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ sensorValue.position = actorDataPtr->getPosition();
+ sensorValue.relativePosition = actorDataPtr->getRelativePosition();
+ sensorValue.velocity = actorDataPtr->getVelocity();
+ sensorValue.targetPWM = actorDataPtr->getTargetPWM();
+ sensorValue.motorCurrent = actorDataPtr->getTargetPWM();
+ sensorValue.minPWM = actorDataPtr->getCurrentMinPWM();
+ sensorValue.maxPWM = actorDataPtr->getCurrentMaxPWM();
+ sensorValue.velocityTicksPerMs = actorDataPtr->getVelocityTicks();
+ sensorValue.torque = estimateTorque(sensorValue.velocityTicksPerMs, sensorValue.targetPWM);
+ }
+
+ void KITGripperBasisBoard::ActorRobotUnitDevice::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+
+ }
+
+} // namespace
+
+
+
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h
new file mode 100644
index 000000000..7e9b32ed5
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h
@@ -0,0 +1,137 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, 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/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include "KITGripperBasisBoardData.h"
+#include "KITGripperBasisBoardSlave.h"
+
+#include <RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h>
+#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
+
+#include <RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h>
+#include <RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.h>
+
+namespace armarx
+{
+ using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
+ using JointKITGripperStopMovementControllerPtr = std::shared_ptr<class JointKITGripperStopMovementController>;
+ using JointKITGripperEmergencyStopControllerPtr = std::shared_ptr<class JointKITGripperEmergencyStopController>;
+ using JointPWMVelocityControllerPtr = std::shared_ptr<class JointPWMVelocityController>;
+ using JointPWMPositionControllerPtr = std::shared_ptr<class JointPWMPositionController>;
+ using JointPWMZeroTorqueControllerPtr = std::shared_ptr<class JointPWMZeroTorqueController>;
+ using ParallelGripperPositionControllerPtr = std::shared_ptr<class ParallelGripperPositionController>;
+ using ParallelGripperVelocityControllerPtr = std::shared_ptr<class ParallelGripperVelocityController>;
+ using PWMVelocityControllerConfigurationPtr = std::shared_ptr<class PWMVelocityControllerConfiguration>;
+
+
+ class KITGripperBasisBoard :
+ public SensorDevice,
+ // public ControlDevice,
+ public AbstractFunctionalDevice
+ {
+ static VirtualDeviceFactory::SubClassRegistry registry;
+ public:
+
+ class ActorRobotUnitDevice :
+ public ControlDevice,
+ public SensorDevice
+ {
+ friend class KITGripperBasisBoard;
+
+ // SensorDevice interface
+ public:
+ ActorRobotUnitDevice(size_t connectorIndex, const std::string& deviceName, VirtualRobot::RobotNodePtr robotNode);
+ const SensorValueBase* getSensorValue() const override
+ {
+ return &sensorValue;
+ }
+
+ void init(KITGripperBasisBoardPtr dev, KITGripperBasisBoardDataPtr actorDataPtr, RapidXmlReaderNode configNode, DefaultRapidXmlReaderNode defaultConfigurationNode);
+ protected:
+ KITGripperBasisBoardPtr board;
+ // KITGripperBasisBoardDataPtr dataPtr;
+ VirtualRobot::RobotNodePtr robotNode;
+ size_t actorIndex;
+ ActorDataPtr actorDataPtr;
+
+ JointKITGripperEmergencyStopControllerPtr emergencyController;
+ JointKITGripperStopMovementControllerPtr stopMovementController;
+ JointPWMVelocityControllerPtr velocityController;
+ JointPWMPositionControllerPtr positionController;
+ JointPWMZeroTorqueControllerPtr zeroTorqueController;
+
+ /// The data object for copying to non-rt part
+ KITGripperActorSensorData sensorValue;
+
+
+ // SensorDevice interface
+ public:
+ void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+
+ // ControlDevice interface
+ public:
+ void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ const VirtualRobot::RobotNodePtr& getRobotNode() const;
+ ActorDataPtr getActorDataPtr() const;
+ };
+ using ActorRobotUnitDevicePtr = std::shared_ptr<ActorRobotUnitDevice>;
+
+ KITGripperBasisBoard(RapidXmlReaderNode node, DefaultRapidXmlReaderNode defaultConfigurationNode, VirtualRobot::RobotPtr const& robot);
+ void init(KITGripperBasisBoardSlavePtr slave);
+
+ // AbstractFunctionalDevice interface
+ public:
+ void initData() override;
+
+ // SensorDevice interface
+ public:
+ const SensorValueBase* getSensorValue() const override;
+ const SlaveIdentifier& getSlaveIdentifier() const;
+ const std::vector<ActorRobotUnitDevicePtr >& getDevices() const;
+ size_t getActorIndex(const std::string& actorName);
+ protected:
+ // SensorDevice interface
+ public:
+ void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+
+ // // ControlDevice interface
+ // public:
+ // void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+
+ private:
+ RapidXmlReaderNode configNode;
+ DefaultRapidXmlReaderNode defaultConfigurationNode;
+ VirtualRobot::RobotPtr robot;
+ KITGripperBasisBoardDataPtr dataPtr;
+ KITGripperBasisBoardSlavePtr slave;
+ SensorValueKITGripperBasisBoard sensorValue;
+ SlaveIdentifier slaveIdentifier;
+ std::vector<ActorRobotUnitDevicePtr > devices;
+ PWMVelocityControllerConfigurationPtr velocityControllerConfigDataPtr;
+
+ };
+
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
new file mode 100644
index 000000000..a3550e98c
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
@@ -0,0 +1,249 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, 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/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "KITGripperBasisBoardData.h"
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+namespace armarx
+{
+
+ KITGripperBasisBoardData::KITGripperBasisBoardData(const RapidXmlReaderNode& node, DefaultRapidXmlReaderNode defaultConfigurationNode, KITGripperBasisBoardOUT_t* sensorOUT, KITGripperBasisBoardIN_t* sensorIN, VirtualRobot::RobotPtr robot) :
+ sensorOUT(sensorOUT),
+ sensorIN(sensorIN)
+ {
+ ARMARX_CHECK_EXPRESSION(sensorOUT);
+ ARMARX_CHECK_EXPRESSION(sensorIN);
+ actorData.resize(3);
+ for (auto motorNode : node.nodes("Actor"))
+ {
+ auto connectorIndex = motorNode.attribute_as_uint("connector");
+ auto name = motorNode.attribute_value("name");
+ auto enabled = motorNode.attribute_as_bool("enabled", "true", "false");
+ auto conversionNode = defaultConfigurationNode.first_node("KITGripperBasisBoardConversionParametersDefaultConfig").
+ add_node_at_end(motorNode.first_node("ConversionParameters"));
+ auto configNode = defaultConfigurationNode.first_node("KITGripperActorDefaultConfiguration").
+ add_node_at_end(motorNode);
+ auto positionControlEnabled = configNode.first_node("PositionControlEnabled").value_as_bool("1", "0");
+ auto velocityControlEnabled = configNode.first_node("VelocityControlEnabled").value_as_bool("1", "0");
+ ARMARX_IMPORTANT << "Creating actor data class for " << name << " at index " << connectorIndex;
+ auto initActorData = [&](int* position, int* velocity, int* torque, int* targetPWM)
+ {
+ actorData.at(connectorIndex).reset(new ActorData);
+ actorData.at(connectorIndex)->targetPWMPtr.init(targetPWM, conversionNode.first_node("pwm"));
+ actorData.at(connectorIndex)->maxPWM = configNode.first_node("maxPWM").value_as_uint32();
+ actorData.at(connectorIndex)->position.init(&actorData.at(connectorIndex)->rawABSEncoderTicks, conversionNode.first_node("position"));
+ actorData.at(connectorIndex)->relativePosition.init(position, conversionNode.first_node("relativePosition"));
+ actorData.at(connectorIndex)->velocity.init(velocity, conversionNode.first_node("velocity"));
+ actorData.at(connectorIndex)->torque.init(torque, conversionNode.first_node("torque"));
+ actorData.at(connectorIndex)->robotNode = robot->getRobotNode(name);
+ 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)->currentPWMBoundGradient = configNode.first_node("CurrentPWMBoundGradient").value_as_float();
+ actorData.at(connectorIndex)->currentPWMBoundOffset = configNode.first_node("CurrentPWMBoundOffset").value_as_int32();
+ };
+ switch (connectorIndex)
+ {
+ case 0:
+ initActorData(&sensorOUT->motor1_current_pos, &sensorOUT->motor1_current_speed, &sensorOUT->motor1_current_torque, &sensorIN->motor1_target_pwm);
+ break;
+ case 1:
+ initActorData(&sensorOUT->motor2_current_pos, &sensorOUT->motor2_current_speed, &sensorOUT->motor2_current_torque, &sensorIN->motor2_target_pwm);
+ break;
+ case 2:
+ initActorData(&sensorOUT->motor3_current_pos, &sensorOUT->motor3_current_speed, &sensorOUT->motor3_current_torque, &sensorIN->motor3_target_pwm);
+ break;
+ default:
+ throw LocalException("Motor index out of range: ") << connectorIndex;
+ }
+
+ }
+ }
+
+ void KITGripperBasisBoardData::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ int i = 0;
+ for (auto& ptr : actorData)
+ {
+ if (!ptr)
+ {
+ ++i;
+ continue;
+ }
+ ActorData& d = *ptr;
+
+ 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;
+ }
+ else if (i == 1)
+ {
+ d.rawABSEncoderTicks = (((uint32_t)sensorOUT->RawABSEncoder2ValueBytes[0] << 24 | (uint32_t)sensorOUT->RawABSEncoder2ValueBytes[1] << 16 | (uint32_t)sensorOUT->RawABSEncoder2ValueBytes[2] << 8 | (uint32_t)sensorOUT->RawABSEncoder2ValueBytes[3]) & 0xFFFFF000) >> 12;
+ }
+ else
+ {
+ d.rawABSEncoderTicks = 0;
+ }
+
+ d.position.read();
+
+ d.relativePosition.read();
+ if (std::isnan(d.relativePositionOffset)) // initialize on first run
+ {
+ d.relativePositionOffset = -d.relativePosition.value + d.position.value;
+ }
+
+ //ARMARX_RT_LOGF_INFO("position %d, relative position: %d", (int)d.rawABSEncoderTicks, d.relativePosition.value).deactivateSpam(0.5);
+ d.velocity.read();
+ d.torque.read();
+ d.currentMaxPWM = std::round(*d.velocityTicks * d.currentPWMBoundGradient + d.currentPWMBoundOffset);
+ d.currentMinPWM = std::round(*d.velocityTicks * d.currentPWMBoundGradient - d.currentPWMBoundOffset);
+ // d.acceleration.read();
+ // d.gravityTorque.read();
+ // d.motorCurrent.read();
+ // d.motorTemperature.read();
+ i++;
+ }
+ }
+
+ void KITGripperBasisBoardData::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ // int i = 0;
+ // for (auto& ptr : actorData)
+ // {
+ // if (!ptr)
+ // {
+ // ++i;
+ // continue;
+ // }
+ // ActorData& d = *ptr;
+ // d.targetPWMPtr.write();
+
+ // i++;
+ // }
+ }
+
+ ActorDataPtr& KITGripperBasisBoardData::getActorData(size_t actorIndex)
+ {
+ ARMARX_CHECK_LESS(actorIndex, actorData.size());
+ ARMARX_CHECK_EXPRESSION_W_HINT(actorData.at(actorIndex), actorIndex);
+ return actorData.at(actorIndex);
+ }
+
+ void ActorData::setTargetPWM(int32_t targetPWM)
+ {
+ targetPWM = math::MathUtils::LimitMinMax(currentMinPWM, currentMaxPWM, targetPWM);
+ targetPWM = math::MathUtils::LimitTo(targetPWM, maxPWM);
+
+ targetPWMPtr.value = targetPWM;
+ targetPWMPtr.write();
+ // ARMARX_RT_LOGF_INFO(" pwm: %d, raw pwm: %d, factor: %.f", targetPWMPtr.value, targetPWMPtr.getRaw(), targetPWMPtr.getFactor());
+ }
+
+ float ActorData::getPosition() const
+ {
+ return position.value;
+ }
+
+ float ActorData::getRelativePosition() const
+ {
+ return relativePosition.value;
+ }
+
+ float ActorData::getAdjustedRelativePosition() const
+ {
+ return relativePosition.value + relativePositionOffset;
+ }
+
+ float ActorData::getVelocity() const
+ {
+ return velocity.value;
+ }
+
+ float ActorData::getTorque() const
+ {
+ return torque.value;
+ }
+
+ float ActorData::getSoftLimitHi() const
+ {
+ return robotNode->getJointLimitHigh();
+ }
+
+ float ActorData::getSoftLimitLo() const
+ {
+ return robotNode->getJointLimitLo();
+ }
+
+ bool ActorData::isLimitless() const
+ {
+ return robotNode->isLimitless();
+ }
+
+ int32_t ActorData::getTargetPWM() const
+ {
+ return targetPWMPtr.value;
+ }
+
+ int32_t ActorData::getVelocityTicks() const
+ {
+ return *velocityTicks;
+ }
+
+ int32_t ActorData::getCurrentMinPWM() const
+ {
+ return currentMinPWM;
+ }
+
+ int32_t ActorData::getCurrentMaxPWM() const
+ {
+ return currentMaxPWM;
+ }
+
+ int32_t ActorData::getParallelControlActorIndex() const
+ {
+ return parallelControlEnabled;
+ }
+
+ bool ActorData::getPositionControlEnabled() const
+ {
+ return positionControlEnabled;
+ }
+
+ bool ActorData::getVelocityControlEnabled() const
+ {
+ return velocityControlEnabled;
+ }
+
+ float ActorData::getCurrentPWMBoundGradient() const
+ {
+ return currentPWMBoundGradient;
+ }
+
+ int32_t ActorData::getCurrentPWMBoundOffset() const
+ {
+ return currentPWMBoundOffset;
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
new file mode 100644
index 000000000..d93760904
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
@@ -0,0 +1,150 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, 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/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include "KITGripperBasisBoardSlave.h"
+
+namespace armarx
+{
+ class SensorValueKITGripperBasisBoard :
+ // virtual public SensorValue1DoFActuatorMotorTemperature,
+ virtual public SensorValueIMU
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ static SensorValueInfo<SensorValueKITGripperBasisBoard> GetClassMemberInfo()
+ {
+ SensorValueInfo<SensorValueKITGripperBasisBoard> svi;
+ // svi.addBaseClass<SensorValue1DoFActuatorMotorTemperature>();
+ svi.addBaseClass<SensorValueIMU>();
+ return svi;
+ }
+
+ };
+
+ class KITGripperActorSensorData : virtual public SensorValue1DoFRealActuator
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ int32_t targetPWM;
+ float relativePosition;
+ float velocityTicksPerMs;
+ int32_t maxPWM;
+ int32_t minPWM;
+
+ static SensorValueInfo<KITGripperActorSensorData> GetClassMemberInfo()
+ {
+ SensorValueInfo<KITGripperActorSensorData> svi;
+ // svi.addBaseClass<SensorValue1DoFActuatorMotorTemperature>();
+ svi.addBaseClass<SensorValue1DoFRealActuator>();
+ svi.addMemberVariable(&KITGripperActorSensorData::targetPWM, "targetPWM");
+ svi.addMemberVariable(&KITGripperActorSensorData::relativePosition, "relativePosition");
+ svi.addMemberVariable(&KITGripperActorSensorData::maxPWM, "maxPWM");
+ svi.addMemberVariable(&KITGripperActorSensorData::minPWM, "minPWM");
+ svi.addMemberVariable(&KITGripperActorSensorData::velocityTicksPerMs, "velocityTicksPerMs");
+ return svi;
+ }
+
+ };
+
+
+ class ActorData
+ {
+ public:
+ void setTargetPWM(int32_t targetPWM);
+ float getPosition() const;
+ float getRelativePosition() const;
+ float getAdjustedRelativePosition() const;
+ float getVelocity() const;
+ float getTorque() const;
+ float getSoftLimitHi() const;
+ float getSoftLimitLo() const;
+ bool isLimitless() const;
+ int32_t getTargetPWM() const;
+ int32_t getVelocityTicks() const;
+
+ int32_t getCurrentMinPWM() const;
+
+ int32_t getCurrentMaxPWM() const;
+
+ int32_t getParallelControlActorIndex() const;
+
+ bool getPositionControlEnabled() const;
+
+ bool getVelocityControlEnabled() const;
+
+ float getCurrentPWMBoundGradient() const;
+
+ int32_t getCurrentPWMBoundOffset() const;
+
+ private:
+ u_int32_t rawABSEncoderTicks;
+ LinearConvertedValue<int32_t> relativePosition;
+ float relativePositionOffset = std::nan("");
+ LinearConvertedValue<u_int32_t> position;
+ LinearConvertedValue<int32_t> velocity;
+ LinearConvertedValue<int32_t> torque;
+ LinearConvertedValue<int32_t> targetPWM;
+ int32_t* velocityTicks;
+ int32_t currentMaxPWM = 0;
+ int32_t currentMinPWM = 0;
+ size_t maxPWM;
+ int32_t parallelControlEnabled = -1;
+ VirtualRobot::RobotNodePtr robotNode;
+ LinearConvertedValue<int32_t> targetPWMPtr;
+ bool positionControlEnabled = true;
+ bool velocityControlEnabled = true;
+ float currentPWMBoundGradient = 3.75;
+ int32_t currentPWMBoundOffset = 1500;
+ friend class KITGripperBasisBoardData;
+ };
+ using ActorDataPtr = std::shared_ptr<ActorData>;
+
+ class KITGripperBasisBoardData : public AbstractData
+ {
+ public:
+ KITGripperBasisBoardData(const RapidXmlReaderNode& node, DefaultRapidXmlReaderNode defaultConfigurationNode,
+ KITGripperBasisBoardOUT_t* sensorOUT, KITGripperBasisBoardIN_t* sensorIN, VirtualRobot::RobotPtr robot);
+
+ // AbstractData interface
+ public:
+ void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ ActorDataPtr& getActorData(size_t actorIndex);
+
+ private:
+ KITGripperBasisBoardOUT_t* sensorOUT;
+ KITGripperBasisBoardIN_t* sensorIN;
+
+ std::vector<ActorDataPtr> actorData;
+
+ };
+ using KITGripperBasisBoardDataPtr = std::shared_ptr<KITGripperBasisBoardData>;
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp
new file mode 100644
index 000000000..4a3eec9b1
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp
@@ -0,0 +1,140 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, 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/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "KITGripperBasisBoard.h"
+#include "KITGripperBasisBoardSlave.h"
+
+#include <ethercat.h>
+#include <RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h>
+#include <RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.h>
+
+namespace armarx
+{
+
+
+
+ KITGripperBasisBoardSlave::KITGripperBasisBoardSlave(const SlaveIdentifier slaveIdentifier, uint16_t slaveNumber) :
+ AbstractSlave(slaveIdentifier, slaveNumber)
+ {
+ setTag("KITGripperBasisBoardSlave_" + slaveIdentifier.humanName);
+ }
+
+ void KITGripperBasisBoardSlave::doMappings()
+ {
+ }
+
+ bool KITGripperBasisBoardSlave::prepare()
+ {
+ return true;
+ }
+
+ void KITGripperBasisBoardSlave::execute()
+ {
+ /*if (::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance())
+ {
+ //ARMARX_RT_LOGF_INFO("relative position 1: %d, current speed 1: %d", (int)outputs->motor1_current_pos, (int)outputs->motor1_current_speed).deactivateSpam(0.5);
+ //ARMARX_RT_LOGF_INFO("relative position 2: %d, current speed 2: %d", (int)outputs->motor2_current_pos, (int)outputs->motor2_current_speed).deactivateSpam(0.5);
+ //ARMARX_RT_LOGF_INFO("relative position 3: %d, current speed 3: %d", (int)outputs->motor3_current_pos, (int)outputs->motor3_current_speed).deactivateSpam(0.5);
+ }*/
+ }
+
+ bool KITGripperBasisBoardSlave::shutdown()
+ {
+ return true;
+ }
+
+ void KITGripperBasisBoardSlave::setInputPDO(void* ptr)
+ {
+ inputs = static_cast<KITGripperBasisBoardIN_t*>(ptr);
+ }
+
+ void KITGripperBasisBoardSlave::setOutputPDO(void* ptr)
+ {
+ outputs = static_cast<KITGripperBasisBoardOUT_t*>(ptr);
+ }
+
+ void KITGripperBasisBoardSlave::prepareForOp()
+ {
+ }
+
+ bool KITGripperBasisBoardSlave::hasError()
+ {
+ return false;
+ }
+
+ bool KITGripperBasisBoardSlave::hasPDOMapping() const
+ {
+ return true;
+ }
+
+ KITGripperBasisBoardIN_t* KITGripperBasisBoardSlave::getInputs() const
+ {
+ return inputs;
+ }
+
+ KITGripperBasisBoardOUT_t* KITGripperBasisBoardSlave::getOutputs() const
+ {
+ return outputs;
+ }
+
+
+ /**
+ * register this class in the super class factory
+ */
+ KITGripperBasisBoardFactory::SubClassRegistry KITGripperBasisBoardFactory::registry(KITGripperBasisBoardFactory::getName(), &KITGripperBasisBoardFactory::createInstance);
+
+ KITGripperBasisBoardFactory::SharedPointerType KITGripperBasisBoardFactory::createInstance(EtherCATFactoryArgs args)
+ {
+ EtherCAT* etherCAT = std::get<0>(args);
+ ARMARX_CHECK_EXPRESSION(etherCAT);
+ auto slaveIndex = std::get<1>(args);
+ auto deviceContainer = std::get<2>(args);
+
+ auto devs = deviceContainer->getDevicesOfType<KITGripperBasisBoard>();
+ // if ((ec_slave[slaveIndex].mbx_proto & ECT_MBXPROT_COE) == 0) // TODO: valid for this slave?
+ {
+ for (auto& dev : devs)
+ {
+ if (ec_slave[slaveIndex].eep_man == H2T_VENDOR_ID &&
+ ec_slave[slaveIndex].eep_id == dev->getSlaveIdentifier().ProductID)
+ {
+ ARMARX_INFO << "KITGripperBasisBoard '" << dev->getSlaveIdentifier().humanName << "' found";
+ auto slave = std::make_shared<KITGripperBasisBoardSlave>(dev->getSlaveIdentifier(), slaveIndex);
+ dev->init(slave);
+ KITGripperBasisBoardFactoryPtr objFac(new KITGripperBasisBoardFactory);
+ objFac->addSlave(slave);
+ objFac->addSensorDevice(dev);
+ for (auto& virtualDev : dev->getDevices())
+ {
+ objFac->addControlDevice(virtualDev);
+ objFac->addSensorDevice(virtualDev);
+ }
+ return objFac;
+ }
+
+ }
+ }
+ return SharedPointerType();
+ }
+
+}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h
new file mode 100644
index 000000000..54b92e0fa
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h
@@ -0,0 +1,139 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, 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/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h>
+#include <RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.h>
+#include <memory>
+
+namespace armarx
+{
+
+ /**
+ * @brief PDO mapping sensorB->master
+ */
+ struct KITGripperBasisBoardOUT_t
+ {
+ u_int8_t RawABSEncoderValueBytes[4];
+ u_int8_t RawABSEncoderValueCRC;
+ int16_t pad1;
+ int8_t pad2;
+
+ u_int8_t RawABSEncoder2ValueBytes[4];
+ u_int8_t RawABSEncoder2ValueCRC;
+ int16_t pad3;
+ int8_t pad4;
+
+ int16_t IMUVector1[3];
+ int16_t IMUVector2[3];
+ int16_t IMUQuaternionW;
+ int16_t IMUQuaternionX;
+ int16_t IMUQuaternionY;
+ int16_t IMUQuaternionZ;
+ int8_t IMUTemperature;
+
+ int16_t pad5;
+ int8_t pad6;
+
+
+ int32_t motor1_current_pos;
+ int32_t motor1_current_speed; // ticks pro milliseconds
+ int32_t motor1_current_torque;
+ int32_t motor2_current_pos;
+ int32_t motor2_current_speed;
+ int32_t motor2_current_torque;
+ int32_t motor3_current_pos;
+ int32_t motor3_current_speed;
+ int32_t motor3_current_torque;
+
+ } __attribute__((__packed__));
+
+ /**
+ * @brief PDO mapping master->sensorB
+ */
+ struct KITGripperBasisBoardIN_t
+ {
+ uint16_t LED_PG15;
+ uint16_t LED_2;
+ uint16_t LED_3;
+ uint16_t LED_4;
+
+ int32_t motor1_target_pwm;
+ int32_t motor1_target_speed;
+ int32_t motor1_target_torque;
+ int32_t motor2_target_pwm;
+ int32_t motor2_target_speed;
+ int32_t motor2_target_torque;
+ int32_t motor3_target_pwm;
+ int32_t motor3_target_speed;
+ int32_t motor3_target_torque;
+ int32_t motor4_target_pwm;
+ } __attribute__((__packed__));
+
+ class KITGripperBasisBoardSlave;
+ typedef std::shared_ptr<KITGripperBasisBoardSlave> KITGripperBasisBoardSlavePtr;
+
+
+ class KITGripperBasisBoardSlave : public AbstractSlave
+ {
+ public:
+ KITGripperBasisBoardSlave(const armarx::SlaveIdentifier slaveIdentifier, uint16_t slaveNumber);
+
+ // AbstractSlave interface
+ public:
+ void doMappings() override;
+ bool prepare() override;
+ void execute() override;
+ bool shutdown() override;
+ void setInputPDO(void* ptr) override;
+ void setOutputPDO(void* ptr) override;
+ void prepareForOp() override;
+ bool hasError() override;
+ bool hasPDOMapping() const override;
+ KITGripperBasisBoardIN_t* getInputs() const;
+
+ KITGripperBasisBoardOUT_t* getOutputs() const;
+
+ protected:
+ KITGripperBasisBoardIN_t* inputs;
+ KITGripperBasisBoardOUT_t* outputs;
+ };
+
+ class KITGripperBasisBoardFactory : public EtherCATDeviceFactory
+ {
+ KITGripperBasisBoardFactory() {}
+ // AbstractFactoryMethod
+ public:
+ static std::string getName()
+ {
+ return "KITGripperBasisBoardFactory";
+ }
+ private:
+ static SubClassRegistry registry;
+ static SharedPointerType createInstance(EtherCATFactoryArgs args);
+ };
+ using KITGripperBasisBoardFactoryPtr = std::shared_ptr<KITGripperBasisBoardFactory>;
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h
new file mode 100644
index 000000000..3c94921cc
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h
@@ -0,0 +1,261 @@
+#pragma once
+/* Includes ------------------------------------------------------------------*/
+
+#include "TorqueEstimationWeights.h"
+
+float imgIn[5];
+float imgFcl1[32];
+float imgFcl2[32];
+float imgFcl3[32];
+float imgFcl4[16];
+float imgFcl5[8];
+float imgFcl6[1];
+
+
+uint8_t fcl1(void)
+{
+ uint8_t outFNum = sizeof imgFcl1 / sizeof(float);
+ uint8_t inFNum = sizeof imgIn / sizeof(float);
+ uint8_t outF = 0;
+ uint8_t inF = 0;
+ float buf = 0.0;
+ memset(imgFcl1, 0.0, sizeof imgFcl1 / sizeof(float));
+
+ for (outF = 0; outF < outFNum; outF++)
+ {
+ for (inF = 0; inF < inFNum; inF++)
+ {
+ buf += imgIn[inF] * fc1Weights[inF][outF]; //OLDENTRIES-1-
+ }
+
+ buf += fc1Bias[outF];
+ if (buf < 0) //relu
+ {
+ buf = 0.0;
+ }
+ imgFcl1[outF] = buf;
+ buf = 0.;
+ }
+ return 1;
+}
+
+
+uint8_t fcl2(void)
+{
+ uint8_t outFNum = sizeof imgFcl2 / sizeof(float);
+ uint8_t inFNum = sizeof imgFcl1 / sizeof(float);
+ uint8_t outF = 0;
+ uint8_t inF = 0;
+ float buf = 0.0;
+ memset(imgFcl2, 0.0, sizeof imgFcl2 / sizeof(float));
+
+ for (outF = 0; outF < outFNum; outF++)
+ {
+ for (inF = 0; inF < inFNum; inF++)
+ {
+ buf += imgFcl1[inF] * fc2Weights[inF][outF];
+ }
+
+ buf += fc2Bias[outF];
+ if (buf < 0) //relu
+ {
+ buf = 0.0;
+ }
+ imgFcl2[outF] = buf;
+ buf = 0.;
+ }
+ return 1;
+}
+
+uint8_t fcl3(void)
+{
+ uint8_t outFNum = sizeof imgFcl3 / sizeof(float);
+ uint8_t inFNum = sizeof imgFcl2 / sizeof(float);
+ uint8_t outF = 0;
+ uint8_t inF = 0;
+ float buf = 0.0;
+ memset(imgFcl3, 0.0, sizeof imgFcl3 / sizeof(float));
+
+ for (outF = 0; outF < outFNum; outF++)
+ {
+ for (inF = 0; inF < inFNum; inF++)
+ {
+ buf += imgFcl2[inF] * fc3Weights[inF][outF];
+ }
+
+ buf += fc3Bias[outF];
+ if (buf < 0) //relu
+ {
+ buf = 0.0;
+ }
+ imgFcl3[outF] = buf;
+ buf = 0.;
+ }
+ return 1;
+}
+uint8_t fcl4(void)
+{
+ uint8_t outFNum = sizeof imgFcl4 / sizeof(float);
+ uint8_t inFNum = sizeof imgFcl3 / sizeof(float);
+ uint8_t outF = 0;
+ uint8_t inF = 0;
+ float buf = 0.0;
+ memset(imgFcl4, 0.0, sizeof imgFcl4 / sizeof(float));
+
+ for (outF = 0; outF < outFNum; outF++)
+ {
+ for (inF = 0; inF < inFNum; inF++)
+ {
+ buf += imgFcl3[inF] * fc4Weights[inF][outF];
+ }
+
+ buf += fc4Bias[outF];
+ if (buf < 0) //relu
+ {
+ buf = 0.0;
+ }
+ imgFcl4[outF] = buf;
+ buf = 0.;
+ }
+ return 1;
+}
+
+uint8_t fcl5(void)
+{
+ uint8_t outFNum = sizeof imgFcl5 / sizeof(float);
+ uint8_t inFNum = sizeof imgFcl4 / sizeof(float);
+ uint8_t outF = 0;
+ uint8_t inF = 0;
+ float buf = 0.0;
+ memset(imgFcl5, 0.0, sizeof imgFcl5 / sizeof(float));
+
+ for (outF = 0; outF < outFNum; outF++)
+ {
+ for (inF = 0; inF < inFNum; inF++)
+ {
+ buf += imgFcl4[inF] * fc5Weights[inF][outF];
+ }
+
+ buf += fc5Bias[outF];
+ if (buf < 0) //relu
+ {
+ buf = 0.0;
+ }
+ imgFcl5[outF] = buf;
+ buf = 0.;
+ }
+ return 1;
+}
+
+uint8_t fcl6(void)
+{
+ uint8_t outFNum = sizeof imgFcl6 / sizeof(float);
+ uint8_t inFNum = sizeof imgFcl5 / sizeof(float);
+ uint8_t outF = 0;
+ uint8_t inF = 0;
+ float buf = 0.0;
+ memset(imgFcl6, 0.0, sizeof imgFcl6 / sizeof(float));
+
+ for (outF = 0; outF < outFNum; outF++)
+ {
+ for (inF = 0; inF < inFNum; inF++)
+ {
+ buf += imgFcl5[inF] * fc6Weights[inF][outF];
+ }
+
+ buf += fc6Bias[outF];
+ //if(buf < 0) //relu
+ //{
+ // buf = 0.0;
+ //}
+ imgFcl6[outF] = buf;
+ buf = 0.;
+ }
+ return 1;
+}
+
+
+float linearModel_dcx22(int32_t nI, int32_t pwm)
+{
+ float n = (float) nI * 1000 * 60 / 4096; //ticks/ms -> U/min
+ float T_f = 1000. / 231.; //übersetzung getriebe + Nm->m
+ float motor_a = 226. + 30.; //Drehzahlkonstante [min-1 V-1]
+ float motor_b = 123.; //Kennliniensteigung [min-1 mNm-1]
+ float motor_eta = 0.4; //Wirkungsgrad Getriebe *Motor, max from datasheet:0.75*0.85
+ float Umax = 48.; //Spannung bei pwm max
+ float pwmmax = 3000.;
+ float pwm_zero = 250.;
+ float U;
+ float T_motor;
+
+
+ U = (float(fabs(pwm) - pwm_zero) / (pwmmax - pwm_zero)) * Umax;
+ if (U < 0)
+ {
+ U = 0;
+ }
+ if (pwm < 0)
+ {
+ U *= -1;
+ }
+ if (pwm == 0)
+ {
+ U = 0;
+ }
+
+ //U(M,n)=(n-b*M)/a
+ T_motor = (U * motor_a - n) / -motor_b;
+ auto T = T_motor * motor_eta / T_f;
+
+ return T;
+}
+
+
+float estimateTorque(int32_t n, int32_t pwm)
+{
+ float n_input = (float)n / n_factor;
+ float pwm_input = (float)pwm / pwm_factor;
+ // float inputData[6];
+ static float pwmXn_old = 0;
+ float pwmXn = n_input * pwm_input;
+ float torque = 0.;
+
+ if (pwmXn < 0)
+ {
+ pwmXn = -1;
+ pwmXn_old = pwmXn;
+ }
+ else if (pwmXn > 0)
+ {
+ pwmXn = 1;
+ pwmXn_old = pwmXn;
+ }
+ else
+ {
+ pwmXn = pwmXn_old;
+ }
+
+ // powerDir = linearModel_dcx22(n, pwm);
+ // if(powerDir < 0)
+ // {
+ // powerDir = -1;
+ // }
+ // if(powerDir > 0 )
+ // {
+ // powerDir = 1;
+ // }
+
+ imgIn[0] = n_input;
+ imgIn[1] = pwm_input;
+ imgIn[2] = n_input;
+ imgIn[3] = pwm_input;
+ imgIn[4] = pwmXn;
+ fcl1();
+ fcl2();
+ fcl3();
+ fcl4();
+ fcl5();
+ fcl6();
+ torque = imgFcl6[0];
+ return torque + linearModel_dcx22(n, pwm);
+}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationWeights.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationWeights.h
new file mode 100644
index 000000000..2ef7dfd6c
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationWeights.h
@@ -0,0 +1,189 @@
+// written from class_nn_T_prediction_11_Delta_simple2.py; testName:aktor_l
+//2018-12-20 15:15
+#ifndef __WEIGHTS_H
+#define __WEIGHTS_H
+
+static float n_factor = 459.0;
+static float pwm_factor = 1700.0;
+
+
+const float fc1Weights[5][32] =
+{
+ {-0.2055357695f, 0.1997956634f, 0.0573679060f, 5.6070003510f, -0.0359823219f, -0.6203082204f, -3.5134258270f, -0.0713983923f, 0.0685816705f, -0.0381882004f, -1.7217775583f, -3.8536903858f, -0.3756493032f, -0.1594707817f, -6.3813128471f, -0.3226724863f, 0.2063246369f, -0.6387553811f, 0.0372939110f, 3.8864912987f, 1.4991726875f, 4.3318767548f, -1.9102532864f, -0.0856776312f, -0.5104419589f, 0.3844818473f, -0.5385549664f, 0.0184886176f, -0.2233840525f, 0.3443737030f, -0.1556139588f, 8.5595130920f},
+ {0.2853892446f, 0.9347148538f, 0.2580135465f, -5.0483689308f, -0.1692553312f, 0.6664142609f, -0.8050979972f, -0.1831562817f, 0.2196862400f, 0.0588629805f, 2.8238334656f, 4.0935006142f, 0.3776662350f, 0.1199822798f, 6.6613926888f, -0.1866912246f, 0.0059743905f, -4.2951517105f, -0.1723812819f, -3.6761276722f, 1.8258478642f, -1.4123811722f, 1.8817381859f, -0.3389021456f, 0.4059905112f, -0.3896758854f, 0.7206512094f, 0.2583603859f, -2.8177876472f, -0.2914963961f, 0.4590151906f, -9.9321269989f},
+ {-0.5632920861f, 0.1988572478f, -0.5542681813f, 5.7822799683f, 0.3820048273f, -0.2525918782f, -3.3970746994f, 0.2208536267f, 0.0045266999f, 0.3900515437f, -1.7668331861f, -3.6709997654f, -0.2005964518f, 0.1454627365f, -6.3249039650f, 0.2582260370f, -0.2433532774f, -0.9390440583f, -0.5737978816f, 4.1182165146f, 1.6590707302f, 4.8758029938f, -1.8478810787f, 0.0048876349f, 0.1774665117f, -0.1789420843f, -0.5673617721f, -0.2723075449f, -0.3745986819f, 0.0240579937f, -0.2824915946f, 8.3010101318f},
+ {0.4449025393f, 0.6115619540f, 0.0187926833f, -5.4022917747f, 0.0334641673f, 0.4216177166f, -0.8479185700f, -0.4422490299f, -0.2795608342f, -0.3466927111f, 2.2280035019f, 3.5217893124f, 0.1864205599f, -0.1767526865f, 6.5718340874f, 0.3395559490f, -0.0630193949f, -4.2917470932f, 0.4928318858f, -3.4380824566f, 2.2824761868f, -1.3895356655f, 1.8980522156f, 0.3309353590f, 0.0092654871f, 0.2050092816f, 0.4185230136f, -0.0931341946f, -3.0571875572f, -0.0199668277f, 0.0238261241f, -10.0575485229f},
+ {-0.0596951246f, -1.8140755892f, -0.1749392897f, 0.2104561627f, -0.2132787555f, -0.1574463993f, -6.4865732193f, -0.7267509699f, -0.1174281165f, -0.0856533423f, 0.4403370619f, -0.6915963888f, -0.2986042798f, -0.3891125619f, 1.3234760761f, 0.0340486430f, -0.0694809407f, -7.1216664314f, -0.1790750027f, 1.6705595255f, -5.9638838768f, -0.9045498967f, -0.1985210180f, -0.1773930788f, -0.1488362402f, -0.0660701320f, -0.0532661825f, -0.1185604334f, 0.3091884255f, -0.1427213848f, -0.0880882740f, 1.5660281181f}
+};
+
+
+const float fc1Bias[32] =
+{-0.2996839881f, -2.2546701431f, -0.5477045178f, -1.2242631912f, -0.2746987045f, -0.5186939240f, -6.1057929993f, -0.9974002242f, -0.2025506645f, -0.2643158436f, -0.5174910426f, -0.6349564195f, -0.5265557766f, -0.5465639234f, -0.8746579289f, -0.2214775383f, -0.3936641812f, -9.3305616379f, -0.3976268172f, -0.0949812457f, -4.9294414520f, -0.2506873310f, -1.1473840475f, -0.2789034545f, -0.2843038142f, -0.1568380445f, -0.3574894667f, -0.1897565275f, -0.0161396451f, -0.5330590010f, -0.3432305753f, -2.3037126064f};
+
+
+const float fc2Weights[32][32] =
+{
+ {0.7028352022f, -0.2518471181f, -0.3251887262f, -0.1293392181f, -0.0480363145f, -0.2164561749f, -0.1114568934f, 0.0847787485f, 0.2538841367f, 0.0906840563f, 0.2075847685f, 0.0800402164f, -0.2112799883f, -0.3601601422f, -0.2693049312f, -0.3371258378f, 0.1860402822f, 0.1293898672f, 0.5152844191f, -0.0005433710f, -0.3649958968f, -0.5045146346f, 0.7517039180f, 0.0467921384f, -0.0150561901f, -0.2420729399f, -0.2145421207f, -0.7155304551f, 0.1590870321f, -1.1922683716f, 0.1453628838f, -0.3018942773f},
+ {0.4329630733f, 0.2430671006f, -0.8269199729f, -0.1718535721f, 0.6189324260f, -1.1788749695f, 2.3799111843f, -1.0581681728f, -1.0104187727f, -0.6895720959f, 0.6333325505f, 0.1227911711f, -1.3420652151f, 0.0161968544f, -0.9277707934f, -1.6182627678f, 3.4310810566f, -0.0272786003f, 0.9764053226f, 1.7808814049f, -1.4981951714f, -0.2589629292f, 0.0806718767f, -0.0988150090f, 0.0121101709f, -3.1666336060f, -0.9463910460f, -1.3826171160f, -0.1921611279f, -0.4246963561f, 0.0819954872f, -1.1248966455f},
+ {0.3439708948f, 0.1729307026f, -0.4266345203f, -0.1223015115f, -0.2364580184f, 0.0005747200f, 0.1383670121f, -0.0798279345f, -0.0409396328f, 0.2926900089f, 0.4414319098f, -0.0774125233f, -0.0639059693f, 0.0000417514f, -0.3764672279f, -0.2038611472f, 0.1025535613f, -0.2993794680f, 0.0265453998f, -0.0355118513f, 0.2918146253f, -0.4358098209f, 0.0194903836f, -0.0299784839f, -0.2588784993f, -0.2383993119f, -0.2728497982f, -0.3301697373f, 0.1411939114f, 0.1025134176f, -0.3637280166f, 0.1707316041f},
+ {-0.3416239917f, -0.1238532588f, 0.0436132401f, -0.3475806415f, -2.6594336033f, -0.9691415429f, -2.7146120071f, -0.4172722399f, -5.5016303062f, -3.0360279083f, -0.4493477941f, -0.3950566947f, -1.1143312454f, 0.0175548382f, 0.5904526711f, -2.4258356094f, -6.8880228996f, -0.3979260027f, -0.5621471405f, -8.3594980240f, -0.5225185752f, -0.0404085517f, 2.6085379124f, 0.0378435813f, -0.0098032495f, 2.7800383568f, 2.0576617718f, -1.8591514826f, -0.7343788147f, -0.4695331156f, 0.1112806350f, 1.4430732727f},
+ {-0.0525006801f, 0.3783730865f, 0.1891198754f, 0.0213207081f, -0.2073568851f, 0.1397801638f, 0.0859736055f, 0.2507794499f, 0.1215454638f, -0.1463970691f, 0.1565153897f, 0.0612230189f, 0.2093681097f, 0.1229698434f, -0.0846480504f, -0.2200313061f, 0.2009770274f, 0.0123162027f, -0.0025048242f, 0.0691637024f, -0.0181692336f, 0.2928285897f, -0.0372800417f, -0.3242078722f, 0.2153842747f, 0.0912938938f, 0.0259480923f, 0.2408835292f, -0.0780527815f, -0.0694554597f, -0.5314068198f, -0.3713911176f},
+ {-0.3659447432f, -1.0637227297f, -0.6574027538f, 0.1915589571f, -0.0923616737f, -0.6854819655f, -0.7665759921f, -0.3916678429f, 0.0046403264f, 0.1432497799f, -0.9219073057f, 0.3549784422f, -0.1066768840f, 0.0990313441f, -0.9819003344f, -0.6210372448f, 0.1118606776f, 0.2755944431f, -0.8569695354f, -0.1976287961f, -0.2853681743f, -0.6686331034f, 0.2660720646f, -0.1277324557f, 0.0465747900f, 0.4344924688f, -0.2714323103f, 0.2368822843f, -0.0763014555f, -0.2818318605f, -0.3344349861f, 0.1119496822f},
+ {-8.4284162521f, -0.5330117941f, 0.2430908680f, -0.9680790901f, 17.7502479553f, -11.1728906631f, -9.4553775787f, 6.4060249329f, -19.9450950623f, -9.8665761948f, -0.4943230152f, -0.0203626100f, 3.4065568447f, 0.1984576881f, 0.1283037961f, -1.1749160290f, -14.7431478500f, -0.9983112216f, -2.9252219200f, -6.1029782295f, -3.7169921398f, 0.0391248949f, -5.0480856895f, 0.1063139364f, -0.0771418661f, 9.4767627716f, -4.7266154289f, 2.5198585987f, -0.2968094051f, 0.0457970649f, -0.0931691378f, 5.0110268593f},
+ {-0.3695061207f, 0.1182122529f, -0.1756125242f, -1.9989162683f, -0.1902875155f, -0.5620827079f, -1.4479638338f, -0.0801911950f, 0.1420777738f, -1.3847892284f, -0.7704963088f, 0.0931626707f, -0.1455746740f, -0.2081220299f, -0.1101334319f, -0.1995007843f, 0.1743076593f, -0.2777135074f, -0.5316527486f, -0.7597036958f, 0.1699331254f, -0.0814936012f, -0.1331967264f, -0.1476776600f, -0.1340599060f, -0.1023281068f, -0.0449587218f, 0.5310350657f, -0.4073688686f, -0.1218718365f, -0.3036770225f, 0.5314686298f},
+ {-0.6574068069f, -0.4164880514f, -0.0907117650f, 0.0752569512f, -0.1371883452f, -0.0603592172f, -0.4597431421f, 0.1653049141f, -0.2182130367f, -0.1304199547f, 0.1074393615f, -0.0336866528f, -0.2670994699f, -0.0481679738f, -0.0528879240f, -0.0296069179f, 0.2772396803f, -0.0399867371f, 0.0935128778f, -0.2692438662f, 0.1292352676f, -0.4318626225f, 0.0235706642f, -0.0248435289f, -0.0899237916f, 0.2455069125f, -0.3291662335f, -1.0510460138f, 0.0582070835f, -0.0467071161f, 0.2144248039f, 0.2426371127f},
+ {-0.3157160878f, -0.4219513237f, 0.0111433808f, 0.0273274425f, 0.2885468304f, 0.4742646515f, 0.4801633060f, -0.0572001375f, -0.3388701677f, -0.6163141727f, -0.3216553330f, -0.1361154020f, -0.2719146311f, -0.0309467372f, 0.0720715001f, -0.5097295642f, 0.1592271924f, -0.1735037714f, 0.2940475941f, -0.3154088259f, 0.1547768563f, -0.3129318058f, -0.4059058130f, -0.1461700797f, -0.2799991965f, -0.2784145772f, 0.2787654102f, -0.7949113846f, -0.3778579533f, -0.0571825393f, 0.3065904379f, 0.5446800590f},
+ {-11.8102788925f, -0.5548009276f, -0.4933342040f, -0.0146987140f, 14.7732305527f, -3.3242385387f, -4.3986229897f, 0.9681160450f, -11.3604030609f, -8.9482917786f, -0.5679091215f, -1.4008077383f, -12.8347654343f, -0.3398197591f, -2.0602822304f, -10.3154926300f, 3.3564658165f, -0.3715273142f, -1.1404612064f, -0.5240572095f, -5.8742475510f, -0.8057600856f, -1.5432174206f, -0.0802463219f, -0.1602455229f, -0.8414320946f, -4.2538943291f, 4.6012043953f, -0.2584485114f, -0.3790409267f, -0.7869391441f, 0.8633705378f},
+ {-1.0586528778f, -0.1419243515f, -0.0970374048f, 0.2961471677f, 3.8987035751f, -2.2678067684f, -1.4299131632f, 0.7676140070f, -9.9476909637f, 5.8023109436f, -3.9433639050f, -0.4502748251f, 0.8263508677f, -0.2590869963f, -0.0040865224f, -2.3295035362f, 0.8782870770f, -0.6496896148f, 0.4475262761f, 2.5988130569f, 0.7252852321f, -0.4031449258f, -0.9870574474f, -0.3840217292f, -0.2451169640f, 1.9900392294f, 1.5628668070f, 0.8736712933f, -0.0124829793f, 0.0401136354f, -0.3562023640f, 0.2699736655f},
+ {-0.0120646609f, 0.3022353053f, -0.1795586795f, -0.0325300470f, 0.1483502388f, -0.3772916794f, -0.0676282048f, 0.2362668216f, -0.0225414429f, 0.4359962046f, -0.0775927231f, -0.0436186716f, -0.4518926442f, 0.1925306767f, -0.0252785441f, -0.2602519393f, 0.0216296762f, 0.0660978556f, 0.0655388087f, 0.1850759685f, 0.1910172254f, 0.1433989257f, 0.0925811827f, -0.1367113143f, -0.3113737106f, 0.0142191835f, -0.4499391317f, -0.6342935562f, 0.0016022279f, -0.0294085406f, -0.4484280348f, -0.1730211675f},
+ {-1.0707663298f, 1.4295915365f, -1.2444705963f, -0.1815876514f, -0.2770722806f, 0.3313229382f, -0.0980075672f, -0.4719111025f, 0.2169308513f, -0.4073899686f, -0.3677752912f, 0.1686977446f, -0.3691555560f, -0.0566529781f, 0.2436147034f, 0.0352467895f, 0.2247329205f, 0.1287635118f, 0.2276323736f, -1.1428972483f, 0.0267670061f, -0.3803151846f, 0.1016131118f, -0.3375857770f, -0.3619975746f, -0.0580476969f, -0.2426059097f, -0.2319097221f, 0.1949766576f, -2.0283784866f, -0.1499034166f, 0.3203954101f},
+ {-9.3854074478f, -0.0246858224f, -0.8499143124f, -0.2564937472f, -9.3853092194f, -0.6312729120f, -5.0737833977f, 1.9478274584f, 2.6517164707f, 2.3842592239f, -5.2550106049f, -0.3242772520f, 0.8110681176f, -0.3066453636f, -1.3071999550f, 2.7409677505f, 4.5771627426f, -0.5061553717f, -1.4946135283f, 3.5521233082f, -2.9335832596f, -0.5765511990f, 0.0422952175f, -0.1721477956f, -0.2773832083f, 0.2279552370f, -5.2490863800f, -0.7335987091f, -0.1851830781f, 0.0654127821f, -2.0507786274f, 0.3237775564f},
+ {-0.2222796977f, -0.3723528683f, 0.0691528544f, -0.3326320350f, 0.1267025322f, -0.0477940738f, 0.0229985509f, -0.0779023990f, 0.1464604437f, -0.1281969845f, 0.0605426840f, 0.0904146507f, 0.0433506742f, 0.1155809388f, 0.1346668303f, 0.0832128003f, 0.0945276693f, -0.2366465777f, -0.0344646983f, -0.0648121014f, -0.1341129988f, 0.0577474162f, -0.1170159131f, -0.0575251095f, 0.2074271739f, 0.1984294653f, 0.2561211288f, -0.4803146720f, 0.1444168389f, -1.3286510706f, 0.2394987047f, 0.0752803981f},
+ {0.1440746784f, -0.0171720050f, 0.4093155265f, 0.0168101508f, 0.3510462344f, -0.2379340529f, -0.1087244079f, 0.0702282861f, -0.2610715032f, -0.1717523634f, -0.0145056592f, -0.3848222494f, 0.1109743863f, 0.1177851558f, -0.1664983183f, -0.1298100203f, -0.1001451164f, -0.1466456205f, -0.3134774268f, -0.4611427784f, 0.0405314565f, -0.3590387702f, 0.2124959528f, 0.2551239133f, -0.1097064912f, 0.2295900434f, 0.1829279214f, -0.0514878631f, -0.1421726197f, 0.1224616244f, -0.2044410706f, -0.0536189489f},
+ {-1.1521507502f, -0.3603971601f, 0.0563264266f, -1.3970239162f, -1.4270666838f, -2.8226051331f, -3.5476942062f, -2.5657920837f, -0.5939258933f, -5.4976310730f, -4.0049266815f, -0.5885877609f, -0.1167592034f, 0.0184592586f, -1.5224845409f, -4.8727149963f, 3.0517530441f, -0.1223363504f, -0.2621241212f, -3.8385307789f, -2.8868563175f, -0.3684002161f, -0.3500567973f, 0.1355150640f, -0.4692768753f, -6.4998006821f, -1.3951023817f, -0.6174461842f, -0.1671719849f, -0.5628139973f, -0.2307008952f, 8.5936250687f},
+ {-0.3384640515f, 0.2626960576f, -0.2724999785f, -0.3156481981f, 0.1593002975f, -0.0599933714f, -0.5736973286f, -0.0508794934f, 0.0508684218f, 0.1900248677f, -0.2414632142f, -0.0151213873f, -0.0642893165f, -0.0736586526f, 0.3415043652f, 0.2434819192f, 0.1858230978f, -0.0688576773f, 0.0958920494f, 0.0314638801f, -0.2458027154f, -0.5904662013f, 0.0937323049f, 0.1025661081f, -0.1099876687f, 0.1343368292f, -0.2600298226f, 0.3431377113f, -0.2248598337f, -0.0926240832f, -0.0758377016f, -0.0951057747f},
+ {-10.8200063705f, -0.2333872318f, 0.0629985109f, -0.5098025203f, 5.5954618454f, -0.0266051386f, 1.0398765802f, 2.6189880371f, -0.8284100294f, -1.0288726091f, -19.0959739685f, -0.4239546657f, 2.2480175495f, -0.3452661932f, 0.5333261490f, 3.0123581886f, -0.5058676600f, -0.5662975311f, -1.4740906954f, 2.3249070644f, 1.9839859009f, 0.0691346005f, -8.4895219803f, -0.0517179966f, -0.3145155013f, 0.0487585813f, 0.5590797067f, 1.1825811863f, -1.1296035051f, -0.4631800354f, 0.0042153052f, 4.9763317108f},
+ {0.8741941452f, 0.2847254574f, -0.0165548436f, 0.0214925446f, 0.0281693470f, -3.5478408337f, 5.1184897423f, -1.0630372763f, -0.3170503676f, -7.8191223145f, -0.7177888751f, 0.0192223061f, -6.6766848564f, 0.0839038268f, -4.0633893013f, -0.6456161737f, 1.9720232487f, 0.5165043473f, 0.1431490332f, 3.3671495914f, -2.0822234154f, -0.2774647176f, 3.5052692890f, 0.0942892134f, -0.1689221412f, -23.8342685699f, -5.3589863777f, -15.0655832291f, 0.1842747778f, 0.0636580139f, -0.8683874011f, -9.0183506012f},
+ {5.8148365021f, -0.0181446839f, 0.0218663495f, -0.6435270309f, -4.6314897537f, 1.0660556555f, -0.7006200552f, -1.7170858383f, -11.6315021515f, -1.8548130989f, 2.3387854099f, 0.1161770225f, -4.4872589111f, 0.0984403118f, 2.3550364971f, -1.3147095442f, -1.7980459929f, 0.2122275829f, 0.2098008990f, 0.8191279769f, -2.6600685120f, -0.9796918035f, 3.9549865723f, -0.3468909264f, 0.1095151901f, 2.1919665337f, 0.8045359850f, 1.2023586035f, 0.1413695663f, -0.4612948000f, -0.0958333239f, 0.5004554391f},
+ {-10.4134435654f, -1.6761026382f, -2.1109883785f, -0.8899091482f, 0.9217932224f, -0.1489774138f, -1.2502399683f, -0.2223093659f, 0.2842899561f, 0.2826307416f, -11.5551824570f, 0.2193767130f, 0.2752348483f, 0.0379434153f, -3.4593420029f, 0.2076737136f, 0.1856303215f, 0.2192035168f, -0.7756754160f, 0.0748336688f, 0.4596520662f, -0.6056127548f, -0.0012871140f, -0.3025635183f, -0.5217733383f, -0.4117718935f, -0.3294989765f, 0.2200052887f, 0.2029565573f, 0.2765416205f, -0.4646576941f, -0.0226360932f},
+ {-0.3001706302f, -1.0916574001f, -0.1813789010f, -0.1071366295f, -0.5642485023f, -0.4381268024f, -0.2561828494f, 0.2427830398f, -0.0773036852f, 0.1807658523f, -0.6189127564f, -0.5280420780f, -0.2278218716f, 0.2025674731f, -0.1903136224f, 0.2945006490f, -0.0809962600f, 0.1900134534f, -0.5843535066f, -0.1001444906f, 0.1827683747f, 0.2382754385f, 0.3387266994f, -0.3220880032f, 0.1786493659f, 0.0670973137f, -0.1765727550f, -0.0752916187f, -0.1503725797f, -0.1599606723f, -0.5018463135f, 0.0873332471f},
+ {-0.1636323780f, 0.1241440102f, 0.0475576073f, 0.1504822969f, -0.1423517764f, -0.1402205229f, -0.1181961671f, 0.3235143721f, 0.0912692472f, 0.3641940355f, -0.2788277864f, -1.0504397154f, 0.1891589761f, 0.1606433988f, 0.5445721745f, -0.1664946973f, 0.2456973046f, -0.3874065876f, -0.3910988867f, 0.1926820129f, -0.0088137649f, -0.4388289452f, -0.1335452497f, -0.0635569021f, -0.2904116213f, -0.0950278938f, -1.6520583630f, 0.0426815301f, -0.2116074115f, -0.2022051066f, -0.7181299925f, 0.1868721247f},
+ {0.0034010340f, 0.2792718112f, -0.4612697959f, 0.0420143865f, -0.2565088868f, 0.1659194678f, 0.2364908457f, 0.3249883950f, 0.1776097417f, -0.1085670367f, -0.1002039164f, -0.0526343510f, 0.1710232794f, -0.1661372930f, 0.1210514680f, 0.0878346786f, -0.3646489382f, -0.0152188586f, -0.1779934615f, 0.0524123646f, 0.2448512614f, -0.1211499125f, -0.1922912896f, -0.3014973998f, -0.2541061938f, -0.0387258530f, 0.1648880988f, -0.0043392545f, 0.2601354420f, 0.0364522561f, 0.1693428457f, 0.0329939090f},
+ {-0.2273397893f, -0.3498373628f, -0.0433691703f, -0.6629248857f, 0.1507195234f, -0.1990272850f, -0.1220994815f, 0.1678037792f, -4.3043742180f, -0.0007548440f, -0.3417711258f, -0.1170029715f, -1.3333457708f, -0.0142377056f, 0.5767194033f, 0.4187206626f, -0.0696189702f, 0.1859471947f, -0.3047918975f, 0.4045199156f, 0.2059099227f, -0.7050588131f, 0.1555795074f, -0.0812577009f, -0.3910994828f, -0.0145086460f, -0.0485258102f, -0.0066310908f, -0.1054076031f, -0.5957096219f, -0.2342762351f, 0.0350304283f},
+ {0.0149504738f, -0.0129147042f, -0.5884258151f, 0.0588622056f, 0.0843603089f, -0.0650319681f, -0.1795133650f, -2.2022593021f, 0.1941308379f, -0.2966808379f, -0.2518850267f, -0.2675821781f, -0.5772732496f, 0.1356097460f, 0.1572682261f, -0.4143220186f, -1.1145333052f, -0.1922410876f, -0.1281387955f, 0.0762870386f, -0.2557103634f, 0.0021827971f, 0.1089263260f, 0.2346664369f, 0.2724923491f, 0.0007219575f, 0.2104968876f, -0.0320289358f, -0.1094986573f, -0.0914234072f, -0.2628631294f, 0.1553151160f},
+ {0.7854294777f, -1.2805747986f, -0.4509948492f, -0.5390222073f, -7.2329969406f, -4.1658449173f, 0.9921216369f, -19.2893733978f, -0.0606014021f, -1.3520914316f, -2.8341577053f, -0.5397177935f, -0.4183989465f, -0.3706202805f, -4.5540037155f, 0.8042452931f, 2.8141219616f, -0.0732958987f, -0.5256106257f, -0.1778799146f, -1.4509961605f, 0.0517916530f, 0.2504193485f, -0.3624364734f, 0.0715965852f, -2.5190768242f, -20.3786926270f, -0.2376282662f, 0.0138650360f, -0.1234268844f, -0.4907496870f, -1.5723351240f},
+ {0.1309531778f, -0.4162278175f, 0.0861416981f, -0.0754620507f, 0.3168306947f, -0.3067710400f, -0.0102485251f, -0.1974389702f, 0.1207621619f, -0.0179167688f, -0.1688643098f, 0.0122922705f, 0.1167635992f, 0.0521559902f, -0.3053426147f, -0.3361171484f, -0.3374931216f, -0.2523073256f, -0.0067732469f, 0.0612988696f, 0.1934308410f, 0.0817848817f, -0.1016546264f, -0.1203937978f, 0.1065604016f, -0.1992163658f, 0.3401925266f, -0.0275416337f, -0.2688343525f, -0.0388706326f, 0.1756630838f, 0.4808184803f},
+ {0.4282532632f, -0.0259445906f, 0.3069318831f, -0.3668542504f, 0.3145154119f, -0.3847868145f, 0.2201283127f, -0.0448469892f, 0.1542371064f, 0.2123837769f, -0.1569138467f, 0.0211382862f, -0.2548147142f, -0.2737289667f, -0.3175657392f, -0.5842364430f, -0.0748774335f, -0.1540294737f, -0.1290857196f, 0.1770722121f, 0.1833150536f, 0.0081414832f, 0.2064791620f, -0.1276530772f, 0.1270321757f, 0.1417734623f, -0.2329282910f, -0.3905624747f, 0.2545000315f, -0.3035842478f, 0.0064902604f, -0.3016678095f},
+ {-7.5552926064f, -0.9295108318f, -0.2481519580f, 0.7649185658f, 1.9046311378f, -0.1586459428f, 2.0594851971f, -1.3088877201f, -1.2542868853f, -4.6562218666f, 6.3942017555f, -0.6503939629f, -0.7048695683f, 0.2057881355f, 6.5724821091f, 1.7823606730f, -2.8745467663f, -0.3315669894f, 0.9826352596f, -2.7425603867f, -1.9275763035f, -0.2427215725f, -0.5757774115f, 0.2017655820f, -0.1918183118f, -16.7525539398f, -0.7728430629f, 1.0563217402f, -0.4434075356f, -0.7302488089f, -0.2927037179f, 2.4355039597f}
+};
+
+
+const float fc2Bias[32] =
+{-1.4964368343f, -0.4743577838f, -0.4512416422f, -0.4922141731f, -5.1149101257f, 8.8500652313f, -15.7193136215f, -0.0800511166f, 2.7641799450f, 3.4012768269f, 0.0683615580f, -0.1829399616f, 5.3457593918f, -0.4106808603f, -16.5018920898f, -11.1810235977f, -8.3992519379f, -0.4962880909f, -0.9232602119f, -11.2513113022f, 2.3576202393f, -0.4867421985f, -3.7326567173f, -0.2190053761f, -0.3950267136f, -1.8300843239f, 7.8722882271f, -1.0580558777f, -0.4571195245f, -0.4307751656f, -0.3282840848f, -4.9084253311f};
+
+
+const float fc3Weights[32][32] =
+{
+ {2.1267073154f, -0.3118368387f, -1.0274667740f, -0.3681430817f, -0.2503374219f, -1.2477926016f, -1.3221302032f, 1.8165496588f, -0.1359463781f, -1.7964631319f, -0.0097085517f, -0.3852086663f, -3.3534936905f, -0.8397763371f, -0.4446444511f, -1.6745910645f, 3.2378242016f, -1.0022313595f, -9.9781904221f, 0.1582326442f, 0.5432472825f, 0.0471910089f, 5.9129772186f, -0.2324501723f, -4.1757726669f, -2.3055243492f, 0.0340588912f, -9.1570177078f, 3.9973771572f, -5.0203042030f, -0.4807384312f, -0.2449323237f},
+ {0.2926399410f, -0.3126508892f, -0.2911956012f, -0.6654194593f, -1.9371089935f, -0.5545672178f, 0.4246672988f, 0.3819307983f, 0.2514972389f, -0.4406578541f, -0.9821344614f, -0.3851819932f, 1.1248499155f, 0.0522659570f, 0.0575070083f, 0.4138618112f, -0.8817019463f, -0.6708815694f, 0.4475752711f, -0.0536939353f, 0.2634044886f, -0.7456439734f, 0.3254241347f, 0.7578423023f, -0.0451397039f, -0.3245976269f, 0.3969579041f, -0.3185728490f, 0.5564263463f, -0.6522557735f, -0.1052212864f, -0.4474395216f},
+ {0.1283721924f, -0.0823663324f, -0.0326546133f, -0.4661917388f, 0.2401303649f, -0.2880379856f, -0.1517074406f, 0.2752179205f, -0.1285577416f, -0.9086889029f, 0.0597265214f, 0.0445332415f, 0.0693372712f, 0.3575956523f, -0.2324096560f, 0.6556770205f, -0.2550288737f, 0.0115993991f, -0.1578786522f, -0.2495867163f, 0.5065354705f, 0.1579007357f, -0.3488564789f, -0.3209209442f, -0.1637986600f, 0.0921269283f, -0.0206926819f, -0.4815258384f, 0.1049012691f, 0.0869352594f, 0.0118843094f, 0.0378016829f},
+ {-0.1867472231f, -0.0983402878f, -0.2410253137f, 0.1428901106f, 0.5535175800f, 0.4710266590f, 0.7919189930f, -0.1014919057f, -0.0083244918f, 0.1866499782f, -0.2188591063f, -0.2367116362f, -0.3858147562f, 0.2450350076f, 0.1948615909f, 0.2732373476f, -0.7079418898f, -0.4475770295f, -0.0726167932f, 0.0758384317f, -0.0683538988f, -0.3337415755f, 0.6586756110f, -0.3540558219f, 0.1567319185f, 0.2762941718f, -0.1348233819f, -0.2360792607f, -0.0053496510f, 0.3404291868f, 0.0747083575f, -0.4553619027f},
+ {-0.7439086437f, -0.5030723810f, -0.0015834231f, -0.2131769657f, -0.1265947670f, 0.6866528392f, -6.0031766891f, -1.1717530489f, -0.8875666857f, -0.7657486796f, -0.4995601177f, -1.3289493322f, 0.5762737989f, -0.1594046950f, -1.7588888407f, 0.0773057416f, -1.0809998512f, 3.3575456142f, -2.1498513222f, -0.1858167648f, 1.1153688431f, -0.2805164158f, -2.4953896999f, -3.0942769051f, -4.6361846924f, -7.2082462311f, -0.2423758358f, 9.1199541092f, -1.9552760124f, 1.5397961140f, -4.8174195290f, -0.1466594040f},
+ {-12.2784042358f, 0.2199732214f, -0.1896684021f, -0.3432999253f, -0.2287421525f, 3.4079124928f, -1.9381728172f, -0.6183780432f, -2.6809947491f, -13.9357881546f, -0.3349778652f, -1.1822427511f, -0.4702038467f, -0.4513809979f, -4.1594862938f, -1.3155949116f, -2.8984313011f, 0.8626719713f, 0.9431877732f, 0.1867329180f, 1.8311676979f, 0.0268813707f, -0.2230326235f, 1.5717616081f, 4.5641150475f, -5.9756793976f, -0.3749696314f, 6.8801670074f, -0.5390860438f, 2.2278966904f, 1.6726257801f, -0.4617499709f},
+ {-1.1322755814f, -1.6074182987f, 0.3383499086f, -0.0879338682f, -0.3024970293f, -0.9650658965f, -0.7258930802f, -2.2438650131f, -8.0822734833f, -1.1866042614f, -0.1294266731f, -0.3437821269f, -5.2474503517f, -3.8555898666f, -0.9632506967f, -1.5082763433f, -2.1201927662f, 1.0083860159f, 0.9984830022f, 0.0244099908f, 1.3445159197f, 0.0025331527f, 0.3188533485f, -1.2511210442f, 4.4629726410f, -0.6043246984f, -0.2112076432f, 0.2277358621f, -0.4532658756f, -2.5137176514f, -0.6861492395f, -0.2597316206f},
+ {-11.9988451004f, -0.1067576185f, -0.2066107839f, 0.0828568414f, -0.3275893927f, 2.5277581215f, 3.2627487183f, -5.6365389824f, 5.3499884605f, -3.2246093750f, -0.2226941139f, 0.1935922205f, -4.1013288498f, 0.0029573692f, -1.5204852819f, -4.4882822037f, 4.6080155373f, 4.0036969185f, 0.9089803696f, -0.2275806665f, -2.7591450214f, -0.1230073869f, 2.7025136948f, 5.8001189232f, -2.8862383366f, 1.4778681993f, -0.5003541112f, -8.5913944244f, 0.0913238823f, -3.5371487141f, 0.1153692529f, 0.0406519361f},
+ {-2.5129640102f, 0.3217219710f, -0.0095533794f, 0.1789117903f, -0.0394163057f, -12.0436925888f, 7.2080974579f, -0.6646071076f, 7.5206618309f, 3.8631877899f, -0.1600254625f, -0.8080578446f, -0.5643880963f, -0.0537163280f, -0.5046658516f, 0.4704113901f, -1.0934047699f, 1.3934750557f, 1.6021199226f, 0.2552498877f, -2.1392021179f, -0.1866302639f, -1.9532939196f, 4.3028411865f, -2.0663371086f, -1.1156477928f, 0.0436181799f, -4.5952415466f, -4.1031899452f, -4.2674293518f, -1.3652423620f, -0.3086400926f},
+ {-2.4256632328f, -0.8204917312f, -0.0080023678f, 0.0641652644f, 0.1045749411f, -6.7571711540f, -5.8372755051f, -2.5383017063f, 1.3260424137f, 0.7004460692f, -0.2244220227f, 0.5836414695f, -0.1815587580f, -0.3951809406f, -0.1336556673f, -4.6909227371f, -6.2935724258f, 3.7900424004f, 2.5683553219f, -0.1162255332f, 4.1622815132f, 0.1073398590f, -5.1608195305f, -8.2401542664f, -5.8381042480f, -7.4464135170f, -0.1182934269f, 3.2788193226f, 1.4351627827f, 6.3681168556f, 5.1041111946f, -0.1208729222f},
+ {4.9961571693f, 0.0185269248f, -0.2817080617f, -0.0505358987f, -0.2017169148f, 0.5220227242f, -9.6225223541f, 3.5301513672f, -1.2750257254f, -16.6149158478f, -0.2861652076f, -0.1112170741f, -2.9041886330f, -0.7071534991f, -1.5988026857f, -8.9261875153f, -14.3294153214f, 0.6273730397f, 1.6051287651f, -0.1642416716f, -3.4014506340f, 0.0300757475f, 6.7346963882f, -1.6067913771f, 1.6891504526f, -3.9646015167f, -0.0779341832f, 4.6224765778f, -0.5721715689f, -5.5780882835f, -0.1785165519f, 0.0633503497f},
+ {0.1060310677f, -0.5470390320f, -0.2241495699f, -0.4295381904f, -0.0263416618f, 0.2030417621f, -0.1120280996f, -0.0827973634f, -0.3317763507f, 0.0817295536f, 0.0069632535f, -0.1319767535f, 0.9747350216f, 0.2557510436f, 0.1425391138f, 0.1720905155f, -0.0782878473f, -0.0193020143f, 0.1281307042f, -0.2256549150f, 0.1310895681f, -0.5182238221f, 0.3424457908f, -0.1979330033f, 0.1891248226f, -0.0348026715f, 0.0921394229f, -0.0947700441f, -0.0883120522f, 0.0940800980f, 0.1856457591f, -1.7943713665f},
+ {-4.0017580986f, -0.9870634079f, -0.3691159189f, -0.4045964777f, -0.9596207142f, -4.4315557480f, -2.6966378689f, -2.4839437008f, 2.0009076595f, -2.3518767357f, 0.0318453647f, -0.5925750136f, -8.7461175919f, 1.2080634832f, -0.9580079317f, -1.3870022297f, -2.9447979927f, -4.2511048317f, -0.7817761898f, -0.3507715464f, 0.2093277872f, -0.2879707217f, 0.9649841189f, -10.7083244324f, -7.3718595505f, 1.7227441072f, -0.2694068253f, -2.7931368351f, 1.4890755415f, 0.6705868244f, 1.4951418638f, -0.1209532097f},
+ {-0.2123307288f, -0.3103626966f, -0.2075639665f, -0.0577281974f, -0.0194090679f, 0.2053133696f, -0.0279764533f, -0.1785610318f, -0.0686176419f, 0.2033739239f, -0.1825354397f, -0.0740804151f, 0.2677070498f, 0.2260797173f, -0.3779144287f, 0.0846814588f, 0.2516773045f, 0.1258523315f, 0.0956230313f, -0.1808306128f, 0.3722532988f, -0.1553636193f, -0.0801539496f, 0.2497462928f, -0.1287349761f, -0.1542355567f, -0.2239110470f, -0.0359012038f, 0.0341420211f, 0.1991766691f, -0.1235702857f, -0.0031303430f},
+ {6.9049019814f, -0.6181007624f, -0.1168652996f, -0.3459298015f, 0.1081510484f, -0.6562979817f, 2.9601066113f, 4.0602450371f, -0.7127177119f, -0.0100999130f, 0.5118251443f, 0.2948195040f, -14.6811800003f, -0.1756128967f, -2.3726568222f, -2.2973806858f, 0.2021040469f, 3.7788739204f, 0.0394109413f, -0.5389692783f, -2.5069026947f, -0.1306917369f, 1.1527950764f, -4.1568374634f, -5.4282240868f, 1.0624045134f, 0.2346842736f, -7.4216456413f, -10.1741409302f, -5.2678761482f, -1.5980980396f, -0.4154963493f},
+ {-5.9673295021f, -0.4644198418f, -0.4291569293f, -0.2272513807f, -0.4974555075f, 2.6671023369f, -6.5609960556f, 0.3609015644f, 10.3618955612f, -2.0600147247f, -0.5190568566f, -0.2184191942f, -1.4022922516f, -1.4489669800f, -4.0840444565f, -4.2195286751f, -0.4894198477f, -2.7413768768f, -6.3571834564f, -0.1267714947f, -0.8546049595f, -0.2224794775f, -0.3131478429f, -0.9897703528f, -6.1158819199f, 0.3078280985f, -0.4307037592f, -14.2193670273f, -0.3561481237f, -9.1594533920f, -0.7577811480f, -0.3548552096f},
+ {-6.2391667366f, -0.1269952357f, -0.2936189175f, -0.9923886061f, 0.0176051259f, -1.9780510664f, 0.0654375777f, -1.7040052414f, -0.1822838187f, -1.7437926531f, -1.0801513195f, -0.7526912093f, -5.0536708832f, -0.7413324714f, -2.9236466885f, -6.1675448418f, -0.9600590467f, -2.5335817337f, -1.1323231459f, -0.8290102482f, 0.5944318175f, -0.0634517670f, 0.9594070911f, 1.3713343143f, -3.9021794796f, -0.7598190904f, -0.7681146860f, -1.0350772142f, 0.8626580238f, -1.0701311827f, 0.6357883215f, -0.1052244753f},
+ {0.0669496581f, 0.1127517670f, -0.0209338553f, -0.0054164180f, -0.1271978915f, 0.0695113614f, -0.3660996258f, -0.7821827531f, -0.2065943033f, 0.2308539152f, 0.4742313325f, 0.1034071371f, -0.0490562394f, 0.4140049219f, -0.0322051346f, -0.0112497425f, 0.0862999111f, -0.1826869249f, -0.0142615261f, -0.0710267127f, 0.0182412416f, 0.0835287198f, -0.0485102497f, -0.1113768369f, -0.3792001903f, -0.0910689160f, 0.0768057555f, -0.4180118442f, -0.0000109978f, -0.0632229522f, 0.0589419529f, 0.1596196741f},
+ {0.3151653707f, -0.4588074088f, -0.3387854099f, 0.1461272687f, 0.0001908212f, -0.0586236343f, -0.8286342025f, 0.2795176804f, 0.4374869168f, 0.0628984645f, -0.2587254047f, 0.2453358322f, -0.6134222746f, 0.0773593485f, 0.2486565858f, -0.5328485370f, 0.6034097075f, -0.3007964790f, -0.2765316963f, -0.2508219779f, -0.2341709882f, 0.0143393278f, 0.1970690787f, -0.2759173810f, -0.7320910692f, -0.2636430860f, -0.0779949948f, -0.5681775808f, 0.0010494586f, 0.3486377001f, 0.5590596199f, -0.1548434943f},
+ {0.2904222906f, 0.2120339572f, -1.5837268829f, 0.1351595223f, -1.0482976437f, 0.6585662961f, 1.2837238312f, -8.8914051056f, 0.1900334507f, -6.2876024246f, -1.1750501394f, -0.9563908577f, -6.0589551926f, -0.0554546751f, -2.5099968910f, 0.2854703367f, 0.5719563365f, -0.4985203147f, -0.8484790921f, -0.2796662748f, -2.6506903172f, -0.0737289563f, 0.1099997908f, -6.8096685410f, -9.5710783005f, -3.5462114811f, -0.3454555869f, -4.1742453575f, -6.2087631226f, 1.0913355350f, -5.9188799858f, -0.0447143987f},
+ {-0.1762033552f, -0.9464151859f, -0.3476518989f, -0.1438073069f, -0.1068934649f, -2.6918101311f, 13.4009866714f, -0.0717068240f, -1.2019355297f, -2.8347525597f, 0.1641165912f, -0.0925235450f, 4.8379797935f, 2.1199939251f, -0.3954177201f, -1.5702075958f, 0.0889840126f, -1.7895882130f, 1.9846193790f, -0.0691496804f, -6.4447302818f, 0.1634601355f, 9.6448106766f, -0.7868277431f, 2.1918652058f, -0.5956330895f, -0.4198028147f, 2.8396642208f, 3.0820310116f, 3.5346202850f, 1.1110432148f, 0.1005667076f},
+ {0.2411281914f, 0.1411192566f, -0.0250760857f, -0.1230037659f, 0.6549731493f, 0.2007940114f, -1.5664811134f, 0.3535908461f, 0.6514756680f, -0.3146148622f, 0.0244260952f, -0.1043881625f, 0.5631551743f, -0.1026171371f, -0.2293619066f, 0.0751082525f, 0.1610344499f, 0.3488910794f, -0.3952502310f, 0.0538705476f, -0.1643067747f, -0.0639704242f, 0.1503835022f, 0.1106679291f, 0.3851269782f, 0.3049305081f, 0.2240979224f, 0.4056433737f, -0.3284457624f, -0.4254478812f, -0.0813698918f, -1.5299969912f},
+ {4.3450713158f, -0.4628526270f, -0.1491935551f, -0.2041818649f, -0.5786065459f, -1.2504427433f, -0.2758893967f, -0.3101694584f, 2.9785020351f, -4.7803659439f, -0.3506944776f, -0.9885157347f, -7.2183914185f, -0.1302880943f, -2.8009421825f, -0.6073452830f, -0.8039718270f, -12.0689706802f, -0.7140820026f, -0.3246946931f, 2.0155956745f, -0.3901022673f, 3.3020424843f, -0.5596037507f, 0.5555099249f, 0.2735705674f, -0.2914109826f, -14.4507541656f, -17.0698108673f, -4.3910245895f, -5.2524790764f, -0.2280410379f},
+ {-0.0042843656f, -0.1069235355f, 0.0691635162f, 0.0867756158f, 0.2937769592f, 0.0736219883f, -0.1853623986f, -0.1654763520f, 0.2185866535f, 0.1437660605f, -0.2649201155f, -0.1712868661f, -0.0538765527f, -0.1329404116f, 0.0423769951f, 0.3043175936f, 0.2732996941f, 0.0199668072f, 0.2512804568f, 0.1751035899f, -0.2383348942f, -0.2895450890f, -0.0679779947f, -0.2974975705f, 0.1156523153f, -0.2165189683f, 0.2342301458f, -0.1439705789f, 0.3337920308f, 0.2263115346f, 0.2996229827f, 0.1000883803f},
+ {-0.0903539732f, -0.2626895308f, -0.2928705215f, -0.1522098780f, -0.1990660727f, 0.1050468609f, 0.0752090216f, -0.0103869336f, 0.0290090404f, -0.2140946537f, -0.3905937076f, -0.0324644707f, 0.1388606578f, -0.0920412987f, 0.2089990675f, -0.0512218550f, 0.0448999256f, -0.0192409158f, 0.0232043508f, -0.1123628542f, 0.1863525957f, -0.1689641625f, -0.0414763764f, -0.0482044294f, 0.1756522357f, 0.3836692274f, -0.2097899765f, 0.0034760062f, 0.3211987317f, -0.1981586516f, 0.0146787502f, 0.1057996526f},
+ {-0.5354076028f, -0.3253239095f, -0.1333258152f, -0.0967850685f, 0.0488308556f, 1.7821354866f, -2.4696009159f, -1.0925548077f, 4.8946523666f, -5.5363497734f, -0.0556999668f, -0.3859304488f, -0.4668995738f, 1.1459211111f, 0.0986527577f, -5.5068421364f, -2.1916167736f, 0.9777641892f, 1.2964626551f, -0.1227272600f, -2.6892449856f, 0.2693175077f, 2.4651014805f, -0.2143563330f, -8.1490335464f, -1.1221495867f, -0.2091181278f, -2.5353107452f, 1.5514105558f, -0.8118920326f, -0.0759748369f, 0.0815865844f},
+ {-1.1687350273f, 0.0523806289f, 0.1928860396f, -0.1731245518f, -0.0422611423f, -3.5962536335f, 2.1258342266f, 0.0682783425f, -0.6722916365f, 0.6611677408f, -0.4290418625f, -0.2772609890f, 0.0911868215f, -0.2093296349f, -2.5827946663f, 4.6710624695f, 2.0906822681f, 0.5159549713f, 0.3715068996f, -0.2340186238f, -0.4284092188f, 0.0376657546f, -5.7211947441f, -1.1870640516f, 2.2118372917f, -8.0735073090f, -0.0517498069f, -4.1637449265f, -17.3150138855f, 3.2159905434f, -5.9302067757f, -0.0362711921f},
+ {-9.7952108383f, -0.6339132190f, -1.7921937704f, -0.3130472600f, -0.7404204011f, -1.6049377918f, 0.1309763938f, -7.6337146759f, 0.4408401549f, 3.3682234287f, -0.7966508269f, -0.0931789875f, -4.3933353424f, -7.4179029465f, 0.2328766286f, -2.2703931332f, -11.0213117599f, -11.4555244446f, 0.8035743237f, -0.3479077518f, 2.3253028393f, -0.1451899409f, -0.3695332706f, -16.2041606903f, -3.2792532444f, 0.6592780352f, -0.4765171707f, 1.1891194582f, -0.2045537680f, -0.2613292336f, -7.1603960991f, -0.0806212798f},
+ {-0.2485560924f, -0.5306785703f, -0.2062491179f, -0.1363802254f, -0.0375753008f, -0.2761560082f, -0.2498460114f, -0.0567790791f, -0.4666529298f, -0.0051496779f, 0.0906768441f, 0.3301909864f, 0.0531814285f, 0.2584980726f, 0.0183127709f, 0.1766013205f, 0.1447227001f, -0.1579496711f, 0.0043631280f, 0.1031887531f, 0.0396930017f, -0.0095448559f, 0.1428825110f, -0.0905142054f, 0.0705604851f, -0.7297466397f, 0.1558683962f, -0.0418609716f, 0.1233214065f, 0.1279907525f, 0.1352362633f, -0.0770157427f},
+ {-0.0049988753f, 0.1279107481f, 0.0449896753f, 0.6752018332f, -0.2889954150f, 0.4311439991f, -0.3787408769f, -0.4591981173f, -0.1940811723f, -0.0184744801f, 0.0441148244f, -0.1790693253f, -0.7149969935f, -0.0285517797f, -0.1489535421f, -0.5780433416f, 0.5109094977f, 0.6176788211f, -0.3211218417f, 0.1074986085f, 0.2505583465f, -2.5777461529f, 0.3480081260f, -0.8229233623f, -0.6535633206f, 0.5414181352f, 0.8112808466f, -0.3283018768f, 0.4965822697f, 0.8790674806f, 1.1569755077f, 0.2319966406f},
+ {-0.2767451704f, 0.2554717064f, -0.2503568828f, 0.2911026180f, -0.0896157026f, 0.0468162894f, -1.6308424473f, -0.2833184302f, -1.0214830637f, -0.1677722186f, 0.0416059718f, -0.0212873127f, -1.5793803930f, -0.0655662566f, -0.2267671078f, 0.3937034011f, 0.0103700571f, 0.1117373928f, 0.3512356877f, -0.1141320691f, 0.2422832102f, -3.8471693993f, -0.6802755594f, 0.1919109225f, -0.2617484927f, -0.5890094638f, 0.0800860524f, 0.1804080307f, -0.0786418170f, 0.2081480175f, -0.2514747679f, -1.6461024284f},
+ {-4.4110560417f, 0.0582131147f, -0.2296621650f, 0.0098368116f, -0.3111234009f, -0.7137008309f, 2.6443011761f, -5.5492348671f, -4.7353625298f, -1.0368877649f, -0.4160888791f, -1.2896853685f, 3.9724817276f, -10.6603651047f, 2.2762250900f, -3.0436787605f, -8.0274572372f, -1.4319875240f, -3.7743313313f, -0.0725755915f, -0.0383329280f, -0.3231822848f, 0.4311948717f, -2.6726655960f, -0.5350571871f, -1.4796928167f, -0.1884928495f, -5.7419962883f, -0.9663215280f, 0.1886327565f, -8.7959461212f, -0.1736448258f}
+};
+
+
+const float fc3Bias[32] =
+{-11.7193555832f, -0.6158396602f, -0.5767308474f, -0.4492973387f, -0.4815680087f, -1.5243271589f, -28.5404338837f, 4.9779524803f, -48.1950645447f, 5.1014094353f, -0.8038403392f, -0.7135465741f, 7.9977378845f, -3.9328453541f, -22.0385246277f, 0.3583163023f, 1.1570060253f, -13.7082090378f, 2.4488532543f, -0.4611511827f, -7.0892620087f, -0.3534394205f, -4.1933374405f, -13.2186746597f, -16.0344810486f, 5.2995457649f, -0.7092553973f, -11.6922359467f, 5.9151840210f, -4.7581868172f, -5.7193002701f, -0.4998539686f};
+
+
+const float fc4Weights[32][16] =
+{
+ {0.1313932389f, -0.6613871455f, 0.0097358534f, -0.1018039361f, -0.4058391750f, 0.4376264215f, -1.2572003603f, -3.6500022411f, -0.4249550700f, -0.2536546886f, -0.8922878504f, -0.4847670496f, -0.5938788652f, 0.1258193851f, -0.2272187024f, -0.1410650164f},
+ {0.0492341593f, 0.0943892971f, -0.5367519855f, 0.2006352544f, -0.3140752912f, 0.3790577352f, 0.5460243821f, 1.5730974674f, 0.4506726563f, -0.3239695728f, 0.1827834398f, -0.2436640859f, 0.4437866211f, 0.3330729902f, -0.2623860538f, -0.2274987251f},
+ {0.3261446357f, -0.5036940575f, -0.0645739883f, 0.1797081381f, -0.0971707106f, 0.7298631668f, -0.4873719513f, -0.1038509980f, -0.8186036348f, -0.0861302018f, 0.0887329206f, -0.2477209568f, -0.4017193019f, -0.2803779542f, 0.5708403587f, 0.7890117168f},
+ {-0.4605375826f, -0.3520908654f, 0.4579149187f, -0.6693316698f, 0.3497380316f, 0.2658064663f, -0.1681881249f, -0.3272072375f, -0.0845411718f, 0.2394114435f, -0.0101059023f, 0.4046859443f, -0.7417863607f, 0.4150537550f, 0.0869048312f, 0.0200589113f},
+ {-0.7960591316f, 0.7115456462f, 0.5414740443f, 0.2834898829f, -0.1049120426f, 1.2471122742f, 0.9764256477f, -0.2314370126f, 0.4565517008f, -0.1377106160f, 0.0066791684f, -0.0875304118f, -0.3973374367f, -0.4656803310f, -1.2865468264f, -0.2754203975f},
+ {-0.1267081201f, -0.3586117029f, -0.4626653492f, 0.0707737282f, -0.4280650318f, 0.3731042147f, -1.5224643946f, -0.0001562309f, -0.5457718372f, -0.8392512798f, -0.3297198415f, -3.6849846840f, -0.3235607445f, -0.4571735561f, -2.5906243324f, -0.0216693431f},
+ {-0.1975702047f, -0.6478233337f, -1.0000374317f, -0.7892091870f, -0.6129112840f, 0.4033009410f, -1.0652874708f, -5.0731873512f, -0.8335696459f, -0.0490954109f, -4.0255231857f, -2.5913162231f, -0.0984867662f, -0.3165186942f, -1.6619985104f, -0.1019358039f},
+ {-0.0070813317f, -1.4723222256f, -0.1055053324f, -0.0588658154f, -0.0424999259f, -2.5744652748f, -1.9830906391f, 1.0004559755f, 0.7691277862f, -0.1811856925f, -0.2902224660f, -0.2105268091f, 1.2195326090f, -0.0701754391f, -0.4191590548f, 0.0362841599f},
+ {-0.1953794360f, -0.7642983794f, -0.5502391458f, -0.6046341658f, -0.0868658274f, -1.0930783749f, -0.4096246660f, -0.2226173431f, -0.9467145205f, 0.0366492942f, -1.0827271938f, -1.3997668028f, 0.1914290041f, -0.1873319149f, -0.3942896426f, 0.0503584519f},
+ {-0.0302787200f, -5.7183461189f, 0.0836869702f, -0.1351985931f, 0.1218350157f, -0.0037351907f, 0.9376742840f, 0.5179937482f, -1.2707405090f, -0.2760854661f, 0.9394776821f, -0.6231134534f, -0.5011545420f, -1.0431542397f, -0.3772255182f, -0.9186507463f},
+ {0.5151054263f, -0.1792936772f, 0.5858933330f, -0.0756824464f, -0.1400850862f, 0.3227327764f, -0.0678724647f, 0.6420112848f, 0.7452553511f, 0.0104118586f, 0.1161525100f, -0.9172155261f, -0.3198927939f, 0.0625916570f, 1.1083164215f, 0.5252248645f},
+ {0.0453939959f, 0.0149580799f, -0.6745067239f, 0.2029461563f, 0.2238395065f, -0.2415017635f, -0.0304794274f, 0.0479286723f, -0.3008814454f, -0.2162462026f, -0.3752769828f, 0.7910355926f, -0.1502458900f, 0.2189511806f, -0.4534323215f, 0.2343945056f},
+ {-0.0660357103f, 0.5701023936f, -0.7573124170f, -0.4243010581f, -0.3647210896f, 0.9252396226f, -2.1346116066f, -1.5707424879f, -1.0428055525f, 0.0289310124f, -2.2915866375f, -2.0114760399f, -1.4218503237f, -0.6069518924f, -3.6163923740f, -1.0943173170f},
+ {-0.3510537148f, 0.0646613464f, -0.1941086054f, -0.3585000038f, -0.6358650923f, 0.4817451835f, 0.1834470332f, 1.5077445507f, 0.1200657785f, 0.0157124996f, 0.0893150121f, 0.2747304440f, -0.8162755370f, -2.2185630798f, -0.7672387958f, -0.7171642184f},
+ {-0.2655964494f, -0.6097341180f, 0.2751416266f, 0.0416029692f, -0.3487569392f, -4.3967308998f, -0.0399694182f, -0.5985686183f, -0.3526073098f, -0.0124356151f, -0.7412110567f, 0.3568145633f, 2.2140672207f, 0.3069616854f, -0.3047554493f, -0.9152814150f},
+ {-0.3112998009f, -1.2923207283f, -0.2642335892f, 0.1059029102f, -0.6062253118f, 1.7047382593f, -0.2192801982f, -7.2568459511f, -0.6452113390f, -0.1957286149f, 0.4478967488f, 2.1868736744f, 1.0964838266f, -0.5934657454f, -0.9350823164f, 0.1335169673f},
+ {-0.1445592940f, -3.8598067760f, -0.0888997465f, -0.4536363184f, -0.0818780512f, -0.5499716997f, 0.8879400492f, 0.3036511838f, -2.1018016338f, -0.3131541610f, -2.5331704617f, -2.7884752750f, -1.5264098644f, -0.2974909842f, -0.3884962499f, -1.6347447634f},
+ {-0.1576830745f, -1.0188829899f, -0.1347837746f, 0.0133346859f, -0.1020436883f, 0.9006328583f, 0.4465196133f, -1.9912528992f, -1.1890530586f, -0.2928483784f, -1.1970791817f, -1.0793333054f, -0.6542327404f, 0.0027635724f, -0.7239921689f, -0.1350315958f},
+ {-0.0101720933f, 0.8065249920f, -0.2725579739f, -0.5590948462f, -0.6773648858f, -0.9759730697f, 1.1105380058f, -0.9370074272f, -1.0632905960f, -0.2358790785f, -2.6182923317f, -0.5131548643f, 0.3938317299f, -0.6527919173f, -0.5773690343f, -2.7888607979f},
+ {0.9983167648f, 0.0334987827f, 0.1105532423f, -0.4079656899f, -0.1334102899f, 0.1476265639f, 0.0125124194f, -0.2545211911f, -0.3887112439f, -0.3177830279f, -0.3589894176f, 0.1081808954f, -0.3721842766f, 0.0556281544f, -0.0388795845f, 0.2037374824f},
+ {-0.4827512801f, -0.2132459283f, -0.9375121593f, -1.3464082479f, -0.5630852580f, 0.0437785275f, -6.4064550400f, -2.0924882889f, -0.2812258899f, -0.5477465987f, -1.5397117138f, -4.5946688652f, -0.4861118793f, -0.7390319705f, -1.4275629520f, -1.5372459888f},
+ {0.0196762551f, -0.0118051507f, 0.2246439159f, -0.2613828778f, -0.5333120823f, -0.1223717630f, -0.1976639330f, -0.1101496667f, -0.0134799089f, 0.0676583350f, 0.2612578273f, 0.1215140149f, 0.2203066051f, 0.2337986380f, 0.1590026319f, -0.0366739817f},
+ {-2.0189671516f, 0.4665688872f, -0.8338592052f, -0.3301258981f, -0.4463534951f, -0.3213590384f, -0.2176769376f, -0.7976086736f, -1.4433599710f, -0.9578914046f, -2.3176016808f, -3.3081896305f, 0.1362658292f, -0.9325780272f, -7.5809931755f, -6.1999273300f},
+ {-0.2162996083f, -1.1839522123f, -1.3205081224f, -0.0846010596f, -0.2445309013f, -0.2086048126f, 0.8832997084f, 0.2233153582f, 0.6593582034f, -0.2484957576f, -0.5232675672f, -0.0044967751f, 0.1215033680f, -0.2973840833f, -0.0051185489f, 0.4212923348f},
+ {-0.3386851847f, -0.1535801440f, 0.2589180470f, -0.5164783001f, -0.0374755822f, 0.2122802138f, 0.2694437504f, -1.9194368124f, -0.4101884961f, -0.5941445827f, -1.8583507538f, -0.8249487281f, -1.2600407600f, -0.3161348701f, -1.8951197863f, -0.5458792448f},
+ {-0.5639970899f, 0.5414865613f, -0.9907631874f, -0.7151839733f, -0.3994543254f, -0.8817944527f, -0.5968976021f, -0.2674676180f, 0.4723920822f, -0.6860534549f, -1.4526910782f, -2.7732286453f, 0.6615548134f, -0.4297663867f, -0.3635542095f, -0.7897509336f},
+ {0.2338507622f, -0.0548883379f, -0.6124376655f, 0.3055166602f, -0.6472301483f, -0.0934592113f, -0.0791300610f, 0.0744187683f, -0.0494301580f, -0.0888935924f, -0.2844451666f, -0.3919087350f, -0.1899581403f, 0.0696015507f, -0.3865183890f, 0.1351541430f},
+ {-0.3884226382f, 0.6438637376f, -0.0235484447f, -0.4085457325f, -0.3678089380f, -0.3611978292f, -1.9616941214f, -0.6029480100f, -1.0217937231f, 0.0628737658f, 1.1272052526f, -0.7026557922f, -2.6631255150f, -0.0570945218f, -0.8197408915f, -0.0155918878f},
+ {-0.3045629263f, -1.2819237709f, -0.4050366580f, -1.0614979267f, -0.1641227305f, -1.2281309366f, -1.3765566349f, 0.6788370609f, -1.5758328438f, -1.0314040184f, -6.2401552200f, -3.9707064629f, 0.3741535544f, -0.5852952600f, 0.2783090174f, -0.1958702654f},
+ {-0.3730004132f, 0.3823136389f, -0.0965899155f, -0.1237012967f, -0.0356198289f, -0.2304152846f, -2.5172274113f, -0.2180451900f, -0.3073972762f, -0.2923755348f, -4.9185404778f, -3.6769018173f, -0.4479543269f, -0.0074586943f, 0.7228356004f, -0.3836637735f},
+ {-1.5993598700f, -1.3894209862f, 0.0539587401f, -0.3277336061f, -0.2855097055f, -3.6847970486f, -4.5063147545f, -1.4260863066f, -3.9039046764f, -1.1701396704f, -1.8200896978f, -2.4587380886f, 0.2993653715f, 0.0570639670f, -2.6918485165f, -1.4997549057f},
+ {0.1219597459f, 0.0154820858f, 0.3974801600f, 0.3066018224f, 0.1819918901f, 0.0161614101f, 0.0640004650f, 0.1952118576f, -0.2755865157f, 0.0411446691f, 0.0500190333f, 0.1724539101f, -0.1740499139f, 0.3841970265f, 0.3317279220f, 0.4177183211f}
+};
+
+
+const float fc4Bias[16] =
+{-0.5141325593f, -0.8478075862f, -1.0021078587f, -1.7178856134f, -0.9628381133f, 0.1455254108f, -3.6562759876f, 1.7083742619f, -8.0320730209f, -0.9758637547f, -3.8376631737f, -0.1638645679f, 3.8785557747f, -1.0468775034f, -29.1870574951f, -6.1543283463f};
+
+
+const float fc5Weights[16][8] =
+{
+ {-0.0377756208f, -0.0376064293f, -0.0772271156f, 0.5590630174f, -0.3684624434f, 1.0009193420f, 1.3445713520f, 0.5627732277f},
+ {-0.4041293859f, 0.1303688884f, -0.3772308826f, -0.8144312501f, -0.3996099234f, -0.8786695004f, -0.1038818136f, -0.6491676569f},
+ {-0.1548538208f, -0.4342842698f, -0.3643324077f, 0.7171005011f, -0.2240766436f, -0.3029537201f, 0.0784914643f, 0.2668318450f},
+ {0.0582836233f, 0.1667208225f, 0.1605709791f, -0.8526514769f, -0.0394123383f, 0.3542331457f, -0.0112714395f, -0.8478502035f},
+ {0.1886940002f, 1.4096331596f, -0.0266252328f, 0.1160744503f, 1.3083670139f, -0.4767547250f, -0.7634902000f, -1.2924789190f},
+ {-0.3800563812f, -0.2717871964f, -0.2375701517f, -0.8516035080f, -0.3561956882f, -0.5633419156f, 0.1343641281f, -0.8182802796f},
+ {-0.4533548355f, 0.4980821609f, -0.0113001689f, -0.6163378358f, -0.0099237291f, -0.0464528799f, 1.4334065914f, -0.1762148142f},
+ {-0.4913391173f, 0.1923918128f, -0.5117462277f, -0.5446628928f, -0.7394183278f, 0.1009852663f, -0.2048954815f, -0.3028838933f},
+ {0.0514116511f, -0.1429781914f, -0.2054089010f, -0.3337486684f, -0.9298329949f, -0.0842700675f, -0.9120995402f, -1.0035445690f},
+ {0.1642776728f, -0.1338489354f, -0.3949478865f, 0.2676415145f, 0.2334752530f, 0.1165231466f, 0.3371087313f, -0.0303090699f},
+ {-0.0316087417f, -0.3532579839f, -0.5091804862f, 0.0389130265f, -0.1303991377f, -1.4411540031f, -0.0542913377f, -0.6798272133f},
+ {-0.0166958608f, -0.3853805959f, -0.0088427681f, -0.1329081804f, -0.1517693698f, -0.7518531680f, -0.0679639205f, 0.1632424593f},
+ {-0.2969943583f, 0.2169936597f, -0.2704928219f, -0.5222067237f, -0.2086613327f, -0.5919396281f, -0.1555189341f, -0.5921875238f},
+ {0.3620398343f, -0.0771873146f, -0.4576121271f, 0.4077250361f, 0.1402812898f, -0.3508163095f, -0.0904585123f, 0.1637402475f},
+ {-0.3381429911f, -0.9172439575f, -0.2056993395f, -0.0469768532f, -0.0229178257f, 0.0243457071f, -0.1943701357f, -0.0122073563f},
+ {-0.2032035887f, -0.0370536968f, -0.2462941855f, 0.3719790280f, 0.1286954135f, 0.1040267125f, 0.2068356276f, 0.0400213264f}
+};
+
+
+const float fc5Bias[8] =
+{-0.4388153255f, -2.2450299263f, -0.3559921980f, -0.9536105990f, -1.0454660654f, -0.8810431957f, 1.6276029348f, -0.6507120728f};
+
+
+const float fc6Weights[8][1] =
+{
+ {-0.5150820613f},
+ {0.1128230840f},
+ {0.0093749743f},
+ {-0.4838168621f},
+ {0.2524490356f},
+ {-0.4693250954f},
+ {-0.1718080938f},
+ {-0.4312117100f}
+};
+
+
+const float fc6Bias[1] =
+{0.0394757539f};
+
+
+#endif
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/test/CMakeLists.txt b/source/RobotAPI/libraries/KITGripperEtherCAT/test/CMakeLists.txt
new file mode 100644
index 000000000..8ac96acdc
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore ImagineEtherCAT)
+
+armarx_add_test(ImagineEtherCATTest ImagineEtherCATTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/test/ImagineEtherCATTest.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/test/ImagineEtherCATTest.cpp
new file mode 100644
index 000000000..8d9d97624
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/test/ImagineEtherCATTest.cpp
@@ -0,0 +1,48 @@
+/*
+ * 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 ImagineRT::ArmarXObjects::ImagineEtherCAT
+ * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE ImagineRT::ArmarXLibraries::ImagineEtherCAT
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h>
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ TIMING_START(NN_Calc);
+ float t = 0;
+ int iterations = 1000;
+ for (int i = 0; i < iterations; i++)
+ {
+ t = estimateTorque(rand() % 800, rand() % 3000);
+ ARMARX_INFO << VAROUT(t);
+ }
+ TIMING_END(NN_Calc);
+ BOOST_CHECK_EQUAL(true, true);
+}
--
GitLab