diff --git a/source/RobotAPI/statecharts/deprecated/OpenHand/CMakeLists.txt b/source/RobotAPI/statecharts/deprecated/OpenHand/CMakeLists.txt
deleted file mode 100644
index 61b37547f5b78930e6f6c7109ce170bbdd425f52..0000000000000000000000000000000000000000
--- a/source/RobotAPI/statecharts/deprecated/OpenHand/CMakeLists.txt
+++ /dev/null
@@ -1,26 +0,0 @@
-armarx_set_target("RobotAPI Library: OpenHand")
-
-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 (Eigen3_FOUND AND Simox_FOUND)
- include_directories(
- ${Eigen3_INCLUDE_DIR}
- ${Simox_INCLUDE_DIRS})
-endif()
-
-set(LIB_NAME OpenHand)
-set(LIB_VERSION 0.1.0)
-set(LIB_SOVERSION 0)
-
-set(LIBS RobotAPICore ArmarXCoreObservers)
-
-set(LIB_FILES OpenHand.cpp
- )
-set(LIB_HEADERS OpenHand.h
- )
-
-armarx_add_library("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/statecharts/deprecated/OpenHand/OpenHand.cpp b/source/RobotAPI/statecharts/deprecated/OpenHand/OpenHand.cpp
deleted file mode 100644
index af03e8db22284cb3448c9ad7b8f4e47c78c35b91..0000000000000000000000000000000000000000
--- a/source/RobotAPI/statecharts/deprecated/OpenHand/OpenHand.cpp
+++ /dev/null
@@ -1,164 +0,0 @@
-/*
-* 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::OpenHand
-* @author Markus Przybylski
-* @date 2014 Markus Przybylski
-
-* @copyright http://www.gnu.org/licenses/gpl.txt
-* GNU General Public License
-*/
-
-#include "OpenHand.h"
-#include <RobotAPI/libraries/core/RobotStatechartContext.h>
-
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include <RobotAPI/libraries/core/RobotStatechartContext.h>
-
-
-#include <Core/observers/variant/ChannelRef.h>
-#include <Core/observers/variant/DatafieldRef.h>
-#include <Core/observers/variant/SingleTypeVariantList.h>
-
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Robot.h>
-
-namespace armarx
-{
- // ****************************************************************
- // Implementation of StatechartOpenHand
- // ****************************************************************
- void StatechartOpenHand::defineParameters()
- {
- //openHand settings:
- addToInput("jointAnglesOpenHand", VariantType::List(VariantType::Float), false);
- addToInput("timeoutOpenHandSuccess", VariantType::Float, false);
- addToInput("timeoutOpenHandDummyFailure", VariantType::Float, false);
-
- addToInput("robotNodeSetName", VariantType::String, false);
- addToLocal("jointNames", VariantType::List(VariantType::String));
-
- //addToLocal("jointVelocitiesDatafields", VariantType::List(VariantType::DatafieldRef));
-
- }
-
- void StatechartOpenHand::defineSubstates()
- {
- StatePtr stateOpenHand = addState<StateOpenHand>("stateOpenHand");
- StatePtr stateSuccess = addState<SuccessState>("stateSuccess");
- StatePtr stateFailure = addState<FailureState>("stateFailure");
-
- ParameterMappingPtr mapOpenHandInfo = ParameterMapping::createMapping()
- ->mapFromParent("jointAnglesOpenHand", "jointAnglesOpenHand")
- ->mapFromParent("timeoutOpenHandSuccess", "timeoutOpenHandSuccess")
- ->mapFromParent("timeoutOpenHandDummyFailure", "timeoutOpenHandDummyFailure")
- ->mapFromParent("jointNames", "jointNames");
-
- setInitState(stateOpenHand, mapOpenHandInfo);
-
- //transitions
- addTransition<EvOpenHandSuccessTimeout>(stateOpenHand, stateSuccess);
- addTransition<EvOpenHandDummyFailureTimeout>(stateOpenHand, stateFailure); //Do we need to include a failure state?
-
- }
-
- void StatechartOpenHand::onEnter()
- {
- ARMARX_LOG << eINFO << "Entering StatechartOpenHand";
-
- RobotStatechartContext* context = getContext<RobotStatechartContext>();
- //setLocal("jointVelocityChannel", context->getChannelRef(context->getKinematicUnitObserverName(),"jointvelocities"));
- ChannelRefPtr tempChannelRef = 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);
- SingleTypeVariantList dataFields(VariantType::DatafieldRef);
-
-
- for (size_t i=0; i<nodeSet->getSize(); i++)
- {
- jointNames.addVariant(nodeSet->getNode(i)->getName()); //nodes = joints
- dataFields.addVariant(context->getDatafieldRef(tempChannelRef, nodeSet->getNode(i)->getName()));
- }
-
- setLocal("jointNames", jointNames);
- setLocal("jointVelocitiesDatafields", dataFields);
-
- }
-
- void StatechartOpenHand::onExit()
- {
-
- ARMARX_LOG << eINFO << "Exiting StatechartOpenHand...";
-
- }
-
- // ****************************************************************
- // Implementation of StateOpenHand
- // ****************************************************************
-
- void StateOpenHand::defineParameters()
- {
- addToInput("jointAnglesOpenHand", VariantType::List(VariantType::Float), false);
- addToInput("timeoutOpenHandSuccess", VariantType::Float, false);
-
- addToInput("jointNames", VariantType::List(VariantType::String), false);
- //addToInput("useTorquesForGrasping", VariantType::Int, false);
- }
-
- void StateOpenHand::onEnter()
- {
- ARMARX_LOG << eIMPORTANT << "Entering StateOpenHand";
-
- RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
- SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
- SingleTypeVariantListPtr jointAnglesOpenHandList = getInput<SingleTypeVariantList>("jointAnglesOpenHand");
- NameValueMap jointNamesAndValues;
-
- if (jointNames->getSize() == jointAnglesOpenHandList->getSize())
- {
- for (int j=0; j<jointNames->getSize(); j++)
- {
- jointNamesAndValues[jointNames->getVariant(j)->getString()]=jointAnglesOpenHandList->getVariant(j)->getFloat();
- }
- }
- else
- throw LocalException("StateOpenHand: List lengths do not match!");
-
- rsContext->kinematicUnitPrx->setJointAngles(jointNamesAndValues);
-
- ARMARX_LOG << eINFO << "Installing openHand timeout condition";
- float timeoutOpenHandSuccess = getInput<float>("timeoutOpenHandSuccess");
-
- condOpenHandSuccessTimeout = setTimeoutEvent(timeoutOpenHandSuccess, createEvent<EvOpenHandSuccessTimeout>());
- condOpenHandDummyFailureTimeout = setTimeoutEvent(timeoutOpenHandSuccess, createEvent<EvOpenHandDummyFailureTimeout>());
-
- ARMARX_LOG << eINFO << "StateOpenHand: Done onEnter()";
- }
-
- void StateOpenHand::onExit()
- {
- removeTimeoutEvent(condOpenHandSuccessTimeout);
- removeTimeoutEvent(condOpenHandDummyFailureTimeout);
- ARMARX_LOG << eIMPORTANT << "StateOpenHand: Done onExit()";
- }
-
-
-
-}
-
diff --git a/source/RobotAPI/statecharts/deprecated/OpenHand/OpenHand.h b/source/RobotAPI/statecharts/deprecated/OpenHand/OpenHand.h
deleted file mode 100644
index 3fb167f12847b5f0365a654d49034a00822923c4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/statecharts/deprecated/OpenHand/OpenHand.h
+++ /dev/null
@@ -1,74 +0,0 @@
-/*
-* 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::OpenHand
-* @author Markus Przybylski
-* @date 2014 Markus Przybylski
-* @copyright http://www.gnu.org/licenses/gpl.txt
-* GNU General Public License
-*/
-
-#ifndef ARMARX_COMPONENT_OPEN_HAND_H
-#define ARMARX_COMPONENT_OPEN_HAND_H
-
-#include <Core/statechart/Statechart.h>
-
-
-namespace armarx
-{
- // ****************************************************************
- // Events
- // ****************************************************************
-
- //only for first tests
- DEFINEEVENT(EvOpenHandSuccessTimeout)
- DEFINEEVENT(EvOpenHandDummyFailureTimeout)
-
-
- // ****************************************************************
- // Definition of StatechartOpenHand
- // ****************************************************************
- struct StatechartOpenHand :
- StateTemplate<StatechartOpenHand>
- {
- void defineParameters();
- void defineSubstates();
- void onEnter();
- void onExit();
- };
-
-
- // ****************************************************************
- // Definition of StateOpenHand
- // ****************************************************************
- /**
- * StateOpenHand: Move the fingers to a open configuration, then wait a little.
- */
-
- struct StateOpenHand :
- StateTemplate<StateOpenHand>
- {
- void defineParameters();
- void onEnter();
- void onExit();
-
- StateUtility::ActionEventIdentifier condOpenHandSuccessTimeout;
- StateUtility::ActionEventIdentifier condOpenHandDummyFailureTimeout;
- };
-
-}
-
-#endif
diff --git a/source/RobotAPI/statecharts/deprecated/graspingwithtorques/CMakeLists.txt b/source/RobotAPI/statecharts/deprecated/graspingwithtorques/CMakeLists.txt
deleted file mode 100644
index 62eb4d0ceb609395d938c49797cc634424f64e6a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/statecharts/deprecated/graspingwithtorques/CMakeLists.txt
+++ /dev/null
@@ -1,26 +0,0 @@
-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 (Eigen3_FOUND AND Simox_FOUND)
- include_directories(
- ${Eigen3_INCLUDE_DIR}
- ${Simox_INCLUDE_DIRS})
-endif()
-
-set(LIB_NAME GraspingWithTorques)
-set(LIB_VERSION 0.1.0)
-set(LIB_SOVERSION 0)
-
-set(LIBS RobotAPICore ArmarXCoreObservers ${Simox_LIBRARIES})
-
-set(LIB_FILES GraspingWithTorques.cpp
- )
-set(LIB_HEADERS GraspingWithTorques.h
- )
-
-armarx_add_library("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/statecharts/deprecated/graspingwithtorques/GraspingWithTorques.cpp b/source/RobotAPI/statecharts/deprecated/graspingwithtorques/GraspingWithTorques.cpp
deleted file mode 100644
index f35b67278d26deb004a874b51f0f5510561bfa1c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/statecharts/deprecated/graspingwithtorques/GraspingWithTorques.cpp
+++ /dev/null
@@ -1,409 +0,0 @@
-/*
-* 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 <RobotAPI/libraries/core/RobotStatechartContext.h>
-
-#include <Core/observers/variant/ChannelRef.h>
-#include <Core/observers/variant/DatafieldRef.h>
-#include <Core/observers/variant/SingleTypeVariantList.h>
-#include <RobotAPI/libraries/core/Pose.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Robot.h>
-
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include <RobotAPI/interface/units/HandUnitInterface.h>
-
-#include <RobotAPI/libraries/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);
- addToInput("useTorquesForGrasping", VariantType::Int, 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);
-
- //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);
- addToInput("thresholdVelocity", VariantType::Float, false);
- addToInput("handUnitName", VariantType::String, false);
-
-
- addToInput("robotNodeSetName", VariantType::String, false);
-
- addToLocal("jointNames", VariantType::List(VariantType::String));
-
- //addToLocal("jointVelocityChannel", VariantType::ChannelRef); //first try ...
-
- addToLocal("jointVelocitiesDatafields", VariantType::List(VariantType::DatafieldRef));
-
- }
-
- void StatechartGraspingWithTorques::defineSubstates()
- {
- 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");
-
- ParameterMappingPtr mapPreshapeInfo = ParameterMapping::createMapping()
- ->mapFromParent("jointAnglesPreshape", "jointAnglesPreshape")
- ->mapFromParent("timeoutPreshape", "timeoutPreshape")
- ->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")
- ->mapFromParent("jointNames")
- ->mapFromParent("handUnitName", "handUnitName")
- ->mapFromParent("jointVelocitiesDatafields");
- //->mapFromParent("jointVelocityChannel"); //first try...
-
- setInitState(statePreshape, mapPreshapeInfo);
-
- //transitions
- addTransition<EvPreshapeTimeout_ToCloseWithTorques>(statePreshape, stateCloseHandWithTorques, mapCloseHandWithTorquesInfo);
- addTransition<EvPreshapeTimeout_ToCloseWithJointAngles>(statePreshape, stateCloseHandWithJointAngles, mapCloseHandWithJointAnglesInfo);
- addTransition<EvMinExecutionTimeout>(stateCloseHandWithTorques, stateInstallTerminateConditions, mapInstallTerminateConditionsInfo);
- //addTransition<EvGraspWithTorquesTimeout>(stateInstallTerminateConditions, stateSuccess);
- addTransition<EvMinExecutionTimeout>(stateInstallTerminateConditions, stateSuccess); // timeout is ok for now....
- addTransition<EvAllJointVelocitiesLow>(stateInstallTerminateConditions, stateSuccess);
- addTransition<EvGraspWithJointAnglesTimeout>(stateCloseHandWithJointAngles, stateSuccess);
-
- }
-
- void StatechartGraspingWithTorques::onEnter()
- {
- ARMARX_LOG << eINFO << "Entering StatechartGraspingWithTorques";
-
- RobotStatechartContext* context = getContext<RobotStatechartContext>();
- //setLocal("jointVelocityChannel", context->getChannelRef(context->getKinematicUnitObserverName(),"jointvelocities"));
- ChannelRefPtr tempChannelRef = 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);
- SingleTypeVariantList dataFields(VariantType::DatafieldRef);
-
-
- for (size_t i=0; i<nodeSet->getSize(); i++)
- {
- jointNames.addVariant(nodeSet->getNode(i)->getName()); //nodes = joints
- dataFields.addVariant(context->getDatafieldRef(tempChannelRef, nodeSet->getNode(i)->getName()));
- }
-
- setLocal("jointNames", jointNames);
- setLocal("jointVelocitiesDatafields", dataFields);
-
- }
-
- 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));
- addToInput("jointNames", VariantType::List(VariantType::String), false);
- addToInput("useTorquesForGrasping", VariantType::Int, false);
- }
-
- void StatePreshape::onEnter()
- {
- ARMARX_LOG << eIMPORTANT << "Entering statePreshape";
-
- RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
- SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
- SingleTypeVariantListPtr jointAnglesPreshapeList = getInput<SingleTypeVariantList>("jointAnglesPreshape");
- 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!");
-
- rsContext->kinematicUnitPrx->setJointAngles(jointNamesAndValues);
-
- ARMARX_LOG << eINFO << "Installing preshape timeout condition";
- float timeoutPreshape = getInput<float>("timeoutPreshape");
-
- 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 StatePreshape::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 StateCloseHandWithTorques::onEnter()";
- }
-
- void StateCloseHandWithTorques::onExit()
- {
- 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;
- NameControlModeMap controlModes;
-
- if (jointNames->getSize() == jointAnglesGraspList->getSize())
- {
- for (int j=0; j<jointNames->getSize(); j++)
- {
- jointNamesAndValues[jointNames->getVariant(j)->getString()]=jointAnglesGraspList->getVariant(j)->getFloat();
- controlModes[jointNames->getVariant(j)->getString()] = ePositionControl;
- }
- }
- else
- throw LocalException("stateCloseHandWithJointAngles: List lengths do not match!");
-
- rsContext->kinematicUnitPrx->switchControlMode(controlModes);
- rsContext->kinematicUnitPrx->setJointAngles(jointNamesAndValues);
-
- ARMARX_LOG << eINFO << "Installing timeoutGrasp condition";
- float timeoutGrasp = getInput<float>("timeoutGrasp");
- condGraspWithJointAnglesTimeout = setTimeoutEvent(timeoutGrasp, createEvent<EvGraspWithJointAnglesTimeout>());
-
- ARMARX_LOG << eIMPORTANT << "Done B " << BOOST_CURRENT_FUNCTION;
- }
-
- void StateCloseHandWithJointAngles::onExit()
- {
- removeTimeoutEvent(condGraspWithJointAnglesTimeout);
- }
-
-
- // ****************************************************************
- // 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);
-
- //addToInput("jointVelocityChannel", VariantType::ChannelRef, false); //first try ...
-
- addToInput("jointVelocitiesDatafields", VariantType::List(VariantType::DatafieldRef), false);
- addToInput("handUnitName", VariantType::String, 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>());
-
- float thresholdVelocity = getInput<float>("thresholdVelocity");
- ARMARX_LOG << eINFO << "Installing allJointVelocitiesLow condition, threshold: " << thresholdVelocity << flush;
-
- //=======
- //first try ... (with ChannelRef)
- //=======
-
- /*
- 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>());
- */
-
- //=======
- //second try ... (with DatafieldRef)
- //=======
- Term allJointVelocitiesLow_NEW;
- //SingleTypeVariantListPtr jointNamesList = getInput<SingleTypeVariantList>("jointNames");
- SingleTypeVariantListPtr dataFieldsList = getInput<SingleTypeVariantList>("jointVelocitiesDatafields");
-
- //for (int i=0; i<jointNamesList->getSize(); i++)
- for (int i=0; i<dataFieldsList->getSize(); i++)
- {
- allJointVelocitiesLow_NEW = allJointVelocitiesLow_NEW &&
- Literal(dataFieldsList->getVariant(i)->get<DatafieldRef>()->getDataFieldIdentifier(),
- "inrange", Literal::createParameterList(-thresholdVelocity, thresholdVelocity));
- }
-
- condAllJointVelocitiesLow = installCondition(allJointVelocitiesLow_NEW, createEvent<EvAllJointVelocitiesLow>());
-
- }
-
- void StateInstallTerminateConditions::onExit()
- {
- ARMARX_LOG << eIMPORTANT << "onExit() stateInstallTerminateConditions";
- removeTimeoutEvent(condGraspTimeout);
- removeCondition(condAllJointVelocitiesLow);
-
- //---------
- //set joint velocities manually to zero (DEBUG)
- //---------
-
- std::string handUnitName = getInput<std::string>("handUnitName");
- ARMARX_LOG << eIMPORTANT << "xx..................SENDING OBJECTGRASPED TO hand unit name " << handUnitName << "...................";
-
- RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
- if (rsContext->getHandUnit(handUnitName))
- {
- ARMARX_LOG << eIMPORTANT << "xx..................SENDING OBJECTGRASPED TO hand unit name " << handUnitName << ".......OK: sending Vitalis............";
- rsContext->getHandUnit(handUnitName)->setObjectGrasped("Vitalis");
- }
-
- SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
- //SingleTypeVariantListPtr jointTorquesGraspList = getInput<SingleTypeVariantList>("jointTorquesGrasp");
- NameValueMap jointNamesAndValues;
- NameControlModeMap controlModes;
-
- for (int j=0; j<jointNames->getSize(); j++)
- {
- // no, now we set it to zero.... we want to ensure that the object stayes grasped -> apply a small velocity (todo: find a better solution!)
- jointNamesAndValues[jointNames->getVariant(j)->getString()] = 0.0;//1f; //set everything to zero
- controlModes[jointNames->getVariant(j)->getString()] = eVelocityControl;
- }
-
- rsContext->kinematicUnitPrx->switchControlMode(controlModes);
-// rsContext->kinematicUnitPrx->setJointTorques(jointNamesAndValues);
- rsContext->kinematicUnitPrx->setJointVelocities(jointNamesAndValues);
- }
-
-}
-
diff --git a/source/RobotAPI/statecharts/deprecated/graspingwithtorques/GraspingWithTorques.h b/source/RobotAPI/statecharts/deprecated/graspingwithtorques/GraspingWithTorques.h
deleted file mode 100644
index 6b73aeb0f572a0c999dcd153c20d442b3733f8b0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/statecharts/deprecated/graspingwithtorques/GraspingWithTorques.h
+++ /dev/null
@@ -1,136 +0,0 @@
-/*
-* 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_ToCloseWithTorques)
- DEFINEEVENT(EvPreshapeTimeout_ToCloseWithJointAngles)
- DEFINEEVENT(EvMinExecutionTimeout)
- DEFINEEVENT(EvGraspWithJointAnglesTimeout)
- //DEFINEEVENT(EvGraspWithTorquesTimeout)
- 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 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
- // ****************************************************************
- /**
- * StateInstallTerminateConditions:
- */
-
- struct StateInstallTerminateConditions :
- StateTemplate<StateInstallTerminateConditions>
- {
- void defineParameters();
- void onEnter();
- void onExit();
-
- ConditionIdentifier condAllJointVelocitiesLow;
- StateUtility::ActionEventIdentifier condGraspTimeout;
- };
-
-
-}
-
-#endif
diff --git a/source/RobotAPI/statecharts/deprecated/motioncontrol/CMakeLists.txt b/source/RobotAPI/statecharts/deprecated/motioncontrol/CMakeLists.txt
deleted file mode 100644
index 2d1b702d342ec483758c64d00c058bb297f229a5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/statecharts/deprecated/motioncontrol/CMakeLists.txt
+++ /dev/null
@@ -1,28 +0,0 @@
-armarx_set_target("RobotAPI MotionControl Library: MotionControl")
-
-
-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 (Eigen3_FOUND AND Simox_FOUND)
- include_directories(
- ${Eigen3_INCLUDE_DIR}
- ${Simox_INCLUDE_DIRS})
-endif()
-
-set(LIB_NAME MotionControl)
-set(LIB_VERSION 0.1.0)
-set(LIB_SOVERSION 0)
-
-set(LIBS RobotAPICore ArmarXCore ArmarXCoreObservers ${Simox_LIBRARIES})
-
-set(LIB_FILES MotionControl.cpp
- ZeroForceControl.cpp)
-set(LIB_HEADERS MotionControl.h
- ZeroForceControl.h)
-
-armarx_add_library("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-
diff --git a/source/RobotAPI/statecharts/deprecated/motioncontrol/MotionControl.cpp b/source/RobotAPI/statecharts/deprecated/motioncontrol/MotionControl.cpp
deleted file mode 100644
index 2356eb710d12f67b75b57770bc564aa563d49b48..0000000000000000000000000000000000000000
--- a/source/RobotAPI/statecharts/deprecated/motioncontrol/MotionControl.cpp
+++ /dev/null
@@ -1,1120 +0,0 @@
-#include "MotionControl.h"
-
-// Core
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <Core/observers/variant/ChannelRef.h>
-#include <Core/observers/ConditionCheck.h>
-#include <Core/core/system/ArmarXDataPath.h>
-
-// Virtual Robot
-#include <VirtualRobot/IK/GazeIK.h>
-#include <VirtualRobot/IK/GenericIKSolver.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <VirtualRobot/MathTools.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/LinkedCoordinate.h>
-
-
-#include <Eigen/src/Geometry/Quaternion.h>
-
-#include "ZeroForceControl.h"
-
-using namespace armarx;
-using namespace armarx::MotionControl;
-
-MotionControlOfferer::MotionControlOfferer()
-{
-}
-
-
-
-void MotionControlOfferer::onInitRemoteStateOfferer()
-{
- addState<MoveJoints>("MoveJoints");
- //addState<MotionControlTestState>("MotionControlTestState");
- addState<MotionControlTestStateIK>("MotionControlTestStateIK");
- addState<ZeroForceControl>("ZeroForceControl");
-}
-
-
-
-void MotionControlOfferer::onConnectRemoteStateOfferer()
-{
-}
-
-
-
-void MoveJoints::defineParameters()
-{
- context = getContext<RobotStatechartContext>();
-
- //setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
-
- addToInput("jointNames", VariantType::List(VariantType::String), false);
- addToInput("targetJointValues", VariantType::List(VariantType::Float), false);
-
- // TODO: add when we have decided how to do it
- addToInput("jointMaxSpeeds", VariantType::List(VariantType::Float), true);
- //addToInput("jointMaxSpeed", VariantType::List(VariantType::Float), true);
- addToInput("jointMaxSpeed", VariantType::Float, true);
-
- addToInput("jointTargetTolerance", VariantType::Float, false);
- addToInput("timeoutInMs", VariantType::Int, false);
-
- addToOutput("jointDistancesToTargets", VariantType::List(VariantType::Float), true);
-}
-
-
-
-void MoveJoints::onEnter()
-{
- NameValueMap targetJointAngles;
- SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
- SingleTypeVariantListPtr targetJointValues = getInput<SingleTypeVariantList>("targetJointValues");
-
- if (jointNames->getSize()!=targetJointValues->getSize())
- {
- throw LocalException("Sizes of joint name list and joint value list do not match!");
- }
-
- ARMARX_VERBOSE << "number of joints that will be set: " << jointNames->getSize() << flush;
-
- for (int i=0; i<jointNames->getSize(); i++)
- {
- targetJointAngles[jointNames->getVariant(i)->getString()] = targetJointValues->getVariant(i)->getFloat();
- ARMARX_VERBOSE << "setting joint angle for joint '" << jointNames->getVariant(i)->getString() << "' to " << targetJointValues->getVariant(i)->getFloat() << std::endl;
- }
- // TODO: Set Max Velocities
-
- // now install the condition
- Term cond;
- float tolerance = getInput<float>("jointTargetTolerance");
- for (int i=0; i<jointNames->getSize(); i++)
- {
- ARMARX_VERBOSE << "creating condition (" << i << " of " << jointNames->getSize() << ")" << flush;
- ParameterList inrangeCheck;
- inrangeCheck.push_back(new Variant(targetJointValues->getVariant(i)->getFloat()-tolerance));
- inrangeCheck.push_back(new Variant(targetJointValues->getVariant(i)->getFloat()+tolerance));
-
- Literal inrangeLiteral(DataFieldIdentifier(context->getKinematicUnitObserverName(), "jointangles", jointNames->getVariant(i)->getString()), "inrange", inrangeCheck);
- cond = cond && inrangeLiteral;
- }
-
- ARMARX_VERBOSE << "installing condition: EvJointTargetReached" << std::endl;
- targetReachedCondition = installCondition<EvJointTargetReached>(cond);
- ARMARX_VERBOSE << "condition installed: EvJointTargetReached" << std::endl;
- timeoutEvent = setTimeoutEvent(getInput<int>("timeoutInMs"), createEvent<EvTimeout>());
- ARMARX_VERBOSE << "timeout installed" << std::endl;
- context->kinematicUnitPrx->setJointAngles(targetJointAngles);
-}
-
-
-
-void MoveJoints::onBreak()
-{
- removeCondition(targetReachedCondition);
- removeTimeoutEvent(timeoutEvent);
-}
-
-
-
-void MoveJoints::onExit()
-{
- SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
- SingleTypeVariantListPtr targetJointValues = getInput<SingleTypeVariantList>("targetJointValues");
- SingleTypeVariantList resultList(VariantType::Float);
-
- for(int i=0; i<jointNames->getSize(); i++ )
- {
- DataFieldIdentifierPtr datafield = new DataFieldIdentifier(context->getKinematicUnitObserverName(),"jointangles", jointNames->getVariant(i)->getString());
- float jointAngleActual = context->kinematicUnitObserverPrx->getDataField(datafield)->getFloat();
- float jointTargetValue = targetJointValues->getVariant(i)->getFloat();
- resultList.addVariant(Variant(jointAngleActual - jointTargetValue));
- }
- setOutput("jointDistancesToTargets", resultList);
-
- //ARMARX_INFO << "output: " << StateUtilFunctions::getDictionaryString(getOutputParameters()) << std::endl;
-
- removeCondition(targetReachedCondition);
- removeTimeoutEvent(timeoutEvent);
-}
-
-
-
-void MoveJointsVelocityControl::defineParameters()
-{
- context = getContext<RobotStatechartContext>();
-
- //setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
-
- addToInput("jointNames", VariantType::List(VariantType::String), false);
- addToInput("targetJointVelocities", VariantType::List(VariantType::Float), false);
-
- // TODO: add when we have decided how to do it
- addToInput("jointMaxSpeeds", VariantType::List(VariantType::Float), true);
- addToInput("jointMaxSpeed", VariantType::Float, true);
-
- addToInput("accelerationTime", VariantType::Float, false);
-
- addToInput("targetJointVelocityTolerance", VariantType::Float, false);
-
- addToInput("timeoutInMs", VariantType::Int, false);
-
- addToOutput("jointVelocitiesDistancesToTargets", VariantType::Float, true);
-}
-
-
-
-void MoveJointsVelocityControl::onEnter()
-{
- //float accelerationTime = getInput<float>("accelerationTime");
- NameValueMap targetJointVelocities;
- SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
- SingleTypeVariantListPtr targetVelocitiesValues = getInput<SingleTypeVariantList>("targetJointVelocities");
-
- if (jointNames->getSize()!=targetVelocitiesValues->getSize())
- {
- throw LocalException("Sizes of joint name list and joint velocities list do not match!");
- }
-
- for(int i=0; i<jointNames->getSize(); i++)
- {
- targetJointVelocities[jointNames->getVariant(i)->getString()] = targetVelocitiesValues->getVariant(i)->getFloat();
- ARMARX_VERBOSE << "setting joint velocity for joint '" << jointNames->getVariant(i)->getString() << "' to " << targetVelocitiesValues->getVariant(i)->getFloat() << std::endl;
- }
- // TODO: Set Max Velocities
-
- // now install the condition
-
- Term cond;
- float tolerance = getInput<float>("targetJointVelocityTolerance");
- for(int i=0; i<jointNames->getSize(); i++)
- {
- ParameterList inrangeCheck;
- inrangeCheck.push_back(new Variant(targetVelocitiesValues->getVariant(i)->getFloat()-tolerance));
- inrangeCheck.push_back(new Variant(targetVelocitiesValues->getVariant(i)->getFloat()+tolerance));
-
- Literal inrangeLiteral(DataFieldIdentifier(context->getKinematicUnitObserverName(),"jointvelocities", jointNames->getVariant(i)->getString()), "inrange", inrangeCheck);
- cond = cond && inrangeLiteral;
- }
-
- targetVelocitiesReachedCondition = installCondition<EvJointVelocitiesTargetReached>(cond);
- ARMARX_INFO << "condition installed" << std::endl;
- timeoutEvent = setTimeoutEvent(getInput<int>("timeoutInMs"), createEvent<EvTimeout>());
- ARMARX_INFO << "timeout installed" << std::endl;
-// context->kinematicUnitPrx->setAccelerationTime(accelerationTime);
- context->kinematicUnitPrx->setJointVelocities(targetJointVelocities);
-}
-
-
-
-void MoveJointsVelocityControl::onExit()
-{
- SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
- SingleTypeVariantListPtr targetJointVelocities = getInput<SingleTypeVariantList>("targetJointVelocities");
- SingleTypeVariantList resultList(VariantType::Float);
-
- for(int i=0; i<jointNames->getSize(); i++)
- {
- DataFieldIdentifierPtr datafield = new DataFieldIdentifier(context->getKinematicUnitObserverName(),"jointvelocities", jointNames->getVariant(i)->getString());
- float jointVelocityActual = context->kinematicUnitObserverPrx->getDataField(datafield)->getFloat();
- float jointVelocityTarget = targetJointVelocities->getVariant(i)->getFloat();
- resultList.addVariant(Variant(jointVelocityActual - jointVelocityTarget));
- }
- setOutput("jointVelocitiesDistancesToTargets", resultList);
-
- ARMARX_INFO << "output: " << StateUtilFunctions::getDictionaryString(getOutputParameters()) << std::endl;
-
- removeCondition(targetVelocitiesReachedCondition);
- removeTimeoutEvent(timeoutEvent);
-}
-
-
-
-void MoveTCPPoseIK::defineParameters()
-{
- context = getContext<RobotStatechartContext>();
-
- //setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
-
- addToInput("kinematicChainName", VariantType::String, false);
- addToInput("targetTCPPosition", VariantType::FramedPosition, false);
- addToInput("targetTCPOrientation", VariantType::FramedOrientation, false);
-
- addToInput("tcpMaxSpeed", VariantType::Float, true);
-
- addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
- addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
- addToInput("ikWithOrientation", VariantType::Bool, false);
-
- addToInput("timeoutInMs", VariantType::Int, false);
-
- addToInput("jointTargetTolerance", VariantType::Float, false);
-
- addToOutput("TCPDistanceToTarget", VariantType::Float, true);
- addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true);
-}
-
-
-
-void MoveTCPPoseIK::onEnter()
-{
- timeoutEvent = setTimeoutEvent(getInput<int>("timeoutInMs"), createEvent<EvTimeout>());
-}
-
-
-
-void MoveTCPPoseIK::onExit()
-{
- removeTimeoutEvent(timeoutEvent);
-}
-
-
-
-void MoveTCPPoseIK::defineSubstates()
-{
- StatePtr calculateJointAngleConfiguration = addState<CalculateJointAngleConfiguration>("CalculateJointAngleConfiguration");
- StatePtr moveJoints = addState<MoveJoints>("MoveJoints");
- StatePtr validateJointAngleConfiguration = addState<ValidateJointAngleConfiguration>("ValidateJointAngleConfiguration");
- StatePtr movingDone = addState<SuccessState>("movingDone") ;
- StatePtr movingFailed = addState<FailureState>("movingFailed") ;
-
- setInitState(calculateJointAngleConfiguration, createMapping()->mapFromParent("*","*")); // PM::createMapping()->mapFromParent("*","*") ??
- addTransition<EvCalculationDone>(calculateJointAngleConfiguration, moveJoints, createMapping()->mapFromParent("*","*")->mapFromOutput("*","*"));
- addTransition<EvJointTargetReached>(moveJoints, validateJointAngleConfiguration, createMapping()->mapFromParent("*","*")->mapFromOutput("*","*"));
- addTransition<Success>(validateJointAngleConfiguration, movingDone, createMapping()->mapFromOutput("*","*"));
- addTransitionFromAllStates<EvTimeout>(movingFailed);
- addTransitionFromAllStates<Failure>(movingFailed);
-}
-
-
-
-void MoveTCPTrajectory::defineParameters()
-{
- context = getContext<RobotStatechartContext>();
-
- //setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
-
- addToInput("kinematicChainName", VariantType::String, false);
- addToInput("targetTCPPositions", VariantType::List(VariantType::FramedPosition), false);
- addToInput("targetTCPOrientations", VariantType::List(VariantType::FramedOrientation), false);
-
- addToInput("tcpMaxSpeed", VariantType::Float, true);
-
- addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
- addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
- addToInput("ikWithOrientation", VariantType::Bool, false);
-
- addToInput("timeoutInMs", VariantType::Int, false);
-
- addToInput("jointTargetTolerance", VariantType::Float, false);
-
- addToOutput("TCPDistanceToTarget", VariantType::Float, true);
- addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true);
-
- addToLocal("trajectoryPointCounterID", VariantType::ChannelRef);
-}
-
-
-
-void MoveTCPTrajectory::onEnter()
-{
- timeoutEvent = setTimeoutEvent(getInput<int>("timeoutInMs"), createEvent<EvTimeout>());
- int numberOfPoints = getInput<SingleTypeVariantList>("targetTCPPositions")->getSize();
- if (numberOfPoints == 0)
- {
- sendEvent<Success>();
- }
- else if (numberOfPoints != getInput<SingleTypeVariantList>("targetTCPOrientations")->getSize())
- {
- throw armarx::exceptions::local::eStatechartLogicError("the number of targetTCPPositions does not equal the number of targetTCPOrientations");
- sendEvent<Failure>();
- }
- // create a counter to keep track of the poses that were already reached. As we dont use the event, set the max to n+1 so it will never be sent
-
- ARMARX_INFO << "number of points on trajectory: " << numberOfPoints;
- trajectoryPointCounterID = setCounterEvent(numberOfPoints, createEvent<Success>());
- setLocal("trajectoryPointCounterID", Variant(trajectoryPointCounterID.actionId));
-}
-
-
-
-void MoveTCPTrajectory::onExit()
-{
- removeTimeoutEvent(timeoutEvent);
- removeCounterEvent(trajectoryPointCounterID);
-}
-
-
-
-void MoveTCPTrajectory::defineSubstates()
-{
- StatePtr moveTCPTrajectoryInit = addState<MoveTCPTrajectoryInit>("MoveTCPTrajectoryInit");
- StatePtr moveTCPTrajectoryCheckCounter = addState<MoveTCPTrajectoryCheckCounter>("MoveTCPTrajectoryCheckCounter");
- StatePtr moveTCPTrajectoryNextPoint = addState<MoveTCPTrajectoryNextPoint>("MoveTCPTrajectoryNextPoint");
- StatePtr movingDone = addState<SuccessState>("movingDone");
- StatePtr movingFailed = addState<FailureState>("movingFailed");
- setInitState(moveTCPTrajectoryInit, PM::createMapping()->mapFromParent("*","*"));
- addTransition<Success>(moveTCPTrajectoryInit, moveTCPTrajectoryCheckCounter, createMapping()->mapFromParent("*","*")->mapFromOutput("*","*"));
- addTransition<EvLastPointNotYetReached>(moveTCPTrajectoryCheckCounter, moveTCPTrajectoryNextPoint, createMapping()->mapFromParent("*","*")->mapFromOutput("*","*"));
- addTransition<Success>(moveTCPTrajectoryNextPoint, moveTCPTrajectoryCheckCounter, createMapping()->mapFromParent("*","*")->mapFromOutput("*","*"));
- addTransition<EvLastPointReached>(moveTCPTrajectoryCheckCounter, movingDone, createMapping()->mapFromParent("*","*")->mapFromOutput("*","*"));
- addTransitionFromAllStates<EvTimeout>(movingFailed);
- addTransitionFromAllStates<Failure>(movingFailed);
-}
-
-
-
-
-void MoveTCPTrajectoryInit::defineParameters()
-{
-// addToInput("targetTCPPositions", VariantType::List(VariantType::FramedPosition), false);
-// addToOutput("trajectoryPointCounterID", VariantType::ChannelRef, false);
-}
-
-
-
-
-void MoveTCPTrajectoryInit::onEnter()
-{
-
-
- sendEvent<Success>();
-}
-
-
-
-
-void MoveTCPTrajectoryCheckCounter::defineParameters()
-{
- addToInput("targetTCPPositions", VariantType::List(VariantType::FramedPosition), false);
- addToInput("trajectoryPointCounterID", VariantType::ChannelRef, false);
-
-}
-
-
-
-
-void MoveTCPTrajectoryCheckCounter::onEnter()
-{
- context = getContext<RobotStatechartContext>();
- ChannelRefPtr trajectoryPointCounterID = getInput<ChannelRef>("trajectoryPointCounterID");
-
- int counterValue = trajectoryPointCounterID->getDataField("value")->getInt();
-
- ARMARX_INFO << "counter: " << counterValue;
-
- if (counterValue < getInput<SingleTypeVariantList>("targetTCPPositions")->getSize())
- {
- sendEvent<EvLastPointNotYetReached>();
- }
- else
- {
- sendEvent<EvLastPointReached>();
- }
-}
-
-
-
-
-
-void MoveTCPTrajectoryNextPoint::defineParameters()
-{
- addToInput("kinematicChainName", VariantType::String, false);
- addToInput("targetTCPPositions", VariantType::List(VariantType::FramedPosition), false);
- addToInput("targetTCPOrientations", VariantType::List(VariantType::FramedOrientation), false);
-
- addToInput("tcpMaxSpeed", VariantType::Float, true);
-
- addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
- addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
- addToInput("ikWithOrientation", VariantType::Bool, false);
-
- addToInput("timeoutInMs", VariantType::Int, false);
-
- addToInput("trajectoryPointCounterID", VariantType::ChannelRef, false);
-
- addToInput("jointTargetTolerance", VariantType::Float, false);
-
- addToLocal("targetTCPPosition", VariantType::FramedPosition);
- addToLocal("targetTCPOrientation", VariantType::FramedOrientation);
-
-
- addToOutput("TCPDistanceToTarget", VariantType::Float, true);
- addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true);
-}
-
-
-
-void MoveTCPTrajectoryNextPoint::onEnter()
-{
- context = getContext<RobotStatechartContext>();
- ChannelRefPtr trajectoryPointCounterID = getInput<ChannelRef>("trajectoryPointCounterID");
-
- int counterValue = trajectoryPointCounterID->getDataField("value")->getInt();
-
- setLocal("targetTCPPosition", *VariantPtr::dynamicCast(getInput<SingleTypeVariantList>("targetTCPPositions")->getVariant(counterValue)));
- setLocal("targetTCPOrientation", *VariantPtr::dynamicCast(getInput<SingleTypeVariantList>("targetTCPOrientations")->getVariant(counterValue)));
-
- context->systemObserverPrx->incrementCounter(trajectoryPointCounterID);
-
-}
-
-
-
-
-void MoveTCPTrajectoryNextPoint::defineSubstates()
-{
- StatePtr moveTCPPoseIK = addState<MoveTCPPoseIK>("MoveTCPPoseIK");
- StatePtr movingDone = addState<SuccessState>("movingDone") ;
- StatePtr movingFailed = addState<FailureState>("movingFailed") ;
- setInitState(moveTCPPoseIK, PM::createMapping()->mapFromParent("*","*"));
- addTransition<Success>(moveTCPPoseIK, movingDone);
- addTransitionFromAllStates<EvTimeout>(movingFailed);
- addTransitionFromAllStates<Failure>(movingFailed);
-}
-
-
-
-void CalculateJointAngleConfiguration::defineParameters()
-{
- addToInput("kinematicChainName", VariantType::String, false);
- addToInput("targetTCPPosition", VariantType::FramedPosition, false);
- addToInput("targetTCPOrientation", VariantType::FramedOrientation, false);
- addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
- addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
- addToInput("ikWithOrientation", VariantType::Bool, false);
-
- addToOutput("jointNames", VariantType::List(VariantType::String), true);
- addToOutput("targetJointValues", VariantType::List(VariantType::Float), true);
-}
-
-
-
-void CalculateJointAngleConfiguration::run()
-{
- //RobotStatechartContext* context = getContext<RobotStatechartContext>();
- //VirtualRobot::RobotPtr robot(new RemoteRobot(context->robotStateComponent->getRobotSnapshot("CalculateTCPPoseTime")));
-
- // TODO: with the following line, the IK doesn't find a solution, so this terrible hack must be used. Fix it!!!
- //VirtualRobot::RobotPtr robot = robotPtr->clone("CalculateTCPPoseClone");
- //std::string robotModelFile;
- //ArmarXDataPath::getAbsolutePath("Armar4/robotmodel/ArmarIV.xml", robotModelFile);
- //ArmarXDataPath::getAbsolutePath("Armar3/robotmodel/ArmarIII.xml", robotModelFile);
- //VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str());
-
- std::string kinChainName = getInput<std::string>("kinematicChainName");
- float maxError = getInput<float>("targetPositionDistanceTolerance");
- float maxErrorRot = getInput<float>("targetOrientationDistanceTolerance");
- bool withOrientation = getInput<bool>("ikWithOrientation");
- VirtualRobot::IKSolver::CartesianSelection ikType = VirtualRobot::IKSolver::All;
- if (!withOrientation)
- ikType = VirtualRobot::IKSolver::Position;
-
-
- RobotStatechartContext* context = getContext<RobotStatechartContext>();
- if (!context)
- {
- ARMARX_WARNING << "Need a RobotStatechartContext" << flush;
- sendEvent<Failure>();
- }
-
- if (!context->robotStateComponent)
- {
- ARMARX_WARNING << "No RobotStatechartContext->robotStateComponent" << flush;
- sendEvent<Failure>();
- }
-
- VirtualRobot::RobotPtr robot = RemoteRobot::createLocalClone(context->robotStateComponent);
- if (!robot)
- {
- ARMARX_WARNING << "Could not create a local clone of RemoteRobot" << flush;
- sendEvent<Failure>();
- }
- if (!robot->hasRobotNodeSet(kinChainName))
- {
- ARMARX_WARNING << "Robot does not know kinematic chain with name " << kinChainName << flush;
- sendEvent<Failure>();
- }
- ARMARX_INFO << "Setting up ik solver for kin chain: " << kinChainName << ", max position error:" << maxError << ", max orientation erorr " << maxErrorRot << ", useOrientation:" << withOrientation << armarx::flush;
- VirtualRobot::GenericIKSolverPtr ikSolver(new VirtualRobot::GenericIKSolver(robot->getRobotNodeSet(kinChainName), VirtualRobot::JacobiProvider::eSVDDamped));
- ikSolver->setVerbose(true);
- ikSolver->setMaximumError(maxError,maxErrorRot);
- ikSolver->setupJacobian(0.6f, 10);
-
- VirtualRobot::LinkedCoordinate target = FramedPose::createLinkedCoordinate(robot, getInput<FramedPosition>("targetTCPPosition"), getInput<FramedOrientation>("targetTCPOrientation"));
- ARMARX_INFO << "IK target: " << target.getPose() << armarx::flush;
- Eigen::Matrix4f goalInRoot = target.getInFrame(robot->getRootNode());
-
-// // test
-// VirtualRobot::RobotNodePtr rn = robot->getRobotNode("LeftTCP");
-// Eigen::Matrix4f goalTmpTCP = rn->getGlobalPose();
- Eigen::Matrix4f goalGlobal = robot->getRootNode()->toGlobalCoordinateSystem(goalInRoot);
- ARMARX_INFO << "GOAL in root: " << goalInRoot << armarx::flush;
- ARMARX_INFO << "GOAL global: " << goalGlobal << armarx::flush;
-// ARMARX_INFO << "GOAL TCP:" << goalTmpTCP << endl;
-
- ARMARX_INFO << "Calculating IK" << flush;
- //if (!ikSolver->solve(goalGlobal, VirtualRobot::IKSolver::Position, 50))
- if (!ikSolver->solve(goalGlobal, ikType, 5))
- {
- ARMARX_WARNING << "IKSolver found no solution! " << flush;
- sendEvent<Failure>();
- }
- else
- {
-
- SingleTypeVariantList jointNames = SingleTypeVariantList(VariantType::String);
- SingleTypeVariantList targetJointValues = SingleTypeVariantList(VariantType::Float);
- VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(kinChainName);
- for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
- {
- jointNames.addVariant(Variant(kinematicChain->getNode(i)->getName()));
- targetJointValues.addVariant(Variant(kinematicChain->getNode(i)->getJointValue()));
- ARMARX_LOG << " joint: " << kinematicChain->getNode(i)->getName() << " value: " << kinematicChain->getNode(i)->getJointValue() << flush;
- }
- ARMARX_LOG << "number of joints to be set: " << jointNames.getSize() << flush;
- ARMARX_LOG << "setting output: jointNames" << flush;
- setOutput("jointNames", jointNames);
- ARMARX_LOG << "setting output: targetJointValues" << flush;
- setOutput("targetJointValues", targetJointValues);
- sendEvent<EvCalculationDone>();
- }
-}
-
-
-
-void ValidateJointAngleConfiguration::defineParameters()
-{
- addToInput("targetTCPPosition", VariantType::FramedPosition, false);
- addToInput("targetTCPOrientation", VariantType::FramedOrientation, false);
- addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
- addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
- addToInput("ikWithOrientation", VariantType::Bool, false);
- addToInput("kinematicChainName", VariantType::String, false);
-
- addToOutput("TCPDistanceToTarget", VariantType::Float, true);
- addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true);
-}
-
-
-
-void ValidateJointAngleConfiguration::onEnter()
-{
- using namespace Eigen;
-
- RobotStatechartContext* context = getContext<RobotStatechartContext>();
- VirtualRobot::RobotPtr robot(new RemoteRobot(context->robotStateComponent->getRobotSnapshot("ValidateTCPPoseTime")));
- VirtualRobot::LinkedCoordinate actualPose(robot);
- actualPose.set(robot->getRobotNodeSet(getInput<std::string>("kinematicChainName"))->getTCP()->getName(), Vector3f(0,0,0));
- actualPose.changeFrame(robot->getRootNode());
- Vector3f actualPosition = actualPose.getPosition();
- Quaternionf actualOrientation(actualPose.getPose().block<3,3>(0,0));
-
-
- VirtualRobot::LinkedCoordinate targetPose = FramedPose::createLinkedCoordinate(robot, getInput<FramedPosition>("targetTCPPosition"), getInput<FramedOrientation>("targetTCPOrientation"));
- targetPose.changeFrame(robot->getRootNode());
- Vector3f targetPosition = targetPose.getPosition();
- Quaternionf targetOrientation(targetPose.getPose().block<3,3>(0,0));
-
- float cartesianDistance = sqrtf(((targetPosition-actualPosition).dot(targetPosition-actualPosition)));
- float orientationDistance = actualOrientation.angularDistance(targetOrientation);
-
- setOutput("TCPDistanceToTarget", cartesianDistance);
- setOutput("TCPOrientationDistanceToTarget", orientationDistance);
- bool withOri = getInput<bool>("ikWithOrientation");
- if (cartesianDistance <= getInput<float>("targetPositionDistanceTolerance") && (!withOri || orientationDistance <= getInput<float>("targetOrientationDistanceTolerance")))
- {
- sendEvent<Success>();
- }
- else
- {
- ARMARX_WARNING << "Target pose has not been reached with the desired accuracy. Cartesian distance: " << cartesianDistance << ", orientation distance: " << orientationDistance << " ";
- ARMARX_INFO << "Actual pose: " << actualPose.getPose();
- ARMARX_INFO << "Target pose: " << targetPose.getPose();
- sendEvent<Success>();
- }
-}
-
-
-
-void MotionControlTestState::defineParameters()
-{
- //setConfigFile("RobotAPIConfigs/stateconfigs/MotionControl.xml");
-
- addToInput("jointNames", VariantType::List(VariantType::String), false);
- addToInput("targetJointValues", VariantType::List(VariantType::Float), false);
-
- addToInput("jointMaxSpeed", VariantType::Float, true);
-
- addToInput("jointTargetTolerance", VariantType::Float, false);
-
- addToInput("timeoutInMs", VariantType::Int, false);
-
- addToOutput("jointDistancesToTargets", VariantType::Float, true);
-}
-
-
-
-void MotionControlTestState::defineSubstates()
-{
- StatePtr moveJoints = addState<MoveJoints>("MoveJoints");
- StatePtr movingDone = addState<SuccessState>("movingDone");
- StatePtr movingFailed = addState<FailureState>("movingFailed");
-
- setInitState(moveJoints, PM::createMapping()->mapFromParent("*","*"));
-
- addTransition<EvJointTargetReached>(moveJoints, movingDone);
- addTransition<EvTimeout>(moveJoints, movingFailed);
-}
-
-
-
-void MotionControlTestStateIK::defineParameters()
-{
- //setConfigFile("RobotAPIConfigs/stateconfigs/MotionControl.xml");
-
- addToInput("kinematicChainName", VariantType::String, false);
- //addToInput("targetTCPPosition", VariantType::FramedPosition, false);
- //addToInput("targetTCPOrientation", VariantType::FramedOrientation, false);
- addToInput("targetTCPPositions", VariantType::List(VariantType::FramedPosition), false);
- addToInput("targetTCPOrientations", VariantType::List(VariantType::FramedOrientation), false);
-
- addToInput("tcpMaxSpeed", VariantType::Float, true);
-
- addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
- addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
- addToInput("ikWithOrientation", VariantType::Bool, false);
-
- addToInput("timeoutInMs", VariantType::Int, false);
-
-
- addToOutput("TCPDistanceToTarget", VariantType::Float, true);
- addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true);
-}
-
-
-
-void MotionControlTestStateIK::defineSubstates()
-{
- //StatePtr moveTCPPoseIK = addState<MoveTCPPoseIK>("MoveTCPPoseIK");
- StatePtr moveTCPTrajectory = addState<MoveTCPTrajectory>("MoveTCPTrajectory");
- StatePtr movingDone = addState<SuccessState>("movingDone");
- StatePtr movingFailed = addState<FailureState>("movingFailed");
-
- //setInitState(moveTCPPoseIK, PM::createMapping()->mapFromParent("*","*"));
- setInitState(moveTCPTrajectory, PM::createMapping()->mapFromParent("*","*"));
-
- //addTransition<Success>(moveTCPPoseIK, movingDone, PM::createMapping()->mapFromOutput("*","*"));
- //addTransition<EvTimeout>(moveTCPPoseIK, movingFailed);
- //addTransition<Failure>(moveTCPPoseIK, movingFailed);
- addTransition<Success>(moveTCPTrajectory, movingDone, PM::createMapping()->mapFromOutput("*","*"));
- addTransition<EvTimeout>(moveTCPTrajectory, movingFailed);
- addTransition<Failure>(moveTCPTrajectory, movingFailed);
-}
-
-
-
-void MotionControl::StopRobot::onEnter()
-{
- ARMARX_LOG << "entering MotionControl::StopRobot" << flush;
-
- RobotStatechartContext* context = getContext<RobotStatechartContext>();
-
- armarx::NameList allNodes = context->robotStateComponent->getRobotSnapshot("StopRobotTime")->getRobotNodes();
-
- NameValueMap jointVelocities;
- armarx::NameList::iterator it = allNodes.begin();
- for(int i=0; it != allNodes.end(); it++, i++)
- {
- jointVelocities[*it] = 0.0f;
- }
-
- context->kinematicUnitPrx->begin_setJointVelocities(jointVelocities);
-
- sendEvent<Success>();
-}
-
-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(counter, PM::createMapping()->mapFromParent("*"));
- StatePtr success = addState<SuccessState>("SuccessState");
- StatePtr failure = addState<FailureState>("FailureState");
-
- addTransition<Success>(preshape, counter, PM::createMapping()->mapFromParent("*"));
- addTransition<Failure>(preshape, failure, PM::createMapping()->mapFromParent("*"));
- addTransition<EvReachedIntermediatePreshape>(counter, preshape, PM::createMapping()->mapFromParent("*"));
- addTransition<EvReachedFinalPreshape>(counter, success, PM::createMapping()->mapFromParent("*"));
-
-}
-
-
-
-void DoPreshapeSet::onEnter()
-{
- 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);
-}
-
-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<Success>(doPreshape, success);
-
-}
-
-
-
-void SelectAndDoPreshape::onEnter()
-{
- int index = getInput<ChannelRef>("counterRef")->getDataField("value")->getInt();
- ARMARX_IMPORTANT << "index: " << index;
- if(index >= getInput<SingleTypeVariantList>("preshapes")->getSize() )
- {
- setLocal("selectedPreshapeName", std::string(""));
- sendEvent<Failure>();
- }
- else
- {
- 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");
- std::string handUnitName;
- if (useLeftHand)
- {
- handUnitName = "LeftHandUnit";
- }
- else
- {
- handUnitName = "RightHandUnit";
- }
-
- 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->getShapeJointValues(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<Success>(jointCondition,"joint angles reached");
- it->second->setShape(getInput<std::string>("preshapeName"));
- action = setTimeoutEvent(5000, createEvent<Success>());
-
- }
- else
- {
- ARMARX_ERROR << "No Proxy for HandUnit with Name " << handUnitName << " known in RobotStatechartContext - check configFile property 'handUnits'";
- sendEvent<Failure>();
- }
-
-}
-
-void DoPreshape::onExit()
-{
- removeCondition(cond);
- removeTimeoutEvent(action);
-}
-
-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<Success>(doPreshape, success);
-}
-
-void DoPrefixPreshapeSet::defineParameters()
-{
- addToInput("useLeftHand", VariantType::Bool, false);
- addToInput("preshapePrefix", VariantType::String, false);
- addToLocal("preshapes", VariantType::List(VariantType::String));
-}
-
-
-void DoPrefixPreshapeSet::onEnter()
-{
- RobotStatechartContext* context = getContext<RobotStatechartContext>();
-
- bool useLeftHand = getInput<bool>("useLeftHand");
- std::string handUnitName;
- if (useLeftHand)
- {
- handUnitName = "LeftHandUnit";
- }
- else
- {
- handUnitName = "RightHandUnit";
- }
-
- std::map<std::string, HandUnitInterfacePrx>::iterator it = context->handUnits.find(handUnitName);
- if(it != context->handUnits.end())
- {
- setLocal("preshapes", DoPreshape::GetPreshapeSet(SingleTypeVariantListPtr::dynamicCast(it->second->getShapeNames()), getInput<std::string>("preshapePrefix")));
- }
- else
- {
- ARMARX_ERROR << "No Proxy for HandUnit with Name " << handUnitName << " known";
- }
-}
-
-
-
-
-
-void OpenHand::defineSubstates()
-{
- StatePtr doPreshape = addState<DoPrefixPreshapeSet>("DoPrefixPreshapeSet");
- PMPtr mapping = PM::createMapping()->
- mapFromParent("*");
- setInitState(doPreshape, mapping);
- StatePtr success = addState<SuccessState>("SuccessState");
- addTransition<Success>(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>("DoPrefixPreshapeSet");
- PMPtr mapping = PM::createMapping()->
- mapFromParent("*");
- setInitState(doPreshape, mapping);
- StatePtr success = addState<SuccessState>("SuccessState");
- addTransition<Success>(doPreshape, success);
-}
-
-void CloseHand::defineParameters()
-{
- addToInput("useLeftHand", VariantType::Bool, false);
- addToLocal("preshapePrefix", VariantType::String);
-}
-
-void CloseHand::onEnter()
-{
- setLocal("preshapePrefix", "Close");
-
-}
-
-void HeadLookAtTarget::defineParameters()
-{
- addToInput("target", VariantType::FramedPosition, false);
- addToInput("headIKKinematicChainName", VariantType::String, false);
- addToInput("jointTargetTolerance", VariantType::Float, false);
- addToInput("timeoutInMs", VariantType::Int, false);
-}
-
-
-void HeadLookAtTarget::defineSubstates()
-{
- StatePtr stateCalculateHeadIK = addState<CalculateHeadIK>("CalculateHeadIKState");
- StatePtr stateMoveHeadJoints = addState<MoveJoints>("MoveHeadJointsState");
- StatePtr stateSuccess = addState<SuccessState>("Success");
- StatePtr stateFailure = addState<FailureState>("Failure");
-
- ParameterMappingPtr initialMapping = ParameterMapping::createMapping()->mapFromParent("*", "*");
- ParameterMappingPtr transitionMapping = ParameterMapping::createMapping()->mapFromOutput("*", "*")->mapFromParent("*", "*");
-
- setInitState(stateCalculateHeadIK, initialMapping);
- addTransition<EvCalculationDone>(stateCalculateHeadIK, stateMoveHeadJoints, transitionMapping);
- addTransition<EvJointTargetReached>(stateMoveHeadJoints, stateSuccess, transitionMapping);
- addTransitionFromAllStates<Failure>(stateFailure);
- addTransitionFromAllStates<EvTimeout>(stateFailure);
-}
-
-
-
-
-void CalculateHeadIK::defineParameters()
-{
- addToInput("target", VariantType::FramedPosition, false);
- addToInput("headIKKinematicChainName", VariantType::String, false);
-
- addToOutput("jointNames", VariantType::List(VariantType::String), true);
- addToOutput("targetJointValues", VariantType::List(VariantType::Float), true);
-}
-
-
-void CalculateHeadIK::onEnter()
-{
- targetPosition = getInput<FramedPosition>("target");
- kinematicChainName = getInput<std::string>("headIKKinematicChainName");
- virtualPrismaticJointName = getInput<std::string>("headIKVirtualPrismaticJointName");
-}
-
-
-void CalculateHeadIK::run()
-{
- RobotStatechartContext* context = getContext<RobotStatechartContext>();
- VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(context->robotStateComponent->getRobotSnapshot("CalculateTCPPoseTime")));
- // TODO: with the following line, the IK doesn't find a solution, so this terrible hack must be used. Fix it!!!
- //VirtualRobot::RobotPtr robot = robotPtr->clone("CalculateTCPPoseClone");
- VirtualRobot::RobotConfigPtr robotSnapshotConfig = robotSnapshot->getConfig();
- std::string robotModelFile;
- ArmarXDataPath::getAbsolutePath("Armar3/robotmodel/ArmarIII.xml", robotModelFile);
- VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str(), VirtualRobot::RobotIO::eStructure);
- robot->setConfig(robotSnapshotConfig);
-
- if (!robot)
- {
- ARMARX_WARNING << "No robot!" << flush;
- sendEvent<Failure>();
- }
-
- VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(kinematicChainName);
- if (!kinematicChain)
- {
- ARMARX_WARNING << "No kinematicChain with name " << kinematicChainName << flush;
- sendEvent<Failure>();
- }
-
- VirtualRobot::RobotNodePrismaticPtr virtualJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(robot->getRobotNode(virtualPrismaticJointName));
- if (!virtualJoint)
- {
- ARMARX_WARNING << "No virtualJoint with name " << virtualPrismaticJointName << flush;
- sendEvent<Failure>();
- }
-
- /*for (unsigned int i=0; i<kinematicChain->getSize(); i++)
- {
- if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") == 0)
- {
- virtualJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(kinematicChain->getNode(i));
- }
- }*/
- VirtualRobot::GazeIK ikSolver(kinematicChain, virtualJoint);
- ikSolver.enableJointLimitAvoidance(true);
-
- Eigen::Matrix3f m = Eigen::Matrix3f::Identity();
- FramedOrientationPtr targetOri = new FramedOrientation(m, targetPosition->frame);
- VirtualRobot::LinkedCoordinate target = FramedPose::createLinkedCoordinate(robot, targetPosition, targetOri);
- ARMARX_VERBOSE << "Target: " << target.getPose() << armarx::flush;
- Eigen::Matrix4f goalInRoot = target.getInFrame(robot->getRootNode());
- //Eigen::Matrix4f goalGlobal = robot->getRootNode()->toGlobalCoordinateSystem(goalInRoot);
- Eigen::Vector3f targetPos = goalInRoot.block<3,1>(0,3);
-
- ARMARX_VERBOSE << "Calculating IK for target position " << targetPos;
- if (!ikSolver.solve(targetPos))
- {
- ARMARX_WARNING << "IKSolver found no solution! ";
- sendEvent<Failure>();
- }
- else
- {
-
- SingleTypeVariantList jointNames = SingleTypeVariantList(VariantType::String);
- SingleTypeVariantList targetJointValues = SingleTypeVariantList(VariantType::Float);
- for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
- {
- if (kinematicChain->getNode(i)->getName().compare(virtualPrismaticJointName) != 0)
- {
- jointNames.addVariant(Variant(kinematicChain->getNode(i)->getName()));
- targetJointValues.addVariant(Variant(kinematicChain->getNode(i)->getJointValue()));
- ARMARX_VERBOSE << " joint: " << kinematicChain->getNode(i)->getName() << " value: " << kinematicChain->getNode(i)->getJointValue() << flush;
- }
- }
- ARMARX_VERBOSE << "number of joints to be set: " << jointNames.getSize() << flush;
- ARMARX_VERBOSE << "setting output: jointNames" << flush;
- setOutput("jointNames", jointNames);
- ARMARX_VERBOSE << "setting output: targetJointValues" << flush;
- setOutput("targetJointValues", targetJointValues);
- sendEvent<EvCalculationDone>();
- }
-}
-
-
-
-
-
diff --git a/source/RobotAPI/statecharts/deprecated/motioncontrol/MotionControl.h b/source/RobotAPI/statecharts/deprecated/motioncontrol/MotionControl.h
deleted file mode 100644
index 27fd999c71705bd04d159ad1dcc5422d954390aa..0000000000000000000000000000000000000000
--- a/source/RobotAPI/statecharts/deprecated/motioncontrol/MotionControl.h
+++ /dev/null
@@ -1,320 +0,0 @@
-#ifndef MOTIONCONTROL_H
-#define MOTIONCONTROL_H
-
-#include <Core/statechart/Statechart.h>
-#include <RobotAPI/libraries/core/RobotStatechartContext.h>
-
-namespace armarx
-{
-
-namespace MotionControl
-{
-
- /**
- * \class MotionControlHandler
- * \ingroup MotionControl
- * MotionControlHandler is the remote state interface for the MotionControl-
- * States. It contains convenience states that allow to move parts of the robot,
- * i.e. direct and inverse kinematics for arbitrary kinematic chains and predefined
- * parts like the arms.
- */
- class MotionControlOfferer : public RemoteStateOfferer<RobotStatechartContext>
- {
- public:
- MotionControlOfferer();
- void onInitRemoteStateOfferer();
- void onConnectRemoteStateOfferer();
- std::string getStateOffererName() const { return "MotionControl"; }
- };
-
-
- DEFINEEVENT(EvJointTargetReached)
- DEFINEEVENT(EvJointVelocitiesTargetReached)
- DEFINEEVENT(EvCalculationDone)
- DEFINEEVENT(EvTimeout)
-
-
- // test states
-
- struct MotionControlTestState : StateTemplate<MotionControlTestState>
- {
- void defineParameters();
- void defineSubstates();
- };
-
-
- struct MotionControlTestStateIK : StateTemplate<MotionControlTestStateIK>
- {
- void defineParameters();
- void defineSubstates();
- };
-
-
-
- /**
- * \ingroup MotionControl
- * Move the joints of a kinematic chain to the desired values.
- * \param jointNames the names of the joints to be moved
- * \param targetJointValues the desired joint values
- * \param targetTolerance tolerance defining how precisely the joint position has to be reached
- * \param timeoutInMs a timeout after which the attempt to move is aborted
- *
- * \warning Deprecated, use RobotSkills/statecharts/MotionControlGroup/JointPositionControl instead
- */
- struct MoveJoints : StateTemplate<MoveJoints>
- {
- RobotStatechartContext* context;
- ConditionIdentifier targetReachedCondition;
- ActionEventIdentifier timeoutEvent;
- void defineParameters();
- void onEnter();
- void onBreak();
- void onExit();
- };
-
-
- /**
- * \ingroup MotionControl
- * Set the velocities of the joints of a kinematic chain.
- * \param jointNames the names of the joints to be moved
- * \param targetJointVelocities the desired joint velocities
- * \param targetJointVelocityTolerance tolerance defining how precisely the joint velocity has to be reached
- * \param timeoutInMs a timeout after which the attempt is aborted
- */
- struct MoveJointsVelocityControl : StateTemplate<MoveJointsVelocityControl>
- {
- RobotStatechartContext* context;
- ConditionIdentifier targetVelocitiesReachedCondition;
- ActionEventIdentifier timeoutEvent;
- void defineParameters();
- void onEnter();
- void onExit();
- };
-
-
- /**
- * \ingroup MotionControl
- * Move the TCP to a desired pose.
- * \param kinematicChainName the name of the kinematic chain that is used
- * \param targetTCPPosition the target position for the TCP
- * \param targetTCPOrientation the target orientation
- * \param targetPositionDistanceTolerance tolerance for the position to decide if the motion was successfull
- * \param ikWithOrientation: Consider orientation for IK computation. if false, the rotation of the TCP is not considered.
- * \param targetOrientationDistanceTolerance tolerance for the orientation to decide if the motion was successfull
- * \param timeoutInMs a timeout after which the motion is aborted
- */
- struct MoveTCPPoseIK : StateTemplate<MoveTCPPoseIK>
- {
- RobotStatechartContext* context;
- ConditionIdentifier targetReachedCondition;
- ActionEventIdentifier timeoutEvent;
- void defineParameters();
- void defineSubstates();
- void onEnter();
- void onExit();
- };
-
-
- DEFINEEVENT(EvLastPointReached)
- DEFINEEVENT(EvLastPointNotYetReached)
-
- /**
- * \ingroup MotionControl
- * Move the TCP along a trajectory of poses.
- * \param kinematicChainName the name of the kinematic chain that is used
- * \param targetTCPPositions the list of target positions for the TCP
- * \param targetTCPOrientations the list of target orientations
- * \param targetPositionDistanceTolerance tolerance for the position to decide if the motion was successfull
- * \param targetOrientationDistanceTolerance tolerance for the orientation to decide if the motion was successfull
- * \param ikWithOrientation: Consider orientation for IK computation. if false, the rotation of the TCP is not considered.
- * \param timeoutInMs a timeout after which the motion is aborted
- */
- struct MoveTCPTrajectory : StateTemplate<MoveTCPTrajectory>
- {
- RobotStatechartContext* context;
- ConditionIdentifier lastTargetReachedCondition;
- ActionEventIdentifier timeoutEvent;
- ActionEventIdentifier trajectoryPointCounterID;
- void defineParameters();
- void defineSubstates();
- void onEnter();
- void onExit();
- };
-
-
-
-
- /**
- * \ingroup MotionControl
- * Stop all motion of the robot. It sets all velocities to 0. No Smooth stopping
- * is implemented here.
- */
- struct StopRobot : StateTemplate<StopRobot>
- {
- RobotStatechartContext* context;
-
- 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 DoPreshape : StateTemplate<DoPreshape>
- {
- void defineParameters();
- void onEnter();
- void onExit();
- static SingleTypeVariantList GetPreshapeSet(const SingleTypeVariantListPtr preshapes, const std::string& preshapePrefix);
-
- ConditionIdentifier cond;
- ActionEventIdentifier action;
- };
-
- 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();
- };
-
-
-
-
-
- /**
- * \ingroup MotionControl
- * Opens the given hand of the robot
- * \param useLeftHand True if left hand should be opened, false for the right hand
- */
- 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();
- };
-
-
-
-
- /**
- * \ingroup MotionControl
- * Moves the head so that it looks at the specified target position
- * \param target Position at which the robot will try to look
- * \param headIKKinematicChainName Name of the kinematic chain for the head that will be sued in the IK
- */
- struct HeadLookAtTarget : StateTemplate<HeadLookAtTarget>
- {
- void defineParameters();
- void defineSubstates();
- };
-
-
-
-
- /////////////////////////////////////////////////////////////////////
- //// intermediate states
- /////////////////////////////////////////////////////////////////////
-
- struct CalculateJointAngleConfiguration : StateTemplate<CalculateJointAngleConfiguration>
- {
-
- ActionEventIdentifier timeoutEvent;
- void defineParameters();
- void run();
-
- };
-
- struct ValidateJointAngleConfiguration : StateTemplate<ValidateJointAngleConfiguration>
- {
- RobotStatechartContext* context;
- ActionEventIdentifier timeoutEvent;
- void defineParameters();
- void onEnter();
- };
-
-
-
-
- struct MoveTCPTrajectoryInit : StateTemplate<MoveTCPTrajectoryInit>
- {
- void defineParameters();
- void onEnter();
- };
-
-
-
-
- struct MoveTCPTrajectoryCheckCounter : StateTemplate<MoveTCPTrajectoryCheckCounter>
- {
- RobotStatechartContext* context;
- void defineParameters();
- void onEnter();
- };
-
-
-
-
- struct MoveTCPTrajectoryNextPoint : StateTemplate<MoveTCPTrajectoryNextPoint>
- {
- RobotStatechartContext* context;
- void defineParameters();
- void defineSubstates();
- void onEnter();
- };
-
-
-
-
- struct CalculateHeadIK : StateTemplate<CalculateHeadIK>
- {
- void defineParameters();
- void onEnter();
- void run();
- FramedPositionPtr targetPosition;
- std::string kinematicChainName;
- std::string virtualPrismaticJointName;
- };
-
-}
-
-}
-
-#endif
diff --git a/source/RobotAPI/statecharts/deprecated/motioncontrol/ZeroForceControl.cpp b/source/RobotAPI/statecharts/deprecated/motioncontrol/ZeroForceControl.cpp
deleted file mode 100644
index 0c32196cd29e10f99f11d2d5df47ac5aa285f71f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/statecharts/deprecated/motioncontrol/ZeroForceControl.cpp
+++ /dev/null
@@ -1,154 +0,0 @@
-/*
-* 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 ArmarX::
-* @author Mirko Waechter ( mirko.waechter at kit dot edu)
-* @date 2013
-* @copyright http://www.gnu.org/licenses/gpl.txt
-* GNU General Public License
-*/
-
-#include "ZeroForceControl.h"
-
-#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
-#include <Core/core/exceptions/local/ExpressionException.h>
-
-#include <RobotAPI/libraries/core/RobotStatechartContext.h>
-
-namespace armarx {
-
- ZeroForceControl::ZeroForceControl()
- {
- }
-
-
-
- void ZeroForceControl::defineSubstates()
- {
- StatePtr calc = addState<ZeroForceControlForceToAcc>("ZeroForceControlForceToAcc");
-
- PMPtr mapping = PM::createMapping()
- ->mapFromParent("*")
- ->mapFromDataField(new DataFieldIdentifier(".."), "currentForce")
- ->mapFromDataField(new DataFieldIdentifier(".."), "currentTorque")
- ;
-
- setInitState(calc, mapping);
- addTransition<EvSensorUpdate>(calc,calc, mapping->mapFromOutput("*") );
- }
-
- void ZeroForceControl::defineParameters()
- {
- addToInput("robotNodeSetName",VariantType::String, false);
- addToInput("tcpName",VariantType::String, false);
- addToInput("sensitivity",VariantType::Float, false);
- addToInput("maxAcc",VariantType::Float, false);
-
- addToLocal("currentSensitivity",VariantType::Float);
- addToLocal("currentVelocity",VariantType::Float);
- addToLocal("currentAcc",VariantType::Float);
- addToLocal("timestamp",VariantType::Float);
- }
-
- void ZeroForceControl::onEnter()
- {
- RobotStatechartContext* c = getContext<RobotStatechartContext>();
- c->tcpControlPrx->request();
- Eigen::Vector3f null(3);
- null.setZero();
- setLocal("currentSensitivity", 0.0f);
- setLocal("currentVelocity", new FramedDirection(null, getInput<std::string>("tcpName")));
- setLocal("currentAcc", new FramedDirection(null, getInput<std::string>("tcpName")));
- setLocal("timestamp", (float)IceUtil::Time::now().toMilliSecondsDouble());
- }
-
- void ZeroForceControl::onExit()
- {
- RobotStatechartContext* c = getContext<RobotStatechartContext>();
- c->tcpControlPrx->release();
- }
-
-
- ////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////
-
- ZeroForceControlForceToAcc::ZeroForceControlForceToAcc()
- {
-
- }
-
- void ZeroForceControlForceToAcc::defineParameters()
- {
- addToInput("robotNodeSetName",VariantType::String, false);
- addToInput("sensitivity",VariantType::Float, false);
- addToInput("maxAcc",VariantType::Float, false);
-
-
- addToInput("currentSensitivity",VariantType::Float, false);
- addToInput("currentVelocity",VariantType::FramedDirection, false);
- addToInput("currentAcc",VariantType::FramedDirection, false);
- addToInput("currentForce",VariantType::FramedDirection, false);
- addToInput("currentTorque",VariantType::FramedDirection, false);
- addToInput("timestamp",VariantType::Float, false);
-
- addToOutput("currentSensitivity",VariantType::Float, false);
- addToOutput("currentVelocity",VariantType::Float, false);
- addToOutput("currentAcc",VariantType::Float, false);
-
- }
-
- void ZeroForceControlForceToAcc::onEnter()
- {
- Literal update("ForceTorqueUnit","updated", Literal::createParameterList());
- installCondition<EvSensorUpdate>(update);
- IceUtil::Time duration = IceUtil::Time::now() - IceUtil::Time::milliSecondsDouble(getInput<float>("timestamp"));
-
- std::string robotNodeSetName = getInput<std::string>("robotNodeSetName");
-// FramedDirectionPtr vel = getInput<FramedDirection>("currentVelocity");
- FramedDirectionPtr vel = getInput<FramedDirection>("currentVelocity");
- FramedDirectionPtr curAcc = getInput<FramedDirection>("currentAcc");
- FramedDirectionPtr curForce = getInput<FramedDirection>("currentForce");
- ARMARX_CHECK_EXPRESSION(vel->frame == curAcc->frame);
- ARMARX_CHECK_EXPRESSION(vel->frame == curForce->frame);
- float forceThreshold = 2.0f;
- //float maxSensitivity = 2.0f;
-
- Eigen::Vector3f newVel(3);
- Eigen::Vector3f newAcc(3);
- if(curForce->toEigen().norm() > forceThreshold){
- newAcc = 2 * curForce->toEigen() * 5;
- }
- else
- {
- newAcc = -10*vel->toEigen().normalized();
- }
-
- newVel = vel->toEigen() + newAcc * duration.toMilliSecondsDouble()*0.001;
-
-
- setOutput("currentAcc", Variant(new FramedDirection(newAcc, curAcc->frame)));
- RobotStatechartContext* c = getContext<RobotStatechartContext>();
-
- c->tcpControlPrx->setTCPVelocity(robotNodeSetName, "", new FramedDirection(newVel, vel->frame), NULL);
- }
-
- void ZeroForceControlForceToAcc::onExit()
- {
-
- }
-
-}
diff --git a/source/RobotAPI/statecharts/deprecated/motioncontrol/ZeroForceControl.h b/source/RobotAPI/statecharts/deprecated/motioncontrol/ZeroForceControl.h
deleted file mode 100644
index d5acb4e47e9dc1d39772cb89f1bd695f2b518cee..0000000000000000000000000000000000000000
--- a/source/RobotAPI/statecharts/deprecated/motioncontrol/ZeroForceControl.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
-* 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 ArmarX::
-* @author Mirko Waechter ( mirko.waechter at kit dot edu)
-* @date 2013
-* @copyright http://www.gnu.org/licenses/gpl.txt
-* GNU General Public License
-*/
-#ifndef _ARMARX_ZEROFORCECONTROL_H
-#define _ARMARX_ZEROFORCECONTROL_H
-
-#include <Core/statechart/StateTemplate.h>
-
-namespace armarx {
-
- DEFINEEVENT(EvSensorUpdate)
- class ZeroForceControl : public StateTemplate<ZeroForceControl>
- {
- public:
- ZeroForceControl();
-
- // StateBase interface
- protected:
- void defineSubstates();
- void defineParameters();
- void onEnter();
- void onExit();
- };
-
-
-
- class ZeroForceControlForceToAcc : public StateTemplate<ZeroForceControlForceToAcc>
- {
- public:
- ZeroForceControlForceToAcc();
-
- // StateBase interface
- protected:
- void defineParameters();
- void onEnter();
- void onExit();
- };
-
-}
-
-#endif
diff --git a/source/RobotAPI/statecharts/deprecated/placeobject/CMakeLists.txt b/source/RobotAPI/statecharts/deprecated/placeobject/CMakeLists.txt
deleted file mode 100644
index 86e2a1bc709c782e3684ca560cd5c5f54200e36a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/statecharts/deprecated/placeobject/CMakeLists.txt
+++ /dev/null
@@ -1,26 +0,0 @@
-armarx_set_target("RobotAPI Library: PlaceObject")
-
-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 (Eigen3_FOUND AND Simox_FOUND)
- include_directories(
- ${Eigen3_INCLUDE_DIR}
- ${Simox_INCLUDE_DIRS})
-endif()
-
-set(LIB_NAME PlaceObject)
-set(LIB_VERSION 0.1.0)
-set(LIB_SOVERSION 0)
-
-set(LIBS RobotAPICore ArmarXCoreObservers)
-
-set(LIB_FILES PlaceObject.cpp
- )
-set(LIB_HEADERS PlaceObject.h
- )
-
-armarx_add_library("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/statecharts/deprecated/placeobject/PlaceObject.cpp b/source/RobotAPI/statecharts/deprecated/placeobject/PlaceObject.cpp
deleted file mode 100644
index acbe09da549dbc5aabedf4bb7636eb66f2a887df..0000000000000000000000000000000000000000
--- a/source/RobotAPI/statecharts/deprecated/placeobject/PlaceObject.cpp
+++ /dev/null
@@ -1,352 +0,0 @@
-/*
-* 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::PlaceObject
-* @author Nikolaus Vahrenkamp
-* @date 2014
-
-* @copyright http://www.gnu.org/licenses/gpl.txt
-* GNU General Public License
-*/
-
-#include "PlaceObject.h"
-#include <RobotAPI/libraries/core/RobotStatechartContext.h>
-
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include <RobotAPI/libraries/core/RobotStatechartContext.h>
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
-#include <Core/observers/variant/ChannelRef.h>
-#include <Core/observers/variant/SingleTypeVariantList.h>
-
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Robot.h>
-
-
-namespace armarx
-{
- // ****************************************************************
- // Implementation of StatechartPlaceObject
- // ****************************************************************
- void StatechartPlaceObject::defineParameters()
- {
- addToInput("maxHeightToMoveDown", VariantType::Float, false);
- addToInput("minForceForSuccess", VariantType::Float, false);
- addToInput("timeoutPlaceObject", VariantType::Int, false);
-
- addToInput("robotNodeSetName", VariantType::String, false);
- addToInput("robotBaseFrame", VariantType::String, false);
-
-
- addToLocal("startPose", VariantType::FramedPose);
- addToLocal("goalPose", VariantType::FramedPose);
-
- //addToLocal("jointNames", VariantType::List(VariantType::String));
- //addToLocal("jointVelocitiesDatafields", VariantType::List(VariantType::DatafieldRef));
- }
-
- void StatechartPlaceObject::defineSubstates()
- {
- StatePtr stateMoveDown= addState<StateMoveDown>("stateMoveDown");
- //StatePtr stateCheckForces = addState<StateCheckForces>("stateCheckForces");
- StatePtr stateSuccess = addState<SuccessState>("stateSuccess");
- StatePtr stateFailure = addState<FailureState>("stateFailure");
-
- ParameterMappingPtr mapMoveDown = ParameterMapping::createMapping()
- ->mapFromParent("*", "*");
- //ParameterMappingPtr mapCheckForces = ParameterMapping::createMapping()
- // ->mapFromParent("*", "*");
-
- setInitState(stateMoveDown, mapMoveDown);
-
- //transitions
- addTransition<Success>(stateMoveDown, stateMoveDown,mapMoveDown);
- addTransition<Failure>(stateMoveDown, stateFailure);
- //addTransition<Success>(stateCheckForces, stateSuccess);
- //addTransition<Failure>(stateCheckForces, stateMoveDown);
-
- // has to be defined in parent states
- //addTransition<EvPlaceObjectCollision>(stateMoveDown, stateSuccess);
- // for now: success
- //addTransition<EvPlaceObjectTimeout>(stateMoveDown, stateSuccess);
-
- //addTransition<EvPlaceObjectTimeout>(stateCheckForces, stateSuccess);
- //addTransition<EvPlaceObjectTimeout>(stateCheckForces, stateFailure);
-
- }
-
- void StatechartPlaceObject::onEnter()
- {
- ARMARX_DEBUG << "Entering StatechartPlaceObject";
-
- RobotStatechartContext* context = getContext<RobotStatechartContext>();
- //setLocal("jointVelocityChannel", context->getChannelRef(context->getKinematicUnitObserverName(),"jointvelocities"));
- //ChannelRefPtr tempChannelRef = context->getChannelRef(context->getKinematicUnitObserverName(),"jointvelocities");
-
- std::string rnsName = getInput<std::string>("robotNodeSetName");
- std::string rnBaseName = getInput<std::string>("robotBaseFrame");
- ARMARX_LOG << "RNS:" << rnsName << ", rnBaseName:" << rnBaseName << flush;
-
- VirtualRobot::RobotNodeSetPtr nodeSet = context->remoteRobot->getRobotNodeSet(rnsName);
- if (!nodeSet)
- {
- throw UserException("RobotNodeSet not found: " + rnsName);
- }
- if (!nodeSet->getTCP())
- {
- throw UserException("RobotNodeSet without tcp: " + rnsName);
- }
- VirtualRobot::RobotNodePtr nodeBase = context->remoteRobot->getRobotNode(rnBaseName);
- if (!nodeBase)
- {
- throw UserException("RobotNode not found: " + rnBaseName);
- }
-
- Eigen::Matrix4f tcpPose = nodeSet->getTCP()->getGlobalPose();
- tcpPose = nodeBase->toLocalCoordinateSystem(tcpPose);
- FramedPosePtr fp(new FramedPose(tcpPose,rnBaseName));
-
- setLocal("startPose", fp);
- ARMARX_LOG << "startPose:" << tcpPose << ", rnBaseName:" << rnBaseName << flush;
-
-
- Eigen::Matrix4f tcpGoalPose = nodeSet->getTCP()->getGlobalPose();
- float maxHeight = getInput<float>("maxHeightToMoveDown");
- tcpGoalPose(2,3) -= maxHeight;
- tcpGoalPose = nodeBase->toLocalCoordinateSystem(tcpGoalPose);
- FramedPosePtr fpGoal(new FramedPose(tcpGoalPose,rnBaseName));
-
- setLocal("goalPose", fpGoal);
- ARMARX_LOG << "goalPose:" << tcpGoalPose << ", rnBaseName:" << rnBaseName << flush;
-
- ARMARX_LOG << eINFO << "Installing place object timeout condition";
- float timeoutPlaceObject = float(getInput<int>("timeoutPlaceObject"));
-
- condPlaceObjectTimeout = setTimeoutEvent(timeoutPlaceObject, createEvent<EvPlaceObjectTimeout>());
-
-
-
- // install the force condition
-
- float force = getInput<float>("minForceForSuccess");
- Term cond;
- for(unsigned int i=0; i<nodeSet->getSize(); i++)
- {
- ParameterList inrangeCheck;
- inrangeCheck.push_back(new Variant(-force));
- inrangeCheck.push_back(new Variant(force));
-
- Literal inrangeLiteral(DataFieldIdentifier(context->getKinematicUnitObserverName(),"jointtorques", nodeSet->getNode(i)->getName()), "inrange", inrangeCheck);
- cond = cond && !inrangeLiteral;
- }
- ARMARX_LOG << "Installed force consition, minForce:" << force << flush;
- condPlaceObjectCollision = installCondition<EvPlaceObjectCollision>(cond);
- }
-
- void StatechartPlaceObject::onExit()
- {
- removeTimeoutEvent(condPlaceObjectTimeout);
- removeCondition(condPlaceObjectCollision);
-
- RobotStatechartContext *context = getContext<RobotStatechartContext>();
- HandUnitInterfacePrx handUnitPrx = context->getHandUnit("RightHandUnit");
- if (handUnitPrx)
- {
- ARMARX_INFO << "Sending open hand command TODO...." << flush;
- //handUnitPrx->open();
- ARMARX_INFO << "TEST: set object released: Vitalis" << flush;
- handUnitPrx->setObjectReleased("Vitalis");
- } else
- ARMARX_LOG << eINFO << "No hand unit found...";
-
- ARMARX_LOG << eINFO << "Exiting StatechartPlaceObject...";
- }
-
- // ****************************************************************
- // Implementation of StateMoveDown
- // ****************************************************************
-
- void StateMoveDown::defineParameters()
- {
- addToInput("maxHeightToMoveDown", VariantType::Float, false);
- addToInput("minForceForSuccess", VariantType::Float, false);
- addToInput("timeoutPlaceObject", VariantType::Int, false);
- addToInput("robotNodeSetName", VariantType::String, false);
- addToInput("robotBaseFrame", VariantType::String, false);
- addToInput("startPose", VariantType::FramedPose, false);
- addToInput("goalPose", VariantType::FramedPose, false);
- }
-
- void StateMoveDown::onEnter()
- {
- ARMARX_INFO << "Entering StateMoveDown" << flush;
-
- RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
- std::string rnsName = getInput<std::string>("robotNodeSetName");
- std::string rnBaseName = getInput<std::string>("robotBaseFrame");
- VirtualRobot::RobotPtr localRobot = RemoteRobot::createLocalClone(rsContext->robotStateComponent);
-
- VirtualRobot::RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(rnsName);
- VirtualRobot::RobotNodePtr nodeBase = localRobot->getRobotNode(rnBaseName);
- FramedPosePtr startPoseFramed = getInput<FramedPose>("startPose");
- FramedPosePtr goalPoseFramed = getInput<FramedPose>("goalPose");
-
- Eigen::Matrix4f startPose = startPoseFramed->toEigen();
- Eigen::Matrix4f goalPose = goalPoseFramed->toEigen();
- Eigen::Matrix4f currentPose = nodeSet->getTCP()->getGlobalPose();
- currentPose = nodeBase->toLocalCoordinateSystem(currentPose);
-
- ARMARX_INFO << "startPose (coordsystem " << rnBaseName << ") : " << startPose << flush;
- ARMARX_INFO << "goalPose (coordsystem " << rnBaseName << ") : " << goalPose << flush;
- ARMARX_INFO << "currentPose (coordsystem " << rnBaseName << ") : " << currentPose << flush;
-
- Eigen::Vector3f goalVect = goalPose.block(0,3,3,1) - currentPose.block(0,3,3,1);
- float maxDist = 10.0f;
- float dist = goalVect.norm();
- if (dist<maxDist)
- {
- ARMARX_ERROR << "Did not hit any surface, sending failure event..." << flush;
- sendEvent<Failure>();
- }
- goalVect = goalVect.normalized() * maxDist;
-
- goalPose.block(0,3,3,1) = currentPose.block(0,3,3,1) + goalVect;
- ARMARX_INFO << "goalPose cut (coordsystem " << rnBaseName << ") : " << goalPose << flush;
-
-
- VirtualRobot::DifferentialIKPtr ikSolver(new VirtualRobot::DifferentialIK(nodeSet,nodeBase));
-
- ikSolver->setGoal(goalPose,nodeSet->getTCP(),VirtualRobot::IKSolver::Position,10.0f);
-
- bool ikOK = ikSolver->solveIK(0.6f,0,10);
-
- if (!ikOK)
- {
- ARMARX_ERROR << "No IK solution for goal pose (coordsystem " << rnBaseName << ") : " << goalPose << flush;
- sendEvent<Failure>();
- }
-
- std::vector<float> jointValues = nodeSet->getJointValues();
- NameValueMap jointNamesAndValues;
- NameControlModeMap controlModes;
- for (unsigned int j=0; j<jointValues.size(); j++)
- {
- std::string nName = nodeSet->getNode(j)->getName();
- jointNamesAndValues[nName]=jointValues[j];
- controlModes[nName] = ePositionControl;
- ARMARX_INFO << "target for joint :" << nodeSet->getNode(j)->getName() << " -> " << jointValues[j] << flush;
- }
- rsContext->kinematicUnitPrx->switchControlMode(controlModes);
- rsContext->kinematicUnitPrx->setJointAngles(jointNamesAndValues);
- sendEvent<Success>();
- ARMARX_INFO << "StateMoveDown: Done onEnter()";
- }
-
- void StateMoveDown::onExit()
- {
- ARMARX_INFO << "StatePlaceObject: Done onExit()";
- }
-
-
- // ****************************************************************
- // Implementation of StateCheckForces
- // ****************************************************************
-/*
- void StateCheckForces::defineParameters()
- {
- addToInput("maxHeightToMoveDown", VariantType::List(VariantType::Float), false);
- addToInput("minForceForSuccess", VariantType::List(VariantType::Float), false);
- addToInput("timeoutPlaceObject", VariantType::Float, false);
- addToInput("robotNodeSetName", VariantType::String, false);
- addToInput("robotBaseFrame", VariantType::String, false);
- addToInput("startPose", VariantType::List(VariantType::FramedPose), false);
- }
-
- void StateCheckForces::onEnter()
- {
- ARMARX_DEBUG << "Entering StateCheckForces" << flush;
-
- float force = getInput<float>("minForceForSuccess");
- RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
- rsContext->kinematicUnitObserverPrx->getDataFields()
-
-
- RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
- std::string rnsName = getInput<std::string>("robotNodeSetName");
- std::string rnBaseName = getInput<std::string>("robotBaseFrame");
-
-
- VirtualRobot::RobotPtr localRobot = RemoteRobot::createLocalClone(context->robotStateComponent);
- std::string rnsName = getInput<std::string>("robotNodeSetName");
- std::string rnBaseName = getInput<std::string>("robotBaseFrame");
- VirtualRobot::RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(rnsName);
- VirtualRobot::RobotNodePtr nodeBase = localRobot->getRobotNode(rnBaseName);
- FramedPosePtr startPoseFramed = getInput<FramedPose>("startPose");
- FramedPosePtr goalPoseFramed = getInput<FramedPose>("goalPose");
-
- Eigen::Matrix4f startPose = startPoseFramed->toEigen();
- Eigen::Matrix4f goalPose = goalPoseFramed->toEigen();
- Eigen::Matrix4f currentPose = nodeSet->getTCP()->getGlobalPose();
- currentPose = nodeBase->toLocalCoordinateSystem(currentPose);
-
- ARMARX_DEBUG << "startPose (coordsystem " << rnBaseName << ") : " << startPose << flush;
- ARMARX_DEBUG << "goalPose (coordsystem " << rnBaseName << ") : " << goalPose << flush;
- ARMARX_DEBUG << "currentPose (coordsystem " << rnBaseName << ") : " << currentPose << flush;
-
- float dist = (goalPose.block(0,3,3,1) - currentPose.block(0,3,3,1)).norm();
- if (dist<10.0f)
- {
- ARMARX_ERROR << "Did not hit any surface, sending failure event..." << flush;
- sendEvent<Failure>();
- }
-
-
- VirtualRobot::DifferentialIKPtr ikSolver(new VirtualRobot::DifferentialIKPtr(nodeSet,nodeBase));
-
- ikSolver->setGoal(goalPose,nodeSet->getTCP(),VirtualRobot::IKSolver::Position,10.0f);
-
- bool ikOK = ikSolver->solveIK(0.6f,0,10);
-
- if (!ikOK)
- {
- ARMARX_ERROR << "No IK solution for goal pose (coordsystem " << rnBaseName << ") : " << goalPose << flush;
- sendEvent<Failure>();
- }
-
- std::vector<float> jointValues = nodeSet->getJointValues();
- NameValueMap jointNamesAndValues;
-
- for (unsigned int j=0; j<jointValues->getSize(); j++)
- {
- jointNamesAndValues[nodeSet->getNode(j)->getName()]=jointValues[j];
- ARMARX_DEBUG << "target for joint :" << nodeSet->getNode(j)->getName() << " -> " << jointValues[j] << flush;
- }
-
- rsContext->kinematicUnitPrx->setJointAngles(jointNamesAndValues);
-
- ARMARX_DEBUG << "StateCheckForces: Done onEnter()";
- }
-
- void StateCheckForces::onExit()
- {
- ARMARX_DEBUG << "StateCheckForces: Done onExit()";
- }
-
-*/
-
-}
-
diff --git a/source/RobotAPI/statecharts/deprecated/placeobject/PlaceObject.h b/source/RobotAPI/statecharts/deprecated/placeobject/PlaceObject.h
deleted file mode 100644
index 5e3eac02627245b978b8b011543adc60ae03ba3d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/statecharts/deprecated/placeobject/PlaceObject.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
-* 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::PlaceObject
-* @author Nikolaus Vahrenkamp
-* @date 2014
-* @copyright http://www.gnu.org/licenses/gpl.txt
-* GNU General Public License
-*/
-
-#ifndef ARMARX_COMPONENT_PLACE_OBJECT_H
-#define ARMARX_COMPONENT_PLACE_OBJECT_H
-
-#include <Core/statechart/Statechart.h>
-
-
-namespace armarx
-{
- // ****************************************************************
- // Events
- // ****************************************************************
-
- DEFINEEVENT(EvPlaceObjectTimeout)
- DEFINEEVENT(EvPlaceObjectCollision)
-
-
- // ****************************************************************
- // Definition of StatechartPlaceObject
- // ****************************************************************
- struct StatechartPlaceObject :
- StateTemplate<StatechartPlaceObject>
- {
- void defineParameters();
- void defineSubstates();
- void onEnter();
- void onExit();
- StateUtility::ActionEventIdentifier condPlaceObjectTimeout;
- ConditionIdentifier condPlaceObjectCollision;
- };
-
-
-
- // ****************************************************************
- // Definition of states
- // ****************************************************************
- /**
- * StatePlaceMoveDown: Move down until a surface is hit
- */
-
- struct StateMoveDown :
- StateTemplate<StateMoveDown>
- {
- void defineParameters();
- void onEnter();
- void onExit();
- };
-
-
- /**
- * StateCheckForces: Check if a surface was hit -> forces must increase
- */
- /*struct StateCheckForces :
- StateTemplate<StateCheckForces>
- {
- void defineParameters();
- void onEnter();
- void onExit();
- };*/
-
-}
-
-#endif