Dual arm planning error with IKConstraintSampler

[bi3ri]

Description

I want to plan and move two UR robots (UR5e and UR16e) at the same time. I followed the example provided by @felixvd.

I am able to set two Poses through the RVIZ GUI and plan and execute a motion with both robots simultaneously (see gif above), but if I use the Moveit cpp API (like this) I get the following error:

[...]
[ERROR] [1646681894.535879134, 851.070000000]: IKConstraintSampler received dirty robot state, but valid transforms are required. Failing.
[ERROR] [1646681894.535901334, 851.070000000]: IKConstraintSampler received dirty robot state, but valid transforms are required. Failing.
[ERROR] [1646681894.535920515, 851.070000000]: IKConstraintSampler received dirty robot state, but valid transforms are required. Failing.
[Thread 0x7fffb97fa700 (LWP 62796) exited]
[ERROR] [1646681894.538607503, 851.073000000]: rocket_and_groot/rocket_and_groot: Unable to sample any valid states for goal tree
[ INFO] [1646681894.538643433, 851.073000000]: rocket_and_groot/rocket_and_groot: Created 1 states (1 start + 0 goal)
[ INFO] [1646681894.538657673, 851.073000000]: No solution found after 0.080671 seconds
[ INFO] [1646681894.538684344, 851.073000000]: Unable to solve the planning problem
[ WARN] [1646681894.538734974, 851.073000000]: More than one constraint is set. If your move_group does not have multiple end effectors/arms, this is unusual. Are you using a move_group_interface and forgetting to call clearPoseTargets() or equivalent?

I can not find any difference between mine and Felix's example, so I guess something changed on the MoveIt side. Felix's example used melodic/kinetic and I am using noetic. Could this explain the error?

Your environment

• ROS Distro: noetic
• OS Version: Pop OS 20.04
• Binary 1.1.8-1focal.20220226.080138

Steps to reproduce

I created a small demo project you can find here. (Note: project was changed and is working now. See first commit for initial version.)

mkdir -p ~/dual_arm_ws/src
cd ~/dual_arm_ws/src
git clone https://github.com/bi3ri/dual_arm_demo.git
git submodule update --init --recursive
catkin build && source ~/dual_arm_ws/devel/setup.bash

#run in two shells
roslaunch dual_arm_demo app.launch 
rosrun dual_arm_demo moveit_demo

Thanks for your help! Johannes

[welcome[bot]]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[simonschmeisser]

This is a well done bug report!

A RobotState is dirty when the joint positions have changed but the forward kinematics have not been updated yet. It can be done by calling RobotState::update(). I'd recommend searching the code for the error message and try to see if you get an idea what it tries to tell you. Maybe try adding update in some places and see if it helps? If you can make it work please open a PR, it does not need to shine at all and we can try to figure out the root cause

Also what happens if your goal is clearly invalid? (ie 10m away or so) Do you get the same error message?

[bi3ri]

Guten Morgen Simon! Dankeschoen :)

Thanks for the hint. I ll try my best to find the bug!

With a clearly invalid goal, the error message stays the same. Does this give you a better idea of where to search for the problem?

---

Edit: Okay so far I found out that if I am using the GUI no constraint sampler is called, while if I am using the MoveIT CPP Api the default constraint sampler is called.

Do I understand right that the constraint sampler is trying to find a joint configuration of the robot, which satisfies the pose I am giving it? If so, that would totally make sense: Using the GUI MoveIt is sampling the joint configuration separately for each robot and then probably setting a joint goal for each robot.

[Lucas-ohmygoal]

Hi. I met exactly the same problem. One odd thing is that I can move both arms of my robot without any errors, but not for legs or other joints. Could anyone offer a solution? It would be warmly appreciated :)

[bi3ri]

Okay, I could solve the problem for me.

As the planning group consists of two kinematic chains (the two robots) you can not plan pose targets directly. You first need to use IK to generate fitting joint configurations for the desired poses of the two robots and then plan for joint targets.

bool rocket_found_ik = kinematic_state->setFromIK(rocket_joint_model_group, rocket_pose, timeout);
bool groot_found_ik = kinematic_state->setFromIK(groot_joint_model_group, groot_pose, timeout);

kinematic_state->copyJointGroupPositions(rocket_joint_model_group, rocket_joint_values);
kinematic_state->copyJointGroupPositions(groot_joint_model_group, 

// planning group with rocket and groot
rng_group_interface.setJointValueTarget(rocket_joint_names, 
rng_group_interface.setJointValueTarget(groot_joint_names, 

bool success = (rng_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

I updated my demo repo if someone else is struggling. You can find it here.

---

For MoveIt I am not sure if it makes sense to fix setting pose targets to planning groups with multiple kinematic chains. I guess you would just need to check if there are one or multiple kinematic chains and then call the IKConstraintSampler for each separately.

What are your thoughts?

[felixvd]

Sorry for the late response. I agree that this is a great bug report, thanks. Your solution works fine, but the behavior is surprising. You should not need to do this workaround of solving the IK "manually" and setting the joint targets.

As the planning group consists of two kinematic chains (the two robots) you can not plan pose targets directly.

I don't think this is true. It's not in the examples you linked, but we do it in this part of the code. But your code here does look equivalent, so I'm not sure why it wouldn't work.

I don't have an environment to test in and can't spend time on this, but I would A) confirm that the o2ac code works in Noetic (you can run roslauch o2ac_moveit_config demo.launch and roslaunch o2ac_routines assembly.py and test placing the tray, which uses these dual_arm motions) and B) investigate if your planning requests differ if you use Pose targets vs Joint targets for the same poses. A few strategic printouts in move_group_interface and the relevant parts of moveit_core should be insightful.

[bi3ri]

Thanks for your reply @felixvd!

Unfortunately, I could not get the o2ac workspace running. My demo project builds in 18 seconds.

I added robotiq grippers to the demo project and added a python file following your example. There is no difference to your example I could find and still, I receive the same error msgs.

[cambel]

@felixvd I am getting the same error reported here since I updated our repository to Noetic. I am also having issues getting TRAC-IK to return the shortest solution instead of the first solution. It works fine in Melodic but not in Noetic. I don't have a lot of time right now, but I'll check it as soon as I can. But at this moment, I don't really know what is different between the MoveIt's Melodic and Noetic versions.

[bi3ri]

@cambel For me TRAC-IK is also not finding the shortest solution

[cambel]

Okay, I forked @bi3ri's dual demo and added some docker magic to make it easier to reproduce. Then I set melodic and noetic versions (different branches).

As a result, the expected behavior without the workaround works fine in Melodic. The error only starts occurring after updating to Noetic.

[felixvd]

I am getting the same error reported here since I updated our repository to Noetic. [...] I don't really know what is different between the MoveIt's Melodic and Noetic versions.

In the O2AC project we were compiling MoveIt from source (master/noetic-devel) with our changes applied , so this is not about melodic-devel vs noetic-devel (although the problem may exist there too). But we branched off of master at commit 95ab9eef in June 2021, so something must have happened between then and now.

If you can write a test that isolates the problem, it will be easier to find the offending commit.

[cambel]

@felixvd right, I rebased our fork of MoveIt to the commit f2cc2348 on March 9, 2022. So, yes, the bug might have been introduced sometime in between.

[cambel]

I have been trying to debug this problem. From what I understand so far, there are two attempts at constructing a constrained state sampler. The first correctly has the group's constraints information while the second attempt does not (everything is empty). Then, the first attempt seems to succeed and creates a sampler for group 'ab_bot' as a union of 2 samplers (a_bot and b_bot). The second attempt just seems to not find the subgroups and results in No constraints sampler allocated for group 'ab_bot'. And I assume that may be a problem, not sure though.

You can find the log with debug info that I have created here.

I still don't fully understand the behavior of MoveIt for this multi-arm use case. Either the second attempt at creating the sampler is not needed or the necessary constraints information is not being passed correctly.

I would appreciate any ideas about this issue.

I will spend some more time next week checking it. I will try to compare the behavior in the melodic version that actually works and see if I can find the difference between the two.