diff --git a/source/RobotAPI/core/RobotStatechartContext.cpp b/source/RobotAPI/core/RobotStatechartContext.cpp
index be7c3ac667d15cd60081ec0e71eace4fffc03b07..b2b22deb812204bb50ae98070b12bbbc7b01068d 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 f6925bc9df6664db38cc162c259757db7a56ad6a..dfb89b41710459a7aad22955aa5e957f370fcf97 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/RobotStateComponent.h b/source/RobotAPI/robotstate/RobotStateComponent.h
index 11c4128aa387f64d63029daa97e621fc06cfc3ed..d4dafacca64b65902fa00f4007275fcca091598e 100644
--- a/source/RobotAPI/robotstate/RobotStateComponent.h
+++ b/source/RobotAPI/robotstate/RobotStateComponent.h
@@ -61,6 +61,7 @@ namespace armarx
* While the synchronized robot will constantly update its internal state
* the robot snapshot is a clone of the original robot an won't update its
* configuration over time.
+ * See \ref remoterobot for more details and the usage of this component.
*/
class ARMARXCOMPONENT_IMPORT_EXPORT RobotStateComponent :
virtual public Component,
diff --git a/source/RobotAPI/robotstate/SharedRobotServants.h b/source/RobotAPI/robotstate/SharedRobotServants.h
index 89a063b4649f3f48caaede8487210235810c7b4b..c0c78cf0221a72f24417280e8151aeccf76da0a1 100644
--- a/source/RobotAPI/robotstate/SharedRobotServants.h
+++ b/source/RobotAPI/robotstate/SharedRobotServants.h
@@ -16,6 +16,10 @@
namespace armarx {
// Zugriff muss mutex geschuetzt werden!
+ /**
+ * @brief The base class for remote objects on the ice server
+ *
+ */
class SharedObjectBase :
virtual public SharedObjectInterface
@@ -30,6 +34,13 @@ namespace armarx {
boost::mutex _counterMutex;
};
+ /**
+ * @brief The SharedRobotNodeServant class is a remote represenation of a Simox VirtualRobot::Robot
+ *
+ * @details This class is used only internally by the the RobotStateComponent. Other classes such as the LinkedPose, RemoteRobot,
+ * TCPControlUnit and HeadIKUnit classes address this class by the SharedRobotNodeInterface and SharedRobotNodeInterfacePrx generated by
+ * ICE.
+ */
class SharedRobotNodeServant :
virtual public SharedRobotNodeInterface,
public SharedObjectBase
@@ -65,6 +76,12 @@ namespace armarx {
VirtualRobot::RobotNodePtr _node;
};
+ /**
+ * @brief The SharedRobotNodeServant class is a remote represenation of a Simox VirtualRobot::Robot
+ *
+ * @details This class is used only internally by the the RobotStateComponent. The RemoteRobot class SharedRobotInterface and SharedRobotInterfacePrx
+ * classes generated by ICE.
+ */
class SharedRobotServant :
public virtual SharedRobotInterface,
diff --git a/source/RobotAPI/robotstate/remote/RemoteRobot.cpp b/source/RobotAPI/robotstate/remote/RemoteRobot.cpp
index 53302b488e0d5371e370134fa7902bdc1ac64d06..dc2be4c3c2621b3d607e747b1eb92fd7312c22e6 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 b0b6341107b10cd81c79bba011671a12bb235ae1..a21845b8db105d41f0abd7456ac10be9ba0bdc48 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/CMakeLists.txt b/source/RobotAPI/statecharts/CMakeLists.txt
index bcb3ae3b2041a18fbf5f7d1571e4b8669caa8899..b5fcf48dbf26bc0207dc0e82ad12909292eca793 100644
--- a/source/RobotAPI/statecharts/CMakeLists.txt
+++ b/source/RobotAPI/statecharts/CMakeLists.txt
@@ -2,3 +2,4 @@ add_subdirectory(GraspingWithTorques)
add_subdirectory(OpenHand)
add_subdirectory(MovePlatform)
add_subdirectory(MovePlatformToLandmark)
+add_subdirectory(PlaceObject)
diff --git a/source/RobotAPI/statecharts/MovePlatform/MovePlatform.cpp b/source/RobotAPI/statecharts/MovePlatform/MovePlatform.cpp
index b70d3f73a0f39d16ec58d4d0ad65297c0ebf80d0..17104797fada56e243bb004c59752defffb625cd 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/statecharts/PlaceObject/CMakeLists.txt b/source/RobotAPI/statecharts/PlaceObject/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6a81c9bd881778d0ef244aefbbedcfb29f4a25da
--- /dev/null
+++ b/source/RobotAPI/statecharts/PlaceObject/CMakeLists.txt
@@ -0,0 +1,26 @@
+armarx_set_target("RobotAPI Library: PlaceObject")
+
+find_package(Eigen3 QUIET)
+find_package(Simox QUIET)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+if (Eigen3_FOUND AND Simox_FOUND)
+ include_directories(
+ ${Eigen3_INCLUDE_DIR}
+ ${Simox_INCLUDE_DIRS})
+endif()
+
+set(LIB_NAME PlaceObject)
+set(LIB_VERSION 0.1.0)
+set(LIB_SOVERSION 0)
+
+set(LIBS RobotAPICore RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers)
+
+set(LIB_FILES PlaceObject.cpp
+ )
+set(LIB_HEADERS PlaceObject.h
+ )
+
+armarx_add_library("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/statecharts/PlaceObject/PlaceObject.cpp b/source/RobotAPI/statecharts/PlaceObject/PlaceObject.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b0c5867a9fb07a95a068de2b85708b279ab41627
--- /dev/null
+++ b/source/RobotAPI/statecharts/PlaceObject/PlaceObject.cpp
@@ -0,0 +1,318 @@
+/*
+* 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 RobotAPI::PlaceObject
+* @author Nikolaus Vahrenkamp
+* @date 2014
+
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#include "PlaceObject.h"
+#include "../../core/RobotStatechartContext.h"
+
+#include <Core/observers/variant/ChannelRef.h>
+#include <Core/observers/variant/SingleTypeVariantList.h>
+#include <RobotAPI/robotstate/remote/ArmarPose.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+
+#include <RobotAPI/core/RobotStatechartContext.h>
+
+namespace armarx
+{
+ // ****************************************************************
+ // Implementation of StatechartPlaceObject
+ // ****************************************************************
+ void StatechartPlaceObject::defineParameters()
+ {
+ addToInput("maxHeightToMoveDown", VariantType::List(VariantType::Float), false);
+ addToInput("minForceForSuccess", VariantType::List(VariantType::Float), false);
+ addToInput("timeoutPlaceObject", VariantType::Float, false);
+
+ addToInput("robotNodeSetName", VariantType::String, false);
+ addToInput("robotBaseFrame", VariantType::String, false);
+
+
+ addToLocal("startPose", VariantType::List(VariantType::FramedPose));
+ addToLocal("goalPose", VariantType::List(VariantType::FramedPose));
+
+ //addToLocal("jointNames", VariantType::List(VariantType::String));
+ //addToLocal("jointVelocitiesDatafields", VariantType::List(VariantType::DatafieldRef));
+ }
+
+ void StatechartPlaceObject::defineSubstates()
+ {
+ StatePtr stateMoveDown= addState<StateMoveDown>("stateMoveDown");
+ //StatePtr stateCheckForces = addState<StateCheckForces>("stateCheckForces");
+ StatePtr stateSuccess = addState<SuccessState>("stateSuccess");
+ StatePtr stateFailure = addState<FailureState>("stateFailure");
+
+ ParameterMappingPtr mapMoveDown = ParameterMapping::createMapping()
+ ->mapFromParent("*", "*");
+ //ParameterMappingPtr mapCheckForces = ParameterMapping::createMapping()
+ // ->mapFromParent("*", "*");
+
+ setInitState(stateMoveDown, mapMoveDown);
+
+ //transitions
+ addTransition<EvSuccess>(stateMoveDown, stateMoveDown);
+ addTransition<EvFailure>(stateMoveDown, stateFailure);
+ //addTransition<EvSuccess>(stateCheckForces, stateSuccess);
+ //addTransition<EvFailure>(stateCheckForces, stateMoveDown);
+
+ addTransition<EvPlaceObjectCollision>(stateMoveDown, stateSuccess);
+ //addTransition<EvPlaceObjectTimeout>(stateCheckForces, stateFailure);
+
+ }
+
+ void StatechartPlaceObject::onEnter()
+ {
+ ARMARX_DEBUG << "Entering StatechartPlaceObject";
+
+ RobotStatechartContext* context = getContext<RobotStatechartContext>();
+ //setLocal("jointVelocityChannel", context->getChannelRef(context->getKinematicUnitObserverName(),"jointvelocities"));
+ //ChannelRefPtr tempChannelRef = context->getChannelRef(context->getKinematicUnitObserverName(),"jointvelocities");
+
+ std::string rnsName = getInput<std::string>("robotNodeSetName");
+ std::string rnBaseName = getInput<std::string>("robotBaseFrame");
+ VirtualRobot::RobotNodeSetPtr nodeSet = context->remoteRobot->getRobotNodeSet(rnsName);
+ if (!nodeSet)
+ {
+ throw UserException("RobotNodeSet not found: " + rnsName);
+ }
+ if (!nodeSet->getTCP())
+ {
+ throw UserException("RobotNodeSet without tcp: " + rnsName);
+ }
+ VirtualRobot::RobotNodePtr nodeBase = context->remoteRobot->getRobotNode(rnBaseName);
+ if (!nodeBase)
+ {
+ throw UserException("RobotNode not found: " + rnBaseName);
+ }
+
+ Eigen::Matrix4f tcpPose = nodeSet->getTCP()->getGlobalPose();
+ tcpPose = nodeBase->toLocalCoordinateSystem(tcpPose);
+ FramedPosePtr fp(new FramedPose(tcpPose,rnBaseName));
+
+ setLocal("startPose", fp);
+
+ Eigen::Matrix4f tcpGoalPose = nodeSet->getTCP()->getGlobalPose();
+ float maxHeight = getInput<float>("maxHeightToMoveDown");
+ tcpGoalPose(0,3) -= maxHeight;
+ tcpGoalPose = nodeBase->toLocalCoordinateSystem(tcpGoalPose);
+ FramedPosePtr fpGoal(new FramedPose(tcpGoalPose,rnBaseName));
+
+ setLocal("goalPose", fpGoal);
+
+ ARMARX_LOG << eINFO << "Installing place object timeout condition";
+ float timeoutPlaceObject = getInput<float>("timeoutPlaceObject");
+
+ condPlaceObjectTimeout = setTimeoutEvent(timeoutPlaceObject, createEvent<EvPlaceObjectTimeout>());
+
+
+
+ // install the force condition
+ float force = getInput<float>("minForceForSuccess");
+ Term cond;
+ for(unsigned int i=0; i<nodeSet->getSize(); i++)
+ {
+ ParameterList inrangeCheck;
+ inrangeCheck.push_back(new Variant(-force));
+ inrangeCheck.push_back(new Variant(force));
+
+ Literal inrangeLiteral(DataFieldIdentifier(context->getKinematicUnitObserverName(),"jointtorques", nodeSet->getNode(i)->getName()), "inrange", inrangeCheck);
+ cond = cond && !inrangeLiteral;
+ }
+
+ condPlaceObjectCollision = installCondition<EvPlaceObjectCollision>(cond);
+ }
+
+ void StatechartPlaceObject::onExit()
+ {
+ removeTimeoutEvent(condPlaceObjectTimeout);
+ removeCondition(condPlaceObjectCollision);
+
+ ARMARX_LOG << eINFO << "Exiting StatechartPlaceObject...";
+ }
+
+ // ****************************************************************
+ // Implementation of StateMoveDown
+ // ****************************************************************
+
+ void StateMoveDown::defineParameters()
+ {
+ addToInput("maxHeightToMoveDown", VariantType::List(VariantType::Float), false);
+ addToInput("minForceForSuccess", VariantType::List(VariantType::Float), false);
+ addToInput("timeoutPlaceObject", VariantType::Float, false);
+ addToInput("robotNodeSetName", VariantType::String, false);
+ addToInput("robotBaseFrame", VariantType::String, false);
+ addToInput("startPose", VariantType::List(VariantType::FramedPose), false);
+ }
+
+ void StateMoveDown::onEnter()
+ {
+ ARMARX_DEBUG << "Entering StateMoveDown" << flush;
+
+ RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
+ std::string rnsName = getInput<std::string>("robotNodeSetName");
+ std::string rnBaseName = getInput<std::string>("robotBaseFrame");
+ VirtualRobot::RobotPtr localRobot = RemoteRobot::createLocalClone(rsContext->robotStateComponent);
+
+ VirtualRobot::RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(rnsName);
+ VirtualRobot::RobotNodePtr nodeBase = localRobot->getRobotNode(rnBaseName);
+ FramedPosePtr startPoseFramed = getInput<FramedPose>("startPose");
+ FramedPosePtr goalPoseFramed = getInput<FramedPose>("goalPose");
+
+ Eigen::Matrix4f startPose = startPoseFramed->toEigen();
+ Eigen::Matrix4f goalPose = goalPoseFramed->toEigen();
+ Eigen::Matrix4f currentPose = nodeSet->getTCP()->getGlobalPose();
+ currentPose = nodeBase->toLocalCoordinateSystem(currentPose);
+
+ ARMARX_DEBUG << "startPose (coordsystem " << rnBaseName << ") : " << startPose << flush;
+ ARMARX_DEBUG << "goalPose (coordsystem " << rnBaseName << ") : " << goalPose << flush;
+ ARMARX_DEBUG << "currentPose (coordsystem " << rnBaseName << ") : " << currentPose << flush;
+
+ float dist = (goalPose.block(0,3,3,1) - currentPose.block(0,3,3,1)).norm();
+ if (dist<10.0f)
+ {
+ ARMARX_ERROR << "Did not hit any surface, sending failure event..." << flush;
+ sendEvent<EvFailure>();
+ }
+
+
+ VirtualRobot::DifferentialIKPtr ikSolver(new VirtualRobot::DifferentialIK(nodeSet,nodeBase));
+
+ ikSolver->setGoal(goalPose,nodeSet->getTCP(),VirtualRobot::IKSolver::Position,10.0f);
+
+ bool ikOK = ikSolver->solveIK(0.6f,0,10);
+
+ if (!ikOK)
+ {
+ ARMARX_ERROR << "No IK solution for goal pose (coordsystem " << rnBaseName << ") : " << goalPose << flush;
+ sendEvent<EvFailure>();
+ }
+
+ std::vector<float> jointValues = nodeSet->getJointValues();
+ NameValueMap jointNamesAndValues;
+
+ for (unsigned int j=0; j<jointValues.size(); j++)
+ {
+ jointNamesAndValues[nodeSet->getNode(j)->getName()]=jointValues[j];
+ ARMARX_DEBUG << "target for joint :" << nodeSet->getNode(j)->getName() << " -> " << jointValues[j] << flush;
+ }
+
+ rsContext->kinematicUnitPrx->setJointAngles(jointNamesAndValues);
+
+ ARMARX_DEBUG << "StateMoveDown: Done onEnter()";
+ }
+
+ void StateMoveDown::onExit()
+ {
+ ARMARX_DEBUG << "StatePlaceObject: Done onExit()";
+ }
+
+
+ // ****************************************************************
+ // Implementation of StateCheckForces
+ // ****************************************************************
+/*
+ void StateCheckForces::defineParameters()
+ {
+ addToInput("maxHeightToMoveDown", VariantType::List(VariantType::Float), false);
+ addToInput("minForceForSuccess", VariantType::List(VariantType::Float), false);
+ addToInput("timeoutPlaceObject", VariantType::Float, false);
+ addToInput("robotNodeSetName", VariantType::String, false);
+ addToInput("robotBaseFrame", VariantType::String, false);
+ addToInput("startPose", VariantType::List(VariantType::FramedPose), false);
+ }
+
+ void StateCheckForces::onEnter()
+ {
+ ARMARX_DEBUG << "Entering StateCheckForces" << flush;
+
+ float force = getInput<float>("minForceForSuccess");
+ RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
+ rsContext->kinematicUnitObserverPrx->getDataFields()
+
+
+ RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
+ std::string rnsName = getInput<std::string>("robotNodeSetName");
+ std::string rnBaseName = getInput<std::string>("robotBaseFrame");
+
+
+ VirtualRobot::RobotPtr localRobot = RemoteRobot::createLocalClone(context->robotStateComponent);
+ std::string rnsName = getInput<std::string>("robotNodeSetName");
+ std::string rnBaseName = getInput<std::string>("robotBaseFrame");
+ VirtualRobot::RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(rnsName);
+ VirtualRobot::RobotNodePtr nodeBase = localRobot->getRobotNode(rnBaseName);
+ FramedPosePtr startPoseFramed = getInput<FramedPose>("startPose");
+ FramedPosePtr goalPoseFramed = getInput<FramedPose>("goalPose");
+
+ Eigen::Matrix4f startPose = startPoseFramed->toEigen();
+ Eigen::Matrix4f goalPose = goalPoseFramed->toEigen();
+ Eigen::Matrix4f currentPose = nodeSet->getTCP()->getGlobalPose();
+ currentPose = nodeBase->toLocalCoordinateSystem(currentPose);
+
+ ARMARX_DEBUG << "startPose (coordsystem " << rnBaseName << ") : " << startPose << flush;
+ ARMARX_DEBUG << "goalPose (coordsystem " << rnBaseName << ") : " << goalPose << flush;
+ ARMARX_DEBUG << "currentPose (coordsystem " << rnBaseName << ") : " << currentPose << flush;
+
+ float dist = (goalPose.block(0,3,3,1) - currentPose.block(0,3,3,1)).norm();
+ if (dist<10.0f)
+ {
+ ARMARX_ERROR << "Did not hit any surface, sending failure event..." << flush;
+ sendEvent<EvFailure>();
+ }
+
+
+ VirtualRobot::DifferentialIKPtr ikSolver(new VirtualRobot::DifferentialIKPtr(nodeSet,nodeBase));
+
+ ikSolver->setGoal(goalPose,nodeSet->getTCP(),VirtualRobot::IKSolver::Position,10.0f);
+
+ bool ikOK = ikSolver->solveIK(0.6f,0,10);
+
+ if (!ikOK)
+ {
+ ARMARX_ERROR << "No IK solution for goal pose (coordsystem " << rnBaseName << ") : " << goalPose << flush;
+ sendEvent<EvFailure>();
+ }
+
+ std::vector<float> jointValues = nodeSet->getJointValues();
+ NameValueMap jointNamesAndValues;
+
+ for (unsigned int j=0; j<jointValues->getSize(); j++)
+ {
+ jointNamesAndValues[nodeSet->getNode(j)->getName()]=jointValues[j];
+ ARMARX_DEBUG << "target for joint :" << nodeSet->getNode(j)->getName() << " -> " << jointValues[j] << flush;
+ }
+
+ rsContext->kinematicUnitPrx->setJointAngles(jointNamesAndValues);
+
+ ARMARX_DEBUG << "StateCheckForces: Done onEnter()";
+ }
+
+ void StateCheckForces::onExit()
+ {
+ ARMARX_DEBUG << "StateCheckForces: Done onExit()";
+ }
+
+*/
+
+}
+
diff --git a/source/RobotAPI/statecharts/PlaceObject/PlaceObject.h b/source/RobotAPI/statecharts/PlaceObject/PlaceObject.h
new file mode 100644
index 0000000000000000000000000000000000000000..5e3eac02627245b978b8b011543adc60ae03ba3d
--- /dev/null
+++ b/source/RobotAPI/statecharts/PlaceObject/PlaceObject.h
@@ -0,0 +1,85 @@
+/*
+* 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 RobotAPI::PlaceObject
+* @author Nikolaus Vahrenkamp
+* @date 2014
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#ifndef ARMARX_COMPONENT_PLACE_OBJECT_H
+#define ARMARX_COMPONENT_PLACE_OBJECT_H
+
+#include <Core/statechart/Statechart.h>
+
+
+namespace armarx
+{
+ // ****************************************************************
+ // Events
+ // ****************************************************************
+
+ DEFINEEVENT(EvPlaceObjectTimeout)
+ DEFINEEVENT(EvPlaceObjectCollision)
+
+
+ // ****************************************************************
+ // Definition of StatechartPlaceObject
+ // ****************************************************************
+ struct StatechartPlaceObject :
+ StateTemplate<StatechartPlaceObject>
+ {
+ void defineParameters();
+ void defineSubstates();
+ void onEnter();
+ void onExit();
+ StateUtility::ActionEventIdentifier condPlaceObjectTimeout;
+ ConditionIdentifier condPlaceObjectCollision;
+ };
+
+
+
+ // ****************************************************************
+ // Definition of states
+ // ****************************************************************
+ /**
+ * StatePlaceMoveDown: Move down until a surface is hit
+ */
+
+ struct StateMoveDown :
+ StateTemplate<StateMoveDown>
+ {
+ void defineParameters();
+ void onEnter();
+ void onExit();
+ };
+
+
+ /**
+ * StateCheckForces: Check if a surface was hit -> forces must increase
+ */
+ /*struct StateCheckForces :
+ StateTemplate<StateCheckForces>
+ {
+ void defineParameters();
+ void onEnter();
+ void onExit();
+ };*/
+
+}
+
+#endif
diff --git a/source/RobotAPI/units/ForceTorqueUnit.cpp b/source/RobotAPI/units/ForceTorqueUnit.cpp
index 6f6c4fb808c4ee3269bb70ce322dc7a385331168..b5b4822eee5704a5144a3b28484a4030f6aa07a1 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 3c56c9afb73502266d46e492b23a5a6f2dba5265..89f8975aef8836043847e05f67e5305e23cb2ec6 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 1862188317a891a9bf08fc3d59d4cb02b248e425..8b09ca71ea75e7854cfd4aee5760df9f49267814 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;