{"id":103,"date":"2021-11-21T11:21:10","date_gmt":"2021-11-21T11:21:10","guid":{"rendered":"http:\/\/47.103.123.166\/?p=103"},"modified":"2021-11-21T13:09:58","modified_gmt":"2021-11-21T13:09:58","slug":"%e8%bf%90%e5%8a%a8%e8%a7%84%e5%88%92%e5%ad%a6%e4%b9%a0%e7%ac%94%e8%ae%b04-%e6%8e%a2%e7%b4%a2ompl","status":"publish","type":"post","link":"http:\/\/47.103.123.166\/?p=103","title":{"rendered":"\u8fd0\u52a8\u89c4\u5212\u5b66\u4e60\u7b14\u8bb04\u2014\u2014\u63a2\u7d22OMPL"},"content":{"rendered":"

\u5728\u8fd9\u91cc\u5206\u4eab\u4e86\u8fd0\u52a8\u89c4\u5212\u65b9\u9762\u7684\u4e00\u4e9b\u57fa\u672c\u7684\u7b97\u6cd5\u539f\u7406\u548c\u4f2a\u4ee3\u7801\u5b9e\u73b0\uff0c\u5176\u4e2d\u5185\u5bb9\u53ef\u80fd\u5b58\u5728\u4e0d\u5b8c\u5584\u548c\u9519\u8bef\u4e4b\u5904\uff0c\u5982\u6709\u8bfb\u8005\u53d1\u73b0\uff0c\u6b22\u8fce\u6279\u8bc4\u6307\u6b63\u3002<\/p>\n

A\u3001OMPL\u7f16\u8bd1\u4e0e\u5b89\u88c5<\/h1>\n
    \n
  1. \u65b9\u5f0f\u4e00\uff1a\u901a\u8fc7\u5b98\u7f51\u4e0b\u8f7dinstall-ompl-ubuntu.sh\u6587\u4ef6\uff0c\u8fd0\u884c\u540e\uff0c\u81ea\u52a8\u7f16\u8bd1\u5b89\u88c5 https:\/\/ompl.kavrakilab.org\/installation.html<\/a><\/a>\uff0c\u6b64\u65b9\u6cd5\u5728\u4f7f\u7528\u5e93\u6587\u4ef6\u65f6\uff0ccmakeconfig\u6587\u4ef6\u53ef\u80fd\u6709\u95ee\u9898\uff0c\u8fd8\u662f\u5efa\u8bae\u65b9\u6cd5\u4e8c\u3002<\/li>\n
  2. \u65b9\u5f0f\u4e8c\uff1a\u901a\u8fc7Github\u4e0b\u8f7d\u6e90\u7801\uff0c\u81ea\u5df1\u7f16\u8bd1\u5b89\u88c5\uff0c https:\/\/github.com\/ompl\/ompl<\/a><\/a>\uff0c<\/li>\n
  3. \u68c0\u67e5\u5b89\u88c5\u6210\u529f \u53ca ROS\u4e2d\u4f7f\u7528 https:\/\/blog.csdn.net\/zghforever\/article\/details\/106688410<\/a><\/a><\/li>\n
  4. \u5b98\u65b9\u6559\u7a0b https:\/\/ompl.kavrakilab.org\/tutorials.html<\/a><\/a><\/li>\n<\/ol>\n

    B\u3001OMPL\u4f7f\u7528<\/h1>\n

    B1\u3001\u57fa\u672c\u5b9a\u4e49<\/h2>\n
      \n
    1. \u6c42\u89e3\u65b9\u5f0f
      \n\u6c42\u89e3\u95ee\u9898\u7684\u8fc7\u7a0b\u4e2d\uff0c\u6839\u636e\u662f\u5426\u4f7f\u7528ompl::geometric::SimpleSetup<\/strong>\u7c7b\uff0c\u6709\u4e24\u79cd\u65b9\u5f0f\uff0c\u7b80\u5355\u89c4\u5212\u548c\u5b8c\u6574\u89c4\u5212\uff0c\u89c1\u4ee3\u7801\u90e8\u5206C1<\/strong>\u3002<\/li>\n
    2. \u6c42\u89e3\u8fc7\u7a0b\uff1a\u5229\u7528OMPL\u63d0\u4f9b\u7684\u7c7b\u548c\u65b9\u6cd5\u6765\u89c4\u5212\uff0c\u57fa\u672c\u8fc7\u7a0b\u5305\u62ec\u4ee5\u4e0b\u51e0\u4e2a\u90e8\u5206
      \n♥ \u786e\u8ba4\u5b9e\u9645\u95ee\u9898\u7684\u72b6\u6001\u7a7a\u95f4 \uff08\u5982SE(3)\uff09
      \n♥ \u4eceOMPL\u63d0\u4f9b\u7684\u7c7b\u4e2d\u6784\u5efa\u4e00\u4e2a\u72b6\u6001\u7a7a\u95f4\uff08\u5982ompl::base::SE3StateSpace is appropriate\uff09
      \n♥ \u8bbe\u7f6e\u72b6\u6001\u7a7a\u95f4\u7684\u8fb9\u754c
      \n♥ \u5b9a\u4e49\u72b6\u6001\u6709\u6548\u6027\u7684\u5224\u65ad\u65b9\u6cd5
      \n♥ \u5b9a\u4e49\u8d77\u70b9\u548c\u7ec8\u70b9\u7684\u72b6\u6001
      \n♥ \u6c42\u89e3\u548c\u53ef\u89c6\u5316<\/li>\n
    3. \n

      \u72b6\u6001\u4e0e\u72b6\u6001\u7a7a\u95f4<\/p>\n

      \/\/\u72b6\u6001\u4e0e\u72b6\u6001\u7a7a\u95f4\u7684\u57fa\u672c\u5b9a\u4e49\nompl::base::StateSpacePtr space(new T());\nompl::base::ScopedState<> state1(space);    \/\/\u76f4\u63a5\u901a\u8fc7\u72b6\u6001\u7a7a\u95f4StateSpace\u521d\u59cb\u5316\nompl::base::SpaceInformationPtr si(space);\nompl::base::ScopedState<T> state2(si);      \/\/\u901a\u8fc7SpaceInformation\u521d\u59cb\u5316\n\/\/\u4e3e\u4e2a\u6817\u5b50\nompl::base::StateSpacePtr space(new ompl::base::SE2StateSpace());\nompl::base::ScopedState<ompl::base::SE2StateSpace> state(space);\n    state->setX(0.1);\n    state->setY(0.2);\n    state->setYaw(0.0);\nompl::base::ScopedState<> backup = state;   \/\/state == backup, backup\u672c\u8d28\u662fState*, \u4f5c\u7528\u662f\u5907\u4efd\u53c2\u6570\uff0c \u56e0\u6b64setX()\u7b49\u51fd\u6570\u65e0\u6548\n\/\/\u72b6\u6001\u7a7a\u95f4\u7684\u5408\u5e76\n\/\/\u5408\u5e76\u65b9\u5f0f1\nompl::base::CompoundStateSpace *cs = new ompl::base::CompoundStateSpace();  \/\/\u5229\u7528CompoundStateSpace\u7c7b\u6765\u6dfb\u52a0\u4e0d\u540c\u72b6\u6001\u7a7a\u95f4\ncs->addSubspace(ompl::base::StateSpacePtr(new ompl::base::SO2StateSpace()), 1.0);\ncs->addSubspace(ompl::base::StateSpacePtr(new ompl::base::SO3StateSpace()), 1.0);\nompl::base::StateSpacePtr space(cs);\n\/\/\u5408\u5e76\u65b9\u5f0f2\n\/\/ define the individual state spaces\nompl::base::StateSpacePtr so2(new ompl::base::SO2StateSpace());\nompl::base::StateSpacePtr so3(new ompl::base::SO3StateSpace());\nompl::base::StateSpacePtr space = so2 + so3;\n\n\/\/\u867d\u7136\u7a7a\u95f4\u53ef\u4ee5\u6df7\u5408\uff0c\u4f46\u5bf9\u4e8e\u72b6\u6001\u4ecd\u7136\u9700\u8981\u6307\u5b9a\u5177\u4f53\u7684\u7c7b\u578b\nompl::base::ScopedState<ompl::base::CompoundStateSpace> state(space);\nstate->as<ompl::base::SO2StateSpace::StateType>(0)->setIdentity();<\/code><\/pre>\n<\/li>\n
    4. \n
      \u6709\u6548\u6027\u68c0\u67e5
      \nOMPL\u4e0d\u63d0\u4f9b\u5177\u4f53\u68c0\u67e5\u65b9\u5f0f\uff0c\u9700\u8981\u6839\u636e\u5177\u4f53\u7684\u95ee\u9898\u5177\u4f53\u786e\u5b9a\u3002\u7528\u6237\u6307\u5b9a\u6709\u6548\u6027\u68c0\u67e5\u7684\u65b9\u5f0f\u6709\u4e24\u79cd\uff0c\u89c1\u4ee3\u7801\u90e8\u5206C2<\/strong>
      \n♥ \u7ee7\u627fOMPL\u5b9a\u4e49\u7684\u4e24\u4e2a\u62bd\u8c61\u7c7b
      \n♥ \u76f4\u63a5\u5b9a\u4e49\u6709\u6548\u6027\u68c0\u67e5\u51fd\u6570<\/p>\n
      \/\/ \u65b9\u6cd52\uff1a\u76f4\u63a5\u5b9a\u4e49\u5224\u65ad\u51fd\u6570\nbool myStateValidityCheckerFunction(const base::State *state)\n{\n return ...;\n}\n\/\/\u65b9\u6cd52\uff1a\u4f7f\u7528\nbase::SpaceInformationPtr si(space);\nsi->setStateValidityChecker(myStateValidityCheckerFunction);\nsi->setStateValidityCheckingResolution(0.03); \/\/ 3%\nsi->setup();<\/code><\/pre>\n<\/li>\n
    5. \u91c7\u6837
      \n\u91c7\u6837\u7684\u4e24\u79cd\u65b9\u5f0f\uff1a\u57fa\u4e8eompl::base::StateSampler<\/strong>\u548c\u57fa\u4e8eompl::base::ValidStateSampler<\/strong>\u3002
      \n♥ \u57fa\u4e8eompl::base::StateSampler<\/strong>\uff1a\u751f\u6210\u72b6\u6001\u7a7a\u95f4\u7684\u4e00\u4e2a\u72b6\u6001\uff0c\u751f\u6210\u72b6\u6001\u9644\u8fd1\u7684\u4e00\u4e2a\u72b6\u6001\uff0c\u751f\u6210\u9ad8\u65af\u5206\u5e03\u7684\u4e00\u7ec4\u72b6\u6001\u3002
      \n♥ \u57fa\u4e8eompl::base::ValidStateSampler<\/strong>\uff1a\u4ee5ompl::base::StateSampler\u4e3a\u57fa\u7840\uff0c\u4e0d\u65ad\u91c7\u6837\uff0c\u76f4\u5230\u91c7\u5230\u4e00\u4e2a\u6709\u6548\u72b6\u6001\u6216\u8005\u5230\u8fbe\u8fed\u4ee3\u6700\u5927\u503c\uff0c\u6709\u6548\u6027\u901a\u8fc7ompl::base::SpaceInformation::isValid\u5224\u65ad\u3002
      \n♥ OMPL\u63d0\u4f9b\u4e86\u51e0\u4e2a\u7ee7\u627f\u81eaompl::base::ValidStateSampler\u7684\u7c7b\uff0c\u5982\u4e0b\uff0c\u53ef\u4ee5\u4f7f\u7528\u8fd9\u4e9b\u7c7b\u6216\u7ee7\u627fompl::base::ValidStateSampler\u91cd\u65b0\u7f16\u5199\u91c7\u6837\u5668\uff0c\u57fa\u672c\u4f7f\u7528\u65b9\u6cd5\u5982\u4e0b\uff0c\u5177\u4f53\u4ee3\u7801\u4e0e\u89e3\u91ca\u89c1\u4ee3\u7801\u90e8\u5206C3<\/strong>\u3002<\/li>\n<\/ol>\n\n\n\n\n\n\n\n\n
      <\/th>\n<\/th>\n<\/tr>\n<\/thead>\n
      ompl::base::UniformValidStateSampler<\/td>\n\u9ed8\u8ba4\u91c7\u6837\u5668<\/td>\n<\/tr>\n
      ompl::base::ObstacleBasedValidStateSampler<\/td>\n\u91c7\u6837\u4e00\u4e2a\u6709\u6548\u72b6\u6001\u548c\u4e00\u4e2a\u65e0\u6548\u72b6\u6001\uff0c\u5728\u4e24\u8005\u4e4b\u95f4\u63d2\u503c\uff0c\u8fd4\u56de\u6700\u63a5\u8fd1\u65e0\u6548\u72b6\u6001\u7684\u4e00\u4e2a\u6709\u6548\u7684\u63d2\u503c\u72b6\u6001\uff0c\u5373\u903c\u8fd1\u969c\u788d\u7269<\/td>\n<\/tr>\n
      ompl::base::GaussianValidStateSampler<\/td>\n\u91c7\u6837\u4e00\u5bf9\u72b6\u6001\uff0c\u7b2c\u4e00\u4e2a\u72b6\u6001\u5747\u5300\u968f\u673a\u91c7\u96c6\uff0c\u7b2c\u4e8c\u4e2a\u72b6\u6001\u5728\u4ee5\u7b2c\u4e00\u4e2a\u72b6\u6001\u4e3a\u4e2d\u5fc3\u7684\u9ad8\u65af\u5206\u5e03\u4e0a\u91c7\u96c6\uff0c\u4e24\u72b6\u6001\u90fd\u6709\u6548\u6216\u90fd\u65e0\u6548\uff0c\u5219\u518d\u53d6\u4e00\u5bf9\uff0c\u5426\u5219\u8fd4\u56de\u6709\u6548\u72b6\u6001\uff0c\u4e5f\u5728\u903c\u8fd1\u969c\u788d\u7269<\/td>\n<\/tr>\n
      ompl::base::MaximizeClearanceValidStateSampler<\/td>\n\u5728\u9ed8\u8ba4\u91c7\u6837\u5668\u7684\u57fa\u7840\u4e0a\uff0c\u51fa\u4e8e\u5b89\u5168\u8003\u8651\uff0c\u6700\u5927\u5316\u8def\u5f84\u95f4\u9699\uff0c\u4f7f\u5f97\u72b6\u6001\u4e0e\u969c\u788d\u7269\u4e4b\u95f4\u7684\u95f4\u9699\u6700\u5927\u5316<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n
      \/\/\u57fa\u672c\u4f7f\u7528\u65b9\u5f0f\n\/\/***************************\u5b9a\u4e49\u72b6\u6001\u5206\u914d\u5668\uff08\u51fd\u6570\uff09***************************\nompl::base::ValidStateSamplerPtr allocOBValidStateSampler(const ompl::base::SpaceInformation *si)\n{\n    \/\/ we can perform any additional setup \/ configuration of a sampler here,\n    \/\/ but there is nothing to tweak in case of the ObstacleBasedValidStateSampler.\n    return std::make_shared<ompl::base::ObstacleBasedValidStateSampler>(si);\n}\n\/\/***************************\u4f7f\u7528***************************\nss.getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);<\/code><\/pre>\n
      B2\u3001\u8def\u5f84\u53ef\u89c6\u5316<\/h2>\n
        \n
      1. OMPL\u4e0d\u63d0\u4f9b\u8def\u5f84\u53ef\u89c6\u5316\u5de5\u5177\uff0c\u76f4\u63a5\u4e00\u70b9\u7684\u65b9\u5f0f\u662f\u4f5c\u4e3a\u77e9\u9635\u6253\u5370\u8f93\u51fa\uff0c\u4fdd\u5b58\u6210\u6587\u672c\u6587\u4ef6\u3002<\/li>\n
      2. OMPL\u63d0\u4f9b\u4e24\u79cd\u8def\u5f84\uff1aompl::geometric::PathGeometric<\/strong> \u548c ompl::control::PathControl<\/strong>\uff0c\u4e24\u4e2a\u7c7b\u53ef\u7528\u6210\u5458\u51fd\u6570printAsMatrix()\u6253\u5370\uff0c\u524d\u8005\u6bcf\u884c\u6253\u5370\u4e00\u4e2a\u72b6\u6001\uff0c\u540e\u8005\u6bcf\u884c\u9664\u4e86\u72b6\u6001\uff0c\u8fd8\u6709\u63a7\u5236\u6307\u4ee4\u548c\u63a7\u5236\u95f4\u9694\u65f6\u95f4\u3002<\/li>\n
      3. \n
        \u6253\u5370\u65b9\u5f0f\u5982\u4e0b\uff1a<\/p>\n
        bool solved = ss.solve(20.0);\nif (solved)\n{\n\/\/ if ss is a ompl::geometric::SimpleSetup object\nss.getSolutionPath().printAsMatrix(std::cout);\n\n\/\/ if ss is a ompl::control::SimpleSetup object\nss.getSolutionPath().asGeometric().printAsMatrix(std::cout);\n}<\/code><\/pre>\n<\/li>\n<\/ol>\n
        B3\u3001API Overview<\/h2>\n
        \"\u5728\u8fd9\u91cc\u63d2\u5165\u56fe\u7247\u63cf\u8ff0\"<\/p>\n
        C\u3001\u4ee3\u7801\u9644\u5f55<\/h1>\n
        C1\u3001Geometric Planning for a Rigid Body in 3D<\/h2>\n
        \/\/\u6ce8\uff1a\u8fd9\u662f\u5728ROS\u91cc\u9762\u5199\u7684\uff0c\u867d\u7136\u8fd9\u4e00\u90e8\u5206\u8ddfROS\u6682\u65f6\u6ca1\u5173\u7cfb\u3002\n#include <ros\/ros.h>\n#include <iostream>\n\n#include <ompl\/base\/spaces\/SE3StateSpace.h>\n\n\/\/\u4f7f\u7528ompl::geometric::SimpleSetup\u7c7b\n#include <ompl\/geometric\/SimpleSetup.h>\n\/\/\u4e0d\u4f7f\u7528ompl::geometric::SimpleSetup\u7c7b\n#include <ompl\/base\/SpaceInformation.h>\n#include <ompl\/geometric\/planners\/rrt\/RRTConnect.h>\n\nnamespace ob = ompl::base;\nnamespace og = ompl::geometric;\n\n\/\/\u72b6\u6001\u68c0\u67e5\u51fd\u6570\nbool isStateValid(const ob::State *state)\n{\n    \/\/ cast the abstract state type to the type we expect\n    \/\/ \u5c06\u62bd\u8c61\u72b6\u6001\u7c7b\u578b\u8f6c\u6362\u4e3a\u6211\u4eec\u671f\u671b\u7684\u7c7b\u578b\n    const auto *se3state = state->as<ob::SE3StateSpace::StateType>();\n\n    \/\/ extract the first component of the state and cast it to what we expect \n    \/\/ \u63d0\u53d6\u72b6\u6001\u7684\u7b2c\u4e00\u4e2a\u7ec4\u4ef6\u5e76\u5c06\u5176\u8f6c\u6362\u4e3a\u6211\u4eec\u6240\u671f\u671b\u7684\n    const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);\n\n    \/\/ extract the second component of the state and cast it to what we expect\n    \/\/ \u63d0\u53d6\u72b6\u6001\u7684\u7b2c\u4e8c\u4e2a\u7ec4\u4ef6\u5e76\u5c06\u5176\u8f6c\u6362\u4e3a\u6211\u4eec\u6240\u671f\u671b\u7684\n    const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1);\n\n    \/\/ check validity of state defined by pos & rot\n\n    \/\/ return a value that is always true but uses the two variables we define, so we avoid compiler warnings\n    \/\/ \u8fd4\u56de\u4e00\u4e2a\u59cb\u7ec8\u4e3atrue\u4f46\u4f7f\u7528\u6211\u4eec\u5b9a\u4e49\u7684\u4e24\u4e2a\u53d8\u91cf\u7684\u503c\n    return (const void*)rot != (const void*)pos;\n}\n\n\/\/\u7b80\u5355\u89c4\u5212\uff1a\u4f7f\u7528ompl::geometric::SimpleSetup\u7c7b\nvoid planWithSimpleSetup()\n{\n    \/\/ \u72b6\u6001\u7a7a\u95f4\uff1a\u6784\u5efa\n    auto space(std::make_shared<ob::SE3StateSpace>());\n\n    \/\/ \u72b6\u6001\u7a7a\u95f4\uff1a\u8fb9\u754c\n    ob::RealVectorBounds bounds(3);\n    bounds.setLow(-1);\n    bounds.setHigh(1);\n    space->setBounds(bounds);\n\n    \/\/ \u914d\u7f6e\u7c7b\uff1a\u6784\u5efa\n    og::SimpleSetup ss(space);\n\n    \/\/ \u914d\u7f6e\u7c7b\uff1a\u8bbe\u7f6e\u72b6\u6001\u6709\u6548\u6027\u68c0\u67e5\u5668\n    ss.setStateValidityChecker([](const ob::State *state) { return isStateValid(state); });\n\n    \/\/ \u914d\u7f6e\u7c7b\uff1a\u8bbe\u7f6e\u8d77\u7ec8\u70b9 \n    ob::ScopedState<> start(space);\n    start.random();\n    ob::ScopedState<> goal(space);\n    goal.random();\n    ss.setStartAndGoalStates(start, goal);\n\n    \/\/ this call is optional, but we put it in to get more output information\n    ss.setup();\n    ss.print();\n\n    \/\/ attempt to solve the problem within one second of planning time\n    ob::PlannerStatus solved = ss.solve(1.0);\n\n    if (solved)\n    {\n        std::cout << \"Found solution:\" << std::endl;\n        \/\/ print the path to screen\n        ss.simplifySolution();\n        ss.getSolutionPath().print(std::cout);\n    }\n    else\n        std::cout << \"No solution found\" << std::endl;\n}\n\n\/\/\u5b8c\u6574\u89c4\u5212\uff1a\u4e0d\u4f7f\u7528ompl::geometric::SimpleSetup\u7c7b\nvoid plan()\n{\n    \/\/ \u72b6\u6001\u7a7a\u95f4\uff1a\u6784\u5efa\n    auto space(std::make_shared<ob::SE3StateSpace>());\n\n    \/\/ \u72b6\u6001\u7a7a\u95f4\uff1a\u8fb9\u754c\n    ob::RealVectorBounds bounds(3);\n    bounds.setLow(-1);\n    bounds.setHigh(1);\n    space->setBounds(bounds);\n\n    \/\/ \u7a7a\u95f4\u4fe1\u606f\uff1a\u6784\u5efa\n    auto si(std::make_shared<ob::SpaceInformation>(space));\n\n    \/\/ \u7a7a\u95f4\u4fe1\u606f\uff1a\u8bbe\u7f6e\u72b6\u6001\u6709\u6548\u6027\u68c0\u67e5\u5668\n    si->setStateValidityChecker(isStateValid);\n\n    \/\/ \u95ee\u9898\u5b9e\u4f8b\uff1a\u6784\u5efa\n    auto pdef(std::make_shared<ob::ProblemDefinition>(si));\n\n    \/\/ \u95ee\u9898\u5b9e\u4f8b\uff1a\u8bbe\u7f6e\u8d77\u7ec8\u70b9\n    ob::ScopedState<> start(space);\n    start.random();\n    ob::ScopedState<> goal(space);\n    goal.random();\n    pdef->setStartAndGoalStates(start, goal);\n\n    \/\/ \u89c4\u5212\u65b9\u6cd5\uff1a\u6784\u5efa\n    auto planner(std::make_shared<og::RRTConnect>(si));\n\n    \/\/ \u89c4\u5212\u65b9\u6cd5\uff1a\u5b9e\u9645\u95ee\u9898\n    planner->setProblemDefinition(pdef);\n\n    \/\/ \u89c4\u5212\u65b9\u6cd5\uff1a\u521d\u59cb\u5316\n    planner->setup();\n\n    \/\/ \u6253\u5370\u7a7a\u95f4\u4fe1\u606f\u548c\u95ee\u9898\u5b9e\u4f8b\n    si->printSettings(std::cout);\n    pdef->print(std::cout);\n\n    \/\/ \u6c42\u89e3\n    ob::PlannerStatus solved = planner->ob::Planner::solve(1.0);\n\n    if (solved)\n    {\n        \/\/ get the goal representation from the problem definition (not the same as the goal state)\n        \/\/ and inquire about the found path\n        ob::PathPtr path = pdef->getSolutionPath();\n        std::cout << \"Found solution:\" << std::endl;\n\n        \/\/ print the path to screen\n        path->print(std::cout);\n    }\n    else\n        std::cout << \"No solution found\" << std::endl;\n}\n\nint main(int argc, char** argv)\n{\n    ros::init(argc, argv, \"rigid_body_planning\");\n    ros::NodeHandle nh;\n\n    std::cout << \"OMPL version: \" << OMPL_VERSION << std::endl;\n    plan();\n    std::cout << std::endl << std::endl;\n    planWithSimpleSetup();  \n\n    return 0;\n}<\/code><\/pre>\n
        C2\u3001\u6709\u6548\u6027\u68c0\u67e5\u65b9\u5f0f<\/h2>\n
        \/\/\u65b9\u6cd51\uff1a\u7ee7\u627fOMPL\u5b9a\u4e49\u7684\u62bd\u8c61\u7c7b base::StateValidityChecker\nclass myStateValidityCheckerClass : public base::StateValidityChecker\n{\npublic:\n     myStateValidityCheckerClass(const base::SpaceInformationPtr &si) :\n       base::StateValidityChecker(si)\n        {\n     }\n\n     virtual bool isValid(const base::State *state) const\n     {\n             return ...;\n     }\n};\n\/\/\u65b9\u6cd51\uff1a\u4f7f\u7528\nbase::SpaceInformationPtr si(space);\nsi->setStateValidityChecker(std::make_shared<myStateValidityCheckerClass>(si));\nsi->setup();\n\n\/\/\u65b9\u6cd51\uff1a\u7ee7\u627fOMPL\u5b9a\u4e49\u7684\u62bd\u8c61\u7c7b base::MotionValidator\nclass myMotionValidator : public base::MotionValidator\n{\npublic:\n    \/\/ implement checkMotion()\n};\n\/\/\u65b9\u6cd51\uff1a\u4f7f\u7528\nbase::SpaceInformationPtr si(space);\nsi->setMotionValidator(std::make_shared<myMotionValidator>(si));\nsi->setup();\n\n\/\/ \u65b9\u6cd52\uff1a\u76f4\u63a5\u5b9a\u4e49\u5224\u65ad\u51fd\u6570\nbool myStateValidityCheckerFunction(const base::State *state)\n{\n     return ...;\n}\n\/\/\u65b9\u6cd52\uff1a\u4f7f\u7528\nbase::SpaceInformationPtr si(space);\nsi->setStateValidityChecker(myStateValidityCheckerFunction);\nsi->setStateValidityCheckingResolution(0.03); \/\/ 3%\nsi->setup();<\/code><\/pre>\n
        C3\u3001\u91c7\u6837\u65b9\u5f0f<\/h2>\n
        1 \u5df2\u6709\u91c7\u6837\u5668<\/h3>\n
        \u4e0d\u80fd\u76f4\u63a5\u5728SimpleSetup\u6216\u8005SpaceInformation\u7c7b\u4e2d\u914d\u7f6e\u91c7\u6837\u65b9\u5f0f\uff0c\u9700\u8981\u901a\u8fc7\u5b9a\u4e49\u4e0b\u9762\u7c7b\u578b\u7684\u51fd\u6570\uff0c\u8f93\u5165ompl::base::SpaceInformation\uff0c\u8fd4\u56deompl::base::ValidStateSamplerPtr\u3002<\/p>\n
        \/\/***************************\u5b9a\u4e49\u72b6\u6001\u5206\u914d\u5668\uff08\u51fd\u6570\uff09***************************\nompl::base::ValidStateSamplerPtr allocOBValidStateSampler(const ompl::base::SpaceInformation *si)\n{\n    \/\/ we can perform any additional setup \/ configuration of a sampler here,\n    \/\/ but there is nothing to tweak in case of the ObstacleBasedValidStateSampler.\n    return std::make_shared<ompl::base::ObstacleBasedValidStateSampler>(si);\n}\n\/\/***************************\u4f7f\u7528***************************\nss.getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);<\/code><\/pre>\n
        2 \u81ea\u5b9a\u4e49\u91c7\u6837\u5668<\/h3>\n
        \u7ee7\u627fompl::base::ValidStateSampler\u91cd\u65b0\u7f16\u5199\u91c7\u6837\u5668\uff0c\u7ee7\u627f\u540e\u7684\u7c7b\u4e0eOMPL\u63d0\u4f9b\u7684\u7c7b\u7684\u4f7f\u7528\u65b9\u6cd5\u76f8\u540c\u3002<\/p>\n
        \/\/***************************\u5b9a\u4e49\u5b50\u7c7b***************************\nnamespace ob = ompl::base;\nnamespace og = ompl::geometric;\n\n\/\/ This is a problem-specific sampler that automatically generates valid\n\/\/ states; it doesn't need to call SpaceInformation::isValid. This is an\n\/\/ example of constrained sampling. If you can explicitly describe the set valid\n\/\/ states and can draw samples from it, then this is typically much more\n\/\/ efficient than generating random samples from the entire state space and\n\/\/ checking for validity.\nclass MyValidStateSampler : public ob::ValidStateSampler\n{\npublic:\n    MyValidStateSampler(const ob::SpaceInformation *si) : ValidStateSampler(si)\n    {\n        name_ = \"my sampler\";\n    }\n    \/\/ Generate a sample in the valid part of the R^3 state space\n    \/\/ Valid states satisfy the following constraints:\n    \/\/ -1<= x,y,z <=1\n    \/\/ if .25 <= z <= .5, then |x|>.8 and |y|>.8\n    bool sample(ob::State *state) override\n    {\n        double* val = static_cast<ob::RealVectorStateSpace::StateType*>(state)->values;\n        double z = rng_.uniformReal(-1,1);\n\n        if (z>.25 && z<.5)\n        {\n            double x = rng_.uniformReal(0,1.8), y = rng_.uniformReal(0,.2);\n            switch(rng_.uniformInt(0,3))\n            {\n                case 0: val[0]=x-1;  val[1]=y-1;  break;\n                case 1: val[0]=x-.8; val[1]=y+.8; break;\n                case 2: val[0]=y-1;  val[1]=x-1;  break;\n                case 3: val[0]=y+.8; val[1]=x-.8; break;\n            }\n        }\n        else\n        {\n            val[0] = rng_.uniformReal(-1,1);\n            val[1] = rng_.uniformReal(-1,1);\n        }\n        val[2] = z;\n        assert(si_->isValid(state));\n        return true;\n    }\n    \/\/ We don't need this in the example below.\n    bool sampleNear(ob::State* \/*state*\/, const ob::State* \/*near*\/, const double \/*distance*\/) override\n    {\n        throw ompl::Exception(\"MyValidStateSampler::sampleNear\", \"not implemented\");\n        return false;\n    }\nprotected:\n    ompl::RNG rng_;\n};\n\/\/***************************\u5b9a\u4e49\u72b6\u6001\u5206\u914d\u5668\uff08\u51fd\u6570\uff09***************************\nob::ValidStateSamplerPtr allocMyValidStateSampler(const ob::SpaceInformation *si)\n{\n    return std::make_shared<MyValidStateSampler>(si);\n}\n\/\/***************************\u4f7f\u7528***************************\nss.getSpaceInformation()->setValidStateSamplerAllocator(allocMyValidStateSampler);<\/code><\/pre>\n","protected":false},"excerpt":{"rendered":"
        \u5728\u8fd9\u91cc\u5206\u4eab\u4e86\u8fd0\u52a8\u89c4\u5212\u65b9\u9762\u7684\u4e00\u4e9b\u57fa\u672c\u7684\u7b97\u6cd5\u539f\u7406\u548c\u4f2a\u4ee3\u7801\u5b9e\u73b0\uff0c\u5176\u4e2d\u5185\u5bb9\u53ef\u80fd\u5b58\u5728\u4e0d\u5b8c\u5584\u548c\u9519\u8bef\u4e4b\u5904\uff0c\u5982\u6709\u8bfb\u8005\u53d1\u73b0\uff0c\u6b22\u8fce\u6279\u8bc4\u6307\u6b63\u3002 A\u3001OMPL\u7f16\u8bd1\u4e0e\u5b89\u88c5 \u65b9\u5f0f\u4e00\uff1a\u901a\u8fc7\u5b98\u7f51\u4e0b\u8f7dinstall-ompl-ubuntu.sh\u6587\u4ef6\uff0c\u8fd0\u884c\u540e\uff0c\u81ea\u52a8\u7f16\u8bd1\u5b89\u88c5 https:\/\/ompl.kavrakilab.org\/installation.html\uff0c\u6b64\u65b9\u6cd5\u5728\u4f7f\u7528\u5e93\u6587\u4ef6\u65f6\uff0ccmakeconfig\u6587\u4ef6\u53ef\u80fd\u6709\u95ee\u9898\uff0c\u8fd8\u662f\u5efa\u8bae\u65b9\u6cd5\u4e8c\u3002 \u65b9\u5f0f\u4e8c\uff1a\u901a\u8fc7Github\u4e0b\u8f7d\u6e90\u7801\uff0c\u81ea\u5df1\u7f16\u8bd1\u5b89\u88c5\uff0c https:\/\/github.com\/ompl\/ompl\uff0c \u68c0\u67e5\u5b89\u88c5\u6210\u529f \u53ca ROS\u4e2d\u4f7f\u7528 https:\/\/blog.csdn.net\/zghforever\/article\/details\/106688410 \u5b98\u65b9\u6559\u7a0b https:\/\/ompl.kavrakilab.org\/tutorials.html B\u3001OMPL\u4f7f\u7528 B1\u3001\u57fa\u672c\u5b9a\u4e49 \u6c42\u89e3\u65b9\u5f0f \u6c42\u89e3\u95ee\u9898\u7684\u8fc7\u7a0b\u4e2d\uff0c\u6839\u636e\u662f\u5426\u4f7f\u7528ompl::geometric::SimpleSetup\u7c7b\uff0c\u6709\u4e24\u79cd\u65b9\u5f0f\uff0c\u7b80\u5355\u89c4\u5212\u548c\u5b8c\u6574\u89c4\u5212\uff0c\u89c1\u4ee3\u7801\u90e8\u5206C1\u3002 \u6c42\u89e3\u8fc7\u7a0b\uff1a\u5229\u7528OMPL\u63d0\u4f9b\u7684\u7c7b\u548c\u65b9\u6cd5\u6765\u89c4\u5212\uff0c\u57fa\u672c\u8fc7\u7a0b\u5305\u62ec\u4ee5\u4e0b\u51e0\u4e2a\u90e8\u5206 ♥ \u786e\u8ba4\u5b9e\u9645\u95ee\u9898\u7684\u72b6\u6001\u7a7a\u95f4 \uff08\u5982SE(3)\uff09 ♥ \u4eceOMPL\u63d0\u4f9b\u7684\u7c7b\u4e2d\u6784\u5efa\u4e00\u4e2a\u72b6\u6001\u7a7a\u95f4\uff08\u5982ompl::base::SE3StateSpace is appropriate\uff09 ♥ \u8bbe\u7f6e\u72b6\u6001\u7a7a\u95f4\u7684\u8fb9\u754c ♥ \u5b9a\u4e49\u72b6\u6001\u6709\u6548\u6027\u7684\u5224\u65ad\u65b9\u6cd5 ♥ \u5b9a\u4e49\u8d77\u70b9\u548c\u7ec8\u70b9\u7684\u72b6\u6001 ♥ \u6c42\u89e3\u548c\u53ef\u89c6\u5316 \u72b6\u6001\u4e0e\u72b6\u6001\u7a7a\u95f4 \/\/\u72b6\u6001\u4e0e\u72b6\u6001\u7a7a\u95f4\u7684\u57fa\u672c\u5b9a\u4e49 ompl::base::StateSpacePtr space(new T()); ompl::base::ScopedState<> state1(space); \/\/\u76f4\u63a5\u901a\u8fc7\u72b6\u6001\u7a7a\u95f4StateSpace\u521d\u59cb\u5316 ompl::base::SpaceInformationPtr si(space); ompl::base::ScopedState<T> state2(si); \/\/\u901a\u8fc7SpaceInformation\u521d\u59cb\u5316 \/\/\u4e3e\u4e2a\u6817\u5b50 ompl::base::StateSpacePtr space(new ompl::base::SE2StateSpace()); ompl::base::ScopedState<ompl::base::SE2StateSpace> state(space); state->setX(0.1); state->setY(0.2); state->setYaw(0.0); ompl::base::ScopedState<> […]<\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":[],"categories":[2],"tags":[],"_links":{"self":[{"href":"http:\/\/47.103.123.166\/index.php?rest_route=\/wp\/v2\/posts\/103"}],"collection":[{"href":"http:\/\/47.103.123.166\/index.php?rest_route=\/wp\/v2\/posts"}],"about":[{"href":"http:\/\/47.103.123.166\/index.php?rest_route=\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"http:\/\/47.103.123.166\/index.php?rest_route=\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"http:\/\/47.103.123.166\/index.php?rest_route=%2Fwp%2Fv2%2Fcomments&post=103"}],"version-history":[{"count":3,"href":"http:\/\/47.103.123.166\/index.php?rest_route=\/wp\/v2\/posts\/103\/revisions"}],"predecessor-version":[{"id":194,"href":"http:\/\/47.103.123.166\/index.php?rest_route=\/wp\/v2\/posts\/103\/revisions\/194"}],"wp:attachment":[{"href":"http:\/\/47.103.123.166\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=103"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"http:\/\/47.103.123.166\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=103"},{"taxonomy":"post_tag","embeddable":true,"href":"http:\/\/47.103.123.166\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=103"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}