From b62dae0a3c235fd86ae7d2d3884eee1944755560 Mon Sep 17 00:00:00 2001
From: Ali Paikan <ali.paikan@iit.it>
Date: Tue, 15 Jul 2014 19:14:38 +0200
Subject: [PATCH] Fixed Hand Motion States
---
.../RobotAPI/core/RobotStatechartContext.cpp | 4 +-
source/RobotAPI/core/RobotStatechartContext.h | 2 +-
.../RobotAPI/motioncontrol/MotionControl.cpp | 60 +++++++++++++++----
source/RobotAPI/motioncontrol/MotionControl.h | 4 ++
4 files changed, 56 insertions(+), 14 deletions(-)
diff --git a/source/RobotAPI/core/RobotStatechartContext.cpp b/source/RobotAPI/core/RobotStatechartContext.cpp
index 5570d7d06..07046eed4 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 1596eb373..6513191b2 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 00de70399..14d506848 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 33e35cc5a..c629105c0 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)
--
GitLab