From 765adaf494e236c42f98b72518a6e6424ebb2272 Mon Sep 17 00:00:00 2001
From: Markus <Markus.Przybylski@kit.edu>
Date: Thu, 23 Jan 2014 11:56:51 +0100
Subject: [PATCH] Added statechart that closes the robot hand by setting
predefined torques to the finger joints.
---
source/RobotAPI/CMakeLists.txt | 2 +-
.../GraspingWithTorques/CMakeLists.txt | 31 ++
.../GraspingWithTorques.cpp | 294 ++++++++++++++++++
.../GraspingWithTorques/GraspingWithTorques.h | 114 +++++++
4 files changed, 440 insertions(+), 1 deletion(-)
create mode 100644 source/RobotAPI/GraspingWithTorques/CMakeLists.txt
create mode 100644 source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp
create mode 100644 source/RobotAPI/GraspingWithTorques/GraspingWithTorques.h
diff --git a/source/RobotAPI/CMakeLists.txt b/source/RobotAPI/CMakeLists.txt
index 475cc9398..da048e516 100644
--- a/source/RobotAPI/CMakeLists.txt
+++ b/source/RobotAPI/CMakeLists.txt
@@ -2,4 +2,4 @@ add_subdirectory(core)
add_subdirectory(motioncontrol)
add_subdirectory(applications)
add_subdirectory(units)
-
+add_subdirectory(GraspingWithTorques)
diff --git a/source/RobotAPI/GraspingWithTorques/CMakeLists.txt b/source/RobotAPI/GraspingWithTorques/CMakeLists.txt
new file mode 100644
index 000000000..17b564c43
--- /dev/null
+++ b/source/RobotAPI/GraspingWithTorques/CMakeLists.txt
@@ -0,0 +1,31 @@
+armarx_set_target("RobotAPI Library: GraspingWithTorques")
+
+
+find_package(Eigen3 QUIET)
+find_package(Simox QUIET)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+if (ARMARX_BUILD)
+ include_directories(${Eigen3_INCLUDE_DIR})
+ include_directories(${VirtualRobot_INCLUDE_DIRS})
+
+ set(LIB_NAME GraspingWithTorques)
+ set(LIB_VERSION 0.1.0)
+ set(LIB_SOVERSION 0)
+
+ set(LIBS RobotAPICore RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers)
+
+ set(LIB_FILES GraspingWithTorques.cpp
+ )
+ set(LIB_HEADERS GraspingWithTorques.h
+ )
+
+ add_library(${LIB_NAME} SHARED ${LIB_FILES} ${LIB_HEADERS})
+
+ library_settings("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_HEADERS}")
+ target_link_ice(${LIB_NAME})
+ target_link_libraries(${LIB_NAME} ${LIBS})
+endif()
+
diff --git a/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp b/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp
new file mode 100644
index 000000000..fe01e4fd8
--- /dev/null
+++ b/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp
@@ -0,0 +1,294 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package RobotAPI::GraspingWithTorques
+* @author Markus Przybylski
+* @date 2014 Markus Przybylski
+
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#include "GraspingWithTorques.h"
+//#include "Armar3GraspContext.h" //MP: Maybe necessary again later?
+#include "../core/RobotStatechartContext.h" //MP 2014-01-21
+
+//#include "../../units/KinematicUnitHand/KinematicUnitHand.h"
+
+#include <Core/observers/variant/ChannelRef.h>
+#include <Core/observers/variant/SingleTypeVariantList.h>
+#include <Core/robotstate/remote/ArmarPose.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+
+#include <Core/interface/units/KinematicUnitInterface.h>
+
+#include <RobotAPI/core/RobotStatechartContext.h>
+
+namespace armarx
+{
+ // ****************************************************************
+ // Implementation of StatechartGraspingWithTorques
+ // ****************************************************************
+ void StatechartGraspingWithTorques::defineParameters()
+ {
+ //preshape settings:
+ addToInput("jointAnglesPreshape", VariantType::List(VariantType::Float), false);
+ //how long to wait before switching from preshape to grasp
+ addToInput("timeoutPreshape", VariantType::Float, false);
+
+ //closeHandWithTorques settings:
+ addToInput("jointTorquesGrasp", VariantType::List(VariantType::Float), false);
+ //Make sure that the joints have time to start moving before we check
+ //low joint velocities as a termination criterion.
+ addToInput("timeoutMinExecutionTime", 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);
+ addToInput("thresholdVelocity", VariantType::Float, false);
+
+
+ addToInput("robotNodeSetName", VariantType::String, false); //RobotNodeSet: LeftHand (auskommentarisiert MP 2014-01-21)
+
+ addToLocal("jointNames", VariantType::List(VariantType::String));
+ //addToLocal("jointNames", SingleTypeVariantList(VariantType::String)); //Geht das? (2014-01-21)
+
+ addToLocal("jointVelocityChannel", VariantType::ChannelRef);
+ //addToLocal("jointVelocityChannel", VariantType::ChannelRef, false);
+
+ }
+
+ void StatechartGraspingWithTorques::defineSubstates()
+ {
+ StatePtr statePreshape = addState<StatePreshape>("statePreshape");
+ StatePtr stateCloseHandWithTorques = addState<StateCloseHandWithTorques>("stateCloseHandWithTorques");
+ StatePtr stateInstallTerminateConditions = addState<StateInstallTerminateConditions>("stateInstallTerminateConditions");
+ StatePtr stateSuccess = addState<SuccessState>("stateSuccess");
+ StatePtr stateFailure = addState<FailureState>("stateFailure");
+
+ ParameterMappingPtr mapPreshapeInfo = ParameterMapping::createMapping()
+ ->mapFromParent("jointAnglesPreshape", "jointAnglesPreshape")
+ ->mapFromParent("timeoutPreshape", "timeoutPreshape")
+ ->mapFromParent("jointNames", "jointNames");
+
+ ParameterMappingPtr mapCloseHandWithTorquesInfo = ParameterMapping::createMapping()
+ ->mapFromParent("jointTorquesGrasp", "jointTorquesGrasp")
+ ->mapFromParent("timeoutMinExecutionTime", "timeoutMinExecutionTime")
+ ->mapFromParent("jointNames", "jointNames");
+
+ ParameterMappingPtr mapInstallTerminateConditionsInfo = ParameterMapping::createMapping()
+ ->mapFromParent("timeoutGrasp", "timeoutGrasp")
+ ->mapFromParent("thresholdVelocity", "thresholdVelocity")
+ ->mapFromParent("jointNames")
+ ->mapFromParent("jointVelocityChannel"); //bei gleichem Namen muss man ihn nur einmal angeben
+
+ setInitState(statePreshape, mapPreshapeInfo);
+
+ //transitions
+ addTransition<EvPreshapeTimeout>(statePreshape, stateCloseHandWithTorques, mapCloseHandWithTorquesInfo);
+ addTransition<EvMinExecutionTimeout>(stateCloseHandWithTorques, stateInstallTerminateConditions, mapInstallTerminateConditionsInfo);
+ addTransition<EvGraspTimeout>(stateInstallTerminateConditions, stateFailure);
+ addTransition<EvAllJointVelocitiesLow>(stateInstallTerminateConditions, stateSuccess);
+
+ }
+
+ void StatechartGraspingWithTorques::onEnter()
+ {
+ ARMARX_LOG << eINFO << "Entering StatechartGraspingWithTorques";
+
+ RobotStatechartContext* context = getContext<RobotStatechartContext>();
+ setLocal("jointVelocityChannel", context->getChannelRef(context->getKinematicUnitObserverName(),"jointvelocities"));
+
+ VirtualRobot::RobotNodeSetPtr nodeSet = context->remoteRobot->getRobotNodeSet(getInput<std::string>("robotNodeSetName")); //woher robotNodeSetName holen? (Argument für getRobotNodeSet())
+
+ SingleTypeVariantList jointNames(VariantType::String);
+
+ for (int i=0; i<nodeSet->getSize(); i++)
+ {
+ jointNames.addVariant(nodeSet->getNode(i)->getName()); //nodes = joints
+ }
+
+ setLocal("jointNames",jointNames); //befüllen...!?
+ }
+
+ void StatechartGraspingWithTorques::onExit()
+ {
+
+ ARMARX_LOG << eINFO << "Exiting StatechartGraspingWithTorques...";
+
+ }
+
+ // ****************************************************************
+ // Implementation of StatePreshape
+ // ****************************************************************
+
+ void StatePreshape::defineParameters()
+ {
+ addToInput("jointAnglesPreshape", VariantType::List(VariantType::Float), false);
+ addToInput("timeoutPreshape", VariantType::Float, false);
+
+ //addToLocal("jointNames", VariantType::List(VariantType::String)); //ACHTUNG: addToLocal() oder addToInput()? (2013-01-21)
+ addToInput("jointNames", VariantType::List(VariantType::String), false); //ACHTUNG: addToLocal() oder addToInput()? (2013-01-21)
+ }
+
+ void StatePreshape::onEnter()
+ {
+ ARMARX_LOG << eIMPORTANT << "Entering statePreshape";
+
+ RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
+ SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
+ SingleTypeVariantListPtr jointAnglesPreshapeList = getInput<SingleTypeVariantList>("jointAnglesPreshape");
+ //NameValueMap jointAnglesPreshapeMap;
+ NameValueMap jointNamesAndValues;
+
+ if (jointNames->getSize() == jointAnglesPreshapeList->getSize())
+ {
+ for (int j=0; j<jointNames->getSize(); j++)
+ {
+ jointNamesAndValues[jointNames->getVariant(j)->getString()]=jointAnglesPreshapeList->getVariant(j)->getFloat();
+ }
+ }
+ else
+ throw LocalException("StatePreshape: List lengths do not match!");
+
+ //ARMARX_INFO << "node.size()=" << nodes.size() << flush;
+ //ARMARX_INFO << "jointAnglesPreshapeList.size()=" << jointAnglesPreshapeList->getSize() << flush;
+
+ rsContext->kinematicUnitPrx->setJointAngles(jointNamesAndValues);
+
+ ARMARX_LOG << eINFO << "Installing preshape timeout condition";
+ float timeoutPreshape = getInput<float>("timeoutPreshape");
+ condPreshapeTimeout = setTimeoutEvent(timeoutPreshape, createEvent<EvPreshapeTimeout>());
+
+ ARMARX_LOG << eINFO << "Done onEnter()";
+ }
+
+ void StatePreshape::onExit()
+ {
+ removeTimeoutEvent(condPreshapeTimeout);
+ ARMARX_LOG << eIMPORTANT << "StatePreshape: Done onExit()";
+ }
+
+ // ****************************************************************
+ // Implementation of StateCloseHandWithTorques
+ // ****************************************************************
+
+ void StateCloseHandWithTorques::defineParameters()
+ {
+ //closeHandWithTorques settings:
+ addToInput("jointTorquesGrasp", VariantType::List(VariantType::Float), false);
+ addToInput("timeoutMinExecutionTime", VariantType::Float, false);
+
+ addToInput("jointNames", VariantType::List(VariantType::String), false);
+ }
+
+ void StateCloseHandWithTorques::onEnter()
+ {
+ ARMARX_LOG << eIMPORTANT << "Entering stateCloseHandWithTorques";
+
+ RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
+ SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
+ SingleTypeVariantListPtr jointTorquesGraspList = getInput<SingleTypeVariantList>("jointTorquesGrasp");
+ NameValueMap jointNamesAndValues;
+
+ if (jointNames->getSize() == jointTorquesGraspList->getSize())
+ {
+ for (int j=0; j<jointNames->getSize(); j++)
+ {
+ jointNamesAndValues[jointNames->getVariant(j)->getString()]=jointTorquesGraspList->getVariant(j)->getFloat();
+ }
+ }
+ else
+ throw LocalException("StateCloseHandWithTorques: List lengths do not match!");
+
+ rsContext->kinematicUnitPrx->setJointTorques(jointNamesAndValues);
+
+ ARMARX_LOG << eINFO << "Installing timeoutMinExecutionTime condition";
+ float timeoutMinExecutionTime = getInput<float>("timeoutMinExecutionTime");
+ condMinimumExecutionTimeout = setTimeoutEvent(timeoutMinExecutionTime, createEvent<EvMinExecutionTimeout>());
+
+ ARMARX_LOG << eIMPORTANT << "Done onEnter()";
+ }
+
+ void StateCloseHandWithTorques::onExit()
+ {
+ removeTimeoutEvent(condMinimumExecutionTimeout);
+ }
+
+ // ****************************************************************
+ // Implementation of StateInstallTerminateConditions
+ // ****************************************************************
+
+ void StateInstallTerminateConditions::defineParameters()
+ {
+ //installTerminateConditions settings:
+ //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("jointNames", VariantType::List(VariantType::String), false);
+
+ //addToLocal("jointVelocityChannel", VariantType::ChannelRef);
+ addToInput("jointVelocityChannel", VariantType::ChannelRef, false);
+ }
+
+ void StateInstallTerminateConditions::onEnter()
+ {
+ ARMARX_LOG << eIMPORTANT << "onEnter() stateInstallTerminateConditions";
+
+ ARMARX_LOG << eINFO << "Installing timeoutGrasp condition";
+ float timeoutGrasp = getInput<float>("timeoutGrasp");
+ condGraspTimeout = setTimeoutEvent(timeoutGrasp, createEvent<EvMinExecutionTimeout>());
+
+ ARMARX_LOG << eINFO << "Installing allJointVelocitiesLow condition";
+ float thresholdVelocity = getInput<float>("thresholdVelocity");
+
+ /*
+ Literal JointVelocityLowJoint0(getInput<ChannelRef>("jointVelocityChannel")
+ ->getDataFieldIdentifier(getVariant->getString()), "smaller", Literal::createParameterList(thresholdVelocity)); //siehe SingleTypeVariantList, Auslesen mit getInput()
+ Literal JointVelocityLowJoint1(getInput<ChannelRef>(markerChannelName)
+ ->getDataFieldIdentifier("localized"), "smaller", Literal::createParameterList(thresholdVelocity));
+ */
+
+ //Term allJointVelocitiesLow = JointVelocityLowJoint0 && JointVelocityLowJoint1; //TODO: weitere Literale
+ //condAllJointVelocitiesLow = installCondition(allJointVelocitiesLow, createEvent<EvAllJointVelocitiesLow>());
+
+ //TODO: korrekter ChannelName für Hand-/Fingergelenke? Korrekter DatafieldIdentifier?
+ //TODO: Condition zusammenbauen in einer for-Schleife (siehe ArmarX-Doku zu Conditions...)
+ //TODO: korrekte Parametrierung von getDataFieldIdentifier(), mit getInput()...
+
+ Term allJointVelocitiesLow;
+ SingleTypeVariantListPtr jointNamesList = getInput<SingleTypeVariantList>("jointNames");
+ for (int i=0; i<jointNamesList->getSize(); i++)
+ {
+ allJointVelocitiesLow = allJointVelocitiesLow && Literal(getInput<ChannelRef>("jointVelocityChannel")
+ ->getDataFieldIdentifier(jointNamesList->getVariant(i)->getString()), "smaller", Literal::createParameterList(thresholdVelocity));
+ }
+
+ condAllJointVelocitiesLow = installCondition(allJointVelocitiesLow, createEvent<EvAllJointVelocitiesLow>());
+
+ }
+
+ void StateInstallTerminateConditions::onExit()
+ {
+ ARMARX_LOG << eIMPORTANT << "onExit() stateInstallTerminateConditions";
+ removeTimeoutEvent(condGraspTimeout);
+ removeCondition(condAllJointVelocitiesLow);
+
+ }
+
+}
+
diff --git a/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.h b/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.h
new file mode 100644
index 000000000..998511502
--- /dev/null
+++ b/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.h
@@ -0,0 +1,114 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package RobotAPI::GraspingWithTorques
+* @author Markus Przybylski
+* @date 2014 Markus Przybylski
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#ifndef ARMARX_COMPONENT_GRASPINT_WITH_TORQUES_H
+#define ARMARX_COMPONENT_GRASPINT_WITH_TORQUES_H
+
+#include <Core/statechart/Statechart.h>
+
+
+namespace armarx
+{
+ // ****************************************************************
+ // Events
+ // ****************************************************************
+
+ //only for first tests
+ DEFINEEVENT(EvPreshapeTimeout)
+ DEFINEEVENT(EvMinExecutionTimeout)
+ DEFINEEVENT(EvGraspTimeout)
+ DEFINEEVENT(EvAllJointVelocitiesLow)
+
+ // ****************************************************************
+ // Definition of StatechartGraspingWithTorques
+ // ****************************************************************
+ struct StatechartGraspingWithTorques :
+ StateTemplate<StatechartGraspingWithTorques>
+ {
+ void defineParameters();
+ void defineSubstates();
+ void onEnter();
+ void onExit();
+
+ };
+
+
+
+ // ****************************************************************
+ // Definition of StatePreshape
+ // ****************************************************************
+ /**
+ * StatePreshape: Move the fingers to a preshape configuration, then wait a little.
+ */
+
+ struct StatePreshape :
+ StateTemplate<StatePreshape>
+ {
+ void defineParameters();
+ void onEnter();
+ void onExit();
+
+ StateUtility::ActionEventIdentifier condPreshapeTimeout;
+ };
+
+ // ****************************************************************
+ // Definition of StateCloseHandWithTorques
+ // ****************************************************************
+
+ /**
+ * StateCloseHandWithTorques: Put torques on the hand's joints.
+ */
+ struct StateCloseHandWithTorques :
+ StateTemplate<StateCloseHandWithTorques>
+ {
+ 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 condMinimumExecutionTimeout;
+ };
+
+ // ****************************************************************
+ // Definition of StateInstallTerminateConditions
+ // ****************************************************************
+ /**
+ * StateInstallTerminateConditions:
+ */
+
+ struct StateInstallTerminateConditions :
+ StateTemplate<StateInstallTerminateConditions>
+ {
+ void defineParameters();
+ void onEnter();
+ void onExit();
+
+ ConditionIdentifier condAllJointVelocitiesLow;
+ StateUtility::ActionEventIdentifier condGraspTimeout;
+ };
+
+
+}
+
+#endif
--
GitLab