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.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 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
