Added OffsetFilter: ForceTorqueObserver now supports nulling the forces

in an individual datafield

source/RobotAPI/components/units/ForceTorqueObserver.cpp

@@ -6,6 +6,7 @@
 #include <Core/observers/checks/ConditionCheckLarger.h>
 #include <Core/observers/checks/ConditionCheckSmaller.h>
 #include <RobotAPI/libraries/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h>
+#include <RobotAPI/libraries/robotstate/remote/observerfilters/OffsetFilter.h>
 
 #include <RobotAPI/libraries/robotstate/remote/RobotStateObjectFactories.h>
 
@@ -129,3 +130,9 @@ void armarx::ForceTorqueObserver::reportSensorValues(const std::string &sensorNo
         offerValue(channelName, type, torques);
     }
 }
+
+
+DatafieldRefBasePtr armarx::ForceTorqueObserver::createNulledDatafield(const DatafieldRefBasePtr &forceTorqueDatafieldRef, const Ice::Current &)
+{
+    return createFilteredDatafield(new filters::OffsetFilter(), forceTorqueDatafieldRef);
+}

source/RobotAPI/components/units/ForceTorqueObserver.h

@@ -49,6 +49,10 @@ namespace armarx
 
 
         void offerValue(std::string channelName, const std::string &type, FramedVector3BasePtr value);
+
+        // ForceTorqueUnitObserverInterface interface
+    public:
+        DatafieldRefBasePtr createNulledDatafield(const DatafieldRefBasePtr &forceTorqueDatafieldRef, const Ice::Current &);
     };
 }
 

source/RobotAPI/interface/CMakeLists.txt

@@ -9,6 +9,7 @@ set(SLICE_FILES
 
     observers/KinematicUnitObserverInterface.ice
     observers/PlatformUnitObserverInterface.ice
+    observers/ObserverFilters.ice
 
     robotstate/LinkedPoseBase.ice
     robotstate/PoseBase.ice

source/RobotAPI/interface/observers/ObserverFilters.ice

+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU 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    ArmarX::RobotAPI
+* @author     Mirko Waechter (waechter at kit dot edu)
+* @copyright  2015 Humanoids Group, H2T, KIT
+* @license    http://www.gnu.org/licenses/gpl-2.0.txt
+*             GNU General Public License
+*/
+
+#ifndef _ARMARX_ROBOTAPI_OBSERVER_FILTERS_SLICE_
+#define _ARMARX_ROBOTAPI_OBSERVER_FILTERS_SLICE_
+
+#include <Core/interface/observers/VariantBase.ice>
+#include <Core/interface/observers/Filters.ice>
+
+module armarx
+{
+    ["cpp:virtual"]
+    class OffsetFilterBase extends DatafieldFilterBase
+    {
+
+    };
+};
+
+#endif

source/RobotAPI/interface/units/ForceTorqueUnit.ice

@@ -53,7 +53,7 @@ module armarx
 
     interface ForceTorqueUnitObserverInterface extends ObserverInterface, ForceTorqueUnitListener
     {
-
+        DatafieldRefBase createNulledDatafield(DatafieldRefBase forceTorqueDatafieldRef);
     };
 };
 

source/RobotAPI/libraries/robotstate/remote/CMakeLists.txt

@@ -31,6 +31,7 @@ set(LIB_HEADERS ArmarPose.h
             RobotStateObserver.h
             RemoteRobot.h
             observerfilters/PoseMedianFilter.h
+            observerfilters/OffsetFilter.h
             checks/ConditionCheckEqualsPose.h
             checks/ConditionCheckEqualsPoseWithTolerance.h
             checks/ConditionCheckMagnitudeChecks.h

source/RobotAPI/libraries/robotstate/remote/observerfilters/OffsetFilter.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    ArmarX::
+* @author     Mirko Waechter ( mirko.waechter at kit dot edu)
+* @date       2014
+* @copyright  http://www.gnu.org/licenses/gpl.txt
+*             GNU General Public License
+*/
+
+#ifndef _ARMARX_ROBOTAPI_OFFSETFILTER_H
+#define _ARMARX_ROBOTAPI_OFFSETFILTER_H
+
+#include <RobotAPI/interface/observers/ObserverFilters.h>
+#include <Core/observers/filters/DatafieldFilter.h>
+#include <RobotAPI/libraries/robotstate/remote/ArmarPose.h>
+
+namespace armarx {
+    namespace filters {
+
+        /**
+         * @class OffsetFilter
+         * @ingroup ObserverFilters
+         * @brief The OffsetFilter class returns values
+         * relative to value from the first call of the filter.
+         * E.g. this is useful for Forces which should be nulled
+         * at a specific moment.
+         */
+        class OffsetFilter :
+                public ::armarx::OffsetFilterBase,
+                public DatafieldFilter
+        {
+        public:
+            OffsetFilter()
+            {
+                firstRun = true;
+            }
+
+
+            // DatafieldFilterBase interface
+        public:
+            VariantBasePtr calculate(const Ice::Current &c = Ice::Current()) const
+            {
+
+                VariantPtr newVariant;
+                if( initialValue && dataHistory.size() > 0)
+                {
+                    VariantTypeId type = dataHistory.begin()->second->getType();
+                    VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
+                    if(currentValue->getType() != initialValue->getType())
+                    {
+                        ARMARX_ERROR_S << "Types in OffsetFilter are different: " << Variant::typeToString(currentValue->getType()) << " and " << Variant::typeToString(initialValue->getType());
+                        return NULL;
+                    }
+                    if(type == VariantType::Int)
+                    {
+                        int newValue = dataHistory.rbegin()->second->getInt() - initialValue->getInt();
+                        newVariant = new Variant(newValue);
+                    }
+                    else if(type == VariantType::Float)
+                    {
+                        float newValue = dataHistory.rbegin()->second->getFloat() - initialValue->getFloat();
+                        newVariant = new Variant(newValue);
+                    }
+                    else if(type == VariantType::Double)
+                    {
+                        double newValue = dataHistory.rbegin()->second->getDouble() - initialValue->getDouble();
+                        newVariant = new Variant(newValue);
+                    }
+                    else if(type == VariantType::FramedVector3)
+                    {
+                        FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(currentValue->get<FramedVector3>());
+                        FramedVector3Ptr intialVec = FramedVector3Ptr::dynamicCast(initialValue->get<FramedVector3>());
+                        Eigen::Vector3f newValue =  vec->toEigen() - intialVec->toEigen();
+
+                        newVariant = new Variant(new FramedVector3(newValue, vec->frame, vec->agent));
+                    }
+                    else if(type == VariantType::FramedPosition)
+                    {
+                        FramedPositionPtr pos = FramedPositionPtr::dynamicCast(currentValue->get<FramedPosition>());
+                        FramedPositionPtr intialPos = FramedPositionPtr::dynamicCast(initialValue->get<FramedPosition>());
+                        Eigen::Vector3f newValue =  pos->toEigen() - intialPos->toEigen();
+                        newVariant = new Variant(new FramedPosition(newValue, pos->frame, pos->agent));
+                    }
+                }
+                return newVariant;
+
+            }
+            ParameterTypeList getSupportedTypes(const Ice::Current &) const
+            {
+                ParameterTypeList result;
+                result.push_back(VariantType::Int);
+                result.push_back(VariantType::Float);
+                result.push_back(VariantType::Double);
+                result.push_back(VariantType::FramedVector3);
+                result.push_back(VariantType::FramedPosition);
+                return result;
+            }
+        private:
+            bool firstRun;
+            VariantPtr initialValue;
+
+            // DatafieldFilterBase interface
+        public:
+            void update(Ice::Long timestamp, const VariantBasePtr &value, const Ice::Current &c)
+            {
+                DatafieldFilter::update(timestamp, value, c);
+                if(firstRun)
+                {
+                    if( dataHistory.size() == 0 )
+                        return;
+                    initialValue = VariantPtr::dynamicCast(dataHistory.begin()->second);
+                    firstRun = false;
+                }
+            }
+        };
+    }
+}
+
+#endif // OFFSETFILTER_H

source/RobotAPI/libraries/robotstate/remote/observerfilters/PoseMedianFilter.h

@@ -73,9 +73,9 @@ namespace armarx {
             ParameterTypeList getSupportedTypes(const Ice::Current &c) const
             {
                 ParameterTypeList result = MedianFilter::getSupportedTypes(c);
-                result.push_back(VariantType::Vector3);
+//                result.push_back(VariantType::Vector3);
                 result.push_back(VariantType::FramedVector3);
-                result.push_back(VariantType::FramedPosition);
+//                result.push_back(VariantType::FramedPosition);
                 return result;
             }