Orientation constraint wrt attached collision object ignores the reference frame world

[DaniGarciaLopez]

I was experiencing problems with our robot setup using orientation constrained when an attached collision object is used in the link_name field of a constraint. The main goal is to plan a pick/place task where the object remains upright during the place connect stage to prevent spilling liquid. To illustrate the issue, I created a simple repro using the pick_place_demo.

How to reproduce the issue

Modify the connect stage in pick_place_task.cpp to include the orientation constraint:

moveit_msgs::msg::Constraints upright_constraint;
upright_constraint.name = "hand:upright";
upright_constraint.orientation_constraints.resize(1);
{
  moveit_msgs::msg::OrientationConstraint & c = upright_constraint.orientation_constraints[0];
  c.header.frame_id = params.world_frame; // Reference frame ("world")
  c.link_name = params.object_name; // Constraining tool frame ("object")
  
  c.absolute_x_axis_tolerance = 0.1; // +/- 0.1 radian tolerance around X axis
  c.absolute_y_axis_tolerance = 0.1; // +/- 0.1 radian tolerance around Y axis
  c.absolute_z_axis_tolerance = M_PI; // Complete freedom around Z axis
  c.weight = 1.0;
}

{
  // Connect the grasped state to the pre-place state, i.e. realize the object transport
  auto stage = std::make_unique<stages::Connect>("move to place", stages::Connect::GroupPlannerVector{ { params.arm_group_name, sampling_planner } });
  stage->setTimeout(20.0);
  stage->setPathConstraints(upright_constraint);
  stage->properties().configureInitFrom(Stage::PARENT);
  t.add(std::move(stage));
}

Change the place pose of the object in panda_config.yaml so its more challenging to find a constrained path:

# Place pose [x,y,z,r,p,y]
place_pose: [-0.2, -0.6, 0.0, 0.0, 0.0, 0.0]
place_surface_offset: 0.0001 # place offset from table

Use enforce_constrained_state_space and projection_evaluator in demo.launch.py:

moveit_config.planning_pipelines["ompl"]["panda_arm"]["enforce_constrained_state_space"] = True
moveit_config.planning_pipelines["ompl"]["panda_arm"]["projection_evaluator"] = "joints(panda_joint1,panda_joint2)"
moveit_config.planning_pipelines["ompl"]["panda_arm_hand"]["enforce_constrained_state_space"] = True
moveit_config.planning_pipelines["ompl"]["panda_arm_hand"]["projection_evaluator"] = "joints(panda_joint1,panda_joint2)"

Run in one terminal:

ros2 launch moveit_task_constructor_demo demo.launch.py

And in another:

ros2 launch moveit_task_constructor_demo run.launch.py exe:=pick_place_demo

Results

These are the 10 successful trajectories we obtained after 1,910 failed connect stages:

rviz.mp4

As observed, the object doesn't maintain an upright orientation, even though absolute_x_axis_tolerance and absolute_y_axis_tolerance are set to ±0.1 radians and absolute_z_axis_tolerance to ±PI radians. However, there is a pattern in all the generated trajectories where the object spins around the arm wrist (i.e. panda_link8 Z-axis). This could suggest that perhaps the constraint is defined wrt panda_link8 rather than world coordinates.

I don’t believe this issue is related to the MTC code. However, since this is the de facto pick/place pipeline for MoveIt, I thought it was appropriate to post here. I can open an issue in moveit2 if necessary. I'm using jazzy with the latest commits of moveit2 and MTC.

The ResolveConstraintFrames adapter is correctly loaded, but the issue might be somewhere here when transforming the pose of the attached collision object to a robot link.

Is this a bug, or am I doing something wrong? Thanks in advance!

[rhaschke]

I can confirm this issue for ROS1. I'm afraid that the constraint frame resolver is fundamentally broken: while the resolver adapts the target orientation to be interpreted for the robot link instead of the attached object (or subframe), the tolerance vector is not (and cannot be) adapted. This results in the tolerance values being interpreted w.r.t. the wrong (new) axes. This is a fundamental issue of the current constraint representation!

[DaniGarciaLopez]

Thanks for the confirmation! That's unfortunate, I thought it would have an easier fix. Why not drop support for attached collision objects when used in orientation constraints? I don't really foresee any use case if the tolerance values are not converted to the new frame. At least, printing a warning message would help users quickly identify the problem.

It seems the only current workaround is to define all constraints using robot links, though this limits the flexibility in MTC because it implies that you can only grasp the object in a certain way. Furthermore, using the panda arm, I had difficulties doing this since panda_link8 is rotated 45º relative to panda_hand. There's already a previous issue about this that wasn’t fully resolved.

I don’t fully understand if the orientation field of the constraint message represents the rotation from the reference frame (header.frame_id) to the constrained frame (link_name) or the other way around, I tried different combinations and none worked. Additionally, if tolerances are relative to the constrained frame, how can you allow freedom (±π radians) around the world Z-axis when there is no axis of panda_link8 pointing in that direction when you pick the object?

The only fix I've found for this, as explained in the issue, is to define a new robot link that matches the orientation of the world axis, so you can leave the orientation field blank. However, I bet there's a cleaner solution that doesn't involve modifying the robot description.

[rhaschke]

I need to correct myself: It's true that the tolerances cannot be adapted if represented as Euler angles. But they can if using the rotation vector representation!
@DaniGarciaLopez: Could you please review moveit/moveit#3656?

[rhaschke]

You also might want to use moveit/moveit#3655

[DaniGarciaLopez]

It's definitely better now, but I still get some trajectories that don't respect the constraint:

rviz2.mp4

This is how the constraint looks like now:

moveit_msgs::msg::OrientationConstraint & c = upright_constraint.orientation_constraints[0];
c.header.frame_id = params.world_frame; // Reference frame ("world")
c.link_name = params.object_name; // Constraining tool frame ("object")

c.parameterization = moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR;

c.absolute_x_axis_tolerance = 0.1; // +/- 0.1 radian tolerance around X axis
c.absolute_y_axis_tolerance = 0.1; // +/- 0.1 radian tolerance around Y axis
c.absolute_z_axis_tolerance = M_PI; // Complete freedom around Z axis
c.weight = 1.0;

I applied moveit/moveit#3655 and moveit/moveit#3656. However, I'm using ROS2, maybe there is a missing patch for moveit2 to make this work?

[DaniGarciaLopez]

I'm also getting the error:

[planning_scene_interface_100976016393168.moveit.moveit.planners.ompl.planner_manager]: ./src/ompl/base/src/StateSpace.cpp:729 - No default projection is set. Perhaps setup() needs to be called

According to moveit/moveit2#2143, this might be related to the projection_evaluatorparameter. I double-checked, and it's loaded correctly, so maybe there is an error somewhere there?

[DaniGarciaLopez]

As expected, I was not loading the projection_evaluator in the run.launch.py, so the MTC executable was missing the parameter. Now, the previous error is gone. However, I am still getting the same results: some trajectories do not obey the orientation constraint.

Additionally, the planning time is very long. It took 7 minutes to generate 10 successful trajectories (22 failed), whereas without the constraint, it only takes 3 seconds! Something must be wrong here. Some trajectories failed due to TIMED_OUT, even though I increased the timeout for the Connect stage to 20 seconds.

I believe this is excessive time for such a simple task with only two collision objects in the scene. There is already a previous issue about this, which remains unsolved.

[rhaschke]

With moveit/moveit#3655 and moveit/moveit#3656 I get good (and fast) results for a similar pick-and-place example as yours:

pick-place.mp4

Will test on MoveIt2 (Humble) later.
I noticed, that you don't explicitly specify a target orientation. There should be a console warning stating that the identity is used.

[DaniGarciaLopez]

That's very fast planning! I've never gotten results that quickly using constraints in ROS2. I'm going to try your example and reach you back.

I'm currently using Jazzy with the latest commits. I mentioned it in the first comment, but maybe it was overlooked. Previously, we were using Humble, and constrained planning was also taking a very long time.

I noticed, that you don't explicitly specify a target orientation. There should be a console warning stating that the identity is used.

Do you mean that I don't specify the orientation in the constraint message? There should be no need, at least in the CPP API (I'm not familiar with the Python API). By default the quaternion is [1, 0, 0, 0] (w, x, y, z), so there should be no need to specify orientation.w = 1, I don't get any warning message.

[rhaschke]

I've got my demo script running in Humble. However, I'm getting the following error message:
[moveit_kinematic_constraints.kinematic_constraints]: Could not find link model for link name object

This indicates that the planning request adapter ResolveConstraintFrames is not running. Looks like the planning pipeline setup is broken - the parameter ompl.request_adapters is just loaded for the move_group node, but not the MTC node.
I hate ROS2's approach to parameter handling - one reason I didn't switch yet!

Any help to fix the parameter config is highly welcome, @DaniGarciaLopez, @sjahr, @sea-bass.
I'm running these two commands from within the MTC folder:

ros2 launch moveit_task_constructor_demo demo.launch.py
ros2 launch moveit_task_constructor_demo run.launch.py exe:=demo/scripts/constrained.py

@DaniGarciaLopez: If you see a similar error message, this explains why the constraint is not considered...

[DaniGarciaLopez]

I replicated your example using the CPP API, and it seems that the constraint problem is fixed with your latest commit! With the first commit, I was getting a START_STATE_INVALID error in this task.

rviz3.mp4

So far, I haven't encountered any trajectories that breaks the constraint. However, I'll try it on our setup to confirm if this fully resolves the issue. As you can see, the planning time is still too long (1 minute), whereas without the constraint, it only takes 0.8 seconds.

I couldn't get the Python demos to work on jazzy. When I run your constrained.py, I get the following error:

[ERROR] [1729027019.192923844] [moveit_2325514396.moveit.planners.ompl.planner_manager]: ./src/ompl/base/src/StateSpace.cpp:729 - No default projection is set. Perhaps setup() needs to be called
[ERROR] [1729027019.203995526] [moveit_2325514396.moveit.core.joint_model_group]: Link 'panda_hand' not found in group 'panda_arm'`

Also, pickplace.py is importing moveit_commander here, but it's not ported yet, correct?

However, I'm getting the following error message: [moveit_kinematic_constraints.kinematic_constraints]: Could not find link model for link name object

As far as I'm concerned, ResolveConstraintFrames was always loading properly, I didn't get this error before.

[livanov93]

I've got my demo script running in Humble. However, I'm getting the following error message: [moveit_kinematic_constraints.kinematic_constraints]: Could not find link model for link name object

This indicates that the planning request adapter ResolveConstraintFrames is not running. Looks like the planning pipeline setup is broken - the parameter ompl.request_adapters is just loaded for the move_group node, but not the MTC node. I hate ROS2's approach to parameter handling - one reason I didn't switch yet!

Any help to fix the parameter config is highly welcome, @DaniGarciaLopez, @sjahr, @sea-bass. I'm running these two commands from within the MTC folder:

ros2 launch moveit_task_constructor_demo demo.launch.py
ros2 launch moveit_task_constructor_demo run.launch.py exe:=demo/scripts/constrained.py

@DaniGarciaLopez: If you see a similar error message, this explains why the constraint is not considered...

Is this successful for you on the humble?
I only get:

[constrained.py-1] [INFO] [1729867084.071974062] [moveit.ompl_planning.model_based_planning_context]: panda_arm: Allocating specialized state sampler for state space

[DaniGarciaLopez]

I've just try it out in humble (moveit2 from main and moveit_task_constructor from ros2 branch) and parameters are properly loaded by the MTC node. What I get is a collision between the fingers and the object:

[constrained.py-1] Failing stage(s):
[constrained.py-1] modify planning scene (0/1): panda_leftfinger colliding with objec

Was not colliding for you @rhaschke? By default the robot spawns with the gripper closed, we should add a stage to open the gripper before attaching the object.

Are you also planning to fill a PR porting this changes to moveit2?

[rhaschke]

What I get is a collision between the fingers and the object

You are right. I got this initial collision as well. In MoveIt1, the robot is spawned with the gripper open...
I didn't look where MoveIt2 defines the initial joint states...

Are you also planning to fill a PR porting moveit/moveit#3656 to moveit2?

Yes. If those changes are merged, I will port them.

[rhaschke]

Can we close this issue? I ported the mentioned PRs to MoveIt2: moveit/moveit2#3052

[DaniGarciaLopez]

I just tried it out, and it works great! Thanks, @rhaschke!

We still have the problem of long planning time for constrained trajectories in ROS2, apparently only affects OMPL. Since it’s not fully related to this issue, I think it’s better to open a new ticket in the moveit2 repo and close this one.