diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
index 7f67995656a2c61d0b50ef5dae42a1d430443de8..1d2c433b93afb51f8d5b3aba5788440fb6c7f0a9 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
@@ -32,7 +32,7 @@ set(LIB_HEADERS
)
-if (DMP_FOUND AND MMMCore_FOUND AND MMMTools_FOUND)
+if (DMP_FOUND )
message(STATUS "DMP libraries is ${DMP_LIBRARIES}")
include_directories(${DMP_INCLUDE_DIRS})
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index 28f315c41525c72ba1a89caf313162603acd9d2c..9dfb86763888c436e53384b6019d849582d9c966 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -96,27 +96,27 @@ namespace armarx
void NJointTSDMPController::controllerRun()
{
+ if (!controllerSensorData.updateReadBuffer())
+ {
+ return;
+ }
+ double deltaT = controllerSensorData.getReadBuffer().deltaT;
+ currentState = controllerSensorData.getReadBuffer().currentState;
+
double mpcFactor = 1;
double error = 0;
double phaseStop = 0;
double posError = 0;
double oriError = 0;
- double deltaT = 0;
- std::vector<double> currentposi;
- std::vector<double> currentori;
- currentposi.resize(3);
- currentori.resize(3);
+
if (canVal > 1e-8)
{
- if (!controllerSensorData.updateReadBuffer())
- {
- return;
- }
- deltaT = controllerSensorData.getReadBuffer().deltaT;
- currentState = controllerSensorData.getReadBuffer().currentState;
-
+ std::vector<double> currentposi;
+ std::vector<double> currentori;
+ currentposi.resize(3);
+ currentori.resize(3);
for (size_t i = 0; i < 3; ++i)
{
currentposi[i] = currentState[i].pos;
@@ -189,51 +189,33 @@ namespace armarx
{
targetVels[i] = 0;
}
-
- for (size_t i = 0; i < 3; ++i)
- {
- currentposi[i] = currentState[i].pos;
- }
-
- for (size_t i = 0; i < 3; ++i)
- {
- currentori[i] = currentState[i + 3].pos;
- }
}
-
- debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels[0];
- debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels[1];
- debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels[2];
- debugOutputData.getWriteBuffer().latestTargetVelocities["angle_x"] = targetVels[3];
- debugOutputData.getWriteBuffer().latestTargetVelocities["angle_y"] = targetVels[4];
- debugOutputData.getWriteBuffer().latestTargetVelocities["angle_z"] = targetVels[5];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_x"] = targetState[0];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_roll"] = targetState[3];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_pitch"] = targetState[4];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_yaw"] = targetState[5];
-
- debugOutputData.getWriteBuffer().realTCP["real_x"] = currentposi[0];
- debugOutputData.getWriteBuffer().realTCP["real_y"] = currentposi[1];
- debugOutputData.getWriteBuffer().realTCP["real_z"] = currentposi[2];
- debugOutputData.getWriteBuffer().realTCP["real_roll"] = currentori[0];
- debugOutputData.getWriteBuffer().realTCP["real_pitch"] = currentori[1];
- debugOutputData.getWriteBuffer().realTCP["real_yaw"] = currentori[2];
+ // 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().dmpTargets["dmp_x"] = targetState[0];
+ // debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
+ // debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
+ // debugOutputData.getWriteBuffer().dmpTargets["dmp_roll"] = targetState[3];
+ // debugOutputData.getWriteBuffer().dmpTargets["dmp_pitch"] = targetState[4];
+ // debugOutputData.getWriteBuffer().dmpTargets["dmp_yaw"] = targetState[5];
debugOutputData.getWriteBuffer().currentCanVal = canVal;
- debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
- debugOutputData.getWriteBuffer().error = error;
- debugOutputData.getWriteBuffer().phaseStop = phaseStop;
- debugOutputData.getWriteBuffer().posError = posError;
- debugOutputData.getWriteBuffer().oriError = oriError;
- debugOutputData.getWriteBuffer().deltaT = deltaT;
+ // debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
+ // debugOutputData.getWriteBuffer().error = error;
+ // debugOutputData.getWriteBuffer().phaseStop = phaseStop;
+ // debugOutputData.getWriteBuffer().posError = posError;
+ // debugOutputData.getWriteBuffer().oriError = oriError;
+ // debugOutputData.getWriteBuffer().deltaT = deltaT;
debugOutputData.commitWrite();
- LockGuardType guard {controlDataMutex};
+ // LockGuardType guard {controlDataMutex};
getWriterControlStruct().targetTSVel = targetVels;
writeControlStruct();
}
@@ -498,31 +480,24 @@ namespace armarx
void NJointTSDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
{
StringVariantBaseMap datafields;
- auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
+ // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
+ // for (auto& pair : values)
+ // {
+ // datafields[pair.first] = new Variant(pair.second);
- auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
- for (auto& pair : dmpTargets)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- auto realTCP = debugOutputData.getUpToDateReadBuffer().realTCP;
- for (auto& pair : realTCP)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
+ // auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
+ // for (auto& pair : dmpTargets)
+ // {
+ // datafields[pair.first] = new Variant(pair.second);
+ // }
datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
- datafields["poseError"] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
- datafields["phaseStop"] = new Variant(debugOutputData.getUpToDateReadBuffer().phaseStop);
- datafields["posError"] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
- datafields["oriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
- datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
+ // datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
+ // datafields["poseError"] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
+ // datafields["phaseStop"] = new Variant(debugOutputData.getUpToDateReadBuffer().phaseStop);
+ // datafields["posError"] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
+ // datafields["oriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
+ // datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
debugObs->setDebugChannel("DMPController", datafields);
}
@@ -535,8 +510,8 @@ namespace armarx
void NJointTSDMPController::onDisconnectComponent()
{
- controllerTask->stop();
ARMARX_INFO << "stopped ...";
+ controllerTask->stop();
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index d472aee7a1c6a8a62235891567560768fd05e41e..620b5c81145b5fb4be3755ad37bdf48c19ed5924 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -91,16 +91,15 @@ namespace armarx
private:
struct DebugBufferData
{
- StringFloatDictionary latestTargetVelocities;
- StringFloatDictionary dmpTargets;
- StringFloatDictionary realTCP;
+ // StringFloatDictionary latestTargetVelocities;
+ // StringFloatDictionary dmpTargets;
double currentCanVal;
- double mpcFactor;
- double error;
- double phaseStop;
- double posError;
- double oriError;
- double deltaT;
+ // double mpcFactor;
+ // double error;
+ // double phaseStop;
+ // double posError;
+ // double oriError;
+ // double deltaT;
};
TripleBuffer<DebugBufferData> debugOutputData;