WIP: simplify state space selection in the OMPL interface

[JeroenDM]

Description

This is a proposal to simplify the OMPL interface by changing how the state space type is selected. It removes the flexibility of the current priority system. But, as there are only two different state spaces, it seemed a bit overengineered.

Before

The current system created a state space in two steps:

1. A state space factory is created based on a priority calculated for each state space type.
2. The state space factory creates the state space.

The planning context manager has a map with state-space factories. Based on the planning request, each factory gets a priority, calculated by ModelBasedStateSpace::canRepresentProblem. For a specific request, the factory with the highest priority is selected, unless enforce_joint_model_state_space is true (specified by the user in ompl_config.yaml).

The priority is calculated based on

• Is there an IK solver?
• Are there joint or visibility constraints?
• Are there path constraints?

The priority values and selection process is shown below:

After

The factories and priority system is replaced with an if-else statement to select the correct state space type:

or in code:

const std::string& ompl_interface::PlanningContextManager::selectStateSpaceType(
    const moveit_msgs::MotionPlanRequest& req, bool enforce_joint_model_state_space) const
{
  // the user forced us to plan in joint space
  if (enforce_joint_model_state_space)
  {
    return ompl_interface::JointModelStateSpace::PARAMETERIZATION_TYPE;
  }

  // if there are joint or visibility constraints, we need to plan in joint space
  else if (req.path_constraints.joint_constraints.empty() && req.path_constraints.visibility_constraints.empty())
  {
    return ompl_interface::JointModelStateSpace::PARAMETERIZATION_TYPE;
  }

  // if we have path constraints and an inverse kinematics solver, we prefer interpolating in pose space
  else if ((!req.path_constraints.position_constraints.empty() ||
            !req.path_constraints.orientation_constraints.empty()) &&
           doesGroupHaveIKSolver(req.group_name))
  {
    return ompl_interface::PoseModelStateSpace::PARAMETERIZATION_TYPE;
  }
  // in all other cases, plan in joint space
  else
  {
    return ompl_interface::JointModelStateSpace::PARAMETERIZATION_TYPE;
  }
}

Things that I think are less ideal with this implementation

• The PARAMETERIZATION_TYPE is a generic string. It would be nice if it were some kind of class enum or something.
• The selectStateSpaceType and createStateSpace could be combined in a single method, were it not for the fact that state spaces are cached using their parameterization type. But I don't know a better way.

Alternatives

We can also keep the state space factories and only remove the priority system.

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[JeroenDM]

Failed test, but only sometimes, why is it not repeatable? Maybe related to caching of the planning context?

rostest moveit_ros_planning_interface move_group_interface_cpp_test.test

catkin run_tests --no-deps --this

[ROSTEST]-----------------------------------------------------------------------

[moveit_ros_planning_interface.rosunit-move_group_interface_cpp_test/MoveToPoseTest][passed]
[moveit_ros_planning_interface.rosunit-move_group_interface_cpp_test/JointSpaceGoalTest][passed]
[moveit_ros_planning_interface.rosunit-move_group_interface_cpp_test/PathConstraintTest][FAILURE]
/home/jeroen/Documents/github/moveit_ws/src/moveit/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp:100
      Expected: move_group_->plan(my_plan)
      Which is: 4-byte object <FF-FF FF-FF>
To be equal to: moveit::planning_interface::MoveItErrorCode::SUCCESS
      Which is: 1
Google Test trace:
/home/jeroen/Documents/github/moveit_ws/src/moveit/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp:98: planAndMove
/home/jeroen/Documents/github/moveit_ws/src/moveit/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp:91: planAndMoveToPose
/home/jeroen/Documents/github/moveit_ws/src/moveit/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp:207: PathConstraintTest
--------------------------------------------------------------------------------


[moveit_ros_planning_interface.rosunit-move_group_interface_cpp_test/PathConstraintTest][FAILURE]
/home/jeroen/Documents/github/moveit_ws/src/moveit/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp:109
Value of: actual.isApprox(expected, EPSILON)
  Actual: false
Expected: true
expected: 
   1    0    0 0.28
   0    1    0 -0.2
   0    0    1  0.5
   0    0    0    1
actual: 
           1 -4.09182e-06  5.02734e-06     0.549998
 4.09182e-06            1 -1.93899e-06   -0.0499997
-5.02733e-06  1.93901e-06            1     0.799997
           0            0            0            1
Google Test trace:
/home/jeroen/Documents/github/moveit_ws/src/moveit/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp:106: testEigenPose
/home/jeroen/Documents/github/moveit_ws/src/moveit/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp:114: testPose(const Eigen::Isometry3d&)
/home/jeroen/Documents/github/moveit_ws/src/moveit/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp:126: testPose(const geometry_msgs::Pose&)
/home/jeroen/Documents/github/moveit_ws/src/moveit/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp:207: PathConstraintTest
--------------------------------------------------------------------------------

[moveit_ros_planning_interface.rosunit-move_group_interface_cpp_test/CartPathTest][passed]
[moveit_ros_planning_interface.rosunit-move_group_interface_cpp_test/CollisionObjectsTest][FAILURE]
/home/jeroen/Documents/github/moveit_ws/src/moveit/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp:92
Value of: move_group_->setJointValueTarget(pose)
  Actual: false
Expected: true
Google Test trace:
/home/jeroen/Documents/github/moveit_ws/src/moveit/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp:91: planAndMoveToPose
/home/jeroen/Documents/github/moveit_ws/src/moveit/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp:286: CollisionObjectsTest
--------------------------------------------------------------------------------


SUMMARY
 * RESULT: FAIL
 * TESTS: 5
 * ERRORS: 0
 * FAILURES: 2