Remove magic numbers in MANNAutoregressiveInputBuilder

ami-iit · Jun 5, 2023 · d3d5b3c · d3d5b3c
1 parent 7427ba9
commit d3d5b3c
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 2 deletions.
diff --git a/src/ML/include/BipedalLocomotion/ML/MANNAutoregressiveInputBuilder.h b/src/ML/include/BipedalLocomotion/ML/MANNAutoregressiveInputBuilder.h
@@ -69,6 +69,8 @@ class MANNAutoregressiveInputBuilder
      * | `max_facing_direction_angle_side_opposite_sign` | `double` |         Maximum angle of the facing direction when the robot is walking lateral and is facing towards the walking direction         |    Yes    |
      * |   `max_facing_direction_angle_side_same_sign`   | `double` | Maximum angle of the facing direction when the robot is walking lateral and is facing towards the opposite of the walking direction |    Yes    |
      * |                `number_of_knots`                |   `int`  |                                            Number of knots considered in the Bezier curve                                           |    Yes    |
+     * |          `forward_direction_threshold`          | `double` |     The minimum value for the normalized user-specified motion direction to be interpreted as forward motion (Default value 0.2)    |    No     |
+     * |            `side_direction_threshold`           | `double` |       The minimum value for the normalized user-specified motion direction to be interpreted as side motion (Default value 0.2)     |    No     |
      * @return True in case of success, false otherwise.
      */
     bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;

diff --git a/src/ML/src/MANNAutoregressiveInputBuilder.cpp b/src/ML/src/MANNAutoregressiveInputBuilder.cpp
@@ -36,6 +36,8 @@ struct MANNAutoregressiveInputBuilder::Impl
     double maxFacingDirectionAngleBackward;
     double maxFacingDirectionAngleSideOppositeSign;
     double maxFacingDirectionAngleSideSameSign;
+    double forwardDirectionThreshold{0.2};
+    double sideDirectionThreshold{0.2};
     int numberOfKnots;
 
     enum class FSM
@@ -57,12 +59,12 @@ double MANNAutoregressiveInputBuilder::Impl::evaluateMaxFacingDirectionAngle(
     Eigen::Ref<const Eigen::Vector2d> normalizedMotionDirection,
     Eigen::Ref<const Eigen::Vector2d> rawFacingDirection) const
 {
-    if (normalizedMotionDirection[0] < 0.2)
+    if (normalizedMotionDirection[0] < this->forwardDirectionThreshold)
     {
         return maxFacingDirectionAngleBackward;
     }
 
-    if (std::abs(normalizedMotionDirection[1]) > 0.2)
+    if (std::abs(normalizedMotionDirection[1]) > this->sideDirectionThreshold)
     {
         // same sign
         if (normalizedMotionDirection[1] * rawFacingDirection[1] >= 0)
@@ -162,6 +164,30 @@ bool MANNAutoregressiveInputBuilder::initialize(
         return true;
     };
 
+    auto getOptionalParameter
+        = [logPrefix, ptr](const std::string& paramName, auto& param) -> void {
+        typename std::remove_reference<decltype(param)>::type temp;
+
+        if (!ptr->getParameter(paramName, temp))
+        {
+            log()->info("{} Unable to find the parameter {}. The default one will be used {}.",
+                        logPrefix,
+                        paramName,
+                        param);
+            return;
+        }
+        if (temp <= 0)
+        {
+            log()->error("{} The parameter {} cannot be negative. The default one will be used {}.",
+                         logPrefix,
+                         paramName,
+                         param);
+            return;
+        }
+        param = temp;
+        return;
+    };
+
     // get all the required parameters
     bool ok = getParameter("base_vel_norm", m_pimpl->baseVelocityNorm);
     ok = ok && getParameter("ellipsoid_forward_axis", m_pimpl->ellipsoidForwardAxis);
@@ -200,6 +226,9 @@ bool MANNAutoregressiveInputBuilder::initialize(
     m_pimpl->curve.initialPoint.setZero();
     m_pimpl->frontalDirection << 1, 0;
 
+    getOptionalParameter("forward_direction_threshold", m_pimpl->forwardDirectionThreshold);
+    getOptionalParameter("side_direction_threshold", m_pimpl->sideDirectionThreshold);
+
     m_pimpl->fsm = Impl::FSM::Initialized;
 
     return ok;