ompl一些示例.pdf-资料库

example解析 #include #include #include #include #include #include #include #include using namespace std; namespace ob = ompl::base; namespace og = ompl::geometric; namespace po = boost::program_options; bool isStateValidEasy(const ob::SpaceInformation *si, const ob::State *state) { const auto *s = state->as(); double x = s->getX(), y = s->getY(); / / 空间状态的定义 h t t p : / / o m p l . k a v r a k i l a b . o r g / c l a s s o m p l _ 1 _ 1 b a s e _ 1 _ 1 S c o p e d S t a t e . h t m l / / 几何问题通常所需要的类集 h t t p : / / o m p l . k a v r a k i l a b . o r g / c l a s s o m p l _ 1 _ 1 g e o m e t r i c _ 1 _ 1 S i m p l e S e t u p . h t m l / / 定义了 O M P L 版本号信息，及相关功能是否编译过 ~ / / 程序命令行参数的输入及解析 h t t p s : / / b l o g . c s d n . n e t / m o r n i n g _ c o l o r / a r t i c l e / d e t a i l s / 5 0 2 4 1 9 8 7 / / 行驶路径的约束条件

return si->satisfiesBounds(s) && (x < 5 || x>13 || (y > 8.5 && y < 9.5)); } bool isStateValidHard(const ob::SpaceInformation *si, const ob::State *state) { return si->satisfiesBounds(state); } void printTrajectory(const ob::StateSpacePtr& space, const std::vector& pt) { if (pt.size() != 3) throw ompl::Exception("3 arguments required for trajectory option"); const unsigned int num_pts = 50; ob::ScopedState<> from(space), to(space), s(space); std::vector reals; from[0] = from[1] = from[2] = 0.; to[0] = pt[0]; to[1] = pt[1]; to[2] = pt[2]; std::cout << "distance: " << space->distance(from(), to()) << "\npath:\n"; for (unsigned int i = 0; i <= num_pts; ++i) { space->interpolate(from(), to(), (double)i / num_pts, s()); reals = s.reals(); std::cout << "path " << reals[0] << ' ' << reals[1] << ' ' << reals[2] << ' ' << std::endl; } } void printDistanceGrid(const ob::StateSpacePtr& space) { const unsigned int num_pts = 200; ob::ScopedState<> from(space), to(space); from[0] = from[1] = from[2] = 0.; / / p r i n t t h e d i s t a n c e f o r ( x , y , t h e t a ) f o r a l l p o i n t s i n a 3 D g r i d i n S E ( 2 ) / / o v e r [ - 5 , 5 ) x [ - 5 , 5 ) x [ - p i , p i ) . / / / / T h e o u t p u t s h o u l d b e r e d i r e c t e d t o a f i l e , s a y , d i s t a n c e . t x t . T h i s / / c a n t h e n b e r e a d a n d p l o t t e d i n M a t l a b l i k e s o : / / x = r e s h a p e ( l o a d ( ' d i s t a n c e . t x t ' ) , 2 0 0 , 2 0 0 , 2 0 0 ) ; / / f o r i = 1 : 2 0 0 , / / c o n t o u r f ( s q u e e z e ( x ( i , : , : ) ) , 3 0 ) ; / / a x i s e q u a l ; a x i s t i g h t ; c o l o r b a r ; p a u s e ; / / e n d ; / * o f s t r e a m o u t f ; o u t f . o p e n ( " d i s t a n c e . t x t " ) ; * /

for (unsigned int i = 0; i < num_pts; ++i) for (unsigned int j = 0; j < num_pts; ++j) for (unsigned int k = 0; k < num_pts; ++k) { } num_pts - 1.); } to[0] = 5. * (2. * (double)i / num_pts - 1.); to[1] = 5. * (2. * (double)j / num_pts - 1.); to[2] = boost::math::constants::pi() * (2. * (double)k / void plan(const ob::StateSpacePtr &space, bool easy) { ob::ScopedState<> start(space), goal(space); ob::RealVectorBounds bounds(2); bounds.setLow(0); int k=bounds.high.size(); if (easy) bounds.setHigh(18); else { } bounds.high[0] = 6; bounds.high[1] = .6; space->as()->setBounds(bounds); og::SimpleSetup ss(space); const ob::SpaceInformation *si = ss.getSpaceInformation().get(); auto isStateValid = easy ? isStateValidEasy : isStateValidHard; ss.setStateValidityChecker([isStateValid, si](const ob::State *state) {return isStateValid(si, state); }); if (easy) { start[0] = start[1] = 1.0; start[2] = 0; goal[0] = goal[1] = 16; goal[2] = -.99*boost::math::constants::pi(); } else / / o u t f < < s p a c e - > d i s t a n c e ( f r o m ( ) , t o ( ) ) < < " " ; / / s t d : : c o u t < < s p a c e - > d i s t a n c e ( f r o m ( ) , t o ( ) ) < < ' \ n ' ; / / o u t f . c l o s e ( ) ; / / 根据所使用的空间分配状态 / / 边界空间的维度此处 2 维每个维度都有个 l o w 和 h i g h 值故 b o u n d s . l o w 有两个值 / / a s 转换为某个实例所需要的类型 / / 构造所需的规划空间 / / 对此空间的有效性进行检查 h t t p : / / o m p l . k a v r a k i l a b . o r g / s t a t e V a l i d a t i o n . h t m l / / 函数作为右值赋值给函数指针，成员函数需取地址后赋值给函数指针（语法）

{ } start[0] = start[1] = .5; start[2] = .5*boost::math::constants::pi();; goal[0] = 5.5; goal[1] = .5; goal[2] = .5*boost::math::constants::pi(); ss.setStartAndGoalStates(start, goal); ss.getSpaceInformation()->setStateValidityCheckingResolution(0.005); ss.setup(); ss.print(); ob::PlannerStatus sloved = ss.solve( ); if (sloved) { vector reals; cout << "solution" << endl; ss.simplifySolution(); og::PathGeometric path = ss.getSolutionPath(); path.interpolate(); path.printAsMatrix(cout); } else cout << "no solution" << endl; } int main(int argc,char** argv) { cout << OMPL_VERSION << endl; argc = 7; argv[0] = ""; argv[1] = "--easyplan"; argv[2] = "--trajectory"; argv[3] = "7.1"; argv[4] = "6.2"; argv[5] = "1.34"; argv[6] = "--distance"; try { po::options_description desc("Options"); / / 可选的参数，从而获得更多的输出信息 / / 设置需要验证状态有效性的分辨率，以便将两个状态之间的运动视为有效。该值指定为空间范围的一部分 / / 自动创建规划所需的类， s l o v e ( ) 函数的 / * c o n s t b a s e : : P l a n n e r T e r m i n a t i o n C o n d i t i o n & p t c * / / / 按时间，也有按自定义函数 / / 尝试简化当前路径，简化方式，默认 0 s 内简化，或按自定义函数简化 / / 获取路径， / / 默认插值，指定插值数量插值法 / / 打印矩阵形式 / / 深入浅出 b o o s t : : p r o g r a m _ o p t i o n s ，命令行或配置文件读取参数选项 h t t p s : / / b l o g . c s d n . n e t / W i n d g s _ Y F / a r t i c l e / d e t a i l s / 8 1 2 0 1 4 5 6 / / 输入参数的描述

desc.add_options() ("help", "show help message") ("dubins", "use Dubins state space") ("dubinssym", "use symmetrized Dubins state space") ("reedsshepp", "use reeds-shepp state space(default)") ("easyplan", "easy planning problem & print path") ("hardplan", "hard planning problem & print path") ("trajectory", po::value>()->multitoken(), "print x y theta from start goal") ()); ("distance", "print distance grid") ; po::variables_map vm; po::store(po::parse_command_line(argc, argv, desc), vm); po::notify(vm); if (vm.count("help")) { cout << desc << "/n"; return 1; }; ob::StateSpacePtr space(make_shared() ); if (vm.count("dubins")!=0u) space = make_shared(); if (vm.count("dubinssym")!=0u) space = make_shared(1., true); if (vm.count("easyplan") != 0u) plan(space, true); if (vm.count("hardplan") != 0u) plan(space, false); if (vm.count("trajectory") != 0u) printTrajectory(space, vm["trajectroy"].as> if (vm.count("distance") != 0u) printDistanceGrid(space); } catch (const std::exception&e) { } cerr << "error:" << e.what() << "\n"; return 1; catch (...) { / / m u l t i t o k e n ( ) 的作用就是告诉编译器 , 该选项可接受多个值 / / 容器 , 用于存储解析后的选项 , 是 s t d : : m a p 的派生类 , 可以像关联容器一样使用 / / 命令行的参数用 s t o r e 解析出来并存储至 v m / / 更新所有外部变量 / / c o u n t ( ) - > 检测某个选项是否被输入 / / 指向 r e e d s s h e p p 状态空间的智能指针 / / 对称 d u b i n s 状态空间 / / v a r i a b l e s _ m a p ( 选项存储器 ) 是 s t d : : m a p 的派生类 , 可以像关联容器一样使用 , 通过 o p e r a t o r [ ] 来取出其中的元素 . 但其内部的元素类型 v a l u e _ t y p e 是 b o o s t : : a n y , 用来存储不确定类型的参数值 , 必须通过模板成员函数 a s < t y p e > ( ) 做类型转换后 , 才能获取其具体值 .

cerr << "exception of unkonw type!\n"; } return 0; } 参考学习1 原博链接：https://blog.csdn.net/ljq31446/article/category/7534293 code #include #include #include #include #include #include #include #include #include //绑定函数 using namespace std; namespace ob = ompl::base; namespace og = ompl::geometric; bool isStateValid(const ob::State *state) { const ob::SE3StateSpace::StateType *se3state = state->as(); const ob::RealVectorStateSpace::StateType *pos = se3state- >as(0); const ob::SO3StateSpace::StateType *rot = se3state->as(1); return (const void *)rot != (const void *)pos; } / / 抽象类型转换为我们期望类型 / / 提取第 1 、 2 状态的组成，并转换为我们期望的 / / 确定状态是否可行，这里一直为 t r u e ，避免编译器警告

void planWithSimpleSetup() { ob::StateSpacePtr space(new ob::SE3StateSpace()); ob::RealVectorBounds bounds(3); bounds.setHigh(1); bounds.setLow(-1); space->as()->setBounds(bounds); og::SimpleSetup ss(space); ss.setStateValidityChecker(boost::bind(&isStateValid,_1)); ob::ScopedState<> start(space),goal(space); start.random(); goal.random(); start.print(); ss.setStartAndGoalStates(start, goal); ob::PlannerPtr planner(new og::RRT(ss.getSpaceInformation())); ss.setPlanner(planner); ob::PlannerStatus solved = ss.solve(1.0); if (solved) { } else cout << "Found solution\n" << endl; ofstream osf0("path0.txt"); ss.getSolutionPath().printAsMatrix(osf0); ofstream osf1("path1.txt"); ss.simplifySolution(); ss.getSolutionPath().printAsMatrix(osf1); cout << "No found" << endl; } int main(int, char**) { cout << "OMPL_VERSION:" << OMPL_VERSION << endl; planWithSimpleSetup(); / / 声明我们规划所在的空间维度 / / 设置三维空间的边界 / / 定义一个简易类 / / 路径约束检查 , 使用 b i n d 绑定函数，参考 h t t p s : / / b l o g . c s d n . n e t / g i e p y / a r t i c l e / d e t a i l s / 4 5 0 4 6 7 3 7 / / 随机创建一个起始点和目标点 / / 加入起终点 / / 设定规划方法 / / 在规划的时间内解决 / / 解决则导出生成的路径

return 0; } 结果学习到的点 1. 如何使用不同的规划方法声明一个构造所需的规划空间： og::SimpleSetup ss(space); 将起终点加入到： ss.setStartAndGoalStates(start, goal); 是否对行驶路径进行约束： isStateValid 添加规划方法（在\ompl\geometric\planners里有各种规划方法）： ob::PlannerPtr planner(new og::InformedRRTstar(ss.getSpaceInformation())); 加入到规划空间中： ss.setPlanner(planner); 生成规划路径： ss.solve(）若找到路径则进行···处理： if (sloved) {...} 2. 使用bind进行绑定函数，输入为空_1占位 3. 生成的结果每一行有七位数，前3位表示真实位置，后四位表示so3群的值，参考 https://blog.csdn.net/qq_28448117/article/details/79644920 4. matlab 使用plot3画三维图

资料库

ompl一些示例.pdf

相关推荐

开发技术

热门标签

最新资料