Added ForceTorque Sensor components

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
+    {
 
     };
 

source/RobotAPI/CMakeLists.txt

 add_subdirectory(core)
 add_subdirectory(motioncontrol)
 add_subdirectory(applications)
+add_subdirectory(units)
 

source/RobotAPI/applications/CMakeLists.txt

 
-
+add_subdirectory(ForceTorqueObserver)
 add_subdirectory(MotionControlTest)
 

source/RobotAPI/applications/ForceTorqueObserver/CMakeLists.txt

+
+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()

source/RobotAPI/applications/ForceTorqueObserver/ForceTorqueObserverApp.h

+/**
+ * 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) );
+        }
+    };
+}
+
+
+

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})
 

source/RobotAPI/units/CMakeLists.txt

+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()
+

source/RobotAPI/units/ForceTorqueObserver.cpp

+#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()));
+}

source/RobotAPI/units/ForceTorqueObserver.h

+#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