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

source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp

@@ -551,28 +551,29 @@ armarx::ObstacleAvoidingPlatformUnit::post_process_vel(
     // Smooth by calculating mean over the last `m_control_data.amount_smoothing` elements.
     const Eigen::Vector2f mean_modulated_vel = calculate_mean_modulated_vel();
 
-    // Determine max velocity by considering the coherency between the last
-    // `m_control_data.amount_max_vel` pairs of desired and modulated velocities.
-    const float max_vel = calculate_adaptive_max_vel();
+    const bool is_near_target = this->is_near_target(mean_modulated_vel);
 
-    ARMARX_CHECK(mean_modulated_vel.allFinite());
-    ARMARX_CHECK(std::isfinite(max_vel));
+    // Min velocity is determined through distance to target.
+    const float min_vel = is_near_target ? m_control_data.min_vel_near_target : m_control_data.min_vel_general;
 
+    // Determine max velocity by considering the coherency between the last
+    // `m_control_data.amount_max_vel` pairs of desired and modulated velocities.
     // If the robot is almost at the goal position and no collision needs to be avoided
     // (aka moving into the direction of the target position), we can prevent overshooting by
     // clipping the velocity. The reference velocity is computed by a standard P-controller.
-    if (is_almost_reaching_target(mean_modulated_vel))
-    {
-        return simox::math::norm_max(mean_modulated_vel, target_vel.norm());
-    }
+    const float max_vel = is_near_target ? target_vel.norm() : calculate_adaptive_max_vel();
+
+    ARMARX_CHECK(mean_modulated_vel.allFinite());
+    ARMARX_CHECK(std::isfinite(min_vel));
+    ARMARX_CHECK(std::isfinite(max_vel));
 
-    return simox::math::norm_max(mean_modulated_vel, max_vel);
+    return simox::math::norm_clamp(mean_modulated_vel, min_vel, max_vel);
 }
 
 
-bool armarx::ObstacleAvoidingPlatformUnit::is_almost_reaching_target(const Eigen::Vector2f& control_velocity) const noexcept
+bool armarx::ObstacleAvoidingPlatformUnit::is_near_target(const Eigen::Vector2f& control_velocity) const noexcept
 {
-    if (m_control_data.target_dist < 300) // FIXME param
+    if (m_control_data.target_dist < m_control_data.pos_near_threshold)
     {
         const Eigen::Vector2f target_direction = m_control_data.target_pos - m_control_data.agent_pos;
         const Eigen::Vector2f control_direction = control_velocity / control_velocity.norm();
@@ -858,6 +859,13 @@ armarx::ObstacleAvoidingPlatformUnit::createPropertyDefinitions()
                   "Adaptive max vel exponent.  This throttles the max_vel adaptively "
                   "depending on the angle between target velocity and modulated velocity.  "
                   "0 = disable.");
+    def->optional(m_control_data.min_vel_near_target, "min_vel_near_target", "Velocity in [mm/s] "
+                  "the robot should at least set when near the target");
+    def->optional(m_control_data.min_vel_general, "min_vel_general", "Velocity in [mm/s] the robot "
+                  "should at least set on general");
+    def->optional(m_control_data.pos_near_threshold, "pos_near_threshold", "Distance in [mm] after "
+                  "which the robot is considered to be near the target for min velocity, "
+                  "smoothing, etc.");
 
     // Control parameters.
     def->optional(m_control_data.kp, "control.pos.kp");

source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h

@@ -94,8 +94,11 @@ namespace armarx
             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;
@@ -257,9 +260,8 @@ namespace armarx
         void
         visualize(const velocities& vels);
 
-
         bool
-        is_almost_reaching_target(const Eigen::Vector2f& control_velocity)
+        is_near_target(const Eigen::Vector2f& control_velocity)
         const
         noexcept;