FTSensorCalibrationGuiWidgetController.h

Go to the documentation of this file.

/*
 * 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 version 2 as
 * published by the Free Software Foundation.
 *
 * 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 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::gui-plugins::FTSensorCalibrationGuiWidgetController
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#pragma once
 
#include <fstream>
 
#include <RobotAPI/gui-plugins/FTSensorCalibrationGui/ui_FTSensorCalibrationGuiWidget.h>
 
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
 
#include <ArmarXCore/core/system/ImportExportComponent.h>
 
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
 
//compensation_table
namespace armarx
{
    template<class T>
    struct compensation_table
    {
        using value_t = T;
        using entry_t = std::array<value_t, 3>;
        std::vector<entry_t> table;
 
        value_t compensate(value_t decision, value_t value) const;
    private:
        value_t apply(const entry_t& f, value_t value) const;
        value_t interpolate(value_t decision, const entry_t& lo, const entry_t& hi, value_t value) const;
 
        mutable std::size_t last_idx = 0;
        mutable entry_t     search_dummy;
    };
 
    template<class T> inline
    typename compensation_table<T>::value_t
    compensation_table<T>::compensate(value_t decision, value_t value) const
    {
        if (table.empty())
        {
            return value;
        }
        search_dummy.at(0) = decision;
 
        const auto lower = std::lower_bound(
                               table.begin(), table.end(), decision,
                               [](const entry_t& e, value_t v)
        {
            return e.at(0) < v;
        });
        if (lower == table.end())
        {
            return apply(table.back(), value);
        }
        if (lower == table.begin())
        {
            return apply(table.front(), value);
        }
        //interpolate
        return interpolate(decision, *(lower - 1), *lower, value);
    }
    template<class T> inline
    typename compensation_table<T>::value_t
    compensation_table<T>::apply(const entry_t& f, value_t value) const
    {
        return f.at(1) * value + f.at(2);
    }
    template<class T> inline
    typename compensation_table<T>::value_t
    compensation_table<T>::interpolate(value_t decision, const entry_t& lo,
                                       const entry_t& hi, value_t value) const
    {
        const value_t t = (decision - lo.at(0)) / (hi.at(0) - lo.at(0));
        const value_t a = lo.at(1) * (1 - t) + hi.at(1) * t;
        const value_t b = lo.at(2) * (1 - t) + hi.at(2) * t;
        return a * value + b;
    }
}
 
namespace armarx
{
    /**
        \page RobotAPI-GuiPlugins-FTSensorCalibrationGui FTSensorCalibrationGui
        \brief The FTSensorCalibrationGui allows visualizing ...
 
        \image html FTSensorCalibrationGui.png
        The user can
 
        API Documentation \ref FTSensorCalibrationGuiWidgetController
 
        \see FTSensorCalibrationGuiGuiPlugin
        */
 
    /**
         * \class FTSensorCalibrationGuiWidgetController
         * \brief FTSensorCalibrationGuiWidgetController brief one line description
         *
         * Detailed description
         */
    class ARMARXCOMPONENT_IMPORT_EXPORT
        FTSensorCalibrationGuiWidgetController:
        public armarx::ArmarXComponentWidgetControllerTemplate < FTSensorCalibrationGuiWidgetController >,
        public virtual RobotUnitComponentPluginUser,
        public virtual RobotStateComponentPluginUser,
        public virtual DebugObserverComponentPluginUser
    {
        Q_OBJECT
 
    public:
        explicit FTSensorCalibrationGuiWidgetController();
        virtual ~FTSensorCalibrationGuiWidgetController();
 
        void loadSettings(QSettings* settings) override;
        void saveSettings(QSettings* settings) override;
        QPointer<QDialog> getConfigDialog(QWidget* parent) override;
        void configured() override;
 
        /**
         * Returns the Widget name displayed in the ArmarXGui to create an
         * instance of this class.
         */
        static QString GetWidgetName()
        {
            return "Debugging.FTSensorCalibrationGui";
        }
 
        void onInitComponent()       override {}
        void onConnectComponent()    override;
        void onDisconnectComponent() override;
        void onExitComponent()       override {}
 
    protected:
        void timerEvent(QTimerEvent* event) override;
 
    private slots:
        void loadCalibFromFile();
        void startRecording();
        void stopRecording();
        void updateCalibration();
        void updateCompensation();
        void loadDefaultArmar6FTL();
        void loadDefaultArmar6FTR();
 
    private:
        QPointer<SimpleConfigDialog>      _dialog;
        mutable std::recursive_mutex      _all_mutex;
        VirtualRobot::RobotPtr            _robot;
        Ui::FTSensorCalibrationGuiWidget  _widget;
        bool                              _recording = false;
        bool                              _comboboxes_are_set_up = false;
        std::set<std::string>             _all_logging_names;
 
        Eigen::Matrix6f                   _conversion_matrix;
        Eigen::Vector6f                   _offset;
        Eigen::Vector6i                   _channel_order;
        float                             _ticks_to_volt_factor;
 
        RobotUnitDataStreamingReceiverPtr _streaming_handler;
        std::array<RobotUnitDataStreaming::DataEntry, 6> _adc;
        std::array<RobotUnitDataStreaming::DataEntry, 6> _adc_temp;
        std::vector<RobotUnitDataStreaming::DataEntry>   _joints;
        std::ofstream                                    _logfile;
 
        compensation_table<float> _comp_table;
    };
}