From 0d819c2bd09cbbf1a6e3d7a99913a721aa07c89a Mon Sep 17 00:00:00 2001
From: Armar6 <armar6@h2t.com>
Date: Wed, 11 Apr 2018 12:28:17 +0200
Subject: [PATCH] modified the jsdmpctrl
---
.../NJointJointSpaceDMPController.cpp | 26 +++++++++----------
.../NJointJointSpaceDMPController.h | 7 ++---
.../test/CMakeLists.txt | 4 +--
3 files changed, 18 insertions(+), 19 deletions(-)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp
index 9626967f6..e6882de92 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp
@@ -71,12 +71,11 @@ namespace armarx
currentVelocity.push_back(currentPos.vel);
error += pow(currentPos.pos - targetState[i], 2);
-
}
double phaseDist;
- if(isDisturbance)
+ if (isDisturbance)
{
phaseDist = phaseDist1;
}
@@ -89,12 +88,12 @@ namespace armarx
phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
mpcFactor = 1 - (phaseStop / phaseL);
- if(mpcFactor < 0.1)
+ if (mpcFactor < 0.1)
{
isDisturbance = true;
}
- if(mpcFactor > 0.9)
+ if (mpcFactor > 0.9)
{
isDisturbance = false;
}
@@ -117,8 +116,8 @@ namespace armarx
const auto& jointName = dimNames.at(i);
if (targets.count(jointName) == 1)
{
- vel0 = currentState[i].vel / timeDuration;
- vel1 = phaseKp * (targetState[i] - currentPosition[i]);
+ double vel0 = currentState[i].vel / timeDuration;
+ double vel1 = phaseKp * (targetState[i] - currentPosition[i]);
double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
targets[jointName]->velocity = finished ? 0.0f : vel;
@@ -126,6 +125,8 @@ namespace armarx
}
}
+ debugOutputData.getWriteBuffer().currentCanVal = canVal;
+ debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
debugOutputData.commitWrite();
}
else
@@ -198,13 +199,8 @@ namespace armarx
}
- void NJointJointSpaceDMPController::showMessages(const Ice::Current &)
+ void NJointJointSpaceDMPController::showMessages(const Ice::Current&)
{
- ARMARX_INFO << "mpcFactor: " << mpcFactor;
- ARMARX_INFO << "canVal: " << canVal;
- ARMARX_INFO << "vel0: " << vel0;
- ARMARX_INFO << "vel1: " << vel1;
- ARMARX_INFO << "isDisturbance: " << isDisturbance;
}
void NJointJointSpaceDMPController::setTemporalFactor(double tau, const Ice::Current&)
@@ -225,15 +221,17 @@ namespace armarx
}
- void NJointJointSpaceDMPController::onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &debugObs)
+ void NJointJointSpaceDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
{
StringVariantBaseMap datafields;
auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
- for(auto& pair : values)
+ for (auto& pair : values)
{
datafields[pair.first] = new Variant(pair.second);
}
+ datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
+ datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
debugObs->setDebugChannel("latestDMPTargetVelocities", datafields);
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
index 6a3f4251a..a74cbb7bb 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
@@ -70,12 +70,14 @@ namespace armarx
void rtPreActivateController() override;
void rtPostDeactivateController() override;
- virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &);
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
private:
struct DebugBufferData
{
StringFloatDictionary latestTargetVelocities;
+ double currentCanVal;
+ double mpcFactor;
};
std::map<std::string, const SensorValue1DoFActuatorTorque*> torqueSensors;
@@ -106,13 +108,12 @@ namespace armarx
double phaseKp;
double mpcFactor;
- double vel0;
- double vel1;
bool isDisturbance;
std::vector<std::string> dimNames;
DMP::Vec<DMP::DMPState> currentState;
DMP::DVec targetState;
+
};
} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/test/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/test/CMakeLists.txt
index 19fe5d6d8..3afbfc395 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/test/CMakeLists.txt
@@ -1,5 +1,5 @@
# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore RobotAPINJointsController)
+SET(LIBS ${LIBS} ArmarXCore RobotAPINJointControllers)
-armarx_add_test(RobotAPINJointsControllerTest RobotAPINJointsControllerTest.cpp "${LIBS}")
\ No newline at end of file
+armarx_add_test(RobotAPINJointsControllerTest RobotAPINJointsControllerTest.cpp "${LIBS}")
--
GitLab