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
2. 准备roboticslibrary库的master分支
一定要使用master手动编译,才有URDF加载的支持(虽然该支持还不完美)
git clone https://github.com/roboticslibrary/rl.git
3. 修改plan模块,以添加ompl支持
按照ompl集成中提到的commit修改。此时会因commit时版本过老旧,导致大量报错。故我带着大家逐步修改:
3.1 修改rl根目录下CMakeLists.txt
第9行
set(CMAKE_CXX_STANDARD 11)
为
set(CMAKE_CXX_STANDARD 14)
避免了报错 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 ...)
前,加上
set(ompl_DIR "/usr/share/ompl/cmake") find_package(ompl REQUIRED)
设置ompl_DIR是为了防止cmake时链接到MoveIt2 humble中默认二进制安装的OMPL1.6。
因为观察到若链接到MoveIt2 humble中的OMPL1.6,cmake时会有大量警告。 -
然后,在
add_library(...)
前,加上
if(OMPL_FOUND) list(APPEND HDRS OMPL.h) list(APPEND SRCS OMPL.cpp) message(${SRCS}) endif(OMPL_FOUND)
-
最后,在
target_link_libraries(...)
后,加上
if(OMPL_FOUND) target_include_directories(plan PUBLIC ${OMPL_INCLUDE_DIRS}) target_link_libraries(plan ${OMPL_LIBRARIES}) endif(OMPL_FOUND)
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
为
class RL_PLAN_EXPORT OMPLPlanner : public ::rl::plan::Planner
-
修改OMPL.h第62行
virtual void getPath(VectorList& path);
为
virtual VectorList getPath();
对应的修改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); } }
为
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; }
-
OMPL.cpp中,改所有
model->getMaximum(maximum); model->getMinimum(minimum);
为
maximum = model->getMaximum(); minimum = model->getMinimum();
包括 改
space_->as<OMPLStateSpace>()->model->getMaximum(maximum); space_->as<OMPLStateSpace>()->model->getMinimum(minimum );
为
maximum = space_->as<OMPLStateSpace>()->model->getMaximum(); minimum = space_->as<OMPLStateSpace>()->model->getMinimum();
-
修改
return setup.solve(this->duration);
为
double seconds = std::chrono::duration<double>(this->duration).count(); return setup.solve(seconds);
-
修改 OMPL.cpp 223行
virtual void project(const State *state, EuclideanProjection &projection) const
为
virtual void project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const
4. 按照rl官方步骤编译
二、Robotics Library + URDF + SRDF
1. URDF
1.1. urdf源自ROS的机器人描述文件,若为xacro形式则需转换
ros2 run xacro xacro -o xxx.urdf yyy.xacro
注意:rl读取urdf时,对大于边长10m的geometry box会报错,目前暂时改为9.99m,或采用多个拼装为大于10m的,如
<geometry>
<box size="5 9.99 0.05"/>
</geometry>
注意:所有""中带有数字的部分,尽量不要有多余的空格,读取时有崩溃风险,如
<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. URDF中的filename为相对/绝对路径
待续…
2. SRDF
- SRDF可以由moveit_setup_assistant导出而来,我们仅使用其中disable标签中的内容.
待续…