ObstacleAwarePlatformUnit.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::ObstacleAwarePlatformUnit
 * @author     Christian R. G. Dreher <c.dreher@kit.edu>
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#pragma once
 
 
// STD/STL
#include <mutex>
#include <string>
#include <vector>
 
// Eigen
#include <Eigen/Core>
 
// Ice
#include <IceUtil/Time.h>
 
// Simox
#include <VirtualRobot/Safety.h>
#include <VirtualRobot/VirtualRobot.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/DynamicObstacleManagerInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/libraries/core/PIDController.h>
 
namespace armarx
{
 
    class ObstacleAwarePlatformUnit :
        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<ObstacleAwarePlatformUnit>::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;
        };
 
        struct visualization
        {
            Eigen::Vector3f start;
            std::vector<Eigen::Vector3f> path;
            bool enabled = true;
            VirtualRobot::Circle boundingCircle;
        };
 
        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:
        ObstacleAwarePlatformUnit();
 
        ~ObstacleAwarePlatformUnit() override;
 
        std::string getDefaultName() const override;
 
        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;
 
        void move(float target_vel_x,
                  float target_vel_y,
                  float target_rot_vel,
                  const Ice::Current& = Ice::Current{}) override;
 
        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;
 
        void setMaxVelocities(float max_pos_vel,
                              float max_rot_vel,
                              const Ice::Current& = Ice::Current{}) override;
 
        void stopPlatform(const Ice::Current& = Ice::Current{}) override;
 
    protected:
        void onInitPlatformUnit() override;
 
        void onStartPlatformUnit() override;
 
        void onExitPlatformUnit() override;
 
        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;
 
        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;
        VirtualRobot::RobotPtr m_robot;
        DynamicObstacleManagerInterfacePrx m_obsman;
 
        ObstacleAwarePlatformUnit::control_loop m_control_loop;
        ObstacleAwarePlatformUnit::control_data m_control_data;
 
        mutable PIDController m_rot_pid_controller{1.0,
                                                   0.00009,
                                                   0.0,
                                                   std::numeric_limits<double>::max(),
                                                   std::numeric_limits<double>::max(),
                                                   true};
 
        visualization m_viz;
    };
 
} // namespace armarx