diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
index ee6a6782e55eed678c05913df78bf38ee0678e5e..eaacf16044ba5a825876780a0c84c1f1bbb0564c 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
@@ -30,9 +30,7 @@ module armarx
class NJointJointSpaceDMPControllerConfig extends NJointControllerConfig
{
Ice::StringSeq jointNames;
- float DMPKd = 20;
int kernelSize = 100;
- double tau = 1;
int baseMode = 1;
double phaseL = 10;
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 86cffecf07d78f4e49cc765159a177704d51f115..e80c3cf92487806e92f7ea6fa90fc64f4b96e15a 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -563,6 +563,8 @@ module armarx
void setTargetForceInRootFrame(float force);
double getCanVal();
+
+ Ice::FloatSeq getAnomalyInput();
};
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
index c200c425ec99ee1ff3c6fa4e97bc921f7f1e7d9a..efb6befa7bbd81921ec4738d05f71d23bbae2a6f 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
@@ -263,9 +263,7 @@ namespace armarx
float dTf = (float)deltaT;
Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
- rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
- rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
- rt2UserData.commitWrite();
+
Eigen::Vector3f currentToolDir;
currentToolDir.setZero();
@@ -288,6 +286,11 @@ namespace armarx
velocityHorizonList.pop_front();
}
+ rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
+ rt2UserData.getWriteBuffer().tcpTranslVel = currentTwist.head(3);
+ rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
+
+ rt2UserData.commitWrite();
Eigen::VectorXf targetVel(6);
Eigen::Vector3f axis;
@@ -785,6 +788,13 @@ namespace armarx
dmpRunning = false;
}
+ std::vector<float> NJointAnomalyDetectionAdaptiveWipingController::getAnomalyInput(const Ice::Current&)
+ {
+ Eigen::Vector3f tvel = rt2UserData.getUpToDateReadBuffer().tcpTranslVel;
+ std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
+ return tvelvec;
+ }
+
void NJointAnomalyDetectionAdaptiveWipingController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& debugDrawer, const DebugObserverInterfacePrx& debugObs)
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
index 7e9e5998a6a7360691d1061d7663c98426cbabd1..8940f3b0476e6d178ee4736351818373391bff31 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
@@ -81,6 +81,7 @@ namespace armarx
return dmpCtrl->canVal;
}
+ std::vector<float> getAnomalyInput(const Ice::Current&);
protected:
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
@@ -149,6 +150,7 @@ namespace armarx
struct RTToUserData
{
Eigen::Matrix4f currentTcpPose;
+ Eigen::Vector3f tcpTranslVel;
float waitTimeForCalibration;
};
TripleBuffer<RTToUserData> rt2UserData;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
index e149fadcd8125c1abea14feba1f4d099537043c9..85746f5c1a71fe02e92b1469c117f8eecf6a33a9 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
@@ -16,12 +16,15 @@ namespace armarx
NJointJSDMPController::NJointJSDMPController(const RobotUnitPtr&, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
{
+ ARMARX_INFO << "creating joint space dmp controller ... ";
+ useSynchronizedRtRobot();
+
cfg = NJointJointSpaceDMPControllerConfigPtr::dynamicCast(config);
ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "Needed type: NJointJointSpaceDMPControllerConfigPtr");
for (std::string jointName : cfg->jointNames)
{
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::VelocityTorque);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
const SensorValueBase* sv = useSensorValue(jointName);
targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
positionSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
@@ -31,8 +34,9 @@ namespace armarx
{
ARMARX_ERROR << "cfg->jointNames.size() == 0";
}
+ ARMARX_INFO << "start creating dmpPtr ... " << " baseMode: " << cfg->baseMode;
- dmpPtr.reset(new DMP::UMIDMP(cfg->kernelSize, cfg->DMPKd, cfg->baseMode, 1));
+ dmpPtr.reset(new DMP::UMIDMP(cfg->kernelSize, 100, cfg->baseMode, 1));
timeDuration = cfg->timeDuration;
phaseL = cfg->phaseL;
phaseK = cfg->phaseK;
@@ -40,6 +44,7 @@ namespace armarx
phaseDist1 = cfg->phaseDist1;
phaseKp = cfg->phaseKp;
dimNames = cfg->jointNames;
+ ARMARX_INFO << "created dmpPtr ... ";
targetVels.resize(cfg->jointNames.size());
NJointJSDMPControllerControlData initData;
@@ -47,6 +52,7 @@ namespace armarx
for (size_t i = 0; i < cfg->jointNames.size(); ++i)
{
initData.targetJointVels[i] = 0;
+ targetVels[i] = 0;
}
reinitTripleBuffer(initData);
@@ -59,11 +65,14 @@ namespace armarx
controllerSensorData.reinitAllBuffers(initSensorData);
deltaT = 0;
+
+ qpos.resize(dimNames.size());
+ qvel.resize(dimNames.size());
}
void NJointJSDMPController::controllerRun()
{
- if (!started)
+ if (!started || finished)
{
for (size_t i = 0; i < dimNames.size(); ++i)
{
@@ -74,6 +83,7 @@ namespace armarx
{
currentState = controllerSensorData.getUpToDateReadBuffer().currentState;
double deltaT = controllerSensorData.getUpToDateReadBuffer().deltaT;
+
if (canVal > 1e-8)
{
double phaseStop = 0;
@@ -121,16 +131,18 @@ namespace armarx
canVal -= tau * deltaT * 1 / (1 + phaseStop);
double dmpDeltaT = deltaT / timeDuration;
- currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, dmpDeltaT, targetState);
+ currentDMPState = dmpPtr->calculateDirectlyVelocity(currentDMPState, canVal / timeDuration, dmpDeltaT, targetState);
- for (size_t i = 0; i < currentState.size(); ++i)
+ for (size_t i = 0; i < currentDMPState.size(); ++i)
{
- double vel0 = tau * currentState[i].vel / timeDuration;
+ double vel0 = tau * currentDMPState[i].vel / timeDuration;
double vel1 = phaseKp * (targetState[i] - currentPosition[i]);
- double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
- targetVels[i] = finished ? 0.0f : vel;
+ // double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
+ double vel = vel1 + vel0;
+ targetVels[i] = vel;
+ debugOutputData.getWriteBuffer().latestTargetVelocities[dimNames[i]] = (float)vel;
+ debugOutputData.getWriteBuffer().latestTargets[dimNames[i]] = (float)currentDMPState[i].pos;
- debugOutputData.getWriteBuffer().latestTargetVelocities[dimNames[i]] = vel;
}
debugOutputData.getWriteBuffer().currentCanVal = canVal;
@@ -161,16 +173,25 @@ 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;
+ qpos[i] = currentPos.pos;
+ qvel[i] = currentPos.vel;
controllerSensorData.getWriteBuffer().currentState[i] = currentPos;
}
controllerSensorData.getWriteBuffer().deltaT = timeSinceLastIteration.toSecondsDouble();
controllerSensorData.getWriteBuffer().currentTime += timeSinceLastIteration.toSecondsDouble();
-
controllerSensorData.commitWrite();
- std::vector<double> targetJointVels = rtGetControlStruct().targetJointVels;
+
+ rt2UserData.getWriteBuffer().qpos = qpos;
+ rt2UserData.getWriteBuffer().qvel = qvel;
+ rt2UserData.commitWrite();
+
+ Eigen::VectorXf targetJointVels = rtGetControlStruct().targetJointVels;
+ // ARMARX_INFO << targetJointVels;
+
for (size_t i = 0; i < dimNames.size(); ++i)
{
+
if (fabs(targetJointVels[i]) > cfg->maxJointVel)
{
targets[dimNames[i]]->velocity = targetJointVels[i] < 0 ? -cfg->maxJointVel : cfg->maxJointVel;
@@ -179,6 +200,7 @@ namespace armarx
{
targets[dimNames[i]]->velocity = targetJointVels[i];
}
+
}
@@ -206,13 +228,12 @@ namespace armarx
}
}
dmpPtr->learnFromTrajectories(trajs);
- dmpPtr->setOneStepMPC(true);
dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
ARMARX_INFO << "Learned DMP ... ";
}
- void NJointJSDMPController::runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&)
+ void NJointJSDMPController::runDMP(const Ice::DoubleSeq& goals, double times, const Ice::Current&)
{
while (!rt2UserData.updateReadBuffer())
{
@@ -223,6 +244,10 @@ namespace armarx
targetState.resize(dimNames.size());
currentState.clear();
currentState.resize(dimNames.size());
+ currentDMPState.clear();
+ currentDMPState.resize(dimNames.size());
+
+ std::vector<double> goalVec = goals;
for (size_t i = 0; i < dimNames.size(); i++)
{
DMP::DMPState currentPos;
@@ -230,16 +255,45 @@ namespace armarx
currentPos.vel = rt2UserData.getReadBuffer().qvel[i];
currentState[i] = currentPos;
+ currentDMPState[i] = currentPos;
+
targetState.push_back(currentPos.pos);
- }
- dmpPtr->prepareExecution(goals, currentState, 1, 1);
+ if (rtGetRobot()->getRobotNode(dimNames[i])->isLimitless())
+ {
+ double tjv = goalVec[i];
+ double cjv = currentPos.pos;
+ double diff = std::fmod(tjv - cjv, 2 * M_PI);
+ if (fabs(diff) > M_PI)
+ {
+ if (signbit(diff))
+ {
+ diff = - 2 * M_PI - diff;
+ }
+ else
+ {
+ diff = 2 * M_PI - diff;
+ }
+ tjv = cjv - diff;
+ }
+ else
+ {
+ tjv = cjv + diff;
+ }
+
+ goalVec[i] = tjv;
+ ARMARX_INFO << "dim name: " << dimNames[i] << " current state: qpos: " << currentPos.pos << " orig target: " << goals[i] << " current goal: " << tjv;
+ }
+
+
+ }
- this->goals = goals;
- canVal = timeDuration * tau;
+ dmpPtr->prepareExecution(goalVec, currentDMPState, 1, 1);
+ canVal = timeDuration;
finished = false;
isDisturbance = false;
+ tau = times;
ARMARX_INFO << "run DMP";
started = true;
@@ -273,6 +327,12 @@ namespace armarx
datafields[pair.first] = new Variant(pair.second);
}
+ values = debugOutputData.getUpToDateReadBuffer().latestTargets;
+ for (auto& pair : values)
+ {
+ datafields[pair.first + "_pos"] = new Variant(pair.second);
+ }
+
datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
debugObs->setDebugChannel("latestDMPTargetVelocities", datafields);
@@ -300,8 +360,6 @@ namespace armarx
void NJointJSDMPController::onDisconnectNJointController()
{
- controllerTask->stop();
- ARMARX_INFO << "stopped ...";
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
index 1f0e7a64e8d4bf0ba5c527fc12f98ec18e71bff5..9b3e627f6f9927a436b07c1ca2ad5130bf73510e 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
@@ -20,7 +20,7 @@ namespace armarx
class NJointJSDMPControllerControlData
{
public:
- std::vector<double> targetJointVels;
+ Eigen::VectorXf targetJointVels;
};
/**
@@ -49,7 +49,7 @@ namespace armarx
void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
void setSpeed(double times, const Ice::Current&) override;
- void runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&) override;
+ void runDMP(const Ice::DoubleSeq& goals, double times, const Ice::Current&) override;
void showMessages(const Ice::Current&) override;
@@ -63,6 +63,8 @@ namespace armarx
struct DebugBufferData
{
StringFloatDictionary latestTargetVelocities;
+ StringFloatDictionary latestTargets;
+
double currentCanVal;
double mpcFactor;
};
@@ -84,17 +86,18 @@ namespace armarx
};
TripleBuffer<RTToUserData> rt2UserData;
+
std::map<std::string, const SensorValue1DoFActuatorPosition*> positionSensors;
std::map<std::string, const SensorValue1DoFActuatorVelocity*> velocitySensors;
std::map<std::string, ControlTarget1DoFActuatorVelocity*> targets;
IceUtil::Time last;
- std::vector<double> goals;
DMP::UMIDMPPtr dmpPtr;
- bool DMPAsForwardControl;
double timeDuration;
DMP::Vec<DMP::DMPState> currentState;
+ DMP::Vec<DMP::DMPState> currentDMPState;
+
double canVal;
double deltaT;
double tau;
@@ -112,10 +115,12 @@ namespace armarx
bool started;
std::vector<std::string> dimNames;
DMP::DVec targetState;
- std::vector<double> targetVels;
+ Eigen::VectorXf targetVels;
mutable MutexType controllerMutex;
- PeriodicTask<NJointJSDMPController>::pointer_type controllerTask;
+
+ Eigen::VectorXf qpos;
+ Eigen::VectorXf qvel;
// ManagedIceObject interface
protected:
void controllerRun();