diff --git a/source/RobotAPI/core/RobotStatechartContext.cpp b/source/RobotAPI/core/RobotStatechartContext.cpp
index ca0e20b251d6fbec28687dd816a0339a244bd685..be7c3ac667d15cd60081ec0e71eace4fffc03b07 100644
--- a/source/RobotAPI/core/RobotStatechartContext.cpp
+++ b/source/RobotAPI/core/RobotStatechartContext.cpp
@@ -68,10 +68,15 @@ namespace armarx
ARMARX_LOG << eINFO << "Starting RobotStatechartContext" << flush;
// retrieve proxies
- robotStateComponent = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
- kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
+ std::string rbStateName = "RobotStateComponent";
+ robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(rbStateName);
+ std::string kinUnitName = getProperty<std::string>("KinematicUnitName").getValue();
+ kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(kinUnitName);
kinematicUnitObserverPrx = getProxy<KinematicUnitObserverInterfacePrx>(kinematicUnitObserverName);
- ARMARX_LOG << eINFO << "Fetched proxies" << kinematicUnitPrx << " " << robotStateComponent << flush;
+
+ ARMARX_LOG << eINFO << "Fetched proxies " << kinUnitName << ":" << kinematicUnitPrx << ", " << rbStateName << ": " << robotStateComponent << flush;
+
+
if( !getProperty<std::string>("HandUnits").getValue().empty())
{
@@ -81,7 +86,9 @@ namespace armarx
for(size_t i = 0; i < handUnitList.size(); i++)
{
boost::algorithm::trim(handUnitList.at(i));
- handUnits[handUnitList.at(i)] = getProxy<HandUnitInterfacePrx>(handUnitList.at(i));
+ HandUnitInterfacePrx handPrx = getProxy<HandUnitInterfacePrx>(handUnitList.at(i));
+ handUnits[handUnitList.at(i)] = handPrx;
+ ARMARX_LOG << eINFO << "Fetched handUnit proxy " << handUnitList.at(i) << ": " << handPrx << flush;
}
}
// initialize remote robot
@@ -92,7 +99,24 @@ namespace armarx
PropertyDefinitionsPtr RobotStatechartContext::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new RobotStatechartContextProperties(
- getConfigIdentifier()));
+ getConfigIdentifier()));
+ }
+
+ HandUnitInterfacePrx RobotStatechartContext::getHandUnit(const std::string &handUnitName)
+ {
+ if (handUnits.find(handUnitName)!=handUnits.end())
+ {
+ ARMARX_LOG << eINFO << "Found proxy of hand unit with name " << handUnitName << flush;
+ return handUnits[handUnitName];
+ }
+ ARMARX_LOG << eINFO << "Do not know proxy of hand unit with name " << handUnitName << flush;
+ std::map<std::string, HandUnitInterfacePrx>::iterator it = handUnits.begin();
+ while (it!=handUnits.end())
+ {
+ ARMARX_LOG << eINFO << "************ Known hand units: " << it->first << ":" << it->second << flush;
+ it++;
+ }
+ return HandUnitInterfacePrx();
}
/* const VirtualRobot::RobotPtr armarx::Armar4Context::getRobot()
diff --git a/source/RobotAPI/core/RobotStatechartContext.h b/source/RobotAPI/core/RobotStatechartContext.h
index 09b35842c788ddcb20bbf77ae0e539c3aea5409d..f6925bc9df6664db38cc162c259757db7a56ad6a 100644
--- a/source/RobotAPI/core/RobotStatechartContext.h
+++ b/source/RobotAPI/core/RobotStatechartContext.h
@@ -54,7 +54,6 @@ 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");
-
}
};
@@ -83,6 +82,8 @@ namespace armarx
std::string getKinematicUnitObserverName() { return kinematicUnitObserverName; }
+ HandUnitInterfacePrx getHandUnit(const std::string &handUnitName);
+
//! Prx for the RobotState
RobotStateComponentInterfacePrx robotStateComponent;
KinematicUnitInterfacePrx kinematicUnitPrx;
@@ -90,6 +91,7 @@ namespace armarx
TCPControlUnitInterfacePrx tcpControlPrx;
VirtualRobot::RobotPtr remoteRobot;
std::map<std::string, HandUnitInterfacePrx> handUnits;
+
private:
std::string kinematicUnitObserverName;
};
diff --git a/source/RobotAPI/interface/units/HandUnitInterface.ice b/source/RobotAPI/interface/units/HandUnitInterface.ice
index 524aa2dea69c0ef58387530b7b91421248fd6f27..3b64ffd2f7caecf350401d8c5cafa467d20c1740 100644
--- a/source/RobotAPI/interface/units/HandUnitInterface.ice
+++ b/source/RobotAPI/interface/units/HandUnitInterface.ice
@@ -41,6 +41,19 @@ module armarx
void preshape(string preshapeName);
SingleTypeVariantListBase getPreshapeNames();
NameValueMap getPreshapeJointValues(string preshapeName);
+
+ /*!
+ * \brief Inform the hand unit that an object has been successfully grasped.
+ * \param objectName The name of the object
+ * E.g. a state that verifies if a grasp was successful can call this routine.
+ */
+ void setObjectGrasped(string objectName);
+
+ /*!
+ * \brief setObjectReleased Inform the hand unit that an object has been released.
+ * \param objectName The name of the object
+ */
+ void setObjectReleased(string objectName);
};
interface HandUnitListener
@@ -48,6 +61,9 @@ module armarx
void reportHandClosed();
void reportHandOpened();
void reportHandPreshaped();
+
+ // informs all listeners that we grasped an object
+ void reportObjectGrasped(string robotName, string robotNodeName, string objectName);
};
};
diff --git a/source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.cpp b/source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.cpp
index 8345036888870295fc582c90ea9593f421a14581..c6c8c29bd3ba6d99cb46e28c738c88ba89c74842 100644
--- a/source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.cpp
+++ b/source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.cpp
@@ -32,6 +32,7 @@
#include <VirtualRobot/IK/DifferentialIK.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/interface/units/HandUnitInterface.h>
#include <RobotAPI/core/RobotStatechartContext.h>
@@ -61,6 +62,7 @@ namespace armarx
//how long to wait until you think the hand is closed and grasping is completed
addToInput("timeoutGrasp", VariantType::Float, false);
addToInput("thresholdVelocity", VariantType::Float, false);
+ addToInput("handUnitName", VariantType::String, false);
addToInput("robotNodeSetName", VariantType::String, false);
@@ -102,6 +104,7 @@ namespace armarx
->mapFromParent("timeoutGrasp", "timeoutGrasp")
->mapFromParent("thresholdVelocity", "thresholdVelocity")
->mapFromParent("jointNames")
+ ->mapFromParent("handUnitName", "handUnitName")
->mapFromParent("jointVelocitiesDatafields");
//->mapFromParent("jointVelocityChannel"); //first try...
@@ -111,7 +114,8 @@ namespace armarx
addTransition<EvPreshapeTimeout_ToCloseWithTorques>(statePreshape, stateCloseHandWithTorques, mapCloseHandWithTorquesInfo);
addTransition<EvPreshapeTimeout_ToCloseWithJointAngles>(statePreshape, stateCloseHandWithJointAngles, mapCloseHandWithJointAnglesInfo);
addTransition<EvMinExecutionTimeout>(stateCloseHandWithTorques, stateInstallTerminateConditions, mapInstallTerminateConditionsInfo);
- addTransition<EvGraspWithTorquesTimeout>(stateInstallTerminateConditions, stateFailure);
+ //addTransition<EvGraspWithTorquesTimeout>(stateInstallTerminateConditions, stateSuccess);
+ addTransition<EvMinExecutionTimeout>(stateInstallTerminateConditions, stateSuccess); // timeout is ok for now....
addTransition<EvAllJointVelocitiesLow>(stateInstallTerminateConditions, stateSuccess);
addTransition<EvGraspWithJointAnglesTimeout>(stateCloseHandWithJointAngles, stateSuccess);
@@ -314,7 +318,7 @@ namespace armarx
//addToInput("jointVelocityChannel", VariantType::ChannelRef, false); //first try ...
addToInput("jointVelocitiesDatafields", VariantType::List(VariantType::DatafieldRef), false);
-
+ addToInput("handUnitName", VariantType::String, false);
}
void StateInstallTerminateConditions::onEnter()
@@ -325,8 +329,8 @@ namespace armarx
float timeoutGrasp = getInput<float>("timeoutGrasp");
condGraspTimeout = setTimeoutEvent(timeoutGrasp, createEvent<EvMinExecutionTimeout>());
- ARMARX_LOG << eINFO << "Installing allJointVelocitiesLow condition";
float thresholdVelocity = getInput<float>("thresholdVelocity");
+ ARMARX_LOG << eINFO << "Installing allJointVelocitiesLow condition, threshold: " << thresholdVelocity << flush;
//=======
//first try ... (with ChannelRef)
@@ -372,7 +376,16 @@ namespace armarx
//set joint velocities manually to zero (DEBUG)
//---------
+ std::string handUnitName = getInput<std::string>("handUnitName");
+ ARMARX_LOG << eIMPORTANT << "xx..................SENDING OBJECTGRASPED TO hand unit name " << handUnitName << "...................";
+
RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
+ if (rsContext->getHandUnit(handUnitName))
+ {
+ ARMARX_LOG << eIMPORTANT << "xx..................SENDING OBJECTGRASPED TO hand unit name " << handUnitName << ".......OK: sending Vitalis............";
+ rsContext->getHandUnit(handUnitName)->setObjectGrasped("Vitalis");
+ }
+
SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
//SingleTypeVariantListPtr jointTorquesGraspList = getInput<SingleTypeVariantList>("jointTorquesGrasp");
NameValueMap jointNamesAndValues;
@@ -380,8 +393,8 @@ namespace armarx
for (int j=0; j<jointNames->getSize(); j++)
{
- // we want to ensure that the object stayes grasped -> apply a small velocity (todo: find a better solution!)
- jointNamesAndValues[jointNames->getVariant(j)->getString()] = 0.01f; //set everything to zero
+ // no, now we set it to zero.... we want to ensure that the object stayes grasped -> apply a small velocity (todo: find a better solution!)
+ jointNamesAndValues[jointNames->getVariant(j)->getString()] = 0.0;//1f; //set everything to zero
controlModes[jointNames->getVariant(j)->getString()] = eVelocityControl;
}
diff --git a/source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.h b/source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.h
index 6d03db50ed60b5873931bfc4a1355585f028c6ad..6b73aeb0f572a0c999dcd153c20d442b3733f8b0 100644
--- a/source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.h
+++ b/source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.h
@@ -38,7 +38,7 @@ namespace armarx
DEFINEEVENT(EvPreshapeTimeout_ToCloseWithJointAngles)
DEFINEEVENT(EvMinExecutionTimeout)
DEFINEEVENT(EvGraspWithJointAnglesTimeout)
- DEFINEEVENT(EvGraspWithTorquesTimeout)
+ //DEFINEEVENT(EvGraspWithTorquesTimeout)
DEFINEEVENT(EvAllJointVelocitiesLow)
// ****************************************************************
diff --git a/source/RobotAPI/units/HandUnit.cpp b/source/RobotAPI/units/HandUnit.cpp
index 8545fa4352f89820c70a3d976c696823d1742b1f..1172b4ba656a2f1cea938a225d6f74c06df867f4 100644
--- a/source/RobotAPI/units/HandUnit.cpp
+++ b/source/RobotAPI/units/HandUnit.cpp
@@ -37,7 +37,7 @@ using namespace VirtualRobot;
void HandUnit::onInitComponent()
{
- std::string endeffectorFile = getProperty<std::string>("EndeffectorFile").getValue();
+ std::string endeffectorFile = getProperty<std::string>("RobotFileName").getValue();
std::string endeffectorName = getProperty<std::string>("EndeffectorName").getValue();
if (!ArmarXDataPath::getAbsolutePath(endeffectorFile, endeffectorFile))
@@ -63,6 +63,29 @@ void HandUnit::onInitComponent()
{
throw UserException("Robot does not contain an endeffector with name: " + endeffectorName);
}
+ eef = robot->getEndEffector(endeffectorName);
+ robotName = robot->getName();
+ if (eef->getTcp())
+ tcpName = robot->getEndEffector(endeffectorName)->getTcp()->getName();
+ else
+ throw UserException("Endeffector without TCP: " + endeffectorName);
+
+ // get all joints
+ std::vector<EndEffectorActorPtr> actors;
+ eef->getActors(actors);
+ for (size_t i=0;i<actors.size();i++)
+ {
+ EndEffectorActorPtr a = actors[i];
+ std::vector<EndEffectorActor::ActorDefinition> ads = a->getDefinition();
+ for (size_t j=0;j<ads.size();j++)
+ {
+ EndEffectorActor::ActorDefinition ad = ads[j];
+ if (ad.directionAndSpeed!=0 && ad.robotNode)
+ {
+ handJoints[ad.robotNode->getName()] = ad.directionAndSpeed;
+ }
+ }
+ }
const std::vector<std::string> endeffectorPreshapes = robot->getEndEffector(endeffectorName)->getPreshapes();
@@ -134,3 +157,16 @@ NameValueMap HandUnit::getPreshapeJointValues(const std::string &preshapeName, c
RobotConfigPtr rc = efp->getPreshape(preshapeName);
return rc->getRobotNodeJointValueMap();
}
+
+void HandUnit::setObjectGrasped(const std::string &objectName, const Ice::Current &)
+{
+ ARMARX_INFO << "Object grasped " << objectName << flush;
+ graspedObject = objectName;
+}
+
+void HandUnit::setObjectReleased(const std::string &objectName, const Ice::Current &)
+{
+ ARMARX_INFO << "Object released " << objectName << flush;
+ graspedObject = "";
+}
+
diff --git a/source/RobotAPI/units/HandUnit.h b/source/RobotAPI/units/HandUnit.h
index a73ee435eb585bdd8621a88cec76600e78516287..8c4bc201abf8de39ebf294278bb18ac0db025d29 100644
--- a/source/RobotAPI/units/HandUnit.h
+++ b/source/RobotAPI/units/HandUnit.h
@@ -32,6 +32,7 @@
#include <Core/core/system/ImportExportComponent.h>
#include <Core/observers/variant/SingleTypeVariantList.h>
+#include <VirtualRobot/EndEffector/EndEffector.h>
namespace VirtualRobot
{
@@ -52,7 +53,7 @@ namespace armarx
HandUnitPropertyDefinitions(std::string prefix):
ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("EndeffectorFile","VirtualRobot XML file in which the endeffector is is stored.");
+ defineRequiredProperty<std::string>("RobotFileName","VirtualRobot XML file in which the endeffector is is stored.");
defineRequiredProperty<std::string>("EndeffectorName","Name of the endeffector as stored in the XML file (will publish values on EndeffectorName + 'State')");
}
};
@@ -130,6 +131,9 @@ namespace armarx
NameValueMap getPreshapeJointValues(const std::string &preshapeName, const Ice::Current &);
+ void setObjectGrasped(const std::string &objectName, const Ice::Current &);
+ void setObjectReleased(const std::string &objectName, const Ice::Current &);
+
/**
* @see armarx::PropertyUser::createPropertyDefinitions()
@@ -151,8 +155,14 @@ namespace armarx
SingleTypeVariantListPtr preshapeNames;
VirtualRobot::RobotPtr robot;
+ VirtualRobot::EndEffectorPtr eef;
+
+ std::string robotName;
+ std::string tcpName;
+ std::map <std::string, float> handJoints;
+ std::string graspedObject;
};
}