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 <mutex>
#include <string>
#include <tuple>
#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/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/libraries/core/PIDController.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;
    };
 
} // namespace armarx