Skip to content
Snippets Groups Projects
Commit 6443c28f authored by Fabian Reister's avatar Fabian Reister
Browse files

Merge branch 'master' into fix/obstacle_awareness

parents e4a2f516 33e59cb0
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 108 additions and 60 deletions
source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+ 2
− 0
View file @ 6443c28f
......@@ -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();
......
source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+ 65
− 34
View file @ 6443c28f
......@@ -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();
}
}
source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+ 25
− 23
View file @ 6443c28f
......@@ -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;
......
source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+ 6
− 3
View file @ 6443c28f
......@@ -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";
......
source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+ 10
− 0
View file @ 6443c28f
......@@ -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));
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment