[core] Do not enforce not configuration bounds at init for spring-dam…

…per contact model.
duburcqa · Oct 17, 2023 · b103a23 · b103a23
1 parent 5572cbf
commit b103a23
Showing 1 changed file with 13 additions and 6 deletions.
diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc
@@ -1128,14 +1128,21 @@ namespace jiminy
                 return hresult_t::ERROR_BAD_INPUT;
             }
 
-            // Note that EPS allows to be very slightly out-of-bounds
-            if ((system.robot->mdlOptions_->joints.enablePositionLimit &&
+            /* Check that the initial configuration is not out-of-bounds if appropriate
+               Note that EPS allows to be very slightly out-of-bounds, which may occurs because of rounding errors. */
+            if ((system.robot->mdlOptions_->joints.enablePositionLimit && (contactModel_ == contactModel_t::CONSTRAINT) &&
                  ((EPS < q.array() - system.robot->getPositionLimitMax().array()).any() ||
-                  (EPS < system.robot->getPositionLimitMin().array() - q.array()).any())) ||
-                (system.robot->mdlOptions_->joints.enableVelocityLimit &&
-                 (EPS < v.array().abs() - system.robot->getVelocityLimit().array()).any()))
+                  (EPS < system.robot->getPositionLimitMin().array() - q.array()).any())))
             {
-                PRINT_ERROR("The initial configuration or velocity is out-of-bounds for system '", system.name, "'.");
+                PRINT_ERROR("The initial configuration is out-of-bounds for system '", system.name, "'.");
+                return hresult_t::ERROR_BAD_INPUT;
+            }
+
+            // Check that the initial velocity is not out-of-bounds
+            if ((system.robot->mdlOptions_->joints.enableVelocityLimit &&
+                 (system.robot->getVelocityLimit().array() < v.array().abs()).any()))
+            {
+                PRINT_ERROR("The initial velocity is out-of-bounds for system '", system.name, "'.");
                 return hresult_t::ERROR_BAD_INPUT;
             }