Move from string-based links and joints to id-based logic (using hashes)

[rjoomen]

Requires tesseract-robotics/tesseract#1274 and tesseract-robotics/tesseract_planning#734

[Copilot]

Pull request overview

Note

Copilot was unable to run its full agentic suite in this review.

This PR migrates trajectory-optimization and collision code from string-based link/joint identifiers to LinkId/JointId-based identifiers (hashed IDs), and updates collision evaluation/config to match.

Changes:

• Replace string link/frame usage with tesseract::common::LinkId/JointId across trajopt and trajopt_ifopt code and tests.
• Update collision coefficient utilities/serialization to use LinkIdPair keys and add deterministic ordering for continuous-collision constraint rows.
• Switch contact manager plugin defaults to Coal managers and add new octomap regression tests/configs.

Reviewed changes

Copilot reviewed 54 out of 54 changed files in this pull request and generated 6 comments.

Show a summary per file

File	Description
trajopt_common/src/collision_types.cpp	Reworks CollisionCoeffData storage/overloads around LinkIdPair and updates equality logic
trajopt_common/include/trajopt_common/yaml_extensions.h	Updates YAML emission for pair coefficient data using LinkId names
trajopt/src/file_write_callback.cpp	Updates FK output map key type to LinkId
trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp	Adds deterministic iteration order via sorted contact-pair keys
trajopt_common/data/contact_manager_plugins.yaml	Changes default collision managers to Coal and adds Coal plugin entries
trajopt/test/cast_cost_octomap_arm_unit.cpp / trajopt/test/cast_constraint_octomap_unit.cpp	Adds new octomap-focused backend comparison/constraint tests
trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp	Adjusts OSQP settings (adaptive rho + higher max iterations)

Comments suppressed due to low confidence (2)

trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp:1

• Sorting ContactResultMap::KeyType relies on operator< being well-defined for that type. With the move to ID-based keys, this may no longer be true (or may sort by an implementation detail). To keep determinism robust and compilation-independent, provide an explicit comparator (e.g., compare (first.value(), second.value()) or (first.name(), second.name())) matching the intended stable ordering.
  trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp:1
• Increasing OSQP max iterations 4× (and enabling adaptive rho) can significantly increase test runtime and mask convergence/regression issues (tests may “eventually” converge instead of exposing a configuration or modeling problem). If this is needed only for a specific scenario, consider tightening the problem setup/tolerances or making these settings localized/conditional so the broader test suite doesn’t become slower.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

Changing the default discrete/continuous managers from Bullet to Coal is a behavior change that can break existing deployments where Coal plugins aren’t present/loaded, and it changes collision results/gradients by default. Consider keeping Bullet as the default and adding Coal as an opt-in (or selecting Coal only when the plugin is available). Also verify the search_libraries entry name (tesseract_collision_coal_coal_factories) matches the actual built plugin library; a mismatch will prevent plugin discovery at runtime.

[Copilot]

Changing the default discrete/continuous managers from Bullet to Coal is a behavior change that can break existing deployments where Coal plugins aren’t present/loaded, and it changes collision results/gradients by default. Consider keeping Bullet as the default and adding Coal as an opt-in (or selecting Coal only when the plugin is available). Also verify the search_libraries entry name (tesseract_collision_coal_coal_factories) matches the actual built plugin library; a mismatch will prevent plugin discovery at runtime.