Open
Description
BodyNodeVectorField added in #268 relies on having an upper bound on the integration step size to prevent the integrator from stepping out of joint limits. The RK4(5) integrator that we are currently using does not have a maximum step size, so we currently set this parameter to the initial step size. This is incorrect because the step size may increase - meaning that the integrator may step outside of joint limits.
It would be better to modify the step size controller to reject steps that violate the Testable constraint that the planner is enforcing. This likely requires a similar set of changes as #275.