diff --git a/interface/slice/units/TCPControlUnit.ice b/interface/slice/units/TCPControlUnit.ice
index b35bb035f312bfb555b589c0543a2cfed0309d40..6b7517f5f42fbe41cb08266ce4b053d859a38438 100644
--- a/interface/slice/units/TCPControlUnit.ice
+++ b/interface/slice/units/TCPControlUnit.ice
@@ -30,6 +30,7 @@
#include <observers/VariantBase.ice>
#include <observers/ObserverInterface.ice>
#include <robotstate/RobotState.ice>
+#include <robotstate/PoseBase.ice>
module armarx
{
@@ -37,11 +38,25 @@ module armarx
interface TCPControlUnitInterface extends armarx::SensorActorUnitInterface
{
void setCycleTime(int milliseconds);
- void setTCPVelocity(string tcpNodeName, FramedVector3Base translationVelocity, FramedVector3Base orientationVelocityRPY);
+// void setTCPVelocity(string robotNodeSetName, FramedVector3Base translationVelocity, FramedVector3Base orientationVelocityRPY);
+ void setTCPVelocity(string robotNodeSetName, string tcpNodeName, FramedVector3Base translationVelocity, FramedVector3Base orientationVelocityRPY);
};
+ dictionary<string, FramedPoseBase> FramedPoseBaseMap;
+ interface TCPControlUnitListener
+ {
+ void reportTCPPose(FramedPoseBaseMap tcpPoses);
+ void reportTCPVelocities(FramedVector3Map tcpTranslationVelocities, FramedVector3Map tcpOrienationVelocities);
+
+ };
+
+ interface TCPControlUnitObserverInterface extends ObserverInterface, TCPControlUnitListener
+ {
+
+ };
+
};
diff --git a/source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h b/source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h
index 7aa35ef134e9537ab660d3820a04a3d6e30b33b8..4ab7d2dee5cd1843c729a4ea0f3ab2571b6cf801 100755
--- a/source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h
+++ b/source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h
@@ -24,6 +24,7 @@
#include <Core/core/application/Application.h>
#include <RobotAPI/units/TCPControlUnit.h>
+#include <RobotAPI/units/TCPControlUnitObserver.h>
namespace armarx
{
@@ -33,6 +34,7 @@ namespace armarx
void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
{
registry->addObject( Component::create<TCPControlUnit>(properties) );
+ registry->addObject( Component::create<TCPControlUnitObserver>(properties) );
}
};
}
diff --git a/source/RobotAPI/units/CMakeLists.txt b/source/RobotAPI/units/CMakeLists.txt
index c5a957fdda25a6e960718e6755f4023c51be57e5..50c02b62cac78eb3aeb48e831c13cda9125baf48 100755
--- a/source/RobotAPI/units/CMakeLists.txt
+++ b/source/RobotAPI/units/CMakeLists.txt
@@ -17,13 +17,16 @@ if (ARMARX_BUILD)
set(LIBS RobotAPIInterfaces RobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRemoteRobot ${VirtualRobot_LIBRARIES})
- set(LIB_FILES
+ set(LIB_HEADERS
ForceTorqueObserver.h
TCPControlUnit.h
+ TCPControlUnitObserver.h
+
)
- set(LIB_HEADERS
+ set(LIB_FILES
ForceTorqueObserver.cpp
TCPControlUnit.cpp
+ TCPControlUnitObserver.cpp
)
diff --git a/source/RobotAPI/units/ForceTorqueObserver.cpp b/source/RobotAPI/units/ForceTorqueObserver.cpp
index d5db0ce9fa9a0e2e14ee41896f7bd1fb1c7c391e..dd43ebb29a7ad5e331d6aed852f6e672f196bbbc 100644
--- a/source/RobotAPI/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/units/ForceTorqueObserver.cpp
@@ -25,7 +25,7 @@ ForceTorqueObserver::ForceTorqueObserver()
void ForceTorqueObserver::onInitObserver()
{
- usingTopic(getProperty<std::string>("ForceTorqueUnitName").getValue() + "State");
+ usingTopic(getProperty<std::string>("ForceTorqueTopicName").getValue());
// register all checks
offerConditionCheck("updated", new ConditionCheckUpdated());
diff --git a/source/RobotAPI/units/ForceTorqueObserver.h b/source/RobotAPI/units/ForceTorqueObserver.h
index 59d453b4bd2c9380ed2907a0dd59478ae2ed9557..8b9c34dc2276e7f88c74577ed65fbbaebd62e450 100644
--- a/source/RobotAPI/units/ForceTorqueObserver.h
+++ b/source/RobotAPI/units/ForceTorqueObserver.h
@@ -14,7 +14,7 @@ namespace armarx
ForceTorqueObserverPropertyDefinitions(std::string prefix):
ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("ForceTorqueUnitName","Name of the ForceTorqueUnit");
+ defineRequiredProperty<std::string>("ForceTorqueTopicName","Name of the ForceTorqueUnit Topic");
}
};
diff --git a/source/RobotAPI/units/TCPControlUnit.cpp b/source/RobotAPI/units/TCPControlUnit.cpp
index 687f352d40ef074ee68ab4088c17f549c8ba56dd..5f0123486f1bf598a746cbcd66397d93828eee71 100644
--- a/source/RobotAPI/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/units/TCPControlUnit.cpp
@@ -32,6 +32,7 @@
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/XML/RobotIO.h>
+using namespace VirtualRobot;
@@ -49,6 +50,7 @@ namespace armarx
{
usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
usingProxy("RobotStateComponent");
+ offeringTopic("TCPControlUnitState");
}
@@ -91,10 +93,16 @@ namespace armarx
// setTCPVelocity("RightLeg", tcpVel, NULL);
// setTCPVelocity("LeftLeg", tcpVel, NULL);
// request();
+
+ listener = getTopic<TCPControlUnitListenerPrx>("TCPControlUnitState");
+ topicTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicReport,cycleTime, false, "TCPDataProvider");
+ topicTask->start();
}
void TCPControlUnit::onDisconnectComponent()
{
+ if(topicTask)
+ topicTask->stop();
release();
}
@@ -112,14 +120,26 @@ namespace armarx
execTask->changeInterval(cycleTime);
}
- void TCPControlUnit::setTCPVelocity(const std::string &tcpNodeName, const FramedVector3BasePtr &translationVelocity, const FramedVector3BasePtr &orientationVelocityRPY, const Ice::Current &c)
+ 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_VERBOSE << "Setting new Velocity\n" << FramedVector3Ptr::dynamicCast(translationVelocity)->toEigen();
if(translationVelocity)
- targetTranslationVelocities[tcpNodeName] = translationVelocity;
+ ARMARX_VERBOSE << "Setting new Velocity for " << nodeSetName << "\n" << FramedVector3Ptr::dynamicCast(translationVelocity)->toEigen();
if(orientationVelocityRPY)
- targetOrientationVelocitiesRPY[tcpNodeName] = orientationVelocityRPY;
+ ARMARX_VERBOSE << "Orientation Velo: " << FramedVector3Ptr::dynamicCast(orientationVelocityRPY)->toEigen();
+ TCPVelocityData data;
+ data.nodeSetName = nodeSetName;
+ data.translationVelocity = FramedVector3Ptr::dynamicCast(translationVelocity);
+ data.orientationVelocity = FramedVector3Ptr::dynamicCast(orientationVelocityRPY);
+ if(tcpName.empty())
+ data.tcpName = localRobot->getRobotNodeSet(nodeSetName)->getTCP()->getName();
+ else
+ data.tcpName = tcpName;
+ tcpVelocitiesMap[data.tcpName] = data;
+// if(translationVelocity)
+// targetTranslationVelocities[nodeSetName] = translationVelocity;
+// if(orientationVelocityRPY)
+// targetOrientationVelocitiesRPY[nodeSetName] = orientationVelocityRPY;
}
@@ -161,8 +181,11 @@ namespace armarx
cycleTime = this->cycleTime;
}
ARMARX_IMPORTANT << "Requesting TCPControlUnit";
+ if(execTask)
+ execTask->stop();
execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, true, "TCPVelocityCalculator");
execTask->start();
+ ARMARX_IMPORTANT << "Requested TCPControlUnit";
}
void TCPControlUnit::release(const Ice::Current & c)
@@ -172,165 +195,174 @@ namespace armarx
if(execTask)
execTask->stop();
kinematicUnitPrx->stop();
+ ARMARX_IMPORTANT << "Released TCPControlUnit";
+
}
void TCPControlUnit::periodicExec()
{
+ ARMARX_INFO << deactivateSpam(4) << "periodicExec for " << tcpVelocitiesMap.size();
// IceUtil::Time startTime = IceUtil::Time::now();
ScopedLock lock(dataMutex);
-// std::stringstream time;
-// time << IceUtil::Time::now().toMicroSeconds();
- // VirtualRobot::RobotPtr remoteRobot;
- // remoteRobot.reset( new RemoteRobot(robotStateComponentPrx->getRobotSnapshot(time.str())));
- // remoteRobot->setUpdateVisualization(false);
- // remoteRobot->setThreadsafe(false);
+
NameValueMap robotConfigMap = remoteRobotPrx->getConfig();
- NameValueMap::iterator it = robotConfigMap.begin();
+// NameValueMap::iterator it = robotConfigMap.begin();
std::map<std::string, float> jointValues(robotConfigMap.begin(), robotConfigMap.end());
-// std::vector<std::string> nodeNames;
-// std::vector<float> nodeValues;
-// for(; it != robotConfigMap.end(); it++)
-// {
-// std::string name = it->first;
-// if(name.find("LeftArm") != std::string::npos)
-// ARMARX_INFO << "New Values: " << it->first << " is " << it->second;
-// nodeNames.push_back(it->first);
-// nodeValues.push_back(it->second);
-// localRobot->getRobotNode(it->first)->setJointValue(it->second);
-// ARMARX_INFO << "New Values: " << it->first << " is " << localRobot->getRobotNode(it->first)->getJointValue();
-// }
-// VirtualRobot::RobotConfigPtr robotConfig(new VirtualRobot::RobotConfig(localRobot, "localRobot", nodeNames, nodeValues));
-// if(!robotConfig->setJointValues())
-// throw LocalException("Could not update local robot joint values");
+
localRobot->setJointValues(jointValues);
- // ARMARX_IMPORTANT << "LeftArm_Joint3: " << localRobot->getRobotNode("LeftArm_Joint3")->getJointValue();
- FramedVector3Map::const_iterator itTrans = targetTranslationVelocities.begin();
- for(; itTrans != targetTranslationVelocities.end(); itTrans++)
+// dIKMap.clear();
+ TCPVelocityDataMap::iterator it = tcpVelocitiesMap.begin();
+ for(; it != tcpVelocitiesMap.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*/));
+ boost::shared_dynamic_cast<EDifferentialIK>(dIKMap[data.nodeSetName])->clearGoals();
+ }
- Eigen::VectorXf jointDelta;
+ using namespace Eigen;
- FramedVector3Ptr targetTranslationVelocity = FramedVector3Ptr::dynamicCast(itTrans->second);
- if(targetTranslationVelocity->getFrame() == "")
- continue;
- VirtualRobot::RobotNodeSetPtr robotNodeSet = localRobot->getRobotNodeSet(itTrans->first);
- std::string refFrame = targetTranslationVelocity->getFrame();
+ it = tcpVelocitiesMap.begin();
+ for(; it != tcpVelocitiesMap.end(); it++)
+ {
+ TCPVelocityData& data = it->second;
+ RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(data.nodeSetName);
+ std::string refFrame = localRobot->getRootNode()->getName();
- FramedVector3Map::const_iterator itOri = targetOrientationVelocitiesRPY.find(itTrans->first);
- if(itOri != targetOrientationVelocitiesRPY.end() && itOri->second)
+ IKSolver::CartesianSelection mode;
+ if(data.translationVelocity && data.orientationVelocity)
{
- FramedVector3Ptr targetOrientationVelocitiesRPY = FramedVector3Ptr::dynamicCast(itOri->second);;
- if(targetOrientationVelocitiesRPY->getFrame() == "")
- continue;
-
-
-// if(targetTranslationVelocity->toEigen().norm() == 0
-// && targetOrientationVelocitiesRPY->toEigen().norm() == 0)
-// continue;
+ mode = IKSolver::All;
+// ARMARX_INFO << deactivateSpam(4) << "FullMode";
+ }
+ else if(data.translationVelocity && !data.orientationVelocity)
+ mode = IKSolver::Position;
+ else if(!data.translationVelocity && data.orientationVelocity)
+ mode = IKSolver::Orientation;
+ else
+ {
+// ARMARX_VERBOSE << deactivateSpam(2) << "No mode feasible for " << data.nodeSetName << " - skipping";
+ continue;
+ }
+ RobotNodePtr tcpNode = localRobot->getRobotNode(data.tcpName);
+ Eigen::Matrix4f m;
+ m.setIdentity();
+ if(data.orientationVelocity)
+ {
+ data.orientationVelocity = FramedVector3::ChangeFrame(localRobot, *data.orientationVelocity, refFrame);
+// ARMARX_INFO << deactivateSpam(1) << "Orientation in " << refFrame << ": " << data.orientationVelocity->toEigen();
+ Eigen::Matrix3f rot;
+ rot = Eigen::AngleAxisf(data.orientationVelocity->z, Eigen::Vector3f::UnitZ())
+ * Eigen::AngleAxisf(data.orientationVelocity->y, Eigen::Vector3f::UnitY())
+ * Eigen::AngleAxisf(data.orientationVelocity->x, Eigen::Vector3f::UnitX());
+ m.block(0,0,3,3) = rot * tcpNode->getGlobalPose().block(0,0,3,3);
+ }
+// ARMARX_VERBOSE << deactivateSpam(1) << "Delta Mat: \n" << m;
- FramedVector3Ptr lVecTrans = FramedVector3::ChangeFrame(localRobot, *targetTranslationVelocity, refFrame);
- FramedVector3Ptr lVecOri = FramedVector3::ChangeFrame(localRobot, *targetOrientationVelocitiesRPY, refFrame);
- Eigen::VectorXf tcpDelta(6);
- tcpDelta.head(3) = lVecTrans->toEigen();
- tcpDelta.tail(3) = lVecOri->toEigen();
- jointDelta = calcJointVelocities(robotNodeSet, tcpDelta, localRobot->getRobotNode(refFrame), VirtualRobot::IKSolver::All);
+// m = m * tcpNode->getGlobalPose();
- // for (int r = 0; r < jointDelta.rows(); ++r) {
- // if(jointDelta(r) > maxJointVelocity)
- // {
- //
- // break;
- // }
- // }
+ if(data.translationVelocity)
+ {
+ data.translationVelocity = FramedVector3::ChangeFrame(localRobot, *data.translationVelocity, refFrame);
+// ARMARX_INFO << deactivateSpam(1) << "Translation in " << refFrame << ": " << data.translationVelocity->toEigen();
+ m.block(0,3,3,1) = tcpNode->getGlobalPose().block(0,3,3,1) + data.translationVelocity->toEigen();
+ }
- }
- else // only position control
+ DifferentialIKPtr dIK = dIKMap[data.nodeSetName];
+ if(!dIK)
{
-// if(targetTranslationVelocity->toEigen().norm() == 0)
-// continue;
+ ARMARX_WARNING << deactivateSpam(1) << "DiffIK is NULL for robot node set: " << data.nodeSetName;
+ continue;
+ }
+// ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << tcpNode->getGlobalPose();
+// ARMARX_VERBOSE << deactivateSpam(1) << "New Goal: \n" << m;
+ dIK->setGoal(m, tcpNode,mode);
- FramedVector3Ptr lVecTrans = FramedVector3::ChangeFrame(localRobot, *targetTranslationVelocity, refFrame);
+// ARMARX_VERBOSE << deactivateSpam(1) << "Delta to Goal: " << dIK->getDeltaToGoal(tcpNode);
+ }
- Eigen::VectorXf tcpDelta(3);
- tcpDelta = lVecTrans->toEigen();
- jointDelta = calcJointVelocities(robotNodeSet, tcpDelta, localRobot->getRobotNode(refFrame), VirtualRobot::IKSolver::Position);
+ NameValueMap targetVelocities;
+ NameControlModeMap controlModes;
+ DIKMap::iterator itDIK = dIKMap.begin();
+ for(; itDIK != dIKMap.end(); itDIK++)
+ {
+ RobotNodeSetPtr robotNodeSet = localRobot->getRobotNodeSet(itDIK->first);
+ EDifferentialIKPtr dIK = boost::shared_dynamic_cast<EDifferentialIK>(itDIK->second);
+ Eigen::VectorXf jointDelta = dIK->computeStep(1.0);
- }
+ MatrixXf fullJac = dIK->calcFullJacobian();
+ MatrixXf fullJacInv = dIK->computePseudoInverseJacobianMatrix(fullJac);
+ Eigen::VectorXf jointLimitCompensation = CalcJointLimitAvoidanceDeltas(robotNodeSet,fullJac, fullJacInv);
+ jointDelta += jointLimitCompensation;
- jointDelta = applyMaxJointVelocity(jointDelta,this->maxJointVelocity);
+ jointDelta = applyMaxJointVelocity(jointDelta, maxJointVelocity);
Eigen::Vector3f currentTCPPosition = robotNodeSet->getTCP()->getGlobalPose().block(0,3,3,1);
-// ARMARX_INFO << "Current TCP Position: \n" << currentTCPPosition;
- ARMARX_DEBUG << deactivateSpam(2)<< "ActualTCPVelocity: " << (currentTCPPosition - lastTCPPose.block(0,3,3,1))/(0.001*cycleTime);
+ // ARMARX_INFO << "Current TCP Position: \n" << currentTCPPosition;
+ ARMARX_VERBOSE << deactivateSpam(2)<< "ActualTCPVelocity: " << (currentTCPPosition - lastTCPPose.block(0,3,3,1))/(0.001*cycleTime);
lastTCPPose = robotNodeSet->getTCP()->getGlobalPose();
// build name value map
- NameValueMap targetVelocities;
- NameControlModeMap controlModes;
+
const std::vector< VirtualRobot::RobotNodePtr > nodes = robotNodeSet->getAllRobotNodes();
std::vector< VirtualRobot::RobotNodePtr >::const_iterator iter = nodes.begin();
int i = 0;
while (iter != nodes.end() && i < jointDelta.rows())
{
+ if(targetVelocities.find((*iter)->getName()) != targetVelocities.end())
+ ARMARX_WARNING << deactivateSpam(2) << (*iter)->getName() << " is set from two joint delta calculations - overwriting first value";
targetVelocities.insert(std::make_pair((*iter)->getName(), jointDelta(i)));
controlModes.insert(std::make_pair((*iter)->getName(), eVelocityControl));
i++;
iter++;
};
-
- // execute velocities
- kinematicUnitPrx->switchControlMode(controlModes);
- kinematicUnitPrx->setJointVelocities(targetVelocities);
-
}
-// ARMARX_INFO << "Compensation (usec): " << (IceUtil::Time::now() - startTime).toMicroSecondsDouble();
-
- }
- Eigen::VectorXf TCPControlUnit::calcJointVelocities(VirtualRobot::RobotNodeSetPtr robotNodeSet, Eigen::VectorXf tcpDelta, VirtualRobot::RobotNodePtr refFrame, VirtualRobot::IKSolver::CartesianSelection mode)
- {
-// IceUtil::Time startCreation = IceUtil::Time::now();
- // TODO: Turn into member variable!
- VirtualRobot::DifferentialIKPtr dIK(new VirtualRobot::DifferentialIK(robotNodeSet, refFrame/*, VirtualRobot::JacobiProvider::eTranspose*/));
- VirtualRobot::RobotNodePtr tcpNode = robotNodeSet->getTCP();
-// ARMARX_INFO << "creation: " << (IceUtil::Time::now() - startCreation).toMicroSecondsDouble();
+ kinematicUnitPrx->switchControlMode(controlModes);
+ kinematicUnitPrx->setJointVelocities(targetVelocities);
-// IceUtil::Time startJacobian = IceUtil::Time::now();
- Eigen::MatrixXf J = dIK->getJacobianMatrix(tcpNode, mode);
- Eigen::MatrixXf Ji = dIK->getPseudoInverseJacobianMatrix(tcpNode, mode);
-// ARMARX_INFO << "Jacobian (usec): " << (IceUtil::Time::now() - startJacobian).toMicroSecondsDouble();
+ }
- // ARMARX_INFO << "TCP Delta (" << tcpDelta.rows() << ")\n" << tcpDelta;
- ARMARX_CHECK_EXPRESSION_W_HINT(tcpDelta.rows() == 6 || tcpDelta.rows() == 3, "TCPDelta Vector must have 6 or 3 items");
+ void TCPControlUnit::periodicReport()
+ {
+ ScopedLock lock(dataMutex);
-// IceUtil::Time startCompensation = IceUtil::Time::now();
- Eigen::VectorXf jointLimitCompensation = CalcJointLimitAvoidanceDeltas(robotNodeSet, J, Ji);
-// ARMARX_INFO << "Compensation (usec): " << (IceUtil::Time::now() - startCompensation).toMicroSecondsDouble();
+ NameValueMap robotConfigMap = remoteRobotPrx->getConfig();
+// NameValueMap::iterator it = robotConfigMap.begin();
+ std::map<std::string, float> jointValues(robotConfigMap.begin(), robotConfigMap.end());
- // calculat joint error
- Eigen::VectorXf deltaJoint = Ji * (tcpDelta);
-// ARMARX_IMPORTANT << deactivateSpam(true, 0.5) << "joint compensation deltas: " << jointLimitCompensation;
-// ARMARX_IMPORTANT << deactivateSpam(true, 0.5) << "joint deltas: " << deltaJoint;
- deltaJoint += jointLimitCompensation;
- return deltaJoint;
+ localRobot->setJointValues(jointValues);
+ std::vector<RobotNodeSetPtr > nodeSets = localRobot->getRobotNodeSets();
+ FramedPoseBaseMap tcpPoses;
+ std::string rootFrame = localRobot->getRootNode()->getName();
+ for(unsigned int i=0; i < nodeSets.size(); i++)
+ {
+ RobotNodeSetPtr nodeSet = nodeSets.at(i);
+ tcpPoses[nodeSet->getTCP()->getName()]= new FramedPose(nodeSet->getTCP()->getGlobalPose(),rootFrame);
+ }
+ listener->reportTCPPose(tcpPoses);
}
+
Eigen::VectorXf TCPControlUnit::CalcJointLimitAvoidanceDeltas(VirtualRobot::RobotNodeSetPtr robotNodeSet, const Eigen::MatrixXf& jacobian, const Eigen::MatrixXf& jacobianInverse, Eigen::VectorXf desiredJointValues)
{
@@ -401,6 +433,53 @@ namespace armarx
getConfigIdentifier()));
}
+ EDifferentialIK::EDifferentialIK(RobotNodeSetPtr rns, RobotNodePtr coordSystem, JacobiProvider::InverseJacobiMethod invJacMethod) :
+ DifferentialIK(rns, coordSystem, invJacMethod)
+ {
+
+ }
+
+ Eigen::MatrixXf EDifferentialIK::calcFullJacobian()
+ {
+ if (nRows==0) this->setNRows();
+ size_t nDoF = nodes.size();
+
+ using namespace Eigen;
+ MatrixXf Jacobian(nRows,nDoF);
+
+ 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())
+ {
+ IKSolver::CartesianSelection mode = this->modes[tcp];
+ MatrixXf partJacobian = this->getJacobianMatrix(tcp,mode);
+ Jacobian.block(index,0,partJacobian.rows(),nDoF) = partJacobian;
+ }
+ else
+ VR_ERROR << "Internal error?!" << endl; // Error
+
+
+ }
+ return Jacobian;
+
+ }
+
+ void EDifferentialIK::clearGoals()
+ {
+ targets.clear();
+ modes.clear();
+ tolerancePosition.clear();
+ toleranceRotation.clear();
+ parents.clear();
+ }
+
+ void EDifferentialIK::setRefFrame(RobotNodePtr coordSystem)
+ {
+ this->coordSystem = coordSystem;
+ }
+
}
diff --git a/source/RobotAPI/units/TCPControlUnit.h b/source/RobotAPI/units/TCPControlUnit.h
index 3da7bb6728d7781b0a6adf7911c6ffe3534a737a..d66e04be6b015669238fc104ab18c48d27a1e5d3 100644
--- a/source/RobotAPI/units/TCPControlUnit.h
+++ b/source/RobotAPI/units/TCPControlUnit.h
@@ -66,7 +66,7 @@ namespace armarx {
// TCPControlUnitInterface interface
void setCycleTime(Ice::Int milliseconds, const Ice::Current &c = Ice::Current());
- void setTCPVelocity(const std::string &tcpNodeName, const::armarx::FramedVector3BasePtr &translationVelocity, const::armarx::FramedVector3BasePtr &orientationVelocityRPY, const Ice::Current &c = Ice::Current());
+ void setTCPVelocity(const std::string &nodeSetName, const std::string &tcpName, const::armarx::FramedVector3BasePtr &translationVelocity, const::armarx::FramedVector3BasePtr &orientationVelocityRPY, const Ice::Current &c = Ice::Current());
// UnitExecutionManagementInterface interface
void init(const Ice::Current &c = Ice::Current());
@@ -85,6 +85,7 @@ namespace armarx {
static Eigen::VectorXf CalcJointLimitAvoidanceDeltas(VirtualRobot::RobotNodeSetPtr robotNodeSet, const Eigen::MatrixXf &jacobian, const Eigen::MatrixXf &jacobianInverse, Eigen::VectorXf desiredJointValues = Eigen::VectorXf());
private:
void periodicExec();
+ void periodicReport();
Eigen::VectorXf calcJointVelocities(VirtualRobot::RobotNodeSetPtr robotNodeSet, Eigen::VectorXf tcpDelta, VirtualRobot::RobotNodePtr refFrame, VirtualRobot::IKSolver::CartesianSelection mode);
Eigen::VectorXf applyMaxJointVelocity(const Eigen::VectorXf &jointVelocity, float maxJointVelocity);
@@ -92,15 +93,30 @@ namespace armarx {
int cycleTime;
Eigen::Matrix4f lastTCPPose;
- FramedVector3Map targetTranslationVelocities;
- FramedVector3Map targetOrientationVelocitiesRPY;
+ struct TCPVelocityData
+ {
+ FramedVector3Ptr translationVelocity;
+ FramedVector3Ptr orientationVelocity;
+ std::string nodeSetName;
+ std::string tcpName;
+ };
+
+ typedef std::map<std::string, TCPVelocityData> TCPVelocityDataMap;
+ TCPVelocityDataMap tcpVelocitiesMap;
+
+ typedef std::map<std::string, VirtualRobot::DifferentialIKPtr> DIKMap;
+ DIKMap dIKMap;
+// FramedVector3Map targetTranslationVelocities;
+// FramedVector3Map targetOrientationVelocitiesRPY;
PeriodicTask<TCPControlUnit>::pointer_type execTask;
+ PeriodicTask<TCPControlUnit>::pointer_type topicTask;
RobotStateComponentInterfacePrx robotStateComponentPrx;
KinematicUnitInterfacePrx kinematicUnitPrx;
SharedRobotInterfacePrx remoteRobotPrx;
VirtualRobot::RobotPtr localRobot;
+ TCPControlUnitListenerPrx listener;
// VirtualRobot::RobotNodeSetPtr robotNodeSet;
// VirtualRobot::DifferentialIKPtr dIK;
@@ -109,6 +125,21 @@ namespace armarx {
};
+ class EDifferentialIK : public VirtualRobot::DifferentialIK
+ {
+ public:
+ EDifferentialIK(VirtualRobot::RobotNodeSetPtr rns,VirtualRobot:: RobotNodePtr coordSystem = VirtualRobot::RobotNodePtr(), VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = eSVD);
+
+ VirtualRobot::RobotNodePtr getRefFrame(){return coordSystem;}
+ int getJacobianRows(){ return nRows;}
+
+ Eigen::MatrixXf calcFullJacobian();
+
+ void clearGoals();
+ void setRefFrame(VirtualRobot:: RobotNodePtr coordSystem);
+ };
+ typedef boost::shared_ptr<EDifferentialIK> EDifferentialIKPtr;
+
}
#endif
diff --git a/source/RobotAPI/units/TCPControlUnitObserver.cpp b/source/RobotAPI/units/TCPControlUnitObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..20ae2d59ecdfecab25d2a9681d052aa732ec8e75
--- /dev/null
+++ b/source/RobotAPI/units/TCPControlUnitObserver.cpp
@@ -0,0 +1,117 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::
+* @author Mirko Waechter ( mirko.waechter at kit dot edu)
+* @date 2013
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#include "TCPControlUnitObserver.h"
+
+//#include <Core/robotstate/remote/checks/ConditionCheckEqualsPoseWithTolerance.h>
+#include <Core/observers/checks/ConditionCheckUpdated.h>
+#include <Core/observers/checks/ConditionCheckEquals.h>
+#include <Core/observers/checks/ConditionCheckInRange.h>
+#include <Core/observers/checks/ConditionCheckLarger.h>
+#include <Core/observers/checks/ConditionCheckSmaller.h>
+#include <Core/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h>
+
+namespace armarx {
+
+ TCPControlUnitObserver::TCPControlUnitObserver()
+ {
+ }
+
+ void TCPControlUnitObserver::onInitObserver()
+ {
+ usingTopic(getProperty<std::string>("TCPControlUnitName").getValue() + "State");
+
+ // register all checks
+ offerConditionCheck("updated", new ConditionCheckUpdated());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
+ // offerConditionCheck("magnitudeinrange", new ConditionCheckInRange());
+ offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
+// offerConditionCheck("equalsPoseWithTol", new ConditionCheckEqualsPoseWithTolerance());
+
+
+ offerChannel("TCPPose", "TCP poses of the robot.");
+ offerChannel("TCPVelocities", "TCP velocities of the robot.");
+
+ }
+
+ void TCPControlUnitObserver::onConnectObserver()
+ {
+
+ }
+
+ PropertyDefinitionsPtr TCPControlUnitObserver::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new TCPControlUnitObserverPropertyDefinitions(
+ getConfigIdentifier()));
+ }
+
+ void TCPControlUnitObserver::reportTCPPose(const FramedPoseBaseMap &poseMap, const Ice::Current &)
+ {
+ ScopedLock lock(dataMutex);
+ FramedPoseBaseMap::const_iterator it = poseMap.begin();
+ for(; it != poseMap.end(); it++)
+ {
+
+ FramedPosePtr vec = FramedPosePtr::dynamicCast(it->second);
+ const std::string &tcpName = it->first;
+ if(!existsDataField("TCPPose", tcpName))
+ offerDataFieldWithDefault("TCPPose", tcpName, Variant(it->second), "Pose of " + tcpName);
+ else
+ setDataField("TCPPose", tcpName, Variant(it->second));
+
+ if(!existsChannel(tcpName))
+ {
+ offerChannel(tcpName, "pose components of " + tcpName);
+ offerDataFieldWithDefault(tcpName,"x", Variant(vec->position->x), "X axis");
+ offerDataFieldWithDefault(tcpName,"y", Variant(vec->position->y), "Y axis");
+ offerDataFieldWithDefault(tcpName,"z", Variant(vec->position->z), "Z axis");
+ offerDataFieldWithDefault(tcpName,"qx", Variant(vec->orientation->qx), "Quaternion part x");
+ offerDataFieldWithDefault(tcpName,"qy", Variant(vec->orientation->qy), "Quaternion part y");
+ offerDataFieldWithDefault(tcpName,"qz", Variant(vec->orientation->qz), "Quaternion part z");
+ offerDataFieldWithDefault(tcpName,"qw", Variant(vec->orientation->qw), "Quaternion part w");
+ offerDataFieldWithDefault(tcpName,"frame", Variant(vec->frame), "Reference Frame");
+ }
+ else
+ {
+ setDataField(tcpName,"x", Variant(vec->position->x));
+ setDataField(tcpName,"y", Variant(vec->position->y));
+ setDataField(tcpName,"z", Variant(vec->position->z));
+ setDataField(tcpName,"qx", Variant(vec->orientation->qx));
+ setDataField(tcpName,"qy", Variant(vec->orientation->qy));
+ setDataField(tcpName,"qz", Variant(vec->orientation->qz));
+ setDataField(tcpName,"qw", Variant(vec->orientation->qw));
+ setDataField(tcpName,"frame", Variant(vec->frame));
+ }
+
+ }
+ }
+
+ void TCPControlUnitObserver::reportTCPVelocities(const FramedVector3Map &, const FramedVector3Map &, const Ice::Current &)
+ {
+ }
+
+}
+
+
diff --git a/source/RobotAPI/units/TCPControlUnitObserver.h b/source/RobotAPI/units/TCPControlUnitObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..727a9a17ac9300ad0afc94865ee4b952a58aa24a
--- /dev/null
+++ b/source/RobotAPI/units/TCPControlUnitObserver.h
@@ -0,0 +1,69 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::
+* @author Mirko Waechter ( mirko.waechter at kit dot edu)
+* @date 2013
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+#ifndef _ARMARX_TCPCONTROLUNITOBSERVER_H
+#define _ARMARX_TCPCONTROLUNITOBSERVER_H
+
+#include <Core/observers/Observer.h>
+#include <RobotAPI/interface/units/TCPControlUnit.h>
+
+namespace armarx {
+
+ class TCPControlUnitObserverPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ TCPControlUnitObserverPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineRequiredProperty<std::string>("TCPControlUnitName","Name of the TCPControlUnit");
+ }
+ };
+
+ class TCPControlUnitObserver :
+ virtual public Observer,
+ virtual public TCPControlUnitObserverInterface
+ {
+ public:
+ TCPControlUnitObserver();
+
+ // framework hooks
+ virtual std::string getDefaultName() const { return "TCPControlUnitObserver"; }
+ void onInitObserver();
+ void onConnectObserver();
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ // TCPControlUnitListener interface
+ public:
+ void reportTCPPose(const FramedPoseBaseMap &poseMap, const Ice::Current &);
+ void reportTCPVelocities(const FramedVector3Map &, const FramedVector3Map &, const Ice::Current &);
+
+ Mutex dataMutex;
+ };
+
+}
+
+#endif