SRDF end effector configuration for multiple robots and subgroups

edit

Hello folks,

im having a problem getting the srdf configuration just right.

What im trying to do is having 3 separate robots in one instance of moveit and to plan for them simultaneosly. What i dont understand is, how the end_effector should be defined when multiple subgroups are available. Also if one frame can be defined as end effector.

I attached a simplified URDF and SRDF configuration below. I can plan and move the standalone groups just fine, also the "full" groups do work as intended. The compound groups "all_robots*" do not work correctly.

Either i get "IKConstraintSampler received dirty robot state, but valid transforms are required. Failing." while moveit tries to calculate an IK, or "No goal constraints specified. There is no problem to solve."

urdf example:

<robot name="robots">
<link name="base_link" />

<link name="robot1_base_link" />
<link name="robot1_link_1" />
<link name="robot1_link_2" />
<link name="robot1_link_3" />
<link name="robot1_link_4" />
<link name="robot1_link_5" />
<link name="robot1_link_6" />
<link name="robot1_flange" />

<joint name="robot1_joint1" type="prismatic">
    <parent link="robot1_base_link" />
    <child link="robot1_link_1" />
<joint />
... and so on until flange ...

# tool of robot 1
<link name="tool_robot1_base_link" />
<link name="tool_robot1_center_link" />
<link name="tool_robot1_tcp_link" />

# true tcp frame of robots tool
<joint name="tool_robot1_base_link-tool_robot1_tcp_link" type="fixed">
    <parent link="tool_robot1_base_link" />
    <child link="tool_robot1_tcp_link" />
<joint />

<joint name="tool_robot1_base_link-tool_robot1_center_link" type="fixed">
    <parent link="tool_robot1_base_link" />
    <child link="tool_robot1_center_link" />
<joint />

# connection between robots flange and tools base
<joint name="robot1_flange-tool_robot1_base_link" type="fixed">
    <parent link="robot1_flange" />
    <child link="tool_robot1_base_link" />
<joint />


<link name="robot2_base_link" />
<link name="robot2_link_1" />
.... the same as robot1 ....
<link name="robot2_link_6" />
<link name="robot2_flange" />

... joint configuration the same as robot1 ...
... tool configuration the same as robot1 ...

<link name="robot3_base_link" />
<link name="robot3_link_1" />
.... the same as robot1 ....
<link name="robot3_link_6" />
<link name="robot3_flange" />

... joint configuration the same as robot1 ...

</robot>

<?xml version="1.0" encoding="UTF-8"?>

srdf example:

<robot name="robots">
# robot 1 configuration from link 1 to tool tcp
<group name="robot1_standalone">
    <chain base_link="robot1_link_1" tip_link="tool_robot1_tcp_link"/>
</group>

# adding a joint to the standalone group
<group name="robot1_full">
    <joint name="robot1_joint1" />
    <group name="robot1_standalone" />
</group>

... the same as above for robot 2 and 3 ...

<group name="all_robots_full">
    <group name="robot1_full" />
    <group name="robot2_full" />
    <group name="robot3_full" />
</group>

<group name="all_robots_standalone">
    <group name="robot1_standalone" />
    <group name="robot2_standalone" />
    <group name="robot3_standalone" />
</group>

# defining tcps for smallest group
<end_effector name="tool_robot1_tcp_link" parent_link="robot1_link_1" group="robot1_standalone"/>
<end_effector name="tool_robot2_tcp_link" parent_link="robot2_link_1" group="robot2_standalone"/>
... the same for robot 3

</robot>