From a333bcc8bb35a3c6f9b98108b1972fc6ea3098b5 Mon Sep 17 00:00:00 2001
From: Markus <Markus.Przybylski@kit.edu>
Date: Wed, 19 Feb 2014 17:49:39 +0100
Subject: [PATCH] It is now possible to specify via the parameter
'useTorquesForGrasping' whether you want to close the hand based on torques
(useTorquesForGrasping=1) or based on joint angles (useTorquesForGrasping=0).
---
.../GraspingWithTorques.cpp | 76 +++++++++++++++++--
.../GraspingWithTorques/GraspingWithTorques.h | 26 ++++++-
2 files changed, 95 insertions(+), 7 deletions(-)
diff --git a/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp b/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp
index 37b4156fa..23d7a53bc 100644
--- a/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp
+++ b/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp
@@ -46,6 +46,7 @@ namespace armarx
addToInput("jointAnglesPreshape", VariantType::List(VariantType::Float), false);
//how long to wait before switching from preshape to grasp
addToInput("timeoutPreshape", VariantType::Float, false);
+ addToInput("useTorquesForGrasping", VariantType::Int, false);
//closeHandWithTorques settings:
addToInput("jointTorquesGrasp", VariantType::List(VariantType::Float), false);
@@ -53,6 +54,9 @@ namespace armarx
//low joint velocities as a termination criterion.
addToInput("timeoutMinExecutionTime", VariantType::Float, false);
+ //closeHandWithJointAngles settings:
+ addToInput("jointAnglesGrasp", VariantType::List(VariantType::Float), false);
+
//installTerminateConditions settings:
//how long to wait until you think the hand is closed and grasping is completed
addToInput("timeoutGrasp", VariantType::Float, false);
@@ -73,6 +77,7 @@ namespace armarx
{
StatePtr statePreshape = addState<StatePreshape>("statePreshape");
StatePtr stateCloseHandWithTorques = addState<StateCloseHandWithTorques>("stateCloseHandWithTorques");
+ StatePtr stateCloseHandWithJointAngles = addState<StateCloseHandWithJointAngles>("stateCloseHandWithJointAngles");
StatePtr stateInstallTerminateConditions = addState<StateInstallTerminateConditions>("stateInstallTerminateConditions");
StatePtr stateSuccess = addState<SuccessState>("stateSuccess");
StatePtr stateFailure = addState<FailureState>("stateFailure");
@@ -80,13 +85,19 @@ namespace armarx
ParameterMappingPtr mapPreshapeInfo = ParameterMapping::createMapping()
->mapFromParent("jointAnglesPreshape", "jointAnglesPreshape")
->mapFromParent("timeoutPreshape", "timeoutPreshape")
- ->mapFromParent("jointNames", "jointNames");
+ ->mapFromParent("jointNames", "jointNames")
+ ->mapFromParent("useTorquesForGrasping", "useTorquesForGrasping");
ParameterMappingPtr mapCloseHandWithTorquesInfo = ParameterMapping::createMapping()
->mapFromParent("jointTorquesGrasp", "jointTorquesGrasp")
->mapFromParent("timeoutMinExecutionTime", "timeoutMinExecutionTime")
->mapFromParent("jointNames", "jointNames");
+ ParameterMappingPtr mapCloseHandWithJointAnglesInfo = ParameterMapping::createMapping()
+ ->mapFromParent("jointAnglesGrasp", "jointAnglesGrasp")
+ ->mapFromParent("timeoutGrasp", "timeoutGrasp")
+ ->mapFromParent("jointNames", "jointNames");
+
ParameterMappingPtr mapInstallTerminateConditionsInfo = ParameterMapping::createMapping()
->mapFromParent("timeoutGrasp", "timeoutGrasp")
->mapFromParent("thresholdVelocity", "thresholdVelocity")
@@ -97,10 +108,12 @@ namespace armarx
setInitState(statePreshape, mapPreshapeInfo);
//transitions
- addTransition<EvPreshapeTimeout>(statePreshape, stateCloseHandWithTorques, mapCloseHandWithTorquesInfo);
+ addTransition<EvPreshapeTimeout_ToCloseWithTorques>(statePreshape, stateCloseHandWithTorques, mapCloseHandWithTorquesInfo);
+ addTransition<EvPreshapeTimeout_ToCloseWithJointAngles>(statePreshape, stateCloseHandWithJointAngles, mapCloseHandWithJointAnglesInfo);
addTransition<EvMinExecutionTimeout>(stateCloseHandWithTorques, stateInstallTerminateConditions, mapInstallTerminateConditionsInfo);
- addTransition<EvGraspTimeout>(stateInstallTerminateConditions, stateFailure);
+ addTransition<EvGraspWithTorquesTimeout>(stateInstallTerminateConditions, stateFailure);
addTransition<EvAllJointVelocitiesLow>(stateInstallTerminateConditions, stateSuccess);
+ addTransition<EvGraspWithJointAnglesTimeout>(stateCloseHandWithJointAngles, stateSuccess);
}
@@ -124,7 +137,7 @@ namespace armarx
dataFields.addVariant(context->getDatafieldRef(tempChannelRef, nodeSet->getNode(i)->getName()));
}
- setLocal("jointNames", jointNames); //befüllen...!?
+ setLocal("jointNames", jointNames);
setLocal("jointVelocitiesDatafields", dataFields);
}
@@ -147,6 +160,7 @@ namespace armarx
//addToLocal("jointNames", VariantType::List(VariantType::String));
addToInput("jointNames", VariantType::List(VariantType::String), false);
+ addToInput("useTorquesForGrasping", VariantType::Int, false);
}
void StatePreshape::onEnter()
@@ -172,7 +186,12 @@ namespace armarx
ARMARX_LOG << eINFO << "Installing preshape timeout condition";
float timeoutPreshape = getInput<float>("timeoutPreshape");
- condPreshapeTimeout = setTimeoutEvent(timeoutPreshape, createEvent<EvPreshapeTimeout>());
+
+ int useTorquesForGrasping = getInput<int>("useTorquesForGrasping");
+ if (useTorquesForGrasping > 0)
+ condPreshapeTimeout = setTimeoutEvent(timeoutPreshape, createEvent<EvPreshapeTimeout_ToCloseWithTorques>());
+ else
+ condPreshapeTimeout = setTimeoutEvent(timeoutPreshape, createEvent<EvPreshapeTimeout_ToCloseWithJointAngles>());
ARMARX_LOG << eINFO << "Done onEnter()";
}
@@ -229,6 +248,53 @@ namespace armarx
removeTimeoutEvent(condMinimumExecutionTimeout);
}
+ // ****************************************************************
+ // Implementation of StateCloseHandWithJointAngles
+ // ****************************************************************
+
+ void StateCloseHandWithJointAngles::defineParameters()
+ {
+ //closeHandWithJointAngles settings:
+ addToInput("jointAnglesGrasp", VariantType::List(VariantType::Float), false);
+ addToInput("timeoutGrasp", VariantType::Float, false);
+
+ addToInput("jointNames", VariantType::List(VariantType::String), false);
+ }
+
+ void StateCloseHandWithJointAngles::onEnter()
+ {
+ ARMARX_LOG << eIMPORTANT << "Entering stateCloseHandWithJointAngles !!!";
+
+ RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
+ SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
+ SingleTypeVariantListPtr jointAnglesGraspList = getInput<SingleTypeVariantList>("jointAnglesGrasp");
+ NameValueMap jointNamesAndValues;
+
+ if (jointNames->getSize() == jointAnglesGraspList->getSize())
+ {
+ for (int j=0; j<jointNames->getSize(); j++)
+ {
+ jointNamesAndValues[jointNames->getVariant(j)->getString()]=jointAnglesGraspList->getVariant(j)->getFloat();
+ }
+ }
+ else
+ throw LocalException("stateCloseHandWithJointAngles: List lengths do not match!");
+
+ rsContext->kinematicUnitPrx->setJointAngles(jointNamesAndValues);
+
+ ARMARX_LOG << eINFO << "Installing timeoutGrasp condition";
+ float timeoutGrasp = getInput<float>("timeoutGrasp");
+ condGraspWithJointAnglesTimeout = setTimeoutEvent(timeoutGrasp, createEvent<EvGraspWithJointAnglesTimeout>());
+
+ ARMARX_LOG << eIMPORTANT << "Done onEnter()";
+ }
+
+ void StateCloseHandWithJointAngles::onExit()
+ {
+ removeTimeoutEvent(condGraspWithJointAnglesTimeout);
+ }
+
+
// ****************************************************************
// Implementation of StateInstallTerminateConditions
// ****************************************************************
diff --git a/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.h b/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.h
index 998511502..984ca7781 100644
--- a/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.h
+++ b/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.h
@@ -34,9 +34,11 @@ namespace armarx
// ****************************************************************
//only for first tests
- DEFINEEVENT(EvPreshapeTimeout)
+ DEFINEEVENT(EvPreshapeTimeout_ToCloseWithTorques)
+ DEFINEEVENT(EvPreshapeTimeout_ToCloseWithJointAngles)
DEFINEEVENT(EvMinExecutionTimeout)
- DEFINEEVENT(EvGraspTimeout)
+ DEFINEEVENT(EvGraspWithJointAnglesTimeout)
+ DEFINEEVENT(EvGraspWithTorquesTimeout)
DEFINEEVENT(EvAllJointVelocitiesLow)
// ****************************************************************
@@ -90,6 +92,26 @@ namespace armarx
StateUtility::ActionEventIdentifier condMinimumExecutionTimeout;
};
+ // ****************************************************************
+ // Definition of StateCloseHandWithJointAngles
+ // ****************************************************************
+
+ /**
+ * StateCloseHandWithJointAngles: Close the hand using a target joint angle configuration.
+ */
+ struct StateCloseHandWithJointAngles :
+ StateTemplate<StateCloseHandWithJointAngles>
+ {
+ void defineParameters();
+ void onEnter();
+ void onExit();
+
+ //Make sure that the joints have time to start moving before we check
+ //low joint velocities as a termination criterion.
+ StateUtility::ActionEventIdentifier condGraspWithJointAnglesTimeout;
+ };
+
+
// ****************************************************************
// Definition of StateInstallTerminateConditions
// ****************************************************************
--
GitLab