def __init__(self, planner, limb, collider=None):
RobotCSpace.__init__(self, planner.robot, collider=None)
self.planner = planner
self.limb = limb
self.robot = planner.robot
id_to_index = dict([(self.robot.link(i).getID(),i) for i in range(self.robot.numLinks())])
if limb=='left':
self.limb_indices = [id_to_index[i.getID()] for i in planner.left_arm_links]
else:
self.limb_indices = [id_to_index[i.getID()] for i in planner.right_arm_links]
qmin,qmax = self.robot.getJointLimits()
self.bound = [(qmin[i]-1e-6,qmax[i]+1e-6) for i in self.limb_indices]
self.eps = 1e-1
def feasible(self,q):
if not RobotCSpace.feasible(self,q):
print "LimbCSpace.feasible: Configuration is out of bounds"
return False
if not self.planner.check_limb_collision_free(self.limb,q):
print "LimbCSpace.feasible: Configuration is in collision"
return False
return True
