diff --git a/interface/slice/units/ForceTorqueUnit.ice b/interface/slice/units/ForceTorqueUnit.ice
index a090ab5711e755276493ce9eec7b5ce572b5c333..4adc738ddcfebd4e4d2b5a4e522c153833028dac 100644
--- a/interface/slice/units/ForceTorqueUnit.ice
+++ b/interface/slice/units/ForceTorqueUnit.ice
@@ -14,9 +14,9 @@
* 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::Core
- * @author Manfred Kroehnert (manfred dot kroehnert at kit dot edu)
- * @copyright 2013 Manfred Kroehnert
+ * @package RobotAPI
+ * @author Mirko Waechter <waechter at kit dot edu>
+ * @copyright 2013 Mirko Waechter
* @license http://www.gnu.org/licenses/gpl.txt
* GNU General Public License
*/
@@ -28,25 +28,30 @@
#include <core/UserException.ice>
#include <core/BasicTypes.ice>
#include <observers/VariantBase.ice>
-#include <robotstate/LinkedPoseBase.ice>
+#include <observers/ObserverInterface.ice>
+#include <robotstate/RobotState.ice>
-module robotapi
+module armarx
{
- dictionary<string, armarx::LinkedPoseBase> LinkedPoseMap;
+
+
+ dictionary<string, armarx::FramedVector3Base> FramedVector3Map;
sequence<string> StringMap;
interface ForceTorqueUnitInterface extends armarx::SensorActorUnitInterface
{
- void setOffset(LinkedPoseMap forceTorqueOffsets);
- void setForceTorqueToNull(StringMap sensorNames);
-
+ void setOffset(FramedVector3Map forceTorqueOffsets);
+ void setToNull(StringMap sensorNames);
};
interface ForceTorqueUnitListener
{
- void reportForceTorqueRaw(LinkedPoseMap forceTorques);
- void reportForceTorqueRelativeToOffset(LinkedPoseMap forceTorques);
+ void reportSensorValues(string type, FramedVector3Map forces);
+ };
+
+ interface ForceTorqueUnitObserverInterface extends ObserverInterface, ForceTorqueUnitListener
+ {
};
diff --git a/source/RobotAPI/CMakeLists.txt b/source/RobotAPI/CMakeLists.txt
index 1000114d8d592e8078c0f1ad2b18b0e79a175ab9..475cc9398af64bbc32318c7edb7a311f92ae2e9c 100644
--- a/source/RobotAPI/CMakeLists.txt
+++ b/source/RobotAPI/CMakeLists.txt
@@ -1,4 +1,5 @@
add_subdirectory(core)
add_subdirectory(motioncontrol)
add_subdirectory(applications)
+add_subdirectory(units)
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index 6135bbd62b00d9c085039eb23e53326e051c3334..1c8b369ef7e5740f86680c28e206aafb66fdab43 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -1,4 +1,4 @@
-
+add_subdirectory(ForceTorqueObserver)
add_subdirectory(MotionControlTest)
diff --git a/source/RobotAPI/applications/ForceTorqueObserver/CMakeLists.txt b/source/RobotAPI/applications/ForceTorqueObserver/CMakeLists.txt
new file mode 100755
index 0000000000000000000000000000000000000000..3a02531313f90edf32ded55e11ec325969453925
--- /dev/null
+++ b/source/RobotAPI/applications/ForceTorqueObserver/CMakeLists.txt
@@ -0,0 +1,28 @@
+
+armarx_component_set_name(ForceTorqueObserver)
+
+set(COMPONENT_BUILD TRUE)
+
+find_package(Simox QUIET)
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+armarx_build_if(COMPONENT_BUILD "component disabled")
+
+# check if ArmarXCoreUnits library gets built
+# LOCATION is NOT-FOUND (equal to FALSE) if library is disabled
+GET_TARGET_PROPERTY(ArmarXCoreUnits_ENABLED ArmarXCoreUnits LOCATION)
+armarx_build_if(ArmarXCoreUnits_ENABLED "ArmarXCoreUnits library disabled")
+
+GET_TARGET_PROPERTY(ArmarXCoreObservers_ENABLED ArmarXCoreObservers LOCATION)
+armarx_build_if(ArmarXCoreObservers_ENABLED "ArmarXCoreObservers library disabled")
+
+
+if (ARMARX_BUILD)
+ set(COMPONENT_LIBS RobotAPIUnits RobotAPICore RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRemoteRobot)
+
+ set(SOURCES "")
+ set(HEADERS ForceTorqueObserverApp.h)
+
+ armarx_component_add_executable("${SOURCES}" "${HEADERS}")
+
+endif()
diff --git a/source/RobotAPI/applications/ForceTorqueObserver/ForceTorqueObserverApp.h b/source/RobotAPI/applications/ForceTorqueObserver/ForceTorqueObserverApp.h
new file mode 100755
index 0000000000000000000000000000000000000000..cab40777f7e5607e02dd0df0e5974c4f44d8af5f
--- /dev/null
+++ b/source/RobotAPI/applications/ForceTorqueObserver/ForceTorqueObserverApp.h
@@ -0,0 +1,41 @@
+/**
+ * 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 ArmarXCore::applications
+ * @author Kai Welke (weöle dot at kit dot edu)
+ * @date 2012
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+
+#include <Core/core/application/Application.h>
+#include <RobotAPI/units/ForceTorqueObserver.h>
+
+namespace armarx
+{
+ class ForceTorqueObserverApp :
+ virtual public armarx::Application
+ {
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<ForceTorqueObserver>(properties) );
+ }
+ };
+}
+
+
+
diff --git a/source/RobotAPI/core/CMakeLists.txt b/source/RobotAPI/core/CMakeLists.txt
index 1761fc4df670e2dd3536f51daaa96a23e187925f..73413bd6fe23dab50c7542ca7763c872ae2b53f4 100755
--- a/source/RobotAPI/core/CMakeLists.txt
+++ b/source/RobotAPI/core/CMakeLists.txt
@@ -15,10 +15,13 @@ if (ARMARX_BUILD)
set(LIB_VERSION 0.1.0)
set(LIB_SOVERSION 0)
- set(LIBS ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreRobotStateComponent)
+ set(LIBS ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreRobotStateComponent ${VirtualRobot_LIBRARIES})
+
+ set(LIB_FILES RobotStatechartContext.cpp
+ )
+ set(LIB_HEADERS RobotStatechartContext.h
+ )
- set(LIB_FILES RobotStatechartContext.cpp)
- set(LIB_HEADERS RobotStatechartContext.h)
add_library(${LIB_NAME} SHARED ${LIB_FILES} ${LIB_HEADERS})
diff --git a/source/RobotAPI/units/CMakeLists.txt b/source/RobotAPI/units/CMakeLists.txt
new file mode 100755
index 0000000000000000000000000000000000000000..e711efd938685f9b9fc598a95213a549d420d9b0
--- /dev/null
+++ b/source/RobotAPI/units/CMakeLists.txt
@@ -0,0 +1,34 @@
+armarx_set_target("RobotAPI Units Library: RobotAPIUnits")
+
+
+find_package(Eigen3 QUIET)
+find_package(Simox QUIET)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+if (ARMARX_BUILD)
+ include_directories(${Eigen3_INCLUDE_DIR})
+ include_directories(${VirtualRobot_INCLUDE_DIRS})
+
+ set(LIB_NAME RobotAPIUnits)
+ set(LIB_VERSION 0.1.0)
+ set(LIB_SOVERSION 0)
+
+ set(LIBS RobotAPIInterfaces RobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRemoteRobot ${VirtualRobot_LIBRARIES})
+
+ set(LIB_FILES
+ ForceTorqueObserver.h
+ )
+ set(LIB_HEADERS
+ ForceTorqueObserver.cpp
+ )
+
+
+ add_library(${LIB_NAME} SHARED ${LIB_FILES} ${LIB_HEADERS})
+
+ library_settings("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_HEADERS}")
+ target_link_ice(${LIB_NAME})
+ target_link_libraries(${LIB_NAME} ${LIBS})
+endif()
+
diff --git a/source/RobotAPI/units/ForceTorqueObserver.cpp b/source/RobotAPI/units/ForceTorqueObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d5db0ce9fa9a0e2e14ee41896f7bd1fb1c7c391e
--- /dev/null
+++ b/source/RobotAPI/units/ForceTorqueObserver.cpp
@@ -0,0 +1,132 @@
+#include "ForceTorqueObserver.h"
+
+#include <Core/observers/checks/ConditionCheckUpdated.h>
+#include <Core/observers/checks/ConditionCheckEquals.h>
+#include <Core/observers/checks/ConditionCheckInRange.h>
+#include <Core/observers/checks/ConditionCheckLarger.h>
+#include <Core/observers/checks/ConditionCheckSmaller.h>
+#include <Core/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h>
+
+#include <Core/robotstate/remote/RobotStateObjectFactories.h>
+
+#define RAWFORCE "_rawforcevectors"
+#define OFFSETFORCE "_forceswithoffsetvectors"
+#define FILTEREDFORCE "_filteredforcesvectors"
+#define RAWTORQUE "_rawtorquevectors"
+#define OFFSETTORQUE "_torqueswithoffsetvectors"
+#define FORCETORQUEVECTORS "_forceTorqueVectors"
+
+
+using namespace armarx;
+
+ForceTorqueObserver::ForceTorqueObserver()
+{
+}
+
+void ForceTorqueObserver::onInitObserver()
+{
+ usingTopic(getProperty<std::string>("ForceTorqueUnitName").getValue() + "State");
+
+ // register all checks
+ offerConditionCheck("updated", new ConditionCheckUpdated());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
+// offerConditionCheck("magnitudeinrange", new ConditionCheckInRange());
+ offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
+// offerConditionCheck("magnitudesmaller", new ConditionCheckSmaller());
+
+ // register all channels
+// offerChannel(FORCETORQUEVECTORS,"Force and Torque vectors on specific parts of the robot.");
+
+// offerChannel(RAWFORCE,"Forces on specific parts of the robot.");
+// offerChannel(OFFSETFORCE,"Force Torques of specific parts of the robot with an offset.");
+// offerChannel(FILTEREDFORCE,"Gaussian filtered force torques of specific parts of the robot.");
+
+// offerChannel(RAWTORQUE,"Torques on specific parts of the robot.");
+
+}
+
+void ForceTorqueObserver::onConnectObserver()
+{
+
+
+}
+
+//void ForceTorqueObserver::reportForceRaw(const FramedVector3Map &newForces, const Ice::Current &)
+//{
+
+//}
+
+//void ForceTorqueObserver::reportForceWithOffset(const FramedVector3Map &newForces, const Ice::Current &)
+//{
+// FramedVector3Map::const_iterator it = newForces.begin();
+// for(; it != newForces.end(); it++)
+// {
+
+// FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(it->second);
+// if(!existsDataField(OFFSETFORCE, it->first))
+// offerDataFieldWithDefault(OFFSETFORCE, it->first, Variant(it->second), "3D vector for Force Torques of " + it->first);
+// else
+// setDataField(OFFSETFORCE, it->first, Variant(it->second));
+
+
+// if(!existsChannel(it->first))
+// {
+// offerChannel(it->first,"Force on " + it->first);
+// offerDataFieldWithDefault(it->first,"force_x", Variant(vec->x), "Force on X axis");
+// offerDataFieldWithDefault(it->first,"force_y", Variant(vec->y), "Force on Y axis");
+// offerDataFieldWithDefault(it->first,"force_z", Variant(vec->z), "Force on Z axis");
+// }
+// else
+// {
+// setDataField(it->first,"force_x", Variant(vec->x));
+// setDataField(it->first,"force_y", Variant(vec->y));
+// setDataField(it->first,"force_z", Variant(vec->z));
+// }
+
+// }
+//}
+
+
+
+void ForceTorqueObserver::reportSensorValues(const std::string &type, const FramedVector3Map &newValues, const Ice::Current &)
+{
+ ScopedLock lock(dataMutex);
+ FramedVector3Map::const_iterator it = newValues.begin();
+ if(!existsChannel(type))
+ offerChannel(type, "Force and Torque vectors on specific parts of the robot.");
+ for(; it != newValues.end(); it++)
+ {
+
+ FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(it->second);
+ std::string identifier = it->first + "_" + type + "_in_frame_" + vec->frame;
+ if(!existsDataField(type, identifier))
+ offerDataFieldWithDefault(type, identifier, Variant(it->second), "3D vector for Force Torques of " + it->first);
+ else
+ setDataField(type, identifier, Variant(it->second));
+
+ if(!existsChannel(identifier))
+ {
+ offerChannel(identifier,type + " on " + it->first);
+ offerDataFieldWithDefault(identifier,type + "_x", Variant(vec->x), type + " on X axis");
+ offerDataFieldWithDefault(identifier,type + "_y", Variant(vec->y), type + " on Y axis");
+ offerDataFieldWithDefault(identifier,type + "_z", Variant(vec->z), type + " on Z axis");
+ offerDataFieldWithDefault(identifier,type + "_frame", Variant(vec->frame), "Frame of " + type);
+ }
+ else
+ {
+ setDataField(identifier,type + "_x", Variant(vec->x));
+ setDataField(identifier,type + "_y", Variant(vec->y));
+ setDataField(identifier,type + "_z", Variant(vec->z));
+ setDataField(identifier,type + "_frame", Variant(vec->frame));
+ }
+
+ }
+}
+
+PropertyDefinitionsPtr ForceTorqueObserver::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new ForceTorqueObserverPropertyDefinitions(
+ getConfigIdentifier()));
+}
diff --git a/source/RobotAPI/units/ForceTorqueObserver.h b/source/RobotAPI/units/ForceTorqueObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..59d453b4bd2c9380ed2907a0dd59478ae2ed9557
--- /dev/null
+++ b/source/RobotAPI/units/ForceTorqueObserver.h
@@ -0,0 +1,48 @@
+#ifndef _ARMARX_ROBOTAPI_FORCETORQUEOBSERVER_H
+#define _ARMARX_ROBOTAPI_FORCETORQUEOBSERVER_H
+
+#include <RobotAPI/interface/units/ForceTorqueUnit.h>
+#include <Core/observers/Observer.h>
+
+namespace armarx
+{
+
+ class ForceTorqueObserverPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ ForceTorqueObserverPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineRequiredProperty<std::string>("ForceTorqueUnitName","Name of the ForceTorqueUnit");
+ }
+ };
+
+ class ForceTorqueObserver :
+ virtual public Observer,
+ virtual public ForceTorqueUnitObserverInterface
+ {
+ public:
+ ForceTorqueObserver();
+
+ // framework hooks
+ virtual std::string getDefaultName() const { return "ForceTorqueUnitObserver"; }
+ void onInitObserver();
+ void onConnectObserver();
+
+ void reportSensorValues(const std::string & type, const ::armarx::FramedVector3Map& newForces, const ::Ice::Current& = ::Ice::Current());
+// void reportForceWithOffset(const ::armarx::FramedVector3Map& newForces, const ::Ice::Current& = ::Ice::Current());
+// void reportTorqueRaw(const ::armarx::FramedVector3Map& newTorques, const ::Ice::Current& = ::Ice::Current());
+// void reportTorqueWithOffset(const ::armarx::FramedVector3Map& newTorques, const ::Ice::Current& = ::Ice::Current());
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+ private:
+ armarx::Mutex dataMutex;
+
+ };
+}
+
+#endif