diff --git a/interface/slice/selflocalisation/SelfLocalisationProcess.ice b/interface/slice/selflocalisation/SelfLocalisationProcess.ice
new file mode 100644
index 0000000000000000000000000000000000000000..0a7f47958914e87b86b4ef80040f7374b519064b
--- /dev/null
+++ b/interface/slice/selflocalisation/SelfLocalisationProcess.ice
@@ -0,0 +1,36 @@
+/*
+ * 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
+ * @author Thomas von der Heyde <tvh242 at hotmail dot com>
+ * @copyright 2014
+ * @license http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_SELFLOCALISATION_SELFLOCALISATIONPROCESS_SLICE_
+#define _ARMARX_ROBOTAPI_SELFLOCALISATION_SELFLOCALISATIONPROCESS_SLICE_
+
+module armarx
+{
+ interface SelfLocalisationProcessInterface
+ {
+ void startSelfLocalisationProcess();
+ void stopSelfLocalisationProcess();
+ };
+};
+
+#endif
diff --git a/source/RobotAPI/CMakeLists.txt b/source/RobotAPI/CMakeLists.txt
index 66e7f98242116f441b8659d3169014188dc627ed..6d1f81d3a7c643d86aedf55c21fe10b861b9ad9c 100644
--- a/source/RobotAPI/CMakeLists.txt
+++ b/source/RobotAPI/CMakeLists.txt
@@ -2,7 +2,7 @@ add_subdirectory(core)
add_subdirectory(motioncontrol)
add_subdirectory(applications)
add_subdirectory(units)
-add_subdirectory(GraspingWithTorques)
-add_subdirectory(armarx-objects)
+#add_subdirectory(armarx-objects)
add_subdirectory(OpenHand)
+add_subdirectory(statecharts)
diff --git a/source/RobotAPI/motioncontrol/MotionControl.cpp b/source/RobotAPI/motioncontrol/MotionControl.cpp
index b99a21ea97860b0b86f73564b8ddb3ec9c9939ff..644d0dcc4445cb3c6b2899e6dab34d9e6190f1e4 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.cpp
+++ b/source/RobotAPI/motioncontrol/MotionControl.cpp
@@ -483,7 +483,7 @@ void CalculateJointAngleConfiguration::defineParameters()
void CalculateJointAngleConfiguration::run()
{
- RobotStatechartContext* context = getContext<RobotStatechartContext>();
+ //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!!!
diff --git a/source/RobotAPI/motioncontrol/ZeroForceControl.cpp b/source/RobotAPI/motioncontrol/ZeroForceControl.cpp
index 16f32e51b576b3e66a9f289ebf4fb6cf4cde8f00..f300daa87dc5631051d93c2f4a493b9754d9a946 100644
--- a/source/RobotAPI/motioncontrol/ZeroForceControl.cpp
+++ b/source/RobotAPI/motioncontrol/ZeroForceControl.cpp
@@ -125,7 +125,7 @@ namespace armarx {
ARMARX_CHECK_EXPRESSION(vel->frame == curAcc->frame);
ARMARX_CHECK_EXPRESSION(vel->frame == curForce->frame);
float forceThreshold = 2.0f;
- float maxSensitivity = 2.0f;
+ //float maxSensitivity = 2.0f;
Eigen::Vector3f newVel(3);
Eigen::Vector3f newAcc(3);
diff --git a/source/RobotAPI/statecharts/CMakeLists.txt b/source/RobotAPI/statecharts/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d49eaefadf4a1deb4c5fd4ddc881c5baca4a2b53
--- /dev/null
+++ b/source/RobotAPI/statecharts/CMakeLists.txt
@@ -0,0 +1,2 @@
+add_subdirectory(GraspingWithTorques)
+add_subdirectory(OpenHand)
diff --git a/source/RobotAPI/GraspingWithTorques/CMakeLists.txt b/source/RobotAPI/statecharts/GraspingWithTorques/CMakeLists.txt
similarity index 100%
rename from source/RobotAPI/GraspingWithTorques/CMakeLists.txt
rename to source/RobotAPI/statecharts/GraspingWithTorques/CMakeLists.txt
diff --git a/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp b/source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.cpp
similarity index 98%
rename from source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp
rename to source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.cpp
index 9d5477b5501d5740ca34e3fdd6930c70f4b49a9d..02ffd83b67625df55a4a7e03e4c5c61b62523642 100644
--- a/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp
+++ b/source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.cpp
@@ -24,7 +24,7 @@
#include "GraspingWithTorques.h"
//#include "Armar3GraspContext.h" //MP: Maybe necessary again later?
-#include "../core/RobotStatechartContext.h"
+#include "../../core/RobotStatechartContext.h"
#include <Core/observers/variant/ChannelRef.h>
#include <Core/observers/variant/SingleTypeVariantList.h>
@@ -131,7 +131,7 @@ namespace armarx
SingleTypeVariantList dataFields(VariantType::DatafieldRef);
- for (int i=0; i<nodeSet->getSize(); i++)
+ 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()));
@@ -369,7 +369,7 @@ namespace armarx
//set joint velocities manually to zero (DEBUG)
//---------
- RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
+ //RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
//SingleTypeVariantListPtr jointTorquesGraspList = getInput<SingleTypeVariantList>("jointTorquesGrasp");
NameValueMap jointNamesAndValues;
@@ -381,11 +381,7 @@ namespace armarx
//rsContext->kinematicUnitPrx->setJointTorques(jointNamesAndValues);
-
//rsContext->kinematicUnitPrx->setJointVelocities(jointNamesAndValues); //Auskommentiert nach letztem Test MP 2014-02-20
-
-
-
}
}
diff --git a/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.h b/source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.h
similarity index 100%
rename from source/RobotAPI/GraspingWithTorques/GraspingWithTorques.h
rename to source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.h
diff --git a/source/RobotAPI/OpenHand/CMakeLists.txt b/source/RobotAPI/statecharts/OpenHand/CMakeLists.txt
similarity index 100%
rename from source/RobotAPI/OpenHand/CMakeLists.txt
rename to source/RobotAPI/statecharts/OpenHand/CMakeLists.txt
diff --git a/source/RobotAPI/OpenHand/OpenHand.cpp b/source/RobotAPI/statecharts/OpenHand/OpenHand.cpp
similarity index 98%
rename from source/RobotAPI/OpenHand/OpenHand.cpp
rename to source/RobotAPI/statecharts/OpenHand/OpenHand.cpp
index e756c1b24f086bdb324245f0760ee39bdf9f3e22..1baf286ef48decad27ad31f9499b5b58ac11faa7 100644
--- a/source/RobotAPI/OpenHand/OpenHand.cpp
+++ b/source/RobotAPI/statecharts/OpenHand/OpenHand.cpp
@@ -24,7 +24,7 @@
#include "OpenHand.h"
//#include "Armar3GraspContext.h" //MP: Maybe necessary again later?
-#include "../core/RobotStatechartContext.h"
+#include "../../core/RobotStatechartContext.h"
#include <Core/observers/variant/ChannelRef.h>
#include <Core/observers/variant/SingleTypeVariantList.h>
@@ -88,7 +88,7 @@ namespace armarx
SingleTypeVariantList dataFields(VariantType::DatafieldRef);
- for (int i=0; i<nodeSet->getSize(); i++)
+ 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()));
diff --git a/source/RobotAPI/OpenHand/OpenHand.h b/source/RobotAPI/statecharts/OpenHand/OpenHand.h
similarity index 100%
rename from source/RobotAPI/OpenHand/OpenHand.h
rename to source/RobotAPI/statecharts/OpenHand/OpenHand.h
diff --git a/source/RobotAPI/units/TCPControlUnit.cpp b/source/RobotAPI/units/TCPControlUnit.cpp
index 3a7a42baad3cb0acdc99d0de9f5182740fa0ffdf..1a4cbcd61679ffa03263ebda36db17d524afbb3f 100644
--- a/source/RobotAPI/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/units/TCPControlUnit.cpp
@@ -720,7 +720,6 @@ namespace armarx
while (step<maxNStep);
float bestJVError = std::numeric_limits<float>::max();
- int bestIndex = -1;
for(unsigned int i=0; i < jointDeltaIterations.size(); i++)
{
if(jointDeltaIterations.at(i).first < bestJVError)
@@ -728,6 +727,8 @@ namespace armarx
bestJVError = jointDeltaIterations.at(i).first;
resultJointDelta = jointDeltaIterations.at(i).second;
bestIndex = i;
+
+
}
}
robot->setJointValues(rns,oldJointValues + resultJointDelta);