mdl: make Joint::clamp terminate for any finite q

[webertob]

Joint::clamp normalises a wraparound joint by repeated subtraction:

while (q(i) > this->max(i)) { q(i) -= range; }
while (q(i) < this->min(i)) { q(i) += range; }

Neither loop has a termination guarantee. Once |q| is large enough that q - range == q in double precision the value stops changing and the loop never exits — on finite input, with an ordinary range of 2π. A range of zero does the same immediately.

Why it is reachable

JacobianInverseKinematics::solve → Metric::step → Joint::step → Joint::clamp. A solve that diverges can produce a large iterate, and the loop sits below solve()s own iteration and duration checks, so neither bounds it. The calling thread spins with no error and no way to interrupt it.

We hit this embedding the library in a Unity plugin: entering play mode froze the editor with one core pinned, no exception and nothing logged. A debugger put the thread here:

rl::mdl::Joint::clamp
rl::mdl::Joint::step
rl::mdl::Metric::step
rl::mdl::JacobianInverseKinematics::solve

The change

Normalise with fmod — one step for any magnitude, cannot spin. Inputs fmod cannot express, a non-finite q or a degenerate range, fall through to the existing rl::std17::clamp, so an infinity lands on the limit and a NaN stays a NaN for the caller to notice.

For inputs where the old loop terminated the result is unchanged: both map q into [min, max] by whole multiples of the range.

Happy to adjust style or split the guard from the normalisation if you would prefer them separate.

[Copilot]

Pull request overview

This PR updates rl::mdl::Joint::clamp to avoid potential non-termination when normalizing wraparound joints by replacing repeated subtraction/addition loops with a std::fmod-based normalization, and falling back to rl::std17::clamp for non-finite inputs or degenerate ranges.

Changes:

• Add <cmath> and switch wraparound normalization to use std::fmod for constant-time wrapping.
• Add guards to avoid fmod for non-finite q or zero/degenerate ranges, falling back to rl::std17::clamp.
• Add explanatory in-code documentation describing the previous infinite-loop failure mode and the new behavior.

Suppressed comments (1)

src/rl/mdl/Joint.cpp:95

• fmod(q - min, range) maps exact multiples of range to 0, which makes q(i) == max(i) (and q(i) == max(i) + k*range) normalize to min(i) instead of staying at max(i) as the previous while-loop implementation did. This is a behavioral change for continuous revolute joints (e.g., [-pi, pi]) and contradicts the PR description that results are unchanged when the old loop terminated.

					::rl::math::Real wrapped = ::std::fmod(q(i) - this->min(i), range);
				
					if (wrapped < 0)
					{
						wrapped += range;
					}
				
					q(i) = this->min(i) + wrapped;

---

💡 Add a code-review agent skill or configure MCP servers for context-aware, tailored reviews. Learn more in the docs.