From a239e0510fe869b7514dc4966e880c88f60ccff0 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Wed, 27 Aug 2014 00:55:00 +0200
Subject: [PATCH] Added MotionControl Statechart Group
---
.../MotionControlGroup/CMakeLists.txt | 33 +++++
.../MotionControlGroup.scgxml | 8 +
.../MotionControlGroupRemoteStateOfferer.cpp | 67 +++++++++
.../MotionControlGroupRemoteStateOfferer.h | 54 +++++++
.../MotionControlGroup/MoveJoints.cpp | 140 ++++++++++++++++++
.../MotionControlGroup/MoveJoints.h | 63 ++++++++
.../MotionControlGroup/MoveJoints.xml | 23 +++
.../testing/testMoveJoints.cpp | 83 +++++++++++
.../testing/testMoveJoints.h | 58 ++++++++
.../testing/testMoveJoints.xml | 40 +++++
10 files changed, 569 insertions(+)
create mode 100644 source/RobotAPI/statecharts/MotionControlGroup/CMakeLists.txt
create mode 100644 source/RobotAPI/statecharts/MotionControlGroup/MotionControlGroup.scgxml
create mode 100644 source/RobotAPI/statecharts/MotionControlGroup/MotionControlGroupRemoteStateOfferer.cpp
create mode 100644 source/RobotAPI/statecharts/MotionControlGroup/MotionControlGroupRemoteStateOfferer.h
create mode 100644 source/RobotAPI/statecharts/MotionControlGroup/MoveJoints.cpp
create mode 100644 source/RobotAPI/statecharts/MotionControlGroup/MoveJoints.h
create mode 100644 source/RobotAPI/statecharts/MotionControlGroup/MoveJoints.xml
create mode 100644 source/RobotAPI/statecharts/MotionControlGroup/testing/testMoveJoints.cpp
create mode 100644 source/RobotAPI/statecharts/MotionControlGroup/testing/testMoveJoints.h
create mode 100644 source/RobotAPI/statecharts/MotionControlGroup/testing/testMoveJoints.xml
diff --git a/source/RobotAPI/statecharts/MotionControlGroup/CMakeLists.txt b/source/RobotAPI/statecharts/MotionControlGroup/CMakeLists.txt
new file mode 100644
index 000000000..e4ba4cbeb
--- /dev/null
+++ b/source/RobotAPI/statecharts/MotionControlGroup/CMakeLists.txt
@@ -0,0 +1,33 @@
+armarx_component_set_name("MotionControlGroup")
+
+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(COMPONENT_LIBS RobotAPICore RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers)
+
+# Sources
+
+set(SOURCES
+MotionControlGroupRemoteStateOfferer.cpp
+./MoveJoints.cpp
+testing/testMoveJoints.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+
+set(HEADERS
+MotionControlGroupRemoteStateOfferer.h
+./MoveJoints.h
+testing/testMoveJoints.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/statecharts/MotionControlGroup/MotionControlGroup.scgxml b/source/RobotAPI/statecharts/MotionControlGroup/MotionControlGroup.scgxml
new file mode 100644
index 000000000..3a4556031
--- /dev/null
+++ b/source/RobotAPI/statecharts/MotionControlGroup/MotionControlGroup.scgxml
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="utf-8"?>
+<StatechartGroup name="MotionControlGroup" package="RobotAPI">
+ <Folder basename="testing">
+ <State filename="testMoveJoints.xml"/>
+ </Folder>
+ <State filename="MoveJoints.xml"/>
+</StatechartGroup>
+
diff --git a/source/RobotAPI/statecharts/MotionControlGroup/MotionControlGroupRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/MotionControlGroup/MotionControlGroupRemoteStateOfferer.cpp
new file mode 100644
index 000000000..a07cfa75d
--- /dev/null
+++ b/source/RobotAPI/statecharts/MotionControlGroup/MotionControlGroupRemoteStateOfferer.cpp
@@ -0,0 +1,67 @@
+/*
+ * 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::MotionControlGroup::MotionControlGroupRemoteStateOfferer
+ * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "MotionControlGroupRemoteStateOfferer.h"
+
+using namespace armarx;
+using namespace MotionControlGroup;
+
+// DO NOT EDIT NEXT LINE
+MotionControlGroupRemoteStateOfferer::SubClassRegistry MotionControlGroupRemoteStateOfferer::Registry(MotionControlGroupRemoteStateOfferer::GetName(), &MotionControlGroupRemoteStateOfferer::CreateInstance);
+
+
+
+MotionControlGroupRemoteStateOfferer::MotionControlGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer < StatechartContext > (reader)
+{
+}
+
+void MotionControlGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+{
+
+}
+
+void MotionControlGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+{
+
+}
+
+void MotionControlGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+{
+
+}
+
+// DO NOT EDIT NEXT FUNCTION
+std::string MotionControlGroupRemoteStateOfferer::GetName()
+{
+ return "MotionControlGroupRemoteStateOfferer";
+}
+
+// DO NOT EDIT NEXT FUNCTION
+XMLStateOffererFactoryBasePtr MotionControlGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+{
+ return XMLStateOffererFactoryBasePtr(new MotionControlGroupRemoteStateOfferer(reader));
+}
+
+
+
diff --git a/source/RobotAPI/statecharts/MotionControlGroup/MotionControlGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/MotionControlGroup/MotionControlGroupRemoteStateOfferer.h
new file mode 100644
index 000000000..480f36ddb
--- /dev/null
+++ b/source/RobotAPI/statecharts/MotionControlGroup/MotionControlGroupRemoteStateOfferer.h
@@ -0,0 +1,54 @@
+/*
+ * 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::MotionControlGroup
+ * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_XMLUSERCODE_RobotAPI_MotionControlGroup_REMOTESTATEOFFERER_H
+#define _ARMARX_XMLUSERCODE_RobotAPI_MotionControlGroup_REMOTESTATEOFFERER_H
+
+#include <Core/statechart/xmlstates/XMLRemoteStateOfferer.h>
+
+namespace armarx
+{
+ namespace MotionControlGroup
+ {
+ class MotionControlGroupRemoteStateOfferer :
+ virtual public XMLRemoteStateOfferer < StatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ {
+ public:
+ MotionControlGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+
+ // inherited from RemoteStateOfferer
+ void onInitXMLRemoteStateOfferer();
+ void onConnectXMLRemoteStateOfferer();
+ void onExitXMLRemoteStateOfferer();
+
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+ static SubClassRegistry Registry;
+
+
+ };
+ }
+}
+
+#endif
diff --git a/source/RobotAPI/statecharts/MotionControlGroup/MoveJoints.cpp b/source/RobotAPI/statecharts/MotionControlGroup/MoveJoints.cpp
new file mode 100644
index 000000000..3d37d50d4
--- /dev/null
+++ b/source/RobotAPI/statecharts/MotionControlGroup/MoveJoints.cpp
@@ -0,0 +1,140 @@
+/*
+ * 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::MotionControlGroup
+ * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "MoveJoints.h"
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+
+#include <RobotAPI/core/RobotStatechartContext.h>
+
+using namespace armarx;
+using namespace MotionControlGroup;
+
+// DO NOT EDIT NEXT LINE
+MoveJoints::SubClassRegistry MoveJoints::Registry(MoveJoints::GetName(), &MoveJoints::CreateInstance);
+
+
+
+MoveJoints::MoveJoints(XMLStateConstructorParams stateData) :
+ XMLStateTemplate < MoveJoints > (stateData)
+{
+}
+
+void MoveJoints::onEnter()
+{
+ // put your user code for the enter-point here
+ // execution time should be short (<100ms)
+
+ RobotStatechartContext* context = getContext<RobotStatechartContext>();
+
+ NameValueMap targetJointAngles;
+ NameValueMap targetJointVelocities;
+ SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
+ SingleTypeVariantListPtr targetJointValues = getInput<SingleTypeVariantList>("targetJointValues");
+ NameControlModeMap controlModes;
+ 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();
+ controlModes[jointNames->getVariant(i)->getString()] = ePositionControl;
+ ARMARX_VERBOSE << "setting joint angle for joint '" << jointNames->getVariant(i)->getString() << "' to " << targetJointValues->getVariant(i)->getFloat() << std::endl;
+ }
+
+ // TODO: Set Max Velocities
+ if(isInputParameterSet("jointMaxSpeeds") && getInput<SingleTypeVariantList>("jointMaxSpeeds")->getSize() == jointNames->getSize())
+ {
+ SingleTypeVariantListPtr maxJointValues = getInput<SingleTypeVariantList>("jointMaxSpeeds");
+ for (int i=0; i<jointNames->getSize(); i++)
+ {
+ targetJointVelocities[jointNames->getVariant(i)->getString()] = maxJointValues->getVariant(i)->getFloat();
+ }
+ }
+ else
+ {
+ float maxVel = getInput<float>("jointMaxSpeed");
+ for (int i=0; i<jointNames->getSize(); i++)
+ {
+ targetJointVelocities[jointNames->getVariant(i)->getString()] = maxVel;
+ }
+ }
+
+ // 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->switchControlMode(controlModes);
+ context->kinematicUnitPrx->setJointVelocities(targetJointVelocities);
+ context->kinematicUnitPrx->setJointAngles(targetJointAngles);
+}
+
+
+
+void MoveJoints::onBreak()
+{
+ // put your user code for the breaking point here
+ // execution time should be short (<100ms)
+}
+
+void MoveJoints::onExit()
+{
+ // put your user code for the exit point here
+ // execution time should be short (<100ms)
+ removeTimeoutEvent(timeoutEvent);
+ removeCondition(targetReachedCondition);
+
+}
+
+// DO NOT EDIT NEXT FUNCTION
+std::string MoveJoints::GetName()
+{
+ return "MoveJoints";
+}
+
+// DO NOT EDIT NEXT FUNCTION
+XMLStateFactoryBasePtr MoveJoints::CreateInstance(XMLStateConstructorParams stateData)
+{
+ return XMLStateFactoryBasePtr(new MoveJoints(stateData));
+}
+
diff --git a/source/RobotAPI/statecharts/MotionControlGroup/MoveJoints.h b/source/RobotAPI/statecharts/MotionControlGroup/MoveJoints.h
new file mode 100644
index 000000000..f1a958c87
--- /dev/null
+++ b/source/RobotAPI/statecharts/MotionControlGroup/MoveJoints.h
@@ -0,0 +1,63 @@
+/*
+ * 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::MotionControlGroup
+ * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_XMLUSERCODE_RobotAPI_MotionControlGroup_MoveJoints_H
+#define _ARMARX_XMLUSERCODE_RobotAPI_MotionControlGroup_MoveJoints_H
+
+#include <Core/statechart/xmlstates/XMLState.h>
+//#include <Core/statechart/StateUtilFunctions.h>
+
+namespace armarx
+{
+ namespace MotionControlGroup
+ {
+ DEFINEEVENT(EvJointTargetReached)
+ DEFINEEVENT(EvTimeout)
+ class MoveJoints :
+ virtual public XMLStateTemplate < MoveJoints > ,
+ public XMLStateFactoryBase
+ {
+ public:
+ MoveJoints(XMLStateConstructorParams stateData);
+
+ // inherited from StateBase
+ void onEnter();
+ void onBreak();
+ void onExit();
+
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
+
+ ConditionIdentifier targetReachedCondition;
+ ActionEventIdentifier timeoutEvent;
+
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
+ }
+}
+
+#endif
diff --git a/source/RobotAPI/statecharts/MotionControlGroup/MoveJoints.xml b/source/RobotAPI/statecharts/MotionControlGroup/MoveJoints.xml
new file mode 100644
index 000000000..0ac4f83fe
--- /dev/null
+++ b/source/RobotAPI/statecharts/MotionControlGroup/MoveJoints.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.0" name="MoveJoints" uuid="1A25B687-CFBC-4CA3-A1C7-32EEAF004C45" width="800" height="600">
+ <Description>State to perform position control on states and wait for them to reach the target position</Description>
+ <InputParameters>
+ <Parameter name="jointMaxSpeed" type="::armarx::FloatVariantData" optional="no" default='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.8000000119209290}}'/>
+ <Parameter name="jointMaxSpeeds" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" optional="yes" default='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.0}}],"type":"::armarx::SingleTypeVariantListBase"}'/>
+ <Parameter name="jointNames" type="::armarx::SingleTypeVariantListBase(::armarx::StringVariantData)" optional="no" default='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":""}}],"type":"::armarx::SingleTypeVariantListBase"}'/>
+ <Parameter name="jointTargetTolerance" type="::armarx::DoubleVariantData" optional="yes" default='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::DoubleVariantData","value":0.050}}'/>
+ <Parameter name="targetJointValues" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" optional="no" default='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.0}}],"type":"::armarx::SingleTypeVariantListBase"}'/>
+ <Parameter name="timeoutInMs" type="::armarx::IntVariantData" optional="yes" default='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::IntVariantData","value":2000}}'/>
+ </InputParameters>
+ <OutputParameters>
+ <Parameter name="jointDistancesToTargets" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" optional="yes" default='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.0}}],"type":"::armarx::SingleTypeVariantListBase"}'/>
+ </OutputParameters>
+ <LocalParameters/>
+ <Substates/>
+ <Events>
+ <Event name="EvJointTargetReached"/>
+ <Event name="EvTimeout"/>
+ </Events>
+ <Transitions/>
+</State>
+
diff --git a/source/RobotAPI/statecharts/MotionControlGroup/testing/testMoveJoints.cpp b/source/RobotAPI/statecharts/MotionControlGroup/testing/testMoveJoints.cpp
new file mode 100644
index 000000000..70b0d2fe7
--- /dev/null
+++ b/source/RobotAPI/statecharts/MotionControlGroup/testing/testMoveJoints.cpp
@@ -0,0 +1,83 @@
+/*
+ * 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::MotionControlGroup
+ * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "testMoveJoints.h"
+
+using namespace armarx;
+using namespace MotionControlGroup;
+
+// DO NOT EDIT NEXT LINE
+testMoveJoints::SubClassRegistry testMoveJoints::Registry(testMoveJoints::GetName(), &testMoveJoints::CreateInstance);
+
+
+
+testMoveJoints::testMoveJoints(XMLStateConstructorParams stateData) :
+ XMLStateTemplate < testMoveJoints > (stateData)
+{
+}
+
+void testMoveJoints::onEnter()
+{
+ // put your user code for the enter-point here
+ // execution time should be short (<100ms)
+}
+
+void testMoveJoints::run()
+{
+ // put your user code for the execution-phase here
+ // runs in seperate thread, thus can do complex operations
+ // should check constantly whether isRunningTaskStopped() returns true
+
+
+ while (!isRunningTaskStopped()) // stop run function if returning true
+ {
+ // do your calculations
+ }
+
+}
+
+void testMoveJoints::onBreak()
+{
+ // put your user code for the breaking point here
+ // execution time should be short (<100ms)
+}
+
+void testMoveJoints::onExit()
+{
+ // put your user code for the exit point here
+ // execution time should be short (<100ms)
+
+}
+
+// DO NOT EDIT NEXT FUNCTION
+std::string testMoveJoints::GetName()
+{
+ return "testMoveJoints";
+}
+
+// DO NOT EDIT NEXT FUNCTION
+XMLStateFactoryBasePtr testMoveJoints::CreateInstance(XMLStateConstructorParams stateData)
+{
+ return XMLStateFactoryBasePtr(new testMoveJoints(stateData));
+}
+
diff --git a/source/RobotAPI/statecharts/MotionControlGroup/testing/testMoveJoints.h b/source/RobotAPI/statecharts/MotionControlGroup/testing/testMoveJoints.h
new file mode 100644
index 000000000..dbdb77ea8
--- /dev/null
+++ b/source/RobotAPI/statecharts/MotionControlGroup/testing/testMoveJoints.h
@@ -0,0 +1,58 @@
+/*
+ * 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::MotionControlGroup
+ * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_XMLUSERCODE_RobotAPI_MotionControlGroup_testMoveJoints_H
+#define _ARMARX_XMLUSERCODE_RobotAPI_MotionControlGroup_testMoveJoints_H
+
+#include <Core/statechart/xmlstates/XMLState.h>
+
+namespace armarx
+{
+ namespace MotionControlGroup
+ {
+ class testMoveJoints :
+ virtual public XMLStateTemplate < testMoveJoints > ,
+ public XMLStateFactoryBase
+ {
+ public:
+ testMoveJoints(XMLStateConstructorParams stateData);
+
+ // inherited from StateBase
+ void onEnter();
+ void run();
+ void onBreak();
+ void onExit();
+
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
+
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
+ }
+}
+
+#endif
diff --git a/source/RobotAPI/statecharts/MotionControlGroup/testing/testMoveJoints.xml b/source/RobotAPI/statecharts/MotionControlGroup/testing/testMoveJoints.xml
new file mode 100644
index 000000000..24fdc0f7d
--- /dev/null
+++ b/source/RobotAPI/statecharts/MotionControlGroup/testing/testMoveJoints.xml
@@ -0,0 +1,40 @@
+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.0" name="testMoveJoints" uuid="846CFED7-7E7C-4E0B-B8FE-B5CF9D86A8EF" width="800" height="600">
+ <InputParameters>
+ <Parameter name="jointNames" type="::armarx::SingleTypeVariantListBase(::armarx::StringVariantData)" optional="yes" default='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":""}}],"type":"::armarx::SingleTypeVariantListBase"}'/>
+ <Parameter name="testJointValues" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" optional="yes" default='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":3.140000104904175}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.50}}],"type":"::armarx::SingleTypeVariantListBase"}'/>
+ </InputParameters>
+ <OutputParameters>
+ <Parameter name="resultTolerance" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" optional="yes" default='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.0}}],"type":"::armarx::SingleTypeVariantListBase"}'/>
+ </OutputParameters>
+ <LocalParameters/>
+ <Substates>
+ <EndState name="EvFailure" event="EvFailure" left="577.44" top="362.477" boundingSquareSize="200"/>
+ <EndState name="EvSuccess" event="EvSuccess" left="577.44" top="120.4" boundingSquareSize="200"/>
+ <LocalState name="MoveJoints" refuuid="1A25B687-CFBC-4CA3-A1C7-32EEAF004C45" left="174.053" top="308.984" boundingSquareSize="200"/>
+ </Substates>
+ <Events/>
+ <StartState substateName="MoveJoints">
+ <ParameterMappings>
+ <ParameterMapping sourceType="Parent" from="jointNames" to="jointNames"/>
+ <ParameterMapping sourceType="Parent" from="testJointValues" to="targetJointValues"/>
+ </ParameterMappings>
+ </StartState>
+ <Transitions>
+ <Transition from="MoveJoints" to="EvSuccess" eventName="EvJointTargetReached">
+ <ParameterMappings>
+ <ParameterMapping sourceType="Output" from="jointDistancesToTargets" to="resultTolerance"/>
+ </ParameterMappings>
+ <SupportPoints>
+ <SupportPoint posX="526.151" posY="237.127"/>
+ </SupportPoints>
+ </Transition>
+ <Transition from="MoveJoints" to="EvFailure" eventName="EvTimeout">
+ <ParameterMappings/>
+ <SupportPoints>
+ <SupportPoint posX="487.029" posY="490.223"/>
+ </SupportPoints>
+ </Transition>
+ </Transitions>
+</State>
+
--
GitLab