From af7090e32aaa34aa181a38fac2cb05519c966343 Mon Sep 17 00:00:00 2001
From: Ali Paikan <ali.paikan@iit.it>
Date: Thu, 3 Jul 2014 20:10:46 +0200
Subject: [PATCH] Added more hand control states to execute a preshape or a set
of preshapes
---
.../RobotAPI/motioncontrol/MotionControl.cpp | 159 ++++++++++++++++--
source/RobotAPI/motioncontrol/MotionControl.h | 49 +++++-
2 files changed, 195 insertions(+), 13 deletions(-)
diff --git a/source/RobotAPI/motioncontrol/MotionControl.cpp b/source/RobotAPI/motioncontrol/MotionControl.cpp
index 6eded6e89..00de70399 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.cpp
+++ b/source/RobotAPI/motioncontrol/MotionControl.cpp
@@ -698,16 +698,86 @@ void MotionControl::StopRobot::onEnter()
sendEvent<EvSuccess>();
}
+void DoPreshapeSet::defineParameters()
+{
+ addToInput("useLeftHand", VariantType::Bool, false);
+ addToInput("preshapes", VariantType::List(VariantType::String), false);
+ addToLocal("counterRef", VariantType::ChannelRef);
+}
-void CloseHand::defineParameters()
+void DoPreshapeSet::defineSubstates()
+{
+ StatePtr preshape = addState<SelectAndDoPreshape>("SelectAndDoPreshape");
+ setInitState(preshape, PM::createMapping()->mapFromParent("*"));
+ StatePtr success = addState<SuccessState>("SuccessState");
+ StatePtr counter = addState<CounterStateTemplate<EvReachedIntermediatePreshape, EvReachedFinalPreshape> >("EvReachedIntermediatePreshape");
+
+ addTransition<EvSuccess>(preshape, counter, PM::createMapping()->mapFromParent("*"));
+ addTransition<EvReachedIntermediatePreshape>(counter, preshape, PM::createMapping()->mapFromParent("*"));
+ addTransition<EvReachedFinalPreshape>(counter, success, PM::createMapping()->mapFromParent("*"));
+
+}
+
+
+
+void DoPreshapeSet::onEnter()
+{
+
+ ChannelRefPtr counterRef = ChannelRefPtr::dynamicCast(getContext()->systemObserverPrx->startCounter(0,"preshapeCounter"));
+ setLocal("counterRef", counterRef);
+}
+
+void DoPreshapeSet::onExit()
+{
+ getContext()->systemObserverPrx->removeCounter(getLocal<ChannelRef>("counterRef"));
+}
+
+
+
+
+
+
+
+void SelectAndDoPreshape::defineParameters()
{
addToInput("useLeftHand", VariantType::Bool, false);
+ addToInput("preshapes", VariantType::List(VariantType::String), false);
+ addToInput("counterRef", VariantType::ChannelRef, false);
+
+ addToLocal("selectedPreshapeName", VariantType::String);
+
}
+void SelectAndDoPreshape::defineSubstates()
+{
+ StatePtr doPreshape = addState<DoPreshape>("DoPreshape");
+ PMPtr mapping = PM::createMapping()->
+ mapFromParent("*")
+ ->mapFromParent("selectedPreshapeName", "preshapeName");
+ setInitState(doPreshape, mapping);
+ StatePtr success = addState<SuccessState >("SuccessState");
+ addTransition<EvSuccess>(doPreshape, success);
-void CloseHand::onEnter()
+}
+
+
+
+void SelectAndDoPreshape::onEnter()
+{
+ int index = getInput<ChannelRef>("counterRef")->getDataField("value")->getInt();
+ std::string preshapeName = getInput<SingleTypeVariantList>("preshapes")->getVariant(index)->getString();
+ setLocal("selectedPreshapeName", preshapeName);
+}
+
+void DoPreshape::defineParameters()
+{
+ addToInput("useLeftHand", VariantType::Bool, false);
+ addToInput("preshapeName", VariantType::String, false);
+}
+
+void DoPreshape::onEnter()
{
RobotStatechartContext* context = getContext<RobotStatechartContext>();
bool useLeftHand = getInput<bool>("useLeftHand");
@@ -724,30 +794,54 @@ void CloseHand::onEnter()
std::map<std::string, HandUnitInterfacePrx>::iterator it = context->handUnits.find(handUnitName);
if(it != context->handUnits.end())
{
- it->second->close();
+// NameValueMap jointValues =it->second->getPreshapeJointValues(getInput<std::string>("PreshapeName"));
+ it->second->preshape(getInput<std::string>("preshapeName"));
+ setTimeoutEvent(2000, createEvent<EvSuccess>());
+
}
else
{
ARMARX_ERROR << "No Proxy for HandUnit with Name " << handUnitName << " known";
}
-
-
- sendEvent<EvSuccess>();
}
+SingleTypeVariantList DoPreshape::GetPreshapeSet(const SingleTypeVariantListPtr preshapes, const std::string &preshapePrefix)
+{
+ SingleTypeVariantList result(VariantType::String);
+ for(int i=0; i < preshapes->getSize(); i++)
+ {
+ const std::string & preshapeName = preshapes->getVariant(i)->getString();
+ if(preshapeName.find(preshapePrefix) != std::string::npos)
+ {
+ result.addVariant(preshapeName);
+ }
+ }
+ return result;
+}
+void DoPrefixPreshapeSet::defineSubstates()
+{
+ StatePtr doPreshape = addState<DoPreshapeSet>("DoPreshapeSet");
+ PMPtr mapping = PM::createMapping()->
+ mapFromParent("*");
+ setInitState(doPreshape, mapping);
+ StatePtr success = addState<SuccessState>("SuccessState");
+ addTransition<EvSuccess>(doPreshape, success);
+}
-void OpenHand::defineParameters()
+void DoPrefixPreshapeSet::defineParameters()
{
addToInput("useLeftHand", VariantType::Bool, false);
+ addToInput("preshapePrefix", VariantType::String, false);
}
-void OpenHand::onEnter()
+void DoPrefixPreshapeSet::onEnter()
{
RobotStatechartContext* context = getContext<RobotStatechartContext>();
+
bool useLeftHand = getInput<bool>("useLeftHand");
std::string handUnitName;
if (useLeftHand)
@@ -762,19 +856,61 @@ void OpenHand::onEnter()
std::map<std::string, HandUnitInterfacePrx>::iterator it = context->handUnits.find(handUnitName);
if(it != context->handUnits.end())
{
- it->second->open();
+ setLocal("preshapes", DoPreshape::GetPreshapeSet(SingleTypeVariantListPtr::dynamicCast(it->second->getPreshapeNames()), getInput<std::string>("preshapePrefix")));
}
else
{
ARMARX_ERROR << "No Proxy for HandUnit with Name " << handUnitName << " known";
}
+}
- sendEvent<EvSuccess>();
+
+
+
+
+void OpenHand::defineSubstates()
+{
+ StatePtr doPreshape = addState<DoPrefixPreshapeSet>("DoPreshapeSet");
+ PMPtr mapping = PM::createMapping()->
+ mapFromParent("*");
+ setInitState(doPreshape, mapping);
+ StatePtr success = addState<SuccessState>("SuccessState");
+ addTransition<EvSuccess>(doPreshape, success);
}
+void OpenHand::defineParameters()
+{
+ addToInput("useLeftHand", VariantType::Bool, false);
+ addToLocal("preshapePrefix", VariantType::String);
+}
+
+void OpenHand::onEnter()
+{
+ setLocal("preshapePrefix", "Open");
+
+}
+
+void CloseHand::defineSubstates()
+{
+ StatePtr doPreshape = addState<DoPrefixPreshapeSet>("DoPreshapeSet");
+ PMPtr mapping = PM::createMapping()->
+ mapFromParent("*");
+ setInitState(doPreshape, mapping);
+ StatePtr success = addState<SuccessState>("SuccessState");
+ addTransition<EvSuccess>(doPreshape, success);
+}
+void CloseHand::defineParameters()
+{
+ addToInput("useLeftHand", VariantType::Bool, false);
+ addToLocal("preshapePrefix", VariantType::String);
+}
+void CloseHand::onEnter()
+{
+ setLocal("preshapePrefix", "Close");
+}
void HeadLookAtTarget::defineParameters()
{
@@ -884,3 +1020,6 @@ void CalculateHeadIK::run()
}
+
+
+
diff --git a/source/RobotAPI/motioncontrol/MotionControl.h b/source/RobotAPI/motioncontrol/MotionControl.h
index 8132c75e7..33e35cc5a 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.h
+++ b/source/RobotAPI/motioncontrol/MotionControl.h
@@ -224,15 +224,43 @@ namespace MotionControl
};
-
-
/**
* \ingroup MotionControl
* Closes the given hand of the robot
* \param useLeftHand True if left hand should be closed, false for the right hand
*/
- struct CloseHand : StateTemplate<CloseHand>
+ struct DoPreshape : StateTemplate<DoPreshape>
+ {
+ void defineParameters();
+ void onEnter();
+ static SingleTypeVariantList GetPreshapeSet(const SingleTypeVariantListPtr preshapes, const std::string& preshapePrefix);
+ };
+
+ DEFINEEVENT(EvReachedIntermediatePreshape)
+ DEFINEEVENT(EvReachedFinalPreshape)
+ struct SelectAndDoPreshape : StateTemplate<SelectAndDoPreshape>
+ {
+ void defineSubstates();
+ void defineParameters();
+ void onEnter();
+ };
+
+ struct DoPreshapeSet : StateTemplate<DoPreshapeSet>
+ {
+ void defineSubstates();
+ void defineParameters();
+ void onEnter();
+ void onExit();
+
+ };
+
+
+
+
+
+ struct DoPrefixPreshapeSet : StateTemplate<DoPrefixPreshapeSet>
{
+ void defineSubstates();
void defineParameters();
void onEnter();
};
@@ -240,6 +268,7 @@ namespace MotionControl
+
/**
* \ingroup MotionControl
* Opens the given hand of the robot
@@ -247,6 +276,20 @@ namespace MotionControl
*/
struct OpenHand : StateTemplate<OpenHand>
{
+ void defineSubstates();
+ void defineParameters();
+ void onEnter();
+ };
+
+
+ /**
+ * \ingroup MotionControl
+ * Closes the given hand of the robot
+ * \param useLeftHand True if left hand should be closed, false for the right hand
+ */
+ struct CloseHand : StateTemplate<CloseHand>
+ {
+ void defineSubstates();
void defineParameters();
void onEnter();
};
--
GitLab