Added torqueFactor and torqueOffset for hardware config of elbow roll joint for ARMAR-7

source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Config.cpp

@@ -29,8 +29,10 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sensor_board
         voltageRef(hwConfig.getFloat("elbowTorque.voltage_ref")),
         sensitivity(hwConfig.getFloat("elbowTorque.sensitivity")),
         voltageSupplied(hwConfig.getFloat("elbowTorque.voltage_supply")),
-        voltageFactor(hwConfig.getMatrix<float, 4, 1>("elbowTorque.factor")),
-        voltageOffset(hwConfig.getMatrix<float, 4, 1>("elbowTorque.offset")),
+        voltageFactor(hwConfig.getMatrix<float, 4, 1>("elbowTorque.voltage_factor")),
+        voltageOffset(hwConfig.getMatrix<float, 4, 1>("elbowTorque.voltage_offset")),
+        torqueFactor(hwConfig.getFloat("elbowTorque.torque_factor")),
+        torqueOffset(hwConfig.getFloat("elbowTorque.torque_offset")),
         temperature(hwConfig.getLinearConfig("elbowTorque.temperature"))
     {
     }

source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Config.h

@@ -56,6 +56,9 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sensor_board
         Eigen::Matrix<float, NUM_VALUES, 1> voltageFactor;
         Eigen::Matrix<float, NUM_VALUES, 1> voltageOffset;
 
+        float torqueFactor;
+        float torqueOffset;
+
         hwconfig::types::LinearConfig temperature;
     };
 

source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.cpp

@@ -261,6 +261,8 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sensor_board
         unAmplifiedVoltageToTorqueFactor(1 / (config.sensitivity * config.voltageSupplied)),
         voltageFactor(config.voltageFactor),
         voltageOffset(config.voltageOffset),
+        torqueFactor(config.torqueFactor),
+        torqueOffset(config.torqueOffset),
         slaveOut(slaveOut),
         torqueTicks(Veci::Zero()),
         unAmplifiedVoltage(Vecf::Zero()),
@@ -315,7 +317,7 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sensor_board
     float
     ElbowTorqueData::getActualTorque() const
     {
-        return torques.mean() +2.75; // HACK FIXME REMOVE
+        return torqueFactor * torques.mean() + torqueOffset;
     }
 
     std::int32_t

source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.h

@@ -159,6 +159,8 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sensor_board
         const float unAmplifiedVoltageToTorqueFactor;
         const Vecf voltageFactor; 
         const Vecf voltageOffset; 
+        const float torqueFactor;
+        const float torqueOffset;
 
         sensor_board::SlaveOut* slaveOut;