Merge branch 'fix/obstacle_avoiding_controller_behavior' of gitlab.com:ArmarX/RobotAPI

source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp

@@ -24,26 +24,28 @@
 #include <RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h>
 
 
-// STD/STL
-#include <limits>
-
 // Simox
 #include <SimoxUtility/math.h>
 
 
+armarx::ObstacleAvoidingPlatformUnitHelper::ObstacleAvoidingPlatformUnitHelper(
+    armarx::PlatformUnitInterfacePrx platform_unit,
+    VirtualRobot::RobotPtr robot) :
+    ObstacleAvoidingPlatformUnitHelper{platform_unit, robot, Config{}}
+{
+    // pass
+}
+
+
 armarx::ObstacleAvoidingPlatformUnitHelper::ObstacleAvoidingPlatformUnitHelper(
     armarx::PlatformUnitInterfacePrx platform_unit,
     VirtualRobot::RobotPtr robot,
-    const float pos_reached_threshold,
-    const float ori_reached_threshold) :
+    const Config& cfg) :
     m_platform_unit{platform_unit},
     m_robot{robot},
-    m_pos_reached_threshold{pos_reached_threshold},
-    m_ori_reached_threshold{ori_reached_threshold}
+    m_cfg{cfg}
 {
-    const float inf = std::numeric_limits<float>::infinity();
-    m_target.pos = Eigen::Vector2f{inf, inf};
-    m_target.ori = inf;
+    // pass
 }
 
 
@@ -58,36 +60,119 @@ armarx::ObstacleAvoidingPlatformUnitHelper::setTarget(
     const Eigen::Vector2f& target_pos,
     const float target_ori)
 {
-    m_target.pos = target_pos;
-    m_target.ori = target_ori;
+    setTarget(Target{target_pos, target_ori});
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnitHelper::setTarget(const Target& target)
+{
+    m_waypoints.clear();
+    m_waypoints.push_back(target);
+    m_current_waypoint_index = 0;
+    m_waypoint_changed = true;
+}
+
+
+armarx::ObstacleAvoidingPlatformUnitHelper::Target
+armarx::ObstacleAvoidingPlatformUnitHelper::getCurrentTarget()
+const
+{
+    return m_waypoints.at(m_current_waypoint_index);
 }
 
 
 void
 armarx::ObstacleAvoidingPlatformUnitHelper::update()
 {
-    m_platform_unit->moveTo(m_target.pos.x(), m_target.pos.y(), m_target.ori,
-                            m_pos_reached_threshold, m_ori_reached_threshold);
+    // Skip to next waypoint if in proximity.
+    if (isCurrentTargetNear() and not isLastWaypoint())
+    {
+        m_current_waypoint_index++;
+        m_waypoint_changed = true;
+    }
+
+    // Only call `moveTo` on platform unit if the target actually changed, as `update` is designed
+    // to be called within a high-frequency loop.  Prevents constantly invalidating the buffers in
+    // platform unit.
+    if (m_waypoint_changed)
+    {
+        // Use reached-thresholds regardless of whether this is the final target or just a waypoint.
+        // The near-thresholds are more unconstrained as the reached-thresholds, so this helper will
+        // change the position target before the unit will actually reach it, preventing slow downs
+        // or stops.
+        const float pos_thresh = m_cfg.pos_reached_threshold;
+        const float ori_thresh = m_cfg.ori_reached_threshold;
+
+        // Control.
+        const Target target = getCurrentTarget();
+        m_platform_unit->moveTo(target.pos.x(), target.pos.y(), target.ori, pos_thresh, ori_thresh);
+
+        m_waypoint_changed = false;
+    }
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnitHelper::addWaypoint(const Eigen::Vector2f& waypoint_pos, float waypoint_ori)
+{
+    addWaypoint(Target{waypoint_pos, waypoint_ori});
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnitHelper::setWaypoints(const std::vector<Target>& waypoints)
+{
+    m_waypoints = waypoints;
+    m_current_waypoint_index = 0;
+    m_waypoint_changed = true;
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnitHelper::addWaypoint(const Target& waypoint)
+{
+    m_waypoints.push_back(waypoint);
 }
 
 
 bool
-armarx::ObstacleAvoidingPlatformUnitHelper::isFinalTargetReached()
+armarx::ObstacleAvoidingPlatformUnitHelper::isLastWaypoint()
 const
 {
-    using namespace simox::math;
+    return m_current_waypoint_index + 1 >= m_waypoints.size();
+}
 
-    // Determine agent position.
-    const Eigen::Vector2f agent_pos = m_robot->getGlobalPosition().head<2>();
-    const float agent_ori =
-        periodic_clamp<float>(mat4f_to_rpy(m_robot->getGlobalPose()).z(), -M_PI, M_PI);
 
-    // Determine distance and angular distance to goal position and orientation.
-    const float target_dist = (m_target.pos - agent_pos).norm();
-    const float target_angular_dist = periodic_clamp<float>(m_target.ori - agent_ori, -M_PI, M_PI);
+bool
+armarx::ObstacleAvoidingPlatformUnitHelper::isCurrentTargetNear()
+const
+{
+    return getPositionError() < m_cfg.pos_near_threshold and getOrientationError() < m_cfg.ori_near_threshold;
+}
+
 
-    return target_dist < m_pos_reached_threshold and
-           std::fabs(target_angular_dist) < m_ori_reached_threshold;
+bool
+armarx::ObstacleAvoidingPlatformUnitHelper::isCurrentTargetReached()
+const
+{
+    return getPositionError() < m_cfg.pos_reached_threshold and getOrientationError() < m_cfg.ori_reached_threshold;
+}
+
+
+bool
+armarx::ObstacleAvoidingPlatformUnitHelper::isFinalTargetNear()
+const
+{
+    return isLastWaypoint() and isCurrentTargetNear();
+}
+
+
+bool
+armarx::ObstacleAvoidingPlatformUnitHelper::isFinalTargetReached()
+const
+{
+    return isLastWaypoint() and isCurrentTargetReached();
 }
 
 
@@ -96,3 +181,24 @@ armarx::ObstacleAvoidingPlatformUnitHelper::setMaxVelocities(float max_vel, floa
 {
     m_platform_unit->setMaxVelocities(max_vel, max_angular_vel);
 }
+
+
+float
+armarx::ObstacleAvoidingPlatformUnitHelper::getPositionError()
+const
+{
+    const Eigen::Vector2f agent_pos = m_robot->getGlobalPosition().head<2>();
+    return (getCurrentTarget().pos - agent_pos).norm();
+}
+
+
+float
+armarx::ObstacleAvoidingPlatformUnitHelper::getOrientationError()
+const
+{
+    using namespace simox::math;
+
+    const float agent_ori =
+        periodic_clamp<float>(mat4f_to_rpy(m_robot->getGlobalPose()).z(), -M_PI, M_PI);
+    return std::fabs(periodic_clamp<float>(getCurrentTarget().ori - agent_ori, -M_PI, M_PI));
+}

source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h

@@ -42,11 +42,30 @@ namespace armarx
 
     public:
 
+        struct Config
+        {
+            float pos_reached_threshold = 10;  // [mm]
+            float ori_reached_threshold = 0.1;  // [rad]
+            float pos_near_threshold = 50;  // [mm]
+            float ori_near_threshold = 0.2;  // [rad]
+        };
+
+        struct Target
+        {
+            Eigen::Vector2f pos;
+            float ori;
+        };
+
+    public:
+
+        ObstacleAvoidingPlatformUnitHelper(
+            armarx::PlatformUnitInterfacePrx platform_unit,
+            VirtualRobot::RobotPtr robot);
+
         ObstacleAvoidingPlatformUnitHelper(
             armarx::PlatformUnitInterfacePrx platform_unit,
             VirtualRobot::RobotPtr robot,
-            float pos_reached_threshold,
-            float ori_reached_threshold);
+            const Config& cfg);
 
         virtual
         ~ObstacleAvoidingPlatformUnitHelper();
@@ -54,9 +73,40 @@ namespace armarx
         void
         setTarget(const Eigen::Vector2f& target_pos, float target_ori);
 
+        void
+        setTarget(const Target& target);
+
+        Target
+        getCurrentTarget() const;
+
         void
         update();
 
+        void
+        setWaypoints(const std::vector<Target>& waypoints);
+
+        void
+        addWaypoint(const Eigen::Vector2f& waypoint_pos, float waypoint_ori);
+
+        void
+        addWaypoint(const Target& waypoint);
+
+        bool
+        isLastWaypoint()
+        const;
+
+        bool
+        isCurrentTargetNear()
+        const;
+
+        bool
+        isCurrentTargetReached()
+        const;
+
+        bool
+        isFinalTargetNear()
+        const;
+
         bool
         isFinalTargetReached()
         const;
@@ -64,22 +114,25 @@ namespace armarx
         void
         setMaxVelocities(float max_vel, float max_angular_vel);
 
-    private:
+        float
+        getPositionError()
+        const;
 
-        struct target
-        {
-            Eigen::Vector2f pos;
-            float ori;
-        };
+        float
+        getOrientationError()
+        const;
+
+    private:
 
         armarx::PlatformUnitInterfacePrx m_platform_unit;
 
         VirtualRobot::RobotPtr m_robot;
 
-        target m_target;
+        std::vector<Target> m_waypoints;
+        unsigned int m_current_waypoint_index = 0;
+        bool m_waypoint_changed = false;
 
-        float m_pos_reached_threshold;
-        float m_ori_reached_threshold;
+        Config m_cfg;
 
     };