OMPL 2D刚体规划Demo

RRT,Rigidbody,SE2

Posted by elmagnifico on March 16, 2018

Foreword

本篇内容基本来自于官网的第一篇教程,但是这里还是有些东西没有解释清楚。

http://ompl.kavrakilab.org/geometricPlanningSE3.html

比如其中的RealVectorBounds维度边界为什么是(-1,1)?

source code

try:
    from ompl import base as ob
    from ompl import geometric as og
except:
    # if the ompl module is not in the PYTHONPATH assume it is installed in a
    # subdirectory of the parent directory called "py-bindings."
    from os.path import abspath, dirname, join
    import sys
    sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings'))
    from ompl import util as ou
    from ompl import base as ob
    from ompl import geometric as og

def isStateValid(state):
    # 不是很明白为什么这里用0.6来判断
    # Some arbitrary condition on the state (note that thanks to
    # dynamic type checking we can just call getX() and do not need
    # to convert state to an SE2State.)
    return state.getX() < .6

def planWithSimpleSetup():
    # 这里使用的状态空间是 (x,y,yaw) 二维加一个航向角
    # create an SE2 state space
    space = ob.SE2StateSpace()

    # 设置向量是2维,上下边界为(-1,1)
    # set lower and upper bounds
    bounds = ob.RealVectorBounds(2)
    bounds.setLow(-1)
    bounds.setHigh(1)
    space.setBounds(bounds)

    # create a simple setup object
    ss = og.SimpleSetup(space)
    ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))

    start = ob.State(space)
    # we can pick a random start state...
    start.random()
    # ... or set specific values
    start().setX(.5)

    goal = ob.State(space)
    # we can pick a random goal state...
    goal.random()
    # ... or set specific values
    goal().setX(-.5)

    ss.setStartAndGoalStates(start, goal)

    # 这里的1.0应该是指规划的上限时间
    # this will automatically choose a default planner with
    # default parameters
    solved = ss.solve(1.0)

    if solved:
        # try to shorten the path
        ss.simplifySolution()
        # print the simplified path
        print(ss.getSolutionPath())

def planTheHardWay():
    # create an SE2 state space
    space = ob.SE2StateSpace()
    # set lower and upper bounds
    bounds = ob.RealVectorBounds(2)
    bounds.setLow(-1)
    bounds.setHigh(1)
    space.setBounds(bounds)
    # construct an instance of space information from this state space
    si = ob.SpaceInformation(space)
    # set state validity checking for this space
    si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
    # create a random start state
    start = ob.State(space)
    start.random()
    # create a random goal state
    goal = ob.State(space)
    goal.random()

    # 使用 ompl::geometric::SimpleSetup 的区别基本就在这一块
    # 不使用的情况下,需要在这里描述问题,就是用使用 ProblemDefinition
    # 这种方式可以指定使用什么来规划
    # create a problem instance
    pdef = ob.ProblemDefinition(si)
    # set the start and goal states
    pdef.setStartAndGoalStates(start, goal)

    # 这里使用RRT进行求解
    # create a planner for the defined space
    planner = og.RRTConnect(si)
    # set the problem we are trying to solve for the planner
    planner.setProblemDefinition(pdef)
    # perform setup steps for the planner
    planner.setup()
    # print the settings for this space
    print(si.settings())
    # print the problem settings
    print(pdef)
    # attempt to solve the problem within one second of planning time
    solved = planner.solve(1.0)

    if solved:
        # get the goal representation from the problem definition (not the same as the goal state)
        # and inquire about the found path
        path = pdef.getSolutionPath()
        print("Found solution:\n%s" % path)
    else:
        print("No solution found")


if __name__ == "__main__":
    # 这个是使用 ompl::geometric::SimpleSetup Class 来完成规划
    planWithSimpleSetup()
    print("")
    # 这个是不使用 ompl::geometric::SimpleSetup Class,总体需要配置的东西会多一些。
    planTheHardWay()

这段代码中,planWithSimpleSetup()总是可以得到解,而planTheHardWay()不一定,是因为前者将x的值限定在了0.6的有效范围内,而后者的x值是完全随机的,所以运行demo的时候有时候后一次没有解,其原因也是其开始位置和结束位置并不正确造成的。

ompl.app

这个除了通过直接运行代码得到几个中间运动路径点,也可以通过app直接看到规划结果。

其中

  • 环境选择 cubicles_env.dae
  • 模型选择 cubicles_robot.dae
  • Robot type 选择 Rigid Body plannning 2D
  • Planner 选择 RRT

修改一下Goal pose ,选择一个合理可以到达没有模型穿插的地方.

然后运行解就能看到对应的结果了,当然教程里所用的是3D规划,是一个刚体(x,y,z)以及一个四元数用来表示旋转角度,和这个略有不同。

Summary

总的来说 ompl::geometric::SimpleSetup Class 是教程中推荐使用的,其bug更少,而且对功能的限制也更少,封装层级也更高一些。

Quote

http://ompl.kavrakilab.org/core/

http://ompl.kavrakilab.org/geometricPlanningSE3.html

http://ompl.kavrakilab.org/classompl_1_1base_1_1SE2StateSpace.html