moveit自定义路径规划算法,终于实现了,花了五天时间,一直解决不了bug,下面讲讲如何在moveit中自定义路径规划算法
sudo apt-get remove ros-melodic-moveit-*
rosdep update
sudo apt-get update
sudo apt-get dist-upgrade
常见bug:rosdep update会报错,更新不了,一定能解决的方法,见下连接:
rosdep update一定能通过de简单方法_努力是明天快乐的源泉-CSDN博客_rosdep update
sudo apt-get install python-wstool python-catkin-tools clang-format-3.9
mkdir ~/ws_moveit
cd ~/ws_moveit
source /opt/ros/melodic/setup.bash
wstool init src
wstool merge -t src https://raw.githubusercontent.com/ros-planning/moveit/master/moveit.rosinstall
wstool update -t src
rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO}
catkin config --extend /opt/ros/${ROS_DISTRO} --cmake-args -DCMAKE_BUILD_TYPE=Release
sudo catkin build
常见bug:在进行 wstool merge -t src https://raw.githubusercontent.com/ros-planning/moveit/master/moveit.rosinstall报错:
ERROR in config: Unable to download URL [https://raw.githubusercontent.com/ros-planning/moveit/master/moveit.rosinstall]:
解决办法:
点击进入https://raw.githubusercontent.com/ros-planning/moveit/master/moveit.rosinstall
分别进入这6个链接,下载melodic版本的源码到/home/arl/ws_moveit/src
cd ~/moveit_ws/src
git clone https://github.com/ompl/ompl
注意:git clone 可能不好用,建议点击链接进行下载
cd ~/moveit_ws/
wget https://raw.githubusercontent.com/ros-gbp/ompl-release/debian/melodic/xenial/ompl/package.xml
sudo catkin make
source ~/ws_moveit/devel/setup.bash
roslaunch panda_moveit_config demo.launch
Could NOT find CUDA: Found unsuitable version "10.2", but required is exact
version "10.0" (found /usr/local/cuda-10.0)
见cmake - OpenCV claims to find "wrong" cuda version - Stack Overflow
CUDA多个版本的切换----亲测可用_OscarMind的博客-CSDN博客_cuda多个版本
sudo rm -rf cuda
sudo ln -s /usr/local/cuda-[需要的版本] /usr/local/cuda
cd ~/moveit_ws/
sudo rm -rf build
sudo catkin build
(1)在路径(~/moveit_ws/src/ompl/src/ompl/geometric/planners/rrt/src)复制TRRT.cpp,并重命名为TORRT.cpp。在路径(~/moveit_ws/src/ompl/src/ompl/geometric/planners/rrt)复制TRRT.h,并重命名为TORRT.h;
(2)使用ctrl+F将TORRT.cpp和TORRT.h文件中的TRRT替换为TORRT
(3)在路径(~moveit_ws/src/moveit/moveit_planners/ompl/ompl_interface/src)中找到planning_context_manager.cpp,添加头文件
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
添加接口
void ompl_interface::PlanningContextManager::registerDefaultPlanners()
{
registerPlannerAllocator( //
"geometric::RRT", //
std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::RRTConnect", //
std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::LazyRRT", //
std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::TRRT", //
std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::TORRT", //
std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
(4) 打开/home/arl/ws_moveit/src/panda_moveit_config/config/ompl_planning.yaml
改三处
第一处
planner_configs:
SBL:
type: geometric::SBL
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
EST:
type: geometric::EST
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
LBKPIECE:
type: geometric::LBKPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
BKPIECE:
type: geometric::BKPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
KPIECE:
type: geometric::KPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.]
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
RRT:
type: geometric::RRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
RRTConnect:
type: geometric::RRTConnect
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
RRTstar:
type: geometric::RRTstar
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1
TRRT:
type: geometric::TRRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
max_states_failed: 10 # when to start increasing temp. default: 10
temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0
min_temperature: 10e-10 # lower limit of temp change. default: 10e-10
init_temperature: 10e-6 # initial temperature. default: 10e-6
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup()
TORRT:
type: geometric::TORRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
max_states_failed: 10 # when to start increasing temp. default: 10
temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0
min_temperature: 10e-10 # lower limit of temp change. default: 10e-10
init_temperature: 10e-6 # initial temperature. default: 10e-6
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup()
第二和三处
panda_arm:
default_planner_config: RRT
planner_configs:
- SBL
- EST
- LBKPIECE
- BKPIECE
- KPIECE
- RRT
- RRTConnect
- RRTstar
- TRRT
- TORRT
- PRM
- PRMstar
- FMT
- BFMT
- PDST
- STRIDE
- BiTRRT
- LBTRRT
- BiEST
- ProjEST
- LazyPRM
- LazyPRMstar
- SPARS
- SPARStwo
projection_evaluator: joints(panda_joint1,panda_joint2)
longest_valid_segment_fraction: 0.005
hand:
default_planner_config: RRT
planner_configs:
- SBL
- EST
- LBKPIECE
- BKPIECE
- KPIECE
- RRT
- RRTConnect
- RRTstar
- TRRT
- TORRT
- PRM
- PRMstar
- FMT
- BFMT
- PDST
- STRIDE
- BiTRRT
- LBTRRT
- BiEST
- ProjEST
- LazyPRM
- LazyPRMstar
- SPARS
- SPARStwo
(5)
cd ~/moveit_ws/
sudo catkin make
报错:Errors << moveit_planners_ompl:make /home/arl/ws_moveit/logs/moveit_planners_ompl/build.make.020.log
Errors << moveit_planners_ompl:make /home/arl/ws_moveit/logs/moveit_planners_ompl/build.make.020.log
/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.1.0.8:对‘ompl::geometric::TORRT::~TORRT()’未定义的引用
/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.1.0.8:对‘ompl::geometric::TORRT::TORRT(std::shared_ptr const&)’未定义的引用
collect2: error: ld returned 1 exit status
make[2]: *** [/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/moveit_planners_ompl/moveit_ompl_planner] Error 1
make[1]: *** [ompl_interface/CMakeFiles/moveit_ompl_planner.dir/all] Error 2
make[1]: *** 正在等待未完成的任务....
/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.1.0.8:对‘ompl::geometric::TORRT::~TORRT()’未定义的引用
/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.1.0.8:对‘ompl::geometric::TORRT::TORRT(std::shared_ptr const&)’未定义的引用
collect2: error: ld returned 1 exit status
make[2]: *** [/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/moveit_planners_ompl/generate_state_database] Error 1
make[1]: *** [ompl_interface/CMakeFiles/moveit_generate_state_database.dir/all] Error 2
make: *** [all] Error 2
cd /home/arl/ws_moveit/build/moveit_planners_ompl; catkin build --get-env moveit_planners_ompl | catkin env -si /usr/bin/make --jobserver-fds=6,7 -j; cd -
解决办法:
ROS Kinetic版本下如何一步一步实现一个基本机械臂运动规划算法,如PRM? - 知乎
cd /home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/
sudo rm -rf libmoveit_ompl_interface.so.1.0.8
cd ~/moveit_ws/
sudo catkin make
再运行demo就可以了
roslaunch panda_moveit_config demo.launch
MoveIt 1 Source Build - Linux | MoveIt
Dependencies Source Install | MoveIt