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 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):
GLWidgetProgram.__init__(self, "Manual poser")
#start up the poser in the currently commanded configuration
q = motion.robot.getKlamptCommandedPosition()
world.robot(0).setConfig(q)
self.world = world
self.robot = world.robot(0)
self.robotPoser = RobotPoser(world.robot(0))
self.addWidget(self.robotPoser)
self.roadMap = ([], [])
robot = world.robot(0)
left_arm_link_names = [
'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
'left_wrist'
]
right_arm_link_names = [
'right_upper_shoulder', 'right_lower_shoulder',
'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
'right_lower_forearm', 'right_wrist'
]
self.left_arm_link_names = left_arm_link_names
self.right_arm_link_names = right_arm_link_names
missing_left_arm_names = [
'left_upper_forearm_visual', 'left_upper_elbow_visual'
]
missing_right_arm_names = [
'right_upper_forearm_visual', 'right_upper_elbow_visual'
]
#self.left_arm_link_names+=missing_left_arm_names
#self.right_arm_link_names+=missing_right_arm_names
self.left_arm_link_names = [
'left_upper_forearm', 'left_lower_forearm', 'left_wrist',
'left_upper_forearm_visual', 'left_gripper:base'
]
self.right_arm_link_names = [
'right_upper_forearm', 'right_lower_forearm', 'right_wrist',
'right_upper_forearm_visual', 'right_gripper:base'
]
self.left_arm_link_indices = [
robot.link(l).index for l in left_arm_link_names
]
self.right_arm_link_indices = [
robot.link(l).index for l in right_arm_link_names
]
self.firstTimeHack = True
self.world.makeTerrain('terrain')
self.subscribe()
self.newPC = None
self.collider = WorldCollider(self.world)
def getClosestConfig(self, robot, target, iterations, c, numsteps):
cost = 9999
res = None
start = robot.getConfig()
#goal = ik.objective(robot.link("left_gripper:base"), local = [(0,0,0)], world = [target])
#goal = ik.objective(robot.link("left_wrist"), local = [(0,0,0)], world = [target])
s = IKSolver(robot)
objective = IKObjective()
objective.setFixedPoint(
robot.link("left_gripper:base").getIndex(), (0, 0, 0), target)
s.add(objective)
s.setActiveDofs([12, 13, 14, 15, 16, 18, 19])
print "Active DOFs:", s.getActiveDofs()
for i in range(iterations):
#if ik.solve(goal):
(ret, iters) = s.solve(100, 1e-4)
if ret:
end = robot.getConfig()
print "*****************************", end
qmin, qmax = robot.getJointLimits()
#print qmin
#print qmax
flag = False
for k in range(len(end)):
if end[k] < qmin[k] or end[k] > qmax[k]:
flag = True
break
if flag:
start = self.perturb(start, 0.1)
robot.setConfig(start)
continue
for j in xrange(numsteps + 1):
u = float(j) / numsteps
print "u is:", u
q = robot.interpolate(end, start, u)
#q = end
print "interpolated q is:", q
if not inCollision(WorldCollider(self.world), robot, q):
newcost = vectorops.distance(
q, end) + c * vectorops.distance(q, start)
if newcost < cost:
res = q
cost = newcost
break
start = self.perturb(start, 0.1)
robot.setConfig(start)
print "res is:", res
return res
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 __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]
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]
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example toggles simulation / movie mode
if c == 'h':
print '[space]: send the current posed milestone'
print 'q: clean quit'
elif c == ' ':
q = self.robotPoser.get()
q0 = motion.robot.getKlamptCommandedPosition()
t1 = time.time()
#collisions = obstaclecollision(WorldCollider(self.world),self.world.robot(0),q0,q)
collides = bisection(WorldCollider(self.world),
self.world.robot(0), q0, q)
print "Obstacle collision detection done in time", +time.time() - t1
exit(0)
for i in range(self.world.robot(0).numLinks()):
self.world.robot(0).link(i).appearance().setColor(
0.5, 0.5, 0.5, 1)
#if not self.firstTimeHack and selfcollision(self.world.robot(0),q0,q):
if collides:
print "Invalid configuration, it self-collides"
elif not self.firstTimeHack and collisions != None:
#clear all links to gray
for pairs in collisions:
print "Link " + str(
pairs[0].getIndex()) + " collides with obstacle"
self.world.robot(0).link(
pairs[0].getIndex()).appearance().setColor(1, 0, 0, 1)
else:
self.firstTimeHack = False
robot = motion.robot
robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
qlg = robot.left_gripper.configFromKlampt(q)
qrg = robot.right_gripper.configFromKlampt(q)
robot.left_gripper.command(qlg, [1] * len(qlg), [1] * len(qlg))
robot.right_gripper.command(qrg, [1] * len(qrg),
[1] * len(qrg))
#robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices])
#robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices])
elif c == 'q':
motion.robot.shutdown()
exit(0)
else:
GLWidgetProgram.keyboardfunc(self, c, x, y)
self.refresh()
def getClosestConfig(self, robot, target, iterations, c, numsteps):
""" given a target object position, returns a configuration with end effector close to the target object but without colliding with it"""
cost = 9999
res = None
start = robot.getConfig()
s = IKSolver(robot)
objective = IKObjective()
objective.setFixedPoint(
robot.link("left_gripper:base").getIndex(), (0, 0, 0), target)
s.add(objective)
s.setActiveDofs([12, 13, 14, 15, 16, 18, 19])
for i in range(iterations):
(ret, iters) = s.solve(100, 1e-4)
if ret:
end = robot.getConfig()
qmin, qmax = robot.getJointLimits()
flag = False
for k in range(len(end)):
if end[k] < qmin[k] or end[k] > qmax[k]:
flag = True
break
if flag:
start = self.perturb(start, 0.1)
robot.setConfig(start)
continue
for j in xrange(numsteps + 1):
u = float(j) / numsteps
q = robot.interpolate(end, start, u)
if not inCollision(WorldCollider(self.world), robot, q):
newcost = vectorops.distance(
q, end) + c * vectorops.distance(q, start)
if newcost < cost:
res = q
cost = newcost
break
start = self.perturb(start, 0.1)
robot.setConfig(start)
return res
def __init__(self,world,robotController):
self.world = world
self.robot = world.robot(0)
self.controller = robotController
self.knowledge = KnowledgeBase()
self.planner = LimbPlanner(self.world,self.knowledge)
self.state = 'ready'
self.active_limb = None
self.active_grasp = None
self.current_bin = None
self.held_object = None
#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]
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.collider = WorldCollider(world)
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()
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
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example toggles simulation / movie mode
if c == 'h':
print '[space]: send the current posed milestone'
print 'q: clean quit'
elif c == ' ':
q = self.robotPoser.get()
#print "space q:", q
q0 = motion.robot.getKlamptCommandedPosition()
#print q0
#collider = WorldCollider(self.world)
collisions = obstaclecollision(WorldCollider(self.world),
self.world.robot(0), q0, q)
for i in range(self.world.robot(0).numLinks()):
self.world.robot(0).link(i).appearance().setColor(
0.5, 0.5, 0.5, 1)
if not self.firstTimeHack and selfcollision(
self.world.robot(0), q0, q):
print "Invalid configuration, it self-collides"
elif not self.firstTimeHack and collisions != None:
#clear all links to gray
for pairs in collisions:
print "Link " + str(
pairs[0].getIndex()) + " collides with obstacle"
self.world.robot(0).link(
pairs[0].getIndex()).appearance().setColor(1, 0, 0, 1)
else:
self.firstTimeHack = False
robot = motion.robot
robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
print
print
print "Moving", q0, "->", q
print
print
qlg = robot.left_gripper.configFromKlampt(q)
qrg = robot.right_gripper.configFromKlampt(q)
print "space prg:", qrg
robot.left_gripper.command(qlg, [1] * len(qlg), [1] * len(qlg))
robot.right_gripper.command(qrg, [1] * len(qrg),
[1] * len(qrg))
#robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices])
#robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices])
elif c == 'q':
motion.robot.shutdown()
exit(0)
elif c == 'p':
"""given a target object position, automatically moves the end effector close to the target object"""
#print "Joint limits:", self.robot.getJointLimits()
start = self.world.robot(0).getConfig()
#print "end effector original position:", self.robot.link('left_gripper:base').getWorldPosition((0,0,0))
#print "start", start
"""target position currently fixed at (0.8,0.1,1) for testing"""
target = self.getClosestConfig(self.world.robot(0), (0.8, 0.1, 1),
100, 0.1, 100)
if target == None:
# print "Cannot solve IK"
return
self.robot.setConfig(target)
#print "ik result:", target
#print "end effector position:", self.robot.link('left_gripper:base').getWorldPosition((0,0,0))
path = self.getCollisionFreePath(start, target, 10)
#print "path:", path
for q in path:
robot = motion.robot
robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
qlg = robot.left_gripper.configFromKlampt(q)
qrg = robot.right_gripper.configFromKlampt(q)
if qlg:
robot.left_gripper.command(qlg, [1] * len(qlg),
[1] * len(qlg))
if qrg:
robot.right_gripper.command(qrg, [1] * len(qrg),
[1] * len(qrg))
print "getKlamptCommandedPosition:", robot.getKlamptCommandedPosition(
)
else:
GLWidgetProgram.keyboardfunc(self, c, x, y)
self.refresh()
class PickingController:
"""Maintains the robot's knowledge base and internal state. Most of
your code will go here. Members include:
- knowledge: a KnowledgeBase object
- planner: an LimbPlanner object, which *you will implement and use*
- state: either 'ready', or 'holding'
- configuration: the robot's current configuration
- active_limb: the limb currently active, either holding or viewing a state
- current_bin: the name of the bin where the camera is viewing or the gripper is located
- held_object: the held object, if one is held, or None otherwise
External modules can call viewBinAction(), graspAction(), ungraspAction(),
and placeInOrderBinAction()
"""
def __init__(self,world,robotController):
self.world = world
self.robot = world.robot(0)
self.controller = robotController
self.knowledge = KnowledgeBase()
self.planner = LimbPlanner(self.world,self.knowledge)
self.state = 'ready'
self.active_limb = None
self.active_grasp = None
self.current_bin = None
self.held_object = None
#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]
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.collider = WorldCollider(world)
def waitForMove(self,timeout = None, pollRate = 0.5):
"""Waits for the move to complete, or timeout seconds is elapsed,
before terminating."""
iters = 0
t = 0
while self.controller.isMoving():
if iters % 10 == 0:
print "Waiting for move to complete..."
time.sleep(pollRate)
t += pollRate
if timeout != None and t > timeout:
return False
iters += 1
return True
def viewBinAction(self,b):
self.waitForMove()
if self.state != 'ready':
print "Already holding an object, can't move to bin"
return False
else:
if b in apc.bin_names:
if self.move_camera_to_bin(b):
self.waitForMove()
self.current_bin = b
run_perception_on_bin(self.knowledge,b)
print "Sensed bin",b,"with camera",self.active_limb
else:
print "Move to bin",b,"failed"
return False
else:
print "Invalid bin",b
return False
return True
def graspAction(self):
self.waitForMove()
self.controller.commandGripper(self.active_limb,[1])
self.waitForMove()
if self.current_bin == None:
print "Not located at a bin"
return False
elif self.state != 'ready':
print "Already holding an object, can't grasp another"
return False
elif len(self.knowledge.bin_contents[self.current_bin])==0:
print "The current bin is empty"
return False
else:
if self.move_to_grasp_object(self.knowledge.bin_contents[self.current_bin][0]):
self.waitForMove()
#now close the gripper
self.controller.commandGripper(self.active_limb,self.active_grasp.gripper_close_command)
self.waitForMove()
self.held_object = self.knowledge.bin_contents[self.current_bin].pop(0)
self.state = 'holding'
print "Holding object",self.held_object.info.name,"in hand",self.active_limb
return True
else:
print "Grasp failed"
return False
def ungraspAction(self):
self.waitForMove()
if self.state != 'holding':
print "Not holding an object"
return False
else:
if self.move_to_ungrasp_object(self.held_object):
self.waitForMove()
#now open the gripper
self.controller.commandGripper(self.active_limb,self.active_grasp.gripper_open_command)
self.waitForMove()
print "Object",self.held_object.info.name,"placed back in bin"
self.knowledge.bin_contents[self.current_bin].append(self.held_object)
self.state = 'ready'
self.held_object = None
return True
else:
print "Ungrasp failed"
return False
def placeInOrderBinAction(self):
self.waitForMove()
if self.state != 'holding':
print "Not holding an object"
else:
if self.move_to_order_bin(self.held_object):
self.waitForMove()
#now open the gripper
self.controller.commandGripper(self.active_limb,self.active_grasp.gripper_open_command)
self.waitForMove()
print "Successfully placed",self.held_object.info.name,"into order bin"
self.knowledge.order_bin_contents.append(self.held_object)
self.held_object.xform = None
self.held_object.bin_name = 'order_bin'
self.state = 'ready'
self.held_object = None
return True
else:
print "Move to order bin failed"
return False
def fulfillOrderAction(self,objectList):
"""Given a list of objects to be put in the order bin, run
until completed."""
remainingObjects = objectList
for b in apc.bin_names:
if self.knowledge.bin_contents[b]==None:
if not self.viewBinAction(b):
print "Could not view bin",b
continue
donextbin = False
while any(o.info.name in remainingObjects for o in self.knowledge.bin_contents[b]) and not donextbin:
#pick up and put down objects until you are holding one that is in the remainingObjects list
if not self.graspAction():
print "Error grasping object"
donextbin = True
break
while not donextbin and (self.held_object == None or self.held_object.info.name not in remainingObjects):
#cycle through objects by putting down and picking up the next object
if not self.ungraspAction():
print "Error putting down object"
return False
if not self.graspAction():
print "Error grasping object"
donextbin = True
break
obj = self.held_object
if self.placeInOrderBinAction():
remainingObjects.remove(obj.info.name)
else:
print "Error putting object into order bin"
return False
if len(remainingObjects)==0:
return True
print "These items are remaining from the order:",remainingObjects
return False
def randomize_limb_position(self,limb,range=None):
"""Helper: randomizes the limb configuration in self.robot.
limb can be 'left' or 'right'. If range is provided, then
this samples in a range around the current commanded config"""
qmin,qmax = self.robot.getJointLimits()
if range == None:
q = baxter_rest_config[:]
if limb == 'left':
for j in self.left_arm_indices:
q[j] = random.uniform(qmin[j],qmax[j])
else:
for j in self.right_arm_indices:
q[j] = random.uniform(qmin[j],qmax[j])
self.robot.setConfig(q)
else:
q = self.controller.getCommandedConfig()
if limb == 'left':
for j in self.left_arm_indices:
q[j] = max(qmin[j],min(qmax[j],random.uniform(q[j]-range,q[j]+range)))
else:
for j in self.right_arm_indices:
q[j] = max(qmin[j],min(qmax[j],random.uniform(q[j]-range,q[j]+range)))
self.robot.setConfig(q)
return
def move_camera_to_bin(self,bin_name):
"""Starts a motion so the camera has a viewpoint that
observes bin_name. Will also change self.active_limb to the
appropriate limb.
If successful, sends the motion to the low-level controller and
returns True.
Otherwise, does not modify the low-level controller and returns False.
"""
world_offset = self.knowledge.bin_vantage_point(bin_name)
#place +z in the +x axis, y in the +z axis, and x in the -y axis
left_goal = ik.objective(self.left_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
right_goal = ik.objective(self.right_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
qcmd = self.controller.getCommandedConfig()
collisionTries = 0
for i in range(100):
if random.random() < 0.5:
oldConfig = self.robot.getConfig()
if i == 0:
self.robot.setConfig(qcmd)
else:
self.randomize_limb_position('left')
if ik.solve(left_goal):
#TODO: plan a path
# check that this solution is self collision free
# we call the self collision function of WorldCollider
# and see how many items the generator returns. If it is 0
# then we don't collide (if it is not zero, we don't care what
# they are, we throw the solution away and retry with a different
# initial arm configuration).
collisionTries += 1
collisions = self.collider.robotSelfCollisions(robot = self.robot)
numCols = 0
for cols in collisions:
numCols += 1
self.controller.setMilestone(self.robot.getConfig())
if numCols == 0:
self.controller.setMilestone(self.robot.getConfig())
self.active_limb = 'left'
print "Found self collision free IK solution after " + str(collisionTries) + " tries"
return True
else:
if i == 0:
self.robot.setConfig(qcmd)
else:
self.randomize_limb_position('right')
if ik.solve(right_goal):
#TODO: plan a path
# check that this solution is self collision free
# we call the self collision function of WorldCollider
# and see how many items the generator returns. If it is 0
# then we don't collide (if it is not zero, we don't care what
# they are, we throw the solution away and retry with a different
# initial arm configuration).
collisionTries += 1
collisions = self.collider.robotSelfCollisions(robot = self.robot)
numCols = 0
for cols in collisions:
numCols += 1
if numCols == 0:
self.controller.setMilestone(self.robot.getConfig())
self.active_limb = 'right'
print "Found self collision free IK solution after " + str(collisionTries) + " tries"
return True
return False
def move_to_grasp_object(self,object):
"""Sets the robot's configuration so the gripper grasps object at
one of its potential grasp locations. Might change self.active_limb
to the appropriate limb. Must change self.active_grasp to the
selected grasp.
If successful, sends the motion to the low-level controller and
returns True.
Otherwise, does not modify the low-level controller and returns False.
"""
collisionTries = 0
grasps = self.knowledge.grasp_xforms(object)
qmin,qmax = self.robot.getJointLimits()
qcmd = self.controller.getCommandedConfig()
#phase 1: init IK from the commanded config, search among grasps
for (grasp,gxform) in grasps:
if self.active_limb == 'left':
Tg = se3.mul(gxform,se3.inv(left_gripper_center_xform))
goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1])
else:
Tg = se3.mul(gxform,se3.inv(right_gripper_center_xform))
goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1])
self.robot.setConfig(qcmd)
if ik.solve(goal):
# check that this solution is self collision free
# we call the self collision function of WorldCollider
# and see how many items the generator returns. If it is 0
# then we don't collide (if it is not zero, we don't care what
# they are, we throw the solution away and retry with a different
# initial arm configuration).
collisionTries += 1
collisions = self.collider.robotSelfCollisions(robot = self.robot)
numCols = 0
for cols in collisions:
numCols += 1
if numCols == 0:
self.controller.setMilestone(self.robot.getConfig())
self.active_grasp = grasp
print "Found self collision free IK solution after " + str(collisionTries) + " tries"
return True
#Phase 2: that didn't work, now try random sampling
for i in range(100):
#pick a config at random
self.randomize_limb_position(self.active_limb)
#pick a grasp at random
(grasp,gxform) = random.choice(grasps)
if self.active_limb == 'left':
Tg = se3.mul(gxform,se3.inv(left_gripper_center_xform))
goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1])
else:
Tg = se3.mul(gxform,se3.inv(right_gripper_center_xform))
goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1])
if ik.solve(goal):
#TODO: plan a path
# check that this solution is self collision free
# we call the self collision function of WorldCollider
# and see how many items the generator returns. If it is 0
# then we don't collide (if it is not zero, we don't care what
# they are, we throw the solution away and retry with a different
# initial arm configuration).
collisionTries += 1
collisions = self.collider.robotSelfCollisions(robot = self.robot)
numCols = 0
for cols in collisions:
numCols += 1
self.controller.setMilestone(self.robot.getConfig())
if numCols == 0:
self.controller.setMilestone(self.robot.getConfig())
self.active_grasp = grasp
print "Found self collision free IK solution after " + str(collisionTries) + " tries"
return True
return False
def move_to_ungrasp_object(self,object):
"""Sets the robot's configuration so the gripper ungrasps the object.
If successful, sends the motion to the low-level controller and
returns True.
Otherwise, does not modify the low-level controller and returns False.
"""
assert len(object.info.grasps) > 0,"Object doesn't define any grasps"
return True
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, sends the motion to the low-level controller and
returns True.
Otherwise, does not modify the low-level controller and returns False.
"""
collisionTries = 0
left_target = se3.apply(order_bin_xform,[0.0,0.2,order_bin_bounds[1][2]+0.1])
right_target = se3.apply(order_bin_xform,[0.0,-0.2,order_bin_bounds[1][2]+0.1])
qcmd = self.controller.getCommandedConfig()
for i in range(100):
if self.active_limb == 'left':
goal = ik.objective(self.left_gripper_link,local=left_gripper_center_xform[1],world=left_target)
else:
goal = ik.objective(self.right_gripper_link,local=right_gripper_center_xform[1],world=right_target)
#set IK solver initial configuration
if i==0:
self.robot.setConfig(qcmd)
else:
self.randomize_limb_position(self.active_limb)
#solve
if ik.solve(goal,tol=0.1):
#TODO: plan a path
# check that this solution is self collision free
# we call the self collision function of WorldCollider
# and see how many items the generator returns. If it is 0
# then we don't collide (if it is not zero, we don't care what
# they are, we throw the solution away and retry with a different
# initial arm configuration).
collisionTries += 1
collisions = self.collider.robotSelfCollisions(robot = self.robot)
numCols = 0
for cols in collisions:
numCols += 1
self.controller.setMilestone(self.robot.getConfig())
if numCols == 0:
self.controller.setMilestone(self.robot.getConfig())
print "Found self collision free IK solution after " + str(collisionTries) + " tries"
return True
return False
for i in range(world.numTerrains()):
visualization.add("terrain" + str(i), world.terrain(i))
#if you want to just see the robot in a pop up window...
if DO_SIMPLIFY and DEBUG_SIMPLIFY:
visualization.dialog()
#Automatic construction of space
if not CLOSED_LOOP_TEST:
if not MANUAL_SPACE_CREATION:
space = robotplanning.makeSpace(world=world,
robot=robot,
edgeCheckResolution=1e-2,
movingSubset='all')
else:
#Manual construction of space
collider = WorldCollider(world)
space = robotcspace.RobotCSpace(robot, collider)
space.eps = 1e-2
space.setup()
else:
#TESTING: closed loop robot cspace
collider = WorldCollider(world)
obj = ik.objective(robot.link(robot.numLinks() - 1),
local=[0, 0, 0],
world=[0.5, 0, 0.5])
visualization.add("IK goal", obj)
visualization.dialog()
space = robotcspace.ClosedLoopRobotCSpace(robot, obj, collider)
space.eps = 1e-2
space.setup()
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
def __init__(self,world):
self.world = world
self.robot = world.robot(0)
self.collider = WorldCollider(world)
self.roadmap = ([],[])
self.dynamic_objects = []
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
def reinit_collider(self):
self.collider = WorldCollider(self.world)
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)
self.solutions = 0
self.collisions = 0
#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]
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]
def reinit_collider(self):
self.collider = WorldCollider(self.world)
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"""
self.solutions += 1
# Use collider to determine collisions and return false if one has been found
selfCollisionIterator = self.collider.robotSelfCollisions(self.robot)
for collision in selfCollisionIterator:
print "self collision detected", collision[0].index, collision[1].index
self.collisions += 1
return False
# Get list of all terrain indices
terrainIds = []
for i in range(self.world.numTerrains()):
terrainIds.append(self.world.terrain(i).getID())
# Create filter based on indices
terrainFilter = lambda x: x.getID() in terrainIds
# Create filter for robot links
if limb == 'left':
robotLinkFilter = lambda x: x.index in left_arm_geometry_indices
else:
robotLinkFilter = lambda x: x.index in right_arm_geometry_indices
# Check terrain collisions
for collision in self.collider.collisions(filter1=terrainFilter, filter2=robotLinkFilter):
print "terrain robot collision detected", collision[0].index, collision[1].index
return False
# Get list of all object IDs
objectIDs = []
for i in range(self.world.numRigidObjects()):
objectIDs.append(self.world.rigidObject(i).getID())
# Create filter for object IDs
objectFilter = lambda x: x.getID() in objectIDs
# Check object and robot link collisions
for collision in self.collider.collisions(filter1=objectFilter, filter2=robotLinkFilter):
print "object robot collision detected", collision[0].index, collision[1].index
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):
"""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(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:
if limbstart[l] != limbgoal[l]:
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
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 = []
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
def updateWorld(self,new_world):
self.world = new_world
self.collider = WorldCollider(new_world)
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(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]
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]
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"""
for collision in self.collider.robotSelfCollisions(self.robot):
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):
"""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.
return [start,goal]
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:
if limbstart[l] != limbgoal[l]:
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
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
