fix(compute_ik): seed IK solver with current robot state

[michaljohnson]

Summary

Seed the IK solver with the robot's current joint configuration instead of a fresh zero state, so the solver converges to the IK branch nearest the actual robot pose rather than a ±2π wrap-around equivalent.

Problem

compute_ik previously constructed a fresh RobotState(robot_model) whose joints default to the URDF zero values. KDL / numerical IK solvers converge to whichever solution branch is closest to the seed configuration. For a 6-DOF revolute arm, every Cartesian target has multiple IK solutions plus their ±2π wrap-around equivalents on the wrist and shoulder joints.

From a zero seed, the solver had no preference for the natural branch and would frequently land on wrap-around solutions (for example, shoulder_lift = -4.44 rad instead of the near-current -1.5 rad). Downstream callers using compute_ik + plan_to_joint_state then had to traverse a ~2π joint sweep from the actual current state to the wrap-around target, which usually passes through self-collision configurations and surfaces as a generic "Planning failed".

Fix

Read the group's current joint positions from planning_scene_monitor.read_only() and use them to seed the RobotState passed to set_from_ik. Only the group's joints matter for IK seeding because the arm's kinematic chain is the only thing the IK solver evaluates. The monitor is used in read-only context so its view is not mutated.

Notes

attempts is kept on the signature for API stability even though it is not currently threaded into set_from_ik. Happy to drop it if you'd prefer a cleaner signature.