From 02d08ef4dc5c1ae64dcbe89f8f2bb620505a7047 Mon Sep 17 00:00:00 2001
From: Andre Meixner <andre.meixner@kit.edu>
Date: Wed, 9 Oct 2024 18:16:25 +0200
Subject: [PATCH] Added torqueFactor and torqueOffset for hardware config of
elbow roll joint for ARMAR-7
---
.../sensor_actor_unit/armar7_wrist/sensor_board/Config.cpp | 6 ++++--
.../sensor_actor_unit/armar7_wrist/sensor_board/Config.h | 3 +++
.../sensor_actor_unit/armar7_wrist/sensor_board/Data.cpp | 4 +++-
.../sensor_actor_unit/armar7_wrist/sensor_board/Data.h | 2 ++
4 files changed, 12 insertions(+), 3 deletions(-)
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Config.cpp b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Config.cpp
index 6364a1d7..ab7f2aa5 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Config.cpp
+++ b/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"))
{
}
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Config.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Config.h
index 5ab8ac90..5f9f63ff 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Config.h
+++ b/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;
};
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.cpp b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.cpp
index 3a84fbd8..476e8b63 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.cpp
+++ b/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
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.h
index 39885e9d..3e14ad33 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.h
+++ b/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;
--
GitLab