Planner Fails to plan any solution

[PavanMantripragada]

Hi,

I'm trying to run your demo_franka.py with minor modifications and the planner always fails to find a solution. Even for trivial cases. Is there something I can do?

This is the script I'm running

import os.path as osp
import pybullet as p
import math
import sys
import pybullet_data
sys.path.insert(0, '/home/pavan/Documents/Projects/pybullet_ompl')

import pb_ompl

class BoxDemo():
    def __init__(self):
        self.obstacles = []

        p.connect(p.GUI)
        p.setGravity(0, 0, -9.8)
        p.setTimeStep(1./240.)

        p.setAdditionalSearchPath(pybullet_data.getDataPath())
        p.loadURDF("plane.urdf")

        # load robot
        robot_id = p.loadURDF('franka_panda/panda.urdf', (0,0,0), useFixedBase = 1)
        robot = pb_ompl.PbOMPLRobot(robot_id)
        self.robot = robot

        # setup pb_ompl
        self.pb_ompl_interface = pb_ompl.PbOMPL(self.robot, self.obstacles)
        self.pb_ompl_interface.set_planner("BITstar")

        # add obstacles
        # self.add_obstacles()

    def clear_obstacles(self):
        for obstacle in self.obstacles:
            p.removeBody(obstacle)

    def add_obstacles(self):
        # add box
        self.add_box([1, 0, 0.7], [0.5, 0.5, 0.05])

        # store obstacles
        self.pb_ompl_interface.set_obstacles(self.obstacles)

    def add_box(self, box_pos, half_box_size):
        colBoxId = p.createCollisionShape(p.GEOM_BOX, halfExtents=half_box_size)
        box_id = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=colBoxId, basePosition=box_pos)

        self.obstacles.append(box_id)
        return box_id

    def demo(self):
        start = [0,0,0.6,0,0,0,0]
        goal = [0,0,0,0,0,0,0]

        self.robot.set_state(start)
        res, path = self.pb_ompl_interface.plan(goal)
        if res:
            self.pb_ompl_interface.execute(path)
        return res, path

if __name__ == '__main__':
    env = BoxDemo()
    env.demo()

This is the output I got

pybullet build time: May 20 2022 19:44:17
['/home/pavan/Documents/Projects/ompl/py-bindings', '/home/pavan/Documents/Projects/pybullet_ompl', '/home/pavan/Documents/Projects/deepSF/scripts', '/home/pavan/anaconda3/envs/deepSF/lib/python38.zip', '/home/pavan/anaconda3/envs/deepSF/lib/python3.8', '/home/pavan/anaconda3/envs/deepSF/lib/python3.8/lib-dynload', '/home/pavan/.local/lib/python3.8/site-packages', '/home/pavan/anaconda3/envs/deepSF/lib/python3.8/site-packages']
startThreads creating 1 threads.
starting thread 0
started thread 0 
argc=2
argv[0] = --unused
argv[1] = --start_demo_name=Physics Server
ExampleBrowserThreadFunc started
X11 functions dynamically loaded using dlopen/dlsym OK!
X11 functions dynamically loaded using dlopen/dlsym OK!
Creating context
Created GL 3.3 context
Direct GLX rendering context obtained
Making context current
GL_VENDOR=Intel
GL_RENDERER=Mesa Intel(R) UHD Graphics (TGL GT1)
GL_VERSION=4.6 (Core Profile) Mesa 21.2.6
GL_SHADING_LANGUAGE_VERSION=4.60
pthread_getconcurrency()=0
Version = 4.6 (Core Profile) Mesa 21.2.6
Vendor = Intel
Renderer = Mesa Intel(R) UHD Graphics (TGL GT1)
b3Printf: Selected demo: Physics Server
startThreads creating 1 threads.
starting thread 0
started thread 0 
MotionThreadFunc thread started
ven = Intel
Workaround for some crash in the Intel OpenGL driver on Linux/Ubuntu
ven = Intel
Workaround for some crash in the Intel OpenGL driver on Linux/Ubuntu
[0, 1, 2, 3, 4, 5, 6, 9, 10]
[]
Joint bounds: [[-2.9671, 2.9671], [-1.8326, 1.8326], [-2.9671, 2.9671], [-3.1416, 0.0], [-2.9671, 2.9671], [-0.0873, 3.8223], [-2.9671, 2.9671], [0.0, 0.04], [0.0, 0.04]]
start_planning
Warning: kBITstar: This option no longer has any effect; Rewiring is never delayed.
         at line 1250 in /home/pavan/Documents/Projects/ompl/src/ompl/geometric/planners/informedtrees/src/BITstar.cpp
Warning: kBITstar: This option no longer has any effect; The queue is always strictly ordered.
         at line 1211 in /home/pavan/Documents/Projects/ompl/src/ompl/geometric/planners/informedtrees/src/BITstar.cpp
delay_rewiring_to_first_solution = 0
drop_unconnected_samples_on_prune = 0
find_approximate_solutions = 0
prune_threshold_as_fractional_cost_change = 0.05
rewire_factor = 1.1
samples_per_batch = 100
stop_on_each_solution_improvement = 0
use_graph_pruning = 1
use_just_in_time_sampling = 0
use_k_nearest = 1
use_strict_queue_ordering = 1

Info:    kBITstar: No optimization objective specified. Defaulting to optimizing path length.
Warning: kBITstar: Skipping invalid goal state (invalid state)
         at line 311 in /home/pavan/Documents/Projects/ompl/src/ompl/base/src/Planner.cpp
Debug:   kBITstar: Discarded goal state:
RealVectorState [0 0 0 0 0 0 0 0 0]

Warning: kBITstar: Skipping invalid start state (invalid state)
         at line 248 in /home/pavan/Documents/Projects/ompl/src/ompl/base/src/Planner.cpp
Debug:   kBITstar: Discarded start state RealVectorState [0 0 0.6 0 0 0 0 0 0]

Info:    kBITstar: Searching for a solution to the given planning problem.
Debug:   kBITstar: Waiting for goal region samples ...
Warning: kBITstar: A solution cannot be found as no valid start states are available.
         at line 330 in /home/pavan/Documents/Projects/ompl/src/ompl/geometric/planners/informedtrees/src/BITstar.cpp
Warning: kBITstar: A solution cannot be found as no valid goal states are available.
         at line 340 in /home/pavan/Documents/Projects/ompl/src/ompl/geometric/planners/informedtrees/src/BITstar.cpp
Info:    kBITstar (0 iters): Did not find an exact solution from 0 samples after processing 0 edges (0 collision checked) to create 0 vertices and perform 0 rewirings. The final graph has 0 vertices.
Info:    No solution found after 5.052456 seconds
No solution found
numActiveThreads = 0
stopping threads
Thread with taskId 0 exiting
Thread TERMINATED
destroy semaphore
semaphore destroyed
destroy main semaphore
main semaphore destroyed
finished
numActiveThreads = 0
btShutDownExampleBrowser stopping threads
Thread with taskId 0 exiting
Thread TERMINATED
destroy semaphore
semaphore destroyed
destroy main semaphore
main semaphore destroyed

[gradb00st]

@PavanMantripragada I am facing the same problem. Did you find any working solution?

[wzfer]

@PavanMantripragada @gradb00st I checked the relevant source code and found that it might be a self-collision problem of the robot arm parts. In the initial urdf, the position or size of some joints might be inappropriate. Ompl will check the initial conditions before planning, such as whether there is self-collision, collision with the environment, etc. This depends on the detection conditions you define.