From 244ad4717a9609796a780d71b17665e6b73a6597 Mon Sep 17 00:00:00 2001 From: Joe Moore Date: Mon, 17 Nov 2025 11:33:01 +0000 Subject: [PATCH] Support for optimising IK solvers. Use minimum of the remaining stage timeout and the default solver timeout for each IK solve attempt --- core/src/stages/compute_ik.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 9593f95e6..c1da6e7c0 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -413,7 +413,8 @@ void ComputeIK::compute() { tried_current_state_as_seed = true; size_t previous = ik_solutions.size(); - bool succeeded = sandbox_state.setFromIK(jmg, target_pose, link->getName(), remaining_time, is_valid); + auto iteration_timeout = std::min(remaining_time, jmg->getDefaultIKTimeout()); + bool succeeded = sandbox_state.setFromIK(jmg, target_pose, link->getName(), iteration_timeout, is_valid); auto now = std::chrono::steady_clock::now(); remaining_time -= std::chrono::duration(now - start_time).count();