diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index cf81f039..b78bead1 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -1855,7 +1855,7 @@ static mjtNum PrimalSearch(mjPrimalContext* ctx, mjtNum tolerance, mjtNum ls_ite PrimalEval(ctx, &p1); // check for initial convergence - if (mju_abs(p1.deriv[0]) < gtol) { + if (mju_abs(p1.deriv[0]) < gtol && (p1.alpha == 0 || p1.cost < 0)) { if (p1.alpha == 0) { ctx->LSresult = 2; // no improvement, initial convergence } else { @@ -1916,7 +1916,7 @@ static mjtNum PrimalSearch(mjPrimalContext* ctx, mjtNum tolerance, mjtNum ls_ite PrimalEval(ctx, &p1); // check for convergence - if (mju_abs(p1.deriv[0]) < gtol) { + if (mju_abs(p1.deriv[0]) < gtol && p1.cost < 0) { ctx->LSslope = mju_abs(p1.deriv[0])*slopescl; *improvement = -p1.cost; return p1.alpha; // SUCCESS