Add plan_to_tcp_pose_and_joint_configuration() to DualArmPlanner interface

[Victorlouisdg]

Currently the dual arm planner interface provides:

• (🦾,🦾) -> (🦾,🦾) Joints to joints
• (🦾,🦾) -> (🦾,💤) Joints to single arm joints
• (🦾,🦾) -> (💤,📐) Joints to single TCP pose
• (🦾,🦾) -> (📐,📐) Joints to dual TCP poses

However, for the cloth competition I have a use case where I want to move one arm to it's home joints, and the other arm to a TCP pose. So I would need:

• (🦾,🦾) -> (🦾,📐) Joints to joints and TCP pose
• (🦾,🦾) -> (📐,(🦾) Joitns to TCP pose and joints

I believe this is a general and valid use case, so I'm planning on adding:

• plan_to_tcp_pose_and_joint_configuration()
• plan_to_joint_configuration_and_tcp_pose()

The implementation should also be straightforward, as the MultipleGoalPlanner already has logic to handle this.

[adverley]

Do you consider a single point of entry which is dual_planner.plan(left_init, right_init, left_target, right_target) in which you infer on the arguments what the user wants to do? e.g. in dual_planner.plan(q_left_init, q_right_init, None, X_B_TcpRight) I am already implicit about 'plan_to_nowhere_and_tcp_pose()'

[Victorlouisdg]

If you would like a single .plan() method, feel free to add that to the interfaces. It would have to use array shape of the goal(s) to infer the required planning mode. I agree this is in line with how we currently use None to signal that an arm should not move at all.

However I'm wondering whether we should go in the opposite direction. I like that the current method names are explicit. Planning to joint goals and planning to TCP goals are conceptually different things, so I believe it's good to show that in the method names, rather than relying on the variables names (e.g. a reader of your code might not know your variable naming convention).

Similarly, I have had instances where I accidentally swapped goals (None, goal) with (goal, None). Additionally readers of your code could be confused by a variable e.g. "grasp_pose" that implicitly contains None. Even worse, e.g. if grasp selection fails and returns None, you might still accidentally plan and move one of the arm. So maybe we should add methods like .plan_to_tcp_pose_left() to the dual arm interface to be even more explicit, readable and less bug prone?