RandomWalkPlanner.py

#!/usr/bin/env python3
 

 
# Author: Mark Moll, Weihang Guo
 
from ompl import base as ob
from ompl import geometric as og
 
 
class RandomWalkPlanner(ob.Planner):
    def __init__(self, si):
        super().__init__(si, "RandomWalkPlanner")
        self.states_ = []
        self.sampler_ = si.allocStateSampler()
 
    def setup(self):
        pass
 
    def solve(self, ptc):
        pdef = self.getProblemDefinition()
        goal = pdef.getGoal()
        si = self.getSpaceInformation()
        pi = self.getPlannerInputStates()
        st = pi.nextStart()
        while st:
            start_state = si.allocState()
            si.copyState(start_state, st)
            self.states_.append(start_state)
            st = pi.nextStart()
        solution = None
        approxsol = 0
        approxdif = 1e6
        while not ptc():
            rstate = si.allocState()
            # pick a random state in the state space
            self.sampler_.sampleUniform(rstate)
            # check motion
            if si.checkMotion(self.states_[-1], rstate):
                self.states_.append(rstate)
                sat = goal.isSatisfied(rstate)
                dist = goal.distanceGoal(rstate)
                if sat:
                    approxdif = dist
                    solution = len(self.states_)
                    break
                if dist < approxdif:
                    approxdif = dist
                    approxsol = len(self.states_)
        solved = False
        approximate = False
        if not solution:
            solution = approxsol
            approximate = True
        if solution:
            path = og.PathGeometric(si)
            for s in self.states_[:solution]:
                path.append(s)
            pdef.addSolutionPath(path)
            solved = True
        return ob.PlannerStatus(solved, approximate)
 
    def clear(self):
        super(RandomWalkPlanner, self).clear()
        self.states_ = []
 
 

 
 
def isStateValid(state):
    x = state[0]
    y = state[1]
    z = state[2]
    return x * x + y * y + z * z > 0.8
 
 
def plan():
    # create an R^3 state space
    space = ob.RealVectorStateSpace(3)
    # set lower and upper bounds
    bounds = ob.RealVectorBounds(3)
    bounds.setLow(-1)
    bounds.setHigh(1)
    space.setBounds(bounds)
    # create a simple setup object
    ss = og.SimpleSetup(space)
    ss.setStateValidityChecker(isStateValid)
    start = space.allocState()
    start[0] = -1.0
    start[1] = -1.0
    start[2] = -1.0
    goal = space.allocState()
    goal[0] = 1.0
    goal[1] = 1.0
    goal[2] = 1.0
    ss.setStartAndGoalStates(start, goal, 0.05)
    # set the planner
    planner = RandomWalkPlanner(ss.getSpaceInformation())
    ss.setPlanner(planner)
 
    result = ss.solve(10.0)
    if result:
        if result.getStatus() == ob.PlannerStatus.APPROXIMATE_SOLUTION:
            print("Solution is approximate")
        # try to shorten the path
        ss.simplifySolution()
        # print the simplified path
        print(ss.getSolutionPath())
 
 
if __name__ == "__main__":
    plan()