make imarker work in multi_arm_sim, for just the first arm

[marioprats]

This PR should make IMarker teleop work at least with one arm.
Multi-arm IMarker is still not supported.

[dyackzan]

I cherry-picked your changes on the v8.0 branch and it looks like the "Move to Pose" objective that ends up getting pulled in is the one from moveit_pro_objectives so it's attempting to plan for the grasp_link which doesn't exist in this config and it always fails. I'll look into how the objectives are being loaded to see if there is a way we can guarantee the local "Move to Pose" always overwrites the one from core

[marioprats]

Hmm... try to remove your ~/.config/moveit_pro/ directory? I had to do this for it to realize it was a new local version, instead of getting the one from core

[dyackzan]

Hmm... try to remove your ~/.config/moveit_pro/ directory? I had to do this for it to realize it was a new local version, instead of getting the one from core

Oh thanks, that worked for me. And I don't think it should be a concern since that gets removed and re-created every time in the user workflow

[dyackzan]

@marioprats could you change this to target v8.0?

[dyackzan]

Testing in the user workflow and getting an error:

agent_bridge-1  | [move_group-1] Error:   Failed to find root link: Two root links found: [first_robotiq_85_base_link] and [first_robotiq_85_left_finger_link]
agent_bridge-1  | [move_group-1]          at line 264 in ./urdf_parser/src/model.cpp
agent_bridge-1  | [move_group-1] [ERROR] [1747246269.954832724] [move_group.moveit.ros.move_group.get_urdf_service]: Failed to create valid urdf
agent_bridge-1  | [parameter_manager_node-2] [2025-05-14 18:11:10.027] [parameter_manager_node] [info] Retrieving pose from parameter manager.

It looks like the wrong "Move to Pose" may be loaded even in the user workflow...

[dyackzan]

Rebuilding after pruning my docker images and removing my build & install artifacts got it to pull in the correct updated "Move to Pose" objective, but I'm still running into the UI error "Failed to load gripper model Error: Multiple tip frames found for planning group "manipulator": first_grasp_link, fourth_grasp_link, second_grasp_link, third_grasp_link"

I'm seeing this in both the dev workflow and in the user workflow. I'm not sure what I was running before when I said I had it working on the v8.0 branch...

Maybe this isn't quite ready for v8.0 and will need some more infrastructure work, sorry for requesting the retarget

[dyackzan]

Based on the above comments I don't think this solution will work. I'm not sure exactly what is going on but I think the UI is having trouble visualizing the correct gripper because it has multiple parent planning groups (first_manipulator and manipulator) and it lands on the manipulator parent group that breaks things since it has multiple chains.

[dyackzan]

I believe this PR @EzraBrooks put in to help safeguard another issue is the reason we're not able to iMarker here anymore: https://github.com/PickNikRobotics/moveit_pro/pull/12248/files

[marioprats]

Looks like that UI check for multiple tips is not taking into account the planning group that was selected?
I.e. even if you choose 'first_manipulator' in the planning groups drop down, it still checks 'manipulator' instead of 'first_manipulator'. Wondering where that comes from, maybe from a servo parameter? @EzraBrooks

Edit: it looks like the UI is getting the planning group from Servo, here. That would explain why it's always 'manipulator', because that's what the config has.
Changing it to first_manipulator in the Servo config, makes the IMarker work again for the first arm, but then we can only Joint Jog the first arm. Looks like we have to choose between Joint Jog all joints and have IMarker broken, or Joint Jog only the first arm and IMaker only the first arm.

@EzraBrooks @noah-wardlow I think we need to update IMarker to take the planning group from whatever is selected in the drop-down, instead of from Servo. IMarker control is actually independent of Servo, it doesn't need it, so I think it makes sense to make this change. Then we could at least have 1 arm working in IMarker mode, and maybe all of them? since the frontend now communicates the selected planning group to the backend via a port, so we can use this in the Teleoperate Objective to plan for the right group