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 = []