diff --git a/build/.gitkeep b/build/.gitkeep
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
index 0feba2e5ae20c2fc586b0b2eb4b876220d1870fa..ee6a6782e55eed678c05913df78bf38ee0678e5e 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
@@ -42,6 +42,8 @@ module armarx
double phaseKp = 1;
double timeDuration = 10;
+ double maxJointVel = 10;
+ bool isPhaseStop = false;
};
@@ -50,7 +52,7 @@ module armarx
void learnDMPFromFiles(Ice::StringSeq trajfiles);
bool isFinished();
void runDMP(Ice::DoubleSeq goals, double tau);
- void setTemporalFactor(double tau);
+ void setSpeed(double times);
void showMessages();
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 737dd63f2aa882c2a869e162f148cae309852191..86cffecf07d78f4e49cc765159a177704d51f115 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 9eb9f1d1b14210df8d849947c92def273b48a212..c7878d1ad5f066b68f31693a6d6c51dd3237c451 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
@@ -22,7 +22,6 @@ set(LIB_HEADERS)
list(APPEND LIB_HEADERS
- DMPController/NJointJointSpaceDMPController.h
DMPController/NJointJSDMPController.h
DMPController/NJointTSDMPController.h
DMPController/NJointCCDMPController.h
@@ -32,10 +31,9 @@ set(LIB_HEADERS)
DMPController/NJointPeriodicTSDMPForwardVelController.h
DMPController/NJointPeriodicTSDMPCompliantController.h
DMPController/NJointAdaptiveWipingController.h
-
+ DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
)
list(APPEND LIB_FILES
- DMPController/NJointJointSpaceDMPController.cpp
DMPController/NJointJSDMPController.cpp
DMPController/NJointTSDMPController.cpp
DMPController/NJointCCDMPController.cpp
@@ -45,7 +43,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 d19f4e5856fd718aae44d98650d787c8d7efead8..324b543ac056640e5f5c8ebe3ee54b70db577456 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());
@@ -267,7 +268,16 @@ namespace armarx
Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
Eigen::VectorXf targetVel(6);
+ 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);
@@ -275,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();
@@ -302,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)
{
@@ -319,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);
@@ -340,20 +355,40 @@ namespace armarx
if (filteredForceInRoot.norm() > fabs(cfg->minimumReactForce))
{
- Eigen::Vector3f desiredToolDir = filteredForceInRoot.normalized();// / filteredForceInRoot.norm();
+ 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();
- Eigen::Vector3f axis = currentToolDir.cross(desiredToolDir);
- float angle = acosf(currentToolDir.dot(desiredToolDir));
+ axis = currentToolDir.cross(desiredToolDir);
+ axis = axis.normalized();
+ angle = acosf(currentToolDir.dot(desiredToolDir));
+
- if (fabs(angle) < M_PI / 2)
+ if (fabs(angle) < M_PI / 2 && fabs(angle) > cfg->frictionCone)
{
- Eigen::AngleAxisf desiredToolRot(angle, axis);
+ // sigmoid function
+ float adaptedAngularKp = cfg->angularKp / (1 + exp(10 * (angle - M_PI / 4)));
+ float angularKp = fmin(adaptedAngularKp, cfg->angularKp);
+
+ // test axis
+ Eigen::Vector3f fixedAxis;
+ if (axis(1) > 0)
+ {
+ fixedAxis << 0.0, 1.0, 0.0;
+ }
+ else
+ {
+ fixedAxis << 0.0, -1.0, 0.0;
+ }
+ Eigen::AngleAxisf desiredToolRot(angle - cfg->frictionCone, fixedAxis);
Eigen::Matrix3f desiredRotMat = desiredToolRot * Eigen::Matrix3f::Identity();
Eigen::Vector3f angularDiff = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
- targetVel.tail(3) = cfg->angularKp * angularDiff;
+ targetVel.tail(3) = angularKp * angularDiff;
Eigen::Vector3f desiredRPY = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
Eigen::Vector3f checkedToolDir = desiredRotMat * currentToolDir;
@@ -443,11 +478,17 @@ namespace armarx
debugRT.getWriteBuffer().globalPose = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystem(targetPose);
debugRT.getWriteBuffer().currentPose = currentPose;
debugRT.getWriteBuffer().filteredForceInRoot = filteredForceInRoot;
- debugRT.getWriteBuffer().filteredForce = filteredForce;
+ 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.commitWrite();
rt2CtrlData.getWriteBuffer().currentPose = currentPose;
@@ -614,15 +655,26 @@ 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));
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));
@@ -632,6 +684,18 @@ namespace armarx
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));
@@ -641,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 9bfd26e64ad5551f5997634189034bd158e9b43e..7e1949ebe6ef33d71702eee37a72805b7da17736 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,9 @@ namespace armarx
Eigen::Matrix4f targetPose;
Eigen::Vector3f filteredForce;
Eigen::Vector3f filteredForceInRoot;
+ Eigen::Vector3f filteredTorque;
+
+ Eigen::Vector3f rotationAxis;
Eigen::Vector3f reactForce;
Eigen::Vector3f adaptK;
@@ -119,6 +124,9 @@ namespace armarx
Eigen::Matrix4f globalPose;
Eigen::Vector3f globalFilteredForce;
Eigen::Vector3f currentToolDir;
+ Eigen::VectorXf currentTwist;
+
+ float rotAngle;
};
TripleBuffer<DebugRTData> debugRT;
@@ -181,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 0000000000000000000000000000000000000000..c200c425ec99ee1ff3c6fa4e97bc921f7f1e7d9a
--- /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 0000000000000000000000000000000000000000..7e9e5998a6a7360691d1061d7663c98426cbabd1
--- /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;
+ size_t 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;
+ size_t 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
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
index 0a8ba174454a5f6c0b80f40aea0fc4caf75967f1..e149fadcd8125c1abea14feba1f4d099537043c9 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
@@ -1,5 +1,7 @@
#include "NJointJSDMPController.h"
+#include <ArmarXCore/core/time/CycleUtil.h>
+
namespace armarx
@@ -14,7 +16,7 @@ namespace armarx
NJointJSDMPController::NJointJSDMPController(const RobotUnitPtr&, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
{
- NJointJointSpaceDMPControllerConfigPtr cfg = NJointJointSpaceDMPControllerConfigPtr::dynamicCast(config);
+ cfg = NJointJointSpaceDMPControllerConfigPtr::dynamicCast(config);
ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "Needed type: NJointJointSpaceDMPControllerConfigPtr");
for (std::string jointName : cfg->jointNames)
@@ -23,8 +25,6 @@ namespace armarx
const SensorValueBase* sv = useSensorValue(jointName);
targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
positionSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
- torqueSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorTorque>()));
- gravityTorqueSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFGravityTorque>()));
velocitySensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorVelocity>()));
}
if (cfg->jointNames.size() == 0)
@@ -32,7 +32,7 @@ namespace armarx
ARMARX_ERROR << "cfg->jointNames.size() == 0";
}
- dmpPtr.reset(new DMP::UMIDMP(cfg->kernelSize, cfg->DMPKd, cfg->baseMode, cfg->tau));
+ dmpPtr.reset(new DMP::UMIDMP(cfg->kernelSize, cfg->DMPKd, cfg->baseMode, 1));
timeDuration = cfg->timeDuration;
phaseL = cfg->phaseL;
phaseK = cfg->phaseK;
@@ -58,86 +58,93 @@ namespace armarx
initSensorData.currentState.resize(cfg->jointNames.size());
controllerSensorData.reinitAllBuffers(initSensorData);
-
deltaT = 0;
}
void NJointJSDMPController::controllerRun()
{
- if (!controllerSensorData.updateReadBuffer())
+ if (!started)
{
- return;
+ for (size_t i = 0; i < dimNames.size(); ++i)
+ {
+ targetVels[i] = 0;
+ }
}
-
- currentState = controllerSensorData.getUpToDateReadBuffer().currentState;
- double deltaT = controllerSensorData.getUpToDateReadBuffer().deltaT;
- if (canVal > 1e-8)
+ else
{
-
- double phaseStop = 0;
- double error = 0;
- std::vector<double> currentPosition;
- currentPosition.resize(dimNames.size());
-
- for (size_t i = 0; i < currentState.size(); i++)
+ currentState = controllerSensorData.getUpToDateReadBuffer().currentState;
+ double deltaT = controllerSensorData.getUpToDateReadBuffer().deltaT;
+ if (canVal > 1e-8)
{
- DMP::DMPState currentPos = currentState[i];
- currentPosition[i] = currentPos.pos;
- error += pow(currentPos.pos - targetState[i], 2);
- }
-
- double phaseDist;
+ double phaseStop = 0;
+ double mpcFactor = 1;
+
+ std::vector<double> currentPosition;
+ double error = 0;
+ currentPosition.resize(dimNames.size());
+
+ for (size_t i = 0; i < currentState.size(); i++)
+ {
+ DMP::DMPState currentPos = currentState[i];
+ currentPosition[i] = currentPos.pos;
+ error += pow(currentPos.pos - targetState[i], 2);
+ }
+
+ if (cfg->isPhaseStop)
+ {
+ double phaseDist;
+
+ if (isDisturbance)
+ {
+ phaseDist = phaseDist1;
+ }
+ else
+ {
+ phaseDist = phaseDist0;
+ }
+
+ error = sqrt(error);
+ phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
+ mpcFactor = 1 - (phaseStop / phaseL);
+
+ if (mpcFactor < 0.1)
+ {
+ isDisturbance = true;
+ }
+
+ if (mpcFactor > 0.9)
+ {
+ isDisturbance = false;
+ }
+ }
+
+ canVal -= tau * deltaT * 1 / (1 + phaseStop);
+ double dmpDeltaT = deltaT / timeDuration;
+
+ currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, dmpDeltaT, targetState);
+
+ for (size_t i = 0; i < currentState.size(); ++i)
+ {
+ double vel0 = tau * currentState[i].vel / timeDuration;
+ double vel1 = phaseKp * (targetState[i] - currentPosition[i]);
+ double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
+ targetVels[i] = finished ? 0.0f : vel;
+
+ debugOutputData.getWriteBuffer().latestTargetVelocities[dimNames[i]] = vel;
+ }
+
+ debugOutputData.getWriteBuffer().currentCanVal = canVal;
+ debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
+ debugOutputData.commitWrite();
- if (isDisturbance)
- {
- phaseDist = phaseDist1;
}
else
{
- phaseDist = phaseDist0;
- }
-
- error = sqrt(error);
- phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
- double mpcFactor = 1 - (phaseStop / phaseL);
-
- if (mpcFactor < 0.1)
- {
- isDisturbance = true;
- }
-
- if (mpcFactor > 0.9)
- {
- isDisturbance = false;
- }
-
- canVal -= 1 / tau * deltaT * 1 / (1 + phaseStop);
- double dmpDeltaT = deltaT / timeDuration;
- dmpPtr->setTemporalFactor(tau);
-
- currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, dmpDeltaT, targetState);
-
- for (size_t i = 0; i < currentState.size(); ++i)
- {
- double vel0 = currentState[i].vel / timeDuration;
- double vel1 = phaseKp * (targetState[i] - currentPosition[i]);
- double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
- targetVels[i] = finished ? 0.0f : vel;
-
- debugOutputData.getWriteBuffer().latestTargetVelocities[dimNames[i]] = vel;
- }
-
- debugOutputData.getWriteBuffer().currentCanVal = canVal;
- debugOutputData.getWriteBuffer().mpcFactor = deltaT;
- debugOutputData.commitWrite();
-
- }
- else
- {
- finished = true;
- for (size_t i = 0; i < dimNames.size(); ++i)
- {
- targetVels[i] = 0;
+ finished = true;
+ for (size_t i = 0; i < dimNames.size(); ++i)
+ {
+ targetVels[i] = 0;
+ }
}
}
@@ -154,18 +161,24 @@ namespace armarx
DMP::DMPState currentPos;
currentPos.pos = (positionSensors.count(jointName) == 1) ? positionSensors[jointName]->position : 0.0f;
currentPos.vel = (velocitySensors.count(jointName) == 1) ? velocitySensors[jointName]->velocity : 0.0f;
- currentPos.vel *= timeDuration;
controllerSensorData.getWriteBuffer().currentState[i] = currentPos;
}
controllerSensorData.getWriteBuffer().deltaT = timeSinceLastIteration.toSecondsDouble();
- controllerSensorData.getWriteBuffer().currentTime += controllerSensorData.getWriteBuffer().deltaT;
+ controllerSensorData.getWriteBuffer().currentTime += timeSinceLastIteration.toSecondsDouble();
controllerSensorData.commitWrite();
std::vector<double> targetJointVels = rtGetControlStruct().targetJointVels;
for (size_t i = 0; i < dimNames.size(); ++i)
{
- targets[dimNames[i]]->velocity = targetJointVels[i];
+ if (fabs(targetJointVels[i]) > cfg->maxJointVel)
+ {
+ targets[dimNames[i]]->velocity = targetJointVels[i] < 0 ? -cfg->maxJointVel : cfg->maxJointVel;
+ }
+ else
+ {
+ targets[dimNames[i]]->velocity = targetJointVels[i];
+ }
}
@@ -201,40 +214,45 @@ namespace armarx
void NJointJSDMPController::runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&)
{
+ while (!rt2UserData.updateReadBuffer())
+ {
+ usleep(100);
+ }
+
targetState.clear();
targetState.resize(dimNames.size());
currentState.clear();
currentState.resize(dimNames.size());
for (size_t i = 0; i < dimNames.size(); i++)
{
- const auto& jointName = dimNames.at(i);
DMP::DMPState currentPos;
- currentPos.pos = (positionSensors.count(jointName) == 1) ? positionSensors[jointName]->position : 0.0f;
- currentPos.vel = (velocitySensors.count(jointName) == 1) ? velocitySensors[jointName]->velocity : 0.0f;
+ currentPos.pos = rt2UserData.getReadBuffer().qpos[i];
+ currentPos.vel = rt2UserData.getReadBuffer().qvel[i];
currentState[i] = currentPos;
targetState.push_back(currentPos.pos);
}
- dmpPtr->prepareExecution(goals, currentState, 1, tau);
+
+ dmpPtr->prepareExecution(goals, currentState, 1, 1);
this->goals = goals;
- this->tau = tau;
- canVal = timeDuration;
+ canVal = timeDuration * tau;
finished = false;
isDisturbance = false;
- ARMARX_INFO << "run DMP...";
- controllerTask->start();
+ ARMARX_INFO << "run DMP";
+ started = true;
+
}
void NJointJSDMPController::showMessages(const Ice::Current&)
{
}
- void NJointJSDMPController::setTemporalFactor(double tau, const Ice::Current&)
+ void NJointJSDMPController::setSpeed(double times, const Ice::Current&)
{
LockGuardType guard(controllerMutex);
- this->tau = tau;
+ tau = times;
}
void NJointJSDMPController::rtPreActivateController()
@@ -264,7 +282,20 @@ namespace armarx
void NJointJSDMPController::onInitNJointController()
{
ARMARX_INFO << "init ...";
- controllerTask = new PeriodicTask<NJointJSDMPController>(this, &NJointJSDMPController::controllerRun, 0.2);
+ started = false;
+ runTask("NJointJSDMPController", [&]
+ {
+ CycleUtil c(1);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ while (getState() == eManagedIceObjectStarted)
+ {
+ if (isControllerActive())
+ {
+ controllerRun();
+ }
+ c.waitForCycleDuration();
+ }
+ });
}
void NJointJSDMPController::onDisconnectNJointController()
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
index f593d4943bcaae6286a654d8dcfb46823abe3915..1f0e7a64e8d4bf0ba5c527fc12f98ec18e71bff5 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
@@ -23,15 +23,6 @@ namespace armarx
std::vector<double> targetJointVels;
};
-
- // class SimplePID
- // {
- // public:
- // float Kp = 0, Kd = 0;
- // float lastError = 0;
- // float update(float dt, float error);
- // };
-
/**
* @brief The NJointJSDMPController class
* @ingroup Library-RobotUnit-NJointControllers
@@ -56,7 +47,7 @@ namespace armarx
}
void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
- void setTemporalFactor(double tau, const Ice::Current&) override;
+ void setSpeed(double times, const Ice::Current&) override;
void runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&) override;
@@ -68,7 +59,7 @@ namespace armarx
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
private:
-
+ NJointJointSpaceDMPControllerConfigPtr cfg;
struct DebugBufferData
{
StringFloatDictionary latestTargetVelocities;
@@ -86,9 +77,13 @@ namespace armarx
};
TripleBuffer<NJointJSDMPControllerSensorData> controllerSensorData;
+ struct RTToUserData
+ {
+ Eigen::VectorXf qpos;
+ Eigen::VectorXf qvel;
+ };
+ TripleBuffer<RTToUserData> rt2UserData;
- std::map<std::string, const SensorValue1DoFActuatorTorque*> torqueSensors;
- std::map<std::string, const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
std::map<std::string, const SensorValue1DoFActuatorPosition*> positionSensors;
std::map<std::string, const SensorValue1DoFActuatorVelocity*> velocitySensors;
std::map<std::string, ControlTarget1DoFActuatorVelocity*> targets;
@@ -102,8 +97,8 @@ namespace armarx
DMP::Vec<DMP::DMPState> currentState;
double canVal;
double deltaT;
-
double tau;
+
double finished;
// phaseStop parameters
@@ -114,6 +109,7 @@ namespace armarx
double phaseKp;
bool isDisturbance;
+ bool started;
std::vector<std::string> dimNames;
DMP::DVec targetState;
std::vector<double> targetVels;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 5675efd9521c6092afb4e3e84f1cbb784f58584a..e56bb103a8da8ba235d4a38f331fc964f03a54df 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -6,6 +6,7 @@ namespace armarx
NJointTaskSpaceImpedanceDMPController::NJointTaskSpaceImpedanceDMPController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
{
+ ARMARX_INFO << "creating impedance dmp controller";
cfg = NJointTaskSpaceImpedanceDMPControllerConfigPtr::dynamicCast(config);
useSynchronizedRtRobot();
rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
@@ -104,6 +105,8 @@ namespace armarx
controllerSensorData.reinitAllBuffers(initControllerSensorData);
firstRun = true;
+
+ ARMARX_INFO << "Finished controller constructor ";
}
std::string NJointTaskSpaceImpedanceDMPController::getClassName(const Ice::Current&) const