Robotics Library + OMPL + URDF + SRDF

测试环境/版本

• Ubuntu 22.04
• Robotics Library - master
• ROS2/MoveIt2 Humble
• OMPL 1.5
• python3.8

一、Robotics Library + OMPL

1. 安装二进制版本的ompl

此处不对ompl做修改，故直接二进制的，默认版本为1.5

sudo apt-get install libompl-dev ompl-demos
1

2. 准备roboticslibrary库的master分支

一定要使用master手动编译，才有URDF加载的支持（虽然该支持还不完美）

git clone https://github.com/roboticslibrary/rl.git
1

3. 修改plan模块，以添加ompl支持

按照ompl集成中提到的commit修改。此时会因commit时版本过老旧，导致大量报错。故我带着大家逐步修改：

3.1 修改rl根目录下CMakeLists.txt

第9行

set(CMAKE_CXX_STANDARD 11)
1

为

set(CMAKE_CXX_STANDARD 14)
1

避免了报错 error: no template named ‘remove_cv_t’ in namespace ‘std’; did
you mean ‘remove_cv’?

3.2 修改rl/src/rl/plan/CMakeLists.txt

• 首先，在

  set(HDRS ...)
  1

  前，加上

   set(ompl_DIR "/usr/share/ompl/cmake")
   find_package(ompl REQUIRED)
  1
  2

  设置ompl_DIR是为了防止cmake时链接到MoveIt2 humble中默认二进制安装的OMPL1.6。
  因为观察到若链接到MoveIt2 humble中的OMPL1.6，cmake时会有大量警告。

• 然后，在

  add_library(...)
  1

  前，加上

  if(OMPL_FOUND)
      list(APPEND HDRS OMPL.h)
      list(APPEND SRCS OMPL.cpp)
      message(${SRCS})
  endif(OMPL_FOUND)
  1
  2
  3
  4
  5

• 最后，在

  target_link_libraries(...)
  1

  后，加上

  if(OMPL_FOUND)
      target_include_directories(plan PUBLIC ${OMPL_INCLUDE_DIRS})
      target_link_libraries(plan ${OMPL_LIBRARIES})
  endif(OMPL_FOUND)
  1
  2
  3
  4

3.3 添加.h和.cpp到rl/src/rl/plan下

首先复制commit中的OMPL.h和OMPL.cpp到rl/src/rl/plan下

• 修改OMPL.h第48行

  class OMPLPlanner : public  ::rl::plan::Planner
  1

  为

  class RL_PLAN_EXPORT OMPLPlanner : public ::rl::plan::Planner
  1

• 修改OMPL.h第62行

  virtual void getPath(VectorList& path);
  1

  为

   virtual VectorList getPath();
  1

  对应的修改OMPL.cpp 339行起

    void OMPLPlanner::getPath(VectorList& path)
    {
         if (setup.haveSolutionPath())
         {
             setup.simplifySolution();
             ompl::geometric::PathGeometric& p(setup.getSolutionPath());
             path.clear();
             for (unsigned int i = 0; i < p.getStateCount(); ++i)
                 path.push_back(
                     static_cast<ompl::base::OMPLStateSpace::StateType*>(p.getState(i))->rlState);
         }
     }
  1
  2
  3
  4
  5
  6
  7
  8
  9
  10
  11
  12

  为

  VectorList OMPLPlanner::getPath()
  {
      VectorList path;
      if (setup.haveSolutionPath())
      {
          setup.simplifySolution();
          ompl::geometric::PathGeometric& p(setup.getSolutionPath());
          path.clear();
          for (unsigned int i = 0; i < p.getStateCount(); ++i)
              path.push_back(
                  static_cast<ompl::base::OMPLStateSpace::StateType*>(p.getState(i))->rlState);
      }
      return path;
  }
  1
  2
  3
  4
  5
  6
  7
  8
  9
  10
  11
  12
  13
  14

• OMPL.cpp中，改所有

  model->getMaximum(maximum);
  model->getMinimum(minimum);
  1
  2

  为

  maximum = model->getMaximum();
  minimum = model->getMinimum();
  1
  2

  包括 改

  space_->as<OMPLStateSpace>()->model->getMaximum(maximum);
  space_->as<OMPLStateSpace>()->model->getMinimum(minimum );
  1
  2

  为

  maximum = space_->as<OMPLStateSpace>()->model->getMaximum();
  minimum = space_->as<OMPLStateSpace>()->model->getMinimum();
  1
  2

• 修改

  return setup.solve(this->duration);
  1

  为

  double seconds = std::chrono::duration<double>(this->duration).count();
  return setup.solve(seconds);
  1
  2

• 修改 OMPL.cpp 223行

  virtual void project(const State *state, EuclideanProjection &projection) const
  1

  为

  virtual void project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const
  1

4. 按照rl官方步骤编译

官方步骤

二、Robotics Library + URDF + SRDF

1. URDF

1.1. urdf源自ROS的机器人描述文件，若为xacro形式则需转换

ros2 run xacro xacro -o xxx.urdf yyy.xacro
1

注意：rl读取urdf时，对大于边长10m的geometry box会报错，目前暂时改为9.99m,或采用多个拼装为大于10m的，如

<geometry>
	<box size="5 9.99 0.05"/>
</geometry>
1
2
3

注意：所有""中带有数字的部分，尽量不要有多余的空格，读取时有崩溃风险，如

<origin rpy=" 1.0 0 5 " xyz="1.0  2.0  -9.0"/>    读取时有崩溃风险
<origin rpy="1.0 0 5" xyz="1.0 2.0 -9.0"/>
1
2

1.2. URDF中的filename为相对/绝对路径

待续…

2. SRDF

1. SRDF可以由moveit_setup_assistant导出而来，我们仅使用其中disable标签中的内容.

待续…