about orientation constrain planning

[Dingran12]

Hello, I want to use the orientation constraints with the moveit task constructor, it works well with Moveit+ ROS melodic+Ubuntu 18.04. But with Moveit2+ ROS Humble+ Ubuntu 22.04, it planned for a long time, even planned failed many times. We have tried these several cases with moveit2 version:

panda robot------------works well panda robot + orientation constraints applied for the connect stage--------------planned for a long time our own Nachi robot (with ik_fast solver ) ----------works well our own Nachi robot (with ik_fast solver )+ orientation constraints applied for the connect stage/moveto stage -----------planned for a long time or failed. I used the mtc orientation constraint in this way, but it does't work, you can find the planning time is very long. I have also tried this constraint in Moveit+ ROS melodic+Ubuntu 18.04, it planned quickly and the planned path is good. I wonder if there is something wrong with my usage or if this function needs to be improved? Thanks very much!

[rhaschke]

@henningkayser, do you know any reasons why constraint-based planning fails in MoveIt2?

[forrest-rm]

@Dingran12 or @henningkayser, was any solution found for getting this to work faster?

I'm seeing the same thing when I tried to add orientation constraints to a MTC MoveTo Stage with the same setup as Dingran12 (Moveit2+ ROS Humble+ Ubuntu 22.04). When a solution was found, it took about 15x slower than without the constraint and didn't seem to respect the stage timeout parameter. I tried with both OMPL planners RRTConnect and AnytimePathShortening (with parallel RRTConnect planners).

  // Get current joint state
  auto stage_state_current = std::make_unique<mtc::stages::CurrentState>("current");
  mtc::Stage* current_state_ptr = stage_state_current.get();
  task.add(std::move(stage_state_current));

  // Create orientation constraint to keep gripper close to vertical
  moveit_msgs::msg::OrientationConstraint orientation_constraint;
  orientation_constraint.header.frame_id = "bin1_link";
  orientation_constraint.link_name = end_effector_frame_;
  orientation_constraint.orientation.x = 1.;
  orientation_constraint.orientation.y = 0.;
  orientation_constraint.orientation.z = 0.;
  orientation_constraint.orientation.w = 0.;
  orientation_constraint.absolute_x_axis_tolerance = 0.8;
  orientation_constraint.absolute_y_axis_tolerance = 0.8;
  orientation_constraint.absolute_z_axis_tolerance = 3.141;
  orientation_constraint.weight = 1.0;
  moveit_msgs::msg::Constraints orientation_constraints;
  orientation_constraints.orientation_constraints.emplace_back(orientation_constraint);

  auto stage_move_to_place = std::make_unique<mtc::stages::Connect>(
    "move to place", mtc::stages::Connect::GroupPlannerVector{{arm_group_name_, sampling_planner}});
  stage_move_to_place->setTimeout(kPlaceMoveToTimeout);
  stage_move_to_place->properties().configureInitFrom(mtc::Stage::PARENT);
  stage_move_to_place->setPathConstraints(orientation_constraints);
  task.add(std::move(stage_move_to_place));

Something which may or may not be related is that if I set enforce_constrained_state_space: true in ompl_planning.yaml, then the system crashed.

[Dingran12]

@forrest-rm Thank you for trying to provide me with some clues, but I haven't solved this problem yet, and I don't know if it is a usage problem or a problem with this function.

[Danilrivero]

Hello,

I am seeing the same thing as well when adding orientation constraints to a MoveTo stage with the same setup using Humble. Ideally I would need to keep the same orientation constraints for all the stages. I wonder if any clues have been found about this problem.