[moveit_servo] Twist-mode differential IK path unreachable when no kinematics plugin configured

[NickNair]

Description

Checked against main @ 2fb8492

jointDeltaFromIK (command.cpp:301) has two branches: if joint_model_group->getSolverInstance() is set, solve via searchPositionIK; else compute delta_theta = J⁺ * cartesian_position_delta directly via robot_state->getJacobian() (command.cpp:359-366).

That direct-Jacobian branch never actually runs for TWIST or POSE commands. Servo::jointDeltaFromCommand resolves the planning frame before calling jointDeltaFromTwist/jointDeltaFromPose, via getIKSolverBaseFrame/getIKSolverTipFrame (common.cpp:57-84):

const auto ik_solver = robot_state->getJointModelGroup(group_name)->getSolverInstance();
if (ik_solver) return ik_solver->getBaseFrame();
else return std::nullopt;   // no fallback

No IK solver causes Servo::jointDeltaFromCommand to set StatusCode::INVALID and logs "No IK solver for planning group %s." (servo.cpp:403-423 for TWIST, :431-452 for POSE) jointDeltaFromTwist is never even called. So the J⁺ branch inside jointDeltaFromIK is unreachable for TWIST/POSE as currently staged only JOINT_JOG can reach jointDeltaFromIK without a solver, since joint-jog commands don't need a planning frame at all.

Confirmed empirically: removed robot_description_kinematics from a servo node's parameters to try to exercise the fallback, got "No IK solver for planning group arm." logged every cycle instead command rejected outright rather than falling back.

Proposed fix: getIKSolverBaseFrame/getIKSolverTipFrame fall back to the joint model group's own root/tip link name when getSolverInstance() is null, instead of unconditionally returning nullopt. Happy to send a PR if that's the right direction or let me know if there's a reason this fallback was intentionally left out.

ROS Distro

Jazzy

OS and version

Ubuntu 24.04 (Noble)

Source or binary build?

Binary

If binary, which release version?

No response

If source, which branch?

No response

Which RMW are you using?

None

Steps to Reproduce

1. Configure a MoveIt config where the planning group used by Servo (move_group_name in servo.yaml) has NO kinematics_solver set in kinematics.yaml (or omit robot_description_kinematics from the parameters passed to the servo-consuming node).
2. Launch a node that instantiates moveit_servo::Servo directly (or servo_node), set command type to TWIST.
3. Publish a nonzero TwistStamped (or call Servo::getNextJointState with a TwistCommand) at the configured command frame.

Expected behavior

The twist command should be processed via jointDeltaFromIK's direct-Jacobian fallback (command.cpp:359-366, delta_theta = J⁺ * cartesian_position_delta). This branch exists specifically to handle the no-IK-solver case.

Actual behavior

The command is rejected outright before that branch is ever reached. Servo::jointDeltaFromCommand calls getIKSolverBaseFrame() first to resolve the planning frame; that function unconditionally returns nullopt when getSolverInstance() is null (no fallback to the joint group's own root link), so StatusCode::INVALID is set and jointDeltaFromTwist/jointDeltaFromIK are never called at all.

Backtrace or Console output

[ERROR] [servo]: No IK solver for planning group arm.
[WARN] [servo_node]: Servo status: Invalid, holding position

(repeats every control cycle doesn't crash, just permanent command rejection)