diff --git a/source/RobotAPI/core/RobotStatechartContext.cpp b/source/RobotAPI/core/RobotStatechartContext.cpp
index 5570d7d0663c8b5f6d13d35cb3539683a187d79c..07046eed4fe71c42dc2fb370d1fe21160d5fb0ee 100644
--- a/source/RobotAPI/core/RobotStatechartContext.cpp
+++ b/source/RobotAPI/core/RobotStatechartContext.cpp
@@ -48,7 +48,7 @@ namespace armarx
usingProxy("RobotStateComponent");
usingProxy(kinematicUnitObserverName);
- if( getProperty<std::string>("HandUnits").isSet())
+ if(!getProperty<std::string>("HandUnits").getValue().empty())
{
std::string handUnitsProp = getProperty<std::string>("HandUnits").getValue();
std::vector<std::string> handUnitList;
@@ -73,7 +73,7 @@ namespace armarx
kinematicUnitObserverPrx = getProxy<KinematicUnitObserverInterfacePrx>(kinematicUnitObserverName);
ARMARX_LOG << eINFO << "Fetched proxies" << kinematicUnitPrx << " " << robotStateComponent << flush;
- if( getProperty<std::string>("HandUnits").isSet())
+ if( !getProperty<std::string>("HandUnits").getValue().empty())
{
std::string handUnitsProp = getProperty<std::string>("HandUnits").getValue();
std::vector<std::string> handUnitList;
diff --git a/source/RobotAPI/core/RobotStatechartContext.h b/source/RobotAPI/core/RobotStatechartContext.h
index 1596eb373724ebc2e62e2fcc981b900edfa2d450..6513191b24e053422753eef748cbecaf6c030050 100644
--- a/source/RobotAPI/core/RobotStatechartContext.h
+++ b/source/RobotAPI/core/RobotStatechartContext.h
@@ -52,7 +52,7 @@ namespace armarx
{
defineRequiredProperty<std::string>("KinematicUnitName", "Name of the kinematic unit that should be used");
defineRequiredProperty<std::string>("KinematicUnitObserverName", "Name of the kinematic unit observer that should be used");
- 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>("HandUnits", "RightHandUnit", "Name of the comma-seperated hand units that should be used. Unitname for left hand should be LeftHandUnit, and for right hand RightHandUnit");
}
};
diff --git a/source/RobotAPI/motioncontrol/MotionControl.cpp b/source/RobotAPI/motioncontrol/MotionControl.cpp
index 00de7039927261b0fda1950b738581ea1497e8ae..14d506848359356047eefaa1418e3c904a21f8ce 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.cpp
+++ b/source/RobotAPI/motioncontrol/MotionControl.cpp
@@ -703,18 +703,21 @@ void DoPreshapeSet::defineParameters()
addToInput("useLeftHand", VariantType::Bool, false);
addToInput("preshapes", VariantType::List(VariantType::String), false);
addToLocal("counterRef", VariantType::ChannelRef);
+ addToLocal("counterThreshold", VariantType::Int);
}
void DoPreshapeSet::defineSubstates()
{
+ StatePtr counter = addState<CounterStateTemplate<EvReachedIntermediatePreshape, EvReachedFinalPreshape> >("Counter");
StatePtr preshape = addState<SelectAndDoPreshape>("SelectAndDoPreshape");
- setInitState(preshape, PM::createMapping()->mapFromParent("*"));
+ setInitState(counter, PM::createMapping()->mapFromParent("*"));
StatePtr success = addState<SuccessState>("SuccessState");
- StatePtr counter = addState<CounterStateTemplate<EvReachedIntermediatePreshape, EvReachedFinalPreshape> >("EvReachedIntermediatePreshape");
+ StatePtr failure = addState<FailureState>("FailureState");
addTransition<EvSuccess>(preshape, counter, PM::createMapping()->mapFromParent("*"));
+ addTransition<EvFailure>(preshape, failure, PM::createMapping()->mapFromParent("*"));
addTransition<EvReachedIntermediatePreshape>(counter, preshape, PM::createMapping()->mapFromParent("*"));
addTransition<EvReachedFinalPreshape>(counter, success, PM::createMapping()->mapFromParent("*"));
@@ -724,8 +727,13 @@ void DoPreshapeSet::defineSubstates()
void DoPreshapeSet::onEnter()
{
-
- ChannelRefPtr counterRef = ChannelRefPtr::dynamicCast(getContext()->systemObserverPrx->startCounter(0,"preshapeCounter"));
+ std::string preshapes;
+ for (int i = 0; i < getInput<SingleTypeVariantList>("preshapes")->getSize(); ++i) {
+ preshapes += getInput<SingleTypeVariantList>("preshapes")->getVariant(i)->getString() + ", ";
+ }
+ ARMARX_IMPORTANT << "preshapes: " << preshapes;
+ ChannelRefPtr counterRef = ChannelRefPtr::dynamicCast(getContext()->systemObserverPrx->startCounter(-1,"preshapeCounter"));
+ setLocal("counterThreshold", getInput<SingleTypeVariantList>("preshapes")->getSize());
setLocal("counterRef", counterRef);
}
@@ -767,8 +775,17 @@ void SelectAndDoPreshape::defineSubstates()
void SelectAndDoPreshape::onEnter()
{
int index = getInput<ChannelRef>("counterRef")->getDataField("value")->getInt();
- std::string preshapeName = getInput<SingleTypeVariantList>("preshapes")->getVariant(index)->getString();
- setLocal("selectedPreshapeName", preshapeName);
+ ARMARX_IMPORTANT << "index: " << index;
+ if(index >= getInput<SingleTypeVariantList>("preshapes")->getSize() )
+ {
+ setLocal("selectedPreshapeName", std::string(""));
+ sendEvent<EvFailure>();
+ }
+ else
+ {
+ std::string preshapeName = getInput<SingleTypeVariantList>("preshapes")->getVariant(index)->getString();
+ setLocal("selectedPreshapeName", preshapeName);
+ }
}
void DoPreshape::defineParameters()
@@ -792,20 +809,40 @@ void DoPreshape::onEnter()
}
std::map<std::string, HandUnitInterfacePrx>::iterator it = context->handUnits.find(handUnitName);
+ const std::string kinUnitName = getContext<RobotStatechartContext>()->getKinematicUnitObserverName();
+ StringSequence availableJoints = getContext<RobotStatechartContext>()->getChannelRef(kinUnitName, "jointangles")->getDataFieldNames();
+ ARMARX_INFO << VAROUT(availableJoints);
if(it != context->handUnits.end())
{
-// NameValueMap jointValues =it->second->getPreshapeJointValues(getInput<std::string>("PreshapeName"));
+ NameValueMap jointValues = it->second->getPreshapeJointValues(getInput<std::string>("preshapeName"));
+ Term jointCondition;
+ ARMARX_IMPORTANT << VAROUT(jointValues);
+ for (NameValueMap::const_iterator itJ = jointValues.begin(); itJ != jointValues.end(); ++itJ)
+ {
+ if(std::find(availableJoints.begin(), availableJoints.end(), itJ->first) == availableJoints.end())
+ continue;
+ DataFieldIdentifierPtr id = new DataFieldIdentifier(kinUnitName, "jointangles", itJ->first);
+ jointCondition = jointCondition && Literal(id, "inrange", Literal::createParameterList((float)(itJ->second - 2.0f/180.0f*M_PI), (float)(itJ->second + 2.0f/180.0f*M_PI)));
+ }
+ cond = installCondition<EvSuccess>(jointCondition,"joint angles reached");
it->second->preshape(getInput<std::string>("preshapeName"));
- setTimeoutEvent(2000, createEvent<EvSuccess>());
+ action = setTimeoutEvent(5000, createEvent<EvSuccess>());
}
else
{
- ARMARX_ERROR << "No Proxy for HandUnit with Name " << handUnitName << " known";
+ ARMARX_ERROR << "No Proxy for HandUnit with Name " << handUnitName << " known in RobotStatechartContext - check configFile property 'handUnits'";
+ sendEvent<EvFailure>();
}
}
+void DoPreshape::onExit()
+{
+ removeCondition(cond);
+ removeTimeoutEvent(action);
+}
+
SingleTypeVariantList DoPreshape::GetPreshapeSet(const SingleTypeVariantListPtr preshapes, const std::string &preshapePrefix)
{
SingleTypeVariantList result(VariantType::String);
@@ -835,6 +872,7 @@ void DoPrefixPreshapeSet::defineParameters()
{
addToInput("useLeftHand", VariantType::Bool, false);
addToInput("preshapePrefix", VariantType::String, false);
+ addToLocal("preshapes", VariantType::List(VariantType::String));
}
@@ -870,7 +908,7 @@ void DoPrefixPreshapeSet::onEnter()
void OpenHand::defineSubstates()
{
- StatePtr doPreshape = addState<DoPrefixPreshapeSet>("DoPreshapeSet");
+ StatePtr doPreshape = addState<DoPrefixPreshapeSet>("DoPrefixPreshapeSet");
PMPtr mapping = PM::createMapping()->
mapFromParent("*");
setInitState(doPreshape, mapping);
@@ -892,7 +930,7 @@ void OpenHand::onEnter()
void CloseHand::defineSubstates()
{
- StatePtr doPreshape = addState<DoPrefixPreshapeSet>("DoPreshapeSet");
+ StatePtr doPreshape = addState<DoPrefixPreshapeSet>("DoPrefixPreshapeSet");
PMPtr mapping = PM::createMapping()->
mapFromParent("*");
setInitState(doPreshape, mapping);
diff --git a/source/RobotAPI/motioncontrol/MotionControl.h b/source/RobotAPI/motioncontrol/MotionControl.h
index 33e35cc5a1c688c89ff9903c32043ba073c0b1f7..c629105c0f9cc54e91b2ede2107b7260b5910de4 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.h
+++ b/source/RobotAPI/motioncontrol/MotionControl.h
@@ -233,7 +233,11 @@ namespace MotionControl
{
void defineParameters();
void onEnter();
+ void onExit();
static SingleTypeVariantList GetPreshapeSet(const SingleTypeVariantListPtr preshapes, const std::string& preshapePrefix);
+
+ ConditionIdentifier cond;
+ ActionEventIdentifier action;
};
DEFINEEVENT(EvReachedIntermediatePreshape)