From 6ed38ae4b6448ea2127521e91636f9d9e9e1223a Mon Sep 17 00:00:00 2001
From: Nikolaus Vahrenkamp <vahrenkamp@kit.edu>
Date: Mon, 18 Aug 2014 13:36:08 +0200
Subject: [PATCH] initial version of place object statechart
---
source/RobotAPI/statecharts/CMakeLists.txt | 1 +
.../statecharts/PlaceObject/CMakeLists.txt | 26 ++
.../statecharts/PlaceObject/PlaceObject.cpp | 318 ++++++++++++++++++
.../statecharts/PlaceObject/PlaceObject.h | 85 +++++
4 files changed, 430 insertions(+)
create mode 100644 source/RobotAPI/statecharts/PlaceObject/CMakeLists.txt
create mode 100644 source/RobotAPI/statecharts/PlaceObject/PlaceObject.cpp
create mode 100644 source/RobotAPI/statecharts/PlaceObject/PlaceObject.h
diff --git a/source/RobotAPI/statecharts/CMakeLists.txt b/source/RobotAPI/statecharts/CMakeLists.txt
index bcb3ae3b2..b5fcf48db 100644
--- a/source/RobotAPI/statecharts/CMakeLists.txt
+++ b/source/RobotAPI/statecharts/CMakeLists.txt
@@ -2,3 +2,4 @@ add_subdirectory(GraspingWithTorques)
add_subdirectory(OpenHand)
add_subdirectory(MovePlatform)
add_subdirectory(MovePlatformToLandmark)
+add_subdirectory(PlaceObject)
diff --git a/source/RobotAPI/statecharts/PlaceObject/CMakeLists.txt b/source/RobotAPI/statecharts/PlaceObject/CMakeLists.txt
new file mode 100644
index 000000000..6a81c9bd8
--- /dev/null
+++ b/source/RobotAPI/statecharts/PlaceObject/CMakeLists.txt
@@ -0,0 +1,26 @@
+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 RobotAPIInterfaces ArmarXInterfaces ArmarXCore 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/PlaceObject/PlaceObject.cpp b/source/RobotAPI/statecharts/PlaceObject/PlaceObject.cpp
new file mode 100644
index 000000000..b0c5867a9
--- /dev/null
+++ b/source/RobotAPI/statecharts/PlaceObject/PlaceObject.cpp
@@ -0,0 +1,318 @@
+/*
+* 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 "../../core/RobotStatechartContext.h"
+
+#include <Core/observers/variant/ChannelRef.h>
+#include <Core/observers/variant/SingleTypeVariantList.h>
+#include <RobotAPI/robotstate/remote/ArmarPose.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+
+#include <RobotAPI/core/RobotStatechartContext.h>
+
+namespace armarx
+{
+ // ****************************************************************
+ // Implementation of StatechartPlaceObject
+ // ****************************************************************
+ void StatechartPlaceObject::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);
+
+
+ addToLocal("startPose", VariantType::List(VariantType::FramedPose));
+ addToLocal("goalPose", VariantType::List(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<EvSuccess>(stateMoveDown, stateMoveDown);
+ addTransition<EvFailure>(stateMoveDown, stateFailure);
+ //addTransition<EvSuccess>(stateCheckForces, stateSuccess);
+ //addTransition<EvFailure>(stateCheckForces, stateMoveDown);
+
+ addTransition<EvPlaceObjectCollision>(stateMoveDown, 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");
+ 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);
+
+ Eigen::Matrix4f tcpGoalPose = nodeSet->getTCP()->getGlobalPose();
+ float maxHeight = getInput<float>("maxHeightToMoveDown");
+ tcpGoalPose(0,3) -= maxHeight;
+ tcpGoalPose = nodeBase->toLocalCoordinateSystem(tcpGoalPose);
+ FramedPosePtr fpGoal(new FramedPose(tcpGoalPose,rnBaseName));
+
+ setLocal("goalPose", fpGoal);
+
+ ARMARX_LOG << eINFO << "Installing place object timeout condition";
+ float timeoutPlaceObject = getInput<float>("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;
+ }
+
+ condPlaceObjectCollision = installCondition<EvPlaceObjectCollision>(cond);
+ }
+
+ void StatechartPlaceObject::onExit()
+ {
+ removeTimeoutEvent(condPlaceObjectTimeout);
+ removeCondition(condPlaceObjectCollision);
+
+ ARMARX_LOG << eINFO << "Exiting StatechartPlaceObject...";
+ }
+
+ // ****************************************************************
+ // Implementation of StateMoveDown
+ // ****************************************************************
+
+ void StateMoveDown::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 StateMoveDown::onEnter()
+ {
+ ARMARX_DEBUG << "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_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<EvFailure>();
+ }
+
+
+ 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<EvFailure>();
+ }
+
+ std::vector<float> jointValues = nodeSet->getJointValues();
+ NameValueMap jointNamesAndValues;
+
+ for (unsigned int j=0; j<jointValues.size(); 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 << "StateMoveDown: Done onEnter()";
+ }
+
+ void StateMoveDown::onExit()
+ {
+ ARMARX_DEBUG << "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<EvFailure>();
+ }
+
+
+ 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<EvFailure>();
+ }
+
+ 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/PlaceObject/PlaceObject.h b/source/RobotAPI/statecharts/PlaceObject/PlaceObject.h
new file mode 100644
index 000000000..5e3eac026
--- /dev/null
+++ b/source/RobotAPI/statecharts/PlaceObject/PlaceObject.h
@@ -0,0 +1,85 @@
+/*
+* 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
--
GitLab