def planTransit(world, objectIndex, hand): globals = Globals(world) cspace = TransitCSpace(globals, hand) obj = world.rigidObject(objectIndex) robot = world.robot(0) qmin, qmax = robot.getJointLimits() #get the start config q0 = robot.getConfig() q0arm = [q0[i] for i in hand.armIndices] if not cspace.feasible(q0arm): print "Warning, arm start configuration is infeasible" #get the pregrasp config -- TODO: what if the ik solver doesn't work? qpregrasp = None qpregrasparm = None solver = hand.ikSolver(robot, obj.getTransform()[1], [0, 0, 1]) print "Trying to find pregrasp config..." solver.setMaxIters(100) solver.setTolerance(1e-3) res = solver.solve() if res: qpregrasp = robot.getConfig() qpregrasparm = [qpregrasp[i] for i in hand.armIndices] if not cspace.feasible(qpregrasparm): print "Pregrasp config infeasible" cspace.close() return None if qpregrasp == None: print "Pregrasp solve failed" cspace.close() return None print "Planning transit motion to pregrasp config..." MotionPlan.setOptions(connectionThreshold=5.0, perturbationRadius=0.5) planner = MotionPlan(cspace, 'sbl') planner.setEndpoints(q0arm, qpregrasparm) iters = 0 step = 10 while planner.getPath() == None and iters < 1000: planner.planMore(step) iters += step cspace.close() if planner.getPath() == None: print "Failed finding transit path" return None print "Success, found path with", len(planner.getPath()), "milestones" #lift arm path to whole configuration space path path = [] for qarm in planner.getPath(): path.append(q0[:]) for qi, i in zip(qarm, hand.armIndices): path[-1][i] = qi #add a path to the grasp configuration return path + [hand.open(path[-1], 0)]
def planTransfer(world, objectIndex, hand, shift): """Plan a transfer path for the robot given in world, which is currently holding the object indexed by objectIndex in the hand hand. The desired motion should translate the object by shift without rotating the object. """ globals = Globals(world) obj = world.rigidObject(objectIndex) cspace = TransferCSpace(globals, hand, obj) robot = world.robot(0) qmin, qmax = robot.getJointLimits() #get the start config q0 = robot.getConfig() q0arm = [q0[i] for i in hand.armIndices] if not cspace.feasible(q0arm): print "Warning, arm start configuration is infeasible" print "TODO: Complete 2.a to bypass this error" raw_input() cspace.close() return None #TODO: get the ungrasp config using an IK solver qungrasp = None qungrasparm = None print "TODO: Complete 2.b to find a feasible ungrasp config" raw_input() solver = hand.ikSolver(robot, vectorops.add(obj.getTransform()[1], shift), (0, 0, 1)) #plan the transfer path between q0arm and qungrasparm print "Planning transfer motion to ungrasp config..." MotionPlan.setOptions(connectionThreshold=5.0, perturbationRadius=0.5) planner = MotionPlan(cspace, 'sbl') planner.setEndpoints(q0arm, qungrasparm) #TODO: do the planning print "TODO: Complete 2.c to find a feasible transfer path" raw_input() cspace.close() #lift arm path to whole configuration space path path = [] for qarm in planner.getPath(): path.append(q0[:]) for qi, i in zip(qarm, hand.armIndices): path[-1][i] = qi qpostungrasp = hand.open(qungrasp, 1.0) return path + [qpostungrasp]
def init(): space, s, g = spawnFunc() if PLANNER_TYPE == 'prm_neighborhood': plan = MotionPlan(space, type='prm', connectionThreshold=0.1, ignoreConnectedComponents=True) elif PLANNER_TYPE == 'prm_knn': plan = MotionPlan(space, type='prm', knn=5, ignoreConnectedComponents=True) elif PLANNER_TYPE == 'prmstar': plan = MotionPlan(space, type='prm*') elif PLANNER_TYPE == 'rrtstar': plan = MotionPlan(space, type='rrt*', bidirectional=False) else: raise ValueError("Invalid PLANNER_TYPE") plan.setEndpoints(s, g)
def planFree(world, hand, qtarget): """Plans a free-space motion for the robot's arm from the current configuration to the destination qtarget""" globals = Globals(world) cspace = TransitCSpace(globals, hand) robot = world.robot(0) qmin, qmax = robot.getJointLimits() #get the start/goal config q0 = robot.getConfig() q0arm = [q0[i] for i in hand.armIndices] qtargetarm = [qtarget[i] for i in hand.armIndices] if not cspace.feasible(q0arm): print "Warning, arm start configuration is infeasible" if not cspace.feasible(qtargetarm): print "Warning, arm goal configuration is infeasible" print "Planning transit motion to target config..." MotionPlan.setOptions(connectionThreshold=5.0, perturbationRadius=0.5) planner = MotionPlan(cspace, 'sbl') planner.setEndpoints(q0arm, qtargetarm) iters = 0 step = 10 while planner.getPath() == None and iters < 1000: planner.planMore(step) iters += step cspace.close() if planner.getPath() == None: print "Failed finding transit path" return None print "Success" #lift arm path to whole configuration space path path = [] for qarm in planner.getPath(): path.append(q0[:]) for qi, i in zip(qarm, hand.armIndices): path[-1][i] = qi return path
def testPlannerSuccessRate(N=100, duration=10, spawnFunc=lambda: kinkTest(0.0025, False)): import time import matplotlib.pyplot as plt finished = [] for run in range(N): space, s, g = spawnFunc() space.eps = 1e-3 if run == 0 and False: #show space space.drawObstaclesMPL(plt.axes()) plt.scatter([s[0], g[0]], [s[1], g[1]]) plt.show() plan = MotionPlan(space, type='prm', knn=5) plan.setEndpoints(s, g) t0 = time.time() finished.append(None) while time.time() - t0 < duration: plan.planMore(5) if plan.getPath(): finished[-1] = time.time() - t0 print("Found path with", len(plan.getPath()), "milestones in", time.time() - t0, "s") break if finished[-1] is None: print("Failed to find path in", duration, "s") import numpy as np finished = [v for v in finished if v != None] hist, edges = np.histogram(finished, 20, (0, duration)) print(hist, edges) hist = hist * 100 / N chist = np.cumsum(hist) plt.bar(edges[:-1], 100 - chist, duration / 20) plt.xlabel('Time (s)') plt.ylabel('% failed') plt.xlim(0, duration) plt.ylim(0, 100) plt.savefig('histogram.png') plt.show()
class CSpaceObstacleProgram(GLProgram): def __init__(self, space, start=(0.1, 0.5), goal=(0.9, 0.5)): GLProgram.__init__(self) self.space = space #PRM planner MotionPlan.setOptions(type="prm", knn=10, connectionThreshold=0.1) self.optimizingPlanner = False #FMM* planner #MotionPlan.setOptions(type="fmm*") #self.optimizingPlanner = True #RRT planner #MotionPlan.setOptions(type="rrt",perturbationRadius=0.25,bidirectional=True) #self.optimizingPlanner = False #RRT* planner #MotionPlan.setOptions(type="rrt*") #self.optimizingPlanner = True #random-restart RRT planner #MotionPlan.setOptions(type="rrt",perturbationRadius=0.25,bidirectional=True,shortcut=True,restart=True,restartTermCond="{foundSolution:1,maxIters:1000}") #self.optimizingPlanner = True #OMPL planners: #Tested to work fine with OMPL's prm, lazyprm, prm*, lazyprm*, rrt, rrt*, rrtconnect, lazyrrt, lbtrrt, sbl, bitstar. #Note that lbtrrt doesn't seem to continue after first iteration. #Note that stride, pdst, and fmt do not work properly... #MotionPlan.setOptions(type="ompl:rrt",suboptimalityFactor=0.1,knn=10,connectionThreshold=0.1) #self.optimizingPlanner = True self.planner = MotionPlan(space) self.start = start self.goal = goal self.planner.setEndpoints(start, goal) self.path = [] self.G = None def keyboardfunc(self, key, x, y): if key == ' ': if self.optimizingPlanner or not self.path: print "Planning 1..." self.planner.planMore(1) self.path = self.planner.getPath() self.G = self.planner.getRoadmap() self.refresh() elif key == 'p': if self.optimizingPlanner or not self.path: print "Planning 100..." self.planner.planMore(100) self.path = self.planner.getPath() self.G = self.planner.getRoadmap() self.refresh() def display(self): glMatrixMode(GL_PROJECTION) glLoadIdentity() glOrtho(0, 1, 1, 0, -1, 1) glMatrixMode(GL_MODELVIEW) glLoadIdentity() glDisable(GL_LIGHTING) self.space.drawObstaclesGL() if self.path: #draw path glColor3f(0, 1, 0) glBegin(GL_LINE_STRIP) for q in self.path: glVertex2f(q[0], q[1]) glEnd() for q in self.path: self.space.drawRobotGL(q) else: self.space.drawRobotGL(self.start) self.space.drawRobotGL(self.goal) if self.G: #draw graph V, E = self.G glEnable(GL_BLEND) glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) glColor4f(0, 0, 0, 0.5) glPointSize(3.0) glBegin(GL_POINTS) for v in V: glVertex2f(v[0], v[1]) glEnd() glColor4f(0.5, 0.5, 0.5, 0.5) glBegin(GL_LINES) for (i, j) in E: glVertex2f(V[i][0], V[i][1]) glVertex2f(V[j][0], V[j][1]) glEnd() glDisable(GL_BLEND)