def __init__(self,planner,limb):
CSpace.__init__(self)
self.planner = planner
self.limb = limb
#TODO: what Cartesian bounding box should the planner sample from?
self.bound = [(0,1),(0,1)]
#TODO: what collision tolerance to use?
self.eps = 1e-2
def feasible(self, q):
#print "checking feasible"
isfeasible = not inCollision(self.collider, self.robot,
q) and not self.robot.selfCollides()
print CSpace.feasible(self, q), q
if not CSpace.feasible(self, q): return False
return not inCollision(self.collider, self.robot,
q) and not self.robot.selfCollides()
def __init__(self,planner,limb):
CSpace.__init__(self)
self.planner = planner
self.limb = limb
# set up a search space based on the robot 1 m x 2 m x 2 m
(R, t) = self.planner.robot.getLink(0).getParentTransform()
self.bound = self.bounds = [(t[0]-1, t[0]+2), (t[1]-2, t[2]+2), (0, 3)]
self.eps = 1e-2
self.ignore = None
def __init__(self, collider, robot):
CSpace.__init__(self)
qmin, qmax = robot.getJointLimits()
self.bound = []
for i in range(len(qmin)):
self.bound.append((qmin[i], qmax[i]))
self.eps = 1e-3
self.collider = collider
self.robot = robot
def __init__(self):
CSpace.__init__(self)
#set bounds
self.bound = [(0.0, 1.0), (0.0, 1.0)]
#set collision checking resolution
self.eps = 1e-3
#setup a robot with radius 0.05
self.robot = Circle(0, 0, 0.05)
#set obstacles here
self.obstacles = []
def __init__(self):
CSpace.__init__(self)
#set bounds
self.bound = [(0.0, 1.0), (0.0, 1.0), (0.0, math.pi * 2)]
#set collision checking resolution
self.eps = 1e-3
#setup a bar with length 0.1
self.L = 0.1
#set obstacles here
self.obstacles = []
def __init__(self):
CSpace.__init__(self)
#set bounds
self.bound = [(0.0,1.0),(0.0,1.0),(0.0,math.pi*2)]
#set collision checking resolution
self.eps = 1e-3
#setup a bar with length 0.1
self.L = 0.1
#set obstacles here
self.obstacles = []
def __init__(self):
CSpace.__init__(self)
#set bounds
self.bound = [(0.0,1.0),(0.0,1.0)]
#set collision checking resolution
self.eps = 1e-3
#setup a robot with radius 0.05
self.robot = Circle(0,0,0.05)
#set obstacles here
self.obstacles = []
def __init__(self,planner,limb):
CSpace.__init__(self)
self.planner = planner
'''
:type self.planner: LimbPlanner
'''
self.limb = limb
#TODO: what Cartesian bounding box should the planner sample from?
self.bound = [(-pi,pi),(-pi,pi),(-pi,pi),(-pi,pi),(-pi,pi),(-pi,pi),(-pi,pi)]
#TODO: what collision tolerance to use?
self.eps = 1e-2
def __init__(self,planner):
CSpace.__init__(self)
self.planner = planner
#TODO: what Cartesian bounding box should the planner sample from?
self.robot = self.planner.robot
id_to_index = dict([(self.robot.link(i).getID(),i) for i in range(self.robot.numLinks())])
qmin,qmax = self.robot.getJointLimits()
self.bound = [(qmin[i],qmax[i]) for i in range(self.robot.numLinks())]
self.eps = 1e-3
def __init__(self,globals,hand):
CSpace.__init__(self)
self.globals = globals
self.robot = globals.robot
self.hand = hand
#initial whole-body configuratoin
self.q0 = self.robot.getConfig()
#setup CSpace sampling range
qlimits = zip(*self.robot.getJointLimits())
self.bound = [qlimits[i] for i in self.hand.armIndices]
#setup CSpace edge checking epsilon
self.eps = 1e-2
def __init__(self, globals, hand):
CSpace.__init__(self)
self.globals = globals
self.robot = globals.robot
self.hand = hand
#initial whole-body configuratoin
self.q0 = self.robot.getConfig()
#setup CSpace sampling range
qlimits = zip(*self.robot.getJointLimits())
self.bound = [qlimits[i] for i in self.hand.armIndices]
#setup CSpace edge checking epsilon
self.eps = 1e-2
def __init__(self,planner,limb):
CSpace.__init__(self)
self.planner = planner
self.limb = limb
#TODO: what Cartesian bounding box should the planner sample from?
self.robot = self.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 __init__(self,planner,limb):
CSpace.__init__(self)
self.planner = planner
self.limb = limb
#TODO: what Cartesian bounding box should the planner sample from?
get_limb_config(planner.robot.getConfig(), self.limb)
self.setup()
self.milestone.getPath
self.bound = [(0,1),(0,1)]
self.setBounds(self.bound)
#TODO: what collision tolerance to use?
self.eps = 1e-2
def feasible(self,q):
#TODO: implement me
#the default simply checks if q is within the bounds
return CSpace.feasible(self,q)
def __init__(self,planner,limb,obj,grasp):
CSpace.__init__(self)
self.planner = planner
self.limb = limb
self.object = obj
self.grasp = grasp
self.objectGeom = obj.info.geometry
self.robot = self.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 len(q) != len(self.bound):
# print len(q), len(self.bound)
qnew = q[0:len(self.bound)]
q=qnew
for i in range(len(q)):
# print "q", i, q[i], self.bound[i][0], self.bound[i][1]
if (q[i] < self.bound[i][0]) :
print "Joint #",self.limb_indices[i],"(",q[i],") out of limits (min:",self.bound[i][0],")"
print "Changed joint value to its minimum"
q[i] = self.bound[i][0]
if (q[i] > self.bound[i][1]) :
print "Joint #",self.limb_indices[i],"(",q[i],") out of limits (max:",self.bound[i][1],")"
print "Changed joint value to its maximum"
q[i] = self.bound[i][1]
if not CSpace.feasible(self,q):
# print "LimbCSpace.feasible: Configuration is out of bounds"
return False
# cond = self.planner.check_limb_collision_free(self.limb,q)
# if not cond:
if not self.planner.check_limb_collision_free(self.limb,q):
# print "LimbCSpace.feasible: Configuration is in collision"
return False
return True
def __init__(self, planner, limb):
CSpace.__init__(self)
self.planner = planner
self.limb = limb
# TODO: what Cartesian bounding box should the planner sample from?
self.robot = self.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 = left_arm_geometry_indices + left_hand_geometry_indices
self.limb_indices = self.planner.left_arm_indices
else:
# self.limb_indices = right_arm_geometry_indices + right_hand_geometry_indices
self.limb_indices = self.planner.right_arm_indices
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 __init__(self, globals, hand, object):
CSpace.__init__(self)
self.globals = globals
self.robot = globals.robot
self.hand = hand
self.object = object
#setup initial object-robot transform
Thand0 = self.robot.link(hand.link).getTransform()
Tobj0 = object.getTransform()
self.Tgrasp = se3.mul(se3.inv(Thand0), Tobj0)
#initial whole-body configuratoin
self.q0 = self.robot.getConfig()
#setup CSpace sampling range
qlimits = zip(*self.robot.getJointLimits())
self.bound = [qlimits[i] for i in self.hand.armIndices]
#setup CSpace edge checking epsilon
self.eps = 1e-2
def __init__(self,globals,hand,object):
CSpace.__init__(self)
self.globals = globals
self.robot = globals.robot
self.hand = hand
self.object = object
#setup initial object-robot transform
Thand0 = self.robot.link(hand.link).getTransform()
Tobj0 = object.getTransform()
self.Tgrasp = se3.mul(se3.inv(Thand0),Tobj0)
#initial whole-body configuratoin
self.q0 = self.robot.getConfig()
#setup CSpace sampling range
qlimits = zip(*self.robot.getJointLimits())
self.bound = [qlimits[i] for i in self.hand.armIndices]
#setup CSpace edge checking epsilon
self.eps = 1e-2
def feasible(self,q):
if not CSpace.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
def feasible(self, q):
#bounds test
if not CSpace.feasible(self, q): return False
#TODO: Problem 1: implement your feasibility tests here
#currently, only the center is checked, so the robot collides
#with boundary and obstacles
for o in self.obstacles:
if o.contains(q): return False
return True
def feasible(self,q):
#bounds test
if not CSpace.feasible(self,q): return False
#TODO: Problem 1: implement your feasibility tests here
#currently, only the center is checked, so the robot collides
#with boundary and obstacles
for o in self.obstacles:
if o.contains(q): return False
return True
def feasible(self, q):
# TODO: implement me
# the default simply checks if q is within the bounds
# check if the q is within the bounds
if CSpace.feasible(self, q):
if self.planner.check_limb_collision_free(self.limb, q, True):
return True
else:
return False
else:
return False
def feasible(self,q):
if not CSpace.feasible(self,q):
#print "LimbCSpace.feasible: Configuration is out of bounds"
return False
qrob = self.robot.getConfig()
self.planner.set_limb_config(self.limb,q,qrob)
self.robot.setConfig(qrob)
if not self.planner.check_collision_free_with_object(self.limb,self.objectGeom,self.grasp):
#print "LimbCSpace.feasible: Configuration is in collision"
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):
verbose = False#len(self.extra_feasibility_tests)>0
if verbose: print "Custom CSpace test"
if not CSpace.feasible(self,q):
if verbose: print "Failed CSpace test"
return False
for index,test in enumerate(self.extra_feasibility_tests):
if not test(q):
if verbose: print "Extra feasibility test",index,"returned false, returning configuration infeasible"
return False
if verbose: print "Passed"
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):
if not CSpace.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
for i in xrange(self.planner.world.numRigidObjects()):
o = self.planner.world.rigidObject(i)
g = o.geometry()
if g != None and g.type()!="":
if self.planner.vacuumPc.withinDistance(g, .03):
return False
return True
def feasible(self,q):
# set robot to given q
# get joint 55 orientation
# if rotation is not right, return false
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
# self.robot.setConfig(q)
# Rcur = self.robot.link(55).getTransform()[0]
# # Rgoal
# if len(q)>len(self.limb_indices): return False
# for i in range(len(q)):
# # print "q", i, q[i], self.bound[i][0], self.bound[i][1]
# # print self.limb, self.limb_indices, i, len(q)
# if (q[i] < self.bound[i][0]) :
# print "Joint #",self.limb_indices[i],"(",q[i],") out of limits (min:",self.bound[i][0],")"
# print "Changed joint value to its minimum"
# q[i] = self.bound[i][0]
# if (q[i] > self.bound[i][1]) :
# print "Joint #",self.limb_indices[i],"(",q[i],") out of limits (max:",self.bound[i][1],")"
# print "Changed joint value to its maximum"
# q[i] = self.bound[i][1]
# print 1
# # self.robot.setConfig(q)
# # return AdaptiveCSpace.feasible(self,q)
# print 2
# if not self.inJointLimits(q): return False
# print 3
# self.robot.setConfig(q)
# if not self.closedLoop(): return False;
# #check extras
# print 4
# for f in self.extraFeasibilityTesters:
# if not f(q): return False
# print 5
# #check collisions
# if self.selfCollision(): return False
# print 6
# if self.envCollision(): return False
# return True
if not CSpace.feasible(self,q):
print "CSpace.feasible: Configuration is out of bounds"
return False
if not self.planner.check_collision_free(self.limb,q):
# print "ClosedLoopRobotCSpace.feasible: Configuration is in collision"
return False
return True
def feasible(self,q):
#TODO: implement me
#the default simply checks if q is within the bounds
#print "Called"
#print q, CSpace.feasible(self,q)
return CSpace.feasible(self,q) and self.planner.check_limb_collision_free(self.limb, q)
def __init__(self):
CSpace.__init__(self)
self.extra_feasibility_tests = []
