From 01121cf40cdec69e91cfe538622d2877ee9a04da Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Wed, 7 Jan 2015 13:59:26 +0100
Subject: [PATCH] added agent field to FramedPose, ...
---
.../components/units/ForceTorqueUnit.h | 2 +
.../units/ForceTorqueUnitSimulation.cpp | 4 +-
.../RobotAPI/components/units/HeadIKUnit.cpp | 2 +-
.../components/units/TCPControlUnit.cpp | 185 +++++-------------
.../components/units/TCPControlUnit.h | 6 +-
.../GraphVisualizerGuiPlugin.cpp | 18 +-
.../ArmarXTCPMover/TCPMover.cpp | 5 +-
.../interface/robotstate/PoseBase.ice | 4 +
.../robotstate/RobotStateComponent.cpp | 2 +-
.../robotstate/RobotStateComponent.h | 1 +
.../libraries/robotstate/remote/ArmarPose.cpp | 54 +++--
.../libraries/robotstate/remote/ArmarPose.h | 16 +-
.../robotstate/remote/LinkedPose.cpp | 46 ++++-
.../libraries/robotstate/remote/LinkedPose.h | 8 +
14 files changed, 179 insertions(+), 174 deletions(-)
diff --git a/source/RobotAPI/components/units/ForceTorqueUnit.h b/source/RobotAPI/components/units/ForceTorqueUnit.h
index b9631ecb5..0733687a2 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnit.h
+++ b/source/RobotAPI/components/units/ForceTorqueUnit.h
@@ -42,6 +42,8 @@ namespace armarx
ForceTorqueUnitPropertyDefinitions(std::string prefix) :
ComponentPropertyDefinitions(prefix)
{
+ defineRequiredProperty<std::string>("AgentName", "Name of the agent for which the sensor values are provided");
+
// No required properties
}
};
diff --git a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
index d4d6b889a..f7e67c2b8 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
@@ -43,8 +43,8 @@ void ForceTorqueUnitSimulation::onInitForceTorqueUnit()
// forces[*s] = new armarx::FramedVector3(Eigen::Vector3f(0, 0, 0), *s);
// torques[*s] = new armarx::FramedVector3(Eigen::Vector3f(0, 0, 0), *s);
// }
- forces = new armarx::FramedVector3(Eigen::Vector3f(0, 0, 0), sensorName);
- torques = new armarx::FramedVector3(Eigen::Vector3f(0, 0, 0), sensorName);
+ forces = new armarx::FramedVector3(Eigen::Vector3f(0, 0, 0), sensorName, getProperty<std::string>("AgentName").getValue());
+ torques = new armarx::FramedVector3(Eigen::Vector3f(0, 0, 0), sensorName, getProperty<std::string>("AgentName").getValue());
ARMARX_VERBOSE << "Starting ForceTorqueUnitSimulation with " << intervalMs << " ms interval";
simulationTask = new PeriodicTask<ForceTorqueUnitSimulation>(this, &ForceTorqueUnitSimulation::simulationFunction, intervalMs, false, "ForceTorqueUnitSimulation");
diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp
index e3bdc087d..b0cd8339f 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.cpp
+++ b/source/RobotAPI/components/units/HeadIKUnit.cpp
@@ -186,7 +186,7 @@ namespace armarx
ikSolver.enableJointLimitAvoidance(true);
Eigen::Matrix3f m = Eigen::Matrix3f::Identity();
- FramedOrientationPtr targetOri = new FramedOrientation(m, targetPosition->frame);
+ FramedOrientationPtr targetOri = new FramedOrientation(m, targetPosition->frame, localRobot->getName());
VirtualRobot::LinkedCoordinate target = ArmarPose::createLinkedCoordinate(localRobot, targetPosition, targetOri);
Eigen::Matrix4f goalInRoot = target.getInFrame(localRobot->getRootNode());
Eigen::Vector3f targetPos = goalInRoot.block<3,1>(0,3);
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index 8babef4c2..10ee524d3 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -78,20 +78,38 @@ namespace armarx
localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
jointExistenceCheckRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
- /*std::string robotFile = getProperty<std::string>("RobotFileName").getValue();
- if (!ArmarXDataPath::getAbsolutePath(robotFile,robotFile))
+ localReportRobot = localRobot->clone("reportRobot");
+
+ std::string nodesetsString = getProperty<std::string>("TCPsToReport").getValue();
+ if(!nodesetsString.empty())
{
- throw UserException("Could not find robot file " + robotFile);
+ if(nodesetsString=="*")
+ {
+ auto nodesets = localReportRobot->getRobotNodeSets();
+ for(RobotNodeSetPtr& set : nodesets)
+ {
+ if(set->getTCP())
+ nodesToReport.push_back(set->getTCP());
+ }
+ }
+ else
+ {
+ std::vector<std::string> nodesetNames;
+ boost::split(nodesetNames,
+ nodesetsString,
+ boost::is_any_of(","),
+ boost::token_compress_on);
+ for(auto& name : nodesetNames)
+ {
+ auto node = localReportRobot->getRobotNode(name);
+ if(node)
+ nodesToReport.push_back(node);
+ else
+ ARMARX_ERROR << "Could not find node set with name: " << name;
+ }
+ }
}
- localRobot = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);//localRobot->clone("LocalRobot");
- 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++)
{
@@ -101,8 +119,6 @@ namespace armarx
listener = getTopic<TCPControlUnitListenerPrx>(topicName);
-// topicTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicReport,cycleTime, false, "TCPDataProvider");
-// topicTask->start();
requested = false;
if(execTask)
@@ -224,12 +240,9 @@ namespace armarx
void TCPControlUnit::periodicExec()
{
-// ARMARX_INFO << "SPAM";
-// ARMARX_INFO << deactivateSpam(4) << "periodicExec for " << tcpVelocitiesMap.size();
-//
- //double tDelta = (IceUtil::Time::now() - lastReportTime).toSecondsDouble();
+
lastReportTime = IceUtil::Time::now();
-// periodicReport(tDelta);
+
{
ScopedLock lock(dataMutex);
@@ -249,9 +262,7 @@ namespace armarx
}
RemoteRobot::synchronizeLocalClone(localRobot,robotStateComponentPrx);
- /* NameValueMap robotConfigMap = remoteRobotPrx->getConfig();
- std::map<std::string, float> jointValues(robotConfigMap.begin(), robotConfigMap.end());
- localRobot->setJointValues(jointValues);*/
+
calculationRunning = true;
}
@@ -269,67 +280,6 @@ namespace armarx
- void TCPControlUnit::periodicReport(double tDelta)
- {
- std::vector<RobotNodeSetPtr > nodeSets = localRobot->getRobotNodeSets();
- FramedPoseBaseMap tcpPoses;
- FramedVector3Map tcpTranslationVelocities;
- FramedVector3Map tcpOrientationVelocities;
- std::string rootFrame = localRobot->getRootNode()->getName();
-
- IceUtil::Time start = IceUtil::Time::now();
- for(unsigned int i=0; i < nodeSets.size(); i++)
- {
- RobotNodeSetPtr nodeSet = nodeSets.at(i);
- const std::string &tcpName = nodeSet->getTCP()->getName();
- const Eigen::Matrix4f ¤tPose = nodeSet->getTCP()->getGlobalPose();
- tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame);
- 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);
-
- const Eigen::Matrix3f rot = currentPose.block(0,0,3,3) * lastPose->toEigen().block(0,0,3,3).inverse();
- Eigen::AngleAxisf orient(rot.block<3,3>(0,0));
-// Eigen::Matrix3f currentRot = currentPose.block(0,0,3,3);
-// Eigen::AngleAxisf currentAA(currentRot);
-// Eigen::Quaternionf quat(currentAA);
-// debugObs->begin_setDebugDatafield(tcpName+ "rotation","w", new Variant(quat.w()));
-// debugObs->begin_setDebugDatafield(tcpName+ "rotation","x", new Variant(quat.x()));
-// debugObs->begin_setDebugDatafield(tcpName+ "rotation","y", new Variant(quat.y()));
-// debugObs->begin_setDebugDatafield(tcpName+ "rotation","z", new Variant(quat.z()));
-// debugObs->begin_setDebugDatafield(tcpName+ "rotation","w", new Variant(currentAA.angle()));
-// debugObs->begin_setDebugDatafield(tcpName+ "rotation","x", new Variant(currentAA.axis()[0]));
-// debugObs->begin_setDebugDatafield(tcpName+ "rotation","y", new Variant(currentAA.axis()[1]));
-// debugObs->begin_setDebugDatafield(tcpName+ "rotation","z", new Variant(currentAA.axis()[2]));
-// if(oriOffsets.find(tcpName) == oriOffsets.end())
-// oriOffsets[tcpName] = 0.0;
-// double& offset = oriOffsets[tcpName];
-// ContinuousAngles(Eigen::AngleAxisf(lastPose->toEigen().block<3,3>(0,0)),currentAA, offset);
-// Eigen::Quaternionf quatFixed(currentAA);
-
-// debugObs->begin_setDebugDatafield(tcpName+ "rotation","wFixed", new Variant(currentAA.angle()));
-// debugObs->begin_setDebugDatafield(tcpName+ "rotation","xFixed", new Variant(currentAA.axis()[0]));
-// debugObs->begin_setDebugDatafield(tcpName+ "rotation","yFixed", new Variant(currentAA.axis()[1]));
-// debugObs->begin_setDebugDatafield(tcpName+ "rotation","zFixed", new Variant(currentAA.axis()[2]));
-
-
-// debugObs->begin_setDebugDatafield(tcpName+ "rotation","wFixed", new Variant(quatFixed.w()));
-// debugObs->begin_setDebugDatafield(tcpName+ "rotation","xFixed", new Variant(quatFixed.x()));
-// debugObs->begin_setDebugDatafield(tcpName+ "rotation","yFixed", new Variant(quatFixed.y()));
-// debugObs->begin_setDebugDatafield(tcpName+ "rotation","zFixed", new Variant(quatFixed.z()));
- tcpOrientationVelocities[tcpName] = new FramedVector3(orient.axis() * (orient.angle()/tDelta), rootFrame);
- it->second = new FramedPose(currentPose, rootFrame);
- }
- }
- ARMARX_INFO << deactivateSpam(5) << "TCP Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
- listener->begin_reportTCPPose(tcpPoses);
- listener->begin_reportTCPVelocities(tcpTranslationVelocities, tcpOrientationVelocities);
- lastTCPPoses = tcpPoses;
-
- }
-
void TCPControlUnit::calcAndSetVelocities()
{
TCPVelocityDataMap::iterator it = localTcpVelocitiesMap.begin();
@@ -1137,18 +1087,16 @@ void armarx::TCPControlUnit::reportControlModeChanged(const NameControlModeMap &
void armarx::TCPControlUnit::reportJointAngles(const NameValueMap &jointAngles, bool, const Ice::Current &)
{
ScopedLock lock(reportMutex);
- std::vector<RobotNodeSetPtr > nodeSets = localReportRobot->getRobotNodeSets();
FramedPoseBaseMap tcpPoses;
-
std::string rootFrame = localReportRobot->getRootNode()->getName();
localReportRobot->setJointValues(jointAngles);
//IceUtil::Time start = IceUtil::Time::now();
- for(unsigned int i=0; i < nodeSets.size(); i++)
+ for(unsigned int i=0; i < nodesToReport.size(); i++)
{
- RobotNodeSetPtr nodeSet = nodeSets.at(i);
- const std::string &tcpName = nodeSet->getTCP()->getName();
- const Eigen::Matrix4f ¤tPose = nodeSet->getTCP()->getGlobalPose();
- tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame);
+ RobotNodePtr& node = nodesToReport.at(i);
+ const std::string &tcpName = node->getName();
+ const Eigen::Matrix4f ¤tPose = node->getGlobalPose();
+ tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, localReportRobot->getName());
}
// ARMARX_INFO << deactivateSpam(5) << "TCP Pose Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
@@ -1163,20 +1111,18 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel,
ScopedLock lock(reportMutex);
if(!localVelReportRobot)
localVelReportRobot = localReportRobot->clone("VelRobot");
-// ARMARX_INFO << jointVel;
- std::vector<RobotNodeSetPtr > nodeSets = localReportRobot->getRobotNodeSets();
- FramedPoseBaseMap tcpPoses;
+// ARMARX_INFO << jointVel; FramedPoseBaseMap tcpPoses;
FramedVector3Map tcpTranslationVelocities;
FramedVector3Map tcpOrientationVelocities;
std::string rootFrame = localReportRobot->getRootNode()->getName();
NameValueMap tempJointAngles = localReportRobot->getConfig()->getRobotNodeJointValueMap();
-
- for(unsigned int i=0; i < nodeSets.size(); i++)
+ FramedPoseBaseMap tcpPoses;
+ for(unsigned int i=0; i < nodesToReport.size(); i++)
{
- RobotNodeSetPtr nodeSet = nodeSets.at(i);
- const std::string &tcpName = nodeSet->getTCP()->getName();
- const Eigen::Matrix4f ¤tPose = nodeSet->getTCP()->getGlobalPose();
- tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame);
+ RobotNodePtr node = nodesToReport.at(i);
+ const std::string &tcpName = node->getName();
+ const Eigen::Matrix4f ¤tPose = node->getGlobalPose();
+ tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, localReportRobot->getName());
}
@@ -1188,50 +1134,23 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel,
it->second += itSrc->second * tDelta;
}
- nodeSets = localVelReportRobot->getRobotNodeSets();
localVelReportRobot->setJointValues(tempJointAngles);
//IceUtil::Time start = IceUtil::Time::now();
- for(unsigned int i=0; i < nodeSets.size(); i++)
+ for(unsigned int i=0; i < nodesToReport.size(); i++)
{
- RobotNodeSetPtr nodeSet = nodeSets.at(i);
- const std::string &tcpName = nodeSet->getTCP()->getName();
- const Eigen::Matrix4f ¤tPose = nodeSet->getTCP()->getGlobalPose();
+ RobotNodePtr node = nodesToReport.at(i);
+ const std::string &tcpName = node->getName();
+ const Eigen::Matrix4f ¤tPose = node->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);
+ tcpTranslationVelocities[tcpName] = new FramedVector3((currentPose.block(0,3,3,1) - lastPose->toEigen().block(0,3,3,1))/tDelta, rootFrame, localReportRobot->getName());
const Eigen::Matrix3f rot = currentPose.block(0,0,3,3) * lastPose->toEigen().block(0,0,3,3).inverse();
Eigen::AngleAxisf orient(rot.block<3,3>(0,0));
- // Eigen::Matrix3f currentRot = currentPose.block(0,0,3,3);
- // Eigen::AngleAxisf currentAA(currentRot);
- // Eigen::Quaternionf quat(currentAA);
- // debugObs->begin_setDebugDatafield(tcpName+ "rotation","w", new Variant(quat.w()));
- // debugObs->begin_setDebugDatafield(tcpName+ "rotation","x", new Variant(quat.x()));
- // debugObs->begin_setDebugDatafield(tcpName+ "rotation","y", new Variant(quat.y()));
- // debugObs->begin_setDebugDatafield(tcpName+ "rotation","z", new Variant(quat.z()));
- // debugObs->begin_setDebugDatafield(tcpName+ "rotation","w", new Variant(currentAA.angle()));
- // debugObs->begin_setDebugDatafield(tcpName+ "rotation","x", new Variant(currentAA.axis()[0]));
- // debugObs->begin_setDebugDatafield(tcpName+ "rotation","y", new Variant(currentAA.axis()[1]));
- // debugObs->begin_setDebugDatafield(tcpName+ "rotation","z", new Variant(currentAA.axis()[2]));
- // if(oriOffsets.find(tcpName) == oriOffsets.end())
- // oriOffsets[tcpName] = 0.0;
- // double& offset = oriOffsets[tcpName];
- // ContinuousAngles(Eigen::AngleAxisf(lastPose->toEigen().block<3,3>(0,0)),currentAA, offset);
- // Eigen::Quaternionf quatFixed(currentAA);
-
- // debugObs->begin_setDebugDatafield(tcpName+ "rotation","wFixed", new Variant(currentAA.angle()));
- // debugObs->begin_setDebugDatafield(tcpName+ "rotation","xFixed", new Variant(currentAA.axis()[0]));
- // debugObs->begin_setDebugDatafield(tcpName+ "rotation","yFixed", new Variant(currentAA.axis()[1]));
- // debugObs->begin_setDebugDatafield(tcpName+ "rotation","zFixed", new Variant(currentAA.axis()[2]));
-
-
- // debugObs->begin_setDebugDatafield(tcpName+ "rotation","wFixed", new Variant(quatFixed.w()));
- // debugObs->begin_setDebugDatafield(tcpName+ "rotation","xFixed", new Variant(quatFixed.x()));
- // debugObs->begin_setDebugDatafield(tcpName+ "rotation","yFixed", new Variant(quatFixed.y()));
- // debugObs->begin_setDebugDatafield(tcpName+ "rotation","zFixed", new Variant(quatFixed.z()));
- tcpOrientationVelocities[tcpName] = new FramedVector3(orient.axis() * (orient.angle()/tDelta), rootFrame);
+
+ tcpOrientationVelocities[tcpName] = new FramedVector3(orient.axis() * (orient.angle()/tDelta), rootFrame, localReportRobot->getName());
}
diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h
index 8c6037afe..94b1e1fb9 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.h
+++ b/source/RobotAPI/components/units/TCPControlUnit.h
@@ -47,6 +47,8 @@ namespace armarx {
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", 30, "Cycle time of the tcp control in ms");
+ defineOptionalProperty<std::string>("TCPsToReport", "", "comma seperated list of nodesets' endeffectors, which poses and velocities that should be reported. * for all, empty for none");
+
}
};
@@ -90,7 +92,7 @@ namespace armarx {
private:
static void ContinuousAngles(const Eigen::AngleAxisf &oldAngle, Eigen::AngleAxisf &newAngle, double & offset);
void periodicExec();
- void periodicReport(double tDelta);
+ 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);
@@ -133,6 +135,7 @@ namespace armarx {
std::map<std::string, double> oriOffsets;
+ std::vector<VirtualRobot::RobotNodePtr > nodesToReport;
FramedPoseBaseMap lastTCPPoses;
IceUtil::Time lastReportTime;
IceUtil::Time lastTopicReportTime;
@@ -142,6 +145,7 @@ namespace armarx {
Mutex taskMutex;
Mutex reportMutex;
bool calculationRunning;
+ double syncTimeDelta;
std::string topicName;
diff --git a/source/RobotAPI/gui_plugins/GraphVisualizerPlugin/GraphVisualizerGuiPlugin.cpp b/source/RobotAPI/gui_plugins/GraphVisualizerPlugin/GraphVisualizerGuiPlugin.cpp
index a802584c8..0339b3df0 100644
--- a/source/RobotAPI/gui_plugins/GraphVisualizerPlugin/GraphVisualizerGuiPlugin.cpp
+++ b/source/RobotAPI/gui_plugins/GraphVisualizerPlugin/GraphVisualizerGuiPlugin.cpp
@@ -544,15 +544,15 @@ void GraphVisualizerWidget::draw()
{
clearGraph();
- static ::armarx::FramedVector3Ptr tableS {new ::armarx::FramedVector3{Eigen::Vector3f{3400.f,7300.f,1000.f}, "table" }};
- static ::armarx::FramedVector3Ptr fridge {new ::armarx::FramedVector3{Eigen::Vector3f{2150.f,7750.f,1000.f}, "fridge" }};
- static ::armarx::FramedVector3Ptr sink {new ::armarx::FramedVector3{Eigen::Vector3f{2500.f,9700.f,1000.f}, "sink" }};
- static ::armarx::FramedVector3Ptr hub2fst{new ::armarx::FramedVector3{Eigen::Vector3f{3750.f,5150.f,5000.f}, "hub2" }};
- static ::armarx::FramedVector3Ptr hub2snd{new ::armarx::FramedVector3{Eigen::Vector3f{3750.f,5150.f,1000.f}, "hub2" }};
- static ::armarx::FramedVector3Ptr hub1 {new ::armarx::FramedVector3{Eigen::Vector3f{2900.f,8000.f,1000.f}, "hub1" }};
- static ::armarx::FramedVector3Ptr hub3 {new ::armarx::FramedVector3{Eigen::Vector3f{3400.f,2200.f,1000.f}, "hub3" }};
- static ::armarx::FramedVector3Ptr hub4 {new ::armarx::FramedVector3{Eigen::Vector3f{1900.f,3000.f,1000.f}, "hub4" }};
- static ::armarx::FramedVector3Ptr counter{new ::armarx::FramedVector3{Eigen::Vector3f{1890.f,4050.f,1000.f}, "counter"}};
+ static ::armarx::FramedVector3Ptr tableS {new ::armarx::FramedVector3{Eigen::Vector3f{3400.f,7300.f,1000.f}, "table" , "Armar3"}};
+ static ::armarx::FramedVector3Ptr fridge {new ::armarx::FramedVector3{Eigen::Vector3f{2150.f,7750.f,1000.f}, "fridge" , "Armar3"}};
+ static ::armarx::FramedVector3Ptr sink {new ::armarx::FramedVector3{Eigen::Vector3f{2500.f,9700.f,1000.f}, "sink" , "Armar3"}};
+ static ::armarx::FramedVector3Ptr hub2fst{new ::armarx::FramedVector3{Eigen::Vector3f{3750.f,5150.f,5000.f}, "hub2" , "Armar3"}};
+ static ::armarx::FramedVector3Ptr hub2snd{new ::armarx::FramedVector3{Eigen::Vector3f{3750.f,5150.f,1000.f}, "hub2" , "Armar3"}};
+ static ::armarx::FramedVector3Ptr hub1 {new ::armarx::FramedVector3{Eigen::Vector3f{2900.f,8000.f,1000.f}, "hub1" , "Armar3"}};
+ static ::armarx::FramedVector3Ptr hub3 {new ::armarx::FramedVector3{Eigen::Vector3f{3400.f,2200.f,1000.f}, "hub3" , "Armar3"}};
+ static ::armarx::FramedVector3Ptr hub4 {new ::armarx::FramedVector3{Eigen::Vector3f{1900.f,3000.f,1000.f}, "hub4" , "Armar3"}};
+ static ::armarx::FramedVector3Ptr counter{new ::armarx::FramedVector3{Eigen::Vector3f{1890.f,4050.f,1000.f}, "counter", "Armar3"}};
addNode(tableS);
addNode(fridge);
diff --git a/source/RobotAPI/gui_plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp b/source/RobotAPI/gui_plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp
index df6c4e205..c64f2bd97 100644
--- a/source/RobotAPI/gui_plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp
+++ b/source/RobotAPI/gui_plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp
@@ -302,11 +302,12 @@ void TCPMover::execMove()
Eigen::Vector3f vec;
vec << tcpData[selectedTCP][0], tcpData[selectedTCP][1], tcpData[selectedTCP][2];
vec *= ui.sbFactor->value();
- FramedVector3Ptr tcpVel = new FramedVector3(vec, refFrame);
+ const auto agentName = robotPrx->getName();
+ FramedVector3Ptr tcpVel = new FramedVector3(vec, refFrame, agentName);
Eigen::Vector3f vecOri;
vecOri << tcpData[selectedTCP].at(3)/180.f*3.145f, tcpData[selectedTCP].at(4)/180.f*3.145f, tcpData[selectedTCP].at(5)/180.f*3.145f;
vecOri *= ui.sbFactor->value();
- FramedVector3Ptr tcpOri = new FramedVector3(vecOri, refFrame);
+ FramedVector3Ptr tcpOri = new FramedVector3(vecOri, refFrame, agentName);
if(!ui.cbTranslation->isChecked() )
tcpVel = NULL;
diff --git a/source/RobotAPI/interface/robotstate/PoseBase.ice b/source/RobotAPI/interface/robotstate/PoseBase.ice
index f4f55512c..e6859a749 100644
--- a/source/RobotAPI/interface/robotstate/PoseBase.ice
+++ b/source/RobotAPI/interface/robotstate/PoseBase.ice
@@ -56,6 +56,7 @@ module armarx
class FramedVector3Base extends Vector3Base
{
string frame;
+ string agent;
};
@@ -63,18 +64,21 @@ module armarx
class FramedPoseBase extends PoseBase
{
string frame;
+ string agent;
};
["cpp:virtual"]
class FramedPositionBase extends Vector3Base
{
string frame;
+ string agent;
};
["cpp:virtual"]
class FramedOrientationBase extends QuaternionBase
{
string frame;
+ string agent;
};
dictionary<string, FramedVector3Base> FramedVector3Map;
diff --git a/source/RobotAPI/libraries/robotstate/RobotStateComponent.cpp b/source/RobotAPI/libraries/robotstate/RobotStateComponent.cpp
index 90b55b02a..68bd22d9d 100644
--- a/source/RobotAPI/libraries/robotstate/RobotStateComponent.cpp
+++ b/source/RobotAPI/libraries/robotstate/RobotStateComponent.cpp
@@ -63,7 +63,7 @@ namespace armarx
}
this->_synchronized = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
-
+ _synchronized->setName(getProperty<std::string>("AgentName").getValue());
if (this->_synchronized)
{
ARMARX_VERBOSE << "Loaded robot from file " << robotFile << ". Robot name: " << this->_synchronized->getName();
diff --git a/source/RobotAPI/libraries/robotstate/RobotStateComponent.h b/source/RobotAPI/libraries/robotstate/RobotStateComponent.h
index e73aee75a..1093b1324 100644
--- a/source/RobotAPI/libraries/robotstate/RobotStateComponent.h
+++ b/source/RobotAPI/libraries/robotstate/RobotStateComponent.h
@@ -47,6 +47,7 @@ namespace armarx
{
defineRequiredProperty<std::string>("RobotNodeSetName","Set of nodes that is controlled by the KinematicUnit");
defineRequiredProperty<std::string>("RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
+ defineRequiredProperty<std::string>("AgentName", "Name of the agent for which the sensor values are provided");
}
};
diff --git a/source/RobotAPI/libraries/robotstate/remote/ArmarPose.cpp b/source/RobotAPI/libraries/robotstate/remote/ArmarPose.cpp
index 6b9179891..c7e7ae0b4 100644
--- a/source/RobotAPI/libraries/robotstate/remote/ArmarPose.cpp
+++ b/source/RobotAPI/libraries/robotstate/remote/ArmarPose.cpp
@@ -316,10 +316,12 @@ namespace armarx
{
}
- FramedVector3::FramedVector3(const Eigen::Vector3f &vec, const string &frame) :
+ FramedVector3::FramedVector3(const Eigen::Vector3f &vec, const string &frame, const string &agent) :
Vector3(vec)
{
this->frame = frame;
+ this->agent = agent;
+
}
string FramedVector3::getFrame() const
@@ -349,7 +351,7 @@ namespace armarx
string FramedVector3::output(const Ice::Current &c) const
{
std::stringstream s;
- s << toEigen() << std::endl << "frame: " << getFrame();
+ s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty()?"": (" agent: " + agent));
return s.str();
}
@@ -397,6 +399,7 @@ namespace armarx
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
Vector3::serialize(serializer);
obj->setString("frame", frame);
+ obj->setString("agent", agent);
}
@@ -405,6 +408,7 @@ namespace armarx
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
Vector3::deserialize(serializer);
frame = obj->getString("frame");
+ agent = obj->getString("agent");
}
@@ -415,16 +419,18 @@ namespace armarx
frame = "";
}
- FramedPose::FramedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& s) :
+ FramedPose::FramedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& s, const string &agent) :
Pose(m, v)
{
frame = s;
+ this->agent = agent;
}
- FramedPose::FramedPose(const Eigen::Matrix4f& m, const std::string& s) :
+ FramedPose::FramedPose(const Eigen::Matrix4f& m, const std::string& s, const string &agent) :
Pose(m)
{
frame = s;
+ this->agent = agent;
}
std::string FramedPose::getFrame() const
@@ -435,7 +441,7 @@ namespace armarx
string FramedPose::output(const Ice::Current &c) const
{
std::stringstream s;
- s << toEigen() << std::endl << "frame: " << getFrame();
+ s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty()?"": (" agent: " + agent));
return s.str();
}
@@ -452,8 +458,8 @@ namespace armarx
{
if(newFrame == frame)
return;
- FramedPositionPtr pos = new FramedPosition(Vector3Ptr::dynamicCast(position)->toEigen(), frame);
- auto ori = new FramedOrientation(QuaternionPtr::dynamicCast(orientation)->toEigen(), frame);
+ FramedPositionPtr pos = new FramedPosition(Vector3Ptr::dynamicCast(position)->toEigen(), frame, agent);
+ auto ori = new FramedOrientation(QuaternionPtr::dynamicCast(orientation)->toEigen(), frame, agent);
auto coord = ArmarPose::createLinkedCoordinate(referenceRobot, pos, ori);
coord.changeFrame(newFrame);
orientation = new Quaternion(coord.getPose());
@@ -486,6 +492,7 @@ namespace armarx
Pose::serialize(obj, c);
obj->setString("frame", frame);
+ obj->setString("agent", agent);
}
void FramedPose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
@@ -494,6 +501,7 @@ namespace armarx
Pose::deserialize(obj);
frame = obj->getString("frame");
+ agent = obj->getString("agent");
}
@@ -503,16 +511,18 @@ namespace armarx
frame = "";
}
- FramedPosition::FramedPosition(const Eigen::Vector3f &v, const std::string &s) :
+ FramedPosition::FramedPosition(const Eigen::Vector3f &v, const std::string &s, const string &agent) :
Vector3(v)
{
frame = s;
+ this->agent = agent;
}
- FramedPosition::FramedPosition(const Eigen::Matrix4f &m, const std::string &s) :
+ FramedPosition::FramedPosition(const Eigen::Matrix4f &m, const std::string &s, const string &agent) :
Vector3(m)
{
frame = s;
+ this->agent = agent;
}
// this doesnt work for unknown reasons
@@ -531,7 +541,11 @@ namespace armarx
void FramedPosition::changeFrame(const SharedRobotInterfacePrx &referenceRobot, const string &newFrame)
{
+ if(newFrame == frame)
+ return;
+ VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+ changeFrame(sharedRobot, newFrame);
}
void FramedPosition::changeFrame(VirtualRobot::RobotPtr &referenceRobot, const string &newFrame)
@@ -551,7 +565,7 @@ namespace armarx
string FramedPosition::output(const Ice::Current &c) const
{
std::stringstream s;
- s << toEigen() << std::endl << "frame: " << getFrame();
+ s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty()?"": (" agent: " + agent));
return s.str();
}
@@ -564,6 +578,7 @@ namespace armarx
Vector3::readFromXML(xmlData);
frame = (pt.get<std::string>("frame"));
+ agent = (pt.get<std::string>("agent"));
return 1;
}
@@ -573,7 +588,8 @@ namespace armarx
using namespace boost::property_tree;
std::stringstream stream;
stream << Vector3::writeAsXML() <<
- "<frame>"<<frame<<"</frame>";
+ "<frame>"<<frame<<"</frame>"<<
+ "<agent>"<<agent<<"</agent>";
return stream.str();
}
@@ -583,6 +599,7 @@ namespace armarx
Vector3::serialize(obj, c);
obj->setString("frame", frame);
+ obj->setString("agent", agent);
}
void FramedPosition::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
@@ -591,6 +608,7 @@ namespace armarx
Vector3::deserialize(obj);
frame = obj->getString("frame");
+ agent = obj->getString("agent");
}
@@ -600,16 +618,18 @@ namespace armarx
frame = "";
}
- FramedOrientation::FramedOrientation(const Eigen::Matrix3f &m, const std::string &s) :
+ FramedOrientation::FramedOrientation(const Eigen::Matrix3f &m, const std::string &s, const string &agent) :
Quaternion(m)
{
frame = s;
+ this->agent = agent;
}
- FramedOrientation::FramedOrientation(const Eigen::Matrix4f &m, const std::string &s) :
+ FramedOrientation::FramedOrientation(const Eigen::Matrix4f &m, const std::string &s, const std::string &agent) :
Quaternion(m)
{
frame = s;
+ this->agent = agent;
}
// this doesnt work for an unknown reason
@@ -628,7 +648,7 @@ namespace armarx
string FramedOrientation::output(const Ice::Current &c) const
{
std::stringstream s;
- s << toEigen() << std::endl << "frame: " << getFrame();
+ s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty()?"": (" agent: " + agent));
return s.str();
}
@@ -669,6 +689,7 @@ namespace armarx
Quaternion::readFromXML(xmlData);
frame = (pt.get<std::string>("frame"));
+ agent = (pt.get<std::string>("agent"));
return 1;
}
@@ -678,7 +699,8 @@ namespace armarx
using namespace boost::property_tree;
std::stringstream stream;
stream << Quaternion::writeAsXML() <<
- "<frame>" << frame << "</frame>";
+ "<frame>" << frame << "</frame>" <<
+ "<agent>" << agent << "</agent>";
return stream.str();
}
@@ -688,6 +710,7 @@ namespace armarx
Quaternion::serialize(obj, c);
obj->setString("frame", frame);
+ obj->setString("agent", agent);
}
void FramedOrientation::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
@@ -696,6 +719,7 @@ namespace armarx
Quaternion::deserialize(obj);
frame = obj->getString("frame");
+ agent = obj->getString("agent");
}
namespace ArmarPose
diff --git a/source/RobotAPI/libraries/robotstate/remote/ArmarPose.h b/source/RobotAPI/libraries/robotstate/remote/ArmarPose.h
index dc2d8d67d..4754fefd8 100644
--- a/source/RobotAPI/libraries/robotstate/remote/ArmarPose.h
+++ b/source/RobotAPI/libraries/robotstate/remote/ArmarPose.h
@@ -55,6 +55,7 @@ namespace armarx
const VariantTypeId FramedOrientation = Variant::addTypeName("::armarx::FramedOrientationBase");
}
+ std::string const GlobalFrame = "Global";
/**
* @class Vector3
@@ -287,8 +288,8 @@ namespace armarx
{
public:
FramedPose();
- FramedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& frame);
- FramedPose(const Eigen::Matrix4f& m, const std::string& frame);
+ FramedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& frame, const std::string& agent);
+ FramedPose(const Eigen::Matrix4f& m, const std::string& frame, const std::string& agent);
std::string getFrame() const;
@@ -339,6 +340,7 @@ namespace armarx
return stream;
};
+
public:
virtual void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const;
virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
@@ -369,7 +371,7 @@ namespace armarx
public:
FramedVector3();
FramedVector3(const FramedVector3& source);
- FramedVector3(const Eigen::Vector3f & vec, const std::string &frame );
+ FramedVector3(const Eigen::Vector3f & vec, const std::string &frame, const std::string& agent );
std::string getFrame() const;
static FramedVector3Ptr ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedVector3& framedVec, const std::string &newFrame);
@@ -437,8 +439,8 @@ namespace armarx
{
public:
FramedPosition( );
- FramedPosition(const Eigen::Vector3f &, const std::string &frame );
- FramedPosition(const Eigen::Matrix4f &, const std::string &frame );
+ FramedPosition(const Eigen::Vector3f &, const std::string &frame, const std::string &agent);
+ FramedPosition(const Eigen::Matrix4f &, const std::string &frame, const std::string &agent);
//FramedPosition(const Vector3BasePtr pos, const std::string &frame ); // this doesnt work for unknown reasons
std::string getFrame() const;
@@ -506,8 +508,8 @@ namespace armarx
{
public:
FramedOrientation();
- FramedOrientation(const Eigen::Matrix4f &, const std::string &frame );
- FramedOrientation(const Eigen::Matrix3f &, const std::string &frame );
+ FramedOrientation(const Eigen::Matrix4f &, const std::string &frame, const std::string &agent );
+ FramedOrientation(const Eigen::Matrix3f &, const std::string &frame, const std::string &agent );
// this doesnt work for an unknown reason
//FramedOrientation(const QuaternionBasePtr ori, const std::string &frame );
std::string getFrame()const ;
diff --git a/source/RobotAPI/libraries/robotstate/remote/LinkedPose.cpp b/source/RobotAPI/libraries/robotstate/remote/LinkedPose.cpp
index f3c821354..79d362794 100644
--- a/source/RobotAPI/libraries/robotstate/remote/LinkedPose.cpp
+++ b/source/RobotAPI/libraries/robotstate/remote/LinkedPose.cpp
@@ -42,7 +42,7 @@ namespace armarx {
LinkedPose::LinkedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& s, const SharedRobotInterfacePrx& referenceRobot) :
Pose(m, v),
- FramedPose(m, v, s)
+ FramedPose(m, v, s, referenceRobot->getName())
{
referenceRobot->ref();
this->referenceRobot = referenceRobot;
@@ -50,7 +50,7 @@ namespace armarx {
LinkedPose::LinkedPose(const Eigen::Matrix4f& m, const std::string& s, const SharedRobotInterfacePrx& referenceRobot) :
Pose(m),
- FramedPose(m, s)
+ FramedPose(m, s, referenceRobot->getName())
{
referenceRobot->ref();
this->referenceRobot = referenceRobot;
@@ -223,7 +223,7 @@ namespace armarx {
}
LinkedVector3::LinkedVector3(const Eigen::Vector3f &v, const std::string &frame, const SharedRobotInterfacePrx &referenceRobot) :
- FramedVector3(v, frame)
+ FramedVector3(v, frame, referenceRobot->getName())
{
referenceRobot->ref();
this->referenceRobot = referenceRobot;
@@ -280,7 +280,47 @@ namespace armarx {
}
+#if ICE_INT_VERSION >= 30500
+ void LinkedVector3::__read(IceInternal::BasicStream *__is)
+ {
+ LinkedVector3Base::__read(__is);
+ if(referenceRobot)
+ {
+ //ARMARX_WARNING_S << "Calling referenceRobot->ref() in __read(IceInternal::BasicStream *__is, bool __rid) of LinkedPose";
+ referenceRobot->ref();
+ }
+ }
+
+ void LinkedVector3::__read(const Ice::InputStreamPtr &__is)
+ {
+ LinkedVector3Base::__read(__is);
+ if(referenceRobot)
+ {
+ //ARMARX_WARNING_S << "Calling referenceRobot->ref() in __read(const Ice::InputStreamPtr &__is, bool __rid) of LinkedPose";
+ referenceRobot->ref();
+ }
+ }
+#else
+ void LinkedVector3::__read(IceInternal::BasicStream *__is, bool __rid)
+ {
+ LinkedVector3Base::__read(__is, __rid);
+ if(referenceRobot)
+ {
+ //ARMARX_WARNING_S << "Calling referenceRobot->ref() in __read(IceInternal::BasicStream *__is, bool __rid) of LinkedPose";
+ referenceRobot->ref();
+ }
+ }
+ void LinkedVector3::__read(const Ice::InputStreamPtr &__is, bool __rid)
+ {
+ LinkedVector3Base::__read(__is, __rid);
+ if(referenceRobot)
+ {
+ //ARMARX_WARNING_S << "Calling referenceRobot->ref() in __read(const Ice::InputStreamPtr &__is, bool __rid) of LinkedPose";
+ referenceRobot->ref();
+ }
+ }
+#endif
}
diff --git a/source/RobotAPI/libraries/robotstate/remote/LinkedPose.h b/source/RobotAPI/libraries/robotstate/remote/LinkedPose.h
index ce13e5c47..9acccdac2 100644
--- a/source/RobotAPI/libraries/robotstate/remote/LinkedPose.h
+++ b/source/RobotAPI/libraries/robotstate/remote/LinkedPose.h
@@ -198,6 +198,14 @@ namespace armarx
virtual void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const;
virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
+#if ICE_INT_VERSION >= 30500
+ virtual void __read(::IceInternal::BasicStream *__is);
+ virtual void __read(const ::Ice::InputStreamPtr &__is);
+#else
+ virtual void __read(::IceInternal::BasicStream *__is, bool __rid);
+ virtual void __read(const ::Ice::InputStreamPtr &__is, bool __rid);
+#endif
+
};
typedef IceInternal::Handle<LinkedVector3> LinkedVector3Ptr;
}
--
GitLab