diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt b/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt
index 80459c63fea24e1b4167117570081127a9581bcf..81eef4fb07077177511304025347cd4d61117738 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt
@@ -51,6 +51,7 @@ set(LIB_FILES
KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp
KITGripperBasisBoard/KITGripperBasisBoardData.cpp
KITGripperBasisBoard/KITGripperBasisBoard.cpp
+KITGripperBasisBoard/Misc/TorqueEstimation.cpp
KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
index ca62981433b1a07e729846848137806bb4282fe9..2387c60e07bbf8d54dded3d7c072cb57fc16ca09 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
@@ -32,7 +32,6 @@
#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>);
@@ -173,7 +172,13 @@ namespace armarx
auto zeroTorqueControllerCfg = PWMZeroTorqueControllerConfiguration::CreateConfigDataFromXml(
defaultConfigurationNode.first_node("JointPWMZeroTorqueControllerDefaultConfiguration")
.add_node_at_end(configNode.first_node("JointPWMZeroTorqueControllerConfig")));
-
+ torqueEstimator.reset(new MotorTorqueEstimator(
+ actorIndex,
+ actorDataPtr->getNonlinearModelEnabled(),
+ actorDataPtr->getMotorModelTypeIndex(),
+ actorDataPtr->getMotorInertia()));
+ // std::cout << "pos control enable: " << actorDataPtr->getPositionControlEnabled() << std::endl;
+ // std::cout << "motor type: " << actorDataPtr->getMotorModelTypeIndex() << std::endl;
// ARMARX_CHECK_EQUAL_W_HINT(
// configNode.has_node("ParallelGripperDecoupplingFactor"),
// configNode.has_node("SiblingConnectorId"),
@@ -267,13 +272,17 @@ namespace armarx
sensorValue.position = actorDataPtr->getPosition();
sensorValue.relativePosition = actorDataPtr->getRelativePosition();
sensorValue.velocity = actorDataPtr->getVelocity();
+ // TODO add new entry for acceleration
sensorValue.absoluteEncoderVelocity = actorDataPtr->getAbsoluteEncoderVelocity();
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);
+ torqueEstimator->estimateTorque(sensorValue.velocityTicksPerMs, sensorValue.targetPWM);
+ sensorValue.torque = torqueEstimator->getTorque();
+ sensorValue.torqueWithInertia = torqueEstimator->getTorqueWithInertia();
+ // TODO add new entry torqueWithInertial
}
void KITGripperBasisBoard::ActorRobotUnitDevice::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h
index 7e9b32ed5c4338ec6250391bd699813a2150ecbc..987930597f0cd3b5a4bf0ff6ce82da3e81e6181a 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h
@@ -32,6 +32,8 @@
#include <RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h>
#include <RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.h>
+#include "Misc/TorqueEstimation.h"
+
namespace armarx
{
using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
@@ -84,6 +86,9 @@ namespace armarx
/// The data object for copying to non-rt part
KITGripperActorSensorData sensorValue;
+ //estimated torque value
+ MotorTorqueEstimatorPtr torqueEstimator;
+
// SensorDevice interface
public:
@@ -129,7 +134,6 @@ namespace armarx
SlaveIdentifier slaveIdentifier;
std::vector<ActorRobotUnitDevicePtr > devices;
PWMVelocityControllerConfigurationPtr velocityControllerConfigDataPtr;
-
};
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
index a6530c67463588757065f62cad4e5215174122b7..28c0f7569ed29862a1e53c65d02bbd5f22f19dff 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
@@ -43,6 +43,9 @@ namespace armarx
add_node_at_end(motorNode.first_node("ConversionParameters"));
auto configNode = defaultConfigurationNode.first_node("KITGripperActorDefaultConfiguration").
add_node_at_end(motorNode);
+ // auto torqueEstimatorNode = motorNode.first_node("MotorTorqueEstimationConfig");
+ auto torqueEstimatorNode = defaultConfigurationNode.first_node("MotorTorqueEstimationDefaultConfig").
+ add_node_at_end(motorNode.first_node("MotorTorqueEstimationConfig"));
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;
@@ -66,6 +69,9 @@ namespace armarx
actorData.at(connectorIndex)->currentPWMBoundGradient = configNode.first_node("CurrentPWMBoundGradient").value_as_float();
actorData.at(connectorIndex)->currentPWMBoundOffset = configNode.first_node("CurrentPWMBoundOffset").value_as_int32();
+ actorData.at(connectorIndex)->motorModelType = torqueEstimatorNode.first_node("motorModelTypeIndex").value_as_string();
+ actorData.at(connectorIndex)->nonlinearModelEnabled = torqueEstimatorNode.first_node("nonlinearModelEnabled").value_as_bool("1", "0");
+ actorData.at(connectorIndex)->motorInertia = torqueEstimatorNode.first_node("motorInertia").value_as_float();
// ARMARX_IMPORTANT << "get decoupling factor: " << actorData.at(connectorIndex)->parallelGripperDecouplingFactor << " and enabled: " << actorData.at(connectorIndex)->parallelControlEnabled;
};
switch (connectorIndex)
@@ -337,4 +343,19 @@ namespace armarx
return absoluteEncoderVelocity;
}
+ std::string ActorData::getMotorModelTypeIndex() const
+ {
+ return motorModelType;
+ }
+
+ bool ActorData::getNonlinearModelEnabled() const
+ {
+ return nonlinearModelEnabled;
+ }
+
+ float ActorData::getMotorInertia() const
+ {
+ return motorInertia;
+ }
+
} // namespace armarx
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
index c541417722afafbb5ba7d659f151336e5d782f87..bdab8fb834a09f12e17e5679329e5c93cf53dfe6 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
@@ -58,6 +58,7 @@ namespace armarx
float absoluteEncoderVelocity;
int32_t maxPWM;
int32_t minPWM;
+ float torqueWithInertia;
static SensorValueInfo<KITGripperActorSensorData> GetClassMemberInfo()
{
@@ -70,6 +71,7 @@ namespace armarx
svi.addMemberVariable(&KITGripperActorSensorData::minPWM, "minPWM");
svi.addMemberVariable(&KITGripperActorSensorData::velocityTicksPerMs, "velocityTicksPerMs");
svi.addMemberVariable(&KITGripperActorSensorData::absoluteEncoderVelocity, "absoluteEncoderVelocity");
+ svi.addMemberVariable(&KITGripperActorSensorData::torqueWithInertia, "torqueWithInertia");
return svi;
}
@@ -111,6 +113,9 @@ namespace armarx
float getParallelGripperDecouplingFactor() const;
float getAbsoluteEncoderVelocity() const;
+ std::string getMotorModelTypeIndex() const;
+ bool getNonlinearModelEnabled() const;
+ float getMotorInertia() const;
private:
u_int32_t rawABSEncoderTicks;
@@ -138,6 +143,9 @@ namespace armarx
bool velocityControlEnabled = true;
float currentPWMBoundGradient = 3.75;
int32_t currentPWMBoundOffset = 1500;
+ std::string motorModelType = "None";
+ bool nonlinearModelEnabled = 0;
+ float motorInertia = 0.0;
friend class KITGripperBasisBoardData;
};
using ActorDataPtr = std::shared_ptr<ActorData>;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..beba4292de198e975ba1778c01fe0d60f2206df6
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.cpp
@@ -0,0 +1,304 @@
+#include "TorqueEstimation.h"
+
+bool MotorTorqueEstimator::fcl1()
+{
+ 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 true;
+}
+
+
+bool MotorTorqueEstimator::fcl2()
+{
+ 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 true;
+}
+
+bool MotorTorqueEstimator::fcl3()
+{
+ 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 true;
+}
+bool MotorTorqueEstimator::fcl4()
+{
+ 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 true;
+}
+
+bool MotorTorqueEstimator::fcl5()
+{
+ 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 true;
+}
+
+bool MotorTorqueEstimator::fcl6()
+{
+ 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 true;
+}
+
+
+float MotorTorqueEstimator::linearModel_dcx16(int32_t nI, int32_t pwm)
+{
+ // 4096: ticks per turn
+ float n = (float) nI * 1000 * 60 / 4096; //ticks/ms -> turns/min
+ float T_f = 1000.0 / 406.0; //übersetzung getriebe + Nm->m
+ float motor_a = 265.0; //Drehzahlkonstante [min-1 V-1]
+ float motor_b = 620.0; //Kennliniensteigung [min-1 mNm-1]
+ float motor_eta = 0.55 * 0.78; //Wirkungsgrad Getriebe *Motor, max from datasheet:0.75*0.85
+ float Umax = 48.0; //Spannung bei pwm max
+ float pwmmax = 3000.0;
+ float pwm_zero = 400.0;
+ 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 MotorTorqueEstimator::linearModel_dcx22(int32_t nI, int32_t pwm)
+{
+ float n = (float) nI * 1000 * 60 / 4096; //ticks/ms -> turns/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;
+}
+
+
+void MotorTorqueEstimator::estimateTorque(int32_t vel, int32_t pwm)//, int motorModel, bool linearModelOnly = false)
+{
+ // calculate the torque based on the linear model
+ // std::cout << "model type: " << motorModel << std::endl;
+ if (motorModel == "Motor16")
+ {
+ torque = linearModel_dcx16(vel, pwm);
+ }
+ else if (motorModel == "Motor22")
+ {
+ torque = linearModel_dcx22(vel, pwm);
+ }
+ else
+ {
+ std::cout << "motor model should be set to 'Motor16' or 'Motor22'" << std::endl;
+ torque = 0.0;
+ }
+
+ // calculate the torque induced by non-linear model
+ if (!linearModelOnly)
+ {
+ float vel_input = (float)vel / vel_factor;
+ float pwm_input = (float)pwm / pwm_factor;
+ static float pwmXvel_old = 0;
+ float pwmXvel = vel_input * pwm_input;
+ if (pwmXvel < 0)
+ {
+ pwmXvel = -1;
+ pwmXvel_old = pwmXvel;
+ }
+ else if (pwmXvel > 0)
+ {
+ pwmXvel = 1;
+ pwmXvel_old = pwmXvel;
+ }
+ else
+ {
+ pwmXvel = pwmXvel_old;
+ }
+
+ imgIn[0] = vel_input;
+ imgIn[1] = pwm_input;
+ imgIn[2] = vel_input;
+ imgIn[3] = pwm_input;
+ imgIn[4] = pwmXvel;
+ fcl1();
+ fcl2();
+ fcl3();
+ fcl4();
+ fcl5();
+ fcl6();
+ torque += imgFcl6[0];
+ }
+
+ // calculating the torque induced by motor inertia
+ double timeNow = IceUtil::Time::now().toMilliSecondsDouble();
+ // float deltaT = timeNow - last_time;
+ float deltaT = 1; // in millisecond
+ last_time = timeNow;
+
+ float angVel = (float) 2 * M_PI * vel / 4.096 / deltaT; //radian / s
+ angVel = lowpassFilterFactor * angVel + (1 - lowpassFilterFactor) * vel_old;
+ float acc = (float) 0.001 * (angVel - vel_old) / deltaT;
+ vel_old = angVel;
+ float inertiaTorque = acc * inertia;
+ torqueWithInertia = torque + inertiaTorque;
+}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h
index 51736ff07a9d6e1fc9106f874a2dd486f7d69131..09950a54655822f3af22add0204485c530a0ea14 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h
@@ -3,314 +3,69 @@
#include "TorqueEstimationWeights.h"
#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+#include <iostream>
+#include <memory>
+#include <ArmarXCore/core/time/TimeUtil.h>
-float imgIn[5];
-float imgFcl1[32];
-float imgFcl2[32];
-float imgFcl3[32];
-float imgFcl4[16];
-float imgFcl5[8];
-float imgFcl6[1];
+using namespace armarx;
+using namespace std;
-
-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_dcx16(int32_t nI, int32_t pwm)
+class MotorTorqueEstimator
{
- float n = (float) nI * 1000 * 60 / 4096; //ticks/ms -> turns/min
- float T_f = 1000.0 / 406.0; //übersetzung getriebe + Nm->m
- float motor_a = 265.0; //Drehzahlkonstante [min-1 V-1]
- float motor_b = 620.0; //Kennliniensteigung [min-1 mNm-1]
- float motor_eta = 0.55 * 0.78; //Wirkungsgrad Getriebe *Motor, max from datasheet:0.75*0.85
- float Umax = 48.0; //Spannung bei pwm max
- float pwmmax = 3000.0;
- float pwm_zero = 400.0;
- 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 linearModel_dcx22(int32_t nI, int32_t pwm)
-{
- float n = (float) nI * 1000 * 60 / 4096; //ticks/ms -> turns/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)//, int motorModel, bool linearModelOnly = false)
-{
- bool linearModelOnly = true;
- int motorModel = 16;
- if (linearModelOnly)
- {
- if (motorModel == 16)
- {
- return linearModel_dcx16(n, pwm);
- }
- else if (motorModel == 22)
- {
- return linearModel_dcx22(n, pwm);
- }
- else
- {
- std::cout << "motor model should be set to 16 or 22" << std::endl;
- return 0.0;
- }
- // return linearModel_dcx22(n, 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);
-}
+public:
+ MotorTorqueEstimator(size_t index, bool nonlinearModelEnabled = false, std::string motorType = "None", float motorInertia = 0.0)
+ {
+ actorIndex = index;
+ linearModelOnly = !nonlinearModelEnabled;
+ motorModel = motorType;
+ inertia = motorInertia;
+ vel_old = 0.0;
+ last_time = 0.0;
+ lowpassFilterFactor = 0.7;
+ }
+ ~MotorTorqueEstimator() {}
+ bool fcl1();
+ bool fcl2();
+ bool fcl3();
+ bool fcl4();
+ bool fcl5();
+ bool fcl6();
+ void estimateTorque(int32_t n, int32_t pwm);
+ float linearModel_dcx16(int32_t nI, int32_t pwm);
+ float linearModel_dcx22(int32_t nI, int32_t pwm);
+ size_t getActorIndex()
+ {
+ return actorIndex;
+ }
+ float getTorque()
+ {
+ return torque;
+ }
+ float getTorqueWithInertia()
+ {
+ return torqueWithInertia;
+ }
+private:
+ float torque;
+ float torqueWithInertia;
+ float inertia;
+ float vel_old;
+ bool inertiaEnabled;
+ size_t actorIndex;
+ bool linearModelOnly;
+ std::string motorModel;
+ double last_time;
+ float lowpassFilterFactor;
+
+ float imgIn[5];
+ float imgFcl1[32];
+ float imgFcl2[32];
+ float imgFcl3[32];
+ float imgFcl4[16];
+ float imgFcl5[8];
+ float imgFcl6[1];
+};
+using MotorTorqueEstimatorPtr = std::shared_ptr<MotorTorqueEstimator>;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationWeights.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationWeights.h
index 2ef7dfd6cb02eb3de517e327b2b393995ff92b8e..4353434f460414767819fe77262d7bf675f6e56e 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationWeights.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationWeights.h
@@ -3,10 +3,9 @@
#ifndef __WEIGHTS_H
#define __WEIGHTS_H
-static float n_factor = 459.0;
+static float vel_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},
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/test/KITGripperEtherCATTest.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/test/KITGripperEtherCATTest.cpp
index 557a8c79eb6f37d1e5b6d73a59305f92dd34b10e..3da76c7c49a5157c458de2703e5538b56a0b1384 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/test/KITGripperEtherCATTest.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/test/KITGripperEtherCATTest.cpp
@@ -40,7 +40,7 @@ BOOST_AUTO_TEST_CASE(torqueEstimationPerformanceTest)
int iterations = 1000;
for (int i = 0; i < iterations; i++)
{
- t = estimateTorque(rand() % 800, rand() % 3000);
+ // t = estimateTorque(rand() % 800, rand() % 3000);
// ARMARX_INFO << VAROUT(t);
}
TIMING_END(NN_Calc);