diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index dd17bc15be467cf494facd91bd1ac7fd2b58ef43..d39e94c87ce0ea5d5ed5b21e252d31f1e2ca41f9 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -333,6 +333,7 @@ module armarx
void setViaPoints(double canVal, Ice::DoubleSeq point);
void setGoals(Ice::DoubleSeq goals);
+ void setDefaultNullSpaceJointValues(Eigen::VectorXf jointValues);
double getVirtualTime();
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 74c1e94c43a4af671f56e4a636aa829f69eb7daa..7e9acde0bdc18f3bd937f5972ce0081727aabc98 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -4,9 +4,12 @@
namespace armarx
{
- NJointControllerRegistration<NJointTaskSpaceImpedanceDMPController> registrationControllerNJointTaskSpaceImpedanceDMPController("NJointTaskSpaceImpedanceDMPController");
+ NJointControllerRegistration<NJointTaskSpaceImpedanceDMPController> registrationControllerNJointTaskSpaceImpedanceDMPController(
+ "NJointTaskSpaceImpedanceDMPController");
- NJointTaskSpaceImpedanceDMPController::NJointTaskSpaceImpedanceDMPController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ NJointTaskSpaceImpedanceDMPController::NJointTaskSpaceImpedanceDMPController(const RobotUnitPtr& robotUnit,
+ const armarx::NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&)
{
ARMARX_INFO << "creating impedance dmp controller";
cfg = NJointTaskSpaceImpedanceDMPControllerConfigPtr::dynamicCast(config);
@@ -37,8 +40,9 @@ namespace armarx
positionSensors.push_back(positionSensor);
};
- tcp = rns->getTCP();
- ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ tcp = rns->getTCP();
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(),
+ VirtualRobot::JacobiProvider::eSVDDamped));
numOfJoints = targets.size();
// set DMP
TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
@@ -65,14 +69,15 @@ namespace armarx
isNullSpaceJointDMPLearned = false;
- defaultNullSpaceJointValues.resize(targets.size());
+ Eigen::VectorXf nullspaceValues(targets.size());
+
ARMARX_CHECK_EQUAL(cfg->defaultNullSpaceJointValues.size(), targets.size());
for (size_t i = 0; i < targets.size(); ++i)
{
- defaultNullSpaceJointValues(i) = cfg->defaultNullSpaceJointValues.at(i);
+ nullspaceValues(i) = cfg->defaultNullSpaceJointValues.at(i);
}
-
+ defaultNullSpaceJointValues.reinitAllBuffers(nullspaceValues);
Eigen::Vector3f kpos(cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2]);
Eigen::Vector3f dpos(cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2]);
@@ -126,7 +131,7 @@ namespace armarx
initData.targetPose = tcp->getPoseInRootFrame();
initData.targetVel.resize(6);
initData.targetVel.setZero();
- initData.desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ initData.desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
reinitTripleBuffer(initData);
@@ -154,8 +159,8 @@ namespace armarx
if (!started)
{
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ LockGuardType guard{controlDataMutex};
+ getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
getWriterControlStruct().targetVel.setZero(6);
getWriterControlStruct().targetPose = currentPose;
getWriterControlStruct().canVal = 1.0;
@@ -167,8 +172,8 @@ namespace armarx
if (stopped)
{
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ LockGuardType guard{controlDataMutex};
+ getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
getWriterControlStruct().targetVel.setZero(6);
getWriterControlStruct().targetPose = oldPose;
getWriterControlStruct().canVal = dmpCtrl->canVal;
@@ -180,7 +185,7 @@ namespace armarx
if (dmpCtrl->canVal < 1e-8)
{
finished = true;
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().targetVel.setZero();
writeControlStruct();
return;
@@ -192,7 +197,10 @@ namespace armarx
if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
{
DMP::DVec targetJointState;
- currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(currentJointState, dmpCtrl->canVal / timeDuration, deltaT / timeDuration, targetJointState);
+ currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(currentJointState,
+ dmpCtrl->canVal / timeDuration,
+ deltaT / timeDuration,
+ targetJointState);
if (targetJointState.size() == jointNames.size())
{
@@ -203,15 +211,15 @@ namespace armarx
}
else
{
- desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
}
}
else
{
- desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
}
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
@@ -223,7 +231,8 @@ namespace armarx
}
}
- void NJointTaskSpaceImpedanceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void NJointTaskSpaceImpedanceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
double deltaT = timeSinceLastIteration.toSecondsDouble();
@@ -261,7 +270,7 @@ namespace armarx
firstRun = false;
targetPose = currentPose;
targetVel.setZero(6);
- desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
}
else
{
@@ -280,54 +289,32 @@ namespace armarx
Eigen::Vector3f targetTCPLinearVelocity;
targetTCPLinearVelocity << 0.001 * targetVel(0), 0.001 * targetVel(1), 0.001 * targetVel(2);
Eigen::Vector3f currentTCPLinearVelocity;
- currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1), 0.001 * currentTwist(2);
+ currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1), 0.001 * currentTwist(2);
Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
- Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
+ Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) +
+ dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
Eigen::Vector3f currentTCPAngularVelocity;
- currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
+ currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
- jointControlWrench << tcpDesiredForce, tcpDesiredTorque;
+ jointControlWrench << tcpDesiredForce, tcpDesiredTorque;
}
Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
- Eigen::VectorXf diffJoints = desiredNullSpaceJointValues;
+ Eigen::VectorXf nullspaceTorque =
+ knull.cwiseProduct(desiredNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
+ Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
+ Eigen::VectorXf jointDesiredTorques =
+ jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
+
- for (size_t i = 0; i < rns->getSize(); i++)
- {
- VirtualRobot::RobotNodePtr rn = rns->getNode(i);
- if (rn->isLimitless() && std::abs(diffJoints(i) - qpos(i)) > M_PI)
- {
- if (diffJoints(i) > qpos(i))
- {
- diffJoints(i) = -(2 * M_PI - std::abs(diffJoints(i) - qpos(i)));
- }
- else
- {
- diffJoints(i) = 2 * M_PI - std::abs(diffJoints(i) - qpos(i));
- }
- }
- else
- {
- diffJoints(i) -= qpos(i);
- }
- }
- Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(diffJoints) - dnull.cwiseProduct(qvel);
- Eigen::MatrixXf jtp = jacobi.transpose();
- auto svd = Eigen::JacobiSVD(jtp, Eigen::ComputeThinU | Eigen::ComputeThinV);
- auto sv = svd.singularValues();
- double minSigVal = sv(sv.rows() - 1, sv.cols() - 1);
- double damping = minSigVal < 1e-2 ? 1e-2 : 1e-8;
- ik->setDampedSvdLambda(damping);
- Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jtp, 2.0);
- Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
// torque limit
ARMARX_CHECK_EXPRESSION(!targets.empty());
@@ -341,7 +328,7 @@ namespace armarx
desiredTorque = 0;
}
- desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
debugOutputData.getWriteBuffer().desired_torques[jointNames[i]] = jointDesiredTorques(i);
@@ -350,7 +337,8 @@ namespace armarx
targets.at(i)->torque = desiredTorque;
if (!targets.at(i)->isValid())
{
- ARMARX_INFO << deactivateSpam(1) << "Torque controller target is invalid - setting to zero! set value: " << targets.at(i)->torque;
+ ARMARX_INFO << deactivateSpam(1) << "Torque controller target is invalid - setting to zero! set value: "
+ << targets.at(i)->torque;
targets.at(i)->torque = 0.0f;
}
}
@@ -396,7 +384,8 @@ namespace armarx
ARMARX_INFO << "Learned DMP ... ";
}
- void NJointTaskSpaceImpedanceDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint,
+ const Ice::Current&)
{
LockGuardType guard(controllerMutex);
ARMARX_INFO << "setting via points ";
@@ -410,9 +399,11 @@ namespace armarx
}
- void NJointTaskSpaceImpedanceDMPController::learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::learnJointDMPFromFiles(const std::string& fileName,
+ const Ice::FloatSeq& currentJVS,
+ const Ice::Current&)
{
- DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
+ DMP::Vec<DMP::SampledTrajectoryV2> trajs;
DMP::DVec ratios;
DMP::SampledTrajectoryV2 traj;
traj.readFromCSVFile(fileName);
@@ -432,8 +423,8 @@ namespace armarx
goal.at(i) = traj.rbegin()->getPosition(i);
currentJointState.at(i).pos = currentJVS.at(i);
currentJointState.at(i).vel = 0;
- ARMARX_INFO << goal.at(i);
}
+
trajs.push_back(traj);
nullSpaceJointDMPPtr->learnFromTrajectories(trajs);
@@ -470,7 +461,8 @@ namespace armarx
}
- void NJointTaskSpaceImpedanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration,
+ const Ice::Current&)
{
firstRun = true;
while (firstRun)
@@ -520,8 +512,8 @@ namespace armarx
}
-
- void NJointTaskSpaceImpedanceDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+ void NJointTaskSpaceImpedanceDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx& debugObs)
{
StringVariantBaseMap datafields;
auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
@@ -615,7 +607,7 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::removeAllViaPoints(const Ice::Current&)
{
- LockGuardType guard {controllerMutex};
+ LockGuardType guard{controllerMutex};
ARMARX_INFO << "setting via points ";
dmpCtrl->removeAllViaPoints();
}
@@ -656,5 +648,14 @@ namespace armarx
}
+ void NJointTaskSpaceImpedanceDMPController::setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues,
+ const Ice::Current&)
+ {
+ ARMARX_CHECK_EQUAL(jointValues.size(), targets.size());
+ defaultNullSpaceJointValues.getWriteBuffer() = jointValues;
+ defaultNullSpaceJointValues.commitWrite();
+
+ }
+
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index 594be8eede44d4fd62cf466c06bf76523afc450f..ea8cef187de6296e07eeb270e4ca5ac2ad7b78f6 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -42,37 +42,37 @@ namespace armarx
NJointTaskSpaceImpedanceDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const;
+ std::string getClassName(const Ice::Current&) const override;
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
// NJointTaskSpaceImpedanceDMPControllerInterface interface
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
- bool isFinished(const Ice::Current&)
+ void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
+ bool isFinished(const Ice::Current&) override
{
return finished;
}
- void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
+ void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
+ void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
- void learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&);
- void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&);
- void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
+ void learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&) override;
+ void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&) override;
+ void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&) override;
- Ice::Double getVirtualTime(const Ice::Current&)
+ Ice::Double getVirtualTime(const Ice::Current&) override
{
return dmpCtrl->canVal;
}
- void stopDMP(const Ice::Current&);
- void resumeDMP(const Ice::Current&);
- void resetDMP(const Ice::Current&);
+ void stopDMP(const Ice::Current&) override;
+ void resumeDMP(const Ice::Current&) override;
+ void resetDMP(const Ice::Current&) override;
- void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&);
- DoubleSeqSeq getMPWeights(const Ice::Current&);
+ void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&) override;
+ DoubleSeqSeq getMPWeights(const Ice::Current&) override;
void removeAllViaPoints(const Ice::Current&) override;
@@ -81,13 +81,14 @@ namespace armarx
void setAngularVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&) override;
void setAngularVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&) override;
void setUseNullSpaceJointDMP(bool enable, const Ice::Current&) override;
+ void setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues, const Ice::Current&) override;
protected:
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
- void onInitNJointController();
- void onDisconnectNJointController();
+ void onInitNJointController() override;
+ void onDisconnectNJointController() override;
void controllerRun();
private:
@@ -195,7 +196,7 @@ namespace armarx
bool isNullSpaceJointDMPLearned;
- Eigen::VectorXf defaultNullSpaceJointValues;
+ armarx::TripleBuffer<Eigen::VectorXf> defaultNullSpaceJointValues;
std::vector<std::string> jointNames;
mutable MutexType controllerMutex;
PeriodicTask<NJointTaskSpaceImpedanceDMPController>::pointer_type controllerTask;