[BUG] DART IK problem instances don’t have same bounds as robot

[ell0ry]

Expected Behavior

When asking robowflex::darts::TSR for a solution, one is provided that both satisfies the underlying TSR constraint and the allowed bounds for the robot.

Current Behavior

Sometimes, a collision satisfying configuration is provided that doesn’t obey the robot’s bounds. This is due to the associated DART IK module’s (robowflex::darts::TSR::ik_) problem not having the correct bounds set. Currently, they are all set to (-infinity, infinity).

Possible Solution

When setting up the robowflex::darts::TSR for a given state space, look for the associated robot and manually set the bounds. This involves getting the robot object, getting the associated joint bounds, and setting them correctly

[ell0ry]

Attempted to solve this by getting the problem instance and setting the bounds prior to using robowflex::darts::TSR::ik_->getProblem()->setLowerBounds() and robowflex::darts::TSR::ik_->getProblem()->setUpperBounds(). This didn’t actually seem to work as solutions were still provided with invalid joint values.

Looking into the source code for dart::dynamics::InverseKinematics, whenever we call anything that solves the IK instance, the problem bounds are reset here. The bounds are set to those from the skeleton of the body node associated with the IK module.