def getCollisionFreePath(self, start, goal, iterations):
""" Given a start and a goal configuration, returns a collision-free path between the two configurations"""
""" Currently takes forever to find a path... Needs more work"""
#MotionPlan.setOptions(type="rrt", perturbationRadius=2.0, bidirectional=True)
#MotionPlan.setOptions(type="prm", knn=10, connectionThreshold=0.25)
MotionPlan.setOptions(type="sbl",
perturbationRadius=2.0,
connectionThreshold=0.5,
bidirectional=True)
#MotionPlan.setOptions(type="lazyrrg*")
#space = ObstacleCSpace(self.collider, self.robot)
#planner = MotionPlan(space)
#planner = robotplanning.planToConfig(self.world, self.robot, goal, type="prm", knn=10, connectionThreshold=0.1)
space = robotcspace.RobotCSpace(self.robot, WorldCollider(self.world))
jointLimits = self.robot.getJointLimits()
lower = jointLimits[0]
higher = jointLimits[1]
for i in range(12):
lower[i] = 0
higher[i] = 0
newLimits = (lower, higher)
space.bound = zip(*newLimits)
planner = cspace.MotionPlan(space)
planner.setEndpoints(start, goal)
planner.planMore(iterations)
V, E = planner.getRoadmap()
self.roadMap = (V, E)
return planner.getPath()
def __init__(self,world,knowledge):
self.world = world
self.robot = world.robot(0)
self.knowledge = knowledge
self.collider = WorldCollider(world)
#these may be helpful
self.left_camera_link = self.robot.getLink(left_camera_link_name)
self.right_camera_link = self.robot.getLink(right_camera_link_name)
self.left_gripper_link = self.robot.getLink(left_gripper_link_name)
self.right_gripper_link = self.robot.getLink(right_gripper_link_name)
self.left_arm_links = [self.robot.getLink(i) for i in left_arm_link_names]
self.right_arm_links = [self.robot.getLink(i) for i in right_arm_link_names]
self.id_to_index = id_to_index = dict([(self.robot.getLink(i).getID(),i) for i in range(self.robot.numLinks())])
self.left_arm_indices = [id_to_index[i.getID()] for i in self.left_arm_links]
self.right_arm_indices = [id_to_index[i.getID()] for i in self.right_arm_links]
self.arm_links = {
'right': [ (i, self.robot.getLink(i)) for i in right_arm_geometry_indices + right_hand_geometry_indices ],
'left': [ (i, self.robot.getLink(i)) for i in left_arm_geometry_indices + left_hand_geometry_indices ]
}
self.robot_links = [ (i, self.robot.getLink(i)) for i in range(self.robot.numLinks()) ]
# plan a path using RRT
MotionPlan.setOptions(type="rrt",shortcut=1,restart=1)
self.cs = {}
for limb in [ 'left', 'right' ]:
# set up C-space
self.cs[limb] = LimbCSpace(self, limb)
self.cs[limb].setup()
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
self.planner = MotionPlan(space)
self.start=start
self.goal=goal
self.planner.setEndpoints(start,goal)
self.path = []
self.G = None
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
self.planner = MotionPlan(space)
self.start = start
self.goal = goal
self.planner.setEndpoints(start, goal)
self.path = []
self.G = None
def __init__(self,space,start=(0.1,0.5),goal=(0.9,0.5)):
GLProgram.__init__(self)
self.space = space
MotionPlan.setOptions(type="prm",knn=10,connectionThreshold=0.1)
#MotionPlan.setOptions(type="rrt",perturbationRadius=0.25,bidirectional=True)
self.planner = MotionPlan(space)
self.start=start
self.goal=goal
self.planner.setEndpoints(start,goal)
self.path = []
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..."
(res,iters) = solver.solve(100,1e-3);
if res:
qpregrasp = robot.getConfig()
qpregrasparm = [qpregrasp[i] for i in hand.armIndices]
if not cspace.feasible(qpregrasparm):
print "Pregrasp config infeasible"
return None
if qpregrasp == None:
print "Pregrasp solve failed"
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
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
#add a path to the grasp configuration
return path + [hand.open(path[-1],0)]
def plan_limb(self,limb,limbstart,limbgoal,ignore=None):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
# update the ignored opbjects list
self.cs[limb].ignore = ignore
mp = MotionPlan(self.cs[limb])
try:
mp.setEndpoints(limbstart, limbgoal)
# need this to catch the RuntimeError raised when the goal is infeasible
except RuntimeError as e:
print 'Error:', e
return None
# try 10 rounds of 10 iterations
# if a path isn't found in 50 iterations, report failure
# so a new IK solution is attempted
for i in range(10):
mp.planMore(10)
path = mp.getPath()
# give the simulation a chance to keep up
time.sleep(0.01)
if path:
break
# clean up
mp.close()
if path:
# ensure the path starts with the initial position
if path[0] != limbstart:
path.insert(0, limbstart)
for q in path:
if len(q) < len(limbstart):
print 'fix length'
q.extend([0]*(len(limbstart) - len(q)))
if not self.check_limb_collision_free(limb, q, ignore):
print 'collision (coarse)!'
return None
# the robot can't keep up if the path is too coarsely sampled
# thus, linearly interpolate between "distant" poses in joint space
fine_path = self.resample_path(path, pi/50)
for q in fine_path:
# it seems that the path planner sometimes produces motion plans
# that when finely linearly interpolated have collision states
# thus, check for those collisions and reject the plan
if not self.check_limb_collision_free(limb, q, ignore):
print 'collision (fine)!'
return None
return fine_path
else:
return None
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
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 plan_limb(self, limb, limbstart, limbgoal):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
# TODO: implement me.
# Hint: you should implement the hooks in LimbCSpace above, and consult/
# use the MotionPlan class in klampt.cspace to run an existing
# planner.
space = LimbCSpace(self, limb)
MotionPlan.setOptions(type="rrt", connectionThreshold=5, bidirectional=True)
motionPlan = MotionPlan(space)
motionPlan.setEndpoints(limbstart, limbgoal)
t1 = time.clock()
motionPlan.planMore(500)
print time.clock() - t1
path = motionPlan.getPath()
motionPlan.close()
space.close()
if path == None:
return False
else:
print len(path)
return path
def plan_limb(self,limb,limbstart,limbgoal):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
#TODO: implement me.
#Hint: you should implement the hooks in ArmCSpace above, and consult
#use the MotionPlan class in klampt.cspace to run an existing
#planner.
rrt_planner = MotionPlan(LimbCSpace(self, limb), type="rrt")
rrt_planner.setEndpoints(limbstart, limbgoal)
print "Planning more"
while rrt_planner.getPath() is None:
print "iter...",random.random()
rrt_planner.planMore(10)
print "Planning more done"
print rrt_planner.getPath()
return [limbstart,limbgoal]
class CSpaceObstacleProgram(GLProgram):
def __init__(self,space,start=(0.1,0.5),goal=(0.9,0.5)):
GLProgram.__init__(self)
self.space = space
MotionPlan.setOptions(type="prm",knn=10,connectionThreshold=0.1)
#MotionPlan.setOptions(type="rrt",perturbationRadius=0.25,bidirectional=True)
self.planner = MotionPlan(space)
self.start=start
self.goal=goal
self.planner.setEndpoints(start,goal)
self.path = []
def keyboardfunc(self,key,x,y):
if key==' ':
if not self.path:
print "Planning 1..."
self.planner.planMore(1)
self.path = self.planner.getPath()
glutPostRedisplay()
elif key=='p':
if not self.path:
print "Planning 100..."
self.planner.planMore(100)
self.path = self.planner.getPath()
glutPostRedisplay()
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:
glColor3f(0,1,0)
glBegin(GL_LINE_STRIP)
for q in self.path:
print q
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)
def getCollisionFreePath(self, start, goal, iterations):
#MotionPlan.setOptions(type="rrt", perturbationRadius=2.0, bidirectional=True)
#MotionPlan.setOptions(type="prm", knn=10, connectionThreshold=0.25)
MotionPlan.setOptions(type="sbl",
perturbationRadius=2.0,
connectionThreshold=0.5,
bidirectional=True)
#MotionPlan.setOptions(type="lazyrrg*")
#space = ObstacleCSpace(self.collider, self.robot)
#planner = MotionPlan(space)
#planner = robotplanning.planToConfig(self.world, self.robot, goal, type="prm", knn=10, connectionThreshold=0.1)
print "milestone 1"
space = robotcspace.RobotCSpace(self.robot, WorldCollider(self.world))
jointLimits = self.robot.getJointLimits()
lower = jointLimits[0]
higher = jointLimits[1]
for i in range(12):
lower[i] = 0
higher[i] = 0
newLimits = (lower, higher)
space.bound = zip(*newLimits)
print "milestone 2"
planner = cspace.MotionPlan(space)
print "milestone 3"
planner.setEndpoints(start, goal)
print "before planning"
planner.planMore(iterations)
print "after planning"
V, E = planner.getRoadmap()
self.roadMap = (V, E)
print "No. of vertices:", len(V)
print "Vertices:", V
print "Edges:", E
return planner.getPath()
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()
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()
#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 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()
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()
#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 plan_limb(self,limb,limbstart,limbgoal):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
#TODO: implement me.
#Hint: you should implement the hooks in ArmCSpace above, and consult
#use the MotionPlan class in klampt.cspace to run an existing
#planner.
# Create limb cspace and new motion plan
limb_cspace = LimbCSpace(self, limb)
motion_plan = MotionPlan(limb_cspace, 'prm')
# Set start and endpoints and get path between them
motion_plan.setEndpoints(limbstart, limbgoal)
path = motion_plan.getPath()
# Memory cleanup
limb_cspace.close()
motion_plan.close()
return path
def plan_limb(self,limb,limbstart,limbgoal, printer=True, iks = None):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
self.rebuild_dynamic_objects()
# NOTE: Not sure, but I think this is what's happening
# Need to reset pathToDraw here because the MyGLViewer.draw() is called
# concurrently, it uses an old path (ex. of left arm) while using the
# new limb (ex. right limb) for drawing the trajectory. This throws an
# Index-Out-of-Range exception for drawing the path
self.pathToDraw = []
# NOTE:
cspace = LimbCSpace(self,limb)
if iks != None:
print "Initializing ClosedLoopCSPace"
cspace = ClosedLoopCSpaceTest(self,limb,iks)
if not cspace.feasible(limbstart):
cspace.feasible(limbstart)
print " Start configuration is infeasible!"
return 1
if not cspace.feasible(limbgoal):
print " Goal configuration is infeasible!"
return 2
# MotionPlan.setOptions(connectionThreshold=5.0)
# MotionPlan.setOptions(shortcut=1)
# plan = MotionPlan(cspace,'sbl')
# MotionPlan.setOptions(type='rrt*')
# MotionPlan.setOptions(type="prm",knn=10,connectionThreshold=0.01,shortcut=True)
# MotionPlan.setOptions(type='fmm*')
# MotionPlan.setOptions(bidirectional = 1)
# MotionPlan.setOptions(type="sbl", perturbationRadius = 0.25, connectionThreshold=2.0, bidirectional = True)
# MotionPlan.setOptions(type="rrt",perturbationRadius=0.1,bidirectional=True)
# MotionPlan.setOptions(type="rrt", perturbationRadius = 0.25, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
# MotionPlan.setOptions(type="rrt*", perturbationRadius = 0.25, connectionThreshold=2, bidirectional = True)
MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
# MotionPlan.setOptions(type="prm",knn=1,connectionThreshold=0.01)
# MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
# plan = MotionPlan(cspace, type='sbl')
# works best for non-ClosedLoopRobotCSpace
# MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
plan = MotionPlan(cspace)
plan.setEndpoints(limbstart,limbgoal)
maxPlanIters = 200
maxSmoothIters = 100
print " Planning.",
for iters in xrange(maxPlanIters):
# print iters
if limb=='left':
self.limb_indices = self.left_arm_indices
self.activeLimb = 'left'
else:
self.limb_indices = self.right_arm_indices
self.activeLimb = 'right'
self.roadmap = plan.getRoadmap()
# print plan.getRoadmap()
V,E =self.roadmap
# print "iter:",iters,"--",len(V),"feasible milestones sampled,",len(E),"edges connected"
# print iters,"/V",len(V),"/E",len(E),
print ".",
plan.planMore(10) # 100
path = plan.getPath()
if path != None:
if printer:
print "\n Found a path on iteration",iters
if len(path) > 2:
print " Smoothing path"
# plan.planMore(min(maxPlanIters-iters,maxSmoothIters))
plan.planMore(maxSmoothIters)
path = plan.getPath()
cspace.close()
plan.close()
# testing
# print " Returning path (limb", self.activeLimb,", (",len(self.limb_indices),"/",len(path[0]),"))"
self.pathToDraw = path
return path
cspace.close()
plan.close()
if printer:
print " No path found"
return False
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
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()
glutPostRedisplay()
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()
glutPostRedisplay()
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:
print q
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)
def plan_limb(self,limb,limbstart,limbgoal, printer=True, iks = None, ignoreColShelfSpatula=False):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
self.rebuild_dynamic_objects()
# NOTE: Not sure, but I think this is what's happening
# Need to reset pathToDraw here because the MyGLViewer.draw() is called
# concurrently, it uses an old path (ex. of left arm) while using the
# new limb (ex. right limb) for drawing the trajectory. This throws an
# Index-Out-of-Range exception for drawing the path
self.pathToDraw = []
if limb=='left':
self.limb_indices = self.left_arm_indices
self.activeLimb = 'left'
else:
self.limb_indices = self.right_arm_indices
self.activeLimb = 'right'
# NOTE:
cspace = LimbCSpace(self,limb)
MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
plan = MotionPlan(cspace)
plan.setEndpoints(limbstart,limbgoal)
maxPlanIters = 20
planMoreVal = 10
if iks != None:
print "Initializing ClosedLoopCSPace"
# cspace = ClosedLoopCSpaceTest(self,limb,iks)
collider = WorldCollider(self.world)
cspace = robotcspace.ClosedLoopRobotCSpace(self.robot, iks, collider)
cspace.eps = 1e-2
cspace.setup()
q = limbstart
qcurrent = self.robot.getConfig()
if len(q) < len(qcurrent):
if len(self.limb_indices) != len(q):
return False
else:
for i in range(len(self.limb_indices)):
qcurrent[self.limb_indices[i]] = q[i]
q = qcurrent
limbstart = q
q = limbgoal
qcurrent = self.robot.getConfig()
if len(q) < len(qcurrent):
if len(self.limb_indices) != len(q):
return False
else:
for i in range(len(self.limb_indices)):
qcurrent[self.limb_indices[i]] = q[i]
q = qcurrent
limbgoal = q
# print "limbstart:", limbstart
# print "limbgoal:", limbgoal
limbstart = cspace.solveConstraints(limbstart)
limbgoal = cspace.solveConstraints(limbgoal)
# print "limbstart:", limbstart
# print "limbgoal:", limbgoal
settings = { 'type':"sbl", 'perturbationRadius':0.5, 'bidirectional':1, 'shortcut':1, 'restart':1, 'restartTermCond':"{foundSolution:1,maxIters:1000}" }
self.robot.setConfig(limbstart)
plan = robotplanning.planToConfig(self.world, self.robot, limbgoal, movingSubset='all', **settings)
maxPlanIters = 1
planMoreVal = 500
if not cspace.feasible(limbstart):
print " Start configuration is infeasible!"
return 1
if not cspace.feasible(limbgoal):
print " Goal configuration is infeasible!"
return 2
# MotionPlan.setOptions(connectionThreshold=5.0)
# MotionPlan.setOptions(shortcut=1)
# plan = MotionPlan(cspace,'sbl')
# MotionPlan.setOptions(type='rrt*')
# MotionPlan.setOptions(type="prm",knn=10,connectionThreshold=0.01,shortcut=True)
# MotionPlan.setOptions(type='fmm*')
# MotionPlan.setOptions(bidirectional = 1)
# MotionPlan.setOptions(type="sbl", perturbationRadius = 0.25, connectionThreshold=2.0, bidirectional = True)
# MotionPlan.setOptions(type="rrt",perturbationRadius=0.1,bidirectional=True)
# MotionPlan.setOptions(type="rrt", perturbationRadius = 0.25, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
# MotionPlan.setOptions(type="rrt*", perturbationRadius = 0.25, connectionThreshold=2, bidirectional = True)
# MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
# MotionPlan.setOptions(type="prm",knn=1,connectionThreshold=0.01)
# MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
# plan = MotionPlan(cspace, type='sbl')
# works best for non-ClosedLoopRobotCSpace
# MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
# plan = MotionPlan(cspace)
# maxPlanIters = 20
maxSmoothIters = 100
print " Planning.",
for iters in xrange(maxPlanIters):
# print iters
self.roadmap = plan.getRoadmap()
# print plan.getRoadmap()
V,E =self.roadmap
# print "iter:",iters,"--",len(V),"feasible milestones sampled,",len(E),"edges connected"
# print iters,"/V",len(V),"/E",len(E),
print ".",
plan.planMore(planMoreVal) # 100
path = plan.getPath()
if path != None:
if printer:
print "\n Found a path on iteration",iters
if len(path) > 2:
print " Smoothing path"
if iks == None:
# plan.planMore(min(maxPlanIters-iters,maxSmoothIters))
plan.planMore(maxSmoothIters)
path = plan.getPath()
if iks!=None:
path = cspace.discretizePath(path)
cspace.close()
plan.close()
# testing
# print " Returning path (limb", self.activeLimb,", (",len(self.limb_indices),"/",len(path[0]),"))"
self.pathToDraw = path
return path
cspace.close()
plan.close()
if printer:
print " No path found"
return False
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
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()
glutPostRedisplay()
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()
glutPostRedisplay()
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:
print q
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)
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
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 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..."
(res, iters) = solver.solve(100, 1e-3)
if res:
qpregrasp = robot.getConfig()
qpregrasparm = [qpregrasp[i] for i in hand.armIndices]
if not cspace.feasible(qpregrasparm):
print "Pregrasp config infeasible"
return None
if qpregrasp == None:
print "Pregrasp solve failed"
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
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
#add a path to the grasp configuration
return path + [hand.open(path[-1], 0)]
def plan_limb_transfer(self,limb,limbstart,limbgoal,heldobject,grasp):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal while holding the given object.
Returns False if planning failed"""
self.rebuild_dynamic_objects()
cspace = TransferCSpace(self,limb,heldobject,grasp)
if not cspace.feasible(limbstart):
print " Start configuration is infeasible!"
return False
if not cspace.feasible(limbgoal):
print " Goal configuration is infeasible!"
return False
MotionPlan.setOptions(connectionThreshold=5.0)
MotionPlan.setOptions(shortcut=1)
plan = MotionPlan(cspace,'sbl')
# MotionPlan.setOptions(type='rrt*')
# MotionPlan.setOptions(type="prm",knn=10,connectionThreshold=0.1,shortcut=True)
# MotionPlan.setOptions(type='fmm*')
plan = MotionPlan(cspace)
plan.setEndpoints(limbstart,limbgoal)
maxPlanIters = 200
maxSmoothIters = 10
for iters in xrange(maxPlanIters):
plan.planMore(1)
path = plan.getPath()
if path != None:
print " Found a path on iteration",iters,'/',maxPlanIters
if len(path) > 2:
print " Smoothing..."
plan.planMore(min(maxPlanIters-iters,maxSmoothIters))
path = plan.getPath()
cspace.close()
plan.close()
return path
cspace.close()
plan.close()
print "No path found"
return False
def plan_limb(self,limb,limbstart,limbgoal, printer=True, iks = None):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
self.rebuild_dynamic_objects()
# NOTE: Not sure, but I think this is what's happening
# Need to reset pathToDraw here because the MyGLViewer.draw() is called
# concurrently, it uses an old path (ex. of left arm) while using the
# new limb (ex. right limb) for drawing the trajectory. This throws an
# Index-Out-of-Range exception for drawing the path
self.pathToDraw = []
# NOTE:
cspace = LimbCSpace(self,limb)
if iks != None:
print "Initializing ClosedLoopCSPace"
cspace = ClosedLoopCSpaceTest(self,limb,iks)
if not cspace.feasible(limbstart):
print " Start configuration is infeasible!"
return 1
if not cspace.feasible(limbgoal):
print " Goal configuration is infeasible!"
return 2
MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
plan = MotionPlan(cspace)
plan.setEndpoints(limbstart,limbgoal)
# maxPlanIters = 20
# maxSmoothIters = 100
maxPlanIters = 10
maxSmoothIters = 100
print " Planning.",
for iters in xrange(maxPlanIters):
# print iters
if limb=='left':
self.limb_indices = self.left_arm_indices
self.activeLimb = 'left'
else:
self.limb_indices = self.right_arm_indices
self.activeLimb = 'right'
self.roadmap = plan.getRoadmap()
V,E =self.roadmap
print ".",
plan.planMore(10) # 100
path = plan.getPath()
if path != None:
if printer:
print "\n Found a path on iteration",iters
if len(path) > 2:
print " Smoothing path"
# plan.planMore(min(maxPlanIters-iters,maxSmoothIters))
plan.planMore(maxSmoothIters)
path = plan.getPath()
cspace.close()
plan.close()
self.pathToDraw = path
return path
cspace.close()
plan.close()
if printer:
print " No path found"
return False
def plan_limb(self,limb,limbstart,limbgoal,Cspace='Normal',argsTuple=None,extra_feasibility_tests=[]):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
if(Cspace == 'scoop'):
cspace = ScoopCSpace(self,limb)
elif(Cspace == 'height'):
cspace = HeightCSpace(self,limb,argsTuple[0],argsTuple[1])
elif(Cspace == 'xrestrict'):
cspace = CSpaceRestrictX(self,limb,xrestrict=argsTuple[0])
else:
cspace = LimbCSpace(self,limb)
cspace.extra_feasibility_tests = extra_feasibility_tests
if not cspace.feasible(limbstart):
print " Start configuration is infeasible!"
visualization.debug(self.world)
return False
if not cspace.feasible(limbgoal):
print " Goal configuration is infeasible!"
visualization.debug(self.world)
return False
MotionPlan.setOptions(connectionThreshold=3.5,perturbationRadius=1)
MotionPlan.setOptions(shortcut=1)
plan = MotionPlan(cspace,'sbl')
plan.setEndpoints(limbstart,limbgoal)
maxPlanIters = 2000
maxSmoothIters = 50
for iters in xrange(maxPlanIters):
plan.planMore(1)
path = plan.getPath()
if path != None:
#print " Found a path on iteration",iters
if len(path) >= 2 and maxSmoothIters > 0:
#print " Smoothing..."
plan.planMore(min(maxPlanIters-iters,maxSmoothIters))
path = plan.getPath()
cspace.close()
plan.close()
#print "Average collision check time",cspace.collisionCheckTime/cspace.numCollisionChecks
#print cspace.numCollisionChecks,"collision checks"
return path
#print "Average collision check time",cspace.collisionCheckTime/cspace.numCollisionChecks
#print cspace.numCollisionChecks,"collision checks"
cspace.close()
plan.close()
#print " No path found"
return False
def plan_limb_transfer(self,limb,limbstart,limbgoal,heldobject,grasp,type='Normal',xrestrict=None,extra_feasibility_tests=[]):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal while holding the given object.
Returns False if planning failed"""
if(type == 'Normal'):
cspace = TransferCSpace(self,limb,heldobject,grasp,height=xrestrict)
elif(type == 'restrict'):
cspace = TransferCSpaceRestrict(self,limb,heldobject,grasp)
elif(type == 'upright'):
cspace = TransferCSpaceUpright(self,limb,heldobject,grasp)
elif(type == 'xrestrict'):
cspace = TransferCSpaceRestrict(self,limb,heldobject,grasp,xrestrict)
cspace.extra_feasibility_tests = extra_feasibility_tests
if not cspace.feasible(limbstart):
print " Start configuration is infeasible!"
return False
if not cspace.feasible(limbgoal):
print " Goal configuration is infeasible!"
return False
MotionPlan.setOptions(connectionThreshold=3.5,perturbationRadius=1)
MotionPlan.setOptions(shortcut=1)
plan = MotionPlan(cspace,'sbl')
plan.setEndpoints(limbstart,limbgoal)
maxPlanIters = 2000
maxSmoothIters = 50
for iters in xrange(maxPlanIters):
plan.planMore(1)
path = plan.getPath()
if path != None:
#print " Found a path on iteration",iters
if len(path) > 2 and maxSmoothIters > 0:
#print " Smoothing..."
plan.planMore(min(maxPlanIters-iters,maxSmoothIters))
path = plan.getPath()
cspace.close()
plan.close()
return path
cspace.close()
plan.close()
#print "No path found"
return False
def plan_limb(self,limb,limbstart,limbgoal):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
self.rebuild_dynamic_objects()
cspace = LimbCSpace(self,limb)
if not cspace.feasible(limbstart):
print " Start configuration is infeasible!"
return False
if not cspace.feasible(limbgoal):
print " Goal configuration is infeasible!"
return False
MotionPlan.setOptions(connectionThreshold=5.0)
MotionPlan.setOptions(shortcut=1)
plan = MotionPlan(cspace,'sbl')
plan.setEndpoints(limbstart,limbgoal)
maxPlanIters = 200
maxSmoothIters = 10
for iters in xrange(maxPlanIters):
plan.planMore(1)
path = plan.getPath()
if path != None:
print " Found a path on iteration",iters
if len(path) > 2:
print " Smoothing..."
plan.planMore(min(maxPlanIters-iters,maxSmoothIters))
path = plan.getPath()
cspace.close()
plan.close()
return path
cspace.close()
plan.close()
print " No path found"
return False
