Joint reordering utilities leave tolerance/velocity/acceleration/effort vectors in original order

[rjoomen]

Summary

Two related sites in tesseract_planning reorder a waypoint's joint identifiers and position vector but leave joint-correlated auxiliary vectors in the original order. After the call, those auxiliary vectors silently correspond to the wrong joints.

Site 1 — formatJointPosition

tesseract::command_language::formatJointPosition in command_language/src/utils.cpp reorders position (and any seed.position for Cartesian waypoints) to match a caller-supplied joint ordering, then overwrites the stored joint identifiers. It does not permute:

• JointWaypoint::lower_tolerance_, JointWaypoint::upper_tolerance_
• StateWaypoint → JointState::velocity, acceleration, effort
• CartesianWaypoint seed velocity / acceleration / effort

Both the pre-existing std::vectorstd::string overload on master and the new std::vector overload introduced by PR #734 have this behavior — the ID overload faithfully preserved the older logic.

Site 2 — FormatAsInputTask

task_composer/planning/src/nodes/format_as_input_task.cpp:144-151:

auto& jwp = mi.getWaypoint().as();
if (!jwp.isConstrained() || (jwp.isConstrained() && jwp.isToleranced()))
{
jwp.setJointIds(getJointIds(umi.getWaypoint()));
jwp.setPosition(getJointPosition(umi.getWaypoint()));
}

This path specifically targets toleranced JointWaypoints. It replaces both joint_ids_ and position_ with the post-planning ordering but never touches lower_tolerance_ / upper_tolerance_. If post-planning joint order differs from the user's input ordering — the normal case, since otherwise the reformatting step
would be a no-op — the tolerances silently mis-associate.

Impact

formatProgram is invoked from FormatPlanningInputTask (task_composer/planning/src/nodes/format_planning_input_task.cpp:111) on every planning request that uses the standard input-formatting pipeline, so Site 1 is on the hot path whenever that task is wired up. Site 2 affects the same planner hookups that use
FormatAsInputTask to push post-planning state back into the pre-planning program.

Downstream, trajopt_ifopt_utils.cpp:109-110 consumes the tolerances as:

lower_limit = position + getLowerTolerance();
upper_limit = position + getUpperTolerance();

A mis-ordered tolerance vector produces incorrect joint bounds for the optimizer. Symptoms:

• Silent: no exception, no assertion.
• Only triggers when the supplied ordering differs from the ordering already stored in the waypoint — which is exactly the reason these utilities get called.

Reproduction

Any program whose waypoint joint ordering differs from the manipulator's getJointNames() / getJointIds(), run through either formatProgram (Site 1) or FormatAsInputTask (Site 2). The tolerance-bound mismatch is hard to spot without instrumenting the trajopt constraints.

Proposed fix

For both sites:

1. Compute the permutation index map once (same approach already used for position in Site 1).
2. Apply the same permutation to every joint-correlated vector on the waypoint when its size matches position.
3. Handle mismatched/empty auxiliary vectors defensively — skip when the auxiliary vector is empty, throw when non-empty and size doesn't match position.
4. Add unit tests that exercise a non-identity permutation and verify all auxiliary vectors move with position. Include both JointWaypoint (tolerances) and StateWaypoint (velocity/acceleration/effort) cases, plus the FormatAsInputTask path.