def feasible_plan(world, robot, qtarget):
"""Plans for some number of iterations from the robot's current configuration to
configuration qtarget. Returns the first path found.
Returns None if no path was found, otherwise returns the plan.
"""
t0 = time.time()
moving_joints = [1, 2, 3, 4, 5, 6, 7]
# space = robotplanning.makeSpace(world=world,robot=robot,edgeCheckResolution=1e-2,movingSubset=moving_joints)
plan = robotplanning.planToConfig(world,
robot,
qtarget,
movingSubset=moving_joints)
#TODO: maybe you should use planToConfig?
numIters = 100
path = plan.getPath()
t1 = time.time()
while path == None and t1 - t0 <= 10:
plan.planMore(numIters)
t1 = time.time()
path = plan.getPath()
print("Planning time,", numIters, "iterations", t1 - t0)
#to be nice to the C++ module, do this to free up memory
plan.space.close()
plan.close()
return path
def optimizing_plan(world, robot, qtarget):
"""Plans for some number of iterations from the robot's current configuration to
configuration qtarget.
Returns None if no path was found, otherwise returns the best plan found.
"""
#TODO: copy what's in feasible_plan, but change the way in which you to terminate
t0 = time.time()
moving_joints = [1, 2, 3, 4, 5, 6, 7]
# space = robotplanning.makeSpace(world=world,robot=robot,edgeCheckResolution=1e-2,movingSubset=moving_joints)
plan = robotplanning.planToConfig(world,
robot,
qtarget,
movingSubset=moving_joints,
type='sbl',
perturbationRadius=0.5)
if plan is None:
return None
#TODO: maybe you should use planToConfig?
numIters = 100
t1 = time.time()
while t1 - t0 <= 15:
plan.planMore(numIters)
t1 = time.time()
path = plan.getPath()
print("Planning time,", numIters, "iterations", t1 - t0)
# debug_plan_results(plan, robot)
#to be nice to the C++ module, do this to free up memory
plan.space.close()
plan.close()
return path
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example prints the config when [space] is pressed
if c == ' ':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
print("Config:", subconfig)
self.subrobots[0].setConfig(subconfig)
elif c == 'r':
self.subrobots[0].randomizeConfig()
self.robotWidget.set(self.robot.getConfig())
elif c == 'p':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
self.subrobots[0].setConfig(self.subrobots[0].fromfull(
self.startConfig))
settings = {
'type': "sbl",
'perturbationRadius': 0.5,
'bidirectional': 1,
'shortcut': 1,
'restart': 1,
'restartTermCond': "{foundSolution:1,maxIters:1000}"
}
plan = robotplanning.planToConfig(self.world,
self.subrobots[0],
subconfig,
movingSubset='all',
**settings)
plan.planMore(1000)
print(plan.getPath())
else:
GLWidgetPlugin.keyboardfunc(self, c, x, y)
def baxter_planner(qi, q, qSubset, settings):
global world
global robot
t0 = time.time()
print "Creating plan..."
#this code uses the robotplanning module's convenience functions
robot.setConfig(qi)
print 'len(q)', len(q)
plan = robotplanning.planToConfig(world,
robot,
q,
movingSubset=qSubset,
**settings)
if plan is None:
print 'plan is None...'
return None
print "Planner creation time", time.time() - t0
t0 = time.time()
plan.space.cspace.enableAdaptiveQueries(True)
print "Planning..."
for round in range(10):
plan.planMore(50)
print "Planning time, 500 iterations", time.time() - t0
#this code just gives some debugging information. it may get expensive
#V,E = plan.getRoadmap()
#print len(V),"feasible milestones sampled,",len(E),"edges connected"
path = plan.getPath()
if path is None or len(path) == 0:
print "Failed to plan path between configuration"
print qi
print "and"
print q
# #debug some sampled configurations
# print V[0:min(10,len(V))]
"""
print "Constraint testing order:"
print plan.space.cspace.feasibilityQueryOrder()
print "Manually optimizing constraint testing order..."
plan.space.cspace.optimizeQueryOrder()
print "Optimized constraint testing order:"
print plan.space.cspace.feasibilityQueryOrder()
print "Plan stats:"
print plan.getStats()
print "CSpace stats:"
print plan.space.getStats()
"""
#to be nice to the C++ module, do this to free up memory
plan.space.close()
plan.close()
return path
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example prints the config when [space] is pressed
if c == 'h' or c == '?':
print("Controls:")
print("- [space]: print the widget's sub-robot configuration")
print("- r: randomize the sub-robot configuration")
print("- p: plan to the widget's sub-robot configuration")
print("- i: test the IK functions")
elif c == ' ':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
print("Config:", subconfig)
self.subrobots[0].setConfig(subconfig)
elif c == 'r':
self.subrobots[0].randomizeConfig()
self.robotWidget.set(self.robot.getConfig())
elif c == 'p':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
self.subrobots[0].setConfig(self.subrobots[0].fromfull(
self.startConfig))
settings = {
'type': "sbl",
'perturbationRadius': 0.5,
'bidirectional': 1,
'shortcut': 1,
'restart': 1,
'restartTermCond': "{foundSolution:1,maxIters:1000}"
}
plan = robotplanning.planToConfig(self.world,
self.subrobots[0],
subconfig,
movingSubset='all',
**settings)
plan.planMore(1000)
print(plan.getPath())
elif c == 'i':
link = self.subrobots[0].link(self.subrobots[0].numLinks() - 1)
print("IK solve for ee to go 10cm upward...")
obj = ik.objective(link,
local=[0, 0, 0],
world=vectorops.add(
link.getWorldPosition([0, 0, 0]),
[0, 0, 0.1]))
solver = ik.solver(obj)
res = solver.solve()
print(" result", res)
print(" residual", solver.getResidual())
print(self.robotWidget.set(self.robot.getConfig()))
else:
GLWidgetPlugin.keyboardfunc(self, c, x, y)
def feasible_plan(world, robot, object, qtarget):
"""Plans for some number of iterations from the robot's current configuration to
configuration qtarget. Returns the first path found.
Returns None if no path was found, otherwise returns the plan.
"""
t0 = time.time()
moving_joints = [1, 2, 3, 4, 5, 6, 7]
kwargs = {'type':'sbl','perturbationRadius':0.5, 'bidirectional': 1, 'shortcut':True,'restart':True,
'restartTermCond':"{foundSolution:1,maxIters:100}"}
def check_collision(q):
robot.setConfig(q)
return is_collision_free_grasp(world, robot, object)
planner = robotplanning.planToConfig(world=world, robot=robot, target=qtarget, edgeCheckResolution=0.1,
movingSubset=moving_joints, extraConstraints=[check_collision], **kwargs)
# qtarget is not feasible
if planner is None:
return None
path = None
numIters = 0
increment = 100 # there is a little overhead for each planMore call, so it is best not to set the increment too low
t0 = time.time()
while time.time() - t0 < 100: # max 20 seconds of planning
planner.planMore(increment)
numIters = numIters + 1
path = planner.getPath()
if path is not None:
if len(path) > 0:
print("Solved, path has", len(path), "milestones")
break
t1 = time.time()
print("Planning time,", numIters, "iterations", t1 - t0)
# to be nice to the C++ module, do this to free up memory
planner.space.close()
planner.close()
return path
def planOptimizedTrajectory(world,
robot,
target,
maxTime=float('inf'),
numRestarts=1,
plannerSettings={
'type': 'sbl',
'shortcut': True
},
plannerMaxIters=3000,
plannerMaxTime=10.0,
plannerContinueOnRestart=False,
optimizationMaxIters=200,
kinematicsCache=None,
settings=None,
logFile=None):
"""
Args:
world (WorldModel): contains all objects
robot (RobotModel): the moving robot
target: a target configuration
maxTime (float, optional): a total max time for all planning / optimization calls
numRestarts (int): number of restarts of the randomized planner
plannerSettings (dict): a Klamp't planner settings dictionary
plannerMaxIters (int): max number of iterations to perform each planner call
plannerMaxTime (float): max time for each planner call
plannerContinueOnRestart (bool): if true, the planner doesn't get reset every restart
optimizationMaxIters (int): max number of total optimization iterations
kinematicsCache (RobotKinematicsCache, optional): modifies the kinematics cache.
settings (SemiInfiniteOptimizationSettings, optional): modifies the optimizer setting
logFile (file, optional): if given, this is a CSV file that logs the output
Returns a RobotTrajectory that (with more iterations) optimizes path length
"""
bestPath = None
bestPathCost = float('inf')
best_instantiated_params = None
t0 = time.time()
if kinematicsCache is None:
kinematicsCache = geometryopt.RobotKinematicsCache(robot)
if settings is None:
settings = geometryopt.SemiInfiniteOptimizationSettings()
settings.minimum_constraint_value = -0.02
optimizing = False
if 'optimizing' in plannerSettings:
optimizing = plannerSettings['optimizing']
plannerSettings = plannerSettings.copy()
del plannerSettings['optimizing']
obstacles = [
world.rigidObject(i) for i in xrange(world.numRigidObjects())
] + [world.terrain(i) for i in xrange(world.numTerrains())]
obstaclegeoms = [
geometryopt.PenetrationDepthGeometry(obs.geometry(), None, 0.04)
for obs in obstacles
]
t1 = time.time()
print("Preprocessing took time", t1 - t0)
if logFile:
logFile.write(
"numRestarts,plannerSettings,plannerMaxIters,plannerMaxTime,optimizationMaxIters\n"
)
logFile.write("%d,%s,%d,%g,%d\n" %
(numRestarts, str(plannerSettings), plannerMaxIters,
plannerMaxTime, optimizationMaxIters))
logFile.write("restart,iterations,time,method,path length\n")
tstart = time.time()
startConfig = robot.getConfig()
for restart in xrange(numRestarts):
print(
"***************************************************************")
#optimize existing best path, if available
if bestPath is not None and optimizationMaxIters > 0:
t0 = time.time()
#optimize the best path
settings.max_iters = optimizationMaxIters // (numRestarts * 2)
settings.instantiated_params = best_instantiated_params
print("Optimizing current best path with", settings.max_iters,
"iters over", len(bestPath.milestones), "milestones")
if logFile:
logFile.write("%d,0,%g,optimize-best,%g\n" %
(restart, t0 - tstart, bestPathCost))
trajCache = geometryopt.RobotTrajectoryCache(
kinematicsCache,
len(bestPath.milestones) - 2,
qstart=startConfig,
qend=target)
trajCache.tstart = 0
trajCache.tend = bestPath.times[-1]
obj = geometryopt.TrajectoryLengthObjective(trajCache)
constraint = geometryopt.RobotTrajectoryCollisionConstraint(
obstaclegeoms, trajCache)
#constraint = geometryopt.MultiSemiInfiniteConstraint([geometryopt.RobotLinkTrajectoryCollisionConstraint(robot.link(i),obstaclegeoms[0],trajCache) for i in range(robot.numLinks())])
traj, traj_trace, traj_times, constraintPts = geometryopt.optimizeCollFreeTrajectory(
trajCache,
bestPath,
obstacles,
constraints=[constraint],
settings=settings)
if logFile:
#write the whole trace
for i, (traji, ti) in enumerate(zip(traj_trace, traj_times)):
if i == 0: continue
logFile.write("%d,%d,%g,optimize-best,%g\n" %
(restart, i, t0 + ti - tstart,
min(bestPathCost, traji.length())))
xtraj = trajCache.trajectoryToState(traj)
#cost = obj.value(xtraj)
cost = traj.length()
constraint.setx(xtraj)
residual = constraint.minvalue(xtraj)
constraint.clearx()
print("Optimized cost", cost, "and residual", residual)
if cost < bestPathCost and residual[0] >= 0:
print("Got a better cost path by optimizing current best",
cost, "<", bestPathCost)
bestPathCost = cost
bestPath = traj
best_instantiated_params = constraintPts
t1 = time.time()
if t1 - tstart > maxTime:
break
#do sampling-based planningif bestPath is not None:
if restart == 0 or (not plannerContinueOnRestart
and planner.getPath()):
robot.setConfig(startConfig)
planner = robotplanning.planToConfig(world, robot, target,
**plannerSettings)
t0 = time.time()
if logFile:
logFile.write("%d,0,%g,sample,%g\n" %
(restart, t0 - tstart, bestPathCost))
path = None
oldpath = None
for it in xrange(0, plannerMaxIters, 50):
oldpath = path
planner.planMore(50)
t1 = time.time()
path = planner.getPath()
if path:
if oldpath is None:
print("Found a feasible path on restart", restart,
"iteration", it)
path = RobotTrajectory(robot, range(len(path)), path)
cost = path.length()
if cost < bestPathCost:
print("Got a better cost path from planner", cost, "<",
bestPathCost)
bestPathCost = cost
bestPath = path
if logFile:
if oldpath is not None:
logFile.write(
"%d,%d,%g,sample-optimize,%g\n" %
(restart, it + 50, t1 - tstart, bestPathCost))
else:
logFile.write(
"%d,%d,%g,sample,%g\n" %
(restart, it + 50, t1 - tstart, bestPathCost))
if not optimizing:
break
if t1 - t0 > plannerMaxTime:
break
if t1 - tstart > maxTime:
break
if logFile:
if oldpath is not None:
logFile.write("%d,%d,%g,sample-optimize,%g\n" %
(restart, it + 50, t1 - tstart, bestPathCost))
else:
logFile.write("%d,%d,%g,sample,%g\n" %
(restart, it + 50, t1 - tstart, bestPathCost))
if t1 - tstart > maxTime:
break
#optimize new planned path, if available
if path and len(path.milestones) == 2:
#straight line path works, this is certainly optimal
return path
elif path and len(path.milestones) > 2:
#found a feasible path
t0 = time.time()
if len(path.milestones) < 30:
traj0 = arc_length_refine(
path, min(30 - len(path.milestones), len(path.milestones)))
else:
traj0 = path
traj0.times = [
float(i) / (len(traj0.milestones) - 1)
for i in xrange(len(traj0.times))
]
trajCache = geometryopt.RobotTrajectoryCache(
kinematicsCache,
len(traj0.milestones) - 2,
qstart=startConfig,
qend=target)
trajCache.tstart = 0
trajCache.tend = traj0.times[-1]
print()
print("Planned trajectory with", len(traj0.milestones),
"milestones")
obj = geometryopt.TrajectoryLengthObjective(trajCache)
constraint = geometryopt.RobotTrajectoryCollisionConstraint(
obstaclegeoms, trajCache)
xtraj = trajCache.trajectoryToState(traj0)
#cost0 = obj.value(xtraj)
cost0 = traj0.length()
constraint.setx(xtraj)
residual0 = constraint.minvalue(xtraj)
constraint.clearx()
print("Planned cost", cost0, "and residual", residual0)
if bestPath:
settings.max_iters = optimizationMaxIters // (numRestarts * 2)
else:
settings.max_iters = optimizationMaxIters // (numRestarts)
settings.instantiated_params = None
if residual0[0] < 0:
print(
"Warning, the planned path has a negative constraint residual?",
residual0[0])
print(" Residual met @ parameter", residual0[1:])
#raw_input("Warning, the planned path has a negative constraint residual?")
if optimizationMaxIters > 0:
if logFile:
logFile.write("%d,0,%g,optimize,%g\n" %
(restart, t0 - tstart, bestPathCost))
print("Optimizing planned path with", settings.max_iters,
"iters")
traj, traj_trace, traj_times, constraintPts = geometryopt.optimizeCollFreeTrajectory(
trajCache,
traj0,
obstacles,
constraints=[constraint],
settings=settings)
if logFile:
#write the whole trace
for i, (traji,
ti) in enumerate(zip(traj_trace, traj_times)):
if i == 0: continue
xtraj = trajCache.trajectoryToState(traji)
constraint.setx(xtraj)
residual = constraint.minvalue(xtraj)
constraint.clearx()
if residual[0] >= 0:
logFile.write("%d,%d,%g,optimize,%g\n" %
(restart, i, t0 + ti - tstart,
min(bestPathCost, traji.length())))
xtraj = trajCache.trajectoryToState(traj)
#cost = obj.value(xtraj)
cost = traj.length()
constraint.setx(xtraj)
residual = constraint.minvalue(xtraj)
constraint.clearx()
print("Optimized cost", cost, "and residual", residual)
if cost < bestPathCost and residual[0] >= 0:
print("Got a better cost path from optimizer", cost, "<",
bestPathCost)
bestPathCost = cost
bestPath = traj
best_instantiated_params = constraintPts
else:
print("Optimizer produced a worse cost (", cost, "vs",
bestPathCost, ") or negative residual, skipping")
if residual[0] < 0:
#raw_input("Warning, the optimized path has a negative constraint residual...")
print(
"Warning, the optimized path has a negative constraint residual..."
)
t1 = time.time()
#if logFile:
# logFile.write("%d,%d,%g,optimize,%g\n"%(restart,settings.max_iters,t1-tstart,bestPathCost))
if t1 - tstart > maxTime:
break
else:
print("No feasible path was found by the planner on restart",
restart)
return bestPath
robot.setConfig(configs[i + 1])
obj = ik.fixed_objective(robot.link(robot.numLinks() - 1),
local=[0, 0, 0])
#start from end of previous path
robot.setConfig(wholepath[-1])
plan = robotplanning.planToCartesianObjective(world,
robot,
obj,
movingSubset='all',
**settings)
else:
#this code uses the robotplanning module's convenience functions
robot.setConfig(configs[i])
plan = robotplanning.planToConfig(world,
robot,
configs[i + 1],
movingSubset='all',
**settings)
if plan is None:
break
print("Planner creation time", time.time() - t0)
t0 = time.time()
plan.space.cspace.enableAdaptiveQueries(True)
print("Planning...")
for round in range(10):
plan.planMore(50)
print("Planning time, 500 iterations", time.time() - t0)
#this code just gives some debugging information. it may get expensive
V, E = plan.getRoadmap()
print(len(V), "feasible milestones sampled,", len(E), "edges connected")
path = plan.getPath()
def transfer_plan(world, robot, qtarget, object, Tobject_gripper):
"""Plans for some number of iterations from the robot's current configuration to
configuration qtarget, assuming the object is fixed to the gripper.
Returns the first path found.
Returns None if no path was found, otherwise returns the plan.
"""
moving_joints = [1, 2, 3, 4, 5, 6, 7]
gripper_link = 9
# create a new copy of the world
world1 = world.copy()
# remove the hammer object from the world
world1.remove(world1.rigidObject(object.index))
# find the robot in world1
robot1 = world1.robot(0)
robot1.setConfig(robot.getConfig())
# find the gripper link in new world????
gripper_link1 = robot1.link(gripper_link)
#kwargs = {'type':'rrt*', 'bidirectional':1}
kwargs = {
'type': 'sbl',
'perturbationRadius': 0.5,
'bidirectional': 1,
'shortcut': True,
'restart': True,
'restartTermCond': "{foundSolution:1,maxIters:100}"
}
t0 = time.time()
# collision check function
finger_pad_links = ['gripper:Link_4', 'gripper:Link_6']
def check_collision(q):
# set robot to current config
robot1.setConfig(q)
finger_pad_links_idx = []
for i in finger_pad_links:
idx = robot1.link(i).getIndex()
finger_pad_links_idx.append(idx)
Tworld_gripper = gripper_link1.getTransform()
Tworld_object = se3.mul(Tworld_gripper, Tobject_gripper)
object.setTransform(Tworld_object[0], Tworld_object[1])
# self collide
if robot1.selfCollides():
print('self collides')
return False
# collision between object and robot
for i in range(robot1.numLinks()):
if i in finger_pad_links_idx:
continue
if robot1.link(i).geometry().collides(object.geometry()):
print('hammer and robot', i)
return False
# collision between robot and world environment
for i in range(world1.numTerrains()):
for j in range(robot1.numLinks()):
if robot1.link(j).geometry().collides(
world1.terrain(i).geometry()):
print('robot and world')
return False
if object.geometry().collides(world1.terrain(i).geometry()):
print('hammer and world', i, world1.terrain(i).getName())
return False
# collision between robot and objects in world
for i in range(world1.numRigidObjects()):
for j in range(robot1.numLinks()):
if world1.rigidObject(
i) == object and j in finger_pad_links_idx:
continue
if robot1.link(j).geometry().collides(
world1.rigidObject(i).geometry()):
print('robot and other objects')
return False
if object.geometry().collides(world1.rigidObject(i).geometry()):
print('hammer and other objects')
return False
return True
planner = robotplanning.planToConfig(world=world1,
robot=robot1,
target=qtarget,
edgeCheckResolution=0.1,
movingSubset=moving_joints,
extraConstraints=[check_collision],
**kwargs)
if planner is None:
return None
# plan for 10 seconds
path = None
increment = 100 # there is a little overhead for each planMore call, so it is best not to set the increment too low
t0 = time.time()
while time.time() - t0 < 10: # max 10 seconds of planning
planner.planMore(increment)
path = planner.getPath()
if path is not None:
if len(path) > 0:
print("Solved, path has", len(path), "milestones")
break
t1 = time.time()
print("Planning time,", t1 - t0)
# to be nice to the C++ module, do this to free up memory
planner.space.close()
planner.close()
print('path:', path)
return path
def optimizing_plan(world,robot,qtarget):
"""Plans for some number of iterations from the robot's current configuration to
configuration qtarget.
Returns None if no path was found, otherwise returns the best plan found.
"""
t0 = time.time()
moving_joints = [1, 2, 3, 4, 5, 6, 7]
arg_list = [{'type':'sbl','shortcut':True,'perturbationRadius':0.5},
{'type':'sbl','perturbationRadius':0.5,'shortcut':True,'restart':True,'restartTermCond':"{foundSolution:1,maxIters:100}"},
{'type':'rrt*'},
{'type':'lazyrrg*'}]
planner_names = ['sbl1', 'sbl2', 'rrt*', 'lazyrrg']
success = [0, 0, 0, 0]
path_len = [[], [], [], []]
best_path = None
best_path_len = np.inf
qinit = robot.getConfig()
for i, kwarg in enumerate(arg_list):
robot.setConfig(qinit)
print('')
print(planner_names[i])
planner = robotplanning.planToConfig(world=world, robot=robot, target=qtarget, edgeCheckResolution=0.01,
movingSubset=moving_joints, **kwarg)
if planner is None:
return None
# there are 18 dimensions in config space, but we only allow the first 7 joints to move
path = None
numIters = 0
increment = 100 # there is a little overhead for each planMore call, so it is best not to set the increment too low
for j in range(10):
t0 = time.time()
while time.time() - t0 < 10: # max 20 seconds of planning
planner.planMore(increment)
numIters = numIters + 1
path = planner.getPath()
if path is not None:
if len(path) > 0:
print("Solved, path has", len(path), "milestones")
success[i] = success[i] + 1
path_len[i].append(len(path))
# update best_path if a better path is found
if len(path) < best_path_len:
best_path = copy.deepcopy(path)
best_path_len = len(path)
break
# to be nice to the C++ module, do this to free up memory
planner.space.close()
planner.close()
print('success rate:', success[i]/10.)
print('optimal path len:', min(path_len[i]), 'mean:', np.mean(path_len[i]), 'std:', np.std(path_len[i]))
return best_path