class LimbPlanner:
"""Much of your code for HW4 will go here.
Attributes:
- world: the WorldModel
- robot: the RobotModel
- knowledge: the KnowledgeBase objcet
- collider: a WorldCollider object (see the klampt.robotcollide module)
"""
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.link(left_camera_link_name)
self.right_camera_link = self.robot.link(right_camera_link_name)
self.left_gripper_link = self.robot.link(left_gripper_link_name)
self.right_gripper_link = self.robot.link(right_gripper_link_name)
self.left_arm_links = [self.robot.link(i) for i in left_arm_link_names]
self.right_arm_links = [self.robot.link(i) for i in right_arm_link_names]
id_to_index = dict([(self.robot.link(i).getID(),i) for i in range(self.robot.numLinks())])
self.left_arm_indices = left_arm_geometry_indices + left_hand_geometry_indices
self.right_arm_indices = right_arm_geometry_indices + right_hand_geometry_indices
self.dynamic_objects = []
self.roadmap = ([],[])
self.limb_indices = []
self.pathToDraw = []
self.colMargin = 0
self.activeLimb = None
def set_limb_config(self,limb,limbconfig,q):
"""Helper: Sets the 7-DOF configuration of the given limb in
q. Other DOFs are not touched."""
assert len(q) == self.robot.numLinks()
if limb=='left':
for (i,v) in zip(self.left_arm_indices,limbconfig):
q[i] = v
else:
for (i,v) in zip(self.right_arm_indices,limbconfig):
q[i] = v
def get_limb_config(self,q,limb):
"""Helper: Gets the 7-DOF configuration of the given limb given
a full-robot configuration q"""
qlimb = [0.0]*len(self.left_arm_indices)
if limb=='left':
qlimb = [0.0]*len(self.left_arm_indices)
for (i,j) in enumerate(self.left_arm_indices):
qlimb[i] = q[j]
else:
qlimb = [0.0]*len(self.right_arm_indices)
for (i,j) in enumerate(self.right_arm_indices):
qlimb[i] = q[j]
return qlimb
def check_collision_free(self,limb):
"""Checks whether the given limb is collision free at the robot's
current configuration"""
armfilter = None
if limb=='left':
collindices = set(left_arm_geometry_indices+left_hand_geometry_indices)
# collindices = set(self.left_arm_indices)
else:
collindices = set(right_arm_geometry_indices+right_hand_geometry_indices)
#print "collindices", collindices
armfilter = lambda x:isinstance(x,RobotModelLink) and (x.index in collindices)
ignoreList = ["Amazon_Picking_Shelf", "bin_"]
ignoreList = '\t'.join(ignoreList)
spatulaIgnoreList = [57]
#check with objects in world model
for o1,o2 in self.collider.collisionTests(armfilter,lambda x:True): # NOTE: what is bb_reject??
#print "Collision Test: Collision between",o1[0].getName(),o2[0].getName()
#print "Collision Test: Collision for ",o2[0].getName()
#print o1[0].index, o2[0].index
if o1[0].getName()[0:3] in ignoreList and o2[0].index in spatulaIgnoreList:
# print "ignoring collision between shelf and spautla"
continue
if o2[0].getName()[0:3] in ignoreList and o1[0].index in spatulaIgnoreList:
# print "ignoring collision between shelf and spautla"
continue
if o1[0].getName()[0:3] in ignoreList:
for obj in self.dynamic_objects:
assert obj.info.geometry != None
if o1[1].collides(obj.info.geometry):
continue
if o2[0].getName()[0:3] in ignoreList:
for obj in self.dynamic_objects:
assert obj.info.geometry != None
if o1[1].collides(obj.info.geometry):
continue
if (o1[0].index == 15 and o2[0].index == 33) or (o2[0].index == 15 and o1[0].index == 33):
continue
if (o1[0].index == 35 and o2[0].index == 33) or (o2[0].index == 35 and o1[0].index == 33):
continue
if o1[1].collides(o2[1]):
# print "Collision between",o1[0].getName(),o2[0].getName()
# print "Collision between",o1[0].index,o2[0].index
return False
return True
def rebuild_dynamic_objects(self):
# self.dynamic_objects = []
# #check with objects in knowledge
# for (k,objList) in self.knowledge.bin_contents.iteritems():
# if objList == None:
# #not sensed
# continue
# for item in objList:
# assert item.info.geometry != None
# item.info.geometry.setCurrentTransform(*item.xform)
# self.dynamic_objects.append(item)
return
def check_limb_collision_free(self,limb,limbconfig):
"""Checks whether the given 7-DOF limb configuration is collision
free, keeping the rest of self.robot fixed."""
q = self.robot.getConfig()
self.set_limb_config(limb,limbconfig,q)
self.robot.setConfig(q)
return self.check_collision_free(limb)
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(self,start,goal,limb,printer=True, iks = None, ignoreColShelfSpatula = True):
"""Plans a motion for the robot to move from configuration start
to configuration goal. By default, moves the left arm first,
then the right. To move the right first, set the 'order' argument
to ['right','left']"""
limbstart = {}
limbgoal = {}
# for l in ['left','right']:
l =limb
limbstart[l] = self.get_limb_config(start,l)
limbgoal[l] = self.get_limb_config(goal,l)
path = [start]
curconfig = start[:]
diff = sum((a-b)**2 for a,b in zip(limbstart[l],limbgoal[l]))
if diff > 1e-8:
if printer:
print "< Planning for limb",l,">"
# print " Euclidean distance:",math.sqrt(diff)
self.robot.setConfig(curconfig)
#do the limb planning
if not ignoreColShelfSpatula:
self.left_arm_indices = left_arm_geometry_indices + left_hand_geometry_indices + [55,56,57]
else:
self.left_arm_indices = left_arm_geometry_indices + left_hand_geometry_indices
# print " Left arm links", self.left_arm_indices
if iks == None:
limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],printer=printer, iks=iks)
else:
trajectory = self.plan_closedLoop(start,goal,iks=iks)
return trajectory
if limbpath == 1 or limbpath == 2 or limbpath == False:
if printer:
print " Failed to plan for limb",l,"\n"
return limbpath
if printer:
print " Planned successfully for limb",l, "\n"
#concatenate whole body path
for qlimb in limbpath[1:]:
q = path[-1][:]
self.set_limb_config(l,qlimb,q)
path.append(q)
self.set_limb_config(l,limbgoal[l],curconfig)
return path
def plan_closedLoop(self,qstart,qgoal,iks,printer=False):
if printer:
print "starting config: ", qstart
print "goal config: ", qgoal
self.world.terrain(0).geometry().setCollisionMargin(0.05)
cspace = robotcspace.ClosedLoopRobotCSpace(self.robot, iks, self.collider)
cspace.eps = 1e-3
cspace.setup()
configs=[]
configs.append(qstart)
configs.append(qgoal)
configs[0] = cspace.solveConstraints(configs[0])
configs[1] = cspace.solveConstraints(configs[1])
settings = { 'type':"sbl", 'perturbationRadius':0.5, 'bidirectional':1, 'shortcut':1, 'restart':1, 'restartTermCond':"{foundSolution:1,maxIters:1000}" }
wholepath = [configs[0]]
for i in range(len(configs)-1):
#this code uses the robotplanning module's convenience functions
self.robot.setConfig(configs[i])
plan = robotplanning.planToConfig(self.world,self.robot,configs[i+1],
movingSubset='all',
**settings)
if plan is None:
return False
print " Planning..."
keepPlanning = True
while keepPlanning:
plan.planMore(500)
#this code just gives some debugging information. it may get expensive
# V,E = plan.getRoadmap()
# self.roadmap = 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"
#debug some sampled configurations
# print V[0:max(10,len(V))]
# return False
else:
keepPlanning = False
self.pathToDraw = path
#the path is currently a set of milestones: discretize it so that it stays near the contact surface
path = cspace.discretizePath(path,epsilon=1e-2)
# path = cspace.discretizePath(path,epsilon=1e-4)
wholepath += path[1:]
#to be nice to the C++ module, do this to free up memory
plan.space.close()
plan.close()
return wholepath
class LimbPlanner:
"""Much of your code for HW4 will go here.
Attributes:
- world: the WorldModel
- robot: the RobotModel
- knowledge: the KnowledgeBase objcet
- collider: a WorldCollider object (see the klampt.robotcollide module)
"""
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(baxter.left_camera_link_name)
self.right_camera_link = self.robot.getLink(baxter.right_camera_link_name)
self.left_gripper_link = self.robot.getLink(baxter.left_gripper_link_name)
self.right_gripper_link = self.robot.getLink(baxter.right_gripper_link_name)
self.left_arm_links = [self.robot.getLink(i) for i in baxter.left_arm_link_names]
self.right_arm_links = [self.robot.getLink(i) for i in baxter.right_arm_link_names]
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.dynamic_objects = []
def set_limb_config(self,limb,limbconfig,q):
"""Helper: Sets the 7-DOF configuration of the given limb in
q. Other DOFs are not touched."""
assert len(q) == self.robot.numLinks()
if limb=='left':
for (i,v) in zip(self.left_arm_indices,limbconfig):
q[i] = v
else:
for (i,v) in zip(self.right_arm_indices,limbconfig):
q[i] = v
def get_limb_config(self,q,limb):
"""Helper: Gets the 7-DOF configuration of the given limb given
a full-robot configuration q"""
qlimb = [0.0]*len(self.left_arm_indices)
if limb=='left':
for (i,j) in enumerate(self.left_arm_indices):
qlimb[i] = q[j]
else:
for (i,j) in enumerate(self.right_arm_indices):
qlimb[i] = q[j]
return qlimb
def check_collision_free(self,limb,exclude=None,verbose=False):
"""Checks whether the given limb is collision free at the robot's
current configuration"""
exclude = exclude or []
armfilter = None
if limb=='left':
collindices = set(baxter.left_arm_geometry_indices+baxter.left_hand_geometry_indices)
handindices = baxter.left_hand_geometry_indices
else:
collindices = set(baxter.right_arm_geometry_indices+baxter.right_hand_geometry_indices)
handindices = baxter.right_hand_geometry_indices
armfilter = lambda x:isinstance(x,RobotModelLink) and (x.index in collindices)
#check with objects in world model
for o1,o2 in self.collider.collisionTests(armfilter,lambda x:True):
#ignore hand self collisions
if isinstance(o1[0],RobotModelLink) and o1[0].index in handindices and isinstance(o2[0],RobotModelLink) and o2[0].index in handindices:
continue
if o1[0].getID() not in exclude and o2[0].getID() not in exclude:
if o1[1].collides(o2[1]):
if verbose:
print "Collision between",o1[0].getName(),o2[0].getName()
return False
return True
def check_collision_free_with_object(self,limb,object,grasp):
"""Checks whether the given limb, holding an object at the given
grasp, is collision free at the robot's current configuration"""
objToGripperXForm = se3.mul(baxter.left_gripper_center_xform,se3.inv(grasp))
#assume robot configuration is updated
if limb=='left':
gripperLink = self.robot.getLink(baxter.left_gripper_link_name)
else:
gripperLink = self.robot.getLink(baxter.right_gripper_link_name)
if not isinstance(object,list):
if object==None:
object = []
else:
object = [object]
Tgripper = gripperLink.getTransform()
Tobj = se3.mul(Tgripper,objToGripperXForm)
for o in object:
o.setTransform(*Tobj)
excludeids = [ o.getID() for o in object]
if not self.check_collision_free(limb,exclude=excludeids):
#print 'Limb is not collision free'
return False
for t in xrange(self.world.numRigidObjects()):
other = self.world.rigidObject(t)
if(other.getID() not in excludeids):
if any(other.geometry().collides(o.geometry()) for o in object):
#visualization.debug(self.world)
#print "Held object-shelf collision"
return False
return True
def check_limb_collision_free(self,limb,limbconfig):
"""Checks whether the given 7-DOF limb configuration is collision
free, keeping the rest of self.robot fixed."""
q = self.robot.getConfig()
self.set_limb_config(limb,limbconfig,q)
self.robot.setConfig(q)
return self.check_collision_free(limb)
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(self,start,goal,order=['left','right'],Cspace='Normal',argsTuple=None,extra_feasibility_tests=[]):
"""Plans a motion for the robot to move from configuration start
to configuration goal. By default, moves the left arm first,
then the right. To move the right first, set the 'order' argument
to ['right','left']"""
limbstart = {}
limbgoal = {}
for l in ['left','right']:
limbstart[l] = self.get_limb_config(start,l)
limbgoal[l] = self.get_limb_config(goal,l)
path = [start]
curconfig = start[:]
for l in order:
diff = sum((a-b)**2 for a,b in zip(limbstart[l],limbgoal[l]))
if diff > 1e-8:
#print "Planning for limb",l
#print " Euclidean distance:",math.sqrt(diff)
self.robot.setConfig(curconfig)
#do the limb planning
if(Cspace == 'scoop'):
limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],Cspace='scoop',extra_feasibility_tests=extra_feasibility_tests)
elif(Cspace == 'height'):
limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],Cspace='height',argsTuple=argsTuple,extra_feasibility_tests=extra_feasibility_tests)
elif(Cspace == 'xrestrictNobj'):
limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],Cspace='xrestrict',argsTuple=argsTuple,extra_feasibility_tests=extra_feasibility_tests)
else:
limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],extra_feasibility_tests=extra_feasibility_tests)
if limbpath == False:
#print " Failed to plan for limb",l
return None
#print " Planned successfully for limb",l
#concatenate whole body path
for qlimb in limbpath[1:]:
q = path[-1][:]
self.set_limb_config(l,qlimb,q)
path.append(q)
self.set_limb_config(l,limbgoal[l],curconfig)
return path
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_transfer(self,start,goal,limb,heldobject,grasp,type='Normal',xrestrict=None,extra_feasibility_tests=[]):
"""Plans a motion for the robot to move from configuration start
to configuration goal while holding the given object."""
limbstart = self.get_limb_config(start,limb)
limbgoal = self.get_limb_config(goal,limb)
path = [start]
#print "Planning transfer path for limb",limb
self.robot.setConfig(start)
#do the limb planning
limbpath = self.plan_limb_transfer(limb,limbstart,limbgoal,heldobject,grasp,type,xrestrict,extra_feasibility_tests=extra_feasibility_tests)
if limbpath == False:
#print " Failed to plan transfer path for limb",limb
return None
#print " Planned transfer path successfully for limb",limb
#concatenate whole body path
for qlimb in limbpath[1:]:
q = path[-1][:]
self.set_limb_config(limb,qlimb,q)
path.append(q)
return path
def updateWorld(self,new_world):
self.world = new_world
self.collider = WorldCollider(new_world)
def check_collision_free_with_object_base(self,limb,object,grasp):
"""Checks whether the given limb, holding an object at the given
grasp, is collision free at the robot's current configuration"""
objToGripperXForm = se3.mul(baxter.left_gripper_center_xform,se3.inv(grasp))
#assume robot configuration is updated
if limb=='left':
gripperLink = self.robot.getLink(baxter.left_gripper_link_name)
else:
gripperLink = self.robot.getLink(baxter.right_gripper_link_name)
if not isinstance(object,list):
if object==None:
object = []
else:
object = [object]
Tgripper = gripperLink.getTransform()
Tobj = se3.mul(Tgripper,objToGripperXForm)
for o in object:
o.setTransform(*Tobj)
excludeids = [ o.getID() for o in object]
if not self.check_collision_free_hand(limb,exclude=excludeids):
#print 'Limb is not collision free'
return False
#if not self.check_collision_free(limb,exclude=excludeids):
# #print 'Limb is not collision free'
# return False
for t in xrange(self.world.numRigidObjects()):
other = self.world.rigidObject(t)
if(other.getID() not in excludeids):
if any(other.geometry().collides(o.geometry()) for o in object):
#visualization.debug(self.world)
#print "Held object-shelf collision"
return False
return True
def check_collision_free_hand(self,limb,exclude=None,verbose=False):
"""Checks whether the given limb is collision free at the robot's
current configuration"""
exclude = exclude or []
armfilter = None
if limb=='left':
collindices = set(baxter.left_arm_geometry_indices+baxter.left_hand_geometry_indices)
handindices = baxter.left_hand_geometry_indices
else:
collindices = set(baxter.right_arm_geometry_indices+baxter.right_hand_geometry_indices)
handindices = baxter.right_hand_geometry_indices
#add in the base
collindices.add(0)
collindices.add(1)
collindices.add(2)
collindices.add(3)
collindices.add(4)
collindices.add(5)
armfilter = lambda x:isinstance(x,RobotModelLink) and (x.index in collindices)
#check with objects in world model
for o1,o2 in self.collider.collisionTests(armfilter,lambda x:True):
#ignore hand self collisions
if isinstance(o1[0],RobotModelLink) and o1[0].index in handindices and isinstance(o2[0],RobotModelLink) and o2[0].index in handindices:
continue
if o1[0].getID() not in exclude and o2[0].getID() not in exclude:
if o1[1].collides(o2[1]):
if verbose:
print "Collision between",o1[0].getName(),o2[0].getName()
return False
if(not isinstance(o1[0],RobotModelLink) and o2[0].index not in handindices):
print 'checking object with base'
if o1[1].collides(o2[1]):
if verbose:
print "Collision between",o1[0].getName(),o2[0].getName()
return False
if(not isinstance(o2[0],RobotModelLink) and o1[0].index not in handindices):
if o1[1].collides(o2[1]):
if verbose:
print "Collision between",o1[0].getName(),o2[0].getName()
return False
return True
class LimbPlanner:
"""Much of your code for HW4 will go here.
Attributes:
- world: the WorldModel
- robot: the RobotModel
- knowledge: the KnowledgeBase objcet
- collider: a WorldCollider object (see the klampt.robotcollide module)
"""
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.link(left_camera_link_name)
self.right_camera_link = self.robot.link(right_camera_link_name)
self.left_gripper_link = self.robot.link(left_gripper_link_name)
self.right_gripper_link = self.robot.link(right_gripper_link_name)
self.left_arm_links = [self.robot.link(i) for i in left_arm_link_names]
self.right_arm_links = [self.robot.link(i) for i in right_arm_link_names]
id_to_index = dict([(self.robot.link(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.dynamic_objects = []
def set_limb_config(self,limb,limbconfig,q):
"""Helper: Sets the 7-DOF configuration of the given limb in
q. Other DOFs are not touched."""
assert len(q) == self.robot.numLinks()
if limb=='left':
for (i,v) in zip(self.left_arm_indices,limbconfig):
q[i] = v
else:
for (i,v) in zip(self.right_arm_indices,limbconfig):
q[i] = v
def get_limb_config(self,q,limb):
"""Helper: Gets the 7-DOF configuration of the given limb given
a full-robot configuration q"""
qlimb = [0.0]*len(self.left_arm_indices)
if limb=='left':
for (i,j) in enumerate(self.left_arm_indices):
qlimb[i] = q[j]
else:
for (i,j) in enumerate(self.right_arm_indices):
qlimb[i] = q[j]
return qlimb
def check_collision_free(self,limb):
"""Checks whether the given limb is collision free at the robot's
current configuration"""
armfilter = None
if limb=='left':
collindices = set(left_arm_geometry_indices+left_hand_geometry_indices)
else:
collindices = set(right_arm_geometry_indices+right_hand_geometry_indices)
armfilter = lambda x:isinstance(x,RobotModelLink) and (x.index in collindices)
#check with objects in world model
for o1,o2 in self.collider.collisionTests(armfilter,lambda x:True):
if o1[1].collides(o2[1]):
# print "Collision between",o1[0].getName(),o2[0].getName()
return False
for obj in self.dynamic_objects:
assert obj.info.geometry != None
for link in collindices:
if self.robot.link(link).geometry().collides(obj.info.geometry):
# NOTE: Uncomment this line to show collision warnings
# print "Collision between link",self.robot.link(link).getName()," and dynamic object"
return False
return True
def check_collision_free_with_object(self,limb,objectGeom,grasp):
"""Checks whether the given limb, holding an object at the given
grasp, is collision free at the robot's current configuration"""
if not self.check_collision_free(limb):
return False
objToGripperXForm = se3.mul(left_gripper_center_xform,se3.inv(grasp.grasp_xform))
#assume robot configuration is updated
if limb=='left':
gripperLink = self.robot.link(left_gripper_link_name)
else:
gripperLink = self.robot.link(right_gripper_link_name)
Tgripper = gripperLink.getTransform();
Tobj = se3.mul(Tgripper,objToGripperXForm)
objectGeom.setCurrentTransform(*Tobj)
for t in xrange(self.world.numTerrains()):
if self.world.terrain(t).geometry().collides(objectGeom):
# print "Held object-shelf collision"
return False
for o in self.dynamic_objects:
if o.info.geometry.collides(objectGeom):
# print "Held object-object collision"
return False
return True
def rebuild_dynamic_objects(self):
self.dynamic_objects = []
#check with objects in knowledge
for (k,objList) in self.knowledge.bin_contents.iteritems():
if objList == None:
#not sensed
continue
for item in objList:
assert item.info.geometry != None
item.info.geometry.setCurrentTransform(*item.xform)
self.dynamic_objects.append(item)
return
def check_limb_collision_free(self,limb,limbconfig):
"""Checks whether the given 7-DOF limb configuration is collision
free, keeping the rest of self.robot fixed."""
q = self.robot.getConfig()
self.set_limb_config(limb,limbconfig,q)
self.robot.setConfig(q)
return self.check_collision_free(limb)
def plan_limb(self,limb,limbstart,limbgoal, printer=True):
"""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:
cspace = LimbCSpace(self,limb)
cspaceTest = RobotCSpaceTest(self,limb)
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.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:
if printer:
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()
if printer:
print " No path found"
return False
def plan(self,start,goal,order=['left','right'],printer=True):
"""Plans a motion for the robot to move from configuration start
to configuration goal. By default, moves the left arm first,
then the right. To move the right first, set the 'order' argument
to ['right','left']"""
limbstart = {}
limbgoal = {}
for l in ['left','right']:
limbstart[l] = self.get_limb_config(start,l)
limbgoal[l] = self.get_limb_config(goal,l)
path = [start]
curconfig = start[:]
for l in order:
diff = sum((a-b)**2 for a,b in zip(limbstart[l],limbgoal[l]))
if diff > 1e-8:
if printer:
print "< Planning for limb",l,">"
print " Euclidean distance:",math.sqrt(diff)
self.robot.setConfig(curconfig)
#do the limb planning
limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],printer=printer)
if limbpath == 1 or limbpath == 2 or limbpath == False:
if printer:
print " Failed to plan for limb",l,"\n"
return limbpath
if printer:
print " Planned successfully for limb",l, "\n"
#concatenate whole body path
for qlimb in limbpath[1:]:
q = path[-1][:]
self.set_limb_config(l,qlimb,q)
path.append(q)
self.set_limb_config(l,limbgoal[l],curconfig)
return path
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_transfer(self,start,goal,limb,heldobject,grasp):
"""Plans a motion for the robot to move from configuration start
to configuration goal while holding the given object."""
limbstart = self.get_limb_config(start,limb)
limbgoal = self.get_limb_config(goal,limb)
path = [start]
print "Planning transfer path for limb",limb
self.robot.setConfig(start)
#do the limb planning
limbpath = self.plan_limb_transfer(limb,limbstart,limbgoal,heldobject,grasp)
if limbpath == False:
print " Failed to plan transfer path for limb",limb
return None
print " Planned transfer path successfully for limb",limb
#concatenate whole body path
for qlimb in limbpath[1:]:
q = path[-1][:]
self.set_limb_config(limb,qlimb,q)
path.append(q)
return path
class LimbPlanner:
"""Much of your code for HW4 will go here.
Attributes:
- world: the WorldModel
- robot: the RobotModel
- knowledge: the KnowledgeBase objcet
- collider: a WorldCollider object (see the klampt.robotcollide module)
"""
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.link(left_camera_link_name)
self.right_camera_link = self.robot.link(right_camera_link_name)
self.left_gripper_link = self.robot.link(left_gripper_link_name)
self.right_gripper_link = self.robot.link(right_gripper_link_name)
self.left_arm_links = [self.robot.link(i) for i in left_arm_link_names]
self.right_arm_links = [self.robot.link(i) for i in right_arm_link_names]
id_to_index = dict([(self.robot.link(i).getID(),i) for i in range(self.robot.numLinks())])
self.left_arm_indices = left_arm_geometry_indices + left_hand_geometry_indices
self.right_arm_indices = right_arm_geometry_indices + right_hand_geometry_indices
self.dynamic_objects = []
# NOTE: added from lab3e.py
self.roadmap = ([],[])
self.limb_indices = []
self.pathToDraw = []
self.colMargin = 0
self.activeLimb = None
def set_limb_config(self,limb,limbconfig,q):
"""Helper: Sets the 7-DOF configuration of the given limb in
q. Other DOFs are not touched."""
assert len(q) == self.robot.numLinks()
if limb=='left':
for (i,v) in zip(self.left_arm_indices,limbconfig):
q[i] = v
else:
for (i,v) in zip(self.right_arm_indices,limbconfig):
q[i] = v
def get_limb_config(self,q,limb):
"""Helper: Gets the 7-DOF configuration of the given limb given
a full-robot configuration q"""
qlimb = [0.0]*len(self.left_arm_indices)
if limb=='left':
qlimb = [0.0]*len(self.left_arm_indices)
for (i,j) in enumerate(self.left_arm_indices):
qlimb[i] = q[j]
else:
qlimb = [0.0]*len(self.right_arm_indices)
for (i,j) in enumerate(self.right_arm_indices):
qlimb[i] = q[j]
return qlimb
def check_collision_free(self,limb,ignoreColShelfSpatula=False):
"""Checks whether the given limb is collision free at the robot's
current configuration"""
armfilter = None
if limb=='left':
# if ignoreColShelfSpatula:
# collindices = set(left_arm_geometry_indices)
# else:
# collindices = set(left_arm_geometry_indices+left_hand_geometry_indices)
collindices = set(left_arm_geometry_indices+left_hand_geometry_indices)
else:
collindices = set(right_arm_geometry_indices+right_hand_geometry_indices)
armfilter = lambda x:isinstance(x,RobotModelLink) and (x.index in collindices)
ignoreList = ["Amazon_Picking_Shelf", "bin_"]
ignoreList = '\t'.join(ignoreList)
spatulaIgnoreList = [55,56,57]
# if ignoreColShelfSpatula:
# spatulaIgnoreList = [54,55,56,57]
#check with objects in world model
for o1,o2 in self.collider.collisionTests(armfilter,lambda x:True): # NOTE: what is bb_reject??
# print "Collision Test: Collision between",o1[0].getName(),o2[0].getName()
# if ignoreColShelfSpatula:
# print o1[0].getName()[0:4]
if o1[0].getName()[0:3] in ignoreList and o2[0].index in spatulaIgnoreList:
# print "ignoring collision between shelf and spautla"
continue
if o2[0].getName()[0:3] in ignoreList and o1[0].index in spatulaIgnoreList:
# print "ignoring collision between shelf and spautla"
continue
if o1[0].getName()[0:3] in ignoreList:
for obj in self.dynamic_objects:
assert obj.info.geometry != None
if o1[1].collides(obj.info.geometry):
continue
if o2[0].getName()[0:3] in ignoreList:
for obj in self.dynamic_objects:
assert obj.info.geometry != None
if o1[1].collides(obj.info.geometry):
continue
if o1[1].collides(o2[1]):
print "Collision between",o1[0].getName(),o2[0].getName()
return False
# for obj in self.dynamic_objects:
# # print "checking collision with objects"
# # print obj.info.geometry
# assert obj.info.geometry != None
# for link in collindices:
# if self.robot.link(link).geometry().collides(obj.info.geometry):
# print "Collision between link",self.robot.link(link).getName()," and dynamic object"
# return False
# for link in collindices:
# # print "Collision Test: Collision between",self.robot.link(link).getName(),"shelf"
# shelfGeometry = self.world.terrain(0).geometry()
# linkGeometry = self.robot.link(link).geometry()
# if shelfGeometry.collides(linkGeometry):
# print "link #",link,"collides with terrain"
# return False
# print self.robot.getConfig()
return True
def rebuild_dynamic_objects(self):
self.dynamic_objects = []
#check with objects in knowledge
for (k,objList) in self.knowledge.bin_contents.iteritems():
if objList == None:
#not sensed
continue
for item in objList:
assert item.info.geometry != None
item.info.geometry.setCurrentTransform(*item.xform)
self.dynamic_objects.append(item)
return
def check_limb_collision_free(self,limb,limbconfig,ignoreColShelfSpatula=False):
"""Checks whether the given 7-DOF limb configuration is collision
free, keeping the rest of self.robot fixed."""
q = self.robot.getConfig()
self.set_limb_config(limb,limbconfig,q)
self.robot.setConfig(q)
return self.check_collision_free(limb,ignoreColShelfSpatula=ignoreColShelfSpatula)
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
def plan(self,start,goal,limb,printer=True, iks = None, ignoreColShelfSpatula = False):
"""Plans a motion for the robot to move from configuration start
to configuration goal. By default, moves the left arm first,
then the right. To move the right first, set the 'order' argument
to ['right','left']"""
limbstart = {}
limbgoal = {}
# for l in ['left','right']:
l =limb
limbstart[l] = self.get_limb_config(start,l)
limbgoal[l] = self.get_limb_config(goal,l)
path = [start]
curconfig = start[:]
# for l in order:
# diff = sum((a-b)**2 for a,b in zip(limbstart[l],limbgoal[l]))
# if diff > 1e-8:
# if printer:
# print "< Planning for limb",l,">"
# print " Euclidean distance:",math.sqrt(diff)
# self.robot.setConfig(curconfig)
# #do the limb planning
# limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],printer=printer, iks=iks)
# if limbpath == 1 or limbpath == 2 or limbpath == False:
# if printer:
# print " Failed to plan for limb",l,"\n"
# return limbpath
# if printer:
# print " Planned successfully for limb",l, "\n"
# #concatenate whole body path
# for qlimb in limbpath[1:]:
# q = path[-1][:]
# self.set_limb_config(l,qlimb,q)
# path.append(q)
# self.set_limb_config(l,limbgoal[l],curconfig)
diff = sum((a-b)**2 for a,b in zip(limbstart[l],limbgoal[l]))
if diff > 1e-8:
if printer:
print "< Planning for limb",l,">"
# print " Euclidean distance:",math.sqrt(diff)
self.robot.setConfig(curconfig)
#do the limb planning
if iks == None:
limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],printer=printer, iks=iks, ignoreColShelfSpatula = ignoreColShelfSpatula)
else:
limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],printer=printer, iks=iks)
if limbpath == 1 or limbpath == 2 or limbpath == False:
if printer:
print " Failed to plan for limb",l,"\n"
return limbpath
if printer:
print " Planned successfully for limb",l, "\n"
#concatenate whole body path
for qlimb in limbpath[1:]:
q = path[-1][:]
self.set_limb_config(l,qlimb,q)
path.append(q)
self.set_limb_config(l,limbgoal[l],curconfig)
return path
class LimbPlanner:
"""Much of your code for HW4 will go here.
Attributes:
- world: the WorldModel
- robot: the RobotModel
- collider: a WorldCollider object (see the klampt.robotcollide module)
"""
def __init__(self,world):
self.world = world
self.robot = world.robot(0)
self.collider = WorldCollider(world)
self.roadmap = ([],[])
self.dynamic_objects = []
def check_collision_free(self,q):
self.robot.setConfig(q)
armfilter = None
collindices = set(range(0,self.robot.numLinks()))
#print "collindices", collindices
armfilter = lambda x:isinstance(x,RobotModelLink) and (x.index in collindices)
#check with objects in world model (includes the plane)
for o1,o2 in self.collider.collisionTests(armfilter,lambda x:True):
# print "Collision Test: Collision between",o1[0].getName(),o2[0].getName()
#print "Collision Test: Collision for ",o2[0].getName()
#print o1[0].index, o2[0].index
if o1[1].collides(o2[1]):
# print "Collision between",o1[0].getName(),o2[0].getName()
# print "Collision between",o1[0].index,o2[0].index
return False
for obj in self.dynamic_objects:
# print "checking collision with objects:", obj.getName()
# print obj.info.geometry
assert obj.geometry() != None
for link in collindices:
if self.robot.link(link).geometry().collides(obj.geometry()):
# print "Collision between link",self.robot.link(link).getName()," and dynamic object"
return False
return True
def rebuild_dynamic_objects(self):
item = self.world.rigidObject(0)
assert item.geometry() != None
# item.info.geometry.setCurrentTransform(*item.xform)
self.dynamic_objects.append(item)
return
def plan_limb(self,start,goal, 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)
if iks != None:
print "Initializing ClosedLoopCSPace"
cspace = ClosedLoopCSpaceTest(self,limb,iks)
if not cspace.feasible(start):
print " Start configuration is infeasible!"
return 1
if not cspace.feasible(goal):
print " Goal configuration is infeasible!"
return 2
# MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
# plan = MotionPlan(cspace,'sbl')
plan = MotionPlan(cspace)
plan.setEndpoints(start,goal)
# maxPlanIters = 20
# maxSmoothIters = 100
maxPlanIters = 100
maxSmoothIters = 1000
# print "start =",start,"\n","goal = ", goal
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 "iter:",iters,"--",len(V),"feasible milestones sampled,",len(E),"edges connected"
# print iters,"/V",len(V),"/E",len(E),
# print len(V),".",
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(self,start,goal,printer=True, iks = None, ignoreColShelfSpatula = True):
"""Plans a motion for the robot to move from configuration start
to configuration goal. By default, moves the left arm first,
then the right. To move the right first, set the 'order' argument
to ['right','left']"""
path = [start]
curconfig = start[:]
diff = sum((a-b)**2 for a,b in zip(start,goal))
if diff > 1e-8:
if printer:
print "< Planning ... >"
# print " Euclidean distance:",math.sqrt(diff)
self.robot.setConfig(curconfig)
# #do the limb planning
# if not ignoreColShelfSpatula:
# self.left_arm_indices = left_arm_geometry_indices + left_hand_geometry_indices + [55,56,57]
# else:
# self.left_arm_indices = left_arm_geometry_indices + left_hand_geometry_indices
# print " Left arm links", self.left_arm_indices
# if iks == None:
path = self.plan_limb(start,goal,printer=printer)
# else:
# trajectory = self.plan_closedLoop(start,goal,iks=iks)
# return trajectory
if path == 1 or path == 2 or path == False:
if printer:
print " Failed to plan (type =", path,")", "\n"
return None
if printer:
print " Planned successfully\n"
#concatenate whole body path
# for qlimb in limbpath[1:]:
# q = path[-1][:]
# self.set_limb_config(l,qlimb,q)
# path.append(q)
# self.set_limb_config(l,limbgoal[l],curconfig)
return path
def plan_closedLoop(self,qstart,qgoal,iks,printer=False):
if printer:
print "starting config: ", qstart
print "goal config: ", qgoal
self.world.terrain(0).geometry().setCollisionMargin(0.05)
cspace = robotcspace.ClosedLoopRobotCSpace(self.robot, iks, self.collider)
cspace.eps = 1e-3
cspace.setup()
configs=[]
configs.append(qstart)
configs.append(qgoal)
configs[0] = cspace.solveConstraints(configs[0])
configs[1] = cspace.solveConstraints(configs[1])
settings = { 'type':"sbl", 'perturbationRadius':0.5, 'bidirectional':1, 'shortcut':1, 'restart':1, 'restartTermCond':"{foundSolution:1,maxIters:1000}" }
wholepath = [configs[0]]
for i in range(len(configs)-1):
#this code uses the robotplanning module's convenience functions
self.robot.setConfig(configs[i])
plan = robotplanning.planToConfig(self.world,self.robot,configs[i+1],
movingSubset='all',
**settings)
if plan is None:
return False
print " Planning..."
keepPlanning = True
while keepPlanning:
plan.planMore(500)
#this code just gives some debugging information. it may get expensive
# V,E = plan.getRoadmap()
# self.roadmap = 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"
#debug some sampled configurations
# print V[0:max(10,len(V))]
# return False
else:
keepPlanning = False
self.pathToDraw = path
#the path is currently a set of milestones: discretize it so that it stays near the contact surface
path = cspace.discretizePath(path,epsilon=1e-2)
# path = cspace.discretizePath(path,epsilon=1e-4)
wholepath += path[1:]
#to be nice to the C++ module, do this to free up memory
plan.space.close()
plan.close()
return wholepath
class LimbPlanner:
"""Much of your code for HW4 will go here.
Attributes:
- world: the WorldModel
- robot: the RobotModel
- knowledge: the KnowledgeBase objcet
- collider: a WorldCollider object (see the klampt.robotcollide module)
"""
def __init__(self,world, vacuumPc):
self.world = world
self.robot = world.robot(0)
self.collider = WorldCollider(world)
self.vacuumPc = vacuumPc
#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_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]
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.dynamic_objects = []
def set_limb_config(self,limb,limbconfig,q):
"""Helper: Sets the 7-DOF configuration of the given limb in
q. Other DOFs are not touched."""
assert len(q) == self.robot.numLinks()
if limb=='left':
for (i,v) in zip(self.left_arm_indices,limbconfig):
q[i] = v
else:
for (i,v) in zip(self.right_arm_indices,limbconfig):
q[i] = v
def get_limb_config(self,q,limb):
"""Helper: Gets the 7-DOF configuration of the given limb given
a full-robot configuration q"""
qlimb = [0.0]*len(self.left_arm_indices)
if limb=='left':
for (i,j) in enumerate(self.left_arm_indices):
qlimb[i] = q[j]
else:
for (i,j) in enumerate(self.right_arm_indices):
qlimb[i] = q[j]
return qlimb
def check_collision_free(self,limb):
"""Checks whether the given limb is collision free at the robot's
current configuration"""
armfilter = None
left_arm_geometry_indices = [15,16,17,18,19,21,22,23,30,31]
right_arm_geometry_indices = [35,36,37,38,39,41,42,43,50,51]
left_hand_geometry_indices = [54,55,56]
right_hand_geometry_indices = [57,58,59]
if limb=='left':
collindices = set(left_arm_geometry_indices+left_hand_geometry_indices)
else:
collindices = set(right_arm_geometry_indices+right_hand_geometry_indices)
armfilter = lambda x:isinstance(x,RobotModelLink) and (x.index in collindices)
#check with objects in world model
for o1,o2 in self.collider.collisionTests(armfilter,lambda x:True):
if o1[1].collides(o2[1]):
#print "Collision between",o1[0].getName(),o2[0].getName()
return False
for obj in self.dynamic_objects:
assert obj.info.geometry != None
for link in collindices:
if self.robot.getLink(link).geometry().collides(obj.info.geometry):
print "Collision between link",self.robot.getLink(link).getName()," and dynamic object"
return False
for i in xrange(self.world.numRigidObjects()):
o = self.world.rigidObject(i)
g = o.geometry()
if g != None and g.type()!="":
if self.vacuumPc.withinDistance(g, .03):
return False
return True
def rebuild_dynamic_objects(self):
pass
# self.dynamic_objects = []
# #check with objects in knowledge
# for (k,objList) in self.knowledge.bin_contents.iteritems():
# if objList == None:
# #not sensed
# continue
# for item in objList:
# assert item.info.geometry != None
# item.info.geometry.setCurrentTransform(*item.xform)
# self.dynamic_objects.append(item)
# return
def check_limb_collision_free(self,limb,limbconfig):
"""Checks whether the given 7-DOF limb configuration is collision
free, keeping the rest of self.robot fixed."""
q = self.robot.getConfig()
self.set_limb_config(limb,limbconfig,q)
self.robot.setConfig(q)
return self.check_collision_free(limb)
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)
cspace.setup()
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()
valid_path = True
if path != None:
for milestone in path:
if not cspace.feasible(milestone):
valid_path = False
if path != None and valid_path:
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
def plan(self,start,goal,order=['left','right']):
"""Plans a motion for the robot to move from configuration start
to configuration goal. By default, moves the left arm first,
then the right. To move the right first, set the 'order' argument
to ['right','left']"""
limbstart = {}
limbgoal = {}
for l in ['left','right']:
limbstart[l] = self.get_limb_config(start,l)
limbgoal[l] = self.get_limb_config(goal,l)
path = [start]
curconfig = start[:]
for l in order:
diff = sum((a-b)**2 for a,b in zip(limbstart[l],limbgoal[l]))
if diff > 1e-8:
print "Planning for limb",l
print " Euclidean distance:",math.sqrt(diff)
self.robot.setConfig(curconfig)
#do the limb planning
limbpath = self.plan_limb(l,limbstart[l],limbgoal[l])
if limbpath == False:
print " Failed to plan for limb",l
return None
print " Planned successfully for limb",l
#concatenate whole body path
for qlimb in limbpath[1:]:
q = path[-1][:]
self.set_limb_config(l,qlimb,q)
path.append(q)
self.set_limb_config(l,limbgoal[l],curconfig)
return path
