def __init__(self,
bounds,
robot,
collider,
lamp,
milestones,
base_height_link=2,
robot_height=1.5,
float_height=0.08,
linear_dofs=[0, 1],
angular_dofs=[4, 5, 6, 7, 8],
light_local_position=[0, 0, 0]):
CSpace.__init__(self)
self.base_height_link = base_height_link
#set bounds
limits = robot.getJointLimits()
limits[0][0] = 0
limits[0][1] = 0
limits[0][self.base_height_link] = 0
limits[1][0] = bounds[0]
limits[1][1] = bounds[1]
limits[1][self.base_height_link] = 2
# we also set all the joint limits of all joints that are not active to be
# equal to their current positions:
cfig = robot.getConfig()
self.zero_cfig = np.array(cfig)
tmp = set(linear_dofs + angular_dofs)
tmp2 = set(list(range(len(cfig))))
fixed_dofs = list(tmp2.difference(tmp))
self.fixed_dofs = fixed_dofs
for fixed_dof in fixed_dofs:
limits[0][fixed_dof] = cfig[fixed_dof]
limits[1][fixed_dof] = cfig[fixed_dof]
robot.setJointLimits(limits[0], limits[1])
self.lamp = lamp
self.max_vals = limits[1] + [bounds[0], bounds[1], robot_height]
self.min_vals = limits[0] + [0, 0, 0]
bound = []
for a, b in zip(self.min_vals, self.max_vals):
bound.append((a, b))
self.bound = bound
self.milestones = milestones
#set collision checking resolution
self.eps = 0.01
#setup a robot with radius 0.05
self.robot = robot
#set obstacles here
self.collider = collider
self.remaining_milestones = self.milestones[:]
self.fraction = 1
self.remaining = set(range(self.remaining_milestones.shape[0]))
self.S = None
self.clf = None
self.linear_dofs = linear_dofs
self.angular_dofs = angular_dofs
self.local_lamp_coords = light_local_position
self.counter = 0
def __init__(self, xRange, yRange, minDistance):
CSpace.__init__(self)
#set bounds
self.bound = [(xRange[0], xRange[1]), (yRange[0], yRange[1])]
#set collision checking resolution
self.eps = 1e-3
#set obstacles here
self.obstacles = []
self.minDistance = minDistance # Sets the distance which a robot needs to exceed from any obstacle
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, x_bound=3.0, y_bound=3.0):
CSpace.__init__(self)
#set bounds
self.bound = [(-2.0, x_bound), (-2.0, y_bound)]
#set collision checking resolution
self.eps = 0.025
#setup a robot with radius 0.05
self.robot = Circle(0, 0, 0.05)
#set obstacles here
self.obstacles = []
self.fraction = 1
def __init__(self, globals):
CSpace.__init__(self)
self.globals = globals
self.robot = globals.robot
#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 range(len(self.q0))]
#setup CSpace edge checking epsilon
self.eps = 1e-2
def __init__(self, world):
CSpace.__init__(self)
#set bounds
self.bound = [(-5, 5), (-5, 5), (0, math.pi * 2)]
#set collision checking resolution
self.eps = 1e-1
#get the robot (a RobotModel instance)
self.robot = world.robot(0)
#get obstacles here, these will be TerrainModel instances
self.obstacles = [
world.terrain(i) for i in xrange(world.numTerrains())
]
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 = list(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, bounds, milestones):
CSpace.__init__(self)
#set bounds
bound = []
for a, b in zip(self.min_vals[:2], self.max_vals[:2]):
bound.append((a, b))
self.bound = bound
self.milestones = milestones
#set collision checking resolution
self.eps = 0.05
#setup a robot with radius 0.05
#set obstacles here
self.remaining_milestones = self.milestones[:]
self.fraction = 1
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)
if not CSpace.feasible(self, (p1[0], p1[1], 0)): return False
if not CSpace.feasible(self, (p2[0], p2[1], 0)): return False
#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 __init__(self, globals, hand, object):
CSpace.__init__(self)
self.globals = globals
self.robot = globals.robot
self.hand = hand
self.object = object
#initial whole-body configuratoin
self.q0 = self.robot.getConfig()
#setup initial grasp transform
Tobj0 = object.getTransform()
self.Tgrasp = graspTransform(self.robot, hand, self.q0, Tobj0)
#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):
"""TODO: Implement this feasibility test. It is used by the motion planner to
determine whether the robot at configuration q is feasible."""
robotRadius = self.robot.radius
# bounds test
if not CSpace.feasible(self, q): return False
for o in self.obstacles:
if o.distance(q) <= robotRadius: return False
# bound the robot in a square defined by robot's radius
bottomLeft = vectorops.add(q, (-robotRadius, -robotRadius))
topRight = vectorops.add(q, (robotRadius, robotRadius))
# make sure you can put the square in the config space
# if topLeft is not feasible bottomLeft or topRight will be infeasible
# if bottomRight is not feasible bottomLeft or topRight will be infeasible
boundingSquareFeasible = CSpace.feasible(
self, bottomLeft) and CSpace.feasible(self, topRight)
return boundingSquareFeasible
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 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):
"""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
def __init__(self):
CSpace.__init__(self)
self.bound = [[0, 1], [0, 1]]
self.bmin = [0, 0]
self.bmax = [1, 1]
self.obstacles = []
