From 1cfdd413dfba3261989679a7a96f514adaa3804b Mon Sep 17 00:00:00 2001
From: JeffGao <jianfenggaobit@gmail.com>
Date: Thu, 5 Sep 2019 16:44:22 +0200
Subject: [PATCH] add anomaly detection wiping controller
---
.../NJointTaskSpaceDMPController.ice | 95 ++
.../RobotAPINJointControllers/CMakeLists.txt | 4 +-
.../NJointAdaptiveWipingController.cpp | 60 +-
.../NJointAdaptiveWipingController.h | 11 +
...omalyDetectionAdaptiveWipingController.cpp | 931 ++++++++++++++++++
...AnomalyDetectionAdaptiveWipingController.h | 257 +++++
6 files changed, 1343 insertions(+), 15 deletions(-)
create mode 100644 source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
create mode 100644 source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 737dd63f2..86cffecf0 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -450,6 +450,7 @@ module armarx
Ice::FloatSeq handCOM;
float handMass;
float angularKp;
+ float frictionCone;
};
@@ -469,5 +470,99 @@ module armarx
double getCanVal();
};
+ class NJointAnomalyDetectionAdaptiveWipingControllerConfig extends NJointControllerConfig
+ {
+
+ // dmp configuration
+ int kernelSize = 100;
+ double dmpAmplitude = 1;
+ double timeDuration = 10;
+
+ double phaseL = 10;
+ double phaseK = 10;
+ float phaseDist0 = 50;
+ float phaseDist1 = 10;
+ double phaseKpPos = 1;
+ double phaseKpOri = 0.1;
+ double posToOriRatio = 100;
+
+
+ // velocity controller configuration
+ string nodeSetName = "";
+
+ float maxJointTorque;
+ Ice::FloatSeq desiredNullSpaceJointValues;
+
+ Ice::FloatSeq Kpos;
+ Ice::FloatSeq Dpos;
+ Ice::FloatSeq Kori;
+ Ice::FloatSeq Dori;
+ Ice::FloatSeq Knull;
+ Ice::FloatSeq Dnull;
+
+ string forceSensorName = "FT R";
+ string forceFrameName = "ArmR8_Wri2";
+ float forceFilter = 0.8;
+ float waitTimeForCalibration = 0.1;
+
+ // anomaly detection and friction estimation
+ int velocityHorizon = 100;
+ int frictionHorizon = 100;
+
+ // pid params
+ bool isForceControlEnabled;
+ bool isRotControlEnabled;
+ bool isTorqueControlEnabled;
+ bool isLCRControlEnabled;
+ Ice::FloatSeq pidForce;
+ Ice::FloatSeq pidRot;
+ Ice::FloatSeq pidTorque;
+ Ice::FloatSeq pidLCR;
+
+ float minimumReactForce = 0;
+ float forceDeadZone;
+ float velFilter;
+
+ float maxLinearVel;
+ float maxAngularVel;
+
+ Ice::FloatSeq ws_x;
+ Ice::FloatSeq ws_y;
+ Ice::FloatSeq ws_z;
+
+ float adaptCoeff;
+ float reactThreshold;
+ float dragForceDeadZone;
+ float adaptForceCoeff;
+ float changePositionTolerance;
+ float changeTimerThreshold;
+
+ Ice::FloatSeq ftOffset;
+ Ice::FloatSeq handCOM;
+ float handMass;
+
+ float ftCommandFilter;
+ float frictionCone;
+ float fricEstiFilter;
+ float velNormThreshold;
+ float maxInteractionForce;
+ };
+
+
+ interface NJointAnomalyDetectionAdaptiveWipingControllerInterface extends NJointControllerInterface
+ {
+ void learnDMPFromFiles(Ice::StringSeq trajfiles);
+ void learnDMPFromTrajectory(TrajectoryBase trajectory);
+
+ bool isFinished();
+ void runDMP(Ice::DoubleSeq goals, double tau);
+
+ void setSpeed(double times);
+ void setGoals(Ice::DoubleSeq goals);
+ void setAmplitude(double amplitude);
+ void setTargetForceInRootFrame(float force);
+
+ double getCanVal();
+ };
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
index 369cf82eb..e8a66b9b7 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
@@ -50,7 +50,7 @@ set(LIB_HEADERS
DMPController/NJointPeriodicTSDMPForwardVelController.h
DMPController/NJointPeriodicTSDMPCompliantController.h
DMPController/NJointAdaptiveWipingController.h
-
+ DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
)
list(APPEND LIB_FILES
DMPController/NJointJointSpaceDMPController.cpp
@@ -63,7 +63,7 @@ set(LIB_HEADERS
DMPController/NJointPeriodicTSDMPForwardVelController.cpp
DMPController/NJointPeriodicTSDMPCompliantController.cpp
DMPController/NJointAdaptiveWipingController.cpp
-
+ DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
)
list(APPEND LIBS ${DMP_LIBRARIES} DMPController)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
index a58e3ae3d..324b543ac 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
@@ -74,6 +74,7 @@ namespace armarx
dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
kpf = cfg->Kpf;
+ // forcePID.reset(new PIDController(cfg->kpf, ));
knull.setZero(targets.size());
dnull.setZero(targets.size());
@@ -270,6 +271,13 @@ namespace armarx
Eigen::Vector3f axis;
axis.setZero();
targetVel.setZero();
+ Eigen::Vector3f forceInToolFrame;
+ forceInToolFrame << 0, 0, 0;
+
+ Eigen::Vector3f torqueInToolFrame;
+ torqueInToolFrame << 0, 0, 0;
+
+ float angle = 0;
if (firstRun || !dmpRunning)
{
lastPosition = currentPose.block<2, 1>(0, 3);
@@ -277,12 +285,13 @@ namespace armarx
firstRun = false;
filteredForce.setZero();
Eigen::Vector3f currentForce = forceSensor->force - forceOffset;
+
Eigen::Matrix3f forceFrameOri = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3, 3>(0, 0);
Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
currentForce = currentForce - handGravity;
- currentForceOffset = 0.1 * currentForceOffset + 0.9 * currentForce;
+ currentForceOffset = 0.1 * currentForceOffset + 0.9 * currentForce;
origHandOri = currentPose.block<3, 3>(0, 0);
toolTransform = origHandOri.transpose();
targetVel.setZero();
@@ -304,9 +313,10 @@ namespace armarx
currentForce = currentForce - handGravity;
filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * currentForce;
- // Eigen::Vector3f currentTorque = forceSensor->torque - torqueOffset;
- // Eigen::Vector3f handTorque = handCOM.cross(gravityInForceFrame);
- // currentTorque = currentTorque - handTorque;
+ Eigen::Vector3f currentTorque = forceSensor->torque - torqueOffset;
+ Eigen::Vector3f handTorque = handCOM.cross(gravityInForceFrame);
+ currentTorque = currentTorque - handTorque;
+ filteredTorque = (1 - cfg->forceFilter) * filteredTorque + cfg->forceFilter * currentTorque;
for (size_t i = 0; i < 3; ++i)
{
@@ -321,12 +331,15 @@ namespace armarx
}
filteredForceInRoot = forceFrameOri * filteredForce;
+ filteredTorqueInRoot = forceFrameOri * filteredTorque;
float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
Eigen::Matrix3f currentToolOri = currentHandOri * toolTransform;
- Eigen::Vector3f forceInToolFrame = currentToolOri.transpose() * filteredForceInRoot;
+ forceInToolFrame = currentToolOri.transpose() * filteredForceInRoot;
+ torqueInToolFrame = currentToolOri.transpose() * filteredTorqueInRoot;
+
float desiredZVel = kpf * (targetForce - forceInToolFrame(2));
targetVel(2) -= desiredZVel;
targetVel.block<3, 1>(0, 0) = currentToolOri * targetVel.block<3, 1>(0, 0);
@@ -351,13 +364,13 @@ namespace armarx
axis = currentToolDir.cross(desiredToolDir);
axis = axis.normalized();
- float angle = acosf(currentToolDir.dot(desiredToolDir));
+ angle = acosf(currentToolDir.dot(desiredToolDir));
- if (fabs(angle) < M_PI / 2)
+ if (fabs(angle) < M_PI / 2 && fabs(angle) > cfg->frictionCone)
{
// sigmoid function
- float adaptedAngularKp = 1 / (1 + exp(0.2 * (angle - M_PI / 4)));
+ float adaptedAngularKp = cfg->angularKp / (1 + exp(10 * (angle - M_PI / 4)));
float angularKp = fmin(adaptedAngularKp, cfg->angularKp);
// test axis
@@ -370,7 +383,7 @@ namespace armarx
{
fixedAxis << 0.0, -1.0, 0.0;
}
- Eigen::AngleAxisf desiredToolRot(angle, fixedAxis);
+ Eigen::AngleAxisf desiredToolRot(angle - cfg->frictionCone, fixedAxis);
Eigen::Matrix3f desiredRotMat = desiredToolRot * Eigen::Matrix3f::Identity();
Eigen::Vector3f angularDiff = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
@@ -466,11 +479,16 @@ namespace armarx
debugRT.getWriteBuffer().currentPose = currentPose;
debugRT.getWriteBuffer().filteredForceInRoot = filteredForceInRoot;
debugRT.getWriteBuffer().rotationAxis = axis;
- debugRT.getWriteBuffer().filteredForce = filteredForce;
+ debugRT.getWriteBuffer().filteredForce = forceInToolFrame;
debugRT.getWriteBuffer().globalFilteredForce = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(filteredForceInRoot);
debugRT.getWriteBuffer().targetVel = targetVel;
debugRT.getWriteBuffer().adaptK = adaptK;
debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
+ debugRT.getWriteBuffer().rotAngle = angle;
+ debugRT.getWriteBuffer().currentTwist = currentTwist;
+ debugRT.getWriteBuffer().filteredTorque = torqueInToolFrame;
+
+
debugRT.commitWrite();
rt2CtrlData.getWriteBuffer().currentPose = currentPose;
@@ -637,9 +655,15 @@ namespace armarx
datafields["current_z"] = new Variant(currentPoseDebug(2, 3));
Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
- datafields["filteredforce_x"] = new Variant(filteredForce(0));
- datafields["filteredforce_y"] = new Variant(filteredForce(1));
- datafields["filteredforce_z"] = new Variant(filteredForce(2));
+ datafields["filteredforceInTool_x"] = new Variant(filteredForce(0));
+ datafields["filteredforceInTool_y"] = new Variant(filteredForce(1));
+ datafields["filteredforceInTool_z"] = new Variant(filteredForce(2));
+
+ Eigen::Vector3f filteredTorque = debugRT.getUpToDateReadBuffer().filteredTorque;
+ datafields["filteredtorqueInTool_x"] = new Variant(filteredTorque(0));
+ datafields["filteredtorqueInTool_y"] = new Variant(filteredTorque(1));
+ datafields["filteredtorqueInTool_z"] = new Variant(filteredTorque(2));
+
Eigen::Vector3f filteredForceInRoot = debugRT.getUpToDateReadBuffer().filteredForceInRoot;
datafields["filteredForceInRoot_x"] = new Variant(filteredForceInRoot(0));
@@ -664,6 +688,15 @@ namespace armarx
datafields["targetVel_pi"] = new Variant(targetVel(4));
datafields["targetVel_ya"] = new Variant(targetVel(5));
+ Eigen::VectorXf currentTwist = debugRT.getUpToDateReadBuffer().currentTwist;
+ datafields["currentTwist_x"] = new Variant(currentTwist(0));
+ datafields["currentTwist_y"] = new Variant(currentTwist(1));
+ datafields["currentTwist_z"] = new Variant(currentTwist(2));
+ datafields["currentTwist_ro"] = new Variant(currentTwist(3));
+ datafields["currentTwist_pi"] = new Variant(currentTwist(4));
+ datafields["currentTwist_ya"] = new Variant(currentTwist(5));
+
+
Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
datafields["adaptK_x"] = new Variant(adaptK(0));
datafields["adaptK_y"] = new Variant(adaptK(1));
@@ -672,6 +705,7 @@ namespace armarx
datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
datafields["PhaseStop"] = new Variant(debugRT.getUpToDateReadBuffer().isPhaseStop);
+ datafields["rotAngle"] = new Variant(debugRT.getUpToDateReadBuffer().rotAngle);
// datafields["targetVel_rx"] = new Variant(targetVel(3));
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h
index 9c2b8d01d..7e1949ebe 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h
@@ -16,6 +16,8 @@
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
#include <RobotAPI/libraries/core/Trajectory.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+
namespace armarx
{
@@ -109,6 +111,8 @@ namespace armarx
Eigen::Matrix4f targetPose;
Eigen::Vector3f filteredForce;
Eigen::Vector3f filteredForceInRoot;
+ Eigen::Vector3f filteredTorque;
+
Eigen::Vector3f rotationAxis;
Eigen::Vector3f reactForce;
@@ -120,6 +124,9 @@ namespace armarx
Eigen::Matrix4f globalPose;
Eigen::Vector3f globalFilteredForce;
Eigen::Vector3f currentToolDir;
+ Eigen::VectorXf currentTwist;
+
+ float rotAngle;
};
TripleBuffer<DebugRTData> debugRT;
@@ -182,11 +189,15 @@ namespace armarx
Eigen::VectorXf nullSpaceJointsVec;
const SensorValueForceTorque* forceSensor;
+ PIDControllerPtr forcePID;
+
Eigen::Vector3f filteredForce;
Eigen::Vector3f filteredTorque;
Eigen::Vector3f forceOffset;
Eigen::Vector3f currentForceOffset;
+
Eigen::Vector3f torqueOffset;
+ Eigen::Vector3f currentTorqueOffset;
float handMass;
Eigen::Vector3f handCOM;
Eigen::Vector3f gravityInRoot;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
new file mode 100644
index 000000000..c200c425e
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
@@ -0,0 +1,931 @@
+#include "NJointAnomalyDetectionAdaptiveWipingController.h"
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointAnomalyDetectionAdaptiveWipingController> registrationControllerNJointAnomalyDetectionAdaptiveWipingController("NJointAnomalyDetectionAdaptiveWipingController");
+
+ NJointAnomalyDetectionAdaptiveWipingController::NJointAnomalyDetectionAdaptiveWipingController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ useSynchronizedRtRobot();
+ cfg = NJointAnomalyDetectionAdaptiveWipingControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
+ VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
+
+ ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName);
+ for (size_t i = 0; i < rns->getSize(); ++i)
+ {
+ std::string jointName = rns->getNode(i)->getName();
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
+
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+
+ velocitySensors.push_back(velocitySensor);
+ positionSensors.push_back(positionSensor);
+ };
+
+ tcp = rns->getTCP();
+ // set tcp controller
+ nodeSetName = cfg->nodeSetName;
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+
+ TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
+ taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
+ taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
+ taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
+ taskSpaceDMPConfig.DMPMode = "Linear";
+ taskSpaceDMPConfig.DMPStyle = "Periodic";
+ taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
+ taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
+ taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
+ taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
+ taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
+ taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
+ taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
+ taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
+ taskSpaceDMPConfig.phaseStopParas.Dori = 0;
+
+ lastCanVal = cfg->timeDuration;
+
+ dmpCtrl.reset(new TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
+
+ NJointAnomalyDetectionAdaptiveWipingControllerControlData initData;
+ initData.targetTSVel.resize(6);
+ for (size_t i = 0; i < 6; ++i)
+ {
+ initData.targetTSVel(i) = 0;
+ }
+ reinitTripleBuffer(initData);
+
+ firstRun = true;
+ dmpRunning = false;
+
+ // anomaly detection
+ velocityHorizon = cfg->velocityHorizon;
+
+ // friction estimation
+ frictionHorizon = cfg->frictionHorizon;
+ estimatedFriction << 0.0, 0.0;
+ lastForceInToolXY.setZero();
+
+ ARMARX_CHECK_EQUAL(cfg->Kpos.size(), 3);
+ ARMARX_CHECK_EQUAL(cfg->Dpos.size(), 3);
+ ARMARX_CHECK_EQUAL(cfg->Kori.size(), 3);
+ ARMARX_CHECK_EQUAL(cfg->Dori.size(), 3);
+
+ kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
+ dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
+ kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
+ dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
+
+ isForceControlEnabled = cfg->isForceControlEnabled;
+ isRotControlEnabled = cfg->isRotControlEnabled;
+ isTorqueControlEnabled = cfg->isTorqueControlEnabled;
+ isLCRControlEnabled = cfg->isLCRControlEnabled;
+ forcePID.reset(new PIDController(cfg->pidForce[0], cfg->pidForce[1], cfg->pidForce[2], cfg->pidForce[3]));
+ rotPID.reset(new PIDController(cfg->pidRot[0], cfg->pidRot[1], cfg->pidRot[2], cfg->pidRot[3]));
+ torquePID.reset(new PIDController(cfg->pidTorque[0], cfg->pidTorque[1], cfg->pidTorque[2], cfg->pidTorque[3]));
+ lcrPID.reset(new PIDController(cfg->pidLCR[0], cfg->pidLCR[1], cfg->pidLCR[2], cfg->pidLCR[3]));
+
+ knull.setZero(targets.size());
+ dnull.setZero(targets.size());
+
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ knull(i) = cfg->Knull.at(i);
+ dnull(i) = cfg->Dnull.at(i);
+ }
+
+ nullSpaceJointsVec.resize(cfg->desiredNullSpaceJointValues.size());
+ for (size_t i = 0; i < cfg->desiredNullSpaceJointValues.size(); ++i)
+ {
+ nullSpaceJointsVec(i) = cfg->desiredNullSpaceJointValues.at(i);
+ }
+
+
+ const SensorValueBase* svlf = robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
+ forceSensor = svlf->asA<SensorValueForceTorque>();
+
+
+ ARMARX_CHECK_EQUAL(cfg->ftOffset.size(), 6);
+
+ currentForceOffset.setZero();
+ forceOffset << cfg->ftOffset[0], cfg->ftOffset[1], cfg->ftOffset[2];
+ torqueOffset << cfg->ftOffset[3], cfg->ftOffset[4], cfg->ftOffset[5];
+
+ handMass = cfg->handMass;
+ handCOM << cfg->handCOM[0], cfg->handCOM[1], cfg->handCOM[2];
+
+
+ filteredForce.setZero();
+ filteredTorque.setZero();
+ filteredFTCommand.setZero();
+ filteredForceInRoot.setZero();
+ filteredTorqueInRoot.setZero();
+ targetFTInToolFrame.setZero();
+
+ UserToRTData initUserData;
+ initUserData.targetForce = 0;
+ user2rtData.reinitAllBuffers(initUserData);
+
+ oriToolDir << 0, 0, 1;
+ gravityInRoot << 0, 0, -9.8;
+
+ qvel_filtered.setZero(targets.size());
+
+ ARMARX_CHECK_EQUAL(cfg->ws_x.size(), 2);
+ ARMARX_CHECK_EQUAL(cfg->ws_y.size(), 2);
+ ARMARX_CHECK_EQUAL(cfg->ws_z.size(), 2);
+ // only for ARMAR-6 (safe-guard)
+ ARMARX_CHECK_LESS(cfg->ws_x[0], cfg->ws_x[1]);
+ ARMARX_CHECK_LESS(cfg->ws_x[0], 1000);
+ ARMARX_CHECK_LESS(-200, cfg->ws_x[1]);
+
+ ARMARX_CHECK_LESS(cfg->ws_y[0], cfg->ws_y[1]);
+ ARMARX_CHECK_LESS(cfg->ws_y[0], 1200);
+ ARMARX_CHECK_LESS(0, cfg->ws_y[1]);
+
+ ARMARX_CHECK_LESS(cfg->ws_z[0], cfg->ws_z[1]);
+ ARMARX_CHECK_LESS(cfg->ws_z[0], 1800);
+ ARMARX_CHECK_LESS(300, cfg->ws_z[1]);
+
+ adaptK = kpos;
+ lastDiff = 0;
+ changeTimer = 0;
+ }
+
+ void NJointAnomalyDetectionAdaptiveWipingController::onInitNJointController()
+ {
+ ARMARX_INFO << "init ...";
+
+
+ RTToControllerData initSensorData;
+ initSensorData.deltaT = 0;
+ initSensorData.currentTime = 0;
+ initSensorData.currentPose = tcp->getPoseInRootFrame();
+ initSensorData.currentTwist.setZero();
+ initSensorData.isPhaseStop = false;
+ rt2CtrlData.reinitAllBuffers(initSensorData);
+
+ RTToUserData initInterfaceData;
+ initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
+ initInterfaceData.waitTimeForCalibration = 0;
+ rt2UserData.reinitAllBuffers(initInterfaceData);
+
+ started = false;
+
+ runTask("NJointAnomalyDetectionAdaptiveWipingController", [&]
+ {
+ CycleUtil c(1);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ while (getState() == eManagedIceObjectStarted)
+ {
+ if (isControllerActive())
+ {
+ controllerRun();
+ }
+ c.waitForCycleDuration();
+ }
+ });
+
+ }
+
+ std::string NJointAnomalyDetectionAdaptiveWipingController::getClassName(const Ice::Current&) const
+ {
+ return "NJointAnomalyDetectionAdaptiveWipingController";
+ }
+
+ void NJointAnomalyDetectionAdaptiveWipingController::controllerRun()
+ {
+ if (!started)
+ {
+ return;
+ }
+
+ if (!dmpCtrl)
+ {
+ return;
+ }
+
+ Eigen::VectorXf targetVels(6);
+ bool isPhaseStop = rt2CtrlData.getUpToDateReadBuffer().isPhaseStop;
+ if (isPhaseStop)
+ {
+ targetVels.setZero();
+ }
+ else
+ {
+
+ double deltaT = rt2CtrlData.getUpToDateReadBuffer().deltaT;
+ Eigen::Matrix4f currentPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
+ Eigen::VectorXf currentTwist = rt2CtrlData.getUpToDateReadBuffer().currentTwist;
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->flow(deltaT, currentPose, currentTwist);
+
+ targetVels = dmpCtrl->getTargetVelocity();
+
+ debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
+ VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
+ debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
+ debugOutputData.getWriteBuffer().mpcFactor = dmpCtrl->debugData.mpcFactor;
+ debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
+ debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
+ debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
+ debugOutputData.getWriteBuffer().deltaT = deltaT;
+ debugOutputData.commitWrite();
+ }
+ getWriterControlStruct().canVal = dmpCtrl->canVal;
+ getWriterControlStruct().targetTSVel = targetVels;
+ writeControlStruct();
+
+ dmpRunning = true;
+ }
+
+
+ void NJointAnomalyDetectionAdaptiveWipingController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+ float dTf = (float)deltaT;
+
+ Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
+ rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
+ rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
+ rt2UserData.commitWrite();
+
+ Eigen::Vector3f currentToolDir;
+ currentToolDir.setZero();
+ Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+
+ Eigen::VectorXf qpos(positionSensors.size());
+ Eigen::VectorXf qvel(velocitySensors.size());
+ for (size_t i = 0; i < positionSensors.size(); ++i)
+ {
+ qpos(i) = positionSensors[i]->position;
+ qvel(i) = velocitySensors[i]->velocity;
+ }
+
+ qvel_filtered = (1 - cfg->velFilter) * qvel_filtered + cfg->velFilter * qvel;
+ Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
+
+ velocityHorizonList.push_back(currentTwist);
+ if (velocityHorizonList.size() > velocityHorizon)
+ {
+ velocityHorizonList.pop_front();
+ }
+
+
+ Eigen::VectorXf targetVel(6);
+ Eigen::Vector3f axis;
+ Eigen::Vector3f forceInToolFrame;
+ Eigen::Vector3f torqueInToolFrame;
+ Eigen::Vector6f targetFTInRootFrame;
+ Eigen::Vector3f velPInToolFrame;
+ targetVel.setZero();
+ axis.setZero();
+ forceInToolFrame.setZero();
+ torqueInToolFrame.setZero();
+ targetFTInRootFrame.setZero();
+ velPInToolFrame.setZero();
+ float angle = 0;
+
+ if (firstRun || !dmpRunning)
+ {
+ initHandPose = currentPose;
+ lastPosition = currentPose.block<2, 1>(0, 3);
+ targetPose = currentPose;
+ firstRun = false;
+ filteredForce.setZero();
+ Eigen::Vector3f currentForce = forceSensor->force - forceOffset;
+
+ Eigen::Matrix3f forceFrameOri = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3, 3>(0, 0);
+ Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
+ Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
+ currentForce = currentForce - handGravity;
+
+ currentForceOffset = 0.1 * currentForceOffset + 0.9 * currentForce;
+ origHandOri = currentPose.block<3, 3>(0, 0);
+ toolTransform = origHandOri.transpose();
+ targetVel.setZero();
+ }
+ else
+ {
+ Eigen::Matrix3f currentToolOri = currentPose.block<3, 3>(0, 0) * toolTransform;
+
+ /* -------------------------- get target vel from dmp thread --------------------------------- */
+ rtUpdateControlStruct();
+ targetVel = rtGetControlStruct().targetTSVel;
+ targetVel(2) = 0;
+ targetVel.head(3) = currentToolOri * targetVel.head(3);
+ targetVel.tail(3) = currentToolOri * targetVel.tail(3);
+
+ double canVal = rtGetControlStruct().canVal;
+ if (canVal - lastCanVal > 0.9 * cfg->timeDuration)
+ {
+ wipingCounter++;
+ mu = 1.0;
+ }
+ lastCanVal = canVal;
+
+ /* -------------------------- force feedback, filter and transform frame --------------------------------- */
+ Eigen::Vector3f currentForce = forceSensor->force - forceOffset - currentForceOffset;
+
+ Eigen::Matrix3f forceFrameOri = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3, 3>(0, 0);
+ Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
+ Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
+
+ currentForce = currentForce - handGravity;
+ filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * currentForce;
+
+ Eigen::Vector3f currentTorque = forceSensor->torque - torqueOffset;
+ Eigen::Vector3f handTorque = handCOM.cross(gravityInForceFrame);
+ currentTorque = currentTorque - handTorque;
+ filteredTorque = (1 - cfg->forceFilter) * filteredTorque + cfg->forceFilter * currentTorque;
+
+ Eigen::Vector3f forceInRootForFricEst = forceFrameOri * filteredForce;
+ Eigen::Vector3f forceInToolForFricEst = currentToolOri.transpose() * forceInRootForFricEst;
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ if (fabs(filteredForce(i)) > cfg->forceDeadZone)
+ {
+ filteredForce(i) -= (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
+ }
+ else
+ {
+ filteredForce(i) = 0;
+ }
+ }
+
+ filteredForceInRoot = forceFrameOri * filteredForce;
+ filteredTorqueInRoot = forceFrameOri * filteredTorque;
+ float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
+
+ forceInToolFrame = currentToolOri.transpose() * filteredForceInRoot;
+ // TODO this is wrong
+ torqueInToolFrame = currentToolOri.transpose() * filteredTorqueInRoot;
+ velPInToolFrame = currentToolOri.transpose() * currentTwist.head(3);
+ // Eigen::Vector3f velRInToolFrame = currentToolOri.transpose() * currentTwist.tail(3);
+
+ /* -------------------------- friction estimation --------------------------------- */
+
+ Eigen::Vector2f v_xy;
+ Eigen::Vector2f f_xy;
+ v_xy << velPInToolFrame(0), velPInToolFrame(1);
+ f_xy << forceInToolForFricEst(0), forceInToolForFricEst(1);
+ f_xy = cfg->fricEstiFilter * f_xy + (1 - cfg->fricEstiFilter) * lastForceInToolXY;
+ lastForceInToolXY = f_xy;
+
+ if (wipingCounter > 0)
+ {
+ if (v_xy.norm() > cfg->velNormThreshold && fabs(forceInToolForFricEst(2) - targetForce) < 0.5 * targetForce)
+ {
+ recordFrictionNorm.push_back(f_xy.norm());
+ recordForceNormToSurface.push_back(forceInToolForFricEst(2));
+ }
+ if (recordFrictionNorm.size() > frictionHorizon)
+ {
+ recordFrictionNorm.pop_front();
+ recordForceNormToSurface.pop_front();
+ float dotProduct = 0.0;
+ float normSquare = 0.0;
+ for (size_t i = 0; i < recordFrictionNorm.size(); i++)
+ {
+ dotProduct += (recordFrictionNorm[i] * recordForceNormToSurface[i]);
+ normSquare += (recordForceNormToSurface[i] * recordForceNormToSurface[i]);
+ }
+ if (normSquare > 0)
+ {
+ float mu_tmp = dotProduct / normSquare;
+ if (mu_tmp > 0)
+ {
+ mu = fmax(fmin(mu, mu_tmp), safeFrictionConeLowerLimit);
+ }
+ }
+ if (v_xy.norm() > cfg->velNormThreshold)
+ {
+ estimatedFriction = - v_xy * mu * forceInToolForFricEst(2) / v_xy.norm();
+ }
+ }
+ }
+
+ /* -------------------------- Force Regulation and Torque PID Controller --------------------------------- */
+
+ forcePID->update(deltaT, forceInToolFrame(2), targetForce);
+ torquePID->update(deltaT, torqueInToolFrame(1), 0.0);
+
+ /* -------------------------- Rotation PID Controller --------------------------------- */
+
+ currentToolDir = currentToolOri * oriToolDir;
+ for (int i = 3; i < 6; ++i)
+ {
+ targetVel(i) = 0;
+ }
+ float frictionCone;
+ if (cfg->frictionCone < 0.0)
+ {
+ frictionCone = atan(mu);
+
+ }
+ else
+ {
+ frictionCone = cfg->frictionCone;
+ }
+ float adaptedAngularKp = 0.0;
+ Eigen::Vector3f angularDiff;
+ angularDiff.setZero();
+ // rotation changes
+ if (filteredForceInRoot.norm() > fabs(cfg->minimumReactForce))
+ {
+ Eigen::Vector3f toolYDir;
+ toolYDir << 0, 1.0, 0;
+ Eigen::Vector3f toolYDirInRoot = currentToolOri * toolYDir;
+ Eigen::Vector3f projectedFilteredForceInRoot = filteredForceInRoot - filteredForceInRoot.dot(toolYDirInRoot) * toolYDirInRoot;
+ Eigen::Vector3f desiredToolDir = projectedFilteredForceInRoot.normalized();// / projectedFilteredForceInRoot.norm();
+ currentToolDir.normalize();
+
+ axis = currentToolDir.cross(desiredToolDir);
+ axis = axis.normalized();
+ angle = acosf(currentToolDir.dot(desiredToolDir));
+
+ int sign = 1;
+ if (axis(1) < 0)
+ {
+ sign = -1;
+ }
+
+ if (fabs(angle) < M_PI / 2 && fabs(angle) > frictionCone)
+ {
+ // sigmoid function
+ adaptedAngularKp = cfg->pidRot[0] / (1 + exp(10 * (angle - M_PI / 4)));
+ adaptedAngularKp = fmin(adaptedAngularKp, cfg->pidRot[0]);
+ rotPID->Kp = adaptedAngularKp;
+ angle -= frictionCone;
+ angle *= sign;
+ }
+ else
+ {
+ angle = 0.0;
+ rotPID->Kp = cfg->pidRot[0];
+ }
+ }
+ rotPID->update(deltaT, angle, 0.0);
+
+ /* -------------------------- Lose Contact Recover PID Controller --------------------------------- */
+
+ if (forceInToolFrame(2) > loseContactRatio * targetForce)
+ {
+ makingContactCounter++;
+ if (makingContactCounter > 100)
+ {
+ isMakingContact = true;
+ isLCREnabled = false;
+ }
+ else
+ {
+ isMakingContact = false;
+ }
+ }
+ if (!isContactedOnce && isMakingContact)
+ {
+ isContactedOnce = true;
+ }
+ Eigen::Vector3f compensationAxis;
+ compensationAxis.setZero();
+ if (isContactedOnce && fabs(velPInToolFrame(2)) > 10 && frictionCone < 1.0)
+ {
+ makingContactCounter = 0;
+ Eigen::Vector3f v;
+ v << velPInToolFrame(0), 0.0, 0.0;
+ Eigen::Vector3f f;
+ f << 0.0, 0.0, targetForce;
+ compensationAxis = f.cross(v);
+ if (compensationAxis.norm() > 0)
+ {
+ compensationAxis.normalized();
+ }
+ else
+ {
+ compensationAxis.setZero();
+ }
+ forceControlGate *= 0.5;
+ isLCREnabled = true;
+ lcrCounter -= 1;
+ }
+ else
+ {
+ forceControlGate = 1.0;
+ // TODO: restart vmp controller
+ }
+ float velInForceDir = 0.0;
+ if (lcrCounter < 500)
+ {
+ velInForceDir = fabs(velPInToolFrame(2));
+ lcrCounter -= 1;
+ if (lcrCounter == 0)
+ {
+ lcrCounter = 500;
+ }
+ }
+ lcrPID->update(deltaT, velInForceDir, 0.0);
+
+ // get PID control commands
+ if (isLCRControlEnabled)
+ {
+ // targetFTInToolFrame.tail(3) += (float)lcrPID->getControlValue() * compensationAxis;
+ targetVel.tail(3) += (float)lcrPID->getControlValue() * compensationAxis;
+ }
+ if (isForceControlEnabled)
+ {
+ // targetFTInToolFrame(2) -= (float)forcePID->getControlValue() * forceControlGate;
+ targetVel(2) -= (float)forcePID->getControlValue();
+ targetVel.head(3) = currentToolOri * targetVel.head(3);
+ }
+ if (isRotControlEnabled)
+ {
+ // targetFTInToolFrame(4) -= (float)rotPID->getControlValue();
+ // targetVel.tail(3) = adaptedAngularKp * angularDiff;
+ targetVel(4) -= (float)rotPID->getControlValue();
+ }
+ if (isTorqueControlEnabled)
+ {
+ // targetFTInToolFrame(4) -= (float)torquePID->getControlValue();
+ targetVel(4) += (float)torquePID->getControlValue();
+ }
+ // targetFTInRootFrame.head(3) = currentToolOri * targetFTInToolFrame.head(3);
+ // targetFTInRootFrame.tail(3) = currentToolOri * targetFTInToolFrame.tail(3);
+ }
+
+ /* -------------------------- VMP Phase Stop --------------------------------- */
+
+ bool isPhaseStop = false;
+
+ float diff = (targetPose.block<2, 1>(0, 3) - currentPose.block<2, 1>(0, 3)).norm();
+ if (diff > cfg->phaseDist0)
+ {
+ isPhaseStop = true;
+ }
+
+ /* -------------------------- Drag Force Adaptation --------------------------------- */
+
+ if (filteredForceInRoot.block<2, 1>(0, 0).norm() > cfg->dragForceDeadZone)
+ {
+ Eigen::Vector2f dragForce = filteredForceInRoot.block<2, 1>(0, 0) - cfg->dragForceDeadZone * filteredForceInRoot.block<2, 1>(0, 0) / filteredForceInRoot.block<2, 1>(0, 0).norm();
+ adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
+ adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
+ lastDiff = diff;
+ }
+ else
+ {
+ adaptK(0) = fmin(adaptK(0) + fabs(dTf * cfg->adaptCoeff), kpos(0));
+ adaptK(1) = fmin(adaptK(1) + fabs(dTf * cfg->adaptCoeff), kpos(1));
+ }
+ adaptK(2) = kpos(2);
+
+ /* -------------------------- Integrate Target Velocity 2 Target Pose --------------------------------- */
+
+ targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
+ Eigen::Matrix3f rotMat = VirtualRobot::MathTools::rpy2eigen3f(dTf * targetVel(3), dTf * targetVel(4), dTf * targetVel(5));
+ targetPose.block<3, 3>(0, 0) = rotMat * targetPose.block<3, 3>(0, 0);
+
+ if (isPhaseStop)
+ {
+ Eigen::Vector2f currentXY = currentPose.block<2, 1>(0, 3);
+ if ((lastPosition - currentXY).norm() < cfg->changePositionTolerance)
+ {
+ changeTimer += deltaT;
+ }
+ else
+ {
+ lastPosition = currentPose.block<2, 1>(0, 3);
+ changeTimer = 0;
+ }
+
+ if (changeTimer > cfg->changeTimerThreshold)
+ {
+ targetPose(0, 3) = currentPose(0, 3);
+ targetPose(1, 3) = currentPose(1, 3);
+ isPhaseStop = false;
+ changeTimer = 0;
+ }
+ }
+ else
+ {
+ changeTimer = 0;
+ }
+
+
+ targetPose(0, 3) = targetPose(0, 3) > cfg->ws_x[0] ? targetPose(0, 3) : cfg->ws_x[0];
+ targetPose(0, 3) = targetPose(0, 3) < cfg->ws_x[1] ? targetPose(0, 3) : cfg->ws_x[1];
+
+ targetPose(1, 3) = targetPose(1, 3) > cfg->ws_y[0] ? targetPose(1, 3) : cfg->ws_y[0];
+ targetPose(1, 3) = targetPose(1, 3) < cfg->ws_y[1] ? targetPose(1, 3) : cfg->ws_y[1];
+
+ targetPose(2, 3) = targetPose(2, 3) > cfg->ws_z[0] ? targetPose(2, 3) : cfg->ws_z[0];
+ targetPose(2, 3) = targetPose(2, 3) < cfg->ws_z[1] ? targetPose(2, 3) : cfg->ws_z[1];
+
+ /* -------------------------- Force/Torque Impedance Controller --------------------------------- */
+
+ // inverse dynamic controller
+ jacobi.block<3, 8>(0, 0) = 0.001 * jacobi.block<3, 8>(0, 0); // convert mm to m
+
+ Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).transpose();
+ Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+ Eigen::Vector6f taskFTControlCommand;
+
+ taskFTControlCommand.head(3) = adaptK.cwiseProduct(targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) * 0.001 +
+ dpos.cwiseProduct(- currentTwist.head(3)) * 0.001 +
+ targetFTInRootFrame.head(3);
+ taskFTControlCommand.tail(3) = kori.cwiseProduct(rpy) +
+ dori.cwiseProduct(- currentTwist.tail(3)) +
+ targetFTInRootFrame.tail(3);
+
+ filteredFTCommand = cfg->ftCommandFilter * taskFTControlCommand + (1 - cfg->ftCommandFilter) * filteredFTCommand;
+
+ /* -------------------------- NullSpace and Joint Torque Controller --------------------------------- */
+
+ Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
+ Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(nullSpaceJointsVec - qpos) - dnull.cwiseProduct(qvel);
+ Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
+ Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * filteredFTCommand + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
+ // if (forceInToolFrame.norm() > cfg->maxInteractionForce)
+ // {
+ // jointDesiredTorques.setZero();
+ // }
+
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->torque = jointDesiredTorques(i);
+
+ if (!targets.at(i)->isValid())
+ {
+ targets.at(i)->torque = 0.0f;
+ }
+ else
+ {
+ if (fabs(targets.at(i)->torque) > fabs(cfg->maxJointTorque))
+ {
+ targets.at(i)->torque = fabs(cfg->maxJointTorque) * (targets.at(i)->torque / fabs(targets.at(i)->torque));
+ }
+ }
+ }
+
+ /* -------------------------- Communication --------------------------------- */
+
+ debugRT.getWriteBuffer().currentToolDir = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(currentToolDir);
+ debugRT.getWriteBuffer().targetPose = targetPose;
+ debugRT.getWriteBuffer().globalPose = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystem(targetPose);
+ debugRT.getWriteBuffer().currentPose = currentPose;
+ debugRT.getWriteBuffer().filteredForceInRoot = filteredForceInRoot;
+ debugRT.getWriteBuffer().rotationAxis = axis;
+ debugRT.getWriteBuffer().filteredForce = forceInToolFrame;
+ debugRT.getWriteBuffer().globalFilteredForce = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(filteredForceInRoot);
+ debugRT.getWriteBuffer().targetVel = targetVel;
+ debugRT.getWriteBuffer().adaptK = adaptK;
+ debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
+ debugRT.getWriteBuffer().rotAngle = angle;
+ debugRT.getWriteBuffer().currentTwist = currentTwist;
+ debugRT.getWriteBuffer().filteredTorque = torqueInToolFrame;
+ debugRT.getWriteBuffer().wipingCounter = wipingCounter;
+ debugRT.getWriteBuffer().mu = mu;
+ debugRT.getWriteBuffer().estimatedFriction = estimatedFriction;
+ debugRT.getWriteBuffer().frictionInToolXY = lastForceInToolXY;
+ debugRT.getWriteBuffer().velPInTool = velPInToolFrame;
+
+ debugRT.commitWrite();
+
+ rt2CtrlData.getWriteBuffer().currentPose = currentPose;
+ rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
+ rt2CtrlData.getWriteBuffer().deltaT = deltaT;
+ rt2CtrlData.getWriteBuffer().currentTime += deltaT;
+ rt2CtrlData.getWriteBuffer().isPhaseStop = isPhaseStop;
+ rt2CtrlData.commitWrite();
+
+ }
+
+
+ void NJointAnomalyDetectionAdaptiveWipingController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
+ {
+ ARMARX_INFO << "Learning DMP ... ";
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->learnDMPFromFiles(fileNames);
+
+ }
+
+ void NJointAnomalyDetectionAdaptiveWipingController::learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&)
+ {
+ ARMARX_INFO << "Learning DMP ... ";
+ ARMARX_CHECK_EXPRESSION(trajectory);
+ TrajectoryPtr dmpTraj = TrajectoryPtr::dynamicCast(trajectory);
+ ARMARX_CHECK_EXPRESSION(dmpTraj);
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->learnDMPFromTrajectory(dmpTraj);
+
+ }
+
+ void NJointAnomalyDetectionAdaptiveWipingController::setSpeed(Ice::Double times, const Ice::Current&)
+ {
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->setSpeed(times);
+ }
+
+
+ void NJointAnomalyDetectionAdaptiveWipingController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
+ {
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->setGoalPoseVec(goals);
+ }
+
+ void NJointAnomalyDetectionAdaptiveWipingController::setAmplitude(Ice::Double amp, const Ice::Current&)
+ {
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->setAmplitude(amp);
+ }
+
+ void NJointAnomalyDetectionAdaptiveWipingController::setTargetForceInRootFrame(float targetForce, const Ice::Current&)
+ {
+ LockGuardType guard {controlDataMutex};
+ user2rtData.getWriteBuffer().targetForce = targetForce;
+ user2rtData.commitWrite();
+ }
+
+ void NJointAnomalyDetectionAdaptiveWipingController::runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&)
+ {
+ firstRun = true;
+ while (firstRun || rt2UserData.getUpToDateReadBuffer().waitTimeForCalibration < cfg->waitTimeForCalibration)
+ {
+ usleep(100);
+ }
+
+
+ Eigen::Matrix4f pose = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
+ dmpCtrl->setSpeed(tau);
+
+ ARMARX_IMPORTANT << "run DMP";
+ started = true;
+ dmpRunning = false;
+ }
+
+
+ void NJointAnomalyDetectionAdaptiveWipingController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& debugDrawer, const DebugObserverInterfacePrx& debugObs)
+ {
+ std::string datafieldName;
+ std::string debugName = "Periodic";
+ StringVariantBaseMap datafields;
+
+ Eigen::Matrix4f targetPoseDebug = debugRT.getUpToDateReadBuffer().targetPose;
+ datafields["target_x"] = new Variant(targetPoseDebug(0, 3));
+ datafields["target_y"] = new Variant(targetPoseDebug(1, 3));
+ datafields["target_z"] = new Variant(targetPoseDebug(2, 3));
+
+ Eigen::Matrix4f currentPoseDebug = debugRT.getUpToDateReadBuffer().currentPose;
+ datafields["current_x"] = new Variant(currentPoseDebug(0, 3));
+ datafields["current_y"] = new Variant(currentPoseDebug(1, 3));
+ datafields["current_z"] = new Variant(currentPoseDebug(2, 3));
+
+ Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
+ datafields["filteredforceInTool_x"] = new Variant(filteredForce(0));
+ datafields["filteredforceInTool_y"] = new Variant(filteredForce(1));
+ datafields["filteredforceInTool_z"] = new Variant(filteredForce(2));
+
+ Eigen::Vector3f filteredTorque = debugRT.getUpToDateReadBuffer().filteredTorque;
+ datafields["filteredtorqueInTool_x"] = new Variant(filteredTorque(0));
+ datafields["filteredtorqueInTool_y"] = new Variant(filteredTorque(1));
+ datafields["filteredtorqueInTool_z"] = new Variant(filteredTorque(2));
+
+
+ Eigen::Vector3f filteredForceInRoot = debugRT.getUpToDateReadBuffer().filteredForceInRoot;
+ datafields["filteredForceInRoot_x"] = new Variant(filteredForceInRoot(0));
+ datafields["filteredForceInRoot_y"] = new Variant(filteredForceInRoot(1));
+ datafields["filteredForceInRoot_z"] = new Variant(filteredForceInRoot(2));
+
+ Eigen::Vector3f rotationAxis = debugRT.getUpToDateReadBuffer().rotationAxis;
+ datafields["rotationAxis_x"] = new Variant(rotationAxis(0));
+ datafields["rotationAxis_y"] = new Variant(rotationAxis(1));
+ datafields["rotationAxis_z"] = new Variant(rotationAxis(2));
+
+ Eigen::Vector3f reactForce = debugRT.getUpToDateReadBuffer().reactForce;
+ datafields["reactForce_x"] = new Variant(reactForce(0));
+ datafields["reactForce_y"] = new Variant(reactForce(1));
+ datafields["reactForce_z"] = new Variant(reactForce(2));
+
+ Eigen::VectorXf targetVel = debugRT.getUpToDateReadBuffer().targetVel;
+ datafields["targetVel_x"] = new Variant(targetVel(0));
+ datafields["targetVel_y"] = new Variant(targetVel(1));
+ datafields["targetVel_z"] = new Variant(targetVel(2));
+ datafields["targetVel_ro"] = new Variant(targetVel(3));
+ datafields["targetVel_pi"] = new Variant(targetVel(4));
+ datafields["targetVel_ya"] = new Variant(targetVel(5));
+
+ Eigen::VectorXf currentTwist = debugRT.getUpToDateReadBuffer().currentTwist;
+ datafields["currentTwist_x"] = new Variant(currentTwist(0));
+ datafields["currentTwist_y"] = new Variant(currentTwist(1));
+ datafields["currentTwist_z"] = new Variant(currentTwist(2));
+ datafields["currentTwist_ro"] = new Variant(currentTwist(3));
+ datafields["currentTwist_pi"] = new Variant(currentTwist(4));
+ datafields["currentTwist_ya"] = new Variant(currentTwist(5));
+
+
+ Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
+ datafields["adaptK_x"] = new Variant(adaptK(0));
+ datafields["adaptK_y"] = new Variant(adaptK(1));
+
+ datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
+ datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
+
+ datafields["PhaseStop"] = new Variant(debugRT.getUpToDateReadBuffer().isPhaseStop);
+ datafields["rotAngle"] = new Variant(debugRT.getUpToDateReadBuffer().rotAngle);
+ datafields["wipingCounter"] = new Variant(debugRT.getUpToDateReadBuffer().wipingCounter);
+ datafields["mu"] = new Variant(debugRT.getUpToDateReadBuffer().mu);
+
+ Eigen::VectorXf estFri = debugRT.getUpToDateReadBuffer().estimatedFriction;
+ datafields["estimatedFriction_x"] = new Variant(estFri(0));
+ datafields["estimatedFriction_y"] = new Variant(estFri(1));
+
+ Eigen::VectorXf frictionInToolXY = debugRT.getUpToDateReadBuffer().frictionInToolXY;
+ datafields["frictionInToolXY_x"] = new Variant(frictionInToolXY(0));
+ datafields["frictionInToolXY_y"] = new Variant(frictionInToolXY(1));
+
+ Eigen::VectorXf velPInTool = debugRT.getUpToDateReadBuffer().velPInTool;
+ datafields["velPInTool_x"] = new Variant(velPInTool(0));
+ datafields["velPInTool_y"] = new Variant(velPInTool(1));
+ datafields["velPInTool_z"] = new Variant(velPInTool(2));
+
+ // datafields["targetVel_rx"] = new Variant(targetVel(3));
+ // datafields["targetVel_ry"] = new Variant(targetVel(4));
+ // datafields["targetVel_rz"] = new Variant(targetVel(5));
+
+ // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
+ // for (auto& pair : values)
+ // {
+ // datafieldName = pair.first + "_" + debugName;
+ // datafields[datafieldName] = new Variant(pair.second);
+ // }
+
+ // auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
+ // for (auto& pair : currentPose)
+ // {
+ // datafieldName = pair.first + "_" + debugName;
+ // datafields[datafieldName] = new Variant(pair.second);
+ // }
+
+ // datafieldName = "canVal_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
+ // datafieldName = "mpcFactor_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
+ // datafieldName = "error_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
+ // datafieldName = "posError_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
+ // datafieldName = "oriError_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
+ // datafieldName = "deltaT_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
+
+ datafieldName = "PeriodicDMP";
+ debugObs->setDebugChannel(datafieldName, datafields);
+
+
+ // draw force;
+ Eigen::Matrix4f globalPose = debugRT.getUpToDateReadBuffer().globalPose;
+ Eigen::Vector3f handPosition = globalPose.block<3, 1>(0, 3);
+ Eigen::Vector3f forceDir = debugRT.getUpToDateReadBuffer().globalFilteredForce;
+
+ debugDrawer->setArrowVisu("Force", "currentForce", new Vector3(handPosition), new Vector3(forceDir), DrawColor {0, 0, 1, 1}, 10 * forceDir.norm(), 3);
+
+ // draw direction of the tool
+ Eigen::Vector3f currentToolDir = debugRT.getUpToDateReadBuffer().currentToolDir;
+ debugDrawer->setArrowVisu("Tool", "Tool", new Vector3(handPosition), new Vector3(currentToolDir), DrawColor {1, 0, 0, 1}, 100, 3);
+ debugDrawer->setPoseVisu("target", "targetPose", new Pose(globalPose));
+
+ }
+
+
+
+ void NJointAnomalyDetectionAdaptiveWipingController::onDisconnectNJointController()
+ {
+ ARMARX_INFO << "stopped ...";
+ }
+
+
+
+}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
new file mode 100644
index 000000000..4de83bd73
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
@@ -0,0 +1,257 @@
+#pragma once
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+#include <RobotAPI/libraries/core/Trajectory.h>
+
+#include <RobotAPI/libraries/core/PIDController.h>
+
+namespace armarx
+{
+
+ TYPEDEF_PTRS_HANDLE(NJointAnomalyDetectionAdaptiveWipingController);
+ TYPEDEF_PTRS_HANDLE(NJointAnomalyDetectionAdaptiveWipingControllerControlData);
+
+ class NJointAnomalyDetectionAdaptiveWipingControllerControlData
+ {
+ public:
+ Eigen::VectorXf targetTSVel;
+ double canVal;
+ };
+
+ class pidController
+ {
+ public:
+ float Kp = 0, Kd = 0;
+ float lastError = 0;
+ float update(float dt, float error)
+ {
+ float derivative = (error - lastError) / dt;
+ float retVal = Kp * error + Kd * derivative;
+ lastError = error;
+ return retVal;
+ }
+ };
+
+ /**
+ * @brief The NJointAnomalyDetectionAdaptiveWipingController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
+ class NJointAnomalyDetectionAdaptiveWipingController :
+ public NJointControllerWithTripleBuffer<NJointAnomalyDetectionAdaptiveWipingControllerControlData>,
+ public NJointAnomalyDetectionAdaptiveWipingControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointAnomalyDetectionAdaptiveWipingControllerConfigPtr;
+ NJointAnomalyDetectionAdaptiveWipingController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current&) const;
+
+ // NJointController interface
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ // NJointAnomalyDetectionAdaptiveWipingControllerInterface interface
+ void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
+ void learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&);
+ bool isFinished(const Ice::Current&)
+ {
+ return false;
+ }
+
+ void setSpeed(Ice::Double times, const Ice::Current&);
+ void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
+ void setAmplitude(Ice::Double amp, const Ice::Current&);
+ void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
+ void setTargetForceInRootFrame(Ice::Float force, const Ice::Current&);
+ double getCanVal(const Ice::Current&)
+ {
+ return dmpCtrl->canVal;
+ }
+
+ protected:
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+ void onInitNJointController();
+ void onDisconnectNJointController();
+ void controllerRun();
+
+ private:
+ struct DebugBufferData
+ {
+ StringFloatDictionary latestTargetVelocities;
+ StringFloatDictionary currentPose;
+ double currentCanVal;
+ double mpcFactor;
+ double error;
+ double phaseStop;
+ double posError;
+ double oriError;
+ double deltaT;
+ };
+
+ TripleBuffer<DebugBufferData> debugOutputData;
+
+
+ struct DebugRTData
+ {
+ Eigen::Matrix4f targetPose;
+ Eigen::Vector3f filteredForce;
+ Eigen::Vector3f filteredForceInRoot;
+ Eigen::Vector3f filteredTorque;
+
+ Eigen::Vector3f rotationAxis;
+
+ Eigen::Vector3f reactForce;
+ Eigen::Vector3f adaptK;
+ Eigen::VectorXf targetVel;
+ Eigen::Matrix4f currentPose;
+ bool isPhaseStop;
+
+ Eigen::Matrix4f globalPose;
+ Eigen::Vector3f globalFilteredForce;
+ Eigen::Vector3f currentToolDir;
+ Eigen::VectorXf currentTwist;
+
+ float rotAngle;
+
+ int wipingCounter;
+ float mu;
+ Eigen::Vector2f estimatedFriction;
+ Eigen::Vector3f velPInTool;
+ Eigen::Vector2f frictionInToolXY;
+ };
+ TripleBuffer<DebugRTData> debugRT;
+
+
+ struct RTToControllerData
+ {
+ double currentTime;
+ double deltaT;
+ Eigen::Matrix4f currentPose;
+ Eigen::VectorXf currentTwist;
+ bool isPhaseStop;
+ };
+ TripleBuffer<RTToControllerData> rt2CtrlData;
+
+ struct RTToUserData
+ {
+ Eigen::Matrix4f currentTcpPose;
+ float waitTimeForCalibration;
+ };
+ TripleBuffer<RTToUserData> rt2UserData;
+
+ struct UserToRTData
+ {
+ float targetForce;
+ };
+ TripleBuffer<UserToRTData> user2rtData;
+
+
+ TaskSpaceDMPControllerPtr dmpCtrl;
+
+ std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
+ std::vector<ControlTarget1DoFActuatorTorque*> targets;
+
+ // anomaly detection
+ std::deque<Eigen::VectorXf> velocityHorizonList;
+ int velocityHorizon;
+
+ // velocity ik controller parameters
+ std::string nodeSetName;
+
+ bool started;
+ bool firstRun;
+ bool dmpRunning;
+
+ // friction estimation
+ float mu = 1.5f; // init friction coefficient
+ Eigen::Vector2f lastForceInToolXY;
+ double lastCanVal = 0.0;
+ int wipingCounter = 0;
+ std::deque<float> recordFrictionNorm;
+ std::deque<float> recordForceNormToSurface;
+ int frictionHorizon;
+ Eigen::Vector2f estimatedFriction;
+ float safeFrictionConeLowerLimit = 0.2;
+
+ // lose contact detection
+ float loseContactRatio = 0.2f;
+ int makingContactCounter = 0;
+ bool isMakingContact = false;
+ bool isLCREnabled = false;
+ bool isContactedOnce = false;
+ float forceControlGate = 1.0;
+ int lcrCounter = 500;
+
+ VirtualRobot::DifferentialIKPtr ik;
+ VirtualRobot::RobotNodePtr tcp;
+
+ NJointAnomalyDetectionAdaptiveWipingControllerConfigPtr cfg;
+ mutable MutexType controllerMutex;
+ PeriodicTask<NJointAnomalyDetectionAdaptiveWipingController>::pointer_type controllerTask;
+ Eigen::Matrix4f targetPose;
+ Eigen::Matrix4f initHandPose;
+
+ Eigen::Vector3f kpos;
+ Eigen::Vector3f dpos;
+ Eigen::Vector3f kori;
+ Eigen::Vector3f dori;
+ Eigen::VectorXf knull;
+ Eigen::VectorXf dnull;
+
+ Eigen::VectorXf nullSpaceJointsVec;
+ const SensorValueForceTorque* forceSensor;
+
+ // pid controllers
+ bool isForceControlEnabled;
+ bool isRotControlEnabled;
+ bool isTorqueControlEnabled;
+ bool isLCRControlEnabled;
+ PIDControllerPtr forcePID;
+ PIDControllerPtr rotPID;
+ PIDControllerPtr torquePID;
+ PIDControllerPtr lcrPID; // lose contact recover pid controller
+
+ // force torque related
+ Eigen::Vector6f targetFTInToolFrame;
+ Eigen::Vector3f filteredForce;
+ Eigen::Vector3f filteredTorque;
+ Eigen::Vector6f filteredFTCommand;
+ Eigen::Vector3f forceOffset;
+ Eigen::Vector3f currentForceOffset;
+
+ Eigen::Vector3f torqueOffset;
+ Eigen::Vector3f currentTorqueOffset;
+ float handMass;
+ Eigen::Vector3f handCOM;
+ Eigen::Vector3f gravityInRoot;
+
+ Eigen::Vector3f filteredForceInRoot;
+ Eigen::Vector3f filteredTorqueInRoot;
+
+ Eigen::Matrix3f toolTransform;
+ Eigen::Vector3f oriToolDir;
+ Eigen::Matrix3f origHandOri;
+ Eigen::VectorXf qvel_filtered;
+
+ Eigen::Vector3f adaptK;
+ float lastDiff;
+ Eigen::Vector2f lastPosition;
+ double changeTimer;
+
+ };
+} // namespace armarx
--
GitLab