Collision detection is too close

[17729703508]

When using the rrt* mode with the 'shortcut' mode enabled, collisions may occur. This repository checks for methods related to collision volume expansion.

settings = {'type':'rrt*',
'perturbationRadius':0.025,
'bidirectional':True,
'shortcut':True,
'restart':False,
'restartTermCond':"{foundSolution:1,maxIters:10000}"
}

#this is the CSpace that will be used. Standard collision and joint limit constraints
#will be checked
space = robotplanning.makeSpace(world,robot,edgeCheckResolution=0.05)

#fire up a visual editor to get some start and goal configurations
qstart = robot.getConfig()
qgoal = robot.getConfig()
save,qstart = resource.edit("Start config",qstart,"Config",world=world)
#it's worthwile to make sure that it's feasible
while save and not space.feasible(qstart):
print("Start configuration isn't feasible, please pick one that is collision-free")
save,qstart = resource.edit("Start config",qstart,"Config",world=world)
print(qstart )
save,qgoal = resource.edit("Goal config",qgoal,"Config",world=world)
while save and not space.feasible(qgoal):
print("Goal configuration isn't feasible, please pick one that is collision-free")
save,qgoal = resource.edit("Goal config",qgoal,"Config",world=world)
print(qgoal)

settings = {'type':'rrt*',
'perturbationRadius':0.025,
'bidirectional':True,
'shortcut':True,
'restart':False,
'restartTermCond':"{foundSolution:1,maxIters:10000}"
}

t0 = time.time()
print("Creating planner...")
#Manual construction of planner
planner = cspace.MotionPlan(space, **settings)
planner.setEndpoints(qstart,qgoal)
print("Planner creation time",time.time()-t0)
t0 = time.time()
print("Planning...")
numIters = 0
for round in range(10):
planner.planMore(500)
numIters += 1
if planner.getPath() is not None:
break
print("Planning time,",numIters,"iterations",time.time()-t0)

path = planner.getPath()
if path is not None:
print("Got a path with",len(path),"milestones")
else:
print("No feasible path was found")