From 6f4364ad07bf87ef6d55596a87b46a343f040a7e Mon Sep 17 00:00:00 2001
From: Nikolaus Vahrenkamp <vahrenkamp@kit.edu>
Date: Mon, 18 Aug 2014 11:51:53 +0200
Subject: [PATCH] Added headIK to RobotContext (optional) Changed headIK, so
that the remoteRobot is cloned to get a local robot (instead of loading a
complete robot instance) Worked on ForceTorqueUnit Added mutex to
RemoteRobot->clone
---
.../RobotAPI/core/RobotStatechartContext.cpp | 11 ++++++++++
source/RobotAPI/core/RobotStatechartContext.h | 9 ++++++++-
.../robotstate/remote/RemoteRobot.cpp | 5 +++++
.../RobotAPI/robotstate/remote/RemoteRobot.h | 3 +++
.../statecharts/MovePlatform/MovePlatform.cpp | 14 ++++++++++---
source/RobotAPI/units/ForceTorqueUnit.cpp | 1 +
source/RobotAPI/units/HeadIKUnit.cpp | 20 ++++++++++---------
source/RobotAPI/units/HeadIKUnit.h | 2 +-
8 files changed, 51 insertions(+), 14 deletions(-)
diff --git a/source/RobotAPI/core/RobotStatechartContext.cpp b/source/RobotAPI/core/RobotStatechartContext.cpp
index be7c3ac66..b2b22deb8 100644
--- a/source/RobotAPI/core/RobotStatechartContext.cpp
+++ b/source/RobotAPI/core/RobotStatechartContext.cpp
@@ -59,6 +59,12 @@ namespace armarx
usingProxy(handUnitList.at(i));
}
}
+
+ // headIKUnit
+ headIKUnitName = getProperty<std::string>("HeadIKUnitName").getValue();
+ headIKKinematicChainName = getProperty<std::string>("HeadIKKinematicChainName").getValue();
+ if (!headIKUnitName.empty())
+ usingProxy(headIKUnitName);
}
@@ -76,6 +82,11 @@ namespace armarx
ARMARX_LOG << eINFO << "Fetched proxies " << kinUnitName << ":" << kinematicUnitPrx << ", " << rbStateName << ": " << robotStateComponent << flush;
+ if (!headIKUnitName.empty())
+ {
+ headIKUnitPrx = getProxy<HeadIKUnitInterfacePrx>(headIKUnitName);
+ ARMARX_LOG << eINFO << "Fetched headIK proxy " << headIKUnitName << ":" << headIKUnitPrx << ", head IK kin chain:" << headIKKinematicChainName << flush;
+ }
if( !getProperty<std::string>("HandUnits").getValue().empty())
diff --git a/source/RobotAPI/core/RobotStatechartContext.h b/source/RobotAPI/core/RobotStatechartContext.h
index f6925bc9d..dfb89b417 100644
--- a/source/RobotAPI/core/RobotStatechartContext.h
+++ b/source/RobotAPI/core/RobotStatechartContext.h
@@ -33,6 +33,7 @@
#include <RobotAPI/interface/units/HandUnitInterface.h>
#include <RobotAPI/interface/units/TCPControlUnit.h>
+#include <RobotAPI/interface/units/HeadIKUnit.h>
#include <RobotAPI/units/KinematicUnitObserver.h>
//#include <VirtualRobot/VirtualRobot.h>
@@ -54,6 +55,8 @@ namespace armarx
defineRequiredProperty<std::string>("KinematicUnitObserverName", "Name of the kinematic unit observer that should be used");
//HandUnits should only be changed via config file and default parameter should remain empty
defineOptionalProperty<std::string>("HandUnits", "", "Name of the comma-seperated hand units that should be used. Unitname for left hand should be LeftHandUnit, and for right hand RightHandUnit");
+ defineOptionalProperty<std::string>("HeadIKUnitName", "", "Name of the head unit that should be used.");
+ defineOptionalProperty<std::string>("HeadIKKinematicChainName", "", "Name of the inematic chain that should be used for head IK.");
}
};
@@ -92,9 +95,13 @@ namespace armarx
VirtualRobot::RobotPtr remoteRobot;
std::map<std::string, HandUnitInterfacePrx> handUnits;
+ HeadIKUnitInterfacePrx headIKUnitPrx;
+ std::string headIKKinematicChainName;
+
private:
std::string kinematicUnitObserverName;
- };
+ std::string headIKUnitName;
+ };
}
#endif
diff --git a/source/RobotAPI/robotstate/remote/RemoteRobot.cpp b/source/RobotAPI/robotstate/remote/RemoteRobot.cpp
index 53302b488..dc2be4c3c 100644
--- a/source/RobotAPI/robotstate/remote/RemoteRobot.cpp
+++ b/source/RobotAPI/robotstate/remote/RemoteRobot.cpp
@@ -19,6 +19,8 @@ using namespace Eigen;
namespace armarx{
+boost::recursive_mutex RemoteRobot::m;
+
RemoteRobot::RemoteRobot(SharedRobotInterfacePrx robot) :
Robot(),
_robot(robot)
@@ -142,6 +144,7 @@ string RemoteRobot::getName()
VirtualRobot::RobotNodePtr RemoteRobot::createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr>& allNodes, std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> > &childrenMap, RobotPtr robo)
{
+ boost::recursive_mutex::scoped_lock cloneLock(m);
static int nonameCounter = 0;
if (!remoteNode || !robo)
{
@@ -252,6 +255,7 @@ VirtualRobot::RobotNodePtr RemoteRobot::createLocalNode(SharedRobotNodeInterface
VirtualRobot::RobotPtr RemoteRobot::createLocalClone()
{
+ boost::recursive_mutex::scoped_lock cloneLock(m);
std::string robotType = getName();
std::string robotName = getName();
VirtualRobot::RobotPtr robo(new VirtualRobot::LocalRobot(robotName,robotType));
@@ -285,6 +289,7 @@ VirtualRobot::RobotPtr RemoteRobot::createLocalClone()
VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const string &filename)
{
+ boost::recursive_mutex::scoped_lock cloneLock(m);
ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")" << endl;
VirtualRobot::RobotPtr result;
diff --git a/source/RobotAPI/robotstate/remote/RemoteRobot.h b/source/RobotAPI/robotstate/remote/RemoteRobot.h
index b0b634110..a21845b8d 100644
--- a/source/RobotAPI/robotstate/remote/RemoteRobot.h
+++ b/source/RobotAPI/robotstate/remote/RemoteRobot.h
@@ -38,6 +38,7 @@
#include <RobotAPI/interface/robotstate/RobotState.h>
// boost
+#include <boost/thread/mutex.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_base_of.hpp>
@@ -223,6 +224,8 @@ namespace armarx
std::map<std::string, VirtualRobot::RobotNodePtr> _cachedNodes;
VirtualRobot::RobotNodePtr _root;
+ static boost::recursive_mutex m;
+
static VirtualRobot::RobotNodePtr createRemoteRobotNode(SharedRobotNodeInterfacePrx, VirtualRobot::RobotPtr);
};
diff --git a/source/RobotAPI/statecharts/MovePlatform/MovePlatform.cpp b/source/RobotAPI/statecharts/MovePlatform/MovePlatform.cpp
index b70d3f73a..17104797f 100644
--- a/source/RobotAPI/statecharts/MovePlatform/MovePlatform.cpp
+++ b/source/RobotAPI/statecharts/MovePlatform/MovePlatform.cpp
@@ -111,14 +111,22 @@ namespace armarx
void StateMoveToNext::onEnter()
{
- ARMARX_LOG << eVERBOSE << "Entering StateMoveToNext::onEnter";
+ ARMARX_LOG << eVERBOSE << "Entering StateMoveToNext::onEnter" << flush;
PlatformContext* context = getContext<PlatformContext>();
ChannelRefPtr counter = getInput<ChannelRef>("positionCounter");
int positionIndex = counter->getDataField("value")->getInt();
-
+ ARMARX_DEBUG << "Entering positionIndex:" << positionIndex << flush;
SingleTypeVariantListPtr points = getInput<SingleTypeVariantList>("targetPositions");
+ ARMARX_DEBUG << "points->getSize:" << points->getSize() << flush;
if (positionIndex < points->getSize()) {
- Vector3Ptr currentTarget = getInput<SingleTypeVariantList>("targetPositions")->getVariant(positionIndex)->get<Vector3>();
+ SingleTypeVariantListPtr list = getInput<SingleTypeVariantList>("targetPositions");
+
+ ARMARX_DEBUG << "list->getSize:" << list->getSize() << flush;
+ VariantPtr v = list->getVariant(positionIndex);
+ ARMARX_DEBUG << "v->getTypeName():" << v->getTypeName() << flush;
+
+ Vector3Ptr currentTarget = v->get<Vector3>();
+ ARMARX_DEBUG << "currentTarget:" << currentTarget->toEigen().transpose() << flush;
float positionalAccuracy = getInput<float>("positionalAccuracy");
float orientationalAccuracy = getInput<float>("orientationalAccuracy");
context->platformUnitPrx->moveTo(currentTarget->x, currentTarget->y, currentTarget->z, positionalAccuracy, orientationalAccuracy);
diff --git a/source/RobotAPI/units/ForceTorqueUnit.cpp b/source/RobotAPI/units/ForceTorqueUnit.cpp
index 6f6c4fb80..b5b4822ee 100644
--- a/source/RobotAPI/units/ForceTorqueUnit.cpp
+++ b/source/RobotAPI/units/ForceTorqueUnit.cpp
@@ -34,6 +34,7 @@ void ForceTorqueUnit::onInitComponent()
void ForceTorqueUnit::onConnectComponent()
{
+ ARMARX_INFO << "setting report topic to " << listenerName << flush;
listenerPrx = getTopic<ForceTorqueUnitListenerPrx>(listenerName);
onStartForceTorqueUnit();
}
diff --git a/source/RobotAPI/units/HeadIKUnit.cpp b/source/RobotAPI/units/HeadIKUnit.cpp
index 3c56c9afb..89f8975ae 100644
--- a/source/RobotAPI/units/HeadIKUnit.cpp
+++ b/source/RobotAPI/units/HeadIKUnit.cpp
@@ -38,14 +38,15 @@ namespace armarx
kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
- remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
+ //remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
+ localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
- // TODO!
- std::string robotModelFile;
- ArmarXDataPath::getAbsolutePath("Armar3/data/robotmodel/ArmarIII.xml", robotModelFile);
- localRobot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str(), VirtualRobot::RobotIO::eStructure);
- VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx));
- localRobot->setConfig(robotSnapshot->getConfig());
+
+ //std::string robotModelFile;
+ //ArmarXDataPath::getAbsolutePath("Armar3/data/robotmodel/ArmarIII.xml", robotModelFile);
+ //localRobot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str(), VirtualRobot::RobotIO::eStructure);
+ //VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx));
+ //localRobot->setConfig(robotSnapshot->getConfig());
requested = false;
if (execTask) execTask->stop();
@@ -166,8 +167,9 @@ namespace armarx
if (doCalculation)
{
- VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx));
- localRobot->setConfig(robotSnapshot->getConfig());
+ RemoteRobot::synchronizeLocalClone(localRobot,robotStateComponentPrx);
+ //VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx));
+ //localRobot->setConfig(robotSnapshot->getConfig());
VirtualRobot::RobotNodeSetPtr kinematicChain = localRobot->getRobotNodeSet(robotNodeSetName);
VirtualRobot::RobotNodePrismaticPtr virtualJoint;
diff --git a/source/RobotAPI/units/HeadIKUnit.h b/source/RobotAPI/units/HeadIKUnit.h
index 186218831..8b09ca71e 100644
--- a/source/RobotAPI/units/HeadIKUnit.h
+++ b/source/RobotAPI/units/HeadIKUnit.h
@@ -87,7 +87,7 @@ namespace armarx
RobotStateComponentInterfacePrx robotStateComponentPrx;
KinematicUnitInterfacePrx kinematicUnitPrx;
- SharedRobotInterfacePrx remoteRobotPrx;
+ //SharedRobotInterfacePrx remoteRobotPrx;
VirtualRobot::RobotPtr localRobot;
std::string robotNodeSetName;
--
GitLab