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

[rjoomen]

Requires tesseract-robotics/tesseract#1274

[Copilot]

Pull request overview

This PR migrates motion-planning and command-language code from string-based link/joint identification to id-based logic (e.g., LinkId, JointId), enabling hash/id-driven lookups across kinematics, collision checking, planners, and examples.

Changes:

• Replace link/joint name strings with LinkId/JointId in manipulator info, collision configuration, FK transform maps, and waypoint seeds.
• Update command-language waypoint models + utilities to store/operate on joint IDs and adjust cereal serialization accordingly.
• Update planners/tests/examples to the new id-based APIs, including active collision object configuration and FK transform handling.

Reviewed changes

Copilot reviewed 85 out of 85 changed files in this pull request and generated 2 comments.

Show a summary per file

File	Description
time_parameterization/totg/test/time_optimal_trajectory_generation_tests.cpp	Update ManipulatorInfo frames to LinkId.
time_parameterization/ruckig/test/ruckig_trajectory_smoothing_tests.cpp	Update ManipulatorInfo frames to LinkId.
time_parameterization/kdl/test/constant_tcp_speed_parameterization_tests.cpp	Update ManipulatorInfo frames to LinkId.
time_parameterization/kdl/src/constant_tcp_speed_parameterization.cpp	Switch KDL FK transform map + scene graph parsing to id/name accessors.
time_parameterization/isp/test/iterative_spline_tests.cpp	Update ManipulatorInfo frames to LinkId.
task_composer/test/tesseract_task_composer_planning_unit.cpp	Use LinkId frames and seed JointState via joint IDs.
task_composer/test/fix_state_bounds_task_unit.cpp	Construct ManipulatorInfo using LinkId frames.
task_composer/planning/src/nodes/format_as_result_task.cpp	Populate StateWaypoint using joint IDs.
task_composer/planning/src/nodes/format_as_input_task.cpp	Seed/format joint waypoints using joint IDs.
task_composer/planning/src/nodes/fix_state_collision_task.cpp	Convert collision checking to active link IDs + id-based FK state.
task_composer/planning/src/nodes/discrete_contact_check_task.cpp	Use active link IDs for discrete contact checks.
task_composer/planning/src/nodes/continuous_contact_check_task.cpp	Use active link IDs for continuous contact checks.
task_composer/core/include/tesseract/task_composer/test_suite/test_programs.hpp	Construct ManipulatorInfo using LinkId frames in test programs.
motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_move_profile.cpp	Build joint waypoints from state waypoints via joint IDs.
motion_planners/trajopt/test/trajopt_planner_tests.cpp	Update tests to use LinkId working/tcp frames.
motion_planners/trajopt/src/trajopt_utils.cpp	Update TrajOpt term-info utilities to accept LinkId parameters.
motion_planners/trajopt/src/profile/trajopt_default_move_profile.cpp	Update active-link checks + fixed joint waypoint creation to joint IDs.
motion_planners/trajopt/include/tesseract/motion_planners/trajopt/trajopt_utils.h	Update TrajOpt utility APIs to use LinkId.
motion_planners/simple/test/simple_planner_test_utils.hpp	Update ManipulatorInfo frames to LinkId.
motion_planners/simple/test/simple_planner_lvs_move_unit.cpp	Build explicit JointState seeds using JointId.
motion_planners/simple/test/simple_planner_lvs_assign_no_ik_move_unit.cpp	Build explicit JointState seeds using JointId.
motion_planners/simple/test/simple_planner_lvs_assign_move_unit.cpp	Build explicit JointState seeds using JointId.
motion_planners/simple/test/simple_planner_fixed_size_move_unit.cpp	Build explicit JointState seeds using JointId.
motion_planners/simple/test/simple_planner_fixed_size_assign_move_unit.cpp	Build explicit JointState seeds using JointId.
motion_planners/simple/src/simple_motion_planner.cpp	Format results as input using joint IDs; validate joint order via joint IDs.
motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_move_profile.cpp	Query current joint values via joint IDs.
motion_planners/simple/src/profile/simple_planner_lvs_assign_move_profile.cpp	Query current joint values via joint IDs.
motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_move_profile.cpp	Query current joint values via joint IDs.
motion_planners/simple/src/profile/simple_planner_fixed_size_assign_move_profile.cpp	Query current joint values via joint IDs.
motion_planners/simple/src/interpolation.cpp	Replace string frame checks and FK caches with LinkId/LinkIdTransformMap.
motion_planners/simple/include/tesseract/motion_planners/simple/interpolation.h	Change instruction-info structs to store LinkId frames.
motion_planners/ompl/test/ompl_planner_tests.cpp	Use LinkId link/joint fields and manipulator frames in tests.
motion_planners/ompl/src/utils.cpp	Set collision transforms in bulk using id-based FK result map.
motion_planners/ompl/src/state_collision_validator.cpp	Use active link IDs + id-based FK transform map for validity checks.
motion_planners/ompl/src/profile/ompl_real_vector_move_profile.cpp	Use joint IDs for joint-format checking and active link IDs for collision.
motion_planners/ompl/src/continuous_motion_validator.cpp	Use active link IDs + id-based FK transform maps for motion validation.
motion_planners/ompl/src/collision_cost_objective.cpp	Use active link IDs + id-based FK transform map for cost evaluation.
motion_planners/ompl/include/tesseract/motion_planners/ompl/state_collision_validator.h	Store active link IDs instead of names.
motion_planners/ompl/include/tesseract/motion_planners/ompl/continuous_motion_validator.h	Store active link IDs instead of names.
motion_planners/ompl/include/tesseract/motion_planners/ompl/collision_cost_objective.h	Store active link IDs instead of names.
motion_planners/examples/raster_example.cpp	Update ManipulatorInfo frames to LinkId.
motion_planners/examples/freespace_example.cpp	Update ManipulatorInfo frames to LinkId.
motion_planners/examples/chain_example.cpp	Update ManipulatorInfo frames to LinkId.
motion_planners/descartes/test/descartes_planner_tests.cpp	Update ManipulatorInfo frames to LinkId.
motion_planners/descartes/src/descartes_collision.cpp	Use LinkIdTransformMap and id-based FK results for collision checks.
motion_planners/descartes/include/tesseract/motion_planners/descartes/impl/profile/descartes_default_move_profile.hpp	Validate joint format via joint IDs; pass frame names from LinkId.
motion_planners/descartes/include/tesseract/motion_planners/descartes/impl/descartes_robot_sampler.hpp	Construct IK inputs via LinkId; use joint IDs + id-based FK cache.
motion_planners/descartes/include/tesseract/motion_planners/descartes/impl/descartes_collision_edge_evaluator.hpp	Use active link IDs and id-based collision margin access.
motion_planners/descartes/include/tesseract/motion_planners/descartes/descartes_collision_edge_evaluator.h	Store active link IDs (comment needs update).
motion_planners/descartes/include/tesseract/motion_planners/descartes/descartes_collision.h	Switch FK cache type and active link storage to id-based types.
motion_planners/core/src/utils.cpp	Switch toolpath pose computation + contact-check program plumbing to joint/link IDs.
motion_planners/core/include/tesseract/motion_planners/robot_config.h	Use LinkIdTransformMap and LinkId lookups for FK-derived poses.
examples/src/scene_graph_example.cpp	Update joint parent/child fields to use LinkId.
examples/src/puzzle_piece_example.cpp	Update ManipulatorInfo frames to LinkId.
examples/src/puzzle_piece_auxillary_axes_example.cpp	Update ManipulatorInfo frames to LinkId.
examples/src/pick_and_place_example.cpp	Use LinkId for joint parent/child and for ManipulatorInfo frames.
examples/src/online_planning_example.cpp	Use LinkId FK lookup for tool pose; set environment state via joint-name accessor.
examples/src/glass_upright_ompl_example.cpp	Update FK usage to id-based map (currently extracts pose incorrectly).
examples/src/glass_upright_example.cpp	Use LinkId for joint parent/child and ManipulatorInfo frames.
examples/src/freespace_ompl_example.cpp	Use LinkId for joint parent/child and ManipulatorInfo frames.
examples/src/freespace_hybrid_example.cpp	Use LinkId for joint parent/child and ManipulatorInfo frames.
examples/src/car_seat_example.cpp	Use LinkId for joint parent/child, transforms, and ManipulatorInfo frames.
examples/src/basic_cartesian_example.cpp	Use LinkId for joint parent/child and ManipulatorInfo frames.
command_language/test/type_erasure_benchmark.cpp	Construct ManipulatorInfo using LinkId.
command_language/test/command_language_utils_unit.cpp	Update tests to use LinkId and build seed joint IDs.
command_language/test/command_language_unit.cpp	Construct ManipulatorInfo using LinkId.
command_language/test/command_language_test_program.hpp	Build seed joint IDs in test program generation.
command_language/src/utils.cpp	Add joint-ID utilities and migrate waypoint joint extraction/formatting.
command_language/src/state_waypoint.cpp	Store joint IDs internally; derive names on demand; add joint-id accessors.
command_language/src/poly/state_waypoint_poly.cpp	Expose joint-id accessors; return names by value.
command_language/src/poly/joint_waypoint_poly.cpp	Expose joint-id accessors; return names by value.
command_language/src/poly/cartesian_waypoint_poly.cpp	Treat cartesian seed validity as joint-id based.
command_language/src/joint_waypoint.cpp	Store joint IDs internally; derive names on demand; add joint-id accessors.
command_language/include/tesseract/command_language/utils.h	Change joint-name extraction to return by value; add joint-id APIs.
command_language/include/tesseract/command_language/test_suite/state_waypoint_poly_unit.hpp	Update tests to use setters instead of mutating name refs.
command_language/include/tesseract/command_language/test_suite/move_instruction_poly_unit.hpp	Construct ManipulatorInfo using LinkId.
command_language/include/tesseract/command_language/test_suite/joint_waypoint_poly_unit.hpp	Update tests to use setters instead of mutating name refs.
command_language/include/tesseract/command_language/test_suite/cartesian_waypoint_poly_unit.hpp	Build cartesian seed state using joint IDs.
command_language/include/tesseract/command_language/state_waypoint.h	Add joint-id storage/accessors + cereal load/save friend declarations.
command_language/include/tesseract/command_language/poly/state_waypoint_poly.h	Extend interface to include joint IDs; return names by value.
command_language/include/tesseract/command_language/poly/joint_waypoint_poly.h	Extend interface to include joint IDs; return names by value.
command_language/include/tesseract/command_language/joint_waypoint.h	Add joint-id storage/accessors + cereal load/save friend declarations.
command_language/include/tesseract/command_language/cereal_serialization.h	Switch Joint/State waypoint serialization to non-member load/save for id-backed storage.
command_language/POLY_TYPE_DESIGN.md	New documentation describing the poly/type-erasure architecture.
CLAUDE.md	New repository guidance file (architecture + patterns overview).

Comments suppressed due to low confidence (1)

examples/src/glass_upright_ompl_example.cpp:199

• Same issue here: transforms.begin()->second is not guaranteed to be the TCP (or any specific) link transform, so the constraint may compute an angle for the wrong frame. Select the intended link explicitly from the FK results (e.g., by keying on the tool link id/name) instead of taking the first element.

    auto transforms = fwd_kin_->calcFwdKin(x);
    const Eigen::Isometry3d& pose = transforms.begin()->second;

    Eigen::Vector3d z_axis = pose.matrix().col(2).template head<3>().normalized();

    out[0] = std::atan2(z_axis.cross(normal_).norm(), z_axis.dot(normal_));

---

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

[Copilot]

Pull request overview

Copilot reviewed 87 out of 87 changed files in this pull request and generated 10 comments.

---

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

[Copilot]

Pull request overview

Copilot reviewed 83 out of 83 changed files in this pull request and generated 5 comments.

Comments suppressed due to low confidence (2)

examples/src/glass_upright_ompl_example.cpp:205

• This collision constraint constructs a LinkId from tcp_link_ on every evaluation. Since LinkId creation likely hashes the string, consider storing a tesseract::common::LinkId member (initialized once in the ctor) and using it directly in transforms.at(...) to reduce per-call overhead in OMPL constraints.

  GlassUprightConstraint(const Eigen::Vector3d& normal,
                         tesseract::kinematics::ForwardKinematics::Ptr fwd_kin,
                         std::string tcp_link)
    : ompl::base::Constraint(fwd_kin->numJoints(), 1)
    , normal_(normal.normalized())
    , fwd_kin_(std::move(fwd_kin))
    , tcp_link_(std::move(tcp_link))
  {
  }

  ~GlassUprightConstraint() override = default;

  void function(const Eigen::Ref<const Eigen::VectorXd>& x, Eigen::Ref<Eigen::VectorXd> out) const override
  {
    auto transforms = fwd_kin_->calcFwdKin(x);
    const Eigen::Isometry3d& pose = transforms.at(tesseract::common::LinkId(tcp_link_));

    Eigen::Vector3d z_axis = pose.matrix().col(2).template head<3>().normalized();

    out[0] = std::atan2(z_axis.cross(normal_).norm(), z_axis.dot(normal_));
  }

motion_planners/core/include/tesseract/motion_planners/robot_config.h:35

• This header now uses tesseract::common::LinkId and tesseract::common::LinkIdTransformMap, but it doesn't include the header that defines them. Please add a direct include (e.g., <tesseract/common/types.h>) so robot_config.h is self-contained and doesn't rely on transitive includes.

---

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

[Copilot]

Pull request overview

Copilot reviewed 83 out of 83 changed files in this pull request and generated 7 comments.

Comments suppressed due to low confidence (4)

task_composer/planning/src/nodes/fix_state_collision_task.cpp:1

• state is thread_local and is no longer cleared between calls. If calcFwdKin doesn’t overwrite all entries every time (or if the active links set changes), stale transforms can persist and be consumed by collision checking, potentially producing incorrect results without throwing. Clear the map before calcFwdKin(...) (or switch to an API that returns a fresh transform map per call) to avoid reusing stale data.
  task_composer/planning/src/nodes/fix_state_collision_task.cpp:1
• calcFwdKin(start_pos) returning a transform map by value may allocate and re-hash on each invocation. Since this codepath can run in iterative collision-fixing loops, consider reusing an output LinkIdTransformMap (e.g., thread-local or stack-allocated and cleared) and calling the calcFwdKin(out, ...) overload if available to reduce per-iteration allocations.
  motion_planners/simple/test/simple_planner_lvs_move_unit.cpp:1
• This manual loop for converting joint names to JointIds is repeated across multiple simple planner tests in this PR. Consider using a shared helper (e.g., tesseract::common::toIds<tesseract::common::JointId>(joint_names_)) to reduce duplication and keep the tests consistent.
  motion_planners/core/src/utils.cpp:1
• StateWaypointPoly::getNames() now returns by value, so this assert creates an unnecessary allocation/copy just to get a size. Prefer checking swp.getJointIds().size() (or the stored position size invariants) to avoid extra work in a hot utility function.

---

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

[Copilot]

Pull request overview

Copilot reviewed 74 out of 74 changed files in this pull request and generated 1 comment.

---

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