diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 93b018801eb5e7f6b870e06b914798d511dc5780..d39e94c87ce0ea5d5ed5b21e252d31f1e2ca41f9 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -325,6 +325,7 @@ module armarx
{
void learnDMPFromFiles(Ice::StringSeq trajfiles);
void learnJointDMPFromFiles(string jointTrajFile, Ice::FloatSeq currentJVS);
+ void setUseNullSpaceJointDMP(bool enable);
bool isFinished();
void runDMP(Ice::DoubleSeq goals);
@@ -332,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 65da4d862d7696bccaca2b68e5576777e722d54d..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,25 +289,28 @@ 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 nullspaceTorque = knull.cwiseProduct(desiredNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
+ 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;
+ Eigen::VectorXf jointDesiredTorques =
+ jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
@@ -316,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);
@@ -325,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;
}
}
@@ -371,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 ";
@@ -385,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);
@@ -439,8 +455,14 @@ namespace armarx
stopped = false;
}
+ void NJointTaskSpaceImpedanceDMPController::setUseNullSpaceJointDMP(bool enable, const Ice::Current&)
+ {
+ useNullSpaceJointDMP = enable;
+ }
+
- 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)
@@ -490,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;
@@ -585,7 +607,7 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::removeAllViaPoints(const Ice::Current&)
{
- LockGuardType guard {controllerMutex};
+ LockGuardType guard{controllerMutex};
ARMARX_INFO << "setting via points ";
dmpCtrl->removeAllViaPoints();
}
@@ -626,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 1b49c99634870147df440d88709f828c0e074872..ea8cef187de6296e07eeb270e4ca5ac2ad7b78f6 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -42,51 +42,53 @@ 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;
void setLinearVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&) override;
- void setLinearVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)override;
- void setAngularVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)override;
- void setAngularVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)override;
+ void setLinearVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&) override;
+ 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:
@@ -190,11 +192,11 @@ namespace armarx
Eigen::VectorXf dnull;
int numOfJoints;
- bool useNullSpaceJointDMP;
+ std::atomic_bool useNullSpaceJointDMP;
bool isNullSpaceJointDMPLearned;
- Eigen::VectorXf defaultNullSpaceJointValues;
+ armarx::TripleBuffer<Eigen::VectorXf> defaultNullSpaceJointValues;
std::vector<std::string> jointNames;
mutable MutexType controllerMutex;
PeriodicTask<NJointTaskSpaceImpedanceDMPController>::pointer_type controllerTask;
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index c8db83835ccc53f21e416b723745892a1455f801..5a3a4a0d4abd221dd9ce8385fbce98a747f36c09 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -284,14 +284,17 @@ namespace armarx::armem::common::robot_state::localization
std::vector<::armarx::armem::robot_state::Transform> transforms;
transforms.reserve(entity.history().size());
- const auto entitySnapshots = simox::alg::get_values(entity.history());
+ // const auto entitySnapshots = simox::alg::get_values(entity.history());
+
+ const std::vector<wm::EntitySnapshot> entitySnapshots = {entity.getLatestSnapshot()};
+
std::transform(
entitySnapshots.begin(),
entitySnapshots.end(),
std::back_inserter(transforms),
- [](const auto & entity)
+ [](const auto & entitySnapshot)
{
- return convertEntityToTransform(entity.getInstance(0));
+ return convertEntityToTransform(entitySnapshot.getInstance(0));
});
ARMARX_DEBUG << "obtaining transform";
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index f140f9512bcb990a82e50ed3eb271536817d4d88..9527835a6dd48107f5e11846ec882d30b056c54a 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -57,6 +57,11 @@ void CartesianVelocityController::calculateJacobis(VirtualRobot::IKSolver::Carte
_jacobiWithCosts(r, c) = jacobi(r, c) / _jointCosts(c);
}
}
+ auto svd = Eigen::JacobiSVD(_jacobiWithCosts, 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);
_inv = ik->computePseudoInverseJacobianMatrix(_jacobiWithCosts, ik->getJacobiRegularization(mode));
}
@@ -267,6 +272,11 @@ bool CartesianVelocityController::clampJacobiAtJointLimits(VirtualRobot::IKSolve
}
if (modifiedJacobi)
{
+ auto svd = Eigen::JacobiSVD(jacobi, 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);
inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
}