Re: [ompl-users] GoalLazySamples using Python Bindings

[Emre C. G. (S. <emr...@sa...>]

Hello,

How about goal sampling on a separate thread using threading module of
Python, where sampled goal states are added to an ob.GoalStates class? I
think it is working. Can you check the following demo that I modified from
RigidBodyPlannning?

*My Code:*

import threading
import time
import numpy as np
from ompl import base as ob
from ompl import geometric as og


def isStateValid(state):
    if 0.2 <= state[0] <= 0.8 and 0.2 <= state[1] <= 0.8:
        return False
    else:
        return True


def sampleFunc():
    t = 1000 * time.time()
    np.random.seed(int(t) % 2 ** 32)
    random_state = np.random.uniform(low=np.array([0.75, 0.75]),
high=np.array([0.95, 0.95]), size=(2,))
    if isStateValid(random_state):
        return random_state
    else:
        return None


def add_goal(stop, ss):
    while True:
        sgoal = ob.State(ss)
        random_state = sampleFunc()
        if random_state is not None:
            for i in range(2):
                sgoal[i] = random_state[i]
            goal.addState(sgoal)

        if stop():
            break


if __name__ == '__main__':
    # create state space and add bounds
    space = ob.RealVectorStateSpace(2)
    bounds = ob.RealVectorBounds(2)
    bounds.setLow(0.)
    bounds.setHigh(1.)
    space.setBounds(bounds)

    # set state validity checker
    ss = og.SimpleSetup(space)
    ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))

    # define start state
    sstart = ob.State(space)
    for i in range(2):
        sstart[i] = 0.

    # define goal as global variable
    global goal

    # goal as GoalStates
    goal = ob.GoalStates(ss.getSpaceInformation())

    # Initially no goal states in goal
    print("Initial Number of Goal States: ", goal.getStateCount())

    # define goal thread and start
    stop_thread = False
    goal_thread = threading.Thread(target=add_goal, args=(lambda:
stop_thread, space,))
    goal_thread.start()

    # define and set an optimum planner to see if it finds different goals
    planner = og.RRTstar(ss.getSpaceInformation())
    planner.setProblemDefinition(ss.getProblemDefinition())
    ss.setPlanner(planner)

    # set start state and the goal
    ss.setStartState(sstart)
    ss.setGoal(goal)

    # check how many goal states are found before planning
    print("Number of Goal States: ", goal.getStateCount())

    # start planning
    solved = ss.solve(10.0)

    if solved:
        # stop thread
        stop_thread = True
        goal_thread.join()

        # total number of states
        print("Final Number of Goal States: ", goal.getStateCount())

        # print the simplified path
        print(ss.getSolutionPath())

*The output:*

Initial Number of Goal States:  0
Number of Goal States:  68
Debug:   RRTstar: Planner range detected to be 0.282843
Info:    RRTstar: No optimization objective specified. Defaulting to
optimizing path length for the allowed planning time.
Info:    RRTstar: Started planning with 1 states. Seeking a solution better
than 0.00000.
Info:    RRTstar: Initial k-nearest value of 25
Info:    RRTstar: Found an initial solution with a cost of 1.61 in 66
iterations (24 vertices in the graph)
Info:    RRTstar: Created 708 new states. Checked 130827 rewire options. 52
goal states in tree. Final solution cost 1.460
Info:    Solution found in 10.038575 seconds
Final Number of Goal States:  93746
Geometric path with 8 states
RealVectorState [0 0]
RealVectorState [0.188465 0.0446338]
RealVectorState [0.463898 0.105294]
RealVectorState [0.578831 0.131403]
RealVectorState [0.808084 0.189118]
RealVectorState [0.866363 0.437726]
RealVectorState [0.896772 0.648098]
RealVectorState [0.921206 0.808713]



On Mon, Aug 30, 2021 at 6:55 PM Mark Moll <mm...@ri...> wrote:

> Hi Emre,
>
> This may be difficult to do. GoalLazySamples spawns a separate thread to
> generate valid goal samples. If this thread also calls Python code (e.g.,
> if your StateValidityChecker is written in Python), then you run into
> trouble with Python’s Global Interpreter Lock (GIL). This is a limitation
> of the current bindings.
>
> Best,
>
> Mark
>
>
>
> On Aug 29, 2021, at 11:56 AM, Emre Cemal Gönen (Student) <
> emr...@sa...> wrote:
>
> <image.gif>Hello,
>
> Can you provide an example of how to define GoalLazySamples using Python
> Bindings?
>
> I define it in the following way, but I get an error of
>
> terminate called after throwing an instance of
> 'boost::python::error_already_set'
> Aborted (core dumped)
>
>
> My GoalLazySamples definition:
>
> class GoalLRegion(ob.GoalLazySamples):
>     def __init__(self, si, samplerFunc, goalRegionParam):
>         super(GoalLRegion, self).__init__(si, samplerFunc)
>         self.mySampler = ob.ValidStateSampler(si)
>         self.goalRegionParam = goalRegionParam
>
>     def distanceGoal(self, state):
>         d = np.sqrt((state[0] - self.goalRegionParam[0]) ** 2 + (state[0] - self.goalRegionParam[1]) ** 2) - \
>             self.goalRegionParam[2]
>         return d
>
>     def sampleGoal(self, state):
>         self.mySampler.sample(state)
>
>     def maxSampleCount(self):
>         return 100
>
>     def mySamplingFunction(self, gls, state):
>         return True
>
>
> goal = GoalLRegion(ss.getSpaceInformation(), ob.GoalSamplingFn(GoalLRegion.mySamplingFunction), goalRegionParam)
>
>
> --
> *EMRE CEMAL GÖNEN*
> YÜKSEK LİSANS ÖĞRENCİSİ
> GRADUATE STUDENT
> ------------------------------
> Sabancı Üniversitesi
> Üniversite Caddesi No:27
> 34956 Orta Mahalle
> Tuzla - İstanbul
> *T*   0 216 483 90 00 - 2331
> *www.sabanciuniv.edu* <https://www.sabanciuniv.edu/>
> <http://www.sabanciuniv.edu/>
>
> <image.gif>
>
> _______________________________________________
> ompl-users mailing list
> omp...@li...
> https://lists.sourceforge.net/lists/listinfo/ompl-users
>
>
>

-- 
*EMRE CEMAL GÖNEN*
YÜKSEK LİSANS ÖĞRENCİSİ
GRADUATE STUDENT
------------------------------
Sabancı Üniversitesi
Üniversite Caddesi No:27
34956 Orta Mahalle
Tuzla - İstanbul
*T*   0 216 483 90 00 - 2331
*www.sabanciuniv.edu* <https://www.sabanciuniv.edu/>
<http://www.sabanciuniv.edu>