diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationNetwork.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationNetwork.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..52424a842e96c0ae0bc36f64463264fce566b822
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationNetwork.cpp
@@ -0,0 +1,60 @@
+#include "TorqueEstimationNetwork.h"
+#include "TorqueEstimationNetworkWeights.h"
+
+#include <algorithm>
+
+template <unsigned int layer_in_dim, unsigned int layer_out_dim>
+void TorqueEstimationNetwork::fully_connected_layer_sigmoid_forward(float (&layer_in) [layer_in_dim],
+ float (&layer_out) [layer_out_dim],
+ const float (&layer_weights) [layer_in_dim][layer_out_dim],
+ const float (&layer_bias) [layer_out_dim])
+{
+ std::fill_n(layer_out, layer_out_dim, 0.0f);
+ for (unsigned int outF = 0; outF < layer_out_dim; outF++)
+ {
+ for (unsigned int inF = 0; inF < layer_in_dim; inF++)
+ {
+ layer_out[outF] += layer_in[inF] * layer_weights[inF][outF];
+ }
+
+ //BIAS
+ layer_out[outF] += layer_bias[outF];
+
+ //SIGMOID
+ layer_out[outF] = std::max(0.0f, std::min(1.0f, layer_out[outF] * 0.2f + 0.5f));
+ }
+}
+
+template <unsigned int layer_in_dim, unsigned int layer_out_dim>
+void TorqueEstimationNetwork::fully_connected_layer_forward(float (&layer_in) [layer_in_dim],
+ float (&layer_out) [layer_out_dim],
+ const float (&layer_weights) [layer_in_dim][layer_out_dim],
+ const float (&layer_bias) [layer_out_dim])
+{
+ std::fill_n(layer_out, layer_out_dim, 0.0f);
+ for (unsigned int outF = 0; outF < layer_out_dim; outF++)
+ {
+ for (unsigned int inF = 0; inF < layer_in_dim; inF++)
+ {
+ layer_out[outF] += layer_in[inF] * layer_weights[inF][outF];
+ }
+
+ //BIAS
+ layer_out[outF] += layer_bias[outF];
+ }
+}
+
+float TorqueEstimationNetwork::feedforward(float vel_input, float pwm_input, float pwmXvel)
+{
+ layer_in[0] = vel_input;
+ layer_in[1] = pwm_input;
+ layer_in[2] = vel_input;
+ layer_in[3] = pwm_input;
+ layer_in[4] = pwmXvel;
+
+ fully_connected_layer_sigmoid_forward(layer_in, fcl1, fc1Weights, fc1Bias);
+ fully_connected_layer_sigmoid_forward(fcl1, fcl2, fc2Weights, fc2Bias);
+ fully_connected_layer_forward(fcl2, fcl3, fc3Weights, fc3Bias);
+
+ return fcl3[0];
+}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationNetwork.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationNetwork.h
new file mode 100644
index 0000000000000000000000000000000000000000..8c8e9aa8f3ad73c603c22bc57c453e034627627e
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationNetwork.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include <memory>
+
+class TorqueEstimationNetwork
+{
+public:
+ TorqueEstimationNetwork() {}
+ ~TorqueEstimationNetwork() {}
+
+ float feedforward(float vel_input, float pwm_input, float pwmXvel);
+
+private:
+ // Neural Network definition
+ float layer_in[5];
+ float fcl1[16];
+ float fcl2[16];
+ float fcl3[1];
+
+ template <unsigned int layer_in_dim, unsigned int layer_out_dim>
+ void fully_connected_layer_sigmoid_forward(float (&layer_in) [layer_in_dim],
+ float (&layer_out) [layer_out_dim],
+ const float (&layer_weights) [layer_in_dim][layer_out_dim],
+ const float (&layer_bias) [layer_out_dim]);
+
+ template <unsigned int layer_in_dim, unsigned int layer_out_dim>
+ void fully_connected_layer_forward(float (&layer_in) [layer_in_dim],
+ float (&layer_out) [layer_out_dim],
+ const float (&layer_weights) [layer_in_dim][layer_out_dim],
+ const float (&layer_bias) [layer_out_dim]);
+};
+using TorqueEstimationNetworkPtr = std::shared_ptr<TorqueEstimationNetwork>;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationNetworkWeights.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationNetworkWeights.h
new file mode 100644
index 0000000000000000000000000000000000000000..d11116bb1643b6b5af096b8c894196a25bdbcd2f
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationNetworkWeights.h
@@ -0,0 +1,65 @@
+[[maybe_unused]] static float vel_factor = 3.0;
+[[maybe_unused]] static float pwm_factor = 1700.0;
+
+const float fc1Bias[16] =
+{-1.993366f, -3.353783f, 2.0842178f, -2.108467f, -0.17377396f, -0.011586582f, -0.01995976f, -3.998212f, -2.3260808f, -0.7437165f, -1.334686f, -1.2316095f, -3.128098f, -2.1560073f, -2.8738499f, -1.2502924};
+
+
+const float fc1Weights[5][16] =
+{
+ {0.0019203589f, 0.011463187f, -0.0f, -0.0f, -0.0f, -0.0f, -0.0f, 0.12249695f, 957.8274f, -0.0f, -0.0f, -0.0f, 2.8007276f, -0.0f, 0.0025868164f, 7.358892},
+ {3.669117f, 2.6355286f, 1.3176162f, 1.4215765f, 0.002866666f, 0.0022693626f, 0.0020984258f, 0.76744515f, 0.17698947f, 1.1935575f, 2.7593822f, 0.99390316f, 2.8798153f, 0.0034996672f, 2.5799122f, 1.758198},
+ {11.595929f, 9.064497f, -0.0f, -0.0f, 1.9158126f, 2.0298774f, 2.032499f, 1.7281736f, 0.019053876f, 6.418544f, 7.2518983f, 4.0169854f, 5.687731f, 3.9053767f, 8.633225f, 1.142447},
+ {3.6807227f, 2.6383371f, 1.3143287f, 1.4018519f, 0.002866666f, 0.0022693626f, 0.0020984258f, 0.73717934f, 0.17698947f, 1.2059019f, 2.7258534f, 1.0029843f, 2.8440082f, 0.0034996672f, 2.5822191f, 1.7838416},
+ {0.03962015f, 0.07641957f, 0.09732962f, 0.011884165f, 0.093084514f, 0.019968498f, 0.016433358f, 0.06024412f, 0.07921968f, 0.021295143f, 0.045907795f, 0.051374473f, 0.073292986f, 0.070220105f, 0.019093573f, 0.018902898}
+};
+
+
+const float fc2Bias[16] =
+{-2.50055f, -3.0220435f, -2.505797f, -5.089169f, -5.3303022f, -3.2109323f, -2.4226182f, -10.111999f, -15.827857f, -13.4906645f, -2.5078464f, -8.573066f, -2.500719f, -7.8720303f, -2.5022852f, -4.463582};
+
+
+const float fc2Weights[16][16] =
+{
+ {-0.0f, -0.0f, -0.0f, 0.00088876125f, 0.00028149225f, -0.0f, -0.0f, 0.010856812f, 4.7823973f, 0.00818446f, -0.0f, 0.00024972018f, -0.0f, 0.01003965f, -0.0f, -0.0},
+ {-0.0f, -0.0f, -0.0f, 0.61162364f, 0.22457922f, 91.416245f, -0.0f, -0.0f, -0.0f, 7.895859f, -0.0f, 0.009150834f, -0.0f, 0.0031958844f, -0.0f, -0.0},
+ {-0.0f, 0.010659449f, -0.0f, 0.5190987f, 4.8779454f, 2.832136f, 4.571784f, 5.2758613f, 10.015845f, 6.8645496f, -0.0f, 4.5919137f, -0.0f, 5.019818f, -0.0f, 0.0024521255},
+ {-0.0f, -0.0f, -0.0f, 0.0008787768f, 3.1732256e-05f, -0.0f, 0.56419355f, 2.1883733f, 0.0010742147f, 7.8888288f, -0.0f, 0.00035522945f, -0.0f, 0.3440993f, -0.0f, 6.6156707},
+ {-0.0f, 0.29709733f, -0.0f, 2.117408f, 0.18019901f, 0.8663609f, -0.0f, 0.003267595f, 2.0086424f, 2.1174119f, -0.0f, 2.1622138f, -0.0f, 0.0034431864f, -0.0f, 2.6367278},
+ {-0.0f, 0.5256185f, -0.0f, 2.7272859f, 0.5847616f, 0.79404944f, -0.0f, 0.0031886457f, 4.410126f, 2.6712153f, -0.0f, 2.8573105f, -0.0f, 0.0036124773f, -0.0f, 2.6030123},
+ {-0.0f, 0.40544593f, -0.0f, 2.6838672f, 0.7222986f, 0.84556276f, -0.0f, 0.003241526f, 3.6613982f, 2.9627938f, -0.0f, 2.857706f, -0.0f, 0.0036016589f, -0.0f, 2.8247402},
+ {-0.0f, 0.972594f, -0.0f, 1.493246f, 0.7513589f, -0.0f, -0.0f, 1.0576926f, 0.05612972f, -0.0f, -0.0f, 1.2323714f, -0.0f, 0.53340423f, -0.0f, -0.0},
+ {-0.0f, 66.4835f, -0.0f, -0.0f, -0.0f, -0.0f, -0.0f, 1.6681877f, 3.0665662f, -0.0f, -0.0f, -0.0f, -0.0f, 0.033550937f, -0.0f, -0.0},
+ {-0.0f, 0.000534668f, -0.0f, 0.00020585256f, 0.0014423464f, -0.0f, 0.4236225f, 0.0052004205f, 2.8163059f, 0.012232317f, -0.0f, 0.00020056736f, -0.0f, 0.31105915f, -0.0f, 0.131825},
+ {-0.0f, -0.0f, -0.0f, 2.571936f, 0.00071555167f, -0.0f, 0.32076737f, 0.08852068f, 14.720435f, 0.0012971199f, -0.0f, 1.1486727f, -0.0f, 0.014174897f, -0.0f, 0.0009955773},
+ {-0.0f, -0.0f, -0.0f, 0.015745606f, 0.3207095f, 0.1343552f, -0.0f, -0.0f, 0.010236579f, 1.9739591f, -0.0f, 0.5383741f, -0.0f, 0.0045692073f, -0.0f, 0.45998842},
+ {-0.0f, -0.0f, -0.0f, 3.4014232f, 0.68705267f, 9.01106f, 0.0006072507f, 5.171076f, 25.952158f, 7.0910916f, -0.0f, 0.001092497f, -0.0f, 0.057861365f, -0.0f, 102.66719},
+ {-0.0f, 21.953018f, -0.0f, 0.3036113f, 6.321213f, -0.0f, -0.0f, 0.53539693f, -0.0f, 2.9014556f, -0.0f, 3.3248265f, -0.0f, 7.0556455f, -0.0f, 0.0028456543},
+ {-0.0f, -0.0f, -0.0f, 0.009063761f, 0.007869589f, -0.0f, -0.0f, 0.0031327044f, 5.0298448f, 5.120441f, -0.0f, 0.0059641735f, -0.0f, 0.0129446415f, -0.0f, 66.358406},
+ {-0.0f, -0.0f, -0.0f, -0.0f, -0.0f, -0.0f, -0.0f, 0.004904504f, 3.8093722f, 0.00032608406f, -0.0f, -0.0f, -0.0f, 0.0010990424f, -0.0f, -0.0}
+};
+
+
+const float fc3Bias[1] =
+{-9.735222};
+
+
+const float fc3Weights[16][1] =
+{
+ {-0.0},
+ {1.0945561},
+ {-0.0},
+ {0.96704805},
+ {0.62445843},
+ {0.32226333},
+ {3.4995396},
+ {4.7148805},
+ {2.8875463},
+ {2.0148194},
+ {-0.0},
+ {2.3068774},
+ {-0.0},
+ {1.4982132},
+ {-0.0},
+ {1.2723411}
+};
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/convertToCppHeader.py b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/convertToCppHeader.py
new file mode 100644
index 0000000000000000000000000000000000000000..bc3c01af2e8f2152823d664c6139f2d76d2ecf8b
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/convertToCppHeader.py
@@ -0,0 +1,50 @@
+import h5py
+
+
+def traverse_datasets(hdf_file):
+ def h5py_dataset_iterator(g, prefix=''):
+ for key in g.keys():
+ item = g[key]
+ path = f'{prefix}/{key}'
+ if isinstance(item, h5py.Dataset): # test for dataset
+ yield (path, item)
+ elif isinstance(item, h5py.Group): # test for group (go down)
+ yield from h5py_dataset_iterator(item, path)
+
+ with h5py.File(hdf_file, 'r') as f:
+ for path, _ in h5py_dataset_iterator(f):
+ yield path
+
+def oned1array(array):
+ return f'{{{"f, ".join(str(x) for x in array)}}}'
+
+def run(filename):
+ with h5py.File(filename, 'r') as f:
+ print("namespace torqueEstimation")
+ print("{")
+
+ for dset in traverse_datasets(filename):
+ #print('Path:', dset)
+ #print('Shape:', f[dset].shape)
+ #print('Dimension:', len(f[dset].shape))
+ #print('Data type:', f[dset].dtype)
+
+ if len(f[dset].shape) == 1:
+ print(f'\tconst float {dset}[{f[dset].shape[0]}] =')
+ print(f'\t\t{oned1array(f[dset])};')
+ print("\n")
+ if len(f[dset].shape) == 2:
+ print(f'\tconst float {dset}[{f[dset].shape[0]}][{f[dset].shape[1]}] =')
+ print("\t{")
+ arrays = []
+ for array in f[dset]:
+ arrays.append(oned1array(array))
+ limiter = ", \n\t\t"
+ print(f'\t\t{limiter.join(str(arr) for arr in arrays)}')
+ print("\t};")
+ print("\n")
+
+ print("}")
+
+if __name__ == "__main__":
+ run("NN_T_predict_nonneg_hard_sigmoid_16.h5")