From 951f66c8ee91ae7a1981cb205c8d70306aa6b78e Mon Sep 17 00:00:00 2001
From: ArmarX User <armarx@kit.edu>
Date: Wed, 6 Mar 2019 13:33:22 +0100
Subject: [PATCH] final work Innsbruck 2019
---
.../NJointTaskSpaceDMPController.ice | 2 +
.../DMPController/TaskSpaceDMPController.cpp | 5 +
.../DMPController/TaskSpaceDMPController.h | 11 +
.../JointPWMPositionController.cpp | 40 +-
.../JointPWMPositionController.h | 1 +
.../JointController/PWMVelocityController.cpp | 1 +
.../JointController/PWMVelocityController.h | 1 +
.../KITGripperBasisBoardData.cpp | 42 +-
.../KITGripperBasisBoardData.h | 6 +
.../Misc/TorqueEstimation.h | 59 +-
.../RobotAPINJointControllers/CMakeLists.txt | 4 +-
.../DMPController/NJointTSDMPController.cpp | 12 +-
.../DMPController/NJointTSDMPController.h | 3 +-
.../NJointTSMultiMPController.cpp | 562 ++++++++++++++++++
.../DMPController/NJointTSMultiMPController.h | 195 ++++++
15 files changed, 924 insertions(+), 20 deletions(-)
create mode 100644 source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSMultiMPController.cpp
create mode 100644 source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSMultiMPController.h
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 8388479b7..6c97923e2 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -92,6 +92,8 @@ module armarx
double getCanVal();
void resetDMP();
void stopDMP();
+ void pauseDMP();
+ void resumeDMP();
void setControllerTarget(float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
void setTorqueKp(StringFloatDictionary torqueKp);
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
index eb748893a..de7de3cdc 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
@@ -29,6 +29,11 @@ using namespace armarx;
void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist)
{
+ if (paused)
+ {
+ targetVel.setZero();
+ return;
+ }
if (canVal < 1e-8)
{
if (config.DMPStyle == "Periodic")
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
index 777c75da7..eb8d5c541 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
@@ -106,6 +106,7 @@ namespace armarx
this->isPhaseStopControl = isPhaseStopControl;
dmpName = name;
+ this->paused = false;
}
std::string getName()
@@ -167,10 +168,20 @@ namespace armarx
return dmpPtr;
}
+ void pauseController()
+ {
+ this->paused = true;
+ }
+ void resumeController()
+ {
+ this->paused = false;
+ }
+
double canVal;
bool isPhaseStopControl;
std::string dmpName;
TaskSpaceDMPControllerConfig config;
+ bool paused;
private:
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
index af1ca8a65..31b2274b1 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
@@ -27,6 +27,22 @@ JointPWMPositionController::JointPWMPositionController(const std::string deviceN
actorIndex = board->getActorIndex(deviceName);
dataPtr = jointData;
+ // add limitless joint controller to solve the flipping sensor value problem
+ limitlessController.positionPeriodLo = dataPtr->getSoftLimitLo();
+ limitlessController.positionPeriodHi = dataPtr->getSoftLimitHi();
+ limitlessController.maxDt = velocityControllerConfigDataPtr->maxDt;
+ limitlessController.maxV = velocityControllerConfigDataPtr->maxVelocityRad;
+ limitlessController.p = velocityControllerConfigDataPtr->posControlP;
+ // ARMARX_INFO << deviceName << " pos P: " << limitlessController.p;
+ // limitlessController.p = 10.0f; // for bit joint
+ // limitlessController.p = 3.0f; // for base joint
+ limitlessController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
+ limitlessController.deceleration = velocityControllerConfigDataPtr->maxDecelerationRad;
+ limitlessController.accuracy = 0.001f;
+ // ARMARX_INFO << "period low: " << limitlessController.positionPeriodLo << ", period high: " << limitlessController.positionPeriodHi;
+ // ARMARX_INFO << "maxDt: " << limitlessController.maxDt << ", maxV: " << limitlessController.maxV;
+ // ARMARX_INFO << "acceleration: " << limitlessController.acceleration << ", deceleration: " << limitlessController.deceleration;
+
posController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
posController.deceleration = velocityControllerConfigDataPtr->maxDecelerationRad;
posController.maxDt = velocityControllerConfigDataPtr->maxDt;
@@ -38,7 +54,7 @@ JointPWMPositionController::JointPWMPositionController(const std::string deviceN
posController.positionLimitLo = jointData->getSoftLimitLo();
posController.p = 3.0f;
this->isLimitless = jointData->isLimitless();
-
+ // ARMARX_INFO << "limitless: " << this->isLimitless;
}
JointPWMPositionController::~JointPWMPositionController() noexcept(true)
@@ -59,18 +75,26 @@ void JointPWMPositionController::rtRun(const IceUtil::Time& sensorValuesTimestam
if (target.isValid())
{
auto currentPosition = dataPtr->getPosition();
+ double newVel = 0.0;
if (isLimitless)
{
- ARMARX_RT_LOGF_WARNING("Position controller not implemented for limitless joints").deactivateSpam(10);
+ limitlessController.currentV = lastTargetVelocity;
+ limitlessController.dt = timeSinceLastIteration.toSecondsDouble();
+ limitlessController.currentPosition = currentPosition;
+ limitlessController.targetPosition = target.position;
+ newVel = limitlessController.run();
+ // ARMARX_IMPORTANT << "using limitless position controller";
+ // ARMARX_RT_LOGF_WARNING("Position controller not implemented for limitless joints").deactivateSpam(10);
}
else
{
posController.currentPosition = currentPosition;
+ posController.currentV = lastTargetVelocity;
+ posController.dt = timeSinceLastIteration.toSecondsDouble();
+ posController.targetPosition = target.position;
+ newVel = posController.run();
}
- posController.currentV = lastTargetVelocity;
- posController.dt = timeSinceLastIteration.toSecondsDouble();
- posController.targetPosition = target.position;
- double newVel = posController.run();
+
// double newVel = posController.p * (posController.targetPosition - posController.currentPosition);
// newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
// ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel);
@@ -92,8 +116,6 @@ void JointPWMPositionController::rtRun(const IceUtil::Time& sensorValuesTimestam
// ARMARX_RT_LOGF_INFO("%s: position: %.f, target position: %.f, targetvelocity: %.f, target PWM: %d", name,
// currentPosition, target.position, newVel, targetPWM).deactivateSpam(1);
// ARMARX_INFO << deactivateSpam(1) << VAROUT(name) << VAROUT(currentPosition) << VAROUT(target.position) << VAROUT(newVel) << VAROUT(targetPWM);
-
-
}
else
{
@@ -235,8 +257,6 @@ StringVariantBaseMap JointPWMPositionController::publish(const DebugDrawerInterf
{"pidPropCV", new Variant(controller.pid->previousError * controller.pid->Kp)},
{"pidDiffCV", new Variant(controller.pid->derivative * controller.pid->Kd)},
{"pidUsed", new Variant(posController.getCurrentlyPIDActive())}
-
-
};
}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
index b5ab09fff..62818578f 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
@@ -38,6 +38,7 @@ namespace armarx
PWMVelocityControllerConfigurationCPtr config;
PWMVelocityController controller;
PositionThroughVelocityControllerWithAccelerationAndPositionBounds posController;
+ PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition limitlessController;
ControlTarget1DoFActuatorPosition target;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp
index 318dde917..d5451bbd6 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp
@@ -86,6 +86,7 @@ namespace armarx
configData.maxDecelerationRad = node.first_node("maxDecelerationRad").value_as_float();
configData.maxDt = node.first_node("maxDt").value_as_float();
configData.maxDt = node.first_node("directSetVLimit").value_as_float();
+ configData.posControlP = node.first_node("posControlP").value_as_float();
configData.p = node.first_node("p").value_as_float();
configData.i = node.first_node("i").value_as_float();
configData.d = node.first_node("d").value_as_float();
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h
index 47bc0c2e9..2dc6b7e80 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h
@@ -48,6 +48,7 @@ namespace armarx
float maxDecelerationRad;
float maxDt;
float directSetVLimit;
+ float posControlP;
float p;
float i;
float d;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
index 7cc4f2762..a6530c674 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
@@ -62,6 +62,7 @@ namespace armarx
actorData.at(connectorIndex)->parallelGripperDecouplingFactor = configNode.first_node("ParallelGripperDecouplingFactor").value_as_float();
actorData.at(connectorIndex)->parallelControlEnabled = configNode.first_node("ParallelControlSiblingIndex").value_as_int32();
+ actorData.at(connectorIndex)->relativePositionSensorOnly = configNode.first_node("RelativePositionSensorOnly").value_as_int32();
actorData.at(connectorIndex)->currentPWMBoundGradient = configNode.first_node("CurrentPWMBoundGradient").value_as_float();
actorData.at(connectorIndex)->currentPWMBoundOffset = configNode.first_node("CurrentPWMBoundOffset").value_as_int32();
@@ -83,6 +84,9 @@ namespace armarx
}
}
+ jumpingOffset = 0.0;
+ firstRun = true;
+ lastAbsoluteAngle = 0.0;
}
void KITGripperBasisBoardData::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
@@ -103,13 +107,29 @@ namespace armarx
if (d.getSiblingControlActorIndex() >= 0)
{
auto& d2 = actorData.at(d.getSiblingControlActorIndex());
- // ARMARX_IMPORTANT << "decoupling factor: " << d.parallelGripperDecouplingFactor;
+ if (firstRun)
+ {
+ lastAbsoluteAngle = d2->position.value;
+ firstRun = false;
+ }
+
+ // better to use relative position, but experiments shows that relative position measurements fails sometimes, reason unknown
+ // d.adjustedRelativePosition = d.relativePosition.value + d2->relativePosition.value * d.parallelGripperDecouplingFactor;
+
+ if (d2->position.value - lastAbsoluteAngle > M_PI)
+ {
+ jumpingOffset -= 2 * M_PI;
+ }
+ else if (d2->position.value - lastAbsoluteAngle < -M_PI)
+ {
+ jumpingOffset += 2 * M_PI;
+ }
d.adjustedRelativePosition = d.relativePosition.value +
- d2->relativePosition.value * d.parallelGripperDecouplingFactor;
+ (d2->position.value + jumpingOffset) * d.parallelGripperDecouplingFactor;
+ lastAbsoluteAngle = d2->position.value;
}
else
{
- //ARMARX_IMPORTANT << "not even in the right place";
d.adjustedRelativePosition = d.relativePosition.value;
}
@@ -132,7 +152,16 @@ namespace armarx
}
- if (i > 1)
+ // if (i > 1)
+ // {
+ // d.position.value = d.adjustedRelativePosition;
+ // }
+ // else
+ // {
+ // d.position.read();
+ // }
+
+ if (d.getSiblingControlActorIndex() >= 0 || d.getRelativePositionSensorOnly() >= 0)
{
d.position.value = d.adjustedRelativePosition;
}
@@ -273,6 +302,11 @@ namespace armarx
return parallelControlEnabled;
}
+ int32_t ActorData::getRelativePositionSensorOnly() const
+ {
+ return relativePositionSensorOnly;
+ }
+
bool ActorData::getPositionControlEnabled() const
{
return positionControlEnabled;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
index f0d9ae529..c54141772 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
@@ -98,6 +98,8 @@ namespace armarx
int32_t getSiblingControlActorIndex() const;
+ int32_t getRelativePositionSensorOnly() const;
+
bool getPositionControlEnabled() const;
bool getVelocityControlEnabled() const;
@@ -129,6 +131,7 @@ namespace armarx
int32_t currentMinPWM = 0;
size_t maxPWM;
int32_t parallelControlEnabled = -1;
+ int32_t relativePositionSensorOnly = -1;
VirtualRobot::RobotNodePtr robotNode;
LinearConvertedValue<int32_t> targetPWMPtr;
bool positionControlEnabled = true;
@@ -156,6 +159,9 @@ namespace armarx
KITGripperBasisBoardIN_t* sensorIN;
std::vector<ActorDataPtr> actorData;
+ bool firstRun;
+ float jumpingOffset;
+ float lastAbsoluteAngle; // this is the memory for last joint angle of the PG_base joint. to detect the jumping of the sensor value
};
using KITGripperBasisBoardDataPtr = std::shared_ptr<KITGripperBasisBoardData>;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h
index 3c94921cc..51736ff07 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h
@@ -2,6 +2,7 @@
/* Includes ------------------------------------------------------------------*/
#include "TorqueEstimationWeights.h"
+#include <stdio.h>
float imgIn[5];
float imgFcl1[32];
@@ -175,9 +176,44 @@ uint8_t fcl6(void)
}
+float linearModel_dcx16(int32_t nI, int32_t pwm)
+{
+ float n = (float) nI * 1000 * 60 / 4096; //ticks/ms -> turns/min
+ float T_f = 1000.0 / 406.0; //übersetzung getriebe + Nm->m
+ float motor_a = 265.0; //Drehzahlkonstante [min-1 V-1]
+ float motor_b = 620.0; //Kennliniensteigung [min-1 mNm-1]
+ float motor_eta = 0.55 * 0.78; //Wirkungsgrad Getriebe *Motor, max from datasheet:0.75*0.85
+ float Umax = 48.0; //Spannung bei pwm max
+ float pwmmax = 3000.0;
+ float pwm_zero = 400.0;
+ float U;
+ float T_motor;
+
+
+ U = (float(fabs(pwm) - pwm_zero) / (pwmmax - pwm_zero)) * Umax;
+ if (U < 0)
+ {
+ U = 0;
+ }
+ if (pwm < 0)
+ {
+ U *= -1;
+ }
+ if (pwm == 0)
+ {
+ U = 0;
+ }
+
+ //U(M,n)=(n-b*M)/a
+ T_motor = (U * motor_a - n) / -motor_b;
+ auto T = T_motor * motor_eta / T_f;
+
+ return T;
+}
+
float linearModel_dcx22(int32_t nI, int32_t pwm)
{
- float n = (float) nI * 1000 * 60 / 4096; //ticks/ms -> U/min
+ float n = (float) nI * 1000 * 60 / 4096; //ticks/ms -> turns/min
float T_f = 1000. / 231.; //übersetzung getriebe + Nm->m
float motor_a = 226. + 30.; //Drehzahlkonstante [min-1 V-1]
float motor_b = 123.; //Kennliniensteigung [min-1 mNm-1]
@@ -211,8 +247,27 @@ float linearModel_dcx22(int32_t nI, int32_t pwm)
}
-float estimateTorque(int32_t n, int32_t pwm)
+float estimateTorque(int32_t n, int32_t pwm)//, int motorModel, bool linearModelOnly = false)
{
+ bool linearModelOnly = true;
+ int motorModel = 16;
+ if (linearModelOnly)
+ {
+ if (motorModel == 16)
+ {
+ return linearModel_dcx16(n, pwm);
+ }
+ else if (motorModel == 22)
+ {
+ return linearModel_dcx22(n, pwm);
+ }
+ else
+ {
+ std::cout << "motor model should be set to 16 or 22" << std::endl;
+ return 0.0;
+ }
+ // return linearModel_dcx22(n, pwm);
+ }
float n_input = (float)n / n_factor;
float pwm_input = (float)pwm / pwm_factor;
// float inputData[6];
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
index c2da980f6..e12f16cab 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
@@ -49,7 +49,7 @@ if (DMP_FOUND )
DMPController/NJointBimanualCCDMPController.h
DMPController/NJointTaskSpaceImpedanceDMPController.h
DMPController/NJointBimanualForceMPController.h
-
+# DMPController/NJointTSMultiMPController.h
)
list(APPEND LIB_FILES
DMPController/NJointJointSpaceDMPController.cpp
@@ -59,7 +59,7 @@ if (DMP_FOUND )
DMPController/NJointBimanualCCDMPController.cpp
DMPController/NJointTaskSpaceImpedanceDMPController.cpp
DMPController/NJointBimanualForceMPController.cpp
-
+# DMPController/NJointTSMultiMPController.cpp
)
list(APPEND LIBS ${DMP_LIBRARIES} DMPController)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index ec9bf0b23..b24cb76fb 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -245,7 +245,7 @@ namespace armarx
targetPose = currentPose;
}
- ARMARX_IMPORTANT << "targetPose: " << targetPose;
+ // ARMARX_IMPORTANT << "targetPose: " << targetPose;
Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
@@ -418,6 +418,16 @@ namespace armarx
writeControlStruct();
}
+ void NJointTSDMPController::pauseDMP(const Ice::Current&)
+ {
+ taskSpaceDMPController->pauseController();
+ }
+
+ void NJointTSDMPController::resumeDMP(const Ice::Current&)
+ {
+ taskSpaceDMPController->resumeController();
+ }
+
void NJointTSDMPController::resetDMP(const Ice::Current&)
{
if (started)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index e50b9890b..9eea8c9bb 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -86,7 +86,8 @@ namespace armarx
void setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&) override;
void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&) override;
void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&) override;
-
+ void pauseDMP(const Ice::Current&) override;
+ void resumeDMP(const Ice::Current&) override;
void resetDMP(const Ice::Current&) override;
void stopDMP(const Ice::Current&) override;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSMultiMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSMultiMPController.cpp
new file mode 100644
index 000000000..4a65e60de
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSMultiMPController.cpp
@@ -0,0 +1,562 @@
+/* Jeff
+ *
+ */
+#include "NJointTSMultiMPController.h"
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointTSMultiMPController> registrationControllerNJointTSMultiMPController("NJointTSMultiMPController");
+
+ NJointTSMultiMPController::NJointTSMultiMPController(const RobotUnitPtr&, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ useSynchronizedRtRobot();
+ cfg = NJointTaskSpaceDMPControllerConfigPtr::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::Velocity1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
+
+ // const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
+ // const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
+
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+
+ // torqueSensors.push_back(torqueSensor);
+ // gravityTorqueSensors.push_back(gravityTorqueSensor);
+ velocitySensors.push_back(velocitySensor);
+ positionSensors.push_back(positionSensor);
+ };
+
+ tcp = (cfg->tcpName.empty()) ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName);
+ ARMARX_CHECK_EXPRESSION_W_HINT(tcp, cfg->tcpName);
+
+ // set tcp controller
+ tcpController.reset(new CartesianVelocityController(rns, tcp));
+ nodeSetName = cfg->nodeSetName;
+ torquePIDs.resize(tcpController->rns->getSize(), pidController());
+
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+
+
+ finished = false;
+ TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
+ taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
+ taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
+ taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
+ taskSpaceDMPConfig.DMPStyle = cfg->dmpStyle;
+ taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
+ taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
+ taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
+ taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
+ taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
+ taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
+ taskSpaceDMPConfig.phaseStopParas.Dori = 0;
+ taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
+ taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
+ taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
+
+ taskSpaceDMPController.reset(new TaskSpaceDMPController("default", taskSpaceDMPConfig, false));
+
+ // initialize tcp position and orientation
+
+
+ NJointTSMultiMPControllerSensorData initSensorData;
+ initSensorData.deltaT = 0;
+ initSensorData.currentTime = 0;
+ initSensorData.currentPose.setZero();
+ initSensorData.currentTwist.setZero();
+ controllerSensorData.reinitAllBuffers(initSensorData);
+
+
+
+ debugName = cfg->debugName;
+
+ KpF = cfg->Kp_LinearVel;
+ KoF = cfg->Kp_AngularVel;
+ DpF = cfg->Kd_LinearVel;
+ DoF = cfg->Kd_AngularVel;
+
+ filtered_qvel.setZero(targets.size());
+ vel_filter_factor = cfg->vel_filter;
+
+ filtered_position.setZero();
+ pos_filter_factor = cfg->pos_filter;
+
+ // jlhigh = rns->getNode("..")->getJointLimitHi();
+ // jllow = rns->getNode("")->getJointLimitLo();
+ firstRun = true;
+
+ jointLowLimits.setZero(targets.size());
+ jointHighLimits.setZero(targets.size());
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ VirtualRobot::RobotNodePtr rn = rns->getAllRobotNodes().at(i);
+
+ jointLowLimits(i) = rn->getJointLimitLo();
+ jointHighLimits(i) = rn->getJointLimitHi();
+ }
+
+ started = false;
+
+ NJointTSMultiMPControllerInterfaceData initInterfaceData;
+ initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
+ interfaceData.reinitAllBuffers(initInterfaceData);
+ }
+
+ std::string NJointTSMultiMPController::getClassName(const Ice::Current&) const
+ {
+ return "NJointTSMultiMPController";
+ }
+
+ void NJointTSMultiMPController::controllerRun()
+ {
+ if (!started)
+ {
+ return;
+ }
+
+ if (!controllerSensorData.updateReadBuffer() || !taskSpaceDMPController)
+ {
+ return;
+ }
+ double deltaT = controllerSensorData.getReadBuffer().deltaT;
+ Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
+ Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
+
+
+ LockGuardType guard {controllerMutex};
+ taskSpaceDMPController->flow(deltaT, currentPose, currentTwist);
+
+
+ if (taskSpaceDMPController->canVal < 1e-8)
+ {
+ finished = true;
+ }
+ targetVels = taskSpaceDMPController->getTargetVelocity();
+ targetPose = taskSpaceDMPController->getTargetPoseMat();
+ std::vector<double> targetState = taskSpaceDMPController->getTargetPose();
+
+ // ARMARX_IMPORTANT << "CanVal: " << taskSpaceDMPController->canVal;
+ // ARMARX_IMPORTANT << "currentPose: " << currentPose;
+ // ARMARX_IMPORTANT << "targetVels: " << targetVels;
+ // ARMARX_IMPORTANT << "targetPose: " << targetPose;
+
+ 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_qw"] = targetState[3];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_qx"] = targetState[4];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_qy"] = targetState[5];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_qz"] = targetState[6];
+ 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 = taskSpaceDMPController->debugData.canVal;
+ debugOutputData.getWriteBuffer().mpcFactor = taskSpaceDMPController->debugData.mpcFactor;
+ debugOutputData.getWriteBuffer().error = taskSpaceDMPController->debugData.poseError;
+ debugOutputData.getWriteBuffer().posError = taskSpaceDMPController->debugData.posiError;
+ debugOutputData.getWriteBuffer().oriError = taskSpaceDMPController->debugData.oriError;
+ debugOutputData.getWriteBuffer().deltaT = deltaT;
+
+ debugOutputData.commitWrite();
+
+ getWriterControlStruct().targetTSVel = targetVels;
+ getWriterControlStruct().targetPose = targetPose;
+
+ writeControlStruct();
+
+ }
+
+
+ void NJointTSMultiMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
+ interfaceData.getWriteBuffer().currentTcpPose = currentPose;
+ interfaceData.commitWrite();
+
+ if (firstRun)
+ {
+ filtered_position = currentPose.block<3, 1>(0, 3);
+
+ firstRun = false;
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->velocity = 0;
+ }
+ return;
+ }
+ else
+ {
+ filtered_position = (1 - pos_filter_factor) * filtered_position + pos_filter_factor * currentPose.block<3, 1>(0, 3);
+ }
+
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+
+ Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+
+ Eigen::VectorXf qvel;
+ qvel.resize(velocitySensors.size());
+ Eigen::VectorXf qpos;
+ qpos.resize(positionSensors.size());
+ for (size_t i = 0; i < velocitySensors.size(); ++i)
+ {
+ qvel(i) = velocitySensors[i]->velocity;
+ qpos(i) = positionSensors[i]->position;
+ }
+
+ filtered_qvel = (1 - vel_filter_factor) * filtered_qvel + vel_filter_factor * qvel;
+ Eigen::VectorXf tcptwist = jacobi * filtered_qvel;
+
+ controllerSensorData.getWriteBuffer().currentPose = currentPose;
+ controllerSensorData.getWriteBuffer().currentTwist = tcptwist;
+ controllerSensorData.getWriteBuffer().deltaT = deltaT;
+ controllerSensorData.getWriteBuffer().currentTime += deltaT;
+ controllerSensorData.commitWrite();
+
+
+
+ Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
+ Eigen::Matrix4f targetPose = rtGetControlStruct().targetPose;
+
+ if (started)
+ {
+ targetVel = rtGetControlStruct().targetTSVel;
+ targetPose = rtGetControlStruct().targetPose;
+ }
+ else
+ {
+ targetVel.setZero(6);
+ targetPose = currentPose;
+ }
+
+ // ARMARX_IMPORTANT << "targetPose: " << targetPose;
+
+ Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
+ Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+
+ Eigen::VectorXf rtTargetVel;
+ rtTargetVel.resize(6);
+ rtTargetVel.block<3, 1>(0, 0) = KpF * (targetPose.block<3, 1>(0, 3) - filtered_position) + DpF * (targetVel.block<3, 1>(0, 0) - tcptwist.block<3, 1>(0, 0));
+ rtTargetVel.block<3, 1>(3, 0) = KoF * errorRPY + DoF * (targetVel.block<3, 1>(3, 0) - tcptwist.block<3, 1>(3, 0));
+ // rtTargetVel = targetVel;
+
+
+
+ float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
+ if (normLinearVelocity > cfg->maxLinearVel)
+ {
+ rtTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
+ }
+
+ float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
+ if (normAngularVelocity > cfg->maxAngularVel)
+ {
+ rtTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
+ }
+
+
+ // cartesian vel controller
+ Eigen::VectorXf x;
+ auto mode = rtGetControlStruct().mode;
+
+ if (mode == VirtualRobot::IKSolver::All)
+ {
+ x.resize(6);
+ for (size_t i = 0; i < 6; i++)
+ {
+ x(i) = rtTargetVel(i);
+ }
+ }
+ else if (mode == VirtualRobot::IKSolver::Position)
+ {
+ x.resize(3);
+ for (size_t i = 0; i < 3; i++)
+ {
+ x(i) = rtTargetVel(i);
+ }
+
+ }
+ else if (mode == VirtualRobot::IKSolver::Orientation)
+ {
+ x.resize(3);
+ for (size_t i = 0; i < 3; i++)
+ {
+ x(i) = rtTargetVel(i + 3);
+ }
+ }
+ else
+ {
+
+ // No mode has been set
+ return;
+ }
+
+ Eigen::VectorXf jnv = Eigen::VectorXf::Zero(tcpController->rns->getSize());
+ float jointLimitAvoidanceKp = rtGetControlStruct().avoidJointLimitsKp;
+ if (jointLimitAvoidanceKp > 0)
+ {
+ jnv += jointLimitAvoidanceKp * tcpController->calculateJointLimitAvoidance();
+ }
+ for (size_t i = 0; i < tcpController->rns->getSize(); i++)
+ {
+ jnv(i) += rtGetControlStruct().nullspaceJointVelocities.at(i);
+ }
+
+ Eigen::VectorXf jointTargetVelocities = tcpController->calculate(x, jnv, mode);
+ ARMARX_CHECK_EXPRESSION(!targets.empty());
+ ARMARX_CHECK_LESS(targets.size(), 1000);
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->velocity = jointTargetVelocities(i);
+
+ if (!targets.at(i)->isValid() || fabs(targets.at(i)->velocity) > cfg->maxJointVelocity)
+ {
+ ARMARX_IMPORTANT << deactivateSpam(1, std::to_string(i))
+ << "Velocity controller target is invalid - setting to zero! set value: " << targets.at(i)->velocity;
+ targets.at(i)->velocity = 0.0f;
+ }
+
+ }
+
+
+ }
+
+
+ void NJointTSMultiMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
+ {
+ taskSpaceDMPController->learnDMPFromFiles(fileNames);
+
+ ARMARX_INFO << "Learned DMP ... ";
+ }
+
+ void NJointTSMultiMPController::setSpeed(Ice::Double times, const Ice::Current&)
+ {
+ LockGuardType guard {controllerMutex};
+ taskSpaceDMPController->setSpeed(times);
+ }
+
+ void NJointTSMultiMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
+ {
+ LockGuardType guard {controllerMutex};
+ ARMARX_INFO << "setting via points ";
+ taskSpaceDMPController->setViaPose(u, viapoint);
+ }
+
+ void NJointTSMultiMPController::removeAllViaPoints(const Ice::Current&)
+ {
+ LockGuardType guard {controllerMutex};
+ ARMARX_INFO << "setting via points ";
+ taskSpaceDMPController->removeAllViaPoints();
+ }
+
+
+ void NJointTSMultiMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
+ {
+ taskSpaceDMPController->setGoalPoseVec(goals);
+ }
+
+ void NJointTSMultiMPController::setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&)
+ {
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().avoidJointLimitsKp = avoidJointLimitsKp;
+ getWriterControlStruct().mode = ModeFromIce(mode);
+ writeControlStruct();
+ }
+
+ VirtualRobot::IKSolver::CartesianSelection NJointTSMultiMPController::ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode)
+ {
+ if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::ePosition)
+ {
+ return VirtualRobot::IKSolver::CartesianSelection::Position;
+ }
+ if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eOrientation)
+ {
+ return VirtualRobot::IKSolver::CartesianSelection::Orientation;
+ }
+ if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eAll)
+ {
+ return VirtualRobot::IKSolver::CartesianSelection::All;
+ }
+ ARMARX_ERROR_S << "invalid mode " << mode;
+ return (VirtualRobot::IKSolver::CartesianSelection)0;
+ }
+
+
+ void NJointTSMultiMPController::setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&)
+ {
+ LockGuardType guard {controlDataMutex};
+ for (size_t i = 0; i < tcpController->rns->getSize(); i++)
+ {
+ getWriterControlStruct().torqueKp.at(i) = torqueKp.at(tcpController->rns->getNode(i)->getName());
+ }
+ writeControlStruct();
+ }
+
+ void NJointTSMultiMPController::setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&)
+ {
+ LockGuardType guard {controlDataMutex};
+ for (size_t i = 0; i < tcpController->rns->getSize(); i++)
+ {
+ getWriterControlStruct().nullspaceJointVelocities.at(i) = nullspaceJointVelocities.at(tcpController->rns->getNode(i)->getName());
+ }
+ writeControlStruct();
+ }
+
+ void NJointTSMultiMPController::resetDMP(const Ice::Current&)
+ {
+ if (started)
+ {
+ ARMARX_INFO << "Cannot reset running DMP";
+ }
+ firstRun = true;
+ }
+
+ void NJointTSMultiMPController::stopDMP(const Ice::Current&)
+ {
+ mpMagazine.at(mpName).firstRun = true;
+ mpMagazine.at(mpName).status = mpStatus::stopped;
+ // started = false;
+ // firstRun = true;
+ }
+
+ void NJointTSMultiMPController::runDMP(const std::string& mpName, const Ice::DoubleSeq& goals, double tau, const Ice::Current&)
+ {
+ mpMagazine.at(mpName).firstRun = true;
+ while (mpMagazine.at(mpName).firstRun)
+ {
+ usleep(100);
+ }
+ while (!interfaceData.updateReadBuffer())
+ {
+ usleep(100);
+ }
+ // firstRun = true;
+ // while (firstRun)
+ // {
+ // usleep(100);
+ // }
+ // while (!interfaceData.updateReadBuffer())
+ // {
+ // usleep(100);
+ // }
+
+ Eigen::Matrix4f pose = interfaceData.getReadBuffer().currentTcpPose;
+ ARMARX_INFO << "current pose: " << pose;
+ LockGuardType guard {controllerMutex};
+ mpMagazine.at(mpName).mpController->prepareExecution(mpMagazine.at(mpName).mpController->eigen4f2vec(pose), goals, tau);
+ mpMagazine.at(mpName).status = mpStatus::running;
+ // taskSpaceDMPController->prepareExecution(taskSpaceDMPController->eigen4f2vec(pose), goals, tau);
+ // finished = false;
+
+ // ARMARX_INFO << "run DMP";
+ // started = true;
+ }
+
+
+
+
+ void NJointTSMultiMPController::rtPreActivateController()
+ {
+ }
+
+ void NJointTSMultiMPController::rtPostDeactivateController()
+ {
+
+ }
+
+ void NJointTSMultiMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+ {
+ std::string datafieldName = debugName;
+ StringVariantBaseMap datafields;
+ auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
+ for (auto& pair : values)
+ {
+ datafieldName = pair.first + "_" + debugName;
+ datafields[datafieldName] = new Variant(pair.second);
+ }
+
+ auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
+ for (auto& pair : dmpTargets)
+ {
+ 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 = "DMPController_" + debugName;
+
+ debugObs->setDebugChannel(datafieldName, datafields);
+ }
+
+ void NJointTSMultiMPController::onInitComponent()
+ {
+ ARMARX_INFO << "init ...";
+ targetVels.resize(6);
+ NJointTSMultiMPControllerControlData initData;
+ initData.targetTSVel.resize(6);
+ for (size_t i = 0; i < 6; ++i)
+ {
+ initData.targetTSVel(i) = 0;
+ targetVels(i) = 0;
+ initData.targetPose = tcp->getPoseInRootFrame();
+ }
+ initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
+ initData.torqueKp.resize(tcpController->rns->getSize(), 0);
+ initData.torqueKd.resize(tcpController->rns->getSize(), 0);
+ initData.mode = ModeFromIce(cfg->mode);
+ reinitTripleBuffer(initData);
+
+
+
+ controllerTask = new PeriodicTask<NJointTSMultiMPController>(this, &NJointTSMultiMPController::controllerRun, 0.3);
+ controllerTask->start();
+
+
+ }
+
+ void NJointTSMultiMPController::onDisconnectComponent()
+ {
+ ARMARX_INFO << "stopped ...";
+ controllerTask->stop();
+ }
+
+
+
+}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSMultiMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSMultiMPController.h
new file mode 100644
index 000000000..55d7cfb33
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSMultiMPController.h
@@ -0,0 +1,195 @@
+/* Jeff
+ *
+ */
+#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 <dmp/representation/dmp/umitsmp.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
+
+using namespace DMP;
+namespace armarx
+{
+ TYPEDEF_PTRS_HANDLE(NJointTSMultiMPController);
+ TYPEDEF_PTRS_HANDLE(NJointTSMultiMPControllerControlData);
+
+ using ViaPoint = std::pair<double, DVec >;
+ using ViaPointsSet = std::vector<ViaPoint >;
+ class NJointTSMultiMPControllerControlData
+ {
+ public:
+ Eigen::VectorXf targetTSVel;
+ Eigen::Matrix4f targetPose;
+ // cartesian velocity control data
+ std::vector<float> nullspaceJointVelocities;
+ float avoidJointLimitsKp = 0;
+ std::vector<float> torqueKp;
+ std::vector<float> torqueKd;
+ VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
+ };
+
+ 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 NJointTSMultiMPController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
+ class NJointTSMultiMPController :
+ public NJointControllerWithTripleBuffer<NJointTSMultiMPControllerControlData>,
+ public NJointTaskSpaceDMPControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointTaskSpaceDMPControllerConfigPtr;
+ NJointTSMultiMPController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ std::string getClassName(const Ice::Current&) const override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+
+ // NJointTSMultiMPControllerInterface interface
+ void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
+ bool isFinished(const Ice::Current&) override
+ {
+ return finished;
+ }
+
+ void runDMP(const std::string& mpName, const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&) override;
+ void setSpeed(Ice::Double times, const Ice::Current&) override;
+ void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
+ void removeAllViaPoints(const Ice::Current&) override;
+
+ void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
+
+ void setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&) override;
+ void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&) override;
+ void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&) override;
+
+
+ void resetDMP(const Ice::Current&) override;
+ void stopDMP(const std::string& mpName, const Ice::Current&) override;
+ double getCanVal(const Ice::Current&) override
+ {
+ return taskSpaceDMPController->canVal / cfg->timeDuration;
+ }
+
+ protected:
+ void rtPreActivateController() override;
+ void rtPostDeactivateController() override;
+ VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
+
+ void onInitComponent() override;
+ void onDisconnectComponent() override;
+ void controllerRun();
+
+ private:
+ struct DebugBufferData
+ {
+ StringFloatDictionary latestTargetVelocities;
+ StringFloatDictionary dmpTargets;
+ StringFloatDictionary currentPose;
+ double currentCanVal;
+ double mpcFactor;
+ double error;
+ double phaseStop;
+ double posError;
+ double oriError;
+ double deltaT;
+ };
+
+ TripleBuffer<DebugBufferData> debugOutputData;
+
+ struct NJointTSMultiMPControllerSensorData
+ {
+ double currentTime;
+ double deltaT;
+ Eigen::Matrix4f currentPose;
+ Eigen::VectorXf currentTwist;
+ };
+ TripleBuffer<NJointTSMultiMPControllerSensorData> controllerSensorData;
+
+ struct NJointTSMultiMPControllerInterfaceData
+ {
+ Eigen::Matrix4f currentTcpPose;
+ };
+
+ TripleBuffer<NJointTSMultiMPControllerInterfaceData> interfaceData;
+
+ std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
+ std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
+ std::vector<ControlTarget1DoFActuatorVelocity*> targets;
+ std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
+
+
+
+ // velocity ik controller parameters
+ std::vector<pidController> torquePIDs;
+ CartesianVelocityControllerPtr tcpController;
+ std::string nodeSetName;
+
+ /* set up movement magazine and parameters
+ *
+ */
+ // DMP magazine
+ enum mpStatus {running, paused, finished};
+ struct mp
+ {
+ TaskSpaceDMPControllerPtr mpController;
+ mpStatus status;
+ bool firstRun;
+ };
+ std::map<std::string, mp> mpMagazine;
+ // TaskSpaceDMPControllerPtr taskSpaceDMPController;
+ // dmp parameters
+ // bool finished;
+ // bool started;
+
+ VirtualRobot::DifferentialIKPtr ik;
+ VirtualRobot::RobotNodePtr tcp;
+ Eigen::VectorXf targetVels;
+ Eigen::Matrix4f targetPose;
+
+ NJointTaskSpaceDMPControllerConfigPtr cfg;
+ mutable MutexType controllerMutex;
+ PeriodicTask<NJointTSMultiMPController>::pointer_type controllerTask;
+
+
+ std::string debugName;
+
+ Eigen::VectorXf filtered_qvel;
+ Eigen::Vector3f filtered_position;
+ float vel_filter_factor;
+ float pos_filter_factor;
+ bool firstRun;
+
+ float KpF;
+ float DpF;
+ float KoF;
+ float DoF;
+
+ Eigen::VectorXf jointHighLimits;
+ Eigen::VectorXf jointLowLimits;
+ };
+
+} // namespace armarx
+
--
GitLab