ObstacleAvoidingPlatformUnit.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::ArmarXObjects::ObstacleAvoidingPlatformUnit
 * @author     Christian R. G. Dreher <c.dreher@kit.edu>
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#pragma once
 
 
// STD/STL
#include <deque>
#include <string>
#include <tuple>
#include <mutex>
#include <vector>
 
// Eigen
#include <Eigen/Core>
 
// Ice
#include <IceUtil/Time.h>
 
// Simox
#include <VirtualRobot/Nodes/RobotNode.h>
 
// ArmarX
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <ArmarXCore/util/tasks.h>
#include <ArmarXCore/util/time.h>
 
// RobotAPI
#include <RobotAPI/components/units/PlatformUnit.h>
#include <RobotAPI/interface/components/ObstacleAvoidance/ObstacleAvoidanceInterface.h>
#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
 
 
namespace armarx
{
 
    class ObstacleAvoidingPlatformUnit :
        virtual public PlatformUnit,
        virtual public RobotStateComponentPluginUser,
        virtual public ArVizComponentPluginUser,
        virtual public DebugObserverComponentPluginUser
    {
 
    public:
 
        enum class control_mode
        {
            position,
            velocity,
            none
        };
 
    private:
 
        struct control_loop
        {
            std::mutex mutex;
            control_mode mode = control_mode::none;
            RunningTask<ObstacleAvoidingPlatformUnit>::pointer_type task;
            IceUtil::Time cycle_time = IceUtil::Time::milliSeconds(10);
        };
 
        struct control_data
        {
            std::mutex mutex;
            Eigen::Vector2f target_pos;
            float target_ori;
            Eigen::Vector2f target_vel;
            float target_rot_vel;
            float target_dist;
            float target_angular_dist;
            Eigen::Vector2f agent_pos;
            float agent_ori;
            double agent_safety_margin = 0;
            float min_vel_near_target = 50;
            float min_vel_general = 100;
            float max_vel = 200;
            float max_rot_vel = 0.3;
            float pos_near_threshold = 250;
            float pos_reached_threshold = 8;
            float ori_reached_threshold = 0.1;
            float kp = 3.5;
            std::deque<std::tuple<Eigen::Vector2f, Eigen::Vector2f>> vel_history;
            unsigned amount_smoothing = 50;
            unsigned amount_max_vel = 3;
            float adaptive_max_vel_exp = 0;
        };
 
        struct visualization
        {
            Eigen::Vector3f start;
            std::vector<Eigen::Vector3f> path;
            bool enabled = true;
        };
 
        struct velocities
        {
            Eigen::Vector2f target_local;
            Eigen::Vector2f modulated_local;
            Eigen::Vector2f target_global;
            Eigen::Vector2f modulated_global;
            float target_rot;
            float err_dist;
            float err_angular_dist;
        };
 
    public:
 
        ObstacleAvoidingPlatformUnit();
 
        virtual
        ~ObstacleAvoidingPlatformUnit()
        override;
 
        virtual
        std::string
        getDefaultName()
        const override;
 
        virtual
        void
        moveTo(
            float target_pos_x,
            float target_pos_y,
            float target_ori,
            float pos_reached_threshold,
            float ori_reached_threshold,
            const Ice::Current& = Ice::Current{})
        override;
 
        virtual
        void
        move(
            float target_vel_x,
            float target_vel_y,
            float target_rot_vel,
            const Ice::Current& = Ice::Current{})
        override;
 
        virtual
        void
        moveRelative(
            float target_pos_delta_x,
            float target_pos_delta_y,
            float target_delta_ori,
            float pos_reached_threshold,
            float ori_reached_threshold,
            const Ice::Current& = Ice::Current{})
        override;
 
        virtual
        void
        setMaxVelocities(
            float max_pos_vel,
            float max_rot_vel,
            const Ice::Current& = Ice::Current{})
        override;
 
        virtual
        void
        stopPlatform(const Ice::Current& = Ice::Current{})
        override;
 
    protected:
 
        virtual
        void
        onInitPlatformUnit()
        override;
 
        virtual
        void
        onStartPlatformUnit()
        override;
 
        virtual
        void
        onExitPlatformUnit()
        override;
 
        virtual
        PropertyDefinitionsPtr
        createPropertyDefinitions()
        override;
 
    private:
 
        void
        schedule_high_level_control_loop(control_mode mode);
 
        void
        high_level_control_loop();
 
        velocities
        get_velocities();
 
        void
        update_agent_dependent_values();
 
        Eigen::Vector2f
        get_target_velocity()
        const;
 
        float
        get_target_rotational_velocity()
        const;
 
        bool
        target_position_reached()
        const;
 
        bool
        target_orientation_reached()
        const;
 
        bool
        target_reached()
        const;
 
        Eigen::Vector2f
        post_process_vel(const Eigen::Vector2f& target_vel, const Eigen::Vector2f& modulated_vel);
 
        Eigen::Vector2f
        calculate_mean_modulated_vel()
        const;
 
        unsigned
        calculate_adaptive_smoothing_buffer_size()
        const;
 
        float
        calculate_adaptive_max_vel()
        const;
 
        void
        visualize();
 
        void
        visualize(const velocities& vels);
 
        bool
        is_near_target(const Eigen::Vector2f& control_velocity)
        const
        noexcept;
 
    public:
 
        static const std::string default_name;
 
    private:
 
        PlatformUnitInterfacePrx m_platform;
        ObstacleAvoidanceInterfacePrx m_obstacle_avoidance;
        VirtualRobot::RobotPtr m_robot;
 
        ObstacleAvoidingPlatformUnit::control_loop m_control_loop;
        ObstacleAvoidingPlatformUnit::control_data m_control_data;
 
        mutable PIDController m_rot_pid_controller{1.0, 0.00009, 0.0};
 
        visualization m_viz;
 
    };
 
}