feature: First draft of obstacle aware platform controller.

source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp

@@ -239,6 +239,35 @@ namespace armarx
         ARMARX_INFO << "Finished waiting for obstacles";
     }
 
+    float
+    DynamicObstacleManager::distanceToObstacle(const Eigen::Vector2f& position, const Eigen::Vector2f& goal, const Ice::Current&)
+    {
+        std::shared_lock<std::shared_mutex> l{m_managed_obstacles_mutex};
+
+        const float sample_step = 50; // in [mm], towards goal.
+        const float distance_to_goal = (goal - position).norm();
+        float current_distance = sample_step;
+
+        while (sample_step < distance_to_goal)
+        {
+            for (const auto& man_obstacle : m_managed_obstacles)
+            {
+                Eigen::Vector2f sample = position + ((goal - position).normalized() * sample_step);
+                obstacledetection::Obstacle obstacle = man_obstacle->m_obstacle;
+                Eigen::Vector2f obstacle_origin{obstacle.posX, obstacle.posY};
+
+                if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin, obstacle.axisLengthX, obstacle.axisLengthY, obstacle.yaw, sample))
+                {
+                    return current_distance;
+                }
+            }
+
+            current_distance += sample_step;
+        }
+
+        return std::numeric_limits<float>::infinity();
+    }
+
 
     void DynamicObstacleManager::directly_update_obstacle(const std::string& name, const Eigen::Vector2f& obs_pos, float e_rx, float e_ry, float e_yaw, const Ice::Current&)
     {

source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h

@@ -73,6 +73,7 @@ namespace armarx
         void directly_update_obstacle(const std::string& name, const Eigen::Vector2f&, float, float, float, const Ice::Current& = Ice::Current()) override;
         void remove_obstacle(const std::string& name, const Ice::Current& = Ice::Current()) override;
         void wait_unitl_obstacles_are_ready(const Ice::Current& = Ice::Current()) override;
+        float distanceToObstacle(const Eigen::Vector2f& position, const Eigen::Vector2f& goal, const Ice::Current& = Ice::Current()) override;
 
     protected:
         void onInitComponent() override;

source/RobotAPI/components/units/CMakeLists.txt

@@ -75,6 +75,7 @@ set(LIB_FILES
 armarx_add_library("${LIB_NAME}"  "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
 
 add_subdirectory(ObstacleAvoidingPlatformUnit)
+add_subdirectory(ObstacleAwarePlatformUnit)
 add_subdirectory(relays)
 add_subdirectory(RobotUnit)
 

source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt

+armarx_add_component(
+    COMPONENT_NAME  ObstacleAwarePlatformUnit
+    HEADERS         ObstacleAwarePlatformUnit.h
+    SOURCES         ObstacleAwarePlatformUnit.cpp
+    COMPONENT_LIBS  ArmarXCoreComponentPlugins
+                    RobotAPICore
+                    RobotAPIComponentPlugins
+                    RobotUnit
+)
+armarx_add_component_executable("main.cpp")

source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h

+/*
+ * 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 <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/DynamicObstacleManagerInterface.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.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;
+        };
+
+        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};
+
+        visualization m_viz;
+
+    };
+
+}

source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp

+/*
+ * 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
+ */
+
+
+// STD/STL
+#include <string>
+
+// ArmarX
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+
+// RobotAPI
+#include <RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h>
+
+
+int main(int argc, char* argv[])
+{
+    using namespace armarx;
+    const std::string name = ObstacleAwarePlatformUnit::default_name;
+    return runSimpleComponentApp<ObstacleAwarePlatformUnit>(argc, argv, name);
+}

source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice

@@ -48,6 +48,8 @@ module armarx
         remove_obstacle(string name);
 
         void wait_unitl_obstacles_are_ready();
+
+        float distanceToObstacle(Eigen::Vector2f position, Eigen::Vector2f goal);
     };
 };