diff --git a/source/RobotAPI/units/TCPControlUnit.cpp b/source/RobotAPI/units/TCPControlUnit.cpp
index 48ec1595cbbdfd66d696e3f68cade8d9c64c4dd3..cd6d14879dd2d797f3afd769dd9bc990f62f6d15 100644
--- a/source/RobotAPI/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/units/TCPControlUnit.cpp
@@ -44,8 +44,9 @@ namespace armarx
TCPControlUnit::TCPControlUnit() :
maxJointVelocity(30.f/180*3.141),
- cycleTime(20),
- requested(false)
+ cycleTime(30),
+ requested(false),
+ calculationRunning(false)
{
}
@@ -73,12 +74,8 @@ namespace armarx
kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
-
-
-
remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
-
std::string robotFile = getProperty<std::string>("RobotFileName").getValue();
if (!ArmarXDataPath::getAbsolutePath(robotFile,robotFile))
{
@@ -88,10 +85,13 @@ namespace armarx
localRobot->setUpdateVisualization(false);
localRobot->setThreadsafe(false);
localReportRobot = localRobot->clone("reportRobot");
+ jointExistenceCheckRobot = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
+ jointExistenceCheckRobot->setUpdateVisualization(false);
+ jointExistenceCheckRobot->setThreadsafe(false);
std::vector<RobotNodePtr> nodes = localRobot->getRobotNodes();
for(unsigned int i=0; i < nodes.size(); i++)
{
- ARMARX_INFO << nodes.at(i)->getName();
+ ARMARX_VERBOSE << nodes.at(i)->getName();
}
// dIK = new VirtualRobot::DifferentialIK(robotNodeSet);
@@ -117,6 +117,7 @@ namespace armarx
execTask->stop();
execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
execTask->start();
+ execTask->setDelayWarningTolerance(100);
}
@@ -144,7 +145,7 @@ 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);
+ ARMARX_CHECK_EXPRESSION_W_HINT(jointExistenceCheckRobot->hasRobotNodeSet(nodeSetName), "The robot does not have the node set: " + nodeSetName);
if(translationVelocity)
ARMARX_VERBOSE << "Setting new Velocity for " << nodeSetName << " in frame " << translationVelocity->frame << ":\n" << FramedVector3Ptr::dynamicCast(translationVelocity)->toEigen();
@@ -155,10 +156,10 @@ namespace armarx
data.translationVelocity = FramedVector3Ptr::dynamicCast(translationVelocity);
data.orientationVelocity = FramedVector3Ptr::dynamicCast(orientationVelocityRPY);
if(tcpName.empty())
- data.tcpName = localRobot->getRobotNodeSet(nodeSetName)->getTCP()->getName();
+ data.tcpName = jointExistenceCheckRobot->getRobotNodeSet(nodeSetName)->getTCP()->getName();
else
{
- ARMARX_CHECK_EXPRESSION_W_HINT(localRobot->hasRobotNode(tcpName), "The robot does not have the node: " + tcpName);
+ ARMARX_CHECK_EXPRESSION_W_HINT(jointExistenceCheckRobot->hasRobotNode(tcpName), "The robot does not have the node: " + tcpName);
data.tcpName = tcpName;
}
@@ -210,6 +211,7 @@ namespace armarx
lastReportTime = IceUtil::Time::now();
execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
execTask->start();
+ execTask->setDelayWarningTolerance(100);
ARMARX_IMPORTANT << "Requested TCPControlUnit";
}
@@ -217,7 +219,9 @@ namespace armarx
{
ARMARX_IMPORTANT << "Releasing TCPControlUnit";
ScopedLock lock(dataMutex);
+ while (calculationRunning) IceUtil::ThreadControl::yield();
tcpVelocitiesMap.clear();
+ localTcpVelocitiesMap.clear();
requested = false;
kinematicUnitPrx->stop();
ARMARX_IMPORTANT << "Released TCPControlUnit";
@@ -229,36 +233,50 @@ namespace armarx
{
// ARMARX_INFO << "SPAM";
// ARMARX_INFO << deactivateSpam(4) << "periodicExec for " << tcpVelocitiesMap.size();
-// IceUtil::Time startTime = IceUtil::Time::now();
-
- ScopedLock lock(dataMutex);
-
- double tDelta = (IceUtil::Time::now() - lastReportTime).toSecondsDouble();
+//
+ //double tDelta = (IceUtil::Time::now() - lastReportTime).toSecondsDouble();
lastReportTime = IceUtil::Time::now();
+// periodicReport(tDelta);
+ {
+ ScopedLock lock(dataMutex);
+ localTcpVelocitiesMap.clear();
+ TCPVelocityDataMap::iterator it = tcpVelocitiesMap.begin();
+ for(; it != tcpVelocitiesMap.end(); it++)
+ {
+ localTcpVelocitiesMap[it->first] = it->second;
+ }
- NameValueMap robotConfigMap = remoteRobotPrx->getConfig();
-// NameValueMap::iterator it = robotConfigMap.begin();
- std::map<std::string, float> jointValues(robotConfigMap.begin(), robotConfigMap.end());
+ localDIKMap.clear();
+ DIKMap::iterator itDIK = dIKMap.begin();
+ for(; itDIK != dIKMap.end(); itDIK++)
+ {
+ localDIKMap[itDIK->first] = itDIK->second;
+ }
- localRobot->setJointValues(jointValues);
+ NameValueMap robotConfigMap = remoteRobotPrx->getConfig();
+ std::map<std::string, float> jointValues(robotConfigMap.begin(), robotConfigMap.end());
+ localRobot->setJointValues(jointValues);
-// periodicReport(tDelta);
+ calculationRunning = true;
+ }
- if(!requested)
- return;
- calcAndSetVelocities();
+ if (requested)
+ {
+ IceUtil::Time startTime = IceUtil::Time::now();
+ calcAndSetVelocities();
+ ARMARX_INFO << "Time for calcAndSetVelocities(): " << (IceUtil::Time::now()-startTime).toMilliSeconds() << " ms";
+ }
+ calculationRunning = false;
+ }
- }
void TCPControlUnit::periodicReport(double tDelta)
{
-
-
std::vector<RobotNodeSetPtr > nodeSets = localRobot->getRobotNodeSets();
FramedPoseBaseMap tcpPoses;
FramedVector3Map tcpTranslationVelocities;
@@ -275,7 +293,6 @@ namespace armarx
FramedPoseBaseMap::iterator it = lastTCPPoses.find(tcpName);
if(it != lastTCPPoses.end())
{
-
FramedPosePtr lastPose = FramedPosePtr::dynamicCast(it->second);
tcpTranslationVelocities[tcpName] = new FramedVector3((currentPose.block(0,3,3,1) - lastPose->toEigen().block(0,3,3,1))/tDelta, rootFrame);
@@ -321,23 +338,23 @@ namespace armarx
void TCPControlUnit::calcAndSetVelocities()
{
- TCPVelocityDataMap::iterator it = tcpVelocitiesMap.begin();
- for(; it != tcpVelocitiesMap.end(); it++)
+ TCPVelocityDataMap::iterator it = localTcpVelocitiesMap.begin();
+ for(; it != localTcpVelocitiesMap.end(); it++)
{
const TCPVelocityData& data = it->second;
RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(data.nodeSetName);
// std::string refFrame = nodeSet->getTCP()->getName();
- DIKMap::iterator itDIK = dIKMap.find(data.nodeSetName);
- if(itDIK == dIKMap.end())
- dIKMap[data.nodeSetName].reset(new EDifferentialIK(nodeSet, localRobot->getRootNode()/*, VirtualRobot::JacobiProvider::eTranspose*/));
- EDifferentialIKPtr dIk = boost::shared_dynamic_cast<EDifferentialIK>(dIKMap[data.nodeSetName]);
+ DIKMap::iterator itDIK = localDIKMap.find(data.nodeSetName);
+ if(itDIK == localDIKMap.end())
+ localDIKMap[data.nodeSetName].reset(new EDifferentialIK(nodeSet, localRobot->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ EDifferentialIKPtr dIk = boost::shared_dynamic_cast<EDifferentialIK>(localDIKMap[data.nodeSetName]);
dIk->clearGoals();
}
using namespace Eigen;
- it = tcpVelocitiesMap.begin();
- for(; it != tcpVelocitiesMap.end(); it++)
+ it = localTcpVelocitiesMap.begin();
+ for(; it != localTcpVelocitiesMap.end(); it++)
{
TCPVelocityData& data = it->second;
@@ -388,7 +405,7 @@ namespace armarx
}
- DifferentialIKPtr dIK = dIKMap[data.nodeSetName];
+ DifferentialIKPtr dIK = localDIKMap[data.nodeSetName];
if(!dIK)
{
ARMARX_WARNING << deactivateSpam(1) << "DiffIK is NULL for robot node set: " << data.nodeSetName;
@@ -412,25 +429,42 @@ namespace armarx
NameValueMap targetVelocities;
NameControlModeMap controlModes;
- DIKMap::iterator itDIK = dIKMap.begin();
- for(; itDIK != dIKMap.end(); itDIK++)
+ DIKMap::iterator itDIK = localDIKMap.begin();
+ for(; itDIK != localDIKMap.end(); itDIK++)
{
RobotNodeSetPtr robotNodeSet = localRobot->getRobotNodeSet(itDIK->first);
EDifferentialIKPtr dIK = boost::shared_dynamic_cast<EDifferentialIK>(itDIK->second);
// ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << robotNodeSet->getTCP()->getGlobalPose();
// dIK->setVerbose(true);
Eigen::VectorXf jointDelta;
- dIK->computeSteps(jointDelta, 1, 0.00001, 100);
+ dIK->computeSteps(jointDelta, 0.8f, 0.001, 1); // 1.0, 0.00001, 50
// 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);
+ Eigen::VectorXf jointLimitCompensation = CalcJointLimitAvoidanceDeltas(robotNodeSet, fullJac, fullJacInv);
+
+ // for debugging only!
+ {
+ MatrixXf posJac = fullJac.block(0, 0, fullJac.rows(), 3);
+ MatrixXf oriJac = fullJac.block(0, 3, fullJac.rows(), 3);
+ JacobiSVD<MatrixXf> svdPos(posJac);
+ JacobiSVD<MatrixXf> svdOri(oriJac);
+ if (svdPos.singularValues()[2] < 90)
+ {
+ ARMARX_IMPORTANT << "The robot is close to a singularity! Minimal singular value of the position Jacobian: " << svdPos.singularValues()[2];
+ }
+ if (svdOri.singularValues()[2] < 0.6)
+ {
+ ARMARX_IMPORTANT << "The robot is close to a singularity! Minimal singular value of the orientation Jacobian: " << svdOri.singularValues()[2];
+ }
+ }
// added by David S
- if (jointLimitCompensation.norm() > 0.8*jointDelta.norm())
+ const float maxJointLimitCompensationRatio = 0.8;
+ if (jointLimitCompensation.norm() > maxJointLimitCompensationRatio*jointDelta.norm())
{
- jointLimitCompensation = 0.8*jointDelta.norm() / jointLimitCompensation.norm() * jointLimitCompensation;
+ jointLimitCompensation = maxJointLimitCompensationRatio*jointDelta.norm()/jointLimitCompensation.norm() * jointLimitCompensation;
}
jointDelta += jointLimitCompensation;
@@ -461,8 +495,6 @@ namespace armarx
}
-
-
kinematicUnitPrx->switchControlMode(controlModes);
kinematicUnitPrx->setJointVelocities(targetVelocities);
}
@@ -493,13 +525,11 @@ namespace armarx
else if(fabs(newAngle.angle()+offset - oldAngle.angle()) > fabs((newAngle.angle()+M_PI*2+offset) - oldAngle.angle()) )
offset += M_PI*2;
newAngle.angle() += offset;
-
}
Eigen::VectorXf TCPControlUnit::CalcJointLimitAvoidanceDeltas(VirtualRobot::RobotNodeSetPtr robotNodeSet, const Eigen::MatrixXf& jacobian, const Eigen::MatrixXf& jacobianInverse, Eigen::VectorXf desiredJointValues)
{
-
std::vector< VirtualRobot::RobotNodePtr > nodes = robotNodeSet->getAllRobotNodes();
Eigen::VectorXf actualJointValues(nodes.size());
if(desiredJointValues.rows() == 0)
@@ -551,7 +581,7 @@ namespace armarx
if(currentMaxJointVel > maxJointVelocity)
{
- ARMARX_WARNING << deactivateSpam(1) << "max joint velocity too high: " << currentMaxJointVel << " allowed: " << maxJointVelocity;
+ ARMARX_IMPORTANT << deactivateSpam(1) << "max joint velocity too high: " << currentMaxJointVel << " allowed: " << maxJointVelocity;
return jointVelocity * (maxJointVelocity/currentMaxJointVel);
}
else
@@ -597,7 +627,6 @@ namespace armarx
}
return Jacobian;
-
}
void EDifferentialIK::clearGoals()
@@ -802,7 +831,6 @@ namespace armarx
}
-
applyDOFWeightsToJacobian(Jacobian);
MatrixXf pseudo = computePseudoInverseJacobianMatrix(Jacobian);
@@ -813,7 +841,6 @@ namespace armarx
VectorXf EDifferentialIK::computeStepIndependently(float stepSize )
{
-
if (nRows==0) this->setNRows();
size_t nDoF = nodes.size();
@@ -867,18 +894,12 @@ namespace armarx
}
-
-
-
-
ARMARX_INFO << deactivateSpam(0.05) << "step size adjusted error to goal:\n" << partError;
applyDOFWeightsToJacobian(partJacobian);
// ARMARX_INFO << "Jac:\n" << partJacobian;
-
-
MatrixXf pseudo = computePseudoInverseJacobianMatrix(partJacobian);
Eigen::VectorXf tcpWeightVec;
@@ -905,7 +926,6 @@ namespace armarx
}
-
if(pseudo.cols() == tcpWeightVec.rows())
{
@@ -921,7 +941,6 @@ namespace armarx
ARMARX_WARNING << deactivateSpam(3) << "Wrong dimension of tcp weights: " << tcpWeightVec.rows() << ", but should be: " << pseudo.cols();
-
thetaSum += pseudo * partError;
}
@@ -1112,6 +1131,7 @@ namespace armarx
}
+
}
@@ -1128,7 +1148,7 @@ void armarx::TCPControlUnit::reportJointAngles(const NameValueMap &jointAngles,
std::string rootFrame = localReportRobot->getRootNode()->getName();
localReportRobot->setJointValues(jointAngles);
- IceUtil::Time start = IceUtil::Time::now();
+ //IceUtil::Time start = IceUtil::Time::now();
for(unsigned int i=0; i < nodeSets.size(); i++)
{
RobotNodeSetPtr nodeSet = nodeSets.at(i);
@@ -1176,7 +1196,7 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel,
nodeSets = localVelReportRobot->getRobotNodeSets();
localVelReportRobot->setJointValues(tempJointAngles);
- IceUtil::Time start = IceUtil::Time::now();
+ //IceUtil::Time start = IceUtil::Time::now();
for(unsigned int i=0; i < nodeSets.size(); i++)
{
RobotNodeSetPtr nodeSet = nodeSets.at(i);
@@ -1184,9 +1204,6 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel,
const Eigen::Matrix4f ¤tPose = nodeSet->getTCP()->getGlobalPose();
-
-
-
FramedPosePtr lastPose = FramedPosePtr::dynamicCast(tcpPoses[tcpName]);
tcpTranslationVelocities[tcpName] = new FramedVector3((currentPose.block(0,3,3,1) - lastPose->toEigen().block(0,3,3,1))/tDelta, rootFrame);
@@ -1226,7 +1243,6 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel,
}
// ARMARX_INFO << deactivateSpam(5) << "TCP Pose Vel Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
listener->reportTCPVelocities(tcpTranslationVelocities, tcpOrientationVelocities);
-
}
void armarx::TCPControlUnit::reportJointTorques(const NameValueMap &, bool, const Ice::Current &)
diff --git a/source/RobotAPI/units/TCPControlUnit.h b/source/RobotAPI/units/TCPControlUnit.h
index 122b21b629c218ab5122814791f6556b74afa3ad..762165a695af1ef5ee30e588349ec7f1250f3399 100644
--- a/source/RobotAPI/units/TCPControlUnit.h
+++ b/source/RobotAPI/units/TCPControlUnit.h
@@ -46,7 +46,7 @@ namespace armarx {
defineRequiredProperty<std::string>("KinematicUnitName","Name of the KinematicUnit Proxy");
defineOptionalProperty<float>("MaxJointVelocity",30.f/180*3.141, "Maximal joint velocity in rad/sec");
defineRequiredProperty<std::string>("RobotFileName", "Robot file name, e.g. robot_model.xml");
- defineOptionalProperty<int>("CycleTime", 50, "Cycle time of the tcp control in ms");
+ defineOptionalProperty<int>("CycleTime", 30, "Cycle time of the tcp control in ms");
}
};
@@ -107,10 +107,10 @@ namespace armarx {
};
typedef std::map<std::string, TCPVelocityData> TCPVelocityDataMap;
- TCPVelocityDataMap tcpVelocitiesMap;
+ TCPVelocityDataMap tcpVelocitiesMap, localTcpVelocitiesMap;
typedef std::map<std::string, VirtualRobot::DifferentialIKPtr> DIKMap;
- DIKMap dIKMap;
+ DIKMap dIKMap, localDIKMap;
// FramedVector3Map targetTranslationVelocities;
// FramedVector3Map targetOrientationVelocitiesRPY;
@@ -120,6 +120,7 @@ namespace armarx {
RobotStateComponentInterfacePrx robotStateComponentPrx;
KinematicUnitInterfacePrx kinematicUnitPrx;
SharedRobotInterfacePrx remoteRobotPrx;
+ VirtualRobot::RobotPtr jointExistenceCheckRobot;
VirtualRobot::RobotPtr localRobot;
VirtualRobot::RobotPtr localReportRobot;
VirtualRobot::RobotPtr localVelReportRobot;
@@ -140,6 +141,8 @@ namespace armarx {
Mutex dataMutex;
Mutex taskMutex;
Mutex reportMutex;
+ bool calculationRunning;
+
std::string topicName;