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 <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
 
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
 
#include <RobotAPI/gui-plugins/FTSensorCalibrationGui/ui_FTSensorCalibrationGuiWidget.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.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
 
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;
    };
} // namespace armarx