From 8fd1e8ac16c07fdf1f9eb936d2a4ff82220ef516 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Fri, 6 Dec 2013 00:15:21 +0100
Subject: [PATCH] Extended DifferentialIK with optional weights for joint
angles and tcp DOFs (the letter not tested yet) Extended DifferentialIK with
considering joint limits
---
source/RobotAPI/units/TCPControlUnit.cpp | 244 +++++++++++++++++++++--
source/RobotAPI/units/TCPControlUnit.h | 12 +-
2 files changed, 240 insertions(+), 16 deletions(-)
diff --git a/source/RobotAPI/units/TCPControlUnit.cpp b/source/RobotAPI/units/TCPControlUnit.cpp
index 5f0123486..55a7689e5 100644
--- a/source/RobotAPI/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/units/TCPControlUnit.cpp
@@ -33,6 +33,7 @@
#include <VirtualRobot/XML/RobotIO.h>
using namespace VirtualRobot;
+using namespace Eigen;
@@ -95,8 +96,8 @@ namespace armarx
// request();
listener = getTopic<TCPControlUnitListenerPrx>("TCPControlUnitState");
- topicTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicReport,cycleTime, false, "TCPDataProvider");
- topicTask->start();
+// topicTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicReport,cycleTime, false, "TCPDataProvider");
+// topicTask->start();
}
void TCPControlUnit::onDisconnectComponent()
@@ -123,6 +124,8 @@ namespace armarx
void TCPControlUnit::setTCPVelocity(const std::string &nodeSetName, const std::string &tcpName, const FramedVector3BasePtr &translationVelocity, const FramedVector3BasePtr &orientationVelocityRPY, const Ice::Current &c)
{
ScopedLock lock(dataMutex);
+ ARMARX_CHECK_EXPRESSION_W_HINT(localRobot->hasRobotNodeSet(nodeSetName), "The robot does not have the node set: " + nodeSetName);
+
if(translationVelocity)
ARMARX_VERBOSE << "Setting new Velocity for " << nodeSetName << "\n" << FramedVector3Ptr::dynamicCast(translationVelocity)->toEigen();
if(orientationVelocityRPY)
@@ -134,7 +137,10 @@ namespace armarx
if(tcpName.empty())
data.tcpName = localRobot->getRobotNodeSet(nodeSetName)->getTCP()->getName();
else
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(localRobot->hasRobotNode(tcpName), "The robot does not have the node: " + tcpName);
data.tcpName = tcpName;
+ }
tcpVelocitiesMap[data.tcpName] = data;
// if(translationVelocity)
// targetTranslationVelocities[nodeSetName] = translationVelocity;
@@ -212,6 +218,8 @@ namespace armarx
localRobot->setJointValues(jointValues);
+ periodicReport();
+
// dIKMap.clear();
TCPVelocityDataMap::iterator it = tcpVelocitiesMap.begin();
for(; it != tcpVelocitiesMap.end(); it++)
@@ -222,7 +230,13 @@ namespace armarx
DIKMap::iterator itDIK = dIKMap.find(data.nodeSetName);
if(itDIK == dIKMap.end())
dIKMap[data.nodeSetName].reset(new EDifferentialIK(nodeSet, localRobot->getRootNode()/*, VirtualRobot::JacobiProvider::eTranspose*/));
- boost::shared_dynamic_cast<EDifferentialIK>(dIKMap[data.nodeSetName])->clearGoals();
+ EDifferentialIKPtr dIk = boost::shared_dynamic_cast<EDifferentialIK>(dIKMap[data.nodeSetName]);
+ dIk->clearGoals();
+ Eigen::VectorXf dofWeights(nodeSet->getSize());
+ dofWeights.setOnes();
+ dofWeights(0) = 0.2;
+ dofWeights(1) = 0.0;
+ dIk->setDOFWeights(dofWeights);
}
using namespace Eigen;
@@ -234,7 +248,6 @@ namespace armarx
TCPVelocityData& data = it->second;
RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(data.nodeSetName);
std::string refFrame = localRobot->getRootNode()->getName();
-
IKSolver::CartesianSelection mode;
if(data.translationVelocity && data.orientationVelocity)
{
@@ -288,7 +301,7 @@ namespace armarx
}
// ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << tcpNode->getGlobalPose();
// ARMARX_VERBOSE << deactivateSpam(1) << "New Goal: \n" << m;
- dIK->setGoal(m, tcpNode,mode);
+ dIK->setGoal(m, tcpNode,mode, 0.001, 0.001/180.0f*3.14159);
// ARMARX_VERBOSE << deactivateSpam(1) << "Delta to Goal: " << dIK->getDeltaToGoal(tcpNode);
}
@@ -301,12 +314,15 @@ namespace armarx
{
RobotNodeSetPtr robotNodeSet = localRobot->getRobotNodeSet(itDIK->first);
EDifferentialIKPtr dIK = boost::shared_dynamic_cast<EDifferentialIK>(itDIK->second);
- Eigen::VectorXf jointDelta = dIK->computeStep(1.0);
+ ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << robotNodeSet->getTCP()->getGlobalPose();
+ dIK->setVerbose(true);
+ Eigen::VectorXf jointDelta = dIK->computeSteps(0.5, 0.00001, 20);
+ ARMARX_VERBOSE << deactivateSpam(1) << "New Goal: \n" << robotNodeSet->getTCP()->getGlobalPose();
- MatrixXf fullJac = dIK->calcFullJacobian();
- MatrixXf fullJacInv = dIK->computePseudoInverseJacobianMatrix(fullJac);
- Eigen::VectorXf jointLimitCompensation = CalcJointLimitAvoidanceDeltas(robotNodeSet,fullJac, fullJacInv);
- jointDelta += jointLimitCompensation;
+// MatrixXf fullJac = dIK->calcFullJacobian();
+// MatrixXf fullJacInv = dIK->computePseudoInverseJacobianMatrix(fullJac);
+// Eigen::VectorXf jointLimitCompensation = CalcJointLimitAvoidanceDeltas(robotNodeSet,fullJac, fullJacInv);
+// jointDelta += jointLimitCompensation;
jointDelta = applyMaxJointVelocity(jointDelta, maxJointVelocity);
@@ -344,13 +360,13 @@ namespace armarx
void TCPControlUnit::periodicReport()
{
- ScopedLock lock(dataMutex);
+// ScopedLock lock(dataMutex);
- NameValueMap robotConfigMap = remoteRobotPrx->getConfig();
-// NameValueMap::iterator it = robotConfigMap.begin();
- std::map<std::string, float> jointValues(robotConfigMap.begin(), robotConfigMap.end());
+// NameValueMap robotConfigMap = remoteRobotPrx->getConfig();
+//// NameValueMap::iterator it = robotConfigMap.begin();
+// std::map<std::string, float> jointValues(robotConfigMap.begin(), robotConfigMap.end());
- localRobot->setJointValues(jointValues);
+// localRobot->setJointValues(jointValues);
std::vector<RobotNodeSetPtr > nodeSets = localRobot->getRobotNodeSets();
FramedPoseBaseMap tcpPoses;
std::string rootFrame = localRobot->getRootNode()->getName();
@@ -473,6 +489,7 @@ namespace armarx
tolerancePosition.clear();
toleranceRotation.clear();
parents.clear();
+ tcpWeights = Eigen::VectorXf();
}
void EDifferentialIK::setRefFrame(RobotNodePtr coordSystem)
@@ -480,6 +497,203 @@ namespace armarx
this->coordSystem = coordSystem;
}
+ void EDifferentialIK::setDOFWeights(Eigen::VectorXf dofWeights)
+ {
+ this->dofWeights = dofWeights;
+ }
+
+ void EDifferentialIK::setResultDimensionWeights(Eigen::VectorXf tcpWeights)
+ {
+ this->tcpWeights = tcpWeights;
+ }
+
+ Eigen::VectorXf EDifferentialIK::computeSteps(float stepSize, float minumChange, int maxNStep)
+ {
+ VR_ASSERT(rns);
+ VR_ASSERT(nodes.size() == rns->getSize());
+ //std::vector<RobotNodePtr> rn = this->nodes;
+ RobotPtr robot = rns->getRobot();
+ VR_ASSERT(robot);
+ std::vector<float> jv(nodes.size(),0.0f);
+ std::vector<float> jvBest = rns->getJointValues();
+ int step = 0;
+ checkTolerances();
+ float lastDist = FLT_MAX;
+
+ VectorXf dThetaSum(nodes.size());
+ dThetaSum.setZero();
+ while (step<maxNStep)
+ {
+ VectorXf dTheta = this->computeStep(stepSize);
+ if(adjustDOFWeightsToJointLimits(dTheta))
+ dTheta = computeStep(stepSize);
+ dThetaSum += dTheta;
+ for (unsigned int i=0; i<nodes.size();i++)
+ {
+ jv[i] = (nodes[i]->getJointValue() + dTheta[i]);
+ if (boost::math::isnan(jv[i]) || boost::math::isinf(jv[i]))
+ {
+ VR_WARNING << "Aborting, invalid joint value (nan)" << endl;
+ return dThetaSum;
+ }
+ //nodes[i]->setJointValue(nodes[i]->getJointValue() + dTheta[i]);
+ }
+
+ robot->setJointValues(rns,jv);
+
+ // check tolerances
+ if (checkTolerances())
+ {
+ if (verbose)
+ VR_INFO << "Tolerances ok, loop:" << step << endl;
+ return dThetaSum;
+ }
+ float d = dTheta.norm();
+ if (dTheta.norm()<minumChange)
+ {
+ if (verbose)
+ VR_INFO << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << endl;
+ return dThetaSum;
+ }
+ float posDist = getMeanErrorPosition();
+ if (checkImprovement && posDist>lastDist)
+ {
+ if (verbose)
+ VR_INFO << "Could not improve result any more (current position error=" << posDist << ", last loop's error:" << lastDist << "), loop:" << step << endl;
+ robot->setJointValues(rns,jvBest);
+ return dThetaSum;
+ }
+ jvBest = jv;
+ lastDist = posDist;
+ step++;
+ }
+ if (verbose)
+ {
+ VR_INFO << "IK failed, loop:" << step << endl;
+ VR_INFO << "pos error:" << getErrorPosition() << endl;
+ VR_INFO << "rot error:" << getErrorRotation() << endl;
+ }
+ return dThetaSum;
+ }
+
+ void EDifferentialIK::applyWeightsToJacobian(MatrixXf &Jacobian)
+ {
+ if(dofWeights.rows() == Jacobian.cols())
+ {
+ Eigen::MatrixXf weightMat(dofWeights.rows(), dofWeights.rows());
+ weightMat.setIdentity();
+ weightMat.diagonal() = dofWeights;
+// ARMARX_INFO << deactivateSpam(1) << "Jac before:\n" << Jacobian;
+// ARMARX_INFO << deactivateSpam(1) << "Weights:\n" << weightMat;
+ Jacobian = Jacobian * weightMat;
+// ARMARX_INFO << deactivateSpam(1) << "Jac after:\n" << Jacobian;
+
+ }
+
+ if(tcpWeights.rows() == Jacobian.rows())
+ {
+ Eigen::MatrixXf weightMat(tcpWeights.rows(), tcpWeights.rows());
+ weightMat.setIdentity();
+ weightMat.diagonal() = dofWeights;
+
+ Jacobian = weightMat * Jacobian;
+
+ }
+ }
+
+ VectorXf EDifferentialIK::computeStep(float stepSize )
+ {
+
+ if (nRows==0) this->setNRows();
+ size_t nDoF = nodes.size();
+
+ MatrixXf Jacobian(nRows,nDoF);
+ VectorXf error(nRows);
+
+ size_t index=0;
+ for (size_t i=0; i<tcp_set.size();i++)
+ {
+ RobotNodePtr tcp = tcp_set[i];
+ if (this->targets.find(tcp)!=this->targets.end())
+ {
+ Eigen::VectorXf delta = getDeltaToGoal(tcp);
+ IKSolver::CartesianSelection mode = this->modes[tcp];
+ MatrixXf partJacobian = this->getJacobianMatrix(tcp,mode);
+
+ Jacobian.block(index,0,partJacobian.rows(),nDoF) = partJacobian;
+ Vector3f position = delta.head(3);
+ position *=stepSize;
+ if (mode & IKSolver::X)
+ {
+ error(index) = position(0);
+ index++;
+ }
+ if (mode & IKSolver::Y)
+ {
+ error(index) = position(1);
+ index++;
+ }
+ if (mode & IKSolver::Z)
+ {
+ error(index) = position(2);
+ index++;
+ }
+ if (mode & IKSolver::Orientation)
+ {
+ error.segment(index,3) = delta.tail(3)*stepSize;
+ index+=3;
+ }
+
+ }
+ else
+ VR_ERROR << "Internal error?!" << endl; // Error
+
+
+ }
+
+ applyWeightsToJacobian(Jacobian);
+
+ //cout << "ERROR:" << endl;
+ //cout << error << endl;
+ VectorXf dTheta(nDoF);
+
+ //MatrixXf pseudo = (Jacobian.transpose() * Jacobian).inverse() * Jacobian.transpose();
+ //MatrixXf pseudo = Jacobian.transpose() * (Jacobian*Jacobian.transpose()).inverse();
+ MatrixXf pseudo = computePseudoInverseJacobianMatrix(Jacobian);
+ //cout << "PSEUDO:" << endl;
+ //cout << pseudo << endl;
+
+ dTheta = pseudo * error;
+ //cout << "THETA:" << endl;
+ //cout << dTheta << endl;
+ return dTheta;
+
+ }
+
+ bool EDifferentialIK::adjustDOFWeightsToJointLimits(const VectorXf &plannedJointDeltas)
+ {
+ if(dofWeights.rows() != plannedJointDeltas.rows())
+ {
+ dofWeights.resize(plannedJointDeltas.rows());
+ dofWeights.setOnes();
+
+ }
+
+ bool result = false;
+ for (unsigned int i=0; i<nodes.size();i++)
+ {
+ float angle = nodes[i]->getJointValue() + plannedJointDeltas[i]*0.1;
+ if(angle > nodes[i]->getJointLimitHi() || angle < nodes[i]->getJointLimitLo())
+ {
+ ARMARX_VERBOSE << deactivateSpam(3) << nodes[i]->getName() << " joint deactivated because of joint limit";
+ dofWeights(i) = 0;
+ result = true;
+ }
+ }
+ return result;
+
+ }
+
}
diff --git a/source/RobotAPI/units/TCPControlUnit.h b/source/RobotAPI/units/TCPControlUnit.h
index d66e04be6..368bdd30b 100644
--- a/source/RobotAPI/units/TCPControlUnit.h
+++ b/source/RobotAPI/units/TCPControlUnit.h
@@ -125,7 +125,7 @@ namespace armarx {
};
- class EDifferentialIK : public VirtualRobot::DifferentialIK
+ class EDifferentialIK : public VirtualRobot::DifferentialIK, virtual public Logging
{
public:
EDifferentialIK(VirtualRobot::RobotNodeSetPtr rns,VirtualRobot:: RobotNodePtr coordSystem = VirtualRobot::RobotNodePtr(), VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = eSVD);
@@ -137,6 +137,16 @@ namespace armarx {
void clearGoals();
void setRefFrame(VirtualRobot:: RobotNodePtr coordSystem);
+
+ void setDOFWeights(Eigen::VectorXf dofWeights);
+ void setResultDimensionWeights(Eigen::VectorXf tcpWeights);
+ Eigen::VectorXf computeSteps(float stepSize, float minumChange, int maxNStep);
+ Eigen::VectorXf computeStep(float stepSize);
+ protected:
+ bool adjustDOFWeightsToJointLimits(const Eigen::VectorXf &plannedJointDeltas);
+
+ Eigen::VectorXf dofWeights;
+ Eigen::VectorXf tcpWeights;
};
typedef boost::shared_ptr<EDifferentialIK> EDifferentialIKPtr;
--
GitLab