Planning for end-effector pose with a torso

edit

Is there a way to plan given an end-effector goal pose with a group that is a tree (not a chain)?

Based on the info here https://github.com/ros-planning/movei... the answer is “no” but I find that hard to believe. Is there a workaround? When I try to do this, the rviz plugin lets me drag around the end effector (lets say the right gripper) and plan just fine, but it does not make use of the torso joints (even though they are in the group). In python, when I try to set end-effector it throws an error.

The robot is a Y, it has two torso joints, connected to two 7-dof chains. A relevant snippet of the SRDF:

   <group name="left_arm">
        <!-- drive 50 is the end of the left arm -->
        <chain base_link="torso" tip_link="drive50" />
    </group>
    <group name="right_arm">
        <!-- drive 10 is the end of the right arm -->
        <chain base_link="torso" tip_link="drive10" />
    </group>
    <group name="left_gripper">
        <joint name="leftgripper" />
        <joint name="leftgripper2" />
    </group>
    <group name="right_gripper">
        <joint name="rightgripper" />
        <joint name="rightgripper2" />
    </group>
    <group name="torso">
        <chain base_link="drive56" tip_link="torso" />
    </group>
    <group name="both_arms">
        <group name="torso"/>
        <group name="left_arm"/>
        <group name="right_arm"/>
    </group>
    <group name="whole_body">
        <group name="torso"/>
        <group name="left_arm"/>
        <group name="right_arm"/>
        <group name="left_gripper"/>
        <group name="right_gripper"/>
    </group>

    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
    <end_effector name="left_gripper" parent_link="drive50" group="left_gripper" parent_group="left_arm" />
    <end_effector name="right_gripper" parent_link="drive10" group="right_gripper" parent_group="right_arm" />

When I try to run

    move_group = moveit_commander.MoveGroupCommander("both_arms")
    move_group.set_end_effector_link("rightgripper_link")

I get

Traceback (most recent call last):
  File "/home/peter/catkin_ws/src/arm_robots/arm_robots/scripts/test_val_pose_error.py", line 25, in <module>
    main()
  File "/home/peter/catkin_ws/src/arm_robots/arm_robots/scripts/test_val_pose_error.py", line 18, in main
    val.plan_to_pose('both_arms', 'rightgripper_link', [0.3, 0.8, 0.5, -np.pi / 2, np.pi / 2, 0])
  File "/home/peter/catkin_ws/src/arm_robots/arm_robots/src/arm_robots/robot.py", line 143, in plan_to_pose
    move_group.set_end_effector_link(ee_link_name)
  File "/opt/ros/noetic/lib/python3/dist-packages/moveit_commander/move_group.py", line 86, in set_end_effector_link
    raise MoveItCommanderException("Unable to set end effector link")
moveit_commander.exception.MoveItCommanderException: Unable to set end effector link

If instead I use the right_arm group, it works fine.