def feasible(self, q):
"""TODO: Implement this feasibility test. It is used by the motion planner to
determine whether the robot at configuration q is feasible."""
#modified bounds test: we don't care about angle
if not CSpace.feasible(self, (q[0], q[1], 0)): return False
self.robot.setConfig(q)
base = self.robot.link(2)
for o in self.obstacles:
if o.geometry().collides(base.geometry()): return False
return True
def feasible(self, q):
#bounds test
if not CSpace.feasible(self, (q[0], q[1], 0)): return False
#get the endpoints of the bar
p1, p2 = self.endpoints(q)
#TODO: implement your feasibility test here: the current implementation
#checks endpoints, but doesnt consider collisions with the
#intermediate segment between the endpoints
for o in self.obstacles:
if o.contains(p1) or o.contains(p2): return False
return True
def feasible(self,q):
"""TODO: Implement this feasibility test. It is used by the motion planner to
determine whether the robot at configuration q is feasible."""
#bounds test for each possible tangent location
tang_locs = [[q[0] + self.robot.radius, q[1]], [q[0] - self.robot.radius, q[1]],
[q[0], q[1] + self.robot.radius], [q[0], q[1] - self.robot.radius]]
for each_loc in tang_locs:
if not CSpace.feasible(self,each_loc):
return False
#make sure center point at least distance r from obstacles
for o in self.obstacles:
if o.distance(q) <= self.robot.radius:
return False
return True